1. 基础矩阵的定义

如图所示是对极几何约束关系图，图中 O 1 , O 2 O_{1},O_{2} O1,O2分别表示相机的两个位姿，平面 I 1 , I 2 I_{1},I_{2} I1,I2分别表示对应的投影平面， P P P点为三维环境中的一个位置点， p 1 , p 2 p_{1},p_{2} p1,p2分别表示 P P P点在两个投影平面上的投影．假设位姿 O 2 O_{2} O2相对于位姿 O 1 O_{1} O1的旋转变换矩阵为 R \mathbf{R} R、平移向量为 t \mathbf{t} t，相机的内参为 K \mathbf{K} K，则根据相机投影关系可得:
s 1 p 1 = K P , s 2 p 2 = K ( R P + t ) (1) s_{1}\mathbf{p}_{1} = \mathbf{KP},s_{2}\mathbf{p}_{2} = \mathbf{K}(\mathbf{RP+t}) \tag{1} s1p1=KP,s2p2=K(RP+t)(1)
令 x 1 = P / s 1 , x 2 = ( R P + t ) / s ２ \mathbf{x}_{1}=\mathbf{P}/s_{1},\mathbf{x}_{2}=\mathbf{(RP+t)}/s_{２} x1=P/s1,x2=(RP+t)/s２(懂相机投影知识的应该都知道，其实 x 1 , x 2 \mathbf{x}_{1},\mathbf{x}_{2} x1,x2就是点 P \mathbf{P} P在相机的归一化平面上的投影)，可得:
x 1 = K − 1 p 1 , x 2 = K − 1 p 2 (2) \mathbf{x}_{1} = \mathbf{K}^{-1}\mathbf{p}_{1},\mathbf{x}_{2} = \mathbf{K}^{-1}\mathbf{p}_{2} \tag{2} x1=K−1p1,x2=K−1p2(2)
将(2)式带入(1)式消去 P \mathbf{P} P可得:
s 2 x 2 = R s 1 x 1 + t (3) s_{2}\mathbf{x}_{2} = \mathbf{R}s_{1}\mathbf{x}_{1} + \mathbf{t} \tag{3} s2x2=Rs1x1+t(3)
等式(3)两边同时乘以 t \mathbf{t} t的反对成矩阵 t ^ \mathbf{t}^{\hat{}} t^可得:
t ^ s 2 x 2 = t ^ R s 1 x 1 (4) \mathbf{t}^{\hat{}}s_{2}\mathbf{x}_{2} = \mathbf{t}^{\hat{}}\mathbf{R}s_{1}\mathbf{x}_{1} \tag{4} t^s2x2=t^Rs1x1(4)
等式(4)两边同时乘以 x 2 T \mathbf{x}_{2}^{T} x2T可得:
s 2 x 2 T t ^ x 2 = s 1 x 2 T t ^ R x 1 (5) s_{2}\mathbf{x}_{2}^{T}\mathbf{t}^{\hat{}}\mathbf{x}_{2} = s_{1}\mathbf{x}_{2}^{T}\mathbf{t}^{\hat{}}\mathbf{R}\mathbf{x}_{1} \tag{5} s2x2Tt^x2=s1x2Tt^Rx1(5)
由于 t ^ x 2 \mathbf{t}^{\hat{}}\mathbf{x}_{2} t^x2的结果是一个垂直于 t \mathbf{t} t和 x 2 \mathbf{x}_{2} x2的向量，因此向量 x 2 \mathbf{x}_{2} x2与该垂向量的点积为0，因此式(5)可变为:
x 2 T t ^ R x 1 = 0 (6) \mathbf{x}_{2}^{T}\mathbf{t}^{\hat{}}\mathbf{R}\mathbf{x}_{1} = \mathbf{0} \tag{6} x2Tt^Rx1=0(6)
将公式(2)带入公式(6)可得:
p 2 T K − T t ^ R K − 1 p 1 = 0 (7) \mathbf{p}_{2}^{T}\mathbf{K}^{-T}\mathbf{t}^{\hat{}}\mathbf{R}\mathbf{K}^{-1}\mathbf{p}_{1} = \mathbf{0} \tag{7} p2TK−Tt^RK−1p1=0(7)
令: E = t ^ R \mathbf{E}=\mathbf{t}^{\hat{}}\mathbf{R} E=t^R和 F = K T t ^ R K − 1 \mathbf{F}=\mathbf{K}^{T}\mathbf{t}^{\hat{}}\mathbf{R}\mathbf{K}^{-1} F=KTt^RK−1可得:
x 2 T E x 1 = p 2 T F p 1 = 0 (8) \mathbf{x}_{2}^{T}\mathbf{E}\mathbf{x}_{1} = \mathbf{p}_{2}^{T}\mathbf{F}\mathbf{p}_{1} = 0 \tag{8} x2TEx1=p2TFp1=0(8)
式中 E \mathbf{E} E即为本质矩阵， F \mathbf{F} F即为基础矩阵.

