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

Unit tests for correct "Agrawal06iros" versions: they are indeed correct.

release/4.3a0
Frank Dellaert 2010-01-10 12:26:31 +00:00
parent 30367e35fb
commit 6bcb9d08d6
1 changed files with 27 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

34
cpp/testPose3.cpp
View File

@ -269,19 +269,39 @@ TEST( Pose3, composeTransform )
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST(Pose3, manifold) { TEST(Pose3, manifold)
//cout << "manifold" << endl; {
//cout << "manifold" << endl;
Pose3 t1 = T; Pose3 t1 = T;
Pose3 t2 = pose1; Pose3 t2 = pose1;
Pose3 origin; Pose3 origin;
Vector d12 = logmap(t1,t2); Vector d12 = logmap(t1, t2);
CHECK(assert_equal(t2, expmap(t1,d12))); CHECK(assert_equal(t2, expmap(t1,d12)));
// todo: richard - commented out because this tests for "compose-style" (new) expmap // todo: richard - commented out because this tests for "compose-style" (new) expmap
//CHECK(assert_equal(t2, expmap(origin,d12)*t1)); // CHECK(assert_equal(t2, expmap(origin,d12)*t1));
Vector d21 = logmap(t2,t1); Vector d21 = logmap(t2, t1);
CHECK(assert_equal(t1, expmap(t2,d21))); CHECK(assert_equal(t1, expmap(t2,d21)));
// todo: richard - commented out because this tests for "compose-style" (new) expmap // todo: richard - commented out because this tests for "compose-style" (new) expmap
//CHECK(assert_equal(t1, expmap(origin,d21)*t2)); // CHECK(assert_equal(t1, expmap(origin,d21)*t2));
// Check that log(t1,t2)=-log(t2,t1) - this holds even for incorrect expmap :-)
CHECK(assert_equal(d12,-d21));
#ifdef SLOW_BUT_CORRECT_EXPMAP
// todo: Frank - Below only works for correct "Agrawal06iros style expmap
// lines in canonical coordinates correspond to Abelian subgroups in SE(3)
Vector d = Vector_(6,0.1,0.2,0.3,0.4,0.5,0.6);
// exp(-d)=inverse(exp(d))
CHECK(assert_equal(expmap<Pose3>(-d),inverse(expmap<Pose3>(d))));
// exp(5d)=exp(2*d+3*d)=exp(2*d)exp(3*d)=exp(3*d)exp(2*d)
Pose3 T2 = expmap<Pose3>(2*d);
Pose3 T3 = expmap<Pose3>(3*d);
Pose3 T5 = expmap<Pose3>(5*d);
CHECK(assert_equal(T5,T2*T3));
CHECK(assert_equal(T5,T3*T2));
#endif
} }
/* ************************************************************************* */ /* ************************************************************************* */