241 lines
9.2 KiB
Python
241 lines
9.2 KiB
Python
"""
|
|
GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
|
|
Atlanta, Georgia 30332-0415
|
|
All Rights Reserved
|
|
|
|
See LICENSE for the license information
|
|
|
|
Pose3 unit tests.
|
|
Author: Frank Dellaert, Duy Nguyen Ta
|
|
"""
|
|
# pylint: disable=no-name-in-module
|
|
import math
|
|
import unittest
|
|
|
|
import numpy as np
|
|
from gtsam.utils.test_case import GtsamTestCase
|
|
|
|
import gtsam
|
|
from gtsam import Point3, Pose3, Rot3
|
|
|
|
|
|
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."""
|
|
|
|
def test_between(self):
|
|
"""Test between method."""
|
|
T2 = Pose3(Rot3.Rodrigues(0.3, 0.2, 0.1), Point3(3.5, -8.2, 4.2))
|
|
T3 = Pose3(Rot3.Rodrigues(-90, 0, 0), Point3(1, 2, 3))
|
|
expected = T2.inverse().compose(T3)
|
|
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))
|
|
actual = pose.transformTo(Point3(3, 2, 10))
|
|
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)
|
|
self.assertEqual(actual_array.shape, (3, 2))
|
|
expected_array = np.stack([expected, expected]).T
|
|
np.testing.assert_allclose(actual_array, expected_array, atol=1e-6)
|
|
|
|
def test_transformFrom(self):
|
|
"""Test transformFrom method."""
|
|
pose = Pose3(Rot3.Rodrigues(0, 0, -math.pi/2), Point3(2, 4, 0))
|
|
actual = pose.transformFrom(Point3(2, 1, 10))
|
|
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)
|
|
self.assertEqual(actual_array.shape, (3, 2))
|
|
expected_array = np.stack([expected, expected]).T
|
|
np.testing.assert_allclose(actual_array, expected_array, atol=1e-6)
|
|
|
|
def test_range(self):
|
|
"""Test range method."""
|
|
l1 = Point3(1, 0, 0)
|
|
l2 = Point3(1, 1, 0)
|
|
x1 = Pose3()
|
|
|
|
xl1 = Pose3(Rot3.Ypr(0.0, 0.0, 0.0), Point3(1, 0, 0))
|
|
xl2 = Pose3(Rot3.Ypr(0.0, 1.0, 0.0), Point3(1, 1, 0))
|
|
|
|
# establish range is indeed zero
|
|
self.assertEqual(1, x1.range(point=l1))
|
|
|
|
# establish range is indeed sqrt2
|
|
self.assertEqual(math.sqrt(2.0), x1.range(point=l2))
|
|
|
|
# establish range is indeed zero
|
|
self.assertEqual(1, x1.range(pose=xl1))
|
|
|
|
# establish range is indeed sqrt2
|
|
self.assertEqual(math.sqrt(2.0), x1.range(pose=xl2))
|
|
|
|
def test_adjoint(self):
|
|
"""Test adjoint methods."""
|
|
T = Pose3()
|
|
xi = np.array([1, 2, 3, 4, 5, 6])
|
|
# test calling functions
|
|
T.AdjointMap()
|
|
T.Adjoint(xi)
|
|
T.AdjointTranspose(xi)
|
|
Pose3.adjointMap(xi)
|
|
Pose3.adjoint(xi, xi)
|
|
# test correctness of adjoint(x, y)
|
|
expected = np.dot(Pose3.adjointMap_(xi), xi)
|
|
actual = Pose3.adjoint_(xi, xi)
|
|
np.testing.assert_array_equal(actual, expected)
|
|
|
|
def test_serialization(self):
|
|
"""Test if serialization is working normally"""
|
|
expected = Pose3(Rot3.Ypr(0.0, 1.0, 0.0), Point3(1, 1, 0))
|
|
actual = Pose3()
|
|
serialized = expected.serialize()
|
|
actual.deserialize(serialized)
|
|
self.gtsamAssertEquals(expected, actual, 1e-10)
|
|
|
|
def test_align_squares(self):
|
|
"""Test if Align method can align 2 squares."""
|
|
square = np.array([[0,0,0],[0,1,0],[1,1,0],[1,0,0]], float).T
|
|
sTt = Pose3(Rot3.Rodrigues(0, 0, -math.pi), Point3(2, 4, 0))
|
|
transformed = sTt.transformTo(square)
|
|
|
|
st_pairs = []
|
|
for j in range(4):
|
|
st_pairs.append((square[:,j], transformed[:,j]))
|
|
|
|
# Recover the transformation sTt
|
|
estimated_sTt = Pose3.Align(st_pairs)
|
|
self.gtsamAssertEquals(estimated_sTt, sTt, 1e-10)
|
|
|
|
# Matrix version
|
|
estimated_sTt = Pose3.Align(square, transformed)
|
|
self.gtsamAssertEquals(estimated_sTt, sTt, 1e-10)
|
|
|
|
|
|
if __name__ == "__main__":
|
|
unittest.main()
|