2. 基础矩阵的求解(八点法)

我们假设存在一对匹配的像素点 p 1 = [ u 1 , v 1 ] \mathbf{p}_{1} = [u_{1}, v_{1}] p1=[u1,v1]， p 2 = [ u 2 , v 2 ] \mathbf{p}_{2} = [u_{2},v_{2}] p2=[u2,v2]，则根据公式(8)可得:
( u 1 , v 1 , 1 ) ( f 1 f 2 f 3 f 4 f 5 f 6 f 7 f 8 f 9 ) ( u 2 , v 2 , 1 ) T = 0 (9) (u_{1}, v_{1}, 1) \begin{pmatrix} f_{1} & f_{2} & f_{3} \\ f_{4} & f_{5} & f_{6}\\ f_{7} & f_{8} & f_{9} \end{pmatrix} (u_{2}, v_{2}, 1)^{T} = 0 \tag{9} (u1,v1,1)⎝⎛f1f4f7f2f5f8f3f6f9⎠⎞(u2,v2,1)T=0(9)
令: F = [ f 1 , f 2 , f 3 , f 4 , f 5 , f 6 , f 7 , f 8 , f 9 ] T \mathbf{F}=[f_{1}, f_{2}, f_{3}, f_{4}, f_{5}, f_{6}, f_{7}, f_{8}, f_{9}]^{T} F=[f1,f2,f3,f4,f5,f6,f7,f8,f9]T，则可将公式(9)转换成与 F \mathbf{F} F有关的线性表达:
[ u 1 u 2 , u 1 v 2 , u 1 , v 1 u 2 , v 1 v 2 , v 1 , u 2 , v 2 , 1 ] ⋅ F = 0 (10) [u_{1}u_{2}, u_{1}v_{2},u_{1},v_{1}u_{2},v_{1}v_{2},v_{1}, u_{2},v_{2},1]\cdot\mathbf{F} = 0\tag{10} [u1u2,u1v2,u1,v1u2,v1v2,v1,u2,v2,1]⋅F=0(10)
从上述推导可以看出来，每一对匹配点都可以建立一个如公式(10)所示的与基础矩阵有关的齐次线性方程．基础矩阵 F \mathbf{F} F有9个未知数，因此需要9对匹配点来求解，但基础矩阵又有一个重要的性质是它具有尺度等价性，因此实际只有8个未知数，因此可以用8对匹配点来求解基础矩阵，这就是八点法．因此可以根据8对匹配点建立如下齐次线性方程组:
( u 1 1 u 2 1 , u 1 1 v 2 1 , u 1 1 , v 1 1 u 2 1 , v 1 1 v 2 1 , v 1 1 , u 2 1 , v 2 1 , 1 u 1 2 u 2 2 , u 1 2 v 2 2 , u 1 2 , v 1 2 u 2 2 , v 1 2 v 2 2 , v 1 2 , u 2 2 , v 2 2 , 1 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . u 1 7 u 2 7 , u 1 7 v 2 7 , u 1 7 , v 1 7 u 2 7 , v 1 7 v 2 7 , v 1 7 , u 2 7 , v 2 7 , 1 u 1 8 u 2 8 , u 1 8 v 2 8 , u 1 8 , v 1 8 u 2 8 , v 1 8 v 2 8 , v 1 8 , u 2 8 , v 2 8 , 1 ) ⋅ F = 0 (11) \begin{pmatrix} u_{1}^{1}u_{2}^{1}, u_{1}^{1}v_{2}^{1},u_{1}^{1},v_{1}^{1}u_{2}^{1},v_{1}^{1}v_{2}^{1},v_{1}^{1}, u_{2}^{1},v_{2}^{1},1\\ u_{1}^{2}u_{2}^{2}, u_{1}^{2}v_{2}^{2},u_{1}^{2},v_{1}^{2}u_{2}^{2},v_{1}^{2}v_{2}^{2},v_{1}^{2}, u_{2}^{2},v_{2}^{2},1\\ .......................................................\\ u_{1}^{7}u_{2}^{7}, u_{1}^{7}v_{2}^{7},u_{1}^{7},v_{1}^{7}u_{2}^{7},v_{1}^{7}v_{2}^{7},v_{1}^{7}, u_{2}^{7},v_{2}^{7},1\\ u_{1}^{8}u_{2}^{8}, u_{1}^{8}v_{2}^{8},u_{1}^{8},v_{1}^{8}u_{2}^{8},v_{1}^{8}v_{2}^{8},v_{1}^{8}, u_{2}^{8},v_{2}^{8},1\\ \end{pmatrix} \cdot \mathbf{F} = 0 \tag{11} ⎝⎜⎜⎜⎜⎛u11u21,u11v21,u11,v11u21,v11v21,v11,u21,v21,1u12u22,u12v22,u12,v12u22,v12v22,v12,u22,v22,1.......................................................u17u27,u17v27,u17,v17u27,v17v27,v17,u27,v27,1u18u28,u18v28,u18,v18u28,v18v28,v18,u28,v28,1⎠⎟⎟⎟⎟⎞⋅F=0(11)
求解齐次线性方程组(11)的常用方法是利用SVD求解，经过SVD分解后，方程组的解即为最小奇异值对应的 V T \mathbf{V}^{T} VT的列向量，详细求解过程可参考 https://blog.csdn.net/youngpan1101/article/details/54574130．至此完成基础矩阵的求解.

