diff --git a/gtsam/nonlinear/LagoInitializer.cpp b/gtsam/nonlinear/LagoInitializer.cpp index 9acd52f96..973dd6fe2 100644 --- a/gtsam/nonlinear/LagoInitializer.cpp +++ b/gtsam/nonlinear/LagoInitializer.cpp @@ -20,6 +20,9 @@ namespace gtsam { +Matrix I = eye(1); +Matrix I3 = eye(3); + /* ************************************************************************* */ double computeThetaToRoot(const Key nodeKey, const PredecessorMap& tree, const key2doubleMap& deltaThetaMap, const key2doubleMap& thetaFromRootMap) { @@ -129,7 +132,6 @@ GaussianFactorGraph buildLinearOrientationGraph(const std::vector& spann Vector deltaTheta; noiseModel::Diagonal::shared_ptr model_deltaTheta; - Matrix I = eye(1); // put original measurements in the spanning tree BOOST_FOREACH(const size_t& factorId, spanningTreeIds){ const FastVector& keys = g[factorId]->keys(); @@ -211,6 +213,68 @@ VectorValues initializeOrientationsLago(const NonlinearFactorGraph& graph) { return computeLagoOrientations(pose2Graph); } +/* ************************************************************************* */ +Values computeLagoPoses(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::shared_ptr< BetweenFactor > pose2Between = + boost::dynamic_pointer_cast< BetweenFactor >(factor); + + if(pose2Between){ + Key key1 = pose2Between->keys()[0]; + double theta1 = orientationsLago.at(key1)(0); + 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(); + linearDeltaRot = Rot2(linearDeltaRot).theta(); // to normalize + if(fabs(linearDeltaRot)>M_PI) + std::cout << "linearDeltaRot " << linearDeltaRot << std::endl; + + 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; + // Retrieve the noise model for the relative rotation + boost::shared_ptr diagonalModel = + 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!"); + } + } + // 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)); + + // 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)); + initialGuessLago.insert(key, poseLago); + } + } + return initialGuessLago; +} + /* ************************************************************************* */ Values initializeLago(const NonlinearFactorGraph& graph) { @@ -221,24 +285,8 @@ Values initializeLago(const NonlinearFactorGraph& graph) { // Get orientations from relative orientation measurements VectorValues orientationsLago = computeLagoOrientations(pose2Graph); - Values initialGuessLago; - // for all nodes in the tree - for(VectorValues::const_iterator it = orientationsLago.begin(); it != orientationsLago.end(); ++it ){ - Key key = it->first; - Vector orientation = orientationsLago.at(key); - Pose2 poseLago = Pose2(0.0,0.0,orientation(0)); - initialGuessLago.insert(key, poseLago); - } - // add prior needed by GN - pose2Graph.add(PriorFactor(keyAnchor, Pose2(), priorPose2Noise)); - - // Optimize Pose2, with initialGuessLago as initial guess - GaussNewtonParams params; - params.setMaxIterations(1); - GaussNewtonOptimizer pose2optimizer(pose2Graph, initialGuessLago, params); - initialGuessLago = pose2optimizer.optimize(); - initialGuessLago.erase(keyAnchor); // that was fictitious - return initialGuessLago; + // Compute the full poses + return computeLagoPoses(pose2Graph, orientationsLago); } /* ************************************************************************* */ diff --git a/gtsam/nonlinear/tests/testLagoInitializer.cpp b/gtsam/nonlinear/tests/testLagoInitializer.cpp index d08e79569..4be325734 100644 --- a/gtsam/nonlinear/tests/testLagoInitializer.cpp +++ b/gtsam/nonlinear/tests/testLagoInitializer.cpp @@ -237,15 +237,15 @@ TEST( Lago, smallGraphNoisy_orientations ) { // Results from Matlab // VERTEX_SE2 0 0.000000 0.000000 0.000000 - // VERTEX_SE2 1 0.000000 0.000000 1.568774 - // VERTEX_SE2 2 0.000000 0.000000 3.142238 - // VERTEX_SE2 3 0.000000 0.000000 4.702950 + // 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.568774), initialGuessLago.at(1), 1e-5)); - EXPECT(assert_equal((Vector(1) << 3.14223 - 2*M_PI), initialGuessLago.at(2), 1e-5)); - EXPECT(assert_equal((Vector(1) << 4.702950 - 2*M_PI), initialGuessLago.at(3), 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)); } /* *************************************************************************** */ @@ -264,18 +264,18 @@ TEST( Lago, smallGraphNoisy ) { Values actual = initializeLago(graphWithPrior); // Optimized results from matlab - // VERTEX_SE2 0 0.000000 0.000000 0.000000 - // VERTEX_SE2 1 1.141931 0.980395 1.569023 - // VERTEX_SE2 2 0.124207 2.140972 -3.140451 - // VERTEX_SE2 3 -0.958080 1.070072 -1.577699 +// 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 Values expected; - expected.insert(x0,Pose2(0.000000, 0.000000, 0.000000)); - expected.insert(x1,Pose2(1.141931, 0.980395, 1.569023)); - expected.insert(x2,Pose2(0.124207, 2.140972, -3.140451)); - expected.insert(x3,Pose2(-0.958080, 1.070072, -1.577699)); + 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)); - EXPECT(assert_equal(expected, actual, 1e-6)); + EXPECT(assert_equal(expected, actual, 1e-5)); } /* ************************************************************************* */