12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

change params to struct

release/4.3a0
Akshay Krishnan 2022-05-07 11:24:31 -07:00
parent 0e8c5eb5a7
commit b42d3a3d2f
3 changed files with 13 additions and 30 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
gtsam/sfm/TranslationRecovery.cpp
View File

@ -84,7 +84,7 @@ NonlinearFactorGraph TranslationRecovery::buildGraph() const {
void TranslationRecovery::addPrior( void TranslationRecovery::addPrior(
const std::vector<BinaryMeasurement<Point3>> &betweenTranslations, const std::vector<BinaryMeasurement<Point3>> &betweenTranslations,
const double scale, const boost::shared_ptr<NonlinearFactorGraph> graph, const double scale, NonlinearFactorGraph *graph,
const SharedNoiseModel &priorNoiseModel) const { const SharedNoiseModel &priorNoiseModel) const {
auto edge = relativeTranslations_.begin(); auto edge = relativeTranslations_.begin();
if (edge == relativeTranslations_.end()) return; 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 // Create a lambda expression that checks whether value exists and randomly
// initializes if not. // initializes if not.
Values initial; Values initial;
const Values inputInitial = params_.getInitialValues();
auto insert = [&](Key j) { auto insert = [&](Key j) {
if (initial.exists(j)) return; if (initial.exists(j)) return;
if (inputInitial.exists(j)) { if (params_.initial.exists(j)) {
initial.insert<Point3>(j, inputInitial.at<Point3>(j)); initial.insert<Point3>(j, params_.initial.at<Point3>(j));
} else { } else {
initial.insert<Point3>( initial.insert<Point3>(
j, Point3(randomVal(*rng), randomVal(*rng), randomVal(*rng))); j, Point3(randomVal(*rng), randomVal(*rng), randomVal(*rng)));
@ -158,11 +157,10 @@ Values TranslationRecovery::initializeRandomly() const {
Values TranslationRecovery::run( Values TranslationRecovery::run(
const std::vector<BinaryMeasurement<Point3>> &betweenTranslations, const std::vector<BinaryMeasurement<Point3>> &betweenTranslations,
const double scale) const { const double scale) const {
boost::shared_ptr<NonlinearFactorGraph> graph_ptr = NonlinearFactorGraph graph = buildGraph();
boost::make_shared<NonlinearFactorGraph>(buildGraph()); addPrior(betweenTranslations, scale, &graph);
addPrior(betweenTranslations, scale, graph_ptr);
const Values initial = initializeRandomly(); const Values initial = initializeRandomly();
LevenbergMarquardtOptimizer lm(*graph_ptr, initial, params_.getLMParams()); LevenbergMarquardtOptimizer lm(graph, initial, params_.lmParams);
Values result = lm.optimize(); Values result = lm.optimize();
return addSameTranslationNodes(result); return addSameTranslationNodes(result);
} }

20
gtsam/sfm/TranslationRecovery.h
View File

@ -29,24 +29,12 @@
namespace gtsam { namespace gtsam {
// Parameters for the Translation Recovery problem. // Parameters for the Translation Recovery problem.
class TranslationRecoveryParams { struct 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:
// LevenbergMarquardtParams for optimization. // LevenbergMarquardtParams for optimization.
LevenbergMarquardtParams lmParams_; LevenbergMarquardtParams lmParams;
// Initial values, random intialization will be used if not provided. // 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 // Set up an optimization problem for the unknown translations Ti in the world
@ -114,7 +102,7 @@ class TranslationRecovery {
*/ */
void addPrior( void addPrior(
const std::vector<BinaryMeasurement<Point3>> &betweenTranslations, const std::vector<BinaryMeasurement<Point3>> &betweenTranslations,
const double scale, const boost::shared_ptr<NonlinearFactorGraph> graph, const double scale, NonlinearFactorGraph *graph,
const SharedNoiseModel &priorNoiseModel = const SharedNoiseModel &priorNoiseModel =
noiseModel::Isotropic::Sigma(3, 0.01)) const; noiseModel::Isotropic::Sigma(3, 0.01)) const;

9
gtsam/sfm/sfm.i
View File

@ -263,11 +263,8 @@ class MFAS {
#include <gtsam/sfm/TranslationRecovery.h> #include <gtsam/sfm/TranslationRecovery.h>
class TranslationRecoveryParams { class TranslationRecoveryParams {
gtsam::Values getInitialValues() const; gtsam::LevenbergMarquardtParams lmParams;
gtsam::LevenbergMarquardtParams getLMParams() const; gtsam::Values initial;
void setInitialValues(const gtsam::Values& values);
void setLMParams(const gtsam::LevenbergMarquardtParams& lmParams);
}; };
class TranslationRecovery { class TranslationRecovery {
@ -279,7 +276,7 @@ class TranslationRecovery {
gtsam::Values run(const gtsam::BinaryMeasurementsPoint3& betweenTranslations, gtsam::Values run(const gtsam::BinaryMeasurementsPoint3& betweenTranslations,
const double scale) const; const double scale) const;
// default empty betweenTranslations // 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 gtsam::Values run() const; // default scale = 1.0
}; };