Merge pull request #619 from borglab/fix/zero_translation_avg

Handling edges with pure rotation in translation averaging

gtsam/sfm/TranslationRecovery.cpp

@@ -11,13 +11,12 @@
 
 /**
  * @file TranslationRecovery.cpp
- * @author Frank Dellaert
+ * @author Frank Dellaert, Akshay Krishnan
  * @date March 2020
  * @brief Source code for recovering translations when rotations are given
  */
 
-#include <gtsam/sfm/TranslationRecovery.h>
-
+#include <gtsam/base/DSFMap.h>
 #include <gtsam/geometry/Point3.h>
 #include <gtsam/geometry/Pose3.h>
 #include <gtsam/geometry/Unit3.h>
@@ -27,11 +26,45 @@
 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
 #include <gtsam/nonlinear/Values.h>
 #include <gtsam/sfm/TranslationFactor.h>
+#include <gtsam/sfm/TranslationRecovery.h>
 #include <gtsam/slam/PriorFactor.h>
 
+#include <set>
+#include <utility>
+
 using namespace gtsam;
 using namespace std;
 
+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.
+  DSFMap<Key> sameTranslationDSF;
+  for (const auto &edge : relativeTranslations) {
+    Key key1 = sameTranslationDSF.find(edge.key1());
+    Key key2 = sameTranslationDSF.find(edge.key2());
+    if (key1 != key2 && edge.measured().equals(Unit3(0.0, 0.0, 0.0))) {
+      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());
+    if (key1 == key2) continue;
+    relativeTranslations_.emplace_back(key1, key2, edge.measured(),
+                                       edge.noiseModel());
+  }
+  // Store the DSF map for post-processing results.
+  sameTranslationNodes_ = sameTranslationDSF.sets();
+}
+
 NonlinearFactorGraph TranslationRecovery::buildGraph() const {
   NonlinearFactorGraph graph;
 
@@ -44,13 +77,14 @@ NonlinearFactorGraph TranslationRecovery::buildGraph() const {
   return graph;
 }
 
-void TranslationRecovery::addPrior(const double scale,
-                                   NonlinearFactorGraph *graph,
-                                   const SharedNoiseModel &priorNoiseModel) const {
+void TranslationRecovery::addPrior(
+    const double scale, NonlinearFactorGraph *graph,
+    const SharedNoiseModel &priorNoiseModel) const {
   auto edge = relativeTranslations_.begin();
-  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);
+  graph->emplace_shared<PriorFactor<Point3> >(
+      edge->key2(), scale * edge->measured().point3(), edge->noiseModel());
 }
 
 Values TranslationRecovery::initalizeRandomly() const {
@@ -77,6 +111,19 @@ Values TranslationRecovery::run(const double scale) const {
   const Values initial = initalizeRandomly();
   LevenbergMarquardtOptimizer lm(graph, initial, params_);
   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;
 }
 

gtsam/sfm/TranslationRecovery.h

@@ -16,14 +16,16 @@
  * @brief Recovering translations in an epipolar graph when rotations are given.
  */
 
+#include <map>
+#include <set>
+#include <utility>
+#include <vector>
+
 #include <gtsam/geometry/Unit3.h>
 #include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
 #include <gtsam/nonlinear/Values.h>
 #include <gtsam/sfm/BinaryMeasurement.h>
 
-#include <utility>
-#include <vector>
-
 namespace gtsam {
 
 // Set up an optimization problem for the unknown translations Ti in the world
@@ -52,23 +54,30 @@ class TranslationRecovery {
   using TranslationEdges = std::vector<BinaryMeasurement<Unit3>>;
 
  private:
+  // 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_;
+
  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. 
+   * 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. 
+   * default LM parameters.
    */
-  TranslationRecovery(const TranslationEdges &relativeTranslations,
-                      const LevenbergMarquardtParams &lmParams = LevenbergMarquardtParams())
-      : relativeTranslations_(relativeTranslations), params_(lmParams) {}
+  TranslationRecovery(
+      const TranslationEdges &relativeTranslations,
+      const LevenbergMarquardtParams &lmParams = LevenbergMarquardtParams());
 
   /**
    * @brief Build the factor graph to do the optimization.
@@ -108,8 +117,8 @@ class TranslationRecovery {
    *
    * @param poses SE(3) ground truth poses stored as Values
    * @param edges pairs (a,b) for which a measurement w_aZb will be generated.
-   * @return TranslationEdges vector of binary measurements where the keys are 
-   * the cameras and the measurement is the simulated Unit3 translation 
+   * @return TranslationEdges vector of binary measurements where the keys are
+   * the cameras and the measurement is the simulated Unit3 translation
    * direction between the cameras.
    */
   static TranslationEdges SimulateMeasurements(