ORB-SLAM2代码整理--tracking线程
首先,自己一开始看的代码是从官网上下的,看得好多不太理解的地方,很头痛,后发现了泡泡机器人的吴博和谢晓佳大神做的对ORB-SLAM2代码中文注释,写的很详细,超级感谢.地址:https://gitee.com/paopaoslam/ORB-SLAM2 从码云中拷贝下来后,在clion中,无法实现跳转,因为注释部分大都在src文件夹中,我将带有注释的src文件夹替换了从官网下载的src文..
首先,自己一开始看的代码是从官网上下的,看得好多不太理解的地方,很头痛,后发现了泡泡机器人的吴博和谢晓佳大神做的对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.octave<levelL-1 || kpR.octave>levelL+1)
continue;
const float &uR = kpR.pt.x;
if(uR>=minU && uR<=maxU)
{
const cv::Mat &dR = mDescriptorsRight.row(iR);
const int dist = ORBmatcher::DescriptorDistance(dL,dR);
if(dist<bestDist)
{
bestDist = dist;//得到最小距离
bestIdxR = iR;//得到最好的匹配点
}
}
}
// 最好的匹配的匹配误差存在bestDist，匹配点位置存在bestIdxR中
// Subpixel match by correlation
// 步骤2.2：通过SAD匹配提高像素匹配修正量bestincR
if(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 search
const int w = 5; // 滑动窗口的大小11*11 注意该窗口取自resize后的图像
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; //这个地方是否应该是scaleduR0-L-w (wubo???)
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最小值不是以抛物线形式出现，SAD匹配失败，同时放弃求该特征点的深度
continue;
// Sub-pixel match (Parabola fitting)
// 步骤2.3：做抛物线拟合找谷底得到亚像素匹配deltaR
// (bestincR,dist) (bestincR-1,dist) (bestincR+1,dist)三个点拟合出抛物线
// bestincR+deltaR就是抛物线谷底的位置，相对SAD匹配出的最小值bestincR的修正量为deltaR
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));
// 抛物线拟合得到的修正量不能超过一个像素，否则放弃求该特征点的深度
if(deltaR<-1 || deltaR>1)
continue;
// Re-scaled coordinate
// 通过描述子匹配得到匹配点位置为scaleduR0
// 通过SAD匹配找到修正量bestincR
// 通过抛物线拟合找到亚像素修正量deltaR
float bestuR = mvScaleFactors[kpL.octave]*((float)scaleduR0+(float)bestincR+deltaR);
// 这里是disparity，根据它算出depth
float disparity = (uL-bestuR);
if(disparity>=minD && disparity<maxD) // 最后判断视差是否在范围内
{
if(disparity<=0)
{
disparity=0.01;
bestuR = uL-0.01;
}
// depth 是在这里计算的
// depth=baseline*fx/disparity
mvDepth[iL]=mbf/disparity; // 深度
mvuRight[iL] = bestuR; // 匹配对在右图的横坐标
vDistIdx.push_back(pair<int,int>(bestDist,iL)); // 该特征点SAD匹配最小匹配偏差
}
}
}
// 步骤3：剔除SAD匹配偏差较大的匹配特征点
// 前面SAD匹配只判断滑动窗口中是否有局部最小值，这里通过对比剔除SAD匹配偏差比较大的特征点的深度
sort(vDistIdx.begin(),vDistIdx.end()); // 根据所有匹配对的SAD偏差进行排序, 距离由小到大
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
{
mvuRight[vDistIdx[i].second]=-1;
mvDepth[vDistIdx[i].second]=-1;
}
}
}
5.1 主要是对右目图像进行带状的搜索匹配,需要计算一个根据金字塔层数的搜索半径r = 2.0f*mvScaleFactors[mvKeysRight[iR].octave] 就得到了纵坐标下的搜索范围
5.2 利用描述子在右目中找到每个特征点的最佳匹配点
5.3 通过SAD滑窗得到匹配修正量bestincR
5.4 (bestincR,dist) (bestincR-1,dist) (bestincR+1,dist)三个点拟合出抛物线得到亚像素修正量deltaR
5.5 最终得到匹配点的位置bestuR
5.6 得到视差d
5.7 在高博14讲P91页,取图,可得到深度Z,记录在mvDepth[iL]
5.8 剔除SAD匹配偏差较大的匹配特征点(通过自适应的阈值距离)
回到Frame类
6 对应的mappoints(由计算得到的深度得到mappoints?)
7 this is done only for the first Frame (or after a change in the calibration)计算图像的边界
8 得到视差
9 AssignFeaturesToGrid()将关键点分布到64*48分割而成的网格中,为了加速匹配和均匀化关键点分布
再回到Tracking类GrabImageStereo函数中,下面就是进入Track函数线程了.
由图也可知把Frame传给Tracking线程处理
void Tracking::Track()
{
// track包含两部分：估计运动、跟踪局部地图
// mState为tracking的状态机
// SYSTME_NOT_READY, NO_IMAGE_YET, NOT_INITIALIZED, OK, LOST
// 如果图像复位过、或者第一次运行，则为NO_IMAGE_YET状态
if(mState==NO_IMAGES_YET)
{
mState = NOT_INITIALIZED;
}
// mLastProcessedState存储了Tracking最新的状态，用于FrameDrawer中的绘制
mLastProcessedState=mState;
// Get Map Mutex -> Map cannot be changed
unique_lock<mutex> lock(mpMap->mMutexMapUpdate);
// 步骤1：初始化
if(mState==NOT_INITIALIZED)
{
if(mSensor==System::STEREO || mSensor==System::RGBD)
StereoInitialization();
else
MonocularInitialization();
mpFrameDrawer->Update(this);
if(mState!=OK)
return;
}
else// 步骤2：跟踪
{
// System is initialized. Track Frame.
// bOK为临时变量，用于表示每个函数是否执行成功
bool bOK;
// Initial camera pose estimation using motion model or relocalization (if tracking is lost)
// 在viewer中有个开关menuLocalizationMode，有它控制是否ActivateLocalizationMode，并最终管控mbOnlyTracking
// mbOnlyTracking等于false表示正常VO模式（有地图更新），mbOnlyTracking等于true表示用户手动选择定位模式
if(!mbOnlyTracking)
{
// Local Mapping is activated. This is the normal behaviour, unless
// you explicitly activate the "only tracking" mode.
// 正常初始化成功
if(mState==OK)
{
// Local Mapping might have changed some MapPoints tracked in last frame
// 检查并更新上一帧被替换的MapPoints
// 更新Fuse函数和SearchAndFuse函数替换的MapPoints
CheckReplacedInLastFrame();
// 步骤2.1：跟踪上一帧或者参考帧或者重定位
// 运动模型是空的或刚完成重定位
// mCurrentFrame.mnId<mnLastRelocFrameId+2这个判断不应该有
// 应该只要mVelocity不为空，就优先选择TrackWithMotionModel
// mnLastRelocFrameId上一次重定位的那一帧
if(mVelocity.empty() || mCurrentFrame.mnId<mnLastRelocFrameId+2) //速度为空
{
// 将上一帧的位姿作为当前帧的初始位姿
// 通过BoW的方式在参考帧中找当前帧特征点的匹配点
// 优化每个特征点都对应3D点重投影误差即可得到位姿
bOK = TrackReferenceKeyFrame();
}
else
{
// 根据恒速模型设定当前帧的初始位姿
// 通过投影的方式在参考帧中找当前帧特征点的匹配点
// 优化每个特征点所对应3D点的投影误差即可得到位姿
bOK = TrackWithMotionModel();
if(!bOK)
// TrackReferenceKeyFrame是跟踪参考帧，不能根据固定运动速度模型预测当前帧的位姿态，通过bow加速匹配（SearchByBow）
// 最后通过优化得到优化后的位姿
bOK = TrackReferenceKeyFrame();
}
}
else
{
// BOW搜索，PnP求解位姿
bOK = Relocalization();
}
}
// 将最新的关键帧作为reference frame
mCurrentFrame.mpReferenceKF = mpReferenceKF;
// If we have an initial estimation of the camera pose and matching. Track the local map.
// 步骤2.2：在帧间匹配得到初始的姿态后，现在对local map进行跟踪得到更多的匹配，并优化当前位姿
// local map:当前帧、当前帧的MapPoints、当前关键帧与其它关键帧共视关系
// 在步骤2.1中主要是两两跟踪（恒速模型跟踪上一帧、跟踪参考帧），这里搜索局部关键帧后搜集所有局部MapPoints，
// 然后将局部MapPoints和当前帧进行投影匹配，得到更多匹配的MapPoints后进行Pose优化
if(!mbOnlyTracking)
{
if(bOK)
bOK = TrackLocalMap();
}
else
{
// mbVO true means that there are few matches to MapPoints in the map. We cannot retrieve
// a local map and therefore we do not perform TrackLocalMap(). Once the system relocalizes
// the camera we will use the local map again.
// 重定位成功
if(bOK && !mbVO)
bOK = TrackLocalMap();
}
if(bOK)
mState = OK;
else
mState=LOST;
// Update drawer
mpFrameDrawer->Update(this);
// If tracking were good, check if we insert a keyframe
if(bOK)
{
// Update motion model
if(!mLastFrame.mTcw.empty())
{
// 步骤2.3：更新恒速运动模型TrackWithMotionModel中的mVelocity
cv::Mat LastTwc = cv::Mat::eye(4,4,CV_32F);
mLastFrame.GetRotationInverse().copyTo(LastTwc.rowRange(0,3).colRange(0,3));
mLastFrame.GetCameraCenter().copyTo(LastTwc.rowRange(0,3).col(3));
mVelocity = mCurrentFrame.mTcw*LastTwc; // Tcl
}
else
mVelocity = cv::Mat();
mpMapDrawer->SetCurrentCameraPose(mCurrentFrame.mTcw);
// Clean VO matches
// 步骤2.4：清除UpdateLastFrame中为当前帧临时添加的MapPoints
for(int i=0; i<mCurrentFrame.N; i++)
{
MapPoint* pMP = mCurrentFrame.mvpMapPoints[i];
if(pMP)
// 排除UpdateLastFrame函数中为了跟踪增加的MapPoints
if(pMP->Observations()<1)
{
mCurrentFrame.mvbOutlier[i] = false;
mCurrentFrame.mvpMapPoints[i]=static_cast<MapPoint*>(NULL);
}
}
// Delete temporal MapPoints
// 步骤2.5：清除临时的MapPoints，这些MapPoints在TrackWithMotionModel的UpdateLastFrame函数里生成（仅双目和rgbd）
// 步骤2.4中只是在当前帧中将这些MapPoints剔除，这里从MapPoints数据库中删除
// 这里生成的仅仅是为了提高双目或rgbd摄像头的帧间跟踪效果，用完以后就扔了，没有添加到地图中
for(list<MapPoint*>::iterator lit = mlpTemporalPoints.begin(), lend = mlpTemporalPoints.end(); lit!=lend; lit++)
{
MapPoint* pMP = *lit;
delete pMP;
}
// 这里不仅仅是清除mlpTemporalPoints，通过delete pMP还删除了指针指向的MapPoint
mlpTemporalPoints.clear();
// Check if we need to insert a new keyframe
// 步骤2.6：检测并插入关键帧，对于双目会产生新的MapPoints
if(NeedNewKeyFrame())
CreateNewKeyFrame();
// We allow points with high innovation (considererd outliers by the Huber Function)
// pass to the new keyframe, so that bundle adjustment will finally decide
// if they are outliers or not. We don't want next frame to estimate its position
// with those points so we discard them in the frame.
// 删除那些在bundle adjustment中检测为outlier的3D map点
for(int i=0; i<mCurrentFrame.N;i++)
{
if(mCurrentFrame.mvpMapPoints[i] && mCurrentFrame.mvbOutlier[i])
mCurrentFrame.mvpMapPoints[i]=static_cast<MapPoint*>(NULL);
}
}
// Reset if the camera get lost soon after initialization
// 跟踪失败，并且relocation也没有搞定，只能重新Reset
if(mState==LOST)
{
if(mpMap->KeyFramesInMap()<=5)
{
cout << "Track lost soon after initialisation, reseting..." << endl;
mpSystem->Reset();
return;
}
}
if(!mCurrentFrame.mpReferenceKF)
mCurrentFrame.mpReferenceKF = mpReferenceKF;
// 保存上一帧的数据
mLastFrame = Frame(mCurrentFrame);
}
// Store frame pose information to retrieve the complete camera trajectory afterwards.
// 步骤3：记录位姿信息，用于轨迹复现
if(!mCurrentFrame.mTcw.empty())
{
// 计算相对姿态T_currentFrame_referenceKeyFrame
cv::Mat Tcr = mCurrentFrame.mTcw*mCurrentFrame.mpReferenceKF->GetPoseInverse();
mlRelativeFramePoses.push_back(Tcr);
mlpReferences.push_back(mpReferenceKF);
mlFrameTimes.push_back(mCurrentFrame.mTimeStamp);
mlbLost.push_back(mState==LOST);
}
else
{
// This can happen if tracking is lost
// 如果跟踪失败，则相对位姿使用上一次值
mlRelativeFramePoses.push_back(mlRelativeFramePoses.back());
mlpReferences.push_back(mlpReferences.back());
mlFrameTimes.push_back(mlFrameTimes.back());
mlbLost.push_back(mState==LOST);
}
}
1 首先进行初始化,由于前面从双目进行过来的,这里先看双目的.
/**
* @brief 双目和rgbd的地图初始化
*
* 由于具有深度信息，直接生成MapPoints
*/
void Tracking::StereoInitialization()
{
if(mCurrentFrame.N>500)
{
// Set Frame pose to the origin
// 步骤1：设定初始位姿
mCurrentFrame.SetPose(cv::Mat::eye(4,4,CV_32F));
// Create KeyFrame
// 步骤2：将当前帧构造为初始关键帧
// mCurrentFrame的数据类型为Frame
// KeyFrame包含Frame、地图3D点、以及BoW
// KeyFrame里有一个mpMap，Tracking里有一个mpMap，而KeyFrame里的mpMap都指向Tracking里的这个mpMap
// KeyFrame里有一个mpKeyFrameDB，Tracking里有一个mpKeyFrameDB，而KeyFrame里的mpMap都指向Tracking里的这个mpKeyFrameDB
KeyFrame* pKFini = new KeyFrame(mCurrentFrame,mpMap,mpKeyFrameDB);
// Insert KeyFrame in the map
// KeyFrame中包含了地图、反过来地图中也包含了KeyFrame，相互包含
// 步骤3：在地图中添加该初始关键帧
mpMap->AddKeyFrame(pKFini);
// Create MapPoints and asscoiate to KeyFrame
// 步骤4：为每个特征点构造MapPoint
for(int i=0; i<mCurrentFrame.N;i++)
{
float z = mCurrentFrame.mvDepth[i];
if(z>0)
{
// 步骤4.1：通过反投影得到该特征点的3D坐标
cv::Mat x3D = mCurrentFrame.UnprojectStereo(i);
// 步骤4.2：将3D点构造为MapPoint
MapPoint* pNewMP = new MapPoint(x3D,pKFini,mpMap);
// 步骤4.3：为该MapPoint添加属性：
// a.观测到该MapPoint的关键帧
// b.该MapPoint的描述子
// c.该MapPoint的平均观测方向和深度范围
// a.表示该MapPoint可以被哪个KeyFrame的哪个特征点观测到
pNewMP->AddObservation(pKFini,i);
// b.从众多观测到该MapPoint的特征点中挑选区分读最高的描述子
pNewMP->ComputeDistinctiveDescriptors();
// c.更新该MapPoint平均观测方向以及观测距离的范围
pNewMP->UpdateNormalAndDepth();
// 步骤4.4：在地图中添加该MapPoint
mpMap->AddMapPoint(pNewMP);
// 步骤4.5：表示该KeyFrame的哪个特征点可以观测到哪个3D点
pKFini->AddMapPoint(pNewMP,i);
// 步骤4.6：将该MapPoint添加到当前帧的mvpMapPoints中
// 为当前Frame的特征点与MapPoint之间建立索引
mCurrentFrame.mvpMapPoints[i]=pNewMP;
}
}
cout << "New map created with " << mpMap->MapPointsInMap() << " points" << endl;
// 步骤4：在局部地图中添加该初始关键帧
mpLocalMapper->InsertKeyFrame(pKFini);
mLastFrame = Frame(mCurrentFrame);
mnLastKeyFrameId=mCurrentFrame.mnId;
mpLastKeyFrame = pKFini;
mvpLocalKeyFrames.push_back(pKFini);
mvpLocalMapPoints=mpMap->GetAllMapPoints();
mpReferenceKF = pKFini;
mCurrentFrame.mpReferenceKF = pKFini;
// 把当前（最新的）局部MapPoints作为ReferenceMapPoints
// ReferenceMapPoints是DrawMapPoints函数画图的时候用的
mpMap->SetReferenceMapPoints(mvpLocalMapPoints);
mpMap->mvpKeyFrameOrigins.push_back(pKFini);
mpMapDrawer->SetCurrentCameraPose(mCurrentFrame.mTcw);
mState=OK;
}
}
1.1 设定初始位姿
1.1.1 clone Tcw
1.1.2 跟据Tcw计算mRcw、mtcw和mRwc、mOw
void Frame::UpdatePoseMatrices()
{
// [x_camera 1] = [R|t]*[x_world 1]，坐标为齐次形式
// x_camera = R*x_world + t
mRcw = mTcw.rowRange(0,3).colRange(0,3);
mRwc = mRcw.t();
mtcw = mTcw.rowRange(0,3).col(3);
// mtcw,这个不就是平移量吗???
即相机坐标系下相机坐标系到世界坐标系间的向量, 向量方向由相机坐标系指向世界坐标系
// mOw, 即世界坐标系下世界坐标系到相机坐标系间的向量, 向量方向由世界坐标系指向相机坐标系
mOw = -mRcw.t()*mtcw;
}
1.2 将当前帧构造为初始关键帧(我感觉这里应该是一个类的继承,c++不太好...需要补一下) 点进去,主要涉及了网格的分布及位姿的整理,其中的Ow是Twc的平移部分,tcw是Tcw的平移部分
1.3 将初始关键帧放进map中
1.4 为每个特征点构造mappoint. 话说前面Frame类中有一个
mvpMapPoints = vector<MapPoint*>(N,static_cast<MapPoint*>(NULL));
//其中有N个空指针，因此这里的MapPoint并没有指向实际的地图点(虽然有对应的idx，但是是要进行判断的)
1.4.1 通过反投影得到该特征点的3D坐标,点进去可分析出得到的是世界坐标系下的3D坐标
1.4.2 将3D点构造成Mappoint
1.4.3 给Mappoint添加属性
a.表示该MapPoint可以被哪个KeyFrame的哪个特征点观测到
b.从众多观测到该MapPoint的特征点中挑选区分读最高的描述子
c.更新该MapPoint平均观测方向以及观测距离的范围
1.4.4 在地图中添加该Mappoint
1.4.5 该KeyFrame的哪个特征点可以观测到哪个3D点
1.4.6 将该MapPoint添加到当前帧的mvpMapPoints中(这里就和前面的Frame中构造的mvpMapPoints联系上了,也为当前Frame的特征点与MapPoint之间建立索引
1.5 在局部地图中添加该初始关键帧
1.6 将lastframe,lastKeyframe,参考关键帧,mvpLocalMapPoints等赋值
把当前（最新的）局部MapPoints作为ReferenceMapPoints ,ReferenceMapPoints是DrawMapPoints函数画图的时候用
2 传入的Frame时系统如果已经初始化了,进入跟踪,这里只分析局部地图跟跟踪部分都运行
3 CheckReplacedInLastFrame() 这个函数
Local Mapping线程可能会将关键帧中某些MapPoints进行替换，由于tracking中需要用到mLastFrame，这里检查并更新上一帧中被替换的MapPoints
可以放到后面
4 运动模型为空或刚完成重定位,进入跟踪参考帧模式
TrackReferenceKeyFrame();
5 否则利用运动模型跟踪,而运动模型又没成功的情况下用跟踪参考帧模式
TrackWithMotionModel();
6 如果当前帧与最近邻关键帧的匹配也失败了，那么意味着需要重新定位才能继续跟踪。
Relocalization();
456分别跳一下...
先看4
将上一帧的位姿作为当前帧的初始位姿
计算当前帧的词包，将当前帧的特征点分到特定层的nodes上
对属于同一node的描述子通过BoW的方式在参考帧找当前帧特征点的匹配点
优化每个特征点都对应3D点重投影误差即可得到位姿
4.1 将当前帧的描述子转化为BoW向量
4.2 创建 ORB特征点匹配器 最小距离 < 0.7*次小距离 匹配成功
4.3 通过特征点的BoW加快当前帧与参考帧之间的特征点匹配
对SearchByBoW跳一下
/**
* @brief 通过词包，对关键帧的特征点进行跟踪
* 通过bow对pKF和F中的特征点进行快速匹配（不属于同一node的特征点直接跳过匹配） \n
* 对属于同一node的特征点通过描述子距离进行匹配 \n
* 根据匹配，用pKF中特征点对应的MapPoint更新F中特征点对应的MapPoints \n
* 每个特征点都对应一个MapPoint，因此pKF中每个特征点的MapPoint也就是F中对应点的MapPoint \n
* 通过距离阈值、比例阈值和角度投票进行剔除误匹配
* @param pKF KeyFrame
* @param F Current Frame
* @param vpMapPointMatches F中MapPoints对应的匹配，NULL表示未匹配
* @return 成功匹配的数量
*/
4.3.1 分别取出属于同一node的当前帧和上一关键帧的ORB特征点
4.3.2 遍历KF中属于该node的特征点
4.3.3 取出KF中该特征对应的MapPoint 赋值为pMP
4.3.4 取出KF中该特征点对应的描述子
4.3.5 遍历当前帧属于该node的特征点
4.3.6 取出当前帧该特征对应的描述子
4.3.7 求KF与当前帧的描述子的距离dist
4.3.8 判断dist < bestDist1 < bestDist2，更新bestDist1 bestDist2. 或bestDist1 < dist < bestDist2，更新bestDist2
4.3.9 最佳匹配比次佳匹配明显要好，那么最佳匹配才真正靠谱
4.3.10 更新特征点的MapPoint
4.3.11 如果需要根据方向一致性剔除不好的匹配点：(所有的特征点的角度变化应该是一致的，通过直方图统计得到最准确的角度变化值)
计算方向差直方图中三个计数最多的方向差，
根据数量差异选择保留第一 /第一+第二 /第一+第二+第三方向差对应的匹配点对。
4.3.12 统计成功匹配数量
跳出
4.4 将上一帧的位姿态作为当前帧位姿的初始值(可以加速优化)
4.5 通过优化3D-2D的重投影误差来获得位姿.只优化了位姿,没有优化mappoint(这里就不跳进去,就是g2o的优化+一些其他细节)
4.6 剔除优化后的outlier匹配点（MapPoints）
4.7 根据内点数量判断跟踪参考帧模式是否成功
4 跳出
跳进 5 运动模型
根据匀速度模型对上一帧的MapPoints进行跟踪
非单目情况，需要对上一帧产生一些新的MapPoints（临时）
将上一帧的MapPoints投影到当前帧的图像平面上，在投影的位置进行区域匹配
通过投影的方式在参考帧中找当前帧特征点的匹配点
根据匹配对估计当前帧的姿态
根据姿态剔除误匹配
5.1 创建 ORB特征点匹配器 最小距离 < 0.9*次小距离 匹配成功
5.2 UpdateLastFrame
对于双目或rgbd摄像头，根据深度值为上一关键帧生成新的MapPoints
（跟踪过程中需要将当前帧与上一帧进行特征点匹配，将上一帧的MapPoints投影到当前帧可以缩小匹配范围）
在跟踪过程中，去除outlier的MapPoint，如果不及时增加MapPoint会逐渐减少
这个函数的功能就是补充增加RGBD和双目相机上一帧的MapPoints数
5.2.1 更新最近一帧的位姿
5.2.2 得到最近一帧的位姿(世界坐标系下)
5.2.3 后面单目情况下不执行, 如果最近一帧为关键帧也不执行
对于双目或rgbd摄像头，为上一帧临时生成新的MapPoints
注意这些MapPoints不加入到Map中，在tracking的最后会删除
跟踪过程中需要将将上一帧的MapPoints投影到当前帧可以缩小匹配范围，加快当前帧与上一帧进行特征点匹配
5.2.4 得到上一帧有深度的特征点
5.2.5 按照深度大小排序
5.2.6 将距离比较近的点包装成MapPoints
5.2.7 mLastFrame.mvpMapPoints[i]=pNewMP; // 添加新的MapPoint
5.2.8 标记为临时添加的MapPoint，之后在CreateNewKeyFrame之前会全部删除.因为这些mappoints没有通过
a.AddMapPoint、
b.AddObservation、
c.ComputeDistinctiveDescriptors、
d.UpdateNormalAndDepth添加属性，
这些MapPoint仅仅为了提高双目和RGBD的跟踪成功率
5.3 根据Const Velocity Model(认为这两帧之间的相对运动和之前两帧间相对运动相同)估计当前帧的位姿
5.4 fil函数（x1,y1,选项1，x2,y2,选项2，······）按向量元素的下标渐增次序依次用直线段连接x,y对应元素定义的数据点
5.5 SearchByProjection
上一帧中包含了MapPoints，对这些MapPoints进行tracking，由此增加当前帧的MapPoints \n
* 1. 将上一帧的MapPoints投影到当前帧(根据速度模型可以估计当前帧的Tcw)
* 2. 在投影点附近根据描述子距离选取匹配，以及最终的方向投票机制进行剔除
5.5.1 Rlw*twc(w) = twc(l) twc(w) = -Rcw.t()*tcw 这里的不是太好理解
5.5.2 判断前进还是后退,非单目情况下,Z大于基线就是前进
5.5.3 得到最近帧的mappoint的位置,相机坐标系下坐标,像素位置
5.5.4 用窗口形式搜索.尺度越大,搜索范围越大.
尺度越大,图像越小
以下可以这么理解，例如一个有一定面积的圆点，在某个尺度n下它是一个特征点
当前进时，圆点的面积增大，在某个尺度m下它是一个特征点，由于面积增大，则需要在更高的尺度下才能检测出来
因此m>=n，对应前进的情况，nCurOctave>=nLastOctave。后退的情况可以类推
5.5.5 判断前进还是后退,在对应的范围搜索特征点
5.5.6 得到上一帧的描述子
5.5.7 遍历5.5.5搜索到的特征点,判断是否特征点已经有对应的mappoint
5.5.8 需要保证右图的点也在搜索半径以内
5.5.9 匹配距离(误差)小于阈值
5.5.10 为当前帧添加mappoint
5.5.11 再对方向验证 (见4.3.11)
5.5.12 根据方向剔除误匹配的点
5.5.13 计算rotHist中最大的三个的index
5.5.14 如果特征点的旋转角度变化量属于这三个组，则保留
5.5.15 将除了ind1 ind2 ind3以外的匹配点去掉
5.5.16 返回匹配数量
跳出
5.6 如果跟踪的点少，则扩大搜索半径再来一次,如仍少,失败
5.7 优化位姿 同4.5
5.8 优化位姿后剔除outlier的mvpMapPoints
5.9 根据内点数量判断跟踪运动模式是否成功
跳出
6 Relocalization模式
6.1 计算当前帧特征点的Bow映射
6.2 找到与当前帧相似的候选关键帧 DetectRelocalizationCandidates
/**
* @brief 在重定位中找到与该帧相似的关键帧
*
* 1. 找出和当前帧具有公共单词的所有关键帧
* 2. 只和具有共同单词较多的关键帧进行相似度计算
* 3. 将与关键帧相连（权值最高）的前十个关键帧归为一组，计算累计得分
* 4. 只返回累计得分较高的组中分数最高的关键帧
相对于关键帧的闭环检测DetectLoopCandidates，重定位检测中没法获得相连的关键帧
6.2.1 用于保存可能与F形成回环的候选帧（只要有相同的word，且不属于局部相连帧） 这里的F在Tracking中就是当前帧
6.2.2 步骤一:找出和当前帧具有公共单词的所有关键帧
6.2.3 words是检测图像是否匹配的枢纽，遍历该F的每一个word
6.2.4 提取所有包含该word的关键帧lKFs
6.2.5 遍历刚刚提取的关键帧 不包括当前帧
6.2.6 统计所有刚刚记录的候选关键帧中与当前帧F具有共同单词最多的单词数，并以此决定阈值
6.2.7 最小单词数为最大单词数*0.8作为阈值
6.2.8 遍历所有候选关键帧，挑选出共有单词数大于阈值minCommonWords存入lScoreAndMatch
6.2.9
// Lets now accumulate score by covisibility
// 步骤4：计算候选帧组得分，得到最高组得分bestAccScore，并以此决定阈值minScoreToRetain
// 单单计算当前帧和某一关键帧的相似性是不够的，这里将与关键帧相连（权值最高，共视程度最高）的前十个关键帧归为一组，计算累计得分
// 具体而言：lScoreAndMatch中每一个KeyFrame都把与自己共视程度较高的帧归为一组，每一组会计算组得分并记录该组分数最高的KeyFrame，记录于lAccScoreAndMatch
for(list<pair<float,KeyFrame*> >::iterator it=lScoreAndMatch.begin(), itend=lScoreAndMatch.end(); it!=itend; it++)
{
KeyFrame* pKFi = it->second;
vector<KeyFrame*> vpNeighs = pKFi->GetBestCovisibilityKeyFrames(10);
float bestScore = it->first; // 该组最高分数
float accScore = bestScore; // 该组累计得分
KeyFrame* pBestKF = pKFi; // 该组最高分数对应的关键帧
for(vector<KeyFrame*>::iterator vit=vpNeighs.begin(), vend=vpNeighs.end(); vit!=vend; vit++)
{
KeyFrame* pKF2 = *vit;
if(pKF2->mnRelocQuery!=F->mnId)
continue;
accScore+=pKF2->mRelocScore;// 只有pKF2也在候选帧中，才能贡献分数
if(pKF2->mRelocScore>bestScore)// 统计得到组里分数最高的KeyFrame
{
pBestKF=pKF2;
bestScore = pKF2->mRelocScore;
}
}
lAccScoreAndMatch.push_back(make_pair(accScore,pBestKF));
if(accScore>bestAccScore) // 记录所有组中组得分最高的组
bestAccScore=accScore; // 得到所有组中最高的累计得分
}
lAccScoreAndMatch.push_back(make_pair(accScore,pBestKF)); //accScore是组的累计得分,后面那个是组内最好关键帧
6.2.10 得到组得分大于阈值的(最高得分组的得分总和*0.7)，组内得分最高的关键帧
6.2.11 把组内得分最高的关键帧放入队列
跳出
6.3 创建 ORB特征点匹配器 最小距离 < 0.75*次小距离 匹配成功。
6.4 创建 PnPsolver 位姿变换求解器数组，对应上面的多个候选关键帧。
6.5 遍历每一个候选关键帧使用BOW特征向量(SearchByBoW见4.3)加速匹配,如果匹配点数少于15，跳过此候选关键帧。
6.6 如果匹配成功,初始化PnPsolver ,计算RANSAC迭代次数
6.7 通过EPnP算法估计姿态
6.8 通过PoseOptimization对姿态进行优化求解
6.9 如果内点较少(小于50)，则通过投影的方式对之前未匹配的点进行匹配，再进行优化求解
SearchByProjection(Frame &CurrentFrame, KeyFrame *pKF, const set<MapPoint*> &sAlreadyFound, const float th , const int ORBdist)
对比在跟踪运动模型下5.5(跟踪的是lastframe,是最近帧)
SearchByProjection(Frame &CurrentFrame, const Frame &LastFrame, const float th, const bool bMono)
而本次函数中跟踪的是关键帧(候选关键帧)
6.9.1 获得当前帧的旋转矩阵,平移矩阵,Ow也就是twc吧
6.9.2 旋转直方图,来检测旋转一致性
6.9.3 获取pKF对应的mappoint
6.9.4 若该点为NULL，isBad或者在SearchByBow(6.5)中已经匹配，抛弃
6.9.5 得到候选关键帧的位姿,然后通过位姿,得到当前帧的mappoint
6.9.6 得到像素坐标系下的像素坐标u,v,再根据u,v经过一个小筛选
6.9.7 得到地图点的距离dist3D
6.9.8 通过地图点的距离dist3D，预测特征对应金字塔层nPredictedLevel,
6.9.9 获取搜索window大小(th*scale)
6.9.10 得到候选匹配点向量vIndices2(找到在 以x,y为中心,边长为2r的方形内且在[minLevel, maxLevel]的特征点)
6.9.11 计算地图点的描述子和候选匹配点的描述子距离
6.9.12 获得最近距离的最佳匹配但是也要满足距离小于设定的阈值ORBdist
6.9.13 后面的和5.5.10以后相同了
跳出
6.10 如果再次反投影搜索的匹配点数量和之前得到的匹配点数量 大于50,再一次使用位姿优化
6.11 如果经过6.10的位姿优化后得到的内点数量大于30小于50 ,再一次使用SearchByProjection函数(6.9),寻找匹配点
6.12 如果数量大于50,位姿优化
6.13 如果内点数量 大于等于50 ，则重定位成功。
跳出
跳回一开始的Tracking::Track()该第7步了.
7 将最新的关键帧作为reference frame
8 TrackLocalMap();
：在帧间匹配得到初始的姿态后，现在对local map进行跟踪得到更多的匹配，并优化当前位姿
local map:当前帧、当前帧的MapPoints、当前关键帧与其它关键帧共视关系
在步骤456中主要是两两跟踪（恒速模型跟踪上一帧、跟踪参考帧），这里搜索局部关键帧后搜集所有局部MapPoints，
然后将局部MapPoints和当前帧进行投影匹配，得到更多匹配的MapPoints后进行Pose优化
bool Tracking::TrackLocalMap()
{
// We have an estimation of the camera pose and some map points tracked in the frame.
// We retrieve the local map and try to find matches to points in the local map.
// Update Local KeyFrames and Local Points
// 步骤1：更新局部关键帧mvpLocalKeyFrames和局部地图点mvpLocalMapPoints
//8.1
UpdateLocalMap(); //针对双目
// 步骤2：在局部地图中查找在当前帧视野范围内的点，将视野范围内的点和当前帧的特征点进行投影匹配
//8.1
SearchLocalPoints();
// Optimize Pose
// 在这个函数之前，在Relocalization、TrackReferenceKeyFrame、TrackWithMotionModel中都有位姿优化，
// 步骤3：更新局部所有MapPoints后对位姿再次优化
//8.3
Optimizer::PoseOptimization(&mCurrentFrame);
mnMatchesInliers = 0;
// Update MapPoints Statistics
// 步骤3：更新当前帧的MapPoints被观测程度，并统计跟踪局部地图的效果
for(int i=0; i<mCurrentFrame.N; i++)
{
if(mCurrentFrame.mvpMapPoints[i])
{
// 由于当前帧的MapPoints可以被当前帧观测到，其被观测统计量加1
if(!mCurrentFrame.mvbOutlier[i])
{
mCurrentFrame.mvpMapPoints[i]->IncreaseFound();
if(!mbOnlyTracking)
{
// 该MapPoint被其它关键帧观测到过
if(mCurrentFrame.mvpMapPoints[i]->Observations()>0)
mnMatchesInliers++;
}
else
// 记录当前帧跟踪到的MapPoints，用于统计跟踪效果
mnMatchesInliers++;
}
else if(mSensor==System::STEREO)
mCurrentFrame.mvpMapPoints[i] = static_cast<MapPoint*>(NULL);
}
}
// Decide if the tracking was succesful
// More restrictive if there was a relocalization recently
// 步骤4：决定是否跟踪成功
if(mCurrentFrame.mnId<mnLastRelocFrameId+mMaxFrames && mnMatchesInliers<50)
return false;
if(mnMatchesInliers<30)
return false;
else
return true;
}
8.1 UpdateLocalMap()针对双目
更新局部关键帧mvpLocalKeyFrames和局部地图点mvpLocalMapPoints
8.1.1 SetReferenceMapPoints 设置参考MapPoints，将用于DrawMapPoints函数画图
8.1.2 UpdateLocalKeyFrames();更新局部关键帧
遍历当前帧的MapPoints，将观测到这些MapPoints的关键帧和相邻的关键帧取出，更新mvpLocalKeyFrames
8.1.2.1 遍历当前帧的MapPoints，记录所有能观测到当前帧MapPoints的关键帧
8.1.2.2 更新局部关键帧（mvpLocalKeyFrames），添加局部关键帧有三个策略
先清空局部关键帧
策略1：能观测到当前帧MapPoints的关键帧作为局部关键帧
mvpLocalKeyFrames.push_back(it->first);
策略2：与策略1得到的局部关键帧共视程度很高的关键帧作为局部关键帧
最佳共视的10帧
自己的子关键帧
自己的父关键帧
步骤3：更新当前帧的参考关键帧，与自己共视程度最高的关键帧作为参考关键帧pKFmax
8.1.3 UpdateLocalPoints();
8.1.3.1 清空局部MapPoints
8.1.3.2 遍历局部关键帧mvpLocalKeyFrames
8.1.3.3 将局部关键帧的MapPoints添加到mvpLocalMapPoints
8.2 SearchLocalPoint
在局部地图中查找在当前帧视野范围内的点，将视野范围内的点和当前帧的特征点进行投影匹配
8.2.1 遍历当前帧的mvpMapPoints，标记这些MapPoints不参与之后的搜索 因为当前的mvpMapPoints一定在当前帧的视野中
8.2.2 将所有局部MapPoints投影到当前帧，判断是否在视野范围内，然后进行投影匹配
8.2.3 已经被当前帧观测到MapPoint不再判断是否能被当前帧观测到
8.2.4 判断LocalMapPoints中的点是否在在视野内 mCurrentFrame.isInFrustum(pMP,0.5)) 这里的e0.5表示cos60度
8.2.5 创建0RB匹配器ORBmatcher matcher(0.8);
8.2.6 如果不久前进行过重定位，那么进行一个更加宽泛的搜索，阈值需要增大
8.2.7 对视野范围内的MapPoints通过投影进行特征点匹配
SearchByProjection
/**
* @brief 通过投影，对Local MapPoint进行跟踪
*
* 将Local MapPoint投影到当前帧中, 由此增加当前帧的MapPoints \n
* 在SearchLocalPoints()中已经将Local MapPoints重投影（isInFrustum()）到当前帧 \n
* 并标记了这些点是否在当前帧的视野中，即mbTrackInView \n
* 对这些MapPoints，在其投影点附近根据描述子距离选取匹配，以及最终的方向投票机制进行剔除
* @param F 当前帧
* @param vpMapPoints Local MapPoints
* @param th 阈值
* @return 成功匹配的数量
* @see SearchLocalPoints() isInFrustum()
*/
8.2.7.1 遍历mappoints
8.2.7.2 通过距离预测的金字塔层数，该层数相对于当前的帧
8.2.7.3 搜索窗口的大小取决于视角, 若当前视角和平均视角夹角接近0度时, r取一个较小的值
8.2.7.4 通过投影点(投影到当前帧,见isInFrustum())以及搜索窗口和预测的尺度进行搜索, 找出附近的兴趣点
8.2.7.5 得到Mappoint的描述子
8.2.7.6 遍历搜索得到的兴趣点
8.2.7.7 如果该兴趣点已经有对应的mappoint,退出此次循环
8.2.7.8 根据描述子寻找描述子距离最小和次小的特征点 且距离最小特征点小于阈值并且大于距离次小乘以比例, 为Frame中的兴趣点添加对应的Mappoint
跳出
跳出
8.3 更新局部所有MapPoints后对位姿再次优化
8.4 更新当前帧的MapPoints被观测程度，并统计跟踪局部地图的效果
8.5 决定是否跟踪成功
如果刚刚进行过重定位 则需要 内点匹配点对数 大于50 才认为 成功
正常情况下，找到的内点匹配点对数 大于30 算成功
跳出
9 mpFrameDrawer->Update(this);
将跟踪线程的数据拷贝到绘图线程（图像、特征点、地图、跟踪状态）
10 更新运动模型中的mVelocity(最近帧到当前帧位姿Tcl)
11 清除UpdateLastFrame中为当前帧临时添加的MapPoints
12 清除临时的MapPoints，这些MapPoints在TrackWithMotionModel的UpdateLastFrame函数里生成（仅双目和rgbd）
// 步骤11中只是在当前帧中将这些MapPoints剔除，这里从MapPoints数据库中删除
// 这里生成的仅仅是为了提高双目或rgbd摄像头的帧间跟踪效果，用完以后就扔了，没有添加到地图中
13 判断是否需要插入关键帧 NeedNewKeyFrame()
13.1 如果局部地图被闭环检测使用，则不插入关键帧
13.2 如果关键帧比较少，则考虑插入关键帧 或距离上一次重定位超过1s，则考虑插入关键帧
13.3 得到参考关键帧跟踪到的MapPoints数量
//在UpdateLocalKeyFrames函数中会将与当前关键帧共视程度最高的关键帧设定为当前帧的参考关键帧
13.4 查询局部地图管理器是否繁忙,即队列是否还有关键帧待处理
13.5 对于双目或RGBD摄像头，统计总的可以添加的MapPoints数量和跟踪到地图中的MapPoints数量
13.6决策是否需要插入关键帧
设定inlier阈值，和之前帧特征点匹配的inlier比例
13.7 cla 很长时间没有插入关键帧
clb localMapper 处于空闲状态
clc 跟踪将要跪
c2 上面条件成立之前必须当 当前帧与之前参考帧（最近的一个关键帧）重复度不是太高。
13.8 需要新建关键帧的条件 (c1a || c1b || c1c) && c2
13.9 队列里关键帧数量不多时插入,队列里不能阻塞太多关键帧
// tracking插入关键帧不是直接插入，而且先插入到mlNewKeyFrames中，
// 然后localmapper再逐个pop出来插入到mspKeyFrames
14 CreateNewKeyFrame()
创建新的关键帧
* 对于非单目的情况，同时创建新的MapPoints
14.1 将当前帧构造成关键帧
14.2 将当前关键帧设置为当前帧的参考关键帧
// 在UpdateLocalKeyFrames函数中会将与当前关键帧共视程度最高的关键帧设定为当前帧的参考关键帧
14.3 对于双目或rgbd摄像头，为当前帧生成新的MapPoints
根据Tcw计算mRcw、mtcw和mRwc、mOw
得到当前帧深度大于0的特征点
将深度排序
将距离比较近的点包装成mappoint
为mappoint添加属性.(区别updatelastframe,在运动模型下)
14.4 将当前关键帧设置为最近关键帧
15 删除那些在bundle adjustment中检测为outlier的3D map点
16 跟踪失败且重定位失败,rest
17 将最新的关键帧作为参考关键帧
18 将当前帧设置为最近帧
19 记录位姿信息,用于轨复现
20 计算相对姿态T_currentFrame_referenceKeyFrame
如果跟踪失败，则相对位姿使用上一次值
双目tracking部分结束 后面添加上单目的
参考: 吴博跟谢晓佳的代码注释
EwenWanW的博客
sylvester0510的博客
---------------------
瓜分20万奖金 获得内推名额 丰厚实物奖励 易参与易上手
更多推荐
2
0- 0
已为社区贡献10条内容
所有评论(0)