From 732409faf38afa0bfd658c0161064419af65573b Mon Sep 17 00:00:00 2001 From: dellaert Date: Sat, 31 May 2014 12:51:37 -0400 Subject: [PATCH 01/10] Changed filenames --- gtsam/nonlinear/{LagoInitializer.cpp => lago.cpp} | 4 ++-- gtsam/nonlinear/{LagoInitializer.h => lago.h} | 2 +- .../nonlinear/tests/{testLagoInitializer.cpp => testLago.cpp} | 2 +- 3 files changed, 4 insertions(+), 4 deletions(-) rename gtsam/nonlinear/{LagoInitializer.cpp => lago.cpp} (99%) rename gtsam/nonlinear/{LagoInitializer.h => lago.h} (99%) rename gtsam/nonlinear/tests/{testLagoInitializer.cpp => testLago.cpp} (99%) diff --git a/gtsam/nonlinear/LagoInitializer.cpp b/gtsam/nonlinear/lago.cpp similarity index 99% rename from gtsam/nonlinear/LagoInitializer.cpp rename to gtsam/nonlinear/lago.cpp index 3451beaf3..e2f13fded 100644 --- a/gtsam/nonlinear/LagoInitializer.cpp +++ b/gtsam/nonlinear/lago.cpp @@ -10,13 +10,13 @@ * -------------------------------------------------------------------------- */ /** - * @file LagoInitializer.h + * @file lago.h * @author Luca Carlone * @author Frank Dellaert * @date May 14, 2014 */ -#include +#include #include #include diff --git a/gtsam/nonlinear/LagoInitializer.h b/gtsam/nonlinear/lago.h similarity index 99% rename from gtsam/nonlinear/LagoInitializer.h rename to gtsam/nonlinear/lago.h index b351365c1..91f6ecd97 100644 --- a/gtsam/nonlinear/LagoInitializer.h +++ b/gtsam/nonlinear/lago.h @@ -10,7 +10,7 @@ * -------------------------------------------------------------------------- */ /** - * @file LagoInitializer.h + * @file lago.h * @brief Initialize Pose2 in a factor graph using LAGO * (Linear Approximation for Graph Optimization). see papers: * diff --git a/gtsam/nonlinear/tests/testLagoInitializer.cpp b/gtsam/nonlinear/tests/testLago.cpp similarity index 99% rename from gtsam/nonlinear/tests/testLagoInitializer.cpp rename to gtsam/nonlinear/tests/testLago.cpp index 64e43ae9b..576266f22 100644 --- a/gtsam/nonlinear/tests/testLagoInitializer.cpp +++ b/gtsam/nonlinear/tests/testLago.cpp @@ -28,7 +28,7 @@ #include #include -#include +#include #include #include From 4afb11a2eda5cfe89b273af92eefa15db041ba03 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sat, 31 May 2014 12:51:47 -0400 Subject: [PATCH 02/10] Fixed up examples --- .cproject | 162 +++++++++++++++++------------ examples/Pose2SLAMExample_g2o.cpp | 35 +++---- examples/Pose2SLAMExample_lago.cpp | 38 +++---- 3 files changed, 131 insertions(+), 104 deletions(-) diff --git a/.cproject b/.cproject index af6b32cd2..a43d32f58 100644 --- a/.cproject +++ b/.cproject @@ -568,6 +568,7 @@ make + tests/testBayesTree.run true false @@ -575,6 +576,7 @@ make + testBinaryBayesNet.run true false @@ -622,6 +624,7 @@ make + testSymbolicBayesNet.run true false @@ -629,6 +632,7 @@ make + tests/testSymbolicFactor.run true false @@ -636,6 +640,7 @@ make + testSymbolicFactorGraph.run true false @@ -651,6 +656,7 @@ make + tests/testBayesTree true false @@ -728,46 +734,6 @@ true true - - make - -j5 - testValues.run - true - true - true - - - make - -j5 - testOrdering.run - true - true - true - - - make - -j5 - testKey.run - true - true - true - - - make - -j5 - testLinearContainerFactor.run - true - true - true - - - make - -j6 -j8 - testWhiteNoiseFactor.run - true - true - true - make -j2 @@ -1114,6 +1080,7 @@ make + testErrors.run true false @@ -1159,6 +1126,14 @@ true true + + make + -j5 + testParticleFactor.run + true + true + true + make -j2 @@ -1239,14 +1214,6 @@ true true - - make - -j5 - testParticleFactor.run - true - true - true - make -j2 @@ -1351,6 +1318,22 @@ true true + + make + -j5 + testImuFactor.run + true + true + true + + + make + -j5 + testCombinedImuFactor.run + true + true + true + make -j2 @@ -1433,7 +1416,6 @@ make - testSimulated2DOriented.run true false @@ -1473,7 +1455,6 @@ make - testSimulated2D.run true false @@ -1481,7 +1462,6 @@ make - testSimulated3D.run true false @@ -1495,22 +1475,6 @@ true true - - make - -j5 - testImuFactor.run - true - true - true - - - make - -j5 - testCombinedImuFactor.run - true - true - true - make -j5 @@ -1768,6 +1732,7 @@ cpack + -G DEB true false @@ -1775,6 +1740,7 @@ cpack + -G RPM true false @@ -1782,6 +1748,7 @@ cpack + -G TGZ true false @@ -1789,6 +1756,7 @@ cpack + --config CPackSourceConfig.cmake true false @@ -2547,6 +2515,7 @@ make + testGraph.run true false @@ -2554,6 +2523,7 @@ make + testJunctionTree.run true false @@ -2561,6 +2531,7 @@ make + testSymbolicBayesNetB.run true false @@ -2830,6 +2801,62 @@ true true + + make + -j5 + Pose2SLAMExample_lago.run + true + true + true + + + make + -j5 + Pose2SLAMExample_g2o.run + true + true + true + + + make + -j5 + testLago.run + true + true + true + + + make + -j5 + testLinearContainerFactor.run + true + true + true + + + make + -j5 + testOrdering.run + true + true + true + + + make + -j5 + testValues.run + true + true + true + + + make + -j5 + testWhiteNoiseFactor.run + true + true + true + make -j4 @@ -2848,7 +2875,6 @@ make - tests/testGaussianISAM2 true false diff --git a/examples/Pose2SLAMExample_g2o.cpp b/examples/Pose2SLAMExample_g2o.cpp index 393bba86d..565a26965 100644 --- a/examples/Pose2SLAMExample_g2o.cpp +++ b/examples/Pose2SLAMExample_g2o.cpp @@ -18,27 +18,22 @@ * @author Luca Carlone */ -#include -#include -#include #include -#include -#include +#include #include -#include -#include #include -#include using namespace std; using namespace gtsam; +int main(const int argc, const char *argv[]) { -int main(const int argc, const char *argv[]){ - + // Read graph from file + string g2oFile; if (argc < 2) - std::cout << "Please specify: 1st argument: input file (in g2o format) and 2nd argument: output file" << std::endl; - const string g2oFile = argv[1]; + g2oFile = "../../examples/Data/noisyToyGraph.txt"; + else + g2oFile = argv[1]; NonlinearFactorGraph graph; Values initial; @@ -46,7 +41,8 @@ int main(const int argc, const char *argv[]){ // Add prior on the pose having index (key) = 0 NonlinearFactorGraph graphWithPrior = graph; - noiseModel::Diagonal::shared_ptr priorModel = noiseModel::Diagonal::Variances((Vector(3) << 1e-6, 1e-6, 1e-8)); + noiseModel::Diagonal::shared_ptr priorModel = // + noiseModel::Diagonal::Variances((Vector(3) << 1e-6, 1e-6, 1e-8)); graphWithPrior.add(PriorFactor(0, Pose2(), priorModel)); std::cout << "Optimizing the factor graph" << std::endl; @@ -54,10 +50,13 @@ int main(const int argc, const char *argv[]){ Values result = optimizer.optimize(); std::cout << "Optimization complete" << std::endl; - const string outputFile = argv[2]; - std::cout << "Writing results to file: " << outputFile << std::endl; - writeG2o(outputFile, graph, result); - std::cout << "done! " << std::endl; - + if (argc < 3) { + result.print("result"); + } else { + const string outputFile = argv[2]; + std::cout << "Writing results to file: " << outputFile << std::endl; + writeG2o(outputFile, graph, result); + std::cout << "done! " << std::endl; + } return 0; } diff --git a/examples/Pose2SLAMExample_lago.cpp b/examples/Pose2SLAMExample_lago.cpp index d61f1b533..c84ea8c7c 100644 --- a/examples/Pose2SLAMExample_lago.cpp +++ b/examples/Pose2SLAMExample_lago.cpp @@ -12,33 +12,29 @@ /** * @file Pose2SLAMExample_lago.cpp * @brief A 2D Pose SLAM example that reads input from g2o, and solve the Pose2 problem - * using LAGO (Linear Approximation for Graph Optimization). See class LagoInitializer.h + * using LAGO (Linear Approximation for Graph Optimization). See class lago.h * Output is written on a file, in g2o format * Syntax for the script is ./Pose2SLAMExample_lago input.g2o output.g2o * @date May 15, 2014 * @author Luca Carlone */ -#include -#include -#include +#include #include -#include -#include -#include -#include +#include #include -#include using namespace std; using namespace gtsam; +int main(const int argc, const char *argv[]) { -int main(const int argc, const char *argv[]){ - + // Read graph from file + string g2oFile; if (argc < 2) - std::cout << "Please specify: 1st argument: input file (in g2o format) and 2nd argument: output file" << std::endl; - const string g2oFile = argv[1]; + g2oFile = "../../examples/Data/noisyToyGraph.txt"; + else + g2oFile = argv[1]; NonlinearFactorGraph graph; Values initial; @@ -46,17 +42,23 @@ int main(const int argc, const char *argv[]){ // Add prior on the pose having index (key) = 0 NonlinearFactorGraph graphWithPrior = graph; - noiseModel::Diagonal::shared_ptr priorModel = noiseModel::Diagonal::Variances((Vector(3) << 1e-6, 1e-6, 1e-8)); + noiseModel::Diagonal::shared_ptr priorModel = // + noiseModel::Diagonal::Variances((Vector(3) << 1e-6, 1e-6, 1e-8)); graphWithPrior.add(PriorFactor(0, Pose2(), priorModel)); + graphWithPrior.print(); std::cout << "Computing LAGO estimate" << std::endl; Values estimateLago = initializeLago(graphWithPrior); std::cout << "done!" << std::endl; - const string outputFile = argv[2]; - std::cout << "Writing results to file: " << outputFile << std::endl; - writeG2o(outputFile, graph, estimateLago); - std::cout << "done! " << std::endl; + if (argc < 3) { + estimateLago.print("estimateLago"); + } else { + const string outputFile = argv[2]; + std::cout << "Writing results to file: " << outputFile << std::endl; + writeG2o(outputFile, graph, estimateLago); + std::cout << "done! " << std::endl; + } return 0; } From acdc3304e05e50556746a01eed11a8a3ed5ae847 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sat, 31 May 2014 13:00:08 -0400 Subject: [PATCH 03/10] Cleaned up headers --- gtsam/nonlinear/lago.cpp | 5 ++++- gtsam/nonlinear/lago.h | 8 ++------ 2 files changed, 6 insertions(+), 7 deletions(-) diff --git a/gtsam/nonlinear/lago.cpp b/gtsam/nonlinear/lago.cpp index e2f13fded..08fa8175f 100644 --- a/gtsam/nonlinear/lago.cpp +++ b/gtsam/nonlinear/lago.cpp @@ -17,7 +17,10 @@ */ #include -#include +#include +#include +#include + #include namespace gtsam { diff --git a/gtsam/nonlinear/lago.h b/gtsam/nonlinear/lago.h index 91f6ecd97..4450e1108 100644 --- a/gtsam/nonlinear/lago.h +++ b/gtsam/nonlinear/lago.h @@ -34,15 +34,11 @@ #pragma once -#include -#include +#include +#include #include #include #include -#include -#include -#include -#include namespace gtsam { From fe33c80b5f7ad961af0fb5b1a905d01f9a9aaadf Mon Sep 17 00:00:00 2001 From: dellaert Date: Sat, 31 May 2014 13:04:15 -0400 Subject: [PATCH 04/10] Introduced namespace --- examples/Pose2SLAMExample_lago.cpp | 2 +- gtsam/nonlinear/lago.cpp | 2 ++ gtsam/nonlinear/lago.h | 2 ++ gtsam/nonlinear/tests/testLago.cpp | 1 + 4 files changed, 6 insertions(+), 1 deletion(-) diff --git a/examples/Pose2SLAMExample_lago.cpp b/examples/Pose2SLAMExample_lago.cpp index c84ea8c7c..75072d7da 100644 --- a/examples/Pose2SLAMExample_lago.cpp +++ b/examples/Pose2SLAMExample_lago.cpp @@ -48,7 +48,7 @@ int main(const int argc, const char *argv[]) { graphWithPrior.print(); std::cout << "Computing LAGO estimate" << std::endl; - Values estimateLago = initializeLago(graphWithPrior); + Values estimateLago = lago::initializeLago(graphWithPrior); std::cout << "done!" << std::endl; if (argc < 3) { diff --git a/gtsam/nonlinear/lago.cpp b/gtsam/nonlinear/lago.cpp index 08fa8175f..28de71b8b 100644 --- a/gtsam/nonlinear/lago.cpp +++ b/gtsam/nonlinear/lago.cpp @@ -24,6 +24,7 @@ #include namespace gtsam { +namespace lago { static Matrix I = eye(1); static Matrix I3 = eye(3); @@ -347,4 +348,5 @@ Values initializeLago(const NonlinearFactorGraph& graph, const Values& initialGu return initialGuessLago; } +} // end of namespace lago } // end of namespace gtsam diff --git a/gtsam/nonlinear/lago.h b/gtsam/nonlinear/lago.h index 4450e1108..85d9863b1 100644 --- a/gtsam/nonlinear/lago.h +++ b/gtsam/nonlinear/lago.h @@ -41,6 +41,7 @@ #include namespace gtsam { +namespace lago { typedef std::map key2doubleMap; const Key keyAnchor = symbol('Z',9999999); @@ -93,4 +94,5 @@ GTSAM_EXPORT Values initializeLago(const NonlinearFactorGraph& graph, bool useOd /* Only corrects the orientation part in initialGuess */ GTSAM_EXPORT Values initializeLago(const NonlinearFactorGraph& graph, const Values& initialGuess); +} // end of namespace lago } // end of namespace gtsam diff --git a/gtsam/nonlinear/tests/testLago.cpp b/gtsam/nonlinear/tests/testLago.cpp index 576266f22..29dbcc720 100644 --- a/gtsam/nonlinear/tests/testLago.cpp +++ b/gtsam/nonlinear/tests/testLago.cpp @@ -37,6 +37,7 @@ using namespace std; using namespace gtsam; +using namespace lago; using namespace boost::assign; Symbol x0('x', 0), x1('x', 1), x2('x', 2), x3('x', 3); From 1d43a1f2065baf64870b64c74e5e0ef31dc73b12 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sat, 31 May 2014 13:13:14 -0400 Subject: [PATCH 05/10] removed redundant "Lago" from several function names --- examples/Pose2SLAMExample_lago.cpp | 2 +- gtsam/nonlinear/lago.cpp | 26 +++++++----- gtsam/nonlinear/lago.h | 11 ++--- gtsam/nonlinear/tests/testLago.cpp | 68 +++++++++++++++--------------- 4 files changed, 54 insertions(+), 53 deletions(-) diff --git a/examples/Pose2SLAMExample_lago.cpp b/examples/Pose2SLAMExample_lago.cpp index 75072d7da..a6340caf7 100644 --- a/examples/Pose2SLAMExample_lago.cpp +++ b/examples/Pose2SLAMExample_lago.cpp @@ -48,7 +48,7 @@ int main(const int argc, const char *argv[]) { graphWithPrior.print(); std::cout << "Computing LAGO estimate" << std::endl; - Values estimateLago = lago::initializeLago(graphWithPrior); + Values estimateLago = lago::initialize(graphWithPrior); std::cout << "done!" << std::endl; if (argc < 3) { diff --git a/gtsam/nonlinear/lago.cpp b/gtsam/nonlinear/lago.cpp index 28de71b8b..4f838ca96 100644 --- a/gtsam/nonlinear/lago.cpp +++ b/gtsam/nonlinear/lago.cpp @@ -26,8 +26,12 @@ namespace gtsam { namespace lago { -static Matrix I = eye(1); -static Matrix I3 = eye(3); +static const Matrix I = eye(1); +static const Matrix I3 = eye(3); + +static const Key keyAnchor = symbol('Z',9999999); +static const noiseModel::Diagonal::shared_ptr priorOrientationNoise = noiseModel::Diagonal::Variances((Vector(1) << 1e-8)); +static const noiseModel::Diagonal::shared_ptr priorPose2Noise = noiseModel::Diagonal::Variances((Vector(3) << 1e-6, 1e-6, 1e-8)); /* ************************************************************************* */ double computeThetaToRoot(const Key nodeKey, const PredecessorMap& tree, @@ -216,7 +220,7 @@ PredecessorMap findOdometricPath(const NonlinearFactorGraph& pose2Graph) { } /* ************************************************************************* */ -VectorValues computeLagoOrientations(const NonlinearFactorGraph& pose2Graph, bool useOdometricPath){ +VectorValues computeOrientations(const NonlinearFactorGraph& pose2Graph, bool useOdometricPath){ // Find a minimum spanning tree PredecessorMap tree; @@ -244,18 +248,18 @@ VectorValues computeLagoOrientations(const NonlinearFactorGraph& pose2Graph, boo } /* ************************************************************************* */ -VectorValues initializeOrientationsLago(const NonlinearFactorGraph& graph, bool useOdometricPath) { +VectorValues initializeOrientations(const NonlinearFactorGraph& graph, bool useOdometricPath) { // We "extract" the Pose2 subgraph of the original graph: this // is done to properly model priors and avoiding operating on a larger graph NonlinearFactorGraph pose2Graph = buildPose2graph(graph); // Get orientations from relative orientation measurements - return computeLagoOrientations(pose2Graph, useOdometricPath); + return computeOrientations(pose2Graph, useOdometricPath); } /* ************************************************************************* */ -Values computeLagoPoses(const NonlinearFactorGraph& pose2graph, VectorValues& orientationsLago) { +Values computePoses(const NonlinearFactorGraph& pose2graph, VectorValues& orientationsLago) { // Linearized graph on full poses GaussianFactorGraph linearPose2graph; @@ -315,25 +319,25 @@ Values computeLagoPoses(const NonlinearFactorGraph& pose2graph, VectorValues& or } /* ************************************************************************* */ -Values initializeLago(const NonlinearFactorGraph& graph, bool useOdometricPath) { +Values initialize(const NonlinearFactorGraph& graph, bool useOdometricPath) { // We "extract" the Pose2 subgraph of the original graph: this // is done to properly model priors and avoiding operating on a larger graph NonlinearFactorGraph pose2Graph = buildPose2graph(graph); // Get orientations from relative orientation measurements - VectorValues orientationsLago = computeLagoOrientations(pose2Graph, useOdometricPath); + VectorValues orientationsLago = computeOrientations(pose2Graph, useOdometricPath); // Compute the full poses - return computeLagoPoses(pose2Graph, orientationsLago); + return computePoses(pose2Graph, orientationsLago); } /* ************************************************************************* */ -Values initializeLago(const NonlinearFactorGraph& graph, const Values& initialGuess) { +Values initialize(const NonlinearFactorGraph& graph, const Values& initialGuess) { Values initialGuessLago; // get the orientation estimates from LAGO - VectorValues orientations = initializeOrientationsLago(graph); + VectorValues orientations = initializeOrientations(graph); // for all nodes in the tree for(VectorValues::const_iterator it = orientations.begin(); it != orientations.end(); ++it ){ diff --git a/gtsam/nonlinear/lago.h b/gtsam/nonlinear/lago.h index 85d9863b1..094b4325d 100644 --- a/gtsam/nonlinear/lago.h +++ b/gtsam/nonlinear/lago.h @@ -44,9 +44,6 @@ namespace gtsam { namespace lago { typedef std::map key2doubleMap; -const Key keyAnchor = symbol('Z',9999999); -noiseModel::Diagonal::shared_ptr priorOrientationNoise = noiseModel::Diagonal::Variances((Vector(1) << 1e-8)); -noiseModel::Diagonal::shared_ptr priorPose2Noise = noiseModel::Diagonal::Variances((Vector(3) << 1e-6, 1e-6, 1e-8)); /* This function computes the cumulative orientation (without wrapping) wrt the root of a spanning tree (tree) * for a node (nodeKey). The function starts at the nodes and moves towards the root @@ -83,16 +80,16 @@ GTSAM_EXPORT GaussianFactorGraph buildLinearOrientationGraph(const std::vector */ -GTSAM_EXPORT VectorValues computeLagoOrientations(const NonlinearFactorGraph& pose2Graph, bool useOdometricPath = true); +GTSAM_EXPORT VectorValues computeOrientations(const NonlinearFactorGraph& pose2Graph, bool useOdometricPath = true); /* LAGO: Returns the orientations of the Pose2 in a generic factor graph */ -GTSAM_EXPORT VectorValues initializeOrientationsLago(const NonlinearFactorGraph& graph, bool useOdometricPath = true); +GTSAM_EXPORT VectorValues initializeOrientations(const NonlinearFactorGraph& graph, bool useOdometricPath = true); /* Returns the values for the Pose2 in a generic factor graph */ -GTSAM_EXPORT Values initializeLago(const NonlinearFactorGraph& graph, bool useOdometricPath = true); +GTSAM_EXPORT Values initialize(const NonlinearFactorGraph& graph, bool useOdometricPath = true); /* Only corrects the orientation part in initialGuess */ -GTSAM_EXPORT Values initializeLago(const NonlinearFactorGraph& graph, const Values& initialGuess); +GTSAM_EXPORT Values initialize(const NonlinearFactorGraph& graph, const Values& initialGuess); } // end of namespace lago } // end of namespace gtsam diff --git a/gtsam/nonlinear/tests/testLago.cpp b/gtsam/nonlinear/tests/testLago.cpp index 29dbcc720..867e44089 100644 --- a/gtsam/nonlinear/tests/testLago.cpp +++ b/gtsam/nonlinear/tests/testLago.cpp @@ -148,25 +148,25 @@ TEST( Lago, regularizedMeasurements ) { /* *************************************************************************** */ TEST( Lago, smallGraphVectorValues ) { bool useOdometricPath = false; - VectorValues initialGuessLago = initializeOrientationsLago(simple::graph(), useOdometricPath); + VectorValues initial = initializeOrientations(simple::graph(), useOdometricPath); // 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) << 0.0), initial.at(x0), 1e-6)); + EXPECT(assert_equal((Vector(1) << 0.5 * M_PI), initial.at(x1), 1e-6)); + EXPECT(assert_equal((Vector(1) << M_PI - 2*M_PI), initial.at(x2), 1e-6)); + EXPECT(assert_equal((Vector(1) << 1.5 * M_PI - 2*M_PI), initial.at(x3), 1e-6)); } /* *************************************************************************** */ TEST( Lago, smallGraphVectorValuesSP ) { - VectorValues initialGuessLago = initializeOrientationsLago(simple::graph()); + VectorValues initial = initializeOrientations(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)); + EXPECT(assert_equal((Vector(1) << 0.0), initial.at(x0), 1e-6)); + EXPECT(assert_equal((Vector(1) << 0.5 * M_PI), initial.at(x1), 1e-6)); + EXPECT(assert_equal((Vector(1) << M_PI ), initial.at(x2), 1e-6)); + EXPECT(assert_equal((Vector(1) << 1.5 * M_PI ), initial.at(x3), 1e-6)); } /* *************************************************************************** */ @@ -174,26 +174,26 @@ TEST( Lago, multiplePosePriors ) { bool useOdometricPath = false; NonlinearFactorGraph g = simple::graph(); g.add(PriorFactor(x1, simple::pose1, model)); - VectorValues initialGuessLago = initializeOrientationsLago(g, useOdometricPath); + VectorValues initial = initializeOrientations(g, useOdometricPath); // 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) << 0.0), initial.at(x0), 1e-6)); + EXPECT(assert_equal((Vector(1) << 0.5 * M_PI), initial.at(x1), 1e-6)); + EXPECT(assert_equal((Vector(1) << M_PI - 2*M_PI), initial.at(x2), 1e-6)); + EXPECT(assert_equal((Vector(1) << 1.5 * M_PI - 2*M_PI), initial.at(x3), 1e-6)); } /* *************************************************************************** */ TEST( Lago, multiplePosePriorsSP ) { NonlinearFactorGraph g = simple::graph(); g.add(PriorFactor(x1, simple::pose1, model)); - VectorValues initialGuessLago = initializeOrientationsLago(g); + VectorValues initial = initializeOrientations(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)); + EXPECT(assert_equal((Vector(1) << 0.0), initial.at(x0), 1e-6)); + EXPECT(assert_equal((Vector(1) << 0.5 * M_PI), initial.at(x1), 1e-6)); + EXPECT(assert_equal((Vector(1) << M_PI ), initial.at(x2), 1e-6)); + EXPECT(assert_equal((Vector(1) << 1.5 * M_PI ), initial.at(x3), 1e-6)); } /* *************************************************************************** */ @@ -201,26 +201,26 @@ TEST( Lago, multiplePoseAndRotPriors ) { bool useOdometricPath = false; NonlinearFactorGraph g = simple::graph(); g.add(PriorFactor(x1, simple::pose1.theta(), model)); - VectorValues initialGuessLago = initializeOrientationsLago(g, useOdometricPath); + VectorValues initial = initializeOrientations(g, useOdometricPath); // 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) << 0.0), initial.at(x0), 1e-6)); + EXPECT(assert_equal((Vector(1) << 0.5 * M_PI), initial.at(x1), 1e-6)); + EXPECT(assert_equal((Vector(1) << M_PI - 2*M_PI), initial.at(x2), 1e-6)); + EXPECT(assert_equal((Vector(1) << 1.5 * M_PI - 2*M_PI), initial.at(x3), 1e-6)); } /* *************************************************************************** */ TEST( Lago, multiplePoseAndRotPriorsSP ) { NonlinearFactorGraph g = simple::graph(); g.add(PriorFactor(x1, simple::pose1.theta(), model)); - VectorValues initialGuessLago = initializeOrientationsLago(g); + VectorValues initial = initializeOrientations(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)); + EXPECT(assert_equal((Vector(1) << 0.0), initial.at(x0), 1e-6)); + EXPECT(assert_equal((Vector(1) << 0.5 * M_PI), initial.at(x1), 1e-6)); + EXPECT(assert_equal((Vector(1) << M_PI ), initial.at(x2), 1e-6)); + EXPECT(assert_equal((Vector(1) << 1.5 * M_PI ), initial.at(x3), 1e-6)); } /* *************************************************************************** */ @@ -234,7 +234,7 @@ TEST( Lago, smallGraphValues ) { initialGuess.insert(x3,Pose2(simple::pose3.x(),simple::pose3.y(),0.0)); // lago does not touch the Cartesian part and only fixed the orientations - Values actual = initializeLago(simple::graph(), initialGuess); + Values actual = lago::initialize(simple::graph(), initialGuess); // we are in a noiseless case Values expected; @@ -250,7 +250,7 @@ TEST( Lago, smallGraphValues ) { TEST( Lago, smallGraph2 ) { // lago does not touch the Cartesian part and only fixed the orientations - Values actual = initializeLago(simple::graph()); + Values actual = lago::initialize(simple::graph()); // we are in a noiseless case Values expected; @@ -275,7 +275,7 @@ TEST( Lago, largeGraphNoisy_orientations ) { noiseModel::Diagonal::shared_ptr priorModel = noiseModel::Diagonal::Variances((Vector(3) << 1e-2, 1e-2, 1e-4)); graphWithPrior.add(PriorFactor(0, Pose2(), priorModel)); - VectorValues actualVV = initializeOrientationsLago(graphWithPrior); + VectorValues actualVV = initializeOrientations(graphWithPrior); Values actual; Key keyAnc = symbol('Z',9999999); for(VectorValues::const_iterator it = actualVV.begin(); it != actualVV.end(); ++it ){ @@ -310,7 +310,7 @@ TEST( Lago, largeGraphNoisy ) { noiseModel::Diagonal::shared_ptr priorModel = noiseModel::Diagonal::Variances((Vector(3) << 1e-2, 1e-2, 1e-4)); graphWithPrior.add(PriorFactor(0, Pose2(), priorModel)); - Values actual = initializeLago(graphWithPrior); + Values actual = lago::initialize(graphWithPrior); NonlinearFactorGraph gmatlab; Values expected; From 9b8af1bf6efeb36fa5e14157cc73c7bbfd53cdea Mon Sep 17 00:00:00 2001 From: dellaert Date: Sat, 31 May 2014 13:15:51 -0400 Subject: [PATCH 06/10] Use namespace explicitly --- gtsam/nonlinear/tests/testLago.cpp | 37 +++++++++++++++--------------- 1 file changed, 18 insertions(+), 19 deletions(-) diff --git a/gtsam/nonlinear/tests/testLago.cpp b/gtsam/nonlinear/tests/testLago.cpp index 867e44089..4b116a108 100644 --- a/gtsam/nonlinear/tests/testLago.cpp +++ b/gtsam/nonlinear/tests/testLago.cpp @@ -37,7 +37,6 @@ using namespace std; using namespace gtsam; -using namespace lago; using namespace boost::assign; Symbol x0('x', 0), x1('x', 1), x2('x', 2), x3('x', 3); @@ -78,10 +77,10 @@ TEST( Lago, checkSTandChords ) { PredecessorMap tree = findMinimumSpanningTree >(g); - key2doubleMap deltaThetaMap; + lago::key2doubleMap deltaThetaMap; vector spanningTreeIds; // ids of between factors forming the spanning tree T vector chordsIds; // ids of between factors corresponding to chordsIds wrt T - getSymbolicGraph(spanningTreeIds, chordsIds, deltaThetaMap, tree, g); + lago::getSymbolicGraph(spanningTreeIds, chordsIds, deltaThetaMap, tree, g); DOUBLES_EQUAL(spanningTreeIds[0], 0, 1e-6); // factor 0 is the first in the ST (0->1) DOUBLES_EQUAL(spanningTreeIds[1], 3, 1e-6); // factor 3 is the second in the ST(2->0) @@ -101,19 +100,19 @@ TEST( Lago, orientationsOverSpanningTree ) { EXPECT_LONGS_EQUAL(tree[x2], x0); EXPECT_LONGS_EQUAL(tree[x3], x0); - key2doubleMap expected; + lago::key2doubleMap expected; expected[x0]= 0; expected[x1]= M_PI/2; // edge x0->x1 (consistent with edge (x0,x1)) expected[x2]= -M_PI; // edge x0->x2 (traversed backwards wrt edge (x2,x0)) expected[x3]= -M_PI/2; // edge x0->x3 (consistent with edge (x0,x3)) - key2doubleMap deltaThetaMap; + lago::key2doubleMap deltaThetaMap; vector spanningTreeIds; // ids of between factors forming the spanning tree T vector chordsIds; // ids of between factors corresponding to chordsIds wrt T - getSymbolicGraph(spanningTreeIds, chordsIds, deltaThetaMap, tree, g); + lago::getSymbolicGraph(spanningTreeIds, chordsIds, deltaThetaMap, tree, g); - key2doubleMap actual; - actual = computeThetasToRoot(deltaThetaMap, tree); + lago::key2doubleMap actual; + actual = lago::computeThetasToRoot(deltaThetaMap, tree); DOUBLES_EQUAL(expected[x0], actual[x0], 1e-6); DOUBLES_EQUAL(expected[x1], actual[x1], 1e-6); DOUBLES_EQUAL(expected[x2], actual[x2], 1e-6); @@ -126,14 +125,14 @@ TEST( Lago, regularizedMeasurements ) { PredecessorMap tree = findMinimumSpanningTree >(g); - key2doubleMap deltaThetaMap; + lago::key2doubleMap deltaThetaMap; vector spanningTreeIds; // ids of between factors forming the spanning tree T vector chordsIds; // ids of between factors corresponding to chordsIds wrt T - getSymbolicGraph(spanningTreeIds, chordsIds, deltaThetaMap, tree, g); + lago::getSymbolicGraph(spanningTreeIds, chordsIds, deltaThetaMap, tree, g); - key2doubleMap orientationsToRoot = computeThetasToRoot(deltaThetaMap, tree); + lago::key2doubleMap orientationsToRoot = lago::computeThetasToRoot(deltaThetaMap, tree); - GaussianFactorGraph lagoGraph = buildLinearOrientationGraph(spanningTreeIds, chordsIds, g, orientationsToRoot, tree); + GaussianFactorGraph lagoGraph = lago::buildLinearOrientationGraph(spanningTreeIds, chordsIds, g, orientationsToRoot, tree); std::pair actualAb = lagoGraph.jacobian(); // jacobian corresponding to the orientation measurements (last entry is the prior on the anchor and is disregarded) Vector actual = (Vector(5) << actualAb.second(0),actualAb.second(1),actualAb.second(2),actualAb.second(3),actualAb.second(4)); @@ -148,7 +147,7 @@ TEST( Lago, regularizedMeasurements ) { /* *************************************************************************** */ TEST( Lago, smallGraphVectorValues ) { bool useOdometricPath = false; - VectorValues initial = initializeOrientations(simple::graph(), useOdometricPath); + VectorValues initial = lago::initializeOrientations(simple::graph(), useOdometricPath); // comparison is up to M_PI, that's why we add some multiples of 2*M_PI EXPECT(assert_equal((Vector(1) << 0.0), initial.at(x0), 1e-6)); @@ -160,7 +159,7 @@ TEST( Lago, smallGraphVectorValues ) { /* *************************************************************************** */ TEST( Lago, smallGraphVectorValuesSP ) { - VectorValues initial = initializeOrientations(simple::graph()); + VectorValues initial = lago::initializeOrientations(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), initial.at(x0), 1e-6)); @@ -174,7 +173,7 @@ TEST( Lago, multiplePosePriors ) { bool useOdometricPath = false; NonlinearFactorGraph g = simple::graph(); g.add(PriorFactor(x1, simple::pose1, model)); - VectorValues initial = initializeOrientations(g, useOdometricPath); + VectorValues initial = lago::initializeOrientations(g, useOdometricPath); // comparison is up to M_PI, that's why we add some multiples of 2*M_PI EXPECT(assert_equal((Vector(1) << 0.0), initial.at(x0), 1e-6)); @@ -187,7 +186,7 @@ TEST( Lago, multiplePosePriors ) { TEST( Lago, multiplePosePriorsSP ) { NonlinearFactorGraph g = simple::graph(); g.add(PriorFactor(x1, simple::pose1, model)); - VectorValues initial = initializeOrientations(g); + VectorValues initial = lago::initializeOrientations(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), initial.at(x0), 1e-6)); @@ -201,7 +200,7 @@ TEST( Lago, multiplePoseAndRotPriors ) { bool useOdometricPath = false; NonlinearFactorGraph g = simple::graph(); g.add(PriorFactor(x1, simple::pose1.theta(), model)); - VectorValues initial = initializeOrientations(g, useOdometricPath); + VectorValues initial = lago::initializeOrientations(g, useOdometricPath); // comparison is up to M_PI, that's why we add some multiples of 2*M_PI EXPECT(assert_equal((Vector(1) << 0.0), initial.at(x0), 1e-6)); @@ -214,7 +213,7 @@ TEST( Lago, multiplePoseAndRotPriors ) { TEST( Lago, multiplePoseAndRotPriorsSP ) { NonlinearFactorGraph g = simple::graph(); g.add(PriorFactor(x1, simple::pose1.theta(), model)); - VectorValues initial = initializeOrientations(g); + VectorValues initial = lago::initializeOrientations(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), initial.at(x0), 1e-6)); @@ -275,7 +274,7 @@ TEST( Lago, largeGraphNoisy_orientations ) { noiseModel::Diagonal::shared_ptr priorModel = noiseModel::Diagonal::Variances((Vector(3) << 1e-2, 1e-2, 1e-4)); graphWithPrior.add(PriorFactor(0, Pose2(), priorModel)); - VectorValues actualVV = initializeOrientations(graphWithPrior); + VectorValues actualVV = lago::initializeOrientations(graphWithPrior); Values actual; Key keyAnc = symbol('Z',9999999); for(VectorValues::const_iterator it = actualVV.begin(); it != actualVV.end(); ++it ){ From dfb620b5decef984927a39cbdc10ed74b72e0ae4 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sat, 31 May 2014 13:21:34 -0400 Subject: [PATCH 07/10] Header discipline --- gtsam/nonlinear/lago.cpp | 1 + gtsam/nonlinear/lago.h | 1 - gtsam/nonlinear/tests/testLago.cpp | 16 +++++----------- 3 files changed, 6 insertions(+), 12 deletions(-) diff --git a/gtsam/nonlinear/lago.cpp b/gtsam/nonlinear/lago.cpp index 4f838ca96..792dad80c 100644 --- a/gtsam/nonlinear/lago.cpp +++ b/gtsam/nonlinear/lago.cpp @@ -19,6 +19,7 @@ #include #include #include +#include #include #include diff --git a/gtsam/nonlinear/lago.h b/gtsam/nonlinear/lago.h index 094b4325d..8c8ef9a73 100644 --- a/gtsam/nonlinear/lago.h +++ b/gtsam/nonlinear/lago.h @@ -34,7 +34,6 @@ #pragma once -#include #include #include #include diff --git a/gtsam/nonlinear/tests/testLago.cpp b/gtsam/nonlinear/tests/testLago.cpp index 4b116a108..f2cab4506 100644 --- a/gtsam/nonlinear/tests/testLago.cpp +++ b/gtsam/nonlinear/tests/testLago.cpp @@ -19,20 +19,14 @@ * @date May 14, 2014 */ -#include - +#include +#include +#include +#include #include -#include -#include -#include - -#include -#include - -#include #include -#include + #include using namespace std; From e5344d3d92abbd6f15514d58b8dfd683e4985a31 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sat, 31 May 2014 13:26:13 -0400 Subject: [PATCH 08/10] Doxygen documentation --- gtsam/nonlinear/lago.h | 79 +++++++++++++++++++++++++----------------- 1 file changed, 48 insertions(+), 31 deletions(-) diff --git a/gtsam/nonlinear/lago.h b/gtsam/nonlinear/lago.h index 8c8ef9a73..fa8eef995 100644 --- a/gtsam/nonlinear/lago.h +++ b/gtsam/nonlinear/lago.h @@ -42,53 +42,70 @@ namespace gtsam { namespace lago { -typedef std::map key2doubleMap; +typedef std::map key2doubleMap; -/* This function computes the cumulative orientation (without wrapping) wrt the root of a spanning tree (tree) - * for a node (nodeKey). The function starts at the nodes and moves towards the root - * summing up the (directed) rotation measurements. Relative measurements are encoded in "deltaThetaMap" - * The root is assumed to have orientation zero. +/** + * Compute the cumulative orientation (without wrapping) wrt the root of a + * spanning tree (tree) for a node (nodeKey). The function starts at the nodes and + * moves towards the root summing up the (directed) rotation measurements. + * Relative measurements are encoded in "deltaThetaMap". + * The root is assumed to have orientation zero. */ -GTSAM_EXPORT double computeThetaToRoot(const Key nodeKey, const PredecessorMap& tree, - const key2doubleMap& deltaThetaMap, const key2doubleMap& thetaFromRootMap); +GTSAM_EXPORT double computeThetaToRoot(const Key nodeKey, + const PredecessorMap& tree, const key2doubleMap& deltaThetaMap, + const key2doubleMap& thetaFromRootMap); -/* This function computes the cumulative orientations (without wrapping) - * for all node wrt the root (root has zero orientation) +/** + * Compute the cumulative orientations (without wrapping) + * for all nodes wrt the root (root has zero orientation). */ -GTSAM_EXPORT key2doubleMap computeThetasToRoot(const key2doubleMap& deltaThetaMap, - const PredecessorMap& tree); +GTSAM_EXPORT key2doubleMap computeThetasToRoot( + const key2doubleMap& deltaThetaMap, const PredecessorMap& tree); -/* Given a factor graph "g", and a spanning tree "tree", the function selects the nodes belonging to the tree and to g, - * and stores the factor slots corresponding to edges in the tree and to chordsIds wrt this tree - * Also it computes deltaThetaMap which is a fast way to encode relative orientations along the tree: - * for a node key2, s.t. tree[key2]=key1, the values deltaThetaMap[key2] is the relative orientation theta[key2]-theta[key1] +/** + * Given a factor graph "g", and a spanning tree "tree", select the nodes belonging + * to the tree and to g, and stores the factor slots corresponding to edges in the + * tree and to chordsIds wrt this tree. + * Also it computes deltaThetaMap which is a fast way to encode relative + * orientations along the tree: for a node key2, s.t. tree[key2]=key1, + * the value deltaThetaMap[key2] is relative orientation theta[key2]-theta[key1] */ GTSAM_EXPORT void getSymbolicGraph( - /*OUTPUTS*/ std::vector& spanningTreeIds, std::vector& chordsIds, key2doubleMap& deltaThetaMap, - /*INPUTS*/ const PredecessorMap& tree, const NonlinearFactorGraph& g); +/*OUTPUTS*/std::vector& spanningTreeIds, std::vector& chordsIds, + key2doubleMap& deltaThetaMap, + /*INPUTS*/const PredecessorMap& tree, const NonlinearFactorGraph& g); -/* Retrieves the deltaTheta and the corresponding noise model from a BetweenFactor */ +/** + * Retrieve the deltaTheta and the corresponding noise model from a BetweenFactor + */ GTSAM_EXPORT void getDeltaThetaAndNoise(NonlinearFactor::shared_ptr factor, Vector& deltaTheta, noiseModel::Diagonal::shared_ptr& model_deltaTheta); -/* Linear factor graph with regularized orientation measurements */ -GTSAM_EXPORT GaussianFactorGraph buildLinearOrientationGraph(const std::vector& spanningTreeIds, const std::vector& chordsIds, - const NonlinearFactorGraph& g, const key2doubleMap& orientationsToRoot, const PredecessorMap& tree); +/** Linear factor graph with regularized orientation measurements */ +GTSAM_EXPORT GaussianFactorGraph buildLinearOrientationGraph( + const std::vector& spanningTreeIds, + const std::vector& chordsIds, const NonlinearFactorGraph& g, + const key2doubleMap& orientationsToRoot, const PredecessorMap& tree); -/* Selects the subgraph of betweenFactors and transforms priors into between wrt a fictitious node */ -GTSAM_EXPORT NonlinearFactorGraph buildPose2graph(const NonlinearFactorGraph& graph); +/** Select the subgraph of betweenFactors and transforms priors into between wrt a fictitious node */ +GTSAM_EXPORT NonlinearFactorGraph buildPose2graph( + const NonlinearFactorGraph& graph); -/* Returns the orientations of a graph including only BetweenFactors */ -GTSAM_EXPORT VectorValues computeOrientations(const NonlinearFactorGraph& pose2Graph, bool useOdometricPath = true); +/** Return the orientations of a graph including only BetweenFactors */ +GTSAM_EXPORT VectorValues computeOrientations( + const NonlinearFactorGraph& pose2Graph, bool useOdometricPath = true); -/* LAGO: Returns the orientations of the Pose2 in a generic factor graph */ -GTSAM_EXPORT VectorValues initializeOrientations(const NonlinearFactorGraph& graph, bool useOdometricPath = true); +/** LAGO: Return the orientations of the Pose2 in a generic factor graph */ +GTSAM_EXPORT VectorValues initializeOrientations( + const NonlinearFactorGraph& graph, bool useOdometricPath = true); -/* Returns the values for the Pose2 in a generic factor graph */ -GTSAM_EXPORT Values initialize(const NonlinearFactorGraph& graph, bool useOdometricPath = true); +/** Return the values for the Pose2 in a generic factor graph */ +GTSAM_EXPORT Values initialize(const NonlinearFactorGraph& graph, + bool useOdometricPath = true); -/* Only corrects the orientation part in initialGuess */ -GTSAM_EXPORT Values initialize(const NonlinearFactorGraph& graph, const Values& initialGuess); +/** Only correct the orientation part in initialGuess */ +GTSAM_EXPORT Values initialize(const NonlinearFactorGraph& graph, + const Values& initialGuess); } // end of namespace lago } // end of namespace gtsam From 970d49f60b273b7e180931f966bacf1d118ae364 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sat, 31 May 2014 13:37:10 -0400 Subject: [PATCH 09/10] Standard formatting and some BOOST_FOREACH uses --- gtsam/nonlinear/lago.cpp | 205 ++++++++++++++++++++++----------------- 1 file changed, 118 insertions(+), 87 deletions(-) diff --git a/gtsam/nonlinear/lago.cpp b/gtsam/nonlinear/lago.cpp index 792dad80c..f8b089539 100644 --- a/gtsam/nonlinear/lago.cpp +++ b/gtsam/nonlinear/lago.cpp @@ -30,9 +30,11 @@ namespace lago { static const Matrix I = eye(1); static const Matrix I3 = eye(3); -static const Key keyAnchor = symbol('Z',9999999); -static const noiseModel::Diagonal::shared_ptr priorOrientationNoise = noiseModel::Diagonal::Variances((Vector(1) << 1e-8)); -static const noiseModel::Diagonal::shared_ptr priorPose2Noise = noiseModel::Diagonal::Variances((Vector(3) << 1e-6, 1e-6, 1e-8)); +static const Key keyAnchor = symbol('Z', 9999999); +static const noiseModel::Diagonal::shared_ptr priorOrientationNoise = + noiseModel::Diagonal::Variances((Vector(1) << 1e-8)); +static const noiseModel::Diagonal::shared_ptr priorPose2Noise = + noiseModel::Diagonal::Variances((Vector(3) << 1e-6, 1e-6, 1e-8)); /* ************************************************************************* */ double computeThetaToRoot(const Key nodeKey, const PredecessorMap& tree, @@ -41,16 +43,16 @@ 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 - while(1){ + while (1) { // We check if we reached the root - if(tree.at(key_child)==key_child) // if we reached the root + if (tree.at(key_child) == key_child) // if we reached the root break; // we sum the delta theta corresponding to the edge parent->child nodeTheta += deltaThetaMap.at(key_child); // we get the parent key_parent = tree.at(key_child); // the parent // we check if we connected to some part of the tree we know - if(thetaFromRootMap.find(key_parent)!=thetaFromRootMap.end()){ + if (thetaFromRootMap.find(key_parent) != thetaFromRootMap.end()) { nodeTheta += thetaFromRootMap.at(key_parent); break; } @@ -64,15 +66,14 @@ key2doubleMap computeThetasToRoot(const key2doubleMap& deltaThetaMap, const PredecessorMap& tree) { 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 ){ + BOOST_FOREACH(const key2doubleMap::value_type& it, deltaThetaMap) { // compute the orientation wrt root - Key nodeKey = it->first; + Key nodeKey = it.first; double nodeTheta = computeThetaToRoot(nodeKey, tree, deltaThetaMap, thetaToRootMap); thetaToRootMap.insert(std::pair(nodeKey, nodeTheta)); @@ -82,35 +83,38 @@ key2doubleMap computeThetasToRoot(const key2doubleMap& deltaThetaMap, /* ************************************************************************* */ void getSymbolicGraph( - /*OUTPUTS*/ std::vector& spanningTreeIds, std::vector& chordsIds, key2doubleMap& deltaThetaMap, - /*INPUTS*/ const PredecessorMap& tree, const NonlinearFactorGraph& g){ +/*OUTPUTS*/std::vector& spanningTreeIds, std::vector& chordsIds, + key2doubleMap& deltaThetaMap, + /*INPUTS*/const PredecessorMap& tree, const NonlinearFactorGraph& g) { // Get keys for which you want the orientation - size_t id=0; + size_t id = 0; // Loop over the factors - BOOST_FOREACH(const boost::shared_ptr& factor, g){ - if (factor->keys().size() == 2){ + BOOST_FOREACH(const boost::shared_ptr& factor, g) { + if (factor->keys().size() == 2) { Key key1 = factor->keys()[0]; Key key2 = factor->keys()[1]; // recast to a between - boost::shared_ptr< BetweenFactor > pose2Between = - boost::dynamic_pointer_cast< BetweenFactor >(factor); - if (!pose2Between) continue; + boost::shared_ptr > pose2Between = + boost::dynamic_pointer_cast >(factor); + if (!pose2Between) + continue; // get the orientation - measured().theta(); double deltaTheta = pose2Between->measured().theta(); // insert (directed) orientations in the map "deltaThetaMap" - bool inTree=false; - if(tree.at(key1)==key2){ // key2 -> key1 + bool inTree = false; + if (tree.at(key1) == key2) { // key2 -> key1 deltaThetaMap.insert(std::pair(key1, -deltaTheta)); inTree = true; - } else if(tree.at(key2)==key1){ // key1 -> key2 - deltaThetaMap.insert(std::pair(key2, deltaTheta)); + } else if (tree.at(key2) == key1) { // key1 -> key2 + deltaThetaMap.insert(std::pair(key2, deltaTheta)); inTree = true; } // store factor slot, distinguishing spanning tree edges from chordsIds - if(inTree == true) + if (inTree == true) spanningTreeIds.push_back(id); - else // it's a chord! + else + // it's a chord! chordsIds.push_back(id); } id++; @@ -125,7 +129,8 @@ void getDeltaThetaAndNoise(NonlinearFactor::shared_ptr factor, boost::shared_ptr > pose2Between = boost::dynamic_pointer_cast >(factor); if (!pose2Between) - throw std::invalid_argument("buildLinearOrientationGraph: invalid between factor!"); + throw std::invalid_argument( + "buildLinearOrientationGraph: invalid between factor!"); deltaTheta = (Vector(1) << pose2Between->measured().theta()); // Retrieve the noise model for the relative rotation @@ -133,63 +138,73 @@ void getDeltaThetaAndNoise(NonlinearFactor::shared_ptr factor, boost::shared_ptr diagonalModel = boost::dynamic_pointer_cast(model); if (!diagonalModel) - throw std::invalid_argument("buildLinearOrientationGraph: invalid noise model " - "(current version assumes diagonal noise model)!"); - Vector std_deltaTheta = (Vector(1) << diagonalModel->sigma(2) ); // std on the angular measurement + throw std::invalid_argument( + "buildLinearOrientationGraph: invalid noise model " + "(current version assumes diagonal noise model)!"); + Vector std_deltaTheta = (Vector(1) << diagonalModel->sigma(2)); // std on the angular measurement model_deltaTheta = noiseModel::Diagonal::Sigmas(std_deltaTheta); } /* ************************************************************************* */ -GaussianFactorGraph buildLinearOrientationGraph(const std::vector& spanningTreeIds, const std::vector& chordsIds, - const NonlinearFactorGraph& g, const key2doubleMap& orientationsToRoot, const PredecessorMap& tree){ +GaussianFactorGraph buildLinearOrientationGraph( + const std::vector& spanningTreeIds, + const std::vector& chordsIds, const NonlinearFactorGraph& g, + const key2doubleMap& orientationsToRoot, const PredecessorMap& tree) { GaussianFactorGraph lagoGraph; Vector deltaTheta; noiseModel::Diagonal::shared_ptr model_deltaTheta; // put original measurements in the spanning tree - BOOST_FOREACH(const size_t& factorId, spanningTreeIds){ + BOOST_FOREACH(const size_t& factorId, spanningTreeIds) { const FastVector& keys = g[factorId]->keys(); Key key1 = keys[0], key2 = keys[1]; getDeltaThetaAndNoise(g[factorId], deltaTheta, model_deltaTheta); - lagoGraph.add(JacobianFactor(key1, -I, key2, I, deltaTheta, model_deltaTheta)); + lagoGraph.add( + JacobianFactor(key1, -I, key2, I, deltaTheta, model_deltaTheta)); } // put regularized measurements in the chordsIds - BOOST_FOREACH(const size_t& factorId, chordsIds){ + BOOST_FOREACH(const size_t& factorId, chordsIds) { const FastVector& keys = g[factorId]->keys(); 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 = boost::math::round(k2pi_noise/(2*M_PI)); + 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 = boost::math::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 - Vector deltaThetaRegularized = (Vector(1) << key1_DeltaTheta_key2 - 2*k*M_PI); - lagoGraph.add(JacobianFactor(key1, -I, key2, I, deltaThetaRegularized, model_deltaTheta)); + Vector deltaThetaRegularized = (Vector(1) + << key1_DeltaTheta_key2 - 2 * k * M_PI); + lagoGraph.add( + JacobianFactor(key1, -I, key2, I, deltaThetaRegularized, + model_deltaTheta)); } // prior on the anchor orientation - lagoGraph.add(JacobianFactor(keyAnchor, I, (Vector(1) << 0.0), priorOrientationNoise)); + lagoGraph.add( + JacobianFactor(keyAnchor, I, (Vector(1) << 0.0), priorOrientationNoise)); return lagoGraph; } /* ************************************************************************* */ -NonlinearFactorGraph buildPose2graph(const NonlinearFactorGraph& graph){ +NonlinearFactorGraph buildPose2graph(const NonlinearFactorGraph& graph) { NonlinearFactorGraph pose2Graph; - BOOST_FOREACH(const boost::shared_ptr& factor, graph){ + BOOST_FOREACH(const boost::shared_ptr& factor, graph) { // recast to a between on Pose2 - boost::shared_ptr< BetweenFactor > pose2Between = - boost::dynamic_pointer_cast< BetweenFactor >(factor); + boost::shared_ptr > pose2Between = + boost::dynamic_pointer_cast >(factor); if (pose2Between) pose2Graph.add(pose2Between); // recast PriorFactor to BetweenFactor - boost::shared_ptr< PriorFactor > pose2Prior = - boost::dynamic_pointer_cast< PriorFactor >(factor); + boost::shared_ptr > pose2Prior = + boost::dynamic_pointer_cast >(factor); if (pose2Prior) - pose2Graph.add(BetweenFactor(keyAnchor, pose2Prior->keys()[0], - pose2Prior->prior(), pose2Prior->get_noiseModel())); + pose2Graph.add( + BetweenFactor(keyAnchor, pose2Prior->keys()[0], + pose2Prior->prior(), pose2Prior->get_noiseModel())); } return pose2Graph; } @@ -201,46 +216,49 @@ PredecessorMap findOdometricPath(const NonlinearFactorGraph& pose2Graph) { Key minKey; bool minUnassigned = true; - BOOST_FOREACH(const boost::shared_ptr& factor, pose2Graph){ + 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){ + if (minUnassigned) { minKey = key1; minUnassigned = false; } - if( key2 - key1 == 1){ // consecutive keys + if (key2 - key1 == 1) { // consecutive keys tree.insert(key2, key1); - if(key1 < minKey) + if (key1 < minKey) minKey = key1; } } - tree.insert(minKey,keyAnchor); - tree.insert(keyAnchor,keyAnchor); // root + tree.insert(minKey, keyAnchor); + tree.insert(keyAnchor, keyAnchor); // root return tree; } /* ************************************************************************* */ -VectorValues computeOrientations(const NonlinearFactorGraph& pose2Graph, bool useOdometricPath){ +VectorValues computeOrientations(const NonlinearFactorGraph& pose2Graph, + bool useOdometricPath) { // Find a minimum spanning tree PredecessorMap tree; if (useOdometricPath) tree = findOdometricPath(pose2Graph); else - tree = findMinimumSpanningTree >(pose2Graph); + tree = findMinimumSpanningTree >(pose2Graph); // Create a linear factor graph (LFG) of scalars key2doubleMap deltaThetaMap; std::vector spanningTreeIds; // ids of between factors forming the spanning tree T - std::vector chordsIds; // ids of between factors corresponding to chordsIds wrt T + std::vector chordsIds; // ids of between factors corresponding to chordsIds wrt T getSymbolicGraph(spanningTreeIds, chordsIds, deltaThetaMap, tree, pose2Graph); // temporary structure to correct wraparounds along loops key2doubleMap orientationsToRoot = computeThetasToRoot(deltaThetaMap, tree); // regularize measurements and plug everything in a factor graph - GaussianFactorGraph lagoGraph = buildLinearOrientationGraph(spanningTreeIds, chordsIds, pose2Graph, orientationsToRoot, tree); + GaussianFactorGraph lagoGraph = buildLinearOrientationGraph(spanningTreeIds, + chordsIds, pose2Graph, orientationsToRoot, tree); // Solve the LFG VectorValues orientationsLago = lagoGraph.optimize(); @@ -249,7 +267,8 @@ VectorValues computeOrientations(const NonlinearFactorGraph& pose2Graph, bool us } /* ************************************************************************* */ -VectorValues initializeOrientations(const NonlinearFactorGraph& graph, bool useOdometricPath) { +VectorValues initializeOrientations(const NonlinearFactorGraph& graph, + bool useOdometricPath) { // We "extract" the Pose2 subgraph of the original graph: this // is done to properly model priors and avoiding operating on a larger graph @@ -260,59 +279,69 @@ VectorValues initializeOrientations(const NonlinearFactorGraph& graph, bool useO } /* ************************************************************************* */ -Values computePoses(const NonlinearFactorGraph& pose2graph, VectorValues& orientationsLago) { +Values computePoses(const NonlinearFactorGraph& pose2graph, + VectorValues& orientationsLago) { // Linearized graph on full poses GaussianFactorGraph linearPose2graph; // We include the linear version of each between factor - BOOST_FOREACH(const boost::shared_ptr& factor, pose2graph){ + BOOST_FOREACH(const boost::shared_ptr& factor, pose2graph) { - boost::shared_ptr< BetweenFactor > pose2Between = - boost::dynamic_pointer_cast< BetweenFactor >(factor); + boost::shared_ptr > pose2Between = + boost::dynamic_pointer_cast >(factor); - if(pose2Between){ + if (pose2Between) { Key key1 = pose2Between->keys()[0]; double theta1 = orientationsLago.at(key1)(0); - double s1 = sin(theta1); double c1 = cos(theta1); + double s1 = sin(theta1); + double c1 = cos(theta1); Key key2 = pose2Between->keys()[1]; double theta2 = orientationsLago.at(key2)(0); - double linearDeltaRot = theta2 - theta1 - pose2Between->measured().theta(); + double linearDeltaRot = theta2 - theta1 + - pose2Between->measured().theta(); linearDeltaRot = Rot2(linearDeltaRot).theta(); // to normalize double dx = pose2Between->measured().x(); double dy = pose2Between->measured().y(); - Vector globalDeltaCart = (Vector(2) << c1*dx - s1*dy, s1*dx + c1*dy); - Vector b = (Vector(3) << globalDeltaCart, linearDeltaRot );// rhs - Matrix J1 = - I3; - J1(0,2) = s1*dx + c1*dy; - J1(1,2) = -c1*dx + s1*dy; + Vector globalDeltaCart = (Vector(2) << c1 * dx - s1 * dy, s1 * dx + + c1 * dy); + Vector b = (Vector(3) << globalDeltaCart, linearDeltaRot); // rhs + Matrix J1 = -I3; + J1(0, 2) = s1 * dx + c1 * dy; + J1(1, 2) = -c1 * dx + s1 * dy; // Retrieve the noise model for the relative rotation boost::shared_ptr diagonalModel = - boost::dynamic_pointer_cast(pose2Between->get_noiseModel()); + boost::dynamic_pointer_cast( + pose2Between->get_noiseModel()); - linearPose2graph.add(JacobianFactor(key1, J1, key2, I3, b, diagonalModel)); - }else{ - throw std::invalid_argument("computeLagoPoses: cannot manage non between factor here!"); + linearPose2graph.add( + JacobianFactor(key1, J1, key2, I3, b, diagonalModel)); + } else { + throw std::invalid_argument( + "computeLagoPoses: cannot manage non between factor here!"); } } // add prior - noiseModel::Diagonal::shared_ptr priorModel = noiseModel::Diagonal::Variances((Vector(3) << 1e-2, 1e-2, 1e-4)); - linearPose2graph.add(JacobianFactor(keyAnchor, I3, (Vector(3) << 0.0,0.0,0.0), priorModel)); + noiseModel::Diagonal::shared_ptr priorModel = noiseModel::Diagonal::Variances( + (Vector(3) << 1e-2, 1e-2, 1e-4)); + linearPose2graph.add( + JacobianFactor(keyAnchor, I3, (Vector(3) << 0.0, 0.0, 0.0), priorModel)); // optimize VectorValues posesLago = linearPose2graph.optimize(); // put into Values structure Values initialGuessLago; - for(VectorValues::const_iterator it = posesLago.begin(); it != posesLago.end(); ++it ){ - Key key = it->first; - if (key != keyAnchor){ - Vector poseVector = posesLago.at(key); - Pose2 poseLago = Pose2(poseVector(0),poseVector(1),orientationsLago.at(key)(0)+poseVector(2)); + BOOST_FOREACH(const VectorValues::value_type& it, posesLago) { + Key key = it.first; + if (key != keyAnchor) { + const Vector& poseVector = it.second; + Pose2 poseLago = Pose2(poseVector(0), poseVector(1), + orientationsLago.at(key)(0) + poseVector(2)); initialGuessLago.insert(key, poseLago); } } @@ -327,26 +356,28 @@ Values initialize(const NonlinearFactorGraph& graph, bool useOdometricPath) { NonlinearFactorGraph pose2Graph = buildPose2graph(graph); // Get orientations from relative orientation measurements - VectorValues orientationsLago = computeOrientations(pose2Graph, useOdometricPath); + VectorValues orientationsLago = computeOrientations(pose2Graph, + useOdometricPath); // Compute the full poses return computePoses(pose2Graph, orientationsLago); } /* ************************************************************************* */ -Values initialize(const NonlinearFactorGraph& graph, const Values& initialGuess) { +Values initialize(const NonlinearFactorGraph& graph, + const Values& initialGuess) { Values initialGuessLago; // get the orientation estimates from LAGO VectorValues orientations = initializeOrientations(graph); // for all nodes in the tree - for(VectorValues::const_iterator it = orientations.begin(); it != orientations.end(); ++it ){ - Key key = it->first; - if (key != keyAnchor){ - Pose2 pose = initialGuess.at(key); - Vector orientation = orientations.at(key); - Pose2 poseLago = Pose2(pose.x(),pose.y(),orientation(0)); + BOOST_FOREACH(const VectorValues::value_type& it, orientations) { + Key key = it.first; + if (key != keyAnchor) { + const Pose2& pose = initialGuess.at(key); + const Vector& orientation = it.second; + Pose2 poseLago = Pose2(pose.x(), pose.y(), orientation(0)); initialGuessLago.insert(key, poseLago); } } From 8d54460d2c2fc799b2f2b831c6bebbc03a409abf Mon Sep 17 00:00:00 2001 From: dellaert Date: Sat, 31 May 2014 13:44:23 -0400 Subject: [PATCH 10/10] Made three helper functions static (not directly called in testLago) --- gtsam/nonlinear/lago.cpp | 24 ++++++++++++++++++------ gtsam/nonlinear/lago.h | 25 ------------------------- 2 files changed, 18 insertions(+), 31 deletions(-) diff --git a/gtsam/nonlinear/lago.cpp b/gtsam/nonlinear/lago.cpp index f8b089539..c2a7d9454 100644 --- a/gtsam/nonlinear/lago.cpp +++ b/gtsam/nonlinear/lago.cpp @@ -37,8 +37,16 @@ static const noiseModel::Diagonal::shared_ptr priorPose2Noise = noiseModel::Diagonal::Variances((Vector(3) << 1e-6, 1e-6, 1e-8)); /* ************************************************************************* */ -double computeThetaToRoot(const Key nodeKey, const PredecessorMap& tree, - const key2doubleMap& deltaThetaMap, const key2doubleMap& thetaFromRootMap) { +/** + * Compute the cumulative orientation (without wrapping) wrt the root of a + * spanning tree (tree) for a node (nodeKey). The function starts at the nodes and + * moves towards the root summing up the (directed) rotation measurements. + * Relative measurements are encoded in "deltaThetaMap". + * The root is assumed to have orientation zero. + */ +static double computeThetaToRoot(const Key nodeKey, + const PredecessorMap& tree, const key2doubleMap& deltaThetaMap, + const key2doubleMap& thetaFromRootMap) { double nodeTheta = 0; Key key_child = nodeKey; // the node @@ -122,7 +130,8 @@ void getSymbolicGraph( } /* ************************************************************************* */ -void getDeltaThetaAndNoise(NonlinearFactor::shared_ptr factor, +// Retrieve the deltaTheta and the corresponding noise model from a BetweenFactor +static void getDeltaThetaAndNoise(NonlinearFactor::shared_ptr factor, Vector& deltaTheta, noiseModel::Diagonal::shared_ptr& model_deltaTheta) { // Get the relative rotation measurement from the between factor @@ -187,7 +196,8 @@ GaussianFactorGraph buildLinearOrientationGraph( } /* ************************************************************************* */ -NonlinearFactorGraph buildPose2graph(const NonlinearFactorGraph& graph) { +// Select the subgraph of betweenFactors and transforms priors into between wrt a fictitious node +static NonlinearFactorGraph buildPose2graph(const NonlinearFactorGraph& graph) { NonlinearFactorGraph pose2Graph; BOOST_FOREACH(const boost::shared_ptr& factor, graph) { @@ -210,7 +220,8 @@ NonlinearFactorGraph buildPose2graph(const NonlinearFactorGraph& graph) { } /* ************************************************************************* */ -PredecessorMap findOdometricPath(const NonlinearFactorGraph& pose2Graph) { +static PredecessorMap findOdometricPath( + const NonlinearFactorGraph& pose2Graph) { PredecessorMap tree; Key minKey; @@ -236,7 +247,8 @@ PredecessorMap findOdometricPath(const NonlinearFactorGraph& pose2Graph) { } /* ************************************************************************* */ -VectorValues computeOrientations(const NonlinearFactorGraph& pose2Graph, +// Return the orientations of a graph including only BetweenFactors +static VectorValues computeOrientations(const NonlinearFactorGraph& pose2Graph, bool useOdometricPath) { // Find a minimum spanning tree diff --git a/gtsam/nonlinear/lago.h b/gtsam/nonlinear/lago.h index fa8eef995..0df80ac42 100644 --- a/gtsam/nonlinear/lago.h +++ b/gtsam/nonlinear/lago.h @@ -44,17 +44,6 @@ namespace lago { typedef std::map key2doubleMap; -/** - * Compute the cumulative orientation (without wrapping) wrt the root of a - * spanning tree (tree) for a node (nodeKey). The function starts at the nodes and - * moves towards the root summing up the (directed) rotation measurements. - * Relative measurements are encoded in "deltaThetaMap". - * The root is assumed to have orientation zero. - */ -GTSAM_EXPORT double computeThetaToRoot(const Key nodeKey, - const PredecessorMap& tree, const key2doubleMap& deltaThetaMap, - const key2doubleMap& thetaFromRootMap); - /** * Compute the cumulative orientations (without wrapping) * for all nodes wrt the root (root has zero orientation). @@ -75,26 +64,12 @@ GTSAM_EXPORT void getSymbolicGraph( key2doubleMap& deltaThetaMap, /*INPUTS*/const PredecessorMap& tree, const NonlinearFactorGraph& g); -/** - * Retrieve the deltaTheta and the corresponding noise model from a BetweenFactor - */ -GTSAM_EXPORT void getDeltaThetaAndNoise(NonlinearFactor::shared_ptr factor, - Vector& deltaTheta, noiseModel::Diagonal::shared_ptr& model_deltaTheta); - /** Linear factor graph with regularized orientation measurements */ GTSAM_EXPORT GaussianFactorGraph buildLinearOrientationGraph( const std::vector& spanningTreeIds, const std::vector& chordsIds, const NonlinearFactorGraph& g, const key2doubleMap& orientationsToRoot, const PredecessorMap& tree); -/** Select the subgraph of betweenFactors and transforms priors into between wrt a fictitious node */ -GTSAM_EXPORT NonlinearFactorGraph buildPose2graph( - const NonlinearFactorGraph& graph); - -/** Return the orientations of a graph including only BetweenFactors */ -GTSAM_EXPORT VectorValues computeOrientations( - const NonlinearFactorGraph& pose2Graph, bool useOdometricPath = true); - /** LAGO: Return the orientations of the Pose2 in a generic factor graph */ GTSAM_EXPORT VectorValues initializeOrientations( const NonlinearFactorGraph& graph, bool useOdometricPath = true);