orb-slam系列 Tracking线程匹配(四)

ComputeStereoMatches() 函数

1）在同一行，设定一个阈值，找到大致的位置
2）在范围内所有的目标关键点进行甄别，需要满足条件在合理的视差范围内，并且金字塔层数不能超过一

    const float minZ = mb;   //人为设置最小深度const float minD = 0;    //最小视差const float maxD = mbf/minZ;    //最大视差

3）计算符合的描述子距离，选择最佳描述子。
4）SAD匹配提高像素匹配修正量bestincR

// 通过SAD匹配提高像素匹配修正量bestincRif(bestDist<thOrbDist){// coordinates in image pyramid at keypoint scale// kpL.pt.x对应金字塔最底层坐标，将最佳匹配的特征点对尺度变换到尺度对应层 (scaleduL, scaledvL) (scaleduR0, )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);// sliding window searchconst int w = 5; 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;const int L = 5;vector<float> vDists;vDists.resize(2*L+1); // 11// 滑动窗口的滑动范围为（-L, L）,提前判断滑动窗口滑动过程中是否会越界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++){// 横向滑动窗口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); float dist = cv::norm(IL,IR,cv::NORM_L1); // 一范数，计算差的绝对值if(dist<bestDist){bestDist =  dist;// SAD匹配目前最小匹配偏差bestincR = incR; // SAD匹配目前最佳的修正量}vDists[L+incR] = dist; // 正常情况下，这里面的数据应该以抛物线形式变化}if(bestincR==-L || bestincR==L) // SAD匹配失败，同时放弃求该特征点的深度continue;// Sub-pixel match (Parabola fitting)// 做抛物线拟合找谷底得到亚像素匹配deltaR// (bestincR,dist) (bestincR-1,dist) (bestincR+1,dist)三个点拟合出抛物线// bestincR+deltaR就是抛物线谷底的位置，相对SAD匹配出的最小值bestincR的修正量为deltaRconst 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));// 抛物线拟合得到的修正量不能超过一个像素，否则放弃求该特征点的深度if(deltaR<-1 || deltaR>1)continue;// Re-scaled coordinate// 通过描述子匹配得到匹配点位置为scaleduR0// 通过SAD匹配找到修正量bestincR// 通过抛物线拟合找到亚像素修正量deltaRfloat bestuR = mvScaleFactors[kpL.octave]*((float)scaleduR0+(float)bestincR+deltaR);// 这里是disparity，根据它算出depthfloat disparity = (uL-bestuR);if(disparity>=minD && disparity<maxD) // 最后判断视差是否在范围内{if(disparity<=0){disparity=0.01;bestuR = uL-0.01;}// depth 是在这里计算的// depth=baseline*fx/disparitymvDepth[iL]=mbf/disparity;   // 深度mvuRight[iL] = bestuR;       // 匹配对在右图的横坐标vDistIdx.push_back(pair<int,int>(bestDist,iL)); // 该特征点SAD匹配最小匹配偏差}

特征点选取完毕

ComputeImageBounds()

/******************************************************************     函数属性：类Frame的成员函数ComputeImageBounds(const cv::Mat &imLeft)*     函数功能：*                 函数分为两部分，一部分是当图片需要矫正时：图像的边界为矫正后的图像边界*                 第二部分是函数不需要矫正时 图像的边界就是原图像的边界*                 此函数的最终结果为：将图形的边界赋值即mnMinX、mnMaxX、mnMinY、mnMaxY*     函数参数介绍：*                         imLeft：图像彩色图对应的灰度图*     备注：计算图像边界（在初始化时调用）******************************************************************/
void Frame::ComputeImageBounds(const cv::Mat &imLeft)
{if(mDistCoef.at<float>(0)!=0.0)  //如果图片需要失真矫正{// 矫正前四个边界点：(0,0) (cols,0) (0,rows) (cols,rows)cv::Mat mat(4,2,CV_32F);mat.at<float>(0,0)=0.0; mat.at<float>(0,1)=0.0;mat.at<float>(1,0)=imLeft.cols; mat.at<float>(1,1)=0.0;mat.at<float>(2,0)=0.0; mat.at<float>(2,1)=imLeft.rows;mat.at<float>(3,0)=imLeft.cols; mat.at<float>(3,1)=imLeft.rows;// Undistort corners  对rgb图进行失真矫正mat=mat.reshape(2);cv::undistortPoints(mat,mat,mK,mDistCoef,cv::Mat(),mK);   //将矫正后的图像放入mat中mat=mat.reshape(1);mnMinX = min(mat.at<float>(0,0),mat.at<float>(2,0));  //左上和左下横坐标最小的mnMaxX = max(mat.at<float>(1,0),mat.at<float>(3,0)); //右上和右下横坐标最大的mnMinY = min(mat.at<float>(0,1),mat.at<float>(1,1));  //左上和右上纵坐标最小的mnMaxY = max(mat.at<float>(2,1),mat.at<float>(3,1)); //左下和右下纵坐标最小的}else          //如果图片不需要失真矫正{mnMinX = 0.0f;mnMaxX = imLeft.cols;mnMinY = 0.0f;mnMaxY = imLeft.rows;}
}

