test_rigid_transform.py 文件源码

def test_inverse(self):
        R_a_b = RigidTransform.random_rotation()
        t_a_b = RigidTransform.random_translation()
        T_a_b = RigidTransform(R_a_b, t_a_b, 'a', 'b')
        T_b_a = T_a_b.inverse()

        # multiple with numpy arrays
        M_a_b = np.r_[np.c_[R_a_b, t_a_b], [[0,0,0,1]]]
        M_b_a = np.linalg.inv(M_a_b)

        self.assertTrue(np.sum(np.abs(T_b_a.matrix - M_b_a)) < 1e-5, msg='Inverse gave incorrect transformation') 

        # check frames
        self.assertEqual(T_b_a.from_frame, 'b', msg='Inverse has incorrect input frame')
        self.assertEqual(T_b_a.to_frame, 'a', msg='Inverse has incorrect output frame')