raw

src/Atlas.cc

@@ -0,0 +1,305 @@
+/**
+* This file is part of ORB-SLAM3
+*
+* Copyright (C) 2017-2020 Carlos Campos, Richard Elvira, Juan J. Gómez Rodríguez, José M.M. Montiel and Juan D. Tardós, University of Zaragoza.
+* Copyright (C) 2014-2016 Raúl Mur-Artal, José M.M. Montiel and Juan D. Tardós, University of Zaragoza.
+*
+* ORB-SLAM3 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-SLAM3 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-SLAM3.
+* If not, see <http://www.gnu.org/licenses/>.
+*/
+
+#include "Atlas.h"
+#include "Viewer.h"
+// #include "unistd.h"
+
+#include "GeometricCamera.h"
+#include "Pinhole.h"
+#include "KannalaBrandt8.h"
+
+namespace ORB_SLAM3
+{
+
+Atlas::Atlas(){
+    mpCurrentMap = static_cast<Map*>(NULL);
+}
+
+Atlas::Atlas(int initKFid): mnLastInitKFidMap(initKFid), mHasViewer(false)
+{
+    mpCurrentMap = static_cast<Map*>(NULL);
+    CreateNewMap();
+}
+
+Atlas::~Atlas()
+{
+    for(std::set<Map*>::iterator it = mspMaps.begin(), end = mspMaps.end(); it != end;)
+    {
+        Map* pMi = *it;
+
+        if(pMi)
+        {
+            delete pMi;
+            pMi = static_cast<Map*>(NULL);
+
+            it = mspMaps.erase(it);
+        }
+        else
+            ++it;
+
+    }
+}
+
+void Atlas::CreateNewMap()
+{
+    unique_lock<mutex> lock(mMutexAtlas);
+    cout << "Creation of new map with id: " << Map::nNextId << endl;
+    if(mpCurrentMap){
+        cout << "Exits current map " << endl;
+        if(!mspMaps.empty() && mnLastInitKFidMap < mpCurrentMap->GetMaxKFid())
+            mnLastInitKFidMap = mpCurrentMap->GetMaxKFid()+1; //The init KF is the next of current maximum
+
+        mpCurrentMap->SetStoredMap();
+        cout << "Saved map with ID: " << mpCurrentMap->GetId() << endl;
+
+        //if(mHasViewer)
+        //    mpViewer->AddMapToCreateThumbnail(mpCurrentMap);
+    }
+    cout << "Creation of new map with last KF id: " << mnLastInitKFidMap << endl;
+
+    mpCurrentMap = new Map(mnLastInitKFidMap);
+    mpCurrentMap->SetCurrentMap();
+    mspMaps.insert(mpCurrentMap);
+}
+
+void Atlas::ChangeMap(Map* pMap)
+{
+    unique_lock<mutex> lock(mMutexAtlas);
+    cout << "Chage to map with id: " << pMap->GetId() << endl;
+    if(mpCurrentMap){
+        mpCurrentMap->SetStoredMap();
+    }
+
+    mpCurrentMap = pMap;
+    mpCurrentMap->SetCurrentMap();
+}
+
+unsigned long int Atlas::GetLastInitKFid()
+{
+    unique_lock<mutex> lock(mMutexAtlas);
+    return mnLastInitKFidMap;
+}
+
+void Atlas::SetViewer(Viewer* pViewer)
+{
+    mpViewer = pViewer;
+    mHasViewer = true;
+}
+
+void Atlas::AddKeyFrame(KeyFrame* pKF)
+{
+    Map* pMapKF = pKF->GetMap();
+    pMapKF->AddKeyFrame(pKF);
+}
+
+void Atlas::AddMapPoint(MapPoint* pMP)
+{
+    Map* pMapMP = pMP->GetMap();
+    pMapMP->AddMapPoint(pMP);
+}
+
+void Atlas::AddCamera(GeometricCamera* pCam)
+{
+    mvpCameras.push_back(pCam);
+}
+
+void Atlas::SetReferenceMapPoints(const std::vector<MapPoint*> &vpMPs)
+{
+    unique_lock<mutex> lock(mMutexAtlas);
+    mpCurrentMap->SetReferenceMapPoints(vpMPs);
+}
+
+void Atlas::InformNewBigChange()
+{
+    unique_lock<mutex> lock(mMutexAtlas);
+    mpCurrentMap->InformNewBigChange();
+}
+
+int Atlas::GetLastBigChangeIdx()
+{
+    unique_lock<mutex> lock(mMutexAtlas);
+    return mpCurrentMap->GetLastBigChangeIdx();
+}
+
+long unsigned int Atlas::MapPointsInMap()
+{
+    unique_lock<mutex> lock(mMutexAtlas);
+    return mpCurrentMap->MapPointsInMap();
+}
+
+long unsigned Atlas::KeyFramesInMap()
+{
+    unique_lock<mutex> lock(mMutexAtlas);
+    return mpCurrentMap->KeyFramesInMap();
+}
+
+std::vector<KeyFrame*> Atlas::GetAllKeyFrames()
+{
+    unique_lock<mutex> lock(mMutexAtlas);
+    return mpCurrentMap->GetAllKeyFrames();
+}
+
+std::vector<MapPoint*> Atlas::GetAllMapPoints()
+{
+    unique_lock<mutex> lock(mMutexAtlas);
+    return mpCurrentMap->GetAllMapPoints();
+}
+
+std::vector<MapPoint*> Atlas::GetReferenceMapPoints()
+{
+    unique_lock<mutex> lock(mMutexAtlas);
+    return mpCurrentMap->GetReferenceMapPoints();
+}
+
+vector<Map*> Atlas::GetAllMaps()
+{
+    unique_lock<mutex> lock(mMutexAtlas);
+    struct compFunctor
+    {
+        inline bool operator()(Map* elem1 ,Map* elem2)
+        {
+            return elem1->GetId() < elem2->GetId();
+        }
+    };
+    vector<Map*> vMaps(mspMaps.begin(),mspMaps.end());
+    sort(vMaps.begin(), vMaps.end(), compFunctor());
+    return vMaps;
+}
+
+int Atlas::CountMaps()
+{
+    unique_lock<mutex> lock(mMutexAtlas);
+    return mspMaps.size();
+}
+
+void Atlas::clearMap()
+{
+    unique_lock<mutex> lock(mMutexAtlas);
+    mpCurrentMap->clear();
+}
+
+void Atlas::clearAtlas()
+{
+    unique_lock<mutex> lock(mMutexAtlas);
+    /*for(std::set<Map*>::iterator it=mspMaps.begin(), send=mspMaps.end(); it!=send; it++)
+    {
+        (*it)->clear();
+        delete *it;
+    }*/
+    mspMaps.clear();
+    mpCurrentMap = static_cast<Map*>(NULL);
+    mnLastInitKFidMap = 0;
+}
+
+Map* Atlas::GetCurrentMap()
+{
+    unique_lock<mutex> lock(mMutexAtlas);
+    if(!mpCurrentMap)
+        CreateNewMap();
+    while(mpCurrentMap->IsBad())
+        usleep(3000);
+
+    return mpCurrentMap;
+}
+
+void Atlas::SetMapBad(Map* pMap)
+{
+    mspMaps.erase(pMap);
+    pMap->SetBad();
+
+    mspBadMaps.insert(pMap);
+}
+
+void Atlas::RemoveBadMaps()
+{
+    /*for(Map* pMap : mspBadMaps)
+    {
+        delete pMap;
+        pMap = static_cast<Map*>(NULL);
+    }*/
+    mspBadMaps.clear();
+}
+
+bool Atlas::isInertial()
+{
+    unique_lock<mutex> lock(mMutexAtlas);
+    return mpCurrentMap->IsInertial();
+}
+
+void Atlas::SetInertialSensor()
+{
+    unique_lock<mutex> lock(mMutexAtlas);
+    mpCurrentMap->SetInertialSensor();
+}
+
+void Atlas::SetImuInitialized()
+{
+    unique_lock<mutex> lock(mMutexAtlas);
+    mpCurrentMap->SetImuInitialized();
+}
+
+bool Atlas::isImuInitialized()
+{
+    unique_lock<mutex> lock(mMutexAtlas);
+    return mpCurrentMap->isImuInitialized();
+}
+
+void Atlas::SetKeyFrameDababase(KeyFrameDatabase* pKFDB)
+{
+    mpKeyFrameDB = pKFDB;
+}
+
+KeyFrameDatabase* Atlas::GetKeyFrameDatabase()
+{
+    return mpKeyFrameDB;
+}
+
+void Atlas::SetORBVocabulary(ORBVocabulary* pORBVoc)
+{
+    mpORBVocabulary = pORBVoc;
+}
+
+ORBVocabulary* Atlas::GetORBVocabulary()
+{
+    return mpORBVocabulary;
+}
+
+long unsigned int Atlas::GetNumLivedKF()
+{
+    unique_lock<mutex> lock(mMutexAtlas);
+    long unsigned int num = 0;
+    for(Map* mMAPi : mspMaps)
+    {
+        num += mMAPi->GetAllKeyFrames().size();
+    }
+
+    return num;
+}
+
+long unsigned int Atlas::GetNumLivedMP() {
+    unique_lock<mutex> lock(mMutexAtlas);
+    long unsigned int num = 0;
+    for (Map *mMAPi : mspMaps) {
+        num += mMAPi->GetAllMapPoints().size();
+    }
+
+    return num;
+}
+
+} //namespace ORB_SLAM3

src/CameraModels/KannalaBrandt8.cpp

@@ -0,0 +1,536 @@
+/**
+* This file is part of ORB-SLAM3
+*
+* Copyright (C) 2017-2020 Carlos Campos, Richard Elvira, Juan J. Gómez Rodríguez, José M.M. Montiel and Juan D. Tardós, University of Zaragoza.
+* Copyright (C) 2014-2016 Raúl Mur-Artal, José M.M. Montiel and Juan D. Tardós, University of Zaragoza.
+*
+* ORB-SLAM3 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-SLAM3 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-SLAM3.
+* If not, see <http://www.gnu.org/licenses/>.
+*/
+
+#include "KannalaBrandt8.h"
+
+#include <boost/serialization/export.hpp>
+
+namespace ORB_SLAM3 {
+
+    cv::Point2f KannalaBrandt8::project(const cv::Point3f &p3D) {
+        const float x2_plus_y2 = p3D.x * p3D.x + p3D.y * p3D.y;
+        const float theta = atan2f(sqrtf(x2_plus_y2), p3D.z);
+        const float psi = atan2f(p3D.y, p3D.x);
+
+        const float theta2 = theta * theta;
+        const float theta3 = theta * theta2;
+        const float theta5 = theta3 * theta2;
+        const float theta7 = theta5 * theta2;
+        const float theta9 = theta7 * theta2;
+        const float r = theta + mvParameters[4] * theta3 + mvParameters[5] * theta5
+                        + mvParameters[6] * theta7 + mvParameters[7] * theta9;
+
+        return cv::Point2f(mvParameters[0] * r * cos(psi) + mvParameters[2],
+                           mvParameters[1] * r * sin(psi) + mvParameters[3]);
+
+    }
+
+    cv::Point2f KannalaBrandt8::project(const cv::Matx31f &m3D) {
+        return this->project(cv::Point3f(m3D(0),m3D(1),m3D(2)));
+    }
+
+    cv::Point2f KannalaBrandt8::project(const cv::Mat &m3D) {
+        const float* p3D = m3D.ptr<float>();
+
+        return this->project(cv::Point3f(p3D[0],p3D[1],p3D[2]));
+    }
+
+    Eigen::Vector2d KannalaBrandt8::project(const Eigen::Vector3d &v3D) {
+        const double x2_plus_y2 = v3D[0] * v3D[0] + v3D[1] * v3D[1];
+        const double theta = atan2f(sqrtf(x2_plus_y2), v3D[2]);
+        const double psi = atan2f(v3D[1], v3D[0]);
+
+        const double theta2 = theta * theta;
+        const double theta3 = theta * theta2;
+        const double theta5 = theta3 * theta2;
+        const double theta7 = theta5 * theta2;
+        const double theta9 = theta7 * theta2;
+        const double r = theta + mvParameters[4] * theta3 + mvParameters[5] * theta5
+                        + mvParameters[6] * theta7 + mvParameters[7] * theta9;
+
+        Eigen::Vector2d res;
+        res[0] = mvParameters[0] * r * cos(psi) + mvParameters[2];
+        res[1] = mvParameters[1] * r * sin(psi) + mvParameters[3];
+
+        return res;
+    }
+
+    cv::Mat KannalaBrandt8::projectMat(const cv::Point3f &p3D) {
+        cv::Point2f point = this->project(p3D);
+        cv::Mat ret = (cv::Mat_<float>(2,1) << point.x, point.y);
+        return ret.clone();
+    }
+
+    float KannalaBrandt8::uncertainty2(const Eigen::Matrix<double,2,1> &p2D)
+    {
+        return 1.f;
+    }
+
+    cv::Mat KannalaBrandt8::unprojectMat(const cv::Point2f &p2D){
+        cv::Point3f ray = this->unproject(p2D);
+        cv::Mat ret = (cv::Mat_<float>(3,1) << ray.x, ray.y, ray.z);
+        return ret.clone();
+    }
+
+        cv::Matx31f KannalaBrandt8::unprojectMat_(const cv::Point2f &p2D) {
+            cv::Point3f ray = this->unproject(p2D);
+            cv::Matx31f r{ray.x, ray.y, ray.z};
+            return r;
+    }
+
+    cv::Point3f KannalaBrandt8::unproject(const cv::Point2f &p2D) {
+        //Use Newthon method to solve for theta with good precision (err ~ e-6)
+        cv::Point2f pw((p2D.x - mvParameters[2]) / mvParameters[0], (p2D.y - mvParameters[3]) / mvParameters[1]);
+        float scale = 1.f;
+        float theta_d = sqrtf(pw.x * pw.x + pw.y * pw.y);
+        theta_d = fminf(fmaxf(-CV_PI / 2.f, theta_d), CV_PI / 2.f);
+
+        if (theta_d > 1e-8) {
+            //Compensate distortion iteratively
+            float theta = theta_d;
+
+            for (int j = 0; j < 10; j++) {
+                float theta2 = theta * theta, theta4 = theta2 * theta2, theta6 = theta4 * theta2, theta8 =
+                        theta4 * theta4;
+                float k0_theta2 = mvParameters[4] * theta2, k1_theta4 = mvParameters[5] * theta4;
+                float k2_theta6 = mvParameters[6] * theta6, k3_theta8 = mvParameters[7] * theta8;
+                float theta_fix = (theta * (1 + k0_theta2 + k1_theta4 + k2_theta6 + k3_theta8) - theta_d) /
+                                  (1 + 3 * k0_theta2 + 5 * k1_theta4 + 7 * k2_theta6 + 9 * k3_theta8);
+                theta = theta - theta_fix;
+                if (fabsf(theta_fix) < precision)
+                    break;
+            }
+            //scale = theta - theta_d;
+            scale = std::tan(theta) / theta_d;
+        }
+
+        return cv::Point3f(pw.x * scale, pw.y * scale, 1.f);
+    }
+
+    cv::Mat KannalaBrandt8::projectJac(const cv::Point3f &p3D) {
+        float x2 = p3D.x * p3D.x, y2 = p3D.y * p3D.y, z2 = p3D.z * p3D.z;
+        float r2 = x2 + y2;
+        float r = sqrt(r2);
+        float r3 = r2 * r;
+        float theta = atan2(r, p3D.z);
+
+        float theta2 = theta * theta, theta3 = theta2 * theta;
+        float theta4 = theta2 * theta2, theta5 = theta4 * theta;
+        float theta6 = theta2 * theta4, theta7 = theta6 * theta;
+        float theta8 = theta4 * theta4, theta9 = theta8 * theta;
+
+        float f = theta + theta3 * mvParameters[4] + theta5 * mvParameters[5] + theta7 * mvParameters[6] +
+                  theta9 * mvParameters[7];
+        float fd = 1 + 3 * mvParameters[4] * theta2 + 5 * mvParameters[5] * theta4 + 7 * mvParameters[6] * theta6 +
+                   9 * mvParameters[7] * theta8;
+
+        cv::Mat Jac(2, 3, CV_32F);
+        Jac.at<float>(0, 0) = mvParameters[0] * (fd * p3D.z * x2 / (r2 * (r2 + z2)) + f * y2 / r3);
+        Jac.at<float>(1, 0) =
+                mvParameters[1] * (fd * p3D.z * p3D.y * p3D.x / (r2 * (r2 + z2)) - f * p3D.y * p3D.x / r3);
+
+        Jac.at<float>(0, 1) =
+                mvParameters[0] * (fd * p3D.z * p3D.y * p3D.x / (r2 * (r2 + z2)) - f * p3D.y * p3D.x / r3);
+        Jac.at<float>(1, 1) = mvParameters[1] * (fd * p3D.z * y2 / (r2 * (r2 + z2)) + f * x2 / r3);
+
+        Jac.at<float>(0, 2) = -mvParameters[0] * fd * p3D.x / (r2 + z2);
+        Jac.at<float>(1, 2) = -mvParameters[1] * fd * p3D.y / (r2 + z2);
+
+        std::cout << "CV JAC: " << Jac << std::endl;
+
+        return Jac.clone();
+    }
+
+    Eigen::Matrix<double, 2, 3> KannalaBrandt8::projectJac(const Eigen::Vector3d &v3D) {
+        double x2 = v3D[0] * v3D[0], y2 = v3D[1] * v3D[1], z2 = v3D[2] * v3D[2];
+        double r2 = x2 + y2;
+        double r = sqrt(r2);
+        double r3 = r2 * r;
+        double theta = atan2(r, v3D[2]);
+
+        double theta2 = theta * theta, theta3 = theta2 * theta;
+        double theta4 = theta2 * theta2, theta5 = theta4 * theta;
+        double theta6 = theta2 * theta4, theta7 = theta6 * theta;
+        double theta8 = theta4 * theta4, theta9 = theta8 * theta;
+
+        double f = theta + theta3 * mvParameters[4] + theta5 * mvParameters[5] + theta7 * mvParameters[6] +
+                  theta9 * mvParameters[7];
+        double fd = 1 + 3 * mvParameters[4] * theta2 + 5 * mvParameters[5] * theta4 + 7 * mvParameters[6] * theta6 +
+                   9 * mvParameters[7] * theta8;
+
+        Eigen::Matrix<double, 2, 3> JacGood;
+        JacGood(0, 0) = mvParameters[0] * (fd * v3D[2] * x2 / (r2 * (r2 + z2)) + f * y2 / r3);
+        JacGood(1, 0) =
+                mvParameters[1] * (fd * v3D[2] * v3D[1] * v3D[0] / (r2 * (r2 + z2)) - f * v3D[1] * v3D[0] / r3);
+
+        JacGood(0, 1) =
+                mvParameters[0] * (fd * v3D[2] * v3D[1] * v3D[0] / (r2 * (r2 + z2)) - f * v3D[1] * v3D[0] / r3);
+        JacGood(1, 1) = mvParameters[1] * (fd * v3D[2] * y2 / (r2 * (r2 + z2)) + f * x2 / r3);
+
+        JacGood(0, 2) = -mvParameters[0] * fd * v3D[0] / (r2 + z2);
+        JacGood(1, 2) = -mvParameters[1] * fd * v3D[1] / (r2 + z2);
+
+        return JacGood;
+    }
+
+    cv::Mat KannalaBrandt8::unprojectJac(const cv::Point2f &p2D) {
+        return cv::Mat();
+    }
+
+    bool KannalaBrandt8::ReconstructWithTwoViews(const std::vector<cv::KeyPoint>& vKeys1, const std::vector<cv::KeyPoint>& vKeys2, const std::vector<int> &vMatches12,
+                                          cv::Mat &R21, cv::Mat &t21, std::vector<cv::Point3f> &vP3D, std::vector<bool> &vbTriangulated){
+        if(!tvr){
+            cv::Mat K = this->toK();
+            tvr = new TwoViewReconstruction(K);
+        }
+
+        //Correct FishEye distortion
+        std::vector<cv::KeyPoint> vKeysUn1 = vKeys1, vKeysUn2 = vKeys2;
+        std::vector<cv::Point2f> vPts1(vKeys1.size()), vPts2(vKeys2.size());
+
+        for(size_t i = 0; i < vKeys1.size(); i++) vPts1[i] = vKeys1[i].pt;
+        for(size_t i = 0; i < vKeys2.size(); i++) vPts2[i] = vKeys2[i].pt;
+
+        cv::Mat D = (cv::Mat_<float>(4,1) << mvParameters[4], mvParameters[5], mvParameters[6], mvParameters[7]);
+        cv::Mat R = cv::Mat::eye(3,3,CV_32F);
+        cv::Mat K = this->toK();
+        cv::fisheye::undistortPoints(vPts1,vPts1,K,D,R,K);
+        cv::fisheye::undistortPoints(vPts2,vPts2,K,D,R,K);
+
+        for(size_t i = 0; i < vKeys1.size(); i++) vKeysUn1[i].pt = vPts1[i];
+        for(size_t i = 0; i < vKeys2.size(); i++) vKeysUn2[i].pt = vPts2[i];
+
+        return tvr->Reconstruct(vKeysUn1,vKeysUn2,vMatches12,R21,t21,vP3D,vbTriangulated);
+    }
+
+
+    cv::Mat KannalaBrandt8::toK() {
+        cv::Mat K = (cv::Mat_<float>(3, 3)
+                << mvParameters[0], 0.f, mvParameters[2], 0.f, mvParameters[1], mvParameters[3], 0.f, 0.f, 1.f);
+        return K;
+    }
+
+    cv::Matx33f KannalaBrandt8::toK_() {
+        cv::Matx33f K{mvParameters[0], 0.f, mvParameters[2], 0.f, mvParameters[1], mvParameters[3], 0.f, 0.f, 1.f};
+
+        return K;
+    }
+
+    bool KannalaBrandt8::epipolarConstrain(GeometricCamera* pCamera2, const cv::KeyPoint &kp1, const cv::KeyPoint &kp2, const cv::Mat &R12, const cv::Mat &t12, const float sigmaLevel, const float unc) {
+        cv::Mat p3D;
+        return this->TriangulateMatches(pCamera2,kp1,kp2,R12,t12,sigmaLevel,unc,p3D) > 0.0001f;
+    }
+
+    bool KannalaBrandt8::epipolarConstrain_(GeometricCamera* pCamera2, const cv::KeyPoint &kp1, const cv::KeyPoint &kp2, const cv::Matx33f &R12, const cv::Matx31f &t12, const float sigmaLevel, const float unc) {
+        cv::Matx31f p3D;
+        return this->TriangulateMatches_(pCamera2,kp1,kp2,R12,t12,sigmaLevel,unc,p3D) > 0.0001f;
+    }
+
+    bool KannalaBrandt8::matchAndtriangulate(const cv::KeyPoint& kp1, const cv::KeyPoint& kp2, GeometricCamera* pOther,
+                                             cv::Mat& Tcw1, cv::Mat& Tcw2,
+                                            const float sigmaLevel1, const float sigmaLevel2,
+                                            cv::Mat& x3Dtriangulated){
+        cv::Mat Rcw1 = Tcw1.colRange(0,3).rowRange(0,3);
+        cv::Mat Rwc1 = Rcw1.t();
+        cv::Mat tcw1 = Tcw1.rowRange(0,3).col(3);
+
+        cv::Mat Rcw2 = Tcw2.colRange(0,3).rowRange(0,3);
+        cv::Mat Rwc2 = Rcw2.t();
+        cv::Mat tcw2 = Tcw2.rowRange(0,3).col(3);
+
+        cv::Point3f ray1c = this->unproject(kp1.pt);
+        cv::Point3f ray2c = pOther->unproject(kp2.pt);
+
+        cv::Mat r1(3,1,CV_32F);
+        r1.at<float>(0) = ray1c.x;
+        r1.at<float>(1) = ray1c.y;
+        r1.at<float>(2) = ray1c.z;
+
+        cv::Mat r2(3,1,CV_32F);
+        r2.at<float>(0) = ray2c.x;
+        r2.at<float>(1) = ray2c.y;
+        r2.at<float>(2) = ray2c.z;
+
+        //Check parallax between rays
+        cv::Mat ray1 = Rwc1*r1;
+        cv::Mat ray2 = Rwc2*r2;
+
+        const float cosParallaxRays = ray1.dot(ray2)/(cv::norm(ray1)*cv::norm(ray2));
+
+        //If parallax is lower than 0.9998, reject this match
+        if(cosParallaxRays > 0.9998){
+            return false;
+        }
+
+        //Parallax is good, so we try to triangulate
+        cv::Point2f p11,p22;
+
+        p11.x = ray1c.x;
+        p11.y = ray1c.y;
+
+        p22.x = ray2c.x;
+        p22.y = ray2c.y;
+
+        cv::Mat x3D;
+
+        Triangulate(p11,p22,Tcw1,Tcw2,x3D);
+
+        cv::Mat x3Dt = x3D.t();
+
+        //Check triangulation in front of cameras
+        float z1 = Rcw1.row(2).dot(x3Dt)+tcw1.at<float>(2);
+        if(z1<=0){  //Point is not in front of the first camera
+            return false;
+        }
+
+
+        float z2 = Rcw2.row(2).dot(x3Dt)+tcw2.at<float>(2);
+        if(z2<=0){ //Point is not in front of the first camera
+            return false;
+        }
+
+        //Check reprojection error in first keyframe
+        //  -Transform point into camera reference system
+        cv::Mat x3D1 = Rcw1 * x3D + tcw1;
+        cv::Point2f uv1 = this->project(x3D1);
+
+        float errX1 = uv1.x - kp1.pt.x;
+        float errY1 = uv1.y - kp1.pt.y;
+
+        if((errX1*errX1+errY1*errY1)>5.991*sigmaLevel1){   //Reprojection error is high
+            return false;
+        }
+
+        //Check reprojection error in second keyframe;
+        //  -Transform point into camera reference system
+        cv::Mat x3D2 = Rcw2 * x3D + tcw2;
+        cv::Point2f uv2 = pOther->project(x3D2);
+
+        float errX2 = uv2.x - kp2.pt.x;
+        float errY2 = uv2.y - kp2.pt.y;
+
+        if((errX2*errX2+errY2*errY2)>5.991*sigmaLevel2){   //Reprojection error is high
+            return false;
+        }
+
+        //Since parallax is big enough and reprojection errors are low, this pair of points
+        //can be considered as a match
+        x3Dtriangulated = x3D.clone();
+
+        return true;
+    }
+
+    float KannalaBrandt8::TriangulateMatches(GeometricCamera *pCamera2, const cv::KeyPoint &kp1, const cv::KeyPoint &kp2, const cv::Mat &R12, const cv::Mat &t12, const float sigmaLevel, const float unc, cv::Mat& p3D) {
+        cv::Mat r1 = this->unprojectMat(kp1.pt);
+        cv::Mat r2 = pCamera2->unprojectMat(kp2.pt);
+
+        //Check parallax
+        cv::Mat r21 = R12*r2;
+
+        const float cosParallaxRays = r1.dot(r21)/(cv::norm(r1)*cv::norm(r21));
+
+        if(cosParallaxRays > 0.9998){
+            return -1;
+        }
+
+        //Parallax is good, so we try to triangulate
+        cv::Point2f p11,p22;
+        const float* pr1 = r1.ptr<float>();
+        const float* pr2 = r2.ptr<float>();
+
+        p11.x = pr1[0];
+        p11.y = pr1[1];
+
+        p22.x = pr2[0];
+        p22.y = pr2[1];
+
+        cv::Mat x3D;
+        cv::Mat Tcw1 = (cv::Mat_<float>(3,4) << 1.f,0.f,0.f,0.f,
+                                                0.f,1.f,0.f,0.f,
+                                                0.f,0.f,1.f,0.f);
+        cv::Mat Tcw2;
+        cv::Mat R21 = R12.t();
+        cv::Mat t21 = -R21*t12;
+        cv::hconcat(R21,t21,Tcw2);
+
+        Triangulate(p11,p22,Tcw1,Tcw2,x3D);
+        cv::Mat x3Dt = x3D.t();
+
+        float z1 = x3D.at<float>(2);
+        if(z1 <= 0){
+            return -1;
+        }
+
+        float z2 = R21.row(2).dot(x3Dt)+t21.at<float>(2);
+        if(z2<=0){
+            return -1;
+        }
+
+        //Check reprojection error
+        cv::Point2f uv1 = this->project(x3D);
+
+        float errX1 = uv1.x - kp1.pt.x;
+        float errY1 = uv1.y - kp1.pt.y;
+
+        if((errX1*errX1+errY1*errY1)>5.991 * sigmaLevel){   //Reprojection error is high
+            return -1;
+        }
+
+        cv::Mat x3D2 = R21 * x3D + t21;
+        cv::Point2f uv2 = pCamera2->project(x3D2);
+
+        float errX2 = uv2.x - kp2.pt.x;
+        float errY2 = uv2.y - kp2.pt.y;
+
+        if((errX2*errX2+errY2*errY2)>5.991 * unc){   //Reprojection error is high
+            return -1;
+        }
+
+        p3D = x3D.clone();
+
+        return z1;
+    }
+
+    float KannalaBrandt8::TriangulateMatches_(GeometricCamera *pCamera2, const cv::KeyPoint &kp1, const cv::KeyPoint &kp2, const cv::Matx33f &R12, const cv::Matx31f &t12, const float sigmaLevel, const float unc, cv::Matx31f& p3D) {
+        cv::Matx31f r1 = this->unprojectMat_(kp1.pt);
+        cv::Matx31f r2 = pCamera2->unprojectMat_(kp2.pt);
+
+        //Check parallax
+        cv::Matx31f r21 = R12*r2;
+
+        const float cosParallaxRays = r1.dot(r21)/(cv::norm(r1)*cv::norm(r21));
+
+        if(cosParallaxRays > 0.9998){
+            return -1;
+        }
+
+        //Parallax is good, so we try to triangulate
+        cv::Point2f p11,p22;
+
+        p11.x = r1(0);
+        p11.y = r1(1);
+
+        p22.x = r2(0);
+        p22.y = r2(1);
+
+        cv::Matx31f x3D;
+        cv::Matx44f Tcw1{1.f,0.f,0.f,0.f,
+                0.f,1.f,0.f,0.f,
+                0.f,0.f,1.f,0.f};
+
+        cv::Matx33f R21 = R12.t();
+        cv::Matx31f t21 = -R21*t12;
+
+        cv::Matx44f Tcw2{R21(0,0),R21(0,1),R21(0,2),t21(0),
+                         R21(1,0),R21(1,1),R21(1,2),t21(1),
+                         R21(2,0),R21(2,1),R21(2,2),t21(2),
+                         0.f,0.f,0.f,1.f};
+
+        Triangulate_(p11,p22,Tcw1,Tcw2,x3D);
+        cv::Matx13f x3Dt = x3D.t();
+
+        float z1 = x3D(2);
+        if(z1 <= 0){
+            return -1;
+        }
+
+        float z2 = R21.row(2).dot(x3Dt)+t21(2);
+        if(z2<=0){
+            return -1;
+        }
+
+        //Check reprojection error
+        cv::Point2f uv1 = this->project(x3D);
+
+        float errX1 = uv1.x - kp1.pt.x;
+        float errY1 = uv1.y - kp1.pt.y;
+
+        if((errX1*errX1+errY1*errY1)>5.991 * sigmaLevel){   //Reprojection error is high
+            return -1;
+        }
+
+        cv::Matx31f x3D2 = R21 * x3D + t21;
+        cv::Point2f uv2 = pCamera2->project(x3D2);
+
+        float errX2 = uv2.x - kp2.pt.x;
+        float errY2 = uv2.y - kp2.pt.y;
+
+        if((errX2*errX2+errY2*errY2)>5.991 * unc){   //Reprojection error is high
+            return -1;
+        }
+
+        p3D = x3D;
+
+        return z1;
+    }
+
+    std::ostream & operator<<(std::ostream &os, const KannalaBrandt8 &kb) {
+        os << kb.mvParameters[0] << " " << kb.mvParameters[1] << " " << kb.mvParameters[2] << " " << kb.mvParameters[3] << " "
+           << kb.mvParameters[4] << " " << kb.mvParameters[5] << " " << kb.mvParameters[6] << " " << kb.mvParameters[7];
+        return os;
+    }
+
+    std::istream & operator>>(std::istream &is, KannalaBrandt8 &kb) {
+        float nextParam;
+        for(size_t i = 0; i < 8; i++){
+            assert(is.good());  //Make sure the input stream is good
+            is >> nextParam;
+            kb.mvParameters[i] = nextParam;
+
+        }
+        return is;
+    }
+
+    void KannalaBrandt8::Triangulate(const cv::Point2f &p1, const cv::Point2f &p2, const cv::Mat &Tcw1, const cv::Mat &Tcw2, cv::Mat &x3D)
+    {
+        cv::Mat A(4,4,CV_32F);
+
+        A.row(0) = p1.x*Tcw1.row(2)-Tcw1.row(0);
+        A.row(1) = p1.y*Tcw1.row(2)-Tcw1.row(1);
+        A.row(2) = p2.x*Tcw2.row(2)-Tcw2.row(0);
+        A.row(3) = p2.y*Tcw2.row(2)-Tcw2.row(1);
+
+        cv::Mat u,w,vt;
+        cv::SVD::compute(A,w,u,vt,cv::SVD::MODIFY_A| cv::SVD::FULL_UV);
+        x3D = vt.row(3).t();
+        x3D = x3D.rowRange(0,3)/x3D.at<float>(3);
+    }
+
+    void KannalaBrandt8::Triangulate_(const cv::Point2f &p1, const cv::Point2f &p2, const cv::Matx44f &Tcw1, const cv::Matx44f &Tcw2, cv::Matx31f &x3D)
+    {
+        cv::Matx14f A0, A1, A2, A3;
+
+
+        A0 = p1.x * Tcw1.row(2) - Tcw1.row(0);
+        A1 = p1.y * Tcw1.row(2) - Tcw1.row(1);
+        A2 = p2.x * Tcw2.row(2) - Tcw2.row(0);
+        A3 = p2.y * Tcw2.row(2) - Tcw2.row(1);
+        cv::Matx44f A{A0(0), A0(1), A0(2), A0(3),
+                      A1(0), A1(1), A1(2), A1(3),
+                      A2(0), A2(1), A2(2), A2(3),
+                      A3(0), A3(1), A3(2), A3(3)};
+
+        // cv::Mat u,w,vt;
+        cv::Matx44f u, vt;
+        cv::Matx41f w;
+
+        cv::SVD::compute(A,w,u,vt,cv::SVD::MODIFY_A| cv::SVD::FULL_UV);
+        cv::Matx41f x3D_h = vt.row(3).t();
+        x3D = x3D_h.get_minor<3,1>(0,0) / x3D_h(3);
+    }
+}

src/CameraModels/Pinhole.cpp

@@ -0,0 +1,214 @@
+/**
+* This file is part of ORB-SLAM3
+*
+* Copyright (C) 2017-2020 Carlos Campos, Richard Elvira, Juan J. Gómez Rodríguez, José M.M. Montiel and Juan D. Tardós, University of Zaragoza.
+* Copyright (C) 2014-2016 Raúl Mur-Artal, José M.M. Montiel and Juan D. Tardós, University of Zaragoza.
+*
+* ORB-SLAM3 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-SLAM3 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-SLAM3.
+* If not, see <http://www.gnu.org/licenses/>.
+*/
+
+#include "Pinhole.h"
+
+#include <boost/serialization/export.hpp>
+
+namespace ORB_SLAM3 {
+
+    long unsigned int GeometricCamera::nNextId=0;
+
+    cv::Point2f Pinhole::project(const cv::Point3f &p3D) {
+        return cv::Point2f(mvParameters[0] * p3D.x / p3D.z + mvParameters[2],
+                           mvParameters[1] * p3D.y / p3D.z + mvParameters[3]);
+    }
+
+    cv::Point2f Pinhole::project(const cv::Matx31f &m3D) {
+        return this->project(cv::Point3f(m3D(0),m3D(1),m3D(2)));
+    }
+
+    cv::Point2f Pinhole::project(const cv::Mat &m3D) {
+        const float* p3D = m3D.ptr<float>();
+
+        return this->project(cv::Point3f(p3D[0],p3D[1],p3D[2]));
+    }
+
+    Eigen::Vector2d Pinhole::project(const Eigen::Vector3d &v3D) {
+        Eigen::Vector2d res;
+        res[0] = mvParameters[0] * v3D[0] / v3D[2] + mvParameters[2];
+        res[1] = mvParameters[1] * v3D[1] / v3D[2] + mvParameters[3];
+
+        return res;
+    }
+
+    cv::Mat Pinhole::projectMat(const cv::Point3f &p3D) {
+        cv::Point2f point = this->project(p3D);
+        return (cv::Mat_<float>(2,1) << point.x, point.y);
+    }
+
+    float Pinhole::uncertainty2(const Eigen::Matrix<double,2,1> &p2D)
+    {
+        return 1.0;
+    }
+
+    cv::Point3f Pinhole::unproject(const cv::Point2f &p2D) {
+        return cv::Point3f((p2D.x - mvParameters[2]) / mvParameters[0], (p2D.y - mvParameters[3]) / mvParameters[1],
+                           1.f);
+    }
+
+    cv::Mat Pinhole::unprojectMat(const cv::Point2f &p2D){
+        cv::Point3f ray = this->unproject(p2D);
+        return (cv::Mat_<float>(3,1) << ray.x, ray.y, ray.z);
+    }
+
+    cv::Matx31f Pinhole::unprojectMat_(const cv::Point2f &p2D) {
+        cv::Point3f ray = this->unproject(p2D);
+        cv::Matx31f r{ray.x, ray.y, ray.z};
+        return r;
+    }
+
+    cv::Mat Pinhole::projectJac(const cv::Point3f &p3D) {
+        cv::Mat Jac(2, 3, CV_32F);
+        Jac.at<float>(0, 0) = mvParameters[0] / p3D.z;
+        Jac.at<float>(0, 1) = 0.f;
+        Jac.at<float>(0, 2) = -mvParameters[0] * p3D.x / (p3D.z * p3D.z);
+        Jac.at<float>(1, 0) = 0.f;
+        Jac.at<float>(1, 1) = mvParameters[1] / p3D.z;
+        Jac.at<float>(1, 2) = -mvParameters[1] * p3D.y / (p3D.z * p3D.z);
+
+        return Jac;
+    }
+
+    Eigen::Matrix<double, 2, 3> Pinhole::projectJac(const Eigen::Vector3d &v3D) {
+        Eigen::Matrix<double, 2, 3> Jac;
+        Jac(0, 0) = mvParameters[0] / v3D[2];
+        Jac(0, 1) = 0.f;
+        Jac(0, 2) = -mvParameters[0] * v3D[0] / (v3D[2] * v3D[2]);
+        Jac(1, 0) = 0.f;
+        Jac(1, 1) = mvParameters[1] / v3D[2];
+        Jac(1, 2) = -mvParameters[1] * v3D[1] / (v3D[2] * v3D[2]);
+
+        return Jac;
+    }
+
+    cv::Mat Pinhole::unprojectJac(const cv::Point2f &p2D) {
+        cv::Mat Jac(3, 2, CV_32F);
+        Jac.at<float>(0, 0) = 1 / mvParameters[0];
+        Jac.at<float>(0, 1) = 0.f;
+        Jac.at<float>(1, 0) = 0.f;
+        Jac.at<float>(1, 1) = 1 / mvParameters[1];
+        Jac.at<float>(2, 0) = 0.f;
+        Jac.at<float>(2, 1) = 0.f;
+
+        return Jac;
+    }
+
+    bool Pinhole::ReconstructWithTwoViews(const std::vector<cv::KeyPoint>& vKeys1, const std::vector<cv::KeyPoint>& vKeys2, const std::vector<int> &vMatches12,
+                                 cv::Mat &R21, cv::Mat &t21, std::vector<cv::Point3f> &vP3D, std::vector<bool> &vbTriangulated){
+        if(!tvr){
+            cv::Mat K = this->toK();
+            tvr = new TwoViewReconstruction(K);
+        }
+
+        return tvr->Reconstruct(vKeys1,vKeys2,vMatches12,R21,t21,vP3D,vbTriangulated);
+    }
+
+
+    cv::Mat Pinhole::toK() {
+        cv::Mat K = (cv::Mat_<float>(3, 3)
+                << mvParameters[0], 0.f, mvParameters[2], 0.f, mvParameters[1], mvParameters[3], 0.f, 0.f, 1.f);
+        return K;
+    }
+
+    cv::Matx33f Pinhole::toK_() {
+        cv::Matx33f K{mvParameters[0], 0.f, mvParameters[2], 0.f, mvParameters[1], mvParameters[3], 0.f, 0.f, 1.f};
+
+        return K;
+    }
+
+    bool Pinhole::epipolarConstrain(GeometricCamera* pCamera2,  const cv::KeyPoint &kp1, const cv::KeyPoint &kp2, const cv::Mat &R12, const cv::Mat &t12, const float sigmaLevel, const float unc) {
+        //Compute Fundamental Matrix
+        cv::Mat t12x = SkewSymmetricMatrix(t12);
+        cv::Mat K1 = this->toK();
+        cv::Mat K2 = pCamera2->toK();
+        cv::Mat F12 = K1.t().inv()*t12x*R12*K2.inv();
+
+        // Epipolar line in second image l = x1'F12 = [a b c]
+        const float a = kp1.pt.x*F12.at<float>(0,0)+kp1.pt.y*F12.at<float>(1,0)+F12.at<float>(2,0);
+        const float b = kp1.pt.x*F12.at<float>(0,1)+kp1.pt.y*F12.at<float>(1,1)+F12.at<float>(2,1);
+        const float c = kp1.pt.x*F12.at<float>(0,2)+kp1.pt.y*F12.at<float>(1,2)+F12.at<float>(2,2);
+
+        const float num = a*kp2.pt.x+b*kp2.pt.y+c;
+
+        const float den = a*a+b*b;
+
+        if(den==0)
+            return false;
+
+        const float dsqr = num*num/den;
+
+        return dsqr<3.84*unc;
+    }
+
+    bool Pinhole::epipolarConstrain_(GeometricCamera *pCamera2, const cv::KeyPoint &kp1, const cv::KeyPoint &kp2, const cv::Matx33f &R12, const cv::Matx31f &t12, const float sigmaLevel, const float unc) {
+        //Compute Fundamental Matrix
+        auto t12x = SkewSymmetricMatrix_(t12);
+        auto K1 = this->toK_();
+        auto K2 = pCamera2->toK_();
+        cv::Matx33f F12 = K1.t().inv()*t12x*R12*K2.inv();
+
+        // Epipolar line in second image l = x1'F12 = [a b c]
+        const float a = kp1.pt.x*F12(0,0)+kp1.pt.y*F12(1,0)+F12(2,0);
+        const float b = kp1.pt.x*F12(0,1)+kp1.pt.y*F12(1,1)+F12(2,1);
+        const float c = kp1.pt.x*F12(0,2)+kp1.pt.y*F12(1,2)+F12(2,2);
+
+        const float num = a*kp2.pt.x+b*kp2.pt.y+c;
+
+        const float den = a*a+b*b;
+
+        if(den==0)
+            return false;
+
+        const float dsqr = num*num/den;
+
+        return dsqr<3.84*unc;
+    }
+
+    std::ostream & operator<<(std::ostream &os, const Pinhole &ph) {
+        os << ph.mvParameters[0] << " " << ph.mvParameters[1] << " " << ph.mvParameters[2] << " " << ph.mvParameters[3];
+        return os;
+    }
+
+    std::istream & operator>>(std::istream &is, Pinhole &ph) {
+        float nextParam;
+        for(size_t i = 0; i < 4; i++){
+            assert(is.good());  //Make sure the input stream is good
+            is >> nextParam;
+            ph.mvParameters[i] = nextParam;
+
+        }
+        return is;
+    }
+
+    cv::Mat Pinhole::SkewSymmetricMatrix(const cv::Mat &v)
+    {
+        return (cv::Mat_<float>(3,3) <<             0, -v.at<float>(2), v.at<float>(1),
+                v.at<float>(2),               0,-v.at<float>(0),
+                -v.at<float>(1),  v.at<float>(0),              0);
+    }
+
+    cv::Matx33f Pinhole::SkewSymmetricMatrix_(const cv::Matx31f &v)
+    {
+        cv::Matx33f skew{0.f, -v(2), v(1),
+                         v(2), 0.f, -v(0),
+                         -v(1), v(0), 0.f};
+
+        return skew;
+    }
+}

src/Converter.cc

@@ -0,0 +1,217 @@
+/**
+* This file is part of ORB-SLAM3
+*
+* Copyright (C) 2017-2020 Carlos Campos, Richard Elvira, Juan J. Gómez Rodríguez, José M.M. Montiel and Juan D. Tardós, University of Zaragoza.
+* Copyright (C) 2014-2016 Raúl Mur-Artal, José M.M. Montiel and Juan D. Tardós, University of Zaragoza.
+*
+* ORB-SLAM3 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-SLAM3 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-SLAM3.
+* If not, see <http://www.gnu.org/licenses/>.
+*/
+
+#include "Converter.h"
+
+namespace ORB_SLAM3
+{
+
+std::vector<cv::Mat> Converter::toDescriptorVector(const cv::Mat &Descriptors)
+{
+    std::vector<cv::Mat> vDesc;
+    vDesc.reserve(Descriptors.rows);
+    for (int j=0;j<Descriptors.rows;j++)
+        vDesc.push_back(Descriptors.row(j));
+
+    return vDesc;
+}
+
+g2o::SE3Quat Converter::toSE3Quat(const cv::Mat &cvT)
+{
+    Eigen::Matrix<double,3,3> R;
+    R << cvT.at<float>(0,0), cvT.at<float>(0,1), cvT.at<float>(0,2),
+         cvT.at<float>(1,0), cvT.at<float>(1,1), cvT.at<float>(1,2),
+         cvT.at<float>(2,0), cvT.at<float>(2,1), cvT.at<float>(2,2);
+
+    Eigen::Matrix<double,3,1> t(cvT.at<float>(0,3), cvT.at<float>(1,3), cvT.at<float>(2,3));
+
+    return g2o::SE3Quat(R,t);
+}
+
+cv::Mat Converter::toCvMat(const g2o::SE3Quat &SE3)
+{
+    Eigen::Matrix<double,4,4> eigMat = SE3.to_homogeneous_matrix();
+    return toCvMat(eigMat);
+}
+
+cv::Mat Converter::toCvMat(const g2o::Sim3 &Sim3)
+{
+    Eigen::Matrix3d eigR = Sim3.rotation().toRotationMatrix();
+    Eigen::Vector3d eigt = Sim3.translation();
+    double s = Sim3.scale();
+    return toCvSE3(s*eigR,eigt);
+}
+
+cv::Mat Converter::toCvMat(const Eigen::Matrix<double,4,4> &m)
+{
+    cv::Mat cvMat(4,4,CV_32F);
+    for(int i=0;i<4;i++)
+        for(int j=0; j<4; j++)
+            cvMat.at<float>(i,j)=m(i,j);
+
+    return cvMat.clone();
+}
+
+cv::Mat Converter::toCvMat(const Eigen::Matrix3d &m)
+{
+    cv::Mat cvMat(3,3,CV_32F);
+    for(int i=0;i<3;i++)
+        for(int j=0; j<3; j++)
+            cvMat.at<float>(i,j)=m(i,j);
+
+    return cvMat.clone();
+}
+
+cv::Mat Converter::toCvMat(const Eigen::MatrixXd &m)
+{
+    cv::Mat cvMat(m.rows(),m.cols(),CV_32F);
+    for(int i=0;i<m.rows();i++)
+        for(int j=0; j<m.cols(); j++)
+            cvMat.at<float>(i,j)=m(i,j);
+
+    return cvMat.clone();
+}
+
+cv::Mat Converter::toCvMat(const Eigen::Matrix<double,3,1> &m)
+{
+    cv::Mat cvMat(3,1,CV_32F);
+    for(int i=0;i<3;i++)
+            cvMat.at<float>(i)=m(i);
+
+    return cvMat.clone();
+}
+
+cv::Mat Converter::toCvSE3(const Eigen::Matrix<double,3,3> &R, const Eigen::Matrix<double,3,1> &t)
+{
+    cv::Mat cvMat = cv::Mat::eye(4,4,CV_32F);
+    for(int i=0;i<3;i++)
+    {
+        for(int j=0;j<3;j++)
+        {
+            cvMat.at<float>(i,j)=R(i,j);
+        }
+    }
+    for(int i=0;i<3;i++)
+    {
+        cvMat.at<float>(i,3)=t(i);
+    }
+
+    return cvMat.clone();
+}
+
+Eigen::Matrix<double,3,1> Converter::toVector3d(const cv::Mat &cvVector)
+{
+    Eigen::Matrix<double,3,1> v;
+    v << cvVector.at<float>(0), cvVector.at<float>(1), cvVector.at<float>(2);
+
+    return v;
+}
+
+Eigen::Matrix<double,3,1> Converter::toVector3d(const cv::Point3f &cvPoint)
+{
+    Eigen::Matrix<double,3,1> v;
+    v << cvPoint.x, cvPoint.y, cvPoint.z;
+
+    return v;
+}
+
+Eigen::Matrix<double,3,3> Converter::toMatrix3d(const cv::Mat &cvMat3)
+{
+    Eigen::Matrix<double,3,3> M;
+
+    M << cvMat3.at<float>(0,0), cvMat3.at<float>(0,1), cvMat3.at<float>(0,2),
+         cvMat3.at<float>(1,0), cvMat3.at<float>(1,1), cvMat3.at<float>(1,2),
+         cvMat3.at<float>(2,0), cvMat3.at<float>(2,1), cvMat3.at<float>(2,2);
+
+    return M;
+}
+
+Eigen::Matrix<double,4,4> Converter::toMatrix4d(const cv::Mat &cvMat4)
+{
+    Eigen::Matrix<double,4,4> M;
+
+    M << cvMat4.at<float>(0,0), cvMat4.at<float>(0,1), cvMat4.at<float>(0,2), cvMat4.at<float>(0,3),
+         cvMat4.at<float>(1,0), cvMat4.at<float>(1,1), cvMat4.at<float>(1,2), cvMat4.at<float>(1,3),
+         cvMat4.at<float>(2,0), cvMat4.at<float>(2,1), cvMat4.at<float>(2,2), cvMat4.at<float>(2,3),
+         cvMat4.at<float>(3,0), cvMat4.at<float>(3,1), cvMat4.at<float>(3,2), cvMat4.at<float>(3,3);
+    return M;
+}
+
+
+std::vector<float> Converter::toQuaternion(const cv::Mat &M)
+{
+    Eigen::Matrix<double,3,3> eigMat = toMatrix3d(M);
+    Eigen::Quaterniond q(eigMat);
+
+    std::vector<float> v(4);
+    v[0] = q.x();
+    v[1] = q.y();
+    v[2] = q.z();
+    v[3] = q.w();
+
+    return v;
+}
+
+cv::Mat Converter::tocvSkewMatrix(const cv::Mat &v)
+{
+    return (cv::Mat_<float>(3,3) <<             0, -v.at<float>(2), v.at<float>(1),
+            v.at<float>(2),               0,-v.at<float>(0),
+            -v.at<float>(1),  v.at<float>(0),              0);
+}
+
+bool Converter::isRotationMatrix(const cv::Mat &R)
+{
+    cv::Mat Rt;
+    cv::transpose(R, Rt);
+    cv::Mat shouldBeIdentity = Rt * R;
+    cv::Mat I = cv::Mat::eye(3,3, shouldBeIdentity.type());
+
+    return  cv::norm(I, shouldBeIdentity) < 1e-6;
+
+}
+
+std::vector<float> Converter::toEuler(const cv::Mat &R)
+{
+    assert(isRotationMatrix(R));
+    float sy = sqrt(R.at<float>(0,0) * R.at<float>(0,0) +  R.at<float>(1,0) * R.at<float>(1,0) );
+
+    bool singular = sy < 1e-6;
+
+    float x, y, z;
+    if (!singular)
+    {
+        x = atan2(R.at<float>(2,1) , R.at<float>(2,2));
+        y = atan2(-R.at<float>(2,0), sy);
+        z = atan2(R.at<float>(1,0), R.at<float>(0,0));
+    }
+    else
+    {
+        x = atan2(-R.at<float>(1,2), R.at<float>(1,1));
+        y = atan2(-R.at<float>(2,0), sy);
+        z = 0;
+    }
+
+    std::vector<float> v_euler(3);
+    v_euler[0] = x;
+    v_euler[1] = y;
+    v_euler[2] = z;
+
+    return v_euler;
+}
+
+} //namespace ORB_SLAM

src/FrameDrawer.cc

@@ -0,0 +1,404 @@
+/**
+* This file is part of ORB-SLAM3
+*
+* Copyright (C) 2017-2020 Carlos Campos, Richard Elvira, Juan J. Gómez Rodríguez, José M.M. Montiel and Juan D. Tardós, University of Zaragoza.
+* Copyright (C) 2014-2016 Raúl Mur-Artal, José M.M. Montiel and Juan D. Tardós, University of Zaragoza.
+*
+* ORB-SLAM3 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-SLAM3 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-SLAM3.
+* If not, see <http://www.gnu.org/licenses/>.
+*/
+
+#include "FrameDrawer.h"
+#include "Tracking.h"
+
+#include <opencv2/core/core.hpp>
+#include <opencv2/highgui/highgui.hpp>
+
+#include<mutex>
+
+namespace ORB_SLAM3
+{
+
+FrameDrawer::FrameDrawer(Atlas* pAtlas):both(false),mpAtlas(pAtlas)
+{
+    mState=Tracking::SYSTEM_NOT_READY;
+    mIm = cv::Mat(480,640,CV_8UC3, cv::Scalar(0,0,0));
+    mImRight = cv::Mat(480,640,CV_8UC3, cv::Scalar(0,0,0));
+}
+
+cv::Mat FrameDrawer::DrawFrame(bool bOldFeatures)
+{
+    cv::Mat im;
+    vector<cv::KeyPoint> vIniKeys; // Initialization: KeyPoints in reference frame
+    vector<int> vMatches; // Initialization: correspondeces with reference keypoints
+    vector<cv::KeyPoint> vCurrentKeys; // KeyPoints in current frame
+    vector<bool> vbVO, vbMap; // Tracked MapPoints in current frame
+    vector<pair<cv::Point2f, cv::Point2f> > vTracks;
+    int state; // Tracking state
+
+    Frame currentFrame;
+    vector<MapPoint*> vpLocalMap;
+    vector<cv::KeyPoint> vMatchesKeys;
+    vector<MapPoint*> vpMatchedMPs;
+    vector<cv::KeyPoint> vOutlierKeys;
+    vector<MapPoint*> vpOutlierMPs;
+    map<long unsigned int, cv::Point2f> mProjectPoints;
+    map<long unsigned int, cv::Point2f> mMatchedInImage;
+
+    //Copy variables within scoped mutex
+    {
+        unique_lock<mutex> lock(mMutex);
+        state=mState;
+        if(mState==Tracking::SYSTEM_NOT_READY)
+            mState=Tracking::NO_IMAGES_YET;
+
+        mIm.copyTo(im);
+
+        if(mState==Tracking::NOT_INITIALIZED)
+        {
+            vCurrentKeys = mvCurrentKeys;
+            vIniKeys = mvIniKeys;
+            vMatches = mvIniMatches;
+            vTracks = mvTracks;
+        }
+        else if(mState==Tracking::OK)
+        {
+            vCurrentKeys = mvCurrentKeys;
+            vbVO = mvbVO;
+            vbMap = mvbMap;
+
+            currentFrame = mCurrentFrame;
+            vpLocalMap = mvpLocalMap;
+            vMatchesKeys = mvMatchedKeys;
+            vpMatchedMPs = mvpMatchedMPs;
+            vOutlierKeys = mvOutlierKeys;
+            vpOutlierMPs = mvpOutlierMPs;
+            mProjectPoints = mmProjectPoints;
+            mMatchedInImage = mmMatchedInImage;
+
+        }
+        else if(mState==Tracking::LOST)
+        {
+            vCurrentKeys = mvCurrentKeys;
+        }
+    }
+
+    if(im.channels()<3) //this should be always true
+        cvtColor(im,im,cv::COLOR_GRAY2BGR);
+
+    //Draw
+    if(state==Tracking::NOT_INITIALIZED)
+    {
+        for(unsigned int i=0; i<vMatches.size(); i++)
+        {
+            if(vMatches[i]>=0)
+            {
+                cv::line(im,vIniKeys[i].pt,vCurrentKeys[vMatches[i]].pt,
+                        cv::Scalar(0,255,0));
+            }
+        }
+        for(vector<pair<cv::Point2f, cv::Point2f> >::iterator it=vTracks.begin(); it!=vTracks.end(); it++)
+            cv::line(im,(*it).first,(*it).second, cv::Scalar(0,255,0),5);
+
+    }
+    else if(state==Tracking::OK && bOldFeatures) //TRACKING
+    {
+        mnTracked=0;
+        mnTrackedVO=0;
+        const float r = 5;
+        int n = vCurrentKeys.size();
+        for(int i=0;i<n;i++)
+        {
+            if(vbVO[i] || vbMap[i])
+            {
+                cv::Point2f pt1,pt2;
+                pt1.x=vCurrentKeys[i].pt.x-r;
+                pt1.y=vCurrentKeys[i].pt.y-r;
+                pt2.x=vCurrentKeys[i].pt.x+r;
+                pt2.y=vCurrentKeys[i].pt.y+r;
+
+                // This is a match to a MapPoint in the map
+                if(vbMap[i])
+                {
+                    cv::rectangle(im,pt1,pt2,cv::Scalar(0,255,0));
+                    cv::circle(im,vCurrentKeys[i].pt,2,cv::Scalar(0,255,0),-1);
+                    mnTracked++;
+                }
+                else // This is match to a "visual odometry" MapPoint created in the last frame
+                {
+                    cv::rectangle(im,pt1,pt2,cv::Scalar(255,0,0));
+                    cv::circle(im,vCurrentKeys[i].pt,2,cv::Scalar(255,0,0),-1);
+                    mnTrackedVO++;
+                }
+            }
+
+        }
+    }
+    else if(state==Tracking::OK && !bOldFeatures)
+    {
+        mnTracked=0;
+        int nTracked2 = 0;
+        mnTrackedVO=0;
+        int n = vCurrentKeys.size();
+
+        for(int i=0; i < n; ++i)
+        {
+
+            // This is a match to a MapPoint in the map
+            if(vbMap[i])
+            {
+                mnTracked++;
+            }
+        }
+
+        map<long unsigned int, cv::Point2f>::iterator it_match = mMatchedInImage.begin();
+        while(it_match != mMatchedInImage.end())
+        {
+            long unsigned int mp_id = it_match->first;
+            cv::Point2f p_image = it_match->second;
+
+            if(mProjectPoints.find(mp_id) != mProjectPoints.end())
+            {
+                cv::Point2f p_proj = mMatchedInImage[mp_id];
+                cv::line(im, p_proj, p_image, cv::Scalar(0, 255, 0), 2);
+                nTracked2++;
+            }
+            else
+            {
+                cv::circle(im,p_image,2,cv::Scalar(0,0,255),-1);
+            }
+
+
+            it_match++;
+        }
+
+        n = vOutlierKeys.size();
+        for(int i=0; i < n; ++i)
+        {
+            cv::Point2f point3d_proy;
+            float u, v;
+            currentFrame.ProjectPointDistort(vpOutlierMPs[i] , point3d_proy, u, v);
+
+            cv::Point2f point_im = vOutlierKeys[i].pt;
+
+            cv::line(im,cv::Point2f(u, v), point_im,cv::Scalar(0, 0, 255), 1);
+        }
+
+    }
+
+    cv::Mat imWithInfo;
+    DrawTextInfo(im,state, imWithInfo);
+
+    return imWithInfo;
+}
+
+cv::Mat FrameDrawer::DrawRightFrame()
+{
+    cv::Mat im;
+    vector<cv::KeyPoint> vIniKeys; // Initialization: KeyPoints in reference frame
+    vector<int> vMatches; // Initialization: correspondeces with reference keypoints
+    vector<cv::KeyPoint> vCurrentKeys; // KeyPoints in current frame
+    vector<bool> vbVO, vbMap; // Tracked MapPoints in current frame
+    int state; // Tracking state
+
+    //Copy variables within scoped mutex
+    {
+        unique_lock<mutex> lock(mMutex);
+        state=mState;
+        if(mState==Tracking::SYSTEM_NOT_READY)
+            mState=Tracking::NO_IMAGES_YET;
+
+        mImRight.copyTo(im);
+
+        if(mState==Tracking::NOT_INITIALIZED)
+        {
+            vCurrentKeys = mvCurrentKeysRight;
+            vIniKeys = mvIniKeys;
+            vMatches = mvIniMatches;
+        }
+        else if(mState==Tracking::OK)
+        {
+            vCurrentKeys = mvCurrentKeysRight;
+            vbVO = mvbVO;
+            vbMap = mvbMap;
+        }
+        else if(mState==Tracking::LOST)
+        {
+            vCurrentKeys = mvCurrentKeysRight;
+        }
+    } // destroy scoped mutex -> release mutex
+
+    if(im.channels()<3) //this should be always true
+        cvtColor(im,im,cv::COLOR_GRAY2BGR);
+
+    //Draw
+    if(state==Tracking::NOT_INITIALIZED) //INITIALIZING
+    {
+        for(unsigned int i=0; i<vMatches.size(); i++)
+        {
+            if(vMatches[i]>=0)
+            {
+                cv::line(im,vIniKeys[i].pt,vCurrentKeys[vMatches[i]].pt,
+                         cv::Scalar(0,255,0));
+            }
+        }
+    }
+    else if(state==Tracking::OK) //TRACKING
+    {
+        mnTracked=0;
+        mnTrackedVO=0;
+        const float r = 5;
+        const int n = mvCurrentKeysRight.size();
+        const int Nleft = mvCurrentKeys.size();
+
+        for(int i=0;i<n;i++)
+        {
+            if(vbVO[i + Nleft] || vbMap[i + Nleft])
+            {
+                cv::Point2f pt1,pt2;
+                pt1.x=mvCurrentKeysRight[i].pt.x-r;
+                pt1.y=mvCurrentKeysRight[i].pt.y-r;
+                pt2.x=mvCurrentKeysRight[i].pt.x+r;
+                pt2.y=mvCurrentKeysRight[i].pt.y+r;
+
+                // This is a match to a MapPoint in the map
+                if(vbMap[i + Nleft])
+                {
+                    cv::rectangle(im,pt1,pt2,cv::Scalar(0,255,0));
+                    cv::circle(im,mvCurrentKeysRight[i].pt,2,cv::Scalar(0,255,0),-1);
+                    mnTracked++;
+                }
+                else // This is match to a "visual odometry" MapPoint created in the last frame
+                {
+                    cv::rectangle(im,pt1,pt2,cv::Scalar(255,0,0));
+                    cv::circle(im,mvCurrentKeysRight[i].pt,2,cv::Scalar(255,0,0),-1);
+                    mnTrackedVO++;
+                }
+            }
+        }
+    }
+
+    cv::Mat imWithInfo;
+    DrawTextInfo(im,state, imWithInfo);
+
+    return imWithInfo;
+}
+
+
+
+void FrameDrawer::DrawTextInfo(cv::Mat &im, int nState, cv::Mat &imText)
+{
+    stringstream s;
+    if(nState==Tracking::NO_IMAGES_YET)
+        s << " WAITING FOR IMAGES";
+    else if(nState==Tracking::NOT_INITIALIZED)
+        s << " TRYING TO INITIALIZE ";
+    else if(nState==Tracking::OK)
+    {
+        if(!mbOnlyTracking)
+            s << "SLAM MODE |  ";
+        else
+            s << "LOCALIZATION | ";
+        int nMaps = mpAtlas->CountMaps();
+        int nKFs = mpAtlas->KeyFramesInMap();
+        int nMPs = mpAtlas->MapPointsInMap();
+        s << "Maps: " << nMaps << ", KFs: " << nKFs << ", MPs: " << nMPs << ", Matches: " << mnTracked;
+        if(mnTrackedVO>0)
+            s << ", + VO matches: " << mnTrackedVO;
+    }
+    else if(nState==Tracking::LOST)
+    {
+        s << " TRACK LOST. TRYING TO RELOCALIZE ";
+    }
+    else if(nState==Tracking::SYSTEM_NOT_READY)
+    {
+        s << " LOADING ORB VOCABULARY. PLEASE WAIT...";
+    }
+
+    int baseline=0;
+    cv::Size textSize = cv::getTextSize(s.str(),cv::FONT_HERSHEY_PLAIN,1,1,&baseline);
+
+    imText = cv::Mat(im.rows+textSize.height+10,im.cols,im.type());
+    im.copyTo(imText.rowRange(0,im.rows).colRange(0,im.cols));
+    imText.rowRange(im.rows,imText.rows) = cv::Mat::zeros(textSize.height+10,im.cols,im.type());
+    cv::putText(imText,s.str(),cv::Point(5,imText.rows-5),cv::FONT_HERSHEY_PLAIN,1,cv::Scalar(255,255,255),1,8);
+
+}
+
+void FrameDrawer::Update(Tracking *pTracker)
+{
+    unique_lock<mutex> lock(mMutex);
+    pTracker->mImGray.copyTo(mIm);
+    mvCurrentKeys=pTracker->mCurrentFrame.mvKeys;
+
+    if(both){
+        mvCurrentKeysRight = pTracker->mCurrentFrame.mvKeysRight;
+        pTracker->mImRight.copyTo(mImRight);
+        N = mvCurrentKeys.size() + mvCurrentKeysRight.size();
+    }
+    else{
+        N = mvCurrentKeys.size();
+    }
+
+    mvbVO = vector<bool>(N,false);
+    mvbMap = vector<bool>(N,false);
+    mbOnlyTracking = pTracker->mbOnlyTracking;
+
+    //Variables for the new visualization
+    mCurrentFrame = pTracker->mCurrentFrame;
+    mmProjectPoints = mCurrentFrame.mmProjectPoints;
+    mmMatchedInImage.clear();
+
+    mvpLocalMap = pTracker->GetLocalMapMPS();
+    mvMatchedKeys.clear();
+    mvMatchedKeys.reserve(N);
+    mvpMatchedMPs.clear();
+    mvpMatchedMPs.reserve(N);
+    mvOutlierKeys.clear();
+    mvOutlierKeys.reserve(N);
+    mvpOutlierMPs.clear();
+    mvpOutlierMPs.reserve(N);
+
+    if(pTracker->mLastProcessedState==Tracking::NOT_INITIALIZED)
+    {
+        mvIniKeys=pTracker->mInitialFrame.mvKeys;
+        mvIniMatches=pTracker->mvIniMatches;
+    }
+    else if(pTracker->mLastProcessedState==Tracking::OK)
+    {
+        for(int i=0;i<N;i++)
+        {
+            MapPoint* pMP = pTracker->mCurrentFrame.mvpMapPoints[i];
+            if(pMP)
+            {
+                if(!pTracker->mCurrentFrame.mvbOutlier[i])
+                {
+                    if(pMP->Observations()>0)
+                        mvbMap[i]=true;
+                    else
+                        mvbVO[i]=true;
+
+                    mmMatchedInImage[pMP->mnId] = mvCurrentKeys[i].pt;
+
+                }
+                else
+                {
+                    mvpOutlierMPs.push_back(pMP);
+                    mvOutlierKeys.push_back(mvCurrentKeys[i]);
+                }
+            }
+        }
+
+    }
+    mState=static_cast<int>(pTracker->mLastProcessedState);
+}
+
+} //namespace ORB_SLAM

src/G2oTypes.cc

@@ -0,0 +1,874 @@
+/**
+* This file is part of ORB-SLAM3
+*
+* Copyright (C) 2017-2020 Carlos Campos, Richard Elvira, Juan J. Gómez Rodríguez, José M.M. Montiel and Juan D. Tardós, University of Zaragoza.
+* Copyright (C) 2014-2016 Raúl Mur-Artal, José M.M. Montiel and Juan D. Tardós, University of Zaragoza.
+*
+* ORB-SLAM3 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-SLAM3 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-SLAM3.
+* If not, see <http://www.gnu.org/licenses/>.
+*/
+
+#include "G2oTypes.h"
+#include "ImuTypes.h"
+#include "Converter.h"
+namespace ORB_SLAM3
+{
+
+ImuCamPose::ImuCamPose(KeyFrame *pKF):its(0)
+{
+    // Load IMU pose
+    twb = Converter::toVector3d(pKF->GetImuPosition());
+    Rwb = Converter::toMatrix3d(pKF->GetImuRotation());
+
+    // Load camera poses
+    int num_cams;
+    if(pKF->mpCamera2)
+        num_cams=2;
+    else
+        num_cams=1;
+
+    tcw.resize(num_cams);
+    Rcw.resize(num_cams);
+    tcb.resize(num_cams);
+    Rcb.resize(num_cams);
+    Rbc.resize(num_cams);
+    tbc.resize(num_cams);
+    pCamera.resize(num_cams);
+
+    // Left camera
+    tcw[0] = Converter::toVector3d(pKF->GetTranslation());
+    Rcw[0] = Converter::toMatrix3d(pKF->GetRotation());
+    tcb[0] = Converter::toVector3d(pKF->mImuCalib.Tcb.rowRange(0,3).col(3));
+    Rcb[0] = Converter::toMatrix3d(pKF->mImuCalib.Tcb.rowRange(0,3).colRange(0,3));
+    Rbc[0] = Rcb[0].transpose();
+    tbc[0] = Converter::toVector3d(pKF->mImuCalib.Tbc.rowRange(0,3).col(3));
+    pCamera[0] = pKF->mpCamera;
+    bf = pKF->mbf;
+
+    if(num_cams>1)
+    {
+        Eigen::Matrix4d Trl = Converter::toMatrix4d(pKF->mTrl);
+        Rcw[1] = Trl.block<3,3>(0,0)*Rcw[0];
+        tcw[1] = Trl.block<3,3>(0,0)*tcw[0]+Trl.block<3,1>(0,3);
+        tcb[1] = Trl.block<3,3>(0,0)*tcb[0]+Trl.block<3,1>(0,3);
+        Rcb[1] = Trl.block<3,3>(0,0)*Rcb[0];
+        Rbc[1] = Rcb[1].transpose();
+        tbc[1] = -Rbc[1]*tcb[1];
+        pCamera[1] = pKF->mpCamera2;
+    }
+
+    // For posegraph 4DoF
+    Rwb0 = Rwb;
+    DR.setIdentity();
+}
+
+ImuCamPose::ImuCamPose(Frame *pF):its(0)
+{
+    // Load IMU pose
+    twb = Converter::toVector3d(pF->GetImuPosition());
+    Rwb = Converter::toMatrix3d(pF->GetImuRotation());
+
+    // Load camera poses
+    int num_cams;
+    if(pF->mpCamera2)
+        num_cams=2;
+    else
+        num_cams=1;
+
+    tcw.resize(num_cams);
+    Rcw.resize(num_cams);
+    tcb.resize(num_cams);
+    Rcb.resize(num_cams);
+    Rbc.resize(num_cams);
+    tbc.resize(num_cams);
+    pCamera.resize(num_cams);
+
+    // Left camera
+    tcw[0] = Converter::toVector3d(pF->mTcw.rowRange(0,3).col(3));
+    Rcw[0] = Converter::toMatrix3d(pF->mTcw.rowRange(0,3).colRange(0,3));
+    tcb[0] = Converter::toVector3d(pF->mImuCalib.Tcb.rowRange(0,3).col(3));
+    Rcb[0] = Converter::toMatrix3d(pF->mImuCalib.Tcb.rowRange(0,3).colRange(0,3));
+    Rbc[0] = Rcb[0].transpose();
+    tbc[0] = Converter::toVector3d(pF->mImuCalib.Tbc.rowRange(0,3).col(3));
+    pCamera[0] = pF->mpCamera;
+    bf = pF->mbf;
+
+    if(num_cams>1)
+    {
+        Eigen::Matrix4d Trl = Converter::toMatrix4d(pF->mTrl);
+        Rcw[1] = Trl.block<3,3>(0,0)*Rcw[0];
+        tcw[1] = Trl.block<3,3>(0,0)*tcw[0]+Trl.block<3,1>(0,3);
+        tcb[1] = Trl.block<3,3>(0,0)*tcb[0]+Trl.block<3,1>(0,3);
+        Rcb[1] = Trl.block<3,3>(0,0)*Rcb[0];
+        Rbc[1] = Rcb[1].transpose();
+        tbc[1] = -Rbc[1]*tcb[1];
+        pCamera[1] = pF->mpCamera2;
+    }
+
+    // For posegraph 4DoF
+    Rwb0 = Rwb;
+    DR.setIdentity();
+}
+
+ImuCamPose::ImuCamPose(Eigen::Matrix3d &_Rwc, Eigen::Vector3d &_twc, KeyFrame* pKF): its(0)
+{
+    // This is only for posegrpah, we do not care about multicamera
+    tcw.resize(1);
+    Rcw.resize(1);
+    tcb.resize(1);
+    Rcb.resize(1);
+    Rbc.resize(1);
+    tbc.resize(1);
+    pCamera.resize(1);
+
+    tcb[0] = Converter::toVector3d(pKF->mImuCalib.Tcb.rowRange(0,3).col(3));
+    Rcb[0] = Converter::toMatrix3d(pKF->mImuCalib.Tcb.rowRange(0,3).colRange(0,3));
+    Rbc[0] = Rcb[0].transpose();
+    tbc[0] = Converter::toVector3d(pKF->mImuCalib.Tbc.rowRange(0,3).col(3));
+    twb = _Rwc*tcb[0]+_twc;
+    Rwb = _Rwc*Rcb[0];
+    Rcw[0] = _Rwc.transpose();
+    tcw[0] = -Rcw[0]*_twc;
+    pCamera[0] = pKF->mpCamera;
+    bf = pKF->mbf;
+
+    // For posegraph 4DoF
+    Rwb0 = Rwb;
+    DR.setIdentity();
+}
+
+void ImuCamPose::SetParam(const std::vector<Eigen::Matrix3d> &_Rcw, const std::vector<Eigen::Vector3d> &_tcw, const std::vector<Eigen::Matrix3d> &_Rbc,
+              const std::vector<Eigen::Vector3d> &_tbc, const double &_bf)
+{
+    Rbc = _Rbc;
+    tbc = _tbc;
+    Rcw = _Rcw;
+    tcw = _tcw;
+    const int num_cams = Rbc.size();
+    Rcb.resize(num_cams);
+    tcb.resize(num_cams);
+
+    for(int i=0; i<tcb.size(); i++)
+    {
+        Rcb[i] = Rbc[i].transpose();
+        tcb[i] = -Rcb[i]*tbc[i];
+    }
+    Rwb = Rcw[0].transpose()*Rcb[0];
+    twb = Rcw[0].transpose()*(tcb[0]-tcw[0]);
+
+    bf = _bf;
+}
+
+Eigen::Vector2d ImuCamPose::Project(const Eigen::Vector3d &Xw, int cam_idx) const
+{
+    Eigen::Vector3d Xc = Rcw[cam_idx]*Xw+tcw[cam_idx];
+
+    return pCamera[cam_idx]->project(Xc);
+}
+
+Eigen::Vector3d ImuCamPose::ProjectStereo(const Eigen::Vector3d &Xw, int cam_idx) const
+{
+    Eigen::Vector3d Pc = Rcw[cam_idx]*Xw+tcw[cam_idx];
+    Eigen::Vector3d pc;
+    double invZ = 1/Pc(2);
+    pc.head(2) = pCamera[cam_idx]->project(Pc);
+    pc(2) = pc(0) - bf*invZ;
+    return pc;
+}
+
+bool ImuCamPose::isDepthPositive(const Eigen::Vector3d &Xw, int cam_idx) const
+{
+    return (Rcw[cam_idx].row(2)*Xw+tcw[cam_idx](2))>0.0;
+}
+
+void ImuCamPose::Update(const double *pu)
+{
+    Eigen::Vector3d ur, ut;
+    ur << pu[0], pu[1], pu[2];
+    ut << pu[3], pu[4], pu[5];
+
+    // Update body pose
+    twb += Rwb*ut;
+    Rwb = Rwb*ExpSO3(ur);
+
+    // Normalize rotation after 5 updates
+    its++;
+    if(its>=3)
+    {
+        NormalizeRotation(Rwb);
+        its=0;
+    }
+
+    // Update camera poses
+    const Eigen::Matrix3d Rbw = Rwb.transpose();
+    const Eigen::Vector3d tbw = -Rbw*twb;
+
+    for(int i=0; i<pCamera.size(); i++)
+    {
+        Rcw[i] = Rcb[i]*Rbw;
+        tcw[i] = Rcb[i]*tbw+tcb[i];
+    }
+
+}
+
+void ImuCamPose::UpdateW(const double *pu)
+{
+    Eigen::Vector3d ur, ut;
+    ur << pu[0], pu[1], pu[2];
+    ut << pu[3], pu[4], pu[5];
+
+
+    const Eigen::Matrix3d dR = ExpSO3(ur);
+    DR = dR*DR;
+    Rwb = DR*Rwb0;
+    // Update body pose
+    twb += ut;
+
+    // Normalize rotation after 5 updates
+    its++;
+    if(its>=5)
+    {
+        DR(0,2)=0.0;
+        DR(1,2)=0.0;
+        DR(2,0)=0.0;
+        DR(2,1)=0.0;
+        NormalizeRotation(DR);
+        its=0;
+    }
+
+    // Update camera pose
+    const Eigen::Matrix3d Rbw = Rwb.transpose();
+    const Eigen::Vector3d tbw = -Rbw*twb;
+
+    for(int i=0; i<pCamera.size(); i++)
+    {
+        Rcw[i] = Rcb[i]*Rbw;
+        tcw[i] = Rcb[i]*tbw+tcb[i];
+    }
+}
+
+InvDepthPoint::InvDepthPoint(double _rho, double _u, double _v, KeyFrame* pHostKF): u(_u), v(_v), rho(_rho),
+    fx(pHostKF->fx), fy(pHostKF->fy), cx(pHostKF->cx), cy(pHostKF->cy), bf(pHostKF->mbf)
+{
+}
+
+void InvDepthPoint::Update(const double *pu)
+{
+    rho += *pu;
+}
+
+
+bool VertexPose::read(std::istream& is)
+{
+    std::vector<Eigen::Matrix<double,3,3> > Rcw;
+    std::vector<Eigen::Matrix<double,3,1> > tcw;
+    std::vector<Eigen::Matrix<double,3,3> > Rbc;
+    std::vector<Eigen::Matrix<double,3,1> > tbc;
+
+    const int num_cams = _estimate.Rbc.size();
+    for(int idx = 0; idx<num_cams; idx++)
+    {
+        for (int i=0; i<3; i++){
+            for (int j=0; j<3; j++)
+                is >> Rcw[idx](i,j);
+        }
+        for (int i=0; i<3; i++){
+            is >> tcw[idx](i);
+        }
+
+        for (int i=0; i<3; i++){
+            for (int j=0; j<3; j++)
+                is >> Rbc[idx](i,j);
+        }
+        for (int i=0; i<3; i++){
+            is >> tbc[idx](i);
+        }
+
+        float nextParam;
+        for(size_t i = 0; i < _estimate.pCamera[idx]->size(); i++){
+            is >> nextParam;
+            _estimate.pCamera[idx]->setParameter(nextParam,i);
+        }
+    }
+
+    double bf;
+    is >> bf;
+    _estimate.SetParam(Rcw,tcw,Rbc,tbc,bf);
+    updateCache();
+    
+    return true;
+}
+
+bool VertexPose::write(std::ostream& os) const
+{
+    std::vector<Eigen::Matrix<double,3,3> > Rcw = _estimate.Rcw;
+    std::vector<Eigen::Matrix<double,3,1> > tcw = _estimate.tcw;
+
+    std::vector<Eigen::Matrix<double,3,3> > Rbc = _estimate.Rbc;
+    std::vector<Eigen::Matrix<double,3,1> > tbc = _estimate.tbc;
+
+    const int num_cams = tcw.size();
+
+    for(int idx = 0; idx<num_cams; idx++)
+    {
+        for (int i=0; i<3; i++){
+            for (int j=0; j<3; j++)
+                os << Rcw[idx](i,j) << " ";
+        }
+        for (int i=0; i<3; i++){
+            os << tcw[idx](i) << " ";
+        }
+
+        for (int i=0; i<3; i++){
+            for (int j=0; j<3; j++)
+                os << Rbc[idx](i,j) << " ";
+        }
+        for (int i=0; i<3; i++){
+            os << tbc[idx](i) << " ";
+        }
+
+        for(size_t i = 0; i < _estimate.pCamera[idx]->size(); i++){
+            os << _estimate.pCamera[idx]->getParameter(i) << " ";
+        }
+    }
+
+    os << _estimate.bf << " ";
+
+    return os.good();
+}
+
+
+void EdgeMono::linearizeOplus()
+{
+    const VertexPose* VPose = static_cast<const VertexPose*>(_vertices[1]);
+    const g2o::VertexSBAPointXYZ* VPoint = static_cast<const g2o::VertexSBAPointXYZ*>(_vertices[0]);
+
+    const Eigen::Matrix3d &Rcw = VPose->estimate().Rcw[cam_idx];
+    const Eigen::Vector3d &tcw = VPose->estimate().tcw[cam_idx];
+    const Eigen::Vector3d Xc = Rcw*VPoint->estimate() + tcw;
+    const Eigen::Vector3d Xb = VPose->estimate().Rbc[cam_idx]*Xc+VPose->estimate().tbc[cam_idx];
+    const Eigen::Matrix3d &Rcb = VPose->estimate().Rcb[cam_idx];
+
+    const Eigen::Matrix<double,2,3> proj_jac = VPose->estimate().pCamera[cam_idx]->projectJac(Xc);
+    _jacobianOplusXi = -proj_jac * Rcw;
+
+    Eigen::Matrix<double,3,6> SE3deriv;
+    double x = Xb(0);
+    double y = Xb(1);
+    double z = Xb(2);
+
+    SE3deriv << 0.0, z,   -y, 1.0, 0.0, 0.0,
+            -z , 0.0, x, 0.0, 1.0, 0.0,
+            y ,  -x , 0.0, 0.0, 0.0, 1.0;
+
+    _jacobianOplusXj = proj_jac * Rcb * SE3deriv; // TODO optimize this product
+}
+
+void EdgeMonoOnlyPose::linearizeOplus()
+{
+    const VertexPose* VPose = static_cast<const VertexPose*>(_vertices[0]);
+
+    const Eigen::Matrix3d &Rcw = VPose->estimate().Rcw[cam_idx];
+    const Eigen::Vector3d &tcw = VPose->estimate().tcw[cam_idx];
+    const Eigen::Vector3d Xc = Rcw*Xw + tcw;
+    const Eigen::Vector3d Xb = VPose->estimate().Rbc[cam_idx]*Xc+VPose->estimate().tbc[cam_idx];
+    const Eigen::Matrix3d &Rcb = VPose->estimate().Rcb[cam_idx];
+
+    Eigen::Matrix<double,2,3> proj_jac = VPose->estimate().pCamera[cam_idx]->projectJac(Xc);
+
+    Eigen::Matrix<double,3,6> SE3deriv;
+    double x = Xb(0);
+    double y = Xb(1);
+    double z = Xb(2);
+    SE3deriv << 0.0, z,   -y, 1.0, 0.0, 0.0,
+            -z , 0.0, x, 0.0, 1.0, 0.0,
+            y ,  -x , 0.0, 0.0, 0.0, 1.0;
+    _jacobianOplusXi = proj_jac * Rcb * SE3deriv; // symbol different becasue of update mode
+}
+
+void EdgeStereo::linearizeOplus()
+{
+    const VertexPose* VPose = static_cast<const VertexPose*>(_vertices[1]);
+    const g2o::VertexSBAPointXYZ* VPoint = static_cast<const g2o::VertexSBAPointXYZ*>(_vertices[0]);
+
+    const Eigen::Matrix3d &Rcw = VPose->estimate().Rcw[cam_idx];
+    const Eigen::Vector3d &tcw = VPose->estimate().tcw[cam_idx];
+    const Eigen::Vector3d Xc = Rcw*VPoint->estimate() + tcw;
+    const Eigen::Vector3d Xb = VPose->estimate().Rbc[cam_idx]*Xc+VPose->estimate().tbc[cam_idx];
+    const Eigen::Matrix3d &Rcb = VPose->estimate().Rcb[cam_idx];
+    const double bf = VPose->estimate().bf;
+    const double inv_z2 = 1.0/(Xc(2)*Xc(2));
+
+    Eigen::Matrix<double,3,3> proj_jac;
+    proj_jac.block<2,3>(0,0) = VPose->estimate().pCamera[cam_idx]->projectJac(Xc);
+    proj_jac.block<1,3>(2,0) = proj_jac.block<1,3>(0,0);
+    proj_jac(2,2) += bf*inv_z2;
+
+    _jacobianOplusXi = -proj_jac * Rcw;
+
+    Eigen::Matrix<double,3,6> SE3deriv;
+    double x = Xb(0);
+    double y = Xb(1);
+    double z = Xb(2);
+
+    SE3deriv << 0.0, z,   -y, 1.0, 0.0, 0.0,
+            -z , 0.0, x, 0.0, 1.0, 0.0,
+            y ,  -x , 0.0, 0.0, 0.0, 1.0;
+
+    _jacobianOplusXj = proj_jac * Rcb * SE3deriv;
+}
+
+void EdgeStereoOnlyPose::linearizeOplus()
+{
+    const VertexPose* VPose = static_cast<const VertexPose*>(_vertices[0]);
+
+    const Eigen::Matrix3d &Rcw = VPose->estimate().Rcw[cam_idx];
+    const Eigen::Vector3d &tcw = VPose->estimate().tcw[cam_idx];
+    const Eigen::Vector3d Xc = Rcw*Xw + tcw;
+    const Eigen::Vector3d Xb = VPose->estimate().Rbc[cam_idx]*Xc+VPose->estimate().tbc[cam_idx];
+    const Eigen::Matrix3d &Rcb = VPose->estimate().Rcb[cam_idx];
+    const double bf = VPose->estimate().bf;
+    const double inv_z2 = 1.0/(Xc(2)*Xc(2));
+
+    Eigen::Matrix<double,3,3> proj_jac;
+    proj_jac.block<2,3>(0,0) = VPose->estimate().pCamera[cam_idx]->projectJac(Xc);
+    proj_jac.block<1,3>(2,0) = proj_jac.block<1,3>(0,0);
+    proj_jac(2,2) += bf*inv_z2;
+
+    Eigen::Matrix<double,3,6> SE3deriv;
+    double x = Xb(0);
+    double y = Xb(1);
+    double z = Xb(2);
+    SE3deriv << 0.0, z,   -y, 1.0, 0.0, 0.0,
+            -z , 0.0, x, 0.0, 1.0, 0.0,
+            y ,  -x , 0.0, 0.0, 0.0, 1.0;
+    _jacobianOplusXi = proj_jac * Rcb * SE3deriv;
+}
+
+VertexVelocity::VertexVelocity(KeyFrame* pKF)
+{
+    setEstimate(Converter::toVector3d(pKF->GetVelocity()));
+}
+
+VertexVelocity::VertexVelocity(Frame* pF)
+{
+    setEstimate(Converter::toVector3d(pF->mVw));
+}
+
+VertexGyroBias::VertexGyroBias(KeyFrame *pKF)
+{
+    setEstimate(Converter::toVector3d(pKF->GetGyroBias()));
+}
+
+VertexGyroBias::VertexGyroBias(Frame *pF)
+{
+    Eigen::Vector3d bg;
+    bg << pF->mImuBias.bwx, pF->mImuBias.bwy,pF->mImuBias.bwz;
+    setEstimate(bg);
+}
+
+VertexAccBias::VertexAccBias(KeyFrame *pKF)
+{
+    setEstimate(Converter::toVector3d(pKF->GetAccBias()));
+}
+
+VertexAccBias::VertexAccBias(Frame *pF)
+{
+    Eigen::Vector3d ba;
+    ba << pF->mImuBias.bax, pF->mImuBias.bay,pF->mImuBias.baz;
+    setEstimate(ba);
+}
+
+
+
+EdgeInertial::EdgeInertial(IMU::Preintegrated *pInt):JRg(Converter::toMatrix3d(pInt->JRg)),
+    JVg(Converter::toMatrix3d(pInt->JVg)), JPg(Converter::toMatrix3d(pInt->JPg)), JVa(Converter::toMatrix3d(pInt->JVa)),
+    JPa(Converter::toMatrix3d(pInt->JPa)), mpInt(pInt), dt(pInt->dT)
+{
+    // This edge links 6 vertices
+    resize(6);
+    g << 0, 0, -IMU::GRAVITY_VALUE;
+    cv::Mat cvInfo = pInt->C.rowRange(0,9).colRange(0,9).inv(cv::DECOMP_SVD);
+    Matrix9d Info;
+    for(int r=0;r<9;r++)
+        for(int c=0;c<9;c++)
+            Info(r,c)=cvInfo.at<float>(r,c);
+    Info = (Info+Info.transpose())/2;
+    Eigen::SelfAdjointEigenSolver<Eigen::Matrix<double,9,9> > es(Info);
+     Eigen::Matrix<double,9,1> eigs = es.eigenvalues();
+     for(int i=0;i<9;i++)
+         if(eigs[i]<1e-12)
+             eigs[i]=0;
+    Info = es.eigenvectors()*eigs.asDiagonal()*es.eigenvectors().transpose();
+    setInformation(Info);
+}
+
+
+
+
+void EdgeInertial::computeError()
+{
+    // TODO Maybe Reintegrate inertial measurments when difference between linearization point and current estimate is too big
+    const VertexPose* VP1 = static_cast<const VertexPose*>(_vertices[0]);
+    const VertexVelocity* VV1= static_cast<const VertexVelocity*>(_vertices[1]);
+    const VertexGyroBias* VG1= static_cast<const VertexGyroBias*>(_vertices[2]);
+    const VertexAccBias* VA1= static_cast<const VertexAccBias*>(_vertices[3]);
+    const VertexPose* VP2 = static_cast<const VertexPose*>(_vertices[4]);
+    const VertexVelocity* VV2 = static_cast<const VertexVelocity*>(_vertices[5]);
+    const IMU::Bias b1(VA1->estimate()[0],VA1->estimate()[1],VA1->estimate()[2],VG1->estimate()[0],VG1->estimate()[1],VG1->estimate()[2]);
+    const Eigen::Matrix3d dR = Converter::toMatrix3d(mpInt->GetDeltaRotation(b1));
+    const Eigen::Vector3d dV = Converter::toVector3d(mpInt->GetDeltaVelocity(b1));
+    const Eigen::Vector3d dP = Converter::toVector3d(mpInt->GetDeltaPosition(b1));
+
+    const Eigen::Vector3d er = LogSO3(dR.transpose()*VP1->estimate().Rwb.transpose()*VP2->estimate().Rwb);
+    const Eigen::Vector3d ev = VP1->estimate().Rwb.transpose()*(VV2->estimate() - VV1->estimate() - g*dt) - dV;
+    const Eigen::Vector3d ep = VP1->estimate().Rwb.transpose()*(VP2->estimate().twb - VP1->estimate().twb
+                                                               - VV1->estimate()*dt - g*dt*dt/2) - dP;
+
+    _error << er, ev, ep;
+}
+
+void EdgeInertial::linearizeOplus()
+{
+    const VertexPose* VP1 = static_cast<const VertexPose*>(_vertices[0]);
+    const VertexVelocity* VV1= static_cast<const VertexVelocity*>(_vertices[1]);
+    const VertexGyroBias* VG1= static_cast<const VertexGyroBias*>(_vertices[2]);
+    const VertexAccBias* VA1= static_cast<const VertexAccBias*>(_vertices[3]);
+    const VertexPose* VP2 = static_cast<const VertexPose*>(_vertices[4]);
+    const VertexVelocity* VV2= static_cast<const VertexVelocity*>(_vertices[5]);
+    const IMU::Bias b1(VA1->estimate()[0],VA1->estimate()[1],VA1->estimate()[2],VG1->estimate()[0],VG1->estimate()[1],VG1->estimate()[2]);
+    const IMU::Bias db = mpInt->GetDeltaBias(b1);
+    Eigen::Vector3d dbg;
+    dbg << db.bwx, db.bwy, db.bwz;
+
+    const Eigen::Matrix3d Rwb1 = VP1->estimate().Rwb;
+    const Eigen::Matrix3d Rbw1 = Rwb1.transpose();
+    const Eigen::Matrix3d Rwb2 = VP2->estimate().Rwb;
+
+    const Eigen::Matrix3d dR = Converter::toMatrix3d(mpInt->GetDeltaRotation(b1));
+    const Eigen::Matrix3d eR = dR.transpose()*Rbw1*Rwb2;
+    const Eigen::Vector3d er = LogSO3(eR);
+    const Eigen::Matrix3d invJr = InverseRightJacobianSO3(er);
+
+    // Jacobians wrt Pose 1
+    _jacobianOplus[0].setZero();
+     // rotation
+    _jacobianOplus[0].block<3,3>(0,0) = -invJr*Rwb2.transpose()*Rwb1; // OK
+    _jacobianOplus[0].block<3,3>(3,0) = Skew(Rbw1*(VV2->estimate() - VV1->estimate() - g*dt)); // OK
+    _jacobianOplus[0].block<3,3>(6,0) = Skew(Rbw1*(VP2->estimate().twb - VP1->estimate().twb
+                                                   - VV1->estimate()*dt - 0.5*g*dt*dt)); // OK
+    // translation
+    _jacobianOplus[0].block<3,3>(6,3) = -Eigen::Matrix3d::Identity(); // OK
+
+    // Jacobians wrt Velocity 1
+    _jacobianOplus[1].setZero();
+    _jacobianOplus[1].block<3,3>(3,0) = -Rbw1; // OK
+    _jacobianOplus[1].block<3,3>(6,0) = -Rbw1*dt; // OK
+
+    // Jacobians wrt Gyro 1
+    _jacobianOplus[2].setZero();
+    _jacobianOplus[2].block<3,3>(0,0) = -invJr*eR.transpose()*RightJacobianSO3(JRg*dbg)*JRg; // OK
+    _jacobianOplus[2].block<3,3>(3,0) = -JVg; // OK
+    _jacobianOplus[2].block<3,3>(6,0) = -JPg; // OK
+
+    // Jacobians wrt Accelerometer 1
+    _jacobianOplus[3].setZero();
+    _jacobianOplus[3].block<3,3>(3,0) = -JVa; // OK
+    _jacobianOplus[3].block<3,3>(6,0) = -JPa; // OK
+
+    // Jacobians wrt Pose 2
+    _jacobianOplus[4].setZero();
+    // rotation
+    _jacobianOplus[4].block<3,3>(0,0) = invJr; // OK
+    // translation
+    _jacobianOplus[4].block<3,3>(6,3) = Rbw1*Rwb2; // OK
+
+    // Jacobians wrt Velocity 2
+    _jacobianOplus[5].setZero();
+    _jacobianOplus[5].block<3,3>(3,0) = Rbw1; // OK
+}
+
+EdgeInertialGS::EdgeInertialGS(IMU::Preintegrated *pInt):JRg(Converter::toMatrix3d(pInt->JRg)),
+    JVg(Converter::toMatrix3d(pInt->JVg)), JPg(Converter::toMatrix3d(pInt->JPg)), JVa(Converter::toMatrix3d(pInt->JVa)),
+    JPa(Converter::toMatrix3d(pInt->JPa)), mpInt(pInt), dt(pInt->dT)
+{
+    // This edge links 8 vertices
+    resize(8);
+    gI << 0, 0, -IMU::GRAVITY_VALUE;
+    cv::Mat cvInfo = pInt->C.rowRange(0,9).colRange(0,9).inv(cv::DECOMP_SVD);
+    Matrix9d Info;
+    for(int r=0;r<9;r++)
+        for(int c=0;c<9;c++)
+            Info(r,c)=cvInfo.at<float>(r,c);
+    Info = (Info+Info.transpose())/2;
+    Eigen::SelfAdjointEigenSolver<Eigen::Matrix<double,9,9> > es(Info);
+     Eigen::Matrix<double,9,1> eigs = es.eigenvalues();
+     for(int i=0;i<9;i++)
+         if(eigs[i]<1e-12)
+             eigs[i]=0;
+    Info = es.eigenvectors()*eigs.asDiagonal()*es.eigenvectors().transpose();
+    setInformation(Info);
+}
+
+
+
+void EdgeInertialGS::computeError()
+{
+    // TODO Maybe Reintegrate inertial measurments when difference between linearization point and current estimate is too big
+    const VertexPose* VP1 = static_cast<const VertexPose*>(_vertices[0]);
+    const VertexVelocity* VV1= static_cast<const VertexVelocity*>(_vertices[1]);
+    const VertexGyroBias* VG= static_cast<const VertexGyroBias*>(_vertices[2]);
+    const VertexAccBias* VA= static_cast<const VertexAccBias*>(_vertices[3]);
+    const VertexPose* VP2 = static_cast<const VertexPose*>(_vertices[4]);
+    const VertexVelocity* VV2 = static_cast<const VertexVelocity*>(_vertices[5]);
+    const VertexGDir* VGDir = static_cast<const VertexGDir*>(_vertices[6]);
+    const VertexScale* VS = static_cast<const VertexScale*>(_vertices[7]);
+    const IMU::Bias b(VA->estimate()[0],VA->estimate()[1],VA->estimate()[2],VG->estimate()[0],VG->estimate()[1],VG->estimate()[2]);
+    g = VGDir->estimate().Rwg*gI;
+    const double s = VS->estimate();
+    const Eigen::Matrix3d dR = Converter::toMatrix3d(mpInt->GetDeltaRotation(b));
+    const Eigen::Vector3d dV = Converter::toVector3d(mpInt->GetDeltaVelocity(b));
+    const Eigen::Vector3d dP = Converter::toVector3d(mpInt->GetDeltaPosition(b));
+
+    const Eigen::Vector3d er = LogSO3(dR.transpose()*VP1->estimate().Rwb.transpose()*VP2->estimate().Rwb);
+    const Eigen::Vector3d ev = VP1->estimate().Rwb.transpose()*(s*(VV2->estimate() - VV1->estimate()) - g*dt) - dV;
+    const Eigen::Vector3d ep = VP1->estimate().Rwb.transpose()*(s*(VP2->estimate().twb - VP1->estimate().twb - VV1->estimate()*dt) - g*dt*dt/2) - dP;
+
+    _error << er, ev, ep;
+}
+
+void EdgeInertialGS::linearizeOplus()
+{
+    const VertexPose* VP1 = static_cast<const VertexPose*>(_vertices[0]);
+    const VertexVelocity* VV1= static_cast<const VertexVelocity*>(_vertices[1]);
+    const VertexGyroBias* VG= static_cast<const VertexGyroBias*>(_vertices[2]);
+    const VertexAccBias* VA= static_cast<const VertexAccBias*>(_vertices[3]);
+    const VertexPose* VP2 = static_cast<const VertexPose*>(_vertices[4]);
+    const VertexVelocity* VV2 = static_cast<const VertexVelocity*>(_vertices[5]);
+    const VertexGDir* VGDir = static_cast<const VertexGDir*>(_vertices[6]);
+    const VertexScale* VS = static_cast<const VertexScale*>(_vertices[7]);
+    const IMU::Bias b(VA->estimate()[0],VA->estimate()[1],VA->estimate()[2],VG->estimate()[0],VG->estimate()[1],VG->estimate()[2]);
+    const IMU::Bias db = mpInt->GetDeltaBias(b);
+
+    Eigen::Vector3d dbg;
+    dbg << db.bwx, db.bwy, db.bwz;
+
+    const Eigen::Matrix3d Rwb1 = VP1->estimate().Rwb;
+    const Eigen::Matrix3d Rbw1 = Rwb1.transpose();
+    const Eigen::Matrix3d Rwb2 = VP2->estimate().Rwb;
+    const Eigen::Matrix3d Rwg = VGDir->estimate().Rwg;
+    Eigen::MatrixXd Gm = Eigen::MatrixXd::Zero(3,2);
+    Gm(0,1) = -IMU::GRAVITY_VALUE;
+    Gm(1,0) = IMU::GRAVITY_VALUE;
+    const double s = VS->estimate();
+    const Eigen::MatrixXd dGdTheta = Rwg*Gm;
+    const Eigen::Matrix3d dR = Converter::toMatrix3d(mpInt->GetDeltaRotation(b));
+    const Eigen::Matrix3d eR = dR.transpose()*Rbw1*Rwb2;
+    const Eigen::Vector3d er = LogSO3(eR);
+    const Eigen::Matrix3d invJr = InverseRightJacobianSO3(er);
+
+    // Jacobians wrt Pose 1
+    _jacobianOplus[0].setZero();
+     // rotation
+    _jacobianOplus[0].block<3,3>(0,0) = -invJr*Rwb2.transpose()*Rwb1;
+    _jacobianOplus[0].block<3,3>(3,0) = Skew(Rbw1*(s*(VV2->estimate() - VV1->estimate()) - g*dt));
+    _jacobianOplus[0].block<3,3>(6,0) = Skew(Rbw1*(s*(VP2->estimate().twb - VP1->estimate().twb
+                                                   - VV1->estimate()*dt) - 0.5*g*dt*dt));
+    // translation
+    _jacobianOplus[0].block<3,3>(6,3) = -s*Eigen::Matrix3d::Identity();
+
+    // Jacobians wrt Velocity 1
+    _jacobianOplus[1].setZero();
+    _jacobianOplus[1].block<3,3>(3,0) = -s*Rbw1;
+    _jacobianOplus[1].block<3,3>(6,0) = -s*Rbw1*dt;
+
+    // Jacobians wrt Gyro bias
+    _jacobianOplus[2].setZero();
+    _jacobianOplus[2].block<3,3>(0,0) = -invJr*eR.transpose()*RightJacobianSO3(JRg*dbg)*JRg;
+    _jacobianOplus[2].block<3,3>(3,0) = -JVg;
+    _jacobianOplus[2].block<3,3>(6,0) = -JPg;
+
+    // Jacobians wrt Accelerometer bias
+    _jacobianOplus[3].setZero();
+    _jacobianOplus[3].block<3,3>(3,0) = -JVa;
+    _jacobianOplus[3].block<3,3>(6,0) = -JPa;
+
+    // Jacobians wrt Pose 2
+    _jacobianOplus[4].setZero();
+    // rotation
+    _jacobianOplus[4].block<3,3>(0,0) = invJr;
+    // translation
+    _jacobianOplus[4].block<3,3>(6,3) = s*Rbw1*Rwb2;
+
+    // Jacobians wrt Velocity 2
+    _jacobianOplus[5].setZero();
+    _jacobianOplus[5].block<3,3>(3,0) = s*Rbw1;
+
+    // Jacobians wrt Gravity direction
+    _jacobianOplus[6].setZero();
+    _jacobianOplus[6].block<3,2>(3,0) = -Rbw1*dGdTheta*dt;
+    _jacobianOplus[6].block<3,2>(6,0) = -0.5*Rbw1*dGdTheta*dt*dt;
+
+    // Jacobians wrt scale factor
+    _jacobianOplus[7].setZero();
+    _jacobianOplus[7].block<3,1>(3,0) = Rbw1*(VV2->estimate()-VV1->estimate());
+    _jacobianOplus[7].block<3,1>(6,0) = Rbw1*(VP2->estimate().twb-VP1->estimate().twb-VV1->estimate()*dt);
+}
+
+EdgePriorPoseImu::EdgePriorPoseImu(ConstraintPoseImu *c)
+{
+    resize(4);
+    Rwb = c->Rwb;
+    twb = c->twb;
+    vwb = c->vwb;
+    bg = c->bg;
+    ba = c->ba;
+    setInformation(c->H);
+}
+
+void EdgePriorPoseImu::computeError()
+{
+    const VertexPose* VP = static_cast<const VertexPose*>(_vertices[0]);
+    const VertexVelocity* VV = static_cast<const VertexVelocity*>(_vertices[1]);
+    const VertexGyroBias* VG = static_cast<const VertexGyroBias*>(_vertices[2]);
+    const VertexAccBias* VA = static_cast<const VertexAccBias*>(_vertices[3]);
+
+    const Eigen::Vector3d er = LogSO3(Rwb.transpose()*VP->estimate().Rwb);
+    const Eigen::Vector3d et = Rwb.transpose()*(VP->estimate().twb-twb);
+    const Eigen::Vector3d ev = VV->estimate() - vwb;
+    const Eigen::Vector3d ebg = VG->estimate() - bg;
+    const Eigen::Vector3d eba = VA->estimate() - ba;
+
+    _error << er, et, ev, ebg, eba;
+}
+
+void EdgePriorPoseImu::linearizeOplus()
+{
+    const VertexPose* VP = static_cast<const VertexPose*>(_vertices[0]);
+    const Eigen::Vector3d er = LogSO3(Rwb.transpose()*VP->estimate().Rwb);
+    _jacobianOplus[0].setZero();
+    _jacobianOplus[0].block<3,3>(0,0) = InverseRightJacobianSO3(er);
+    _jacobianOplus[0].block<3,3>(3,3) = Rwb.transpose()*VP->estimate().Rwb;
+    _jacobianOplus[1].setZero();
+    _jacobianOplus[1].block<3,3>(6,0) = Eigen::Matrix3d::Identity();
+    _jacobianOplus[2].setZero();
+    _jacobianOplus[2].block<3,3>(9,0) = Eigen::Matrix3d::Identity();
+    _jacobianOplus[3].setZero();
+    _jacobianOplus[3].block<3,3>(12,0) = Eigen::Matrix3d::Identity();
+}
+
+void EdgePriorAcc::linearizeOplus()
+{
+    // Jacobian wrt bias
+    _jacobianOplusXi.block<3,3>(0,0) = Eigen::Matrix3d::Identity();
+
+}
+
+void EdgePriorGyro::linearizeOplus()
+{
+    // Jacobian wrt bias
+    _jacobianOplusXi.block<3,3>(0,0) = Eigen::Matrix3d::Identity();
+
+}
+
+// SO3 FUNCTIONS
+Eigen::Matrix3d ExpSO3(const Eigen::Vector3d &w)
+{
+    return ExpSO3(w[0],w[1],w[2]);
+}
+
+Eigen::Matrix3d ExpSO3(const double x, const double y, const double z)
+{
+    const double d2 = x*x+y*y+z*z;
+    const double d = sqrt(d2);
+    Eigen::Matrix3d W;
+    W << 0.0, -z, y,z, 0.0, -x,-y,  x, 0.0;
+    if(d<1e-5)
+    {
+        Eigen::Matrix3d res = Eigen::Matrix3d::Identity() + W +0.5*W*W;
+        return Converter::toMatrix3d(IMU::NormalizeRotation(Converter::toCvMat(res)));
+    }
+    else
+    {
+        Eigen::Matrix3d res =Eigen::Matrix3d::Identity() + W*sin(d)/d + W*W*(1.0-cos(d))/d2;
+        return Converter::toMatrix3d(IMU::NormalizeRotation(Converter::toCvMat(res)));
+    }
+}
+
+Eigen::Vector3d LogSO3(const Eigen::Matrix3d &R)
+{
+    const double tr = R(0,0)+R(1,1)+R(2,2);
+    Eigen::Vector3d w;
+    w << (R(2,1)-R(1,2))/2, (R(0,2)-R(2,0))/2, (R(1,0)-R(0,1))/2;
+    const double costheta = (tr-1.0)*0.5f;
+    if(costheta>1 || costheta<-1)
+        return w;
+    const double theta = acos(costheta);
+    const double s = sin(theta);
+    if(fabs(s)<1e-5)
+        return w;
+    else
+        return theta*w/s;
+}
+
+Eigen::Matrix3d InverseRightJacobianSO3(const Eigen::Vector3d &v)
+{
+    return InverseRightJacobianSO3(v[0],v[1],v[2]);
+}
+
+Eigen::Matrix3d InverseRightJacobianSO3(const double x, const double y, const double z)
+{
+    const double d2 = x*x+y*y+z*z;
+    const double d = sqrt(d2);
+
+    Eigen::Matrix3d W;
+    W << 0.0, -z, y,z, 0.0, -x,-y,  x, 0.0;
+    if(d<1e-5)
+        return Eigen::Matrix3d::Identity();
+    else
+        return Eigen::Matrix3d::Identity() + W/2 + W*W*(1.0/d2 - (1.0+cos(d))/(2.0*d*sin(d)));
+}
+
+Eigen::Matrix3d RightJacobianSO3(const Eigen::Vector3d &v)
+{
+    return RightJacobianSO3(v[0],v[1],v[2]);
+}
+
+Eigen::Matrix3d RightJacobianSO3(const double x, const double y, const double z)
+{
+    const double d2 = x*x+y*y+z*z;
+    const double d = sqrt(d2);
+
+    Eigen::Matrix3d W;
+    W << 0.0, -z, y,z, 0.0, -x,-y,  x, 0.0;
+    if(d<1e-5)
+    {
+        return Eigen::Matrix3d::Identity();
+    }
+    else
+    {
+        return Eigen::Matrix3d::Identity() - W*(1.0-cos(d))/d2 + W*W*(d-sin(d))/(d2*d);
+    }
+}
+
+Eigen::Matrix3d Skew(const Eigen::Vector3d &w)
+{
+    Eigen::Matrix3d W;
+    W << 0.0, -w[2], w[1],w[2], 0.0, -w[0],-w[1],  w[0], 0.0;
+    return W;
+}
+
+Eigen::Matrix3d NormalizeRotation(const Eigen::Matrix3d &R)
+{
+    Eigen::JacobiSVD<Eigen::Matrix3d> svd(R,Eigen::ComputeFullU | Eigen::ComputeFullV);
+    return svd.matrixU()*svd.matrixV();
+}
+}

src/ImuTypes.cc

@@ -0,0 +1,521 @@
+/**
+* This file is part of ORB-SLAM3
+*
+* Copyright (C) 2017-2020 Carlos Campos, Richard Elvira, Juan J. Gómez Rodríguez, José M.M. Montiel and Juan D. Tardós, University of Zaragoza.
+* Copyright (C) 2014-2016 Raúl Mur-Artal, José M.M. Montiel and Juan D. Tardós, University of Zaragoza.
+*
+* ORB-SLAM3 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-SLAM3 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-SLAM3.
+* If not, see <http://www.gnu.org/licenses/>.
+*/
+
+#include "ImuTypes.h"
+#include<iostream>
+
+namespace ORB_SLAM3
+{
+
+namespace IMU
+{
+
+const float eps = 1e-4;
+
+cv::Mat NormalizeRotation(const cv::Mat &R)
+{
+    cv::Mat U,w,Vt;
+    cv::SVDecomp(R,w,U,Vt,cv::SVD::FULL_UV);
+    return U*Vt;
+}
+
+cv::Mat Skew(const cv::Mat &v)
+{
+    const float x = v.at<float>(0);
+    const float y = v.at<float>(1);
+    const float z = v.at<float>(2);
+    return (cv::Mat_<float>(3,3) << 0, -z, y,
+            z, 0, -x,
+            -y,  x, 0);
+}
+
+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);
+}
+
+Eigen::Matrix<double,3,3> ExpSO3(const double &x, const double &y, const double &z)
+{
+    Eigen::Matrix<double,3,3> I = Eigen::MatrixXd::Identity(3,3);
+    const double d2 = x*x+y*y+z*z;
+    const double d = sqrt(d2);
+    Eigen::Matrix<double,3,3> W;
+    W(0,0) = 0;
+    W(0,1) = -z;
+    W(0,2) = y;
+    W(1,0) = z;
+    W(1,1) = 0;
+    W(1,2) = -x;
+    W(2,0) = -y;
+    W(2,1) = x;
+    W(2,2) = 0;
+
+    if(d<eps)
+        return (I + W + 0.5*W*W);
+    else
+        return (I + W*sin(d)/d + W*W*(1.0-cos(d))/d2);
+}
+
+cv::Mat ExpSO3(const cv::Mat &v)
+{
+    return ExpSO3(v.at<float>(0),v.at<float>(1),v.at<float>(2));
+}
+
+cv::Mat LogSO3(const cv::Mat &R)
+{
+    const float tr = R.at<float>(0,0)+R.at<float>(1,1)+R.at<float>(2,2);
+    cv::Mat w = (cv::Mat_<float>(3,1) <<(R.at<float>(2,1)-R.at<float>(1,2))/2,
+                                        (R.at<float>(0,2)-R.at<float>(2,0))/2,
+                                        (R.at<float>(1,0)-R.at<float>(0,1))/2);
+    const float costheta = (tr-1.0f)*0.5f;
+    if(costheta>1 || costheta<-1)
+        return w;
+    const float theta = acos(costheta);
+    const float s = sin(theta);
+    if(fabs(s)<eps)
+        return w;
+    else
+        return theta*w/s;
+}
+
+cv::Mat RightJacobianSO3(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 cv::Mat::eye(3,3,CV_32F);
+    }
+    else
+    {
+        return I - W*(1.0f-cos(d))/d2 + W*W*(d-sin(d))/(d2*d);
+    }
+}
+
+cv::Mat RightJacobianSO3(const cv::Mat &v)
+{
+    return RightJacobianSO3(v.at<float>(0),v.at<float>(1),v.at<float>(2));
+}
+
+cv::Mat InverseRightJacobianSO3(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 cv::Mat::eye(3,3,CV_32F);
+    }
+    else
+    {
+        return I + W/2 + W*W*(1.0f/d2 - (1.0f+cos(d))/(2.0f*d*sin(d)));
+    }
+}
+
+cv::Mat InverseRightJacobianSO3(const cv::Mat &v)
+{
+    return InverseRightJacobianSO3(v.at<float>(0),v.at<float>(1),v.at<float>(2));
+}
+
+
+IntegratedRotation::IntegratedRotation(const cv::Point3f &angVel, const Bias &imuBias, const float &time):
+    deltaT(time)
+{
+    const float x = (angVel.x-imuBias.bwx)*time;
+    const float y = (angVel.y-imuBias.bwy)*time;
+    const float z = (angVel.z-imuBias.bwz)*time;
+
+    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)
+    {
+        deltaR = I + W;
+        rightJ = cv::Mat::eye(3,3,CV_32F);
+    }
+    else
+    {
+        deltaR = I + W*sin(d)/d + W*W*(1.0f-cos(d))/d2;
+        rightJ = I - W*(1.0f-cos(d))/d2 + W*W*(d-sin(d))/(d2*d);
+    }
+}
+
+Preintegrated::Preintegrated(const Bias &b_, const Calib &calib)
+{
+    Nga = calib.Cov.clone();
+    NgaWalk = calib.CovWalk.clone();
+    Initialize(b_);
+}
+
+// Copy constructor
+Preintegrated::Preintegrated(Preintegrated* pImuPre): dT(pImuPre->dT), C(pImuPre->C.clone()), Info(pImuPre->Info.clone()),
+    Nga(pImuPre->Nga.clone()), NgaWalk(pImuPre->NgaWalk.clone()), b(pImuPre->b), dR(pImuPre->dR.clone()), dV(pImuPre->dV.clone()),
+    dP(pImuPre->dP.clone()), JRg(pImuPre->JRg.clone()), JVg(pImuPre->JVg.clone()), JVa(pImuPre->JVa.clone()), JPg(pImuPre->JPg.clone()),
+    JPa(pImuPre->JPa.clone()), avgA(pImuPre->avgA.clone()), avgW(pImuPre->avgW.clone()), bu(pImuPre->bu), db(pImuPre->db.clone()), mvMeasurements(pImuPre->mvMeasurements)
+{
+
+}
+
+void Preintegrated::CopyFrom(Preintegrated* pImuPre)
+{
+    std::cout << "Preintegrated: start clone" << std::endl;
+    dT = pImuPre->dT;
+    C = pImuPre->C.clone();
+    Info = pImuPre->Info.clone();
+    Nga = pImuPre->Nga.clone();
+    NgaWalk = pImuPre->NgaWalk.clone();
+    std::cout << "Preintegrated: first clone" << std::endl;
+    b.CopyFrom(pImuPre->b);
+    dR = pImuPre->dR.clone();
+    dV = pImuPre->dV.clone();
+    dP = pImuPre->dP.clone();
+    JRg = pImuPre->JRg.clone();
+    JVg = pImuPre->JVg.clone();
+    JVa = pImuPre->JVa.clone();
+    JPg = pImuPre->JPg.clone();
+    JPa = pImuPre->JPa.clone();
+    avgA = pImuPre->avgA.clone();
+    avgW = pImuPre->avgW.clone();
+    std::cout << "Preintegrated: second clone" << std::endl;
+    bu.CopyFrom(pImuPre->bu);
+    db = pImuPre->db.clone();
+    std::cout << "Preintegrated: third clone" << std::endl;
+    mvMeasurements = pImuPre->mvMeasurements;
+    std::cout << "Preintegrated: end clone" << std::endl;
+}
+
+
+void Preintegrated::Initialize(const Bias &b_)
+{
+    dR = cv::Mat::eye(3,3,CV_32F);
+    dV = cv::Mat::zeros(3,1,CV_32F);
+    dP = cv::Mat::zeros(3,1,CV_32F);
+    JRg = cv::Mat::zeros(3,3,CV_32F);
+    JVg = cv::Mat::zeros(3,3,CV_32F);
+    JVa = cv::Mat::zeros(3,3,CV_32F);
+    JPg = cv::Mat::zeros(3,3,CV_32F);
+    JPa = cv::Mat::zeros(3,3,CV_32F);
+    C = cv::Mat::zeros(15,15,CV_32F);
+    Info=cv::Mat();
+    db = cv::Mat::zeros(6,1,CV_32F);
+    b=b_;
+    bu=b_;
+    avgA = cv::Mat::zeros(3,1,CV_32F);
+    avgW = cv::Mat::zeros(3,1,CV_32F);
+    dT=0.0f;
+    mvMeasurements.clear();
+}
+
+void Preintegrated::Reintegrate()
+{
+    std::unique_lock<std::mutex> lock(mMutex);
+    const std::vector<integrable> aux = mvMeasurements;
+    Initialize(bu);
+    for(size_t i=0;i<aux.size();i++)
+        IntegrateNewMeasurement(aux[i].a,aux[i].w,aux[i].t);
+}
+
+void Preintegrated::IntegrateNewMeasurement(const cv::Point3f &acceleration, const cv::Point3f &angVel, const float &dt)
+{
+    mvMeasurements.push_back(integrable(acceleration,angVel,dt));
+
+    // Position is updated firstly, as it depends on previously computed velocity and rotation.
+    // Velocity is updated secondly, as it depends on previously computed rotation.
+    // Rotation is the last to be updated.
+
+    //Matrices to compute covariance
+    cv::Mat A = cv::Mat::eye(9,9,CV_32F);
+    cv::Mat B = cv::Mat::zeros(9,6,CV_32F);
+
+    cv::Mat acc = (cv::Mat_<float>(3,1) << acceleration.x-b.bax,acceleration.y-b.bay, acceleration.z-b.baz);
+    cv::Mat accW = (cv::Mat_<float>(3,1) << angVel.x-b.bwx, angVel.y-b.bwy, angVel.z-b.bwz);
+
+    avgA = (dT*avgA + dR*acc*dt)/(dT+dt);
+    avgW = (dT*avgW + accW*dt)/(dT+dt);
+
+    // Update delta position dP and velocity dV (rely on no-updated delta rotation)
+    dP = dP + dV*dt + 0.5f*dR*acc*dt*dt;
+    dV = dV + dR*acc*dt;
+
+    // Compute velocity and position parts of matrices A and B (rely on non-updated delta rotation)
+    cv::Mat Wacc = (cv::Mat_<float>(3,3) << 0, -acc.at<float>(2), acc.at<float>(1),
+                                                   acc.at<float>(2), 0, -acc.at<float>(0),
+                                                   -acc.at<float>(1), acc.at<float>(0), 0);
+    A.rowRange(3,6).colRange(0,3) = -dR*dt*Wacc;
+    A.rowRange(6,9).colRange(0,3) = -0.5f*dR*dt*dt*Wacc;
+    A.rowRange(6,9).colRange(3,6) = cv::Mat::eye(3,3,CV_32F)*dt;
+    B.rowRange(3,6).colRange(3,6) = dR*dt;
+    B.rowRange(6,9).colRange(3,6) = 0.5f*dR*dt*dt;
+
+    // Update position and velocity jacobians wrt bias correction
+    JPa = JPa + JVa*dt -0.5f*dR*dt*dt;
+    JPg = JPg + JVg*dt -0.5f*dR*dt*dt*Wacc*JRg;
+    JVa = JVa - dR*dt;
+    JVg = JVg - dR*dt*Wacc*JRg;
+
+    // Update delta rotation
+    IntegratedRotation dRi(angVel,b,dt);
+    dR = NormalizeRotation(dR*dRi.deltaR);
+
+    // Compute rotation parts of matrices A and B
+    A.rowRange(0,3).colRange(0,3) = dRi.deltaR.t();
+    B.rowRange(0,3).colRange(0,3) = dRi.rightJ*dt;
+
+    // Update covariance
+    C.rowRange(0,9).colRange(0,9) = A*C.rowRange(0,9).colRange(0,9)*A.t() + B*Nga*B.t();
+    C.rowRange(9,15).colRange(9,15) = C.rowRange(9,15).colRange(9,15) + NgaWalk;
+
+    // Update rotation jacobian wrt bias correction
+    JRg = dRi.deltaR.t()*JRg - dRi.rightJ*dt;
+
+    // Total integrated time
+    dT += dt;
+}
+
+void Preintegrated::MergePrevious(Preintegrated* pPrev)
+{
+    if (pPrev==this)
+        return;
+
+    std::unique_lock<std::mutex> lock1(mMutex);
+    std::unique_lock<std::mutex> lock2(pPrev->mMutex);
+    Bias bav;
+    bav.bwx = bu.bwx;
+    bav.bwy = bu.bwy;
+    bav.bwz = bu.bwz;
+    bav.bax = bu.bax;
+    bav.bay = bu.bay;
+    bav.baz = bu.baz;
+
+    const std::vector<integrable > aux1 = pPrev->mvMeasurements;
+    const std::vector<integrable> aux2 = mvMeasurements;
+
+    Initialize(bav);
+    for(size_t i=0;i<aux1.size();i++)
+        IntegrateNewMeasurement(aux1[i].a,aux1[i].w,aux1[i].t);
+    for(size_t i=0;i<aux2.size();i++)
+        IntegrateNewMeasurement(aux2[i].a,aux2[i].w,aux2[i].t);
+
+}
+
+void Preintegrated::SetNewBias(const Bias &bu_)
+{
+    std::unique_lock<std::mutex> lock(mMutex);
+    bu = bu_;
+
+    db.at<float>(0) = bu_.bwx-b.bwx;
+    db.at<float>(1) = bu_.bwy-b.bwy;
+    db.at<float>(2) = bu_.bwz-b.bwz;
+    db.at<float>(3) = bu_.bax-b.bax;
+    db.at<float>(4) = bu_.bay-b.bay;
+    db.at<float>(5) = bu_.baz-b.baz;
+}
+
+IMU::Bias Preintegrated::GetDeltaBias(const Bias &b_)
+{
+    std::unique_lock<std::mutex> lock(mMutex);
+    return IMU::Bias(b_.bax-b.bax,b_.bay-b.bay,b_.baz-b.baz,b_.bwx-b.bwx,b_.bwy-b.bwy,b_.bwz-b.bwz);
+}
+
+cv::Mat Preintegrated::GetDeltaRotation(const Bias &b_)
+{
+    std::unique_lock<std::mutex> lock(mMutex);
+    cv::Mat dbg = (cv::Mat_<float>(3,1) << b_.bwx-b.bwx,b_.bwy-b.bwy,b_.bwz-b.bwz);
+    return NormalizeRotation(dR*ExpSO3(JRg*dbg));
+}
+
+cv::Mat Preintegrated::GetDeltaVelocity(const Bias &b_)
+{
+    std::unique_lock<std::mutex> lock(mMutex);
+    cv::Mat dbg = (cv::Mat_<float>(3,1) << b_.bwx-b.bwx,b_.bwy-b.bwy,b_.bwz-b.bwz);
+    cv::Mat dba = (cv::Mat_<float>(3,1) << b_.bax-b.bax,b_.bay-b.bay,b_.baz-b.baz);
+    return dV + JVg*dbg + JVa*dba;
+}
+
+cv::Mat Preintegrated::GetDeltaPosition(const Bias &b_)
+{
+    std::unique_lock<std::mutex> lock(mMutex);
+    cv::Mat dbg = (cv::Mat_<float>(3,1) << b_.bwx-b.bwx,b_.bwy-b.bwy,b_.bwz-b.bwz);
+    cv::Mat dba = (cv::Mat_<float>(3,1) << b_.bax-b.bax,b_.bay-b.bay,b_.baz-b.baz);
+    return dP + JPg*dbg + JPa*dba;
+}
+
+cv::Mat Preintegrated::GetUpdatedDeltaRotation()
+{
+    std::unique_lock<std::mutex> lock(mMutex);
+    return NormalizeRotation(dR*ExpSO3(JRg*db.rowRange(0,3)));
+}
+
+cv::Mat Preintegrated::GetUpdatedDeltaVelocity()
+{
+    std::unique_lock<std::mutex> lock(mMutex);
+    return dV + JVg*db.rowRange(0,3) + JVa*db.rowRange(3,6);
+}
+
+cv::Mat Preintegrated::GetUpdatedDeltaPosition()
+{
+    std::unique_lock<std::mutex> lock(mMutex);
+    return dP + JPg*db.rowRange(0,3) + JPa*db.rowRange(3,6);
+}
+
+cv::Mat Preintegrated::GetOriginalDeltaRotation()
+{
+    std::unique_lock<std::mutex> lock(mMutex);
+    return dR.clone();
+}
+
+cv::Mat Preintegrated::GetOriginalDeltaVelocity()
+{
+    std::unique_lock<std::mutex> lock(mMutex);
+    return dV.clone();
+}
+
+cv::Mat Preintegrated::GetOriginalDeltaPosition()
+{
+    std::unique_lock<std::mutex> lock(mMutex);
+    return dP.clone();
+}
+
+Bias Preintegrated::GetOriginalBias()
+{
+    std::unique_lock<std::mutex> lock(mMutex);
+    return b;
+}
+
+Bias Preintegrated::GetUpdatedBias()
+{
+    std::unique_lock<std::mutex> lock(mMutex);
+    return bu;
+}
+
+cv::Mat Preintegrated::GetDeltaBias()
+{
+    std::unique_lock<std::mutex> lock(mMutex);
+    return db.clone();
+}
+
+Eigen::Matrix<double,15,15> Preintegrated::GetInformationMatrix()
+{
+    std::unique_lock<std::mutex> lock(mMutex);
+    if(Info.empty())
+    {
+        Info = cv::Mat::zeros(15,15,CV_32F);
+        Info.rowRange(0,9).colRange(0,9)=C.rowRange(0,9).colRange(0,9).inv(cv::DECOMP_SVD);
+        for(int i=9;i<15;i++)
+            Info.at<float>(i,i)=1.0f/C.at<float>(i,i);
+    }
+
+    Eigen::Matrix<double,15,15> EI;
+    for(int i=0;i<15;i++)
+        for(int j=0;j<15;j++)
+            EI(i,j)=Info.at<float>(i,j);
+    return EI;
+}
+
+void Bias::CopyFrom(Bias &b)
+{
+    bax = b.bax;
+    bay = b.bay;
+    baz = b.baz;
+    bwx = b.bwx;
+    bwy = b.bwy;
+    bwz = b.bwz;
+}
+
+std::ostream& operator<< (std::ostream &out, const Bias &b)
+{
+    if(b.bwx>0)
+        out << " ";
+    out << b.bwx << ",";
+    if(b.bwy>0)
+        out << " ";
+    out << b.bwy << ",";
+    if(b.bwz>0)
+        out << " ";
+    out << b.bwz << ",";
+    if(b.bax>0)
+        out << " ";
+    out << b.bax << ",";
+    if(b.bay>0)
+        out << " ";
+    out << b.bay << ",";
+    if(b.baz>0)
+        out << " ";
+    out << b.baz;
+
+    return out;
+}
+
+void Calib::Set(const cv::Mat &Tbc_, const float &ng, const float &na, const float &ngw, const float &naw)
+{
+    Tbc = Tbc_.clone();
+    Tcb = cv::Mat::eye(4,4,CV_32F);
+    Tcb.rowRange(0,3).colRange(0,3) = Tbc.rowRange(0,3).colRange(0,3).t();
+    Tcb.rowRange(0,3).col(3) = -Tbc.rowRange(0,3).colRange(0,3).t()*Tbc.rowRange(0,3).col(3);
+    Cov = cv::Mat::eye(6,6,CV_32F);
+    const float ng2 = ng*ng;
+    const float na2 = na*na;
+    Cov.at<float>(0,0) = ng2;
+    Cov.at<float>(1,1) = ng2;
+    Cov.at<float>(2,2) = ng2;
+    Cov.at<float>(3,3) = na2;
+    Cov.at<float>(4,4) = na2;
+    Cov.at<float>(5,5) = na2;
+    CovWalk = cv::Mat::eye(6,6,CV_32F);
+    const float ngw2 = ngw*ngw;
+    const float naw2 = naw*naw;
+    CovWalk.at<float>(0,0) = ngw2;
+    CovWalk.at<float>(1,1) = ngw2;
+    CovWalk.at<float>(2,2) = ngw2;
+    CovWalk.at<float>(3,3) = naw2;
+    CovWalk.at<float>(4,4) = naw2;
+    CovWalk.at<float>(5,5) = naw2;
+}
+
+Calib::Calib(const Calib &calib)
+{
+    Tbc = calib.Tbc.clone();
+    Tcb = calib.Tcb.clone();
+    Cov = calib.Cov.clone();
+    CovWalk = calib.CovWalk.clone();
+}
+
+} //namespace IMU
+
+} //namespace ORB_SLAM2

src/Initializer.cc

@@ -0,0 +1,922 @@
+/**
+* This file is part of ORB-SLAM3
+*
+* Copyright (C) 2017-2020 Carlos Campos, Richard Elvira, Juan J. Gómez Rodríguez, José M.M. Montiel and Juan D. Tardós, University of Zaragoza.
+* Copyright (C) 2014-2016 Raúl Mur-Artal, José M.M. Montiel and Juan D. Tardós, University of Zaragoza.
+*
+* ORB-SLAM3 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-SLAM3 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-SLAM3.
+* If not, see <http://www.gnu.org/licenses/>.
+*/
+
+#include "Initializer.h"
+
+#include "Thirdparty/DBoW2/DUtils/Random.h"
+
+#include "Optimizer.h"
+#include "ORBmatcher.h"
+
+#include<thread>
+#include <include/CameraModels/Pinhole.h>
+
+namespace ORB_SLAM3
+{
+
+Initializer::Initializer(const Frame &ReferenceFrame, float sigma, int iterations)
+{
+    mpCamera = ReferenceFrame.mpCamera;
+    mK = ReferenceFrame.mK.clone();
+
+    mvKeys1 = ReferenceFrame.mvKeysUn;
+
+    mSigma = sigma;
+    mSigma2 = sigma*sigma;
+    mMaxIterations = iterations;
+}
+
+bool Initializer::Initialize(const Frame &CurrentFrame, const vector<int> &vMatches12, cv::Mat &R21, cv::Mat &t21,
+                             vector<cv::Point3f> &vP3D, vector<bool> &vbTriangulated)
+{
+
+    mvKeys2 = CurrentFrame.mvKeysUn;
+
+    mvMatches12.clear();
+    mvMatches12.reserve(mvKeys2.size());
+    mvbMatched1.resize(mvKeys1.size());
+    for(size_t i=0, iend=vMatches12.size();i<iend; i++)
+    {
+        if(vMatches12[i]>=0)
+        {
+            mvMatches12.push_back(make_pair(i,vMatches12[i]));
+            mvbMatched1[i]=true;
+        }
+        else
+            mvbMatched1[i]=false;
+    }
+
+
+
+    const int N = mvMatches12.size();
+
+    vector<size_t> vAllIndices;
+    vAllIndices.reserve(N);
+    vector<size_t> vAvailableIndices;
+
+    for(int i=0; i<N; i++)
+    {
+        vAllIndices.push_back(i);
+    }
+    // Generate sets of 8 points for each RANSAC iteration
+    mvSets = vector< vector<size_t> >(mMaxIterations,vector<size_t>(8,0));
+
+    DUtils::Random::SeedRandOnce(0);
+
+    for(int it=0; it<mMaxIterations; it++)
+    {
+        vAvailableIndices = vAllIndices;
+
+        // Select a minimum set
+        for(size_t j=0; j<8; j++)
+        {
+            int randi = DUtils::Random::RandomInt(0,vAvailableIndices.size()-1);
+            int idx = vAvailableIndices[randi];
+
+            mvSets[it][j] = idx;
+
+            vAvailableIndices[randi] = vAvailableIndices.back();
+            vAvailableIndices.pop_back();
+        }
+    }
+
+    // Launch threads to compute in parallel a fundamental matrix and a homography
+    vector<bool> vbMatchesInliersH, vbMatchesInliersF;
+    float SH, SF;
+    cv::Mat H, F;
+
+    thread threadH(&Initializer::FindHomography,this,ref(vbMatchesInliersH), ref(SH), ref(H));
+    thread threadF(&Initializer::FindFundamental,this,ref(vbMatchesInliersF), ref(SF), ref(F));
+
+    // Wait until both threads have finished
+    threadH.join();
+    threadF.join();
+
+    // Compute ratio of scores
+    float RH = SH/(SH+SF);
+
+    float minParallax = 1.0; // 1.0 originally
+
+    cv::Mat K = static_cast<Pinhole*>(mpCamera)->toK();
+    // Try to reconstruct from homography or fundamental depending on the ratio (0.40-0.45)
+    if(RH>0.40)
+    {
+        return ReconstructH(vbMatchesInliersH,H, K,R21,t21,vP3D,vbTriangulated,minParallax,50);
+    }
+    else
+    {
+        return ReconstructF(vbMatchesInliersF,F,K,R21,t21,vP3D,vbTriangulated,minParallax,50);
+    }
+
+    return false;
+}
+
+
+void Initializer::FindHomography(vector<bool> &vbMatchesInliers, float &score, cv::Mat &H21)
+{
+    // Number of putative matches
+    const int N = mvMatches12.size();
+
+    // Normalize coordinates
+    vector<cv::Point2f> vPn1, vPn2;
+    cv::Mat T1, T2;
+    Normalize(mvKeys1,vPn1, T1);
+    Normalize(mvKeys2,vPn2, T2);
+    cv::Mat T2inv = T2.inv();
+
+    // Best Results variables
+    score = 0.0;
+    vbMatchesInliers = vector<bool>(N,false);
+
+    // Iteration variables
+    vector<cv::Point2f> vPn1i(8);
+    vector<cv::Point2f> vPn2i(8);
+    cv::Mat H21i, H12i;
+    vector<bool> vbCurrentInliers(N,false);
+    float currentScore;
+
+    // Perform all RANSAC iterations and save the solution with highest score
+    for(int it=0; it<mMaxIterations; it++)
+    {
+        // Select a minimum set
+        for(size_t j=0; j<8; j++)
+        {
+            int idx = mvSets[it][j];
+
+            vPn1i[j] = vPn1[mvMatches12[idx].first];
+            vPn2i[j] = vPn2[mvMatches12[idx].second];
+        }
+
+        cv::Mat Hn = ComputeH21(vPn1i,vPn2i);
+        H21i = T2inv*Hn*T1;
+        H12i = H21i.inv();
+
+        currentScore = CheckHomography(H21i, H12i, vbCurrentInliers, mSigma);
+
+        if(currentScore>score)
+        {
+            H21 = H21i.clone();
+            vbMatchesInliers = vbCurrentInliers;
+            score = currentScore;
+        }
+    }
+}
+
+
+void Initializer::FindFundamental(vector<bool> &vbMatchesInliers, float &score, cv::Mat &F21)
+{
+    // Number of putative matches
+    const int N = vbMatchesInliers.size();
+
+    // Normalize coordinates
+    vector<cv::Point2f> vPn1, vPn2;
+    cv::Mat T1, T2;
+    Normalize(mvKeys1,vPn1, T1);
+    Normalize(mvKeys2,vPn2, T2);
+    cv::Mat T2t = T2.t();
+
+    // Best Results variables
+    score = 0.0;
+    vbMatchesInliers = vector<bool>(N,false);
+
+    // Iteration variables
+    vector<cv::Point2f> vPn1i(8);
+    vector<cv::Point2f> vPn2i(8);
+    cv::Mat F21i;
+    vector<bool> vbCurrentInliers(N,false);
+    float currentScore;
+
+    // Perform all RANSAC iterations and save the solution with highest score
+    for(int it=0; it<mMaxIterations; it++)
+    {
+        // Select a minimum set
+        for(int j=0; j<8; j++)
+        {
+            int idx = mvSets[it][j];
+
+            vPn1i[j] = vPn1[mvMatches12[idx].first];
+            vPn2i[j] = vPn2[mvMatches12[idx].second];
+        }
+
+        cv::Mat Fn = ComputeF21(vPn1i,vPn2i);
+
+        F21i = T2t*Fn*T1;
+
+        currentScore = CheckFundamental(F21i, vbCurrentInliers, mSigma);
+
+        if(currentScore>score)
+        {
+            F21 = F21i.clone();
+            vbMatchesInliers = vbCurrentInliers;
+            score = currentScore;
+        }
+    }
+}
+
+
+cv::Mat Initializer::ComputeH21(const vector<cv::Point2f> &vP1, const vector<cv::Point2f> &vP2)
+{
+    const int N = vP1.size();
+
+    cv::Mat A(2*N,9,CV_32F);
+
+    for(int i=0; i<N; i++)
+    {
+        const float u1 = vP1[i].x;
+        const float v1 = vP1[i].y;
+        const float u2 = vP2[i].x;
+        const float v2 = vP2[i].y;
+
+        A.at<float>(2*i,0) = 0.0;
+        A.at<float>(2*i,1) = 0.0;
+        A.at<float>(2*i,2) = 0.0;
+        A.at<float>(2*i,3) = -u1;
+        A.at<float>(2*i,4) = -v1;
+        A.at<float>(2*i,5) = -1;
+        A.at<float>(2*i,6) = v2*u1;
+        A.at<float>(2*i,7) = v2*v1;
+        A.at<float>(2*i,8) = v2;
+
+        A.at<float>(2*i+1,0) = u1;
+        A.at<float>(2*i+1,1) = v1;
+        A.at<float>(2*i+1,2) = 1;
+        A.at<float>(2*i+1,3) = 0.0;
+        A.at<float>(2*i+1,4) = 0.0;
+        A.at<float>(2*i+1,5) = 0.0;
+        A.at<float>(2*i+1,6) = -u2*u1;
+        A.at<float>(2*i+1,7) = -u2*v1;
+        A.at<float>(2*i+1,8) = -u2;
+
+    }
+
+    cv::Mat u,w,vt;
+
+    cv::SVDecomp(A,w,u,vt,cv::SVD::MODIFY_A | cv::SVD::FULL_UV);
+
+    return vt.row(8).reshape(0, 3);
+}
+
+cv::Mat Initializer::ComputeF21(const vector<cv::Point2f> &vP1,const vector<cv::Point2f> &vP2)
+{
+    const int N = vP1.size();
+
+    cv::Mat A(N,9,CV_32F);
+
+    for(int i=0; i<N; i++)
+    {
+        const float u1 = vP1[i].x;
+        const float v1 = vP1[i].y;
+        const float u2 = vP2[i].x;
+        const float v2 = vP2[i].y;
+
+        A.at<float>(i,0) = u2*u1;
+        A.at<float>(i,1) = u2*v1;
+        A.at<float>(i,2) = u2;
+        A.at<float>(i,3) = v2*u1;
+        A.at<float>(i,4) = v2*v1;
+        A.at<float>(i,5) = v2;
+        A.at<float>(i,6) = u1;
+        A.at<float>(i,7) = v1;
+        A.at<float>(i,8) = 1;
+    }
+
+    cv::Mat u,w,vt;
+
+    cv::SVDecomp(A,w,u,vt,cv::SVD::MODIFY_A | cv::SVD::FULL_UV);
+
+    cv::Mat Fpre = vt.row(8).reshape(0, 3);
+
+    cv::SVDecomp(Fpre,w,u,vt,cv::SVD::MODIFY_A | cv::SVD::FULL_UV);
+
+    w.at<float>(2)=0;
+
+    return  u*cv::Mat::diag(w)*vt;
+}
+
+float Initializer::CheckHomography(const cv::Mat &H21, const cv::Mat &H12, vector<bool> &vbMatchesInliers, float sigma)
+{   
+    const int N = mvMatches12.size();
+
+    const float h11 = H21.at<float>(0,0);
+    const float h12 = H21.at<float>(0,1);
+    const float h13 = H21.at<float>(0,2);
+    const float h21 = H21.at<float>(1,0);
+    const float h22 = H21.at<float>(1,1);
+    const float h23 = H21.at<float>(1,2);
+    const float h31 = H21.at<float>(2,0);
+    const float h32 = H21.at<float>(2,1);
+    const float h33 = H21.at<float>(2,2);
+
+    const float h11inv = H12.at<float>(0,0);
+    const float h12inv = H12.at<float>(0,1);
+    const float h13inv = H12.at<float>(0,2);
+    const float h21inv = H12.at<float>(1,0);
+    const float h22inv = H12.at<float>(1,1);
+    const float h23inv = H12.at<float>(1,2);
+    const float h31inv = H12.at<float>(2,0);
+    const float h32inv = H12.at<float>(2,1);
+    const float h33inv = H12.at<float>(2,2);
+
+    vbMatchesInliers.resize(N);
+
+    float score = 0;
+
+    const float th = 5.991;
+
+    const float invSigmaSquare = 1.0/(sigma*sigma);
+
+    for(int i=0; i<N; i++)
+    {
+        bool bIn = true;
+
+        const cv::KeyPoint &kp1 = mvKeys1[mvMatches12[i].first];
+        const cv::KeyPoint &kp2 = mvKeys2[mvMatches12[i].second];
+
+        const float u1 = kp1.pt.x;
+        const float v1 = kp1.pt.y;
+        const float u2 = kp2.pt.x;
+        const float v2 = kp2.pt.y;
+
+        // Reprojection error in first image
+        // x2in1 = H12*x2
+
+        const float w2in1inv = 1.0/(h31inv*u2+h32inv*v2+h33inv);
+        const float u2in1 = (h11inv*u2+h12inv*v2+h13inv)*w2in1inv;
+        const float v2in1 = (h21inv*u2+h22inv*v2+h23inv)*w2in1inv;
+
+        const float squareDist1 = (u1-u2in1)*(u1-u2in1)+(v1-v2in1)*(v1-v2in1);
+
+        const float chiSquare1 = squareDist1*invSigmaSquare;
+
+        if(chiSquare1>th)
+            bIn = false;
+        else
+            score += th - chiSquare1;
+
+        // Reprojection error in second image
+        // x1in2 = H21*x1
+
+        const float w1in2inv = 1.0/(h31*u1+h32*v1+h33);
+        const float u1in2 = (h11*u1+h12*v1+h13)*w1in2inv;
+        const float v1in2 = (h21*u1+h22*v1+h23)*w1in2inv;
+
+        const float squareDist2 = (u2-u1in2)*(u2-u1in2)+(v2-v1in2)*(v2-v1in2);
+
+        const float chiSquare2 = squareDist2*invSigmaSquare;
+
+        if(chiSquare2>th)
+            bIn = false;
+        else
+            score += th - chiSquare2;
+
+        if(bIn)
+            vbMatchesInliers[i]=true;
+        else
+            vbMatchesInliers[i]=false;
+    }
+
+    return score;
+}
+
+float Initializer::CheckFundamental(const cv::Mat &F21, vector<bool> &vbMatchesInliers, float sigma)
+{
+    const int N = mvMatches12.size();
+
+    const float f11 = F21.at<float>(0,0);
+    const float f12 = F21.at<float>(0,1);
+    const float f13 = F21.at<float>(0,2);
+    const float f21 = F21.at<float>(1,0);
+    const float f22 = F21.at<float>(1,1);
+    const float f23 = F21.at<float>(1,2);
+    const float f31 = F21.at<float>(2,0);
+    const float f32 = F21.at<float>(2,1);
+    const float f33 = F21.at<float>(2,2);
+
+    vbMatchesInliers.resize(N);
+
+    float score = 0;
+
+    const float th = 3.841;
+    const float thScore = 5.991;
+
+    const float invSigmaSquare = 1.0/(sigma*sigma);
+
+    for(int i=0; i<N; i++)
+    {
+        bool bIn = true;
+
+        const cv::KeyPoint &kp1 = mvKeys1[mvMatches12[i].first];
+        const cv::KeyPoint &kp2 = mvKeys2[mvMatches12[i].second];
+
+        const float u1 = kp1.pt.x;
+        const float v1 = kp1.pt.y;
+        const float u2 = kp2.pt.x;
+        const float v2 = kp2.pt.y;
+
+        // Reprojection error in second image
+        // l2=F21x1=(a2,b2,c2)
+
+        const float a2 = f11*u1+f12*v1+f13;
+        const float b2 = f21*u1+f22*v1+f23;
+        const float c2 = f31*u1+f32*v1+f33;
+
+        const float num2 = a2*u2+b2*v2+c2;
+
+        const float squareDist1 = num2*num2/(a2*a2+b2*b2);
+
+        const float chiSquare1 = squareDist1*invSigmaSquare;
+
+        if(chiSquare1>th)
+            bIn = false;
+        else
+            score += thScore - chiSquare1;
+
+        // Reprojection error in second image
+        // l1 =x2tF21=(a1,b1,c1)
+
+        const float a1 = f11*u2+f21*v2+f31;
+        const float b1 = f12*u2+f22*v2+f32;
+        const float c1 = f13*u2+f23*v2+f33;
+
+        const float num1 = a1*u1+b1*v1+c1;
+
+        const float squareDist2 = num1*num1/(a1*a1+b1*b1);
+
+        const float chiSquare2 = squareDist2*invSigmaSquare;
+
+        if(chiSquare2>th)
+            bIn = false;
+        else
+            score += thScore - chiSquare2;
+
+        if(bIn)
+            vbMatchesInliers[i]=true;
+        else
+            vbMatchesInliers[i]=false;
+    }
+
+    return score;
+}
+
+bool Initializer::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)
+{
+    int N=0;
+    for(size_t i=0, iend = vbMatchesInliers.size() ; i<iend; i++)
+        if(vbMatchesInliers[i])
+            N++;
+
+    // Compute Essential Matrix from Fundamental Matrix
+    cv::Mat E21 = K.t()*F21*K;
+
+    cv::Mat R1, R2, t;
+
+    // Recover the 4 motion hypotheses
+    DecomposeE(E21,R1,R2,t);  
+
+    cv::Mat t1=t;
+    cv::Mat t2=-t;
+
+    // Reconstruct with the 4 hyphoteses and check
+    vector<cv::Point3f> vP3D1, vP3D2, vP3D3, vP3D4;
+    vector<bool> vbTriangulated1,vbTriangulated2,vbTriangulated3, vbTriangulated4;
+    float parallax1,parallax2, parallax3, parallax4;
+
+    int nGood1 = CheckRT(R1,t1,mvKeys1,mvKeys2,mvMatches12,vbMatchesInliers,K, vP3D1, 4.0*mSigma2, vbTriangulated1, parallax1);
+    int nGood2 = CheckRT(R2,t1,mvKeys1,mvKeys2,mvMatches12,vbMatchesInliers,K, vP3D2, 4.0*mSigma2, vbTriangulated2, parallax2);
+    int nGood3 = CheckRT(R1,t2,mvKeys1,mvKeys2,mvMatches12,vbMatchesInliers,K, vP3D3, 4.0*mSigma2, vbTriangulated3, parallax3);
+    int nGood4 = CheckRT(R2,t2,mvKeys1,mvKeys2,mvMatches12,vbMatchesInliers,K, vP3D4, 4.0*mSigma2, vbTriangulated4, parallax4);
+
+    int maxGood = max(nGood1,max(nGood2,max(nGood3,nGood4)));
+
+    R21 = cv::Mat();
+    t21 = cv::Mat();
+
+    int nMinGood = max(static_cast<int>(0.9*N),minTriangulated);
+
+    int nsimilar = 0;
+    if(nGood1>0.7*maxGood)
+        nsimilar++;
+    if(nGood2>0.7*maxGood)
+        nsimilar++;
+    if(nGood3>0.7*maxGood)
+        nsimilar++;
+    if(nGood4>0.7*maxGood)
+        nsimilar++;
+
+    // If there is not a clear winner or not enough triangulated points reject initialization
+    if(maxGood<nMinGood || nsimilar>1)
+    {
+        return false;
+    }
+
+    // If best reconstruction has enough parallax initialize
+    if(maxGood==nGood1)
+    {
+        if(parallax1>minParallax)
+        {
+            vP3D = vP3D1;
+            vbTriangulated = vbTriangulated1;
+
+            R1.copyTo(R21);
+            t1.copyTo(t21);
+            return true;
+        }
+    }else if(maxGood==nGood2)
+    {
+        if(parallax2>minParallax)
+        {
+            vP3D = vP3D2;
+            vbTriangulated = vbTriangulated2;
+
+            R2.copyTo(R21);
+            t1.copyTo(t21);
+            return true;
+        }
+    }else if(maxGood==nGood3)
+    {
+        if(parallax3>minParallax)
+        {
+            vP3D = vP3D3;
+            vbTriangulated = vbTriangulated3;
+
+            R1.copyTo(R21);
+            t2.copyTo(t21);
+            return true;
+        }
+    }else if(maxGood==nGood4)
+    {
+        if(parallax4>minParallax)
+        {
+            vP3D = vP3D4;
+            vbTriangulated = vbTriangulated4;
+
+            R2.copyTo(R21);
+            t2.copyTo(t21);
+            return true;
+        }
+    }
+
+    return false;
+}
+
+bool Initializer::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)
+{
+    int N=0;
+    for(size_t i=0, iend = vbMatchesInliers.size() ; i<iend; i++)
+        if(vbMatchesInliers[i])
+            N++;
+
+    // We recover 8 motion hypotheses using the method of Faugeras et al.
+    // Motion and structure from motion in a piecewise planar environment.
+    // International Journal of Pattern Recognition and Artificial Intelligence, 1988
+    cv::Mat invK = K.inv();
+    cv::Mat A = invK*H21*K;
+
+    cv::Mat U,w,Vt,V;
+    cv::SVD::compute(A,w,U,Vt,cv::SVD::FULL_UV);
+    V=Vt.t();
+
+    float s = cv::determinant(U)*cv::determinant(Vt);
+
+    float d1 = w.at<float>(0);
+    float d2 = w.at<float>(1);
+    float d3 = w.at<float>(2);
+
+    if(d1/d2<1.00001 || d2/d3<1.00001)
+    {
+        return false;
+    }
+
+    vector<cv::Mat> vR, vt, vn;
+    vR.reserve(8);
+    vt.reserve(8);
+    vn.reserve(8);
+
+    //n'=[x1 0 x3] 4 posibilities e1=e3=1, e1=1 e3=-1, e1=-1 e3=1, e1=e3=-1
+    float aux1 = sqrt((d1*d1-d2*d2)/(d1*d1-d3*d3));
+    float aux3 = sqrt((d2*d2-d3*d3)/(d1*d1-d3*d3));
+    float x1[] = {aux1,aux1,-aux1,-aux1};
+    float x3[] = {aux3,-aux3,aux3,-aux3};
+
+    //case d'=d2
+    float aux_stheta = sqrt((d1*d1-d2*d2)*(d2*d2-d3*d3))/((d1+d3)*d2);
+
+    float ctheta = (d2*d2+d1*d3)/((d1+d3)*d2);
+    float stheta[] = {aux_stheta, -aux_stheta, -aux_stheta, aux_stheta};
+
+    for(int i=0; i<4; i++)
+    {
+        cv::Mat Rp=cv::Mat::eye(3,3,CV_32F);
+        Rp.at<float>(0,0)=ctheta;
+        Rp.at<float>(0,2)=-stheta[i];
+        Rp.at<float>(2,0)=stheta[i];
+        Rp.at<float>(2,2)=ctheta;
+
+        cv::Mat R = s*U*Rp*Vt;
+        vR.push_back(R);
+
+        cv::Mat tp(3,1,CV_32F);
+        tp.at<float>(0)=x1[i];
+        tp.at<float>(1)=0;
+        tp.at<float>(2)=-x3[i];
+        tp*=d1-d3;
+
+        cv::Mat t = U*tp;
+        vt.push_back(t/cv::norm(t));
+
+        cv::Mat np(3,1,CV_32F);
+        np.at<float>(0)=x1[i];
+        np.at<float>(1)=0;
+        np.at<float>(2)=x3[i];
+
+        cv::Mat n = V*np;
+        if(n.at<float>(2)<0)
+            n=-n;
+        vn.push_back(n);
+    }
+
+    //case d'=-d2
+    float aux_sphi = sqrt((d1*d1-d2*d2)*(d2*d2-d3*d3))/((d1-d3)*d2);
+
+    float cphi = (d1*d3-d2*d2)/((d1-d3)*d2);
+    float sphi[] = {aux_sphi, -aux_sphi, -aux_sphi, aux_sphi};
+
+    for(int i=0; i<4; i++)
+    {
+        cv::Mat Rp=cv::Mat::eye(3,3,CV_32F);
+        Rp.at<float>(0,0)=cphi;
+        Rp.at<float>(0,2)=sphi[i];
+        Rp.at<float>(1,1)=-1;
+        Rp.at<float>(2,0)=sphi[i];
+        Rp.at<float>(2,2)=-cphi;
+
+        cv::Mat R = s*U*Rp*Vt;
+        vR.push_back(R);
+
+        cv::Mat tp(3,1,CV_32F);
+        tp.at<float>(0)=x1[i];
+        tp.at<float>(1)=0;
+        tp.at<float>(2)=x3[i];
+        tp*=d1+d3;
+
+        cv::Mat t = U*tp;
+        vt.push_back(t/cv::norm(t));
+
+        cv::Mat np(3,1,CV_32F);
+        np.at<float>(0)=x1[i];
+        np.at<float>(1)=0;
+        np.at<float>(2)=x3[i];
+
+        cv::Mat n = V*np;
+        if(n.at<float>(2)<0)
+            n=-n;
+        vn.push_back(n);
+    }
+
+
+    int bestGood = 0;
+    int secondBestGood = 0;    
+    int bestSolutionIdx = -1;
+    float bestParallax = -1;
+    vector<cv::Point3f> bestP3D;
+    vector<bool> bestTriangulated;
+
+    // Instead of applying the visibility constraints proposed in the Faugeras' paper (which could fail for points seen with low parallax)
+    // We reconstruct all hypotheses and check in terms of triangulated points and parallax
+    for(size_t i=0; i<8; i++)
+    {
+        float parallaxi;
+        vector<cv::Point3f> vP3Di;
+        vector<bool> vbTriangulatedi;
+        int nGood = CheckRT(vR[i],vt[i],mvKeys1,mvKeys2,mvMatches12,vbMatchesInliers,K,vP3Di, 4.0*mSigma2, vbTriangulatedi, parallaxi);
+
+        if(nGood>bestGood)
+        {
+            secondBestGood = bestGood;
+            bestGood = nGood;
+            bestSolutionIdx = i;
+            bestParallax = parallaxi;
+            bestP3D = vP3Di;
+            bestTriangulated = vbTriangulatedi;
+        }
+        else if(nGood>secondBestGood)
+        {
+            secondBestGood = nGood;
+        }
+    }
+
+
+    if(secondBestGood<0.75*bestGood && bestParallax>=minParallax && bestGood>minTriangulated && bestGood>0.9*N)
+    {
+        vR[bestSolutionIdx].copyTo(R21);
+        vt[bestSolutionIdx].copyTo(t21);
+        vP3D = bestP3D;
+        vbTriangulated = bestTriangulated;
+
+        return true;
+    }
+
+    return false;
+}
+
+void Initializer::Triangulate(const cv::KeyPoint &kp1, const cv::KeyPoint &kp2, const cv::Mat &P1, const cv::Mat &P2, cv::Mat &x3D)
+{
+    cv::Mat A(4,4,CV_32F);
+
+    A.row(0) = kp1.pt.x*P1.row(2)-P1.row(0);
+    A.row(1) = kp1.pt.y*P1.row(2)-P1.row(1);
+    A.row(2) = kp2.pt.x*P2.row(2)-P2.row(0);
+    A.row(3) = kp2.pt.y*P2.row(2)-P2.row(1);
+
+    cv::Mat u,w,vt;
+    cv::SVD::compute(A,w,u,vt,cv::SVD::MODIFY_A| cv::SVD::FULL_UV);
+    x3D = vt.row(3).t();
+    x3D = x3D.rowRange(0,3)/x3D.at<float>(3);
+}
+
+void Initializer::Normalize(const vector<cv::KeyPoint> &vKeys, vector<cv::Point2f> &vNormalizedPoints, cv::Mat &T)
+{
+    float meanX = 0;
+    float meanY = 0;
+    const int N = vKeys.size();
+
+    vNormalizedPoints.resize(N);
+
+    for(int i=0; i<N; i++)
+    {
+        meanX += vKeys[i].pt.x;
+        meanY += vKeys[i].pt.y;
+    }
+
+    meanX = meanX/N;
+    meanY = meanY/N;
+
+    float meanDevX = 0;
+    float meanDevY = 0;
+
+    for(int i=0; i<N; i++)
+    {
+        vNormalizedPoints[i].x = vKeys[i].pt.x - meanX;
+        vNormalizedPoints[i].y = vKeys[i].pt.y - meanY;
+
+        meanDevX += fabs(vNormalizedPoints[i].x);
+        meanDevY += fabs(vNormalizedPoints[i].y);
+    }
+
+    meanDevX = meanDevX/N;
+    meanDevY = meanDevY/N;
+
+    float sX = 1.0/meanDevX;
+    float sY = 1.0/meanDevY;
+
+    for(int i=0; i<N; i++)
+    {
+        vNormalizedPoints[i].x = vNormalizedPoints[i].x * sX;
+        vNormalizedPoints[i].y = vNormalizedPoints[i].y * sY;
+    }
+
+    T = cv::Mat::eye(3,3,CV_32F);
+    T.at<float>(0,0) = sX;
+    T.at<float>(1,1) = sY;
+    T.at<float>(0,2) = -meanX*sX;
+    T.at<float>(1,2) = -meanY*sY;
+}
+
+
+int Initializer::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> &vbMatchesInliers,
+                       const cv::Mat &K, vector<cv::Point3f> &vP3D, float th2, vector<bool> &vbGood, float &parallax)
+{
+    vbGood = vector<bool>(vKeys1.size(),false);
+    vP3D.resize(vKeys1.size());
+
+    vector<float> vCosParallax;
+    vCosParallax.reserve(vKeys1.size());
+
+    // Camera 1 Projection Matrix K[I|0]
+    cv::Mat P1(3,4,CV_32F,cv::Scalar(0));
+    K.copyTo(P1.rowRange(0,3).colRange(0,3));
+
+    cv::Mat O1 = cv::Mat::zeros(3,1,CV_32F);
+
+    // Camera 2 Projection Matrix K[R|t]
+    cv::Mat P2(3,4,CV_32F);
+    R.copyTo(P2.rowRange(0,3).colRange(0,3));
+    t.copyTo(P2.rowRange(0,3).col(3));
+    P2 = K*P2;
+
+    cv::Mat O2 = -R.t()*t;
+
+    int nGood=0;
+
+    for(size_t i=0, iend=vMatches12.size();i<iend;i++)
+    {
+        if(!vbMatchesInliers[i])
+            continue;
+
+        const cv::KeyPoint &kp1 = vKeys1[vMatches12[i].first];
+        const cv::KeyPoint &kp2 = vKeys2[vMatches12[i].second];
+        cv::Mat p3dC1;
+
+        Triangulate(kp1,kp2,P1,P2,p3dC1);
+
+        if(!isfinite(p3dC1.at<float>(0)) || !isfinite(p3dC1.at<float>(1)) || !isfinite(p3dC1.at<float>(2)))
+        {
+            vbGood[vMatches12[i].first]=false;
+            continue;
+        }
+
+        // Check parallax
+        cv::Mat normal1 = p3dC1 - O1;
+        float dist1 = cv::norm(normal1);
+
+        cv::Mat normal2 = p3dC1 - O2;
+        float dist2 = cv::norm(normal2);
+
+        float cosParallax = normal1.dot(normal2)/(dist1*dist2);
+
+        // Check depth in front of first camera (only if enough parallax, as "infinite" points can easily go to negative depth)
+        if(p3dC1.at<float>(2)<=0 && cosParallax<0.99998)
+            continue;
+
+        // Check depth in front of second camera (only if enough parallax, as "infinite" points can easily go to negative depth)
+        cv::Mat p3dC2 = R*p3dC1+t;
+
+        if(p3dC2.at<float>(2)<=0 && cosParallax<0.99998)
+            continue;
+
+        // Check reprojection error in first image
+        cv::Point2f uv1 = mpCamera->project(p3dC1);
+        float squareError1 = (uv1.x-kp1.pt.x)*(uv1.x-kp1.pt.x)+(uv1.y-kp1.pt.y)*(uv1.y-kp1.pt.y);
+
+        if(squareError1>th2)
+            continue;
+
+        // Check reprojection error in second image
+        cv::Point2f uv2 = mpCamera->project(p3dC2);
+        float squareError2 = (uv2.x-kp2.pt.x)*(uv2.x-kp2.pt.x)+(uv2.y-kp2.pt.y)*(uv2.y-kp2.pt.y);
+
+        if(squareError2>th2)
+            continue;
+
+        vCosParallax.push_back(cosParallax);
+        vP3D[vMatches12[i].first] = cv::Point3f(p3dC1.at<float>(0),p3dC1.at<float>(1),p3dC1.at<float>(2));
+        nGood++;
+
+        if(cosParallax<0.99998)
+            vbGood[vMatches12[i].first]=true;
+    }
+
+    if(nGood>0)
+    {
+        sort(vCosParallax.begin(),vCosParallax.end());
+
+        size_t idx = min(50,int(vCosParallax.size()-1));
+        parallax = acos(vCosParallax[idx])*180/CV_PI;
+    }
+    else
+        parallax=0;
+
+    return nGood;
+}
+
+void Initializer::DecomposeE(const cv::Mat &E, cv::Mat &R1, cv::Mat &R2, cv::Mat &t)
+{
+    cv::Mat u,w,vt;
+    cv::SVD::compute(E,w,u,vt);
+
+    u.col(2).copyTo(t);
+    t=t/cv::norm(t);
+
+    cv::Mat W(3,3,CV_32F,cv::Scalar(0));
+    W.at<float>(0,1)=-1;
+    W.at<float>(1,0)=1;
+    W.at<float>(2,2)=1;
+
+    R1 = u*W*vt;
+    if(cv::determinant(R1)<0)
+        R1=-R1;
+
+    R2 = u*W.t()*vt;
+    if(cv::determinant(R2)<0)
+        R2=-R2;
+}
+
+} //namespace ORB_SLAM

src/KeyFrameDatabase.cc

@@ -0,0 +1,857 @@
+/**
+* This file is part of ORB-SLAM3
+*
+* Copyright (C) 2017-2020 Carlos Campos, Richard Elvira, Juan J. Gómez Rodríguez, José M.M. Montiel and Juan D. Tardós, University of Zaragoza.
+* Copyright (C) 2014-2016 Raúl Mur-Artal, José M.M. Montiel and Juan D. Tardós, University of Zaragoza.
+*
+* ORB-SLAM3 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-SLAM3 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-SLAM3.
+* If not, see <http://www.gnu.org/licenses/>.
+*/
+
+
+#include "KeyFrameDatabase.h"
+
+#include "KeyFrame.h"
+#include "Thirdparty/DBoW2/DBoW2/BowVector.h"
+
+#include<mutex>
+
+using namespace std;
+
+namespace ORB_SLAM3
+{
+
+KeyFrameDatabase::KeyFrameDatabase (const ORBVocabulary &voc):
+    mpVoc(&voc)
+{
+    mvInvertedFile.resize(voc.size());
+}
+
+
+void KeyFrameDatabase::add(KeyFrame *pKF)
+{
+    unique_lock<mutex> lock(mMutex);
+
+    for(DBoW2::BowVector::const_iterator vit= pKF->mBowVec.begin(), vend=pKF->mBowVec.end(); vit!=vend; vit++)
+        mvInvertedFile[vit->first].push_back(pKF);
+}
+
+void KeyFrameDatabase::erase(KeyFrame* pKF)
+{
+    unique_lock<mutex> lock(mMutex);
+
+    // Erase elements in the Inverse File for the entry
+    for(DBoW2::BowVector::const_iterator vit=pKF->mBowVec.begin(), vend=pKF->mBowVec.end(); vit!=vend; vit++)
+    {
+        // List of keyframes that share the word
+        list<KeyFrame*> &lKFs =   mvInvertedFile[vit->first];
+
+        for(list<KeyFrame*>::iterator lit=lKFs.begin(), lend= lKFs.end(); lit!=lend; lit++)
+        {
+            if(pKF==*lit)
+            {
+                lKFs.erase(lit);
+                break;
+            }
+        }
+    }
+}
+
+void KeyFrameDatabase::clear()
+{
+    mvInvertedFile.clear();
+    mvInvertedFile.resize(mpVoc->size());
+}
+
+void KeyFrameDatabase::clearMap(Map* pMap)
+{
+    unique_lock<mutex> lock(mMutex);
+
+    // Erase elements in the Inverse File for the entry
+    for(std::vector<list<KeyFrame*> >::iterator vit=mvInvertedFile.begin(), vend=mvInvertedFile.end(); vit!=vend; vit++)
+    {
+        // List of keyframes that share the word
+        list<KeyFrame*> &lKFs =  *vit;
+
+        for(list<KeyFrame*>::iterator lit=lKFs.begin(), lend= lKFs.end(); lit!=lend;)
+        {
+            KeyFrame* pKFi = *lit;
+            if(pMap == pKFi->GetMap())
+            {
+                lit = lKFs.erase(lit);
+                // Dont delete the KF because the class Map clean all the KF when it is destroyed
+            }
+            else
+            {
+                ++lit;
+            }
+        }
+    }
+}
+
+vector<KeyFrame*> KeyFrameDatabase::DetectLoopCandidates(KeyFrame* pKF, float minScore)
+{
+    set<KeyFrame*> spConnectedKeyFrames = pKF->GetConnectedKeyFrames();
+    list<KeyFrame*> lKFsSharingWords;
+
+    // Search all keyframes that share a word with current keyframes
+    // Discard keyframes connected to the query keyframe
+    {
+        unique_lock<mutex> lock(mMutex);
+
+        for(DBoW2::BowVector::const_iterator vit=pKF->mBowVec.begin(), vend=pKF->mBowVec.end(); vit != vend; vit++)
+        {
+            list<KeyFrame*> &lKFs =   mvInvertedFile[vit->first];
+
+            for(list<KeyFrame*>::iterator lit=lKFs.begin(), lend= lKFs.end(); lit!=lend; lit++)
+            {
+                KeyFrame* pKFi=*lit;
+                if(pKFi->GetMap()==pKF->GetMap()) // For consider a loop candidate it a candidate it must be in the same map
+                {
+                    if(pKFi->mnLoopQuery!=pKF->mnId)
+                    {
+                        pKFi->mnLoopWords=0;
+                        if(!spConnectedKeyFrames.count(pKFi))
+                        {
+                            pKFi->mnLoopQuery=pKF->mnId;
+                            lKFsSharingWords.push_back(pKFi);
+                        }
+                    }
+                    pKFi->mnLoopWords++;
+                }
+
+
+            }
+        }
+    }
+
+    if(lKFsSharingWords.empty())
+        return vector<KeyFrame*>();
+
+    list<pair<float,KeyFrame*> > lScoreAndMatch;
+
+    // Only compare against those keyframes that share enough words
+    int maxCommonWords=0;
+    for(list<KeyFrame*>::iterator lit=lKFsSharingWords.begin(), lend= lKFsSharingWords.end(); lit!=lend; lit++)
+    {
+        if((*lit)->mnLoopWords>maxCommonWords)
+            maxCommonWords=(*lit)->mnLoopWords;
+    }
+
+    int minCommonWords = maxCommonWords*0.8f;
+
+    int nscores=0;
+
+    // Compute similarity score. Retain the matches whose score is higher than minScore
+    for(list<KeyFrame*>::iterator lit=lKFsSharingWords.begin(), lend= lKFsSharingWords.end(); lit!=lend; lit++)
+    {
+        KeyFrame* pKFi = *lit;
+
+        if(pKFi->mnLoopWords>minCommonWords)
+        {
+            nscores++;
+
+            float si = mpVoc->score(pKF->mBowVec,pKFi->mBowVec);
+
+            pKFi->mLoopScore = si;
+            if(si>=minScore)
+                lScoreAndMatch.push_back(make_pair(si,pKFi));
+        }
+    }
+
+    if(lScoreAndMatch.empty())
+        return vector<KeyFrame*>();
+
+    list<pair<float,KeyFrame*> > lAccScoreAndMatch;
+    float bestAccScore = minScore;
+
+    // Lets now accumulate score by covisibility
+    for(list<pair<float,KeyFrame*> >::iterator it=lScoreAndMatch.begin(), itend=lScoreAndMatch.end(); it!=itend; it++)
+    {
+        KeyFrame* pKFi = it->second;
+        vector<KeyFrame*> vpNeighs = pKFi->GetBestCovisibilityKeyFrames(10);
+
+        float bestScore = it->first;
+        float accScore = it->first;
+        KeyFrame* pBestKF = pKFi;
+        for(vector<KeyFrame*>::iterator vit=vpNeighs.begin(), vend=vpNeighs.end(); vit!=vend; vit++)
+        {
+            KeyFrame* pKF2 = *vit;
+            if(pKF2->mnLoopQuery==pKF->mnId && pKF2->mnLoopWords>minCommonWords)
+            {
+                accScore+=pKF2->mLoopScore;
+                if(pKF2->mLoopScore>bestScore)
+                {
+                    pBestKF=pKF2;
+                    bestScore = pKF2->mLoopScore;
+                }
+            }
+        }
+
+        lAccScoreAndMatch.push_back(make_pair(accScore,pBestKF));
+        if(accScore>bestAccScore)
+            bestAccScore=accScore;
+    }
+
+    // Return all those keyframes with a score higher than 0.75*bestScore
+    float minScoreToRetain = 0.75f*bestAccScore;
+
+    set<KeyFrame*> spAlreadyAddedKF;
+    vector<KeyFrame*> vpLoopCandidates;
+    vpLoopCandidates.reserve(lAccScoreAndMatch.size());
+
+    for(list<pair<float,KeyFrame*> >::iterator it=lAccScoreAndMatch.begin(), itend=lAccScoreAndMatch.end(); it!=itend; it++)
+    {
+        if(it->first>minScoreToRetain)
+        {
+            KeyFrame* pKFi = it->second;
+            if(!spAlreadyAddedKF.count(pKFi))
+            {
+                vpLoopCandidates.push_back(pKFi);
+                spAlreadyAddedKF.insert(pKFi);
+            }
+        }
+    }
+
+
+    return vpLoopCandidates;
+}
+
+void KeyFrameDatabase::DetectCandidates(KeyFrame* pKF, float minScore,vector<KeyFrame*>& vpLoopCand, vector<KeyFrame*>& vpMergeCand)
+{
+    set<KeyFrame*> spConnectedKeyFrames = pKF->GetConnectedKeyFrames();
+    list<KeyFrame*> lKFsSharingWordsLoop,lKFsSharingWordsMerge;
+
+    // Search all keyframes that share a word with current keyframes
+    // Discard keyframes connected to the query keyframe
+    {
+        unique_lock<mutex> lock(mMutex);
+
+        for(DBoW2::BowVector::const_iterator vit=pKF->mBowVec.begin(), vend=pKF->mBowVec.end(); vit != vend; vit++)
+        {
+            list<KeyFrame*> &lKFs = mvInvertedFile[vit->first];
+
+            for(list<KeyFrame*>::iterator lit=lKFs.begin(), lend= lKFs.end(); lit!=lend; lit++)
+            {
+                KeyFrame* pKFi=*lit;
+                if(pKFi->GetMap()==pKF->GetMap()) // For consider a loop candidate it a candidate it must be in the same map
+                {
+                    if(pKFi->mnLoopQuery!=pKF->mnId)
+                    {
+                        pKFi->mnLoopWords=0;
+                        if(!spConnectedKeyFrames.count(pKFi))
+                        {
+                            pKFi->mnLoopQuery=pKF->mnId;
+                            lKFsSharingWordsLoop.push_back(pKFi);
+                        }
+                    }
+                    pKFi->mnLoopWords++;
+                }
+                else if(!pKFi->GetMap()->IsBad())
+                {
+                    if(pKFi->mnMergeQuery!=pKF->mnId)
+                    {
+                        pKFi->mnMergeWords=0;
+                        if(!spConnectedKeyFrames.count(pKFi))
+                        {
+                            pKFi->mnMergeQuery=pKF->mnId;
+                            lKFsSharingWordsMerge.push_back(pKFi);
+                        }
+                    }
+                    pKFi->mnMergeWords++;
+                }
+            }
+        }
+    }
+
+    if(lKFsSharingWordsLoop.empty() && lKFsSharingWordsMerge.empty())
+        return;
+
+    if(!lKFsSharingWordsLoop.empty())
+    {
+        list<pair<float,KeyFrame*> > lScoreAndMatch;
+
+        // Only compare against those keyframes that share enough words
+        int maxCommonWords=0;
+        for(list<KeyFrame*>::iterator lit=lKFsSharingWordsLoop.begin(), lend= lKFsSharingWordsLoop.end(); lit!=lend; lit++)
+        {
+            if((*lit)->mnLoopWords>maxCommonWords)
+                maxCommonWords=(*lit)->mnLoopWords;
+        }
+
+        int minCommonWords = maxCommonWords*0.8f;
+
+        int nscores=0;
+
+        // Compute similarity score. Retain the matches whose score is higher than minScore
+        for(list<KeyFrame*>::iterator lit=lKFsSharingWordsLoop.begin(), lend= lKFsSharingWordsLoop.end(); lit!=lend; lit++)
+        {
+            KeyFrame* pKFi = *lit;
+
+            if(pKFi->mnLoopWords>minCommonWords)
+            {
+                nscores++;
+
+                float si = mpVoc->score(pKF->mBowVec,pKFi->mBowVec);
+
+                pKFi->mLoopScore = si;
+                if(si>=minScore)
+                    lScoreAndMatch.push_back(make_pair(si,pKFi));
+            }
+        }
+
+        if(!lScoreAndMatch.empty())
+        {
+            list<pair<float,KeyFrame*> > lAccScoreAndMatch;
+            float bestAccScore = minScore;
+
+            // Lets now accumulate score by covisibility
+            for(list<pair<float,KeyFrame*> >::iterator it=lScoreAndMatch.begin(), itend=lScoreAndMatch.end(); it!=itend; it++)
+            {
+                KeyFrame* pKFi = it->second;
+                vector<KeyFrame*> vpNeighs = pKFi->GetBestCovisibilityKeyFrames(10);
+
+                float bestScore = it->first;
+                float accScore = it->first;
+                KeyFrame* pBestKF = pKFi;
+                for(vector<KeyFrame*>::iterator vit=vpNeighs.begin(), vend=vpNeighs.end(); vit!=vend; vit++)
+                {
+                    KeyFrame* pKF2 = *vit;
+                    if(pKF2->mnLoopQuery==pKF->mnId && pKF2->mnLoopWords>minCommonWords)
+                    {
+                        accScore+=pKF2->mLoopScore;
+                        if(pKF2->mLoopScore>bestScore)
+                        {
+                            pBestKF=pKF2;
+                            bestScore = pKF2->mLoopScore;
+                        }
+                    }
+                }
+
+                lAccScoreAndMatch.push_back(make_pair(accScore,pBestKF));
+                if(accScore>bestAccScore)
+                    bestAccScore=accScore;
+            }
+
+            // Return all those keyframes with a score higher than 0.75*bestScore
+            float minScoreToRetain = 0.75f*bestAccScore;
+
+            set<KeyFrame*> spAlreadyAddedKF;
+            vpLoopCand.reserve(lAccScoreAndMatch.size());
+
+            for(list<pair<float,KeyFrame*> >::iterator it=lAccScoreAndMatch.begin(), itend=lAccScoreAndMatch.end(); it!=itend; it++)
+            {
+                if(it->first>minScoreToRetain)
+                {
+                    KeyFrame* pKFi = it->second;
+                    if(!spAlreadyAddedKF.count(pKFi))
+                    {
+                        vpLoopCand.push_back(pKFi);
+                        spAlreadyAddedKF.insert(pKFi);
+                    }
+                }
+            }
+        }
+
+    }
+
+    if(!lKFsSharingWordsMerge.empty())
+    {
+        list<pair<float,KeyFrame*> > lScoreAndMatch;
+
+        // Only compare against those keyframes that share enough words
+        int maxCommonWords=0;
+        for(list<KeyFrame*>::iterator lit=lKFsSharingWordsMerge.begin(), lend=lKFsSharingWordsMerge.end(); lit!=lend; lit++)
+        {
+            if((*lit)->mnMergeWords>maxCommonWords)
+                maxCommonWords=(*lit)->mnMergeWords;
+        }
+
+        int minCommonWords = maxCommonWords*0.8f;
+
+        int nscores=0;
+
+        // Compute similarity score. Retain the matches whose score is higher than minScore
+        for(list<KeyFrame*>::iterator lit=lKFsSharingWordsMerge.begin(), lend=lKFsSharingWordsMerge.end(); lit!=lend; lit++)
+        {
+            KeyFrame* pKFi = *lit;
+
+            if(pKFi->mnMergeWords>minCommonWords)
+            {
+                nscores++;
+
+                float si = mpVoc->score(pKF->mBowVec,pKFi->mBowVec);
+
+                pKFi->mMergeScore = si;
+                if(si>=minScore)
+                    lScoreAndMatch.push_back(make_pair(si,pKFi));
+            }
+        }
+
+        if(!lScoreAndMatch.empty())
+        {
+            list<pair<float,KeyFrame*> > lAccScoreAndMatch;
+            float bestAccScore = minScore;
+
+            // Lets now accumulate score by covisibility
+            for(list<pair<float,KeyFrame*> >::iterator it=lScoreAndMatch.begin(), itend=lScoreAndMatch.end(); it!=itend; it++)
+            {
+                KeyFrame* pKFi = it->second;
+                vector<KeyFrame*> vpNeighs = pKFi->GetBestCovisibilityKeyFrames(10);
+
+                float bestScore = it->first;
+                float accScore = it->first;
+                KeyFrame* pBestKF = pKFi;
+                for(vector<KeyFrame*>::iterator vit=vpNeighs.begin(), vend=vpNeighs.end(); vit!=vend; vit++)
+                {
+                    KeyFrame* pKF2 = *vit;
+                    if(pKF2->mnMergeQuery==pKF->mnId && pKF2->mnMergeWords>minCommonWords)
+                    {
+                        accScore+=pKF2->mMergeScore;
+                        if(pKF2->mMergeScore>bestScore)
+                        {
+                            pBestKF=pKF2;
+                            bestScore = pKF2->mMergeScore;
+                        }
+                    }
+                }
+
+                lAccScoreAndMatch.push_back(make_pair(accScore,pBestKF));
+                if(accScore>bestAccScore)
+                    bestAccScore=accScore;
+            }
+
+            // Return all those keyframes with a score higher than 0.75*bestScore
+            float minScoreToRetain = 0.75f*bestAccScore;
+
+            set<KeyFrame*> spAlreadyAddedKF;
+            vpMergeCand.reserve(lAccScoreAndMatch.size());
+
+            for(list<pair<float,KeyFrame*> >::iterator it=lAccScoreAndMatch.begin(), itend=lAccScoreAndMatch.end(); it!=itend; it++)
+            {
+                if(it->first>minScoreToRetain)
+                {
+                    KeyFrame* pKFi = it->second;
+                    if(!spAlreadyAddedKF.count(pKFi))
+                    {
+                        vpMergeCand.push_back(pKFi);
+                        spAlreadyAddedKF.insert(pKFi);
+                    }
+                }
+            }
+        }
+
+    }
+
+    for(DBoW2::BowVector::const_iterator vit=pKF->mBowVec.begin(), vend=pKF->mBowVec.end(); vit != vend; vit++)
+    {
+        list<KeyFrame*> &lKFs = mvInvertedFile[vit->first];
+
+        for(list<KeyFrame*>::iterator lit=lKFs.begin(), lend= lKFs.end(); lit!=lend; lit++)
+        {
+            KeyFrame* pKFi=*lit;
+            pKFi->mnLoopQuery=-1;
+            pKFi->mnMergeQuery=-1;
+        }
+    }
+
+}
+
+void KeyFrameDatabase::DetectBestCandidates(KeyFrame *pKF, vector<KeyFrame*> &vpLoopCand, vector<KeyFrame*> &vpMergeCand, int nMinWords)
+{
+    list<KeyFrame*> lKFsSharingWords;
+    set<KeyFrame*> spConnectedKF;
+
+    // Search all keyframes that share a word with current frame
+    {
+        unique_lock<mutex> lock(mMutex);
+
+        spConnectedKF = pKF->GetConnectedKeyFrames();
+
+        for(DBoW2::BowVector::const_iterator vit=pKF->mBowVec.begin(), vend=pKF->mBowVec.end(); vit != vend; vit++)
+        {
+            list<KeyFrame*> &lKFs =   mvInvertedFile[vit->first];
+
+            for(list<KeyFrame*>::iterator lit=lKFs.begin(), lend= lKFs.end(); lit!=lend; lit++)
+            {
+                KeyFrame* pKFi=*lit;
+                if(spConnectedKF.find(pKFi) != spConnectedKF.end())
+                {
+                    continue;
+                }
+                if(pKFi->mnPlaceRecognitionQuery!=pKF->mnId)
+                {
+                    pKFi->mnPlaceRecognitionWords=0;
+                    pKFi->mnPlaceRecognitionQuery=pKF->mnId;
+                    lKFsSharingWords.push_back(pKFi);
+                }
+               pKFi->mnPlaceRecognitionWords++;
+
+            }
+        }
+    }
+    if(lKFsSharingWords.empty())
+        return;
+
+    // Only compare against those keyframes that share enough words
+    int maxCommonWords=0;
+    for(list<KeyFrame*>::iterator lit=lKFsSharingWords.begin(), lend= lKFsSharingWords.end(); lit!=lend; lit++)
+    {
+        if((*lit)->mnPlaceRecognitionWords>maxCommonWords)
+            maxCommonWords=(*lit)->mnPlaceRecognitionWords;
+    }
+
+    int minCommonWords = maxCommonWords*0.8f;
+
+    if(minCommonWords < nMinWords)
+    {
+        minCommonWords = nMinWords;
+    }
+
+    list<pair<float,KeyFrame*> > lScoreAndMatch;
+
+    int nscores=0;
+
+    // Compute similarity score.
+    for(list<KeyFrame*>::iterator lit=lKFsSharingWords.begin(), lend= lKFsSharingWords.end(); lit!=lend; lit++)
+    {
+        KeyFrame* pKFi = *lit;
+
+        if(pKFi->mnPlaceRecognitionWords>minCommonWords)
+        {
+            nscores++;
+            float si = mpVoc->score(pKF->mBowVec,pKFi->mBowVec);
+            pKFi->mPlaceRecognitionScore=si;
+            lScoreAndMatch.push_back(make_pair(si,pKFi));
+        }
+    }
+
+    if(lScoreAndMatch.empty())
+        return;
+
+    list<pair<float,KeyFrame*> > lAccScoreAndMatch;
+    float bestAccScore = 0;
+
+    // Lets now accumulate score by covisibility
+    for(list<pair<float,KeyFrame*> >::iterator it=lScoreAndMatch.begin(), itend=lScoreAndMatch.end(); it!=itend; it++)
+    {
+        KeyFrame* pKFi = it->second;
+        vector<KeyFrame*> vpNeighs = pKFi->GetBestCovisibilityKeyFrames(10);
+
+        float bestScore = it->first;
+        float accScore = bestScore;
+        KeyFrame* pBestKF = pKFi;
+        for(vector<KeyFrame*>::iterator vit=vpNeighs.begin(), vend=vpNeighs.end(); vit!=vend; vit++)
+        {
+            KeyFrame* pKF2 = *vit;
+            if(pKF2->mnPlaceRecognitionQuery!=pKF->mnId)
+                continue;
+
+            accScore+=pKF2->mPlaceRecognitionScore;
+            if(pKF2->mPlaceRecognitionScore>bestScore)
+            {
+                pBestKF=pKF2;
+                bestScore = pKF2->mPlaceRecognitionScore;
+            }
+
+        }
+        lAccScoreAndMatch.push_back(make_pair(accScore,pBestKF));
+        if(accScore>bestAccScore)
+            bestAccScore=accScore;
+    }
+
+    // Return all those keyframes with a score higher than 0.75*bestScore
+    float minScoreToRetain = 0.75f*bestAccScore;
+    set<KeyFrame*> spAlreadyAddedKF;
+    vpLoopCand.reserve(lAccScoreAndMatch.size());
+    vpMergeCand.reserve(lAccScoreAndMatch.size());
+    for(list<pair<float,KeyFrame*> >::iterator it=lAccScoreAndMatch.begin(), itend=lAccScoreAndMatch.end(); it!=itend; it++)
+    {
+        const float &si = it->first;
+        if(si>minScoreToRetain)
+        {
+            KeyFrame* pKFi = it->second;
+            if(!spAlreadyAddedKF.count(pKFi))
+            {
+                if(pKF->GetMap() == pKFi->GetMap())
+                {
+                    vpLoopCand.push_back(pKFi);
+                }
+                else
+                {
+                    vpMergeCand.push_back(pKFi);
+                }
+                spAlreadyAddedKF.insert(pKFi);
+            }
+        }
+    }
+}
+
+bool compFirst(const pair<float, KeyFrame*> & a, const pair<float, KeyFrame*> & b)
+{
+    return a.first > b.first;
+}
+
+
+void KeyFrameDatabase::DetectNBestCandidates(KeyFrame *pKF, vector<KeyFrame*> &vpLoopCand, vector<KeyFrame*> &vpMergeCand, int nNumCandidates)
+{
+    list<KeyFrame*> lKFsSharingWords;
+    set<KeyFrame*> spConnectedKF;
+
+    // Search all keyframes that share a word with current frame
+    {
+        unique_lock<mutex> lock(mMutex);
+
+        spConnectedKF = pKF->GetConnectedKeyFrames();
+
+        for(DBoW2::BowVector::const_iterator vit=pKF->mBowVec.begin(), vend=pKF->mBowVec.end(); vit != vend; vit++)
+        {
+            list<KeyFrame*> &lKFs =   mvInvertedFile[vit->first];
+
+            for(list<KeyFrame*>::iterator lit=lKFs.begin(), lend= lKFs.end(); lit!=lend; lit++)
+            {
+                KeyFrame* pKFi=*lit;
+                if(pKFi->mnPlaceRecognitionQuery!=pKF->mnId)
+                {
+                    pKFi->mnPlaceRecognitionWords=0;
+                    if(!spConnectedKF.count(pKFi))
+                    {
+
+                        pKFi->mnPlaceRecognitionQuery=pKF->mnId;
+                        lKFsSharingWords.push_back(pKFi);
+                    }
+                }
+                pKFi->mnPlaceRecognitionWords++;
+
+            }
+        }
+    }
+    if(lKFsSharingWords.empty())
+        return;
+
+    // Only compare against those keyframes that share enough words
+    int maxCommonWords=0;
+    for(list<KeyFrame*>::iterator lit=lKFsSharingWords.begin(), lend= lKFsSharingWords.end(); lit!=lend; lit++)
+    {
+        if((*lit)->mnPlaceRecognitionWords>maxCommonWords)
+            maxCommonWords=(*lit)->mnPlaceRecognitionWords;
+    }
+
+    int minCommonWords = maxCommonWords*0.8f;
+
+    list<pair<float,KeyFrame*> > lScoreAndMatch;
+
+    int nscores=0;
+
+    // Compute similarity score.
+    for(list<KeyFrame*>::iterator lit=lKFsSharingWords.begin(), lend= lKFsSharingWords.end(); lit!=lend; lit++)
+    {
+        KeyFrame* pKFi = *lit;
+
+        if(pKFi->mnPlaceRecognitionWords>minCommonWords)
+        {
+            nscores++;
+            float si = mpVoc->score(pKF->mBowVec,pKFi->mBowVec);
+            pKFi->mPlaceRecognitionScore=si;
+            lScoreAndMatch.push_back(make_pair(si,pKFi));
+        }
+    }
+
+    if(lScoreAndMatch.empty())
+        return;
+
+    list<pair<float,KeyFrame*> > lAccScoreAndMatch;
+    float bestAccScore = 0;
+
+    // Lets now accumulate score by covisibility
+    for(list<pair<float,KeyFrame*> >::iterator it=lScoreAndMatch.begin(), itend=lScoreAndMatch.end(); it!=itend; it++)
+    {
+        KeyFrame* pKFi = it->second;
+        vector<KeyFrame*> vpNeighs = pKFi->GetBestCovisibilityKeyFrames(10);
+
+        float bestScore = it->first;
+        float accScore = bestScore;
+        KeyFrame* pBestKF = pKFi;
+        for(vector<KeyFrame*>::iterator vit=vpNeighs.begin(), vend=vpNeighs.end(); vit!=vend; vit++)
+        {
+            KeyFrame* pKF2 = *vit;
+            if(pKF2->mnPlaceRecognitionQuery!=pKF->mnId)
+                continue;
+
+            accScore+=pKF2->mPlaceRecognitionScore;
+            if(pKF2->mPlaceRecognitionScore>bestScore)
+            {
+                pBestKF=pKF2;
+                bestScore = pKF2->mPlaceRecognitionScore;
+            }
+
+        }
+        lAccScoreAndMatch.push_back(make_pair(accScore,pBestKF));
+        if(accScore>bestAccScore)
+            bestAccScore=accScore;
+    }
+
+    lAccScoreAndMatch.sort(compFirst);
+
+    vpLoopCand.reserve(nNumCandidates);
+    vpMergeCand.reserve(nNumCandidates);
+    set<KeyFrame*> spAlreadyAddedKF;
+    int i = 0;
+    list<pair<float,KeyFrame*> >::iterator it=lAccScoreAndMatch.begin();
+    while(i < lAccScoreAndMatch.size() && (vpLoopCand.size() < nNumCandidates || vpMergeCand.size() < nNumCandidates))
+    {
+        KeyFrame* pKFi = it->second;
+        if(pKFi->isBad())
+            continue;
+
+        if(!spAlreadyAddedKF.count(pKFi))
+        {
+            if(pKF->GetMap() == pKFi->GetMap() && vpLoopCand.size() < nNumCandidates)
+            {
+                vpLoopCand.push_back(pKFi);
+            }
+            else if(pKF->GetMap() != pKFi->GetMap() && vpMergeCand.size() < nNumCandidates && !pKFi->GetMap()->IsBad())
+            {
+                vpMergeCand.push_back(pKFi);
+            }
+            spAlreadyAddedKF.insert(pKFi);
+        }
+        i++;
+        it++;
+    }
+}
+
+
+vector<KeyFrame*> KeyFrameDatabase::DetectRelocalizationCandidates(Frame *F, Map* pMap)
+{
+    list<KeyFrame*> lKFsSharingWords;
+
+    // Search all keyframes that share a word with current frame
+    {
+        unique_lock<mutex> lock(mMutex);
+
+        for(DBoW2::BowVector::const_iterator vit=F->mBowVec.begin(), vend=F->mBowVec.end(); vit != vend; vit++)
+        {
+            list<KeyFrame*> &lKFs =   mvInvertedFile[vit->first];
+
+            for(list<KeyFrame*>::iterator lit=lKFs.begin(), lend= lKFs.end(); lit!=lend; lit++)
+            {
+                KeyFrame* pKFi=*lit;
+                if(pKFi->mnRelocQuery!=F->mnId)
+                {
+                    pKFi->mnRelocWords=0;
+                    pKFi->mnRelocQuery=F->mnId;
+                    lKFsSharingWords.push_back(pKFi);
+                }
+                pKFi->mnRelocWords++;
+            }
+        }
+    }
+    if(lKFsSharingWords.empty())
+        return vector<KeyFrame*>();
+
+    // Only compare against those keyframes that share enough words
+    int maxCommonWords=0;
+    for(list<KeyFrame*>::iterator lit=lKFsSharingWords.begin(), lend= lKFsSharingWords.end(); lit!=lend; lit++)
+    {
+        if((*lit)->mnRelocWords>maxCommonWords)
+            maxCommonWords=(*lit)->mnRelocWords;
+    }
+
+    int minCommonWords = maxCommonWords*0.8f;
+
+    list<pair<float,KeyFrame*> > lScoreAndMatch;
+
+    int nscores=0;
+
+    // Compute similarity score.
+    for(list<KeyFrame*>::iterator lit=lKFsSharingWords.begin(), lend= lKFsSharingWords.end(); lit!=lend; lit++)
+    {
+        KeyFrame* pKFi = *lit;
+
+        if(pKFi->mnRelocWords>minCommonWords)
+        {
+            nscores++;
+            float si = mpVoc->score(F->mBowVec,pKFi->mBowVec);
+            pKFi->mRelocScore=si;
+            lScoreAndMatch.push_back(make_pair(si,pKFi));
+        }
+    }
+
+    if(lScoreAndMatch.empty())
+        return vector<KeyFrame*>();
+
+    list<pair<float,KeyFrame*> > lAccScoreAndMatch;
+    float bestAccScore = 0;
+
+    // Lets now accumulate score by covisibility
+    for(list<pair<float,KeyFrame*> >::iterator it=lScoreAndMatch.begin(), itend=lScoreAndMatch.end(); it!=itend; it++)
+    {
+        KeyFrame* pKFi = it->second;
+        vector<KeyFrame*> vpNeighs = pKFi->GetBestCovisibilityKeyFrames(10);
+
+        float bestScore = it->first;
+        float accScore = bestScore;
+        KeyFrame* pBestKF = pKFi;
+        for(vector<KeyFrame*>::iterator vit=vpNeighs.begin(), vend=vpNeighs.end(); vit!=vend; vit++)
+        {
+            KeyFrame* pKF2 = *vit;
+            if(pKF2->mnRelocQuery!=F->mnId)
+                continue;
+
+            accScore+=pKF2->mRelocScore;
+            if(pKF2->mRelocScore>bestScore)
+            {
+                pBestKF=pKF2;
+                bestScore = pKF2->mRelocScore;
+            }
+
+        }
+        lAccScoreAndMatch.push_back(make_pair(accScore,pBestKF));
+        if(accScore>bestAccScore)
+            bestAccScore=accScore;
+    }
+
+    // Return all those keyframes with a score higher than 0.75*bestScore
+    float minScoreToRetain = 0.75f*bestAccScore;
+    set<KeyFrame*> spAlreadyAddedKF;
+    vector<KeyFrame*> vpRelocCandidates;
+    vpRelocCandidates.reserve(lAccScoreAndMatch.size());
+    for(list<pair<float,KeyFrame*> >::iterator it=lAccScoreAndMatch.begin(), itend=lAccScoreAndMatch.end(); it!=itend; it++)
+    {
+        const float &si = it->first;
+        if(si>minScoreToRetain)
+        {
+            KeyFrame* pKFi = it->second;
+            if (pKFi->GetMap() != pMap)
+                continue;
+            if(!spAlreadyAddedKF.count(pKFi))
+            {
+                vpRelocCandidates.push_back(pKFi);
+                spAlreadyAddedKF.insert(pKFi);
+            }
+        }
+    }
+
+    return vpRelocCandidates;
+}
+
+void KeyFrameDatabase::SetORBVocabulary(ORBVocabulary* pORBVoc)
+{
+    ORBVocabulary** ptr;
+    ptr = (ORBVocabulary**)( &mpVoc );
+    *ptr = pORBVoc;
+
+    mvInvertedFile.clear();
+    mvInvertedFile.resize(mpVoc->size());
+}
+
+} //namespace ORB_SLAM

src/Map.cc

@@ -0,0 +1,493 @@
+/**
+* This file is part of ORB-SLAM3
+*
+* Copyright (C) 2017-2020 Carlos Campos, Richard Elvira, Juan J. Gómez Rodríguez, José M.M. Montiel and Juan D. Tardós, University of Zaragoza.
+* Copyright (C) 2014-2016 Raúl Mur-Artal, José M.M. Montiel and Juan D. Tardós, University of Zaragoza.
+*
+* ORB-SLAM3 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-SLAM3 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-SLAM3.
+* If not, see <http://www.gnu.org/licenses/>.
+*/
+
+
+#include "Map.h"
+
+#include<mutex>
+
+namespace ORB_SLAM3
+{
+
+long unsigned int Map::nNextId=0;
+
+Map::Map():mnMaxKFid(0),mnBigChangeIdx(0), mbImuInitialized(false), mnMapChange(0), mpFirstRegionKF(static_cast<KeyFrame*>(NULL)),
+mbFail(false), mIsInUse(false), mHasTumbnail(false), mbBad(false), mnMapChangeNotified(0), mbIsInertial(false), mbIMU_BA1(false), mbIMU_BA2(false)
+{
+    mnId=nNextId++;
+    mThumbnail = static_cast<GLubyte*>(NULL);
+}
+
+Map::Map(int initKFid):mnInitKFid(initKFid), mnMaxKFid(initKFid),mnLastLoopKFid(initKFid), mnBigChangeIdx(0), mIsInUse(false),
+                       mHasTumbnail(false), mbBad(false), mbImuInitialized(false), mpFirstRegionKF(static_cast<KeyFrame*>(NULL)),
+                       mnMapChange(0), mbFail(false), mnMapChangeNotified(0), mbIsInertial(false), mbIMU_BA1(false), mbIMU_BA2(false)
+{
+    mnId=nNextId++;
+    mThumbnail = static_cast<GLubyte*>(NULL);
+}
+
+Map::~Map()
+{
+    //TODO: erase all points from memory
+    mspMapPoints.clear();
+
+    //TODO: erase all keyframes from memory
+    mspKeyFrames.clear();
+
+    if(mThumbnail)
+        delete mThumbnail;
+    mThumbnail = static_cast<GLubyte*>(NULL);
+
+    mvpReferenceMapPoints.clear();
+    mvpKeyFrameOrigins.clear();
+}
+
+void Map::AddKeyFrame(KeyFrame *pKF)
+{
+    unique_lock<mutex> lock(mMutexMap);
+    if(mspKeyFrames.empty()){
+        cout << "First KF:" << pKF->mnId << "; Map init KF:" << mnInitKFid << endl;
+        mnInitKFid = pKF->mnId;
+        mpKFinitial = pKF;
+        mpKFlowerID = pKF;
+    }
+    mspKeyFrames.insert(pKF);
+    if(pKF->mnId>mnMaxKFid)
+    {
+        mnMaxKFid=pKF->mnId;
+    }
+    if(pKF->mnId<mpKFlowerID->mnId)
+    {
+        mpKFlowerID = pKF;
+    }
+}
+
+void Map::AddMapPoint(MapPoint *pMP)
+{
+    unique_lock<mutex> lock(mMutexMap);
+    mspMapPoints.insert(pMP);
+}
+
+void Map::SetImuInitialized()
+{
+    unique_lock<mutex> lock(mMutexMap);
+    mbImuInitialized = true;
+}
+
+bool Map::isImuInitialized()
+{
+    unique_lock<mutex> lock(mMutexMap);
+    return mbImuInitialized;
+}
+
+void Map::EraseMapPoint(MapPoint *pMP)
+{
+    unique_lock<mutex> lock(mMutexMap);
+    mspMapPoints.erase(pMP);
+
+    // TODO: This only erase the pointer.
+    // Delete the MapPoint
+}
+
+void Map::EraseKeyFrame(KeyFrame *pKF)
+{
+    unique_lock<mutex> lock(mMutexMap);
+    mspKeyFrames.erase(pKF);
+    if(mspKeyFrames.size()>0)
+    {
+        if(pKF->mnId == mpKFlowerID->mnId)
+        {
+            vector<KeyFrame*> vpKFs = vector<KeyFrame*>(mspKeyFrames.begin(),mspKeyFrames.end());
+            sort(vpKFs.begin(),vpKFs.end(),KeyFrame::lId);
+            mpKFlowerID = vpKFs[0];
+        }
+    }
+    else
+    {
+        mpKFlowerID = 0;
+    }
+
+    // TODO: This only erase the pointer.
+    // Delete the MapPoint
+}
+
+void Map::SetReferenceMapPoints(const vector<MapPoint *> &vpMPs)
+{
+    unique_lock<mutex> lock(mMutexMap);
+    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);
+    return vector<KeyFrame*>(mspKeyFrames.begin(),mspKeyFrames.end());
+}
+
+vector<MapPoint*> Map::GetAllMapPoints()
+{
+    unique_lock<mutex> lock(mMutexMap);
+    return vector<MapPoint*>(mspMapPoints.begin(),mspMapPoints.end());
+}
+
+long unsigned int Map::MapPointsInMap()
+{
+    unique_lock<mutex> lock(mMutexMap);
+    return mspMapPoints.size();
+}
+
+long unsigned int Map::KeyFramesInMap()
+{
+    unique_lock<mutex> lock(mMutexMap);
+    return mspKeyFrames.size();
+}
+
+vector<MapPoint*> Map::GetReferenceMapPoints()
+{
+    unique_lock<mutex> lock(mMutexMap);
+    return mvpReferenceMapPoints;
+}
+
+long unsigned int Map::GetId()
+{
+    return mnId;
+}
+long unsigned int Map::GetInitKFid()
+{
+    unique_lock<mutex> lock(mMutexMap);
+    return mnInitKFid;
+}
+
+void Map::SetInitKFid(long unsigned int initKFif)
+{
+    unique_lock<mutex> lock(mMutexMap);
+    mnInitKFid = initKFif;
+}
+
+long unsigned int Map::GetMaxKFid()
+{
+    unique_lock<mutex> lock(mMutexMap);
+    return mnMaxKFid;
+}
+
+KeyFrame* Map::GetOriginKF()
+{
+    return mpKFinitial;
+}
+
+void Map::SetCurrentMap()
+{
+    mIsInUse = true;
+}
+
+void Map::SetStoredMap()
+{
+    mIsInUse = false;
+}
+
+void Map::clear()
+{
+//    for(set<MapPoint*>::iterator sit=mspMapPoints.begin(), send=mspMapPoints.end(); sit!=send; sit++)
+//        delete *sit;
+
+    for(set<KeyFrame*>::iterator sit=mspKeyFrames.begin(), send=mspKeyFrames.end(); sit!=send; sit++)
+    {
+        KeyFrame* pKF = *sit;
+        pKF->UpdateMap(static_cast<Map*>(NULL));
+//        delete *sit;
+    }
+
+    mspMapPoints.clear();
+    mspKeyFrames.clear();
+    mnMaxKFid = mnInitKFid;
+    mnLastLoopKFid = 0;
+    mbImuInitialized = false;
+    mvpReferenceMapPoints.clear();
+    mvpKeyFrameOrigins.clear();
+    mbIMU_BA1 = false;
+    mbIMU_BA2 = false;
+}
+
+bool Map::IsInUse()
+{
+    return mIsInUse;
+}
+
+void Map::SetBad()
+{
+    mbBad = true;
+}
+
+bool Map::IsBad()
+{
+    return mbBad;
+}
+
+void Map::RotateMap(const cv::Mat &R)
+{
+    unique_lock<mutex> lock(mMutexMap);
+
+    cv::Mat Txw = cv::Mat::eye(4,4,CV_32F);
+    R.copyTo(Txw.rowRange(0,3).colRange(0,3));
+
+    KeyFrame* pKFini = mvpKeyFrameOrigins[0];
+    cv::Mat Twc_0 = pKFini->GetPoseInverse();
+    cv::Mat Txc_0 = Txw*Twc_0;
+    cv::Mat Txb_0 = Txc_0*pKFini->mImuCalib.Tcb;
+    cv::Mat Tyx = cv::Mat::eye(4,4,CV_32F);
+    Tyx.rowRange(0,3).col(3) = -Txb_0.rowRange(0,3).col(3);
+    cv::Mat Tyw = Tyx*Txw;
+    cv::Mat Ryw = Tyw.rowRange(0,3).colRange(0,3);
+    cv::Mat tyw = Tyw.rowRange(0,3).col(3);
+
+    for(set<KeyFrame*>::iterator sit=mspKeyFrames.begin(); sit!=mspKeyFrames.end(); sit++)
+    {
+        KeyFrame* pKF = *sit;
+        cv::Mat Twc = pKF->GetPoseInverse();
+        cv::Mat Tyc = Tyw*Twc;
+        cv::Mat Tcy = cv::Mat::eye(4,4,CV_32F);
+        Tcy.rowRange(0,3).colRange(0,3) = Tyc.rowRange(0,3).colRange(0,3).t();
+        Tcy.rowRange(0,3).col(3) = -Tcy.rowRange(0,3).colRange(0,3)*Tyc.rowRange(0,3).col(3);
+        pKF->SetPose(Tcy);
+        cv::Mat Vw = pKF->GetVelocity();
+        pKF->SetVelocity(Ryw*Vw);
+    }
+    for(set<MapPoint*>::iterator sit=mspMapPoints.begin(); sit!=mspMapPoints.end(); sit++)
+    {
+        MapPoint* pMP = *sit;
+        pMP->SetWorldPos(Ryw*pMP->GetWorldPos()+tyw);
+        pMP->UpdateNormalAndDepth();
+    }
+}
+
+void Map::ApplyScaledRotation(const cv::Mat &R, const float s, const bool bScaledVel, const cv::Mat t)
+{
+    unique_lock<mutex> lock(mMutexMap);
+
+    // Body position (IMU) of first keyframe is fixed to (0,0,0)
+    cv::Mat Txw = cv::Mat::eye(4,4,CV_32F);
+    R.copyTo(Txw.rowRange(0,3).colRange(0,3));
+
+    cv::Mat Tyx = cv::Mat::eye(4,4,CV_32F);
+
+    cv::Mat Tyw = Tyx*Txw;
+    Tyw.rowRange(0,3).col(3) = Tyw.rowRange(0,3).col(3)+t;
+    cv::Mat Ryw = Tyw.rowRange(0,3).colRange(0,3);
+    cv::Mat tyw = Tyw.rowRange(0,3).col(3);
+
+    for(set<KeyFrame*>::iterator sit=mspKeyFrames.begin(); sit!=mspKeyFrames.end(); sit++)
+    {
+        KeyFrame* pKF = *sit;
+        cv::Mat Twc = pKF->GetPoseInverse();
+        Twc.rowRange(0,3).col(3)*=s;
+        cv::Mat Tyc = Tyw*Twc;
+        cv::Mat Tcy = cv::Mat::eye(4,4,CV_32F);
+        Tcy.rowRange(0,3).colRange(0,3) = Tyc.rowRange(0,3).colRange(0,3).t();
+        Tcy.rowRange(0,3).col(3) = -Tcy.rowRange(0,3).colRange(0,3)*Tyc.rowRange(0,3).col(3);
+        pKF->SetPose(Tcy);
+        cv::Mat Vw = pKF->GetVelocity();
+        if(!bScaledVel)
+            pKF->SetVelocity(Ryw*Vw);
+        else
+            pKF->SetVelocity(Ryw*Vw*s);
+
+    }
+    for(set<MapPoint*>::iterator sit=mspMapPoints.begin(); sit!=mspMapPoints.end(); sit++)
+    {
+        MapPoint* pMP = *sit;
+        pMP->SetWorldPos(s*Ryw*pMP->GetWorldPos()+tyw);
+        pMP->UpdateNormalAndDepth();
+    }
+    mnMapChange++;
+}
+
+void Map::SetInertialSensor()
+{
+    unique_lock<mutex> lock(mMutexMap);
+    mbIsInertial = true;
+}
+
+bool Map::IsInertial()
+{
+    unique_lock<mutex> lock(mMutexMap);
+    return mbIsInertial;
+}
+
+void Map::SetIniertialBA1()
+{
+    unique_lock<mutex> lock(mMutexMap);
+    mbIMU_BA1 = true;
+}
+
+void Map::SetIniertialBA2()
+{
+    unique_lock<mutex> lock(mMutexMap);
+    mbIMU_BA2 = true;
+}
+
+bool Map::GetIniertialBA1()
+{
+    unique_lock<mutex> lock(mMutexMap);
+    return mbIMU_BA1;
+}
+
+bool Map::GetIniertialBA2()
+{
+    unique_lock<mutex> lock(mMutexMap);
+    return mbIMU_BA2;
+}
+
+void Map::PrintEssentialGraph()
+{
+    //Print the essential graph
+    vector<KeyFrame*> vpOriginKFs = mvpKeyFrameOrigins;
+    int count=0;
+    cout << "Number of origin KFs: " << vpOriginKFs.size() << endl;
+    KeyFrame* pFirstKF;
+    for(KeyFrame* pKFi : vpOriginKFs)
+    {
+        if(!pFirstKF)
+            pFirstKF = pKFi;
+        else if(!pKFi->GetParent())
+            pFirstKF = pKFi;
+    }
+    if(pFirstKF->GetParent())
+    {
+        cout << "First KF in the essential graph has a parent, which is not possible" << endl;
+    }
+
+    cout << "KF: " << pFirstKF->mnId << endl;
+    set<KeyFrame*> spChilds = pFirstKF->GetChilds();
+    vector<KeyFrame*> vpChilds;
+    vector<string> vstrHeader;
+    for(KeyFrame* pKFi : spChilds){
+        vstrHeader.push_back("--");
+        vpChilds.push_back(pKFi);
+    }
+    for(int i=0; i<vpChilds.size() && count <= (mspKeyFrames.size()+10); ++i)
+    {
+        count++;
+        string strHeader = vstrHeader[i];
+        KeyFrame* pKFi = vpChilds[i];
+
+        cout << strHeader << "KF: " << pKFi->mnId << endl;
+
+        set<KeyFrame*> spKFiChilds = pKFi->GetChilds();
+        for(KeyFrame* pKFj : spKFiChilds)
+        {
+            vpChilds.push_back(pKFj);
+            vstrHeader.push_back(strHeader+"--");
+        }
+    }
+    if (count == (mspKeyFrames.size()+10))
+        cout << "CYCLE!!"    << endl;
+
+    cout << "------------------" << endl << "End of the essential graph" << endl;
+}
+
+bool Map::CheckEssentialGraph(){
+    vector<KeyFrame*> vpOriginKFs = mvpKeyFrameOrigins;
+    int count=0;
+    cout << "Number of origin KFs: " << vpOriginKFs.size() << endl;
+    KeyFrame* pFirstKF;
+    for(KeyFrame* pKFi : vpOriginKFs)
+    {
+        if(!pFirstKF)
+            pFirstKF = pKFi;
+        else if(!pKFi->GetParent())
+            pFirstKF = pKFi;
+    }
+    cout << "Checking if the first KF has parent" << endl;
+    if(pFirstKF->GetParent())
+    {
+        cout << "First KF in the essential graph has a parent, which is not possible" << endl;
+    }
+
+    set<KeyFrame*> spChilds = pFirstKF->GetChilds();
+    vector<KeyFrame*> vpChilds;
+    vpChilds.reserve(mspKeyFrames.size());
+    for(KeyFrame* pKFi : spChilds)
+        vpChilds.push_back(pKFi);
+
+    for(int i=0; i<vpChilds.size() && count <= (mspKeyFrames.size()+10); ++i)
+    {
+        count++;
+        KeyFrame* pKFi = vpChilds[i];
+        set<KeyFrame*> spKFiChilds = pKFi->GetChilds();
+        for(KeyFrame* pKFj : spKFiChilds)
+            vpChilds.push_back(pKFj);
+    }
+
+    cout << "count/tot" << count << "/" << mspKeyFrames.size() << endl;
+    if (count != (mspKeyFrames.size()-1))
+        return false;
+    else
+        return true;
+}
+
+void Map::ChangeId(long unsigned int nId)
+{
+    mnId = nId;
+}
+
+unsigned int Map::GetLowerKFID()
+{
+    unique_lock<mutex> lock(mMutexMap);
+    if (mpKFlowerID) {
+        return mpKFlowerID->mnId;
+    }
+    return 0;
+}
+
+int Map::GetMapChangeIndex()
+{
+    unique_lock<mutex> lock(mMutexMap);
+    return mnMapChange;
+}
+
+void Map::IncreaseChangeIndex()
+{
+    unique_lock<mutex> lock(mMutexMap);
+    mnMapChange++;
+}
+
+int Map::GetLastMapChange()
+{
+    unique_lock<mutex> lock(mMutexMap);
+    return mnMapChangeNotified;
+}
+
+void Map::SetLastMapChange(int currentChangeId)
+{
+    unique_lock<mutex> lock(mMutexMap);
+    mnMapChangeNotified = currentChangeId;
+}
+
+
+} //namespace ORB_SLAM3

src/MapDrawer.cc

@@ -0,0 +1,512 @@
+/**
+* This file is part of ORB-SLAM3
+*
+* Copyright (C) 2017-2020 Carlos Campos, Richard Elvira, Juan J. Gómez Rodríguez, José M.M. Montiel and Juan D. Tardós, University of Zaragoza.
+* Copyright (C) 2014-2016 Raúl Mur-Artal, José M.M. Montiel and Juan D. Tardós, University of Zaragoza.
+*
+* ORB-SLAM3 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-SLAM3 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-SLAM3.
+* If not, see <http://www.gnu.org/licenses/>.
+*/
+
+
+#include "MapDrawer.h"
+#include "MapPoint.h"
+#include "KeyFrame.h"
+#include <pangolin/pangolin.h>
+#include <mutex>
+
+namespace ORB_SLAM3
+{
+
+
+MapDrawer::MapDrawer(Atlas* pAtlas, const string &strSettingPath):mpAtlas(pAtlas)
+{
+    cv::FileStorage fSettings(strSettingPath, cv::FileStorage::READ);
+
+    bool is_correct = ParseViewerParamFile(fSettings);
+
+    if(!is_correct)
+    {
+        std::cerr << "**ERROR in the config file, the format is not correct**" << std::endl;
+        try
+        {
+            throw -1;
+        }
+        catch(exception &e)
+        {
+
+        }
+    }
+}
+
+bool MapDrawer::ParseViewerParamFile(cv::FileStorage &fSettings)
+{
+    bool b_miss_params = false;
+
+    cv::FileNode node = fSettings["Viewer.KeyFrameSize"];
+    if(!node.empty())
+    {
+        mKeyFrameSize = node.real();
+    }
+    else
+    {
+        std::cerr << "*Viewer.KeyFrameSize parameter doesn't exist or is not a real number*" << std::endl;
+        b_miss_params = true;
+    }
+
+    node = fSettings["Viewer.KeyFrameLineWidth"];
+    if(!node.empty())
+    {
+        mKeyFrameLineWidth = node.real();
+    }
+    else
+    {
+        std::cerr << "*Viewer.KeyFrameLineWidth parameter doesn't exist or is not a real number*" << std::endl;
+        b_miss_params = true;
+    }
+
+    node = fSettings["Viewer.GraphLineWidth"];
+    if(!node.empty())
+    {
+        mGraphLineWidth = node.real();
+    }
+    else
+    {
+        std::cerr << "*Viewer.GraphLineWidth parameter doesn't exist or is not a real number*" << std::endl;
+        b_miss_params = true;
+    }
+
+    node = fSettings["Viewer.PointSize"];
+    if(!node.empty())
+    {
+        mPointSize = node.real();
+    }
+    else
+    {
+        std::cerr << "*Viewer.PointSize parameter doesn't exist or is not a real number*" << std::endl;
+        b_miss_params = true;
+    }
+
+    node = fSettings["Viewer.CameraSize"];
+    if(!node.empty())
+    {
+        mCameraSize = node.real();
+    }
+    else
+    {
+        std::cerr << "*Viewer.CameraSize parameter doesn't exist or is not a real number*" << std::endl;
+        b_miss_params = true;
+    }
+
+    node = fSettings["Viewer.CameraLineWidth"];
+    if(!node.empty())
+    {
+        mCameraLineWidth = node.real();
+    }
+    else
+    {
+        std::cerr << "*Viewer.CameraLineWidth parameter doesn't exist or is not a real number*" << std::endl;
+        b_miss_params = true;
+    }
+
+    return !b_miss_params;
+}
+
+void MapDrawer::DrawMapPoints()
+{
+    const vector<MapPoint*> &vpMPs = mpAtlas->GetAllMapPoints();
+    const vector<MapPoint*> &vpRefMPs = mpAtlas->GetReferenceMapPoints();
+
+    set<MapPoint*> spRefMPs(vpRefMPs.begin(), vpRefMPs.end());
+
+    if(vpMPs.empty())
+        return;
+
+    glPointSize(mPointSize);
+    glBegin(GL_POINTS);
+    glColor3f(0.0,0.0,0.0);
+
+    for(size_t i=0, iend=vpMPs.size(); i<iend;i++)
+    {
+        if(vpMPs[i]->isBad() || spRefMPs.count(vpMPs[i]))
+            continue;
+        cv::Mat pos = vpMPs[i]->GetWorldPos();
+        glVertex3f(pos.at<float>(0),pos.at<float>(1),pos.at<float>(2));
+    }
+    glEnd();
+
+    glPointSize(mPointSize);
+    glBegin(GL_POINTS);
+    glColor3f(1.0,0.0,0.0);
+
+    for(set<MapPoint*>::iterator sit=spRefMPs.begin(), send=spRefMPs.end(); sit!=send; sit++)
+    {
+        if((*sit)->isBad())
+            continue;
+        cv::Mat pos = (*sit)->GetWorldPos();
+        glVertex3f(pos.at<float>(0),pos.at<float>(1),pos.at<float>(2));
+
+    }
+
+    glEnd();
+}
+
+void MapDrawer::DrawKeyFrames(const bool bDrawKF, const bool bDrawGraph, const bool bDrawInertialGraph)
+{
+    const float &w = mKeyFrameSize;
+    const float h = w*0.75;
+    const float z = w*0.6;
+
+    const vector<KeyFrame*> vpKFs = mpAtlas->GetAllKeyFrames();
+
+    if(bDrawKF)
+    {
+        for(size_t i=0; i<vpKFs.size(); i++)
+        {
+            KeyFrame* pKF = vpKFs[i];
+            cv::Mat Twc = pKF->GetPoseInverse().t();
+            unsigned int index_color = pKF->mnOriginMapId;
+
+            glPushMatrix();
+
+            glMultMatrixf(Twc.ptr<GLfloat>(0));
+
+            if(!pKF->GetParent()) // It is the first KF in the map
+            {
+                glLineWidth(mKeyFrameLineWidth*5);
+                glColor3f(1.0f,0.0f,0.0f);
+                glBegin(GL_LINES);
+            }
+            else
+            {
+                glLineWidth(mKeyFrameLineWidth);
+                glColor3f(mfFrameColors[index_color][0],mfFrameColors[index_color][1],mfFrameColors[index_color][2]);
+                glBegin(GL_LINES);
+            }
+
+            glVertex3f(0,0,0);
+            glVertex3f(w,h,z);
+            glVertex3f(0,0,0);
+            glVertex3f(w,-h,z);
+            glVertex3f(0,0,0);
+            glVertex3f(-w,-h,z);
+            glVertex3f(0,0,0);
+            glVertex3f(-w,h,z);
+
+            glVertex3f(w,h,z);
+            glVertex3f(w,-h,z);
+
+            glVertex3f(-w,h,z);
+            glVertex3f(-w,-h,z);
+
+            glVertex3f(-w,h,z);
+            glVertex3f(w,h,z);
+
+            glVertex3f(-w,-h,z);
+            glVertex3f(w,-h,z);
+            glEnd();
+
+            glPopMatrix();
+
+            glEnd();
+        }
+    }
+
+    if(bDrawGraph)
+    {
+        glLineWidth(mGraphLineWidth);
+        glColor4f(0.0f,1.0f,0.0f,0.6f);
+        glBegin(GL_LINES);
+
+        for(size_t i=0; i<vpKFs.size(); i++)
+        {
+            // Covisibility Graph
+            const vector<KeyFrame*> vCovKFs = vpKFs[i]->GetCovisiblesByWeight(100);
+            cv::Mat Ow = vpKFs[i]->GetCameraCenter();
+            if(!vCovKFs.empty())
+            {
+                for(vector<KeyFrame*>::const_iterator vit=vCovKFs.begin(), vend=vCovKFs.end(); vit!=vend; vit++)
+                {
+                    if((*vit)->mnId<vpKFs[i]->mnId)
+                        continue;
+                    cv::Mat Ow2 = (*vit)->GetCameraCenter();
+                    glVertex3f(Ow.at<float>(0),Ow.at<float>(1),Ow.at<float>(2));
+                    glVertex3f(Ow2.at<float>(0),Ow2.at<float>(1),Ow2.at<float>(2));
+                }
+            }
+
+            // Spanning tree
+            KeyFrame* pParent = vpKFs[i]->GetParent();
+            if(pParent)
+            {
+                cv::Mat Owp = pParent->GetCameraCenter();
+                glVertex3f(Ow.at<float>(0),Ow.at<float>(1),Ow.at<float>(2));
+                glVertex3f(Owp.at<float>(0),Owp.at<float>(1),Owp.at<float>(2));
+            }
+
+            // Loops
+            set<KeyFrame*> sLoopKFs = vpKFs[i]->GetLoopEdges();
+            for(set<KeyFrame*>::iterator sit=sLoopKFs.begin(), send=sLoopKFs.end(); sit!=send; sit++)
+            {
+                if((*sit)->mnId<vpKFs[i]->mnId)
+                    continue;
+                cv::Mat Owl = (*sit)->GetCameraCenter();
+                glVertex3f(Ow.at<float>(0),Ow.at<float>(1),Ow.at<float>(2));
+                glVertex3f(Owl.at<float>(0),Owl.at<float>(1),Owl.at<float>(2));
+            }
+        }
+
+        glEnd();
+    }
+
+    if(bDrawInertialGraph && mpAtlas->isImuInitialized())
+    {
+        glLineWidth(mGraphLineWidth);
+        glColor4f(1.0f,0.0f,0.0f,0.6f);
+        glBegin(GL_LINES);
+
+        //Draw inertial links
+        for(size_t i=0; i<vpKFs.size(); i++)
+        {
+            KeyFrame* pKFi = vpKFs[i];
+            cv::Mat Ow = pKFi->GetCameraCenter();
+            KeyFrame* pNext = pKFi->mNextKF;
+            if(pNext)
+            {
+                cv::Mat Owp = pNext->GetCameraCenter();
+                glVertex3f(Ow.at<float>(0),Ow.at<float>(1),Ow.at<float>(2));
+                glVertex3f(Owp.at<float>(0),Owp.at<float>(1),Owp.at<float>(2));
+            }
+        }
+
+        glEnd();
+    }
+
+    vector<Map*> vpMaps = mpAtlas->GetAllMaps();
+
+    if(bDrawKF)
+    {
+        for(Map* pMap : vpMaps)
+        {
+            if(pMap == mpAtlas->GetCurrentMap())
+                continue;
+
+            vector<KeyFrame*> vpKFs = pMap->GetAllKeyFrames();
+
+            for(size_t i=0; i<vpKFs.size(); i++)
+            {
+                KeyFrame* pKF = vpKFs[i];
+                cv::Mat Twc = pKF->GetPoseInverse().t();
+                unsigned int index_color = pKF->mnOriginMapId;
+
+                glPushMatrix();
+
+                glMultMatrixf(Twc.ptr<GLfloat>(0));
+
+                if(!vpKFs[i]->GetParent()) // It is the first KF in the map
+                {
+                    glLineWidth(mKeyFrameLineWidth*5);
+                    glColor3f(1.0f,0.0f,0.0f);
+                    glBegin(GL_LINES);
+                }
+                else
+                {
+                    glLineWidth(mKeyFrameLineWidth);
+                    glColor3f(mfFrameColors[index_color][0],mfFrameColors[index_color][1],mfFrameColors[index_color][2]);
+                    glBegin(GL_LINES);
+                }
+
+                glVertex3f(0,0,0);
+                glVertex3f(w,h,z);
+                glVertex3f(0,0,0);
+                glVertex3f(w,-h,z);
+                glVertex3f(0,0,0);
+                glVertex3f(-w,-h,z);
+                glVertex3f(0,0,0);
+                glVertex3f(-w,h,z);
+
+                glVertex3f(w,h,z);
+                glVertex3f(w,-h,z);
+
+                glVertex3f(-w,h,z);
+                glVertex3f(-w,-h,z);
+
+                glVertex3f(-w,h,z);
+                glVertex3f(w,h,z);
+
+                glVertex3f(-w,-h,z);
+                glVertex3f(w,-h,z);
+                glEnd();
+
+                glPopMatrix();
+            }
+        }
+    }
+}
+
+void MapDrawer::DrawCurrentCamera(pangolin::OpenGlMatrix &Twc)
+{
+    const float &w = mCameraSize;
+    const float h = w*0.75;
+    const float z = w*0.6;
+
+    glPushMatrix();
+
+#ifdef HAVE_GLES
+        glMultMatrixf(Twc.m);
+#else
+        glMultMatrixd(Twc.m);
+#endif
+
+    glLineWidth(mCameraLineWidth);
+    glColor3f(0.0f,1.0f,0.0f);
+    glBegin(GL_LINES);
+    glVertex3f(0,0,0);
+    glVertex3f(w,h,z);
+    glVertex3f(0,0,0);
+    glVertex3f(w,-h,z);
+    glVertex3f(0,0,0);
+    glVertex3f(-w,-h,z);
+    glVertex3f(0,0,0);
+    glVertex3f(-w,h,z);
+
+    glVertex3f(w,h,z);
+    glVertex3f(w,-h,z);
+
+    glVertex3f(-w,h,z);
+    glVertex3f(-w,-h,z);
+
+    glVertex3f(-w,h,z);
+    glVertex3f(w,h,z);
+
+    glVertex3f(-w,-h,z);
+    glVertex3f(w,-h,z);
+    glEnd();
+
+    glPopMatrix();
+}
+
+
+void MapDrawer::SetCurrentCameraPose(const cv::Mat &Tcw)
+{
+    unique_lock<mutex> lock(mMutexCamera);
+    mCameraPose = Tcw.clone();
+}
+
+void MapDrawer::GetCurrentOpenGLCameraMatrix(pangolin::OpenGlMatrix &M, pangolin::OpenGlMatrix &MOw)
+{
+    if(!mCameraPose.empty())
+    {
+        cv::Mat Rwc(3,3,CV_32F);
+        cv::Mat twc(3,1,CV_32F);
+        {
+            unique_lock<mutex> lock(mMutexCamera);
+            Rwc = mCameraPose.rowRange(0,3).colRange(0,3).t();
+            twc = -Rwc*mCameraPose.rowRange(0,3).col(3);
+        }
+
+        M.m[0] = Rwc.at<float>(0,0);
+        M.m[1] = Rwc.at<float>(1,0);
+        M.m[2] = Rwc.at<float>(2,0);
+        M.m[3]  = 0.0;
+
+        M.m[4] = Rwc.at<float>(0,1);
+        M.m[5] = Rwc.at<float>(1,1);
+        M.m[6] = Rwc.at<float>(2,1);
+        M.m[7]  = 0.0;
+
+        M.m[8] = Rwc.at<float>(0,2);
+        M.m[9] = Rwc.at<float>(1,2);
+        M.m[10] = Rwc.at<float>(2,2);
+        M.m[11]  = 0.0;
+
+        M.m[12] = twc.at<float>(0);
+        M.m[13] = twc.at<float>(1);
+        M.m[14] = twc.at<float>(2);
+        M.m[15]  = 1.0;
+
+        MOw.SetIdentity();
+        MOw.m[12] = twc.at<float>(0);
+        MOw.m[13] = twc.at<float>(1);
+        MOw.m[14] = twc.at<float>(2);
+    }
+    else
+    {
+        M.SetIdentity();
+        MOw.SetIdentity();
+    }
+}
+
+void MapDrawer::GetCurrentOpenGLCameraMatrix(pangolin::OpenGlMatrix &M, pangolin::OpenGlMatrix &MOw, pangolin::OpenGlMatrix &MTwwp)
+{
+    if(!mCameraPose.empty())
+    {
+        cv::Mat Rwc(3,3,CV_32F);
+        cv::Mat twc(3,1,CV_32F);
+        cv::Mat Rwwp(3,3,CV_32F);
+        {
+            unique_lock<mutex> lock(mMutexCamera);
+            Rwc = mCameraPose.rowRange(0,3).colRange(0,3).t();
+            twc = -Rwc*mCameraPose.rowRange(0,3).col(3);
+        }
+
+        M.m[0] = Rwc.at<float>(0,0);
+        M.m[1] = Rwc.at<float>(1,0);
+        M.m[2] = Rwc.at<float>(2,0);
+        M.m[3]  = 0.0;
+
+        M.m[4] = Rwc.at<float>(0,1);
+        M.m[5] = Rwc.at<float>(1,1);
+        M.m[6] = Rwc.at<float>(2,1);
+        M.m[7]  = 0.0;
+
+        M.m[8] = Rwc.at<float>(0,2);
+        M.m[9] = Rwc.at<float>(1,2);
+        M.m[10] = Rwc.at<float>(2,2);
+        M.m[11]  = 0.0;
+
+        M.m[12] = twc.at<float>(0);
+        M.m[13] = twc.at<float>(1);
+        M.m[14] = twc.at<float>(2);
+        M.m[15]  = 1.0;
+
+        MOw.SetIdentity();
+        MOw.m[12] = twc.at<float>(0);
+        MOw.m[13] = twc.at<float>(1);
+        MOw.m[14] = twc.at<float>(2);
+
+        MTwwp.SetIdentity();
+        MTwwp.m[0] = Rwwp.at<float>(0,0);
+        MTwwp.m[1] = Rwwp.at<float>(1,0);
+        MTwwp.m[2] = Rwwp.at<float>(2,0);
+
+        MTwwp.m[4] = Rwwp.at<float>(0,1);
+        MTwwp.m[5] = Rwwp.at<float>(1,1);
+        MTwwp.m[6] = Rwwp.at<float>(2,1);
+
+        MTwwp.m[8] = Rwwp.at<float>(0,2);
+        MTwwp.m[9] = Rwwp.at<float>(1,2);
+        MTwwp.m[10] = Rwwp.at<float>(2,2);
+
+        MTwwp.m[12] = twc.at<float>(0);
+        MTwwp.m[13] = twc.at<float>(1);
+        MTwwp.m[14] = twc.at<float>(2);
+    }
+    else
+    {
+        M.SetIdentity();
+        MOw.SetIdentity();
+        MTwwp.SetIdentity();
+    }
+
+}
+
+} //namespace ORB_SLAM

src/MapPoint.cc

@@ -0,0 +1,599 @@
+/**
+* This file is part of ORB-SLAM3
+*
+* Copyright (C) 2017-2020 Carlos Campos, Richard Elvira, Juan J. Gómez Rodríguez, José M.M. Montiel and Juan D. Tardós, University of Zaragoza.
+* Copyright (C) 2014-2016 Raúl Mur-Artal, José M.M. Montiel and Juan D. Tardós, University of Zaragoza.
+*
+* ORB-SLAM3 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-SLAM3 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-SLAM3.
+* If not, see <http://www.gnu.org/licenses/>.
+*/
+
+#include "MapPoint.h"
+#include "ORBmatcher.h"
+
+#include<mutex>
+
+namespace ORB_SLAM3
+{
+
+long unsigned int MapPoint::nNextId=0;
+mutex MapPoint::mGlobalMutex;
+
+MapPoint::MapPoint():
+    mnFirstKFid(0), mnFirstFrame(0), nObs(0), mnTrackReferenceForFrame(0),
+    mnLastFrameSeen(0), mnBALocalForKF(0), mnFuseCandidateForKF(0), mnLoopPointForKF(0), mnCorrectedByKF(0),
+    mnCorrectedReference(0), mnBAGlobalForKF(0), mnVisible(1), mnFound(1), mbBad(false),
+    mpReplaced(static_cast<MapPoint*>(NULL))
+{
+    mpReplaced = static_cast<MapPoint*>(NULL);
+}
+
+MapPoint::MapPoint(const cv::Mat &Pos, KeyFrame *pRefKF, Map* pMap):
+    mnFirstKFid(pRefKF->mnId), mnFirstFrame(pRefKF->mnFrameId), nObs(0), mnTrackReferenceForFrame(0),
+    mnLastFrameSeen(0), mnBALocalForKF(0), mnFuseCandidateForKF(0), mnLoopPointForKF(0), mnCorrectedByKF(0),
+    mnCorrectedReference(0), mnBAGlobalForKF(0), mpRefKF(pRefKF), mnVisible(1), mnFound(1), mbBad(false),
+    mpReplaced(static_cast<MapPoint*>(NULL)), mfMinDistance(0), mfMaxDistance(0), mpMap(pMap),
+    mnOriginMapId(pMap->GetId())
+{
+    Pos.copyTo(mWorldPos);
+    mWorldPosx = cv::Matx31f(Pos.at<float>(0), Pos.at<float>(1), Pos.at<float>(2));
+    mNormalVector = cv::Mat::zeros(3,1,CV_32F);
+    mNormalVectorx = cv::Matx31f::zeros();
+
+    mbTrackInViewR = false;
+    mbTrackInView = false;
+
+    // MapPoints can be created from Tracking and Local Mapping. This mutex avoid conflicts with id.
+    unique_lock<mutex> lock(mpMap->mMutexPointCreation);
+    mnId=nNextId++;
+}
+
+MapPoint::MapPoint(const double invDepth, cv::Point2f uv_init, KeyFrame* pRefKF, KeyFrame* pHostKF, Map* pMap):
+    mnFirstKFid(pRefKF->mnId), mnFirstFrame(pRefKF->mnFrameId), nObs(0), mnTrackReferenceForFrame(0),
+    mnLastFrameSeen(0), mnBALocalForKF(0), mnFuseCandidateForKF(0), mnLoopPointForKF(0), mnCorrectedByKF(0),
+    mnCorrectedReference(0), mnBAGlobalForKF(0), mpRefKF(pRefKF), mnVisible(1), mnFound(1), mbBad(false),
+    mpReplaced(static_cast<MapPoint*>(NULL)), mfMinDistance(0), mfMaxDistance(0), mpMap(pMap),
+    mnOriginMapId(pMap->GetId())
+{
+    mInvDepth=invDepth;
+    mInitU=(double)uv_init.x;
+    mInitV=(double)uv_init.y;
+    mpHostKF = pHostKF;
+
+    mNormalVector = cv::Mat::zeros(3,1,CV_32F);
+    mNormalVectorx = cv::Matx31f::zeros();
+
+    // Worldpos is not set
+    // MapPoints can be created from Tracking and Local Mapping. This mutex avoid conflicts with id.
+    unique_lock<mutex> lock(mpMap->mMutexPointCreation);
+    mnId=nNextId++;
+}
+
+MapPoint::MapPoint(const cv::Mat &Pos, Map* pMap, Frame* pFrame, const int &idxF):
+    mnFirstKFid(-1), mnFirstFrame(pFrame->mnId), nObs(0), mnTrackReferenceForFrame(0), mnLastFrameSeen(0),
+    mnBALocalForKF(0), mnFuseCandidateForKF(0),mnLoopPointForKF(0), mnCorrectedByKF(0),
+    mnCorrectedReference(0), mnBAGlobalForKF(0), mpRefKF(static_cast<KeyFrame*>(NULL)), mnVisible(1),
+    mnFound(1), mbBad(false), mpReplaced(NULL), mpMap(pMap), mnOriginMapId(pMap->GetId())
+{
+    Pos.copyTo(mWorldPos);
+    mWorldPosx = cv::Matx31f(Pos.at<float>(0), Pos.at<float>(1), Pos.at<float>(2));
+
+    cv::Mat Ow;
+    if(pFrame -> Nleft == -1 || idxF < pFrame -> Nleft){
+        Ow = pFrame->GetCameraCenter();
+    }
+    else{
+        cv::Mat Rwl = pFrame -> mRwc;
+        cv::Mat tlr = pFrame -> mTlr.col(3);
+        cv::Mat twl = pFrame -> mOw;
+
+        Ow = Rwl * tlr + twl;
+    }
+    mNormalVector = mWorldPos - Ow;
+    mNormalVector = mNormalVector/cv::norm(mNormalVector);
+    mNormalVectorx = cv::Matx31f(mNormalVector.at<float>(0), mNormalVector.at<float>(1), mNormalVector.at<float>(2));
+
+
+    cv::Mat PC = Pos - Ow;
+    const float dist = cv::norm(PC);
+    const int level = (pFrame -> Nleft == -1) ? pFrame->mvKeysUn[idxF].octave
+                                              : (idxF < pFrame -> Nleft) ? pFrame->mvKeys[idxF].octave
+                                                                         : pFrame -> mvKeysRight[idxF].octave;
+    const float levelScaleFactor =  pFrame->mvScaleFactors[level];
+    const int nLevels = pFrame->mnScaleLevels;
+
+    mfMaxDistance = dist*levelScaleFactor;
+    mfMinDistance = mfMaxDistance/pFrame->mvScaleFactors[nLevels-1];
+
+    pFrame->mDescriptors.row(idxF).copyTo(mDescriptor);
+
+    // MapPoints can be created from Tracking and Local Mapping. This mutex avoid conflicts with id.
+    unique_lock<mutex> lock(mpMap->mMutexPointCreation);
+    mnId=nNextId++;
+}
+
+void MapPoint::SetWorldPos(const cv::Mat &Pos)
+{
+    unique_lock<mutex> lock2(mGlobalMutex);
+    unique_lock<mutex> lock(mMutexPos);
+    Pos.copyTo(mWorldPos);
+    mWorldPosx = cv::Matx31f(Pos.at<float>(0), Pos.at<float>(1), Pos.at<float>(2));
+}
+
+cv::Mat MapPoint::GetWorldPos()
+{
+    unique_lock<mutex> lock(mMutexPos);
+    return mWorldPos.clone();
+}
+
+cv::Mat MapPoint::GetNormal()
+{
+    unique_lock<mutex> lock(mMutexPos);
+    return mNormalVector.clone();
+}
+
+cv::Matx31f MapPoint::GetWorldPos2()
+{
+    unique_lock<mutex> lock(mMutexPos);
+    return mWorldPosx;
+}
+
+cv::Matx31f MapPoint::GetNormal2()
+{
+    unique_lock<mutex> lock(mMutexPos);
+    return mNormalVectorx;
+}
+
+KeyFrame* MapPoint::GetReferenceKeyFrame()
+{
+    unique_lock<mutex> lock(mMutexFeatures);
+    return mpRefKF;
+}
+
+void MapPoint::AddObservation(KeyFrame* pKF, int idx)
+{
+    unique_lock<mutex> lock(mMutexFeatures);
+    tuple<int,int> indexes;
+
+    if(mObservations.count(pKF)){
+        indexes = mObservations[pKF];
+    }
+    else{
+        indexes = tuple<int,int>(-1,-1);
+    }
+
+    if(pKF -> NLeft != -1 && idx >= pKF -> NLeft){
+        get<1>(indexes) = idx;
+    }
+    else{
+        get<0>(indexes) = idx;
+    }
+
+    mObservations[pKF]=indexes;
+
+    if(!pKF->mpCamera2 && pKF->mvuRight[idx]>=0)
+        nObs+=2;
+    else
+        nObs++;
+}
+
+void MapPoint::EraseObservation(KeyFrame* pKF)
+{
+    bool bBad=false;
+    {
+        unique_lock<mutex> lock(mMutexFeatures);
+        if(mObservations.count(pKF))
+        {
+            tuple<int,int> indexes = mObservations[pKF];
+            int leftIndex = get<0>(indexes), rightIndex = get<1>(indexes);
+
+            if(leftIndex != -1){
+                if(!pKF->mpCamera2 && pKF->mvuRight[leftIndex]>=0)
+                    nObs-=2;
+                else
+                    nObs--;
+            }
+            if(rightIndex != -1){
+                nObs--;
+            }
+
+            mObservations.erase(pKF);
+
+            if(mpRefKF==pKF)
+                mpRefKF=mObservations.begin()->first;
+
+            // If only 2 observations or less, discard point
+            if(nObs<=2)
+                bBad=true;
+        }
+    }
+
+    if(bBad)
+        SetBadFlag();
+}
+
+
+std::map<KeyFrame*, std::tuple<int,int>>  MapPoint::GetObservations()
+{
+    unique_lock<mutex> lock(mMutexFeatures);
+    return mObservations;
+}
+
+int MapPoint::Observations()
+{
+    unique_lock<mutex> lock(mMutexFeatures);
+    return nObs;
+}
+
+void MapPoint::SetBadFlag()
+{
+    map<KeyFrame*, tuple<int,int>> obs;
+    {
+        unique_lock<mutex> lock1(mMutexFeatures);
+        unique_lock<mutex> lock2(mMutexPos);
+        mbBad=true;
+        obs = mObservations;
+        mObservations.clear();
+    }
+    for(map<KeyFrame*, tuple<int,int>>::iterator mit=obs.begin(), mend=obs.end(); mit!=mend; mit++)
+    {
+        KeyFrame* pKF = mit->first;
+        int leftIndex = get<0>(mit -> second), rightIndex = get<1>(mit -> second);
+        if(leftIndex != -1){
+            pKF->EraseMapPointMatch(leftIndex);
+        }
+        if(rightIndex != -1){
+            pKF->EraseMapPointMatch(rightIndex);
+        }
+    }
+
+    mpMap->EraseMapPoint(this);
+}
+
+MapPoint* MapPoint::GetReplaced()
+{
+    unique_lock<mutex> lock1(mMutexFeatures);
+    unique_lock<mutex> lock2(mMutexPos);
+    return mpReplaced;
+}
+
+void MapPoint::Replace(MapPoint* pMP)
+{
+    if(pMP->mnId==this->mnId)
+        return;
+
+    int nvisible, nfound;
+    map<KeyFrame*,tuple<int,int>> obs;
+    {
+        unique_lock<mutex> lock1(mMutexFeatures);
+        unique_lock<mutex> lock2(mMutexPos);
+        obs=mObservations;
+        mObservations.clear();
+        mbBad=true;
+        nvisible = mnVisible;
+        nfound = mnFound;
+        mpReplaced = pMP;
+    }
+
+    for(map<KeyFrame*,tuple<int,int>>::iterator mit=obs.begin(), mend=obs.end(); mit!=mend; mit++)
+    {
+        // Replace measurement in keyframe
+        KeyFrame* pKF = mit->first;
+
+        tuple<int,int> indexes = mit -> second;
+        int leftIndex = get<0>(indexes), rightIndex = get<1>(indexes);
+
+        if(!pMP->IsInKeyFrame(pKF))
+        {
+            if(leftIndex != -1){
+                pKF->ReplaceMapPointMatch(leftIndex, pMP);
+                pMP->AddObservation(pKF,leftIndex);
+            }
+            if(rightIndex != -1){
+                pKF->ReplaceMapPointMatch(rightIndex, pMP);
+                pMP->AddObservation(pKF,rightIndex);
+            }
+        }
+        else
+        {
+            if(leftIndex != -1){
+                pKF->EraseMapPointMatch(leftIndex);
+            }
+            if(rightIndex != -1){
+                pKF->EraseMapPointMatch(rightIndex);
+            }
+        }
+    }
+    pMP->IncreaseFound(nfound);
+    pMP->IncreaseVisible(nvisible);
+    pMP->ComputeDistinctiveDescriptors();
+
+    mpMap->EraseMapPoint(this);
+}
+
+bool MapPoint::isBad()
+{
+    unique_lock<mutex> lock1(mMutexFeatures,std::defer_lock);
+    unique_lock<mutex> lock2(mMutexPos,std::defer_lock);
+    lock(lock1, lock2);
+
+    return mbBad;
+}
+
+void MapPoint::IncreaseVisible(int n)
+{
+    unique_lock<mutex> lock(mMutexFeatures);
+    mnVisible+=n;
+}
+
+void MapPoint::IncreaseFound(int n)
+{
+    unique_lock<mutex> lock(mMutexFeatures);
+    mnFound+=n;
+}
+
+float MapPoint::GetFoundRatio()
+{
+    unique_lock<mutex> lock(mMutexFeatures);
+    return static_cast<float>(mnFound)/mnVisible;
+}
+
+void MapPoint::ComputeDistinctiveDescriptors()
+{
+    // Retrieve all observed descriptors
+    vector<cv::Mat> vDescriptors;
+
+    map<KeyFrame*,tuple<int,int>> observations;
+
+    {
+        unique_lock<mutex> lock1(mMutexFeatures);
+        if(mbBad)
+            return;
+        observations=mObservations;
+    }
+
+    if(observations.empty())
+        return;
+
+    vDescriptors.reserve(observations.size());
+
+    for(map<KeyFrame*,tuple<int,int>>::iterator mit=observations.begin(), mend=observations.end(); mit!=mend; mit++)
+    {
+        KeyFrame* pKF = mit->first;
+
+        if(!pKF->isBad()){
+            tuple<int,int> indexes = mit -> second;
+            int leftIndex = get<0>(indexes), rightIndex = get<1>(indexes);
+
+            if(leftIndex != -1){
+                vDescriptors.push_back(pKF->mDescriptors.row(leftIndex));
+            }
+            if(rightIndex != -1){
+                vDescriptors.push_back(pKF->mDescriptors.row(rightIndex));
+            }
+        }
+    }
+
+    if(vDescriptors.empty())
+        return;
+
+    // Compute distances between them
+    const auto N = vDescriptors.size();
+
+    //float Distances[N][N];
+    //cv::Mat Distances = cv::Mat(N, N, CV_32F);
+    vector<vector<float>> Distances;
+    typedef vector<float> Row;
+    
+    // Fill Distances
+    for (size_t i = 0; i < N; ++i)
+    {
+        Row row(N);
+
+        for (size_t j = 0; j < N; ++j)
+        {
+            row[j] = 0;
+        }
+
+        Distances.push_back(row); // push each row after you fill it
+    }
+
+    for(size_t i=0;i<N;i++)
+    {
+        Distances[i][i]=0;
+        for(size_t j=i+1;j<N;j++)
+        {
+            int distij = ORBmatcher::DescriptorDistance(vDescriptors[i],vDescriptors[j]);
+            Distances[i][j]=distij;
+            Distances[j][i]=distij;
+        }
+    }
+
+    // Take the descriptor with least median distance to the rest
+    int BestMedian = INT_MAX;
+    int BestIdx = 0;
+    for(size_t i=0;i<N;i++)
+    {
+        // vector<int> vDists(Distances[i], Distances[i] + N);
+        // Since the line above just copies the array, let's do it in a normal way
+        vector<int> vDists(Distances[i].begin(), Distances[i].end());
+        sort(vDists.begin(),vDists.end());
+        int median = vDists[0.5*(N-1)];
+
+        if(median<BestMedian)
+        {
+            BestMedian = median;
+            BestIdx = i;
+        }
+    }
+
+    {
+        unique_lock<mutex> lock(mMutexFeatures);
+        mDescriptor = vDescriptors[BestIdx].clone();
+    }
+}
+
+cv::Mat MapPoint::GetDescriptor()
+{
+    unique_lock<mutex> lock(mMutexFeatures);
+    return mDescriptor.clone();
+}
+
+tuple<int,int> MapPoint::GetIndexInKeyFrame(KeyFrame *pKF)
+{
+    unique_lock<mutex> lock(mMutexFeatures);
+    if(mObservations.count(pKF))
+        return mObservations[pKF];
+    else
+        return tuple<int,int>(-1,-1);
+}
+
+bool MapPoint::IsInKeyFrame(KeyFrame *pKF)
+{
+    unique_lock<mutex> lock(mMutexFeatures);
+    return (mObservations.count(pKF));
+}
+
+void MapPoint::UpdateNormalAndDepth()
+{
+    map<KeyFrame*,tuple<int,int>> observations;
+    KeyFrame* pRefKF;
+    cv::Mat Pos;
+    {
+        unique_lock<mutex> lock1(mMutexFeatures);
+        unique_lock<mutex> lock2(mMutexPos);
+        if(mbBad)
+            return;
+        observations=mObservations;
+        pRefKF=mpRefKF;
+        Pos = mWorldPos.clone();
+    }
+
+    if(observations.empty())
+        return;
+
+    cv::Mat normal = cv::Mat::zeros(3,1,CV_32F);
+    int n=0;
+    for(map<KeyFrame*,tuple<int,int>>::iterator mit=observations.begin(), mend=observations.end(); mit!=mend; mit++)
+    {
+        KeyFrame* pKF = mit->first;
+
+        tuple<int,int> indexes = mit -> second;
+        int leftIndex = get<0>(indexes), rightIndex = get<1>(indexes);
+
+        if(leftIndex != -1){
+            cv::Mat Owi = pKF->GetCameraCenter();
+            cv::Mat normali = mWorldPos - Owi;
+            normal = normal + normali/cv::norm(normali);
+            n++;
+        }
+        if(rightIndex != -1){
+            cv::Mat Owi = pKF->GetRightCameraCenter();
+            cv::Mat normali = mWorldPos - Owi;
+            normal = normal + normali/cv::norm(normali);
+            n++;
+        }
+    }
+
+    cv::Mat PC = Pos - pRefKF->GetCameraCenter();
+    const float dist = cv::norm(PC);
+
+    tuple<int ,int> indexes = observations[pRefKF];
+    int leftIndex = get<0>(indexes), rightIndex = get<1>(indexes);
+    int level;
+    if(pRefKF -> NLeft == -1){
+        level = pRefKF->mvKeysUn[leftIndex].octave;
+    }
+    else if(leftIndex != -1){
+        level = pRefKF -> mvKeys[leftIndex].octave;
+    }
+    else{
+        level = pRefKF -> mvKeysRight[rightIndex - pRefKF -> NLeft].octave;
+    }
+
+    const float levelScaleFactor =  pRefKF->mvScaleFactors[level];
+    const int nLevels = pRefKF->mnScaleLevels;
+
+    {
+        unique_lock<mutex> lock3(mMutexPos);
+        mfMaxDistance = dist*levelScaleFactor;
+        mfMinDistance = mfMaxDistance/pRefKF->mvScaleFactors[nLevels-1];
+        mNormalVector = normal/n;
+        mNormalVectorx = cv::Matx31f(mNormalVector.at<float>(0), mNormalVector.at<float>(1), mNormalVector.at<float>(2));
+    }
+}
+
+void MapPoint::SetNormalVector(cv::Mat& normal)
+{
+    unique_lock<mutex> lock3(mMutexPos);
+    mNormalVector = normal;
+    mNormalVectorx = cv::Matx31f(mNormalVector.at<float>(0), mNormalVector.at<float>(1), mNormalVector.at<float>(2));
+}
+
+float MapPoint::GetMinDistanceInvariance()
+{
+    unique_lock<mutex> lock(mMutexPos);
+    return 0.8f*mfMinDistance;
+}
+
+float MapPoint::GetMaxDistanceInvariance()
+{
+    unique_lock<mutex> lock(mMutexPos);
+    return 1.2f*mfMaxDistance;
+}
+
+int MapPoint::PredictScale(const float &currentDist, KeyFrame* pKF)
+{
+    float ratio;
+    {
+        unique_lock<mutex> lock(mMutexPos);
+        ratio = mfMaxDistance/currentDist;
+    }
+
+    int nScale = ceil(log(ratio)/pKF->mfLogScaleFactor);
+    if(nScale<0)
+        nScale = 0;
+    else if(nScale>=pKF->mnScaleLevels)
+        nScale = pKF->mnScaleLevels-1;
+
+    return nScale;
+}
+
+int MapPoint::PredictScale(const float &currentDist, Frame* pF)
+{
+    float ratio;
+    {
+        unique_lock<mutex> lock(mMutexPos);
+        ratio = mfMaxDistance/currentDist;
+    }
+
+    int nScale = ceil(log(ratio)/pF->mfLogScaleFactor);
+    if(nScale<0)
+        nScale = 0;
+    else if(nScale>=pF->mnScaleLevels)
+        nScale = pF->mnScaleLevels-1;
+
+    return nScale;
+}
+
+Map* MapPoint::GetMap()
+{
+    unique_lock<mutex> lock(mMutexMap);
+    return mpMap;
+}
+
+void MapPoint::UpdateMap(Map* pMap)
+{
+    unique_lock<mutex> lock(mMutexMap);
+    mpMap = pMap;
+}
+
+} //namespace ORB_SLAM

src/OptimizableTypes.cpp

@@ -0,0 +1,332 @@
+/**
+* This file is part of ORB-SLAM3
+*
+* Copyright (C) 2017-2020 Carlos Campos, Richard Elvira, Juan J. Gómez Rodríguez, José M.M. Montiel and Juan D. Tardós, University of Zaragoza.
+* Copyright (C) 2014-2016 Raúl Mur-Artal, José M.M. Montiel and Juan D. Tardós, University of Zaragoza.
+*
+* ORB-SLAM3 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-SLAM3 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-SLAM3.
+* If not, see <http://www.gnu.org/licenses/>.
+*/
+
+#include "OptimizableTypes.h"
+
+namespace ORB_SLAM3 {
+    bool EdgeSE3ProjectXYZOnlyPose::read(std::istream& is){
+        for (int i=0; i<2; i++){
+            is >> _measurement[i];
+        }
+        for (int i=0; i<2; i++)
+            for (int j=i; j<2; j++) {
+                is >> information()(i,j);
+                if (i!=j)
+                    information()(j,i)=information()(i,j);
+            }
+        return true;
+    }
+
+    bool EdgeSE3ProjectXYZOnlyPose::write(std::ostream& os) const {
+
+        for (int i=0; i<2; i++){
+            os << measurement()[i] << " ";
+        }
+
+        for (int i=0; i<2; i++)
+            for (int j=i; j<2; j++){
+                os << " " <<  information()(i,j);
+            }
+        return os.good();
+    }
+
+
+    void EdgeSE3ProjectXYZOnlyPose::linearizeOplus() {
+        g2o::VertexSE3Expmap * vi = static_cast<g2o::VertexSE3Expmap *>(_vertices[0]);
+        Eigen::Vector3d xyz_trans = vi->estimate().map(Xw);
+
+        double x = xyz_trans[0];
+        double y = xyz_trans[1];
+        double z = xyz_trans[2];
+
+        Eigen::Matrix<double,3,6> SE3deriv;
+        SE3deriv << 0.f, z,   -y, 1.f, 0.f, 0.f,
+                     -z , 0.f, x, 0.f, 1.f, 0.f,
+                     y ,  -x , 0.f, 0.f, 0.f, 1.f;
+
+        _jacobianOplusXi = -pCamera->projectJac(xyz_trans) * SE3deriv;
+    }
+
+    bool EdgeSE3ProjectXYZOnlyPoseToBody::read(std::istream& is){
+        for (int i=0; i<2; i++){
+            is >> _measurement[i];
+        }
+        for (int i=0; i<2; i++)
+            for (int j=i; j<2; j++) {
+                is >> information()(i,j);
+                if (i!=j)
+                    information()(j,i)=information()(i,j);
+            }
+        return true;
+    }
+
+    bool EdgeSE3ProjectXYZOnlyPoseToBody::write(std::ostream& os) const {
+
+        for (int i=0; i<2; i++){
+            os << measurement()[i] << " ";
+        }
+
+        for (int i=0; i<2; i++)
+            for (int j=i; j<2; j++){
+                os << " " <<  information()(i,j);
+            }
+        return os.good();
+    }
+
+    void EdgeSE3ProjectXYZOnlyPoseToBody::linearizeOplus() {
+        g2o::VertexSE3Expmap * vi = static_cast<g2o::VertexSE3Expmap *>(_vertices[0]);
+        g2o::SE3Quat T_lw(vi->estimate());
+        Eigen::Vector3d X_l = T_lw.map(Xw);
+        Eigen::Vector3d X_r = mTrl.map(T_lw.map(Xw));
+
+        double x_w = X_l[0];
+        double y_w = X_l[1];
+        double z_w = X_l[2];
+
+        Eigen::Matrix<double,3,6> SE3deriv;
+        SE3deriv << 0.f, z_w,   -y_w, 1.f, 0.f, 0.f,
+                -z_w , 0.f, x_w, 0.f, 1.f, 0.f,
+                y_w ,  -x_w , 0.f, 0.f, 0.f, 1.f;
+
+        _jacobianOplusXi = -pCamera->projectJac(X_r) * mTrl.rotation().toRotationMatrix() * SE3deriv;
+    }
+
+    EdgeSE3ProjectXYZ::EdgeSE3ProjectXYZ() : BaseBinaryEdge<2, Eigen::Vector2d, g2o::VertexSBAPointXYZ, g2o::VertexSE3Expmap>() {
+    }
+
+    bool EdgeSE3ProjectXYZ::read(std::istream& is){
+        for (int i=0; i<2; i++){
+            is >> _measurement[i];
+        }
+        for (int i=0; i<2; i++)
+            for (int j=i; j<2; j++) {
+                is >> information()(i,j);
+                if (i!=j)
+                    information()(j,i)=information()(i,j);
+            }
+        return true;
+    }
+
+    bool EdgeSE3ProjectXYZ::write(std::ostream& os) const {
+
+        for (int i=0; i<2; i++){
+            os << measurement()[i] << " ";
+        }
+
+        for (int i=0; i<2; i++)
+            for (int j=i; j<2; j++){
+                os << " " <<  information()(i,j);
+            }
+        return os.good();
+    }
+
+
+    void EdgeSE3ProjectXYZ::linearizeOplus() {
+        g2o::VertexSE3Expmap * vj = static_cast<g2o::VertexSE3Expmap *>(_vertices[1]);
+        g2o::SE3Quat T(vj->estimate());
+        g2o::VertexSBAPointXYZ* vi = static_cast<g2o::VertexSBAPointXYZ*>(_vertices[0]);
+        Eigen::Vector3d xyz = vi->estimate();
+        Eigen::Vector3d xyz_trans = T.map(xyz);
+
+        double x = xyz_trans[0];
+        double y = xyz_trans[1];
+        double z = xyz_trans[2];
+
+        auto projectJac = -pCamera->projectJac(xyz_trans);
+
+        _jacobianOplusXi =  projectJac * T.rotation().toRotationMatrix();
+
+        Eigen::Matrix<double,3,6> SE3deriv;
+        SE3deriv << 0.f, z,   -y, 1.f, 0.f, 0.f,
+                -z , 0.f, x, 0.f, 1.f, 0.f,
+                y ,  -x , 0.f, 0.f, 0.f, 1.f;
+
+        _jacobianOplusXj = projectJac * SE3deriv;
+    }
+
+    EdgeSE3ProjectXYZToBody::EdgeSE3ProjectXYZToBody() : BaseBinaryEdge<2, Eigen::Vector2d, g2o::VertexSBAPointXYZ, g2o::VertexSE3Expmap>() {
+    }
+
+    bool EdgeSE3ProjectXYZToBody::read(std::istream& is){
+        for (int i=0; i<2; i++){
+            is >> _measurement[i];
+        }
+        for (int i=0; i<2; i++)
+            for (int j=i; j<2; j++) {
+                is >> information()(i,j);
+                if (i!=j)
+                    information()(j,i)=information()(i,j);
+            }
+        return true;
+    }
+
+    bool EdgeSE3ProjectXYZToBody::write(std::ostream& os) const {
+
+        for (int i=0; i<2; i++){
+            os << measurement()[i] << " ";
+        }
+
+        for (int i=0; i<2; i++)
+            for (int j=i; j<2; j++){
+                os << " " <<  information()(i,j);
+            }
+        return os.good();
+    }
+
+
+    void EdgeSE3ProjectXYZToBody::linearizeOplus() {
+        g2o::VertexSE3Expmap * vj = static_cast<g2o::VertexSE3Expmap *>(_vertices[1]);
+        g2o::SE3Quat T_lw(vj->estimate());
+        g2o::SE3Quat T_rw = mTrl * T_lw;
+        g2o::VertexSBAPointXYZ* vi = static_cast<g2o::VertexSBAPointXYZ*>(_vertices[0]);
+        Eigen::Vector3d X_w = vi->estimate();
+        Eigen::Vector3d X_l = T_lw.map(X_w);
+        Eigen::Vector3d X_r = mTrl.map(T_lw.map(X_w));
+
+        _jacobianOplusXi =  -pCamera->projectJac(X_r) * T_rw.rotation().toRotationMatrix();
+
+        double x = X_l[0];
+        double y = X_l[1];
+        double z = X_l[2];
+
+        Eigen::Matrix<double,3,6> SE3deriv;
+        SE3deriv << 0.f, z,   -y, 1.f, 0.f, 0.f,
+                -z , 0.f, x, 0.f, 1.f, 0.f,
+                y ,  -x , 0.f, 0.f, 0.f, 1.f;
+
+        _jacobianOplusXj = -pCamera->projectJac(X_r) * mTrl.rotation().toRotationMatrix() * SE3deriv;
+    }
+
+
+    VertexSim3Expmap::VertexSim3Expmap() : BaseVertex<7, g2o::Sim3>()
+    {
+        _marginalized=false;
+        _fix_scale = false;
+    }
+
+    bool VertexSim3Expmap::read(std::istream& is)
+    {
+        g2o::Vector7d cam2world;
+        for (int i=0; i<6; i++){
+            is >> cam2world[i];
+        }
+        is >> cam2world[6];
+
+        float nextParam;
+        for(size_t i = 0; i < pCamera1->size(); i++){
+            is >> nextParam;
+            pCamera1->setParameter(nextParam,i);
+        }
+
+        for(size_t i = 0; i < pCamera2->size(); i++){
+            is >> nextParam;
+            pCamera2->setParameter(nextParam,i);
+        }
+
+        setEstimate(g2o::Sim3(cam2world).inverse());
+        return true;
+    }
+
+    bool VertexSim3Expmap::write(std::ostream& os) const
+    {
+        g2o::Sim3 cam2world(estimate().inverse());
+        g2o::Vector7d lv=cam2world.log();
+        for (int i=0; i<7; i++){
+            os << lv[i] << " ";
+        }
+
+        for(size_t i = 0; i < pCamera1->size(); i++){
+            os << pCamera1->getParameter(i) << " ";
+        }
+
+        for(size_t i = 0; i < pCamera2->size(); i++){
+            os << pCamera2->getParameter(i) << " ";
+        }
+
+        return os.good();
+    }
+
+    EdgeSim3ProjectXYZ::EdgeSim3ProjectXYZ() :
+            g2o::BaseBinaryEdge<2, Eigen::Vector2d, g2o::VertexSBAPointXYZ, VertexSim3Expmap>()
+    {
+    }
+
+    bool EdgeSim3ProjectXYZ::read(std::istream& is)
+    {
+        for (int i=0; i<2; i++)
+        {
+            is >> _measurement[i];
+        }
+
+        for (int i=0; i<2; i++)
+            for (int j=i; j<2; j++) {
+                is >> information()(i,j);
+                if (i!=j)
+                    information()(j,i)=information()(i,j);
+            }
+        return true;
+    }
+
+    bool EdgeSim3ProjectXYZ::write(std::ostream& os) const
+    {
+        for (int i=0; i<2; i++){
+            os  << _measurement[i] << " ";
+        }
+
+        for (int i=0; i<2; i++)
+            for (int j=i; j<2; j++){
+                os << " " <<  information()(i,j);
+            }
+        return os.good();
+    }
+
+    EdgeInverseSim3ProjectXYZ::EdgeInverseSim3ProjectXYZ() :
+            g2o::BaseBinaryEdge<2, Eigen::Vector2d, g2o::VertexSBAPointXYZ, VertexSim3Expmap>()
+    {
+    }
+
+    bool EdgeInverseSim3ProjectXYZ::read(std::istream& is)
+    {
+        for (int i=0; i<2; i++)
+        {
+            is >> _measurement[i];
+        }
+
+        for (int i=0; i<2; i++)
+            for (int j=i; j<2; j++) {
+                is >> information()(i,j);
+                if (i!=j)
+                    information()(j,i)=information()(i,j);
+            }
+        return true;
+    }
+
+    bool EdgeInverseSim3ProjectXYZ::write(std::ostream& os) const
+    {
+        for (int i=0; i<2; i++){
+            os  << _measurement[i] << " ";
+        }
+
+        for (int i=0; i<2; i++)
+            for (int j=i; j<2; j++){
+                os << " " <<  information()(i,j);
+            }
+        return os.good();
+    }
+
+}

src/Sim3Solver.cc

@@ -0,0 +1,508 @@
+/**
+* This file is part of ORB-SLAM3
+*
+* Copyright (C) 2017-2020 Carlos Campos, Richard Elvira, Juan J. Gómez Rodríguez, José M.M. Montiel and Juan D. Tardós, University of Zaragoza.
+* Copyright (C) 2014-2016 Raúl Mur-Artal, José M.M. Montiel and Juan D. Tardós, University of Zaragoza.
+*
+* ORB-SLAM3 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-SLAM3 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-SLAM3.
+* If not, see <http://www.gnu.org/licenses/>.
+*/
+
+
+#include "Sim3Solver.h"
+
+#include <vector>
+#include <cmath>
+#include <opencv2/core/core.hpp>
+
+#include "KeyFrame.h"
+#include "ORBmatcher.h"
+
+#include "Thirdparty/DBoW2/DUtils/Random.h"
+
+namespace ORB_SLAM3
+{
+
+
+Sim3Solver::Sim3Solver(KeyFrame *pKF1, KeyFrame *pKF2, const vector<MapPoint *> &vpMatched12, const bool bFixScale,
+                       vector<KeyFrame*> vpKeyFrameMatchedMP):
+    mnIterations(0), mnBestInliers(0), mbFixScale(bFixScale),
+    pCamera1(pKF1->mpCamera), pCamera2(pKF2->mpCamera)
+{
+    bool bDifferentKFs = false;
+    if(vpKeyFrameMatchedMP.empty())
+    {
+        bDifferentKFs = true;
+        vpKeyFrameMatchedMP = vector<KeyFrame*>(vpMatched12.size(), pKF2);
+    }
+
+    mpKF1 = pKF1;
+    mpKF2 = pKF2;
+
+    vector<MapPoint*> vpKeyFrameMP1 = pKF1->GetMapPointMatches();
+
+    mN1 = vpMatched12.size();
+
+    mvpMapPoints1.reserve(mN1);
+    mvpMapPoints2.reserve(mN1);
+    mvpMatches12 = vpMatched12;
+    mvnIndices1.reserve(mN1);
+    mvX3Dc1.reserve(mN1);
+    mvX3Dc2.reserve(mN1);
+
+    cv::Mat Rcw1 = pKF1->GetRotation();
+    cv::Mat tcw1 = pKF1->GetTranslation();
+    cv::Mat Rcw2 = pKF2->GetRotation();
+    cv::Mat tcw2 = pKF2->GetTranslation();
+
+    mvAllIndices.reserve(mN1);
+
+    size_t idx=0;
+
+    KeyFrame* pKFm = pKF2; //Default variable
+    for(int i1=0; i1<mN1; i1++)
+    {
+        if(vpMatched12[i1])
+        {
+            MapPoint* pMP1 = vpKeyFrameMP1[i1];
+            MapPoint* pMP2 = vpMatched12[i1];
+
+            if(!pMP1)
+                continue;
+
+            if(pMP1->isBad() || pMP2->isBad())
+                continue;
+
+            if(bDifferentKFs)
+                pKFm = vpKeyFrameMatchedMP[i1];
+
+            int indexKF1 = get<0>(pMP1->GetIndexInKeyFrame(pKF1));
+            int indexKF2 = get<0>(pMP2->GetIndexInKeyFrame(pKFm));
+
+            if(indexKF1<0 || indexKF2<0)
+                continue;
+
+            const cv::KeyPoint &kp1 = pKF1->mvKeysUn[indexKF1];
+            const cv::KeyPoint &kp2 = pKFm->mvKeysUn[indexKF2];
+
+            const float sigmaSquare1 = pKF1->mvLevelSigma2[kp1.octave];
+            const float sigmaSquare2 = pKFm->mvLevelSigma2[kp2.octave];
+
+            mvnMaxError1.push_back(9.210*sigmaSquare1);
+            mvnMaxError2.push_back(9.210*sigmaSquare2);
+
+            mvpMapPoints1.push_back(pMP1);
+            mvpMapPoints2.push_back(pMP2);
+            mvnIndices1.push_back(i1);
+
+            cv::Mat X3D1w = pMP1->GetWorldPos();
+            mvX3Dc1.push_back(Rcw1*X3D1w+tcw1);
+
+            cv::Mat X3D2w = pMP2->GetWorldPos();
+            mvX3Dc2.push_back(Rcw2*X3D2w+tcw2);
+
+            mvAllIndices.push_back(idx);
+            idx++;
+        }
+    }
+
+    mK1 = pKF1->mK;
+    mK2 = pKF2->mK;
+
+    FromCameraToImage(mvX3Dc1,mvP1im1,pCamera1);
+    FromCameraToImage(mvX3Dc2,mvP2im2,pCamera2);
+
+    SetRansacParameters();
+}
+
+void Sim3Solver::SetRansacParameters(double probability, int minInliers, int maxIterations)
+{
+    mRansacProb = probability;
+    mRansacMinInliers = minInliers;
+    mRansacMaxIts = maxIterations;    
+
+    N = mvpMapPoints1.size(); // number of correspondences
+
+    mvbInliersi.resize(N);
+
+    // Adjust Parameters according to number of correspondences
+    float epsilon = (float)mRansacMinInliers/N;
+
+    // Set RANSAC iterations according to probability, epsilon, and max iterations
+    int nIterations;
+
+    if(mRansacMinInliers==N)
+        nIterations=1;
+    else
+        nIterations = ceil(log(1-mRansacProb)/log(1-pow(epsilon,3)));
+
+    mRansacMaxIts = max(1,min(nIterations,mRansacMaxIts));
+
+    mnIterations = 0;
+}
+
+cv::Mat Sim3Solver::iterate(int nIterations, bool &bNoMore, vector<bool> &vbInliers, int &nInliers)
+{
+    bNoMore = false;
+    vbInliers = vector<bool>(mN1,false);
+    nInliers=0;
+
+    if(N<mRansacMinInliers)
+    {
+        bNoMore = true;
+        return cv::Mat();
+    }
+
+    vector<size_t> vAvailableIndices;
+
+    cv::Mat P3Dc1i(3,3,CV_32F);
+    cv::Mat P3Dc2i(3,3,CV_32F);
+
+    int nCurrentIterations = 0;
+    while(mnIterations<mRansacMaxIts && nCurrentIterations<nIterations)
+    {
+        nCurrentIterations++;
+        mnIterations++;
+
+        vAvailableIndices = mvAllIndices;
+
+        // 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];
+
+            mvX3Dc1[idx].copyTo(P3Dc1i.col(i));
+            mvX3Dc2[idx].copyTo(P3Dc2i.col(i));
+
+            vAvailableIndices[randi] = vAvailableIndices.back();
+            vAvailableIndices.pop_back();
+        }
+
+        ComputeSim3(P3Dc1i,P3Dc2i);
+
+        CheckInliers();
+
+        if(mnInliersi>=mnBestInliers)
+        {
+            mvbBestInliers = mvbInliersi;
+            mnBestInliers = mnInliersi;
+            mBestT12 = mT12i.clone();
+            mBestRotation = mR12i.clone();
+            mBestTranslation = mt12i.clone();
+            mBestScale = ms12i;
+
+            if(mnInliersi>mRansacMinInliers)
+            {
+                nInliers = mnInliersi;
+                for(int i=0; i<N; i++)
+                    if(mvbInliersi[i])
+                        vbInliers[mvnIndices1[i]] = true;
+                return mBestT12;
+            }
+        }
+    }
+
+    if(mnIterations>=mRansacMaxIts)
+        bNoMore=true;
+
+    return cv::Mat();
+}
+
+cv::Mat Sim3Solver::iterate(int nIterations, bool &bNoMore, vector<bool> &vbInliers, int &nInliers, bool &bConverge)
+{
+    bNoMore = false;
+    bConverge = false;
+    vbInliers = vector<bool>(mN1,false);
+    nInliers=0;
+
+    if(N<mRansacMinInliers)
+    {
+        bNoMore = true;
+        return cv::Mat();
+    }
+
+    vector<size_t> vAvailableIndices;
+
+    cv::Mat P3Dc1i(3,3,CV_32F);
+    cv::Mat P3Dc2i(3,3,CV_32F);
+
+    int nCurrentIterations = 0;
+
+    cv::Mat bestSim3;
+
+    while(mnIterations<mRansacMaxIts && nCurrentIterations<nIterations)
+    {
+        nCurrentIterations++;
+        mnIterations++;
+
+        vAvailableIndices = mvAllIndices;
+
+        // 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];
+
+            mvX3Dc1[idx].copyTo(P3Dc1i.col(i));
+            mvX3Dc2[idx].copyTo(P3Dc2i.col(i));
+
+            vAvailableIndices[randi] = vAvailableIndices.back();
+            vAvailableIndices.pop_back();
+        }
+
+        ComputeSim3(P3Dc1i,P3Dc2i);
+
+        CheckInliers();
+
+        if(mnInliersi>=mnBestInliers)
+        {
+            mvbBestInliers = mvbInliersi;
+            mnBestInliers = mnInliersi;
+            mBestT12 = mT12i.clone();
+            mBestRotation = mR12i.clone();
+            mBestTranslation = mt12i.clone();
+            mBestScale = ms12i;
+
+            if(mnInliersi>mRansacMinInliers)
+            {
+                nInliers = mnInliersi;
+                for(int i=0; i<N; i++)
+                    if(mvbInliersi[i])
+                        vbInliers[mvnIndices1[i]] = true;
+                bConverge = true;
+                return mBestT12;
+            }
+            else
+            {
+                bestSim3 = mBestT12;
+            }
+        }
+    }
+
+    if(mnIterations>=mRansacMaxIts)
+        bNoMore=true;
+
+    return bestSim3;
+}
+
+cv::Mat Sim3Solver::find(vector<bool> &vbInliers12, int &nInliers)
+{
+    bool bFlag;
+    return iterate(mRansacMaxIts,bFlag,vbInliers12,nInliers);
+}
+
+void Sim3Solver::ComputeCentroid(cv::Mat &P, cv::Mat &Pr, cv::Mat &C)
+{
+    cv::reduce(P,C,1,cv::REDUCE_SUM);
+    C = C/P.cols;
+
+    for(int i=0; i<P.cols; i++)
+    {
+        Pr.col(i)=P.col(i)-C;
+    }
+}
+
+void Sim3Solver::ComputeSim3(cv::Mat &P1, cv::Mat &P2)
+{
+    // Custom implementation of:
+    // Horn 1987, Closed-form solution of absolute orientataion using unit quaternions
+
+    // Step 1: Centroid and relative coordinates
+
+    cv::Mat Pr1(P1.size(),P1.type()); // Relative coordinates to centroid (set 1)
+    cv::Mat Pr2(P2.size(),P2.type()); // Relative coordinates to centroid (set 2)
+    cv::Mat O1(3,1,Pr1.type()); // Centroid of P1
+    cv::Mat O2(3,1,Pr2.type()); // Centroid of P2
+
+    ComputeCentroid(P1,Pr1,O1);
+    ComputeCentroid(P2,Pr2,O2);
+
+    // Step 2: Compute M matrix
+
+    cv::Mat M = Pr2*Pr1.t();
+
+    // Step 3: Compute N matrix
+
+    double N11, N12, N13, N14, N22, N23, N24, N33, N34, N44;
+
+    cv::Mat N(4,4,P1.type());
+
+    N11 = M.at<float>(0,0)+M.at<float>(1,1)+M.at<float>(2,2);
+    N12 = M.at<float>(1,2)-M.at<float>(2,1);
+    N13 = M.at<float>(2,0)-M.at<float>(0,2);
+    N14 = M.at<float>(0,1)-M.at<float>(1,0);
+    N22 = M.at<float>(0,0)-M.at<float>(1,1)-M.at<float>(2,2);
+    N23 = M.at<float>(0,1)+M.at<float>(1,0);
+    N24 = M.at<float>(2,0)+M.at<float>(0,2);
+    N33 = -M.at<float>(0,0)+M.at<float>(1,1)-M.at<float>(2,2);
+    N34 = M.at<float>(1,2)+M.at<float>(2,1);
+    N44 = -M.at<float>(0,0)-M.at<float>(1,1)+M.at<float>(2,2);
+
+    N = (cv::Mat_<float>(4,4) << N11, N12, N13, N14,
+                                 N12, N22, N23, N24,
+                                 N13, N23, N33, N34,
+                                 N14, N24, N34, N44);
+
+
+    // Step 4: Eigenvector of the highest eigenvalue
+
+    cv::Mat eval, evec;
+
+    cv::eigen(N,eval,evec); //evec[0] is the quaternion of the desired rotation
+
+    cv::Mat vec(1,3,evec.type());
+    (evec.row(0).colRange(1,4)).copyTo(vec); //extract imaginary part of the quaternion (sin*axis)
+
+    // Rotation angle. sin is the norm of the imaginary part, cos is the real part
+    double ang=atan2(norm(vec),evec.at<float>(0,0));
+
+    vec = 2*ang*vec/norm(vec); //Angle-axis representation. quaternion angle is the half
+
+    mR12i.create(3,3,P1.type());
+
+    cv::Rodrigues(vec,mR12i); // computes the rotation matrix from angle-axis
+
+    // Step 5: Rotate set 2
+
+    cv::Mat P3 = mR12i*Pr2;
+
+    // Step 6: Scale
+
+    if(!mbFixScale)
+    {
+        double nom = Pr1.dot(P3);
+        cv::Mat aux_P3(P3.size(),P3.type());
+        aux_P3=P3;
+        cv::pow(P3,2,aux_P3);
+        double den = 0;
+
+        for(int i=0; i<aux_P3.rows; i++)
+        {
+            for(int j=0; j<aux_P3.cols; j++)
+            {
+                den+=aux_P3.at<float>(i,j);
+            }
+        }
+
+        ms12i = nom/den;
+    }
+    else
+        ms12i = 1.0f;
+
+    // Step 7: Translation
+
+    mt12i.create(1,3,P1.type());
+    mt12i = O1 - ms12i*mR12i*O2;
+
+    // Step 8: Transformation
+
+    // Step 8.1 T12
+    mT12i = cv::Mat::eye(4,4,P1.type());
+
+    cv::Mat sR = ms12i*mR12i;
+
+    sR.copyTo(mT12i.rowRange(0,3).colRange(0,3));
+    mt12i.copyTo(mT12i.rowRange(0,3).col(3));
+
+    // Step 8.2 T21
+
+    mT21i = cv::Mat::eye(4,4,P1.type());
+
+    cv::Mat sRinv = (1.0/ms12i)*mR12i.t();
+
+    sRinv.copyTo(mT21i.rowRange(0,3).colRange(0,3));
+    cv::Mat tinv = -sRinv*mt12i;
+    tinv.copyTo(mT21i.rowRange(0,3).col(3));
+}
+
+
+void Sim3Solver::CheckInliers()
+{
+    vector<cv::Mat> vP1im2, vP2im1;
+    Project(mvX3Dc2,vP2im1,mT12i,pCamera1);
+    Project(mvX3Dc1,vP1im2,mT21i,pCamera2);
+
+    mnInliersi=0;
+
+    for(size_t i=0; i<mvP1im1.size(); i++)
+    {
+        cv::Mat dist1 = mvP1im1[i]-vP2im1[i];
+        cv::Mat dist2 = vP1im2[i]-mvP2im2[i];
+
+        const float err1 = dist1.dot(dist1);
+        const float err2 = dist2.dot(dist2);
+
+        if(err1<mvnMaxError1[i] && err2<mvnMaxError2[i])
+        {
+            mvbInliersi[i]=true;
+            mnInliersi++;
+        }
+        else
+            mvbInliersi[i]=false;
+    }
+}
+
+
+cv::Mat Sim3Solver::GetEstimatedRotation()
+{
+    return mBestRotation.clone();
+}
+
+cv::Mat Sim3Solver::GetEstimatedTranslation()
+{
+    return mBestTranslation.clone();
+}
+
+float Sim3Solver::GetEstimatedScale()
+{
+    return mBestScale;
+}
+
+void Sim3Solver::Project(const vector<cv::Mat> &vP3Dw, vector<cv::Mat> &vP2D, cv::Mat Tcw, GeometricCamera* pCamera)
+{
+    cv::Mat Rcw = Tcw.rowRange(0,3).colRange(0,3);
+    cv::Mat tcw = Tcw.rowRange(0,3).col(3);
+
+    vP2D.clear();
+    vP2D.reserve(vP3Dw.size());
+
+    for(size_t i=0, iend=vP3Dw.size(); i<iend; i++)
+    {
+        cv::Mat P3Dc = Rcw*vP3Dw[i]+tcw;
+        const float invz = 1/(P3Dc.at<float>(2));
+        const float x = P3Dc.at<float>(0);
+        const float y = P3Dc.at<float>(1);
+        const float z = P3Dc.at<float>(2);
+
+        vP2D.push_back(pCamera->projectMat(cv::Point3f(x,y,z)));
+    }
+}
+
+void Sim3Solver::FromCameraToImage(const vector<cv::Mat> &vP3Dc, vector<cv::Mat> &vP2D, GeometricCamera* pCamera)
+{
+    vP2D.clear();
+    vP2D.reserve(vP3Dc.size());
+
+    for(size_t i=0, iend=vP3Dc.size(); i<iend; i++)
+    {
+        const float invz = 1/(vP3Dc[i].at<float>(2));
+        const float x = vP3Dc[i].at<float>(0);
+        const float y = vP3Dc[i].at<float>(1);
+        const float z = vP3Dc[i].at<float>(2);
+
+        vP2D.push_back(pCamera->projectMat(cv::Point3f(x,y,z)));
+    }
+}
+
+} //namespace ORB_SLAM

src/System.cc

@@ -0,0 +1,790 @@
+/**
+* This file is part of ORB-SLAM3
+*
+* Copyright (C) 2017-2020 Carlos Campos, Richard Elvira, Juan J. Gómez Rodríguez, José M.M. Montiel and Juan D. Tardós, University of Zaragoza.
+* Copyright (C) 2014-2016 Raúl Mur-Artal, José M.M. Montiel and Juan D. Tardós, University of Zaragoza.
+*
+* ORB-SLAM3 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-SLAM3 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-SLAM3.
+* If not, see <http://www.gnu.org/licenses/>.
+*/
+
+
+
+#include "System.h"
+#include "Converter.h"
+#include <thread>
+#include <pangolin/pangolin.h>
+#include <iomanip>
+#include <openssl/md5.h>
+#include <boost/serialization/base_object.hpp>
+#include <boost/serialization/string.hpp>
+#include <boost/archive/text_iarchive.hpp>
+#include <boost/archive/text_oarchive.hpp>
+#include <boost/archive/binary_iarchive.hpp>
+#include <boost/archive/binary_oarchive.hpp>
+#include <boost/archive/xml_iarchive.hpp>
+#include <boost/archive/xml_oarchive.hpp>
+// #include "unistd.h"
+
+void usleep(__int64 usec) {
+    std::this_thread::sleep_for(std::chrono::microseconds(usec));
+}
+
+namespace ORB_SLAM3
+{
+
+Verbose::eLevel Verbose::th = Verbose::VERBOSITY_NORMAL;
+
+
+System::System(const string &strVocFile, const string &strSettingsFile, const eSensor sensor,
+               const bool bUseViewer, const int initFr, const string &strSequence, const string &strLoadingFile):
+    mSensor(sensor), mpViewer(static_cast<Viewer*>(NULL)), mbReset(false), mbResetActiveMap(false),
+    mbActivateLocalizationMode(false), mbDeactivateLocalizationMode(false)
+{
+    // Output welcome message
+    cout << endl <<
+    "ORB-SLAM3 Copyright (C) 2017-2020 Carlos Campos, Richard Elvira, Juan J. Gómez, José M.M. Montiel and Juan D. Tardós, University of Zaragoza." << endl <<
+    "ORB-SLAM2 Copyright (C) 2014-2016 Raúl Mur-Artal, José M.M. Montiel and Juan D. Tardós, University of Zaragoza." << endl <<
+    "This program comes with ABSOLUTELY NO WARRANTY;" << endl  <<
+    "This is free software, and you are welcome to redistribute it" << endl <<
+    "under certain conditions. See LICENSE.txt." << endl << endl;
+
+    cout << "Input sensor was set to: ";
+
+    if(mSensor==MONOCULAR)
+        cout << "Monocular" << endl;
+    else if(mSensor==STEREO)
+        cout << "Stereo" << endl;
+    else if(mSensor==RGBD)
+        cout << "RGB-D" << endl;
+    else if(mSensor==IMU_MONOCULAR)
+        cout << "Monocular-Inertial" << endl;
+    else if(mSensor==IMU_STEREO)
+        cout << "Stereo-Inertial" << endl;
+
+    //Check settings file
+    cv::FileStorage fsSettings(strSettingsFile.c_str(), cv::FileStorage::READ);
+    if(!fsSettings.isOpened())
+    {
+       cerr << "Failed to open settings file at: " << strSettingsFile << endl;
+       exit(-1);
+    }
+
+    bool loadedAtlas = false;
+
+    //----
+    //Load ORB Vocabulary
+    cout << endl << "Loading ORB Vocabulary. This could take a while..." << endl;
+
+    mpVocabulary = new ORBVocabulary();
+    bool bVocLoad = mpVocabulary->loadFromTextFile(strVocFile);
+    if(!bVocLoad)
+    {
+        cerr << "Wrong path to vocabulary. " << endl;
+        cerr << "Falied to open at: " << strVocFile << endl;
+        exit(-1);
+    }
+    cout << "Vocabulary loaded!" << endl << endl;
+
+    //Create KeyFrame Database
+    mpKeyFrameDatabase = new KeyFrameDatabase(*mpVocabulary);
+
+    //Create the Atlas
+    mpAtlas = new Atlas(0);
+
+    if (mSensor==IMU_STEREO || mSensor==IMU_MONOCULAR)
+        mpAtlas->SetInertialSensor();
+
+    //Create Drawers. These are used by the Viewer
+    mpFrameDrawer = new FrameDrawer(mpAtlas);
+    mpMapDrawer = new MapDrawer(mpAtlas, strSettingsFile);
+
+    //Initialize the Tracking thread
+    //(it will live in the main thread of execution, the one that called this constructor)
+    cout << "Seq. Name: " << strSequence << endl;
+    mpTracker = new Tracking(this, mpVocabulary, mpFrameDrawer, mpMapDrawer,
+                             mpAtlas, mpKeyFrameDatabase, strSettingsFile, mSensor, strSequence);
+
+    //Initialize the Local Mapping thread and launch
+    mpLocalMapper = new LocalMapping(this, mpAtlas, mSensor==MONOCULAR || mSensor==IMU_MONOCULAR, mSensor==IMU_MONOCULAR || mSensor==IMU_STEREO, strSequence);
+    mptLocalMapping = new thread(&ORB_SLAM3::LocalMapping::Run,mpLocalMapper);
+    mpLocalMapper->mThFarPoints = fsSettings["thFarPoints"];
+    if(mpLocalMapper->mThFarPoints!=0)
+    {
+        cout << "Discard points further than " << mpLocalMapper->mThFarPoints << " m from current camera" << endl;
+        mpLocalMapper->mbFarPoints = true;
+    }
+    else
+        mpLocalMapper->mbFarPoints = false;
+
+    //Initialize the Loop Closing thread and launch
+    mpLoopCloser = new LoopClosing(mpAtlas, mpKeyFrameDatabase, mpVocabulary, mSensor!=MONOCULAR); // mSensor!=MONOCULAR);
+    mptLoopClosing = new thread(&ORB_SLAM3::LoopClosing::Run, mpLoopCloser);
+
+    //Initialize the Viewer thread and launch
+    if(bUseViewer)
+    {
+        mpViewer = new Viewer(this, mpFrameDrawer,mpMapDrawer,mpTracker,strSettingsFile);
+        mptViewer = new thread(&Viewer::Run, mpViewer);
+        mpTracker->SetViewer(mpViewer);
+        mpLoopCloser->mpViewer = mpViewer;
+        mpViewer->both = mpFrameDrawer->both;
+    }
+
+    //Set pointers between threads
+    mpTracker->SetLocalMapper(mpLocalMapper);
+    mpTracker->SetLoopClosing(mpLoopCloser);
+
+    mpLocalMapper->SetTracker(mpTracker);
+    mpLocalMapper->SetLoopCloser(mpLoopCloser);
+
+    mpLoopCloser->SetTracker(mpTracker);
+    mpLoopCloser->SetLocalMapper(mpLocalMapper);
+
+    // Fix verbosity
+    Verbose::SetTh(Verbose::VERBOSITY_QUIET);
+
+}
+
+int System::GetCurID() {
+    return mpAtlas->GetCurrentMap()->GetId();
+}
+
+cv::Mat System::TrackStereo(const cv::Mat &imLeft, const cv::Mat &imRight, const double &timestamp, const vector<IMU::Point>& vImuMeas, string filename)
+{
+    if(mSensor!=STEREO && mSensor!=IMU_STEREO)
+    {
+        cerr << "ERROR: you called TrackStereo but input sensor was not set to Stereo nor Stereo-Inertial." << endl;
+        exit(-1);
+    }   
+
+    // Check mode change
+    {
+        unique_lock<mutex> lock(mMutexMode);
+        if(mbActivateLocalizationMode)
+        {
+            mpLocalMapper->RequestStop();
+
+            // Wait until Local Mapping has effectively stopped
+            while(!mpLocalMapper->isStopped(    ))
+            {
+                usleep(1000);
+            }
+
+            mpTracker->InformOnlyTracking(true);
+            mbActivateLocalizationMode = false;
+        }
+        if(mbDeactivateLocalizationMode)
+        {
+            mpTracker->InformOnlyTracking(false);
+            mpLocalMapper->Release();
+            mbDeactivateLocalizationMode = false;
+        }
+    }
+
+    // Check reset
+    {
+        unique_lock<mutex> lock(mMutexReset);
+        if(mbReset)
+        {
+            mpTracker->Reset();
+            cout << "Reset stereo..." << endl;
+            mbReset = false;
+            mbResetActiveMap = false;
+        }
+        else if(mbResetActiveMap)
+        {
+            mpTracker->ResetActiveMap();
+            mbResetActiveMap = false;
+        }
+    }
+
+    if (mSensor == System::IMU_STEREO)
+        for(size_t i_imu = 0; i_imu < vImuMeas.size(); i_imu++)
+            mpTracker->GrabImuData(vImuMeas[i_imu]);
+
+    cv::Mat Tcw = mpTracker->GrabImageStereo(imLeft,imRight,timestamp,filename);
+
+    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, string filename)
+{
+    if(mSensor!=RGBD)
+    {
+        cerr << "ERROR: you called TrackRGBD but input sensor was not set to RGBD." << endl;
+        exit(-1);
+    }    
+
+    // Check mode change
+    {
+        unique_lock<mutex> lock(mMutexMode);
+        if(mbActivateLocalizationMode)
+        {
+            mpLocalMapper->RequestStop();
+
+            // Wait until Local Mapping has effectively stopped
+            while(!mpLocalMapper->isStopped())
+            {
+                usleep(1000);
+            }
+
+            mpTracker->InformOnlyTracking(true);
+            mbActivateLocalizationMode = false;
+        }
+        if(mbDeactivateLocalizationMode)
+        {
+            mpTracker->InformOnlyTracking(false);
+            mpLocalMapper->Release();
+            mbDeactivateLocalizationMode = false;
+        }
+    }
+
+    // Check reset
+    {
+        unique_lock<mutex> lock(mMutexReset);
+        if(mbReset)
+        {
+            mpTracker->Reset();
+            mbReset = false;
+            mbResetActiveMap = false;
+        }
+        else if(mbResetActiveMap)
+        {
+            mpTracker->ResetActiveMap();
+            mbResetActiveMap = false;
+        }
+    }
+
+
+    cv::Mat Tcw = mpTracker->GrabImageRGBD(im,depthmap,timestamp,filename);
+
+    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, const vector<IMU::Point>& vImuMeas, string filename)
+{
+    if(mSensor!=MONOCULAR && mSensor!=IMU_MONOCULAR)
+    {
+        cerr << "ERROR: you called TrackMonocular but input sensor was not set to Monocular nor Monocular-Inertial." << endl;
+        exit(-1);
+    }
+
+    // Check mode change
+    {
+        unique_lock<mutex> lock(mMutexMode);
+        if(mbActivateLocalizationMode)
+        {
+            mpLocalMapper->RequestStop();
+
+            // Wait until Local Mapping has effectively stopped
+            while(!mpLocalMapper->isStopped())
+            {
+                usleep(1000);
+            }
+
+            mpTracker->InformOnlyTracking(true);
+            mbActivateLocalizationMode = false;
+        }
+        if(mbDeactivateLocalizationMode)
+        {
+            mpTracker->InformOnlyTracking(false);
+            mpLocalMapper->Release();
+            mbDeactivateLocalizationMode = false;
+        }
+    }
+
+    // Check reset
+    {
+        unique_lock<mutex> lock(mMutexReset);
+        if(mbReset)
+        {
+            mpTracker->Reset();
+            mbReset = false;
+            mbResetActiveMap = false;
+        }
+        else if(mbResetActiveMap)
+        {
+            cout << "SYSTEM-> Reseting active map in monocular case" << endl;
+            mpTracker->ResetActiveMap();
+            mbResetActiveMap = false;
+        }
+    }
+
+    if (mSensor == System::IMU_MONOCULAR)
+        for(size_t i_imu = 0; i_imu < vImuMeas.size(); i_imu++)
+            mpTracker->GrabImuData(vImuMeas[i_imu]);
+
+    cv::Mat Tcw = mpTracker->GrabImageMonocular(im,timestamp,filename);
+
+    unique_lock<mutex> lock2(mMutexState);
+    mTrackingState = mpTracker->mState;
+    mTrackedMapPoints = mpTracker->mCurrentFrame.mvpMapPoints;
+    mTrackedKeyPointsUn = mpTracker->mCurrentFrame.mvKeysUn;
+
+    return Tcw;
+}
+
+
+
+void System::ActivateLocalizationMode()
+{
+    unique_lock<mutex> lock(mMutexMode);
+    mbActivateLocalizationMode = true;
+}
+
+void System::DeactivateLocalizationMode()
+{
+    unique_lock<mutex> lock(mMutexMode);
+    mbDeactivateLocalizationMode = true;
+}
+
+bool System::MapChanged()
+{
+    static int n=0;
+    int curn = mpAtlas->GetLastBigChangeIdx();
+    if(n<curn)
+    {
+        n=curn;
+        return true;
+    }
+    else
+        return false;
+}
+
+void System::Reset()
+{
+    unique_lock<mutex> lock(mMutexReset);
+    mbReset = true;
+}
+
+void System::ResetActiveMap()
+{
+    unique_lock<mutex> lock(mMutexReset);
+    mbResetActiveMap = true;
+}
+
+void System::Shutdown()
+{
+    mpLocalMapper->RequestFinish();
+    mpLoopCloser->RequestFinish();
+    if(mpViewer)
+    {
+        mpViewer->RequestFinish();
+        while(!mpViewer->isFinished())
+            usleep(5000);
+    }
+
+    // Wait until all thread have effectively stopped
+    while(!mpLocalMapper->isFinished() || !mpLoopCloser->isFinished() || mpLoopCloser->isRunningGBA())
+    {
+        if(!mpLocalMapper->isFinished())
+            cout << "mpLocalMapper is not finished" << endl;
+        if(!mpLoopCloser->isFinished())
+            cout << "mpLoopCloser is not finished" << endl;
+        if(mpLoopCloser->isRunningGBA()){
+            cout << "mpLoopCloser is running GBA" << endl;
+            cout << "break anyway..." << endl;
+            break;
+        }
+        usleep(5000);
+    }
+
+    if(mpViewer)
+        pangolin::BindToContext("ORB-SLAM2: Map Viewer");
+
+#ifdef REGISTER_TIMES
+    mpTracker->PrintTimeStats();
+#endif
+}
+
+
+
+void System::SaveTrajectoryTUM(const string &filename)
+{
+    cout << endl << "Saving camera trajectory to " << filename << " ..." << endl;
+    if(mSensor==MONOCULAR)
+    {
+        cerr << "ERROR: SaveTrajectoryTUM cannot be used for monocular." << endl;
+        return;
+    }
+
+    vector<KeyFrame*> vpKFs = mpAtlas->GetAllKeyFrames();
+    sort(vpKFs.begin(),vpKFs.end(),KeyFrame::lId);
+
+    // Transform all keyframes so that the first keyframe is at the origin.
+    // After a loop closure the first keyframe might not be at the origin.
+    cv::Mat Two = vpKFs[0]->GetPoseInverse();
+
+    ofstream f;
+    f.open(filename.c_str());
+    f << fixed;
+
+    // Frame pose is stored relative to its reference keyframe (which is optimized by BA and pose graph).
+    // We need to get first the keyframe pose and then concatenate the relative transformation.
+    // Frames not localized (tracking failure) are not saved.
+
+    // For each frame we have a reference keyframe (lRit), the timestamp (lT) and a flag
+    // which is true when tracking failed (lbL).
+    list<ORB_SLAM3::KeyFrame*>::iterator lRit = mpTracker->mlpReferences.begin();
+    list<double>::iterator lT = mpTracker->mlFrameTimes.begin();
+    list<bool>::iterator lbL = mpTracker->mlbLost.begin();
+    for(list<cv::Mat>::iterator lit=mpTracker->mlRelativeFramePoses.begin(),
+        lend=mpTracker->mlRelativeFramePoses.end();lit!=lend;lit++, lRit++, lT++, lbL++)
+    {
+        if(*lbL)
+            continue;
+
+        KeyFrame* pKF = *lRit;
+
+        cv::Mat Trw = cv::Mat::eye(4,4,CV_32F);
+
+        // If the reference keyframe was culled, traverse the spanning tree to get a suitable keyframe.
+        while(pKF->isBad())
+        {
+            Trw = Trw*pKF->mTcp;
+            pKF = pKF->GetParent();
+        }
+
+        Trw = Trw*pKF->GetPose()*Two;
+
+        cv::Mat Tcw = (*lit)*Trw;
+        cv::Mat Rwc = Tcw.rowRange(0,3).colRange(0,3).t();
+        cv::Mat twc = -Rwc*Tcw.rowRange(0,3).col(3);
+
+        vector<float> q = Converter::toQuaternion(Rwc);
+
+        f << setprecision(6) << *lT << " " <<  setprecision(9) << twc.at<float>(0) << " " << twc.at<float>(1) << " " << twc.at<float>(2) << " " << q[0] << " " << q[1] << " " << q[2] << " " << q[3] << endl;
+    }
+    f.close();
+    // cout << endl << "trajectory saved!" << endl;
+}
+
+void System::SaveKeyFrameTrajectoryTUM(const string &filename)
+{
+    cout << endl << "Saving keyframe trajectory to " << filename << " ..." << endl;
+
+    vector<KeyFrame*> vpKFs = mpAtlas->GetAllKeyFrames();
+    sort(vpKFs.begin(),vpKFs.end(),KeyFrame::lId);
+
+    // Transform all keyframes so that the first keyframe is at the origin.
+    // After a loop closure the first keyframe might not be at the origin.
+    ofstream f;
+    f.open(filename.c_str());
+    f << fixed;
+
+    for(size_t i=0; i<vpKFs.size(); i++)
+    {
+        KeyFrame* pKF = vpKFs[i];
+
+       // pKF->SetPose(pKF->GetPose()*Two);
+
+        if(pKF->isBad())
+            continue;
+
+        cv::Mat R = pKF->GetRotation().t();
+        vector<float> q = Converter::toQuaternion(R);
+        cv::Mat t = pKF->GetCameraCenter();
+        f << setprecision(6) << pKF->mTimeStamp << setprecision(7) << " " << t.at<float>(0) << " " << t.at<float>(1) << " " << t.at<float>(2)
+          << " " << q[0] << " " << q[1] << " " << q[2] << " " << q[3] << endl;
+
+    }
+
+    f.close();
+}
+
+void System::SaveTrajectoryEuRoC(const string &filename)
+{
+
+    cout << endl << "Saving trajectory to " << filename << " ..." << endl;
+    /*if(mSensor==MONOCULAR)
+    {
+        cerr << "ERROR: SaveTrajectoryEuRoC cannot be used for monocular." << endl;
+        return;
+    }*/
+
+    vector<Map*> vpMaps = mpAtlas->GetAllMaps();
+    Map* pBiggerMap;
+    int numMaxKFs = 0;
+    for(Map* pMap :vpMaps)
+    {
+        if(pMap->GetAllKeyFrames().size() > numMaxKFs)
+        {
+            numMaxKFs = pMap->GetAllKeyFrames().size();
+            pBiggerMap = pMap;
+        }
+    }
+
+    vector<KeyFrame*> vpKFs = pBiggerMap->GetAllKeyFrames();
+    sort(vpKFs.begin(),vpKFs.end(),KeyFrame::lId);
+
+    // Transform all keyframes so that the first keyframe is at the origin.
+    // After a loop closure the first keyframe might not be at the origin.
+    cv::Mat Twb; // Can be word to cam0 or world to b dependingo on IMU or not.
+    if (mSensor==IMU_MONOCULAR || mSensor==IMU_STEREO)
+        Twb = vpKFs[0]->GetImuPose();
+    else
+        Twb = vpKFs[0]->GetPoseInverse();
+
+    ofstream f;
+    f.open(filename.c_str());
+    f << fixed;
+
+    // Frame pose is stored relative to its reference keyframe (which is optimized by BA and pose graph).
+    // We need to get first the keyframe pose and then concatenate the relative transformation.
+    // Frames not localized (tracking failure) are not saved.
+
+    // For each frame we have a reference keyframe (lRit), the timestamp (lT) and a flag
+    // which is true when tracking failed (lbL).
+    list<ORB_SLAM3::KeyFrame*>::iterator lRit = mpTracker->mlpReferences.begin();
+    list<double>::iterator lT = mpTracker->mlFrameTimes.begin();
+    list<bool>::iterator lbL = mpTracker->mlbLost.begin();
+
+    for(list<cv::Mat>::iterator lit=mpTracker->mlRelativeFramePoses.begin(),
+        lend=mpTracker->mlRelativeFramePoses.end();lit!=lend;lit++, lRit++, lT++, lbL++)
+    {
+        if(*lbL)
+            continue;
+
+
+        KeyFrame* pKF = *lRit;
+
+        cv::Mat Trw = cv::Mat::eye(4,4,CV_32F);
+
+        // If the reference keyframe was culled, traverse the spanning tree to get a suitable keyframe.
+        if (!pKF)
+            continue;
+
+        while(pKF->isBad())
+        {
+            Trw = Trw*pKF->mTcp;
+            pKF = pKF->GetParent();
+        }
+
+        if(!pKF || pKF->GetMap() != pBiggerMap)
+        {
+            continue;
+        }
+
+        Trw = Trw*pKF->GetPose()*Twb; // Tcp*Tpw*Twb0=Tcb0 where b0 is the new world reference
+
+        if (mSensor == IMU_MONOCULAR || mSensor == IMU_STEREO)
+        {
+            cv::Mat Tbw = pKF->mImuCalib.Tbc*(*lit)*Trw;
+            cv::Mat Rwb = Tbw.rowRange(0,3).colRange(0,3).t();
+            cv::Mat twb = -Rwb*Tbw.rowRange(0,3).col(3);
+            vector<float> q = Converter::toQuaternion(Rwb);
+            f << setprecision(6) << 1e9*(*lT) << " " <<  setprecision(9) << twb.at<float>(0) << " " << twb.at<float>(1) << " " << twb.at<float>(2) << " " << q[0] << " " << q[1] << " " << q[2] << " " << q[3] << endl;
+        }
+        else
+        {
+            cv::Mat Tcw = (*lit)*Trw;
+            cv::Mat Rwc = Tcw.rowRange(0,3).colRange(0,3).t();
+            cv::Mat twc = -Rwc*Tcw.rowRange(0,3).col(3);
+            vector<float> q = Converter::toQuaternion(Rwc);
+            f << setprecision(6) << 1e9*(*lT) << " " <<  setprecision(9) << twc.at<float>(0) << " " << twc.at<float>(1) << " " << twc.at<float>(2) << " " << q[0] << " " << q[1] << " " << q[2] << " " << q[3] << endl;
+        }
+
+    }
+    //cout << "end saving trajectory" << endl;
+    f.close();
+    cout << endl << "End of saving trajectory to " << filename << " ..." << endl;
+}
+
+
+void System::SaveKeyFrameTrajectoryEuRoC(const string &filename)
+{
+    cout << endl << "Saving keyframe trajectory to " << filename << " ..." << endl;
+
+    vector<Map*> vpMaps = mpAtlas->GetAllMaps();
+    Map* pBiggerMap;
+    int numMaxKFs = 0;
+    for(Map* pMap :vpMaps)
+    {
+        if(pMap->GetAllKeyFrames().size() > numMaxKFs)
+        {
+            numMaxKFs = pMap->GetAllKeyFrames().size();
+            pBiggerMap = pMap;
+        }
+    }
+
+    vector<KeyFrame*> vpKFs = pBiggerMap->GetAllKeyFrames();
+    sort(vpKFs.begin(),vpKFs.end(),KeyFrame::lId);
+
+    // Transform all keyframes so that the first keyframe is at the origin.
+    // After a loop closure the first keyframe might not be at the origin.
+    ofstream f;
+    f.open(filename.c_str());
+    f << fixed;
+
+    for(size_t i=0; i<vpKFs.size(); i++)
+    {
+        KeyFrame* pKF = vpKFs[i];
+
+        if(pKF->isBad())
+            continue;
+        if (mSensor == IMU_MONOCULAR || mSensor == IMU_STEREO)
+        {
+            cv::Mat R = pKF->GetImuRotation().t();
+            vector<float> q = Converter::toQuaternion(R);
+            cv::Mat twb = pKF->GetImuPosition();
+            f << setprecision(6) << 1e9*pKF->mTimeStamp  << " " <<  setprecision(9) << twb.at<float>(0) << " " << twb.at<float>(1) << " " << twb.at<float>(2) << " " << q[0] << " " << q[1] << " " << q[2] << " " << q[3] << endl;
+
+        }
+        else
+        {
+            cv::Mat R = pKF->GetRotation();
+            vector<float> q = Converter::toQuaternion(R);
+            cv::Mat t = pKF->GetCameraCenter();
+            f << setprecision(6) << 1e9*pKF->mTimeStamp << " " <<  setprecision(9) << t.at<float>(0) << " " << t.at<float>(1) << " " << t.at<float>(2) << " " << q[0] << " " << q[1] << " " << q[2] << " " << q[3] << endl;
+        }
+    }
+    f.close();
+}
+
+void System::SaveTrajectoryKITTI(const string &filename)
+{
+    cout << endl << "Saving camera trajectory to " << filename << " ..." << endl;
+    if(mSensor==MONOCULAR)
+    {
+        cerr << "ERROR: SaveTrajectoryKITTI cannot be used for monocular." << endl;
+        return;
+    }
+
+    vector<KeyFrame*> vpKFs = mpAtlas->GetAllKeyFrames();
+    sort(vpKFs.begin(),vpKFs.end(),KeyFrame::lId);
+
+    // Transform all keyframes so that the first keyframe is at the origin.
+    // After a loop closure the first keyframe might not be at the origin.
+    cv::Mat Two = vpKFs[0]->GetPoseInverse();
+
+    ofstream f;
+    f.open(filename.c_str());
+    f << fixed;
+
+    // Frame pose is stored relative to its reference keyframe (which is optimized by BA and pose graph).
+    // We need to get first the keyframe pose and then concatenate the relative transformation.
+    // Frames not localized (tracking failure) are not saved.
+
+    // For each frame we have a reference keyframe (lRit), the timestamp (lT) and a flag
+    // which is true when tracking failed (lbL).
+    list<ORB_SLAM3::KeyFrame*>::iterator lRit = mpTracker->mlpReferences.begin();
+    list<double>::iterator lT = mpTracker->mlFrameTimes.begin();
+    for(list<cv::Mat>::iterator lit=mpTracker->mlRelativeFramePoses.begin(), lend=mpTracker->mlRelativeFramePoses.end();lit!=lend;lit++, lRit++, lT++)
+    {
+        ORB_SLAM3::KeyFrame* pKF = *lRit;
+
+        cv::Mat Trw = cv::Mat::eye(4,4,CV_32F);
+
+        while(pKF->isBad())
+        {
+            Trw = Trw*pKF->mTcp;
+            pKF = pKF->GetParent();
+        }
+
+        Trw = Trw*pKF->GetPose()*Two;
+
+        cv::Mat Tcw = (*lit)*Trw;
+        cv::Mat Rwc = Tcw.rowRange(0,3).colRange(0,3).t();
+        cv::Mat twc = -Rwc*Tcw.rowRange(0,3).col(3);
+
+        f << setprecision(9) << Rwc.at<float>(0,0) << " " << Rwc.at<float>(0,1)  << " " << Rwc.at<float>(0,2) << " "  << twc.at<float>(0) << " " <<
+             Rwc.at<float>(1,0) << " " << Rwc.at<float>(1,1)  << " " << Rwc.at<float>(1,2) << " "  << twc.at<float>(1) << " " <<
+             Rwc.at<float>(2,0) << " " << Rwc.at<float>(2,1)  << " " << Rwc.at<float>(2,2) << " "  << twc.at<float>(2) << endl;
+    }
+    f.close();
+}
+
+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;
+}
+
+double System::GetTimeFromIMUInit()
+{
+    double aux = mpLocalMapper->GetCurrKFTime()-mpLocalMapper->mFirstTs;
+    if ((aux>0.) && mpAtlas->isImuInitialized())
+        return mpLocalMapper->GetCurrKFTime()-mpLocalMapper->mFirstTs;
+    else
+        return 0.f;
+}
+
+bool System::isLost()
+{
+    if (!mpAtlas->isImuInitialized())
+        return false;
+    else
+    {
+        if ((mpTracker->mState==Tracking::LOST))
+            return true;
+        else
+            return false;
+    }
+}
+
+
+bool System::isFinished()
+{
+    return (GetTimeFromIMUInit()>0.1);
+}
+
+void System::ChangeDataset()
+{
+    if(mpAtlas->GetCurrentMap()->KeyFramesInMap() < 12)
+    {
+        mpTracker->ResetActiveMap();
+    }
+    else
+    {
+        mpTracker->CreateMapInAtlas();
+    }
+
+    mpTracker->NewDataset();
+}
+
+#ifdef REGISTER_TIMES
+void System::InsertRectTime(double& time)
+{
+    mpTracker->vdRectStereo_ms.push_back(time);
+}
+
+void System::InsertTrackTime(double& time)
+{
+    mpTracker->vdTrackTotal_ms.push_back(time);
+}
+#endif
+
+
+} //namespace ORB_SLAM
+
+

src/TwoViewReconstruction.cc

@@ -0,0 +1,935 @@
+/**
+* This file is part of ORB-SLAM3
+*
+* Copyright (C) 2017-2020 Carlos Campos, Richard Elvira, Juan J. Gómez Rodríguez, José M.M. Montiel and Juan D. Tardós, University of Zaragoza.
+* Copyright (C) 2014-2016 Raúl Mur-Artal, José M.M. Montiel and Juan D. Tardós, University of Zaragoza.
+*
+* ORB-SLAM3 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-SLAM3 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-SLAM3.
+* If not, see <http://www.gnu.org/licenses/>.
+*/
+
+#include "TwoViewReconstruction.h"
+
+#include "Thirdparty/DBoW2/DUtils/Random.h"
+
+#include<thread>
+
+
+using namespace std;
+namespace ORB_SLAM3
+{
+
+TwoViewReconstruction::TwoViewReconstruction(cv::Mat& K, float sigma, int iterations)
+{
+    mK = K.clone();
+
+    mSigma = sigma;
+    mSigma2 = sigma*sigma;
+    mMaxIterations = iterations;
+}
+
+bool TwoViewReconstruction::Reconstruct(const std::vector<cv::KeyPoint>& vKeys1, const std::vector<cv::KeyPoint>& vKeys2, const vector<int> &vMatches12,
+                                        cv::Mat &R21, cv::Mat &t21, vector<cv::Point3f> &vP3D, vector<bool> &vbTriangulated)
+{
+    mvKeys1.clear();
+    mvKeys2.clear();
+
+    mvKeys1 = vKeys1;
+    mvKeys2 = vKeys2;
+
+    // Fill structures with current keypoints and matches with reference frame
+    // Reference Frame: 1, Current Frame: 2
+    mvMatches12.clear();
+    mvMatches12.reserve(mvKeys2.size());
+    mvbMatched1.resize(mvKeys1.size());
+    for(size_t i=0, iend=vMatches12.size();i<iend; i++)
+    {
+        if(vMatches12[i]>=0)
+        {
+            mvMatches12.push_back(make_pair(i,vMatches12[i]));
+            mvbMatched1[i]=true;
+        }
+        else
+            mvbMatched1[i]=false;
+    }
+
+    const int N = mvMatches12.size();
+
+    // 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);
+    }
+
+    // Generate sets of 8 points for each RANSAC iteration
+    mvSets = vector< vector<size_t> >(mMaxIterations,vector<size_t>(8,0));
+
+    DUtils::Random::SeedRandOnce(0);
+
+    for(int it=0; it<mMaxIterations; it++)
+    {
+        vAvailableIndices = vAllIndices;
+
+        // Select a minimum set
+        for(size_t j=0; j<8; j++)
+        {
+            int randi = DUtils::Random::RandomInt(0,vAvailableIndices.size()-1);
+            int idx = vAvailableIndices[randi];
+
+            mvSets[it][j] = idx;
+
+            vAvailableIndices[randi] = vAvailableIndices.back();
+            vAvailableIndices.pop_back();
+        }
+    }
+
+    // Launch threads to compute in parallel a fundamental matrix and a homography
+    vector<bool> vbMatchesInliersH, vbMatchesInliersF;
+    float SH, SF;
+    cv::Mat H, F;
+
+    thread threadH(&TwoViewReconstruction::FindHomography,this,ref(vbMatchesInliersH), ref(SH), ref(H));
+    thread threadF(&TwoViewReconstruction::FindFundamental,this,ref(vbMatchesInliersF), ref(SF), ref(F));
+
+    // Wait until both threads have finished
+    threadH.join();
+    threadF.join();
+
+    // Compute ratio of scores
+    if(SH+SF == 0.f) return false;
+    float RH = SH/(SH+SF);
+
+    float minParallax = 1.0;
+
+    // Try to reconstruct from homography or fundamental depending on the ratio (0.40-0.45)
+    if(RH>0.50) // if(RH>0.40)
+    {
+        //cout << "Initialization from Homography" << endl;
+        return ReconstructH(vbMatchesInliersH,H, mK,R21,t21,vP3D,vbTriangulated,minParallax,50);
+    }
+    else //if(pF_HF>0.6)
+    {
+        //cout << "Initialization from Fundamental" << endl;
+        return ReconstructF(vbMatchesInliersF,F,mK,R21,t21,vP3D,vbTriangulated,minParallax,50);
+    }
+}
+
+void TwoViewReconstruction::FindHomography(vector<bool> &vbMatchesInliers, float &score, cv::Mat &H21)
+{
+    // Number of putative matches
+    const int N = mvMatches12.size();
+
+    // Normalize coordinates
+    vector<cv::Point2f> vPn1, vPn2;
+    cv::Mat T1, T2;
+    Normalize(mvKeys1,vPn1, T1);
+    Normalize(mvKeys2,vPn2, T2);
+    cv::Mat T2inv = T2.inv();
+
+    // Best Results variables
+    score = 0.0;
+    vbMatchesInliers = vector<bool>(N,false);
+
+    // Iteration variables
+    vector<cv::Point2f> vPn1i(8);
+    vector<cv::Point2f> vPn2i(8);
+    cv::Mat H21i, H12i;
+    vector<bool> vbCurrentInliers(N,false);
+    float currentScore;
+
+    // Perform all RANSAC iterations and save the solution with highest score
+    for(int it=0; it<mMaxIterations; it++)
+    {
+        // Select a minimum set
+        for(size_t j=0; j<8; j++)
+        {
+            int idx = mvSets[it][j];
+
+            vPn1i[j] = vPn1[mvMatches12[idx].first];
+            vPn2i[j] = vPn2[mvMatches12[idx].second];
+        }
+
+        cv::Mat Hn = ComputeH21(vPn1i,vPn2i);
+        H21i = T2inv*Hn*T1;
+        H12i = H21i.inv();
+
+        currentScore = CheckHomography(H21i, H12i, vbCurrentInliers, mSigma);
+
+        if(currentScore>score)
+        {
+            H21 = H21i.clone();
+            vbMatchesInliers = vbCurrentInliers;
+            score = currentScore;
+        }
+    }
+}
+
+
+void TwoViewReconstruction::FindFundamental(vector<bool> &vbMatchesInliers, float &score, cv::Mat &F21)
+{
+    // Number of putative matches
+    const int N = vbMatchesInliers.size();
+
+    // Normalize coordinates
+    vector<cv::Point2f> vPn1, vPn2;
+    cv::Mat T1, T2;
+    Normalize(mvKeys1,vPn1, T1);
+    Normalize(mvKeys2,vPn2, T2);
+    cv::Mat T2t = T2.t();
+
+    // Best Results variables
+    score = 0.0;
+    vbMatchesInliers = vector<bool>(N,false);
+
+    // Iteration variables
+    vector<cv::Point2f> vPn1i(8);
+    vector<cv::Point2f> vPn2i(8);
+    cv::Mat F21i;
+    vector<bool> vbCurrentInliers(N,false);
+    float currentScore;
+
+    // Perform all RANSAC iterations and save the solution with highest score
+    for(int it=0; it<mMaxIterations; it++)
+    {
+        // Select a minimum set
+        for(int j=0; j<8; j++)
+        {
+            int idx = mvSets[it][j];
+
+            vPn1i[j] = vPn1[mvMatches12[idx].first];
+            vPn2i[j] = vPn2[mvMatches12[idx].second];
+        }
+
+        cv::Mat Fn = ComputeF21(vPn1i,vPn2i);
+
+        F21i = T2t*Fn*T1;
+
+        currentScore = CheckFundamental(F21i, vbCurrentInliers, mSigma);
+
+        if(currentScore>score)
+        {
+            F21 = F21i.clone();
+            vbMatchesInliers = vbCurrentInliers;
+            score = currentScore;
+        }
+    }
+}
+
+
+cv::Mat TwoViewReconstruction::ComputeH21(const vector<cv::Point2f> &vP1, const vector<cv::Point2f> &vP2)
+{
+    const int N = vP1.size();
+
+    cv::Mat A(2*N,9,CV_32F);
+
+    for(int i=0; i<N; i++)
+    {
+        const float u1 = vP1[i].x;
+        const float v1 = vP1[i].y;
+        const float u2 = vP2[i].x;
+        const float v2 = vP2[i].y;
+
+        A.at<float>(2*i,0) = 0.0;
+        A.at<float>(2*i,1) = 0.0;
+        A.at<float>(2*i,2) = 0.0;
+        A.at<float>(2*i,3) = -u1;
+        A.at<float>(2*i,4) = -v1;
+        A.at<float>(2*i,5) = -1;
+        A.at<float>(2*i,6) = v2*u1;
+        A.at<float>(2*i,7) = v2*v1;
+        A.at<float>(2*i,8) = v2;
+
+        A.at<float>(2*i+1,0) = u1;
+        A.at<float>(2*i+1,1) = v1;
+        A.at<float>(2*i+1,2) = 1;
+        A.at<float>(2*i+1,3) = 0.0;
+        A.at<float>(2*i+1,4) = 0.0;
+        A.at<float>(2*i+1,5) = 0.0;
+        A.at<float>(2*i+1,6) = -u2*u1;
+        A.at<float>(2*i+1,7) = -u2*v1;
+        A.at<float>(2*i+1,8) = -u2;
+
+    }
+
+    cv::Mat u,w,vt;
+
+    cv::SVDecomp(A,w,u,vt,cv::SVD::MODIFY_A | cv::SVD::FULL_UV);
+
+    return vt.row(8).reshape(0, 3);
+}
+
+cv::Mat TwoViewReconstruction::ComputeF21(const vector<cv::Point2f> &vP1,const vector<cv::Point2f> &vP2)
+{
+    const int N = vP1.size();
+
+    cv::Mat A(N,9,CV_32F);
+
+    for(int i=0; i<N; i++)
+    {
+        const float u1 = vP1[i].x;
+        const float v1 = vP1[i].y;
+        const float u2 = vP2[i].x;
+        const float v2 = vP2[i].y;
+
+        A.at<float>(i,0) = u2*u1;
+        A.at<float>(i,1) = u2*v1;
+        A.at<float>(i,2) = u2;
+        A.at<float>(i,3) = v2*u1;
+        A.at<float>(i,4) = v2*v1;
+        A.at<float>(i,5) = v2;
+        A.at<float>(i,6) = u1;
+        A.at<float>(i,7) = v1;
+        A.at<float>(i,8) = 1;
+    }
+
+    cv::Mat u,w,vt;
+
+    cv::SVDecomp(A,w,u,vt,cv::SVD::MODIFY_A | cv::SVD::FULL_UV);
+
+    cv::Mat Fpre = vt.row(8).reshape(0, 3);
+
+    cv::SVDecomp(Fpre,w,u,vt,cv::SVD::MODIFY_A | cv::SVD::FULL_UV);
+
+    w.at<float>(2)=0;
+
+    return  u*cv::Mat::diag(w)*vt;
+}
+
+float TwoViewReconstruction::CheckHomography(const cv::Mat &H21, const cv::Mat &H12, vector<bool> &vbMatchesInliers, float sigma)
+{   
+    const int N = mvMatches12.size();
+
+    const float h11 = H21.at<float>(0,0);
+    const float h12 = H21.at<float>(0,1);
+    const float h13 = H21.at<float>(0,2);
+    const float h21 = H21.at<float>(1,0);
+    const float h22 = H21.at<float>(1,1);
+    const float h23 = H21.at<float>(1,2);
+    const float h31 = H21.at<float>(2,0);
+    const float h32 = H21.at<float>(2,1);
+    const float h33 = H21.at<float>(2,2);
+
+    const float h11inv = H12.at<float>(0,0);
+    const float h12inv = H12.at<float>(0,1);
+    const float h13inv = H12.at<float>(0,2);
+    const float h21inv = H12.at<float>(1,0);
+    const float h22inv = H12.at<float>(1,1);
+    const float h23inv = H12.at<float>(1,2);
+    const float h31inv = H12.at<float>(2,0);
+    const float h32inv = H12.at<float>(2,1);
+    const float h33inv = H12.at<float>(2,2);
+
+    vbMatchesInliers.resize(N);
+
+    float score = 0;
+
+    const float th = 5.991;
+
+    const float invSigmaSquare = 1.0/(sigma*sigma);
+
+    for(int i=0; i<N; i++)
+    {
+        bool bIn = true;
+
+        const cv::KeyPoint &kp1 = mvKeys1[mvMatches12[i].first];
+        const cv::KeyPoint &kp2 = mvKeys2[mvMatches12[i].second];
+
+        const float u1 = kp1.pt.x;
+        const float v1 = kp1.pt.y;
+        const float u2 = kp2.pt.x;
+        const float v2 = kp2.pt.y;
+
+        // Reprojection error in first image
+        // x2in1 = H12*x2
+
+        const float w2in1inv = 1.0/(h31inv*u2+h32inv*v2+h33inv);
+        const float u2in1 = (h11inv*u2+h12inv*v2+h13inv)*w2in1inv;
+        const float v2in1 = (h21inv*u2+h22inv*v2+h23inv)*w2in1inv;
+
+        const float squareDist1 = (u1-u2in1)*(u1-u2in1)+(v1-v2in1)*(v1-v2in1);
+
+        const float chiSquare1 = squareDist1*invSigmaSquare;
+
+        if(chiSquare1>th)
+            bIn = false;
+        else
+            score += th - chiSquare1;
+
+        // Reprojection error in second image
+        // x1in2 = H21*x1
+
+        const float w1in2inv = 1.0/(h31*u1+h32*v1+h33);
+        const float u1in2 = (h11*u1+h12*v1+h13)*w1in2inv;
+        const float v1in2 = (h21*u1+h22*v1+h23)*w1in2inv;
+
+        const float squareDist2 = (u2-u1in2)*(u2-u1in2)+(v2-v1in2)*(v2-v1in2);
+
+        const float chiSquare2 = squareDist2*invSigmaSquare;
+
+        if(chiSquare2>th)
+            bIn = false;
+        else
+            score += th - chiSquare2;
+
+        if(bIn)
+            vbMatchesInliers[i]=true;
+        else
+            vbMatchesInliers[i]=false;
+    }
+
+    return score;
+}
+
+float TwoViewReconstruction::CheckFundamental(const cv::Mat &F21, vector<bool> &vbMatchesInliers, float sigma)
+{
+    const int N = mvMatches12.size();
+
+    const float f11 = F21.at<float>(0,0);
+    const float f12 = F21.at<float>(0,1);
+    const float f13 = F21.at<float>(0,2);
+    const float f21 = F21.at<float>(1,0);
+    const float f22 = F21.at<float>(1,1);
+    const float f23 = F21.at<float>(1,2);
+    const float f31 = F21.at<float>(2,0);
+    const float f32 = F21.at<float>(2,1);
+    const float f33 = F21.at<float>(2,2);
+
+    vbMatchesInliers.resize(N);
+
+    float score = 0;
+
+    const float th = 3.841;
+    const float thScore = 5.991;
+
+    const float invSigmaSquare = 1.0/(sigma*sigma);
+
+    for(int i=0; i<N; i++)
+    {
+        bool bIn = true;
+
+        const cv::KeyPoint &kp1 = mvKeys1[mvMatches12[i].first];
+        const cv::KeyPoint &kp2 = mvKeys2[mvMatches12[i].second];
+
+        const float u1 = kp1.pt.x;
+        const float v1 = kp1.pt.y;
+        const float u2 = kp2.pt.x;
+        const float v2 = kp2.pt.y;
+
+        // Reprojection error in second image
+        // l2=F21x1=(a2,b2,c2)
+
+        const float a2 = f11*u1+f12*v1+f13;
+        const float b2 = f21*u1+f22*v1+f23;
+        const float c2 = f31*u1+f32*v1+f33;
+
+        const float num2 = a2*u2+b2*v2+c2;
+
+        const float squareDist1 = num2*num2/(a2*a2+b2*b2);
+
+        const float chiSquare1 = squareDist1*invSigmaSquare;
+
+        if(chiSquare1>th)
+            bIn = false;
+        else
+            score += thScore - chiSquare1;
+
+        // Reprojection error in second image
+        // l1 =x2tF21=(a1,b1,c1)
+
+        const float a1 = f11*u2+f21*v2+f31;
+        const float b1 = f12*u2+f22*v2+f32;
+        const float c1 = f13*u2+f23*v2+f33;
+
+        const float num1 = a1*u1+b1*v1+c1;
+
+        const float squareDist2 = num1*num1/(a1*a1+b1*b1);
+
+        const float chiSquare2 = squareDist2*invSigmaSquare;
+
+        if(chiSquare2>th)
+            bIn = false;
+        else
+            score += thScore - chiSquare2;
+
+        if(bIn)
+            vbMatchesInliers[i]=true;
+        else
+            vbMatchesInliers[i]=false;
+    }
+
+    return score;
+}
+
+bool TwoViewReconstruction::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)
+{
+    int N=0;
+    for(size_t i=0, iend = vbMatchesInliers.size() ; i<iend; i++)
+        if(vbMatchesInliers[i])
+            N++;
+
+    // Compute Essential Matrix from Fundamental Matrix
+    cv::Mat E21 = K.t()*F21*K;
+
+    cv::Mat R1, R2, t;
+
+    // Recover the 4 motion hypotheses
+    DecomposeE(E21,R1,R2,t);  
+
+    cv::Mat t1=t;
+    cv::Mat t2=-t;
+
+    // Reconstruct with the 4 hyphoteses and check
+    vector<cv::Point3f> vP3D1, vP3D2, vP3D3, vP3D4;
+    vector<bool> vbTriangulated1,vbTriangulated2,vbTriangulated3, vbTriangulated4;
+    float parallax1,parallax2, parallax3, parallax4;
+
+    int nGood1 = CheckRT(R1,t1,mvKeys1,mvKeys2,mvMatches12,vbMatchesInliers,K, vP3D1, 4.0*mSigma2, vbTriangulated1, parallax1);
+    int nGood2 = CheckRT(R2,t1,mvKeys1,mvKeys2,mvMatches12,vbMatchesInliers,K, vP3D2, 4.0*mSigma2, vbTriangulated2, parallax2);
+    int nGood3 = CheckRT(R1,t2,mvKeys1,mvKeys2,mvMatches12,vbMatchesInliers,K, vP3D3, 4.0*mSigma2, vbTriangulated3, parallax3);
+    int nGood4 = CheckRT(R2,t2,mvKeys1,mvKeys2,mvMatches12,vbMatchesInliers,K, vP3D4, 4.0*mSigma2, vbTriangulated4, parallax4);
+
+    int maxGood = max(nGood1,max(nGood2,max(nGood3,nGood4)));
+
+    R21 = cv::Mat();
+    t21 = cv::Mat();
+
+    int nMinGood = max(static_cast<int>(0.9*N),minTriangulated);
+
+    int nsimilar = 0;
+    if(nGood1>0.7*maxGood)
+        nsimilar++;
+    if(nGood2>0.7*maxGood)
+        nsimilar++;
+    if(nGood3>0.7*maxGood)
+        nsimilar++;
+    if(nGood4>0.7*maxGood)
+        nsimilar++;
+
+    // If there is not a clear winner or not enough triangulated points reject initialization
+    if(maxGood<nMinGood || nsimilar>1)
+    {
+        return false;
+    }
+
+    // If best reconstruction has enough parallax initialize
+    if(maxGood==nGood1)
+    {
+        if(parallax1>minParallax)
+        {
+            vP3D = vP3D1;
+            vbTriangulated = vbTriangulated1;
+
+            R1.copyTo(R21);
+            t1.copyTo(t21);
+            return true;
+        }
+    }else if(maxGood==nGood2)
+    {
+        if(parallax2>minParallax)
+        {
+            vP3D = vP3D2;
+            vbTriangulated = vbTriangulated2;
+
+            R2.copyTo(R21);
+            t1.copyTo(t21);
+            return true;
+        }
+    }else if(maxGood==nGood3)
+    {
+        if(parallax3>minParallax)
+        {
+            vP3D = vP3D3;
+            vbTriangulated = vbTriangulated3;
+
+            R1.copyTo(R21);
+            t2.copyTo(t21);
+            return true;
+        }
+    }else if(maxGood==nGood4)
+    {
+        if(parallax4>minParallax)
+        {
+            vP3D = vP3D4;
+            vbTriangulated = vbTriangulated4;
+
+            R2.copyTo(R21);
+            t2.copyTo(t21);
+            return true;
+        }
+    }
+
+    return false;
+}
+
+bool TwoViewReconstruction::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)
+{
+    int N=0;
+    for(size_t i=0, iend = vbMatchesInliers.size() ; i<iend; i++)
+        if(vbMatchesInliers[i])
+            N++;
+
+    // We recover 8 motion hypotheses using the method of Faugeras et al.
+    // Motion and structure from motion in a piecewise planar environment.
+    // International Journal of Pattern Recognition and Artificial Intelligence, 1988
+    cv::Mat invK = K.inv();
+    cv::Mat A = invK*H21*K;
+
+    cv::Mat U,w,Vt,V;
+    cv::SVD::compute(A,w,U,Vt,cv::SVD::FULL_UV);
+    V=Vt.t();
+
+    float s = cv::determinant(U)*cv::determinant(Vt);
+
+    float d1 = w.at<float>(0);
+    float d2 = w.at<float>(1);
+    float d3 = w.at<float>(2);
+
+    if(d1/d2<1.00001 || d2/d3<1.00001)
+    {
+        return false;
+    }
+
+    vector<cv::Mat> vR, vt, vn;
+    vR.reserve(8);
+    vt.reserve(8);
+    vn.reserve(8);
+
+    //n'=[x1 0 x3] 4 posibilities e1=e3=1, e1=1 e3=-1, e1=-1 e3=1, e1=e3=-1
+    float aux1 = sqrt((d1*d1-d2*d2)/(d1*d1-d3*d3));
+    float aux3 = sqrt((d2*d2-d3*d3)/(d1*d1-d3*d3));
+    float x1[] = {aux1,aux1,-aux1,-aux1};
+    float x3[] = {aux3,-aux3,aux3,-aux3};
+
+    //case d'=d2
+    float aux_stheta = sqrt((d1*d1-d2*d2)*(d2*d2-d3*d3))/((d1+d3)*d2);
+
+    float ctheta = (d2*d2+d1*d3)/((d1+d3)*d2);
+    float stheta[] = {aux_stheta, -aux_stheta, -aux_stheta, aux_stheta};
+
+    for(int i=0; i<4; i++)
+    {
+        cv::Mat Rp=cv::Mat::eye(3,3,CV_32F);
+        Rp.at<float>(0,0)=ctheta;
+        Rp.at<float>(0,2)=-stheta[i];
+        Rp.at<float>(2,0)=stheta[i];
+        Rp.at<float>(2,2)=ctheta;
+
+        cv::Mat R = s*U*Rp*Vt;
+        vR.push_back(R);
+
+        cv::Mat tp(3,1,CV_32F);
+        tp.at<float>(0)=x1[i];
+        tp.at<float>(1)=0;
+        tp.at<float>(2)=-x3[i];
+        tp*=d1-d3;
+
+        cv::Mat t = U*tp;
+        vt.push_back(t/cv::norm(t));
+
+        cv::Mat np(3,1,CV_32F);
+        np.at<float>(0)=x1[i];
+        np.at<float>(1)=0;
+        np.at<float>(2)=x3[i];
+
+        cv::Mat n = V*np;
+        if(n.at<float>(2)<0)
+            n=-n;
+        vn.push_back(n);
+    }
+
+    //case d'=-d2
+    float aux_sphi = sqrt((d1*d1-d2*d2)*(d2*d2-d3*d3))/((d1-d3)*d2);
+
+    float cphi = (d1*d3-d2*d2)/((d1-d3)*d2);
+    float sphi[] = {aux_sphi, -aux_sphi, -aux_sphi, aux_sphi};
+
+    for(int i=0; i<4; i++)
+    {
+        cv::Mat Rp=cv::Mat::eye(3,3,CV_32F);
+        Rp.at<float>(0,0)=cphi;
+        Rp.at<float>(0,2)=sphi[i];
+        Rp.at<float>(1,1)=-1;
+        Rp.at<float>(2,0)=sphi[i];
+        Rp.at<float>(2,2)=-cphi;
+
+        cv::Mat R = s*U*Rp*Vt;
+        vR.push_back(R);
+
+        cv::Mat tp(3,1,CV_32F);
+        tp.at<float>(0)=x1[i];
+        tp.at<float>(1)=0;
+        tp.at<float>(2)=x3[i];
+        tp*=d1+d3;
+
+        cv::Mat t = U*tp;
+        vt.push_back(t/cv::norm(t));
+
+        cv::Mat np(3,1,CV_32F);
+        np.at<float>(0)=x1[i];
+        np.at<float>(1)=0;
+        np.at<float>(2)=x3[i];
+
+        cv::Mat n = V*np;
+        if(n.at<float>(2)<0)
+            n=-n;
+        vn.push_back(n);
+    }
+
+
+    int bestGood = 0;
+    int secondBestGood = 0;    
+    int bestSolutionIdx = -1;
+    float bestParallax = -1;
+    vector<cv::Point3f> bestP3D;
+    vector<bool> bestTriangulated;
+
+    // Instead of applying the visibility constraints proposed in the Faugeras' paper (which could fail for points seen with low parallax)
+    // We reconstruct all hypotheses and check in terms of triangulated points and parallax
+    for(size_t i=0; i<8; i++)
+    {
+        float parallaxi;
+        vector<cv::Point3f> vP3Di;
+        vector<bool> vbTriangulatedi;
+        int nGood = CheckRT(vR[i],vt[i],mvKeys1,mvKeys2,mvMatches12,vbMatchesInliers,K,vP3Di, 4.0*mSigma2, vbTriangulatedi, parallaxi);
+
+        if(nGood>bestGood)
+        {
+            secondBestGood = bestGood;
+            bestGood = nGood;
+            bestSolutionIdx = i;
+            bestParallax = parallaxi;
+            bestP3D = vP3Di;
+            bestTriangulated = vbTriangulatedi;
+        }
+        else if(nGood>secondBestGood)
+        {
+            secondBestGood = nGood;
+        }
+    }
+
+
+    if(secondBestGood<0.75*bestGood && bestParallax>=minParallax && bestGood>minTriangulated && bestGood>0.9*N)
+    {
+        vR[bestSolutionIdx].copyTo(R21);
+        vt[bestSolutionIdx].copyTo(t21);
+        vP3D = bestP3D;
+        vbTriangulated = bestTriangulated;
+
+        return true;
+    }
+
+    return false;
+}
+
+void TwoViewReconstruction::Triangulate(const cv::KeyPoint &kp1, const cv::KeyPoint &kp2, const cv::Mat &P1, const cv::Mat &P2, cv::Mat &x3D)
+{
+    cv::Mat A(4,4,CV_32F);
+
+    A.row(0) = kp1.pt.x*P1.row(2)-P1.row(0);
+    A.row(1) = kp1.pt.y*P1.row(2)-P1.row(1);
+    A.row(2) = kp2.pt.x*P2.row(2)-P2.row(0);
+    A.row(3) = kp2.pt.y*P2.row(2)-P2.row(1);
+
+    cv::Mat u,w,vt;
+    cv::SVD::compute(A,w,u,vt,cv::SVD::MODIFY_A| cv::SVD::FULL_UV);
+    x3D = vt.row(3).t();
+    x3D = x3D.rowRange(0,3)/x3D.at<float>(3);
+}
+
+void TwoViewReconstruction::Normalize(const vector<cv::KeyPoint> &vKeys, vector<cv::Point2f> &vNormalizedPoints, cv::Mat &T)
+{
+    float meanX = 0;
+    float meanY = 0;
+    const int N = vKeys.size();
+
+    vNormalizedPoints.resize(N);
+
+    for(int i=0; i<N; i++)
+    {
+        meanX += vKeys[i].pt.x;
+        meanY += vKeys[i].pt.y;
+    }
+
+    meanX = meanX/N;
+    meanY = meanY/N;
+
+    float meanDevX = 0;
+    float meanDevY = 0;
+
+    for(int i=0; i<N; i++)
+    {
+        vNormalizedPoints[i].x = vKeys[i].pt.x - meanX;
+        vNormalizedPoints[i].y = vKeys[i].pt.y - meanY;
+
+        meanDevX += fabs(vNormalizedPoints[i].x);
+        meanDevY += fabs(vNormalizedPoints[i].y);
+    }
+
+    meanDevX = meanDevX/N;
+    meanDevY = meanDevY/N;
+
+    float sX = 1.0/meanDevX;
+    float sY = 1.0/meanDevY;
+
+    for(int i=0; i<N; i++)
+    {
+        vNormalizedPoints[i].x = vNormalizedPoints[i].x * sX;
+        vNormalizedPoints[i].y = vNormalizedPoints[i].y * sY;
+    }
+
+    T = cv::Mat::eye(3,3,CV_32F);
+    T.at<float>(0,0) = sX;
+    T.at<float>(1,1) = sY;
+    T.at<float>(0,2) = -meanX*sX;
+    T.at<float>(1,2) = -meanY*sY;
+}
+
+
+int TwoViewReconstruction::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> &vbMatchesInliers,
+                       const cv::Mat &K, vector<cv::Point3f> &vP3D, float th2, vector<bool> &vbGood, float &parallax)
+{
+    // Calibration parameters
+    const float fx = K.at<float>(0,0);
+    const float fy = K.at<float>(1,1);
+    const float cx = K.at<float>(0,2);
+    const float cy = K.at<float>(1,2);
+
+    vbGood = vector<bool>(vKeys1.size(),false);
+    vP3D.resize(vKeys1.size());
+
+    vector<float> vCosParallax;
+    vCosParallax.reserve(vKeys1.size());
+
+    // Camera 1 Projection Matrix K[I|0]
+    cv::Mat P1(3,4,CV_32F,cv::Scalar(0));
+    K.copyTo(P1.rowRange(0,3).colRange(0,3));
+
+    cv::Mat O1 = cv::Mat::zeros(3,1,CV_32F);
+
+    // Camera 2 Projection Matrix K[R|t]
+    cv::Mat P2(3,4,CV_32F);
+    R.copyTo(P2.rowRange(0,3).colRange(0,3));
+    t.copyTo(P2.rowRange(0,3).col(3));
+    P2 = K*P2;
+
+    cv::Mat O2 = -R.t()*t;
+
+    int nGood=0;
+
+    for(size_t i=0, iend=vMatches12.size();i<iend;i++)
+    {
+        if(!vbMatchesInliers[i])
+            continue;
+
+        const cv::KeyPoint &kp1 = vKeys1[vMatches12[i].first];
+        const cv::KeyPoint &kp2 = vKeys2[vMatches12[i].second];
+        cv::Mat p3dC1;
+
+        Triangulate(kp1,kp2,P1,P2,p3dC1);
+
+        if(!isfinite(p3dC1.at<float>(0)) || !isfinite(p3dC1.at<float>(1)) || !isfinite(p3dC1.at<float>(2)))
+        {
+            vbGood[vMatches12[i].first]=false;
+            continue;
+        }
+
+        // Check parallax
+        cv::Mat normal1 = p3dC1 - O1;
+        float dist1 = cv::norm(normal1);
+
+        cv::Mat normal2 = p3dC1 - O2;
+        float dist2 = cv::norm(normal2);
+
+        float cosParallax = normal1.dot(normal2)/(dist1*dist2);
+
+        // Check depth in front of first camera (only if enough parallax, as "infinite" points can easily go to negative depth)
+        if(p3dC1.at<float>(2)<=0 && cosParallax<0.99998)
+            continue;
+
+        // Check depth in front of second camera (only if enough parallax, as "infinite" points can easily go to negative depth)
+        cv::Mat p3dC2 = R*p3dC1+t;
+
+        if(p3dC2.at<float>(2)<=0 && cosParallax<0.99998)
+            continue;
+
+        // Check reprojection error in first image
+        float im1x, im1y;
+        float invZ1 = 1.0/p3dC1.at<float>(2);
+        im1x = fx*p3dC1.at<float>(0)*invZ1+cx;
+        im1y = fy*p3dC1.at<float>(1)*invZ1+cy;
+
+        float squareError1 = (im1x-kp1.pt.x)*(im1x-kp1.pt.x)+(im1y-kp1.pt.y)*(im1y-kp1.pt.y);
+
+        if(squareError1>th2)
+            continue;
+
+        // Check reprojection error in second image
+        float im2x, im2y;
+        float invZ2 = 1.0/p3dC2.at<float>(2);
+        im2x = fx*p3dC2.at<float>(0)*invZ2+cx;
+        im2y = fy*p3dC2.at<float>(1)*invZ2+cy;
+
+        float squareError2 = (im2x-kp2.pt.x)*(im2x-kp2.pt.x)+(im2y-kp2.pt.y)*(im2y-kp2.pt.y);
+
+        if(squareError2>th2)
+            continue;
+
+        vCosParallax.push_back(cosParallax);
+        vP3D[vMatches12[i].first] = cv::Point3f(p3dC1.at<float>(0),p3dC1.at<float>(1),p3dC1.at<float>(2));
+        nGood++;
+
+        if(cosParallax<0.99998)
+            vbGood[vMatches12[i].first]=true;
+    }
+
+    if(nGood>0)
+    {
+        sort(vCosParallax.begin(),vCosParallax.end());
+
+        size_t idx = min(50,int(vCosParallax.size()-1));
+        parallax = acos(vCosParallax[idx])*180/CV_PI;
+    }
+    else
+        parallax=0;
+
+    return nGood;
+}
+
+void TwoViewReconstruction::DecomposeE(const cv::Mat &E, cv::Mat &R1, cv::Mat &R2, cv::Mat &t)
+{
+    cv::Mat u,w,vt;
+    cv::SVD::compute(E,w,u,vt);
+
+    u.col(2).copyTo(t);
+    t=t/cv::norm(t);
+
+    cv::Mat W(3,3,CV_32F,cv::Scalar(0));
+    W.at<float>(0,1)=-1;
+    W.at<float>(1,0)=1;
+    W.at<float>(2,2)=1;
+
+    R1 = u*W*vt;
+    if(cv::determinant(R1)<0)
+        R1=-R1;
+
+    R2 = u*W.t()*vt;
+    if(cv::determinant(R2)<0)
+        R2=-R2;
+}
+
+} //namespace ORB_SLAM

src/Viewer.cc

@@ -0,0 +1,374 @@
+/**
+* This file is part of ORB-SLAM3
+*
+* Copyright (C) 2017-2020 Carlos Campos, Richard Elvira, Juan J. Gómez Rodríguez, José M.M. Montiel and Juan D. Tardós, University of Zaragoza.
+* Copyright (C) 2014-2016 Raúl Mur-Artal, José M.M. Montiel and Juan D. Tardós, University of Zaragoza.
+*
+* ORB-SLAM3 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-SLAM3 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-SLAM3.
+* If not, see <http://www.gnu.org/licenses/>.
+*/
+
+
+#include "Viewer.h"
+#include <pangolin/pangolin.h>
+//#include <unistd.h>
+
+#include <mutex>
+
+namespace ORB_SLAM3
+{
+
+Viewer::Viewer(System* pSystem, FrameDrawer *pFrameDrawer, MapDrawer *pMapDrawer, Tracking *pTracking, const string &strSettingPath):
+    both(false), mpSystem(pSystem), mpFrameDrawer(pFrameDrawer),mpMapDrawer(pMapDrawer), mpTracker(pTracking),
+    mbFinishRequested(false), mbFinished(true), mbStopped(true), mbStopRequested(false)
+{
+    cv::FileStorage fSettings(strSettingPath, cv::FileStorage::READ);
+
+    bool is_correct = ParseViewerParamFile(fSettings);
+
+    if(!is_correct)
+    {
+        std::cerr << "**ERROR in the config file, the format is not correct**" << std::endl;
+        try
+        {
+            throw -1;
+        }
+        catch(exception &e)
+        {
+
+        }
+    }
+
+    mbStopTrack = false;
+}
+
+bool Viewer::ParseViewerParamFile(cv::FileStorage &fSettings)
+{
+    bool b_miss_params = false;
+
+    float fps = fSettings["Camera.fps"];
+    if(fps<1)
+        fps=30;
+    mT = 1e3/fps;
+
+    cv::FileNode node = fSettings["Camera.width"];
+    if(!node.empty())
+    {
+        mImageWidth = node.real();
+    }
+    else
+    {
+        std::cerr << "*Camera.width parameter doesn't exist or is not a real number*" << std::endl;
+        b_miss_params = true;
+    }
+
+    node = fSettings["Camera.height"];
+    if(!node.empty())
+    {
+        mImageHeight = node.real();
+    }
+    else
+    {
+        std::cerr << "*Camera.height parameter doesn't exist or is not a real number*" << std::endl;
+        b_miss_params = true;
+    }
+
+    node = fSettings["Viewer.ViewpointX"];
+    if(!node.empty())
+    {
+        mViewpointX = node.real();
+    }
+    else
+    {
+        std::cerr << "*Viewer.ViewpointX parameter doesn't exist or is not a real number*" << std::endl;
+        b_miss_params = true;
+    }
+
+    node = fSettings["Viewer.ViewpointY"];
+    if(!node.empty())
+    {
+        mViewpointY = node.real();
+    }
+    else
+    {
+        std::cerr << "*Viewer.ViewpointY parameter doesn't exist or is not a real number*" << std::endl;
+        b_miss_params = true;
+    }
+
+    node = fSettings["Viewer.ViewpointZ"];
+    if(!node.empty())
+    {
+        mViewpointZ = node.real();
+    }
+    else
+    {
+        std::cerr << "*Viewer.ViewpointZ parameter doesn't exist or is not a real number*" << std::endl;
+        b_miss_params = true;
+    }
+
+    node = fSettings["Viewer.ViewpointF"];
+    if(!node.empty())
+    {
+        mViewpointF = node.real();
+    }
+    else
+    {
+        std::cerr << "*Viewer.ViewpointF parameter doesn't exist or is not a real number*" << std::endl;
+        b_miss_params = true;
+    }
+
+    return !b_miss_params;
+}
+
+void Viewer::Run()
+{
+    mbFinished = false;
+    mbStopped = false;
+
+    pangolin::CreateWindowAndBind("ORB-SLAM3: Map Viewer",1024,768);
+
+    // 3D Mouse handler requires depth testing to be enabled
+    glEnable(GL_DEPTH_TEST);
+
+    // Issue specific OpenGl we might need
+    glEnable (GL_BLEND);
+    glBlendFunc (GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
+
+    pangolin::CreatePanel("menu").SetBounds(0.0,1.0,0.0,pangolin::Attach::Pix(175));
+    pangolin::Var<bool> menuFollowCamera("menu.Follow Camera",true,true);
+    pangolin::Var<bool> menuCamView("menu.Camera View",false,false);
+    pangolin::Var<bool> menuTopView("menu.Top View",false,false);
+    pangolin::Var<bool> menuShowPoints("menu.Show Points",true,true);
+    pangolin::Var<bool> menuShowKeyFrames("menu.Show KeyFrames",true,true);
+    pangolin::Var<bool> menuShowGraph("menu.Show Graph",false,true);
+    pangolin::Var<bool> menuShowInertialGraph("menu.Show Inertial Graph",true,true);
+    pangolin::Var<bool> menuLocalizationMode("menu.Localization Mode",false,true);
+    pangolin::Var<bool> menuReset("menu.Reset",false,false);
+
+    // Define Camera Render Object (for view / scene browsing)
+    pangolin::OpenGlRenderState s_cam(
+                pangolin::ProjectionMatrix(1024,768,mViewpointF,mViewpointF,512,389,0.1,1000),
+                pangolin::ModelViewLookAt(mViewpointX,mViewpointY,mViewpointZ, 0,0,0,0.0,-1.0, 0.0)
+                );
+
+    // Add named OpenGL viewport to window and provide 3D Handler
+    pangolin::View& d_cam = pangolin::CreateDisplay()
+            .SetBounds(0.0, 1.0, pangolin::Attach::Pix(175), 1.0, -1024.0f/768.0f)
+            .SetHandler(new pangolin::Handler3D(s_cam));
+
+    pangolin::OpenGlMatrix Twc, Twr;
+    Twc.SetIdentity();
+    pangolin::OpenGlMatrix Ow; // Oriented with g in the z axis
+    Ow.SetIdentity();
+    pangolin::OpenGlMatrix Twwp; // Oriented with g in the z axis, but y and x from camera
+    Twwp.SetIdentity();
+    cv::namedWindow("ORB-SLAM3: Current Frame");
+
+    bool bFollow = true;
+    bool bLocalizationMode = false;
+    bool bStepByStep = false;
+    bool bCameraView = true;
+
+    if(mpTracker->mSensor == mpSystem->MONOCULAR || mpTracker->mSensor == mpSystem->STEREO || mpTracker->mSensor == mpSystem->RGBD)
+    {
+        menuShowGraph = true;
+    }
+
+    while(1)
+    {
+        glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
+
+        mpMapDrawer->GetCurrentOpenGLCameraMatrix(Twc,Ow,Twwp);
+
+        if(mbStopTrack)
+        {
+            mbStopTrack = false;
+        }
+
+        if(menuFollowCamera && bFollow)
+        {
+            if(bCameraView)
+                s_cam.Follow(Twc);
+            else
+                s_cam.Follow(Ow);
+        }
+        else if(menuFollowCamera && !bFollow)
+        {
+            if(bCameraView)
+            {
+                s_cam.SetProjectionMatrix(pangolin::ProjectionMatrix(1024,768,mViewpointF,mViewpointF,512,389,0.1,1000));
+                s_cam.SetModelViewMatrix(pangolin::ModelViewLookAt(mViewpointX,mViewpointY,mViewpointZ, 0,0,0,0.0,-1.0, 0.0));
+                s_cam.Follow(Twc);
+            }
+            else
+            {
+                s_cam.SetProjectionMatrix(pangolin::ProjectionMatrix(1024,768,3000,3000,512,389,0.1,1000));
+                s_cam.SetModelViewMatrix(pangolin::ModelViewLookAt(0,0.01,10, 0,0,0,0.0,0.0, 1.0));
+                s_cam.Follow(Ow);
+            }
+            bFollow = true;
+        }
+        else if(!menuFollowCamera && bFollow)
+        {
+            bFollow = false;
+        }
+
+        if(menuCamView)
+        {
+            menuCamView = false;
+            bCameraView = true;
+            s_cam.SetProjectionMatrix(pangolin::ProjectionMatrix(1024,768,mViewpointF,mViewpointF,512,389,0.1,10000));
+            s_cam.SetModelViewMatrix(pangolin::ModelViewLookAt(mViewpointX,mViewpointY,mViewpointZ, 0,0,0,0.0,-1.0, 0.0));
+            s_cam.Follow(Twc);
+        }
+
+        if(menuTopView && mpMapDrawer->mpAtlas->isImuInitialized())
+        {
+            menuTopView = false;
+            bCameraView = false;
+            s_cam.SetProjectionMatrix(pangolin::ProjectionMatrix(1024,768,3000,3000,512,389,0.1,10000));
+            s_cam.SetModelViewMatrix(pangolin::ModelViewLookAt(0,0.01,50, 0,0,0,0.0,0.0, 1.0));
+            s_cam.Follow(Ow);
+        }
+
+        if(menuLocalizationMode && !bLocalizationMode)
+        {
+            mpSystem->ActivateLocalizationMode();
+            bLocalizationMode = true;
+        }
+        else if(!menuLocalizationMode && bLocalizationMode)
+        {
+            mpSystem->DeactivateLocalizationMode();
+            bLocalizationMode = false;
+        }
+
+        d_cam.Activate(s_cam);
+        glClearColor(1.0f,1.0f,1.0f,1.0f);
+        mpMapDrawer->DrawCurrentCamera(Twc);
+        if(menuShowKeyFrames || menuShowGraph || menuShowInertialGraph)
+            mpMapDrawer->DrawKeyFrames(menuShowKeyFrames,menuShowGraph, menuShowInertialGraph);
+        if(menuShowPoints)
+            mpMapDrawer->DrawMapPoints();
+
+        pangolin::FinishFrame();
+
+        cv::Mat toShow;
+        cv::Mat im = mpFrameDrawer->DrawFrame(true);
+
+        if(both){
+            cv::Mat imRight = mpFrameDrawer->DrawRightFrame();
+            cv::hconcat(im,imRight,toShow);
+        }
+        else{
+            toShow = im;
+        }
+
+        cv::imshow("ORB-SLAM3: Current Frame",toShow);
+        cv::waitKey(mT);
+
+        if(menuReset)
+        {
+            menuShowGraph = true;
+            menuShowInertialGraph = true;
+            menuShowKeyFrames = true;
+            menuShowPoints = true;
+            menuLocalizationMode = false;
+            if(bLocalizationMode)
+                mpSystem->DeactivateLocalizationMode();
+            bLocalizationMode = false;
+            bFollow = true;
+            menuFollowCamera = true;
+            mpSystem->ResetActiveMap();
+            menuReset = false;
+        }
+
+        if(Stop())
+        {
+            while(isStopped())
+            {
+                usleep(3000);
+            }
+        }
+
+        if(CheckFinish())
+            break;
+    }
+
+    SetFinish();
+}
+
+void Viewer::RequestFinish()
+{
+    unique_lock<mutex> lock(mMutexFinish);
+    mbFinishRequested = true;
+}
+
+bool Viewer::CheckFinish()
+{
+    unique_lock<mutex> lock(mMutexFinish);
+    return mbFinishRequested;
+}
+
+void Viewer::SetFinish()
+{
+    unique_lock<mutex> lock(mMutexFinish);
+    mbFinished = true;
+}
+
+bool Viewer::isFinished()
+{
+    unique_lock<mutex> lock(mMutexFinish);
+    return mbFinished;
+}
+
+void Viewer::RequestStop()
+{
+    unique_lock<mutex> lock(mMutexStop);
+    if(!mbStopped)
+        mbStopRequested = true;
+}
+
+bool Viewer::isStopped()
+{
+    unique_lock<mutex> lock(mMutexStop);
+    return mbStopped;
+}
+
+bool Viewer::Stop()
+{
+    unique_lock<mutex> lock(mMutexStop);
+    unique_lock<mutex> lock2(mMutexFinish);
+
+    if(mbFinishRequested)
+        return false;
+    else if(mbStopRequested)
+    {
+        mbStopped = true;
+        mbStopRequested = false;
+        return true;
+    }
+
+    return false;
+
+}
+
+void Viewer::Release()
+{
+    unique_lock<mutex> lock(mMutexStop);
+    mbStopped = false;
+}
+
+void Viewer::SetTrackingPause()
+{
+    mbStopTrack = true;
+}
+
+}