参考文献：《机器人SLAM导航-核心技术与实战》第七章

一、SLAM的问题起源与发展

​ 起初，机器人的定位与建图问题被看成是两个独立的问题来研究。

​ 所谓机器人的定位问题，是在已知全局地图的条件下，通过机器人传感器测量环境，利用环境测量信息与地图之间存在的关系求解机器人在地图中的位姿。定位问题的关键是必须事先给定环境地图。例如分拣仓库中的二维码路标，就是人为给定的环境地图路标信息，机器人只需识别二维码就能得到机器人当前的位姿。

​ 机器人的建图问题，是在已知机器人全局位姿的条件下，通过机器人传感器测量环境，利用测量地图路标时的机器人位姿和测量距离及方位信息，求解出观测到的地图路标点坐标值。建图问题的关键是必须事先给定机器人观测时刻的全局位姿。例如装载了GPS定位的测绘飞机，飞机由GPS提供定位信息，基于GPS定位信息完成地形测绘。显然，这种建立在环境先验基础上的定位和建图具有很大的局限性。倘若把机器人放置在未知环境中，机器人将无法实现定位与建图，陷入进退两难的局面。

​ 2006年，Durrant-Whyte和Bailey在其研究课题中首次使用SLAM这一名词进行表述。SLAM按照时间可以简单划分为古典SLAM和现代SLAM。

(一)古典SLAM

​ 传统的定位和建图问题，采用构建机器人运动的数学模型的方法来解决，显然这种方法没有考虑到实际机器人问题中存在的众多不确定性因素，如传感器测量噪声、电机控制偏差、软件近似精度等等。因此实际上需要特别关注机器人中的不确定性问题。

​ 1986年，Smith等人，将机器人定位与建图问题转换为状态估计问题，在概率框架之中展开研究，利用扩展卡尔曼滤波(EKF)、粒子滤波（PF）等滤波方法来求解。其中有两个开创性的点，算得上是SLAM问题的起源：

1. 第一采用了基于概率论理论框架对机器人的**不确定性**进行讨论；
1. 第二将将**定位与建图**中的机器人位姿量与地图路标点作为同一的估计量**进行整体状态估计**；

​ 2006年，Whyte等人，逐渐建立起来SLAM理论体系，该理论框架的收敛性得到了验证。古典SLAM时期，滤波方法是解决SLAM问题的主要方法，例如EKF-SLAM算法，基于Rao-Blackwellized粒子滤波SLAM算法(即FAST-slam). 而基于Rao-Blackwellized粒子滤波来研究栅格地图的SLAM算法，就是著名的Gmapping算法。可以说，基于粒子滤波的SLAM算法大大提高了求解效率，使得SLAM在工程实践中具有可行性。

(二)现代SLAM

​ 贝叶斯网络中采用滤波法求解SLAM的方法，需要实时获取每一时刻的信息，并把信息分解到贝叶斯网络的概率分布中去，因此滤波方法是一种在线SLAM系统，计算代价非常大，因此只构建小规模的地图。

​ 为了进行大规模建图，在因子图中采用优化方法求解SLAM的方法被提出，优化方法的思路与滤波方法相反，它只是简单地累计获取到的信息，然后利用之前所有时刻累计到的全局信息离线计算机器人的轨迹和路标点，即优化方法是一种完全SLAM系统。随着优化方法在稀疏性和增量求解方面的突破，以及闭环检测方面的额研究，体现出了巨大的价值，例如基于视觉传感器的优化方法成为现代SLAM研究的主流方向。特别是2016年ORB_SLAM2开源算法的问世。

1.1 数据关联、收敛、一致性

数据关联

​ 将传感器观测数据与已构建出来的地图进行匹配，判断传感器观测数据中哪些路标特征是新的、哪些是旧的，并将新路标特征数据有效地融入到地图中。

严格的路标特征剔除机制是地图信息自我纠错的有效手段，闭环检测则能大大降低定位累计误差。

收敛

​ 收敛用于衡量SLAM系统在理论上的可行性。由于机器人观测模型和运动模型都具有不确定性，所以将SLAM放在概率框架下讨论，并利用状态估计去求解，即估计量都是带有不确定性的估计值。

