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

Adds jacobians for Pose2 component extraction 

Adds jacobians to translation() and rotation() for Pose2 to bring it
into spec with Pose3's equilivent functions. Also adds tests.
release/4.3a0
Dan McGann 2023-03-27 17:28:45 -04:00
parent 005c7d4e2d
commit 6baa7e102b
3 changed files with 46 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
gtsam/geometry/Pose2.cpp
View File

@ -32,6 +32,21 @@ GTSAM_CONCEPT_POSE_INST(Pose2)
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 {
Matrix2 R = r_.matrix();

8
gtsam/geometry/Pose2.h
View File

@ -252,16 +252,16 @@ public:
inline double theta() const { return r_.theta(); }
/// translation
inline const Point2& t() const { return t_; }
inline const Point2& t() const { return translation(); }
/// rotation
inline const Rot2& r() const { return r_; }
inline const Rot2& r() const { return rotation(); }
/// translation
inline const Point2& translation() const { return t_; }
const Point2& translation(OptionalJacobian<2, 3> Hself={}) const;
/// rotation
inline const Rot2& rotation() const { return r_; }
const Rot2& rotation(OptionalJacobian<1, 3> Hself={}) const;
//// return transformation matrix
GTSAM_EXPORT Matrix3 matrix() const;

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

@ -474,6 +474,33 @@ TEST( Pose2, compose_matrix )
EXPECT(assert_equal(gM1*_1M2,matrix(gT1.compose(_1T2)))); // RIGHT DOES NOT
}
/* ************************************************************************* */
TEST( Pose2, translation ) {
Pose2 pose(3.5, -8.2, 4.2);
Matrix actualH;
EXPECT(assert_equal((Vector2() << 3.5, -8.2).finished(), pose.translation(actualH), 1e-8));
std::function<Point2(const Pose2&)> f = [](const Pose2& T) { return T.translation(); };
Matrix numericalH = numericalDerivative11<Point2, Pose2>(f, pose);
EXPECT(assert_equal(numericalH, actualH, 1e-6));
}
/* ************************************************************************* */
TEST( Pose2, rotation ) {
Pose2 pose(3.5, -8.2, 4.2);
Matrix actualH(4, 3);
EXPECT(assert_equal(Rot2(4.2), pose.rotation(actualH), 1e-8));
std::function<Rot2(const Pose2&)> f = [](const Pose2& T) { return T.rotation(); };
Matrix numericalH = numericalDerivative11<Rot2, Pose2>(f, pose);
EXPECT(assert_equal(numericalH, actualH, 1e-6));
}
/* ************************************************************************* */
TEST( Pose2, between )
{