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

added jacobians for all pose3 methods in python wrappers #1234

Merged
merged 3 commits into from
Jul 7, 2022
Merged

added jacobians for all pose3 methods in python wrappers #1234

Show file tree
Hide file tree
Changes from 1 commit
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
3595e85
added jacobians for all pose3 methods in python wrappers
Jul 4, 2022
7348586
1. changed interp name to slerp
Jul 7, 2022
7c077f8
bugfix
Jul 7, 2022
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
5 changes: 5 additions & 0 deletions gtsam/geometry/Pose3.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -489,6 +489,11 @@ boost::optional<Pose3> align(const Point3Pairs &baPointPairs) {
}
#endif

/* ************************************************************************* */
Pose3 Pose3::interp(double t, const Pose3& other, OptionalJacobian<6, 6> Hx, OptionalJacobian<6, 6> Hy) const {
return interpolate(*this, other, t, Hx, Hy);
}

/* ************************************************************************* */
std::ostream &operator<<(std::ostream &os, const Pose3& pose) {
// Both Rot3 and Point3 have ostream definitions so we use them.
Expand Down
8 changes: 8 additions & 0 deletions gtsam/geometry/Pose3.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -379,6 +379,14 @@ class GTSAM_EXPORT Pose3: public LieGroup<Pose3, 6> {
return std::make_pair(0, 2);
}

/**
* @brief Spherical Linear interpolation between *this and other
* @param s a value between 0 and 1.5
* @param other final point of interpolation geodesic on manifold
*/
Pose3 interp(double t, const Pose3& other, OptionalJacobian<6, 6> Hx = boost::none,
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

On second thought, I actually prefer this since Lie::interpolate is a free function. Can you please rename this to slerp similar to Rot3?

Copy link
Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Hmm, isn't "slerp" referring to interpolation of rotations on the quaternion unit sphere specifically?

OptionalJacobian<6, 6> Hy = boost::none) const;

/// Output stream operator
GTSAM_EXPORT
friend std::ostream &operator<<(std::ostream &os, const Pose3& p);
Expand Down
33 changes: 33 additions & 0 deletions gtsam/geometry/geometry.i
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -446,24 +446,43 @@ class Pose3 {
// Group
static gtsam::Pose3 identity();
gtsam::Pose3 inverse() const;
gtsam::Pose3 inverse(Eigen::Ref<Eigen::MatrixXd> H) const;
gtsam::Pose3 compose(const gtsam::Pose3& pose) const;
gtsam::Pose3 compose(const gtsam::Pose3& pose,
Eigen::Ref<Eigen::MatrixXd> H1,
Eigen::Ref<Eigen::MatrixXd> H2) const;
gtsam::Pose3 between(const gtsam::Pose3& pose) const;
gtsam::Pose3 between(const gtsam::Pose3& pose,
Eigen::Ref<Eigen::MatrixXd> H1,
Eigen::Ref<Eigen::MatrixXd> H2) const;
gtsam::Pose3 interp(double t, const gtsam::Pose3& pose) const;
gtsam::Pose3 interp(double t, const gtsam::Pose3& pose,
Eigen::Ref<Eigen::MatrixXd> Hx,
Eigen::Ref<Eigen::MatrixXd> Hy) const;

// Operator Overloads
gtsam::Pose3 operator*(const gtsam::Pose3& pose) const;

// Manifold
gtsam::Pose3 retract(Vector v) const;
gtsam::Pose3 retract(Vector v, Eigen::Ref<Eigen::MatrixXd> Hxi) const;
Vector localCoordinates(const gtsam::Pose3& pose) const;
Vector localCoordinates(const gtsam::Pose3& pose, Eigen::Ref<Eigen::MatrixXd> Hxi) const;

// Lie Group
static gtsam::Pose3 Expmap(Vector v);
static gtsam::Pose3 Expmap(Vector v, Eigen::Ref<Eigen::MatrixXd> Hxi);
static Vector Logmap(const gtsam::Pose3& pose);
static Vector Logmap(const gtsam::Pose3& pose, Eigen::Ref<Eigen::MatrixXd> Hpose);
gtsam::Pose3 expmap(Vector v);
Vector logmap(const gtsam::Pose3& pose);
Matrix AdjointMap() const;
Vector Adjoint(Vector xi) const;
Vector Adjoint(Vector xi, Eigen::Ref<Eigen::MatrixXd> H_this,
Eigen::Ref<Eigen::MatrixXd> H_xib) const;
Vector AdjointTranspose(Vector xi) const;
Vector AdjointTranspose(Vector xi, Eigen::Ref<Eigen::MatrixXd> H_this,
Eigen::Ref<Eigen::MatrixXd> H_x) const;
static Matrix adjointMap(Vector xi);
static Vector adjoint(Vector xi, Vector y);
static Matrix adjointMap_(Vector xi);
Expand All @@ -476,23 +495,37 @@ class Pose3 {

// Group Action on Point3
gtsam::Point3 transformFrom(const gtsam::Point3& point) const;
gtsam::Point3 transformFrom(const gtsam::Point3& point, Eigen::Ref<Eigen::MatrixXd> Hself,
Eigen::Ref<Eigen::MatrixXd> Hpoint) const;
gtsam::Point3 transformTo(const gtsam::Point3& point) const;
gtsam::Point3 transformTo(const gtsam::Point3& point, Eigen::Ref<Eigen::MatrixXd> Hself,
Eigen::Ref<Eigen::MatrixXd> Hpoint) const;

// Matrix versions
Matrix transformFrom(const Matrix& points) const;
Matrix transformTo(const Matrix& points) const;

// Standard Interface
gtsam::Rot3 rotation() const;
gtsam::Rot3 rotation(Eigen::Ref<Eigen::MatrixXd> Hself) const;
gtsam::Point3 translation() const;
gtsam::Point3 translation(Eigen::Ref<Eigen::MatrixXd> Hself) const;
double x() const;
double y() const;
double z() const;
Matrix matrix() const;
gtsam::Pose3 transformPoseFrom(const gtsam::Pose3& pose) const;
gtsam::Pose3 transformPoseFrom(const gtsam::Pose3& pose, Eigen::Ref<Eigen::MatrixXd> Hself,
Eigen::Ref<Eigen::MatrixXd> HaTb) const;
gtsam::Pose3 transformPoseTo(const gtsam::Pose3& pose) const;
gtsam::Pose3 transformPoseTo(const gtsam::Pose3& pose, Eigen::Ref<Eigen::MatrixXd> Hself,
Eigen::Ref<Eigen::MatrixXd> HwTb) const;
double range(const gtsam::Point3& point);
double range(const gtsam::Point3& point, Eigen::Ref<Eigen::MatrixXd> Hself,
Eigen::Ref<Eigen::MatrixXd> Hpoint);
double range(const gtsam::Pose3& pose);
double range(const gtsam::Pose3& pose, Eigen::Ref<Eigen::MatrixXd> Hself,
Eigen::Ref<Eigen::MatrixXd> Hpose);

// enabling serialization functionality
void serialize() const;
Expand Down