new test doesnt pass

gtsam/sfm/TranslationRecovery.cpp

@@ -16,8 +16,7 @@
  * @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,39 @@
 #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) {
+  TranslationEdges tempRelativeTranslations;
+  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);
+    }
+  }
+  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());
+  }
+  sameTranslationNodes_ = sameTranslationDSF.sets();
+}
+
 NonlinearFactorGraph TranslationRecovery::buildGraph() const {
   NonlinearFactorGraph graph;
 
@@ -44,13 +71,14 @@ NonlinearFactorGraph TranslationRecovery::buildGraph() const {
   return graph;
 }
 
-void TranslationRecovery::addPrior(const double scale,
+void TranslationRecovery::addPrior(
-                                   NonlinearFactorGraph *graph,
+    const double scale, NonlinearFactorGraph *graph,
-                                   const SharedNoiseModel &priorNoiseModel) const {
+    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->key1(), Point3(0, 0, 0),
-  graph->emplace_shared<PriorFactor<Point3> >(edge->key2(), scale * edge->measured().point3(),
+                                              priorNoiseModel);
-                                              edge->noiseModel());
+  graph->emplace_shared<PriorFactor<Point3> >(
+      edge->key2(), scale * edge->measured().point3(), edge->noiseModel());
 }
 
 Values TranslationRecovery::initalizeRandomly() const {
@@ -77,6 +105,15 @@ Values TranslationRecovery::run(const double scale) const {
   const Values initial = initalizeRandomly();
   LevenbergMarquardtOptimizer lm(graph, initial, params_);
   Values result = lm.optimize();
+
+  for (const auto &sameTranslationKeys : sameTranslationNodes_) {
+    Key optimizedKey = sameTranslationKeys.first;
+    std::set<Key> duplicateKeys = sameTranslationKeys.second;
+    for (const Key duplicateKey : duplicateKeys) {
+      if (result.exists(duplicateKey)) continue;
+      result.insert<Point3>(duplicateKey, result.at<Point3>(optimizedKey));
+    }
+  }
   return result;
 }
 

gtsam/sfm/TranslationRecovery.h

@@ -23,6 +23,8 @@
 
 #include <utility>
 #include <vector>
+#include <set>
+#include <map>
 
 namespace gtsam {
 
@@ -54,6 +56,7 @@ class TranslationRecovery {
  private:
   TranslationEdges relativeTranslations_;
   LevenbergMarquardtParams params_;
+  std::map<Key, std::set<Key>> sameTranslationNodes_;
 
  public:
   /**
@@ -67,8 +70,7 @@ class TranslationRecovery {
    * default LM parameters. 
    */
   TranslationRecovery(const TranslationEdges &relativeTranslations,
-                      const LevenbergMarquardtParams &lmParams = LevenbergMarquardtParams())
+                      const LevenbergMarquardtParams &lmParams = LevenbergMarquardtParams());
-      : relativeTranslations_(relativeTranslations), params_(lmParams) {}
 
   /**
    * @brief Build the factor graph to do the optimization.

tests/testTranslationRecovery.cpp

@@ -16,9 +16,8 @@
  * @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;
@@ -49,14 +48,14 @@ TEST(TranslationRecovery, BAL) {
       poses, {{0, 1}, {0, 2}, {1, 2}});
 
   // Check
-  Unit3 w_aZb_stored; // measurement between 0 and 1 stored for next unit test
+  Unit3 w_aZb_stored;  // measurement between 0 and 1 stored for next unit test
-  for(auto& unitTranslation : relativeTranslations) {
+  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) {
+    if (unitTranslation.key1() == 0 && unitTranslation.key2() == 1) {
       w_aZb_stored = unitTranslation.measured();
     }
   }
@@ -77,14 +76,99 @@ TEST(TranslationRecovery, BAL) {
   Point3 Ta = poses.at<Pose3>(0).translation();
   Point3 Tb = poses.at<Pose3>(1).translation();
   Point3 Tc = poses.at<Pose3>(2).translation();
-  Point3 expected = 
+  Point3 expected = (Tc - Ta) * (scale / (Tb - Ta).norm());
-      (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, ZeroRelativeTranslations) {
+  // Create a graph with 3 cameras.
+  // __      __
+  // \/      \/
+  //  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 very little FOV overlap.
+  Values poses;
+  poses.insert(0, Pose3(Rot3(), Point3()));
+  poses.insert(1, Pose3(Rot3(), Point3(2, 0, 0)));
+  poses.insert(2, Pose3(Rot3::RzRyRx(-M_PI / 2, 0, 0), Point3(2, 0, 0)));
+
+  auto relativeTranslations =
+      TranslationRecovery::SimulateMeasurements(poses, {{0, 1}, {1, 2}});
+
+  // Check
+  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));
+  }
+
+  TranslationRecovery algorithm(relativeTranslations);
+  const auto graph = algorithm.buildGraph();
+  EXPECT_LONGS_EQUAL(1, graph.size());
+
+  // Translation recovery, version 1
+  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, 0, 0), result.at<Point3>(1)));
+  EXPECT(assert_equal(Point3(2, 0, 0), result.at<Point3>(2)));
+}
+
+TEST(TranslationRecovery, ZeroRelativeTranslations4Cameras) {
+  // Create a graph with 4 cameras.
+  // __      __
+  // \/      \/
+  //  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(0, Pose3(Rot3(), Point3()));
+  poses.insert(1, Pose3(Rot3(), Point3(2, 0, 0)));
+  poses.insert(2, Pose3(Rot3::RzRyRx(-M_PI / 2, 0, 0), Point3(2, 0, 0)));
+  poses.insert(3, Pose3(Rot3(), Point3(1, -1, 0)));
+
+  auto relativeTranslations = TranslationRecovery::SimulateMeasurements(
+      poses, {{0, 1}, {1, 2}, {1, 3}, {3, 0}});
+
+  // Check
+  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));
+  }
+
+  TranslationRecovery algorithm(relativeTranslations);
+  const auto graph = algorithm.buildGraph();
+  EXPECT_LONGS_EQUAL(3, graph.size());
+
+  // Translation recovery, version 1
+  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, 0, 0), result.at<Point3>(1)));
+  EXPECT(assert_equal(Point3(2, 0, 0), result.at<Point3>(2)));
+  EXPECT(assert_equal(Point3(1, -1, 0), result.at<Point3>(3)));
+}
+
 /* ************************************************************************* */
 int main() {
   TestResult tr;