12345qiupeng
/
orb_slam3_details
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

fix some comments

master
electech6 2021-11-26 16:09:32 +08:00
parent 3fd2fcccb7
commit a1174f1df8
14 changed files with 138 additions and 105 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
include/KeyFrame.h
View File

@ -202,9 +202,9 @@ public:
long unsigned int mnMergeQuery;
int mnMergeWords;
float mMergeScore;
long unsigned int mnPlaceRecognitionQuery;
long unsigned int mnPlaceRecognitionQuery; //标记该关键帧被当前关键帧访问到（也就是有公共单词），防止重复添加
int mnPlaceRecognitionWords;
float mPlaceRecognitionScore;
float mPlaceRecognitionScore; //记录该候选帧与当前帧的相似度
bool mbCurrentPlaceRecognition;

9
include/LocalMapping.h
View File

@ -141,8 +141,8 @@ protected:
System *mpSystem;
bool mbMonocular;
bool mbInertial;
bool mbMonocular; //mSensor==MONOCULAR || mSensor==IMU_MONOCULAR
bool mbInertial; //mSensor==IMU_MONOCULAR || mSensor==IMU_STEREO
void ResetIfRequested();
bool mbResetRequested;
@ -178,10 +178,11 @@ protected:
bool mbAcceptKeyFrames;
std::mutex mMutexAccept;
// IMU初始化函数，通过控制不同的参数来表示不同阶段
void InitializeIMU(float priorG = 1e2, float priorA = 1e6, bool bFirst = false);
// 单目惯性模式下优化尺度和重力方向
void ScaleRefinement();
//跟踪线程使用，如果为true，暂不添加关键帧
bool bInitializing;
Eigen::MatrixXd infoInertial;

2
include/LoopClosing.h
View File

@ -179,7 +179,7 @@ protected:
int mnLoopNumCoincidences;
int mnLoopNumNotFound;
KeyFrame* mpLoopLastCurrentKF;
g2o::Sim3 mg2oLoopSlw;
g2o::Sim3 ;
g2o::Sim3 mg2oLoopScw;
KeyFrame* mpLoopMatchedKF;
std::vector<MapPoint*> mvpLoopMPs;

82
include/Tracking.h
View File

