SLAM入门之数学基础

2 初识SLAM

Simultaneous Localization and Mapping
同时定位与地图构建

2.1 传感器

定位：自身状态
建图：外在环境

1. 单目 Monocular
   三维空间的二维投影，要恢复三维结构需要改变相机视角
   运动（Motion） 若相机右移，则图像中物体向左移
   结构（Structure）近处的物体移动快，远处的物体移动慢——视差
   尺度（Scale） 估计轨迹地图和真实轨迹地图相差一个因子——尺度不确定性
   缺点：平移后才能计算深度；无法确定真实尺度；
2. 双目 Stereo
   基线（Baseline）两个单目相机间的距离
   优点：消除尺度不确定性；室内室外皆可；
   缺点：测量深度与基线有关，基线越大，测量越远；配置标定复杂，计算量大；
3. 深度 RGB-D
   红外结构光或者TOF（Time-of-Flight）、
   缺点：测量范围窄、噪声大、视野小、易受日光干扰、无法测量投射材质、主要用于室内。

2.2 视觉SLAM框架

传感器信息读取、视觉里程计、后端优化、回环检测、建图

1. 视觉里程计 Visual Odometry（VO） 前端（Front End）

• 通过相邻帧间的图像判断相机运动状态并恢复空间结构
• 存在累计漂移（accumulating drift）

2. 后端优化 Optimization 后端（Back End）

• 处理噪声的问题：通过带有噪声的数据估计整个系统的状态和这个状态估计的不确定性有多大，即最大后验概率估计（MAP）。
• 主要是滤波和非线性优化算法。
• SLAM的本质：对运动主体自身和周围环境空间不确定性的估计。

3. 回环检测 Loop Closing

• 解决了位置估计随时间漂移的问题。实际中机器人回到原点而估计值未回到原点，则把估计位置“拉”到原点消除漂移，即回环检测。在检测到回环后，把A与B是同一点这个信息告诉后端优化，后端据此信息进行轨迹和地图调整，得到全局一致的轨迹和地图。

4. 建图 Mapping

• 度量地图（metric map）

  • 稀疏（spare）：一定程度抽象，不需要表达所有物体——路标（landmark）
  • 稠密（dense）：重建所有能看到的东西 。二维用小格子（Grid），三维用小方块（Voxel）。小方块含有占据、空闲、未知三种状态以表示该格内是否有物体。

• 拓扑地图（topological map）
  是一个图（graph），由节点和边组成。

2.3 数学表述

