diff --git a/include/KeyFrame.h b/include/KeyFrame.h index de9f00a..7580ad9 100644 --- a/include/KeyFrame.h +++ b/include/KeyFrame.h @@ -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; diff --git a/include/LocalMapping.h b/include/LocalMapping.h index e5a21ad..3c228e9 100644 --- a/include/LocalMapping.h +++ b/include/LocalMapping.h @@ -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; diff --git a/include/LoopClosing.h b/include/LoopClosing.h index 7ce6392..04b8dc0 100644 --- a/include/LoopClosing.h +++ b/include/LoopClosing.h @@ -179,7 +179,7 @@ protected: int mnLoopNumCoincidences; int mnLoopNumNotFound; KeyFrame* mpLoopLastCurrentKF; - g2o::Sim3 mg2oLoopSlw; + g2o::Sim3 ; g2o::Sim3 mg2oLoopScw; KeyFrame* mpLoopMatchedKF; std::vector mvpLoopMPs; diff --git a/include/Tracking.h b/include/Tracking.h index d932274..f416a13 100644 --- a/include/Tracking.h +++ b/include/Tracking.h @@ -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 mlRelativeFramePoses; list mlpReferences; list 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 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 &vpFs, float &bwx, float &bwy, float &bwz); void ComputeVelocitiesAccBias(const vector &vpFs, float &bax, float &bay, float &baz); @@ -230,14 +239,14 @@ protected: IMU::Preintegrated *mpImuPreintegratedFromLastKF; // Queue of IMU measurements between frames - std::list mlQueueImuData; + std::list mlQueueImuData; //存放两帧之间的IMU数据 // Vector of IMU measurements from previous to current frame (to be filled by PreintegrateIMU) std::vector 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 mlpTemporalPoints; + list mlpTemporalPoints; // 临时地图点 //int nMapChangeIndex; @@ -340,7 +350,7 @@ protected: double mTime_LocalMapTrack; double mTime_NewKF_Dec; - GeometricCamera* mpCamera, *mpCamera2; + GeometricCamera* mpCamera, *mpCamera2; //相机类 int initID, lastID; diff --git a/outline.png b/outline.png index 869022d..eb25edb 100644 Binary files a/outline.png and b/outline.png differ diff --git a/src/Frame.cc b/src/Frame.cc index 2d85bd3..11fd5cd 100644 --- a/src/Frame.cc +++ b/src/Frame.cc @@ -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 > vRowIndices(nRows,vector()); diff --git a/src/Initializer.cc b/src/Initializer.cc index 8665f1e..37c3fd9 100644 --- a/src/Initializer.cc +++ b/src/Initializer.cc @@ -1648,7 +1648,7 @@ int Initializer::CheckRT(const cv::Mat &R, const cv::Mat &t, const vector0) { // 从小到大排序,注意vCosParallax值越大,视差越小 diff --git a/src/KeyFrameDatabase.cc b/src/KeyFrameDatabase.cc index 3477488..ad6764c 100644 --- a/src/KeyFrameDatabase.cc +++ b/src/KeyFrameDatabase.cc @@ -691,7 +691,7 @@ void KeyFrameDatabase::DetectNBestCandidates(KeyFrame *pKF, vector &v // 如果该关键帧不是当前关键帧的共视关键帧 if(!spConnectedKF.count(pKFi)) { - // 标记改关键帧被当前关键帧访问到 + // 标记该关键帧被当前关键帧访问到(也就是有公共单词) pKFi->mnPlaceRecognitionQuery=pKF->mnId; // 把当前关键帧添加到有公共单词的关键帧列表中 lKFsSharingWords.push_back(pKFi); @@ -713,7 +713,7 @@ void KeyFrameDatabase::DetectNBestCandidates(KeyFrame *pKF, vector &v return; // Only compare against those keyframes that share enough words - // Step 2 统计所有候选帧中与当前关键帧的公共单词数最多的单词数,并筛选 + // Step 2 统计所有候选帧中与当前关键帧的公共单词数最多的单词数maxCommonWords,并筛选 int maxCommonWords=0; for(list::iterator lit=lKFsSharingWords.begin(), lend= lKFsSharingWords.end(); lit!=lend; lit++) { @@ -738,7 +738,7 @@ void KeyFrameDatabase::DetectNBestCandidates(KeyFrame *pKF, vector &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 &v for(vector::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 &v //for(list >::iterator it=lAccScoreAndMatch.begin(), itend=lAccScoreAndMatch.end(); it!=itend; it++) int i = 0; list >::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; diff --git a/src/LocalMapping.cc b/src/LocalMapping.cc index 7270cf9..c955e0c 100644 --- a/src/LocalMapping.cc +++ b/src/LocalMapping.cc @@ -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 >(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 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; diff --git a/src/LoopClosing.cc b/src/LoopClosing.cc index d976d19..4117531 100644 --- a/src/LoopClosing.cc +++ b/src/LoopClosing.cc @@ -43,7 +43,7 @@ LoopClosing::LoopClosing(Atlas *pAtlas, KeyFrameDatabase *pDB, ORBVocabulary *pV mpLastCurrentKF = static_cast(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 &vpBowCand, KeyFrame* &pMatchedKF2, KeyFrame* &pLastCurrentKF, g2o::Sim3 &g2oScw, int &nNumCoincidences, std::vector &vpMPs, std::vector &vpMatchedMPs) @@ -758,7 +761,7 @@ bool LoopClosing::DetectCommonRegionsFromBoW(std::vector &vpBowCand, int nNumGuidedMatching = 0; // Varibles to select the best numbe - // 一些用与统计最优数据的变量,我们最后返回的是最佳的一个关键帧(几何校验匹配数最高的) + // 一些用于统计最优数据的变量,我们最后返回的是最佳的一个关键帧(几何校验匹配数最高的) KeyFrame* pBestMatchedKF; int nBestMatchesReproj = 0; @@ -772,10 +775,9 @@ bool LoopClosing::DetectCommonRegionsFromBoW(std::vector &vpBowCand, // 这三个变量是作者为了后面打印观察记录的信息,可以忽略 vector vnStage(numCandidates, 0); vector vnMatchesStage(numCandidates, 0); - int index = 0; - //对于每个候选关键帧 + //对于每个候选关键帧,进行Sim3计算和检验 for(KeyFrame* pKFi : vpBowCand) { //cout << endl << "-------------------------------" << endl; @@ -811,7 +813,7 @@ bool LoopClosing::DetectCommonRegionsFromBoW(std::vector &vpBowCand, // 记录在W_km中有最多匹配点的帧的局部index, 这个后面没有用到 int nIndexMostBoWMatchesKF=0; - //! bug: 并没有重新赋值pMostBoWMatchesKF, 一直是初始值: 候选关键帧 + //! bug: 以下循环中并没有重新赋值pMostBoWMatchesKF, 一直是初始值: 候选关键帧 // 遍历窗口内Wm的每个关键帧 // Step 1.1 通过Bow寻找候选帧窗口内的关键帧地图点与当前关键帧的匹配点 for(int j=0; j &vpBowCand, for(int j=0; j &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 &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 &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 &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 vbInliers; @@ -962,17 +965,18 @@ bool LoopClosing::DetectCommonRegionsFromBoW(std::vector &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 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 &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 &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 候选帧 diff --git a/src/ORBmatcher.cc b/src/ORBmatcher.cc index 1a04d7c..a519327 100644 --- a/src/ORBmatcher.cc +++ b/src/ORBmatcher.cc @@ -42,6 +42,7 @@ ORBmatcher::ORBmatcher(float nnratio, bool checkOri): mfNNratio(nnratio), mbChec { } +// 用于Tracking::SearchLocalPoints中匹配更多地图点 int ORBmatcher::SearchByProjection(Frame &F, const vector &vpMapPoints, const float th, const bool bFarPoints, const float thFarPoints) { int nmatches=0, left = 0, right = 0; diff --git a/src/Optimizer.cc b/src/Optimizer.cc index e281cac..2d7a791 100644 --- a/src/Optimizer.cc +++ b/src/Optimizer.cc @@ -4304,7 +4304,7 @@ int Optimizer::OptimizeSim3(KeyFrame *pKF1, KeyFrame *pKF2, vector & * @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 vpAdju bool bShowImages = false; vector vpMPs; -// 1. 构建g2o优化器 + // 1. 构建g2o优化器 g2o::SparseOptimizer optimizer; g2o::BlockSolver_6_3::LinearSolverType * linearSolver; diff --git a/src/Tracking.cc b/src/Tracking.cc index 7679c2d..fc27771 100644 --- a/src/Tracking.cc +++ b/src/Tracking.cc @@ -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的情况 (mnMatchesInliers15); + // 新增的条件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 keyframeCounter; + // 如果IMU未初始化 或者 刚刚完成重定位 if(!mpAtlas->isImuInitialized() || (mCurrentFrame.mnId 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