Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Add Pose2 Component Jacobians #1504

Merged
merged 5 commits into from
Jun 9, 2023
Merged

Add Pose2 Component Jacobians #1504

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
5 commits
Select commit Hold shift + click to select a range
6baa7e1
Adds jacobians for Pose2 component extraction
DanMcGann Mar 27, 2023
d6a2484
Extends python iface Pose2 component jacobians
DanMcGann Mar 27, 2023
7e30cc7
Convert Pose2 r/t accessors to inline
DanMcGann Apr 9, 2023
2175f80
Revert changes to Pose2 r(), t()
DanMcGann Apr 9, 2023
a4e4e1f
Merge branch 'develop' into pose2_component_jacobians
varunagrawal Jun 9, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
13 changes: 11 additions & 2 deletions gtsam/geometry/Pose2.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -258,10 +258,19 @@ class Pose2: public LieGroup<Pose2, 3> {
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;
Expand Down
2 changes: 2 additions & 0 deletions gtsam/geometry/geometry.i
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
27 changes: 27 additions & 0 deletions gtsam/geometry/tests/testPose2.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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 )
{
Expand Down