激光雷达+rtk+rgb联合使用（3）

这一篇简要说一下我是如何标定激光雷达和rtk的标定。

        首先来讲，我也不太明确是不是叫标定，还是先说一下我的需求，我是希望将雷达假设在某个移动物体上，比如在车顶，然后通过移动雷达扫描得到一条街道或者一栋楼之类的三维模型，但是这里的问题是，雷达得到每一个点都是这个点与当时雷达原点的关系，而雷达又是在移动的，除非雷达自己做了处理，否则直接导出生成的点云，会是一团点云，没有物体的样子。这里就是需要得到雷达每时每刻的运动姿态，从而将与之相关的每一个点都转换一下，就比如第1s得到的这个点的位置，在第10s的时候，他与当前原点的位置已知，然后需要得到他跟第1s的原点的位置关系，总之就是要固定一个原点，然后得到点云中所有点与他的关系。

        在之前，不使用rtk的话，就是通过定点拍摄的方式，只要定的点足够密集的话，将每个点的小点云直接通过点云匹配就可以配准好，效果也是不错的，就是太费人工了。我们需要的还是实时的，此时rtk就可以发挥作用，将其和雷达放在一起，这样rtk的移动距离，三个角的变化同样可以反应在雷达上。但是还需要解决的是，rtk毕竟不是雷达，他和雷达的原点还有一层关系，这个关系也需要一个公式体现出来。

        在网上查到，这种标定，一般都是通过ros里面，有人提供了相应的方法来标定，看起来不是很复杂，对我来说，难题在于ros没有玩过。那么不用ros行不行呢，无非就是几个公式的运算，我老板是这么跟我说的，就是原理是很清晰的，每个点云的三维坐标已经时间点是可以得到的，然后每个时间段的移动，rtk也是可以得到的，就是几个矩阵运算，至少我可以先做着试试看，一些误差值是不是可以试出来。

        这里先说一下，我用的livox激光雷达，在当时相关的配套软件好像不是很完善，总之就是，想要得到时间戳信息，必须要linux中使用ptp时间同步，但是linux的录制软件还没有发布，所以我需要用到sdk来解析，而sdk是c++。还好有demo，而且我之前做了好几年的c工程师，做的是嵌入式中间件，虽然c语法都忘得差不多了，但是基本编程逻辑还是知道的，看一下demo的编写，大概知道他是做了哪些操作，而且其已经写的比较完善了，只需要删掉不需要得到的一些信息，然后续写一下需要的部分，根据文档具体解析一下，最后写入一个csv文档中即可。一方面是，c毕竟效率更高，能解析就尽量解析一下，可是c已经忘了很多，写起来很痛苦，面向百度编程很烦，所以也只能做到解析完写文档，后面还是python接手处理吧，不要太为难自己。简单吐槽一下，本来是想写文档，用cout来拼接字符串并写入文件的，结果是乱码，还是要fprintf，指定格式并写进入，用一个全局变量，不然每个回调还要先打开文件再怎么怎么操作，最后写一个kill事件回调，程序退出前要自己保存好。

        livox的时间戳单位是ns，我这边是将每100ms的所有点都保存为一个点云文件，倒也不是说不能间隔更短一点，比如50ms，30ms什么的，主要是rtk的时间间隔，最短就是50ms，而且雷达设备就是每轮100ms，没必要再继续切分了。这样每个小点云就是对应两个位置信息，求个平均，而且rtk有时候会吞数据，两个数据缺失一个还有一个能用，如果都没有，那这个时间点的点云就不要了。说明一下，小点云的获取生成什么的，简要说就是用python读取csv文件，通过时间戳筛选点，再用open3d写入一个pcd文件。但是我发现处理速度太慢了，就写成多进程，结果速度快没快不知道，因为还没做完内存就爆了，再后来不断的优化代码，找原因，最后发现，其实是我太纠结一定要用numpy来做处理了，第一步先做整合，npz格式就像一个字典，key为时间戳截取到单位为100ms后的数字，这样相同key的点云就全都加到一起，他们都是同一个100ms内的，而我之前是用的np的stack，完全没必要，使用list就可以了，这一步改动使得整体的速度暴增。用npz就是想把这个过程写两步，而且每个小点云也不是就直接保存就完了，我是希望先保存一个大的npz，再生成相应时间key的位置信息，再根据时间戳提取对应的点云集合和位置转换后生成pcd。

        点云的信息，除了位置信息外，还有一些状态信息，比如sdk提供了一些置信度，这里试了一下，都选最高的就可以了，剩下的点足够。然后xyz三轴坐标要怎么处理，rtk提供了有用信息，分别是当前位置的经纬度，海拔，三个姿态角。这里的原理我是不懂的，就是查到，a位置转为b位置，需要的是一个4*4的矩阵，通过欧拉角转换得到位置，再加上三轴偏移量就可以了。所以实施的时候，以第一个点云为基准，得到下一个点云与他的三轴相对位移和欧拉角，再转换一下，得到变换矩阵，再用open3d将这个点云与矩阵相乘，就能得到第一个点云同视角的点云，了吧。。。

        并没有，苦思冥想很久，甚至试过不要转换了，如果每个点云变换不大的话，可以尝试open3d直接配准，但是效果不理想，除非真的没有变换角度，录的一条笔直道路，而我是绕着一个广场旋转，希望得到广场和广场周围一圈的建筑。后面就一直觉得是不是rtk的误差，感觉不大而实际谬之千里，就一直在纠结这个转换部分。

        再回头说转换矩阵如何生成，首先要得到欧拉角，就是那三个角，对于我这种完全没接触过这种东西的菜鸟，没玩过imu，就只能对着数据发愁。主要就是三个角度要怎么转换，肯定不会是直接用原始数据，最起码，前面说了三个角值域不一样，要先统一吧。其实都试一下就可以了，我这边的做法是，先在rtk0,90,180,270分别录一个点云，将这四个点云，手工操作一下，再配准，得到四个旋转矩阵，再将利用矩阵计算欧拉角，也就是先得到答案，我再去套关系，顺便测一下rtk的距离精度，这还是比较准的，用卷尺量了，画一个正方形，四个角录就可以了，矩阵的偏移距离计算一下跟实际距离对比。反正最后发现角度都转为+-180就可以了，可能还要取负号，不记得了。

        在发现旋转后对的不是很齐的情况，这边对比了一下手工配准的矩阵和我生成的矩阵，数字上感觉是差不多，莫非真的是rtk误差或者是rtk没有和雷达标定的原因嘛。

        因为这是一个记录贴，所以以记录为主，所以会非常啰嗦，总之就是自己太坑，正如前面说的，求出相对角度，这个是不对的，而且我还利用两点经纬度位移，再利用角度计算sin，cos什么的，得到横向纵向位移，因为要得到三轴位移嘛，这里的计算，一开始就是不对的，还亏我画了半天正方体，猜想三个角分别对应啥，如何求位移。具体错误的想法就不提了。最后的解决就是，某天突然想通了，不应该选取第一个点云作为基准，这样的话，计算x、y轴位移求不出来，而都转为相对正北，也就是0度的相对位置，那么，x、y轴就是对应纬线和经线的位移了，分别通过纬度和经度就可以计算，不需要什么相对角度什么的。再试一次，果然直接原始匹配就是比较准的了，为了更为精准，还可以后面再跟一个icp配准。

下篇继续记录后续