gmapping 激光匹配原理

对应gmapping中的scanmach的匹配是如何匹配的详细讲解

匹配用得分来衡量，得分计算：

首先，他是将这一帧激光点作用于某一粒子，粒子就是一个机器人。

首先是把该粒子上的激光雷达坐标转换到世界物理系上，然后对这帧激光的每一束遍历。

首先计算，激光束打到的点在世界系下的真实坐标phit，也转换为地图索引坐标iphit。

然后计算这个打到的点前面一定距离的点认为肯定是空闲点，这个距离参数是freeDelta，得到这个空闲点的世界坐标。计算下这两个坐标的1范数。

下面开始匹配了

由于地图索引坐标是对连续世界的离散，导致可能不同实际物理坐标变成索引后是一个栅格。

我们取一个地图索引坐标系尺度下的范围值，kernerSize. 比如这束激光打到的点转换为栅格坐标后是格子1，我们认为这个1周围的绿色格子也可能是占用的（kernerSize=1时，2就是周围两个格子可能也打到了），

我们分别考察这几个绿色格子，考察什么呢，我们认为某个栅格，其被击中了，但不是说就是同一个物理击中点，因为栅格地图是一个离散的过程，如下图

所以对于某一个栅格的具体坐标，用击中这个栅格时的真实物理坐标的累计值求平均得到。

代码中是cell.mean(). 那么现在这束激光来了，打到了物体了，激光点实际世界物理坐标是phit， 但是我不能用这一个点去匹配，因为上面说了，同一个栅格可能是打到的点，那么同样的道理，我这次打到了坐标phit，下次另一个接近phit的坐标，他们可能是同一个栅格，所以我们考察一个窗口kernerSize内就是上面的绿色+黄色这个窗口内的情况。

比如我们取一个格子3，我可以得到之前击中这个栅格的所有点世界物理坐标的平均值，用这次击中的世界物理坐标减去这个平均值，我们遍历格子1-9, 都这么算，取相减最小的。然后计算得分了，得分用高斯密度函数得到，高斯密度函数是

要比较得分，代码用的是exp部分，x-u 就是上面的，x是本次激光帧某一束激光的世界坐标， u是本次打中点坐标栅格被打中的坐标所累计值的平均值，方差sigma由用户参数传入或使用默认值，得到这束激光的得分temp_score,

这个粒子在本帧激光的得分是累加每束激光的temp_score.

这个就是激光帧与栅格地图的匹配方法。

很多个粒子，但是激光帧是一样的，想当于把真实的机器人位姿处的激光帧放到每个粒子处，显然，粒子距离真实位置越近，激光中的距离值就跟符合真实位置测到的，粒子也就越集中在真实位置，越匹配目前的地图。

其包括机器人真实位置和每个粒子的位置，相同的是都要进行里程计的运动更新，但是激光测量数据是真实的，即一帧数据过来了，这一帧是真的机器人位姿下得到的测量结果，然后把测量结果作用在每一个位置上的粒子，经过坐标转换到实际的世界坐标系下，跟地图匹配的效果肯定是不一样的，显然是越接近实际位置的，匹配效果越好了。