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

Improved range and derivatives (no more rotating of translation, as range is invariant to it)

release/4.3a0
Frank Dellaert 2013-06-22 05:52:14 +00:00
parent e35c73388f
commit e618df7bfa
1 changed files with 17 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

30
gtsam/geometry/Pose2.cpp
View File

@ -221,27 +221,31 @@ Rot2 Pose2::bearing(const Pose2& point,
/* ************************************************************************* */ /* ************************************************************************* */
double Pose2::range(const Point2& point, double Pose2::range(const Point2& point,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const { boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
if (!H1 && !H2) return transform_to(point).norm(); Point2 d = point - t_;
Point2 d = transform_to(point, H1, H2); if (!H1 && !H2) return d.norm();
Matrix H; Matrix H;
double r = d.norm(H); double r = d.norm(H);
if (H1) *H1 = H * (*H1); if (H1) *H1 = H * Matrix_(2, 3,
if (H2) *H2 = H * (*H2); -r_.c(), r_.s(), 0.0,
-r_.s(), -r_.c(), 0.0);
if (H2) *H2 = H;
return r; return r;
} }
/* ************************************************************************* */ /* ************************************************************************* */
double Pose2::range(const Pose2& point, double Pose2::range(const Pose2& pose2,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H1,
boost::optional<Matrix&> H2) const { boost::optional<Matrix&> H2) const {
double r = range(point.t(), H1, H2); Point2 d = pose2.t() - t_;
if (H2) { if (!H1 && !H2) return d.norm();
// NOTE: expmap changes the orientation of expmap direction, Matrix H;
// so we must rotate the jacobian double r = d.norm(H);
Matrix H2_ = *H2 * point.r().matrix(); if (H1) *H1 = H * Matrix_(2, 3,
*H2 = zeros(1, 3); -r_.c(), r_.s(), 0.0,
insertSub(*H2, H2_, 0, 0); -r_.s(), -r_.c(), 0.0);
} if (H2) *H2 = H * Matrix_(2, 3,
pose2.r_.c(), -pose2.r_.s(), 0.0,
pose2.r_.s(), pose2.r_.c(), 0.0);
return r; return r;
} }