前言
这个函数的实现依赖于之前学习的检验三角化结果的函数：
ORB-SLAM2源码学习：Initializer.cc⑧: Initializer::CheckRT检验三角化结果-CSDN博客
位姿可能有多组解，到底哪个才是真正的解呢？ 方法是实践出真知。根据相应的论文我们分两种情况分别恢复出不同的解，最后得到一个最好的。每种可能的解都需要重复计算一次，最终根据如下条件选择最佳的解：
1.最优解成功三角化点数目明显大于次优解的点数目。
2.最优解的视差大于设定点阈值。
3.最优解成功三角化点数目大于设定的阈值。
4.最优解成功三角化点数目占所有特征点数目的90%以上。
1.函数声明
cpp
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)2.函数定义
1.首先利用论文的方法分两种情况分别对八个解进行求解
实际情况这部分已经是固定了求解方法我这里只做了简单的了解实际中这部分已经不需要进行优化了。
cpp
// 目的 ：通过单应矩阵H恢复两帧图像之间的旋转矩阵R和平移向量T
// 参考 ：Motion and structure from motion in a piecewise plannar environment.
// International Journal of Pattern Recognition and Artificial Intelligence, 1988
// https://www.researchgate.net/publication/243764888_Motion_and_Structure_from_Motion_in_a_Piecewise_Planar_Environment
// 流程:
// 1. 根据H矩阵的奇异值d'= d2 或者 d' = -d2 分别计算 H 矩阵分解的 8 组解
// 1.1 讨论 d' > 0 时的 4 组解
// 1.2 讨论 d' < 0 时的 4 组解
// 2. 对 8 组解进行验证，并选择产生相机前方最多3D点的解为最优解
// 统计匹配的特征点对中属于内点(Inlier)或有效点个数
int N=0;
for(size_t i=0, iend = vbMatchesInliers.size() ; i<iend; i++)
if(vbMatchesInliers[i])
N++;
// 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
// 参考SLAM十四讲第二版p170-p171
// H = K * (R - t * n / d) * K_inv
// 其中: K表示内参数矩阵
// K_inv 表示内参数矩阵的逆
// R 和 t 表示旋转和平移向量
// n 表示平面法向量
// 令 H = K * A * K_inv
// 则 A = k_inv * H * k
cv::Mat invK = K.inv();
cv::Mat A = invK*H21*K;
// 对矩阵A进行SVD分解
// A 等待被进行奇异值分解的矩阵
// w 奇异值矩阵
// U 奇异值分解左矩阵
// Vt 奇异值分解右矩阵，注意函数返回的是转置
// cv::SVD::FULL_UV 全部分解
// A = U * w * Vt
cv::Mat U,w,Vt,V;
cv::SVD::compute(A, w, U, Vt, cv::SVD::FULL_UV);
// 根据文献eq(8)，计算关联变量
V=Vt.t();
// 计算变量s = det(U) * det(V)
// 因为det(V)==det(Vt), 所以 s = det(U) * det(Vt)
float s = cv::determinant(U)*cv::determinant(Vt);
// 取得矩阵的各个奇异值
float d1 = w.at<float>(0);
float d2 = w.at<float>(1);
float d3 = w.at<float>(2);
// SVD分解正常情况下特征值di应该是正的，且满足d1>=d2>=d3
if(d1/d2<1.00001 || d2/d3<1.00001) {
return false;
}
// 在ORBSLAM中没有对奇异值 d1 d2 d3按照论文中描述的关系进行分类讨论, 而是直接进行了计算
// 定义8中情况下的旋转矩阵、平移向量和空间向量
vector<cv::Mat> vR, vt, vn;
vR.reserve(8);
vt.reserve(8);
vn.reserve(8);
// Step 1.1 讨论 d' > 0 时的 4 组解
// 根据论文eq.(12)有
// x1 = e1 * sqrt((d1 * d1 - d2 * d2) / (d1 * d1 - d3 * d3))
// x2 = 0
// x3 = e3 * sqrt((d2 * d2 - d3 * d3) / (d1 * d1 - d3 * d3))
// 令 aux1 = sqrt((d1*d1-d2*d2)/(d1*d1-d3*d3))
// aux3 = sqrt((d2*d2-d3*d3)/(d1*d1-d3*d3))
// 则
// x1 = e1 * aux1
// x3 = e3 * aux2
// 因为 e1,e2,e3 = 1 or -1
// 所以有x1和x3有四种组合
// x1 = {aux1,aux1,-aux1,-aux1}
// x3 = {aux3,-aux3,aux3,-aux3}
float aux1 = sqrt((d1*d1-d2*d2)/(d1*d1-d3*d3));
float aux3 = sqrt((d2*d2-d3*d3)/(d1*d1-d3*d3));
float x1[] = {aux1,aux1,-aux1,-aux1};
float x3[] = {aux3,-aux3,aux3,-aux3};
// 根据论文eq.(13)有
// sin(theta) = e1 * e3 * sqrt(( d1 * d1 - d2 * d2) * (d2 * d2 - d3 * d3)) /(d1 + d3)/d2
// cos(theta) = (d2* d2 + d1 * d3) / (d1 + d3) / d2
// 令 aux_stheta = sqrt((d1*d1-d2*d2)*(d2*d2-d3*d3))/((d1+d3)*d2)
// 则 sin(theta) = e1 * e3 * aux_stheta
// cos(theta) = (d2*d2+d1*d3)/((d1+d3)*d2)
// 因为 e1 e2 e3 = 1 or -1
// 所以 sin(theta) = {aux_stheta, -aux_stheta, -aux_stheta, aux_stheta}
float aux_stheta = sqrt((d1*d1-d2*d2)*(d2*d2-d3*d3))/((d1+d3)*d2);
float ctheta = (d2*d2+d1*d3)/((d1+d3)*d2);
float stheta[] = {aux_stheta, -aux_stheta, -aux_stheta, aux_stheta};
// 计算旋转矩阵 R'
//根据不同的e1 e3组合所得出来的四种R t的解
// | ctheta 0 -aux_stheta| | aux1|
// Rp = | 0 1 0 | tp = | 0 |
// | aux_stheta 0 ctheta | |-aux3|
// | ctheta 0 aux_stheta| | aux1|
// Rp = | 0 1 0 | tp = | 0 |
// |-aux_stheta 0 ctheta | | aux3|
// | ctheta 0 aux_stheta| |-aux1|
// Rp = | 0 1 0 | tp = | 0 |
// |-aux_stheta 0 ctheta | |-aux3|
// | ctheta 0 -aux_stheta| |-aux1|
// Rp = | 0 1 0 | tp = | 0 |
// | aux_stheta 0 ctheta | | aux3|
// 开始遍历这四种情况中的每一种
for(int i=0; i<4; i++)
{
//生成Rp，就是eq.(8) 的 R'
cv::Mat Rp=cv::Mat::eye(3,3,CV_32F);
Rp.at<float>(0,0)=ctheta;
Rp.at<float>(0,2)=-stheta[i];
Rp.at<float>(2,0)=stheta[i];
Rp.at<float>(2,2)=ctheta;
// eq.(8) 计算R
cv::Mat R = s*U*Rp*Vt;
// 保存
vR.push_back(R);
// eq. (14) 生成tp
cv::Mat tp(3,1,CV_32F);
tp.at<float>(0)=x1[i];
tp.at<float>(1)=0;
tp.at<float>(2)=-x3[i];
tp*=d1-d3;
// 这里虽然对t有归一化，并没有决定单目整个SLAM过程的尺度
// 因为CreateInitialMapMonocular函数对3D点深度会缩放，然后反过来对 t 有改变
// eq.(8)恢复原始的t
cv::Mat t = U*tp;
vt.push_back(t/cv::norm(t));
// 构造法向量np
cv::Mat np(3,1,CV_32F);
np.at<float>(0)=x1[i];
np.at<float>(1)=0;
np.at<float>(2)=x3[i];
// eq.(8) 恢复原始的法向量
cv::Mat n = V*np;
//看PPT 16页的图，保持平面法向量向上
if(n.at<float>(2)<0)
n=-n;
// 添加到vector
vn.push_back(n);
}
// Step 1.2 讨论 d' < 0 时的 4 组解
float aux_sphi = sqrt((d1*d1-d2*d2)*(d2*d2-d3*d3))/((d1-d3)*d2);
// cos_theta项
float cphi = (d1*d3-d2*d2)/((d1-d3)*d2);
// 考虑到e1,e2的取值，这里的sin_theta有两种可能的解
float sphi[] = {aux_sphi, -aux_sphi, -aux_sphi, aux_sphi};
// 对于每种由e1 e3取值的组合而形成的四种解的情况
for(int i=0; i<4; i++)
{
// 计算旋转矩阵 R'
cv::Mat Rp=cv::Mat::eye(3,3,CV_32F);
Rp.at<float>(0,0)=cphi;
Rp.at<float>(0,2)=sphi[i];
Rp.at<float>(1,1)=-1;
Rp.at<float>(2,0)=sphi[i];
Rp.at<float>(2,2)=-cphi;
// 恢复出原来的R
cv::Mat R = s*U*Rp*Vt;
// 然后添加到vector中
vR.push_back(R);
// 构造tp
cv::Mat tp(3,1,CV_32F);
tp.at<float>(0)=x1[i];
tp.at<float>(1)=0;
tp.at<float>(2)=x3[i];
tp*=d1+d3;
// 恢复出原来的t
cv::Mat t = U*tp;
// 归一化之后加入到vector中,要提供给上面的平移矩阵都是要进行过归一化的
vt.push_back(t/cv::norm(t));
// 构造法向量np
cv::Mat np(3,1,CV_32F);
np.at<float>(0)=x1[i];
np.at<float>(1)=0;
np.at<float>(2)=x3[i];
// 恢复出原来的法向量
cv::Mat n = V*np;
// 保证法向量指向上方
if(n.at<float>(2)<0)
n=-n;
// 添加到vector中
vn.push_back(n);
}2.遍历求解出来的八个解
调用CheckRT函数来对其三角化结果进行检验并更新一些变量。
cpp
// Step 2. 对 8 组解进行验证，并选择产生相机前方最多3D点的解为最优解
for(size_t i=0; i<8; i++)
{
// 第i组解对应的比较大的视差角
float parallaxi;
// 三角化测量之后的特征点的空间坐标
vector<cv::Point3f> vP3Di;
// 特征点对是否被三角化的标记
vector<bool> vbTriangulatedi;
// 调用 Initializer::CheckRT(), 计算good点的数目
int nGood = CheckRT(vR[i],vt[i], //当前组解的旋转矩阵和平移向量
mvKeys1,mvKeys2, //特征点
mvMatches12,vbMatchesInliers, //特征匹配关系以及Inlier标记
K, //相机的内参数矩阵
vP3Di, //存储三角化测量之后的特征点空间坐标的
4.0*mSigma2, //三角化过程中允许的最大重投影误差
vbTriangulatedi, //特征点是否被成功进行三角测量的标记
parallaxi); // 这组解在三角化测量的时候的比较大的视差角
// 更新历史最优和次优的解
// 保留最优的和次优的解.保存次优解的目的是看看最优解是否突出
if(nGood>bestGood)
{
// 如果当前组解的good点数是历史最优，那么之前的历史最优就变成了历史次优
secondBestGood = bestGood;
// 更新历史最优点
bestGood = nGood;
// 最优解的组索引为i（就是当前次遍历）
bestSolutionIdx = i;
// 更新变量
bestParallax = parallaxi;
bestP3D = vP3Di;
bestTriangulated = vbTriangulatedi;
}
// 如果当前组的good计数小于历史最优但却大于历史次优
else if(nGood>secondBestGood)
{
// 说明当前组解是历史次优点，更新之
secondBestGood = nGood;
}
}3.结合前边提及的条件来筛选最优解
cpp
// Step 3 选择最优解。要满足下面的四个条件
// 1. good点数最优解明显大于次优解，这里取0.75经验值
// 2. 视角差大于规定的阈值
// 3. good点数要大于规定的最小的被三角化的点数量
// 4. good数要足够多，达到总数的90%以上
if(secondBestGood<0.75*bestGood &&
bestParallax>=minParallax &&
bestGood>minTriangulated &&
bestGood>0.9*N)
{
// 从最佳的解的索引访问到R，t
vR[bestSolutionIdx].copyTo(R21);
vt[bestSolutionIdx].copyTo(t21);
// 获得最佳解时，成功三角化的三维点，以后作为初始地图点使用
vP3D = bestP3D;
// 获取特征点的被成功进行三角化的标记
vbTriangulated = bestTriangulated;
//返回真，找到了最好的解
return true;
}
return false;完整的代码分析
cpp
/*
用H矩阵恢复R, t和三维点
H矩阵分解常见有两种方法：Faugeras SVD-based decomposition 和 Zhang SVD-based decomposition
代码使用了Faugeras SVD-based decomposition算法，参考文献
Motion and structure from motion in a piecewise planar environment. International Journal of Pattern Recognition and Artificial Intelligence, 1988
vbMatchesInliers 匹配点对的内点标记
H21 从参考帧到当前帧的单应矩阵
K 相机的内参数矩阵
R21 计算出来的相机旋转
t21 计算出来的相机平移
vP3D 世界坐标系下，三角化测量特征点对之后得到的特征点的空间坐标
vbTriangulated 特征点是否成功三角化的标记
minParallax 对特征点的三角化测量中，认为其测量有效时需要满足的最小视差角（如果视差角过小则会引起非常大的观测误差）,单位是角度
minTriangulated 为了进行运动恢复，所需要的最少的三角化测量成功的点个数
return true 单应矩阵成功计算出位姿和三维点
return false 初始化失败
*/
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)
{
// 目的 ：通过单应矩阵H恢复两帧图像之间的旋转矩阵R和平移向量T
// 参考 ：Motion and structure from motion in a piecewise plannar environment.
// International Journal of Pattern Recognition and Artificial Intelligence, 1988
// https://www.researchgate.net/publication/243764888_Motion_and_Structure_from_Motion_in_a_Piecewise_Planar_Environment
// 流程:
// 1. 根据H矩阵的奇异值d'= d2 或者 d' = -d2 分别计算 H 矩阵分解的 8 组解
// 1.1 讨论 d' > 0 时的 4 组解
// 1.2 讨论 d' < 0 时的 4 组解
// 2. 对 8 组解进行验证，并选择产生相机前方最多3D点的解为最优解
// 统计匹配的特征点对中属于内点(Inlier)或有效点个数
int N=0;
for(size_t i=0, iend = vbMatchesInliers.size() ; i<iend; i++)
if(vbMatchesInliers[i])
N++;
// 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
// 参考SLAM十四讲第二版p170-p171
// H = K * (R - t * n / d) * K_inv
// 其中: K表示内参数矩阵
// K_inv 表示内参数矩阵的逆
// R 和 t 表示旋转和平移向量
// n 表示平面法向量
// 令 H = K * A * K_inv
// 则 A = k_inv * H * k
cv::Mat invK = K.inv();
cv::Mat A = invK*H21*K;
// 对矩阵A进行SVD分解
// A 等待被进行奇异值分解的矩阵
// w 奇异值矩阵
// U 奇异值分解左矩阵
// Vt 奇异值分解右矩阵，注意函数返回的是转置
// cv::SVD::FULL_UV 全部分解
// A = U * w * Vt
cv::Mat U,w,Vt,V;
cv::SVD::compute(A, w, U, Vt, cv::SVD::FULL_UV);
// 根据文献eq(8)，计算关联变量
V=Vt.t();
// 计算变量s = det(U) * det(V)
// 因为det(V)==det(Vt), 所以 s = det(U) * det(Vt)
float s = cv::determinant(U)*cv::determinant(Vt);
// 取得矩阵的各个奇异值
float d1 = w.at<float>(0);
float d2 = w.at<float>(1);
float d3 = w.at<float>(2);
// SVD分解正常情况下特征值di应该是正的，且满足d1>=d2>=d3
if(d1/d2<1.00001 || d2/d3<1.00001) {
return false;
}
// 在ORBSLAM中没有对奇异值 d1 d2 d3按照论文中描述的关系进行分类讨论, 而是直接进行了计算
// 定义8中情况下的旋转矩阵、平移向量和空间向量
vector<cv::Mat> vR, vt, vn;
vR.reserve(8);
vt.reserve(8);
vn.reserve(8);
// Step 1.1 讨论 d' > 0 时的 4 组解
// 根据论文eq.(12)有
// x1 = e1 * sqrt((d1 * d1 - d2 * d2) / (d1 * d1 - d3 * d3))
// x2 = 0
// x3 = e3 * sqrt((d2 * d2 - d3 * d3) / (d1 * d1 - d3 * d3))
// 令 aux1 = sqrt((d1*d1-d2*d2)/(d1*d1-d3*d3))
// aux3 = sqrt((d2*d2-d3*d3)/(d1*d1-d3*d3))
// 则
// x1 = e1 * aux1
// x3 = e3 * aux2
// 因为 e1,e2,e3 = 1 or -1
// 所以有x1和x3有四种组合
// x1 = {aux1,aux1,-aux1,-aux1}
// x3 = {aux3,-aux3,aux3,-aux3}
float aux1 = sqrt((d1*d1-d2*d2)/(d1*d1-d3*d3));
float aux3 = sqrt((d2*d2-d3*d3)/(d1*d1-d3*d3));
float x1[] = {aux1,aux1,-aux1,-aux1};
float x3[] = {aux3,-aux3,aux3,-aux3};
// 根据论文eq.(13)有
// sin(theta) = e1 * e3 * sqrt(( d1 * d1 - d2 * d2) * (d2 * d2 - d3 * d3)) /(d1 + d3)/d2
// cos(theta) = (d2* d2 + d1 * d3) / (d1 + d3) / d2
// 令 aux_stheta = sqrt((d1*d1-d2*d2)*(d2*d2-d3*d3))/((d1+d3)*d2)
// 则 sin(theta) = e1 * e3 * aux_stheta
// cos(theta) = (d2*d2+d1*d3)/((d1+d3)*d2)
// 因为 e1 e2 e3 = 1 or -1
// 所以 sin(theta) = {aux_stheta, -aux_stheta, -aux_stheta, aux_stheta}
float aux_stheta = sqrt((d1*d1-d2*d2)*(d2*d2-d3*d3))/((d1+d3)*d2);
float ctheta = (d2*d2+d1*d3)/((d1+d3)*d2);
float stheta[] = {aux_stheta, -aux_stheta, -aux_stheta, aux_stheta};
// 计算旋转矩阵 R'
//根据不同的e1 e3组合所得出来的四种R t的解
// | ctheta 0 -aux_stheta| | aux1|
// Rp = | 0 1 0 | tp = | 0 |
// | aux_stheta 0 ctheta | |-aux3|
// | ctheta 0 aux_stheta| | aux1|
// Rp = | 0 1 0 | tp = | 0 |
// |-aux_stheta 0 ctheta | | aux3|
// | ctheta 0 aux_stheta| |-aux1|
// Rp = | 0 1 0 | tp = | 0 |
// |-aux_stheta 0 ctheta | |-aux3|
// | ctheta 0 -aux_stheta| |-aux1|
// Rp = | 0 1 0 | tp = | 0 |
// | aux_stheta 0 ctheta | | aux3|
// 开始遍历这四种情况中的每一种
for(int i=0; i<4; i++)
{
//生成Rp，就是eq.(8) 的 R'
cv::Mat Rp=cv::Mat::eye(3,3,CV_32F);
Rp.at<float>(0,0)=ctheta;
Rp.at<float>(0,2)=-stheta[i];
Rp.at<float>(2,0)=stheta[i];
Rp.at<float>(2,2)=ctheta;
// eq.(8) 计算R
cv::Mat R = s*U*Rp*Vt;
// 保存
vR.push_back(R);
// eq. (14) 生成tp
cv::Mat tp(3,1,CV_32F);
tp.at<float>(0)=x1[i];
tp.at<float>(1)=0;
tp.at<float>(2)=-x3[i];
tp*=d1-d3;
// 这里虽然对t有归一化，并没有决定单目整个SLAM过程的尺度
// 因为CreateInitialMapMonocular函数对3D点深度会缩放，然后反过来对 t 有改变
// eq.(8)恢复原始的t
cv::Mat t = U*tp;
vt.push_back(t/cv::norm(t));
// 构造法向量np
cv::Mat np(3,1,CV_32F);
np.at<float>(0)=x1[i];
np.at<float>(1)=0;
np.at<float>(2)=x3[i];
// eq.(8) 恢复原始的法向量
cv::Mat n = V*np;
//看PPT 16页的图，保持平面法向量向上
if(n.at<float>(2)<0)
n=-n;
// 添加到vector
vn.push_back(n);
}
// Step 1.2 讨论 d' < 0 时的 4 组解
float aux_sphi = sqrt((d1*d1-d2*d2)*(d2*d2-d3*d3))/((d1-d3)*d2);
// cos_theta项
float cphi = (d1*d3-d2*d2)/((d1-d3)*d2);
// 考虑到e1,e2的取值，这里的sin_theta有两种可能的解
float sphi[] = {aux_sphi, -aux_sphi, -aux_sphi, aux_sphi};
// 对于每种由e1 e3取值的组合而形成的四种解的情况
for(int i=0; i<4; i++)
{
// 计算旋转矩阵 R'
cv::Mat Rp=cv::Mat::eye(3,3,CV_32F);
Rp.at<float>(0,0)=cphi;
Rp.at<float>(0,2)=sphi[i];
Rp.at<float>(1,1)=-1;
Rp.at<float>(2,0)=sphi[i];
Rp.at<float>(2,2)=-cphi;
// 恢复出原来的R
cv::Mat R = s*U*Rp*Vt;
// 然后添加到vector中
vR.push_back(R);
// 构造tp
cv::Mat tp(3,1,CV_32F);
tp.at<float>(0)=x1[i];
tp.at<float>(1)=0;
tp.at<float>(2)=x3[i];
tp*=d1+d3;
// 恢复出原来的t
cv::Mat t = U*tp;
// 归一化之后加入到vector中,要提供给上面的平移矩阵都是要进行过归一化的
vt.push_back(t/cv::norm(t));
// 构造法向量np
cv::Mat np(3,1,CV_32F);
np.at<float>(0)=x1[i];
np.at<float>(1)=0;
np.at<float>(2)=x3[i];
// 恢复出原来的法向量
cv::Mat n = V*np;
// 保证法向量指向上方
if(n.at<float>(2)<0)
n=-n;
// 添加到vector中
vn.push_back(n);
}
// 最好的good点
int bestGood = 0;
// 其次最好的good点
int secondBestGood = 0;
// 最好的解的索引，初始值为-1
int bestSolutionIdx = -1;
// 最大的视差角
float bestParallax = -1;
// 存储最好解对应的，对特征点对进行三角化测量的结果
vector<cv::Point3f> bestP3D;
// 最佳解所对应的，那些可以被三角化测量的点的标记
vector<bool> bestTriangulated;
// Instead of applying the visibility constraints proposed in the WFaugeras' paper (which could fail for points seen with low parallax)
// We reconstruct all hypotheses and check in terms of triangulated points and parallax
// Step 2. 对 8 组解进行验证，并选择产生相机前方最多3D点的解为最优解
for(size_t i=0; i<8; i++)
{
// 第i组解对应的比较大的视差角
float parallaxi;
// 三角化测量之后的特征点的空间坐标
vector<cv::Point3f> vP3Di;
// 特征点对是否被三角化的标记
vector<bool> vbTriangulatedi;
// 调用 Initializer::CheckRT(), 计算good点的数目
int nGood = CheckRT(vR[i],vt[i], //当前组解的旋转矩阵和平移向量
mvKeys1,mvKeys2, //特征点
mvMatches12,vbMatchesInliers, //特征匹配关系以及Inlier标记
K, //相机的内参数矩阵
vP3Di, //存储三角化测量之后的特征点空间坐标的
4.0*mSigma2, //三角化过程中允许的最大重投影误差
vbTriangulatedi, //特征点是否被成功进行三角测量的标记
parallaxi); // 这组解在三角化测量的时候的比较大的视差角
// 更新历史最优和次优的解
// 保留最优的和次优的解.保存次优解的目的是看看最优解是否突出
if(nGood>bestGood)
{
// 如果当前组解的good点数是历史最优，那么之前的历史最优就变成了历史次优
secondBestGood = bestGood;
// 更新历史最优点
bestGood = nGood;
// 最优解的组索引为i（就是当前次遍历）
bestSolutionIdx = i;
// 更新变量
bestParallax = parallaxi;
bestP3D = vP3Di;
bestTriangulated = vbTriangulatedi;
}
// 如果当前组的good计数小于历史最优但却大于历史次优
else if(nGood>secondBestGood)
{
// 说明当前组解是历史次优点，更新之
secondBestGood = nGood;
}
}
// Step 3 选择最优解。要满足下面的四个条件
// 1. good点数最优解明显大于次优解，这里取0.75经验值
// 2. 视角差大于规定的阈值
// 3. good点数要大于规定的最小的被三角化的点数量
// 4. good数要足够多，达到总数的90%以上
if(secondBestGood<0.75*bestGood &&
bestParallax>=minParallax &&
bestGood>minTriangulated &&
bestGood>0.9*N)
{
// 从最佳的解的索引访问到R，t
vR[bestSolutionIdx].copyTo(R21);
vt[bestSolutionIdx].copyTo(t21);
// 获得最佳解时，成功三角化的三维点，以后作为初始地图点使用
vP3D = bestP3D;
// 获取特征点的被成功进行三角化的标记
vbTriangulated = bestTriangulated;
//返回真，找到了最好的解
return true;
}
return false;
}结束语
以上就是我学习到的内容，如果对您有帮助请多多支持我，如果哪里有问题欢迎大家在评论区积极讨论，我看到会及时回复。