cartographer_learn_21分支定界算法

续接前篇

前一篇我们讨论到了计算论文中的“Precomputed grids”，猛一看就容易产生疑问，计算这个东西有什么用？它和分支定界有啥关系？本篇就尝试解释一个这个问题，同时也介绍一下分支定界算法的过程。本篇不会涉及代码，所以可能会比较短小，但是作者认为有必要让它独立成篇，因为只有明白了这个后才能后好的阅读后续的代码。同时这里也希望读者对照这论文《Real-Time Loop Closure in 2D LIDAR SLAM》一起观看。

分支定界算法

先来看看分支定界算法，这里先明确分支是什么。这里所谓的分支是对x，y方向上的分支，即对于角度θ上的窗口还是需要一个个的去检索。所以我们不妨先假设我们已经找到了最佳的角度${\theta }_b$，接下来我们的任务就是在 $（x_0\pm ku,y_0\pm ku）$的范围内找到最佳的$（x，y）$，其中k是指定的参数用以表示搜索窗口的大小（一般为8），u是搜索步长（一般为栅格地图上栅格的尺寸）。
首先来看看 $（x_0\pm ku,y_0\pm ku）$这个范围，它是一个正方形，如果一个个解的去遍历的话算法的复杂度将是$o(k^2)$，时间复杂度不小。Cartographer采用的是先把这个正方形平均分成四个区域，即左上角，右上角，左下角和右下角四个小正方形区域，如下图：

我们暂且称大正方形为大正方形，四个小正方形都是小正方形（废话。。。）。分成四份后要判断要找的最优解究竟在四份中的哪一份中，即这四个区域的得分谁最高（得分后面再解释）。然后再把得分最高的小正方性区域当成大正方形，重复以上的操作，直到找到某个小正方形中只有一个解，那么这个解就是最优解。这样子操作，整个算法的复杂度为$o(logk)$，比前一种逐个遍历的做法复杂度低不少。
接下来就得说明怎么计算某个正方形区域内的得分了。再讨论得分的就算前要先明白得分是什么？先想象一下，你有一张二维的栅格地图和某个时刻（理想的）激光的扫描结果即点云，那你肯定想找到一个$(x,y)$使得点云中的每个点都对应着栅格地图中栅格值较高的栅格。这里便有一个想法，其实每个点都可以对应一个栅格。
再想象一个理想的情况（不可发生的）此时的激光扫描得到的点云中只有一个点，那这种情况下得分是什么呢？得分就是在这个小正方形区域内找到一个$(x,y)$使得这个点经过这个转换后，其对应的栅格值最高，这个最大的栅格值就是得分。
回到正常的情况，我们的点云中有很多个点$p_0,p_1,......p_n$，那么此时的得分是什么呢？是这样处理的，针对$p_0$在小正方形的区域内找到一个$(x_0,y_0)$使得$p_0$转换后，其对应的栅格值最高，计该栅格值为$v_0$。针对$p_1$在小正方形的区域内找到一个$(x_1,y_1)$（注意这个转换和前一个转换不一定一样）使得$p_1$转换后，其对应的栅格值最高，计该栅格值为$v_1$。其余的点也类似。最终得分$v=(v_0+v_1+......+v_n)/n$。

得分的计算及Precomputed grids和得分计算的关系

回到那种理想的情况下，即此时激光扫描得到的点云中只有一个点。我们考察某个小正方形，即左上角橙色的小正方形：

假设这个小正方形的区域内包含4*4=16个独立的解，且其位于最左上角上独立的解（即蓝色正方形）对应的转换为$(x_l,y_l)$。设点云中的那个点在经过$(x_l,y_l)$这个转换的变换后，其对应的栅格为$(X_l,Y_l)$（注意小写的x，y是表示空间坐标系的坐标，大写X，Y表示栅格地图上的栅格坐标）。此时我们就可以换一个角度来看看得分意义了，这一个点的得分就是在栅格地图中$(X_l-X_l+4,Y_l-Y_l+4)$这个范围内栅格值最高的栅格的值。于是我们发现在某个小正方形区域内的某个点得分就是在求栅格地图中某个区域内拥有最高栅格值的栅格的值。在上诉情况中这个区域是一个4 * 4的区域。
有了这一理解后我们回到真实的情况，我们一次扫描得到的点云会有很多个点，且我们经常要一个栅格地图和多次扫描做匹配。所以能不能对栅格地图做一些处理，使得每次一个扫描到来后我们能很方便的计算某个点的得分呢？能，即计算Precomputed grids。上一篇已经说明了计算Precomputed grids对应的意义是什么了，读者回头对比一下便能理解了。