v1

include/Converter.h

@@ -0,0 +1,57 @@
+/**
+* This file is part of ORB-SLAM2.
+*
+* Copyright (C) 2014-2016 Raúl Mur-Artal <raulmur at unizar dot es> (University of Zaragoza)
+* For more information see <https://github.com/raulmur/ORB_SLAM2>
+*
+* ORB-SLAM2 is free software: you can redistribute it and/or modify
+* it under the terms of the GNU General Public License as published by
+* the Free Software Foundation, either version 3 of the License, or
+* (at your option) any later version.
+*
+* ORB-SLAM2 is distributed in the hope that it will be useful,
+* but WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU General Public License for more details.
+*
+* You should have received a copy of the GNU General Public License
+* along with ORB-SLAM2. If not, see <http://www.gnu.org/licenses/>.
+*/
+
+#ifndef CONVERTER_H
+#define CONVERTER_H
+
+#include<opencv2/core/core.hpp>
+
+#include<Eigen/Dense>
+#include"Thirdparty/g2o/g2o/types/types_six_dof_expmap.h"
+#include"Thirdparty/g2o/g2o/types/types_seven_dof_expmap.h"
+
+namespace ORB_SLAM2
+{
+
+class Converter
+{
+public:
+    static std::vector<cv::Mat> toDescriptorVector(const cv::Mat &Descriptors);
+
+    static g2o::SE3Quat toSE3Quat(const cv::Mat &cvT);
+    static g2o::SE3Quat toSE3Quat(const g2o::Sim3 &gSim3);
+
+    static cv::Mat toCvMat(const g2o::SE3Quat &SE3);
+    static cv::Mat toCvMat(const g2o::Sim3 &Sim3);
+    static cv::Mat toCvMat(const Eigen::Matrix<double,4,4> &m);
+    static cv::Mat toCvMat(const Eigen::Matrix3d &m);
+    static cv::Mat toCvMat(const Eigen::Matrix<double,3,1> &m);
+    static cv::Mat toCvSE3(const Eigen::Matrix<double,3,3> &R, const Eigen::Matrix<double,3,1> &t);
+
+    static Eigen::Matrix<double,3,1> toVector3d(const cv::Mat &cvVector);
+    static Eigen::Matrix<double,3,1> toVector3d(const cv::Point3f &cvPoint);
+    static Eigen::Matrix<double,3,3> toMatrix3d(const cv::Mat &cvMat3);
+
+    static std::vector<float> toQuaternion(const cv::Mat &M);
+};
+
+}// namespace ORB_SLAM
+
+#endif // CONVERTER_H

include/Frame.h

@@ -0,0 +1,213 @@
+/**
+* This file is part of ORB-SLAM2.
+*
+* Copyright (C) 2014-2016 Raúl Mur-Artal <raulmur at unizar dot es> (University of Zaragoza)
+* For more information see <https://github.com/raulmur/ORB_SLAM2>
+*
+* ORB-SLAM2 is free software: you can redistribute it and/or modify
+* it under the terms of the GNU General Public License as published by
+* the Free Software Foundation, either version 3 of the License, or
+* (at your option) any later version.
+*
+* ORB-SLAM2 is distributed in the hope that it will be useful,
+* but WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU General Public License for more details.
+*
+* You should have received a copy of the GNU General Public License
+* along with ORB-SLAM2. If not, see <http://www.gnu.org/licenses/>.
+*/
+
+#ifndef FRAME_H
+#define FRAME_H
+
+#include<vector>
+
+#include "MapPoint.h"
+#include "Thirdparty/DBoW2/DBoW2/BowVector.h"
+#include "Thirdparty/DBoW2/DBoW2/FeatureVector.h"
+#include "ORBVocabulary.h"
+#include "KeyFrame.h"
+#include "ORBextractor.h"
+
+#include <opencv2/opencv.hpp>
+
+namespace ORB_SLAM2
+{
+#define FRAME_GRID_ROWS 48
+#define FRAME_GRID_COLS 64
+
+class MapPoint;
+class KeyFrame;
+
+class Frame
+{
+public:
+    Frame();
+
+    // Copy constructor.
+    Frame(const Frame &frame);
+
+    // Constructor for stereo cameras.
+    Frame(const cv::Mat &imLeft, const cv::Mat &imRight, const double &timeStamp, ORBextractor* extractorLeft, ORBextractor* extractorRight, ORBVocabulary* voc, cv::Mat &K, cv::Mat &distCoef, const float &bf, const float &thDepth);
+
+    // 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);
+
+    // 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);
+
+    // Extract ORB on the image. 0 for left image and 1 for right image.
+    void ExtractORB(int flag, const cv::Mat &im);
+
+    // Compute Bag of Words representation.
+    void ComputeBoW();
+
+    // Set the camera pose.
+    void SetPose(cv::Mat Tcw);
+
+    // Computes rotation, translation and camera center matrices from the camera pose.
+    void UpdatePoseMatrices();
+
+    // Returns the camera center.
+    inline cv::Mat GetCameraCenter(){
+        return mOw.clone();
+    }
+
+    // Returns inverse of rotation
+    inline cv::Mat GetRotationInverse(){
+        return mRwc.clone();
+    }
+
+    // Check if a MapPoint is in the frustum of the camera
+    // and fill variables of the MapPoint to be used by the tracking
+    bool isInFrustum(MapPoint* pMP, float viewingCosLimit);
+
+    // Compute the cell of a keypoint (return false if outside the grid)
+    bool PosInGrid(const cv::KeyPoint &kp, int &posX, int &posY);
+
+    vector<size_t> GetFeaturesInArea(const float &x, const float  &y, const float  &r, const int minLevel=-1, const int maxLevel=-1) const;
+
+    // Search a match for each keypoint in the left image to a keypoint in the right image.
+    // If there is a match, depth is computed and the right coordinate associated to the left keypoint is stored.
+    void ComputeStereoMatches();
+
+    // Associate a "right" coordinate to a keypoint if there is valid depth in the depthmap.
+    void ComputeStereoFromRGBD(const cv::Mat &imDepth);
+
+    // Backprojects a keypoint (if stereo/depth info available) into 3D world coordinates.
+    cv::Mat UnprojectStereo(const int &i);
+
+public:
+    // Vocabulary used for relocalization.
+    ORBVocabulary* mpORBvocabulary;
+
+    // Feature extractor. The right is used only in the stereo case.
+    ORBextractor* mpORBextractorLeft, *mpORBextractorRight;
+
+    // Frame timestamp.
+    double mTimeStamp;
+
+    // Calibration matrix and OpenCV distortion parameters.
+    cv::Mat mK;
+    static float fx;
+    static float fy;
+    static float cx;
+    static float cy;
+    static float invfx;
+    static float invfy;
+    cv::Mat mDistCoef;
+
+    // Stereo baseline multiplied by fx.
+    float mbf;
+
+    // Stereo baseline in meters.
+    float mb;
+
+    // Threshold close/far points. Close points are inserted from 1 view.
+    // Far points are inserted as in the monocular case from 2 views.
+    float mThDepth;
+
+    // Number of KeyPoints.
+    int N;
+
+    // Vector of keypoints (original for visualization) and undistorted (actually used by the system).
+    // In the stereo case, mvKeysUn is redundant as images must be rectified.
+    // In the RGB-D case, RGB images can be distorted.
+    std::vector<cv::KeyPoint> mvKeys, mvKeysRight;
+    std::vector<cv::KeyPoint> mvKeysUn;
+
+    // Corresponding stereo coordinate and depth for each keypoint.
+    // "Monocular" keypoints have a negative value.
+    std::vector<float> mvuRight;
+    std::vector<float> mvDepth;
+
+    // Bag of Words Vector structures.
+    DBoW2::BowVector mBowVec;
+    DBoW2::FeatureVector mFeatVec;
+
+    // ORB descriptor, each row associated to a keypoint.
+    cv::Mat mDescriptors, mDescriptorsRight;
+
+    // MapPoints associated to keypoints, NULL pointer if no association.
+    std::vector<MapPoint*> mvpMapPoints;
+
+    // Flag to identify outlier associations.
+    std::vector<bool> mvbOutlier;
+
+    // Keypoints are assigned to cells in a grid to reduce matching complexity when projecting MapPoints.
+    static float mfGridElementWidthInv;
+    static float mfGridElementHeightInv;
+    std::vector<std::size_t> mGrid[FRAME_GRID_COLS][FRAME_GRID_ROWS];
+
+    // Camera pose.
+    cv::Mat mTcw;
+
+    // Current and Next Frame id.
+    static long unsigned int nNextId;
+    long unsigned int mnId;
+
+    // Reference Keyframe.
+    KeyFrame* mpReferenceKF;
+
+    // Scale pyramid info.
+    int mnScaleLevels;
+    float mfScaleFactor;
+    float mfLogScaleFactor;
+    vector<float> mvScaleFactors;
+    vector<float> mvInvScaleFactors;
+    vector<float> mvLevelSigma2;
+    vector<float> mvInvLevelSigma2;
+
+    // Undistorted Image Bounds (computed once).
+    static float mnMinX;
+    static float mnMaxX;
+    static float mnMinY;
+    static float mnMaxY;
+
+    static bool mbInitialComputations;
+
+
+private:
+
+    // Undistort keypoints given OpenCV distortion parameters.
+    // Only for the RGB-D case. Stereo must be already rectified!
+    // (called in the constructor).
+    void UndistortKeyPoints();
+
+    // Computes image bounds for the undistorted image (called in the constructor).
+    void ComputeImageBounds(const cv::Mat &imLeft);
+
+    // Assign keypoints to the grid for speed up feature matching (called in the constructor).
+    void AssignFeaturesToGrid();
+
+    // Rotation, translation and camera center
+    cv::Mat mRcw;
+    cv::Mat mtcw;
+    cv::Mat mRwc;
+    cv::Mat mOw; //==mtwc
+};
+
+}// namespace ORB_SLAM
+
+#endif // FRAME_H

include/FrameDrawer.h

