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

I reverted back to the old expmap in Pose3 :-( It does not work well for Urban, and now it turns out Yong-Dian has problems, too. It seems there is still something that we do not quite understand about the whole Lie group optimization business. If you want to run with the new expmap, define CORRECT_POSE3_EXPMAP

release/4.3a0
Frank Dellaert 2010-03-12 19:16:53 +00:00
parent 91f020fee1
commit f217a5bd8a
3 changed files with 15 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
cpp/Pose3.cpp
View File

@ -44,7 +44,7 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
#ifndef FASTER_BUT_INCORRECT_EXMAP #ifdef CORRECT_POSE3_EXMAP
/** Modified from Murray94book version (which assumes w and v normalized?) */ /** Modified from Murray94book version (which assumes w and v normalized?) */
template<> Pose3 expmap(const Vector& xi) { template<> Pose3 expmap(const Vector& xi) {
@ -144,7 +144,7 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
Matrix Dtransform_from1(const Pose3& pose, const Point3& p) { Matrix Dtransform_from1(const Pose3& pose, const Point3& p) {
#ifndef FASTER_BUT_INCORRECT_EXMAP #ifdef CORRECT_POSE3_EXMAP
const Matrix R = pose.rotation().matrix(); const Matrix R = pose.rotation().matrix();
Matrix DR = R*skewSymmetric(-p.x(), -p.y(), -p.z()); Matrix DR = R*skewSymmetric(-p.x(), -p.y(), -p.z());
return collect(2,&DR,&R); return collect(2,&DR,&R);
@ -170,7 +170,7 @@ namespace gtsam {
Matrix Dtransform_to1(const Pose3& pose, const Point3& p) { Matrix Dtransform_to1(const Pose3& pose, const Point3& p) {
Point3 q = transform_to(pose,p); Point3 q = transform_to(pose,p);
Matrix DR = skewSymmetric(q.x(), q.y(), q.z()); Matrix DR = skewSymmetric(q.x(), q.y(), q.z());
#ifndef FASTER_BUT_INCORRECT_EXMAP #ifdef CORRECT_POSE3_EXMAP
return collect(2, &DR, &_I3); return collect(2, &DR, &_I3);
#else #else
Matrix DT = - pose.rotation().transpose(); // negative because of sub Matrix DT = - pose.rotation().transpose(); // negative because of sub
@ -186,7 +186,7 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
// compose = Pose3(compose(R1,R2),transform_from(p1,t2) // compose = Pose3(compose(R1,R2),transform_from(p1,t2)
Matrix Dcompose1(const Pose3& p1, const Pose3& p2) { Matrix Dcompose1(const Pose3& p1, const Pose3& p2) {
#ifndef FASTER_BUT_INCORRECT_EXMAP #ifdef CORRECT_POSE3_EXMAP
return AdjointMap(inverse(p2)); return AdjointMap(inverse(p2));
#else #else
const Rot3& R2 = p2.rotation(); const Rot3& R2 = p2.rotation();
@ -199,7 +199,7 @@ namespace gtsam {
} }
Matrix Dcompose2(const Pose3& p1, const Pose3& p2) { Matrix Dcompose2(const Pose3& p1, const Pose3& p2) {
#ifndef FASTER_BUT_INCORRECT_EXMAP #ifdef CORRECT_POSE3_EXMAP
return I6; return I6;
#else #else
@ -214,7 +214,7 @@ namespace gtsam {
// inverse = Pose3(inverse(R),-unrotate(R,t)); // inverse = Pose3(inverse(R),-unrotate(R,t));
// TODO: combined function will save ! // TODO: combined function will save !
Matrix Dinverse(const Pose3& p) { Matrix Dinverse(const Pose3& p) {
#ifndef FASTER_BUT_INCORRECT_EXMAP #ifdef CORRECT_POSE3_EXMAP
return - AdjointMap(p); return - AdjointMap(p);
#else #else
const Rot3& R = p.rotation(); const Rot3& R = p.rotation();

6
cpp/testPose3.cpp
View File

@ -34,7 +34,7 @@ TEST( Pose3, expmap_a)
fill(v.begin(), v.end(), 0); fill(v.begin(), v.end(), 0);
v(0) = 0.3; v(0) = 0.3;
CHECK(assert_equal(expmap(id,v), Pose3(R, Point3()))); CHECK(assert_equal(expmap(id,v), Pose3(R, Point3())));
#ifndef FASTER_BUT_INCORRECT_EXMAP #ifdef CORRECT_POSE3_EXMAP
v(3)=0.2;v(4)=0.394742;v(5)=-2.08998; v(3)=0.2;v(4)=0.394742;v(5)=-2.08998;
#else #else
@ -52,7 +52,7 @@ TEST(Pose3, expmap_b)
CHECK(assert_equal(expected, p2)); CHECK(assert_equal(expected, p2));
} }
#ifndef FASTER_BUT_INCORRECT_EXMAP #ifdef CORRECT_POSE3_EXMAP
/* ************************************************************************* */ /* ************************************************************************* */
// test case for screw motion in the plane // test case for screw motion in the plane
@ -384,7 +384,7 @@ TEST(Pose3, manifold)
// Check that log(t1,t2)=-log(t2,t1) - this holds even for incorrect expmap :-) // Check that log(t1,t2)=-log(t2,t1) - this holds even for incorrect expmap :-)
CHECK(assert_equal(d12,-d21)); CHECK(assert_equal(d12,-d21));
#ifndef FASTER_BUT_INCORRECT_EXMAP #ifdef CORRECT_POSE3_EXMAP
// todo: Frank - Below only works for correct "Agrawal06iros style expmap // todo: Frank - Below only works for correct "Agrawal06iros style expmap

12
cpp/testPose3Config.cpp
View File

@ -36,17 +36,17 @@ TEST( Pose3Config, pose3Circle )
TEST( Pose3Config, expmap ) TEST( Pose3Config, expmap )
{ {
Pose3Config expected; Pose3Config expected;
#ifdef FASTER_BUT_INCORRECT_EXMAP #ifdef CORRECT_POSE3_EXMAP
expected.insert(0, Pose3(R1, Point3( 1.0, 0.1, 0)));
expected.insert(1, Pose3(R2, Point3(-0.1, 1.0, 0)));
expected.insert(2, Pose3(R3, Point3(-1.0,-0.1, 0)));
expected.insert(3, Pose3(R4, Point3( 0.1,-1.0, 0)));
#else
// expected is circle shifted to East // expected is circle shifted to East
expected.insert(0, Pose3(R1, Point3( 1.1, 0, 0))); expected.insert(0, Pose3(R1, Point3( 1.1, 0, 0)));
expected.insert(1, Pose3(R2, Point3( 0.1, 1, 0))); expected.insert(1, Pose3(R2, Point3( 0.1, 1, 0)));
expected.insert(2, Pose3(R3, Point3(-0.9, 0, 0))); expected.insert(2, Pose3(R3, Point3(-0.9, 0, 0)));
expected.insert(3, Pose3(R4, Point3( 0.1,-1, 0))); expected.insert(3, Pose3(R4, Point3( 0.1,-1, 0)));
#else
expected.insert(0, Pose3(R1, Point3( 1.0, 0.1, 0)));
expected.insert(1, Pose3(R2, Point3(-0.1, 1.0, 0)));
expected.insert(2, Pose3(R3, Point3(-1.0,-0.1, 0)));
expected.insert(3, Pose3(R4, Point3( 0.1,-1.0, 0)));
#endif #endif
// Note expmap coordinates are in global coordinates with non-compose expmap // Note expmap coordinates are in global coordinates with non-compose expmap