slam的回顾（一）

LaserSLAM代码地址

https://github.com/meyiao/LaserSLAM
整体是个matlab的2d激光slam。图优化部分没有写完，代码中用暴力匹配后，没有进一步优化，需要后续继续写程序

BruteMatch与FastMatch

Brute Force匹配和FLANN匹配——BFMatcher（BruteForceMatcher）和FlannBasedMatcher。

二者的区别在于BFMatcher总是尝试所有可能的匹配，从而使得它总能够找到最佳匹配，这也是Brute Force（暴力法）的原始含义。而FlannBasedMatcher中FLANN的含义是Fast Library forApproximate Nearest Neighbors，从字面意思可知它是一种近似法，算法更快但是找到的是最近邻近似匹配，所以当我们需要找到一个相对好的匹配但是不需要最佳匹配的时候往往使用FlannBasedMatcher。当然也可以通过调整FlannBasedMatcher的参数来提高匹配的精度或者提高算法速度，但是相应地算法速度或者算法精度会受到影响。

ICP

参考：

Iterative Closest Point (ICP) and other matching algorithms

http://www.mrpt.org/Iterative_Closest_Point_%28ICP%29_and_other_matching_algorithms

PCL学习笔记二：Registration (ICP算法)

http://www.voidcn.com/blog/u010696366/article/p-3712120.html

https://github.com/ClayFlannigan/icp/blob/master/icp.py

ICP迭代最近点算法综述

http://wenku.baidu.com/link?url=iJJoFALkKpgMl7ilivLCM3teN5yn60TKt5uWM6hIZejYPob8Rcy1R4Tm_2ZyX_DvX_Su9XBFCfPc4TqHioU0Gb93jKbhoj-TQ70vfn4VEJC

graph slam tutorial : 从推导到应用1

• https://blog.csdn.net/heyijia0327/article/details/47686523?reload

在这个graph slam tutorial系列中，还是熟悉的配方熟悉的味道，和其他从推导到应用系列博文一样将通过简单的例子，理论推导，程序应用等三个方面来介绍graph slam。

两个任务：

1. 构建图，机器人的位姿是一个节点（node）或顶点(vertex)，位姿之间的关系构成边(edge)。
2. 优化图，调整机器人位姿顶点尽量满足边的约束

例子一

如下图所示，假设一个机器人初始起点在0处，然后机器人向前移动，通过编码器测得它向前移动了1m，到达第二个地点。接着，又向后返回，编码器测得它向后移动了0.8米。但是，通过闭环检测，发现它回到了原始起点。可以看出，编码器误差导致计算的位姿和观测到有差异，那机器人这几个状态中的位姿到底是怎么样的才最好的满足这些条件呢？

首先构建位姿之间的关系，即图的边：

线性方程组中变量小于方程的个数，要计算出最优的结果，使出杀手锏最小二乘法。先构建残差平方和函数

为了使残差平方和最小，我们对上面的函数每个变量求偏导，并使得偏导数等于0.

整理得到：

接着矩阵求解线性方程组：

所以调整以后为满足这些边的条件，机器人的位姿为：

在这里例子中我们发现，闭环检测起了决定性的作用。

例子二

前面是用闭环检测，这次用观测的路标（landmark）来构建边。如下图所示，假设一个机器人初始起点在0处，并观测到其正前方2m处有一个路标。然后机器人向前移动，通过编码器测得它向前移动了1m，这时观测到路标在其前方0.8m。请问，机器人位姿和路标位姿的最优状态？

在这个图中，我们把路标也当作了一个顶点。构建边的关系如下：

即

残差平方和：

求偏导数：

最后整理并计算得：

得到路标和机器人位姿：

接下来，将引入了一个重要的概念。我们知道传感器的精度是有差别的，也就是说我们对传感器的相信程度应该不同。比如假设这里编码器信息很精确，测得的路标距离不准，我们应该赋予编码器信息更高的权重，假设是10。重新得到残差平方和如下：

求偏导得：


最后计算得到:

将这个结果和之前对比，可以看到这里的机器人位姿x1更靠近编码器测量的结果。请记住这种思想，这里的权重就是在后面将要经常提到的边的信息矩阵，在后面还将介绍。

通过两个例子了解了graph based slam以后，在下一篇博文中，将对图优化的后端（back-end）进行理论推导，并结合matlab仿真程序进行编程应用。

（出处：http://blog.csdn.net/heyijia0327 ）

graph slam tutorial ：从推导到应用２

• 链接：https://blog.csdn.net/heyijia0327/article/details/47731631
  机器人SLAM过程中最优轨迹可以表示成求解机器人位姿使得下面误差平方函数最小。

