hector slam源码流程思路及相关参考整理

前言（原理简述）

在学习cartographer之前，先用这个项目练练手，因为cartographer相当于hector slam+karto slam结合的升级版。
hector slam论文题目：《A Flexible and Scalable SLAM System with Full 3D Motion Estimation》
hector的论文和其他博客中，都有详细讲解原理，这里我说一下我看源码的目的：cartographer中的前端匹配使用了双三次线性插值＋ceres库求解非线性优化问题(构造最小二乘，优化匹配)，而hector slam中使用了双线性插值＋高斯牛顿求解非线性优化问题，所以，先学习一下hector的思想，对cartographer是有帮助的，而且cartographer源码直接阅读难度有点大，所以暂且先学习这个。

代码梳理（不包含栅格地图构建部分）

安装源码之后，会看到有好几个文件夹：

程序的入口main函数在“hector_mapping文件夹的src文件下，找到main.cpp文件”。源码如下所示：

#include <ros/ros.h>

#include "HectorMappingRos.h"

int main(int argc, char** argv)
{
    //解析ros的初始化参数
  ros::init(argc, argv, "hector_slam");

  //创建HectorMappingRos类的对象，此处会调用该类的构造函数，因此我们转到该类的构造函数进行查看。
  HectorMappingRos sm;

  //ROS消息回调处理函数,主程序到这儿就不往下执行了,后面的return没有作用。一直调用前面的消息接收信息。
  ros::spin();

  return(0);
}

注意：构建类的对象的时候，会调用该类的构造函数，因此这是找到内部源码的关键（原谅我这么啰嗦，我只不过说出了我的顿悟啊）

接下来我们跳转到构造函数进行查看：

  //p_scan_subscriber_queue_size_默认为５,进入此scanCallback函数
  //告诉 master我们要订阅 p_scan_topic_ topic上的消息。当有消息到达topic时，ROS就会调用HectorMappingRos::scanCallback()函数。
  // 第二个参数是队列大小，以防我们处理消息的速度不够快，在缓存了5个消息后，再有新的消息到来就将开始丢弃先前接收的消息。
  // NodeHandle::subscribe()返回ros::Subscriber对象,你必须让它处于活动状态直到你不再想订阅该消息。
  // 当这个对象销毁时，它将自动退订上的消息。
  scanSubscriber_ = node_.subscribe(p_scan_topic_, p_scan_subscriber_queue_size_, &HectorMappingRos::scanCallback, this);
  sysMsgSubscriber_ = node_.subscribe(p_sys_msg_topic_, 2, &HectorMappingRos::sysMsgCallback, this);

  告诉节点管理器master我们将要在p_pose_update_topic_ topic上发布一个geometry_msgs::PoseWithCovarianceStamped的消息。
  /// 这样master就会告诉所有订阅了p_pose_update_topic_ topic的节点，将要有数据发布。第二个参数是发布序列的大小。
  /// 在这样的情况下，如果我们发布的消息太快，缓冲区中的消息在大于1个的时候就会开始丢弃先前发布的消息。
  /// NodeHandle::advertise() 返回一个 ros::Publisher对象,它有两个作用:
  /// 1) 它有一个publish()成员函数可以让你在topic上发布消息； 2) 如果消息类型不对,它会拒绝发布。
  poseUpdatePublisher_ = node_.advertise<geometry_msgs::PoseWithCovarianceStamped>(p_pose_update_topic_, 1, false);
  posePublisher_ = node_.advertise<geometry_msgs::PoseStamped>("slam_out_pose", 1, false);

构造函数的前面都是参数设置之类的，我们找到这个ros的订阅函数，通过注释我们大概理解一下，这个订阅函数，会一直回调一个函数scanCallback()函数进行话题的订阅，这个函数就是我们接下来的目标啊！点进去！