​ 由路标特征组成的地图是实际环境特征的估计，估计出来的路标特征与实际环境特征存在偏差的原因是观测存在不充分性。换句话说，只要给机器人足够的机会对环境进行彻底的观测，所得到地图的不确定性将最终消除，即构建出来的地图路标特征会收敛于实际环境特征。

一致性

收敛一致性讨论的是估计量收敛于实际数值的问题。

1. 弱一致性收敛：观测值规模趋于无穷大时，依概率收敛。
2. 强一致性收敛：观测值规模趋于无穷大时，严格收敛。

二、SLAM的基本理论

​ Gmapping算法：滤波方法+激光+占据栅格地图

​ ORG_SLAM算法：优化方法+视觉+路标特征地图

​ 学习SLAM，需要在全局上把握理论本质，再将具体的SLAM实现算法在机器人本体上用起来。

​ SLAM是一个状态估计问题，按照求解方法的不同，已经形成了两大类别，即滤波方法和优化方法。

(一) 滤波方法

​ 滤波方法可以看成一种增量算法，机器人需要实时获取每一时刻的信息，并把信息分解到贝叶斯网络的概率分布中去，状态估计只针对当前时刻。计算信息都存储在平均状态矢量以及对应的协方差矩阵中，而协方差矩阵的规模随着地图路标数量的二次方而增长，也就是说其具有O(n²)计算复杂度。滤波方法在每一次观测后，都要对该协方差矩阵执行更新计算，当地图规模很大时，计算将无法执行下去。

(二) 优化方法

​ 优化方法简单地累积获取到的信息，然后利用之前所有时刻累积到的全局性信息离线计算机器人的轨迹和路标点，这样就可以处理大规模地图了。

​ 滤波方法和优化方法其实就是最大似然和最小二乘的区别。滤波方法是增量式的算法，能实时在线更新机器人位姿和地图路标点。二优化方法是非增量式的算法，要计算机器人位姿和地图路标点，每次都要在历史信息中推算一遍，因此不能做到实时。滤波方法的问题在于计算复杂度，而优化方法的问题在于历史信息的存储。

2.1 SLAM中的概率理论

​ 考虑到实际机器人问题中存在的众多不确定性因素，例如传感器测量噪声、电机控制偏差、计算机软件计算精度近似等，然后利用概率描述机器人中的不确定性，这样机器人中的不确定性就可以被计算和推演，这就是概率机器人学。

​ 在概率机器人位置例子中，描述机器人位置x的概率分布P(x)也称为置信度，在实际工程中可以直接取置信度最大的区间作为随机变量的估计值。

​ 虽然机器人的运动过程和观测过程都存在不确定性，但是通过不断引入观测信息，这种不确定性会越来越小。

2.2 状态估计问题

​ SLAM问题中的不确定性，即状态估计问题。

​ 在机器人的实际运动过程中，运动位移量一般由轮式里程计或者IMU反馈得到，观测通常由机器人上的激光雷达或相机来完成，运动过程和观测过程都存在误差。随时间推移，机器人的位姿将受误差影响偏离真实位姿；因机器人位姿偏差，加上观测误差，观测到的路标特征也自然会偏离真实路标特征。因为误差渗透进了各个地方，所以机器人的位姿和路标特征坐标的真实值是无法直接通过观测信息得到的。那么**SLAM问题其实就是一个状态估计问题，待估计量是机器人位姿和路标点。通过机器人运动过程和观测过程**所提供的信息，利用统计手段逐步减小状态估计量与真实值的偏差，从而完成对机器人位姿和路标特征点的估计。

​ 机器人通过观测和运动两个过程与环境进行交互，同时运行在机器人上的SLAM算法不断地对路标点$m_i$和机器人位姿$x_k$进行估计。用条件概率描述观测过程和运动过程的不确定性，分布如图中的概率运动模型和概率观测模型所示。而SLAM问题就是对路标点$m_i$和机器人位姿$x_k$的状态估计问题。

​ 从状态估计问题的概率表述中可以发现，单独的定位问题是在路标点$m$已知的前提下对机器人位姿$x_k$进行估计，单独的建图问题是在机器人各个位姿$x_{0:k}$已知的前提下对路标点$m$进行估计，而SLAM问题是同时对机器人位姿和路标点进行估计。但是，单独定位问题的概率分布乘以单独建图问题的概率分布并不等于SLAM问题的概率分布，这也说明SLAM问题并不等同于定位和建图问题。

