1. changed interp name to slerp

2. added python tests for jacobians for some pose3 apis

gtsam/geometry/Pose3.cpp

@@ -490,7 +490,7 @@ 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 {
+Pose3 Pose3::slerp(double t, const Pose3& other, OptionalJacobian<6, 6> Hx, OptionalJacobian<6, 6> Hy) const {
   return interpolate(*this, other, t, Hx, Hy);
 }
 

gtsam/geometry/Pose3.h

@@ -384,7 +384,7 @@ public:
    * @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,
+  Pose3 slerp(double t, const Pose3& other, OptionalJacobian<6, 6> Hx = boost::none,
                                              OptionalJacobian<6, 6> Hy = boost::none) const;
 
   /// Output stream operator

gtsam/geometry/geometry.i

@@ -455,8 +455,8 @@ class Pose3 {
   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 slerp(double t, const gtsam::Pose3& pose) const;
-  gtsam::Pose3 interp(double t, const gtsam::Pose3& pose,
+  gtsam::Pose3 slerp(double t, const gtsam::Pose3& pose,
                            Eigen::Ref<Eigen::MatrixXd> Hx,
                            Eigen::Ref<Eigen::MatrixXd> Hy) 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,5 @@ class TestPose3(GtsamTestCase):
 
 
 if __name__ == "__main__":
-    unittest.main()
+    # unittest.main()
+    TestPose3().test_slerp()