目录
1. 为 NDT 设计一个匹配度评估指标,利用该指标可以判断 NDT 匹配的好坏。
2. 利用第 1 题的指标,修改程序,实现 mapping 部分的回环检测。
3. 将建图结果导出为 NDT map,即将 NDT 体素内的均值和协方差都存储成文件。
4. 实现基于 NDT map 的激光定位。根据车辆实时位姿,加载所需的 NDT 体素并完成定位。
5. 给出上述结果相比于 PCL NDT 的性能、存储空间等关键指标
1. 为 NDT 设计一个匹配度评估指标,利用该指标可以判断 NDT 匹配的好坏。
2. 利用第 1 题的指标,修改程序,实现 mapping 部分的回环检测。
下图是
PCL
版本
NDT
匹配,在阈值设置(
ndt_score_th
)为
4.5
时的匹配结果(
其中加载
的关键帧数目我删掉了一些,因为实在是耗时有点久。。。)
![](https://i-blog.csdnimg.cn/blog_migrate/ec1d96d7bfc8507a7d82573775e0b2fa.png)
根据上面
PCL
版本的
NDT
检测结果来确定适配自定义指标的阈值(
ndt_score_th
)。
![](https://i-blog.csdnimg.cn/blog_migrate/14e947fd0c80e249a16e2d8d59c3b5f7.png)
使用原来的阈值明显不对,观察,得分在
0.2
附近,于是试着在这个范围寻找合适的阈值。
![](https://i-blog.csdnimg.cn/blog_migrate/4fe1f5ecf785e93c184a0144af3db0e0.png)
可见,阈值设置为
0.15
和
pcl
版本的阈值为
4.5
时的结果差不多。
3. 将建图结果导出为 NDT map,即将 NDT 体素内的均值和协方差都存储成文件。
这里参考
split_map.cc
代码的内容:加载关键帧对应的点云,对其进行滤波,然后计算点
云中的每个点对应的地图区块
id
,最后将区块索引和对应点云分别存储起来。
我们要导出
NDT map
,需要构建
NDT
体素并计算均值和协方差,这些在第七章的
ndt_3d.h
的
SetTarge
函数中已经实现。
![](https://i-blog.csdnimg.cn/blog_migrate/daa2aedc1d5c766cb134bfd3177285cc.png)
![](https://i-blog.csdnimg.cn/blog_migrate/68bee12bec4ec48a6efbb2404ee8bd26.png)
![](https://i-blog.csdnimg.cn/blog_migrate/8b7a0a003823a0ad4afbae66c675a6bd.png)
保存的结果:
![](https://i-blog.csdnimg.cn/blog_migrate/ec01ea97915cdd4ad6dc70866d4a98fd.png)
4. 实现基于 NDT map 的激光定位。根据车辆实时位姿,加载所需的 NDT 体素并完成定位。
原先基于点云地图的激光定位使用的是
PCL
版本
NDT
,
在
RTK
角度搜索中用到了
10
米,
5
米,
4
米,
2
米的多分辨率
NDT
匹配来确定
RTK
的朝向,所以实现基于
NDT map
的激光定位,也需要加载多分辨率
的体素,所以参照第
3
题重新修改为保存多分辨率的
NDT map
。代码如下:
![](https://i-blog.csdnimg.cn/blog_migrate/fbf55c0ef2fa3486ad74ed2c538c40cf.png)
![](https://i-blog.csdnimg.cn/blog_migrate/165a83bd021681d5c11c6d7aada2ffb3.png)
存储
NDT
体素中的均值和协方差矩阵信息。
![](https://i-blog.csdnimg.cn/blog_migrate/485446993991e1e6997235576ef8565d.png)
①首先,在
fusion
初始化时配置要加载的
NDT map
路径,并参考原来的
loadMapIdex()
函
数,将多分辨率的
NDT
地图数据加载进来。
![](https://i-blog.csdnimg.cn/blog_migrate/71c66dc5ba274d7e063e99cc74fc1e65.png)
![](https://i-blog.csdnimg.cn/blog_migrate/057cdcdb5de98b07b32fb931c87e10d5.png)
②接下来就是定位流程
ProcessMeasurements(m)
。首先要确定
RTK
朝向,加载多分辨率
NDT map
,使用多分辨率来进行寻找。
![](https://i-blog.csdnimg.cn/blog_migrate/6fe06e298097efe3053e5228f5ab6d57.png)
![](https://i-blog.csdnimg.cn/blog_migrate/fbb0c6364d36d7f5b9a297fd03f3a08e.png)
![](https://i-blog.csdnimg.cn/blog_migrate/a9d6b12ef27f3aadfc9a46e82bc78175.png)
当然,同样也要准备卸载超出范围的
NDT
地图。
![](https://i-blog.csdnimg.cn/blog_migrate/53340102f6b3bf3cf5ebc1ce34876776.png)
以上过程包含在
loadNdtMap
函数中。
![](https://i-blog.csdnimg.cn/blog_migrate/b69a1de8a5de24bc8de5468c4a012b86.png)
RTK
初始化成功后后续定位流程在
LidarLocalization()
中进行。
![](https://i-blog.csdnimg.cn/blog_migrate/ad2c6cf736b204148919bcf09b729199.png)
至此,实现了基于
NDT map
进行激光定位的功能。
③实现效果:
![](https://i-blog.csdnimg.cn/blog_migrate/bf529448866ab9ef8e833faf64e1482c.png)
![](https://i-blog.csdnimg.cn/blog_migrate/899de84a26c76e1fd925ce7cf25a604a.png)
两者定位效果差不多。
5. 给出上述结果相比于 PCL NDT 的性能、存储空间等关键指标。
①统计比较两者在加载地图和配准过程两方面的耗时情况。
![](https://i-blog.csdnimg.cn/blog_migrate/9603bab92756e355f0e076cb7b7677a2.png)
![](https://i-blog.csdnimg.cn/blog_migrate/91973f9b8e46f05893624dfc232312f8.png)
可以发现,基于
NDT map
的方法,加载地图数据耗时要比加载点云要慢很多;而配准方面
会快很多。
比较一下跑完一个
bag
,总的耗时情况。
![](https://i-blog.csdnimg.cn/blog_migrate/25e0d383e6624419a656cdf607f62fed.png)
![](https://i-blog.csdnimg.cn/blog_migrate/777e78b5e6f0ba5f086d41f071349fbc.png)
经过对比发现,基于
NDT map
的激光雷达定位效率是基于
PCL NDT
的接近六倍。
②统计比较两者需要加载的地图数据占存储大小。
原先加载的是以
100x100
大小地图区块索引命名的
pcd
格式点云数据,而现在需要加载
4
种不同分辨率的
NDT
体素地图数据中,仅存储了均值和信息矩阵。因此存储空间应该要小得多,果然,对比如下:
![](https://i-blog.csdnimg.cn/blog_migrate/d9d00be979ba7cf26318d7af26d20b3b.png)