2.3 概率运动模型

​ 机器人感知自身在环境中运动的距离，工程中常用的是前馈与反馈两种表示方法，如图7*6所示。

​ 一般假设底盘在$\Delta t$时间内以前馈速度$u_k$做匀速运动，那么底盘的运动就可以用$u_k*\Delta t$来表示，然而实际上底盘的运动速度会与控制命令有很多出入，所以底盘基本上都配备了轮式里程计来获取实际运动反馈量，轮式里程计并不需要底盘匀速运动的假设就能有效预测底盘的运动情况。这两种方式就是所谓的速度运动模型与里程计运动模型。

1.速度运动模型

​ 速度运动模型的推导基于两大假设，一是假设运动时间$\Delta t$很小，二是假设$u_k、x_k-1和x_k$都服从高斯分布。但是这两个假设性太强了，不符合实际情况。

2.里程计运动模型

​ 移动机器人一般都会配备轮式里程计用于反馈运动情况，因此里程计运动模型是主流的模型。

​ (1) 不考虑里程计的误差

​ (2) 考虑里程计的误差

​ 里程计的误差既可以用底盘系数$a_1{\sim}a_4$描述，也可以用6x6的协方差矩阵来描述。例如，ROS中的Gmapping算法使用的就是系数$a_1{\sim}a_4$；

​ 机器人利用里程计运动模型推算位姿，其不确定性的演变过程如图7-9所示。图中轨迹上的黑点表示机器人可能出现的位姿，黑点周围的密度表示概率大小，黑点越集中的地方表示出现的概率越大。随着机器人的移动，由于里程计存在的误差，直接利用里程计推导出来的机器人位姿的不确定性越来愈大，最终这些可能的位姿将分散到整个空间，即机器人位姿将变得完全随机。为了不让机器人位姿发散，必须在里程计运动模型的基础上，引入传感器对环境的观测数据。即概率观测模型。

2.4 概率观测模型

​ 工程中常用的环境观测传感器是激光雷达和相机。

传感器环境观测分为两步：

• 第一步是提取环境路标特征；
  • 例如单线激光雷达，路标特征可以使用观测角度和测量距离来表示；
  • 多线激光雷达，路标特征除了观测角度和测量距离，还能包含高层级点云轮廓信息；
  • 而相机中，路标特征可以用像素坐标、像素颜色、高层级特征等；
• 第二步是数据关联；
  • 路标特征提取出后，需要判断该路标特征是否为新路标，并将新出现的路标特征融入到已有地图中，这就是数据关联。

以激光雷达观测模型的不确定性问题为例，主要通过波束模型和似然域模型对观测不确定性进行建模，即所谓的概率观测模型。

1.波束模型

​ 激光雷达依靠激光束对物体距离进行探测，单个激光束在激光雷达旋转带动下实现各个角度范围的探测。

​ 这里以单线激光雷达为例，每一个扫描角度上发出的激光束可以探测一个对应的距离值$z^i_k$，激光雷达旋转一圈的n个观测值$z_k=\lbrace z^1_k,z^2_k,z^3_k,\cdots,z^i_k,\cdots,z^n_k \rbrace$

​ 那么观测$z_k$的不确定性由每一个单独激光束$z^i_k$的不确定性相乘得到，如公式所示：
$$
P(z_k|x_k,m)=\prod_{i=1}^{n}{P(z^i_k|x_k,m)}\tag{7-17}
$$
​ 波束模型描述的就是这里面的单个激光束测距值$z^i_k$的不确定性。单个激光束测距值的误差主要分为四个方面：

• 读数误差

  传感器由于构造、计算精度、分辨率等因素给测量带来的误差，$P_{hit}(z^i_k|x_k,m)$

• 动态干扰

  环境中移动的障碍物会挡住激光束，带来的误差，$P_{short}(z^i_k|x_k,m)$

• 测量失效

  环境中的黑色吸光、透明、镜面反射等障碍物会导致测量失效，即环境中有障碍物，但是得到的激光测距值是超量程的值或无穷远，可以用最大量程$z_{max}$附近的窄矩形分布来描述。$P_{max}(z^i_k|x_k,m)$

• 随机误差

  不能明确建模的误差，使用一个均匀分布的随机误差来统一描述，均匀分布的范围是激光波束的整个量程区间；$P_{rand}(z^i_k|x_k,m)$

