一起做激光反光板（四）-框架搭建

我们已经推导了基本的反光板建图和定位的基本公式，接下来，开始搭建一个框架，尝试进行反光板的建图定位的代码加进去。

首先，搭建一个ROS框架，在我的个人github的reflector_ekf_slam上上传一个基于激光反光板的EKF建图定位方案。

基本框架：

ROS接收topic：

scan: sensor_msgs::LaserScan
odom: nav_msgs::Odometry

需要注意的是：

（1）轮速计数据必须有线速度和角速度，轮速计的方差写死了：

Qu_ << 0.02 * 0.02, 0.f, 0.f, 0.034 * 0.034;

（2）只接受单线激光雷达的数据。

发出的topic：

ekf_slam/landmark：visualization_msgs::MarkerArray
ekf_slam/pose： geometry_msgs::PoseWithCovarianceStamped
ekf_slam/path： nav_msgs::Path

发布的消息：

（1）反光板地图

（2）实时位姿

（3）历史轨迹

反光板检测与匹配

（1）反光板检测：

其实代码中所写的检测方案是最容易想到的，但是在实际使用的时候，在工程上无法满足需求。有提升的方法，但是在博客中不太好介绍，故当前的检测要求是环境中布置的都是30cm长的反光板，且只能支持这一种反光板。检测的要求是长度不超过3cm的偏差。

另外，代码中没有进行运动畸变校正操作，这是非常危险的，因为在高速运动的过程中，运动畸变会很大，容易造成检测失败和关联错误。

（2）反光板匹配

反光板的匹配是通过马氏距离计算的，马氏距离的阈值设置为0.05。

（3）外参转换问题

外参在代码中写死了：

lidar_to_base_link_ << 0.41589, 0.25639, 0.0;

需要标定的呀。之所以写在类内，是因为之后打算做在线外参估计。当然，这个程序也可以作为单线激光雷达外参标定程序来用。

（4）观测残差

在类内写死了：

Qt_ << 0.03 * 0.03, 0.f, 0.f, 0.03 * 0.03; 

建图和定位EKF

建图和定位用的是一个框架，具体实现需要看代码。

地图的保存和读取

通过txt保存，后续可以改进为xml、json、proto等。保存的内容：

反光板的坐标$(x,y)$和协方差$\Sigma$

问题

（1）反光板检测和匹配（不再优化）

（2）激光帧未进行运动畸变校正

（3）激光帧的信息只用了反射强度，点云没用，有点可惜

（4）激光雷达的外参需要在线估计

后续待更新部分

（1）激光帧未进行运动畸变校正

（2）激光雷达的外参在线估计

（3）基于滑窗的EKF公式推导

（4）扩充观测：激光线特征、直角特征

（5）扩充观测：帧间ICP

（6）FEJ

（7）利用EKF位姿构建栅格地图