diff --git a/Examples/Monocular/mono_euroc.cc b/Examples/Monocular/mono_euroc.cc index eea5617..86fdf89 100644 --- a/Examples/Monocular/mono_euroc.cc +++ b/Examples/Monocular/mono_euroc.cc @@ -55,7 +55,7 @@ int main(int argc, char **argv) } // Create SLAM system. It initializes all system threads and gets ready to process frames. - ORB_SLAM2::System SLAM(argv[1],argv[2],ORB_SLAM2::System::MONOCULAR,true, true, false, false); + ORB_SLAM2::System SLAM(argv[1],argv[2],ORB_SLAM2::System::MONOCULAR,true, true, false, false, false); // Vector for tracking time statistics vector vTimesTrack; diff --git a/Examples/Monocular/mono_kitti.cc b/Examples/Monocular/mono_kitti.cc index 97fa83f..c02a553 100644 --- a/Examples/Monocular/mono_kitti.cc +++ b/Examples/Monocular/mono_kitti.cc @@ -50,7 +50,7 @@ int main(int argc, char **argv) int nImages = vstrImageFilenames.size(); // Create SLAM system. It initializes all system threads and gets ready to process frames. - ORB_SLAM2::System SLAM(argv[1],argv[2],ORB_SLAM2::System::MONOCULAR,true, true, true, false); + ORB_SLAM2::System SLAM(argv[1],argv[2],ORB_SLAM2::System::MONOCULAR,true, true, true, false, false); // Vector for tracking time statistics vector vTimesTrack; diff --git a/Examples/Monocular/mono_tum.cc b/Examples/Monocular/mono_tum.cc index 09d7e72..7c4190c 100644 --- a/Examples/Monocular/mono_tum.cc +++ b/Examples/Monocular/mono_tum.cc @@ -50,7 +50,7 @@ int main(int argc, char **argv) int nImages = vstrImageFilenames.size(); // Create SLAM system. It initializes all system threads and gets ready to process frames. - ORB_SLAM2::System SLAM(argv[1],argv[2],ORB_SLAM2::System::MONOCULAR,true, true, true, false); + ORB_SLAM2::System SLAM(argv[1],argv[2],ORB_SLAM2::System::MONOCULAR,true, true, true, false, false); // Vector for tracking time statistics vector vTimesTrack; diff --git a/Examples/RGB-D/rgbd_tum.cc b/Examples/RGB-D/rgbd_tum.cc index 0c9671b..1ed1110 100644 --- a/Examples/RGB-D/rgbd_tum.cc +++ b/Examples/RGB-D/rgbd_tum.cc @@ -62,7 +62,7 @@ int main(int argc, char **argv) } // Create SLAM system. It initializes all system threads and gets ready to process frames. - ORB_SLAM2::System SLAM(argv[1],argv[2],ORB_SLAM2::System::RGBD, true, true, false, false); //viewer localmap, loop,trackonly + ORB_SLAM2::System SLAM(argv[1],argv[2],ORB_SLAM2::System::RGBD, true, false, false, false, false); //viewer, localmap, loop, bow, trackonly // Vector for tracking time statistics vector vTimesTrack; diff --git a/Examples/Stereo/stereo_euroc.cc b/Examples/Stereo/stereo_euroc.cc index 425ed7b..9f4bbd8 100644 --- a/Examples/Stereo/stereo_euroc.cc +++ b/Examples/Stereo/stereo_euroc.cc @@ -101,7 +101,7 @@ int main(int argc, char **argv) const int nImages = vstrImageLeft.size(); // Create SLAM system. It initializes all system threads and gets ready to process frames. - ORB_SLAM2::System SLAM(argv[1],argv[2],ORB_SLAM2::System::STEREO,true,true,true,false); + ORB_SLAM2::System SLAM(argv[1],argv[2],ORB_SLAM2::System::STEREO,true,true,true,false,false); // Vector for tracking time statistics vector vTimesTrack; diff --git a/Examples/Stereo/stereo_kitti.cc b/Examples/Stereo/stereo_kitti.cc index 2e293d6..29d8603 100644 --- a/Examples/Stereo/stereo_kitti.cc +++ b/Examples/Stereo/stereo_kitti.cc @@ -51,7 +51,7 @@ int main(int argc, char **argv) const int nImages = vstrImageLeft.size(); // Create SLAM system. It initializes all system threads and gets ready to process frames. - ORB_SLAM2::System SLAM(argv[1],argv[2],ORB_SLAM2::System::STEREO,true,true,true,false); + ORB_SLAM2::System SLAM(argv[1],argv[2],ORB_SLAM2::System::STEREO,true,true,true,false,false); // Vector for tracking time statistics vector vTimesTrack; diff --git a/eval_script/png/1_room_ATE_SP_1.png b/eval_script/png/1_room_ATE_SP_1.png new file mode 100644 index 0000000..bdf7eb3 Binary files /dev/null and b/eval_script/png/1_room_ATE_SP_1.png differ diff --git a/eval_script/png/1_room_RPE_SP_1.png b/eval_script/png/1_room_RPE_SP_1.png new file mode 100644 index 0000000..70fb3bc Binary files /dev/null and b/eval_script/png/1_room_RPE_SP_1.png differ diff --git a/eval_script/txt/ATE.txt b/eval_script/txt/ATE.txt new file mode 100644 index 0000000..2ca7e37 --- /dev/null +++ b/eval_script/txt/ATE.txt @@ -0,0 +1,13 @@ +absolute_translational_error.rmse 0.132134 mabsolute_translational_error.mean 0.114728 mabsolute_translational_error.median 0.094413 mabsolute_translational_error.std 0.065551 m +absolute_translational_error.rmse 0.132134 mabsolute_translational_error.mean 0.114728 mabsolute_translational_error.median 0.094413 mabsolute_translational_error.std 0.065551 m +absolute_translational_error.rmse 0.132134 mabsolute_translational_error.mean 0.114728 mabsolute_translational_error.median 0.094413 mabsolute_translational_error.std 0.065551 m +absolute_translational_error.rmse 0.132134 mabsolute_translational_error.mean 0.114728 mabsolute_translational_error.median 0.094413 mabsolute_translational_error.std 0.065551 m +absolute_translational_error.rmse 0.132134 mabsolute_translational_error.mean 0.114728 mabsolute_translational_error.median 0.094413 mabsolute_translational_error.std 0.065551 m +absolute_translational_error.rmse 0.132134 mabsolute_translational_error.mean 0.114728 mabsolute_translational_error.median 0.094413 mabsolute_translational_error.std 0.065551 m +absolute_translational_error.rmse 0.234038 mabsolute_translational_error.mean 0.214413 mabsolute_translational_error.median 0.190113 mabsolute_translational_error.std 0.093813 m +absolute_translational_error.rmse 0.219648 mabsolute_translational_error.mean 0.201928 mabsolute_translational_error.median 0.203248 mabsolute_translational_error.std 0.086431 m +absolute_translational_error.rmse 0.210300 mabsolute_translational_error.mean 0.189854 mabsolute_translational_error.median 0.171276 mabsolute_translational_error.std 0.090452 m +absolute_translational_error.rmse 0.235712 mabsolute_translational_error.mean 0.214860 mabsolute_translational_error.median 0.193107 mabsolute_translational_error.std 0.096929 m +absolute_translational_error.rmse 0.279557 mabsolute_translational_error.mean 0.253273 mabsolute_translational_error.median 0.244458 mabsolute_translational_error.std 0.118343 m +absolute_translational_error.rmse 0.242542 mabsolute_translational_error.mean 0.220632 mabsolute_translational_error.median 0.204323 mabsolute_translational_error.std 0.100738 m +absolute_translational_error.rmse 0.212963 mabsolute_translational_error.mean 0.194018 mabsolute_translational_error.median 0.169772 mabsolute_translational_error.std 0.087808 m diff --git a/eval_script/txt/rpe.txt b/eval_script/txt/rpe.txt new file mode 100644 index 0000000..406b05c --- /dev/null +++ b/eval_script/txt/rpe.txt @@ -0,0 +1,13 @@ +translational_error.rmse 0.038629 translational_error.mean 0.030651 translational_error.median 0.024689 translational_error.std 0.023510 rotational_error.rmse 1.805696 rotational_error.mean 1.574448 rotational_error.median 1.464517 rotational_error.std 0.884110 +translational_error.rmse 0.038629 translational_error.mean 0.030651 translational_error.median 0.024689 translational_error.std 0.023510 rotational_error.rmse 1.805696 rotational_error.mean 1.574448 rotational_error.median 1.464517 rotational_error.std 0.884110 +translational_error.rmse 0.038629 translational_error.mean 0.030651 translational_error.median 0.024689 translational_error.std 0.023510 rotational_error.rmse 1.805696 rotational_error.mean 1.574448 rotational_error.median 1.464517 rotational_error.std 0.884110 +translational_error.rmse 0.038629 translational_error.mean 0.030651 translational_error.median 0.024689 translational_error.std 0.023510 rotational_error.rmse 1.805696 rotational_error.mean 1.574448 rotational_error.median 1.464517 rotational_error.std 0.884110 +translational_error.rmse 0.038629 translational_error.mean 0.030651 translational_error.median 0.024689 translational_error.std 0.023510 rotational_error.rmse 1.805696 rotational_error.mean 1.574448 rotational_error.median 1.464517 rotational_error.std 0.884110 +translational_error.rmse 0.038629 translational_error.mean 0.030651 translational_error.median 0.024689 translational_error.std 0.023510 rotational_error.rmse 1.805696 rotational_error.mean 1.574448 rotational_error.median 1.464517 rotational_error.std 0.884110 +translational_error.rmse 0.058719 translational_error.mean 0.050650 translational_error.median 0.044497 translational_error.std 0.029707 rotational_error.rmse 2.297889 rotational_error.mean 2.104660 rotational_error.median 2.038534 rotational_error.std 0.922334 +translational_error.rmse 0.049866 translational_error.mean 0.040282 translational_error.median 0.034538 translational_error.std 0.029395 rotational_error.rmse 1.998709 rotational_error.mean 1.791138 rotational_error.median 1.727023 rotational_error.std 0.886941 +translational_error.rmse 0.046899 translational_error.mean 0.038322 translational_error.median 0.032393 translational_error.std 0.027036 rotational_error.rmse 1.994059 rotational_error.mean 1.776784 rotational_error.median 1.636145 rotational_error.std 0.905159 +translational_error.rmse 0.045942 translational_error.mean 0.038221 translational_error.median 0.031950 translational_error.std 0.025491 rotational_error.rmse 2.045922 rotational_error.mean 1.778760 rotational_error.median 1.561137 rotational_error.std 1.010848 +translational_error.rmse 0.051602 translational_error.mean 0.042306 translational_error.median 0.035588 translational_error.std 0.029545 rotational_error.rmse 2.233656 rotational_error.mean 1.911310 rotational_error.median 1.687597 rotational_error.std 1.155905 +translational_error.rmse 0.051635 translational_error.mean 0.040043 translational_error.median 0.029454 translational_error.std 0.032600 rotational_error.rmse 2.059074 rotational_error.mean 1.791552 rotational_error.median 1.621788 rotational_error.std 1.014952 +translational_error.rmse 0.042123 translational_error.mean 0.034934 translational_error.median 0.030233 translational_error.std 0.023536 rotational_error.rmse 1.934358 rotational_error.mean 1.712831 rotational_error.median 1.579578 rotational_error.std 0.898860 diff --git a/include/Frame.h b/include/Frame.h index 049c1ba..6a04c69 100644 --- a/include/Frame.h +++ b/include/Frame.h @@ -53,10 +53,13 @@ class Frame // Constructor for RGB-D cameras. Frame(const cv::Mat &imGray, const cv::Mat &imDepth, const double &timeStamp, ORBextractor* extractor,ORBVocabulary* voc, cv::Mat &K, cv::Mat &distCoef, const float &bf, const float &thDepth); - //==========================================// + //zoe 20181016 Frame(const cv::Mat &imGray, const cv::Mat &imDepth, const double &timeStamp, ORBextractor* extractor,LFNETVocabulary* voclfnet, cv::Mat &K, cv::Mat &distCoef, const float &bf, const float &thDepth); - //============================================// + + //zoe 20190520 + Frame(const cv::Mat &imGray, const cv::Mat &imDepth, const double &timeStamp, cv::Mat &K, cv::Mat &distCoef, const float &bf, const float &thDepth); + // Constructor for Monocular cameras. Frame(const cv::Mat &imGray, const double &timeStamp, ORBextractor* extractor,ORBVocabulary* voc, cv::Mat &K, cv::Mat &distCoef, const float &bf, const float &thDepth); diff --git a/include/KeyFrame.h b/include/KeyFrame.h index bcb8947..e0e8a90 100644 --- a/include/KeyFrame.h +++ b/include/KeyFrame.h @@ -44,6 +44,7 @@ class KeyFrame { public: KeyFrame(Frame &F, Map* pMap, KeyFrameDatabase* pKFDB); + KeyFrame(Frame &F, Map* pMap);// zoe 20190520 // Pose functions void SetPose(const cv::Mat &Tcw); diff --git a/include/ORBmatcher.h b/include/ORBmatcher.h index 7a4ee84..f3e97ab 100644 --- a/include/ORBmatcher.h +++ b/include/ORBmatcher.h @@ -67,6 +67,9 @@ class ORBmatcher // Used in Relocalisation and Loop Detection int SearchByBoWLFNet(KeyFrame *pKF, Frame &F, std::vector &vpMapPointMatches); int SearchByBoWLFNet(KeyFrame *pKF1, KeyFrame* pKF2, std::vector &vpMatches12); + // zoe 20190522 + int SearchByBFLFNet(KeyFrame *pKF, Frame &F, std::vector &vpMapPointMatches); + int SearchByBFLFNet(KeyFrame *pKF1, KeyFrame* pKF2, std::vector &vpMatches12); // Matching for the Map Initialization (only used in the monocular case) int SearchForInitializationLFNet(Frame &F1, Frame &F2, std::vector &vbPrevMatched, std::vector &vnMatches12, int windowSize=10); diff --git a/include/System.h b/include/System.h index 02d5780..b8e695d 100644 --- a/include/System.h +++ b/include/System.h @@ -59,7 +59,7 @@ class System public: // Initialize the SLAM system. It launches the Local Mapping, Loop Closing and Viewer threads. - System(const string &strVocFile, const string &strSettingsFile, const eSensor sensor, const bool bUseViewer = true, const bool bUseLocalMap = true, const bool bUseLoop = true, const bool bOnlyTracking = false); + System(const string &strVocFile, const string &strSettingsFile, const eSensor sensor, const bool bUseViewer = true, const bool bUseLocalMap = true, const bool bUseLoop = true, const bool bUseBoW = false, const bool bOnlyTracking = false); // Proccess the given stereo frame. Images must be synchronized and rectified. // Input images: RGB (CV_8UC3) or grayscale (CV_8U). RGB is converted to grayscale. @@ -169,6 +169,8 @@ class System //zoe 20190511 bool mbUseLocalMap; bool mbUseLoop; + // zoe 20190522 + bool mbUseBoW; // zoe 20190513 bool mbOnlyTracking; diff --git a/include/Tracking.h b/include/Tracking.h index 220df13..e754960 100644 --- a/include/Tracking.h +++ b/include/Tracking.h @@ -56,11 +56,15 @@ class Tracking public: Tracking(System* pSys, ORBVocabulary* pVoc, FrameDrawer* pFrameDrawer, MapDrawer* pMapDrawer, Map* pMap, KeyFrameDatabase* pKFDB, const string &strSettingPath, const int sensor, const bool bOnlyTracking);//zoe 20190513 增加最后一个参数 - //======================================================// + //zoe 20181016 Tracking(System* pSys, LFNETVocabulary* pVocLFNet, FrameDrawer* pFrameDrawer, MapDrawer* pMapDrawer, Map* pMap, KeyFrameDatabase* pKFDB, const string &strSettingPath, const int sensor, const bool bOnlyTracking);// zoe 20190513 增加最后一个参数 - //======================================================// + + //zoe 20190520 + Tracking(System* pSys, FrameDrawer* pFrameDrawer, MapDrawer* pMapDrawer, Map* pMap, + const string &strSettingPath, const int sensor, const bool bOnlyTracking); + // Preprocess the input and call Track(). Extract features and performs stereo matching. cv::Mat GrabImageStereo(const cv::Mat &imRectLeft,const cv::Mat &imRectRight, const double ×tamp); cv::Mat GrabImageRGBD(const cv::Mat &imRGB,const cv::Mat &imD, const double ×tamp); diff --git a/src/Frame.cc b/src/Frame.cc index bf6a6ff..717b5ea 100644 --- a/src/Frame.cc +++ b/src/Frame.cc @@ -1,4 +1,4 @@ -/** + /** * This file is part of ORB-SLAM2. * * Copyright (C) 2014-2016 Raúl Mur-Artal (University of Zaragoza) @@ -359,6 +359,155 @@ Frame::Frame(const cv::Mat &imGray, const cv::Mat &imDepth, const double &timeSt AssignFeaturesToGridLFNet(); } +//zoe 20190520 +Frame::Frame(const cv::Mat &imGray, const cv::Mat &imDepth, const double &timeStamp, cv::Mat &K, cv::Mat &distCoef, const float &bf, const float &thDepth) + :mpLFNETvocabulary(static_cast(NULL)),mpORBextractorLeft(static_cast(NULL)),mpORBextractorRight(static_cast(NULL)), + mTimeStamp(timeStamp), mK(K.clone()),mDistCoef(distCoef.clone()), mbf(bf), mThDepth(thDepth) +{ + // Frame ID + mnId=nNextId++; + + // Scale Level Info + mfScaleFactor = 1.0f;//mpORBextractorLeft->GetScaleFactor(); + mfLogScaleFactor = log(mfScaleFactor); + + // ORB extraction + //ExtractORB(0,imGray); + //cout << "ORB feats : " << mDescriptors << endl; + + string strLFNetPath = "/home/yuchao/Data/rgbd_dataset_freiburg1_room/SP/"; + int kpts_num = 1000;//超参数,可以根据情况修改 每张照片提取特征点的个数 1000个,目前是跟ORB本身的个数是一样的. + mnScaleLevels = 5; //超参数 + float Scale[mnScaleLevels] = {1.41421356, 1.18920712, 1.0, 0.84089642, 0.70710678}; + + mvScaleFactors.resize(mnScaleLevels); + mvLevelSigma2.resize(mnScaleLevels); + mvScaleFactors[0]=1.0f; + mvLevelSigma2[0]=1.0f; + for(int i=1; i umax; + umax.resize(HALF_PATCH_SIZE_SP + 1); + + int v, v0, vmax = cvFloor(HALF_PATCH_SIZE_SP * sqrt(2.f) / 2 + 1); + int vmin = cvCeil(HALF_PATCH_SIZE_SP * sqrt(2.f) / 2); + const double hp2 = HALF_PATCH_SIZE_SP*HALF_PATCH_SIZE_SP; + for (v = 0; v <= vmax; ++v) + umax[v] = cvRound(sqrt(hp2 - v * v)); + + // Make sure we are symmetric + for (v = HALF_PATCH_SIZE_SP, v0 = 0; v >= vmin; --v) + { + while (umax[v0] == umax[v0 + 1]) + ++v0; + umax[v] = v0; + ++v0; + } + + float fkpts[kpts_num][2] = { 0 };//定义一个1000*2的矩阵，用于存放kpts数据 + float ffeats[kpts_num][256] = { 0 };//定义一个1000*256的矩阵，用于存放feats数据 + //float scale;//暂存尺度 + string strTimeStamp = to_string(mTimeStamp); + ifstream kptsfile;//定义读取文件流，相对于程序来说是in + ifstream featsfile;//定义读取文件流，相对于程序来说是in + //ifstream kptsorifile;//定义读取文件流，相对于程序来说是in + //ifstream kptsscalefile;//定义读取文件流，相对于程序来说是in + kptsfile.open(strLFNetPath + strTimeStamp + "_kpts.txt");//打开文件 + featsfile.open(strLFNetPath + strTimeStamp + "_feats.txt");//打开文件 + //kptsorifile.open(strLFNetPath + strTimeStamp + "_kpts_ori.txt");//打开文件 + //kptsscalefile.open(strLFNetPath + strTimeStamp + "_kpts_scale.txt");//打开文件 + + mvKpts.resize(kpts_num);//reserve 不行 + mvDspts.clear(); + + for (int i = 0; i < kpts_num; i++)//定义行循环 + { + for (int j = 0; j < 2; j++)//定义列循环 + { + kptsfile >> fkpts[i][j];//读取一个值（空格、制表符、换行隔开）就写入到矩阵中，行列不断循环进行 + //std::cout<< fkpts[i][j] << std::endl; + } + mvKpts[i].pt.x = fkpts[i][0]; + mvKpts[i].pt.y = fkpts[i][1]; + mvKpts[i].angle = SP_Angle(imGray, mvKpts[i], umax); //20190511 zoe + //kptsscalefile >> scale; + //for (int k = 0; k < mnScaleLevels; k++ ) + //{ + // if(abs(scale-Scale[k]) < 0.05 ) + // mvKpts[i].octave = k; + //} + mvKpts[i].octave = 0; + + //cout << "LFNet fkpts : " << mvKpts[i].pt << endl; + std::vector dspt; + dspt.resize(256); + for (int j = 0; j < 256; j++)//定义列循环 + { + featsfile >> ffeats[i][j];//读取一个值（空格、制表符、换行隔开）就写入到矩阵中，行列不断循环进行 + if(ffeats[i][j] < -0.5) + dspt[j] = -0.5;//0 + else if(ffeats[i][j] > 0.5) + dspt[j] = 0.5;//255 + else + dspt[j] = ffeats[i][j]; + + } + mvDspts.push_back(dspt); + } + kptsfile.close();//读取完成之后关闭文件 + featsfile.close();//读取完成之后关闭文件 + //kptsorifile.close();//读取完成之后关闭文件 + //kptsscalefile.close();//读取完成之后关闭文件 + + // 以下为完成相同的功能,仿照函数功能新写一些函数 + + N = mvKpts.size();//在这里输出过ORB特征点的数量 >=1000,不会少于1000的 + if(mvKpts.empty()) + return; + + UndistortKeyPointsLFNet(); + + ComputeStereoFromRGBDLFNet(imDepth); + + mvpMapPoints = vector(N,static_cast(NULL)); + mvbOutlier = vector(N,false); + // This is done only for the first Frame (or after a change in the calibration) + if(mbInitialComputations) + { + ComputeImageBounds(imGray); + + mfGridElementWidthInv=static_cast(FRAME_GRID_COLS)/static_cast(mnMaxX-mnMinX); + mfGridElementHeightInv=static_cast(FRAME_GRID_ROWS)/static_cast(mnMaxY-mnMinY); + + fx = K.at(0,0); + fy = K.at(1,1); + cx = K.at(0,2); + cy = K.at(1,2); + invfx = 1.0f/fx; + invfy = 1.0f/fy; + + mbInitialComputations=false; + } + + mb = mbf/fx; + + AssignFeaturesToGridLFNet(); +} Frame::Frame(const cv::Mat &imGray, const double &timeStamp, ORBextractor* extractor,ORBVocabulary* voc, cv::Mat &K, cv::Mat &distCoef, const float &bf, const float &thDepth) :mpORBvocabulary(voc),mpORBextractorLeft(extractor),mpORBextractorRight(static_cast(NULL)), @@ -617,7 +766,7 @@ void Frame::ComputeBoW() //zoe 20181016 void Frame::ComputeBoWLFNet() { - if(mBowVec.empty()) + if(mBowVec.empty() && mpLFNETvocabulary) { //vector vCurrentDesc = Converter::toDescriptorVector(mDescriptors); mpLFNETvocabulary->transform(mvDspts,mBowVec,mFeatVec,4); diff --git a/src/KeyFrame.cc b/src/KeyFrame.cc index 58c716c..bd6a267 100644 --- a/src/KeyFrame.cc +++ b/src/KeyFrame.cc @@ -55,6 +55,34 @@ KeyFrame::KeyFrame(Frame &F, Map *pMap, KeyFrameDatabase *pKFDB): SetPose(F.mTcw); } +// zoe 20190520 +KeyFrame::KeyFrame(Frame &F, Map *pMap): + mnFrameId(F.mnId), mTimeStamp(F.mTimeStamp), mnGridCols(FRAME_GRID_COLS), mnGridRows(FRAME_GRID_ROWS), + mfGridElementWidthInv(F.mfGridElementWidthInv), mfGridElementHeightInv(F.mfGridElementHeightInv), + mnTrackReferenceForFrame(0), mnFuseTargetForKF(0), mnBALocalForKF(0), mnBAFixedForKF(0), + mnLoopQuery(0), mnLoopWords(0), mnRelocQuery(0), mnRelocWords(0), mnBAGlobalForKF(0), + fx(F.fx), fy(F.fy), cx(F.cx), cy(F.cy), invfx(F.invfx), invfy(F.invfy), + mbf(F.mbf), mb(F.mb), mThDepth(F.mThDepth), N(F.N), mvKeys(F.mvKeys), mvKeysUn(F.mvKeysUn),mvKpts(F.mvKpts), mvKptsUn(F.mvKptsUn),//zoe 20181016 + mvuRight(F.mvuRight), mvDepth(F.mvDepth), mDescriptors(F.mDescriptors.clone()), mvDspts(F.mvDspts),//zoe 20181019 + mBowVec(F.mBowVec), mFeatVec(F.mFeatVec), mnScaleLevels(F.mnScaleLevels), mfScaleFactor(F.mfScaleFactor), + mfLogScaleFactor(F.mfLogScaleFactor), mvScaleFactors(F.mvScaleFactors), mvLevelSigma2(F.mvLevelSigma2), + mvInvLevelSigma2(F.mvInvLevelSigma2), mnMinX(F.mnMinX), mnMinY(F.mnMinY), mnMaxX(F.mnMaxX), + mnMaxY(F.mnMaxY), mK(F.mK), mvpMapPoints(F.mvpMapPoints), mpKeyFrameDB(static_cast(NULL)), + mpORBvocabulary(F.mpORBvocabulary),mpLFNETvocabulary(F.mpLFNETvocabulary), mbFirstConnection(true), mpParent(NULL), mbNotErase(false),//zoe 20181016 + mbToBeErased(false), mbBad(false), mHalfBaseline(F.mb/2), mpMap(pMap) +{ + mnId=nNextId++; + + mGrid.resize(mnGridCols); + for(int i=0; i vCurrentDesc = Converter::toDescriptorVector(mDescriptors); // Feature vector associate features with nodes in the 4th level (from leaves up) diff --git a/src/ORBmatcher.cc b/src/ORBmatcher.cc index 4ee830f..704caa9 100644 --- a/src/ORBmatcher.cc +++ b/src/ORBmatcher.cc @@ -20,14 +20,14 @@ #include "ORBmatcher.h" -#include +#include -#include -#include +#include +#include #include "Thirdparty/DBoW2/DBoW2/FeatureVector.h" -#include +#include using namespace std; @@ -485,6 +485,107 @@ int ORBmatcher::SearchByBoWLFNet(KeyFrame* pKF,Frame &F, vector &vpMa } + if(mbCheckOrientation) + { + int ind1=-1; + int ind2=-1; + int ind3=-1; + + ComputeThreeMaxima(rotHist,HISTO_LENGTH,ind1,ind2,ind3); + + for(int i=0; i(NULL); + nmatches--; + } + } + } + + return nmatches; +} +// zoe 20190522 +int ORBmatcher::SearchByBFLFNet(KeyFrame* pKF,Frame &F, vector &vpMapPointMatches) +{ + const vector vpMapPointsKF = pKF->GetMapPointMatches(); + + vpMapPointMatches = vector(F.N,static_cast(NULL)); + + int nmatches=0; + + vector rotHist[HISTO_LENGTH]; + for(int i=0;imvDspts.size(); iKF++) + { + + MapPoint* pMP = vpMapPointsKF[iKF]; + + if(!pMP) + continue; + + if(pMP->isBad()) + continue; + + const std::vector &dKF= pKF->mvDspts[iKF]; + + float bestDist1 = FLOAT_MAX; + int bestIdxF =-1 ; + float bestDist2 = FLOAT_MAX; + + for(size_t iF=0; iF &dF = F.mvDspts[iF]; + + const float dist = DescriptorDistanceLFNet_float(dKF, dF); + + if(dist(bestDist1)(bestDist2)) + { + vpMapPointMatches[bestIdxF]=pMP; + + const cv::KeyPoint &kp = pKF->mvKptsUn[iKF];//zoe 20181018 + + if(mbCheckOrientation) + { + float rot = kp.angle-F.mvKpts[bestIdxF].angle;//zoe 20181018 + if(rot<0.0) + rot+=360.0f; + int bin = round(rot*factor); + if(bin==HISTO_LENGTH) + bin=0; + assert(bin>=0 && bin &vpMatches12) +{ + const vector &vKptsUn1 = pKF1->mvKptsUn; + const vector vpMapPoints1 = pKF1->GetMapPointMatches(); + const std::vector> &Descriptors1 = pKF1->mvDspts; + + const vector &vKptsUn2 = pKF2->mvKptsUn; + const vector vpMapPoints2 = pKF2->GetMapPointMatches(); + const std::vector> &Descriptors2 = pKF2->mvDspts; + + vpMatches12 = vector(vpMapPoints1.size(),static_cast(NULL)); + vector vbMatched2(vpMapPoints2.size(),false); + + vector rotHist[HISTO_LENGTH]; + for(int i=0;iisBad()) + continue; + + const std::vector &d1 = Descriptors1[i1]; + + float bestDist1 = FLOAT_MAX; + int bestIdx2 =-1 ; + float bestDist2 = FLOAT_MAX; + + for(size_t i2=0, iend2=Descriptors2.size(); i2isBad()) + continue; + + const std::vector &d2 = Descriptors2[i2]; + + float distLFNet = DescriptorDistanceLFNet_float(d1,d2); + + if(distLFNet(bestDist1)(bestDist2)) + { + vpMatches12[i1]=vpMapPoints2[bestIdx2]; + vbMatched2[bestIdx2]=true; + + if(mbCheckOrientation) + { + float rot = vKptsUn1[i1].angle-vKptsUn2[bestIdx2].angle; + if(rot<0.0) + rot+=360.0f; + int bin = round(rot*factor); + if(bin==HISTO_LENGTH) + bin=0; + assert(bin>=0 && bin(NULL); + nmatches--; + } + } + } + + return nmatches; +} int ORBmatcher::SearchForTriangulation(KeyFrame *pKF1, KeyFrame *pKF2, cv::Mat F12, vector > &vMatchedPairs, const bool bOnlyStereo) diff --git a/src/System.cc b/src/System.cc index d877f61..1c118ee 100644 --- a/src/System.cc +++ b/src/System.cc @@ -30,8 +30,8 @@ namespace ORB_SLAM2 { //zoe 20190513 System::System(const string &strVocFile, const string &strSettingsFile, const eSensor sensor, - const bool bUseViewer, const bool bUseLocalMap, const bool bUseLoop, const bool bOnlyTracking):mSensor(sensor), mpViewer(static_cast(NULL)),mpLocalMapper(static_cast(NULL)),mpLoopCloser(static_cast(NULL)), mbReset(false),mbActivateLocalizationMode(false), - mbDeactivateLocalizationMode(false), mbUseLocalMap(bUseLocalMap), mbUseLoop(bUseLoop),mbOnlyTracking(bOnlyTracking) + const bool bUseViewer, const bool bUseLocalMap, const bool bUseLoop, const bool bUseBow, const bool bOnlyTracking):mSensor(sensor), mpViewer(static_cast(NULL)),mpLocalMapper(static_cast(NULL)),mpLoopCloser(static_cast(NULL)), mbReset(false),mbActivateLocalizationMode(false), + mbDeactivateLocalizationMode(false), mbUseLocalMap(bUseLocalMap), mbUseLoop(bUseLoop), mbUseBoW(bUseBow), mbOnlyTracking(bOnlyTracking) { // Output welcome message cout << endl << @@ -56,28 +56,13 @@ System::System(const string &strVocFile, const string &strSettingsFile, const eS cerr << "Failed to open settings file at: " << strSettingsFile << endl; exit(-1); } + // turn off the loop or not //zoe 20190511 cout << "Loop Mapping is set: " << mbUseLocalMap << endl; cout << "Loop Closing is set: " << mbUseLoop << endl; + cout << "Use BoW is set: " << mbUseBoW << endl; cout << "Only Track is set: " << mbOnlyTracking << endl; - //Load Vocabulary - cout << endl << "Loading Vocabulary. This could take a while..." << endl; - - //zoe 20181016 - mpVocabularyLFNet = new LFNETVocabulary(); - bool bVocLoadLFNet = mpVocabularyLFNet->loadFromTextFile(strVocFile); - if(!bVocLoadLFNet) - { - cerr << "Wrong path to vocabulary. " << endl; - cerr << "Falied to open at: " << strVocFile << endl; - exit(-1); - } - cout << "Vocabulary loaded!" << endl << endl; - - //Create KeyFrame Database - mpKeyFrameDatabase = new KeyFrameDatabase(*mpVocabularyLFNet);// zoe database名字没改 - //Create the Map mpMap = new Map(); @@ -85,12 +70,32 @@ System::System(const string &strVocFile, const string &strSettingsFile, const eS mpFrameDrawer = new FrameDrawer(mpMap); mpMapDrawer = new MapDrawer(mpMap, strSettingsFile); - //Initialize the Tracking thread - //(it will live in the main thread of execution, the one that called this constructor) - //zoe 20181016 track名字没改 - mpTracker = new Tracking(this, mpVocabularyLFNet, mpFrameDrawer, mpMapDrawer, - mpMap, mpKeyFrameDatabase, strSettingsFile, mSensor, mbOnlyTracking); //zoe 20190513 增加tracking参数 + if (mbUseBoW) + { + //Load Vocabulary + cout << endl << "Loading Vocabulary. This could take a while..." << endl; + + //zoe 20181016 + mpVocabularyLFNet = new LFNETVocabulary(); + bool bVocLoadLFNet = mpVocabularyLFNet->loadFromTextFile(strVocFile); + if(!bVocLoadLFNet) + { + cerr << "Wrong path to vocabulary. " << endl; + cerr << "Falied to open at: " << strVocFile << endl; + exit(-1); + } + cout << "Vocabulary loaded!" << endl << endl; + //Create KeyFrame Database + mpKeyFrameDatabase = new KeyFrameDatabase(*mpVocabularyLFNet);// zoe database名字没改 + + mpTracker = new Tracking(this, mpVocabularyLFNet, mpFrameDrawer, mpMapDrawer, + mpMap, mpKeyFrameDatabase, strSettingsFile, mSensor, mbOnlyTracking); //zoe 20190513 增加tracking参数 + } + else + { + mpTracker = new Tracking(this, mpFrameDrawer, mpMapDrawer, mpMap, strSettingsFile, mSensor, mbOnlyTracking); //zoe 20190520 + } //Initialize the Local Mapping thread and launch if (mbUseLocalMap) { @@ -100,7 +105,7 @@ System::System(const string &strVocFile, const string &strSettingsFile, const eS //Initialize the Loop Closing thread and launch //zoe 20190511 关闭回环检测 - if (mbUseLoop) + if (mbUseBoW && mbUseLoop) { //zoe 20181016 mpLoopCloser = new LoopClosing(mpMap, mpKeyFrameDatabase, mpVocabularyLFNet, mSensor!=MONOCULAR); @@ -124,7 +129,7 @@ System::System(const string &strVocFile, const string &strSettingsFile, const eS } //zoe 20190511 - if (mbUseLoop) + if (mbUseBoW && mbUseLoop) { mpTracker->SetLoopClosing(mpLoopCloser); mpLocalMapper->SetLoopCloser(mpLoopCloser); @@ -233,12 +238,12 @@ cv::Mat System::TrackRGBD(const cv::Mat &im, const cv::Mat &depthmap, const doub // Check reset { - unique_lock lock(mMutexReset); - if(mbReset) - { - mpTracker->Reset(); - mbReset = false; - } + unique_lock lock(mMutexReset); + if(mbReset) + { + mpTracker->Reset(); + mbReset = false; + } } cv::Mat Tcw = mpTracker->GrabImageRGBD(im,depthmap,timestamp); diff --git a/src/Tracking.cc b/src/Tracking.cc index 8ecb8c9..757059f 100644 --- a/src/Tracking.cc +++ b/src/Tracking.cc @@ -21,21 +21,21 @@ #include "Tracking.h" -#include -#include +#include +#include -#include"ORBmatcher.h" -#include"FrameDrawer.h" -#include"Converter.h" -#include"Map.h" -#include"Initializer.h" +#include "ORBmatcher.h" +#include "FrameDrawer.h" +#include "Converter.h" +#include "Map.h" +#include "Initializer.h" -#include"Optimizer.h" -#include"PnPsolver.h" +#include "Optimizer.h" +#include "PnPsolver.h" -#include +#include -#include +#include using namespace std; @@ -205,6 +205,110 @@ Tracking::Tracking(System *pSys, LFNETVocabulary* pVocLFNet, FrameDrawer *pFrame cout << "- fps: " << fps << endl; + int nRGB = fSettings["Camera.RGB"]; + mbRGB = nRGB; + + if(mbRGB) + cout << "- color order: RGB (ignored if grayscale)" << endl; + else + cout << "- color order: BGR (ignored if grayscale)" << endl; + + // Load ORB parameters + /* + int nFeatures = fSettings["ORBextractor.nFeatures"]; + float fScaleFactor = fSettings["ORBextractor.scaleFactor"]; + int nLevels = fSettings["ORBextractor.nLevels"]; + int fIniThFAST = fSettings["ORBextractor.iniThFAST"]; + int fMinThFAST = fSettings["ORBextractor.minThFAST"]; + //zoe 20181019 + mpORBextractorLeft = new ORBextractor(nFeatures,fScaleFactor,nLevels,fIniThFAST,fMinThFAST); + + if(sensor==System::STEREO) + mpORBextractorRight = new ORBextractor(nFeatures,fScaleFactor,nLevels,fIniThFAST,fMinThFAST); + + if(sensor==System::MONOCULAR) + mpIniORBextractor = new ORBextractor(2*nFeatures,fScaleFactor,nLevels,fIniThFAST,fMinThFAST); + + cout << endl << "ORB Extractor Parameters: " << endl; + cout << "- Number of Features: " << nFeatures << endl; + cout << "- Scale Levels: " << nLevels << endl; + cout << "- Scale Factor: " << fScaleFactor << endl; + cout << "- Initial Fast Threshold: " << fIniThFAST << endl; + cout << "- Minimum Fast Threshold: " << fMinThFAST << endl; + */ + if(sensor==System::STEREO || sensor==System::RGBD) + { + mThDepth = mbf*(float)fSettings["ThDepth"]/fx; + cout << endl << "Depth Threshold (Close/Far Points): " << mThDepth << endl; + } + + if(sensor==System::RGBD) + { + mDepthMapFactor = fSettings["DepthMapFactor"]; + if(fabs(mDepthMapFactor)<1e-5) + mDepthMapFactor=1; + else + mDepthMapFactor = 1.0f/mDepthMapFactor; + } + +} +// zoe 20190520 +Tracking::Tracking(System *pSys, FrameDrawer *pFrameDrawer, MapDrawer *pMapDrawer, Map *pMap, const string &strSettingPath, const int sensor, const bool bOnlyTracking): + mState(NO_IMAGES_YET), mSensor(sensor), mbOnlyTracking(bOnlyTracking), mbVO(false), mpLFNETVocabulary(static_cast(NULL)), + mpKeyFrameDB(static_cast(NULL)), mpInitializer(static_cast(NULL)), mpSystem(pSys), mpViewer(NULL), + mpFrameDrawer(pFrameDrawer), mpMapDrawer(pMapDrawer), mpMap(pMap), mnLastRelocFrameId(0) //zoe 20190513 tracking参数赋值修改 +{ + // Load camera parameters from settings file + cv::FileStorage fSettings(strSettingPath, cv::FileStorage::READ); + float fx = fSettings["Camera.fx"]; + float fy = fSettings["Camera.fy"]; + float cx = fSettings["Camera.cx"]; + float cy = fSettings["Camera.cy"]; + + cv::Mat K = cv::Mat::eye(3,3,CV_32F); + K.at(0,0) = fx; + K.at(1,1) = fy; + K.at(0,2) = cx; + K.at(1,2) = cy; + K.copyTo(mK); + + cv::Mat DistCoef(4,1,CV_32F); + DistCoef.at(0) = fSettings["Camera.k1"]; + DistCoef.at(1) = fSettings["Camera.k2"]; + DistCoef.at(2) = fSettings["Camera.p1"]; + DistCoef.at(3) = fSettings["Camera.p2"]; + const float k3 = fSettings["Camera.k3"]; + if(k3!=0) + { + DistCoef.resize(5); + DistCoef.at(4) = k3; + } + DistCoef.copyTo(mDistCoef); + + mbf = fSettings["Camera.bf"]; + + float fps = fSettings["Camera.fps"]; + if(fps==0) + fps=30; + + // Max/Min Frames to insert keyframes and to check relocalisation + mMinFrames = 0; + mMaxFrames = fps; + + cout << endl << "Camera Parameters: " << endl; + cout << "- fx: " << fx << endl; + cout << "- fy: " << fy << endl; + cout << "- cx: " << cx << endl; + cout << "- cy: " << cy << endl; + cout << "- k1: " << DistCoef.at(0) << endl; + cout << "- k2: " << DistCoef.at(1) << endl; + if(DistCoef.rows==5) + cout << "- k3: " << DistCoef.at(4) << endl; + cout << "- p1: " << DistCoef.at(2) << endl; + cout << "- p2: " << DistCoef.at(3) << endl; + cout << "- fps: " << fps << endl; + + int nRGB = fSettings["Camera.RGB"]; mbRGB = nRGB; @@ -308,7 +412,6 @@ cv::Mat Tracking::GrabImageStereo(const cv::Mat &imRectLeft, const cv::Mat &imRe return mCurrentFrame.mTcw.clone(); } - cv::Mat Tracking::GrabImageRGBD(const cv::Mat &imRGB,const cv::Mat &imD, const double ×tamp) { mImGray = imRGB; @@ -334,8 +437,10 @@ cv::Mat Tracking::GrabImageRGBD(const cv::Mat &imRGB,const cv::Mat &imD, const d //mCurrentFrame = Frame(mImGray,imDepth,timestamp,mpORBextractorLeft,mpORBVocabulary,mK,mDistCoef,mbf,mThDepth);// zoe mark //zoe 20181016 - - mCurrentFrame = Frame(mImGray,imDepth,timestamp,mpORBextractorLeft,mpLFNETVocabulary,mK,mDistCoef,mbf,mThDepth); + if (mpLFNETVocabulary) + mCurrentFrame = Frame(mImGray, imDepth, timestamp, mpORBextractorLeft, mpLFNETVocabulary, mK, mDistCoef, mbf, mThDepth); + else + mCurrentFrame = Frame(mImGray, imDepth, timestamp, mK, mDistCoef, mbf, mThDepth); Track(); @@ -638,7 +743,14 @@ void Tracking::StereoInitialization() mCurrentFrame.SetPose(cv::Mat::eye(4,4,CV_32F)); // Create KeyFrame - KeyFrame* pKFini = new KeyFrame(mCurrentFrame,mpMap,mpKeyFrameDB); + KeyFrame* pKFini; + if (mpKeyFrameDB) + pKFini = new KeyFrame(mCurrentFrame,mpMap,mpKeyFrameDB); + else + { + pKFini = new KeyFrame(mCurrentFrame,mpMap); + } + // Insert KeyFrame in the map mpMap->AddKeyFrame(pKFini); @@ -670,8 +782,9 @@ void Tracking::StereoInitialization() if (mpLocalMapper) mpLocalMapper->InsertKeyFrame(pKFini); else + { pKFini->ComputeBoWLFNet();// zoe 20190513 - + } mLastFrame = Frame(mCurrentFrame); mnLastKeyFrameId=mCurrentFrame.mnId; mpLastKeyFrame = pKFini; @@ -897,15 +1010,26 @@ bool Tracking::TrackReferenceKeyFrame() // We perform first an ORB matching with the reference keyframe // If enough matches are found we setup a PnP solver + ORBmatcher matcher(0.7,true); vector vpMapPointMatches; //int nmatches = matcher.SearchByBoW(mpReferenceKF,mCurrentFrame,vpMapPointMatches); - int nmatches = matcher.SearchByBoWLFNet(mpReferenceKF,mCurrentFrame,vpMapPointMatches);//zoe 20181017 + int nmatches = 0; + if (mpLFNETVocabulary) + { + nmatches = matcher.SearchByBoWLFNet(mpReferenceKF,mCurrentFrame,vpMapPointMatches);//zoe 20181017 + } + else + { + nmatches = matcher.SearchByBFLFNet(mpReferenceKF,mCurrentFrame,vpMapPointMatches); + } + if(nmatches<15) return false; + mCurrentFrame.mvpMapPoints = vpMapPointMatches; mCurrentFrame.SetPose(mLastFrame.mTcw); @@ -933,6 +1057,7 @@ bool Tracking::TrackReferenceKeyFrame() } return nmatchesMap>=10; + } void Tracking::UpdateLastFrame() @@ -1125,7 +1250,7 @@ bool Tracking::NeedNewKeyFrame() if(mpLocalMapper->isStopped() || mpLocalMapper->stopRequested()) return false; } - + const int nKFs = mpMap->KeyFramesInMap(); // Do not insert keyframes if not enough frames have passed from last relocalisation @@ -1212,9 +1337,13 @@ void Tracking::CreateNewKeyFrame() if(mpLocalMapper && !mpLocalMapper->SetNotStop(true)) return; - - KeyFrame* pKF = new KeyFrame(mCurrentFrame,mpMap,mpKeyFrameDB); - + + KeyFrame* pKF; + if (mpKeyFrameDB) + pKF = new KeyFrame(mCurrentFrame,mpMap,mpKeyFrameDB); + else + pKF = new KeyFrame(mCurrentFrame,mpMap); + mpReferenceKF = pKF; mCurrentFrame.mpReferenceKF = pKF; @@ -1289,6 +1418,8 @@ void Tracking::CreateNewKeyFrame() else { pKF->ComputeBoWLFNet();//zoe 20190513 + // Insert Keyframe in Map + mpMap->AddKeyFrame(pKF); } mnLastKeyFrameId = mCurrentFrame.mnId; @@ -1505,156 +1636,316 @@ bool Tracking::Relocalization() // Relocalization is performed when tracking is lost // Track Lost: Query KeyFrame Database for keyframe candidates for relocalisation - vector vpCandidateKFs = mpKeyFrameDB->DetectRelocalizationCandidates(&mCurrentFrame); - - if(vpCandidateKFs.empty()) + bool bMatch = false; + if (mpKeyFrameDB) { - return false; - } + vector vpCandidateKFs = mpKeyFrameDB->DetectRelocalizationCandidates(&mCurrentFrame); - const int nKFs = vpCandidateKFs.size(); + if(vpCandidateKFs.empty()) + { + return false; + } + + const int nKFs = vpCandidateKFs.size(); - // We perform first an ORB matching with each candidate - // If enough matches are found we setup a PnP solver - ORBmatcher matcher(0.75,true); + // We perform first an ORB matching with each candidate + // If enough matches are found we setup a PnP solver + ORBmatcher matcher(0.75,true); - vector vpPnPsolvers; - vpPnPsolvers.resize(nKFs); + vector vpPnPsolvers; + vpPnPsolvers.resize(nKFs); - vector > vvpMapPointMatches; - vvpMapPointMatches.resize(nKFs); + vector > vvpMapPointMatches; + vvpMapPointMatches.resize(nKFs); - vector vbDiscarded; - vbDiscarded.resize(nKFs); + vector vbDiscarded; + vbDiscarded.resize(nKFs); - int nCandidates=0; + int nCandidates=0; - for(int i=0; iisBad()) - vbDiscarded[i] = true; - else + for(int i=0; iisBad()) vbDiscarded[i] = true; - continue; - } else { - PnPsolver* pSolver = new PnPsolver(mCurrentFrame,vvpMapPointMatches[i]); - pSolver->SetRansacParameters(0.99,10,300,4,0.5,5.991); - vpPnPsolvers[i] = pSolver; - nCandidates++; + //int nmatches = matcher.SearchByBoW(pKF,mCurrentFrame,vvpMapPointMatches[i]);//zoe 20181017 + int nmatches = matcher.SearchByBoWLFNet(pKF,mCurrentFrame,vvpMapPointMatches[i]); + + if(nmatches<15) + { + vbDiscarded[i] = true; + continue; + } + else + { + PnPsolver* pSolver = new PnPsolver(mCurrentFrame,vvpMapPointMatches[i]); + pSolver->SetRansacParameters(0.99,10,300,4,0.5,5.991); + vpPnPsolvers[i] = pSolver; + nCandidates++; + } } } - } + - // Alternatively perform some iterations of P4P RANSAC - // Until we found a camera pose supported by enough inliers - bool bMatch = false; - ORBmatcher matcher2(0.9,true); + // Alternatively perform some iterations of P4P RANSAC + // Until we found a camera pose supported by enough inliers + + ORBmatcher matcher2(0.9,true); - while(nCandidates>0 && !bMatch) + while(nCandidates>0 && !bMatch) + { + for(int i=0; i vbInliers; + int nInliers; + bool bNoMore; + + PnPsolver* pSolver = vpPnPsolvers[i]; + cv::Mat Tcw = pSolver->iterate(5,bNoMore,vbInliers,nInliers); + + // If Ransac reachs max. iterations discard keyframe + if(bNoMore) + { + vbDiscarded[i]=true; + nCandidates--; + } + + // If a Camera Pose is computed, optimize + if(!Tcw.empty()) + { + Tcw.copyTo(mCurrentFrame.mTcw); + + set sFound; + + const int np = vbInliers.size(); + + for(int j=0; j(NULL); + + // If few inliers, search by projection in a coarse window and optimize again + if(nGood<50) + { + //int nadditional =matcher2.SearchByProjection(mCurrentFrame,vpCandidateKFs[i],sFound,10,100);//zoe 20181017 + const float FLOAT_MAX=5.0;//zoe + int nadditional =matcher2.SearchByProjectionLFNet(mCurrentFrame,vpCandidateKFs[i],sFound,10,FLOAT_MAX/2); + + if(nadditional+nGood>=50) + { + nGood = Optimizer::PoseOptimization(&mCurrentFrame); + + // If many inliers but still not enough, search by projection again in a narrower window + // the camera has been already optimized with many points + if(nGood>30 && nGood<50) + { + sFound.clear(); + for(int ip =0; ip=50) + { + nGood = Optimizer::PoseOptimization(&mCurrentFrame); + + for(int io =0; io=50) + { + bMatch = true; + break; + } + } + } + } + } + else { - for(int i=0; i vpCandidateKFs = mpMap->GetAllKeyFrames(); + + if(vpCandidateKFs.empty()) { - if(vbDiscarded[i]) - continue; + return false; + } + + const int nKFs = vpCandidateKFs.size(); + + // We perform first an ORB matching with each candidate + // If enough matches are found we setup a PnP solver + ORBmatcher matcher(0.75,true); - // Perform 5 Ransac Iterations - vector vbInliers; - int nInliers; - bool bNoMore; + vector vpPnPsolvers; + vpPnPsolvers.resize(nKFs); - PnPsolver* pSolver = vpPnPsolvers[i]; - cv::Mat Tcw = pSolver->iterate(5,bNoMore,vbInliers,nInliers); + vector > vvpMapPointMatches; + vvpMapPointMatches.resize(nKFs); - // If Ransac reachs max. iterations discard keyframe - if(bNoMore) + vector vbDiscarded; + vbDiscarded.resize(nKFs); + + int nCandidates=0; + + for(int i=0; iisBad()) + vbDiscarded[i] = true; + else { - vbDiscarded[i]=true; - nCandidates--; + //int nmatches = matcher.SearchByBoW(pKF,mCurrentFrame,vvpMapPointMatches[i]);//zoe 20181017 + int nmatches = matcher.SearchByBFLFNet(pKF,mCurrentFrame,vvpMapPointMatches[i]); + + if(nmatches<15) + { + vbDiscarded[i] = true; + continue; + } + else + { + PnPsolver* pSolver = new PnPsolver(mCurrentFrame,vvpMapPointMatches[i]); + pSolver->SetRansacParameters(0.99,10,300,4,0.5,5.991); + vpPnPsolvers[i] = pSolver; + nCandidates++; + } } + } + - // If a Camera Pose is computed, optimize - if(!Tcw.empty()) + // Alternatively perform some iterations of P4P RANSAC + // Until we found a camera pose supported by enough inliers + + ORBmatcher matcher2(0.9,true); + + while(nCandidates>0 && !bMatch) + { + for(int i=0; i sFound; + // Perform 5 Ransac Iterations + vector vbInliers; + int nInliers; + bool bNoMore; - const int np = vbInliers.size(); + PnPsolver* pSolver = vpPnPsolvers[i]; + cv::Mat Tcw = pSolver->iterate(5,bNoMore,vbInliers,nInliers); - for(int j=0; j sFound; - for(int io =0; io(NULL); + const int np = vbInliers.size(); - // If few inliers, search by projection in a coarse window and optimize again - if(nGood<50) - { - //int nadditional =matcher2.SearchByProjection(mCurrentFrame,vpCandidateKFs[i],sFound,10,100);//zoe 20181017 - const float FLOAT_MAX=5.0;//zoe - int nadditional =matcher2.SearchByProjectionLFNet(mCurrentFrame,vpCandidateKFs[i],sFound,10,FLOAT_MAX/2); - - if(nadditional+nGood>=50) + for(int j=0; j30 && nGood<50) + if(nGood<10) + continue; + + for(int io =0; io(NULL); + + // If few inliers, search by projection in a coarse window and optimize again + if(nGood<50) + { + //int nadditional =matcher2.SearchByProjection(mCurrentFrame,vpCandidateKFs[i],sFound,10,100);//zoe 20181017 + const float FLOAT_MAX=5.0;//zoe + int nadditional =matcher2.SearchByProjectionLFNet(mCurrentFrame,vpCandidateKFs[i],sFound,10,FLOAT_MAX/2); + + if(nadditional+nGood>=50) { - sFound.clear(); - for(int ip =0; ip=50) + nGood = Optimizer::PoseOptimization(&mCurrentFrame); + + // If many inliers but still not enough, search by projection again in a narrower window + // the camera has been already optimized with many points + if(nGood>30 && nGood<50) { - nGood = Optimizer::PoseOptimization(&mCurrentFrame); + sFound.clear(); + for(int ip =0; ip=50) + { + nGood = Optimizer::PoseOptimization(&mCurrentFrame); - for(int io =0; io=50) - { - bMatch = true; - break; + // If the pose is supported by enough inliers stop ransacs and continue + if(nGood>=50) + { + bMatch = true; + break; + } } } } } + if(!bMatch) { @@ -1696,9 +1987,12 @@ void Tracking::Reset() } // Clear BoW Database - cout << "Reseting Database..."; - mpKeyFrameDB->clear(); - cout << " done" << endl; + if (mpKeyFrameDB) + { + cout << "Reseting Database..."; + mpKeyFrameDB->clear(); + cout << " done" << endl; + } // Clear Map (this erase MapPoints and KeyFrames) mpMap->clear();