livox_mapping点云上色

前言

github源项目:
本人零基础，一些浅显的理解，在我看来好像是简陋的fastlio;基于c++实现的;实时的效果正在测，目前用rosbag包看效果
需要依赖的库---opencv(处理相机节点的图像)，Eigen(坐标变换需要矩阵乘法), livox_to_driver(lidar消息要转换成标准的点云，rviz可视化)
需要具备的知识--相机标定的参数(xyz+rpy转换成变换矩阵);机器人的坐标转换(转换关系和矩阵对齐扩展);基础的ros使用;

要测试代码需要准备的节点

1.livox/lidar;
2.camera
3.odom
4.base_link-lidar-camera三者的变换矩阵(odom在我理解来看是基于base_link融合用的，SLAM的误差一部分就来自odom-baselink的误差)

源码阅读与更改

易错点

验证变换矩阵的办法

验证lidar_to_baselink矩阵(源码里面没有baselink的矩阵，这是可以的)：

没有lidar_to_baselink的变换矩阵，可能在rviz里面显示的点云效果不在坐标原点，也可能是歪的，但是每一帧消息的点云形状是可以分辨的；
如果有lidar_to_baselink的变换矩阵，经过转换后，在rviz里面每一帧显示的效果就是现实世界的样子，一定不是歪的，除非你的世界坐标系设定是歪的，但这不可能，这是常识约定;

验证odom融合矩阵

首先对于odom矩阵使用的理解，不同位置的pose不同，但是又相同的参照物，我要基于这些参照物+odom的pose信息，让整个记录的数据连贯起来
其次要用odom融合，一定要先在baselink坐标系下面，也就是在世界坐标系下面，这是因为记录的pose是基于世界坐标系的;(没看懂源项目的操作，所以全部都改了)
如果odom矩阵有错误，在rviz里面，持续的订阅到消息，融合显示的效果会全部糊在一起，这是因为矩阵不正确，不同帧的参照物错了;
正确的融合效果是整个场景基本能够还原出来(小细节对不上可能是时间戳没有对齐或者固有误差等)

验证相机外参数矩阵

外参数矩阵根据标定结果xyz+rpy算出来的是3X3的矩阵，要注意补齐成4x4的
这个只是换了一个坐标系而已，正确的效果的场景里面物体的相对关系不会改变

相机的内参数矩阵

相机的内参矩阵表征了从三维场景到二维像素点的映射;
首先要初步检查自己相机的照片的大小，和内参矩阵的cx,cy的关系是不是正确的，因为cx,cy一般来说代表了图像的像素中心，可能不完全是一半，但是也差不了多少，要在一个合理的范围内。
其次在相机的标定的时候，用的图片的像素和现有的图片的大小可能不对应，内参矩阵要给一个缩放因子，外参矩阵不变。

着色要确定相机的前方对应lidar的前方

一般都是像素映射后在图片范围内作为第一个筛选条件，第二个筛选条件是(点云的x或y或z,大于或小于0)可以筛选掉很多不必要的点(fastlio算法里面引入了可视半径，可以筛掉更更多的点)

消息类型转换是不是正确可以保存中间的逐帧的结果查看

逐一检查都没问题一般就可以成功了。

时间的优化(目前都是基于PC的时间讨论，上板验证还没有做)

除了初始化所需要的时间，主要检查的时间的资源消耗有两个地方1.图片格式的转换和畸变修正 2.从3D点云到2D像素映射的时候要遍历点云中的所有点，用矩阵转换

第一个优化：

cmakelist 里面可以设置opencv，Eigen库的加速，有一定的效果; 还有一些其他的设置，目前我不了解，是老师给的结果我直接用，也是有效果的，但是效果不大，主要还是对于程序的优化

第二个优化

图片转换里面remap的耗时很久;
解决的办法:换一个函数;对图像降采样; 这两种都试过，结果是不可接受的
最终只能选择remap; 640X360的图像，经过优化后，每帧的耗时大约在0.5-1ms之间浮动

第三个优化

在找需要是上色的点云的时候，筛选掉一些不必要的点
Eigen库的矩阵转换和乘法耗时很久，目前不知道原因是什么，按理来说4维的矩阵乘法应该不会耗时很久
解决办法:因为维度比较低，矩阵乘法的结果直接用double计算出来就好，不调用Eigen库
结果:从7-8ms降低到0.5ms以下