From 8be6d33714136a120c8d9b4faf33557b35e0eb0d Mon Sep 17 00:00:00 2001 From: lcarlone Date: Thu, 24 Sep 2020 19:10:14 -0400 Subject: [PATCH] added nice unit test --- gtsam/sfm/ShonanAveraging.cpp | 21 --------------------- gtsam/sfm/tests/testShonanAveraging.cpp | 22 ++++++++++++---------- gtsam/slam/FrobeniusFactor.cpp | 12 ++++++------ gtsam/slam/FrobeniusFactor.h | 2 +- 4 files changed, 19 insertions(+), 38 deletions(-) diff --git a/gtsam/sfm/ShonanAveraging.cpp b/gtsam/sfm/ShonanAveraging.cpp index 0ac893b7c..14f5665b6 100644 --- a/gtsam/sfm/ShonanAveraging.cpp +++ b/gtsam/sfm/ShonanAveraging.cpp @@ -139,35 +139,25 @@ ShonanAveraging::ShonanAveraging(const Measurements &measurements, /* ************************************************************************* */ template NonlinearFactorGraph ShonanAveraging::buildGraphAt(size_t p) const { - std::cout << "zz0" << std::endl; NonlinearFactorGraph graph; auto G = boost::make_shared(SO<-1>::VectorizedGenerators(p)); - std::cout << "zz1" << std::endl; for (const auto &measurement : measurements_) { const auto &keys = measurement.keys(); const auto &Rij = measurement.measured(); const auto &model = measurement.noiseModel(); - measurement.print("measurement"); - std::cout << "zzzz1" << std::endl; - model->print(); - std::cout << "zzzz2" << std::endl; graph.emplace_shared>(keys[0], keys[1], Rij, p, model, G); - std::cout << "zzzz3" << std::endl; } - // Possibly add Karcher prior if (parameters_.beta > 0) { const size_t dim = SOn::Dimension(p); graph.emplace_shared>(graph.keys(), dim); } - std::cout << "zz2" << std::endl; // Possibly add gauge factors - they are probably useless as gradient is zero if (parameters_.gamma > 0 && p > d + 1) { for (auto key : graph.keys()) graph.emplace_shared(key, p, d, parameters_.gamma); } - std::cout << "z3" << std::endl; return graph; } @@ -185,7 +175,6 @@ ShonanAveraging::createOptimizerAt(size_t p, const Values &initial) const { // Build graph NonlinearFactorGraph graph = buildGraphAt(p); - std::cout << "yy1" << std::endl; // Anchor prior is added here as depends on initial value (and cost is zero) if (parameters_.alpha > 0) { size_t i; @@ -196,7 +185,6 @@ ShonanAveraging::createOptimizerAt(size_t p, const Values &initial) const { graph.emplace_shared>(i, SOn::Lift(p, value.matrix()), model); } - std::cout << "yy2" << std::endl; // Optimize return boost::make_shared(graph, initial, parameters_.lm); @@ -206,9 +194,7 @@ ShonanAveraging::createOptimizerAt(size_t p, const Values &initial) const { template Values ShonanAveraging::tryOptimizingAt(size_t p, const Values &initial) const { - std::cout << "xx1" << std::endl; auto lm = createOptimizerAt(p, initial); - std::cout << "xx2" << std::endl; return lm->optimize(); } @@ -814,29 +800,23 @@ std::pair ShonanAveraging::run(const Values &initialEstimate, Values initialSOp = LiftTo(pMin, initialEstimate); // lift to pMin! for (size_t p = pMin; p <= pMax; p++) { // Optimize until convergence at this level - std::cout << "4a" << std::endl; Qstar = tryOptimizingAt(p, initialSOp); - std::cout << "4aa" << std::endl; if(parameters_.useHuber){ // in this case, there is no optimality verification - std::cout << "4aaa" << std::endl; if(pMin!=pMax) std::cout << "When using robust norm, Shonan only tests a single rank" << std::endl; const Values SO3Values = roundSolution(Qstar); return std::make_pair(SO3Values, 0); } - std::cout << "4b" << std::endl; // Check certificate of global optimzality Vector minEigenVector; double minEigenValue = computeMinEigenValue(Qstar, &minEigenVector); - std::cout << "4bb" << std::endl; if (minEigenValue > parameters_.optimalityThreshold) { // If at global optimum, round and return solution const Values SO3Values = roundSolution(Qstar); return std::make_pair(SO3Values, minEigenValue); } - std::cout << "4c" << std::endl; // Not at global optimimum yet, so check whether we will go to next level if (p != pMax) { // Calculate initial estimate for next level by following minEigenVector @@ -844,7 +824,6 @@ std::pair ShonanAveraging::run(const Values &initialEstimate, initializeWithDescent(p + 1, Qstar, minEigenVector, minEigenValue); } } - std::cout << "4d" << std::endl; throw std::runtime_error("Shonan::run did not converge for given pMax"); } diff --git a/gtsam/sfm/tests/testShonanAveraging.cpp b/gtsam/sfm/tests/testShonanAveraging.cpp index fd4d5e34f..269b2c855 100644 --- a/gtsam/sfm/tests/testShonanAveraging.cpp +++ b/gtsam/sfm/tests/testShonanAveraging.cpp @@ -330,23 +330,25 @@ TEST(ShonanAveraging2, noisyToyGraphWithHuber) { auto measurements = parseMeasurements(g2oFile); parameters.setUseHuber(true); parameters.print(); - std::cout << "1" << std::endl; ShonanAveraging2 shonan(measurements, parameters); EXPECT_LONGS_EQUAL(4, shonan.nrUnknowns()); // Check graph building - std::cout << "2" << std::endl; -// NonlinearFactorGraph graph = shonan.buildGraphAt(2); -// graph.print(); -// EXPECT_LONGS_EQUAL(6, graph.size()); - std::cout << "3" << std::endl; + NonlinearFactorGraph graph = shonan.buildGraphAt(2); + EXPECT_LONGS_EQUAL(6, graph.size()); + // test that each factor is actually robust + for (size_t i=0; i<=4; i++) { // note: last is the Gauge factor and is not robust + const auto &robust = boost::dynamic_pointer_cast( + boost::dynamic_pointer_cast(graph[i])->noiseModel()); + EXPECT(robust); // we expect the factors to be use a robust noise model (in particular, Huber) + } + + // test result auto initial = shonan.initializeRandomly(kRandomNumberGenerator); - std::cout << "4" << std::endl; auto result = shonan.run(initial, 2,3); - std::cout << "5" << std::endl; -// EXPECT_DOUBLES_EQUAL(0.0008211, shonan.cost(result.first), 1e-6); -// EXPECT_DOUBLES_EQUAL(0, result.second, 1e-10); // certificate! + EXPECT_DOUBLES_EQUAL(0.0008211, shonan.cost(result.first), 1e-6); + EXPECT_DOUBLES_EQUAL(0, result.second, 1e-10); // certificate! } /* ************************************************************************* */ diff --git a/gtsam/slam/FrobeniusFactor.cpp b/gtsam/slam/FrobeniusFactor.cpp index 7af4958e8..2d7417283 100644 --- a/gtsam/slam/FrobeniusFactor.cpp +++ b/gtsam/slam/FrobeniusFactor.cpp @@ -23,18 +23,18 @@ using namespace std; namespace gtsam { //****************************************************************************** -boost::shared_ptr +SharedNoiseModel ConvertNoiseModel(const SharedNoiseModel &model, size_t d, bool defaultToUnit) { double sigma = 1.0; - std::cout << "111111" << std::endl; if (model != nullptr) { const auto &robust = boost::dynamic_pointer_cast(model); Vector sigmas; - if(robust) - sigmas[0] = 1; - else - sigmas = model->sigmas(); + if(robust){ + sigma = 1; // Rot2 + goto exit; + } //else: + sigmas = model->sigmas(); size_t n = sigmas.size(); if (n == 1) { sigma = sigmas(0); // Rot2 diff --git a/gtsam/slam/FrobeniusFactor.h b/gtsam/slam/FrobeniusFactor.h index 1fc37c785..9915a617d 100644 --- a/gtsam/slam/FrobeniusFactor.h +++ b/gtsam/slam/FrobeniusFactor.h @@ -34,7 +34,7 @@ namespace gtsam { * isotropic. If it is, we extend to 'n' dimensions, otherwise we throw an * error. If defaultToUnit == false throws an exception on unexepcted input. */ -GTSAM_EXPORT boost::shared_ptr +GTSAM_EXPORT SharedNoiseModel ConvertNoiseModel(const SharedNoiseModel &model, size_t n, bool defaultToUnit = true);