diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index f261a2302..2938e90f5 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -490,7 +490,7 @@ boost::optional 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); } diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h index e2128b28d..33fb55250 100644 --- a/gtsam/geometry/Pose3.h +++ b/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 diff --git a/gtsam/geometry/geometry.i b/gtsam/geometry/geometry.i index 4deb5c48a..92d9ec0a3 100644 --- a/gtsam/geometry/geometry.i +++ b/gtsam/geometry/geometry.i @@ -455,8 +455,8 @@ class Pose3 { 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, + gtsam::Pose3 slerp(double t, const gtsam::Pose3& pose) const; + gtsam::Pose3 slerp(double t, const gtsam::Pose3& pose, Eigen::Ref Hx, Eigen::Ref Hy) const; diff --git a/python/gtsam/tests/test_Pose3.py b/python/gtsam/tests/test_Pose3.py index cde71de53..cdc214a2c 100644 --- a/python/gtsam/tests/test_Pose3.py +++ b/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() \ No newline at end of file