史上最简SLAM零基础解读(7) - Jacobian matrix(雅可比矩阵) → 理论分析与应用详解(Bundle Adjustment)

本人讲解关于slam一系列文章汇总链接:史上最全slam从零开始
 
$文末正下方中心提供了本人联系方式，点击本人照片即可显示WX\to 官方认证$
 

一、前言

该篇博客主要讲解→雅可比矩阵定义、推导、以及其应用，并且有相应的示例解读
         雅可比在函数行列式方面有一篇著名的论文：《论行列式的形成与性质》(1841)。文中求出了函数行列式的导数公式；还利用函数行列式作工具证明了，函数之间相关或无关的条件是雅克比行列式等于零或不等于零。他又给出了雅可比行列式的乘积定理。
         在该论文中针对 $n$ 个 $n$ 元函数的相关性，提出了所谓的雅可比行列式，如果这个行列式不为 0 ，则这些函数是独立的。因此，他提出的也叫函数行列式。虽然他没有单独提出雅克比矩阵，但是等到后来有了矩阵的概念，人们还是将其称为雅可比矩阵。雅可比行列式是坐标变换理论的基础之一，在数学分析隐函数理论中发挥着重要作用。雅克比矩阵定义如下所示：
$$\begin{array}{rlr}{\mathbf{J}}_F& =\left[\begin{array}{ccc}\frac{\partial \mathbf{f}}{\partial x_1}& \cdots & \frac{\partial \mathbf{f}}{\partial x_n}\end{array}\right]& =\left[\begin{array}{cccc}\frac{\partial f_1}{\partial x_1}& \frac{\partial f_1}{\partial x_2}& \cdots & \frac{\partial f_1}{\partial x_n}\\ ⋮& \ddots & ⋮& \\ \frac{\partial f_m}{\partial x_1}& \frac{\partial f_m}{\partial x_2}& \cdots & \frac{\partial f_m}{\partial x_n}\end{array}\right]\end{array}\text{\hspace{1em}(01)}$$那么这个矩阵是如何来的，又有什么作用呢？不要着急，下面就来一一分析。
 

二、基本理论

1、初步了解

假设 $F:{\mathbb{R}}^{\mathrm{n}}\to {\mathbb{R}}^{\mathrm{m}}$ 是一个从 $n$ 维欧氏空间映射到 $m$ 维欧氏空间的函数。这个函数由 $m$ 个实函数组成：$$y_1\left(x_1,\cdots ,x_n\right),\cdots ,y_m\left(x_1,\cdots ,x_n\right)\text{\hspace{1em}(02)}$$这些函数的偏导数(如果存在)可以组成一个$m$ 行 $n$ 列的矩阵，这个矩阵就是所谓的雅可比矩阵，如下所示：$$\left[\begin{array}{cccc}\frac{\partial y_1}{\partial x_1}& \frac{\partial y_1}{\partial x_2}& \cdots & \frac{\partial y_1}{\partial x_n}\\ ⋮& \ddots & ⋮& \\ \frac{\partial y_m}{\partial x_1}& \frac{\partial y_m}{\partial x_2}& \cdots & \frac{\partial y_m}{\partial x_n}\end{array}\right]\text{\hspace{1em}(03)}$$此矩阵用符号为：${\mathbf{J}}_F(x_1,x_2,...,x_n)$，或者$\frac{\partial \left(y_1,\dots ,y_m\right)}{\partial \left(x_1,\dots ,x_n\right)}$，另外这个矩阵的第 $i$ 行 $y_i(i=1,...,m)$ 是由梯度函数的转置表示的，如果 $p$ 是 ${\mathbb{R}}^{\mathrm{n}}$ 中的一点，$F$ 在 $p$ 点可微分，根据高等微积分，${\mathbf{J}}_F(p)$ 是在这点的导数。在此情况下，${\mathbf{J}}_F(p)$ 这个线性映射即 $F$ 在点 $p$ 附近最优线性逼近，也就是说当 $x$ 足够靠近点 $p$ 时，有$$F(x)\approx F(p)+{\mathbf{J}}_F(p)\cdot (x-p)\text{\hspace{1em}(04)}$$上述公式可以与泰勒展开进行拟比，下面进行一个深层次的理解。
 

