python_激光点云变换到图像平面

输入

pcd文件路径

激光到相机的变换矩阵Tr(4x4)

输出

相机坐标系下的点云图像

代码

def lidar2CameraImage(file_path, Tr):
    pcd = o3d.io.read_point_cloud(file_path)
    points = np.asarray(pcd.points)     #such as (10, 3)
    R = Tr[0:3,0:3]           #(3,3)
    t = Tr[0:3,3].reshape([3,1])                #(3,)
    counts = points.shape[0]
    for i in range(counts):
        #方法1
        Pw = points[i].reshape([3,1])      #(3,)
        Pc = R.dot(Pw)+t
        print(Pw)
        print(Pc)

        #方法2 HERETO
        Pw1 = np.vstack((Pw, [1] ))     #(3,)
        Pc1 = Tr.dot(Pw1)
        print(Pc1)

    return True