gtsam 4.1.1 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
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 <map>
20#include <set>
21#include <utility>
22#include <vector>
23
24#include <gtsam/geometry/Unit3.h>
25
28
29namespace gtsam {
30
31// Set up an optimization problem for the unknown translations Ti in the world
32// coordinate frame, given the known camera attitudes wRi with respect to the
33// world frame, and a set of (noisy) translation directions of type Unit3,
34// w_aZb. The measurement equation is
35// w_aZb = Unit3(Tb - Ta) (1)
36// i.e., w_aZb is the translation direction from frame A to B, in world
37// coordinates. Although Unit3 instances live on a manifold, following
38// Wilson14eccv_1DSfM.pdf error we compute the *chordal distance* in the
39// ambient world coordinate frame.
40//
41// It is clear that we cannot recover the scale, nor the absolute position,
42// so the gauge freedom in this case is 3 + 1 = 4. We fix these by taking fixing
43// the translations Ta and Tb associated with the first measurement w_aZb,
44// clamping them to their initial values as given to this method. If no initial
45// values are given, we use the origin for Tb and set Tb to make (1) come
46// through, i.e.,
47// Tb = s * wRa * Point3(w_aZb) (2)
48// where s is an arbitrary scale that can be supplied, default 1.0. Hence, two
49// versions are supplied below corresponding to whether we have initial values
50// or not.
52 public:
53 using KeyPair = std::pair<Key, Key>;
54 using TranslationEdges = std::vector<BinaryMeasurement<Unit3>>;
55
56 private:
57 // Translation directions between camera pairs.
58 TranslationEdges relativeTranslations_;
59
60 // Parameters used by the LM Optimizer.
62
63 // Map from a key in the graph to a set of keys that share the same
64 // translation.
65 std::map<Key, std::set<Key>> sameTranslationNodes_;
66
67 public:
79 const TranslationEdges &relativeTranslations,
81
88
96 void addPrior(const double scale, NonlinearFactorGraph *graph,
97 const SharedNoiseModel &priorNoiseModel =
98 noiseModel::Isotropic::Sigma(3, 0.01)) const;
99
106
113 Values run(const double scale = 1.0) const;
114
124 static TranslationEdges SimulateMeasurements(
125 const Values &poses, const std::vector<KeyPair> &edges);
126};
127} // namespace gtsam
A non-templated config holding any types of Manifold-group elements.
A nonlinear optimizer that uses the Levenberg-Marquardt trust-region scheme.
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
Note, deliberately not in noiseModel namespace.
Definition: NoiseModel.h:736
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
Parameters for Levenberg-Marquardt optimization.
Definition: LevenbergMarquardtParams.h:35
A non-linear factor graph is a graph of non-Gaussian, i.e.
Definition: NonlinearFactorGraph.h:78
A non-templated config holding any types of Manifold-group elements.
Definition: Values.h:63
Definition: TranslationRecovery.h:51
NonlinearFactorGraph buildGraph() const
Build the factor graph to do the optimization.
Definition: TranslationRecovery.cpp:68
static TranslationEdges SimulateMeasurements(const Values &poses, const std::vector< KeyPair > &edges)
Simulate translation direction measurements.
Definition: TranslationRecovery.cpp:140
Values initalizeRandomly() const
Create random initial translations.
Definition: TranslationRecovery.cpp:91
Values run(const double scale=1.0) const
Build and optimize factor graph.
Definition: TranslationRecovery.cpp:118
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:80
TranslationRecovery(const TranslationEdges &relativeTranslations, const LevenbergMarquardtParams &lmParams=LevenbergMarquardtParams())
Construct a new Translation Recovery object.
Definition: TranslationRecovery.cpp:38