比起單目初始化，而雙目實(shí)現(xiàn)地圖的初始化非常簡(jiǎn)單，只需要一幀（左右目圖像）即可完成初始化。

1. 行特征點(diǎn)統(tǒng)計(jì)?？紤]用圖像金字塔尺度作為偏移量，在當(dāng)前點(diǎn)上下正負(fù)偏移量（r)內(nèi)的縱坐標(biāo)值都認(rèn)為是匹配點(diǎn)可能存在的行數(shù)。之所以這樣做，是因?yàn)闃O線矯正后仍然存在一定的誤差，通過(guò)這種方式可以避免漏匹配。對(duì)于左圖中極線上的投影像素點(diǎn)，在右圖中搜索的縱坐標(biāo)范圍是minr~maxr。
2. 粗匹配。在左圖中的特征點(diǎn)與右圖中的候選匹配點(diǎn)進(jìn)行逐個(gè)比較，得到描述子距離最小的點(diǎn)作為最佳的粗匹配點(diǎn)。根據(jù)三維點(diǎn)的距離范圍可以將橫坐標(biāo)搜索范圍限制在minU~maxU。maxU對(duì)應(yīng)的是三維點(diǎn)位于無(wú)窮遠(yuǎn)處，視差為0時(shí)的橫坐標(biāo)，而minU對(duì)應(yīng)的是三維點(diǎn)位于最近距離時(shí)的橫坐標(biāo)。
3. 在粗匹配的基礎(chǔ)上，在圖像塊滑動(dòng)窗口內(nèi)用差的絕對(duì)和（Sum of Absolute Differences,SAD)實(shí)現(xiàn)精確匹配。此時(shí)得到的匹配像素坐標(biāo)仍然是整數(shù)坐標(biāo)。如圖10-4，

?

?

