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;