@@ -0,0 +1,73 @@
+/**
+* This file is part of ORB-SLAM2.
+*
+* Copyright (C) 2014-2016 Raúl Mur-Artal <raulmur at unizar dot es> (University of Zaragoza)
+* For more information see <https://github.com/raulmur/ORB_SLAM2>
+*
+* ORB-SLAM2 is free software: you can redistribute it and/or modify
+* it under the terms of the GNU General Public License as published by
+* the Free Software Foundation, either version 3 of the License, or
+* (at your option) any later version.
+*
+* ORB-SLAM2 is distributed in the hope that it will be useful,
+* but WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU General Public License for more details.
+*
+* You should have received a copy of the GNU General Public License
+* along with ORB-SLAM2. If not, see <http://www.gnu.org/licenses/>.
+*/
+
+#ifndef FRAMEDRAWER_H
+#define FRAMEDRAWER_H
+
+#include "Tracking.h"
+#include "MapPoint.h"
+#include "Map.h"
+
+#include<opencv2/core/core.hpp>
+#include<opencv2/features2d/features2d.hpp>
+
+#include<mutex>
+
+
+namespace ORB_SLAM2
+{
+
+class Tracking;
+class Viewer;
+
+class FrameDrawer
+{
+public:
+    FrameDrawer(Map* pMap);
+
+    // Update info from the last processed frame.
+    void Update(Tracking *pTracker);
+
+    // Draw last processed frame.
+    cv::Mat DrawFrame();
+
+protected:
+
+    void DrawTextInfo(cv::Mat &im, int nState, cv::Mat &imText);
+
+    // Info of the frame to be drawn
+    cv::Mat mIm;
+    int N;
+    vector<cv::KeyPoint> mvCurrentKeys;
+    vector<bool> mvbMap, mvbVO;
+    bool mbOnlyTracking;
+    int mnTracked, mnTrackedVO;
+    vector<cv::KeyPoint> mvIniKeys;
+    vector<int> mvIniMatches;
+    int mState;
+
+    Map* mpMap;
+
+    std::mutex mMutex;
+};
+
+} //namespace ORB_SLAM
+
+#endif // FRAMEDRAWER_H

include/Initializer.h

@@ -0,0 +1,101 @@
+/**
+* This file is part of ORB-SLAM2.
+*
+* Copyright (C) 2014-2016 Raúl Mur-Artal <raulmur at unizar dot es> (University of Zaragoza)
+* For more information see <https://github.com/raulmur/ORB_SLAM2>
+*
+* ORB-SLAM2 is free software: you can redistribute it and/or modify
+* it under the terms of the GNU General Public License as published by
+* the Free Software Foundation, either version 3 of the License, or
+* (at your option) any later version.
+*
+* ORB-SLAM2 is distributed in the hope that it will be useful,
+* but WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU General Public License for more details.
+*
+* You should have received a copy of the GNU General Public License
+* along with ORB-SLAM2. If not, see <http://www.gnu.org/licenses/>.
+*/
+#ifndef INITIALIZER_H
+#define INITIALIZER_H
+
+#include<opencv2/opencv.hpp>
+#include "Frame.h"
+
+
+namespace ORB_SLAM2
+{
+
+// THIS IS THE INITIALIZER FOR MONOCULAR SLAM. NOT USED IN THE STEREO OR RGBD CASE.
+class Initializer
+{
+    typedef pair<int,int> Match;
+
+public:
+
+    // Fix the reference frame
+    Initializer(const Frame &ReferenceFrame, float sigma = 1.0, int iterations = 200);
+
+    // Computes in parallel a fundamental matrix and a homography
+    // Selects a model and tries to recover the motion and the structure from motion
+    bool Initialize(const Frame &CurrentFrame, const vector<int> &vMatches12,
+                    cv::Mat &R21, cv::Mat &t21, vector<cv::Point3f> &vP3D, vector<bool> &vbTriangulated);
+
+
+private:
+
+    void FindHomography(vector<bool> &vbMatchesInliers, float &score, cv::Mat &H21);
+    void FindFundamental(vector<bool> &vbInliers, float &score, cv::Mat &F21);
+
+    cv::Mat ComputeH21(const vector<cv::Point2f> &vP1, const vector<cv::Point2f> &vP2);
+    cv::Mat ComputeF21(const vector<cv::Point2f> &vP1, const vector<cv::Point2f> &vP2);
+
+    float CheckHomography(const cv::Mat &H21, const cv::Mat &H12, vector<bool> &vbMatchesInliers, float sigma);
+
+    float CheckFundamental(const cv::Mat &F21, vector<bool> &vbMatchesInliers, float sigma);
+
+    bool ReconstructF(vector<bool> &vbMatchesInliers, cv::Mat &F21, cv::Mat &K,
+                      cv::Mat &R21, cv::Mat &t21, vector<cv::Point3f> &vP3D, vector<bool> &vbTriangulated, float minParallax, int minTriangulated);
+
+    bool ReconstructH(vector<bool> &vbMatchesInliers, cv::Mat &H21, cv::Mat &K,
+                      cv::Mat &R21, cv::Mat &t21, vector<cv::Point3f> &vP3D, vector<bool> &vbTriangulated, float minParallax, int minTriangulated);
+
+    void Triangulate(const cv::KeyPoint &kp1, const cv::KeyPoint &kp2, const cv::Mat &P1, const cv::Mat &P2, cv::Mat &x3D);
+
+    void Normalize(const vector<cv::KeyPoint> &vKeys, vector<cv::Point2f> &vNormalizedPoints, cv::Mat &T);
+
+    int CheckRT(const cv::Mat &R, const cv::Mat &t, const vector<cv::KeyPoint> &vKeys1, const vector<cv::KeyPoint> &vKeys2,
+                       const vector<Match> &vMatches12, vector<bool> &vbInliers,
+                       const cv::Mat &K, vector<cv::Point3f> &vP3D, float th2, vector<bool> &vbGood, float &parallax);
+
+    void DecomposeE(const cv::Mat &E, cv::Mat &R1, cv::Mat &R2, cv::Mat &t);
+
+
+    // Keypoints from Reference Frame (Frame 1)
+    vector<cv::KeyPoint> mvKeys1;
+
+    // Keypoints from Current Frame (Frame 2)
+    vector<cv::KeyPoint> mvKeys2;
+
+    // Current Matches from Reference to Current
+    vector<Match> mvMatches12;
+    vector<bool> mvbMatched1;
+
+    // Calibration
+    cv::Mat mK;
+
+    // Standard Deviation and Variance
+    float mSigma, mSigma2;
+
+    // Ransac max iterations
+    int mMaxIterations;
+
+    // Ransac sets
+    vector<vector<size_t> > mvSets;   
+
+};
+
+} //namespace ORB_SLAM
+
+#endif // INITIALIZER_H

include/KeyFrame.h

@@ -0,0 +1,238 @@
+/**
+* This file is part of ORB-SLAM2.
+*
+* Copyright (C) 2014-2016 Raúl Mur-Artal <raulmur at unizar dot es> (University of Zaragoza)
+* For more information see <https://github.com/raulmur/ORB_SLAM2>
+*
+* ORB-SLAM2 is free software: you can redistribute it and/or modify
+* it under the terms of the GNU General Public License as published by
+* the Free Software Foundation, either version 3 of the License, or
+* (at your option) any later version.
+*
+* ORB-SLAM2 is distributed in the hope that it will be useful,
+* but WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU General Public License for more details.
+*
+* You should have received a copy of the GNU General Public License
+* along with ORB-SLAM2. If not, see <http://www.gnu.org/licenses/>.
+*/
+
+#ifndef KEYFRAME_H
+#define KEYFRAME_H
+
+#include "MapPoint.h"
+#include "Thirdparty/DBoW2/DBoW2/BowVector.h"
+#include "Thirdparty/DBoW2/DBoW2/FeatureVector.h"
+#include "ORBVocabulary.h"
+#include "ORBextractor.h"
+#include "Frame.h"
+#include "KeyFrameDatabase.h"
+
+#include <mutex>
+
+
+namespace ORB_SLAM2
+{
+
+class Map;
+class MapPoint;
+class Frame;
+class KeyFrameDatabase;
+
+class KeyFrame
+{
+public:
+    KeyFrame(Frame &F, Map* pMap, KeyFrameDatabase* pKFDB);
+
+    // Pose functions
+    void SetPose(const cv::Mat &Tcw);
+    cv::Mat GetPose();
+    cv::Mat GetPoseInverse();
+    cv::Mat GetCameraCenter();
+    cv::Mat GetStereoCenter();
+    cv::Mat GetRotation();
+    cv::Mat GetTranslation();
+
+    // Bag of Words Representation
+    void ComputeBoW();
+
+    // Covisibility graph functions
+    void AddConnection(KeyFrame* pKF, const int &weight);
+    void EraseConnection(KeyFrame* pKF);
+    void UpdateConnections();
+    void UpdateBestCovisibles();
+    std::set<KeyFrame *> GetConnectedKeyFrames();
+    std::vector<KeyFrame* > GetVectorCovisibleKeyFrames();
+    std::vector<KeyFrame*> GetBestCovisibilityKeyFrames(const int &N);
+    std::vector<KeyFrame*> GetCovisiblesByWeight(const int &w);
+    int GetWeight(KeyFrame* pKF);
+
+    // Spanning tree functions
+    void AddChild(KeyFrame* pKF);
+    void EraseChild(KeyFrame* pKF);
+    void ChangeParent(KeyFrame* pKF);
+    std::set<KeyFrame*> GetChilds();
+    KeyFrame* GetParent();
+    bool hasChild(KeyFrame* pKF);
+
+    // Loop Edges
+    void AddLoopEdge(KeyFrame* pKF);
+    std::set<KeyFrame*> GetLoopEdges();
+
+    // MapPoint observation functions
+    void AddMapPoint(MapPoint* pMP, const size_t &idx);
+    void EraseMapPointMatch(const size_t &idx);
+    void EraseMapPointMatch(MapPoint* pMP);
+    void ReplaceMapPointMatch(const size_t &idx, MapPoint* pMP);
+    std::set<MapPoint*> GetMapPoints();
+    std::vector<MapPoint*> GetMapPointMatches();
+    int TrackedMapPoints(const int &minObs);
+    MapPoint* GetMapPoint(const size_t &idx);
+
+    // KeyPoint functions
+    std::vector<size_t> GetFeaturesInArea(const float &x, const float  &y, const float  &r) const;
+    cv::Mat UnprojectStereo(int i);
+
+    // Image
+    bool IsInImage(const float &x, const float &y) const;
+
+    // Enable/Disable bad flag changes
+    void SetNotErase();
+    void SetErase();
+
+    // Set/check bad flag
+    void SetBadFlag();
+    bool isBad();
+
+    // Compute Scene Depth (q=2 median). Used in monocular.
+    float ComputeSceneMedianDepth(const int q);
+
+    static bool weightComp( int a, int b){
+        return a>b;
+    }
+
+    static bool lId(KeyFrame* pKF1, KeyFrame* pKF2){
+        return pKF1->mnId<pKF2->mnId;
+    }
+
+
+    // The following variables are accesed from only 1 thread or never change (no mutex needed).
+public:
+
+    static long unsigned int nNextId;
+    long unsigned int mnId;
+    const long unsigned int mnFrameId;
+
+    const double mTimeStamp;
+
+    // Grid (to speed up feature matching)
+    const int mnGridCols;
+    const int mnGridRows;
+    const float mfGridElementWidthInv;
+    const float mfGridElementHeightInv;
+
+    // Variables used by the tracking
+    long unsigned int mnTrackReferenceForFrame;
+    long unsigned int mnFuseTargetForKF;
+
+    // Variables used by the local mapping
+    long unsigned int mnBALocalForKF;
+    long unsigned int mnBAFixedForKF;
+
+    // Variables used by the keyframe database
+    long unsigned int mnLoopQuery;
+    int mnLoopWords;
+    float mLoopScore;
+    long unsigned int mnRelocQuery;
+    int mnRelocWords;
+    float mRelocScore;
+
+    // Variables used by loop closing
+    cv::Mat mTcwGBA;
+    cv::Mat mTcwBefGBA;
+    long unsigned int mnBAGlobalForKF;
+
+    // Calibration parameters
+    const float fx, fy, cx, cy, invfx, invfy, mbf, mb, mThDepth;
+
+    // Number of KeyPoints
+    const int N;
+
+    // KeyPoints, stereo coordinate and descriptors (all associated by an index)
+    const std::vector<cv::KeyPoint> mvKeys;
+    const std::vector<cv::KeyPoint> mvKeysUn;
+    const std::vector<float> mvuRight; // negative value for monocular points
+    const std::vector<float> mvDepth; // negative value for monocular points
+    const cv::Mat mDescriptors;
+
+    //BoW
+    DBoW2::BowVector mBowVec;
+    DBoW2::FeatureVector mFeatVec;
+
+    // Pose relative to parent (this is computed when bad flag is activated)
+    cv::Mat mTcp;
+
+    // Scale
+    const int mnScaleLevels;
+    const float mfScaleFactor;
+    const float mfLogScaleFactor;
+    const std::vector<float> mvScaleFactors;
+    const std::vector<float> mvLevelSigma2;
+    const std::vector<float> mvInvLevelSigma2;
+
+    // Image bounds and calibration
+    const int mnMinX;
+    const int mnMinY;
+    const int mnMaxX;
+    const int mnMaxY;
+    const cv::Mat mK;
+
+
+    // The following variables need to be accessed trough a mutex to be thread safe.
+protected:
+
+    // SE3 Pose and camera center
+    cv::Mat Tcw;
+    cv::Mat Twc;
+    cv::Mat Ow;
+
+    cv::Mat Cw; // Stereo middel point. Only for visualization
+
+    // MapPoints associated to keypoints
+    std::vector<MapPoint*> mvpMapPoints;
+
+    // BoW
+    KeyFrameDatabase* mpKeyFrameDB;
+    ORBVocabulary* mpORBvocabulary;
+
+    // Grid over the image to speed up feature matching
+    std::vector< std::vector <std::vector<size_t> > > mGrid;
+
+    std::map<KeyFrame*,int> mConnectedKeyFrameWeights;
+    std::vector<KeyFrame*> mvpOrderedConnectedKeyFrames;
+    std::vector<int> mvOrderedWeights;
+
+    // Spanning Tree and Loop Edges
+    bool mbFirstConnection;
+    KeyFrame* mpParent;
+    std::set<KeyFrame*> mspChildrens;
+    std::set<KeyFrame*> mspLoopEdges;
+
+    // Bad flags
+    bool mbNotErase;
+    bool mbToBeErased;
+    bool mbBad;    
+
+    float mHalfBaseline; // Only for visualization
+
+    Map* mpMap;
+
+    std::mutex mMutexPose;
+    std::mutex mMutexConnections;
+    std::mutex mMutexFeatures;
+};
+
+} //namespace ORB_SLAM
+
+#endif // KEYFRAME_H

