当前位置：首页 > news >正文

C++与Python实现LiDAR点云投影对比：关键差异与易错点详解

news 2025/7/4 14:27:46

问题背景

在多传感器融合任务中，常需将LiDAR点云投影到相机图像。然而，不同语言（如C++和Python）的实现可能存在细微差异，导致投影结果不一致。本文通过对比两段功能相近的代码（C++和Python），分析常见错误场景，总结避坑指南。

代码结构对比

C++代码核心流程（代码1）

// 1. 加载内参和外参
LoadIntrinsic(intrinsic_json, K, dist_coeffs);
LoadExtrinsic(extrinsic_json, T_lidar2cam);

// 2. 转换点云到相机坐标系
pcl::transformPointCloud(pcd, pcd_cam, T_lidar2cam);

// 3. 投影到原始图像（应用畸变参数）
cv::Mat img_result = ProjectToRawImage(
    img, K, dist_coeffs, T_lidar2cam, pcd_cam
);

// 4. 显示结果（Pangolin）

Python代码核心流程（代码2）

# 1. 加载内参和外参
K, dist_coeffs = load_intrinsic(intrinsic_json)
T_lidar2cam = load_extrinsic(extrinsic_json)

# 2. 转换点云到相机坐标系
pts_cam = transform_points(pts, T_lidar2cam)

# 3. 生成去畸变图像（错误步骤）
img_undistorted = cv2.undistort(img, K, dist_coeffs)

# 4. 投影到图像（错误使用原始参数）
pts_2d, _ = cv2.projectPoints(
    pts_cam, rvec, tvec, K, dist_coeffs  # 导致重复畸变
)

# 5. 显示结果（OpenCV）

关键差异与易错点

1. 畸变处理逻辑

步骤	C++代码	Python代码（原错误实现）	正确做法（Python修正）
图像预处理	保持原始图像	对图像去畸变	选择：保持原始或更新内参
投影参数	使用原始K和畸变参数	错误使用原始K和畸变参数	去畸变后需用新K并无畸变
结果一致性	与原始图像匹配	与去畸变图像不匹配	根据需求选择方案

错误示例（Python）

# 错误：去畸变后仍用原始参数
img_undistorted = cv2.undistort(img, K, dist_coeffs)
pts_2d, _ = cv2.projectPoints(pts_cam, ..., K, dist_coeffs)

正确修正

# 方案一：保持原始图像
img_result = img.copy()
pts_2d, _ = cv2.projectPoints(pts_cam, ..., K, dist_coeffs)

# 方案二：更新内参并无畸变
new_K, _ = cv2.getOptimalNewCameraMatrix(K, dist_coeffs, (w, h), 0)
img_undistorted = cv2.undistort(img, K, dist_coeffs, None, new_K)
pts_2d, _ = cv2.projectPoints(pts_cam, ..., new_K, None)

2. 外参矩阵加载与乘法顺序

C++代码

// 正确：点云坐标右乘外参矩阵（T * p）
pcl::transformPointCloud(pcd, pcd_cam, T_lidar2cam);

Python代码

# 正确：齐次坐标右乘外参矩阵（T @ p_hom.T）
pts_hom = np.hstack((pts, np.ones((len(pts),1))))
pts_cam = (T_lidar2cam @ pts_hom.T).T[:, :3]

3. 数据类型与精度

易错点

# 错误：int()直接截断小数
x, y = int(pt[0][0]), int(pt[0][1])

# 正确：四舍五入
x, y = np.round(pt[0]).astype(int)

4. 坐标系定义

Python 易错点

# 检查点云坐标系（示例：Y轴取反）
pts[:, 1] *= -1  # 根据实际传感器调整

总结：避坑指南

畸变处理二选一：
- 原始图像：使用原始内参和畸变参数。
- 去畸变图像：更新内参并无畸变。
矩阵加载验证：

# 打印外参矩阵确认
print("T_lidar2cam:\n", T_lidar2cam)

坐标系对齐：
- 通过标定板验证投影点与图像特征是否对齐。
- 检查点云与相机的轴方向定义。
精度一致性：
- 对比C++和Python的浮点计算中间结果（如变换后的点坐标）。
可视化调试：

# 绘制特征点辅助调试
cv2.circle(img, (cx, cy), 5, (0,0,255), -1)  # 图像中心

完整代码参考

C++关键代码（正确实现）

cv::Mat ProjectToRawImage(cv::Mat img, Eigen::Matrix3d K,
                         std::vector<float> dist, Eigen::Matrix4d T) {
    pcl::PointCloud<pcl::PointXYZI> pcd_cam;
    pcl::transformPointCloud(pcd, pcd_cam, T);
    std::vector<cv::Point3f> points_3d;
    for (auto &p : pcd_cam) {
        if (p.z > 0) points_3d.emplace_back(p.x, p.y, p.z);
    }
    std::vector<cv::Point2f> points_2d;
    cv::projectPoints(points_3d, cv::Vec3f(0,0,0), cv::Vec3f(0,0,0),
                     convertMatrix(K), convertDistCoeffs(dist), points_2d);
    cv::Mat result = img.clone();
    for (auto &pt : points_2d) {
        if (pt.x >=0 && pt.x < img.cols && pt.y >=0 && pt.y < img.rows) {
            cv::circle(result, pt, 2, cv::Scalar(0,255,0), -1);
        }
    }
    return result;
}