Adding options to translation averaging to support betweenfactor translation priors

gtsam/sfm/TranslationRecovery.cpp

@@ -21,13 +21,16 @@
 #include <gtsam/geometry/Pose3.h>
 #include <gtsam/geometry/Unit3.h>
 #include <gtsam/linear/NoiseModel.h>
+#include <gtsam/nonlinear/ExpressionFactor.h>
 #include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
 #include <gtsam/nonlinear/NonlinearFactor.h>
 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
 #include <gtsam/nonlinear/Values.h>
 #include <gtsam/sfm/TranslationFactor.h>
 #include <gtsam/sfm/TranslationRecovery.h>
+#include <gtsam/slam/BetweenFactor.h>
 #include <gtsam/slam/PriorFactor.h>
+#include <gtsam/slam/expressions.h>
 
 #include <set>
 #include <utility>
@@ -40,8 +43,8 @@ static std::mt19937 kRandomNumberGenerator(42);
 
 TranslationRecovery::TranslationRecovery(
     const TranslationRecovery::TranslationEdges &relativeTranslations,
-    const LevenbergMarquardtParams &lmParams)
-    : params_(lmParams) {
+    const TranslationRecoveryParams &params)
+    : params_(params) {
   // Some relative translations may be zero. We treat nodes that have a zero
   // relativeTranslation as a single node.
 
@@ -71,24 +74,48 @@ TranslationRecovery::TranslationRecovery(
 NonlinearFactorGraph TranslationRecovery::buildGraph() const {
   NonlinearFactorGraph graph;
 
-  // Add all relative translation edges
+  // Add translation factors for input translation directions.
   for (auto edge : relativeTranslations_) {
     graph.emplace_shared<TranslationFactor>(edge.key1(), edge.key2(),
                                             edge.measured(), edge.noiseModel());
   }
-
   return graph;
 }
 
 void TranslationRecovery::addPrior(
+    const std::vector<BinaryMeasurement<Point3>> &betweenTranslations,
     const double scale, NonlinearFactorGraph *graph,
     const SharedNoiseModel &priorNoiseModel) const {
   auto edge = relativeTranslations_.begin();
   if (edge == relativeTranslations_.end()) return;
-  graph->emplace_shared<PriorFactor<Point3> >(edge->key1(), Point3(0, 0, 0),
-                                              priorNoiseModel);
-  graph->emplace_shared<PriorFactor<Point3> >(
-      edge->key2(), scale * edge->measured().point3(), edge->noiseModel());
+  graph->emplace_shared<PriorFactor<Point3>>(edge->key1(), Point3(0, 0, 0),
+                                             priorNoiseModel);
+
+  // Add between factors for optional relative translations.
+  for (auto edge : betweenTranslations) {
+    Key k1 = getSameTranslationRootNode(edge.key1()),
+        k2 = getSameTranslationRootNode(edge.key2());
+    if (k1 != k2) {
+      graph->emplace_shared<BetweenFactor<Point3>>(k1, k2, edge.measured(),
+                                                   edge.noiseModel());
+    }
+  }
+
+  // Add a scale prior only if no other between factors were added.
+  if (betweenTranslations.empty()) {
+    graph->emplace_shared<PriorFactor<Point3>>(
+        edge->key2(), scale * edge->measured().point3(), edge->noiseModel());
+  }
+}
+
+Key TranslationRecovery::getSameTranslationRootNode(const Key i) const {
+  for (const auto &optimizedAndDuplicateKeys : sameTranslationNodes_) {
+    Key optimizedKey = optimizedAndDuplicateKeys.first;
+    std::set<Key> duplicateKeys = optimizedAndDuplicateKeys.second;
+    if (i == optimizedKey || duplicateKeys.count(i)) return optimizedKey;
+  }
+  // Unlikely case, when i is not in the graph.
+  return i;
 }
 
 Values TranslationRecovery::initializeRandomly(std::mt19937 *rng) const {
@@ -97,10 +124,14 @@ Values TranslationRecovery::initializeRandomly(std::mt19937 *rng) const {
   // initializes if not.
   Values initial;
   auto insert = [&](Key j) {
-    if (!initial.exists(j)) {
+    if (initial.exists(j)) return;
+    if (params_.initial.exists(j)) {
+      initial.insert<Point3>(j, params_.initial.at<Point3>(j));
+    } else {
       initial.insert<Point3>(
           j, Point3(randomVal(*rng), randomVal(*rng), randomVal(*rng)));
     }
+    // Assumes all nodes connected by zero-edges have the same initialization.
   };
 
   // Loop over measurements and add a random translation
@@ -124,13 +155,20 @@ Values TranslationRecovery::initializeRandomly() const {
   return initializeRandomly(&kRandomNumberGenerator);
 }
 
-Values TranslationRecovery::run(const double scale) const {
-  auto graph = buildGraph();
-  addPrior(scale, &graph);
+Values TranslationRecovery::run(
+    const std::vector<BinaryMeasurement<Point3>> &betweenTranslations,
+    const double scale) const {
+  NonlinearFactorGraph graph = buildGraph();
+  addPrior(betweenTranslations, scale, &graph);
   const Values initial = initializeRandomly();
-  LevenbergMarquardtOptimizer lm(graph, initial, params_);
+  LevenbergMarquardtOptimizer lm(graph, initial, params_.lmParams);
   Values result = lm.optimize();
+  return addSameTranslationNodes(result);
+}
 
+Values TranslationRecovery::addSameTranslationNodes(
+    const Values &result) const {
+  Values final_result = result;
   // Nodes that were not optimized are stored in sameTranslationNodes_ as a map
   // from a key that was optimized to keys that were not optimized. Iterate over
   // map and add results for keys not optimized.
@@ -139,11 +177,12 @@ Values TranslationRecovery::run(const double scale) const {
     std::set<Key> duplicateKeys = optimizedAndDuplicateKeys.second;
     // Add the result for the duplicate key if it does not already exist.
     for (const Key duplicateKey : duplicateKeys) {
-      if (result.exists(duplicateKey)) continue;
-      result.insert<Point3>(duplicateKey, result.at<Point3>(optimizedKey));
+      if (final_result.exists(duplicateKey)) continue;
+      final_result.insert<Point3>(duplicateKey,
+                                  final_result.at<Point3>(optimizedKey));
     }
   }
-  return result;
+  return final_result;
 }
 
 TranslationRecovery::TranslationEdges TranslationRecovery::SimulateMeasurements(

gtsam/sfm/TranslationRecovery.h

@@ -28,6 +28,15 @@
 
 namespace gtsam {
 
+// Parameters for the Translation Recovery problem.
+struct TranslationRecoveryParams {
+  // LevenbergMarquardtParams for optimization.
+  LevenbergMarquardtParams lmParams;
+
+  // Initial values, random intialization will be used if not provided.
+  Values initial;
+};
+
 // 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,
@@ -57,8 +66,8 @@ class TranslationRecovery {
   // Translation directions between camera pairs.
   TranslationEdges relativeTranslations_;
 
-  // Parameters used by the LM Optimizer.
-  LevenbergMarquardtParams params_;
+  // Parameters.
+  TranslationRecoveryParams params_;
 
   // Map from a key in the graph to a set of keys that share the same
   // translation.
@@ -71,13 +80,11 @@ class TranslationRecovery {
    * @param relativeTranslations the relative translations, in world coordinate
    * frames, vector of BinaryMeasurements of Unit3, where each key of a
    * measurement is a point in 3D.
-   * @param lmParams (optional) gtsam::LavenbergMarquardtParams that can be
-   * used to modify the parameters for the LM optimizer. By default, uses the
-   * default LM parameters.
+   * @param params (optional) parameters for the recovery problem.
    */
   TranslationRecovery(
       const TranslationEdges &relativeTranslations,
-      const LevenbergMarquardtParams &lmParams = LevenbergMarquardtParams());
+      const TranslationRecoveryParams &params = TranslationRecoveryParams());
 
   /**
    * @brief Build the factor graph to do the optimization.
@@ -93,9 +100,11 @@ class TranslationRecovery {
    * @param graph factor graph to which prior is added.
    * @param priorNoiseModel the noise model to use with the prior.
    */
-  void addPrior(const double scale, NonlinearFactorGraph *graph,
-                const SharedNoiseModel &priorNoiseModel =
-                    noiseModel::Isotropic::Sigma(3, 0.01)) const;
+  void addPrior(
+      const std::vector<BinaryMeasurement<Point3>> &betweenTranslations,
+      const double scale, NonlinearFactorGraph *graph,
+      const SharedNoiseModel &priorNoiseModel =
+          noiseModel::Isotropic::Sigma(3, 0.01)) const;
 
   /**
    * @brief Create random initial translations.
@@ -116,9 +125,12 @@ class TranslationRecovery {
    * @brief Build and optimize factor graph.
    *
    * @param scale scale for first relative translation which fixes gauge.
+   * The scale is only used if relativeTranslations in the params is empty.
    * @return Values
    */
-  Values run(const double scale = 1.0) const;
+  Values run(
+      const std::vector<BinaryMeasurement<Point3>> &betweenTranslations = {},
+      const double scale = 1.0) const;
 
   /**
    * @brief Simulate translation direction measurements
@@ -131,5 +143,25 @@ class TranslationRecovery {
    */
   static TranslationEdges SimulateMeasurements(
       const Values &poses, const std::vector<KeyPair> &edges);
+
+ private:
+  /**
+   * @brief Gets the key of the variable being optimized among multiple input
+   * variables that have the same translation.
+   *
+   * @param i key of input variable.
+   * @return Key of optimized variable - same as input if it does not have any
+   * zero-translation edges.
+   */
+  Key getSameTranslationRootNode(const Key i) const;
+
+  /**
+   * @brief Adds nodes that were not optimized for because they were connected
+   * to another node with a zero-translation edge in the input.
+   *
+   * @param result optimization problem result
+   * @return translation estimates for all variables in the input.
+   */
+  Values addSameTranslationNodes(const Values &result) const;
 };
 }  // namespace gtsam

gtsam/sfm/sfm.i

@@ -4,6 +4,8 @@
 
 namespace gtsam {
 
+#include <gtsam/nonlinear/NonlinearFactorGraph.h>
+#include <gtsam/nonlinear/Values.h>
 #include <gtsam/sfm/SfmTrack.h>
 class SfmTrack {
   SfmTrack();
@@ -83,6 +85,7 @@ class BinaryMeasurement {
 
 typedef gtsam::BinaryMeasurement<gtsam::Unit3> BinaryMeasurementUnit3;
 typedef gtsam::BinaryMeasurement<gtsam::Rot3> BinaryMeasurementRot3;
+typedef gtsam::BinaryMeasurement<gtsam::Point3> BinaryMeasurementPoint3;
 
 class BinaryMeasurementsUnit3 {
   BinaryMeasurementsUnit3();
@@ -91,6 +94,13 @@ class BinaryMeasurementsUnit3 {
   void push_back(const gtsam::BinaryMeasurement<gtsam::Unit3>& measurement);
 };
 
+class BinaryMeasurementsPoint3 {
+  BinaryMeasurementsPoint3();
+  size_t size() const;
+  gtsam::BinaryMeasurement<gtsam::Point3> at(size_t idx) const;
+  void push_back(const gtsam::BinaryMeasurement<gtsam::Point3>& measurement);
+};
+
 #include <gtsam/sfm/ShonanAveraging.h>
 
 // TODO(frank): copy/pasta below until we have integer template paremeters in
@@ -142,8 +152,8 @@ class ShonanAveraging2 {
   ShonanAveraging2(string g2oFile);
   ShonanAveraging2(string g2oFile,
                    const gtsam::ShonanAveragingParameters2& parameters);
-  ShonanAveraging2(const gtsam::BetweenFactorPose2s &factors,
-                   const gtsam::ShonanAveragingParameters2 &parameters);
+  ShonanAveraging2(const gtsam::BetweenFactorPose2s& factors,
+                   const gtsam::ShonanAveragingParameters2& parameters);
 
   // Query properties
   size_t nrUnknowns() const;
@@ -252,14 +262,21 @@ class MFAS {
 };
 
 #include <gtsam/sfm/TranslationRecovery.h>
+class TranslationRecoveryParams {
+  gtsam::LevenbergMarquardtParams lmParams;
+  gtsam::Values initial;
+};
+
 class TranslationRecovery {
   TranslationRecovery(
       const gtsam::BinaryMeasurementsUnit3& relativeTranslations,
-      const gtsam::LevenbergMarquardtParams& lmParams);
-  TranslationRecovery(
-      const gtsam::BinaryMeasurementsUnit3&
-          relativeTranslations);  // default LevenbergMarquardtParams
-  gtsam::Values run(const double scale) const;
+      const gtsam::TranslationRecoveryParams& lmParams);
+  TranslationRecovery(const gtsam::BinaryMeasurementsUnit3&
+                          relativeTranslations);  // default params
+  gtsam::Values run(const gtsam::BinaryMeasurementsPoint3& betweenTranslations,
+                    const double scale) const;
+  // default empty betweenTranslations
+  // gtsam::Values run(const double scale) const;
   gtsam::Values run() const;  // default scale = 1.0
 };
 

gtsam/slam/dataset.h

@@ -223,6 +223,7 @@ parse3DFactors(const std::string &filename,
                size_t maxIndex = 0);
 
 using BinaryMeasurementsUnit3 = std::vector<BinaryMeasurement<Unit3>>;
+using BinaryMeasurementsPoint3 = std::vector<BinaryMeasurement<Point3>>;
 
 #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42
 inline boost::optional<IndexedPose> GTSAM_DEPRECATED

python/CMakeLists.txt

@@ -47,6 +47,7 @@ set(ignore
     gtsam::Pose3Vector
     gtsam::Rot3Vector
     gtsam::KeyVector
+    gtsam::BinaryMeasurementsPoint3
     gtsam::BinaryMeasurementsUnit3
     gtsam::DiscreteKey
     gtsam::KeyPairDoubleMap)
@@ -128,6 +129,7 @@ if(GTSAM_UNSTABLE_BUILD_PYTHON)
             gtsam::Pose3Vector
             gtsam::KeyVector
             gtsam::FixedLagSmootherKeyTimestampMapValue
+            gtsam::BinaryMeasurementsPoint3
             gtsam::BinaryMeasurementsUnit3
             gtsam::CameraSetCal3_S2
             gtsam::CameraSetCal3Bundler

python/gtsam/specializations/sfm.h

@@ -11,6 +11,8 @@
  * and saves one copy operation.
  */
 
+py::bind_vector<std::vector<gtsam::BinaryMeasurement<gtsam::Point3> > >(
+    m_, "BinaryMeasurementsPoint3");
 py::bind_vector<std::vector<gtsam::BinaryMeasurement<gtsam::Unit3> > >(
     m_, "BinaryMeasurementsUnit3");
 py::bind_map<gtsam::KeyPairDoubleMap>(m_, "KeyPairDoubleMap");