三维栅格地图构建之三：点集到栅格的投射

为什么要有点集到证据栅格的投射过程？

答：点集到证据栅格的投射时将空间的点放到栅格中的过程，其后会涉及到概率的计算和叠加。栅格是一种概率（或权重）的表示方式，在理论上有严谨的逻辑（越是模糊的东西在理论上就越严谨）；有助于地图的拼接。

证据栅格（evidence grid）或占据栅格（occupancy grid）是由前期的概率栅格发展而来。概率到栅格的演变是有对数公式：log（p/(1-p)）实现的。

为什么要演变到证据栅格？

答：为了地图更新的方便。原来的概率栅格，即使是很小的概率经叠加后也可以变大，但证据栅格的范围是（-1,1），叠加的效果是小的越小，大的越大。

我的实验

（先囧下：Moravec论文中的方法我没看懂，Franz Andert论文中的方法我亦没看懂！于是乎自己胡编乱造了一种方法。若有大牛光顾小弟博客还望多指点，告知我真相！）

我的假设：我觉得摄像头测距模型并不符合高斯分布或正太分布什么的。因为视觉测距完全受配准点的影响，若匹配的好，则测距误差很小（摄像头测距误差在2mm内，这是10多年前的经验，现在不知是多少了），但若匹配的不好，则测得的距离偏离真实值很大。所以干脆认为有双目系统得到的距离数据是正确的概率为100%。这是一种非常理想的模型。

有了上面的假设，接下来的事就简单了，先对空间点作正尺度变换，都变为正坐标，再对栅格尺寸取余直接放到多维数组中就OK了。空间点的位置设置比较大的概率，空间点到双目系统间的栅格全部设置比较小的概率，其它的栅格不去管（初始化时设置）。

空栅格和占据栅格投射代码如下：

for i=1:size(PointMsr,1)
   x=PointMsr(i,1);
 y=PointMsr(i,2);
z=PointMsr(i,3);
 ordx=PointMsr(i,1);
 ordy=PointMsr(i,2);
 ordz=PointMsr(i,3);
 l=sqrt(ordx^2+ordy^2+ordz^2);
  angx=acos(abs(ordx)/sqrt(ordx^2+ordy^2+ordz^2));
  angy=acos(abs(ordy)/sqrt(ordx^2+ordy^2+ordz^2));
  angz=acos(abs(ordz)/sqrt(ordx^2+ordy^2+ordz^2));
  dl=cellsize;
while 1
      l=l-dl;
      if l

得到的实验结果如下：

上图中，白色部分是代表的双目系统与空间点之间的空白区域，黑色点代表空间点，灰色部分代表未知区域。当然这都是以点表示的，每个点代表栅格的中心点。