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 largen";
} else if (searchDir[2] < -0.2f) {
searchDir[2] = -0.2f;
std::cout << "SearchDir angle change too largen";
}
/* 更新姿态的估计 */
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 =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 << "nsn" << 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;
}

最后

以上就是整齐夏天为你收集整理的hector 源码分析的全部内容，希望文章能够帮你解决hector 源码分析所遇到的程序开发问题。

如果觉得靠谱客网站的内容还不错，欢迎将靠谱客网站推荐给程序员好友。