/** 雙目匹配函數(shù)** 為左圖的每一個(gè)特征點(diǎn)在右圖中找到匹配點(diǎn) \n* 根據(jù)基線(有冗余范圍)上描述子距離找到匹配, 再進(jìn)行SAD精確定位 \n ‘* 這里所說(shuō)的SAD是一種雙目立體視覺(jué)匹配算法，可參考[https://blog.csdn.net/u012507022/article/details/51446891]* 最后對(duì)所有SAD的值進(jìn)行排序, 剔除SAD值較大的匹配對(duì)，然后利用拋物線擬合得到亞像素精度的匹配 \n * 這里所謂的亞像素精度，就是使用這個(gè)擬合得到一個(gè)小于一個(gè)單位像素的修正量，這樣可以取得更好的估計(jì)結(jié)果，計(jì)算出來(lái)的點(diǎn)的深度也就越準(zhǔn)確* 匹配成功后會(huì)更新 mvuRight(ur) 和 mvDepth(Z)*/
void Frame::ComputeStereoMatches()
{/*兩幀圖像稀疏立體匹配（即：ORB特征點(diǎn)匹配，非逐像素的密集匹配，但依然滿足行對(duì)齊）* 輸入：兩幀立體矯正后的圖像img_left 和 img_right 對(duì)應(yīng)的orb特征點(diǎn)集* 過(guò)程：1. 行特征點(diǎn)統(tǒng)計(jì). 統(tǒng)計(jì)img_right每一行上的ORB特征點(diǎn)集，便于使用立體匹配思路(行搜索/極線搜索）進(jìn)行同名點(diǎn)搜索, 避免逐像素的判斷.2. 粗匹配. 根據(jù)步驟1的結(jié)果，對(duì)img_left第i行的orb特征點(diǎn)pi，在img_right的第i行上的orb特征點(diǎn)集中搜索相似orb特征點(diǎn), 得到qi3. 精確匹配. 以點(diǎn)qi為中心，半徑為r的范圍內(nèi)，進(jìn)行塊匹配（歸一化SAD），進(jìn)一步優(yōu)化匹配結(jié)果4. 亞像素精度優(yōu)化. 步驟3得到的視差為uchar/int類(lèi)型精度，并不一定是真實(shí)視差，通過(guò)亞像素差值（拋物線插值)獲取float精度的真實(shí)視差5. 最優(yōu)視差值/深度選擇. 通過(guò)勝者為王算法（WTA）獲取最佳匹配點(diǎn)。6. 刪除離群點(diǎn)(outliers). 塊匹配相似度閾值判斷，歸一化sad最小，并不代表就一定是正確匹配，比如光照變化、弱紋理等會(huì)造成誤匹配* 輸出：稀疏特征點(diǎn)視差圖/深度圖（亞像素精度）mvDepth 匹配結(jié)果 mvuRight*/// 為匹配結(jié)果預(yù)先分配內(nèi)存，數(shù)據(jù)類(lèi)型為float型// mvuRight存儲(chǔ)右圖匹配點(diǎn)索引// mvDepth存儲(chǔ)特征點(diǎn)的深度信息mvuRight = vector<float>(N,-1.0f);mvDepth = vector<float>(N,-1.0f);// orb特征相似度閾值  -> mean ～= (max  + min) / 2const int thOrbDist = (ORBmatcher::TH_HIGH+ORBmatcher::TH_LOW)/2;// 金字塔底層（0層）圖像高 nRowsconst int nRows = mpORBextractorLeft->mvImagePyramid[0].rows;// 二維vector存儲(chǔ)每一行的orb特征點(diǎn)的列坐標(biāo)的索引，為什么是vector，因?yàn)槊恳恍械奶卣鼽c(diǎn)有可能不一樣，例如// vRowIndices[0] = [1，2，5，8, 11]   第1行有5個(gè)特征點(diǎn),他們的列號(hào)（即x坐標(biāo)）分別是1,2,5,8,11// vRowIndices[1] = [2，6，7，9, 13, 17, 20]  第2行有7個(gè)特征點(diǎn).etcvector<vector<size_t> > vRowIndices(nRows, vector<size_t>());for(int i=0; i<nRows; i++) vRowIndices[i].reserve(200);// 右圖特征點(diǎn)數(shù)量，N表示數(shù)量 r表示右圖，且不能被修改const int Nr = mvKeysRight.size();// Step 1. 行特征點(diǎn)統(tǒng)計(jì)。 考慮用圖像金字塔尺度作為偏移，左圖中對(duì)應(yīng)右圖的一個(gè)特征點(diǎn)可能存在于多行，而非唯一的一行for(int iR = 0; iR < Nr; iR++) {// 獲取特征點(diǎn)ir的y坐標(biāo)，即行號(hào)const cv::KeyPoint &kp = mvKeysRight[iR];const float &kpY = kp.pt.y;// 計(jì)算特征點(diǎn)ir在行方向上，可能的偏移范圍r，即可能的行號(hào)為[kpY + r, kpY -r]// 2 表示在全尺寸(scale = 1)的情況下，假設(shè)有2個(gè)像素的偏移，隨著尺度變化，r也跟著變化const float r = 2.0f * mvScaleFactors[mvKeysRight[iR].octave];const int maxr = ceil(kpY + r);const int minr = floor(kpY - r);// 將特征點(diǎn)ir保證在可能的行號(hào)中for(int yi=minr;yi<=maxr;yi++)vRowIndices[yi].push_back(iR);}// 下面是 粗匹配 + 精匹配的過(guò)程// 對(duì)于立體矯正后的兩張圖，在列方向(x)存在最大視差maxd和最小視差mind// 也即是左圖中任何一點(diǎn)p，在右圖上的匹配點(diǎn)的范圍為應(yīng)該是[p - maxd, p - mind], 而不需要遍歷每一行所有的像素// maxd = baseline * length_focal / minZ// mind = baseline * length_focal / maxZconst float minZ = mb;const float minD = 0;			// 最小視差為0，對(duì)應(yīng)無(wú)窮遠(yuǎn) const float maxD = mbf/minZ;    // 最大視差對(duì)應(yīng)的距離是相機(jī)的基線// 保存sad塊匹配相似度和左圖特征點(diǎn)索引vector<pair<int, int> > vDistIdx;vDistIdx.reserve(N);// 為左圖每一個(gè)特征點(diǎn)il，在右圖搜索最相似的特征點(diǎn)irfor(int iL=0; iL<N; iL++) {const cv::KeyPoint &kpL = mvKeys[iL];const int &levelL = kpL.octave;const float &vL = kpL.pt.y;const float &uL = kpL.pt.x;// 獲取左圖特征點(diǎn)il所在行，以及在右圖對(duì)應(yīng)行中可能的匹配點(diǎn)const vector<size_t> &vCandidates = vRowIndices[vL];if(vCandidates.empty()) continue;// 計(jì)算理論上的最佳搜索范圍const float minU = uL-maxD;const float maxU = uL-minD;// 最大搜索范圍小于0，說(shuō)明無(wú)匹配點(diǎn)if(maxU<0) continue;// 初始化最佳相似度，用最大相似度，以及最佳匹配點(diǎn)索引int bestDist = ORBmatcher::TH_HIGH;size_t bestIdxR = 0;const cv::Mat &dL = mDescriptors.row(iL);// Step 2. 粗配準(zhǔn)。左圖特征點(diǎn)il與右圖中的可能的匹配點(diǎn)進(jìn)行逐個(gè)比較,得到最相似匹配點(diǎn)的描述子距離和索引for(size_t iC=0; iC<vCandidates.size(); iC++) {const size_t iR = vCandidates[iC];const cv::KeyPoint &kpR = mvKeysRight[iR];// 左圖特征點(diǎn)il與待匹配點(diǎn)ic的空間尺度差超過(guò)2，放棄if(kpR.octave<levelL-1 || kpR.octave>levelL+1)continue;// 使用列坐標(biāo)(x)進(jìn)行匹配，和stereomatch一樣const float &uR = kpR.pt.x;// 超出理論搜索范圍[minU, maxU]，可能是誤匹配，放棄if(uR >= minU && uR <= maxU) {// 計(jì)算匹配點(diǎn)il和待匹配點(diǎn)ic的相似度distconst cv::Mat &dR = mDescriptorsRight.row(iR);const int dist = ORBmatcher::DescriptorDistance(dL,dR);//統(tǒng)計(jì)最小相似度及其對(duì)應(yīng)的列坐標(biāo)(x)if( dist<bestDist ) {bestDist = dist;bestIdxR = iR;}}}// Step 3. 圖像塊滑動(dòng)窗口用SAD(Sum of absolute differences，差的絕對(duì)和)實(shí)現(xiàn)精確匹配. if(bestDist<thOrbDist) {// 如果剛才匹配過(guò)程中的最佳描述子距離小于給定的閾值// 計(jì)算右圖特征點(diǎn)x坐標(biāo)和對(duì)應(yīng)的金字塔尺度const float uR0 = mvKeysRight[bestIdxR].pt.x;const float scaleFactor = mvInvScaleFactors[kpL.octave];// 尺度縮放后的左右圖特征點(diǎn)坐標(biāo)const float scaleduL = round(kpL.pt.x*scaleFactor);			const float scaledvL = round(kpL.pt.y*scaleFactor);const float scaleduR0 = round(uR0*scaleFactor);// 滑動(dòng)窗口搜索, 類(lèi)似模版卷積或?yàn)V波// w表示sad相似度的窗口半徑const int w = 5;// 提取左圖中，以特征點(diǎn)(scaleduL,scaledvL)為中心, 半徑為w的圖像塊patchcv::Mat IL = mpORBextractorLeft->mvImagePyramid[kpL.octave].rowRange(scaledvL-w,scaledvL+w+1).colRange(scaleduL-w,scaleduL+w+1);IL.convertTo(IL,CV_32F);// 圖像塊均值歸一化，降低亮度變化對(duì)相似度計(jì)算的影響IL = IL - IL.at<float>(w,w) * cv::Mat::ones(IL.rows,IL.cols,CV_32F);//初始化最佳相似度int bestDist = INT_MAX;// 通過(guò)滑動(dòng)窗口搜索優(yōu)化，得到的列坐標(biāo)偏移量int bestincR = 0;//滑動(dòng)窗口的滑動(dòng)范圍為（-L, L）const int L = 5;// 初始化存儲(chǔ)圖像塊相似度vector<float> vDists;vDists.resize(2*L+1); // 計(jì)算滑動(dòng)窗口滑動(dòng)范圍的邊界，因?yàn)槭菈K匹配，還要算上圖像塊的尺寸// 列方向起點(diǎn) iniu = r0 - 最大窗口滑動(dòng)范圍 - 圖像塊尺寸// 列方向終點(diǎn) eniu = r0 + 最大窗口滑動(dòng)范圍 + 圖像塊尺寸 + 1// 此次 + 1 和下面的提取圖像塊是列坐標(biāo)+1是一樣的，保證提取的圖像塊的寬是2 * w + 1// ! 源碼： const float iniu = scaleduR0+L-w; 錯(cuò)誤// scaleduR0：右圖特征點(diǎn)x坐標(biāo)const float iniu = scaleduR0-L-w;const float endu = scaleduR0+L+w+1;// 判斷搜索是否越界if(iniu<0 || endu >= mpORBextractorRight->mvImagePyramid[kpL.octave].cols)continue;// 在搜索范圍內(nèi)從左到右滑動(dòng)，并計(jì)算圖像塊相似度f(wàn)or(int incR=-L; incR<=+L; incR++) {// 提取右圖中，以特征點(diǎn)(scaleduL,scaledvL)為中心, 半徑為w的圖像快patchcv::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);// 圖像塊均值歸一化，降低亮度變化對(duì)相似度計(jì)算的影響IR = IR - IR.at<float>(w,w) * cv::Mat::ones(IR.rows,IR.cols,CV_32F);// sad 計(jì)算，值越小越相似float dist = cv::norm(IL,IR,cv::NORM_L1);// 統(tǒng)計(jì)最小sad和偏移量if(dist<bestDist) {bestDist = dist;bestincR = incR;}//L+incR 為refine后的匹配點(diǎn)列坐標(biāo)(x)vDists[L+incR] = dist; 	}// 搜索窗口越界判斷if(bestincR==-L || bestincR==L)continue;// Step 4. 亞像素插值, 使用最佳匹配點(diǎn)及其左右相鄰點(diǎn)構(gòu)成拋物線來(lái)得到最小sad的亞像素坐標(biāo)// 使用3點(diǎn)擬合拋物線的方式，用極小值代替之前計(jì)算的最優(yōu)是差值//    \                 / <- 由視差為14，15，16的相似度擬合的拋物線//      .             .(16)//         .14     .(15) <- int/uchar最佳視差值//              . //           （14.5）<- 真實(shí)的視差值//   deltaR = 15.5 - 16 = -0.5// 公式參考o(jì)pencv sgbm源碼中的亞像素插值公式// 或論文<<On Building an Accurate Stereo Matching System on Graphics Hardware>> 公式7const 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));// 亞像素精度的修正量應(yīng)該是在[-1,1]之間，否則就是誤匹配if(deltaR<-1 || deltaR>1)continue;// 根據(jù)亞像素精度偏移量delta調(diào)整最佳匹配索引float bestuR = mvScaleFactors[kpL.octave]*((float)scaleduR0+(float)bestincR+deltaR);float disparity = (uL-bestuR);if(disparity>=minD && disparity<maxD) {// 如果存在負(fù)視差，則約束為0.01if( disparity <=0 ) {disparity=0.01;bestuR = uL-0.01;}// 根據(jù)視差值計(jì)算深度信息// 保存最相似點(diǎn)的列坐標(biāo)(x)信息// 保存歸一化sad最小相似度// Step 5. 最優(yōu)視差值/深度選擇.mvDepth[iL]=mbf/disparity;mvuRight[iL] = bestuR;vDistIdx.push_back(pair<int,int>(bestDist,iL));}   }}// Step 6. 刪除離群點(diǎn)(outliers)// 塊匹配相似度閾值判斷，歸一化sad最小，并不代表就一定是匹配的，比如光照變化、弱紋理、無(wú)紋理等同樣會(huì)造成誤匹配// 誤匹配判斷條件  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 i=vDistIdx.size()-1;i>=0;i--) {if(vDistIdx[i].first<thDist)break;else {// 誤匹配點(diǎn)置為-1，和初始化時(shí)保持一直，作為error codemvuRight[vDistIdx[i].second]=-1;mvDepth[vDistIdx[i].second]=-1;}}
}