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(
const std::vector<BinaryMeasurement<Point3>> &betweenTranslations,
const double scale, const boost::shared_ptr<NonlinearFactorGraph> 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<Point3>(j, inputInitial.at<Point3>(j));
if (params_.initial.exists(j)) {
initial.insert<Point3>(j, params_.initial.at<Point3>(j));
} else {
initial.insert<Point3>(
j, Point3(randomVal(*rng), randomVal(*rng), randomVal(*rng)));
@ -158,11 +157,10 @@ Values TranslationRecovery::initializeRandomly() const {
Values TranslationRecovery::run(
const std::vector<BinaryMeasurement<Point3>> &betweenTranslations,
const double scale) const {
boost::shared_ptr<NonlinearFactorGraph> graph_ptr =
boost::make_shared<NonlinearFactorGraph>(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);
}

20
gtsam/sfm/TranslationRecovery.h
View File

@ -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<BinaryMeasurement<Point3>> &betweenTranslations,
const double scale, const boost::shared_ptr<NonlinearFactorGraph> graph,
const double scale, NonlinearFactorGraph *graph,
const SharedNoiseModel &priorNoiseModel =
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>
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
};