Autoware 1.12学习整理--03--NDT点云定位

〇、前言

在得到点云地图后，接下来为点云定位，是汽车导航的前提，autoware中使用ndt_matching模块完成基于点云的定位，是一种scan-to-map的点云配准算法，本文将使用《Autoware 1.12学习整理–02–构建点云地图》中的点云地图和rosbag数据包完成ndt点云定位。

一、ndt点云定位

ndt点云定位时的tf树与建图时的稍有不同，从map到base_link的坐标系转换由autoware中的ndt_matching模块完成的，如下图所示：

1.1、加载bag文件
启动autoware，切换到Simulation页面，点击Ref按钮加载bag文件，Start Time设置为0，点击Play然后点击Pause暂停播放，如下图所示。为了保证autoware的正常运行，在启动autoware后首先要做的就是加载bag文件，否则会出现时间戳同步问题。

1.2、完成从base_link到velodyne的坐标系转换
切换到autoware的Setup页面下，确定Localizer选的是Velodyne，设置Baselink to Localizer的x、y、z、yaw、pitch、roll数值，其为雷达中心点与车身后轴中心点的相对位置关系，接着点击TF按钮，最后点击Vehicle Model按钮，如果其后为空，那么会加载一个默认模型。

1.3、完成从world到map的坐标系转换
切换到autoware的Map页面下，点击TF右侧的Ref按钮，加载如下路径的launch文件，其中是从/world坐标系转换到/map坐标系的tf变换，最后点击TF按钮。

autoware.ai/src/autoware/documentation/autoware_quickstart_examples/launch/tf_local.launch

1.4、加载点云地图
在Map页面下，点击Point Cloud右侧的Ref按钮，加载pcd点云地图，并点击Point Cloud按钮，进度条显示OK，则加载完毕，如下所示：

1.5、点云降采样
voxel_grid_filter，也就是体素滤波，PCL实现的体素滤波VoxelGrid类通过输入的点云数据创建三维体素栅格（可以将体素栅格想象为微小的空间三维立方体的集合），然后在每个体素内，用体素中的所有点的重心近似显示体素中的其他点，这样该体素内所有点就用一个重心点最终表示，然后将所有体素处理后得到过滤后的点云。（这种方法比用体素中心来逼近的方法更慢，但对于采样点对应曲面的表示更为准确。）
切换到autoware的Sensing页面下，点击Points Downsampler下voxel_grid_filter右侧的app按钮，设置点云话题名Points topic为/points_raw，下面两个参数默认即可，Voxel Leaf Size参数值为2，表示2米的立方体内的全部点近似用1个重心代替，Measurement Range的参数值为200，表示点云的有效的距离是200米。设置完参数后点击OK，最后勾选voxel_grid_filter。


1.6、完成map到base_link的坐标系转换
切换到autoware的Computing页面下，点击lidar_localizer下ndt_matching右侧的app按钮，在弹出的页面中topic:/config/ndt选择Initial Pos选项，x，y，z，roll，pitch，yaw的值表示激光数据的初始位姿，如果有GNSS设备可以选择GNSS进行初始定位。Method Type选择pcl_anh_gpu，即使用GPU进行点云匹配计算，然后点击OK，最后勾选ndt_matching。


1.7、播放bag数据
点击autoware界面的RViz按钮，打开rviz，依次点击file->open config加载配置文件：

autoware.ai/src/autoware/documentation/autoware_quickstart_examples/launch/rosbag_demo/default.rviz

切回到autoware的Simulation界面，点击Pause按钮开始播放数据，Rviz界面显示如下：

整个运行过程如下：

在开始时，没有定位成功，汽车前进了一段距离后突然回到起始位置，此时才定位成功，后面汽车就能一直将点云与地图匹配起来，完成汽车在点云地图中的定位。开始时如果长时间没有定位成功，可以使用rviz中的2D Pose Estimate工具指定汽车的初始位姿。利用ndt_matching进行点云定位时的节点图如下：

最后可以看一下voxel_grid_filter降采样前后的对比：

文章不妥之处还望指正

参考文章：
https://www.cnblogs.com/hgl0417/p/11143107.html
https://zhuanlan.zhihu.com/p/64226544