$\left\{ \begin{aligned} x_k&=f(x_{k-1},u_k,w_k) &运动方程\\ z_{k,j}&=h(y_j,x_k,v_{k,j}) &观测方程 \end{aligned} \right.$
状态估计问题

3 三维空间刚体运动

3.1 旋转矩阵

	旋转矩阵	变换矩阵
描述旋转	$\pmb{a^{'}}=\pmb{Ra}+\pmb{t}$	$\pmb{T}=\left[\begin{matrix}\pmb{R} &\pmb{t}\\\pmb{0}^T&1\end{matrix} \right]$
集合	$SO(n)$ 特殊正交群	$SE(n)$ 特殊欧氏群

$a^{'}$表示旋转后， $a$表示旋转前
旋转矩阵R是行列式为1的正交矩阵，证明

非齐次坐标、齐次坐标：详细说明
从普通坐标转换成齐次坐标时： 如果(x,y,z)是个点，则变为(x,y,z,1); 如果(x,y,z)是个向量，则变为(x,y,z,0)。

3.2 旋转向量（轴角）

描述旋转：用一个旋转轴 $\pmb{n}$和一个旋转角 $\theta$表示旋转

1.旋转向量==>旋转矩阵 ：罗德里格斯公式 推导

$\pmb{R}=cos\theta\pmb{I}+(1-cos\theta)\pmb{n}\pmb{n}^T+sin\theta\pmb{n}$^

2.旋转矩阵==>旋转向量
$\left\{ \begin{aligned} \theta&=arcoss(\dfrac{tr(\pmb{R})-1}{2}) \\ \pmb{Rn}&=\pmb{n} \end{aligned} \right.$

3.3 欧拉角

欧拉角常使用“偏航-俯仰-滚转”（yaw-pitch-roll），等价于 ZYX轴的旋转，注意是先y再p再r。

• 存在万向锁问题

3.4 四元数 quaternion

3.4.1 四元数定义

$\pmb{q}=q_0+q_1i+q_2j+q_3k$
$\pmb{q}=[s,\pmb{v}]$

1.旋转向量==>四元数
$\pmb{q}=[cos\dfrac{\theta}{2},n_xsin\dfrac{\theta}{2},n_ysin\dfrac{\theta}{2},n_zsin\dfrac{\theta}{2}]^T$
2.四元数==>旋转向量
$\left\{ \begin{aligned} &\theta=2arcossq_0 \\ &[n_x,n_y,n_z]^T =[n_x,n_y,n_z]^T/sin\dfrac{\theta}{2} \end{aligned} \right.$

• $\pmb{q}_0=[ \underline{+}1,0,0,0]$表示无旋转

3.四元数==>旋转矩阵

4.旋转矩阵==>四元数

3.4.2 四元数描述旋转

• $\pmb{p}=[0,x,y,x]$
  $\pmb{q}=[cos\dfrac{\theta}{2},\pmb{n}sin\dfrac{\theta}{2}]$
  $\pmb{p^{'}}=\pmb{q}\pmb{p}\pmb{q}^{-1}$
  旋转后 $\pmb{p^{'}}$为纯虚四元数，虚部为3D坐标 证明

4 李群和李代数

$SO(3)$乘法方便，但加法不方便，所以不好进行微积分运算，位姿优化需要求导，所以通过李群和李代数来计算导数。

4.1 基础

1. 群：一种集合加一种运算，满足“封结幺逆”的性质
   李群：具有连续（光滑）性质的群

• 向量可变为反对称矩阵 $\pmb{a}$^ = $\pmb{A}$ = $\left[\begin{matrix} 0 & -a_3 &a_2\\a_3&0&-a_1\\-a_2&a_1&0 \end{matrix} \right]$
  反对称矩阵可变为向量 $\pmb{A}$ $^v$ = $\pmb{a}$

• 推导可得： $\pmb{R} = exp(\phi_0$^ $t)$}
  其中 $\pmb{R}$为旋转矩阵， $\phi_0$^表示反对称矩阵， $\phi$为三维向量，对应了 $SO(3)$的李代数 $\pmb{\mathfrak{so}}$(3)

2. 李代数

• 李代数描述了李群的局部性质，由一个集合、一个数域、一个二元运算[,]（李括号）组成。记为 $\pmb{\mathfrak{g}}$
  eg:三维向量上定义的叉积X是一种李括号。
• $SO(3)$对应的李代数是定义在 $\mathbb{R^3}$上的向量 $\phi$，每个 $\phi$可以生成一个反对称矩阵： $\pmb{\Phi} = \phi$^ $=\left[\begin{matrix} 0 & -a_3 &a_2\\a_3&0&-a_1\\-a_2&a_1&0 \end{matrix} \right]$
• 在不引起歧义的情况下就说 $\pmb{\mathfrak{so}}$(3)的元素是三维向量或者三维反对称矩阵

4.2 映射

1. $SO(3)$ 指数映射

李代数 ==>李群 ：

• $exp(\theta\pmb{a}$^ $)=cos\theta\pmb{I}+(1-cos\theta)\pmb{a}\pmb{a}^T+sin\theta\pmb{a}$

• $\phi$的模长为 $\theta$,方向为 $\pmb{a}$，长度为1， $\phi=\theta\pmb{a}$
  $\pmb{\mathfrak{so}}$(3)实际上就是旋转向量组成的空间，指数映射就是罗德里格斯公式。

李群 ==>李代数 ：见课本或第3讲中又旋转矩阵求旋转向量

• 旋转角度固定在± $\pi$之间时，李群和李代数之间一一对应。旋转矩阵的导数可以由旋转向量指定，知道着如何在旋转矩阵中进行微积分运算。

4.3 李代数求导与扰动模型

1. BCH公式和近似形式

$ln(exp(\pmb{A})$^ $exp(\pmb{B})$^ $)=\pmb{A}+\pmb{B}+\dfrac{1}{2} [\pmb{A},\pmb{B}]$

$ln(exp(\phi_1$^ $)exp(\phi_2$^ $))^V$ $\approx \left\{\begin{aligned} \pmb{J}_l(\phi_2)^{-1}\phi_1+\phi_2 &&当\phi_1为小量\\\pmb{J}_r(\phi_1)^{-1}\phi_2+\phi_1 &&当\phi_2为小量 \end{aligned}\right.$

以左乘为例，假定某个旋转 $\pmb{R}$，对应的李代数为 $\phi$，左乘一个 $\Delta\pmb{R}$，对应的李代数是 $\Delta \phi$。

• 李群： $\Delta\pmb{R} \cdot \pmb{R}$
  李代数： $exp(\Delta\phi$^ $)exp(\phi$^ $)= exp((\phi+\pmb{J}_l(\phi)\Delta\phi)$ ^ $)$

李代数加法近似于李群上带左右雅可比的乘法

• 李代数： $exp((\phi+\Delta\phi)$ ^ $)$ $= exp((\pmb{J}_l\Delta\phi)$^ $) exp(\phi$^ $)$

2. $SO(3)$李代数求导

对空间 $p$点进行旋转，得到 $\pmb{Rp}点，若求点坐标对旋转的倒数，可不严谨的记为$ $\dfrac{\partial \pmb{Rp}}{\partial \pmb{R}}$
因为 $SO(3)$没有加法，设 $\pmb{R}$的李代数为 $\phi$，转而计算 $\dfrac{\partial (exp(\phi)^{>}\pmb{p} )}{\partial \phi}$

• 求导模型 $\dfrac{\partial \pmb{Rp}}{\partial \phi}=(-\pmb{Rp})$^ $\pmb{J}_l$

• 扰动模型(左乘) $\dfrac{\partial \pmb{Rp}}{\partial \psi}=(-\pmb{Rp})$^

3. 为什么要求导？
   我们经常会构建与位姿有关的函数，然后讨论该函数关于位姿的导数，以调整当前估计值。
   $\pmb{T }$为位姿， $\pmb{p}$世界点， $\pmb{z}$观测点， $\pmb{w}$观测噪声 ， $\pmb{e}$为误差
   $\pmb{z}=\pmb{T }\pmb{p}+\pmb{w}$
   $\pmb{e}=\pmb{z}-\pmb{T }\pmb{p}$
   假设有N个路标点和观测，就有N个误差，所谓的位姿估计就相当于找一个最优的 $\pmb{T}$，使得整体误差最小化：
   $\underset {T}{min} J(\pmb{T})=\displaystyle\sum_{i=1}^N||\pmb{z}_i-\pmb{T }\pmb{p}_i||^2$

5 相机模型

5.1 针孔相机

现实世界空间点 $P$坐标： $[X,Y,Z]^T$
物理成像平面 $P'$坐标： $[X',Y',Z']^T$ $(Z'$就是 $f)$
像素平面上 $P'$的像素坐标： $[u,v]^T$

• 将倒像对称为正像，研究正像模型
  $\dfrac{Z}{f} = \dfrac{X}{X'} = \dfrac{Y}{Y'}$

• $X' = f\dfrac{X}{Z}$

• $Y' = f\dfrac{Y}{Z}$

• 像素坐标系与成像平面之间，相差了一个缩放和一个原点的平移。
  设像素坐标在 $u$轴上缩放了 $α$倍，在 $v$上缩放了 $β$倍。同时，原点平移了 $[c_x,c_y]^T$。那么， P′ 的坐标与像素坐标 $[u,v]^T$的关系为： $\left\{\begin{aligned} u = α X' +c_x\\ v = β Y' +c_y \end{aligned}\right..$ 把 $\pmb{αf}$合并为 $\pmb{f_x}$，把 $\pmb{βf}$合并为 $\pmb{f_y}$，得

• $\left\{\begin{aligned} u = f_x\dfrac{X}{Z} +c_x\\ v = f_y\dfrac{Y}{Z} +c_y \end{aligned}\right..$
  写成矩阵形式：

• $Z\left(\begin{matrix}u\\v\\1\end{matrix}\right) = \left(\begin{matrix}f_x & 0 & c_x\\ 0 &f_y& c_y\\0 &0&1\end{matrix}\right)\left(\begin{matrix}X\\Y\\Z\end{matrix}\right) = \pmb{KP}$

中间矩阵为相机的内参矩阵 K， $f$单位为米， $α,β$单位为像素 / 米， $f_x,f_y,c_x,c_y$单位为像素。

上面中的 $\pmb{P}$是相机坐标系，当给定世界坐标系时 $\pmb{P_w}$，需要根据相机当前位姿 $(\pmb{R,t})$变换到相机坐标系
$Z\pmb{P_{uv}} = Z\left[\begin{matrix}u\\v\\1\end{matrix}\right] = \pmb{K(RP_w+t)} = \pmb{KTP_w}$

相机的位姿 $(\pmb{R,t})$称为相机的外参数，在机器人或自动驾驶车辆中，外参有时也解释成相机坐标系到机器人本体坐标系之间的变换，描述“相机安装在什么地方”

• 注意后一个式子隐含了一次齐次坐标到非齐次坐标的转换在 $\pmb{T}$ $\pmb{P}$中使用齐次坐标，再转化为非齐次坐标，再与 $\pmb{K}$相乘。

投影过程还可以从另一个角度来看。我们可以把一个世界坐标点先转换到相机坐标系，再除掉它最后一维的数值（即该点距离相机成像平面的深度），这相当于把最后一维进行归一化处理，得到点 P 在相机归一化平面上的投影： $(\pmb{RP_w+t}) =\underbrace{[X,Y,Z]^T}_{\text{相机坐标}} \rightarrow\underbrace{[\dfrac{X}{Z},\dfrac{Y}{Z},1]^T}_{\text{归一化坐标}}$

• 归一化坐标可看成相机前方z = 1 处的平面上的一个点，这个 z = 1 平面也称为归一化平面。归一化坐标再左乘内参就得到了像素坐标，所以我们可以把像素坐标 $[u,v]^T$看成对归一化平面上的点进行量化测量的结果。

5.2 畸变 Distortion

• 径向畸变：由透镜形状引起的畸变，主要分为桶形畸变和枕形畸变两类
• 切向畸变：在相机的组装过程中由于不能使透镜和成像面严格平行

1. 将三维空间点投影到归一化图像平面。设它的归一化坐标为 $[x,y]^T$。
2. 对归一化平面上的点计算径向畸变和切向畸变。
   $\left\{\begin{aligned} x_{distorted} = x(1+k_1r^2+k_2r^4+k_3r^6)+2p_1xy+p_2(r^2+2x^2)\\ y_{distorted} = y(1+k_1r^2+k_2r^4+k_3r^6)+p_1(r^2+2y^2)+2p_2xy \end{aligned}\right.$
3. 将畸变后的点通过内参数矩阵投影到像素平面，得到该点在图像上的正确位置。
   $\left\{\begin{aligned} u = f_xx_{distorted}+c_x\\ v = f_yy_{distorted} +c_y \end{aligned}\right.$

5.3 单目相机

小结：

1. 首先，世界坐标系下有一个固定的点 $\pmb{P}$，世界坐标为 $\pmb{P_w}$
2. 由于相机在运动，它的运动由 $\pmb{R,t}$ 或变换矩阵 $\pmb{T}\in SE(3)$ 描述。 $P$的相机坐标为 $\tilde{\pmb{P_c}} = \pmb{RP_w+t}$。
3. 这时的 $\tilde{\pmb{P_c}}$ 的分量为 $X,Y,Z$，把它们投影到归一化平面 $Z = 1$上，得到 $P$ 的归一化坐标： $\pmb{P_c} = [X/Z,Y/Z,1]^T$
4. 有畸变时，根据畸变参数计算 $\pmb{P_c}$发生畸变后的坐标。
5. 最后， $P$的归一化坐标经过内参后，对应到它的像素坐标： $\pmb{P_{uv}} = \pmb{KP_c}$

• 世界坐标、相机坐标、归一化坐标、像素坐标

5.4 双目相机

• 基线 baseline：都位于 x 轴上的两个相机的光圈中心间的距离 b

• 视差 disparity：左右图的横坐标之差 d

• $z = \dfrac{fb}{d}$

• $d = u_L-u_R$

5.5 RGB-D相机

1. 通过红外结构光（ Structured Light）来测量像素距离的。例子有 Kinect 1 代、 Project Tango1代、Intel RealSense 等。
2. 通过飞行时间法（ Time-of-flight，ToF）原理测量像素距离的。例子有Kinect 2 代和一些现有的ToF传感器等。

5.6 图像

待补充。。。

6 非线性优化

讨论如何在有噪声的数据中进行准确的状态估计

6.1 状态估计问题

1. $\left\{ \begin{aligned} x_k&=f(x_{k-1},u_k)+w_k &\text{运动方程}\\ z_{k,j}&=h(y_j,x_k)+v_{k,j} &\text{观测方程} \end{aligned} \right.$

$x_k$	$y_j$	$z_{k,j}$	$w_k$， $v_{k,j}$	$u_k$
位姿	路标	像素位置	高斯噪声	输入,在视觉中暂且不谈

$w_k$~ $N(0,R_k)$
$v_k$~ $N(0,Q_{k,j})$， $R_k，Q_{k,j}$为协方差矩阵

• 我们希望通过带噪声的数据 $z$ 和 $u$ 推断位姿 $x$ 和地图 $y$（以及它们的概率分布）

2. 增量法（滤波器）：数据是随时间逐渐到来的，持有一个当前时刻的估计状态，用新的数据来更新它。仅关心当前时刻的状态估计 $x_k$，而对之前的状态则不多考虑
   批量法：则是把数据“攒”起来一并处理,可以在更大的范围达到最优化，是当前视觉 SLAM 的主流方法
   SfM（ Structure from Motion）：让机器人或无人机收集所有时刻的数据，再带回计算中心统一处理，是不实时的

3.对机器人状态的估计就是已知输入数据 $u$ 和观测数据 $z$ 条件下，求状态 $x,y$ 的条件概率分布： $P(x,y|z,u)$
直接求后验分布是困难的，但可以求出使得后验概率的最大化的一个状态（MAP）:
$(x,y)_{MAP}^* = arg max P(x,y|z,u)$
由贝叶斯法则：
$\underbrace{P(x,y|z,u)}_{\text{后验}} = \dfrac{P(z,u|x,y)P(x,y)}{P(z,u)} \propto \underbrace{P(z,u|x,y)}_{\text{似然}}\underbrace{P(x,y)}_{\text{先验}}$

所以求 $(x,y)_{MAP}^* = arg max P(x,y|z,u)$解最大后验概率等价于最大化似然和先验的乘积，进一步，若我不知道机器人位姿或路标大概在什么地方，此时就没有了先验。那么，可以求解最大似然估计（MLE）
$(x,y)_{MLE}^* = arg max P(z,u|x,y)$

直观讲，似然是指“在现在的位姿下，可能产生怎样的观测数据”。由于我们知道观测数据，所以最大似然估计可以理解成：“在什么样的状态下，最可能产生现在观测到的数据”。

6.2 最小二乘问题

• 对于某一次观测 $z_{k,j}=h(y_j,x_k)+v_{k,j}$

• 观测数据的条件概率 $P(z_{j,k}|x_k,y_j)=N(h(y_j,x_k),Q_{k,j})$

• 这是一个高斯分布，可用最小化负对数求该分布的最大似然
  高斯分布展开后对其取负对数，因为对数函数是单调递增的，所以对原函数求最大化相当于对负对数求最小化。
  $(x_k,y_j)^* = arg max N(h(y_j,x_k),Q_{k,j})$

• 我们发现，该式等价于最小化噪声项（即误差）的一个二次型。这个二次型称为马哈拉诺比斯距离，又叫马氏距离。它也可以看成是由 $Q_{k,j}^{-1}$ 加权之后的欧氏距离（二范数），这里 $Q_{k,j}^{-1}$ 也叫做信息矩阵，即高斯分布协方差矩阵之逆。
  $(x_k,y_j)^*=arg min ((z_{k,j}-h(x_k,y_j))^TQ_{k,j}^{-1}(z_{k,j}-h(x_k,y_j)))$

• 批量时刻的数据通常假设各个时刻的输入和观测是相互独立的，对联合分布进行因式分解： $P(z,u|x,y) = \displaystyle{\prod_{k}}P(u_k|x_{k-1},x_k)\prod_{k,j}P(z_{k,j}|x_k,y_j)$
  定义各次输入和观测数据与模型之间的误差：
  $e_{u,k}=x_k-f(x_{k-1},u_k)$
  $e_{z,j,k}=z_{k,j}-h(x_k,y_j)$

• 最小化所有时刻估计值与真实读数之间马氏距离，等价于求最大似然估计。负对数允许我们把乘积变成求和：
  $minJ(x,y) = \sum_k e_{u,k}^TR_k^{-1}e_{u,k}+\sum_k \sum_j e_{z,k,j}^TQ_{k,j}^{-1}e_{z,k,j}$

这样就得到了一个最小二乘问题（ Least Square Problem）

由于噪声的存在，当我们把估计的轨迹与地图代入 SLAM 的运动、观测方程中时，它们并不会完美地成立。这时怎么办呢？我们对状态的估计值进行微调，使得整体的误差下降一些。当然这个下降也有限度，它一般会到达一个极小值。这就是一个典型非线性优化的过程

6.3 非线性最小二乘

考虑一个简单的最小二乘问题： $\underset{x}{min} F(x) = \frac{1}{2}||f(x)||_2^2$
注意这里的系数 $\frac{1}{2}$ 是无关紧要的，有些文献上带有这个系数，有些文献则不带，它也不会影响之后的结论。

• 下面讨论如何求解这样一个优化问题
• 如果 $f$ 是个数学形式上很简单的函数，那么该问题可以用解析形式来求。令目标函数的导数为零，然后求解 x 的最优值，就和求二元函数的极值一样
• 对于不方便直接求解的最小二乘问题，我们可以用迭代的方式，从一个初始值出发，不断地更新当前的优化变量，使目标函数下降。

1. 给定某个初始值 $x_0$
2. 对于第 $k$次迭代，寻找一个增量 $\Delta x_k$，使得 $∥f (x_k + \Delta x_k)∥_2^2$达到极小值。
3. 若 $\Delta x_k$ 足够小，则停止。
4. 否则，令 $x_{k+1} = x_k + ∆x_k$，返回第 2 步。

• 这让求解导函数为零的问题变成了一个不断寻找下降增量 $∆x_k$ 的问题，接下来我们考察如何寻找这个增量 $∆x_k$。

6.3.1 一阶和二阶梯度法

现在考虑第 $k$次迭代，假设我们在 $x_k$ 处，想要寻到增量 $∆x_k$，那么最直观的方式是将目标函数在 $x_k$ 附近进行泰勒展开：
$F(x_k + ∆x_k) ≈ F(x_k) + J (x_k)^T ∆x_k + \frac{1}{2}∆x_k^TH(x_k)∆x_k$
其中 $J(x_k)$ 是 $F(x)$ 关于 $x$ 的一阶导数(也叫梯度、雅可比矩阵) $H$ 则是二阶导数(海塞矩阵)

• 一阶梯度法
  保留泰勒展开的一阶项，取增量为反向的梯度，即可保证函数下降
  $∆x^∗ = −J(x_k)$
  这只是个方向，通常我们还要再指定一个步长 λ。这种方法被称为最速下降法。它的直观意义是只要我们沿着反向梯度方向前进，在一阶（线性）的近似下，目标函数必定会下降。

• 二阶梯度法
  $∆x^∗ = arg mmin(F(x_k)+J(x_k)^T∆x + \frac{1}{2}∆x_k^TH∆x_k)$
  求右侧等式关于 ∆x 的导数并令它为零,得
  $J + H∆x = 0 \Rightarrow H∆x = −J$
  求解这个线性方程，就得到了增量,该方法又称为牛顿法。

• 最速下降法过于贪心，容易走出锯齿路线，反而增加了迭代次数。
  牛顿法则需要计算目标函数的 $H$ 矩阵，这在问题规模较大时非
  常困难，我们通常倾向于避免 $H$ 的计算。

6.3.2 高斯牛顿法

将 $f(x)$ 进行一阶的泰勒展开
$f(x + ∆x) ≈ f(x) + J (x_k)^T ∆x_k$
代人展开求导后令其为零得
$J(x)f(x)+J(x)J^T(x)∆x = 0$

$\underbrace{J(x)J^T(x)}_{H(x)} ∆x = \underbrace{-J(x)f(x)}_{g(x)}$

该方程是关于变量 $∆x$ 的线性方程组，称为增量方程，或高斯牛顿方程，或正规方程。把左边的系数定义为 $H$，右边定义为 $g$，那么上式变为： $H∆x=g$.
对比牛顿法可见，高斯牛顿法用 $JJ^T$ 作为牛顿法中二阶Hessian矩阵的近似，从而省略了计算 $H$ 的过程。 求解增量方程是整个优化问题的核心所在。

1. 给定某个初始值 $x_0$。
2. 对于第 $k$次迭代，求出当前的雅可比矩阵 $J(x_k)$ 和误差 $f(x_k)$。
3. 求解增量方程： $H∆x=g$。
4. 若 $\Delta x_k$ 足够小，则停止。 否则，令 $x_{k+1} = x_k + ∆x_k$，返回第 2 步。

• 在使用高斯牛顿法时,可能出现 $JJ^T$ 为奇异矩阵或者病态的情况，此时增量的稳定性较差，导致算法不收敛。尽管高斯牛顿法有这些缺点，但它依然算是非线性优化方面一种简单有效的方法，值得我们去学习。在非线性优化领域，相当多的算法都可以归结为高斯牛顿法的变种。

6.3.3 列文伯格—马夸尔特方法

• 给 $∆x$ 添加一个范围，称为信赖区域（ Trust Region）。这个范围定义了在什么情况下二阶近似是有效的，这类方法也称为信赖区域方法（Trust Region Method）。在信赖区域里边，我们认为近似是有效的，出了这个区域，近似可能会出问题。
• 待补充
• 在实际中，还存在许多其他的方式来求解增量，例如 Dog-Leg等方法

6.3.4 图优化

• 图优化，是把优化问题表现成图（ Graph） 的一种方式。这里的图是图论意义上的图。一个图
  由若干个顶点（ Vertex），以及连接着这些顶点的边（ Edge） 组成。进而，用顶点表示优化变量，用边表示误差项。于是，对任意一个上述形式的非线性最小二乘问题，我们可以构建与之对应的一个图。我们可以简单地称它为图，也可以用概率图里的定义，称之为贝叶斯图或因子图。
• 图6-2是一个简单的图优化例子。我们用三角形表示相机位姿节点，用圆形表示路标点，它们构成了图优化的顶点；同时，实线表示相机的运动模型，虚线表示观测模型，它们构成了图优化的边。此时，虽然整个问题的数学形式仍是最小二乘那样，但现在我们可以直观地看到问题的结构了。如果希望，也可以做去掉孤立顶点或优先优化边数较多（或按图论的术语，度数较大）的顶点这样的改进。但是最基本的图优化是用图模型来表达一个非线性最小二乘的优化问题。而我们可以利用图模型的某些性质做更好的优化。