📚 课程目录第3章 LiDAR与相机联合标定结果的可视化验证

3. LiDAR与相机联合标定结果的可视化验证

任务目标

加载LiDAR与相机的联合标定参数（外参矩阵与相机内参），将3D点云投影到2D图像平面，通过可视化叠加验证投影精度。若投影存在系统性偏差，调整外参平移量并重新投影，直至点云边缘与图像中物体轮廓对齐。

环境与依赖

Python 3.8+，NumPy，OpenCV（cv2），Matplotlib（可选，用于交互式调参）
数据准备：pointcloud.npy（N×3，XYZ米制坐标）、image.jpg（标定场景图像）、calib_params.npz（含相机内参与外参）

标定参数格式

calib_params.npz包含：

K：3×3相机内参矩阵
dist：5×1畸变系数（k1,k2,p1,p2,k3）
T_lidar_cam：4×4外参矩阵，将LiDAR坐标系下的点变换到相机坐标系

关键完整代码

import numpy as np
import cv2

def load_calib(path):
    data = np.load(path)
    return data['K'], data['dist'], data['T_lidar_cam']

def project_lidar_to_image(points, K, dist, T):
    """将LiDAR点云投影到图像平面，返回像素坐标与深度"""
    # 齐次坐标变换到相机系
    pts_cam = (T @ np.hstack((points, np.ones((len(points),1)))).T).T[:, :3]
    # 去除相机后方的点
    mask = pts_cam[:, 2] > 0.1
    pts_cam = pts_cam[mask]
    # 投影到像素
    pts_img, _ = cv2.projectPoints(pts_cam, np.zeros(3), np.zeros(3), K, dist)
    pts_img = pts_img.reshape(-1, 2)
    return pts_img, pts_cam[:, 2], mask

def visualize_projection(image, pts_img, depth, max_depth=50.0):
    """绘制彩色深度点云投影"""
    vis = image.copy()
    valid = depth < max_depth
    pts = pts_img[valid].astype(int)
    d = depth[valid]
    # 根据深度着色（近红远蓝）
    colors = cv2.applyColorMap(
        (255 * (1 - d / max_depth)).astype(np.uint8), cv2.COLORMAP_JET)
    for (u, v), c in zip(pts, colors[:, 0, :]):
        cv2.circle(vis, (u, v), 2, (int(c[0]), int(c[1]), int(c[2])), -1)
    return vis

def adjust_and_verify(image, points, K, dist, T, x_offset=0.0, y_offset=0.0, z_offset=0.0):
    """调整外参平移量并重新投影验证"""
    T_adj = T.copy()
    T_adj[:3, 3] += np.array([x_offset, y_offset, z_offset])
    pts_img, depth, _ = project_lidar_to_image(points, K, dist, T_adj)
    return visualize_projection(image, pts_img, depth)

if __name__ == "__main__":
    # 加载数据
    K, dist, T = load_calib("calib_params.npz")
    points = np.load("pointcloud.npy")
    img = cv2.imread("image.jpg")
    
    # 初始投影
    pts_img, depth, mask = project_lidar_to_image(points, K, dist, T)
    vis = visualize_projection(img, pts_img, depth)
    cv2.imwrite("projection_initial.jpg", vis)
    
    # 交互式调参（按q退出，a/d左右，w/s上下，z/x前后）
    print("调整外参平移量：a/d=左右, w/s=上下, z/x=前后, q=退出")
    offsets = [0.0, 0.0, 0.0]
    while True:
        vis = adjust_and_verify(img, points, K, dist, T, *offsets)
        cv2.imshow("Adjust Projection", vis)
        key = cv2.waitKey(100) & 0xFF
        if key == ord('q'):
            break
        elif key == ord('a'): offsets[0] -= 0.02
        elif key == ord('d'): offsets[0] += 0.02
        elif key == ord('w'): offsets[1] -= 0.02
        elif key == ord('s'): offsets[1] += 0.02
        elif key == ord('z'): offsets[2] -= 0.02
        elif key == ord('x'): offsets[2] += 0.02
        print(f"当前偏移: x={offsets[0]:.2f}, y={offsets[1]:.2f}, z={offsets[2]:.2f}")
    cv2.destroyAllWindows()
    
    # 保存最终调整后的外参
    T_final = T.copy()
    T_final[:3, 3] += np.array(offsets)
    np.savez("calib_params_adjusted.npz", K=K, dist=dist, T_lidar_cam=T_final)
    print("调整后的外参已保存至 calib_params_adjusted.npz")

运行与验证

将上述代码保存为 verify_calib.py，与三个数据文件置于同一目录。
运行 python verify_calib.py，程序生成 projection_initial.jpg 并弹出交互窗口。
通过标准：观察投影点云是否与图像中车道线、路沿、车辆轮廓对齐。若点云整体偏移，使用按键微调外参平移量，直至点云边缘与图像物体边缘重合（误差小于2像素）。
按 q 退出后，调整后的外参自动保存。

常见问题

投影点严重错位：检查外参矩阵的旋转部分是否为单位阵附近的值，若旋转错误需重新标定，平移微调无法修正旋转误差。
图像畸变明显：确认 dist 系数是否来自同一相机的标定结果，畸变参数错误会导致边缘区域投影偏差大。
点云稀疏：仅当LiDAR点云与相机视野重叠区域足够大时，验证才有意义；确保标定场景中物体位于5～30米范围内。