12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

working unit tests

release/4.3a0
Luca 2014-09-03 15:23:35 -04:00
parent 35d5b56b65
commit 103d2a8ae9
3 changed files with 71 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

33
gtsam/slam/InitializePose3.cpp
View File

@ -147,7 +147,7 @@ Values computeOrientationsChordal(const NonlinearFactorGraph& pose3Graph) {
/* ************************************************************************* */
// Return the orientations of a graph including only BetweenFactors<Pose3>
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<Rot3>(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<Rot3>(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<Rot3>(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<Rot3>(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));

2
gtsam/slam/InitializePose3.h
View File

@ -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);

54
gtsam/slam/tests/testInitializePose3.cpp
View File

@ -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<Rot3>(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<Rot3>(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<Rot3>(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<Rot3>(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<Rot3>(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);