include/KeyFrameDatabase.h

@@ -0,0 +1,74 @@
+/**
+* This file is part of ORB-SLAM2.
+*
+* Copyright (C) 2014-2016 Raúl Mur-Artal <raulmur at unizar dot es> (University of Zaragoza)
+* For more information see <https://github.com/raulmur/ORB_SLAM2>
+*
+* ORB-SLAM2 is free software: you can redistribute it and/or modify
+* it under the terms of the GNU General Public License as published by
+* the Free Software Foundation, either version 3 of the License, or
+* (at your option) any later version.
+*
+* ORB-SLAM2 is distributed in the hope that it will be useful,
+* but WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU General Public License for more details.
+*
+* You should have received a copy of the GNU General Public License
+* along with ORB-SLAM2. If not, see <http://www.gnu.org/licenses/>.
+*/
+
+#ifndef KEYFRAMEDATABASE_H
+#define KEYFRAMEDATABASE_H
+
+#include <vector>
+#include <list>
+#include <set>
+
+#include "KeyFrame.h"
+#include "Frame.h"
+#include "ORBVocabulary.h"
+
+#include<mutex>
+
+
+namespace ORB_SLAM2
+{
+
+class KeyFrame;
+class Frame;
+
+
+class KeyFrameDatabase
+{
+public:
+
+    KeyFrameDatabase(const ORBVocabulary &voc);
+
+   void add(KeyFrame* pKF);
+
+   void erase(KeyFrame* pKF);
+
+   void clear();
+
+   // Loop Detection
+   std::vector<KeyFrame *> DetectLoopCandidates(KeyFrame* pKF, float minScore);
+
+   // Relocalization
+   std::vector<KeyFrame*> DetectRelocalizationCandidates(Frame* F);
+
+protected:
+
+  // Associated vocabulary
+  const ORBVocabulary* mpVoc;
+
+  // Inverted file
+  std::vector<list<KeyFrame*> > mvInvertedFile;
+
+  // Mutex
+  std::mutex mMutex;
+};
+
+} //namespace ORB_SLAM
+
+#endif

include/LocalMapping.h

@@ -0,0 +1,128 @@
+/**
+* This file is part of ORB-SLAM2.
+*
+* Copyright (C) 2014-2016 Raúl Mur-Artal <raulmur at unizar dot es> (University of Zaragoza)
+* For more information see <https://github.com/raulmur/ORB_SLAM2>
+*
+* ORB-SLAM2 is free software: you can redistribute it and/or modify
+* it under the terms of the GNU General Public License as published by
+* the Free Software Foundation, either version 3 of the License, or
+* (at your option) any later version.
+*
+* ORB-SLAM2 is distributed in the hope that it will be useful,
+* but WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU General Public License for more details.
+*
+* You should have received a copy of the GNU General Public License
+* along with ORB-SLAM2. If not, see <http://www.gnu.org/licenses/>.
+*/
+
+#ifndef LOCALMAPPING_H
+#define LOCALMAPPING_H
+
+#include "KeyFrame.h"
+#include "Map.h"
+#include "LoopClosing.h"
+#include "Tracking.h"
+#include "KeyFrameDatabase.h"
+
+#include <mutex>
+
+
+namespace ORB_SLAM2
+{
+
+class Tracking;
+class LoopClosing;
+class Map;
+
+class LocalMapping
+{
+public:
+    LocalMapping(Map* pMap, const float bMonocular);
+
+    void SetLoopCloser(LoopClosing* pLoopCloser);
+
+    void SetTracker(Tracking* pTracker);
+
+    // Main function
+    void Run();
+
+    void InsertKeyFrame(KeyFrame* pKF);
+
+    // Thread Synch
+    void RequestStop();
+    void RequestReset();
+    bool Stop();
+    void Release();
+    bool isStopped();
+    bool stopRequested();
+    bool AcceptKeyFrames();
+    void SetAcceptKeyFrames(bool flag);
+    bool SetNotStop(bool flag);
+
+    void InterruptBA();
+
+    void RequestFinish();
+    bool isFinished();
+
+    int KeyframesInQueue(){
+        unique_lock<std::mutex> lock(mMutexNewKFs);
+        return mlNewKeyFrames.size();
+    }
+
+protected:
+
+    bool CheckNewKeyFrames();
+    void ProcessNewKeyFrame();
+    void CreateNewMapPoints();
+
+    void MapPointCulling();
+    void SearchInNeighbors();
+
+    void KeyFrameCulling();
+
+    cv::Mat ComputeF12(KeyFrame* &pKF1, KeyFrame* &pKF2);
+
+    cv::Mat SkewSymmetricMatrix(const cv::Mat &v);
+
+    bool mbMonocular;
+
+    void ResetIfRequested();
+    bool mbResetRequested;
+    std::mutex mMutexReset;
+
+    bool CheckFinish();
+    void SetFinish();
+    bool mbFinishRequested;
+    bool mbFinished;
+    std::mutex mMutexFinish;
+
+    Map* mpMap;
+
+    LoopClosing* mpLoopCloser;
+    Tracking* mpTracker;
+
+    std::list<KeyFrame*> mlNewKeyFrames;
+
+    KeyFrame* mpCurrentKeyFrame;
+
+    std::list<MapPoint*> mlpRecentAddedMapPoints;
+
+    std::mutex mMutexNewKFs;
+
+    bool mbAbortBA;
+
+    bool mbStopped;
+    bool mbStopRequested;
+    bool mbNotStop;
+    std::mutex mMutexStop;
+
+    bool mbAcceptKeyFrames;
+    std::mutex mMutexAccept;
+};
+
+} //namespace ORB_SLAM
+
+#endif // LOCALMAPPING_H

include/LoopClosing.h

