the beautiful Jacobian of compose in pose2
parent
c40760485d
commit
87c8465315
|
@ -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) {
|
||||
|
|
|
@ -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 */
|
||||
|
||||
|
|
|
@ -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));
|
||||
|
|
Loading…
Reference in New Issue