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

Merge pull request #1504 from DanMcGann/pose2_component_jacobians

release/4.3a0
Varun Agrawal 2023-06-09 02:31:57 -04:00 committed by GitHub
parent 13cb9d8f8c a4e4e1f83e
commit 107f5410cc
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
3 changed files with 40 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
gtsam/geometry/Pose2.h
View File

@ -258,10 +258,19 @@ public:
inline const Rot2& r() const { return r_; }
/// translation
inline const Point2& translation() const { return t_; }
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
inline const Rot2& rotation() const { return r_; }
inline const Rot2& rotation(OptionalJacobian<1, 3> Hself={}) const {
if (Hself) *Hself << 0, 0, 1;
return r_;
}
//// return transformation matrix
GTSAM_EXPORT Matrix3 matrix() const;

2
gtsam/geometry/geometry.i
View File

@ -431,7 +431,9 @@ class Pose2 {
gtsam::Rot2 bearing(const gtsam::Point2& point) const;
double range(const gtsam::Point2& point) const;
gtsam::Point2 translation() const;
gtsam::Point2 translation(Eigen::Ref<Eigen::MatrixXd> Hself) const;
gtsam::Rot2 rotation() const;
gtsam::Rot2 rotation(Eigen::Ref<Eigen::MatrixXd> Hself) const;
Matrix matrix() const;
// enabling serialization functionality

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 )
{