从零开始搭二维激光SLAM --- Hector论文公式推导与相关代码解析
这篇文章将带领大家推导一下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 求激光点转换函数的导数
首先求 激光点转换函数的导数．
激光点转换函数Ｓ是个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、bfor (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再进行研究了。
接下来的文章将开始使用里程计，首先简单讲解下里程计，之后对激光雷达数据进行畸变校正处理。
从零开始搭二维激光SLAM --- Hector论文公式推导与相关代码解析相关推荐
- 从零开始搭二维激光SLAM --- 基于GMapping的栅格地图的构建
上篇文章讲解了如何在ROS中发布栅格地图,以及如何向栅格地图赋值． 这篇文章来讲讲如何将激光雷达的数据构建成栅格地图． 雷达的数据点所在位置表示为占用,从雷达开始到这点之间的区域表示为空闲． 1 GM ...
- 从零开始搭二维激光SLAM --- 激光雷达数据效果对比
我们知道,不同品牌的激光雷达产生的数据是不一样的,那这些不同点是如何影响建图效果的呢? 这篇文章就是来分析这个问题,将从不同光强下的点云效果,不同夹角下的点云效果,以及 1 激光雷达的技术指标 激光雷 ...
- 从零开始搭二维激光SLAM --- 基于ICP的帧间匹配
上一篇文章讲解了如何将激光雷达的sensor_msgs/LaserScan格式转换成pcl::PointCloud< pcl::PointXYZ>格式, 本篇文章将要讲解如何使用这个格式调 ...
- 二维激光SLAM( 使用Laser Scan Matcher )
目录 一.Laser Scan Matcher安装配置 二.二维激光定位 一.Laser Scan Matcher安装配置 ROS自带的laser_scan_matcher库,使用的是CSM(Cano ...
- 实现二维码-完整三种编码流程加代码解析（javascript）
效果 输入内容:XXXwedewed生日//&sss乐❤XXXwedewed生日//&sss乐❤ 完整的演示效果为,输入内容后会将解码绘制的每一步都展示(有点长就不全截图了,可以直接移 ...
- 使用二维激光雷达和cartographer_ros实现实时SLAM
在前面已经完成了cartographer_ros的安装和demo的运行了.接下来,就要放到机器人上,实时进行SLAM了. 前一篇内容的链接如下: Cartographer_ros的下载.配置及编译与问 ...
- 计算机动画类型中只有一维动画和二维动画,二维动画与计算机技术论文(2)
二维动画与计算机技术论文篇二 <计算机二维动画艺术设计浅析> [摘要]计算机技术是二维动画艺术设计的主要制作手法,掌握扎实的专业知识是熟练进行计算机二维动画艺术设计的前提.本文概述了利用计 ...
- 动画与计算机技术的区别,二维动画与计算机技术论文
计算机技术是二维动画艺术设计的主要制作手法,掌握扎实的专业知识是熟练进行计算机二维动画艺术设计的前提.下面是学习啦小编给大家推荐的二维动画与计算机技术论文,希望大家喜欢! 二维动画与计算机技术论文篇一 ...
- 3D激光SLAM:LOAM 论文--算法详细解读
3D激光SLAM:LOAM 论文--算法详细解读 LOAM简介 论文里面的符号表示 算法部分 激光雷达里程计 A 特征点提取 B 找特征点的匹配对 C 运动估计 lidar 建图 测试结果 LOAM是 ...
最新文章
- 数字货币 Electron Cash钱包 如何离线转账
- [科普]浅入浅出Liunx Shellcode
- 【计算理论】上下文无关语法 ( 代数表达式 | 代数表达式示例 | 确定性有限自动机 DFA 转为 上下文无关语法 )
- Ie6下asp.net 中treeview自动随鼠标变小的修复
- BCI2000对win10的支持
- html 标签 r语言,从R中的字符串中删除html标签
- Codeforces 895 B XK Segments 思维 二分
- 陆辰是一名初级药剂师，16西药执业药师一次过17中药一次过 考中级药师#医学生
- 台式机单硬盘安装黑苹果体验
- MovieClip添加点击事件
- android 手机如何截屏,安卓手机一般怎么截屏 安卓如何截图手机屏幕 - 云骑士一键重装系统...
- unity 彩带粒子_超级技术贴：Unity粒子遇上着色器，引爆视觉特效
- luliyu-python-day02
- 扩展名为bat的文件的创建
- 高等数学（第七版）同济大学 习题11-2 个人解答
- 精确查找top k和非精确查找top k
- 推荐7个免费自学网站提升自我价值必备网站
- 电脑里文件消失了,存储内存还占用着,怎么办?
- ora-00942表或视图不存在,解决办法
- 设计一个事务增强的动态代理类, 对持久层的用户的CRUD操作进行事务增强 即: