purify the old code

Thirdparty/DBoW2/DBoW2/ScoringObject.h

@@ -39,7 +39,7 @@ class GeneralScoring
 	static const double LOG_EPS; 
   // If you change the type of WordValue, make sure you change also the
 	// epsilon value (this is needed by the KL method)
-  virtual ~GeneralScoring(){}
+	
 };
 
 /** 

include/Frame.h

@@ -206,7 +206,7 @@ inline void updatePointStatistics(PointStatistics* stats){
     ORBextractor* mpORBextractor;
 
     // Frame timestamp
-    double mTimeStamp;
+    const double mTimeStamp;
     // Camera Pose
     Sophus::SE3d mTcw;
     Eigen::Vector3d mOw;

include/Tracking.h

@@ -34,11 +34,13 @@
 #include"ORBextractor.h"
 #include "Initializer.h"
 #include "MapPublisher.h"
+#include "StereoImageLoader.h" //dataset_type
 
 #include "sophus/sim3.hpp"
 
 #include "vio_g2o/anchored_points.h"
 #include "vio_g2o/IMU_constraint.h"
+
 #include <g2o/core/block_solver.h> //for sparseoptimizer
 
 #include <viso2/p_match.h> //for matches adopted from libviso2
@@ -62,6 +64,32 @@ class Map;
 class LocalMapping;
 class LoopClosing;
 
+
+enum TrackingState{
+    SYSTEM_NOT_READY=-1,
+    NO_IMAGES_YET=0,
+    NOT_INITIALIZED=1,
+    INITIALIZING=2,
+    WORKING=3,
+    LOST=4
+};
+
+struct TrackingResult
+{
+    TrackingState status_; //tracking status at timestamp_
+    double timestamp_;
+    Sophus::SE3d T_Wc_C_; //transformation from camera frame to the custom world frame at timestamp_
+    Eigen::Matrix<double, 9,1> vwsBaBg_; // velocity of the device in the world frame, acc bias, gyro bias at timestamp_
+    TrackingResult():status_(TrackingState::SYSTEM_NOT_READY), timestamp_(-1)
+    {}
+    friend ostream& operator<<(ostream& os, const TrackingResult& tr);
+private:
+
+    TrackingResult(const TrackingResult&);
+    TrackingResult& operator= (const TrackingResult&);
+};
+
+
 class Tracking
 {  
 public:
@@ -73,23 +101,14 @@ class Tracking
              Map* pMap, string strSettingPath);
 #endif
     ~Tracking();
-    enum eTrackingState{
-        SYSTEM_NOT_READY=-1,
-        NO_IMAGES_YET=0,
-        NOT_INITIALIZED=1,
-        INITIALIZING=2,
-        WORKING=3,
-        LOST=4
-    };
+
     double GetFPS() const {return (double)mFps;}
     int GetRegNumFeatures() const {return mnFeatures;}
     void SetLocalMapper(LocalMapping* pLocalMapper);
     void SetLoopClosing(LoopClosing* pLoopClosing);
     void SetKeyFrameDatabase(KeyFrameDatabase* pKFDB);
     bool isInTemporalWindow(const Frame* pFrame)const;
     long unsigned int GetCurrentFrameId() {return mpCurrentFrame->mnId;}
-    // This is the main function of the Tracking Thread
-    void Run();
 
     void ForceRelocalisation(const g2o::Sim3 );
     Sophus::Sim3d GetSnew2old(){
@@ -104,8 +123,22 @@ class Tracking
     /// Optimize some of the observed 3D points.
     void optimizeStructure(FramePtr frame, size_t max_n_pts, int max_iter);
     vk::PinholeCamera* GetCameraModel(){return cam_;}
-    eTrackingState mState;
-    eTrackingState mLastProcessedState;    
+
+
+    void GetLastestPoseEstimate(TrackingResult & rhs) const;
+    void GetViso2PoseEstimate(TrackingResult & rhs) const;
+    bool ProcessAMonocularFrame(cv::Mat &left_img, double time_frame);
+    bool ProcessAStereoFrame(cv::Mat &left_img, cv::Mat &right_img, double time_frame);
+
+    /**
+     * @brief PrepareImuProcessor construct the ImuProcessor, and reads the initial values for IMU pose in the world frame at start time,
+     *  and the its velocity and acc bias and gyro bias. The start time is tied to the first frame that is covered by inertial data
+     */
+    void PrepareImuProcessor();
+    void ResizeCameraModel(const int downscale);
+
+    TrackingState mState;
+    TrackingState mLastProcessedState;
 
     // Current Frame
     Frame* mpCurrentFrame;
@@ -117,6 +150,13 @@ class Tracking
     std::vector<cv::Point2f> mvbPrevMatched;
     std::vector<cv::Point3f> mvIniP3D;
     Frame* mpInitialFrame;