@@ -0,0 +1,151 @@
+/**
+* This file is part of ORB-SLAM2.
+*
+* Copyright (C) 2014-2016 Raúl Mur-Artal <raulmur at unizar dot es> (University of Zaragoza)
+* For more information see <https://github.com/raulmur/ORB_SLAM2>
+*
+* ORB-SLAM2 is free software: you can redistribute it and/or modify
+* it under the terms of the GNU General Public License as published by
+* the Free Software Foundation, either version 3 of the License, or
+* (at your option) any later version.
+*
+* ORB-SLAM2 is distributed in the hope that it will be useful,
+* but WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU General Public License for more details.
+*
+* You should have received a copy of the GNU General Public License
+* along with ORB-SLAM2. If not, see <http://www.gnu.org/licenses/>.
+*/
+
+#ifndef LOOPCLOSING_H
+#define LOOPCLOSING_H
+
+#include "KeyFrame.h"
+#include "LocalMapping.h"
+#include "Map.h"
+#include "ORBVocabulary.h"
+#include "Tracking.h"
+
+#include "KeyFrameDatabase.h"
+
+#include <thread>
+#include <mutex>
+#include "Thirdparty/g2o/g2o/types/types_seven_dof_expmap.h"
+
+namespace ORB_SLAM2
+{
+
+class Tracking;
+class LocalMapping;
+class KeyFrameDatabase;
+
+
+class LoopClosing
+{
+public:
+
+    typedef pair<set<KeyFrame*>,int> ConsistentGroup;    
+    typedef map<KeyFrame*,g2o::Sim3,std::less<KeyFrame*>,
+        Eigen::aligned_allocator<std::pair<const KeyFrame*, g2o::Sim3> > > KeyFrameAndPose;
+
+public:
+
+    LoopClosing(Map* pMap, KeyFrameDatabase* pDB, ORBVocabulary* pVoc,const bool bFixScale);
+
+    void SetTracker(Tracking* pTracker);
+
+    void SetLocalMapper(LocalMapping* pLocalMapper);
+
+    // Main function
+    void Run();
+
+    void InsertKeyFrame(KeyFrame *pKF);
+
+    void RequestReset();
+
+    // This function will run in a separate thread
+    void RunGlobalBundleAdjustment(unsigned long nLoopKF);
+
+    bool isRunningGBA(){
+        unique_lock<std::mutex> lock(mMutexGBA);
+        return mbRunningGBA;
+    }
+    bool isFinishedGBA(){
+        unique_lock<std::mutex> lock(mMutexGBA);
+        return mbFinishedGBA;
+    }   
+
+    void RequestFinish();
+
+    bool isFinished();
+
+    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+protected:
+
+    bool CheckNewKeyFrames();
+
+    bool DetectLoop();
+
+    bool ComputeSim3();
+
+    void SearchAndFuse(const KeyFrameAndPose &CorrectedPosesMap);
+
+    void CorrectLoop();
+
+    void ResetIfRequested();
+    bool mbResetRequested;
+    std::mutex mMutexReset;
+
+    bool CheckFinish();
+    void SetFinish();
+    bool mbFinishRequested;
+    bool mbFinished;
+    std::mutex mMutexFinish;
+
+    Map* mpMap;
+    Tracking* mpTracker;
+
+    KeyFrameDatabase* mpKeyFrameDB;
+    ORBVocabulary* mpORBVocabulary;
+
+    LocalMapping *mpLocalMapper;
+
+    std::list<KeyFrame*> mlpLoopKeyFrameQueue;
+
+    std::mutex mMutexLoopQueue;
+
+    // Loop detector parameters
+    float mnCovisibilityConsistencyTh;
+
+    // Loop detector variables
+    KeyFrame* mpCurrentKF;
+    KeyFrame* mpMatchedKF;
+    std::vector<ConsistentGroup> mvConsistentGroups;
+    std::vector<KeyFrame*> mvpEnoughConsistentCandidates;
+    std::vector<KeyFrame*> mvpCurrentConnectedKFs;
+    std::vector<MapPoint*> mvpCurrentMatchedPoints;
+    std::vector<MapPoint*> mvpLoopMapPoints;
+    cv::Mat mScw;
+    g2o::Sim3 mg2oScw;
+
+    long unsigned int mLastLoopKFid;
+
+    // Variables related to Global Bundle Adjustment
+    bool mbRunningGBA;
+    bool mbFinishedGBA;
+    bool mbStopGBA;
+    std::mutex mMutexGBA;
+    std::thread* mpThreadGBA;
+
+    // Fix scale in the stereo/RGB-D case
+    bool mbFixScale;
+
+
+    bool mnFullBAIdx;
+};
+
+} //namespace ORB_SLAM
+
+#endif // LOOPCLOSING_H

include/Map.h

@@ -0,0 +1,85 @@
+/**
+* This file is part of ORB-SLAM2.
+*
+* Copyright (C) 2014-2016 Raúl Mur-Artal <raulmur at unizar dot es> (University of Zaragoza)
+* For more information see <https://github.com/raulmur/ORB_SLAM2>
+*
+* ORB-SLAM2 is free software: you can redistribute it and/or modify
+* it under the terms of the GNU General Public License as published by
+* the Free Software Foundation, either version 3 of the License, or
+* (at your option) any later version.
+*
+* ORB-SLAM2 is distributed in the hope that it will be useful,
+* but WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU General Public License for more details.
+*
+* You should have received a copy of the GNU General Public License
+* along with ORB-SLAM2. If not, see <http://www.gnu.org/licenses/>.
+*/
+
+#ifndef MAP_H
+#define MAP_H
+
+#include "MapPoint.h"
+#include "KeyFrame.h"
+#include <set>
+
+#include <mutex>
+
+
+
+namespace ORB_SLAM2
+{
+
+class MapPoint;
+class KeyFrame;
+
+class Map
+{
+public:
+    Map();
+
+    void AddKeyFrame(KeyFrame* pKF);
+    void AddMapPoint(MapPoint* pMP);
+    void EraseMapPoint(MapPoint* pMP);
+    void EraseKeyFrame(KeyFrame* pKF);
+    void SetReferenceMapPoints(const std::vector<MapPoint*> &vpMPs);
+    void InformNewBigChange();
+    int GetLastBigChangeIdx();
+
+    std::vector<KeyFrame*> GetAllKeyFrames();
+    std::vector<MapPoint*> GetAllMapPoints();
+    std::vector<MapPoint*> GetReferenceMapPoints();
+
+    long unsigned int MapPointsInMap();
+    long unsigned  KeyFramesInMap();
+
+    long unsigned int GetMaxKFid();
+
+    void clear();
+
+    vector<KeyFrame*> mvpKeyFrameOrigins;
+
+    std::mutex mMutexMapUpdate;
+
+    // This avoid that two points are created simultaneously in separate threads (id conflict)
+    std::mutex mMutexPointCreation;
+
+protected:
+    std::set<MapPoint*> mspMapPoints;
+    std::set<KeyFrame*> mspKeyFrames;
+
+    std::vector<MapPoint*> mvpReferenceMapPoints;
+
+    long unsigned int mnMaxKFid;
+
+    // Index related to a big change in the map (loop closure, global BA)
+    int mnBigChangeIdx;
+
+    std::mutex mMutexMap;
+};
+
+} //namespace ORB_SLAM
+
+#endif // MAP_H

include/MapDrawer.h

@@ -0,0 +1,64 @@
+/**
+* This file is part of ORB-SLAM2.
+*
+* Copyright (C) 2014-2016 Raúl Mur-Artal <raulmur at unizar dot es> (University of Zaragoza)
+* For more information see <https://github.com/raulmur/ORB_SLAM2>
+*
+* ORB-SLAM2 is free software: you can redistribute it and/or modify
+* it under the terms of the GNU General Public License as published by
+* the Free Software Foundation, either version 3 of the License, or
+* (at your option) any later version.
+*
+* ORB-SLAM2 is distributed in the hope that it will be useful,
+* but WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU General Public License for more details.
+*
+* You should have received a copy of the GNU General Public License
+* along with ORB-SLAM2. If not, see <http://www.gnu.org/licenses/>.
+*/
+
+#ifndef MAPDRAWER_H
+#define MAPDRAWER_H
+
+#include"Map.h"
+#include"MapPoint.h"
+#include"KeyFrame.h"
+#include<pangolin/pangolin.h>
+
+#include<mutex>
+
+namespace ORB_SLAM2
+{
+
+class MapDrawer
+{
+public:
+    MapDrawer(Map* pMap, const string &strSettingPath);
+
+    Map* mpMap;
+
+    void DrawMapPoints();
+    void DrawKeyFrames(const bool bDrawKF, const bool bDrawGraph);
+    void DrawCurrentCamera(pangolin::OpenGlMatrix &Twc);
+    void SetCurrentCameraPose(const cv::Mat &Tcw);
+    void SetReferenceKeyFrame(KeyFrame *pKF);
+    void GetCurrentOpenGLCameraMatrix(pangolin::OpenGlMatrix &M);
+
+private:
+
+    float mKeyFrameSize;
+    float mKeyFrameLineWidth;
+    float mGraphLineWidth;
+    float mPointSize;
+    float mCameraSize;
+    float mCameraLineWidth;
+
+    cv::Mat mCameraPose;
+
+    std::mutex mMutexCamera;
+};
+
+} //namespace ORB_SLAM
+
+#endif // MAPDRAWER_H

include/MapPoint.h

