从零开始搭二维激光SLAM --- Hector论文公式推导与相关代码解析

2024-04-02 22:18
文章标签 代码 公式 解析 slam 相关 从零开始 论文 激光 二维 推导 hector

本文主要是介绍从零开始搭二维激光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论文公式推导与相关代码解析的文章就介绍到这儿，希望我们推荐的文章对编程师们有所帮助！


http://www.chinasem.cn/article/871206。

相关文章

网页解析 lxml 库--实战

网页解析 lxml 库--实战

lxml库使用流程 lxml 是 Python 的第三方解析库，完全使用 Python 语言编写，它对 XPath表达式提供了良好的支 持，因此能够了高效地解析 HTML/XML 文档。本节讲解如何通过 lxml 库解析 HTML 文档。 pip install lxml lxm| 库提供了一个 etree 模块，该模块专门用来解析 HTML/XML 文档，下面来介绍一下 lxml 库
阅读更多...
无人叉车3d激光slam多房间建图定位异常处理方案-墙体画线地图切分方案

无人叉车3d激光slam多房间建图定位异常处理方案-墙体画线地图切分方案

墙体画线地图切分方案 针对问题：墙体两侧特征混淆误匹配，导致建图和定位偏差，表现为过门跳变、外月台走歪等 ·解决思路：预期的根治方案IGICP需要较长时间完成上线，先使用切分地图的工程化方案，即墙体两侧切分为不同地图，在某一侧只使用该侧地图进行定位 方案思路 切分原理：切分地图基于关键帧位置，而非点云。 理论基础：光照是直线的，一帧点云必定只能照射到墙的一侧，无法同时照到两侧实践考虑：关
阅读更多...
poj2576(二维背包)

poj2576(二维背包)

题意：n个人分成两组，两组人数只差小于1 ， 并且体重只差最小 对于人数要求恰好装满，对于体重要求尽量多，一开始没做出来，看了下解题，按照自己的感觉写，然后a了 状态转移方程：dp[i][j] = max(dp[i][j],dp[i-1][j-c[k]]+c[k]);其中i表示人数，j表示背包容量，k表示输入的体重的 代码如下： #include<iostream>#include<
阅读更多...
hdu2159（二维背包）

hdu2159（二维背包）

这是我的第一道二维背包题，没想到自己一下子就A了，但是代码写的比较乱，下面的代码是我有重新修改的 状态转移：dp[i][j] = max(dp[i][j], dp[i-1][j-c[z]]+v[z]); 其中dp[i][j]表示，打了i个怪物，消耗j的耐力值，所得到的最大经验值 代码如下： #include<iostream>#include<algorithm>#include<
阅读更多...
sqlite3 相关知识

sqlite3 相关知识

WAL 模式 VS 回滚模式 特性WAL 模式回滚模式（Rollback Journal）定义使用写前日志来记录变更。使用回滚日志来记录事务的所有修改。特点更高的并发性和性能；支持多读者和单写者。支持安全的事务回滚，但并发性较低。性能写入性能更好，尤其是读多写少的场景。写操作会造成较大的性能开销，尤其是在事务开始时。写入流程数据首先写入 WAL 文件，然后才从 WAL 刷新到主数据库。数据在开始
阅读更多...
【C++】_list常用方法解析及模拟实现

【C++】_list常用方法解析及模拟实现

相信自己的力量，只要对自己始终保持信心，尽自己最大努力去完成任何事，就算事情最终结果是失败了，努力了也不留遗憾。💓💓💓 目录   ✨说在前面 🍋知识点一：什么是list？ •🌰1.list的定义 •🌰2.list的基本特性 •🌰3.常用接口介绍 🍋知识点二：list常用接口 •🌰1.默认成员函数 🔥构造函数（⭐） 🔥析构函数 •🌰2.list对象
阅读更多...
活用c4d官方开发文档查询代码

活用c4d官方开发文档查询代码

当你问AI助手比如豆包，如何用python禁止掉xpresso标签时候，它会提示到 这时候要用到两个东西。https://developers.maxon.net/论坛搜索和开发文档 比如这里我就在官方找到正确的id描述 然后我就把参数标签换过来
阅读更多...
poj 1258 Agri-Net（最小生成树模板代码）

poj 1258 Agri-Net（最小生成树模板代码）

感觉用这题来当模板更适合。 题意就是给你邻接矩阵求最小生成树啦。~ prim代码：效率很高。172k...0ms。 #include<stdio.h>#include<algorithm>using namespace std;const int MaxN = 101;const int INF = 0x3f3f3f3f;int g[MaxN][MaxN];int n
阅读更多...
uva 10014 Simple calculations（数学推导）

uva 10014 Simple calculations（数学推导）

直接按照题意来推导最后的结果就行了。 开始的时候只做到了第一个推导，第二次没有继续下去。 代码： #include<stdio.h>int main(){int T, n, i;double a, aa, sum, temp, ans;scanf("%d", &T);while(T--){scanf("%d", &n);scanf("%lf", &first);scanf
阅读更多...
AI hospital 论文Idea

AI hospital 论文Idea

一、Benchmarking Large Language Models on Communicative Medical Coaching: A Dataset and a Novel System论文地址含代码 大多数现有模型和工具主要迎合以患者为中心的服务。这项工作深入探讨了LLMs在提高医疗专业人员的沟通能力。目标是构建一个模拟实践环境，人类医生（即医学学习者）可以在其中与患者代理进行医学
阅读更多...

Copyright China编程 23002807@qq.com

沪ICP备2023033072号-2