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
@@ -54,21 +56,22 @@ class TranslationRecovery {
  private:
   TranslationEdges relativeTranslations_;
   LevenbergMarquardtParams params_;
+  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 +111,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(

tests/testTranslationRecovery.cpp

@@ -11,19 +11,29 @@
 
 /**
  * @file testTranslationRecovery.cpp
- * @author Frank Dellaert
+ * @author Frank Dellaert, Akshay Krishnan
  * @date March 2020
  * @brief test recovering translations when rotations are given.
  */
 
-#include <gtsam/sfm/TranslationRecovery.h>
-
 #include <CppUnitLite/TestHarness.h>
+
+#include <gtsam/sfm/TranslationRecovery.h>
 #include <gtsam/slam/dataset.h>
 
 using namespace std;
 using namespace gtsam;
 
+// Returns the Unit3 direction as measured in the binary measurement, but
+// computed from the input poses. Helper function used in the unit tests.
+Unit3 GetDirectionFromPoses(const Values& poses,
+                            const BinaryMeasurement<Unit3>& unitTranslation) {
+  const Pose3 wTa = poses.at<Pose3>(unitTranslation.key1()),
+              wTb = poses.at<Pose3>(unitTranslation.key2());
+  const Point3 Ta = wTa.translation(), Tb = wTb.translation();
+  return Unit3(Tb - Ta);
+}
+
 /* ************************************************************************* */
 // We read the BAL file, which has 3 cameras in it, with poses. We then assume
 // the rotations are correct, but translations have to be estimated from
@@ -48,43 +58,186 @@ TEST(TranslationRecovery, BAL) {
   const auto relativeTranslations = TranslationRecovery::SimulateMeasurements(
       poses, {{0, 1}, {0, 2}, {1, 2}});
 
-  // Check
-  Unit3 w_aZb_stored; // measurement between 0 and 1 stored for next unit test
-  for(auto& unitTranslation : relativeTranslations) {
-    const Pose3 wTa = poses.at<Pose3>(unitTranslation.key1()), 
-                wTb = poses.at<Pose3>(unitTranslation.key2());
-    const Point3 Ta = wTa.translation(), Tb = wTb.translation();
-    const Unit3 w_aZb = unitTranslation.measured();
-    EXPECT(assert_equal(Unit3(Tb - Ta), w_aZb));
-    if(unitTranslation.key1() == 0 && unitTranslation.key2() == 1) {
-      w_aZb_stored = unitTranslation.measured();
-    }
+  // Check simulated measurements.
+  for (auto& unitTranslation : relativeTranslations) {
+    EXPECT(assert_equal(GetDirectionFromPoses(poses, unitTranslation),
+                        unitTranslation.measured()));
   }
 
   TranslationRecovery algorithm(relativeTranslations);
   const auto graph = algorithm.buildGraph();
   EXPECT_LONGS_EQUAL(3, graph.size());
 
-  // Translation recovery, version 1
+  // Run translation recovery
   const double scale = 2.0;
   const auto result = algorithm.run(scale);
 
   // Check result for first two translations, determined by prior
   EXPECT(assert_equal(Point3(0, 0, 0), result.at<Point3>(0)));
-  EXPECT(assert_equal(Point3(2 * w_aZb_stored.point3()), result.at<Point3>(1)));
+  EXPECT(assert_equal(
+      Point3(2 * GetDirectionFromPoses(poses, relativeTranslations[0])),
+      result.at<Point3>(1)));
 
   // Check that the third translations is correct
   Point3 Ta = poses.at<Pose3>(0).translation();
   Point3 Tb = poses.at<Pose3>(1).translation();
   Point3 Tc = poses.at<Pose3>(2).translation();
-  Point3 expected = 
-      (Tc - Ta) * (scale / (Tb - Ta).norm());
+  Point3 expected = (Tc - Ta) * (scale / (Tb - Ta).norm());
   EXPECT(assert_equal(expected, result.at<Point3>(2), 1e-4));
 
   // TODO(frank): how to get stats back?
   // EXPECT_DOUBLES_EQUAL(0.0199833, actualError, 1e-5);
 }
 
+TEST(TranslationRecovery, TwoPoseTest) {
+  // Create a dataset with 2 poses.
+  // __      __
+  // \/      \/
+  //  0 _____ 1
+  //
+  // 0 and 1 face in the same direction but have a translation offset.
+  Values poses;
+  poses.insert<Pose3>(0, Pose3(Rot3(), Point3(0, 0, 0)));
+  poses.insert<Pose3>(1, Pose3(Rot3(), Point3(2, 0, 0)));
+
+  auto relativeTranslations =
+      TranslationRecovery::SimulateMeasurements(poses, {{0, 1}});
+
+  // Check simulated measurements.
+  for (auto& unitTranslation : relativeTranslations) {
+    EXPECT(assert_equal(GetDirectionFromPoses(poses, unitTranslation),
+                        unitTranslation.measured()));
+  }
+
+  TranslationRecovery algorithm(relativeTranslations);
+  const auto graph = algorithm.buildGraph();
+  EXPECT_LONGS_EQUAL(1, graph.size());
+
+  // Run translation recovery
+  const auto result = algorithm.run(/*scale=*/3.0);
+
+  // Check result for first two translations, determined by prior
+  EXPECT(assert_equal(Point3(0, 0, 0), result.at<Point3>(0)));
+  EXPECT(assert_equal(Point3(3, 0, 0), result.at<Point3>(1)));
+}
+
+TEST(TranslationRecovery, ThreePoseTest) {
+  // Create a dataset with 3 poses.
+  // __      __
+  // \/      \/
+  //  0 _____ 1
+  //    \ __ /
+  //     \\//
+  //       3
+  //
+  // 0 and 1 face in the same direction but have a translation offset. 3 is in
+  // the same direction as 0 and 1, in between 0 and 1, with some Y axis offset.
+
+  Values poses;
+  poses.insert<Pose3>(0, Pose3(Rot3(), Point3(0, 0, 0)));
+  poses.insert<Pose3>(1, Pose3(Rot3(), Point3(2, 0, 0)));
+  poses.insert<Pose3>(3, Pose3(Rot3(), Point3(1, -1, 0)));
+
+  auto relativeTranslations = TranslationRecovery::SimulateMeasurements(
+      poses, {{0, 1}, {1, 3}, {3, 0}});
+
+  // Check simulated measurements.
+  for (auto& unitTranslation : relativeTranslations) {
+    EXPECT(assert_equal(GetDirectionFromPoses(poses, unitTranslation),
+                        unitTranslation.measured()));
+  }
+
+  TranslationRecovery algorithm(relativeTranslations);
+  const auto graph = algorithm.buildGraph();
+  EXPECT_LONGS_EQUAL(3, graph.size());
+
+  const auto result = algorithm.run(/*scale=*/3.0);
+
+  // Check result
+  EXPECT(assert_equal(Point3(0, 0, 0), result.at<Point3>(0)));
+  EXPECT(assert_equal(Point3(3, 0, 0), result.at<Point3>(1)));
+  EXPECT(assert_equal(Point3(1.5, -1.5, 0), result.at<Point3>(3)));
+}
+
+TEST(TranslationRecovery, ThreePosesIncludingZeroTranslation) {
+  // Create a dataset with 3 poses.
+  // __      __
+  // \/      \/
+  //  0 _____ 1
+  //          2 <|
+  //
+  // 0 and 1 face in the same direction but have a translation offset. 2 is at
+  // the same point as 1 but is rotated, with little FOV overlap.
+  Values poses;
+  poses.insert<Pose3>(0, Pose3(Rot3(), Point3(0, 0, 0)));
+  poses.insert<Pose3>(1, Pose3(Rot3(), Point3(2, 0, 0)));
+  poses.insert<Pose3>(2, Pose3(Rot3::RzRyRx(-M_PI / 2, 0, 0), Point3(2, 0, 0)));
+
+  auto relativeTranslations =
+      TranslationRecovery::SimulateMeasurements(poses, {{0, 1}, {1, 2}});
+
+  // Check simulated measurements.
+  for (auto& unitTranslation : relativeTranslations) {
+    EXPECT(assert_equal(GetDirectionFromPoses(poses, unitTranslation),
+                        unitTranslation.measured()));
+  }
+
+  TranslationRecovery algorithm(relativeTranslations);
+  const auto graph = algorithm.buildGraph();
+  // There is only 1 non-zero translation edge.
+  EXPECT_LONGS_EQUAL(1, graph.size()); 
+
+  // Run translation recovery
+  const auto result = algorithm.run(/*scale=*/3.0);
+
+  // Check result
+  EXPECT(assert_equal(Point3(0, 0, 0), result.at<Point3>(0)));
+  EXPECT(assert_equal(Point3(3, 0, 0), result.at<Point3>(1)));
+  EXPECT(assert_equal(Point3(3, 0, 0), result.at<Point3>(2)));
+}
+
+TEST(TranslationRecovery, FourPosesIncludingZeroTranslation) {
+  // Create a dataset with 4 poses.
+  // __      __
+  // \/      \/
+  //  0 _____ 1
+  //    \ __  2 <|
+  //     \\//
+  //       3
+  //
+  // 0 and 1 face in the same direction but have a translation offset. 2 is at
+  // the same point as 1 but is rotated, with very little FOV overlap. 3 is in
+  // the same direction as 0 and 1, in between 0 and 1, with some Y axis offset.
+
+  Values poses;
+  poses.insert<Pose3>(0, Pose3(Rot3(), Point3(0, 0, 0)));
+  poses.insert<Pose3>(1, Pose3(Rot3(), Point3(2, 0, 0)));
+  poses.insert<Pose3>(2, Pose3(Rot3::RzRyRx(-M_PI / 2, 0, 0), Point3(2, 0, 0)));
+  poses.insert<Pose3>(3, Pose3(Rot3(), Point3(1, -1, 0)));
+
+  auto relativeTranslations = TranslationRecovery::SimulateMeasurements(
+      poses, {{0, 1}, {1, 2}, {1, 3}, {3, 0}});
+
+  // Check simulated measurements.
+  for (auto& unitTranslation : relativeTranslations) {
+    EXPECT(assert_equal(GetDirectionFromPoses(poses, unitTranslation),
+                        unitTranslation.measured()));
+  }
+
+  TranslationRecovery algorithm(relativeTranslations);
+  const auto graph = algorithm.buildGraph();
+  EXPECT_LONGS_EQUAL(3, graph.size());
+
+  // Run translation recovery
+  const auto result = algorithm.run(/*scale=*/4.0);
+
+  // Check result
+  EXPECT(assert_equal(Point3(0, 0, 0), result.at<Point3>(0)));
+  EXPECT(assert_equal(Point3(4, 0, 0), result.at<Point3>(1)));
+  EXPECT(assert_equal(Point3(4, 0, 0), result.at<Point3>(2)));
+  EXPECT(assert_equal(Point3(2, -2, 0), result.at<Point3>(3)));
+}
+
 /* ************************************************************************* */
 int main() {
   TestResult tr;