@@ -0,0 +1,152 @@
+/**
+* This file is part of ORB-SLAM2.
+*
+* Copyright (C) 2014-2016 Raúl Mur-Artal <raulmur at unizar dot es> (University of Zaragoza)
+* For more information see <https://github.com/raulmur/ORB_SLAM2>
+*
+* ORB-SLAM2 is free software: you can redistribute it and/or modify
+* it under the terms of the GNU General Public License as published by
+* the Free Software Foundation, either version 3 of the License, or
+* (at your option) any later version.
+*
+* ORB-SLAM2 is distributed in the hope that it will be useful,
+* but WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU General Public License for more details.
+*
+* You should have received a copy of the GNU General Public License
+* along with ORB-SLAM2. If not, see <http://www.gnu.org/licenses/>.
+*/
+
+#ifndef MAPPOINT_H
+#define MAPPOINT_H
+
+#include"KeyFrame.h"
+#include"Frame.h"
+#include"Map.h"
+
+#include<opencv2/core/core.hpp>
+#include<mutex>
+
+namespace ORB_SLAM2
+{
+
+class KeyFrame;
+class Map;
+class Frame;
+
+
+class MapPoint
+{
+public:
+    MapPoint(const cv::Mat &Pos, KeyFrame* pRefKF, Map* pMap);
+    MapPoint(const cv::Mat &Pos,  Map* pMap, Frame* pFrame, const int &idxF);
+
+    void SetWorldPos(const cv::Mat &Pos);
+    cv::Mat GetWorldPos();
+
+    cv::Mat GetNormal();
+    KeyFrame* GetReferenceKeyFrame();
+
+    std::map<KeyFrame*,size_t> GetObservations();
+    int Observations();
+
+    void AddObservation(KeyFrame* pKF,size_t idx);
+    void EraseObservation(KeyFrame* pKF);
+
+    int GetIndexInKeyFrame(KeyFrame* pKF);
+    bool IsInKeyFrame(KeyFrame* pKF);
+
+    void SetBadFlag();
+    bool isBad();
+
+    void Replace(MapPoint* pMP);    
+    MapPoint* GetReplaced();
+
+    void IncreaseVisible(int n=1);
+    void IncreaseFound(int n=1);
+    float GetFoundRatio();
+    inline int GetFound(){
+        return mnFound;
+    }
+
+    void ComputeDistinctiveDescriptors();
+
+    cv::Mat GetDescriptor();
+
+    void UpdateNormalAndDepth();
+
+    float GetMinDistanceInvariance();
+    float GetMaxDistanceInvariance();
+    int PredictScale(const float &currentDist, KeyFrame*pKF);
+    int PredictScale(const float &currentDist, Frame* pF);
+
+public:
+    long unsigned int mnId;
+    static long unsigned int nNextId;
+    long int mnFirstKFid;
+    long int mnFirstFrame;
+    int nObs;
+
+    // Variables used by the tracking
+    float mTrackProjX;
+    float mTrackProjY;
+    float mTrackProjXR;
+    bool mbTrackInView;
+    int mnTrackScaleLevel;
+    float mTrackViewCos;
+    long unsigned int mnTrackReferenceForFrame;
+    long unsigned int mnLastFrameSeen;
+
+    // Variables used by local mapping
+    long unsigned int mnBALocalForKF;
+    long unsigned int mnFuseCandidateForKF;
+
+    // Variables used by loop closing
+    long unsigned int mnLoopPointForKF;
+    long unsigned int mnCorrectedByKF;
+    long unsigned int mnCorrectedReference;    
+    cv::Mat mPosGBA;
+    long unsigned int mnBAGlobalForKF;
+
+
+    static std::mutex mGlobalMutex;
+
+protected:    
+
+     // Position in absolute coordinates
+     cv::Mat mWorldPos;
+
+     // Keyframes observing the point and associated index in keyframe
+     std::map<KeyFrame*,size_t> mObservations;
+
+     // Mean viewing direction
+     cv::Mat mNormalVector;
+
+     // Best descriptor to fast matching
+     cv::Mat mDescriptor;
+
+     // Reference KeyFrame
+     KeyFrame* mpRefKF;
+
+     // Tracking counters
+     int mnVisible;
+     int mnFound;
+
+     // Bad flag (we do not currently erase MapPoint from memory)
+     bool mbBad;
+     MapPoint* mpReplaced;
+
+     // Scale invariance distances
+     float mfMinDistance;
+     float mfMaxDistance;
+
+     Map* mpMap;
+
+     std::mutex mMutexPos;
+     std::mutex mMutexFeatures;
+};
+
+} //namespace ORB_SLAM
+
+#endif // MAPPOINT_H

include/ORBVocabulary.h

@@ -0,0 +1,36 @@
+/**
+* This file is part of ORB-SLAM2.
+*
+* Copyright (C) 2014-2016 Raúl Mur-Artal <raulmur at unizar dot es> (University of Zaragoza)
+* For more information see <https://github.com/raulmur/ORB_SLAM2>
+*
+* ORB-SLAM2 is free software: you can redistribute it and/or modify
+* it under the terms of the GNU General Public License as published by
+* the Free Software Foundation, either version 3 of the License, or
+* (at your option) any later version.
+*
+* ORB-SLAM2 is distributed in the hope that it will be useful,
+* but WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU General Public License for more details.
+*
+* You should have received a copy of the GNU General Public License
+* along with ORB-SLAM2. If not, see <http://www.gnu.org/licenses/>.
+*/
+
+
+#ifndef ORBVOCABULARY_H
+#define ORBVOCABULARY_H
+
+#include"Thirdparty/DBoW2/DBoW2/FORB.h"
+#include"Thirdparty/DBoW2/DBoW2/TemplatedVocabulary.h"
+
+namespace ORB_SLAM2
+{
+
+typedef DBoW2::TemplatedVocabulary<DBoW2::FORB::TDescriptor, DBoW2::FORB>
+  ORBVocabulary;
+
+} //namespace ORB_SLAM
+
+#endif // ORBVOCABULARY_H

include/ORBextractor.h

@@ -0,0 +1,116 @@
+/**
+* This file is part of ORB-SLAM2.
+*
+* Copyright (C) 2014-2016 Raúl Mur-Artal <raulmur at unizar dot es> (University of Zaragoza)
+* For more information see <https://github.com/raulmur/ORB_SLAM2>
+*
+* ORB-SLAM2 is free software: you can redistribute it and/or modify
+* it under the terms of the GNU General Public License as published by
+* the Free Software Foundation, either version 3 of the License, or
+* (at your option) any later version.
+*
+* ORB-SLAM2 is distributed in the hope that it will be useful,
+* but WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU General Public License for more details.
+*
+* You should have received a copy of the GNU General Public License
+* along with ORB-SLAM2. If not, see <http://www.gnu.org/licenses/>.
+*/
+
+#ifndef ORBEXTRACTOR_H
+#define ORBEXTRACTOR_H
+
+#include <vector>
+#include <list>
+#include <opencv/cv.h>
+
+
+namespace ORB_SLAM2
+{
+
+class ExtractorNode
+{
+public:
+    ExtractorNode():bNoMore(false){}
+
+    void DivideNode(ExtractorNode &n1, ExtractorNode &n2, ExtractorNode &n3, ExtractorNode &n4);
+
+    std::vector<cv::KeyPoint> vKeys;
+    cv::Point2i UL, UR, BL, BR;
+    std::list<ExtractorNode>::iterator lit;
+    bool bNoMore;
+};
+
+class ORBextractor
+{
+public:
+    
+    enum {HARRIS_SCORE=0, FAST_SCORE=1 };
+
+    ORBextractor(int nfeatures, float scaleFactor, int nlevels,
+                 int iniThFAST, int minThFAST);
+
+    ~ORBextractor(){}
+
+    // Compute the ORB features and descriptors on an image.
+    // ORB are dispersed on the image using an octree.
+    // Mask is ignored in the current implementation.
+    void operator()( cv::InputArray image, cv::InputArray mask,
+      std::vector<cv::KeyPoint>& keypoints,
+      cv::OutputArray descriptors);
+
+    int inline GetLevels(){
+        return nlevels;}
+
+    float inline GetScaleFactor(){
+        return scaleFactor;}
+
+    std::vector<float> inline GetScaleFactors(){
+        return mvScaleFactor;
+    }
+
+    std::vector<float> inline GetInverseScaleFactors(){
+        return mvInvScaleFactor;
+    }
+
+    std::vector<float> inline GetScaleSigmaSquares(){
+        return mvLevelSigma2;
+    }
+
+    std::vector<float> inline GetInverseScaleSigmaSquares(){
+        return mvInvLevelSigma2;
+    }
+
+    std::vector<cv::Mat> mvImagePyramid;
+
+protected:
+
+    void ComputePyramid(cv::Mat image);
+    void ComputeKeyPointsOctTree(std::vector<std::vector<cv::KeyPoint> >& allKeypoints);    
+    std::vector<cv::KeyPoint> DistributeOctTree(const std::vector<cv::KeyPoint>& vToDistributeKeys, const int &minX,
+                                           const int &maxX, const int &minY, const int &maxY, const int &nFeatures, const int &level);
+
+    void ComputeKeyPointsOld(std::vector<std::vector<cv::KeyPoint> >& allKeypoints);
+    std::vector<cv::Point> pattern;
+
+    int nfeatures;
+    double scaleFactor;
+    int nlevels;
+    int iniThFAST;
+    int minThFAST;
+
+    std::vector<int> mnFeaturesPerLevel;
+
+    std::vector<int> umax;
+
+    std::vector<float> mvScaleFactor;
+    std::vector<float> mvInvScaleFactor;    
+    std::vector<float> mvLevelSigma2;
+    std::vector<float> mvInvLevelSigma2;
+};
+
+} //namespace ORB_SLAM
+
+#endif
+

include/ORBmatcher.h

@@ -0,0 +1,106 @@
+/**
+* This file is part of ORB-SLAM2.
+*
+* Copyright (C) 2014-2016 Raúl Mur-Artal <raulmur at unizar dot es> (University of Zaragoza)
+* For more information see <https://github.com/raulmur/ORB_SLAM2>
+*
+* ORB-SLAM2 is free software: you can redistribute it and/or modify
+* it under the terms of the GNU General Public License as published by
+* the Free Software Foundation, either version 3 of the License, or
+* (at your option) any later version.
+*
+* ORB-SLAM2 is distributed in the hope that it will be useful,
+* but WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU General Public License for more details.
+*
+* You should have received a copy of the GNU General Public License
+* along with ORB-SLAM2. If not, see <http://www.gnu.org/licenses/>.
+*/
+
+
+#ifndef ORBMATCHER_H
+#define ORBMATCHER_H
+
+#include<vector>
+#include<opencv2/core/core.hpp>
+#include<opencv2/features2d/features2d.hpp>
+
+#include"MapPoint.h"
+#include"KeyFrame.h"
+#include"Frame.h"
+
+
+namespace ORB_SLAM2
+{
+
+class ORBmatcher
+{    
+public:
+
+    ORBmatcher(float nnratio=0.6, bool checkOri=true);
+
+    // Computes the Hamming distance between two ORB descriptors
+    static int DescriptorDistance(const cv::Mat &a, const cv::Mat &b);
+
+    // Search matches between Frame keypoints and projected MapPoints. Returns number of matches
+    // Used to track the local map (Tracking)
+    int SearchByProjection(Frame &F, const std::vector<MapPoint*> &vpMapPoints, const float th=3);
+
+    // Project MapPoints tracked in last frame into the current frame and search matches.
+    // Used to track from previous frame (Tracking)
+    int SearchByProjection(Frame &CurrentFrame, const Frame &LastFrame, const float th, const bool bMono);
+
+    // Project MapPoints seen in KeyFrame into the Frame and search matches.
+    // Used in relocalisation (Tracking)
+    int SearchByProjection(Frame &CurrentFrame, KeyFrame* pKF, const std::set<MapPoint*> &sAlreadyFound, const float th, const int ORBdist);
+
+    // Project MapPoints using a Similarity Transformation and search matches.
+    // Used in loop detection (Loop Closing)
+     int SearchByProjection(KeyFrame* pKF, cv::Mat Scw, const std::vector<MapPoint*> &vpPoints, std::vector<MapPoint*> &vpMatched, int th);
+
+    // Search matches between MapPoints in a KeyFrame and ORB in a Frame.
+    // Brute force constrained to ORB that belong to the same vocabulary node (at a certain level)
+    // Used in Relocalisation and Loop Detection
+    int SearchByBoW(KeyFrame *pKF, Frame &F, std::vector<MapPoint*> &vpMapPointMatches);
+    int SearchByBoW(KeyFrame *pKF1, KeyFrame* pKF2, std::vector<MapPoint*> &vpMatches12);
+
+    // Matching for the Map Initialization (only used in the monocular case)
+    int SearchForInitialization(Frame &F1, Frame &F2, std::vector<cv::Point2f> &vbPrevMatched, std::vector<int> &vnMatches12, int windowSize=10);
+
+    // Matching to triangulate new MapPoints. Check Epipolar Constraint.
+    int SearchForTriangulation(KeyFrame *pKF1, KeyFrame* pKF2, cv::Mat F12,
+                               std::vector<pair<size_t, size_t> > &vMatchedPairs, const bool bOnlyStereo);
+
+    // Search matches between MapPoints seen in KF1 and KF2 transforming by a Sim3 [s12*R12|t12]
+    // In the stereo and RGB-D case, s12=1
+    int SearchBySim3(KeyFrame* pKF1, KeyFrame* pKF2, std::vector<MapPoint *> &vpMatches12, const float &s12, const cv::Mat &R12, const cv::Mat &t12, const float th);
+
+    // Project MapPoints into KeyFrame and search for duplicated MapPoints.
+    int Fuse(KeyFrame* pKF, const vector<MapPoint *> &vpMapPoints, const float th=3.0);
+
+    // Project MapPoints into KeyFrame using a given Sim3 and search for duplicated MapPoints.
+    int Fuse(KeyFrame* pKF, cv::Mat Scw, const std::vector<MapPoint*> &vpPoints, float th, vector<MapPoint *> &vpReplacePoint);
+
+public:
+
+    static const int TH_LOW;
+    static const int TH_HIGH;
+    static const int HISTO_LENGTH;
+
+
+protected:
+
+    bool CheckDistEpipolarLine(const cv::KeyPoint &kp1, const cv::KeyPoint &kp2, const cv::Mat &F12, const KeyFrame *pKF);
+
+    float RadiusByViewingCos(const float &viewCos);
+
+    void ComputeThreeMaxima(std::vector<int>* histo, const int L, int &ind1, int &ind2, int &ind3);
+
+    float mfNNratio;
+    bool mbCheckOrientation;
+};
+
+}// namespace ORB_SLAM
+
+#endif // ORBMATCHER_H

