将激光3D真值标签数据投影到图片上

前言

在自动驾驶领域，激光雷达（LiDAR）技术因其高精度的三维空间测量能力而变得至关重要。除了点云数据，激光雷达系统还能提供丰富的标签数据，这些数据对于对象识别和场景理解非常重要。本文将探讨如何将这些标签数据有效地投影到二维图片上，以便于分析和可视化。

投影原理

激光雷达数据通常以点云的形式存在，每个点包含三维坐标（X, Y, Z）。要将这些点投影到二维图像上，需要利用相机模型和外参（即两个传感器之间的相对位置和姿态），遵循针孔相机模型的基本原理。关键步骤包括：

1. 坐标变换：首先，将激光雷达的点云坐标从激光雷达坐标系转换到相机坐标系。这涉及到旋转和平移操作，通常由外参矩阵（包括旋转矩阵R和平移向量T）完成。

2. 深度映射：接着，利用相机内参（焦距f、主点坐标(cx, cy)）将相机坐标系下的三维点转换为归一化图像坐标（u, v）。

3. 畸变校正：最后，考虑镜头畸变（径向和切向畸变），将归一化坐标映射到实际像素坐标，并进行必要的畸变。

投影代码示例

以下是一个简化的Python代码示例，展示了如何将激光雷达点云数据投影到图像上。

1、获取内外参数

def obtain_camera_param(yaml_file_path):
    # 使用FileStorage打开YAML文件，使用READ标志读取文件
    fs = cv2.FileStorage(yaml_file_path, cv2.FILE_STORAGE_READ)
    if not fs.isOpened():
        print("Error: Could not open YAML file.")
        exit()
 
    # 读取相机矩阵
    camera_matrix = fs.getNode('CameraMat').mat()
    camera_extrin_matrix = fs.getNode('CameraExtrinsicMat').mat()
    # 读取畸变系数
    dist_coeffs = fs.getNode('DistCoeff').mat()
    # 打印参数
    print("Camera Matrix:\n", camera_matrix)
    print("Distortion Coefficients:\n", dist_coeffs)
    print("Camera extri Matrinx:\n", camera_extrin_matrix)
    # 关闭FileStorage对象
    fs.release()
    return camera_matrix, dist_coeffs, camera_extrin_matrix

2、使用标签获取角点信息

def get_3d_box_corners(x, y, z, l, w, h, yaw):
    center = [x, y, z ]
    size = [l, w, h]
    rot = np.asmatrix([[math.cos(yaw), -math.sin(yaw)],\
                    [math.sin(yaw),  math.cos(yaw)]])
    plain_pts = np.asmatrix([[0.5 * size[0], 0.5*size[1]],\
                        [0.5 * size[0], -0.5*size[1]],\
                        [-0.5 * size[0], -0.5*size[1]],\
                        [-0.5 * size[0], 0.5*size[1]]])
    tran_pts = np.asarray(rot * plain_pts.transpose())
    tran_pts = tran_pts.transpose()
    corners = np.arange(24).astype(np.float32).reshape(8, 3)
    for i in range(8):
        corners[i][0] = center[0] + tran_pts[i%4][0]
        corners[i][1] = center[1] + tran_pts[i%4][1]
        corners[i][2] = center[2] + (float(i >= 4) - 0.5) * size[2]
    return corners

3、将角点投影到图片上

def project_to_2d(points_3d, camera_matrix, dist_coeffs, camera_extrin_matrix):
    R = camera_extrin_matrix[0:3, 0:3]
    T = camera_extrin_matrix[0:3, 3]
    points_3d = points_3d.astype(np.float32)
    points_2d, _ = cv2.projectPoints(points_3d, R, T, camera_matrix, dist_coeffs)
    return points_2d.reshape(-1, 2)

4、结果可视化