From 5abe293cdf48bc47c76938ed71512cd296abec4d Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 7 Mar 2020 20:57:08 -0500 Subject: [PATCH 01/12] 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/12] 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/12] 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/12] 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/12] 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/12] 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/12] 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/12] 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/12] 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 3ea9ff012005b1205b8c20f5ea2b676d4054ce76 Mon Sep 17 00:00:00 2001 From: akrishnan86 Date: Tue, 23 Jun 2020 07:58:38 -0700 Subject: [PATCH 10/12] 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 11/12] 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 12/12] 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)); } /* ************************************************************************* */