From e17eddf99f8eac95077887b871f1859f7d5eb802 Mon Sep 17 00:00:00 2001 From: Akshay Krishnan Date: Mon, 9 May 2022 22:46:16 -0700 Subject: [PATCH] move values from params to run, remove params struct --- gtsam/sfm/TranslationRecovery.cpp | 20 ++++---- gtsam/sfm/TranslationRecovery.h | 46 +++++++++---------- gtsam/sfm/sfm.i | 12 ++--- .../gtsam/tests/test_TranslationRecovery.py | 4 +- 4 files changed, 41 insertions(+), 41 deletions(-) diff --git a/gtsam/sfm/TranslationRecovery.cpp b/gtsam/sfm/TranslationRecovery.cpp index 122b17ce6..810fe7de9 100644 --- a/gtsam/sfm/TranslationRecovery.cpp +++ b/gtsam/sfm/TranslationRecovery.cpp @@ -134,15 +134,15 @@ void TranslationRecovery::addPrior( Values TranslationRecovery::initializeRandomly( const std::vector> &relativeTranslations, - std::mt19937 *rng) const { + std::mt19937 *rng, const Values &initialValues) const { uniform_real_distribution randomVal(-1, 1); // Create a lambda expression that checks whether value exists and randomly // initializes if not. Values initial; auto insert = [&](Key j) { if (initial.exists(j)) return; - if (params_.initial.exists(j)) { - initial.insert(j, params_.initial.at(j)); + if (initialValues.exists(j)) { + initial.insert(j, initialValues.at(j)); } else { initial.insert( j, Point3(randomVal(*rng), randomVal(*rng), randomVal(*rng))); @@ -159,13 +159,16 @@ Values TranslationRecovery::initializeRandomly( } Values TranslationRecovery::initializeRandomly( - const std::vector> &relativeTranslations) const { - return initializeRandomly(relativeTranslations, &kRandomNumberGenerator); + const std::vector> &relativeTranslations, + const Values &initialValues) const { + return initializeRandomly(relativeTranslations, &kRandomNumberGenerator, + initialValues); } Values TranslationRecovery::run( const TranslationEdges &relativeTranslations, const double scale, - const std::vector> &betweenTranslations) const { + const std::vector> &betweenTranslations, + const Values &initialValues) const { // Find edges that have a zero-translation, and recompute relativeTranslations // and betweenTranslations by retaining only one node for every zero-edge. DSFMap sameTranslationDSFMap = @@ -184,7 +187,8 @@ Values TranslationRecovery::run( &graph); // Uses initial values from params if provided. - Values initial = initializeRandomly(nonzeroRelativeTranslations); + Values initial = + initializeRandomly(nonzeroRelativeTranslations, initialValues); // If there are no valid edges, but zero-distance edges exist, initialize one // of the nodes in a connected component of zero-distance edges. @@ -195,7 +199,7 @@ Values TranslationRecovery::run( } } - LevenbergMarquardtOptimizer lm(graph, initial, params_.lmParams); + LevenbergMarquardtOptimizer lm(graph, initial, lmParams_); Values result = lm.optimize(); return addSameTranslationNodes(result, sameTranslationDSFMap); } diff --git a/gtsam/sfm/TranslationRecovery.h b/gtsam/sfm/TranslationRecovery.h index 3462cce02..7863f5133 100644 --- a/gtsam/sfm/TranslationRecovery.h +++ b/gtsam/sfm/TranslationRecovery.h @@ -28,15 +28,6 @@ namespace gtsam { -// Parameters for the Translation Recovery problem. -struct TranslationRecoveryParams { - // LevenbergMarquardtParams for optimization. - LevenbergMarquardtParams lmParams; - - // Initial values, random intialization will be used if not provided. - Values initial; -}; - // 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, @@ -67,16 +58,16 @@ class TranslationRecovery { TranslationEdges relativeTranslations_; // Parameters. - TranslationRecoveryParams params_; + LevenbergMarquardtParams lmParams_; public: /** * @brief Construct a new Translation Recovery object * - * @param params parameters for the recovery problem. + * @param lmParams parameters for optimization. */ - TranslationRecovery(const TranslationRecoveryParams ¶ms) - : params_(params) {} + TranslationRecovery(const LevenbergMarquardtParams &lmParams) + : lmParams_(lmParams) {} /** * @brief Default constructor. @@ -94,7 +85,11 @@ class TranslationRecovery { const std::vector> &relativeTranslations) const; /** - * @brief Add priors on ednpoints of first measurement edge. + * @brief Add 3 factors to the graph: + * - A prior on the first point to lie at (0, 0, 0) + * - If betweenTranslations is non-empty, between factors provided by it. + * - If betweenTranslations is empty, a prior on scale of the first + * relativeTranslations edge. * * @param relativeTranslations unit translation directions between * translations to be estimated @@ -113,28 +108,29 @@ class TranslationRecovery { noiseModel::Isotropic::Sigma(3, 0.01)) const; /** - * @brief Create random initial translations. Uses inial values from params if - * provided. + * @brief Create random initial translations. * * @param relativeTranslations unit translation directions between * translations to be estimated * @param rng random number generator + * @param intialValues (optional) initial values from a prior * @return Values */ Values initializeRandomly( const std::vector> &relativeTranslations, - std::mt19937 *rng) const; + std::mt19937 *rng, const Values &initialValues = Values()) const; /** - * @brief Version of initializeRandomly with a fixed seed. Uses initial values - * from params if provided. + * @brief Version of initializeRandomly with a fixed seed. * * @param relativeTranslations unit translation directions between * translations to be estimated + * @param initialValues (optional) initial values from a prior * @return Values */ Values initializeRandomly( - const std::vector> &relativeTranslations) const; + const std::vector> &relativeTranslations, + const Values &initialValues = Values()) const; /** * @brief Build and optimize factor graph. @@ -146,12 +142,14 @@ class TranslationRecovery { * The scale is only used if betweenTranslations is empty. * @param betweenTranslations relative translations (with scale) between 2 * points in world coordinate frame known a priori. + * @param initialValues intial values for optimization. Initializes randomly + * if not provided. * @return Values */ - Values run(const TranslationEdges &relativeTranslations, - const double scale = 1.0, - const std::vector> &betweenTranslations = - {}) const; + Values run( + const TranslationEdges &relativeTranslations, const double scale = 1.0, + const std::vector> &betweenTranslations = {}, + const Values &initialValues = Values()) const; /** * @brief Simulate translation direction measurements diff --git a/gtsam/sfm/sfm.i b/gtsam/sfm/sfm.i index 122a9b5a6..33f23614a 100644 --- a/gtsam/sfm/sfm.i +++ b/gtsam/sfm/sfm.i @@ -262,14 +262,9 @@ class MFAS { }; #include -class TranslationRecoveryParams { - gtsam::LevenbergMarquardtParams lmParams; - gtsam::Values initial; - TranslationRecoveryParams(); -}; class TranslationRecovery { - TranslationRecovery(const gtsam::TranslationRecoveryParams& lmParams); + TranslationRecovery(const gtsam::LevenbergMarquardtParams& lmParams); TranslationRecovery(); // default params. void addPrior(const gtsam::BinaryMeasurementsUnit3& relativeTranslations, const double scale, @@ -282,6 +277,11 @@ class TranslationRecovery { gtsam::NonlinearFactorGraph @graph) const; gtsam::NonlinearFactorGraph buildGraph( const gtsam::BinaryMeasurementsUnit3& relativeTranslations) const; + gtsam::Values run(const gtsam::BinaryMeasurementsUnit3& relativeTranslations, + const double scale, + const gtsam::BinaryMeasurementsPoint3& betweenTranslations, + const gtsam::Values& initialValues) const; + // default random initial values gtsam::Values run( const gtsam::BinaryMeasurementsUnit3& relativeTranslations, const double scale, diff --git a/python/gtsam/tests/test_TranslationRecovery.py b/python/gtsam/tests/test_TranslationRecovery.py index 2875e897e..0908f0a69 100644 --- a/python/gtsam/tests/test_TranslationRecovery.py +++ b/python/gtsam/tests/test_TranslationRecovery.py @@ -46,10 +46,8 @@ class TestTranslationRecovery(unittest.TestCase): # Set verbosity to Silent for tests lmParams = gtsam.LevenbergMarquardtParams() lmParams.setVerbosityLM("silent") - params = gtsam.TranslationRecoveryParams() - params.lmParams = lmParams - algorithm = gtsam.TranslationRecovery(params) + algorithm = gtsam.TranslationRecovery(lmParams) scale = 2.0 result = algorithm.run(measurements, scale)