将这四种误差加权平均后就是光束模型。

2. 似然域模型

​ 从波束模型的概率描述图形可以看出，波束模型缺乏光滑性，导致构建出来的地图有很多毛刺。

​ 似然域模型可以解决这一问题。

​ 似然域模型的思路：先利用测量值对应的机器人位姿，将测量值$z^i_k$投射到已有的地图上，并将超过量程的测量值直接丢弃，然后用似然域描述测量值的分布。

​ 似然域：激光束上障碍物出现的概率大小由其附近的地图点远近决定。

​ 如图7-11，激光束观测值$z^i_k$被投射到已有地图上，地图中已有障碍点由A、B、C，然后找到地图中离$z^i_k$最近的点，这里最近点显然是C，计算其最近点距离值dist，然后利用距离值dist构造$z^i_k$的似然分布就可以了。

利用距离值dist构造$z^i_k$的似然分布：
$$
P_{hit}(z^i_k|x_k,m)=\frac{1}{\sigma_{hit}\sqrt{2\pi}}e^{-\frac{dist^2}{2\sigma^2_{hit}}}\tag{7-23}
$$
同样的，将各种误差加权平均后就是似然域模型：
$$
P(z^i_k|x_k,m)=z_{hit}\cdot P_{hit}+z_{max}\cdot P_{max}+z_{rand}\cdot P_{rand}\tag{7-24}
$$
​ 利用上式可以计算出单个激光束测量值的似然域，将各单独测量似然域按式(7-17)相乘就得到整个测量的似然域。整个似然域在投射地图上将呈现出一个个区域，距离地图中已有障碍物点近的投射区域，其分布自然就集中，反之，其分布自然就分散。

2.5概率图模型

​ 概率图模型，描述的是运动模型与观测模型之间的概率关系。概率图模型是概率理论与图论结合的产物，将随机变量之间的概率关系用图结构表示，就是所谓的概率图模型。

​ 利用图论结构化的数据表示方式，可以让随机变量之间的依赖关系变得更加直观。图结构由节点和边组成，图中的每一个节点都代表一个随机变量，连接节点的边代表随机变量之间的概率依赖关系。如果连接节点的边有方向，就是有向图，也称为贝叶斯网络；如果边没有方向，就是无向图，也称为马尔可夫网络。

1. 贝叶斯网络

​ 贝叶斯网络中有向边的箭头的含义可以理解为两个随机变量之间的依赖是因果关系，贝叶斯网络是有向无环图，即箭头指向的路径不能闭合。根据随机变量集的属性，还可以分为静态贝叶斯网络和动态贝叶斯网络。

​ 静态贝叶斯网络中每一个随机变量节点都对应一个特定的事件。当随机变量集$X=\lbrace s_0,s_1,\dots,s_k\rbrace$是由随机变量s的时序状态构成，每个状态$s_k$产生对应的观测$l_k$，两个状态节点之间的箭头表示状态更新方向，状态节点与观测节点之间的箭头表示观测过程，这些节点和箭头就构成了动态贝叶斯网络。显然，动态贝叶斯网络可以描述机器人运动模型与观测模型之间的概率关系。

​

​ 构建图的目的是更方便地表示概率分布，这个过程也称为图结构参数化。贝叶斯网络的参数化，是将随机变量集上的联合概率分布分解为局部条件概率分布的乘积，如图7-12a 对应的链式乘法运算：
$$
P(A,B,C,D,E,F,G)=P(A)P(B)P(C|A,B)P(D,B)P(E|C)P(F|D)P(G|E,F) \tag{7-30}
$$

2. 马尔可夫网络

​ 马尔可夫网络的结构如图7-13所示，图中的每一个节点表示一个随机变量，连接两个节点之间的边是没有方向的，边代表两个随机变量之间的相关性。

​ 由于实际中，两个随机变量的因果关系很难确定，往往只知道两个随机变量之间存在某种相关性，无向图就是用于构建这种非直观相关性的模型。