@ -63,18 +63,20 @@ public:
~Tracking();
// Parse the config file
// 提取配置文件数据
bool ParseCamParamFile(cv::FileStorage &fSettings);
bool ParseORBParamFile(cv::FileStorage &fSettings);
bool ParseIMUParamFile(cv::FileStorage &fSettings);
// Preprocess the input and call Track(). Extract features and performs stereo matching.
// 输入图像输出位姿Tcw
cv::Mat GrabImageStereo(const cv::Mat &imRectLeft,const cv::Mat &imRectRight, const double &timestamp, string filename);
cv::Mat GrabImageRGBD(const cv::Mat &imRGB,const cv::Mat &imD, const double &timestamp, string filename);
cv::Mat GrabImageMonocular(const cv::Mat &im, const double &timestamp, string filename);
// cv::Mat GrabImageImuMonocular(const cv::Mat &im, const double &timestamp);
// 放置IMU数据
void GrabImuData(const IMU::Point &imuMeasurement);
// 设置线程指针
void SetLocalMapper(LocalMapping* pLocalMapper);
void SetLoopClosing(LoopClosing* pLoopClosing);
void SetViewer(Viewer* pViewer);
@ -82,23 +84,27 @@ public:
// Load new settings
// The focal lenght should be similar or scale prediction will fail when projecting points
// 更换新的标定参数，未使用
void ChangeCalibration(const string &strSettingPath);
// Use this function if you have deactivated local mapping and you only want to localize the camera.
// 设置是否仅定位模式还是SLAM模式
void InformOnlyTracking(const bool &flag);
// localmapping中更新了关键帧的位姿后，更新普通帧的位姿，通过IMU积分更新速度。localmapping中初始化imu中使用
void UpdateFrameIMU(const float s, const IMU::Bias &b, KeyFrame* pCurrentKeyFrame);
KeyFrame* GetLastKeyFrame()
{
return mpLastKeyFrame;
}
// 新建地图
void CreateMapInAtlas();
std::mutex mMutexTracks;
//--
// 更新数据集
void NewDataset();
// 获得数据集总数
int GetNumberDataset();
// 获取匹配内点总数
int GetMatchesInliers();
public:
@ -122,7 +128,7 @@ public:
// Current Frame
Frame mCurrentFrame;
Frame mLastFrame;
Frame mLastFrame; //跟踪成功后，保存当前帧数据
cv::Mat mImGray;
@ -135,6 +141,7 @@ public:
// Lists used to recover the full camera trajectory at the end of the execution.
// Basically we store the reference keyframe for each frame and its relative transformation
// 代码结束后保存位姿用的列表
list<cv::Mat> mlRelativeFramePoses;
list<KeyFrame*> mlpReferences;
list<double> mlFrameTimes;
@ -145,6 +152,7 @@ public:
bool mbStep;
// True if local mapping is deactivated and we are performing only localization
// true表示仅定位模式，此时局部建图线程和闭环线程关闭
bool mbOnlyTracking;
void Reset(bool bLocMap = false);
@ -156,7 +164,7 @@ public:
double t0vis; // time-stamp of first inserted keyframe
double t0IMU; // time-stamp of IMU initialization
// 获取局部地图点
vector<MapPoint*> GetLocalMapMPS();
bool mbWriteStats;
@ -186,40 +194,41 @@ public:
protected:
// Main tracking function. It is independent of the input sensor.
void Track();
void Track(); //主要的跟踪函数
// Map initialization for stereo and RGB-D
void StereoInitialization();
void StereoInitialization(); //双目初始化
// Map initialization for monocular
void MonocularInitialization();
void CreateNewMapPoints();
void MonocularInitialization(); //单目初始化
void CreateNewMapPoints(); //创建新的地图点
cv::Mat ComputeF12(KeyFrame *&pKF1, KeyFrame *&pKF2);
void CreateInitialMapMonocular();
void CreateInitialMapMonocular(); //单目模式下创建初始化地图
void CheckReplacedInLastFrame();
bool TrackReferenceKeyFrame();
void UpdateLastFrame();
bool TrackWithMotionModel();
bool PredictStateIMU();
void CheckReplacedInLastFrame();//检查上一帧中的地图点是否需要被替换
bool TrackReferenceKeyFrame(); // 参考关键帧跟踪
void UpdateLastFrame(); //更新上一帧位姿，在上一帧中生成临时地图点
bool TrackWithMotionModel(); //恒速模型跟踪
bool PredictStateIMU(); //用IMU预测位姿
bool Relocalization();
bool Relocalization(); //重定位
void UpdateLocalMap();
void UpdateLocalPoints();
void UpdateLocalKeyFrames();
void UpdateLocalMap(); //更新局部地图
void UpdateLocalPoints(); //更新局部地图点
void UpdateLocalKeyFrames(); //更新局部地图里的关键帧
bool TrackLocalMap();
bool TrackLocalMap_old();
void SearchLocalPoints();
bool TrackLocalMap(); //局部地图跟踪
bool TrackLocalMap_old(); //未定义
void SearchLocalPoints(); //搜索局部地图点
bool NeedNewKeyFrame();
void CreateNewKeyFrame();
bool NeedNewKeyFrame(); //判断是否需要插入关键帧
void CreateNewKeyFrame(); //创建关键帧
// Perform preintegration from last frame
void PreintegrateIMU();
void PreintegrateIMU(); //IMU预积分
// Reset IMU biases and compute frame velocity
// 重置并重新计算IMU偏置。未使用
void ComputeGyroBias(const vector<Frame*> &vpFs, float &bwx, float &bwy, float &bwz);
void ComputeVelocitiesAccBias(const vector<Frame*> &vpFs, float &bax, float &bay, float &baz);
@ -230,14 +239,14 @@ protected:
IMU::Preintegrated *mpImuPreintegratedFromLastKF;
// Queue of IMU measurements between frames
std::list<IMU::Point> mlQueueImuData;
std::list<IMU::Point> mlQueueImuData; //存放两帧之间的IMU数据
// Vector of IMU measurements from previous to current frame (to be filled by PreintegrateIMU)
std::vector<IMU::Point> mvImuFromLastFrame;
std::mutex mMutexImuQueue;
// Imu calibration parameters
IMU::Calib *mpImuCalib;
IMU::Calib *mpImuCalib; //IMU标定参数
// Last Bias Estimation (at keyframe creation)
IMU::Bias mLastBias;
@ -249,10 +258,11 @@ protected:
bool mbVO;
//Other Thread Pointers
// 其他线程的指针
LocalMapping* mpLocalMapper;
LoopClosing* mpLoopClosing;
//ORB
//ORB特征提取器
ORBextractor* mpORBextractorLeft, *mpORBextractorRight;
ORBextractor* mpIniORBextractor;
@ -297,13 +307,13 @@ protected:
// Threshold close/far points
// Points seen as close by the stereo/RGBD sensor are considered reliable
// and inserted from just one frame. Far points requiere a match in two keyframes.
float mThDepth;
float mThDepth; //近远点阈值，基线的倍数
// For RGB-D inputs only. For some datasets (e.g. TUM) the depthmap values are scaled.
float mDepthMapFactor;
float mDepthMapFactor; // RGB-D尺度缩放因子
//Current matches in frame
int mnMatchesInliers;
int mnMatchesInliers; //当前帧匹配内点数
//Last Frame, KeyFrame and Relocalisation Info
KeyFrame* mpLastKeyFrame;
@ -321,12 +331,12 @@ protected:
//Motion Model
cv::Mat mVelocity;
cv::Mat mVelocity; // 恒速模型的速度。通过位姿增量获得或者IMU积分得到
//Color order (true RGB, false BGR, ignored if grayscale)
bool mbRGB;
list<MapPoint*> mlpTemporalPoints;
list<MapPoint*> mlpTemporalPoints; // 临时地图点
//int nMapChangeIndex;
@ -340,7 +350,7 @@ protected:
double mTime_LocalMapTrack;
double mTime_NewKF_Dec;
GeometricCamera* mpCamera, *mpCamera2;
GeometricCamera* mpCamera, *mpCamera2; //相机类
int initID, lastID;

BIN
outline.png
View File

Binary file not shown.
Side by Side Swipe Overlay

Before

Width:  |  Height:  |  Size: 915 KiB

After

Width:  |  Height:  |  Size: 112 KiB

2
src/Frame.cc
View File

@ -1051,7 +1051,7 @@ void Frame::ComputeStereoMatches()
// 金字塔顶层（0层）图像高 nRows
const int nRows = mpORBextractorLeft->mvImagePyramid[0].rows;
// 二维vector存储每一行的orb特征点的列坐标，为什么是vector，因为每一行的特征点有可能不一样，例如
// 二维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<vector<size_t> > vRowIndices(nRows,vector<size_t>());

2
src/Initializer.cc
View File

