hector 源码分析

参考论文《A Flexible and Scalable SLAM System with Full 3D Motion Estimation》

在https://github.com/tu-darmstadt-ros-pkg/hector_slam 上下载hector 源码，用understand 打开。

总的来说，hector的实现过程要比gmapping简单些，但由于博主是c++ 小白一个，门都没入，看着各种函数、运算符的重载，以及一些标准库的应用，简直想死。

main()函数在hector_mapping文件夹下。
转到 HecMappingRos.cpp 文件下，
进入HectorMappingRos 类的构造函数。挑出几个重要的函数（其实就一个是最重要的，其它的都是，都是一些与初始化相关的，以及调试相关的）。

  scanSubscriber_ = node_.subscribe(p_scan_topic_, p_scan_subscriber_queue_size_, &HectorMappingRos::scanCallback, this);

查看scanCallback（）这个函数。

void HectorMappingRos::scanCallback(const sensor_msgs::LaserScan& scan)
{
/*这个函数太长了，全部粘贴出来看的就吧清晰了，挑重点*/
...
...
...
/*
重点在这里 ，update()函数，第一个参数，激光数据的封装包，第二个参数，上一次机器人在世界坐标系下的位姿
*/
          slamProcessor->update(laserScanContainer,slamProcessor->getLastScanMatchPose());
...
...
...
}

查看 update()函数

  void update(const DataContainer& dataContainer, const Eigen::Vector3f& poseHintWorld, bool map_without_matching = false)
  {
   
    Eigen::Vector3f newPoseEstimateWorld;
/*重点来了，

扫描匹配
====

的核心代码matchDate（）函数
arg1 : t-1时刻机器人在世界坐标系下的位姿
arg2 : 激光数据
arg3 : t-1时刻扫描匹配的协方差
*/
    if (!map_without_matching){
        newPoseEstimateWorld = (mapRep->matchData(poseHintWorld, dataContainer, lastScanMatchCov));
    }else{
        newPoseEstimateWorld = poseHintWorld;
    }

    lastScanMatchPose = newPoseEstimateWorld;

    /*判断位移增量与角度增量是否符合要求*/
    if(util::poseDifferenceLargerThan(newPoseEstimateWorld, lastMapUpdatePose, paramMinDistanceDiffForMapUpdate, paramMinAngleDiffForMapUpdate) || map_without_matching){
   

/*重点，

地图更新
====

  updateByScan
  arg1 : 激光数据
  arg2 : 机器人在世界坐标系下的位姿
*/
      mapRep->updateByScan(dataContainer, newPoseEstimateWorld);

      mapRep->onMapUpdated();
      lastMapUpdatePose = newPoseEstimateWorld;
    }
  }

开始寻找mapRep->matchDate（）函数，由于mapRep声明的是 MapRepresentationInterface* 类型，实例化为MapRepMultiMap类型，所以转到 MapRepMultiMap.h 下

  virtual Eigen::Vector3f matchData(const Eigen::Vector3f& beginEstimateWorld, 
                                    const DataContainer& dataContainer, 
                                    Eigen::Matrix3f& covMatrix)
  {
    size_t size = mapContainer.size();

    Eigen::Vector3f tmp(beginEstimateWorld);

    /*默认三层地图，分辨率0.025m 0.05m 0.1m 从0.1m层开始处理
    论文中说，使用分层地图计算是为了避免陷入局部最小值，且其地图的分层，并不是通过高分辨率的地图降采样得到，
    而是使用不同的地图存储器，每种分辨率的地图单独极端，从低分辨率的地图开始进行扫描匹配，然后将方程的解迭代进入高精度的地图再解算，多次迭代后，得到当前时刻，机器人的位姿（更精确）。另一个优点是：使用多分辨率的地图，可是导航，路径规划更高效。
    */
    for (int index = size - 1; index >= 0; --index){
      //std::cout << " m " << i;
      if (index == 0){
        tmp  = (mapContainer[index].matchData(tmp, dataContainer, covMatrix, 5));
      }else{
      /*pow(x,y)为求x的y次幂*/
        dataContainers[index-1].setFrom(dataContainer, static_cast<float>(1.0 / pow(2.0, static_cast<double>(index))));
        tmp  = (mapContainer[index].matchData(tmp, dataContainers[index-1], covMatrix, 3));
      }
    }
    return tmp;
  }

转到 MapProcContainer.h 查看 matchDate()