​ 马尔可夫网络的参数化较为抽象，其联合概率分布可以分解为团$Q_i$的势能函数$\Psi_{Q_i}(Q_i)$的乘积。团是一个节点集合，团中的节点必须两两之间都有边连接。以团中包含的随机变量作为自变量，构建团的势能函数。团的势能函数是一个抽象函数，需要在实际问题中被具体化为特定形式。以每条边上的两个节点作为一个团是最简单的形式，如图7-13a所示，划分出来的团依次为$Q_1=\lbrace A,B\rbrace 、Q_2=\lbrace B,C\rbrace、Q_3=\lbrace C,D\rbrace、Q_4=\lbrace C,E\rbrace、Q_5=\lbrace D,E\rbrace、Q_6=\lbrace D,F\rbrace、Q_7=\lbrace E,F\rbrace$，其联合概率分布可以用式7-32所示的势能函数乘积表示。其中$Z$是归一化常数，以确保计算结果仍为合法概率分布：
$$
P(A,B,C,D,E,F)=\frac{1}{Z}\psi_{Q_1}(A,B)\psi_{Q_2}(B,C)\psi_{Q_3}(C,D)\psi_{Q_4}(C,E)\psi_{Q_5}(D,E)\psi_{Q_6}(D,F)\psi_{Q_7}(E,F)\tag{7-32}
$$

最大团：如果往团中加入团外的其他任何节点，将不再成团，这样的团就是最大团。

例如$Q_3、Q_4、Q_5$可以合并成一个最大团$Q_8=\lbrace C,D,E \rbrace$，团$Q_5，Q_6，Q_7$可以合并一个最大团$Q_9=\lbrace D,E,F \rbrace$。

式7-32可以写为：
$$
P(A,B,C,D,E,F)=\frac{1}{Z}\psi_{Q_1}\lbrace A,B \rbrace \psi_{Q_2}\lbrace B,C \rbrace \psi_{Q_8}\lbrace C,D,E \rbrace \psi_{Q_9}\lbrace D,E,F \rbrace \tag {7-33}
$$
​

​ 以一个最大团中的随机变量为自变量构造出来的势能函数只能笼统地表示各随机变量之间的整体关系，而不能表示各随机变量之间更直接的关系。将势能函数进行因式分解，分解出来的因子项，能描述随机变量之间更直接的关系。

​ 一个势能函数的因式分解结果不唯一，例如：
$$
\psi_{Q_9}\lbrace D,E,F \rbrace = f_1(D,E)\cdot f_2(D,F)\cdot f_3(E,F) \tag{7-35}
$$

$$
\psi_{Q_9}\lbrace D,E,F \rbrace=f_4(D,E,F)\cdot f_5(D,F) \tag {7-36}
$$

这样，马尔可夫网络就可以用因子图更为显式地表示。式7-35和7-36的因子图如图7-14所示。该因子图除了引入随机变量节点（圆形节点），还引入了另外一种因子节点（方形节点），因子节点和随机变量节点用边连接。

显然，各个因子函数对随机变量的概率分布关系描述得更详细，即因子图是马尔可夫网络的细粒度参数化形式。

贝叶斯网络也可以转化为马尔可夫网络，例如：

概率图模型中主要模型之间的关系，如图7-16.

​ 用贝叶斯网络表示机器人SLAM中的观测与运动之间的概率关系最为直接，因为机器人的位姿状态是时序变量，所以实际上是动态贝叶斯网络，如图：

​ 图7-19中，**$u_k$运动量和观测量$z_k$均可被观测，其对应的条件概率分布$P(x_k|x_{k-1},u_k)$和$P(z_k|x_k,m)$。而环境路标特征$m_i$和机器人位姿状态$x_k$不能直接被观测，需要借助贝叶斯网络的推理得到。这种借助贝叶斯网络的推理，其实就是对变量$m_i$和$x_k$的状态估计，按照所估计状态变量的不同，又可以分为在线SLAM和完全SLAM系统**。

3. 在线SLAM系统与完全SLAM系统

​ 在线SLAM系统只对环境路标特征$m=\lbrace m_1,m_2,\dots,m_i \rbrace$和机器人当前位姿状态$x_k$进行估计，该估计问题的概率表述如式（7-38）所示。在线SLAM系统，求解该概率估计问题最典型的方法是扩展卡尔曼滤波(EKF).

