amcl与icp融合算法的探究

因为amcl定位算法精度不够高，而且必须有一定的移动距离或角度变化才能有新的位姿发布，会影响导航最终精度。 所以需要对amcl的定位位姿做一定的矫正，输出相对更准确的位姿。
自然而然想到了icp的方法，icp在匹配时，受到初始值的影响，如果直接将地图与激光帧匹配消耗时间较长，且匹配结果差。只有两片点云“足够近”后，使用icp匹配位姿才会更为准确。
下面的任务就是找到地图点云和激光点云以及初始值。
地图点云：通过map topic 传过来的占据栅格地图，提取其中值为100的点，x、y存入点云cloud_map
激光点云：根据scan topic 通过运算

cloud_in->points[i].x = cos(laser_scan->angle_min+i*laser_scan->angle_increment+tf::getYaw(p.pose.pose.orientation))*laser_scan->ranges[i]+p.pose.pose.position.x;
 cloud_in->points[i].y = sin(laser_scan->angle_min+i*laser_scan->angle_increment+tf::getYaw(p.pose.pose.orientation))*laser_scan->ranges[i]+p.pose.pose.position.y;

其中p.pose就是amcl输出的位姿。在这里已经对激光雷达点云操作了平移和旋转，也就是icp的初始值。
将点云cloud_in和cloud_map做icp，此时如果amcl估计位姿校准的话，icp后的结果也较为准确。
把icp后的旋转矩阵作用到amcl估计位姿p点上，即可求出更为精确的位姿。
我不是大神，只是分享记录一下，肯定还存在许多不足，如果有什么地方出现错误，请批评指正。