diff --git a/.gitignore b/.gitignore
index 0172760..ca5eb19d9 100644
--- a/.gitignore
+++ b/.gitignore
@@ -17,8 +17,23 @@ Examples/Monocular/mono_euroc
 Examples/Monocular/mono_kitti
 Examples/Monocular/mono_tum
 Examples/RGB-D/rgbd_tum
+Examples/ROS/ORB_SLAM2/CMakeLists.txt.user
+Examples/ROS/ORB_SLAM2/Mono
+Examples/ROS/ORB_SLAM2/MonoAR
+Examples/ROS/ORB_SLAM2/RGBD
+Examples/ROS/ORB_SLAM2/Stereo
+Examples/ROS/ORB_SLAM2/build/
 Examples/Stereo/stereo_euroc
 Examples/Stereo/stereo_kitti
+KeyFrameTrajectory.txt
+Thirdparty/DBoW2/build/
+Thirdparty/DBoW2/lib/
+Thirdparty/g2o/build/
+Thirdparty/g2o/config.h
+Thirdparty/g2o/lib/
+Vocabulary/ORBvoc.txt
+build/
+*~
+lib/
 
 Examples/ROS/ORB_VIO/VIO
-
diff --git a/CMakeLists.txt b/CMakeLists.txt
index a5f089a..22ac8fa 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -28,7 +28,14 @@ endif()
 
 LIST(APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake_modules)
 
-find_package(OpenCV 2.4.3 REQUIRED)
+find_package(OpenCV 3.0 QUIET)
+if(NOT OpenCV_FOUND)
+   find_package(OpenCV 2.4.3 QUIET)
+   if(NOT OpenCV_FOUND)
+      message(FATAL_ERROR "OpenCV > 2.4.3 not found.")
+   endif()
+endif()
+
 find_package(Eigen3 3.1.0 REQUIRED)
 find_package(Pangolin REQUIRED)
 find_package(Cholmod REQUIRED)
diff --git a/Examples/ROS/ORB_SLAM2/CMakeLists.txt b/Examples/ROS/ORB_SLAM2/CMakeLists.txt
index 3c032fe..7cbf71c 100644
--- a/Examples/ROS/ORB_SLAM2/CMakeLists.txt
+++ b/Examples/ROS/ORB_SLAM2/CMakeLists.txt
@@ -30,7 +30,14 @@ endif()
 
 LIST(APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/../../../cmake_modules)
 
-find_package(OpenCV 2.4.3 REQUIRED)
+find_package(OpenCV 3.0 QUIET)
+if(NOT OpenCV_FOUND)
+   find_package(OpenCV 2.4.3 QUIET)
+   if(NOT OpenCV_FOUND)
+      message(FATAL_ERROR "OpenCV > 2.4.3 not found.")
+   endif()
+endif()
+
 find_package(Eigen3 3.1.0 REQUIRED)
 find_package(Pangolin REQUIRED)
 
@@ -59,6 +66,17 @@ target_link_libraries(Mono
 ${LIBS}
 )
 
