Merge pull request #1234 from shteren1/pose3_jacobians

gtsam/geometry/Pose3.cpp

@@ -489,6 +489,11 @@ boost::optional<Pose3> align(const Point3Pairs &baPointPairs) {
 }
 #endif
 
+/* ************************************************************************* */
+Pose3 Pose3::slerp(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.

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 slerp(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);

gtsam/geometry/geometry.i

@@ -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 slerp(double t, const gtsam::Pose3& pose) const;
+  gtsam::Pose3 slerp(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);
@@ -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<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;
@@ -484,15 +507,25 @@ class Pose3 {
 
   // 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;

python/gtsam/tests/test_Pose3.py

@@ -19,6 +19,62 @@ from gtsam import Point3, Pose3, Rot3, Point3Pairs
 from gtsam.utils.test_case import GtsamTestCase
 
 
+def numerical_derivative_pose(pose, method, delta=1e-5):
+    jacobian = np.zeros((6, 6))
+    for idx in range(6):
+        xplus = np.zeros(6)
+        xplus[idx] = delta
+        xminus = np.zeros(6)
+        xminus[idx] = -delta
+        pose_plus = pose.retract(xplus).__getattribute__(method)()
+        pose_minus = pose.retract(xminus).__getattribute__(method)()
+        jacobian[:, idx] = pose_minus.localCoordinates(pose_plus) / (2 * delta)
+    return jacobian
+
+
+def numerical_derivative_2_poses(pose, other_pose, method, delta=1e-5, inputs=()):
+    jacobian = np.zeros((6, 6))
+    other_jacobian = np.zeros((6, 6))
+    for idx in range(6):
+        xplus = np.zeros(6)
+        xplus[idx] = delta
+        xminus = np.zeros(6)
+        xminus[idx] = -delta
+
+        pose_plus = pose.retract(xplus).__getattribute__(method)(*inputs, other_pose)
+        pose_minus = pose.retract(xminus).__getattribute__(method)(*inputs, other_pose)
+        jacobian[:, idx] = pose_minus.localCoordinates(pose_plus) / (2 * delta)
+
+        other_pose_plus = pose.__getattribute__(method)(*inputs, other_pose.retract(xplus))
+        other_pose_minus = pose.__getattribute__(method)(*inputs, other_pose.retract(xminus))
+        other_jacobian[:, idx] = other_pose_minus.localCoordinates(other_pose_plus) / (2 * delta)
+    return jacobian, other_jacobian
+
+
+def numerical_derivative_pose_point(pose, point, method, delta=1e-5):
+    jacobian = np.zeros((3, 6))
+    point_jacobian = np.zeros((3, 3))
+    for idx in range(6):
+        xplus = np.zeros(6)
+        xplus[idx] = delta
+        xminus = np.zeros(6)
+        xminus[idx] = -delta
+
+        point_plus = pose.retract(xplus).__getattribute__(method)(point)
+        point_minus = pose.retract(xminus).__getattribute__(method)(point)
+        jacobian[:, idx] = (point_plus - point_minus) / (2 * delta)
+
+        if idx < 3:
+            xplus = np.zeros(3)
+            xplus[idx] = delta
+            xminus = np.zeros(3)
+            xminus[idx] = -delta
+            point_plus = pose.__getattribute__(method)(point + xplus)
+            point_minus = pose.__getattribute__(method)(point + xminus)
+            point_jacobian[:, idx] = (point_plus - point_minus) / (2 * delta)
+    return jacobian, point_jacobian
+
+
 class TestPose3(GtsamTestCase):
     """Test selected Pose3 methods."""
 
@@ -30,6 +86,47 @@ class TestPose3(GtsamTestCase):
         actual = T2.between(T3)
         self.gtsamAssertEquals(actual, expected, 1e-6)
 
+        #test jacobians
+        jacobian = np.zeros((6, 6), order='F')
+        jacobian_other = np.zeros((6, 6), order='F')
+        T2.between(T3, jacobian, jacobian_other)
+        jacobian_numerical, jacobian_numerical_other = numerical_derivative_2_poses(T2, T3, 'between')
+        self.gtsamAssertEquals(jacobian, jacobian_numerical)
+        self.gtsamAssertEquals(jacobian_other, jacobian_numerical_other)
+
+    def test_inverse(self):
+        """Test between method."""
+        pose = Pose3(Rot3.Rodrigues(0, 0, -math.pi/2), Point3(2, 4, 0))
+        expected = Pose3(Rot3.Rodrigues(0, 0, math.pi/2), Point3(4, -2, 0))
+        actual = pose.inverse()
+        self.gtsamAssertEquals(actual, expected, 1e-6)
+
+        #test jacobians
+        jacobian = np.zeros((6, 6), order='F')
+        pose.inverse(jacobian)
+        jacobian_numerical = numerical_derivative_pose(pose, 'inverse')
+        self.gtsamAssertEquals(jacobian, jacobian_numerical)
+
+    def test_slerp(self):
+        """Test slerp method."""
+        pose0 = gtsam.Pose3()
+        pose1 = Pose3(Rot3.Rodrigues(0, 0, -math.pi/2), Point3(2, 4, 0))
+        actual_alpha_0 = pose0.slerp(0, pose1)
+        self.gtsamAssertEquals(actual_alpha_0, pose0)
+        actual_alpha_1 = pose0.slerp(1, pose1)
+        self.gtsamAssertEquals(actual_alpha_1, pose1)
+        actual_alpha_half = pose0.slerp(0.5, pose1)
+        expected_alpha_half = Pose3(Rot3.Rodrigues(0, 0, -math.pi/4), Point3(0.17157288, 2.41421356, 0))
+        self.gtsamAssertEquals(actual_alpha_half, expected_alpha_half, tol=1e-6)
+
+        # test jacobians
+        jacobian = np.zeros((6, 6), order='F')
+        jacobian_other = np.zeros((6, 6), order='F')
+        pose0.slerp(0.5, pose1, jacobian, jacobian_other)
+        jacobian_numerical, jacobian_numerical_other = numerical_derivative_2_poses(pose0, pose1, 'slerp', inputs=[0.5])
+        self.gtsamAssertEquals(jacobian, jacobian_numerical)
+        self.gtsamAssertEquals(jacobian_other, jacobian_numerical_other)
+
     def test_transformTo(self):
         """Test transformTo method."""
         pose = Pose3(Rot3.Rodrigues(0, 0, -math.pi/2), Point3(2, 4, 0))
@@ -37,6 +134,15 @@ class TestPose3(GtsamTestCase):
         expected = Point3(2, 1, 10)
         self.gtsamAssertEquals(actual, expected, 1e-6)
 
+        #test jacobians
+        point = Point3(3, 2, 10)
+        jacobian_pose = np.zeros((3, 6), order='F')
+        jacobian_point = np.zeros((3, 3), order='F')
+        pose.transformTo(point, jacobian_pose, jacobian_point)
+        jacobian_numerical_pose, jacobian_numerical_point = numerical_derivative_pose_point(pose, point, 'transformTo')
+        self.gtsamAssertEquals(jacobian_pose, jacobian_numerical_pose)
+        self.gtsamAssertEquals(jacobian_point, jacobian_numerical_point)
+
         # multi-point version
         points = np.stack([Point3(3, 2, 10), Point3(3, 2, 10)]).T
         actual_array = pose.transformTo(points)
@@ -51,6 +157,15 @@ class TestPose3(GtsamTestCase):
         expected = Point3(3, 2, 10)
         self.gtsamAssertEquals(actual, expected, 1e-6)
 
+        #test jacobians
+        point = Point3(3, 2, 10)
+        jacobian_pose = np.zeros((3, 6), order='F')
+        jacobian_point = np.zeros((3, 3), order='F')
+        pose.transformFrom(point, jacobian_pose, jacobian_point)
+        jacobian_numerical_pose, jacobian_numerical_point = numerical_derivative_pose_point(pose, point, 'transformFrom')
+        self.gtsamAssertEquals(jacobian_pose, jacobian_numerical_pose)
+        self.gtsamAssertEquals(jacobian_point, jacobian_numerical_point)
+
         # multi-point version
         points = np.stack([Point3(2, 1, 10), Point3(2, 1, 10)]).T
         actual_array = pose.transformFrom(points)
@@ -122,4 +237,4 @@ class TestPose3(GtsamTestCase):
 
 
 if __name__ == "__main__":
-    unittest.main()
+    unittest.main()