void HectorMappingRos::scanCallback(const sensor_msgs::LaserScan& scan)
{
  if (hectorDrawings)
  {
    hectorDrawings->setTime(scan.header.stamp);
  }

  ros::WallTime startTime = ros::WallTime::now();

  //如果p_use_tf_scan_transformation_为false，则进入循环
  if (!p_use_tf_scan_transformation_)
  {
      //此函数的作用为将激光点的距离数据转化为激光坐标系下的x,y坐标(应该是已经按照比例转换到地图的坐标中)存放于dataContainer类中。
    if (rosLaserScanToDataContainer(scan, laserScanContainer,slamProcessor->getScaleToMap()))
    {
        //updata()函数传入值为：dataContainer类、上一帧的位姿（x,y,theta）
      slamProcessor->update(laserScanContainer,slamProcessor->getLastScanMatchPose());
    }
  }
  else
  {
      //等待0.5秒
    ros::Duration dur (0.5);

因为我的目标是只看匹配优化部分，所以这里仅截取函数的前半部分源码。其中注释部分是我自己的理解，如果有错误，非常欢迎大家批评改正哈！（手动抱拳啦），可以看到，这里的主角是update()函数啦，所以，点进去！

void update(const DataContainer& dataContainer, const Eigen::Vector3f& poseHintWorld, bool map_without_matching = false)
  {
    //std::cout << "\nph:\n" << poseHintWorld << "\n";

    Eigen::Vector3f newPoseEstimateWorld;

    //若map_without_matching为false，则进行匹配。matchData（）为匹配函数，重要！
    if (!map_without_matching){

        //matchData（）函数传入数据为t-1时刻匹配的世界位姿，当前帧的激光数据类，t-1时刻匹配的协方差（或者为信息矩阵）
        //小技巧：点击matchData函数会进入到虚函数里面，此时点击左侧的小圆圈就会跳转到具体实现，同理，类也可以找到继承关系的类。
        newPoseEstimateWorld = (mapRep->matchData(poseHintWorld, dataContainer, lastScanMatchCov));
    }else{
        //若map_without_matching为true，则赋值为t-1时刻匹配位姿。
        newPoseEstimateWorld = poseHintWorld;
    }

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

        //地图更新，也是重点！(还没有看)
      mapRep->updateByScan(dataContainer, newPoseEstimateWorld);

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

同理，我还是值截取了相关的我想看的部分哈，见谅！同样，我们看到这里的主角是matchData()函数，点进去！我是先跳转到了虚函数声明部分，但我用的clion可以再点击虚函数左侧的小圆点跳转到具体实现的源码部分，如果你没有找到的话，那直接告诉你在：
hector_mapping/include/hector_slam_lib/slam_main/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))));

        /*
        arg1 : t-1时刻机器人在世界坐标系下的位姿
        arg2 : 激光数据
        arg3 : t-1时刻扫描匹配的协方差
        arg4 : 最大迭代次数
        ret : t时刻机器人在世界坐标系下的位姿
        */
        tmp  = (mapContainer[index].matchData(tmp, dataContainers[index-1], covMatrix, 3));
      }
    }
    return tmp;
  }

这里继续，matchData()函数点进去！然后再点一下函数体内部的matchData()函数

