amcl+ICP融合思路

阅读论文high-accuracy vehicle localization.....

d_mi是由激光传感器获取的第i个点离激光传感器的距离。对应amcl代码的ldata.range[i][0],

a_min对应angle_min, delta=angle_increment.

公式中的a_min+delta*i其实就对应着ldata.range[i][1];

x_oi，y_oi是地图中障碍物占据的坐标，x_e,y_e和theta_e是机器人估计的位姿，公式中的d_vi和i是需要求的，d_vi代表虚拟的测量距离，i是对应虚拟测量距离对应的第i个测量距离。由d_vi和i来结合公式（3）求得虚拟的第二个点云（x_vi,y_vi）。由两个点云来当做ICP算法的的输入！

机器人估计的位子中，x_e=p.pose.pose.position.x, y_e=p.pose.pose.position.y，theta_e=p.pose.pose.orientation(这是用  四元数表示的方向).