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

MAJOR: I made the correct Pose3 expmap the default. It turns out that all but the transform_to derivatives were in fact correct (and I fixed transform_to based on new math in math.lyx), but there was still a wrong expmap and logmap in the header file that I forgot about. I now moved those the te cpp file. The new code, which executes a correct "screw motion" for the exponential map, is now linked in by default. If you want the old, incorrect behavior, configure with FASTER_BUT_INCORRECT_EXPMAP defined. Note that the old code is faster per expmap, but *converges* faster (and to a deeper minimum) and hence is faster overall in many cases.

release/4.3a0
Frank Dellaert 2010-03-11 21:52:24 +00:00
parent b4a9ad2a9c
commit 26304b749a
4 changed files with 78 additions and 48 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

44
cpp/Pose3.cpp
View File

@ -16,7 +16,7 @@ namespace gtsam {
/** Explicit instantiation of base class to export members */ /** Explicit instantiation of base class to export members */
INSTANTIATE_LIE(Pose3); INSTANTIATE_LIE(Pose3);
static const Matrix I3 = eye(3), I6 = eye(6), Z3 = zeros(3, 3); static const Matrix I3 = eye(3), _I3=-I3, I6 = eye(6), Z3 = zeros(3, 3);
/* ************************************************************************* */ /* ************************************************************************* */
// Calculate Adjoint map // Calculate Adjoint map
@ -44,7 +44,7 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
#ifdef SLOW_BUT_CORRECT_EXPMAP #ifndef FASTER_BUT_INCORRECT_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) {
@ -80,6 +80,14 @@ namespace gtsam {
} }
} }
Pose3 expmap(const Pose3& T, const Vector& d) {
return compose(T,expmap<Pose3>(d));
}
Vector logmap(const Pose3& T1, const Pose3& T2) {
return logmap(between(T1,T2));
}
#else #else
/* incorrect versions for which we know how to compute derivatives */ /* incorrect versions for which we know how to compute derivatives */
@ -96,6 +104,22 @@ namespace gtsam {
return concatVectors(2, &w, &u); return concatVectors(2, &w, &u);
} }
/** These are the "old-style" expmap and logmap about the specified
* pose. Increments the offset and rotation independently given a translation and
* canonical rotation coordinates. Created to match ML derivatives, but
* superseded by the correct exponential map story in .cpp */
Pose3 expmap(const Pose3& p0, const Vector& d) {
return Pose3(expmap(p0.rotation(), sub(d, 0, 3)),
expmap(p0.translation(), sub(d, 3, 6)));
}
/** Independently computes the logmap of the translation and rotation. */
Vector logmap(const Pose3& p0, const Pose3& pp) {
const Vector r(logmap(p0.rotation(), pp.rotation())),
t(logmap(p0.translation(), pp.translation()));
return concatVectors(2, &r, &t);
}
#endif #endif
/* ************************************************************************* */ /* ************************************************************************* */
@ -120,7 +144,7 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
Matrix Dtransform_from1(const Pose3& pose, const Point3& p) { Matrix Dtransform_from1(const Pose3& pose, const Point3& p) {
#ifdef SLOW_BUT_CORRECT_EXPMAP #ifndef FASTER_BUT_INCORRECT_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);
@ -142,11 +166,16 @@ namespace gtsam {
} }
/* ************************************************************************* */ /* ************************************************************************* */
// see math.lyx, derivative of action
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
return collect(2, &DR, &_I3);
#else
Matrix DT = - pose.rotation().transpose(); // negative because of sub Matrix DT = - pose.rotation().transpose(); // negative because of sub
return collect(2,&DR,&DT); return collect(2,&DR,&DT);
#endif
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -157,8 +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) {
#ifdef SLOW_BUT_CORRECT_EXPMAP #ifndef FASTER_BUT_INCORRECT_EXMAP
// actually does NOT pan out at the moment
return AdjointMap(inverse(p2)); return AdjointMap(inverse(p2));
#else #else
const Rot3& R2 = p2.rotation(); const Rot3& R2 = p2.rotation();
@ -171,7 +199,8 @@ namespace gtsam {
} }
Matrix Dcompose2(const Pose3& p1, const Pose3& p2) { Matrix Dcompose2(const Pose3& p1, const Pose3& p2) {
#ifdef SLOW_BUT_CORRECT_EXPMAP #ifndef FASTER_BUT_INCORRECT_EXMAP
return I6; return I6;
#else #else
Matrix R1 = p1.rotation().matrix(); Matrix R1 = p1.rotation().matrix();
@ -185,8 +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) {
#ifdef SLOW_BUT_CORRECT_EXPMAP #ifndef FASTER_BUT_INCORRECT_EXMAP
// actually does NOT pan out at the moment
return - AdjointMap(p); return - AdjointMap(p);
#else #else
const Rot3& R = p.rotation(); const Rot3& R = p.rotation();

18
cpp/Pose3.h
View File

@ -108,21 +108,11 @@ namespace gtsam {
* coordinates of a pose. */ * coordinates of a pose. */
Vector logmap(const Pose3& p); Vector logmap(const Pose3& p);
/** todo: these are the "old-style" expmap and logmap about the specified /** Exponential map around another pose */
* pose. Pose3 expmap(const Pose3& T, const Vector& d);
* Increments the offset and rotation independently given a translation and
* canonical rotation coordinates */
template<> inline Pose3 expmap<Pose3>(const Pose3& p0, const Vector& d) {
return Pose3(expmap(p0.rotation(), sub(d, 0, 3)),
expmap(p0.translation(), sub(d, 3, 6)));
}
/** Independently computes the logmap of the translation and rotation. */ /** Logarithm map around another pose T1 */
template<> inline Vector logmap<Pose3>(const Pose3& p0, const Pose3& pp) { Vector logmap(const Pose3& T1, const Pose3& T2);
const Vector r(logmap(p0.rotation(), pp.rotation())),
t(logmap(p0.translation(), pp.translation()));
return concatVectors(2, &r, &t);
}
/** receives the point in Pose coordinates and transforms it to world coordinates */ /** receives the point in Pose coordinates and transforms it to world coordinates */
Point3 transform_from(const Pose3& pose, const Point3& p); Point3 transform_from(const Pose3& pose, const Point3& p);

51
cpp/testPose3.cpp
View File

@ -17,8 +17,6 @@ static Pose3 T(R,Point3(3.5,-8.2,4.2));
static Pose3 T2(rodriguez(0.3,0.2,0.1),Point3(3.5,-8.2,4.2)); static Pose3 T2(rodriguez(0.3,0.2,0.1),Point3(3.5,-8.2,4.2));
static Pose3 T3(rodriguez(-90, 0, 0), Point3(1, 2, 3)); static Pose3 T3(rodriguez(-90, 0, 0), Point3(1, 2, 3));
double error = 1e-8;
/* ************************************************************************* */ /* ************************************************************************* */
TEST( Pose3, equals) TEST( Pose3, equals)
{ {
@ -36,8 +34,13 @@ 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
v(3)=0.2;v(4)=0.394742;v(5)=-2.08998;
#else
v(3)=0.2;v(4)=0.7;v(5)=-2; v(3)=0.2;v(4)=0.7;v(5)=-2;
CHECK(assert_equal(expmap(id,v), Pose3(R, P))); #endif
CHECK(assert_equal(Pose3(R, P),expmap(id,v),1e-5));
} }
TEST(Pose3, expmap_b) TEST(Pose3, expmap_b)
@ -49,7 +52,8 @@ TEST(Pose3, expmap_b)
CHECK(assert_equal(expected, p2)); CHECK(assert_equal(expected, p2));
} }
#ifdef SLOW_BUT_CORRECT_EXPMAP #ifndef FASTER_BUT_INCORRECT_EXMAP
/* ************************************************************************* */ /* ************************************************************************* */
// test case for screw motion in the plane // test case for screw motion in the plane
namespace screw { namespace screw {
@ -147,11 +151,11 @@ TEST( Pose3, compose )
{ {
Matrix actual = (T2*T2).matrix(); Matrix actual = (T2*T2).matrix();
Matrix expected = T2.matrix()*T2.matrix(); Matrix expected = T2.matrix()*T2.matrix();
CHECK(assert_equal(actual,expected,error)); CHECK(assert_equal(actual,expected,1e-8));
Matrix numericalH1 = numericalDerivative21<Pose3,Pose3,Pose3>(compose, T2, T2, 1e-5); Matrix numericalH1 = numericalDerivative21<Pose3,Pose3,Pose3>(compose, T2, T2, 1e-5);
Matrix actualDcompose1 = Dcompose1(T2, T2); Matrix actualDcompose1 = Dcompose1(T2, T2);
CHECK(assert_equal(numericalH1,actualDcompose1)); CHECK(assert_equal(numericalH1,actualDcompose1,5e-5));
Matrix actualDcompose2 = Dcompose2(T2, T2); Matrix actualDcompose2 = Dcompose2(T2, T2);
Matrix numericalH2 = numericalDerivative22<Pose3,Pose3,Pose3>(compose, T2, T2, 1e-5); Matrix numericalH2 = numericalDerivative22<Pose3,Pose3,Pose3>(compose, T2, T2, 1e-5);
@ -164,11 +168,11 @@ TEST( Pose3, compose2 )
const Pose3& T1 = T; const Pose3& T1 = T;
Matrix actual = (T1*T2).matrix(); Matrix actual = (T1*T2).matrix();
Matrix expected = T1.matrix()*T2.matrix(); Matrix expected = T1.matrix()*T2.matrix();
CHECK(assert_equal(actual,expected,error)); CHECK(assert_equal(actual,expected,1e-8));
Matrix numericalH1 = numericalDerivative21<Pose3,Pose3,Pose3>(compose, T1, T2, 1e-5); Matrix numericalH1 = numericalDerivative21<Pose3,Pose3,Pose3>(compose, T1, T2, 1e-5);
Matrix actualDcompose1 = Dcompose1(T1, T2); Matrix actualDcompose1 = Dcompose1(T1, T2);
CHECK(assert_equal(numericalH1,actualDcompose1)); CHECK(assert_equal(numericalH1,actualDcompose1,5e-5));
Matrix actualDcompose2 = Dcompose2(T1, T2); Matrix actualDcompose2 = Dcompose2(T1, T2);
Matrix numericalH2 = numericalDerivative22<Pose3,Pose3,Pose3>(compose, T1, T2, 1e-5); Matrix numericalH2 = numericalDerivative22<Pose3,Pose3,Pose3>(compose, T1, T2, 1e-5);
@ -179,7 +183,7 @@ TEST( Pose3, inverse)
{ {
Matrix actual = inverse(T).matrix(); Matrix actual = inverse(T).matrix();
Matrix expected = inverse(T.matrix()); Matrix expected = inverse(T.matrix());
CHECK(assert_equal(actual,expected,error)); CHECK(assert_equal(actual,expected,1e-8));
Matrix numericalH = numericalDerivative11<Pose3,Pose3>(inverse, T, 1e-5); Matrix numericalH = numericalDerivative11<Pose3,Pose3>(inverse, T, 1e-5);
Matrix actualDinverse = Dinverse(T); Matrix actualDinverse = Dinverse(T);
@ -195,7 +199,7 @@ TEST( Pose3, inverseDerivatives2)
Matrix numericalH = numericalDerivative11<Pose3,Pose3>(inverse, T, 1e-5); Matrix numericalH = numericalDerivative11<Pose3,Pose3>(inverse, T, 1e-5);
Matrix actualDinverse = Dinverse(T); Matrix actualDinverse = Dinverse(T);
CHECK(assert_equal(numericalH,actualDinverse)); CHECK(assert_equal(numericalH,actualDinverse,5e-5));
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -203,7 +207,7 @@ TEST( Pose3, compose_inverse)
{ {
Matrix actual = (T*inverse(T)).matrix(); Matrix actual = (T*inverse(T)).matrix();
Matrix expected = eye(4,4); Matrix expected = eye(4,4);
CHECK(assert_equal(actual,expected,error)); CHECK(assert_equal(actual,expected,1e-8));
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -211,7 +215,7 @@ TEST( Pose3, Dtransform_from1_a)
{ {
Matrix actualDtransform_from1 = Dtransform_from1(T, P); Matrix actualDtransform_from1 = Dtransform_from1(T, P);
Matrix numerical = numericalDerivative21(transform_from,T,P); Matrix numerical = numericalDerivative21(transform_from,T,P);
CHECK(assert_equal(numerical,actualDtransform_from1,error)); CHECK(assert_equal(numerical,actualDtransform_from1,1e-8));
} }
TEST( Pose3, Dtransform_from1_b) TEST( Pose3, Dtransform_from1_b)
@ -219,7 +223,7 @@ TEST( Pose3, Dtransform_from1_b)
Pose3 origin; Pose3 origin;
Matrix actualDtransform_from1 = Dtransform_from1(origin, P); Matrix actualDtransform_from1 = Dtransform_from1(origin, P);
Matrix numerical = numericalDerivative21(transform_from,origin,P); Matrix numerical = numericalDerivative21(transform_from,origin,P);
CHECK(assert_equal(numerical,actualDtransform_from1,error)); CHECK(assert_equal(numerical,actualDtransform_from1,1e-8));
} }
TEST( Pose3, Dtransform_from1_c) TEST( Pose3, Dtransform_from1_c)
@ -228,7 +232,7 @@ TEST( Pose3, Dtransform_from1_c)
Pose3 T0(R,origin); Pose3 T0(R,origin);
Matrix actualDtransform_from1 = Dtransform_from1(T0, P); Matrix actualDtransform_from1 = Dtransform_from1(T0, P);
Matrix numerical = numericalDerivative21(transform_from,T0,P); Matrix numerical = numericalDerivative21(transform_from,T0,P);
CHECK(assert_equal(numerical,actualDtransform_from1,error)); CHECK(assert_equal(numerical,actualDtransform_from1,1e-8));
} }
TEST( Pose3, Dtransform_from1_d) TEST( Pose3, Dtransform_from1_d)
@ -240,7 +244,7 @@ TEST( Pose3, Dtransform_from1_d)
//print(computed, "Dtransform_from1_d computed:"); //print(computed, "Dtransform_from1_d computed:");
Matrix numerical = numericalDerivative21(transform_from,T0,P); Matrix numerical = numericalDerivative21(transform_from,T0,P);
//print(numerical, "Dtransform_from1_d numerical:"); //print(numerical, "Dtransform_from1_d numerical:");
CHECK(assert_equal(numerical,actualDtransform_from1,error)); CHECK(assert_equal(numerical,actualDtransform_from1,1e-8));
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -248,7 +252,7 @@ TEST( Pose3, Dtransform_from2)
{ {
Matrix actualDtransform_from2 = Dtransform_from2(T); Matrix actualDtransform_from2 = Dtransform_from2(T);
Matrix numerical = numericalDerivative22(transform_from,T,P); Matrix numerical = numericalDerivative22(transform_from,T,P);
CHECK(assert_equal(numerical,actualDtransform_from2,error)); CHECK(assert_equal(numerical,actualDtransform_from2,1e-8));
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -256,7 +260,7 @@ TEST( Pose3, Dtransform_to1)
{ {
Matrix computed = Dtransform_to1(T, P); Matrix computed = Dtransform_to1(T, P);
Matrix numerical = numericalDerivative21(transform_to,T,P); Matrix numerical = numericalDerivative21(transform_to,T,P);
CHECK(assert_equal(numerical,computed,error)); CHECK(assert_equal(numerical,computed,1e-8));
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -264,7 +268,7 @@ TEST( Pose3, Dtransform_to2)
{ {
Matrix computed = Dtransform_to2(T,P); Matrix computed = Dtransform_to2(T,P);
Matrix numerical = numericalDerivative22(transform_to,T,P); Matrix numerical = numericalDerivative22(transform_to,T,P);
CHECK(assert_equal(numerical,computed,error)); CHECK(assert_equal(numerical,computed,1e-8));
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -314,7 +318,7 @@ TEST( Pose3, transformPose_to_origin)
{ {
// transform to origin // transform to origin
Pose3 actual = T3.transform_to(Pose3()); Pose3 actual = T3.transform_to(Pose3());
CHECK(assert_equal(T3, actual, error)); CHECK(assert_equal(T3, actual, 1e-8));
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -322,7 +326,7 @@ TEST( Pose3, transformPose_to_itself)
{ {
// transform to itself // transform to itself
Pose3 actual = T3.transform_to(T3); Pose3 actual = T3.transform_to(T3);
CHECK(assert_equal(Pose3(), actual, error)); CHECK(assert_equal(Pose3(), actual, 1e-8));
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -333,7 +337,7 @@ TEST( Pose3, transformPose_to_translation)
Pose3 pose2(r, Point3(21.,32.,13.)); Pose3 pose2(r, Point3(21.,32.,13.));
Pose3 actual = pose2.transform_to(Pose3(Rot3(), Point3(1,2,3))); Pose3 actual = pose2.transform_to(Pose3(Rot3(), Point3(1,2,3)));
Pose3 expected(r, Point3(20.,30.,10.)); Pose3 expected(r, Point3(20.,30.,10.));
CHECK(assert_equal(expected, actual, error)); CHECK(assert_equal(expected, actual, 1e-8));
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -380,7 +384,8 @@ 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));
#ifdef SLOW_BUT_CORRECT_EXPMAP #ifndef FASTER_BUT_INCORRECT_EXMAP
// todo: Frank - Below only works for correct "Agrawal06iros style expmap // todo: Frank - Below only works for correct "Agrawal06iros style expmap
// lines in canonical coordinates correspond to Abelian subgroups in SE(3) // lines in canonical coordinates correspond to Abelian subgroups in SE(3)
@ -406,7 +411,7 @@ TEST( Pose3, between )
CHECK(assert_equal(expected,actual)); CHECK(assert_equal(expected,actual));
Matrix numericalH1 = numericalDerivative21(between<Pose3> , T2, T3, 1e-5); Matrix numericalH1 = numericalDerivative21(between<Pose3> , T2, T3, 1e-5);
CHECK(assert_equal(numericalH1,actualDBetween1)); // chain rule does not work ?? CHECK(assert_equal(numericalH1,actualDBetween1,5e-5));
Matrix numericalH2 = numericalDerivative22(between<Pose3> , T2, T3, 1e-5); Matrix numericalH2 = numericalDerivative22(between<Pose3> , T2, T3, 1e-5);
CHECK(assert_equal(numericalH2,actualDBetween2)); CHECK(assert_equal(numericalH2,actualDBetween2));

9
cpp/testPose3Config.cpp
View File

@ -35,12 +35,19 @@ TEST( Pose3Config, pose3Circle )
/* ************************************************************************* */ /* ************************************************************************* */
TEST( Pose3Config, expmap ) TEST( Pose3Config, expmap )
{ {
// expected is circle shifted to East
Pose3Config expected; Pose3Config expected;
#ifdef FASTER_BUT_INCORRECT_EXMAP
// 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
// Note expmap coordinates are in global coordinates with non-compose expmap // Note expmap coordinates are in global coordinates with non-compose expmap
// so shifting to East requires little thought, different from with Pose2 !!! // so shifting to East requires little thought, different from with Pose2 !!!