include/Optimizer.h

@@ -0,0 +1,62 @@
+/**
+* This file is part of ORB-SLAM2.
+*
+* Copyright (C) 2014-2016 Raúl Mur-Artal <raulmur at unizar dot es> (University of Zaragoza)
+* For more information see <https://github.com/raulmur/ORB_SLAM2>
+*
+* ORB-SLAM2 is free software: you can redistribute it and/or modify
+* it under the terms of the GNU General Public License as published by
+* the Free Software Foundation, either version 3 of the License, or
+* (at your option) any later version.
+*
+* ORB-SLAM2 is distributed in the hope that it will be useful,
+* but WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU General Public License for more details.
+*
+* You should have received a copy of the GNU General Public License
+* along with ORB-SLAM2. If not, see <http://www.gnu.org/licenses/>.
+*/
+
+#ifndef OPTIMIZER_H
+#define OPTIMIZER_H
+
+#include "Map.h"
+#include "MapPoint.h"
+#include "KeyFrame.h"
+#include "LoopClosing.h"
+#include "Frame.h"
+
+#include "Thirdparty/g2o/g2o/types/types_seven_dof_expmap.h"
+
+namespace ORB_SLAM2
+{
+
+class LoopClosing;
+
+class Optimizer
+{
+public:
+    void static BundleAdjustment(const std::vector<KeyFrame*> &vpKF, const std::vector<MapPoint*> &vpMP,
+                                 int nIterations = 5, bool *pbStopFlag=NULL, const unsigned long nLoopKF=0,
+                                 const bool bRobust = true);
+    void static GlobalBundleAdjustemnt(Map* pMap, int nIterations=5, bool *pbStopFlag=NULL,
+                                       const unsigned long nLoopKF=0, const bool bRobust = true);
+    void static LocalBundleAdjustment(KeyFrame* pKF, bool *pbStopFlag, Map *pMap);
+    int static PoseOptimization(Frame* pFrame);
+
+    // if bFixScale is true, 6DoF optimization (stereo,rgbd), 7DoF otherwise (mono)
+    void static OptimizeEssentialGraph(Map* pMap, KeyFrame* pLoopKF, KeyFrame* pCurKF,
+                                       const LoopClosing::KeyFrameAndPose &NonCorrectedSim3,
+                                       const LoopClosing::KeyFrameAndPose &CorrectedSim3,
+                                       const map<KeyFrame *, set<KeyFrame *> > &LoopConnections,
+                                       const bool &bFixScale);
+
+    // if bFixScale is true, optimize SE3 (stereo,rgbd), Sim3 otherwise (mono)
+    static int OptimizeSim3(KeyFrame* pKF1, KeyFrame* pKF2, std::vector<MapPoint *> &vpMatches1,
+                            g2o::Sim3 &g2oS12, const float th2, const bool bFixScale);
+};
+
+} //namespace ORB_SLAM
+
+#endif // OPTIMIZER_H

include/PnPsolver.h

@@ -0,0 +1,198 @@
+/**
+* This file is part of ORB-SLAM2.
+* This file is a modified version of EPnP <http://cvlab.epfl.ch/EPnP/index.php>, see FreeBSD license below.
+*
+* Copyright (C) 2014-2016 Raúl Mur-Artal <raulmur at unizar dot es> (University of Zaragoza)
+* For more information see <https://github.com/raulmur/ORB_SLAM2>
+*
+* ORB-SLAM2 is free software: you can redistribute it and/or modify
+* it under the terms of the GNU General Public License as published by
+* the Free Software Foundation, either version 3 of the License, or
+* (at your option) any later version.
+*
+* ORB-SLAM2 is distributed in the hope that it will be useful,
+* but WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU General Public License for more details.
+*
+* You should have received a copy of the GNU General Public License
+* along with ORB-SLAM2. If not, see <http://www.gnu.org/licenses/>.
+*/
+
+/**
+* Copyright (c) 2009, V. Lepetit, EPFL
+* All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions are met:
+*
+* 1. Redistributions of source code must retain the above copyright notice, this
+*    list of conditions and the following disclaimer.
+* 2. Redistributions in binary form must reproduce the above copyright notice,
+*    this list of conditions and the following disclaimer in the documentation
+*    and/or other materials provided with the distribution.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR
+* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*
+* The views and conclusions contained in the software and documentation are those
+* of the authors and should not be interpreted as representing official policies,
+*   either expressed or implied, of the FreeBSD Project
+*/
+
+#ifndef PNPSOLVER_H
+#define PNPSOLVER_H
+
+#include <opencv2/core/core.hpp>
+#include "MapPoint.h"
+#include "Frame.h"
+
+namespace ORB_SLAM2
+{
+
+class PnPsolver {
+ public:
+  PnPsolver(const Frame &F, const vector<MapPoint*> &vpMapPointMatches);
+
+  ~PnPsolver();
+
+  void SetRansacParameters(double probability = 0.99, int minInliers = 8 , int maxIterations = 300, int minSet = 4, float epsilon = 0.4,
+                           float th2 = 5.991);
+
+  cv::Mat find(vector<bool> &vbInliers, int &nInliers);
+
+  cv::Mat iterate(int nIterations, bool &bNoMore, vector<bool> &vbInliers, int &nInliers);
+
+ private:
+
+  void CheckInliers();
+  bool Refine();
+
+  // Functions from the original EPnP code
+  void set_maximum_number_of_correspondences(const int n);
+  void reset_correspondences(void);
+  void add_correspondence(const double X, const double Y, const double Z,
+              const double u, const double v);
+
+  double compute_pose(double R[3][3], double T[3]);
+
+  void relative_error(double & rot_err, double & transl_err,
+              const double Rtrue[3][3], const double ttrue[3],
+              const double Rest[3][3],  const double test[3]);
+
+  void print_pose(const double R[3][3], const double t[3]);
+  double reprojection_error(const double R[3][3], const double t[3]);
+
+  void choose_control_points(void);
+  void compute_barycentric_coordinates(void);
+  void fill_M(CvMat * M, const int row, const double * alphas, const double u, const double v);
+  void compute_ccs(const double * betas, const double * ut);
+  void compute_pcs(void);
+
+  void solve_for_sign(void);
+
+  void find_betas_approx_1(const CvMat * L_6x10, const CvMat * Rho, double * betas);
+  void find_betas_approx_2(const CvMat * L_6x10, const CvMat * Rho, double * betas);
+  void find_betas_approx_3(const CvMat * L_6x10, const CvMat * Rho, double * betas);
+  void qr_solve(CvMat * A, CvMat * b, CvMat * X);
+
+  double dot(const double * v1, const double * v2);
+  double dist2(const double * p1, const double * p2);
+
+  void compute_rho(double * rho);
+  void compute_L_6x10(const double * ut, double * l_6x10);
+
+  void gauss_newton(const CvMat * L_6x10, const CvMat * Rho, double current_betas[4]);
+  void compute_A_and_b_gauss_newton(const double * l_6x10, const double * rho,
+				    double cb[4], CvMat * A, CvMat * b);
+
+  double compute_R_and_t(const double * ut, const double * betas,
+			 double R[3][3], double t[3]);
+
+  void estimate_R_and_t(double R[3][3], double t[3]);
+
+  void copy_R_and_t(const double R_dst[3][3], const double t_dst[3],
+		    double R_src[3][3], double t_src[3]);
+
+  void mat_to_quat(const double R[3][3], double q[4]);
+
+
+  double uc, vc, fu, fv;
+
+  double * pws, * us, * alphas, * pcs;
+  int maximum_number_of_correspondences;
+  int number_of_correspondences;
+
+  double cws[4][3], ccs[4][3];
+  double cws_determinant;
+
+  vector<MapPoint*> mvpMapPointMatches;
+
+  // 2D Points
+  vector<cv::Point2f> mvP2D;
+  vector<float> mvSigma2;
+
+  // 3D Points
+  vector<cv::Point3f> mvP3Dw;
+
+  // Index in Frame
+  vector<size_t> mvKeyPointIndices;
+
+  // Current Estimation
+  double mRi[3][3];
+  double mti[3];
+  cv::Mat mTcwi;
+  vector<bool> mvbInliersi;
+  int mnInliersi;
+
+  // Current Ransac State
+  int mnIterations;
+  vector<bool> mvbBestInliers;
+  int mnBestInliers;
+  cv::Mat mBestTcw;
+
+  // Refined
+  cv::Mat mRefinedTcw;
+  vector<bool> mvbRefinedInliers;
+  int mnRefinedInliers;
+
+  // Number of Correspondences
+  int N;
+
+  // Indices for random selection [0 .. N-1]
+  vector<size_t> mvAllIndices;
+
+  // RANSAC probability
+  double mRansacProb;
+
+  // RANSAC min inliers
+  int mRansacMinInliers;
+
+  // RANSAC max iterations
+  int mRansacMaxIts;
+
+  // RANSAC expected inliers/total ratio
+  float mRansacEpsilon;
+
+  // RANSAC Threshold inlier/outlier. Max error e = dist(P1,T_12*P2)^2
+  float mRansacTh;
+
+  // RANSAC Minimun Set used at each iteration
+  int mRansacMinSet;
+
+  // Max square error associated with scale level. Max error = th*th*sigma(level)*sigma(level)
+  vector<float> mvMaxError;
+
+};
+
+} //namespace ORB_SLAM
+
+#endif //PNPSOLVER_H

