bow off

zoeyuchao · May 23, 2019 · 87b13c9 · 87b13c9
1 parent d65777e
commit 87b13c9
Show file tree

Hide file tree

Showing 20 changed files with 898 additions and 176 deletions.
diff --git 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<float> vTimesTrack;

diff --git 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<float> vTimesTrack;

diff --git 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<float> vTimesTrack;

diff --git 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<float> vTimesTrack;

diff --git 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<float> vTimesTrack;

diff --git 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<float> vTimesTrack;

diff --git a/eval_script/png/1_room_ATE_SP_1.png b/eval_script/png/1_room_ATE_SP_1.png
diff --git a/eval_script/png/1_room_RPE_SP_1.png b/eval_script/png/1_room_RPE_SP_1.png
diff --git a/eval_script/txt/ATE.txt 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
@@ -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
@@ -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
@@ -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
@@ -67,6 +67,9 @@ class ORBmatcher
     // Used in Relocalisation and Loop Detection
     int SearchByBoWLFNet(KeyFrame *pKF, Frame &F, std::vector<MapPoint*> &vpMapPointMatches);
     int SearchByBoWLFNet(KeyFrame *pKF1, KeyFrame* pKF2, std::vector<MapPoint*> &vpMatches12);
+    // zoe 20190522
+    int SearchByBFLFNet(KeyFrame *pKF, Frame &F, std::vector<MapPoint*> &vpMapPointMatches);
+    int SearchByBFLFNet(KeyFrame *pKF1, KeyFrame* pKF2, std::vector<MapPoint*> &vpMatches12);
     // Matching for the Map Initialization (only used in the monocular case)
     int SearchForInitializationLFNet(Frame &F1, Frame &F2, std::vector<cv::Point2f> &vbPrevMatched, std::vector<int> &vnMatches12, int windowSize=10);
 

diff --git 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
@@ -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 &timestamp);
     cv::Mat GrabImageRGBD(const cv::Mat &imRGB,const cv::Mat &imD, const double &timestamp);

diff --git 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 <raulmur at unizar dot es> (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<LFNETVocabulary*>(NULL)),mpORBextractorLeft(static_cast<ORBextractor*>(NULL)),mpORBextractorRight(static_cast<ORBextractor*>(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<mnScaleLevels; i++)
+    {
+        mvScaleFactors[i]=mvScaleFactors[i-1]*mfScaleFactor;
+        mvLevelSigma2[i]=mvScaleFactors[i]*mvScaleFactors[i];
+    }
+    //计算每一层想对于原始图片放大倍数的逆
+    mvInvScaleFactors.resize(mnScaleLevels);
+    mvInvLevelSigma2.resize(mnScaleLevels);
+    for(int i=0; i<mnScaleLevels; i++)
+    {
+        mvInvScaleFactors[i]=1.0f/mvScaleFactors[i];
+        mvInvLevelSigma2[i]=1.0f/mvLevelSigma2[i];
+    }
+
+    //This is for orientation
+    // pre-compute the end of a row in a circular patch
+    //用于计算特征方向时，每个v坐标对应最大的u坐标
+    std::vector<int> 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<float> 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<MapPoint*>(N,static_cast<MapPoint*>(NULL));
+    mvbOutlier = vector<bool>(N,false);
+    // This is done only for the first Frame (or after a change in the calibration)
+    if(mbInitialComputations)
+    {
+        ComputeImageBounds(imGray);
+
+        mfGridElementWidthInv=static_cast<float>(FRAME_GRID_COLS)/static_cast<float>(mnMaxX-mnMinX);
+        mfGridElementHeightInv=static_cast<float>(FRAME_GRID_ROWS)/static_cast<float>(mnMaxY-mnMinY);
+
+        fx = K.at<float>(0,0);
+        fy = K.at<float>(1,1);
+        cx = K.at<float>(0,2);
+        cy = K.at<float>(1,2);
+        invfx = 1.0f/fx;
+        invfy = 1.0f/fy;
+
+        mbInitialComputations=false;
+    }
+
+    mb = mbf/fx;
+
+    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<ORBextractor*>(NULL)),
@@ -617,7 +766,7 @@ void Frame::ComputeBoW()
 //zoe 20181016
 void Frame::ComputeBoWLFNet()
 {
-    if(mBowVec.empty())
+    if(mBowVec.empty() && mpLFNETvocabulary)
     {
         //vector<cv::Mat> vCurrentDesc = Converter::toDescriptorVector(mDescriptors);
         mpLFNETvocabulary->transform(mvDspts,mBowVec,mFeatVec,4);