视觉SLAM数学基础

本文系统梳理从相机成像模型，通过不同图像帧之间的构造几何约束求解位姿变换，再根据位姿变换和匹配点还原三维坐标的过程，可以作为基于特征点法的视觉SLAM的数学基础。

1、相机成像模型

1.1、针孔相机模型

实际相机的成像方式通常很复杂，针孔模型是对这种复杂成像方式的简化描述，其原理如下图

图中，$O-x-y-z$为相机坐标系{C}\{C\}{C}，其$x$轴，$y$轴位于针孔平面上，一般要求从相机后方看去$x$轴向右，$y$轴向下，$O$是针孔模型中的针孔，也称为摄像机的光心；$O^{\prime }-x^{\mathrm{img}}-y^{\mathrm{img}}$为物理成像坐标系，位于物理成像平面上。针孔平面与物理成像平面平行，二者之间距离为焦距$f$，也即$O{O}^{\prime }$的距离。习惯上，规定从相机后侧看时，$x,x^{\mathrm{img}}$轴向右，$y,y^{\mathrm{img}}$轴向下，$z$轴向前。

从相机坐标系下的空间点$\mathbf{p}=[X,Y,Z{]}^T$发出的光线，穿过针孔$O$，落在物理成像平面上，成为点$[X^{\mathrm{img}},Y^{\mathrm{img}}{]}^T$，考虑到$X$和$X^{\mathrm{img}}$，$Y$和$Y^{\mathrm{img}}$符号必定相反，根据三角形相似关系，可以得到
$$\frac{Z}{f}=-\frac{X}{X^{\mathrm{img}}}=-\frac{Y}{Y^{\mathrm{img}}}\text{\hspace{1em}(1)}$$
整理得到物理成像平面上的坐标
$$\begin{gathered} X^{\mathrm{img}}=-f\frac{X}{Z} \\ {Y}^{\mathrm{img}}=-f\frac{Y}{Z} \end{gathered}$$
负号表示成像为倒向，在实际成像过程中，为方便起见，经过算法内部调整为正像，即将物理成像平面上的坐标变为
$$\begin{array}{r}{X}^{\mathrm{img}}=f\frac{X}{Z}\\ Y^{\mathrm{img}}=f\frac{Y}{Z}\end{array}\text{\hspace{1em}(2a)(2b)}$$

1.2、像素坐标系

