From 48ec78bb405dc36d617736441ebab488cf3d4ae9 Mon Sep 17 00:00:00 2001 From: Luca Date: Wed, 28 May 2014 13:39:22 -0400 Subject: [PATCH] cleaned up code and added comments --- gtsam/nonlinear/LagoInitializer.h | 5 +- gtsam/nonlinear/tests/testLagoInitializer.cpp | 68 ++++++++++--------- 2 files changed, 40 insertions(+), 33 deletions(-) diff --git a/gtsam/nonlinear/LagoInitializer.h b/gtsam/nonlinear/LagoInitializer.h index ddb81a254..92f23a66d 100644 --- a/gtsam/nonlinear/LagoInitializer.h +++ b/gtsam/nonlinear/LagoInitializer.h @@ -21,7 +21,10 @@ * for graph-based simultaneous localization and mapping, RSS, 2011. * * @param graph: nonlinear factor graph (can include arbitrary factors but we assume - * that there is a subgraph involving Pose2 and betweenFactors) + * that there is a subgraph involving Pose2 and betweenFactors). Also in the current + * version we assume that there is an odometric spanning path (x0->x1, x1->x2, etc) + * and a prior on x0. This assumption can be relaxed by using the extra argument + * useOdometricPath = false, although this part of code is not stable yet. * @return Values: initial guess from LAGO (only pose2 are initialized) * * @author Luca Carlone diff --git a/gtsam/nonlinear/tests/testLagoInitializer.cpp b/gtsam/nonlinear/tests/testLagoInitializer.cpp index 313beffeb..64e43ae9b 100644 --- a/gtsam/nonlinear/tests/testLagoInitializer.cpp +++ b/gtsam/nonlinear/tests/testLagoInitializer.cpp @@ -183,8 +183,7 @@ TEST( Lago, multiplePosePriors ) { } /* *************************************************************************** */ -TEST_UNSAFE( Lago, multiplePosePriorsSP ) { - std::cout << "test we care about" << std::endl; +TEST( Lago, multiplePosePriorsSP ) { NonlinearFactorGraph g = simple::graph(); g.add(PriorFactor(x1, simple::pose1, model)); VectorValues initialGuessLago = initializeOrientationsLago(g); @@ -262,63 +261,68 @@ TEST( Lago, smallGraph2 ) { EXPECT(assert_equal(expected, actual, 1e-6)); } -/* *************************************************************************** * -TEST( Lago, smallGraphNoisy_orientations ) { +/* *************************************************************************** */ +TEST( Lago, largeGraphNoisy_orientations ) { NonlinearFactorGraph g; Values initial; - readG2o("/home/aspn/Desktop/noisyToyGraph.txt", g, initial); + string inputFile = findExampleDataFile("noisyToyGraph"); + readG2o(inputFile, g, initial); // Add prior on the pose having index (key) = 0 NonlinearFactorGraph graphWithPrior = g; noiseModel::Diagonal::shared_ptr priorModel = noiseModel::Diagonal::Variances((Vector(3) << 1e-2, 1e-2, 1e-4)); graphWithPrior.add(PriorFactor(0, Pose2(), priorModel)); - VectorValues initialGuessLago = initializeOrientationsLago(graphWithPrior); + VectorValues actualVV = initializeOrientationsLago(graphWithPrior); + Values actual; + Key keyAnc = symbol('Z',9999999); + for(VectorValues::const_iterator it = actualVV.begin(); it != actualVV.end(); ++it ){ + Key key = it->first; + if (key != keyAnc){ + Vector orientation = actualVV.at(key); + Pose2 poseLago = Pose2(0.0,0.0,orientation(0)); + actual.insert(key, poseLago); + } + } + NonlinearFactorGraph gmatlab; + Values expected; + string matlabFile = findExampleDataFile("orientationsNoisyToyGraph"); + readG2o(matlabFile, gmatlab, expected); - // Results from Matlab - // VERTEX_SE2 0 0.000000 0.000000 0.000000 - // VERTEX_SE2 1 0.000000 0.000000 1.565449 - // VERTEX_SE2 2 0.000000 0.000000 3.134143 - // VERTEX_SE2 3 0.000000 0.000000 4.710123 - - // comparison is up to M_PI, that's why we add some multiples of 2*M_PI - EXPECT(assert_equal((Vector(1) << 0.0), initialGuessLago.at(0), 1e-5)); - EXPECT(assert_equal((Vector(1) << 1.565449), initialGuessLago.at(1), 1e-5)); - EXPECT(assert_equal((Vector(1) << 3.134143), initialGuessLago.at(2), 1e-5)); - EXPECT(assert_equal((Vector(1) << 4.710123 - 2*M_PI), initialGuessLago.at(3), 1e-5)); + BOOST_FOREACH(const Values::KeyValuePair& key_val, expected){ + Key k = key_val.key; + EXPECT(assert_equal(expected.at(k), actual.at(k), 1e-5)); + } } -/* *************************************************************************** * -TEST( Lago, smallGraphNoisy ) { +/* *************************************************************************** */ +TEST( Lago, largeGraphNoisy ) { NonlinearFactorGraph g; Values initial; - readG2o("/home/aspn/Desktop/noisyToyGraph.txt", g, initial); + string inputFile = findExampleDataFile("noisyToyGraph"); + readG2o(inputFile, g, initial); // Add prior on the pose having index (key) = 0 NonlinearFactorGraph graphWithPrior = g; noiseModel::Diagonal::shared_ptr priorModel = noiseModel::Diagonal::Variances((Vector(3) << 1e-2, 1e-2, 1e-4)); graphWithPrior.add(PriorFactor(0, Pose2(), priorModel)); - // lago does not touch the Cartesian part and only fixed the orientations Values actual = initializeLago(graphWithPrior); - // Optimized results from matlab -// VERTEX_SE2 0 0.000000 -0.000000 0.000000 -// VERTEX_SE2 1 0.955797 1.137643 -0.022408 -// VERTEX_SE2 2 0.129867 1.989651 0.067117 -// VERTEX_SE2 3 -1.047715 0.933789 0.033559 - + NonlinearFactorGraph gmatlab; Values expected; - expected.insert(0,Pose2(0.000000, 0.000000, 0.000000)); - expected.insert(1,Pose2(0.955797, 1.137643, 1.565449 -0.022408)); - expected.insert(2,Pose2(0.129867, 1.989651, 3.134143 + 0.067117)); - expected.insert(3,Pose2(-1.047715, 0.933789, 4.710123 + 0.033559)); + string matlabFile = findExampleDataFile("optimizedNoisyToyGraph"); + readG2o(matlabFile, gmatlab, expected); - EXPECT(assert_equal(expected, actual, 1e-5)); + BOOST_FOREACH(const Values::KeyValuePair& key_val, expected){ + Key k = key_val.key; + EXPECT(assert_equal(expected.at(k), actual.at(k), 1e-2)); + } } + /* ************************************************************************* */ int main() { TestResult tr;