本次主要讲解的是单目初始化中求解本质矩阵并计算分数的函数Initializer::FindFundamental
首先，函数的参数为
void Initializer::FindFundamental(vector<bool> &vbMatchesInliers, // 标记是否是外点
float &score, // 计算基础矩阵的得分
cv::Mat &F21) // 基础矩阵的结果
其中主要进行了以下步骤
-
特征点坐标归一化与变量初始化
// 匹配的特征点对总数 // 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; -
选择8个点用于计算
// Step 2 选择8个归一化之后的点对进行迭代 for(size_t j=0; j<8; j++) { //从mvSets中获取当前次迭代的某个特征点对的索引信息 int idx = mvSets[it][j]; // vPn1i和vPn2i为匹配的特征点对的归一化后的坐标 // 首先根据这个特征点对的索引信息分别找到两个特征点在各自图像特征点向量中的索引，然后读取其归一化之后的特征点坐标 vPn1i[j] = vPn1[mvMatches12[idx].first]; //first存储在参考帧1中的特征点索引 vPn2i[j] = vPn2[mvMatches12[idx].second]; //second存储在参考帧2中的特征点索引 }//读取8对特征点的归一化之后的坐标 -
使用归一化后的特征点计算基础矩阵并利用归一化矩阵恢复原始坐标对应的基础矩阵
// 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 = 0 F21i = T2t*Fn*T1;这里需要注意由于单应矩阵和基础矩阵的方程定义不同，所以用归一化矩阵恢复的方式也不同
其中的
ComputeF21函数如下cv::Mat Initializer::ComputeF21( const vector<cv::Point2f> &vP1, //归一化后的点, in reference frame const vector<cv::Point2f> &vP2) //归一化后的点, in current frame { // x'Fx = 0 整理可得：Af = 0 // A = | x'x x'y x' y'x y'y y' x y 1 |, f = | f1 f2 f3 f4 f5 f6 f7 f8 f9 | // 通过SVD求解Af = 0，A'A最小特征值对应的特征向量即为解 //获取参与计算的特征点对数 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个奇异值设置为0 w.at<float>(2)=0; // 重新组合好满足秩约束的基础矩阵，作为最终计算结果返回 return u*cv::Mat::diag(w)*vt; }这里利用的就是直接线性求解方程组的方法，简要的推导已经给出，详细的数学证明请参考十四讲
这里需要注意基础矩阵除了尺度等价性，由于 t ∧ t^{\wedge} t∧的秩为2，且两个矩阵相乘的秩小于任意一个矩阵的秩导致基础矩阵额外还有一个最小奇异值为0的约束
-
利用重投影误差标记内点并计算分数
// Step 4 利用重投影误差为当次RANSAC的结果评分 currentScore = CheckFundamental(F21i, vbCurrentInliers, mSigma);其中的
CheckFundamental函数如下float Initializer::CheckFundamental( const cv::Mat &F21, //当前帧和参考帧之间的基础矩阵 vector<bool> &vbMatchesInliers, //匹配的特征点对属于inliers的标记 float sigma) //方差 { // 说明：在已值n维观测数据误差服从N(0，sigma）的高斯分布时 // 其误差加权最小二乘结果为 sum_error = SUM(e(i)^T * Q^(-1) * e(i)) // 其中：e(i) = [e_x,e_y,...]^T, Q维观测数据协方差矩阵，即sigma * sigma组成的协方差矩阵 // 误差加权最小二次结果越小，说明观测数据精度越高 // 那么，score = SUM((th - e(i)^T * Q^(-1) * e(i)))的分数就越高 // 算法目标：检查基础矩阵 // 检查方式：利用对极几何原理 p2^T * F * p1 = 0 // 假设：三维空间中的点 P 在 img1 和 img2 两图像上的投影分别为 p1 和 p2（两个为同名点） // 则：p2 一定存在于极线 l2 上，即 p2*l2 = 0. 而l2 = F*p1 = (a, b, c)^T // 所以，这里的误差项 e 为 p2 到 极线 l2 的距离，如果在直线上，则 e = 0 // 根据点到直线的距离公式：d = (ax + by + c) / sqrt(a * a + b * b) // 所以，e = (a * p2.x + b * p2.y + c) / sqrt(a * a + b * b) // 算法流程 // input: 基础矩阵 F 左右视图匹配点集 mvKeys1 // do: // for p1(i), p2(i) in mvKeys: // l2 = F * p1(i) // l1 = p2(i) * F // error_i1 = dist_point_to_line(x2,l2) // error_i2 = dist_point_to_line(x1,l1) // // w1 = 1 / sigma / sigma // w2 = 1 / sigma / sigma // // if error1 < th // score += thScore - error_i1 * w1 // if error2 < th // score += thScore - error_i2 * w2 // // if error_1i < th and error_2i < th // p1(i), p2(i) are inner points // vbMatchesInliers(i) = true // else // p1(i), p2(i) are outliers // vbMatchesInliers(i) = false // end // end // output: score, inliers // 获取匹配的特征点对的总对数 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++) { //默认为这对特征点是Inliers bool 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; else score += thScore - chiSquare2; // Step 2.5 保存结果 if(bIn) vbMatchesInliers[i]=true; else vbMatchesInliers[i]=false; } // 返回评分 return score; }注意，这里由于点线距离只有一个自由度，所以这里外点标记使用的是一自由度的卡方阈值，但是为了和单应矩阵进行评分比较，计算得分时使用的是二自由度的卡方阈值
-
更新最佳得分和对应的内点标记
// Step 5 更新具有最优评分的基础矩阵计算结果,并且保存所对应的特征点对的内点标记 if(currentScore>score) { //如果当前的结果得分更高，那么就更新最优计算结果 F21 = F21i.clone(); vbMatchesInliers = vbCurrentInliers; score = currentScore; }
5545
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
