本次主要讲解的是单目初始化中求解单应矩阵并计算分数的函数Initializer::FindHomography
首先,函数的参数为
void Initializer::FindHomography(vector<bool> &vbMatchesInliers, // 标记是否是外点
float &score, // 计算单应矩阵的得分
cv::Mat &H21) // 单应矩阵的结果
其中主要进行了以下步骤
- 特征点坐标归一化与变量初始化
//匹配的特征点对总数 const int N = mvMatches12.size(); // Normalize coordinates // Step 1 将当前帧和参考帧中的特征点坐标进行归一化,主要是平移和尺度变换 // 具体来说,就是将mvKeys1和mvKey2归一化到均值为0,一阶绝对矩为1,归一化矩阵分别为T1、T2 // 这里所谓的一阶绝对矩其实就是随机变量到取值的中心的绝对值的平均值 // 归一化矩阵就是把上述归一化的操作用矩阵来表示。这样特征点坐标乘归一化矩阵可以得到归一化后的坐标 //归一化后的参考帧1和当前帧2中的特征点坐标 vector<cv::Point2f> vPn1, vPn2; // 记录各自的归一化矩阵 cv::Mat T1, T2; Normalize(mvKeys1,vPn1, T1); Normalize(mvKeys2,vPn2, T2); //这里求的逆在后面的代码中要用到,辅助进行原始尺度的恢复 cv::Mat T2inv = T2.inv(); // Best Results variables // 记录最佳评分 score = 0.0; // 取得历史最佳评分时,特征点对的inliers标记 vbMatchesInliers = vector<bool>(N,false); // Iteration variables //某次迭代中,参考帧的特征点坐标 vector<cv::Point2f> vPn1i(8); //某次迭代中,当前帧的特征点坐标 vector<cv::Point2f> vPn2i(8); //以及计算出来的单应矩阵、及其逆矩阵 cv::Mat H21i, H12i; // 每次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 八点法计算单应矩阵 // 利用生成的8个归一化特征点对, 调用函数 Initializer::ComputeH21() 使用八点法计算单应矩阵 // 关于为什么计算之前要对特征点进行归一化,后面又恢复这个矩阵的尺度? // 可以在《计算机视觉中的多视图几何》这本书中P193页中找到答案 // 书中这里说,8点算法成功的关键是在构造解的方称之前应对输入的数据认真进行适当的归一化 cv::Mat Hn = ComputeH21(vPn1i,vPn2i); // 单应矩阵原理:X2=H21*X1,其中X1,X2 为归一化后的特征点 // 特征点归一化:vPn1 = T1 * mvKeys1, vPn2 = T2 * mvKeys2 得到:T2 * mvKeys2 = Hn * T1 * mvKeys1 // 进一步得到:mvKeys2 = T2.inv * Hn * T1 * mvKeys1 H21i = T2inv*Hn*T1; //然后计算逆 H12i = H21i.inv();
ComputeH21
函数如下
这里利用的就是直接线性求解方程组的方法,简要的推导已经给出,详细的数学证明请参考十四讲cv::Mat Initializer::ComputeH21( const vector<cv::Point2f> &vP1, //归一化后的点, in reference frame const vector<cv::Point2f> &vP2) //归一化后的点, in current frame { // |x'| | h1 h2 h3 ||x| // |y'| = a | h4 h5 h6 ||y| 简写: x' = a H x, a为一个尺度因子 // |1 | | h7 h8 h9 ||1| // 使用DLT(direct linear tranform)求解该模型 // x' = a H x // ---> (x') 叉乘 (H x) = 0 (因为方向相同) (取前两行就可以推导出下面的了) // ---> Ah = 0 // A = | 0 0 0 -x -y -1 xy' yy' y'| h = | h1 h2 h3 h4 h5 h6 h7 h8 h9 | // |-x -y -1 0 0 0 xx' yx' x'| // 通过SVD求解Ah = 0,A^T*A最小特征值对应的特征向量即为解 // 其实也就是右奇异值矩阵的最后一列 //获取参与计算的特征点的数目 const int N = vP1.size(); // 构造用于计算的矩阵 A cv::Mat A(2*N, //行,注意每一个点的数据对应两行 9, //列 CV_32F); //float数据类型 // 构造矩阵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>(2*i,0) = 0.0; A.at<float>(2*i,1) = 0.0; A.at<float>(2*i,2) = 0.0; A.at<float>(2*i,3) = -u1; A.at<float>(2*i,4) = -v1; A.at<float>(2*i,5) = -1; A.at<float>(2*i,6) = v2*u1; A.at<float>(2*i,7) = v2*v1; A.at<float>(2*i,8) = v2; //生成这个点的第二行 A.at<float>(2*i+1,0) = u1; A.at<float>(2*i+1,1) = v1; A.at<float>(2*i+1,2) = 1; A.at<float>(2*i+1,3) = 0.0; A.at<float>(2*i+1,4) = 0.0; A.at<float>(2*i+1,5) = 0.0; A.at<float>(2*i+1,6) = -u2*u1; A.at<float>(2*i+1,7) = -u2*v1; A.at<float>(2*i+1,8) = -u2; } // 定义输出变量,u是左边的正交矩阵U, w为奇异矩阵,vt中的t表示是右正交矩阵V的转置 cv::Mat u,w,vt; //使用opencv提供的进行奇异值分解的函数 cv::SVDecomp(A, //输入,待进行奇异值分解的矩阵 w, //输出,奇异值矩阵 u, //输出,矩阵U vt, //输出,矩阵V^T cv::SVD::MODIFY_A | //输入,MODIFY_A是指允许计算函数可以修改待分解的矩阵,官方文档上说这样可以加快计算速度、节省内存 cv::SVD::FULL_UV); //FULL_UV=把U和VT补充成单位正交方阵 // 返回最小奇异值所对应的右奇异向量 // 注意前面说的是右奇异值矩阵的最后一列,但是在这里因为是vt,转置后了,所以是行;由于A有9列数据,故最后一列的下标为8 return vt.row(8).reshape(0, //转换后的通道数,这里设置为0表示是与前面相同 3); //转换后的行数,对应V的最后一列 }
- 利用重投影误差标记内点并计算分数
其中的// Step 4 利用重投影误差为当次RANSAC的结果评分 currentScore = CheckHomography(H21i, H12i, //输入,单应矩阵的计算结果 vbCurrentInliers, //输出,特征点对的Inliers标记 mSigma); //TODO 测量误差,在Initializer类对象构造的时候,由外部给定的
CheckHomography
函数如下
这里主要是利用二自由度,显著性水平为0.05的卡方分布来标记外点,并将内点的重投影误差与卡方阈值的差的和作为本次RANSAC迭代的得分float Initializer::CheckHomography( const cv::Mat &H21, //从参考帧到当前帧的单应矩阵 const cv::Mat &H12, //从当前帧到参考帧的单应矩阵 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)))的分数就越高 // 算法目标: 检查单应变换矩阵 // 检查方式:通过H矩阵,进行参考帧和当前帧之间的双向投影,并计算起加权最小二乘投影误差 // 算法流程 // input: 单应性矩阵 H21, H12, 匹配点集 mvKeys1 // do: // for p1(i), p2(i) in mvKeys: // error_i1 = ||p2(i) - H21 * p1(i)||2 // error_i2 = ||p1(i) - H12 * p2(i)||2 // // w1 = 1 / sigma / sigma // w2 = 1 / sigma / sigma // // if error1 < th // score += th - error_i1 * w1 // if error2 < th // score += th - 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 h11 = H21.at<float>(0,0); const float h12 = H21.at<float>(0,1); const float h13 = H21.at<float>(0,2); const float h21 = H21.at<float>(1,0); const float h22 = H21.at<float>(1,1); const float h23 = H21.at<float>(1,2); const float h31 = H21.at<float>(2,0); const float h32 = H21.at<float>(2,1); const float h33 = H21.at<float>(2,2); // 获取从当前帧到参考帧的单应矩阵的各个元素 const float h11inv = H12.at<float>(0,0); const float h12inv = H12.at<float>(0,1); const float h13inv = H12.at<float>(0,2); const float h21inv = H12.at<float>(1,0); const float h22inv = H12.at<float>(1,1); const float h23inv = H12.at<float>(1,2); const float h31inv = H12.at<float>(2,0); const float h32inv = H12.at<float>(2,1); const float h33inv = H12.at<float>(2,2); // 给特征点对的Inliers标记预分配空间 vbMatchesInliers.resize(N); // 初始化score值 float score = 0; // 基于卡方检验计算出的阈值(假设测量有一个像素的偏差) // 自由度为2的卡方分布,显著性水平为0.05,对应的临界阈值 const float th = 5.991; //信息矩阵,方差平方的倒数 const float invSigmaSquare = 1.0/(sigma * sigma); // Step 2 通过H矩阵,进行参考帧和当前帧之间的双向投影,并计算起加权重投影误差 // H21 表示从img1 到 img2的变换矩阵 // H12 表示从img2 到 img1的变换矩阵 for(int i = 0; i < N; i++) { // 一开始都默认为Inlier 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; // Step 2.2 计算 img2 到 img1 的重投影误差 // x1 = H12*x2 // 将图像2中的特征点通过单应变换投影到图像1中 // |u1| |h11inv h12inv h13inv||u2| |u2in1| // |v1| = |h21inv h22inv h23inv||v2| = |v2in1| * w2in1inv // |1 | |h31inv h32inv h33inv||1 | | 1 | // 计算投影归一化坐标 const float w2in1inv = 1.0/(h31inv * u2 + h32inv * v2 + h33inv); const float u2in1 = (h11inv * u2 + h12inv * v2 + h13inv) * w2in1inv; const float v2in1 = (h21inv * u2 + h22inv * v2 + h23inv) * w2in1inv; // 计算重投影误差 = ||p1(i) - H12 * p2(i)||2 const float squareDist1 = (u1 - u2in1) * (u1 - u2in1) + (v1 - v2in1) * (v1 - v2in1); const float chiSquare1 = squareDist1 * invSigmaSquare; // Step 2.3 用阈值标记离群点,内点的话累加得分 if(chiSquare1>th) bIn = false; else // 误差越大,得分越低 score += th - chiSquare1; // 计算从img1 到 img2 的投影变换误差 // x1in2 = H21*x1 // 将图像2中的特征点通过单应变换投影到图像1中 // |u2| |h11 h12 h13||u1| |u1in2| // |v2| = |h21 h22 h23||v1| = |v1in2| * w1in2inv // |1 | |h31 h32 h33||1 | | 1 | // 计算投影归一化坐标 const float w1in2inv = 1.0/(h31*u1+h32*v1+h33); const float u1in2 = (h11*u1+h12*v1+h13)*w1in2inv; const float v1in2 = (h21*u1+h22*v1+h23)*w1in2inv; // 计算重投影误差 const float squareDist2 = (u2-u1in2)*(u2-u1in2)+(v2-v1in2)*(v2-v1in2); const float chiSquare2 = squareDist2*invSigmaSquare; // 用阈值标记离群点,内点的话累加得分 if(chiSquare2>th) bIn = false; else score += th - chiSquare2; // Step 2.4 如果从img2 到 img1 和 从img1 到img2的重投影误差均满足要求,则说明是Inlier point if(bIn) vbMatchesInliers[i]=true; else vbMatchesInliers[i]=false; } return score; }
- 更新最佳得分和对应的内点标记
// Step 5 更新具有最优评分的单应矩阵计算结果,并且保存所对应的特征点对的内点标记 if(currentScore>score) { //如果当前的结果得分更高,那么就更新最优计算结果 H21 = H21i.clone(); //保存匹配好的特征点对的Inliers标记 vbMatchesInliers = vbCurrentInliers; //更新历史最优评分 score = currentScore; }