From bda6222da40e537772657958172cb01da11704b6 Mon Sep 17 00:00:00 2001 From: Sushmita Date: Sun, 11 Oct 2020 16:46:10 -0400 Subject: [PATCH 01/62] python wrapper for sfmdata --- gtsam/gtsam.i | 17 +++++++++++++++-- gtsam/slam/dataset.cpp | 12 ++++++------ gtsam/slam/dataset.h | 30 +++++++++++++++++++++++++++--- python/CMakeLists.txt | 2 ++ python/gtsam/preamble.h | 2 ++ python/gtsam/specializations.h | 2 ++ 6 files changed, 54 insertions(+), 11 deletions(-) diff --git a/gtsam/gtsam.i b/gtsam/gtsam.i index 880b1d4c7..73ce4f203 100644 --- a/gtsam/gtsam.i +++ b/gtsam/gtsam.i @@ -2759,21 +2759,34 @@ virtual class EssentialMatrixFactor : gtsam::NoiseModelFactor { }; #include +class SfmMeasurement{}; +class SiftIndex{ }; +class SfmMeasurements{}; + class SfmTrack { + SfmTrack(); Point3 point3() const; size_t number_measurements() const; - pair measurement(size_t idx) const; - pair siftIndex(size_t idx) const; + gtsam::SfmMeasurement measurement(size_t idx) const; + gtsam::SiftIndex siftIndex(size_t idx) const; + // gtsam::Measurements add_measurement(pair m); + void add_measurement(pair m); + SfmMeasurements& measurements(); }; class SfmData { + SfmData(); size_t number_cameras() const; size_t number_tracks() const; gtsam::PinholeCamera camera(size_t idx) const; gtsam::SfmTrack track(size_t idx) const; + // std::vector add_track(gtsam::SfmTrack t); + void add_track(gtsam::SfmTrack t); + void delete_track(size_t idx); }; gtsam::SfmData readBal(string filename); +bool writeBAL(string filename, gtsam::SfmData& data); gtsam::Values initialCamerasEstimate(const gtsam::SfmData& db); gtsam::Values initialCamerasAndPointsEstimate(const gtsam::SfmData& db); diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp index 270dbeb95..9b610b231 100644 --- a/gtsam/slam/dataset.cpp +++ b/gtsam/slam/dataset.cpp @@ -1052,13 +1052,13 @@ bool readBundler(const string &filename, SfmData &data) { size_t nvisible = 0; is >> nvisible; - track.measurements.reserve(nvisible); + track.Measurements.reserve(nvisible); track.siftIndices.reserve(nvisible); for (size_t k = 0; k < nvisible; k++) { size_t cam_idx = 0, point_idx = 0; float u, v; is >> cam_idx >> point_idx >> u >> v; - track.measurements.emplace_back(cam_idx, Point2(u, -v)); + track.Measurements.emplace_back(cam_idx, Point2(u, -v)); track.siftIndices.emplace_back(cam_idx, point_idx); } @@ -1089,7 +1089,7 @@ bool readBAL(const string &filename, SfmData &data) { size_t i = 0, j = 0; float u, v; is >> i >> j >> u >> v; - data.tracks[j].measurements.emplace_back(i, Point2(u, -v)); + data.tracks[j].Measurements.emplace_back(i, Point2(u, -v)); } // Get the information for the camera poses @@ -1163,7 +1163,7 @@ bool writeBAL(const string &filename, SfmData &data) { for (size_t k = 0; k < track.number_measurements(); k++) { // for each observation of the 3D point j - size_t i = track.measurements[k].first; // camera id + size_t i = track.Measurements[k].first; // camera id double u0 = data.cameras[i].calibration().u0(); double v0 = data.cameras[i].calibration().v0(); @@ -1173,9 +1173,9 @@ bool writeBAL(const string &filename, SfmData &data) { << endl; } - double pixelBALx = track.measurements[k].second.x() - + double pixelBALx = track.Measurements[k].second.x() - u0; // center of image is the origin - double pixelBALy = -(track.measurements[k].second.y() - + double pixelBALy = -(track.Measurements[k].second.y() - v0); // center of image is the origin Point2 pixelMeasurement(pixelBALx, pixelBALy); os << i /*camera id*/ << " " << j /*point id*/ << " " diff --git a/gtsam/slam/dataset.h b/gtsam/slam/dataset.h index a8fddcfe4..5a206929c 100644 --- a/gtsam/slam/dataset.h +++ b/gtsam/slam/dataset.h @@ -210,6 +210,7 @@ GTSAM_EXPORT GraphAndValues load3D(const std::string& filename); /// A measurement with its camera index typedef std::pair SfmMeasurement; +typedef std::vector SfmMeasurements; /// SfmTrack typedef std::pair SiftIndex; @@ -219,15 +220,16 @@ struct SfmTrack { SfmTrack(): p(0,0,0) {} Point3 p; ///< 3D position of the point float r, g, b; ///< RGB color of the 3D point - std::vector measurements; ///< The 2D image projections (id,(u,v)) + std::vector Measurements; ///< The 2D image projections (id,(u,v)) std::vector siftIndices; + /// Total number of measurements in this track size_t number_measurements() const { - return measurements.size(); + return Measurements.size(); } /// Get the measurement (camera index, Point2) at pose index `idx` SfmMeasurement measurement(size_t idx) const { - return measurements[idx]; + return Measurements[idx]; } /// Get the SIFT feature index corresponding to the measurement at `idx` SiftIndex siftIndex(size_t idx) const { @@ -236,13 +238,27 @@ struct SfmTrack { Point3 point3() const { return p; } + void add_measurement(pair m) { + Measurements.push_back(m); + } + + SfmMeasurements& measurements() { + return Measurements; + } + + void clear() { + Measurements.clear(); + } + }; + /// Define the structure for the camera poses typedef PinholeCamera SfmCamera; /// Define the structure for SfM data struct SfmData { + std::vector Measurements; std::vector cameras; ///< Set of cameras std::vector tracks; ///< Sparse set of points size_t number_cameras() const { @@ -260,6 +276,14 @@ struct SfmData { SfmTrack track(size_t idx) const { return tracks[idx]; } + + void add_track(SfmTrack t) { + tracks.push_back(t); + } + /// Delete track at `idx` + void delete_track(size_t idx){ + tracks[idx].clear(); + } }; /** diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index 00b537340..322968694 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -36,6 +36,8 @@ set(ignore gtsam::BetweenFactorPose3s gtsam::Point2Vector gtsam::Pose3Vector + gtsam::SfmMeasurement + gtsam::SiftIndex gtsam::KeyVector gtsam::BinaryMeasurementsUnit3 gtsam::KeyPairDoubleMap) diff --git a/python/gtsam/preamble.h b/python/gtsam/preamble.h index 6166f615e..8525ed3a3 100644 --- a/python/gtsam/preamble.h +++ b/python/gtsam/preamble.h @@ -10,3 +10,5 @@ PYBIND11_MAKE_OPAQUE(std::vector); PYBIND11_MAKE_OPAQUE(std::vector > >); PYBIND11_MAKE_OPAQUE(std::vector > >); PYBIND11_MAKE_OPAQUE(std::vector); +PYBIND11_MAKE_OPAQUE(std::vector); +PYBIND11_MAKE_OPAQUE(std::vector); \ No newline at end of file diff --git a/python/gtsam/specializations.h b/python/gtsam/specializations.h index cacad874c..03e2b606c 100644 --- a/python/gtsam/specializations.h +++ b/python/gtsam/specializations.h @@ -13,3 +13,5 @@ py::bind_vector > >(m_, "Bina py::bind_map(m_, "IndexPairSetMap"); py::bind_vector(m_, "IndexPairVector"); py::bind_map(m_, "KeyPairDoubleMap"); +py::bind_vector >(m_, "Measurement"); +py::bind_vector >(m_, "SiftIndexVector"); \ No newline at end of file From 8a7ce130ad99bf0054c8786d2f516ebb99a91545 Mon Sep 17 00:00:00 2001 From: Frank dellaert Date: Mon, 12 Oct 2020 14:32:21 -0400 Subject: [PATCH 02/62] Fix warning on clang --- cmake/GtsamBuildTypes.cmake | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cmake/GtsamBuildTypes.cmake b/cmake/GtsamBuildTypes.cmake index 840d37427..d13e3aa12 100644 --- a/cmake/GtsamBuildTypes.cmake +++ b/cmake/GtsamBuildTypes.cmake @@ -109,7 +109,7 @@ else() ((CMAKE_CXX_COMPILER_ID MATCHES "Clang") AND (NOT CMAKE_CXX_COMPILER_VERSION VERSION_LESS 12.0.0)) OR (CMAKE_CXX_COMPILER_ID MATCHES "GNU") ) - set(flag_override_ -Wsuggest-override) # -Werror=suggest-override: Add again someday + # set(flag_override_ -Wsuggest-override) # -Werror=suggest-override: Add again someday endif() set(GTSAM_COMPILE_OPTIONS_PRIVATE_COMMON From ed387e38173d837cf8c421eab649483b5e560231 Mon Sep 17 00:00:00 2001 From: Sushmita Date: Sun, 18 Oct 2020 11:17:10 -0400 Subject: [PATCH 03/62] unittested features in SfmData --- gtsam/gtsam.i | 7 +-- gtsam/slam/dataset.h | 15 +++--- python/CMakeLists.txt | 1 + python/gtsam/preamble.h | 3 +- python/gtsam/specializations.h | 3 +- python/gtsam/tests/test_SfmData.py | 79 ++++++++++++++++++++++++++++++ 6 files changed, 96 insertions(+), 12 deletions(-) create mode 100644 python/gtsam/tests/test_SfmData.py diff --git a/gtsam/gtsam.i b/gtsam/gtsam.i index 73ce4f203..b3792fbec 100644 --- a/gtsam/gtsam.i +++ b/gtsam/gtsam.i @@ -2759,17 +2759,19 @@ virtual class EssentialMatrixFactor : gtsam::NoiseModelFactor { }; #include +// Dummy classes, for MATLAB wrappers class SfmMeasurement{}; class SiftIndex{ }; class SfmMeasurements{}; +class SfmCamera{}; class SfmTrack { SfmTrack(); Point3 point3() const; size_t number_measurements() const; + void setP(gtsam::Point3& p_); gtsam::SfmMeasurement measurement(size_t idx) const; gtsam::SiftIndex siftIndex(size_t idx) const; - // gtsam::Measurements add_measurement(pair m); void add_measurement(pair m); SfmMeasurements& measurements(); }; @@ -2780,9 +2782,8 @@ class SfmData { size_t number_tracks() const; gtsam::PinholeCamera camera(size_t idx) const; gtsam::SfmTrack track(size_t idx) const; - // std::vector add_track(gtsam::SfmTrack t); void add_track(gtsam::SfmTrack t); - void delete_track(size_t idx); + void add_camera(gtsam::SfmCamera cam); }; gtsam::SfmData readBal(string filename); diff --git a/gtsam/slam/dataset.h b/gtsam/slam/dataset.h index 5a206929c..2e4c715be 100644 --- a/gtsam/slam/dataset.h +++ b/gtsam/slam/dataset.h @@ -227,6 +227,11 @@ struct SfmTrack { size_t number_measurements() const { return Measurements.size(); } + /// Set 3D point + void setP(Point3& p_){ + p = p_; + } + /// Get the measurement (camera index, Point2) at pose index `idx` SfmMeasurement measurement(size_t idx) const { return Measurements[idx]; @@ -246,10 +251,6 @@ struct SfmTrack { return Measurements; } - void clear() { - Measurements.clear(); - } - }; @@ -280,9 +281,9 @@ struct SfmData { void add_track(SfmTrack t) { tracks.push_back(t); } - /// Delete track at `idx` - void delete_track(size_t idx){ - tracks[idx].clear(); + + void add_camera(SfmCamera cam){ + cameras.push_back(cam); } }; diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index 322968694..7d12b9800 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -37,6 +37,7 @@ set(ignore gtsam::Point2Vector gtsam::Pose3Vector gtsam::SfmMeasurement + gtsam::SfmCamera gtsam::SiftIndex gtsam::KeyVector gtsam::BinaryMeasurementsUnit3 diff --git a/python/gtsam/preamble.h b/python/gtsam/preamble.h index 8525ed3a3..e6ed64689 100644 --- a/python/gtsam/preamble.h +++ b/python/gtsam/preamble.h @@ -11,4 +11,5 @@ PYBIND11_MAKE_OPAQUE(std::vector > >); PYBIND11_MAKE_OPAQUE(std::vector); PYBIND11_MAKE_OPAQUE(std::vector); -PYBIND11_MAKE_OPAQUE(std::vector); \ No newline at end of file +PYBIND11_MAKE_OPAQUE(std::vector); +PYBIND11_MAKE_OPAQUE(std::vector); \ No newline at end of file diff --git a/python/gtsam/specializations.h b/python/gtsam/specializations.h index 03e2b606c..dcaaa4c76 100644 --- a/python/gtsam/specializations.h +++ b/python/gtsam/specializations.h @@ -14,4 +14,5 @@ py::bind_map(m_, "IndexPairSetMap"); py::bind_vector(m_, "IndexPairVector"); py::bind_map(m_, "KeyPairDoubleMap"); py::bind_vector >(m_, "Measurement"); -py::bind_vector >(m_, "SiftIndexVector"); \ No newline at end of file +py::bind_vector >(m_, "SiftIndexVector"); +py::bind_vector >(m_, "cameras"); \ No newline at end of file diff --git a/python/gtsam/tests/test_SfmData.py b/python/gtsam/tests/test_SfmData.py new file mode 100644 index 000000000..4664d746e --- /dev/null +++ b/python/gtsam/tests/test_SfmData.py @@ -0,0 +1,79 @@ +""" +GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved + +See LICENSE for the license information + +Unit tests for testing dataset access. +Author: Frank Dellaert (Python: Sushmita Warrier) +""" +# pylint: disable=invalid-name, no-name-in-module, no-member + +from __future__ import print_function + +import unittest + +import numpy as np + +import gtsam +#from gtsam import SfmCamera +from gtsam.utils.test_case import GtsamTestCase + + +class TestSfmData(GtsamTestCase): + """Tests for SfmData and SfmTrack modules.""" + + def setUp(self): + """Initialize SfmData and SfmTrack""" + self.data = gtsam.SfmData() + self.tracks = gtsam.SfmTrack() + + def test_tracks(self): + """Test functions in SfmTrack""" + # measurement is of format (camera_idx, imgPoint) + # create camera indices for two cameras + i1, i2 = np.random.randint(5), np.random.randint(5) + # create imgPoint for cameras i1 and i2 + uv_i1 = gtsam.Point2(np.random.randint(5), np.random.randint(5)) + uv_i2 = gtsam.Point2(np.random.randint(5), np.random.randint(5)) + m_i1 = (i1, uv_i1) + m_i2 = (i2, uv_i2) + # add measurements to the track + self.tracks.add_measurement(m_i1) + self.tracks.add_measurement(m_i2) + # Number of measurements in the track is 2 + self.assertEqual(self.tracks.number_measurements(), 2) + # camera_idx in the first measurement of the track corresponds to i1 + self.assertEqual(self.tracks.measurement(0)[0], i1) + # Set arbitrary 3D point corresponding to the track + self.tracks.setP(gtsam.Point3(2.5, 3.3, 1.2)) + np.testing.assert_array_almost_equal( + gtsam.Point3(2.5,3.3,1.2), + self.tracks.point3() + ) + + + def test_data(self): + """Test functions in SfmData""" + #cam1 = gtsam.SfmCamera(1500, 1200, 0, 640, 480) + # Create new track with 3 measurements + track2 = gtsam.SfmTrack() + i1, i2, i3 = np.random.randint(5), np.random.randint(5), np.random.randint(5) + uv_i1 = gtsam.Point2(np.random.randint(5), np.random.randint(5)) + uv_i2 = gtsam.Point2(np.random.randint(5), np.random.randint(5)) + uv_i3 = gtsam.Point2(np.random.randint(5), np.random.randint(5)) + m_i1, m_i2, m_i3 = (i1, uv_i1), (i2, uv_i2), (i3, uv_i3) + # add measurements to the track + track2.add_measurement(m_i1) + track2.add_measurement(m_i2) + track2.add_measurement(m_i3) + self.data.add_track(self.tracks) + self.data.add_track(track2) + # Number of tracks in SfmData is 2 + self.assertEqual(self.data.number_tracks(), 2) + # camera idx of first measurement of second track corresponds to i1 + self.assertEqual(self.data.track(1).measurement(0)[0], i1) + +if __name__ == '__main__': + unittest.main() From 1f09f4aab680f49da367d2ceb3639356bd20746f Mon Sep 17 00:00:00 2001 From: Sushmita Date: Sun, 18 Oct 2020 19:05:09 -0400 Subject: [PATCH 04/62] python wrapped SfmData and SfmTrack --- gtsam/slam/SmartFactorBase.h | 4 ++-- gtsam/slam/tests/testDataset.cpp | 16 ++++++++-------- gtsam/slam/tests/testEssentialMatrixFactor.cpp | 8 ++++---- gtsam/slam/tests/testSmartProjectionFactor.cpp | 4 ++-- tests/testGeneralSFMFactorB.cpp | 2 +- 5 files changed, 17 insertions(+), 17 deletions(-) diff --git a/gtsam/slam/SmartFactorBase.h b/gtsam/slam/SmartFactorBase.h index d9f2b9c3d..5afc159bd 100644 --- a/gtsam/slam/SmartFactorBase.h +++ b/gtsam/slam/SmartFactorBase.h @@ -144,8 +144,8 @@ protected: template void add(const SFM_TRACK& trackToAdd) { for (size_t k = 0; k < trackToAdd.number_measurements(); k++) { - this->measured_.push_back(trackToAdd.measurements[k].second); - this->keys_.push_back(trackToAdd.measurements[k].first); + this->measured_.push_back(trackToAdd.Measurements[k].second); + this->keys_.push_back(trackToAdd.Measurements[k].first); } } diff --git a/gtsam/slam/tests/testDataset.cpp b/gtsam/slam/tests/testDataset.cpp index aad9124c5..ff5cf8b46 100644 --- a/gtsam/slam/tests/testDataset.cpp +++ b/gtsam/slam/tests/testDataset.cpp @@ -166,9 +166,9 @@ TEST( dataSet, Balbianello) EXPECT_LONGS_EQUAL(3,track0.number_measurements()); // Check projection of a given point - EXPECT_LONGS_EQUAL(0,track0.measurements[0].first); + EXPECT_LONGS_EQUAL(0,track0.Measurements[0].first); const SfmCamera& camera0 = mydata.cameras[0]; - Point2 expected = camera0.project(track0.p), actual = track0.measurements[0].second; + Point2 expected = camera0.project(track0.p), actual = track0.Measurements[0].second; EXPECT(assert_equal(expected,actual,1)); } @@ -476,9 +476,9 @@ TEST( dataSet, readBAL_Dubrovnik) EXPECT_LONGS_EQUAL(3,track0.number_measurements()); // Check projection of a given point - EXPECT_LONGS_EQUAL(0,track0.measurements[0].first); + EXPECT_LONGS_EQUAL(0,track0.Measurements[0].first); const SfmCamera& camera0 = mydata.cameras[0]; - Point2 expected = camera0.project(track0.p), actual = track0.measurements[0].second; + Point2 expected = camera0.project(track0.p), actual = track0.Measurements[0].second; EXPECT(assert_equal(expected,actual,12)); } @@ -557,8 +557,8 @@ TEST( dataSet, writeBAL_Dubrovnik) // check measurements for (size_t k = 0; k < actualTrack.number_measurements(); k++){ - EXPECT_LONGS_EQUAL(expectedTrack.measurements[k].first,actualTrack.measurements[k].first); - EXPECT(assert_equal(expectedTrack.measurements[k].second,actualTrack.measurements[k].second)); + EXPECT_LONGS_EQUAL(expectedTrack.Measurements[k].first,actualTrack.Measurements[k].first); + EXPECT(assert_equal(expectedTrack.Measurements[k].second,actualTrack.Measurements[k].second)); } } } @@ -600,9 +600,9 @@ TEST( dataSet, writeBALfromValues_Dubrovnik){ EXPECT_LONGS_EQUAL(3,track0.number_measurements()); // Check projection of a given point - EXPECT_LONGS_EQUAL(0,track0.measurements[0].first); + EXPECT_LONGS_EQUAL(0,track0.Measurements[0].first); const SfmCamera& camera0 = writtenData.cameras[0]; - Point2 expected = camera0.project(track0.p), actual = track0.measurements[0].second; + Point2 expected = camera0.project(track0.p), actual = track0.Measurements[0].second; EXPECT(assert_equal(expected,actual,12)); Pose3 expectedPose = camera0.pose(); diff --git a/gtsam/slam/tests/testEssentialMatrixFactor.cpp b/gtsam/slam/tests/testEssentialMatrixFactor.cpp index 95db611d7..6bece036f 100644 --- a/gtsam/slam/tests/testEssentialMatrixFactor.cpp +++ b/gtsam/slam/tests/testEssentialMatrixFactor.cpp @@ -46,10 +46,10 @@ EssentialMatrix trueE(trueRotation, trueDirection); double baseline = 0.1; // actual baseline of the camera Point2 pA(size_t i) { - return data.tracks[i].measurements[0].second; + return data.tracks[i].Measurements[0].second; } Point2 pB(size_t i) { - return data.tracks[i].measurements[1].second; + return data.tracks[i].Measurements[1].second; } Vector vA(size_t i) { return EssentialMatrix::Homogeneous(pA(i)); @@ -367,10 +367,10 @@ EssentialMatrix trueE(aRb, Unit3(aTb)); double baseline = 10; // actual baseline of the camera Point2 pA(size_t i) { - return data.tracks[i].measurements[0].second; + return data.tracks[i].Measurements[0].second; } Point2 pB(size_t i) { - return data.tracks[i].measurements[1].second; + return data.tracks[i].Measurements[1].second; } boost::shared_ptr // diff --git a/gtsam/slam/tests/testSmartProjectionFactor.cpp b/gtsam/slam/tests/testSmartProjectionFactor.cpp index 1fd06cc9f..1f3a5d1cb 100644 --- a/gtsam/slam/tests/testSmartProjectionFactor.cpp +++ b/gtsam/slam/tests/testSmartProjectionFactor.cpp @@ -283,14 +283,14 @@ TEST(SmartProjectionFactor, perturbPoseAndOptimizeFromSfM_tracks ) { SfmTrack track1; for (size_t i = 0; i < 3; ++i) { - track1.measurements.emplace_back(i + 1, measurements_cam1.at(i)); + track1.Measurements.emplace_back(i + 1, measurements_cam1.at(i)); } SmartFactor::shared_ptr smartFactor1(new SmartFactor(unit2)); smartFactor1->add(track1); SfmTrack track2; for (size_t i = 0; i < 3; ++i) { - track2.measurements.emplace_back(i + 1, measurements_cam2.at(i)); + track2.Measurements.emplace_back(i + 1, measurements_cam2.at(i)); } SmartFactor::shared_ptr smartFactor2(new SmartFactor(unit2)); smartFactor2->add(track2); diff --git a/tests/testGeneralSFMFactorB.cpp b/tests/testGeneralSFMFactorB.cpp index 05b4c7f66..fafe87d13 100644 --- a/tests/testGeneralSFMFactorB.cpp +++ b/tests/testGeneralSFMFactorB.cpp @@ -50,7 +50,7 @@ TEST(PinholeCamera, BAL) { NonlinearFactorGraph graph; for (size_t j = 0; j < db.number_tracks(); j++) { - for (const SfmMeasurement& m: db.tracks[j].measurements) + for (const SfmMeasurement& m: db.tracks[j].Measurements) graph.emplace_shared(m.second, unit2, m.first, P(j)); } From b0cca7eeddbc280aa1c4eaa4c8588e44905159f1 Mon Sep 17 00:00:00 2001 From: Sushmita Date: Sun, 18 Oct 2020 19:05:42 -0400 Subject: [PATCH 05/62] python wrapper sfmtrack reflected in other files --- examples/CreateSFMExampleData.cpp | 2 +- examples/SFMExampleExpressions_bal.cpp | 2 +- examples/SFMExample_bal.cpp | 2 +- examples/SFMExample_bal_COLAMD_METIS.cpp | 2 +- 4 files changed, 4 insertions(+), 4 deletions(-) diff --git a/examples/CreateSFMExampleData.cpp b/examples/CreateSFMExampleData.cpp index aa0bde8f6..602d396d9 100644 --- a/examples/CreateSFMExampleData.cpp +++ b/examples/CreateSFMExampleData.cpp @@ -51,7 +51,7 @@ void createExampleBALFile(const string& filename, const vector& P, // Project points in both cameras for (size_t i = 0; i < 2; i++) - track.measurements.push_back(make_pair(i, data.cameras[i].project(p))); + track.Measurements.push_back(make_pair(i, data.cameras[i].project(p))); // Add track to data data.tracks.push_back(track); diff --git a/examples/SFMExampleExpressions_bal.cpp b/examples/SFMExampleExpressions_bal.cpp index 3768ee2a3..fd9ef5648 100644 --- a/examples/SFMExampleExpressions_bal.cpp +++ b/examples/SFMExampleExpressions_bal.cpp @@ -77,7 +77,7 @@ int main(int argc, char* argv[]) { for (const SfmTrack& track : mydata.tracks) { // Leaf expression for j^th point Point3_ point_('p', j); - for (const SfmMeasurement& m : track.measurements) { + for (const SfmMeasurement& m : track.Measurements) { size_t i = m.first; Point2 uv = m.second; // Leaf expression for i^th camera diff --git a/examples/SFMExample_bal.cpp b/examples/SFMExample_bal.cpp index ffb5b195b..4572d8424 100644 --- a/examples/SFMExample_bal.cpp +++ b/examples/SFMExample_bal.cpp @@ -55,7 +55,7 @@ int main (int argc, char* argv[]) { // Add measurements to the factor graph size_t j = 0; for(const SfmTrack& track: mydata.tracks) { - for(const SfmMeasurement& m: track.measurements) { + for(const SfmMeasurement& m: track.Measurements) { size_t i = m.first; Point2 uv = m.second; graph.emplace_shared(uv, noise, C(i), P(j)); // note use of shorthand symbols C and P diff --git a/examples/SFMExample_bal_COLAMD_METIS.cpp b/examples/SFMExample_bal_COLAMD_METIS.cpp index b79a9fa28..d378d89c3 100644 --- a/examples/SFMExample_bal_COLAMD_METIS.cpp +++ b/examples/SFMExample_bal_COLAMD_METIS.cpp @@ -59,7 +59,7 @@ int main(int argc, char* argv[]) { // Add measurements to the factor graph size_t j = 0; for (const SfmTrack& track : mydata.tracks) { - for (const SfmMeasurement& m : track.measurements) { + for (const SfmMeasurement& m : track.Measurements) { size_t i = m.first; Point2 uv = m.second; graph.emplace_shared( From 045780a151513a1723efce05fc95cdec28e14673 Mon Sep 17 00:00:00 2001 From: Sushmita Date: Wed, 21 Oct 2020 23:43:17 -0400 Subject: [PATCH 06/62] changed Measurements to measurements --- examples/CreateSFMExampleData.cpp | 2 +- examples/SFMExampleExpressions_bal.cpp | 2 +- examples/SFMExample_bal.cpp | 2 +- examples/SFMExample_bal_COLAMD_METIS.cpp | 2 +- gtsam/gtsam.i | 25 +++++++++++++------ gtsam/slam/SmartFactorBase.h | 4 +-- gtsam/slam/dataset.cpp | 12 ++++----- gtsam/slam/dataset.h | 18 ++++++------- gtsam/slam/tests/testDataset.cpp | 16 ++++++------ .../slam/tests/testEssentialMatrixFactor.cpp | 8 +++--- .../slam/tests/testSmartProjectionFactor.cpp | 4 +-- python/gtsam/preamble.h | 3 +-- python/gtsam/specializations.h | 3 +-- tests/testGeneralSFMFactorB.cpp | 2 +- 14 files changed, 55 insertions(+), 48 deletions(-) diff --git a/examples/CreateSFMExampleData.cpp b/examples/CreateSFMExampleData.cpp index 602d396d9..aa0bde8f6 100644 --- a/examples/CreateSFMExampleData.cpp +++ b/examples/CreateSFMExampleData.cpp @@ -51,7 +51,7 @@ void createExampleBALFile(const string& filename, const vector& P, // Project points in both cameras for (size_t i = 0; i < 2; i++) - track.Measurements.push_back(make_pair(i, data.cameras[i].project(p))); + track.measurements.push_back(make_pair(i, data.cameras[i].project(p))); // Add track to data data.tracks.push_back(track); diff --git a/examples/SFMExampleExpressions_bal.cpp b/examples/SFMExampleExpressions_bal.cpp index fd9ef5648..3768ee2a3 100644 --- a/examples/SFMExampleExpressions_bal.cpp +++ b/examples/SFMExampleExpressions_bal.cpp @@ -77,7 +77,7 @@ int main(int argc, char* argv[]) { for (const SfmTrack& track : mydata.tracks) { // Leaf expression for j^th point Point3_ point_('p', j); - for (const SfmMeasurement& m : track.Measurements) { + for (const SfmMeasurement& m : track.measurements) { size_t i = m.first; Point2 uv = m.second; // Leaf expression for i^th camera diff --git a/examples/SFMExample_bal.cpp b/examples/SFMExample_bal.cpp index 4572d8424..ffb5b195b 100644 --- a/examples/SFMExample_bal.cpp +++ b/examples/SFMExample_bal.cpp @@ -55,7 +55,7 @@ int main (int argc, char* argv[]) { // Add measurements to the factor graph size_t j = 0; for(const SfmTrack& track: mydata.tracks) { - for(const SfmMeasurement& m: track.Measurements) { + for(const SfmMeasurement& m: track.measurements) { size_t i = m.first; Point2 uv = m.second; graph.emplace_shared(uv, noise, C(i), P(j)); // note use of shorthand symbols C and P diff --git a/examples/SFMExample_bal_COLAMD_METIS.cpp b/examples/SFMExample_bal_COLAMD_METIS.cpp index d378d89c3..b79a9fa28 100644 --- a/examples/SFMExample_bal_COLAMD_METIS.cpp +++ b/examples/SFMExample_bal_COLAMD_METIS.cpp @@ -59,7 +59,7 @@ int main(int argc, char* argv[]) { // Add measurements to the factor graph size_t j = 0; for (const SfmTrack& track : mydata.tracks) { - for (const SfmMeasurement& m : track.Measurements) { + for (const SfmMeasurement& m : track.measurements) { size_t i = m.first; Point2 uv = m.second; graph.emplace_shared( diff --git a/gtsam/gtsam.i b/gtsam/gtsam.i index b3792fbec..5334a233d 100644 --- a/gtsam/gtsam.i +++ b/gtsam/gtsam.i @@ -2759,11 +2759,20 @@ virtual class EssentialMatrixFactor : gtsam::NoiseModelFactor { }; #include -// Dummy classes, for MATLAB wrappers -class SfmMeasurement{}; -class SiftIndex{ }; -class SfmMeasurements{}; -class SfmCamera{}; +class SfmMeasurement{ + SfmMeasurement(); + size_t i() const; + Point2 j() const; +}; +class SiftIndex{ + SiftIndex(); + size_t i() const; + size_t j() const; + }; +class SfmMeasurements{ + SfmMeasurements(); + +}; class SfmTrack { SfmTrack(); @@ -2772,7 +2781,7 @@ class SfmTrack { void setP(gtsam::Point3& p_); gtsam::SfmMeasurement measurement(size_t idx) const; gtsam::SiftIndex siftIndex(size_t idx) const; - void add_measurement(pair m); + void add_measurement(const pair& m); SfmMeasurements& measurements(); }; @@ -2782,8 +2791,8 @@ class SfmData { size_t number_tracks() const; gtsam::PinholeCamera camera(size_t idx) const; gtsam::SfmTrack track(size_t idx) const; - void add_track(gtsam::SfmTrack t); - void add_camera(gtsam::SfmCamera cam); + void add_track(const gtsam::SfmTrack& t) ; + void add_camera(const gtsam::SfmCamer& cam); }; gtsam::SfmData readBal(string filename); diff --git a/gtsam/slam/SmartFactorBase.h b/gtsam/slam/SmartFactorBase.h index 5afc159bd..d9f2b9c3d 100644 --- a/gtsam/slam/SmartFactorBase.h +++ b/gtsam/slam/SmartFactorBase.h @@ -144,8 +144,8 @@ protected: template void add(const SFM_TRACK& trackToAdd) { for (size_t k = 0; k < trackToAdd.number_measurements(); k++) { - this->measured_.push_back(trackToAdd.Measurements[k].second); - this->keys_.push_back(trackToAdd.Measurements[k].first); + this->measured_.push_back(trackToAdd.measurements[k].second); + this->keys_.push_back(trackToAdd.measurements[k].first); } } diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp index 9b610b231..270dbeb95 100644 --- a/gtsam/slam/dataset.cpp +++ b/gtsam/slam/dataset.cpp @@ -1052,13 +1052,13 @@ bool readBundler(const string &filename, SfmData &data) { size_t nvisible = 0; is >> nvisible; - track.Measurements.reserve(nvisible); + track.measurements.reserve(nvisible); track.siftIndices.reserve(nvisible); for (size_t k = 0; k < nvisible; k++) { size_t cam_idx = 0, point_idx = 0; float u, v; is >> cam_idx >> point_idx >> u >> v; - track.Measurements.emplace_back(cam_idx, Point2(u, -v)); + track.measurements.emplace_back(cam_idx, Point2(u, -v)); track.siftIndices.emplace_back(cam_idx, point_idx); } @@ -1089,7 +1089,7 @@ bool readBAL(const string &filename, SfmData &data) { size_t i = 0, j = 0; float u, v; is >> i >> j >> u >> v; - data.tracks[j].Measurements.emplace_back(i, Point2(u, -v)); + data.tracks[j].measurements.emplace_back(i, Point2(u, -v)); } // Get the information for the camera poses @@ -1163,7 +1163,7 @@ bool writeBAL(const string &filename, SfmData &data) { for (size_t k = 0; k < track.number_measurements(); k++) { // for each observation of the 3D point j - size_t i = track.Measurements[k].first; // camera id + size_t i = track.measurements[k].first; // camera id double u0 = data.cameras[i].calibration().u0(); double v0 = data.cameras[i].calibration().v0(); @@ -1173,9 +1173,9 @@ bool writeBAL(const string &filename, SfmData &data) { << endl; } - double pixelBALx = track.Measurements[k].second.x() - + double pixelBALx = track.measurements[k].second.x() - u0; // center of image is the origin - double pixelBALy = -(track.Measurements[k].second.y() - + double pixelBALy = -(track.measurements[k].second.y() - v0); // center of image is the origin Point2 pixelMeasurement(pixelBALx, pixelBALy); os << i /*camera id*/ << " " << j /*point id*/ << " " diff --git a/gtsam/slam/dataset.h b/gtsam/slam/dataset.h index 2e4c715be..062d8728c 100644 --- a/gtsam/slam/dataset.h +++ b/gtsam/slam/dataset.h @@ -220,12 +220,12 @@ struct SfmTrack { SfmTrack(): p(0,0,0) {} Point3 p; ///< 3D position of the point float r, g, b; ///< RGB color of the 3D point - std::vector Measurements; ///< The 2D image projections (id,(u,v)) + std::vector measurements; ///< The 2D image projections (id,(u,v)) std::vector siftIndices; /// Total number of measurements in this track size_t number_measurements() const { - return Measurements.size(); + return measurements.size(); } /// Set 3D point void setP(Point3& p_){ @@ -234,7 +234,7 @@ struct SfmTrack { /// Get the measurement (camera index, Point2) at pose index `idx` SfmMeasurement measurement(size_t idx) const { - return Measurements[idx]; + return measurements[idx]; } /// Get the SIFT feature index corresponding to the measurement at `idx` SiftIndex siftIndex(size_t idx) const { @@ -243,12 +243,12 @@ struct SfmTrack { Point3 point3() const { return p; } - void add_measurement(pair m) { - Measurements.push_back(m); + void add_measurement(pair& m) const{ + measurements.push_back(m); } SfmMeasurements& measurements() { - return Measurements; + return measurements; } }; @@ -259,7 +259,7 @@ typedef PinholeCamera SfmCamera; /// Define the structure for SfM data struct SfmData { - std::vector Measurements; + std::vector measurements; /// cameras; ///< Set of cameras std::vector tracks; ///< Sparse set of points size_t number_cameras() const { @@ -278,11 +278,11 @@ struct SfmData { return tracks[idx]; } - void add_track(SfmTrack t) { + void add_track(SfmTrack& t) const { tracks.push_back(t); } - void add_camera(SfmCamera cam){ + void add_camera(SfmCamera& cam) const{ cameras.push_back(cam); } }; diff --git a/gtsam/slam/tests/testDataset.cpp b/gtsam/slam/tests/testDataset.cpp index ff5cf8b46..aad9124c5 100644 --- a/gtsam/slam/tests/testDataset.cpp +++ b/gtsam/slam/tests/testDataset.cpp @@ -166,9 +166,9 @@ TEST( dataSet, Balbianello) EXPECT_LONGS_EQUAL(3,track0.number_measurements()); // Check projection of a given point - EXPECT_LONGS_EQUAL(0,track0.Measurements[0].first); + EXPECT_LONGS_EQUAL(0,track0.measurements[0].first); const SfmCamera& camera0 = mydata.cameras[0]; - Point2 expected = camera0.project(track0.p), actual = track0.Measurements[0].second; + Point2 expected = camera0.project(track0.p), actual = track0.measurements[0].second; EXPECT(assert_equal(expected,actual,1)); } @@ -476,9 +476,9 @@ TEST( dataSet, readBAL_Dubrovnik) EXPECT_LONGS_EQUAL(3,track0.number_measurements()); // Check projection of a given point - EXPECT_LONGS_EQUAL(0,track0.Measurements[0].first); + EXPECT_LONGS_EQUAL(0,track0.measurements[0].first); const SfmCamera& camera0 = mydata.cameras[0]; - Point2 expected = camera0.project(track0.p), actual = track0.Measurements[0].second; + Point2 expected = camera0.project(track0.p), actual = track0.measurements[0].second; EXPECT(assert_equal(expected,actual,12)); } @@ -557,8 +557,8 @@ TEST( dataSet, writeBAL_Dubrovnik) // check measurements for (size_t k = 0; k < actualTrack.number_measurements(); k++){ - EXPECT_LONGS_EQUAL(expectedTrack.Measurements[k].first,actualTrack.Measurements[k].first); - EXPECT(assert_equal(expectedTrack.Measurements[k].second,actualTrack.Measurements[k].second)); + EXPECT_LONGS_EQUAL(expectedTrack.measurements[k].first,actualTrack.measurements[k].first); + EXPECT(assert_equal(expectedTrack.measurements[k].second,actualTrack.measurements[k].second)); } } } @@ -600,9 +600,9 @@ TEST( dataSet, writeBALfromValues_Dubrovnik){ EXPECT_LONGS_EQUAL(3,track0.number_measurements()); // Check projection of a given point - EXPECT_LONGS_EQUAL(0,track0.Measurements[0].first); + EXPECT_LONGS_EQUAL(0,track0.measurements[0].first); const SfmCamera& camera0 = writtenData.cameras[0]; - Point2 expected = camera0.project(track0.p), actual = track0.Measurements[0].second; + Point2 expected = camera0.project(track0.p), actual = track0.measurements[0].second; EXPECT(assert_equal(expected,actual,12)); Pose3 expectedPose = camera0.pose(); diff --git a/gtsam/slam/tests/testEssentialMatrixFactor.cpp b/gtsam/slam/tests/testEssentialMatrixFactor.cpp index 6bece036f..95db611d7 100644 --- a/gtsam/slam/tests/testEssentialMatrixFactor.cpp +++ b/gtsam/slam/tests/testEssentialMatrixFactor.cpp @@ -46,10 +46,10 @@ EssentialMatrix trueE(trueRotation, trueDirection); double baseline = 0.1; // actual baseline of the camera Point2 pA(size_t i) { - return data.tracks[i].Measurements[0].second; + return data.tracks[i].measurements[0].second; } Point2 pB(size_t i) { - return data.tracks[i].Measurements[1].second; + return data.tracks[i].measurements[1].second; } Vector vA(size_t i) { return EssentialMatrix::Homogeneous(pA(i)); @@ -367,10 +367,10 @@ EssentialMatrix trueE(aRb, Unit3(aTb)); double baseline = 10; // actual baseline of the camera Point2 pA(size_t i) { - return data.tracks[i].Measurements[0].second; + return data.tracks[i].measurements[0].second; } Point2 pB(size_t i) { - return data.tracks[i].Measurements[1].second; + return data.tracks[i].measurements[1].second; } boost::shared_ptr // diff --git a/gtsam/slam/tests/testSmartProjectionFactor.cpp b/gtsam/slam/tests/testSmartProjectionFactor.cpp index 1f3a5d1cb..1fd06cc9f 100644 --- a/gtsam/slam/tests/testSmartProjectionFactor.cpp +++ b/gtsam/slam/tests/testSmartProjectionFactor.cpp @@ -283,14 +283,14 @@ TEST(SmartProjectionFactor, perturbPoseAndOptimizeFromSfM_tracks ) { SfmTrack track1; for (size_t i = 0; i < 3; ++i) { - track1.Measurements.emplace_back(i + 1, measurements_cam1.at(i)); + track1.measurements.emplace_back(i + 1, measurements_cam1.at(i)); } SmartFactor::shared_ptr smartFactor1(new SmartFactor(unit2)); smartFactor1->add(track1); SfmTrack track2; for (size_t i = 0; i < 3; ++i) { - track2.Measurements.emplace_back(i + 1, measurements_cam2.at(i)); + track2.measurements.emplace_back(i + 1, measurements_cam2.at(i)); } SmartFactor::shared_ptr smartFactor2(new SmartFactor(unit2)); smartFactor2->add(track2); diff --git a/python/gtsam/preamble.h b/python/gtsam/preamble.h index e6ed64689..8525ed3a3 100644 --- a/python/gtsam/preamble.h +++ b/python/gtsam/preamble.h @@ -11,5 +11,4 @@ PYBIND11_MAKE_OPAQUE(std::vector > >); PYBIND11_MAKE_OPAQUE(std::vector); PYBIND11_MAKE_OPAQUE(std::vector); -PYBIND11_MAKE_OPAQUE(std::vector); -PYBIND11_MAKE_OPAQUE(std::vector); \ No newline at end of file +PYBIND11_MAKE_OPAQUE(std::vector); \ No newline at end of file diff --git a/python/gtsam/specializations.h b/python/gtsam/specializations.h index dcaaa4c76..03e2b606c 100644 --- a/python/gtsam/specializations.h +++ b/python/gtsam/specializations.h @@ -14,5 +14,4 @@ py::bind_map(m_, "IndexPairSetMap"); py::bind_vector(m_, "IndexPairVector"); py::bind_map(m_, "KeyPairDoubleMap"); py::bind_vector >(m_, "Measurement"); -py::bind_vector >(m_, "SiftIndexVector"); -py::bind_vector >(m_, "cameras"); \ No newline at end of file +py::bind_vector >(m_, "SiftIndexVector"); \ No newline at end of file diff --git a/tests/testGeneralSFMFactorB.cpp b/tests/testGeneralSFMFactorB.cpp index fafe87d13..05b4c7f66 100644 --- a/tests/testGeneralSFMFactorB.cpp +++ b/tests/testGeneralSFMFactorB.cpp @@ -50,7 +50,7 @@ TEST(PinholeCamera, BAL) { NonlinearFactorGraph graph; for (size_t j = 0; j < db.number_tracks(); j++) { - for (const SfmMeasurement& m: db.tracks[j].Measurements) + for (const SfmMeasurement& m: db.tracks[j].measurements) graph.emplace_shared(m.second, unit2, m.first, P(j)); } From a68b0798f9f63b49fd57243804fd49f7d8272a31 Mon Sep 17 00:00:00 2001 From: Sushmita Date: Wed, 21 Oct 2020 23:44:02 -0400 Subject: [PATCH 07/62] wrapped sfmtrack --- gtsam/gtsam.i | 21 +++------------------ gtsam/slam/dataset.h | 11 ++++------- python/gtsam/preamble.h | 4 ++-- python/gtsam/specializations.h | 4 ++-- 4 files changed, 11 insertions(+), 29 deletions(-) diff --git a/gtsam/gtsam.i b/gtsam/gtsam.i index 5334a233d..5307ef45d 100644 --- a/gtsam/gtsam.i +++ b/gtsam/gtsam.i @@ -2759,30 +2759,15 @@ virtual class EssentialMatrixFactor : gtsam::NoiseModelFactor { }; #include -class SfmMeasurement{ - SfmMeasurement(); - size_t i() const; - Point2 j() const; -}; -class SiftIndex{ - SiftIndex(); - size_t i() const; - size_t j() const; - }; -class SfmMeasurements{ - SfmMeasurements(); - -}; class SfmTrack { SfmTrack(); Point3 point3() const; size_t number_measurements() const; void setP(gtsam::Point3& p_); - gtsam::SfmMeasurement measurement(size_t idx) const; - gtsam::SiftIndex siftIndex(size_t idx) const; + pair measurement(size_t idx) const; + pair siftIndex(size_t idx) const; void add_measurement(const pair& m); - SfmMeasurements& measurements(); }; class SfmData { @@ -2792,7 +2777,7 @@ class SfmData { gtsam::PinholeCamera camera(size_t idx) const; gtsam::SfmTrack track(size_t idx) const; void add_track(const gtsam::SfmTrack& t) ; - void add_camera(const gtsam::SfmCamer& cam); + void add_camera(const gtsam::SfmCamera& cam); }; gtsam::SfmData readBal(string filename); diff --git a/gtsam/slam/dataset.h b/gtsam/slam/dataset.h index 062d8728c..1a01d8a38 100644 --- a/gtsam/slam/dataset.h +++ b/gtsam/slam/dataset.h @@ -228,7 +228,7 @@ struct SfmTrack { return measurements.size(); } /// Set 3D point - void setP(Point3& p_){ + void setP(const Point3& p_){ p = p_; } @@ -243,13 +243,10 @@ struct SfmTrack { Point3 point3() const { return p; } - void add_measurement(pair& m) const{ + void add_measurement(const pair& m) { measurements.push_back(m); } - SfmMeasurements& measurements() { - return measurements; - } }; @@ -278,11 +275,11 @@ struct SfmData { return tracks[idx]; } - void add_track(SfmTrack& t) const { + void add_track(const SfmTrack& t) { tracks.push_back(t); } - void add_camera(SfmCamera& cam) const{ + void add_camera(const SfmCamera& cam){ cameras.push_back(cam); } }; diff --git a/python/gtsam/preamble.h b/python/gtsam/preamble.h index 8525ed3a3..a8dfc5787 100644 --- a/python/gtsam/preamble.h +++ b/python/gtsam/preamble.h @@ -10,5 +10,5 @@ PYBIND11_MAKE_OPAQUE(std::vector); PYBIND11_MAKE_OPAQUE(std::vector > >); PYBIND11_MAKE_OPAQUE(std::vector > >); PYBIND11_MAKE_OPAQUE(std::vector); -PYBIND11_MAKE_OPAQUE(std::vector); -PYBIND11_MAKE_OPAQUE(std::vector); \ No newline at end of file +//PYBIND11_MAKE_OPAQUE(std::vector); +//PYBIND11_MAKE_OPAQUE(std::vector); \ No newline at end of file diff --git a/python/gtsam/specializations.h b/python/gtsam/specializations.h index 03e2b606c..2fd353b5f 100644 --- a/python/gtsam/specializations.h +++ b/python/gtsam/specializations.h @@ -13,5 +13,5 @@ py::bind_vector > >(m_, "Bina py::bind_map(m_, "IndexPairSetMap"); py::bind_vector(m_, "IndexPairVector"); py::bind_map(m_, "KeyPairDoubleMap"); -py::bind_vector >(m_, "Measurement"); -py::bind_vector >(m_, "SiftIndexVector"); \ No newline at end of file +//py::bind_vector >(m_, "Measurement"); +//py::bind_vector >(m_, "SiftIndexVector"); \ No newline at end of file From 6362b5648adebb3a480aaed4d317cf66835bce3f Mon Sep 17 00:00:00 2001 From: Sushmita Date: Thu, 22 Oct 2020 08:41:50 -0400 Subject: [PATCH 08/62] removed measurements from sfmdata --- gtsam/slam/dataset.h | 1 - 1 file changed, 1 deletion(-) diff --git a/gtsam/slam/dataset.h b/gtsam/slam/dataset.h index 1a01d8a38..c9c451660 100644 --- a/gtsam/slam/dataset.h +++ b/gtsam/slam/dataset.h @@ -256,7 +256,6 @@ typedef PinholeCamera SfmCamera; /// Define the structure for SfM data struct SfmData { - std::vector measurements; /// cameras; ///< Set of cameras std::vector tracks; ///< Sparse set of points size_t number_cameras() const { From 7aab3796b0f5cee7b6cd97ff9fbd40df1bea5f3d Mon Sep 17 00:00:00 2001 From: Jose Luis Blanco-Claraco Date: Fri, 23 Oct 2020 12:50:38 +0200 Subject: [PATCH 09/62] Add alternativeName() --- gtsam/inference/Symbol.h | 6 ++++++ gtsam/inference/tests/testKey.cpp | 19 +++++++++++++++++++ 2 files changed, 25 insertions(+) diff --git a/gtsam/inference/Symbol.h b/gtsam/inference/Symbol.h index 5be195db3..000a026c2 100644 --- a/gtsam/inference/Symbol.h +++ b/gtsam/inference/Symbol.h @@ -162,6 +162,12 @@ inline Key W(std::uint64_t j) { return Symbol('w', j); } inline Key X(std::uint64_t j) { return Symbol('x', j); } inline Key Y(std::uint64_t j) { return Symbol('y', j); } inline Key Z(std::uint64_t j) { return Symbol('z', j); } + +/** Generates symbol shorthands with alternative names different than the + * one-letter predefined ones. */ +inline std::function alternativeName(const char c) { + return [c](std::uint64_t j) { return gtsam::Symbol(c, j); }; +} } /// traits diff --git a/gtsam/inference/tests/testKey.cpp b/gtsam/inference/tests/testKey.cpp index a60258581..f34b77c14 100644 --- a/gtsam/inference/tests/testKey.cpp +++ b/gtsam/inference/tests/testKey.cpp @@ -40,6 +40,25 @@ TEST(Key, KeySymbolConversion) { EXPECT(assert_equal(original, actual)) } +/* ************************************************************************* */ +TEST(Key, SymbolAlternativeNames) { + const auto x1 = gtsam::symbol_shorthand::X(1); + const auto v1 = gtsam::symbol_shorthand::V(1); + const auto a1 = gtsam::symbol_shorthand::A(1); + + const auto Z = gtsam::symbol_shorthand::alternativeName('x'); + const auto DZ = gtsam::symbol_shorthand::alternativeName('v'); + const auto DDZ = gtsam::symbol_shorthand::alternativeName('a'); + + const auto z1 = Z(1); + const auto dz1 = DZ(1); + const auto ddz1 = DDZ(1); + + EXPECT(assert_equal(x1, z1)); + EXPECT(assert_equal(v1, dz1)); + EXPECT(assert_equal(a1, ddz1)); +} + /* ************************************************************************* */ template Key KeyTestValue(); From e3a28767edb7f86c39e0369a1910c488fa5aca8c Mon Sep 17 00:00:00 2001 From: Jose Luis Blanco Claraco Date: Sat, 24 Oct 2020 08:41:00 +0200 Subject: [PATCH 10/62] replaced lambda with class plus functor --- gtsam/inference/Symbol.h | 10 ++++++---- gtsam/inference/tests/testKey.cpp | 9 ++++----- 2 files changed, 10 insertions(+), 9 deletions(-) diff --git a/gtsam/inference/Symbol.h b/gtsam/inference/Symbol.h index 000a026c2..6dbb306db 100644 --- a/gtsam/inference/Symbol.h +++ b/gtsam/inference/Symbol.h @@ -165,13 +165,15 @@ inline Key Z(std::uint64_t j) { return Symbol('z', j); } /** Generates symbol shorthands with alternative names different than the * one-letter predefined ones. */ -inline std::function alternativeName(const char c) { - return [c](std::uint64_t j) { return gtsam::Symbol(c, j); }; -} +class SymbolGenerator { + const char c_; +public: + SymbolGenerator(const char c) : c_(c) {} + Symbol operator()(const std::uint64_t j) const { return Symbol(c_, j); } +}; } /// traits template<> struct traits : public Testable {}; } // \ namespace gtsam - diff --git a/gtsam/inference/tests/testKey.cpp b/gtsam/inference/tests/testKey.cpp index f34b77c14..124ba7652 100644 --- a/gtsam/inference/tests/testKey.cpp +++ b/gtsam/inference/tests/testKey.cpp @@ -41,14 +41,14 @@ TEST(Key, KeySymbolConversion) { } /* ************************************************************************* */ -TEST(Key, SymbolAlternativeNames) { +TEST(Key, SymbolGenerator) { const auto x1 = gtsam::symbol_shorthand::X(1); const auto v1 = gtsam::symbol_shorthand::V(1); const auto a1 = gtsam::symbol_shorthand::A(1); - const auto Z = gtsam::symbol_shorthand::alternativeName('x'); - const auto DZ = gtsam::symbol_shorthand::alternativeName('v'); - const auto DDZ = gtsam::symbol_shorthand::alternativeName('a'); + const auto Z = gtsam::symbol_shorthand::SymbolGenerator('x'); + const auto DZ = gtsam::symbol_shorthand::SymbolGenerator('v'); + const auto DDZ = gtsam::symbol_shorthand::SymbolGenerator('a'); const auto z1 = Z(1); const auto dz1 = DZ(1); @@ -125,4 +125,3 @@ int main() { return TestRegistry::runAllTests(tr); } /* ************************************************************************* */ - From 672635aad46ab69c4e7aaba8a223b7732359a0f8 Mon Sep 17 00:00:00 2001 From: Jose Luis Blanco Claraco Date: Sat, 24 Oct 2020 08:44:31 +0200 Subject: [PATCH 11/62] less verbose name path for SymbolGenerator --- gtsam/inference/Symbol.h | 2 +- gtsam/inference/tests/testKey.cpp | 6 +++--- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/gtsam/inference/Symbol.h b/gtsam/inference/Symbol.h index 6dbb306db..469082f16 100644 --- a/gtsam/inference/Symbol.h +++ b/gtsam/inference/Symbol.h @@ -162,6 +162,7 @@ inline Key W(std::uint64_t j) { return Symbol('w', j); } inline Key X(std::uint64_t j) { return Symbol('x', j); } inline Key Y(std::uint64_t j) { return Symbol('y', j); } inline Key Z(std::uint64_t j) { return Symbol('z', j); } +} /** Generates symbol shorthands with alternative names different than the * one-letter predefined ones. */ @@ -171,7 +172,6 @@ public: SymbolGenerator(const char c) : c_(c) {} Symbol operator()(const std::uint64_t j) const { return Symbol(c_, j); } }; -} /// traits template<> struct traits : public Testable {}; diff --git a/gtsam/inference/tests/testKey.cpp b/gtsam/inference/tests/testKey.cpp index 124ba7652..64674c36f 100644 --- a/gtsam/inference/tests/testKey.cpp +++ b/gtsam/inference/tests/testKey.cpp @@ -46,9 +46,9 @@ TEST(Key, SymbolGenerator) { const auto v1 = gtsam::symbol_shorthand::V(1); const auto a1 = gtsam::symbol_shorthand::A(1); - const auto Z = gtsam::symbol_shorthand::SymbolGenerator('x'); - const auto DZ = gtsam::symbol_shorthand::SymbolGenerator('v'); - const auto DDZ = gtsam::symbol_shorthand::SymbolGenerator('a'); + const auto Z = gtsam::SymbolGenerator('x'); + const auto DZ = gtsam::SymbolGenerator('v'); + const auto DDZ = gtsam::SymbolGenerator('a'); const auto z1 = Z(1); const auto dz1 = DZ(1); From 38010860e449f91634deab3bf8613498b9a3aca8 Mon Sep 17 00:00:00 2001 From: Sushmita Date: Sat, 24 Oct 2020 15:46:47 -0400 Subject: [PATCH 12/62] changed setP method name removed commented code --- gtsam/gtsam.i | 2 +- gtsam/slam/dataset.h | 14 ++++++-------- python/CMakeLists.txt | 3 --- python/gtsam/preamble.h | 4 +--- python/gtsam/specializations.h | 2 -- python/gtsam/tests/test_SfmData.py | 22 ++++++++++++---------- 6 files changed, 20 insertions(+), 27 deletions(-) diff --git a/gtsam/gtsam.i b/gtsam/gtsam.i index 5307ef45d..d6ae409f8 100644 --- a/gtsam/gtsam.i +++ b/gtsam/gtsam.i @@ -2764,7 +2764,7 @@ class SfmTrack { SfmTrack(); Point3 point3() const; size_t number_measurements() const; - void setP(gtsam::Point3& p_); + void set_point3(gtsam::Point3& p_); pair measurement(size_t idx) const; pair siftIndex(size_t idx) const; void add_measurement(const pair& m); diff --git a/gtsam/slam/dataset.h b/gtsam/slam/dataset.h index c9c451660..85b32ff5a 100644 --- a/gtsam/slam/dataset.h +++ b/gtsam/slam/dataset.h @@ -210,9 +210,8 @@ GTSAM_EXPORT GraphAndValues load3D(const std::string& filename); /// A measurement with its camera index typedef std::pair SfmMeasurement; -typedef std::vector SfmMeasurements; -/// SfmTrack +/// Sift index for SfmTrack typedef std::pair SiftIndex; /// Define the structure for the 3D points @@ -228,10 +227,9 @@ struct SfmTrack { return measurements.size(); } /// Set 3D point - void setP(const Point3& p_){ + void set_point3(const Point3& p_){ p = p_; } - /// Get the measurement (camera index, Point2) at pose index `idx` SfmMeasurement measurement(size_t idx) const { return measurements[idx]; @@ -240,14 +238,14 @@ struct SfmTrack { SiftIndex siftIndex(size_t idx) const { return siftIndices[idx]; } + /// Get 3D point Point3 point3() const { return p; } + /// Add measurement to track void add_measurement(const pair& m) { measurements.push_back(m); } - - }; @@ -273,11 +271,11 @@ struct SfmData { SfmTrack track(size_t idx) const { return tracks[idx]; } - + /// Add a track to SfmData void add_track(const SfmTrack& t) { tracks.push_back(t); } - + /// Add a camera to SfmData void add_camera(const SfmCamera& cam){ cameras.push_back(cam); } diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index 7d12b9800..00b537340 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -36,9 +36,6 @@ set(ignore gtsam::BetweenFactorPose3s gtsam::Point2Vector gtsam::Pose3Vector - gtsam::SfmMeasurement - gtsam::SfmCamera - gtsam::SiftIndex gtsam::KeyVector gtsam::BinaryMeasurementsUnit3 gtsam::KeyPairDoubleMap) diff --git a/python/gtsam/preamble.h b/python/gtsam/preamble.h index a8dfc5787..e31b5e6d8 100644 --- a/python/gtsam/preamble.h +++ b/python/gtsam/preamble.h @@ -9,6 +9,4 @@ PYBIND11_MAKE_OPAQUE(std::vector); PYBIND11_MAKE_OPAQUE(std::vector > >); PYBIND11_MAKE_OPAQUE(std::vector > >); -PYBIND11_MAKE_OPAQUE(std::vector); -//PYBIND11_MAKE_OPAQUE(std::vector); -//PYBIND11_MAKE_OPAQUE(std::vector); \ No newline at end of file +PYBIND11_MAKE_OPAQUE(std::vector); \ No newline at end of file diff --git a/python/gtsam/specializations.h b/python/gtsam/specializations.h index 2fd353b5f..cacad874c 100644 --- a/python/gtsam/specializations.h +++ b/python/gtsam/specializations.h @@ -13,5 +13,3 @@ py::bind_vector > >(m_, "Bina py::bind_map(m_, "IndexPairSetMap"); py::bind_vector(m_, "IndexPairVector"); py::bind_map(m_, "KeyPairDoubleMap"); -//py::bind_vector >(m_, "Measurement"); -//py::bind_vector >(m_, "SiftIndexVector"); \ No newline at end of file diff --git a/python/gtsam/tests/test_SfmData.py b/python/gtsam/tests/test_SfmData.py index 4664d746e..6263f19a0 100644 --- a/python/gtsam/tests/test_SfmData.py +++ b/python/gtsam/tests/test_SfmData.py @@ -32,11 +32,12 @@ class TestSfmData(GtsamTestCase): def test_tracks(self): """Test functions in SfmTrack""" # measurement is of format (camera_idx, imgPoint) - # create camera indices for two cameras - i1, i2 = np.random.randint(5), np.random.randint(5) - # create imgPoint for cameras i1 and i2 - uv_i1 = gtsam.Point2(np.random.randint(5), np.random.randint(5)) - uv_i2 = gtsam.Point2(np.random.randint(5), np.random.randint(5)) + # create arbitrary camera indices for two cameras + i1, i2 = 4,5 + # create arbitrary image measurements for cameras i1 and i2 + uv_i1 = gtsam.Point2(12.6, 82) + # translating point uv_i1 along X-axis + uv_i2 = gtsam.Point2(24.88, 82) m_i1 = (i1, uv_i1) m_i2 = (i2, uv_i2) # add measurements to the track @@ -47,7 +48,7 @@ class TestSfmData(GtsamTestCase): # camera_idx in the first measurement of the track corresponds to i1 self.assertEqual(self.tracks.measurement(0)[0], i1) # Set arbitrary 3D point corresponding to the track - self.tracks.setP(gtsam.Point3(2.5, 3.3, 1.2)) + self.tracks.set_point3(gtsam.Point3(2.5, 3.3, 1.2)) np.testing.assert_array_almost_equal( gtsam.Point3(2.5,3.3,1.2), self.tracks.point3() @@ -59,10 +60,11 @@ class TestSfmData(GtsamTestCase): #cam1 = gtsam.SfmCamera(1500, 1200, 0, 640, 480) # Create new track with 3 measurements track2 = gtsam.SfmTrack() - i1, i2, i3 = np.random.randint(5), np.random.randint(5), np.random.randint(5) - uv_i1 = gtsam.Point2(np.random.randint(5), np.random.randint(5)) - uv_i2 = gtsam.Point2(np.random.randint(5), np.random.randint(5)) - uv_i3 = gtsam.Point2(np.random.randint(5), np.random.randint(5)) + i1, i2, i3 = 3,5,6 + uv_i1 = gtsam.Point2(21.23, 45.64) + # translating along X-axis + uv_i2 = gtsam.Point2(45.7, 45.64) + uv_i3 = gtsam.Point2(68.35, 45.64) m_i1, m_i2, m_i3 = (i1, uv_i1), (i2, uv_i2), (i3, uv_i3) # add measurements to the track track2.add_measurement(m_i1) From a7b71cf203b88fdae9244c6faab3e1b12a96f6d2 Mon Sep 17 00:00:00 2001 From: Sushmita Date: Sat, 24 Oct 2020 19:06:35 -0400 Subject: [PATCH 13/62] remved commented code --- python/gtsam/tests/test_SfmData.py | 2 -- 1 file changed, 2 deletions(-) diff --git a/python/gtsam/tests/test_SfmData.py b/python/gtsam/tests/test_SfmData.py index 6263f19a0..aa93a69bb 100644 --- a/python/gtsam/tests/test_SfmData.py +++ b/python/gtsam/tests/test_SfmData.py @@ -17,7 +17,6 @@ import unittest import numpy as np import gtsam -#from gtsam import SfmCamera from gtsam.utils.test_case import GtsamTestCase @@ -57,7 +56,6 @@ class TestSfmData(GtsamTestCase): def test_data(self): """Test functions in SfmData""" - #cam1 = gtsam.SfmCamera(1500, 1200, 0, 640, 480) # Create new track with 3 measurements track2 = gtsam.SfmTrack() i1, i2, i3 = 3,5,6 From ee0eefbc86a20725a95ac4c8f97b3ae060a8da5e Mon Sep 17 00:00:00 2001 From: Sushmita Date: Tue, 27 Oct 2020 21:52:31 -0400 Subject: [PATCH 14/62] added new constructor and changed to emplace --- gtsam/gtsam.i | 6 +++--- gtsam/slam/dataset.h | 13 +++++-------- python/gtsam/tests/test_SfmData.py | 24 +++++++++++------------- 3 files changed, 19 insertions(+), 24 deletions(-) diff --git a/gtsam/gtsam.i b/gtsam/gtsam.i index d6ae409f8..dcacf14d9 100644 --- a/gtsam/gtsam.i +++ b/gtsam/gtsam.i @@ -2762,12 +2762,12 @@ virtual class EssentialMatrixFactor : gtsam::NoiseModelFactor { class SfmTrack { SfmTrack(); - Point3 point3() const; + SfmTrack(const gtsam::Point3& pt); + const Point3& point3() const; size_t number_measurements() const; - void set_point3(gtsam::Point3& p_); pair measurement(size_t idx) const; pair siftIndex(size_t idx) const; - void add_measurement(const pair& m); + void add_measurement(size_t idx, const gtsam::Point2& m); }; class SfmData { diff --git a/gtsam/slam/dataset.h b/gtsam/slam/dataset.h index 85b32ff5a..93bd2b2ee 100644 --- a/gtsam/slam/dataset.h +++ b/gtsam/slam/dataset.h @@ -217,6 +217,7 @@ typedef std::pair SiftIndex; /// Define the structure for the 3D points struct SfmTrack { SfmTrack(): p(0,0,0) {} + SfmTrack(const gtsam::Point3& pt) : p(pt) {} Point3 p; ///< 3D position of the point float r, g, b; ///< RGB color of the 3D point std::vector measurements; ///< The 2D image projections (id,(u,v)) @@ -226,10 +227,6 @@ struct SfmTrack { size_t number_measurements() const { return measurements.size(); } - /// Set 3D point - void set_point3(const Point3& p_){ - p = p_; - } /// Get the measurement (camera index, Point2) at pose index `idx` SfmMeasurement measurement(size_t idx) const { return measurements[idx]; @@ -239,12 +236,12 @@ struct SfmTrack { return siftIndices[idx]; } /// Get 3D point - Point3 point3() const { + const Point3& point3() const { return p; } - /// Add measurement to track - void add_measurement(const pair& m) { - measurements.push_back(m); + /// Add measurement (camera_idx, Point2) to track + void add_measurement(size_t idx, const gtsam::Point2& m) { + measurements.emplace_back(idx, m); } }; diff --git a/python/gtsam/tests/test_SfmData.py b/python/gtsam/tests/test_SfmData.py index aa93a69bb..51564fb6f 100644 --- a/python/gtsam/tests/test_SfmData.py +++ b/python/gtsam/tests/test_SfmData.py @@ -26,7 +26,8 @@ class TestSfmData(GtsamTestCase): def setUp(self): """Initialize SfmData and SfmTrack""" self.data = gtsam.SfmData() - self.tracks = gtsam.SfmTrack() + # initialize SfmTrack with 3D point + self.tracks = gtsam.SfmTrack(gtsam.Point3(2.5, 3.3, 1.2)) def test_tracks(self): """Test functions in SfmTrack""" @@ -37,17 +38,14 @@ class TestSfmData(GtsamTestCase): uv_i1 = gtsam.Point2(12.6, 82) # translating point uv_i1 along X-axis uv_i2 = gtsam.Point2(24.88, 82) - m_i1 = (i1, uv_i1) - m_i2 = (i2, uv_i2) # add measurements to the track - self.tracks.add_measurement(m_i1) - self.tracks.add_measurement(m_i2) + self.tracks.add_measurement(i1, uv_i1) + self.tracks.add_measurement(i2, uv_i2) # Number of measurements in the track is 2 self.assertEqual(self.tracks.number_measurements(), 2) # camera_idx in the first measurement of the track corresponds to i1 - self.assertEqual(self.tracks.measurement(0)[0], i1) - # Set arbitrary 3D point corresponding to the track - self.tracks.set_point3(gtsam.Point3(2.5, 3.3, 1.2)) + cam_idx, img_measurement = self.tracks.measurement(0) + self.assertEqual(cam_idx, i1) np.testing.assert_array_almost_equal( gtsam.Point3(2.5,3.3,1.2), self.tracks.point3() @@ -63,17 +61,17 @@ class TestSfmData(GtsamTestCase): # translating along X-axis uv_i2 = gtsam.Point2(45.7, 45.64) uv_i3 = gtsam.Point2(68.35, 45.64) - m_i1, m_i2, m_i3 = (i1, uv_i1), (i2, uv_i2), (i3, uv_i3) # add measurements to the track - track2.add_measurement(m_i1) - track2.add_measurement(m_i2) - track2.add_measurement(m_i3) + track2.add_measurement(i1, uv_i1) + track2.add_measurement(i2, uv_i2) + track2.add_measurement(i3, uv_i3) self.data.add_track(self.tracks) self.data.add_track(track2) # Number of tracks in SfmData is 2 self.assertEqual(self.data.number_tracks(), 2) # camera idx of first measurement of second track corresponds to i1 - self.assertEqual(self.data.track(1).measurement(0)[0], i1) + cam_idx, img_measurement = self.data.track(1).measurement(0) + self.assertEqual(cam_idx, i1) if __name__ == '__main__': unittest.main() From 1da968afd3d9122ed8d97dec96a2b4b8183a6cfe Mon Sep 17 00:00:00 2001 From: Jose Luis Blanco-Claraco Date: Wed, 28 Oct 2020 07:43:16 +0100 Subject: [PATCH 15/62] Automatic detection of correct suggest-override flag --- cmake/GtsamBuildTypes.cmake | 15 ++++++++++----- 1 file changed, 10 insertions(+), 5 deletions(-) diff --git a/cmake/GtsamBuildTypes.cmake b/cmake/GtsamBuildTypes.cmake index d13e3aa12..3155161be 100644 --- a/cmake/GtsamBuildTypes.cmake +++ b/cmake/GtsamBuildTypes.cmake @@ -1,3 +1,5 @@ +include(CheckCXXCompilerFlag) # for check_cxx_compiler_flag() + # Set cmake policy to recognize the AppleClang compiler # independently from the Clang compiler. if(POLICY CMP0025) @@ -105,11 +107,14 @@ if(MSVC) else() # Common to all configurations, next for each configuration: - if ( - ((CMAKE_CXX_COMPILER_ID MATCHES "Clang") AND (NOT CMAKE_CXX_COMPILER_VERSION VERSION_LESS 12.0.0)) OR - (CMAKE_CXX_COMPILER_ID MATCHES "GNU") - ) - # set(flag_override_ -Wsuggest-override) # -Werror=suggest-override: Add again someday + if (NOT MSVC) + check_cxx_compiler_flag(-Wsuggest-override COMPILER_HAS_WSUGGEST_OVERRIDE) + check_cxx_compiler_flag(-Wmissing COMPILER_HAS_WMISSING_OVERRIDE) + if (COMPILER_HAS_WSUGGEST_OVERRIDE) + set(flag_override_ -Wsuggest-override) # -Werror=suggest-override: Add again someday + elseif(COMPILER_HAS_WMISSING_OVERRIDE) + set(flag_override_ -Wmissing-override) # -Werror=missing-override: Add again someday + endif() endif() set(GTSAM_COMPILE_OPTIONS_PRIVATE_COMMON From 405771397ba3938f094f7d023e75ccbb9a8e3ea3 Mon Sep 17 00:00:00 2001 From: Tim McGrath Date: Sat, 31 Oct 2020 13:40:05 -0400 Subject: [PATCH 16/62] adding additional Unit3 support in the wrapper: PriorFactorUnit3, Values::insert/update/at(Unit3) --- gtsam/gtsam.i | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/gtsam/gtsam.i b/gtsam/gtsam.i index 575a216d6..b578d134d 100644 --- a/gtsam/gtsam.i +++ b/gtsam/gtsam.i @@ -2157,6 +2157,7 @@ class Values { void insert(size_t j, const gtsam::SOn& P); void insert(size_t j, const gtsam::Rot3& rot3); void insert(size_t j, const gtsam::Pose3& pose3); + void insert(size_t j, const gtsam::Unit3& unit3); void insert(size_t j, const gtsam::Cal3_S2& cal3_s2); void insert(size_t j, const gtsam::Cal3DS2& cal3ds2); void insert(size_t j, const gtsam::Cal3Bundler& cal3bundler); @@ -2175,6 +2176,7 @@ class Values { void update(size_t j, const gtsam::SOn& P); void update(size_t j, const gtsam::Rot3& rot3); void update(size_t j, const gtsam::Pose3& pose3); + void update(size_t j, const gtsam::Unit3& unit3); void update(size_t j, const gtsam::Cal3_S2& cal3_s2); void update(size_t j, const gtsam::Cal3DS2& cal3ds2); void update(size_t j, const gtsam::Cal3Bundler& cal3bundler); @@ -2186,7 +2188,7 @@ class Values { void update(size_t j, Vector vector); void update(size_t j, Matrix matrix); - template, gtsam::imuBias::ConstantBias, gtsam::NavState, Vector, Matrix}> + template, gtsam::imuBias::ConstantBias, gtsam::NavState, Vector, Matrix}> T at(size_t j); /// version for double @@ -2529,7 +2531,7 @@ class NonlinearISAM { #include #include -template}> +template}> virtual class PriorFactor : gtsam::NoiseModelFactor { PriorFactor(size_t key, const T& prior, const gtsam::noiseModel::Base* noiseModel); T prior() const; From 65a6d06bf170206779ab82b301a0b95354ee07f0 Mon Sep 17 00:00:00 2001 From: Sushmita Date: Sun, 1 Nov 2020 21:29:38 -0500 Subject: [PATCH 17/62] sfmtrack constructor changed to accept point --- python/gtsam/preamble.h | 2 +- python/gtsam/tests/test_SfmData.py | 10 ++++++---- 2 files changed, 7 insertions(+), 5 deletions(-) diff --git a/python/gtsam/preamble.h b/python/gtsam/preamble.h index e31b5e6d8..6166f615e 100644 --- a/python/gtsam/preamble.h +++ b/python/gtsam/preamble.h @@ -9,4 +9,4 @@ PYBIND11_MAKE_OPAQUE(std::vector); PYBIND11_MAKE_OPAQUE(std::vector > >); PYBIND11_MAKE_OPAQUE(std::vector > >); -PYBIND11_MAKE_OPAQUE(std::vector); \ No newline at end of file +PYBIND11_MAKE_OPAQUE(std::vector); diff --git a/python/gtsam/tests/test_SfmData.py b/python/gtsam/tests/test_SfmData.py index 51564fb6f..9c965ddc5 100644 --- a/python/gtsam/tests/test_SfmData.py +++ b/python/gtsam/tests/test_SfmData.py @@ -27,7 +27,7 @@ class TestSfmData(GtsamTestCase): """Initialize SfmData and SfmTrack""" self.data = gtsam.SfmData() # initialize SfmTrack with 3D point - self.tracks = gtsam.SfmTrack(gtsam.Point3(2.5, 3.3, 1.2)) + self.tracks = gtsam.SfmTrack() def test_tracks(self): """Test functions in SfmTrack""" @@ -47,7 +47,7 @@ class TestSfmData(GtsamTestCase): cam_idx, img_measurement = self.tracks.measurement(0) self.assertEqual(cam_idx, i1) np.testing.assert_array_almost_equal( - gtsam.Point3(2.5,3.3,1.2), + gtsam.Point3(0.,0.,0.), self.tracks.point3() ) @@ -55,13 +55,15 @@ class TestSfmData(GtsamTestCase): def test_data(self): """Test functions in SfmData""" # Create new track with 3 measurements - track2 = gtsam.SfmTrack() i1, i2, i3 = 3,5,6 uv_i1 = gtsam.Point2(21.23, 45.64) # translating along X-axis uv_i2 = gtsam.Point2(45.7, 45.64) uv_i3 = gtsam.Point2(68.35, 45.64) - # add measurements to the track + # add measurements and arbitrary point to the track + measurements = [(i1, uv_i1), (i2, uv_i2), (i3, uv_i3)] + pt = gtsam.Point3(1.0, 6.0, 2.0) + track2 = gtsam.SfmTrack(pt) track2.add_measurement(i1, uv_i1) track2.add_measurement(i2, uv_i2) track2.add_measurement(i3, uv_i3) From 63ca1c1f5eec82cc04737611469f3ad69b486f16 Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Mon, 2 Nov 2020 10:46:36 -0500 Subject: [PATCH 18/62] Attempt to fix spooky boost in Homebrew --- .github/workflows/build-macos.yml | 4 ++-- .github/workflows/build-python.yml | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/.github/workflows/build-macos.yml b/.github/workflows/build-macos.yml index 363cd690f..b2b71ea7d 100644 --- a/.github/workflows/build-macos.yml +++ b/.github/workflows/build-macos.yml @@ -35,7 +35,7 @@ jobs: - name: Install (macOS) if: runner.os == 'macOS' run: | - brew install cmake ninja boost + brew install cmake ninja boost@1.60 if [ "${{ matrix.compiler }}" = "gcc" ]; then brew install gcc@${{ matrix.version }} echo "::set-env name=CC::gcc-${{ matrix.version }}" @@ -48,4 +48,4 @@ jobs: - name: Build and Test (macOS) if: runner.os == 'macOS' run: | - bash .github/scripts/unix.sh -t \ No newline at end of file + bash .github/scripts/unix.sh -t diff --git a/.github/workflows/build-python.yml b/.github/workflows/build-python.yml index fbec491f2..6c90b8bf7 100644 --- a/.github/workflows/build-python.yml +++ b/.github/workflows/build-python.yml @@ -87,7 +87,7 @@ jobs: - name: Install (macOS) if: runner.os == 'macOS' run: | - brew install cmake ninja boost + brew install cmake ninja boost@1.60 if [ "${{ matrix.compiler }}" = "gcc" ]; then brew install gcc@${{ matrix.version }} echo "::set-env name=CC::gcc-${{ matrix.version }}" @@ -109,4 +109,4 @@ jobs: - name: Build (macOS) if: runner.os == 'macOS' run: | - bash .github/scripts/python.sh \ No newline at end of file + bash .github/scripts/python.sh From d8089c71322ad1c75f92f0b2de4d2fb286ac27a7 Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Mon, 2 Nov 2020 11:01:43 -0500 Subject: [PATCH 19/62] Use my tap --- .github/workflows/build-macos.yml | 3 ++- .github/workflows/build-python.yml | 3 ++- 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/.github/workflows/build-macos.yml b/.github/workflows/build-macos.yml index b2b71ea7d..fd6fb8d0b 100644 --- a/.github/workflows/build-macos.yml +++ b/.github/workflows/build-macos.yml @@ -35,7 +35,8 @@ jobs: - name: Install (macOS) if: runner.os == 'macOS' run: | - brew install cmake ninja boost@1.60 + brew tap ProfFan/robotics + brew install cmake ninja boost@1.68 if [ "${{ matrix.compiler }}" = "gcc" ]; then brew install gcc@${{ matrix.version }} echo "::set-env name=CC::gcc-${{ matrix.version }}" diff --git a/.github/workflows/build-python.yml b/.github/workflows/build-python.yml index 6c90b8bf7..5f79ae90d 100644 --- a/.github/workflows/build-python.yml +++ b/.github/workflows/build-python.yml @@ -87,7 +87,8 @@ jobs: - name: Install (macOS) if: runner.os == 'macOS' run: | - brew install cmake ninja boost@1.60 + brew tap ProfFan/robotics + brew install cmake ninja boost@1.68 if [ "${{ matrix.compiler }}" = "gcc" ]; then brew install gcc@${{ matrix.version }} echo "::set-env name=CC::gcc-${{ matrix.version }}" From 1ee5650fc774157f824267741852a0b2ec43f919 Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Mon, 2 Nov 2020 11:12:52 -0500 Subject: [PATCH 20/62] Use my tap --- .github/workflows/build-macos.yml | 4 ++-- .github/workflows/build-python.yml | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/.github/workflows/build-macos.yml b/.github/workflows/build-macos.yml index fd6fb8d0b..9dca41b84 100644 --- a/.github/workflows/build-macos.yml +++ b/.github/workflows/build-macos.yml @@ -35,8 +35,8 @@ jobs: - name: Install (macOS) if: runner.os == 'macOS' run: | - brew tap ProfFan/robotics - brew install cmake ninja boost@1.68 + brew tap ProfFan/my + brew install cmake ninja boost@1.59 if [ "${{ matrix.compiler }}" = "gcc" ]; then brew install gcc@${{ matrix.version }} echo "::set-env name=CC::gcc-${{ matrix.version }}" diff --git a/.github/workflows/build-python.yml b/.github/workflows/build-python.yml index 5f79ae90d..ef7c30a62 100644 --- a/.github/workflows/build-python.yml +++ b/.github/workflows/build-python.yml @@ -87,8 +87,8 @@ jobs: - name: Install (macOS) if: runner.os == 'macOS' run: | - brew tap ProfFan/robotics - brew install cmake ninja boost@1.68 + brew tap ProfFan/my + brew install cmake ninja boost@1.59 if [ "${{ matrix.compiler }}" = "gcc" ]; then brew install gcc@${{ matrix.version }} echo "::set-env name=CC::gcc-${{ matrix.version }}" From dddc97c487c2abe91254ac19a4cc759278d7a349 Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Mon, 2 Nov 2020 11:20:48 -0500 Subject: [PATCH 21/62] Use explict tap name --- .github/workflows/build-macos.yml | 2 +- .github/workflows/build-python.yml | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/.github/workflows/build-macos.yml b/.github/workflows/build-macos.yml index 9dca41b84..c9ccfbfc3 100644 --- a/.github/workflows/build-macos.yml +++ b/.github/workflows/build-macos.yml @@ -36,7 +36,7 @@ jobs: if: runner.os == 'macOS' run: | brew tap ProfFan/my - brew install cmake ninja boost@1.59 + brew install cmake ninja ProfFan/my/boost@1.59 if [ "${{ matrix.compiler }}" = "gcc" ]; then brew install gcc@${{ matrix.version }} echo "::set-env name=CC::gcc-${{ matrix.version }}" diff --git a/.github/workflows/build-python.yml b/.github/workflows/build-python.yml index ef7c30a62..f75701215 100644 --- a/.github/workflows/build-python.yml +++ b/.github/workflows/build-python.yml @@ -88,7 +88,7 @@ jobs: if: runner.os == 'macOS' run: | brew tap ProfFan/my - brew install cmake ninja boost@1.59 + brew install cmake ninja ProfFan/my/boost@1.59 if [ "${{ matrix.compiler }}" = "gcc" ]; then brew install gcc@${{ matrix.version }} echo "::set-env name=CC::gcc-${{ matrix.version }}" From 315380db03f25eec0d05ad6eabeecef0b541e920 Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Mon, 2 Nov 2020 11:28:49 -0500 Subject: [PATCH 22/62] Last resort: use the explict 1.73 formula --- .github/workflows/build-macos.yml | 4 ++-- .github/workflows/build-python.yml | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/.github/workflows/build-macos.yml b/.github/workflows/build-macos.yml index c9ccfbfc3..212924cc7 100644 --- a/.github/workflows/build-macos.yml +++ b/.github/workflows/build-macos.yml @@ -35,8 +35,8 @@ jobs: - name: Install (macOS) if: runner.os == 'macOS' run: | - brew tap ProfFan/my - brew install cmake ninja ProfFan/my/boost@1.59 + brew install cmake ninja + brew install https://github.com/Homebrew/homebrew-core/raw/0687baaf14e7b16c383d101fcc3b954cfb05ffe8/Formula/boost.rb if [ "${{ matrix.compiler }}" = "gcc" ]; then brew install gcc@${{ matrix.version }} echo "::set-env name=CC::gcc-${{ matrix.version }}" diff --git a/.github/workflows/build-python.yml b/.github/workflows/build-python.yml index f75701215..f6930a8c4 100644 --- a/.github/workflows/build-python.yml +++ b/.github/workflows/build-python.yml @@ -87,8 +87,8 @@ jobs: - name: Install (macOS) if: runner.os == 'macOS' run: | - brew tap ProfFan/my - brew install cmake ninja ProfFan/my/boost@1.59 + brew install cmake ninja + brew install https://github.com/Homebrew/homebrew-core/raw/0687baaf14e7b16c383d101fcc3b954cfb05ffe8/Formula/boost.rb if [ "${{ matrix.compiler }}" = "gcc" ]; then brew install gcc@${{ matrix.version }} echo "::set-env name=CC::gcc-${{ matrix.version }}" From 325b868e8b55f186567de398d85c61b2400cfa80 Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Mon, 2 Nov 2020 14:42:21 -0500 Subject: [PATCH 23/62] Use my tap --- .github/workflows/build-macos.yml | 3 ++- .github/workflows/build-python.yml | 3 ++- 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/.github/workflows/build-macos.yml b/.github/workflows/build-macos.yml index 212924cc7..3ce98055a 100644 --- a/.github/workflows/build-macos.yml +++ b/.github/workflows/build-macos.yml @@ -35,8 +35,9 @@ jobs: - name: Install (macOS) if: runner.os == 'macOS' run: | + brew tap ProfFan/robotics brew install cmake ninja - brew install https://github.com/Homebrew/homebrew-core/raw/0687baaf14e7b16c383d101fcc3b954cfb05ffe8/Formula/boost.rb + brew install ProfFan/robotics/boost if [ "${{ matrix.compiler }}" = "gcc" ]; then brew install gcc@${{ matrix.version }} echo "::set-env name=CC::gcc-${{ matrix.version }}" diff --git a/.github/workflows/build-python.yml b/.github/workflows/build-python.yml index f6930a8c4..8255fb8ab 100644 --- a/.github/workflows/build-python.yml +++ b/.github/workflows/build-python.yml @@ -87,8 +87,9 @@ jobs: - name: Install (macOS) if: runner.os == 'macOS' run: | + brew tap ProfFan/robotics brew install cmake ninja - brew install https://github.com/Homebrew/homebrew-core/raw/0687baaf14e7b16c383d101fcc3b954cfb05ffe8/Formula/boost.rb + brew install ProfFan/robotics/boost if [ "${{ matrix.compiler }}" = "gcc" ]; then brew install gcc@${{ matrix.version }} echo "::set-env name=CC::gcc-${{ matrix.version }}" From 51b6fd0b43852bf14148b39c14d2563f2d87c3d4 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 3 Nov 2020 12:11:17 -0500 Subject: [PATCH 24/62] Added flag for absolute error --- gtsam/base/Matrix.h | 2 +- gtsam/base/Vector.cpp | 6 ++++-- gtsam/base/Vector.h | 3 ++- gtsam/geometry/tests/testRot3.cpp | 6 +++--- 4 files changed, 10 insertions(+), 7 deletions(-) diff --git a/gtsam/base/Matrix.h b/gtsam/base/Matrix.h index 071c33fca..9adf4e1c1 100644 --- a/gtsam/base/Matrix.h +++ b/gtsam/base/Matrix.h @@ -90,7 +90,7 @@ bool equal_with_abs_tol(const Eigen::DenseBase& A, const Eigen::DenseBas for(size_t i=0; i::max())) { + else if (abs(a - b) <= + tol * min(larger, std::numeric_limits::max()) && + !absolute) { return true; } diff --git a/gtsam/base/Vector.h b/gtsam/base/Vector.h index 319ad6ee6..b0fc74f26 100644 --- a/gtsam/base/Vector.h +++ b/gtsam/base/Vector.h @@ -87,7 +87,8 @@ static_assert( * * Return true if two numbers are close wrt tol. */ -GTSAM_EXPORT bool fpEqual(double a, double b, double tol); +GTSAM_EXPORT bool fpEqual(double a, double b, double tol, + bool absolute = false); /** * print without optional string, must specify cout yourself diff --git a/gtsam/geometry/tests/testRot3.cpp b/gtsam/geometry/tests/testRot3.cpp index 7b792f8bd..889f68580 100644 --- a/gtsam/geometry/tests/testRot3.cpp +++ b/gtsam/geometry/tests/testRot3.cpp @@ -807,15 +807,15 @@ TEST(Rot3, RQ_derivative) { test_xyz.push_back(VecAndErr{{0, 0, 0}, error}); test_xyz.push_back(VecAndErr{{0, 0.5, -0.5}, error}); test_xyz.push_back(VecAndErr{{0.3, 0, 0.2}, error}); - test_xyz.push_back(VecAndErr{{-0.6, 1.3, 0}, error}); + test_xyz.push_back(VecAndErr{{-0.6, 1.3, 0}, 1e-8}); test_xyz.push_back(VecAndErr{{1.0, 0.7, 0.8}, error}); test_xyz.push_back(VecAndErr{{3.0, 0.7, -0.6}, error}); test_xyz.push_back(VecAndErr{{M_PI / 2, 0, 0}, error}); test_xyz.push_back(VecAndErr{{0, 0, M_PI / 2}, error}); // Test close to singularity - test_xyz.push_back(VecAndErr{{0, M_PI / 2 - 1e-1, 0}, 1e-8}); - test_xyz.push_back(VecAndErr{{0, 3 * M_PI / 2 + 1e-1, 0}, 1e-8}); + test_xyz.push_back(VecAndErr{{0, M_PI / 2 - 1e-1, 0}, 1e-7}); + test_xyz.push_back(VecAndErr{{0, 3 * M_PI / 2 + 1e-1, 0}, 1e-7}); test_xyz.push_back(VecAndErr{{0, M_PI / 2 - 1.1e-2, 0}, 1e-4}); test_xyz.push_back(VecAndErr{{0, 3 * M_PI / 2 + 1.1e-2, 0}, 1e-4}); From c68a64745e1ad99cb047286434cfda1e1b7aedc7 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 3 Nov 2020 14:27:05 -0500 Subject: [PATCH 25/62] add test --- gtsam/base/tests/testMatrix.cpp | 13 +++++++++++++ 1 file changed, 13 insertions(+) diff --git a/gtsam/base/tests/testMatrix.cpp b/gtsam/base/tests/testMatrix.cpp index 468e842f2..15087093a 100644 --- a/gtsam/base/tests/testMatrix.cpp +++ b/gtsam/base/tests/testMatrix.cpp @@ -1163,6 +1163,19 @@ TEST(Matrix , IsVectorSpace) { BOOST_CONCEPT_ASSERT((IsVectorSpace)); } +TEST(Matrix, AbsoluteError) { + double a = 2000, b = 1997, tol=1e-1; + bool isEqual; + + // Test absolute error + isEqual = fpEqual(a, b, tol, true); + EXPECT(isEqual == false); + + // Test relative error + isEqual = fpEqual(a, b, tol, false); + EXPECT(isEqual == true); +} + /* ************************************************************************* */ int main() { TestResult tr; From 326957b0d35ece7ae44cf5d06be42798c9f577dd Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 3 Nov 2020 19:31:39 -0500 Subject: [PATCH 26/62] cleaner assertion --- gtsam/base/tests/testMatrix.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam/base/tests/testMatrix.cpp b/gtsam/base/tests/testMatrix.cpp index 15087093a..d22ffc0db 100644 --- a/gtsam/base/tests/testMatrix.cpp +++ b/gtsam/base/tests/testMatrix.cpp @@ -1169,11 +1169,11 @@ TEST(Matrix, AbsoluteError) { // Test absolute error isEqual = fpEqual(a, b, tol, true); - EXPECT(isEqual == false); + EXPECT(!isEqual); // Test relative error isEqual = fpEqual(a, b, tol, false); - EXPECT(isEqual == true); + EXPECT(isEqual); } /* ************************************************************************* */ From 6c6cb965d88318623653454aac28b572aeef776b Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 4 Nov 2020 12:12:10 -0500 Subject: [PATCH 27/62] Consistent interface for pixel center (#579) * defined u0 and v0 in all camera models for consistent interface * deprecate u0/v0 and use px/py everywhere * Use deprecation macro * fix various issues --- gtsam/geometry/Cal3Bundler.h | 12 ++++++++++++ gtsam/gtsam.i | 14 ++++++++++++-- gtsam/nonlinear/tests/testAdaptAutoDiff.cpp | 2 +- gtsam/slam/dataset.cpp | 4 ++-- 4 files changed, 27 insertions(+), 5 deletions(-) diff --git a/gtsam/geometry/Cal3Bundler.h b/gtsam/geometry/Cal3Bundler.h index 96765f899..2e3fab002 100644 --- a/gtsam/geometry/Cal3Bundler.h +++ b/gtsam/geometry/Cal3Bundler.h @@ -98,6 +98,17 @@ public: return k2_; } + /// image center in x + inline double px() const { + return u0_; + } + + /// image center in y + inline double py() const { + return v0_; + } + +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V41 /// get parameter u0 inline double u0() const { return u0_; @@ -107,6 +118,7 @@ public: inline double v0() const { return v0_; } +#endif /** diff --git a/gtsam/gtsam.i b/gtsam/gtsam.i index 575a216d6..8ed93868f 100644 --- a/gtsam/gtsam.i +++ b/gtsam/gtsam.i @@ -597,6 +597,7 @@ class Rot3 { Rot3(double R11, double R12, double R13, double R21, double R22, double R23, double R31, double R32, double R33); + Rot3(double w, double x, double y, double z); static gtsam::Rot3 Rx(double t); static gtsam::Rot3 Ry(double t); @@ -980,8 +981,8 @@ class Cal3Bundler { double fy() const; double k1() const; double k2() const; - double u0() const; - double v0() const; + double px() const; + double py() const; Vector vector() const; Vector k() const; Matrix K() const; @@ -2761,7 +2762,16 @@ virtual class EssentialMatrixFactor : gtsam::NoiseModelFactor { }; #include + class SfmTrack { + SfmTrack(); + + double r; + double g; + double b; + // TODO Need to close wrap#10 to allow this to work. + // std::vector> measurements; + Point3 point3() const; size_t number_measurements() const; pair measurement(size_t idx) const; diff --git a/gtsam/nonlinear/tests/testAdaptAutoDiff.cpp b/gtsam/nonlinear/tests/testAdaptAutoDiff.cpp index 2f4f21286..1423b473e 100644 --- a/gtsam/nonlinear/tests/testAdaptAutoDiff.cpp +++ b/gtsam/nonlinear/tests/testAdaptAutoDiff.cpp @@ -41,7 +41,7 @@ struct Cal3Bundler0 : public Cal3Bundler { double v0 = 0) : Cal3Bundler(f, k1, k2, u0, v0) {} Cal3Bundler0 retract(const Vector& d) const { - return Cal3Bundler0(fx() + d(0), k1() + d(1), k2() + d(2), u0(), v0()); + return Cal3Bundler0(fx() + d(0), k1() + d(1), k2() + d(2), px(), py()); } Vector3 localCoordinates(const Cal3Bundler0& T2) const { return T2.vector() - vector(); diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp index 270dbeb95..74e074c9e 100644 --- a/gtsam/slam/dataset.cpp +++ b/gtsam/slam/dataset.cpp @@ -1164,8 +1164,8 @@ bool writeBAL(const string &filename, SfmData &data) { for (size_t k = 0; k < track.number_measurements(); k++) { // for each observation of the 3D point j size_t i = track.measurements[k].first; // camera id - double u0 = data.cameras[i].calibration().u0(); - double v0 = data.cameras[i].calibration().v0(); + double u0 = data.cameras[i].calibration().px(); + double v0 = data.cameras[i].calibration().py(); if (u0 != 0 || v0 != 0) { cout << "writeBAL has not been tested for calibration with nonzero " From acd6fff5623e33ddd2a086ec6074c737097693c7 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sat, 7 Nov 2020 17:28:50 -0500 Subject: [PATCH 28/62] minor typo fixes --- gtsam/geometry/Rot3.h | 2 +- wrap/cmake/PybindWrap.cmake | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h index 2334312f6..abd74e063 100644 --- a/gtsam/geometry/Rot3.h +++ b/gtsam/geometry/Rot3.h @@ -526,7 +526,7 @@ namespace gtsam { /** * @brief Spherical Linear intERPolation between *this and other - * @param s a value between 0 and 1 + * @param t a value between 0 and 1 * @param other final point of iterpolation geodesic on manifold */ Rot3 slerp(double t, const Rot3& other) const; diff --git a/wrap/cmake/PybindWrap.cmake b/wrap/cmake/PybindWrap.cmake index 85f956d50..ff69b5aed 100644 --- a/wrap/cmake/PybindWrap.cmake +++ b/wrap/cmake/PybindWrap.cmake @@ -146,7 +146,7 @@ function(install_python_scripts else() set(build_type_tag "") endif() - # Split up filename to strip trailing '/' in WRAP_CYTHON_INSTALL_PATH if + # Split up filename to strip trailing '/' in GTSAM_PY_INSTALL_PATH if # there is one get_filename_component(location "${dest_directory}" PATH) get_filename_component(name "${dest_directory}" NAME) From bb313eb7ced9ec799c909611e604139e626cae5f Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sat, 7 Nov 2020 18:19:04 -0500 Subject: [PATCH 29/62] Fix python discovery for MATLAB wrapper --- cmake/HandlePython.cmake | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/cmake/HandlePython.cmake b/cmake/HandlePython.cmake index ac7401906..e5d55b451 100644 --- a/cmake/HandlePython.cmake +++ b/cmake/HandlePython.cmake @@ -1,4 +1,5 @@ -if(GTSAM_BUILD_PYTHON) +# Set Python version if either Python or MATLAB wrapper is requested. +if(GTSAM_BUILD_PYTHON OR GTSAM_INSTALL_MATLAB_TOOLBOX) if(${GTSAM_PYTHON_VERSION} STREQUAL "Default") # Get info about the Python3 interpreter # https://cmake.org/cmake/help/latest/module/FindPython3.html#module:FindPython3 @@ -14,7 +15,9 @@ if(GTSAM_BUILD_PYTHON) "The version of Python to build the wrappers against." FORCE) endif() +endif() +if(GTSAM_BUILD_PYTHON) if(GTSAM_UNSTABLE_BUILD_PYTHON) if (NOT GTSAM_BUILD_UNSTABLE) message(WARNING "GTSAM_UNSTABLE_BUILD_PYTHON requires the unstable module to be enabled.") From c5c54da588f4cb8150437e417c3141c64e714cd8 Mon Sep 17 00:00:00 2001 From: Jose Luis Blanco-Claraco Date: Sun, 8 Nov 2020 11:07:50 +0100 Subject: [PATCH 30/62] Avoid redundant calls to error() --- gtsam/nonlinear/NonlinearOptimizer.cpp | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/gtsam/nonlinear/NonlinearOptimizer.cpp b/gtsam/nonlinear/NonlinearOptimizer.cpp index 9a9c487b6..fd9961742 100644 --- a/gtsam/nonlinear/NonlinearOptimizer.cpp +++ b/gtsam/nonlinear/NonlinearOptimizer.cpp @@ -88,20 +88,24 @@ void NonlinearOptimizer::defaultOptimize() { } // Iterative loop + double newError = currentError; // used to avoid repeated calls to error() do { // Do next iteration - currentError = error(); // TODO(frank): don't do this twice at first !? Computed above! + currentError = newError; iterate(); tictoc_finishedIteration(); + // Update newError for either printouts or conditional-end checks: + newError = error(); + // Maybe show output if (params.verbosity >= NonlinearOptimizerParams::VALUES) values().print("newValues"); if (params.verbosity >= NonlinearOptimizerParams::ERROR) - cout << "newError: " << error() << endl; + cout << "newError: " << newError << endl; } while (iterations() < params.maxIterations && !checkConvergence(params.relativeErrorTol, params.absoluteErrorTol, params.errorTol, - currentError, error(), params.verbosity) && std::isfinite(currentError)); + currentError, newError, params.verbosity) && std::isfinite(currentError)); // Printing if verbose if (params.verbosity >= NonlinearOptimizerParams::TERMINATION) { From c5576c534f3a29f0077c58858321829da859dd6c Mon Sep 17 00:00:00 2001 From: Jose Luis Blanco-Claraco Date: Sun, 8 Nov 2020 11:57:47 +0100 Subject: [PATCH 31/62] Add iteration hook in non-linear optimizers --- .../NonlinearConjugateGradientOptimizer.h | 4 ++ gtsam/nonlinear/NonlinearOptimizer.cpp | 6 ++- gtsam/nonlinear/NonlinearOptimizer.h | 2 +- gtsam/nonlinear/NonlinearOptimizerParams.h | 30 ++++++----- tests/testNonlinearOptimizer.cpp | 52 +++++++++++++++++++ 5 files changed, 80 insertions(+), 14 deletions(-) diff --git a/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.h b/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.h index abf6b257a..a85f27425 100644 --- a/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.h +++ b/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.h @@ -200,6 +200,10 @@ boost::tuple nonlinearConjugateGradient(const S &system, currentValues = system.advance(prevValues, alpha, direction); currentError = system.error(currentValues); + // User hook: + if (params.iterationHook) + params.iterationHook(iteration, prevError, currentError); + // Maybe show output if (params.verbosity >= NonlinearOptimizerParams::ERROR) std::cout << "iteration: " << iteration << ", currentError: " << currentError << std::endl; diff --git a/gtsam/nonlinear/NonlinearOptimizer.cpp b/gtsam/nonlinear/NonlinearOptimizer.cpp index fd9961742..0d7e9e17f 100644 --- a/gtsam/nonlinear/NonlinearOptimizer.cpp +++ b/gtsam/nonlinear/NonlinearOptimizer.cpp @@ -97,7 +97,11 @@ void NonlinearOptimizer::defaultOptimize() { // Update newError for either printouts or conditional-end checks: newError = error(); - + + // User hook: + if (params.iterationHook) + params.iterationHook(iterations(), currentError, newError); + // Maybe show output if (params.verbosity >= NonlinearOptimizerParams::VALUES) values().print("newValues"); diff --git a/gtsam/nonlinear/NonlinearOptimizer.h b/gtsam/nonlinear/NonlinearOptimizer.h index 9935f44ce..6fe369dd3 100644 --- a/gtsam/nonlinear/NonlinearOptimizer.h +++ b/gtsam/nonlinear/NonlinearOptimizer.h @@ -81,7 +81,7 @@ protected: public: /** A shared pointer to this class */ - typedef boost::shared_ptr shared_ptr; + using shared_ptr = boost::shared_ptr; /// @name Standard interface /// @{ diff --git a/gtsam/nonlinear/NonlinearOptimizerParams.h b/gtsam/nonlinear/NonlinearOptimizerParams.h index 65fdd1c92..53805f5f0 100644 --- a/gtsam/nonlinear/NonlinearOptimizerParams.h +++ b/gtsam/nonlinear/NonlinearOptimizerParams.h @@ -33,22 +33,19 @@ namespace gtsam { */ class GTSAM_EXPORT NonlinearOptimizerParams { public: + NonlinearOptimizerParams() = default; + /** See NonlinearOptimizerParams::verbosity */ enum Verbosity { SILENT, TERMINATION, ERROR, VALUES, DELTA, LINEAR }; - size_t maxIterations; ///< The maximum iterations to stop iterating (default 100) - double relativeErrorTol; ///< The maximum relative error decrease to stop iterating (default 1e-5) - double absoluteErrorTol; ///< The maximum absolute error decrease to stop iterating (default 1e-5) - double errorTol; ///< The maximum total error to stop iterating (default 0.0) - Verbosity verbosity; ///< The printing verbosity during optimization (default SILENT) - Ordering::OrderingType orderingType; ///< The method of ordering use during variable elimination (default COLAMD) - - NonlinearOptimizerParams() : - maxIterations(100), relativeErrorTol(1e-5), absoluteErrorTol(1e-5), errorTol( - 0.0), verbosity(SILENT), orderingType(Ordering::COLAMD), - linearSolverType(MULTIFRONTAL_CHOLESKY) {} + size_t maxIterations = 100; ///< The maximum iterations to stop iterating (default 100) + double relativeErrorTol = 1e-5; ///< The maximum relative error decrease to stop iterating (default 1e-5) + double absoluteErrorTol = 1e-5; ///< The maximum absolute error decrease to stop iterating (default 1e-5) + double errorTol = 0.0; ///< The maximum total error to stop iterating (default 0.0) + Verbosity verbosity = SILENT; ///< The printing verbosity during optimization (default SILENT) + Ordering::OrderingType orderingType = Ordering::COLAMD; ///< The method of ordering use during variable elimination (default COLAMD) virtual ~NonlinearOptimizerParams() { } @@ -71,6 +68,15 @@ public: static Verbosity verbosityTranslator(const std::string &s) ; static std::string verbosityTranslator(Verbosity value) ; + /** Type for an optional user-provided hook to be called after each + * internal optimizer iteration */ + using IterationHook = std::function< + void(size_t /*iteration*/, double/*errorBefore*/, double/*errorAfter*/)>; + + /** Optional user-provided iteration hook to be called after each + * optimization iteration (Default: empty) */ + IterationHook iterationHook; + /** See NonlinearOptimizerParams::linearSolverType */ enum LinearSolverType { MULTIFRONTAL_CHOLESKY, @@ -81,7 +87,7 @@ public: CHOLMOD, /* Experimental Flag */ }; - LinearSolverType linearSolverType; ///< The type of linear solver to use in the nonlinear optimizer + LinearSolverType linearSolverType = MULTIFRONTAL_CHOLESKY; ///< The type of linear solver to use in the nonlinear optimizer boost::optional ordering; ///< The optional variable elimination ordering, or empty to use COLAMD (default: empty) IterativeOptimizationParameters::shared_ptr iterativeParams; ///< The container for iterativeOptimization parameters. used in CG Solvers. diff --git a/tests/testNonlinearOptimizer.cpp b/tests/testNonlinearOptimizer.cpp index dc19801a2..6415174d5 100644 --- a/tests/testNonlinearOptimizer.cpp +++ b/tests/testNonlinearOptimizer.cpp @@ -566,6 +566,58 @@ TEST( NonlinearOptimizer, logfile ) // EXPECT(actual.str()==expected.str()); } +/* ************************************************************************* */ +TEST( NonlinearOptimizer, iterationHook_LM ) +{ + NonlinearFactorGraph fg(example::createReallyNonlinearFactorGraph()); + + Point2 x0(3,3); + Values c0; + c0.insert(X(1), x0); + + // Levenberg-Marquardt + LevenbergMarquardtParams lmParams; + size_t lastIterCalled = 0; + lmParams.iterationHook = [&](size_t iteration, double oldError, double newError) + { + // Tests: + lastIterCalled = iteration; + EXPECT(newError " << newError <<"\n"; + }; + LevenbergMarquardtOptimizer(fg, c0, lmParams).optimize(); + + EXPECT(lastIterCalled>5); +} +/* ************************************************************************* */ +TEST( NonlinearOptimizer, iterationHook_CG ) +{ + NonlinearFactorGraph fg(example::createReallyNonlinearFactorGraph()); + + Point2 x0(3,3); + Values c0; + c0.insert(X(1), x0); + + // Levenberg-Marquardt + NonlinearConjugateGradientOptimizer::Parameters cgParams; + size_t lastIterCalled = 0; + cgParams.iterationHook = [&](size_t iteration, double oldError, double newError) + { + // Tests: + lastIterCalled = iteration; + EXPECT(newError " << newError <<"\n"; + }; + NonlinearConjugateGradientOptimizer(fg, c0, cgParams).optimize(); + + EXPECT(lastIterCalled>5); +} + + /* ************************************************************************* */ //// Minimal traits example struct MyType : public Vector3 { From 426ad93812b03c8988a4dc8f2e70026c4a43bb38 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 8 Nov 2020 14:47:47 -0500 Subject: [PATCH 32/62] Use new env files for github actions --- .github/workflows/build-linux.yml | 12 ++++++------ .github/workflows/build-macos.yml | 8 ++++---- .github/workflows/build-python.yml | 18 +++++++++--------- .github/workflows/build-special.yml | 26 +++++++++++++------------- .github/workflows/build-windows.yml | 14 +++++++------- 5 files changed, 39 insertions(+), 39 deletions(-) diff --git a/.github/workflows/build-linux.yml b/.github/workflows/build-linux.yml index 87a0430f8..62b81bdf5 100644 --- a/.github/workflows/build-linux.yml +++ b/.github/workflows/build-linux.yml @@ -63,17 +63,17 @@ jobs: sudo apt install cmake build-essential pkg-config libpython-dev python-numpy - echo "::set-env name=BOOST_ROOT::$(echo $BOOST_ROOT_1_69_0)" - echo "::set-env name=LD_LIBRARY_PATH::$(echo $BOOST_ROOT_1_69_0/lib)" + echo "BOOST_ROOT=$(echo $BOOST_ROOT_1_69_0)" >> $GITHUB_ENV + echo "LD_LIBRARY_PATH=$(echo $BOOST_ROOT_1_69_0/lib)" >> $GITHUB_ENV if [ "${{ matrix.compiler }}" = "gcc" ]; then sudo apt-get install -y g++-${{ matrix.version }} g++-${{ matrix.version }}-multilib - echo "::set-env name=CC::gcc-${{ matrix.version }}" - echo "::set-env name=CXX::g++-${{ matrix.version }}" + echo "CC=gcc-${{ matrix.version }}" >> $GITHUB_ENV + echo "CXX=g++-${{ matrix.version }}" >> $GITHUB_ENV else sudo apt-get install -y clang-${{ matrix.version }} g++-multilib - echo "::set-env name=CC::clang-${{ matrix.version }}" - echo "::set-env name=CXX::clang++-${{ matrix.version }}" + echo "CC=clang-${{ matrix.version }}" >> $GITHUB_ENV + echo "CXX=clang++-${{ matrix.version }}" >> $GITHUB_ENV fi - name: Check Boost version if: runner.os == 'Linux' diff --git a/.github/workflows/build-macos.yml b/.github/workflows/build-macos.yml index 3ce98055a..69873980a 100644 --- a/.github/workflows/build-macos.yml +++ b/.github/workflows/build-macos.yml @@ -40,12 +40,12 @@ jobs: brew install ProfFan/robotics/boost if [ "${{ matrix.compiler }}" = "gcc" ]; then brew install gcc@${{ matrix.version }} - echo "::set-env name=CC::gcc-${{ matrix.version }}" - echo "::set-env name=CXX::g++-${{ matrix.version }}" + echo "CC=gcc-${{ matrix.version }}" >> $GITHUB_ENV + echo "CXX=g++-${{ matrix.version }}" >> $GITHUB_ENV else sudo xcode-select -switch /Applications/Xcode_${{ matrix.version }}.app - echo "::set-env name=CC::clang" - echo "::set-env name=CXX::clang++" + echo "CC=clang" >> $GITHUB_ENV + echo "CXX=clang++" >> $GITHUB_ENV fi - name: Build and Test (macOS) if: runner.os == 'macOS' diff --git a/.github/workflows/build-python.yml b/.github/workflows/build-python.yml index 8255fb8ab..97b68f6b7 100644 --- a/.github/workflows/build-python.yml +++ b/.github/workflows/build-python.yml @@ -77,12 +77,12 @@ jobs: if [ "${{ matrix.compiler }}" = "gcc" ]; then sudo apt-get install -y g++-${{ matrix.version }} g++-${{ matrix.version }}-multilib - echo "::set-env name=CC::gcc-${{ matrix.version }}" - echo "::set-env name=CXX::g++-${{ matrix.version }}" + echo "CC=gcc-${{ matrix.version }}" >> $GITHUB_ENV + echo "CXX=g++-${{ matrix.version }}" >> $GITHUB_ENV else sudo apt-get install -y clang-${{ matrix.version }} g++-multilib - echo "::set-env name=CC::clang-${{ matrix.version }}" - echo "::set-env name=CXX::clang++-${{ matrix.version }}" + echo "CC=clang-${{ matrix.version }}" >> $GITHUB_ENV + echo "CXX=clang++-${{ matrix.version }}" >> $GITHUB_ENV fi - name: Install (macOS) if: runner.os == 'macOS' @@ -92,17 +92,17 @@ jobs: brew install ProfFan/robotics/boost if [ "${{ matrix.compiler }}" = "gcc" ]; then brew install gcc@${{ matrix.version }} - echo "::set-env name=CC::gcc-${{ matrix.version }}" - echo "::set-env name=CXX::g++-${{ matrix.version }}" + echo "CC=gcc-${{ matrix.version }}" >> $GITHUB_ENV + echo "CXX=g++-${{ matrix.version }}" >> $GITHUB_ENV else sudo xcode-select -switch /Applications/Xcode_${{ matrix.version }}.app - echo "::set-env name=CC::clang" - echo "::set-env name=CXX::clang++" + echo "CC=clang" >> $GITHUB_ENV + echo "CXX=clang++" >> $GITHUB_ENV fi - name: Set GTSAM_WITH_TBB Flag if: matrix.flag == 'tbb' run: | - echo "::set-env name=GTSAM_WITH_TBB::ON" + echo "GTSAM_WITH_TBB=ON" >> $GITHUB_ENV echo "GTSAM Uses TBB" - name: Build (Linux) if: runner.os == 'Linux' diff --git a/.github/workflows/build-special.yml b/.github/workflows/build-special.yml index c314acb16..70f3f40ec 100644 --- a/.github/workflows/build-special.yml +++ b/.github/workflows/build-special.yml @@ -64,17 +64,17 @@ jobs: sudo apt install cmake build-essential pkg-config libpython-dev python-numpy - echo "::set-env name=BOOST_ROOT::$(echo $BOOST_ROOT_1_69_0)" - echo "::set-env name=LD_LIBRARY_PATH::$(echo $BOOST_ROOT_1_69_0/lib)" + echo "BOOST_ROOT=$(echo $BOOST_ROOT_1_69_0)" >> $GITHUB_ENV + echo "LD_LIBRARY_PATH=$(echo $BOOST_ROOT_1_69_0/lib)" >> $GITHUB_ENV if [ "${{ matrix.compiler }}" = "gcc" ]; then sudo apt-get install -y g++-${{ matrix.version }} g++-${{ matrix.version }}-multilib - echo "::set-env name=CC::gcc-${{ matrix.version }}" - echo "::set-env name=CXX::g++-${{ matrix.version }}" + echo "CC=gcc-${{ matrix.version }}" >> $GITHUB_ENV + echo "CXX=g++-${{ matrix.version }}" >> $GITHUB_ENV else sudo apt-get install -y clang-${{ matrix.version }} g++-multilib - echo "::set-env name=CC::clang-${{ matrix.version }}" - echo "::set-env name=CXX::clang++-${{ matrix.version }}" + echo "CC=clang-${{ matrix.version }}" >> $GITHUB_ENV + echo "CXX=clang++-${{ matrix.version }}" >> $GITHUB_ENV fi - name: Install (macOS) @@ -83,30 +83,30 @@ jobs: brew install cmake ninja boost if [ "${{ matrix.compiler }}" = "gcc" ]; then brew install gcc@${{ matrix.version }} - echo "::set-env name=CC::gcc-${{ matrix.version }}" - echo "::set-env name=CXX::g++-${{ matrix.version }}" + echo "CC=gcc-${{ matrix.version }}" >> $GITHUB_ENV + echo "CXX=g++-${{ matrix.version }}" >> $GITHUB_ENV else sudo xcode-select -switch /Applications/Xcode_${{ matrix.version }}.app - echo "::set-env name=CC::clang" - echo "::set-env name=CXX::clang++" + echo "CC=clang" >> $GITHUB_ENV + echo "CXX=clang++" >> $GITHUB_ENV fi - name: Set Allow Deprecated Flag if: matrix.flag == 'deprecated' run: | - echo "::set-env name=GTSAM_ALLOW_DEPRECATED_SINCE_V41::ON" + echo "GTSAM_ALLOW_DEPRECATED_SINCE_V41=ON" >> $GITHUB_ENV echo "Allow deprecated since version 4.1" - name: Set Use Quaternions Flag if: matrix.flag == 'quaternions' run: | - echo "::set-env name=GTSAM_USE_QUATERNIONS::ON" + echo "GTSAM_USE_QUATERNIONS=ON" >> $GITHUB_ENV echo "Use Quaternions for rotations" - name: Set GTSAM_WITH_TBB Flag if: matrix.flag == 'tbb' run: | - echo "::set-env name=GTSAM_WITH_TBB::ON" + echo "GTSAM_WITH_TBB=ON" >> $GITHUB_ENV echo "GTSAM Uses TBB" - name: Build & Test diff --git a/.github/workflows/build-windows.yml b/.github/workflows/build-windows.yml index b3150a751..432f7490d 100644 --- a/.github/workflows/build-windows.yml +++ b/.github/workflows/build-windows.yml @@ -50,17 +50,17 @@ jobs: # See: https://github.com/DaanDeMeyer/doctest/runs/231595515 # See: https://github.community/t5/GitHub-Actions/Something-is-wrong-with-the-chocolatey-installed-version-of-gcc/td-p/32413 scoop install gcc --global - echo "::set-env name=CC::gcc" - echo "::set-env name=CXX::g++" + echo "CC=gcc" >> $GITHUB_ENV + echo "CXX=g++" >> $GITHUB_ENV } elseif ("${{ matrix.compiler }}" -eq "clang") { - echo "::set-env name=CC::clang" - echo "::set-env name=CXX::clang++" + echo "CC=clang" >> $GITHUB_ENV + echo "CXX=clang++" >> $GITHUB_ENV } else { - echo "::set-env name=CC::${{ matrix.compiler }}" - echo "::set-env name=CXX::${{ matrix.compiler }}" + echo "CC=${{ matrix.compiler }}" >> $GITHUB_ENV + echo "CXX=${{ matrix.compiler }}" >> $GITHUB_ENV } # Scoop modifies the PATH so we make the modified PATH global. - echo "::set-env name=PATH::$env:PATH" + echo "$env" >> $GITHUB_PATH - name: Build (Windows) if: runner.os == 'Windows' run: | From 86cea10b558f7911563656b5fefaadf3c1dc2cff Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 10 Nov 2020 17:21:49 -0500 Subject: [PATCH 33/62] ubuntu with gcc-9 in CI --- .github/workflows/build-python.yml | 11 +++++------ 1 file changed, 5 insertions(+), 6 deletions(-) diff --git a/.github/workflows/build-python.yml b/.github/workflows/build-python.yml index 8255fb8ab..6cedf0079 100644 --- a/.github/workflows/build-python.yml +++ b/.github/workflows/build-python.yml @@ -19,7 +19,7 @@ jobs: # See https://help.github.com/en/articles/workflow-syntax-for-github-actions. name: [ ubuntu-18.04-gcc-5, - # ubuntu-18.04-gcc-9, # TODO Disabled for now because of timeouts + ubuntu-18.04-gcc-9, ubuntu-18.04-clang-9, macOS-10.15-xcode-11.3.1, ubuntu-18.04-gcc-5-tbb, @@ -33,11 +33,10 @@ jobs: compiler: gcc version: "5" - # TODO Disabled for now because of timeouts - # - name: ubuntu-18.04-gcc-9 - # os: ubuntu-18.04 - # compiler: gcc - # version: "9" + - name: ubuntu-18.04-gcc-9 + os: ubuntu-18.04 + compiler: gcc + version: "9" - name: ubuntu-18.04-clang-9 os: ubuntu-18.04 From a04cd2cea1b257264784bfd5462c505d59f87aa3 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 10 Nov 2020 19:33:39 -0500 Subject: [PATCH 34/62] fix path setting for Windows --- .github/workflows/build-windows.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/build-windows.yml b/.github/workflows/build-windows.yml index 432f7490d..920613745 100644 --- a/.github/workflows/build-windows.yml +++ b/.github/workflows/build-windows.yml @@ -60,7 +60,7 @@ jobs: echo "CXX=${{ matrix.compiler }}" >> $GITHUB_ENV } # Scoop modifies the PATH so we make the modified PATH global. - echo "$env" >> $GITHUB_PATH + echo "$env:PATH" >> $GITHUB_PATH - name: Build (Windows) if: runner.os == 'Windows' run: | From 2315df694aff7e648d2e22a478685946e7de4f24 Mon Sep 17 00:00:00 2001 From: Martin Vonheim Larsen Date: Wed, 11 Nov 2020 12:42:19 +0100 Subject: [PATCH 35/62] Docs: Use https for mathjax --- doc/Doxyfile.in | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/doc/Doxyfile.in b/doc/Doxyfile.in index d5c969a8a..fd7f4e5f6 100644 --- a/doc/Doxyfile.in +++ b/doc/Doxyfile.in @@ -1188,7 +1188,7 @@ USE_MATHJAX = YES # MathJax, but it is strongly recommended to install a local copy of MathJax # before deployment. -MATHJAX_RELPATH = http://cdn.mathjax.org/mathjax/latest +MATHJAX_RELPATH = https://cdn.mathjax.org/mathjax/latest # The MATHJAX_EXTENSIONS tag can be used to specify one or MathJax extension # names that should be enabled during MathJax rendering. From c2455082dbb342567181d9126b37ed41214e7a52 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 11 Nov 2020 13:34:40 -0500 Subject: [PATCH 36/62] Force EXPMAP option for both if either POSE3 or ROT3 is set --- cmake/HandleGeneralOptions.cmake | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/cmake/HandleGeneralOptions.cmake b/cmake/HandleGeneralOptions.cmake index 27d73bd86..4bc74804c 100644 --- a/cmake/HandleGeneralOptions.cmake +++ b/cmake/HandleGeneralOptions.cmake @@ -31,6 +31,13 @@ if(NOT MSVC AND NOT XCODE_VERSION) option(GTSAM_BUILD_WITH_CCACHE "Use ccache compiler cache" ON) endif() +# Enable GTSAM_ROT3_EXPMAP if GTSAM_POSE3_EXPMAP is enabled, and vice versa. +if(GTSAM_POSE3_EXPMAP) + set(GTSAM_ROT3_EXPMAP 1 CACHE BOOL "" FORCE) +elseif(GTSAM_ROT3_EXPMAP) + set(GTSAM_POSE3_EXPMAP 1 CACHE BOOL "" FORCE) +endif() + # Options relating to MATLAB wrapper # TODO: Check for matlab mex binary before handling building of binaries option(GTSAM_INSTALL_MATLAB_TOOLBOX "Enable/Disable installation of matlab toolbox" OFF) From ddc0d136002c6f3c060691dc4dbdeddf48535bef Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 11 Nov 2020 13:34:53 -0500 Subject: [PATCH 37/62] uncomment tests --- gtsam/geometry/tests/testPose3.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/gtsam/geometry/tests/testPose3.cpp b/gtsam/geometry/tests/testPose3.cpp index 9639ffbcf..8fafcb25f 100644 --- a/gtsam/geometry/tests/testPose3.cpp +++ b/gtsam/geometry/tests/testPose3.cpp @@ -906,9 +906,9 @@ TEST(Pose3 , ChartDerivatives) { Pose3 id; if (ROT3_DEFAULT_COORDINATES_MODE == Rot3::EXPMAP) { CHECK_CHART_DERIVATIVES(id,id); -// CHECK_CHART_DERIVATIVES(id,T2); -// CHECK_CHART_DERIVATIVES(T2,id); -// CHECK_CHART_DERIVATIVES(T2,T3); + CHECK_CHART_DERIVATIVES(id,T2); + CHECK_CHART_DERIVATIVES(T2,id); + CHECK_CHART_DERIVATIVES(T2,T3); } } From b5284e4b3fb6933c4443212ba695d1c151961e74 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 11 Nov 2020 22:46:16 -0500 Subject: [PATCH 38/62] Improved CayleyChart Local --- gtsam/geometry/Rot3M.cpp | 18 +++++++----------- 1 file changed, 7 insertions(+), 11 deletions(-) diff --git a/gtsam/geometry/Rot3M.cpp b/gtsam/geometry/Rot3M.cpp index 02e5b771f..eb1bd6461 100644 --- a/gtsam/geometry/Rot3M.cpp +++ b/gtsam/geometry/Rot3M.cpp @@ -176,17 +176,13 @@ Vector3 Rot3::CayleyChart::Local(const Rot3& R, OptionalJacobian<3,3> H) { if (H) throw std::runtime_error("Rot3::CayleyChart::Local Derivative"); // Create a fixed-size matrix Matrix3 A = R.matrix(); - // Mathematica closed form optimization (procrastination?) gone wild: - const double a = A(0, 0), b = A(0, 1), c = A(0, 2); - const double d = A(1, 0), e = A(1, 1), f = A(1, 2); - const double g = A(2, 0), h = A(2, 1), i = A(2, 2); - const double di = d * i, ce = c * e, cd = c * d, fg = f * g; - const double M = 1 + e - f * h + i + e * i; - const double K = -4.0 / (cd * h + M + a * M - g * (c + ce) - b * (d + di - fg)); - const double x = a * f - cd + f; - const double y = b * f - ce - c; - const double z = fg - di - d; - return K * Vector3(x, y, z); + const Matrix3 P = A + I_3x3; + // Check if (A+I) is invertible. Same as checking for -1 eigenvalue. + if (P.determinant() == 0.0) { + throw std::runtime_error("Rot3::CayleyChart::Local Invalid Rotation"); + } + Matrix3 Pinv = (A + I_3x3).inverse(); + return SO3::Vee(Pinv * (A - I_3x3)) * 2; } /* ************************************************************************* */ From 098e7045110d64db1da2424c847c23a62ba056ce Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 11 Nov 2020 22:46:39 -0500 Subject: [PATCH 39/62] Similarity3 test only for Rot3 Expmap --- gtsam_unstable/geometry/tests/testSimilarity3.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/gtsam_unstable/geometry/tests/testSimilarity3.cpp b/gtsam_unstable/geometry/tests/testSimilarity3.cpp index b985eb374..937761f02 100644 --- a/gtsam_unstable/geometry/tests/testSimilarity3.cpp +++ b/gtsam_unstable/geometry/tests/testSimilarity3.cpp @@ -361,8 +361,12 @@ TEST(Similarity3, AlignPose3) { vector correspondences{bTa1, bTa2}; + // Cayley transform cannot accommodate 180 degree rotations, + // hence we only test for Expmap +#ifdef GTSAM_ROT3_EXPMAP Similarity3 actual_aSb = Similarity3::Align(correspondences); EXPECT(assert_equal(expected_aSb, actual_aSb)); +#endif } //****************************************************************************** From 40b5f31cbdf23a199309b725018b5635b8ddbd2c Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 12 Nov 2020 05:42:52 -0500 Subject: [PATCH 40/62] comment out Windows build which is timing out --- .github/workflows/build-windows.yml | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/.github/workflows/build-windows.yml b/.github/workflows/build-windows.yml index 920613745..74dbbed19 100644 --- a/.github/workflows/build-windows.yml +++ b/.github/workflows/build-windows.yml @@ -25,9 +25,11 @@ jobs: build_type: [Debug, Release] build_unstable: [ON] include: - - name: windows-2016-cl - os: windows-2016 - compiler: cl + + #TODO This build keeps timing out, need to understand why. + # - name: windows-2016-cl + # os: windows-2016 + # compiler: cl - name: windows-2019-cl os: windows-2019 From 9d261626d7a63b8394c9c7feab92d56fcb6c9057 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 12 Nov 2020 06:12:29 -0500 Subject: [PATCH 41/62] Update Boost since 1.69.0 has been deprecated in CI images --- .github/workflows/build-linux.yml | 4 ++-- .github/workflows/build-special.yml | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/.github/workflows/build-linux.yml b/.github/workflows/build-linux.yml index 62b81bdf5..d80a7f4ba 100644 --- a/.github/workflows/build-linux.yml +++ b/.github/workflows/build-linux.yml @@ -63,8 +63,8 @@ jobs: sudo apt install cmake build-essential pkg-config libpython-dev python-numpy - echo "BOOST_ROOT=$(echo $BOOST_ROOT_1_69_0)" >> $GITHUB_ENV - echo "LD_LIBRARY_PATH=$(echo $BOOST_ROOT_1_69_0/lib)" >> $GITHUB_ENV + echo "BOOST_ROOT=$(echo $BOOST_ROOT_1_72_0)" >> $GITHUB_ENV + echo "LD_LIBRARY_PATH=$(echo $BOOST_ROOT_1_72_0/lib)" >> $GITHUB_ENV if [ "${{ matrix.compiler }}" = "gcc" ]; then sudo apt-get install -y g++-${{ matrix.version }} g++-${{ matrix.version }}-multilib diff --git a/.github/workflows/build-special.yml b/.github/workflows/build-special.yml index 70f3f40ec..715f25733 100644 --- a/.github/workflows/build-special.yml +++ b/.github/workflows/build-special.yml @@ -64,8 +64,8 @@ jobs: sudo apt install cmake build-essential pkg-config libpython-dev python-numpy - echo "BOOST_ROOT=$(echo $BOOST_ROOT_1_69_0)" >> $GITHUB_ENV - echo "LD_LIBRARY_PATH=$(echo $BOOST_ROOT_1_69_0/lib)" >> $GITHUB_ENV + echo "BOOST_ROOT=$(echo $BOOST_ROOT_1_72_0)" >> $GITHUB_ENV + echo "LD_LIBRARY_PATH=$(echo $BOOST_ROOT_1_72_0/lib)" >> $GITHUB_ENV if [ "${{ matrix.compiler }}" = "gcc" ]; then sudo apt-get install -y g++-${{ matrix.version }} g++-${{ matrix.version }}-multilib From f2b6a0446a173fdbca525dcdca7004bc1129351a Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 12 Nov 2020 07:14:30 -0500 Subject: [PATCH 42/62] Comment out Windows 2016 since it times out --- .github/workflows/build-windows.yml | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/.github/workflows/build-windows.yml b/.github/workflows/build-windows.yml index 74dbbed19..0a55de880 100644 --- a/.github/workflows/build-windows.yml +++ b/.github/workflows/build-windows.yml @@ -18,7 +18,8 @@ jobs: # Github Actions requires a single row to be added to the build matrix. # See https://help.github.com/en/articles/workflow-syntax-for-github-actions. name: [ - windows-2016-cl, + #TODO This build keeps timing out, need to understand why. + # windows-2016-cl, windows-2019-cl, ] From 6bf410c0d8a9180c715d1a87789c0e20ab41bd07 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 12 Nov 2020 10:57:40 -0500 Subject: [PATCH 43/62] Revert "Improved CayleyChart Local" This reverts commit b5284e4b3fb6933c4443212ba695d1c151961e74. --- gtsam/geometry/Rot3M.cpp | 18 +++++++++++------- 1 file changed, 11 insertions(+), 7 deletions(-) diff --git a/gtsam/geometry/Rot3M.cpp b/gtsam/geometry/Rot3M.cpp index eb1bd6461..02e5b771f 100644 --- a/gtsam/geometry/Rot3M.cpp +++ b/gtsam/geometry/Rot3M.cpp @@ -176,13 +176,17 @@ Vector3 Rot3::CayleyChart::Local(const Rot3& R, OptionalJacobian<3,3> H) { if (H) throw std::runtime_error("Rot3::CayleyChart::Local Derivative"); // Create a fixed-size matrix Matrix3 A = R.matrix(); - const Matrix3 P = A + I_3x3; - // Check if (A+I) is invertible. Same as checking for -1 eigenvalue. - if (P.determinant() == 0.0) { - throw std::runtime_error("Rot3::CayleyChart::Local Invalid Rotation"); - } - Matrix3 Pinv = (A + I_3x3).inverse(); - return SO3::Vee(Pinv * (A - I_3x3)) * 2; + // Mathematica closed form optimization (procrastination?) gone wild: + const double a = A(0, 0), b = A(0, 1), c = A(0, 2); + const double d = A(1, 0), e = A(1, 1), f = A(1, 2); + const double g = A(2, 0), h = A(2, 1), i = A(2, 2); + const double di = d * i, ce = c * e, cd = c * d, fg = f * g; + const double M = 1 + e - f * h + i + e * i; + const double K = -4.0 / (cd * h + M + a * M - g * (c + ce) - b * (d + di - fg)); + const double x = a * f - cd + f; + const double y = b * f - ce - c; + const double z = fg - di - d; + return K * Vector3(x, y, z); } /* ************************************************************************* */ From 14ef7db636bfd1fc5a16f3dff41fcc7bb0df4a62 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 12 Nov 2020 11:14:54 -0500 Subject: [PATCH 44/62] Use older and faster Cayley transform but add det check and docs --- gtsam/geometry/Rot3M.cpp | 12 +++++++++++- 1 file changed, 11 insertions(+), 1 deletion(-) diff --git a/gtsam/geometry/Rot3M.cpp b/gtsam/geometry/Rot3M.cpp index 02e5b771f..67b774a74 100644 --- a/gtsam/geometry/Rot3M.cpp +++ b/gtsam/geometry/Rot3M.cpp @@ -176,7 +176,17 @@ Vector3 Rot3::CayleyChart::Local(const Rot3& R, OptionalJacobian<3,3> H) { if (H) throw std::runtime_error("Rot3::CayleyChart::Local Derivative"); // Create a fixed-size matrix Matrix3 A = R.matrix(); - // Mathematica closed form optimization (procrastination?) gone wild: + + // Check if (A+I) is invertible. Same as checking for -1 eigenvalue. + if ((A + I_3x3).determinant() == 0.0) { + throw std::runtime_error("Rot3::CayleyChart::Local Invalid Rotation"); + } + + // Mathematica closed form optimization. + // The following are the essential computations for the following algorithm + // 1. Compute the inverse of P = (A+I), using a closed-form formula since P is 3x3 + // 2. Compute the Cayley transform C = P^{-1} * (A-I) + // 3. C is skew-symmetric, so we pick out the computations corresponding only to x, y, and z. const double a = A(0, 0), b = A(0, 1), c = A(0, 2); const double d = A(1, 0), e = A(1, 1), f = A(1, 2); const double g = A(2, 0), h = A(2, 1), i = A(2, 2); From a94b6dacaf9309dfc125a9b44f5eb71911d0da83 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 12 Nov 2020 11:15:50 -0500 Subject: [PATCH 45/62] Print message when either Pose3 or Rot3 expmap is ON --- cmake/HandleGeneralOptions.cmake | 2 ++ 1 file changed, 2 insertions(+) diff --git a/cmake/HandleGeneralOptions.cmake b/cmake/HandleGeneralOptions.cmake index 4bc74804c..85a529e49 100644 --- a/cmake/HandleGeneralOptions.cmake +++ b/cmake/HandleGeneralOptions.cmake @@ -33,8 +33,10 @@ endif() # Enable GTSAM_ROT3_EXPMAP if GTSAM_POSE3_EXPMAP is enabled, and vice versa. if(GTSAM_POSE3_EXPMAP) + message(STATUS "GTSAM_POSE3_EXPMAP=ON, enabling GTSAM_ROT3_EXPMAP as well") set(GTSAM_ROT3_EXPMAP 1 CACHE BOOL "" FORCE) elseif(GTSAM_ROT3_EXPMAP) + message(STATUS "GTSAM_ROT3_EXPMAP=ON, enabling GTSAM_POSE3_EXPMAP as well") set(GTSAM_POSE3_EXPMAP 1 CACHE BOOL "" FORCE) endif() From 4e3ac1b8398a66e1824947d7a1a25ae21821d362 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 12 Nov 2020 11:16:50 -0500 Subject: [PATCH 46/62] CI path for Cayley transform --- .github/scripts/unix.sh | 2 ++ .github/workflows/build-special.yml | 14 ++++++++++++++ 2 files changed, 16 insertions(+) diff --git a/.github/scripts/unix.sh b/.github/scripts/unix.sh index f85e67dc1..55a8ac372 100644 --- a/.github/scripts/unix.sh +++ b/.github/scripts/unix.sh @@ -66,6 +66,8 @@ function configure() -DGTSAM_BUILD_EXAMPLES_ALWAYS=${GTSAM_BUILD_EXAMPLES_ALWAYS:-ON} \ -DGTSAM_ALLOW_DEPRECATED_SINCE_V41=${GTSAM_ALLOW_DEPRECATED_SINCE_V41:-OFF} \ -DGTSAM_USE_QUATERNIONS=${GTSAM_USE_QUATERNIONS:-OFF} \ + -DGTSAM_ROT3_EXPMAP=${GTSAM_ROT3_EXPMAP:-ON} \ + -DGTSAM_POSE3_EXPMAP=${GTSAM_POSE3_EXPMAP:-ON} \ -DGTSAM_BUILD_WITH_MARCH_NATIVE=OFF \ -DBOOST_ROOT=$BOOST_ROOT \ -DBoost_NO_SYSTEM_PATHS=ON \ diff --git a/.github/workflows/build-special.yml b/.github/workflows/build-special.yml index 715f25733..532f917c1 100644 --- a/.github/workflows/build-special.yml +++ b/.github/workflows/build-special.yml @@ -24,6 +24,7 @@ jobs: ubuntu-gcc-deprecated, ubuntu-gcc-quaternions, ubuntu-gcc-tbb, + ubuntu-cayleymap, ] build_type: [Debug, Release] @@ -47,6 +48,12 @@ jobs: version: "9" flag: tbb + - name: ubuntu-cayleymap + os: ubuntu-18.04 + compiler: gcc + version: "9" + flag: cayley + steps: - name: Checkout uses: actions/checkout@master @@ -109,6 +116,13 @@ jobs: echo "GTSAM_WITH_TBB=ON" >> $GITHUB_ENV echo "GTSAM Uses TBB" + - name: Use Cayley Transform for Rot3 + if: matrix.flag == 'cayley' + run: | + echo "GTSAM_POSE3_EXPMAP=OFF" >> $GITHUB_ENV + echo "GTSAM_ROT3_EXPMAP=OFF" >> $GITHUB_ENV + echo "GTSAM Uses Cayley map for Rot3" + - name: Build & Test run: | bash .github/scripts/unix.sh -t From 9e421c407bac3307ed8d552f01352fa7a3a23211 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 12 Nov 2020 18:17:16 -0500 Subject: [PATCH 47/62] small doc fix --- gtsam/geometry/Rot3M.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/geometry/Rot3M.cpp b/gtsam/geometry/Rot3M.cpp index 67b774a74..07cc7302a 100644 --- a/gtsam/geometry/Rot3M.cpp +++ b/gtsam/geometry/Rot3M.cpp @@ -185,7 +185,7 @@ Vector3 Rot3::CayleyChart::Local(const Rot3& R, OptionalJacobian<3,3> H) { // Mathematica closed form optimization. // The following are the essential computations for the following algorithm // 1. Compute the inverse of P = (A+I), using a closed-form formula since P is 3x3 - // 2. Compute the Cayley transform C = P^{-1} * (A-I) + // 2. Compute the Cayley transform C = 2 * P^{-1} * (A-I) // 3. C is skew-symmetric, so we pick out the computations corresponding only to x, y, and z. const double a = A(0, 0), b = A(0, 1), c = A(0, 2); const double d = A(1, 0), e = A(1, 1), f = A(1, 2); From c86010d8740fb3af052be32a11e025ba7261f361 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 15 Nov 2020 18:15:52 -0500 Subject: [PATCH 48/62] Add new assertions and update tests --- gtsam/base/TestableAssertions.h | 41 +++++++++++++++++++ gtsam/geometry/tests/testCal3_S2.cpp | 11 +++++ gtsam/geometry/tests/testPose3.cpp | 26 ++---------- .../nonlinear/tests/testFunctorizedFactor.cpp | 13 +----- gtsam/nonlinear/tests/testValues.cpp | 10 +---- 5 files changed, 59 insertions(+), 42 deletions(-) diff --git a/gtsam/base/TestableAssertions.h b/gtsam/base/TestableAssertions.h index 1a31aa284..88dd619e5 100644 --- a/gtsam/base/TestableAssertions.h +++ b/gtsam/base/TestableAssertions.h @@ -23,6 +23,7 @@ #include #include #include +#include #include namespace gtsam { @@ -349,4 +350,44 @@ bool assert_inequal(const V& expected, const V& actual, double tol = 1e-9) { return false; } +/** + * Capture std out via cout stream and compare against string. + */ +template +bool assert_stdout_equal(const std::string& expected, const V& actual) { + // Redirect output to buffer so we can compare + std::stringstream buffer; + // Save the original output stream so we can reset later + std::streambuf* old = std::cout.rdbuf(buffer.rdbuf()); + + // We test against actual std::cout for faithful reproduction + std::cout << actual; + + // Get output string and reset stdout + std::string actual_ = buffer.str(); + std::cout.rdbuf(old); + + return assert_equal(expected, actual_); +} + +/** + * Capture print function output and compare against string. + */ +template +bool assert_print_equal(const std::string& expected, const V& actual) { + // Redirect output to buffer so we can compare + std::stringstream buffer; + // Save the original output stream so we can reset later + std::streambuf* old = std::cout.rdbuf(buffer.rdbuf()); + + // We test against actual std::cout for faithful reproduction + actual.print(); + + // Get output string and reset stdout + std::string actual_ = buffer.str(); + std::cout.rdbuf(old); + + return assert_equal(expected, actual_); +} + } // \namespace gtsam diff --git a/gtsam/geometry/tests/testCal3_S2.cpp b/gtsam/geometry/tests/testCal3_S2.cpp index ccecb09c3..55ea32e32 100644 --- a/gtsam/geometry/tests/testCal3_S2.cpp +++ b/gtsam/geometry/tests/testCal3_S2.cpp @@ -16,6 +16,7 @@ #include #include +#include #include #include @@ -127,6 +128,16 @@ TEST(Cal3_S2, between) { } +/* ************************************************************************* */ +TEST(Cal3_S2, Print) { + Cal3_S2 cal(5, 5, 5, 5, 5); + std::stringstream os; + os << "{fx: " << cal.fx() << ", fy: " << cal.fy() << ", s:" << cal.skew() << ", px:" << cal.px() + << ", py:" << cal.py() << "}"; + + EXPECT(assert_stdout_equal(os.str(), cal)); +} + /* ************************************************************************* */ int main() { TestResult tr; diff --git a/gtsam/geometry/tests/testPose3.cpp b/gtsam/geometry/tests/testPose3.cpp index 8fafcb25f..6de2c0a33 100644 --- a/gtsam/geometry/tests/testPose3.cpp +++ b/gtsam/geometry/tests/testPose3.cpp @@ -18,6 +18,7 @@ #include #include #include +#include #include // for operator += using namespace boost::assign; @@ -1028,32 +1029,13 @@ TEST(Pose3, Create) { } /* ************************************************************************* */ -TEST(Pose3, print) { - std::stringstream redirectStream; - std::streambuf* ssbuf = redirectStream.rdbuf(); - std::streambuf* oldbuf = std::cout.rdbuf(); - // redirect cout to redirectStream - std::cout.rdbuf(ssbuf); - +TEST(Pose3, Print) { Pose3 pose(Rot3::identity(), Point3(1, 2, 3)); - // output is captured to redirectStream - pose.print(); // Generate the expected output - std::stringstream expected; - Point3 translation(1, 2, 3); + std::string expected = "R: [\n\t1, 0, 0;\n\t0, 1, 0;\n\t0, 0, 1\n]\nt: 1 2 3\n"; - // Add expected rotation - expected << "R: [\n\t1, 0, 0;\n\t0, 1, 0;\n\t0, 0, 1\n]\n"; - expected << "t: 1 2 3\n"; - - // reset cout to the original stream - std::cout.rdbuf(oldbuf); - - // Get substring corresponding to translation part - std::string actual = redirectStream.str(); - - CHECK_EQUAL(expected.str(), actual); + EXPECT(assert_print_equal(expected, pose)); } /* ************************************************************************* */ diff --git a/gtsam/nonlinear/tests/testFunctorizedFactor.cpp b/gtsam/nonlinear/tests/testFunctorizedFactor.cpp index 48ab73ad0..cb91320cf 100644 --- a/gtsam/nonlinear/tests/testFunctorizedFactor.cpp +++ b/gtsam/nonlinear/tests/testFunctorizedFactor.cpp @@ -19,6 +19,7 @@ #include #include +#include #include #include #include @@ -115,16 +116,6 @@ TEST(FunctorizedFactor, Print) { auto factor = MakeFunctorizedFactor(key, X, model, MultiplyFunctor(multiplier)); - // redirect output to buffer so we can compare - stringstream buffer; - streambuf *old = cout.rdbuf(buffer.rdbuf()); - - factor.print(); - - // get output string and reset stdout - string actual = buffer.str(); - cout.rdbuf(old); - string expected = " keys = { X0 }\n" " noise model: unit (9) \n" @@ -135,7 +126,7 @@ TEST(FunctorizedFactor, Print) { "]\n" " noise model sigmas: 1 1 1 1 1 1 1 1 1\n"; - CHECK_EQUAL(expected, actual); + EXPECT(assert_print_equal(expected, factor)); } /* ************************************************************************* */ diff --git a/gtsam/nonlinear/tests/testValues.cpp b/gtsam/nonlinear/tests/testValues.cpp index 09b358efb..88cfb666f 100644 --- a/gtsam/nonlinear/tests/testValues.cpp +++ b/gtsam/nonlinear/tests/testValues.cpp @@ -595,15 +595,7 @@ TEST(Values, Demangle) { values.insert(key1, v); string expected = "Values with 1 values:\nValue v1: (Eigen::Matrix)\n[\n 5, 6, 7\n]\n\n"; - stringstream buffer; - streambuf * old = cout.rdbuf(buffer.rdbuf()); - - values.print(); - - string actual = buffer.str(); - cout.rdbuf(old); - - EXPECT(assert_equal(expected, actual)); + EXPECT(assert_print_equal(expected, values)); } /* ************************************************************************* */ From f2bfdbd317b02dfc75143c6c34c91b7f16a72f6f Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 15 Nov 2020 18:40:02 -0500 Subject: [PATCH 49/62] upload build directory after workflow completes --- .github/workflows/build-linux.yml | 7 ++++++- .github/workflows/build-macos.yml | 5 +++++ 2 files changed, 11 insertions(+), 1 deletion(-) diff --git a/.github/workflows/build-linux.yml b/.github/workflows/build-linux.yml index d80a7f4ba..07397c999 100644 --- a/.github/workflows/build-linux.yml +++ b/.github/workflows/build-linux.yml @@ -82,4 +82,9 @@ jobs: - name: Build and Test (Linux) if: runner.os == 'Linux' run: | - bash .github/scripts/unix.sh -t \ No newline at end of file + bash .github/scripts/unix.sh -t + - name: Upload build directory + uses: actions/upload-artifact@v2 + with: + name: gtsam-${{ matrix.name }} + path: $GITHUB_WORKSPACE/build/ diff --git a/.github/workflows/build-macos.yml b/.github/workflows/build-macos.yml index 69873980a..524cef18a 100644 --- a/.github/workflows/build-macos.yml +++ b/.github/workflows/build-macos.yml @@ -51,3 +51,8 @@ jobs: if: runner.os == 'macOS' run: | bash .github/scripts/unix.sh -t + - name: Upload build directory + uses: actions/upload-artifact@v2 + with: + name: gtsam-${{ matrix.name }} + path: $GITHUB_WORKSPACE/build/ From 91d275c9c74c955bb1ff86ea9b213e0344b796de Mon Sep 17 00:00:00 2001 From: Jose Luis Blanco Claraco Date: Mon, 16 Nov 2020 07:25:30 +0100 Subject: [PATCH 50/62] Add docs, fix ctor placement --- gtsam/nonlinear/NonlinearOptimizerParams.h | 40 ++++++++++++++++------ 1 file changed, 30 insertions(+), 10 deletions(-) diff --git a/gtsam/nonlinear/NonlinearOptimizerParams.h b/gtsam/nonlinear/NonlinearOptimizerParams.h index 53805f5f0..92c800e0c 100644 --- a/gtsam/nonlinear/NonlinearOptimizerParams.h +++ b/gtsam/nonlinear/NonlinearOptimizerParams.h @@ -33,8 +33,6 @@ namespace gtsam { */ class GTSAM_EXPORT NonlinearOptimizerParams { public: - NonlinearOptimizerParams() = default; - /** See NonlinearOptimizerParams::verbosity */ enum Verbosity { SILENT, TERMINATION, ERROR, VALUES, DELTA, LINEAR @@ -47,10 +45,6 @@ public: Verbosity verbosity = SILENT; ///< The printing verbosity during optimization (default SILENT) Ordering::OrderingType orderingType = Ordering::COLAMD; ///< The method of ordering use during variable elimination (default COLAMD) - virtual ~NonlinearOptimizerParams() { - } - virtual void print(const std::string& str = "") const; - size_t getMaxIterations() const { return maxIterations; } double getRelativeErrorTol() const { return relativeErrorTol; } double getAbsoluteErrorTol() const { return absoluteErrorTol; } @@ -68,13 +62,33 @@ public: static Verbosity verbosityTranslator(const std::string &s) ; static std::string verbosityTranslator(Verbosity value) ; - /** Type for an optional user-provided hook to be called after each - * internal optimizer iteration */ + /** Type for an optional user-provided hook to be called after each + * internal optimizer iteration. See iterationHook below. */ using IterationHook = std::function< void(size_t /*iteration*/, double/*errorBefore*/, double/*errorAfter*/)>; - /** Optional user-provided iteration hook to be called after each - * optimization iteration (Default: empty) */ + /** Optional user-provided iteration hook to be called after each + * optimization iteration (Default: none). + * Note that `IterationHook` is defined as a std::function<> with this + * signature: + * \code + * void(size_t iteration, double errorBefore, double errorAfter) + * \endcode + * which allows binding by means of a reference to a regular function: + * \code + * void foo(size_t iteration, double errorBefore, double errorAfter); + * // ... + * lmOpts.iterationHook = &foo; + * \endcode + * or to a C++11 lambda: + * \code + * lmOpts.iterationHook = [&](size_t iter, double oldError, double newError) + * { + * // ... + * }; + * \endcode + * or to the result of a properly-formed `std::bind` call. + */ IterationHook iterationHook; /** See NonlinearOptimizerParams::linearSolverType */ @@ -91,6 +105,12 @@ public: boost::optional ordering; ///< The optional variable elimination ordering, or empty to use COLAMD (default: empty) IterativeOptimizationParameters::shared_ptr iterativeParams; ///< The container for iterativeOptimization parameters. used in CG Solvers. + NonlinearOptimizerParams() = default; + virtual ~NonlinearOptimizerParams() { + } + + virtual void print(const std::string& str = "") const; + inline bool isMultifrontal() const { return (linearSolverType == MULTIFRONTAL_CHOLESKY) || (linearSolverType == MULTIFRONTAL_QR); From 9a4bd10e222918cfd984d793a04fad24741b7c21 Mon Sep 17 00:00:00 2001 From: Jose Luis Blanco Claraco Date: Mon, 16 Nov 2020 07:32:03 +0100 Subject: [PATCH 51/62] further extended docs --- gtsam/nonlinear/NonlinearOptimizerParams.h | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/gtsam/nonlinear/NonlinearOptimizerParams.h b/gtsam/nonlinear/NonlinearOptimizerParams.h index 92c800e0c..a7bc10a1f 100644 --- a/gtsam/nonlinear/NonlinearOptimizerParams.h +++ b/gtsam/nonlinear/NonlinearOptimizerParams.h @@ -80,7 +80,9 @@ public: * // ... * lmOpts.iterationHook = &foo; * \endcode - * or to a C++11 lambda: + * or to a C++11 lambda (preferred if you need to capture additional + * context variables, such that the optimizer object itself, the factor graph, + * etc.): * \code * lmOpts.iterationHook = [&](size_t iter, double oldError, double newError) * { From ac089ce6a37d6c6becded9ac4bee19f8cf1fe15c Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 16 Nov 2020 12:22:09 -0500 Subject: [PATCH 52/62] use KeyVector to allow proper wrapping with TBB --- gtsam/sfm/MFAS.cpp | 6 +++--- gtsam/sfm/MFAS.h | 2 +- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/gtsam/sfm/MFAS.cpp b/gtsam/sfm/MFAS.cpp index 4cd983ecd..913752d8a 100644 --- a/gtsam/sfm/MFAS.cpp +++ b/gtsam/sfm/MFAS.cpp @@ -121,8 +121,8 @@ MFAS::MFAS(const TranslationEdges& relativeTranslations, } } -vector MFAS::computeOrdering() const { - vector ordering; // Nodes in MFAS order (result). +KeyVector MFAS::computeOrdering() const { + KeyVector ordering; // Nodes in MFAS order (result). // A graph is an unordered map from keys to nodes. Each node contains a list // of its adjacent nodes. Create the graph from the edgeWeights. @@ -140,7 +140,7 @@ vector MFAS::computeOrdering() const { map MFAS::computeOutlierWeights() const { // Find the ordering. - vector ordering = computeOrdering(); + KeyVector ordering = computeOrdering(); // Create a map from the node key to its position in the ordering. This makes // it easier to lookup positions of different nodes. diff --git a/gtsam/sfm/MFAS.h b/gtsam/sfm/MFAS.h index 3b01122a9..decfbed0f 100644 --- a/gtsam/sfm/MFAS.h +++ b/gtsam/sfm/MFAS.h @@ -84,7 +84,7 @@ class MFAS { * @brief Computes the 1D MFAS ordering of nodes in the graph * @return orderedNodes: vector of nodes in the obtained order */ - std::vector computeOrdering() const; + KeyVector computeOrdering() const; /** * @brief Computes the outlier weights of the graph. We define the outlier From 283582cd5d88e3a10ae8c9a619fb25b379029927 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 16 Nov 2020 12:31:44 -0500 Subject: [PATCH 53/62] update MFAS tests --- gtsam/sfm/tests/testMFAS.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/gtsam/sfm/tests/testMFAS.cpp b/gtsam/sfm/tests/testMFAS.cpp index 362027d5d..a1f6dfa5f 100644 --- a/gtsam/sfm/tests/testMFAS.cpp +++ b/gtsam/sfm/tests/testMFAS.cpp @@ -25,7 +25,7 @@ using namespace gtsam; vector edges = {make_pair(3, 2), make_pair(0, 1), make_pair(3, 1), make_pair(1, 2), make_pair(0, 2), make_pair(3, 0)}; // nodes in the graph -vector nodes = {Key(0), Key(1), Key(2), Key(3)}; +KeyVector nodes = {Key(0), Key(1), Key(2), Key(3)}; // weights from projecting in direction-1 (bad direction, outlier accepted) vector weights1 = {2, 1.5, 0.5, 0.25, 1, 0.75}; // weights from projecting in direction-2 (good direction, outlier rejected) @@ -47,10 +47,10 @@ map getEdgeWeights(const vector &edges, TEST(MFAS, OrderingWeights2) { MFAS mfas_obj(getEdgeWeights(edges, weights2)); - vector ordered_nodes = mfas_obj.computeOrdering(); + KeyVector ordered_nodes = mfas_obj.computeOrdering(); // ground truth (expected) ordering in this example - vector gt_ordered_nodes = {0, 1, 3, 2}; + KeyVector gt_ordered_nodes = {0, 1, 3, 2}; // check if the expected ordering is obtained for (size_t i = 0; i < ordered_nodes.size(); i++) { @@ -77,10 +77,10 @@ TEST(MFAS, OrderingWeights2) { TEST(MFAS, OrderingWeights1) { MFAS mfas_obj(getEdgeWeights(edges, weights1)); - vector ordered_nodes = mfas_obj.computeOrdering(); + KeyVector ordered_nodes = mfas_obj.computeOrdering(); // "ground truth" expected ordering in this example - vector gt_ordered_nodes = {3, 0, 1, 2}; + KeyVector gt_ordered_nodes = {3, 0, 1, 2}; // check if the expected ordering is obtained for (size_t i = 0; i < ordered_nodes.size(); i++) { From 5c94b5ccc70d374a49f024dcb24bcec94382c817 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 16 Nov 2020 13:11:10 -0500 Subject: [PATCH 54/62] fix indentation and add upload for Windows --- .github/workflows/build-linux.yml | 6 +++--- .github/workflows/build-macos.yml | 6 +++--- .github/workflows/build-windows.yml | 7 ++++++- 3 files changed, 12 insertions(+), 7 deletions(-) diff --git a/.github/workflows/build-linux.yml b/.github/workflows/build-linux.yml index 07397c999..514f7c6d5 100644 --- a/.github/workflows/build-linux.yml +++ b/.github/workflows/build-linux.yml @@ -85,6 +85,6 @@ jobs: bash .github/scripts/unix.sh -t - name: Upload build directory uses: actions/upload-artifact@v2 - with: - name: gtsam-${{ matrix.name }} - path: $GITHUB_WORKSPACE/build/ + with: + name: gtsam-${{ matrix.name }} + path: $GITHUB_WORKSPACE/build/ diff --git a/.github/workflows/build-macos.yml b/.github/workflows/build-macos.yml index 524cef18a..298fc316b 100644 --- a/.github/workflows/build-macos.yml +++ b/.github/workflows/build-macos.yml @@ -53,6 +53,6 @@ jobs: bash .github/scripts/unix.sh -t - name: Upload build directory uses: actions/upload-artifact@v2 - with: - name: gtsam-${{ matrix.name }} - path: $GITHUB_WORKSPACE/build/ + with: + name: gtsam-${{ matrix.name }} + path: $GITHUB_WORKSPACE/build/ diff --git a/.github/workflows/build-windows.yml b/.github/workflows/build-windows.yml index 0a55de880..446e708a6 100644 --- a/.github/workflows/build-windows.yml +++ b/.github/workflows/build-windows.yml @@ -75,4 +75,9 @@ jobs: cmake --build build --config ${{ matrix.build_type }} --target wrap cmake --build build --config ${{ matrix.build_type }} --target check.base cmake --build build --config ${{ matrix.build_type }} --target check.base_unstable - cmake --build build --config ${{ matrix.build_type }} --target check.linear \ No newline at end of file + cmake --build build --config ${{ matrix.build_type }} --target check.linear + - name: Upload build directory + uses: actions/upload-artifact@v2 + with: + name: gtsam-${{ matrix.name }} + path: $GITHUB_WORKSPACE/build/ From aa2d9225dcc52848342d68435b28dc5995698700 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 16 Nov 2020 14:42:17 -0500 Subject: [PATCH 55/62] correct form for workspace env variable --- .github/workflows/build-linux.yml | 2 +- .github/workflows/build-macos.yml | 2 +- .github/workflows/build-windows.yml | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/.github/workflows/build-linux.yml b/.github/workflows/build-linux.yml index 514f7c6d5..1778286d9 100644 --- a/.github/workflows/build-linux.yml +++ b/.github/workflows/build-linux.yml @@ -87,4 +87,4 @@ jobs: uses: actions/upload-artifact@v2 with: name: gtsam-${{ matrix.name }} - path: $GITHUB_WORKSPACE/build/ + path: ${{ github.workspace }}/build/ diff --git a/.github/workflows/build-macos.yml b/.github/workflows/build-macos.yml index 298fc316b..2a3af6b9e 100644 --- a/.github/workflows/build-macos.yml +++ b/.github/workflows/build-macos.yml @@ -55,4 +55,4 @@ jobs: uses: actions/upload-artifact@v2 with: name: gtsam-${{ matrix.name }} - path: $GITHUB_WORKSPACE/build/ + path: ${{ github.workspace }}/build/ diff --git a/.github/workflows/build-windows.yml b/.github/workflows/build-windows.yml index 446e708a6..1de0d0493 100644 --- a/.github/workflows/build-windows.yml +++ b/.github/workflows/build-windows.yml @@ -80,4 +80,4 @@ jobs: uses: actions/upload-artifact@v2 with: name: gtsam-${{ matrix.name }} - path: $GITHUB_WORKSPACE/build/ + path: ${{ github.workspace }}/build/ From d5a09aad12ef04392cacb8b8bfe1589a66e42fca Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 16 Nov 2020 17:47:42 -0500 Subject: [PATCH 56/62] differentiate between Release and Debug builds --- .github/workflows/build-linux.yml | 2 +- .github/workflows/build-macos.yml | 2 +- .github/workflows/build-windows.yml | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/.github/workflows/build-linux.yml b/.github/workflows/build-linux.yml index 1778286d9..cd4933cd9 100644 --- a/.github/workflows/build-linux.yml +++ b/.github/workflows/build-linux.yml @@ -86,5 +86,5 @@ jobs: - name: Upload build directory uses: actions/upload-artifact@v2 with: - name: gtsam-${{ matrix.name }} + name: gtsam-${{ matrix.name }}-${{ matrix.build_type }} path: ${{ github.workspace }}/build/ diff --git a/.github/workflows/build-macos.yml b/.github/workflows/build-macos.yml index 2a3af6b9e..5e6f8ca04 100644 --- a/.github/workflows/build-macos.yml +++ b/.github/workflows/build-macos.yml @@ -54,5 +54,5 @@ jobs: - name: Upload build directory uses: actions/upload-artifact@v2 with: - name: gtsam-${{ matrix.name }} + name: gtsam-${{ matrix.name }}-${{ matrix.build_type }} path: ${{ github.workspace }}/build/ diff --git a/.github/workflows/build-windows.yml b/.github/workflows/build-windows.yml index 1de0d0493..63c276020 100644 --- a/.github/workflows/build-windows.yml +++ b/.github/workflows/build-windows.yml @@ -79,5 +79,5 @@ jobs: - name: Upload build directory uses: actions/upload-artifact@v2 with: - name: gtsam-${{ matrix.name }} + name: gtsam-${{ matrix.name }}-${{ matrix.build_type }} path: ${{ github.workspace }}/build/ From 1fd0e57fb0a2a9cd24accd3d8dc04d91328dccf6 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 18 Nov 2020 14:48:05 -0500 Subject: [PATCH 57/62] Better fkag naming, and more docs --- gtsam/base/Matrix.h | 2 +- gtsam/base/Vector.cpp | 14 +++++++------- gtsam/base/Vector.h | 5 ++++- gtsam/base/tests/testMatrix.cpp | 10 +++++----- 4 files changed, 17 insertions(+), 14 deletions(-) diff --git a/gtsam/base/Matrix.h b/gtsam/base/Matrix.h index 9adf4e1c1..a3a14c6c3 100644 --- a/gtsam/base/Matrix.h +++ b/gtsam/base/Matrix.h @@ -90,7 +90,7 @@ bool equal_with_abs_tol(const Eigen::DenseBase& A, const Eigen::DenseBas for(size_t i=0; i abs(a)) ? abs(b) : abs(a); // handle NaNs - if(std::isnan(a) || isnan(b)) { + if(isnan(a) || isnan(b)) { return isnan(a) && isnan(b); } // handle inf @@ -60,15 +60,15 @@ bool fpEqual(double a, double b, double tol, bool absolute) { else if(a == 0 || b == 0 || (abs(a) + abs(b)) < DOUBLE_MIN_NORMAL) { return abs(a-b) <= tol * DOUBLE_MIN_NORMAL; } - // Check if the numbers are really close - // Needed when comparing numbers near zero or tol is in vicinity - else if(abs(a-b) <= tol) { + // Check if the numbers are really close. + // Needed when comparing numbers near zero or tol is in vicinity. + else if (abs(a - b) <= tol) { return true; } - // Use relative error + // Check for relative error else if (abs(a - b) <= tol * min(larger, std::numeric_limits::max()) && - !absolute) { + check_relative) { return true; } diff --git a/gtsam/base/Vector.h b/gtsam/base/Vector.h index b0fc74f26..49b7e6d9d 100644 --- a/gtsam/base/Vector.h +++ b/gtsam/base/Vector.h @@ -85,10 +85,13 @@ static_assert( * respectively for the comparison to be true. * If one is NaN/Inf and the other is not, returns false. * + * The `check_relative` flag toggles checking for relative error as well. By + * default, the flag is true. + * * Return true if two numbers are close wrt tol. */ GTSAM_EXPORT bool fpEqual(double a, double b, double tol, - bool absolute = false); + bool check_relative = true); /** * print without optional string, must specify cout yourself diff --git a/gtsam/base/tests/testMatrix.cpp b/gtsam/base/tests/testMatrix.cpp index d22ffc0db..a7c218705 100644 --- a/gtsam/base/tests/testMatrix.cpp +++ b/gtsam/base/tests/testMatrix.cpp @@ -1164,15 +1164,15 @@ TEST(Matrix , IsVectorSpace) { } TEST(Matrix, AbsoluteError) { - double a = 2000, b = 1997, tol=1e-1; + double a = 2000, b = 1997, tol = 1e-1; bool isEqual; - // Test absolute error - isEqual = fpEqual(a, b, tol, true); + // Test only absolute error + isEqual = fpEqual(a, b, tol, false); EXPECT(!isEqual); - // Test relative error - isEqual = fpEqual(a, b, tol, false); + // Test relative error as well + isEqual = fpEqual(a, b, tol); EXPECT(isEqual); } From 0737a4594a1c2a5eb814b6da060ffcab8749d1f6 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 19 Nov 2020 07:44:42 -0500 Subject: [PATCH 58/62] better flag name and docs --- gtsam/base/Vector.cpp | 4 ++-- gtsam/base/Vector.h | 8 +++++--- 2 files changed, 7 insertions(+), 5 deletions(-) diff --git a/gtsam/base/Vector.cpp b/gtsam/base/Vector.cpp index d2f3ae868..658ab9a0d 100644 --- a/gtsam/base/Vector.cpp +++ b/gtsam/base/Vector.cpp @@ -39,7 +39,7 @@ namespace gtsam { * 1. https://randomascii.wordpress.com/2012/02/25/comparing-floating-point-numbers-2012-edition/ * 2. https://floating-point-gui.de/errors/comparison/ * ************************************************************************* */ -bool fpEqual(double a, double b, double tol, bool check_relative) { +bool fpEqual(double a, double b, double tol, bool check_relative_also) { using std::abs; using std::isnan; using std::isinf; @@ -68,7 +68,7 @@ bool fpEqual(double a, double b, double tol, bool check_relative) { // Check for relative error else if (abs(a - b) <= tol * min(larger, std::numeric_limits::max()) && - check_relative) { + check_relative_also) { return true; } diff --git a/gtsam/base/Vector.h b/gtsam/base/Vector.h index 49b7e6d9d..ed90a7126 100644 --- a/gtsam/base/Vector.h +++ b/gtsam/base/Vector.h @@ -85,13 +85,15 @@ static_assert( * respectively for the comparison to be true. * If one is NaN/Inf and the other is not, returns false. * - * The `check_relative` flag toggles checking for relative error as well. By - * default, the flag is true. + * @param check_relative_also is a flag which toggles additional checking for + * relative error. This means that if either the absolute error or the relative + * error is within the tolerance, the result will be true. + * By default, the flag is true. * * Return true if two numbers are close wrt tol. */ GTSAM_EXPORT bool fpEqual(double a, double b, double tol, - bool check_relative = true); + bool check_relative_also = true); /** * print without optional string, must specify cout yourself From 9e2007562a8448065224a30c72240ea4aea1b643 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 19 Nov 2020 08:04:30 -0500 Subject: [PATCH 59/62] only upload release builds --- .github/workflows/build-linux.yml | 1 + .github/workflows/build-macos.yml | 1 + .github/workflows/build-windows.yml | 1 + 3 files changed, 3 insertions(+) diff --git a/.github/workflows/build-linux.yml b/.github/workflows/build-linux.yml index cd4933cd9..940e34b82 100644 --- a/.github/workflows/build-linux.yml +++ b/.github/workflows/build-linux.yml @@ -85,6 +85,7 @@ jobs: bash .github/scripts/unix.sh -t - name: Upload build directory uses: actions/upload-artifact@v2 + if: ${{ matrix.build_type }} == "Release" with: name: gtsam-${{ matrix.name }}-${{ matrix.build_type }} path: ${{ github.workspace }}/build/ diff --git a/.github/workflows/build-macos.yml b/.github/workflows/build-macos.yml index 5e6f8ca04..cccfd7b5b 100644 --- a/.github/workflows/build-macos.yml +++ b/.github/workflows/build-macos.yml @@ -53,6 +53,7 @@ jobs: bash .github/scripts/unix.sh -t - name: Upload build directory uses: actions/upload-artifact@v2 + if: ${{ matrix.build_type }} == "Release" with: name: gtsam-${{ matrix.name }}-${{ matrix.build_type }} path: ${{ github.workspace }}/build/ diff --git a/.github/workflows/build-windows.yml b/.github/workflows/build-windows.yml index 63c276020..a14d7de5e 100644 --- a/.github/workflows/build-windows.yml +++ b/.github/workflows/build-windows.yml @@ -78,6 +78,7 @@ jobs: cmake --build build --config ${{ matrix.build_type }} --target check.linear - name: Upload build directory uses: actions/upload-artifact@v2 + if: ${{ matrix.build_type }} == "Release" with: name: gtsam-${{ matrix.name }}-${{ matrix.build_type }} path: ${{ github.workspace }}/build/ From 802580eec79d5c39995af170e40a7c0ba1eb54e6 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 19 Nov 2020 12:40:38 -0500 Subject: [PATCH 60/62] enforce constant term in quadratic to be 0 --- gtsam_unstable/linear/QPSParser.cpp | 17 ++++++++++------- 1 file changed, 10 insertions(+), 7 deletions(-) diff --git a/gtsam_unstable/linear/QPSParser.cpp b/gtsam_unstable/linear/QPSParser.cpp index 3039185f2..df21c0132 100644 --- a/gtsam_unstable/linear/QPSParser.cpp +++ b/gtsam_unstable/linear/QPSParser.cpp @@ -81,7 +81,7 @@ class QPSVisitor { varname_to_key; // Variable QPS string name to key std::unordered_map> H; // H from hessian - double f; // Constant term of quadratic cost + double f = 0; // Constant term of quadratic cost std::string obj_name; // the objective function has a name in the QPS std::string name_; // the quadratic program has a name in the QPS std::unordered_map @@ -175,10 +175,11 @@ class QPSVisitor { string var_ = fromChars<1>(vars); string row_ = fromChars<3>(vars); double coefficient = at_c<5>(vars); - if (row_ == obj_name) + if (row_ == obj_name) { f = -coefficient; - else + } else { b[row_] = coefficient; + } if (debug) { cout << "Added RHS for Var: " << var_ << " Row: " << row_ @@ -194,15 +195,17 @@ class QPSVisitor { string row2_ = fromChars<7>(vars); double coefficient1 = at_c<5>(vars); double coefficient2 = at_c<9>(vars); - if (row1_ == obj_name) + if (row1_ == obj_name) { f = -coefficient1; - else + } else { b[row1_] = coefficient1; + } - if (row2_ == obj_name) + if (row2_ == obj_name) { f = -coefficient2; - else + } else { b[row2_] = coefficient2; + } if (debug) { cout << "Added RHS for Var: " << var_ << " Row: " << row1_ From 99e3a111e1d7d0d16cc05e8b12fed6390bbbdf28 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 19 Nov 2020 14:58:50 -0500 Subject: [PATCH 61/62] correct conditional syntax --- .github/workflows/build-linux.yml | 2 +- .github/workflows/build-macos.yml | 2 +- .github/workflows/build-windows.yml | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/.github/workflows/build-linux.yml b/.github/workflows/build-linux.yml index 940e34b82..4039711f2 100644 --- a/.github/workflows/build-linux.yml +++ b/.github/workflows/build-linux.yml @@ -85,7 +85,7 @@ jobs: bash .github/scripts/unix.sh -t - name: Upload build directory uses: actions/upload-artifact@v2 - if: ${{ matrix.build_type }} == "Release" + if: ${{ matrix.build_type == "Release" }} with: name: gtsam-${{ matrix.name }}-${{ matrix.build_type }} path: ${{ github.workspace }}/build/ diff --git a/.github/workflows/build-macos.yml b/.github/workflows/build-macos.yml index cccfd7b5b..1d36e048f 100644 --- a/.github/workflows/build-macos.yml +++ b/.github/workflows/build-macos.yml @@ -53,7 +53,7 @@ jobs: bash .github/scripts/unix.sh -t - name: Upload build directory uses: actions/upload-artifact@v2 - if: ${{ matrix.build_type }} == "Release" + if: ${{ matrix.build_type == "Release" }} with: name: gtsam-${{ matrix.name }}-${{ matrix.build_type }} path: ${{ github.workspace }}/build/ diff --git a/.github/workflows/build-windows.yml b/.github/workflows/build-windows.yml index a14d7de5e..8e469d49f 100644 --- a/.github/workflows/build-windows.yml +++ b/.github/workflows/build-windows.yml @@ -78,7 +78,7 @@ jobs: cmake --build build --config ${{ matrix.build_type }} --target check.linear - name: Upload build directory uses: actions/upload-artifact@v2 - if: ${{ matrix.build_type }} == "Release" + if: ${{ matrix.build_type == "Release" }} with: name: gtsam-${{ matrix.name }}-${{ matrix.build_type }} path: ${{ github.workspace }}/build/ From c206ed9c2fcfff0802951ae7cabea5c8ba7ecd5e Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 19 Nov 2020 15:35:09 -0500 Subject: [PATCH 62/62] syntax update --- .github/workflows/build-linux.yml | 2 +- .github/workflows/build-macos.yml | 2 +- .github/workflows/build-windows.yml | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/.github/workflows/build-linux.yml b/.github/workflows/build-linux.yml index 4039711f2..32c3bd8aa 100644 --- a/.github/workflows/build-linux.yml +++ b/.github/workflows/build-linux.yml @@ -85,7 +85,7 @@ jobs: bash .github/scripts/unix.sh -t - name: Upload build directory uses: actions/upload-artifact@v2 - if: ${{ matrix.build_type == "Release" }} + if: matrix.build_type == 'Release' with: name: gtsam-${{ matrix.name }}-${{ matrix.build_type }} path: ${{ github.workspace }}/build/ diff --git a/.github/workflows/build-macos.yml b/.github/workflows/build-macos.yml index 1d36e048f..cf1a474e3 100644 --- a/.github/workflows/build-macos.yml +++ b/.github/workflows/build-macos.yml @@ -53,7 +53,7 @@ jobs: bash .github/scripts/unix.sh -t - name: Upload build directory uses: actions/upload-artifact@v2 - if: ${{ matrix.build_type == "Release" }} + if: matrix.build_type == 'Release' with: name: gtsam-${{ matrix.name }}-${{ matrix.build_type }} path: ${{ github.workspace }}/build/ diff --git a/.github/workflows/build-windows.yml b/.github/workflows/build-windows.yml index 8e469d49f..7eb908c94 100644 --- a/.github/workflows/build-windows.yml +++ b/.github/workflows/build-windows.yml @@ -78,7 +78,7 @@ jobs: cmake --build build --config ${{ matrix.build_type }} --target check.linear - name: Upload build directory uses: actions/upload-artifact@v2 - if: ${{ matrix.build_type == "Release" }} + if: matrix.build_type == 'Release' with: name: gtsam-${{ matrix.name }}-${{ matrix.build_type }} path: ${{ github.workspace }}/build/