自己动手写一套2D的SLAM算法（2)

上一节提到根据 OPENCV mat的图像像素值来判断雷达数据是否和图像的像素值一样进行匹配，试验可知，像素匹配方法精确度不是很高，并且还需要从Mat 中读取像素值，在树莓派上运行时速度受限。所以考虑直接从数组读取，并优化匹配方案。

lid@lid-VirtualBox:~/share/gpsmap/openCRobotics/gridmappingMatchSearch_xy$ ./gridmapping 
begin 42308 ms
end 42346 ms
1 42347 ms
point clod 353 
shift complete best score:-30 degree,-7.819 (-2,0) -997.109192
.point clod 353 
shift complete best score:-25 degree,-7.792 (-2,0) -970.931641
.point clod 353 
shift complete best score:-20 degree,-8.151 (0,0) -1027.207031
.point clod 353 
shift complete best score:-15 degree,-7.923 (0,0) -1037.255615
.point clod 353 
shift complete best score:-10 degree,-7.666 (-2,0) -1064.507202
.point clod 353 
shift complete best score:-5 degree,-8.244 (0,0) -1088.777954
.point clod 353 
shift complete best score:0 degree,-45.818 (0,0) -1030.964966
.point clod 353 
shift complete best score:5 degree,-36.317 (0,0) -1023.889343
.point clod 353 
shift complete best score:10 degree,-34.962 (0,0) -1030.258667
.point clod 353 
shift complete best score:15 degree,-32.055 (0,0) -1456.318359
.point clod 353 
shift complete best score:20 degree,-32.287 (0,0) -1557.056030
.point clod 353 
shift complete best score:25 degree,-31.581 (0,0) -1619.559692
.point clod 353 
shift complete best score:30 degree,-30.413 (0,0) -1182.527466
.2 42351 ms
Map search complete, correction a=0.0deg, x=0mm, y=0mm, score=-45.818413

经过测试两帧同样的雷达数据，一帧作为地图保存，另一帧作为测试数据和地图进行匹配，匹配方法从-30度到30度每5度一步，同时x\y平移10个像素，每步平移2个像素。

最后得到 在0度 平移x=0 和平移y=0时得分最好。

测试成功。