gtsam  4.1.0
gtsam
TranslationRecovery.h
Go to the documentation of this file.
1 /* ----------------------------------------------------------------------------
2 
3  * GTSAM Copyright 2010-2020, Georgia Tech Research Corporation,
4  * Atlanta, Georgia 30332-0415
5  * All Rights Reserved
6  * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
7 
8  * See LICENSE for the license information
9 
10  * -------------------------------------------------------------------------- */
11 
19 #include <gtsam/geometry/Unit3.h>
21 #include <gtsam/nonlinear/Values.h>
23 
24 #include <utility>
25 #include <vector>
26 
27 namespace gtsam {
28 
29 // Set up an optimization problem for the unknown translations Ti in the world
30 // coordinate frame, given the known camera attitudes wRi with respect to the
31 // world frame, and a set of (noisy) translation directions of type Unit3,
32 // w_aZb. The measurement equation is
33 // w_aZb = Unit3(Tb - Ta) (1)
34 // i.e., w_aZb is the translation direction from frame A to B, in world
35 // coordinates. Although Unit3 instances live on a manifold, following
36 // Wilson14eccv_1DSfM.pdf error we compute the *chordal distance* in the
37 // ambient world coordinate frame.
38 //
39 // It is clear that we cannot recover the scale, nor the absolute position,
40 // so the gauge freedom in this case is 3 + 1 = 4. We fix these by taking fixing
41 // the translations Ta and Tb associated with the first measurement w_aZb,
42 // clamping them to their initial values as given to this method. If no initial
43 // values are given, we use the origin for Tb and set Tb to make (1) come
44 // through, i.e.,
45 // Tb = s * wRa * Point3(w_aZb) (2)
46 // where s is an arbitrary scale that can be supplied, default 1.0. Hence, two
47 // versions are supplied below corresponding to whether we have initial values
48 // or not.
50  public:
51  using KeyPair = std::pair<Key, Key>;
52  using TranslationEdges = std::vector<BinaryMeasurement<Unit3>>;
53 
54  private:
55  TranslationEdges relativeTranslations_;
57 
58  public:
69  TranslationRecovery(const TranslationEdges &relativeTranslations,
71  : relativeTranslations_(relativeTranslations), params_(lmParams) {}
72 
79 
87  void addPrior(const double scale, NonlinearFactorGraph *graph,
88  const SharedNoiseModel &priorNoiseModel =
89  noiseModel::Isotropic::Sigma(3, 0.01)) const;
90 
96  Values initalizeRandomly() const;
97 
104  Values run(const double scale = 1.0) const;
105 
115  static TranslationEdges SimulateMeasurements(
116  const Values &poses, const std::vector<KeyPair> &edges);
117 };
118 } // namespace gtsam
gtsam::TranslationRecovery
Definition: TranslationRecovery.h:49
gtsam::NonlinearFactorGraph
A non-linear factor graph is a graph of non-Gaussian, i.e.
Definition: NonlinearFactorGraph.h:78
BinaryMeasurement.h
Binary measurement represents a measurement between two keys in a graph. A binary measurement is simi...
gtsam::TranslationRecovery::SimulateMeasurements
static TranslationEdges SimulateMeasurements(const Values &poses, const std::vector< KeyPair > &edges)
Simulate translation direction measurements.
Definition: TranslationRecovery.cpp:83
gtsam::TranslationRecovery::run
Values run(const double scale=1.0) const
Build and optimize factor graph.
Definition: TranslationRecovery.cpp:74
gtsam::SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
Note, deliberately not in noiseModel namespace.
Definition: NoiseModel.h:734
gtsam::TranslationRecovery::initalizeRandomly
Values initalizeRandomly() const
Create random initial translations.
Definition: TranslationRecovery.cpp:56
gtsam
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
gtsam::TranslationRecovery::addPrior
void addPrior(const double scale, NonlinearFactorGraph *graph, const SharedNoiseModel &priorNoiseModel=noiseModel::Isotropic::Sigma(3, 0.01)) const
Add priors on ednpoints of first measurement edge.
Definition: TranslationRecovery.cpp:47
LevenbergMarquardtOptimizer.h
A nonlinear optimizer that uses the Levenberg-Marquardt trust-region scheme.
gtsam::LevenbergMarquardtParams
Parameters for Levenberg-Marquardt optimization.
Definition: LevenbergMarquardtParams.h:33
gtsam::TranslationRecovery::TranslationRecovery
TranslationRecovery(const TranslationEdges &relativeTranslations, const LevenbergMarquardtParams &lmParams=LevenbergMarquardtParams())
Construct a new Translation Recovery object.
Definition: TranslationRecovery.h:69
gtsam::Values
A non-templated config holding any types of Manifold-group elements.
Definition: Values.h:71
Values.h
A non-templated config holding any types of Manifold-group elements.
gtsam::TranslationRecovery::buildGraph
NonlinearFactorGraph buildGraph() const
Build the factor graph to do the optimization.
Definition: TranslationRecovery.cpp:35
gtsam::noiseModel::Isotropic::Sigma
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:567