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