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

Fixed calls of adjointMap and tested correct exponential map derivatives based on them

release/4.3a0
Frank Dellaert 2012-01-04 14:23:50 +00:00
parent 94049675a4
commit 53946b28d0
2 changed files with 24 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
gtsam/geometry/Pose3.cpp
View File

@ -31,7 +31,7 @@ namespace gtsam {
static const Matrix I3 = eye(3), Z3 = zeros(3, 3);
#ifdef CORRECT_POSE3_EXMAP
static const _I3=-I3, I6 = eye(6);
static const Matrix _I3=-I3, I6 = eye(6);
#endif
/* ************************************************************************* */
@ -100,13 +100,13 @@ namespace gtsam {
/* ************************************************************************* */
/* ************************************************************************* */
Pose3 Pose3::retract(const Vector& d) {
Pose3 Pose3::retract(const Vector& d) const {
return compose(Expmap(d));
}
/* ************************************************************************* */
Vector Pose3::localCoordinates(const Pose3& T1, const Pose3& T2) {
return Logmap(T1.between(T2));
Vector Pose3::localCoordinates(const Pose3& T2) const {
return Logmap(between(T2));
}
#else
@ -188,7 +188,7 @@ namespace gtsam {
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
if (H1) {
#ifdef CORRECT_POSE3_EXMAP
*H1 = adjointMap(inverse(p2)); // FIXME: this function doesn't exist with this interface
*H1 = p2.inverse().adjointMap();
#else
const Rot3& R2 = p2.rotation();
const Point3& t2 = p2.translation();
@ -216,8 +216,7 @@ namespace gtsam {
Pose3 Pose3::inverse(boost::optional<Matrix&> H1) const {
if (H1)
#ifdef CORRECT_POSE3_EXMAP
// FIXME: this function doesn't exist with this interface - should this be "*H1 = -adjointMap();" ?
{ *H1 = - adjointMap(p); }
{ *H1 = - adjointMap(); }
#else
{
Matrix Rt = R_.transpose();

20
gtsam/geometry/tests/testPose3.cpp
View File

@ -100,6 +100,7 @@ namespace screw {
}
/* ************************************************************************* */
// Checks correct exponential map (Expmap) with brute force matrix exponential
TEST(Pose3, expmap_c_full)
{
EXPECT(assert_equal(screw::expected, expm<Pose3>(screw::xi),1e-6));
@ -124,7 +125,7 @@ TEST(Pose3, Adjoint_full)
}
/* ************************************************************************* */
/** Agrawal06iros version */
/** Agrawal06iros version of exponential map */
Pose3 Agrawal06iros(const Vector& xi) {
Vector w = xi.head(3);
Vector v = xi.tail(3);
@ -181,6 +182,8 @@ TEST(Pose3, Adjoint_compose_full)
}
/* ************************************************************************* */
// Check compose and its pushforward
// NOTE: testing::compose<Pose3>(t1,t2) = t1.compose(t2) (see lieProxies.h)
TEST( Pose3, compose )
{
Matrix actual = (T2*T2).matrix();
@ -192,12 +195,16 @@ TEST( Pose3, compose )
Matrix numericalH1 = numericalDerivative21(testing::compose<Pose3>, T2, T2);
EXPECT(assert_equal(numericalH1,actualDcompose1,5e-3));
#ifdef CORRECT_POSE3_EXMAP
EXPECT(assert_equal(T2.inverse().adjointMap(),actualDcompose1,5e-3));
#endif
Matrix numericalH2 = numericalDerivative22(testing::compose<Pose3>, T2, T2);
EXPECT(assert_equal(numericalH2,actualDcompose2,1e-4));
}
/* ************************************************************************* */
// Check compose and its pushforward, another case
TEST( Pose3, compose2 )
{
const Pose3& T1 = T;
@ -210,6 +217,9 @@ TEST( Pose3, compose2 )
Matrix numericalH1 = numericalDerivative21(testing::compose<Pose3>, T1, T2);
EXPECT(assert_equal(numericalH1,actualDcompose1,5e-3));
#ifdef CORRECT_POSE3_EXMAP
EXPECT(assert_equal(T2.inverse().adjointMap(),actualDcompose1,5e-3));
#endif
Matrix numericalH2 = numericalDerivative22(testing::compose<Pose3>, T1, T2);
EXPECT(assert_equal(numericalH2,actualDcompose2,1e-5));
@ -225,6 +235,9 @@ TEST( Pose3, inverse)
Matrix numericalH = numericalDerivative11(testing::inverse<Pose3>, T);
EXPECT(assert_equal(numericalH,actualDinverse,5e-3));
#ifdef CORRECT_POSE3_EXMAP
EXPECT(assert_equal(-T.adjointMap(),actualDinverse,5e-3));
#endif
}
/* ************************************************************************* */
@ -238,6 +251,9 @@ TEST( Pose3, inverseDerivatives2)
Matrix actualDinverse;
T.inverse(actualDinverse);
EXPECT(assert_equal(numericalH,actualDinverse,5e-3));
#ifdef CORRECT_POSE3_EXMAP
EXPECT(assert_equal(-T.adjointMap(),actualDinverse,5e-3));
#endif
}
/* ************************************************************************* */
@ -460,7 +476,7 @@ TEST(Pose3, manifold)
// 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))
EXPECT(assert_equal(Pose3::Expmap(-d),inverse(Pose3::Expmap(d))));
EXPECT(assert_equal(Pose3::Expmap(-d),Pose3::Expmap(d).inverse()));
// exp(5d)=exp(2*d+3*d)=exp(2*d)exp(3*d)=exp(3*d)exp(2*d)
Pose3 T2 = Pose3::Expmap(2*d);
Pose3 T3 = Pose3::Expmap(3*d);