参考论文《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 =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<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;
}