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>
@@ -38,16 +41,13 @@ using namespace std;
 // In Wrappers we have no access to this so have a default ready.
 static std::mt19937 kRandomNumberGenerator(42);
 
-TranslationRecovery::TranslationRecovery(
-    const TranslationRecovery::TranslationEdges &relativeTranslations,
-    const LevenbergMarquardtParams &lmParams)
-    : params_(lmParams) {
-  // Some relative translations may be zero. We treat nodes that have a zero
-  // relativeTranslation as a single node.
-
-  // A DSFMap is used to find sets of nodes that have a zero relative
-  // translation. Add the nodes in each edge to the DSFMap, and merge nodes that
-  // are connected by a zero relative translation.
+// Some relative translations may be zero. We treat nodes that have a zero
+// relativeTranslation as a single node.
+// A DSFMap is used to find sets of nodes that have a zero relative
+// translation. Add the nodes in each edge to the DSFMap, and merge nodes that
+// are connected by a zero relative translation.
+DSFMap<Key> getSameTranslationDSFMap(
+    const std::vector<BinaryMeasurement<Unit3>> &relativeTranslations) {
   DSFMap<Key> sameTranslationDSF;
   for (const auto &edge : relativeTranslations) {
     Key key1 = sameTranslationDSF.find(edge.key1());
@@ -56,94 +56,152 @@ TranslationRecovery::TranslationRecovery(
       sameTranslationDSF.merge(key1, key2);
     }
   }
-  // Use only those edges for which two keys have a distinct root in the DSFMap.
-  for (const auto &edge : relativeTranslations) {
-    Key key1 = sameTranslationDSF.find(edge.key1());
-    Key key2 = sameTranslationDSF.find(edge.key2());
+  return sameTranslationDSF;
+}
+
+// Removes zero-translation edges from measurements, and combines the nodes in
+// these edges into a single node.
+template <typename T>
+std::vector<BinaryMeasurement<T>> removeSameTranslationNodes(
+    const std::vector<BinaryMeasurement<T>> &edges,
+    const DSFMap<Key> &sameTranslationDSFMap) {
+  std::vector<BinaryMeasurement<T>> newEdges;
+  for (const auto &edge : edges) {
+    Key key1 = sameTranslationDSFMap.find(edge.key1());
+    Key key2 = sameTranslationDSFMap.find(edge.key2());
     if (key1 == key2) continue;
-    relativeTranslations_.emplace_back(key1, key2, edge.measured(),
-                                       edge.noiseModel());
+    newEdges.emplace_back(key1, key2, edge.measured(), edge.noiseModel());
+  }
+  return newEdges;
+}
+
+// Adds nodes that were not optimized for because they were connected
+// to another node with a zero-translation edge in the input.
+Values addSameTranslationNodes(const Values &result,
+                               const DSFMap<Key> &sameTranslationDSFMap) {
+  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.
+  for (const auto &optimizedAndDuplicateKeys : sameTranslationDSFMap.sets()) {
+    Key optimizedKey = optimizedAndDuplicateKeys.first;
+    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 (final_result.exists(duplicateKey)) continue;
+      final_result.insert<Point3>(duplicateKey,
+                                  final_result.at<Point3>(optimizedKey));
+    }
   }
-  // Store the DSF map for post-processing results.
-  sameTranslationNodes_ = sameTranslationDSF.sets();
+  return final_result;
 }
 
-NonlinearFactorGraph TranslationRecovery::buildGraph() const {
+NonlinearFactorGraph TranslationRecovery::buildGraph(
+    const std::vector<BinaryMeasurement<Unit3>> &relativeTranslations) const {
   NonlinearFactorGraph graph;
 
-  // Add all relative translation edges
-  for (auto edge : relativeTranslations_) {
+  // 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 double scale, NonlinearFactorGraph *graph,
+    const std::vector<BinaryMeasurement<Unit3>> &relativeTranslations,
+    const double scale,
+    const std::vector<BinaryMeasurement<Point3>> &betweenTranslations,
+    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());
+  auto edge = relativeTranslations.begin();
+  if (edge == relativeTranslations.end()) return;
+  graph->emplace_shared<PriorFactor<Point3>>(edge->key1(), Point3(0, 0, 0),
+                                             priorNoiseModel);
+
+  // Add between factors for optional relative translations.
+  for (auto edge : betweenTranslations) {
+    graph->emplace_shared<BetweenFactor<Point3>>(
+        edge.key1(), edge.key2(), 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());
+  }
 }
 
-Values TranslationRecovery::initializeRandomly(std::mt19937 *rng) const {
+Values TranslationRecovery::initializeRandomly(
+    const std::vector<BinaryMeasurement<Unit3>> &relativeTranslations,
+    std::mt19937 *rng, const Values &initialValues) const {
   uniform_real_distribution<double> randomVal(-1, 1);
   // Create a lambda expression that checks whether value exists and randomly
   // initializes if not.
   Values initial;
   auto insert = [&](Key j) {
-    if (!initial.exists(j)) {
+    if (initial.exists(j)) return;
+    if (initialValues.exists(j)) {
+      initial.insert<Point3>(j, initialValues.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
-  for (auto edge : relativeTranslations_) {
+  for (auto edge : relativeTranslations) {
     insert(edge.key1());
     insert(edge.key2());
   }
+  return initial;
+}
+
+Values TranslationRecovery::initializeRandomly(
+    const std::vector<BinaryMeasurement<Unit3>> &relativeTranslations,
+    const Values &initialValues) const {
+  return initializeRandomly(relativeTranslations, &kRandomNumberGenerator,
+                            initialValues);
+}
+
+Values TranslationRecovery::run(
+    const TranslationEdges &relativeTranslations, const double scale,
+    const std::vector<BinaryMeasurement<Point3>> &betweenTranslations,
+    const Values &initialValues) const {
+  // Find edges that have a zero-translation, and recompute relativeTranslations
+  // and betweenTranslations by retaining only one node for every zero-edge.
+  DSFMap<Key> sameTranslationDSFMap =
+      getSameTranslationDSFMap(relativeTranslations);
+  const TranslationEdges nonzeroRelativeTranslations =
+      removeSameTranslationNodes(relativeTranslations, sameTranslationDSFMap);
+  const std::vector<BinaryMeasurement<Point3>> nonzeroBetweenTranslations =
+      removeSameTranslationNodes(betweenTranslations, sameTranslationDSFMap);
+
+  // Create graph of translation factors.
+  NonlinearFactorGraph graph = buildGraph(nonzeroRelativeTranslations);
+
+  // Add global frame prior and scale (either from betweenTranslations or
+  // scale).
+  addPrior(nonzeroRelativeTranslations, scale, nonzeroBetweenTranslations,
+           &graph);
+
+  // Uses initial values from params if provided.
+  Values initial =
+      initializeRandomly(nonzeroRelativeTranslations, initialValues);
 
   // If there are no valid edges, but zero-distance edges exist, initialize one
   // of the nodes in a connected component of zero-distance edges.
-  if (initial.empty() && !sameTranslationNodes_.empty()) {
-    for (const auto &optimizedAndDuplicateKeys : sameTranslationNodes_) {
+  if (initial.empty() && !sameTranslationDSFMap.sets().empty()) {
+    for (const auto &optimizedAndDuplicateKeys : sameTranslationDSFMap.sets()) {
       Key optimizedKey = optimizedAndDuplicateKeys.first;
       initial.insert<Point3>(optimizedKey, Point3(0, 0, 0));
     }
   }
-  return initial;
-}
-
-Values TranslationRecovery::initializeRandomly() const {
-  return initializeRandomly(&kRandomNumberGenerator);
-}
 
-Values TranslationRecovery::run(const double scale) const {
-  auto graph = buildGraph();
-  addPrior(scale, &graph);
-  const Values initial = initializeRandomly();
-  LevenbergMarquardtOptimizer lm(graph, initial, params_);
+  LevenbergMarquardtOptimizer lm(graph, initial, lmParams_);
   Values result = lm.optimize();
-
-  // 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.
-  for (const auto &optimizedAndDuplicateKeys : sameTranslationNodes_) {
-    Key optimizedKey = optimizedAndDuplicateKeys.first;
-    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));
-    }
-  }
-  return result;
+  return addSameTranslationNodes(result, sameTranslationDSFMap);
 }
 
 TranslationRecovery::TranslationEdges TranslationRecovery::SimulateMeasurements(

gtsam/sfm/TranslationRecovery.h

@@ -11,7 +11,7 @@
 
 /**
  * @file TranslationRecovery.h
- * @author Frank Dellaert
+ * @author Frank Dellaert, Akshay Krishnan
  * @date March 2020
  * @brief Recovering translations in an epipolar graph when rotations are given.
  */
@@ -57,68 +57,99 @@ class TranslationRecovery {
   // Translation directions between camera pairs.
   TranslationEdges relativeTranslations_;
 
-  // Parameters used by the LM Optimizer.
-  LevenbergMarquardtParams params_;
-
-  // Map from a key in the graph to a set of keys that share the same
-  // translation.
-  std::map<Key, std::set<Key>> sameTranslationNodes_;
+  // Parameters.
+  LevenbergMarquardtParams lmParams_;
 
  public:
   /**
    * @brief Construct a new Translation Recovery object
    *
-   * @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 lmParams parameters for optimization.
+   */
+  TranslationRecovery(const LevenbergMarquardtParams &lmParams)
+      : lmParams_(lmParams) {}
+
+  /**
+   * @brief Default constructor.
    */
-  TranslationRecovery(
-      const TranslationEdges &relativeTranslations,
-      const LevenbergMarquardtParams &lmParams = LevenbergMarquardtParams());
+  TranslationRecovery() = default;
 
   /**
    * @brief Build the factor graph to do the optimization.
    *
+   * @param relativeTranslations unit translation directions between
+   * translations to be estimated
    * @return NonlinearFactorGraph
    */
-  NonlinearFactorGraph buildGraph() const;
+  NonlinearFactorGraph buildGraph(
+      const std::vector<BinaryMeasurement<Unit3>> &relativeTranslations) const;
 
   /**
-   * @brief Add priors on ednpoints of first measurement edge.
+   * @brief Add 3 factors to the graph:
+   *    - A prior on the first point to lie at (0, 0, 0)
+   *    - If betweenTranslations is non-empty, between factors provided by it.
+   *    - If betweenTranslations is empty, a prior on scale of the first
+   * relativeTranslations edge.
    *
+   * @param relativeTranslations unit translation directions between
+   * translations to be estimated
    * @param scale scale for first relative translation which fixes gauge.
    * @param graph factor graph to which prior is added.
+   * @param betweenTranslations relative translations (with scale) between 2
+   * points in world coordinate frame known a priori.
    * @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<Unit3>> &relativeTranslations,
+      const double scale,
+      const std::vector<BinaryMeasurement<Point3>> &betweenTranslations,
+      NonlinearFactorGraph *graph,
+      const SharedNoiseModel &priorNoiseModel =
+          noiseModel::Isotropic::Sigma(3, 0.01)) const;
 
   /**
    * @brief Create random initial translations.
    *
+   * @param relativeTranslations unit translation directions between
+   * translations to be estimated
    * @param rng random number generator
+   * @param intialValues (optional) initial values from a prior
    * @return Values
    */
-  Values initializeRandomly(std::mt19937 *rng) const;
+  Values initializeRandomly(
+      const std::vector<BinaryMeasurement<Unit3>> &relativeTranslations,
+      std::mt19937 *rng, const Values &initialValues = Values()) const;
 
   /**
    * @brief Version of initializeRandomly with a fixed seed.
    *
+   * @param relativeTranslations unit translation directions between
+   * translations to be estimated
+   * @param initialValues (optional) initial values from a prior
    * @return Values
    */
-  Values initializeRandomly() const;
+  Values initializeRandomly(
+      const std::vector<BinaryMeasurement<Unit3>> &relativeTranslations,
+      const Values &initialValues = Values()) const;
 
   /**
    * @brief Build and optimize factor graph.
    *
+   * @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 scale scale for first relative translation which fixes gauge.
+   * The scale is only used if betweenTranslations is empty.
+   * @param betweenTranslations relative translations (with scale) between 2
+   * points in world coordinate frame known a priori.
+   * @param initialValues intial values for optimization. Initializes randomly
+   * if not provided.
    * @return Values
    */
-  Values run(const double scale = 1.0) const;
+  Values run(
+      const TranslationEdges &relativeTranslations, const double scale = 1.0,
+      const std::vector<BinaryMeasurement<Point3>> &betweenTranslations = {},
+      const Values &initialValues = Values()) const;
 
   /**
    * @brief Simulate translation direction measurements