+# Node for monocular camera (Augmented Reality Demo)
+rosbuild_add_executable(MonoAR
+src/AR/ros_mono_ar.cc
+src/AR/ViewerAR.h
+src/AR/ViewerAR.cc
+)
+
+target_link_libraries(MonoAR
+${LIBS}
+)
+
 # Node for stereo camera
 rosbuild_add_executable(Stereo
 src/ros_stereo.cc
diff --git a/Examples/ROS/ORB_SLAM2/src/AR/ViewerAR.cc b/Examples/ROS/ORB_SLAM2/src/AR/ViewerAR.cc
new file mode 100644
index 0000000..9c548e8
--- /dev/null
+++ b/Examples/ROS/ORB_SLAM2/src/AR/ViewerAR.cc
@@ -0,0 +1,642 @@
+/**
+* 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/>.
+*/
+
+#include "ViewerAR.h"
+
+#include <opencv2/highgui/highgui.hpp>
+
+#include <mutex>
+#include <thread>
+#include <cstdlib>
+
+using namespace std;
+
+namespace ORB_SLAM2
+{
+
+const float eps = 1e-4;
+
+cv::Mat ExpSO3(const float &x, const float &y, const float &z)
+{
+    cv::Mat I = cv::Mat::eye(3,3,CV_32F);
+    const float d2 = x*x+y*y+z*z;
+    const float d = sqrt(d2);
+    cv::Mat W = (cv::Mat_<float>(3,3) << 0, -z, y,
+                 z, 0, -x,
+                 -y,  x, 0);
+    if(d<eps)
+        return (I + W + 0.5f*W*W);
+    else
+        return (I + W*sin(d)/d + W*W*(1.0f-cos(d))/d2);
+}
+
+cv::Mat ExpSO3(const cv::Mat &v)
+{
+    return ExpSO3(v.at<float>(0),v.at<float>(1),v.at<float>(2));
+}
+
+ViewerAR::ViewerAR(){}
+
+void ViewerAR::Run()
+{
+    int w,h,wui;
+
+    cv::Mat im, Tcw;
+    int status;
+    vector<cv::KeyPoint> vKeys;
+    vector<MapPoint*> vMPs;
+
+    while(1)
+    {
+        GetImagePose(im,Tcw,status,vKeys,vMPs);
+        if(im.empty())
+            cv::waitKey(mT);
+        else
+        {
+            w = im.cols;
+            h = im.rows;
+            break;
+        }
+    }
+
+    wui=200;
+
+    pangolin::CreateWindowAndBind("Viewer",w+wui,h);
+
+    glEnable(GL_DEPTH_TEST);
+    glEnable (GL_BLEND);
+
+    pangolin::CreatePanel("menu").SetBounds(0.0,1.0,0.0,pangolin::Attach::Pix(wui));
+    pangolin::Var<bool> menu_detectplane("menu.Insert Cube",false,false);
+    pangolin::Var<bool> menu_clear("menu.Clear All",false,false);
+    pangolin::Var<bool> menu_drawim("menu.Draw Image",true,true);
+    pangolin::Var<bool> menu_drawcube("menu.Draw Cube",true,true);
+    pangolin::Var<float> menu_cubesize("menu. Cube Size",0.05,0.01,0.3);
+    pangolin::Var<bool> menu_drawgrid("menu.Draw Grid",true,true);
+    pangolin::Var<int> menu_ngrid("menu. Grid Elements",3,1,10);
+    pangolin::Var<float> menu_sizegrid("menu. Element Size",0.05,0.01,0.3);
+    pangolin::Var<bool> menu_drawpoints("menu.Draw Points",false,true);
+
+    pangolin::Var<bool> menu_LocalizationMode("menu.Localization Mode",false,true);
+    bool bLocalizationMode = false;
+
+    pangolin::View& d_image = pangolin::Display("image")
+            .SetBounds(0,1.0f,pangolin::Attach::Pix(wui),1.0f,(float)w/h)
+            .SetLock(pangolin::LockLeft, pangolin::LockTop);
+
+    pangolin::GlTexture imageTexture(w,h,GL_RGB,false,0,GL_RGB,GL_UNSIGNED_BYTE);
+
+    pangolin::OpenGlMatrixSpec P = pangolin::ProjectionMatrixRDF_TopLeft(w,h,fx,fy,cx,cy,0.001,1000);
+
+    vector<Plane*> vpPlane;
+
+    while(1)
+    {
+
+        if(menu_LocalizationMode && !bLocalizationMode)
+        {
+            mpSystem->ActivateLocalizationMode();
+            bLocalizationMode = true;
+        }
+        else if(!menu_LocalizationMode && bLocalizationMode)
+        {
+            mpSystem->DeactivateLocalizationMode();
+            bLocalizationMode = false;
+        }
+
+        glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
+
+        // Activate camera view
+        d_image.Activate();
+        glColor3f(1.0,1.0,1.0);
+
+        // Get last image and its computed pose from SLAM
+        GetImagePose(im,Tcw,status,vKeys,vMPs);
+
+        // Add text to image
+        PrintStatus(status,bLocalizationMode,im);
+
+        if(menu_drawpoints)
+            DrawTrackedPoints(vKeys,vMPs,im);
+
+        // Draw image
+        if(menu_drawim)
+            DrawImageTexture(imageTexture,im);
+
+        glClear(GL_DEPTH_BUFFER_BIT);
+
+        // Load camera projection
+        glMatrixMode(GL_PROJECTION);
+        P.Load();
+
+        glMatrixMode(GL_MODELVIEW);
+
+        // Load camera pose
+        LoadCameraPose(Tcw);
+
+        // Draw virtual things
+        if(status==2)
+        {
+            if(menu_clear)
+            {
+                if(!vpPlane.empty())
+                {
+                    for(size_t i=0; i<vpPlane.size(); i++)
+                    {
+                        delete vpPlane[i];
+                    }
+                    vpPlane.clear();
+                    cout << "All cubes erased!" << endl;
+                }
+                menu_clear = false;
+            }
+            if(menu_detectplane)
+            {
+                Plane* pPlane = DetectPlane(Tcw,vMPs,50);
+                if(pPlane)
+                {
+                    cout << "New virtual cube inserted!" << endl;
+                    vpPlane.push_back(pPlane);
+                }
+                else
+                {
+                    cout << "No plane detected. Point the camera to a planar region." << endl;
+                }
+                menu_detectplane = false;
+            }
+
+            if(!vpPlane.empty())
+            {
+                // Recompute plane if there has been a loop closure or global BA
+                // In localization mode, map is not updated so we do not need to recompute
+                bool bRecompute = false;
+                if(!bLocalizationMode)
+                {
+                    if(mpSystem->MapChanged())
+                    {
+                        cout << "Map changed. All virtual elements are recomputed!" << endl;
+                        bRecompute = true;
+                    }
+                }
+
+                for(size_t i=0; i<vpPlane.size(); i++)
+                {
+                    Plane* pPlane = vpPlane[i];
+
+                    if(pPlane)
+                    {
+                        if(bRecompute)
+                        {
+                            pPlane->Recompute();
+                        }
+                        glPushMatrix();
+                        pPlane->glTpw.Multiply();
+
+                        // Draw cube
+                        if(menu_drawcube)
+                        {
+                            DrawCube(menu_cubesize);
+                        }
+
+                        // Draw grid plane
+                        if(menu_drawgrid)
+                        {
+                            DrawPlane(menu_ngrid,menu_sizegrid);
+                        }
+
+                        glPopMatrix();
+                    }
+                }
+            }
+
+
+        }
+
+        pangolin::FinishFrame();
+        usleep(mT*1000);
+    }
+
+}
+
+void ViewerAR::SetImagePose(const cv::Mat &im, const cv::Mat &Tcw, const int &status, const vector<cv::KeyPoint> &vKeys, const vector<ORB_SLAM2::MapPoint*> &vMPs)
+{
+    unique_lock<mutex> lock(mMutexPoseImage);
+    mImage = im.clone();
+    mTcw = Tcw.clone();
+    mStatus = status;
+    mvKeys = vKeys;
+    mvMPs = vMPs;
+}
+
+void ViewerAR::GetImagePose(cv::Mat &im, cv::Mat &Tcw, int &status, std::vector<cv::KeyPoint> &vKeys,  std::vector<MapPoint*> &vMPs)
+{
+    unique_lock<mutex> lock(mMutexPoseImage);
+    im = mImage.clone();
+    Tcw = mTcw.clone();
+    status = mStatus;
+    vKeys = mvKeys;
+    vMPs = mvMPs;
+}
+
+void ViewerAR::LoadCameraPose(const cv::Mat &Tcw)
+{
+    if(!Tcw.empty())
+    {
+        pangolin::OpenGlMatrix M;
+
+        M.m[0] = Tcw.at<float>(0,0);
+        M.m[1] = Tcw.at<float>(1,0);
+        M.m[2] = Tcw.at<float>(2,0);
+        M.m[3]  = 0.0;
+
+        M.m[4] = Tcw.at<float>(0,1);
+        M.m[5] = Tcw.at<float>(1,1);
+        M.m[6] = Tcw.at<float>(2,1);
+        M.m[7]  = 0.0;
+
+        M.m[8] = Tcw.at<float>(0,2);
+        M.m[9] = Tcw.at<float>(1,2);
+        M.m[10] = Tcw.at<float>(2,2);
+        M.m[11]  = 0.0;
+
+        M.m[12] = Tcw.at<float>(0,3);
+        M.m[13] = Tcw.at<float>(1,3);
+        M.m[14] = Tcw.at<float>(2,3);
+        M.m[15]  = 1.0;
+
+        M.Load();
+    }
+}
+
+void ViewerAR::PrintStatus(const int &status, const bool &bLocMode, cv::Mat &im)
+{
+    if(!bLocMode)
+    {
+        switch(status)
+        {
+        case 1:  {AddTextToImage("SLAM NOT INITIALIZED",im,255,0,0); break;}
+        case 2:  {AddTextToImage("SLAM ON",im,0,255,0); break;}
+        case 3:  {AddTextToImage("SLAM LOST",im,255,0,0); break;}
+        }
+    }
+    else
+    {
+        switch(status)
+        {
+        case 1:  {AddTextToImage("SLAM NOT INITIALIZED",im,255,0,0); break;}
+        case 2:  {AddTextToImage("LOCALIZATION ON",im,0,255,0); break;}
+        case 3:  {AddTextToImage("LOCALIZATION LOST",im,255,0,0); break;}
+        }
+    }
+}
+
+void ViewerAR::AddTextToImage(const string &s, cv::Mat &im, const int r, const int g, const int b)
+{
+    int l = 10;
+    //imText.rowRange(im.rows-imText.rows,imText.rows) = cv::Mat::zeros(textSize.height+10,im.cols,im.type());
+    cv::putText(im,s,cv::Point(l,im.rows-l),cv::FONT_HERSHEY_PLAIN,1.5,cv::Scalar(255,255,255),2,8);
+    cv::putText(im,s,cv::Point(l-1,im.rows-l),cv::FONT_HERSHEY_PLAIN,1.5,cv::Scalar(255,255,255),2,8);
+    cv::putText(im,s,cv::Point(l+1,im.rows-l),cv::FONT_HERSHEY_PLAIN,1.5,cv::Scalar(255,255,255),2,8);
+    cv::putText(im,s,cv::Point(l-1,im.rows-(l-1)),cv::FONT_HERSHEY_PLAIN,1.5,cv::Scalar(255,255,255),2,8);
+    cv::putText(im,s,cv::Point(l,im.rows-(l-1)),cv::FONT_HERSHEY_PLAIN,1.5,cv::Scalar(255,255,255),2,8);
+    cv::putText(im,s,cv::Point(l+1,im.rows-(l-1)),cv::FONT_HERSHEY_PLAIN,1.5,cv::Scalar(255,255,255),2,8);
+    cv::putText(im,s,cv::Point(l-1,im.rows-(l+1)),cv::FONT_HERSHEY_PLAIN,1.5,cv::Scalar(255,255,255),2,8);
+    cv::putText(im,s,cv::Point(l,im.rows-(l+1)),cv::FONT_HERSHEY_PLAIN,1.5,cv::Scalar(255,255,255),2,8);
+    cv::putText(im,s,cv::Point(l+1,im.rows-(l+1)),cv::FONT_HERSHEY_PLAIN,1.5,cv::Scalar(255,255,255),2,8);
+
+    cv::putText(im,s,cv::Point(l,im.rows-l),cv::FONT_HERSHEY_PLAIN,1.5,cv::Scalar(r,g,b),2,8);
+}
+
+void ViewerAR::DrawImageTexture(pangolin::GlTexture &imageTexture, cv::Mat &im)
+{
+    if(!im.empty())
+    {
+        imageTexture.Upload(im.data,GL_RGB,GL_UNSIGNED_BYTE);
+        imageTexture.RenderToViewportFlipY();
+    }
+}
+
+void ViewerAR::DrawCube(const float &size,const float x, const float y, const float z)
+{
+    pangolin::OpenGlMatrix M = pangolin::OpenGlMatrix::Translate(-x,-size-y,-z);
+    glPushMatrix();
+    M.Multiply();
+    pangolin::glDrawColouredCube(-size,size);
+    glPopMatrix();
+}
+
+void ViewerAR::DrawPlane(Plane *pPlane, int ndivs, float ndivsize)
+{
+    glPushMatrix();
+    pPlane->glTpw.Multiply();
+    DrawPlane(ndivs,ndivsize);
+    glPopMatrix();
+}
+
+void ViewerAR::DrawPlane(int ndivs, float ndivsize)
+{
+    // Plane parallel to x-z at origin with normal -y
+    const float minx = -ndivs*ndivsize;
+    const float minz = -ndivs*ndivsize;
+    const float maxx = ndivs*ndivsize;
+    const float maxz = ndivs*ndivsize;
+
+
+    glLineWidth(2);
+    glColor3f(0.7f,0.7f,1.0f);
+    glBegin(GL_LINES);
+
+    for(int n = 0; n<=2*ndivs; n++)
+    {
+        glVertex3f(minx+ndivsize*n,0,minz);
+        glVertex3f(minx+ndivsize*n,0,maxz);
+        glVertex3f(minx,0,minz+ndivsize*n);
+        glVertex3f(maxx,0,minz+ndivsize*n);
+    }
+
+    glEnd();
+
+}
+
+void ViewerAR::DrawTrackedPoints(const std::vector<cv::KeyPoint> &vKeys, const std::vector<MapPoint *> &vMPs, cv::Mat &im)
+{
+    const int N = vKeys.size();
+
+
+    for(int i=0; i<N; i++)
+    {
+        if(vMPs[i])
+        {
+            cv::circle(im,vKeys[i].pt,1,cv::Scalar(0,255,0),-1);
+        }
+    }
+}
+
+Plane* ViewerAR::DetectPlane(const cv::Mat Tcw, const std::vector<MapPoint*> &vMPs, const int iterations)
+{
+    // Retrieve 3D points
+    vector<cv::Mat> vPoints;
+    vPoints.reserve(vMPs.size());
+    vector<MapPoint*> vPointMP;
+    vPointMP.reserve(vMPs.size());
+
+    for(size_t i=0; i<vMPs.size(); i++)
+    {
+        MapPoint* pMP=vMPs[i];
+        if(pMP)
+        {
+            if(pMP->Observations()>5)
+            {
+                vPoints.push_back(pMP->GetWorldPos());
+                vPointMP.push_back(pMP);
+            }
+        }
+    }
+
+    const int N = vPoints.size();
+
+    if(N<50)
+        return NULL;
+
+
+    // Indices for minimum set selection
+    vector<size_t> vAllIndices;
+    vAllIndices.reserve(N);
+    vector<size_t> vAvailableIndices;
+
+    for(int i=0; i<N; i++)
+    {
+        vAllIndices.push_back(i);
+    }
+
+    float bestDist = 1e10;
+    vector<float> bestvDist;
+
+    //RANSAC
+    for(int n=0; n<iterations; n++)
+    {
+        vAvailableIndices = vAllIndices;
+
+        cv::Mat A(3,4,CV_32F);
+        A.col(3) = cv::Mat::ones(3,1,CV_32F);
+
+        // Get min set of points
+        for(short i = 0; i < 3; ++i)
+        {
+            int randi = DUtils::Random::RandomInt(0, vAvailableIndices.size()-1);
+
+            int idx = vAvailableIndices[randi];
+
+            A.row(i).colRange(0,3) = vPoints[idx].t();
+
+            vAvailableIndices[randi] = vAvailableIndices.back();
+            vAvailableIndices.pop_back();
+        }
+
+        cv::Mat u,w,vt;
+        cv::SVDecomp(A,w,u,vt,cv::SVD::MODIFY_A | cv::SVD::FULL_UV);
+
+        const float a = vt.at<float>(3,0);
+        const float b = vt.at<float>(3,1);
+        const float c = vt.at<float>(3,2);
+        const float d = vt.at<float>(3,3);
+
+        vector<float> vDistances(N,0);
+
+        const float f = 1.0f/sqrt(a*a+b*b+c*c+d*d);
+
+        for(int i=0; i<N; i++)
+        {
+            vDistances[i] = fabs(vPoints[i].at<float>(0)*a+vPoints[i].at<float>(1)*b+vPoints[i].at<float>(2)*c+d)*f;
+        }
+
+        vector<float> vSorted = vDistances;
+        sort(vSorted.begin(),vSorted.end());
+
+        int nth = max((int)(0.2*N),20);
+        const float medianDist = vSorted[nth];
+
+        if(medianDist<bestDist)
+        {
+            bestDist = medianDist;
+            bestvDist = vDistances;
+        }
+    }
+
+    // Compute threshold inlier/outlier
+    const float th = 1.4*bestDist;
+    vector<bool> vbInliers(N,false);
+    int nInliers = 0;
+    for(int i=0; i<N; i++)
+    {
+        if(bestvDist[i]<th)
+        {
+            nInliers++;
+            vbInliers[i]=true;
+        }
+    }
+
+    vector<MapPoint*> vInlierMPs(nInliers,NULL);
+    int nin = 0;
+    for(int i=0; i<N; i++)
+    {
+        if(vbInliers[i])
+        {
+            vInlierMPs[nin] = vPointMP[i];
+            nin++;
+        }
+    }
+
+    return new Plane(vInlierMPs,Tcw);
+}
+
+Plane::Plane(const std::vector<MapPoint *> &vMPs, const cv::Mat &Tcw):mvMPs(vMPs),mTcw(Tcw.clone())
+{
+    rang = -3.14f/2+((float)rand()/RAND_MAX)*3.14f;
+    Recompute();
+}
+
+void Plane::Recompute()
+{
+    const int N = mvMPs.size();
+
+    // Recompute plane with all points
+    cv::Mat A = cv::Mat(N,4,CV_32F);
+    A.col(3) = cv::Mat::ones(N,1,CV_32F);
+
+    o = cv::Mat::zeros(3,1,CV_32F);
+
+    int nPoints = 0;
+    for(int i=0; i<N; i++)
+    {
+        MapPoint* pMP = mvMPs[i];
+        if(!pMP->isBad())
+        {
+            cv::Mat Xw = pMP->GetWorldPos();
+            o+=Xw;
+            A.row(nPoints).colRange(0,3) = Xw.t();
+            nPoints++;
+        }
+    }
+    A.resize(nPoints);
+
+    cv::Mat u,w,vt;
+    cv::SVDecomp(A,w,u,vt,cv::SVD::MODIFY_A | cv::SVD::FULL_UV);
+
+    float a = vt.at<float>(3,0);
+    float b = vt.at<float>(3,1);
+    float c = vt.at<float>(3,2);
+
+    o = o*(1.0f/nPoints);
+    const float f = 1.0f/sqrt(a*a+b*b+c*c);
+
+    // Compute XC just the first time
+    if(XC.empty())
+    {
+        cv::Mat Oc = -mTcw.colRange(0,3).rowRange(0,3).t()*mTcw.rowRange(0,3).col(3);
+        XC = Oc-o;
+    }
+
+    if((XC.at<float>(0)*a+XC.at<float>(1)*b+XC.at<float>(2)*c)>0)
+    {
+        a=-a;
+        b=-b;
+        c=-c;
+    }
+
+    const float nx = a*f;
+    const float ny = b*f;
+    const float nz = c*f;
+
+    n = (cv::Mat_<float>(3,1)<<nx,ny,nz);
+
+    cv::Mat up = (cv::Mat_<float>(3,1) << 0.0f, 1.0f, 0.0f);
+
+    cv::Mat v = up.cross(n);
+    const float sa = cv::norm(v);
+    const float ca = up.dot(n);
+    const float ang = atan2(sa,ca);
+    Tpw = cv::Mat::eye(4,4,CV_32F);
+
+
+    Tpw.rowRange(0,3).colRange(0,3) = ExpSO3(v*ang/sa)*ExpSO3(up*rang);
+    o.copyTo(Tpw.col(3).rowRange(0,3));
+
+    glTpw.m[0] = Tpw.at<float>(0,0);
+    glTpw.m[1] = Tpw.at<float>(1,0);
+    glTpw.m[2] = Tpw.at<float>(2,0);
+    glTpw.m[3]  = 0.0;
+
+    glTpw.m[4] = Tpw.at<float>(0,1);
+    glTpw.m[5] = Tpw.at<float>(1,1);
+    glTpw.m[6] = Tpw.at<float>(2,1);
+    glTpw.m[7]  = 0.0;
+
+    glTpw.m[8] = Tpw.at<float>(0,2);
+    glTpw.m[9] = Tpw.at<float>(1,2);
+    glTpw.m[10] = Tpw.at<float>(2,2);
+    glTpw.m[11]  = 0.0;
+
+    glTpw.m[12] = Tpw.at<float>(0,3);
+    glTpw.m[13] = Tpw.at<float>(1,3);
+    glTpw.m[14] = Tpw.at<float>(2,3);
+    glTpw.m[15]  = 1.0;
+
+}
+
+Plane::Plane(const float &nx, const float &ny, const float &nz, const float &ox, const float &oy, const float &oz)
+{
+    n = (cv::Mat_<float>(3,1)<<nx,ny,nz);
+    o = (cv::Mat_<float>(3,1)<<ox,oy,oz);
+
+    cv::Mat up = (cv::Mat_<float>(3,1) << 0.0f, 1.0f, 0.0f);
+
+    cv::Mat v = up.cross(n);
+    const float s = cv::norm(v);
+    const float c = up.dot(n);
+    const float a = atan2(s,c);
+    Tpw = cv::Mat::eye(4,4,CV_32F);
+    const float rang = -3.14f/2+((float)rand()/RAND_MAX)*3.14f;
+    cout << rang;
+    Tpw.rowRange(0,3).colRange(0,3) = ExpSO3(v*a/s)*ExpSO3(up*rang);
+    o.copyTo(Tpw.col(3).rowRange(0,3));
+
+    glTpw.m[0] = Tpw.at<float>(0,0);
+    glTpw.m[1] = Tpw.at<float>(1,0);
+    glTpw.m[2] = Tpw.at<float>(2,0);
+    glTpw.m[3]  = 0.0;
+
+    glTpw.m[4] = Tpw.at<float>(0,1);
+    glTpw.m[5] = Tpw.at<float>(1,1);
+    glTpw.m[6] = Tpw.at<float>(2,1);
+    glTpw.m[7]  = 0.0;
+
+    glTpw.m[8] = Tpw.at<float>(0,2);
+    glTpw.m[9] = Tpw.at<float>(1,2);
+    glTpw.m[10] = Tpw.at<float>(2,2);
+    glTpw.m[11]  = 0.0;
+
+    glTpw.m[12] = Tpw.at<float>(0,3);
+    glTpw.m[13] = Tpw.at<float>(1,3);
+    glTpw.m[14] = Tpw.at<float>(2,3);
+    glTpw.m[15]  = 1.0;
+}
+
+}
diff --git a/Examples/ROS/ORB_SLAM2/src/AR/ViewerAR.h b/Examples/ROS/ORB_SLAM2/src/AR/ViewerAR.h
new file mode 100644
index 0000000..cd173ca
--- /dev/null
+++ b/Examples/ROS/ORB_SLAM2/src/AR/ViewerAR.h
@@ -0,0 +1,120 @@
+/**
+* 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 VIEWERAR_H
+#define VIEWERAR_H
+
+#include <mutex>
+#include <opencv2/core/core.hpp>
+#include <pangolin/pangolin.h>
+#include <string>
+#include"../../../include/System.h"
+
+namespace ORB_SLAM2
+{
+
+class Plane
+{
+public:
+    Plane(const std::vector<MapPoint*> &vMPs, const cv::Mat &Tcw);
+    Plane(const float &nx, const float &ny, const float &nz, const float &ox, const float &oy, const float &oz);
+
+    void Recompute();
+
+    //normal
+    cv::Mat n;
+    //origin
+    cv::Mat o;
+    //arbitrary orientation along normal
+    float rang;
+    //transformation from world to the plane
+    cv::Mat Tpw;
+    pangolin::OpenGlMatrix glTpw;
+    //MapPoints that define the plane
+    std::vector<MapPoint*> mvMPs;
+    //camera pose when the plane was first observed (to compute normal direction)
+    cv::Mat mTcw, XC;
+};
+
+class ViewerAR
+{
+public:
+    ViewerAR();
+
+    void SetFPS(const float fps){
+        mFPS = fps;
+        mT=1e3/fps;
+    }
+
+    void SetSLAM(ORB_SLAM2::System* pSystem){
+        mpSystem = pSystem;
+    }
+
+    // Main thread function. 
+    void Run();
+
+    void SetCameraCalibration(const float &fx_, const float &fy_, const float &cx_, const float &cy_){
+        fx = fx_; fy = fy_; cx = cx_; cy = cy_;
+    }
+
+    void SetImagePose(const cv::Mat &im, const cv::Mat &Tcw, const int &status,
+                      const std::vector<cv::KeyPoint> &vKeys, const std::vector<MapPoint*> &vMPs);
+
+    void GetImagePose(cv::Mat &im, cv::Mat &Tcw, int &status,
+                      std::vector<cv::KeyPoint> &vKeys,  std::vector<MapPoint*> &vMPs);
+
+private:
+
+    //SLAM
+    ORB_SLAM2::System* mpSystem;
+
+    void PrintStatus(const int &status, const bool &bLocMode, cv::Mat &im);
+    void AddTextToImage(const std::string &s, cv::Mat &im, const int r=0, const int g=0, const int b=0);
+    void LoadCameraPose(const cv::Mat &Tcw);
+    void DrawImageTexture(pangolin::GlTexture &imageTexture, cv::Mat &im);
+    void DrawCube(const float &size, const float x=0, const float y=0, const float z=0);
+    void DrawPlane(int ndivs, float ndivsize);
+    void DrawPlane(Plane* pPlane, int ndivs, float ndivsize);
+    void DrawTrackedPoints(const std::vector<cv::KeyPoint> &vKeys, const std::vector<MapPoint*> &vMPs, cv::Mat &im);
+
+    Plane* DetectPlane(const cv::Mat Tcw, const std::vector<MapPoint*> &vMPs, const int iterations=50);
+
+    // frame rate
+    float mFPS, mT;
+    float fx,fy,cx,cy;
+
+    // Last processed image and computed pose by the SLAM
+    std::mutex mMutexPoseImage;
+    cv::Mat mTcw;
+    cv::Mat mImage;
+    int mStatus;
+    std::vector<cv::KeyPoint> mvKeys;
+    std::vector<MapPoint*> mvMPs;
+
+};
+
+
+}
+
+
+#endif // VIEWERAR_H
+	
+
diff --git a/Examples/ROS/ORB_SLAM2/src/AR/ros_mono_ar.cc b/Examples/ROS/ORB_SLAM2/src/AR/ros_mono_ar.cc
new file mode 100644
index 0000000..58d204a
--- /dev/null
+++ b/Examples/ROS/ORB_SLAM2/src/AR/ros_mono_ar.cc
@@ -0,0 +1,169 @@
+/**
+* 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/>.
+*/
+
+
+#include<iostream>
+#include<algorithm>
+#include<fstream>
+#include<chrono>
+
+#include<ros/ros.h>
+#include <cv_bridge/cv_bridge.h>
+
+#include<opencv2/core/core.hpp>
+#include<opencv2/imgproc/imgproc.hpp>
+
+#include"../../../include/System.h"
+
+#include"ViewerAR.h"
+
+using namespace std;
+
+
+ORB_SLAM2::ViewerAR viewerAR;
+bool bRGB = true;
+
+cv::Mat K;
+cv::Mat DistCoef;
+
+
+class ImageGrabber
+{
+public:
+    ImageGrabber(ORB_SLAM2::System* pSLAM):mpSLAM(pSLAM){}
+
+    void GrabImage(const sensor_msgs::ImageConstPtr& msg);
+
+    ORB_SLAM2::System* mpSLAM;
+};
+
+int main(int argc, char **argv)
+{
+    ros::init(argc, argv, "Mono");
+    ros::start();
+
+    if(argc != 3)
+    {
+        cerr << endl << "Usage: rosrun ORB_SLAM2 Mono path_to_vocabulary path_to_settings" << endl;        
+        ros::shutdown();
+        return 1;
+    }
+
+    // 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,false);
+
+
+    cout << endl << endl;
+    cout << "-----------------------" << endl;
+    cout << "Augmented Reality Demo" << endl;
+    cout << "1) Translate the camera to initialize SLAM." << endl;
+    cout << "2) Look at a planar region and translate the camera." << endl;
+    cout << "3) Press Insert Cube to place a virtual cube in the plane. " << endl;
+    cout << endl;
+    cout << "You can place several cubes in different planes." << endl;
+    cout << "-----------------------" << endl;
+    cout << endl;
+
+
+    viewerAR.SetSLAM(&SLAM);
+
+    ImageGrabber igb(&SLAM);
+
+    ros::NodeHandle nodeHandler;
+    ros::Subscriber sub = nodeHandler.subscribe("/camera/image_raw", 1, &ImageGrabber::GrabImage,&igb);
+
+
+    cv::FileStorage fSettings(argv[2], cv::FileStorage::READ);
+    bRGB = static_cast<bool>((int)fSettings["Camera.RGB"]);
+    float fps = fSettings["Camera.fps"];
+    viewerAR.SetFPS(fps);
+
+    float fx = fSettings["Camera.fx"];
+    float fy = fSettings["Camera.fy"];
+    float cx = fSettings["Camera.cx"];
+    float cy = fSettings["Camera.cy"];
+
+    viewerAR.SetCameraCalibration(fx,fy,cx,cy);
+
+    K = cv::Mat::eye(3,3,CV_32F);
+    K.at<float>(0,0) = fx;
+    K.at<float>(1,1) = fy;
+    K.at<float>(0,2) = cx;
+    K.at<float>(1,2) = cy;
+
+    DistCoef = cv::Mat::zeros(4,1,CV_32F);
+    DistCoef.at<float>(0) = fSettings["Camera.k1"];
+    DistCoef.at<float>(1) = fSettings["Camera.k2"];
+    DistCoef.at<float>(2) = fSettings["Camera.p1"];
+    DistCoef.at<float>(3) = fSettings["Camera.p2"];
+    const float k3 = fSettings["Camera.k3"];
+    if(k3!=0)
+    {
+        DistCoef.resize(5);
+        DistCoef.at<float>(4) = k3;
+    }
+
+    thread tViewer = thread(&ORB_SLAM2::ViewerAR::Run,&viewerAR);
+
+    ros::spin();
+
+    // Stop all threads
+    SLAM.Shutdown();
+
+    // Save camera trajectory
+    SLAM.SaveKeyFrameTrajectoryTUM("KeyFrameTrajectory.txt");
+
+    ros::shutdown();
+
+    return 0;
+}
+
+void ImageGrabber::GrabImage(const sensor_msgs::ImageConstPtr& msg)
+{
+    // Copy the ros image message to cv::Mat.
+    cv_bridge::CvImageConstPtr cv_ptr;
+    try
+    {
+        cv_ptr = cv_bridge::toCvShare(msg);
+    }
+    catch (cv_bridge::Exception& e)
+    {
+        ROS_ERROR("cv_bridge exception: %s", e.what());
+        return;
+    }
+    cv::Mat im = cv_ptr->image.clone();
+    cv::Mat imu;
+    cv::Mat Tcw = mpSLAM->TrackMonocular(cv_ptr->image,cv_ptr->header.stamp.toSec());
+    int state = mpSLAM->GetTrackingState();
+    vector<ORB_SLAM2::MapPoint*> vMPs = mpSLAM->GetTrackedMapPoints();
+    vector<cv::KeyPoint> vKeys = mpSLAM->GetTrackedKeyPointsUn();
+
+    cv::undistort(im,imu,K,DistCoef);
+
+    if(bRGB)
+        viewerAR.SetImagePose(imu,Tcw,state,vKeys,vMPs);
+    else
+    {
+        cv::cvtColor(imu,imu,CV_RGB2BGR);
+        viewerAR.SetImagePose(imu,Tcw,state,vKeys,vMPs);
+    }    
+}
+
+
diff --git a/README.md b/README.md
index 80c28b2..53403b4 100644
--- a/README.md
+++ b/README.md
@@ -18,23 +18,29 @@ Below is the primary README of ORBSLAM2.
 # ORB-SLAM2
 **Authors:** [Raul Mur-Artal](http://webdiis.unizar.es/~raulmur/), [Juan D. Tardos](http://webdiis.unizar.es/~jdtardos/), [J. M. M. Montiel](http://webdiis.unizar.es/~josemari/) and [Dorian Galvez-Lopez](http://doriangalvez.com/) ([DBoW2](https://github.com/dorian3d/DBoW2))
 
-**Current version:** 1.0.0 
+**13 Jan 2017**: OpenCV 3 and Eigen 3.3 are now supported.
+
+**22 Dec 2016**: Added AR demo (see section 7).
 
 ORB-SLAM2 is a real-time SLAM library for **Monocular**, **Stereo** and **RGB-D** cameras that computes the camera trajectory and a sparse 3D reconstruction (in the stereo and RGB-D case with true scale). It is able to detect loops and relocalize the camera in real time. We provide examples to run the SLAM system in the [KITTI dataset](http://www.cvlibs.net/datasets/kitti/eval_odometry.php) as stereo or monocular, in the [TUM dataset](http://vision.in.tum.de/data/datasets/rgbd-dataset) as RGB-D or monocular, and in the [EuRoC dataset](http://projects.asl.ethz.ch/datasets/doku.php?id=kmavvisualinertialdatasets) as stereo or monocular. We also provide a ROS node to process live monocular, stereo or RGB-D streams. **The library can be compiled without ROS**. ORB-SLAM2 provides a GUI to change between a *SLAM Mode* and *Localization Mode*, see section 9 of this document.
 
 <a href="https://www.youtube.com/embed/ufvPS5wJAx0" target="_blank"><img src="http://img.youtube.com/vi/ufvPS5wJAx0/0.jpg" 
 alt="ORB-SLAM2" width="240" height="180" border="10" /></a>
+<a href="https://www.youtube.com/embed/T-9PYCKhDLM" target="_blank"><img src="http://img.youtube.com/vi/T-9PYCKhDLM/0.jpg" 
+alt="ORB-SLAM2" width="240" height="180" border="10" /></a>
+<a href="https://www.youtube.com/embed/kPwy8yA4CKM" target="_blank"><img src="http://img.youtube.com/vi/kPwy8yA4CKM/0.jpg" 
+alt="ORB-SLAM2" width="240" height="180" border="10" /></a>
 
 
-###Related Publications:
+### Related Publications:
 
 [Monocular] Raúl Mur-Artal, J. M. M. Montiel and Juan D. Tardós. **ORB-SLAM: A Versatile and Accurate Monocular SLAM System**. *IEEE Transactions on Robotics,* vol. 31, no. 5, pp. 1147-1163, 2015. (**2015 IEEE Transactions on Robotics Best Paper Award**). **[PDF](http://webdiis.unizar.es/~raulmur/MurMontielTardosTRO15.pdf)**.
 
-[Stereo and RGB-D] Raúl Mur-Artal and Juan D. Tardós. **ORB-SLAM2: an Open-Source SLAM System for Monocular, Stereo and RGB-D Cameras**. *ArXiv preprint arXiv:1610.06475* **[PDF](https://128.84.21.199/pdf/1610.06475.pdf)**.
+[Stereo and RGB-D] Raúl Mur-Artal and Juan D. Tardós. **ORB-SLAM2: an Open-Source SLAM System for Monocular, Stereo and RGB-D Cameras**. *IEEE Transactions on Robotics,* vol. 33, no. 5, pp. 1255-1262, 2017. **[PDF](https://128.84.21.199/pdf/1610.06475.pdf)**.
 
 [DBoW2 Place Recognizer] Dorian Gálvez-López and Juan D. Tardós. **Bags of Binary Words for Fast Place Recognition in Image Sequences**. *IEEE Transactions on Robotics,* vol. 28, no. 5, pp.  1188-1197, 2012. **[PDF](http://doriangalvez.com/php/dl.php?dlp=GalvezTRO12.pdf)**
 
-#1. License
+# 1. License
 
 ORB-SLAM2 is released under a [GPLv3 license](https://github.com/raulmur/ORB_SLAM2/blob/master/License-gpl.txt). For a list of all code/library dependencies (and associated licenses), please see [Dependencies.md](https://github.com/raulmur/ORB_SLAM2/blob/master/Dependencies.md).
 
@@ -58,12 +64,16 @@ if you use ORB-SLAM2 (Stereo or RGB-D) in an academic work, please cite:
     @article{murORB2,
       title={{ORB-SLAM2}: an Open-Source {SLAM} System for Monocular, Stereo and {RGB-D} Cameras},
       author={Mur-Artal, Ra\'ul and Tard\'os, Juan D.},
-      journal={arXiv preprint arXiv:1610.06475},
-      year={2016}
+      journal={IEEE Transactions on Robotics},
+      volume={33},
+      number={5},
+      pages={1255--1262},
+      doi = {10.1109/TRO.2017.2705103},
+      year={2017}
      }
 
-#2. Prerequisites
-We have tested the library in **Ubuntu 12.04** and **14.04**, but it should be easy to compile in other platforms. A powerful computer (e.g. i7) will ensure real-time performance and provide more stable and accurate results.
+# 2. Prerequisites
+We have tested the library in **Ubuntu 12.04**, **14.04** and **16.04**, but it should be easy to compile in other platforms. A powerful computer (e.g. i7) will ensure real-time performance and provide more stable and accurate results.
 
 ## C++11 or C++0x Compiler
 We use the new thread and chrono functionalities of C++11.
@@ -72,7 +82,7 @@ We use the new thread and chrono functionalities of C++11.
 We use [Pangolin](https://github.com/stevenlovegrove/Pangolin) for visualization and user interface. Dowload and install instructions can be found at: https://github.com/stevenlovegrove/Pangolin.
 
 ## OpenCV
-We use [OpenCV](http://opencv.org) to manipulate images and features. Dowload and install instructions can be found at: http://opencv.org. **Required at leat 2.4.3. Tested with OpenCV 2.4.11**.
+We use [OpenCV](http://opencv.org) to manipulate images and features. Dowload and install instructions can be found at: http://opencv.org. **Required at leat 2.4.3. Tested with OpenCV 2.4.11 and OpenCV 3.2**.
 
 ## Eigen3
 Required by g2o (see below). Download and install instructions can be found at: http://eigen.tuxfamily.org. **Required at least 3.1.0**.
@@ -90,7 +100,7 @@ We use modified versions of the [DBoW2](https://github.com/dorian3d/DBoW2) libra
 ## ROS (optional)
 We provide some examples to process the live input of a monocular, stereo or RGB-D camera using [ROS](ros.org). Building these examples is optional. In case you want to use ROS, a version Hydro or newer is needed.
 
-#3. Building ORB-SLAM2 library and TUM/KITTI examples
+# 3. Building ORB-SLAM2 library and examples
 
 Clone the repository:
 ```
@@ -106,7 +116,7 @@ chmod +x build.sh
 
 This will create **libORB_SLAM2.so**  at *lib* folder and the executables **mono_tum**, **mono_kitti**, **rgbd_tum**, **stereo_kitti**, **mono_euroc** and **stereo_euroc** in *Examples* folder.
 
-#4. Monocular Examples
+# 4. Monocular Examples
 
 ## TUM Dataset
 
@@ -139,7 +149,7 @@ This will create **libORB_SLAM2.so**  at *lib* folder and the executables **mono
 ./Examples/Monocular/mono_euroc Vocabulary/ORBvoc.txt Examples/Monocular/EuRoC.yaml PATH_TO_SEQUENCE/cam0/data Examples/Monocular/EuRoC_TimeStamps/SEQUENCE.txt 
 ```
 
-#5. Stereo Examples
+# 5. Stereo Examples
 
 ## KITTI Dataset
 
@@ -162,7 +172,7 @@ This will create **libORB_SLAM2.so**  at *lib* folder and the executables **mono
 ./Examples/Stereo/stereo_euroc Vocabulary/ORBvoc.txt Examples/Stereo/EuRoC.yaml PATH_TO_SEQUENCE/cam0/data PATH_TO_SEQUENCE/cam1/data Examples/Stereo/EuRoC_TimeStamps/SEQUENCE.txt
 ```
 
-#6. RGB-D Example
+# 6. RGB-D Example
 
 ## TUM Dataset
 
@@ -180,22 +190,20 @@ This will create **libORB_SLAM2.so**  at *lib* folder and the executables **mono
   ./Examples/RGB-D/rgbd_tum Vocabulary/ORBvoc.txt Examples/RGB-D/TUMX.yaml PATH_TO_SEQUENCE_FOLDER ASSOCIATIONS_FILE
   ```
 
-#7. ROS Examples
+# 7. ROS Examples
 
-### Building the nodes for mono, stereo and RGB-D
+### Building the nodes for mono, monoAR, stereo and RGB-D
 1. Add the path including *Examples/ROS/ORB_SLAM2* to the ROS_PACKAGE_PATH environment variable. Open .bashrc file and add at the end the following line. Replace PATH by the folder where you cloned ORB_SLAM2:
 
   ```
   export ROS_PACKAGE_PATH=${ROS_PACKAGE_PATH}:PATH/ORB_SLAM2/Examples/ROS
   ```
   
-2. Go to *Examples/ROS/ORB_SLAM2* folder and execute:
+2. Execute `build_ros.sh` script:
 
   ```
-  mkdir build
-  cd build
-  cmake .. -DROS_BUILD_TYPE=Release
-  make -j
+  chmod +x build_ros.sh
+  ./build_ros.sh
   ```
   
 ### Running Monocular Node
@@ -205,6 +213,14 @@ For a monocular input from topic `/camera/image_raw` run node ORB_SLAM2/Mono. Yo
   rosrun ORB_SLAM2 Mono PATH_TO_VOCABULARY PATH_TO_SETTINGS_FILE
   ```
   
+### Running Monocular Augmented Reality Demo
+This is a demo of augmented reality where you can use an interface to insert virtual cubes in planar regions of the scene.
+The node reads images from topic `/camera/image_raw`.
+
+  ```
+  rosrun ORB_SLAM2 MonoAR PATH_TO_VOCABULARY PATH_TO_SETTINGS_FILE
+  ```
+  
 ### Running Stereo Node
 For a stereo input from topic `/camera/left/image_raw` and `/camera/right/image_raw` run node ORB_SLAM2/Stereo. You will need to provide the vocabulary file and a settings file. If you **provide rectification matrices** (see Examples/Stereo/EuRoC.yaml example), the node will recitify the images online, **otherwise images must be pre-rectified**.
 
@@ -234,10 +250,10 @@ For an RGB-D input from topics `/camera/rgb/image_raw` and `/camera/depth_regist
   rosrun ORB_SLAM2 RGBD PATH_TO_VOCABULARY PATH_TO_SETTINGS_FILE
   ```
   
-#8. Processing your own sequences
+# 8. Processing your own sequences
 You will need to create a settings file with the calibration of your camera. See the settings file provided for the TUM and KITTI datasets for monocular, stereo and RGB-D cameras. We use the calibration model of OpenCV. See the examples to learn how to create a program that makes use of the ORB-SLAM2 library and how to pass images to the SLAM system. Stereo input must be synchronized and rectified. RGB-D input must be synchronized and depth registered.
 
-#9. SLAM and Localization Modes
+# 9. SLAM and Localization Modes
 You can change between the *SLAM* and *Localization mode* using the GUI of the map viewer.
 
 ### SLAM Mode
diff --git a/Thirdparty/DBoW2/CMakeLists.txt b/Thirdparty/DBoW2/CMakeLists.txt
index 4da56b2..0eb5126 100644
--- a/Thirdparty/DBoW2/CMakeLists.txt
+++ b/Thirdparty/DBoW2/CMakeLists.txt
@@ -24,7 +24,13 @@ set(SRCS_DUTILS
   DUtils/Random.cpp
   DUtils/Timestamp.cpp)
 
-find_package(OpenCV REQUIRED)
+find_package(OpenCV 3.0 QUIET)
+if(NOT OpenCV_FOUND)
+   find_package(OpenCV 2.4.3 QUIET)
+   if(NOT OpenCV_FOUND)
+      message(FATAL_ERROR "OpenCV > 2.4.3 not found.")
+   endif()
+endif()
 
 set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
 
diff --git a/build_ros.sh b/build_ros.sh
new file mode 100755
index 0000000..86b7c14
--- /dev/null
+++ b/build_ros.sh
@@ -0,0 +1,7 @@
+echo "Building ROS nodes"
+
+cd Examples/ROS/ORB_SLAM2
+mkdir build
+cd build
+cmake .. -DROS_BUILD_TYPE=Release
+make -j
diff --git a/include/Map.h b/include/Map.h
index 31ee8f8..779c386 100644
--- a/include/Map.h
+++ b/include/Map.h
@@ -56,6 +56,8 @@ class Map
     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();
@@ -83,6 +85,9 @@ class Map
 
     long unsigned int mnMaxKFid;
 
+    // Index related to a big change in the map (loop closure, global BA)
+    int mnBigChangeIdx;
+
     std::mutex mMutexMap;
 };
 
diff --git a/include/System.h b/include/System.h
index 43085c5..5c99c2a 100644
--- a/include/System.h
+++ b/include/System.h
@@ -89,6 +89,10 @@ class System
     // 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();
 
@@ -119,6 +123,12 @@ class System
     // 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
@@ -165,6 +175,12 @@ class System
     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
diff --git a/src/FrameDrawer.cc b/src/FrameDrawer.cc
index 02aadba..6b046cf 100644
--- a/src/FrameDrawer.cc
+++ b/src/FrameDrawer.cc
@@ -94,7 +94,8 @@ cv::Mat FrameDrawer::DrawFrame()
         mnTracked=0;
         mnTrackedVO=0;
         const float r = 5;
-        for(int i=0;i<N;i++)
+        const int n = vCurrentKeys.size();
+        for(int i=0;i<n;i++)
         {
             if(vbVO[i] || vbMap[i])
             {
diff --git a/src/LoopClosing.cc b/src/LoopClosing.cc
index a02ba8d..1c73e1a 100644
--- a/src/LoopClosing.cc
+++ b/src/LoopClosing.cc
@@ -589,6 +589,7 @@ void LoopClosing::CorrectLoop()
 
     // Map updated, set flag for Tracking
     SetMapUpdateFlagInTracking(true);
+    mpMap->InformNewBigChange();
 
     // Add loop edge
     mpMatchedKF->AddLoopEdge(mpCurrentKF);
@@ -778,6 +779,8 @@ void LoopClosing::RunGlobalBundleAdjustment(unsigned long nLoopKF)
                 }
             }
 
+            mpMap->InformNewBigChange();
+
             mpLocalMapper->Release();
 
             cout << "Map updated!" << endl;
diff --git a/src/Map.cc b/src/Map.cc
index 62c32f0..52f1e14 100644
--- a/src/Map.cc
+++ b/src/Map.cc
@@ -52,7 +52,7 @@ void Map::UpdateScale(const double &scale)
 
 //---------------------------------------
 
-Map::Map():mnMaxKFid(0)
+Map::Map():mnMaxKFid(0),mnBigChangeIdx(0)
 {
 }
 
@@ -94,6 +94,18 @@ void Map::SetReferenceMapPoints(const vector<MapPoint *> &vpMPs)
     mvpReferenceMapPoints = vpMPs;
 }
 
+void Map::InformNewBigChange()
+{
+    unique_lock<mutex> lock(mMutexMap);
+    mnBigChangeIdx++;
+}
+
+int Map::GetLastBigChangeIdx()
+{
+    unique_lock<mutex> lock(mMutexMap);
+    return mnBigChangeIdx;
+}
+
 vector<KeyFrame*> Map::GetAllKeyFrames()
 {
     unique_lock<mutex> lock(mMutexMap);
diff --git a/src/System.cc b/src/System.cc
index b6ae421..b43a5dc 100644
--- a/src/System.cc
+++ b/src/System.cc
@@ -148,7 +148,7 @@ cv::Mat System::TrackMonoVI(const cv::Mat &im, const std::vector<IMUData> &vimu,
 //-------------------------------------------------------------------------------------------
 
 System::System(const string &strVocFile, const string &strSettingsFile, const eSensor sensor,
-               const bool bUseViewer):mSensor(sensor),mbReset(false),mbActivateLocalizationMode(false),
+               const bool bUseViewer):mSensor(sensor), mpViewer(static_cast<Viewer*>(NULL)), mbReset(false),mbActivateLocalizationMode(false),
         mbDeactivateLocalizationMode(false)
 {
     // Output welcome message
@@ -220,11 +220,12 @@ System::System(const string &strVocFile, const string &strSettingsFile, const eS
     mptLoopClosing = new thread(&ORB_SLAM2::LoopClosing::Run, mpLoopCloser);
 
     //Initialize the Viewer thread and launch
-    mpViewer = new Viewer(this, mpFrameDrawer,mpMapDrawer,mpTracker,strSettingsFile);
     if(bUseViewer)
+    {
+        mpViewer = new Viewer(this, mpFrameDrawer,mpMapDrawer,mpTracker,strSettingsFile);
         mptViewer = new thread(&Viewer::Run, mpViewer);
-
-    mpTracker->SetViewer(mpViewer);
+        mpTracker->SetViewer(mpViewer);
+    }
 
     //Set pointers between threads
     mpTracker->SetLocalMapper(mpLocalMapper);
@@ -279,7 +280,13 @@ cv::Mat System::TrackStereo(const cv::Mat &imLeft, const cv::Mat &imRight, const
     }
     }
 
-    return mpTracker->GrabImageStereo(imLeft,imRight,timestamp);
+    cv::Mat Tcw = mpTracker->GrabImageStereo(imLeft,imRight,timestamp);
+
+    unique_lock<mutex> lock2(mMutexState);
+    mTrackingState = mpTracker->mState;
+    mTrackedMapPoints = mpTracker->mCurrentFrame.mvpMapPoints;
+    mTrackedKeyPointsUn = mpTracker->mCurrentFrame.mvKeysUn;
+    return Tcw;
 }
 
 cv::Mat System::TrackRGBD(const cv::Mat &im, const cv::Mat &depthmap, const double &timestamp)
@@ -324,7 +331,13 @@ cv::Mat System::TrackRGBD(const cv::Mat &im, const cv::Mat &depthmap, const doub
     }
     }
 
-    return mpTracker->GrabImageRGBD(im,depthmap,timestamp);
+    cv::Mat Tcw = mpTracker->GrabImageRGBD(im,depthmap,timestamp);
+
+    unique_lock<mutex> lock2(mMutexState);
+    mTrackingState = mpTracker->mState;
+    mTrackedMapPoints = mpTracker->mCurrentFrame.mvpMapPoints;
+    mTrackedKeyPointsUn = mpTracker->mCurrentFrame.mvKeysUn;
+    return Tcw;
 }
 
 cv::Mat System::TrackMonocular(const cv::Mat &im, const double &timestamp)
@@ -369,7 +382,14 @@ cv::Mat System::TrackMonocular(const cv::Mat &im, const double &timestamp)
     }
     }
 
-    return mpTracker->GrabImageMonocular(im,timestamp);
+    cv::Mat Tcw = mpTracker->GrabImageMonocular(im,timestamp);
+
+    unique_lock<mutex> lock2(mMutexState);
+    mTrackingState = mpTracker->mState;
+    mTrackedMapPoints = mpTracker->mCurrentFrame.mvpMapPoints;
+    mTrackedKeyPointsUn = mpTracker->mCurrentFrame.mvKeysUn;
+
+    return Tcw;
 }
 
 void System::ActivateLocalizationMode()
@@ -384,6 +404,19 @@ void System::DeactivateLocalizationMode()
     mbDeactivateLocalizationMode = true;
 }
 
+bool System::MapChanged()
+{
+    static int n=0;
+    int curn = mpMap->GetLastBigChangeIdx();
+    if(n<curn)
+    {
+        n=curn;
+        return true;
+    }
+    else
+        return false;
+}
+
 void System::Reset()
 {
     unique_lock<mutex> lock(mMutexReset);
@@ -394,7 +427,6 @@ void System::Shutdown()
 {
     mpLocalMapper->RequestFinish();
     mpLoopCloser->RequestFinish();
-    mpViewer->RequestFinish();
 
     // Wait until all thread have effectively stopped
     while(!mpLocalMapper->isFinished() || !mpLoopCloser->isFinished()  ||
@@ -403,7 +435,8 @@ void System::Shutdown()
         usleep(5000);
     }
 
-    pangolin::BindToContext("ORB-SLAM2: Map Viewer");
+    if(mpViewer)
+        pangolin::BindToContext("ORB-SLAM2: Map Viewer");
 }
 
 void System::SaveTrajectoryTUM(const string &filename)
@@ -558,4 +591,22 @@ void System::SaveTrajectoryKITTI(const string &filename)
     cout << endl << "trajectory saved!" << endl;
 }
 
+int System::GetTrackingState()
+{
+    unique_lock<mutex> lock(mMutexState);
+    return mTrackingState;
+}
+
+vector<MapPoint*> System::GetTrackedMapPoints()
+{
+    unique_lock<mutex> lock(mMutexState);
+    return mTrackedMapPoints;
+}
+
+vector<cv::KeyPoint> System::GetTrackedKeyPointsUn()
+{
+    unique_lock<mutex> lock(mMutexState);
+    return mTrackedKeyPointsUn;
+}
+
 } //namespace ORB_SLAM
diff --git a/src/Tracking.cc b/src/Tracking.cc
index 6723233..3c52990 100644
--- a/src/Tracking.cc
+++ b/src/Tracking.cc
@@ -498,7 +498,7 @@ cv::Mat Tracking::GrabImageMonoVI(const cv::Mat &im, const std::vector<IMUData>
 Tracking::Tracking(System *pSys, ORBVocabulary* pVoc, FrameDrawer *pFrameDrawer, MapDrawer *pMapDrawer,
                    Map *pMap, KeyFrameDatabase* pKFDB, const string &strSettingPath, const int sensor,ConfigParam* pParams):
     mState(NO_IMAGES_YET), mSensor(sensor), mbOnlyTracking(false), mbVO(false), mpORBVocabulary(pVoc),
-    mpKeyFrameDB(pKFDB), mpInitializer(static_cast<Initializer*>(NULL)), mpSystem(pSys),
+    mpKeyFrameDB(pKFDB), mpInitializer(static_cast<Initializer*>(NULL)), mpSystem(pSys), mpViewer(NULL),
     mpFrameDrawer(pFrameDrawer), mpMapDrawer(pMapDrawer), mpMap(pMap), mnLastRelocFrameId(0)
 {
     mbCreateNewKFAfterReloc = false;
@@ -1705,10 +1705,8 @@ void Tracking::SearchLocalPoints()
         if(pMP->isBad())
             continue;
         // Project (this fills MapPoint variables for matching)
-
         if(mCurrentFrame.isInFrustum(pMP,0.5))
         {
-
             pMP->IncreaseVisible();
             nToMatch++;
         }
@@ -2062,11 +2060,14 @@ bool Tracking::Relocalization()
 
 void Tracking::Reset()
 {
-    mpViewer->RequestStop();
 
     cout << "System Reseting" << endl;
-    while(!mpViewer->isStopped())
-        usleep(3000);
+    if(mpViewer)
+    {
+        mpViewer->RequestStop();
+        while(!mpViewer->isStopped())
+            usleep(3000);
+    }
 
     // Reset Local Mapping
     cout << "Reseting Local Mapper...";
@@ -2101,7 +2102,8 @@ void Tracking::Reset()
     mlFrameTimes.clear();
     mlbLost.clear();
 
-    mpViewer->Release();
+    if(mpViewer)
+        mpViewer->Release();
 }
 
 void Tracking::ChangeCalibration(const string &strSettingPath)
diff --git a/src/Viewer.cc b/src/Viewer.cc
index 92fa846..dec3204 100644
--- a/src/Viewer.cc
+++ b/src/Viewer.cc
@@ -28,7 +28,7 @@ namespace ORB_SLAM2
 
 Viewer::Viewer(System* pSystem, FrameDrawer *pFrameDrawer, MapDrawer *pMapDrawer, Tracking *pTracking, const string &strSettingPath):
     mpSystem(pSystem), mpFrameDrawer(pFrameDrawer),mpMapDrawer(pMapDrawer), mpTracker(pTracking),
-    mbFinishRequested(false), mbFinished(true), mbStopped(false), mbStopRequested(false)
+    mbFinishRequested(false), mbFinished(true), mbStopped(true), mbStopRequested(false)
 {
     cv::FileStorage fSettings(strSettingPath, cv::FileStorage::READ);
 
@@ -54,6 +54,7 @@ Viewer::Viewer(System* pSystem, FrameDrawer *pFrameDrawer, MapDrawer *pMapDrawer
 void Viewer::Run()
 {
     mbFinished = false;
+    mbStopped = false;
 
     pangolin::CreateWindowAndBind("ORB-SLAM2: Map Viewer",1024,768);