test_rigid_transform.py 文件源码

def test_point_cloud_transformation(self, num_points=10):
        R_a_b = RigidTransform.random_rotation()
        t_a_b = RigidTransform.random_translation()
        T_a_b = RigidTransform(R_a_b, t_a_b, 'a', 'b')

        x_a = np.random.rand(3, num_points)
        pc_a = PointCloud(x_a, 'a')

        # multiply with numpy arrays
        x_b = R_a_b.dot(x_a) + np.tile(t_a_b.reshape(3,1), [1, num_points])

        # use multiplication operator
        pc_b = T_a_b * pc_a

        self.assertTrue(np.sum(np.abs(pc_b.data - x_b)) < 1e-5, msg='Point cloud transformation incorrect:\n Expected:\n {}\n Got:\n {}'.format(x_b, pc_b.data)) 

        # check frames
        self.assertEqual(pc_b.frame, 'b', msg='Transformed point cloud has incorrect frame')