+
+    dataset_type experimDataset;
+    /// the following two are used to initialize the imu processor at start or reset it when tracking is lost
+    Sophus::SE3d initTws; // initial transformation from the inertial sensor frame to the world frame
+    Eigen::Matrix<double,9,1> initVwsBaBg; // initial velocity of IMU sensor in the world frame, accelerometer bias and gyro bias
+
+    Sophus::SE3d mT_Wc_W; //transformation from the world frame to the custom world frame, only used for visualization and output
 protected:
 
     //process stereo image pair, left_img and right_img, they shared the same time in seconds,
@@ -241,8 +281,6 @@ class Tracking
     //Motion Model
     Sophus::SE3d mVelocity; //T prev to curr
     Eigen::Vector3d mVelByStereoOdometry; //differentiated from visual stereo odometry
-    //Color order (true RGB, false BGR, ignored if grayscale)
-    bool mbRGB;
 
 #ifdef SLAM_USE_ROS
     // Transfor broadcaster (for visualization in rviz)
@@ -253,20 +291,23 @@ class Tracking
     double imu_sample_interval;             //sampling interval in second
     vio::G2oIMUParameters imu_;
 
-    int mnStartId; // used to offset the ID of frames
     long unsigned int mnFrameIdOfSecondKF; // the id of the second kf upon initialization of the map relative to the image sequence,
 
     const int mnFeatures;// how many point features to detect in a frame, for the initial keyframes, 2 times points
 
     PointStatistics point_stats;
     std::vector<KeyFrame*> core_kfs_;                      //!< Keyframes in the closer neighbourhood.
     MotionModel mMotionModel;
+    vio::IMUProcessor* mpImuProcessor;
 
     ///the following parameters determines necessary conditions to create a new keyframe
     float mfTrackedFeatureRatio; /// if the current frame tracks less than this ratio of features in the reference keyframe
     int mnMinTrackedFeatures; /// if the current frame tracks less than this number of features in the reference keyframe
 };
-    std::vector<p_match> cropMatches(const std::vector<p_match> &pMatches, float xl, float xr);
+
+std::vector<p_match> cropMatches(const std::vector<p_match> &pMatches, float xl, float xr);
+
+
 } //namespace ORB_SLAM
 
 #endif // TRACKING_H

include/global.h

@@ -15,7 +15,7 @@
   #define SLAM_WARN_STREAM_THROTTLE(rate, x) ROS_WARN_STREAM_THROTTLE(rate, x)
   #define SLAM_ERROR_STREAM(x) ROS_ERROR_STREAM(x)
 #else
-  #define SLAM_INFO_STREAM(x) std::cerr<<"\033[0;0m[INFO] "<<x<<"\033[0;0m"<<std::endl;
+  #define SLAM_INFO_STREAM(x) std::cout<<"\033[0;0m[INFO] "<<x<<"\033[0;0m"<<std::endl;
   #ifdef SLAM_DEBUG_OUTPUT
     #define SLAM_DEBUG_STREAM(x) SLAM_INFO_STREAM(x)
   #else

src/Frame.cc

@@ -69,7 +69,7 @@ Frame::Frame(const Frame &frame)
      mvLevelSigma2(frame.mvLevelSigma2), mvInvLevelSigma2(frame.mvInvLevelSigma2),
      mnMinX(frame.mnMinX),mnMaxX(frame.mnMaxX),mnMinY(frame.mnMinY), mnMaxY(frame.mnMaxY),
      viso2LeftId2StereoId(frame.viso2LeftId2StereoId),viso2RightId2StereoId(frame.viso2RightId2StereoId),
