无人驾驶定位地图调度

      我们知道，目前对于无人驾驶的定位而言，还需要将在线的地图组织成一定的形式，然后和离线地图进行匹配，确定相对于当前初始位置的更加精确的相对位置。拿3D激光作为传感器建图来说，一种方式是用2D - 2D的匹配。

• 2D - 2D的匹配

可以将slam后的激光点云处理成二维的图片格式，图片像素值大小对应当前grid内的强度值。然后将在线的3D点云处理成图片格式，可能会遇到在线点云过于稀疏导致在线生成的2D图片数据过于稀疏，这时可以将连续几帧的点云合并，一起来生成当前帧的2D图片。然后用当前帧的pose调用离线的2D地图，将在线地图和离线地图匹配。可以采用SSD或者NCC的方法，求出在线相对于离线的相对位置，从而给出由激光点云匹配得出的一个定位量，将来用于参与计算多传感器的融合定位。

• 3D - 3D的匹配

这里以NDT匹配为例。首先还是slam。这里的地图可以有两种形式，一种是直接保存离线相对应位置的3D点云，然后利用在线的3D点云和离线的3D点云进行对齐，可以直接利用PCL的库函数 pcl::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ>  进行对齐操作。

还有一种方式是，提前计算好离线点云的NDT信息，将其按照三维voxel存储起来作为离线地图。然后在线做NDT匹配时候，只需要计算在线的NDT就行了，这样可以加快计算。当然，我们得实现自己的NDT匹配计算了。

 

      不管怎么说，随着场景的增大，我们不可能一次性把几百公里的地图都加载到内存中。而更可行的做法是将地图根据实际物理尺寸分割成块，例如我在做定位地图时将地图分成200 m * 200m 一块（暂时不考虑立交桥），然后用经纬度作为每块地图的绝对索引。在程序运行时，车跑到哪个位置，就加载那个位置附近的地图。但是这就有一个问题，由于地图都是二进制文件存放在硬盘里，一次性加载多块地图会很耗时，而对于定位这种实时运算来说，不能实时及时做的再精确也没有任何用处。那么我们就想到一个办法就是，用另一个或者多个线程进行地图的预加载，加载之后可能会用到的地图。还有一个问题就是，加载地图的选定。下面就来说地图的二级调度。

                       

图中每个方块代表一块地图。其中预加载地图的个数，应该根据最大车速调整。

加载地图分为两步，

1. 第一步为加载当前pose所在的地图和其周围的地图到cache1，这个俗称九宫格地图;
2. 第二步为预加载下一次可能会用到的地图到cache2。

这里的两个cache是典型的 LRU(Least recently used,最近最少使用)类型的。如果查找当前地图块在cache内，则应该把该快提到cache的最前段。如果cache大于一定的size，则自动删除最后的不经常使用的地图块。

下面给出流程图   

                                                                   

上图为实时加载离线地图并与在线地图匹配，输出相对位姿的流程图。                                                                               

 

                                      

上面这张图给出的就是二级cache调度的逻辑关系。多线程从硬盘加载，可以提前设定一个线程池。