From 5abe293cdf48bc47c76938ed71512cd296abec4d Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 7 Mar 2020 20:57:08 -0500 Subject: [PATCH 01/54] Setup and simulateMeasurements --- tests/testTranslationRecovery.cpp | 126 ++++++++++++++++++++++++++++++ 1 file changed, 126 insertions(+) create mode 100644 tests/testTranslationRecovery.cpp diff --git a/tests/testTranslationRecovery.cpp b/tests/testTranslationRecovery.cpp new file mode 100644 index 000000000..45c882cc9 --- /dev/null +++ b/tests/testTranslationRecovery.cpp @@ -0,0 +1,126 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file testTranslationRecovery.cpp + * @author Frank Dellaert + * @date March 2020 + * @brief test recovering translations when rotations are given. + */ + +#include +// #include +#include +#include +#include +#include +#include +#include +#include + +#include + +using namespace std; +using namespace gtsam; + +class TranslationFactor {}; + +// Set up an optimization problem for the unknown translations Ti in the world +// coordinate frame, given the known camera attitudes wRi with respect to the +// world frame, and a set of (noisy) translation directions of type Unit3, +// aZb. The measurement equation is +// aZb = Unit3(aRw * (Tb - Ta)) (1) +// i.e., aZb is the normalized translation of B's origin in the camera frame A. +// It is clear that we cannot recover the scale, nor the absolute position, so +// the gauge freedom in this case is 3 + 1 = 4. We fix these by taking fixing +// the translations Ta and Tb associated with the first measurement aZb, +// clamping them to their initial values as given to this method. If no initial +// values are given, we use the origin for Tb and set Tb to make (1) come +// through, i.e., +// Tb = s * wRa * Point3(aZb) (2) +// where s is an arbitrary scale that can be supplied, default 1.0. Hence, two +// versions are supplied below corresponding to whether we have initial values +// or not. Because the latter one is called from the first one, this is prime. + +void recoverTranslations(const double scale = 1.0) { + // graph.emplace_shared(m.second, unit2, m.first, + // P(j)); +} + +using KeyPair = pair; + +/** + * @brief Simulate translation direction measurements + * + * @param poses SE(3) ground truth poses stored as Values + * @param edges pairs (a,b) for which a measurement aZb will be generated. + */ +vector simulateMeasurements(const Values& poses, + const vector& edges) { + vector measurements; + for (auto edge : edges) { + Key a, b; + std::tie(a, b) = edge; + Pose3 wTa = poses.at(a), wTb = poses.at(b); + Point3 Ta = wTa.translation(), Tb = wTb.translation(); + auto aRw = wTa.rotation().inverse(); + Unit3 aZb = Unit3(aRw * (Tb - Ta)); + measurements.push_back(aZb); + } + return measurements; +} + +/* ************************************************************************* */ +// We read the BAL file, which has 3 cameras in it, with poses. We then assume +// the rotations are correct, but translations have to be estimated from +// translation directions only. Since we have 3 cameras, A, B, and C, we can at +// most create three relative measurements, let's call them aZb, aZc, and bZc. +// These will be of type Unit3. We then call `recoverTranslations` which sets up +// an optimization problem for the three unknown translations. +TEST(TranslationRecovery, BAL) { + string filename = findExampleDataFile("dubrovnik-3-7-pre"); + SfM_data db; + bool success = readBAL(filename, db); + if (!success) throw runtime_error("Could not access file!"); + + SharedNoiseModel unit2 = noiseModel::Unit::Create(2); + NonlinearFactorGraph graph; + + size_t i = 0; + Values poses; + for (auto camera : db.cameras) { + GTSAM_PRINT(camera); + poses.insert(i++, camera.pose()); + } + Pose3 wTa = poses.at(0), wTb = poses.at(1), + wTc = poses.at(2); + Point3 Ta = wTa.translation(), Tb = wTb.translation(), Tc = wTc.translation(); + auto measurements = simulateMeasurements(poses, {{0, 1}, {0, 2}, {1, 2}}); + auto aRw = wTa.rotation().inverse(); + Unit3 aZb = measurements[0]; + EXPECT(assert_equal(Unit3(aRw * (Tb - Ta)), aZb)); + Unit3 aZc = measurements[1]; + EXPECT(assert_equal(Unit3(aRw * (Tc - Ta)), aZc)); + // Values initial = initialCamerasAndPointsEstimate(db); + + // LevenbergMarquardtOptimizer lm(graph, initial); + + // Values actual = lm.optimize(); + // double actualError = graph.error(actual); + // EXPECT_DOUBLES_EQUAL(0.0199833, actualError, 1e-5); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ From 146f411a99df7d82648f0c1ebbeb27833f11f041 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 8 Mar 2020 01:45:04 -0500 Subject: [PATCH 02/54] Built TranslationFactor class and partially completed TranslationRecovery class --- tests/testTranslationRecovery.cpp | 323 +++++++++++++++++++++++------- 1 file changed, 246 insertions(+), 77 deletions(-) diff --git a/tests/testTranslationRecovery.cpp b/tests/testTranslationRecovery.cpp index 45c882cc9..330a507e5 100644 --- a/tests/testTranslationRecovery.cpp +++ b/tests/testTranslationRecovery.cpp @@ -1,3 +1,211 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file TranslationFactor.h + * @author Frank Dellaert + * @date March 2020 + * @brief Binary factor for a relative translation direction measurement. + */ + +#include +#include +#include +#include + +namespace gtsam { + +/** + * Binary factor for a relative translation direction measurement + * w_aZb. The measurement equation is + * w_aZb = Unit3(Tb - Ta) + * i.e., w_aZb is the translation direction from frame A to B, in world + * coordinates. Although Unit3 instances live on a manifold, following + * Wilson14eccv_1DSfM.pdf error we compute the *chordal distance* in the + * ambient world coordinate frame: + * normalized(Tb - Ta) - w_aZb.point3() + * + * @addtogroup SAM + */ +class TranslationFactor : public NoiseModelFactor2 { + private: + typedef NoiseModelFactor2 Base; + Point3 measured_w_aZb_; + + public: + /// default constructor + TranslationFactor() {} + + TranslationFactor(Key a, Key b, const Unit3& w_aZb, + const SharedNoiseModel& noiseModel) + : Base(noiseModel, a, b), measured_w_aZb_(w_aZb.point3()) {} + + /** + * @brief Caclulate error norm(p1-p2) - measured + * + * @param Ta translation for key a + * @param Tb translation for key b + * @param H1 optional jacobian in Ta + * @param H2 optional jacobian in Tb + * @return * Vector + */ + Vector evaluateError( + const Point3& Ta, const Point3& Tb, + boost::optional H1 = boost::none, + boost::optional H2 = boost::none) const override { + const Point3 dir = Tb - Ta; + Matrix33 H_predicted_dir; + const Point3 predicted = + dir.normalized(H1 || H2 ? &H_predicted_dir : nullptr); + if (H1) *H1 = H_predicted_dir; + if (H2) *H2 = -H_predicted_dir; + return predicted - measured_w_aZb_; + } + + private: + friend class boost::serialization::access; + template + void serialize(ARCHIVE& ar, const unsigned int /*version*/) { + ar& boost::serialization::make_nvp( + "Base", boost::serialization::base_object(*this)); + } +}; // \ TranslationFactor +} // namespace gtsam + +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file TranslationRecovery.h + * @author Frank Dellaert + * @date March 2020 + * @brief test recovering translations when rotations are given. + */ + +// #include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace gtsam { + +// Set up an optimization problem for the unknown translations Ti in the world +// coordinate frame, given the known camera attitudes wRi with respect to the +// world frame, and a set of (noisy) translation directions of type Unit3, +// w_aZb. The measurement equation is +// w_aZb = Unit3(Tb - Ta) (1) +// i.e., w_aZb is the translation direction from frame A to B, in world +// coordinates. Although Unit3 instances live on a manifold, following +// Wilson14eccv_1DSfM.pdf error we compute the *chordal distance* in the +// ambient world coordinate frame. +// +// It is clear that we cannot recover the scale, nor the absolute position, +// so the gauge freedom in this case is 3 + 1 = 4. We fix these by taking fixing +// the translations Ta and Tb associated with the first measurement w_aZb, +// clamping them to their initial values as given to this method. If no initial +// values are given, we use the origin for Tb and set Tb to make (1) come +// through, i.e., +// Tb = s * wRa * Point3(w_aZb) (2) +// where s is an arbitrary scale that can be supplied, default 1.0. Hence, two +// versions are supplied below corresponding to whether we have initial values +// or not. Because the latter one is called from the first one, this is prime. + +class TranslationRecovery { + public: + using KeyPair = std::pair; + using TranslationEdges = std::map; + + private: + TranslationEdges relativeTranslations_; + + public: + /** + * @brief Construct a new Translation Recovery object + * + * @param relativeTranslations the relative translations, in world coordinate + * frames, indexed in a map by a pair of Pose keys. + */ + TranslationRecovery(const TranslationEdges& relativeTranslations) + : relativeTranslations_(relativeTranslations) {} + + /** + * @brief Build the factor graph to do the optimization. + * + * @return NonlinearFactorGraph + */ + NonlinearFactorGraph buildGraph() const { + auto noiseModel = noiseModel::Isotropic::Sigma(3, 0.01); + NonlinearFactorGraph graph; + for (auto edge : relativeTranslations_) { + Key a, b; + std::tie(a, b) = edge.first; + const Unit3 w_aZb = edge.second; + graph.emplace_shared(a, b, w_aZb, noiseModel); + } + return graph; + } + + /** + * @brief Build and optimize factor graph. + * + * @param scale scale for first relative translation which fixes gauge. + * @return Values + */ + Values run(const double scale = 1.0) const { + const auto graph = buildGraph(); + // Values initial = randomTranslations(); + + // LevenbergMarquardtOptimizer lm(graph, initial); + + Values result; // = lm.optimize(); + + return result; + } + + /** + * @brief Simulate translation direction measurements + * + * @param poses SE(3) ground truth poses stored as Values + * @param edges pairs (a,b) for which a measurement w_aZb will be generated. + */ + static TranslationEdges SimulateMeasurements( + const Values& poses, const std::vector& edges) { + TranslationEdges relativeTranslations; + for (auto edge : edges) { + Key a, b; + std::tie(a, b) = edge; + const Pose3 wTa = poses.at(a), wTb = poses.at(b); + const Point3 Ta = wTa.translation(), Tb = wTb.translation(); + const Unit3 w_aZb(Tb - Ta); + relativeTranslations[edge] = w_aZb; + } + return relativeTranslations; + } +}; +} // namespace gtsam + /* ---------------------------------------------------------------------------- * GTSAM Copyright 2010, Georgia Tech Research Corporation, @@ -16,100 +224,61 @@ * @brief test recovering translations when rotations are given. */ -#include -// #include -#include -#include -#include -#include -#include -#include -#include - #include +#include using namespace std; using namespace gtsam; -class TranslationFactor {}; - -// Set up an optimization problem for the unknown translations Ti in the world -// coordinate frame, given the known camera attitudes wRi with respect to the -// world frame, and a set of (noisy) translation directions of type Unit3, -// aZb. The measurement equation is -// aZb = Unit3(aRw * (Tb - Ta)) (1) -// i.e., aZb is the normalized translation of B's origin in the camera frame A. -// It is clear that we cannot recover the scale, nor the absolute position, so -// the gauge freedom in this case is 3 + 1 = 4. We fix these by taking fixing -// the translations Ta and Tb associated with the first measurement aZb, -// clamping them to their initial values as given to this method. If no initial -// values are given, we use the origin for Tb and set Tb to make (1) come -// through, i.e., -// Tb = s * wRa * Point3(aZb) (2) -// where s is an arbitrary scale that can be supplied, default 1.0. Hence, two -// versions are supplied below corresponding to whether we have initial values -// or not. Because the latter one is called from the first one, this is prime. - -void recoverTranslations(const double scale = 1.0) { - // graph.emplace_shared(m.second, unit2, m.first, - // P(j)); -} - -using KeyPair = pair; - -/** - * @brief Simulate translation direction measurements - * - * @param poses SE(3) ground truth poses stored as Values - * @param edges pairs (a,b) for which a measurement aZb will be generated. - */ -vector simulateMeasurements(const Values& poses, - const vector& edges) { - vector measurements; - for (auto edge : edges) { - Key a, b; - std::tie(a, b) = edge; - Pose3 wTa = poses.at(a), wTb = poses.at(b); - Point3 Ta = wTa.translation(), Tb = wTb.translation(); - auto aRw = wTa.rotation().inverse(); - Unit3 aZb = Unit3(aRw * (Tb - Ta)); - measurements.push_back(aZb); - } - return measurements; -} - /* ************************************************************************* */ // We read the BAL file, which has 3 cameras in it, with poses. We then assume // the rotations are correct, but translations have to be estimated from // translation directions only. Since we have 3 cameras, A, B, and C, we can at -// most create three relative measurements, let's call them aZb, aZc, and bZc. -// These will be of type Unit3. We then call `recoverTranslations` which sets up -// an optimization problem for the three unknown translations. +// most create three relative measurements, let's call them w_aZb, w_aZc, and +// bZc. These will be of type Unit3. We then call `recoverTranslations` which +// sets up an optimization problem for the three unknown translations. TEST(TranslationRecovery, BAL) { - string filename = findExampleDataFile("dubrovnik-3-7-pre"); + const string filename = findExampleDataFile("dubrovnik-3-7-pre"); SfM_data db; bool success = readBAL(filename, db); if (!success) throw runtime_error("Could not access file!"); - SharedNoiseModel unit2 = noiseModel::Unit::Create(2); - NonlinearFactorGraph graph; - - size_t i = 0; + // Get camera poses, as Values + size_t j = 0; Values poses; for (auto camera : db.cameras) { - GTSAM_PRINT(camera); - poses.insert(i++, camera.pose()); + poses.insert(j++, camera.pose()); } - Pose3 wTa = poses.at(0), wTb = poses.at(1), - wTc = poses.at(2); - Point3 Ta = wTa.translation(), Tb = wTb.translation(), Tc = wTc.translation(); - auto measurements = simulateMeasurements(poses, {{0, 1}, {0, 2}, {1, 2}}); - auto aRw = wTa.rotation().inverse(); - Unit3 aZb = measurements[0]; - EXPECT(assert_equal(Unit3(aRw * (Tb - Ta)), aZb)); - Unit3 aZc = measurements[1]; - EXPECT(assert_equal(Unit3(aRw * (Tc - Ta)), aZc)); - // Values initial = initialCamerasAndPointsEstimate(db); + + // Simulate measurements + const auto relativeTranslations = TranslationRecovery::SimulateMeasurements( + poses, {{0, 1}, {0, 2}, {1, 2}}); + + // Check + const Pose3 wTa = poses.at(0), wTb = poses.at(1), + wTc = poses.at(2); + const Point3 Ta = wTa.translation(), Tb = wTb.translation(), + Tc = wTc.translation(); + const Rot3 aRw = wTa.rotation().inverse(); + const Unit3 w_aZb = relativeTranslations.at({0, 1}); + EXPECT(assert_equal(Unit3(Tb - Ta), w_aZb)); + const Unit3 w_aZc = relativeTranslations.at({0, 2}); + EXPECT(assert_equal(Unit3(Tc - Ta), w_aZc)); + + TranslationRecovery algorithm(relativeTranslations); + const auto graph = algorithm.buildGraph(); + EXPECT_LONGS_EQUAL(3, graph.size()); + + // Translation recovery, version 1 + const auto result = algorithm.run(2); + + // Check result + // Pose3 expected0(wTa.rotation(), Point3(0, 0, 0)); + // EXPECT(assert_equal(expected0, result.at(0))); + // Pose3 expected1(wTb.rotation(), 2 * w_aZb.point3()); + // EXPECT(assert_equal(expected1, result.at(1))); + + // Values initial = randomTranslations(); // LevenbergMarquardtOptimizer lm(graph, initial); From d8f3ca46a4966f0c13250caa929de74fb441966f Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 8 Mar 2020 01:58:22 -0500 Subject: [PATCH 03/54] Added initalizeRandomly --- tests/testTranslationRecovery.cpp | 29 ++++++++++++++++++++++++++--- 1 file changed, 26 insertions(+), 3 deletions(-) diff --git a/tests/testTranslationRecovery.cpp b/tests/testTranslationRecovery.cpp index 330a507e5..f513184e3 100644 --- a/tests/testTranslationRecovery.cpp +++ b/tests/testTranslationRecovery.cpp @@ -167,6 +167,29 @@ class TranslationRecovery { return graph; } + /** + * @brief Create random initial translations. + * + * @return Values + */ + Values initalizeRandomly() const { + Values initial; + auto insert = [&initial](Key j) { + if (!initial.exists(j)) { + initial.insert(j, Vector3::Random()); + } + }; + + // Loop over measurements and add a random translation + for (auto edge : relativeTranslations_) { + Key a, b; + std::tie(a, b) = edge.first; + insert(a); + insert(b); + } + return initial; + } + /** * @brief Build and optimize factor graph. * @@ -175,11 +198,11 @@ class TranslationRecovery { */ Values run(const double scale = 1.0) const { const auto graph = buildGraph(); - // Values initial = randomTranslations(); + const Values initial = initalizeRandomly(); - // LevenbergMarquardtOptimizer lm(graph, initial); + LevenbergMarquardtOptimizer lm(graph, initial); - Values result; // = lm.optimize(); + Values result = lm.optimize(); return result; } From 652302f5ad6d868c6c339bb4e76ae6b4f9c9ea22 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 8 Mar 2020 03:05:38 -0400 Subject: [PATCH 04/54] Running optimization --- tests/testTranslationRecovery.cpp | 21 ++++++++------------- 1 file changed, 8 insertions(+), 13 deletions(-) diff --git a/tests/testTranslationRecovery.cpp b/tests/testTranslationRecovery.cpp index f513184e3..bd8a81f20 100644 --- a/tests/testTranslationRecovery.cpp +++ b/tests/testTranslationRecovery.cpp @@ -200,8 +200,9 @@ class TranslationRecovery { const auto graph = buildGraph(); const Values initial = initalizeRandomly(); - LevenbergMarquardtOptimizer lm(graph, initial); - + LevenbergMarquardtParams params; + params.setVerbosityLM("Summary"); + LevenbergMarquardtOptimizer lm(graph, initial, params); Values result = lm.optimize(); return result; @@ -296,18 +297,12 @@ TEST(TranslationRecovery, BAL) { const auto result = algorithm.run(2); // Check result - // Pose3 expected0(wTa.rotation(), Point3(0, 0, 0)); - // EXPECT(assert_equal(expected0, result.at(0))); - // Pose3 expected1(wTb.rotation(), 2 * w_aZb.point3()); - // EXPECT(assert_equal(expected1, result.at(1))); + EXPECT(assert_equal(Point3(0, 0, 0), result.at(0))); + EXPECT(assert_equal(Point3(2 * w_aZb.point3()), result.at(1))); + EXPECT(assert_equal(Point3(0, 0, 0), result.at(2))); - // Values initial = randomTranslations(); - - // LevenbergMarquardtOptimizer lm(graph, initial); - - // Values actual = lm.optimize(); - // double actualError = graph.error(actual); - // EXPECT_DOUBLES_EQUAL(0.0199833, actualError, 1e-5); + // TODO(frank): how to get stats back + // EXPECT_DOUBLES_EQUAL(0.0199833, actualError, 1e-5); } /* ************************************************************************* */ From 83a0f515875b666a9cdae1e4af1668d6ec737fc8 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 8 Mar 2020 03:15:38 -0400 Subject: [PATCH 05/54] Moved TranslationFactor prototype into sfm directory --- gtsam/sfm/TranslationFactor.h | 83 ++++++++++++++++++++++++++++++ tests/testTranslationRecovery.cpp | 84 +------------------------------ 2 files changed, 84 insertions(+), 83 deletions(-) create mode 100644 gtsam/sfm/TranslationFactor.h diff --git a/gtsam/sfm/TranslationFactor.h b/gtsam/sfm/TranslationFactor.h new file mode 100644 index 000000000..626214dcc --- /dev/null +++ b/gtsam/sfm/TranslationFactor.h @@ -0,0 +1,83 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +#pragma once + +/** + * @file TranslationFactor.h + * @author Frank Dellaert + * @date March 2020 + * @brief Binary factor for a relative translation direction measurement. + */ + +#include +#include +#include +#include + +namespace gtsam { + +/** + * Binary factor for a relative translation direction measurement + * w_aZb. The measurement equation is + * w_aZb = Unit3(Tb - Ta) + * i.e., w_aZb is the translation direction from frame A to B, in world + * coordinates. Although Unit3 instances live on a manifold, following + * Wilson14eccv_1DSfM.pdf error we compute the *chordal distance* in the + * ambient world coordinate frame: + * normalized(Tb - Ta) - w_aZb.point3() + * + * @addtogroup SAM + */ +class TranslationFactor : public NoiseModelFactor2 { + private: + typedef NoiseModelFactor2 Base; + Point3 measured_w_aZb_; + + public: + /// default constructor + TranslationFactor() {} + + TranslationFactor(Key a, Key b, const Unit3& w_aZb, + const SharedNoiseModel& noiseModel) + : Base(noiseModel, a, b), measured_w_aZb_(w_aZb.point3()) {} + + /** + * @brief Caclulate error norm(p1-p2) - measured + * + * @param Ta translation for key a + * @param Tb translation for key b + * @param H1 optional jacobian in Ta + * @param H2 optional jacobian in Tb + * @return * Vector + */ + Vector evaluateError( + const Point3& Ta, const Point3& Tb, + boost::optional H1 = boost::none, + boost::optional H2 = boost::none) const override { + const Point3 dir = Tb - Ta; + Matrix33 H_predicted_dir; + const Point3 predicted = + dir.normalized(H1 || H2 ? &H_predicted_dir : nullptr); + if (H1) *H1 = H_predicted_dir; + if (H2) *H2 = -H_predicted_dir; + return predicted - measured_w_aZb_; + } + + private: + friend class boost::serialization::access; + template + void serialize(ARCHIVE& ar, const unsigned int /*version*/) { + ar& boost::serialization::make_nvp( + "Base", boost::serialization::base_object(*this)); + } +}; // \ TranslationFactor +} // namespace gtsam diff --git a/tests/testTranslationRecovery.cpp b/tests/testTranslationRecovery.cpp index bd8a81f20..8742820f4 100644 --- a/tests/testTranslationRecovery.cpp +++ b/tests/testTranslationRecovery.cpp @@ -1,85 +1,3 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/** - * @file TranslationFactor.h - * @author Frank Dellaert - * @date March 2020 - * @brief Binary factor for a relative translation direction measurement. - */ - -#include -#include -#include -#include - -namespace gtsam { - -/** - * Binary factor for a relative translation direction measurement - * w_aZb. The measurement equation is - * w_aZb = Unit3(Tb - Ta) - * i.e., w_aZb is the translation direction from frame A to B, in world - * coordinates. Although Unit3 instances live on a manifold, following - * Wilson14eccv_1DSfM.pdf error we compute the *chordal distance* in the - * ambient world coordinate frame: - * normalized(Tb - Ta) - w_aZb.point3() - * - * @addtogroup SAM - */ -class TranslationFactor : public NoiseModelFactor2 { - private: - typedef NoiseModelFactor2 Base; - Point3 measured_w_aZb_; - - public: - /// default constructor - TranslationFactor() {} - - TranslationFactor(Key a, Key b, const Unit3& w_aZb, - const SharedNoiseModel& noiseModel) - : Base(noiseModel, a, b), measured_w_aZb_(w_aZb.point3()) {} - - /** - * @brief Caclulate error norm(p1-p2) - measured - * - * @param Ta translation for key a - * @param Tb translation for key b - * @param H1 optional jacobian in Ta - * @param H2 optional jacobian in Tb - * @return * Vector - */ - Vector evaluateError( - const Point3& Ta, const Point3& Tb, - boost::optional H1 = boost::none, - boost::optional H2 = boost::none) const override { - const Point3 dir = Tb - Ta; - Matrix33 H_predicted_dir; - const Point3 predicted = - dir.normalized(H1 || H2 ? &H_predicted_dir : nullptr); - if (H1) *H1 = H_predicted_dir; - if (H2) *H2 = -H_predicted_dir; - return predicted - measured_w_aZb_; - } - - private: - friend class boost::serialization::access; - template - void serialize(ARCHIVE& ar, const unsigned int /*version*/) { - ar& boost::serialization::make_nvp( - "Base", boost::serialization::base_object(*this)); - } -}; // \ TranslationFactor -} // namespace gtsam - /* ---------------------------------------------------------------------------- * GTSAM Copyright 2010, Georgia Tech Research Corporation, @@ -98,7 +16,7 @@ class TranslationFactor : public NoiseModelFactor2 { * @brief test recovering translations when rotations are given. */ -// #include +#include #include #include #include From 656f4ad57725110f5f24d2ae51b1648f84773fa8 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 8 Mar 2020 03:26:41 -0400 Subject: [PATCH 06/54] unit test, incl Jacobians --- gtsam/sfm/TranslationFactor.h | 6 +- gtsam/sfm/tests/testTranslationFactor.cpp | 90 +++++++++++++++++++++++ 2 files changed, 93 insertions(+), 3 deletions(-) create mode 100644 gtsam/sfm/tests/testTranslationFactor.cpp diff --git a/gtsam/sfm/TranslationFactor.h b/gtsam/sfm/TranslationFactor.h index 626214dcc..bf65b9bc8 100644 --- a/gtsam/sfm/TranslationFactor.h +++ b/gtsam/sfm/TranslationFactor.h @@ -35,7 +35,7 @@ namespace gtsam { * ambient world coordinate frame: * normalized(Tb - Ta) - w_aZb.point3() * - * @addtogroup SAM + * @addtogroup SFM */ class TranslationFactor : public NoiseModelFactor2 { private: @@ -67,8 +67,8 @@ class TranslationFactor : public NoiseModelFactor2 { Matrix33 H_predicted_dir; const Point3 predicted = dir.normalized(H1 || H2 ? &H_predicted_dir : nullptr); - if (H1) *H1 = H_predicted_dir; - if (H2) *H2 = -H_predicted_dir; + if (H1) *H1 = -H_predicted_dir; + if (H2) *H2 = H_predicted_dir; return predicted - measured_w_aZb_; } diff --git a/gtsam/sfm/tests/testTranslationFactor.cpp b/gtsam/sfm/tests/testTranslationFactor.cpp new file mode 100644 index 000000000..54928d684 --- /dev/null +++ b/gtsam/sfm/tests/testTranslationFactor.cpp @@ -0,0 +1,90 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file testTranslationFactor.cpp + * @brief Unit tests for TranslationFactor Class + * @author Frank dellaert + * @date March 2020 + */ + +#include +#include +#include + +#include + +using namespace std; +using namespace gtsam; + +// Create a noise model for the chordal error +static SharedNoiseModel model(noiseModel::Isotropic::Sigma(3, 0.05)); + +// Keys are deliberately *not* in sorted order to test that case. +static const Key kKey1(2), kKey2(1); +static const Unit3 kMeasured(1, 0, 0); + +/* ************************************************************************* */ +TEST(TranslationFactor, Constructor) { + TranslationFactor factor(kKey1, kKey2, kMeasured, model); +} + +/* ************************************************************************* */ +TEST(TranslationFactor, Error) { + // Create a factor + TranslationFactor factor(kKey1, kKey2, kMeasured, model); + + // Set the linearization + Point3 T1(1, 0, 0), T2(2, 0, 0); + + // Use the factor to calculate the error + Vector actualError(factor.evaluateError(T1, T2)); + + // Verify we get the expected error + Vector expected = (Vector3() << 0, 0, 0).finished(); + EXPECT(assert_equal(expected, actualError, 1e-9)); +} + +/* ************************************************************************* */ +Vector factorError(const Point3& T1, const Point3& T2, + const TranslationFactor& factor) { + return factor.evaluateError(T1, T2); +} + +TEST(TranslationFactor, Jacobian) { + // Create a factor + TranslationFactor factor(kKey1, kKey2, kMeasured, model); + + // Set the linearization + Point3 T1(1, 0, 0), T2(2, 0, 0); + + // Use the factor to calculate the Jacobians + Matrix H1Actual, H2Actual; + factor.evaluateError(T1, T2, H1Actual, H2Actual); + + // Use numerical derivatives to calculate the Jacobians + Matrix H1Expected, H2Expected; + H1Expected = numericalDerivative11( + boost::bind(&factorError, _1, T2, factor), T1); + H2Expected = numericalDerivative11( + boost::bind(&factorError, T1, _1, factor), T2); + + // Verify the Jacobians are correct + EXPECT(assert_equal(H1Expected, H1Actual, 1e-9)); + EXPECT(assert_equal(H2Expected, H2Actual, 1e-9)); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ From 7910f00c2c6524a27aef5b513c18dd929d9625ca Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 8 Mar 2020 03:50:45 -0400 Subject: [PATCH 07/54] Optimization works! --- tests/testTranslationRecovery.cpp | 49 +++++++++++++++++++++++-------- 1 file changed, 37 insertions(+), 12 deletions(-) diff --git a/tests/testTranslationRecovery.cpp b/tests/testTranslationRecovery.cpp index 8742820f4..94ee14c28 100644 --- a/tests/testTranslationRecovery.cpp +++ b/tests/testTranslationRecovery.cpp @@ -16,7 +16,6 @@ * @brief test recovering translations when rotations are given. */ -#include #include #include #include @@ -26,6 +25,8 @@ #include #include #include +#include +#include namespace gtsam { @@ -57,6 +58,7 @@ class TranslationRecovery { private: TranslationEdges relativeTranslations_; + LevenbergMarquardtParams params_; public: /** @@ -66,7 +68,9 @@ class TranslationRecovery { * frames, indexed in a map by a pair of Pose keys. */ TranslationRecovery(const TranslationEdges& relativeTranslations) - : relativeTranslations_(relativeTranslations) {} + : relativeTranslations_(relativeTranslations) { + params_.setVerbosityLM("Summary"); + } /** * @brief Build the factor graph to do the optimization. @@ -76,15 +80,35 @@ class TranslationRecovery { NonlinearFactorGraph buildGraph() const { auto noiseModel = noiseModel::Isotropic::Sigma(3, 0.01); NonlinearFactorGraph graph; + + // Add all relative translation edges for (auto edge : relativeTranslations_) { Key a, b; std::tie(a, b) = edge.first; const Unit3 w_aZb = edge.second; graph.emplace_shared(a, b, w_aZb, noiseModel); } + return graph; } + /** + * @brief Add priors on ednpoints of first measurement edge. + * + * @param scale scale for first relative translation which fixes gauge. + * @param graph factor graph to which prior is added. + */ + void addPrior(const double scale, NonlinearFactorGraph* graph) const { + auto noiseModel = noiseModel::Isotropic::Sigma(3, 0.01); + auto edge = relativeTranslations_.begin(); + Key a, b; + std::tie(a, b) = edge->first; + const Unit3 w_aZb = edge->second; + graph->emplace_shared >(a, Point3(0, 0, 0), noiseModel); + graph->emplace_shared >(b, scale * w_aZb.point3(), + noiseModel); + } + /** * @brief Create random initial translations. * @@ -115,14 +139,11 @@ class TranslationRecovery { * @return Values */ Values run(const double scale = 1.0) const { - const auto graph = buildGraph(); + auto graph = buildGraph(); + addPrior(scale, &graph); const Values initial = initalizeRandomly(); - - LevenbergMarquardtParams params; - params.setVerbosityLM("Summary"); - LevenbergMarquardtOptimizer lm(graph, initial, params); + LevenbergMarquardtOptimizer lm(graph, initial, params_); Values result = lm.optimize(); - return result; } @@ -212,14 +233,18 @@ TEST(TranslationRecovery, BAL) { EXPECT_LONGS_EQUAL(3, graph.size()); // Translation recovery, version 1 - const auto result = algorithm.run(2); + const double scale = 2.0; + const auto result = algorithm.run(scale); - // Check result + // Check result for first two translations, determined by prior EXPECT(assert_equal(Point3(0, 0, 0), result.at(0))); EXPECT(assert_equal(Point3(2 * w_aZb.point3()), result.at(1))); - EXPECT(assert_equal(Point3(0, 0, 0), result.at(2))); - // TODO(frank): how to get stats back + // Check that the third translations is correct + Point3 expected = (Tc - Ta) * (scale / (Tb - Ta).norm()); + EXPECT(assert_equal(expected, result.at(2), 1e-4)); + + // TODO(frank): how to get stats back? // EXPECT_DOUBLES_EQUAL(0.0199833, actualError, 1e-5); } From f4b2b8bde0590c0c431f9eaba87ba9f1d73e98a0 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 8 Mar 2020 04:04:21 -0400 Subject: [PATCH 08/54] copyright 2020 --- gtsam/sfm/TranslationFactor.h | 2 +- gtsam/sfm/tests/testTranslationFactor.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam/sfm/TranslationFactor.h b/gtsam/sfm/TranslationFactor.h index bf65b9bc8..584b1fa69 100644 --- a/gtsam/sfm/TranslationFactor.h +++ b/gtsam/sfm/TranslationFactor.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010-2020, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/sfm/tests/testTranslationFactor.cpp b/gtsam/sfm/tests/testTranslationFactor.cpp index 54928d684..da284bfd6 100644 --- a/gtsam/sfm/tests/testTranslationFactor.cpp +++ b/gtsam/sfm/tests/testTranslationFactor.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010-2020, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) From 98b8d6b4f3f272ee952271329163a269344eccd4 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 8 Mar 2020 04:04:49 -0400 Subject: [PATCH 09/54] Move to its own compilation unit --- gtsam/sfm/TranslationRecovery.cpp | 103 ++++++++++++++++++ gtsam/sfm/TranslationRecovery.h | 108 ++++++++++++++++++ tests/testTranslationRecovery.cpp | 175 +----------------------------- 3 files changed, 214 insertions(+), 172 deletions(-) create mode 100644 gtsam/sfm/TranslationRecovery.cpp create mode 100644 gtsam/sfm/TranslationRecovery.h diff --git a/gtsam/sfm/TranslationRecovery.cpp b/gtsam/sfm/TranslationRecovery.cpp new file mode 100644 index 000000000..a0f3eb6b6 --- /dev/null +++ b/gtsam/sfm/TranslationRecovery.cpp @@ -0,0 +1,103 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010-2020, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file TranslationRecovery.h + * @author Frank Dellaert + * @date March 2020 + * @brief test recovering translations when rotations are given. + */ + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace gtsam; +using namespace std; + +NonlinearFactorGraph TranslationRecovery::buildGraph() const { + auto noiseModel = noiseModel::Isotropic::Sigma(3, 0.01); + NonlinearFactorGraph graph; + + // Add all relative translation edges + for (auto edge : relativeTranslations_) { + Key a, b; + tie(a, b) = edge.first; + const Unit3 w_aZb = edge.second; + graph.emplace_shared(a, b, w_aZb, noiseModel); + } + + return graph; +} + +void TranslationRecovery::addPrior(const double scale, + NonlinearFactorGraph* graph) const { + auto noiseModel = noiseModel::Isotropic::Sigma(3, 0.01); + auto edge = relativeTranslations_.begin(); + Key a, b; + tie(a, b) = edge->first; + const Unit3 w_aZb = edge->second; + graph->emplace_shared >(a, Point3(0, 0, 0), noiseModel); + graph->emplace_shared >(b, scale * w_aZb.point3(), + noiseModel); +} + +Values TranslationRecovery::initalizeRandomly() const { + // Create a lambda expression that checks whether value exists and randomly + // initializes if not. + Values initial; + auto insert = [&initial](Key j) { + if (!initial.exists(j)) { + initial.insert(j, Vector3::Random()); + } + }; + + // Loop over measurements and add a random translation + for (auto edge : relativeTranslations_) { + Key a, b; + tie(a, b) = edge.first; + insert(a); + insert(b); + } + return initial; +} + +Values TranslationRecovery::run(const double scale) const { + auto graph = buildGraph(); + addPrior(scale, &graph); + const Values initial = initalizeRandomly(); + LevenbergMarquardtOptimizer lm(graph, initial, params_); + Values result = lm.optimize(); + return result; +} + +TranslationRecovery::TranslationEdges TranslationRecovery::SimulateMeasurements( + const Values& poses, const vector& edges) { + TranslationEdges relativeTranslations; + for (auto edge : edges) { + Key a, b; + tie(a, b) = edge; + const Pose3 wTa = poses.at(a), wTb = poses.at(b); + const Point3 Ta = wTa.translation(), Tb = wTb.translation(); + const Unit3 w_aZb(Tb - Ta); + relativeTranslations[edge] = w_aZb; + } + return relativeTranslations; +} diff --git a/gtsam/sfm/TranslationRecovery.h b/gtsam/sfm/TranslationRecovery.h new file mode 100644 index 000000000..5eea251cf --- /dev/null +++ b/gtsam/sfm/TranslationRecovery.h @@ -0,0 +1,108 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010-2020, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file TranslationRecovery.h + * @author Frank Dellaert + * @date March 2020 + * @brief test recovering translations when rotations are given. + */ + +#include +#include +#include + +#include +#include + +namespace gtsam { + +// Set up an optimization problem for the unknown translations Ti in the world +// coordinate frame, given the known camera attitudes wRi with respect to the +// world frame, and a set of (noisy) translation directions of type Unit3, +// w_aZb. The measurement equation is +// w_aZb = Unit3(Tb - Ta) (1) +// i.e., w_aZb is the translation direction from frame A to B, in world +// coordinates. Although Unit3 instances live on a manifold, following +// Wilson14eccv_1DSfM.pdf error we compute the *chordal distance* in the +// ambient world coordinate frame. +// +// It is clear that we cannot recover the scale, nor the absolute position, +// so the gauge freedom in this case is 3 + 1 = 4. We fix these by taking fixing +// the translations Ta and Tb associated with the first measurement w_aZb, +// clamping them to their initial values as given to this method. If no initial +// values are given, we use the origin for Tb and set Tb to make (1) come +// through, i.e., +// Tb = s * wRa * Point3(w_aZb) (2) +// where s is an arbitrary scale that can be supplied, default 1.0. Hence, two +// versions are supplied below corresponding to whether we have initial values +// or not. +class TranslationRecovery { + public: + using KeyPair = std::pair; + using TranslationEdges = std::map; + + private: + TranslationEdges relativeTranslations_; + LevenbergMarquardtParams params_; + + public: + /** + * @brief Construct a new Translation Recovery object + * + * @param relativeTranslations the relative translations, in world coordinate + * frames, indexed in a map by a pair of Pose keys. + */ + TranslationRecovery(const TranslationEdges& relativeTranslations) + : relativeTranslations_(relativeTranslations) { + params_.setVerbosityLM("Summary"); + } + + /** + * @brief Build the factor graph to do the optimization. + * + * @return NonlinearFactorGraph + */ + NonlinearFactorGraph buildGraph() const; + + /** + * @brief Add priors on ednpoints of first measurement edge. + * + * @param scale scale for first relative translation which fixes gauge. + * @param graph factor graph to which prior is added. + */ + void addPrior(const double scale, NonlinearFactorGraph* graph) const; + + /** + * @brief Create random initial translations. + * + * @return Values + */ + Values initalizeRandomly() const; + + /** + * @brief Build and optimize factor graph. + * + * @param scale scale for first relative translation which fixes gauge. + * @return Values + */ + Values run(const double scale = 1.0) const; + + /** + * @brief Simulate translation direction measurements + * + * @param poses SE(3) ground truth poses stored as Values + * @param edges pairs (a,b) for which a measurement w_aZb will be generated. + */ + static TranslationEdges SimulateMeasurements( + const Values& poses, const std::vector& edges); +}; +} // namespace gtsam diff --git a/tests/testTranslationRecovery.cpp b/tests/testTranslationRecovery.cpp index 94ee14c28..774a999e4 100644 --- a/tests/testTranslationRecovery.cpp +++ b/tests/testTranslationRecovery.cpp @@ -1,177 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/** - * @file TranslationRecovery.h - * @author Frank Dellaert - * @date March 2020 - * @brief test recovering translations when rotations are given. - */ - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -namespace gtsam { - -// Set up an optimization problem for the unknown translations Ti in the world -// coordinate frame, given the known camera attitudes wRi with respect to the -// world frame, and a set of (noisy) translation directions of type Unit3, -// w_aZb. The measurement equation is -// w_aZb = Unit3(Tb - Ta) (1) -// i.e., w_aZb is the translation direction from frame A to B, in world -// coordinates. Although Unit3 instances live on a manifold, following -// Wilson14eccv_1DSfM.pdf error we compute the *chordal distance* in the -// ambient world coordinate frame. -// -// It is clear that we cannot recover the scale, nor the absolute position, -// so the gauge freedom in this case is 3 + 1 = 4. We fix these by taking fixing -// the translations Ta and Tb associated with the first measurement w_aZb, -// clamping them to their initial values as given to this method. If no initial -// values are given, we use the origin for Tb and set Tb to make (1) come -// through, i.e., -// Tb = s * wRa * Point3(w_aZb) (2) -// where s is an arbitrary scale that can be supplied, default 1.0. Hence, two -// versions are supplied below corresponding to whether we have initial values -// or not. Because the latter one is called from the first one, this is prime. - -class TranslationRecovery { - public: - using KeyPair = std::pair; - using TranslationEdges = std::map; - - private: - TranslationEdges relativeTranslations_; - LevenbergMarquardtParams params_; - - public: - /** - * @brief Construct a new Translation Recovery object - * - * @param relativeTranslations the relative translations, in world coordinate - * frames, indexed in a map by a pair of Pose keys. - */ - TranslationRecovery(const TranslationEdges& relativeTranslations) - : relativeTranslations_(relativeTranslations) { - params_.setVerbosityLM("Summary"); - } - - /** - * @brief Build the factor graph to do the optimization. - * - * @return NonlinearFactorGraph - */ - NonlinearFactorGraph buildGraph() const { - auto noiseModel = noiseModel::Isotropic::Sigma(3, 0.01); - NonlinearFactorGraph graph; - - // Add all relative translation edges - for (auto edge : relativeTranslations_) { - Key a, b; - std::tie(a, b) = edge.first; - const Unit3 w_aZb = edge.second; - graph.emplace_shared(a, b, w_aZb, noiseModel); - } - - return graph; - } - - /** - * @brief Add priors on ednpoints of first measurement edge. - * - * @param scale scale for first relative translation which fixes gauge. - * @param graph factor graph to which prior is added. - */ - void addPrior(const double scale, NonlinearFactorGraph* graph) const { - auto noiseModel = noiseModel::Isotropic::Sigma(3, 0.01); - auto edge = relativeTranslations_.begin(); - Key a, b; - std::tie(a, b) = edge->first; - const Unit3 w_aZb = edge->second; - graph->emplace_shared >(a, Point3(0, 0, 0), noiseModel); - graph->emplace_shared >(b, scale * w_aZb.point3(), - noiseModel); - } - - /** - * @brief Create random initial translations. - * - * @return Values - */ - Values initalizeRandomly() const { - Values initial; - auto insert = [&initial](Key j) { - if (!initial.exists(j)) { - initial.insert(j, Vector3::Random()); - } - }; - - // Loop over measurements and add a random translation - for (auto edge : relativeTranslations_) { - Key a, b; - std::tie(a, b) = edge.first; - insert(a); - insert(b); - } - return initial; - } - - /** - * @brief Build and optimize factor graph. - * - * @param scale scale for first relative translation which fixes gauge. - * @return Values - */ - Values run(const double scale = 1.0) const { - auto graph = buildGraph(); - addPrior(scale, &graph); - const Values initial = initalizeRandomly(); - LevenbergMarquardtOptimizer lm(graph, initial, params_); - Values result = lm.optimize(); - return result; - } - - /** - * @brief Simulate translation direction measurements - * - * @param poses SE(3) ground truth poses stored as Values - * @param edges pairs (a,b) for which a measurement w_aZb will be generated. - */ - static TranslationEdges SimulateMeasurements( - const Values& poses, const std::vector& edges) { - TranslationEdges relativeTranslations; - for (auto edge : edges) { - Key a, b; - std::tie(a, b) = edge; - const Pose3 wTa = poses.at(a), wTb = poses.at(b); - const Point3 Ta = wTa.translation(), Tb = wTb.translation(); - const Unit3 w_aZb(Tb - Ta); - relativeTranslations[edge] = w_aZb; - } - return relativeTranslations; - } -}; -} // namespace gtsam - -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010-2020, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) @@ -187,6 +16,8 @@ class TranslationRecovery { * @brief test recovering translations when rotations are given. */ +#include + #include #include From 780345bc2764753aae8fbf277dfe477375eff59e Mon Sep 17 00:00:00 2001 From: Gerry Chen Date: Tue, 9 Jun 2020 02:24:58 -0400 Subject: [PATCH 10/54] put file stream inside scope to force buffer flush This was already fixed for serializeXML but not for serializeToXMLFile or deserializeFromXMLFile. --- gtsam/base/serialization.h | 12 ++++-- gtsam/base/serializationTestHelpers.h | 57 +++++++++++++++++++++++++++ 2 files changed, 65 insertions(+), 4 deletions(-) diff --git a/gtsam/base/serialization.h b/gtsam/base/serialization.h index e475465de..088029903 100644 --- a/gtsam/base/serialization.h +++ b/gtsam/base/serialization.h @@ -106,8 +106,10 @@ bool serializeToXMLFile(const T& input, const std::string& filename, const std:: std::ofstream out_archive_stream(filename.c_str()); if (!out_archive_stream.is_open()) return false; - boost::archive::xml_oarchive out_archive(out_archive_stream); - out_archive << boost::serialization::make_nvp(name.c_str(), input);; + { + boost::archive::xml_oarchive out_archive(out_archive_stream); + out_archive << boost::serialization::make_nvp(name.c_str(), input);; + } out_archive_stream.close(); return true; } @@ -117,8 +119,10 @@ bool deserializeFromXMLFile(const std::string& filename, T& output, const std::s std::ifstream in_archive_stream(filename.c_str()); if (!in_archive_stream.is_open()) return false; - boost::archive::xml_iarchive in_archive(in_archive_stream); - in_archive >> boost::serialization::make_nvp(name.c_str(), output); + { + boost::archive::xml_iarchive in_archive(in_archive_stream); + in_archive >> boost::serialization::make_nvp(name.c_str(), output); + } in_archive_stream.close(); return true; } diff --git a/gtsam/base/serializationTestHelpers.h b/gtsam/base/serializationTestHelpers.h index 06b3462e9..178b256a6 100644 --- a/gtsam/base/serializationTestHelpers.h +++ b/gtsam/base/serializationTestHelpers.h @@ -26,6 +26,7 @@ #include #include +#include // whether to print the serialized text to stdout @@ -40,6 +41,13 @@ T create() { return T(); } +std::string resetFilesystem() { + // Create files in folder in build folder + boost::filesystem::remove_all("actual"); + boost::filesystem::create_directory("actual"); + return "actual/"; +} + // Templated round-trip serialization template void roundtrip(const T& input, T& output) { @@ -50,11 +58,22 @@ void roundtrip(const T& input, T& output) { deserialize(serialized, output); } +// Templated round-trip serialization using a file +template +void roundtripFile(const T& input, T& output) { + std::string path = resetFilesystem(); + + serializeToFile(input, path + "graph.dat"); + deserializeFromFile(path + "graph.dat", output); +} + // This version requires equality operator template bool equality(const T& input = T()) { T output = create(); roundtrip(input,output); + if (input != output) return false; + roundtripFile(input,output); return input==output; } @@ -63,6 +82,8 @@ template bool equalsObj(const T& input = T()) { T output = create(); roundtrip(input,output); + if (!assert_equal(input, output)) return false; + roundtripFile(input,output); return assert_equal(input, output); } @@ -71,6 +92,8 @@ template bool equalsDereferenced(const T& input) { T output = create(); roundtrip(input,output); + if (!input->equals(*output)) return false; + roundtripFile(input,output); return input->equals(*output); } @@ -85,11 +108,25 @@ void roundtripXML(const T& input, T& output) { deserializeXML(serialized, output); } +// Templated round-trip serialization using XML File +template +void roundtripXMLFile(const T& input, T& output) { + std::string path = resetFilesystem(); + + // Serialize + serializeToXMLFile(input, path + "graph.xml"); + + // De-serialize + deserializeFromXMLFile(path + "graph.xml", output); +} + // This version requires equality operator template bool equalityXML(const T& input = T()) { T output = create(); roundtripXML(input,output); + if (input != output) return false; + roundtripXMLFile(input,output); return input==output; } @@ -98,6 +135,8 @@ template bool equalsXML(const T& input = T()) { T output = create(); roundtripXML(input,output); + if (!assert_equal(input, output)) return false; + roundtripXMLFile(input, output); return assert_equal(input, output); } @@ -106,6 +145,8 @@ template bool equalsDereferencedXML(const T& input = T()) { T output = create(); roundtripXML(input,output); + if (!input->equals(*output)) return false; + roundtripXMLFile(input, output); return input->equals(*output); } @@ -120,11 +161,23 @@ void roundtripBinary(const T& input, T& output) { deserializeBinary(serialized, output); } +// Templated round-trip serialization using XML +template +void roundtripBinaryFile(const T& input, T& output) { + std::string path = resetFilesystem(); + // Serialize + serializeToBinaryFile(input, path + "graph.bin"); + // De-serialize + deserializeFromBinaryFile(path + "graph.bin", output); +} + // This version requires equality operator template bool equalityBinary(const T& input = T()) { T output = create(); roundtripBinary(input,output); + if (input != output) return false; + roundtripBinaryFile(input,output); return input==output; } @@ -133,6 +186,8 @@ template bool equalsBinary(const T& input = T()) { T output = create(); roundtripBinary(input,output); + if (!assert_equal(input, output)) return false; + roundtripBinaryFile(input,output); return assert_equal(input, output); } @@ -141,6 +196,8 @@ template bool equalsDereferencedBinary(const T& input = T()) { T output = create(); roundtripBinary(input,output); + if (!input->equals(*output)) return false; + roundtripBinaryFile(input,output); return input->equals(*output); } From c94736b6b6959820efe628d52cc8cd17c0269cdc Mon Sep 17 00:00:00 2001 From: Gerry Chen Date: Tue, 9 Jun 2020 02:39:54 -0400 Subject: [PATCH 11/54] bypass assert_equal tests for file roundtrips --- gtsam/base/serializationTestHelpers.h | 6 ------ 1 file changed, 6 deletions(-) diff --git a/gtsam/base/serializationTestHelpers.h b/gtsam/base/serializationTestHelpers.h index 178b256a6..d29899072 100644 --- a/gtsam/base/serializationTestHelpers.h +++ b/gtsam/base/serializationTestHelpers.h @@ -82,8 +82,6 @@ template bool equalsObj(const T& input = T()) { T output = create(); roundtrip(input,output); - if (!assert_equal(input, output)) return false; - roundtripFile(input,output); return assert_equal(input, output); } @@ -135,8 +133,6 @@ template bool equalsXML(const T& input = T()) { T output = create(); roundtripXML(input,output); - if (!assert_equal(input, output)) return false; - roundtripXMLFile(input, output); return assert_equal(input, output); } @@ -186,8 +182,6 @@ template bool equalsBinary(const T& input = T()) { T output = create(); roundtripBinary(input,output); - if (!assert_equal(input, output)) return false; - roundtripBinaryFile(input,output); return assert_equal(input, output); } From 686b0effbf4e6d222ed26c17d2c00bb7dc878551 Mon Sep 17 00:00:00 2001 From: Gerry Chen Date: Tue, 9 Jun 2020 02:48:53 -0400 Subject: [PATCH 12/54] better comments on serializationTestHelper functions --- gtsam/base/serializationTestHelpers.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam/base/serializationTestHelpers.h b/gtsam/base/serializationTestHelpers.h index d29899072..031a6278f 100644 --- a/gtsam/base/serializationTestHelpers.h +++ b/gtsam/base/serializationTestHelpers.h @@ -41,8 +41,8 @@ T create() { return T(); } +// Creates or empties a folder in the build folder and returns the relative path std::string resetFilesystem() { - // Create files in folder in build folder boost::filesystem::remove_all("actual"); boost::filesystem::create_directory("actual"); return "actual/"; @@ -157,7 +157,7 @@ void roundtripBinary(const T& input, T& output) { deserializeBinary(serialized, output); } -// Templated round-trip serialization using XML +// Templated round-trip serialization using Binary file template void roundtripBinaryFile(const T& input, T& output) { std::string path = resetFilesystem(); From 8a9ade00254e403f15471b17ce9fe75fcf0da084 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 11 Jun 2020 10:58:44 -0500 Subject: [PATCH 13/54] remove extra semicolon --- gtsam/base/serialization.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/base/serialization.h b/gtsam/base/serialization.h index 088029903..191e0ecfb 100644 --- a/gtsam/base/serialization.h +++ b/gtsam/base/serialization.h @@ -108,7 +108,7 @@ bool serializeToXMLFile(const T& input, const std::string& filename, const std:: return false; { boost::archive::xml_oarchive out_archive(out_archive_stream); - out_archive << boost::serialization::make_nvp(name.c_str(), input);; + out_archive << boost::serialization::make_nvp(name.c_str(), input); } out_archive_stream.close(); return true; From 0a44315a7f3b1b2f6e1953e5b73ece40c7097a01 Mon Sep 17 00:00:00 2001 From: David Date: Sat, 20 Jun 2020 08:46:06 +1000 Subject: [PATCH 14/54] Add Pose3-Point3 factor --- gtsam_unstable/slam/PoseToPointFactor.h | 95 +++++++++++++++++++ .../slam/tests/testPoseToPointFactor.h | 84 ++++++++++++++++ 2 files changed, 179 insertions(+) create mode 100644 gtsam_unstable/slam/PoseToPointFactor.h create mode 100644 gtsam_unstable/slam/tests/testPoseToPointFactor.h diff --git a/gtsam_unstable/slam/PoseToPointFactor.h b/gtsam_unstable/slam/PoseToPointFactor.h new file mode 100644 index 000000000..98efd6a0b --- /dev/null +++ b/gtsam_unstable/slam/PoseToPointFactor.h @@ -0,0 +1,95 @@ +/** + * @file PoseToPointFactor.hpp + * @brief This factor can be used to track a 3D landmark over time by providing + * local measurements of its location. + * @author David Wisth + **/ +#pragma once + +#include +#include +#include +#include + +namespace gtsam { + +/** + * A class for a measurement between a pose and a point. + * @addtogroup SLAM + */ +class PoseToPointFactor: public NoiseModelFactor2 { + +private: + + typedef PoseToPointFactor This; + typedef NoiseModelFactor2 Base; + + Point3 measured_; /** the point measurement */ + +public: + + // shorthand for a smart pointer to a factor + typedef boost::shared_ptr shared_ptr; + + /** default constructor - only use for serialization */ + PoseToPointFactor() {} + + /** Constructor */ + PoseToPointFactor(Key key1, Key key2, const Point3& measured, + const SharedNoiseModel& model) : + Base(model, key1, key2), measured_(measured) + { + } + + virtual ~PoseToPointFactor() {} + + /** implement functions needed for Testable */ + + /** print */ + virtual void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { + std::cout << s << "PoseToPointFactor(" + << keyFormatter(this->key1()) << "," + << keyFormatter(this->key2()) << ")\n" + << " measured: " << measured_.transpose() << std::endl; + this->noiseModel_->print(" noise model: "); + } + + /** equals */ + virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const { + const This *e = dynamic_cast (&expected); + return e != nullptr && Base::equals(*e, tol) && traits::Equals(this->measured_, e->measured_, tol); + } + + /** implement functions needed to derive from Factor */ + + /** vector of errors + * Error = pose_est.inverse()*point_est - measured_ + * (The error is in the measurement coordinate system) + */ + Vector evaluateError(const Pose3& W_T_WI, + const Point3& W_r_WC, + boost::optional H1 = boost::none, + boost::optional H2 = boost::none) const + { + return W_T_WI.transformTo(W_r_WC, H1, H2) - measured_; + } + + /** return the measured */ + const Point3& measured() const { + return measured_; + } + +private: + + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { + ar & boost::serialization::make_nvp("NoiseModelFactor2", + boost::serialization::base_object(*this)); + ar & BOOST_SERIALIZATION_NVP(measured_); + } + +}; // \class PoseToPointFactor + +} /// namespace gtsam diff --git a/gtsam_unstable/slam/tests/testPoseToPointFactor.h b/gtsam_unstable/slam/tests/testPoseToPointFactor.h new file mode 100644 index 000000000..0a13e78e2 --- /dev/null +++ b/gtsam_unstable/slam/tests/testPoseToPointFactor.h @@ -0,0 +1,84 @@ +/** + * @file testPoseToPointFactor.cpp + * @brief + * @author David Wisth + * @date June 20, 2020 + */ + +#include +#include +#include + +using namespace gtsam; +using namespace gtsam::noiseModel; + +/// Verify zero error when there is no noise +TEST(LandmarkFactor, errorNoiseless) { + Pose3 pose = Pose3::identity(); + Point3 point(1.0, 2.0, 3.0); + Point3 noise(0.0, 0.0, 0.0); + Point3 measured = t + noise; + + Key pose_key(1); + Key point_key(2); + PoseToPointFactor factor(pose_key, point_key, measured, Isotropic::Sigma(3, 0.05)); + Vector expectedError = Vector3(0.0, 0.0, 0.0); + Vector actualError = factor.evaluateError(pose, point); + EXPECT(assert_equal(expectedError,actualError, 1E-5)); +} + +/// Verify expected error in test scenario +TEST(LandmarkFactor, errorNoise) { + Pose3 pose = Pose3::identity(); + Point3 point(1.0 , 2.0, 3.0); + Point3 noise(-1.0, 0.5, 0.3); + Point3 measured = t + noise; + + Key pose_key(1); + Key point_key(2); + PoseToPointFactor factor(pose_key, point_key, measured, Isotropic::Sigma(3, 0.05)); + Vector expectedError = noise; + Vector actualError = factor.evaluateError(pose, point); + EXPECT(assert_equal(expectedError,actualError, 1E-5)); +} + +/// Check Jacobians are correct +TEST(LandmarkFactor, jacobian) { + // Measurement + gtsam::Point3 l_meas = gtsam::Point3(1, 2, 3); + + // Linearisation point + gtsam::Point3 p_t = gtsam::Point3(-5, 12, 2); + gtsam::Rot3 p_R = gtsam::Rot3::RzRyRx(1.5*M_PI, -0.3*M_PI, 0.4*M_PI); + Pose3 p(p_R, p_t); + + gtsam::Point3 l = gtsam::Point3(3, 0, 5); + + // Factor + Key pose_key(1); + Key point_key(2); + SharedGaussian noise = noiseModel::Diagonal::Sigmas(Vector3(0.1, 0.1, 0.1)); + PoseToPointFactor factor(pose_key, point_key, l_meas, noise); + + // Calculate numerical derivatives + boost::function f = boost::bind( + &LandmarkFactor::evaluateError, factor, _1, _2, boost::none, boost::none); + Matrix numerical_H1 = numericalDerivative21(f, p, l); + Matrix numerical_H2 = numericalDerivative22(f, p, l); + + // Use the factor to calculate the derivative + Matrix actual_H1; + Matrix actual_H2; + factor.evaluateError(p, l, actual_H1, actual_H2); + + // Verify we get the expected error + EXPECT_TRUE(assert_equal(numerical_H1, actual_H1, 1e-8)); + EXPECT_TRUE(assert_equal(numerical_H2, actual_H2, 1e-8)); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ From c422815b9402a0a0840c2902b8df647e6d527ca5 Mon Sep 17 00:00:00 2001 From: David Date: Sat, 20 Jun 2020 09:03:17 +1000 Subject: [PATCH 15/54] Update incorrect test name --- gtsam_unstable/slam/tests/testPoseToPointFactor.h | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/gtsam_unstable/slam/tests/testPoseToPointFactor.h b/gtsam_unstable/slam/tests/testPoseToPointFactor.h index 0a13e78e2..4b2793700 100644 --- a/gtsam_unstable/slam/tests/testPoseToPointFactor.h +++ b/gtsam_unstable/slam/tests/testPoseToPointFactor.h @@ -13,7 +13,7 @@ using namespace gtsam; using namespace gtsam::noiseModel; /// Verify zero error when there is no noise -TEST(LandmarkFactor, errorNoiseless) { +TEST(PoseToPointFactor, errorNoiseless) { Pose3 pose = Pose3::identity(); Point3 point(1.0, 2.0, 3.0); Point3 noise(0.0, 0.0, 0.0); @@ -28,7 +28,7 @@ TEST(LandmarkFactor, errorNoiseless) { } /// Verify expected error in test scenario -TEST(LandmarkFactor, errorNoise) { +TEST(PoseToPointFactor, errorNoise) { Pose3 pose = Pose3::identity(); Point3 point(1.0 , 2.0, 3.0); Point3 noise(-1.0, 0.5, 0.3); @@ -43,7 +43,7 @@ TEST(LandmarkFactor, errorNoise) { } /// Check Jacobians are correct -TEST(LandmarkFactor, jacobian) { +TEST(PoseToPointFactor, jacobian) { // Measurement gtsam::Point3 l_meas = gtsam::Point3(1, 2, 3); From 0ee4e3b77eaaa879d5e995d92714698e94d11455 Mon Sep 17 00:00:00 2001 From: David Date: Sat, 20 Jun 2020 09:45:24 +1000 Subject: [PATCH 16/54] Add more documentation and clang-format --- gtsam_unstable/slam/PoseToPointFactor.h | 81 +++++++++---------- .../slam/tests/testPoseToPointFactor.h | 22 ++--- 2 files changed, 50 insertions(+), 53 deletions(-) diff --git a/gtsam_unstable/slam/PoseToPointFactor.h b/gtsam_unstable/slam/PoseToPointFactor.h index 98efd6a0b..ec7da22ef 100644 --- a/gtsam_unstable/slam/PoseToPointFactor.h +++ b/gtsam_unstable/slam/PoseToPointFactor.h @@ -1,15 +1,15 @@ /** * @file PoseToPointFactor.hpp - * @brief This factor can be used to track a 3D landmark over time by providing - * local measurements of its location. + * @brief This factor can be used to track a 3D landmark over time by + *providing local measurements of its location. * @author David Wisth **/ #pragma once -#include -#include #include #include +#include +#include namespace gtsam { @@ -17,17 +17,14 @@ namespace gtsam { * A class for a measurement between a pose and a point. * @addtogroup SLAM */ -class PoseToPointFactor: public NoiseModelFactor2 { - -private: - +class PoseToPointFactor : public NoiseModelFactor2 { + private: typedef PoseToPointFactor This; typedef NoiseModelFactor2 Base; - Point3 measured_; /** the point measurement */ - -public: + Point3 measured_; /** the point measurement in local coordinates */ + public: // shorthand for a smart pointer to a factor typedef boost::shared_ptr shared_ptr; @@ -36,60 +33,58 @@ public: /** Constructor */ PoseToPointFactor(Key key1, Key key2, const Point3& measured, - const SharedNoiseModel& model) : - Base(model, key1, key2), measured_(measured) - { - } + const SharedNoiseModel& model) + : Base(model, key1, key2), measured_(measured) {} virtual ~PoseToPointFactor() {} /** implement functions needed for Testable */ /** print */ - virtual void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { - std::cout << s << "PoseToPointFactor(" - << keyFormatter(this->key1()) << "," - << keyFormatter(this->key2()) << ")\n" - << " measured: " << measured_.transpose() << std::endl; + virtual void print(const std::string& s, const KeyFormatter& keyFormatter = + DefaultKeyFormatter) const { + std::cout << s << "PoseToPointFactor(" << keyFormatter(this->key1()) << "," + << keyFormatter(this->key2()) << ")\n" + << " measured: " << measured_.transpose() << std::endl; this->noiseModel_->print(" noise model: "); } /** equals */ - virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const { - const This *e = dynamic_cast (&expected); - return e != nullptr && Base::equals(*e, tol) && traits::Equals(this->measured_, e->measured_, tol); + virtual bool equals(const NonlinearFactor& expected, + double tol = 1e-9) const { + const This* e = dynamic_cast(&expected); + return e != nullptr && Base::equals(*e, tol) && + traits::Equals(this->measured_, e->measured_, tol); } /** implement functions needed to derive from Factor */ /** vector of errors - * Error = pose_est.inverse()*point_est - measured_ - * (The error is in the measurement coordinate system) - */ - Vector evaluateError(const Pose3& W_T_WI, - const Point3& W_r_WC, + * @brief Error = wTwi.inverse()*wPwp - measured_ + * @param wTwi The pose of the sensor in world coordinates + * @param wPwp The estimated point location in world coordinates + * + * Note: measured_ and the error are in local coordiantes. + */ + Vector evaluateError(const Pose3& wTwi, const Point3& wPwp, boost::optional H1 = boost::none, - boost::optional H2 = boost::none) const - { - return W_T_WI.transformTo(W_r_WC, H1, H2) - measured_; + boost::optional H2 = boost::none) const { + return wTwi.transformTo(wPwp, H1, H2) - measured_; } /** return the measured */ - const Point3& measured() const { - return measured_; - } - -private: + const Point3& measured() const { return measured_; } + private: /** Serialization function */ friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int /*version*/) { - ar & boost::serialization::make_nvp("NoiseModelFactor2", - boost::serialization::base_object(*this)); - ar & BOOST_SERIALIZATION_NVP(measured_); + template + void serialize(ARCHIVE& ar, const unsigned int /*version*/) { + ar& boost::serialization::make_nvp( + "NoiseModelFactor2", boost::serialization::base_object(*this)); + ar& BOOST_SERIALIZATION_NVP(measured_); } -}; // \class PoseToPointFactor +}; // \class PoseToPointFactor -} /// namespace gtsam +} // namespace gtsam diff --git a/gtsam_unstable/slam/tests/testPoseToPointFactor.h b/gtsam_unstable/slam/tests/testPoseToPointFactor.h index 4b2793700..8f8563e9d 100644 --- a/gtsam_unstable/slam/tests/testPoseToPointFactor.h +++ b/gtsam_unstable/slam/tests/testPoseToPointFactor.h @@ -5,9 +5,9 @@ * @date June 20, 2020 */ +#include #include #include -#include using namespace gtsam; using namespace gtsam::noiseModel; @@ -21,25 +21,27 @@ TEST(PoseToPointFactor, errorNoiseless) { Key pose_key(1); Key point_key(2); - PoseToPointFactor factor(pose_key, point_key, measured, Isotropic::Sigma(3, 0.05)); + PoseToPointFactor factor(pose_key, point_key, measured, + Isotropic::Sigma(3, 0.05)); Vector expectedError = Vector3(0.0, 0.0, 0.0); Vector actualError = factor.evaluateError(pose, point); - EXPECT(assert_equal(expectedError,actualError, 1E-5)); + EXPECT(assert_equal(expectedError, actualError, 1E-5)); } /// Verify expected error in test scenario TEST(PoseToPointFactor, errorNoise) { Pose3 pose = Pose3::identity(); - Point3 point(1.0 , 2.0, 3.0); + Point3 point(1.0, 2.0, 3.0); Point3 noise(-1.0, 0.5, 0.3); Point3 measured = t + noise; Key pose_key(1); Key point_key(2); - PoseToPointFactor factor(pose_key, point_key, measured, Isotropic::Sigma(3, 0.05)); + PoseToPointFactor factor(pose_key, point_key, measured, + Isotropic::Sigma(3, 0.05)); Vector expectedError = noise; Vector actualError = factor.evaluateError(pose, point); - EXPECT(assert_equal(expectedError,actualError, 1E-5)); + EXPECT(assert_equal(expectedError, actualError, 1E-5)); } /// Check Jacobians are correct @@ -48,8 +50,8 @@ TEST(PoseToPointFactor, jacobian) { gtsam::Point3 l_meas = gtsam::Point3(1, 2, 3); // Linearisation point - gtsam::Point3 p_t = gtsam::Point3(-5, 12, 2); - gtsam::Rot3 p_R = gtsam::Rot3::RzRyRx(1.5*M_PI, -0.3*M_PI, 0.4*M_PI); + gtsam::Point3 p_t = gtsam::Point3(-5, 12, 2); + gtsam::Rot3 p_R = gtsam::Rot3::RzRyRx(1.5 * M_PI, -0.3 * M_PI, 0.4 * M_PI); Pose3 p(p_R, p_t); gtsam::Point3 l = gtsam::Point3(3, 0, 5); @@ -61,8 +63,8 @@ TEST(PoseToPointFactor, jacobian) { PoseToPointFactor factor(pose_key, point_key, l_meas, noise); // Calculate numerical derivatives - boost::function f = boost::bind( - &LandmarkFactor::evaluateError, factor, _1, _2, boost::none, boost::none); + auto f = boost::bind(&PoseToPointFactor::evaluateError, factor, _1, _2, + boost::none, boost::none); Matrix numerical_H1 = numericalDerivative21(f, p, l); Matrix numerical_H2 = numericalDerivative22(f, p, l); From 0bd81430573a32712642a546880600f498cac745 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Fri, 19 Jun 2020 23:33:29 -0400 Subject: [PATCH 17/54] Importing Frobenius error factors from Shonan effort --- cython/gtsam/tests/test_FrobeniusFactor.py | 56 +++++ gtsam.h | 29 +++ gtsam/slam/FrobeniusFactor.cpp | 117 ++++++++++ gtsam/slam/FrobeniusFactor.h | 145 ++++++++++++ gtsam/slam/tests/testFrobeniusFactor.cpp | 244 +++++++++++++++++++++ timing/timeFrobeniusFactor.cpp | 110 ++++++++++ 6 files changed, 701 insertions(+) create mode 100644 cython/gtsam/tests/test_FrobeniusFactor.py create mode 100644 gtsam/slam/FrobeniusFactor.cpp create mode 100644 gtsam/slam/FrobeniusFactor.h create mode 100644 gtsam/slam/tests/testFrobeniusFactor.cpp create mode 100644 timing/timeFrobeniusFactor.cpp diff --git a/cython/gtsam/tests/test_FrobeniusFactor.py b/cython/gtsam/tests/test_FrobeniusFactor.py new file mode 100644 index 000000000..82d1fb410 --- /dev/null +++ b/cython/gtsam/tests/test_FrobeniusFactor.py @@ -0,0 +1,56 @@ +""" +GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved + +See LICENSE for the license information + +FrobeniusFactor unit tests. +Author: Frank Dellaert +""" +# pylint: disable=no-name-in-module, import-error, invalid-name +import unittest + +import numpy as np +from gtsam import (SO3, SO4, FrobeniusBetweenFactorSO4, FrobeniusFactorSO4, + FrobeniusWormholeFactor, SOn) + +id = SO4() +v1 = np.array([0, 0, 0, 0.1, 0, 0]) +Q1 = SO4.Expmap(v1) +v2 = np.array([0, 0, 0, 0.01, 0.02, 0.03]) +Q2 = SO4.Expmap(v2) + + +class TestFrobeniusFactorSO4(unittest.TestCase): + """Test FrobeniusFactor factors.""" + + def test_frobenius_factor(self): + """Test creation of a factor that calculates the Frobenius norm.""" + factor = FrobeniusFactorSO4(1, 2) + actual = factor.evaluateError(Q1, Q2) + expected = (Q2.matrix()-Q1.matrix()).transpose().reshape((16,)) + np.testing.assert_array_equal(actual, expected) + + def test_frobenius_between_factor(self): + """Test creation of a Frobenius BetweenFactor.""" + factor = FrobeniusBetweenFactorSO4(1, 2, Q1.between(Q2)) + actual = factor.evaluateError(Q1, Q2) + expected = np.zeros((16,)) + np.testing.assert_array_almost_equal(actual, expected) + + def test_frobenius_wormhole_factor(self): + """Test creation of a factor that calculates Shonan error.""" + R1 = SO3.Expmap(v1[3:]) + R2 = SO3.Expmap(v2[3:]) + factor = FrobeniusWormholeFactor(1, 2, R1.between(R2), p=4) + I4 = SOn(4) + Q1 = I4.retract(v1) + Q2 = I4.retract(v2) + actual = factor.evaluateError(Q1, Q2) + expected = np.zeros((12,)) + np.testing.assert_array_almost_equal(actual, expected, decimal=4) + + +if __name__ == "__main__": + unittest.main() diff --git a/gtsam.h b/gtsam.h index 614db91c7..cbf737390 100644 --- a/gtsam.h +++ b/gtsam.h @@ -598,6 +598,7 @@ class SOn { // Standard Constructors SOn(size_t n); static gtsam::SOn FromMatrix(Matrix R); + static gtsam::SOn Lift(size_t n, Matrix R); // Testable void print(string s) const; @@ -2835,6 +2836,34 @@ virtual class KarcherMeanFactor : gtsam::NonlinearFactor { KarcherMeanFactor(const gtsam::KeyVector& keys); }; +#include +gtsam::noiseModel::Isotropic* ConvertPose3NoiseModel( + gtsam::noiseModel::Base* model, size_t d); + +template +virtual class FrobeniusFactor : gtsam::NoiseModelFactor { + FrobeniusFactor(size_t key1, size_t key2); + FrobeniusFactor(size_t key1, size_t key2, gtsam::noiseModel::Base* model); + + Vector evaluateError(const T& R1, const T& R2); +}; + +template +virtual class FrobeniusBetweenFactor : gtsam::NoiseModelFactor { + FrobeniusBetweenFactor(size_t key1, size_t key2, const T& R12); + FrobeniusBetweenFactor(size_t key1, size_t key2, const T& R12, gtsam::noiseModel::Base* model); + + Vector evaluateError(const T& R1, const T& R2); +}; + +virtual class FrobeniusWormholeFactor : gtsam::NoiseModelFactor { + FrobeniusWormholeFactor(size_t key1, size_t key2, const gtsam::Rot3& R12, + size_t p); + FrobeniusWormholeFactor(size_t key1, size_t key2, const gtsam::Rot3& R12, + size_t p, gtsam::noiseModel::Base* model); + Vector evaluateError(const gtsam::SOn& Q1, const gtsam::SOn& Q2); +}; + //************************************************************************* // Navigation //************************************************************************* diff --git a/gtsam/slam/FrobeniusFactor.cpp b/gtsam/slam/FrobeniusFactor.cpp new file mode 100644 index 000000000..904addb03 --- /dev/null +++ b/gtsam/slam/FrobeniusFactor.cpp @@ -0,0 +1,117 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file FrobeniusFactor.cpp + * @date March 2019 + * @author Frank Dellaert + * @brief Various factors that minimize some Frobenius norm + */ + +#include + +#include +#include +#include + +#include +#include +#include + +using namespace std; + +namespace gtsam { + +//****************************************************************************** +boost::shared_ptr ConvertPose3NoiseModel( + const SharedNoiseModel& model, size_t d, bool defaultToUnit) { + double sigma = 1.0; + if (model != nullptr) { + if (model->dim() != 6) { + if (!defaultToUnit) + throw std::runtime_error("Can only convert Pose3 noise models"); + } else { + auto sigmas = model->sigmas().head(3).eval(); + if (sigmas(1) != sigmas(0) || sigmas(2) != sigmas(0)) { + if (!defaultToUnit) + throw std::runtime_error("Can only convert isotropic rotation noise"); + } else { + sigma = sigmas(0); + } + } + } + return noiseModel::Isotropic::Sigma(d, sigma); +} + +//****************************************************************************** +FrobeniusWormholeFactor::FrobeniusWormholeFactor(Key j1, Key j2, const Rot3& R12, + size_t p, + const SharedNoiseModel& model) + : NoiseModelFactor2(ConvertPose3NoiseModel(model, p * 3), j1, j2), + M_(R12.matrix()), // 3*3 in all cases + p_(p), // 4 for SO(4) + pp_(p * p), // 16 for SO(4) + dimension_(SOn::Dimension(p)), // 6 for SO(4) + G_(pp_, dimension_) // 16*6 for SO(4) +{ + // Calculate G matrix of vectorized generators + Matrix Z = Matrix::Zero(p, p); + for (size_t j = 0; j < dimension_; j++) { + const auto X = SOn::Hat(Eigen::VectorXd::Unit(dimension_, j)); + G_.col(j) = Eigen::Map(X.data(), pp_, 1); + } + assert(noiseModel()->dim() == 3 * p_); +} + +//****************************************************************************** +Vector FrobeniusWormholeFactor::evaluateError( + const SOn& Q1, const SOn& Q2, boost::optional H1, + boost::optional H2) const { + gttic(FrobeniusWormholeFactorP_evaluateError); + + const Matrix& M1 = Q1.matrix(); + const Matrix& M2 = Q2.matrix(); + assert(M1.rows() == p_ && M2.rows() == p_); + + const size_t dim = 3 * p_; // Stiefel manifold dimension + Vector fQ2(dim), hQ1(dim); + + // Vectorize and extract only d leftmost columns, i.e. vec(M2*P) + fQ2 << Eigen::Map(M2.data(), dim, 1); + + // Vectorize M1*P*R12 + const Matrix Q1PR12 = M1.leftCols<3>() * M_; + hQ1 << Eigen::Map(Q1PR12.data(), dim, 1); + + // If asked, calculate Jacobian as (M \otimes M1) * G + if (H1) { + const size_t p2 = 2 * p_; + Matrix RPxQ = Matrix::Zero(dim, pp_); + RPxQ.block(0, 0, p_, dim) << M1 * M_(0, 0), M1 * M_(1, 0), M1 * M_(2, 0); + RPxQ.block(p_, 0, p_, dim) << M1 * M_(0, 1), M1 * M_(1, 1), M1 * M_(2, 1); + RPxQ.block(p2, 0, p_, dim) << M1 * M_(0, 2), M1 * M_(1, 2), M1 * M_(2, 2); + *H1 = -RPxQ * G_; + } + if (H2) { + const size_t p2 = 2 * p_; + Matrix PxQ = Matrix::Zero(dim, pp_); + PxQ.block(0, 0, p_, p_) = M2; + PxQ.block(p_, p_, p_, p_) = M2; + PxQ.block(p2, p2, p_, p_) = M2; + *H2 = PxQ * G_; + } + + return fQ2 - hQ1; +} + +//****************************************************************************** + +} // namespace gtsam diff --git a/gtsam/slam/FrobeniusFactor.h b/gtsam/slam/FrobeniusFactor.h new file mode 100644 index 000000000..60fa82ab4 --- /dev/null +++ b/gtsam/slam/FrobeniusFactor.h @@ -0,0 +1,145 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file FrobeniusFactor.h + * @date March 2019 + * @author Frank Dellaert + * @brief Various factors that minimize some Frobenius norm + */ + +#pragma once + +#include +#include +#include + +namespace gtsam { + +/** + * When creating (any) FrobeniusFactor we convert a 6-dimensional Pose3 + * BetweenFactor noise model into an 9 or 16-dimensional isotropic noise + * model used to weight the Frobenius norm. If the noise model passed is + * null we return a Dim-dimensional isotropic noise model with sigma=1.0. If + * not, we we check if the 3-dimensional noise model on rotations is + * isotropic. If it is, we extend to 'Dim' dimensions, otherwise we throw an + * error. If defaultToUnit == false throws an exception on unexepcted input. + */ +boost::shared_ptr ConvertPose3NoiseModel( + const SharedNoiseModel& model, size_t d, bool defaultToUnit = true); + +/** + * FrobeniusPrior calculates the Frobenius norm between a given matrix and an + * element of SO(3) or SO(4). + */ +template +class FrobeniusPrior : public NoiseModelFactor1 { + enum { Dim = Rot::VectorN2::RowsAtCompileTime }; + using MatrixNN = typename Rot::MatrixNN; + Eigen::Matrix vecM_; ///< vectorized matrix to approximate + + public: + /// Constructor + FrobeniusPrior(Key j, const MatrixNN& M, + const SharedNoiseModel& model = nullptr) + : NoiseModelFactor1(ConvertPose3NoiseModel(model, Dim), j) { + vecM_ << Eigen::Map(M.data(), Dim, 1); + } + + /// Error is just Frobenius norm between Rot element and vectorized matrix M. + Vector evaluateError(const Rot& R, + boost::optional H = boost::none) const { + return R.vec(H) - vecM_; // Jacobian is computed only when needed. + } +}; + +/** + * FrobeniusFactor calculates the Frobenius norm between rotation matrices. + * The template argument can be any fixed-size SO. + */ +template +class FrobeniusFactor : public NoiseModelFactor2 { + enum { Dim = Rot::VectorN2::RowsAtCompileTime }; + + public: + /// Constructor + FrobeniusFactor(Key j1, Key j2, const SharedNoiseModel& model = nullptr) + : NoiseModelFactor2(ConvertPose3NoiseModel(model, Dim), j1, + j2) {} + + /// Error is just Frobenius norm between rotation matrices. + Vector evaluateError(const Rot& R1, const Rot& R2, + boost::optional H1 = boost::none, + boost::optional H2 = boost::none) const { + Vector error = R2.vec(H2) - R1.vec(H1); + if (H1) *H1 = -*H1; + return error; + } +}; + +/** + * FrobeniusBetweenFactor is a BetweenFactor that evaluates the Frobenius norm + * of the rotation error between measured and predicted (rather than the + * Logmap of the error). This factor is only defined for fixed-dimension types, + * and in fact only SO3 and SO4 really work, as we need SO::AdjointMap. + */ +template +class FrobeniusBetweenFactor : public NoiseModelFactor2 { + Rot R12_; ///< measured rotation between R1 and R2 + Eigen::Matrix + R2hat_H_R1_; ///< fixed derivative of R2hat wrpt R1 + enum { Dim = Rot::VectorN2::RowsAtCompileTime }; + + public: + /// Constructor + FrobeniusBetweenFactor(Key j1, Key j2, const Rot& R12, + const SharedNoiseModel& model = nullptr) + : NoiseModelFactor2( + ConvertPose3NoiseModel(model, Dim), j1, j2), + R12_(R12), + R2hat_H_R1_(R12.inverse().AdjointMap()) {} + + /// Error is Frobenius norm between R1*R12 and R2. + Vector evaluateError(const Rot& R1, const Rot& R2, + boost::optional H1 = boost::none, + boost::optional H2 = boost::none) const { + const Rot R2hat = R1.compose(R12_); + Eigen::Matrix vec_H_R2hat; + Vector error = R2.vec(H2) - R2hat.vec(H1 ? &vec_H_R2hat : nullptr); + if (H1) *H1 = -vec_H_R2hat * R2hat_H_R1_; + return error; + } +}; + +/** + * FrobeniusWormholeFactor is a BetweenFactor that moves in SO(p), but will + * land on the SO(3) sub-manifold of SO(p) at the global minimum. It projects + * the SO(p) matrices down to a Stiefel manifold of p*d matrices. + * TODO(frank): template on D=2 or 3 + */ +class FrobeniusWormholeFactor : public NoiseModelFactor2 { + Matrix M_; ///< measured rotation between R1 and R2 + size_t p_, pp_, dimension_; ///< dimensionality constants + Matrix G_; ///< matrix of vectorized generators + + public: + /// Constructor. Note we convert to 3*p-dimensional noise model. + FrobeniusWormholeFactor(Key j1, Key j2, const Rot3& R12, size_t p = 4, + const SharedNoiseModel& model = nullptr); + + /// Error is Frobenius norm between Q1*P*R12 and Q2*P, where P=[I_3x3;0] + /// projects down from SO(p) to the Stiefel manifold of px3 matrices. + Vector evaluateError(const SOn& Q1, const SOn& Q2, + boost::optional H1 = boost::none, + boost::optional H2 = boost::none) const; +}; + +} // namespace gtsam diff --git a/gtsam/slam/tests/testFrobeniusFactor.cpp b/gtsam/slam/tests/testFrobeniusFactor.cpp new file mode 100644 index 000000000..3aefeeb1a --- /dev/null +++ b/gtsam/slam/tests/testFrobeniusFactor.cpp @@ -0,0 +1,244 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * testFrobeniusFactor.cpp + * + * @file testFrobeniusFactor.cpp + * @date March 2019 + * @author Frank Dellaert + * @brief Check evaluateError for various Frobenius norm + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +using namespace std; +using namespace gtsam; + +//****************************************************************************** +namespace so3 { +SO3 id; +Vector3 v1 = (Vector(3) << 0.1, 0, 0).finished(); +SO3 R1 = SO3::Expmap(v1); +Vector3 v2 = (Vector(3) << 0.01, 0.02, 0.03).finished(); +SO3 R2 = SO3::Expmap(v2); +SO3 R12 = R1.between(R2); +} // namespace so3 + +/* ************************************************************************* */ +TEST(FrobeniusPriorSO3, evaluateError) { + using namespace ::so3; + auto factor = FrobeniusPrior(1, R2.matrix()); + Vector actual = factor.evaluateError(R1); + Vector expected = R1.vec() - R2.vec(); + EXPECT(assert_equal(expected, actual, 1e-9)); + + Values values; + values.insert(1, R1); + EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5); +} + +/* ************************************************************************* */ +TEST(FrobeniusPriorSO3, ClosestTo) { + // Example top-left of SO(4) matrix not quite on SO(3) manifold + Matrix3 M; + M << 0.79067393, 0.6051136, -0.0930814, // + 0.4155925, -0.64214347, -0.64324489, // + -0.44948549, 0.47046326, -0.75917576; + + SO3 expected = SO3::ClosestTo(M); + + // manifold optimization gets same result as SVD solution in ClosestTo + NonlinearFactorGraph graph; + graph.emplace_shared >(1, M); + + Values initial; + initial.insert(1, SO3(I_3x3)); + auto result = GaussNewtonOptimizer(graph, initial).optimize(); + EXPECT_DOUBLES_EQUAL(0.0, graph.error(result), 1e-6); + EXPECT(assert_equal(expected, result.at(1), 1e-6)); +} + +/* ************************************************************************* */ +TEST(FrobeniusPriorSO3, ChordalL2mean) { + // See Hartley13ijcv: + // Cost function C(R) = \sum FrobeniusPrior(R,R_i) + // Closed form solution = ClosestTo(C_e), where + // C_e = \sum R_i !!!! + + // We will test by computing mean of R1=exp(v1) R1^T=exp(-v1): + using namespace ::so3; + SO3 expected; // identity + Matrix3 M = R1.matrix() + R1.matrix().transpose(); + EXPECT(assert_equal(expected, SO3::ClosestTo(M), 1e-6)); + EXPECT(assert_equal(expected, SO3::ChordalMean({R1, R1.inverse()}), 1e-6)); + + // manifold optimization gets same result as ChordalMean + NonlinearFactorGraph graph; + graph.emplace_shared >(1, R1.matrix()); + graph.emplace_shared >(1, R1.matrix().transpose()); + + Values initial; + initial.insert(1, R1.inverse()); + auto result = GaussNewtonOptimizer(graph, initial).optimize(); + EXPECT_DOUBLES_EQUAL(0.0, graph.error(result), 0.1); // Why so loose? + EXPECT(assert_equal(expected, result.at(1), 1e-5)); +} + +/* ************************************************************************* */ +TEST(FrobeniusFactorSO3, evaluateError) { + using namespace ::so3; + auto factor = FrobeniusFactor(1, 2); + Vector actual = factor.evaluateError(R1, R2); + Vector expected = R2.vec() - R1.vec(); + EXPECT(assert_equal(expected, actual, 1e-9)); + + Values values; + values.insert(1, R1); + values.insert(2, R2); + EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5); +} + +/* ************************************************************************* */ +// Commented out as SO(n) not yet supported (and might never be) +// TEST(FrobeniusBetweenFactorSOn, evaluateError) { +// using namespace ::so3; +// auto factor = +// FrobeniusBetweenFactor(1, 2, SOn::FromMatrix(R12.matrix())); +// Vector actual = factor.evaluateError(SOn::FromMatrix(R1.matrix()), +// SOn::FromMatrix(R2.matrix())); +// Vector expected = Vector9::Zero(); +// EXPECT(assert_equal(expected, actual, 1e-9)); + +// Values values; +// values.insert(1, R1); +// values.insert(2, R2); +// EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5); +// } + +/* ************************************************************************* */ +TEST(FrobeniusBetweenFactorSO3, evaluateError) { + using namespace ::so3; + auto factor = FrobeniusBetweenFactor(1, 2, R12); + Vector actual = factor.evaluateError(R1, R2); + Vector expected = Vector9::Zero(); + EXPECT(assert_equal(expected, actual, 1e-9)); + + Values values; + values.insert(1, R1); + values.insert(2, R2); + EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5); +} + +//****************************************************************************** +namespace so4 { +SO4 id; +Vector6 v1 = (Vector(6) << 0.1, 0, 0, 0, 0, 0).finished(); +SO4 Q1 = SO4::Expmap(v1); +Vector6 v2 = (Vector(6) << 0.01, 0.02, 0.03, 0.04, 0.05, 0.06).finished(); +SO4 Q2 = SO4::Expmap(v2); +} // namespace so4 + +/* ************************************************************************* */ +TEST(FrobeniusFactorSO4, evaluateError) { + using namespace ::so4; + auto factor = FrobeniusFactor(1, 2, noiseModel::Unit::Create(6)); + Vector actual = factor.evaluateError(Q1, Q2); + Vector expected = Q2.vec() - Q1.vec(); + EXPECT(assert_equal(expected, actual, 1e-9)); + + Values values; + values.insert(1, Q1); + values.insert(2, Q2); + EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5); +} + +/* ************************************************************************* */ +TEST(FrobeniusBetweenFactorSO4, evaluateError) { + using namespace ::so4; + Matrix4 M{I_4x4}; + M.topLeftCorner<3, 3>() = ::so3::R12.matrix(); + auto factor = FrobeniusBetweenFactor(1, 2, Q1.between(Q2)); + Matrix H1, H2; + Vector actual = factor.evaluateError(Q1, Q2, H1, H2); + Vector expected = SO4::VectorN2::Zero(); + EXPECT(assert_equal(expected, actual, 1e-9)); + + Values values; + values.insert(1, Q1); + values.insert(2, Q2); + EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5); +} + +//****************************************************************************** +namespace submanifold { +SO4 id; +Vector6 v1 = (Vector(6) << 0, 0, 0, 0.1, 0, 0).finished(); +SO3 R1 = SO3::Expmap(v1.tail<3>()); +SO4 Q1 = SO4::Expmap(v1); +Vector6 v2 = (Vector(6) << 0, 0, 0, 0.01, 0.02, 0.03).finished(); +SO3 R2 = SO3::Expmap(v2.tail<3>()); +SO4 Q2 = SO4::Expmap(v2); +SO3 R12 = R1.between(R2); +} // namespace submanifold + +/* ************************************************************************* */ +TEST(FrobeniusWormholeFactor, evaluateError) { + auto model = noiseModel::Isotropic::Sigma(6, 1.2); // dimension = 6 not 16 + for (const size_t p : {5, 4, 3}) { + Matrix M = Matrix::Identity(p, p); + M.topLeftCorner(3, 3) = submanifold::R1.matrix(); + SOn Q1(M); + M.topLeftCorner(3, 3) = submanifold::R2.matrix(); + SOn Q2(M); + auto factor = + FrobeniusWormholeFactor(1, 2, Rot3(::so3::R12.matrix()), p, model); + Matrix H1, H2; + factor.evaluateError(Q1, Q2, H1, H2); + + // Test derivatives + Values values; + values.insert(1, Q1); + values.insert(2, Q2); + EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5); + } +} + +/* ************************************************************************* */ +TEST(FrobeniusWormholeFactor, equivalenceToSO3) { + using namespace ::submanifold; + auto R12 = ::so3::R12.retract(Vector3(0.1, 0.2, -0.1)); + auto model = noiseModel::Isotropic::Sigma(6, 1.2); // wrong dimension + auto factor3 = FrobeniusBetweenFactor(1, 2, R12, model); + auto factor4 = FrobeniusWormholeFactor(1, 2, Rot3(R12.matrix()), 4, model); + const Eigen::Map E3(factor3.evaluateError(R1, R2).data()); + const Eigen::Map E4( + factor4.evaluateError(SOn(Q1.matrix()), SOn(Q2.matrix())).data()); + EXPECT(assert_equal((Matrix)E4.topLeftCorner<3, 3>(), E3, 1e-9)); + EXPECT(assert_equal((Matrix)E4.row(3), Matrix13::Zero(), 1e-9)); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ diff --git a/timing/timeFrobeniusFactor.cpp b/timing/timeFrobeniusFactor.cpp new file mode 100644 index 000000000..c8bdd8fec --- /dev/null +++ b/timing/timeFrobeniusFactor.cpp @@ -0,0 +1,110 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file timeFrobeniusFactor.cpp + * @brief time FrobeniusFactor with BAL file + * @author Frank Dellaert + * @date June 6, 2015 + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +using namespace std; +using namespace gtsam; + +static SharedNoiseModel gNoiseModel = noiseModel::Unit::Create(2); + +int main(int argc, char* argv[]) { + // primitive argument parsing: + if (argc > 3) { + throw runtime_error("Usage: timeFrobeniusFactor [g2oFile]"); + } + + string g2oFile; + try { + if (argc > 1) + g2oFile = argv[argc - 1]; + else + g2oFile = + "/Users/dellaert/git/2019c-notes-shonanrotationaveraging/matlabCode/" + "datasets/randomTorus3D.g2o"; + // g2oFile = findExampleDataFile("sphere_smallnoise.graph"); + } catch (const exception& e) { + cerr << e.what() << '\n'; + exit(1); + } + + // Read G2O file + const auto factors = parse3DFactors(g2oFile); + const auto poses = parse3DPoses(g2oFile); + + // Build graph + NonlinearFactorGraph graph; + // graph.add(NonlinearEquality(0, SO4())); + auto priorModel = noiseModel::Isotropic::Sigma(6, 10000); + graph.add(PriorFactor(0, SO4(), priorModel)); + for (const auto& factor : factors) { + const auto& keys = factor->keys(); + const auto& Tij = factor->measured(); + const auto& model = factor->noiseModel(); + graph.emplace_shared( + keys[0], keys[1], SO3(Tij.rotation().matrix()), model); + } + + boost::mt19937 rng(42); + + // Set parameters to be similar to ceres + LevenbergMarquardtParams params; + LevenbergMarquardtParams::SetCeresDefaults(¶ms); + params.setLinearSolverType("MULTIFRONTAL_QR"); + // params.setVerbosityLM("SUMMARY"); + // params.linearSolverType = LevenbergMarquardtParams::Iterative; + // auto pcg = boost::make_shared(); + // pcg->preconditioner_ = + // boost::make_shared(); + // boost::make_shared(); + // params.iterativeParams = pcg; + + // Optimize + for (size_t i = 0; i < 100; i++) { + gttic_(optimize); + Values initial; + initial.insert(0, SO4()); + for (size_t j = 1; j < poses.size(); j++) { + initial.insert(j, SO4::Random(rng)); + } + LevenbergMarquardtOptimizer lm(graph, initial, params); + Values result = lm.optimize(); + cout << "cost = " << graph.error(result) << endl; + } + + tictoc_finishedIteration_(); + tictoc_print_(); + + return 0; +} From 604f1511ccd6bab7e14b6eb71249d29606e5188f Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Sat, 20 Jun 2020 12:09:03 -0400 Subject: [PATCH 18/54] Fix UAF --- gtsam/slam/tests/testFrobeniusFactor.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam/slam/tests/testFrobeniusFactor.cpp b/gtsam/slam/tests/testFrobeniusFactor.cpp index 3aefeeb1a..9cb0c19fa 100644 --- a/gtsam/slam/tests/testFrobeniusFactor.cpp +++ b/gtsam/slam/tests/testFrobeniusFactor.cpp @@ -229,8 +229,8 @@ TEST(FrobeniusWormholeFactor, equivalenceToSO3) { auto model = noiseModel::Isotropic::Sigma(6, 1.2); // wrong dimension auto factor3 = FrobeniusBetweenFactor(1, 2, R12, model); auto factor4 = FrobeniusWormholeFactor(1, 2, Rot3(R12.matrix()), 4, model); - const Eigen::Map E3(factor3.evaluateError(R1, R2).data()); - const Eigen::Map E4( + const Matrix3 E3(factor3.evaluateError(R1, R2).data()); + const Matrix43 E4( factor4.evaluateError(SOn(Q1.matrix()), SOn(Q2.matrix())).data()); EXPECT(assert_equal((Matrix)E4.topLeftCorner<3, 3>(), E3, 1e-9)); EXPECT(assert_equal((Matrix)E4.row(3), Matrix13::Zero(), 1e-9)); From 37673771aec059e14705fad69a0282a9c55e3eba Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Sat, 20 Jun 2020 22:21:17 -0400 Subject: [PATCH 19/54] Fixed all alignment problems --- gtsam.h | 2 +- gtsam/base/make_shared.h | 44 ++++++++++++++++++++++++++++++++++++++++ gtsam/base/types.h | 26 ++++++++++++++++++++++++ gtsam/geometry/SOn.h | 4 ++-- wrap/Module.cpp | 5 ++++- 5 files changed, 77 insertions(+), 4 deletions(-) create mode 100644 gtsam/base/make_shared.h diff --git a/gtsam.h b/gtsam.h index 614db91c7..cf232efa5 100644 --- a/gtsam.h +++ b/gtsam.h @@ -281,7 +281,7 @@ virtual class Value { }; #include -template +template virtual class GenericValue : gtsam::Value { void serializable() const; }; diff --git a/gtsam/base/make_shared.h b/gtsam/base/make_shared.h new file mode 100644 index 000000000..c8b63f5d4 --- /dev/null +++ b/gtsam/base/make_shared.h @@ -0,0 +1,44 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2020, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file make_shared.h + * @brief make_shared trampoline function to ensure proper alignment + * @author Fan Jiang + */ + +#pragma once + +#include +#include + +#include + +#include + +namespace gtsam { + + /** + * And our own `make_shared` as a layer of wrapping on `boost::make_shared` + */ + template + boost::enable_if_t::value, boost::shared_ptr> make_shared(Args&&... args) + { + return boost::allocate_shared(Eigen::aligned_allocator(), std::forward (args)...); + } + + template + boost::enable_if_t::value, boost::shared_ptr> make_shared(Args&&... args) + { + return boost::make_shared(std::forward (args)...); + } + +} \ No newline at end of file diff --git a/gtsam/base/types.h b/gtsam/base/types.h index 2fa6eebb6..3abbdfe81 100644 --- a/gtsam/base/types.h +++ b/gtsam/base/types.h @@ -230,3 +230,29 @@ namespace std { #ifdef ERROR #undef ERROR #endif + +namespace gtsam { + + /** + * A trait to mark classes that need special alignment. + * Please refer to https://github.com/PointCloudLibrary/pcl/pull/3163 + */ +#if __cplusplus < 201703L + template using void_t = void; +#endif + + template> + struct has_custom_allocator : std::false_type { + }; + template + struct has_custom_allocator> : std::true_type { + }; + +} + +/** + * This is necessary for the Cython wrapper to work properly, and possibly reduce future misalignment problems. + */ +#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW \ + EIGEN_MAKE_ALIGNED_OPERATOR_NEW \ + using _custom_allocator_type_trait = void; \ No newline at end of file diff --git a/gtsam/geometry/SOn.h b/gtsam/geometry/SOn.h index 5313d3018..767b12020 100644 --- a/gtsam/geometry/SOn.h +++ b/gtsam/geometry/SOn.h @@ -20,8 +20,8 @@ #include #include +#include #include - #include #include // TODO(frank): how to avoid? @@ -54,7 +54,7 @@ class SO : public LieGroup, internal::DimensionSO(N)> { using VectorN2 = Eigen::Matrix; using MatrixDD = Eigen::Matrix; - EIGEN_MAKE_ALIGNED_OPERATOR_NEW + GTSAM_MAKE_ALIGNED_OPERATOR_NEW protected: MatrixNN matrix_; ///< Rotation matrix diff --git a/wrap/Module.cpp b/wrap/Module.cpp index a7db9e1f6..ec02dc412 100644 --- a/wrap/Module.cpp +++ b/wrap/Module.cpp @@ -350,7 +350,10 @@ void Module::emit_cython_pxd(FileWriter& pxdFile) const { " T* get()\n" " long use_count() const\n" " T& operator*()\n\n" - " cdef shared_ptr[T] dynamic_pointer_cast[T,U](const shared_ptr[U]& r)\n" + " cdef shared_ptr[T] dynamic_pointer_cast[T,U](const shared_ptr[U]& r)\n\n"; + + // gtsam alignment-friendly shared_ptr + pxdFile.oss << "cdef extern from \"gtsam/base/make_shared.h\" namespace \"gtsam\":\n" " cdef shared_ptr[T] make_shared[T](const T& r)\n\n"; for(const TypedefPair& types: typedefs) From fad4fe39f28797f11a61461f3f0224f173046b2e Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Sat, 20 Jun 2020 22:48:40 -0400 Subject: [PATCH 20/54] Add missing include --- gtsam/base/make_shared.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/base/make_shared.h b/gtsam/base/make_shared.h index c8b63f5d4..c5ac98f9c 100644 --- a/gtsam/base/make_shared.h +++ b/gtsam/base/make_shared.h @@ -20,7 +20,7 @@ #include #include -#include +#include #include From 693253f3768bbd618a688a8c173b6f1bd576fdec Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Sat, 20 Jun 2020 22:52:58 -0400 Subject: [PATCH 21/54] Fix wrap tests --- wrap/tests/expected-cython/geometry.pxd | 2 ++ 1 file changed, 2 insertions(+) diff --git a/wrap/tests/expected-cython/geometry.pxd b/wrap/tests/expected-cython/geometry.pxd index 0d9adac5f..3527840a3 100644 --- a/wrap/tests/expected-cython/geometry.pxd +++ b/wrap/tests/expected-cython/geometry.pxd @@ -16,6 +16,8 @@ cdef extern from "boost/shared_ptr.hpp" namespace "boost": T& operator*() cdef shared_ptr[T] dynamic_pointer_cast[T,U](const shared_ptr[U]& r) + +cdef extern from "gtsam/base/make_shared.h" namespace "gtsam": cdef shared_ptr[T] make_shared[T](const T& r) cdef extern from "gtsam/geometry/Point2.h" namespace "gtsam": From eab53f69de01ac06b9d2d20821539b4be4fade21 Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Mon, 22 Jun 2020 13:37:50 -0400 Subject: [PATCH 22/54] Address Frank's comments --- gtsam/base/make_shared.h | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/gtsam/base/make_shared.h b/gtsam/base/make_shared.h index c5ac98f9c..03de30497 100644 --- a/gtsam/base/make_shared.h +++ b/gtsam/base/make_shared.h @@ -17,28 +17,28 @@ #pragma once -#include #include -#include - #include +#include +#include + + namespace gtsam { - /** - * And our own `make_shared` as a layer of wrapping on `boost::make_shared` - */ + /// Add our own `make_shared` as a layer of wrapping on `boost::make_shared` template boost::enable_if_t::value, boost::shared_ptr> make_shared(Args&&... args) { return boost::allocate_shared(Eigen::aligned_allocator(), std::forward (args)...); } + /// Fall back to the boost version if no need for alignment template boost::enable_if_t::value, boost::shared_ptr> make_shared(Args&&... args) { return boost::make_shared(std::forward (args)...); } -} \ No newline at end of file +} From 8f923fa081f0702cb36681675833476c17f692ce Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Mon, 22 Jun 2020 15:18:31 -0400 Subject: [PATCH 23/54] Move away from boost --- gtsam/base/make_shared.h | 13 ++++++++++--- 1 file changed, 10 insertions(+), 3 deletions(-) diff --git a/gtsam/base/make_shared.h b/gtsam/base/make_shared.h index 03de30497..966dd516d 100644 --- a/gtsam/base/make_shared.h +++ b/gtsam/base/make_shared.h @@ -22,21 +22,28 @@ #include #include -#include +#include + +#if __cplusplus <= 201103L +namespace gtsam { + template + using enable_if_t = typename std::enable_if::type; +} +#endif namespace gtsam { /// Add our own `make_shared` as a layer of wrapping on `boost::make_shared` template - boost::enable_if_t::value, boost::shared_ptr> make_shared(Args&&... args) + gtsam::enable_if_t::value, boost::shared_ptr> make_shared(Args&&... args) { return boost::allocate_shared(Eigen::aligned_allocator(), std::forward (args)...); } /// Fall back to the boost version if no need for alignment template - boost::enable_if_t::value, boost::shared_ptr> make_shared(Args&&... args) + gtsam::enable_if_t::value, boost::shared_ptr> make_shared(Args&&... args) { return boost::make_shared(std::forward (args)...); } From 263a1d2afa7714a76b00771579202bbc543fa872 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 22 Jun 2020 16:47:37 -0500 Subject: [PATCH 24/54] export cython install path so it can be picked up by other cmake projects --- gtsam_extra.cmake.in | 1 + 1 file changed, 1 insertion(+) diff --git a/gtsam_extra.cmake.in b/gtsam_extra.cmake.in index 8a9a13648..01ac00b37 100644 --- a/gtsam_extra.cmake.in +++ b/gtsam_extra.cmake.in @@ -9,5 +9,6 @@ set (GTSAM_USE_TBB @GTSAM_USE_TBB@) set (GTSAM_DEFAULT_ALLOCATOR @GTSAM_DEFAULT_ALLOCATOR@) if("@GTSAM_INSTALL_CYTHON_TOOLBOX@") + list(APPEND GTSAM_CYTHON_INSTALL_PATH "@GTSAM_CYTHON_INSTALL_PATH@") list(APPEND GTSAM_EIGENCY_INSTALL_PATH "@GTSAM_EIGENCY_INSTALL_PATH@") endif() From a796f74b8004d7cc12acaf3d20c9b3827a590770 Mon Sep 17 00:00:00 2001 From: Gerry Chen Date: Mon, 22 Jun 2020 20:24:32 -0400 Subject: [PATCH 25/54] use boost paths append to have platform agnostic path separators --- gtsam/base/serializationTestHelpers.h | 27 ++++++++++++++------------- 1 file changed, 14 insertions(+), 13 deletions(-) diff --git a/gtsam/base/serializationTestHelpers.h b/gtsam/base/serializationTestHelpers.h index 031a6278f..602fd68f6 100644 --- a/gtsam/base/serializationTestHelpers.h +++ b/gtsam/base/serializationTestHelpers.h @@ -42,10 +42,11 @@ T create() { } // Creates or empties a folder in the build folder and returns the relative path -std::string resetFilesystem() { - boost::filesystem::remove_all("actual"); - boost::filesystem::create_directory("actual"); - return "actual/"; +boost::filesystem::path resetFilesystem( + boost::filesystem::path folder = "actual") { + boost::filesystem::remove_all(folder); + boost::filesystem::create_directory(folder); + return folder; } // Templated round-trip serialization @@ -61,10 +62,10 @@ void roundtrip(const T& input, T& output) { // Templated round-trip serialization using a file template void roundtripFile(const T& input, T& output) { - std::string path = resetFilesystem(); + boost::filesystem::path path = resetFilesystem()/"graph.dat"; - serializeToFile(input, path + "graph.dat"); - deserializeFromFile(path + "graph.dat", output); + serializeToFile(input, path.string()); + deserializeFromFile(path.string(), output); } // This version requires equality operator @@ -109,13 +110,13 @@ void roundtripXML(const T& input, T& output) { // Templated round-trip serialization using XML File template void roundtripXMLFile(const T& input, T& output) { - std::string path = resetFilesystem(); + boost::filesystem::path path = resetFilesystem()/"graph.xml"; // Serialize - serializeToXMLFile(input, path + "graph.xml"); + serializeToXMLFile(input, path.string()); // De-serialize - deserializeFromXMLFile(path + "graph.xml", output); + deserializeFromXMLFile(path.string(), output); } // This version requires equality operator @@ -160,11 +161,11 @@ void roundtripBinary(const T& input, T& output) { // Templated round-trip serialization using Binary file template void roundtripBinaryFile(const T& input, T& output) { - std::string path = resetFilesystem(); + boost::filesystem::path path = resetFilesystem()/"graph.bin"; // Serialize - serializeToBinaryFile(input, path + "graph.bin"); + serializeToBinaryFile(input, path.string()); // De-serialize - deserializeFromBinaryFile(path + "graph.bin", output); + deserializeFromBinaryFile(path.string(), output); } // This version requires equality operator From 327cbc515ff93db6f6547ed3deead1bdf9cfdb2e Mon Sep 17 00:00:00 2001 From: Gerry Chen Date: Mon, 22 Jun 2020 21:15:07 -0400 Subject: [PATCH 26/54] Separate stream creation and serialization Recommended by @ProfFan in #343 with the objective of making (de)serialize to string and to file more similar --- gtsam/base/serialization.h | 86 +++++++++++++++++++++++--------------- 1 file changed, 53 insertions(+), 33 deletions(-) diff --git a/gtsam/base/serialization.h b/gtsam/base/serialization.h index 191e0ecfb..031ee3a3e 100644 --- a/gtsam/base/serialization.h +++ b/gtsam/base/serialization.h @@ -44,18 +44,28 @@ namespace gtsam { // Serialization directly to strings in compressed format template -std::string serialize(const T& input) { - std::ostringstream out_archive_stream; +void serialize(const T& input, std::ostream & out_archive_stream) { boost::archive::text_oarchive out_archive(out_archive_stream); out_archive << input; +} + +template +void deserialize(std::istream & in_archive_stream, T& output) { + boost::archive::text_iarchive in_archive(in_archive_stream); + in_archive >> output; +} + +template +std::string serialize(const T& input) { + std::ostringstream out_archive_stream; + serialize(input, out_archive_stream); return out_archive_stream.str(); } template void deserialize(const std::string& serialized, T& output) { std::istringstream in_archive_stream(serialized); - boost::archive::text_iarchive in_archive(in_archive_stream); - in_archive >> output; + deserialize(in_archive_stream, output); } template @@ -63,8 +73,7 @@ bool serializeToFile(const T& input, const std::string& filename) { std::ofstream out_archive_stream(filename.c_str()); if (!out_archive_stream.is_open()) return false; - boost::archive::text_oarchive out_archive(out_archive_stream); - out_archive << input; + serialize(input, out_archive_stream); out_archive_stream.close(); return true; } @@ -74,31 +83,37 @@ bool deserializeFromFile(const std::string& filename, T& output) { std::ifstream in_archive_stream(filename.c_str()); if (!in_archive_stream.is_open()) return false; - boost::archive::text_iarchive in_archive(in_archive_stream); - in_archive >> output; + deserialize(in_archive_stream, output); in_archive_stream.close(); return true; } // Serialization to XML format with named structures +template +void serializeXML(const T& input, std::ostream& out_archive_stream, + const std::string& name = "data") { + boost::archive::xml_oarchive out_archive(out_archive_stream); + out_archive << boost::serialization::make_nvp(name.c_str(), input); +} + +template +void deserializeXML(std::istream& in_archive_stream, T& output, + const std::string& name = "data") { + boost::archive::xml_iarchive in_archive(in_archive_stream); + in_archive >> boost::serialization::make_nvp(name.c_str(), output); +} + template std::string serializeXML(const T& input, const std::string& name="data") { std::ostringstream out_archive_stream; - // braces to flush out_archive as it goes out of scope before taking str() - // fixes crash with boost 1.66-1.68 - // see https://github.com/boostorg/serialization/issues/82 - { - boost::archive::xml_oarchive out_archive(out_archive_stream); - out_archive << boost::serialization::make_nvp(name.c_str(), input); - } + serializeXML(input, out_archive_stream, name); return out_archive_stream.str(); } template void deserializeXML(const std::string& serialized, T& output, const std::string& name="data") { std::istringstream in_archive_stream(serialized); - boost::archive::xml_iarchive in_archive(in_archive_stream); - in_archive >> boost::serialization::make_nvp(name.c_str(), output); + deserializeXML(in_archive_stream, output, name); } template @@ -106,10 +121,7 @@ bool serializeToXMLFile(const T& input, const std::string& filename, const std:: std::ofstream out_archive_stream(filename.c_str()); if (!out_archive_stream.is_open()) return false; - { - boost::archive::xml_oarchive out_archive(out_archive_stream); - out_archive << boost::serialization::make_nvp(name.c_str(), input); - } + serializeXML(input, out_archive_stream, name); out_archive_stream.close(); return true; } @@ -119,28 +131,38 @@ bool deserializeFromXMLFile(const std::string& filename, T& output, const std::s std::ifstream in_archive_stream(filename.c_str()); if (!in_archive_stream.is_open()) return false; - { - boost::archive::xml_iarchive in_archive(in_archive_stream); - in_archive >> boost::serialization::make_nvp(name.c_str(), output); - } + deserializeXML(in_archive_stream, output, name); in_archive_stream.close(); return true; } +// Serialization to binary format with named structures +template +void serializeBinary(const T& input, std::ostream& out_archive_stream, + const std::string& name = "data") { + boost::archive::binary_oarchive out_archive(out_archive_stream); + out_archive << boost::serialization::make_nvp(name.c_str(), input); +} + +template +void deserializeBinary(std::istream& in_archive_stream, T& output, + const std::string& name = "data") { + boost::archive::binary_iarchive in_archive(in_archive_stream); + in_archive >> boost::serialization::make_nvp(name.c_str(), output); +} + // Serialization to binary format with named structures template std::string serializeBinary(const T& input, const std::string& name="data") { std::ostringstream out_archive_stream; - boost::archive::binary_oarchive out_archive(out_archive_stream); - out_archive << boost::serialization::make_nvp(name.c_str(), input); + serializeBinary(input, out_archive_stream, name); return out_archive_stream.str(); } template void deserializeBinary(const std::string& serialized, T& output, const std::string& name="data") { std::istringstream in_archive_stream(serialized); - boost::archive::binary_iarchive in_archive(in_archive_stream); - in_archive >> boost::serialization::make_nvp(name.c_str(), output); + deserializeBinary(in_archive_stream, output, name); } template @@ -148,8 +170,7 @@ bool serializeToBinaryFile(const T& input, const std::string& filename, const st std::ofstream out_archive_stream(filename.c_str()); if (!out_archive_stream.is_open()) return false; - boost::archive::binary_oarchive out_archive(out_archive_stream); - out_archive << boost::serialization::make_nvp(name.c_str(), input); + serializeBinary(input, out_archive_stream, name); out_archive_stream.close(); return true; } @@ -159,8 +180,7 @@ bool deserializeFromBinaryFile(const std::string& filename, T& output, const std std::ifstream in_archive_stream(filename.c_str()); if (!in_archive_stream.is_open()) return false; - boost::archive::binary_iarchive in_archive(in_archive_stream); - in_archive >> boost::serialization::make_nvp(name.c_str(), output); + deserializeBinary(in_archive_stream, output, name); in_archive_stream.close(); return true; } From a4737d47066c0ee0e533bea393f4da14347e9abf Mon Sep 17 00:00:00 2001 From: Gerry Chen Date: Mon, 22 Jun 2020 21:18:43 -0400 Subject: [PATCH 27/54] formatting to Google style --- gtsam/base/serialization.h | 78 +++++++++++++++++++------------------- 1 file changed, 40 insertions(+), 38 deletions(-) diff --git a/gtsam/base/serialization.h b/gtsam/base/serialization.h index 031ee3a3e..aaf06275c 100644 --- a/gtsam/base/serialization.h +++ b/gtsam/base/serialization.h @@ -43,46 +43,44 @@ namespace gtsam { // Serialization directly to strings in compressed format -template -void serialize(const T& input, std::ostream & out_archive_stream) { +template +void serialize(const T& input, std::ostream& out_archive_stream) { boost::archive::text_oarchive out_archive(out_archive_stream); out_archive << input; } -template -void deserialize(std::istream & in_archive_stream, T& output) { +template +void deserialize(std::istream& in_archive_stream, T& output) { boost::archive::text_iarchive in_archive(in_archive_stream); in_archive >> output; } -template +template std::string serialize(const T& input) { std::ostringstream out_archive_stream; serialize(input, out_archive_stream); return out_archive_stream.str(); } -template +template void deserialize(const std::string& serialized, T& output) { std::istringstream in_archive_stream(serialized); deserialize(in_archive_stream, output); } -template +template bool serializeToFile(const T& input, const std::string& filename) { std::ofstream out_archive_stream(filename.c_str()); - if (!out_archive_stream.is_open()) - return false; + if (!out_archive_stream.is_open()) return false; serialize(input, out_archive_stream); out_archive_stream.close(); return true; } -template +template bool deserializeFromFile(const std::string& filename, T& output) { std::ifstream in_archive_stream(filename.c_str()); - if (!in_archive_stream.is_open()) - return false; + if (!in_archive_stream.is_open()) return false; deserialize(in_archive_stream, output); in_archive_stream.close(); return true; @@ -103,34 +101,36 @@ void deserializeXML(std::istream& in_archive_stream, T& output, in_archive >> boost::serialization::make_nvp(name.c_str(), output); } -template -std::string serializeXML(const T& input, const std::string& name="data") { +template +std::string serializeXML(const T& input, + const std::string& name = "data") { std::ostringstream out_archive_stream; serializeXML(input, out_archive_stream, name); return out_archive_stream.str(); } -template -void deserializeXML(const std::string& serialized, T& output, const std::string& name="data") { +template +void deserializeXML(const std::string& serialized, T& output, + const std::string& name = "data") { std::istringstream in_archive_stream(serialized); deserializeXML(in_archive_stream, output, name); } -template -bool serializeToXMLFile(const T& input, const std::string& filename, const std::string& name="data") { +template +bool serializeToXMLFile(const T& input, const std::string& filename, + const std::string& name = "data") { std::ofstream out_archive_stream(filename.c_str()); - if (!out_archive_stream.is_open()) - return false; + if (!out_archive_stream.is_open()) return false; serializeXML(input, out_archive_stream, name); out_archive_stream.close(); return true; } -template -bool deserializeFromXMLFile(const std::string& filename, T& output, const std::string& name="data") { +template +bool deserializeFromXMLFile(const std::string& filename, T& output, + const std::string& name = "data") { std::ifstream in_archive_stream(filename.c_str()); - if (!in_archive_stream.is_open()) - return false; + if (!in_archive_stream.is_open()) return false; deserializeXML(in_archive_stream, output, name); in_archive_stream.close(); return true; @@ -139,7 +139,7 @@ bool deserializeFromXMLFile(const std::string& filename, T& output, const std::s // Serialization to binary format with named structures template void serializeBinary(const T& input, std::ostream& out_archive_stream, - const std::string& name = "data") { + const std::string& name = "data") { boost::archive::binary_oarchive out_archive(out_archive_stream); out_archive << boost::serialization::make_nvp(name.c_str(), input); } @@ -152,37 +152,39 @@ void deserializeBinary(std::istream& in_archive_stream, T& output, } // Serialization to binary format with named structures -template -std::string serializeBinary(const T& input, const std::string& name="data") { +template +std::string serializeBinary(const T& input, + const std::string& name = "data") { std::ostringstream out_archive_stream; serializeBinary(input, out_archive_stream, name); return out_archive_stream.str(); } -template -void deserializeBinary(const std::string& serialized, T& output, const std::string& name="data") { +template +void deserializeBinary(const std::string& serialized, T& output, + const std::string& name = "data") { std::istringstream in_archive_stream(serialized); deserializeBinary(in_archive_stream, output, name); } -template -bool serializeToBinaryFile(const T& input, const std::string& filename, const std::string& name="data") { +template +bool serializeToBinaryFile(const T& input, const std::string& filename, + const std::string& name = "data") { std::ofstream out_archive_stream(filename.c_str()); - if (!out_archive_stream.is_open()) - return false; + if (!out_archive_stream.is_open()) return false; serializeBinary(input, out_archive_stream, name); out_archive_stream.close(); return true; } -template -bool deserializeFromBinaryFile(const std::string& filename, T& output, const std::string& name="data") { +template +bool deserializeFromBinaryFile(const std::string& filename, T& output, + const std::string& name = "data") { std::ifstream in_archive_stream(filename.c_str()); - if (!in_archive_stream.is_open()) - return false; + if (!in_archive_stream.is_open()) return false; deserializeBinary(in_archive_stream, output, name); in_archive_stream.close(); return true; } -} // \namespace gtsam +} // namespace gtsam From 82db82bbf5d46bb64dc50ba52507b39ee7f061ed Mon Sep 17 00:00:00 2001 From: Gerry Chen Date: Mon, 22 Jun 2020 23:08:39 -0400 Subject: [PATCH 28/54] fixed unit test failure on `testSerializationBase` object `output` was getting reused, but should be re-loaded into a "blank" object each time. --- gtsam/base/serializationTestHelpers.h | 46 ++++++++++++--------------- 1 file changed, 20 insertions(+), 26 deletions(-) diff --git a/gtsam/base/serializationTestHelpers.h b/gtsam/base/serializationTestHelpers.h index 602fd68f6..bac166e5b 100644 --- a/gtsam/base/serializationTestHelpers.h +++ b/gtsam/base/serializationTestHelpers.h @@ -44,8 +44,8 @@ T create() { // Creates or empties a folder in the build folder and returns the relative path boost::filesystem::path resetFilesystem( boost::filesystem::path folder = "actual") { - boost::filesystem::remove_all(folder); - boost::filesystem::create_directory(folder); + // boost::filesystem::remove_all(folder); + // boost::filesystem::create_directory(folder); return folder; } @@ -71,11 +71,10 @@ void roundtripFile(const T& input, T& output) { // This version requires equality operator template bool equality(const T& input = T()) { - T output = create(); + T output = create(), outputf = create(); roundtrip(input,output); - if (input != output) return false; - roundtripFile(input,output); - return input==output; + roundtripFile(input,outputf); + return (input==output) && (input==outputf); } // This version requires Testable @@ -89,11 +88,10 @@ bool equalsObj(const T& input = T()) { // De-referenced version for pointers, requires equals method template bool equalsDereferenced(const T& input) { - T output = create(); + T output = create(), outputf = create(); roundtrip(input,output); - if (!input->equals(*output)) return false; - roundtripFile(input,output); - return input->equals(*output); + roundtripFile(input,outputf); + return (input->equals(*output)) && (input->equals(*outputf)); } // Templated round-trip serialization using XML @@ -122,11 +120,10 @@ void roundtripXMLFile(const T& input, T& output) { // This version requires equality operator template bool equalityXML(const T& input = T()) { - T output = create(); + T output = create(), outputf = create(); roundtripXML(input,output); - if (input != output) return false; - roundtripXMLFile(input,output); - return input==output; + roundtripXMLFile(input,outputf); + return (input==output) && (input==outputf); } // This version requires Testable @@ -140,11 +137,10 @@ bool equalsXML(const T& input = T()) { // This version is for pointers, requires equals method template bool equalsDereferencedXML(const T& input = T()) { - T output = create(); + T output = create(), outputf = create(); roundtripXML(input,output); - if (!input->equals(*output)) return false; - roundtripXMLFile(input, output); - return input->equals(*output); + roundtripXMLFile(input, outputf); + return (input->equals(*output)) && (input->equals(*outputf)); } // Templated round-trip serialization using XML @@ -171,11 +167,10 @@ void roundtripBinaryFile(const T& input, T& output) { // This version requires equality operator template bool equalityBinary(const T& input = T()) { - T output = create(); + T output = create(), outputf = create(); roundtripBinary(input,output); - if (input != output) return false; - roundtripBinaryFile(input,output); - return input==output; + roundtripBinaryFile(input,outputf); + return (input==output) && (input==outputf); } // This version requires Testable @@ -189,11 +184,10 @@ bool equalsBinary(const T& input = T()) { // This version is for pointers, requires equals method template bool equalsDereferencedBinary(const T& input = T()) { - T output = create(); + T output = create(), outputf = create(); roundtripBinary(input,output); - if (!input->equals(*output)) return false; - roundtripBinaryFile(input,output); - return input->equals(*output); + roundtripBinaryFile(input,outputf); + return (input->equals(*output)) && (input->equals(*outputf)); } } // \namespace serializationTestHelpers From a0a3b8f459df37349a7b4af7d6b52016142204cb Mon Sep 17 00:00:00 2001 From: Gerry Chen Date: Tue, 23 Jun 2020 09:52:29 -0400 Subject: [PATCH 29/54] reset filesystem - forgot to uncomment these after debugging --- gtsam/base/serializationTestHelpers.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam/base/serializationTestHelpers.h b/gtsam/base/serializationTestHelpers.h index bac166e5b..5d67b2687 100644 --- a/gtsam/base/serializationTestHelpers.h +++ b/gtsam/base/serializationTestHelpers.h @@ -44,8 +44,8 @@ T create() { // Creates or empties a folder in the build folder and returns the relative path boost::filesystem::path resetFilesystem( boost::filesystem::path folder = "actual") { - // boost::filesystem::remove_all(folder); - // boost::filesystem::create_directory(folder); + boost::filesystem::remove_all(folder); + boost::filesystem::create_directory(folder); return folder; } From 3ea9ff012005b1205b8c20f5ea2b676d4054ce76 Mon Sep 17 00:00:00 2001 From: akrishnan86 Date: Tue, 23 Jun 2020 07:58:38 -0700 Subject: [PATCH 30/54] optional initialization for LMParams --- gtsam/sfm/TranslationRecovery.h | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/gtsam/sfm/TranslationRecovery.h b/gtsam/sfm/TranslationRecovery.h index 5eea251cf..634825d29 100644 --- a/gtsam/sfm/TranslationRecovery.h +++ b/gtsam/sfm/TranslationRecovery.h @@ -61,8 +61,9 @@ class TranslationRecovery { * @param relativeTranslations the relative translations, in world coordinate * frames, indexed in a map by a pair of Pose keys. */ - TranslationRecovery(const TranslationEdges& relativeTranslations) - : relativeTranslations_(relativeTranslations) { + TranslationRecovery(const TranslationEdges& relativeTranslations, + const LevenbergMarquardtParams& lmParams = LevenbergMarquardtParams()) + : relativeTranslations_(relativeTranslations), params_(lmParams) { params_.setVerbosityLM("Summary"); } From 9daeb39267f80bef157c0d11e60f371e57a179bb Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 23 Jun 2020 16:08:44 -0500 Subject: [PATCH 31/54] Set minimum supported numpy version to 1.11.0 (#366) * add deadsnakes ppa to install python3.6 on Ubuntu Xenial * updated travis distro to Ubuntu 18.04 bionic * Revert "updated travis distro to Ubuntu 18.04 bionic" This reverts commit 323264a924e8554da49c27a374e9a6278c5a659e. * restrict numpy version to be less than 1.19.0 * use ubuntu packaged numpy as baseline version to test against * downgrade minimum required version of numpy to version supported by Ubuntu Xenial * undo explicit pip install --- .travis.yml | 3 ++- cython/requirements.txt | 2 +- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/.travis.yml b/.travis.yml index ca6a426ea..d8094ef4d 100644 --- a/.travis.yml +++ b/.travis.yml @@ -14,7 +14,8 @@ addons: - clang-9 - build-essential pkg-config - cmake - - libpython-dev python-numpy + - python3-dev libpython-dev + - python3-numpy - libboost-all-dev # before_install: diff --git a/cython/requirements.txt b/cython/requirements.txt index cd77b097d..8d3c7aeb4 100644 --- a/cython/requirements.txt +++ b/cython/requirements.txt @@ -1,3 +1,3 @@ Cython>=0.25.2 backports_abc>=0.5 -numpy>=1.12.0 +numpy>=1.11.0 From b41809203f636878dbbc0aad718ac6f7343e82e6 Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Tue, 23 Jun 2020 18:37:12 -0400 Subject: [PATCH 32/54] Revise comments --- gtsam/base/types.h | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/gtsam/base/types.h b/gtsam/base/types.h index 3abbdfe81..93c475c40 100644 --- a/gtsam/base/types.h +++ b/gtsam/base/types.h @@ -235,7 +235,8 @@ namespace gtsam { /** * A trait to mark classes that need special alignment. - * Please refer to https://github.com/PointCloudLibrary/pcl/pull/3163 + * This is required to make boost::make_shared and etc respect alignment, which is essential for the Python + * wrappers to work properly. */ #if __cplusplus < 201703L template using void_t = void; From de7332fceac3224d4ea693a8f1631300b3345c74 Mon Sep 17 00:00:00 2001 From: Gerry Chen Date: Wed, 24 Jun 2020 02:39:44 -0400 Subject: [PATCH 33/54] remove file roundtrip test for pointers --- gtsam/base/serializationTestHelpers.h | 15 ++++++--------- 1 file changed, 6 insertions(+), 9 deletions(-) diff --git a/gtsam/base/serializationTestHelpers.h b/gtsam/base/serializationTestHelpers.h index 5d67b2687..78f9d6d10 100644 --- a/gtsam/base/serializationTestHelpers.h +++ b/gtsam/base/serializationTestHelpers.h @@ -88,10 +88,9 @@ bool equalsObj(const T& input = T()) { // De-referenced version for pointers, requires equals method template bool equalsDereferenced(const T& input) { - T output = create(), outputf = create(); + T output = create(); roundtrip(input,output); - roundtripFile(input,outputf); - return (input->equals(*output)) && (input->equals(*outputf)); + return input->equals(*output); } // Templated round-trip serialization using XML @@ -137,10 +136,9 @@ bool equalsXML(const T& input = T()) { // This version is for pointers, requires equals method template bool equalsDereferencedXML(const T& input = T()) { - T output = create(), outputf = create(); + T output = create(); roundtripXML(input,output); - roundtripXMLFile(input, outputf); - return (input->equals(*output)) && (input->equals(*outputf)); + return input->equals(*output); } // Templated round-trip serialization using XML @@ -184,10 +182,9 @@ bool equalsBinary(const T& input = T()) { // This version is for pointers, requires equals method template bool equalsDereferencedBinary(const T& input = T()) { - T output = create(), outputf = create(); + T output = create(); roundtripBinary(input,output); - roundtripBinaryFile(input,outputf); - return (input->equals(*output)) && (input->equals(*outputf)); + return input->equals(*output); } } // \namespace serializationTestHelpers From 7d7475b881ffc9f21ce2f49c3e4d3890e9e6d21f Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Wed, 24 Jun 2020 14:15:46 -0400 Subject: [PATCH 34/54] Style fixes as commented by @dellaert --- gtsam/base/make_shared.h | 22 +++++++++++++--------- gtsam/base/types.h | 19 ++++++++++--------- 2 files changed, 23 insertions(+), 18 deletions(-) diff --git a/gtsam/base/make_shared.h b/gtsam/base/make_shared.h index 966dd516d..c7d98cdee 100644 --- a/gtsam/base/make_shared.h +++ b/gtsam/base/make_shared.h @@ -25,27 +25,31 @@ #include -#if __cplusplus <= 201103L namespace gtsam { template using enable_if_t = typename std::enable_if::type; } -#endif namespace gtsam { - /// Add our own `make_shared` as a layer of wrapping on `boost::make_shared` + /** Add our own `make_shared` as a layer of wrapping on `boost::make_shared` + * This solves the problem with the stock `make_shared` that custom alignment is not respected, causing SEGFAULTs + * at runtime, which is notoriously hard to debug. + * + * @tparam T The type of object being constructed + * @tparam Args Type of the arguments of the constructor + * @param args Arguments of the constructor + * @return The object created as a boost::shared_ptr + */ template - gtsam::enable_if_t::value, boost::shared_ptr> make_shared(Args&&... args) - { - return boost::allocate_shared(Eigen::aligned_allocator(), std::forward (args)...); + gtsam::enable_if_t::value, boost::shared_ptr> make_shared(Args &&... args) { + return boost::allocate_shared(Eigen::aligned_allocator(), std::forward(args)...); } /// Fall back to the boost version if no need for alignment template - gtsam::enable_if_t::value, boost::shared_ptr> make_shared(Args&&... args) - { - return boost::make_shared(std::forward (args)...); + gtsam::enable_if_t::value, boost::shared_ptr> make_shared(Args &&... args) { + return boost::make_shared(std::forward(args)...); } } diff --git a/gtsam/base/types.h b/gtsam/base/types.h index 93c475c40..a4b2fb6ea 100644 --- a/gtsam/base/types.h +++ b/gtsam/base/types.h @@ -233,27 +233,28 @@ namespace std { namespace gtsam { + /// Convenience void_t as we assume C++11, it will not conflict the std one in C++17 as this is in `gtsam::` + template using void_t = void; + /** - * A trait to mark classes that need special alignment. + * A SFINAE trait to mark classes that need special alignment. * This is required to make boost::make_shared and etc respect alignment, which is essential for the Python * wrappers to work properly. */ -#if __cplusplus < 201703L - template using void_t = void; -#endif - template> - struct has_custom_allocator : std::false_type { + struct needs_eigen_aligned_allocator : std::false_type { }; template - struct has_custom_allocator> : std::true_type { + struct needs_eigen_aligned_allocator> : std::true_type { }; } /** - * This is necessary for the Cython wrapper to work properly, and possibly reduce future misalignment problems. + * This marks a GTSAM object to require alignment. With this macro an object will automatically be allocated in aligned + * memory when one uses `gtsam::make_shared`. It reduces future misalignment problems that is hard to debug. + * See https://eigen.tuxfamily.org/dox/group__DenseMatrixManipulation__Alignement.html for details */ #define GTSAM_MAKE_ALIGNED_OPERATOR_NEW \ EIGEN_MAKE_ALIGNED_OPERATOR_NEW \ - using _custom_allocator_type_trait = void; \ No newline at end of file + using _eigen_aligned_allocator_trait = void; From 6d75e992e896f54a1bd3956ff5f8cd1dddf5e814 Mon Sep 17 00:00:00 2001 From: Gerry Chen Date: Wed, 24 Jun 2020 14:16:00 -0400 Subject: [PATCH 35/54] serialization docstrings --- gtsam/base/serialization.h | 37 ++++++++++++++++++++++++--- gtsam/base/serializationTestHelpers.h | 17 +----------- 2 files changed, 34 insertions(+), 20 deletions(-) diff --git a/gtsam/base/serialization.h b/gtsam/base/serialization.h index aaf06275c..1a319ab17 100644 --- a/gtsam/base/serialization.h +++ b/gtsam/base/serialization.h @@ -42,19 +42,25 @@ namespace gtsam { -// Serialization directly to strings in compressed format +/** @name Standard serialization + * Serialization in default compressed format + */ +///@{ +/// serializes to a stream template void serialize(const T& input, std::ostream& out_archive_stream) { boost::archive::text_oarchive out_archive(out_archive_stream); out_archive << input; } +/// deserializes from a stream template void deserialize(std::istream& in_archive_stream, T& output) { boost::archive::text_iarchive in_archive(in_archive_stream); in_archive >> output; } +/// serializes to a string template std::string serialize(const T& input) { std::ostringstream out_archive_stream; @@ -62,12 +68,14 @@ std::string serialize(const T& input) { return out_archive_stream.str(); } +/// deserializes from a string template void deserialize(const std::string& serialized, T& output) { std::istringstream in_archive_stream(serialized); deserialize(in_archive_stream, output); } +/// serializes to a file template bool serializeToFile(const T& input, const std::string& filename) { std::ofstream out_archive_stream(filename.c_str()); @@ -77,6 +85,7 @@ bool serializeToFile(const T& input, const std::string& filename) { return true; } +/// deserializes from a file template bool deserializeFromFile(const std::string& filename, T& output) { std::ifstream in_archive_stream(filename.c_str()); @@ -85,8 +94,13 @@ bool deserializeFromFile(const std::string& filename, T& output) { in_archive_stream.close(); return true; } +///@} -// Serialization to XML format with named structures +/** @name XML Serialization + * Serialization to XML format with named structures + */ +///@{ +/// serializes to a stream in XML template void serializeXML(const T& input, std::ostream& out_archive_stream, const std::string& name = "data") { @@ -94,6 +108,7 @@ void serializeXML(const T& input, std::ostream& out_archive_stream, out_archive << boost::serialization::make_nvp(name.c_str(), input); } +/// deserializes from a stream in XML template void deserializeXML(std::istream& in_archive_stream, T& output, const std::string& name = "data") { @@ -101,6 +116,7 @@ void deserializeXML(std::istream& in_archive_stream, T& output, in_archive >> boost::serialization::make_nvp(name.c_str(), output); } +/// serializes to a string in XML template std::string serializeXML(const T& input, const std::string& name = "data") { @@ -109,6 +125,7 @@ std::string serializeXML(const T& input, return out_archive_stream.str(); } +/// deserializes from a string in XML template void deserializeXML(const std::string& serialized, T& output, const std::string& name = "data") { @@ -116,6 +133,7 @@ void deserializeXML(const std::string& serialized, T& output, deserializeXML(in_archive_stream, output, name); } +/// serializes to an XML file template bool serializeToXMLFile(const T& input, const std::string& filename, const std::string& name = "data") { @@ -126,6 +144,7 @@ bool serializeToXMLFile(const T& input, const std::string& filename, return true; } +/// deserializes from an XML file template bool deserializeFromXMLFile(const std::string& filename, T& output, const std::string& name = "data") { @@ -135,8 +154,13 @@ bool deserializeFromXMLFile(const std::string& filename, T& output, in_archive_stream.close(); return true; } +///@} -// Serialization to binary format with named structures +/** @name Binary Serialization + * Serialization to binary format with named structures + */ +///@{ +/// serializes to a stream in binary template void serializeBinary(const T& input, std::ostream& out_archive_stream, const std::string& name = "data") { @@ -144,6 +168,7 @@ void serializeBinary(const T& input, std::ostream& out_archive_stream, out_archive << boost::serialization::make_nvp(name.c_str(), input); } +/// deserializes from a stream in binary template void deserializeBinary(std::istream& in_archive_stream, T& output, const std::string& name = "data") { @@ -151,7 +176,7 @@ void deserializeBinary(std::istream& in_archive_stream, T& output, in_archive >> boost::serialization::make_nvp(name.c_str(), output); } -// Serialization to binary format with named structures +/// serializes to a string in binary template std::string serializeBinary(const T& input, const std::string& name = "data") { @@ -160,6 +185,7 @@ std::string serializeBinary(const T& input, return out_archive_stream.str(); } +/// deserializes from a string in binary template void deserializeBinary(const std::string& serialized, T& output, const std::string& name = "data") { @@ -167,6 +193,7 @@ void deserializeBinary(const std::string& serialized, T& output, deserializeBinary(in_archive_stream, output, name); } +/// serializes to a binary file template bool serializeToBinaryFile(const T& input, const std::string& filename, const std::string& name = "data") { @@ -177,6 +204,7 @@ bool serializeToBinaryFile(const T& input, const std::string& filename, return true; } +/// deserializes from a binary file template bool deserializeFromBinaryFile(const std::string& filename, T& output, const std::string& name = "data") { @@ -186,5 +214,6 @@ bool deserializeFromBinaryFile(const std::string& filename, T& output, in_archive_stream.close(); return true; } +///@} } // namespace gtsam diff --git a/gtsam/base/serializationTestHelpers.h b/gtsam/base/serializationTestHelpers.h index 78f9d6d10..5994a5e51 100644 --- a/gtsam/base/serializationTestHelpers.h +++ b/gtsam/base/serializationTestHelpers.h @@ -52,10 +52,8 @@ boost::filesystem::path resetFilesystem( // Templated round-trip serialization template void roundtrip(const T& input, T& output) { - // Serialize std::string serialized = serialize(input); if (verbose) std::cout << serialized << std::endl << std::endl; - deserialize(serialized, output); } @@ -63,12 +61,11 @@ void roundtrip(const T& input, T& output) { template void roundtripFile(const T& input, T& output) { boost::filesystem::path path = resetFilesystem()/"graph.dat"; - serializeToFile(input, path.string()); deserializeFromFile(path.string(), output); } -// This version requires equality operator +// This version requires equality operator and uses string and file round-trips template bool equality(const T& input = T()) { T output = create(), outputf = create(); @@ -96,11 +93,8 @@ bool equalsDereferenced(const T& input) { // Templated round-trip serialization using XML template void roundtripXML(const T& input, T& output) { - // Serialize std::string serialized = serializeXML(input); if (verbose) std::cout << serialized << std::endl << std::endl; - - // De-serialize deserializeXML(serialized, output); } @@ -108,11 +102,7 @@ void roundtripXML(const T& input, T& output) { template void roundtripXMLFile(const T& input, T& output) { boost::filesystem::path path = resetFilesystem()/"graph.xml"; - - // Serialize serializeToXMLFile(input, path.string()); - - // De-serialize deserializeFromXMLFile(path.string(), output); } @@ -144,11 +134,8 @@ bool equalsDereferencedXML(const T& input = T()) { // Templated round-trip serialization using XML template void roundtripBinary(const T& input, T& output) { - // Serialize std::string serialized = serializeBinary(input); if (verbose) std::cout << serialized << std::endl << std::endl; - - // De-serialize deserializeBinary(serialized, output); } @@ -156,9 +143,7 @@ void roundtripBinary(const T& input, T& output) { template void roundtripBinaryFile(const T& input, T& output) { boost::filesystem::path path = resetFilesystem()/"graph.bin"; - // Serialize serializeToBinaryFile(input, path.string()); - // De-serialize deserializeFromBinaryFile(path.string(), output); } From b37be7d640567be9b78c699d158c88035d4f789e Mon Sep 17 00:00:00 2001 From: Gerry Chen Date: Wed, 24 Jun 2020 14:33:08 -0400 Subject: [PATCH 36/54] rename serialization functions with less ambiguous names According to Varun's suggestion. Note: string functions should be automatically inlined by compiler to avoid passing big strings. --- gtsam/base/serialization.h | 88 +++++++++++++++++++++++++++----------- 1 file changed, 64 insertions(+), 24 deletions(-) diff --git a/gtsam/base/serialization.h b/gtsam/base/serialization.h index 1a319ab17..f589ecc5e 100644 --- a/gtsam/base/serialization.h +++ b/gtsam/base/serialization.h @@ -48,31 +48,31 @@ namespace gtsam { ///@{ /// serializes to a stream template -void serialize(const T& input, std::ostream& out_archive_stream) { +void serializeToStream(const T& input, std::ostream& out_archive_stream) { boost::archive::text_oarchive out_archive(out_archive_stream); out_archive << input; } /// deserializes from a stream template -void deserialize(std::istream& in_archive_stream, T& output) { +void deserializeFromStream(std::istream& in_archive_stream, T& output) { boost::archive::text_iarchive in_archive(in_archive_stream); in_archive >> output; } /// serializes to a string template -std::string serialize(const T& input) { +std::string serializeToString(const T& input) { std::ostringstream out_archive_stream; - serialize(input, out_archive_stream); + serializeToStream(input, out_archive_stream); return out_archive_stream.str(); } /// deserializes from a string template -void deserialize(const std::string& serialized, T& output) { +void deserializeFromString(const std::string& serialized, T& output) { std::istringstream in_archive_stream(serialized); - deserialize(in_archive_stream, output); + deserializeFromStream(in_archive_stream, output); } /// serializes to a file @@ -80,7 +80,7 @@ template bool serializeToFile(const T& input, const std::string& filename) { std::ofstream out_archive_stream(filename.c_str()); if (!out_archive_stream.is_open()) return false; - serialize(input, out_archive_stream); + serializeToStream(input, out_archive_stream); out_archive_stream.close(); return true; } @@ -90,10 +90,22 @@ template bool deserializeFromFile(const std::string& filename, T& output) { std::ifstream in_archive_stream(filename.c_str()); if (!in_archive_stream.is_open()) return false; - deserialize(in_archive_stream, output); + deserializeFromStream(in_archive_stream, output); in_archive_stream.close(); return true; } + +/// serializes to a string +template +std::string serialize(const T& input) { + return serializeToString(input); +} + +/// deserializes from a string +template +void deserialize(const std::string& serialized, T& output) { + deserializeFromString(serialized, output); +} ///@} /** @name XML Serialization @@ -102,7 +114,7 @@ bool deserializeFromFile(const std::string& filename, T& output) { ///@{ /// serializes to a stream in XML template -void serializeXML(const T& input, std::ostream& out_archive_stream, +void serializeToXMLStream(const T& input, std::ostream& out_archive_stream, const std::string& name = "data") { boost::archive::xml_oarchive out_archive(out_archive_stream); out_archive << boost::serialization::make_nvp(name.c_str(), input); @@ -110,7 +122,7 @@ void serializeXML(const T& input, std::ostream& out_archive_stream, /// deserializes from a stream in XML template -void deserializeXML(std::istream& in_archive_stream, T& output, +void deserializeFromXMLStream(std::istream& in_archive_stream, T& output, const std::string& name = "data") { boost::archive::xml_iarchive in_archive(in_archive_stream); in_archive >> boost::serialization::make_nvp(name.c_str(), output); @@ -118,19 +130,19 @@ void deserializeXML(std::istream& in_archive_stream, T& output, /// serializes to a string in XML template -std::string serializeXML(const T& input, +std::string serializeToXMLString(const T& input, const std::string& name = "data") { std::ostringstream out_archive_stream; - serializeXML(input, out_archive_stream, name); + serializeToXMLStream(input, out_archive_stream, name); return out_archive_stream.str(); } /// deserializes from a string in XML template -void deserializeXML(const std::string& serialized, T& output, +void deserializeFromXMLString(const std::string& serialized, T& output, const std::string& name = "data") { std::istringstream in_archive_stream(serialized); - deserializeXML(in_archive_stream, output, name); + deserializeFromXMLStream(in_archive_stream, output, name); } /// serializes to an XML file @@ -139,7 +151,7 @@ bool serializeToXMLFile(const T& input, const std::string& filename, const std::string& name = "data") { std::ofstream out_archive_stream(filename.c_str()); if (!out_archive_stream.is_open()) return false; - serializeXML(input, out_archive_stream, name); + serializeToXMLStream(input, out_archive_stream, name); out_archive_stream.close(); return true; } @@ -150,10 +162,24 @@ bool deserializeFromXMLFile(const std::string& filename, T& output, const std::string& name = "data") { std::ifstream in_archive_stream(filename.c_str()); if (!in_archive_stream.is_open()) return false; - deserializeXML(in_archive_stream, output, name); + deserializeFromXMLStream(in_archive_stream, output, name); in_archive_stream.close(); return true; } + +/// serializes to a string in XML +template +std::string serializeXML(const T& input, + const std::string& name = "data") { + return serializeToXMLString(input, name); +} + +/// deserializes from a string in XML +template +void deserializeXML(const std::string& serialized, T& output, + const std::string& name = "data") { + deserializeFromXMLString(serialized, output, name); +} ///@} /** @name Binary Serialization @@ -162,7 +188,7 @@ bool deserializeFromXMLFile(const std::string& filename, T& output, ///@{ /// serializes to a stream in binary template -void serializeBinary(const T& input, std::ostream& out_archive_stream, +void serializeToBinaryStream(const T& input, std::ostream& out_archive_stream, const std::string& name = "data") { boost::archive::binary_oarchive out_archive(out_archive_stream); out_archive << boost::serialization::make_nvp(name.c_str(), input); @@ -170,7 +196,7 @@ void serializeBinary(const T& input, std::ostream& out_archive_stream, /// deserializes from a stream in binary template -void deserializeBinary(std::istream& in_archive_stream, T& output, +void deserializeFromBinaryStream(std::istream& in_archive_stream, T& output, const std::string& name = "data") { boost::archive::binary_iarchive in_archive(in_archive_stream); in_archive >> boost::serialization::make_nvp(name.c_str(), output); @@ -178,19 +204,19 @@ void deserializeBinary(std::istream& in_archive_stream, T& output, /// serializes to a string in binary template -std::string serializeBinary(const T& input, +std::string serializeToBinaryString(const T& input, const std::string& name = "data") { std::ostringstream out_archive_stream; - serializeBinary(input, out_archive_stream, name); + serializeToBinaryStream(input, out_archive_stream, name); return out_archive_stream.str(); } /// deserializes from a string in binary template -void deserializeBinary(const std::string& serialized, T& output, +void deserializeFromBinaryString(const std::string& serialized, T& output, const std::string& name = "data") { std::istringstream in_archive_stream(serialized); - deserializeBinary(in_archive_stream, output, name); + deserializeFromBinaryStream(in_archive_stream, output, name); } /// serializes to a binary file @@ -199,7 +225,7 @@ bool serializeToBinaryFile(const T& input, const std::string& filename, const std::string& name = "data") { std::ofstream out_archive_stream(filename.c_str()); if (!out_archive_stream.is_open()) return false; - serializeBinary(input, out_archive_stream, name); + serializeToBinaryStream(input, out_archive_stream, name); out_archive_stream.close(); return true; } @@ -210,10 +236,24 @@ bool deserializeFromBinaryFile(const std::string& filename, T& output, const std::string& name = "data") { std::ifstream in_archive_stream(filename.c_str()); if (!in_archive_stream.is_open()) return false; - deserializeBinary(in_archive_stream, output, name); + deserializeFromBinaryStream(in_archive_stream, output, name); in_archive_stream.close(); return true; } + +/// serializes to a string in binary +template +std::string serializeBinary(const T& input, + const std::string& name = "data") { + return serializeToBinaryString(input, name); +} + +/// deserializes from a string in binary +template +void deserializeBinary(const std::string& serialized, T& output, + const std::string& name = "data") { + deserializeFromBinaryString(serialized, output, name); +} ///@} } // namespace gtsam From 6dbd7c243abc91489c010261bde27685c491acac Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Wed, 24 Jun 2020 16:25:46 -0400 Subject: [PATCH 37/54] Add comments --- gtsam/base/make_shared.h | 14 +++++++++++++- gtsam/base/types.h | 12 +++++++++++- 2 files changed, 24 insertions(+), 2 deletions(-) diff --git a/gtsam/base/make_shared.h b/gtsam/base/make_shared.h index c7d98cdee..5281eec6d 100644 --- a/gtsam/base/make_shared.h +++ b/gtsam/base/make_shared.h @@ -26,16 +26,28 @@ #include namespace gtsam { + /// An shorthand alias for accessing the ::type inside std::enable_if that can be used in a template directly template using enable_if_t = typename std::enable_if::type; } namespace gtsam { - /** Add our own `make_shared` as a layer of wrapping on `boost::make_shared` + /** + * Add our own `make_shared` as a layer of wrapping on `boost::make_shared` * This solves the problem with the stock `make_shared` that custom alignment is not respected, causing SEGFAULTs * at runtime, which is notoriously hard to debug. * + * Explanation + * =============== + * The template `needs_eigen_aligned_allocator::value` will evaluate to `std::true_type` if the type alias + * `_eigen_aligned_allocator_trait = void` is present in a class, which is automatically added by the + * `GTSAM_MAKE_ALIGNED_OPERATOR_NEW` macro. + * + * This function declaration will only be taken when the above condition is true, so if some object does not need to + * be aligned, `gtsam::make_shared` will fall back to the next definition, which is a simple wrapper for + * `boost::make_shared`. + * * @tparam T The type of object being constructed * @tparam Args Type of the arguments of the constructor * @param args Arguments of the constructor diff --git a/gtsam/base/types.h b/gtsam/base/types.h index a4b2fb6ea..b54cc2f2a 100644 --- a/gtsam/base/types.h +++ b/gtsam/base/types.h @@ -238,8 +238,18 @@ namespace gtsam { /** * A SFINAE trait to mark classes that need special alignment. + * * This is required to make boost::make_shared and etc respect alignment, which is essential for the Python * wrappers to work properly. + * + * Explanation + * ============= + * When a GTSAM type is not declared with the type alias `_eigen_aligned_allocator_trait = void`, the first template + * will be taken so `needs_eigen_aligned_allocator` will be resolved to `std::false_type`. + * + * Otherwise, it will resolve to the second template, which will be resolved to `std::true_type`. + * + * Please refer to `gtsam/base/make_shared.h` for an example. */ template> struct needs_eigen_aligned_allocator : std::false_type { @@ -253,7 +263,7 @@ namespace gtsam { /** * This marks a GTSAM object to require alignment. With this macro an object will automatically be allocated in aligned * memory when one uses `gtsam::make_shared`. It reduces future misalignment problems that is hard to debug. - * See https://eigen.tuxfamily.org/dox/group__DenseMatrixManipulation__Alignement.html for details + * See https://eigen.tuxfamily.org/dox/group__DenseMatrixManipulation__Alignement.html for detailed explanation. */ #define GTSAM_MAKE_ALIGNED_OPERATOR_NEW \ EIGEN_MAKE_ALIGNED_OPERATOR_NEW \ From fb21c553a05555ce4be3de305836d62fa1113bbe Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Wed, 24 Jun 2020 17:15:00 -0400 Subject: [PATCH 38/54] Switch to the new alignment marker type --- gtsam/base/GenericValue.h | 2 +- gtsam/base/Manifold.h | 2 +- gtsam/base/types.h | 9 +++++++++ gtsam/geometry/BearingRange.h | 2 +- gtsam/geometry/CameraSet.h | 2 +- gtsam/geometry/EssentialMatrix.h | 2 +- gtsam/geometry/PinholeCamera.h | 2 +- gtsam/geometry/PinholePose.h | 4 ++-- gtsam/geometry/Pose2.h | 2 +- gtsam/geometry/Pose3.h | 2 +- gtsam/geometry/Rot3.h | 2 +- gtsam/geometry/SOn.h | 2 +- gtsam/geometry/Unit3.h | 2 +- gtsam/geometry/triangulation.h | 2 +- gtsam/navigation/AttitudeFactor.h | 4 ++-- gtsam/navigation/CombinedImuFactor.h | 6 +++--- gtsam/navigation/ImuBias.h | 2 +- gtsam/navigation/PreintegratedRotation.h | 4 ++-- gtsam/navigation/PreintegrationBase.h | 2 +- gtsam/navigation/PreintegrationParams.h | 2 +- gtsam/navigation/TangentPreintegration.h | 2 +- gtsam/nonlinear/ExpressionFactor.h | 2 +- gtsam/nonlinear/NonlinearEquality.h | 6 +++--- gtsam/nonlinear/PriorFactor.h | 2 +- gtsam/nonlinear/internal/ExpressionNode.h | 2 +- gtsam/slam/BetweenFactor.h | 2 +- gtsam/slam/EssentialMatrixConstraint.h | 2 +- gtsam/slam/EssentialMatrixFactor.h | 6 +++--- gtsam/slam/ProjectionFactor.h | 2 +- gtsam/slam/RotateFactor.h | 2 +- gtsam/slam/SmartFactorBase.h | 2 +- 31 files changed, 48 insertions(+), 39 deletions(-) diff --git a/gtsam/base/GenericValue.h b/gtsam/base/GenericValue.h index e1cb3bc2c..2ac3eb80c 100644 --- a/gtsam/base/GenericValue.h +++ b/gtsam/base/GenericValue.h @@ -181,7 +181,7 @@ public: // Alignment, see https://eigen.tuxfamily.org/dox/group__TopicStructHavingEigenMembers.html enum { NeedsToAlign = (sizeof(T) % 16) == 0 }; public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign) + GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign) }; /// use this macro instead of BOOST_CLASS_EXPORT for GenericValues diff --git a/gtsam/base/Manifold.h b/gtsam/base/Manifold.h index f3653f099..9feb2b451 100644 --- a/gtsam/base/Manifold.h +++ b/gtsam/base/Manifold.h @@ -214,7 +214,7 @@ public: enum { NeedsToAlign = (sizeof(M1) % 16) == 0 || (sizeof(M2) % 16) == 0 }; public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign) + GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign) }; // Define any direct product group to be a model of the multiplicative Group concept diff --git a/gtsam/base/types.h b/gtsam/base/types.h index b54cc2f2a..924f339b3 100644 --- a/gtsam/base/types.h +++ b/gtsam/base/types.h @@ -268,3 +268,12 @@ namespace gtsam { #define GTSAM_MAKE_ALIGNED_OPERATOR_NEW \ EIGEN_MAKE_ALIGNED_OPERATOR_NEW \ using _eigen_aligned_allocator_trait = void; + +/** + * This marks a GTSAM object to require alignment. With this macro an object will automatically be allocated in aligned + * memory when one uses `gtsam::make_shared`. It reduces future misalignment problems that is hard to debug. + * See https://eigen.tuxfamily.org/dox/group__DenseMatrixManipulation__Alignement.html for detailed explanation. + */ +#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign) \ + EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign) \ + using _eigen_aligned_allocator_trait = void; \ No newline at end of file diff --git a/gtsam/geometry/BearingRange.h b/gtsam/geometry/BearingRange.h index 7c73f3cbd..8db7abffe 100644 --- a/gtsam/geometry/BearingRange.h +++ b/gtsam/geometry/BearingRange.h @@ -162,7 +162,7 @@ private: NeedsToAlign = (sizeof(B) % 16) == 0 || (sizeof(R) % 16) == 0 }; public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign) + GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign) }; // Declare this to be both Testable and a Manifold diff --git a/gtsam/geometry/CameraSet.h b/gtsam/geometry/CameraSet.h index ecf9a572d..feab8e0fa 100644 --- a/gtsam/geometry/CameraSet.h +++ b/gtsam/geometry/CameraSet.h @@ -319,7 +319,7 @@ private: } public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW + GTSAM_MAKE_ALIGNED_OPERATOR_NEW }; template diff --git a/gtsam/geometry/EssentialMatrix.h b/gtsam/geometry/EssentialMatrix.h index 3235fdedd..ca719eb37 100644 --- a/gtsam/geometry/EssentialMatrix.h +++ b/gtsam/geometry/EssentialMatrix.h @@ -212,7 +212,7 @@ class EssentialMatrix { /// @} public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW + GTSAM_MAKE_ALIGNED_OPERATOR_NEW }; template<> diff --git a/gtsam/geometry/PinholeCamera.h b/gtsam/geometry/PinholeCamera.h index c52127a45..9a80b937b 100644 --- a/gtsam/geometry/PinholeCamera.h +++ b/gtsam/geometry/PinholeCamera.h @@ -325,7 +325,7 @@ private: } public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW + GTSAM_MAKE_ALIGNED_OPERATOR_NEW }; // manifold traits diff --git a/gtsam/geometry/PinholePose.h b/gtsam/geometry/PinholePose.h index 935962423..60088577c 100644 --- a/gtsam/geometry/PinholePose.h +++ b/gtsam/geometry/PinholePose.h @@ -222,7 +222,7 @@ private: } public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW + GTSAM_MAKE_ALIGNED_OPERATOR_NEW }; // end of class PinholeBaseK @@ -425,7 +425,7 @@ private: } public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW + GTSAM_MAKE_ALIGNED_OPERATOR_NEW }; // end of class PinholePose diff --git a/gtsam/geometry/Pose2.h b/gtsam/geometry/Pose2.h index 388318827..2a1f108ca 100644 --- a/gtsam/geometry/Pose2.h +++ b/gtsam/geometry/Pose2.h @@ -317,7 +317,7 @@ public: public: // Align for Point2, which is either derived from, or is typedef, of Vector2 - EIGEN_MAKE_ALIGNED_OPERATOR_NEW + GTSAM_MAKE_ALIGNED_OPERATOR_NEW }; // Pose2 /** specialization for pose2 wedge function (generic template in Lie.h) */ diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h index fa55f98de..ced3b904b 100644 --- a/gtsam/geometry/Pose3.h +++ b/gtsam/geometry/Pose3.h @@ -355,7 +355,7 @@ public: #ifdef GTSAM_USE_QUATERNIONS // Align if we are using Quaternions public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW + GTSAM_MAKE_ALIGNED_OPERATOR_NEW #endif }; // Pose3 class diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h index fc3a8b3f2..8f24f07c8 100644 --- a/gtsam/geometry/Rot3.h +++ b/gtsam/geometry/Rot3.h @@ -544,7 +544,7 @@ namespace gtsam { #ifdef GTSAM_USE_QUATERNIONS // only align if quaternion, Matrix3 has no alignment requirements public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW + GTSAM_MAKE_ALIGNED_OPERATOR_NEW #endif }; diff --git a/gtsam/geometry/SOn.h b/gtsam/geometry/SOn.h index 767b12020..a08f87783 100644 --- a/gtsam/geometry/SOn.h +++ b/gtsam/geometry/SOn.h @@ -54,7 +54,7 @@ class SO : public LieGroup, internal::DimensionSO(N)> { using VectorN2 = Eigen::Matrix; using MatrixDD = Eigen::Matrix; - GTSAM_MAKE_ALIGNED_OPERATOR_NEW + GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF(true) protected: MatrixNN matrix_; ///< Rotation matrix diff --git a/gtsam/geometry/Unit3.h b/gtsam/geometry/Unit3.h index f1a9c1a69..b80e6e4d8 100644 --- a/gtsam/geometry/Unit3.h +++ b/gtsam/geometry/Unit3.h @@ -214,7 +214,7 @@ private: /// @} public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW + GTSAM_MAKE_ALIGNED_OPERATOR_NEW }; // Define GTSAM traits diff --git a/gtsam/geometry/triangulation.h b/gtsam/geometry/triangulation.h index 586b7b165..8cdf0fdc0 100644 --- a/gtsam/geometry/triangulation.h +++ b/gtsam/geometry/triangulation.h @@ -215,7 +215,7 @@ struct CameraProjectionMatrix { private: const Matrix3 K_; public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW + GTSAM_MAKE_ALIGNED_OPERATOR_NEW }; /** diff --git a/gtsam/navigation/AttitudeFactor.h b/gtsam/navigation/AttitudeFactor.h index db588008e..5a0031caf 100644 --- a/gtsam/navigation/AttitudeFactor.h +++ b/gtsam/navigation/AttitudeFactor.h @@ -139,7 +139,7 @@ private: } public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW + GTSAM_MAKE_ALIGNED_OPERATOR_NEW }; /// traits @@ -219,7 +219,7 @@ private: } public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW + GTSAM_MAKE_ALIGNED_OPERATOR_NEW }; /// traits diff --git a/gtsam/navigation/CombinedImuFactor.h b/gtsam/navigation/CombinedImuFactor.h index ca9b2ca1a..6b3bf979a 100644 --- a/gtsam/navigation/CombinedImuFactor.h +++ b/gtsam/navigation/CombinedImuFactor.h @@ -100,7 +100,7 @@ private: } public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW + GTSAM_MAKE_ALIGNED_OPERATOR_NEW }; @@ -210,7 +210,7 @@ public: } public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW + GTSAM_MAKE_ALIGNED_OPERATOR_NEW }; /** @@ -332,7 +332,7 @@ private: } public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW + GTSAM_MAKE_ALIGNED_OPERATOR_NEW }; // class CombinedImuFactor diff --git a/gtsam/navigation/ImuBias.h b/gtsam/navigation/ImuBias.h index ff1a53025..d52b4eb29 100644 --- a/gtsam/navigation/ImuBias.h +++ b/gtsam/navigation/ImuBias.h @@ -171,7 +171,7 @@ private: public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW + GTSAM_MAKE_ALIGNED_OPERATOR_NEW /// @} }; // ConstantBias class diff --git a/gtsam/navigation/PreintegratedRotation.h b/gtsam/navigation/PreintegratedRotation.h index 71ef5d08f..12938a625 100644 --- a/gtsam/navigation/PreintegratedRotation.h +++ b/gtsam/navigation/PreintegratedRotation.h @@ -69,7 +69,7 @@ struct GTSAM_EXPORT PreintegratedRotationParams { #ifdef GTSAM_USE_QUATERNIONS // Align if we are using Quaternions public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW + GTSAM_MAKE_ALIGNED_OPERATOR_NEW #endif }; @@ -182,7 +182,7 @@ class GTSAM_EXPORT PreintegratedRotation { #ifdef GTSAM_USE_QUATERNIONS // Align if we are using Quaternions public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW + GTSAM_MAKE_ALIGNED_OPERATOR_NEW #endif }; diff --git a/gtsam/navigation/PreintegrationBase.h b/gtsam/navigation/PreintegrationBase.h index 9926d207a..eb30c1f13 100644 --- a/gtsam/navigation/PreintegrationBase.h +++ b/gtsam/navigation/PreintegrationBase.h @@ -214,7 +214,7 @@ class GTSAM_EXPORT PreintegrationBase { #endif public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW + GTSAM_MAKE_ALIGNED_OPERATOR_NEW }; } /// namespace gtsam diff --git a/gtsam/navigation/PreintegrationParams.h b/gtsam/navigation/PreintegrationParams.h index 962fef277..4bff625ca 100644 --- a/gtsam/navigation/PreintegrationParams.h +++ b/gtsam/navigation/PreintegrationParams.h @@ -84,7 +84,7 @@ protected: #ifdef GTSAM_USE_QUATERNIONS // Align if we are using Quaternions public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW + GTSAM_MAKE_ALIGNED_OPERATOR_NEW #endif }; diff --git a/gtsam/navigation/TangentPreintegration.h b/gtsam/navigation/TangentPreintegration.h index 11945e53a..edf76e562 100644 --- a/gtsam/navigation/TangentPreintegration.h +++ b/gtsam/navigation/TangentPreintegration.h @@ -141,7 +141,7 @@ private: } public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW + GTSAM_MAKE_ALIGNED_OPERATOR_NEW }; } /// namespace gtsam diff --git a/gtsam/nonlinear/ExpressionFactor.h b/gtsam/nonlinear/ExpressionFactor.h index 04d82fe9a..c42b2bdfc 100644 --- a/gtsam/nonlinear/ExpressionFactor.h +++ b/gtsam/nonlinear/ExpressionFactor.h @@ -209,7 +209,7 @@ private: // Alignment, see https://eigen.tuxfamily.org/dox/group__TopicStructHavingEigenMembers.html enum { NeedsToAlign = (sizeof(T) % 16) == 0 }; public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign) + GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign) }; // ExpressionFactor diff --git a/gtsam/nonlinear/NonlinearEquality.h b/gtsam/nonlinear/NonlinearEquality.h index d4eb655c3..1bba57051 100644 --- a/gtsam/nonlinear/NonlinearEquality.h +++ b/gtsam/nonlinear/NonlinearEquality.h @@ -175,7 +175,7 @@ public: /// @} - EIGEN_MAKE_ALIGNED_OPERATOR_NEW + GTSAM_MAKE_ALIGNED_OPERATOR_NEW private: @@ -265,7 +265,7 @@ public: traits::Print(value_, "Value"); } - EIGEN_MAKE_ALIGNED_OPERATOR_NEW + GTSAM_MAKE_ALIGNED_OPERATOR_NEW private: @@ -331,7 +331,7 @@ public: return traits::Local(x1,x2); } - EIGEN_MAKE_ALIGNED_OPERATOR_NEW + GTSAM_MAKE_ALIGNED_OPERATOR_NEW private: diff --git a/gtsam/nonlinear/PriorFactor.h b/gtsam/nonlinear/PriorFactor.h index 8d8c67d5c..0afbaa588 100644 --- a/gtsam/nonlinear/PriorFactor.h +++ b/gtsam/nonlinear/PriorFactor.h @@ -114,7 +114,7 @@ namespace gtsam { // Alignment, see https://eigen.tuxfamily.org/dox/group__TopicStructHavingEigenMembers.html enum { NeedsToAlign = (sizeof(T) % 16) == 0 }; public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign) + GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign) }; } /// namespace gtsam diff --git a/gtsam/nonlinear/internal/ExpressionNode.h b/gtsam/nonlinear/internal/ExpressionNode.h index 0011efb74..0ae37f130 100644 --- a/gtsam/nonlinear/internal/ExpressionNode.h +++ b/gtsam/nonlinear/internal/ExpressionNode.h @@ -150,7 +150,7 @@ public: return constant_; } - EIGEN_MAKE_ALIGNED_OPERATOR_NEW + GTSAM_MAKE_ALIGNED_OPERATOR_NEW }; //----------------------------------------------------------------------------- diff --git a/gtsam/slam/BetweenFactor.h b/gtsam/slam/BetweenFactor.h index 23138b9e6..b1d4904aa 100644 --- a/gtsam/slam/BetweenFactor.h +++ b/gtsam/slam/BetweenFactor.h @@ -126,7 +126,7 @@ namespace gtsam { // Alignment, see https://eigen.tuxfamily.org/dox/group__TopicStructHavingEigenMembers.html enum { NeedsToAlign = (sizeof(VALUE) % 16) == 0 }; public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign) + GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign) }; // \class BetweenFactor /// traits diff --git a/gtsam/slam/EssentialMatrixConstraint.h b/gtsam/slam/EssentialMatrixConstraint.h index 179200fe1..e474ce5b3 100644 --- a/gtsam/slam/EssentialMatrixConstraint.h +++ b/gtsam/slam/EssentialMatrixConstraint.h @@ -105,7 +105,7 @@ private: } public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW + GTSAM_MAKE_ALIGNED_OPERATOR_NEW }; // \class EssentialMatrixConstraint diff --git a/gtsam/slam/EssentialMatrixFactor.h b/gtsam/slam/EssentialMatrixFactor.h index 8bd155a14..c214a2f48 100644 --- a/gtsam/slam/EssentialMatrixFactor.h +++ b/gtsam/slam/EssentialMatrixFactor.h @@ -81,7 +81,7 @@ public: } public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW + GTSAM_MAKE_ALIGNED_OPERATOR_NEW }; /** @@ -201,7 +201,7 @@ public: } public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW + GTSAM_MAKE_ALIGNED_OPERATOR_NEW }; // EssentialMatrixFactor2 @@ -286,7 +286,7 @@ public: } public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW + GTSAM_MAKE_ALIGNED_OPERATOR_NEW }; // EssentialMatrixFactor3 diff --git a/gtsam/slam/ProjectionFactor.h b/gtsam/slam/ProjectionFactor.h index 856913aae..0bed15fdc 100644 --- a/gtsam/slam/ProjectionFactor.h +++ b/gtsam/slam/ProjectionFactor.h @@ -189,7 +189,7 @@ namespace gtsam { } public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW + GTSAM_MAKE_ALIGNED_OPERATOR_NEW }; /// traits diff --git a/gtsam/slam/RotateFactor.h b/gtsam/slam/RotateFactor.h index b6ccc36a2..948fffe13 100644 --- a/gtsam/slam/RotateFactor.h +++ b/gtsam/slam/RotateFactor.h @@ -113,7 +113,7 @@ public: return error; } - EIGEN_MAKE_ALIGNED_OPERATOR_NEW + GTSAM_MAKE_ALIGNED_OPERATOR_NEW }; } // namespace gtsam diff --git a/gtsam/slam/SmartFactorBase.h b/gtsam/slam/SmartFactorBase.h index 34f9b9e9f..717a9c1f2 100644 --- a/gtsam/slam/SmartFactorBase.h +++ b/gtsam/slam/SmartFactorBase.h @@ -81,7 +81,7 @@ protected: mutable FBlocks Fs; public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW + GTSAM_MAKE_ALIGNED_OPERATOR_NEW /// shorthand for a smart pointer to a factor typedef boost::shared_ptr shared_ptr; From a4ef531a32b6fa078a24cd30ef17cbdb9580688e Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 24 Jun 2020 19:23:37 -0500 Subject: [PATCH 39/54] print Eigen Unsupported status message correctly --- gtsam/3rdparty/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/3rdparty/CMakeLists.txt b/gtsam/3rdparty/CMakeLists.txt index 89149d964..c8fecc339 100644 --- a/gtsam/3rdparty/CMakeLists.txt +++ b/gtsam/3rdparty/CMakeLists.txt @@ -17,7 +17,7 @@ if(NOT GTSAM_USE_SYSTEM_EIGEN) endforeach(eigen_dir) if(GTSAM_WITH_EIGEN_UNSUPPORTED) - message("-- Installing Eigen Unsupported modules") + message(STATUS "Installing Eigen Unsupported modules") # do the same for the unsupported eigen folder file(GLOB_RECURSE unsupported_eigen_headers "${CMAKE_CURRENT_SOURCE_DIR}/Eigen/unsupported/Eigen/*.h") From 7f1384b0f26e89136237e538779c6f6b0972c6b4 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 24 Jun 2020 19:25:56 -0500 Subject: [PATCH 40/54] wrap the biasHat function for PreintegratedMeasurement --- gtsam.h | 2 ++ 1 file changed, 2 insertions(+) diff --git a/gtsam.h b/gtsam.h index 614db91c7..bf4d7f4d1 100644 --- a/gtsam.h +++ b/gtsam.h @@ -2955,6 +2955,7 @@ class PreintegratedImuMeasurements { gtsam::Rot3 deltaRij() const; Vector deltaPij() const; Vector deltaVij() const; + gtsam::imuBias::ConstantBias biasHat() const; Vector biasHatVector() const; gtsam::NavState predict(const gtsam::NavState& state_i, const gtsam::imuBias::ConstantBias& bias) const; @@ -3016,6 +3017,7 @@ class PreintegratedCombinedMeasurements { gtsam::Rot3 deltaRij() const; Vector deltaPij() const; Vector deltaVij() const; + gtsam::imuBias::ConstantBias biasHat() const; Vector biasHatVector() const; gtsam::NavState predict(const gtsam::NavState& state_i, const gtsam::imuBias::ConstantBias& bias) const; From 350808d9dc7b6e2e31814e6b59ede8d59c9dfe8b Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 24 Jun 2020 19:27:45 -0500 Subject: [PATCH 41/54] added .gitignore for when building the sample cmake projects --- cmake/.gitignore | 1 + 1 file changed, 1 insertion(+) create mode 100644 cmake/.gitignore diff --git a/cmake/.gitignore b/cmake/.gitignore new file mode 100644 index 000000000..6f467448b --- /dev/null +++ b/cmake/.gitignore @@ -0,0 +1 @@ +**/build \ No newline at end of file From 17568e67793ad831b02ec701ef6734bbfe950f68 Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Thu, 25 Jun 2020 00:14:21 -0400 Subject: [PATCH 42/54] Add missing lf --- gtsam/base/types.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/base/types.h b/gtsam/base/types.h index 924f339b3..c5dac9ab7 100644 --- a/gtsam/base/types.h +++ b/gtsam/base/types.h @@ -276,4 +276,4 @@ namespace gtsam { */ #define GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign) \ EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign) \ - using _eigen_aligned_allocator_trait = void; \ No newline at end of file + using _eigen_aligned_allocator_trait = void; From fee226a1de09c57c086ded40769a3cc024b2cdc5 Mon Sep 17 00:00:00 2001 From: akrishnan86 Date: Wed, 24 Jun 2020 22:43:55 -0700 Subject: [PATCH 43/54] fix SfmData naming --- tests/testTranslationRecovery.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tests/testTranslationRecovery.cpp b/tests/testTranslationRecovery.cpp index 774a999e4..5a98c3bf5 100644 --- a/tests/testTranslationRecovery.cpp +++ b/tests/testTranslationRecovery.cpp @@ -33,7 +33,7 @@ using namespace gtsam; // sets up an optimization problem for the three unknown translations. TEST(TranslationRecovery, BAL) { const string filename = findExampleDataFile("dubrovnik-3-7-pre"); - SfM_data db; + SfmData db; bool success = readBAL(filename, db); if (!success) throw runtime_error("Could not access file!"); From c8583e921a2c370d244b7cf04e8f1a56a44d7f50 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 25 Jun 2020 10:28:59 -0500 Subject: [PATCH 44/54] Revert "added .gitignore for when building the sample cmake projects" This reverts commit 350808d9dc7b6e2e31814e6b59ede8d59c9dfe8b. --- cmake/.gitignore | 1 - 1 file changed, 1 deletion(-) delete mode 100644 cmake/.gitignore diff --git a/cmake/.gitignore b/cmake/.gitignore deleted file mode 100644 index 6f467448b..000000000 --- a/cmake/.gitignore +++ /dev/null @@ -1 +0,0 @@ -**/build \ No newline at end of file From 9d9c30e5dc399fafc1726c949f24276b160244b4 Mon Sep 17 00:00:00 2001 From: akrishnan86 Date: Sun, 28 Jun 2020 11:03:38 -0700 Subject: [PATCH 45/54] review1 changes --- gtsam/sfm/TranslationFactor.h | 6 ++++-- gtsam/sfm/TranslationRecovery.cpp | 2 +- gtsam/sfm/TranslationRecovery.h | 5 +++++ gtsam/sfm/tests/testTranslationFactor.cpp | 20 +++++++++++++++++++- 4 files changed, 29 insertions(+), 4 deletions(-) diff --git a/gtsam/sfm/TranslationFactor.h b/gtsam/sfm/TranslationFactor.h index 584b1fa69..9a4bd68a7 100644 --- a/gtsam/sfm/TranslationFactor.h +++ b/gtsam/sfm/TranslationFactor.h @@ -51,8 +51,10 @@ class TranslationFactor : public NoiseModelFactor2 { : Base(noiseModel, a, b), measured_w_aZb_(w_aZb.point3()) {} /** - * @brief Caclulate error norm(p1-p2) - measured - * + * @brief Caclulate error: (norm(Tb - Ta) - measurement) + * where Tb and Ta are Point3 translations and measurement is + * the Unit3 translation direction from a to b. + * * @param Ta translation for key a * @param Tb translation for key b * @param H1 optional jacobian in Ta diff --git a/gtsam/sfm/TranslationRecovery.cpp b/gtsam/sfm/TranslationRecovery.cpp index a0f3eb6b6..aeeae688f 100644 --- a/gtsam/sfm/TranslationRecovery.cpp +++ b/gtsam/sfm/TranslationRecovery.cpp @@ -60,7 +60,7 @@ void TranslationRecovery::addPrior(const double scale, } Values TranslationRecovery::initalizeRandomly() const { - // Create a lambda expression that checks whether value exists and randomly + // Create a lambda expression that checks whether value exists and randomly // initializes if not. Values initial; auto insert = [&initial](Key j) { diff --git a/gtsam/sfm/TranslationRecovery.h b/gtsam/sfm/TranslationRecovery.h index 634825d29..bb3c3cdb1 100644 --- a/gtsam/sfm/TranslationRecovery.h +++ b/gtsam/sfm/TranslationRecovery.h @@ -60,6 +60,9 @@ class TranslationRecovery { * * @param relativeTranslations the relative translations, in world coordinate * frames, indexed in a map by a pair of Pose keys. + * @param lmParams (optional) gtsam::LavenbergMarquardtParams that can be + * used to modify the parameters for the LM optimizer. By default, uses the + * default LM parameters. */ TranslationRecovery(const TranslationEdges& relativeTranslations, const LevenbergMarquardtParams& lmParams = LevenbergMarquardtParams()) @@ -102,6 +105,8 @@ class TranslationRecovery { * * @param poses SE(3) ground truth poses stored as Values * @param edges pairs (a,b) for which a measurement w_aZb will be generated. + * @return TranslationEdges map from a KeyPair to the simulated Unit3 + * translation direction measurement between the cameras in KeyPair. */ static TranslationEdges SimulateMeasurements( const Values& poses, const std::vector& edges); diff --git a/gtsam/sfm/tests/testTranslationFactor.cpp b/gtsam/sfm/tests/testTranslationFactor.cpp index da284bfd6..37e8b6c0f 100644 --- a/gtsam/sfm/tests/testTranslationFactor.cpp +++ b/gtsam/sfm/tests/testTranslationFactor.cpp @@ -38,7 +38,7 @@ TEST(TranslationFactor, Constructor) { } /* ************************************************************************* */ -TEST(TranslationFactor, Error) { +TEST(TranslationFactor, ZeroError) { // Create a factor TranslationFactor factor(kKey1, kKey2, kMeasured, model); @@ -51,6 +51,24 @@ TEST(TranslationFactor, Error) { // Verify we get the expected error Vector expected = (Vector3() << 0, 0, 0).finished(); EXPECT(assert_equal(expected, actualError, 1e-9)); + + +} + +/* ************************************************************************* */ +TEST(TranslationFactor, NonZeroError) { + // create a factor + TranslationFactor factor(kKey1, kKey2, kMeasured, model); + + // set the linearization + Point3 T1(0, 1, 1), T2(0, 2, 2); + + // use the factor to calculate the error + Vector actualError(factor.evaluateError(T1, T2)); + + // verify we get the expected error + Vector expected = (Vector3() << -1, 1/sqrt(2), 1/sqrt(2)).finished(); + EXPECT(assert_equal(expected, actualError, 1e-9)); } /* ************************************************************************* */ From 83cbcd0bea250e5c314131d3cca832800c6b3899 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 30 Jun 2020 13:13:04 -0500 Subject: [PATCH 46/54] capture stdout in python test [only for python3] --- cython/gtsam/tests/test_logging_optimizer.py | 12 +++++++++++- 1 file changed, 11 insertions(+), 1 deletion(-) diff --git a/cython/gtsam/tests/test_logging_optimizer.py b/cython/gtsam/tests/test_logging_optimizer.py index c857a6f7d..e7d9645b7 100644 --- a/cython/gtsam/tests/test_logging_optimizer.py +++ b/cython/gtsam/tests/test_logging_optimizer.py @@ -4,6 +4,8 @@ Author: Jing Wu and Frank Dellaert """ # pylint: disable=invalid-name +import io +import sys import unittest from datetime import datetime @@ -37,6 +39,14 @@ class TestOptimizeComet(GtsamTestCase): self.optimizer = gtsam.GaussNewtonOptimizer( graph, initial, self.params) + # setup output capture + self.capturedOutput = io.StringIO() + sys.stdout = self.capturedOutput + + def tearDown(self): + """Reset print capture.""" + sys.stdout = sys.__stdout__ + def test_simple_printing(self): """Test with a simple hook.""" @@ -76,4 +86,4 @@ class TestOptimizeComet(GtsamTestCase): self.gtsamAssertEquals(actual.atRot3(KEY), self.expected) if __name__ == "__main__": - unittest.main() \ No newline at end of file + unittest.main() From 52da4580fb58b79a2aca51c65fa4d1046f6a6f1d Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 1 Jul 2020 18:52:02 -0500 Subject: [PATCH 47/54] make utils and test code python2 compliant --- cython/gtsam/tests/test_logging_optimizer.py | 12 ++++++++---- cython/gtsam/utils/logging_optimizer.py | 10 +++------- 2 files changed, 11 insertions(+), 11 deletions(-) diff --git a/cython/gtsam/tests/test_logging_optimizer.py b/cython/gtsam/tests/test_logging_optimizer.py index e7d9645b7..69665db65 100644 --- a/cython/gtsam/tests/test_logging_optimizer.py +++ b/cython/gtsam/tests/test_logging_optimizer.py @@ -4,8 +4,12 @@ Author: Jing Wu and Frank Dellaert """ # pylint: disable=invalid-name -import io import sys +if sys.version_info.major >= 3: + from io import StringIO +else: + from cStringIO import StringIO + import unittest from datetime import datetime @@ -40,7 +44,7 @@ class TestOptimizeComet(GtsamTestCase): graph, initial, self.params) # setup output capture - self.capturedOutput = io.StringIO() + self.capturedOutput = StringIO() sys.stdout = self.capturedOutput def tearDown(self): @@ -51,7 +55,7 @@ class TestOptimizeComet(GtsamTestCase): """Test with a simple hook.""" # Provide a hook that just prints - def hook(_, error: float): + def hook(_, error): print(error) # Only thing we require from optimizer is an iterate method @@ -75,7 +79,7 @@ class TestOptimizeComet(GtsamTestCase): + str(time.hour)+":"+str(time.minute)+":"+str(time.second)) # I want to do some comet thing here - def hook(optimizer, error: float): + def hook(optimizer, error): comet.log_metric("Karcher error", error, optimizer.iterations()) diff --git a/cython/gtsam/utils/logging_optimizer.py b/cython/gtsam/utils/logging_optimizer.py index b201bb8aa..939453927 100644 --- a/cython/gtsam/utils/logging_optimizer.py +++ b/cython/gtsam/utils/logging_optimizer.py @@ -4,15 +4,11 @@ Author: Jing Wu and Frank Dellaert """ # pylint: disable=invalid-name -from typing import TypeVar - from gtsam import NonlinearOptimizer, NonlinearOptimizerParams import gtsam -T = TypeVar('T') - -def optimize(optimizer: T, check_convergence, hook): +def optimize(optimizer, check_convergence, hook): """ Given an optimizer and a convergence check, iterate until convergence. After each iteration, hook(optimizer, error) is called. After the function, use values and errors to get the result. @@ -36,8 +32,8 @@ def optimize(optimizer: T, check_convergence, hook): current_error = new_error -def gtsam_optimize(optimizer: NonlinearOptimizer, - params: NonlinearOptimizerParams, +def gtsam_optimize(optimizer, + params, hook): """ Given an optimizer and params, iterate until convergence. After each iteration, hook(optimizer) is called. From 564d2c58730e9710385b4009e7e676117076ccb2 Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Sun, 5 Jul 2020 10:19:05 -0400 Subject: [PATCH 48/54] Fix memory leak in Expressions --- gtsam/nonlinear/internal/ExecutionTrace.h | 6 ++++++ gtsam/nonlinear/tests/testExecutionTrace.cpp | 1 + 2 files changed, 7 insertions(+) diff --git a/gtsam/nonlinear/internal/ExecutionTrace.h b/gtsam/nonlinear/internal/ExecutionTrace.h index ace0aaea8..2943b5e68 100644 --- a/gtsam/nonlinear/internal/ExecutionTrace.h +++ b/gtsam/nonlinear/internal/ExecutionTrace.h @@ -169,6 +169,12 @@ class ExecutionTrace { content.ptr->reverseAD2(dTdA, jacobians); } + ~ExecutionTrace() { + if (kind == Function) { + content.ptr->~CallRecord(); + } + } + /// Define type so we can apply it as a meta-function typedef ExecutionTrace type; }; diff --git a/gtsam/nonlinear/tests/testExecutionTrace.cpp b/gtsam/nonlinear/tests/testExecutionTrace.cpp index c2b245780..58f76089a 100644 --- a/gtsam/nonlinear/tests/testExecutionTrace.cpp +++ b/gtsam/nonlinear/tests/testExecutionTrace.cpp @@ -16,6 +16,7 @@ * @brief unit tests for Expression internals */ +#include #include #include From 258d05c9efd5223c29a80441bb7ebdb3aeada224 Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Sat, 4 Jul 2020 20:31:04 -0400 Subject: [PATCH 49/54] Fix TranslationFactor with Vector3 as Point3 --- gtsam/sfm/TranslationFactor.h | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/gtsam/sfm/TranslationFactor.h b/gtsam/sfm/TranslationFactor.h index 9a4bd68a7..d63633d7e 100644 --- a/gtsam/sfm/TranslationFactor.h +++ b/gtsam/sfm/TranslationFactor.h @@ -67,8 +67,7 @@ class TranslationFactor : public NoiseModelFactor2 { boost::optional H2 = boost::none) const override { const Point3 dir = Tb - Ta; Matrix33 H_predicted_dir; - const Point3 predicted = - dir.normalized(H1 || H2 ? &H_predicted_dir : nullptr); + const Point3 predicted = normalize(dir, H1 || H2 ? &H_predicted_dir : nullptr); if (H1) *H1 = -H_predicted_dir; if (H2) *H2 = H_predicted_dir; return predicted - measured_w_aZb_; From df687e5abf4da44933f00b121356817f1c85bd6c Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Fri, 19 Jun 2020 10:40:07 -0400 Subject: [PATCH 50/54] Fix MSVC build --- gtsam/base/types.h | 1 + 1 file changed, 1 insertion(+) diff --git a/gtsam/base/types.h b/gtsam/base/types.h index c5dac9ab7..857de00c9 100644 --- a/gtsam/base/types.h +++ b/gtsam/base/types.h @@ -28,6 +28,7 @@ #include #include +#include #ifdef GTSAM_USE_TBB #include From 3c5f8711caabe0a1db5a7b293b2bed05f74004d2 Mon Sep 17 00:00:00 2001 From: Jose Luis Blanco-Claraco Date: Mon, 6 Jul 2020 20:07:18 +0200 Subject: [PATCH 51/54] Fix missing DLL exported symbol --- gtsam/base/types.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/base/types.h b/gtsam/base/types.h index 857de00c9..aaada3cee 100644 --- a/gtsam/base/types.h +++ b/gtsam/base/types.h @@ -55,7 +55,7 @@ namespace gtsam { /// Function to demangle type name of variable, e.g. demangle(typeid(x).name()) - std::string demangle(const char* name); + std::string GTSAM_EXPORT demangle(const char* name); /// Integer nonlinear key type typedef std::uint64_t Key; From 683e37f14889701b5acd40797d6a5a0648c2e601 Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Mon, 6 Jul 2020 23:36:17 -0400 Subject: [PATCH 52/54] Fix FrobeniusWormholeFactor Python test --- cython/gtsam/tests/test_FrobeniusFactor.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/cython/gtsam/tests/test_FrobeniusFactor.py b/cython/gtsam/tests/test_FrobeniusFactor.py index 82d1fb410..f3f5354bb 100644 --- a/cython/gtsam/tests/test_FrobeniusFactor.py +++ b/cython/gtsam/tests/test_FrobeniusFactor.py @@ -12,7 +12,7 @@ Author: Frank Dellaert import unittest import numpy as np -from gtsam import (SO3, SO4, FrobeniusBetweenFactorSO4, FrobeniusFactorSO4, +from gtsam import (Rot3, SO3, SO4, FrobeniusBetweenFactorSO4, FrobeniusFactorSO4, FrobeniusWormholeFactor, SOn) id = SO4() @@ -43,7 +43,7 @@ class TestFrobeniusFactorSO4(unittest.TestCase): """Test creation of a factor that calculates Shonan error.""" R1 = SO3.Expmap(v1[3:]) R2 = SO3.Expmap(v2[3:]) - factor = FrobeniusWormholeFactor(1, 2, R1.between(R2), p=4) + factor = FrobeniusWormholeFactor(1, 2, Rot3(R1.between(R2).matrix()), p=4) I4 = SOn(4) Q1 = I4.retract(v1) Q2 = I4.retract(v2) From 00106ba3605201b6d0e0b61076521c974458a6f4 Mon Sep 17 00:00:00 2001 From: Mike Sheffler Date: Wed, 8 Jul 2020 02:30:19 -0700 Subject: [PATCH 53/54] Second attempt at a wrapper fix. 1) Some serialization code was missing from SOn.SOn.h (not sure why this wouldn't have been a problem before building the MATLAB toolbox ...) 2) FrobeniusFacotor stuff needed a couple GTSAM_EXPORT statements --- gtsam/geometry/SOn.h | 17 +++++++++++++++++ gtsam/slam/FrobeniusFactor.h | 4 ++-- 2 files changed, 19 insertions(+), 2 deletions(-) diff --git a/gtsam/geometry/SOn.h b/gtsam/geometry/SOn.h index a08f87783..74b7b8521 100644 --- a/gtsam/geometry/SOn.h +++ b/gtsam/geometry/SOn.h @@ -30,6 +30,9 @@ #include #include + // For save/load +#include + namespace gtsam { namespace internal { @@ -292,6 +295,10 @@ class SO : public LieGroup, internal::DimensionSO(N)> { boost::none) const; /// @} + template + friend void save(Archive&, SO&, const unsigned int); + template + friend void load(Archive&, SO&, const unsigned int); template friend void serialize(Archive&, SO&, const unsigned int); friend class boost::serialization::access; @@ -329,6 +336,16 @@ template <> SOn LieGroup::between(const SOn& g, DynamicJacobian H1, DynamicJacobian H2) const; +/** Serialization function */ +template +void serialize( + Archive& ar, SOn& Q, + const unsigned int file_version +) { + Matrix& M = Q.matrix_; + ar& M; +} + /* * Define the traits. internal::LieGroup provides both Lie group and Testable */ diff --git a/gtsam/slam/FrobeniusFactor.h b/gtsam/slam/FrobeniusFactor.h index 60fa82ab4..a73445ae0 100644 --- a/gtsam/slam/FrobeniusFactor.h +++ b/gtsam/slam/FrobeniusFactor.h @@ -33,7 +33,7 @@ namespace gtsam { * isotropic. If it is, we extend to 'Dim' dimensions, otherwise we throw an * error. If defaultToUnit == false throws an exception on unexepcted input. */ -boost::shared_ptr ConvertPose3NoiseModel( + GTSAM_EXPORT boost::shared_ptr ConvertPose3NoiseModel( const SharedNoiseModel& model, size_t d, bool defaultToUnit = true); /** @@ -125,7 +125,7 @@ class FrobeniusBetweenFactor : public NoiseModelFactor2 { * the SO(p) matrices down to a Stiefel manifold of p*d matrices. * TODO(frank): template on D=2 or 3 */ -class FrobeniusWormholeFactor : public NoiseModelFactor2 { +class GTSAM_EXPORT FrobeniusWormholeFactor : public NoiseModelFactor2 { Matrix M_; ///< measured rotation between R1 and R2 size_t p_, pp_, dimension_; ///< dimensionality constants Matrix G_; ///< matrix of vectorized generators From 283999b017f8c0b41ada170a74030f41ef6c1ca1 Mon Sep 17 00:00:00 2001 From: Mike Sheffler Date: Wed, 8 Jul 2020 11:52:51 -0700 Subject: [PATCH 54/54] Unnecessary include statement --- gtsam/geometry/SOn.h | 3 --- 1 file changed, 3 deletions(-) diff --git a/gtsam/geometry/SOn.h b/gtsam/geometry/SOn.h index 74b7b8521..f44c578cc 100644 --- a/gtsam/geometry/SOn.h +++ b/gtsam/geometry/SOn.h @@ -30,9 +30,6 @@ #include #include - // For save/load -#include - namespace gtsam { namespace internal {