Carla从相机拍摄深度图片到深度图转点云

相机的摆放位置涉及到的基础知识：

pitch（俯仰） ：绕x轴旋转，正值将相机向上旋转，负值将相机向下旋转。Yaw（偏航）：绕y轴旋转，正值将相机向右旋转，负值将相机向左旋转。Row（滚转）：绕z轴旋转，正值顺时针旋转，负值逆时针旋转。

将深度相机拍摄得到的深度图的深度值转换为归一化的深度值（Carla的深度图的编码方式就是这样的，转换方法如下）：

# 目的是将深度值转换为归一化后的深度值二维数组
def depth_to_array(image):
    """
    Convert an image containing CARLA encoded depth-map to a 2D array containing
    the depth value of each pixel normalized between [0.0, 1.0].
    """
    array = image
    array = array.astype(np.float32)
    # Apply (R * 256 * 256 + G * 256 + B ) / (256 * 256 * 256 - 1).
    normalized_depth = np.dot(array[:, :, :3], [65536.0, 256.0, 1.0])
    normalized_depth /= 16777215.0  # (256.0 * 256.0 * 256.0 - 1.0)
    return normalized_depth

将深度图传入，并把语义分割图传入作为color。 

def depth_to_local_point_cloud(light_id, image, color=None, max_depth=0.1):
    """
    Convert an image containing CARLA encoded depth-map to a 2D array containing
    the 3D position (relative to the camera) of each pixel and its corresponding
    RGB color of an array.
    "max_depth" is used to omit the points that are far enough.
    """
    far = 1000.0  # max depth in meters.
    fov=90
    normalized_depth = depth_to_array(image)
    img_width=800
    img_height=600

    # 计算相机内参矩阵K
    k = np.identity(3)
    k[0, 2] = img_width / 2.0
    k[1, 2] = img_height / 2.0
    k[0, 0] = k[1, 1] = img_width / \
        (2.0 * math.tan(fov * math.pi / 360.0))

    # 计算像素坐标
    pixel_length = img_width * img_height
    # 生成水平方向的像素坐标 u_coord。
    # 使用 np.matlib.repmat 函数将一个水平递减的序列 [img_width-1, img_width-2, ..., 0] 复制成一个大小为 (img_height, img_width) 的矩阵。
    # 然后，使用 reshape 函数将矩阵展平成一维数组，长度为 pixel_length。这样，u_coord 中的每个元素表示对应像素的水平坐标。
    u_coord = np.matlib.repmat(np.r_[img_width-1:-1:-1],
                     img_height, 1).reshape(pixel_length)
    v_coord = np.matlib.repmat(np.c_[img_height-1:-1:-1],
                     1, img_width).reshape(pixel_length)
    if color is not None:
        print("color.shape",len(color)) # color.shape = 600
        color = color.reshape(pixel_length, 3)
    print("normalized depth before reshape", normalized_depth.shape) #(600,800)
    normalized_depth = np.reshape(normalized_depth, pixel_length)
    print("normalized depth after reshape",normalized_depth.shape) # (480000,)
    
    # 将深度值大于max_depth的像素删除掉
    max_depth_indexes = np.where(normalized_depth > max_depth)
    normalized_depth = np.delete(normalized_depth, max_depth_indexes)
    u_coord = np.delete(u_coord, max_depth_indexes)
    v_coord = np.delete(v_coord, max_depth_indexes)
    if color is not None:
        color = np.delete(color, max_depth_indexes, axis=0)
    # pd2 = [u,v,1]
    p2d = np.array([u_coord, v_coord, np.ones_like(u_coord)])

    # P = [X,Y,Z]
    # 应为内参K是相机坐标系到像素坐标系的，所以要从像素到相机，需要乘内参的逆矩阵。
    p3d = np.dot(np.linalg.inv(k), p2d)
    p3d *= normalized_depth * far

    result = o3d.geometry.PointCloud()
    # Formating the output to:
    # [[X1,Y1,Z1,R1,G1,B1],[X2,Y2,Z2,R2,G2,B2], ... [Xn,Yn,Zn,Rn,Gn,Bn]]
    if color is not None:
        points = np.transpose(p3d)
        points = o3d.utility.Vector3dVector(points)
        result.points = points
        result.colors = o3d.utility.Vector3dVector(color)
    else:
        points = np.transpose(p3d)
        points = o3d.utility.Vector3dVector(points)
        result.points = points
    o3d.io.write_point_cloud(os.path.join('junction_images', f"light_{light_id}_ply.ply"),result)
    print("done")

原始的深度图：

 代码转换后的点云：

下一篇将介绍坐标系的转换。