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

the derivative for transform_from

release/4.3a0
Kai Ni 2010-02-28 08:51:43 +00:00
parent d894e23a06
commit b58f7b8ea7
3 changed files with 38 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
cpp/Pose2.cpp
View File

@ -72,6 +72,15 @@ namespace gtsam {
return eye(3);
}
/* ************************************************************************* */
Point2 transform_from(const Pose2& pose, const Point2& point,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) {
Point2 point_transformed = rotate(pose.r(), point, H1, H2) + pose.t();
Matrix R = pose.r().matrix();
if (H1) *H1 = collect(2, &R, &(*H1));
return point_transformed;
}
/* ************************************************************************* */
Pose2 between(const Pose2& p1, const Pose2& p2, boost::optional<Matrix&> H1,
boost::optional<Matrix&> H2) {

4
cpp/Pose2.h
View File

@ -102,8 +102,8 @@ namespace gtsam {
Matrix Dtransform_to2(const Pose2& pose, const Point2& point);
/** Return point coordinates in global frame */
inline Point2 transform_from(const Pose2& pose, const Point2& point) {
return rotate(pose.r(), point)+pose.t(); }
Point2 transform_from(const Pose2& pose, const Point2& point,
boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 = boost::none);
/** Return relative pose between p1 and p2, in p1 coordinate frame */
Pose2 between(const Pose2& p1, const Pose2& p2,

27
cpp/testPose2.cpp
View File

@ -113,6 +113,33 @@ TEST( Pose2, transform_to )
CHECK(assert_equal(numericalH2,actualH2));
}
/* ************************************************************************* */
Point2 transform_from_proxy(const Pose2& pose, const Point2& point) {
return transform_from(pose, point);
}
TEST (Pose2, transform_from)
{
Pose2 pose(1., 0., M_PI_2);
Point2 pt(2., 1.);
Matrix H1, H2;
Point2 actual = transform_from(pose, pt, H1, H2);
Point2 expected(0., 2.);
CHECK(assert_equal(expected, actual));
Matrix H1_expected = Matrix_(2, 3, 0., -1., -2., 1., 0., -1.);
Matrix H2_expected = Matrix_(2, 2, 0., -1., 1., 0.);
Matrix numericalH1 = numericalDerivative21(transform_from_proxy, pose, pt, 1e-5);
CHECK(assert_equal(H1_expected, H1));
CHECK(assert_equal(H1_expected, numericalH1));
Matrix numericalH2 = numericalDerivative22(transform_from_proxy, pose, pt, 1e-5);
CHECK(assert_equal(H2_expected, H2));
CHECK(assert_equal(H2_expected, numericalH2));
}
/* ************************************************************************* */
TEST(Pose2, compose_a)
{