AssignFeaturesToGrid()

/*****************************************************************
*      函数属性：类Frame的成员函数AssignFeaturesToGrid()*     函数功能：*                 将整张图片分为64×48的网格*                 并将每个特征点的id加入到该网格中，即mGrid容器存储的是特征点的id*     函数参数介绍：NULL*     备注：分配特征点到各个网格，加速特征匹配******************************************************************/
void Frame::AssignFeaturesToGrid()
{int nReserve = 0.5f*N/(FRAME_GRID_COLS*FRAME_GRID_ROWS);for(unsigned int i=0; i<FRAME_GRID_COLS;i++)for (unsigned int j=0; j<FRAME_GRID_ROWS;j++)mGrid[i][j].reserve(nReserve);   //给每个网格预留下空间，为什么要预留这些？//将特征点分配到这些网格中for(int i=0;i<N;i++){const cv::KeyPoint &kp = mvKeysUn[i];int nGridPosX, nGridPosY;  //存储网格位置，证明第(nGridPosX,nGridPosY)个网格if(PosInGrid(kp,nGridPosX,nGridPosY))    //如果第i个特征点位置在第(nGridPosX,nGridPosY)个网格中，就将该特征点的id存入该网格中mGrid[nGridPosX][nGridPosY].push_back(i);}
}

orb-slam系列 Tracking线程匹配(四)相关推荐

OpenCV系列之特征匹配 | 四十四
目标在本章中, 我们将看到如何将一个图像中的特征与其他图像进行匹配. 我们将在OpenCV中使用Brute-Force匹配器和FLANN匹配器 Brute-Force匹配器的基础蛮力匹配器很简单. ...
ORB_SLAM2代码阅读(2)——tracking线程
ORB_SLAM2代码阅读(2)--Tracking线程 1. 说明 2. 简介 2.1 Tracking 流程 2.2 Tracking 线程的二三四 2.2.1 Tracking 线程的二种模式 ...
【理解】ORB特征提取与ORBSLAM特征匹配简要剖析
目录 ORB特征提取优势: 经典FAST特征提取: 经典的BRIEF描述子: ORB特征提取的改进: 如何在FAST检测的基础上维持特征点的尺度不变性? 如何在FAST检测的基础上维持特征点的旋转不 ...
ORB-SLAM2代码阅读笔记（五）：Tracking线程3——Track函数中单目相机初始化
Table of Contents 1.特征点匹配相关理论简介 2.ORB-SLAM2中特征匹配代码分析 (1)Tracking线程中的状态机 (2)单目相机初始化函数MonocularInitial ...
SLAM系列——机器人顶刊T-RO！用于关联、建图和高级任务的物体级SLAM框架
系列文章目录 SLAM系列--第一讲预备知识[2023.1] SLAM系列--第二讲初识SLAM[2023.1] SLAM系列--第三讲三维空间刚体运动[2023.1] SLAM系列--第四讲 ...
ORB_SLAM2中Tracking线程
Tracking线程是ORB_SLAM2的主线程.在System.cc中,使用构造函数进行了初始化,开启了三个线程. 可执行程序->System构造函数(初始化三个线程)->处理输入的 ...
orbslam2代码详解之tracking线程——局部地图跟踪
目录局部地图跟踪 TrackLocalMap() UpdateLocalMap() UpdateLocalKeyFrames() UpdateLocalPoints() SearchLocalPoi ...
ORB_SLAM2中Tracking线程的三种追踪方式
1.参考关键帧追踪模式 bool Tracking::TrackReferenceKeyFrame() 对参考关键帧中的路标点进行跟踪.在Tracking线程中,每传入一帧,都会进行位姿优化. ...
Mybatis系列全解（四）：全网最全！Mybatis配置文件XML全貌详解
封面:洛小汐作者:潘潘做大事和做小事的难度是一样的.两者都会消耗你的时间和精力,所以如果决心做事,就要做大事,要确保你的梦想值得追求,未来的收获可以配得上你的努力. 前言上一篇文章 <My ...

orb-slam系列 Tracking线程匹配(四)

ComputeStereoMatches() 函数

ComputeImageBounds()

AssignFeaturesToGrid()

orb-slam系列 Tracking线程匹配(四)相关推荐

最新文章

热门文章

orb-slam系列 Tracking线程 匹配(四)

ComputeStereoMatches() 函数

ComputeImageBounds()

AssignFeaturesToGrid()

orb-slam系列 Tracking线程 匹配(四)相关推荐

最新文章

热门文章

orb-slam系列 Tracking线程匹配(四)

orb-slam系列 Tracking线程匹配(四)相关推荐