其中，$X_i$Xi​表示图顶点的参数向量，如机器人位姿。表示测量值$Z_{ij}$Zij​，$\Omega_{ij}$Ωij​表示该误差所占权重的矩阵。$e(X_i,X_j,Z_{ij})$e(Xi​,Xj​,Zij​)是一个向量误差函数表示$X_i$Xi​和$X_j$Xj​之间的关系与测量$Z_{ij}$Zij​之间有多吻合。后面为了简化，将误差函数写成下面的形式：

下图为图优化的一个简单例子:

权重矩阵$\Omega_{ij}$Ωij​信息矩阵

对于上述最小二乘问题还可以从最大似然的角度解释。这里也作简单介绍，主要是为了更进一步的理解误差的权重矩阵。对于传感器的测量，我们可以假设它受高斯白噪声的影响。所以每一个测量值的分布可以看作是以真值为中心的高斯分布。如果测量是多变量的，那就是多元高斯分布。

多元高斯分布的协方差矩阵的某一维越大，高斯曲线越矮胖，表示在这个方向上越不确定。并且高斯分布中，均值部位概率最大。所以，对于某个测量，我们应该使它出现在概率最大的地方，这就是最大似然概率。可以得到似然概率的log形式的计算公式：

上式和前面的误差平方和函数很像，只不过这里显式的指明了误差函数的形式。所以我们发现，误差的权重矩阵（正式名称为信息矩阵）等于协防差矩阵的逆。由于图优化里每一条边代表一个测量值，如表示相邻位姿关系的编码器测量值或者图像（激光）匹配得到的位姿变换矩阵。所以图优化里每一条边的信息矩阵就是这些测量协防差矩阵的逆。如果协防差越小，表示这次测量越准越值得相信，信息权重就越大。

我们看到图优化问题变成了求解最小二乘问题，然而机器人位姿之间的变化函数不是线性的，所以是个非线性最小二乘问题，得通过迭代法进行求解。如果迭代开始时有一个好的初始假设值，那我们就能用Guass-Newton法或者 Levenberg-Marquardt法了

非线性最小二乘问题求解

假设我们已经有了好的初始假设值$\hat{x}$x^，需要在这个值附近迭代寻求最优解。求解的方法是把误差函数在该初始值附近进行一节泰勒展开

其中$J_{ij}$Jij​是误差函数 $e_{ij}(x)$eij​(x) 在$\hat{x}$x^附近的雅克比矩阵。并且为了书写方便，使用$e_{ij}$eij​代替$e_{ij}(\hat{x})$eij​(x^)了。

将上面的(5)式代入误差平方和中的某一项可以得到

注意，这里是$e_{ij}$eij​列向量，是$b_{ij}$bij​列向量，也是$\Delta {x}$Δx列向量，$c_{ij}$cij​是一个数值。

将（10）式代入，可以重写误差平方和函数如下：

其中，$c=\sum{c_{ij}}$c=∑cij​,$b=\sum{b_{ij}}$b=∑bij​,$H=\sum{H_{ij}}$H=∑Hij​

为了求解上式，使得其最小，还是求其一阶导数并使其等于0，可以得

其中$H$H是系统的信息矩阵（注意与边的$\Omega_{ij}$Ωij​信息矩阵区分）.

系统的解就是在初始值上叠加这个增量

Guass-Newton迭代法就是不断重复这个过程直到收敛。LM法引入了一个松弛因子来控制迭代速度：

定义一个非线性算子来代表增量

然而，一个值得注意的问题是，上面的步进迭代是用的两个向量直接相加，这是在欧式空间中的做法。而机器人SLAM问题中涉及到平移和旋转，平移是在欧氏空间中，可是旋转就是在非欧空间了。在航迹推演的公式中我们也能看到相邻时刻间位姿的递增由于航向角的存在已经不是简单的相加了。所以得重新定义一个非线性算子来代表增量。

将这个非线性算子用在移动机器人相邻时刻的航迹推演，能够得到：

这里可能有点抽象，别急，在后面2d slam的图优化例子中将具体介绍。

分析b,H,雅克比矩阵结构

误差函数$e_{ij}$eij​只和$X_i,X_j$Xi​,Xj​有关，因此它的雅克比矩阵和$X_i,X_j$Xi​,Xj​无关的列都为0，有如下结构:

清楚了雅克比矩阵的结构，再来看b和H。我们已经知道$b_{ij}$bij​是列向量，可以推算出结构如下，注意下图中$\Omega_{ij}$Ωij​和$e_{ij}$eij​下面的色块代表矩阵和列向量，并且蓝色块代表0，红色块非0。可以看到只有和$X_i,X_j$Xi​,Xj​有关的区域非零。

从上图中可以看出系统是稀疏的，并且正如上面图中的累加一样，程序中也可以对特定的ij计算相应的Hij,bij，然后再把所有的Hij,bij累加起来就行了。

