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

Convert Pose2 r/t accessors to inline

release/4.3a0
Dan McGann 2023-04-09 11:38:40 -04:00 committed by Fan Jiang
parent 995e907e62
commit 283932ae68
2 changed files with 11 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
gtsam/geometry/Pose2.cpp
View File

@ -32,21 +32,6 @@ GTSAM_CONCEPT_POSE_INST(Pose2)
static const Rot2 R_PI_2(Rot2::fromCosSin(0., 1.)); static const Rot2 R_PI_2(Rot2::fromCosSin(0., 1.));
/* ************************************************************************* */
const Point2 &Pose2::translation(OptionalJacobian<2, 3> Hself) const {
if (Hself) {
*Hself = Matrix::Zero(2, 3);
(*Hself).block<2, 2>(0, 0) = rotation().matrix();
}
return t_;
}
/* ************************************************************************* */
const Rot2& Pose2::rotation(OptionalJacobian<1, 3> Hself) const {
if (Hself) *Hself << 0, 0, 1;
return r_;
}
/* ************************************************************************* */ /* ************************************************************************* */
Matrix3 Pose2::matrix() const { Matrix3 Pose2::matrix() const {
Matrix2 R = r_.matrix(); Matrix2 R = r_.matrix();

13
gtsam/geometry/Pose2.h
View File

@ -255,10 +255,19 @@ public:
inline const Rot2& r() const { return rotation(); } inline const Rot2& r() const { return rotation(); }
/// translation /// translation
const Point2& translation(OptionalJacobian<2, 3> Hself={}) const; inline const Point2& translation(OptionalJacobian<2, 3> Hself={}) const {
if (Hself) {
*Hself = Matrix::Zero(2, 3);
(*Hself).block<2, 2>(0, 0) = rotation().matrix();
}
return t_;
}
/// rotation /// rotation
const Rot2& rotation(OptionalJacobian<1, 3> Hself={}) const; inline const Rot2& rotation(OptionalJacobian<1, 3> Hself={}) const {
if (Hself) *Hself << 0, 0, 1;
return r_;
}
//// return transformation matrix //// return transformation matrix
GTSAM_EXPORT Matrix3 matrix() const; GTSAM_EXPORT Matrix3 matrix() const;