From 461047b242a93c6dda96d4708c37d2bf2ed6c527 Mon Sep 17 00:00:00 2001 From: Luca Date: Wed, 28 May 2014 13:14:49 -0400 Subject: [PATCH] first working version --- gtsam/nonlinear/LagoInitializer.cpp | 71 ++++++++++++++++++- gtsam/nonlinear/tests/testLagoInitializer.cpp | 49 +++++++++++-- 2 files changed, 114 insertions(+), 6 deletions(-) diff --git a/gtsam/nonlinear/LagoInitializer.cpp b/gtsam/nonlinear/LagoInitializer.cpp index 23728e6b7..79264ceb9 100644 --- a/gtsam/nonlinear/LagoInitializer.cpp +++ b/gtsam/nonlinear/LagoInitializer.cpp @@ -17,6 +17,7 @@ */ #include +#include namespace gtsam { @@ -30,6 +31,7 @@ double computeThetaToRoot(const Key nodeKey, const PredecessorMap& tree, double nodeTheta = 0; Key key_child = nodeKey; // the node Key key_parent = 0; // the initialization does not matter + ///std::cout << "start" << std::endl; while(1){ // We check if we reached the root if(tree.at(key_child)==key_child) // if we reached the root @@ -45,6 +47,7 @@ double computeThetaToRoot(const Key nodeKey, const PredecessorMap& tree, } key_child = key_parent; // we move upwards in the tree } + ///std::cout << "end" << std::endl; return nodeTheta; } @@ -54,6 +57,10 @@ key2doubleMap computeThetasToRoot(const key2doubleMap& deltaThetaMap, key2doubleMap thetaToRootMap; key2doubleMap::const_iterator it; + + // Orientation of the roo + thetaToRootMap.insert(std::pair(keyAnchor, 0.0)); + // for all nodes in the tree for(it = deltaThetaMap.begin(); it != deltaThetaMap.end(); ++it ){ // compute the orientation wrt root @@ -100,6 +107,15 @@ void getSymbolicGraph( } id++; } + + ///g.print("Before detlta map \n"); + + key2doubleMap::const_iterator it; + for(it = deltaThetaMap.begin(); it != deltaThetaMap.end(); ++it ){ + Key nodeKey = it->first; + ///std::cout << "deltaThMAP = key " << DefaultKeyFormatter(nodeKey) << " th= " << it->second << std::endl; + } + } /* ************************************************************************* */ @@ -145,6 +161,7 @@ GaussianFactorGraph buildLinearOrientationGraph(const std::vector& spann Key key1 = keys[0], key2 = keys[1]; getDeltaThetaAndNoise(g[factorId], deltaTheta, model_deltaTheta); double key1_DeltaTheta_key2 = deltaTheta(0); + ///std::cout << "REG: key1= " << DefaultKeyFormatter(key1) << " key2= " << DefaultKeyFormatter(key2) << std::endl; double k2pi_noise = key1_DeltaTheta_key2 + orientationsToRoot.at(key1) - orientationsToRoot.at(key2); // this coincides to summing up measurements along the cycle induced by the chord double k = round(k2pi_noise/(2*M_PI)); //if (k2pi_noise - 2*k*M_PI > 1e-5) std::cout << k2pi_noise - 2*k*M_PI << std::endl; // for debug @@ -178,11 +195,44 @@ NonlinearFactorGraph buildPose2graph(const NonlinearFactorGraph& graph){ return pose2Graph; } +/* ************************************************************************* */ +PredecessorMap findOdometricPath(const NonlinearFactorGraph& pose2Graph) { + + PredecessorMap tree; + Key minKey; + bool minUnassigned = true; + + BOOST_FOREACH(const boost::shared_ptr& factor, pose2Graph){ + + Key key1 = std::min(factor->keys()[0], factor->keys()[1]); + Key key2 = std::max(factor->keys()[0], factor->keys()[1]); + if(minUnassigned){ + minKey = key1; + minUnassigned = false; + } + if( key2 - key1 == 1){ // consecutive keys + tree.insert(key2, key1); + if(key1 < minKey) + minKey = key1; + } + } + tree.insert(minKey,keyAnchor); + tree.insert(keyAnchor,keyAnchor); // root + return tree; +} + /* ************************************************************************* */ VectorValues computeLagoOrientations(const NonlinearFactorGraph& pose2Graph){ + bool useOdometricPath = true; // Find a minimum spanning tree - PredecessorMap tree = findMinimumSpanningTree >(pose2Graph); + PredecessorMap tree; + if (useOdometricPath) + tree = findOdometricPath(pose2Graph); + else + tree = findMinimumSpanningTree >(pose2Graph); + + ///std::cout << "found spanning tree" << std::endl; // Create a linear factor graph (LFG) of scalars key2doubleMap deltaThetaMap; @@ -190,9 +240,13 @@ VectorValues computeLagoOrientations(const NonlinearFactorGraph& pose2Graph){ std::vector chordsIds; // ids of between factors corresponding to chordsIds wrt T getSymbolicGraph(spanningTreeIds, chordsIds, deltaThetaMap, tree, pose2Graph); + ///std::cout << "found symbolic graph" << std::endl; + // temporary structure to correct wraparounds along loops key2doubleMap orientationsToRoot = computeThetasToRoot(deltaThetaMap, tree); + ///std::cout << "computed orientations from root" << std::endl; + // regularize measurements and plug everything in a factor graph GaussianFactorGraph lagoGraph = buildLinearOrientationGraph(spanningTreeIds, chordsIds, pose2Graph, orientationsToRoot, tree); @@ -280,12 +334,27 @@ Values initializeLago(const NonlinearFactorGraph& graph) { // We "extract" the Pose2 subgraph of the original graph: this // is done to properly model priors and avoiding operating on a larger graph + ///std::cout << "buildPose2graph" << std::endl; NonlinearFactorGraph pose2Graph = buildPose2graph(graph); // Get orientations from relative orientation measurements + ///std::cout << "computeLagoOrientations" << std::endl; VectorValues orientationsLago = computeLagoOrientations(pose2Graph); +// VectorValues orientationsLago; +// NonlinearFactorGraph g; +// Values orientationsLagoV; +// readG2o("/home/aspn/Desktop/orientationsNoisyToyGraph.txt", g, orientationsLagoV); +// +// BOOST_FOREACH(const Values::KeyValuePair& key_val, orientationsLagoV){ +// Key k = key_val.key; +// double th = orientationsLagoV.at(k).theta(); +// orientationsLago.insert(k,(Vector(1) << th)); +// } +// orientationsLago.insert(keyAnchor,(Vector(1) << 0.0)); + // Compute the full poses + ///std::cout << "computeLagoPoses" << std::endl; return computeLagoPoses(pose2Graph, orientationsLago); } diff --git a/gtsam/nonlinear/tests/testLagoInitializer.cpp b/gtsam/nonlinear/tests/testLagoInitializer.cpp index 4be325734..37740f3ee 100644 --- a/gtsam/nonlinear/tests/testLagoInitializer.cpp +++ b/gtsam/nonlinear/tests/testLagoInitializer.cpp @@ -144,7 +144,7 @@ TEST( Lago, regularizedMeasurements ) { EXPECT(assert_equal(expected, actual, 1e-6)); } -/* *************************************************************************** */ +/* *************************************************************************** * TEST( Lago, smallGraphVectorValues ) { VectorValues initialGuessLago = initializeOrientationsLago(simple::graph()); @@ -157,6 +157,18 @@ TEST( Lago, smallGraphVectorValues ) { } /* *************************************************************************** */ +TEST( Lago, smallGraphVectorValuesSP ) { + + VectorValues initialGuessLago = initializeOrientationsLago(simple::graph()); + + // 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(x0), 1e-6)); + EXPECT(assert_equal((Vector(1) << 0.5 * M_PI), initialGuessLago.at(x1), 1e-6)); + EXPECT(assert_equal((Vector(1) << M_PI ), initialGuessLago.at(x2), 1e-6)); + EXPECT(assert_equal((Vector(1) << 1.5 * M_PI ), initialGuessLago.at(x3), 1e-6)); +} + +/* *************************************************************************** * TEST( Lago, multiplePosePriors ) { NonlinearFactorGraph g = simple::graph(); g.add(PriorFactor(x1, simple::pose1, model)); @@ -169,6 +181,33 @@ TEST( Lago, multiplePosePriors ) { EXPECT(assert_equal((Vector(1) << 1.5 * M_PI - 2*M_PI), initialGuessLago.at(x3), 1e-6)); } +/* *************************************************************************** */ +TEST_UNSAFE( Lago, multiplePosePriorsSP ) { + std::cout << "test we care about" << std::endl; + NonlinearFactorGraph g = simple::graph(); + g.add(PriorFactor(x1, simple::pose1, model)); + VectorValues initialGuessLago = initializeOrientationsLago(g); + + // 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(x0), 1e-6)); + EXPECT(assert_equal((Vector(1) << 0.5 * M_PI), initialGuessLago.at(x1), 1e-6)); + EXPECT(assert_equal((Vector(1) << M_PI ), initialGuessLago.at(x2), 1e-6)); + EXPECT(assert_equal((Vector(1) << 1.5 * M_PI ), initialGuessLago.at(x3), 1e-6)); +} + +/* *************************************************************************** * +TEST( Lago, multiplePoseAndRotPriors ) { + NonlinearFactorGraph g = simple::graph(); + g.add(PriorFactor(x1, simple::pose1.theta(), model)); + VectorValues initialGuessLago = initializeOrientationsLago(g); + + // 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(x0), 1e-6)); + EXPECT(assert_equal((Vector(1) << 0.5 * M_PI), initialGuessLago.at(x1), 1e-6)); + EXPECT(assert_equal((Vector(1) << M_PI - 2*M_PI), initialGuessLago.at(x2), 1e-6)); + EXPECT(assert_equal((Vector(1) << 1.5 * M_PI - 2*M_PI), initialGuessLago.at(x3), 1e-6)); +} + /* *************************************************************************** */ TEST( Lago, multiplePoseAndRotPriors ) { NonlinearFactorGraph g = simple::graph(); @@ -178,8 +217,8 @@ TEST( Lago, multiplePoseAndRotPriors ) { // 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(x0), 1e-6)); EXPECT(assert_equal((Vector(1) << 0.5 * M_PI), initialGuessLago.at(x1), 1e-6)); - EXPECT(assert_equal((Vector(1) << M_PI - 2*M_PI), initialGuessLago.at(x2), 1e-6)); - EXPECT(assert_equal((Vector(1) << 1.5 * M_PI - 2*M_PI), initialGuessLago.at(x3), 1e-6)); + EXPECT(assert_equal((Vector(1) << M_PI ), initialGuessLago.at(x2), 1e-6)); + EXPECT(assert_equal((Vector(1) << 1.5 * M_PI ), initialGuessLago.at(x3), 1e-6)); } /* *************************************************************************** */ @@ -221,7 +260,7 @@ TEST( Lago, smallGraph2 ) { EXPECT(assert_equal(expected, actual, 1e-6)); } -/* *************************************************************************** */ +/* *************************************************************************** * TEST( Lago, smallGraphNoisy_orientations ) { NonlinearFactorGraph g; @@ -248,7 +287,7 @@ TEST( Lago, smallGraphNoisy_orientations ) { EXPECT(assert_equal((Vector(1) << 4.710123 - 2*M_PI), initialGuessLago.at(3), 1e-5)); } -/* *************************************************************************** */ +/* *************************************************************************** * TEST( Lago, smallGraphNoisy ) { NonlinearFactorGraph g;