特征匹配（一）：双目稀疏特征匹配
作者：互联网
本次主要讲解ORBSLAM2中的双目稀疏立体匹配函数ComputeStereoMatches()，这个函数主要是用于在左右目图像中寻找对应的匹配点对，并根据匹配点对来恢复特征点的深度，函数主要分为以下几步
-
预分配内存
// 为匹配结果预先分配内存，数据类型为float型 // mvuRight存储右图匹配点索引 // mvDepth存储特征点的深度信息 mvuRight = vector<float>(N,-1.0f); mvDepth = vector<float>(N,-1.0f); // orb特征相似度阈值 -> mean ～= (max + min) / 2 const int thOrbDist = (ORBmatcher::TH_HIGH+ORBmatcher::TH_LOW)/2; // 左图中金字塔顶层（0层）图像高 nRows const int nRows = mpORBextractorLeft->mvImagePyramid[0].rows; // 二维vector存储每一行的orb特征点的列坐标，为什么是vector，因为每一行的特征点有可能不一样，例如 // vRowIndices[0] = [1，2，5，8, 11] 第1行有5个特征点,他们的列号（即x坐标）分别是1,2,5,8,11 // vRowIndices[1] = [2，6，7，9, 13, 17, 20] 第2行有7个特征点.etc vector<vector<size_t> > vRowIndices(nRows, vector<size_t>()); // 预分配内存，认为每一行有200个特征点 for(int i=0; i<nRows; i++) vRowIndices[i].reserve(200); // 右图特征点数量，N表示数量 r表示右图，且不能被修改 const int Nr = mvKeysRight.size();- 行特征点统计，统计右图中每一个特征点可能出现的行号
// Step 1. 行特征点统计. 考虑到尺度金字塔特征，一个特征点可能存在于多行，而非唯一的一行 // 遍历每一个特征点 for(int iR = 0; iR < Nr; iR++) { // 获取特征点ir的y坐标，即行号 const cv::KeyPoint &kp = mvKeysRight[iR]; const float &kpY = kp.pt.y; // 计算特征点ir在行方向上，可能的偏移范围r，即可能的行号为[kpY + r, kpY -r] // 2 表示在全尺寸(scale = 1)的情况下，假设有2个像素的偏移，随着尺度变化，r也跟着变化 const float r = 2.0f * mvScaleFactors[mvKeysRight[iR].octave]; const int maxr = ceil(kpY + r); const int minr = floor(kpY - r); // 将特征点ir保证在可能的行号中 for(int yi=minr;yi<=maxr;yi++) vRowIndices[yi].push_back(iR); }注意，根据尺度的不同，右图中的每一个特征点的真实行坐标可能出现在多行中
- 遍历左图中每一个特征点，进行粗匹配，找到他在对应行号的右图中的特征点的索引
// 左图的特征点 const cv::KeyPoint &kpL = mvKeys[iL]; // 特征点的金字塔层级 const int &levelL = kpL.octave; // 特征点的像素坐标 const float &vL = kpL.pt.y; const float &uL = kpL.pt.x; // 获取左图特征点il所在行，以及在右图对应行中可能的匹配点 const vector<size_t> &vCandidates = vRowIndices[vL]; if(vCandidates.empty()) continue; // 计算理论上的最佳搜索范围 const float minU = uL-maxD; const float maxU = uL-minD; // 最大搜索范围小于0，说明无匹配点 if(maxU<0) continue; // 初始化最佳相似度，用最大相似度，以及最佳匹配点索引 int bestDist = ORBmatcher::TH_HIGH; size_t bestIdxR = 0; // 取出对应的描述子 const cv::Mat &dL = mDescriptors.row(iL); // Step2. 粗配准. 左图特征点il与右图中的可能的匹配点进行逐个比较,得到最相似匹配点的相似度和索引 for(size_t iC=0; iC<vCandidates.size(); iC++) { // 右图中的特征点索引 const size_t iR = vCandidates[iC]; // 右图中的特征点 const cv::KeyPoint &kpR = mvKeysRight[iR]; // 左图特征点il与带匹配点ic的空间尺度差超过2，放弃 if(kpR.octave<levelL-1 || kpR.octave>levelL+1) continue; // 使用列坐标(x)进行匹配，和stereomatch一样 const float &uR = kpR.pt.x; // 超出理论搜索范围[minU, maxU]，可能是误匹配，放弃 if(uR >= minU && uR <= maxU) { // 计算匹配点il和待匹配点ic的相似度dist const cv::Mat &dR = mDescriptorsRight.row(iR); const int dist = ORBmatcher::DescriptorDistance(dL,dR); //统计最小相似度及其对应的列坐标(x) if( dist<bestDist ) { bestDist = dist; bestIdxR = iR; } } } -
精匹配，将左图的特征点缩放到对应的金字塔层图像上，并将右图对应的粗匹配的特征点也投到相同层级的金子塔图像上，使用滑动窗口进行左右滑动，计算每一次滑动的图像块像素范数距离
// 如果刚才匹配过程中的最佳描述子距离小于给定的阈值 // Step 3. 精确匹配. if(bestDist<thOrbDist) { // 计算右图特征点x坐标和对应的金字塔尺度 // 右图对应特征点的x坐标 const float uR0 = mvKeysRight[bestIdxR].pt.x; // 左图特征点的逆缩放因子 const float scaleFactor = mvInvScaleFactors[kpL.octave]; // 尺度缩放后的左右图特征点坐标 // 也就是在对应的尺度金字塔上的坐标 const float scaleduL = round(kpL.pt.x*scaleFactor); const float scaledvL = round(kpL.pt.y*scaleFactor); const float scaleduR0 = round(uR0*scaleFactor); // 滑动窗口搜索, 类似模版卷积或滤波 // w表示sad相似度的窗口半径 // ? 这里为什么不对窗口大小也进行缩放？ const int w = 5; // 提取左图中，以特征点(scaleduL,scaledvL)为中心, 半径为w的图像快patch，注意这里是在特征金字塔上对应的层的图像快 cv::Mat IL = mpORBextractorLeft->mvImagePyramid[kpL.octave].rowRange(scaledvL-w,scaledvL+w+1).colRange(scaleduL-w,scaleduL+w+1); IL.convertTo(IL,CV_32F); // 图像块均值归一化，降低亮度变化对相似度计算的影响 IL = IL - IL.at<float>(w,w) * cv::Mat::ones(IL.rows,IL.cols,CV_32F); //初始化最佳相似度 int bestDist = INT_MAX; // 通过滑动窗口搜索优化，得到的列坐标偏移量 int bestincR = 0; //滑动窗口的滑动范围为（-L, L） const int L = 5; // 初始化存储图像块相似度 vector<float> vDists; vDists.resize(2*L+1); // 计算滑动窗口滑动范围的边界，因为是块匹配，还要算上图像块的尺寸 // 列方向起点 iniu = r0 - 最大窗口滑动范围 - 图像块尺寸 // 列方向终点 eniu = r0 + 最大窗口滑动范围 + 图像块尺寸 + 1 // 此次 + 1 和下面的提取图像块是列坐标+1是一样的，保证提取的图像块的宽是2 * w + 1 // ! 源码： const float iniu = scaleduR0+L-w; 错误 const float iniu = scaleduR0-L-w; const float endu = scaleduR0+L+w+1; // 判断搜索是否越界 if(iniu<0 || endu >= mpORBextractorRight->mvImagePyramid[kpL.octave].cols) continue; // 在搜索范围内从左到右滑动，并计算图像块相似度 for(int incR=-L; incR<=+L; incR++) { // 提取左图中，以特征点(scaleduL,scaledvL)为中心, 半径为w的图像快patch // 注意这里是在右图的对应于左图特征点金子塔对应层数的金字塔图像上左右滑动 cv::Mat IR = mpORBextractorRight->mvImagePyramid[kpL.octave].rowRange(scaledvL-w,scaledvL+w+1).colRange(scaleduR0+incR-w,scaleduR0+incR+w+1); IR.convertTo(IR,CV_32F); // 图像块均值归一化，降低亮度变化对相似度计算的影响 IR = IR - IR.at<float>(w,w) * cv::Mat::ones(IR.rows,IR.cols,CV_32F); // sad 计算 float dist = cv::norm(IL,IR,cv::NORM_L1); // 统计最小sad和偏移量 if(dist<bestDist) { bestDist = dist; bestincR = incR; } //L+incR 为refine后的匹配点列坐标(x) // 这里其实就是为每一个滑动值赋予距离 vDists[L+incR] = dist; } // 搜索窗口越界判断 if(bestincR==-L || bestincR==L) continue; -
亚像素插值：在使用右图中在缩放后图像上最优坐标的左右坐标的距离和当前坐标的距离进行抛物线拟合，得到最低点的坐标，并缩放到原始图像上作为匹配得到的右图对应特征点坐标并计算深度
// Step 4. 亚像素插值, 使用最佳匹配点及其左右相邻点构成抛物线 // 使用3点拟合抛物线的方式，用极小值代替之前计算的最优是差值 // \ / <- 由视差为14，15，16的相似度拟合的抛物线 // . .(16) // .14 .(15) <- int/uchar最佳视差值 // . // （14.5）<- 真实的视差值 // deltaR = 15.5 - 16 = -0.5 // 公式参考opencv sgbm源码中的亚像素插值公式 // 或论文<<On Building an Accurate Stereo Matching System on Graphics Hardware>> 公式7 const float dist1 = vDists[L+bestincR-1]; const float dist2 = vDists[L+bestincR]; const float dist3 = vDists[L+bestincR+1]; const float deltaR = (dist1-dist3)/(2.0f*(dist1+dist3-2.0f*dist2)); // 亚像素精度的修正量应该是在[-1,1]之间，否则就是误匹配 if(deltaR<-1 || deltaR>1) continue; // 根据亚像素精度偏移量delta调整最佳匹配索引 // 这里已经恢复到了原始图像上的坐标 float bestuR = mvScaleFactors[kpL.octave]*((float)scaleduR0+(float)bestincR+deltaR); // 计算视差 float disparity = (uL-bestuR); if(disparity>=minD && disparity<maxD) { // 如果存在负视差，则约束为0.01 if( disparity <=0 ) { disparity=0.01; bestuR = uL-0.01; } // 根据视差值计算深度信息 // 保存最相似点的列坐标(x)信息 // 保存归一化sad最小相似度 // Step 5. 最优视差值/深度选择. mvDepth[iL]=mbf/disparity; mvuRight[iL] = bestuR; vDistIdx.push_back(pair<int,int>(bestDist,iL)); -
根据阈值剔除离群点
// Step 6. 删除离群点(outliers) // 块匹配相似度阈值判断，归一化sad最小，并不代表就一定是匹配的，比如光照变化、弱纹理、无纹理等同样会造成误匹配 // 误匹配判断条件 norm_sad > 1.5 * 1.4 * median sort(vDistIdx.begin(),vDistIdx.end()); const float median = vDistIdx[vDistIdx.size()/2].first; const float thDist = 1.5f*1.4f*median; for(int i=vDistIdx.size()-1;i>=0;i--) { if(vDistIdx[i].first<thDist) // ! 这里break的原因是排序后从小到大进行的遍历，后面不可能有更小的距离了 break; else { // 误匹配点置为-1，和初始化时保持一直，作为error code mvuRight[vDistIdx[i].second]=-1; mvDepth[vDistIdx[i].second]=-1; } }
标签：匹配,特征,float,双目,int,坐标,const 来源： https://blog.csdn.net/weixin_39061796/article/details/119487129