2、深度解析

若在 $n$ 维欧式空间中的一个向量映射成 $m$ 维欧式空间中的另一个向量的对应法则为 $F$，$F$ 由 $m$ 个实函数组成，即：$$F=\{\begin{array}{rl}{y}_1=& f_1\left(x_1,\dots ,x_n\right)\\ y_2=& f_2\left(x_1,\dots ,x_n\right)\\ & \dots \\ y_m=& f_m\left(x_1,\dots ,x_n\right)\end{array}\text{\hspace{1em}(05)}$$那么雅可比矩阵是一个 $m\times n$ 矩阵：$$\begin{array}{rlr}{\mathbf{J}}_F& =\left[\begin{array}{ccc}\frac{\partial \mathbf{f}}{\partial x_1}& \cdots & \frac{\partial \mathbf{f}}{\partial x_n}\end{array}\right]& =\left[\begin{array}{cccc}\frac{\partial f_1}{\partial x_1}& \frac{\partial f_1}{\partial x_2}& \cdots & \frac{\partial f_1}{\partial x_n}\\ ⋮& \ddots & ⋮& \\ \frac{\partial f_m}{\partial x_1}& \frac{\partial f_m}{\partial x_2}& \cdots & \frac{\partial f_m}{\partial x_n}\end{array}\right]\end{array}\text{\hspace{1em}(06)}$$其中，雅克比矩阵就是这个变换对应的偏导而组成的矩阵。输入 $n$ 维向量 $x=(x_1,...,x_n)$，输出 $m$ 维向量 $y=(y_1,...,y_m)$，$x\in {\mathbb{R}}_n$，$y=F(x)\in {\mathbb{R}}_m$。如果 $p$ 是中的一个点，$F$ 在 $p$ 点可微分，根据数学分析，雅可比矩阵 ${\mathbf{J}}_F(p)$ 是在这点的导数，在此情况下，${\mathbf{J}}_F(p)$ 这个线性映射即 $F$ 在点 $p$ 附近最优线性逼近，也就是说当 $x$ 足够靠近点 $p$ 时，有$$F(x)\approx F(p)+{\mathbf{J}}_F(p)\cdot (x-p)\text{\hspace{1em}(07)}$$当 $m=n$ 时，矩阵 ${\mathbf{J}}_F(p)$ 就会变成一个方正，$F$ 就变成从 $n$ 维欧式空间到 $n$ 维欧式空间的映射，方阵的行列式就是雅可比行列式。上式移项得：
$$F(x)-F(p)\approx {\mathbf{J}}_F(p)\cdot (x-p)\text{\hspace{1em}(08)}$$即可的($\Delta y$ 与 $\Delta x$ 都是 $n$ 维空间的向量)：$$\Delta y\approx {\mathbf{J}}_F(\mathrm{p})\cdot \Delta x\text{\hspace{1em}(09)}$$ 则使用微分形式表示如下：$$\mathbf{d}y={\mathbf{J}}_F(p)\mathbf{d}x\text{\hspace{1em}(10)}$$其中 $$\mathbf{d}y=(\mathbf{d}{y}_1,...,\mathbf{d}{y}_n{)}^T\mathbf{d}x=(\mathbf{d}{x}_1,...,\mathbf{d}{x}_n{)}^T\text{\hspace{1em}(11)}$$即$$\left[\begin{array}{c}\mathbf{d}{y}_1\\ \mathbf{d}{y}_2\\ ⋮\\ \mathbf{d}{y}_n\end{array}\right]=\left[\begin{array}{cccc}\frac{\partial f_1}{\partial x_1}& \frac{\partial f_1}{\partial x_2}& \cdots & \frac{\partial f_1}{\partial x_n}\\ \frac{\partial f_2}{\partial x_1}& \frac{\partial f_2}{\partial x_2}& \cdots & \frac{\partial f_2}{\partial x_n}\\ ⋮& ⋮& \ddots & ⋮\\ \frac{\partial f_n}{\partial x_1}& \frac{\partial f_n}{\partial x_2}& \cdots & \frac{\partial f_n}{\partial x_n}\end{array}\right]\left[\begin{array}{c}\mathbf{d}{x}_1\\ \mathbf{d}{x}_2\\ ⋮\\ \mathbf{d}{x}_n\end{array}\right]\text{\hspace{1em}(12)}$$$$\left[\begin{array}{c}\mathbf{d}{y}_1\\ \mathbf{d}{y}_2\\ ⋮\\ \mathbf{d}{y}_n\end{array}\right]=\left[\begin{array}{c}\frac{\partial f_1}{\partial x_1}\mathbf{d}{x}_1+\frac{\partial f_1}{\partial x_2}\mathbf{d}{x}_2+\cdots +\frac{\partial f_1}{\partial x_n}\mathbf{d}{x}_n\\ \frac{\partial f_2}{\partial x_1}\mathbf{d}{x}_1+\frac{\partial f_2}{\partial x_2}\mathbf{d}{x}_2+\cdots +\frac{\partial f_2}{\partial x_n}\mathbf{d}{x}_n\\ ⋮\\ \frac{\partial f_n}{\partial x_1}\mathbf{d}{x}_1+\frac{\partial f_n}{\partial x_2}\mathbf{d}{x}_2+\cdots +\frac{\partial f_n}{\partial x_n}\mathbf{d}{x}_n\end{array}\right]\text{\hspace{1em}(13)}$$
将上述向量写成基于正交的单位向量的形式：$$\left[\begin{array}{cccc}\mathbf{d}{y}_1& 0& \cdots & 0\\ 0& \mathbf{d}{y}_2& \cdots & 0\\ 0& 0& \ddots & 0\\ 0& 0& 0& \mathbf{d}{y}_n\end{array}\right]=\left[\begin{array}{cccc}\frac{\partial f_1}{\partial x_1}\mathbf{d}{x}_1& \frac{\partial f_1}{\partial x_2}\mathbf{d}{x}_2& \cdots & \frac{\partial f_1}{\partial x_n}\mathbf{d}{x}_n\\ \frac{\partial f_2}{\partial x_1}\mathbf{d}{x}_1& \frac{\partial f_2}{\partial x_2}\mathbf{d}{x}_2& \cdots & \frac{\partial f_2}{\partial x_n}\mathbf{d}{x}_n\\ ⋮& ⋮& \ddots & ⋮\\ \frac{\partial f_n}{\partial x_1}\mathbf{d}{x}_1& \frac{\partial f_n}{\partial x_2}\mathbf{d}{x}_2& \cdots & \frac{\partial f_n}{\partial x_n}\mathbf{d}{x}_n\end{array}\right]\text{\hspace{1em}(14)}$$行列式的绝对值表示微元的体积，左侧行列式一定为正，体积不可能为负，右侧行列式加绝对值$$\left|\begin{array}{cccc}\mathbf{d}{y}_1& 0& \cdots & 0\\ 0& \mathbf{d}{y}_2& \cdots & 0\\ 0& 0& \ddots & 0\\ 0& 0& 0& \mathbf{d}{y}_n\end{array}\right|=\Vert \begin{array}{cccc}\frac{\partial f_1}{\partial x_1}\mathbf{d}{x}_1& \frac{\partial f_1}{\partial x_2}\mathbf{d}{x}_2& \cdots & \frac{\partial f_1}{\partial x_n}\mathbf{d}{x}_n\\ \frac{\partial f_2}{\partial x_1}\mathbf{d}{x}_1& \frac{\partial f_2}{\partial x_2}\mathbf{d}{x}_2& \cdots & \frac{\partial f_2}{\partial x_n}\mathbf{d}{x}_n\\ ⋮& ⋮& \ddots & ⋮\\ \frac{\partial f_n}{\partial x_1}\mathbf{d}{x}_1& \frac{\partial f_n}{\partial x_2}\mathbf{d}{x}_2& \cdots & \frac{\partial f_n}{\partial x_n}\mathbf{d}{x}_n\end{array}\Vert \text{\hspace{1em}(15)}$$将公共因数提出
$$\left|\begin{array}{cccc}\mathbf{d}{y}_1& 0& \cdots & 0\\ 0& \mathbf{d}{y}_2& \cdots & 0\\ 0& 0& \ddots & 0\\ 0& 0& 0& \mathbf{d}{y}_n\end{array}\right|=\Vert \begin{array}{cccc}\frac{\partial f_1}{\partial x_1}& \frac{\partial f_1}{\partial x_2}& \cdots & \frac{\partial f_1}{\partial x_n}\\ \frac{\partial f_2}{\partial x_1}& \frac{\partial f_2}{\partial x_2}& \cdots & \frac{\partial f_2}{\partial x_n}\\ ⋮& ⋮& \ddots & ⋮\\ \frac{\partial f_n}{\partial x_1}& \frac{\partial f_n}{\partial x_2}& \cdots & \frac{\partial f_n}{\partial x_n}\end{array}\Vert \mathbf{d}{x}_1\mathbf{d}{x}_2\cdots \mathbf{d}{x}_n\text{\hspace{1em}(16)}$$即，此微元的体积为:$$\mathbf{d}{y}_1\mathbf{d}{y}_2\cdots \mathbf{d}{y}_n=|{\mathbf{J}}_F(p)|\mathbf{d}{x}_1\mathbf{d}{x}_2\cdots \mathbf{d}{x}_n\text{\hspace{1em}(17)}$$
 

三、应用示例→牛顿迭代法

1、一元数量函数

如果学习过高数或者数分，可以知道如何使用牛顿法来计算 $f(x)=0$ 的根，假设根在 $x_k$ 附近，为了找到更好的根 $x_{k+1}$，可以考虑如下$(x_k,f(x_k))$ 处的切线，如下图所示：

切线方程为：
$$y=f(x_k)+f^{\prime }(x_k)(x-x_k)\text{\hspace{1em}(18)}$$根据上图，可以很直观的看到，该切向与 x 轴的交点为$(x_k-\frac{f(x_k)}{f^{\prime }(x_k)},0)$，其揭示了比 $x_k$ 更接近的近似根，记其为 $x_{k+1}$，因此牛顿迭代法的公式为：
$$x_{k+1}=x_k-\frac{f\left(x_k\right)}{f^{\prime }\left(x_k\right)}\text{\hspace{1em}(19)}$$该方法的迭代过程如下图所示，实际操作中对初始值是有一定要求的，简而言之需要初始值在实际根附近。
 

2、多元数量函数

对于多元数量 $F:{\mathbb{R}}^{\mathrm{n}}\mapsto \mathbb{R}$，假设 ${\mathbf{x}}_k$ 接近根，那么下式：$$y-F\left({\mathbf{x}}_k\right)=\nabla F{\left({\mathbf{x}}_k\right)}^T\left(\mathbf{x}-{\mathbf{x}}_k\right)\text{\hspace{1em}(20)}$$是在点 ${\mathbf{x}}_k$ 处对函数 $f$ 的仿射逼近，此时可以沿着梯度所在方向找更近似的根，可以令：
$${\mathbf{x}}_{k+1}={\mathbf{x}}_k+\lambda \nabla F\left({\mathbf{x}}_k\right)\text{\hspace{1em}(21)}$$上式的 $\lambda$ 可以理解为逼近因子，控制逼近的步伐，通常为 $-\frac{F({\mathbf{x}}_k)}{|\nabla F\left({\mathbf{x}}_k\right){|}^2}$，也就是可以写成下式：$${\mathbf{x}}_{k+1}={\mathbf{x}}_k-\frac{F\left({\mathbf{x}}_k\right)}{{|\nabla F\left({\mathbf{x}}_k\right)|}^2}\nabla F\left({\mathbf{x}}_k\right)\text{\hspace{1em}(22)}$$

3、多元向量值函数

对于多元数量 $F:{\mathbb{R}}^{\mathrm{n}}\mapsto {\mathbb{R}}^m$，如前面的(05)式：$$F=\{\begin{array}{rl}{y}_1=& f_1\left(x_1,\dots ,x_n\right)\\ y_2=& f_2\left(x_1,\dots ,x_n\right)\\ & \dots \\ y_m=& f_m\left(x_1,\dots ,x_n\right)\end{array}\text{\hspace{1em}(23)}$$ 计算 $F(x)=0$ 的根，稍微一想，你会发现这个其实是在求解一个方程组的解，这里的方程一般来说是非线性的，当然也可以包括特例，线性方程组。如果是线性方程组，一般能一步到位，但如果是非线性的，往往不能一步到位，那就用迭代法一步一步逼近。假设 $x_k$ 在根附近，该点处函数泰勒一级展开(或者说函数的仿射变换)为：
$$F(x)-F(x_k)\approx {\mathbf{J}}_F(x_k)\cdot (x-x_k)\text{\hspace{1em}(24)}$$那么雅可比矩阵是一个 $m\times n$ 矩阵：$$\begin{array}{rlr}{\mathbf{J}}_F& =\left[\begin{array}{ccc}\frac{\partial \mathbf{f}}{\partial x_1}& \cdots & \frac{\partial \mathbf{f}}{\partial x_n}\end{array}\right]& =\left[\begin{array}{cccc}\frac{\partial f_1}{\partial x_1}& \frac{\partial f_1}{\partial x_2}& \cdots & \frac{\partial f_1}{\partial x_n}\\ ⋮& \ddots & ⋮& \\ \frac{\partial f_m}{\partial x_1}& \frac{\partial f_m}{\partial x_2}& \cdots & \frac{\partial f_m}{\partial x_n}\end{array}\right]\end{array}\text{\hspace{1em}(25)}$$如果 $n=m$，此时牛顿法的迭代公式为，$$x_{k+1}=x_k-{\left[{\mathbf{J}}_{F\left(x_k\right)}\right]}^{-1}F\left(x_k\right)\text{\hspace{1em}(26)}$$上面公式里要计算雅可比矩阵的逆，一般并不会直接去求，而是通过求解下面的方程组:$${\mathbf{J}}_{F\left(x_k\right)}\Delta x=-F\left(x_k\right)\text{\hspace{1em}(27)}$$然后更新$$x_{k+1}=x_k+\Delta x\text{\hspace{1em}(28)}$$这里所谓的迭代式求根方法的主要思路就是从当前的近似解如何得到更近似的解。牛顿法就是利用函数局部的仿射逼近来一步一步逼近根。当然，具体实施中还需要处理初始值等一些细节问题。
 

四、应用示例→Bundle Adjustment

1、Gauss-Newton 非线性优化方法

对于一个函数 $f(x)$，要求其最小值以及最小值对应的 $x^{\ast }$，如果优化问题不方便直接求解，那么通常采用 ‘‘迭代’’ 思想进行求解。即：从一个初始值出发，不断更新当前的优化量，使目标函数值降低，直到优化量的增量足够小或者目标函数值达到要求。其中，使用这种迭代思想的 “Gauss-Newton法” 应用十分广泛。接下来，对高斯牛顿法的基本思想进行简单阐述和数学推导：

已知函数 $f(x)$ 至少二阶段可微，那么求解 $mi{n}_{x}f(x)$ 的时候可以进行一阶泰勒近似：
$$f(x+\Delta x)\approx f(x)+\mathbf{J}(x)\Delta x\text{\hspace{1em}(29)}$$其中 $\mathbf{J}(x)$ 是 $f(x)$ 关于 $x$ 的一阶偏导，也就是前面讲解的雅克比矩阵。根据迭代思想可知→在是为了找到 $f(x)$ 的下降方向 $\Delta x$ 使得函数值逐步达到最小值，因此可以构建一个关于 $\Delta x$ 的非线性化最小二乘问题：
$$\Delta x^{\ast }=\arg \underset{x}{\min }\frac{1}{2}\Vert f(x)+\mathbf{J}(x)\Delta x{\Vert }^2\text{\hspace{1em}(30)}$$对上式进行关于 $\Delta x$ 的求导，一阶导数为0时，当前值就是最优的$\Delta x^{\ast }$，即：$${\mathbf{J}}^T(x)\mathbf{J}(x)\Delta x=-{\mathbf{J}}^T(x)f(x)\text{\hspace{1em}(31)}$$因为此时的目标变量是 $\Delta x$，所以(03)可以看成是关于 $\Delta x$ 的线性方程，也称为增量方程。通常将 $\mathbf{H}\approx {\mathbf{J}}^T\mathbf{J}$ 的作为上式的二阶偏导数，即 $Hessian$ 矩阵的近似。避免了二阶导数的计算。Gauss-Newton迭代算法过程总结如下：
$(1)$ 给定初始值 $x_0$。
$(2)$ 对于第 $k$ 次迭代，求出当前的雅克比矩阵 $\mathbf{J}$
$(3)$ 根据 ${\mathbf{J}}_{F\left(x_k\right)}\Delta x=-F\left(x_k\right)$方程求解 $\Delta x$
$(4)$ 若 $\Delta x$足够小，则停止迭代，否则 $x_{k+1}=x_k+\Delta x_k$，并且返回步骤(2)。
 

2、Bundle Adjustment

对于SLAM，待优化的量包括：$位姿$ 和 $空间点3D位置$。如果采用 ‘‘线性’’ 求解，通常先求解位姿，再根据位姿来求解空间点的3D位置；如果采用“非线性法”，比如Gauss-Newton法，通常是将位姿和空间点位置构成统一的状态空间，进行联合优化。

如上图，已知大 $P$ 世界坐标系下的一点，通过 $P_w\to P_c\to p_{uv}$ 多次转换最终投影到像素坐标系上。用李代数 ${\xi }^{\wedge }$ 表示世界坐标系到相机坐标系的转换。并且：$$P_w=\left[\begin{array}{l}{X}_w\\ Y_w\\ Z_w\end{array}\right],P_c=\left[\begin{array}{l}{X}_c\\ Y_c\\ Z_c\end{array}\right],p_{uv}=\left[\begin{array}{l}\mu \\ v\\ 1\end{array}\right]\text{\hspace{1em}(32)}$$
$注意:$ 世界系→相机系→像素系 的转换过程一定要注意矩阵的维度，会涉及很多坐标的齐次化处理，如果公式中有维度的问题，就是没有将坐标齐次化处理展示，为避免造成误导，特此说明。通过李群李代数(后续补充相关章节)的学习，可知：$$\exp \left({\xi }^{\wedge }\right)=\left[\begin{array}{ll}\mathbf{R}& \mathbf{J}\mathbf{\rho }\\ 0^T& 1\end{array}\right]\text{\hspace{1em}(33)}$$其维度是 4x4，所以根据相机模型有：
$$Z_c\left[\begin{array}{l}u\\ v\\ 1\end{array}\right]=\mathbf{K}\exp \left({\xi }^{\wedge }\right)\left[\begin{array}{l}{X}_w\\ Y_w\\ Z_w\\ 1\end{array}\right]\text{\hspace{1em}(34)}$$其中 $Zc$ 是点 $P$ 的深度，常采用逆深度：简记为 $d=\frac{1}{Z_c}$。则上式可以记为：$$\left[\begin{array}{c}u\\ v\\ 1\end{array}\right]=d\mathbf{K}\exp \left({\xi }^{\wedge }\right)\left[\begin{array}{c}{X}_w\\ Y_w\\ Z_w\\ 1\end{array}\right]\text{\hspace{1em}(35)}$$即：$$p_{uv}=d\mathbf{K}\exp \left({\xi }^{\wedge }\right)P_w\text{\hspace{1em}(36)}$$
在基于特征法的SLAM中，通过ORB匹配或者光流跟踪，可以得到两帧间的像素匹配对。Bundle Adjustment 的思想就是通过特征匹配可以得到两帧间的像素匹配对的观测值；同时利用估计位姿将前帧像素点投影到后帧上得到像素点的估计值。如前面的图像所示，红点 $p_2$ 是通过光流(或其它方式)得到 $p_1$ 在当前帧上匹配对，也就是 $p_1$ 的观测值；黑色 $p_2$ 是通过估计位姿得到的估计值。因为估计位姿极有可能不准确，所以黑色 $p_2$ 和红色 $p_2$ 会有一定的差距，即观测值 != 估计值。那么就需要不断调整估计位姿 ${\xi }^{\wedge }$ 使得观测值和估计值的差距无线接近0。用数学语言描述为：

以 ${\xi }^{\wedge }$ 为优化量，$e\left({\xi }^{\wedge }\right)={\left(p_{uv}\right)}_{\text{estimation }}-{\left(p_{uv}\right)}_{\text{true }}$ 为待优化函数，建立最小二乘问题：
$${\xi }^{\ast }=\arg \underset{\xi }{\min }\frac{1}{2}\sum _{i=1}^n{\Vert p_i-d_i\mathbf{K}\exp \left({\xi }^{\wedge }\right)P_{wi}\Vert }_2^2\text{\hspace{1em}(37)}$$${\sum }_{i=1}^n$：因为两帧图像中存在多对像素匹配点，上图仅仅体现处理一对像素匹配点的关系。${\xi }^{\ast }$ 不仅仅是使一对像素点的匹配误差达到最小，而应该是考虑多对点配，考虑多对约束，使 ${\xi }^{\ast }$ 更健壮，即使不可能得到一个 ${\xi }^{\ast }$ 使得所有点对的像素重投影误差为零。

上式中的下标2，因为 $p_{uv}$ 的第三项永远为1，所以其重投影误差也为0。那么在后续处理中，可以只考虑前两项。
 

3、重投影误差模型的雅克比矩阵

根据前面的Gauss-Newton法，对 $e\left({\xi }^{\ast }\right)=p_i-d_i\mathbf{K}\exp \left({\xi }^{\wedge }\right)P_{wi}$进行一阶泰勒展开：
$$e\left((\xi +\Delta \xi {)}^{\wedge }\right)\approx e\left({\xi }^{\wedge }\right)+\mathbf{J}\left({\xi }^{\wedge }\right)\Delta {\xi }^{\wedge }\text{\hspace{1em}(38)}$$关键就在于 $\mathbf{J}\left({\xi }^{\wedge }\right)$，即上式对应雅克比矩阵的求解。为使结果简洁，所以通常选在“扰动模型”(涉及李群李代数)。此处使用“左乘扰动模型”，并使用链式求导准则。
$$\mathbf{J}\left({\xi }^{\wedge }\right)=\frac{\partial e}{\partial \Delta \xi }=\frac{\partial e}{\partial P_c}\cdot \frac{\partial P_c}{\partial \Delta \xi }\text{\hspace{1em}(39)}$$接下来，就一步一步求导，对于$\frac{\partial e}{\partial P_c}$，已知：$$p_i=\frac{1}{Z_i}\mathbf{K}{P}_{c}u=f_x\frac{X_c}{Z_c}+C_{x}v=f_y\frac{Y_c}{Z_c}+C_y\text{\hspace{1em}(40)}$$所以：
$$\frac{\partial e}{\partial P_c}=\frac{{\left[\partial p_i\right]}_2}{\partial P_c}=\frac{\left[\begin{array}{l}\partial u\\ \partial v\end{array}\right]}{\left[\begin{array}{l}\partial X_c\\ \partial Y_c\\ \partial Z_c\end{array}\right]}=\left[\begin{array}{ccc}\frac{\partial u}{\partial X_c}& \frac{\partial u}{\partial Y_c}& \frac{\partial u}{\partial Z_c}\\ & & \\ \frac{\partial v}{\partial X_c}& \frac{\partial v}{\partial Y_c}& \frac{\partial v}{\partial Z_c}\end{array}\right]={\left[\begin{array}{ccc}\frac{fx}{Z_c}& 0& -\frac{fx{X}_c}{Z_c^2}\\ & & \\ 0& \frac{fY}{Z_c}& -\frac{fy{Y}_c}{Z_c^2}\end{array}\right]}_{2\times 3}\text{\hspace{1em}(41)}$$
由李群李代数的扰动模型可知：$$\frac{\partial P_c}{\partial \Delta \xi }=\left[\begin{array}{cc}\mathbf{I}& -{\left(\mathbf{R}{P}_w+t\right)}^{\wedge }\\ & \\ 0^T& 0^T\end{array}\right]=\left[\begin{array}{cc}\mathbf{I}& -P_c^{\wedge }\\ & \\ 0^T& 0^T\end{array}\right]\text{\hspace{1em}(42)}$$
但是只取前三行$${\left[\frac{\partial P_c}{\partial \Delta \xi }\right]}_3={\left[\begin{array}{ll}\mathbf{I}& -P_c^{\wedge }\end{array}\right]}_{3\times 6}\text{\hspace{1em}(43)}$$由(41)与(43)式可以可得：$$\frac{\partial e}{\partial \Delta \xi }=\frac{[\partial e{]}_2}{\partial P_c}\cdot {\left[\frac{\partial P_c}{\partial \Delta \xi }\right]}_3=\left[\begin{array}{ccc}\frac{fx}{Z_c}& 0& \frac{fx{X}_c}{-Z_c^2}\\ & & \\ 0& \frac{fy}{Z_c}& \frac{f_y{Y}_c}{-Z_c^2}\end{array}\right]\cdot \left[\begin{array}{cccccc}1& 0& 0& 0& Z_c& -Y_c\\ 0& 1& 0& -Z_c& 0& X_c\\ 0& 0& 1& Y_c& -X_c& 0\end{array}\right]\text{\hspace{1em}(44)}$$最终可得：$$\frac{\partial e}{\partial \Delta \xi }=\left[\begin{array}{cccccc}\frac{f_x}{Z_c}& 0& -\frac{f_x{X}_c}{Z_c}& -\frac{f_x{X}_c{Y}_c}{Z_c^2}& f_x+\frac{f_x{X}_c^2}{Z_c^2}& -\frac{f_x{Y}_c}{Z_c}\\ 0& \frac{f_y}{Z_c}& -\frac{f_y{Y}_c}{Z_c^2}& -f_y-\frac{f_y{Y}_c^2}{Z_c^2}& \frac{f_y{X}_c{Y}_c}{Z_c^2}& \frac{f_y{X}_c}{Z_c}\end{array}\right]\text{\hspace{1em}(45)}$$如果需要将位姿和3D空间位置坐标进行联合优化，则还需要求解：$$\frac{\partial e}{\partial P_w}=\frac{\partial e}{\partial P_c}\frac{\partial P_c}{\partial P_w}\frac{\partial P_c}{\partial P_w}=\frac{\partial \left(R{P}_w+t\right)}{\partial P_w}=R\text{\hspace{1em}(46)}$$$$\frac{\partial e}{\partial P_w}=\left[\begin{array}{ccc}\frac{fx}{Z_c}& 0& \frac{f_x{X}_c}{-Z_c^2}\\ & & \\ 0& \frac{fy}{Z_c}& \frac{f_y{Y}_c}{-Z_c^2}\end{array}\right]R\text{\hspace{1em}(47)}$$至此，雅克比矩阵推导完毕。另外如果 se(3) 的定义方式是旋转在前，平移在后时，只要把这个(45)式矩阵的前三列与后三列对调即可。
 

五、结语

雅可比矩阵是一条比较好的能够将多个内容串起来的线索。简单来看，它能将矩阵、仿射变换、行列式、特征值特征向量、导数、泰勒展开、微分方程组、方程求根、最优化甚至流形及其上的度量张量等等内容有机地牵扯起来。总而言之，雅可比矩阵的应用十分广，后续有时间再为大家更新一些应用示例