最后把整个求解过程总结如下：

本篇博文对图优化后端（back-end）进行了推导，但是还有很重要的问题，雅克比矩阵在实际编程中如何计算？前面看到了针对非欧空间的递增设计了非线性算子，那么针对非欧空间的误差函数又该如何设计哩（不能直接两个向量相减求模了）？在下一篇博文中，将结合一个二维平面上的图优化例子进行讲解，同时配有matlab仿真代码。

graph slam tutorial ：从推导到应用3

https://blog.csdn.net/heyijia0327/article/details/47428553

对于二维平面的激光SLAM，数据包括两部分，odometry和laser range data，所以构图过程如下：

1、顶点 vertices
当机器人前进0.5m或者旋转超过0.5弧度时，将新的机器人位姿添加到图的顶点，并且记录相应的激光数据。

2、边edges
当前激光数据和上一次的激光数据进行匹配，通过scan match获得两个相邻位姿之间的变换关系，将该变换关系加入到图的边。

3、闭环优化——相应顶点之间加入一条边
当机器人重新回到一个已知区域时，激光数据和以前的若干组数据进行匹配进行闭环检测。如果匹配成功，在相应顶点之间加入一条边。

误差函数

在构建好图以后，就得根据误差函数求雅克比矩阵，然后根据雅克比矩阵求b以及系统信息矩阵H了。

对于2维SLAM，我们知道机器人某一时刻的位姿可以表示成$X_i^T=(x_i,y_i,\theta_{i})$XiT​=(xi​,yi​,θi​)。在各类论文中（主要是弗莱堡大学Grisetti派系的），把误差函数设定为如下形式：

其中函数t2v()表示homogeneous transformation to vector ，齐次变换矩阵转为向量
其中函数t2v()表示将位姿矩阵转为向量，直接看代码。
==》[x,y,β]

% A =  cos -sin x
%      sin cos y
%      0    0   1
% v = (x,y,theta)
function v = t2v(A)
% T2V homogeneous transformation to vector
v(1:2,1) = A(1:2,3); % 第三列第1,2行，即x,y
v(3,1) = atan2(A(2,1), A(1,1));
end

$X_i,X_j$Xi​,Xj​表示位姿向量$x_I,x_i$xI​,xi​的矩阵形式，使用一个v2t()的函数就行了。

function A = v2t(v)
% V2T vector to homogeneous transformation
c = cos(v(3));
s = sin(v(3));
A = [c, -s, v(1);
    s,  c, v(2);
    0   0  1];
end

误差函数表达式中要注意**$X_i^{-1}X_i$Xi−1​Xi​表示位姿j到位姿i之间的变换矩阵$\hat{Z_{ij}}$Zij​^​，也可以说是坐标j和坐标i之间的差异**，至于为什么这里两个矩阵相乘就能表示它们的差异，这是由于它们是位姿矩阵，看下面的分析。

简单版的推导

$X_{wi}$Xwi​表示位姿i在世界坐标系w中的表示,也表示将坐标系i中的一点转换到世界坐标系w中的转换矩阵，同理$X_{wj}$Xwj​表示坐标系j中的一点转换到世界坐标系w中的转换矩阵，也可以说是坐标系j在世界坐标系w中的表示。

现在可以看看那两个矩阵相乘的含义了：

最后的结果是Xij，而它表示的就是坐标系j到坐标系i的转换矩阵。

严格一点的数学推导

分析$Z_{ij}^{-1}\hat{Z_{ij}}$Zij−1​Zij​^​和分析$X_i^{-1}X_j$Xi−1​Xj​是一样的，所以直接分析$Z_{ij}^{-1}\hat{Z_{ij}}$Zij−1​Zij​^​，即为什么测量出的转换矩阵和理论计算的转换矩阵的误差err要这样计算？

预备知识：对于分块矩阵如何求逆。

现在开始推导。不妨假设j到i变换的变换矩阵估计值$\hat{Z_{ij}}$Zij​^​为：

注意，矩阵中的组成分别是旋转矩阵R和平移向量t。i,j之间变换的测量值变换矩阵为：

误差计算：

误差计算中最后一步是把上面的矩阵向量化。

旋转矩阵$R$R表示逆时针旋转，表示KaTeX parse error: Double superscript at position 4: R^T'̲顺时针旋转，KaTeX parse error: Double superscript at position 4: R^T'̲R两个旋转矩阵相乘再向量化的物理意义不就是两个旋转矩阵角度的差异嘛。而KaTeX parse error: Double superscript at position 4: R^T'̲(t-t')表示两个位移向量的差异。因此，同理$X_i^{-1}X_j$Xi−1​Xj​也表示位姿j到位姿i之间的变化。

