Carla点云坐标系变换

Carla中使用激光雷达获取到的点云是沿着X轴方向，Z轴向上的。

 而从深度图转换成点云的是沿着Z轴的，Y轴向上。

 那么在从深度图转换为点云时，即可将三个坐标轴顺序调整即可获得正确的位置。

# pd2 = [u,v,1]
p2d = np.array([u_coord, v_coord, np.ones_like(u_coord)])
# P = [X,Y,Z]
print("p2d.shape",p2d.shape)
p3d = np.dot(np.linalg.inv(k), p2d)
p3d *= normalized_depth * far

result = o3d.geometry.PointCloud()
if color is not None:
    # 将坐标轴变换
    points = np.transpose(p3d)[:, [2, 0, 1]]
    points = o3d.utility.Vector3dVector(points)
    result.points = points
    result.colors = o3d.utility.Vector3dVector(color)
else:
    points = np.transpose(p3d)[:, [2, 0, 1]]
    points = o3d.utility.Vector3dVector(points)
    result.points = points