diff --git a/gtsam/sfm/TranslationRecovery.cpp b/gtsam/sfm/TranslationRecovery.cpp index ce332ed0b..ce8e9c079 100644 --- a/gtsam/sfm/TranslationRecovery.cpp +++ b/gtsam/sfm/TranslationRecovery.cpp @@ -84,7 +84,7 @@ NonlinearFactorGraph TranslationRecovery::buildGraph() const { void TranslationRecovery::addPrior( const std::vector> &betweenTranslations, - const double scale, const boost::shared_ptr graph, + const double scale, NonlinearFactorGraph *graph, const SharedNoiseModel &priorNoiseModel) const { auto edge = relativeTranslations_.begin(); if (edge == relativeTranslations_.end()) return; @@ -122,11 +122,10 @@ Values TranslationRecovery::initializeRandomly(std::mt19937 *rng) const { // Create a lambda expression that checks whether value exists and randomly // initializes if not. Values initial; - const Values inputInitial = params_.getInitialValues(); auto insert = [&](Key j) { if (initial.exists(j)) return; - if (inputInitial.exists(j)) { - initial.insert(j, inputInitial.at(j)); + if (params_.initial.exists(j)) { + initial.insert(j, params_.initial.at(j)); } else { initial.insert( j, Point3(randomVal(*rng), randomVal(*rng), randomVal(*rng))); @@ -158,11 +157,10 @@ Values TranslationRecovery::initializeRandomly() const { Values TranslationRecovery::run( const std::vector> &betweenTranslations, const double scale) const { - boost::shared_ptr graph_ptr = - boost::make_shared(buildGraph()); - addPrior(betweenTranslations, scale, graph_ptr); + NonlinearFactorGraph graph = buildGraph(); + addPrior(betweenTranslations, scale, &graph); const Values initial = initializeRandomly(); - LevenbergMarquardtOptimizer lm(*graph_ptr, initial, params_.getLMParams()); + LevenbergMarquardtOptimizer lm(graph, initial, params_.lmParams); Values result = lm.optimize(); return addSameTranslationNodes(result); } diff --git a/gtsam/sfm/TranslationRecovery.h b/gtsam/sfm/TranslationRecovery.h index 33de91bf1..0b25ad7b4 100644 --- a/gtsam/sfm/TranslationRecovery.h +++ b/gtsam/sfm/TranslationRecovery.h @@ -29,24 +29,12 @@ namespace gtsam { // Parameters for the Translation Recovery problem. -class TranslationRecoveryParams { - public: - LevenbergMarquardtParams getLMParams() const { return lmParams_; } - - Values getInitialValues() const { return initial_; } - - void setInitialValues(const Values &values) { initial_ = values; } - - void setLMParams(const LevenbergMarquardtParams &lmParams) { - lmParams_ = lmParams; - } - - private: +struct TranslationRecoveryParams { // LevenbergMarquardtParams for optimization. - LevenbergMarquardtParams lmParams_; + LevenbergMarquardtParams lmParams; // Initial values, random intialization will be used if not provided. - Values initial_; + Values initial; }; // Set up an optimization problem for the unknown translations Ti in the world @@ -114,7 +102,7 @@ class TranslationRecovery { */ void addPrior( const std::vector> &betweenTranslations, - const double scale, const boost::shared_ptr graph, + const double scale, NonlinearFactorGraph *graph, const SharedNoiseModel &priorNoiseModel = noiseModel::Isotropic::Sigma(3, 0.01)) const; diff --git a/gtsam/sfm/sfm.i b/gtsam/sfm/sfm.i index 064bc1e63..66280037c 100644 --- a/gtsam/sfm/sfm.i +++ b/gtsam/sfm/sfm.i @@ -263,11 +263,8 @@ class MFAS { #include class TranslationRecoveryParams { - gtsam::Values getInitialValues() const; - gtsam::LevenbergMarquardtParams getLMParams() const; - - void setInitialValues(const gtsam::Values& values); - void setLMParams(const gtsam::LevenbergMarquardtParams& lmParams); + gtsam::LevenbergMarquardtParams lmParams; + gtsam::Values initial; }; class TranslationRecovery { @@ -279,7 +276,7 @@ class TranslationRecovery { gtsam::Values run(const gtsam::BinaryMeasurementsPoint3& betweenTranslations, const double scale) const; // default empty betweenTranslations - gtsam::Values run(const double scale) const; + // gtsam::Values run(const double scale) const; gtsam::Values run() const; // default scale = 1.0 };