相机内外参实践之点云投影矢量图

概述

涉及到的坐标变换

深度值可视化

3D点云的2D投影实现

实现效果

参考文献

概述

Camer的内外参在多模态融合中主要涉及到坐标系变换，即像素坐标、相机坐标以及其他坐标系。这篇就针对点云到图像的投影与反投影做代码实践，来构建一张具有深度信息的2D图片验证。

涉及到的坐标变换

主要涉及三个坐标系的转换（激光坐标系、相机坐标系、像素坐标系），关系如下：

其中，(u，v，1)是某点在图像像素坐标系下的坐标，（Xw，Yw，Zw）为激光坐标系下的坐标值。而中间的两个矩阵分别是相机内参、外参，最前面的系数就是从光心到实际物体的距离在沿着成像中心到光心轴线的投影距离。

深度值可视化

为了实现深度信息在图像上清晰的表达，随便搜了网上的代码，主要是用来将深度值离散到RGB序列上，使不同距离的物体能呈现不同的颜色，起到渐变的效果。这一块只是可视化，意会即可，具体如何可视化可根据自己需要，下方函数主要参考了文献1中的代码。

def color_steps(step=255, src=(0, 255, 255), dst=(0, 0, 0)):
    """
    主要用来产生渐变RGB值表达深度信息
    """
    color_num = step + 1
    from_rgb, to_rgb = src, dst
    colors = [(int(from_rgb[0] + (to_rgb[0] - from_rgb[0]) / step + i),
               int(from_rgb[1] + (to_rgb[1] - from_rgb[1]) / step + i),
               int(from_rgb[2] + (to_rgb[2] - from_rgb[2]) / step + i),
               ) for i in range(color_num)]
    return colors

3D点云的2D投影实现

首先要将点云从其自身传感器的坐标系下变换至相机坐标系下，在此基础上，再归一化深度投影至像素坐标系。注意我这里的过程使用的是齐次坐标变换，这个需要根据标定参数的形式、车上不同坐标系的位姿来具体分析调整更方便的矩阵运算。

def project2image(image_file, pcd_file, in_matrix, rt_matrix, level=100):
    points_cloud = o3d.io.read_point_cloud(pcd_file)
    points_cloud = np.asarray(points_cloud.points_cloud)
    points_cloud = np.hstack((points_cloud, np.ones((points_cloud.shape[0], 1))))
    points_cloud = np.dot(rt_matrix, points_cloud.T)
    points_cloud = points_cloud[0:3, :]
    pixel_depth = copy.deepcopy(points_cloud[2, :])
    points_cloud = points_cloud / points_cloud[2, :]
    pixel = np.dot(in_matrix, points_cloud)
    image = cv2.imread(image_file)
    height, width = image.shape[0:2]
    inner = (pixel[0, :] >= 0) & (pixel[0, :] < width) & (pixel[1, :] >= 0) & (pixel[0, :] < height) & (
                pixel_depth >= 0)
    pixel = pixel[:, inner].astype(np.int32)
    pixel_depth = pixel_depth[inner]
    color_values = color_steps(step=level)
    min_pixel_depth, max_pixel_depth = min(pixel_depth), max(pixel_depth)
    for _h, _w, _d in zip(pixel[1:], pixel[0:], pixel_depth):
        color_id = level * (_d - min_pixel_depth) / (max_pixel_depth - min_pixel_depth)
        cv2.circle(image, (_w, _h), 2, color_values[int(color_id)], -1)
    cv2.imwrite("result.jpg", image)