SLAM 视觉里程估计_1

---

前言

学习SLAM笔记. 如有侵权,请告知删除
$$特征点法\{\begin{array}{l}理解图像特征点的意义,掌握在单幅图像中提取特征点及多幅图像中匹配特征点的方法\\ 理解对极几何的原理,利用对极几何的约束,恢复出图像之间的摄像机的三维运动\\ 理解如何通过三角化,获得二维图像上对应的三维结构\\ 理解PNP问题,及利用已知三维结构与图像的对应关系,求解摄像机的三维运动.\\ 理解ICP问题.及利用点云的匹配关系,求解摄像机的三维运动.\end{array}$$

---

提示：以下是本篇文章正文内容，下面内容例可供参考

一、图像特征点

经典SLAM中以相机位姿-路标来描述SLAM过程
—路标是三维空间中固定不变的点,可以在特定位姿下观测到
—在视觉SLAM中,可以利用图像特征点作为SLAM中的路标

特征点

特征点是图像当中具有代表性的部分,如轮廓点丶较暗区域中的亮点,较亮区域中的暗点等。
----可重复性:相同的区域可以在不同的图像中找到
----可区别性:不同的区域有不同的表达
----高效性:同一图像中,特征点的数量应该小于像素的个数
----本地性:特征仅与一小片图像区域有关.

特征点的信息
----位置、大小、方向、评分等 关键字
----特征点周围的图像信息 描述子

例子:当谈论在一张图像中计算SIFT特征时, 是指提取SIFT关键点,并计算SIFT描述子
充分考虑了在图像变换过程中的光照、尺度、旋转等变化、结果精确、但计算量大。GPU无法实时计算SIFT特征。

1.1、ORB特征点提取

关键点:Oriented FAST(一种改进的FAST角点)
描述子:改进BRIEF

关键点:
FAST:主要检测局部像素灰度变化明显的地方(如一个像素与领域的像素差别较大,则更可能是角点)

$$\begin{gathered} 1、在图像中选取像素p,假设它的亮度为I_p。 \\ 2、设一个阈值T(比如I_p的20\%)。 \\ 3、以像素p为中心,选取半径为3的圆上的16个像素点。 \\ 4、假设选取的圆上,有连续的N个点的亮度大于I_p+T或者小于I_p-T,那么像素p可以被认为是特征点 \\ (N通常取12,记为FAST-12。其他常用的N取值为9和11,他们分别被称为FAST-9,FAST-11)。 \\ 5、循环以上四步,对每个像素执行相同的操作。 \end{gathered}$$

在FAST12中,提出一个高效的检测,来快速排除一大部分非特征点的点。该测试仅仅检查在位置1、5、9、13四个位置的像素如果不满足至少三个角点亮度大于l+T或小于l-T,那么p不可能是一个角点。

FAST缺点:
—原始FAST角点经常出现扎堆的现象(分布不均匀)。所以在第一遍检测之后,还需要用非极大值抑制,在一定区域内仅保留相应极大值的角点,避免角点集中问题。
—由于FAST角点不具有方向信息且存在尺寸问题ORB添加了尺度和旋转的描述:尺度不变性通过构建图像金字塔来实现,旋转是由灰度质心法来实现。

---

1.1.1、关键点

关键点:Oriented FAST(一种改进的FAST角点)
Oriented FAST:尺度不变性通过构建图像金字塔来实现

金字塔是计算图视觉中常用的一种处理方法,如下图所示。金字塔底层是原始图像。每往上一层,就对图像进行一个固定倍率的缩放,这样我们就有了不同分辨率的图像。较小的图像可以看成是远处看过来的场景。在特征匹配算法中,我们可以匹配不同层上的图像,从而实现尺度不变性。例如:如果相机在后退,那么我们应该能够在上一个图像金字塔的上层和下一个图像金字塔的下层中找到匹配。

Oriented FAST:旋转是由灰度质心法来实现。

质心是指以图像块灰度值作为权重的中心。其具体操作步骤如下:
1.在一个小的图像块B中,定义图像块的距为:
m p q = ∑ x , y ∈ B x p y q I ( x , y ) ,      p , q = { 0 , 1 } . m_{pq}=\sum_{x,y∈B}x^py^qI(x,y),~~~~p,q=\{0,1\}. mpq​=x,y∈B∑​xpyqI(x,y),    p,q={0,1}.
2.通过距可以找到图像块的质心:
$$C=(\frac{m_{10}}{m_{00}},\frac{m_{01}}{m_{00}}).$$
$$\begin{gathered} 3.链接图像块的几何中心O质心C,得到一个方向向量\overrightarrow{OC},于是特征点的方向可以被定义为: \\ \theta =arctan(\frac{m_{01}}{m_{10}}). \end{gathered}$$

---

1.1.2、描述子

问：为何要计算描述子？
答：每张图片会提取很多特征点，我们希望相同特征点之间能够建立起联系，为后续的解算相机位姿，三维点等提供准确的条件。上面说的联系即描述子，描述子大致（其中的条件需要自行设置）相同则判定为同一点。

具体可以参考:ORB描述子的理解

描述子:BRIEF
一种二进制描述子,其描述向量由许多个0和1的组成$$\{\begin{array}{ll}1& 关键点附近两个像素l(p)>l(q)\\ 0& 关键点附近两个像素l(p)<l(q)\end{array}$$

• BRIEF-128: 在特征点附近的128次像素比较:

1.在图像块内平均采样;
2.p和q都符合(0, S²1/25)的高斯分布;
3.p符合((0, S²1/25))的高斯分布,而q符合(0, S²1/100)的高斯分布;
4.在空间量化极坐标下的离散位置随机采样;
5.吧p固定为(0,0),q在周围平均采样;

其中(S²1/25)是斜方差. 工业中经常使用第二种的高斯分布

---

描述子与特征点的关系

1.以关键点(紫色点)为圆心(几何质心),以d为半径做圆。
2.计算质心(蓝色点),得到特征点方向(绿色箭头)。
3.选取某一模式下的pq点对.(为了方便选取了2对)。
4.根据特征点方向,将其旋转到与x轴平行,并将所有pq点对进行旋转。
5.按照规则if(l§>l(q)) val=1; else val=0;
6.按照顺序排列描述子。

通过计算特征点方向,使得ORB的描述子具备了旋转不变性,更鲁棒了。

旋转矩阵:

那么:
$$\begin{gathered} x=rcos(\varphi ) \\ y=rsin(\varphi ) \\ {x}^{\prime }=rcos(\theta +\varphi )=rcos(\theta )cos(\varphi )-rsin(\theta )sin(\varphi )=xcos(\theta )-ysin(\theta ) \\ {y}^{\prime }=rsin(\theta +\varphi )=rsin(\theta )cos(\varphi )+rcos(\theta )sin(\varphi )=xsin(\theta )+ycos(\theta ) \end{gathered}$$
得到:
$$\left[\begin{array}{c}{x}^{\prime }\\ y^{\prime }\end{array}\right]=\left[\begin{array}{cc}cos\theta & -sin\theta \\ sin\theta & cos\theta \end{array}\right]\left[\begin{array}{c}x\\ y\end{array}\right]$$

---

描述子:BRIEF总结
1.优点: BRIEF使用了随机选点的比较,速度比较快,而且由于使用了二进制表达,存储起来也十分方便.
2.缺点: 原始的BRIEF描述子不具有旋转不变性,在图像发生旋转时容易走失. 而ORB在FAST特征点提取阶段计算了关键点的方向,计算了旋转之后的"BRIEF"特征,使ORB的描述子具有较好的旋转不变性.
3. 注意BRIEF是一种二进制描述,需要使用汉明距离度量.

二、图像特征点

特征匹配解决了SLAM中的数据关联问题,即确定当前看到的路标与之前看到的路标之间的对应关系。
通过对图像与图像或者图像与地图之间的描述子进行精准匹配,可以为后续的姿态估计、优化等操作减轻大量负担,而且,由于图像特征的局部特性,误匹配的情况存在。

考虑两个时刻的图像:
$$\begin{gathered} --在图像I_t中提取到特征点x_t^m,m=1,2,...,M;在图像I_{t+1}中提取到特征点x_{t+1}^n,n=1,2,...,N \\ --暴力匹配:对每个特征点x_t^m与所有的x_{t+1}^n测量描述子的距离,然后排序,取最近的一个作为匹配点. \\ --当特征点数量很大,暴力匹配的运算量会变得很大,选用一些改进算法,如:快速近似最近邻(FLANN)算法. \end{gathered}$$

三、计算相机运动

-----如果只有两个单目图像,得到2D-2D间的关系(对应极几何)
-----我们得到一些3D点和2维图像投影,即3D-2D间的关系(PNP)
-----当相机为双目、RGB-D, 或者通过某种方法得到距离信息.得到3D-3D间的关系(ICP)

1. 2D-2D对应极几何

已知:1.在像素坐标上的投影点为p1和p2; I1的一个特征点p1,在I2中对应特征点p2(匹配正确);相机内参
想要求:1.第一帧到第二帧的相机运动为R,t
相机坐标系-像素坐标系
$$z\left(\begin{array}{c}u\\ v\\ 1\end{array}\right)=\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)=K{\tilde{P}}_c$$

$$\begin{gathered} 基线:O_1{O}_2 \\ 极点:基线与平面的交点(e_1,e_2) \\ 极平面:包含基线的平面(O_1,O_2,P三个点可以确定一个平面) \\ 极线:对极平面与图像平面的交线(l_1,l_2) \end{gathered}$$

注意 不管p点在那里,只有在红色的射线上(拉近或者放远),它在图像上都对应于p1点的位置.

现在,我们从代数的角度来分析这里的几何关系。在第一帧的坐标系下,设P在空间为:$$P=[X,Y,Z{]}^T$$根据针孔模型,我们知道两个像素点p1,p2,的像素位置为:
$$\begin{gathered} s_1{p}_1=K_{3\ast 3}{P}_{3\ast 1},s_2{p}_2=K(RP+t) \\ 其中s_1\left(\begin{array}{c}u\\ v\\ 1\end{array}\right)=K\left(\begin{array}{c}{x}_c\\ y_c\\ z_c=s_1\end{array}\right):s_1就是P的深度 \end{gathered}$$
这里K为相机内参矩阵,R,t为两个坐标系的相机运动。具体来说,这里计算的是R21和t21.因为它们把第一个坐标系下的坐标换到第二个坐标系下。

1.1 对极几何

对极约束刻画了O1,O2,P 共面的关系,简洁的给出了两个匹配点的空间位置.
$$\begin{gathered} p_2^T{K}^{-T}{t}^{\wedge }R{K}^{-1}{p}_1=0. \\ E=t^{\wedge }R,F=K^{-T}E{K}^{-1},x_2^{T}E{x}_1=p_2^{T}F{p}_1=0 \end{gathered}$$

• 本质矩阵E, 基础矩阵F,在内参已知的情况下,可以使用E
• 相机位姿估计问题分为两步: 1. 根据匹配点的像素位置求出E或者F
  2.根据E或者F求出R,t
  内参在SLAM中通常是已知的,则在实践过程中往往使用形式更简单的E
  

---

本质矩阵:"

• 性质1: E满足对极约束:$$x_2^{T}E{x}_1=0.$$这是等式为零的约束,对E乘以任意非零常数后,对极约束仍然成立, 则成为E在不同尺度下是等价的(尺度等价性).
• 性质2: (内在性质): 一个3×3的矩阵式本质矩阵的充要条件是它的奇异值中有两个相等而第三个为0 (det(E)=0)

ps: 奇异性:$$\begin{gathered} \to 已知A为本质矩阵,证A有一个零奇异值,并且另外两个非零奇异值相等 \\ 证:由题意得A=t^{\wedge }R=TR(其中T=t^{\wedge }),将A奇异值分解为:A=u\Sigma v^T<1> \\ (其中u{u}^T=E,v{v}^T=E,\Sigma =\left(\begin{array}{ccc}{\delta }_1& 0& 0\\ 0& {\delta }_2& 0\\ 0& 0& {\delta }_3\end{array}\right),{\delta }_1,{\delta }_2,{\delta }_3为A的奇异值) \\ A\cdot A^T=TR\cdot (TR{)}^T=TR\cdot R^T{T}^T=T\cdot T^T<2> \\ 另一方面A\cdot A^T=(u\Sigma v^T)(u\Sigma v^T{)}^T=u\Sigma v^{T}v{\Sigma }^T{u}^T=u{\Sigma }^2{u}^T<3> \\ 从上式可以看出u是A\cdot A^T的特征矩阵,{\Sigma }^2对应的特征值. \\ 奇异值和特征值的关系 \\ 下面求A\cdot A^T的特征值: \\ 有<2>A\cdot A^T=T\cdot T^T=\left(\begin{array}{ccc}0& -t_3& t_2\\ t_3& 0& -t_1\\ -t_2& t_1& 0\end{array}\right)\left(\begin{array}{ccc}0& t_3& -t_2\\ -t_3& 0& t_1\\ t_2& -t_1& 0\end{array}\right)=\left(\begin{array}{ccc}{t}_3^2+t_2^2& -t_1^2+t_2^2& -t_1^2+t_3^2\\ -t_1^2+t_2^2& -t_3^2+t_1^2& -t_2^2+t_3^2\\ -t_1^2+t_3^2& -t_2^2+t_3^2& t_2^2+t_1^2\end{array}\right) \\ 记t_1^2+t_2^2+t_3^2=k(k>0)则|\lambda E-T\cdot T^T|=\lambda (k-\lambda {)}^2=0,则{\lambda }_1=0,{\lambda }_2=k,{\lambda }_3=k. \\ 则A的奇异值为0,\sqrt{k},\sqrt{k}. \end{gathered}$$

• 性质3: 本质矩阵E=[t]×R只有五个自由度:旋转矩阵与平移矢量各有三个自由度,但是有一个整体尺度因子的多异性,于是减少了一个自由度。
• 本质矩阵仅依赖于外部参数(R,t)
• $$\{\begin{array}{l}基于E有5个自由度的事实,表明我们最少可以用5对点来求解E\\ 通用的方法把E当成普通3\ast 3矩阵:8对点发\end{array}$$

1.1.1 求E

八点法求E:"
将E看成通常意义下的33的矩阵,去掉尺度因子后剩八个自由度
考虑一对匹配点,其归一化坐标为:
$$x_1=\left[\begin{array}{c}{u}_1\\ v_2\\ 1\end{array}\right],x_2=\left[\begin{array}{c}{u}_2\\ v_2\\ 1\end{array}\right]$$
根据对极约束:
$$(u_2,v_2,1)\left(\begin{array}{ccc}{e}_1& e_2& e_3\\ e_4& e_5& e_6\\ e_7& e_8& e_9\end{array}\right)\left(\begin{array}{c}{u}_1\\ v_1\\ 1\end{array}\right)=0$$写成向量的形式:
$$\begin{gathered} e=[e_1,e_2,e_3,e_4,e_5,e_6,e_7,e_8,e_9{]}^T, \\ [u_2{u}_1,u_2{v}_1,u_2,v_2{u}_1,v_2,u_1,u_1,1]\cdot e=0. \end{gathered}$$

定理:设A是mn矩阵,如果R(A)=r<n,则其次线性方程组Ax=0的基础解析存在(非零解)。且每个基础解析中含有n-r个解向量。
把左侧的系数矩阵记为A$$Ae=0$$对于八点法,A的大小为m:8×n:9。如果系数矩阵的秩r=8,那么它的解空间位数n-r=1,也就是一条直线。这与尺度等价性是一致的。则E的元素可以通过上述方程解得。
如果秩小于8?存在线性相关,比如两点对是共线的。
以上都在讨论8个方程都是线性无关的,如果方程数大于8,有彼此线性相关的方程组,我们该如何求解?

把左侧的系数矩阵记为A$$Ae=0$$
如果给定的匹配点数多于8,该方程构成一个超定方程,即不一定存在e使得上式成立,此时可以计算一个最小二乘解$$min||Ae|{|}_2^2=min{e}^T{A}^{T}Ae$$求出了最小二乘意义下的E矩阵。

最小二乘:
是一种直线拟合的方式。其核心的思想就是说求解未知参数,使得理论值与观测值之差(误差, 或者说残差)的平方和(一般叫做损失函数)达到最小:$$E=\sum _{i=1}^n{e}_i^2=\sum _{i=1}^n(y_i-\hat{y}{)}^2$$

不过,当可能存在误差匹配的情况下,我们会更倾向于使用随机采样一致性来求。该算法适用于很多带误差匹配的数据。
最小二乘的一个缺点:
当异常点较多时.由于最小二乘是使得所有点离目标函数最近,所以为了保证这一点,拟合直线会使得模型鲁棒性较差,进而得出较差的拟合结果。

什么是RANSAC?
RANSAC为RandomSampleConsensus(随机抽样一致)的缩写,它是根据包含异常数据的样本数据集,计算出数学模型参数,得到有效样本的算法。它于1981年Fischler和Bolles提出。

算法:

1. 随机的从S中选择s个数据点组成一个样本作为模型的一个示例,其中s是构建模型需要最少的点,如拟合直线s=2
   
2. 计算S中所有点与模型的距离,确定模型距离阈值t内的数据点集Si,Si成为采样的一致集并定义为S的内点。
   
3. 如果Si的大小(内点的数目)大于某个阈值T,用Si的所有点重新估计模型并结束。
4. 如果Si的大小小于T,选择一个新的子集并重复1,2,3的过程
   
5. 经过N次试验选择最大一致集Si,并用Si所有点重新估计模型。
   
   Conclusion:$$\{\begin{array}{ll}input:& 含有外点的数据集S\\ output& 我们期望拟合出的模型和内点集\end{array}$$
   

1.1.2 从E中分解R和t

对E进行奇异性分解
$$E=U\Sigma V^T$$
(U与V是正交矩阵,Σ为奇异值矩阵)

JacobiSVD<MatrixXd>svd(E,ComputeThinU |ComputeThinV);
Matrix3d sigma;
Vector3d sigma3=svd.singularValues();
sigma<<(sigma3(0)+sigma3(1))/2,0,0,0,(sigma3(0)+sigma(1))/2,0,0,0,0;

根据现行方程解出的E,可能不满足E的内在性质----它的奇异值不一等是σ,σ,0的形式,这时,在做SVD分解时会可以把Σ矩阵调整成上面的样子,具体方法为: 对八点求得的E进行SVD分解后,会得到奇异值矩阵Σ=diag(σ1,σ2,σ3),不妨σ1>>σ2>>σ3,取$$E=Udiag(\frac{{\sigma }_1+{\sigma }_2}{2},\frac{{\sigma }_1+{\sigma }_2}{2},0)V^T$$
$$\begin{gathered} t_1^{\wedge }=U{R}_Z(\frac{\pi }{2})\Sigma U^T{R}_1=U{R}_Z^T(\frac{\pi }{2})V^T \\ {t}_2^{\wedge }=U{R}_Z(-\frac{\pi }{2})\Sigma U^T{R}_2=U{R}_Z^T(-\frac{\pi }{2})V^T \end{gathered}$$

证明: SLAM十四讲7视觉里程
R,t组合起来有四种情况(点在相机前原理)

1.2 单应矩阵H

• 八点法在特征点共面时会退化
• 设一堆匹配好的特征点位于某平面上,则这个平面p满足方程
  $$\begin{gathered} n^{T}P+d=0. \\ 其中n^T为平面法向量 \\ \Downarrow \\ -\frac{n^{T}P}{d}=1. \end{gathered}$$
  
• 两个图像特征点的坐标关系
• $$\begin{gathered} p_2\cong K(RP+t) \\ \cong K(RP+t\cdot (-\frac{n^{T}P}{d})) \\ \cong K(R-\frac{t{n}^T}{d})P \\ \cong K(R-\frac{t{n}^T}{d})K^{-1}{p}_1.记做H \\ \Downarrow \\ {p}_2\cong H{p}_1 \\ H的定义与旋转,平移及平面,内参有关,单应矩阵也是一个3\ast 3的矩阵 \end{gathered}$$

$$\begin{gathered} p_2\cong H{p}_1. \\ 上式展开\Downarrow \\ \left(\begin{array}{c}{u}_2\\ v_2\\ 1\end{array}\right)\cong \left(\begin{array}{ccc}{h}_1& h_2& h_3\\ h_4& h_5& h_6\\ h_7& h_8& h_9\end{array}\right)\left(\begin{array}{c}{u}_1\\ v_1\\ 1\end{array}\right) \\ 去掉前三行\Downarrow \\ {u}_2=\frac{h_1{u}_1+h_2{v}_1+h3}{h_7{u}_1+h_8{v}_1+h9} \\ {v}_2=\frac{h_4{u}_1+h_5{v}_1+h6}{h_7{u}_1+h_8{v}_1+h9} \end{gathered}$$

求解线性方程组可恢复单应矩阵H
对但应矩阵进行分解,才可以得到相应的旋转矩阵R和平移向量t(数值法与解析法)
与本质矩阵分解类似,单应矩阵的分解同样会得到四组解。如果已知成像的地图点和深度为正值(点在相机前方),则可以排除两组解,最后剩下的两组解,可以通过更多的先验信息来判断。

尺度不确定性:
— 从R和t的计算公式中,可以发现$$kE=k{t}^{\wedge }R$$
乘以k倍,等式也成立,所以我们如果程序输出t=0.882, 这个0.882 究竟是0.882米还是0.882厘米,我们没办法确定
— 由于尺度的关系,不同的t决定了以后计算点p的深度也是不同的,所以回复的物体深度也是跟尺度有关的,我们说的结构回复只是回复了物体的结构框架,而不是实际意义的物体尺寸。
— 当我们对两幅图像计算E并回复R和t时,他们的尺度都是一样的,本来是同一个点,在不同尺寸下,深度不一样,这个时候就很麻烦,一次这个尺度需要统一.

三、三角测量

为什么要使用三角测量?
如果一直用对极几何求两帧之间的位姿,这样求得的两帧之间t的尺度每一次都不固定, 所以要先用三角测量得到一个深度, 把尺度固定下来。三角化之后就把地图和相机<固定>在同一个尺度下,但这个尺度还是不<确定>的。

三角测量:是指通过在两处观察同一个点的夹角,从而确定该点的距离。

$$\begin{gathered} 理论上直线O_1{p}_1与O_2{p}_2在场景中会相较于一点p,该点即两个特征点所对应的地图点的三维场景中的位置。 \\ 但是由于噪声的影响,这两条直线往往无法相交,则可以通过最小二乘求解。 \end{gathered}$$

几何关系:
$$\begin{gathered} 设x_1和x_2为两个特征点的归一化坐标 \\ {s}_1{x}_1=s_{2}R{x}_2+t. \\ 现在已知R和t,要求s_1和s_2 \\ 求s_2时,两侧乘x_1^{\wedge } \\ {s}_1{x}_1^{\wedge }{x}_1=s_2{x}_1^{\wedge }R{x}_2+x_1^{\wedge }t. \\ 另外,由于噪声的存在,我们估计的R和t不一定使等号成立,则用最小二乘法求解: \\ 求[x_1,-R{x}_2]\left[\begin{array}{c}{s}_1\\ s_2\end{array}\right]=t的最小二乘. \end{gathered}$$

三角化中的问题:
—三角测量是由平移得到的,有平移才会出现对极几何中的三角形。则纯旋转是无法使用三角测量的。
—平移的大小会引起:像素不确定性导致的深度的不确定性

1. 当存在误差时,平移越大,误差对深度计算的影响越小。
2.但当平移变大时,会导致图像的外观成像发生变化,变化越大特征与匹配就越困难。
3. 于是产生矛盾: 平移太大会导致匹配失效, 平移太小三角化精度不够

三、3D-2D:PNP

3.1 问题描述:

已知:

1. 3D:上一帧的相机坐标系下的3D点
2. 2D: 两幅图片的匹配信息,像素坐标
3. 其他:内参矩阵

求:

两帧之间的变换矩阵R,t

问题求解:
$$\{\begin{array}{l}直接线性变换(DLT)\\ P3P\\ EPNP\\ UPNP\\ 非线性优化(BundleAdjustment)\end{array}$$

3.2 直接线性变换(DLT)

数学语言描述:
某个空间点的其次坐标$$P=\left(\begin{array}{c}X\\ Y\\ Z\\ 1\end{array}\right),在图像I_1(上一帧图像)中,投影到特征点x_1=\left(\begin{array}{c}{u}_1\\ v_1\\ 1\end{array}\right)(归一化坐标)$$
定义曾广矩阵[R|t]为一个3×4的矩阵,包含了旋转和平移信息此时相机的位姿R和t是未知的
$$\begin{gathered} s\left(\begin{array}{c}{u}_1\\ v_1\\ 1\end{array}\right)=\left(\begin{array}{cccc}{t}_1& t_2& t_3& t_4\\ t_5& t_6& t_7& t_8\\ t_9& t_{10}& t_{11}& t_{12}\end{array}\right)\left(\begin{array}{c}X\\ Y\\ Z\\ 1\end{array}\right).归一化平面 \\ 与世界坐标系 \end{gathered}$$
用最后一行把s消去,会得到两个约束
$$u_1=\frac{t_{1}X+t_{2}Y+t_{3}Z+t_4}{t_{9}X+t_{10}Y+t_{11}Z+t_{12}},v_1=\frac{t_{5}X+t_{6}Y+t_{7}Z+t_8}{t_{9}X+t_{10}Y+t_{11}Z+t_{12}}$$
定义矩阵的行向量:
$$\begin{gathered} t_1=(t_1,t_2,t_3,t_4{)}^T,t_2=(t_5,t_6,t_7,t_8{)}^T,t_3=(t_9,t_{1}0,t_{1}1,t_{1}2{)}^T, \\ 则有[R|t]=\left(\begin{array}{c}{t}_1^T\\ t_2^T\\ t_3^T\end{array}\right),带入两个约束中于是有: \\ {t}_1^{T}P-t_3^{T}P{u}_1=0, \\ {t}_2^{T}P-t_3^{T}P{v}_1=0, \end{gathered}$$
则每个特征点提供了两个关于t的线性约束,假设一共有N个特征点,则可以列出下列方程组:
$$\left(\begin{array}{ccc}{P}_1^T& 0& -u_1{P}_1^T\\ 0& P_1^T& -v_1{P}_1^T\\ ⋮& ⋮& ⋮\\ P_N^T& 0& -u_N{p}_N^T\\ 0& P_N^T& -v_N{P}_N^T\end{array}\right)\left(\begin{array}{c}{t}_1\\ t_2\\ t_3\end{array}\right)=0.$$
在这个方程中,[R|t]一共有12维,则最少通过6对匹配点可以实现矩阵的线性求解,当匹配点大于6对时, 对超定方程求最小二乘解即可。

DLT求解局限:
在DLT求解过程中,直接将[R|t]矩阵看成12个未知数,忽略了其中的关系(旋转矩阵R∈SO(3)),因此求得的解不一定满足该约束(未必是正交矩阵),则我们必须针对DLT估计的左边的3×3的矩阵寻找一个最好的旋转矩阵对它进行近似。

3.3 直接线性变换(DLT)

数学语言描述:输入数据为3对3D-2D的匹配点
$$已知\{\begin{array}{ll}O是相机光心& \\ 3D点为A,B,C& (世界坐标系的点)\\ 2D点为a,b,c& (A,B,C在相机平面上的投影)\end{array}求解⟹得到该点在相机坐标系下的3D坐标。$$
一旦3D点在相机坐标系下的坐标可以算出,就得到了3D-3D对应的点,把PNP问题转化为了ICP问题
$$\begin{gathered} \Delta Oab-\Delta OAB,\Delta bc-\Delta OBC,\Delta Oac-\Delta OAC.三组相似三角形 \\ O{A}^2+O{B}^2-2OA\cdot OB\cdot cos<a,b>=A{B}^2 \\ O{B}^2+O{C}^2-2OB\cdot OC\cdot cos<b,c>=B{C}^2余弦定理 \\ O{A}^2+O{B}^2-2OA\cdot OB\cdot cos<a,b>=A{B}^2 \\ 除以O{C}^2,记x=\frac{OA}{OC},y=\frac{OB}{OC}\to \{\begin{array}{l}{x}^2+y^2-2xycos<a,b>=\frac{A{B}^2}{O{C}^2}\\ y^2+1^2-2ycos<b,c>=\frac{B{C}^2}{O{C}^2}\\ x^2+1^2-2xcos<a,c>=\frac{A{C}^2}{O{C}^2}\end{array} \\ \begin{array}{c}{x}^2+y^2-2xycos<a,b>-v=0\\ y^2+1^2-2ycos<b,c>-uv=0\\ x^2+1^2-2xcos<a,c>-wv=0\end{array}记v=\frac{A{B}^2}{O{C}^2},uv=\frac{B{C}^2}{O{C}^2},wv=\frac{A{C}^2}{O{C}^2} \\ 把第一个式子中的v放到等式一边,并带入后两个式子中: \\ (1-u)y^2-u{x}^2-2cos<b,c>y+2uxycos<a,b>+1=0 \\ (1-w)x^2-w{y}^2-2cos<a,c>x+2wxycos<a,b>+1=0 \end{gathered}$$
关于x和y的一元二次方程:

已知:三个余弦角cos<a,b>,cos<b,c>,cos<a,c>
$$\begin{gathered} v=\frac{A{B}^2}{O{C}^2},uv=\frac{B{C}^2}{O{C}^2},wv=\frac{A{C}^2}{O{C}^2} \\ \Downarrow \\ u=\frac{B{C}^2}{A{B}^2},\omega =\frac{A{C}^2}{A{B}^2} \end{gathered}$$
未知:x与y(随着相机移动会发生变化), $$x=\frac{OA}{OC},y=\frac{OB}{OC}$$
求解方法:吴消元法
求解结果:A,B,C在当前相机坐标系下的3D坐标

总结:
$$P3P=\{\begin{array}{l}利用三角形性质\\ 求解A、B、C在当前相机坐标系下的3D坐标\\ 转换一个3D-3D的位姿估计问题==>>ICP(比较容易)\end{array}$$
存在的问题:
1. P3P只利用3个点的信息,当给定的配对点多于三组时,难以利用更多信息.
2. 如果3D点或2D点受噪声影响,或者存在误匹配,则算法失效

3.4 非线性优化(Bundle Adjustment) PNP问题

思想: 前面说的线性方法,往往是先求相机位姿,再求空间点位置
非线性优化则是把他们看成优化变量,放在一起优化,因此可以构建一个定义于李代数上的非线性最小二乘问题
分析目标函数: 我们通过特征匹配知道p1,p2是同一空间点的投影,估计了一个相机位姿,使得3D的实际投影位置与观测位置存在一个误差(重投影误差),则把误差求和,构建最小二乘问题,然后寻找最好的相机位姿,使其最小化
$$T^{\ast }={\arg }_T\min \frac{1}{2}\sum _{i=1}^n||u_i-u_i^{\prime }|{|}_2^2(1)$$

已知:

1. 某空间点在上一帧相机坐标为$$P_i=\left(\begin{array}{c}{X}_1\\ Y_i\\ Z_i\end{array}\right)\begin{array}{c}上一帧相机坐标系-像素坐标系\\ ⟹\\ s\left[\begin{array}{c}{u}_i^{\prime }\\ v_i^{\prime }\\ 1\end{array}\right]=KT\left[\begin{array}{c}{X}_i\\ Y_i\\ Z_i\\ 1\end{array}\right]\begin{array}{c}其次坐标到\\ 非齐次坐标\\ 的转换\end{array}\end{array}观测投影位置:u_i^{\prime }=\left[\begin{array}{c}{u}_i^{\prime }\\ v_i^{\prime }\end{array}\right]$$
2. "实际"投影的像素坐标为:$$u_i^{\prime }=\left[\begin{array}{c}{u}_i^{\prime }\\ v_i^{\prime }\end{array}\right]$$
   带入模型(1):$$T^{\ast }=\arg \underset{T}{\min }\frac{1}{2}\sum _{i=1}^n||u_i-\frac{1}{s_i}KT{P}_i|{|}_2^2目标函数$$
   通过高斯牛顿法,L-M等优化算法进行求解,首先我们要知道每个误差关于优化变量的导数:
   $$e(x+Deltax)\approx e(x)+J\Delta .这里的J是那种形式$$
   优化这个目标函数,用的是李代数扰动,则对扰动小量δξ求导:
   $$\frac{\partial e}{\partial \delta \xi }=\underset{\delta \xi \to 0}{\lim }\frac{e(\delta \xi \oplus \xi )-e(\xi )}{\delta \xi }=\frac{\partial e}{\partial P^{\prime }}\frac{\partial P^{\prime }}{\partial \delta \xi }$$
   其中,P’是P(上一帧相机坐标系下的点)经过外参矩阵T变换到当前帧相机坐标系下的点
   $$P^{\prime }=(TP{)}_{1:3}=[X^{\prime },Y^{\prime },Z^{\prime }{]}^T$$
   那么,相机投影模型相对于P’为$$\begin{gathered} s{u}^{\prime }=K{P}^{\prime } \\ s\left[\begin{array}{c}{u}_i^{\prime }\\ v_i^{\prime }\\ 1\end{array}\right]=K\left[\begin{array}{c}{X}_i\\ Y_i\\ Z_i\end{array}\right]\begin{array}{c}利用第三行消去s(实际上就是P^{\prime }的距离)\\ u^{\prime }=f_x\frac{X^{\prime }}{Z^{\prime }}+c_x,v^{\prime }=f_y\frac{Y^{\prime }}{Z^{\prime }}+c_y\end{array}(3) \end{gathered}$$
   在模型(2)中, 其中u_i是像素坐标的测量值(不会随位姿改变,则为常量),后一项为预测值e_i对P’求导; 实际上也就是:
   $$-\frac{1}{s_i}KT{P}_i=-\frac{1}{s_i}KT{P}_i-\frac{1}{s_i}s{u}^{\prime }其中-\frac{1}{s_i}KT{P}_i为预测模型$$
   对P’求导,由(3)可知
   $$\frac{\partial e}{\partial P^{\prime }}=-\left[\begin{array}{ccc}\frac{\partial u}{\partial X^{\prime }}& \frac{\partial u}{\partial Y^{\prime }}& \frac{\partial u}{\partial Z^{\prime }}\\ \frac{\partial v}{\partial X^{\prime }}& \frac{\partial v}{\partial Y^{\prime }}& \frac{\partial v}{\partial Z^{\prime }}\end{array}\right]=-\left[\begin{array}{ccc}\frac{f_x}{Z^{\prime }}& 0& -\frac{f_x{X}^{\prime }}{Z^{\prime 2}}\\ 0& \frac{f_y}{Z^{\prime }}& -\frac{f_y{Y}^{\prime }}{Z^{\prime 2}}\end{array}\right]$$
   $$\begin{array}{c}\frac{\partial P^{\prime }}{\partial \delta \xi }=\frac{\partial (exp({\xi }^{\wedge })P)}{\partial \delta \xi }=\left[\begin{array}{cc}I& -(RP+t{)}^{\wedge }\\ 0^T& 0^T\end{array}\right]\\ \to \left[\begin{array}{cc}I& -P^{\prime \wedge }\end{array}\right]=\left[\begin{array}{cccccc}1& 0& 0& 0& Z^{\prime }& -Y^{\prime }\\ 0& 1& 0& -Z^{\prime }& 0& X^{\prime }\\ 0& 0& 1& Y^{\prime }& -X^{\prime }& 0\end{array}\right]\end{array}\frac{\partial e}{\partial \delta \xi }=\underset{\delta \xi \to 0}{\lim }\frac{e(\delta \xi \oplus \xi )-e(\xi )}{\delta \xi }=\frac{\partial e}{\partial P^{\prime }}\frac{\partial P^{\prime }}{\partial \delta \xi }$$
   将这两项相乘,就得到了2×6的雅可比矩阵:
   $$\begin{gathered} \frac{\partial e}{\partial \delta \xi }=-\left[\begin{array}{cccccc}\frac{f_x}{Z^{\prime }}& 0& -\frac{f_x{X}^{\prime }}{Z^{\prime 2}}& -\frac{f_x{X}^{\prime }{Y}^{\prime }}{Z^{\prime 2}}& f_x+\frac{f_x{X}^{\prime 2}}{Z^{\prime 2}}& -\frac{f_x{Y}^{\prime }}{Z^{\prime }}\\ & & & & & \\ 0& \frac{f_y}{Z^{\prime }}& -\frac{f_y{Y}^{\prime }}{Z^{\prime 2}}& -f_y-\frac{f_y{Y}^{\prime 2}}{Z^{\prime 2}}& \frac{f_y{X}^{\prime }{Y}^{\prime }}{Z^{\prime 2}}& \frac{f_y{X}^{\prime }}{Z^{\prime }}\end{array}\right] \\ 2\times 6的雅可比矩阵是误差对于扰动小量位姿的偏导数 \end{gathered}$$

对于重投影误差来说。
2. 是把上一帧相机坐标系下的3D点,乘了转换矩阵外参(R和t), 转换到当前帧相机坐标系下的3D点,再乘以内参(K)最后变到当前帧图片的像素坐标系下,得到预测的像素点。 
3. 前段匹配像素点,例如ORRB, 光流匹配的。 
4. 两者像素点相差会构成一个误差函数。这个误差函数的自变量就是外参,通过调整外参,使得误差最小。

---

除了优化位姿(R和t),还希望优化特征点的空间位置,则需要讨论e关于空间点(世界坐标系)P的导数$$\frac{\partial e}{\partial P}=\frac{\partial e}{\partial P^{\prime }}\frac{\partial P^{\prime }}{\partial P}$$
$$\begin{array}{l}\frac{\partial e}{\partial P^{\prime }}=-\left[\begin{array}{ccc}\frac{\partial u}{\partial X^{\prime }}& \frac{\partial u}{\partial Y^{\prime }}& \frac{\partial u}{\partial Z^{\prime }}\\ \frac{\partial v}{\partial X^{\prime }}& \frac{\partial v}{\partial Y^{\prime }}& \frac{\partial v}{\partial Z^{\prime }}\end{array}\right]=\left[\begin{array}{ccc}\frac{f_x}{Z^{\prime }}& 0& -\frac{f_x{X}^{\prime }}{Z^{\prime 2}}\\ 0& \frac{f_y}{Z^{\prime }}& -\frac{f_y{Y}^{\prime }}{Z^{\prime 2}}\end{array}\right]\\ P^{\prime }=RP+t=exp({\xi }^{\wedge })P\frac{\partial P^{\prime }}{\partial P}=R\end{array}\}\frac{\partial e}{\partial P}=\left[\begin{array}{ccc}\frac{f_x}{Z^{\prime }}& 0& -\frac{f_x{X}^{\prime }}{Z^{\prime 2}}\\ 0& \frac{f_y}{Z^{\prime }}& -\frac{f_y{Y}^{\prime }}{Z^{\prime 2}}\end{array}\right]R.$$

总结

引用:

1. [视觉SLAM14讲_7视觉里程1]
2. [视觉SLAM位姿求解方法-P3P]