diff --git a/gtsam/sfm/TranslationRecovery.cpp b/gtsam/sfm/TranslationRecovery.cpp index 665336cc4..319129840 100644 --- a/gtsam/sfm/TranslationRecovery.cpp +++ b/gtsam/sfm/TranslationRecovery.cpp @@ -45,9 +45,8 @@ NonlinearFactorGraph TranslationRecovery::buildGraph() const { } void TranslationRecovery::addPrior(const double scale, - NonlinearFactorGraph *graph) const { - //TODO(akshay-krishnan): make this an input argument - auto priorNoiseModel = noiseModel::Isotropic::Sigma(3, 0.01); + NonlinearFactorGraph *graph, + const SharedNoiseModel &priorNoiseModel) const { auto edge = relativeTranslations_.begin(); graph->emplace_shared >(edge->key1(), Point3(0, 0, 0), priorNoiseModel); graph->emplace_shared >(edge->key2(), scale * edge->measured().point3(), diff --git a/gtsam/sfm/TranslationRecovery.h b/gtsam/sfm/TranslationRecovery.h index 4c6a95cbe..d5538f91b 100644 --- a/gtsam/sfm/TranslationRecovery.h +++ b/gtsam/sfm/TranslationRecovery.h @@ -68,9 +68,7 @@ class TranslationRecovery { */ TranslationRecovery(const TranslationEdges &relativeTranslations, const LevenbergMarquardtParams &lmParams = LevenbergMarquardtParams()) - : relativeTranslations_(relativeTranslations), params_(lmParams) { - params_.setVerbosityLM("Summary"); - } + : relativeTranslations_(relativeTranslations), params_(lmParams) {} /** * @brief Build the factor graph to do the optimization. @@ -84,8 +82,11 @@ class TranslationRecovery { * * @param scale scale for first relative translation which fixes gauge. * @param graph factor graph to which prior is added. + * @param priorNoiseModel the noise model to use with the prior. */ - void addPrior(const double scale, NonlinearFactorGraph *graph) const; + void addPrior(const double scale, NonlinearFactorGraph *graph, + const SharedNoiseModel &priorNoiseModel = + noiseModel::Isotropic::Sigma(3, 0.01)) const; /** * @brief Create random initial translations. diff --git a/python/gtsam/tests/test_TranslationRecovery.py b/python/gtsam/tests/test_TranslationRecovery.py index ec6d6a83f..0fb0518b6 100644 --- a/python/gtsam/tests/test_TranslationRecovery.py +++ b/python/gtsam/tests/test_TranslationRecovery.py @@ -40,7 +40,12 @@ class TestTranslationRecovery(unittest.TestCase): def test_run(self): gt_poses = ExampleValues() measurements = SimulateMeasurements(gt_poses, [[0, 1], [0, 2], [1, 2]]) - algorithm = gtsam.TranslationRecovery(measurements) + + # Set verbosity to Silent for tests + lmParams = gtsam.LevenbergMarquardtParams() + lmParams.setVerbosityLM("silent") + + algorithm = gtsam.TranslationRecovery(measurements, lmParams) scale = 2.0 result = algorithm.run(scale)