if (dataContainer.getSize() != 0)
    {
        //将t-1时刻机器人在世界坐标系下的位姿转化为地图坐标系的坐标
      Eigen::Vector3f beginEstimateMap(gridMapUtil.getMapCoordsPose(beginEstimateWorld));

      //将beginEstimateMap（vector）复制到estimate（vector）,这样保留了beginEstimateMap（vector）内容不变。
      Eigen::Vector3f estimate(beginEstimateMap);

      //关键函数！！！(返回值为求得增量之后的位移)
      estimateTransformationLogLh(estimate, gridMapUtil, dataContainer);
            int numIter = maxIterations;
      
      for (int i = 0; i < numIter; ++i)
      {

        estimateTransformationLogLh(estimate, gridMapUtil, dataContainer);
        //notConverged = estimateTransformationLogLh(estimate, gridMapUtil, dataContainer);

        if(drawInterface){
          float invNumIterf = 1.0f/static_cast<float> (numIter);
          drawInterface->setColor(static_cast<float>(i)*invNumIterf,0.0f, 0.0f);
          drawInterface->drawArrow(gridMapUtil.getWorldCoordsPose(estimate));
          //drawInterface->drawArrow(Eigen::Vector3f(0.0f, static_cast<float>(i)*0.05, 0.0f));
        }

        if(debugInterface)
        {
          debugInterface->addHessianMatrix(H);
        }
      }

      if (drawInterface)
      {
        drawInterface->setColor(0.0,0.0,1.0);
        drawScan(estimate, gridMapUtil, dataContainer);
      }
            estimate[2] = util::normalize_angle(estimate[2]);

      covMatrix = Eigen::Matrix3f::Zero();
      //covMatrix.block<2,2>(0,0) = (H.block<2,2>(0,0).inverse());
      //covMatrix.block<2,2>(0,0) = (H.block<2,2>(0,0));


      /*
      covMatrix(0,0) = 1.0/(0.1*0.1);
      covMatrix(1,1) = 1.0/(0.1*0.1);
      covMatrix(2,2) = 1.0/((M_PI / 18.0f) * (M_PI / 18.0f));
      */


      covMatrix = H;

      return gridMapUtil.getWorldCoordsPose(estimate);
    }

    return beginEstimateWorld;
  }

我对代码的理解已经写在了注释上，也有借鉴别人的，也有我自己的。点进estimateTransformationLogLh()函数，

bool estimateTransformationLogLh(Eigen::Vector3f& estimate, ConcreteOccGridMapUtil& gridMapUtil, const DataContainer& dataPoints)
  {

      //核心函数，计算H矩阵和dTr向量
      //std::vector<Eigen::Vector2f> dataPoints;
    gridMapUtil.getCompleteHessianDerivs(estimate, dataPoints, H, dTr);


      //判断增量非0,避免无用计算
    if ((H(0, 0) != 0.0f) && (H(1, 1) != 0.0f)) {

        //计算优化方程的解，也就是论文中公式12
      //H += Eigen::Matrix3f::Identity() * 1.0f;
      Eigen::Vector3f searchDir (H.inverse() * dTr);

      //std::cout << "\nsearchdir\n" << searchDir  << "\n";

        //对增量进行限位处理
      if (searchDir[2] > 0.2f)
      {
        searchDir[2] = 0.2f;
        std::cout << "SearchDir angle change too large\n";
      } else if (searchDir[2] < -0.2f)
      {
        searchDir[2] = -0.2f;
        std::cout << "SearchDir angle change too large\n";
      }

        //更新估计值 estimate += searchDir
      updateEstimatedPose(estimate, searchDir);
      return true;
    }
    return false;
  }

