河北网站建设中心,海口建设公司网站,标书制作培训机构,找人做一下网站大概多少钱比起单目初始化#xff0c;而双目实现地图的初始化非常简单#xff0c;只需要一帧#xff08;左右目图像#xff09;即可完成初始化。
行特征点统计。考虑用图像金字塔尺度作为偏移量#xff0c;在当前点上下正负偏移量#xff08;r)内的纵坐标值都认为是匹配点可能存在…比起单目初始化而双目实现地图的初始化非常简单只需要一帧左右目图像即可完成初始化。
行特征点统计。考虑用图像金字塔尺度作为偏移量在当前点上下正负偏移量r)内的纵坐标值都认为是匹配点可能存在的行数。之所以这样做是因为极线矫正后仍然存在一定的误差通过这种方式可以避免漏匹配。对于左图中极线上的投影像素点在右图中搜索的纵坐标范围是minr~maxr。粗匹配。在左图中的特征点与右图中的候选匹配点进行逐个比较得到描述子距离最小的点作为最佳的粗匹配点。根据三维点的距离范围可以将横坐标搜索范围限制在minU~maxU。maxU对应的是三维点位于无穷远处视差为0时的横坐标而minU对应的是三维点位于最近距离时的横坐标。在粗匹配的基础上在图像块滑动窗口内用差的绝对和Sum of Absolute Differences,SAD)实现精确匹配。此时得到的匹配像素坐标仍然是整数坐标。如图10-4 /** 双目匹配函数** 为左图的每一个特征点在右图中找到匹配点 \n* 根据基线(有冗余范围)上描述子距离找到匹配, 再进行SAD精确定位 \n ‘* 这里所说的SAD是一种双目立体视觉匹配算法可参考[https://blog.csdn.net/u012507022/article/details/51446891]* 最后对所有SAD的值进行排序, 剔除SAD值较大的匹配对然后利用抛物线拟合得到亚像素精度的匹配 \n * 这里所谓的亚像素精度就是使用这个拟合得到一个小于一个单位像素的修正量这样可以取得更好的估计结果计算出来的点的深度也就越准确* 匹配成功后会更新 mvuRight(ur) 和 mvDepth(Z)*/
void Frame::ComputeStereoMatches()
{/*两帧图像稀疏立体匹配即ORB特征点匹配非逐像素的密集匹配但依然满足行对齐* 输入两帧立体矫正后的图像img_left 和 img_right 对应的orb特征点集* 过程1. 行特征点统计. 统计img_right每一行上的ORB特征点集便于使用立体匹配思路(行搜索/极线搜索进行同名点搜索, 避免逐像素的判断.2. 粗匹配. 根据步骤1的结果对img_left第i行的orb特征点pi在img_right的第i行上的orb特征点集中搜索相似orb特征点, 得到qi3. 精确匹配. 以点qi为中心半径为r的范围内进行块匹配归一化SAD进一步优化匹配结果4. 亚像素精度优化. 步骤3得到的视差为uchar/int类型精度并不一定是真实视差通过亚像素差值抛物线插值)获取float精度的真实视差5. 最优视差值/深度选择. 通过胜者为王算法WTA获取最佳匹配点。6. 删除离群点(outliers). 块匹配相似度阈值判断归一化sad最小并不代表就一定是正确匹配比如光照变化、弱纹理等会造成误匹配* 输出稀疏特征点视差图/深度图亚像素精度mvDepth 匹配结果 mvuRight*/// 为匹配结果预先分配内存数据类型为float型// mvuRight存储右图匹配点索引// mvDepth存储特征点的深度信息mvuRight vectorfloat(N,-1.0f);mvDepth vectorfloat(N,-1.0f);// orb特征相似度阈值 - mean (max min) / 2const int thOrbDist (ORBmatcher::TH_HIGHORBmatcher::TH_LOW)/2;// 金字塔底层0层图像高 nRowsconst int nRows mpORBextractorLeft-mvImagePyramid[0].rows;// 二维vector存储每一行的orb特征点的列坐标的索引为什么是vector因为每一行的特征点有可能不一样例如// vRowIndices[0] [1258, 11] 第1行有5个特征点,他们的列号即x坐标分别是1,2,5,8,11// vRowIndices[1] [2679, 13, 17, 20] 第2行有7个特征点.etcvectorvectorsize_t vRowIndices(nRows, vectorsize_t());for(int i0; inRows; 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 yiminr;yimaxr;yi)vRowIndices[yi].push_back(iR);}// 下面是 粗匹配 精匹配的过程// 对于立体矫正后的两张图在列方向(x)存在最大视差maxd和最小视差mind// 也即是左图中任何一点p在右图上的匹配点的范围为应该是[p - maxd, p - mind], 而不需要遍历每一行所有的像素// maxd baseline * length_focal / minZ// mind baseline * length_focal / maxZconst float minZ mb;const float minD 0; // 最小视差为0对应无穷远 const float maxD mbf/minZ; // 最大视差对应的距离是相机的基线// 保存sad块匹配相似度和左图特征点索引vectorpairint, int vDistIdx;vDistIdx.reserve(N);// 为左图每一个特征点il在右图搜索最相似的特征点irfor(int iL0; iLN; iL) {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 vectorsize_t vCandidates vRowIndices[vL];if(vCandidates.empty()) continue;// 计算理论上的最佳搜索范围const float minU uL-maxD;const float maxU uL-minD;// 最大搜索范围小于0说明无匹配点if(maxU0) continue;// 初始化最佳相似度用最大相似度以及最佳匹配点索引int bestDist ORBmatcher::TH_HIGH;size_t bestIdxR 0;const cv::Mat dL mDescriptors.row(iL);// Step 2. 粗配准。左图特征点il与右图中的可能的匹配点进行逐个比较,得到最相似匹配点的描述子距离和索引for(size_t iC0; iCvCandidates.size(); iC) {const size_t iR vCandidates[iC];const cv::KeyPoint kpR mvKeysRight[iR];// 左图特征点il与待匹配点ic的空间尺度差超过2放弃if(kpR.octavelevelL-1 || kpR.octavelevelL1)continue;// 使用列坐标(x)进行匹配和stereomatch一样const float uR kpR.pt.x;// 超出理论搜索范围[minU, maxU]可能是误匹配放弃if(uR minU uR maxU) {// 计算匹配点il和待匹配点ic的相似度distconst cv::Mat dR mDescriptorsRight.row(iR);const int dist ORBmatcher::DescriptorDistance(dL,dR);//统计最小相似度及其对应的列坐标(x)if( distbestDist ) {bestDist dist;bestIdxR iR;}}}// Step 3. 图像块滑动窗口用SAD(Sum of absolute differences差的绝对和)实现精确匹配. if(bestDistthOrbDist) {// 如果刚才匹配过程中的最佳描述子距离小于给定的阈值// 计算右图特征点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的图像块patchcv::Mat IL mpORBextractorLeft-mvImagePyramid[kpL.octave].rowRange(scaledvL-w,scaledvLw1).colRange(scaleduL-w,scaleduLw1);IL.convertTo(IL,CV_32F);// 图像块均值归一化降低亮度变化对相似度计算的影响IL IL - IL.atfloat(w,w) * cv::Mat::ones(IL.rows,IL.cols,CV_32F);//初始化最佳相似度int bestDist INT_MAX;// 通过滑动窗口搜索优化得到的列坐标偏移量int bestincR 0;//滑动窗口的滑动范围为-L, Lconst int L 5;// 初始化存储图像块相似度vectorfloat vDists;vDists.resize(2*L1); // 计算滑动窗口滑动范围的边界因为是块匹配还要算上图像块的尺寸// 列方向起点 iniu r0 - 最大窗口滑动范围 - 图像块尺寸// 列方向终点 eniu r0 最大窗口滑动范围 图像块尺寸 1// 此次 1 和下面的提取图像块是列坐标1是一样的保证提取的图像块的宽是2 * w 1// ! 源码 const float iniu scaleduR0L-w; 错误// scaleduR0右图特征点x坐标const float iniu scaleduR0-L-w;const float endu scaleduR0Lw1;// 判断搜索是否越界if(iniu0 || endu mpORBextractorRight-mvImagePyramid[kpL.octave].cols)continue;// 在搜索范围内从左到右滑动并计算图像块相似度for(int incR-L; incRL; incR) {// 提取右图中以特征点(scaleduL,scaledvL)为中心, 半径为w的图像快patchcv::Mat IR mpORBextractorRight-mvImagePyramid[kpL.octave].rowRange(scaledvL-w,scaledvLw1).colRange(scaleduR0incR-w,scaleduR0incRw1);IR.convertTo(IR,CV_32F);// 图像块均值归一化降低亮度变化对相似度计算的影响IR IR - IR.atfloat(w,w) * cv::Mat::ones(IR.rows,IR.cols,CV_32F);// sad 计算值越小越相似float dist cv::norm(IL,IR,cv::NORM_L1);// 统计最小sad和偏移量if(distbestDist) {bestDist dist;bestincR incR;}//LincR 为refine后的匹配点列坐标(x)vDists[LincR] dist; }// 搜索窗口越界判断if(bestincR-L || bestincRL)continue;// Step 4. 亚像素插值, 使用最佳匹配点及其左右相邻点构成抛物线来得到最小sad的亚像素坐标// 使用3点拟合抛物线的方式用极小值代替之前计算的最优是差值// \ / - 由视差为141516的相似度拟合的抛物线// . .(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 公式7const float dist1 vDists[LbestincR-1]; const float dist2 vDists[LbestincR];const float dist3 vDists[LbestincR1];const float deltaR (dist1-dist3)/(2.0f*(dist1dist3-2.0f*dist2));// 亚像素精度的修正量应该是在[-1,1]之间否则就是误匹配if(deltaR-1 || deltaR1)continue;// 根据亚像素精度偏移量delta调整最佳匹配索引float bestuR mvScaleFactors[kpL.octave]*((float)scaleduR0(float)bestincRdeltaR);float disparity (uL-bestuR);if(disparityminD disparitymaxD) {// 如果存在负视差则约束为0.01if( disparity 0 ) {disparity0.01;bestuR uL-0.01;}// 根据视差值计算深度信息// 保存最相似点的列坐标(x)信息// 保存归一化sad最小相似度// Step 5. 最优视差值/深度选择.mvDepth[iL]mbf/disparity;mvuRight[iL] bestuR;vDistIdx.push_back(pairint,int(bestDist,iL));} }}// Step 6. 删除离群点(outliers)// 块匹配相似度阈值判断归一化sad最小并不代表就一定是匹配的比如光照变化、弱纹理、无纹理等同样会造成误匹配// 误匹配判断条件 norm_sad 1.5 * 1.4 * mediansort(vDistIdx.begin(),vDistIdx.end());const float median vDistIdx[vDistIdx.size()/2].first;const float thDist 1.5f*1.4f*median;for(int ivDistIdx.size()-1;i0;i--) {if(vDistIdx[i].firstthDist)break;else {// 误匹配点置为-1和初始化时保持一直作为error codemvuRight[vDistIdx[i].second]-1;mvDepth[vDistIdx[i].second]-1;}}
}
