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<float>());    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 =1x-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 - xyFacInv = 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<int>());      //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;  }