点进getCompleteHessianDerivs()函数（hector_mapping/include/hector_slam_lib/map/OccGridMapUtil.h文件里）

 void getCompleteHessianDerivs(const Eigen::Vector3f& pose, const DataContainer& dataPoints, Eigen::Matrix3f& H, Eigen::Vector3f& dTr)
  {
      //当前帧激光点的数量
    int size = dataPoints.getSize();

    //输入为t-1时刻匹配激光传感器在地图坐标系下的位姿
      //由位姿算出转换二维转换矩阵
    // 定3*3的2D平移+旋转矩阵，即仿射变换矩阵,这矩阵是为了将激光坐标系下的激光点坐标变换到地图坐标系下
    //坐标变换参考链接：https://blog.csdn.net/jiajiading/article/details/102989118
    //https://blog.csdn.net/jiajiading/article/details/103369058
    Eigen::Affine2f transform(getTransformForState(pose));

    float sinRot = sin(pose[2]);
    float cosRot = cos(pose[2]);

    H = Eigen::Matrix3f::Zero();
    dTr = Eigen::Vector3f::Zero();

      //对一帧激光中的每个数据点进行处理
    for (int i = 0; i < size; ++i)
    {

        //获取一帧激光的中的第i个激光点在激光坐标系下的坐标（ｘ,ｙ）
      const Eigen::Vector2f& currPoint (dataPoints.getVecEntry(i));

        //通过双线性插值计算栅格概率,取地图的梯度，以及偏导数，对应论文中公式（4）、（5）、（6）、（14）
      //(transform * currPoint)：先将激光坐标系下的坐标x,y转换为栅格地图坐标,然后进行地图插值，得到超越了分辨率的激光点的地图坐标值。
      Eigen::Vector3f transformedPointData(interpMapValueWithDerivatives(transform * currPoint));

        //击中点的栅格概率值应该是1,计算匹配误差(1-M(Pm))
      float funVal = 1.0f - transformedPointData[0];

        //更新x,y的位移增量, dTr 这个向量计算的是公式12 的求和符号后面的部分，没有乘H-1
      dTr[0] += transformedPointData[1] * funVal;
      dTr[1] += transformedPointData[2] * funVal;

        //计算旋转误差(由map->base_link),这个对应论文中公式 13，14 ,计算 M的梯度叉乘S对旋转角度的偏导数
      float rotDeriv = ((-sinRot * currPoint.x() - cosRot * currPoint.y()) * transformedPointData[1] + (cosRot * currPoint.x() - sinRot * currPoint.y()) * transformedPointData[2]);

        //更新角度增量
      dTr[2] += rotDeriv * funVal;

        //更新海塞矩阵,下面的对应于论文中公式13（这个公式有问题，掉了个求和符号，代码是正确的），计算hessian 矩阵
      H(0, 0) += util::sqr(transformedPointData[1]);
      H(1, 1) += util::sqr(transformedPointData[2]);
      H(2, 2) += util::sqr(rotDeriv);

      H(0, 1) += transformedPointData[1] * transformedPointData[2];
      H(0, 2) += transformedPointData[1] * rotDeriv;
      H(1, 2) += transformedPointData[2] * rotDeriv;
    }

    H(1, 0) = H(0, 1);
    H(2, 0) = H(0, 2);
    H(2, 1) = H(1, 2);

  }

