From 5abe293cdf48bc47c76938ed71512cd296abec4d Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 7 Mar 2020 20:57:08 -0500 Subject: [PATCH 01/21] 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/21] 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/21] 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/21] 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/21] 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/21] 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/21] 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/21] 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/21] 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 0bd81430573a32712642a546880600f498cac745 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Fri, 19 Jun 2020 23:33:29 -0400 Subject: [PATCH 10/21] 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 11/21] 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 3ea9ff012005b1205b8c20f5ea2b676d4054ce76 Mon Sep 17 00:00:00 2001 From: akrishnan86 Date: Tue, 23 Jun 2020 07:58:38 -0700 Subject: [PATCH 12/21] 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 fee226a1de09c57c086ded40769a3cc024b2cdc5 Mon Sep 17 00:00:00 2001 From: akrishnan86 Date: Wed, 24 Jun 2020 22:43:55 -0700 Subject: [PATCH 13/21] 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 9d9c30e5dc399fafc1726c949f24276b160244b4 Mon Sep 17 00:00:00 2001 From: akrishnan86 Date: Sun, 28 Jun 2020 11:03:38 -0700 Subject: [PATCH 14/21] 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 15/21] 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 16/21] 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 17/21] 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 18/21] 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 19/21] 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 20/21] 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 21/21] 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)