diff --git a/tests/testTranslationRecovery.cpp b/tests/testTranslationRecovery.cpp index 8742820f4..94ee14c28 100644 --- a/tests/testTranslationRecovery.cpp +++ b/tests/testTranslationRecovery.cpp @@ -16,7 +16,6 @@ * @brief test recovering translations when rotations are given. */ -#include #include #include #include @@ -26,6 +25,8 @@ #include #include #include +#include +#include namespace gtsam { @@ -57,6 +58,7 @@ class TranslationRecovery { private: TranslationEdges relativeTranslations_; + LevenbergMarquardtParams params_; public: /** @@ -66,7 +68,9 @@ class TranslationRecovery { * frames, indexed in a map by a pair of Pose keys. */ TranslationRecovery(const TranslationEdges& relativeTranslations) - : relativeTranslations_(relativeTranslations) {} + : relativeTranslations_(relativeTranslations) { + params_.setVerbosityLM("Summary"); + } /** * @brief Build the factor graph to do the optimization. @@ -76,15 +80,35 @@ class TranslationRecovery { NonlinearFactorGraph buildGraph() const { auto noiseModel = noiseModel::Isotropic::Sigma(3, 0.01); NonlinearFactorGraph graph; + + // Add all relative translation edges for (auto edge : relativeTranslations_) { Key a, b; std::tie(a, b) = edge.first; const Unit3 w_aZb = edge.second; graph.emplace_shared(a, b, w_aZb, noiseModel); } + return graph; } + /** + * @brief Add priors on ednpoints of first measurement edge. + * + * @param scale scale for first relative translation which fixes gauge. + * @param graph factor graph to which prior is added. + */ + void addPrior(const double scale, NonlinearFactorGraph* graph) const { + auto noiseModel = noiseModel::Isotropic::Sigma(3, 0.01); + auto edge = relativeTranslations_.begin(); + Key a, b; + std::tie(a, b) = edge->first; + const Unit3 w_aZb = edge->second; + graph->emplace_shared >(a, Point3(0, 0, 0), noiseModel); + graph->emplace_shared >(b, scale * w_aZb.point3(), + noiseModel); + } + /** * @brief Create random initial translations. * @@ -115,14 +139,11 @@ class TranslationRecovery { * @return Values */ Values run(const double scale = 1.0) const { - const auto graph = buildGraph(); + auto graph = buildGraph(); + addPrior(scale, &graph); const Values initial = initalizeRandomly(); - - LevenbergMarquardtParams params; - params.setVerbosityLM("Summary"); - LevenbergMarquardtOptimizer lm(graph, initial, params); + LevenbergMarquardtOptimizer lm(graph, initial, params_); Values result = lm.optimize(); - return result; } @@ -212,14 +233,18 @@ TEST(TranslationRecovery, BAL) { EXPECT_LONGS_EQUAL(3, graph.size()); // Translation recovery, version 1 - const auto result = algorithm.run(2); + const double scale = 2.0; + const auto result = algorithm.run(scale); - // Check result + // Check result for first two translations, determined by prior EXPECT(assert_equal(Point3(0, 0, 0), result.at(0))); EXPECT(assert_equal(Point3(2 * w_aZb.point3()), result.at(1))); - EXPECT(assert_equal(Point3(0, 0, 0), result.at(2))); - // TODO(frank): how to get stats back + // Check that the third translations is correct + Point3 expected = (Tc - Ta) * (scale / (Tb - Ta).norm()); + EXPECT(assert_equal(expected, result.at(2), 1e-4)); + + // TODO(frank): how to get stats back? // EXPECT_DOUBLES_EQUAL(0.0199833, actualError, 1e-5); }