2023年2月重要补充
这个代码我个人觉得不好用且坑太多，所以后来换了一个。推荐大家用新的代码。
详见更新的一篇博客总结：【学习总结】激光雷达与相机外参标定：代码（cam_lidar_calibration）
这一周多学习并调试了激光雷达和相机外参标定的代码，踩了一堆坑，特此记录。
0. 参考资料：
代码来源：https://github.com/ankitdhall/lidar_camera_calibration
参考论文：LiDAR-Camera Calibration using 3D-3D Point correspondences
修改后的代码：https://github.com/LarryDong/lidar_camera_calibration
之前找了几个开源的标定代码，在github上看就这个代码具有最多的star，觉得比较靠谱。结果这个代码写的真的一言难尽：逻辑混乱，需要注意的细节极其多，以及存在不少坑。这一个多星期，就是踩坑和调试。今天终于能够正常运行出一个正确（但并不准确）的结果，在此记录。
代码混乱的原因：
- 大量变量定义未使用，以及定义与实际使用的距离太远，让人误解；
- 需要修改多个参数文件，参数管理混乱；参数由不同文件读取，同时一些临时变量也存储到文件中再在不同函数中读取调用，以及全局变量的使用；
- 官方使用文档一些细节没有强调。
1. 基本原理
计算雷达和相机的外参，基本原理较为简单：只需要找雷达系下的点与相机系下对应点的坐标关系，然后计算外参即可。之前一篇博客总结了几种常用的方法：【学习记录】激光雷达与相机标定
问题核心是，如何确定对应关系，即correspondence？
所采用的代码的基本原理：用一块矩形纸板，用激光雷达检测纸板的边界线，之后确定纸板的角点，获取纸板角点在lidar系下的坐标；而相机本身角点无法提供深度信息，但我们可以通过aruco提供带深度的marker的角点，再计算纸板四个角的坐标。
2. 代码总览
首先需要按照原始代码链接中的wiki进行安装，之后需要修改一些参数配置文件。如果运气好，搞清楚怎么用，或许就能一次成果。但许多细节在原始的代码readme中没有讲清楚，因此本博客详细整理。
整个代码的逻辑如下：
首先这个代码的核心是 lidar_camera_calibration下的find_transform.launch 启动的标定程序。这个程序接收数据包括：
- 激光雷达的原始扫描数据
- aruco marker 在 相机系下的位姿
- 相应参数配置文件
其中激光雷达的数据有激光雷达启动节点直接提供，aruco marker的消息由aruco_mapping节点提供，相应参数配置文件为 “lidar_camera_calibration/conf/config_file.txt” 提供。
核心代码在收到上述数据后，对雷达数据进行纸板的边界线和角点检测，最后与aruco_mapping提供的位姿进行迭代求解。下面按先后顺序，介绍程序与节点。
3. 代码详细解释
3.1 数据流
首先从数据流的角度看。
图像数据流：相机驱动->aruco_mapping->”lidar_camera_calibration/conf/transform.txt"文件->find_transform节点。其中相机驱动传输完整图像给aruco_mapping，aruco_mapping从其配置文件 “aruco_mapping/data/xxx.ini"文件中获取相机的内参（包括分辨率、K、畸变参数、以及投影参数等），还从"aruco_mapping/launch/aruco_mapping.launch"文件中获取marker的参数。最后aruco_mapping保存位姿数据在"lidar_camera_calibration/conf/transform.txt"文件，再由find_transform节点读取，再根据从"lidar_camera_calibration/config_file.txt"中获取的marker和纸板（以下记作"board”）的参数
用于纸板的四角在相机下参数计算。
激光雷达数据流：激光雷达驱动->find_transform，相对简单。激光雷达将原始点云数据直接发给find_transform节点，节点根据扫描情况以及"lidar_camera_calibration/conf/config_file.txt"中的配置，将点云中的边界点投影到camera的图像下，用于可视化与人工标注。
另外还有一些中间数据，例如find_transform节点将激光雷达检测出的board角点、与计算出的角点在camera系下的3D坐标，临时存储在"lidar_camera_calibration/conf/points.txt"中，用于后续的读取。
3.2 aruco_mapping节点
上述数据流已说明，aruco_mapping节点负责将原始图像获取后输出图像中marker在相机系下的坐标。它所接受的图像topic名称、marker的尺寸在aruco_mapping.launch中给出，同时还需要相机的参数，相机的参数需要提前标定并按照指定格式修改"/data/xxx.ini"配置文件。
输出6DoF位姿到"lidar_camera_calibration/conf/transform.txt"文件。这部分代码还是比较完善的，详细可参考aruco_mapping官方文档。
要点：需修改接收topic名称、marker参数文件、相机参数.ini文件
3.3 激光雷达节点
激光雷达节点较为简单，即激光雷达的ros下驱动，启动后发布激光雷达数据即可。后由find_transform接收。
3.4 find_transform节点（核心）
这个节点是算法的核心，按照先后顺序以此展开。
3.4.1 消息接收与回调函数
从main函数入手，首届先接收了上述节点发送的topic。其中有一处选择，是采用"callback"还是"callback_noCam"，二者的区别是：相机的内参是从topic接收，还是从文件读取。本质上是一样的，我们采用从文件读取，这样就不需要aruco_mapping节点或相机驱动节点再发送topic数据。读取的文件还是上面说的"conf/config_file.txt"。
这里在回调时用了ROS下message_filters的消息同步机制，保证camera的数据和lidar的数据是同步的。但由于场景可以认为静止，这部同步起始没有太大意义。
3.4.2 点云数据接收
进入回调函数，第一件事就是接收lidar驱动节点发出的数据。这里我遇到了第一个坑：【fromRosMsg()报错 Failed to find match for field “intensity”】。出现这个原因是因为我采用的镭神激光雷达的intensity字段定义的是奇葩的uint类型，而RosMsg下的intensity字段是float，因此无法直接转化。为此需要重新定义自定义点数据类型与拷贝函数。详见上述的学习总结。（其实intensity并没有用）
点云数据接收后，需要转成统一的点云格式，作者采用的是自定义的PointXYZRID，其中有用的字段是"I"表示intensity和"R"表示ring，即第几条线扫描到的数据。一般雷达都提供了扫描的线束信息，在雷达转化时加上即可。如果并没有提供线束信息，可以通过类似A-LOAM源码中的计算俯仰角确定线束。
紧接着遇到了第二个坑：无效数据点即NaN的问题。由于后续需要用到intensity进行边界点检测，而如果激光雷达发出了NaN数据，在后续归一化时会出现错误，所以需要提出NaN数据。但不幸的是，PCL自带的去除NaN数据函数removeNaNFromPointCloud不支持自定义的"PointXYZRID"数据类型，因此不能直接调用。只能自己手写一个判断每个点是不是NaN。
3.4.3 点云变换
拿到点云后，首先将Lidar系下的点云，通过transform函数大致变换到相机坐标系。这一步是为了后续点云能够在相机中有个大致的位置，方便相机画图显示。此时，大致的变换参数是我们估计的，采用"conf/config_file.txt"下给出的初始旋转和平移进行。这里的平移和旋转，是“相机系经过该平移旋转到雷达系”，即“雷达系下的坐标点通过旋转平移后成为相机系下的坐标”。而参数文件中第12行的三个角度，是ZYX欧拉角形式，eularAngle(2,1,0)，单位是弧度。可以通过计算得到的旋转矩阵，判断给的旋转角度对不对。
这里补充一句，一定要注意激光雷达的坐标系定义，和相机的坐标系定义。这里相机的坐标系定义为：z轴沿镜头向外，y轴向下，x轴由右手系定义。
3.4.4 点云边界点检测
边界点检测是intensityByRangeDiff()函数提供，得到场景中所有的边界点。这个函数通过判断同一个ring上一定范围内符合intensity的变化强度，确定边界点。然而这里的intensity具体就是采用的range，即距离。具体如下：
从每一圈第2个点开始，判断与前一个以及后一个点的range的差值，当作强度。之后调用normalizeIntensity函数进行归一化（注：这里要求所有的距离是有效的，如果测到极端距离，例如很远点或者NaN点，则归一化效果不佳）。归一化后，保留强度大于一定阈值且在xyz给定的范围内的点，阈值和xyz的范围从"conf/config_file.txt"第2-5行给出。这里吐槽一句，在前面我intensity转化了半天，后来根本没用到；另外明明是根据range进行检测，这里非叫做intensity，造成里不小的理解歧义，可能是为了方便使用PointXYZI格式吧。
提取完边界后，返回场景中所有的边界点。下一步就需要进行检测角点。
3.4.5 点云中纸板角点检测
角点检测是getCorners()函数。吐槽一下这个函数内部，opencv的代码写的贼烂，各种不明含义的mask和来回赋值。
第一步将整个场景中的边界点投影到相机图像中，函数为project，即“相机图像能够看到哪些激光雷达的边界点”。这里用到了相机的投影矩阵，来自从"conf/config_file.txt"第8-10行读取的config参数。
第二步手动标注纸板的边界点。由于场景中有许多边界点，需要知道哪些是纸板哪条边的边界点。因此采用手动标注的方式。运行到这一步后，会出现cloud窗口和polygon窗口，前者显示在图像视角下看到的边界点。我们在cloud窗口中按顺序点4个点，形成一个多边形(polygon)，框住纸板的一条边界，每次单击后按任意键存储坐标。点完4个点后，polygon窗口会画出所绘制的多边形，以及这个多边形框住的边界点。这样完成了第一条边界的框选，然后按顺序框选其他3条边界。
图：左侧为边界点检测，右侧为框选出的第一条边界
注意1：如果没有激光雷达扫描的边界点没有按照预期投影到相机中，请检查初始给的相机和雷达估计外参（主要是旋转）是否合理。
注意2：这里框选一条边时按照顺时针或逆时针顺序形成多边形，不能较差，否则采用opencv判断某个点是否落入多边形时会出现问题；
注意3：框选4条边时要按照顺时针顺序，这个顺序应该和aruco_mapping检测的marker的旋转一致。如果我们按照官方代码给出的图片中marker的摆放方式，即x轴朝向“左上”，z轴“右上”，如下图，那么在标注雷达点的边界时首先标左上这条边，再标右上这条边。因为要求4个角点要按顺序依次对应，而代码后续寻找雷达的角点时存储的顺序依次是前后两次标注的边界的交点，后续find_transformation()函数中相机系下角点的顺序是右上-右下-左下-左上的顺序。
第三步，注意这里有一个迭代。这个迭代是最外侧callback_noCam()中迭代的，即 3.4.1~3.4.6迭代，而非在这个函数中迭代，迭代次数是MAX_ITERATION次，默认为100。迭代时，我们第一次标注了4条边界的4个polygon，后续就自动提取这4个多边形中的边界点，和marker的边界点，进行计算。
第四步，计算纸板角点。由于上一步提取出了每个边界，那么计算每个边界3维直线参数，采用的是PCL的SampleConsensusModelLine方法进行直线拟合，之后采用PCL的lineToLineSegment计算相邻两条直线的中垂线，并取中点作为角点。
至此，完成了一次迭代中纸板角点的提取。第五步，将提取的3D角点存储到临时文件"/conf/points.txt"中，有n个marker就存储n*4行角点。
3.4.6 相机中纸板角点的检测与外参计算
相机系下纸板角点检测和外参计算都在函数find_transformation()中。这个函数需要接收aruco_mapping节点发出的marker的消息lidar_camera_calibration_rt，并从上述的"/conf/points.txt"文件中读取lidar下角点，进行配准。
第一步，计算角点在相机中的三维坐标。直接读取aruco的参数，之后根据参数以及board和marker的尺寸（由“conf/marker_coordinates.txt"下几行数据定义）计算出角点，就是从这里我们能够判断出几个角点在相机下的坐标。计算方式很简单，由于marker定义了xz平面，所以同一平面的纸板的四个角的y坐标都是0，xz坐标可以通过markder的margin和纸板的尺寸确定。确定后，所有点数据又以追写的方式存在了临时文件"/conf/points.txt"当中。吐槽一下，真的是瞎传参。
第二步，由readArray()函数读取"/conf/points.txt"中所有角点在lidar和camera下的三维坐标。
第三步，由calc_RT()函数迭代计算外参。这个函数的计算，就是典型的ICP算法，首先计算平移，然后SVD分解计算R。这里的Rt是camera系变化到lidar系，即lidar系下坐标转移到camera系的Rt。
正如3.4.5中所说，共进行了100次迭代，每次迭代时都经过了3.4.6这几个函数，计算出一次camera系到lidar系的Rt，然后100次后结束迭代，求解平均的Rt。这里需要注意，程序输出了好几个Rt和T，计算的平均R起始只是在初始给定的旋转下的增量，并不是实际的R，而完整的T中所包含的左上角3x3矩阵才是真正的外参R，右上角3x1的是外参t。注意，这里的迭代100次，用的是全局变量，即在3.4.2中首次标注多边形时iteration_counter就是这个全局变量。这里又在用全局变量传参。
最终结果：红框内为真实的Rt
激光雷达在相机的y轴方向5cm处，基本认为标定正确，只是还不准。后续再提升。