def test_right_jacobian_so3(self):
# test first-order approximation
for i in range(100):
rotvec = tf.random_vector(3) * np.pi
perturbation = tf.random_vector(3) / 1000.0
R = tf.expmap_so3(rotvec + perturbation)
Jr = tf.right_jacobian_so3(rotvec)
R_approx = np.dot(tf.expmap_so3(rotvec),
tf.expmap_so3(np.dot(Jr, perturbation)))
npt.assert_array_almost_equal(R, R_approx)
def test_S_inv_eulerZYX_body_deriv(self):
for i in range(100):
euler_coordinates = tf.random_vector(3)
omega = tf.random_vector(3) / 10.0
perturbation = tf.random_vector(3) / 1000.0
e = np.dot(tf.S_inv_eulerZYX_body(euler_coordinates), omega)
e_perturbed = np.dot(
tf.S_inv_eulerZYX_body(euler_coordinates + perturbation),
omega)
J = tf.S_inv_eulerZYX_body_deriv(euler_coordinates, omega)
e_perturbed_predicted = e + np.dot(J, perturbation)
npt.assert_array_almost_equal(e_perturbed, e_perturbed_predicted)
def test_axis_angle(self):
for i in range(100):
axis = tf.random_vector(3)
axis = axis / np.linalg.norm(axis)
theta = np.random.rand() * np.pi
R = tf.axis_angle(axis, theta)
npt.assert_almost_equal(np.linalg.det(R), 1.0)
def random_se3():
"""
:return: a random SE(3) matrix (for debugging)
"""
r = random_so3()
t = tr.random_vector(3)
return se3(r, t)
def test_compare_axis_angle(self):
for i in range(100):
axis = tf.random_vector(3)
axis = axis / np.linalg.norm(axis)
theta = np.random.rand() * np.pi
R_1 = tf.axis_angle(axis, theta)
R_2 = tf.rotation_matrix(theta, axis)[:3, :3]
npt.assert_array_almost_equal(R_1, R_2)
def test_logmap_so3(self):
for i in range(100):
axis = tf.random_vector(3)
axis = axis / np.linalg.norm(axis)
# can't have angles larger than np.pi otherwise the logmap is a bijection
theta = np.random.rand() * np.pi * 0.999
R = tf.expmap_so3(axis * theta)
rotvec = tf.logmap_so3(R)
npt.assert_array_almost_equal(axis * theta, rotvec)
def test_skew(self):
for i in range(100):
v = tf.random_vector(3)
R = tf.skew(v)
npt.assert_array_almost_equal(v, tf.unskew(R))
def test_first_order_rotation(self):
for i in range(100):
rotvec = tf.random_vector(3) * np.pi * 2.0
R = tf.first_order_rotation(rotvec)
npt.assert_array_almost_equal(R, np.identity(3) + tf.skew(rotvec))