finished Pose2.cpp using @dellaert 's temporary matrices idea. Still have a couple of functions that are not fixed for instance wedge<Pose2>, a template specialization from Lie.h.
nsrinivasan7
parent
952f4d7810
commit
490e75b103
|
|
@ -33,15 +33,21 @@ INSTANTIATE_LIE(Pose2);
|
|||
/** instantiate concept checks */
|
||||
GTSAM_CONCEPT_POSE_INST(Pose2);
|
||||
|
||||
static const Matrix I3 = eye(3), Z12 = zeros(1,2);
|
||||
static const Matrix3 I3 = Eigen::Matrix3d::Identity(), Z12 = Eigen::Matrix3d::Zero();
|
||||
static const Rot2 R_PI_2(Rot2::fromCosSin(0., 1.));
|
||||
|
||||
/* ************************************************************************* */
|
||||
Matrix Pose2::matrix() const {
|
||||
Matrix R = r_.matrix();
|
||||
R = stack(2, &R, &Z12);
|
||||
Matrix T = (Matrix(3, 1) << t_.x(), t_.y(), 1.0).finished();
|
||||
return collect(2, &R, &T);
|
||||
Matrix3 Pose2::matrix() const {
|
||||
Matrix2 R = r_.matrix();
|
||||
Matrix32 R0;
|
||||
R0.block(0,0,2,2) = R;
|
||||
R0.block(2,0,1,2).setZero();
|
||||
Matrix31 T;
|
||||
T << t_.x(), t_.y(), 1.0;
|
||||
Matrix3 RT_;
|
||||
RT_.block(0,0,3,2) = R0;
|
||||
RT_.block(0,2,3,1) = T;
|
||||
return RT_;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -140,8 +146,8 @@ Point2 Pose2::transform_to(const Point2& point,
|
|||
|
||||
/* ************************************************************************* */
|
||||
// see doc/math.lyx, SE(2) section
|
||||
Pose2 Pose2::compose(const Pose2& p2, boost::optional<Matrix&> H1,
|
||||
boost::optional<Matrix&> H2) const {
|
||||
Pose2 Pose2::compose(const Pose2& p2, OptionalJacobian<3,3> H1,
|
||||
OptionalJacobian<3,3> H2) const {
|
||||
// TODO: inline and reuse?
|
||||
if(H1) *H1 = p2.inverse().AdjointMap();
|
||||
if(H2) *H2 = I3;
|
||||
|
|
@ -154,10 +160,14 @@ Point2 Pose2::transform_from(const Point2& p,
|
|||
OptionalJacobian<2, 3> H1, OptionalJacobian<2, 2> H2) const {
|
||||
const Point2 q = r_ * p;
|
||||
if (H1 || H2) {
|
||||
const Matrix R = r_.matrix();
|
||||
const Matrix Drotate1 = (Matrix(2, 1) << -q.y(), q.x()).finished();
|
||||
if (H1) *H1 = collect(2, &R, &Drotate1); // [R R_{pi/2}q]
|
||||
if (H2) *H2 = R; // R
|
||||
const Matrix2 R = r_.matrix();
|
||||
Matrix21 Drotate1;
|
||||
Drotate1 << -q.y(), q.x();
|
||||
if (H1) {
|
||||
(*H1).block(0,0,2,2) = R; // [R R_{pi/2}q]
|
||||
(*H1).block(0,2,2,1) = Drotate1;
|
||||
}
|
||||
if (H2) *H2 = R; // R
|
||||
}
|
||||
return q + t_;
|
||||
}
|
||||
|
|
@ -198,24 +208,27 @@ Pose2 Pose2::between(const Pose2& p2, OptionalJacobian<3,3> H1,
|
|||
|
||||
/* ************************************************************************* */
|
||||
Rot2 Pose2::bearing(const Point2& point,
|
||||
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
|
||||
Point2 d = transform_to(point, H1, H2);
|
||||
OptionalJacobian<1, 3> H1, OptionalJacobian<1, 2> H2) const {
|
||||
// make temporary matrices
|
||||
Matrix23 D1; Matrix2 D2;
|
||||
Point2 d = transform_to(point, H1 ? &D1 : 0, H2 ? &D2 : 0); // uses pointer version
|
||||
if (!H1 && !H2) return Rot2::relativeBearing(d);
|
||||
Matrix D_result_d;
|
||||
Matrix12 D_result_d;
|
||||
Rot2 result = Rot2::relativeBearing(d, D_result_d);
|
||||
if (H1) *H1 = D_result_d * (*H1);
|
||||
if (H2) *H2 = D_result_d * (*H2);
|
||||
if (H1) *H1 = D_result_d * (D1);
|
||||
if (H2) *H2 = D_result_d * (D2);
|
||||
return result;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Rot2 Pose2::bearing(const Pose2& pose,
|
||||
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
|
||||
Rot2 result = bearing(pose.t(), H1, H2);
|
||||
OptionalJacobian<1, 3> H1, OptionalJacobian<1, 3> H2) const {
|
||||
Matrix12 D2;
|
||||
Rot2 result = bearing(pose.t(), H1, H2 ? &D2 : 0);
|
||||
if (H2) {
|
||||
Matrix H2_ = *H2 * pose.r().matrix();
|
||||
*H2 = zeros(1, 3);
|
||||
insertSub(*H2, H2_, 0, 0);
|
||||
Matrix12 H2_ = D2 * pose.r().matrix();
|
||||
(*H2).setZero();
|
||||
(*H2).block(0,0,1,2) = H2_;
|
||||
}
|
||||
return result;
|
||||
}
|
||||
|
|
@ -227,29 +240,37 @@ double Pose2::range(const Point2& point,
|
|||
Matrix12 H;
|
||||
double r = d.norm(H);
|
||||
if (H1) {
|
||||
Matrix23 mvalue; // is this the correct name ?
|
||||
mvalue << -r_.c(), r_.s(), 0.0,
|
||||
Matrix23 H1_;
|
||||
H1_ << -r_.c(), r_.s(), 0.0,
|
||||
-r_.s(), -r_.c(), 0.0;
|
||||
*H1 = H * mvalue;
|
||||
*H1 = H * H1_;
|
||||
}
|
||||
if (H2) *H2 = H;
|
||||
return r;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
double Pose2::range(const Pose2& pose2,
|
||||
boost::optional<Matrix&> H1,
|
||||
boost::optional<Matrix&> H2) const {
|
||||
Point2 d = pose2.t() - t_;
|
||||
double Pose2::range(const Pose2& pose,
|
||||
OptionalJacobian<1,3> H1,
|
||||
OptionalJacobian<1,3> H2) const {
|
||||
Point2 d = pose.t() - t_;
|
||||
if (!H1 && !H2) return d.norm();
|
||||
Matrix H;
|
||||
Matrix12 H;
|
||||
double r = d.norm(H);
|
||||
if (H1) *H1 = H * (Matrix(2, 3) <<
|
||||
if (H1) {
|
||||
Matrix23 H1_;
|
||||
H1_ <<
|
||||
-r_.c(), r_.s(), 0.0,
|
||||
-r_.s(), -r_.c(), 0.0).finished();
|
||||
if (H2) *H2 = H * (Matrix(2, 3) <<
|
||||
pose2.r_.c(), -pose2.r_.s(), 0.0,
|
||||
pose2.r_.s(), pose2.r_.c(), 0.0).finished();
|
||||
-r_.s(), -r_.c(), 0.0;
|
||||
*H1 = H * H1_;
|
||||
}
|
||||
if (H2) {
|
||||
Matrix23 H2_;
|
||||
H2_ <<
|
||||
pose.r_.c(), -pose.r_.s(), 0.0,
|
||||
pose.r_.s(), pose.r_.c(), 0.0;
|
||||
*H2 = H * H2_;
|
||||
}
|
||||
return r;
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -75,7 +75,7 @@ public:
|
|||
Pose2(const Rot2& r, const Point2& t) : r_(r), t_(t) {}
|
||||
|
||||
/** Constructor from 3*3 matrix */
|
||||
Pose2(const Matrix &T) :
|
||||
Pose2(const Matrix &T) : // TODO : Change this to Optional Jacobian ??
|
||||
r_(Rot2::atan2(T(1, 0), T(0, 0))), t_(T(0, 2), T(1, 2)) {
|
||||
assert(T.rows() == 3 && T.cols() == 3);
|
||||
}
|
||||
|
|
@ -111,8 +111,8 @@ public:
|
|||
|
||||
/// compose this transformation onto another (first *this and then p2)
|
||||
Pose2 compose(const Pose2& p2,
|
||||
boost::optional<Matrix&> H1 = boost::none,
|
||||
boost::optional<Matrix&> H2 = boost::none) const;
|
||||
OptionalJacobian<3, 3> H1 = boost::none,
|
||||
OptionalJacobian<3, 3> H2 = boost::none) const;
|
||||
|
||||
/// compose syntactic sugar
|
||||
inline Pose2 operator*(const Pose2& p2) const {
|
||||
|
|
@ -168,11 +168,13 @@ public:
|
|||
* v (vx,vy) = 2D velocity
|
||||
* @return xihat, 3*3 element of Lie algebra that can be exponentiated
|
||||
*/
|
||||
static inline Matrix wedge(double vx, double vy, double w) {
|
||||
return (Matrix(3,3) <<
|
||||
static inline Matrix3 wedge(double vx, double vy, double w) {
|
||||
Matrix3 wedge_;
|
||||
wedge_ <<
|
||||
0.,-w, vx,
|
||||
w, 0., vy,
|
||||
0., 0., 0.).finished();
|
||||
0., 0., 0.;
|
||||
return wedge_;
|
||||
}
|
||||
|
||||
/// @}
|
||||
|
|
@ -218,7 +220,7 @@ public:
|
|||
inline const Rot2& rotation() const { return r_; }
|
||||
|
||||
//// return transformation matrix
|
||||
Matrix matrix() const;
|
||||
Matrix3 matrix() const;
|
||||
|
||||
/**
|
||||
* Calculate bearing to a landmark
|
||||
|
|
@ -226,15 +228,15 @@ public:
|
|||
* @return 2D rotation \f$ \in SO(2) \f$
|
||||
*/
|
||||
Rot2 bearing(const Point2& point,
|
||||
boost::optional<Matrix&> H1=boost::none, boost::optional<Matrix&> H2=boost::none) const;
|
||||
OptionalJacobian<1, 3> H1=boost::none, OptionalJacobian<1, 2> H2=boost::none) const;
|
||||
|
||||
/**
|
||||
* Calculate bearing to another pose
|
||||
* @param point SO(2) location of other pose
|
||||
* @return 2D rotation \f$ \in SO(2) \f$
|
||||
*/
|
||||
Rot2 bearing(const Pose2& point,
|
||||
boost::optional<Matrix&> H1=boost::none, boost::optional<Matrix&> H2=boost::none) const;
|
||||
Rot2 bearing(const Pose2& pose,
|
||||
OptionalJacobian<1, 3> H1=boost::none, OptionalJacobian<1, 3> H2=boost::none) const;
|
||||
|
||||
/**
|
||||
* Calculate range to a landmark
|
||||
|
|
@ -251,8 +253,8 @@ public:
|
|||
* @return range (double)
|
||||
*/
|
||||
double range(const Pose2& point,
|
||||
boost::optional<Matrix&> H1=boost::none,
|
||||
boost::optional<Matrix&> H2=boost::none) const;
|
||||
OptionalJacobian<1, 3> H1=boost::none,
|
||||
OptionalJacobian<1, 3> H2=boost::none) const;
|
||||
|
||||
/// @}
|
||||
/// @name Advanced Interface
|
||||
|
|
@ -287,7 +289,7 @@ private:
|
|||
|
||||
/** specialization for pose2 wedge function (generic template in Lie.h) */
|
||||
template <>
|
||||
inline Matrix wedge<Pose2>(const Vector& xi) {
|
||||
inline Matrix wedge<Pose2>(const Vector& xi) { // TODO : Convert to Optional Jacobian ?
|
||||
return Pose2::wedge(xi(0),xi(1),xi(2));
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -44,7 +44,7 @@ TEST(Pose2, constructors) {
|
|||
Pose2 origin;
|
||||
assert_equal(pose,origin);
|
||||
Pose2 t(M_PI/2.0+0.018, Point2(1.015, 2.01));
|
||||
EXPECT(assert_equal(t,Pose2(t.matrix())));
|
||||
EXPECT(assert_equal(t,Pose2((Matrix)t.matrix())));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
Loading…
Reference in New Issue