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()

/*
arg1 : t-1时刻机器人在世界坐标系下的位姿
arg2 : 激光数据
arg3 : t-1时刻扫描匹配的协方差
arg4 : 最大迭代次数
ret : t时刻机器人在世界坐标系下的位姿
*/
  Eigen::Vector3f matchData(const Eigen::Vector3f& beginEstimateWorld, const DataContainer& dataContainer, Eigen::Matrix3f& covMatrix, int maxIterations)
  {
    return scanMatcher->matchData(beginEstimateWorld, *gridMapUtil, dataContainer, covMatrix, maxIterations);
  }

转到ScanMatcher.h 文件，查看 matchDate()，这里就是重点中的核心，飞机中的战斗机了 （找到你真是不容易啊，跳了这么多层，去除无关的代码）

/*
arg1 : t-1时刻机器人在世界坐标系下的位姿
arg2 : 栅格地图
arg3 : 激光数据
arg4 : 当前时刻 hassian  矩阵
arg5 : 最大迭代次数
ret : t时刻机器人在世界坐标系下的位姿
*/
Eigen::Vector3f matchData(const Eigen::Vector3f& beginEstimateWorld, 
                            ConcreteOccGridMapUtil& gridMapUtil, 
                            const DataContainer& dataContainer, 
                            Eigen::Matrix3f& covMatrix, 
                            int maxIterations)
  {

    if (dataContainer.getSize() != 0) {
/* 将世界坐标系下的坐标，换算成栅格地图坐标 */
      Eigen::Vector3f beginEstimateMap(gridMapUtil.getMapCoordsPose(beginEstimateWorld));

      Eigen::Vector3f estimate(beginEstimateMap);
/*这个函数是重点，将计算 hessian 矩阵 ，并估计t时刻，机器人的位姿*/
      estimateTransformationLogLh(estimate, gridMapUtil, dataContainer);

      int numIter = maxIterations;
/*多次迭代计算，以达到更优的结果*/
      for (int i = 0; i < numIter; ++i) {
        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));
        }
      }
      estimate[2] = util::normalize_angle(estimate[2]);
      covMatrix = Eigen::Matrix3f::Zero();
      covMatrix = H;
      return gridMapUtil.getWorldCoordsPose(estimate);
    }
    return beginEstimateWorld;
  }

转到
/*
arg1 : t-1时刻机器人在地图坐标系下的位姿
arg2 : 栅格地图
arg3 : 激光数据
*/

 bool estimateTransformationLogLh(Eigen::Vector3f& estimate, ConcreteOccGridMapUtil& gridMapUtil, const DataContainer& dataPoints)
  {
  /*计算hessian 矩阵*/
    gridMapUtil.getCompleteHessianDerivs(estimate, dataPoints, H, dTr);

    if ((H(0, 0) != 0.0f) && (H(1, 1) != 0.0f)) {

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

      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";
      }
    /* 更新姿态的估计 */
      updateEstimatedPose(estimate, searchDir);
      return true;
    }
    return false;
  }

转到 OccGridMapUtil.h 文件 ，来分析下getCompleteHessianDerivs这个核心函数

  void getCompleteHessianDerivs(const Eigen::Vector3f& pose, const DataContainer& dataPoints, Eigen::Matrix3f& H, Eigen::Vector3f& dTr)
  {
    int size = dataPoints.getSize();
/* 定义3*3的2D平移+旋转矩阵，transform=Po(t-1)xR(t-1) ,这矩阵是为了将载体坐标系下的endpoint变换到统一的地图坐标系下 */
    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) {

      const Eigen::Vector2f& currPoint (dataPoints.getVecEntry(i));
/*取地图的梯度，以及偏导数，对应论文中公式（4）、（5）、（6）、（14），我们进入代码认真分析下，数学和代码结合才是王道呀，看人家这洋洋洒洒的百来行代码，就搞定了这么复杂的一个问题，得认真研读*/
      Eigen::Vector3f transformedPointData(interpMapValueWithDerivatives(transform * currPoint));

      float funVal = 1.0f - transformedPointData[0];
/* dTr 这个向量计算的是公式12 的求和符号后面的部分，没有乘H-1*/
      dTr[0] += transformedPointData[1] * funVal;
      dTr[1] += transformedPointData[2] * funVal;
/*
这个对应论文中公式 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;
/*惊呆了，看着sqr想都不用想，肯定是开方，但是这里是求平方，哎，函数名误导人啊，下面的对应于论文中公式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);
  }

地图倒数计算

  Eigen::Vector3f interpMapValueWithDerivatives(const Eigen::Vector2f& coords)
  {
    /*检查目标点(就是激光的endpoint)是否超出地图范围
    coords = Po(t-1)xR(t-1)xPe(t)
    将载体坐标系下的endpoint变换到统一的地图坐标系下
    */
    if (concreteGridMap->pointOutOfMapBounds(coords)){
      return Eigen::Vector3f(0.0f, 0.0f, 0.0f);
    }

    /* 
    map coords are always positive, floor them by casting to int
    地图坐标系总是正的，向下取整 
    */
    Eigen::Vector2i indMin(coords.cast<int>());

    //get factors for bilinear interpolation
    //这个向量是用于双线性差值用的
    Eigen::Vector2f factors(coords - indMin.cast());

    int sizeX = concreteGridMap->getSizeX();

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

    // 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,就是获取当前实数坐标点相邻的四个栅格坐标，并获取该栅格的占用概率（没占用<0.4,占用>0.6）,这里使用了个小技巧，就是用缓存，先判断当前缓存中数据是否有效，如果有效就读缓存，无效的话就直接读栅格地图中的值，地图想象不出来的话，看下面的图片
*/
    if (!cacheMethod.containsCachedData(index, intensities[0])) {
      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，简单讲解下（已论文中符号为准）
M(P00) = intensities[0]
M(P10) = intensities[1]
M(P01) = intensities[2]
M(P11) = intensities[3]
x1-x0 = y1-y0 =1
x-x0 = factors[0]
y-y0 = factors[1]

dx1 = M(P00) -M(P10) 
dx2 = M(P01) -M(P11)
dy1 = M(P00) -M(P01)
dy2 = M(P10) -M(P11)
xFacInv = x1 - x
yFacInv = y1 - y
剩下的就是自己套公式了
（这个函数的返回值应该是，函数值加偏导数，函数值的公式和论文是一致的，但是偏导数感觉不对啊？？？哪里有问题？？？
按照论文 应该是
dx = -(dx1*yFacInv + dx2*factors[1])
dy = -(dy1*xFacInv + dy2*factors[0])
）
*/
    float dx1 = intensities[0] - intensities[1];
    float dx2 = intensities[2] - intensities[3];

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

    float xFacInv = (1.0f - factors[0]);
    float yFacInv = (1.0f - factors[1]);

    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]))
    );
  }

