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

changed the between factor in Pose2 using Optional Jacobian. This fixes build errors in unstable also.

release/4.3a0
nsrinivasan7 2014-11-29 15:39:43 -05:00
parent 6505e602d8
commit 4d495a0267
2 changed files with 4 additions and 71 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

60
gtsam/geometry/Pose2.cpp
View File

@ -162,30 +162,8 @@ Point2 Pose2::transform_from(const Point2& p,
} }
/* ************************************************************************* */ /* ************************************************************************* */
Pose2 Pose2::between(const Pose2& p2) const { Pose2 Pose2::between(const Pose2& p2, OptionalJacobian<3,3> H1,
// get cosines and sines from rotation matrices OptionalJacobian<3,3> H2) const {
const Rot2& R1 = r_, R2 = p2.r();
double c1=R1.c(), s1=R1.s(), c2=R2.c(), s2=R2.s();
// Assert that R1 and R2 are normalized
assert(std::abs(c1*c1 + s1*s1 - 1.0) < 1e-5 && std::abs(c2*c2 + s2*s2 - 1.0) < 1e-5);
// Calculate delta rotation = between(R1,R2)
double c = c1 * c2 + s1 * s2, s = -s1 * c2 + c1 * s2;
Rot2 R(Rot2::atan2(s,c)); // normalizes
// Calculate delta translation = unrotate(R1, dt);
Point2 dt = p2.t() - t_;
double x = dt.x(), y = dt.y();
// t = R1' * (t2-t1)
Point2 t(c1 * x + s1 * y, -s1 * x + c1 * y);
return Pose2(R,t);
}
/* ************************************************************************* */
Pose2 Pose2::between(const Pose2& p2, boost::optional<Matrix3&> H1,
boost::optional<Matrix3&> H2) const {
// get cosines and sines from rotation matrices // get cosines and sines from rotation matrices
const Rot2& R1 = r_, R2 = p2.r(); const Rot2& R1 = r_, R2 = p2.r();
double c1=R1.c(), s1=R1.s(), c2=R2.c(), s2=R2.s(); double c1=R1.c(), s1=R1.s(), c2=R2.c(), s2=R2.s();
@ -217,40 +195,6 @@ Pose2 Pose2::between(const Pose2& p2, boost::optional<Matrix3&> H1,
return Pose2(R,t); return Pose2(R,t);
} }
/* ************************************************************************* */
Pose2 Pose2::between(const Pose2& p2, boost::optional<Matrix&> H1,
boost::optional<Matrix&> H2) const {
// get cosines and sines from rotation matrices
const Rot2& R1 = r_, R2 = p2.r();
double c1=R1.c(), s1=R1.s(), c2=R2.c(), s2=R2.s();
// Assert that R1 and R2 are normalized
assert(std::abs(c1*c1 + s1*s1 - 1.0) < 1e-5 && std::abs(c2*c2 + s2*s2 - 1.0) < 1e-5);
// Calculate delta rotation = between(R1,R2)
double c = c1 * c2 + s1 * s2, s = -s1 * c2 + c1 * s2;
Rot2 R(Rot2::atan2(s,c)); // normalizes
// Calculate delta translation = unrotate(R1, dt);
Point2 dt = p2.t() - t_;
double x = dt.x(), y = dt.y();
// t = R1' * (t2-t1)
Point2 t(c1 * x + s1 * y, -s1 * x + c1 * y);
// FD: This is just -AdjointMap(between(p2,p1)) inlined and re-using above
if (H1) {
double dt1 = -s2 * x + c2 * y;
double dt2 = -c2 * x - s2 * y;
*H1 = (Matrix(3, 3) <<
-c, -s, dt1,
s, -c, dt2,
0.0, 0.0,-1.0).finished();
}
if (H2) *H2 = I3;
return Pose2(R,t);
}
/* ************************************************************************* */ /* ************************************************************************* */
Rot2 Pose2::bearing(const Point2& point, Rot2 Pose2::bearing(const Point2& point,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const { boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {

15
gtsam/geometry/Pose2.h
View File

@ -122,19 +122,8 @@ public:
/** /**
* 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& p2) const; Pose2 between(const Pose2& p2, OptionalJacobian<3,3> H1 = boost::none,
OptionalJacobian<3,3> H = boost::none) const;
/**
* Return relative pose between p1 and p2, in p1 coordinate frame
*/
Pose2 between(const Pose2& p2, boost::optional<Matrix3&> H1,
boost::optional<Matrix3&> H2) const;
/**
* Return relative pose between p1 and p2, in p1 coordinate frame
*/
Pose2 between(const Pose2& p2, boost::optional<Matrix&> H1,
boost::optional<Matrix&> H2) const;
/// @} /// @}
/// @name Manifold /// @name Manifold