def test_calc_relative_transform():
R_wa = random_rotation_matrix(3)
t_wa = np.random.uniform(-1, 1, 3)
T_wa = motion_matrix(R_wa, t_wa)
R_wb = random_rotation_matrix(3)
t_wb = np.random.uniform(-1, 1, 3)
T_wb = motion_matrix(R_wb, t_wb)
T_ab = calc_relative_transform(T_wa, T_wb)
assert_array_almost_equal(np.dot(T_wa, T_ab), T_wb)
def test_get_rotation_translation():
R = random_rotation_matrix(3)
t = np.random.uniform(-1, 1, 3)
R_, t_ = get_rotation_translation(motion_matrix(R, t))
assert_array_equal(R, R_)
assert_array_equal(t, t_)
def test_transform_se3():
R_10 = np.random.random((3, 3))
t_10 = np.random.random(3)
T_10 = motion_matrix(R_10, t_10)
P0 = np.random.uniform(-10, 10, (10, 3))
P1 = transform_se3(T_10, P0)
assert_array_almost_equal(P1, np.dot(R_10, P0.T).T + t_10)
def test_motion_matrix():
R = np.array([
[0, 1, 2],
[3, 4, 5],
[6, 7, 8]
])
t = np.array([9, 10, 11])
T = motion_matrix(R, t)
assert_array_equal(T,
np.array([
[0, 1, 2, 9],
[3, 4, 5, 10],
[6, 7, 8, 11],
[0, 0, 0, 1]
])
)
def T(self):
return motion_matrix(self.R, self.t)
def test_inv_motion_matirx():
R = random_rotation_matrix(3)
t = np.random.uniform(-1, 1, 3)
T = motion_matrix(R, t)
assert_array_almost_equal(inv_motion_matrix(T), inv(T))