计算雅克比矩阵

误差函数有了，接下来最重要的一步是计算雅克比矩阵，先把误差函数由矩阵形式一步一步向量化，先把$X_i^{-1}X_j$Xi−1​Xj​如下

将上面的矩阵再进一步转化为误差向量。

把上面的误差向量对机器人位姿i求偏导得到Aij。

先对平移向量$x_i,y_i$xi​,yi​求偏导，再对角度$\theta_{i}$θi​求偏导得

同理，对位姿j求偏导得到Bij。

程序中对应的计算如下：

%compute the homoeneous transforms of the previous solutions
zt_ij = v2t(z_ij);%向量转化为矩阵
vt_i = v2t(v_i);
vt_j = v2t(v_j);

%compute the displacement between x_i and x_j
f_ij=(inv(vt_i) * vt_j);% Xi的逆乘以Xj

%this below is too long to explain, to understand it derive it by hand
theta_i = v_i(3);
ti = v_i(1:2,1);
tj = v_j(1:2,1);
dt_ij = tj-ti;

si = sin(theta_i);
ci = cos(theta_i);

% 这里的A，B是还没有乘以Rz转置的
A= [-ci, -si, [-si, ci]*dt_ij; si, -ci, [-ci, -si]*dt_ij; 0, 0, -1 ];
B =[  ci, si, 0           ; -si, ci, 0            ; 0, 0, 1 ];

ztinv = inv(zt_ij);
e = t2v(ztinv * f_ij); % 误差向量
ztinv(1:2,3) = 0;% 偏导A,B计算公式中只用了z的旋转矩阵R，所以把平移向量强制清0
A = ztinv*A; % 乘以Z的转置，即Z的逆，这是由于旋转矩阵的关系
B = ztinv*B;
 

有了雅克比矩阵以后，只要将计算的b,H累加起来就行了

对应代码如下：

    %compute the blocks of H^k
    b_i = -A' * omega * e;
    b_j = -B' * omega * e;
    H_ii=  A' * omega * A;
    H_ij=  A' * omega * B;
    H_jj=  B' * omega * B;
    
    %accumulate the blocks in H and b
    H((id_i-1)*3+1:id_i*3,(id_i-1)*3+1:id_i*3) = ...
        H((id_i-1)*3+1:id_i*3,(id_i-1)*3+1:id_i*3)+ H_ii;
    H((id_j-1)*3+1:id_j*3,(id_j-1)*3+1:id_j*3) = ...
        H((id_j-1)*3+1:id_j*3,(id_j-1)*3+1:id_j*3) + H_jj;
    H((id_i-1)*3+1:id_i*3,(id_j-1)*3+1:id_j*3) = ...
        H((id_i-1)*3+1:id_i*3,(id_j-1)*3+1:id_j*3) + H_ij;
    H((id_j-1)*3+1:id_j*3,(id_i-1)*3+1:id_i*3) = ...
        H((id_j-1)*3+1:id_j*3,(id_i-1)*3+1:id_i*3) + H_ij';
    b((id_i-1)*3+1:id_i*3,1) = ...
        b((id_i-1)*3+1:id_i*3,1) + b_i;
    b((id_j-1)*3+1:id_j*3,1) = ...
        b((id_j-1)*3+1:id_j*3,1) + b_j;
 

所有部分计算完毕以后，就是计算增量，迭代直到收敛。
下面是matlab代码中，图优化前和优化后的机器人轨迹对比：

代码仓库：https://github.com/versatran01/graphslam

图的数据格式

   最后讲一下图优化前端处理以后产生的顶点和边的数据格式，这是写程序时特别关注的，也是众多优化包如g2o的数据格式。

   顶点vertex2: id，pose.x，pose.y，pose.theta 其中id表示位姿的序号，后面三个是位姿参数

   边EDGE2: idFrom idTo mean.x mean.y mean.theta inf.xx inf.xy inf.xt inf.yy inf.yt  inf.tt 其中idfrom,idTo表示连接边的两个位姿顶点序号，mean.xytheta表示测量的位姿变换矩阵。inf表示边的信息矩阵即权重。

reference：

1. Grisetti的课程，有课件下载，http://www.dis.uniroma1.it/~grisetti/teaching/lectures-ls-slam-master/web/

2. Grisetti. 《A Tutorial on Graph-Based SLAM》

3. Grisetti. 课件 《SLAM Back-end》

本文主要是：https://blog.csdn.net/heyijia0327/article/details/47686523

后记

无意中看到一篇文章分析graph优化问题，但是是某人转载 的文章，转载的过去的竟然有地方弄漏了，甚是苦恼。最后找到了原创文章，果然是个大牛。20200322