move betweenTranslations outside params

gtsam/sfm/TranslationRecovery.cpp

@@ -79,28 +79,29 @@ NonlinearFactorGraph TranslationRecovery::buildGraph() const {
     graph.emplace_shared<TranslationFactor>(edge.key1(), edge.key2(),
                                             edge.measured(), edge.noiseModel());
   }
-
-  // Add between factors for optional relative translations.
-  for (auto edge : params_.getBetweenTranslations()) {
-    Key k1 = getUniqueKey(edge.key1()), k2 = getUniqueKey(edge.key2());
-    if (k1 != k2) {
-      graph.emplace_shared<BetweenFactor<Point3>>(k1, k2, edge.measured(),
-                                                  edge.noiseModel());
-    }
-  }
-
   return graph;
 }
 
 void TranslationRecovery::addPrior(
+    const std::vector<BinaryMeasurement<Point3>> &betweenTranslations,
     const double scale, const boost::shared_ptr<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);
+
+  // Add between factors for optional relative translations.
+  for (auto edge : betweenTranslations) {
+    Key k1 = getUniqueKey(edge.key1()), k2 = getUniqueKey(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 (params_.getBetweenTranslations().empty()) {
+  if (betweenTranslations.empty()) {
     graph->emplace_shared<PriorFactor<Point3>>(
         edge->key2(), scale * edge->measured().point3(), edge->noiseModel());
   }
@@ -154,10 +155,12 @@ Values TranslationRecovery::initializeRandomly() const {
   return initializeRandomly(&kRandomNumberGenerator);
 }
 
-Values TranslationRecovery::run(const double scale) const {
+Values TranslationRecovery::run(
+    const std::vector<BinaryMeasurement<Point3>> &betweenTranslations,
+    const double scale) const {
   boost::shared_ptr<NonlinearFactorGraph> graph_ptr =
       boost::make_shared<NonlinearFactorGraph>(buildGraph());
-  addPrior(scale, graph_ptr);
+  addPrior(betweenTranslations, scale, graph_ptr);
   const Values initial = initializeRandomly();
   LevenbergMarquardtOptimizer lm(*graph_ptr, initial, params_.getLMParams());
   Values result = lm.optimize();

gtsam/sfm/TranslationRecovery.h

@@ -31,15 +31,6 @@ namespace gtsam {
 // Parameters for the Translation Recovery problem.
 class TranslationRecoveryParams {
  public:
-  std::vector<BinaryMeasurement<Point3>> getBetweenTranslations() const {
-    return betweenTranslations_;
-  }
-
-  void setBetweenTranslations(
-      const std::vector<BinaryMeasurement<Point3>> &betweenTranslations) {
-    betweenTranslations_ = betweenTranslations;
-  }
-
   LevenbergMarquardtParams getLMParams() const { return lmParams_; }
 
   Values getInitialValues() const { return initial_; }
@@ -51,10 +42,6 @@ class TranslationRecoveryParams {
   }
 
  private:
-  // Relative translations with a known scale used as between factors in the
-  // problem if provided.
-  std::vector<BinaryMeasurement<Point3>> betweenTranslations_;
-
   // LevenbergMarquardtParams for optimization.
   LevenbergMarquardtParams lmParams_;
 
@@ -125,10 +112,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,
-                const boost::shared_ptr<NonlinearFactorGraph> graph,
-                const SharedNoiseModel &priorNoiseModel =
-                    noiseModel::Isotropic::Sigma(3, 0.01)) const;
+  void addPrior(
+      const std::vector<BinaryMeasurement<Point3>> &betweenTranslations,
+      const double scale, const boost::shared_ptr<NonlinearFactorGraph> graph,
+      const SharedNoiseModel &priorNoiseModel =
+          noiseModel::Isotropic::Sigma(3, 0.01)) const;
 
   /**
    * @brief Create random initial translations.
@@ -152,7 +140,9 @@ class TranslationRecovery {
    * 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

tests/testTranslationRecovery.cpp

@@ -68,7 +68,7 @@ TEST(TranslationRecovery, BAL) {
 
   // Run translation recovery
   const double scale = 2.0;
-  const auto result = algorithm.run(scale);
+  const auto result = algorithm.run(/*betweenTranslations=*/{}, scale);
 
   // Check result for first two translations, determined by prior
   EXPECT(assert_equal(Point3(0, 0, 0), result.at<Point3>(0)));
@@ -112,7 +112,7 @@ TEST(TranslationRecovery, TwoPoseTest) {
   EXPECT_LONGS_EQUAL(1, graph.size());
 
   // Run translation recovery
-  const auto result = algorithm.run(/*scale=*/3.0);
+  const auto result = algorithm.run(/*betweenTranslations=*/{}, /*scale=*/3.0);
 
   // Check result for first two translations, determined by prior
   EXPECT(assert_equal(Point3(0, 0, 0), result.at<Point3>(0), 1e-8));
@@ -149,7 +149,7 @@ TEST(TranslationRecovery, ThreePoseTest) {
   const auto graph = algorithm.buildGraph();
   EXPECT_LONGS_EQUAL(3, graph.size());
 
-  const auto result = algorithm.run(/*scale=*/3.0);
+  const auto result = algorithm.run(/*betweenTranslations=*/{}, /*scale=*/3.0);
 
   // Check result
   EXPECT(assert_equal(Point3(0, 0, 0), result.at<Point3>(0), 1e-8));
@@ -186,7 +186,7 @@ TEST(TranslationRecovery, ThreePosesIncludingZeroTranslation) {
   EXPECT_LONGS_EQUAL(1, graph.size());
 
   // Run translation recovery
-  const auto result = algorithm.run(/*scale=*/3.0);
+  const auto result = algorithm.run(/*betweenTranslations=*/{}, /*scale=*/3.0);
 
   // Check result
   EXPECT(assert_equal(Point3(0, 0, 0), result.at<Point3>(0), 1e-8));
@@ -227,7 +227,7 @@ TEST(TranslationRecovery, FourPosesIncludingZeroTranslation) {
   EXPECT_LONGS_EQUAL(3, graph.size());
 
   // Run translation recovery
-  const auto result = algorithm.run(/*scale=*/4.0);
+  const auto result = algorithm.run(/*betweenTranslations=*/{}, /*scale=*/4.0);
 
   // Check result
   EXPECT(assert_equal(Point3(0, 0, 0), result.at<Point3>(0), 1e-8));
@@ -257,7 +257,7 @@ TEST(TranslationRecovery, ThreePosesWithZeroTranslation) {
   EXPECT_LONGS_EQUAL(0, graph.size());
 
   // Run translation recovery
-  const auto result = algorithm.run(/*scale=*/4.0);
+  const auto result = algorithm.run(/*betweenTranslations=*/{}, /*scale=*/4.0);
 
   // Check result
   EXPECT(assert_equal(Point3(0, 0, 0), result.at<Point3>(0), 1e-8));
@@ -282,16 +282,15 @@ TEST(TranslationRecovery, ThreePosesWithOneSoftConstraint) {
   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}, {0, 3}, {1, 3}});
+  auto relativeTranslations = TranslationRecovery::SimulateMeasurements(
+      poses, {{0, 1}, {0, 3}, {1, 3}});
 
-  TranslationRecoveryParams params;
   std::vector<BinaryMeasurement<Point3>> betweenTranslations;
-  betweenTranslations.emplace_back(0, 3, Point3(1, -1, 0), noiseModel::Isotropic::Sigma(3, 1e-2));
-  params.setBetweenTranslations(betweenTranslations);
+  betweenTranslations.emplace_back(0, 3, Point3(1, -1, 0),
+                                   noiseModel::Isotropic::Sigma(3, 1e-2));
 
-  TranslationRecovery algorithm(relativeTranslations, params);
-  auto result = algorithm.run();
+  TranslationRecovery algorithm(relativeTranslations);
+  auto result = algorithm.run(betweenTranslations);
 
   // Check result
   EXPECT(assert_equal(Point3(0, 0, 0), result.at<Point3>(0), 1e-4));
@@ -299,7 +298,6 @@ TEST(TranslationRecovery, ThreePosesWithOneSoftConstraint) {
   EXPECT(assert_equal(Point3(1, -1, 0), result.at<Point3>(3), 1e-4));
 }
 
-
 TEST(TranslationRecovery, ThreePosesWithOneHardConstraint) {
   // Create a dataset with 3 poses.
   // __      __
@@ -317,16 +315,15 @@ TEST(TranslationRecovery, ThreePosesWithOneHardConstraint) {
   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}, {0, 3}, {1, 3}});
+  auto relativeTranslations = TranslationRecovery::SimulateMeasurements(
+      poses, {{0, 1}, {0, 3}, {1, 3}});
 
-  TranslationRecoveryParams params;
   std::vector<BinaryMeasurement<Point3>> betweenTranslations;
-  betweenTranslations.emplace_back(0, 1, Point3(2, 0, 0), noiseModel::Constrained::All(3, 1e2));
-  params.setBetweenTranslations(betweenTranslations);
+  betweenTranslations.emplace_back(0, 1, Point3(2, 0, 0),
+                                   noiseModel::Constrained::All(3, 1e2));
 
-  TranslationRecovery algorithm(relativeTranslations, params);
-  auto result = algorithm.run();
+  TranslationRecovery algorithm(relativeTranslations);
+  auto result = algorithm.run(betweenTranslations);
 
   // Check result
   EXPECT(assert_equal(Point3(0, 0, 0), result.at<Point3>(0), 1e-4));