Loading...
Searching...
No Matches
TranslationRecovery.h
Go to the documentation of this file.
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010-2020, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
#include <gtsam/geometry/Unit3.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/sfm/BinaryMeasurement.h>
#include <map>
#include <set>
#include <utility>
#include <vector>
namespace gtsam {
// Set up an optimization problem for the unknown translations Ti in the world
// coordinate frame, given the known camera attitudes wRi with respect to the
// world frame, and a set of (noisy) translation directions of type Unit3,
// w_aZb. The measurement equation is
// w_aZb = Unit3(Tb - Ta) (1)
// i.e., w_aZb is the translation direction from frame A to B, in world
// coordinates. Although Unit3 instances live on a manifold, following
// Wilson14eccv_1DSfM.pdf error we compute the *chordal distance* in the
// ambient world coordinate frame.
//
// It is clear that we cannot recover the scale, nor the absolute position,
// so the gauge freedom in this case is 3 + 1 = 4. We fix these by taking fixing
// the translations Ta and Tb associated with the first measurement w_aZb,
// clamping them to their initial values as given to this method. If no initial
// values are given, we use the origin for Tb and set Tb to make (1) come
// through, i.e.,
// Tb = s * wRa * Point3(w_aZb) (2)
// where s is an arbitrary scale that can be supplied, default 1.0. Hence, two
// versions are supplied below corresponding to whether we have initial values
// or not.
class TranslationRecovery {
public:
using KeyPair = std::pair<Key, Key>;
using TranslationEdges = std::vector<BinaryMeasurement<Unit3>>;
private:
// Translation directions between camera pairs.
TranslationEdges relativeTranslations_;
// Parameters.
LevenbergMarquardtParams lmParams_;
public:
TranslationRecovery(const LevenbergMarquardtParams &lmParams)
: lmParams_(lmParams) {}
TranslationRecovery() = default;
NonlinearFactorGraph buildGraph(
const std::vector<BinaryMeasurement<Unit3>> &relativeTranslations) const;
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;
Values initializeRandomly(
const std::vector<BinaryMeasurement<Unit3>> &relativeTranslations,
const std::vector<BinaryMeasurement<Point3>> &betweenTranslations,
std::mt19937 *rng, const Values &initialValues = Values()) const;
Values initializeRandomly(
const std::vector<BinaryMeasurement<Unit3>> &relativeTranslations,
const std::vector<BinaryMeasurement<Point3>> &betweenTranslations,
const Values &initialValues = Values()) const;
Values run(
const TranslationEdges &relativeTranslations, const double scale = 1.0,
const std::vector<BinaryMeasurement<Point3>> &betweenTranslations = {},
const Values &initialValues = Values()) const;
static TranslationEdges SimulateMeasurements(
const Values &poses, const std::vector<KeyPair> &edges);
};
} // namespace gtsam
LevenbergMarquardtOptimizer.h
A nonlinear optimizer that uses the Levenberg-Marquardt trust-region scheme.
Values.h
A non-templated config holding any types of Manifold-group elements.
BinaryMeasurement.h
Binary measurement represents a measurement between two keys in a graph. A binary measurement is simi...
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
gtsam::SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
Aliases.
Definition NoiseModel.h:724
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:597
gtsam::LevenbergMarquardtParams
Parameters for Levenberg-Marquardt optimization.
Definition LevenbergMarquardtParams.h:35
gtsam::NonlinearFactorGraph
Definition NonlinearFactorGraph.h:55
gtsam::Values
A non-templated config holding any types of Manifold-group elements.
Definition Values.h:65
gtsam::BinaryMeasurement
Definition BinaryMeasurement.h:36
gtsam::TranslationRecovery::buildGraph
NonlinearFactorGraph buildGraph(const std::vector< BinaryMeasurement< Unit3 > > &relativeTranslations) const
Build the factor graph to do the optimization.
Definition TranslationRecovery.cpp:99
gtsam::TranslationRecovery::SimulateMeasurements
static TranslationEdges SimulateMeasurements(const Values &poses, const std::vector< KeyPair > &edges)
Simulate translation direction measurements.
Definition TranslationRecovery.cpp:216
gtsam::TranslationRecovery::TranslationRecovery
TranslationRecovery()=default
Default constructor.
gtsam::TranslationRecovery::run
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
gtsam::TranslationRecovery::addPrior
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
gtsam::TranslationRecovery::TranslationRecovery
TranslationRecovery(const LevenbergMarquardtParams &lmParams)
Construct a new Translation Recovery object.
Definition TranslationRecovery.h:69
gtsam::TranslationRecovery::initializeRandomly
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