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

Jacobian versions of compose and between

release/4.3a0
Richard Roberts 2010-05-24 05:40:59 +00:00
parent 3b06e6401a
commit 981082e2a4
3 changed files with 30 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
cpp/Point2.h
View File

@ -88,7 +88,13 @@ namespace gtsam {
inline Vector logmap(const Point2& dp) { return Vector_(2, dp.x(), dp.y()); }
/** "Compose", just adds the coordinates of two points. */
inline Point2 compose(const Point2& p1, const Point2& p0) { return p0+p1; }
inline Point2 compose(const Point2& p1, const Point2& p2) { return p1+p2; }
inline Point2 compose(const Point2& p1, const Point2& p2,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2=boost::none) {
if(H1) *H1 = eye(2);
if(H2) *H2 = eye(2);
return compose(p1, p2);
}
inline Matrix Dcompose1(const Point2& p1, const Point2& p0) {
return Matrix_(2,2,
1.0, 0.0,
@ -98,6 +104,15 @@ namespace gtsam {
1.0, 0.0,
0.0, 1.0); }
/** "Between", subtracts point coordinates */
inline Point2 between(const Point2& p1, const Point2& p2) { return p2-p1; }
inline Point2 between(const Point2& p1, const Point2& p2,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2=boost::none) {
if(H1) *H1 = -eye(2);
if(H2) *H2 = eye(2);
return between(p1, p2);
}
/** "Inverse" - negates each coordinate such that compose(p,inverse(p))=Point2() */
inline Point2 inverse(const Point2& p) { return Point2(-p.x(), -p.y()); }

8
cpp/Pose2.cpp
View File

@ -121,6 +121,14 @@ namespace gtsam {
/* ************************************************************************* */
// see doc/math.lyx, SE(2) section
Pose2 compose(const Pose2& p1, const Pose2& p2, boost::optional<Matrix&> H1,
boost::optional<Matrix&> H2) {
// TODO: inline and reuse?
if(H1) *H1 = AdjointMap(inverse(p2));
if(H2) *H2 = I3;
return p1*p2;
}
Matrix Dcompose1(const Pose2& p1, const Pose2& p2) {
return AdjointMap(inverse(p2));
}

9
cpp/Pose2.h
View File

@ -134,11 +134,14 @@ namespace gtsam {
/**
* compose this transformation onto another (first p1 and then p2)
*/
inline Pose2 compose(const Pose2& p0, const Pose2& p1)
{ return Pose2(p0.r()*p1.r(), p0.t() + p0.r()*p1.t());}
inline Pose2 operator*(const Pose2& p1, const Pose2& p2) {
return Pose2(p1.r()*p2.r(), p1.t() + p1.r()*p2.t()); }
inline Pose2 compose(const Pose2& p1, const Pose2& p2) { return p1*p2; }
Pose2 compose(const Pose2& p1, const Pose2& p2,
boost::optional<Matrix&> H1,
boost::optional<Matrix&> H2 = boost::none);
Matrix Dcompose1(const Pose2& p1, const Pose2& p2);
Matrix Dcompose2(const Pose2& p1, const Pose2& p2);
inline Pose2 operator*(const Pose2& p1, const Pose2& p0) { return compose(p1, p0);}
/**
* Return point coordinates in pose coordinate frame