include/Sim3Solver.h

@@ -0,0 +1,133 @@
+/**
+* This file is part of ORB-SLAM2.
+*
+* Copyright (C) 2014-2016 Raúl Mur-Artal <raulmur at unizar dot es> (University of Zaragoza)
+* For more information see <https://github.com/raulmur/ORB_SLAM2>
+*
+* ORB-SLAM2 is free software: you can redistribute it and/or modify
+* it under the terms of the GNU General Public License as published by
+* the Free Software Foundation, either version 3 of the License, or
+* (at your option) any later version.
+*
+* ORB-SLAM2 is distributed in the hope that it will be useful,
+* but WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU General Public License for more details.
+*
+* You should have received a copy of the GNU General Public License
+* along with ORB-SLAM2. If not, see <http://www.gnu.org/licenses/>.
+*/
+
+
+#ifndef SIM3SOLVER_H
+#define SIM3SOLVER_H
+
+#include <opencv2/opencv.hpp>
+#include <vector>
+
+#include "KeyFrame.h"
+
+
+
+namespace ORB_SLAM2
+{
+
+class Sim3Solver
+{
+public:
+
+    Sim3Solver(KeyFrame* pKF1, KeyFrame* pKF2, const std::vector<MapPoint*> &vpMatched12, const bool bFixScale = true);
+
+    void SetRansacParameters(double probability = 0.99, int minInliers = 6 , int maxIterations = 300);
+
+    cv::Mat find(std::vector<bool> &vbInliers12, int &nInliers);
+
+    cv::Mat iterate(int nIterations, bool &bNoMore, std::vector<bool> &vbInliers, int &nInliers);
+
+    cv::Mat GetEstimatedRotation();
+    cv::Mat GetEstimatedTranslation();
+    float GetEstimatedScale();
+
+
+protected:
+
+    void ComputeCentroid(cv::Mat &P, cv::Mat &Pr, cv::Mat &C);
+
+    void ComputeSim3(cv::Mat &P1, cv::Mat &P2);
+
+    void CheckInliers();
+
+    void Project(const std::vector<cv::Mat> &vP3Dw, std::vector<cv::Mat> &vP2D, cv::Mat Tcw, cv::Mat K);
+    void FromCameraToImage(const std::vector<cv::Mat> &vP3Dc, std::vector<cv::Mat> &vP2D, cv::Mat K);
+
+
+protected:
+
+    // KeyFrames and matches
+    KeyFrame* mpKF1;
+    KeyFrame* mpKF2;
+
+    std::vector<cv::Mat> mvX3Dc1;
+    std::vector<cv::Mat> mvX3Dc2;
+    std::vector<MapPoint*> mvpMapPoints1;
+    std::vector<MapPoint*> mvpMapPoints2;
+    std::vector<MapPoint*> mvpMatches12;
+    std::vector<size_t> mvnIndices1;
+    std::vector<size_t> mvSigmaSquare1;
+    std::vector<size_t> mvSigmaSquare2;
+    std::vector<size_t> mvnMaxError1;
+    std::vector<size_t> mvnMaxError2;
+
+    int N;
+    int mN1;
+
+    // Current Estimation
+    cv::Mat mR12i;
+    cv::Mat mt12i;
+    float ms12i;
+    cv::Mat mT12i;
+    cv::Mat mT21i;
+    std::vector<bool> mvbInliersi;
+    int mnInliersi;
+
+    // Current Ransac State
+    int mnIterations;
+    std::vector<bool> mvbBestInliers;
+    int mnBestInliers;
+    cv::Mat mBestT12;
+    cv::Mat mBestRotation;
+    cv::Mat mBestTranslation;
+    float mBestScale;
+
+    // Scale is fixed to 1 in the stereo/RGBD case
+    bool mbFixScale;
+
+    // Indices for random selection
+    std::vector<size_t> mvAllIndices;
+
+    // Projections
+    std::vector<cv::Mat> mvP1im1;
+    std::vector<cv::Mat> mvP2im2;
+
+    // RANSAC probability
+    double mRansacProb;
+
+    // RANSAC min inliers
+    int mRansacMinInliers;
+
+    // RANSAC max iterations
+    int mRansacMaxIts;
+
+    // Threshold inlier/outlier. e = dist(Pi,T_ij*Pj)^2 < 5.991*mSigma2
+    float mTh;
+    float mSigma2;
+
+    // Calibration
+    cv::Mat mK1;
+    cv::Mat mK2;
+
+};
+
+} //namespace ORB_SLAM
+
+#endif // SIM3SOLVER_H

include/System.h

@@ -0,0 +1,183 @@
+/**
+* This file is part of ORB-SLAM2.
+*
+* Copyright (C) 2014-2016 Raúl Mur-Artal <raulmur at unizar dot es> (University of Zaragoza)
+* For more information see <https://github.com/raulmur/ORB_SLAM2>
+*
+* ORB-SLAM2 is free software: you can redistribute it and/or modify
+* it under the terms of the GNU General Public License as published by
+* the Free Software Foundation, either version 3 of the License, or
+* (at your option) any later version.
+*
+* ORB-SLAM2 is distributed in the hope that it will be useful,
+* but WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU General Public License for more details.
+*
+* You should have received a copy of the GNU General Public License
+* along with ORB-SLAM2. If not, see <http://www.gnu.org/licenses/>.
+*/
+
+
+#ifndef SYSTEM_H
+#define SYSTEM_H
+
+#include<string>
+#include<thread>
+#include<opencv2/core/core.hpp>
+
+#include "Tracking.h"
+#include "FrameDrawer.h"
+#include "MapDrawer.h"
+#include "Map.h"
+#include "LocalMapping.h"
+#include "LoopClosing.h"
+#include "KeyFrameDatabase.h"
+#include "ORBVocabulary.h"
+#include "Viewer.h"
+
+void usleep(__int64 usec);
+
+namespace ORB_SLAM2
+{
+
+class Viewer;
+class FrameDrawer;
+class Map;
+class Tracking;
+class LocalMapping;
+class LoopClosing;
+
+class System
+{
+public:
+    // Input sensor
+    enum eSensor{
+        MONOCULAR=0,
+        STEREO=1,
+        RGBD=2
+    };
+
+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);
+
+    // 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.
+    // Returns the camera pose (empty if tracking fails).
+    cv::Mat TrackStereo(const cv::Mat &imLeft, const cv::Mat &imRight, const double &timestamp);
+
+    // Process the given rgbd frame. Depthmap must be registered to the RGB frame.
+    // Input image: RGB (CV_8UC3) or grayscale (CV_8U). RGB is converted to grayscale.
+    // Input depthmap: Float (CV_32F).
+    // Returns the camera pose (empty if tracking fails).
+    cv::Mat TrackRGBD(const cv::Mat &im, const cv::Mat &depthmap, const double &timestamp);
+
+    // Proccess the given monocular frame
+    // Input images: RGB (CV_8UC3) or grayscale (CV_8U). RGB is converted to grayscale.
+    // Returns the camera pose (empty if tracking fails).
+    cv::Mat TrackMonocular(const cv::Mat &im, const double &timestamp);
+
+    // This stops local mapping thread (map building) and performs only camera tracking.
+    void ActivateLocalizationMode();
+    // This resumes local mapping thread and performs SLAM again.
+    void DeactivateLocalizationMode();
+
+    // Returns true if there have been a big map change (loop closure, global BA)
+    // since last call to this function
+    bool MapChanged();
+
+    // Reset the system (clear map)
+    void Reset();
+
+    // All threads will be requested to finish.
+    // It waits until all threads have finished.
+    // This function must be called before saving the trajectory.
+    void Shutdown();
+
+    // Save camera trajectory in the TUM RGB-D dataset format.
+    // Only for stereo and RGB-D. This method does not work for monocular.
+    // Call first Shutdown()
+    // See format details at: http://vision.in.tum.de/data/datasets/rgbd-dataset
+    void SaveTrajectoryTUM(const string &filename);
+
+    // Save keyframe poses in the TUM RGB-D dataset format.
+    // This method works for all sensor input.
+    // Call first Shutdown()
+    // See format details at: http://vision.in.tum.de/data/datasets/rgbd-dataset
+    void SaveKeyFrameTrajectoryTUM(const string &filename);
+
+    // Save camera trajectory in the KITTI dataset format.
+    // Only for stereo and RGB-D. This method does not work for monocular.
+    // Call first Shutdown()
+    // See format details at: http://www.cvlibs.net/datasets/kitti/eval_odometry.php
+    void SaveTrajectoryKITTI(const string &filename);
+
+    // TODO: Save/Load functions
+    // SaveMap(const string &filename);
+    // LoadMap(const string &filename);
+
+    // Information from most recent processed frame
+    // You can call this right after TrackMonocular (or stereo or RGBD)
+    int GetTrackingState();
+    std::vector<MapPoint*> GetTrackedMapPoints();
+    std::vector<cv::KeyPoint> GetTrackedKeyPointsUn();
+
+private:
+
+    // Input sensor
+    eSensor mSensor;
+
+    // ORB vocabulary used for place recognition and feature matching.
+    ORBVocabulary* mpVocabulary;
+
+    // KeyFrame database for place recognition (relocalization and loop detection).
+    KeyFrameDatabase* mpKeyFrameDatabase;
+
+    // Map structure that stores the pointers to all KeyFrames and MapPoints.
+    Map* mpMap;
+
+    // Tracker. It receives a frame and computes the associated camera pose.
+    // It also decides when to insert a new keyframe, create some new MapPoints and
+    // performs relocalization if tracking fails.
+    Tracking* mpTracker;
+
+    // Local Mapper. It manages the local map and performs local bundle adjustment.
+    LocalMapping* mpLocalMapper;
+
+    // Loop Closer. It searches loops with every new keyframe. If there is a loop it performs
+    // a pose graph optimization and full bundle adjustment (in a new thread) afterwards.
+    LoopClosing* mpLoopCloser;
+
+    // The viewer draws the map and the current camera pose. It uses Pangolin.
+    Viewer* mpViewer;
+
+    FrameDrawer* mpFrameDrawer;
+    MapDrawer* mpMapDrawer;
+
+    // System threads: Local Mapping, Loop Closing, Viewer.
+    // The Tracking thread "lives" in the main execution thread that creates the System object.
+    std::thread* mptLocalMapping;
+    std::thread* mptLoopClosing;
+    std::thread* mptViewer;
+
+    // Reset flag
+    std::mutex mMutexReset;
+    bool mbReset;
+
+    // Change mode flags
+    std::mutex mMutexMode;
+    bool mbActivateLocalizationMode;
+    bool mbDeactivateLocalizationMode;
+
+    // Tracking state
+    int mTrackingState;
+    std::vector<MapPoint*> mTrackedMapPoints;
+    std::vector<cv::KeyPoint> mTrackedKeyPointsUn;
+    std::mutex mMutexState;
+};
+
+}// namespace ORB_SLAM
+
+#endif // SYSTEM_H

