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