​ 完全SLAM系统是对环境路标特征$m=\lbrace m_1,m_2,\dots,m_i \rbrace$和机器人当前及过去所有位姿状态$x=\lbrace x_1,x_2,\dots,x_k \rbrace$进行估计，该问题的概率表达式如（7-39）所示。求解该状态估计问题最典型的方法是优化方法。
$$
P(x_k,m|Z_{0:k},U_{0:k},x_0\rbrace \tag{7-38}
$$

$$
P(x_{0:k},m|Z_{0:k},U_{0:k},x_0\rbrace \tag{7-39}
$$

优化方法首先将贝叶斯网络表示的SLAM问题转换因子图表示。这样贝叶斯网络中的最大后验估计就等效于因子图中的最小二乘法估计，这种最小二乘法估计常常是非线性最小二乘法估计。

3. 因子图

​ 如何把贝叶斯网络转化为因子图表示？

​ 利用联合概率分布对SLAM问题进行表示，是为了利用结构化表示的条件概率分布进行推理。即在获得直接测量信息$U$和$Z$后，利用条件概率分布关系对不可测量信息$X$和$m$进行推理，这就是SLAM问题的状态估计过程。

​ 这个估计常用到的是最大后验估计，使用$V=\lbrace Z,U \rbrace$统一表示可测量信息，用$S=\lbrace X,m \rbrace$统一表示不可直接测量信息（即待估计量），则最大后验估计表示为：

$$
S_{MAP}=arg{max}_s P(S|V) \tag {7-41}
$$

​ 使用条件概率将式7-41中的后验估计展开，如式（7-42）：

$$
P(S|V) \propto P(x_0)P(x_1|x_0,u_1)P(x_2|x_1,u_2)P(x_3|x_2,u_3)\rbrace(运动)\times P(z_1|x_1,m_1)P(z_2|x_2,m_1)P(z_3|x_3,m_2)\rbrace(观测)\tag{7-42}
$$
​

因为估计过程关心的是X和m，所以可以用运动与观测这两个约束来构建因子函数，并将不关心的U和Z隐藏到因子函数之中。直接以式7-42中的乘积项作为因子图的因子项，得到式7-43所示的因子图表示。

$$
P(S|V)\propto \psi(S) = \prod_{i}\psi_i{(X,m,Z,U)}\tag{7-43}
$$

$\psi_i(X,m,Z,U)$应当包含运动与观测两种类型的约束。

则式7-41中的最大后验估计可以用因子图的因子项来重写：
$$
S_{MAP}=arg max_S{\psi(S)}\tag{7-44}
$$
机器人的运动方程和观测方程分别如式7-45和式7-46所示，$R_k$运动噪声，$Q_k$观测噪声，假设两种噪声都是0均值高斯噪声：
$$
x_k=g(x_{k-1},u_k)+R_k\tag{7-45}
$$

$$
z_k=h(x_k,m_j)+Q_k\tag{7-46}
$$

$\sum_{R_k}$和$\sum_{Q_k}$表示噪声协方差矩阵，用运动误差和观测误差分别来构造这两种因子项，运动约束的因子项如式7-47所示：

$$
\psi_{i1}(x_k,x_{k-1})\propto e^{-\frac{1}{2}(x_k-g(x_{k-1},u_k))^T \Sigma^{-1}{R_k}(x_k-g(x{k-1},u_k))} \tag {7-47}
$$

观测约束的因子式如式7-48所示：

$$
\psi_{i2}(x_k,m_j) \propto e^{-\frac{1}{2}(z_k-h(x_{k},m_j))^T\Sigma^{-1}{Q_k}(z_k-h(x{k},m_j))}\tag{7-48}
$$

则式7-43就可以写成如式7-49所示的具体形式，
$$
\psi(S) \propto \prod_{i1}\psi_{i1}(x_k,x_{k-1})\cdot \prod_{i2}\psi_{i2}(x_k,m_j)\tag{7-49}
$$
则式7-44就可以写成式7-50：
$$
S_{MAP}=arg max_{s}\psi(S)
$$

对于具有0均值高斯噪声的SLAM问题，所采用的最大后验估计求解方法等价于最小二乘估计(往往还是非线性最小二乘估计)。图7-21是该非线性最小二乘估计对应的因子图，这里省略了因子图连线上的因子节点，所以每条连线就直接代表一个约束。

当有0均值高斯分布条件时，最大后验估计等价于最小均方差误差估计，而最小均方差误差估计这时也等价于最小二乘估计，所以最大后验估计也就是等价于最小二乘估计。

将因子图应用到SLAM问题，对SLAM求解方法从滤波方式转向优化方式起到了关键作用。