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
static gtsam::Pose2 Expmap(Vector v);
static Vector Logmap(const gtsam::Pose2& p);
Matrix adjointMap() const;
Vector adjoint(const Vector& xi) const;
Matrix AdjointMap() const;
Vector Adjoint(const Vector& xi) const;
static Matrix wedge(double vx, double vy, double w);
// 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
// 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();
return Matrix_(3,3,
c, -s, y,
@ -119,7 +119,7 @@ Matrix Pose2::adjointMap() 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())));
}
@ -142,7 +142,7 @@ Point2 Pose2::transform_to(const Point2& point,
Pose2 Pose2::compose(const Pose2& p2, boost::optional<Matrix&> H1,
boost::optional<Matrix&> H2) const {
// TODO: inline and reuse?
if(H1) *H1 = p2.inverse().adjointMap();
if(H1) *H1 = p2.inverse().AdjointMap();
if(H2) *H2 = I3;
return (*this)*p2;
}

6
gtsam/geometry/Pose2.h
View File

@ -158,10 +158,10 @@ public:
* 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)
*/
Matrix adjointMap() const;
inline Vector adjoint(const Vector& xi) const {
Matrix AdjointMap() const;
inline Vector Adjoint(const Vector& xi) const {
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(numericalH1,actualH1));
// 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,
1.0, 0.0, 0.0,