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

move Adjoint to cpp and enable EXP_MAP

release/4.3a0
thduynguyen 2014-08-05 21:56:36 -04:00
parent c6d541741b
commit ecb896ef03
2 changed files with 10 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
gtsam/geometry/Pose2.cpp
View File

@ -33,6 +33,8 @@ INSTANTIATE_LIE(Pose2);
/** instantiate concept checks */ /** instantiate concept checks */
GTSAM_CONCEPT_POSE_INST(Pose2); GTSAM_CONCEPT_POSE_INST(Pose2);
#define SLOW_BUT_CORRECT_EXPMAP
static const Matrix I3 = eye(3), Z12 = zeros(1,2); static const Matrix I3 = eye(3), Z12 = zeros(1,2);
static const Rot2 R_PI_2(Rot2::fromCosSin(0., 1.)); static const Rot2 R_PI_2(Rot2::fromCosSin(0., 1.));
@ -117,6 +119,13 @@ Matrix Pose2::AdjointMap() const {
); );
} }
/* ************************************************************************* */
Vector Pose2::Adjoint(const Vector& xi) const {
assert(xi.size() == 3);
return AdjointMap()*xi;
}
/* ************************************************************************* */ /* ************************************************************************* */
Pose2 Pose2::inverse(boost::optional<Matrix&> H1) const { Pose2 Pose2::inverse(boost::optional<Matrix&> H1) const {
if (H1) *H1 = -AdjointMap(); if (H1) *H1 = -AdjointMap();

5
gtsam/geometry/Pose2.h
View File

@ -159,10 +159,7 @@ public:
* 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 { Vector Adjoint(const Vector& xi) const;
assert(xi.size() == 3);
return AdjointMap()*xi;
}
/** /**
* wedge for SE(2): * wedge for SE(2):