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);