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

14
cpp/Pose2.h
View File

@ -100,19 +100,21 @@ namespace gtsam {
/** Return point coordinates in pose coordinate frame */
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,
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);
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2);
/** 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);
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2);
/** Return relative pose between p1 and p2, in p1 coordinate frame */
Pose2 between(const Pose2& p1, const Pose2& p2,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2);
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2);
/** same as compose (pre-multiply this*p1) */
inline Pose2 operator*(const Pose2& p1, const Pose2& p0) { return compose(p1, p0); }

19
cpp/testPose2.cpp
View File

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