fix some comments
electech6
parent
3fd2fcccb7
commit
a1174f1df8
|
|
@ -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;
|
||||
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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 ×tamp, string filename);
|
||||
cv::Mat GrabImageRGBD(const cv::Mat &imRGB,const cv::Mat &imD, const double ×tamp, string filename);
|
||||
cv::Mat GrabImageMonocular(const cv::Mat &im, const double ×tamp, string filename);
|
||||
// cv::Mat GrabImageImuMonocular(const cv::Mat &im, const double ×tamp);
|
||||
|
||||
// 放置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
BIN
outline.png
Binary file not shown.
|
Before Width: | Height: | Size: 915 KiB After Width: | Height: | Size: 112 KiB |
|
|
@ -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>());
|
||||
|
|
|
|||
|
|
@ -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值越大，视差越小
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
||||
|
|
|
|||
|
|
@ -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 候选帧
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
Loading…
Reference in New Issue