算法-三维栅格子地图创建方法

以上一张子图完成时的最后一帧车辆位姿作为新建子图的local_pose，也作为子图坐标系的原点，并且作为新建三维栅格子图的几何中心。每一张子图需要处理的点云的位置转换到了子图坐标系下，之后先创建一个128×128×128的初始三维栅格地图，三维栅格子图的坐标系原点在左下角，所以三维栅格子图的几何中心（即子图坐标系原点）在栅格子图坐标系下的索引为（64,64,64）。如果点云的索引在（0,0,0）-（128,128,128），则不需要进行子图的扩展，如果超出这个范围，则需要进行子图的扩展。扩展的原则是：子图的几何中心保持不变，向四周扩展，且扩大后的栅格地图的每一维是原来的两倍，当然，扩展后的新的栅格子图坐标系的原点也与原来的位置不同了，变成了新的栅格子图的左下角，此时三维栅格子图的几何中心（即子图坐标系原点）尽管实际位置没变，但是在新栅格子图坐标系下的索引变为（128,128,128）。接下来，需要将扩展前的三维栅格子图的栅格的值投影到新的扩展后的栅格子图的对应栅格中，即将原来栅格索引（64,64,64）的值投影到新栅格索引（128,128,128）中，这样即保持了空间位置对应。
注：子图坐标系与三维栅格子图坐标系不一样