ORBSLAM2之单目初始化(2)
文章目录
- 第二阶段：根据匹配计算两帧之间的位姿
- 特征点归一化-Normalize()
- 解除归一化
- 计算Homography，获取模型评分
- **1.将特征点归一化**
- **2.求解单应性矩阵（归一化四点法）**
- **3.Homography模型评分**
- 计算Fundamental，获取模型评分
- **1.将特征点归一化**
- **2.求解基础矩阵（归一化八点法）**
- **3.Fundamental模型评分**
- 计算Essential
- 为什么Ax=0的解是V的最后一列
- Homograph 矩阵分解恢复R，t
- Essential 矩阵分解恢复R，t
- 参考
第二阶段：根据匹配计算两帧之间的位姿
本质矩阵不能处理平面场景，单应矩阵不能处理非平面的场景,分单应矩阵和本质矩阵来分别恢复R和T
-
1 结构化匹配上的特征点对，并且归一化
Initializer::Normalize（）
-
2 在所有匹配特征点对中随机选择8对匹配特征点为一组，用于FindHomography和FindFundamental求解
-
3 调用多线程分别用于计算fundamental 和homography 矩阵
（Initializer::FindFundamental（） and Initializer::FindHomography（），返回模型分数）
-
4 计算得分比例，选取某个模型
-
5 从H矩阵或F矩阵中恢复R,t（ReconstructH(）or ReconstructF(））
ORB_SLAM2/src/Tracking.cpp--void Tracking::MonocularInitialization()
...
cv::Mat Rcw; // Current Camera Rotation
cv::Mat tcw; // Current Camera Translation
vector<bool> vbTriangulated; // Triangulated Correspondences (mvIniMatches)
//调用Initialize函数，并行地计算基础矩阵和单应性矩阵，选取其中一个模型，恢复出最开始两帧之间的相对姿态以及点云
if(mpInitializer->Initialize(mCurrentFrame, mvIniMatches, Rcw, tcw, mvIniP3D, vbTriangulated))
....
ORB_SLAM2/src/Initializer.cpp--bool Initializer::Initialize()
...
// 用current frame,也就是用SLAM逻辑上的第二帧来初始化整个SLAM，得到最开始两帧之间的R t,以及点云
bool Initializer::Initialize(const Frame &CurrentFrame, const vector<int> &vMatches12, cv::Mat &R21, cv::Mat &t21,vector<cv::Point3f> &vP3D, vector<bool> &vbTriangulated)
{
// Fill structures with current keypoints and matches with reference frame
// Reference Frame: 1, Current Frame: 2
// Frame2 特征点
mvKeys2 = CurrentFrame.mvKeysUn;
// mvMatches12记录匹配上的特征点对
mvMatches12.clear();
mvMatches12.reserve(mvKeys2.size());
// mvbMatched1记录每个特征点是否有匹配的特征点，
// 这个变量后面没有用到，后面只关心匹配上的特征点
mvbMatched1.resize(mvKeys1.size());
// 步骤1：组织特征点对
//typedef pair Match;
// vector mvMatches12;< Match的数据结构是pair,mvMatches12只记录Reference到Current匹配上的特征点对
for(size_t i=0, iend=vMatches12.size();i<iend; i++)
{
if(vMatches12[i]>=0)
{
mvMatches12.push_back(make_pair(i,vMatches12[i]));
//vector mvbMatched1;记录Reference Frame的每个特征点在Current Frame是否有匹配的特征点
mvbMatched1[i]=true;
}
else
mvbMatched1[i]=false;
}
// 匹配上的特征点的个数
const int N = mvMatches12.size();
// Indices for minimum set selection
// 新建一个容器vAllIndices，生成0到N-1的数作为特征点的索引
vector<size_t> vAllIndices;
vAllIndices.reserve(N);
vector<size_t> vAvailableIndices;
for(int i=0; i<N; i++)
{
vAllIndices.push_back(i);
}
// Generate sets of 8 points for each RANSAC iteration
// 步骤2：在所有匹配特征点对中随机选择8对匹配特征点为一组，共选择mMaxIterations组
// 用于FindHomography和FindFundamental求解
// mMaxIterations:200
mvSets = vector< vector<size_t> >(mMaxIterations,vector<size_t>(8,0));//二维容器，外层容器的大小为迭代次数，内层容器大小为每次迭代算H或F矩阵需要的点
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();
}
}
// Launch threads to compute in parallel a fundamental matrix and a homography
// 步骤3：调用多线程分别用于计算fundamental matrix和homography
vector<bool> vbMatchesInliersH, vbMatchesInliersF;
float SH, SF; // score for H and F
cv::Mat H, F; // H and F
// ref是引用的功能:http://en.cppreference.com/w/cpp/utility/functional/ref
// 计算homograpy并打分
thread threadH(&Initializer::FindHomography,this,ref(vbMatchesInliersH), ref(SH), ref(H));
// 计算fundamental matrix并打分
thread threadF(&Initializer::FindFundamental,this,ref(vbMatchesInliersF), ref(SF), ref(F));
// Wait until both threads have finished
threadH.join();
threadF.join();
// Compute ratio of scores
// 步骤4：计算得分比例，选取某个模型
float RH = SH/(SH+SF);
// Try to reconstruct from homography or fundamental depending on the ratio (0.40-0.45)
// 步骤5：从H矩阵或F矩阵中恢复R,t
if(RH>0.40)
return ReconstructH(vbMatchesInliersH,H,mK,R21,t21,vP3D,vbTriangulated,1.0,50);
else //if(pF_HF>0.6)
return ReconstructF(vbMatchesInliersF,F,mK,R21,t21,vP3D,vbTriangulated,1.0,50);
return false;
}
ORB_SLAM实现的是自动初始化，也就是，无论场景平面，还是普通场景，都能完成初始化工作。其做法是同时计算适用于平面场景的单应性矩阵（H）和适用于非平面场景的基础矩阵(F),
首先，由抽样点对，计算出H（归一化4点法）和F矩阵（归一化八点法）；通过若干次RANSAC抽样，计算出最优的H和F矩阵；然后，通过比较H和F矩阵，获得评价分数，选择最优的矩阵，恢复出帧间位姿。
但如果两个模型分值都不高（意味着没有足够的局内点），就重新选择第二帧，重新匹配并尝试初始化。
一旦选择好模型，我们就可以获得相应的运动状态。如果选择单应矩阵，按照Faugeras等人发表的论文中提到的方法，提取8种运动假设，该方法提出用cheriality check测试来选择有效解。
然而，如果在低视差的情况下，这些测试就会失效，因为云点很容易在相机的前面或后面移动，会导致选解错误。我们提出的方法是直接按这8种解将二维点三角化，然后检查是否有一种解可以使得所有的云点都位于两个相机的前面，且重投影误差较小。如果没有一个最优的解，我们就不执行初始化，否则重新选择第二帧，重新匹配并尝试初始化。
特征点归一化-Normalize()
将mvKeys1和mvKey2归一化到均值为0，一阶绝对矩为1，归一化矩阵分别为T1、T2
在计算H 或者 F矩阵的时候需要对特征点进行坐标变换，称之为归一化。
计算单应矩阵时变换特征点的坐标会得到更好的效果，包括坐标的平移和尺度缩放，并且这一步骤必须放在DLT之前。DLT之后再还原到原坐标系。书本指出归一化与条件数确切的说是DTL矩阵A的第一个和倒数第二个奇异值的比例有关。有充分证据表明在精确数据和无限精度的算术运算条件下，归一化并不起作用，但是有噪声存在时解将偏离其正确结果。
步骤
(1) 1. 计 算 坐 标 均 值 ： m e a n X = ( ∑ i = 0 N u i ) / N , m e a n Y = ( ∑ i = 0 N v i ) / N 1.计算坐标均值：meanX=(\sum\limits_{i=0}^{N} u_i)/N,meanY=(\sum\limits_{i=0}^{N} v_i)/N \tag{1} 1.计算坐标均值：meanX=(i=0∑Nui)/N,meanY=(i=0∑Nvi)/N(1)
(2) 2. 计 算 一 阶 绝 对 矩 和 对 应 倒 数 ： m e a n D e v X = ( ∑ i = 0 N ∣ u i − m e a n X ∣ ) / N m e a n D e v Y = ( ∑ i = 0 N ∣ v i − m e a n X ∣ ) / N 2.计算一阶绝对矩和对应倒数：\\meanDevX=(\sum\limits_{i=0}^{N} |u_i-meanX|)/N\\meanDevY=(\sum\limits_{i=0}^{N} |v_i-meanX|)/N \tag{2} 2.计算一阶绝对矩和对应倒数：meanDevX=(i=0∑N∣ui−meanX∣)/NmeanDevY=(i=0∑N∣vi−meanX∣)/N(2)
(3) s X = 1 / m e a n D e v X , s Y = 1 / m e a n D e v Y sX=1/meanDevX,sY=1/meanDevY \tag{3} sX=1/meanDevX,sY=1/meanDevY(3)
(4) 3. 计 算 标 准 化 后 的 坐 标 ： u i , = ( u i − m e a n X ) ∗ s X , v i , = ( v i − m e a n Y ) ∗ s Y 矩 阵 形 式 ： [ u i , v i , 1 ] = [ s X 0 − m e a n X ∗ s X 0 s Y − m e a n Y ∗ s Y 0 0 1 ] [ u i v i 1 ] 3.计算标准化后的坐标：u_i^,=(u_i-meanX)*sX,v_i^,=(v_i-meanY)*sY \\ 矩阵形式：\\ \begin{bmatrix} u_i^, \\ v_i^, \\ 1 \end{bmatrix}=\begin{bmatrix} sX & 0 & -meanX*sX \\ 0 & sY & -meanY*sY \\ 0 & 0 & 1 \end{bmatrix} \begin{bmatrix} u_i \\ v_i \\ 1 \end{bmatrix} \tag{4} 3.计算标准化后的坐标：ui,=(ui−meanX)∗sX,vi,=(vi−meanY)∗sY矩阵形式：⎣⎡ui,vi,1⎦⎤=⎣⎡sX000sY0−meanX∗sX−meanY∗sY1⎦⎤⎣⎡uivi1⎦⎤(4)
(5) 4. 返 回 标 准 化 矩 阵 ： T = [ s X 0 − m e a n X ∗ s X 0 s Y − m e a n Y ∗ s Y 0 0 1 ] 4.返回标准化矩阵：T=\begin{bmatrix} sX & 0 & -meanX*sX \\ 0 & sY & -meanY*sY \\ 0 & 0 & 1 \end{bmatrix} \tag{5} 4.返回标准化矩阵：T=⎣⎡sX000sY0−meanX∗sX−meanY∗sY1⎦⎤(5)
参考自《计算机视觉中的多视图几何》 3.4.4
ORB_SLAM2/src/Initializer.cpp--void Initializer::Normalize()
...
void Initializer::Normalize(const vector<cv::KeyPoint> &vKeys, vector<cv::Point2f> &vNormalizedPoints, cv::Mat &T)
{
float meanX = 0;
float meanY = 0;
const int N = vKeys.size();
vNormalizedPoints.resize(N);
for(int i=0; i<N; i++)
{
meanX += vKeys[i].pt.x;
meanY += vKeys[i].pt.y;
}
meanX = meanX/N;
meanY = meanY/N;
float meanDevX = 0;
float meanDevY = 0;
// 将所有vKeys点减去中心坐标，使x坐标和y坐标均值分别为0
for(int i=0; i<N; i++)
{
vNormalizedPoints[i].x = vKeys[i].pt.x - meanX;
vNormalizedPoints[i].y = vKeys[i].pt.y - meanY;
meanDevX += fabs(vNormalizedPoints[i].x);
meanDevY += fabs(vNormalizedPoints[i].y);
}
meanDevX = meanDevX/N;
meanDevY = meanDevY/N;
float sX = 1.0/meanDevX;
float sY = 1.0/meanDevY;
// 将x坐标和y坐标分别进行尺度缩放，使得x坐标和y坐标的一阶绝对矩分别为1
for(int i=0; i<N; i++)
{
vNormalizedPoints[i].x = vNormalizedPoints[i].x * sX;
vNormalizedPoints[i].y = vNormalizedPoints[i].y * sY;
}
// |sX 0 -meanx*sX|
// |0 sY -meany*sY|
// |0 0 1 |
T = cv::Mat::eye(3,3,CV_32F);
T.at<float>(0,0) = sX;
T.at<float>(1,1) = sY;
T.at<float>(0,2) = -meanX*sX;
T.at<float>(1,2) = -meanY*sY;
}
...
解除归一化
单应矩阵
x 1 , = T x 1 , x 2 , = T , x 2 x 1 , , x 2 , 是 归 一 化 后 的 坐 标 数 据 ， x 1 , x 2 是 原 始 数 据 根 据 单 应 矩 阵 模 型 ： x 1 , = H x 2 , ⟶ T x 1 = H T , x 2 ⟶ x 1 = T − 1 H T , x 2 x_1^,=Tx_1,x_2^,=T^,x_2\\ x_1^,,x_2^,是归一化后的坐标数据，x_1,x_2是原始数据\\ 根据单应矩阵模型：x_1^,=Hx_2^,\\ \longrightarrow Tx_1=HT^,x_2\\ \longrightarrow x_1=T^{-1}HT^,x_2 x1,=Tx1,x2,=T,x2x1,,x2,是归一化后的坐标数据，x1,x2是原始数据根据单应矩阵模型：x1,=Hx2,⟶Tx1=HT,x2⟶x1=T−1HT,x2
基础矩阵
x 1 , = T x 1 , x 2 , = T , x 2 x 1 , , x 2 , 是 归 一 化 后 的 坐 标 数 据 ， x 1 , x 2 是 原 始 数 据 根 据 基 础 矩 阵 性 质 ： ( x 1 , ) T F x 2 , = 0 ⟶ ( T x 1 ) T F T , x 2 = 0 ⟶ x 1 T T T F T , x 2 = 0 x_1^,=Tx_1,x_2^,=T^,x_2\\ x_1^,,x_2^,是归一化后的坐标数据，x_1,x_2是原始数据\\ 根据基础矩阵性质：(x_1^,)^TFx_2^,=0\\ \longrightarrow (Tx_1)^TFT^,x_2=0\\ \longrightarrow x_1^TT^TFT^,x_2=0 x1,=Tx1,x2,=T,x2x1,,x2,是归一化后的坐标数据，x1,x2是原始数据根据基础矩阵性质：(x1,)TFx2,=0⟶(Tx1)TFT,x2=0⟶x1TTTFT,x2=0
计算Homography，获取模型评分
ORB_SLAM2/src/Initializer.cpp--void Initializer::FindHomography()
...
/**
* @brief 计算单应矩阵
*
* 假设场景为平面情况下通过前两帧求取Homography矩阵(current frame 2 到 reference frame 1),并得到该模型的评分
*/
void Initializer::FindHomography(vector<bool> &vbMatchesInliers, float &score, cv::Mat &H21)
{
...
//1.特征点归一化
Normalize(mvKeys1,vPn1, T1);
Normalize(mvKeys2,vPn2, T2);
cv::Mat T2inv = T2.inv();
//2.求解单应矩阵
cv::Mat Hn = ComputeH21(vPn1i,vPn2i);
// 恢复原始的均值和尺度（解除归一化）
H21i = T2inv*Hn*T1;
H12i = H21i.inv();
// 3.利用重投影误差为当次RANSAC的结果评分
currentScore = CheckHomography(H21i, H12i, vbCurrentInliers, mSigma);
...
}
...
1.将特征点归一化
见特征点归一化
2.求解单应性矩阵（归一化四点法）
单应性矩阵的模型：
(1) [ x , y , 1 ] = λ [ h 1 h 2 h 3 h 4 h 5 h 6 h 7 h 8 h 9 ] [ x y 1 ] \begin{bmatrix} x^, \\ y^, \\ 1 \end{bmatrix} =\lambda\begin{bmatrix} h_1 & h_2 & h_3 \\ h_4 & h_5 & h_6 \\ h_7 & h_8 & h_9 \end{bmatrix} \begin{bmatrix} x \\ y \\ 1 \end{bmatrix} \tag{1} ⎣⎡x,y,1⎦⎤=λ⎣⎡h1h4h7h2h5h8h3h6h9⎦⎤⎣⎡xy1⎦⎤(1)
(2) ⟶ x , = λ H x 这 是 一 个 齐 次 矢 量 方 程 ， 因 此 3 维 矢 量 x i , 和 H x i 不 相 等 ， 但 是 具 有 相 同 的 方 向 ， 利 用 叉 乘 性 质 ⟶ x i , × H x i = 0 ⟶ A h = 0 其 中 A = [ 0 0 0 − x − y − 1 x y , y y , y , − x − y − 1 0 0 0 x x , y x , x , ] h = [ h 1 h 2 h 3 h 4 h 5 h 6 h 7 h 8 h 9 ] , \longrightarrow x^,=\lambda Hx \tag{2} \\ 这是一个齐次矢量方程，因此3维矢量x_i^,和Hx_i不相等，但是具有相同的方向，利用叉乘性质\\ \longrightarrow x_i^,\times Hx_i=0 \\ \longrightarrow Ah=0 \\ 其中A=\begin{bmatrix} 0 & 0 & 0 & -x & -y & -1 & xy^,& yy^, & y^,\\ -x & -y & -1 & 0 & 0 & 0 & xx^,& yx^, & x^, \end{bmatrix} \\ h=\begin{bmatrix} h_1 & h_2 & h_3 & h_4 & h_5 & h_6 & h_7& h_8 & h_9\\ \end{bmatrix}^, \\ ⟶x,=λHx这是一个齐次矢量方程，因此3维矢量xi,和Hxi不相等，但是具有相同的方向，利用叉乘性质⟶xi,×Hxi=0⟶Ah=0其中A=[0−x0−y0−1−x0−y0−10xy,xx,yy,yx,y,x,]h=[h1h2h3h4h5h6h7h8h9],(2)
《计算机视觉中的多视图几何》P53-P55
其他推导方法
ORB_SLAM2/src/Initializer.cpp--cv::Mat Initializer::ComputeH21()
...
cv::Mat Initializer::ComputeH21(const vector<cv::Point2f> &vP1, const vector<cv::Point2f> &vP2)
{
const int N = vP1.size();
cv::Mat A(2*N,9,CV_32F); // 2N*9
...
...
cv::Mat u,w,vt;
//cv::SVDecomp():https://docs.opencv.org/3.1.0/d2/de8/group__core__array.html#gab477b5b7b39b370bb03e75b19d2d5109
cv::SVDecomp(A,w,u,vt,cv::SVD::MODIFY_A | cv::SVD::FULL_UV);
return vt.row(8).reshape(0, 3); // v的最后一列
}
...
3.Homography模型评分
重投影误差(图像平面和目的图像平面之间的透视变换矩阵H）:
s i [ x i , y i , 1 ] = H [ x i y i 1 ] 重 投 影 投 影 误 差 : ∑ i ( x i , − h 11 x i + h 12 y i + h 13 h 31 x i + h 32 y i + h 33 ) 2 + ( y i , − h 21 x i + h 22 y i + h 23 h 31 x i + h 32 y i + h 33 ) 2 s_i\begin{bmatrix} x_i^, \\ y_i^, \\ 1 \end{bmatrix}=H \begin{bmatrix} x_i \\ y_i \\ 1 \end{bmatrix} \\ 重投影投影误差: \sum \limits_{i}(x^,_i-\frac{h_{11}x_i+h_{12}y_i+h_{13}}{h_{31}x_i+h_{32}y_i+h_{33}})^2+(y^,_i-\frac{h_{21}x_i+h_{22}y_i+h_{23}}{h_{31}x_i+h_{32}y_i+h_{33}})^2 si⎣⎡xi,yi,1⎦⎤=H⎣⎡xiyi1⎦⎤重投影投影误差:i∑(xi,−h31xi+h32yi+h33h11xi+h12yi+h13)2+(yi,−h31xi+h32yi+h33h21xi+h22yi+h23)2
显然估计出的单应矩阵Hˆ为使得误差最小时H的值。
由于两幅图像中的测量点x,x′都有误差，假设估计的变换为Hˆ，它的逆变换为Hˆ-1。则此时的几何误差就是 (将两点x,y之间的欧氏距离记作d(x,y))：
∑ i = 0 d ( x , , H ^ x i ) 2 + d ( x i , H ^ − 1 x i , ) 2 − 对 称 转 移 误 差 \sum\limits_{i=0}^{}d(x^,,\hat Hx_i)^2+d(x_i,\hat H^{-1}x^,_i)^2-对称转移误差 i=0∑d(x,,H^xi)2+d(xi,H^−1xi,)2−对称转移误差
s c o r e H = ∑ i = 0 N ρ ( T H − ∣ ∣ x , − H ^ x ∣ ∣ 2 / σ 2 ) + ρ ( T H − ∣ ∣ x − H ^ − 1 x , ∣ ∣ 2 / σ 2 ) scoreH=\sum\limits_{i=0}^{N}\rho(T_H-||x^,-\hat Hx||^2/\sigma^2)+\rho(T_H-||x-\hat H^{-1}x^,||^2/\sigma^2)\\ scoreH=i=0∑Nρ(TH−∣∣x,−H^x∣∣2/σ2)+ρ(TH−∣∣x−H^−1x,∣∣2/σ2)
ORB_SLAM2/src/Initializer.cpp--float Initializer::CheckHomography()
...
float Initializer::CheckHomography(const cv::Mat &H21, const cv::Mat &H12, vector<bool> &vbMatchesInliers, float sigma)
计算Fundamental，获取模型评分
ORB_SLAM2/src/Initializer.cpp--void Initializer::FindFundamental()
...
/**
* @brief 计算基础矩阵
*
* 假设场景为非平面情况下通过前两帧求取Fundamental矩阵(current frame 2 到 reference frame 1),并得到该模型的评分
*/
void Initializer::FindFundamental(vector<bool> &vbMatchesInliers, float &score, cv::Mat &F21)
{
...
//1. 归一化
Normalize(mvKeys1,vPn1, T1);
Normalize(mvKeys2,vPn2, T2);
cv::Mat T2t = T2.t();
...
...
//2.计算F矩阵
cv::Mat Fn = ComputeF21(vPn1i,vPn2i);
//解除归一化
F21i = T2t*Fn*T1;
// 3.利用重投影误差为当次RANSAC的结果评分
currentScore = CheckFundamental(F21i, vbCurrentInliers, mSigma);
...
}
1.将特征点归一化
见特征点归一化
2.求解基础矩阵（归一化八点法）
ORB_SLAM2/src/Initializer.cpp--cv::Mat Initializer::ComputeF21()
...
cv::Mat Initializer::ComputeF21(const vector<cv::Point2f> &vP1,const vector<cv::Point2f> &vP2)
{
const int N = vP1.size();
cv::Mat A(N,9,CV_32F); // N*9
for(int i=0; i<N; i++)
{
const float u1 = vP1[i].x;
const float v1 = vP1[i].y;
const float u2 = vP2[i].x;
const float v2 = vP2[i].y;
A.at<float>(i,0) = u2*u1;
A.at<float>(i,1) = u2*v1;
A.at<float>(i,2) = u2;
A.at<float>(i,3) = v2*u1;
A.at<float>(i,4) = v2*v1;
A.at<float>(i,5) = v2;
A.at<float>(i,6) = u1;
A.at<float>(i,7) = v1;
A.at<float>(i,8) = 1;
}
cv::Mat u,w,vt;
cv::SVDecomp(A,w,u,vt,cv::SVD::MODIFY_A | cv::SVD::FULL_UV);
cv::Mat Fpre = vt.row(8).reshape(0, 3); // v的最后一列
cv::SVDecomp(Fpre,w,u,vt,cv::SVD::MODIFY_A | cv::SVD::FULL_UV);
w.at<float>(2)=0; // 秩2约束，将第3个奇异值设为0
return u*cv::Mat::diag(w)*vt;
}
3.Fundamental模型评分
s c o r e F = ∑ i = 0 N ρ ( T F − ∣ ∣ x , F x ∣ ∣ 2 / σ 2 ) scoreF=\sum\limits_{i=0}^{N}\rho(T_F-||x^,Fx||^2/\sigma^2) scoreF=i=0∑Nρ(TF−∣∣x,Fx∣∣2/σ2)
ORB_SLAM2/src/Initializer.cpp--float Initializer::CheckFundamental()
...
float Initializer::CheckFundamental(const cv::Mat &F21, vector<bool> &vbMatchesInliers, float sigma)
计算Essential
ORB_SLAM是先计算基础矩阵F，然后通过相机内参计算E，没有直接计算E;直接求解E可以使用8点法，或者5点法
F = K − T E K − 1 E = K T F K F=K^{-T}EK^{-1} \\ E=K^TFK F=K−TEK−1E=KTFK
《计算机视觉中的多视图几何》P173
ORB_SLAM2/src/Initializer.cpp--bool Initializer::ReconstructF()
...
// Compute Essential Matrix from Fundamental Matrix
cv::Mat E21 = K.t()*F21*K;
...
为什么Ax=0的解是V的最后一列
《计算机视觉中的多视图几何》P412-P413
Homograph 矩阵分解恢复R，t
ORB-SLAM2源码中文详解
Motion and structure from motion in a piecewise planar environment
ORB_SLAM2/src/Initializer.cpp--Initializer::ReconstructH()
...
bool Initializer::ReconstructH(vector<bool> &vbMatchesInliers, cv::Mat &H21, cv::Mat &K,
cv::Mat &R21, cv::Mat &t21, vector<cv::Point3f> &vP3D, vector<bool> &vbTriangulated, float minParallax, int minTriangulated)
{
...
// We recover 8 motion hypotheses using the method of Faugeras et al.
// Motion and structure from motion in a piecewise planar environment.
// International Journal of Pattern Recognition and Artificial Intelligence, 1988
cv::Mat U,w,Vt,V;
//1.求解H的分解矩阵，得到8种运动假设
cv::SVD::compute(A,w,U,Vt,cv::SVD::FULL_UV);
V=Vt.t();
...
...
// Instead of applying the visibility constraints proposed in the Faugeras' paper (which could fail for points seen with low parallax)
// We reconstruct all hypotheses and check in terms of triangulated points and parallax
// d'=d2和d'=-d2分别对应8组(R t)
//2.进行cheirality check,选出最优的
for(size_t i=0; i<8; i++)
{
float parallaxi;
vector<cv::Point3f> vP3Di;
vector<bool> vbTriangulatedi;
int nGood = CheckRT(vR[i],vt[i],mvKeys1,mvKeys2,mvMatches12,vbMatchesInliers,K,vP3Di, 4.0*mSigma2, vbTriangulatedi, parallaxi);
...
}
Essential 矩阵分解恢复R，t
/**
* @brief 从F恢复R t
*
* 度量重构
* 1. 由Fundamental矩阵结合相机内参K，得到Essential矩阵: \f$ E = k'^T F k \f$
* 2. SVD分解得到R t
* 3. 进行cheirality check, 从四个解中找出最合适的解
*
* @see Multiple View Geometry in Computer Vision - Result 9.19 p259
*/
bool Initializer::ReconstructF(vector<bool> &vbMatchesInliers, cv::Mat &F21, cv::Mat &K,
cv::Mat &R21, cv::Mat &t21, vector<cv::Point3f> &vP3D, vector<bool> &vbTriangulated, float minParallax, int minTriangulated)
{
...
// 1.Compute Essential Matrix from Fundamental Matrix
cv::Mat E21 = K.t()*F21*K;
cv::Mat R1, R2, t;
// 2. Recover the 4 motion hypotheses
// 虽然这个函数对t有归一化，但并没有决定单目整个SLAM过程的尺度
// 因为CreateInitialMapMonocular函数对3D点深度会缩放，然后反过来对 t 有改变
// F矩阵通过结合内参可以得到Essential矩阵，分解E矩阵将得到4组解
//这4组解分别为[R1,t],[R1,-t],[R2,t],[R2,-t]
DecomposeE(E21,R1,R2,t);
cv::Mat t1=t;
cv::Mat t2=-t;
// 3. Reconstruct with the 4 hyphoteses and check
vector<cv::Point3f> vP3D1, vP3D2, vP3D3, vP3D4;
vector<bool> vbTriangulated1,vbTriangulated2,vbTriangulated3, vbTriangulated4;
float parallax1,parallax2, parallax3, parallax4;
int nGood1 = CheckRT(R1,t1,mvKeys1,mvKeys2,mvMatches12,vbMatchesInliers,K, vP3D1, 4.0*mSigma2, vbTriangulated1, parallax1);
int nGood2 = CheckRT(R2,t1,mvKeys1,mvKeys2,mvMatches12,vbMatchesInliers,K, vP3D2, 4.0*mSigma2, vbTriangulated2, parallax2);
int nGood3 = CheckRT(R1,t2,mvKeys1,mvKeys2,mvMatches12,vbMatchesInliers,K, vP3D3, 4.0*mSigma2, vbTriangulated3, parallax3);
int nGood4 = CheckRT(R2,t2,mvKeys1,mvKeys2,mvMatches12,vbMatchesInliers,K, vP3D4, 4.0*mSigma2, vbTriangulated4, parallax4);
...
}
参考
https://www.zhihu.com/question/50385799/answer/120902345
https://blog.csdn.net/zhubaohua_bupt/article/details/78560966
ORB-SLAM: A Versatile and Accurate Monocular SLAM System
http://webdiis.unizar.es/~raulmur/orbslam/
https://github.com/raulmur/ORB_SLAM2
https://en.cppreference.com/w/cpp/algorithm/fill
ORB-SLAM2源码中文注释
ORB-SLAM2源码中文详解
《计算机视觉中的多视图几何》