3. 代码解析

求解流程

/*** @brief 计算基础矩阵，假设场景为非平面情况下通过前两帧求取Fundamental矩阵，得到该模型的评分* Step 1 将当前帧和参考帧中的特征点坐标进行归一化* Step 2 选择8个归一化之后的点对进行迭代* Step 3 八点法计算基础矩阵矩阵* Step 4 利用重投影误差为当次RANSAC的结果评分* Step 5 更新具有最优评分的基础矩阵计算结果,并且保存所对应的特征点对的内点标记* * @param[in & out] vbMatchesInliers          标记是否是外点* @param[in & out] score                     计算基础矩阵得分* @param[in & out] F21                       从特征点1到2的基础矩阵*/
void Initializer::FindFundamental(vector<bool> &vbMatchesInliers, float &score, cv::Mat &F21)
{// 计算基础矩阵,其过程和上面的计算单应矩阵的过程十分相似.// Number of putative matches// 匹配的特征点对总数// const int N = vbMatchesInliers.size();  // !源代码出错！请使用下面代替const int N = mvMatches12.size();// Normalize coordinates// Step 1 将当前帧和参考帧中的特征点坐标进行归一化，主要是平移和尺度变换// 具体来说,就是将mvKeys1和mvKey2归一化到均值为0，一阶绝对矩为1，归一化矩阵分别为T1、T2// 这里所谓的一阶绝对矩其实就是随机变量到取值的中心的绝对值的平均值// 归一化矩阵就是把上述归一化的操作用矩阵来表示。这样特征点坐标乘归一化矩阵可以得到归一化后的坐标vector<cv::Point2f> vPn1, vPn2;cv::Mat T1, T2;Normalize(mvKeys1,vPn1, T1);Normalize(mvKeys2,vPn2, T2);// ! 注意这里取的是归一化矩阵T2的转置,因为基础矩阵的定义和单应矩阵不同，两者去归一化的计算也不相同cv::Mat T2t = T2.t();// Best Results variables//最优结果score = 0.0;vbMatchesInliers = vector<bool>(N,false);// Iteration variables// 某次迭代中，参考帧的特征点坐标vector<cv::Point2f> vPn1i(8);// 某次迭代中，当前帧的特征点坐标vector<cv::Point2f> vPn2i(8);// 某次迭代中，计算的基础矩阵cv::Mat F21i;// 每次RANSAC记录的Inliers与得分vector<bool> vbCurrentInliers(N,false);float currentScore;// Perform all RANSAC iterations and save the solution with highest score// 下面进行每次的RANSAC迭代for(int it=0; it<mMaxIterations; it++){// Select a minimum set// Step 2 选择8个归一化之后的点对进行迭代for(int j=0; j<8; j++){int idx = mvSets[it][j];// vPn1i和vPn2i为匹配的特征点对的归一化后的坐标// 首先根据这个特征点对的索引信息分别找到两个特征点在各自图像特征点向量中的索引，然后读取其归一化之后的特征点坐标vPn1i[j] = vPn1[mvMatches12[idx].first];        //first存储在参考帧1中的特征点索引vPn2i[j] = vPn2[mvMatches12[idx].second];       //second存储在参考帧1中的特征点索引}// Step 3 八点法计算基础矩阵cv::Mat Fn = ComputeF21(vPn1i,vPn2i);// 基础矩阵约束：p2^t*F21*p1 = 0，其中p1,p2 为齐次化特征点坐标    // 特征点归一化：vPn1 = T1 * mvKeys1, vPn2 = T2 * mvKeys2  // 根据基础矩阵约束得到:(T2 * mvKeys2)^t* Hn * T1 * mvKeys1 = 0   // 进一步得到:mvKeys2^t * T2^t * Hn * T1 * mvKeys1 = 0F21i = T2t*Fn*T1;// Step 4 利用重投影误差为当次RANSAC的结果评分currentScore = CheckFundamental(F21i, vbCurrentInliers, mSigma);// Step 5 更新具有最优评分的基础矩阵计算结果,并且保存所对应的特征点对的内点标记if(currentScore>score){//如果当前的结果得分更高，那么就更新最优计算结果F21 = F21i.clone();vbMatchesInliers = vbCurrentInliers;score = currentScore;}}
}