include/Tracking.h

@@ -0,0 +1,221 @@
+/**
+* This file is part of ORB-SLAM2.
+*
+* Copyright (C) 2014-2016 Raúl Mur-Artal <raulmur at unizar dot es> (University of Zaragoza)
+* For more information see <https://github.com/raulmur/ORB_SLAM2>
+*
+* ORB-SLAM2 is free software: you can redistribute it and/or modify
+* it under the terms of the GNU General Public License as published by
+* the Free Software Foundation, either version 3 of the License, or
+* (at your option) any later version.
+*
+* ORB-SLAM2 is distributed in the hope that it will be useful,
+* but WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU General Public License for more details.
+*
+* You should have received a copy of the GNU General Public License
+* along with ORB-SLAM2. If not, see <http://www.gnu.org/licenses/>.
+*/
+
+
+#ifndef TRACKING_H
+#define TRACKING_H
+
+#include<opencv2/core/core.hpp>
+#include<opencv2/features2d/features2d.hpp>
+
+#include"Viewer.h"
+#include"FrameDrawer.h"
+#include"Map.h"
+#include"LocalMapping.h"
+#include"LoopClosing.h"
+#include"Frame.h"
+#include "ORBVocabulary.h"
+#include"KeyFrameDatabase.h"
+#include"ORBextractor.h"
+#include "Initializer.h"
+#include "MapDrawer.h"
+#include "System.h"
+
+#include <mutex>
+
+namespace ORB_SLAM2
+{
+
+class Viewer;
+class FrameDrawer;
+class Map;
+class LocalMapping;
+class LoopClosing;
+class System;
+
+class Tracking
+{  
+
+public:
+    Tracking(System* pSys, ORBVocabulary* pVoc, FrameDrawer* pFrameDrawer, MapDrawer* pMapDrawer, Map* pMap,
+             KeyFrameDatabase* pKFDB, const string &strSettingPath, const int sensor);
+
+    // 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);
+    cv::Mat GrabImageMonocular(const cv::Mat &im, const double &timestamp);
+
+    void SetLocalMapper(LocalMapping* pLocalMapper);
+    void SetLoopClosing(LoopClosing* pLoopClosing);
+    void SetViewer(Viewer* pViewer);
+
+    // Load new settings
+    // The focal lenght should be similar or scale prediction will fail when projecting points
+    // TODO: Modify MapPoint::PredictScale to take into account focal lenght
+    void ChangeCalibration(const string &strSettingPath);
+
+    // Use this function if you have deactivated local mapping and you only want to localize the camera.
+    void InformOnlyTracking(const bool &flag);
+
+
+public:
+
+    // Tracking states
+    enum eTrackingState{
+        SYSTEM_NOT_READY=-1,
+        NO_IMAGES_YET=0,
+        NOT_INITIALIZED=1,
+        OK=2,
+        LOST=3
+    };
+
+    eTrackingState mState;
+    eTrackingState mLastProcessedState;
+
+    // Input sensor
+    int mSensor;
+
+    // Current Frame
+    Frame mCurrentFrame;
+    cv::Mat mImGray;
+
+    // Initialization Variables (Monocular)
+    std::vector<int> mvIniLastMatches;
+    std::vector<int> mvIniMatches;
+    std::vector<cv::Point2f> mvbPrevMatched;
+    std::vector<cv::Point3f> mvIniP3D;
+    Frame mInitialFrame;
+
+    // Lists used to recover the full camera trajectory at the end of the execution.
+    // Basically we store the reference keyframe for each frame and its relative transformation
+    list<cv::Mat> mlRelativeFramePoses;
+    list<KeyFrame*> mlpReferences;
+    list<double> mlFrameTimes;
+    list<bool> mlbLost;
+
+    // True if local mapping is deactivated and we are performing only localization
+    bool mbOnlyTracking;
+
+    void Reset();
+
+protected:
+
+    // Main tracking function. It is independent of the input sensor.
+    void Track();
+
+    // Map initialization for stereo and RGB-D
+    void StereoInitialization();
+
+    // Map initialization for monocular
+    void MonocularInitialization();
+    void CreateInitialMapMonocular();
+
+    void CheckReplacedInLastFrame();
+    bool TrackReferenceKeyFrame();
+    void UpdateLastFrame();
+    bool TrackWithMotionModel();
+
+    bool Relocalization();
+
+    void UpdateLocalMap();
+    void UpdateLocalPoints();
+    void UpdateLocalKeyFrames();
+
+    bool TrackLocalMap();
+    void SearchLocalPoints();
+
+    bool NeedNewKeyFrame();
+    void CreateNewKeyFrame();
+
+    // In case of performing only localization, this flag is true when there are no matches to
+    // points in the map. Still tracking will continue if there are enough matches with temporal points.
+    // In that case we are doing visual odometry. The system will try to do relocalization to recover
+    // "zero-drift" localization to the map.
+    bool mbVO;
+
+    //Other Thread Pointers
+    LocalMapping* mpLocalMapper;
+    LoopClosing* mpLoopClosing;
+
+    //ORB
+    ORBextractor* mpORBextractorLeft, *mpORBextractorRight;
+    ORBextractor* mpIniORBextractor;
+
+    //BoW
+    ORBVocabulary* mpORBVocabulary;
+    KeyFrameDatabase* mpKeyFrameDB;
+
+    // Initalization (only for monocular)
+    Initializer* mpInitializer;
+
+    //Local Map
+    KeyFrame* mpReferenceKF;
+    std::vector<KeyFrame*> mvpLocalKeyFrames;
+    std::vector<MapPoint*> mvpLocalMapPoints;
+    
+    // System
+    System* mpSystem;
+    
+    //Drawers
+    Viewer* mpViewer;
+    FrameDrawer* mpFrameDrawer;
+    MapDrawer* mpMapDrawer;
+
+    //Map
+    Map* mpMap;
+
+    //Calibration matrix
+    cv::Mat mK;
+    cv::Mat mDistCoef;
+    float mbf;
+
+    //New KeyFrame rules (according to fps)
+    int mMinFrames;
+    int mMaxFrames;
+
+    // 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;
+
+    // For RGB-D inputs only. For some datasets (e.g. TUM) the depthmap values are scaled.
+    float mDepthMapFactor;
+
+    //Current matches in frame
+    int mnMatchesInliers;
+
+    //Last Frame, KeyFrame and Relocalisation Info
+    KeyFrame* mpLastKeyFrame;
+    Frame mLastFrame;
+    unsigned int mnLastKeyFrameId;
+    unsigned int mnLastRelocFrameId;
+
+    //Motion Model
+    cv::Mat mVelocity;
+
+    //Color order (true RGB, false BGR, ignored if grayscale)
+    bool mbRGB;
+
+    list<MapPoint*> mlpTemporalPoints;
+};
+
+} //namespace ORB_SLAM
+
+#endif // TRACKING_H

include/Viewer.h

@@ -0,0 +1,91 @@
+/**
+* This file is part of ORB-SLAM2.
+*
+* Copyright (C) 2014-2016 Raúl Mur-Artal <raulmur at unizar dot es> (University of Zaragoza)
+* For more information see <https://github.com/raulmur/ORB_SLAM2>
+*
+* ORB-SLAM2 is free software: you can redistribute it and/or modify
+* it under the terms of the GNU General Public License as published by
+* the Free Software Foundation, either version 3 of the License, or
+* (at your option) any later version.
+*
+* ORB-SLAM2 is distributed in the hope that it will be useful,
+* but WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU General Public License for more details.
+*
+* You should have received a copy of the GNU General Public License
+* along with ORB-SLAM2. If not, see <http://www.gnu.org/licenses/>.
+*/
+
+
+#ifndef VIEWER_H
+#define VIEWER_H
+
+#include "FrameDrawer.h"
+#include "MapDrawer.h"
+#include "Tracking.h"
+#include "System.h"
+
+#include <mutex>
+
+namespace ORB_SLAM2
+{
+
+class Tracking;
+class FrameDrawer;
+class MapDrawer;
+class System;
+
+class Viewer
+{
+public:
+    Viewer(System* pSystem, FrameDrawer* pFrameDrawer, MapDrawer* pMapDrawer, Tracking *pTracking, const string &strSettingPath);
+
+    // Main thread function. Draw points, keyframes, the current camera pose and the last processed
+    // frame. Drawing is refreshed according to the camera fps. We use Pangolin.
+    void Run();
+
+    void RequestFinish();
+
+    void RequestStop();
+
+    bool isFinished();
+
+    bool isStopped();
+
+    void Release();
+
+private:
+
+    bool Stop();
+
+    System* mpSystem;
+    FrameDrawer* mpFrameDrawer;
+    MapDrawer* mpMapDrawer;
+    Tracking* mpTracker;
+
+    // 1/fps in ms
+    double mT;
+    float mImageWidth, mImageHeight;
+
+    float mViewpointX, mViewpointY, mViewpointZ, mViewpointF;
+
+    bool CheckFinish();
+    void SetFinish();
+    bool mbFinishRequested;
+    bool mbFinished;
+    std::mutex mMutexFinish;
+
+    bool mbStopped;
+    bool mbStopRequested;
+    std::mutex mMutexStop;
+
+};
+
+}
+
+
+#endif // VIEWER_H
+	
+