这一篇简要说一下我是如何标定激光雷达和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配准。
下篇继续记录后续