以下部分为地图更新部分
在update（）函数里完成matchData（）了之后，紧接着就是updateByScan（）函数
我们转到MapRepMultiMap.h 、MapProcContainer.h、OccGridMapBase.h 文件中查看这个函数。
这个函数英文注释已经写的很详细了，我就不解释了，和gmapping 一样最后也是调用了Bresenham 画线算法，确定激光束所经过的单元格（算法详解自行百度，网上一大堆）
顺便吐槽一下，栅格地图这玩意真的是太占内存了，程序中地图声明的结构元是 LogOddsCell类型的，一个栅格占8个字节（当然最后发布的地图的每个结构元是int8_t类型，共3个值，0表示未占用 ，100表示占用 ，-1 表示未知）
这里说一下，栅格概率如何计算的，在画线的时候，除了endpoint ，其余每个栅格的概率要加上（logOddsFree=ln(2/3)默认值），在endpoint 点，首先减去（logOddsFree=ln(2/3)默认值）然后在加上（logOddsOccupied=ln(3/2)默认值），最后这个栅格到底是free还是occ 用零点判断，小于0为free，大于0为occ

  /**
   * Updates the map using the given scan data and robot pose
   * @param dataContainer Contains the laser scan data
   * @param robotPoseWorld The 2D robot pose in world coordinates
   */
  void updateByScan(const DataContainer& dataContainer, const Eigen::Vector3f& robotPoseWorld)
  {
  /*
  表示这几个变量没看懂存在的意义？？？
  看这个函数最后一句话，说是用于每束激光只能标记该单元格一次（为了代码的健壮性？？？）
*/
    currMarkFreeIndex = currUpdateIndex + 1;
    currMarkOccIndex = currUpdateIndex + 2;

    //Get pose in map coordinates from pose in world coordinates
    Eigen::Vector3f mapPose(this->getMapCoordsPose(robotPoseWorld));

    //Get a 2D homogenous transform that can be left-multiplied to a robot coordinates vector to get world coordinates of that vector
    Eigen::Affine2f poseTransform((Eigen::Translation2f(
                                        mapPose[0], mapPose[1]) * Eigen::Rotation2Df(mapPose[2])));

    //Get start point of all laser beams in map coordinates (same for alle beams, stored in robot coords in dataContainer)
    Eigen::Vector2f scanBeginMapf(poseTransform * dataContainer.getOrigo());

    //Get integer vector of laser beams start point
    Eigen::Vector2i scanBeginMapi(scanBeginMapf[0] + 0.5f, scanBeginMapf[1] + 0.5f);

    //Get number of valid beams in current scan
    int numValidElems = dataContainer.getSize();

    //std::cout << "\n maxD: " << maxDist << " num: " << numValidElems << "\n";

    //Iterate over all valid laser beams
    for (int i = 0; i < numValidElems; ++i) {

      //Get map coordinates of current beam endpoint
      Eigen::Vector2f scanEndMapf(poseTransform * (dataContainer.getVecEntry(i)));
      //std::cout << "\ns\n" << scanEndMapf << "\n";

      //add 0.5 to beam endpoint vector for following integer cast (to round, not truncate)
      scanEndMapf.array() += (0.5f);

      //Get integer map coordinates of current beam endpoint
      Eigen::Vector2i scanEndMapi(scanEndMapf.cast());

      //Update map using a bresenham variant for drawing a line from beam start to beam endpoint in map coordinates
      if (scanBeginMapi != scanEndMapi){
        updateLineBresenhami(scanBeginMapi, scanEndMapi);
      }
    }

    //Tell the map that it has been updated
    this->setUpdated();

    //Increase update index (used for updating grid cells only once per incoming scan)
    currUpdateIndex += 3;
  }