这篇文章将带领大家推导一下hector slam论文中的公式.之后再对这部分公式对应的代码进行讲解下.
markdown打公式太费劲了,所以我用手写了.(懒) 然后csdn又限制了图片文件大小,我是照完照片又截图才传上来的,所以图片有点不清晰.
1 高斯牛顿法
首先借用 <视觉SLAM十四讲> 说明一下高斯牛顿法的思路.
高斯牛顿法首先将目标函数进行一阶泰勒展开,再经过公式推导,可以获得最优解的公式为 Δ x = H − 1 ∗ g \Delta x = H^{-1} * g Δx=H−1∗g ,并且hessian矩阵可以通过 H = J T ∗ J H=J^T * J H=JT∗J 的形式进行求解,免去了计算hessian矩阵的较大计算量.
以上图片引自 <<视觉SLAM十四讲>> ,如有侵权请联系我进行删除.
2 公式推导
2.1 目标函数推导
目标函数的形式为
ξ ∗ = a r g m i n ( ∑ i = 1 N [ 1 − M ( S i ( ξ ) ) ] 2 ) \xi^*=argmin(\sum_{i=1}^N[1-M( S_i(\xi) ) ]^2) ξ∗=argmin(∑i=1N[1−M(Si(ξ))]2) ,
代表当前这帧雷达数据占据地图的概率, M ( S i ( ξ ) ) M( S_i(\xi) ) M(Si(ξ)) 最大为1,最大值不好求,所以通过 1 − M ( S i ( ξ ) ) 1-M( S_i(\xi) ) 1−M(Si(ξ)) 的方式求最小值.
首先,通过右加一个小量 Δ ξ \Delta\xi Δξ , 来使得公式趋近于零.
但是由于目标函数是非线性的,不能直接求导,所以根据高斯牛顿法的思路,先进行一阶泰勒展开,然后再进行求导.
之后,进行左右项的变换.求得
Δ
ξ
=
H
−
1
∗
g
\Delta\xi = H^{-1} * g
Δξ=H−1∗g .
2.2 求解
所以,接下来只需要求出 J ( ξ ) J(\xi) J(ξ) 与 f ( ξ ) f(\xi) f(ξ) 就可以了.
只需分别求出 栅格值 M ( S i ( ξ ) ) M( S_i(\xi)) M(Si(ξ)) , 栅格的导数 ∇ M ( S i ( ξ ) ) \nabla M( S_i(\xi)) ∇M(Si(ξ)), 激光点转换函数的导数 ∇ S i ( ξ ) \nabla S_i(\xi) ∇Si(ξ),即可.
2.2.1 求激光点转换函数的导数
首先求 激光点转换函数的导数.
激光点转换函数S是个2*1维的矩阵,第一行是x坐标的转换公式,第二行是y坐标的转换公式.
所以求导时候 分别对第一行第二行,对
p
x
p_x
px ,
p
y
p_y
py ,
ψ
\psi
ψ 三个变量进行求导,将得到一个2*3的矩阵.
2.2.2 求栅格值 M ( S i ( ξ ) ) M( S_i(\xi)) M(Si(ξ))
点(x,y)的栅格值可以通过 这点周围相邻的4个栅格的栅格值 进行插值来表示.
公式是直接从论文里摘下来的,是通过 拉格朗日2次插值 来求的.
2.2.3 求栅格值的导数 ∇ M ( S i ( ξ ) ) \nabla M( S_i(\xi)) ∇M(Si(ξ))
栅格值的导数是一个1*2的矩阵,是分别对x方向,y方向的导数.
公式是直接从论文里摘下来了,是通过栅格值分别对x与y求偏导得到的。
其中 y1-y0 = x1-x0=1,所以是如下形式。
2.2.4 求 J ( ξ ) J(\xi) J(ξ) 与 f ( ξ ) f(\xi) f(ξ)
图片里求
J
(
ξ
)
J(\xi)
J(ξ) 的最后一步时,对结果进行转置了,
J
(
ξ
)
J(\xi)
J(ξ) 是一个 1*3 的矩阵.
2.2.5 求 H e s s i a n Hessian Hessian矩阵
3 相关代码讲解
3.1 getCompleteHessianDerivs()
这个函数位于 Creating-2D-laser-slam-from-scratch/lesson4/include/lesson4/hector_mapping/map/OccGridMapUtil.h 内.
代码的实现和公式一模一样,唯一的区别就是公式里的求和符合在代码里变成了累加。
/**
* 使用当前pose投影dataPoints到地图,计算出 H 矩阵 b列向量, 理论部分详见Hector论文: 《A Flexible and Scalable SLAM System with Full 3D Motion Estimation》.
* @param pose 地图系上的位姿
* @param dataPoints 已转换为地图尺度的激光点数据
* @param H 需要计算的 H矩阵
* @param dTr 需要计算的 g列向量
*/
void getCompleteHessianDerivs(const Eigen::Vector3f &pose,
const DataContainer &dataPoints,
Eigen::Matrix3f &H,
Eigen::Vector3f &dTr)
{
int size = dataPoints.getSize();
// 获取变换矩阵
Eigen::Affine2f transform(getTransformForState(pose));
float sinRot = sin(pose[2]);
float cosRot = cos(pose[2]);
H = Eigen::Matrix3f::Zero();
dTr = Eigen::Vector3f::Zero();
// 按照公式计算H、b
for (int i = 0; i < size; ++i)
{
// 地图尺度下的激光坐标系下的激光点坐标
const Eigen::Vector2f &currPoint(dataPoints.getVecEntry(i));
// 将激光点坐标转换到地图系上, 通过双线性插值计算栅格概率
// transformedPointData[0]--通过插值得到的栅格值
// transformedPointData[1]--栅格值x方向的梯度
// transformedPointData[2]--栅格值y方向的梯度
Eigen::Vector3f transformedPointData(interpMapValueWithDerivatives(transform * currPoint));
// 目标函数f(x) (1-M(Pm))
float funVal = 1.0f - transformedPointData[0];
// 计算g列向量的 x 与 y 方向的值
dTr[0] += transformedPointData[1] * funVal;
dTr[1] += transformedPointData[2] * funVal;
// 根据公式计算
float rotDeriv = ((-sinRot * currPoint.x() - cosRot * currPoint.y()) * transformedPointData[1] +
(cosRot * currPoint.x() - sinRot * currPoint.y()) * transformedPointData[2]);
// 计算g列向量的 角度 的值
dTr[2] += rotDeriv * funVal;
// 计算 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是对称矩阵,只算上三角就行, 减少计算量。
H(1, 0) = H(0, 1);
H(2, 0) = H(0, 2);
H(2, 1) = H(1, 2);
}
3.2 interpMapValueWithDerivatives()
这个函数位于 Creating-2D-laser-slam-from-scratch/lesson4/include/lesson4/hector_mapping/map/OccGridMapUtil.h 内.
取相邻4点的栅格值时用了一个cache函数,这块感觉实现的不好,可能这里就是导致地图变大时hector计算时间变长的原因。
/**
* 双线性插值计算网格中任一点的得分(占据概率)以及该点处的梯度
* @param coords 激光点地图坐标
* @return ret(0) 是网格值 , ret(1) 是栅格值在x方向的导数 , ret(2)是栅格值在y方向的导数
*/
Eigen::Vector3f interpMapValueWithDerivatives(const Eigen::Vector2f &coords)
{
// 检查coords坐标是否是在地图坐标范围内
if (concreteGridMap->pointOutOfMapBounds(coords))
{
return Eigen::Vector3f(0.0f, 0.0f, 0.0f);
}
// 对坐标进行向下取整,即得到坐标(x0,y0)
Eigen::Vector2i indMin(coords.cast<int>());
// 得到双线性插值的因子
// | x | | x0 | | x-x0 |
// | | - | | = | |
// | y | | y0 | | y-y0 |
Eigen::Vector2f factors(coords - indMin.cast<float>());
// 获得地图的X方向最大边界
int sizeX = concreteGridMap->getSizeX();
// 计算(x0, y0)点的网格索引值
int index = indMin[1] * sizeX + indMin[0];
// 下边这取4个点的栅格值,感觉就是导致hector大地图后计算变慢的原因
/** 首先判断cache中该地图点在本次scan中是否被访问过,若有则直接取值;没有则立马计算概率值并更新到cache **/
/** 这个cache的作用是,避免单次scan重复访问同一网格时带来的重复概率计算。地图更新后,网格logocc改变,cache数据就会无效。 **/
/** 但是这种方式内存开销太大..相当于同时维护两份地图,使用 hash map 是不是会更合适些 **/
if (!cacheMethod.containsCachedData(index, intensities[0]))
{
intensities[0] = getUnfilteredGridPoint(index); // 得到M(P00),P00(x0,y0)
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]);
}
float dx1 = intensities[0] - intensities[1]; // 求得(M(P00) - M(P10))的值
float dx2 = intensities[2] - intensities[3]; // 求得(M(P01) - M(P11))的值
float dy1 = intensities[0] - intensities[2]; // 求得(M(P00) - M(P01))的值
float dy2 = intensities[1] - intensities[3]; // 求得(M(P10) - M(P11))的值
// 得到双线性插值的因子,注意x0+1=x1,y0+1=y1,则
// | x-x0 | | 1-x+x0 | | x1-x |
// 1 - | | = | | = | |
// | y-y0 | | 1-y+y0 | | y1-x |
float xFacInv = (1.0f - factors[0]); // 求得(x1-x)的值
float yFacInv = (1.0f - factors[1]); // 求得(y1-y)的值
// 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))
// 计算网格值,计算梯度 --- 原版这里的dx、dy的计算有错误,已经改过来了
return Eigen::Vector3f(
((intensities[0] * xFacInv + intensities[1] * factors[0]) * (yFacInv)) +
((intensities[2] * xFacInv + intensities[3] * factors[0]) * (factors[1])),
-((dx1 * yFacInv) + (dx2 * factors[1])),
-((dy1 * xFacInv) + (dy2 * factors[0]))
// -((dx1 * xFacInv) + (dx2 * factors[0])), // 应该为: -((dx1 * yFacInv) + (dx2 * factors[1]))
// -((dy1 * yFacInv) + (dy2 * factors[1])) // 应该为: -((dy1 * xFacInv) + (dy2 * factors[0]))
);
}
4 实验
将 2.2 节中的 cacheMethod 相关语句都注释掉,发现同样跑完数据包时,注释掉 cacheMethod 后最终 odom_hector 的频率是9.249hz ,而没有注释时最终 odom_hector 的频率是8.571hz ,可见这块确实会导致时间增长.
原因猜测是本来只需要计算4次网格值就行了,现在还有在cache中进行查找,如果数据量太大的情况下查找的时间将比直接计算4次网格值的时间要大.
虽然频率确实下降的少了,但是还是下降了,应该是还有别的原因导致计算时间变长.
5 总结与思考
5.1 总结
通过三篇文章对hector进行了讲解。
第一篇文章讲了hector的地图是如何构建的,第二篇文章讲了hector是如何进行slam的,第三篇文章讲了hector论文的公式推导以及公式对应的代码的讲解。
虽然成功的构建出了地图,但是还是有几点不足的。
- 首先就是hector的计算太慢了,只进行扫描匹配的操作就要花费大概0.1秒钟,这就导致即使雷达频率再高,hector也处理不过来
- 还有就是hector的地图不能自动更改大小,地图的大小在初始化之后始终是固定的
- hector中获取栅格点时使用了cache,这块实现的不是特别好
- hector中维护的始终是一张地图,有时会出现地图被错误更新之后再也恢复不了的情况
5.2 思考
gmapping与hector维护地图的方式不同.
gmapping的维护地图是通过保存下来的位姿与相应的雷达数据,每次发布的地图都是新生成的地图,将所有的保存的雷达数据写成地图。
hector的维护地图是在初始化阶段时只初始化一张地图,然后使用新的雷达数据对这张地图进行更新与维护。
这就导致,如果某时刻的雷达点出现了偏差,将未知区域更新成空闲区域了,而如果之后不出现偏差的话,这块空闲地图再也不会被更新了,也不会变成未知区域了。
也就是上一篇地图中走廊两边的墙内区域还有一些白色区域,这块的白色区域就不会被更新,也就是不会消失,不会再变成未知区域。
这点就是始终维护一张地图不好的地方。但是,如果这帧偏离的激光数据被gmapping保存下来了,那接下来gmapping生成的地图也始终会存在这一块区域。
6 NEXT
hector其实还可以做很多改进,如将里程计或者imu加进来做位姿预测,地图大小可以动态更新,计算地图这可以使用ceres库等等。
github上有人对hector的高斯牛顿这的代码做了改进,使用ceres库进行计算,感兴趣的可以在github上搜索 hector_slam_Ceres 查看代码。
由于后边还有karto与cartographer这些更优秀的项目等着我,所以我就不对hector再进行研究了。
接下来的文章将开始使用里程计,首先简单讲解下里程计,之后对激光雷达数据进行畸变校正处理。