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.
Frank Dellaert
parent
b4a9ad2a9c
commit
26304b749a
|
|
@ -16,7 +16,7 @@ namespace gtsam {
|
|||
/** Explicit instantiation of base class to export members */
|
||||
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
|
||||
|
|
@ -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?) */
|
||||
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
|
||||
|
||||
/* incorrect versions for which we know how to compute derivatives */
|
||||
|
|
@ -96,7 +104,23 @@ namespace gtsam {
|
|||
return concatVectors(2, &w, &u);
|
||||
}
|
||||
|
||||
#endif
|
||||
/** 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
|
||||
|
||||
/* ************************************************************************* */
|
||||
Matrix Pose3::matrix() const {
|
||||
|
|
@ -120,7 +144,7 @@ namespace gtsam {
|
|||
|
||||
/* ************************************************************************* */
|
||||
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();
|
||||
Matrix DR = R*skewSymmetric(-p.x(), -p.y(), -p.z());
|
||||
return collect(2,&DR,&R);
|
||||
|
|
@ -142,12 +166,17 @@ namespace gtsam {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
// see math.lyx, derivative of action
|
||||
Matrix Dtransform_to1(const Pose3& pose, const Point3& p) {
|
||||
Point3 q = transform_to(pose,p);
|
||||
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
|
||||
return collect(2,&DR,&DT);
|
||||
}
|
||||
#endif
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Matrix Dtransform_to2(const Pose3& pose, const Point3& p) {
|
||||
|
|
@ -157,8 +186,7 @@ namespace gtsam {
|
|||
/* ************************************************************************* */
|
||||
// compose = Pose3(compose(R1,R2),transform_from(p1,t2)
|
||||
Matrix Dcompose1(const Pose3& p1, const Pose3& p2) {
|
||||
#ifdef SLOW_BUT_CORRECT_EXPMAP
|
||||
// actually does NOT pan out at the moment
|
||||
#ifndef FASTER_BUT_INCORRECT_EXMAP
|
||||
return AdjointMap(inverse(p2));
|
||||
#else
|
||||
const Rot3& R2 = p2.rotation();
|
||||
|
|
@ -171,7 +199,8 @@ namespace gtsam {
|
|||
}
|
||||
|
||||
Matrix Dcompose2(const Pose3& p1, const Pose3& p2) {
|
||||
#ifdef SLOW_BUT_CORRECT_EXPMAP
|
||||
#ifndef FASTER_BUT_INCORRECT_EXMAP
|
||||
|
||||
return I6;
|
||||
#else
|
||||
Matrix R1 = p1.rotation().matrix();
|
||||
|
|
@ -185,8 +214,7 @@ namespace gtsam {
|
|||
// inverse = Pose3(inverse(R),-unrotate(R,t));
|
||||
// TODO: combined function will save !
|
||||
Matrix Dinverse(const Pose3& p) {
|
||||
#ifdef SLOW_BUT_CORRECT_EXPMAP
|
||||
// actually does NOT pan out at the moment
|
||||
#ifndef FASTER_BUT_INCORRECT_EXMAP
|
||||
return - AdjointMap(p);
|
||||
#else
|
||||
const Rot3& R = p.rotation();
|
||||
|
|
|
|||
18
cpp/Pose3.h
18
cpp/Pose3.h
|
|
@ -108,21 +108,11 @@ namespace gtsam {
|
|||
* coordinates of a pose. */
|
||||
Vector logmap(const Pose3& p);
|
||||
|
||||
/** todo: 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 */
|
||||
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)));
|
||||
}
|
||||
/** Exponential map around another pose */
|
||||
Pose3 expmap(const Pose3& T, const Vector& d);
|
||||
|
||||
/** Independently computes the logmap of the translation and rotation. */
|
||||
template<> inline Vector logmap<Pose3>(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);
|
||||
}
|
||||
/** Logarithm map around another pose T1 */
|
||||
Vector logmap(const Pose3& T1, const Pose3& T2);
|
||||
|
||||
/** receives the point in Pose coordinates and transforms it to world coordinates */
|
||||
Point3 transform_from(const Pose3& pose, const Point3& p);
|
||||
|
|
|
|||
|
|
@ -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 T3(rodriguez(-90, 0, 0), Point3(1, 2, 3));
|
||||
|
||||
double error = 1e-8;
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( Pose3, equals)
|
||||
{
|
||||
|
|
@ -36,8 +34,13 @@ TEST( Pose3, expmap_a)
|
|||
fill(v.begin(), v.end(), 0);
|
||||
v(0) = 0.3;
|
||||
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;
|
||||
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)
|
||||
|
|
@ -49,7 +52,8 @@ TEST(Pose3, expmap_b)
|
|||
CHECK(assert_equal(expected, p2));
|
||||
}
|
||||
|
||||
#ifdef SLOW_BUT_CORRECT_EXPMAP
|
||||
#ifndef FASTER_BUT_INCORRECT_EXMAP
|
||||
|
||||
/* ************************************************************************* */
|
||||
// test case for screw motion in the plane
|
||||
namespace screw {
|
||||
|
|
@ -147,11 +151,11 @@ TEST( Pose3, compose )
|
|||
{
|
||||
Matrix actual = (T2*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 actualDcompose1 = Dcompose1(T2, T2);
|
||||
CHECK(assert_equal(numericalH1,actualDcompose1));
|
||||
CHECK(assert_equal(numericalH1,actualDcompose1,5e-5));
|
||||
|
||||
Matrix actualDcompose2 = Dcompose2(T2, T2);
|
||||
Matrix numericalH2 = numericalDerivative22<Pose3,Pose3,Pose3>(compose, T2, T2, 1e-5);
|
||||
|
|
@ -164,11 +168,11 @@ TEST( Pose3, compose2 )
|
|||
const Pose3& T1 = T;
|
||||
Matrix actual = (T1*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 actualDcompose1 = Dcompose1(T1, T2);
|
||||
CHECK(assert_equal(numericalH1,actualDcompose1));
|
||||
CHECK(assert_equal(numericalH1,actualDcompose1,5e-5));
|
||||
|
||||
Matrix actualDcompose2 = Dcompose2(T1, T2);
|
||||
Matrix numericalH2 = numericalDerivative22<Pose3,Pose3,Pose3>(compose, T1, T2, 1e-5);
|
||||
|
|
@ -179,7 +183,7 @@ TEST( Pose3, inverse)
|
|||
{
|
||||
Matrix actual = 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 actualDinverse = Dinverse(T);
|
||||
|
|
@ -195,7 +199,7 @@ TEST( Pose3, inverseDerivatives2)
|
|||
|
||||
Matrix numericalH = numericalDerivative11<Pose3,Pose3>(inverse, T, 1e-5);
|
||||
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 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 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)
|
||||
|
|
@ -219,7 +223,7 @@ TEST( Pose3, Dtransform_from1_b)
|
|||
Pose3 origin;
|
||||
Matrix actualDtransform_from1 = Dtransform_from1(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)
|
||||
|
|
@ -228,7 +232,7 @@ TEST( Pose3, Dtransform_from1_c)
|
|||
Pose3 T0(R,origin);
|
||||
Matrix actualDtransform_from1 = Dtransform_from1(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)
|
||||
|
|
@ -240,7 +244,7 @@ TEST( Pose3, Dtransform_from1_d)
|
|||
//print(computed, "Dtransform_from1_d computed:");
|
||||
Matrix numerical = numericalDerivative21(transform_from,T0,P);
|
||||
//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 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 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 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
|
||||
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
|
||||
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 actual = pose2.transform_to(Pose3(Rot3(), Point3(1,2,3)));
|
||||
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(assert_equal(d12,-d21));
|
||||
|
||||
#ifdef SLOW_BUT_CORRECT_EXPMAP
|
||||
#ifndef FASTER_BUT_INCORRECT_EXMAP
|
||||
|
||||
|
||||
// todo: Frank - Below only works for correct "Agrawal06iros style expmap
|
||||
// lines in canonical coordinates correspond to Abelian subgroups in SE(3)
|
||||
|
|
@ -406,7 +411,7 @@ TEST( Pose3, between )
|
|||
CHECK(assert_equal(expected,actual));
|
||||
|
||||
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);
|
||||
CHECK(assert_equal(numericalH2,actualDBetween2));
|
||||
|
|
|
|||
|
|
@ -35,12 +35,19 @@ TEST( Pose3Config, pose3Circle )
|
|||
/* ************************************************************************* */
|
||||
TEST( Pose3Config, expmap )
|
||||
{
|
||||
// expected is circle shifted to East
|
||||
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(1, Pose3(R2, Point3( 0.1, 1, 0)));
|
||||
expected.insert(2, Pose3(R3, Point3(-0.9, 0, 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
|
||||
// so shifting to East requires little thought, different from with Pose2 !!!
|
||||
|
|
|
|||
Loading…
Reference in New Issue