LocationRecovery.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
 
 * -------------------------------------------------------------------------- */
 
#pragma once

 
#include <gtsam/geometry/Unit3.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/sfm/BinaryMeasurement.h>
 
#include <random>
#include <set>
#include <vector>
 
namespace gtsam {
 
// Recover absolute Point3 locations from pairwise Unit3 direction
// measurements. This base class is unopinionated about graph structure:
// measurements can connect any pair of Point3 unknowns (cameras, landmarks,
// sensors, etc.). Subclasses add problem-specific opinions:
//   - GlobalPositioner: bipartite camera+landmark, anchor-only gauge.
//   - TranslationRecovery: homogeneous camera-camera, DSFMap, two-key gauge.
//
// Supports two cost functions via the bilinear flag:
//   false — TranslationFactor: normalized(Tb-Ta) - measured  (chordal)
//   true  — BilinearAngleTranslationFactor: scale*(Tb-Ta) - measured  (BATA)
class GTSAM_EXPORT LocationRecovery {
 public:
  using DirectionEdges = std::vector<BinaryMeasurement<Unit3>>;
 
 protected:
  LevenbergMarquardtParams lmParams_;

  static SharedNoiseModel convertNoiseModel(
      const SharedNoiseModel &unit3NoiseModel);
 
 public:
  explicit LocationRecovery(const LevenbergMarquardtParams &lmParams)
      : lmParams_(lmParams) {}

  LocationRecovery() = default;

  NonlinearFactorGraph buildGraph(const DirectionEdges &edges,
                                  bool bilinear = true) const;

  void addAnchorPrior(
      Key anchorKey, NonlinearFactorGraph *graph,
      const SharedNoiseModel &priorNoiseModel =
          noiseModel::Isotropic::Sigma(3, 0.01)) const;

  Values initializeRandomly(const std::set<Key> &keys, size_t numEdges,
                             bool bilinear, std::mt19937 *rng,
                             const Values &initialValues = Values()) const;

  Values initializeRandomly(const std::set<Key> &keys, size_t numEdges,
                             bool bilinear,
                             const Values &initialValues = Values()) const;
};
 
}  // namespace gtsam