首先,自己一开始看的代码是从官网上下的,看得好多不太理解的地方,很头痛,后发现了泡泡机器人的吴博和谢晓佳大神做的对ORB-SLAM2代码中文注释,写的很详细,超级感谢.地址:https://gitee.com/paopaoslam/ORB-SLAM2
从码云中拷贝下来后,在clion中,无法实现跳转,因为注释部分大都在src文件夹中,我将带有注释的src文件夹替换了从官网下载的src文件夹,并重命名了所有.cpp为.cc.实现了代码的跳转.
然后是自己对代码的一个整理吧,希望自己可以get到里面更多的点.
首先系统模块,主要是界面的显示,选择以及线程的初始化和关联
从这个函数开始
cv::Mat System::TrackStereo(const cv::Mat &imLeft, const cv::Mat &imRight, const double ×tamp)
{
if(mSensor!=STEREO)
{
cerr << "ERROR: you called TrackStereo but input sensor was not set to STEREO." << endl;
exit(-1);
}
// Check mode change
{
unique_lock<mutex> lock(mMutexMode);
if(mbActivateLocalizationMode)
{
mpLocalMapper->RequestStop(); ///如果激活了定位模块,就让局部建图线程停止.???
// Wait until Local Mapping has effectively stopped
while(!mpLocalMapper->isStopped())
{
//usleep(1000);
std::this_thread::sleep_for(std::chrono::milliseconds(1));
}
mpTracker->InformOnlyTracking(true);// 定位时,只跟踪
mbActivateLocalizationMode = false;
}
if(mbDeactivateLocalizationMode)
{
mpTracker->InformOnlyTracking(false);
mpLocalMapper->Release();
mbDeactivateLocalizationMode = false;
}
}
// Check reset
{
unique_lock<mutex> lock(mMutexReset);
if(mbReset)
{
mpTracker->Reset();
mbReset = false;
}
}
return mpTracker->GrabImageStereo(imLeft,imRight,timestamp);//跟踪线程的接口
}
从这个函数的GrabImageStereo进入到Tracking类中.
跳进去
// 输入左右目图像,可以为RGB、BGR、RGBA、GRAY
// 1、将图像转为mImGray和imGrayRight并初始化mCurrentFrame
// 2、进行tracking过程
// 输出世界坐标系到该帧相机坐标系的变换矩阵
cv::Mat Tracking::GrabImageStereo(const cv::Mat &imRectLeft, const cv::Mat &imRectRight, const double ×tamp)
{
mImGray = imRectLeft;
cv::Mat imGrayRight = imRectRight;
// 步骤1:将RGB或RGBA图像转为灰度图像
if(mImGray.channels()==3)
{
if(mbRGB)
{
cvtColor(mImGray,mImGray,CV_RGB2GRAY);
cvtColor(imGrayRight,imGrayRight,CV_RGB2GRAY);
}
else
{
cvtColor(mImGray,mImGray,CV_BGR2GRAY);
cvtColor(imGrayRight,imGrayRight,CV_BGR2GRAY);
}
}
else if(mImGray.channels()==4)
{
if(mbRGB)
{
cvtColor(mImGray,mImGray,CV_RGBA2GRAY);
cvtColor(imGrayRight,imGrayRight,CV_RGBA2GRAY);
}
else
{
cvtColor(mImGray,mImGray,CV_BGRA2GRAY);
cvtColor(imGrayRight,imGrayRight,CV_BGRA2GRAY);
}
}
// 步骤2:构造Frame
//构建成当前帧 并对当前帧进行ID标号,对图像提取ORB特征点,做左右图像的特征点匹配,计算视差,深度,得到mappoints等
mCurrentFrame = Frame(mImGray,imGrayRight,timestamp,mpORBextractorLeft,mpORBextractorRight,mpORBVocabulary,mK,mDistCoef,mbf,mThDepth);
// 步骤3:跟踪
Track();
return mCurrentFrame.mTcw.clone();
}
步骤2中 跳到Frame类
// 双目的初始化
Frame::Frame(const cv::Mat &imLeft, const cv::Mat &imRight, const double &timeStamp, ORBextractor* extractorLeft, ORBextractor* extractorRight, ORBVocabulary* voc, cv::Mat &K, cv::Mat &distCoef, const float &bf, const float &thDepth)
:mpORBvocabulary(voc),mpORBextractorLeft(extractorLeft),mpORBextractorRight(extractorRight), mTimeStamp(timeStamp), mK(K.clone()),mDistCoef(distCoef.clone()), mbf(bf), mb(0), mThDepth(thDepth),
mpReferenceKF(static_cast<KeyFrame*>(NULL))
{
// Frame ID
mnId=nNextId++;
// Scale Level Info
mnScaleLevels = mpORBextractorLeft->GetLevels();
mfScaleFactor = mpORBextractorLeft->GetScaleFactor();
mfLogScaleFactor = log(mfScaleFactor);
mvScaleFactors = mpORBextractorLeft->GetScaleFactors(); //在双目特征点匹配ComputeStereoMatches函数中,在根据金字塔层数计算搜做半径时有用到
mvInvScaleFactors = mpORBextractorLeft->GetInverseScaleFactors(); //这个尺度在双目特征点匹配函数中,在将最佳匹配点对应到层数时有用到
mvLevelSigma2 = mpORBextractorLeft->GetScaleSigmaSquares();
mvInvLevelSigma2 = mpORBextractorLeft->GetInverseScaleSigmaSquares();
// ORB extraction
// 同时对左右目提特征
//提取特征加入双线程同步提取,0,1代表左目和右目
//两张提取的特征点会放在不同的vector中
//对单目和RGBD来说,右目不用,以左为准
thread threadLeft(&Frame::ExtractORB,this,0,imLeft);
thread threadRight(&Frame::ExtractORB,this,1,imRight);
//阻止其他调用函数线程
threadLeft.join();
threadRight.join();
//N为特征点的数量,这里是一个装有特征点keys的vector容器
N = mvKeys.size();
if(mvKeys.empty())
return;
// Undistort特征点,这里没有对双目进行校正,因为要求输入的图像已经进行极线校正
UndistortKeyPoints();
// 计算双目间的匹配, 匹配成功的特征点会计算其深度
// 深度存放在mvuRight 和 mvDepth 中
ComputeStereoMatches();
// 对应的mappoints
mvpMapPoints = vector<MapPoint*>(N,static_cast<MapPoint*>(NULL));
mvbOutlier = vector<bool>(N,false);
// This is done only for the first Frame (or after a change in the calibration)
if(mbInitialComputations)
{
ComputeImageBounds(imLeft);
mfGridElementWidthInv=static_cast<float>(FRAME_GRID_COLS)/(mnMaxX-mnMinX);
mfGridElementHeightInv=static_cast<float>(FRAME_GRID_ROWS)/(mnMaxY-mnMinY);
fx = K.at<float>(0,0);
fy = K.at<float>(1,1);
cx = K.at<float>(0,2);
cy = K.at<float>(1,2);
invfx = 1.0f/fx;
invfy = 1.0f/fy;
mbInitialComputations=false;
}
mb = mbf/fx;
AssignFeaturesToGrid();
}
1 对帧的ID标号
2 尺度信息(具体的不太明确,只知道注释的两个在左右目特征点匹配中有用到)
3 双线程同步提取左右目的ORB特征
4 UndistortKeyPoints()主要是调用OpenCV的矫正函数矫正orb提取的特征点
5 计算双目间的匹配, 匹配成功的特征点会计算其深度,深度存放在mvuRight 和 mvDepth 中
这里多5进行展开,跳进去
/**
* @brief 双目匹配
*
* 为左图的每一个特征点在右图中找到匹配点 \n
* 根据基线(有冗余范围)上描述子距离找到匹配, 再进行SAD精确定位 \n
* 最后对所有SAD的值进行排序, 剔除SAD值较大的匹配对,然后利用抛物线拟合得到亚像素精度的匹配 \n
* 匹配成功后会更新 mvuRight(ur) 和 mvDepth(Z)
*/
void Frame::ComputeStereoMatches()
{
mvuRight = vector<float>(N,-1.0f);
mvDepth = vector<float>(N,-1.0f);
//这里在matcher中设定的阈值,TH_HIGH = 100, TH_LOW = 50,这个阈值怎么来的,具体还不是很清楚
const int thOrbDist = (ORBmatcher::TH_HIGH+ORBmatcher::TH_LOW)/2;
//获得原始图像的行数
const int nRows = mpORBextractorLeft->mvImagePyramid[0].rows;
//Assign keypoints to row table
// 步骤1:建立特征点搜索范围对应表,一个特征点在一个带状区域内搜索匹配特征点
// 匹配搜索的时候,不仅仅是在一条横线上搜索,而是在一条横向搜索带上搜索,简而言之,原本每个特征点的纵坐标为1,这里把特征点体积放大,纵坐标占好几行
// 例如左目图像某个特征点的纵坐标为20,那么在右侧图像上搜索时是在纵坐标为18到22这条带上搜索,搜索带宽度为正负2,搜索带的宽度和特征点所在金字塔层数有关
// 简单来说,如果纵坐标是20,特征点在图像第20行,那么认为18 19 20 21 22行都有这个特征点
// vRowIndices[18]、vRowIndices[19]、vRowIndices[20]、vRowIndices[21]、vRowIndices[22]都有这个特征点编号
vector<vector<size_t> > vRowIndices(nRows,vector<size_t>());
for(int i=0; i<nRows; i++)
vRowIndices[i].reserve(200);//每行最多装200个特征点
const int Nr = mvKeysRight.size();// 右图特征点的数量
for(int iR=0; iR<Nr; iR++)
{
// !!在这个函数中没有对双目进行校正,双目校正是在外层程序中实现的
const cv::KeyPoint &kp = mvKeysRight[iR];
const float &kpY = kp.pt.y; //pt为coordinates of the keypoints
// 计算匹配搜索的纵向宽度,尺度越大(层数越高,距离越近),搜索范围越大
// 如果特征点在金字塔第一层,则搜索范围为:正负2
// 尺度越大其位置不确定性越高,所以其搜索半径越大
const float r = 2.0f*mvScaleFactors[mvKeysRight[iR].octave];//根据金字塔层数得到搜索半径
const int maxr = ceil(kpY+r);
const int minr = floor(kpY-r);
for(int yi=minr;yi<=maxr;yi++)
vRowIndices[yi].push_back(iR);
}
// Set limits for search
// p
// /\ |
// / \ |为Z
// / \ |
// -------- 为b
const float minZ = mb; // NOTE bug mb没有初始化,mb的赋值在构造函数中放在ComputeStereoMatches函数的后面
const float minD = 0; // 最小视差, 设置为0即可
const float maxD = mbf/minZ; // 最大视差, 对应最小深度 mbf/minZ = mbf/mb = mbf/(mbf/fx) = fx (wubo???)
// For each left keypoint search a match in the right image
vector<pair<int, int> > vDistIdx;
vDistIdx.reserve(N);
// 步骤2:对左目相机每个特征点,通过描述子在右目带状搜索区域找到匹配点, 再通过SAD做亚像素匹配
// 注意:这里是校正前的mvKeys,而不是校正后的mvKeysUn
// KeyFrame::UnprojectStereo和Frame::UnprojectStereo函数中不一致
// 这里是不是应该对校正后特征点求深度呢?(wubo???)
for(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;
// 可能的匹配点
const vector<size_t> &vCandidates = vRowIndices[vL];
if(vCandidates.empty())
continue;
const float minU = uL-maxD; // 最小匹配范围
const float maxU = uL-minD; // 最大匹配范围
if(maxU<0)
continue;
//描述子距离阈值
int bestDist = ORBmatcher::TH_HIGH;
size_t bestIdxR = 0;
// 每个特征点描述子占一行,建立一个指针指向iL特征点对应的描述子
const cv::Mat &dL = mDescriptors.row(iL);
// Compare descriptor to right keypoints
// 步骤2.1:遍历右目所有可能的匹配点,找出最佳匹配点(描述子距离最小)
for(size_t iC=0; iC<vCandidates.size(); iC++)
{
const size_t iR = vCandidates[iC];
const cv::KeyPoint &kpR = mvKeysRight[iR];
// 仅对近邻尺度的特征点进行匹配
if(kpR.