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

change Pose2::adjointMap and adjoint to Pose2::AdjointMap and Adjoint to make it consistent with Pose3 and standard Lie group notation

release/4.3a0
Duy-Nguyen Ta 2013-04-30 17:20:23 +00:00
parent 723ff2c9ad
commit 7256c88bbd
4 changed files with 9 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
gtsam.h
View File

@ -448,8 +448,8 @@ virtual class Pose2 : gtsam::Value {
// Lie Group // Lie Group
static gtsam::Pose2 Expmap(Vector v); static gtsam::Pose2 Expmap(Vector v);
static Vector Logmap(const gtsam::Pose2& p); static Vector Logmap(const gtsam::Pose2& p);
Matrix adjointMap() const; Matrix AdjointMap() const;
Vector adjoint(const Vector& xi) const; Vector Adjoint(const Vector& xi) const;
static Matrix wedge(double vx, double vy, double w); static Matrix wedge(double vx, double vy, double w);
// Group Actions on Point2 // Group Actions on Point2

6
gtsam/geometry/Pose2.cpp
View File

@ -108,7 +108,7 @@ Vector Pose2::localCoordinates(const Pose2& p2) const {
/* ************************************************************************* */ /* ************************************************************************* */
// Calculate Adjoint map // Calculate Adjoint map
// Ad_pose is 3*3 matrix that when applied to twist xi, returns Ad_pose(xi) // Ad_pose is 3*3 matrix that when applied to twist xi, returns Ad_pose(xi)
Matrix Pose2::adjointMap() const { Matrix Pose2::AdjointMap() const {
double c = r_.c(), s = r_.s(), x = t_.x(), y = t_.y(); double c = r_.c(), s = r_.s(), x = t_.x(), y = t_.y();
return Matrix_(3,3, return Matrix_(3,3,
c, -s, y, c, -s, y,
@ -119,7 +119,7 @@ Matrix Pose2::adjointMap() const {
/* ************************************************************************* */ /* ************************************************************************* */
Pose2 Pose2::inverse(boost::optional<Matrix&> H1) const { Pose2 Pose2::inverse(boost::optional<Matrix&> H1) const {
if (H1) *H1 = -adjointMap(); if (H1) *H1 = -AdjointMap();
return Pose2(r_.inverse(), r_.unrotate(Point2(-t_.x(), -t_.y()))); return Pose2(r_.inverse(), r_.unrotate(Point2(-t_.x(), -t_.y())));
} }
@ -142,7 +142,7 @@ Point2 Pose2::transform_to(const Point2& point,
Pose2 Pose2::compose(const Pose2& p2, boost::optional<Matrix&> H1, Pose2 Pose2::compose(const Pose2& p2, boost::optional<Matrix&> H1,
boost::optional<Matrix&> H2) const { boost::optional<Matrix&> H2) const {
// TODO: inline and reuse? // TODO: inline and reuse?
if(H1) *H1 = p2.inverse().adjointMap(); if(H1) *H1 = p2.inverse().AdjointMap();
if(H2) *H2 = I3; if(H2) *H2 = I3;
return (*this)*p2; return (*this)*p2;
} }

6
gtsam/geometry/Pose2.h
View File

@ -158,10 +158,10 @@ public:
* Calculate Adjoint map * Calculate Adjoint map
* Ad_pose is 3*3 matrix that when applied to twist xi \f$ [T_x,T_y,\theta] \f$, returns Ad_pose(xi) * Ad_pose is 3*3 matrix that when applied to twist xi \f$ [T_x,T_y,\theta] \f$, returns Ad_pose(xi)
*/ */
Matrix adjointMap() const; Matrix AdjointMap() const;
inline Vector adjoint(const Vector& xi) const { inline Vector Adjoint(const Vector& xi) const {
assert(xi.size() == 3); assert(xi.size() == 3);
return adjointMap()*xi; return AdjointMap()*xi;
} }
/** /**

2
gtsam/geometry/tests/testPose2.cpp
View File

@ -429,7 +429,7 @@ TEST( Pose2, between )
EXPECT(assert_equal(expectedH1,actualH1)); EXPECT(assert_equal(expectedH1,actualH1));
EXPECT(assert_equal(numericalH1,actualH1)); EXPECT(assert_equal(numericalH1,actualH1));
// Assert H1 = -AdjointMap(between(p2,p1)) as in doc/math.lyx // Assert H1 = -AdjointMap(between(p2,p1)) as in doc/math.lyx
EXPECT(assert_equal(-gT2.between(gT1).adjointMap(),actualH1)); EXPECT(assert_equal(-gT2.between(gT1).AdjointMap(),actualH1));
Matrix expectedH2 = Matrix_(3,3, Matrix expectedH2 = Matrix_(3,3,
1.0, 0.0, 0.0, 1.0, 0.0, 0.0,