-     mbBad(frame.mbBad), v_kf_(frame.v_kf_), v_sb_(frame.v_sb_)
+     mbBad(frame.mbBad), v_kf_(NULL), v_sb_(NULL)
 { 
     for(int i=0;i<FRAME_GRID_COLS;i++)
         for(int j=0; j<FRAME_GRID_ROWS; j++)

src/FramePublisher.cc

@@ -34,7 +34,7 @@ namespace ORB_SLAM
 
 FramePublisher::FramePublisher()
 {
-    mState=Tracking::SYSTEM_NOT_READY;
+    mState= SYSTEM_NOT_READY;
     mIm = cv::Mat(480,640,CV_8UC3, cv::Scalar(0,0,0));
     mbUpdated = true;
 #ifdef SLAM_USE_ROS
@@ -71,27 +71,27 @@ cv::Mat FramePublisher::DrawFrame()
     {
         boost::mutex::scoped_lock lock(mMutex);
         state=mState;
-        if(mState==Tracking::SYSTEM_NOT_READY)
-            mState=Tracking::NO_IMAGES_YET;
+        if(mState==SYSTEM_NOT_READY)
+            mState=NO_IMAGES_YET;
 
         mIm.copyTo(im);
 
-        if(mState==Tracking::NOT_INITIALIZED)
+        if(mState==NOT_INITIALIZED)
         {            
             vIniKeys = mvIniKeys;
         }
-        else if(mState==Tracking::INITIALIZING)
+        else if(mState==INITIALIZING)
         {
             vCurrentKeys = mvCurrentKeys;
             vIniKeys = mvIniKeys;
             vMatches = mvIniMatches;
         }
-        else if(mState==Tracking::WORKING)
+        else if(mState==WORKING)
         {
             vCurrentKeys = mvCurrentKeys;
             vMatchedMapPoints = mvpMatchedMapPoints;
         }
-        else if(mState==Tracking::LOST)
+        else if(mState==LOST)
         {
             vCurrentKeys = mvCurrentKeys;
         }
@@ -101,7 +101,7 @@ cv::Mat FramePublisher::DrawFrame()
         cvtColor(im,im,CV_GRAY2BGR);
 
     //Draw
-    if(state==Tracking::INITIALIZING) //INITIALIZING
+    if(state==INITIALIZING) //INITIALIZING
     {
         for(unsigned int i=0; i<vMatches.size(); i++)
         {
@@ -112,7 +112,7 @@ cv::Mat FramePublisher::DrawFrame()
             }
         }        
     }
-    else if(state==Tracking::WORKING) //TRACKING
+    else if(state==WORKING) //TRACKING
     {
         mnTracked=0;
         const float r = 5;
@@ -162,24 +162,24 @@ void FramePublisher::PublishFrame()
 void FramePublisher::DrawTextInfo(cv::Mat &im, int nState, cv::Mat &imText)
 {
     stringstream s;
-    if(nState==Tracking::NO_IMAGES_YET)
+    if(nState==NO_IMAGES_YET)
         s << "WAITING FOR IMAGES. (Topic: /camera/image_raw)";
-    else if(nState==Tracking::NOT_INITIALIZED)
+    else if(nState==NOT_INITIALIZED)
         s << " NOT INITIALIZED ";
-    else if(nState==Tracking::INITIALIZING)
+    else if(nState==INITIALIZING)
         s << " TRYING TO INITIALIZE ";
-    else if(nState==Tracking::WORKING)
+    else if(nState==WORKING)
     {
         s << " TRACKING ";
         int nKFs = mpMap->KeyFramesInMap();
         int nMPs = mpMap->MapPointsInMap();
         s << " - KFs: " << nKFs << " , MPs: " << nMPs << " , Tracked: " << mnTracked;
     }
-    else if(nState==Tracking::LOST)
+    else if(nState==LOST)
     {
         s << " TRACK LOST. TRYING TO RELOCALIZE ";
     }
-    else if(nState==Tracking::SYSTEM_NOT_READY)
+    else if(nState==SYSTEM_NOT_READY)
     {
         s << " LOADING ORB VOCABULARY. PLEASE WAIT...";
     }
@@ -205,7 +205,7 @@ void FramePublisher::Update(Tracking *pTracker, const cv::Mat& im)
         mvbOutliers = pTracker->mpCurrentFrame->mvbOutlier;
     }
 
-    if(pTracker->mLastProcessedState==Tracking::INITIALIZING)
+    if(pTracker->mLastProcessedState==INITIALIZING)
     {
         mvIniKeys=pTracker->mpInitialFrame->mvKeys;
         mvIniMatches=pTracker->mvIniMatches;

src/MapPoint.cc

@@ -176,7 +176,7 @@ void MapPoint::Replace(MapPoint* pMP)
         }
         else
         {
-            assert(nMPId!=mit->second);
+            assert(nMPId!= (int) mit->second);
             pKF->EraseMapPointMatch(mit->second);
         }
     }

src/Optimizer.cc

@@ -1645,7 +1645,7 @@ int Optimizer::LocalOptimize(vk::PinholeCamera * cam,
                         SLAM_DEBUG_STREAM("Bad keyframe in temporal window mnId,N, mbNotErase, keys size:"
                                           <<larry->mnId<<" "<<larry->N<<" "<<larry->GetNotErase()<<" "<< larry->mvKeysUn.size());
 
-//                        assert(false);
+                        assert(false);
                     }
                 }
 #endif
@@ -1773,6 +1773,36 @@ int Optimizer::LocalOptimize(vk::PinholeCamera * cam,
     if(edges.size()==0)
     {
         cout<<"Graph with no edges."<<endl;
+        for (vector <KeyFrame*>::const_iterator  it_win = vpLocalKeyFrames.begin(), it_end_win=vpLocalKeyFrames.end();
+             it_win!=it_end_win; ++it_win)
+        {
+            (*it_win)->v_kf_ = NULL;
+        }
+        for (deque<Frame*>::const_iterator  it_win = vpTemporalFrames.begin(), it_end_win=vpTemporalFrames.end();
+             it_win!=it_end_win; ++it_win)
+        {
+            (*it_win)->v_kf_ = NULL;
+            if(bUseIMUData)
+            {
+                (*it_win)->v_sb_=NULL;
+            }
+        }
+
+        // previous frame
+#ifndef MONO
+        pLastFrame->v_kf_= NULL;
+        if(bUseIMUData)
+        {
+            pLastFrame->v_sb_= NULL;
+        }
+#endif
+        //current frame
+        pCurrentFrame->v_kf_= NULL;
+        if(bUseIMUData)
+        {
+            pCurrentFrame->v_sb_= NULL;
+        }
+
         return 0;
     }
     SLAM_DEBUG_STREAM("opt5 structBA in "<< __func__);