在相机中，最终获得的是$[X^{\mathrm{img}},Y^{\mathrm{img}}{]}^T$对应的像素坐标$[\lfloor U\rfloor ,\lfloor V\rfloor {]}^T$，其中
$$\{\begin{array}{rl}U& =\alpha X^{\mathrm{img}}+c_x\\ V& =\beta Y^{\mathrm{img}}+c_y\end{array}\text{\hspace{1em}(3)}$$
其中，$\alpha ,\beta$表示$u,v$轴上单位距离对应的像素数，$c_x,c_y$是相机光心的像素坐标
$$\{\begin{array}{rl}U& =f_x\frac{X^{}}{Z}+c_x\\ V& =f_y\frac{Y}{Z}+c_y\end{array}\text{\hspace{1em}(4)}$$
其中$f_x=\alpha f$，$f_y=\beta f$

根据以上关系，可以得到下式，其中左侧是齐次坐标，右侧是非齐次坐标
$$\left[\begin{array}{r}U\\ V\\ 1\end{array}\right]=\frac{1}{Z}\left[\begin{array}{ccc}{f}_x& 0& c_x\\ 0& f_y& c_y\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}X\\ Y\\ Z\end{array}\right]\text{\hspace{1em}(5)}$$
定义相机的内参数矩阵
$$\mathbf{K}=\left[\begin{array}{ccc}{f}_x& 0& c_x\\ 0& f_y& c_y\\ 0& 0& 1\end{array}\right]\text{\hspace{1em}(6)}$$
因为$f_x,f_y\ne 0$，容易看出，相机的内参数矩阵是可逆的，该矩阵通常右生产厂商标定，在出厂后不会再发生变化

设$\mathbf{q}=[U,V,1{]}^T$，则
$$\mathbf{q}=\frac{1}{Z}\mathbf{Kp}\text{\hspace{1em}(7)}$$
实际的像素坐标对求出的$\mathbf{q}$取整即可。

设初始相机坐标系{C0}\{C_{0}\}{C0​}为世界坐标系{W}\{W\}{W}，若已知{W}\{W\}{W}到相机坐标系相机坐标系{Ck},k⩾0\{C_{k}\},k\geqslant0{Ck​},k⩾0的位姿，当得到新一帧图像${\mathrm{Img}}_{k+1}$后，根据拍摄到的图像帧估计从相机坐标系{Ck}\{C_{k}\}{Ck​}到{Ck+1}\{C_{k+1}\}{Ck+1​}的旋转矩阵和平移向量${\mathbf{R}}_{c_{k+1}{c}_k},{\mathbf{t}}_{c_{k+1}{c}_k}^{c_{k+1}}$，这两个量在后文中简记作$\mathbf{R},\mathbf{t}$，是计算的目标

1.3、等价成像模型

成像模型可以做两处等价。

第一处等价：根据$(2)$，把“将倒向调整为正像的过程”等价为将成像平面对称地放到针孔平面前方，成像位置位于针孔$\mathbf{p}$与针孔$O$的连线与对称的成像平面的交点上；

第二处等价：根据$(7)$，成像过程可等价为先将空间点$\mathbf{p}=[X,Y,Z{]}^T$转换到归一化平面$z=1$上的$\overline{\mathbf{p}}$，即令
$$\overline{\mathbf{p}}={\left[\begin{array}{c}\overline{X},\overline{Y},1\end{array}\right]}^T={\left[\begin{array}{c}\frac{X}{Z},\frac{Y}{Z},1\end{array}\right]}^T\text{\hspace{1em}(8)}$$
再根据第一处等价投影到对称的成像平面，成为下面的坐标后再取整数
$$\mathbf{q}=\mathbf{K}\overline{\mathbf{p}}\text{\hspace{1em}(9)}$$
上述等价过程的示意图如下：

2、已知2D-2D匹配数据估算相机运动

2.1、2D-2D存在的约束

2.1.1、对极约束

设三维空间点$P$在两帧相机坐标系下坐标为${\mathbf{p}}_1,{\mathbf{p}}_2$，二者的归一化坐标为${\overline{\mathbf{p}}}_1,{\overline{\mathbf{p}}}_2$，这两个归一化坐标对应的空间点分别为$P_1,P_2$，容易看出$O_1,O_2,P,P_1,P_2$位于同一平面上，故
$$\overrightarrow{O_2{P}_2}\cdot \left(\overrightarrow{O_2{O}_1}\times \overrightarrow{O_1{P}_1}\right)=0\text{\hspace{1em}(10)}$$
下面在{C2}\{C_{2}\}{C2​}坐标系下表示$(8)$，在{C2}\{C_{2}\}{C2​}坐标系下，$\overrightarrow{O_2{P}_2}={\overline{\mathbf{p}}}_2$，$\overrightarrow{O_2{O}_1}=\mathbf{t}$，$\overrightarrow{O_1{P}_1}=\mathbf{R}{\overline{\mathbf{p}}}_1+\mathbf{t}$，故$(8)$可改写为
$${\overline{\mathbf{p}}}_2^T\left({\mathbf{t}}^{\wedge }(\mathbf{R}{\overline{\mathbf{p}}}_1+\mathbf{t})\right)=0$$
进一步化简可得
$${\overline{\mathbf{p}}}_2^T\left({\mathbf{t}}^{\wedge }\mathbf{R}\right){\overline{\mathbf{p}}}_1=0\text{\hspace{1em}(11)}$$
根据归一化坐标和像素坐标的转换关系，得到
$${\mathbf{q}}_2^T\left[({\mathbf{K}}^{-1}{)}^T({\mathbf{t}}^{\wedge }\mathbf{R}){\mathbf{K}}^{-1}\right]{\mathbf{q}}_1=0\text{\hspace{1em}(12)}$$
称下面的矩阵为本质矩阵（Essential Matrix）
$$\mathbf{E}={\mathbf{t}}^{\wedge }\mathbf{R}$$
需要说明，一个矩阵能够成为本质矩阵当且仅当其的奇异值为$\sigma ,\sigma ,0$，其中$\sigma$为任意实数

称下面的矩阵为基础矩阵（Fundamental Matrix）
$$\mathbf{F}=({\mathbf{K}}^{-1}{)}^T\mathbf{E}{\mathbf{K}}^{-1}$$
$(11)$可改写为
$$\begin{array}{r}{\overline{\mathbf{p}}}_2^T\mathbf{E}{\overline{\mathbf{p}}}_1=0\end{array}\text{\hspace{1em}(13)}$$
$(12)$可改写为
$${\mathbf{q}}_2^T\mathbf{F}{\mathbf{q}}_1=0\text{\hspace{1em}(14)}$$
$(11),(12)$或$(13),(14)$称为对极约束

2.2、约束矩阵的计算

本质矩阵，基础矩阵，单应矩阵都可以做为相机运动过程中存在的约束，可以通过已知的匹配点可计算这些约束矩阵

2.2.1、基础矩阵的计算

将$(14)$的矩阵乘法写成元素形式
$$\left[\begin{array}{ccc}{U}_2& V_2& 1\end{array}\right]\left[\begin{array}{ccc}{f}_1& f_2& f_3\\ f_4& f_5& f_6\\ f_7& f_8& f_9\end{array}\right]\left[\begin{array}{c}{U}_1\\ V_1\\ 1\end{array}\right]=0$$
令$\mathbf{f}=[f_1,\cdots ,f_9{]}^T$，经过整理，上式可以等价地改写为
$$\left[\begin{array}{ccccccccc}{U}_1{U}_2& U_2{V}_1& U_2& V_2{U}_1& V_1{V}_2& V_2& U_1& V_1& 1\end{array}\right]\mathbf{f}=0\text{\hspace{1em}(15)}$$
通常限定$\Vert \mathbf{f}\Vert =1$保证尺度确定，根据上式可以看出，一组匹配点可以构造出一项约束

设匹配点组成的系数矩阵为$\mathbf{A}\in {\mathbb{R}}^{n\times 9}$，$n$为匹配点对数，则基础矩阵的约束方程$(20)$可改写为
$$\mathbf{Af}=0\Vert \mathbf{f}\Vert =1\text{\hspace{1em}(16)}$$
当匹配点较少时，$\mathrm{rank}(\mathbf{A})<8$时，$(23)$为欠定问题，具有多个解；当$\mathrm{rank}(\mathbf{A})=8$时，$(21)$为适定问题，具有唯一解，可直接通过解线性方程组获得；当匹配点较多时，$\mathrm{rank}(\mathbf{A})>8$时，$(23)$变为超定问题，线性方程组无解，故可以构造如下优化问题求近似解
$$\begin{array}{rl}\tilde{\mathbf{f}}& =\underset{\mathbf{f}:\Vert \mathbf{f}\Vert =1}{\mathrm{argmin}}\frac{1}{2}\Vert \mathbf{Af}{\Vert }^2\end{array}\text{\hspace{1em}(17)}$$
对$\mathbf{A}$做奇异值分解
$$\mathbf{A}=\mathbf{U\Sigma }{\mathbf{V}}^T$$
其中$\mathbf{V}=[{\mathbf{v}}_1,\cdots ,{\mathbf{v}}_9]$按照特征值降序排列，容易知道$\tilde{\mathbf{f}}$可取为
$$\tilde{\mathbf{f}}=\pm {\mathbf{v}}_9\text{\hspace{1em}(18)}$$
将$\tilde{\mathbf{f}}$转变为$\tilde{\mathbf{F}}$得到基础矩阵，并根据定义求出$\tilde{\mathbf{E}}={\mathbf{K}}^T\tilde{\mathbf{F}}\mathbf{K}$。但受误差影响此时$\tilde{\mathbf{E}}$不一定满足成为本质矩阵的充要条件，需要进一步变换，首先对$\tilde{\mathbf{E}}$做SVD分解
$$\tilde{\mathbf{E}}=\tilde{\mathbf{U}}\mathrm{diag}({\tilde{\sigma }}_1,{\tilde{\sigma }}_2,{\tilde{\sigma }}_3){\tilde{\mathbf{V}}}^T$$
可令
$${\mathbf{E}}^{\ast }=\tilde{\mathbf{U}}\mathrm{diag}(\frac{{\tilde{\sigma }}_1+{\tilde{\sigma }}_2}{2},\frac{{\tilde{\sigma }}_1+{\tilde{\sigma }}_2}{2},0){\tilde{\mathbf{V}}}^T\text{\hspace{1em}(19)}$$
为最终所求的本质矩阵，${\mathbf{E}}^{\ast }$满足成为本质矩阵的充要条件

2.3、基于约束矩阵的运动恢复

得到约束矩阵后，可通过分解约束矩阵来恢复运动

2.3.1、基于基础矩阵的运动恢复

得到基础矩阵$\mathbf{F}$后，可以根据相机内参数矩阵转换为本质矩阵$\mathbf{E}={\mathbf{K}}^T\mathbf{FK}$

对$\mathbf{E}$进行奇异值分解
$$\mathbf{E}=\mathbf{U}\mathbf{\Sigma }{\mathbf{V}}^T$$
满足$\mathbf{E}={\mathbf{t}}^{\wedge }\mathbf{R}$的$\mathbf{t}$和$\mathbf{R}$各有两个可行解，具体形式如下
$$\begin{array}{r}{\mathbf{t}}^{\wedge }=\{\begin{array}{l}\mathbf{UW}\mathbf{\Sigma }{\mathbf{U}}^T\\ \mathbf{U}{\mathbf{W}}^T\mathbf{\Sigma }{\mathbf{U}}^T\end{array}\mathbf{R}=\{\begin{array}{l}\mathbf{U}{\mathbf{W}}^{\prime }{\mathbf{V}}^T\\ \mathbf{U}{\mathbf{W}}^{\prime }{}^T{\mathbf{V}}^T\end{array}\end{array}\text{\hspace{1em}(20)}$$
其中$\mathbf{W}=\left[\begin{array}{ccc}0& -1& 0\\ 1& 0& 0\\ 0& 0& 1\end{array}\right]$，${\mathbf{W}}^{\prime }=\{\begin{array}{cl}\mathbf{W}& \det (\mathbf{UV})=1\\ -\mathbf{W}& \det (\mathbf{UV})=-1\end{array}$以保证$\det (\mathbf{R})=1$。满足$\mathbf{E}={\mathbf{t}}^{\wedge }\mathbf{R}$的$(\mathbf{R},\mathbf{t})$共有四个可行解，为$\mathbf{t}$和$\mathbf{R}$各自可行解的两两组合。

可以证明，若本质矩阵正确，则四个解中有且仅有一个解使得在两相机中都可见的空间中点在两个相机中的深度均为正值，实践中可以通过比较三角化成功的点的数量找到正确的解。

3、三角化

已知第一幅图像对应的世界坐标系下的相机位姿$({\mathbf{R}}_1,{\mathbf{t}}_1)$，得到两幅图像对应的相机相对运动$\mathbf{R},\mathbf{t}$后，可以计算第二幅图像对应的世界坐标系下的相机位姿$({\mathbf{R}}_2,{\mathbf{t}}_2)$，知两幅图像的二维匹配点对${\mathbf{q}}_1,{\mathbf{q}}_2$，可以计算从世界坐标系下的三维点到像素坐标的投影矩阵${\mathbf{\Psi }}_1=\mathbf{K}[{\mathbf{R}}_1,{\mathbf{t}}_1],{\mathbf{\Psi }}_2=\mathbf{K}[{\mathbf{R}}_2,{\mathbf{t}}_2]$。三角化即根据${\mathbf{\Psi }}_1,{\mathbf{\Psi }}_2,{\mathbf{q}}_1,{\mathbf{q}}_2$估计二维匹配点对应的世界坐标系下的齐次空间点$\mathbf{X}=\left[\begin{array}{c}\mathbf{x}\\ 1\end{array}\right]\in {\mathbb{R}}^4$，即恢复场景中点的真实三维位置的。因为从几何意义上看，如前图，三角化的过程即为寻找两条从相机光心$O_1,O_2$出发，穿过归一化平面上的投影点的射线在空间中的交点$P$的过程，此时$O_1,O_2,P$组成了一个三角形，故称为三角化。

暂忽略下标，根据上面的讨论可求出相机坐标系下的归一化坐标
$$\mathbf{q}=\frac{1}{Z}\mathbf{\Psi }\mathbf{X}\text{\hspace{1em}(21)}$$
其中$Z$为$[\mathbf{R},\mathbf{t}]\mathbf{X}$的最后一维坐标

在上式等号两侧同时左乘${\mathbf{q}}^{\wedge }$，得到
$${\mathbf{q}}^{\wedge }\mathbf{\Psi }\mathbf{X}=0\text{\hspace{1em}(22)}$$
不妨令$\mathbf{\Psi }=\left[\begin{array}{c}{\mathbf{\Psi }}^1\\ {\mathbf{\Psi }}^2\\ {\mathbf{\Psi }}^3\end{array}\right]$，即${\mathbf{\Psi }}^1,{\mathbf{\Psi }}^2,{\mathbf{\Psi }}^3$为$\mathbf{\Psi }$的行向量，此时$(22)$可以写成
$$\left[\begin{array}{ccc}0& -1& V\\ 1& 0& -U\\ -V& U& 0\end{array}\right]\left[\begin{array}{c}{\mathbf{\Psi }}^1\\ {\mathbf{\Psi }}^2\\ {\mathbf{\Psi }}^3\end{array}\right]\mathbf{X}=0$$
进一步可写为
$$\left[\begin{array}{c}V{\mathbf{\Psi }}^3-{\mathbf{\Psi }}^2\\ {\mathbf{\Psi }}^1-U{\mathbf{\Psi }}^3\\ U{\mathbf{\Psi }}^2-V{\mathbf{\Psi }}^1\end{array}\right]\mathbf{X}=0$$
上式中系数矩阵的第三行可以通过前两行消去，即得到
$$\left[\begin{array}{c}V{\mathbf{\Psi }}^3-{\mathbf{\Psi }}^2\\ {\mathbf{\Psi }}^1-U{\mathbf{\Psi }}^3\end{array}\right]\mathbf{X}=0$$
考虑到两个匹配点各自能产生一个上述方程组，故一对匹配点可以提供$4$个约束方程
$$\left[\begin{array}{c}{V}_1{\mathbf{\Psi }}_1^3-{\mathbf{\Psi }}_1^2\\ {\mathbf{\Psi }}_1^1-U_1{\mathbf{\Psi }}_1^3\\ V_2{\mathbf{\Psi }}_2^3-{\mathbf{\Psi }}_2^2\\ {\mathbf{\Psi }}_2^1-U_2{\mathbf{\Psi }}_2^3\end{array}\right]\mathbf{X}=0\text{\hspace{1em}(23)}$$
设匹配点组成的系数矩阵为$\mathbf{A}\in {\mathbb{R}}^{4n\times 4}$，$n$为匹配点对数，则空间坐标的约束方程$(29)$可写为
$$\mathbf{AX}=0\text{\hspace{1em}(24)}$$
当$\mathrm{rank}(\mathbf{A})<4$时，$(30)$为欠定问题，具有多个解；当$\mathrm{rank}(\mathbf{A})=4$时，$(30)$为适定问题，具有唯一解，可直接通过解线性方程组获得，也可等价地通过SVD分解进行求解，先构造如下优化问题求近似解
$$\begin{array}{rl}\tilde{\mathbf{X}}& =\underset{\mathbf{X}:\Vert \mathbf{X}\Vert =1}{\mathrm{argmin}}\frac{1}{2}\Vert \mathbf{AX}{\Vert }^2\end{array}\text{\hspace{1em}(25)}$$
对$\mathbf{A}$做奇异值分解
$$\mathbf{A}=\mathbf{U\Sigma }{\mathbf{V}}^T$$
其中$\mathbf{V}=[{\mathbf{v}}_1,\cdots ,{\mathbf{v}}_4]$按照特征值降序排列，容易知道$\tilde{\mathbf{P}}$可取为
$$\tilde{\mathbf{X}}=\pm {\mathbf{v}}_4\text{\hspace{1em}(26)}$$
设$\alpha$为$\tilde{\mathbf{X}}$在第四维上的分量，若$\alpha =0$，则认为三角化失败，否则为保证所求结果为齐次坐标，即第四维度上值为$1$，可令
$${\mathbf{X}}^{\ast }=\frac{1}{\alpha }\tilde{\mathbf{X}}\text{\hspace{1em}(27)}$$
求出${\mathbf{X}}^{\ast }$后应继续检验三角化是否失败，若对应的在三维点在两相机坐标系下的深度小于等于$0$，即$[{\mathbf{R}}_1,{\mathbf{t}}_1]{\mathbf{X}}^{\ast }$，$[{\mathbf{R}}_2,{\mathbf{t}}_2]{\mathbf{X}}^{\ast }$在第三个维度上的数值小于等于$0$，则认为三角化失败。

4、已知3D-2D匹配数据估算相机运动

已知世界坐标系下的3D空间点坐标和其在相机中投影的2D像素坐标，估计世界坐标系到相机坐标系的位姿变换的问题称为PnP（Perspective-n-Point）问题。

4.1、DLT

已知空间点在世界坐标系下的齐次坐标为$\mathbf{X}\in {\mathbb{R}}^4$，和其对应的像素坐标$\mathbf{q}=[U,V,1{]}^T$，现计算从世界坐标系下的齐次坐标到相机坐标系的位姿变换$\mathbf{R},\mathbf{t}$

设从世界坐标系下的三维点到像素坐标的投影矩阵$\mathbf{\Psi }=\mathbf{K}[\mathbf{R},\mathbf{t}]$，并令

根据$(21)$
$$Z\left[\begin{array}{c}U\\ V\\ 1\end{array}\right]=\left[\begin{array}{c}{\mathbf{\Psi }}^1\\ {\mathbf{\Psi }}^2\\ {\mathbf{\Psi }}^3\end{array}\right]\mathbf{X}\text{\hspace{1em}(28)}$$
$(36)$可以化简为
$$\begin{array}{rl}{\mathbf{\Psi }}^1\mathbf{X}& =ZU\\ {\mathbf{\Psi }}^2\mathbf{X}& =ZV\\ {\mathbf{\Psi }}^3\mathbf{X}& =Z\end{array}$$
因此可以得到
$$\begin{array}{rl}{\mathbf{\Psi }}^1\mathbf{X}-U{\mathbf{\Psi }}^3\mathbf{X}& =0\\ {\mathbf{\Psi }}^2\mathbf{X}-V{\mathbf{\Psi }}^3\mathbf{X}& =0\end{array}$$
不妨令$\mathbf{\psi }=\left[\begin{array}{c}{\mathbf{\Psi }}^1{}^T\\ {\mathbf{\Psi }}^2{}^T\\ {\mathbf{\Psi }}^3{}^T\end{array}\right]$，写出关于$\mathbf{\psi }$的方程组，有
$$\left[\begin{array}{ccc}{\mathbf{X}}^T& 0^T& -U{\mathbf{X}}^T\\ 0^T& {\mathbf{X}}^T& -V{\mathbf{X}}^T\end{array}\right]\mathbf{\psi }=0\text{\hspace{1em}(29)}$$
可以看到，每对3D-2D匹配的特征点能提供两个关于$\mathbf{\psi }$的约束

设匹配点组成的系数矩阵为$\mathbf{A}\in {\mathbb{R}}^{2n\times 12}$，$n$为匹配点对数，则增广矩阵的约束方程可写为
$$\mathbf{A}\mathbf{\psi }=0\text{\hspace{1em}(30)}$$
当匹配点较少，$\mathrm{rank}(\mathbf{A})<12$时，$(38)$为欠定问题，具有多个解；当$\mathrm{rank}(\mathbf{A})=12$时，$(38)$为适定问题，具有唯一解，可直接通过解线性方程组获得；当匹配点较多时，$\mathrm{rank}(\mathbf{A})>12$时，$(38)$变为超定问题，线性方程组无解，故可以构造如下优化问题求近似解
$$\begin{array}{rl}\tilde{\mathbf{\psi }}& =\underset{\mathbf{\psi }:\Vert \mathbf{\psi }\Vert =1}{\mathrm{argmin}}\frac{1}{2}\Vert \mathbf{A}\mathbf{\psi }{\Vert }^2\end{array}\text{\hspace{1em}(31)}$$
对$\mathbf{A}$做奇异值分解
$$\mathbf{A}=\mathbf{U\Sigma }{\mathbf{V}}^T$$
其中$\mathbf{V}=[{\mathbf{v}}_1,\cdots ,{\mathbf{v}}_{12}]$按照特征值降序排列，容易知道$\tilde{\mathbf{\psi }}$可取为
$$\tilde{\mathbf{\psi }}=\pm {\mathbf{v}}_{12}\text{\hspace{1em}(32)}$$
将$\tilde{\mathbf{\psi }}$转变$\tilde{\mathbf{\Psi }}$为再转变为$[\tilde{\mathbf{R}},\tilde{\mathbf{t}}]={\mathbf{K}}^{-1}\tilde{\mathbf{\Psi }}$即得到所求增广矩阵的初步估计，但所求得的旋转矩阵$\tilde{\mathbf{R}}$不一定在$SO(3)$中，可以构造如下优化问题
$$\begin{array}{rl}\min & -\mathrm{tr}\left(\mathbf{R}{\tilde{\mathbf{R}}}^T\right)\\ \mathrm{subject}\mathrm{to}& {\mathbf{R}}^T\mathbf{R}=\mathbf{I}\det (\mathbf{R})=1\end{array}$$
首先对$\tilde{\mathbf{R}}$进行SVD分解
$$\tilde{\mathbf{R}}=\tilde{\mathbf{U}}\tilde{\mathbf{\Sigma }}{\tilde{\mathbf{V}}}^T$$
根据§1中KKT条件，可以证明，在任何情况下，使$(6)$中目标函数最优的旋转矩阵${\mathbf{R}}^{\ast }$总可以取为下述值
$$\begin{array}{r}{\mathbf{R}}^{\ast }=\tilde{\mathbf{U}}\left[\begin{array}{ccc}1& & \\ & 1& \\ & & \det (\tilde{\mathbf{U}})\det (\tilde{\mathbf{V}})\end{array}\right]{\tilde{\mathbf{V}}}^T\end{array}$$

4.2、最小重投影误差

求解PnP问题还可以将每个三维点坐标投影到像素平面得到的坐标与对应的实际观测到的像素坐标做差得到的重投影误差，再通过优化重投影误差得到最优的从世界坐标系到相机坐标系的位姿变换$\mathbf{T}\in SE(3)$，即

$$\begin{array}{rl}{\mathbf{T}}^{\ast }& =\underset{\mathbf{T}}{\mathrm{argmin}}\sum _{i=1}^n{\Vert {\mathbf{q}}_i-\frac{1}{Z_i}\mathbf{K}(\mathbf{T}{\mathbf{X}}_i{)}_{1:3}\Vert }^2\\ & =\underset{\mathbf{T}}{\mathrm{argmin}}\sum _{i=1}^n{\Vert {\mathbf{e}}_i(\mathbf{T})\Vert }^2\end{array}\text{\hspace{1em}(33)}$$
下面暂时省略下标$i$，世界坐标系下的三维点在相机坐标系下的表示为
$$\mathbf{p}=(\mathbf{TX}{)}_{1:3}=[X,Y,Z{]}^T\text{\hspace{1em}(34)}$$
误差取两维
$$\begin{array}{rl}\mathbf{e}& ={\left(\mathbf{q}-\frac{1}{Z}\mathbf{Kp}\right)}_{1:2}=\left[\begin{array}{c}U\\ V\end{array}\right]-\frac{1}{Z}\left[\begin{array}{ccc}{f}_x& 0& c_x\\ 0& f_y& c_y\end{array}\right]\left[\begin{array}{c}X\\ Y\\ Z\end{array}\right]=\left[\begin{array}{c}U-f_x\frac{X}{Z}+c_y\\ V-f_y\frac{Y}{Z}+c_y\end{array}\right]\end{array}\text{\hspace{1em}(35)}$$
根据链式法则得到
$$\begin{array}{rl}\frac{\partial \mathbf{e}}{\partial \mathbf{T}}& =-\frac{\partial {\mathbf{q}}^{\prime }}{\partial \mathbf{p}}\frac{\partial \mathbf{p}}{\partial \mathbf{T}}\end{array}\text{\hspace{1em}(36)}$$
由$(34),(35)$知
$$\frac{\partial \mathbf{e}}{\partial \mathbf{p}}=\left[\begin{array}{ccc}-\frac{f_x}{Z}& 0& \frac{f_{x}X}{Z^2}\\ 0& -\frac{f_y}{Z}& \frac{f_{x}Y}{Z^2}\end{array}\right]\text{\hspace{1em}(37)}$$
设位姿更新方式为$\mathbf{T}\leftarrow \mathrm{Exp}(\Delta \mathbf{T})\mathbf{T},\Delta \mathbf{T}\in {\mathbb{R}}^6$，则可以通过左乘扰动求导方法计算
$$\frac{\partial \mathbf{p}}{\partial \mathbf{T}}={\left(\frac{\partial \mathbf{TX}}{\partial \mathbf{T}}\right)}_{1:3}=[\mathbf{I},-{\mathbf{p}}^{\wedge }]\text{\hspace{1em}(38)}$$
由$(36),(37),(38)$可计算
$$\frac{\partial \mathbf{e}}{\partial \mathbf{T}}=\left[\begin{array}{cccccc}-\frac{f_x}{Z}& 0& \frac{f_{x}X}{Z^2}& \frac{f_{x}XY}{Z^2}& -f_x-\frac{f_x{X}^2}{Z^2}& \frac{f_{x}Y}{Z}\\ 0& -\frac{f_y}{Z}& \frac{f_{x}Y}{Z^2}& f_y+\frac{f_y{Y}^2}{Z^2}& -\frac{f_{x}XY}{Z^2}& -\frac{f_{x}X}{Z}\end{array}\right]\text{\hspace{1em}(39)}$$
问题$(33)$的雅可比矩阵
$${\mathbf{J}}_i=\frac{\partial {\mathbf{e}}_i}{\partial \mathbf{T}}\text{\hspace{1em}(40)}$$
得到
$$\begin{array}{rl}{\mathbf{H}}_k=\sum _{i=1}^n{\mathbf{J}}_i^T{\mathbf{J}}_i{\mathbf{b}}_k& =\sum _{i=1}^n{\mathbf{J}}_i^T{\mathbf{e}}_i\end{array}$$
则更新量可以通过下式计算
$${\mathbf{H}}_k\Delta {\mathbf{T}}_k=-{\mathbf{b}}_k$$

并通过下式更新
$${\mathbf{T}}_{k+1}=\mathrm{Exp}(\Delta {\mathbf{T}}_k){\mathbf{T}}_k$$
最终得到最优的位姿变换

附录

§1、尺度意义下相等

设${\mathbf{x}}_1,{\mathbf{x}}_2$为某种代数结构，若存在$s\in \mathbb{R}$，使得
$$s{\mathbf{x}}_1={\mathbf{x}}_2\text{\hspace{1em}(A1)}$$
则称${\mathbf{x}}_1$与${\mathbf{x}}_2$在尺度意义下相等，记作
$${\mathbf{x}}_1\backsimeq {\mathbf{x}}_2\text{\hspace{1em}(A2)}$$