12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

1. changed interp name to slerp 

2. added python tests for jacobians for some pose3 apis
release/4.3a0
yotams 2022-07-07 14:09:10 +03:00
parent 3595e8588c
commit 7348586f60
4 changed files with 121 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
gtsam/geometry/Pose3.cpp
View File

@ -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);
}

2
gtsam/geometry/Pose3.h
View File

@ -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

4
gtsam/geometry/geometry.i
View File

@ -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 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<Eigen::MatrixXd> Hx,
Eigen::Ref<Eigen::MatrixXd> Hy) const;

118
python/gtsam/tests/test_Pose3.py
View File

@ -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()