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

re-factored so corresponds to math.lyx

release/4.3a0
Frank Dellaert 2010-03-01 01:33:45 +00:00
parent 6e03783465
commit b943644ddd
1 changed files with 11 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
cpp/Pose2.cpp
View File

@ -90,12 +90,18 @@ namespace gtsam {
}
/* ************************************************************************* */
Point2 transform_from(const Pose2& pose, const Point2& point,
// see doc/math.lyx, SE(2) section
Point2 transform_from(const Pose2& pose, const Point2& p,
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;
const Rot2& rot = pose.r();
const Point2 q = rot * p;
if (H1 || H2) {
const Matrix R = rot.matrix();
const Matrix Drotate1 = Matrix_(2, 1, -q.y(), q.x());
if (H1) *H1 = collect(2, &R, &Drotate1); // [R R_{pi/2}q]
if (H2) *H2 = R; // R
}
return q + pose.t();
}
/* ************************************************************************* */