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

Now uses Adjoint map for derivatives of inverse/compose/between, see doc/math.pdf

release/4.3a0
Frank Dellaert 2010-02-28 09:10:39 +00:00
parent 98aa08bcd0
commit 28f4493a2b
3 changed files with 62 additions and 34 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

46
cpp/Pose2.cpp
View File

@ -13,13 +13,13 @@ namespace gtsam {
/** Explicit instantiation of base class to export members */
INSTANTIATE_LIE(Pose2);
static const Matrix I3 = eye(3);
static const Matrix I3 = eye(3), Z12 = zeros(1,2);
static const Rot2 R_PI_2(0., 1.);
/* ************************************************************************* */
Matrix Pose2::matrix() const {
Matrix R = r_.matrix();
Matrix Z = zeros(1,2);
R = stack(2, &R, &Z);
R = stack(2, &R, &Z12);
Matrix T = Matrix_(3,1, t_.x(), t_.y(), 1.0);
return collect(2, &R, &T);
}
@ -34,6 +34,31 @@ namespace gtsam {
return t_.equals(q.t_, tol) && r_.equals(q.r_, tol);
}
/* ************************************************************************* */
// Calculate Adjoint map
// Ad_pose is 3*3 matrix that when applied to twist xi, returns Ad_pose(xi)
Matrix AdjointMap(const Pose2& p) {
const Rot2 R = p.r();
const Point2 t = p.t();
double c = R.c(), s = R.s(), x = t.x(), y = t.y();
return Matrix_(3,3,
c, -s, y,
s, c, -x,
0.0, 0.0, 1.0
);
}
/* ************************************************************************* */
Pose2 inverse(const Pose2& pose) {
const Rot2& R = pose.r();
const Point2& t = pose.t();
return Pose2(inverse(R), R.unrotate(Point2(-t.x(), -t.y())));
}
Matrix Dinverse(const Pose2& pose) {
return -AdjointMap(pose);
}
/* ************************************************************************* */
Point2 transform_to(const Pose2& pose, const Point2& point, boost::optional<
Matrix&> H1, boost::optional<Matrix&> H2) {
@ -56,20 +81,12 @@ namespace gtsam {
Matrix H; transform_to(pose, point, boost::none, H); return H;
}
/* ************************************************************************* */
static const Rot2 R_PI_2(0., 1.);
static Matrix DR1 = Matrix_(1, 3, 0., 0., 1.);
Matrix Dcompose1(const Pose2& p1, const Pose2& p2) {
Matrix R2_inv = p2.r().transpose();
Point2 Rt2_ = R_PI_2 * inverse(p2.r()) * p2.t();
Matrix Rt2 = Matrix_(2, 1, Rt2_.x(), Rt2_.y());
Matrix Dt1 = collect(2, &R2_inv, &Rt2);
return gtsam::stack(2, &Dt1, &DR1);
return AdjointMap(inverse(p2));
}
Matrix Dcompose2(const Pose2& p1, const Pose2& p2) {
return eye(3);
return I3;
}
/* ************************************************************************* */
@ -97,8 +114,7 @@ namespace gtsam {
double x = dt.x(), y = dt.y();
Point2 t(c1 * x + s1 * y, -s1 * x + c1 * y);
// FD: I don't understand this code (a performance-driven transformation
// from Richard's heavier code) but it works.
// FD: This is just -AdjointMap(between(p2,p1)) inlined and re-using above
if (H1) {
double dt1 = -s2 * x + c2 * y;
double dt2 = -c2 * x - s2 * y;

11
cpp/Pose2.h
View File

@ -71,10 +71,15 @@ namespace gtsam {
/** return DOF, dimensionality of tangent space = 3 */
inline size_t dim(const Pose2&) { return 3; }
/**
* Calculate Adjoint map
* Ad_pose is 3*3 matrix that when applied to twist xi, returns Ad_pose(xi)
*/
Matrix AdjointMap(const Pose2& p);
/** inverse transformation */
inline Pose2 inverse(const Pose2& pose) {
return Pose2(inverse(pose.r()),
pose.r().unrotate(Point2(-pose.t().x(), -pose.t().y()))); }
Pose2 inverse(const Pose2& pose);
Matrix Dinverse(const Pose2& pose);
/** compose this transformation onto another (first p1 and then p2) */
inline Pose2 compose(const Pose2& p0, const Pose2& p1) {

39
cpp/testPose2.cpp
View File

@ -148,8 +148,8 @@ TEST(Pose2, compose_a)
Pose2 pose2(M_PI/2.0, Point2(0.0, 2.0));
Pose2 actual = compose(pose1, pose2);
Matrix actualH1 = Dcompose1(pose1, pose2);
Matrix actualH2 = Dcompose2(pose1, pose2);
Matrix actualDcompose1 = Dcompose1(pose1, pose2);
Matrix actualDcompose2 = Dcompose2(pose1, pose2);
Pose2 expected(3.0*M_PI/4.0, Point2(-sqrt(0.5), 3.0*sqrt(0.5)));
CHECK(assert_equal(expected, actual));
@ -162,10 +162,10 @@ TEST(Pose2, compose_a)
Matrix expectedH2 = eye(3);
Matrix numericalH1 = numericalDerivative21<Pose2, Pose2, Pose2>(compose, pose1, pose2, 1e-5);
Matrix numericalH2 = numericalDerivative22<Pose2, Pose2, Pose2>(compose, pose1, pose2, 1e-5);
CHECK(assert_equal(expectedH1,actualH1));
CHECK(assert_equal(numericalH1,actualH1));
CHECK(assert_equal(expectedH2,actualH2));
CHECK(assert_equal(numericalH2,actualH2));
CHECK(assert_equal(expectedH1,actualDcompose1));
CHECK(assert_equal(numericalH1,actualDcompose1));
CHECK(assert_equal(expectedH2,actualDcompose2));
CHECK(assert_equal(numericalH2,actualDcompose2));
Point2 point(sqrt(0.5), 3.0*sqrt(0.5));
Point2 expected_point(-1.0, -1.0);
@ -186,13 +186,13 @@ TEST(Pose2, compose_b)
Pose2 pose_actual_op = pose1 * pose2;
Pose2 pose_actual_fcn = compose(pose1, pose2);
Matrix actualH1 = Dcompose1(pose1, pose2);
Matrix actualH2 = Dcompose2(pose1, pose2);
Matrix actualDcompose1 = Dcompose1(pose1, pose2);
Matrix actualDcompose2 = Dcompose2(pose1, pose2);
Matrix numericalH1 = numericalDerivative21<Pose2, Pose2, Pose2>(compose, pose1, pose2, 1e-5);
Matrix numericalH2 = numericalDerivative22<Pose2, Pose2, Pose2>(compose, pose1, pose2, 1e-5);
CHECK(assert_equal(numericalH1,actualH1));
CHECK(assert_equal(numericalH2,actualH2));
CHECK(assert_equal(numericalH1,actualDcompose1));
CHECK(assert_equal(numericalH2,actualDcompose2));
CHECK(assert_equal(pose_expected, pose_actual_op));
CHECK(assert_equal(pose_expected, pose_actual_fcn));
@ -209,13 +209,13 @@ TEST(Pose2, compose_c)
Pose2 pose_actual_op = pose1 * pose2;
Pose2 pose_actual_fcn = compose(pose1,pose2);
Matrix actualH1 = Dcompose1(pose1, pose2);
Matrix actualH2 = Dcompose2(pose1, pose2);
Matrix actualDcompose1 = Dcompose1(pose1, pose2);
Matrix actualDcompose2 = Dcompose2(pose1, pose2);
Matrix numericalH1 = numericalDerivative21<Pose2, Pose2, Pose2>(compose, pose1, pose2, 1e-5);
Matrix numericalH2 = numericalDerivative22<Pose2, Pose2, Pose2>(compose, pose1, pose2, 1e-5);
CHECK(assert_equal(numericalH1,actualH1));
CHECK(assert_equal(numericalH2,actualH2));
CHECK(assert_equal(numericalH1,actualDcompose1));
CHECK(assert_equal(numericalH2,actualDcompose2));
CHECK(assert_equal(pose_expected, pose_actual_op));
CHECK(assert_equal(pose_expected, pose_actual_fcn));
@ -234,8 +234,12 @@ TEST(Pose2, inverse )
Point2 l(4,5), g(-4,6);
CHECK(assert_equal(g,gTl*l));
CHECK(assert_equal(l,lTg*g));
}
// Check derivative
Matrix numericalH = numericalDerivative11<Pose2,Pose2>(inverse, lTg, 1e-5);
Matrix actualDinverse = Dinverse(lTg);
CHECK(assert_equal(numericalH,actualDinverse));
}
/* ************************************************************************* */
Vector homogeneous(const Point2& p) {
@ -304,7 +308,7 @@ TEST( Pose2, between )
Matrix actualH1,actualH2;
Pose2 expected(M_PI_2, Point2(2,2));
Pose2 actual1 = between(gT1,gT2); // gT2 * 1Tg does not make sense !!!!!
Pose2 actual1 = between(gT1,gT2);
Pose2 actual2 = between(gT1,gT2,actualH1,actualH2);
CHECK(assert_equal(expected,actual1));
CHECK(assert_equal(expected,actual2));
@ -317,6 +321,8 @@ TEST( Pose2, between )
Matrix numericalH1 = numericalDerivative21(between<Pose2>, gT1, gT2, 1e-5);
CHECK(assert_equal(expectedH1,actualH1));
CHECK(assert_equal(numericalH1,actualH1));
// Assert H1 = -AdjointMap(between(p2,p1)) as in doc/math.lyx
CHECK(assert_equal(-AdjointMap(between(gT2,gT1)),actualH1));
Matrix expectedH2 = Matrix_(3,3,
1.0, 0.0, 0.0,
@ -326,6 +332,7 @@ TEST( Pose2, between )
Matrix numericalH2 = numericalDerivative22(between<Pose2>, gT1, gT2, 1e-5);
CHECK(assert_equal(expectedH2,actualH2));
CHECK(assert_equal(numericalH2,actualH2));
}
/* ************************************************************************* */