在ros上使用mrpt_icp_slam_2d建立地图

    该SLAM算法通过使用ICP算法将新范围扫描与点点或占用网格图对齐来逐步建立地图。

    icp算法 根据点集P1中的点坐标，在曲面S上搜索相应最近点点集P2; 计算两个点集的重心位置坐标，并进行点集中心化生成新的两个点集； 由新的两个点集计算矩阵W=∑ni=1qiq′Ti,并对这个矩阵进行分解为W=U∑VT,R=U∗VT. 得到旋转矩阵和平移矩阵Rt，就可以计算点集P2进行刚体变换之后的新点集P3，由计算P2到P3的距离平方和，以连续两次距离平方和之差绝对值，作为是否收敛的依据。若小于阈值，就收敛，停止迭代。重复计算，直到收敛或达到既定的迭代次数。   

    我最近手里面只有一台激光雷达,于是就尝试了使用mrpt_icp_slam_2d在ros上进行建图.

    首先需要克隆pkg到自己的工作空间, git clone https://github.com/mrpt-ros-pkg/mrpt_slam.git

    这个包下面有非常多的使用mrpt库的slam方法,今天主要介绍graph slam,其他的使用方法请看我其他博客.

    接下来就是编译了,我习惯用catkin_make --only-pkg-with-deps这个命令编译,方便定位package可能出现的error. 

    它的移植是非常简单的,只需要修改frame id和sensor source,然后删除有关bag的语句(我们不跑数据集)就可以了.

   

    但是这个包有个问题是,在rviz下LaserScan常常会读取不到数据,这一点很不稳定. 我仔细看了一下,最后通过tf_monitor定位到问题在于map的频率非常之高,会导致tf变换的时候出现问题. 但这不影响建图.充其量只是在rviz上看不到激光信息而已.

    我对比了一下其他的slam,发觉只有icp_slam的频率如此之高,也就不去管了.

   

 

     可以从上图看出,其实效果也不差,几乎是没有散射的.这个slam方式是基于大量点坐标匹配渲染而成的,也就是说,建立地图时需要在空间当中来回走,才能够给予它足够数量的有效点,我走的回合不多,就可以看到办公室里一些地方都有丢失,没有去做匹配.