八点法计算基础矩阵(此部分对应第二节基础矩阵的求解(八点法))

/*** @brief 根据特征点匹配求fundamental matrix（normalized 8点法）* 注意F矩阵有秩为2的约束，所以需要两次SVD分解* * @param[in] vP1           参考帧中归一化后的特征点* @param[in] vP2           当前帧中归一化后的特征点* @return cv::Mat          最后计算得到的基础矩阵F*/
cv::Mat Initializer::ComputeF21(const vector<cv::Point2f> &vP1, //归一化后的点, in reference frameconst vector<cv::Point2f> &vP2) //归一化后的点, in current frame
{//获取参与计算的特征点对数const int N = vP1.size();//初始化A矩阵cv::Mat A(N,9,CV_32F); // N*9维// 构造矩阵A，将每个特征点添加到矩阵A中的元素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;// 定义输出变量，u是左边的正交矩阵U， w为奇异矩阵，vt中的t表示是右正交矩阵V的转置cv::SVDecomp(A,w,u,vt,cv::SVD::MODIFY_A | cv::SVD::FULL_UV);// 转换成基础矩阵的形式cv::Mat Fpre = vt.row(8).reshape(0, 3); // v的最后一列//基础矩阵的秩为2,而我们不敢保证计算得到的这个结果的秩为2,所以需要通过第二次奇异值分解,来强制使其秩为2// 对初步得来的基础矩阵进行第2次奇异值分解cv::SVDecomp(Fpre,w,u,vt,cv::SVD::MODIFY_A | cv::SVD::FULL_UV);// 秩2约束，强制将第3个奇异值设置为0w.at<float>(2)=0; // 重新组合好满足秩约束的基础矩阵，作为最终计算结果返回 return  u*cv::Mat::diag(w)*vt;
}

利用重投影误差为当次RANSAC的结果评分

/*** @brief 对给定的Fundamental matrix打分* * @param[in] F21                       当前帧和参考帧之间的基础矩阵* @param[in] vbMatchesInliers          匹配的特征点对属于inliers的标记* @param[in] sigma                     方差，默认为1* @return float                        返回得分*/
float Initializer::CheckFundamental(const cv::Mat &F21,             //当前帧和参考帧之间的基础矩阵vector<bool> &vbMatchesInliers, //匹配的特征点对属于inliers的标记float sigma)                    //方差
{// 获取匹配的特征点对的总对数const int N = mvMatches12.size();// Step 1 提取基础矩阵中的元素数据const float f11 = F21.at<float>(0,0);const float f12 = F21.at<float>(0,1);const float f13 = F21.at<float>(0,2);const float f21 = F21.at<float>(1,0);const float f22 = F21.at<float>(1,1);const float f23 = F21.at<float>(1,2);const float f31 = F21.at<float>(2,0);const float f32 = F21.at<float>(2,1);const float f33 = F21.at<float>(2,2);// 预分配空间vbMatchesInliers.resize(N);// 设置评分初始值（因为后面需要进行这个数值的累计）float score = 0;// 基于卡方检验计算出的阈值// 自由度为1的卡方分布，显著性水平为0.05，对应的临界阈值// ?是因为点到直线距离是一个自由度吗？const float th = 3.841;// 自由度为2的卡方分布，显著性水平为0.05，对应的临界阈值const float thScore = 5.991;// 信息矩阵，或 协方差矩阵的逆矩阵const float invSigmaSquare = 1.0/(sigma*sigma);// Step 2 计算img1 和 img2 在估计 F 时的score值for(int i=0; i<N; i++){//默认为这对特征点是Inliersbool bIn = true;// Step 2.1 提取参考帧和当前帧之间的特征匹配点对const cv::KeyPoint &kp1 = mvKeys1[mvMatches12[i].first];const cv::KeyPoint &kp2 = mvKeys2[mvMatches12[i].second];// 提取出特征点的坐标const float u1 = kp1.pt.x;const float v1 = kp1.pt.y;const float u2 = kp2.pt.x;const float v2 = kp2.pt.y;// Reprojection error in second image// Step 2.2 计算 img1 上的点在 img2 上投影得到的极线 l2 = F21 * p1 = (a2,b2,c2)const float a2 = f11*u1+f12*v1+f13;const float b2 = f21*u1+f22*v1+f23;const float c2 = f31*u1+f32*v1+f33;// Step 2.3 计算误差 e = (a * p2.x + b * p2.y + c) /  sqrt(a * a + b * b)const float num2 = a2*u2+b2*v2+c2;const float squareDist1 = num2*num2/(a2*a2+b2*b2);// 带权重误差const float chiSquare1 = squareDist1*invSigmaSquare;// Step 2.4 误差大于阈值就说明这个点是Outlier // ? 为什么判断阈值用的 th（1自由度），计算得分用的thScore（2自由度）// ? 可能是为了和CheckHomography 得分统一？if(chiSquare1>th)bIn = false;else// 误差越大，得分越低score += thScore - chiSquare1;// 计算img2上的点在 img1 上投影得到的极线 l1= p2 * F21 = (a1,b1,c1)const float a1 = f11*u2+f21*v2+f31;const float b1 = f12*u2+f22*v2+f32;const float c1 = f13*u2+f23*v2+f33;// 计算误差 e = (a * p2.x + b * p2.y + c) /  sqrt(a * a + b * b)const float num1 = a1*u1+b1*v1+c1;const float squareDist2 = num1*num1/(a1*a1+b1*b1);// 带权重误差const float chiSquare2 = squareDist2*invSigmaSquare;// 误差大于阈值就说明这个点是Outlier if(chiSquare2>th)bIn = false;elsescore += thScore - chiSquare2;// Step 2.5 保存结果if(bIn)vbMatchesInliers[i]=true;elsevbMatchesInliers[i]=false;}//  返回评分return score;
}

参考

<<视觉slam14讲>>
https://github.com/raulmur/ORB_SLAM2
https://github.com/electech6/ORB_SLAM2_detailed_comments
https://zhuanlan.zhihu.com/p/61614421

ORB_SLAM2中基础矩阵F求解的原理及源码分析相关推荐

Linux中mknod命令实现原理以及源码分析
本篇文章以mknod创建字符设备文件进行讲解字符设备驱动的Demo例子可参考该篇文章 Linux 编写简单驱动并测试 1. mknod 命令 mknod /dev/hello c 520 0 该命令 ...
SIFT原理与源码分析：DoG尺度空间构造
<SIFT原理与源码分析>系列文章索引:http://blog.csdn.net/xiaowei_cqu/article/details/8069548 尺度空间理论自然界中的物体随着观 ...
深入理解Spark 2.1 Core （八）：Standalone模式容错及HA的原理与源码分析
第五.第六.第七篇博文,我们讲解了Standalone模式集群是如何启动的,一个App起来了后,集群是如何分配资源,Worker启动Executor的,Task来是如何执行它,执行得到的结果如何处理, ...
【OpenCV】SIFT原理与源码分析：关键点描述
<SIFT原理与源码分析>系列文章索引:http://blog.csdn.net/xiaowei_cqu/article/details/8069548 由前一篇< 方向赋值> ...
ConcurrentHashMap实现原理及源码分析
ConcurrentHashMap是Java并发包中提供的一个线程安全且高效的HashMap实现(若对HashMap的实现原理还不甚了解,可参考我的另一篇文章HashMap实现原理及源码分析),Con ...
concurrenthashmap_ConcurrentHashMap实现原理及源码分析
ConcurrentHashMap是Java并发包中提供的一个线程安全且高效的HashMap实现(若对HashMap的实现原理还不甚了解,可参考我的另一篇文章HashMap实现原理及源码分析),Con ...
深入理解Spark 2.1 Core （十二）：TimSort 的原理与源码分析
在博文<深入理解Spark 2.1 Core (十):Shuffle Map 端的原理与源码分析 >中我们提到了: 使用Sort等对数据进行排序,其中用到了TimSort 这篇博文我们就来 ...
深入理解Spark 2.1 Core （十一）：Shuffle Reduce 端的原理与源码分析
我们曾经在<深入理解Spark 2.1 Core (一):RDD的原理与源码分析 >讲解过: 为了有效地实现容错,RDD提供了一种高度受限的共享内存,即RDD是只读的,并且只能通过其他RD ...
深入理解Spark 2.1 Core （十）：Shuffle Map 端的原理与源码分析
在上一篇<深入理解Spark 2.1 Core (九):迭代计算和Shuffle的原理与源码分析>提到经过迭代计算后, SortShuffleWriter.write中: // 根据排序方 ...

ORB_SLAM2中基础矩阵F求解的原理及源码分析

1. 基础矩阵的定义

2. 基础矩阵的求解(八点法)

3. 代码解析

参考

ORB_SLAM2中基础矩阵F求解的原理及源码分析相关推荐

最新文章

热门文章