ORBSLAM2之单目初始化(4)
文章目录
- 1、单目相机的投影模型，畸变模型
- 2、Essential、Fundamental 、Homography
- 单目相机只旋转不平移能不能求F，只旋转不平移能不能求H
- 为什么本质矩阵的秩为2
- 为什么基础矩阵的秩为2
- Essential、Fundamental 、Homography的自由度和计算需要的点对数
- 3、单目视觉slam中的尺度漂移
- 4、RANSAC算法
- ORBSLAM2基本矩阵的 **RANSAC** 估计
- ORBSLAM2单应矩阵的 **RANSAC** 估计
- 除了RANSAC之外，其他的鲁棒估计方法
- 待估计模型的自由度n和迭代次数k之间的关系
- 5、ORB特征
- ORB特征的尺度和旋转不变性是如何做的，BRIEF算子是怎么提取的
- Harris角点
- SIFT
- SUFT
- 6、影响三角化精度的因素
- 参考
1、单目相机的投影模型，畸变模型
https://blog.csdn.net/u011178262/article/details/74159480
2、Essential、Fundamental 、Homography
基本矩阵体现了两视图几何（对极几何，epipolar geometry）的内在射影几何（projective geometry）关系，基本矩阵只依赖于摄像机的内参K和外参R,t；
本质矩阵是归一化图像坐标下的基本矩阵的特殊情况。其参数由运动的位姿决定，与相机内参无关；
单应是射影几何中的概念，又称为射影变换。他把一个射影平面上的点映射到另一个平面对应的位置，并且把直线映射为直线，具有保线性质。与对极几何不同的是，对极几何将点映射到线上，而单应矩阵是点对点的关系。
https://blog.csdn.net/kokerf/article/details/72191054
单目相机只旋转不平移能不能求F，只旋转不平移能不能求H
E = t R F = K − T E K − 1 E=t^R \\ F=K^{-T}EK^{-1} E=tRF=K−TEK−1
H = K ( R − t n T d ) K − 1 H=K(R-\frac{tn^T}{d})K^{-1} H=K(R−dtnT)K−1
在相机只有旋转而没有平移的情况，此时t为0，E也将为0，导致无法求解R，这时可以使用单应矩阵H求旋转，但仅有旋转，无法三角化求深度。
为什么本质矩阵的秩为2
E = t ∧ R , t = ( t 1 , t 2 , t 3 ) , t ∧ = [ 0 − t 3 t 2 t 3 0 − t 1 − t 2 t 1 0 ] t 1 , t 2 , t 3 至 少 一 个 不 为 0 ， 否 则 就 没 有 平 移 了 ， 假 设 是 有 两 个 或 者 一 个 为 0 ， 显 然 必 有 R ( t ∧ ) = 2 , 若 都 不 为 0 ， 化 简 t ∧ : [ 0 − t 3 t 2 t 3 0 − t 1 − t 2 t 1 0 ] ⟶ [ 0 − t 3 t 2 t 2 t 3 0 − t 2 t 1 − t 2 t 3 t 1 t 3 0 ] ⟶ [ 0 − t 3 t 2 0 t 1 t 3 − t 2 t 1 − t 2 t 3 t 1 t 3 0 ] ⟶ [ 0 − t 1 t 3 t 1 t 2 0 t 1 t 3 − t 2 t 1 − t 2 t 3 t 1 t 3 0 ] ⟶ [ 0 0 0 0 t 1 t 3 − t 2 t 1 − t 2 t 3 t 1 t 3 0 ] ⟶ R ( t ∧ ) = 2 E=t^{\land}R,t=(t_1,t_2,t_3),t^{\land}=\begin{bmatrix} 0 & -t_3 & t_2 \\ t_3 & 0 & -t_1 \\ -t_2 & t_1 & 0 \end{bmatrix} \\ t_1,t_2,t_3至少一个不为0，否则就没有平移了，假设是有两个或者一个为0，显然必有R(t^{\land})=2,若都不为0，化简\\t^{\land}:\begin{bmatrix} 0 & -t_3 & t_2 \\ t_3 & 0 & -t_1 \\ -t_2 & t_1 & 0 \end{bmatrix} \longrightarrow \begin{bmatrix} 0 & -t_3 & t_2 \\ t_2t_3 & 0 & -t_2t_1 \\ -t_2t_3 & t_1t_3 & 0 \end{bmatrix}\longrightarrow \begin{bmatrix} 0 & -t_3 & t_2 \\ 0 & t_1t_3 & -t_2t_1 \\ -t_2t_3 & t_1t_3 & 0 \end{bmatrix} \\ \longrightarrow \begin{bmatrix} 0 & -t_1t_3 & t_1t_2 \\ 0 & t_1t_3 & -t_2t_1 \\ -t_2t_3 & t_1t_3 & 0 \end{bmatrix} \longrightarrow \begin{bmatrix} 0 & 0 & 0 \\ 0 & t_1t_3 & -t_2t_1 \\ -t_2t_3 & t_1t_3 & 0 \end{bmatrix} \longrightarrow R(t^{\land})=2 E=t∧R,t=(t1,t2,t3),t∧=⎣⎡0t3−t2−t30t1t2−t10⎦⎤t1,t2,t3至少一个不为0，否则就没有平移了，假设是有两个或者一个为0，显然必有R(t∧)=2,若都不为0，化简t∧:⎣⎡0t3−t2−t30t1t2−t10⎦⎤⟶⎣⎡0t2t3−t2t3−t30t1t3t2−t2t10⎦⎤⟶⎣⎡00−t2t3−t3t1t3t1t3t2−t2t10⎦⎤⟶⎣⎡00−t2t3−t1t3t1t3t1t3t1t2−t2t10⎦⎤⟶⎣⎡00−t2t30t1t3t1t30−t2t10⎦⎤⟶R(t∧)=2
R是旋转矩阵，是可逆矩阵，秩是3，因此R(E)=R(t^ R)=R(t^)=2
为什么基础矩阵的秩为2
F = K − T E K − 1 F=K^{-T}EK^{-1} F=K−TEK−1
显然R（F）=R（E)
Essential、Fundamental 、Homography的自由度和计算需要的点对数
- 本质矩阵
平移t的自由度是3，旋转矩阵R的自由度是3，E=t^R加在一起是6个参数，也就是要想确定E矩阵，确定6个参数就可以。同时E满足尺度等价性约束，所以F的自由度是6-1=5.
E矩阵一对匹配点只能构造一个约束方程，因此至少需要五对匹配点才能计算（五点法，另外还有八点法）：
考 虑 一 对 匹 配 点 x 1 = [ u 1 , v 1 , 1 ] T , x 2 = [ u 2 , v 2 , 1 ] T , 跟 据 对 极 约 束 ： ⟶ [ u 1 v 1 1 ] [ e 1 e 2 e 3 e 4 e 5 e 6 e 7 e 8 e 9 ] [ u 2 v 2 1 ] = 0 ⟶ 把 E 展 开 写 出 向 量 形 式 ： u 2 u 2 e 1 + u 1 v 2 e 2 + u 1 e 3 + v 1 u 2 e 4 + v 1 v 2 e 5 + v 1 e 6 + u 2 e 7 + v 2 e 8 + e 9 = 0 考虑一对匹配点x_1=[u_1,v_1,1]^T,x_2=[u_2,v_2,1]^T,跟据对极约束：\\ \longrightarrow \begin{bmatrix} u_1 & v_1 & 1 \end{bmatrix}\begin{bmatrix} e_1 & e_2 & e_3 \\ e_4 & e_5 & e_6 \\ e_7 & e_8 & e_9 \end{bmatrix} \begin{bmatrix} u_2 \\ v_2 \\ 1 \end{bmatrix}=0\\ \longrightarrow 把E展开写出向量形式： u_2u_2e_1+u_1v_2e_2+u_1e_3+v_1u_2e_4+v_1v_2e_5+v_1e_6+u_2e_7+v_2e_8+e_9=0 考虑一对匹配点x1=[u1,v1,1]T,x2=[u2,v2,1]T,跟据对极约束：⟶[u1v11]⎣⎡e1e4e7e2e5e8e3e6e9⎦⎤⎣⎡u2v21⎦⎤=0⟶把E展开写出向量形式：u2u2e1+u1v2e2+u1e3+v1u2e4+v1v2e5+v1e6+u2e7+v2e8+e9=0
- 基础矩阵
F = K − T E K − 1 F=K^{-T}EK^{-1} F=K−TEK−1
相机内参的待定参数为4，E矩阵自由度5，所以计算F的自由度最大是9。
同时F满足下面两个约束，所以F的自由度是9-2=7.
（1）尺度等价性
（2）det(F)=0
F矩阵一对匹配点同样只能构造一个约束方程，因此至少需要7对匹配点才能计算（七点法，另外还有八点法）
- 单应矩阵
单应矩阵9个未知参数，具有尺度等价性：9-1=8
考 虑 一 对 匹 配 点 x 1 = [ u 1 , v 1 , 1 ] T , x 2 = [ u 2 , v 2 , 1 ] T , H 是 单 应 矩 阵 ： ⟶ [ u 2 v 2 1 ] = [ h 1 h 2 h 3 h 4 h 5 h 6 h 7 h 8 h 9 ] [ u 1 v 1 1 ] ⟶ u 2 = h 1 u 1 + h 2 v 1 + h 3 h 7 u 1 + h 8 v 1 + h 9 , v 2 = h 4 u 1 + h 5 v 1 + h 6 h 7 u 1 + h 8 v 1 + h 9 考虑一对匹配点x_1=[u_1,v_1,1]^T,x_2=[u_2,v_2,1]^T,H是单应矩阵：\\ \longrightarrow \begin{bmatrix} u_2\\ v_2\\ 1 \end{bmatrix}=\begin{bmatrix} h_1 & h_2 & h_3 \\ h_4 & h_5 & h_6 \\ h_7 & h_8 & h_9 \end{bmatrix} \begin{bmatrix} u_1 \\ v_1 \\ 1 \end{bmatrix}\\ \longrightarrow u_2=\frac{h_1u_1+h_2v_1+h_3}{h_7u_1+h_8v_1+h_9},v_2=\frac{h_4u_1+h_5v_1+h_6}{h_7u_1+h_8v_1+h_9} 考虑一对匹配点x1=[u1,v1,1]T,x2=[u2,v2,1]T,H是单应矩阵：⟶⎣⎡u2v21⎦⎤=⎣⎡h1h4h7h2h5h8h3h6h9⎦⎤⎣⎡u1v11⎦⎤⟶u2=h7u1+h8v1+h9h1u1+h2v1+h3,v2=h7u1+h8v1+h9h4u1+h5v1+h6
因此一组匹配点对可以构造出两个约束，因此至少4对点（不共线）就可以计算单应矩阵。
3、单目视觉slam中的尺度漂移
单目相机根据一张图片无法得出一张图片中物体的实际大小，同理也就无法得出运动的尺度大小，这是产生尺度漂移的根源。而在优化过程中，单目相机使用对极几何中的三角测量原理，而三角测量中，极小的角度误差在累积之后深度不确定都会变得很大，从而无法保证尺度一致性。修正方式是通过回环检测。
Scale Drift-Aware Large Scale Monocular SLAM
4、RANSAC算法
RANSAC算法的优点是能鲁棒的估计模型参数。例如，他能从包含大量局外点的数据集中估计出高精度的参数。缺点是它计算参数的迭代次数没有上限，如果设置迭代次数的上限，得到的结果可能不是最优的结果，甚至可能得到错误的结果。RANSAC只有一定的概率得到的可信的模型，概率与迭代次数成正比。另一个缺点是它要求设置跟问题相关的阈值，RANSAC只能从特定的数据集中估计出一个模型，如果存在两个（或多个）模型，RANSAC不能找到别的模型。
ORBSLAM2基本矩阵的 RANSAC 估计
RANSAC 方法可以从有误匹配的点对应集中估计基本矩阵，这使得从两幅图像自动估计基本矩 阵成为可能。基本矩阵自动估计的步骤如下：
- 1、自动提取两幅图像的特征点集并建立初始“匹配对集” ；
//mvSets是初始“匹配对集” ，mMaxIterations=200，初始化了200个大小为8，所有值是0的点集
mvSets = vector< vector<size_t> >(mMaxIterations,vector<size_t>(8,0));
DUtils::Random::SeedRandOnce(0);
for(int it=0; it<mMaxIterations; it++)
{
vAvailableIndices = vAllIndices;
// Select a minimum set
for(size_t j=0; j<8; j++)
{
// 产生0到N-1的随机数
int randi = DUtils::Random::RandomInt(0,vAvailableIndices.size()-1);
// idx表示哪一个索引对应的特征点被选中
int idx = vAvailableIndices[randi];
mvSets[it][j] = idx;
// randi对应的索引已经被选过了，从容器中删除
// randi对应的索引用最后一个元素替换，并删掉最后一个元素
vAvailableIndices[randi] = vAvailableIndices.back();
vAvailableIndices.pop_back();
}
}
- 2、 RANSAC 去除错误匹配对
随机从数据集中随机抽出8个样本数据，计算当前抽样所确定的基本矩阵 F模型，记为M；
计算数据集中所有数据（所有内点）与模型M的重投影误差，这里利用卡方检验进行选取内点，若误差小于阈值，加入内点集 I （vbCurrentInliers），并且获得模型的当次RANSAC的结果评分；
如果当前内点集 I 元素个数大于最优内点集 I_best , 则更新 I_best = I
如果迭代次数大于200,则退出 ; 否则迭代次数加1，并重复上述步骤；
vector<bool> vbCurrentInliers(N,false);
float currentScore;
// Perform all RANSAC iterations and save the solution with highest score
for(int it=0; it<mMaxIterations; it++)
{
// Select a minimum set
for(int j=0; j<8; j++)
{
int idx = mvSets[it][j];
vPn1i[j] = vPn1[mvMatches12[idx].first];
vPn2i[j] = vPn2[mvMatches12[idx].second];
}
cv::Mat Fn = ComputeF21(vPn1i,vPn2i);
F21i = T2t*Fn*T1;
// 利用重投影误差为当次RANSAC的结果评分
currentScore = CheckFundamental(F21i, vbCurrentInliers, mSigma);
if(currentScore>score)
{
F21 = F21i.clone();
vbMatchesInliers = vbCurrentInliers;
score = currentScore;
}
}
ORBSLAM2单应矩阵的 RANSAC 估计
随机从数据集中随机抽出8个样本数据，计算出变换矩阵H，记为模型M；
计算数据集中所有数据与模型M的投影误差，若误差小于阈值，加入内点集 I ；
如果当前内点集 I 元素个数大于最优内点集 I_best（也就是得分比较高） , 则更新 I_best = I；
如果迭代次数大于200,则退出 ; 否则迭代次数加1，并重复上述步骤；
// 每次RANSAC的MatchesInliers与得分
vector<bool> vbCurrentInliers(N,false);
float currentScore;
// Perform all RANSAC iterations and save the solution with highest score
for(int it=0; it<mMaxIterations; it++)
{
// Select a minimum set
for(size_t j=0; j<8; j++)
{
int idx = mvSets[it][j];
// vPn1i和vPn2i为匹配的特征点对的坐标
vPn1i[j] = vPn1[mvMatches12[idx].first];
vPn2i[j] = vPn2[mvMatches12[idx].second];
}
cv::Mat Hn = ComputeH21(vPn1i,vPn2i);
// 恢复原始的均值和尺度
H21i = T2inv*Hn*T1;
H12i = H21i.inv();
// 利用重投影误差为当次RANSAC的结果评分
currentScore = CheckHomography(H21i, H12i, vbCurrentInliers, mSigma);
// 得到最优的vbMatchesInliers与score
if(currentScore>score)
{
H21 = H21i.clone();
vbMatchesInliers = vbCurrentInliers;
score = currentScore;
}
}
除了RANSAC之外，其他的鲁棒估计方法
- M－估计方法
- 最小中值方法
- 鲁棒最大后验估计
待估计模型的自由度n和迭代次数k之间的关系
也就是说迭代次数是如何确定的？
在执行 RANSAC 时，通常没有必要尝试每一种可能的抽样，实际上尝试每一种可能的抽样在 计算上也是不可行的。只要选择足够多的抽样次数 K，保证至少能得一个好样本就可以了。
RANSAC是一种概率算法，为了能确保有更好的概率找到真正的内点集合，必须实验足够多的次数。以下为试验次数的计算过程：
1、假设每次选取测量点都是相互独立的，且每个测量点为内点的概率均为w，p为经过k次试验后成功的总体概率；
2、那么在某次实验中，n个随机样本都是内点的可能性是wn（这里n表示为拟合该模型需要的最少数据个数）；
3、因此经过了p次试验，失败的概率是：
1 − p = ( 1 − w n ) k 1-p=(1-w^n)^k 1−p=(1−wn)k
得到最少需要的试验次数为：
k = l o g ( 1 − p ) l o g ( 1 − w n ) k=\frac{log(1-p)}{log(1-w^n)} k=log(1−wn)log(1−p)
也就是说K受到外点比例以及模型自由度的影响。
图表来自《计算机视觉中的数学方法》
也就是说在其他因素（内点比例）等固定的情况下，模型的自由度越高，选出好模型需要的迭代次数越高。
5、ORB特征
ORB特征的尺度和旋转不变性是如何做的，BRIEF算子是怎么提取的
- Fast角点：ORB算法的特征提取是由FAST算法改进的，这里成为Orientated FAST。也就是说，在使用FAST提取出特征点之后，给其定义一个特征点方向，以此来实现特征点的旋转不变形，使用灰度质心法实现。
特征点的尺度不变形：建立金字塔，来实现特征点的多尺度不变性。设置一个比例因子scaleFactor（opencv默认为1.2）和金字塔的层数nlevels（pencv默认为8）。将原图像按比例因子缩小成nlevels幅图像。nlevels幅不同比例的图像提取特征点总和作为这幅图像的oFAST特征点。
- BRIEF特征描述子：BRIEF算法计算出来的是一个二进制串的特征描述符。它是在一个特征点的邻域内，选择n对像素点pi、qi（i=1,2,…,n）。然后比较每个点对的灰度值的大小。如果I(pi)> I(qi)，则生成二进制串中的1，否则为0。所有的点对都进行比较，则生成长度为n的二进制串。一般n取128、256或512，opencv默认为256。原始的BRIEF不具有旋转不变性，使用steeredBRIEF方法得到的特征描述子具有旋转不变性。
参考自：https://blog.csdn.net/weixin_44580210/article/details/89514870
Harris角点
- 角点（corner points）
局部窗口沿各方向移动，均产生明显变化的点
图像局部曲率突变的点 - Harris角点：基于局部信号的自相关函数检测角点的方法。通过一个小的滑动窗口在邻域检测角点在任意方向上移动窗口，若窗口内的灰度值都有剧烈的变化，则窗口的中心就是角点。
优点：计算简单；提取的点特征均匀且合理；稳定，稳定Harris算子对图像旋转、亮度变化、噪声影响和视点变换不敏感。
缺点：对尺度很敏感，不具有尺度不变性；提取的角点精度是像素级的；需要设计角点匹配算法
基于特征描述子的方法-harris
SIFT
- SIFT特征点：利用高斯金字塔和DOG函数进行特征点提取。高斯金字塔的当前层图像是对其前一层图像先进行高斯低通滤波，然后做隔行和隔列的降采样(去除偶数行与偶数列)而生成的。DoG (Difference of Gaussian)是高斯函数的差分，具体到图像处理来讲，就是将同一幅图像经过两个不同高斯滤波得到两幅滤波图像，将这两幅图像相减，得到DoG图。DOG图上的邻域梯度方向直方图峰值即特征点的主方向。
- SIFT特征描述子：以特征点为中心取窗口，通过高斯加权增强特征点附近像素梯度方向信息的贡献，即在4 × 4的小块上计算梯度方向直方图（ 取8个方向），计算梯度方向累加值，形成种子点，构成4× 4 × 8= 128维特征向量。然后进行统计。
优点：SIFT特征对旋转、尺度缩放、亮度变化保持不变性，对视角变化、仿射变换、噪声也保持一定程度的稳定性。
SUFT
SUFT是对SIFT的改进，他们在思路上是一致的，只是采用的方法不同而已
- SUFT特征点：基于Hessian矩阵构造金字塔尺度空间，利用箱式滤波器（box filter）简化二维高斯滤波
- SUFT特征描述子：通过Haar小波特征设定特征点主方向，这样构建的特征描述子4×4×4=64维描述子
6、影响三角化精度的因素
三角化的误差来源有哪几方面：
-
a.图像的分辨率：图像的分辨率越高，一个像素所带来的δβ就越小。
-
b.特征点求取时的精度：是否做到亚像素，在亚像素的基础上，误差有多大？
-
c.p1点的误差：会引起极线l2的误差，从而间接地影响p2点的精度(特征点匹配误差）。
-
d.相机两次位置的平移向量t的大小
所以，这也体现出来了三角化的矛盾：若想提高三角化的精度，其一提高特征点的提取精度，即提高图像的分辨率，但这会导致图像的增大，增加计算成本；其二，使平移量增大，但这会导致图像外观的明显变化，外观变化会使得特征提取与匹配变得困难。总而言之，平移太大，会导致匹配失效；平移太小，三角化精度不够。
参考：三角化—深度滤波器《视觉SLAM14讲》
参考
https://blog.csdn.net/weixin_44580210/article/details/87214464#12_SLAM_192
《视觉SLAM14讲》
《计算机视觉中的数学方法》