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

combined version only

release/4.3a0
Frank Dellaert 2010-03-02 02:27:09 +00:00
parent 1093317fdc
commit f956bae6a4
3 changed files with 23 additions and 32 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
cpp/Pose2.cpp
View File

@ -60,6 +60,7 @@ namespace gtsam {
} }
/* ************************************************************************* */ /* ************************************************************************* */
// see doc/math.lyx, SE(2) section
Point2 transform_to(const Pose2& pose, const Point2& point, boost::optional< Point2 transform_to(const Pose2& pose, const Point2& point, boost::optional<
Matrix&> H1, boost::optional<Matrix&> H2) { Matrix&> H1, boost::optional<Matrix&> H2) {
const Rot2& R = pose.r(); const Rot2& R = pose.r();
@ -73,13 +74,8 @@ namespace gtsam {
return q; return q;
} }
Matrix Dtransform_to1(const Pose2& pose, const Point2& point) { /* ************************************************************************* */
Matrix H; transform_to(pose, point, H, boost::none); return H; // see doc/math.lyx, SE(2) section
}
Matrix Dtransform_to2(const Pose2& pose, const Point2& point) {
Matrix H; transform_to(pose, point, boost::none, H); return H;
}
Matrix Dcompose1(const Pose2& p1, const Pose2& p2) { Matrix Dcompose1(const Pose2& p1, const Pose2& p2) {
return AdjointMap(inverse(p2)); return AdjointMap(inverse(p2));
@ -145,11 +141,11 @@ namespace gtsam {
Rot2 bearing(const Pose2& pose, const Point2& point, Rot2 bearing(const Pose2& pose, const Point2& point,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) { boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) {
if (!H1 && !H2) return bearing(pose, point); if (!H1 && !H2) return bearing(pose, point);
Point2 d = transform_to(pose, point); Point2 d = transform_to(pose, point, H1, H2);
Matrix D_result_d; Matrix D_result_d;
Rot2 result = relativeBearing(d, D_result_d); Rot2 result = relativeBearing(d, D_result_d);
if (H1) *H1 = D_result_d * Dtransform_to1(pose, point); if (H1) *H1 = D_result_d * (*H1);
if (H2) *H2 = D_result_d * Dtransform_to2(pose, point); if (H2) *H2 = D_result_d * (*H2);
return result; return result;
} }
@ -162,11 +158,11 @@ namespace gtsam {
double range(const Pose2& pose, const Point2& point, double range(const Pose2& pose, const Point2& point,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) { boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) {
if (!H1 && !H2) return range(pose, point); if (!H1 && !H2) return range(pose, point);
Point2 d = transform_to(pose, point); Point2 d = transform_to(pose, point, H1, H2);
double x = d.x(), y = d.y(), d2 = x * x + y * y, n = sqrt(d2); double x = d.x(), y = d.y(), d2 = x * x + y * y, n = sqrt(d2);
Matrix D_result_d = Matrix_(1, 2, x / n, y / n); Matrix D_result_d = Matrix_(1, 2, x / n, y / n);
if (H1) *H1 = D_result_d * Dtransform_to1(pose, point); if (H1) *H1 = D_result_d * (*H1);
if (H2) *H2 = D_result_d * Dtransform_to2(pose, point); if (H2) *H2 = D_result_d * (*H2);
return n; return n;
} }

10
cpp/Pose2.h
View File

@ -100,15 +100,17 @@ namespace gtsam {
/** Return point coordinates in pose coordinate frame */ /** Return point coordinates in pose coordinate frame */
inline Point2 transform_to(const Pose2& pose, const Point2& point) { inline Point2 transform_to(const Pose2& pose, const Point2& point) {
return unrotate(pose.r(), point-pose.t()); } return unrotate(pose.r(), point - pose.t());
}
Point2 transform_to(const Pose2& pose, const Point2& point, Point2 transform_to(const Pose2& pose, const Point2& point,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2); boost::optional<Matrix&> H1, boost::optional<Matrix&> H2);
Matrix Dtransform_to1(const Pose2& pose, const Point2& point);
Matrix Dtransform_to2(const Pose2& pose, const Point2& point);
/** Return point coordinates in global frame */ /** 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, Point2 transform_from(const Pose2& pose, const Point2& point,
boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 = boost::none); boost::optional<Matrix&> H1, boost::optional<Matrix&> H2);
/** Return relative pose between p1 and p2, in p1 coordinate frame */ /** Return relative pose between p1 and p2, in p1 coordinate frame */
Pose2 between(const Pose2& p1, const Pose2& p2, Pose2 between(const Pose2& p1, const Pose2& p2,

19
cpp/testPose2.cpp
View File

@ -84,33 +84,26 @@ TEST(Pose2, logmap) {
/* ************************************************************************* */ /* ************************************************************************* */
TEST( Pose2, transform_to ) TEST( Pose2, transform_to )
{ {
//cout << "transform_to" << endl;
Pose2 pose(M_PI_2, Point2(1,2)); // robot at (1,2) looking towards y Pose2 pose(M_PI_2, Point2(1,2)); // robot at (1,2) looking towards y
Point2 point(-1,4); // landmark at (-1,4) Point2 point(-1,4); // landmark at (-1,4)
// expected // expected
Point2 expected(2,2); Point2 expected(2,2);
// Matrix expectedH1 = Matrix_(2,3, 0.0, -1.0, 2.0, 1.0, 0.0, -2.0); Matrix expectedH1 = Matrix_(2,3, -1.0, 0.0, 2.0, 0.0, -1.0, -2.0);
// Matrix expectedH2 = Matrix_(2,2, 0.0, 1.0, -1.0, 0.0); Matrix expectedH2 = Matrix_(2,2, 0.0, 1.0, -1.0, 0.0);
// actual // actual
Point2 actual = transform_to(pose,point); Matrix actualH1, actualH2;
Matrix actualH1 = Dtransform_to1(pose,point); Point2 actual = transform_to(pose,point, actualH1, actualH2);
Matrix actualH2 = Dtransform_to2(pose,point);
CHECK(assert_equal(expected,actual)); CHECK(assert_equal(expected,actual));
// CHECK(assert_equal(expectedH1,actualH1));
// CHECK(assert_equal(expectedH2,actualH2));
CHECK(assert_equal(expectedH1,actualH1));
Matrix numericalH1 = numericalDerivative21(transform_to, pose, point, 1e-5); Matrix numericalH1 = numericalDerivative21(transform_to, pose, point, 1e-5);
CHECK(assert_equal(numericalH1,actualH1)); CHECK(assert_equal(numericalH1,actualH1));
CHECK(assert_equal(expectedH2,actualH2));
Matrix numericalH2 = numericalDerivative22(transform_to, pose, point, 1e-5); Matrix numericalH2 = numericalDerivative22(transform_to, pose, point, 1e-5);
CHECK(assert_equal(numericalH2,actualH2)); CHECK(assert_equal(numericalH2,actualH2));
transform_to(pose,point,actualH1,actualH2);
CHECK(assert_equal(numericalH1,actualH1));
CHECK(assert_equal(numericalH2,actualH2));
} }
/* ************************************************************************* */ /* ************************************************************************* */