ORB-SLAM2第二节---双目地图初始化

比起单目初始化,而双目实现地图的初始化非常简单,只需要一帧(左右目图像)即可完成初始化。

  1. 行特征点统计。考虑用图像金字塔尺度作为偏移量,在当前点上下正负偏移量(r)内的纵坐标值都认为是匹配点可能存在的行数。之所以这样做,是因为极线矫正后仍然存在一定的误差,通过这种方式可以避免漏匹配。对于左图中极线上的投影像素点,在右图中搜索的纵坐标范围是minr~maxr。
  2. 粗匹配。在左图中的特征点与右图中的候选匹配点进行逐个比较,得到描述子距离最小的点作为最佳的粗匹配点。根据三维点的距离范围可以将横坐标搜索范围限制在minU~maxU。maxU对应的是三维点位于无穷远处,视差为0时的横坐标,而minU对应的是三维点位于最近距离时的横坐标。
  3. 在粗匹配的基础上,在图像块滑动窗口内用差的绝对和(Sum of Absolute Differences,SAD)实现精确匹配。此时得到的匹配像素坐标仍然是整数坐标。如图10-4,图像块本身的窗口大小是2w+1,滑动窗口范围是-L~L。
  4. 在精确匹配的基础上,用亚像素插值得到最佳的亚像素匹配坐标。
  5. 根据最佳的亚像素匹配坐标计算视差,从而得到深度值。
  6. 判断并删除外点。

ORB-SLAM2第二节---双目地图初始化_第1张图片

ORB-SLAM2第二节---双目地图初始化_第2张图片

亚像素坐标是指:坐标误差小于一个像素的形式。(浮点数) 

/*
 * 双目匹配函数
 *
 * 为左图的每一个特征点在右图中找到匹配点 \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特征点, 得到qi
          3. 精确匹配. 以点qi为中心,半径为r的范围内,进行块匹配(归一化SAD),进一步优化匹配结果
          4. 亚像素精度优化. 步骤3得到的视差为uchar/int类型精度,并不一定是真实视差,通过亚像素差值(抛物线插值)获取float精度的真实视差
          5. 最优视差值/深度选择. 通过胜者为王算法(WTA)获取最佳匹配点。
          6. 删除离群点(outliers). 块匹配相似度阈值判断,归一化sad最小,并不代表就一定是正确匹配,比如光照变化、弱纹理等会造成误匹配
     * 输出:稀疏特征点视差图/深度图(亚像素精度)mvDepth 匹配结果 mvuRight
     */

    // 为匹配结果预先分配内存,数据类型为float型
    // mvuRight存储右图匹配点索引
    // mvDepth存储特征点的深度信息
	mvuRight = vector(N,-1.0f);
    mvDepth = vector(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 > vRowIndices(nRows, vector());
    for(int i=0; i > vDistIdx;
    vDistIdx.reserve(N);

    // 为左图每一个特征点il,在右图搜索最相似的特征点ir
    for(int iL=0; iL &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);
        
        // Step 2. 粗配准。左图特征点il与右图中的可能的匹配点进行逐个比较,得到最相似匹配点的描述子距离和索引
        for(size_t iC=0; iClevelL+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( distmvImagePyramid[kpL.octave].rowRange(scaledvL-w,scaledvL+w+1).colRange(scaleduL-w,scaleduL+w+1);
            IL.convertTo(IL,CV_32F);
            
            // 图像块均值归一化,降低亮度变化对相似度计算的影响
			IL = IL - IL.at(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 vDists;
            vDists.resize(2*L+1); 

            // 计算滑动窗口滑动范围的边界,因为是块匹配,还要算上图像块的尺寸
            // 列方向起点 iniu = r0 - 最大窗口滑动范围 - 图像块尺寸
            // 列方向终点 eniu = r0 + 最大窗口滑动范围 + 图像块尺寸 + 1
            // 此次 + 1 和下面的提取图像块是列坐标+1是一样的,保证提取的图像块的宽是2 * w + 1
            // ! 源码: const float iniu = scaleduR0+L-w; 错误
            // scaleduR0:右图特征点x坐标
            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(w,w) * cv::Mat::ones(IR.rows,IR.cols,CV_32F);
                
                // sad 计算,值越小越相似
                float dist = cv::norm(IL,IR,cv::NORM_L1);

                // 统计最小sad和偏移量
                if(dist> 公式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(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

你可能感兴趣的:(算法,计算机视觉,人工智能,c++,机器人)