diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index 2da51a625..f261a2302 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -489,6 +489,11 @@ boost::optional 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. diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h index c36047349..e2128b28d 100644 --- a/gtsam/geometry/Pose3.h +++ b/gtsam/geometry/Pose3.h @@ -379,6 +379,14 @@ public: 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, + OptionalJacobian<6, 6> Hy = boost::none) const; + /// Output stream operator GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &os, const Pose3& p); diff --git a/gtsam/geometry/geometry.i b/gtsam/geometry/geometry.i index 517558265..4deb5c48a 100644 --- a/gtsam/geometry/geometry.i +++ b/gtsam/geometry/geometry.i @@ -446,24 +446,43 @@ class Pose3 { // Group static gtsam::Pose3 identity(); gtsam::Pose3 inverse() const; + gtsam::Pose3 inverse(Eigen::Ref H) const; gtsam::Pose3 compose(const gtsam::Pose3& pose) const; + gtsam::Pose3 compose(const gtsam::Pose3& pose, + Eigen::Ref H1, + Eigen::Ref H2) const; gtsam::Pose3 between(const gtsam::Pose3& pose) const; + gtsam::Pose3 between(const gtsam::Pose3& pose, + Eigen::Ref H1, + Eigen::Ref H2) const; + gtsam::Pose3 interp(double t, const gtsam::Pose3& pose) const; + gtsam::Pose3 interp(double t, const gtsam::Pose3& pose, + Eigen::Ref Hx, + Eigen::Ref 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 Hxi) const; Vector localCoordinates(const gtsam::Pose3& pose) const; + Vector localCoordinates(const gtsam::Pose3& pose, Eigen::Ref Hxi) const; // Lie Group static gtsam::Pose3 Expmap(Vector v); + static gtsam::Pose3 Expmap(Vector v, Eigen::Ref Hxi); static Vector Logmap(const gtsam::Pose3& pose); + static Vector Logmap(const gtsam::Pose3& pose, Eigen::Ref 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 H_this, + Eigen::Ref H_xib) const; Vector AdjointTranspose(Vector xi) const; + Vector AdjointTranspose(Vector xi, Eigen::Ref H_this, + Eigen::Ref H_x) const; static Matrix adjointMap(Vector xi); static Vector adjoint(Vector xi, Vector y); static Matrix adjointMap_(Vector xi); @@ -476,7 +495,11 @@ class Pose3 { // Group Action on Point3 gtsam::Point3 transformFrom(const gtsam::Point3& point) const; + gtsam::Point3 transformFrom(const gtsam::Point3& point, Eigen::Ref Hself, + Eigen::Ref Hpoint) const; gtsam::Point3 transformTo(const gtsam::Point3& point) const; + gtsam::Point3 transformTo(const gtsam::Point3& point, Eigen::Ref Hself, + Eigen::Ref Hpoint) const; // Matrix versions Matrix transformFrom(const Matrix& points) const; @@ -484,15 +507,25 @@ class Pose3 { // Standard Interface gtsam::Rot3 rotation() const; + gtsam::Rot3 rotation(Eigen::Ref Hself) const; gtsam::Point3 translation() const; + gtsam::Point3 translation(Eigen::Ref 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 Hself, + Eigen::Ref HaTb) const; gtsam::Pose3 transformPoseTo(const gtsam::Pose3& pose) const; + gtsam::Pose3 transformPoseTo(const gtsam::Pose3& pose, Eigen::Ref Hself, + Eigen::Ref HwTb) const; double range(const gtsam::Point3& point); + double range(const gtsam::Point3& point, Eigen::Ref Hself, + Eigen::Ref Hpoint); double range(const gtsam::Pose3& pose); + double range(const gtsam::Pose3& pose, Eigen::Ref Hself, + Eigen::Ref Hpose); // enabling serialization functionality void serialize() const;