the beautiful Jacobian of compose in pose2

release/4.3a0
Kai Ni 2010-02-26 10:55:41 +00:00
parent c40760485d
commit 87c8465315
3 changed files with 51 additions and 3 deletions

View File

@ -56,6 +56,22 @@ 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);
}
Matrix Dcompose2(const Pose2& p1, const Pose2& p2) {
return eye(3);
}
/* ************************************************************************* */
Pose2 between(const Pose2& p1, const Pose2& p2, boost::optional<Matrix&> H1,
boost::optional<Matrix&> H2) {

View File

@ -76,9 +76,11 @@ namespace gtsam {
return Pose2(inverse(pose.r()),
pose.r().unrotate(Point2(-pose.t().x(), -pose.t().y()))); }
/** compose this transformation onto another (pre-multiply this*p1) */
/** compose this transformation onto another (first p1 and then p2) */
inline Pose2 compose(const Pose2& p0, const Pose2& p1) {
return Pose2(p0.r()*p1.r(), p0.t() + p0.r()*p1.t()); }
return Pose2(p0.r()*p1.r(), p0.t() + p0.r()*p1.t()); }
Matrix Dcompose1(const Pose2& p1, const Pose2& p2);
Matrix Dcompose2(const Pose2& p1, const Pose2& p2);
/** exponential and log maps around identity */

View File

@ -120,10 +120,26 @@ TEST(Pose2, compose_a)
Pose2 pose1(M_PI/4.0, Point2(sqrt(0.5), sqrt(0.5)));
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);
Pose2 expected(3.0*M_PI/4.0, Point2(-sqrt(0.5), 3.0*sqrt(0.5)));
Pose2 actual = pose1 * pose2;
CHECK(assert_equal(expected, actual));
Matrix expectedH1 = Matrix_(3,3,
0.0, 1.0, 0.0,
-1.0, 0.0, 2.0,
0.0, 0.0, 1.0
);
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));
Point2 point(sqrt(0.5), 3.0*sqrt(0.5));
Point2 expected_point(-1.0, -1.0);
Point2 actual_point1 = transform_to(pose1 * pose2, point);
@ -143,6 +159,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 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(pose_expected, pose_actual_op));
CHECK(assert_equal(pose_expected, pose_actual_fcn));
@ -159,6 +182,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 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(pose_expected, pose_actual_op));
CHECK(assert_equal(pose_expected, pose_actual_fcn));