找到interpMapValueWithDerivatives()函数，这就是我想看的双线性插值啦！点进去！

 Eigen::Vector3f interpMapValueWithDerivatives(const Eigen::Vector2f& coords)
  {
    //check if coords are within map limits.
    //检查激光点是否超出设置的地图匹配范围，pointOutOfMapBounds记得看一下？？？，如果超出，设置为０００
    if (concreteGridMap->pointOutOfMapBounds(coords))
    {
      return Eigen::Vector3f(0.0f, 0.0f, 0.0f);
    }

      //对坐标进行向下取整，即得到坐标(x0,y0)
    //map coords are always positive, floor them by casting to int
    Eigen::Vector2i indMin(coords.cast<int>());


      //得到双线性插值的因子
      // | x |   | x0 |   | x-x0 |
      // |   | - |    | = |      |
      // | y |   | y0 |   | y-y0 |
    //get factors for bilinear interpolation
    Eigen::Vector2f factors(coords - indMin.cast<float>());

      //获得地图的X方向最大边界
    int sizeX = concreteGridMap->getSizeX();

      //将地图坐标转换成地图数组索引,地图是以一维数组的形式存储的，每个元素有2个值，val(概率)，index(元素索引，初始化为-1)
      // 这么解释，就应该懂下一句的意思了，就是计算endpoint 在一维数组中的索引值
    int index = indMin[1] * sizeX + indMin[0];


      //下面四个if,就是获取当前实数坐标点相邻的四个栅格坐标，并获取该栅格的占用概率（没占用<0.4,占用>0.6）,这里使用了个小技巧，
      // 就是用缓存，先判断当前缓存中数据是否有效，如果有效就读缓存，无效的话就直接读栅格地图中的值，地图想象不出来的话，看论文图片。
      // 获取当前坐标周围的4个网格点的网格值。 首先检查缓存数据，如果没有包含过滤器gridPoint with gaussian并存储在缓存中。
    // get grid values for the 4 grid points surrounding the current coords. Check cached data first, if not contained
    // filter gridPoint with gaussian and store in cache.
    if (!cacheMethod.containsCachedData(index, intensities[0]))
    {
        //当前栅格的周围的左下角的栅格索引对应的概率值（假设x轴向右，ｙ轴向上）
      intensities[0] = getUnfilteredGridPoint(index);
      cacheMethod.cacheData(index, intensities[0]);
    }

    ++index;

    if (!cacheMethod.containsCachedData(index, intensities[1]))
    {
        //当前栅格的周围的右下角的栅格索引对应的概率值
      intensities[1] = getUnfilteredGridPoint(index);
      cacheMethod.cacheData(index, intensities[1]);
    }

    index += sizeX-1;

    if (!cacheMethod.containsCachedData(index, intensities[2]))
    {
        //当前栅格的周围的左上角的栅格索引对应的概率值
      intensities[2] = getUnfilteredGridPoint(index);
      cacheMethod.cacheData(index, intensities[2]);
    }

    ++index;

    if (!cacheMethod.containsCachedData(index, intensities[3]))
    {
        //当前栅格的周围的右上角的栅格索引对应的概率值
      intensities[3] = getUnfilteredGridPoint(index);
      cacheMethod.cacheData(index, intensities[3]);
    }


    //以下所有都是用于双线性差值用的，具体的就是对照论文中公式4、5、6，简单讲解下（以论文中符号为准）
    float dx1 = intensities[0] - intensities[1];
    float dx2 = intensities[2] - intensities[3];

    float dy1 = intensities[0] - intensities[2];
    float dy2 = intensities[1] - intensities[3];

      //得到双线性插值的因子,注意x0+1=x1,y0+1=y1,则
      //     | x-x0 |   | 1-x+x0 |   | x1-x |
      // 1 - |      | = |        | = |      |
      //     | y-y0 |   | 1-y+y0 |   | y1-ｙ |
    float xFacInv = (1.0f - factors[0]);
    float yFacInv = (1.0f - factors[1]);


      //         y-y0 | x-x0            x1-x        |    y1-y | x-x0            x1-x        |
      //M(Pm) = ------|------ M(P11) + ------ M(P01)| + ------|------ M(P10) + ------ M(P00)|
      //         y1-y0| x1-x0           x1-x0       |    y1-y0| x1-x0           x1-x0       |
      //注意：此处y1-y0=x1-x0=1,那么对应函数返回值，可以写成
      //M(Pm) = (M(P00) * (x1-x) + M(P10) * (x-x0)) * (y1-y) + (M(P01) * (x1-x) + M(P11) * (x-x0)) * (y-y0)

      // d(M(Pm))     y-y0 |                |    y1-y |                |
      //---------- = ------| M(P11) - M(P01)| + ------| M(P10) - M(P00)|
      //    dx        y1-y0|                |    y1-y0|                |
      //同理，化简可得 d(M(Pm))/dx = -((M(P00) - M(P10)) * (y1-y) + (M(P01) - M(P11)) * (y-y0))
      //同样地，也有   d(M(Pm))/dy = -((M(P00) - M(P01)) * (x1-x) + (M(P10) - M(P11)) * (x-x0))
    return Eigen::Vector3f(
      ((intensities[0] * xFacInv + intensities[1] * factors[0]) * (yFacInv)) +
      ((intensities[2] * xFacInv + intensities[3] * factors[0]) * (factors[1])),
      -((dx1 * xFacInv) + (dx2 * factors[0])),
      -((dy1 * yFacInv) + (dy2 * factors[1]))
    );
  }

理解部分直接看注释哈！

参考链接

代码部分的参考链接：
源码参考１
源码参考２

线性插值部分的参考链接：
插值系列的１２３４５
知乎
其他

好啦，这次的分享就先到这里啦！