19#include <gtsam/geometry/Unit3.h>
53 using KeyPair = std::pair<Key, Key>;
54 using TranslationEdges = std::vector<BinaryMeasurement<Unit3>>;
58 TranslationEdges relativeTranslations_;
70 : lmParams_(lmParams) {}
124 std::mt19937 *rng,
const Values &initialValues =
Values())
const;
160 const TranslationEdges &relativeTranslations,
const double scale = 1.0,
174 const Values &poses,
const std::vector<KeyPair> &edges);
A nonlinear optimizer that uses the Levenberg-Marquardt trust-region scheme.
A non-templated config holding any types of Manifold-group elements.
Binary measurement represents a measurement between two keys in a graph. A binary measurement is simi...
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
noiseModel::Base::shared_ptr SharedNoiseModel
Aliases.
Definition: NoiseModel.h:724
static shared_ptr Sigma(size_t dim, double sigma, bool smart=true)
An isotropic noise model created by specifying a standard devation sigma.
Definition: NoiseModel.cpp:597
Parameters for Levenberg-Marquardt optimization.
Definition: LevenbergMarquardtParams.h:35
Definition: NonlinearFactorGraph.h:55
A non-templated config holding any types of Manifold-group elements.
Definition: Values.h:65
Definition: TranslationRecovery.h:51
NonlinearFactorGraph buildGraph(const std::vector< BinaryMeasurement< Unit3 > > &relativeTranslations) const
Build the factor graph to do the optimization.
Definition: TranslationRecovery.cpp:99
static TranslationEdges SimulateMeasurements(const Values &poses, const std::vector< KeyPair > &edges)
Simulate translation direction measurements.
Definition: TranslationRecovery.cpp:216
TranslationRecovery()=default
Default constructor.
Values run(const TranslationEdges &relativeTranslations, const double scale=1.0, const std::vector< BinaryMeasurement< Point3 > > &betweenTranslations={}, const Values &initialValues=Values()) const
Build and optimize factor graph.
Definition: TranslationRecovery.cpp:177
void addPrior(const std::vector< BinaryMeasurement< Unit3 > > &relativeTranslations, const double scale, const std::vector< BinaryMeasurement< Point3 > > &betweenTranslations, NonlinearFactorGraph *graph, const SharedNoiseModel &priorNoiseModel=noiseModel::Isotropic::Sigma(3, 0.01)) const
Add 3 factors to the graph:
Definition: TranslationRecovery.cpp:111
TranslationRecovery(const LevenbergMarquardtParams &lmParams)
Construct a new Translation Recovery object.
Definition: TranslationRecovery.h:69
Values initializeRandomly(const std::vector< BinaryMeasurement< Unit3 > > &relativeTranslations, const std::vector< BinaryMeasurement< Point3 > > &betweenTranslations, std::mt19937 *rng, const Values &initialValues=Values()) const
Create random initial translations.
Definition: TranslationRecovery.cpp:137