—-title: SLAM中的图优化公式推导
date: 2022-09-20 22:17:51
categories:
- 算法推导
这里可以联系cartographer的前端理解。
以下为ubuntu18版本的g2o对应的代码，和16版不兼容。TEB的常见安装问题就是16版到18版不兼容1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17// 求解器一般有两种, TEB用的是 Levenberg
typedef g2o::BlockSolver<g2o::BlockSolverTraits<3, 3>> Block; // 每个误差项优化变量维度为3，误差值维度为1
Block::LinearSolverType* linearSolver = new g2o::LinearSolverDense<Block::PoseMatrixType>();
Block* solver_ptr = new Block(std::unique_ptr<Block::LinearSolverType>(linearSolver) );
g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(std::unique_ptr<Block>(solver_ptr) );
g2o::SparseOptimizer optimizer;
// setAlgorithm()需要传递一个优化算法
optimizer.setAlgorithm(solver);
optimizer.setVerbose(true);
optimizer->initMultiThreading();
// ......
optimizer.optimize(maxIterations); //开始优化，设置迭代次数
Eigen3.1开始，要求we call an initialize函数，如果从不同线程调用Eigen
如果g2o基于OpenMP编译，而且Eigen版本高于3.1，则这个函数调用 Eigen::initParallel
它用来求解线性方程 ，可以从PCG、CSparse、Choldmod、Dense、Eigen选择求解方法。
LinearSolverCSparse：使用CSparse法。继承自LinearSolverCCS
LinearSolverCholmod ：使用sparse cholesky分解法。继承自LinearSolverCCS
LinearSolverDense ：使用dense cholesky分解法。继承自LinearSolver
LinearSolverEigen： 依赖项只有eigen，使用eigen中的sparse Cholesky 求解，因此编译好后可以方便的在其他地方使用，性能和CSparse差不多。继承自LinearSolver
LinearSolverPCG ：使用preconditioned conjugate gradient 法，继承自LinearSolver
选哪个求解器很重要，有一次使用LinearSolverDense，结果260秒才迭代一次，换成LinearSolverCSparse，0.4秒就迭代一次
TEB是涉及到CSparse和Choldmod，默认使用前者
1 | // 加载几组 solver |
construct函数比上面的LinearSolverDense系列更灵活，设置求解器更精准。
使用listSolvers输出所有可用solver到文件，这个结果是和使用了多少G2O_USE_OPTIMIZATION_LIBRARY有关的，上面的5个应该涵盖了所有求解器，结果非常多。比如不加载G2O_USE_OPTIMIZATION_LIBRARY(eigen)，那么lm_var不可用，会报错 SOLVER FACTORY WARNING: Unable to create solver lm_var
listSolvers会输出所有求解器名称和解释，比如1
lm_var Eigen Levenberg: Cholesky solver using Eigen's Sparse Cholesky methods (variable blocksize)
optimizer.setVerbose(true)后才能看到优化过程
或者g2o::SparseOptimizer::chi2()，优化前后各调用一次，可得到优化前后误差变化1
2
3
4
5iteration= 0 chi2= 6973711.783675 time= 0.228985 cumTime= 0.228985 edges= 3995 schur= 0 lambda= 42.450053 levenbergIter= 1
iteration= 1 chi2= 477444.019837 time= 0.219176 cumTime= 0.448161 edges= 3995 schur= 0 lambda= 14.150018 levenbergIter= 1
......
iteration= 20 chi2= 10344.665262 time= 0.200148 cumTime= 4.63974 edges= 3995 schur= 0 lambda= 1.634041 levenbergIter= 1
iteration= 21 chi2= 10344.665262 time= 0.461369 cumTime= 5.10111 edges= 3995 schur= 0 lambda= 112290463347.367386 levenbergIter= 8time是每次的优化时间，cumTime是累计用时。可以看到chi2最后几次迭代不怎么变化了，说明iteration设置大了
1 | Eigen::Matrix3d PoseToTrans(Eigen::Vector3d x) |
1 | Eigen::Vector3d TransToPose(Eigen::Matrix3d trans) |
1 | double ComputeError(std::vector<Eigen::Vector3d>& Vertexs, |
照搬其他人的讲解。
方程个数 > 未知数个数，也就是”超定”线性方程组。方程个数比未知数个数多，可以想象不同方程之间会出现”冲突”的现象，也就是说导致整个方程组没有精确解。但我们只要设定一个误差标准，找到一个让这个误差标准值最小的近似解也是完全可以接收的。即针对超定不相容线性方程组，利用最小二乘误差标准求解近似值。
1.1 首先构建位姿之间的关系，即图的边
下面和开头的非线性最小二乘的推导一样了
问题：用最小二乘拟合直线，什么时候会失败？
答: 矩阵 不可逆的情况。 我的理解是把最小二乘按正规方程解法，也就是先获得 ，这是一个 n×n
的线性方程组，称为正规方程组。显然解为
图优化SLAM通过减去原始传感器的测量，构建了简化的估计问题。这些原始的测量由图中的边取代，边可以看做“虚拟测量”(virtual measurements)。图中的两个节点之间的边是用两个位姿之间的相对位置的概率分布表示的，这一概率分布的条件是它们的相互测量。观测模型 是多模态的，因此高斯分布的假设不成立。这意味着一次观测 可能影响图中连接不同位姿的多条边。 graph connectivity 应当用概率分布来描述。如果直接处理这个多模态问题，将出现极大的复杂度。我们需要将地图变成node和edge的拓扑结构。因此，需要确定不同node之间的edge约束。
我们应当判断一次观测导致的最可能约束，这取决于机器人位姿的概率分布。这一问题称作data association，常由SLAM后端解决。前端需要对机器人轨迹 上的条件先验进行一致估计，这就要求机器人运动过程中对前端和后端穿插地执行。因此后端的准确性和效率对SLAM系统非常重要。
如果观测值受局部高斯噪声影响，而且data association已知，基于图的建图算法的目标就是机器人轨迹的高斯估计后验。包括计算这个高斯分布的均值，然后以均值作为节点的configuration，这样最大化了观测的似然。
Pose(x)与Landmark(m)构成了图的顶点集图优化中边表示约束，Pose到Pose的边与机器人的运动模型相关，机器人从运动到，形成了这条边。同时在运动模型的作用下，可以获得机器人在，控制量为时的状态 g。该值与“真实”的Pose 的差，构成了这条边的约束。由于边的约束是标量，因此取这个差的平方。R表示为运动模型的协方差。公式可以写作
Pose到Landmark的边与机器人的观测模型相关，机器人从 观测到 ，就会形成一条边。同时在观测模型 的作用下，可以获得机器人在 时对 的预测 。该值与此时真正观测到的值 的差，构成了这条边的约束。同样的，取这个差的平方作为约束值。 表示为观测模型的协方差。公式
完整的约束为两种约束的和
SLAM的图优化问题就是寻找合适的 Pose(x), Landmark(m)，使 最小。
看到预测值和观测值的差，很容易想到Cartographer，PoseExtrapolator提供预测值，Scan Matcher提供观测值。
或者可以这样理解：
我们用 表示预测值和观测值之间的差，假设其满足高斯分布 ，或者说观测值的分布是以真值为中心的多元高斯分布。 我们的目标是让预测值和观测值尽可能趋近于0（均值），也就是让 出现在概率最大的地方，这就是个最大似然的问题。 我们让概率密度函数取最大值
把图中的x换成 ， 均值取0，得到下面的最小二乘形式
最小二乘问题的目标函数的变量维度很高，但每个误差项都是简单的，仅与少量变量有关。因此为了直观的展示变量与变量之间的关系，可以把优化问题用图的方式来展现。其中顶点表示优化变量，边表示误差项。
选择你想要的图里的顶点与边的类型，确定它们的参数化形式
往图里加入实际的顶点和边
选择初值，开始迭代
每一步迭代中，计算对应于当前估计值的雅可比矩阵和海塞矩阵
求解稀疏线性方程 ，得到梯度方向
继续用高斯牛顿法(GN)或列文列文伯格-马夸尔特法(LM)进行迭代。如果迭代结束，返回优化值。
滤波类算法最大的问题，是无法处理大尺度场景的建图。由于滤波类算法是基于递归计算的，下一时刻的估计值依赖于上一时刻的估计，因而在大尺度场景下，由于系统参数和传感器观测数据的不准确性，会造成误差慢慢累积，一旦当前时刻的估计出现偏差，无法修正该误差，最终无法获得一致性的地图。
另外滤波方法对计算量和内存消耗都很大，使用图优化方法则效率较高，这种方法位姿不是实时纠正，而是将数据记录下来，最后进行计算优化。
机器人的位姿是一个顶点(vertex)，位姿之间的关系构成边(edge)，边表示两个位姿的空间约束(相对位姿和对应方差)。比如t+1时刻和t时刻之间的里程计关系构成边，或者由视觉计算出来的位姿转换矩阵也可以构成边。图构建完成后，就要调整机器人的位姿去尽量满足这些边构成的约束。图优化就是构建图并调整各顶点的位姿，使预测和观测的误差最小。每一条边的信息矩阵就是协方差矩阵的逆。协方差越小，说明这次测量越准，信息权重就越大。
前端： 从里程计和激光雷达数据中求得位姿，构建图。机器人位姿当做顶点，位姿间关系当做边，常常是传感器信息的堆积
后端： 在图构建好以后，采用回环检测，出现了回环边说明有误差。构造闭环约束，调整机器人位姿，通过最小化观测和估计残差求得优化后的位姿。
通过帧间匹配得到 和 之间的相对位姿，称为观测值
观测方程也有很多形式，如：
同样，它们的具体形式很多样化，这允许我们在讨论slam问题时，不局限于某种特定的传感器或姿态表达方式。
这个减法只是通用的，在SLAM里可能没有定义减法。
首先定义两个粒子集，开始都是空集，一个用于重采样缓冲的set_a，一个是最终需要的set_b，对每份粒子集合创建kdtree。算法的目的是用粒子集合来表示置信度。
先在AMCL里给定一个初始位姿值和协方差矩阵，粒子数为M。一开始，每一个粒子的权重值都是1/M，位姿的分布是一个高斯分布，每个粒子的初始位姿记做(x,y,theta)，其均值是AMCL的初始位姿，方差是从协方差矩阵获得的。将每个粒子插入到kdtree中，对粒子滤波器的粒子集进行聚类。
概率模型是 ， 使用里程计模型进行估计。对每一个粒子更新位姿。位姿是从运动模型中采样获得的，机器人从t-1时刻运动到t时刻，获得相对运动参数rot1,trans,rot2
最终，对于每个粒子的位姿， x加上相对平移量在x轴的分量(乘以cos), y加上相对平移量在y轴的分量， theta加上两次旋转的参数。这些分量除了跟rot1,trans,rot2有关，还跟AMCL中的四个运动噪声参数alpha1~alpha4有关
概率模型是 ，使用似然域模型，用于更新每一个粒子的权重。
似然域测量模型认为测量的结果包含一个高斯分布噪声和随机噪声。激光测量距离（x,y）变换到map坐标系。找到距离激光点最近的障碍，查表获得它们的最近距离dist。这个dist也是高斯噪声的均值，所以dist越大，打到障碍物的可能性越小，则粒子不可信，权重越小。
N为一帧激光的点数
似然域测量模型比较理想，因此在场景有些变化，和动态人存在干扰的情况，估计的pose精度会下降。放大测量方差或许会有效果，最好要是能引入类似非线性优化中的核函数（损失函数）或许不错。
把每个粒子添加到临时粒子集中，也就是说set_a内的粒子的权重是不同的，更新粒子集的平均权重。根据alpha_slow和alpha_fast计算短期和长期的似然平均
从缓冲的粒子集中替换所有粒子，抽取粒子的概率取决于粒子的权重，重采样之后，粒子的数量不变，权重较小的粒子将被过滤掉，权重较大的粒子将被复制
上面的算法还有两个问题： 1. 粒子重采样的退化问题；2. 固定的粒子群导致无用的计算，影响效率。
每个粒子集合中包含的粒子数量很大，粒子的权重即使在多次重采样之后还是很小，导致位置上相邻的粒子的权重差值不大，从而最后输出的预测位姿不准确。
因此AMCL加入了新的结构体 Cluster，表示相邻粒子的集合，具有权重与位姿等属性，与粒子相似。而其位姿是通过该 Cluster中包含的粒子的位姿加权平均得到。开始时 Cluster的数量与粒子数量相等，即每一个 Cluster中只有一个粒子。当粒子在地图上产生聚合后，Cluster的数量就会下降，此时选取权重最大的 Cluster 作为输出结果。
使用KD树存储粒子信息，并且进行聚类分析。树的叶子节点是每一个粒子，而非叶子节点则保存的是该节点的分叉判断的标准。生成树之后，进行粒子聚类，循环遍历每一个叶子节点，在叶子节点的key值的 ±1 范围内的粒子都归为一类。也就是手，每个粒子周围27个粒子若有相连的，就将它们全部连起来，形成聚类。
处理每一个粒子，先生成一个(0,1)的随机数rand，如果 rand< w_diff = (1 - w_fast / w_slow)，那么当前粒子的位姿是随机值。
把上面找出的所有粒子添加到set_b，set_b的粒子个数是M，多数粒子是原来set_a中权重比较大的粒子，而且可能重复。让set_b中的粒子权重全是1 / M
AMCL定位不准的3个依据:
AMCL用到的算法:
梯度是一个矢量，曲面上每点的梯度是常数。
曲面中点的方向导数有无数个，当方向导数与梯度方向一致时，该导数值取得最大，等价于该点在梯度方向具有最快的变化值。梯度方向是函数值增加最快的方向，梯度的反方向是函数值减小最快的方向。
Hessian矩阵是半正定的
对一元函数f(x)来说，就极值而言，一阶导数为0是极值点的必要但不充分条件，一阶导数为0且二阶导数大于0是极小值的充要条件。用二阶泰勒展开就能理解。
对于多元变量，二阶高斯公式展开如下：
作为海森矩阵，也可以写作，可以理解为把梯度向量推广为二阶形式，梯度向量本身也是Jacobian矩阵的一种特例。
把写作 ，类似一元函数，我们希望二次项 对任意的 成立，这就等价于 半正定。
当时，也就是海森矩阵正定，是的局部极小点。但这是充分不必要条件，有的点x可能是极小点，但该点的海森矩阵不是正定的。
但是当时，无法判断此点是否为极值点，所以牛顿法及阻尼牛顿法的缺陷就在这里，因为是半正定的，所以不能保证每次都能收敛到极小值点。
海森矩阵正定，则函数为凸函数
凸函数的任何局部极小点 都是该函数的一个全局极小点。
很多时候我们需要在程序退出的时候做一些诸如释放资源的操作，但程序退出的方式有很多种，比如main()函数运行结束、在程序的某个地方用exit()结束程序、用户通过Ctrl+C或Ctrl+break操作来终止程序等等，因此需要有一种与程序退出方式无关的方法，在退出时执行某函数。方法就是用atexit()函数来注册程序正常终止时要被调用的函数。1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
void atexit_handler_1()
{
std::cout << "at exit #1\n";
}
void atexit_handler_2()
{
std::cout << "at exit #2\n";
}
QApplication a(argc, argv);
const int result_1 = std::atexit(atexit_handler_1);
const int result_2 = std::atexit(atexit_handler_2);
if ((result_1 != 0) || (result_2 != 0)) {
std::cerr << "Registration failed\n";
return EXIT_FAILURE;
}
MainWindow w;
w.show();
return a.exec();
若atexit执行成功，返回0，否则返回非0.
当程序正常终止（调用exit()或者由main函数返回）时，调用atexit()参数中指定的函数。
所有约束在teb_local_planner\include\teb_local_planner\g2o_types，都是头文件。 动态障碍约束EdgeDynamicObstacle，最短路径约束EdgeShortestPath，优先转向约束EdgePreferRotDir，路径点约束EdgeViaPoint没有求雅格比矩阵。
最小化的函数是
penaltyInterval表示惩罚函数，见penaltyBoundToInterval()函数，它只用于速度约束、加速度约束和edge_velocity_obstacle_ratio
向量的维度是2，第一个元素代表线速度，第二个是角速度。
调用：TebOptimalPlanner::AddEdgesVelocity 和 setTebConfig()
1 | class EdgeVelocity : public BaseTebMultiEdge<2, double> |
人工智能开发中很少用到不是0就是1的阶跃函数，而是用更为平滑过渡的sigmoid的函数，在深度学习里用的很多。这里的fast_sigmoid函数是保证速度连续可微，很有借鉴意义。
Rösmann还使用了sign/sigmoid函数决定当前的运动方向。
计算误差函数用到的penaltyBoundToInterval，比较好理解1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49 /* Linear penalty function for bounding var to the interval $-a < var < a $
* var The scalar that should be bounded
* a lower and upper absolute bound
* epsilon safty margin (move bound to the interior of the interval)
* penaltyBoundToIntervalDerivative
* return Penalty / cost value that is nonzero if the constraint is not satisfied
*/
inline double penaltyBoundToInterval(const double& var,const double& a,const double& epsilon)
{
if (var < -a+epsilon)
{
return (-var - (a - epsilon));
}
// 怀疑这里应该是 >=
if (var <= a-epsilon)
{
return 0.;
}
else
{
return (var - (a - epsilon));
}
}
/**
* Linear penalty function for bounding \c var to the interval \f$ a < var < b \f$
* var The scalar that should be bounded
* a lower bound
* b upper bound
* epsilon safty margin (move bound to the interior of the interval)
* penaltyBoundToIntervalDerivative
* return Penalty / cost value that is nonzero if the constraint is not satisfied
*/
inline double penaltyBoundToInterval(const double& var,const double& a, const double& b, const double& epsilon)
{
if (var < a+epsilon)
{
return (-var + (a + epsilon));
}
if (var <= b-epsilon)
{
return 0.;
}
else
{
return (var - (b - epsilon));
}
}
雅格比矩阵的计算被注释了，以为使用重写的雅格比可以加快运算速度，但是Github上有作者的解释，看了似懂非懂：对于一些nontrivial(非显而易见的，难以直观理解的) cost functions (比如 penalizing the distance between a line and a polygon) 最终表达式实在太长了，是否 central difference approximation is slower (2 times evaluating the function) then evaluating the exact Jacobian.
雅克比矩阵的维度为 误差维度 × 优化变量的维度，class EdgeVelocity : public BaseTebMultiEdge<2, double>，是三元边，两个VertexPose顶点，类型PoseSE2(3维)。顶点VertexTimeDiff，类型double(1维)。对于前者，雅格比矩阵是 2x3，对后者是 2x1，也就是如下1
2
3_jacobianOplus[0].resize(2,3); // conf1
_jacobianOplus[1].resize(2,3); // conf2
_jacobianOplus[2].resize(2,1); // deltaT
我们把雅格比矩阵简写为 ， 表示第1个VertexPose， 表示第2个VertexPose， 表示VertexTimeDiff。 是两行三列的矩阵，第一行是线速度的误差方程penaltyBoundToInterval对 , , 求偏导。第二行是角速度的误差方程(penaltyBoundToInterval另一个重载形式)对 , , 求偏导。
和 以此类推
 和 J[0](1, 2)的推导过程](https://s2.loli.net/2023/03/04/8WgEX6FU4SN2wK1.png)