@ -1648,7 +1648,7 @@ int Initializer::CheckRT(const cv::Mat &R, const cv::Mat &t, const vector<cv::Ke
if(cosParallax<0.99998)
vbGood[vMatches12[i].first]=true;
}
// Step 7 得到3D点中较小的视差角，并且转换成为角度制表示
if(nGood>0)
{
// 从小到大排序，注意vCosParallax值越大，视差越小

12
src/KeyFrameDatabase.cc
View File

@ -691,7 +691,7 @@ void KeyFrameDatabase::DetectNBestCandidates(KeyFrame *pKF, vector<KeyFrame*> &v
// 如果该关键帧不是当前关键帧的共视关键帧
if(!spConnectedKF.count(pKFi))
{
// 标记改关键帧被当前关键帧访问到
// 标记该关键帧被当前关键帧访问到（也就是有公共单词）
pKFi->mnPlaceRecognitionQuery=pKF->mnId;
// 把当前关键帧添加到有公共单词的关键帧列表中
lKFsSharingWords.push_back(pKFi);
@ -713,7 +713,7 @@ void KeyFrameDatabase::DetectNBestCandidates(KeyFrame *pKF, vector<KeyFrame*> &v
return;
// Only compare against those keyframes that share enough words
// Step 2 统计所有候选帧中与当前关键帧的公共单词数最多的单词数,并筛选
// Step 2 统计所有候选帧中与当前关键帧的公共单词数最多的单词数maxCommonWords,并筛选
int maxCommonWords=0;
for(list<KeyFrame*>::iterator lit=lKFsSharingWords.begin(), lend= lKFsSharingWords.end(); lit!=lend; lit++)
{
@ -738,7 +738,7 @@ void KeyFrameDatabase::DetectNBestCandidates(KeyFrame *pKF, vector<KeyFrame*> &v
// 如果当前帧的公共单词数大于minCommonWords
if(pKFi->mnPlaceRecognitionWords>minCommonWords)
{
nscores++;
nscores++; //未使用
// 计算相似度
float si = mpVoc->score(pKF->mBowVec,pKFi->mBowVec);
// 记录该候选帧与当前帧的相似度
@ -773,12 +773,12 @@ void KeyFrameDatabase::DetectNBestCandidates(KeyFrame *pKF, vector<KeyFrame*> &v
for(vector<KeyFrame*>::iterator vit=vpNeighs.begin(), vend=vpNeighs.end(); vit!=vend; vit++)
{
KeyFrame* pKF2 = *vit;
// 如果改关键帧没有被当前关键帧访问过(这里标记的是有没有公共单词)则跳过
// 如果该关键帧没有被当前关键帧访问过(也就是没有公共单词)则跳过
if(pKF2->mnPlaceRecognitionQuery!=pKF->mnId)
continue;
// 累加小组总分
accScore+=pKF2->mPlaceRecognitionScore;
// 如果大与组内最高分,则重新记录
// 如果大于组内最高分,则更新当前最高分记录
if(pKF2->mPlaceRecognitionScore>bestScore)
{
pBestKF=pKF2;
@ -806,7 +806,7 @@ void KeyFrameDatabase::DetectNBestCandidates(KeyFrame *pKF, vector<KeyFrame*> &v
//for(list<pair<float,KeyFrame*> >::iterator it=lAccScoreAndMatch.begin(), itend=lAccScoreAndMatch.end(); it!=itend; it++)
int i = 0;
list<pair<float,KeyFrame*> >::iterator it=lAccScoreAndMatch.begin();
// 遍历lAccScoreAndMatch中所有的pair, 每个pair为<小组总相似度,组内相似度最高的关键帧指针>
// 遍历lAccScoreAndMatch中所有的pair, 每个pair为<小组总相似度,组内相似度最高的关键帧指针>，nNumCandidates默认为3
while(i < lAccScoreAndMatch.size() && (vpLoopCand.size() < nNumCandidates || vpMergeCand.size() < nNumCandidates))
{
//cout << "Accum score: " << it->first << endl;

19
src/LocalMapping.cc
View File

@ -174,6 +174,7 @@ void LocalMapping::Run()
// 判断成功跟踪匹配的点数是否足够多
// 条件---------1.1、跟踪成功的内点数目大于75-----1.2、并且是单目--或--2.1、跟踪成功的内点数目大于100-----2.2、并且不是单目
bool bLarge = ((mpTracker->GetMatchesInliers()>75)&&mbMonocular)||((mpTracker->GetMatchesInliers()>100)&&!mbMonocular);
// 局部地图+IMU一起优化，优化关键帧位姿、地图点、IMU参数
Optimizer::LocalInertialBA(mpCurrentKeyFrame, &mbAbortBA, mpCurrentKeyFrame->GetMap(),num_FixedKF_BA,num_OptKF_BA,num_MPs_BA,num_edges_BA, bLarge, !mpCurrentKeyFrame->GetMap()->GetIniertialBA2());
b_doneLBA = true;
}
@ -182,6 +183,7 @@ void LocalMapping::Run()
// Step 6.2 不是IMU模式或者当前关键帧所在的地图还未完成IMU初始化
// 局部地图BA，不包括IMU数据
// 注意这里的第二个参数是按地址传递的,当这里的 mbAbortBA 状态发生变化时，能够及时执行/停止BA
// 局部地图优化，不包括IMU信息。优化关键帧位姿、地图点
Optimizer::LocalBundleAdjustment(mpCurrentKeyFrame,&mbAbortBA, mpCurrentKeyFrame->GetMap(),num_FixedKF_BA,num_OptKF_BA,num_MPs_BA,num_edges_BA);
b_doneLBA = true;
}
@ -233,7 +235,7 @@ void LocalMapping::Run()
timeKFCulling_ms = std::chrono::duration_cast<std::chrono::duration<double,std::milli> >(time_EndKFCulling - time_EndLBA).count();
vdKFCullingSync_ms.push_back(timeKFCulling_ms);
#endif
// Step 9 如果距离初始化成功累计时间差小于100s 并且 是IMU模式，进行VIBA
// Step 9 如果距离IMU第一阶段初始化成功累计时间差小于100s，进行VIBA
if ((mTinit<100.0f) && mbInertial)
{
// Step 9.1 根据条件判断是否进行VIBA1（IMU第二阶段初始化）
@ -283,6 +285,7 @@ void LocalMapping::Run()
(mTinit>75.0f && mTinit<75.5f))){
cout << "start scale ref" << endl;
if (mbMonocular)
// 使用了所有关键帧，但只优化尺度和重力方向
ScaleRefinement();
cout << "end scale ref" << endl;
}
@ -1236,12 +1239,12 @@ void LocalMapping::KeyFrameCulling()
vector<KeyFrame*> vpLocalKeyFrames = mpCurrentKeyFrame->GetVectorCovisibleKeyFrames();
float redundant_th;
// 非IMU时
// 非IMU模式时
if(!mbInertial)
redundant_th = 0.9;
else if (mbMonocular) // imu 且单目时
else if (mbMonocular) // 单目+imu 时
redundant_th = 0.9;
else // 其他imu时
else // 双目+imu时
redundant_th = 0.5;
const bool bInitImu = mpAtlas->isImuInitialized();
@ -1268,7 +1271,7 @@ void LocalMapping::KeyFrameCulling()
{
count++;
KeyFrame* pKF = *vit;
// 如果是地图里第1个关键帧或者是标记为坏帧，则跳过
if((pKF->mnId==pKF->GetMap()->GetInitKFid()) || pKF->isBad())
continue;
// Step 2：提取每个共视关键帧的地图点
@ -1291,7 +1294,7 @@ void LocalMapping::KeyFrameCulling()
{
if(!mbMonocular)
{
// 对于双目，仅考虑近处（不超过基线的 35倍 ）的地图点
// 对于双目，仅考虑近处（不超过基线的40倍 ）的地图点
if(pKF->mvDepth[i]>pKF->mThDepth || pKF->mvDepth[i]<0)
continue;
}
@ -1768,7 +1771,7 @@ void LocalMapping::InitializeIMU(float priorG, float priorA, bool bFIBA)
}
/**
* @brief 通过BA优化进行尺度更新，关键帧小于100，在这里的时间段内时多次进行尺度更新
* @brief 通过BA优化进行尺度更新，关键帧小于100，使用了所有关键帧的信息，但只优化尺度和重力方向。每10s在这里的时间段内时多次进行尺度更新
*/
void LocalMapping::ScaleRefinement()
{
@ -1799,6 +1802,8 @@ void LocalMapping::ScaleRefinement()
const int N = vpKF.size();
// 2. 更新旋转与尺度
// 待优化变量的初值
mRwg = Eigen::Matrix3d::Identity();
mScale=1.0;

57
src/LoopClosing.cc
View File

@ -43,7 +43,7 @@ LoopClosing::LoopClosing(Atlas *pAtlas, KeyFrameDatabase *pDB, ORBVocabulary *pV
mpLastCurrentKF = static_cast<KeyFrame*>(NULL);
}
// 设置追踪线程句柄
// 设置跟踪线程句柄
void LoopClosing::SetTracker(Tracking *pTracker)
{
mpTracker=pTracker;
@ -393,7 +393,7 @@ bool LoopClosing::NewDetectCommonRegions()
// Loop candidates
bool bLoopDetectedInKF = false;
bool bCheckSpatial = false;
// Step 3.1 回环的时序几何校验: 这里的判断条件里mnLoopNumCoincidences刚开始为0, 所以可以先跳过看后面
// Step 3.1 回环的时序几何校验。注意初始化时mnLoopNumCoincidences=0, 所以可以先跳过看后面
// 如果回环的共视几何验证成功帧数目大于0
if(mnLoopNumCoincidences > 0)
{
@ -475,7 +475,7 @@ bool LoopClosing::NewDetectCommonRegions()
//Merge candidates
bool bMergeDetectedInKF = false;
// Step 3.2 融合的时序几何校验: 这里的判断条件里mnMergeNumCoincidences刚开始为0, 所以可以先跳过看后面
// Step 3.2 融合的时序几何校验: 注意初始化时mnMergeNumCoincidences=0, 所以可以先跳过看后面
// 如果融合的共视几何验证成功帧数目大于0
if(mnMergeNumCoincidences > 0)
{
@ -550,6 +550,7 @@ bool LoopClosing::NewDetectCommonRegions()
}
}
// Step 3.3 若校验成功则把当前帧添加进数据库,且返回true表示找到共同区域
// 注意初始化时mbMergeDetected=mbLoopDetected=false
if(mbMergeDetected || mbLoopDetected)
{
//f_time_pr << "Geo" << " " << timeGeoKF_ms.count() << endl;
@ -606,6 +607,8 @@ bool LoopClosing::NewDetectCommonRegions()
// Step 4.1 若当前关键帧没有被检测到回环,并且候选帧数量不为0,则对回环候选帧进行论文中第8页的2-5步
if(!bLoopDetectedInKF && !vpLoopBowCand.empty())
{
// mnLoopNumCoincidences是成功几何验证的帧数，超过3就认为几何验证成功，不超过继续进行时序验证
// mpLoopMatchedKF 最后成功匹配的候选关键帧
mbLoopDetected = DetectCommonRegionsFromBoW(vpLoopBowCand, mpLoopMatchedKF, mpLoopLastCurrentKF, mg2oLoopSlw, mnLoopNumCoincidences, mvpLoopMPs, mvpLoopMatchedMPs);
}
// Merge candidates
@ -614,6 +617,7 @@ bool LoopClosing::NewDetectCommonRegions()
// Step 4.2 若当前关键帧没有被检测到融合,并且候选帧数量不为0,则对融合候帧进行论文中第8页的2-5步
if(!bMergeDetectedInKF && !vpMergeBowCand.empty())
{
// mnLoopNumCoincidences是成功几何验证的帧数，超过3就认为几何验证成功，不超过继续进行时序验证
mbMergeDetected = DetectCommonRegionsFromBoW(vpMergeBowCand, mpMergeMatchedKF, mpMergeLastCurrentKF, mg2oMergeSlw, mnMergeNumCoincidences, mvpMergeMPs, mvpMergeMatchedMPs);
}
@ -679,17 +683,16 @@ bool LoopClosing::DetectAndReffineSim3FromLastKF(KeyFrame* pCurrentKF, KeyFrame*
// 单目情况下不锁定尺度
bool bFixedScale = mbFixScale; // TODO CHECK; Solo para el monocular inertial
// 如果是imu模式且地图没成熟,不锁定尺度
// 如果是imu模式且未完成初始化,不锁定尺度
if(mpTracker->mSensor==System::IMU_MONOCULAR && !pCurrentKF->GetMap()->GetIniertialBA2())
bFixedScale=false;
// 继续优化 Sim3
int numOptMatches = Optimizer::OptimizeSim3(mpCurrentKF, pMatchedKF, vpMatchedMPs, gScm, 10, bFixedScale, mHessian7x7, true);
// 若果匹配的数量大于一定的数目
// 若匹配的数量大于一定的数目
if(numOptMatches > nProjOptMatches)
{
//!bug, 以下gScw_estimation应该通过上述sim3优化后的位姿来更新。以下mScw应该改为 gscm * gswm.t
g2o::Sim3 gScw_estimation(Converter::toMatrix3d(mScw.rowRange(0, 3).colRange(0, 3)),
Converter::toVector3d(mScw.rowRange(0, 3).col(3)),1.0);
@ -718,14 +721,14 @@ bool LoopClosing::DetectAndReffineSim3FromLastKF(KeyFrame* pCurrentKF, KeyFrame*
* 4. 利用地图中的共视关键帧验证(共视几何校验)
*
* @param[in] vpBowCand bow 给出的一些候选关键帧
* @param[out] pMatchedKF2 最后成功匹配的关键帧
* @param[out] pMatchedKF2 最后成功匹配的候选关键帧
* @param[out] pLastCurrentKF 用于记录当前关键帧为上一个关键帧(后续若仍需要时序几何校验需要记录此信息)
* @param[out] g2oScw 世界坐标系在当前关键帧下的Sim3
* @param[out] nNumCoincidences 用来记录validation合格的数目
* @param[out] nNumCoincidences 成功几何验证的帧数，超过3就认为几何验证成功，不超过继续进行时序验证
* @param[out] vpMPs 所有地图点
* @param[out] vpMatchedMPs 成功匹配的地图点
* @return true 检测到一个合格的commen region
* @return false 没检测到一个合格的commen region
* @return true 检测到一个合格的共同区域
* @return false 没检测到一个合格的共同区域
*/
bool LoopClosing::DetectCommonRegionsFromBoW(std::vector<KeyFrame*> &vpBowCand, KeyFrame* &pMatchedKF2, KeyFrame* &pLastCurrentKF, g2o::Sim3 &g2oScw,
int &nNumCoincidences, std::vector<MapPoint*> &vpMPs, std::vector<MapPoint*> &vpMatchedMPs)
@ -758,7 +761,7 @@ bool LoopClosing::DetectCommonRegionsFromBoW(std::vector<KeyFrame*> &vpBowCand,
int nNumGuidedMatching = 0;
// Varibles to select the best numbe
// 一些用与统计最优数据的变量,我们最后返回的是最佳的一个关键帧(几何校验匹配数最高的)
// 一些用于统计最优数据的变量,我们最后返回的是最佳的一个关键帧(几何校验匹配数最高的)
KeyFrame* pBestMatchedKF;
int nBestMatchesReproj = 0;
@ -772,10 +775,9 @@ bool LoopClosing::DetectCommonRegionsFromBoW(std::vector<KeyFrame*> &vpBowCand,
// 这三个变量是作者为了后面打印观察记录的信息,可以忽略
vector<int> vnStage(numCandidates, 0);
vector<int> vnMatchesStage(numCandidates, 0);
int index = 0;
//对于每个候选关键帧
//对于每个候选关键帧，进行Sim3计算和检验
for(KeyFrame* pKFi : vpBowCand)
{
//cout << endl << "-------------------------------" << endl;
@ -811,7 +813,7 @@ bool LoopClosing::DetectCommonRegionsFromBoW(std::vector<KeyFrame*> &vpBowCand,
// 记录在W_km中有最多匹配点的帧的局部index, 这个后面没有用到
int nIndexMostBoWMatchesKF=0;
//! bug: 并没有重新赋值pMostBoWMatchesKF, 一直是初始值: 候选关键帧
//! bug: 以下循环中并没有重新赋值pMostBoWMatchesKF, 一直是初始值: 候选关键帧
// 遍历窗口内Wm的每个关键帧
// Step 1.1 通过Bow寻找候选帧窗口内的关键帧地图点与当前关键帧的匹配点
for(int j=0; j<vpCovKFi.size(); ++j)
@ -834,7 +836,7 @@ bool LoopClosing::DetectCommonRegionsFromBoW(std::vector<KeyFrame*> &vpBowCand,
for(int j=0; j<vpCovKFi.size(); ++j)
{
// 如果窗口中的帧是当前帧的共视帧帧则结束这个循环
// 如果窗口中的帧是当前帧的共视帧则结束这个循环
if(spConnectedKeyFrames.find(vpCovKFi[j]) != spConnectedKeyFrames.end())
{
bAbortByNearKF = true;
@ -849,11 +851,11 @@ bool LoopClosing::DetectCommonRegionsFromBoW(std::vector<KeyFrame*> &vpBowCand,
{
// 地图点指针
MapPoint* pMPi_j = vvpMatchedMPs[j][k];
// 如果指针为空活地图点被标记为bad,则跳过当前循环
// 如果指针为空或地图点被标记为bad,则跳过当前循环
if(!pMPi_j || pMPi_j->isBad())
continue;
// 窗口内不同关键帧与当前关键帧可能能看到相同的3D点, 利用辅助容器避免重复添加
// 窗口内不同关键帧与当前关键帧可能看到相同的3D点, 利用辅助容器避免重复添加
if(spMatchedMPi.find(pMPi_j) == spMatchedMPi.end())
{
// 利用辅助容器记录添加过的点
@ -871,6 +873,7 @@ bool LoopClosing::DetectCommonRegionsFromBoW(std::vector<KeyFrame*> &vpBowCand,
//cout <<"BoW: " << numBoWMatches << " independent putative matches" << endl;
// 当窗口内的帧不是当前关键帧的相邻帧且匹配点足够多时
// Step 2 利用RANSAC寻找候选关键帧窗口与当前关键帧的相对位姿T_am的初始值(可能是Sim3)
// nBoWMatches = 20; // 最低bow匹配特征点数
if(!bAbortByNearKF && numBoWMatches >= nBoWMatches) // TODO pick a good threshold
{
/*cout << "-------------------------------" << endl;
@ -879,7 +882,7 @@ bool LoopClosing::DetectCommonRegionsFromBoW(std::vector<KeyFrame*> &vpBowCand,
// Geometric validation
bool bFixedScale = mbFixScale;
//? 如果是单目带imu的模式且地图没有成熟则不固定scale
// 如果是单目带imu的模式且IMU初始化未完成第三阶段，则不固定scale
if(mpTracker->mSensor==System::IMU_MONOCULAR && !mpCurrentKF->GetMap()->GetIniertialBA2())
bFixedScale=false;
@ -887,7 +890,7 @@ bool LoopClosing::DetectCommonRegionsFromBoW(std::vector<KeyFrame*> &vpBowCand,
// Sim3Solver 的接口与orbslam2略有不同, 因为现在是1-N的对应关系
Sim3Solver solver = Sim3Solver(mpCurrentKF, pMostBoWMatchesKF, vpMatchedPoints, bFixedScale, vpKeyFrameMatchedMP);
//Sim3Solver Ransac 置信度0.99，至少20个inliers 最多300次迭
solver.SetRansacParameters(0.99, nBoWInliers, 300); // at least 15 inliers
solver.SetRansacParameters(0.99, nBoWInliers, 300); // at least 15 inliers, nBoWInliers = 15
bool bNoMore = false;
vector<bool> vbInliers;
@ -962,17 +965,18 @@ bool LoopClosing::DetectCommonRegionsFromBoW(std::vector<KeyFrame*> &vpBowCand,
// Step 3.1 重新利用之前计算的mScw信息, 通过投影寻找更多的匹配点
int numProjMatches = matcher.SearchByProjection(mpCurrentKF, mScw, vpMapPoints, vpKeyFrames, vpMatchedMP, vpMatchedKF, 8, 1.5);
// 如果拿到了足够多的匹配点
// 如果拿到了足够多的匹配点, nProjMatches = 50
if(numProjMatches >= nProjMatches)
{
// Optimize Sim3 transformation with every matches
Eigen::Matrix<double, 7, 7> mHessian7x7;
bool bFixedScale = mbFixScale;
// 在imu模式下没有成熟的地图就不固定scale
// 在imu模式下imu未完成第3阶段初始化就不固定scale
if(mpTracker->mSensor==System::IMU_MONOCULAR && !mpCurrentKF->GetMap()->GetIniertialBA2())
bFixedScale=false;
// Step 3.2 利用搜索到的更多的匹配点优化双向投影误差得到的更好的 gScm (Tam)
// Step 3.2 利用搜索到的更多的匹配点用Sim3优化投影误差得到的更好的 gScm (Tam)
// pKFi是候选关键帧
int numOptMatches = Optimizer::OptimizeSim3(mpCurrentKF, pKFi, vpMatchedMP, gScm, 10, mbFixScale, mHessian7x7, true);
//cout <<"BoW: " << numOptMatches << " inliers in the Sim3 optimization" << endl;
//cout << "Inliers in Sim3 optimization: " << numOptMatches << endl;
@ -992,8 +996,7 @@ bool LoopClosing::DetectCommonRegionsFromBoW(std::vector<KeyFrame*> &vpBowCand,
// 5 : 半径的增益系数(对比之前下降了)---> 更小的半径, 1.0 , hamming distance 的阀值增益系数---> 允许更小的距离
int numProjOptMatches = matcher.SearchByProjection(mpCurrentKF, mScw, vpMapPoints, vpMatchedMP, 5, 1.0);
//cout <<"BoW: " << numProjOptMatches << " matches after of the Sim3 optimization" << endl;
// ? 论文中说好的再优化一次呢?只做了搜索并没有再次进行OptimizeSim3
// 当新的投影得到的内点数量大于nProjOptMatches时
// 当新的投影得到的内点数量大于nProjOptMatches=80时
if(numProjOptMatches >= nProjOptMatches)
{
// Step 4. 用当前关键帧的相邻关键来验证前面得到的Tam(共视几何校验)
@ -1061,7 +1064,7 @@ bool LoopClosing::DetectCommonRegionsFromBoW(std::vector<KeyFrame*> &vpBowCand,
if(nBestMatchesReproj > 0)
{
pLastCurrentKF = mpCurrentKF;
nNumCoincidences = nBestNumCoindicendes;
nNumCoincidences = nBestNumCoindicendes; // 成功几何验证的帧数
pMatchedKF2 = pBestMatchedKF;
pMatchedKF2->SetNotErase();
g2oScw = g2oBestScw;
@ -1164,7 +1167,7 @@ bool LoopClosing::DetectCommonRegionsFromLastKF(KeyFrame* pCurrentKF, KeyFrame*
}
/**
* @brief 包装与searchByProjection之上, 只不过是把窗口内的所有地图点往当前关键帧上投影, 寻找匹配点
* @brief 包装了一下searchByProjection, 把窗口内的所有地图点往当前关键帧上投影, 寻找匹配点
*
* @param[in] pCurrentKF 当前关键帧
* @param[in] pMatchedKFw 候选帧

1
src/ORBmatcher.cc
View File

@ -42,6 +42,7 @@ ORBmatcher::ORBmatcher(float nnratio, bool checkOri): mfNNratio(nnratio), mbChec
{
}
// 用于Tracking::SearchLocalPoints中匹配更多地图点
int ORBmatcher::SearchByProjection(Frame &F, const vector<MapPoint*> &vpMapPoints, const float th, const bool bFarPoints, const float thFarPoints)
{
int nmatches=0, left = 0, right = 0;

4
src/Optimizer.cc
View File

@ -4304,7 +4304,7 @@ int Optimizer::OptimizeSim3(KeyFrame *pKF1, KeyFrame *pKF2, vector<MapPoint *> &
* @param[in] pKF //关键帧
* @param[in] pbStopFlag //是否停止的标志
* @param[in] pMap //地图
* @param[in] num_fixedKF //
* @param[in] num_fixedKF //固定不优化关键帧的数目
* @param[in] num_OptKF
* @param[in] num_MPs
* @param[in] num_edges
@ -6027,7 +6027,7 @@ void Optimizer::LocalBundleAdjustment(KeyFrame* pMainKF,vector<KeyFrame*> vpAdju
bool bShowImages = false;
vector<MapPoint*> vpMPs;
// 1. 构建g2o优化器
// 1. 构建g2o优化器
g2o::SparseOptimizer optimizer;
g2o::BlockSolver_6_3::LinearSolverType * linearSolver;

39
src/Tracking.cc
View File

@ -1454,7 +1454,7 @@ void Tracking::GrabImuData(const IMU::Point &imuMeasurement)
mlQueueImuData.push_back(imuMeasurement);
}
// ?TODO
// 对IMU进行预积分
void Tracking::PreintegrateIMU()
{
// Step 1.拿到两两帧之间待处理的预积分数据，组成一个集合
@ -1838,7 +1838,7 @@ void Tracking::Track()
if(mpAtlas->isImuInitialized())
{
cout << "Timestamp jump detected. State set to LOST. Reseting IMU integration..." << endl;
// ?TODO BA2标志代表什么?,BA优化成功?
// IMU完成第2阶段BA（在localmapping线程里）
if(!pCurrentMap->GetIniertialBA2())
{
// 如果当前子图中imu没有经过BA2，重置active地图
@ -1862,7 +1862,7 @@ void Tracking::Track()
}
}
// Step 3 IMU模式下设置IMU的Bias参数,还要保证上一帧存在非空
// Step 3 IMU模式下设置IMU的Bias参数,还要保证上一帧存在
if ((mSensor == System::IMU_MONOCULAR || mSensor == System::IMU_STEREO) && mpLastKeyFrame)
//认为bias在两帧间不变
mCurrentFrame.SetNewBias(mpLastKeyFrame->GetImuBias());
@ -1873,7 +1873,7 @@ void Tracking::Track()
}
mLastProcessedState=mState;
// Step 4 IMU模式下对IMU数据进行预积分：没有创建地图的情况下
// Step 4 IMU模式且没有创建地图的情况下对IMU数据进行预积分
if ((mSensor == System::IMU_MONOCULAR || mSensor == System::IMU_STEREO) && !mbCreatedMap)
{
#ifdef REGISTER_TIMES
@ -1906,6 +1906,7 @@ void Tracking::Track()
if(nCurMapChangeIndex>nMapChangeIndex)
{
// cout << "Map update detected" << endl;
// 检测到地图更新了
pCurrentMap->SetLastMapChange(nCurMapChangeIndex);
mbMapUpdated = true;
}
@ -2276,22 +2277,23 @@ void Tracking::Track()
}
// Save frame if recent relocalization, since they are used for IMU reset (as we are making copy, it shluld be once mCurrFrame is completely modified)
// 如果刚刚发生重定位并且IMU已经初始化，则保存当前帧信息，重置IMU
if((mCurrentFrame.mnId<(mnLastRelocFrameId+mnFramesToResetIMU)) && // 当前帧距离上次重定位帧小于1s
(mCurrentFrame.mnId > mnFramesToResetIMU) && // 当前帧已经运行了超过1s
((mSensor == System::IMU_MONOCULAR) || (mSensor == System::IMU_STEREO)) && // IMU模式
pCurrentMap->isImuInitialized()) // IMU已经成功初始化
{
// ?TODO check this situation
// 存储指针
Verbose::PrintMess("Saving pointer to frame. imu needs reset...", Verbose::VERBOSITY_NORMAL);
Frame* pF = new Frame(mCurrentFrame);
pF->mpPrevFrame = new Frame(mLastFrame);
// Load preintegration
// ?构造预积分处理器
// IMU重置
pF->mpImuPreintegratedFrame = new IMU::Preintegrated(mCurrentFrame.mpImuPreintegratedFrame);
}
if(pCurrentMap->isImuInitialized()) // IMU成功初始化
// 下面代码看起来没有用
if(pCurrentMap->isImuInitialized())
{
if(bOK) // 跟踪成功
{
@ -3223,7 +3225,7 @@ bool Tracking::TrackWithMotionModel()
}
// 如果还是不能够获得足够的匹配点,那么就认为跟踪失败
// 这里不同于ORB-SLAM2的方式
if(nmatches<20)
{
Verbose::PrintMess("Not enough matches!!", Verbose::VERBOSITY_NORMAL);
@ -3337,13 +3339,14 @@ bool Tracking::TrackLocalMap()
// Step 3：前面新增了更多的匹配关系，BA优化得到更准确的位姿
int inliers;
if (!mpAtlas->isImuInitialized())
// IMU未初始化，仅优化位姿
Optimizer::PoseOptimization(&mCurrentFrame);
else
{
// 初始化，重定位，重新开启一个地图都会使mnLastRelocFrameId变化
if(mCurrentFrame.mnId<=mnLastRelocFrameId+mnFramesToResetIMU)
{
// 如果距离上次重定位时间比较近，积累的IMU数据较少，优化时暂不使用IMU数据
// 如果距离上次重定位时间比较近（<1s），积累的IMU数据较少，优化时暂不使用IMU数据
Verbose::PrintMess("TLM: PoseOptimization ", Verbose::VERBOSITY_DEBUG);
Optimizer::PoseOptimization(&mCurrentFrame);
}
@ -3468,13 +3471,15 @@ bool Tracking::TrackLocalMap()
*/
bool Tracking::NeedNewKeyFrame()
{
// 如果是IMU模式并且当前地图中未完成IMU初始化
if(((mSensor == System::IMU_MONOCULAR) || (mSensor == System::IMU_STEREO)) && !mpAtlas->GetCurrentMap()->isImuInitialized())
{
// 如果是IMU模式，当前帧距离上一关键帧时间戳超过0.25s，则说明需要插入关键帧，不再进行后续判断
if (mSensor == System::IMU_MONOCULAR && (mCurrentFrame.mTimeStamp-mpLastKeyFrame->mTimeStamp)>=0.25)
return true;
else if (mSensor == System::IMU_STEREO && (mCurrentFrame.mTimeStamp-mpLastKeyFrame->mTimeStamp)>=0.25)
return true;
else
else // 否则，则说明不需要插入关键帧，不再进行后续判断
return false;
}
// Step 1：纯VO模式下不插入关键帧
@ -3560,7 +3565,7 @@ bool Tracking::NeedNewKeyFrame()
thRefRatio = 0.9f;
if(mpCamera2) thRefRatio = 0.75f;
//单目+IMU情况下如果，匹配内点数目超过350，插入关键帧的频率可以适当降低
if(mSensor==System::IMU_MONOCULAR)
{
if(mnMatchesInliers>350) // Points tracked from the local map
@ -3577,13 +3582,14 @@ bool Tracking::NeedNewKeyFrame()
const bool c1b = ((mCurrentFrame.mnId>=mnLastKeyFrameId+mMinFrames) && bLocalMappingIdle);
//Condition 1c: tracking is weak
// Step 7.4：在双目，RGB-D的情况下当前帧跟踪到的点比参考关键帧的0.25倍还少，或者满足bNeedToInsertClose
const bool c1c = mSensor!=System::MONOCULAR && mSensor!=System::IMU_MONOCULAR && mSensor!=System::IMU_STEREO && //只考虑在双目，RGB-D的情况
const bool c1c = mSensor!=System::MONOCULAR && mSensor!=System::IMU_MONOCULAR && mSensor!=System::IMU_STEREO && //只考虑在纯双目，RGB-D的情况
(mnMatchesInliers<nRefMatches*0.25 || //当前帧和地图点匹配的数目非常少
bNeedToInsertClose) ; //需要插入
// Condition 2: Few tracked points compared to reference keyframe. Lots of visual odometry compared to map matches.
// Step 7.5：和参考帧相比当前跟踪到的点太少 或者满足bNeedToInsertClose；同时跟踪到的内点还不能太少
const bool c2 = (((mnMatchesInliers<nRefMatches*thRefRatio || bNeedToInsertClose)) && mnMatchesInliers>15);
// 新增的条件c3：单目/双目+IMU模式下，并且IMU完成了初始化（隐藏条件），当前帧和上一关键帧之间时间超过0.5秒，则c3=true
// Temporal condition for Inertial cases
bool c3 = false;
if(mpLastKeyFrame)
@ -3600,12 +3606,14 @@ bool Tracking::NeedNewKeyFrame()
}
}
// 新增的条件c4：单目+IMU模式下，当前帧匹配内点数在15~75之间或者是RECENTLY_LOST状态，c4=true
bool c4 = false;
if ((((mnMatchesInliers<75) && (mnMatchesInliers>15)) || mState==RECENTLY_LOST) && ((mSensor == System::IMU_MONOCULAR))) // MODIFICATION_2, originally ((((mnMatchesInliers<75) && (mnMatchesInliers>15)) || mState==RECENTLY_LOST) && ((mSensor == System::IMU_MONOCULAR)))
c4=true;
else
c4=false;
// 相比ORB-SLAM2多了c3,c4
if(((c1a||c1b||c1c) && c2)||c3 ||c4)
{
// If the mapping accepts keyframes, insert keyframe.
@ -3621,7 +3629,7 @@ bool Tracking::NeedNewKeyFrame()
mpLocalMapper->InterruptBA();
if(mSensor!=System::MONOCULAR && mSensor!=System::IMU_MONOCULAR)
{
// 队列里不能阻塞太多关键帧
// 双目或双目+IMU或RGB-D模式下，如队列里没有阻塞太多关键帧，可以插入
// tracking插入关键帧不是直接插入，而且先插入到mlNewKeyFrames中，
// 然后localmapper再逐个pop出来插入到mspKeyFrames
if(mpLocalMapper->KeyframesInQueue()<3)
@ -3963,6 +3971,7 @@ void Tracking::UpdateLocalKeyFrames()
// Each map point vote for the keyframes in which it has been observed
// Step 1：遍历当前帧的地图点，记录所有能观测到当前帧地图点的关键帧
map<KeyFrame*,int> keyframeCounter;
// 如果IMU未初始化 或者 刚刚完成重定位
if(!mpAtlas->isImuInitialized() || (mCurrentFrame.mnId<mnLastRelocFrameId+2))
{
for(int i=0; i<mCurrentFrame.N; i++)
@ -3991,6 +4000,7 @@ void Tracking::UpdateLocalKeyFrames()
}
else
{
// ?为什么IMU初始化后用mLastFrame？mLastFrame存储的是上一帧跟踪成功后帧数据。
for(int i=0; i<mLastFrame.N; i++)
{
// Using lastframe since current frame has not matches yet
@ -4112,6 +4122,7 @@ void Tracking::UpdateLocalKeyFrames()
}
// Add 10 last temporal KFs (mainly for IMU)
// IMU模式下增加了临时的关键帧
if((mSensor == System::IMU_MONOCULAR || mSensor == System::IMU_STEREO) &&mvpLocalKeyFrames.size()<80)
{
//cout << "CurrentKF: " << mCurrentFrame.mnId << endl;

10
src/TwoViewReconstruction.cc
View File

@ -1148,15 +1148,17 @@ int TwoViewReconstruction::CheckRT(const cv::Mat &R, const cv::Mat &t, const vec
if (cosParallax < 0.99998)
vbGood[vMatches12[i].first] = true;
}
// 步骤8：得到3D点中较大的视差角
// 7 得到3D点中较小的视差角，并且转换成为角度制表示
if (nGood > 0)
{
// 从小到大排序
// 从小到大排序，注意vCosParallax值越大，视差越小
sort(vCosParallax.begin(), vCosParallax.end());
// trick! 排序后并没有取最大的视差角
// 取一个较大的视差角
// !排序后并没有取最小的视差角，而是取一个较小的视差角
// 作者的做法：如果经过检验过后的有效3D点小于50个，那么就取最后那个最小的视差角(cos值最大)
// 如果大于50个，就取排名第50个的较小的视差角即可，为了避免3D点太多时出现太小的视差角
size_t idx = min(50, int(vCosParallax.size() - 1));
//将这个选中的角弧度制转换为角度制
parallax = acos(vCosParallax[idx]) * 180 / CV_PI;
}
else