diff --git a/gtsam/slam/InitializePose3.cpp b/gtsam/slam/InitializePose3.cpp index 7bf29535b..c9eff0536 100644 --- a/gtsam/slam/InitializePose3.cpp +++ b/gtsam/slam/InitializePose3.cpp @@ -147,7 +147,7 @@ Values computeOrientationsChordal(const NonlinearFactorGraph& pose3Graph) { /* ************************************************************************* */ // Return the orientations of a graph including only BetweenFactors -Values computeOrientationsGradient(const NonlinearFactorGraph& pose3Graph, const Values& givenGuess) { +Values computeOrientationsGradient(const NonlinearFactorGraph& pose3Graph, const Values& givenGuess, const size_t maxIter) { gttic(InitializePose3_computeOrientationsGradient); // this works on the inverse rotations, according to Tron&Vidal,2011 @@ -183,7 +183,6 @@ Values computeOrientationsGradient(const NonlinearFactorGraph& pose3Graph, const double rho = 2*a*b; double mu_max = maxNodeDeg * rho; double stepsize = 2/mu_max; // = 1/(a b dG) - size_t maxIter = 1; // gradient iterations for(size_t it=0; it < maxIter; it++){ @@ -194,8 +193,8 @@ Values computeOrientationsGradient(const NonlinearFactorGraph& pose3Graph, const double maxGrad = 0; BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, inverseRot) { Key key = key_value.key; - grad.at(key) = Vector3::Zero(); // reset gradient - + //std::cout << "---------------------------key " << DefaultKeyFormatter(key) << std::endl; + Vector gradKey = Vector3::Zero(); // collect the gradient for each edge incident on key BOOST_FOREACH(const size_t& factorId, adjEdgesMap.at(key)){ Rot3 Rij = factorId2RotMap.at(factorId); @@ -203,18 +202,21 @@ Values computeOrientationsGradient(const NonlinearFactorGraph& pose3Graph, const if( key == (pose3Graph.at(factorId))->keys()[0] ){ Key key1 = (pose3Graph.at(factorId))->keys()[1]; Rot3 Rj = inverseRot.at(key1); - grad.at(key) = grad.at(key) + stepsize * gradientTron(Ri, Rij * Rj, a, b); + gradKey = gradKey + gradientTron(Ri, Rij * Rj, a, b); + //std::cout << "key1 " << DefaultKeyFormatter(key1) << " gradientTron(Ri, Rij * Rj, a, b) \n " << gradientTron(Ri, Rij * Rj, a, b) << std::endl; }else if( key == (pose3Graph.at(factorId))->keys()[1] ){ Key key0 = (pose3Graph.at(factorId))->keys()[0]; Rot3 Rj = inverseRot.at(key0); - grad.at(key) = grad.at(key) + stepsize * gradientTron(Ri, Rij.inverse() * Rj, a, b); + gradKey = gradKey + gradientTron(Ri, Rij.between(Rj), a, b); + //std::cout << "key0 " << DefaultKeyFormatter(key0) << " gradientTron(Ri, Rij.inverse() * Rj, a, b) \n " << gradientTron(Ri, Rij.between(Rj), a, b) << std::endl; }else{ std::cout << "Error in gradient computation" << std::endl; } } // end of i-th gradient computation + grad.at(key) = stepsize * gradKey; - double normGradKey = (grad.at(key)).norm(); - std::cout << "key " << DefaultKeyFormatter(key) <<" grad " << grad.at(key) << std::endl; + double normGradKey = (gradKey).norm(); + std::cout << "key " << DefaultKeyFormatter(key) <<" \n grad \n" << grad.at(key) << std::endl; if(normGradKey>maxGrad) maxGrad = normGradKey; } // end of loop over nodes @@ -223,6 +225,8 @@ Values computeOrientationsGradient(const NonlinearFactorGraph& pose3Graph, const // update estimates inverseRot = inverseRot.retract(grad); + inverseRot.print("inverseRot \n"); + ////////////////////////////////////////////////////////////////////////// // check stopping condition if (it>20 && maxGrad < 5e-3) @@ -230,12 +234,12 @@ Values computeOrientationsGradient(const NonlinearFactorGraph& pose3Graph, const } // enf of gradient iterations // Return correct rotations - Values estimateRot; - BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, inverseRot) { - Key key = key_value.key; - const Rot3& R = inverseRot.at(key); - estimateRot.insert(key, R.inverse()); - } + Values estimateRot; + BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, inverseRot) { + Key key = key_value.key; + const Rot3& R = inverseRot.at(key); + estimateRot.insert(key, R.inverse()); + } return estimateRot; } @@ -275,6 +279,7 @@ void createSymbolicGraph(KeyVectorMap& adjEdgesMap, KeyRotMap& factorId2RotMap, /* ************************************************************************* */ Vector3 gradientTron(const Rot3& R1, const Rot3& R2, const double a, const double b) { Vector3 logRot = Rot3::Logmap(R1.between(R2)); + if(logRot.norm()<1e-4){ // some tolerance Rot3 R1pert = R1.compose( Rot3::Expmap((Vector(3)<< 0.01, 0.01, 0.01)) ); // some perturbation logRot = Rot3::Logmap(R1pert.between(R2)); diff --git a/gtsam/slam/InitializePose3.h b/gtsam/slam/InitializePose3.h index 2f6fb842e..1bb9a8e31 100644 --- a/gtsam/slam/InitializePose3.h +++ b/gtsam/slam/InitializePose3.h @@ -39,7 +39,7 @@ GTSAM_EXPORT Values normalizeRelaxedRotations(const VectorValues& relaxedRot3); GTSAM_EXPORT Values computeOrientationsChordal(const NonlinearFactorGraph& pose3Graph); -GTSAM_EXPORT Values computeOrientationsGradient(const NonlinearFactorGraph& pose3Graph, const Values& givenGuess); +GTSAM_EXPORT Values computeOrientationsGradient(const NonlinearFactorGraph& pose3Graph, const Values& givenGuess, size_t maxIter = 1000); GTSAM_EXPORT void createSymbolicGraph(KeyVectorMap& adjEdgesMap, KeyRotMap& factorId2RotMap, const NonlinearFactorGraph& pose3Graph); diff --git a/gtsam/slam/tests/testInitializePose3.cpp b/gtsam/slam/tests/testInitializePose3.cpp index 2eb0b3e67..c3aafa9a3 100644 --- a/gtsam/slam/tests/testInitializePose3.cpp +++ b/gtsam/slam/tests/testInitializePose3.cpp @@ -160,15 +160,63 @@ TEST( InitializePose3, singleGradient ) { } /* *************************************************************************** */ +TEST( InitializePose3, iterationGradient ) { + NonlinearFactorGraph pose3Graph = InitializePose3::buildPose3graph(simple::graph()); + + // Wrong initial guess - initialization should fix the rotations + Rot3 Rpert = Rot3::Expmap((Vector(3)<< 0.01, 0.01, 0.01)); + Values givenPoses; + givenPoses.insert(x0,simple::pose0); + givenPoses.insert(x1,(simple::pose0).compose( Pose3(Rpert,Point3()) )); + givenPoses.insert(x2, (simple::pose0).compose( Pose3(Rpert.inverse(),Point3()) )); + givenPoses.insert(x3, (simple::pose0).compose( Pose3(Rpert,Point3()) )); + + size_t maxIter = 1; // test gradient at the first iteration + Values orientations = InitializePose3::computeOrientationsGradient(pose3Graph, givenPoses, maxIter); + + const Key keyAnchor = symbol('Z', 9999999); + Matrix Mz = (Matrix(3,3) << 0.999993962808392, -0.002454045561375, 0.002460082752984, + 0.002460082752984, 0.999993962808392, -0.002454045561375, + -0.002454045561375, 0.002460082752984, 0.999993962808392); + Rot3 RzExpected = Rot3(Mz); + EXPECT(assert_equal(RzExpected, orientations.at(keyAnchor), 1e-6)); + + Matrix M0 = (Matrix(3,3) << 0.999344848920642, -0.036021919324717, 0.003506317718352, + 0.036032601656108, 0.999346013522419, -0.003032634950127, + -0.003394783302464, 0.003156989865691, 0.999989254372924); + Rot3 R0Expected = Rot3(M0); + EXPECT(assert_equal(R0Expected, orientations.at(x0), 1e-5)); + + Matrix M1 = (Matrix(3,3) << 0.999905367545392, -0.010866391403031, 0.008436675399114, + 0.010943459008004, 0.999898317528125, -0.009143047050380, + -0.008336465609239, 0.009234508232789, 0.999922610604863); + Rot3 R1Expected = Rot3(M1); + EXPECT(assert_equal(R1Expected, orientations.at(x1), 1e-5)); + + Matrix M2 = (Matrix(3,3) << 0.998936644682875, 0.045376417678595, -0.008158469732553, + -0.045306446926148, 0.998936408933058, 0.008566024448664, + 0.008538487960253, -0.008187284445083, 0.999930028850403); + Rot3 R2Expected = Rot3(M2); + EXPECT(assert_equal(R2Expected, orientations.at(x2), 1e-5)); + + Matrix M3 = (Matrix(3,3) << 0.999898767273093, -0.010834701971459, 0.009223038487275, + 0.010911315499947, 0.999906044037258, -0.008297366559388, + -0.009132272433995, 0.008397162077148, 0.999923041673329); + Rot3 R3Expected = Rot3(M3); + EXPECT(assert_equal(R3Expected, orientations.at(x3), 1e-5)); +} + +/* *************************************************************************** * TEST( InitializePose3, orientationsGradient ) { NonlinearFactorGraph pose3Graph = InitializePose3::buildPose3graph(simple::graph()); // Wrong initial guess - initialization should fix the rotations + Rot3 Rpert = Rot3::Expmap((Vector(3)<< 0.01, 0.01, 0.01)); Values givenPoses; givenPoses.insert(x0,simple::pose0); - givenPoses.insert(x1,simple::pose0); - givenPoses.insert(x2,simple::pose0); - givenPoses.insert(x3,simple::pose0); + givenPoses.insert(x1,(simple::pose0).compose( Pose3(Rpert,Point3()) )); + givenPoses.insert(x2, (simple::pose0).compose( Pose3(Rpert.inverse(),Point3()) )); + givenPoses.insert(x3, (simple::pose0).compose( Pose3(Rpert,Point3()) )); Values initial = InitializePose3::computeOrientationsGradient(pose3Graph, givenPoses); const Key keyAnchor = symbol('Z', 9999999);