第四章：实战与工具——OpenCV/Eigen坐标变换、PCL点云变换与Kalibr/CamOdoCal标定

好，到了这一章，咱们终于要真刀真枪地干了。前面讲了那么多理论，什么旋转矩阵、四元数、李代数，说实话，光看公式容易晕。我自己的经验是，哪怕你理论记得不牢，只要亲手写过几行变换代码，跑通过一次标定流程，那些概念自然就刻在脑子里了。

这一章，我带你过三个最常用的工具：OpenCV 和 Eigen 做坐标变换，PCL 做点云变换，以及 Kalibr 和 CamOdoCal 做传感器标定。每个工具我都会给一段实战代码，再附上我踩过的坑。

核心思路： 坐标变换的本质就是矩阵乘法。不管你是用 OpenCV 的 cv::Mat，还是 Eigen 的 Matrix4d，亦或是 PCL 的 pcl::transformPointCloud，底层干的都是同一件事——把点从 A 坐标系搬到 B 坐标系。

4.1 用 OpenCV 和 Eigen 做坐标变换

先说说 OpenCV。很多做视觉的朋友习惯用 cv::Mat 来存变换矩阵。我个人觉得，对于 2D 图像变换，OpenCV 的 warpAffine 和 warpPerspective 确实方便。但到了 3D 空间，我建议你改用 Eigen。

为什么？因为 Eigen 的模板元编程让矩阵运算快得多，而且它原生支持四元数、李代数（通过 Sophus 扩展）。我在一个视觉惯性里程计项目里，把 OpenCV 的矩阵运算换成 Eigen，速度提升了将近 30%。

来看一个典型的例子：把相机坐标系下的一个点，变换到世界坐标系。

// Eigen 版本：相机到世界坐标变换
#include <Eigen/Dense>
#include <Eigen/Geometry>

// 定义变换矩阵 T_cw (相机->世界)
Eigen::Matrix4d T_cw = Eigen::Matrix4d::Identity();
// 假设旋转部分：绕Z轴转30度
Eigen::AngleAxisd rot_z(30.0 * M_PI / 180.0, Eigen::Vector3d::UnitZ());
T_cw.block<3,3>(0,0) = rot_z.toRotationMatrix();
// 平移部分：相机在世界坐标系中的位置
T_cw.block<3,1>(0,3) = Eigen::Vector3d(1.0, 2.0, 0.5);

// 相机坐标系下的点
Eigen::Vector4d p_cam(0.2, -0.1, 1.5, 1.0);
// 变换到世界坐标系
Eigen::Vector4d p_world = T_cw * p_cam;

std::cout << "世界坐标: " << p_world.transpose() << std::endl;

小技巧： 用 Eigen 时，记得把齐次坐标的最后一维设为 1.0。如果设成 0.0，那就变成方向向量变换了——平移部分会被忽略。这个细节我见过好几个人搞混。

OpenCV 版本其实也类似，只是矩阵类型不同。不过 OpenCV 的 cv::Mat 默认是动态大小，做乘法时类型检查没那么严格。我建议你只在图像层面用 OpenCV，到了 3D 几何层面，果断切 Eigen。

4.2 用 PCL 做点云变换

点云变换，说白了就是把一堆 3D 点整体挪个位置。PCL 里封装好了 pcl::transformPointCloud，你只需要给一个变换矩阵就行。

我记得有一次做激光雷达和相机的融合，激光雷达扫出来的点云在雷达坐标系下，要投影到图像上，就得先变换到相机坐标系。当时我直接用 pcl::transformPointCloud，一行代码搞定。

#include <pcl/common/transforms.h>
#include <pcl/point_types.h>

// 假设你有一个点云
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in(new pcl::PointCloud<pcl::PointXYZ>());
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_out(new pcl::PointCloud<pcl::PointXYZ>());

// 填充点云数据（略）
// ...

// 定义变换矩阵：从雷达坐标系到相机坐标系
Eigen::Matrix4f T_lidar_to_cam = Eigen::Matrix4f::Identity();
T_lidar_to_cam.block<3,3>(0,0) = ...; // 旋转矩阵
T_lidar_to_cam.block<3,1>(0,3) = ...; // 平移向量

// 执行变换
pcl::transformPointCloud(*cloud_in, *cloud_out, T_lidar_to_cam);

注意： PCL 的 transformPointCloud 默认使用齐次坐标。如果你的点云包含法向量（比如 PointNormal 类型），记得用 transformPointCloudWithNormals，否则法向量方向会出错。我曾经因为这个，在点云配准里折腾了两天。

另外，PCL 还支持对点云进行滤波后再变换。比如先通过体素滤波器降采样，再变换，能省不少计算量。我一般习惯：降采样 → 变换 → 裁剪感兴趣区域。

4.3 用 Kalibr 和 CamOdoCal 做标定

标定，是多传感器融合里最头疼的一步。你想想看，相机和 IMU 之间的外参差 1 毫米，投影误差可能就差了十几个像素。Kalibr 是目前做相机-IMU 标定最主流的工具，没有之一。

Kalibr 的标定流程大致分三步：

准备数据： 录制一段包含棋盘格或 AprilTag 的 rosbag，同时采集 IMU 数据。
配置参数： 写好相机内参模型（pinhole、omni 等）和 IMU 噪声参数。
运行标定： 用 kalibr_calibrate_imu_camera 命令，等它跑完。

我刚开始用 Kalibr 时，踩过一个坑：IMU 的坐标系定义和 Kalibr 默认的不一致。Kalibr 用的是 ROS 标准（x 前、y 左、z 上），但有些 IMU 的 datasheet 定义不同。结果标出来的外参完全不对。

避坑指南： 运行标定前，一定要确认 IMU 的坐标系朝向。我建议你先用静止数据检查一下 IMU 的加速度计输出——如果静止时 z 轴读数是 9.8，说明 z 轴朝上；如果是 -9.8，说明 z 轴朝下。这个检查花不了 5 分钟，但能省你半天调试时间。

CamOdoCal 是另一个标定工具，专门做相机到相机的外参标定。如果你有多目相机系统，用它很方便。它的原理是通过检测多相机同时看到的棋盘格角点，联合优化出相机间的位姿。

用 CamOdoCal 时，我建议你注意两点：

棋盘格要足够大，覆盖所有相机的共同视野。太小了，角点检测不稳定。
录制数据时，缓慢移动棋盘格，不要突然加速。运动模糊会导致角点检测失败。

最后，给你一个标定后的验证方法：把标定得到的外参应用到实际数据上，看看投影误差。比如把激光雷达点云投影到图像上，如果边缘对齐误差在 2-3 个像素以内，就算合格。如果差得远，别急着怀疑标定结果，先检查一下时间戳对齐——我遇到过好几次，问题出在时间戳上，而不是外参本身。

我的习惯： 标定完成后，我会把外参矩阵保存成 YAML 文件，并在代码里写一个加载函数。这样下次换传感器或者重新标定时，只需要改一个配置文件，不用改代码。

好了，这一章的内容就是这些。工具只是手段，理解背后的几何意义才是关键。你动手跑一遍这些代码，比看十遍理论都管用。

课程目录

第四章：实战与工具——OpenCV/Eigen坐标变换、PCL点云变换与Kalibr/CamOdoCal标定

4.1 用 OpenCV 和 Eigen 做坐标变换

4.2 用 PCL 做点云变换

4.3 用 Kalibr 和 CamOdoCal 做标定