From 482ec9e6329f98147c451f8fb4d217371f57d956 Mon Sep 17 00:00:00 2001
From: Soldann <guotl321@gmail.com>
Date: Thu, 14 Jul 2022 10:01:10 -0400
Subject: [PATCH 01/26] Remove unused variable

July 15 update

Decouple drawing header files

Reduce viewer coupling

Viewer is now not a dependency of any system component

Move enum out of system

Remove unused member variables and local variables

fix some warnings

Start moving atlas to be Atlas_ptr

Fix stereo_euroc example

Fix all examples, resolves #55

stuff
---
 include/Atlas.h    |  6 ++++--
 include/Frame.h    |  5 +----
 include/Tracking.h |  6 +++---
 src/System.cc      | 13 +++++++------
 src/Tracking.cc    |  6 +++---
 5 files changed, 18 insertions(+), 18 deletions(-)

diff --git a/include/Atlas.h b/include/Atlas.h
index 65685e34..7eb2a1b9 100644
--- a/include/Atlas.h
+++ b/include/Atlas.h
@@ -25,6 +25,8 @@
 #include <boost/serialization/vector.hpp>
 #include <mutex>
 #include <set>
+#include <map>
+#include <vector>
 
 #include "GeometricCamera.h"
 #include "KannalaBrandt8.h"
@@ -101,7 +103,7 @@ class Atlas {
   std::vector<MapPoint*> GetAllMapPoints();
   std::vector<MapPoint*> GetReferenceMapPoints();
 
-  vector<Map*> GetAllMaps();
+  std::vector<Map*> GetAllMaps();
 
   int CountMaps();
 
@@ -123,7 +125,7 @@ class Atlas {
   void PreSave();
   void PostLoad();
 
-  map<long unsigned int, KeyFrame*> GetAtlasKeyframes();
+  std::map<long unsigned int, KeyFrame*> GetAtlasKeyframes();
 
   void SetKeyFrameDababase(KeyFrameDatabase* pKFDB);
   KeyFrameDatabase* GetKeyFrameDatabase();
diff --git a/include/Frame.h b/include/Frame.h
index 4beb90c4..9e8e7605 100644
--- a/include/Frame.h
+++ b/include/Frame.h
@@ -17,8 +17,7 @@
 */
 
 
-#ifndef FRAME_H
-#define FRAME_H
+#pragma once
 
 #include<vector>
 
@@ -370,5 +369,3 @@ class Frame
 };
 
 }// namespace ORB_SLAM
-
-#endif // FRAME_H
diff --git a/include/Tracking.h b/include/Tracking.h
index b427f7b7..964e60bb 100644
--- a/include/Tracking.h
+++ b/include/Tracking.h
@@ -66,11 +66,11 @@ class Tracking {
   // matching.
   Sophus::SE3f GrabImageStereo(const cv::Mat& imRectLeft,
                                const cv::Mat& imRectRight,
-                               const double& timestamp, string filename);
+                               const double& timestamp, const string &filename);
   Sophus::SE3f GrabImageRGBD(const cv::Mat& imRGB, const cv::Mat& imD,
-                             const double& timestamp, string filename);
+                             const double& timestamp, const string &filename);
   Sophus::SE3f GrabImageMonocular(const cv::Mat& im, const double& timestamp,
-                                  string filename);
+                                  const string &filename);
 
   void GrabImuData(const IMU::Point& imuMeasurement);
 
diff --git a/src/System.cc b/src/System.cc
index 3ed92046..a0fb27fa 100644
--- a/src/System.cc
+++ b/src/System.cc
@@ -22,6 +22,7 @@
 #include "System.h"
 
 #include <openssl/md5.h>
+#include <opencv2/opencv.hpp>
 #include <pangolin/pangolin.h>
 
 #include "ImprovedTypes.hpp"
@@ -252,10 +253,10 @@ Sophus::SE3f System::TrackStereo(const cv::Mat& imLeft, const cv::Mat& imRight,
 
   cv::Mat imLeftToFeed, imRightToFeed;
   if (settings_ && settings_->needToRectify()) {
-    cv::Mat M1l = settings_->M1l();
-    cv::Mat M2l = settings_->M2l();
-    cv::Mat M1r = settings_->M1r();
-    cv::Mat M2r = settings_->M2r();
+    const cv::Mat &M1l = settings_->M1l();
+    const cv::Mat &M2l = settings_->M2l();
+    const cv::Mat &M1r = settings_->M1r();
+    const cv::Mat &M2r = settings_->M2r();
 
     cv::remap(imLeft, imLeftToFeed, M1l, M2l, cv::INTER_LINEAR);
     cv::remap(imRight, imRightToFeed, M1r, M2r, cv::INTER_LINEAR);
@@ -263,8 +264,8 @@ Sophus::SE3f System::TrackStereo(const cv::Mat& imLeft, const cv::Mat& imRight,
     cv::resize(imLeft, imLeftToFeed, settings_->newImSize());
     cv::resize(imRight, imRightToFeed, settings_->newImSize());
   } else {
-    imLeftToFeed = imLeft.clone();
-    imRightToFeed = imRight.clone();
+    imLeftToFeed = imLeft;
+    imRightToFeed = imRight;
   }
 
   // Check mode change
diff --git a/src/Tracking.cc b/src/Tracking.cc
index 31b1d8b5..666b94da 100644
--- a/src/Tracking.cc
+++ b/src/Tracking.cc
@@ -1363,7 +1363,7 @@ void Tracking::SetLoopClosing(LoopClosing* pLoopClosing) {
 Sophus::SE3f Tracking::GrabImageStereo(const cv::Mat& imRectLeft,
                                        const cv::Mat& imRectRight,
                                        const double& timestamp,
-                                       string filename) {
+                                       const string &filename) {
   // cout << "GrabImageStereo" << endl;
 
   mImGray = imRectLeft;
@@ -1428,7 +1428,7 @@ Sophus::SE3f Tracking::GrabImageStereo(const cv::Mat& imRectLeft,
 }
 
 Sophus::SE3f Tracking::GrabImageRGBD(const cv::Mat& imRGB, const cv::Mat& imD,
-                                     const double& timestamp, string filename) {
+                                     const double& timestamp, const string &filename) {
   mImGray = imRGB;
   cv::Mat imDepth = imD;
 
@@ -1470,7 +1470,7 @@ Sophus::SE3f Tracking::GrabImageRGBD(const cv::Mat& imRGB, const cv::Mat& imD,
 
 Sophus::SE3f Tracking::GrabImageMonocular(const cv::Mat& im,
                                           const double& timestamp,
-                                          string filename) {
+                                          const string &filename) {
   mImGray = im;
   if (mImGray.channels() == 3) {
     if (mbRGB)

From 112c795d38dc62a413833da818dbf7c0ddccbc3e Mon Sep 17 00:00:00 2001
From: DavidPetkovsek <david.petkovsek@martinrea.com>
Date: Mon, 25 Jul 2022 20:16:03 -0400
Subject: [PATCH 02/26] Do minor cleaning of frame constructor

---
 include/Frame.h        |   2 +-
 include/ORBextractor.h |  13 +--
 src/Frame.cc           |  16 ++--
 src/ORBextractor.cc    | 211 ++++++-----------------------------------
 4 files changed, 41 insertions(+), 201 deletions(-)

diff --git a/include/Frame.h b/include/Frame.h
index 9e8e7605..4f419c09 100644
--- a/include/Frame.h
+++ b/include/Frame.h
@@ -70,7 +70,7 @@ class Frame
     // ~Frame();
 
     // Extract ORB on the image. 0 for left image and 1 for right image.
-    void ExtractORB(int flag, const cv::Mat &im, const int x0, const int x1);
+    void ExtractORB(bool isLeft, const cv::Mat &im, const int x0, const int x1);
 
     // Compute Bag of Words representation.
     void ComputeBoW();
diff --git a/include/ORBextractor.h b/include/ORBextractor.h
index aa08b9b0..bc67cb3a 100644
--- a/include/ORBextractor.h
+++ b/include/ORBextractor.h
@@ -19,8 +19,7 @@
  * ORB-SLAM3. If not, see <http://www.gnu.org/licenses/>.
  */
 
-#ifndef ORBEXTRACTOR_H
-#define ORBEXTRACTOR_H
+#pragma once
 
 #include <list>
 #include <opencv2/opencv.hpp>
@@ -31,6 +30,8 @@ namespace ORB_SLAM3 {
 class ExtractorNode {
  public:
   ExtractorNode() : bNoMore(false) {}
+  ExtractorNode(const cv::Point2i &UL, const cv::Point2i &UR, const cv::Point2i &BL, const cv::Point2i &BR, int keySize) :
+    UL{UL}, UR{UR}, BL{BL}, BR{BR}, bNoMore(false) { vKeys.reserve(keySize); }
 
   void DivideNode(ExtractorNode &n1, ExtractorNode &n2, ExtractorNode &n3,
                   ExtractorNode &n4);
@@ -43,13 +44,9 @@ class ExtractorNode {
 
 class ORBextractor {
  public:
-  enum { HARRIS_SCORE = 0, FAST_SCORE = 1 };
-
   ORBextractor(int nfeatures, float scaleFactor, int nlevels, int iniThFAST,
                int minThFAST);
 
-  ~ORBextractor() {}
-
   // Compute the ORB features and descriptors on an image.
   // ORB are dispersed on the image using an octree.
   // Mask is ignored in the current implementation.
@@ -84,8 +81,6 @@ class ORBextractor {
       const int &maxX, const int &minY, const int &maxY, const int &nFeatures,
       const int &level);
 
-  void ComputeKeyPointsOld(
-      std::vector<std::vector<cv::KeyPoint> > &allKeypoints);
   std::vector<cv::Point> pattern;
 
   int nfeatures;
@@ -105,5 +100,3 @@ class ORBextractor {
 };
 
 }  // namespace ORB_SLAM3
-
-#endif
diff --git a/src/Frame.cc b/src/Frame.cc
index e69fb70b..e5cc874d 100644
--- a/src/Frame.cc
+++ b/src/Frame.cc
@@ -191,8 +191,8 @@ Frame::Frame(const cv::Mat &imLeft, const cv::Mat &imRight,
   std::chrono::steady_clock::time_point time_StartExtORB =
       std::chrono::steady_clock::now();
 #endif
-  thread threadLeft(&Frame::ExtractORB, this, 0, imLeft, 0, 0);
-  thread threadRight(&Frame::ExtractORB, this, 1, imRight, 0, 0);
+  thread threadLeft(&Frame::ExtractORB, this, true, imLeft, 0, 0);
+  thread threadRight(&Frame::ExtractORB, this, false, imRight, 0, 0);
   threadLeft.join();
   threadRight.join();
 #ifdef REGISTER_TIMES
@@ -315,7 +315,7 @@ Frame::Frame(const cv::Mat &imGray, const cv::Mat &imDepth,
   std::chrono::steady_clock::time_point time_StartExtORB =
       std::chrono::steady_clock::now();
 #endif
-  ExtractORB(0, imGray, 0, 0);
+  ExtractORB(true, imGray, 0, 0);
 #ifdef REGISTER_TIMES
   std::chrono::steady_clock::time_point time_EndExtORB =
       std::chrono::steady_clock::now();
@@ -425,7 +425,7 @@ Frame::Frame(const cv::Mat &imGray, const double &timeStamp,
   std::chrono::steady_clock::time_point time_StartExtORB =
       std::chrono::steady_clock::now();
 #endif
-  ExtractORB(0, imGray, 0, 1000);
+  ExtractORB(true, imGray, 0, 1000);
 #ifdef REGISTER_TIMES
   std::chrono::steady_clock::time_point time_EndExtORB =
       std::chrono::steady_clock::now();
@@ -527,10 +527,10 @@ void Frame::AssignFeaturesToGrid() {
   }
 }
 
-void Frame::ExtractORB(int flag, const cv::Mat &im, const int x0,
+void Frame::ExtractORB(bool isLeft, const cv::Mat &im, const int x0,
                        const int x1) {
   vector<int> vLapping = {x0, x1};
-  if (flag == 0)
+  if (isLeft)
     monoLeft =
         (*mpORBextractorLeft)(im, cv::Mat(), mvKeys, mDescriptors, vLapping);
   else
@@ -1138,11 +1138,11 @@ Frame::Frame(const cv::Mat &imLeft, const cv::Mat &imRight,
   std::chrono::steady_clock::time_point time_StartExtORB =
       std::chrono::steady_clock::now();
 #endif
-  thread threadLeft(&Frame::ExtractORB, this, 0, imLeft,
+  thread threadLeft(&Frame::ExtractORB, this, true, imLeft,
                     static_cast<KannalaBrandt8 *>(mpCamera)->mvLappingArea[0],
                     static_cast<KannalaBrandt8 *>(mpCamera)->mvLappingArea[1]);
   thread threadRight(
-      &Frame::ExtractORB, this, 1, imRight,
+      &Frame::ExtractORB, this, false, imRight,
       static_cast<KannalaBrandt8 *>(mpCamera2)->mvLappingArea[0],
       static_cast<KannalaBrandt8 *>(mpCamera2)->mvLappingArea[1]);
   threadLeft.join();
diff --git a/src/ORBextractor.cc b/src/ORBextractor.cc
index 340a7afa..6e1d4567 100644
--- a/src/ORBextractor.cc
+++ b/src/ORBextractor.cc
@@ -477,7 +477,7 @@ void ExtractorNode::DivideNode(ExtractorNode& n1, ExtractorNode& n2,
   const int halfX = ceil(static_cast<float>(UR.x - UL.x) / 2);
   const int halfY = ceil(static_cast<float>(BR.y - UL.y) / 2);
 
-  // Define boundaries of childs
+  // Define boundaries of children
   n1.UL = UL;
   n1.UR = cv::Point2i(UL.x + halfX, UL.y);
   n1.BL = cv::Point2i(UL.x, UL.y + halfY);
@@ -502,7 +502,7 @@ void ExtractorNode::DivideNode(ExtractorNode& n1, ExtractorNode& n2,
   n4.BR = BR;
   n4.vKeys.reserve(vKeys.size());
 
-  // Associate points to childs
+  // Associate points to children
   for (size_t i = 0; i < vKeys.size(); i++) {
     const cv::KeyPoint& kp = vKeys[i];
     if (kp.pt.x < n1.UR.x) {
@@ -552,18 +552,16 @@ vector<cv::KeyPoint> ORBextractor::DistributeOctTree(
   vpIniNodes.resize(nIni);
 
   for (int i = 0; i < nIni; i++) {
-    ExtractorNode ni;
-    ni.UL = cv::Point2i(hX * static_cast<float>(i), 0);
-    ni.UR = cv::Point2i(hX * static_cast<float>(i + 1), 0);
-    ni.BL = cv::Point2i(ni.UL.x, maxY - minY);
-    ni.BR = cv::Point2i(ni.UR.x, maxY - minY);
-    ni.vKeys.reserve(vToDistributeKeys.size());
-
-    lNodes.push_back(ni);
+    auto UL = cv::Point2i(hX * static_cast<float>(i), 0);
+    auto UR = cv::Point2i(hX * static_cast<float>(i + 1), 0);
+    auto BL = cv::Point2i(UL.x, maxY - minY);
+    auto BR = cv::Point2i(UR.x, maxY - minY);
+
+    lNodes.emplace_back(UL, UR, BL, BR, vToDistributeKeys.size());
     vpIniNodes[i] = &lNodes.back();
   }
 
-  // Associate points to childs
+  // Associate points to children
   for (size_t i = 0; i < vToDistributeKeys.size(); i++) {
     const cv::KeyPoint& kp = vToDistributeKeys[i];
     vpIniNodes[kp.pt.x / hX]->vKeys.push_back(kp);
@@ -609,13 +607,13 @@ vector<cv::KeyPoint> ORBextractor::DistributeOctTree(
         ExtractorNode n1, n2, n3, n4;
         lit->DivideNode(n1, n2, n3, n4);
 
-        // Add childs if they contain points
+        // Add children if they contain points
         if (n1.vKeys.size() > 0) {
           lNodes.push_front(n1);
           if (n1.vKeys.size() > 1) {
             nToExpand++;
-            vSizeAndPointerToNode.push_back(
-                make_pair(n1.vKeys.size(), &lNodes.front()));
+            vSizeAndPointerToNode.emplace_back(
+                n1.vKeys.size(), &lNodes.front());
             lNodes.front().lit = lNodes.begin();
           }
         }
@@ -623,8 +621,8 @@ vector<cv::KeyPoint> ORBextractor::DistributeOctTree(
           lNodes.push_front(n2);
           if (n2.vKeys.size() > 1) {
             nToExpand++;
-            vSizeAndPointerToNode.push_back(
-                make_pair(n2.vKeys.size(), &lNodes.front()));
+            vSizeAndPointerToNode.emplace_back(
+                n2.vKeys.size(), &lNodes.front());
             lNodes.front().lit = lNodes.begin();
           }
         }
@@ -632,8 +630,8 @@ vector<cv::KeyPoint> ORBextractor::DistributeOctTree(
           lNodes.push_front(n3);
           if (n3.vKeys.size() > 1) {
             nToExpand++;
-            vSizeAndPointerToNode.push_back(
-                make_pair(n3.vKeys.size(), &lNodes.front()));
+            vSizeAndPointerToNode.emplace_back(
+                n3.vKeys.size(), &lNodes.front());
             lNodes.front().lit = lNodes.begin();
           }
         }
@@ -641,8 +639,8 @@ vector<cv::KeyPoint> ORBextractor::DistributeOctTree(
           lNodes.push_front(n4);
           if (n4.vKeys.size() > 1) {
             nToExpand++;
-            vSizeAndPointerToNode.push_back(
-                make_pair(n4.vKeys.size(), &lNodes.front()));
+            vSizeAndPointerToNode.emplace_back(
+                n4.vKeys.size(), &lNodes.front());
             lNodes.front().lit = lNodes.begin();
           }
         }
@@ -670,36 +668,36 @@ vector<cv::KeyPoint> ORBextractor::DistributeOctTree(
           ExtractorNode n1, n2, n3, n4;
           vPrevSizeAndPointerToNode[j].second->DivideNode(n1, n2, n3, n4);
 
-          // Add childs if they contain points
+          // Add children if they contain points
           if (n1.vKeys.size() > 0) {
             lNodes.push_front(n1);
             if (n1.vKeys.size() > 1) {
-              vSizeAndPointerToNode.push_back(
-                  make_pair(n1.vKeys.size(), &lNodes.front()));
+              vSizeAndPointerToNode.emplace_back(
+                  n1.vKeys.size(), &lNodes.front());
               lNodes.front().lit = lNodes.begin();
             }
           }
           if (n2.vKeys.size() > 0) {
             lNodes.push_front(n2);
             if (n2.vKeys.size() > 1) {
-              vSizeAndPointerToNode.push_back(
-                  make_pair(n2.vKeys.size(), &lNodes.front()));
+              vSizeAndPointerToNode.emplace_back(
+                  n2.vKeys.size(), &lNodes.front());
               lNodes.front().lit = lNodes.begin();
             }
           }
           if (n3.vKeys.size() > 0) {
             lNodes.push_front(n3);
             if (n3.vKeys.size() > 1) {
-              vSizeAndPointerToNode.push_back(
-                  make_pair(n3.vKeys.size(), &lNodes.front()));
+              vSizeAndPointerToNode.emplace_back(
+                  n3.vKeys.size(), &lNodes.front());
               lNodes.front().lit = lNodes.begin();
             }
           }
           if (n4.vKeys.size() > 0) {
             lNodes.push_front(n4);
             if (n4.vKeys.size() > 1) {
-              vSizeAndPointerToNode.push_back(
-                  make_pair(n4.vKeys.size(), &lNodes.front()));
+              vSizeAndPointerToNode.emplace_back(
+                  n4.vKeys.size(), &lNodes.front());
               lNodes.front().lit = lNodes.begin();
             }
           }
@@ -843,157 +841,6 @@ void ORBextractor::ComputeKeyPointsOctTree(
     computeOrientation(mvImagePyramid[level], allKeypoints[level], umax);
 }
 
-void ORBextractor::ComputeKeyPointsOld(
-    std::vector<std::vector<KeyPoint> >& allKeypoints) {
-  allKeypoints.resize(nlevels);
-
-  float imageRatio = (float)mvImagePyramid[0].cols / mvImagePyramid[0].rows;
-
-  for (int level = 0; level < nlevels; ++level) {
-    const int nDesiredFeatures = mnFeaturesPerLevel[level];
-
-    const int levelCols = sqrt((float)nDesiredFeatures / (5 * imageRatio));
-    const int levelRows = imageRatio * levelCols;
-
-    const int minBorderX = EDGE_THRESHOLD;
-    const int minBorderY = minBorderX;
-    const int maxBorderX = mvImagePyramid[level].cols - EDGE_THRESHOLD;
-    const int maxBorderY = mvImagePyramid[level].rows - EDGE_THRESHOLD;
-
-    const int W = maxBorderX - minBorderX;
-    const int H = maxBorderY - minBorderY;
-    const int cellW = ceil((float)W / levelCols);
-    const int cellH = ceil((float)H / levelRows);
-
-    const int nCells = levelRows * levelCols;
-    const int nfeaturesCell = ceil((float)nDesiredFeatures / nCells);
-
-    vector<vector<vector<KeyPoint> > > cellKeyPoints(
-        levelRows, vector<vector<KeyPoint> >(levelCols));
-
-    vector<vector<int> > nToRetain(levelRows, vector<int>(levelCols, 0));
-    vector<vector<int> > nTotal(levelRows, vector<int>(levelCols, 0));
-    vector<vector<bool> > bNoMore(levelRows, vector<bool>(levelCols, false));
-    vector<int> iniXCol(levelCols);
-    vector<int> iniYRow(levelRows);
-    int nNoMore = 0;
-    int nToDistribute = 0;
-
-    float hY = cellH + 6;
-
-    for (int i = 0; i < levelRows; i++) {
-      const float iniY = minBorderY + i * cellH - 3;
-      iniYRow[i] = iniY;
-
-      if (i == levelRows - 1) {
-        hY = maxBorderY + 3 - iniY;
-        if (hY <= 0) continue;
-      }
-
-      float hX = cellW + 6;
-
-      for (int j = 0; j < levelCols; j++) {
-        float iniX;
-
-        if (i == 0) {
-          iniX = minBorderX + j * cellW - 3;
-          iniXCol[j] = iniX;
-        } else {
-          iniX = iniXCol[j];
-        }
-
-        if (j == levelCols - 1) {
-          hX = maxBorderX + 3 - iniX;
-          if (hX <= 0) continue;
-        }
-
-        Mat cellImage = mvImagePyramid[level]
-                            .rowRange(iniY, iniY + hY)
-                            .colRange(iniX, iniX + hX);
-
-        cellKeyPoints[i][j].reserve(nfeaturesCell * 5);
-
-        FAST(cellImage, cellKeyPoints[i][j], iniThFAST, true);
-
-        if (cellKeyPoints[i][j].size() <= 3) {
-          cellKeyPoints[i][j].clear();
-
-          FAST(cellImage, cellKeyPoints[i][j], minThFAST, true);
-        }
-
-        const int nKeys = cellKeyPoints[i][j].size();
-        nTotal[i][j] = nKeys;
-
-        if (nKeys > nfeaturesCell) {
-          nToRetain[i][j] = nfeaturesCell;
-          bNoMore[i][j] = false;
-        } else {
-          nToRetain[i][j] = nKeys;
-          nToDistribute += nfeaturesCell - nKeys;
-          bNoMore[i][j] = true;
-          nNoMore++;
-        }
-      }
-    }
-
-    // Retain by score
-
-    while (nToDistribute > 0 && nNoMore < nCells) {
-      int nNewFeaturesCell =
-          nfeaturesCell + ceil((float)nToDistribute / (nCells - nNoMore));
-      nToDistribute = 0;
-
-      for (int i = 0; i < levelRows; i++) {
-        for (int j = 0; j < levelCols; j++) {
-          if (!bNoMore[i][j]) {
-            if (nTotal[i][j] > nNewFeaturesCell) {
-              nToRetain[i][j] = nNewFeaturesCell;
-              bNoMore[i][j] = false;
-            } else {
-              nToRetain[i][j] = nTotal[i][j];
-              nToDistribute += nNewFeaturesCell - nTotal[i][j];
-              bNoMore[i][j] = true;
-              nNoMore++;
-            }
-          }
-        }
-      }
-    }
-
-    vector<KeyPoint>& keypoints = allKeypoints[level];
-    keypoints.reserve(nDesiredFeatures * 2);
-
-    const int scaledPatchSize = PATCH_SIZE * mvScaleFactor[level];
-
-    // Retain by score and transform coordinates
-    for (int i = 0; i < levelRows; i++) {
-      for (int j = 0; j < levelCols; j++) {
-        vector<KeyPoint>& keysCell = cellKeyPoints[i][j];
-        KeyPointsFilter::retainBest(keysCell, nToRetain[i][j]);
-        if ((int)keysCell.size() > nToRetain[i][j])
-          keysCell.resize(nToRetain[i][j]);
-
-        for (size_t k = 0, kend = keysCell.size(); k < kend; k++) {
-          keysCell[k].pt.x += iniXCol[j];
-          keysCell[k].pt.y += iniYRow[i];
-          keysCell[k].octave = level;
-          keysCell[k].size = scaledPatchSize;
-          keypoints.push_back(keysCell[k]);
-        }
-      }
-    }
-
-    if ((int)keypoints.size() > nDesiredFeatures) {
-      KeyPointsFilter::retainBest(keypoints, nDesiredFeatures);
-      keypoints.resize(nDesiredFeatures);
-    }
-  }
-
-  // and compute orientations
-  for (int level = 0; level < nlevels; ++level)
-    computeOrientation(mvImagePyramid[level], allKeypoints[level], umax);
-}
-
 static void computeDescriptors(const Mat& image, vector<KeyPoint>& keypoints,
                                Mat& descriptors, const vector<Point>& pattern) {
   descriptors = Mat::zeros((int)keypoints.size(), 32, CV_8UC1);
@@ -1018,7 +865,6 @@ int ORBextractor::operator()(InputArray _image, InputArray _mask,
 
   vector<vector<KeyPoint> > allKeypoints;
   ComputeKeyPointsOctTree(allKeypoints);
-  // ComputeKeyPointsOld(allKeypoints);
 
   Mat descriptors;
 
@@ -1092,7 +938,8 @@ void ORBextractor::ComputePyramid(cv::Mat image) {
             cvRound((float)image.rows * scale));
     Size wholeSize(sz.width + EDGE_THRESHOLD * 2,
                    sz.height + EDGE_THRESHOLD * 2);
-    Mat temp(wholeSize, image.type()), masktemp;
+    Mat temp(wholeSize, image.type());
+    Mat masktemp;
     mvImagePyramid[level] =
         temp(Rect(EDGE_THRESHOLD, EDGE_THRESHOLD, sz.width, sz.height));
 

From 105ec8c11018bdd4dae84cd13c56b791079b8594 Mon Sep 17 00:00:00 2001
From: DavidPetkovsek <david.petkovsek@martinrea.com>
Date: Mon, 25 Jul 2022 20:23:05 -0400
Subject: [PATCH 03/26] Pragma once, remove config.cc and .h

---
 CMakeLists.txt                         |  2 -
 include/CameraModels/GeometricCamera.h |  5 +--
 include/CameraModels/KannalaBrandt8.h  |  4 +-
 include/CameraModels/Pinhole.h         |  4 +-
 include/Config.h                       | 54 --------------------------
 include/Converter.h                    |  5 +--
 include/G2oTypes.h                     |  4 +-
 include/GeometricTools.h               |  4 +-
 include/ImuTypes.h                     |  5 +--
 include/KeyFrame.h                     |  4 +-
 include/KeyFrameDatabase.h             |  4 +-
 include/MLPnPsolver.h                  |  5 +--
 include/Map.h                          |  5 +--
 include/MapPoint.h                     |  5 +--
 include/ORBVocabulary.h                |  5 +--
 include/ORBmatcher.h                   |  4 +-
 include/OptimizableTypes.h             |  5 +--
 include/Optimizer.h                    |  4 +-
 include/SerializationUtils.h           |  4 +-
 include/Settings.h                     |  6 +--
 include/Sim3Solver.h                   |  5 +--
 include/TwoViewReconstruction.h        |  5 +--
 src/Config.cc                          | 28 -------------
 23 files changed, 21 insertions(+), 155 deletions(-)
 delete mode 100644 include/Config.h
 delete mode 100644 src/Config.cc

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 4b7d46ec..42b89ab5 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -75,7 +75,6 @@ src/OptimizableTypes.cpp
 src/MLPnPsolver.cpp
 src/GeometricTools.cc
 src/TwoViewReconstruction.cc
-src/Config.cc
 src/Settings.cc
 include/System.h
 include/Tracking.h
@@ -105,7 +104,6 @@ include/MLPnPsolver.h
 include/GeometricTools.h
 include/TwoViewReconstruction.h
 include/SerializationUtils.h
-include/Config.h
 include/Settings.h)
 
 
diff --git a/include/CameraModels/GeometricCamera.h b/include/CameraModels/GeometricCamera.h
index 8b39f584..4e688bcd 100644
--- a/include/CameraModels/GeometricCamera.h
+++ b/include/CameraModels/GeometricCamera.h
@@ -19,8 +19,7 @@
  * ORB-SLAM3. If not, see <http://www.gnu.org/licenses/>.
  */
 
-#ifndef CAMERAMODELS_GEOMETRICCAMERA_H
-#define CAMERAMODELS_GEOMETRICCAMERA_H
+#pragma once
 
 #include <Eigen/Geometry>
 #include <boost/serialization/access.hpp>
@@ -114,5 +113,3 @@ class GeometricCamera {
   unsigned int mnType;
 };
 }  // namespace ORB_SLAM3
-
-#endif  // CAMERAMODELS_GEOMETRICCAMERA_H
diff --git a/include/CameraModels/KannalaBrandt8.h b/include/CameraModels/KannalaBrandt8.h
index 8cb57b71..b5ddbbb0 100644
--- a/include/CameraModels/KannalaBrandt8.h
+++ b/include/CameraModels/KannalaBrandt8.h
@@ -19,8 +19,7 @@
  * ORB-SLAM3. If not, see <http://www.gnu.org/licenses/>.
  */
 
-#ifndef CAMERAMODELS_KANNALABRANDT8_H
-#define CAMERAMODELS_KANNALABRANDT8_H
+#pragma once
 
 #include <assert.h>
 
@@ -133,4 +132,3 @@ class KannalaBrandt8 : public GeometricCamera {
 };
 }  // namespace ORB_SLAM3
 
-#endif  // CAMERAMODELS_KANNALABRANDT8_H
diff --git a/include/CameraModels/Pinhole.h b/include/CameraModels/Pinhole.h
index 9ae6b39f..d114e260 100644
--- a/include/CameraModels/Pinhole.h
+++ b/include/CameraModels/Pinhole.h
@@ -19,8 +19,7 @@
  * ORB-SLAM3. If not, see <http://www.gnu.org/licenses/>.
  */
 
-#ifndef CAMERAMODELS_PINHOLE_H
-#define CAMERAMODELS_PINHOLE_H
+#pragma once
 
 #include <assert.h>
 
@@ -109,4 +108,3 @@ class Pinhole : public GeometricCamera {
 
 // BOOST_CLASS_EXPORT_KEY(ORBSLAM2::Pinhole)
 
-#endif  // CAMERAMODELS_PINHOLE_H
diff --git a/include/Config.h b/include/Config.h
deleted file mode 100644
index a3f6484a..00000000
--- a/include/Config.h
+++ /dev/null
@@ -1,54 +0,0 @@
-/**
- * This file is part of ORB-SLAM3
- *
- * Copyright (C) 2017-2021 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/>.
- */
-
-#ifndef CONFIG_H
-#define CONFIG_H
-
-#include <stdio.h>
-#include <stdlib.h>
-#include <unistd.h>
-
-#include <string>
-
-namespace ORB_SLAM3 {
-
-class ViewerConfig {};
-
-class CameraConfig {};
-
-class ORBExtractorConfig {};
-
-class IMUConfig {};
-
-class ConfigParser {
- public:
-  bool ParseConfigFile(std::string &strConfigFile);
-
- private:
-  ViewerConfig mViewerConfig;
-  CameraConfig mCameraConfig;
-  ORBExtractorConfig mORBConfig;
-  IMUConfig mIMUConfig;
-};
-
-}  // namespace ORB_SLAM3
-
-#endif  // CONFIG_H
diff --git a/include/Converter.h b/include/Converter.h
index 466af230..0978d7d9 100644
--- a/include/Converter.h
+++ b/include/Converter.h
@@ -19,8 +19,7 @@
  * ORB-SLAM3. If not, see <http://www.gnu.org/licenses/>.
  */
 
-#ifndef CONVERTER_H
-#define CONVERTER_H
+#pragma once
 
 #include <Eigen/Dense>
 #include <opencv2/core/core.hpp>
@@ -77,5 +76,3 @@ class Converter {
 };
 
 }  // namespace ORB_SLAM3
-
-#endif  // CONVERTER_H
diff --git a/include/G2oTypes.h b/include/G2oTypes.h
index 60d6cdb0..6ab79193 100644
--- a/include/G2oTypes.h
+++ b/include/G2oTypes.h
@@ -16,8 +16,7 @@
 * If not, see <http://www.gnu.org/licenses/>.
 */
 
-#ifndef G2OTYPES_H
-#define G2OTYPES_H
+#pragma once
 
 #include "g2o/core/base_vertex.h"
 #include "g2o/core/base_binary_edge.h"
@@ -843,4 +842,3 @@ class Edge4DoF : public g2o::BaseBinaryEdge<6,Vector6d,VertexPose4DoF,VertexPose
 
 } //namespace ORB_SLAM2
 
-#endif // G2OTYPES_H
diff --git a/include/GeometricTools.h b/include/GeometricTools.h
index d17d24f9..8c2651b9 100644
--- a/include/GeometricTools.h
+++ b/include/GeometricTools.h
@@ -19,8 +19,7 @@
  * ORB-SLAM3. If not, see <http://www.gnu.org/licenses/>.
  */
 
-#ifndef GEOMETRIC_TOOLS_H
-#define GEOMETRIC_TOOLS_H
+#pragma once
 
 #include <Eigen/Core>
 #include <opencv2/core/core.hpp>
@@ -80,4 +79,3 @@ class GeometricTools {
 
 }  // namespace ORB_SLAM3
 
-#endif  // GEOMETRIC_TOOLS_H
diff --git a/include/ImuTypes.h b/include/ImuTypes.h
index f48a95eb..f9738759 100644
--- a/include/ImuTypes.h
+++ b/include/ImuTypes.h
@@ -19,8 +19,7 @@
  * ORB-SLAM3. If not, see <http://www.gnu.org/licenses/>.
  */
 
-#ifndef IMUTYPES_H
-#define IMUTYPES_H
+#pragma once
 
 #include <Eigen/Core>
 #include <Eigen/Dense>
@@ -271,5 +270,3 @@ Eigen::Matrix3f NormalizeRotation(const Eigen::Matrix3f &R);
 }  // namespace IMU
 
 }  // namespace ORB_SLAM3
-
-#endif  // IMUTYPES_H
diff --git a/include/KeyFrame.h b/include/KeyFrame.h
index 3f0a4ff2..1f7f34af 100644
--- a/include/KeyFrame.h
+++ b/include/KeyFrame.h
@@ -19,8 +19,7 @@
  * ORB-SLAM3. If not, see <http://www.gnu.org/licenses/>.
  */
 
-#ifndef KEYFRAME_H
-#define KEYFRAME_H
+#pragma once
 
 #include <boost/serialization/base_object.hpp>
 #include <boost/serialization/map.hpp>
@@ -541,4 +540,3 @@ class KeyFrame {
 
 }  // namespace ORB_SLAM3
 
-#endif  // KEYFRAME_H
diff --git a/include/KeyFrameDatabase.h b/include/KeyFrameDatabase.h
index d6a1ec98..00c1a584 100644
--- a/include/KeyFrameDatabase.h
+++ b/include/KeyFrameDatabase.h
@@ -19,8 +19,7 @@
  * ORB-SLAM3. If not, see <http://www.gnu.org/licenses/>.
  */
 
-#ifndef KEYFRAMEDATABASE_H
-#define KEYFRAMEDATABASE_H
+#pragma once
 
 #include <boost/serialization/base_object.hpp>
 #include <boost/serialization/list.hpp>
@@ -98,4 +97,3 @@ class KeyFrameDatabase {
 
 }  // namespace ORB_SLAM3
 
-#endif
diff --git a/include/MLPnPsolver.h b/include/MLPnPsolver.h
index bdeed215..aefdea5b 100644
--- a/include/MLPnPsolver.h
+++ b/include/MLPnPsolver.h
@@ -49,8 +49,7 @@
  * SUCH DAMAGE.                                                               *
  ******************************************************************************/
 
-#ifndef ORB_SLAM3_MLPNPSOLVER_H
-#define ORB_SLAM3_MLPNPSOLVER_H
+#pragma once
 
 #include <Eigen/Dense>
 #include <Eigen/Sparse>
@@ -242,5 +241,3 @@ class MLPnPsolver {
 };
 
 }  // namespace ORB_SLAM3
-
-#endif  // ORB_SLAM3_MLPNPSOLVER_H
diff --git a/include/Map.h b/include/Map.h
index e1d20d4f..bf9dd77f 100644
--- a/include/Map.h
+++ b/include/Map.h
@@ -17,8 +17,7 @@
 */
 
 
-#ifndef MAP_H
-#define MAP_H
+#pragma once
 
 #include "MapPoint.h"
 #include "KeyFrame.h"
@@ -203,5 +202,3 @@ class Map
 };
 
 } //namespace ORB_SLAM3
-
-#endif // MAP_H
diff --git a/include/MapPoint.h b/include/MapPoint.h
index 3e714088..7f865be1 100644
--- a/include/MapPoint.h
+++ b/include/MapPoint.h
@@ -17,8 +17,7 @@
 */
 
 
-#ifndef MAPPOINT_H
-#define MAPPOINT_H
+#pragma once
 
 #include "KeyFrame.h"
 #include "Frame.h"
@@ -252,5 +251,3 @@ class MapPoint
 };
 
 } //namespace ORB_SLAM
-
-#endif // MAPPOINT_H
diff --git a/include/ORBVocabulary.h b/include/ORBVocabulary.h
index cf48d179..c5ab1805 100644
--- a/include/ORBVocabulary.h
+++ b/include/ORBVocabulary.h
@@ -19,8 +19,7 @@
  * ORB-SLAM3. If not, see <http://www.gnu.org/licenses/>.
  */
 
-#ifndef ORBVOCABULARY_H
-#define ORBVOCABULARY_H
+#pragma once
 
 #include "DBoW2/FORB.h"
 #include "DBoW2/TemplatedVocabulary.h"
@@ -31,5 +30,3 @@ typedef DBoW2::TemplatedVocabulary<DBoW2::FORB::TDescriptor, DBoW2::FORB>
     ORBVocabulary;
 
 }  // namespace ORB_SLAM3
-
-#endif  // ORBVOCABULARY_H
diff --git a/include/ORBmatcher.h b/include/ORBmatcher.h
index 5c6f3db4..c44c2ac8 100644
--- a/include/ORBmatcher.h
+++ b/include/ORBmatcher.h
@@ -19,8 +19,7 @@
  * ORB-SLAM3. If not, see <http://www.gnu.org/licenses/>.
  */
 
-#ifndef ORBMATCHER_H
-#define ORBMATCHER_H
+#pragma once
 
 #include <opencv2/core/core.hpp>
 #include <opencv2/features2d/features2d.hpp>
@@ -130,4 +129,3 @@ class ORBmatcher {
 
 }  // namespace ORB_SLAM3
 
-#endif  // ORBMATCHER_H
\ No newline at end of file
diff --git a/include/OptimizableTypes.h b/include/OptimizableTypes.h
index a2d3bcbf..eb9210f8 100644
--- a/include/OptimizableTypes.h
+++ b/include/OptimizableTypes.h
@@ -19,8 +19,7 @@
  * ORB-SLAM3. If not, see <http://www.gnu.org/licenses/>.
  */
 
-#ifndef ORB_SLAM3_OPTIMIZABLETYPES_H
-#define ORB_SLAM3_OPTIMIZABLETYPES_H
+#pragma once
 
 #include <GeometricCamera.h>
 #include <g2o/types/sim3.h>
@@ -232,5 +231,3 @@ class EdgeInverseSim3ProjectXYZ
 };
 
 }  // namespace ORB_SLAM3
-
-#endif  // ORB_SLAM3_OPTIMIZABLETYPES_H
diff --git a/include/Optimizer.h b/include/Optimizer.h
index 2ccd77c0..594143f9 100644
--- a/include/Optimizer.h
+++ b/include/Optimizer.h
@@ -19,8 +19,7 @@
  * ORB-SLAM3. If not, see <http://www.gnu.org/licenses/>.
  */
 
-#ifndef OPTIMIZER_H
-#define OPTIMIZER_H
+#pragma once
 
 #include <math.h>
 
@@ -140,4 +139,3 @@ class Optimizer {
 
 }  // namespace ORB_SLAM3
 
-#endif  // OPTIMIZER_H
diff --git a/include/SerializationUtils.h b/include/SerializationUtils.h
index c43fcbcb..11185c13 100644
--- a/include/SerializationUtils.h
+++ b/include/SerializationUtils.h
@@ -19,8 +19,7 @@
  * ORB-SLAM3. If not, see <http://www.gnu.org/licenses/>.
  */
 
-#ifndef SERIALIZATION_UTILS_H
-#define SERIALIZATION_UTILS_H
+#pragma once
 
 #include <Eigen/Core>
 #include <boost/serialization/serialization.hpp>
@@ -153,4 +152,3 @@ void serializeVectorKeyPoints(Archive& ar, const std::vector<cv::KeyPoint>& vKP,
 
 }  // namespace ORB_SLAM3
 
-#endif  // SERIALIZATION_UTILS_H
diff --git a/include/Settings.h b/include/Settings.h
index 252c21d2..ce38db2a 100644
--- a/include/Settings.h
+++ b/include/Settings.h
@@ -19,8 +19,7 @@
  * ORB-SLAM3. If not, see <http://www.gnu.org/licenses/>.
  */
 
-#ifndef ORB_SLAM3_SETTINGS_H
-#define ORB_SLAM3_SETTINGS_H
+#pragma once
 
 // Flag to activate the measurement of time in each process (track,localmap,
 // place recognition).
@@ -235,6 +234,5 @@ class Settings {
    */
   float thFarPoints_;
 };
-};  // namespace ORB_SLAM3
+}  // namespace ORB_SLAM3
 
-#endif  // ORB_SLAM3_SETTINGS_H
diff --git a/include/Sim3Solver.h b/include/Sim3Solver.h
index 56331211..d6d1e219 100644
--- a/include/Sim3Solver.h
+++ b/include/Sim3Solver.h
@@ -19,8 +19,7 @@
  * ORB-SLAM3. If not, see <http://www.gnu.org/licenses/>.
  */
 
-#ifndef SIM3SOLVER_H
-#define SIM3SOLVER_H
+#pragma once
 
 #include <opencv2/opencv.hpp>
 #include <vector>
@@ -136,5 +135,3 @@ class Sim3Solver {
 };
 
 }  // namespace ORB_SLAM3
-
-#endif  // SIM3SOLVER_H
diff --git a/include/TwoViewReconstruction.h b/include/TwoViewReconstruction.h
index 98227e40..e275d531 100644
--- a/include/TwoViewReconstruction.h
+++ b/include/TwoViewReconstruction.h
@@ -16,8 +16,7 @@
 * If not, see <http://www.gnu.org/licenses/>.
 */
 
-#ifndef TwoViewReconstruction_H
-#define TwoViewReconstruction_H
+#pragma once
 
 #include <opencv2/opencv.hpp>
 #include <Eigen/Core>
@@ -95,5 +94,3 @@ namespace ORB_SLAM3
     };
 
 } //namespace ORB_SLAM
-
-#endif // TwoViewReconstruction_H
diff --git a/src/Config.cc b/src/Config.cc
deleted file mode 100644
index d7457f9a..00000000
--- a/src/Config.cc
+++ /dev/null
@@ -1,28 +0,0 @@
-/**
- * This file is part of ORB-SLAM3
- *
- * Copyright (C) 2017-2021 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 "Config.h"
-
-namespace ORB_SLAM3 {
-
-bool ConfigParser::ParseConfigFile(std::string &strConfigFile) { return true; }
-
-}  // namespace ORB_SLAM3

From 3f89307ba57cec071f5381f4f079e5d32ce4eef3 Mon Sep 17 00:00:00 2001
From: DavidPetkovsek <david.petkovsek@martinrea.com>
Date: Mon, 25 Jul 2022 21:00:46 -0400
Subject: [PATCH 04/26] Add isInertial & hasMulticam to CameraType

---
 include/ImprovedTypes.hpp |  3 ++
 include/Tracking.h        |  4 +-
 src/Tracking.cc           | 85 +++++++++++++--------------------------
 3 files changed, 33 insertions(+), 59 deletions(-)

diff --git a/include/ImprovedTypes.hpp b/include/ImprovedTypes.hpp
index 24010ce4..66a5688f 100644
--- a/include/ImprovedTypes.hpp
+++ b/include/ImprovedTypes.hpp
@@ -44,5 +44,8 @@ template<typename KEY, typename VALUE> using umap = std::unordered_map<KEY,VALUE
         IMU_STEREO=4,
         IMU_RGBD=5,
     };
+
+    inline bool isInertial(eSensor sensor) { return sensor == IMU_MONOCULAR || sensor == IMU_STEREO || sensor == IMU_RGBD; }
+    inline bool hasMulticam(eSensor sensor) { return sensor == STEREO || sensor == RGBD || sensor == IMU_STEREO || sensor == IMU_RGBD; }
   }
 }
\ No newline at end of file
diff --git a/include/Tracking.h b/include/Tracking.h
index 964e60bb..003115b4 100644
--- a/include/Tracking.h
+++ b/include/Tracking.h
@@ -52,7 +52,7 @@ class Tracking {
   
   Tracking(System* pSys, ORBVocabulary* pVoc,
            const Atlas_ptr &pAtlas, KeyFrameDatabase* pKFDB,
-           const string& strSettingPath, const int sensor, Settings* settings,
+           const string& strSettingPath, const CameraType::eSensor sensor, Settings* settings,
            const string& _nameSeq = std::string());
 
   ~Tracking();
@@ -119,7 +119,7 @@ class Tracking {
   Tracker::eTrackingState mLastProcessedState;
 
   // Input sensor
-  int mSensor;
+  CameraType::eSensor mSensor;
 
   // Current Frame
   Frame mCurrentFrame;
diff --git a/src/Tracking.cc b/src/Tracking.cc
index 666b94da..b594bf5a 100644
--- a/src/Tracking.cc
+++ b/src/Tracking.cc
@@ -41,7 +41,7 @@ namespace ORB_SLAM3 {
 
 Tracking::Tracking(System* pSys, ORBVocabulary* pVoc, const Atlas_ptr &pAtlas,
                    KeyFrameDatabase* pKFDB, const string& strSettingPath,
-                   const int sensor, Settings* settings, const string& _nameSeq)
+                   const CameraType::eSensor sensor, Settings* settings, const string& _nameSeq)
     : mState(Tracker::NO_IMAGES_YET),
       mSensor(sensor),
       mTrackedFr(0),
@@ -81,8 +81,7 @@ Tracking::Tracking(System* pSys, ORBVocabulary* pVoc, const Atlas_ptr &pAtlas,
     }
 
     bool b_parse_imu = true;
-    if (sensor == CameraType::IMU_MONOCULAR || sensor == CameraType::IMU_STEREO ||
-        sensor == CameraType::IMU_RGBD) {
+    if (CameraType::isInertial(mSensor)) {
       b_parse_imu = ParseIMUParamFile(fSettings);
       if (!b_parse_imu) {
         std::cout << "*Error with the IMU parameters in the config file*"
@@ -578,8 +577,7 @@ void Tracking::newParameterLoader(Settings* settings) {
   mK_(0, 2) = mpCamera->getParameter(2);
   mK_(1, 2) = mpCamera->getParameter(3);
 
-  if ((mSensor == CameraType::STEREO || mSensor == CameraType::IMU_STEREO ||
-       mSensor == CameraType::IMU_RGBD) &&
+  if (CameraType::hasMulticam(mSensor) &&
       settings->cameraType() == Settings::KannalaBrandt) {
     mpCamera2 = settings->camera2();
     mpCamera2 = mpAtlas->AddCamera(mpCamera2);
@@ -587,8 +585,7 @@ void Tracking::newParameterLoader(Settings* settings) {
     mTlr = settings->Tlr();
   }
 
-  if (mSensor == CameraType::STEREO || mSensor == CameraType::RGBD ||
-      mSensor == CameraType::IMU_STEREO || mSensor == CameraType::IMU_RGBD) {
+  if (CameraType::hasMulticam(mSensor)) {
     mbf = settings->bf();
     mThDepth = settings->b() * settings->thDepth();
   }
@@ -897,8 +894,7 @@ bool Tracking::ParseCamParamFile(cv::FileStorage& fSettings) {
       mK_(1, 2) = cy;
     }
 
-    if (mSensor == CameraType::STEREO || mSensor == CameraType::IMU_STEREO ||
-        mSensor == CameraType::IMU_RGBD) {
+    if (CameraType::hasMulticam(mSensor)) {
       // Right camera
       // Camera calibration parameters
       cv::FileNode node = fSettings["Camera2.fx"];
@@ -1091,8 +1087,7 @@ bool Tracking::ParseCamParamFile(cv::FileStorage& fSettings) {
               << std::endl;
   }
 
-  if (mSensor == CameraType::STEREO || mSensor == CameraType::RGBD ||
-      mSensor == CameraType::IMU_STEREO || mSensor == CameraType::IMU_RGBD) {
+  if (CameraType::hasMulticam(mSensor)) {
     cv::FileNode node = fSettings["Camera.bf"];
     if (!node.empty() && node.isReal()) {
       mbf = node.real();
@@ -1123,8 +1118,7 @@ bool Tracking::ParseCamParamFile(cv::FileStorage& fSettings) {
   else
     cout << "- color order: BGR (ignored if grayscale)" << endl;
 
-  if (mSensor == CameraType::STEREO || mSensor == CameraType::RGBD ||
-      mSensor == CameraType::IMU_STEREO || mSensor == CameraType::IMU_RGBD) {
+  if (CameraType::hasMulticam(mSensor)) {
     float fx = mpCamera->getParameter(0);
     cv::FileNode node = fSettings["ThDepth"];
     if (!node.empty() && node.isReal()) {
@@ -1722,8 +1716,7 @@ void Tracking::Track() {
     } 
   }
 
-  if ((mSensor == CameraType::IMU_MONOCULAR || mSensor == CameraType::IMU_STEREO ||
-       mSensor == CameraType::IMU_RGBD) &&
+  if (CameraType::isInertial(mSensor) &&
       mpLastKeyFrame)
     mCurrentFrame.SetNewBias(mpLastKeyFrame->GetImuBias());
 
@@ -1733,8 +1726,7 @@ void Tracking::Track() {
 
   mLastProcessedState = mState;
 
-  if ((mSensor == CameraType::IMU_MONOCULAR || mSensor == CameraType::IMU_STEREO ||
-       mSensor == CameraType::IMU_RGBD) &&
+  if (CameraType::isInertial(mSensor) &&
       !mbCreatedMap) {
 #ifdef REGISTER_TIMES
     std::chrono::steady_clock::time_point time_StartPreIMU =
@@ -1767,8 +1759,7 @@ void Tracking::Track() {
   }
 
   if (mState == Tracker::NOT_INITIALIZED) {
-    if (mSensor == CameraType::STEREO || mSensor == CameraType::RGBD ||
-        mSensor == CameraType::IMU_STEREO || mSensor == CameraType::IMU_RGBD) {
+    if (CameraType::hasMulticam(mSensor)) {
       StereoInitialization();
     } else {
       MonocularInitialization();
@@ -1818,8 +1809,7 @@ void Tracking::Track() {
 
         if (!bOK) {
           if (mCurrentFrame.mnId <= (mnLastRelocFrameId + mnFramesToResetIMU) &&
-              (mSensor == CameraType::IMU_MONOCULAR ||
-               mSensor == CameraType::IMU_STEREO || mSensor == CameraType::IMU_RGBD)) {
+              CameraType::isInertial(mSensor)) {
             mState = Tracker::LOST;
           } else if (pCurrentMap->KeyFramesInMap() > 10) {
             // cout << "KF in map: " << pCurrentMap->KeyFramesInMap() << endl;
@@ -1835,8 +1825,7 @@ void Tracking::Track() {
                              Verbose::VERBOSITY_NORMAL);
 
           bOK = true;
-          if ((mSensor == CameraType::IMU_MONOCULAR ||
-               mSensor == CameraType::IMU_STEREO || mSensor == CameraType::IMU_RGBD)) {
+          if (CameraType::isInertial(mSensor)) {
             if (pCurrentMap->isImuInitialized())
               PredictStateIMU();
             else
@@ -1883,8 +1872,7 @@ void Tracking::Track() {
       // Localization Mode: Local Mapping is deactivated (TODO Not available in
       // inertial mode)
       if (mState == Tracker::LOST) {
-        if (mSensor == CameraType::IMU_MONOCULAR || mSensor == CameraType::IMU_STEREO ||
-            mSensor == CameraType::IMU_RGBD)
+        if (CameraType::isInertial(mSensor))
           Verbose::PrintMess("IMU. State LOST", Verbose::VERBOSITY_NORMAL);
         bOK = Relocalization();
       } else {
@@ -1974,8 +1962,7 @@ void Tracking::Track() {
     if (bOK)
       mState = Tracker::OK;
     else if (mState == Tracker::OK) {
-      if (mSensor == CameraType::IMU_MONOCULAR || mSensor == CameraType::IMU_STEREO ||
-          mSensor == CameraType::IMU_RGBD) {
+      if (CameraType::isInertial(mSensor)) {
         Verbose::PrintMess("Track lost for less than one second...",
                            Verbose::VERBOSITY_NORMAL);
         if (!pCurrentMap->isImuInitialized() ||
@@ -2000,8 +1987,7 @@ void Tracking::Track() {
     // modified)
     if ((mCurrentFrame.mnId < (mnLastRelocFrameId + mnFramesToResetIMU)) &&
         (static_cast<int>(mCurrentFrame.mnId) > mnFramesToResetIMU) &&
-        (mSensor == CameraType::IMU_MONOCULAR || mSensor == CameraType::IMU_STEREO ||
-         mSensor == CameraType::IMU_RGBD) &&
+        CameraType::isInertial(mSensor) &&
         pCurrentMap->isImuInitialized()) {
       // TODO check this situation
       Verbose::PrintMess("Saving pointer to frame. imu needs reset...",
@@ -2050,8 +2036,7 @@ void Tracking::Track() {
         mbVelocity = false;
       }
 
-      // if (mSensor == CameraType::IMU_MONOCULAR || mSensor == CameraType::IMU_STEREO ||
-      //     mSensor == CameraType::IMU_RGBD)
+      // if (CameraType::isInertial(mSensor))
       //   mpMapDrawer->SetCurrentCameraPose(mCurrentFrame.GetPose());
 
       // Clean VO matches
@@ -2082,9 +2067,7 @@ void Tracking::Track() {
       // Check if we need to insert a new keyframe
       // if(bNeedKF && bOK)
       if (bNeedKF && (bOK || (mInsertKFsLost && mState == Tracker::RECENTLY_LOST &&
-                              (mSensor == CameraType::IMU_MONOCULAR ||
-                               mSensor == CameraType::IMU_STEREO ||
-                               mSensor == CameraType::IMU_RGBD))))
+                              CameraType::isInertial(mSensor))))
         CreateNewKeyFrame();
 
 #ifdef REGISTER_TIMES
@@ -2115,8 +2098,7 @@ void Tracking::Track() {
         mpSystem->ResetActiveMap();
         return;
       }
-      if (mSensor == CameraType::IMU_MONOCULAR || mSensor == CameraType::IMU_STEREO ||
-          mSensor == CameraType::IMU_RGBD)
+      if (CameraType::isInertial(mSensor))
         if (!pCurrentMap->isImuInitialized()) {
           Verbose::PrintMess(
               "Track lost before IMU initialisation, reseting...",
@@ -2479,8 +2461,7 @@ void Tracking::CreateInitialMapMonocular() {
 void Tracking::CreateMapInAtlas() {
   mnLastInitFrameId = mCurrentFrame.mnId;
   mpAtlas->CreateNewMap();
-  if (mSensor == CameraType::IMU_STEREO || mSensor == CameraType::IMU_MONOCULAR ||
-      mSensor == CameraType::IMU_RGBD)
+  if (CameraType::isInertial(mSensor))
     mpAtlas->SetInertialSensor();
   mbSetInit = false;
 
@@ -2500,8 +2481,7 @@ void Tracking::CreateMapInAtlas() {
     mbReadyToInitializate = false;
   }
 
-  if ((mSensor == CameraType::IMU_MONOCULAR || mSensor == CameraType::IMU_STEREO ||
-       mSensor == CameraType::IMU_RGBD) &&
+  if (CameraType::isInertial(mSensor) &&
       mpImuPreintegratedFromLastKF) {
     delete mpImuPreintegratedFromLastKF;
     mpImuPreintegratedFromLastKF =
@@ -2581,8 +2561,7 @@ bool Tracking::TrackReferenceKeyFrame() {
     }
   }
 
-  if (mSensor == CameraType::IMU_MONOCULAR || mSensor == CameraType::IMU_STEREO ||
-      mSensor == CameraType::IMU_RGBD)
+  if (CameraType::isInertial(mSensor))
     return true;
   else
     return nmatchesMap >= 10;
@@ -2699,8 +2678,7 @@ bool Tracking::TrackWithMotionModel() {
 
   if (nmatches < 20) {
     Verbose::PrintMess("Not enough matches!!", Verbose::VERBOSITY_NORMAL);
-    if (mSensor == CameraType::IMU_MONOCULAR || mSensor == CameraType::IMU_STEREO ||
-        mSensor == CameraType::IMU_RGBD)
+    if (CameraType::isInertial(mSensor))
       return true;
     else
       return false;
@@ -2735,8 +2713,7 @@ bool Tracking::TrackWithMotionModel() {
     return nmatches > 20;
   }
 
-  if (mSensor == CameraType::IMU_MONOCULAR || mSensor == CameraType::IMU_STEREO ||
-      mSensor == CameraType::IMU_RGBD)
+  if (CameraType::isInertial(mSensor))
     return true;
   else
     return nmatchesMap >= 10;
@@ -2838,8 +2815,7 @@ bool Tracking::TrackLocalMap() {
 }
 
 bool Tracking::NeedNewKeyFrame() {
-  if ((mSensor == CameraType::IMU_MONOCULAR || mSensor == CameraType::IMU_STEREO ||
-       mSensor == CameraType::IMU_RGBD) &&
+  if (CameraType::isInertial(mSensor) &&
       !mpAtlas->GetCurrentMap()->isImuInitialized()) {
     if (mSensor == CameraType::IMU_MONOCULAR &&
         (mCurrentFrame.mTimeStamp - mpLastKeyFrame->mTimeStamp) >= 0.25)
@@ -3016,8 +2992,7 @@ void Tracking::CreateNewKeyFrame() {
                        Verbose::VERBOSITY_NORMAL);
 
   // Reset preintegration from last KF (Create new object)
-  if (mSensor == CameraType::IMU_MONOCULAR || mSensor == CameraType::IMU_STEREO ||
-      mSensor == CameraType::IMU_RGBD) {
+  if (CameraType::isInertial(mSensor)) {
     mpImuPreintegratedFromLastKF =
         new IMU::Preintegrated(pKF->GetImuBias(), pKF->mImuCalib);
   }
@@ -3162,9 +3137,7 @@ void Tracking::SearchLocalPoints() {
         th = 2;
       else
         th = 6;
-    } else if (!mpAtlas->isImuInitialized() &&
-               (mSensor == CameraType::IMU_MONOCULAR ||
-                mSensor == CameraType::IMU_STEREO || mSensor == CameraType::IMU_RGBD)) {
+    } else if (!mpAtlas->isImuInitialized() && CameraType::isInertial(mSensor)) {
       th = 10;
     }
 
@@ -3335,8 +3308,7 @@ void Tracking::UpdateLocalKeyFrames() {
   }
 
   // Add 10 last temporal KFs (mainly for IMU)
-  if ((mSensor == CameraType::IMU_MONOCULAR || mSensor == CameraType::IMU_STEREO ||
-       mSensor == CameraType::IMU_RGBD) &&
+  if (CameraType::isInertial(mSensor) &&
       mvpLocalKeyFrames.size() < 80) {
     KeyFrame* tempKeyFrame = mCurrentFrame.mpLastKeyFrame;
 
@@ -3536,8 +3508,7 @@ void Tracking::Reset(bool bLocMap) {
   // Clear Map (this erase MapPoints and KeyFrames)
   mpAtlas->clearAtlas();
   mpAtlas->CreateNewMap();
-  if (mSensor == CameraType::IMU_STEREO || mSensor == CameraType::IMU_MONOCULAR ||
-      mSensor == CameraType::IMU_RGBD)
+  if (CameraType::isInertial(mSensor))
     mpAtlas->SetInertialSensor();
   mnInitialFrameId = 0;
 

From 98d7fd3eedb770d346a068666986215a3a830610 Mon Sep 17 00:00:00 2001
From: DavidPetkovsek <david.petkovsek@martinrea.com>
Date: Tue, 26 Jul 2022 10:30:14 -0400
Subject: [PATCH 05/26] Increase efficiency of frame ctor

---
 src/Frame.cc        | 56 ++++++++++++++++++++++-----------------------
 src/ORBextractor.cc | 30 ++++++++++++------------
 src/Tracking.cc     | 12 ++++------
 3 files changed, 47 insertions(+), 51 deletions(-)

diff --git a/src/Frame.cc b/src/Frame.cc
index e5cc874d..574112c5 100644
--- a/src/Frame.cc
+++ b/src/Frame.cc
@@ -205,6 +205,26 @@ Frame::Frame(const cv::Mat &imLeft, const cv::Mat &imRight,
           .count();
 #endif
 
+  // This is done only for the first Frame (or after a change in the
+  // calibration)
+  if (mbInitialComputations) {
+    ComputeImageBounds(imLeft);
+
+    mfGridElementWidthInv =
+        static_cast<float>(FRAME_GRID_COLS) / (mnMaxX - mnMinX);
+    mfGridElementHeightInv =
+        static_cast<float>(FRAME_GRID_ROWS) / (mnMaxY - mnMinY);
+
+    fx = K.at<float>(0, 0);
+    fy = K.at<float>(1, 1);
+    cx = K.at<float>(0, 2);
+    cy = K.at<float>(1, 2);
+    invfx = 1.0f / fx;
+    invfy = 1.0f / fy;
+
+    mbInitialComputations = false;
+  }
+
   N = mvKeys.size();
   if (mvKeys.empty()) return;
 
@@ -225,31 +245,11 @@ Frame::Frame(const cv::Mat &imLeft, const cv::Mat &imRight,
           .count();
 #endif
 
-  mvpMapPoints = vector<MapPoint *>(N, static_cast<MapPoint *>(NULL));
+  mvpMapPoints = vector<MapPoint *>(N, nullptr);
   mvbOutlier = vector<bool>(N, false);
   mmProjectPoints.clear();
   mmMatchedInImage.clear();
 
-  // This is done only for the first Frame (or after a change in the
-  // calibration)
-  if (mbInitialComputations) {
-    ComputeImageBounds(imLeft);
-
-    mfGridElementWidthInv =
-        static_cast<float>(FRAME_GRID_COLS) / (mnMaxX - mnMinX);
-    mfGridElementHeightInv =
-        static_cast<float>(FRAME_GRID_ROWS) / (mnMaxY - mnMinY);
-
-    fx = K.at<float>(0, 0);
-    fy = K.at<float>(1, 1);
-    cx = K.at<float>(0, 2);
-    cy = K.at<float>(1, 2);
-    invfx = 1.0f / fx;
-    invfy = 1.0f / fy;
-
-    mbInitialComputations = false;
-  }
-
   mb = mbf / fx;
 
   if (pPrevF) {
@@ -895,7 +895,7 @@ void Frame::ComputeStereoMatches() {
   const int nRows = mpORBextractorLeft->mvImagePyramid[0].rows;
 
   // Assign keypoints to row table
-  vector<vector<size_t>> vRowIndices(nRows, vector<size_t>());
+  vector<vector<size_t>> vRowIndices(nRows);
 
   for (int i = 0; i < nRows; i++) vRowIndices[i].reserve(200);
 
@@ -1157,12 +1157,6 @@ Frame::Frame(const cv::Mat &imLeft, const cv::Mat &imRight,
           .count();
 #endif
 
-  Nleft = mvKeys.size();
-  Nright = mvKeysRight.size();
-  N = Nleft + Nright;
-
-  if (N == 0) return;
-
   // This is done only for the first Frame (or after a change in the
   // calibration)
   if (mbInitialComputations) {
@@ -1183,6 +1177,12 @@ Frame::Frame(const cv::Mat &imLeft, const cv::Mat &imRight,
     mbInitialComputations = false;
   }
 
+  Nleft = mvKeys.size();
+  Nright = mvKeysRight.size();
+  N = Nleft + Nright;
+
+  if (N == 0) return;
+
   mb = mbf / fx;
 
   // Sophus/Eigen
diff --git a/src/ORBextractor.cc b/src/ORBextractor.cc
index 6e1d4567..7c8f2182 100644
--- a/src/ORBextractor.cc
+++ b/src/ORBextractor.cc
@@ -866,23 +866,24 @@ int ORBextractor::operator()(InputArray _image, InputArray _mask,
   vector<vector<KeyPoint> > allKeypoints;
   ComputeKeyPointsOctTree(allKeypoints);
 
-  Mat descriptors;
 
   int nkeypoints = 0;
   for (int level = 0; level < nlevels; ++level)
     nkeypoints += (int)allKeypoints[level].size();
-  if (nkeypoints == 0)
+  // _keypoints = vector<cv::KeyPoint>(nkeypoints);
+  if (nkeypoints == 0){
     _descriptors.release();
-  else {
-    _descriptors.create(nkeypoints, 32, CV_8U);
-    descriptors = _descriptors.getMat();
+    _keypoints.clear();
+    return 0;
   }
-
-  //_keypoints.clear();
-  //_keypoints.reserve(nkeypoints);
-  _keypoints = vector<cv::KeyPoint>(nkeypoints);
-
-  int offset = 0;
+  
+  _descriptors.create(nkeypoints, 32, CV_8U);
+  Mat descriptors = _descriptors.getMat();
+  
+  _keypoints.clear();
+  _keypoints.reserve(nkeypoints);
+
+  // int offset = 0;
   // Modified for speeding up stereo fisheye matching
   int monoIndex = 0, stereoIndex = nkeypoints - 1;
   for (int level = 0; level < nlevels; ++level) {
@@ -892,15 +893,15 @@ int ORBextractor::operator()(InputArray _image, InputArray _mask,
     if (nkeypointsLevel == 0) continue;
 
     // preprocess the resized image
-    Mat workingMat = mvImagePyramid[level].clone();
-    GaussianBlur(workingMat, workingMat, Size(7, 7), 2, 2, BORDER_REFLECT_101);
+    Mat workingMat;// = mvImagePyramid[level].clone();
+    GaussianBlur(mvImagePyramid[level], workingMat, Size(7, 7), 2, 2, BORDER_REFLECT_101);
 
     // Compute the descriptors
     // Mat desc = descriptors.rowRange(offset, offset + nkeypointsLevel);
     Mat desc = cv::Mat(nkeypointsLevel, 32, CV_8U);
     computeDescriptors(workingMat, keypoints, desc, pattern);
 
-    offset += nkeypointsLevel;
+    // offset += nkeypointsLevel;
 
     float scale =
         mvScaleFactor[level];  // getScale(level, firstLevel, scaleFactor);
@@ -939,7 +940,6 @@ void ORBextractor::ComputePyramid(cv::Mat image) {
     Size wholeSize(sz.width + EDGE_THRESHOLD * 2,
                    sz.height + EDGE_THRESHOLD * 2);
     Mat temp(wholeSize, image.type());
-    Mat masktemp;
     mvImagePyramid[level] =
         temp(Rect(EDGE_THRESHOLD, EDGE_THRESHOLD, sz.width, sz.height));
 
diff --git a/src/Tracking.cc b/src/Tracking.cc
index b594bf5a..07fb214c 100644
--- a/src/Tracking.cc
+++ b/src/Tracking.cc
@@ -25,6 +25,7 @@
 #include <chrono>
 #include <iostream>
 #include <mutex>
+#include <stdexcept>
 
 #include "Converter.h"
 #include "G2oTypes.h"
@@ -94,10 +95,7 @@ Tracking::Tracking(System* pSys, ORBVocabulary* pVoc, const Atlas_ptr &pAtlas,
     if (!b_parse_cam || !b_parse_orb || !b_parse_imu) {
       std::cerr << "**ERROR in the config file, the format is not correct**"
                 << std::endl;
-      try {
-        throw -1;
-      } catch (exception& e) {
-      }
+      throw std::runtime_error("**ERROR in the config file, the format is not correct**");
     }
   }
 
@@ -1716,8 +1714,7 @@ void Tracking::Track() {
     } 
   }
 
-  if (CameraType::isInertial(mSensor) &&
-      mpLastKeyFrame)
+  if (CameraType::isInertial(mSensor) && mpLastKeyFrame)
     mCurrentFrame.SetNewBias(mpLastKeyFrame->GetImuBias());
 
   if (mState == Tracker::NO_IMAGES_YET) {
@@ -1726,8 +1723,7 @@ void Tracking::Track() {
 
   mLastProcessedState = mState;
 
-  if (CameraType::isInertial(mSensor) &&
-      !mbCreatedMap) {
+  if (CameraType::isInertial(mSensor) && !mbCreatedMap) {
 #ifdef REGISTER_TIMES
     std::chrono::steady_clock::time_point time_StartPreIMU =
         std::chrono::steady_clock::now();

From 18d924eb330e850bc0cc1e5469385e7576eda4bf Mon Sep 17 00:00:00 2001
From: Soldann <guotl321@gmail.com>
Date: Tue, 26 Jul 2022 05:32:14 -1000
Subject: [PATCH 06/26] build.sh

---
 CMakeLists.txt                                |  2 +-
 Examples/Stereo-Inertial/RealSense_D435i.yaml |  4 ++--
 build.sh                                      | 15 +++++++++++----
 3 files changed, 14 insertions(+), 7 deletions(-)

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 42b89ab5..89036c73 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -24,7 +24,7 @@ endif()
 
 LIST(APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake_modules)
 
-find_package(OpenCV 4.4)
+find_package(OpenCV 4.2)
    if(NOT OpenCV_FOUND)
       message(FATAL_ERROR "OpenCV > 4.4 not found.")
    endif()
diff --git a/Examples/Stereo-Inertial/RealSense_D435i.yaml b/Examples/Stereo-Inertial/RealSense_D435i.yaml
index e69ba166..ff368a04 100755
--- a/Examples/Stereo-Inertial/RealSense_D435i.yaml
+++ b/Examples/Stereo-Inertial/RealSense_D435i.yaml
@@ -82,9 +82,9 @@ Viewer.ViewpointZ: -3.5
 Viewer.ViewpointF: 500.0
 
 # Save/load Map
-System.LoadAtlasFromFile: "/home/ryan/MartinRea/MORB_SLAM/foxtrot5"
+# System.LoadAtlasFromFile: "/home/ryan/MartinRea/MORB_SLAM/foxtrot5"
 # System.LoadAtlasFromFile: "/home/ryan/MartinRea/MORB_SLAM/loopy"
-System.SaveAtlasToFile: "/home/ryan/MartinRea/MORB_SLAM/rando"
+# System.SaveAtlasToFile: "/home/ryan/MartinRea/MORB_SLAM/rando"
 
 
 
diff --git a/build.sh b/build.sh
index d9ea0686..f5c2b3f7 100755
--- a/build.sh
+++ b/build.sh
@@ -1,10 +1,17 @@
+if [ $# == "1" ]; then
+    jobs=$1
+else 
+    jobs="-j$(nproc)"
+fi
+echo "Using argument ${jobs}"
+
 echo "Configuring and building Thirdparty/DBoW2 ..."
 
 cd Thirdparty/DBoW2
 mkdir build 2> /dev/null
 cd build
 cmake .. -DCMAKE_BUILD_TYPE=Release
-make -j$(nproc)
+make ${jobs}
 
 cd ../../g2o
 
@@ -13,7 +20,7 @@ echo "Configuring and building Thirdparty/g2o ..."
 mkdir build 2> /dev/null
 cd build
 cmake .. -DCMAKE_BUILD_TYPE=Release
-make -j$(nproc)
+make ${jobs}
 
 cd ../../Sophus
 
@@ -22,7 +29,7 @@ echo "Configuring and building Thirdparty/Sophus ..."
 mkdir build 2> /dev/null
 cd build
 cmake .. -DCMAKE_BUILD_TYPE=Release
-make -j$(nproc)
+make ${jobs}
 
 cd ../../../
 
@@ -39,4 +46,4 @@ echo "Configuring and building ORB_SLAM3 ..."
 mkdir build 2> /dev/null
 cd build
 cmake .. -DCMAKE_BUILD_TYPE=RelWithDebInfo
-make -j$(nproc)
+make ${jobs}

From eacb18ce3ec4661682eb0b092bcaa1edc5003c1d Mon Sep 17 00:00:00 2001
From: DavidPetkovsek <david.petkovsek@martinrea.com>
Date: Tue, 26 Jul 2022 11:33:11 -0400
Subject: [PATCH 07/26] Move static initializer before exit early

---
 src/Frame.cc | 65 ++++++++++++++++++++++++++--------------------------
 1 file changed, 32 insertions(+), 33 deletions(-)

diff --git a/src/Frame.cc b/src/Frame.cc
index 574112c5..7a7ff86a 100644
--- a/src/Frame.cc
+++ b/src/Frame.cc
@@ -326,21 +326,6 @@ Frame::Frame(const cv::Mat &imGray, const cv::Mat &imDepth,
           .count();
 #endif
 
-  N = mvKeys.size();
-
-  if (mvKeys.empty()) return;
-
-  UndistortKeyPoints();
-
-  ComputeStereoFromRGBD(imDepth);
-
-  mvpMapPoints = vector<MapPoint *>(N, static_cast<MapPoint *>(NULL));
-
-  mmProjectPoints.clear();
-  mmMatchedInImage.clear();
-
-  mvbOutlier = vector<bool>(N, false);
-
   // This is done only for the first Frame (or after a change in the
   // calibration)
   if (mbInitialComputations) {
@@ -361,6 +346,20 @@ Frame::Frame(const cv::Mat &imGray, const cv::Mat &imDepth,
     mbInitialComputations = false;
   }
 
+  N = mvKeys.size();
+  if (mvKeys.empty()) return;
+
+  UndistortKeyPoints();
+
+  ComputeStereoFromRGBD(imDepth);
+
+  mvpMapPoints = vector<MapPoint *>(N, static_cast<MapPoint *>(NULL));
+
+  mmProjectPoints.clear();
+  mmMatchedInImage.clear();
+
+  mvbOutlier = vector<bool>(N, false);
+
   mb = mbf / fx;
 
   if (pPrevF) {
@@ -436,24 +435,6 @@ Frame::Frame(const cv::Mat &imGray, const double &timeStamp,
           .count();
 #endif
 
-  N = mvKeys.size();
-  if (mvKeys.empty()) return;
-
-  UndistortKeyPoints();
-
-  // Set no stereo information
-  mvuRight = vector<float>(N, -1);
-  mvDepth = vector<float>(N, -1);
-  mnCloseMPs = 0;
-
-  mvpMapPoints = vector<MapPoint *>(N, static_cast<MapPoint *>(NULL));
-
-  mmProjectPoints.clear();  // = map<long unsigned int, cv::Point2f>(N,
-                            // static_cast<cv::Point2f>(NULL));
-  mmMatchedInImage.clear();
-
-  mvbOutlier = vector<bool>(N, false);
-
   // This is done only for the first Frame (or after a change in the
   // calibration)
   if (mbInitialComputations) {
@@ -474,6 +455,24 @@ Frame::Frame(const cv::Mat &imGray, const double &timeStamp,
     mbInitialComputations = false;
   }
 
+  N = mvKeys.size();
+  if (mvKeys.empty()) return;
+
+  UndistortKeyPoints();
+
+  // Set no stereo information
+  mvuRight = vector<float>(N, -1);
+  mvDepth = vector<float>(N, -1);
+  mnCloseMPs = 0;
+
+  mvpMapPoints = vector<MapPoint *>(N, static_cast<MapPoint *>(NULL));
+
+  mmProjectPoints.clear();  // = map<long unsigned int, cv::Point2f>(N,
+                            // static_cast<cv::Point2f>(NULL));
+  mmMatchedInImage.clear();
+
+  mvbOutlier = vector<bool>(N, false);
+
   mb = mbf / fx;
 
   // Set no stereo fisheye information

From 7c086586f921900a15d6dcb1e98825115bc13857 Mon Sep 17 00:00:00 2001
From: DavidPetkovsek <david.petkovsek@martinrea.com>
Date: Tue, 26 Jul 2022 11:33:31 -0400
Subject: [PATCH 08/26] Fix adding keypoints to _keypoints

---
 src/ORBextractor.cc | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/ORBextractor.cc b/src/ORBextractor.cc
index 7c8f2182..95716288 100644
--- a/src/ORBextractor.cc
+++ b/src/ORBextractor.cc
@@ -916,11 +916,11 @@ int ORBextractor::operator()(InputArray _image, InputArray _mask,
 
       if (keypoint->pt.x >= vLappingArea[0] &&
           keypoint->pt.x <= vLappingArea[1]) {
-        _keypoints.at(stereoIndex) = (*keypoint);
+        _keypoints.push_back(*keypoint);
         desc.row(i).copyTo(descriptors.row(stereoIndex));
         stereoIndex--;
       } else {
-        _keypoints.at(monoIndex) = (*keypoint);
+        _keypoints.push_back(*keypoint);
         desc.row(i).copyTo(descriptors.row(monoIndex));
         monoIndex++;
       }

From 693b99d87cd1bf721cb0c284f92f0cd3d2be09a7 Mon Sep 17 00:00:00 2001
From: DavidPetkovsek <david.petkovsek@martinrea.com>
Date: Tue, 26 Jul 2022 12:38:32 -0400
Subject: [PATCH 09/26] Update frame ctor to init all vars

---
 include/Frame.h | 10 +++++-----
 src/Frame.cc    | 25 ++++++++++++++++++++-----
 src/Tracking.cc | 29 ++++++++++-------------------
 3 files changed, 35 insertions(+), 29 deletions(-)

diff --git a/include/Frame.h b/include/Frame.h
index 4f419c09..627e9da1 100644
--- a/include/Frame.h
+++ b/include/Frame.h
@@ -58,16 +58,16 @@ class Frame
     Frame(const Frame &frame);
 
     // Constructor for stereo cameras.
-    Frame(const cv::Mat &imLeft, const cv::Mat &imRight, const double &timeStamp, ORBextractor* extractorLeft, ORBextractor* extractorRight, ORBVocabulary* voc, cv::Mat &K, cv::Mat &distCoef, const float &bf, const float &thDepth, GeometricCamera* pCamera,Frame* pPrevF = static_cast<Frame*>(NULL), const IMU::Calib &ImuCalib = IMU::Calib());
+    Frame(const cv::Mat &imLeft, const cv::Mat &imRight, const double &timeStamp, ORBextractor* extractorLeft, ORBextractor* extractorRight, ORBVocabulary* voc, cv::Mat &K, cv::Mat &distCoef, const float &bf, const float &thDepth, GeometricCamera* pCamera, const std::string &pNameFile, int pnNumDataset, Frame* pPrevF = static_cast<Frame*>(NULL), const IMU::Calib &ImuCalib = IMU::Calib());
 
     // Constructor for RGB-D cameras.
-    Frame(const cv::Mat &imGray, const cv::Mat &imDepth, const double &timeStamp, ORBextractor* extractor,ORBVocabulary* voc, cv::Mat &K, cv::Mat &distCoef, const float &bf, const float &thDepth, GeometricCamera* pCamera,Frame* pPrevF = static_cast<Frame*>(NULL), const IMU::Calib &ImuCalib = IMU::Calib());
+    Frame(const cv::Mat &imGray, const cv::Mat &imDepth, const double &timeStamp, ORBextractor* extractor,ORBVocabulary* voc, cv::Mat &K, cv::Mat &distCoef, const float &bf, const float &thDepth, GeometricCamera* pCamera, const std::string &pNameFile, int pnNumDataset, Frame* pPrevF = static_cast<Frame*>(NULL), const IMU::Calib &ImuCalib = IMU::Calib());
 
     // Constructor for Monocular cameras.
-    Frame(const cv::Mat &imGray, const double &timeStamp, ORBextractor* extractor,ORBVocabulary* voc, GeometricCamera* pCamera, cv::Mat &distCoef, const float &bf, const float &thDepth, Frame* pPrevF = static_cast<Frame*>(NULL), const IMU::Calib &ImuCalib = IMU::Calib());
+    Frame(const cv::Mat &imGray, const double &timeStamp, ORBextractor* extractor,ORBVocabulary* voc, GeometricCamera* pCamera, cv::Mat &distCoef, const float &bf, const float &thDepth, const std::string &pNameFile, int pnNumDataset,  Frame* pPrevF = static_cast<Frame*>(NULL), const IMU::Calib &ImuCalib = IMU::Calib());
 
     // Destructor
-    // ~Frame();
+    virtual ~Frame();
 
     // Extract ORB on the image. 0 for left image and 1 for right image.
     void ExtractORB(bool isLeft, const cv::Mat &im, const int x0, const int x1);
@@ -342,7 +342,7 @@ class Frame
     //Grid for the right image
     std::vector<std::size_t> mGridRight[FRAME_GRID_COLS][FRAME_GRID_ROWS];
 
-    Frame(const cv::Mat &imLeft, const cv::Mat &imRight, const double &timeStamp, ORBextractor* extractorLeft, ORBextractor* extractorRight, ORBVocabulary* voc, cv::Mat &K, cv::Mat &distCoef, const float &bf, const float &thDepth, GeometricCamera* pCamera, GeometricCamera* pCamera2, Sophus::SE3f& Tlr,Frame* pPrevF = static_cast<Frame*>(NULL), const IMU::Calib &ImuCalib = IMU::Calib());
+    Frame(const cv::Mat &imLeft, const cv::Mat &imRight, const double &timeStamp, ORBextractor* extractorLeft, ORBextractor* extractorRight, ORBVocabulary* voc, cv::Mat &K, cv::Mat &distCoef, const float &bf, const float &thDepth, GeometricCamera* pCamera, GeometricCamera* pCamera2, const std::string &pNameFile, int pnNumDataset, Sophus::SE3f& Tlr,Frame* pPrevF = static_cast<Frame*>(NULL), const IMU::Calib &ImuCalib = IMU::Calib());
 
     //Stereo fisheye
     void ComputeStereoFishEyeMatches();
diff --git a/src/Frame.cc b/src/Frame.cc
index 7a7ff86a..2b7d8c04 100644
--- a/src/Frame.cc
+++ b/src/Frame.cc
@@ -151,8 +151,8 @@ Frame::Frame(const cv::Mat &imLeft, const cv::Mat &imRight,
              const double &timeStamp, ORBextractor *extractorLeft,
              ORBextractor *extractorRight, ORBVocabulary *voc, cv::Mat &K,
              cv::Mat &distCoef, const float &bf, const float &thDepth,
-             GeometricCamera *pCamera, Frame *pPrevF,
-             const IMU::Calib &ImuCalib)
+             GeometricCamera *pCamera, const std::string &pNameFile, int pnNumDataset,
+             Frame *pPrevF, const IMU::Calib &ImuCalib)
     : mpcpi(NULL),
       mbHasPose(false),
       mbHasVelocity(false),
@@ -170,6 +170,8 @@ Frame::Frame(const cv::Mat &imLeft, const cv::Mat &imRight,
       mpPrevFrame(pPrevF),
       mpImuPreintegratedFrame(NULL),
       mpReferenceKF(static_cast<KeyFrame *>(NULL)),
+      mNameFile{pNameFile},
+      mnDataset{pnNumDataset},
       mbIsSet(false),
       mbImuPreintegrated(false),
       mpCamera(pCamera),
@@ -272,10 +274,15 @@ Frame::Frame(const cv::Mat &imLeft, const cv::Mat &imRight,
   AssignFeaturesToGrid();
 }
 
+Frame::~Frame(){
+  delete mpMutexImu;
+}
+
 Frame::Frame(const cv::Mat &imGray, const cv::Mat &imDepth,
              const double &timeStamp, ORBextractor *extractor,
              ORBVocabulary *voc, cv::Mat &K, cv::Mat &distCoef, const float &bf,
-             const float &thDepth, GeometricCamera *pCamera, Frame *pPrevF,
+             const float &thDepth, GeometricCamera *pCamera,
+             const std::string &pNameFile, int pnNumDataset, Frame *pPrevF,
              const IMU::Calib &ImuCalib)
     : mpcpi(NULL),
       mbHasPose(false),
@@ -294,6 +301,8 @@ Frame::Frame(const cv::Mat &imGray, const cv::Mat &imDepth,
       mpPrevFrame(pPrevF),
       mpImuPreintegratedFrame(NULL),
       mpReferenceKF(static_cast<KeyFrame *>(NULL)),
+      mNameFile{pNameFile},
+      mnDataset{pnNumDataset},
       mbIsSet(false),
       mbImuPreintegrated(false),
       mpCamera(pCamera),
@@ -385,7 +394,8 @@ Frame::Frame(const cv::Mat &imGray, const cv::Mat &imDepth,
 Frame::Frame(const cv::Mat &imGray, const double &timeStamp,
              ORBextractor *extractor, ORBVocabulary *voc,
              GeometricCamera *pCamera, cv::Mat &distCoef, const float &bf,
-             const float &thDepth, Frame *pPrevF, const IMU::Calib &ImuCalib)
+             const float &thDepth, const std::string &pNameFile, int pnNumDataset,
+             Frame *pPrevF, const IMU::Calib &ImuCalib)
     : mpcpi(NULL),
       mbHasPose(false),
       mbHasVelocity(false),
@@ -403,6 +413,8 @@ Frame::Frame(const cv::Mat &imGray, const double &timeStamp,
       mpPrevFrame(pPrevF),
       mpImuPreintegratedFrame(NULL),
       mpReferenceKF(static_cast<KeyFrame *>(NULL)),
+      mNameFile{pNameFile},
+      mnDataset{pnNumDataset},
       mbIsSet(false),
       mbImuPreintegrated(false),
       mpCamera(pCamera),
@@ -1026,7 +1038,7 @@ void Frame::ComputeStereoMatches() {
         }
         mvDepth[iL] = mbf / disparity;
         mvuRight[iL] = bestuR;
-        vDistIdx.push_back(pair<int, int>(bestDist, iL));
+        vDistIdx.emplace_back(bestDist, iL);
       }
     }
   }
@@ -1094,6 +1106,7 @@ Frame::Frame(const cv::Mat &imLeft, const cv::Mat &imRight,
              ORBextractor *extractorRight, ORBVocabulary *voc, cv::Mat &K,
              cv::Mat &distCoef, const float &bf, const float &thDepth,
              GeometricCamera *pCamera, GeometricCamera *pCamera2,
+             const std::string &pNameFile, int pnNumDataset,
              Sophus::SE3f &Tlr, Frame *pPrevF, const IMU::Calib &ImuCalib)
     : mpcpi(NULL),
       mbHasPose(false),
@@ -1112,6 +1125,8 @@ Frame::Frame(const cv::Mat &imLeft, const cv::Mat &imRight,
       mpPrevFrame(pPrevF),
       mpImuPreintegratedFrame(NULL),
       mpReferenceKF(static_cast<KeyFrame *>(NULL)),
+      mNameFile{pNameFile},
+      mnDataset{pnNumDataset},
       mbImuPreintegrated(false),
       mpCamera(pCamera),
       mpCamera2(pCamera2)
diff --git a/src/Tracking.cc b/src/Tracking.cc
index 07fb214c..7b7f30a8 100644
--- a/src/Tracking.cc
+++ b/src/Tracking.cc
@@ -1387,26 +1387,23 @@ Sophus::SE3f Tracking::GrabImageStereo(const cv::Mat& imRectLeft,
   if (mSensor == CameraType::STEREO && !mpCamera2)
     mCurrentFrame = Frame(mImGray, imGrayRight, timestamp, mpORBextractorLeft,
                           mpORBextractorRight, mpORBVocabulary, mK, mDistCoef,
-                          mbf, mThDepth, mpCamera);
+                          mbf, mThDepth, mpCamera, filename, mnNumDataset);
   else if (mSensor == CameraType::STEREO && mpCamera2)
     mCurrentFrame = Frame(mImGray, imGrayRight, timestamp, mpORBextractorLeft,
                           mpORBextractorRight, mpORBVocabulary, mK, mDistCoef,
-                          mbf, mThDepth, mpCamera, mpCamera2, mTlr);
+                          mbf, mThDepth, mpCamera, mpCamera2, filename, mnNumDataset, mTlr);
   else if (mSensor == CameraType::IMU_STEREO && !mpCamera2)
     mCurrentFrame = Frame(mImGray, imGrayRight, timestamp, mpORBextractorLeft,
                           mpORBextractorRight, mpORBVocabulary, mK, mDistCoef,
-                          mbf, mThDepth, mpCamera, &mLastFrame, *mpImuCalib);
+                          mbf, mThDepth, mpCamera, filename, mnNumDataset, &mLastFrame, *mpImuCalib);
   else if (mSensor == CameraType::IMU_STEREO && mpCamera2)
     mCurrentFrame =
         Frame(mImGray, imGrayRight, timestamp, mpORBextractorLeft,
               mpORBextractorRight, mpORBVocabulary, mK, mDistCoef, mbf,
-              mThDepth, mpCamera, mpCamera2, mTlr, &mLastFrame, *mpImuCalib);
+              mThDepth, mpCamera, mpCamera2, filename, mnNumDataset, mTlr, &mLastFrame, *mpImuCalib);
 
   // cout << "Incoming frame ended" << endl;
 
-  mCurrentFrame.mNameFile = filename;
-  mCurrentFrame.mnDataset = mnNumDataset;
-
 #ifdef REGISTER_TIMES
   vdORBExtract_ms.push_back(mCurrentFrame.mTimeORB_Ext);
   vdStereoMatch_ms.push_back(mCurrentFrame.mTimeStereoMatch);
@@ -1442,14 +1439,11 @@ Sophus::SE3f Tracking::GrabImageRGBD(const cv::Mat& imRGB, const cv::Mat& imD,
   if (mSensor == CameraType::RGBD)
     mCurrentFrame =
         Frame(mImGray, imDepth, timestamp, mpORBextractorLeft, mpORBVocabulary,
-              mK, mDistCoef, mbf, mThDepth, mpCamera);
+              mK, mDistCoef, mbf, mThDepth, mpCamera, filename, mnNumDataset);
   else if (mSensor == CameraType::IMU_RGBD)
     mCurrentFrame =
         Frame(mImGray, imDepth, timestamp, mpORBextractorLeft, mpORBVocabulary,
-              mK, mDistCoef, mbf, mThDepth, mpCamera, &mLastFrame, *mpImuCalib);
-
-  mCurrentFrame.mNameFile = filename;
-  mCurrentFrame.mnDataset = mnNumDataset;
+              mK, mDistCoef, mbf, mThDepth, mpCamera, filename, mnNumDataset, &mLastFrame, *mpImuCalib);
 
 #ifdef REGISTER_TIMES
   vdORBExtract_ms.push_back(mCurrentFrame.mTimeORB_Ext);
@@ -1481,27 +1475,24 @@ Sophus::SE3f Tracking::GrabImageMonocular(const cv::Mat& im,
         (lastID - initID) < mMaxFrames)
       mCurrentFrame =
           Frame(mImGray, timestamp, mpIniORBextractor, mpORBVocabulary,
-                mpCamera, mDistCoef, mbf, mThDepth);
+                mpCamera, mDistCoef, mbf, mThDepth, filename, mnNumDataset);
     else
       mCurrentFrame =
           Frame(mImGray, timestamp, mpORBextractorLeft, mpORBVocabulary,
-                mpCamera, mDistCoef, mbf, mThDepth);
+                mpCamera, mDistCoef, mbf, mThDepth, filename, mnNumDataset);
   } else if (mSensor == CameraType::IMU_MONOCULAR) {
     if (mState == Tracker::NOT_INITIALIZED || mState == Tracker::NO_IMAGES_YET) {
       mCurrentFrame =
           Frame(mImGray, timestamp, mpIniORBextractor, mpORBVocabulary,
-                mpCamera, mDistCoef, mbf, mThDepth, &mLastFrame, *mpImuCalib);
+                mpCamera, mDistCoef, mbf, mThDepth, filename, mnNumDataset, &mLastFrame, *mpImuCalib);
     } else
       mCurrentFrame =
           Frame(mImGray, timestamp, mpORBextractorLeft, mpORBVocabulary,
-                mpCamera, mDistCoef, mbf, mThDepth, &mLastFrame, *mpImuCalib);
+                mpCamera, mDistCoef, mbf, mThDepth, filename, mnNumDataset, &mLastFrame, *mpImuCalib);
   }
 
   if (mState == Tracker::NO_IMAGES_YET) t0 = timestamp;
 
-  mCurrentFrame.mNameFile = filename;
-  mCurrentFrame.mnDataset = mnNumDataset;
-
 #ifdef REGISTER_TIMES
   vdORBExtract_ms.push_back(mCurrentFrame.mTimeORB_Ext);
 #endif

From 70a2d26e8f605c37de75537e4a123689bb018caa Mon Sep 17 00:00:00 2001
From: DavidPetkovsek <david.petkovsek@martinrea.com>
Date: Tue, 26 Jul 2022 12:47:25 -0400
Subject: [PATCH 10/26] Clean up & change static cast NULL to nullptr

---
 include/Frame.h    |  8 ++++----
 src/Atlas.cc       | 10 +++++-----
 src/Frame.cc       | 24 ++++++++++++------------
 src/KeyFrame.cc    | 12 ++++++------
 src/LoopClosing.cc | 20 ++++++++++----------
 src/Map.cc         | 12 ++++++------
 src/MapPoint.cc    | 12 ++++++------
 src/ORBmatcher.cc  | 14 +++++++-------
 src/Optimizer.cc   |  6 +++---
 src/Tracking.cc    | 38 +++++++++++++++++++-------------------
 10 files changed, 78 insertions(+), 78 deletions(-)

diff --git a/include/Frame.h b/include/Frame.h
index 627e9da1..a740e866 100644
--- a/include/Frame.h
+++ b/include/Frame.h
@@ -58,13 +58,13 @@ class Frame
     Frame(const Frame &frame);
 
     // Constructor for stereo cameras.
-    Frame(const cv::Mat &imLeft, const cv::Mat &imRight, const double &timeStamp, ORBextractor* extractorLeft, ORBextractor* extractorRight, ORBVocabulary* voc, cv::Mat &K, cv::Mat &distCoef, const float &bf, const float &thDepth, GeometricCamera* pCamera, const std::string &pNameFile, int pnNumDataset, Frame* pPrevF = static_cast<Frame*>(NULL), const IMU::Calib &ImuCalib = IMU::Calib());
+    Frame(const cv::Mat &imLeft, const cv::Mat &imRight, const double &timeStamp, ORBextractor* extractorLeft, ORBextractor* extractorRight, ORBVocabulary* voc, cv::Mat &K, cv::Mat &distCoef, const float &bf, const float &thDepth, GeometricCamera* pCamera, const std::string &pNameFile, int pnNumDataset, Frame* pPrevF = nullptr, const IMU::Calib &ImuCalib = IMU::Calib());
 
     // Constructor for RGB-D cameras.
-    Frame(const cv::Mat &imGray, const cv::Mat &imDepth, const double &timeStamp, ORBextractor* extractor,ORBVocabulary* voc, cv::Mat &K, cv::Mat &distCoef, const float &bf, const float &thDepth, GeometricCamera* pCamera, const std::string &pNameFile, int pnNumDataset, Frame* pPrevF = static_cast<Frame*>(NULL), const IMU::Calib &ImuCalib = IMU::Calib());
+    Frame(const cv::Mat &imGray, const cv::Mat &imDepth, const double &timeStamp, ORBextractor* extractor,ORBVocabulary* voc, cv::Mat &K, cv::Mat &distCoef, const float &bf, const float &thDepth, GeometricCamera* pCamera, const std::string &pNameFile, int pnNumDataset, Frame* pPrevF = nullptr, const IMU::Calib &ImuCalib = IMU::Calib());
 
     // Constructor for Monocular cameras.
-    Frame(const cv::Mat &imGray, const double &timeStamp, ORBextractor* extractor,ORBVocabulary* voc, GeometricCamera* pCamera, cv::Mat &distCoef, const float &bf, const float &thDepth, const std::string &pNameFile, int pnNumDataset,  Frame* pPrevF = static_cast<Frame*>(NULL), const IMU::Calib &ImuCalib = IMU::Calib());
+    Frame(const cv::Mat &imGray, const double &timeStamp, ORBextractor* extractor,ORBVocabulary* voc, GeometricCamera* pCamera, cv::Mat &distCoef, const float &bf, const float &thDepth, const std::string &pNameFile, int pnNumDataset,  Frame* pPrevF = nullptr, const IMU::Calib &ImuCalib = IMU::Calib());
 
     // Destructor
     virtual ~Frame();
@@ -342,7 +342,7 @@ class Frame
     //Grid for the right image
     std::vector<std::size_t> mGridRight[FRAME_GRID_COLS][FRAME_GRID_ROWS];
 
-    Frame(const cv::Mat &imLeft, const cv::Mat &imRight, const double &timeStamp, ORBextractor* extractorLeft, ORBextractor* extractorRight, ORBVocabulary* voc, cv::Mat &K, cv::Mat &distCoef, const float &bf, const float &thDepth, GeometricCamera* pCamera, GeometricCamera* pCamera2, const std::string &pNameFile, int pnNumDataset, Sophus::SE3f& Tlr,Frame* pPrevF = static_cast<Frame*>(NULL), const IMU::Calib &ImuCalib = IMU::Calib());
+    Frame(const cv::Mat &imLeft, const cv::Mat &imRight, const double &timeStamp, ORBextractor* extractorLeft, ORBextractor* extractorRight, ORBVocabulary* voc, cv::Mat &K, cv::Mat &distCoef, const float &bf, const float &thDepth, GeometricCamera* pCamera, GeometricCamera* pCamera2, const std::string &pNameFile, int pnNumDataset, Sophus::SE3f& Tlr,Frame* pPrevF = nullptr, const IMU::Calib &ImuCalib = IMU::Calib());
 
     //Stereo fisheye
     void ComputeStereoFishEyeMatches();
diff --git a/src/Atlas.cc b/src/Atlas.cc
index cad1631a..18601312 100644
--- a/src/Atlas.cc
+++ b/src/Atlas.cc
@@ -27,10 +27,10 @@
 
 namespace ORB_SLAM3 {
 
-Atlas::Atlas() { mpCurrentMap = static_cast<Map*>(NULL); }
+Atlas::Atlas() { mpCurrentMap = nullptr; }
 
 Atlas::Atlas(int initKFid) : mnLastInitKFidMap(initKFid) {
-  mpCurrentMap = static_cast<Map*>(NULL);
+  mpCurrentMap = nullptr;
   CreateNewMap();
 }
 
@@ -42,7 +42,7 @@ Atlas::~Atlas() {
 
     if (pMi) {
       delete pMi;
-      pMi = static_cast<Map*>(NULL);
+      pMi = nullptr;
 
       it = mspMaps.erase(it);
     } else
@@ -198,7 +198,7 @@ void Atlas::clearAtlas() {
       delete *it;
   }*/
   mspMaps.clear();
-  mpCurrentMap = static_cast<Map*>(NULL);
+  mpCurrentMap = nullptr;
   mnLastInitKFidMap = 0;
 }
 
@@ -223,7 +223,7 @@ void Atlas::RemoveBadMaps() {
   /*for(Map* pMap : mspBadMaps)
   {
       delete pMap;
-      pMap = static_cast<Map*>(NULL);
+      pMap = nullptr;
   }*/
   mspBadMaps.clear();
 }
diff --git a/src/Frame.cc b/src/Frame.cc
index 2b7d8c04..32b03f0e 100644
--- a/src/Frame.cc
+++ b/src/Frame.cc
@@ -52,7 +52,7 @@ Frame::Frame()
       mpImuPreintegrated(NULL),
       mpPrevFrame(NULL),
       mpImuPreintegratedFrame(NULL),
-      mpReferenceKF(static_cast<KeyFrame *>(NULL)),
+      mpReferenceKF(nullptr),
       mbIsSet(false),
       mbImuPreintegrated(false) {
 #ifdef REGISTER_TIMES
@@ -169,7 +169,7 @@ Frame::Frame(const cv::Mat &imLeft, const cv::Mat &imRight,
       mpImuPreintegrated(NULL),
       mpPrevFrame(pPrevF),
       mpImuPreintegratedFrame(NULL),
-      mpReferenceKF(static_cast<KeyFrame *>(NULL)),
+      mpReferenceKF(nullptr),
       mNameFile{pNameFile},
       mnDataset{pnNumDataset},
       mbIsSet(false),
@@ -265,9 +265,9 @@ Frame::Frame(const cv::Mat &imLeft, const cv::Mat &imRight,
   // Set no stereo fisheye information
   Nleft = -1;
   Nright = -1;
-  mvLeftToRightMatch = vector<int>(0);
-  mvRightToLeftMatch = vector<int>(0);
-  mvStereo3Dpoints = vector<Eigen::Vector3f>(0);
+  // mvLeftToRightMatch = vector<int>(0);
+  // mvRightToLeftMatch = vector<int>(0);
+  // mvStereo3Dpoints = vector<Eigen::Vector3f>(0);
   monoLeft = -1;
   monoRight = -1;
 
@@ -289,7 +289,7 @@ Frame::Frame(const cv::Mat &imGray, const cv::Mat &imDepth,
       mbHasVelocity(false),
       mpORBvocabulary(voc),
       mpORBextractorLeft(extractor),
-      mpORBextractorRight(static_cast<ORBextractor *>(NULL)),
+      mpORBextractorRight(nullptr),
       mTimeStamp(timeStamp),
       mK(K.clone()),
       mK_(Converter::toMatrix3f(K)),
@@ -300,7 +300,7 @@ Frame::Frame(const cv::Mat &imGray, const cv::Mat &imDepth,
       mpImuPreintegrated(NULL),
       mpPrevFrame(pPrevF),
       mpImuPreintegratedFrame(NULL),
-      mpReferenceKF(static_cast<KeyFrame *>(NULL)),
+      mpReferenceKF(nullptr),
       mNameFile{pNameFile},
       mnDataset{pnNumDataset},
       mbIsSet(false),
@@ -362,7 +362,7 @@ Frame::Frame(const cv::Mat &imGray, const cv::Mat &imDepth,
 
   ComputeStereoFromRGBD(imDepth);
 
-  mvpMapPoints = vector<MapPoint *>(N, static_cast<MapPoint *>(NULL));
+  mvpMapPoints = vector<MapPoint *>(N, nullptr);
 
   mmProjectPoints.clear();
   mmMatchedInImage.clear();
@@ -401,7 +401,7 @@ Frame::Frame(const cv::Mat &imGray, const double &timeStamp,
       mbHasVelocity(false),
       mpORBvocabulary(voc),
       mpORBextractorLeft(extractor),
-      mpORBextractorRight(static_cast<ORBextractor *>(NULL)),
+      mpORBextractorRight(nullptr),
       mTimeStamp(timeStamp),
       mK(static_cast<Pinhole *>(pCamera)->toK()),
       mK_(static_cast<Pinhole *>(pCamera)->toK_()),
@@ -412,7 +412,7 @@ Frame::Frame(const cv::Mat &imGray, const double &timeStamp,
       mpImuPreintegrated(NULL),
       mpPrevFrame(pPrevF),
       mpImuPreintegratedFrame(NULL),
-      mpReferenceKF(static_cast<KeyFrame *>(NULL)),
+      mpReferenceKF(nullptr),
       mNameFile{pNameFile},
       mnDataset{pnNumDataset},
       mbIsSet(false),
@@ -477,7 +477,7 @@ Frame::Frame(const cv::Mat &imGray, const double &timeStamp,
   mvDepth = vector<float>(N, -1);
   mnCloseMPs = 0;
 
-  mvpMapPoints = vector<MapPoint *>(N, static_cast<MapPoint *>(NULL));
+  mvpMapPoints = vector<MapPoint *>(N, nullptr);
 
   mmProjectPoints.clear();  // = map<long unsigned int, cv::Point2f>(N,
                             // static_cast<cv::Point2f>(NULL));
@@ -1124,7 +1124,7 @@ Frame::Frame(const cv::Mat &imLeft, const cv::Mat &imRight,
       mpImuPreintegrated(NULL),
       mpPrevFrame(pPrevF),
       mpImuPreintegratedFrame(NULL),
-      mpReferenceKF(static_cast<KeyFrame *>(NULL)),
+      mpReferenceKF(nullptr),
       mNameFile{pNameFile},
       mnDataset{pnNumDataset},
       mbImuPreintegrated(false),
diff --git a/src/KeyFrame.cc b/src/KeyFrame.cc
index cb9800c7..f42fdf62 100644
--- a/src/KeyFrame.cc
+++ b/src/KeyFrame.cc
@@ -79,8 +79,8 @@ KeyFrame::KeyFrame()
       mnMinY(0),
       mnMaxX(0),
       mnMaxY(0),
-      mPrevKF(static_cast<KeyFrame *>(NULL)),
-      mNextKF(static_cast<KeyFrame *>(NULL)),
+      mPrevKF(nullptr),
+      mNextKF(nullptr),
       mbHasVelocity(false),
       mbFirstConnection(true),
       mpParent(NULL),
@@ -384,15 +384,15 @@ void KeyFrame::AddMapPoint(MapPoint *pMP, const size_t &idx) {
 
 void KeyFrame::EraseMapPointMatch(const int &idx) {
   unique_lock<mutex> lock(mMutexFeatures);
-  mvpMapPoints[idx] = static_cast<MapPoint *>(NULL);
+  mvpMapPoints[idx] = nullptr;
 }
 
 void KeyFrame::EraseMapPointMatch(MapPoint *pMP) {
   tuple<int, int> indexes = pMP->GetIndexInKeyFrame(this);
   int leftIndex = get<0>(indexes), rightIndex = get<1>(indexes);
-  if (leftIndex != -1) mvpMapPoints[leftIndex] = static_cast<MapPoint *>(NULL);
+  if (leftIndex != -1) mvpMapPoints[leftIndex] = nullptr;
   if (rightIndex != -1)
-    mvpMapPoints[rightIndex] = static_cast<MapPoint *>(NULL);
+    mvpMapPoints[rightIndex] = nullptr;
 }
 
 void KeyFrame::ReplaceMapPointMatch(const int &idx, MapPoint *pMP) {
@@ -948,7 +948,7 @@ void KeyFrame::PostLoad(map<long unsigned int, KeyFrame *> &mpKFid,
     if (mvBackupMapPointsId[i] != -1)
       mvpMapPoints[i] = mpMPid[mvBackupMapPointsId[i]];
     else
-      mvpMapPoints[i] = static_cast<MapPoint *>(NULL);
+      mvpMapPoints[i] = nullptr;
   }
 
   // Conected KeyFrames with him weight
diff --git a/src/LoopClosing.cc b/src/LoopClosing.cc
index a0ce746f..20ce1ee0 100644
--- a/src/LoopClosing.cc
+++ b/src/LoopClosing.cc
@@ -51,7 +51,7 @@ LoopClosing::LoopClosing(const Atlas_ptr &pAtlas, KeyFrameDatabase* pDB,
       mpKeyFrameDB(pDB),
       mpORBVocabulary(pVoc),
       mnCovisibilityConsistencyTh(3),
-      mpLastCurrentKF(static_cast<KeyFrame*>(NULL)),
+      mpLastCurrentKF(nullptr),
       mpMatchedKF(NULL),
       mbLoopDetected(false),
       mnLoopNumCoincidences(0),
@@ -574,7 +574,7 @@ bool LoopClosing::DetectAndReffineSim3FromLastKF(
 
       vector<MapPoint*> vpMatchedMP;
       vpMatchedMP.resize(mpCurrentKF->GetMapPointMatches().size(),
-                         static_cast<MapPoint*>(NULL));
+                         nullptr);
 
       nNumProjMatches =
           FindMatchesByProjection(pCurrentKF, pMatchedKF, gScw_estimation,
@@ -661,9 +661,9 @@ bool LoopClosing::DetectCommonRegionsFromBoW(
     int nMostBoWNumMatches = 0;
 
     std::vector<MapPoint*> vpMatchedPoints = std::vector<MapPoint*>(
-        mpCurrentKF->GetMapPointMatches().size(), static_cast<MapPoint*>(NULL));
+        mpCurrentKF->GetMapPointMatches().size(), nullptr);
     std::vector<KeyFrame*> vpKeyFrameMatchedMP = std::vector<KeyFrame*>(
-        mpCurrentKF->GetMapPointMatches().size(), static_cast<KeyFrame*>(NULL));
+        mpCurrentKF->GetMapPointMatches().size(), nullptr);
 
     // int nIndexMostBoWMatchesKF=0; // UNUSED
     for (size_t j = 0; j < vpCovKFi.size(); ++j) {
@@ -766,10 +766,10 @@ bool LoopClosing::DetectCommonRegionsFromBoW(
 
         vector<MapPoint*> vpMatchedMP;
         vpMatchedMP.resize(mpCurrentKF->GetMapPointMatches().size(),
-                           static_cast<MapPoint*>(NULL));
+                           nullptr);
         vector<KeyFrame*> vpMatchedKF;
         vpMatchedKF.resize(mpCurrentKF->GetMapPointMatches().size(),
-                           static_cast<KeyFrame*>(NULL));
+                           nullptr);
         int numProjMatches = matcher.SearchByProjection(
             mpCurrentKF, mScw, vpMapPoints, vpKeyFrames, vpMatchedMP,
             vpMatchedKF, 8, 1.5);
@@ -801,7 +801,7 @@ bool LoopClosing::DetectCommonRegionsFromBoW(
 
             vector<MapPoint*> vpMatchedMP;
             vpMatchedMP.resize(mpCurrentKF->GetMapPointMatches().size(),
-                               static_cast<MapPoint*>(NULL));
+                               nullptr);
             int numProjOptMatches = matcher.SearchByProjection(
                 mpCurrentKF, mScw, vpMapPoints, vpMatchedMP, 5, 1.0);
 
@@ -970,7 +970,7 @@ int LoopClosing::FindMatchesByProjection(
   ORBmatcher matcher(0.9, true);
 
   vpMatchedMapPoints.resize(pCurrentKF->GetMapPointMatches().size(),
-                            static_cast<MapPoint*>(NULL));
+                            nullptr);
   int num_matches = matcher.SearchByProjection(pCurrentKF, mScw, vpMapPoints,
                                                vpMatchedMapPoints, 3, 1.5);
 
@@ -2169,7 +2169,7 @@ void LoopClosing::SearchAndFuse(const KeyFrameAndPose& CorrectedPosesMap,
     Sophus::Sim3f Scw = Converter::toSophus(g2oScw);
 
     vector<MapPoint*> vpReplacePoints(vpMapPoints.size(),
-                                      static_cast<MapPoint*>(NULL));
+                                      nullptr);
     /*int numFused = */matcher.Fuse(pKFi, Scw, vpMapPoints, 4, vpReplacePoints);
 
     // Get Map Mutex
@@ -2210,7 +2210,7 @@ void LoopClosing::SearchAndFuse(const vector<KeyFrame*>& vConectedKFs,
         Scw.translation() - Tcw.translation() << std::endl <<
         Scw.scale() - 1.f << std::endl;*/
     vector<MapPoint*> vpReplacePoints(vpMapPoints.size(),
-                                      static_cast<MapPoint*>(NULL));
+                                      nullptr);
     matcher.Fuse(pKF, Scw, vpMapPoints, 4, vpReplacePoints);
 
     // Get Map Mutex
diff --git a/src/Map.cc b/src/Map.cc
index 7259d344..92c7b87e 100644
--- a/src/Map.cc
+++ b/src/Map.cc
@@ -28,7 +28,7 @@ namespace ORB_SLAM3 {
 long unsigned int Map::nNextId = 0;
 
 Map::Map()
-    : mpFirstRegionKF(static_cast<KeyFrame*>(NULL)),
+    : mpFirstRegionKF(nullptr),
       mbFail(false),
       mbImuInitialized(false),
       mnMapChange(0),
@@ -42,11 +42,11 @@ Map::Map()
       mbIMU_BA1(false),
       mbIMU_BA2(false) {
   mnId = nNextId++;
-  mThumbnail = static_cast<GLubyte*>(NULL);
+  mThumbnail = nullptr;
 }
 
 Map::Map(int initKFid)
-    : mpFirstRegionKF(static_cast<KeyFrame*>(NULL)),
+    : mpFirstRegionKF(nullptr),
       mbFail(false),
       mbImuInitialized(false),
       mnMapChange(0),
@@ -61,7 +61,7 @@ Map::Map(int initKFid)
       mbIMU_BA1(false),
       mbIMU_BA2(false) {
   mnId = nNextId++;
-  mThumbnail = static_cast<GLubyte*>(NULL);
+  mThumbnail = nullptr;
 }
 
 Map::~Map() {
@@ -72,7 +72,7 @@ Map::~Map() {
   mspKeyFrames.clear();
 
   if (mThumbnail) delete mThumbnail;
-  mThumbnail = static_cast<GLubyte*>(NULL);
+  mThumbnail = nullptr;
 
   mvpReferenceMapPoints.clear();
   mvpKeyFrameOrigins.clear();
@@ -207,7 +207,7 @@ void Map::clear() {
                                 send = mspKeyFrames.end();
        sit != send; sit++) {
     KeyFrame* pKF = *sit;
-    pKF->UpdateMap(static_cast<Map*>(NULL));
+    pKF->UpdateMap(nullptr);
     //        delete *sit;
   }
 
diff --git a/src/MapPoint.cc b/src/MapPoint.cc
index 979ae419..756ddb07 100644
--- a/src/MapPoint.cc
+++ b/src/MapPoint.cc
@@ -45,8 +45,8 @@ MapPoint::MapPoint()
       mnVisible(1),
       mnFound(1),
       mbBad(false),
-      mpReplaced(static_cast<MapPoint*>(NULL)) {
-  mpReplaced = static_cast<MapPoint*>(NULL);
+      mpReplaced(nullptr) {
+  mpReplaced = nullptr;
 }
 
 MapPoint::MapPoint(const Eigen::Vector3f& Pos, KeyFrame* pRefKF, Map* pMap)
@@ -66,7 +66,7 @@ MapPoint::MapPoint(const Eigen::Vector3f& Pos, KeyFrame* pRefKF, Map* pMap)
       mnVisible(1),
       mnFound(1),
       mbBad(false),
-      mpReplaced(static_cast<MapPoint*>(NULL)),
+      mpReplaced(nullptr),
       mfMinDistance(0),
       mfMaxDistance(0),
       mpMap(pMap) {
@@ -101,7 +101,7 @@ MapPoint::MapPoint(const double invDepth, cv::Point2f uv_init, KeyFrame* pRefKF,
       mnVisible(1),
       mnFound(1),
       mbBad(false),
-      mpReplaced(static_cast<MapPoint*>(NULL)),
+      mpReplaced(nullptr),
       mfMinDistance(0),
       mfMaxDistance(0),
       mpMap(pMap) {
@@ -133,7 +133,7 @@ MapPoint::MapPoint(const Eigen::Vector3f& Pos, Map* pMap, Frame* pFrame,
       mnCorrectedReference(0),
       mnBAGlobalForKF(0),
       mnOriginMapId(pMap->GetId()),
-      mpRefKF(static_cast<KeyFrame*>(NULL)),
+      mpRefKF(nullptr),
       mnVisible(1),
       mnFound(1),
       mbBad(false),
@@ -628,7 +628,7 @@ void MapPoint::PostLoad(map<long unsigned int, KeyFrame*>& mpKFid,
     cout << "ERROR: MP without KF reference " << mBackupRefKFId
          << "; Num obs: " << nObs << endl;
   }
-  mpReplaced = static_cast<MapPoint*>(NULL);
+  mpReplaced = nullptr;
   if (mBackupReplacedId >= 0) {
     map<long unsigned int, MapPoint*>::iterator it =
         mpMPid.find(mBackupReplacedId);
diff --git a/src/ORBmatcher.cc b/src/ORBmatcher.cc
index 12e0990c..67c9618b 100644
--- a/src/ORBmatcher.cc
+++ b/src/ORBmatcher.cc
@@ -219,7 +219,7 @@ int ORBmatcher::SearchByBoW(KeyFrame *pKF, Frame &F,
                             vector<MapPoint *> &vpMapPointMatches) {
   const vector<MapPoint *> vpMapPointsKF = pKF->GetMapPointMatches();
 
-  vpMapPointMatches = vector<MapPoint *>(F.N, static_cast<MapPoint *>(NULL));
+  vpMapPointMatches = vector<MapPoint *>(F.N, nullptr);
 
   const DBoW2::FeatureVector &vFeatVecKF = pKF->mFeatVec;
 
@@ -385,7 +385,7 @@ int ORBmatcher::SearchByBoW(KeyFrame *pKF, Frame &F,
     for (int i = 0; i < HISTO_LENGTH; i++) {
       if (i == ind1 || i == ind2 || i == ind3) continue;
       for (size_t j = 0, jend = rotHist[i].size(); j < jend; j++) {
-        vpMapPointMatches[rotHist[i][j]] = static_cast<MapPoint *>(NULL);
+        vpMapPointMatches[rotHist[i][j]] = nullptr;
         nmatches--;
       }
     }
@@ -410,7 +410,7 @@ int ORBmatcher::SearchByProjection(KeyFrame *pKF, Sophus::Sim3f &Scw,
 
   // Set of MapPoints already found in the KeyFrame
   set<MapPoint *> spAlreadyFound(vpMatched.begin(), vpMatched.end());
-  spAlreadyFound.erase(static_cast<MapPoint *>(NULL));
+  spAlreadyFound.erase(nullptr);
 
   int nmatches = 0;
 
@@ -511,7 +511,7 @@ int ORBmatcher::SearchByProjection(KeyFrame *pKF, Sophus::Sim3<float> &Scw,
 
   // Set of MapPoints already found in the KeyFrame
   set<MapPoint *> spAlreadyFound(vpMatched.begin(), vpMatched.end());
-  spAlreadyFound.erase(static_cast<MapPoint *>(NULL));
+  spAlreadyFound.erase(nullptr);
 
   int nmatches = 0;
 
@@ -712,7 +712,7 @@ int ORBmatcher::SearchByBoW(KeyFrame *pKF1, KeyFrame *pKF2,
   const cv::Mat &Descriptors2 = pKF2->mDescriptors;
 
   vpMatches12 =
-      vector<MapPoint *>(vpMapPoints1.size(), static_cast<MapPoint *>(NULL));
+      vector<MapPoint *>(vpMapPoints1.size(), nullptr);
   vector<bool> vbMatched2(vpMapPoints2.size(), false);
 
   vector<int> rotHist[HISTO_LENGTH];
@@ -809,7 +809,7 @@ int ORBmatcher::SearchByBoW(KeyFrame *pKF1, KeyFrame *pKF2,
     for (int i = 0; i < HISTO_LENGTH; i++) {
       if (i == ind1 || i == ind2 || i == ind3) continue;
       for (size_t j = 0, jend = rotHist[i].size(); j < jend; j++) {
-        vpMatches12[rotHist[i][j]] = static_cast<MapPoint *>(NULL);
+        vpMatches12[rotHist[i][j]] = nullptr;
         nmatches--;
       }
     }
@@ -1722,7 +1722,7 @@ int ORBmatcher::SearchByProjection(Frame &CurrentFrame, const Frame &LastFrame,
       if (i != ind1 && i != ind2 && i != ind3) {
         for (size_t j = 0, jend = rotHist[i].size(); j < jend; j++) {
           CurrentFrame.mvpMapPoints[rotHist[i][j]] =
-              static_cast<MapPoint *>(NULL);
+              nullptr;
           nmatches--;
         }
       }
diff --git a/src/Optimizer.cc b/src/Optimizer.cc
index da4c8be0..2881faf9 100644
--- a/src/Optimizer.cc
+++ b/src/Optimizer.cc
@@ -2265,7 +2265,7 @@ int Optimizer::OptimizeSim3(KeyFrame* pKF1, KeyFrame* pKF2,
 
     if (e12->chi2() > th2 || e21->chi2() > th2) {
       size_t idx = vnIndexEdge[i];
-      vpMatches1[idx] = static_cast<MapPoint*>(NULL);
+      vpMatches1[idx] = nullptr;
       optimizer.removeEdge(e12);
       optimizer.removeEdge(e21);
       vpEdges12[i] = static_cast<ORB_SLAM3::EdgeSim3ProjectXYZ*>(NULL);
@@ -2307,7 +2307,7 @@ int Optimizer::OptimizeSim3(KeyFrame* pKF1, KeyFrame* pKF2,
 
     if (e12->chi2() > th2 || e21->chi2() > th2) {
       size_t idx = vnIndexEdge[i];
-      vpMatches1[idx] = static_cast<MapPoint*>(NULL);
+      vpMatches1[idx] = nullptr;
     } else {
       nIn++;
     }
@@ -5291,7 +5291,7 @@ void Optimizer::OptimizeEssentialGraph4DoF(
       Siw = vScw[nIDi];
 
     // 1.1.0 Spanning tree edge
-    KeyFrame* pParentKF = static_cast<KeyFrame*>(NULL);
+    KeyFrame* pParentKF = nullptr;
     if (pParentKF) {
       int nIDj = pParentKF->mnId;
 
diff --git a/src/Tracking.cc b/src/Tracking.cc
index 7b7f30a8..e89d102c 100644
--- a/src/Tracking.cc
+++ b/src/Tracking.cc
@@ -55,7 +55,7 @@ Tracking::Tracking(System* pSys, ORBVocabulary* pVoc, const Atlas_ptr &pAtlas,
       mbReadyToInitializate(false),
       mpSystem(pSys),
       mpAtlas(pAtlas),
-      mpLastKeyFrame(static_cast<KeyFrame*>(NULL)),
+      mpLastKeyFrame(nullptr),
       mnLastRelocFrameId(0),
       time_recently_lost(5.0),
       mnFirstFrameId(0),
@@ -1847,7 +1847,7 @@ void Tracking::Track() {
           } else
             CreateMapInAtlas();
 
-          if (mpLastKeyFrame) mpLastKeyFrame = static_cast<KeyFrame*>(NULL);
+          if (mpLastKeyFrame) mpLastKeyFrame = nullptr;
 
           Verbose::PrintMess("done", Verbose::VERBOSITY_NORMAL);
 
@@ -2032,7 +2032,7 @@ void Tracking::Track() {
         if (pMP)
           if (pMP->Observations() < 1) {
             mCurrentFrame.mvbOutlier[i] = false;
-            mCurrentFrame.mvpMapPoints[i] = static_cast<MapPoint*>(NULL);
+            mCurrentFrame.mvpMapPoints[i] = nullptr;
           }
       }
 
@@ -2075,7 +2075,7 @@ void Tracking::Track() {
       // frame. Only has effect if lastframe is tracked
       for (int i = 0; i < mCurrentFrame.N; i++) {
         if (mCurrentFrame.mvpMapPoints[i] && mCurrentFrame.mvbOutlier[i])
-          mCurrentFrame.mvpMapPoints[i] = static_cast<MapPoint*>(NULL);
+          mCurrentFrame.mvpMapPoints[i] = nullptr;
       }
     }
 
@@ -2475,9 +2475,9 @@ void Tracking::CreateMapInAtlas() {
         new IMU::Preintegrated(IMU::Bias(), *mpImuCalib);
   }
 
-  if (mpLastKeyFrame) mpLastKeyFrame = static_cast<KeyFrame*>(NULL);
+  if (mpLastKeyFrame) mpLastKeyFrame = nullptr;
 
-  if (mpReferenceKF) mpReferenceKF = static_cast<KeyFrame*>(NULL);
+  if (mpReferenceKF) mpReferenceKF = nullptr;
 
   mLastFrame = Frame();
   mCurrentFrame = Frame();
@@ -2533,7 +2533,7 @@ bool Tracking::TrackReferenceKeyFrame() {
       if (mCurrentFrame.mvbOutlier[i]) {
         MapPoint* pMP = mCurrentFrame.mvpMapPoints[i];
 
-        mCurrentFrame.mvpMapPoints[i] = static_cast<MapPoint*>(NULL);
+        mCurrentFrame.mvpMapPoints[i] = nullptr;
         mCurrentFrame.mvbOutlier[i] = false;
         if (i < mCurrentFrame.Nleft) {
           pMP->mbTrackInView = false;
@@ -2635,7 +2635,7 @@ bool Tracking::TrackWithMotionModel() {
   }
 
   fill(mCurrentFrame.mvpMapPoints.begin(), mCurrentFrame.mvpMapPoints.end(),
-       static_cast<MapPoint*>(NULL));
+       nullptr);
 
   // Project points seen in previous frame
   int th;
@@ -2654,7 +2654,7 @@ bool Tracking::TrackWithMotionModel() {
     Verbose::PrintMess("Not enough matches, wider window search!!",
                        Verbose::VERBOSITY_NORMAL);
     fill(mCurrentFrame.mvpMapPoints.begin(), mCurrentFrame.mvpMapPoints.end(),
-         static_cast<MapPoint*>(NULL));
+         nullptr);
 
     nmatches = matcher.SearchByProjection(
         mCurrentFrame, mLastFrame, 2 * th,
@@ -2681,7 +2681,7 @@ bool Tracking::TrackWithMotionModel() {
       if (mCurrentFrame.mvbOutlier[i]) {
         MapPoint* pMP = mCurrentFrame.mvpMapPoints[i];
 
-        mCurrentFrame.mvpMapPoints[i] = static_cast<MapPoint*>(NULL);
+        mCurrentFrame.mvpMapPoints[i] = nullptr;
         mCurrentFrame.mvbOutlier[i] = false;
         if (i < mCurrentFrame.Nleft) {
           pMP->mbTrackInView = false;
@@ -2769,7 +2769,7 @@ bool Tracking::TrackLocalMap() {
         } else
           mnMatchesInliers++;
       } else if (mSensor == CameraType::STEREO)
-        mCurrentFrame.mvpMapPoints[i] = static_cast<MapPoint*>(NULL);
+        mCurrentFrame.mvpMapPoints[i] = nullptr;
     }
   }
 
@@ -3020,7 +3020,7 @@ void Tracking::CreateNewKeyFrame() {
           bCreateNew = true;
         else if (pMP->Observations() < 1) {
           bCreateNew = true;
-          mCurrentFrame.mvpMapPoints[i] = static_cast<MapPoint*>(NULL);
+          mCurrentFrame.mvpMapPoints[i] = nullptr;
         }
 
         if (bCreateNew) {
@@ -3084,7 +3084,7 @@ void Tracking::SearchLocalPoints() {
     MapPoint* pMP = *vit;
     if (pMP) {
       if (pMP->isBad()) {
-        *vit = static_cast<MapPoint*>(NULL);
+        *vit = nullptr;
       } else {
         pMP->IncreaseVisible();
         pMP->mnLastFrameSeen = mCurrentFrame.mnId;
@@ -3221,7 +3221,7 @@ void Tracking::UpdateLocalKeyFrames() {
   }
 
   int max = 0;
-  KeyFrame* pKFmax = static_cast<KeyFrame*>(NULL);
+  KeyFrame* pKFmax = nullptr;
 
   mvpLocalKeyFrames.clear();
   mvpLocalKeyFrames.reserve(3 * keyframeCounter.size());
@@ -3420,7 +3420,7 @@ bool Tracking::Relocalization() {
 
         for (int io = 0; io < mCurrentFrame.N; io++)
           if (mCurrentFrame.mvbOutlier[io])
-            mCurrentFrame.mvpMapPoints[io] = static_cast<MapPoint*>(NULL);
+            mCurrentFrame.mvpMapPoints[io] = nullptr;
 
         // If few inliers, search by projection in a coarse window and optimize
         // again
@@ -3513,8 +3513,8 @@ void Tracking::Reset(bool bLocMap) {
   mCurrentFrame = Frame();
   mnLastRelocFrameId = 0;
   mLastFrame = Frame();
-  mpReferenceKF = static_cast<KeyFrame*>(NULL);
-  mpLastKeyFrame = static_cast<KeyFrame*>(NULL);
+  mpReferenceKF = nullptr;
+  mpLastKeyFrame = nullptr;
   mvIniMatches.clear();
 
   Verbose::PrintMess("   End reseting! ", Verbose::VERBOSITY_NORMAL);
@@ -3587,8 +3587,8 @@ void Tracking::ResetActiveMap(bool bLocMap) {
 
   mCurrentFrame = Frame();
   mLastFrame = Frame();
-  mpReferenceKF = static_cast<KeyFrame*>(NULL);
-  mpLastKeyFrame = static_cast<KeyFrame*>(NULL);
+  mpReferenceKF = nullptr;
+  mpLastKeyFrame = nullptr;
   mvIniMatches.clear();
 
   mbVelocity = false;

From dc7147e550e802668fb1cd5e7487e16f193392e9 Mon Sep 17 00:00:00 2001
From: DavidPetkovsek <david.petkovsek@martinrea.com>
Date: Tue, 26 Jul 2022 12:52:16 -0400
Subject: [PATCH 11/26] NULL to nullptr

---
 .../Calibration/recorder_realsense_D435i.cc   |  2 +-
 .../Calibration/recorder_realsense_T265.cc    |  2 +-
 .../mono_inertial_realsense_D435i.cc          |  2 +-
 .../mono_inertial_realsense_t265.cc           |  2 +-
 Examples/Monocular/mono_realsense_D435i.cc    |  2 +-
 Examples/Monocular/mono_realsense_t265.cc     |  2 +-
 .../rgbd_inertial_realsense_D435i.cc          |  2 +-
 Examples/RGB-D/rgbd_realsense_D435i.cc        |  2 +-
 .../stereo_inertial_realsense_D435i.cc        |  2 +-
 .../stereo_inertial_realsense_t265.cc         |  2 +-
 Examples/Stereo/stereo_realsense_D435i.cc     |  2 +-
 Examples/Stereo/stereo_realsense_t265.cc      |  2 +-
 Thirdparty/DBoW2/DBoW2/TemplatedVocabulary.h  | 16 ++++-----
 .../Sophus/test/ceres/test_ceres_se3.cpp      |  4 +--
 .../g2o/g2o/core/estimate_propagator.cpp      |  4 +--
 Thirdparty/g2o/g2o/core/optimizable_graph.h   |  2 +-
 .../g2o/g2o/core/sparse_block_matrix.hpp      |  4 +--
 .../g2o/g2o/core/sparse_block_matrix_ccs.h    |  4 +--
 Thirdparty/g2o/g2o/stuff/os_specific.c        |  4 +--
 Thirdparty/g2o/g2o/stuff/string_tools.cpp     |  4 +--
 include/Frame.h                               |  2 +-
 include/Optimizer.h                           | 10 +++---
 src/Frame.cc                                  | 34 +++++++++----------
 src/KeyFrame.cc                               | 18 +++++-----
 src/LocalMapping.cc                           | 12 +++----
 src/LoopClosing.cc                            |  4 +--
 src/MapPoint.cc                               |  2 +-
 src/ORBmatcher.cc                             |  2 +-
 src/Optimizer.cc                              | 22 ++++++------
 src/Tracking.cc                               | 10 +++---
 30 files changed, 91 insertions(+), 91 deletions(-)

diff --git a/Examples/Calibration/recorder_realsense_D435i.cc b/Examples/Calibration/recorder_realsense_D435i.cc
index ac292ae4..763e2478 100644
--- a/Examples/Calibration/recorder_realsense_D435i.cc
+++ b/Examples/Calibration/recorder_realsense_D435i.cc
@@ -103,7 +103,7 @@ int main(int argc, char **argv) {
     sigemptyset(&sigIntHandler.sa_mask);
     sigIntHandler.sa_flags = 0;
 
-    sigaction(SIGINT, &sigIntHandler, NULL);
+    sigaction(SIGINT, &sigIntHandler, nullptr);
     b_continue_session = true;
 
     double offset = 0; // ms
diff --git a/Examples/Calibration/recorder_realsense_T265.cc b/Examples/Calibration/recorder_realsense_T265.cc
index 352b070a..000857f1 100644
--- a/Examples/Calibration/recorder_realsense_T265.cc
+++ b/Examples/Calibration/recorder_realsense_T265.cc
@@ -110,7 +110,7 @@ int main(int argc, char **argv) {
     sigemptyset(&sigIntHandler.sa_mask);
     sigIntHandler.sa_flags = 0;
 
-    sigaction(SIGINT, &sigIntHandler, NULL);
+    sigaction(SIGINT, &sigIntHandler, nullptr);
     b_continue_session = true;
 
     double offset = 0; // ms
diff --git a/Examples/Monocular-Inertial/mono_inertial_realsense_D435i.cc b/Examples/Monocular-Inertial/mono_inertial_realsense_D435i.cc
index 955730c8..63d13e2f 100644
--- a/Examples/Monocular-Inertial/mono_inertial_realsense_D435i.cc
+++ b/Examples/Monocular-Inertial/mono_inertial_realsense_D435i.cc
@@ -116,7 +116,7 @@ int main(int argc, char **argv) {
     sigemptyset(&sigIntHandler.sa_mask);
     sigIntHandler.sa_flags = 0;
 
-    sigaction(SIGINT, &sigIntHandler, NULL);
+    sigaction(SIGINT, &sigIntHandler, nullptr);
     b_continue_session = true;
 
     double offset = 0; // ms
diff --git a/Examples/Monocular-Inertial/mono_inertial_realsense_t265.cc b/Examples/Monocular-Inertial/mono_inertial_realsense_t265.cc
index 4a11630a..71addae1 100644
--- a/Examples/Monocular-Inertial/mono_inertial_realsense_t265.cc
+++ b/Examples/Monocular-Inertial/mono_inertial_realsense_t265.cc
@@ -75,7 +75,7 @@ int main(int argc, char **argv)
     sigemptyset(&sigIntHandler.sa_mask);
     sigIntHandler.sa_flags = 0;
 
-    sigaction(SIGINT, &sigIntHandler, NULL);
+    sigaction(SIGINT, &sigIntHandler, nullptr);
     b_continue_session = true;
 
     double offset = 0; // ms
diff --git a/Examples/Monocular/mono_realsense_D435i.cc b/Examples/Monocular/mono_realsense_D435i.cc
index 8f7e3bca..422cd62c 100644
--- a/Examples/Monocular/mono_realsense_D435i.cc
+++ b/Examples/Monocular/mono_realsense_D435i.cc
@@ -112,7 +112,7 @@ int main(int argc, char **argv) {
     sigemptyset(&sigIntHandler.sa_mask);
     sigIntHandler.sa_flags = 0;
 
-    sigaction(SIGINT, &sigIntHandler, NULL);
+    sigaction(SIGINT, &sigIntHandler, nullptr);
     b_continue_session = true;
 
     // double offset = 0; // UNUSED // ms
diff --git a/Examples/Monocular/mono_realsense_t265.cc b/Examples/Monocular/mono_realsense_t265.cc
index f784356d..39007c30 100644
--- a/Examples/Monocular/mono_realsense_t265.cc
+++ b/Examples/Monocular/mono_realsense_t265.cc
@@ -66,7 +66,7 @@ int main(int argc, char **argv)
     sigemptyset(&sigIntHandler.sa_mask);
     sigIntHandler.sa_flags = 0;
 
-    sigaction(SIGINT, &sigIntHandler, NULL);
+    sigaction(SIGINT, &sigIntHandler, nullptr);
     b_continue_session = true;
 
 
diff --git a/Examples/RGB-D-Inertial/rgbd_inertial_realsense_D435i.cc b/Examples/RGB-D-Inertial/rgbd_inertial_realsense_D435i.cc
index 90ff063c..68e6adbf 100644
--- a/Examples/RGB-D-Inertial/rgbd_inertial_realsense_D435i.cc
+++ b/Examples/RGB-D-Inertial/rgbd_inertial_realsense_D435i.cc
@@ -120,7 +120,7 @@ int main(int argc, char **argv) {
     sigemptyset(&sigIntHandler.sa_mask);
     sigIntHandler.sa_flags = 0;
 
-    sigaction(SIGINT, &sigIntHandler, NULL);
+    sigaction(SIGINT, &sigIntHandler, nullptr);
     b_continue_session = true;
 
     double offset = 0; // ms
diff --git a/Examples/RGB-D/rgbd_realsense_D435i.cc b/Examples/RGB-D/rgbd_realsense_D435i.cc
index f7b742b9..e2d9d322 100644
--- a/Examples/RGB-D/rgbd_realsense_D435i.cc
+++ b/Examples/RGB-D/rgbd_realsense_D435i.cc
@@ -122,7 +122,7 @@ int main(int argc, char **argv) {
     sigemptyset(&sigIntHandler.sa_mask);
     sigIntHandler.sa_flags = 0;
 
-    sigaction(SIGINT, &sigIntHandler, NULL);
+    sigaction(SIGINT, &sigIntHandler, nullptr);
     b_continue_session = true;
 
     // double offset = 0; // UNUSED // ms
diff --git a/Examples/Stereo-Inertial/stereo_inertial_realsense_D435i.cc b/Examples/Stereo-Inertial/stereo_inertial_realsense_D435i.cc
index 58b3ab87..f7ab875c 100644
--- a/Examples/Stereo-Inertial/stereo_inertial_realsense_D435i.cc
+++ b/Examples/Stereo-Inertial/stereo_inertial_realsense_D435i.cc
@@ -116,7 +116,7 @@ int main(int argc, char **argv) {
     sigemptyset(&sigIntHandler.sa_mask);
     sigIntHandler.sa_flags = 0;
 
-    sigaction(SIGINT, &sigIntHandler, NULL);
+    sigaction(SIGINT, &sigIntHandler, nullptr);
     b_continue_session = true;
 
     double offset = 0; // ms
diff --git a/Examples/Stereo-Inertial/stereo_inertial_realsense_t265.cc b/Examples/Stereo-Inertial/stereo_inertial_realsense_t265.cc
index f00b1f40..a3e2fa99 100644
--- a/Examples/Stereo-Inertial/stereo_inertial_realsense_t265.cc
+++ b/Examples/Stereo-Inertial/stereo_inertial_realsense_t265.cc
@@ -77,7 +77,7 @@ int main(int argc, char **argv)
     sigemptyset(&sigIntHandler.sa_mask);
     sigIntHandler.sa_flags = 0;
 
-    sigaction(SIGINT, &sigIntHandler, NULL);
+    sigaction(SIGINT, &sigIntHandler, nullptr);
     b_continue_session = true;
 
     double offset = 0; // ms
diff --git a/Examples/Stereo/stereo_realsense_D435i.cc b/Examples/Stereo/stereo_realsense_D435i.cc
index bd8f9adc..c39d430c 100644
--- a/Examples/Stereo/stereo_realsense_D435i.cc
+++ b/Examples/Stereo/stereo_realsense_D435i.cc
@@ -110,7 +110,7 @@ int main(int argc, char** argv) {
   sigemptyset(&sigIntHandler.sa_mask);
   sigIntHandler.sa_flags = 0;
 
-  sigaction(SIGINT, &sigIntHandler, NULL);
+  sigaction(SIGINT, &sigIntHandler, nullptr);
   b_continue_session = true;
 
   // double offset = 0;  // UNUSED // ms
diff --git a/Examples/Stereo/stereo_realsense_t265.cc b/Examples/Stereo/stereo_realsense_t265.cc
index 1862ed21..b36d4c0b 100644
--- a/Examples/Stereo/stereo_realsense_t265.cc
+++ b/Examples/Stereo/stereo_realsense_t265.cc
@@ -66,7 +66,7 @@ int main(int argc, char **argv)
     sigemptyset(&sigIntHandler.sa_mask);
     sigIntHandler.sa_flags = 0;
 
-    sigaction(SIGINT, &sigIntHandler, NULL);
+    sigaction(SIGINT, &sigIntHandler, nullptr);
     b_continue_session = true;
 
 
diff --git a/Thirdparty/DBoW2/DBoW2/TemplatedVocabulary.h b/Thirdparty/DBoW2/DBoW2/TemplatedVocabulary.h
index 01959344..f1f95cd0 100644
--- a/Thirdparty/DBoW2/DBoW2/TemplatedVocabulary.h
+++ b/Thirdparty/DBoW2/DBoW2/TemplatedVocabulary.h
@@ -353,7 +353,7 @@ class TemplatedVocabulary
    * @param levelsup
    */
   virtual void transform(const TDescriptor &feature, 
-    WordId &id, WordValue &weight, NodeId* nid = NULL, int levelsup = 0) const;
+    WordId &id, WordValue &weight, NodeId* nid = nullptr, int levelsup = 0) const;
 
   /**
    * Returns the word id associated to a feature
@@ -434,7 +434,7 @@ template<class TDescriptor, class F>
 TemplatedVocabulary<TDescriptor,F>::TemplatedVocabulary
   (int k, int L, WeightingType weighting, ScoringType scoring)
   : m_k(k), m_L(L), m_weighting(weighting), m_scoring(scoring),
-  m_scoring_object(NULL)
+  m_scoring_object(nullptr)
 {
   createScoringObject();
 }
@@ -443,7 +443,7 @@ TemplatedVocabulary<TDescriptor,F>::TemplatedVocabulary
 
 template<class TDescriptor, class F>
 TemplatedVocabulary<TDescriptor,F>::TemplatedVocabulary
-  (const std::string &filename): m_scoring_object(NULL)
+  (const std::string &filename): m_scoring_object(nullptr)
 {
   load(filename);
 }
@@ -452,7 +452,7 @@ TemplatedVocabulary<TDescriptor,F>::TemplatedVocabulary
 
 template<class TDescriptor, class F>
 TemplatedVocabulary<TDescriptor,F>::TemplatedVocabulary
-  (const char *filename): m_scoring_object(NULL)
+  (const char *filename): m_scoring_object(nullptr)
 {
   load(filename);
 }
@@ -463,7 +463,7 @@ template<class TDescriptor, class F>
 void TemplatedVocabulary<TDescriptor,F>::createScoringObject()
 {
   delete m_scoring_object;
-  m_scoring_object = NULL;
+  m_scoring_object = nullptr;
   
   switch(m_scoring)
   {
@@ -516,7 +516,7 @@ void TemplatedVocabulary<TDescriptor,F>::setWeightingType(WeightingType type)
 template<class TDescriptor, class F>
 TemplatedVocabulary<TDescriptor,F>::TemplatedVocabulary(
   const TemplatedVocabulary<TDescriptor, F> &voc)
-  : m_scoring_object(NULL)
+  : m_scoring_object(nullptr)
 {
   *this = voc;
 }
@@ -1224,7 +1224,7 @@ void TemplatedVocabulary<TDescriptor,F>::transform(const TDescriptor &feature,
 
   // level at which the node must be stored in nid, if given
   const int nid_level = m_L - levelsup;
-  if(nid_level <= 0 && nid != NULL) *nid = 0; // root
+  if(nid_level <= 0 && nid != nullptr) *nid = 0; // root
 
   NodeId final_id = 0; // root
   int current_level = 0;
@@ -1248,7 +1248,7 @@ void TemplatedVocabulary<TDescriptor,F>::transform(const TDescriptor &feature,
       }
     }
     
-    if(nid != NULL && current_level == nid_level)
+    if(nid != nullptr && current_level == nid_level)
       *nid = final_id;
     
   } while( !m_nodes[final_id].isLeaf() );
diff --git a/Thirdparty/Sophus/test/ceres/test_ceres_se3.cpp b/Thirdparty/Sophus/test/ceres/test_ceres_se3.cpp
index 6304b74d..45aee1ef 100644
--- a/Thirdparty/Sophus/test/ceres/test_ceres_se3.cpp
+++ b/Thirdparty/Sophus/test/ceres/test_ceres_se3.cpp
@@ -103,13 +103,13 @@ bool test(Sophus::SE3d const& T_w_targ, Sophus::SE3d const& T_w_init,
       new ceres::AutoDiffCostFunction<TestSE3CostFunctor, Sophus::SE3d::DoF,
                                       Sophus::SE3d::num_parameters>(
           new TestSE3CostFunctor(T_w_targ.inverse()));
-  problem.AddResidualBlock(cost_function1, NULL, T_wr.data());
+  problem.AddResidualBlock(cost_function1, nullptr, T_wr.data());
   ceres::CostFunction* cost_function2 =
       new ceres::AutoDiffCostFunction<TestPointCostFunctor, kNumPointParameters,
                                       Sophus::SE3d::num_parameters,
                                       kNumPointParameters>(
           new TestPointCostFunctor(T_w_targ.inverse(), point_b));
-  problem.AddResidualBlock(cost_function2, NULL, T_wr.data(), point_a.data());
+  problem.AddResidualBlock(cost_function2, nullptr, T_wr.data(), point_a.data());
 
   // Set solver options (precision / method)
   ceres::Solver::Options options;
diff --git a/Thirdparty/g2o/g2o/core/estimate_propagator.cpp b/Thirdparty/g2o/g2o/core/estimate_propagator.cpp
index 010dac1c..9f2a4189 100644
--- a/Thirdparty/g2o/g2o/core/estimate_propagator.cpp
+++ b/Thirdparty/g2o/g2o/core/estimate_propagator.cpp
@@ -207,7 +207,7 @@ namespace g2o {
 
   void EstimatePropagator::PriorityQueue::push(AdjacencyMapEntry* entry)
   {
-    assert(entry != NULL);
+    assert(entry != nullptr);
     if (entry->inQueue) {
       assert(entry->queueIt->second == entry);
       erase(entry->queueIt);
@@ -225,7 +225,7 @@ namespace g2o {
     AdjacencyMapEntry* entry = it->second;
     erase(it);
 
-    assert(entry != NULL);
+    assert(entry != nullptr);
     entry->queueIt = end();
     entry->inQueue = false;
     return entry;
diff --git a/Thirdparty/g2o/g2o/core/optimizable_graph.h b/Thirdparty/g2o/g2o/core/optimizable_graph.h
index 9d9b561c..82d83cda 100644
--- a/Thirdparty/g2o/g2o/core/optimizable_graph.h
+++ b/Thirdparty/g2o/g2o/core/optimizable_graph.h
@@ -411,7 +411,7 @@ namespace g2o {
          */
         virtual bool setMeasurementFromState();
 
-        //! if NOT NULL, error of this edge will be robustifed with the kernel
+        //! if NOT nullptr, error of this edge will be robustifed with the kernel
         RobustKernel* robustKernel() const { return _robustKernel;}
         /**
          * specify the robust kernel to be used in this edge
diff --git a/Thirdparty/g2o/g2o/core/sparse_block_matrix.hpp b/Thirdparty/g2o/g2o/core/sparse_block_matrix.hpp
index 8dfa99c1..f351ecc9 100644
--- a/Thirdparty/g2o/g2o/core/sparse_block_matrix.hpp
+++ b/Thirdparty/g2o/g2o/core/sparse_block_matrix.hpp
@@ -464,7 +464,7 @@ namespace g2o {
   template <class MatrixType>
   int SparseBlockMatrix<MatrixType>::fillCCS(double* Cx, bool upperTriangle) const
   {
-    assert(Cx && "Target destination is NULL");
+    assert(Cx && "Target destination is nullptr");
     double* CxStart = Cx;
     for (size_t i=0; i<_blockCols.size(); ++i){
       int cstart=i ? _colBlockIndices[i-1] : 0;
@@ -489,7 +489,7 @@ namespace g2o {
   template <class MatrixType>
   int SparseBlockMatrix<MatrixType>::fillCCS(int* Cp, int* Ci, double* Cx, bool upperTriangle) const
   {
-    assert(Cp && Ci && Cx && "Target destination is NULL");
+    assert(Cp && Ci && Cx && "Target destination is nullptr");
     int nz=0;
     for (size_t i=0; i<_blockCols.size(); ++i){
       int cstart=i ? _colBlockIndices[i-1] : 0;
diff --git a/Thirdparty/g2o/g2o/core/sparse_block_matrix_ccs.h b/Thirdparty/g2o/g2o/core/sparse_block_matrix_ccs.h
index eb9042c2..f0c75fe4 100644
--- a/Thirdparty/g2o/g2o/core/sparse_block_matrix_ccs.h
+++ b/Thirdparty/g2o/g2o/core/sparse_block_matrix_ccs.h
@@ -142,7 +142,7 @@ namespace g2o {
        */
       int fillCCS(int* Cp, int* Ci, double* Cx, bool upperTriangle = false) const
       {
-        assert(Cp && Ci && Cx && "Target destination is NULL");
+        assert(Cp && Ci && Cx && "Target destination is nullptr");
         int nz=0;
         for (size_t i=0; i<_blockCols.size(); ++i){
           int cstart=i ? _colBlockIndices[i-1] : 0;
@@ -175,7 +175,7 @@ namespace g2o {
        */
       int fillCCS(double* Cx, bool upperTriangle = false) const
       {
-        assert(Cx && "Target destination is NULL");
+        assert(Cx && "Target destination is nullptr");
         double* CxStart = Cx;
         int cstart = 0;
         for (size_t i=0; i<_blockCols.size(); ++i){
diff --git a/Thirdparty/g2o/g2o/stuff/os_specific.c b/Thirdparty/g2o/g2o/stuff/os_specific.c
index 85f93689..9a18673b 100644
--- a/Thirdparty/g2o/g2o/stuff/os_specific.c
+++ b/Thirdparty/g2o/g2o/stuff/os_specific.c
@@ -35,7 +35,7 @@ int vasprintf(char** strp, const char* fmt, va_list ap)
   char* p;
   char* np;
 
-  if ((p = (char*)malloc(size * sizeof(char))) == NULL)
+  if ((p = (char*)malloc(size * sizeof(char))) == nullptr)
     return -1;
 
   while (1) {
@@ -52,7 +52,7 @@ int vasprintf(char** strp, const char* fmt, va_list ap)
       size = n+1;
     else
       size *= 2;
-    if ((np = (char*)realloc (p, size * sizeof(char))) == NULL) {
+    if ((np = (char*)realloc (p, size * sizeof(char))) == nullptr) {
       free(p);
       return -1;
     } else
diff --git a/Thirdparty/g2o/g2o/stuff/string_tools.cpp b/Thirdparty/g2o/g2o/stuff/string_tools.cpp
index 0a4f60a2..0bd8744c 100644
--- a/Thirdparty/g2o/g2o/stuff/string_tools.cpp
+++ b/Thirdparty/g2o/g2o/stuff/string_tools.cpp
@@ -94,7 +94,7 @@ std::string strToUpper(const std::string& s)
 
 std::string formatString(const char* fmt, ...)
 {
-  char* auxPtr = NULL;
+  char* auxPtr = nullptr;
   va_list arg_list;
   va_start(arg_list, fmt);
   int numChar = vasprintf(&auxPtr, fmt, arg_list);
@@ -111,7 +111,7 @@ std::string formatString(const char* fmt, ...)
 
 int strPrintf(std::string& str, const char* fmt, ...)
 {
-  char* auxPtr = NULL;
+  char* auxPtr = nullptr;
   va_list arg_list;
   va_start(arg_list, fmt);
   int numChars = vasprintf(&auxPtr, fmt, arg_list);
diff --git a/include/Frame.h b/include/Frame.h
index a740e866..ac93a7bf 100644
--- a/include/Frame.h
+++ b/include/Frame.h
@@ -240,7 +240,7 @@ class Frame
     // ORB descriptor, each row associated to a keypoint.
     cv::Mat mDescriptors, mDescriptorsRight;
 
-    // MapPoints associated to keypoints, NULL pointer if no association.
+    // MapPoints associated to keypoints, nullptr pointer if no association.
     // Flag to identify outlier associations.
     std::vector<bool> mvbOutlier;
     int mnCloseMPs;
diff --git a/include/Optimizer.h b/include/Optimizer.h
index 594143f9..b67b2ccb 100644
--- a/include/Optimizer.h
+++ b/include/Optimizer.h
@@ -46,19 +46,19 @@ class Optimizer {
  public:
   void static BundleAdjustment(const std::vector<KeyFrame *> &vpKF,
                                const std::vector<MapPoint *> &vpMP,
-                               int nIterations = 5, bool *pbStopFlag = NULL,
+                               int nIterations = 5, bool *pbStopFlag = nullptr,
                                const unsigned long nLoopKF = 0,
                                const bool bRobust = true);
   void static GlobalBundleAdjustemnt(Map *pMap, int nIterations = 5,
-                                     bool *pbStopFlag = NULL,
+                                     bool *pbStopFlag = nullptr,
                                      const unsigned long nLoopKF = 0,
                                      const bool bRobust = true);
   void static FullInertialBA(Map *pMap, int its, const bool bFixLocal = false,
                              const unsigned long nLoopKF = 0,
-                             bool *pbStopFlag = NULL, bool bInit = false,
+                             bool *pbStopFlag = nullptr, bool bInit = false,
                              float priorG = 1e2, float priorA = 1e6,
-                             Eigen::VectorXd *vSingVal = NULL,
-                             bool *bHess = NULL);
+                             Eigen::VectorXd *vSingVal = nullptr,
+                             bool *bHess = nullptr);
 
   void static LocalBundleAdjustment(KeyFrame *pKF, bool *pbStopFlag, Map *pMap,
                                     int &num_fixedKF, int &num_OptKF,
diff --git a/src/Frame.cc b/src/Frame.cc
index 32b03f0e..85689176 100644
--- a/src/Frame.cc
+++ b/src/Frame.cc
@@ -46,12 +46,12 @@ float Frame::mfGridElementWidthInv, Frame::mfGridElementHeightInv;
 cv::BFMatcher Frame::BFmatcher = cv::BFMatcher(cv::NORM_HAMMING);
 
 Frame::Frame()
-    : mpcpi(NULL),
+    : mpcpi(nullptr),
       mbHasPose(false),
       mbHasVelocity(false),
-      mpImuPreintegrated(NULL),
-      mpPrevFrame(NULL),
-      mpImuPreintegratedFrame(NULL),
+      mpImuPreintegrated(nullptr),
+      mpPrevFrame(nullptr),
+      mpImuPreintegratedFrame(nullptr),
       mpReferenceKF(nullptr),
       mbIsSet(false),
       mbImuPreintegrated(false) {
@@ -153,7 +153,7 @@ Frame::Frame(const cv::Mat &imLeft, const cv::Mat &imRight,
              cv::Mat &distCoef, const float &bf, const float &thDepth,
              GeometricCamera *pCamera, const std::string &pNameFile, int pnNumDataset,
              Frame *pPrevF, const IMU::Calib &ImuCalib)
-    : mpcpi(NULL),
+    : mpcpi(nullptr),
       mbHasPose(false),
       mbHasVelocity(false),
       mpORBvocabulary(voc),
@@ -166,9 +166,9 @@ Frame::Frame(const cv::Mat &imLeft, const cv::Mat &imRight,
       mbf(bf),
       mThDepth(thDepth),
       mImuCalib(ImuCalib),
-      mpImuPreintegrated(NULL),
+      mpImuPreintegrated(nullptr),
       mpPrevFrame(pPrevF),
-      mpImuPreintegratedFrame(NULL),
+      mpImuPreintegratedFrame(nullptr),
       mpReferenceKF(nullptr),
       mNameFile{pNameFile},
       mnDataset{pnNumDataset},
@@ -284,7 +284,7 @@ Frame::Frame(const cv::Mat &imGray, const cv::Mat &imDepth,
              const float &thDepth, GeometricCamera *pCamera,
              const std::string &pNameFile, int pnNumDataset, Frame *pPrevF,
              const IMU::Calib &ImuCalib)
-    : mpcpi(NULL),
+    : mpcpi(nullptr),
       mbHasPose(false),
       mbHasVelocity(false),
       mpORBvocabulary(voc),
@@ -297,9 +297,9 @@ Frame::Frame(const cv::Mat &imGray, const cv::Mat &imDepth,
       mbf(bf),
       mThDepth(thDepth),
       mImuCalib(ImuCalib),
-      mpImuPreintegrated(NULL),
+      mpImuPreintegrated(nullptr),
       mpPrevFrame(pPrevF),
-      mpImuPreintegratedFrame(NULL),
+      mpImuPreintegratedFrame(nullptr),
       mpReferenceKF(nullptr),
       mNameFile{pNameFile},
       mnDataset{pnNumDataset},
@@ -396,7 +396,7 @@ Frame::Frame(const cv::Mat &imGray, const double &timeStamp,
              GeometricCamera *pCamera, cv::Mat &distCoef, const float &bf,
              const float &thDepth, const std::string &pNameFile, int pnNumDataset,
              Frame *pPrevF, const IMU::Calib &ImuCalib)
-    : mpcpi(NULL),
+    : mpcpi(nullptr),
       mbHasPose(false),
       mbHasVelocity(false),
       mpORBvocabulary(voc),
@@ -409,9 +409,9 @@ Frame::Frame(const cv::Mat &imGray, const double &timeStamp,
       mbf(bf),
       mThDepth(thDepth),
       mImuCalib(ImuCalib),
-      mpImuPreintegrated(NULL),
+      mpImuPreintegrated(nullptr),
       mpPrevFrame(pPrevF),
-      mpImuPreintegratedFrame(NULL),
+      mpImuPreintegratedFrame(nullptr),
       mpReferenceKF(nullptr),
       mNameFile{pNameFile},
       mnDataset{pnNumDataset},
@@ -480,7 +480,7 @@ Frame::Frame(const cv::Mat &imGray, const double &timeStamp,
   mvpMapPoints = vector<MapPoint *>(N, nullptr);
 
   mmProjectPoints.clear();  // = map<long unsigned int, cv::Point2f>(N,
-                            // static_cast<cv::Point2f>(NULL));
+                            // static_cast<cv::Point2f>(nullptr));
   mmMatchedInImage.clear();
 
   mvbOutlier = vector<bool>(N, false);
@@ -1108,7 +1108,7 @@ Frame::Frame(const cv::Mat &imLeft, const cv::Mat &imRight,
              GeometricCamera *pCamera, GeometricCamera *pCamera2,
              const std::string &pNameFile, int pnNumDataset,
              Sophus::SE3f &Tlr, Frame *pPrevF, const IMU::Calib &ImuCalib)
-    : mpcpi(NULL),
+    : mpcpi(nullptr),
       mbHasPose(false),
       mbHasVelocity(false),
       mpORBvocabulary(voc),
@@ -1121,9 +1121,9 @@ Frame::Frame(const cv::Mat &imLeft, const cv::Mat &imRight,
       mbf(bf),
       mThDepth(thDepth),
       mImuCalib(ImuCalib),
-      mpImuPreintegrated(NULL),
+      mpImuPreintegrated(nullptr),
       mpPrevFrame(pPrevF),
-      mpImuPreintegratedFrame(NULL),
+      mpImuPreintegratedFrame(nullptr),
       mpReferenceKF(nullptr),
       mNameFile{pNameFile},
       mnDataset{pnNumDataset},
diff --git a/src/KeyFrame.cc b/src/KeyFrame.cc
index f42fdf62..ef3ad145 100644
--- a/src/KeyFrame.cc
+++ b/src/KeyFrame.cc
@@ -65,10 +65,10 @@ KeyFrame::KeyFrame()
       mb(0),
       mThDepth(0),
       N(0),
-      /*mvKeys(static_cast<vector<cv::KeyPoint>>(NULL)),
-      mvKeysUn(static_cast<vector<cv::KeyPoint>>(NULL)),
-      mvuRight(static_cast<vector<float>>(NULL)),
-      mvDepth(static_cast<vector<float>>(NULL)),*/
+      /*mvKeys(static_cast<vector<cv::KeyPoint>>(nullptr)),
+      mvKeysUn(static_cast<vector<cv::KeyPoint>>(nullptr)),
+      mvuRight(static_cast<vector<float>>(nullptr)),
+      mvDepth(static_cast<vector<float>>(nullptr)),*/
       mnScaleLevels(0),
       mfScaleFactor(0),
       mfLogScaleFactor(0),
@@ -83,7 +83,7 @@ KeyFrame::KeyFrame()
       mNextKF(nullptr),
       mbHasVelocity(false),
       mbFirstConnection(true),
-      mpParent(NULL),
+      mpParent(nullptr),
       mbNotErase(false),
       mbToBeErased(false),
       mbBad(false),
@@ -143,8 +143,8 @@ KeyFrame::KeyFrame(Frame &F, Map *pMap, KeyFrameDatabase *pKFDB)
       mnMinY(F.mnMinY),
       mnMaxX(F.mnMaxX),
       mnMaxY(F.mnMaxY),
-      mPrevKF(NULL),
-      mNextKF(NULL),
+      mPrevKF(nullptr),
+      mNextKF(nullptr),
       mpImuPreintegrated(F.mpImuPreintegrated),
       mImuCalib(F.mImuCalib),
       mNameFile(F.mNameFile),
@@ -156,7 +156,7 @@ KeyFrame::KeyFrame(Frame &F, Map *pMap, KeyFrameDatabase *pKFDB)
       mpKeyFrameDB(pKFDB),
       mpORBvocabulary(F.mpORBvocabulary),
       mbFirstConnection(true),
-      mpParent(NULL),
+      mpParent(nullptr),
       mbNotErase(false),
       mbToBeErased(false),
       mbBad(false),
@@ -479,7 +479,7 @@ void KeyFrame::UpdateConnections(bool upParent) {
   // In case no keyframe counter is over threshold add the one with maximum
   // counter
   int nmax = 0;
-  KeyFrame *pKFmax = NULL;
+  KeyFrame *pKFmax = nullptr;
   int th = 15;
 
   vector<pair<int, KeyFrame *>> vPairs;
diff --git a/src/LocalMapping.cc b/src/LocalMapping.cc
index 443189a7..1f48aaa8 100644
--- a/src/LocalMapping.cc
+++ b/src/LocalMapping.cc
@@ -999,8 +999,8 @@ void LocalMapping::KeyFrameCulling() {
                 pKF->mpImuPreintegrated);
             pKF->mNextKF->mPrevKF = pKF->mPrevKF;
             pKF->mPrevKF->mNextKF = pKF->mNextKF;
-            pKF->mNextKF = NULL;
-            pKF->mPrevKF = NULL;
+            pKF->mNextKF = nullptr;
+            pKF->mPrevKF = nullptr;
             pKF->SetBadFlag();
           } else if (!mpCurrentKeyFrame->GetMap()->GetIniertialBA2() &&
                      ((pKF->GetImuPosition() - pKF->mPrevKF->GetImuPosition())
@@ -1010,8 +1010,8 @@ void LocalMapping::KeyFrameCulling() {
                 pKF->mpImuPreintegrated);
             pKF->mNextKF->mPrevKF = pKF->mPrevKF;
             pKF->mPrevKF->mNextKF = pKF->mNextKF;
-            pKF->mNextKF = NULL;
-            pKF->mPrevKF = NULL;
+            pKF->mNextKF = nullptr;
+            pKF->mPrevKF = nullptr;
             pKF->SetBadFlag();
           }
         }
@@ -1242,11 +1242,11 @@ void LocalMapping::InitializeIMU(float priorG, float priorA, bool bFIBA) {
   if (bFIBA) {
     if (priorA != 0.f)
       Optimizer::FullInertialBA(mpAtlas->GetCurrentMap(), 100, false,
-                                mpCurrentKeyFrame->mnId, NULL, true, priorG,
+                                mpCurrentKeyFrame->mnId, nullptr, true, priorG,
                                 priorA);
     else
       Optimizer::FullInertialBA(mpAtlas->GetCurrentMap(), 100, false,
-                                mpCurrentKeyFrame->mnId, NULL, false);
+                                mpCurrentKeyFrame->mnId, nullptr, false);
   }
 
   // std::chrono::steady_clock::time_point t5 = std::chrono::steady_clock::now(); // UNUSED
diff --git a/src/LoopClosing.cc b/src/LoopClosing.cc
index 20ce1ee0..214143a0 100644
--- a/src/LoopClosing.cc
+++ b/src/LoopClosing.cc
@@ -52,7 +52,7 @@ LoopClosing::LoopClosing(const Atlas_ptr &pAtlas, KeyFrameDatabase* pDB,
       mpORBVocabulary(pVoc),
       mnCovisibilityConsistencyTh(3),
       mpLastCurrentKF(nullptr),
-      mpMatchedKF(NULL),
+      mpMatchedKF(nullptr),
       mbLoopDetected(false),
       mnLoopNumCoincidences(0),
       mnLoopNumNotFound(0),
@@ -62,7 +62,7 @@ LoopClosing::LoopClosing(const Atlas_ptr &pAtlas, KeyFrameDatabase* pDB,
       mbRunningGBA(false),
       mbFinishedGBA(true),
       mbStopGBA(false),
-      mpThreadGBA(NULL),
+      mpThreadGBA(nullptr),
       mbFixScale(bFixScale),
       mnFullBAIdx(0),
       mstrFolderSubTraj("SubTrajectories/"),
diff --git a/src/MapPoint.cc b/src/MapPoint.cc
index 756ddb07..4c23208c 100644
--- a/src/MapPoint.cc
+++ b/src/MapPoint.cc
@@ -137,7 +137,7 @@ MapPoint::MapPoint(const Eigen::Vector3f& Pos, Map* pMap, Frame* pFrame,
       mnVisible(1),
       mnFound(1),
       mbBad(false),
-      mpReplaced(NULL),
+      mpReplaced(nullptr),
       mpMap(pMap) {
   SetWorldPos(Pos);
 
diff --git a/src/ORBmatcher.cc b/src/ORBmatcher.cc
index 67c9618b..d3f84a51 100644
--- a/src/ORBmatcher.cc
+++ b/src/ORBmatcher.cc
@@ -1831,7 +1831,7 @@ int ORBmatcher::SearchByProjection(Frame &CurrentFrame, KeyFrame *pKF,
     for (int i = 0; i < HISTO_LENGTH; i++) {
       if (i != ind1 && i != ind2 && i != ind3) {
         for (size_t j = 0, jend = rotHist[i].size(); j < jend; j++) {
-          CurrentFrame.mvpMapPoints[rotHist[i][j]] = NULL;
+          CurrentFrame.mvpMapPoints[rotHist[i][j]] = nullptr;
           nmatches--;
         }
       }
diff --git a/src/Optimizer.cc b/src/Optimizer.cc
index 2881faf9..9f7d9a0d 100644
--- a/src/Optimizer.cc
+++ b/src/Optimizer.cc
@@ -146,7 +146,7 @@ void Optimizer::BundleAdjustment(const vector<KeyFrame*>& vpKFs,
          mit != observations.end(); mit++) {
       KeyFrame* pKF = mit->first;
       if (pKF->isBad() || pKF->mnId > maxKFid) continue;
-      if (optimizer.vertex(id) == NULL || optimizer.vertex(pKF->mnId) == NULL)
+      if (optimizer.vertex(id) == nullptr || optimizer.vertex(pKF->mnId) == nullptr)
         continue;
       nEdges++;
 
@@ -2268,8 +2268,8 @@ int Optimizer::OptimizeSim3(KeyFrame* pKF1, KeyFrame* pKF2,
       vpMatches1[idx] = nullptr;
       optimizer.removeEdge(e12);
       optimizer.removeEdge(e21);
-      vpEdges12[i] = static_cast<ORB_SLAM3::EdgeSim3ProjectXYZ*>(NULL);
-      vpEdges21[i] = static_cast<ORB_SLAM3::EdgeInverseSim3ProjectXYZ*>(NULL);
+      vpEdges12[i] = static_cast<ORB_SLAM3::EdgeSim3ProjectXYZ*>(nullptr);
+      vpEdges21[i] = static_cast<ORB_SLAM3::EdgeInverseSim3ProjectXYZ*>(nullptr);
       nBad++;
 
       if (!vbIsInKF2[i]) {
@@ -2519,9 +2519,9 @@ void Optimizer::LocalInertialBA(KeyFrame* pKF, bool* pbStopFlag, Map* pMap,
   }
 
   // Create intertial constraints
-  vector<EdgeInertial*> vei(N, (EdgeInertial*)NULL);
-  vector<EdgeGyroRW*> vegr(N, (EdgeGyroRW*)NULL);
-  vector<EdgeAccRW*> vear(N, (EdgeAccRW*)NULL);
+  vector<EdgeInertial*> vei(N, (EdgeInertial*)nullptr);
+  vector<EdgeGyroRW*> vegr(N, (EdgeGyroRW*)nullptr);
+  vector<EdgeAccRW*> vear(N, (EdgeAccRW*)nullptr);
 
   for (int i = 0; i < N; i++) {
     KeyFrame* pKFi = vpOptimizableKFs[i];
@@ -4076,9 +4076,9 @@ void Optimizer::MergeInertialBA(KeyFrame* pCurrKF, KeyFrame* pMergeKF,
   }
 
   // Create intertial constraints
-  vector<EdgeInertial*> vei(N, (EdgeInertial*)NULL);
-  vector<EdgeGyroRW*> vegr(N, (EdgeGyroRW*)NULL);
-  vector<EdgeAccRW*> vear(N, (EdgeAccRW*)NULL);
+  vector<EdgeInertial*> vei(N, (EdgeInertial*)nullptr);
+  vector<EdgeGyroRW*> vegr(N, (EdgeGyroRW*)nullptr);
+  vector<EdgeAccRW*> vear(N, (EdgeAccRW*)nullptr);
   for (int i = 0; i < N; i++) {
     // cout << "inserting inertial edge " << i << endl;
     KeyFrame* pKFi = vpOptimizableKFs[i];
@@ -4215,7 +4215,7 @@ void Optimizer::MergeInertialBA(KeyFrame* pCurrKF, KeyFrame* pMergeKF,
         continue;
       }
 
-      if (optimizer.vertex(id) == NULL || optimizer.vertex(pKFi->mnId) == NULL)
+      if (optimizer.vertex(id) == nullptr || optimizer.vertex(pKFi->mnId) == nullptr)
         continue;
 
       if (!pKFi->isBad()) {
@@ -5155,7 +5155,7 @@ int Optimizer::PoseInertialOptimizationLastFrame(Frame* pFrame, bool bRecInit) {
   } else {
     oldMpcpi = pFp->mpcpi;
     delete pFp->mpcpi;
-    pFp->mpcpi = NULL;
+    pFp->mpcpi = nullptr;
   }
   return nInitialCorrespondences - nBad;
 }
diff --git a/src/Tracking.cc b/src/Tracking.cc
index e89d102c..d3ec4d5b 100644
--- a/src/Tracking.cc
+++ b/src/Tracking.cc
@@ -2319,7 +2319,7 @@ void Tracking::CreateInitialMapMonocular() {
       new KeyFrame(mCurrentFrame, mpAtlas->GetCurrentMap(), mpKeyFrameDB);
 
   if (mSensor == CameraType::IMU_MONOCULAR)
-    pKFini->mpImuPreintegrated = (IMU::Preintegrated*)(NULL);
+    pKFini->mpImuPreintegrated = (IMU::Preintegrated*)(nullptr);
 
   pKFini->ComputeBoW();
   pKFcur->ComputeBoW();
@@ -3194,7 +3194,7 @@ void Tracking::UpdateLocalKeyFrames() {
                it != itend; it++)
             keyframeCounter[it->first]++;
         } else {
-          mCurrentFrame.mvpMapPoints[i] = NULL;
+          mCurrentFrame.mvpMapPoints[i] = nullptr;
         }
       }
     }
@@ -3214,7 +3214,7 @@ void Tracking::UpdateLocalKeyFrames() {
             keyframeCounter[it->first]++;
         } else {
           // MODIFICATION
-          mLastFrame.mvpMapPoints[i] = NULL;
+          mLastFrame.mvpMapPoints[i] = nullptr;
         }
       }
     }
@@ -3411,7 +3411,7 @@ bool Tracking::Relocalization() {
             mCurrentFrame.mvpMapPoints[j] = vvpMapPointMatches[i][j];
             sFound.insert(vvpMapPointMatches[i][j]);
           } else
-            mCurrentFrame.mvpMapPoints[j] = NULL;
+            mCurrentFrame.mvpMapPoints[j] = nullptr;
         }
 
         int nGood = Optimizer::PoseOptimization(&mCurrentFrame);
@@ -3448,7 +3448,7 @@ bool Tracking::Relocalization() {
 
                 for (int io = 0; io < mCurrentFrame.N; io++)
                   if (mCurrentFrame.mvbOutlier[io])
-                    mCurrentFrame.mvpMapPoints[io] = NULL;
+                    mCurrentFrame.mvpMapPoints[io] = nullptr;
               }
             }
           }

From d08474d6bbc038f1eb31c59c91364fb66930d844 Mon Sep 17 00:00:00 2001
From: DavidPetkovsek <david.petkovsek@martinrea.com>
Date: Tue, 26 Jul 2022 18:24:28 -0400
Subject: [PATCH 12/26] Add Camera class that handles ExtractORB threads

---
 CMakeLists.txt     |  2 +
 include/Camera.hpp | 91 ++++++++++++++++++++++++++++++++++++++++++++++
 include/Frame.h    | 11 ++++--
 include/System.h   |  2 +
 include/Tracking.h |  9 +++--
 src/Camera.cpp     | 69 +++++++++++++++++++++++++++++++++++
 src/Frame.cc       | 31 +++++++++-------
 src/System.cc      |  7 ++--
 src/Tracking.cc    | 29 ++++++++-------
 9 files changed, 214 insertions(+), 37 deletions(-)
 create mode 100644 include/Camera.hpp
 create mode 100644 src/Camera.cpp

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 42b89ab5..1356995c 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -50,6 +50,7 @@ find_package(realsense2)
 
 add_library(${PROJECT_NAME} SHARED
 src/System.cc
+src/Camera.cpp
 src/Tracking.cc
 src/LocalMapping.cc
 src/LoopClosing.cc
@@ -77,6 +78,7 @@ src/GeometricTools.cc
 src/TwoViewReconstruction.cc
 src/Settings.cc
 include/System.h
+include/Camera.hpp
 include/Tracking.h
 include/LocalMapping.h
 include/LoopClosing.h
diff --git a/include/Camera.hpp b/include/Camera.hpp
new file mode 100644
index 00000000..5d8836d1
--- /dev/null
+++ b/include/Camera.hpp
@@ -0,0 +1,91 @@
+#pragma once
+#include <thread>
+#include <string>
+#include "ImprovedTypes.hpp"
+#include <mutex>
+#include <condition_variable>
+#include <future>
+#include <functional>
+#include <memory>
+#include <exception>
+#include <stdexcept>
+#include <deque>
+#include <utility>
+
+namespace ORB_SLAM3{
+
+
+template<typename T>
+class ManagedFuture{
+protected:
+    std::shared_ptr<std::promise<T>> promise;
+    std::shared_future<T> future;
+
+   ManagedFuture(): promise{std::make_shared<std::promise<T>>()}, future{promise->get_future()} {} // make default constructor protected
+public:
+    virtual ~ManagedFuture(){}
+
+    // functions from std::shared_future<T>
+
+    const T& get() const { return future.get(); }
+    void wait() const { future.wait(); }
+
+    template<class Rep, class Period>
+    std::future_status wait_for( const std::chrono::duration<Rep,Period>& timeout_duration ) const { return future.wait_for(timeout_duration); }
+
+    template<class Clock, class Duration>
+    std::future_status wait_until( const std::chrono::time_point<Clock,Duration>& timeout_time ) const { return future.wait_until(timeout_time); }
+};
+
+template<typename T>
+class ManagedPromise: public ManagedFuture<T>{
+    std::mutex *mutex;
+public:
+    ManagedPromise(): mutex{new std::mutex{}} {}
+    virtual ~ManagedPromise(){ delete mutex; }
+    
+    // functions from std::promise<T>
+
+    void set_value_at_thread_exit(const T& value){ std::scoped_lock<std::mutex> lock(*mutex); ManagedFuture<T>::promise->set_value_at_thread_exit(value); }
+    void set_value_at_thread_exit(T&& value){ std::scoped_lock<std::mutex> lock(*mutex); ManagedFuture<T>::promise->set_value_at_thread_exit(value); }
+    void set_value_at_thread_exit(T& value){ std::scoped_lock<std::mutex> lock(*mutex); ManagedFuture<T>::promise->set_value_at_thread_exit(value); }
+    void set_value_at_thread_exit(){ std::scoped_lock<std::mutex> lock(*mutex); ManagedFuture<T>::promise->set_value_at_thread_exit(); }
+    void set_exception_at_thread_exit(const std::exception_ptr &p ){ std::scoped_lock<std::mutex> lock(*mutex); ManagedFuture<T>::promise->set_exception_at_thread_exit(p); }
+    void set_exception(const std::exception_ptr &p ){ std::scoped_lock<std::mutex> lock(*mutex); ManagedFuture<T>::promise->set_exception(p); }
+    void set_value(const T& value){ std::scoped_lock<std::mutex> lock(*mutex); ManagedFuture<T>::promise->set_value(value); }
+    void set_value(T&& value){ std::scoped_lock<std::mutex> lock(*mutex); ManagedFuture<T>::promise->set_value(value); }
+    void set_value(T& value){ std::scoped_lock<std::mutex> lock(*mutex); ManagedFuture<T>::promise->set_value(value); }
+    void set_value(){ std::scoped_lock<std::mutex> lock(*mutex); ManagedFuture<T>::promise->set_value(); }
+};
+
+class Camera{
+    std::deque<std::pair<ManagedPromise<bool>, std::function<void(void)>>> ljobs;
+    std::deque<std::pair<ManagedPromise<bool>, std::function<void(void)>>> rjobs;
+
+    std::string name;
+    CameraType::eSensor type;
+    std::thread lthread;
+    std::thread rthread;
+    
+    std::mutex camMutex;
+    std::condition_variable camCV;
+
+    bool shouldStop;
+    
+    void threadExec(std::deque<std::pair<ManagedPromise<bool>, std::function<void(void)>>> *jobs);
+public:
+    Camera(CameraType::eSensor type, const std::string &name="camera");
+    virtual ~Camera();
+
+    ManagedFuture<bool> queue(std::function<void(void)> func, bool isLeft=true);
+    ManagedFuture<bool> queueLeft(const std::function<void(void)> &func);
+    ManagedFuture<bool> queueRight(const std::function<void(void)> &func);
+
+    CameraType::eSensor getType() const;
+    const std::string &getName() const;
+};
+
+typedef std::shared_ptr<Camera> Camera_ptr;
+typedef std::weak_ptr<Camera> Camera_wptr;
+
+} // namespace ORB_SLAM3
\ No newline at end of file
diff --git a/include/Frame.h b/include/Frame.h
index ac93a7bf..1c7dc6d7 100644
--- a/include/Frame.h
+++ b/include/Frame.h
@@ -38,6 +38,8 @@
 #include "Eigen/Core"
 #include "sophus/se3.hpp"
 
+#include "Camera.hpp"
+
 namespace ORB_SLAM3
 {
 #define FRAME_GRID_ROWS 48
@@ -58,13 +60,13 @@ class Frame
     Frame(const Frame &frame);
 
     // Constructor for stereo cameras.
-    Frame(const cv::Mat &imLeft, const cv::Mat &imRight, const double &timeStamp, ORBextractor* extractorLeft, ORBextractor* extractorRight, ORBVocabulary* voc, cv::Mat &K, cv::Mat &distCoef, const float &bf, const float &thDepth, GeometricCamera* pCamera, const std::string &pNameFile, int pnNumDataset, Frame* pPrevF = nullptr, const IMU::Calib &ImuCalib = IMU::Calib());
+    Frame(const Camera_ptr &cam, const cv::Mat &imLeft, const cv::Mat &imRight, const double &timeStamp, ORBextractor* extractorLeft, ORBextractor* extractorRight, ORBVocabulary* voc, cv::Mat &K, cv::Mat &distCoef, const float &bf, const float &thDepth, GeometricCamera* pCamera, const std::string &pNameFile, int pnNumDataset, Frame* pPrevF = nullptr, const IMU::Calib &ImuCalib = IMU::Calib());
 
     // Constructor for RGB-D cameras.
-    Frame(const cv::Mat &imGray, const cv::Mat &imDepth, const double &timeStamp, ORBextractor* extractor,ORBVocabulary* voc, cv::Mat &K, cv::Mat &distCoef, const float &bf, const float &thDepth, GeometricCamera* pCamera, const std::string &pNameFile, int pnNumDataset, Frame* pPrevF = nullptr, const IMU::Calib &ImuCalib = IMU::Calib());
+    Frame(const Camera_ptr &cam, const cv::Mat &imGray, const cv::Mat &imDepth, const double &timeStamp, ORBextractor* extractor,ORBVocabulary* voc, cv::Mat &K, cv::Mat &distCoef, const float &bf, const float &thDepth, GeometricCamera* pCamera, const std::string &pNameFile, int pnNumDataset, Frame* pPrevF = nullptr, const IMU::Calib &ImuCalib = IMU::Calib());
 
     // Constructor for Monocular cameras.
-    Frame(const cv::Mat &imGray, const double &timeStamp, ORBextractor* extractor,ORBVocabulary* voc, GeometricCamera* pCamera, cv::Mat &distCoef, const float &bf, const float &thDepth, const std::string &pNameFile, int pnNumDataset,  Frame* pPrevF = nullptr, const IMU::Calib &ImuCalib = IMU::Calib());
+    Frame(const Camera_ptr &cam, const cv::Mat &imGray, const double &timeStamp, ORBextractor* extractor,ORBVocabulary* voc, GeometricCamera* pCamera, cv::Mat &distCoef, const float &bf, const float &thDepth, const std::string &pNameFile, int pnNumDataset,  Frame* pPrevF = nullptr, const IMU::Calib &ImuCalib = IMU::Calib());
 
     // Destructor
     virtual ~Frame();
@@ -322,6 +324,7 @@ class Frame
     std::mutex *mpMutexImu;
 
 public:
+    Camera_ptr camera; 
     GeometricCamera* mpCamera, *mpCamera2;
 
     //Number of KeyPoints extracted in the left and right images
@@ -342,7 +345,7 @@ class Frame
     //Grid for the right image
     std::vector<std::size_t> mGridRight[FRAME_GRID_COLS][FRAME_GRID_ROWS];
 
-    Frame(const cv::Mat &imLeft, const cv::Mat &imRight, const double &timeStamp, ORBextractor* extractorLeft, ORBextractor* extractorRight, ORBVocabulary* voc, cv::Mat &K, cv::Mat &distCoef, const float &bf, const float &thDepth, GeometricCamera* pCamera, GeometricCamera* pCamera2, const std::string &pNameFile, int pnNumDataset, Sophus::SE3f& Tlr,Frame* pPrevF = nullptr, const IMU::Calib &ImuCalib = IMU::Calib());
+    Frame(const Camera_ptr &cam, const cv::Mat &imLeft, const cv::Mat &imRight, const double &timeStamp, ORBextractor* extractorLeft, ORBextractor* extractorRight, ORBVocabulary* voc, cv::Mat &K, cv::Mat &distCoef, const float &bf, const float &thDepth, GeometricCamera* pCamera, GeometricCamera* pCamera2, const std::string &pNameFile, int pnNumDataset, Sophus::SE3f& Tlr,Frame* pPrevF = nullptr, const IMU::Calib &ImuCalib = IMU::Calib());
 
     //Stereo fisheye
     void ComputeStereoFishEyeMatches();
diff --git a/include/System.h b/include/System.h
index 4afc922d..fb227d31 100644
--- a/include/System.h
+++ b/include/System.h
@@ -36,6 +36,7 @@
 #include "ORBVocabulary.h"
 #include "ImuTypes.h"
 #include "Settings.h"
+#include "Camera.hpp"
 
 
 namespace ORB_SLAM3
@@ -187,6 +188,7 @@ class System
 
     // Input sensor
     CameraType::eSensor mSensor;
+    vector<Camera_ptr> cameras;
 
     // ORB vocabulary used for place recognition and feature matching.
     ORBVocabulary* mpVocabulary;
diff --git a/include/Tracking.h b/include/Tracking.h
index 003115b4..91d22dee 100644
--- a/include/Tracking.h
+++ b/include/Tracking.h
@@ -38,6 +38,7 @@
 #include "Settings.h"
 #include "System.h"
 #include "ImprovedTypes.hpp"
+#include "Camera.hpp"
 
 namespace ORB_SLAM3 {
 
@@ -66,11 +67,13 @@ class Tracking {
   // matching.
   Sophus::SE3f GrabImageStereo(const cv::Mat& imRectLeft,
                                const cv::Mat& imRectRight,
-                               const double& timestamp, const string &filename);
+                               const double& timestamp, const string &filename,
+                               const Camera_ptr &cam);
   Sophus::SE3f GrabImageRGBD(const cv::Mat& imRGB, const cv::Mat& imD,
-                             const double& timestamp, const string &filename);
+                             const double& timestamp, const string &filename,
+                             const Camera_ptr &cam);
   Sophus::SE3f GrabImageMonocular(const cv::Mat& im, const double& timestamp,
-                                  const string &filename);
+                                  const string &filename, const Camera_ptr &cam);
 
   void GrabImuData(const IMU::Point& imuMeasurement);
 
diff --git a/src/Camera.cpp b/src/Camera.cpp
new file mode 100644
index 00000000..bda78c13
--- /dev/null
+++ b/src/Camera.cpp
@@ -0,0 +1,69 @@
+#include "Camera.hpp"
+
+namespace ORB_SLAM3{
+
+Camera::Camera(CameraType::eSensor type, const std::string &name): ljobs{}, rjobs{}, name{name}, type{type},
+    lthread{&Camera::threadExec, this, &ljobs}, rthread{&Camera::threadExec, this, &rjobs}, shouldStop{false} {
+
+}
+
+Camera::~Camera(){
+    {
+        std::scoped_lock<std::mutex> lock(camMutex);
+        shouldStop = true;
+        camCV.notify_all();
+    }
+
+    if(lthread.joinable()) lthread.join();
+    if(rthread.joinable()) rthread.join();
+}
+
+void Camera::threadExec(std::deque<std::pair<ManagedPromise<bool>, std::function<void(void)>>> *jobs){
+    while(true){
+        ManagedPromise<bool> promise;
+        std::function<void(void)> func;
+        {
+            std::unique_lock<std::mutex> lock(camMutex);
+            if(shouldStop) break;
+            if(jobs->empty()){
+                camCV.wait(lock);
+                if(shouldStop) break;
+                if(jobs->empty()) continue;
+            }
+            promise = jobs->front().first;
+            func = jobs->front().second;
+            jobs->pop_front();
+        }
+
+        try{
+            func();
+        }catch(const std::exception &e){
+            promise.set_exception(std::current_exception());
+        }
+        promise.set_value(true);
+    }
+}
+
+ManagedFuture<bool> Camera::queue(std::function<void(void)> func, bool isLeft){
+    ManagedPromise<bool> promise;
+    std::deque<std::pair<ManagedPromise<bool>, std::function<void(void)>>> &jobs = isLeft ? ljobs : rjobs;
+    {
+        std::scoped_lock<std::mutex> lock(camMutex);
+
+        if(!jobs.empty()){ // only the newest! // TODO add a policy variable to enable or disable this!
+            jobs.front().first.set_value(false);
+            jobs.clear();
+        }
+        if(!shouldStop) jobs.emplace_back(promise,func);
+        camCV.notify_all();
+    }
+
+    return promise;
+}
+ManagedFuture<bool> Camera::queueLeft(const std::function<void(void)> &func){ return queue(func, true); }
+ManagedFuture<bool> Camera::queueRight(const std::function<void(void)> &func){ return queue(func, false); }
+
+CameraType::eSensor Camera::getType() const { return type; }
+const std::string &Camera::getName() const { return name; }
+
+} // namespace ORB_SLAM3
\ No newline at end of file
diff --git a/src/Frame.cc b/src/Frame.cc
index 85689176..7314818b 100644
--- a/src/Frame.cc
+++ b/src/Frame.cc
@@ -115,6 +115,7 @@ Frame::Frame(const Frame &frame)
       mbIsSet(frame.mbIsSet),
       mbImuPreintegrated(frame.mbImuPreintegrated),
       mpMutexImu(frame.mpMutexImu),
+      camera{frame.camera},
       mpCamera(frame.mpCamera),
       mpCamera2(frame.mpCamera2),
       Nleft(frame.Nleft),
@@ -147,7 +148,7 @@ Frame::Frame(const Frame &frame)
 #endif
 }
 
-Frame::Frame(const cv::Mat &imLeft, const cv::Mat &imRight,
+Frame::Frame(const Camera_ptr &cam, const cv::Mat &imLeft, const cv::Mat &imRight,
              const double &timeStamp, ORBextractor *extractorLeft,
              ORBextractor *extractorRight, ORBVocabulary *voc, cv::Mat &K,
              cv::Mat &distCoef, const float &bf, const float &thDepth,
@@ -174,6 +175,7 @@ Frame::Frame(const cv::Mat &imLeft, const cv::Mat &imRight,
       mnDataset{pnNumDataset},
       mbIsSet(false),
       mbImuPreintegrated(false),
+      camera(cam),
       mpCamera(pCamera),
       mpCamera2(nullptr) {
   // Frame ID
@@ -193,10 +195,9 @@ Frame::Frame(const cv::Mat &imLeft, const cv::Mat &imRight,
   std::chrono::steady_clock::time_point time_StartExtORB =
       std::chrono::steady_clock::now();
 #endif
-  thread threadLeft(&Frame::ExtractORB, this, true, imLeft, 0, 0);
-  thread threadRight(&Frame::ExtractORB, this, false, imRight, 0, 0);
-  threadLeft.join();
-  threadRight.join();
+  auto leftFut = camera->queueLeft(std::bind(&Frame::ExtractORB, this, true, imLeft, 0, 0));
+  auto rightFut = camera->queueRight(std::bind(&Frame::ExtractORB, this, false, imRight, 0, 0));
+  if(!leftFut.get() || !rightFut.get()) return;
 #ifdef REGISTER_TIMES
   std::chrono::steady_clock::time_point time_EndExtORB =
       std::chrono::steady_clock::now();
@@ -278,7 +279,7 @@ Frame::~Frame(){
   delete mpMutexImu;
 }
 
-Frame::Frame(const cv::Mat &imGray, const cv::Mat &imDepth,
+Frame::Frame(const Camera_ptr &cam, const cv::Mat &imGray, const cv::Mat &imDepth,
              const double &timeStamp, ORBextractor *extractor,
              ORBVocabulary *voc, cv::Mat &K, cv::Mat &distCoef, const float &bf,
              const float &thDepth, GeometricCamera *pCamera,
@@ -305,6 +306,7 @@ Frame::Frame(const cv::Mat &imGray, const cv::Mat &imDepth,
       mnDataset{pnNumDataset},
       mbIsSet(false),
       mbImuPreintegrated(false),
+      camera{cam}, 
       mpCamera(pCamera),
       mpCamera2(nullptr) {
   // Frame ID
@@ -391,7 +393,7 @@ Frame::Frame(const cv::Mat &imGray, const cv::Mat &imDepth,
   AssignFeaturesToGrid();
 }
 
-Frame::Frame(const cv::Mat &imGray, const double &timeStamp,
+Frame::Frame(const Camera_ptr &cam, const cv::Mat &imGray, const double &timeStamp,
              ORBextractor *extractor, ORBVocabulary *voc,
              GeometricCamera *pCamera, cv::Mat &distCoef, const float &bf,
              const float &thDepth, const std::string &pNameFile, int pnNumDataset,
@@ -417,6 +419,7 @@ Frame::Frame(const cv::Mat &imGray, const double &timeStamp,
       mnDataset{pnNumDataset},
       mbIsSet(false),
       mbImuPreintegrated(false),
+      camera{cam},
       mpCamera(pCamera),
       mpCamera2(nullptr) {
   // Frame ID
@@ -1101,7 +1104,7 @@ void Frame::setIntegrated() {
   mbImuPreintegrated = true;
 }
 
-Frame::Frame(const cv::Mat &imLeft, const cv::Mat &imRight,
+Frame::Frame(const Camera_ptr &cam, const cv::Mat &imLeft, const cv::Mat &imRight,
              const double &timeStamp, ORBextractor *extractorLeft,
              ORBextractor *extractorRight, ORBVocabulary *voc, cv::Mat &K,
              cv::Mat &distCoef, const float &bf, const float &thDepth,
@@ -1128,6 +1131,7 @@ Frame::Frame(const cv::Mat &imLeft, const cv::Mat &imRight,
       mNameFile{pNameFile},
       mnDataset{pnNumDataset},
       mbImuPreintegrated(false),
+      camera{cam},
       mpCamera(pCamera),
       mpCamera2(pCamera2)
 
@@ -1152,15 +1156,14 @@ Frame::Frame(const cv::Mat &imLeft, const cv::Mat &imRight,
   std::chrono::steady_clock::time_point time_StartExtORB =
       std::chrono::steady_clock::now();
 #endif
-  thread threadLeft(&Frame::ExtractORB, this, true, imLeft,
+  auto leftFut = camera->queueLeft(std::bind(&Frame::ExtractORB, this, true, imLeft,
                     static_cast<KannalaBrandt8 *>(mpCamera)->mvLappingArea[0],
-                    static_cast<KannalaBrandt8 *>(mpCamera)->mvLappingArea[1]);
-  thread threadRight(
+                    static_cast<KannalaBrandt8 *>(mpCamera)->mvLappingArea[1]));
+  auto rightFut = camera->queueRight(std::bind(
       &Frame::ExtractORB, this, false, imRight,
       static_cast<KannalaBrandt8 *>(mpCamera2)->mvLappingArea[0],
-      static_cast<KannalaBrandt8 *>(mpCamera2)->mvLappingArea[1]);
-  threadLeft.join();
-  threadRight.join();
+      static_cast<KannalaBrandt8 *>(mpCamera2)->mvLappingArea[1]));
+  if(!leftFut.get() || !rightFut.get()) return;
 #ifdef REGISTER_TIMES
   std::chrono::steady_clock::time_point time_EndExtORB =
       std::chrono::steady_clock::now();
diff --git a/src/System.cc b/src/System.cc
index a0fb27fa..f5a383ae 100644
--- a/src/System.cc
+++ b/src/System.cc
@@ -56,6 +56,7 @@ System::System(const std::string& strVocFile, const std::string& strSettingsFile
       mbResetActiveMap(false),
       mbActivateLocalizationMode(false),
       mbDeactivateLocalizationMode(false) {
+  cameras.push_back(make_shared<Camera>(mSensor)); // for now just hard code the sensor we are using, TODO make multicam
   // Output welcome message
   std::cout << std::endl
        << "ORB-SLAM3 Copyright (C) 2017-2020 Carlos Campos, Richard Elvira, "
@@ -308,7 +309,7 @@ Sophus::SE3f System::TrackStereo(const cv::Mat& imLeft, const cv::Mat& imRight,
 
   // std::cout << "start GrabImageStereo" << std::endl;
   Sophus::SE3f Tcw = mpTracker->GrabImageStereo(imLeftToFeed, imRightToFeed,
-                                                timestamp, filename);
+                                                timestamp, filename, cameras[0]); // for now we know cameras[0] is providing the image
 
   // std::cout << "out grabber" << std::endl;
 
@@ -379,7 +380,7 @@ Sophus::SE3f System::TrackRGBD(const cv::Mat& im, const cv::Mat& depthmap,
       mpTracker->GrabImuData(vImuMeas[i_imu]);
 
   Sophus::SE3f Tcw =
-      mpTracker->GrabImageRGBD(imToFeed, imDepthToFeed, timestamp, filename);
+      mpTracker->GrabImageRGBD(imToFeed, imDepthToFeed, timestamp, filename, cameras[0]); // for now we know cameras[0] is providing the image
 
   unique_lock<mutex> lock2(mMutexState);
   mTrackingState = mpTracker->mState;
@@ -450,7 +451,7 @@ Sophus::SE3f System::TrackMonocular(const cv::Mat& im, const double& timestamp,
       mpTracker->GrabImuData(vImuMeas[i_imu]);
 
   Sophus::SE3f Tcw =
-      mpTracker->GrabImageMonocular(imToFeed, timestamp, filename);
+      mpTracker->GrabImageMonocular(imToFeed, timestamp, filename, cameras[0]); // for now we know cameras[0] is providing the image
 
   unique_lock<mutex> lock2(mMutexState);
   mTrackingState = mpTracker->mState;
diff --git a/src/Tracking.cc b/src/Tracking.cc
index d3ec4d5b..3fe5c337 100644
--- a/src/Tracking.cc
+++ b/src/Tracking.cc
@@ -1355,7 +1355,8 @@ void Tracking::SetLoopClosing(LoopClosing* pLoopClosing) {
 Sophus::SE3f Tracking::GrabImageStereo(const cv::Mat& imRectLeft,
                                        const cv::Mat& imRectRight,
                                        const double& timestamp,
-                                       const string &filename) {
+                                       const string &filename,
+                                       const Camera_ptr &cam) {
   // cout << "GrabImageStereo" << endl;
 
   mImGray = imRectLeft;
@@ -1385,20 +1386,20 @@ Sophus::SE3f Tracking::GrabImageStereo(const cv::Mat& imRectLeft,
   // cout << "Incoming frame creation" << endl;
 
   if (mSensor == CameraType::STEREO && !mpCamera2)
-    mCurrentFrame = Frame(mImGray, imGrayRight, timestamp, mpORBextractorLeft,
+    mCurrentFrame = Frame(cam, mImGray, imGrayRight, timestamp, mpORBextractorLeft,
                           mpORBextractorRight, mpORBVocabulary, mK, mDistCoef,
                           mbf, mThDepth, mpCamera, filename, mnNumDataset);
   else if (mSensor == CameraType::STEREO && mpCamera2)
-    mCurrentFrame = Frame(mImGray, imGrayRight, timestamp, mpORBextractorLeft,
+    mCurrentFrame = Frame(cam, mImGray, imGrayRight, timestamp, mpORBextractorLeft,
                           mpORBextractorRight, mpORBVocabulary, mK, mDistCoef,
                           mbf, mThDepth, mpCamera, mpCamera2, filename, mnNumDataset, mTlr);
   else if (mSensor == CameraType::IMU_STEREO && !mpCamera2)
-    mCurrentFrame = Frame(mImGray, imGrayRight, timestamp, mpORBextractorLeft,
+    mCurrentFrame = Frame(cam, mImGray, imGrayRight, timestamp, mpORBextractorLeft,
                           mpORBextractorRight, mpORBVocabulary, mK, mDistCoef,
                           mbf, mThDepth, mpCamera, filename, mnNumDataset, &mLastFrame, *mpImuCalib);
   else if (mSensor == CameraType::IMU_STEREO && mpCamera2)
     mCurrentFrame =
-        Frame(mImGray, imGrayRight, timestamp, mpORBextractorLeft,
+        Frame(cam, mImGray, imGrayRight, timestamp, mpORBextractorLeft,
               mpORBextractorRight, mpORBVocabulary, mK, mDistCoef, mbf,
               mThDepth, mpCamera, mpCamera2, filename, mnNumDataset, mTlr, &mLastFrame, *mpImuCalib);
 
@@ -1417,7 +1418,8 @@ Sophus::SE3f Tracking::GrabImageStereo(const cv::Mat& imRectLeft,
 }
 
 Sophus::SE3f Tracking::GrabImageRGBD(const cv::Mat& imRGB, const cv::Mat& imD,
-                                     const double& timestamp, const string &filename) {
+                                     const double& timestamp, const string &filename,
+                                     const Camera_ptr &cam) {
   mImGray = imRGB;
   cv::Mat imDepth = imD;
 
@@ -1438,11 +1440,11 @@ Sophus::SE3f Tracking::GrabImageRGBD(const cv::Mat& imRGB, const cv::Mat& imD,
 
   if (mSensor == CameraType::RGBD)
     mCurrentFrame =
-        Frame(mImGray, imDepth, timestamp, mpORBextractorLeft, mpORBVocabulary,
+        Frame(cam, mImGray, imDepth, timestamp, mpORBextractorLeft, mpORBVocabulary,
               mK, mDistCoef, mbf, mThDepth, mpCamera, filename, mnNumDataset);
   else if (mSensor == CameraType::IMU_RGBD)
     mCurrentFrame =
-        Frame(mImGray, imDepth, timestamp, mpORBextractorLeft, mpORBVocabulary,
+        Frame(cam, mImGray, imDepth, timestamp, mpORBextractorLeft, mpORBVocabulary,
               mK, mDistCoef, mbf, mThDepth, mpCamera, filename, mnNumDataset, &mLastFrame, *mpImuCalib);
 
 #ifdef REGISTER_TIMES
@@ -1456,7 +1458,8 @@ Sophus::SE3f Tracking::GrabImageRGBD(const cv::Mat& imRGB, const cv::Mat& imD,
 
 Sophus::SE3f Tracking::GrabImageMonocular(const cv::Mat& im,
                                           const double& timestamp,
-                                          const string &filename) {
+                                          const string &filename,
+                                          const Camera_ptr &cam) {
   mImGray = im;
   if (mImGray.channels() == 3) {
     if (mbRGB)
@@ -1474,20 +1477,20 @@ Sophus::SE3f Tracking::GrabImageMonocular(const cv::Mat& im,
     if (mState == Tracker::NOT_INITIALIZED || mState == Tracker::NO_IMAGES_YET ||
         (lastID - initID) < mMaxFrames)
       mCurrentFrame =
-          Frame(mImGray, timestamp, mpIniORBextractor, mpORBVocabulary,
+          Frame(cam, mImGray, timestamp, mpIniORBextractor, mpORBVocabulary,
                 mpCamera, mDistCoef, mbf, mThDepth, filename, mnNumDataset);
     else
       mCurrentFrame =
-          Frame(mImGray, timestamp, mpORBextractorLeft, mpORBVocabulary,
+          Frame(cam, mImGray, timestamp, mpORBextractorLeft, mpORBVocabulary,
                 mpCamera, mDistCoef, mbf, mThDepth, filename, mnNumDataset);
   } else if (mSensor == CameraType::IMU_MONOCULAR) {
     if (mState == Tracker::NOT_INITIALIZED || mState == Tracker::NO_IMAGES_YET) {
       mCurrentFrame =
-          Frame(mImGray, timestamp, mpIniORBextractor, mpORBVocabulary,
+          Frame(cam, mImGray, timestamp, mpIniORBextractor, mpORBVocabulary,
                 mpCamera, mDistCoef, mbf, mThDepth, filename, mnNumDataset, &mLastFrame, *mpImuCalib);
     } else
       mCurrentFrame =
-          Frame(mImGray, timestamp, mpORBextractorLeft, mpORBVocabulary,
+          Frame(cam, mImGray, timestamp, mpORBextractorLeft, mpORBVocabulary,
                 mpCamera, mDistCoef, mbf, mThDepth, filename, mnNumDataset, &mLastFrame, *mpImuCalib);
   }
 

From 7efa8c90609913d110cd6aee9355cdf5ca453fe2 Mon Sep 17 00:00:00 2001
From: DavidPetkovsek <david.petkovsek@martinrea.com>
Date: Wed, 27 Jul 2022 09:32:33 -0400
Subject: [PATCH 13/26] Fixed mutex initialization and destruction issues

---
 include/Camera.hpp | 13 +++++++------
 include/Frame.h    |  2 +-
 include/Tracking.h |  2 +-
 src/Camera.cpp     |  2 +-
 src/Frame.cc       | 12 +++++-------
 src/Tracking.cc    | 25 +++++++++++--------------
 6 files changed, 26 insertions(+), 30 deletions(-)

diff --git a/include/Camera.hpp b/include/Camera.hpp
index 5d8836d1..416f71e0 100644
--- a/include/Camera.hpp
+++ b/include/Camera.hpp
@@ -39,10 +39,10 @@ class ManagedFuture{
 
 template<typename T>
 class ManagedPromise: public ManagedFuture<T>{
-    std::mutex *mutex;
+    std::shared_ptr<std::mutex> mutex;
 public:
-    ManagedPromise(): mutex{new std::mutex{}} {}
-    virtual ~ManagedPromise(){ delete mutex; }
+    ManagedPromise(): mutex{std::make_shared<std::mutex>()} {}
+    virtual ~ManagedPromise(){}
     
     // functions from std::promise<T>
 
@@ -64,14 +64,15 @@ class Camera{
 
     std::string name;
     CameraType::eSensor type;
-    std::thread lthread;
-    std::thread rthread;
-    
     std::mutex camMutex;
+    
     std::condition_variable camCV;
 
     bool shouldStop;
     
+    std::thread lthread;
+    std::thread rthread;
+
     void threadExec(std::deque<std::pair<ManagedPromise<bool>, std::function<void(void)>>> *jobs);
 public:
     Camera(CameraType::eSensor type, const std::string &name="camera");
diff --git a/include/Frame.h b/include/Frame.h
index 1c7dc6d7..d074c3d9 100644
--- a/include/Frame.h
+++ b/include/Frame.h
@@ -321,7 +321,7 @@ class Frame
 
     bool mbImuPreintegrated;
 
-    std::mutex *mpMutexImu;
+    std::shared_ptr<std::mutex> mpMutexImu;
 
 public:
     Camera_ptr camera; 
diff --git a/include/Tracking.h b/include/Tracking.h
index 91d22dee..dde6d595 100644
--- a/include/Tracking.h
+++ b/include/Tracking.h
@@ -258,7 +258,7 @@ class Tracking {
   KeyFrameDatabase* mpKeyFrameDB;
 
   // Initalization (only for monocular)
-  bool mbReadyToInitializate;
+  bool mbReadyToInitialize;
   bool mbSetInit;
 
   // Local Map
diff --git a/src/Camera.cpp b/src/Camera.cpp
index bda78c13..508a4af8 100644
--- a/src/Camera.cpp
+++ b/src/Camera.cpp
@@ -3,7 +3,7 @@
 namespace ORB_SLAM3{
 
 Camera::Camera(CameraType::eSensor type, const std::string &name): ljobs{}, rjobs{}, name{name}, type{type},
-    lthread{&Camera::threadExec, this, &ljobs}, rthread{&Camera::threadExec, this, &rjobs}, shouldStop{false} {
+    shouldStop{false}, lthread{&Camera::threadExec, this, &ljobs}, rthread{&Camera::threadExec, this, &rjobs} {
 
 }
 
diff --git a/src/Frame.cc b/src/Frame.cc
index 7314818b..75592ed8 100644
--- a/src/Frame.cc
+++ b/src/Frame.cc
@@ -261,7 +261,7 @@ Frame::Frame(const Camera_ptr &cam, const cv::Mat &imLeft, const cv::Mat &imRigh
     mVw.setZero();
   }
 
-  mpMutexImu = new std::mutex();
+  mpMutexImu = std::make_shared<std::mutex>();
 
   // Set no stereo fisheye information
   Nleft = -1;
@@ -275,9 +275,7 @@ Frame::Frame(const Camera_ptr &cam, const cv::Mat &imLeft, const cv::Mat &imRigh
   AssignFeaturesToGrid();
 }
 
-Frame::~Frame(){
-  delete mpMutexImu;
-}
+Frame::~Frame(){}
 
 Frame::Frame(const Camera_ptr &cam, const cv::Mat &imGray, const cv::Mat &imDepth,
              const double &timeStamp, ORBextractor *extractor,
@@ -379,7 +377,7 @@ Frame::Frame(const Camera_ptr &cam, const cv::Mat &imGray, const cv::Mat &imDept
     mVw.setZero();
   }
 
-  mpMutexImu = new std::mutex();
+  mpMutexImu = std::make_shared<std::mutex>();
 
   // Set no stereo fisheye information
   Nleft = -1;
@@ -509,7 +507,7 @@ Frame::Frame(const Camera_ptr &cam, const cv::Mat &imGray, const double &timeSta
     mVw.setZero();
   }
 
-  mpMutexImu = new std::mutex();
+  mpMutexImu = std::make_shared<std::mutex>();
 }
 
 void Frame::AssignFeaturesToGrid() {
@@ -1231,7 +1229,7 @@ Frame::Frame(const Camera_ptr &cam, const cv::Mat &imLeft, const cv::Mat &imRigh
 
   AssignFeaturesToGrid();
 
-  mpMutexImu = new std::mutex();
+  mpMutexImu = std::make_shared<std::mutex>();
 
   UndistortKeyPoints();
 }
diff --git a/src/Tracking.cc b/src/Tracking.cc
index 3fe5c337..89ff050f 100644
--- a/src/Tracking.cc
+++ b/src/Tracking.cc
@@ -52,7 +52,7 @@ Tracking::Tracking(System* pSys, ORBVocabulary* pVoc, const Atlas_ptr &pAtlas,
       mbVO(false),
       mpORBVocabulary(pVoc),
       mpKeyFrameDB(pKFDB),
-      mbReadyToInitializate(false),
+      mbReadyToInitialize(false),
       mpSystem(pSys),
       mpAtlas(pAtlas),
       mpLastKeyFrame(nullptr),
@@ -2248,7 +2248,7 @@ void Tracking::StereoInitialization() {
 }
 
 void Tracking::MonocularInitialization() {
-  if (!mbReadyToInitializate) {
+  if (!mbReadyToInitialize) {
     // Set Reference Frame
     if (mCurrentFrame.mvKeys.size() > 100) {
       mInitialFrame = Frame(mCurrentFrame);
@@ -2268,7 +2268,7 @@ void Tracking::MonocularInitialization() {
         mCurrentFrame.mpImuPreintegrated = mpImuPreintegratedFromLastKF;
       }
 
-      mbReadyToInitializate = true;
+      mbReadyToInitialize = true;
 
       return;
     }
@@ -2276,7 +2276,7 @@ void Tracking::MonocularInitialization() {
     if (((int)mCurrentFrame.mvKeys.size() <= 100) ||
         ((mSensor == CameraType::IMU_MONOCULAR) &&
          (mLastFrame.mTimeStamp - mInitialFrame.mTimeStamp > 1.0))) {
-      mbReadyToInitializate = false;
+      mbReadyToInitialize = false;
 
       return;
     }
@@ -2288,7 +2288,7 @@ void Tracking::MonocularInitialization() {
 
     // Check if there are enough correspondences
     if (nmatches < 100) {
-      mbReadyToInitializate = false;
+      mbReadyToInitialize = false;
       return;
     }
 
@@ -2468,14 +2468,12 @@ void Tracking::CreateMapInAtlas() {
   mbVO = false;  // Init value for know if there are enough MapPoints in the
                  // last KF
   if (mSensor == CameraType::MONOCULAR || mSensor == CameraType::IMU_MONOCULAR) {
-    mbReadyToInitializate = false;
+    mbReadyToInitialize = false;
   }
 
-  if (CameraType::isInertial(mSensor) &&
-      mpImuPreintegratedFromLastKF) {
+  if (CameraType::isInertial(mSensor) && mpImuPreintegratedFromLastKF) {
     delete mpImuPreintegratedFromLastKF;
-    mpImuPreintegratedFromLastKF =
-        new IMU::Preintegrated(IMU::Bias(), *mpImuCalib);
+    mpImuPreintegratedFromLastKF = new IMU::Preintegrated(IMU::Bias(), *mpImuCalib);
   }
 
   if (mpLastKeyFrame) mpLastKeyFrame = nullptr;
@@ -2987,8 +2985,7 @@ void Tracking::CreateNewKeyFrame() {
         new IMU::Preintegrated(pKF->GetImuBias(), pKF->mImuCalib);
   }
 
-  if (mSensor != CameraType::MONOCULAR &&
-      mSensor != CameraType::IMU_MONOCULAR)  // TODO check if incluide imu_stereo
+  if (CameraType::hasMulticam(mSensor))  // TODO check if incluide imu_stereo
   {
     mCurrentFrame.UpdatePoseMatrices();
     // cout << "create new MPs" << endl;
@@ -3506,7 +3503,7 @@ void Tracking::Reset(bool bLocMap) {
   Frame::nNextId = 0;
   mState = Tracker::NO_IMAGES_YET;
 
-  mbReadyToInitializate = false;
+  mbReadyToInitialize = false;
   mbSetInit = false;
 
   mlRelativeFramePoses.clear();
@@ -3554,7 +3551,7 @@ void Tracking::ResetActiveMap(bool bLocMap) {
   // mnLastRelocFrameId = mnLastInitFrameId;
   mState = Tracker::NO_IMAGES_YET;  // Tracker::NOT_INITIALIZED;
 
-  mbReadyToInitializate = false;
+  mbReadyToInitialize = false;
 
   list<bool> lbLost;
   // lbLost.reserve(mlbLost.size());

From d8ee34f1e55ea3a0b62d5d16b82bd3d7908d2a18 Mon Sep 17 00:00:00 2001
From: Soldann <guotl321@gmail.com>
Date: Wed, 27 Jul 2022 04:20:09 -1000
Subject: [PATCH 14/26] remove empty covisible list print

---
 src/LoopClosing.cc | 1 -
 1 file changed, 1 deletion(-)

diff --git a/src/LoopClosing.cc b/src/LoopClosing.cc
index 214143a0..85d5df93 100644
--- a/src/LoopClosing.cc
+++ b/src/LoopClosing.cc
@@ -629,7 +629,6 @@ bool LoopClosing::DetectCommonRegionsFromBoW(
     std::vector<KeyFrame*> vpCovKFi =
         pKFi->GetBestCovisibilityKeyFrames(nNumCovisibles);
     if (vpCovKFi.empty()) {
-      std::cout << "Covisible list empty" << std::endl;
       vpCovKFi.push_back(pKFi);
     } else {
       vpCovKFi.push_back(vpCovKFi[0]);

From 0e5805596486fe16f401d077b51ada8df05c9c1e Mon Sep 17 00:00:00 2001
From: DavidPetkovsek <david.petkovsek@martinrea.com>
Date: Thu, 28 Jul 2022 11:42:22 -0400
Subject: [PATCH 15/26] Removed potential memory leak candidates:
 mpORBextractorLeft, mpORBextractorRight, mpIniORBextractor, mpImuCalib,
 mpCamera, and mpCamera2 Co-authored-by: ethanseq <sequeira.ethan@gmail.com>

---
 include/Atlas.h                        |  7 +--
 include/CameraModels/GeometricCamera.h |  2 +-
 include/CameraModels/KannalaBrandt8.h  |  8 +--
 include/CameraModels/Pinhole.h         |  6 +--
 include/Frame.h                        | 14 ++---
 include/G2oTypes.h                     |  2 +-
 include/KeyFrame.h                     |  6 +--
 include/MLPnPsolver.h                  |  2 +-
 include/Map.h                          |  4 +-
 include/OptimizableTypes.h             | 10 ++--
 include/Settings.h                     |  7 +--
 include/Sim3Solver.h                   |  6 +--
 include/Tracking.h                     | 10 ++--
 src/Atlas.cc                           | 16 +++---
 src/CameraModels/KannalaBrandt8.cpp    |  8 +--
 src/CameraModels/Pinhole.cpp           |  6 +--
 src/Frame.cc                           | 46 ++++++++--------
 src/KeyFrame.cc                        |  4 +-
 src/LocalMapping.cc                    |  4 +-
 src/Map.cc                             |  4 +-
 src/ORBmatcher.cc                      |  4 +-
 src/Settings.cc                        | 30 +++++------
 src/Sim3Solver.cc                      |  4 +-
 src/Tracking.cc                        | 75 ++++++++++++--------------
 24 files changed, 143 insertions(+), 142 deletions(-)

diff --git a/include/Atlas.h b/include/Atlas.h
index 7eb2a1b9..0632a296 100644
--- a/include/Atlas.h
+++ b/include/Atlas.h
@@ -23,6 +23,7 @@
 
 #include <boost/serialization/export.hpp>
 #include <boost/serialization/vector.hpp>
+#include <boost/serialization/shared_ptr.hpp>
 #include <mutex>
 #include <set>
 #include <map>
@@ -87,8 +88,8 @@ class Atlas {
   // void EraseMapPoint(MapPoint* pMP);
   // void EraseKeyFrame(KeyFrame* pKF);
 
-  GeometricCamera* AddCamera(GeometricCamera* pCam);
-  std::vector<GeometricCamera*> GetAllCameras();
+  std::shared_ptr<GeometricCamera> AddCamera(std::shared_ptr<GeometricCamera> pCam);
+  std::vector<std::shared_ptr<GeometricCamera>> GetAllCameras();
 
   /* All methods without Map pointer work on current map */
   void SetReferenceMapPoints(const std::vector<MapPoint*>& vpMPs);
@@ -146,7 +147,7 @@ class Atlas {
 
   Map* mpCurrentMap;
 
-  std::vector<GeometricCamera*> mvpCameras;
+  std::vector<std::shared_ptr<GeometricCamera>> mvpCameras;
 
   unsigned long int mnLastInitKFidMap;
 
diff --git a/include/CameraModels/GeometricCamera.h b/include/CameraModels/GeometricCamera.h
index 4e688bcd..e59bd8fd 100644
--- a/include/CameraModels/GeometricCamera.h
+++ b/include/CameraModels/GeometricCamera.h
@@ -77,7 +77,7 @@ class GeometricCamera {
   virtual cv::Mat toK() = 0;
   virtual Eigen::Matrix3f toK_() = 0;
 
-  virtual bool epipolarConstrain(GeometricCamera* otherCamera,
+  virtual bool epipolarConstrain(std::shared_ptr<GeometricCamera> otherCamera,
                                  const cv::KeyPoint& kp1,
                                  const cv::KeyPoint& kp2,
                                  const Eigen::Matrix3f& R12,
diff --git a/include/CameraModels/KannalaBrandt8.h b/include/CameraModels/KannalaBrandt8.h
index b5ddbbb0..79a755fb 100644
--- a/include/CameraModels/KannalaBrandt8.h
+++ b/include/CameraModels/KannalaBrandt8.h
@@ -22,7 +22,7 @@
 #pragma once
 
 #include <assert.h>
-
+#include <memory>
 #include "GeometricCamera.h"
 #include "TwoViewReconstruction.h"
 
@@ -92,12 +92,12 @@ class KannalaBrandt8 : public GeometricCamera {
   cv::Mat toK();
   Eigen::Matrix3f toK_();
 
-  bool epipolarConstrain(GeometricCamera* pCamera2, const cv::KeyPoint& kp1,
+  bool epipolarConstrain(std::shared_ptr<GeometricCamera> pCamera2, const cv::KeyPoint& kp1,
                          const cv::KeyPoint& kp2, const Eigen::Matrix3f& R12,
                          const Eigen::Vector3f& t12, const float sigmaLevel,
                          const float unc);
 
-  float TriangulateMatches(GeometricCamera* pCamera2, const cv::KeyPoint& kp1,
+  float TriangulateMatches(std::shared_ptr<GeometricCamera> pCamera2, const cv::KeyPoint& kp1,
                            const cv::KeyPoint& kp2, const Eigen::Matrix3f& R12,
                            const Eigen::Vector3f& t12, const float sigmaLevel,
                            const float unc, Eigen::Vector3f& p3D);
@@ -115,7 +115,7 @@ class KannalaBrandt8 : public GeometricCamera {
 
   float GetPrecision() { return precision; }
 
-  bool IsEqual(GeometricCamera* pCam);
+  bool IsEqual(std::shared_ptr<GeometricCamera> pCam);
 
  private:
   const float precision;
diff --git a/include/CameraModels/Pinhole.h b/include/CameraModels/Pinhole.h
index d114e260..63ad908a 100644
--- a/include/CameraModels/Pinhole.h
+++ b/include/CameraModels/Pinhole.h
@@ -22,7 +22,7 @@
 #pragma once
 
 #include <assert.h>
-
+#include <memory>
 #include "GeometricCamera.h"
 #include "TwoViewReconstruction.h"
 
@@ -81,7 +81,7 @@ class Pinhole : public GeometricCamera {
   cv::Mat toK();
   Eigen::Matrix3f toK_();
 
-  bool epipolarConstrain(GeometricCamera* pCamera2, const cv::KeyPoint& kp1,
+  bool epipolarConstrain(std::shared_ptr<GeometricCamera> pCamera2, const cv::KeyPoint& kp1,
                          const cv::KeyPoint& kp2, const Eigen::Matrix3f& R12,
                          const Eigen::Vector3f& t12, const float sigmaLevel,
                          const float unc);
@@ -97,7 +97,7 @@ class Pinhole : public GeometricCamera {
   friend std::ostream& operator<<(std::ostream& os, const Pinhole& ph);
   friend std::istream& operator>>(std::istream& os, Pinhole& ph);
 
-  bool IsEqual(GeometricCamera* pCam);
+  bool IsEqual(std::shared_ptr<GeometricCamera> pCam);
 
  private:
   // Parameters vector corresponds to
diff --git a/include/Frame.h b/include/Frame.h
index d074c3d9..8346136c 100644
--- a/include/Frame.h
+++ b/include/Frame.h
@@ -60,13 +60,13 @@ class Frame
     Frame(const Frame &frame);
 
     // Constructor for stereo cameras.
-    Frame(const Camera_ptr &cam, const cv::Mat &imLeft, const cv::Mat &imRight, const double &timeStamp, ORBextractor* extractorLeft, ORBextractor* extractorRight, ORBVocabulary* voc, cv::Mat &K, cv::Mat &distCoef, const float &bf, const float &thDepth, GeometricCamera* pCamera, const std::string &pNameFile, int pnNumDataset, Frame* pPrevF = nullptr, const IMU::Calib &ImuCalib = IMU::Calib());
+    Frame(const Camera_ptr &cam, const cv::Mat &imLeft, const cv::Mat &imRight, const double &timeStamp, const std::shared_ptr<ORBextractor> &extractorLeft, const std::shared_ptr<ORBextractor> &extractorRight, ORBVocabulary* voc, cv::Mat &K, cv::Mat &distCoef, const float &bf, const float &thDepth, std::shared_ptr<GeometricCamera> pCamera, const std::string &pNameFile, int pnNumDataset, Frame* pPrevF = nullptr, const IMU::Calib &ImuCalib = IMU::Calib());
 
     // Constructor for RGB-D cameras.
-    Frame(const Camera_ptr &cam, const cv::Mat &imGray, const cv::Mat &imDepth, const double &timeStamp, ORBextractor* extractor,ORBVocabulary* voc, cv::Mat &K, cv::Mat &distCoef, const float &bf, const float &thDepth, GeometricCamera* pCamera, const std::string &pNameFile, int pnNumDataset, Frame* pPrevF = nullptr, const IMU::Calib &ImuCalib = IMU::Calib());
+    Frame(const Camera_ptr &cam, const cv::Mat &imGray, const cv::Mat &imDepth, const double &timeStamp, const std::shared_ptr<ORBextractor> &extractor,ORBVocabulary* voc, cv::Mat &K, cv::Mat &distCoef, const float &bf, const float &thDepth, std::shared_ptr<GeometricCamera> pCamera, const std::string &pNameFile, int pnNumDataset, Frame* pPrevF = nullptr, const IMU::Calib &ImuCalib = IMU::Calib());
 
     // Constructor for Monocular cameras.
-    Frame(const Camera_ptr &cam, const cv::Mat &imGray, const double &timeStamp, ORBextractor* extractor,ORBVocabulary* voc, GeometricCamera* pCamera, cv::Mat &distCoef, const float &bf, const float &thDepth, const std::string &pNameFile, int pnNumDataset,  Frame* pPrevF = nullptr, const IMU::Calib &ImuCalib = IMU::Calib());
+    Frame(const Camera_ptr &cam, const cv::Mat &imGray, const double &timeStamp, const std::shared_ptr<ORBextractor> &extractor,ORBVocabulary* voc, std::shared_ptr<GeometricCamera> pCamera, cv::Mat &distCoef, const float &bf, const float &thDepth, const std::string &pNameFile, int pnNumDataset,  Frame* pPrevF = nullptr, const IMU::Calib &ImuCalib = IMU::Calib());
 
     // Destructor
     virtual ~Frame();
@@ -194,7 +194,9 @@ class Frame
     ORBVocabulary* mpORBvocabulary;
 
     // Feature extractor. The right is used only in the stereo case.
-    ORBextractor* mpORBextractorLeft, *mpORBextractorRight;
+    std::shared_ptr<ORBextractor> mpORBextractorLeft;
+    std::shared_ptr<ORBextractor> mpORBextractorRight;
+
 
     // Frame timestamp.
     double mTimeStamp;
@@ -325,7 +327,7 @@ class Frame
 
 public:
     Camera_ptr camera; 
-    GeometricCamera* mpCamera, *mpCamera2;
+    std::shared_ptr<GeometricCamera> mpCamera, mpCamera2;
 
     //Number of KeyPoints extracted in the left and right images
     int Nleft, Nright;
@@ -345,7 +347,7 @@ class Frame
     //Grid for the right image
     std::vector<std::size_t> mGridRight[FRAME_GRID_COLS][FRAME_GRID_ROWS];
 
-    Frame(const Camera_ptr &cam, const cv::Mat &imLeft, const cv::Mat &imRight, const double &timeStamp, ORBextractor* extractorLeft, ORBextractor* extractorRight, ORBVocabulary* voc, cv::Mat &K, cv::Mat &distCoef, const float &bf, const float &thDepth, GeometricCamera* pCamera, GeometricCamera* pCamera2, const std::string &pNameFile, int pnNumDataset, Sophus::SE3f& Tlr,Frame* pPrevF = nullptr, const IMU::Calib &ImuCalib = IMU::Calib());
+    Frame(const Camera_ptr &cam, const cv::Mat &imLeft, const cv::Mat &imRight, const double &timeStamp, const std::shared_ptr<ORBextractor> &extractorLeft, const std::shared_ptr<ORBextractor> &extractorRight, ORBVocabulary* voc, cv::Mat &K, cv::Mat &distCoef, const float &bf, const float &thDepth, std::shared_ptr<GeometricCamera> pCamera, std::shared_ptr<GeometricCamera> pCamera2, const std::string &pNameFile, int pnNumDataset, Sophus::SE3f& Tlr,Frame* pPrevF = nullptr, const IMU::Calib &ImuCalib = IMU::Calib());
 
     //Stereo fisheye
     void ComputeStereoFishEyeMatches();
diff --git a/include/G2oTypes.h b/include/G2oTypes.h
index 6ab79193..2e12e29f 100644
--- a/include/G2oTypes.h
+++ b/include/G2oTypes.h
@@ -99,7 +99,7 @@ class ImuCamPose
     std::vector<Eigen::Matrix3d> Rcb, Rbc;
     std::vector<Eigen::Vector3d> tcb, tbc;
     double bf;
-    std::vector<GeometricCamera*> pCamera;
+    std::vector<std::shared_ptr<GeometricCamera>> pCamera;
 
     // For posegraph 4DoF
     Eigen::Matrix3d Rwb0;
diff --git a/include/KeyFrame.h b/include/KeyFrame.h
index 1f7f34af..a3e65cec 100644
--- a/include/KeyFrame.h
+++ b/include/KeyFrame.h
@@ -293,10 +293,10 @@ class KeyFrame {
                              float& v);
 
   void PreSave(set<KeyFrame*>& spKF, set<MapPoint*>& spMP,
-               set<GeometricCamera*>& spCam);
+               set<shared_ptr<GeometricCamera>>& spCam);
   void PostLoad(map<long unsigned int, KeyFrame*>& mpKFid,
                 map<long unsigned int, MapPoint*>& mpMPid,
-                map<unsigned int, GeometricCamera*>& mpCamId);
+                map<unsigned int, std::shared_ptr<GeometricCamera>>& mpCamId);
 
   void SetORBVocabulary(ORBVocabulary* pORBVoc);
   void SetKeyFrameDatabase(KeyFrameDatabase* pKFDB);
@@ -500,7 +500,7 @@ class KeyFrame {
   std::mutex mMutexMap;
 
  public:
-  GeometricCamera *mpCamera, *mpCamera2;
+  std::shared_ptr<GeometricCamera> mpCamera, mpCamera2;
 
   // Indexes of stereo observations correspondences
   std::vector<int> mvLeftToRightMatch, mvRightToLeftMatch;
diff --git a/include/MLPnPsolver.h b/include/MLPnPsolver.h
index aefdea5b..a160db1c 100644
--- a/include/MLPnPsolver.h
+++ b/include/MLPnPsolver.h
@@ -237,7 +237,7 @@ class MLPnPsolver {
   // th*th*sigma(level)*sigma(level)
   vector<float> mvMaxError;
 
-  GeometricCamera* mpCamera;
+  std::shared_ptr<GeometricCamera> mpCamera;
 };
 
 }  // namespace ORB_SLAM3
diff --git a/include/Map.h b/include/Map.h
index bf9dd77f..04b6a9df 100644
--- a/include/Map.h
+++ b/include/Map.h
@@ -129,8 +129,8 @@ class Map
 
     unsigned int GetLowerKFID();
 
-    void PreSave(std::set<GeometricCamera*> &spCams);
-    void PostLoad(KeyFrameDatabase* pKFDB, ORBVocabulary* pORBVoc/*, map<long unsigned int, KeyFrame*>& mpKeyFrameId*/, map<unsigned int, GeometricCamera*> &mpCams);
+    void PreSave(std::set<std::shared_ptr<GeometricCamera>> &spCams);
+    void PostLoad(KeyFrameDatabase* pKFDB, ORBVocabulary* pORBVoc/*, map<long unsigned int, KeyFrame*>& mpKeyFrameId*/, map<unsigned int, std::shared_ptr<GeometricCamera>> &mpCams);
 
     void printReprojectionError(list<KeyFrame*> &lpLocalWindowKFs, KeyFrame* mpCurrentKF, string &name, string &name_folder);
 
diff --git a/include/OptimizableTypes.h b/include/OptimizableTypes.h
index eb9210f8..a0edcccd 100644
--- a/include/OptimizableTypes.h
+++ b/include/OptimizableTypes.h
@@ -57,7 +57,7 @@ class EdgeSE3ProjectXYZOnlyPose
   virtual void linearizeOplus();
 
   Eigen::Vector3d Xw;
-  GeometricCamera* pCamera;
+  std::shared_ptr<GeometricCamera> pCamera;
 };
 
 class EdgeSE3ProjectXYZOnlyPoseToBody
@@ -87,7 +87,7 @@ class EdgeSE3ProjectXYZOnlyPoseToBody
   virtual void linearizeOplus();
 
   Eigen::Vector3d Xw;
-  GeometricCamera* pCamera;
+  std::shared_ptr<GeometricCamera> pCamera;
 
   g2o::SE3Quat mTrl;
 };
@@ -123,7 +123,7 @@ class EdgeSE3ProjectXYZ
 
   virtual void linearizeOplus();
 
-  GeometricCamera* pCamera;
+  std::shared_ptr<GeometricCamera> pCamera;
 };
 
 class EdgeSE3ProjectXYZToBody
@@ -158,7 +158,7 @@ class EdgeSE3ProjectXYZToBody
 
   virtual void linearizeOplus();
 
-  GeometricCamera* pCamera;
+  std::shared_ptr<GeometricCamera> pCamera;
   g2o::SE3Quat mTrl;
 };
 
@@ -180,7 +180,7 @@ class VertexSim3Expmap : public g2o::BaseVertex<7, g2o::Sim3> {
     setEstimate(s * estimate());
   }
 
-  GeometricCamera *pCamera1, *pCamera2;
+  std::shared_ptr<GeometricCamera> pCamera1, pCamera2;
 
   bool _fix_scale;
 };
diff --git a/include/Settings.h b/include/Settings.h
index ce38db2a..3a369530 100644
--- a/include/Settings.h
+++ b/include/Settings.h
@@ -30,6 +30,7 @@
 #include <unistd.h>
 
 #include <string>
+#include <memory>
 
 #include "CameraModels/GeometricCamera.h"
 
@@ -65,8 +66,8 @@ class Settings {
    * Getter methods
    */
   CameraType cameraType() const { return cameraType_; }
-  GeometricCamera* camera1() { return calibration1_; }
-  GeometricCamera* camera2() { return calibration2_; }
+  std::shared_ptr<GeometricCamera> camera1() { return calibration1_; }
+  std::shared_ptr<GeometricCamera> camera2() { return calibration2_; }
   cv::Mat camera1DistortionCoef() {
     return cv::Mat(vPinHoleDistorsion1_.size(), 1, CV_32F,
                    vPinHoleDistorsion1_.data());
@@ -168,7 +169,7 @@ class Settings {
   /*
    * Visual stuff
    */
-  GeometricCamera *calibration1_, *calibration2_;  // Camera calibration
+  std::shared_ptr<GeometricCamera> calibration1_, calibration2_;  // Camera calibration
   GeometricCamera *originalCalib1_, *originalCalib2_;
   std::vector<float> vPinHoleDistorsion1_, vPinHoleDistorsion2_;
 
diff --git a/include/Sim3Solver.h b/include/Sim3Solver.h
index d6d1e219..cbfefb15 100644
--- a/include/Sim3Solver.h
+++ b/include/Sim3Solver.h
@@ -62,10 +62,10 @@ class Sim3Solver {
 
   void Project(const std::vector<Eigen::Vector3f> &vP3Dw,
                std::vector<Eigen::Vector2f> &vP2D, Eigen::Matrix4f Tcw,
-               GeometricCamera *pCamera);
+               std::shared_ptr<GeometricCamera> pCamera);
   void FromCameraToImage(const std::vector<Eigen::Vector3f> &vP3Dc,
                          std::vector<Eigen::Vector2f> &vP2D,
-                         GeometricCamera *pCamera);
+                         std::shared_ptr<GeometricCamera> pCamera);
 
  protected:
   // KeyFrames and matches
@@ -131,7 +131,7 @@ class Sim3Solver {
   // cv::Mat mK1;
   // cv::Mat mK2;
 
-  GeometricCamera *pCamera1, *pCamera2;
+  std::shared_ptr<GeometricCamera> pCamera1, pCamera2;
 };
 
 }  // namespace ORB_SLAM3
diff --git a/include/Tracking.h b/include/Tracking.h
index dde6d595..4b53ceaa 100644
--- a/include/Tracking.h
+++ b/include/Tracking.h
@@ -233,7 +233,7 @@ class Tracking {
   std::mutex mMutexImuQueue;
 
   // Imu calibration parameters
-  IMU::Calib* mpImuCalib;
+  std::shared_ptr<IMU::Calib> mpImuCalib;
 
   // Last Bias Estimation (at keyframe creation)
   IMU::Bias mLastBias;
@@ -250,8 +250,9 @@ class Tracking {
   LoopClosing* mpLoopClosing;
 
   // ORB
-  ORBextractor *mpORBextractorLeft, *mpORBextractorRight;
-  ORBextractor* mpIniORBextractor;
+  std::shared_ptr<ORBextractor> mpORBextractorLeft;
+  std::shared_ptr<ORBextractor> mpORBextractorRight;
+  std::shared_ptr<ORBextractor> mpIniORBextractor;
 
   // BoW
   ORBVocabulary* mpORBVocabulary;
@@ -337,7 +338,8 @@ class Tracking {
   double mTime_LocalMapTrack;
   double mTime_NewKF_Dec;
 
-  GeometricCamera *mpCamera, *mpCamera2;
+  std::shared_ptr<GeometricCamera> mpCamera;
+  std::shared_ptr<GeometricCamera> mpCamera2;
 
   int initID, lastID;
 
diff --git a/src/Atlas.cc b/src/Atlas.cc
index 18601312..d289832d 100644
--- a/src/Atlas.cc
+++ b/src/Atlas.cc
@@ -94,23 +94,23 @@ void Atlas::AddMapPoint(MapPoint* pMP) {
   pMapMP->AddMapPoint(pMP);
 }
 
-GeometricCamera* Atlas::AddCamera(GeometricCamera* pCam) {
+std::shared_ptr<GeometricCamera> Atlas::AddCamera(std::shared_ptr<GeometricCamera> pCam) {
   // Check if the camera already exists
   bool bAlreadyInMap = false;
   int index_cam = -1;
   for (size_t i = 0; i < mvpCameras.size(); ++i) {
-    GeometricCamera* pCam_i = mvpCameras[i];
+    std::shared_ptr<GeometricCamera> pCam_i = mvpCameras[i];
     if (!pCam) std::cout << "Not pCam" << std::endl;
     if (!pCam_i) std::cout << "Not pCam_i" << std::endl;
     if (pCam->GetType() != pCam_i->GetType()) continue;
 
     if (pCam->GetType() == GeometricCamera::CAM_PINHOLE) {
-      if (((Pinhole*)pCam_i)->IsEqual(pCam)) {
+      if (reinterpret_pointer_cast<Pinhole>(pCam_i)->IsEqual(pCam)) {
         bAlreadyInMap = true;
         index_cam = i;
       }
     } else if (pCam->GetType() == GeometricCamera::CAM_FISHEYE) {
-      if (((KannalaBrandt8*)pCam_i)->IsEqual(pCam)) {
+      if (reinterpret_pointer_cast<KannalaBrandt8>(pCam_i)->IsEqual(pCam)) {
         bAlreadyInMap = true;
         index_cam = i;
       }
@@ -125,7 +125,7 @@ GeometricCamera* Atlas::AddCamera(GeometricCamera* pCam) {
   }
 }
 
-std::vector<GeometricCamera*> Atlas::GetAllCameras() { return mvpCameras; }
+std::vector<std::shared_ptr<GeometricCamera>> Atlas::GetAllCameras() { return mvpCameras; }
 
 void Atlas::SetReferenceMapPoints(const std::vector<MapPoint*>& vpMPs) {
   unique_lock<mutex> lock(mMutexAtlas);
@@ -262,7 +262,7 @@ void Atlas::PreSave() {
   };
   std::copy(mspMaps.begin(), mspMaps.end(), std::back_inserter(mvpBackupMaps));
   sort(mvpBackupMaps.begin(), mvpBackupMaps.end(), compFunctor());
-  std::set<GeometricCamera*> spCams(mvpCameras.begin(), mvpCameras.end());
+  std::set<std::shared_ptr<GeometricCamera>> spCams(mvpCameras.begin(), mvpCameras.end());
 
   for (Map* pMi : mvpBackupMaps) {
     if (!pMi || pMi->IsBad()) continue;
@@ -278,8 +278,8 @@ void Atlas::PreSave() {
 }
 
 void Atlas::PostLoad() {
-  map<unsigned int, GeometricCamera*> mpCams;
-  for (GeometricCamera* pCam : mvpCameras) {
+  map<unsigned int, std::shared_ptr<GeometricCamera>> mpCams;
+  for (std::shared_ptr<GeometricCamera> pCam : mvpCameras) {
     mpCams[pCam->GetId()] = pCam;
   }
 
diff --git a/src/CameraModels/KannalaBrandt8.cpp b/src/CameraModels/KannalaBrandt8.cpp
index 739f4774..4c923aad 100644
--- a/src/CameraModels/KannalaBrandt8.cpp
+++ b/src/CameraModels/KannalaBrandt8.cpp
@@ -227,7 +227,7 @@ Eigen::Matrix3f KannalaBrandt8::toK_() {
 }
 
 bool KannalaBrandt8::epipolarConstrain(
-    GeometricCamera *pCamera2, const cv::KeyPoint &kp1, const cv::KeyPoint &kp2,
+    std::shared_ptr<GeometricCamera> pCamera2, const cv::KeyPoint &kp1, const cv::KeyPoint &kp2,
     const Eigen::Matrix3f &R12, const Eigen::Vector3f &t12,
     const float sigmaLevel, const float unc) {
   Eigen::Vector3f p3D;
@@ -321,7 +321,7 @@ bool KannalaBrandt8::matchAndtriangulate(
 }
 
 float KannalaBrandt8::TriangulateMatches(
-    GeometricCamera *pCamera2, const cv::KeyPoint &kp1, const cv::KeyPoint &kp2,
+    std::shared_ptr<GeometricCamera> pCamera2, const cv::KeyPoint &kp1, const cv::KeyPoint &kp2,
     const Eigen::Matrix3f &R12, const Eigen::Vector3f &t12,
     const float sigmaLevel, const float unc, Eigen::Vector3f &p3D) {
   Eigen::Vector3f r1 = this->unprojectEig(kp1.pt);
@@ -427,10 +427,10 @@ void KannalaBrandt8::Triangulate(const cv::Point2f &p1, const cv::Point2f &p2,
   x3D = x3Dh.head(3) / x3Dh(3);
 }
 
-bool KannalaBrandt8::IsEqual(GeometricCamera *pCam) {
+bool KannalaBrandt8::IsEqual(std::shared_ptr<GeometricCamera> pCam) {
   if (pCam->GetType() != GeometricCamera::CAM_FISHEYE) return false;
 
-  KannalaBrandt8 *pKBCam = (KannalaBrandt8 *)pCam;
+  std::shared_ptr<KannalaBrandt8> pKBCam = std::static_pointer_cast<KannalaBrandt8>(pCam);
 
   if (abs(precision - pKBCam->GetPrecision()) > 1e-6) return false;
 
diff --git a/src/CameraModels/Pinhole.cpp b/src/CameraModels/Pinhole.cpp
index 04a75d68..51f741c5 100644
--- a/src/CameraModels/Pinhole.cpp
+++ b/src/CameraModels/Pinhole.cpp
@@ -110,7 +110,7 @@ Eigen::Matrix3f Pinhole::toK_() {
   return K;
 }
 
-bool Pinhole::epipolarConstrain(GeometricCamera *pCamera2,
+bool Pinhole::epipolarConstrain(std::shared_ptr<GeometricCamera> pCamera2,
                                 const cv::KeyPoint &kp1,
                                 const cv::KeyPoint &kp2,
                                 const Eigen::Matrix3f &R12,
@@ -154,10 +154,10 @@ std::istream &operator>>(std::istream &is, Pinhole &ph) {
   return is;
 }
 
-bool Pinhole::IsEqual(GeometricCamera *pCam) {
+bool Pinhole::IsEqual(std::shared_ptr<GeometricCamera> pCam) {
   if (pCam->GetType() != GeometricCamera::CAM_PINHOLE) return false;
 
-  Pinhole *pPinholeCam = (Pinhole *)pCam;
+  std::shared_ptr<Pinhole> pPinholeCam = std::static_pointer_cast<Pinhole>(pCam);
 
   if (size() != pPinholeCam->size()) return false;
 
diff --git a/src/Frame.cc b/src/Frame.cc
index 75592ed8..335dbe19 100644
--- a/src/Frame.cc
+++ b/src/Frame.cc
@@ -149,10 +149,10 @@ Frame::Frame(const Frame &frame)
 }
 
 Frame::Frame(const Camera_ptr &cam, const cv::Mat &imLeft, const cv::Mat &imRight,
-             const double &timeStamp, ORBextractor *extractorLeft,
-             ORBextractor *extractorRight, ORBVocabulary *voc, cv::Mat &K,
+             const double &timeStamp, const std::shared_ptr<ORBextractor> &extractorLeft,
+             const std::shared_ptr<ORBextractor> &extractorRight, ORBVocabulary *voc, cv::Mat &K,
              cv::Mat &distCoef, const float &bf, const float &thDepth,
-             GeometricCamera *pCamera, const std::string &pNameFile, int pnNumDataset,
+             std::shared_ptr<GeometricCamera> pCamera, const std::string &pNameFile, int pnNumDataset,
              Frame *pPrevF, const IMU::Calib &ImuCalib)
     : mpcpi(nullptr),
       mbHasPose(false),
@@ -278,9 +278,9 @@ Frame::Frame(const Camera_ptr &cam, const cv::Mat &imLeft, const cv::Mat &imRigh
 Frame::~Frame(){}
 
 Frame::Frame(const Camera_ptr &cam, const cv::Mat &imGray, const cv::Mat &imDepth,
-             const double &timeStamp, ORBextractor *extractor,
+             const double &timeStamp, const std::shared_ptr<ORBextractor> &extractor,
              ORBVocabulary *voc, cv::Mat &K, cv::Mat &distCoef, const float &bf,
-             const float &thDepth, GeometricCamera *pCamera,
+             const float &thDepth, std::shared_ptr<GeometricCamera> pCamera,
              const std::string &pNameFile, int pnNumDataset, Frame *pPrevF,
              const IMU::Calib &ImuCalib)
     : mpcpi(nullptr),
@@ -392,8 +392,8 @@ Frame::Frame(const Camera_ptr &cam, const cv::Mat &imGray, const cv::Mat &imDept
 }
 
 Frame::Frame(const Camera_ptr &cam, const cv::Mat &imGray, const double &timeStamp,
-             ORBextractor *extractor, ORBVocabulary *voc,
-             GeometricCamera *pCamera, cv::Mat &distCoef, const float &bf,
+             const std::shared_ptr<ORBextractor> &extractor, ORBVocabulary *voc,
+             std::shared_ptr<GeometricCamera> pCamera, cv::Mat &distCoef, const float &bf,
              const float &thDepth, const std::string &pNameFile, int pnNumDataset,
              Frame *pPrevF, const IMU::Calib &ImuCalib)
     : mpcpi(nullptr),
@@ -403,8 +403,8 @@ Frame::Frame(const Camera_ptr &cam, const cv::Mat &imGray, const double &timeSta
       mpORBextractorLeft(extractor),
       mpORBextractorRight(nullptr),
       mTimeStamp(timeStamp),
-      mK(static_cast<Pinhole *>(pCamera)->toK()),
-      mK_(static_cast<Pinhole *>(pCamera)->toK_()),
+      mK(reinterpret_pointer_cast<Pinhole>(pCamera)->toK()),
+      mK_(reinterpret_pointer_cast<Pinhole>(pCamera)->toK_()),
       mDistCoef(distCoef.clone()),
       mbf(bf),
       mThDepth(thDepth),
@@ -458,10 +458,10 @@ Frame::Frame(const Camera_ptr &cam, const cv::Mat &imGray, const double &timeSta
     mfGridElementHeightInv = static_cast<float>(FRAME_GRID_ROWS) /
                              static_cast<float>(mnMaxY - mnMinY);
 
-    fx = static_cast<Pinhole *>(mpCamera)->toK().at<float>(0, 0);
-    fy = static_cast<Pinhole *>(mpCamera)->toK().at<float>(1, 1);
-    cx = static_cast<Pinhole *>(mpCamera)->toK().at<float>(0, 2);
-    cy = static_cast<Pinhole *>(mpCamera)->toK().at<float>(1, 2);
+    fx = reinterpret_pointer_cast<Pinhole>(mpCamera)->toK().at<float>(0, 0);
+    fy = reinterpret_pointer_cast<Pinhole>(mpCamera)->toK().at<float>(1, 1);
+    cx = reinterpret_pointer_cast<Pinhole>(mpCamera)->toK().at<float>(0, 2);
+    cy = reinterpret_pointer_cast<Pinhole>(mpCamera)->toK().at<float>(1, 2);
     invfx = 1.0f / fx;
     invfy = 1.0f / fy;
 
@@ -854,7 +854,7 @@ void Frame::UndistortKeyPoints() {
 
   // Undistort points
   mat = mat.reshape(2);
-  cv::undistortPoints(mat, mat, static_cast<Pinhole *>(mpCamera)->toK(),
+  cv::undistortPoints(mat, mat, reinterpret_pointer_cast<Pinhole>(mpCamera)->toK(),
                       mDistCoef, cv::Mat(), mK);
   mat = mat.reshape(1);
 
@@ -881,7 +881,7 @@ void Frame::ComputeImageBounds(const cv::Mat &imLeft) {
     mat.at<float>(3, 1) = imLeft.rows;
 
     mat = mat.reshape(2);
-    cv::undistortPoints(mat, mat, static_cast<Pinhole *>(mpCamera)->toK(),
+    cv::undistortPoints(mat, mat, reinterpret_pointer_cast<Pinhole>(mpCamera)->toK(),
                         mDistCoef, cv::Mat(), mK);
     mat = mat.reshape(1);
 
@@ -1103,10 +1103,10 @@ void Frame::setIntegrated() {
 }
 
 Frame::Frame(const Camera_ptr &cam, const cv::Mat &imLeft, const cv::Mat &imRight,
-             const double &timeStamp, ORBextractor *extractorLeft,
-             ORBextractor *extractorRight, ORBVocabulary *voc, cv::Mat &K,
+             const double &timeStamp, const std::shared_ptr<ORBextractor> &extractorLeft,
+             const std::shared_ptr<ORBextractor> &extractorRight, ORBVocabulary *voc, cv::Mat &K,
              cv::Mat &distCoef, const float &bf, const float &thDepth,
-             GeometricCamera *pCamera, GeometricCamera *pCamera2,
+             std::shared_ptr<GeometricCamera> pCamera, std::shared_ptr<GeometricCamera> pCamera2,
              const std::string &pNameFile, int pnNumDataset,
              Sophus::SE3f &Tlr, Frame *pPrevF, const IMU::Calib &ImuCalib)
     : mpcpi(nullptr),
@@ -1155,12 +1155,12 @@ Frame::Frame(const Camera_ptr &cam, const cv::Mat &imLeft, const cv::Mat &imRigh
       std::chrono::steady_clock::now();
 #endif
   auto leftFut = camera->queueLeft(std::bind(&Frame::ExtractORB, this, true, imLeft,
-                    static_cast<KannalaBrandt8 *>(mpCamera)->mvLappingArea[0],
-                    static_cast<KannalaBrandt8 *>(mpCamera)->mvLappingArea[1]));
+                    reinterpret_pointer_cast<KannalaBrandt8>(mpCamera)->mvLappingArea[0],
+                    reinterpret_pointer_cast<KannalaBrandt8>(mpCamera)->mvLappingArea[1]));
   auto rightFut = camera->queueRight(std::bind(
       &Frame::ExtractORB, this, false, imRight,
-      static_cast<KannalaBrandt8 *>(mpCamera2)->mvLappingArea[0],
-      static_cast<KannalaBrandt8 *>(mpCamera2)->mvLappingArea[1]));
+      reinterpret_pointer_cast<KannalaBrandt8>(mpCamera2)->mvLappingArea[0],
+      reinterpret_pointer_cast<KannalaBrandt8>(mpCamera2)->mvLappingArea[1]));
   if(!leftFut.get() || !rightFut.get()) return;
 #ifdef REGISTER_TIMES
   std::chrono::steady_clock::time_point time_EndExtORB =
@@ -1271,7 +1271,7 @@ void Frame::ComputeStereoFishEyeMatches() {
           sigma1 = mvLevelSigma2[mvKeys[(*it)[0].queryIdx + monoLeft].octave],
           sigma2 =
               mvLevelSigma2[mvKeysRight[(*it)[0].trainIdx + monoRight].octave];
-      float depth = static_cast<KannalaBrandt8 *>(mpCamera)->TriangulateMatches(
+      float depth = reinterpret_pointer_cast<KannalaBrandt8>(mpCamera)->TriangulateMatches(
           mpCamera2, mvKeys[(*it)[0].queryIdx + monoLeft],
           mvKeysRight[(*it)[0].trainIdx + monoRight], mRlr, mtlr, sigma1,
           sigma2, p3D);
diff --git a/src/KeyFrame.cc b/src/KeyFrame.cc
index ef3ad145..c97385a9 100644
--- a/src/KeyFrame.cc
+++ b/src/KeyFrame.cc
@@ -857,7 +857,7 @@ void KeyFrame::UpdateMap(Map *pMap) {
 }
 
 void KeyFrame::PreSave(set<KeyFrame *> &spKF, set<MapPoint *> &spMP,
-                       set<GeometricCamera *> &spCam) {
+                       set<std::shared_ptr<GeometricCamera>> &spCam) {
   // Save the id of each MapPoint in this KF, there can be null
   // pointer in the vector
   mvBackupMapPointsId.clear();
@@ -932,7 +932,7 @@ void KeyFrame::PreSave(set<KeyFrame *> &spKF, set<MapPoint *> &spMP,
 
 void KeyFrame::PostLoad(map<long unsigned int, KeyFrame *> &mpKFid,
                         map<long unsigned int, MapPoint *> &mpMPid,
-                        map<unsigned int, GeometricCamera *> &mpCamId) {
+                        map<unsigned int, std::shared_ptr<GeometricCamera>> &mpCamId) {
   // Rebuild the empty variables
 
   // Pose
diff --git a/src/LocalMapping.cc b/src/LocalMapping.cc
index 1f48aaa8..72fc2134 100644
--- a/src/LocalMapping.cc
+++ b/src/LocalMapping.cc
@@ -448,8 +448,8 @@ void LocalMapping::CreateNewMapPoints() {
 
     KeyFrame* pKF2 = vpNeighKFs[i];
 
-    GeometricCamera *pCamera1 = mpCurrentKeyFrame->mpCamera,
-                    *pCamera2 = pKF2->mpCamera;
+    std::shared_ptr<GeometricCamera> pCamera1 = mpCurrentKeyFrame->mpCamera,
+                    pCamera2 = pKF2->mpCamera;
 
     // Check first that baseline is not too short
     Eigen::Vector3f Ow2 = pKF2->GetCameraCenter();
diff --git a/src/Map.cc b/src/Map.cc
index 92c7b87e..b6eab939 100644
--- a/src/Map.cc
+++ b/src/Map.cc
@@ -319,7 +319,7 @@ void Map::SetLastMapChange(int currentChangeId) {
   mnMapChangeNotified = currentChangeId;
 }
 
-void Map::PreSave(std::set<GeometricCamera*>& spCams) {
+void Map::PreSave(std::set<std::shared_ptr<GeometricCamera>>& spCams) {
   int nMPWithoutObs = 0;
 
   std::set<MapPoint*> tmp_mspMapPoints;
@@ -386,7 +386,7 @@ void Map::PostLoad(
     KeyFrameDatabase* pKFDB,
     ORBVocabulary*
         pORBVoc /*, map<long unsigned int, KeyFrame*>& mpKeyFrameId*/,
-    map<unsigned int, GeometricCamera*>& mpCams) {
+    map<unsigned int, std::shared_ptr<GeometricCamera>>& mpCams) {
   std::copy(mvpBackupMapPoints.begin(), mvpBackupMapPoints.end(),
             std::inserter(mspMapPoints, mspMapPoints.begin()));
   std::copy(mvpBackupKeyFrames.begin(), mvpBackupKeyFrames.end(),
diff --git a/src/ORBmatcher.cc b/src/ORBmatcher.cc
index d3f84a51..7fd764f7 100644
--- a/src/ORBmatcher.cc
+++ b/src/ORBmatcher.cc
@@ -838,7 +838,7 @@ int ORBmatcher::SearchForTriangulation(
   Eigen::Matrix3f R12;  // for fastest computation
   Eigen::Vector3f t12;  // for fastest computation
 
-  GeometricCamera *pCamera1 = pKF1->mpCamera, *pCamera2 = pKF2->mpCamera;
+  std::shared_ptr<GeometricCamera> pCamera1 = pKF1->mpCamera, pCamera2 = pKF2->mpCamera;
 
   if (!pKF1->mpCamera2 && !pKF2->mpCamera2) {
     T12 = T1w * Tw2;
@@ -1043,7 +1043,7 @@ int ORBmatcher::SearchForTriangulation(
 
 int ORBmatcher::Fuse(KeyFrame *pKF, const vector<MapPoint *> &vpMapPoints,
                      const float th, const bool bRight) {
-  GeometricCamera *pCamera;
+  std::shared_ptr<GeometricCamera> pCamera;
   Sophus::SE3f Tcw;
   Eigen::Vector3f Ow;
 
diff --git a/src/Settings.cc b/src/Settings.cc
index abf93b15..8d4c5b8f 100644
--- a/src/Settings.cc
+++ b/src/Settings.cc
@@ -207,7 +207,7 @@ void Settings::readCamera1(cv::FileStorage& fSettings) {
 
     vCalibration = {fx, fy, cx, cy};
 
-    calibration1_ = new Pinhole(vCalibration);
+    calibration1_ = std::make_shared<Pinhole>(vCalibration);
     originalCalib1_ = new Pinhole(vCalibration);
 
     // Check if it is a distorted PinHole
@@ -247,7 +247,7 @@ void Settings::readCamera1(cv::FileStorage& fSettings) {
 
     vCalibration = {fx, fy, cx, cy};
 
-    calibration1_ = new Pinhole(vCalibration);
+    calibration1_ = std::make_shared<Pinhole>(vCalibration);
     originalCalib1_ = new Pinhole(vCalibration);
 
     // Rectified images are assumed to be ideal PinHole images (no distortion)
@@ -267,7 +267,7 @@ void Settings::readCamera1(cv::FileStorage& fSettings) {
 
     vCalibration = {fx, fy, cx, cy, k0, k1, k2, k3};
 
-    calibration1_ = new KannalaBrandt8(vCalibration);
+    calibration1_ = std::make_shared<KannalaBrandt8>(vCalibration);
     originalCalib1_ = new KannalaBrandt8(vCalibration);
 
     if (sensor_ == ORB_SLAM3::CameraType::STEREO || sensor_ == ORB_SLAM3::CameraType::IMU_STEREO) {
@@ -277,7 +277,7 @@ void Settings::readCamera1(cv::FileStorage& fSettings) {
           readParameter<int>(fSettings, "Camera1.overlappingEnd", found);
       std::vector<int> vOverlapping = {colBegin, colEnd};
 
-      static_cast<KannalaBrandt8*>(calibration1_)->mvLappingArea = vOverlapping;
+      std::reinterpret_pointer_cast<KannalaBrandt8>(calibration1_)->mvLappingArea = vOverlapping;
     }
   } else {
     std::cerr << "Error: " << cameraModel << " not known" << std::endl;
@@ -299,7 +299,7 @@ void Settings::readCamera2(cv::FileStorage& fSettings) {
 
     vCalibration = {fx, fy, cx, cy};
 
-    calibration2_ = new Pinhole(vCalibration);
+    calibration2_ = std::make_shared<Pinhole>(vCalibration);
     originalCalib2_ = new Pinhole(vCalibration);
 
     // Check if it is a distorted PinHole
@@ -336,7 +336,7 @@ void Settings::readCamera2(cv::FileStorage& fSettings) {
 
     vCalibration = {fx, fy, cx, cy, k0, k1, k2, k3};
 
-    calibration2_ = new KannalaBrandt8(vCalibration);
+    calibration2_ = std::make_shared<KannalaBrandt8>(vCalibration);
     originalCalib2_ = new KannalaBrandt8(vCalibration);
 
     int colBegin =
@@ -344,7 +344,7 @@ void Settings::readCamera2(cv::FileStorage& fSettings) {
     int colEnd = readParameter<int>(fSettings, "Camera2.overlappingEnd", found);
     std::vector<int> vOverlapping = {colBegin, colEnd};
 
-    static_cast<KannalaBrandt8*>(calibration2_)->mvLappingArea = vOverlapping;
+    std::reinterpret_pointer_cast<KannalaBrandt8>(calibration2_)->mvLappingArea = vOverlapping;
   }
 
   // Load stereo extrinsic calibration
@@ -420,14 +420,14 @@ void Settings::readImageInfo(cv::FileStorage& fSettings) {
             calibration2_->getParameter(2) * scaleColFactor, 2);
 
         if (cameraType_ == KannalaBrandt) {
-          static_cast<KannalaBrandt8*>(calibration1_)->mvLappingArea[0] *=
+          std::reinterpret_pointer_cast<KannalaBrandt8>(calibration1_)->mvLappingArea[0] *=
               scaleColFactor;
-          static_cast<KannalaBrandt8*>(calibration1_)->mvLappingArea[1] *=
+          std::reinterpret_pointer_cast<KannalaBrandt8>(calibration1_)->mvLappingArea[1] *=
               scaleColFactor;
 
-          static_cast<KannalaBrandt8*>(calibration2_)->mvLappingArea[0] *=
+          std::reinterpret_pointer_cast<KannalaBrandt8>(calibration2_)->mvLappingArea[0] *=
               scaleColFactor;
-          static_cast<KannalaBrandt8*>(calibration2_)->mvLappingArea[1] *=
+          std::reinterpret_pointer_cast<KannalaBrandt8>(calibration2_)->mvLappingArea[1] *=
               scaleColFactor;
         }
       }
@@ -519,9 +519,9 @@ void Settings::readOtherParameters(cv::FileStorage& fSettings) {
 
 void Settings::precomputeRectificationMaps() {
   // Precompute rectification maps, new calibrations, ...
-  cv::Mat K1 = static_cast<Pinhole*>(calibration1_)->toK();
+  cv::Mat K1 = std::reinterpret_pointer_cast<Pinhole>(calibration1_)->toK();
   K1.convertTo(K1, CV_64F);
-  cv::Mat K2 = static_cast<Pinhole*>(calibration2_)->toK();
+  cv::Mat K2 = std::reinterpret_pointer_cast<Pinhole>(calibration2_)->toK();
   K2.convertTo(K2, CV_64F);
 
   cv::Mat cvTlr;
@@ -651,9 +651,9 @@ std::ostream& operator<<(std::ostream& output, const Settings& settings) {
 
     if (settings.cameraType_ == Settings::KannalaBrandt) {
       auto vOverlapping1 =
-          static_cast<KannalaBrandt8*>(settings.calibration1_)->mvLappingArea;
+          std::reinterpret_pointer_cast<KannalaBrandt8>(settings.calibration1_)->mvLappingArea;
       auto vOverlapping2 =
-          static_cast<KannalaBrandt8*>(settings.calibration2_)->mvLappingArea;
+          std::reinterpret_pointer_cast<KannalaBrandt8>(settings.calibration2_)->mvLappingArea;
       output << "\t-Camera 1 overlapping area: [ " << vOverlapping1[0] << " , "
              << vOverlapping1[1] << " ]" << std::endl;
       output << "\t-Camera 2 overlapping area: [ " << vOverlapping2[0] << " , "
diff --git a/src/Sim3Solver.cc b/src/Sim3Solver.cc
index d8b48df2..0fc7c291 100644
--- a/src/Sim3Solver.cc
+++ b/src/Sim3Solver.cc
@@ -425,7 +425,7 @@ float Sim3Solver::GetEstimatedScale() { return mBestScale; }
 
 void Sim3Solver::Project(const vector<Eigen::Vector3f> &vP3Dw,
                          vector<Eigen::Vector2f> &vP2D, Eigen::Matrix4f Tcw,
-                         GeometricCamera *pCamera) {
+                         std::shared_ptr<GeometricCamera> pCamera) {
   Eigen::Matrix3f Rcw = Tcw.block<3, 3>(0, 0);
   Eigen::Vector3f tcw = Tcw.block<3, 1>(0, 3);
 
@@ -441,7 +441,7 @@ void Sim3Solver::Project(const vector<Eigen::Vector3f> &vP3Dw,
 
 void Sim3Solver::FromCameraToImage(const vector<Eigen::Vector3f> &vP3Dc,
                                    vector<Eigen::Vector2f> &vP2D,
-                                   GeometricCamera *pCamera) {
+                                   std::shared_ptr<GeometricCamera> pCamera) {
   vP2D.clear();
   vP2D.reserve(vP3Dc.size());
 
diff --git a/src/Tracking.cc b/src/Tracking.cc
index 89ff050f..0ca20199 100644
--- a/src/Tracking.cc
+++ b/src/Tracking.cc
@@ -104,10 +104,10 @@ Tracking::Tracking(System* pSys, ORBVocabulary* pVoc, const Atlas_ptr &pAtlas,
   mbInitWith3KFs = false;
   mnNumDataset = 0;
 
-  vector<GeometricCamera*> vpCams = mpAtlas->GetAllCameras();
+  vector<std::shared_ptr<GeometricCamera>> vpCams = mpAtlas->GetAllCameras();
   std::cout << "There are " << vpCams.size() << " cameras in the atlas"
             << std::endl;
-  for (GeometricCamera* pCam : vpCams) {
+  for (std::shared_ptr<GeometricCamera> pCam : vpCams) {
     std::cout << "Camera " << pCam->GetId();
     if (pCam->GetType() == GeometricCamera::CAM_PINHOLE) {
       std::cout << " is pinhole" << std::endl;
@@ -607,15 +607,15 @@ void Tracking::newParameterLoader(Settings* settings) {
   int fMinThFAST = settings->minThFAST();
   float fScaleFactor = settings->scaleFactor();
 
-  mpORBextractorLeft = new ORBextractor(nFeatures, fScaleFactor, nLevels,
+  mpORBextractorLeft = std::make_shared<ORBextractor>(nFeatures, fScaleFactor, nLevels,
                                         fIniThFAST, fMinThFAST);
 
   if (mSensor == CameraType::STEREO || mSensor == CameraType::IMU_STEREO)
-    mpORBextractorRight = new ORBextractor(nFeatures, fScaleFactor, nLevels,
+    mpORBextractorRight = std::make_shared<ORBextractor>(nFeatures, fScaleFactor, nLevels,
                                            fIniThFAST, fMinThFAST);
 
   if (mSensor == CameraType::MONOCULAR || mSensor == CameraType::IMU_MONOCULAR)
-    mpIniORBextractor = new ORBextractor(5 * nFeatures, fScaleFactor, nLevels,
+    mpIniORBextractor = std::make_shared<ORBextractor>(5 * nFeatures, fScaleFactor, nLevels,
                                          fIniThFAST, fMinThFAST);
 
   // IMU parameters
@@ -629,7 +629,7 @@ void Tracking::newParameterLoader(Settings* settings) {
   float Naw = settings->accWalk();
 
   const float sf = sqrt(mImuFreq);
-  mpImuCalib = new IMU::Calib(Tbc, Ng * sf, Na * sf, Ngw / sf, Naw / sf);
+  mpImuCalib = std::make_shared<IMU::Calib>(Tbc, Ng * sf, Na * sf, Ngw / sf, Naw / sf);
 
   mpImuPreintegratedFromLastKF =
       new IMU::Preintegrated(IMU::Bias(), *mpImuCalib);
@@ -744,7 +744,7 @@ bool Tracking::ParseCamParamFile(cv::FileStorage& fSettings) {
 
     vector<float> vCamCalib{fx, fy, cx, cy};
 
-    mpCamera = new Pinhole(vCamCalib);
+    mpCamera = std::make_shared<Pinhole>(vCamCalib);
 
     mpCamera = mpAtlas->AddCamera(mpCamera);
 
@@ -866,7 +866,7 @@ bool Tracking::ParseCamParamFile(cv::FileStorage& fSettings) {
       }
 
       vector<float> vCamCalib{fx, fy, cx, cy, k1, k2, k3, k4};
-      mpCamera = new KannalaBrandt8(vCamCalib);
+      mpCamera = std::make_shared<KannalaBrandt8>(vCamCalib);
       mpCamera = mpAtlas->AddCamera(mpCamera);
       std::cout << "- Camera: Fisheye" << std::endl;
       std::cout << "- Image scale: " << mImageScale << std::endl;
@@ -1037,20 +1037,20 @@ bool Tracking::ParseCamParamFile(cv::FileStorage& fSettings) {
           rightLappingEnd = rightLappingEnd * mImageScale;
         }
 
-        static_cast<KannalaBrandt8*>(mpCamera)->mvLappingArea[0] =
+        reinterpret_pointer_cast<KannalaBrandt8>(mpCamera)->mvLappingArea[0] =
             leftLappingBegin;
-        static_cast<KannalaBrandt8*>(mpCamera)->mvLappingArea[1] =
+        reinterpret_pointer_cast<KannalaBrandt8>(mpCamera)->mvLappingArea[1] =
             leftLappingEnd;
 
         vector<float> vCamCalib2{fx, fy, cx, cy, k1, k2, k3, k4};
-        mpCamera2 = new KannalaBrandt8(vCamCalib2);
+        mpCamera2 = std::make_shared<KannalaBrandt8>(vCamCalib2);
         mpCamera2 = mpAtlas->AddCamera(mpCamera2);
 
         mTlr = Converter::toSophus(cvTlr);
 
-        static_cast<KannalaBrandt8*>(mpCamera2)->mvLappingArea[0] =
+        reinterpret_pointer_cast<KannalaBrandt8>(mpCamera2)->mvLappingArea[0] =
             rightLappingBegin;
-        static_cast<KannalaBrandt8*>(mpCamera2)->mvLappingArea[1] =
+        reinterpret_pointer_cast<KannalaBrandt8>(mpCamera2)->mvLappingArea[1] =
             rightLappingEnd;
 
         std::cout << "- Camera1 Lapping: " << leftLappingBegin << ", "
@@ -1213,15 +1213,15 @@ bool Tracking::ParseORBParamFile(cv::FileStorage& fSettings) {
     return false;
   }
 
-  mpORBextractorLeft = new ORBextractor(nFeatures, fScaleFactor, nLevels,
+  mpORBextractorLeft = std::make_shared<ORBextractor>(nFeatures, fScaleFactor, nLevels,
                                         fIniThFAST, fMinThFAST);
 
   if (mSensor == CameraType::STEREO || mSensor == CameraType::IMU_STEREO)
-    mpORBextractorRight = new ORBextractor(nFeatures, fScaleFactor, nLevels,
+    mpORBextractorRight = std::make_shared<ORBextractor>(nFeatures, fScaleFactor, nLevels,
                                            fIniThFAST, fMinThFAST);
 
   if (mSensor == CameraType::MONOCULAR || mSensor == CameraType::IMU_MONOCULAR)
-    mpIniORBextractor = new ORBextractor(5 * nFeatures, fScaleFactor, nLevels,
+    mpIniORBextractor = std::make_shared<ORBextractor>(5 * nFeatures, fScaleFactor, nLevels,
                                          fIniThFAST, fMinThFAST);
 
   cout << endl << "ORB Extractor Parameters: " << endl;
@@ -1269,7 +1269,7 @@ bool Tracking::ParseIMUParamFile(cv::FileStorage& fSettings) {
   node = fSettings["IMU.Frequency"];
   if (!node.empty() && node.isInt()) {
     mImuFreq = node.operator int();
-    mImuPer = 0.001;  // 1.0 / (double) mImuFreq;
+    mImuPer = 0.001;  // 1.0 / (double) mImuFreq;     //TODO: ESTO ESTA BIEN?
   } else {
     std::cerr << "*IMU.Frequency parameter doesn't exist or is not an integer*"
               << std::endl;
@@ -1336,7 +1336,7 @@ bool Tracking::ParseIMUParamFile(cv::FileStorage& fSettings) {
   cout << "IMU accelerometer noise: " << Na << " m/s^2/sqrt(Hz)" << endl;
   cout << "IMU accelerometer walk: " << Naw << " m/s^3/sqrt(Hz)" << endl;
 
-  mpImuCalib = new IMU::Calib(Tbc, Ng * sf, Na * sf, Ngw / sf, Naw / sf);
+  mpImuCalib = std::make_shared<IMU::Calib>(Tbc, Ng * sf, Na * sf, Ngw / sf, Naw / sf);
 
   mpImuPreintegratedFromLastKF =
       new IMU::Preintegrated(IMU::Bias(), *mpImuCalib);
@@ -1528,35 +1528,30 @@ void Tracking::PreintegrateIMU() {
   }
 
   while (true) {
-    bool bSleep = false;
-    {
-      unique_lock<mutex> lock(mMutexImuQueue);
-      if (!mlQueueImuData.empty()) {
-        IMU::Point* m = &mlQueueImuData.front();
-        cout.precision(17);
-        if (m->t < mCurrentFrame.mpPrevFrame->mTimeStamp - mImuPer) {
-          mlQueueImuData.pop_front();
-        } else if (m->t < mCurrentFrame.mTimeStamp - mImuPer) {
-          mvImuFromLastFrame.push_back(*m);
-          mlQueueImuData.pop_front();
-        } else {
-          mvImuFromLastFrame.push_back(*m);
-          break;
-        }
-      } else {
-        break;
-        bSleep = true;
-      }
+    unique_lock<mutex> lock(mMutexImuQueue);
+    if(mlQueueImuData.empty()) break;
+
+    IMU::Point* m = &mlQueueImuData.front();
+    if (m->t < mCurrentFrame.mpPrevFrame->mTimeStamp - mImuPer) {
+      // m is old and therefore invalid
+      mlQueueImuData.pop_front();
+    } else if (m->t < mCurrentFrame.mTimeStamp - mImuPer) {
+      // m is valid
+      mvImuFromLastFrame.push_back(*m);
+      mlQueueImuData.pop_front();
+    } else {
+      // m is newer than the current frame
+      // mvImuFromLastFrame.push_back(*m);
+      break;
     }
-    if (bSleep) usleep(500);
   }
 
-  const int n = mvImuFromLastFrame.size() - 1;
-  if (n == 0) {
+  if (mvImuFromLastFrame.empty()) {
     cout << "Empty IMU measurements vector!!!\n";
     return;
   }
 
+  const int n = mvImuFromLastFrame.size();
   IMU::Preintegrated* pImuPreintegratedFromLastFrame =
       new IMU::Preintegrated(mLastFrame.mImuBias, mCurrentFrame.mImuCalib);
 

From 60ec46b5d6252f5676ee4d626d0b53c98f5b773f Mon Sep 17 00:00:00 2001
From: DavidPetkovsek <david.petkovsek@martinrea.com>
Date: Thu, 28 Jul 2022 13:42:04 -0400
Subject: [PATCH 16/26] Changed some params/functions to const refs/functions
 Co-authored-by: ethanseq <sequeira.ethan@gmail.com

---
 include/Atlas.h                        |  2 +-
 include/CameraModels/GeometricCamera.h | 18 +++++++++---------
 include/CameraModels/KannalaBrandt8.h  | 22 +++++++++++-----------
 include/CameraModels/Pinhole.h         | 20 ++++++++++----------
 include/Frame.h                        |  8 ++++----
 include/Sim3Solver.h                   |  4 ++--
 src/Atlas.cc                           |  2 +-
 src/CameraModels/KannalaBrandt8.cpp    | 22 +++++++++++-----------
 src/CameraModels/Pinhole.cpp           | 20 ++++++++++----------
 src/Frame.cc                           |  8 ++++----
 src/Sim3Solver.cc                      |  4 ++--
 11 files changed, 65 insertions(+), 65 deletions(-)

diff --git a/include/Atlas.h b/include/Atlas.h
index 0632a296..1494f5d5 100644
--- a/include/Atlas.h
+++ b/include/Atlas.h
@@ -88,7 +88,7 @@ class Atlas {
   // void EraseMapPoint(MapPoint* pMP);
   // void EraseKeyFrame(KeyFrame* pKF);
 
-  std::shared_ptr<GeometricCamera> AddCamera(std::shared_ptr<GeometricCamera> pCam);
+  std::shared_ptr<GeometricCamera> AddCamera(const std::shared_ptr<GeometricCamera> &pCam);
   std::vector<std::shared_ptr<GeometricCamera>> GetAllCameras();
 
   /* All methods without Map pointer work on current map */
diff --git a/include/CameraModels/GeometricCamera.h b/include/CameraModels/GeometricCamera.h
index e59bd8fd..553d63ca 100644
--- a/include/CameraModels/GeometricCamera.h
+++ b/include/CameraModels/GeometricCamera.h
@@ -54,15 +54,15 @@ class GeometricCamera {
       : mvParameters(_vParameters) {}
   ~GeometricCamera() {}
 
-  virtual cv::Point2f project(const cv::Point3f& p3D) = 0;
-  virtual Eigen::Vector2d project(const Eigen::Vector3d& v3D) = 0;
-  virtual Eigen::Vector2f project(const Eigen::Vector3f& v3D) = 0;
-  virtual Eigen::Vector2f projectMat(const cv::Point3f& p3D) = 0;
+  virtual cv::Point2f project(const cv::Point3f& p3D) const = 0;
+  virtual Eigen::Vector2d project(const Eigen::Vector3d& v3D) const = 0;
+  virtual Eigen::Vector2f project(const Eigen::Vector3f& v3D) const = 0;
+  virtual Eigen::Vector2f projectMat(const cv::Point3f& p3D) const = 0;
 
   virtual float uncertainty2(const Eigen::Matrix<double, 2, 1>& p2D) = 0;
 
-  virtual Eigen::Vector3f unprojectEig(const cv::Point2f& p2D) = 0;
-  virtual cv::Point3f unproject(const cv::Point2f& p2D) = 0;
+  virtual Eigen::Vector3f unprojectEig(const cv::Point2f& p2D) const = 0;
+  virtual cv::Point3f unproject(const cv::Point2f& p2D) const = 0;
 
   virtual Eigen::Matrix<double, 2, 3> projectJac(
       const Eigen::Vector3d& v3D) = 0;
@@ -74,10 +74,10 @@ class GeometricCamera {
                                        std::vector<cv::Point3f>& vP3D,
                                        std::vector<bool>& vbTriangulated) = 0;
 
-  virtual cv::Mat toK() = 0;
-  virtual Eigen::Matrix3f toK_() = 0;
+  virtual cv::Mat toK() const = 0;
+  virtual Eigen::Matrix3f toK_() const = 0;
 
-  virtual bool epipolarConstrain(std::shared_ptr<GeometricCamera> otherCamera,
+  virtual bool epipolarConstrain(const std::shared_ptr<GeometricCamera> &otherCamera,
                                  const cv::KeyPoint& kp1,
                                  const cv::KeyPoint& kp2,
                                  const Eigen::Matrix3f& R12,
diff --git a/include/CameraModels/KannalaBrandt8.h b/include/CameraModels/KannalaBrandt8.h
index 79a755fb..05028ac1 100644
--- a/include/CameraModels/KannalaBrandt8.h
+++ b/include/CameraModels/KannalaBrandt8.h
@@ -70,15 +70,15 @@ class KannalaBrandt8 : public GeometricCamera {
     mnType = CAM_FISHEYE;
   }
 
-  cv::Point2f project(const cv::Point3f& p3D);
-  Eigen::Vector2d project(const Eigen::Vector3d& v3D);
-  Eigen::Vector2f project(const Eigen::Vector3f& v3D);
-  Eigen::Vector2f projectMat(const cv::Point3f& p3D);
+  cv::Point2f project(const cv::Point3f& p3D) const;
+  Eigen::Vector2d project(const Eigen::Vector3d& v3D) const;
+  Eigen::Vector2f project(const Eigen::Vector3f& v3D) const;
+  Eigen::Vector2f projectMat(const cv::Point3f& p3D) const;
 
   float uncertainty2(const Eigen::Matrix<double, 2, 1>& p2D);
 
-  Eigen::Vector3f unprojectEig(const cv::Point2f& p2D);
-  cv::Point3f unproject(const cv::Point2f& p2D);
+  Eigen::Vector3f unprojectEig(const cv::Point2f& p2D) const;
+  cv::Point3f unproject(const cv::Point2f& p2D) const;
 
   Eigen::Matrix<double, 2, 3> projectJac(const Eigen::Vector3d& v3D);
 
@@ -89,15 +89,15 @@ class KannalaBrandt8 : public GeometricCamera {
                                std::vector<cv::Point3f>& vP3D,
                                std::vector<bool>& vbTriangulated);
 
-  cv::Mat toK();
-  Eigen::Matrix3f toK_();
+  cv::Mat toK() const;
+  Eigen::Matrix3f toK_() const;
 
-  bool epipolarConstrain(std::shared_ptr<GeometricCamera> pCamera2, const cv::KeyPoint& kp1,
+  bool epipolarConstrain(const std::shared_ptr<GeometricCamera> &pCamera2, const cv::KeyPoint& kp1,
                          const cv::KeyPoint& kp2, const Eigen::Matrix3f& R12,
                          const Eigen::Vector3f& t12, const float sigmaLevel,
                          const float unc);
 
-  float TriangulateMatches(std::shared_ptr<GeometricCamera> pCamera2, const cv::KeyPoint& kp1,
+  float TriangulateMatches(const std::shared_ptr<GeometricCamera> &pCamera2, const cv::KeyPoint& kp1,
                            const cv::KeyPoint& kp2, const Eigen::Matrix3f& R12,
                            const Eigen::Vector3f& t12, const float sigmaLevel,
                            const float unc, Eigen::Vector3f& p3D);
@@ -115,7 +115,7 @@ class KannalaBrandt8 : public GeometricCamera {
 
   float GetPrecision() { return precision; }
 
-  bool IsEqual(std::shared_ptr<GeometricCamera> pCam);
+  bool IsEqual(const std::shared_ptr<GeometricCamera> &pCam);
 
  private:
   const float precision;
diff --git a/include/CameraModels/Pinhole.h b/include/CameraModels/Pinhole.h
index 63ad908a..f335ee74 100644
--- a/include/CameraModels/Pinhole.h
+++ b/include/CameraModels/Pinhole.h
@@ -59,15 +59,15 @@ class Pinhole : public GeometricCamera {
     if (tvr) delete tvr;
   }
 
-  cv::Point2f project(const cv::Point3f& p3D);
-  Eigen::Vector2d project(const Eigen::Vector3d& v3D);
-  Eigen::Vector2f project(const Eigen::Vector3f& v3D);
-  Eigen::Vector2f projectMat(const cv::Point3f& p3D);
+  cv::Point2f project(const cv::Point3f& p3D) const;
+  Eigen::Vector2d project(const Eigen::Vector3d& v3D) const;
+  Eigen::Vector2f project(const Eigen::Vector3f& v3D) const;
+  Eigen::Vector2f projectMat(const cv::Point3f& p3D) const;
 
   float uncertainty2(const Eigen::Matrix<double, 2, 1>& p2D);
 
-  Eigen::Vector3f unprojectEig(const cv::Point2f& p2D);
-  cv::Point3f unproject(const cv::Point2f& p2D);
+  Eigen::Vector3f unprojectEig(const cv::Point2f& p2D) const;
+  cv::Point3f unproject(const cv::Point2f& p2D) const;
 
   Eigen::Matrix<double, 2, 3> projectJac(const Eigen::Vector3d& v3D);
 
@@ -78,10 +78,10 @@ class Pinhole : public GeometricCamera {
                                std::vector<cv::Point3f>& vP3D,
                                std::vector<bool>& vbTriangulated);
 
-  cv::Mat toK();
-  Eigen::Matrix3f toK_();
+  cv::Mat toK() const;
+  Eigen::Matrix3f toK_()const;
 
-  bool epipolarConstrain(std::shared_ptr<GeometricCamera> pCamera2, const cv::KeyPoint& kp1,
+  bool epipolarConstrain(const std::shared_ptr<GeometricCamera> &pCamera2, const cv::KeyPoint& kp1,
                          const cv::KeyPoint& kp2, const Eigen::Matrix3f& R12,
                          const Eigen::Vector3f& t12, const float sigmaLevel,
                          const float unc);
@@ -97,7 +97,7 @@ class Pinhole : public GeometricCamera {
   friend std::ostream& operator<<(std::ostream& os, const Pinhole& ph);
   friend std::istream& operator>>(std::istream& os, Pinhole& ph);
 
-  bool IsEqual(std::shared_ptr<GeometricCamera> pCam);
+  bool IsEqual(const std::shared_ptr<GeometricCamera> &pCam);
 
  private:
   // Parameters vector corresponds to
diff --git a/include/Frame.h b/include/Frame.h
index 8346136c..1d2f93be 100644
--- a/include/Frame.h
+++ b/include/Frame.h
@@ -60,13 +60,13 @@ class Frame
     Frame(const Frame &frame);
 
     // Constructor for stereo cameras.
-    Frame(const Camera_ptr &cam, const cv::Mat &imLeft, const cv::Mat &imRight, const double &timeStamp, const std::shared_ptr<ORBextractor> &extractorLeft, const std::shared_ptr<ORBextractor> &extractorRight, ORBVocabulary* voc, cv::Mat &K, cv::Mat &distCoef, const float &bf, const float &thDepth, std::shared_ptr<GeometricCamera> pCamera, const std::string &pNameFile, int pnNumDataset, Frame* pPrevF = nullptr, const IMU::Calib &ImuCalib = IMU::Calib());
+    Frame(const Camera_ptr &cam, const cv::Mat &imLeft, const cv::Mat &imRight, const double &timeStamp, const std::shared_ptr<ORBextractor> &extractorLeft, const std::shared_ptr<ORBextractor> &extractorRight, ORBVocabulary* voc, cv::Mat &K, cv::Mat &distCoef, const float &bf, const float &thDepth, const std::shared_ptr<GeometricCamera> &pCamera, const std::string &pNameFile, int pnNumDataset, Frame* pPrevF = nullptr, const IMU::Calib &ImuCalib = IMU::Calib());
 
     // Constructor for RGB-D cameras.
-    Frame(const Camera_ptr &cam, const cv::Mat &imGray, const cv::Mat &imDepth, const double &timeStamp, const std::shared_ptr<ORBextractor> &extractor,ORBVocabulary* voc, cv::Mat &K, cv::Mat &distCoef, const float &bf, const float &thDepth, std::shared_ptr<GeometricCamera> pCamera, const std::string &pNameFile, int pnNumDataset, Frame* pPrevF = nullptr, const IMU::Calib &ImuCalib = IMU::Calib());
+    Frame(const Camera_ptr &cam, const cv::Mat &imGray, const cv::Mat &imDepth, const double &timeStamp, const std::shared_ptr<ORBextractor> &extractor,ORBVocabulary* voc, cv::Mat &K, cv::Mat &distCoef, const float &bf, const float &thDepth, const std::shared_ptr<GeometricCamera> &pCamera, const std::string &pNameFile, int pnNumDataset, Frame* pPrevF = nullptr, const IMU::Calib &ImuCalib = IMU::Calib());
 
     // Constructor for Monocular cameras.
-    Frame(const Camera_ptr &cam, const cv::Mat &imGray, const double &timeStamp, const std::shared_ptr<ORBextractor> &extractor,ORBVocabulary* voc, std::shared_ptr<GeometricCamera> pCamera, cv::Mat &distCoef, const float &bf, const float &thDepth, const std::string &pNameFile, int pnNumDataset,  Frame* pPrevF = nullptr, const IMU::Calib &ImuCalib = IMU::Calib());
+    Frame(const Camera_ptr &cam, const cv::Mat &imGray, const double &timeStamp, const std::shared_ptr<ORBextractor> &extractor,ORBVocabulary* voc, const std::shared_ptr<GeometricCamera> &pCamera, cv::Mat &distCoef, const float &bf, const float &thDepth, const std::string &pNameFile, int pnNumDataset,  Frame* pPrevF = nullptr, const IMU::Calib &ImuCalib = IMU::Calib());
 
     // Destructor
     virtual ~Frame();
@@ -347,7 +347,7 @@ class Frame
     //Grid for the right image
     std::vector<std::size_t> mGridRight[FRAME_GRID_COLS][FRAME_GRID_ROWS];
 
-    Frame(const Camera_ptr &cam, const cv::Mat &imLeft, const cv::Mat &imRight, const double &timeStamp, const std::shared_ptr<ORBextractor> &extractorLeft, const std::shared_ptr<ORBextractor> &extractorRight, ORBVocabulary* voc, cv::Mat &K, cv::Mat &distCoef, const float &bf, const float &thDepth, std::shared_ptr<GeometricCamera> pCamera, std::shared_ptr<GeometricCamera> pCamera2, const std::string &pNameFile, int pnNumDataset, Sophus::SE3f& Tlr,Frame* pPrevF = nullptr, const IMU::Calib &ImuCalib = IMU::Calib());
+    Frame(const Camera_ptr &cam, const cv::Mat &imLeft, const cv::Mat &imRight, const double &timeStamp, const std::shared_ptr<ORBextractor> &extractorLeft, const std::shared_ptr<ORBextractor> &extractorRight, ORBVocabulary* voc, cv::Mat &K, cv::Mat &distCoef, const float &bf, const float &thDepth, const std::shared_ptr<GeometricCamera> &pCamera, const std::shared_ptr<GeometricCamera> &pCamera2, const std::string &pNameFile, int pnNumDataset, Sophus::SE3f& Tlr,Frame* pPrevF = nullptr, const IMU::Calib &ImuCalib = IMU::Calib());
 
     //Stereo fisheye
     void ComputeStereoFishEyeMatches();
diff --git a/include/Sim3Solver.h b/include/Sim3Solver.h
index cbfefb15..adf512bd 100644
--- a/include/Sim3Solver.h
+++ b/include/Sim3Solver.h
@@ -62,10 +62,10 @@ class Sim3Solver {
 
   void Project(const std::vector<Eigen::Vector3f> &vP3Dw,
                std::vector<Eigen::Vector2f> &vP2D, Eigen::Matrix4f Tcw,
-               std::shared_ptr<GeometricCamera> pCamera);
+               const std::shared_ptr<GeometricCamera> &pCamera);
   void FromCameraToImage(const std::vector<Eigen::Vector3f> &vP3Dc,
                          std::vector<Eigen::Vector2f> &vP2D,
-                         std::shared_ptr<GeometricCamera> pCamera);
+                         const std::shared_ptr<GeometricCamera> &pCamera);
 
  protected:
   // KeyFrames and matches
diff --git a/src/Atlas.cc b/src/Atlas.cc
index d289832d..a2ac52d0 100644
--- a/src/Atlas.cc
+++ b/src/Atlas.cc
@@ -94,7 +94,7 @@ void Atlas::AddMapPoint(MapPoint* pMP) {
   pMapMP->AddMapPoint(pMP);
 }
 
-std::shared_ptr<GeometricCamera> Atlas::AddCamera(std::shared_ptr<GeometricCamera> pCam) {
+std::shared_ptr<GeometricCamera> Atlas::AddCamera(const std::shared_ptr<GeometricCamera> &pCam) {
   // Check if the camera already exists
   bool bAlreadyInMap = false;
   int index_cam = -1;
diff --git a/src/CameraModels/KannalaBrandt8.cpp b/src/CameraModels/KannalaBrandt8.cpp
index 4c923aad..4952082e 100644
--- a/src/CameraModels/KannalaBrandt8.cpp
+++ b/src/CameraModels/KannalaBrandt8.cpp
@@ -28,7 +28,7 @@
 namespace ORB_SLAM3 {
 // BOOST_CLASS_EXPORT_GUID(KannalaBrandt8, "KannalaBrandt8")
 
-cv::Point2f KannalaBrandt8::project(const cv::Point3f &p3D) {
+cv::Point2f KannalaBrandt8::project(const cv::Point3f &p3D) const {
   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);
@@ -45,7 +45,7 @@ cv::Point2f KannalaBrandt8::project(const cv::Point3f &p3D) {
                      mvParameters[1] * r * sin(psi) + mvParameters[3]);
 }
 
-Eigen::Vector2d KannalaBrandt8::project(const Eigen::Vector3d &v3D) {
+Eigen::Vector2d KannalaBrandt8::project(const Eigen::Vector3d &v3D) const {
   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]);
@@ -65,7 +65,7 @@ Eigen::Vector2d KannalaBrandt8::project(const Eigen::Vector3d &v3D) {
   return res;
 }
 
-Eigen::Vector2f KannalaBrandt8::project(const Eigen::Vector3f &v3D) {
+Eigen::Vector2f KannalaBrandt8::project(const Eigen::Vector3f &v3D) const {
   const float x2_plus_y2 = v3D[0] * v3D[0] + v3D[1] * v3D[1];
   const float theta = atan2f(sqrtf(x2_plus_y2), v3D[2]);
   const float psi = atan2f(v3D[1], v3D[0]);
@@ -93,7 +93,7 @@ Eigen::Vector2f KannalaBrandt8::project(const Eigen::Vector3f &v3D) {
   return res;*/
 }
 
-Eigen::Vector2f KannalaBrandt8::projectMat(const cv::Point3f &p3D) {
+Eigen::Vector2f KannalaBrandt8::projectMat(const cv::Point3f &p3D) const {
   cv::Point2f point = this->project(p3D);
   return Eigen::Vector2f(point.x, point.y);
 }
@@ -108,12 +108,12 @@ float KannalaBrandt8::uncertainty2(const Eigen::Matrix<double, 2, 1> &p2D) {
   return 1.f;
 }
 
-Eigen::Vector3f KannalaBrandt8::unprojectEig(const cv::Point2f &p2D) {
+Eigen::Vector3f KannalaBrandt8::unprojectEig(const cv::Point2f &p2D) const {
   cv::Point3f ray = this->unproject(p2D);
   return Eigen::Vector3f(ray.x, ray.y, ray.z);
 }
 
-cv::Point3f KannalaBrandt8::unproject(const cv::Point2f &p2D) {
+cv::Point3f KannalaBrandt8::unproject(const cv::Point2f &p2D) const {
   // 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]);
@@ -214,12 +214,12 @@ bool KannalaBrandt8::ReconstructWithTwoViews(
                           vbTriangulated);
 }
 
-cv::Mat KannalaBrandt8::toK() {
+cv::Mat KannalaBrandt8::toK() const {
   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;
 }
-Eigen::Matrix3f KannalaBrandt8::toK_() {
+Eigen::Matrix3f KannalaBrandt8::toK_() const {
   Eigen::Matrix3f K;
   K << mvParameters[0], 0.f, mvParameters[2], 0.f, mvParameters[1],
       mvParameters[3], 0.f, 0.f, 1.f;
@@ -227,7 +227,7 @@ Eigen::Matrix3f KannalaBrandt8::toK_() {
 }
 
 bool KannalaBrandt8::epipolarConstrain(
-    std::shared_ptr<GeometricCamera> pCamera2, const cv::KeyPoint &kp1, const cv::KeyPoint &kp2,
+    const std::shared_ptr<GeometricCamera> &pCamera2, const cv::KeyPoint &kp1, const cv::KeyPoint &kp2,
     const Eigen::Matrix3f &R12, const Eigen::Vector3f &t12,
     const float sigmaLevel, const float unc) {
   Eigen::Vector3f p3D;
@@ -321,7 +321,7 @@ bool KannalaBrandt8::matchAndtriangulate(
 }
 
 float KannalaBrandt8::TriangulateMatches(
-    std::shared_ptr<GeometricCamera> pCamera2, const cv::KeyPoint &kp1, const cv::KeyPoint &kp2,
+    const std::shared_ptr<GeometricCamera> &pCamera2, const cv::KeyPoint &kp1, const cv::KeyPoint &kp2,
     const Eigen::Matrix3f &R12, const Eigen::Vector3f &t12,
     const float sigmaLevel, const float unc, Eigen::Vector3f &p3D) {
   Eigen::Vector3f r1 = this->unprojectEig(kp1.pt);
@@ -427,7 +427,7 @@ void KannalaBrandt8::Triangulate(const cv::Point2f &p1, const cv::Point2f &p2,
   x3D = x3Dh.head(3) / x3Dh(3);
 }
 
-bool KannalaBrandt8::IsEqual(std::shared_ptr<GeometricCamera> pCam) {
+bool KannalaBrandt8::IsEqual(const std::shared_ptr<GeometricCamera> &pCam) {
   if (pCam->GetType() != GeometricCamera::CAM_FISHEYE) return false;
 
   std::shared_ptr<KannalaBrandt8> pKBCam = std::static_pointer_cast<KannalaBrandt8>(pCam);
diff --git a/src/CameraModels/Pinhole.cpp b/src/CameraModels/Pinhole.cpp
index 51f741c5..e83e1948 100644
--- a/src/CameraModels/Pinhole.cpp
+++ b/src/CameraModels/Pinhole.cpp
@@ -30,12 +30,12 @@ namespace ORB_SLAM3 {
 
 long unsigned int GeometricCamera::nNextId = 0;
 
-cv::Point2f Pinhole::project(const cv::Point3f &p3D) {
+cv::Point2f Pinhole::project(const cv::Point3f &p3D) const {
   return cv::Point2f(mvParameters[0] * p3D.x / p3D.z + mvParameters[2],
                      mvParameters[1] * p3D.y / p3D.z + mvParameters[3]);
 }
 
-Eigen::Vector2d Pinhole::project(const Eigen::Vector3d &v3D) {
+Eigen::Vector2d Pinhole::project(const Eigen::Vector3d &v3D) const {
   Eigen::Vector2d res;
   res[0] = mvParameters[0] * v3D[0] / v3D[2] + mvParameters[2];
   res[1] = mvParameters[1] * v3D[1] / v3D[2] + mvParameters[3];
@@ -43,7 +43,7 @@ Eigen::Vector2d Pinhole::project(const Eigen::Vector3d &v3D) {
   return res;
 }
 
-Eigen::Vector2f Pinhole::project(const Eigen::Vector3f &v3D) {
+Eigen::Vector2f Pinhole::project(const Eigen::Vector3f &v3D) const {
   Eigen::Vector2f res;
   res[0] = mvParameters[0] * v3D[0] / v3D[2] + mvParameters[2];
   res[1] = mvParameters[1] * v3D[1] / v3D[2] + mvParameters[3];
@@ -51,7 +51,7 @@ Eigen::Vector2f Pinhole::project(const Eigen::Vector3f &v3D) {
   return res;
 }
 
-Eigen::Vector2f Pinhole::projectMat(const cv::Point3f &p3D) {
+Eigen::Vector2f Pinhole::projectMat(const cv::Point3f &p3D) const {
   cv::Point2f point = this->project(p3D);
   return Eigen::Vector2f(point.x, point.y);
 }
@@ -60,12 +60,12 @@ float Pinhole::uncertainty2(const Eigen::Matrix<double, 2, 1> &p2D) {
   return 1.0;
 }
 
-Eigen::Vector3f Pinhole::unprojectEig(const cv::Point2f &p2D) {
+Eigen::Vector3f Pinhole::unprojectEig(const cv::Point2f &p2D) const {
   return Eigen::Vector3f((p2D.x - mvParameters[2]) / mvParameters[0],
                          (p2D.y - mvParameters[3]) / mvParameters[1], 1.f);
 }
 
-cv::Point3f Pinhole::unproject(const cv::Point2f &p2D) {
+cv::Point3f Pinhole::unproject(const cv::Point2f &p2D) const {
   return cv::Point3f((p2D.x - mvParameters[2]) / mvParameters[0],
                      (p2D.y - mvParameters[3]) / mvParameters[1], 1.f);
 }
@@ -97,20 +97,20 @@ bool Pinhole::ReconstructWithTwoViews(const std::vector<cv::KeyPoint> &vKeys1,
                           vbTriangulated);
 }
 
-cv::Mat Pinhole::toK() {
+cv::Mat Pinhole::toK() const {
   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;
 }
 
-Eigen::Matrix3f Pinhole::toK_() {
+Eigen::Matrix3f Pinhole::toK_() const {
   Eigen::Matrix3f K;
   K << mvParameters[0], 0.f, mvParameters[2], 0.f, mvParameters[1],
       mvParameters[3], 0.f, 0.f, 1.f;
   return K;
 }
 
-bool Pinhole::epipolarConstrain(std::shared_ptr<GeometricCamera> pCamera2,
+bool Pinhole::epipolarConstrain(const std::shared_ptr<GeometricCamera> &pCamera2,
                                 const cv::KeyPoint &kp1,
                                 const cv::KeyPoint &kp2,
                                 const Eigen::Matrix3f &R12,
@@ -154,7 +154,7 @@ std::istream &operator>>(std::istream &is, Pinhole &ph) {
   return is;
 }
 
-bool Pinhole::IsEqual(std::shared_ptr<GeometricCamera> pCam) {
+bool Pinhole::IsEqual(const std::shared_ptr<GeometricCamera> &pCam) {
   if (pCam->GetType() != GeometricCamera::CAM_PINHOLE) return false;
 
   std::shared_ptr<Pinhole> pPinholeCam = std::static_pointer_cast<Pinhole>(pCam);
diff --git a/src/Frame.cc b/src/Frame.cc
index 335dbe19..9c61ad9e 100644
--- a/src/Frame.cc
+++ b/src/Frame.cc
@@ -152,7 +152,7 @@ Frame::Frame(const Camera_ptr &cam, const cv::Mat &imLeft, const cv::Mat &imRigh
              const double &timeStamp, const std::shared_ptr<ORBextractor> &extractorLeft,
              const std::shared_ptr<ORBextractor> &extractorRight, ORBVocabulary *voc, cv::Mat &K,
              cv::Mat &distCoef, const float &bf, const float &thDepth,
-             std::shared_ptr<GeometricCamera> pCamera, const std::string &pNameFile, int pnNumDataset,
+             const std::shared_ptr<GeometricCamera> &pCamera, const std::string &pNameFile, int pnNumDataset,
              Frame *pPrevF, const IMU::Calib &ImuCalib)
     : mpcpi(nullptr),
       mbHasPose(false),
@@ -280,7 +280,7 @@ Frame::~Frame(){}
 Frame::Frame(const Camera_ptr &cam, const cv::Mat &imGray, const cv::Mat &imDepth,
              const double &timeStamp, const std::shared_ptr<ORBextractor> &extractor,
              ORBVocabulary *voc, cv::Mat &K, cv::Mat &distCoef, const float &bf,
-             const float &thDepth, std::shared_ptr<GeometricCamera> pCamera,
+             const float &thDepth, const std::shared_ptr<GeometricCamera> &pCamera,
              const std::string &pNameFile, int pnNumDataset, Frame *pPrevF,
              const IMU::Calib &ImuCalib)
     : mpcpi(nullptr),
@@ -393,7 +393,7 @@ Frame::Frame(const Camera_ptr &cam, const cv::Mat &imGray, const cv::Mat &imDept
 
 Frame::Frame(const Camera_ptr &cam, const cv::Mat &imGray, const double &timeStamp,
              const std::shared_ptr<ORBextractor> &extractor, ORBVocabulary *voc,
-             std::shared_ptr<GeometricCamera> pCamera, cv::Mat &distCoef, const float &bf,
+             const std::shared_ptr<GeometricCamera> &pCamera, cv::Mat &distCoef, const float &bf,
              const float &thDepth, const std::string &pNameFile, int pnNumDataset,
              Frame *pPrevF, const IMU::Calib &ImuCalib)
     : mpcpi(nullptr),
@@ -1106,7 +1106,7 @@ Frame::Frame(const Camera_ptr &cam, const cv::Mat &imLeft, const cv::Mat &imRigh
              const double &timeStamp, const std::shared_ptr<ORBextractor> &extractorLeft,
              const std::shared_ptr<ORBextractor> &extractorRight, ORBVocabulary *voc, cv::Mat &K,
              cv::Mat &distCoef, const float &bf, const float &thDepth,
-             std::shared_ptr<GeometricCamera> pCamera, std::shared_ptr<GeometricCamera> pCamera2,
+             const std::shared_ptr<GeometricCamera> &pCamera, const std::shared_ptr<GeometricCamera> &pCamera2,
              const std::string &pNameFile, int pnNumDataset,
              Sophus::SE3f &Tlr, Frame *pPrevF, const IMU::Calib &ImuCalib)
     : mpcpi(nullptr),
diff --git a/src/Sim3Solver.cc b/src/Sim3Solver.cc
index 0fc7c291..c1c37fe4 100644
--- a/src/Sim3Solver.cc
+++ b/src/Sim3Solver.cc
@@ -425,7 +425,7 @@ float Sim3Solver::GetEstimatedScale() { return mBestScale; }
 
 void Sim3Solver::Project(const vector<Eigen::Vector3f> &vP3Dw,
                          vector<Eigen::Vector2f> &vP2D, Eigen::Matrix4f Tcw,
-                         std::shared_ptr<GeometricCamera> pCamera) {
+                         const std::shared_ptr<GeometricCamera> &pCamera) {
   Eigen::Matrix3f Rcw = Tcw.block<3, 3>(0, 0);
   Eigen::Vector3f tcw = Tcw.block<3, 1>(0, 3);
 
@@ -441,7 +441,7 @@ void Sim3Solver::Project(const vector<Eigen::Vector3f> &vP3Dw,
 
 void Sim3Solver::FromCameraToImage(const vector<Eigen::Vector3f> &vP3Dc,
                                    vector<Eigen::Vector2f> &vP2D,
-                                   std::shared_ptr<GeometricCamera> pCamera) {
+                                   const std::shared_ptr<GeometricCamera> &pCamera) {
   vP2D.clear();
   vP2D.reserve(vP3Dc.size());
 

From 55cb4c282d14f7a7ccabf8f2b568b9e431231b4b Mon Sep 17 00:00:00 2001
From: Ryan <ryan.zazo@hotmail.com>
Date: Thu, 28 Jul 2022 15:49:06 -0400
Subject: [PATCH 17/26] reverted the change to make it work

---
 src/Tracking.cc | 41 +++++++++++++++++++++++------------------
 1 file changed, 23 insertions(+), 18 deletions(-)

diff --git a/src/Tracking.cc b/src/Tracking.cc
index 0ca20199..4cf9f44d 100644
--- a/src/Tracking.cc
+++ b/src/Tracking.cc
@@ -1528,30 +1528,35 @@ void Tracking::PreintegrateIMU() {
   }
 
   while (true) {
-    unique_lock<mutex> lock(mMutexImuQueue);
-    if(mlQueueImuData.empty()) break;
-
-    IMU::Point* m = &mlQueueImuData.front();
-    if (m->t < mCurrentFrame.mpPrevFrame->mTimeStamp - mImuPer) {
-      // m is old and therefore invalid
-      mlQueueImuData.pop_front();
-    } else if (m->t < mCurrentFrame.mTimeStamp - mImuPer) {
-      // m is valid
-      mvImuFromLastFrame.push_back(*m);
-      mlQueueImuData.pop_front();
-    } else {
-      // m is newer than the current frame
-      // mvImuFromLastFrame.push_back(*m);
-      break;
+    bool bSleep = false;
+    {
+      unique_lock<mutex> lock(mMutexImuQueue);
+      if (!mlQueueImuData.empty()) {
+        IMU::Point* m = &mlQueueImuData.front();
+        cout.precision(17);
+        if (m->t < mCurrentFrame.mpPrevFrame->mTimeStamp - mImuPer) {
+          mlQueueImuData.pop_front();
+        } else if (m->t < mCurrentFrame.mTimeStamp - mImuPer) {
+          mvImuFromLastFrame.push_back(*m);
+          mlQueueImuData.pop_front();
+        } else {
+          mvImuFromLastFrame.push_back(*m);
+          break;
+        }
+      } else {
+        break;
+        bSleep = true;
+      }
     }
+    if (bSleep) usleep(500);
   }
 
-  if (mvImuFromLastFrame.empty()) {
+  const int n = mvImuFromLastFrame.size() - 1;
+  if (n == 0) {
     cout << "Empty IMU measurements vector!!!\n";
     return;
   }
 
-  const int n = mvImuFromLastFrame.size();
   IMU::Preintegrated* pImuPreintegratedFromLastFrame =
       new IMU::Preintegrated(mLastFrame.mImuBias, mCurrentFrame.mImuCalib);
 
@@ -1700,7 +1705,7 @@ void Tracking::Track() {
       mlQueueImuData.clear();
       CreateMapInAtlas();
       return;
-    } 
+    }
   }
 
   if (CameraType::isInertial(mSensor) && mpLastKeyFrame)

From 52de2fff000d2ae872c2443188eeefba44a963c5 Mon Sep 17 00:00:00 2001
From: seque <sequeira.ethan@gmail.com>
Date: Thu, 28 Jul 2022 16:26:02 -0400
Subject: [PATCH 18/26] reverted the revert to the change and uncommented line

---
 src/Tracking.cc | 40 ++++++++++++++++++----------------------
 1 file changed, 18 insertions(+), 22 deletions(-)

diff --git a/src/Tracking.cc b/src/Tracking.cc
index 4cf9f44d..21bcaf10 100644
--- a/src/Tracking.cc
+++ b/src/Tracking.cc
@@ -1528,38 +1528,34 @@ void Tracking::PreintegrateIMU() {
   }
 
   while (true) {
-    bool bSleep = false;
-    {
-      unique_lock<mutex> lock(mMutexImuQueue);
-      if (!mlQueueImuData.empty()) {
-        IMU::Point* m = &mlQueueImuData.front();
-        cout.precision(17);
-        if (m->t < mCurrentFrame.mpPrevFrame->mTimeStamp - mImuPer) {
-          mlQueueImuData.pop_front();
-        } else if (m->t < mCurrentFrame.mTimeStamp - mImuPer) {
-          mvImuFromLastFrame.push_back(*m);
-          mlQueueImuData.pop_front();
-        } else {
-          mvImuFromLastFrame.push_back(*m);
-          break;
-        }
-      } else {
-        break;
-        bSleep = true;
-      }
+    unique_lock<mutex> lock(mMutexImuQueue);
+    if(mlQueueImuData.empty()) break;
+
+    IMU::Point* m = &mlQueueImuData.front();
+    if (m->t < mCurrentFrame.mpPrevFrame->mTimeStamp - mImuPer) {
+      // m is old and therefore invalid
+      mlQueueImuData.pop_front();
+    } else if (m->t < mCurrentFrame.mTimeStamp - mImuPer) {
+      // m is valid
+      mvImuFromLastFrame.push_back(*m);
+      mlQueueImuData.pop_front();
+    } else {
+      // m is newer than the current frame
+      mvImuFromLastFrame.push_back(*m);
+      break;
     }
-    if (bSleep) usleep(500);
   }
 
-  const int n = mvImuFromLastFrame.size() - 1;
-  if (n == 0) {
+  if (mvImuFromLastFrame.empty()) {
     cout << "Empty IMU measurements vector!!!\n";
     return;
   }
 
+  const int n = mvImuFromLastFrame.size();
   IMU::Preintegrated* pImuPreintegratedFromLastFrame =
       new IMU::Preintegrated(mLastFrame.mImuBias, mCurrentFrame.mImuCalib);
 
+
   for (int i = 0; i < n; i++) {
     float tstep;
     Eigen::Vector3f acc, angVel;

From c094fe5371cb657bfa2fb64113d2a575a9954d9d Mon Sep 17 00:00:00 2001
From: DavidPetkovsek <david.petkovsek@martinrea.com>
Date: Fri, 29 Jul 2022 13:50:48 -0400
Subject: [PATCH 19/26] Checked optimizer.cc for potential memory leaks and
 converted frame mpcpi to shared pointer Co-authored-by: ethanseq
 <sequeira.ethan@gmail.com>

---
 include/Frame.h    | 2 +-
 include/G2oTypes.h | 2 +-
 src/G2oTypes.cc    | 2 +-
 src/Optimizer.cc   | 7 +++----
 4 files changed, 6 insertions(+), 7 deletions(-)

diff --git a/include/Frame.h b/include/Frame.h
index 1d2f93be..639e819a 100644
--- a/include/Frame.h
+++ b/include/Frame.h
@@ -122,7 +122,7 @@ class Frame
     // Backprojects a keypoint (if stereo/depth info available) into 3D world coordinates.
     bool UnprojectStereo(const int &i, Eigen::Vector3f &x3D);
 
-    ConstraintPoseImu* mpcpi;
+    std::shared_ptr<ConstraintPoseImu> mpcpi;
 
     bool imuIsPreintegrated();
     void setIntegrated();
diff --git a/include/G2oTypes.h b/include/G2oTypes.h
index 2e12e29f..6086d622 100644
--- a/include/G2oTypes.h
+++ b/include/G2oTypes.h
@@ -731,7 +731,7 @@ class EdgePriorPoseImu : public g2o::BaseMultiEdge<15,Vector15d>
 {
 public:
         
-        EdgePriorPoseImu(ConstraintPoseImu* c);
+        EdgePriorPoseImu(std::shared_ptr<ConstraintPoseImu> c);
 
         virtual bool read(std::istream& is){return false;}
         virtual bool write(std::ostream& os) const{return false;}
diff --git a/src/G2oTypes.cc b/src/G2oTypes.cc
index 7e275e82..ba1d26ef 100644
--- a/src/G2oTypes.cc
+++ b/src/G2oTypes.cc
@@ -726,7 +726,7 @@ void EdgeInertialGS::linearizeOplus() {
       Rbw1 * (VP2->estimate().twb - VP1->estimate().twb - VV1->estimate() * dt);
 }
 
-EdgePriorPoseImu::EdgePriorPoseImu(ConstraintPoseImu* c) {
+EdgePriorPoseImu::EdgePriorPoseImu(std::shared_ptr<ConstraintPoseImu> c) {
   resize(4);
   Rwb = c->Rwb;
   twb = c->twb;
diff --git a/src/Optimizer.cc b/src/Optimizer.cc
index 9f7d9a0d..c0a849ac 100644
--- a/src/Optimizer.cc
+++ b/src/Optimizer.cc
@@ -4750,13 +4750,13 @@ int Optimizer::PoseInertialOptimizationLastKeyFrame(Frame* pFrame,
   }
 
   pFrame->mpcpi =
-      new ConstraintPoseImu(VP->estimate().Rwb, VP->estimate().twb,
+      std::make_shared<ConstraintPoseImu>(VP->estimate().Rwb, VP->estimate().twb,
                             VV->estimate(), VG->estimate(), VA->estimate(), H);
 
   return nInitialCorrespondences - nBad;
 }
 
-static ConstraintPoseImu* oldMpcpi = nullptr;
+static std::shared_ptr<ConstraintPoseImu> oldMpcpi = nullptr;
 
 int Optimizer::PoseInertialOptimizationLastFrame(Frame* pFrame, bool bRecInit) {
   g2o::SparseOptimizer optimizer;
@@ -5147,14 +5147,13 @@ int Optimizer::PoseInertialOptimizationLastFrame(Frame* pFrame, bool bRecInit) {
 
   H = Marginalize(H, 0, 14);
 
-  pFrame->mpcpi = new ConstraintPoseImu(
+  pFrame->mpcpi = std::make_shared<ConstraintPoseImu>(
       VP->estimate().Rwb, VP->estimate().twb, VV->estimate(), VG->estimate(),
       VA->estimate(), H.block<15, 15>(15, 15));
   if (oldMpcpi == pFp->mpcpi) {
     std::cerr << "\033[22;34mSAME MPCPI" << std::endl;
   } else {
     oldMpcpi = pFp->mpcpi;
-    delete pFp->mpcpi;
     pFp->mpcpi = nullptr;
   }
   return nInitialCorrespondences - nBad;

From 2a528de686032491e57ccc7535b33d86204137f7 Mon Sep 17 00:00:00 2001
From: DavidPetkovsek <david.petkovsek@martinrea.com>
Date: Tue, 2 Aug 2022 13:31:33 -0400
Subject: [PATCH 20/26] Changed Map to be a shared pointer Co-authored:
 ethanseq <sequeira.ethan@gmail.com>

---
 include/Atlas.h            |   16 +-
 include/KeyFrame.h         |    8 +-
 include/KeyFrameDatabase.h |    4 +-
 include/LocalMapping.h     |    4 +-
 include/LoopClosing.h      |    8 +-
 include/Map.h              |    6 +-
 include/MapPoint.h         |   12 +-
 include/Optimizer.h        |   20 +-
 include/System.h           |    4 +-
 include/Tracking.h         |    2 +-
 output.txt                 | 1441 ++++++++++++++++++++++++++++++++++++
 src/Atlas.cc               |   50 +-
 src/KeyFrame.cc            |    6 +-
 src/KeyFrameDatabase.cc    |    4 +-
 src/LocalMapping.cc        |    2 +-
 src/LoopClosing.cc         |   18 +-
 src/Map.cc                 |   12 +-
 src/MapDrawer.cc           |    8 +-
 src/MapPoint.cc            |   10 +-
 src/Optimizer.cc           |   30 +-
 src/System.cc              |   16 +-
 src/Tracking.cc            |   10 +-
 22 files changed, 1559 insertions(+), 132 deletions(-)
 create mode 100644 output.txt

diff --git a/include/Atlas.h b/include/Atlas.h
index 1494f5d5..25086845 100644
--- a/include/Atlas.h
+++ b/include/Atlas.h
@@ -78,7 +78,7 @@ class Atlas {
   ~Atlas();
 
   void CreateNewMap();
-  void ChangeMap(Map* pMap);
+  void ChangeMap(std::shared_ptr<Map> pMap);
 
   unsigned long int GetLastInitKFid();
 
@@ -104,7 +104,7 @@ class Atlas {
   std::vector<MapPoint*> GetAllMapPoints();
   std::vector<MapPoint*> GetReferenceMapPoints();
 
-  std::vector<Map*> GetAllMaps();
+  std::vector<std::shared_ptr<Map>> GetAllMaps();
 
   int CountMaps();
 
@@ -112,9 +112,9 @@ class Atlas {
 
   void clearAtlas();
 
-  Map* GetCurrentMap(System* sys = nullptr);
+  std::shared_ptr<Map> GetCurrentMap(System* sys = nullptr);
 
-  void SetMapBad(Map* pMap);
+  void SetMapBad(std::shared_ptr<Map> pMap);
   void RemoveBadMaps();
 
   bool isInertial();
@@ -139,13 +139,13 @@ class Atlas {
   long unsigned int GetNumLivedMP();
 
  protected:
-  std::set<Map*> mspMaps;
-  std::set<Map*> mspBadMaps;
+  std::set<std::shared_ptr<Map>> mspMaps;
+  std::set<std::shared_ptr<Map>> mspBadMaps;
   // Its necessary change the container from set to vector because libboost 1.58
   // and Ubuntu 16.04 have an error with this cointainer
-  std::vector<Map*> mvpBackupMaps;
+  std::vector<std::shared_ptr<Map>> mvpBackupMaps;
 
-  Map* mpCurrentMap;
+  std::shared_ptr<Map> mpCurrentMap;
 
   std::vector<std::shared_ptr<GeometricCamera>> mvpCameras;
 
diff --git a/include/KeyFrame.h b/include/KeyFrame.h
index a3e65cec..b4cabf46 100644
--- a/include/KeyFrame.h
+++ b/include/KeyFrame.h
@@ -190,7 +190,7 @@ class KeyFrame {
  public:
   
   KeyFrame();
-  KeyFrame(Frame& F, Map* pMap, KeyFrameDatabase* pKFDB);
+  KeyFrame(Frame& F, std::shared_ptr<Map> pMap, KeyFrameDatabase* pKFDB);
 
   // Pose functions
   void SetPose(const Sophus::SE3f& Tcw);
@@ -278,8 +278,8 @@ class KeyFrame {
     return pKF1->mnId < pKF2->mnId;
   }
 
-  Map* GetMap();
-  void UpdateMap(Map* pMap);
+  std::shared_ptr<Map> GetMap();
+  void UpdateMap(std::shared_ptr<Map> pMap);
 
   void SetNewBias(const IMU::Bias& b);
   Eigen::Vector3f GetGyroBias();
@@ -480,7 +480,7 @@ class KeyFrame {
 
   float mHalfBaseline;  // Only for visualization
 
-  Map* mpMap;
+  std::shared_ptr<Map> mpMap;
 
   // Backup variables for inertial
   long long int mBackupPrevKFId;
diff --git a/include/KeyFrameDatabase.h b/include/KeyFrameDatabase.h
index 00c1a584..52b46faf 100644
--- a/include/KeyFrameDatabase.h
+++ b/include/KeyFrameDatabase.h
@@ -59,7 +59,7 @@ class KeyFrameDatabase {
   void erase(KeyFrame* pKF);
 
   void clear();
-  void clearMap(Map* pMap);
+  void clearMap(std::shared_ptr<Map> pMap);
 
   // Loop Detection(DEPRECATED)
   std::vector<KeyFrame*> DetectLoopCandidates(KeyFrame* pKF, float minScore);
@@ -75,7 +75,7 @@ class KeyFrameDatabase {
                              int nNumCandidates);
 
   // Relocalization
-  std::vector<KeyFrame*> DetectRelocalizationCandidates(Frame* F, Map* pMap);
+  std::vector<KeyFrame*> DetectRelocalizationCandidates(Frame* F, std::shared_ptr<Map> pMap);
 
   void PreSave();
   void PostLoad(map<long unsigned int, KeyFrame*> mpKFid);
diff --git a/include/LocalMapping.h b/include/LocalMapping.h
index 06e7c37e..77a9a6f4 100644
--- a/include/LocalMapping.h
+++ b/include/LocalMapping.h
@@ -57,7 +57,7 @@ class LocalMapping {
   // Thread Synch
   void RequestStop();
   void RequestReset();
-  void RequestResetActiveMap(Map* pMap);
+  void RequestResetActiveMap(std::shared_ptr<Map> pMap);
   bool Stop();
   void Release();
   bool isStopped();
@@ -144,7 +144,7 @@ class LocalMapping {
   void ResetIfRequested();
   bool mbResetRequested;
   bool mbResetRequestedActiveMap;
-  Map* mpMapToReset;
+  std::shared_ptr<Map> mpMapToReset;
   std::mutex mMutexReset;
 
   bool CheckFinish();
diff --git a/include/LoopClosing.h b/include/LoopClosing.h
index 6d987403..35ba0980 100644
--- a/include/LoopClosing.h
+++ b/include/LoopClosing.h
@@ -64,10 +64,10 @@ class LoopClosing
     void InsertKeyFrame(KeyFrame *pKF);
 
     void RequestReset();
-    void RequestResetActiveMap(Map* pMap);
+    void RequestResetActiveMap(std::shared_ptr<Map> pMap);
 
     // This function will run in a separate thread
-    void RunGlobalBundleAdjustment(Map* pActiveMap, unsigned long nLoopKF);
+    void RunGlobalBundleAdjustment(std::shared_ptr<Map> pActiveMap, unsigned long nLoopKF);
 
     bool isRunningGBA(){
         unique_lock<std::mutex> lock(mMutexGBA);
@@ -145,7 +145,7 @@ class LoopClosing
     void ResetIfRequested();
     bool mbResetRequested;
     bool mbResetActiveMapRequested;
-    Map* mpMapToReset;
+    std::shared_ptr<Map> mpMapToReset;
     std::mutex mMutexReset;
 
     bool CheckFinish();
@@ -182,7 +182,7 @@ class LoopClosing
     g2o::Sim3 mg2oScw;
 
     //-------
-    Map* mpLastMap;
+    std::shared_ptr<Map> mpLastMap;
 
     bool mbLoopDetected;
     int mnLoopNumCoincidences;
diff --git a/include/Map.h b/include/Map.h
index 04b6a9df..a8a27b98 100644
--- a/include/Map.h
+++ b/include/Map.h
@@ -25,7 +25,7 @@
 #include <set>
 #include <pangolin/pangolin.h>
 #include <mutex>
-
+#include <memory>
 #include <boost/serialization/base_object.hpp>
 
 
@@ -129,8 +129,8 @@ class Map
 
     unsigned int GetLowerKFID();
 
-    void PreSave(std::set<std::shared_ptr<GeometricCamera>> &spCams);
-    void PostLoad(KeyFrameDatabase* pKFDB, ORBVocabulary* pORBVoc/*, map<long unsigned int, KeyFrame*>& mpKeyFrameId*/, map<unsigned int, std::shared_ptr<GeometricCamera>> &mpCams);
+    void PreSave(std::set<std::shared_ptr<GeometricCamera>> &spCams, std::shared_ptr<Map> sharedMap);
+    void PostLoad(KeyFrameDatabase* pKFDB, ORBVocabulary* pORBVoc/*, map<long unsigned int, KeyFrame*>& mpKeyFrameId*/, map<unsigned int, std::shared_ptr<GeometricCamera>> &mpCams, std::shared_ptr<Map> sharedMap);
 
     void printReprojectionError(list<KeyFrame*> &lpLocalWindowKFs, KeyFrame* mpCurrentKF, string &name, string &name_folder);
 
diff --git a/include/MapPoint.h b/include/MapPoint.h
index 7f865be1..5725fa18 100644
--- a/include/MapPoint.h
+++ b/include/MapPoint.h
@@ -106,9 +106,9 @@ class MapPoint
     
     MapPoint();
 
-    MapPoint(const Eigen::Vector3f &Pos, KeyFrame* pRefKF, Map* pMap);
-    MapPoint(const double invDepth, cv::Point2f uv_init, KeyFrame* pRefKF, KeyFrame* pHostKF, Map* pMap);
-    MapPoint(const Eigen::Vector3f &Pos,  Map* pMap, Frame* pFrame, const int &idxF);
+    MapPoint(const Eigen::Vector3f &Pos, KeyFrame* pRefKF, std::shared_ptr<Map> pMap);
+    MapPoint(const double invDepth, cv::Point2f uv_init, KeyFrame* pRefKF, KeyFrame* pHostKF, std::shared_ptr<Map> pMap);
+    MapPoint(const Eigen::Vector3f &Pos,  std::shared_ptr<Map> pMap, Frame* pFrame, const int &idxF);
 
     void SetWorldPos(const Eigen::Vector3f &Pos);
     Eigen::Vector3f GetWorldPos();
@@ -151,8 +151,8 @@ class MapPoint
     int PredictScale(const float &currentDist, KeyFrame*pKF);
     int PredictScale(const float &currentDist, Frame* pF);
 
-    Map* GetMap();
-    void UpdateMap(Map* pMap);
+    std::shared_ptr<Map> GetMap();
+    void UpdateMap(std::shared_ptr<Map> pMap);
 
     void PrintObservations();
 
@@ -241,7 +241,7 @@ class MapPoint
      float mfMinDistance;
      float mfMaxDistance;
 
-     Map* mpMap;
+     std::shared_ptr<Map> mpMap;
 
      // Mutex
      std::mutex mMutexPos;
diff --git a/include/Optimizer.h b/include/Optimizer.h
index b67b2ccb..48bcb16e 100644
--- a/include/Optimizer.h
+++ b/include/Optimizer.h
@@ -49,18 +49,18 @@ class Optimizer {
                                int nIterations = 5, bool *pbStopFlag = nullptr,
                                const unsigned long nLoopKF = 0,
                                const bool bRobust = true);
-  void static GlobalBundleAdjustemnt(Map *pMap, int nIterations = 5,
+  void static GlobalBundleAdjustemnt(std::shared_ptr<Map> pMap, int nIterations = 5,
                                      bool *pbStopFlag = nullptr,
                                      const unsigned long nLoopKF = 0,
                                      const bool bRobust = true);
-  void static FullInertialBA(Map *pMap, int its, const bool bFixLocal = false,
+  void static FullInertialBA(std::shared_ptr<Map> pMap, int its, const bool bFixLocal = false,
                              const unsigned long nLoopKF = 0,
                              bool *pbStopFlag = nullptr, bool bInit = false,
                              float priorG = 1e2, float priorA = 1e6,
                              Eigen::VectorXd *vSingVal = nullptr,
                              bool *bHess = nullptr);
 
-  void static LocalBundleAdjustment(KeyFrame *pKF, bool *pbStopFlag, Map *pMap,
+  void static LocalBundleAdjustment(KeyFrame *pKF, bool *pbStopFlag, std::shared_ptr<Map> pMap,
                                     int &num_fixedKF, int &num_OptKF,
                                     int &num_MPs, int &num_edges);
 
@@ -73,7 +73,7 @@ class Optimizer {
   // if bFixScale is true, 6DoF optimization (stereo,rgbd), 7DoF otherwise
   // (mono)
   void static OptimizeEssentialGraph(
-      Map *pMap, KeyFrame *pLoopKF, KeyFrame *pCurKF,
+      std::shared_ptr<Map> pMap, KeyFrame *pLoopKF, KeyFrame *pCurKF,
       const LoopClosing::KeyFrameAndPose &NonCorrectedSim3,
       const LoopClosing::KeyFrameAndPose &CorrectedSim3,
       const map<KeyFrame *, set<KeyFrame *> > &LoopConnections,
@@ -86,7 +86,7 @@ class Optimizer {
 
   // For inertial loopclosing
   void static OptimizeEssentialGraph4DoF(
-      Map *pMap, KeyFrame *pLoopKF, KeyFrame *pCurKF,
+      std::shared_ptr<Map> pMap, KeyFrame *pLoopKF, KeyFrame *pCurKF,
       const LoopClosing::KeyFrameAndPose &NonCorrectedSim3,
       const LoopClosing::KeyFrameAndPose &CorrectedSim3,
       const map<KeyFrame *, set<KeyFrame *> > &LoopConnections);
@@ -102,12 +102,12 @@ class Optimizer {
 
   // For inertial systems
 
-  void static LocalInertialBA(KeyFrame *pKF, bool *pbStopFlag, Map *pMap,
+  void static LocalInertialBA(KeyFrame *pKF, bool *pbStopFlag, std::shared_ptr<Map> pMap,
                               int &num_fixedKF, int &num_OptKF, int &num_MPs,
                               int &num_edges, bool bLarge = false,
                               bool bRecInit = false);
   void static MergeInertialBA(KeyFrame *pCurrKF, KeyFrame *pMergeKF,
-                              bool *pbStopFlag, Map *pMap,
+                              bool *pbStopFlag, std::shared_ptr<Map> pMap,
                               LoopClosing::KeyFrameAndPose &corrPoses);
 
   // Local BA in welding area when two maps are merged
@@ -122,16 +122,16 @@ class Optimizer {
                                      const int &end);
 
   // Inertial pose-graph
-  void static InertialOptimization(Map *pMap, Eigen::Matrix3d &Rwg,
+  void static InertialOptimization(std::shared_ptr<Map> pMap, Eigen::Matrix3d &Rwg,
                                    double &scale, Eigen::Vector3d &bg,
                                    Eigen::Vector3d &ba, bool bMono,
                                    Eigen::MatrixXd &covInertial,
                                    bool bFixedVel = false, bool bGauss = false,
                                    float priorG = 1e2, float priorA = 1e6);
-  void static InertialOptimization(Map *pMap, Eigen::Vector3d &bg,
+  void static InertialOptimization(std::shared_ptr<Map> pMap, Eigen::Vector3d &bg,
                                    Eigen::Vector3d &ba, float priorG = 1e2,
                                    float priorA = 1e6);
-  void static InertialOptimization(Map *pMap, Eigen::Matrix3d &Rwg,
+  void static InertialOptimization(std::shared_ptr<Map> pMap, Eigen::Matrix3d &Rwg,
                                    double &scale);
 
   ;
diff --git a/include/System.h b/include/System.h
index fb227d31..6ea7b460 100644
--- a/include/System.h
+++ b/include/System.h
@@ -141,8 +141,8 @@ class System
     void SaveTrajectoryEuRoC(const string &filename);
     void SaveKeyFrameTrajectoryEuRoC(const string &filename);
 
-    void SaveTrajectoryEuRoC(const string &filename, Map* pMap);
-    void SaveKeyFrameTrajectoryEuRoC(const string &filename, Map* pMap);
+    void SaveTrajectoryEuRoC(const string &filename, std::shared_ptr<Map> pMap);
+    void SaveKeyFrameTrajectoryEuRoC(const string &filename, std::shared_ptr<Map> pMap);
 
     // Save data used for initialization debug
     void SaveDebugData(const int &iniIdx);
diff --git a/include/Tracking.h b/include/Tracking.h
index 4b53ceaa..b5c21db7 100644
--- a/include/Tracking.h
+++ b/include/Tracking.h
@@ -105,7 +105,7 @@ class Tracking {
   void SaveSubTrajectory(string strNameFile_frames, string strNameFile_kf,
                          string strFolder = "");
   void SaveSubTrajectory(string strNameFile_frames, string strNameFile_kf,
-                         Map* pMap);
+                         std::shared_ptr<Map> pMap);
 
   float GetImageScale();
 
diff --git a/output.txt b/output.txt
new file mode 100644
index 00000000..3c6c406f
--- /dev/null
+++ b/output.txt
@@ -0,0 +1,1441 @@
+Using argument -j12
+Configuring and building Thirdparty/DBoW2 ...
+-- Configuring done
+-- Generating done
+-- Build files have been written to: /orin_ssd/MORB_SLAM/Thirdparty/DBoW2/build
+Consolidate compiler generated dependencies of target DBoW2
+[100%] Built target DBoW2
+Configuring and building Thirdparty/g2o ...
+-- BUILD TYPE:Release
+-- Compiling on Unix
+-- Configuring done
+-- Generating done
+-- Build files have been written to: /orin_ssd/MORB_SLAM/Thirdparty/g2o/build
+Consolidate compiler generated dependencies of target g2o
+[100%] Built target g2o
+Configuring and building Thirdparty/Sophus ...
+-- Found installed version of Eigen: /usr/lib/cmake/eigen3
+-- Found required Ceres dependency: Eigen version 3.3.7 in /usr/include/eigen3
+-- Found required Ceres dependency: glog
+-- Found installed version of gflags: /usr/lib/aarch64-linux-gnu/cmake/gflags
+-- Detected gflags version: 2.2.2
+-- Found required Ceres dependency: gflags
+-- Found Ceres version: 1.14.0 installed in: /usr with components: [EigenSparse, SparseLinearAlgebraLibrary, LAPACK, SuiteSparse, CXSparse, SchurSpecializations, OpenMP, Multithreading]
+-- Found installed version of Eigen: /usr/lib/cmake/eigen3
+-- Found required Ceres dependency: Eigen version 3.3.7 in /usr/include/eigen3
+-- Found required Ceres dependency: glog
+-- Found installed version of gflags: /usr/lib/aarch64-linux-gnu/cmake/gflags
+-- Detected gflags version: 2.2.2
+-- Found required Ceres dependency: gflags
+-- Found Ceres version: 1.14.0 installed in: /usr with components: [EigenSparse, SparseLinearAlgebraLibrary, LAPACK, SuiteSparse, CXSparse, SchurSpecializations, OpenMP, Multithreading]
+-- CERES found
+-- Found installed version of Eigen: /usr/lib/cmake/eigen3
+-- Found required Ceres dependency: Eigen version 3.3.7 in /usr/include/eigen3
+-- Found required Ceres dependency: glog
+-- Found installed version of gflags: /usr/lib/aarch64-linux-gnu/cmake/gflags
+-- Detected gflags version: 2.2.2
+-- Found required Ceres dependency: gflags
+-- Found Ceres version: 1.14.0 installed in: /usr with components: [EigenSparse, SparseLinearAlgebraLibrary, LAPACK, SuiteSparse, CXSparse, SchurSpecializations, OpenMP, Multithreading]
+-- Configuring done
+-- Generating done
+-- Build files have been written to: /orin_ssd/MORB_SLAM/Thirdparty/Sophus/build
+Consolidate compiler generated dependencies of target test_common
+Consolidate compiler generated dependencies of target test_rxso2
+Consolidate compiler generated dependencies of target test_so2
+Consolidate compiler generated dependencies of target test_so3
+Consolidate compiler generated dependencies of target test_rxso3
+Consolidate compiler generated dependencies of target test_geometry
+Consolidate compiler generated dependencies of target test_sim2
+Consolidate compiler generated dependencies of target test_sim3
+Consolidate compiler generated dependencies of target test_velocities
+Consolidate compiler generated dependencies of target test_se3
+Consolidate compiler generated dependencies of target test_se2
+Consolidate compiler generated dependencies of target test_ceres_se3.cpp
+[ 15%] Built target test_common
+[ 15%] Built target test_so2
+[ 23%] Built target test_sim2
+[ 30%] Built target test_sim3
+[ 46%] Built target test_velocities
+[ 46%] Built target test_rxso2
+[ 53%] Built target test_rxso3
+[ 69%] Built target test_se3
+[ 69%] Built target test_geometry
+[ 84%] Built target test_ceres_se3.cpp
+[ 84%] Built target test_se2
+[ 92%] Built target test_so3
+Consolidate compiler generated dependencies of target HelloSO3
+[100%] Built target HelloSO3
+Configuring and building ORB_SLAM3 ...
+-- Build type: RelWithDebInfo
+-- OPENCV VERSION:
+-- 4.5.4
+-- BUILD TYPE:RelWithDebInfo
+-- Compiling on Unix
+-- Configuring done
+-- Generating done
+-- Build files have been written to: /orin_ssd/MORB_SLAM/build
+-- Build type: RelWithDebInfo
+-- OPENCV VERSION:
+-- 4.5.4
+-- BUILD TYPE:RelWithDebInfo
+-- Compiling on Unix
+-- Configuring done
+-- Generating done
+-- Build files have been written to: /orin_ssd/MORB_SLAM/build
+[  6%] Built target DBoW2
+[ 33%] Built target g2o
+[ 34%] Building CXX object CMakeFiles/MORB_SLAM.dir/src/Tracking.cc.o
+[ 34%] Building CXX object CMakeFiles/MORB_SLAM.dir/src/System.cc.o
+[ 35%] Building CXX object CMakeFiles/MORB_SLAM.dir/src/LocalMapping.cc.o
+[ 36%] Building CXX object CMakeFiles/MORB_SLAM.dir/src/LoopClosing.cc.o
+[ 37%] Building CXX object CMakeFiles/MORB_SLAM.dir/src/ORBmatcher.cc.o
+[ 39%] Building CXX object CMakeFiles/MORB_SLAM.dir/src/MapPoint.cc.o
+[ 39%] Building CXX object CMakeFiles/MORB_SLAM.dir/src/FrameDrawer.cc.o
+[ 41%] Building CXX object CMakeFiles/MORB_SLAM.dir/src/KeyFrame.cc.o
+[ 41%] Building CXX object CMakeFiles/MORB_SLAM.dir/src/Atlas.cc.o
+[ 42%] Building CXX object CMakeFiles/MORB_SLAM.dir/src/Map.cc.o
+[ 43%] Building CXX object CMakeFiles/MORB_SLAM.dir/src/MapDrawer.cc.o
+[ 44%] Building CXX object CMakeFiles/MORB_SLAM.dir/src/Optimizer.cc.o
+/orin_ssd/MORB_SLAM/src/Map.cc: In member function ‘void ORB_SLAM3::Map::PreSave(std::set<std::shared_ptr<ORB_SLAM3::GeometricCamera> >&, std::shared_ptr<ORB_SLAM3::Map>)’:
+/orin_ssd/MORB_SLAM/src/Map.cc:339:32: error: no match for ‘operator!=’ (operand types are ‘std::shared_ptr<ORB_SLAM3::Map>’ and ‘std::__shared_ptr<ORB_SLAM3::Map, __gnu_cxx::_S_atomic>::element_type*’ {aka ‘ORB_SLAM3::Map*’})
+  339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
+      |            ~~~~~~~~~~~~~~~~~~~ ^~ ~~~~~~~~~~~~~~~
+      |                             |                  |
+      |                             |                  std::__shared_ptr<ORB_SLAM3::Map, __gnu_cxx::_S_atomic>::element_type* {aka ORB_SLAM3::Map*}
+      |                             std::shared_ptr<ORB_SLAM3::Map>
+In file included from /usr/include/c++/9/bits/stl_algobase.h:64,
+                 from /usr/include/c++/9/bits/stl_tree.h:63,
+                 from /usr/include/c++/9/map:60,
+                 from /usr/include/boost/serialization/map.hpp:20,
+                 from /orin_ssd/MORB_SLAM/include/KeyFrame.h:25,
+                 from /orin_ssd/MORB_SLAM/include/MapPoint.h:22,
+                 from /orin_ssd/MORB_SLAM/include/Map.h:22,
+                 from /orin_ssd/MORB_SLAM/src/Map.cc:22:
+/usr/include/c++/9/bits/stl_pair.h:461:5: note: candidate: ‘template<class _T1, class _T2> constexpr bool std::operator!=(const std::pair<_T1, _T2>&, const std::pair<_T1, _T2>&)’
+  461 |     operator!=(const pair<_T1, _T2>& __x, const pair<_T1, _T2>& __y)
+      |     ^~~~~~~~
+/usr/include/c++/9/bits/stl_pair.h:461:5: note:   template argument deduction/substitution failed:
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<ORB_SLAM3::Map>’ is not derived from ‘const std::pair<_T1, _T2>’
+  339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
+      |                                                 ^
+In file included from /usr/include/c++/9/bits/stl_algobase.h:67,
+                 from /usr/include/c++/9/bits/stl_tree.h:63,
+                 from /usr/include/c++/9/map:60,
+                 from /usr/include/boost/serialization/map.hpp:20,
+                 from /orin_ssd/MORB_SLAM/include/KeyFrame.h:25,
+                 from /orin_ssd/MORB_SLAM/include/MapPoint.h:22,
+                 from /orin_ssd/MORB_SLAM/include/Map.h:22,
+                 from /orin_ssd/MORB_SLAM/src/Map.cc:22:
+/usr/include/c++/9/bits/stl_iterator.h:337:5: note: candidate: ‘template<class _Iterator> constexpr bool std::operator!=(const std::reverse_iterator<_Iterator>&, const std::reverse_iterator<_Iterator>&)’
+  337 |     operator!=(const reverse_iterator<_Iterator>& __x,
+      |     ^~~~~~~~
+/usr/include/c++/9/bits/stl_iterator.h:337:5: note:   template argument deduction/substitution failed:
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<ORB_SLAM3::Map>’ is not derived from ‘const std::reverse_iterator<_Iterator>’
+  339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
+      |                                                 ^
+In file included from /usr/include/c++/9/bits/stl_algobase.h:67,
+                 from /usr/include/c++/9/bits/stl_tree.h:63,
+                 from /usr/include/c++/9/map:60,
+                 from /usr/include/boost/serialization/map.hpp:20,
+                 from /orin_ssd/MORB_SLAM/include/KeyFrame.h:25,
+                 from /orin_ssd/MORB_SLAM/include/MapPoint.h:22,
+                 from /orin_ssd/MORB_SLAM/include/Map.h:22,
+                 from /orin_ssd/MORB_SLAM/src/Map.cc:22:
+/usr/include/c++/9/bits/stl_iterator.h:375:5: note: candidate: ‘template<class _IteratorL, class _IteratorR> constexpr bool std::operator!=(const std::reverse_iterator<_Iterator>&, const std::reverse_iterator<_IteratorR>&)’
+  375 |     operator!=(const reverse_iterator<_IteratorL>& __x,
+      |     ^~~~~~~~
+/usr/include/c++/9/bits/stl_iterator.h:375:5: note:   template argument deduction/substitution failed:
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<ORB_SLAM3::Map>’ is not derived from ‘const std::reverse_iterator<_Iterator>’
+  339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
+      |                                                 ^
+In file included from /usr/include/c++/9/bits/stl_algobase.h:67,
+                 from /usr/include/c++/9/bits/stl_tree.h:63,
+                 from /usr/include/c++/9/map:60,
+                 from /usr/include/boost/serialization/map.hpp:20,
+                 from /orin_ssd/MORB_SLAM/include/KeyFrame.h:25,
+                 from /orin_ssd/MORB_SLAM/include/MapPoint.h:22,
+                 from /orin_ssd/MORB_SLAM/include/Map.h:22,
+                 from /orin_ssd/MORB_SLAM/src/Map.cc:22:
+/usr/include/c++/9/bits/stl_iterator.h:1148:5: note: candidate: ‘template<class _IteratorL, class _IteratorR> constexpr bool std::operator!=(const std::move_iterator<_IteratorL>&, const std::move_iterator<_IteratorR>&)’
+ 1148 |     operator!=(const move_iterator<_IteratorL>& __x,
+      |     ^~~~~~~~
+/usr/include/c++/9/bits/stl_iterator.h:1148:5: note:   template argument deduction/substitution failed:
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<ORB_SLAM3::Map>’ is not derived from ‘const std::move_iterator<_IteratorL>’
+  339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
+      |                                                 ^
+In file included from /usr/include/c++/9/bits/stl_algobase.h:67,
+                 from /usr/include/c++/9/bits/stl_tree.h:63,
+                 from /usr/include/c++/9/map:60,
+                 from /usr/include/boost/serialization/map.hpp:20,
+                 from /orin_ssd/MORB_SLAM/include/KeyFrame.h:25,
+                 from /orin_ssd/MORB_SLAM/include/MapPoint.h:22,
+                 from /orin_ssd/MORB_SLAM/include/Map.h:22,
+                 from /orin_ssd/MORB_SLAM/src/Map.cc:22:
+/usr/include/c++/9/bits/stl_iterator.h:1154:5: note: candidate: ‘template<class _Iterator> constexpr bool std::operator!=(const std::move_iterator<_IteratorL>&, const std::move_iterator<_IteratorL>&)’
+ 1154 |     operator!=(const move_iterator<_Iterator>& __x,
+      |     ^~~~~~~~
+/usr/include/c++/9/bits/stl_iterator.h:1154:5: note:   template argument deduction/substitution failed:
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<ORB_SLAM3::Map>’ is not derived from ‘const std::move_iterator<_IteratorL>’
+  339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
+      |                                                 ^
+In file included from /usr/include/c++/9/bits/stl_tree.h:64,
+                 from /usr/include/c++/9/map:60,
+                 from /usr/include/boost/serialization/map.hpp:20,
+                 from /orin_ssd/MORB_SLAM/include/KeyFrame.h:25,
+                 from /orin_ssd/MORB_SLAM/include/MapPoint.h:22,
+                 from /orin_ssd/MORB_SLAM/include/Map.h:22,
+                 from /orin_ssd/MORB_SLAM/src/Map.cc:22:
+/usr/include/c++/9/bits/allocator.h:173:5: note: candidate: ‘template<class _T1, class _T2> bool std::operator!=(const std::allocator<_Tp1>&, const std::allocator<_T2>&)’
+  173 |     operator!=(const allocator<_T1>&, const allocator<_T2>&)
+      |     ^~~~~~~~
+/usr/include/c++/9/bits/allocator.h:173:5: note:   template argument deduction/substitution failed:
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<ORB_SLAM3::Map>’ is not derived from ‘const std::allocator<_Tp1>’
+  339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
+      |                                                 ^
+In file included from /usr/include/c++/9/bits/char_traits.h:40,
+                 from /usr/include/c++/9/string:40,
+                 from /usr/include/c++/9/stdexcept:39,
+                 from /usr/include/c++/9/optional:38,
+                 from /usr/include/c++/9/bits/node_handle.h:39,
+                 from /usr/include/c++/9/bits/stl_tree.h:72,
+                 from /usr/include/c++/9/map:60,
+                 from /usr/include/boost/serialization/map.hpp:20,
+                 from /orin_ssd/MORB_SLAM/include/KeyFrame.h:25,
+                 from /orin_ssd/MORB_SLAM/include/MapPoint.h:22,
+                 from /orin_ssd/MORB_SLAM/include/Map.h:22,
+                 from /orin_ssd/MORB_SLAM/src/Map.cc:22:
+/usr/include/c++/9/bits/postypes.h:227:5: note: candidate: ‘template<class _StateT> bool std::operator!=(const std::fpos<_StateT>&, const std::fpos<_StateT>&)’
+  227 |     operator!=(const fpos<_StateT>& __lhs, const fpos<_StateT>& __rhs)
+      |     ^~~~~~~~
+/usr/include/c++/9/bits/postypes.h:227:5: note:   template argument deduction/substitution failed:
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<ORB_SLAM3::Map>’ is not derived from ‘const std::fpos<_StateT>’
+  339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
+      |                                                 ^
+In file included from /usr/include/c++/9/bits/basic_string.h:48,
+                 from /usr/include/c++/9/string:55,
+                 from /usr/include/c++/9/stdexcept:39,
+                 from /usr/include/c++/9/optional:38,
+                 from /usr/include/c++/9/bits/node_handle.h:39,
+                 from /usr/include/c++/9/bits/stl_tree.h:72,
+                 from /usr/include/c++/9/map:60,
+                 from /usr/include/boost/serialization/map.hpp:20,
+                 from /orin_ssd/MORB_SLAM/include/KeyFrame.h:25,
+                 from /orin_ssd/MORB_SLAM/include/MapPoint.h:22,
+                 from /orin_ssd/MORB_SLAM/include/Map.h:22,
+                 from /orin_ssd/MORB_SLAM/src/Map.cc:22:
+/usr/include/c++/9/string_view:489:5: note: candidate: ‘template<class _CharT, class _Traits> constexpr bool std::operator!=(std::basic_string_view<_CharT, _Traits>, std::basic_string_view<_CharT, _Traits>)’
+  489 |     operator!=(basic_string_view<_CharT, _Traits> __x,
+      |     ^~~~~~~~
+/usr/include/c++/9/string_view:489:5: note:   template argument deduction/substitution failed:
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<ORB_SLAM3::Map>’ is not derived from ‘std::basic_string_view<_CharT, _Traits>’
+  339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
+      |                                                 ^
+In file included from /usr/include/c++/9/bits/basic_string.h:48,
+                 from /usr/include/c++/9/string:55,
+                 from /usr/include/c++/9/stdexcept:39,
+                 from /usr/include/c++/9/optional:38,
+                 from /usr/include/c++/9/bits/node_handle.h:39,
+                 from /usr/include/c++/9/bits/stl_tree.h:72,
+                 from /usr/include/c++/9/map:60,
+                 from /usr/include/boost/serialization/map.hpp:20,
+                 from /orin_ssd/MORB_SLAM/include/KeyFrame.h:25,
+                 from /orin_ssd/MORB_SLAM/include/MapPoint.h:22,
+                 from /orin_ssd/MORB_SLAM/include/Map.h:22,
+                 from /orin_ssd/MORB_SLAM/src/Map.cc:22:
+/usr/include/c++/9/string_view:495:5: note: candidate: ‘template<class _CharT, class _Traits> constexpr bool std::operator!=(std::basic_string_view<_CharT, _Traits>, std::__detail::__idt<std::basic_string_view<_CharT, _Traits> >)’
+  495 |     operator!=(basic_string_view<_CharT, _Traits> __x,
+      |     ^~~~~~~~
+/usr/include/c++/9/string_view:495:5: note:   template argument deduction/substitution failed:
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<ORB_SLAM3::Map>’ is not derived from ‘std::basic_string_view<_CharT, _Traits>’
+  339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
+      |                                                 ^
+In file included from /usr/include/c++/9/bits/basic_string.h:48,
+                 from /usr/include/c++/9/string:55,
+                 from /usr/include/c++/9/stdexcept:39,
+                 from /usr/include/c++/9/optional:38,
+                 from /usr/include/c++/9/bits/node_handle.h:39,
+                 from /usr/include/c++/9/bits/stl_tree.h:72,
+                 from /usr/include/c++/9/map:60,
+                 from /usr/include/boost/serialization/map.hpp:20,
+                 from /orin_ssd/MORB_SLAM/include/KeyFrame.h:25,
+                 from /orin_ssd/MORB_SLAM/include/MapPoint.h:22,
+                 from /orin_ssd/MORB_SLAM/include/Map.h:22,
+                 from /orin_ssd/MORB_SLAM/src/Map.cc:22:
+/usr/include/c++/9/string_view:501:5: note: candidate: ‘template<class _CharT, class _Traits> constexpr bool std::operator!=(std::__detail::__idt<std::basic_string_view<_CharT, _Traits> >, std::basic_string_view<_CharT, _Traits>)’
+  501 |     operator!=(__detail::__idt<basic_string_view<_CharT, _Traits>> __x,
+      |     ^~~~~~~~
+/usr/include/c++/9/string_view:501:5: note:   template argument deduction/substitution failed:
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   mismatched types ‘std::basic_string_view<_CharT, _Traits>’ and ‘std::__shared_ptr<ORB_SLAM3::Map, __gnu_cxx::_S_atomic>::element_type*’ {aka ‘ORB_SLAM3::Map*’}
+  339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
+      |                                                 ^
+In file included from /usr/include/c++/9/string:55,
+                 from /usr/include/c++/9/stdexcept:39,
+                 from /usr/include/c++/9/optional:38,
+                 from /usr/include/c++/9/bits/node_handle.h:39,
+                 from /usr/include/c++/9/bits/stl_tree.h:72,
+                 from /usr/include/c++/9/map:60,
+                 from /usr/include/boost/serialization/map.hpp:20,
+                 from /orin_ssd/MORB_SLAM/include/KeyFrame.h:25,
+                 from /orin_ssd/MORB_SLAM/include/MapPoint.h:22,
+                 from /orin_ssd/MORB_SLAM/include/Map.h:22,
+                 from /orin_ssd/MORB_SLAM/src/Map.cc:22:
+/usr/include/c++/9/bits/basic_string.h:6191:5: note: candidate: ‘template<class _CharT, class _Traits, class _Alloc> bool std::operator!=(const std::__cxx11::basic_string<_CharT, _Traits, _Allocator>&, const std::__cxx11::basic_string<_CharT, _Traits, _Allocator>&)’
+ 6191 |     operator!=(const basic_string<_CharT, _Traits, _Alloc>& __lhs,
+      |     ^~~~~~~~
+/usr/include/c++/9/bits/basic_string.h:6191:5: note:   template argument deduction/substitution failed:
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<ORB_SLAM3::Map>’ is not derived from ‘const std::__cxx11::basic_string<_CharT, _Traits, _Allocator>’
+  339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
+      |                                                 ^
+In file included from /usr/include/c++/9/string:55,
+                 from /usr/include/c++/9/stdexcept:39,
+                 from /usr/include/c++/9/optional:38,
+                 from /usr/include/c++/9/bits/node_handle.h:39,
+                 from /usr/include/c++/9/bits/stl_tree.h:72,
+                 from /usr/include/c++/9/map:60,
+                 from /usr/include/boost/serialization/map.hpp:20,
+                 from /orin_ssd/MORB_SLAM/include/KeyFrame.h:25,
+                 from /orin_ssd/MORB_SLAM/include/MapPoint.h:22,
+                 from /orin_ssd/MORB_SLAM/include/Map.h:22,
+                 from /orin_ssd/MORB_SLAM/src/Map.cc:22:
+/usr/include/c++/9/bits/basic_string.h:6204:5: note: candidate: ‘template<class _CharT, class _Traits, class _Alloc> bool std::operator!=(const _CharT*, const std::__cxx11::basic_string<_CharT, _Traits, _Allocator>&)’
+ 6204 |     operator!=(const _CharT* __lhs,
+      |     ^~~~~~~~
+/usr/include/c++/9/bits/basic_string.h:6204:5: note:   template argument deduction/substitution failed:
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   mismatched types ‘const _CharT*’ and ‘std::shared_ptr<ORB_SLAM3::Map>’
+  339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
+      |                                                 ^
+In file included from /usr/include/c++/9/string:55,
+                 from /usr/include/c++/9/stdexcept:39,
+                 from /usr/include/c++/9/optional:38,
+                 from /usr/include/c++/9/bits/node_handle.h:39,
+                 from /usr/include/c++/9/bits/stl_tree.h:72,
+                 from /usr/include/c++/9/map:60,
+                 from /usr/include/boost/serialization/map.hpp:20,
+                 from /orin_ssd/MORB_SLAM/include/KeyFrame.h:25,
+                 from /orin_ssd/MORB_SLAM/include/MapPoint.h:22,
+                 from /orin_ssd/MORB_SLAM/include/Map.h:22,
+                 from /orin_ssd/MORB_SLAM/src/Map.cc:22:
+/usr/include/c++/9/bits/basic_string.h:6216:5: note: candidate: ‘template<class _CharT, class _Traits, class _Alloc> bool std::operator!=(const std::__cxx11::basic_string<_CharT, _Traits, _Allocator>&, const _CharT*)’
+ 6216 |     operator!=(const basic_string<_CharT, _Traits, _Alloc>& __lhs,
+      |     ^~~~~~~~
+/usr/include/c++/9/bits/basic_string.h:6216:5: note:   template argument deduction/substitution failed:
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<ORB_SLAM3::Map>’ is not derived from ‘const std::__cxx11::basic_string<_CharT, _Traits, _Allocator>’
+  339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
+      |                                                 ^
+In file included from /usr/include/c++/9/bits/node_handle.h:39,
+                 from /usr/include/c++/9/bits/stl_tree.h:72,
+                 from /usr/include/c++/9/map:60,
+                 from /usr/include/boost/serialization/map.hpp:20,
+                 from /orin_ssd/MORB_SLAM/include/KeyFrame.h:25,
+                 from /orin_ssd/MORB_SLAM/include/MapPoint.h:22,
+                 from /orin_ssd/MORB_SLAM/include/Map.h:22,
+                 from /orin_ssd/MORB_SLAM/src/Map.cc:22:
+/usr/include/c++/9/optional:992:5: note: candidate: ‘template<class _Tp, class _Up> constexpr std::__optional_relop_t<decltype ((declval<_Tp>() != declval<_Up>()))> std::operator!=(const std::optional<_Tp>&, const std::optional<_Up>&)’
+  992 |     operator!=(const optional<_Tp>& __lhs, const optional<_Up>& __rhs)
+      |     ^~~~~~~~
+/usr/include/c++/9/optional:992:5: note:   template argument deduction/substitution failed:
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<ORB_SLAM3::Map>’ is not derived from ‘const std::optional<_Tp>’
+  339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
+      |                                                 ^
+In file included from /usr/include/c++/9/bits/node_handle.h:39,
+                 from /usr/include/c++/9/bits/stl_tree.h:72,
+                 from /usr/include/c++/9/map:60,
+                 from /usr/include/boost/serialization/map.hpp:20,
+                 from /orin_ssd/MORB_SLAM/include/KeyFrame.h:25,
+                 from /orin_ssd/MORB_SLAM/include/MapPoint.h:22,
+                 from /orin_ssd/MORB_SLAM/include/Map.h:22,
+                 from /orin_ssd/MORB_SLAM/src/Map.cc:22:
+/usr/include/c++/9/optional:1044:5: note: candidate: ‘template<class _Tp> constexpr bool std::operator!=(const std::optional<_Tp>&, std::nullopt_t)’
+ 1044 |     operator!=(const optional<_Tp>& __lhs, nullopt_t) noexcept
+      |     ^~~~~~~~
+/usr/include/c++/9/optional:1044:5: note:   template argument deduction/substitution failed:
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<ORB_SLAM3::Map>’ is not derived from ‘const std::optional<_Tp>’
+  339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
+      |                                                 ^
+In file included from /usr/include/c++/9/bits/node_handle.h:39,
+                 from /usr/include/c++/9/bits/stl_tree.h:72,
+                 from /usr/include/c++/9/map:60,
+                 from /usr/include/boost/serialization/map.hpp:20,
+                 from /orin_ssd/MORB_SLAM/include/KeyFrame.h:25,
+                 from /orin_ssd/MORB_SLAM/include/MapPoint.h:22,
+                 from /orin_ssd/MORB_SLAM/include/Map.h:22,
+                 from /orin_ssd/MORB_SLAM/src/Map.cc:22:
+/usr/include/c++/9/optional:1049:5: note: candidate: ‘template<class _Tp> constexpr bool std::operator!=(std::nullopt_t, const std::optional<_Tp>&)’
+ 1049 |     operator!=(nullopt_t, const optional<_Tp>& __rhs) noexcept
+      |     ^~~~~~~~
+/usr/include/c++/9/optional:1049:5: note:   template argument deduction/substitution failed:
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   mismatched types ‘const std::optional<_Tp>’ and ‘std::__shared_ptr<ORB_SLAM3::Map, __gnu_cxx::_S_atomic>::element_type*’ {aka ‘ORB_SLAM3::Map*’}
+  339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
+      |                                                 ^
+In file included from /usr/include/c++/9/bits/node_handle.h:39,
+                 from /usr/include/c++/9/bits/stl_tree.h:72,
+                 from /usr/include/c++/9/map:60,
+                 from /usr/include/boost/serialization/map.hpp:20,
+                 from /orin_ssd/MORB_SLAM/include/KeyFrame.h:25,
+                 from /orin_ssd/MORB_SLAM/include/MapPoint.h:22,
+                 from /orin_ssd/MORB_SLAM/include/Map.h:22,
+                 from /orin_ssd/MORB_SLAM/src/Map.cc:22:
+/usr/include/c++/9/optional:1107:5: note: candidate: ‘template<class _Tp, class _Up> constexpr std::__optional_relop_t<decltype ((declval<_Tp>() != declval<_Up>()))> std::operator!=(const std::optional<_Tp>&, const _Up&)’
+ 1107 |     operator!=(const optional<_Tp>& __lhs, const _Up& __rhs)
+      |     ^~~~~~~~
+/usr/include/c++/9/optional:1107:5: note:   template argument deduction/substitution failed:
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<ORB_SLAM3::Map>’ is not derived from ‘const std::optional<_Tp>’
+  339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
+      |                                                 ^
+In file included from /usr/include/c++/9/bits/node_handle.h:39,
+                 from /usr/include/c++/9/bits/stl_tree.h:72,
+                 from /usr/include/c++/9/map:60,
+                 from /usr/include/boost/serialization/map.hpp:20,
+                 from /orin_ssd/MORB_SLAM/include/KeyFrame.h:25,
+                 from /orin_ssd/MORB_SLAM/include/MapPoint.h:22,
+                 from /orin_ssd/MORB_SLAM/include/Map.h:22,
+                 from /orin_ssd/MORB_SLAM/src/Map.cc:22:
+/usr/include/c++/9/optional:1113:5: note: candidate: ‘template<class _Tp, class _Up> constexpr std::__optional_relop_t<decltype ((declval<_Up>() != declval<_Tp>()))> std::operator!=(const _Up&, const std::optional<_Tp>&)’
+ 1113 |     operator!=(const _Up& __lhs, const optional<_Tp>& __rhs)
+      |     ^~~~~~~~
+/usr/include/c++/9/optional:1113:5: note:   template argument deduction/substitution failed:
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   mismatched types ‘const std::optional<_Tp>’ and ‘std::__shared_ptr<ORB_SLAM3::Map, __gnu_cxx::_S_atomic>::element_type*’ {aka ‘ORB_SLAM3::Map*’}
+  339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
+      |                                                 ^
+In file included from /usr/include/c++/9/tuple:39,
+                 from /usr/include/c++/9/bits/stl_map.h:63,
+                 from /usr/include/c++/9/map:61,
+                 from /usr/include/boost/serialization/map.hpp:20,
+                 from /orin_ssd/MORB_SLAM/include/KeyFrame.h:25,
+                 from /orin_ssd/MORB_SLAM/include/MapPoint.h:22,
+                 from /orin_ssd/MORB_SLAM/include/Map.h:22,
+                 from /orin_ssd/MORB_SLAM/src/Map.cc:22:
+/usr/include/c++/9/array:257:5: note: candidate: ‘template<class _Tp, long unsigned int _Nm> bool std::operator!=(const std::array<_Tp, _Nm>&, const std::array<_Tp, _Nm>&)’
+  257 |     operator!=(const array<_Tp, _Nm>& __one, const array<_Tp, _Nm>& __two)
+      |     ^~~~~~~~
+/usr/include/c++/9/array:257:5: note:   template argument deduction/substitution failed:
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<ORB_SLAM3::Map>’ is not derived from ‘const std::array<_Tp, _Nm>’
+  339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
+      |                                                 ^
+In file included from /usr/include/c++/9/bits/stl_map.h:63,
+                 from /usr/include/c++/9/map:61,
+                 from /usr/include/boost/serialization/map.hpp:20,
+                 from /orin_ssd/MORB_SLAM/include/KeyFrame.h:25,
+                 from /orin_ssd/MORB_SLAM/include/MapPoint.h:22,
+                 from /orin_ssd/MORB_SLAM/include/Map.h:22,
+                 from /orin_ssd/MORB_SLAM/src/Map.cc:22:
+/usr/include/c++/9/tuple:1445:5: note: candidate: ‘template<class ... _TElements, class ... _UElements> constexpr bool std::operator!=(const std::tuple<_Tps ...>&, const std::tuple<_Args2 ...>&)’
+ 1445 |     operator!=(const tuple<_TElements...>& __t,
+      |     ^~~~~~~~
+/usr/include/c++/9/tuple:1445:5: note:   template argument deduction/substitution failed:
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<ORB_SLAM3::Map>’ is not derived from ‘const std::tuple<_Tps ...>’
+  339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
+      |                                                 ^
+In file included from /usr/include/c++/9/map:61,
+                 from /usr/include/boost/serialization/map.hpp:20,
+                 from /orin_ssd/MORB_SLAM/include/KeyFrame.h:25,
+                 from /orin_ssd/MORB_SLAM/include/MapPoint.h:22,
+                 from /orin_ssd/MORB_SLAM/include/Map.h:22,
+                 from /orin_ssd/MORB_SLAM/src/Map.cc:22:
+/usr/include/c++/9/bits/stl_map.h:1479:5: note: candidate: ‘template<class _Key, class _Tp, class _Compare, class _Alloc> bool std::operator!=(const std::map<_Key, _Tp, _Compare, _Allocator>&, const std::map<_Key, _Tp, _Compare, _Allocator>&)’
+ 1479 |     operator!=(const map<_Key, _Tp, _Compare, _Alloc>& __x,
+      |     ^~~~~~~~
+/usr/include/c++/9/bits/stl_map.h:1479:5: note:   template argument deduction/substitution failed:
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<ORB_SLAM3::Map>’ is not derived from ‘const std::map<_Key, _Tp, _Compare, _Allocator>’
+  339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
+      |                                                 ^
+In file included from /usr/include/c++/9/map:62,
+                 from /usr/include/boost/serialization/map.hpp:20,
+                 from /orin_ssd/MORB_SLAM/include/KeyFrame.h:25,
+                 from /orin_ssd/MORB_SLAM/include/MapPoint.h:22,
+                 from /orin_ssd/MORB_SLAM/include/Map.h:22,
+                 from /orin_ssd/MORB_SLAM/src/Map.cc:22:
+/usr/include/c++/9/bits/stl_multimap.h:1143:5: note: candidate: ‘template<class _Key, class _Tp, class _Compare, class _Alloc> bool std::operator!=(const std::multimap<_Key, _Tp, _Compare, _Allocator>&, const std::multimap<_Key, _Tp, _Compare, _Allocator>&)’
+ 1143 |     operator!=(const multimap<_Key, _Tp, _Compare, _Alloc>& __x,
+      |     ^~~~~~~~
+/usr/include/c++/9/bits/stl_multimap.h:1143:5: note:   template argument deduction/substitution failed:
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<ORB_SLAM3::Map>’ is not derived from ‘const std::multimap<_Key, _Tp, _Compare, _Allocator>’
+  339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
+      |                                                 ^
+In file included from /usr/include/c++/9/memory:80,
+                 from /usr/include/boost/smart_ptr/scoped_ptr.hpp:22,
+                 from /usr/include/boost/scoped_ptr.hpp:13,
+                 from /usr/include/boost/archive/detail/basic_iarchive.hpp:23,
+                 from /usr/include/boost/serialization/map.hpp:24,
+                 from /orin_ssd/MORB_SLAM/include/KeyFrame.h:25,
+                 from /orin_ssd/MORB_SLAM/include/MapPoint.h:22,
+                 from /orin_ssd/MORB_SLAM/include/Map.h:22,
+                 from /orin_ssd/MORB_SLAM/src/Map.cc:22:
+/usr/include/c++/9/bits/unique_ptr.h:732:5: note: candidate: ‘template<class _Tp, class _Dp, class _Up, class _Ep> bool std::operator!=(const std::unique_ptr<_Tp, _Dp>&, const std::unique_ptr<_Up, _Ep>&)’
+  732 |     operator!=(const unique_ptr<_Tp, _Dp>& __x,
+      |     ^~~~~~~~
+/usr/include/c++/9/bits/unique_ptr.h:732:5: note:   template argument deduction/substitution failed:
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<ORB_SLAM3::Map>’ is not derived from ‘const std::unique_ptr<_Tp, _Dp>’
+  339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
+      |                                                 ^
+In file included from /usr/include/c++/9/memory:80,
+                 from /usr/include/boost/smart_ptr/scoped_ptr.hpp:22,
+                 from /usr/include/boost/scoped_ptr.hpp:13,
+                 from /usr/include/boost/archive/detail/basic_iarchive.hpp:23,
+                 from /usr/include/boost/serialization/map.hpp:24,
+                 from /orin_ssd/MORB_SLAM/include/KeyFrame.h:25,
+                 from /orin_ssd/MORB_SLAM/include/MapPoint.h:22,
+                 from /orin_ssd/MORB_SLAM/include/Map.h:22,
+                 from /orin_ssd/MORB_SLAM/src/Map.cc:22:
+/usr/include/c++/9/bits/unique_ptr.h:738:5: note: candidate: ‘template<class _Tp, class _Dp> bool std::operator!=(const std::unique_ptr<_Tp, _Dp>&, std::nullptr_t)’
+  738 |     operator!=(const unique_ptr<_Tp, _Dp>& __x, nullptr_t) noexcept
+      |     ^~~~~~~~
+/usr/include/c++/9/bits/unique_ptr.h:738:5: note:   template argument deduction/substitution failed:
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<ORB_SLAM3::Map>’ is not derived from ‘const std::unique_ptr<_Tp, _Dp>’
+  339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
+      |                                                 ^
+In file included from /usr/include/c++/9/memory:80,
+                 from /usr/include/boost/smart_ptr/scoped_ptr.hpp:22,
+                 from /usr/include/boost/scoped_ptr.hpp:13,
+                 from /usr/include/boost/archive/detail/basic_iarchive.hpp:23,
+                 from /usr/include/boost/serialization/map.hpp:24,
+                 from /orin_ssd/MORB_SLAM/include/KeyFrame.h:25,
+                 from /orin_ssd/MORB_SLAM/include/MapPoint.h:22,
+                 from /orin_ssd/MORB_SLAM/include/Map.h:22,
+                 from /orin_ssd/MORB_SLAM/src/Map.cc:22:
+/usr/include/c++/9/bits/unique_ptr.h:743:5: note: candidate: ‘template<class _Tp, class _Dp> bool std::operator!=(std::nullptr_t, const std::unique_ptr<_Tp, _Dp>&)’
+  743 |     operator!=(nullptr_t, const unique_ptr<_Tp, _Dp>& __x) noexcept
+      |     ^~~~~~~~
+/usr/include/c++/9/bits/unique_ptr.h:743:5: note:   template argument deduction/substitution failed:
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   mismatched types ‘const std::unique_ptr<_Tp, _Dp>’ and ‘std::__shared_ptr<ORB_SLAM3::Map, __gnu_cxx::_S_atomic>::element_type*’ {aka ‘ORB_SLAM3::Map*’}
+  339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
+      |                                                 ^
+In file included from /usr/include/c++/9/bits/shared_ptr.h:52,
+                 from /usr/include/c++/9/memory:81,
+                 from /usr/include/boost/smart_ptr/scoped_ptr.hpp:22,
+                 from /usr/include/boost/scoped_ptr.hpp:13,
+                 from /usr/include/boost/archive/detail/basic_iarchive.hpp:23,
+                 from /usr/include/boost/serialization/map.hpp:24,
+                 from /orin_ssd/MORB_SLAM/include/KeyFrame.h:25,
+                 from /orin_ssd/MORB_SLAM/include/MapPoint.h:22,
+                 from /orin_ssd/MORB_SLAM/include/Map.h:22,
+                 from /orin_ssd/MORB_SLAM/src/Map.cc:22:
+/usr/include/c++/9/bits/shared_ptr_base.h:1428:5: note: candidate: ‘template<class _Tp1, class _Tp2, __gnu_cxx::_Lock_policy _Lp> bool std::operator!=(const std::__shared_ptr<_Tp1, _Lp>&, const std::__shared_ptr<_Tp2, _Lp>&)’
+ 1428 |     operator!=(const __shared_ptr<_Tp1, _Lp>& __a,
+      |     ^~~~~~~~
+/usr/include/c++/9/bits/shared_ptr_base.h:1428:5: note:   template argument deduction/substitution failed:
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   mismatched types ‘const std::__shared_ptr<_Tp2, _Lp>’ and ‘std::__shared_ptr<ORB_SLAM3::Map, __gnu_cxx::_S_atomic>::element_type*’ {aka ‘ORB_SLAM3::Map*’}
+  339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
+      |                                                 ^
+In file included from /usr/include/c++/9/bits/shared_ptr.h:52,
+                 from /usr/include/c++/9/memory:81,
+                 from /usr/include/boost/smart_ptr/scoped_ptr.hpp:22,
+                 from /usr/include/boost/scoped_ptr.hpp:13,
+                 from /usr/include/boost/archive/detail/basic_iarchive.hpp:23,
+                 from /usr/include/boost/serialization/map.hpp:24,
+                 from /orin_ssd/MORB_SLAM/include/KeyFrame.h:25,
+                 from /orin_ssd/MORB_SLAM/include/MapPoint.h:22,
+                 from /orin_ssd/MORB_SLAM/include/Map.h:22,
+                 from /orin_ssd/MORB_SLAM/src/Map.cc:22:
+/usr/include/c++/9/bits/shared_ptr_base.h:1434:5: note: candidate: ‘template<class _Tp, __gnu_cxx::_Lock_policy _Lp> bool std::operator!=(const std::__shared_ptr<_Tp, _Lp>&, std::nullptr_t)’
+ 1434 |     operator!=(const __shared_ptr<_Tp, _Lp>& __a, nullptr_t) noexcept
+      |     ^~~~~~~~
+/usr/include/c++/9/bits/shared_ptr_base.h:1434:5: note:   template argument deduction/substitution failed:
+/orin_ssd/MORB_SLAM/src/Map.cc:339:48: note:   cannot convert ‘sharedMap.std::shared_ptr<ORB_SLAM3::Map>::<anonymous>.std::__shared_ptr<ORB_SLAM3::Map, __gnu_cxx::_S_atomic>::get()’ (type ‘std::__shared_ptr<ORB_SLAM3::Map, __gnu_cxx::_S_atomic>::element_type*’ {aka ‘ORB_SLAM3::Map*’}) to type ‘std::nullptr_t’
+  339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
+      |                                   ~~~~~~~~~~~~~^~
+In file included from /usr/include/c++/9/bits/shared_ptr.h:52,
+                 from /usr/include/c++/9/memory:81,
+                 from /usr/include/boost/smart_ptr/scoped_ptr.hpp:22,
+                 from /usr/include/boost/scoped_ptr.hpp:13,
+                 from /usr/include/boost/archive/detail/basic_iarchive.hpp:23,
+                 from /usr/include/boost/serialization/map.hpp:24,
+                 from /orin_ssd/MORB_SLAM/include/KeyFrame.h:25,
+                 from /orin_ssd/MORB_SLAM/include/MapPoint.h:22,
+                 from /orin_ssd/MORB_SLAM/include/Map.h:22,
+                 from /orin_ssd/MORB_SLAM/src/Map.cc:22:
+/usr/include/c++/9/bits/shared_ptr_base.h:1439:5: note: candidate: ‘template<class _Tp, __gnu_cxx::_Lock_policy _Lp> bool std::operator!=(std::nullptr_t, const std::__shared_ptr<_Tp, _Lp>&)’
+ 1439 |     operator!=(nullptr_t, const __shared_ptr<_Tp, _Lp>& __a) noexcept
+      |     ^~~~~~~~
+/usr/include/c++/9/bits/shared_ptr_base.h:1439:5: note:   template argument deduction/substitution failed:
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   mismatched types ‘const std::__shared_ptr<_Tp, _Lp>’ and ‘std::__shared_ptr<ORB_SLAM3::Map, __gnu_cxx::_S_atomic>::element_type*’ {aka ‘ORB_SLAM3::Map*’}
+  339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
+      |                                                 ^
+In file included from /usr/include/c++/9/memory:81,
+                 from /usr/include/boost/smart_ptr/scoped_ptr.hpp:22,
+                 from /usr/include/boost/scoped_ptr.hpp:13,
+                 from /usr/include/boost/archive/detail/basic_iarchive.hpp:23,
+                 from /usr/include/boost/serialization/map.hpp:24,
+                 from /orin_ssd/MORB_SLAM/include/KeyFrame.h:25,
+                 from /orin_ssd/MORB_SLAM/include/MapPoint.h:22,
+                 from /orin_ssd/MORB_SLAM/include/Map.h:22,
+                 from /orin_ssd/MORB_SLAM/src/Map.cc:22:
+/usr/include/c++/9/bits/shared_ptr.h:398:5: note: candidate: ‘template<class _Tp, class _Up> bool std::operator!=(const std::shared_ptr<_Tp>&, const std::shared_ptr<_Tp>&)’
+  398 |     operator!=(const shared_ptr<_Tp>& __a, const shared_ptr<_Up>& __b) noexcept
+      |     ^~~~~~~~
+/usr/include/c++/9/bits/shared_ptr.h:398:5: note:   template argument deduction/substitution failed:
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   mismatched types ‘const std::shared_ptr<_Tp>’ and ‘std::__shared_ptr<ORB_SLAM3::Map, __gnu_cxx::_S_atomic>::element_type*’ {aka ‘ORB_SLAM3::Map*’}
+  339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
+      |                                                 ^
+In file included from /usr/include/c++/9/memory:81,
+                 from /usr/include/boost/smart_ptr/scoped_ptr.hpp:22,
+                 from /usr/include/boost/scoped_ptr.hpp:13,
+                 from /usr/include/boost/archive/detail/basic_iarchive.hpp:23,
+                 from /usr/include/boost/serialization/map.hpp:24,
+                 from /orin_ssd/MORB_SLAM/include/KeyFrame.h:25,
+                 from /orin_ssd/MORB_SLAM/include/MapPoint.h:22,
+                 from /orin_ssd/MORB_SLAM/include/Map.h:22,
+                 from /orin_ssd/MORB_SLAM/src/Map.cc:22:
+/usr/include/c++/9/bits/shared_ptr.h:403:5: note: candidate: ‘template<class _Tp> bool std::operator!=(const std::shared_ptr<_Tp>&, std::nullptr_t)’
+  403 |     operator!=(const shared_ptr<_Tp>& __a, nullptr_t) noexcept
+      |     ^~~~~~~~
+/usr/include/c++/9/bits/shared_ptr.h:403:5: note:   template argument deduction/substitution failed:
+/orin_ssd/MORB_SLAM/src/Map.cc:339:48: note:   cannot convert ‘sharedMap.std::shared_ptr<ORB_SLAM3::Map>::<anonymous>.std::__shared_ptr<ORB_SLAM3::Map, __gnu_cxx::_S_atomic>::get()’ (type ‘std::__shared_ptr<ORB_SLAM3::Map, __gnu_cxx::_S_atomic>::element_type*’ {aka ‘ORB_SLAM3::Map*’}) to type ‘std::nullptr_t’
+  339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
+      |                                   ~~~~~~~~~~~~~^~
+In file included from /usr/include/c++/9/memory:81,
+                 from /usr/include/boost/smart_ptr/scoped_ptr.hpp:22,
+                 from /usr/include/boost/scoped_ptr.hpp:13,
+                 from /usr/include/boost/archive/detail/basic_iarchive.hpp:23,
+                 from /usr/include/boost/serialization/map.hpp:24,
+                 from /orin_ssd/MORB_SLAM/include/KeyFrame.h:25,
+                 from /orin_ssd/MORB_SLAM/include/MapPoint.h:22,
+                 from /orin_ssd/MORB_SLAM/include/Map.h:22,
+                 from /orin_ssd/MORB_SLAM/src/Map.cc:22:
+/usr/include/c++/9/bits/shared_ptr.h:408:5: note: candidate: ‘template<class _Tp> bool std::operator!=(std::nullptr_t, const std::shared_ptr<_Tp>&)’
+  408 |     operator!=(nullptr_t, const shared_ptr<_Tp>& __a) noexcept
+      |     ^~~~~~~~
+/usr/include/c++/9/bits/shared_ptr.h:408:5: note:   template argument deduction/substitution failed:
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   mismatched types ‘const std::shared_ptr<_Tp>’ and ‘std::__shared_ptr<ORB_SLAM3::Map, __gnu_cxx::_S_atomic>::element_type*’ {aka ‘ORB_SLAM3::Map*’}
+  339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
+      |                                                 ^
+In file included from /usr/include/c++/9/vector:67,
+                 from /usr/include/boost/archive/detail/helper_collection.hpp:20,
+                 from /usr/include/boost/archive/detail/basic_iarchive.hpp:28,
+                 from /usr/include/boost/serialization/map.hpp:24,
+                 from /orin_ssd/MORB_SLAM/include/KeyFrame.h:25,
+                 from /orin_ssd/MORB_SLAM/include/MapPoint.h:22,
+                 from /orin_ssd/MORB_SLAM/include/Map.h:22,
+                 from /orin_ssd/MORB_SLAM/src/Map.cc:22:
+/usr/include/c++/9/bits/stl_vector.h:1912:5: note: candidate: ‘template<class _Tp, class _Alloc> bool std::operator!=(const std::vector<_Tp, _Alloc>&, const std::vector<_Tp, _Alloc>&)’
+ 1912 |     operator!=(const vector<_Tp, _Alloc>& __x, const vector<_Tp, _Alloc>& __y)
+      |     ^~~~~~~~
+/usr/include/c++/9/bits/stl_vector.h:1912:5: note:   template argument deduction/substitution failed:
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<ORB_SLAM3::Map>’ is not derived from ‘const std::vector<_Tp, _Alloc>’
+  339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
+      |                                                 ^
+In file included from /usr/include/c++/9/functional:59,
+                 from /usr/include/c++/9/pstl/glue_algorithm_defs.h:13,
+                 from /usr/include/c++/9/algorithm:71,
+                 from /usr/include/boost/archive/detail/helper_collection.hpp:23,
+                 from /usr/include/boost/archive/detail/basic_iarchive.hpp:28,
+                 from /usr/include/boost/serialization/map.hpp:24,
+                 from /orin_ssd/MORB_SLAM/include/KeyFrame.h:25,
+                 from /orin_ssd/MORB_SLAM/include/MapPoint.h:22,
+                 from /orin_ssd/MORB_SLAM/include/Map.h:22,
+                 from /orin_ssd/MORB_SLAM/src/Map.cc:22:
+/usr/include/c++/9/bits/std_function.h:764:5: note: candidate: ‘template<class _Res, class ... _Args> bool std::operator!=(const std::function<_Res(_ArgTypes ...)>&, std::nullptr_t)’
+  764 |     operator!=(const function<_Res(_Args...)>& __f, nullptr_t) noexcept
+      |     ^~~~~~~~
+/usr/include/c++/9/bits/std_function.h:764:5: note:   template argument deduction/substitution failed:
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<ORB_SLAM3::Map>’ is not derived from ‘const std::function<_Res(_ArgTypes ...)>’
+  339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
+      |                                                 ^
+In file included from /usr/include/c++/9/functional:59,
+                 from /usr/include/c++/9/pstl/glue_algorithm_defs.h:13,
+                 from /usr/include/c++/9/algorithm:71,
+                 from /usr/include/boost/archive/detail/helper_collection.hpp:23,
+                 from /usr/include/boost/archive/detail/basic_iarchive.hpp:28,
+                 from /usr/include/boost/serialization/map.hpp:24,
+                 from /orin_ssd/MORB_SLAM/include/KeyFrame.h:25,
+                 from /orin_ssd/MORB_SLAM/include/MapPoint.h:22,
+                 from /orin_ssd/MORB_SLAM/include/Map.h:22,
+                 from /orin_ssd/MORB_SLAM/src/Map.cc:22:
+/usr/include/c++/9/bits/std_function.h:770:5: note: candidate: ‘template<class _Res, class ... _Args> bool std::operator!=(std::nullptr_t, const std::function<_Res(_ArgTypes ...)>&)’
+  770 |     operator!=(nullptr_t, const function<_Res(_Args...)>& __f) noexcept
+      |     ^~~~~~~~
+/usr/include/c++/9/bits/std_function.h:770:5: note:   template argument deduction/substitution failed:
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   mismatched types ‘const std::function<_Res(_ArgTypes ...)>’ and ‘std::__shared_ptr<ORB_SLAM3::Map, __gnu_cxx::_S_atomic>::element_type*’ {aka ‘ORB_SLAM3::Map*’}
+  339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
+      |                                                 ^
+In file included from /usr/include/c++/9/unordered_map:47,
+                 from /usr/include/c++/9/functional:61,
+                 from /usr/include/c++/9/pstl/glue_algorithm_defs.h:13,
+                 from /usr/include/c++/9/algorithm:71,
+                 from /usr/include/boost/archive/detail/helper_collection.hpp:23,
+                 from /usr/include/boost/archive/detail/basic_iarchive.hpp:28,
+                 from /usr/include/boost/serialization/map.hpp:24,
+                 from /orin_ssd/MORB_SLAM/include/KeyFrame.h:25,
+                 from /orin_ssd/MORB_SLAM/include/MapPoint.h:22,
+                 from /orin_ssd/MORB_SLAM/include/Map.h:22,
+                 from /orin_ssd/MORB_SLAM/src/Map.cc:22:
+/usr/include/c++/9/bits/unordered_map.h:2099:5: note: candidate: ‘template<class _Key, class _Tp, class _Hash, class _Pred, class _Alloc> bool std::operator!=(const std::unordered_map<_Key1, _Tp1, _Hash1, _Pred1, _Alloc1>&, const std::unordered_map<_Key1, _Tp1, _Hash1, _Pred1, _Alloc1>&)’
+ 2099 |     operator!=(const unordered_map<_Key, _Tp, _Hash, _Pred, _Alloc>& __x,
+      |     ^~~~~~~~
+/usr/include/c++/9/bits/unordered_map.h:2099:5: note:   template argument deduction/substitution failed:
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<ORB_SLAM3::Map>’ is not derived from ‘const std::unordered_map<_Key1, _Tp1, _Hash1, _Pred1, _Alloc1>’
+  339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
+      |                                                 ^
+In file included from /usr/include/c++/9/unordered_map:47,
+                 from /usr/include/c++/9/functional:61,
+                 from /usr/include/c++/9/pstl/glue_algorithm_defs.h:13,
+                 from /usr/include/c++/9/algorithm:71,
+                 from /usr/include/boost/archive/detail/helper_collection.hpp:23,
+                 from /usr/include/boost/archive/detail/basic_iarchive.hpp:28,
+                 from /usr/include/boost/serialization/map.hpp:24,
+                 from /orin_ssd/MORB_SLAM/include/KeyFrame.h:25,
+                 from /orin_ssd/MORB_SLAM/include/MapPoint.h:22,
+                 from /orin_ssd/MORB_SLAM/include/Map.h:22,
+                 from /orin_ssd/MORB_SLAM/src/Map.cc:22:
+/usr/include/c++/9/bits/unordered_map.h:2111:5: note: candidate: ‘template<class _Key, class _Tp, class _Hash, class _Pred, class _Alloc> bool std::operator!=(const std::unordered_multimap<_Key1, _Tp1, _Hash1, _Pred1, _Alloc1>&, const std::unordered_multimap<_Key1, _Tp1, _Hash1, _Pred1, _Alloc1>&)’
+ 2111 |     operator!=(const unordered_multimap<_Key, _Tp, _Hash, _Pred, _Alloc>& __x,
+      |     ^~~~~~~~
+/usr/include/c++/9/bits/unordered_map.h:2111:5: note:   template argument deduction/substitution failed:
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<ORB_SLAM3::Map>’ is not derived from ‘const std::unordered_multimap<_Key1, _Tp1, _Hash1, _Pred1, _Alloc1>’
+  339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
+      |                                                 ^
+In file included from /usr/include/c++/9/bits/ios_base.h:46,
+                 from /usr/include/c++/9/ios:42,
+                 from /usr/include/c++/9/ostream:38,
+                 from /usr/include/c++/9/iterator:64,
+                 from /usr/include/boost/operators.hpp:98,
+                 from /usr/include/boost/serialization/strong_typedef.hpp:27,
+                 from /usr/include/boost/serialization/collection_size_type.hpp:10,
+                 from /usr/include/boost/serialization/map.hpp:27,
+                 from /orin_ssd/MORB_SLAM/include/KeyFrame.h:25,
+                 from /orin_ssd/MORB_SLAM/include/MapPoint.h:22,
+                 from /orin_ssd/MORB_SLAM/include/Map.h:22,
+                 from /orin_ssd/MORB_SLAM/src/Map.cc:22:
+/usr/include/c++/9/system_error:319:3: note: candidate: ‘bool std::operator!=(const std::error_code&, const std::error_code&)’
+  319 |   operator!=(const error_code& __lhs, const error_code& __rhs) noexcept
+      |   ^~~~~~~~
+/usr/include/c++/9/system_error:319:32: note:   no known conversion for argument 1 from ‘std::shared_ptr<ORB_SLAM3::Map>’ to ‘const std::error_code&’
+  319 |   operator!=(const error_code& __lhs, const error_code& __rhs) noexcept
+      |              ~~~~~~~~~~~~~~~~~~^~~~~
+/usr/include/c++/9/system_error:323:3: note: candidate: ‘bool std::operator!=(const std::error_code&, const std::error_condition&)’
+  323 |   operator!=(const error_code& __lhs, const error_condition& __rhs) noexcept
+      |   ^~~~~~~~
+/usr/include/c++/9/system_error:323:32: note:   no known conversion for argument 1 from ‘std::shared_ptr<ORB_SLAM3::Map>’ to ‘const std::error_code&’
+  323 |   operator!=(const error_code& __lhs, const error_condition& __rhs) noexcept
+      |              ~~~~~~~~~~~~~~~~~~^~~~~
+/usr/include/c++/9/system_error:327:3: note: candidate: ‘bool std::operator!=(const std::error_condition&, const std::error_code&)’
+  327 |   operator!=(const error_condition& __lhs, const error_code& __rhs) noexcept
+      |   ^~~~~~~~
+/usr/include/c++/9/system_error:327:37: note:   no known conversion for argument 1 from ‘std::shared_ptr<ORB_SLAM3::Map>’ to ‘const std::error_condition&’
+  327 |   operator!=(const error_condition& __lhs, const error_code& __rhs) noexcept
+      |              ~~~~~~~~~~~~~~~~~~~~~~~^~~~~
+/usr/include/c++/9/system_error:331:3: note: candidate: ‘bool std::operator!=(const std::error_condition&, const std::error_condition&)’
+  331 |   operator!=(const error_condition& __lhs,
+      |   ^~~~~~~~
+/usr/include/c++/9/system_error:331:37: note:   no known conversion for argument 1 from ‘std::shared_ptr<ORB_SLAM3::Map>’ to ‘const std::error_condition&’
+  331 |   operator!=(const error_condition& __lhs,
+      |              ~~~~~~~~~~~~~~~~~~~~~~~^~~~~
+In file included from /usr/include/c++/9/bits/locale_facets.h:48,
+                 from /usr/include/c++/9/bits/basic_ios.h:37,
+                 from /usr/include/c++/9/ios:44,
+                 from /usr/include/c++/9/ostream:38,
+                 from /usr/include/c++/9/iterator:64,
+                 from /usr/include/boost/operators.hpp:98,
+                 from /usr/include/boost/serialization/strong_typedef.hpp:27,
+                 from /usr/include/boost/serialization/collection_size_type.hpp:10,
+                 from /usr/include/boost/serialization/map.hpp:27,
+                 from /orin_ssd/MORB_SLAM/include/KeyFrame.h:25,
+                 from /orin_ssd/MORB_SLAM/include/MapPoint.h:22,
+                 from /orin_ssd/MORB_SLAM/include/Map.h:22,
+                 from /orin_ssd/MORB_SLAM/src/Map.cc:22:
+/usr/include/c++/9/bits/streambuf_iterator.h:214:5: note: candidate: ‘template<class _CharT, class _Traits> bool std::operator!=(const std::istreambuf_iterator<_CharT, _Traits>&, const std::istreambuf_iterator<_CharT, _Traits>&)’
+  214 |     operator!=(const istreambuf_iterator<_CharT, _Traits>& __a,
+      |     ^~~~~~~~
+/usr/include/c++/9/bits/streambuf_iterator.h:214:5: note:   template argument deduction/substitution failed:
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<ORB_SLAM3::Map>’ is not derived from ‘const std::istreambuf_iterator<_CharT, _Traits>’
+  339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
+      |                                                 ^
+In file included from /usr/include/c++/9/iterator:66,
+                 from /usr/include/boost/operators.hpp:98,
+                 from /usr/include/boost/serialization/strong_typedef.hpp:27,
+                 from /usr/include/boost/serialization/collection_size_type.hpp:10,
+                 from /usr/include/boost/serialization/map.hpp:27,
+                 from /orin_ssd/MORB_SLAM/include/KeyFrame.h:25,
+                 from /orin_ssd/MORB_SLAM/include/MapPoint.h:22,
+                 from /orin_ssd/MORB_SLAM/include/Map.h:22,
+                 from /orin_ssd/MORB_SLAM/src/Map.cc:22:
+/usr/include/c++/9/bits/stream_iterator.h:141:5: note: candidate: ‘template<class _Tp, class _CharT, class _Traits, class _Dist> bool std::operator!=(const std::istream_iterator<_Tp, _CharT, _Traits, _Dist>&, const std::istream_iterator<_Tp, _CharT, _Traits, _Dist>&)’
+  141 |     operator!=(const istream_iterator<_Tp, _CharT, _Traits, _Dist>& __x,
+      |     ^~~~~~~~
+/usr/include/c++/9/bits/stream_iterator.h:141:5: note:   template argument deduction/substitution failed:
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<ORB_SLAM3::Map>’ is not derived from ‘const std::istream_iterator<_Tp, _CharT, _Traits, _Dist>’
+  339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
+      |                                                 ^
+In file included from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/so2.hpp:7,
+                 from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/se2.hpp:7,
+                 from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/geometry.hpp:7,
+                 from /orin_ssd/MORB_SLAM/include/Frame.h:27,
+                 from /orin_ssd/MORB_SLAM/include/KeyFrame.h:31,
+                 from /orin_ssd/MORB_SLAM/include/MapPoint.h:22,
+                 from /orin_ssd/MORB_SLAM/include/Map.h:22,
+                 from /orin_ssd/MORB_SLAM/src/Map.cc:22:
+/usr/include/c++/9/complex:481:5: note: candidate: ‘template<class _Tp> constexpr bool std::operator!=(const std::complex<_Tp>&, const std::complex<_Tp>&)’
+  481 |     operator!=(const complex<_Tp>& __x, const complex<_Tp>& __y)
+      |     ^~~~~~~~
+/usr/include/c++/9/complex:481:5: note:   template argument deduction/substitution failed:
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<ORB_SLAM3::Map>’ is not derived from ‘const std::complex<_Tp>’
+  339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
+      |                                                 ^
+In file included from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/so2.hpp:7,
+                 from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/se2.hpp:7,
+                 from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/geometry.hpp:7,
+                 from /orin_ssd/MORB_SLAM/include/Frame.h:27,
+                 from /orin_ssd/MORB_SLAM/include/KeyFrame.h:31,
+                 from /orin_ssd/MORB_SLAM/include/MapPoint.h:22,
+                 from /orin_ssd/MORB_SLAM/include/Map.h:22,
+                 from /orin_ssd/MORB_SLAM/src/Map.cc:22:
+/usr/include/c++/9/complex:486:5: note: candidate: ‘template<class _Tp> constexpr bool std::operator!=(const std::complex<_Tp>&, const _Tp&)’
+  486 |     operator!=(const complex<_Tp>& __x, const _Tp& __y)
+      |     ^~~~~~~~
+/usr/include/c++/9/complex:486:5: note:   template argument deduction/substitution failed:
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<ORB_SLAM3::Map>’ is not derived from ‘const std::complex<_Tp>’
+  339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
+      |                                                 ^
+In file included from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/so2.hpp:7,
+                 from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/se2.hpp:7,
+                 from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/geometry.hpp:7,
+                 from /orin_ssd/MORB_SLAM/include/Frame.h:27,
+                 from /orin_ssd/MORB_SLAM/include/KeyFrame.h:31,
+                 from /orin_ssd/MORB_SLAM/include/MapPoint.h:22,
+                 from /orin_ssd/MORB_SLAM/include/Map.h:22,
+                 from /orin_ssd/MORB_SLAM/src/Map.cc:22:
+/usr/include/c++/9/complex:491:5: note: candidate: ‘template<class _Tp> constexpr bool std::operator!=(const _Tp&, const std::complex<_Tp>&)’
+  491 |     operator!=(const _Tp& __x, const complex<_Tp>& __y)
+      |     ^~~~~~~~
+/usr/include/c++/9/complex:491:5: note:   template argument deduction/substitution failed:
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   mismatched types ‘const std::complex<_Tp>’ and ‘std::__shared_ptr<ORB_SLAM3::Map, __gnu_cxx::_S_atomic>::element_type*’ {aka ‘ORB_SLAM3::Map*’}
+  339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
+      |                                                 ^
+In file included from /usr/include/c++/9/random:49,
+                 from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/common.hpp:10,
+                 from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/types.hpp:8,
+                 from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/rotation_matrix.hpp:10,
+                 from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/so2.hpp:14,
+                 from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/se2.hpp:7,
+                 from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/geometry.hpp:7,
+                 from /orin_ssd/MORB_SLAM/include/Frame.h:27,
+                 from /orin_ssd/MORB_SLAM/include/KeyFrame.h:31,
+                 from /orin_ssd/MORB_SLAM/include/MapPoint.h:22,
+                 from /orin_ssd/MORB_SLAM/include/Map.h:22,
+                 from /orin_ssd/MORB_SLAM/src/Map.cc:22:
+/usr/include/c++/9/bits/random.h:421:5: note: candidate: ‘template<class _UIntType, _UIntType __a, _UIntType __c, _UIntType __m> bool std::operator!=(const std::linear_congruential_engine<_UIntType, __a, __c, __m>&, const std::linear_congruential_engine<_UIntType, __a, __c, __m>&)’
+  421 |     operator!=(const std::linear_congruential_engine<_UIntType, __a,
+      |     ^~~~~~~~
+/usr/include/c++/9/bits/random.h:421:5: note:   template argument deduction/substitution failed:
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<ORB_SLAM3::Map>’ is not derived from ‘const std::linear_congruential_engine<_UIntType, __a, __c, __m>’
+  339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
+      |                                                 ^
+In file included from /usr/include/c++/9/random:49,
+                 from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/common.hpp:10,
+                 from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/types.hpp:8,
+                 from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/rotation_matrix.hpp:10,
+                 from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/so2.hpp:14,
+                 from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/se2.hpp:7,
+                 from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/geometry.hpp:7,
+                 from /orin_ssd/MORB_SLAM/include/Frame.h:27,
+                 from /orin_ssd/MORB_SLAM/include/KeyFrame.h:31,
+                 from /orin_ssd/MORB_SLAM/include/MapPoint.h:22,
+                 from /orin_ssd/MORB_SLAM/include/Map.h:22,
+                 from /orin_ssd/MORB_SLAM/src/Map.cc:22:
+/usr/include/c++/9/bits/random.h:658:5: note: candidate: ‘template<class _UIntType, long unsigned int __w, long unsigned int __n, long unsigned int __m, long unsigned int __r, _UIntType __a, long unsigned int __u, _UIntType __d, long unsigned int __s, _UIntType __b, long unsigned int __t, _UIntType __c, long unsigned int __l, _UIntType __f> bool std::operator!=(const std::mersenne_twister_engine<_UIntType, __w, __n, __m, __r, __a, __u, __d, __s, __b, __t, __c, __l, __f>&, const std::mersenne_twister_engine<_UIntType, __w, __n, __m, __r, __a, __u, __d, __s, __b, __t, __c, __l, __f>&)’
+  658 |     operator!=(const std::mersenne_twister_engine<_UIntType, __w, __n, __m,
+      |     ^~~~~~~~
+/usr/include/c++/9/bits/random.h:658:5: note:   template argument deduction/substitution failed:
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<ORB_SLAM3::Map>’ is not derived from ‘const std::mersenne_twister_engine<_UIntType, __w, __n, __m, __r, __a, __u, __d, __s, __b, __t, __c, __l, __f>’
+  339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
+      |                                                 ^
+In file included from /usr/include/c++/9/random:49,
+                 from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/common.hpp:10,
+                 from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/types.hpp:8,
+                 from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/rotation_matrix.hpp:10,
+                 from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/so2.hpp:14,
+                 from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/se2.hpp:7,
+                 from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/geometry.hpp:7,
+                 from /orin_ssd/MORB_SLAM/include/Frame.h:27,
+                 from /orin_ssd/MORB_SLAM/include/KeyFrame.h:31,
+                 from /orin_ssd/MORB_SLAM/include/MapPoint.h:22,
+                 from /orin_ssd/MORB_SLAM/include/Map.h:22,
+                 from /orin_ssd/MORB_SLAM/src/Map.cc:22:
+/usr/include/c++/9/bits/random.h:859:5: note: candidate: ‘template<class _UIntType, long unsigned int __w, long unsigned int __s, long unsigned int __r> bool std::operator!=(const std::subtract_with_carry_engine<_UIntType, __w, __s, __r>&, const std::subtract_with_carry_engine<_UIntType, __w, __s, __r>&)’
+  859 |     operator!=(const std::subtract_with_carry_engine<_UIntType, __w,
+      |     ^~~~~~~~
+/usr/include/c++/9/bits/random.h:859:5: note:   template argument deduction/substitution failed:
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<ORB_SLAM3::Map>’ is not derived from ‘const std::subtract_with_carry_engine<_UIntType, __w, __s, __r>’
+  339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
+      |                                                 ^
+In file included from /usr/include/c++/9/random:49,
+                 from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/common.hpp:10,
+                 from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/types.hpp:8,
+                 from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/rotation_matrix.hpp:10,
+                 from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/so2.hpp:14,
+                 from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/se2.hpp:7,
+                 from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/geometry.hpp:7,
+                 from /orin_ssd/MORB_SLAM/include/Frame.h:27,
+                 from /orin_ssd/MORB_SLAM/include/KeyFrame.h:31,
+                 from /orin_ssd/MORB_SLAM/include/MapPoint.h:22,
+                 from /orin_ssd/MORB_SLAM/include/Map.h:22,
+                 from /orin_ssd/MORB_SLAM/src/Map.cc:22:
+/usr/include/c++/9/bits/random.h:1082:5: note: candidate: ‘template<class _RandomNumberEngine, long unsigned int __p, long unsigned int __r> bool std::operator!=(const std::discard_block_engine<_RandomNumberEngine, __p, __r>&, const std::discard_block_engine<_RandomNumberEngine, __p, __r>&)’
+ 1082 |     operator!=(const std::discard_block_engine<_RandomNumberEngine, __p,
+      |     ^~~~~~~~
+/usr/include/c++/9/bits/random.h:1082:5: note:   template argument deduction/substitution failed:
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<ORB_SLAM3::Map>’ is not derived from ‘const std::discard_block_engine<_RandomNumberEngine, __p, __r>’
+  339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
+      |                                                 ^
+In file included from /usr/include/c++/9/random:49,
+                 from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/common.hpp:10,
+                 from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/types.hpp:8,
+                 from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/rotation_matrix.hpp:10,
+                 from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/so2.hpp:14,
+                 from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/se2.hpp:7,
+                 from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/geometry.hpp:7,
+                 from /orin_ssd/MORB_SLAM/include/Frame.h:27,
+                 from /orin_ssd/MORB_SLAM/include/KeyFrame.h:31,
+                 from /orin_ssd/MORB_SLAM/include/MapPoint.h:22,
+                 from /orin_ssd/MORB_SLAM/include/Map.h:22,
+                 from /orin_ssd/MORB_SLAM/src/Map.cc:22:
+/usr/include/c++/9/bits/random.h:1279:5: note: candidate: ‘template<class _RandomNumberEngine, long unsigned int __w, class _UIntType> bool std::operator!=(const std::independent_bits_engine<_RandomNumberEngine, __w, _UIntType>&, const std::independent_bits_engine<_RandomNumberEngine, __w, _UIntType>&)’
+ 1279 |     operator!=(const std::independent_bits_engine<_RandomNumberEngine, __w,
+      |     ^~~~~~~~
+/usr/include/c++/9/bits/random.h:1279:5: note:   template argument deduction/substitution failed:
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<ORB_SLAM3::Map>’ is not derived from ‘const std::independent_bits_engine<_RandomNumberEngine, __w, _UIntType>’
+  339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
+      |                                                 ^
+In file included from /usr/include/c++/9/random:49,
+                 from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/common.hpp:10,
+                 from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/types.hpp:8,
+                 from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/rotation_matrix.hpp:10,
+                 from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/so2.hpp:14,
+                 from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/se2.hpp:7,
+                 from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/geometry.hpp:7,
+                 from /orin_ssd/MORB_SLAM/include/Frame.h:27,
+                 from /orin_ssd/MORB_SLAM/include/KeyFrame.h:31,
+                 from /orin_ssd/MORB_SLAM/include/MapPoint.h:22,
+                 from /orin_ssd/MORB_SLAM/include/Map.h:22,
+                 from /orin_ssd/MORB_SLAM/src/Map.cc:22:
+/usr/include/c++/9/bits/random.h:1532:5: note: candidate: ‘template<class _RandomNumberEngine, long unsigned int __k> bool std::operator!=(const std::shuffle_order_engine<_RandomNumberEngine, __k>&, const std::shuffle_order_engine<_RandomNumberEngine, __k>&)’
+ 1532 |     operator!=(const std::shuffle_order_engine<_RandomNumberEngine,
+      |     ^~~~~~~~
+/usr/include/c++/9/bits/random.h:1532:5: note:   template argument deduction/substitution failed:
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<ORB_SLAM3::Map>’ is not derived from ‘const std::shuffle_order_engine<_RandomNumberEngine, __k>’
+  339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
+      |                                                 ^
+In file included from /usr/include/c++/9/random:49,
+                 from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/common.hpp:10,
+                 from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/types.hpp:8,
+                 from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/rotation_matrix.hpp:10,
+                 from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/so2.hpp:14,
+                 from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/se2.hpp:7,
+                 from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/geometry.hpp:7,
+                 from /orin_ssd/MORB_SLAM/include/Frame.h:27,
+                 from /orin_ssd/MORB_SLAM/include/KeyFrame.h:31,
+                 from /orin_ssd/MORB_SLAM/include/MapPoint.h:22,
+                 from /orin_ssd/MORB_SLAM/include/Map.h:22,
+                 from /orin_ssd/MORB_SLAM/src/Map.cc:22:
+/usr/include/c++/9/bits/random.h:1692:5: note: candidate: ‘template<class _IntType> bool std::operator!=(const std::uniform_int_distribution<_IntType>&, const std::uniform_int_distribution<_IntType>&)’
+ 1692 |     operator!=(const std::uniform_int_distribution<_IntType>& __d1,
+      |     ^~~~~~~~
+/usr/include/c++/9/bits/random.h:1692:5: note:   template argument deduction/substitution failed:
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<ORB_SLAM3::Map>’ is not derived from ‘const std::uniform_int_distribution<_IntType>’
+  339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
+      |                                                 ^
+In file included from /usr/include/c++/9/random:49,
+                 from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/common.hpp:10,
+                 from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/types.hpp:8,
+                 from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/rotation_matrix.hpp:10,
+                 from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/so2.hpp:14,
+                 from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/se2.hpp:7,
+                 from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/geometry.hpp:7,
+                 from /orin_ssd/MORB_SLAM/include/Frame.h:27,
+                 from /orin_ssd/MORB_SLAM/include/KeyFrame.h:31,
+                 from /orin_ssd/MORB_SLAM/include/MapPoint.h:22,
+                 from /orin_ssd/MORB_SLAM/include/Map.h:22,
+                 from /orin_ssd/MORB_SLAM/src/Map.cc:22:
+/usr/include/c++/9/bits/random.h:1913:5: note: candidate: ‘template<class _IntType> bool std::operator!=(const std::uniform_real_distribution<_IntType>&, const std::uniform_real_distribution<_IntType>&)’
+ 1913 |     operator!=(const std::uniform_real_distribution<_IntType>& __d1,
+      |     ^~~~~~~~
+/usr/include/c++/9/bits/random.h:1913:5: note:   template argument deduction/substitution failed:
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<ORB_SLAM3::Map>’ is not derived from ‘const std::uniform_real_distribution<_IntType>’
+  339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
+      |                                                 ^
+In file included from /usr/include/c++/9/random:49,
+                 from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/common.hpp:10,
+                 from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/types.hpp:8,
+                 from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/rotation_matrix.hpp:10,
+                 from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/so2.hpp:14,
+                 from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/se2.hpp:7,
+                 from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/geometry.hpp:7,
+                 from /orin_ssd/MORB_SLAM/include/Frame.h:27,
+                 from /orin_ssd/MORB_SLAM/include/KeyFrame.h:31,
+                 from /orin_ssd/MORB_SLAM/include/MapPoint.h:22,
+                 from /orin_ssd/MORB_SLAM/include/Map.h:22,
+                 from /orin_ssd/MORB_SLAM/src/Map.cc:22:
+/usr/include/c++/9/bits/random.h:2170:5: note: candidate: ‘template<class _RealType> bool std::operator!=(const std::normal_distribution<_RealType>&, const std::normal_distribution<_RealType>&)’
+ 2170 |     operator!=(const std::normal_distribution<_RealType>& __d1,
+      |     ^~~~~~~~
+/usr/include/c++/9/bits/random.h:2170:5: note:   template argument deduction/substitution failed:
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<ORB_SLAM3::Map>’ is not derived from ‘const std::normal_distribution<_RealType>’
+  339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
+      |                                                 ^
+In file included from /usr/include/c++/9/random:49,
+                 from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/common.hpp:10,
+                 from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/types.hpp:8,
+                 from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/rotation_matrix.hpp:10,
+                 from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/so2.hpp:14,
+                 from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/se2.hpp:7,
+                 from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/geometry.hpp:7,
+                 from /orin_ssd/MORB_SLAM/include/Frame.h:27,
+                 from /orin_ssd/MORB_SLAM/include/KeyFrame.h:31,
+                 from /orin_ssd/MORB_SLAM/include/MapPoint.h:22,
+                 from /orin_ssd/MORB_SLAM/include/Map.h:22,
+                 from /orin_ssd/MORB_SLAM/src/Map.cc:22:
+/usr/include/c++/9/bits/random.h:2381:5: note: candidate: ‘template<class _RealType> bool std::operator!=(const std::lognormal_distribution<_RealType>&, const std::lognormal_distribution<_RealType>&)’
+ 2381 |     operator!=(const std::lognormal_distribution<_RealType>& __d1,
+      |     ^~~~~~~~
+/usr/include/c++/9/bits/random.h:2381:5: note:   template argument deduction/substitution failed:
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<ORB_SLAM3::Map>’ is not derived from ‘const std::lognormal_distribution<_RealType>’
+  339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
+      |                                                 ^
+In file included from /usr/include/c++/9/random:49,
+                 from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/common.hpp:10,
+                 from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/types.hpp:8,
+                 from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/rotation_matrix.hpp:10,
+                 from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/so2.hpp:14,
+                 from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/se2.hpp:7,
+                 from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/geometry.hpp:7,
+                 from /orin_ssd/MORB_SLAM/include/Frame.h:27,
+                 from /orin_ssd/MORB_SLAM/include/KeyFrame.h:31,
+                 from /orin_ssd/MORB_SLAM/include/MapPoint.h:22,
+                 from /orin_ssd/MORB_SLAM/include/Map.h:22,
+                 from /orin_ssd/MORB_SLAM/src/Map.cc:22:
+/usr/include/c++/9/bits/random.h:2612:6: note: candidate: ‘template<class _RealType> bool std::operator!=(const std::gamma_distribution<_RealType>&, const std::gamma_distribution<_RealType>&)’
+ 2612 |      operator!=(const std::gamma_distribution<_RealType>& __d1,
+      |      ^~~~~~~~
+/usr/include/c++/9/bits/random.h:2612:6: note:   template argument deduction/substitution failed:
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<ORB_SLAM3::Map>’ is not derived from ‘const std::gamma_distribution<_RealType>’
+  339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
+      |                                                 ^
+In file included from /usr/include/c++/9/random:49,
+                 from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/common.hpp:10,
+                 from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/types.hpp:8,
+                 from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/rotation_matrix.hpp:10,
+                 from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/so2.hpp:14,
+                 from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/se2.hpp:7,
+                 from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/geometry.hpp:7,
+                 from /orin_ssd/MORB_SLAM/include/Frame.h:27,
+                 from /orin_ssd/MORB_SLAM/include/KeyFrame.h:31,
+                 from /orin_ssd/MORB_SLAM/include/MapPoint.h:22,
+                 from /orin_ssd/MORB_SLAM/include/Map.h:22,
+                 from /orin_ssd/MORB_SLAM/src/Map.cc:22:
+/usr/include/c++/9/bits/random.h:2836:5: note: candidate: ‘template<class _RealType> bool std::operator!=(const std::chi_squared_distribution<_RealType>&, const std::chi_squared_distribution<_RealType>&)’
+ 2836 |     operator!=(const std::chi_squared_distribution<_RealType>& __d1,
+      |     ^~~~~~~~
+/usr/include/c++/9/bits/random.h:2836:5: note:   template argument deduction/substitution failed:
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<ORB_SLAM3::Map>’ is not derived from ‘const std::chi_squared_distribution<_RealType>’
+  339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
+      |                                                 ^
+In file included from /usr/include/c++/9/random:49,
+                 from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/common.hpp:10,
+                 from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/types.hpp:8,
+                 from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/rotation_matrix.hpp:10,
+                 from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/so2.hpp:14,
+                 from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/se2.hpp:7,
+                 from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/geometry.hpp:7,
+                 from /orin_ssd/MORB_SLAM/include/Frame.h:27,
+                 from /orin_ssd/MORB_SLAM/include/KeyFrame.h:31,
+                 from /orin_ssd/MORB_SLAM/include/MapPoint.h:22,
+                 from /orin_ssd/MORB_SLAM/include/Map.h:22,
+                 from /orin_ssd/MORB_SLAM/src/Map.cc:22:
+/usr/include/c++/9/bits/random.h:3010:5: note: candidate: ‘template<class _RealType> bool std::operator!=(const std::cauchy_distribution<_RealType>&, const std::cauchy_distribution<_RealType>&)’
+ 3010 |     operator!=(const std::cauchy_distribution<_RealType>& __d1,
+      |     ^~~~~~~~
+/usr/include/c++/9/bits/random.h:3010:5: note:   template argument deduction/substitution failed:
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<ORB_SLAM3::Map>’ is not derived from ‘const std::cauchy_distribution<_RealType>’
+  339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
+      |                                                 ^
+In file included from /usr/include/c++/9/random:49,
+                 from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/common.hpp:10,
+                 from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/types.hpp:8,
+                 from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/rotation_matrix.hpp:10,
+                 from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/so2.hpp:14,
+                 from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/se2.hpp:7,
+                 from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/geometry.hpp:7,
+                 from /orin_ssd/MORB_SLAM/include/Frame.h:27,
+                 from /orin_ssd/MORB_SLAM/include/KeyFrame.h:31,
+                 from /orin_ssd/MORB_SLAM/include/MapPoint.h:22,
+                 from /orin_ssd/MORB_SLAM/include/Map.h:22,
+                 from /orin_ssd/MORB_SLAM/src/Map.cc:22:
+/usr/include/c++/9/bits/random.h:3274:5: note: candidate: ‘template<class _RealType> bool std::operator!=(const std::fisher_f_distribution<_RealType>&, const std::fisher_f_distribution<_RealType>&)’
+ 3274 |     operator!=(const std::fisher_f_distribution<_RealType>& __d1,
+      |     ^~~~~~~~
+/usr/include/c++/9/bits/random.h:3274:5: note:   template argument deduction/substitution failed:
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<ORB_SLAM3::Map>’ is not derived from ‘const std::fisher_f_distribution<_RealType>’
+  339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
+      |                                                 ^
+In file included from /usr/include/c++/9/random:49,
+                 from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/common.hpp:10,
+                 from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/types.hpp:8,
+                 from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/rotation_matrix.hpp:10,
+                 from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/so2.hpp:14,
+                 from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/se2.hpp:7,
+                 from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/geometry.hpp:7,
+                 from /orin_ssd/MORB_SLAM/include/Frame.h:27,
+                 from /orin_ssd/MORB_SLAM/include/KeyFrame.h:31,
+                 from /orin_ssd/MORB_SLAM/include/MapPoint.h:22,
+                 from /orin_ssd/MORB_SLAM/include/Map.h:22,
+                 from /orin_ssd/MORB_SLAM/src/Map.cc:22:
+/usr/include/c++/9/bits/random.h:3496:5: note: candidate: ‘template<class _RealType> bool std::operator!=(const std::student_t_distribution<_RealType>&, const std::student_t_distribution<_RealType>&)’
+ 3496 |     operator!=(const std::student_t_distribution<_RealType>& __d1,
+      |     ^~~~~~~~
+/usr/include/c++/9/bits/random.h:3496:5: note:   template argument deduction/substitution failed:
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<ORB_SLAM3::Map>’ is not derived from ‘const std::student_t_distribution<_RealType>’
+  339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
+      |                                                 ^
+In file included from /usr/include/c++/9/random:49,
+                 from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/common.hpp:10,
+                 from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/types.hpp:8,
+                 from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/rotation_matrix.hpp:10,
+                 from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/so2.hpp:14,
+                 from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/se2.hpp:7,
+                 from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/geometry.hpp:7,
+                 from /orin_ssd/MORB_SLAM/include/Frame.h:27,
+                 from /orin_ssd/MORB_SLAM/include/KeyFrame.h:31,
+                 from /orin_ssd/MORB_SLAM/include/MapPoint.h:22,
+                 from /orin_ssd/MORB_SLAM/include/Map.h:22,
+                 from /orin_ssd/MORB_SLAM/src/Map.cc:22:
+/usr/include/c++/9/bits/random.h:3684:3: note: candidate: ‘bool std::operator!=(const std::bernoulli_distribution&, const std::bernoulli_distribution&)’
+ 3684 |   operator!=(const std::bernoulli_distribution& __d1,
+      |   ^~~~~~~~
+/usr/include/c++/9/bits/random.h:3684:49: note:   no known conversion for argument 1 from ‘std::shared_ptr<ORB_SLAM3::Map>’ to ‘const std::bernoulli_distribution&’
+ 3684 |   operator!=(const std::bernoulli_distribution& __d1,
+      |              ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~^~~~
+/usr/include/c++/9/bits/random.h:3959:5: note: candidate: ‘template<class _IntType> bool std::operator!=(const std::binomial_distribution<_IntType>&, const std::binomial_distribution<_IntType>&)’
+ 3959 |     operator!=(const std::binomial_distribution<_IntType>& __d1,
+      |     ^~~~~~~~
+/usr/include/c++/9/bits/random.h:3959:5: note:   template argument deduction/substitution failed:
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<ORB_SLAM3::Map>’ is not derived from ‘const std::binomial_distribution<_IntType>’
+  339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
+      |                                                 ^
+In file included from /usr/include/c++/9/random:49,
+                 from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/common.hpp:10,
+                 from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/types.hpp:8,
+                 from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/rotation_matrix.hpp:10,
+                 from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/so2.hpp:14,
+                 from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/se2.hpp:7,
+                 from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/geometry.hpp:7,
+                 from /orin_ssd/MORB_SLAM/include/Frame.h:27,
+                 from /orin_ssd/MORB_SLAM/include/KeyFrame.h:31,
+                 from /orin_ssd/MORB_SLAM/include/MapPoint.h:22,
+                 from /orin_ssd/MORB_SLAM/include/Map.h:22,
+                 from /orin_ssd/MORB_SLAM/src/Map.cc:22:
+/usr/include/c++/9/bits/random.h:4138:5: note: candidate: ‘template<class _IntType> bool std::operator!=(const std::geometric_distribution<_IntType>&, const std::geometric_distribution<_IntType>&)’
+ 4138 |     operator!=(const std::geometric_distribution<_IntType>& __d1,
+      |     ^~~~~~~~
+/usr/include/c++/9/bits/random.h:4138:5: note:   template argument deduction/substitution failed:
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<ORB_SLAM3::Map>’ is not derived from ‘const std::geometric_distribution<_IntType>’
+  339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
+      |                                                 ^
+In file included from /usr/include/c++/9/random:49,
+                 from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/common.hpp:10,
+                 from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/types.hpp:8,
+                 from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/rotation_matrix.hpp:10,
+                 from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/so2.hpp:14,
+                 from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/se2.hpp:7,
+                 from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/geometry.hpp:7,
+                 from /orin_ssd/MORB_SLAM/include/Frame.h:27,
+                 from /orin_ssd/MORB_SLAM/include/KeyFrame.h:31,
+                 from /orin_ssd/MORB_SLAM/include/MapPoint.h:22,
+                 from /orin_ssd/MORB_SLAM/include/Map.h:22,
+                 from /orin_ssd/MORB_SLAM/src/Map.cc:22:
+/usr/include/c++/9/bits/random.h:4392:5: note: candidate: ‘template<class _IntType> bool std::operator!=(const std::negative_binomial_distribution<_IntType>&, const std::negative_binomial_distribution<_IntType>&)’
+ 4392 |     operator!=(const std::negative_binomial_distribution<_IntType>& __d1,
+      |     ^~~~~~~~
+/usr/include/c++/9/bits/random.h:4392:5: note:   template argument deduction/substitution failed:
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<ORB_SLAM3::Map>’ is not derived from ‘const std::negative_binomial_distribution<_IntType>’
+  339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
+      |                                                 ^
+In file included from /usr/include/c++/9/random:49,
+                 from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/common.hpp:10,
+                 from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/types.hpp:8,
+                 from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/rotation_matrix.hpp:10,
+                 from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/so2.hpp:14,
+                 from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/se2.hpp:7,
+                 from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/geometry.hpp:7,
+                 from /orin_ssd/MORB_SLAM/include/Frame.h:27,
+                 from /orin_ssd/MORB_SLAM/include/KeyFrame.h:31,
+                 from /orin_ssd/MORB_SLAM/include/MapPoint.h:22,
+                 from /orin_ssd/MORB_SLAM/include/Map.h:22,
+                 from /orin_ssd/MORB_SLAM/src/Map.cc:22:
+/usr/include/c++/9/bits/random.h:4618:5: note: candidate: ‘template<class _IntType> bool std::operator!=(const std::poisson_distribution<_IntType>&, const std::poisson_distribution<_IntType>&)’
+ 4618 |     operator!=(const std::poisson_distribution<_IntType>& __d1,
+      |     ^~~~~~~~
+/usr/include/c++/9/bits/random.h:4618:5: note:   template argument deduction/substitution failed:
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<ORB_SLAM3::Map>’ is not derived from ‘const std::poisson_distribution<_IntType>’
+  339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
+      |                                                 ^
+In file included from /usr/include/c++/9/random:49,
+                 from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/common.hpp:10,
+                 from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/types.hpp:8,
+                 from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/rotation_matrix.hpp:10,
+                 from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/so2.hpp:14,
+                 from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/se2.hpp:7,
+                 from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/geometry.hpp:7,
+                 from /orin_ssd/MORB_SLAM/include/Frame.h:27,
+                 from /orin_ssd/MORB_SLAM/include/KeyFrame.h:31,
+                 from /orin_ssd/MORB_SLAM/include/MapPoint.h:22,
+                 from /orin_ssd/MORB_SLAM/include/Map.h:22,
+                 from /orin_ssd/MORB_SLAM/src/Map.cc:22:
+/usr/include/c++/9/bits/random.h:4809:5: note: candidate: ‘template<class _RealType> bool std::operator!=(const std::exponential_distribution<_RealType>&, const std::exponential_distribution<_RealType>&)’
+ 4809 |     operator!=(const std::exponential_distribution<_RealType>& __d1,
+      |     ^~~~~~~~
+/usr/include/c++/9/bits/random.h:4809:5: note:   template argument deduction/substitution failed:
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<ORB_SLAM3::Map>’ is not derived from ‘const std::exponential_distribution<_RealType>’
+  339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
+      |                                                 ^
+In file included from /usr/include/c++/9/random:49,
+                 from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/common.hpp:10,
+                 from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/types.hpp:8,
+                 from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/rotation_matrix.hpp:10,
+                 from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/so2.hpp:14,
+                 from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/se2.hpp:7,
+                 from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/geometry.hpp:7,
+                 from /orin_ssd/MORB_SLAM/include/Frame.h:27,
+                 from /orin_ssd/MORB_SLAM/include/KeyFrame.h:31,
+                 from /orin_ssd/MORB_SLAM/include/MapPoint.h:22,
+                 from /orin_ssd/MORB_SLAM/include/Map.h:22,
+                 from /orin_ssd/MORB_SLAM/src/Map.cc:22:
+/usr/include/c++/9/bits/random.h:5019:5: note: candidate: ‘template<class _RealType> bool std::operator!=(const std::weibull_distribution<_RealType>&, const std::weibull_distribution<_RealType>&)’
+ 5019 |     operator!=(const std::weibull_distribution<_RealType>& __d1,
+      |     ^~~~~~~~
+/usr/include/c++/9/bits/random.h:5019:5: note:   template argument deduction/substitution failed:
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<ORB_SLAM3::Map>’ is not derived from ‘const std::weibull_distribution<_RealType>’
+  339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
+      |                                                 ^
+In file included from /usr/include/c++/9/random:49,
+                 from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/common.hpp:10,
+                 from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/types.hpp:8,
+                 from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/rotation_matrix.hpp:10,
+                 from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/so2.hpp:14,
+                 from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/se2.hpp:7,
+                 from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/geometry.hpp:7,
+                 from /orin_ssd/MORB_SLAM/include/Frame.h:27,
+                 from /orin_ssd/MORB_SLAM/include/KeyFrame.h:31,
+                 from /orin_ssd/MORB_SLAM/include/MapPoint.h:22,
+                 from /orin_ssd/MORB_SLAM/include/Map.h:22,
+                 from /orin_ssd/MORB_SLAM/src/Map.cc:22:
+/usr/include/c++/9/bits/random.h:5229:5: note: candidate: ‘template<class _RealType> bool std::operator!=(const std::extreme_value_distribution<_RealType>&, const std::extreme_value_distribution<_RealType>&)’
+ 5229 |     operator!=(const std::extreme_value_distribution<_RealType>& __d1,
+      |     ^~~~~~~~
+/usr/include/c++/9/bits/random.h:5229:5: note:   template argument deduction/substitution failed:
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<ORB_SLAM3::Map>’ is not derived from ‘const std::extreme_value_distribution<_RealType>’
+  339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
+      |                                                 ^
+In file included from /usr/include/c++/9/random:49,
+                 from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/common.hpp:10,
+                 from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/types.hpp:8,
+                 from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/rotation_matrix.hpp:10,
+                 from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/so2.hpp:14,
+                 from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/se2.hpp:7,
+                 from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/geometry.hpp:7,
+                 from /orin_ssd/MORB_SLAM/include/Frame.h:27,
+                 from /orin_ssd/MORB_SLAM/include/KeyFrame.h:31,
+                 from /orin_ssd/MORB_SLAM/include/MapPoint.h:22,
+                 from /orin_ssd/MORB_SLAM/include/Map.h:22,
+                 from /orin_ssd/MORB_SLAM/src/Map.cc:22:
+/usr/include/c++/9/bits/random.h:5494:5: note: candidate: ‘template<class _IntType> bool std::operator!=(const std::discrete_distribution<_IntType>&, const std::discrete_distribution<_IntType>&)’
+ 5494 |     operator!=(const std::discrete_distribution<_IntType>& __d1,
+      |     ^~~~~~~~
+/usr/include/c++/9/bits/random.h:5494:5: note:   template argument deduction/substitution failed:
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<ORB_SLAM3::Map>’ is not derived from ‘const std::discrete_distribution<_IntType>’
+  339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
+      |                                                 ^
+In file included from /usr/include/c++/9/random:49,
+                 from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/common.hpp:10,
+                 from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/types.hpp:8,
+                 from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/rotation_matrix.hpp:10,
+                 from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/so2.hpp:14,
+                 from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/se2.hpp:7,
+                 from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/geometry.hpp:7,
+                 from /orin_ssd/MORB_SLAM/include/Frame.h:27,
+                 from /orin_ssd/MORB_SLAM/include/KeyFrame.h:31,
+                 from /orin_ssd/MORB_SLAM/include/MapPoint.h:22,
+                 from /orin_ssd/MORB_SLAM/include/Map.h:22,
+                 from /orin_ssd/MORB_SLAM/src/Map.cc:22:
+/usr/include/c++/9/bits/random.h:5765:5: note: candidate: ‘template<class _RealType> bool std::operator!=(const std::piecewise_constant_distribution<_RealType>&, const std::piecewise_constant_distribution<_RealType>&)’
+ 5765 |     operator!=(const std::piecewise_constant_distribution<_RealType>& __d1,
+      |     ^~~~~~~~
+/usr/include/c++/9/bits/random.h:5765:5: note:   template argument deduction/substitution failed:
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<ORB_SLAM3::Map>’ is not derived from ‘const std::piecewise_constant_distribution<_RealType>’
+  339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
+      |                                                 ^
+In file included from /usr/include/c++/9/random:49,
+                 from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/common.hpp:10,
+                 from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/types.hpp:8,
+                 from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/rotation_matrix.hpp:10,
+                 from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/so2.hpp:14,
+                 from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/se2.hpp:7,
+                 from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/geometry.hpp:7,
+                 from /orin_ssd/MORB_SLAM/include/Frame.h:27,
+                 from /orin_ssd/MORB_SLAM/include/KeyFrame.h:31,
+                 from /orin_ssd/MORB_SLAM/include/MapPoint.h:22,
+                 from /orin_ssd/MORB_SLAM/include/Map.h:22,
+                 from /orin_ssd/MORB_SLAM/src/Map.cc:22:
+/usr/include/c++/9/bits/random.h:6038:5: note: candidate: ‘template<class _RealType> bool std::operator!=(const std::piecewise_linear_distribution<_RealType>&, const std::piecewise_linear_distribution<_RealType>&)’
+ 6038 |     operator!=(const std::piecewise_linear_distribution<_RealType>& __d1,
+      |     ^~~~~~~~
+/usr/include/c++/9/bits/random.h:6038:5: note:   template argument deduction/substitution failed:
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<ORB_SLAM3::Map>’ is not derived from ‘const std::piecewise_linear_distribution<_RealType>’
+  339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
+      |                                                 ^
+In file included from /usr/include/c++/9/set:61,
+                 from /orin_ssd/MORB_SLAM/Thirdparty/g2o/g2o/types/../core/optimizable_graph.h:30,
+                 from /orin_ssd/MORB_SLAM/Thirdparty/g2o/g2o/types/../core/base_vertex.h:30,
+                 from /orin_ssd/MORB_SLAM/Thirdparty/g2o/g2o/types/types_seven_dof_expmap.h:34,
+                 from /orin_ssd/MORB_SLAM/include/Converter.h:27,
+                 from /orin_ssd/MORB_SLAM/include/Frame.h:32,
+                 from /orin_ssd/MORB_SLAM/include/KeyFrame.h:31,
+                 from /orin_ssd/MORB_SLAM/include/MapPoint.h:22,
+                 from /orin_ssd/MORB_SLAM/include/Map.h:22,
+                 from /orin_ssd/MORB_SLAM/src/Map.cc:22:
+/usr/include/c++/9/bits/stl_set.h:1003:5: note: candidate: ‘template<class _Key, class _Compare, class _Alloc> bool std::operator!=(const std::set<_Key, _Compare, _Allocator>&, const std::set<_Key, _Compare, _Allocator>&)’
+ 1003 |     operator!=(const set<_Key, _Compare, _Alloc>& __x,
+      |     ^~~~~~~~
+/usr/include/c++/9/bits/stl_set.h:1003:5: note:   template argument deduction/substitution failed:
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<ORB_SLAM3::Map>’ is not derived from ‘const std::set<_Key, _Compare, _Allocator>’
+  339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
+      |                                                 ^
+In file included from /usr/include/c++/9/set:62,
+                 from /orin_ssd/MORB_SLAM/Thirdparty/g2o/g2o/types/../core/optimizable_graph.h:30,
+                 from /orin_ssd/MORB_SLAM/Thirdparty/g2o/g2o/types/../core/base_vertex.h:30,
+                 from /orin_ssd/MORB_SLAM/Thirdparty/g2o/g2o/types/types_seven_dof_expmap.h:34,
+                 from /orin_ssd/MORB_SLAM/include/Converter.h:27,
+                 from /orin_ssd/MORB_SLAM/include/Frame.h:32,
+                 from /orin_ssd/MORB_SLAM/include/KeyFrame.h:31,
+                 from /orin_ssd/MORB_SLAM/include/MapPoint.h:22,
+                 from /orin_ssd/MORB_SLAM/include/Map.h:22,
+                 from /orin_ssd/MORB_SLAM/src/Map.cc:22:
+/usr/include/c++/9/bits/stl_multiset.h:988:5: note: candidate: ‘template<class _Key, class _Compare, class _Alloc> bool std::operator!=(const std::multiset<_Key, _Compare, _Allocator>&, const std::multiset<_Key, _Compare, _Allocator>&)’
+  988 |     operator!=(const multiset<_Key, _Compare, _Alloc>& __x,
+      |     ^~~~~~~~
+/usr/include/c++/9/bits/stl_multiset.h:988:5: note:   template argument deduction/substitution failed:
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<ORB_SLAM3::Map>’ is not derived from ‘const std::multiset<_Key, _Compare, _Allocator>’
+  339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
+      |                                                 ^
+In file included from /usr/include/c++/9/list:63,
+                 from /orin_ssd/MORB_SLAM/Thirdparty/g2o/g2o/types/../core/optimizable_graph.h:32,
+                 from /orin_ssd/MORB_SLAM/Thirdparty/g2o/g2o/types/../core/base_vertex.h:30,
+                 from /orin_ssd/MORB_SLAM/Thirdparty/g2o/g2o/types/types_seven_dof_expmap.h:34,
+                 from /orin_ssd/MORB_SLAM/include/Converter.h:27,
+                 from /orin_ssd/MORB_SLAM/include/Frame.h:32,
+                 from /orin_ssd/MORB_SLAM/include/KeyFrame.h:31,
+                 from /orin_ssd/MORB_SLAM/include/MapPoint.h:22,
+                 from /orin_ssd/MORB_SLAM/include/Map.h:22,
+                 from /orin_ssd/MORB_SLAM/src/Map.cc:22:
+/usr/include/c++/9/bits/stl_list.h:2032:5: note: candidate: ‘template<class _Tp, class _Alloc> bool std::operator!=(const std::__cxx11::list<_Tp, _Alloc>&, const std::__cxx11::list<_Tp, _Alloc>&)’
+ 2032 |     operator!=(const list<_Tp, _Alloc>& __x, const list<_Tp, _Alloc>& __y)
+      |     ^~~~~~~~
+/usr/include/c++/9/bits/stl_list.h:2032:5: note:   template argument deduction/substitution failed:
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<ORB_SLAM3::Map>’ is not derived from ‘const std::__cxx11::list<_Tp, _Alloc>’
+  339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
+      |                                                 ^
+In file included from /usr/include/c++/9/deque:67,
+                 from /usr/include/c++/9/stack:60,
+                 from /orin_ssd/MORB_SLAM/Thirdparty/g2o/g2o/types/../core/base_vertex.h:38,
+                 from /orin_ssd/MORB_SLAM/Thirdparty/g2o/g2o/types/types_seven_dof_expmap.h:34,
+                 from /orin_ssd/MORB_SLAM/include/Converter.h:27,
+                 from /orin_ssd/MORB_SLAM/include/Frame.h:32,
+                 from /orin_ssd/MORB_SLAM/include/KeyFrame.h:31,
+                 from /orin_ssd/MORB_SLAM/include/MapPoint.h:22,
+                 from /orin_ssd/MORB_SLAM/include/Map.h:22,
+                 from /orin_ssd/MORB_SLAM/src/Map.cc:22:
+/usr/include/c++/9/bits/stl_deque.h:299:5: note: candidate: ‘template<class _Tp, class _Ref, class _Ptr> bool std::operator!=(const std::_Deque_iterator<_Tp, _Ref, _Ptr>&, const std::_Deque_iterator<_Tp, _Ref, _Ptr>&)’
+  299 |     operator!=(const _Deque_iterator<_Tp, _Ref, _Ptr>& __x,
+      |     ^~~~~~~~
+/usr/include/c++/9/bits/stl_deque.h:299:5: note:   template argument deduction/substitution failed:
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<ORB_SLAM3::Map>’ is not derived from ‘const std::_Deque_iterator<_Tp, _Ref, _Ptr>’
+  339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
+      |                                                 ^
+In file included from /usr/include/c++/9/deque:67,
+                 from /usr/include/c++/9/stack:60,
+                 from /orin_ssd/MORB_SLAM/Thirdparty/g2o/g2o/types/../core/base_vertex.h:38,
+                 from /orin_ssd/MORB_SLAM/Thirdparty/g2o/g2o/types/types_seven_dof_expmap.h:34,
+                 from /orin_ssd/MORB_SLAM/include/Converter.h:27,
+                 from /orin_ssd/MORB_SLAM/include/Frame.h:32,
+                 from /orin_ssd/MORB_SLAM/include/KeyFrame.h:31,
+                 from /orin_ssd/MORB_SLAM/include/MapPoint.h:22,
+                 from /orin_ssd/MORB_SLAM/include/Map.h:22,
+                 from /orin_ssd/MORB_SLAM/src/Map.cc:22:
+/usr/include/c++/9/bits/stl_deque.h:306:5: note: candidate: ‘template<class _Tp, class _RefL, class _PtrL, class _RefR, class _PtrR> bool std::operator!=(const std::_Deque_iterator<_Tp, _Ref, _Ptr>&, const std::_Deque_iterator<_Tp, _RefR, _PtrR>&)’
+  306 |     operator!=(const _Deque_iterator<_Tp, _RefL, _PtrL>& __x,
+      |     ^~~~~~~~
+/usr/include/c++/9/bits/stl_deque.h:306:5: note:   template argument deduction/substitution failed:
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<ORB_SLAM3::Map>’ is not derived from ‘const std::_Deque_iterator<_Tp, _Ref, _Ptr>’
+  339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
+      |                                                 ^
+In file included from /usr/include/c++/9/deque:67,
+                 from /usr/include/c++/9/stack:60,
+                 from /orin_ssd/MORB_SLAM/Thirdparty/g2o/g2o/types/../core/base_vertex.h:38,
+                 from /orin_ssd/MORB_SLAM/Thirdparty/g2o/g2o/types/types_seven_dof_expmap.h:34,
+                 from /orin_ssd/MORB_SLAM/include/Converter.h:27,
+                 from /orin_ssd/MORB_SLAM/include/Frame.h:32,
+                 from /orin_ssd/MORB_SLAM/include/KeyFrame.h:31,
+                 from /orin_ssd/MORB_SLAM/include/MapPoint.h:22,
+                 from /orin_ssd/MORB_SLAM/include/Map.h:22,
+                 from /orin_ssd/MORB_SLAM/src/Map.cc:22:
+/usr/include/c++/9/bits/stl_deque.h:2338:5: note: candidate: ‘template<class _Tp, class _Alloc> bool std::operator!=(const std::deque<_Tp, _Alloc>&, const std::deque<_Tp, _Alloc>&)’
+ 2338 |     operator!=(const deque<_Tp, _Alloc>& __x,
+      |     ^~~~~~~~
+/usr/include/c++/9/bits/stl_deque.h:2338:5: note:   template argument deduction/substitution failed:
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<ORB_SLAM3::Map>’ is not derived from ‘const std::deque<_Tp, _Alloc>’
+  339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
+      |                                                 ^
+In file included from /usr/include/c++/9/stack:61,
+                 from /orin_ssd/MORB_SLAM/Thirdparty/g2o/g2o/types/../core/base_vertex.h:38,
+                 from /orin_ssd/MORB_SLAM/Thirdparty/g2o/g2o/types/types_seven_dof_expmap.h:34,
+                 from /orin_ssd/MORB_SLAM/include/Converter.h:27,
+                 from /orin_ssd/MORB_SLAM/include/Frame.h:32,
+                 from /orin_ssd/MORB_SLAM/include/KeyFrame.h:31,
+                 from /orin_ssd/MORB_SLAM/include/MapPoint.h:22,
+                 from /orin_ssd/MORB_SLAM/include/Map.h:22,
+                 from /orin_ssd/MORB_SLAM/src/Map.cc:22:
+/usr/include/c++/9/bits/stl_stack.h:337:5: note: candidate: ‘template<class _Tp, class _Seq> bool std::operator!=(const std::stack<_Tp, _Seq>&, const std::stack<_Tp, _Seq>&)’
+  337 |     operator!=(const stack<_Tp, _Seq>& __x, const stack<_Tp, _Seq>& __y)
+      |     ^~~~~~~~
+/usr/include/c++/9/bits/stl_stack.h:337:5: note:   template argument deduction/substitution failed:
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<ORB_SLAM3::Map>’ is not derived from ‘const std::stack<_Tp, _Seq>’
+  339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
+      |                                                 ^
+In file included from /usr/include/c++/9/queue:64,
+                 from /usr/local/include/opencv4/opencv2/stitching/detail/util_inl.hpp:46,
+                 from /usr/local/include/opencv4/opencv2/stitching/detail/util.hpp:119,
+                 from /usr/local/include/opencv4/opencv2/stitching/detail/motion_estimators.hpp:48,
+                 from /usr/local/include/opencv4/opencv2/stitching.hpp:50,
+                 from /usr/local/include/opencv4/opencv2/opencv.hpp:86,
+                 from /orin_ssd/MORB_SLAM/include/Frame.h:36,
+                 from /orin_ssd/MORB_SLAM/include/KeyFrame.h:31,
+                 from /orin_ssd/MORB_SLAM/include/MapPoint.h:22,
+                 from /orin_ssd/MORB_SLAM/include/Map.h:22,
+                 from /orin_ssd/MORB_SLAM/src/Map.cc:22:
+/usr/include/c++/9/bits/stl_queue.h:362:5: note: candidate: ‘template<class _Tp, class _Seq> bool std::operator!=(const std::queue<_Tp, _Seq>&, const std::queue<_Tp, _Seq>&)’
+  362 |     operator!=(const queue<_Tp, _Seq>& __x, const queue<_Tp, _Seq>& __y)
+      |     ^~~~~~~~
+/usr/include/c++/9/bits/stl_queue.h:362:5: note:   template argument deduction/substitution failed:
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<ORB_SLAM3::Map>’ is not derived from ‘const std::queue<_Tp, _Seq>’
+  339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
+      |                                                 ^
+In file included from /orin_ssd/MORB_SLAM/include/Camera.hpp:2,
+                 from /orin_ssd/MORB_SLAM/include/Frame.h:41,
+                 from /orin_ssd/MORB_SLAM/include/KeyFrame.h:31,
+                 from /orin_ssd/MORB_SLAM/include/MapPoint.h:22,
+                 from /orin_ssd/MORB_SLAM/include/Map.h:22,
+                 from /orin_ssd/MORB_SLAM/src/Map.cc:22:
+/usr/include/c++/9/thread:286:3: note: candidate: ‘bool std::operator!=(std::thread::id, std::thread::id)’
+  286 |   operator!=(thread::id __x, thread::id __y) noexcept
+      |   ^~~~~~~~
+/usr/include/c++/9/thread:286:25: note:   no known conversion for argument 1 from ‘std::shared_ptr<ORB_SLAM3::Map>’ to ‘std::thread::id’
+  286 |   operator!=(thread::id __x, thread::id __y) noexcept
+      |              ~~~~~~~~~~~^~~
+make[2]: *** [CMakeFiles/MORB_SLAM.dir/build.make:244: CMakeFiles/MORB_SLAM.dir/src/Map.cc.o] Error 1
+make[2]: *** Waiting for unfinished jobs....
+make[2]: *** [CMakeFiles/MORB_SLAM.dir/build.make:76: CMakeFiles/MORB_SLAM.dir/src/System.cc.o] Interrupt
+make[2]: *** [CMakeFiles/MORB_SLAM.dir/build.make:104: CMakeFiles/MORB_SLAM.dir/src/Tracking.cc.o] Interrupt
+make[2]: *** [CMakeFiles/MORB_SLAM.dir/build.make:272: CMakeFiles/MORB_SLAM.dir/src/Optimizer.cc.o] Interrupt
+make[2]: *** [CMakeFiles/MORB_SLAM.dir/build.make:118: CMakeFiles/MORB_SLAM.dir/src/LocalMapping.cc.o] Interrupt
+make[2]: *** wait: No child processes.  Stop.
+make[1]: *** [CMakeFiles/Makefile2:166: CMakeFiles/MORB_SLAM.dir/all] Error 2
+make: *** [Makefile:136: all] Interrupt
diff --git a/src/Atlas.cc b/src/Atlas.cc
index a2ac52d0..4313a60a 100644
--- a/src/Atlas.cc
+++ b/src/Atlas.cc
@@ -34,21 +34,7 @@ Atlas::Atlas(int initKFid) : mnLastInitKFidMap(initKFid) {
   CreateNewMap();
 }
 
-Atlas::~Atlas() {
-  std::cout << "deleting atlas" << std::endl;
-  for (std::set<Map*>::iterator it = mspMaps.begin(), end = mspMaps.end();
-       it != end;) {
-    Map* pMi = *it;
-
-    if (pMi) {
-      delete pMi;
-      pMi = nullptr;
-
-      it = mspMaps.erase(it);
-    } else
-      ++it;
-  }
-}
+Atlas::~Atlas() {}
 
 void Atlas::CreateNewMap() {
   unique_lock<mutex> lock(mMutexAtlas);
@@ -63,12 +49,12 @@ void Atlas::CreateNewMap() {
   }
   cout << "Creation of new map with last KF id: " << mnLastInitKFidMap << endl;
 
-  mpCurrentMap = new Map(mnLastInitKFidMap);
+  mpCurrentMap = std::make_shared<Map>(mnLastInitKFidMap);
   mpCurrentMap->SetCurrentMap();
   mspMaps.insert(mpCurrentMap);
 }
 
-void Atlas::ChangeMap(Map* pMap) {
+void Atlas::ChangeMap(std::shared_ptr<Map> pMap) {
   unique_lock<mutex> lock(mMutexAtlas);
   cout << "Change to map with id: " << pMap->GetId() << endl;
   if (mpCurrentMap) {
@@ -85,12 +71,12 @@ unsigned long int Atlas::GetLastInitKFid() {
 }
 
 void Atlas::AddKeyFrame(KeyFrame* pKF) {
-  Map* pMapKF = pKF->GetMap();
+  std::shared_ptr<Map> pMapKF = pKF->GetMap();
   pMapKF->AddKeyFrame(pKF);
 }
 
 void Atlas::AddMapPoint(MapPoint* pMP) {
-  Map* pMapMP = pMP->GetMap();
+  std::shared_ptr<Map> pMapMP = pMP->GetMap();
   pMapMP->AddMapPoint(pMP);
 }
 
@@ -167,14 +153,14 @@ std::vector<MapPoint*> Atlas::GetReferenceMapPoints() {
   return mpCurrentMap->GetReferenceMapPoints();
 }
 
-vector<Map*> Atlas::GetAllMaps() {
+vector<std::shared_ptr<Map>> Atlas::GetAllMaps() {
   unique_lock<mutex> lock(mMutexAtlas);
   struct compFunctor {
-    inline bool operator()(Map* elem1, Map* elem2) {
+    inline bool operator()(std::shared_ptr<Map> elem1, std::shared_ptr<Map> elem2) {
       return elem1->GetId() < elem2->GetId();
     }
   };
-  vector<Map*> vMaps(mspMaps.begin(), mspMaps.end());
+  vector<std::shared_ptr<Map>> vMaps(mspMaps.begin(), mspMaps.end());
   sort(vMaps.begin(), vMaps.end(), compFunctor());
   return vMaps;
 }
@@ -202,7 +188,7 @@ void Atlas::clearAtlas() {
   mnLastInitKFidMap = 0;
 }
 
-Map* Atlas::GetCurrentMap(System* sys) {
+std::shared_ptr<Map> Atlas::GetCurrentMap(System* sys) {
   unique_lock<mutex> lock(mMutexAtlas);
   if (!mpCurrentMap) CreateNewMap();
   while (mpCurrentMap->IsBad()) {
@@ -212,7 +198,7 @@ Map* Atlas::GetCurrentMap(System* sys) {
   return mpCurrentMap;
 }
 
-void Atlas::SetMapBad(Map* pMap) {
+void Atlas::SetMapBad(std::shared_ptr<Map> pMap) {
   // mspMaps.erase(pMap);
   pMap->SetBad();
 
@@ -256,7 +242,7 @@ void Atlas::PreSave() {
   }
 
   struct compFunctor {
-    inline bool operator()(Map* elem1, Map* elem2) {
+    inline bool operator()(std::shared_ptr<Map> elem1, std::shared_ptr<Map> elem2) {
       return elem1->GetId() < elem2->GetId();
     }
   };
@@ -264,7 +250,7 @@ void Atlas::PreSave() {
   sort(mvpBackupMaps.begin(), mvpBackupMaps.end(), compFunctor());
   std::set<std::shared_ptr<GeometricCamera>> spCams(mvpCameras.begin(), mvpCameras.end());
 
-  for (Map* pMi : mvpBackupMaps) {
+  for (std::shared_ptr<Map> pMi : mvpBackupMaps) {
     if (!pMi || pMi->IsBad()) continue;
 
     if (pMi->GetAllKeyFrames().size() == 0) {
@@ -272,7 +258,7 @@ void Atlas::PreSave() {
       SetMapBad(pMi);
       continue;
     }
-    pMi->PreSave(spCams);
+    pMi->PreSave(spCams, pMi);
   }
   RemoveBadMaps();
 }
@@ -285,9 +271,9 @@ void Atlas::PostLoad() {
 
   mspMaps.clear();
   unsigned long int numKF = 0, numMP = 0;
-  for (Map* pMi : mvpBackupMaps) {
+  for (std::shared_ptr<Map> pMi : mvpBackupMaps) {
     mspMaps.insert(pMi);
-    pMi->PostLoad(mpKeyFrameDB, mpORBVocabulary, mpCams);
+    pMi->PostLoad(mpKeyFrameDB, mpORBVocabulary, mpCams, pMi);
     numKF += pMi->GetAllKeyFrames().size();
     numMP += pMi->GetAllMapPoints().size();
   }
@@ -309,7 +295,7 @@ ORBVocabulary* Atlas::GetORBVocabulary() { return mpORBVocabulary; }
 long unsigned int Atlas::GetNumLivedKF() {
   unique_lock<mutex> lock(mMutexAtlas);
   long unsigned int num = 0;
-  for (Map* pMap_i : mspMaps) {
+  for (std::shared_ptr<Map> pMap_i : mspMaps) {
     num += pMap_i->GetAllKeyFrames().size();
   }
 
@@ -319,7 +305,7 @@ long unsigned int Atlas::GetNumLivedKF() {
 long unsigned int Atlas::GetNumLivedMP() {
   unique_lock<mutex> lock(mMutexAtlas);
   long unsigned int num = 0;
-  for (Map* pMap_i : mspMaps) {
+  for (std::shared_ptr<Map> pMap_i : mspMaps) {
     num += pMap_i->GetAllMapPoints().size();
   }
 
@@ -328,7 +314,7 @@ long unsigned int Atlas::GetNumLivedMP() {
 
 map<long unsigned int, KeyFrame*> Atlas::GetAtlasKeyframes() {
   map<long unsigned int, KeyFrame*> mpIdKFs;
-  for (Map* pMap_i : mvpBackupMaps) {
+  for (std::shared_ptr<Map>  pMap_i : mvpBackupMaps) {
     vector<KeyFrame*> vpKFs_Mi = pMap_i->GetAllKeyFrames();
 
     for (KeyFrame* pKF_j_Mi : vpKFs_Mi) {
diff --git a/src/KeyFrame.cc b/src/KeyFrame.cc
index c97385a9..18b42df0 100644
--- a/src/KeyFrame.cc
+++ b/src/KeyFrame.cc
@@ -91,7 +91,7 @@ KeyFrame::KeyFrame()
       NLeft(0),
       NRight(0) {}
 
-KeyFrame::KeyFrame(Frame &F, Map *pMap, KeyFrameDatabase *pKFDB)
+KeyFrame::KeyFrame(Frame &F, std::shared_ptr<Map> pMap, KeyFrameDatabase *pKFDB)
     : bImu(pMap->isImuInitialized()),
       mnFrameId(F.mnId),
       mTimeStamp(F.mTimeStamp),
@@ -846,12 +846,12 @@ IMU::Bias KeyFrame::GetImuBias() {
   return mImuBias;
 }
 
-Map *KeyFrame::GetMap() {
+std::shared_ptr<Map> KeyFrame::GetMap() {
   unique_lock<mutex> lock(mMutexMap);
   return mpMap;
 }
 
-void KeyFrame::UpdateMap(Map *pMap) {
+void KeyFrame::UpdateMap(std::shared_ptr<Map> pMap) {
   unique_lock<mutex> lock(mMutexMap);
   mpMap = pMap;
 }
diff --git a/src/KeyFrameDatabase.cc b/src/KeyFrameDatabase.cc
index 45d5a5b0..9ed18483 100644
--- a/src/KeyFrameDatabase.cc
+++ b/src/KeyFrameDatabase.cc
@@ -68,7 +68,7 @@ void KeyFrameDatabase::clear() {
   mvInvertedFile.resize(mpVoc->size());
 }
 
-void KeyFrameDatabase::clearMap(Map* pMap) {
+void KeyFrameDatabase::clearMap(std::shared_ptr<Map> pMap) {
   unique_lock<mutex> lock(mMutex);
 
   // Erase elements in the Inverse File for the entry
@@ -705,7 +705,7 @@ void KeyFrameDatabase::DetectNBestCandidates(KeyFrame* pKF,
 }
 
 vector<KeyFrame*> KeyFrameDatabase::DetectRelocalizationCandidates(Frame* F,
-                                                                   Map* pMap) {
+                                                                   std::shared_ptr<Map> pMap) {
   list<KeyFrame*> lKFsSharingWords;
 
   // Search all keyframes that share a word with current frame
diff --git a/src/LocalMapping.cc b/src/LocalMapping.cc
index 72fc2134..29bacdd6 100644
--- a/src/LocalMapping.cc
+++ b/src/LocalMapping.cc
@@ -1043,7 +1043,7 @@ void LocalMapping::RequestReset() {
   cout << "LM: Map reset, Done!!!" << endl;
 }
 
-void LocalMapping::RequestResetActiveMap(Map* pMap) {
+void LocalMapping::RequestResetActiveMap(std::shared_ptr<Map> pMap) {
   {
     unique_lock<mutex> lock(mMutexReset);
     cout << "LM: Active map reset recieved" << endl;
diff --git a/src/LoopClosing.cc b/src/LoopClosing.cc
index 85d5df93..6cebc6ee 100644
--- a/src/LoopClosing.cc
+++ b/src/LoopClosing.cc
@@ -1032,7 +1032,7 @@ void LoopClosing::CorrectLoop() {
                             mg2oLoopScw.translation() / mg2oLoopScw.scale());
   mpCurrentKF->SetPose(correctedTcw.cast<float>());
 
-  Map* pLoopMap = mpCurrentKF->GetMap();
+  std::shared_ptr<Map> pLoopMap = mpCurrentKF->GetMap();
 
 #ifdef REGISTER_TIMES
   /*KeyFrame* pKF = mpCurrentKF;
@@ -1288,8 +1288,8 @@ void LoopClosing::MergeLocal() {
   // and MPs from the current map. Later, the elements of the current map will
   // be transform to the new active map reference, in order to keep real time
   // tracking
-  Map* pCurrentMap = mpCurrentKF->GetMap();
-  Map* pMergeMap = mpMergeMatchedKF->GetMap();
+  std::shared_ptr<Map> pCurrentMap = mpCurrentKF->GetMap();
+  std::shared_ptr<Map> pMergeMap = mpMergeMatchedKF->GetMap();
 
   // std::cout << "Merge local, Active map: " << pCurrentMap->GetId() <<
   // std::endl; std::cout << "Merge local, Non-Active map: " <<
@@ -1858,8 +1858,8 @@ void LoopClosing::MergeLocal2() {
   }
   // cout << "Local Map stopped" << endl;
 
-  Map* pCurrentMap = mpCurrentKF->GetMap();
-  Map* pMergeMap = mpMergeMatchedKF->GetMap();
+  std::shared_ptr<Map> pCurrentMap = mpCurrentKF->GetMap();
+  std::shared_ptr<Map> pMergeMap = mpMergeMatchedKF->GetMap();
 
   {
     float s_on = mSold_new.scale();
@@ -2162,7 +2162,7 @@ void LoopClosing::SearchAndFuse(const KeyFrameAndPose& CorrectedPosesMap,
        mit != mend; mit++) {
     int num_replaces = 0;
     KeyFrame* pKFi = mit->first;
-    Map* pMap = pKFi->GetMap();
+    std::shared_ptr<Map> pMap = pKFi->GetMap();
 
     g2o::Sim3 g2oScw = mit->second;
     Sophus::Sim3f Scw = Converter::toSophus(g2oScw);
@@ -2200,7 +2200,7 @@ void LoopClosing::SearchAndFuse(const vector<KeyFrame*>& vConectedKFs,
        mit++) {
     int num_replaces = 0;
     KeyFrame* pKF = (*mit);
-    Map* pMap = pKF->GetMap();
+    std::shared_ptr<Map> pMap = pKF->GetMap();
     Sophus::SE3f Tcw = pKF->GetPose();
     Sophus::Sim3f Scw(Tcw.unit_quaternion(), Tcw.translation());
     Scw.setScale(1.f);
@@ -2243,7 +2243,7 @@ void LoopClosing::RequestReset() {
   }
 }
 
-void LoopClosing::RequestResetActiveMap(Map* pMap) {
+void LoopClosing::RequestResetActiveMap(std::shared_ptr<Map> pMap) {
   {
     unique_lock<mutex> lock(mMutexReset);
     mbResetActiveMapRequested = true;
@@ -2280,7 +2280,7 @@ void LoopClosing::ResetIfRequested() {
   }
 }
 
-void LoopClosing::RunGlobalBundleAdjustment(Map* pActiveMap,
+void LoopClosing::RunGlobalBundleAdjustment(std::shared_ptr<Map> pActiveMap,
                                             unsigned long nLoopKF) {
   Verbose::PrintMess("Starting Global Bundle Adjustment",
                      Verbose::VERBOSITY_NORMAL);
diff --git a/src/Map.cc b/src/Map.cc
index b6eab939..ad4627f2 100644
--- a/src/Map.cc
+++ b/src/Map.cc
@@ -319,7 +319,7 @@ void Map::SetLastMapChange(int currentChangeId) {
   mnMapChangeNotified = currentChangeId;
 }
 
-void Map::PreSave(std::set<std::shared_ptr<GeometricCamera>>& spCams) {
+void Map::PreSave(std::set<std::shared_ptr<GeometricCamera>>& spCams, std::shared_ptr<Map> sharedMap) {
   int nMPWithoutObs = 0;
 
   std::set<MapPoint*> tmp_mspMapPoints;
@@ -336,7 +336,7 @@ void Map::PreSave(std::set<std::shared_ptr<GeometricCamera>>& spCams) {
     for (map<KeyFrame*, std::tuple<int, int>>::iterator it = mpObs.begin(),
                                                         end = mpObs.end();
          it != end; ++it) {
-      if (it->first->GetMap() != this || it->first->isBad()) {
+      if ((it->first->GetMap() != sharedMap && this == sharedMap.get()) || it->first->isBad()) {
         pMPi->EraseObservation(it->first);
       }
     }
@@ -386,7 +386,7 @@ void Map::PostLoad(
     KeyFrameDatabase* pKFDB,
     ORBVocabulary*
         pORBVoc /*, map<long unsigned int, KeyFrame*>& mpKeyFrameId*/,
-    map<unsigned int, std::shared_ptr<GeometricCamera>>& mpCams) {
+    map<unsigned int, std::shared_ptr<GeometricCamera>>& mpCams, std::shared_ptr<Map> sharedMap) {
   std::copy(mvpBackupMapPoints.begin(), mvpBackupMapPoints.end(),
             std::inserter(mspMapPoints, mspMapPoints.begin()));
   std::copy(mvpBackupKeyFrames.begin(), mvpBackupKeyFrames.end(),
@@ -394,9 +394,9 @@ void Map::PostLoad(
 
   map<long unsigned int, MapPoint*> mpMapPointId;
   for (MapPoint* pMPi : mspMapPoints) {
-    if (!pMPi || pMPi->isBad()) continue;
+    if ((!pMPi || pMPi->isBad()) || this != sharedMap.get()) continue;
 
-    pMPi->UpdateMap(this);
+    pMPi->UpdateMap(sharedMap);
     mpMapPointId[pMPi->mnId] = pMPi;
   }
 
@@ -404,7 +404,7 @@ void Map::PostLoad(
   for (KeyFrame* pKFi : mspKeyFrames) {
     if (!pKFi || pKFi->isBad()) continue;
 
-    pKFi->UpdateMap(this);
+    pKFi->UpdateMap(sharedMap);
     pKFi->SetORBVocabulary(pORBVoc);
     pKFi->SetKeyFrameDatabase(pKFDB);
     mpKeyFrameId[pKFi->mnId] = pKFi;
diff --git a/src/MapDrawer.cc b/src/MapDrawer.cc
index 51a7ab19..1592bee6 100644
--- a/src/MapDrawer.cc
+++ b/src/MapDrawer.cc
@@ -120,7 +120,7 @@ bool MapDrawer::ParseViewerParamFile(cv::FileStorage &fSettings) {
 }
 
 void MapDrawer::DrawMapPoints() {
-  Map *pActiveMap = mpAtlas->GetCurrentMap();
+  std::shared_ptr<Map> pActiveMap = mpAtlas->GetCurrentMap();
   if (!pActiveMap) return;
 
   const vector<MapPoint *> &vpMPs = pActiveMap->GetAllMapPoints();
@@ -162,7 +162,7 @@ void MapDrawer::DrawKeyFrames(const bool bDrawKF, const bool bDrawGraph,
   const float h = w * 0.75;
   const float z = w * 0.6;
 
-  Map *pActiveMap = mpAtlas->GetCurrentMap();
+  std::shared_ptr<Map> pActiveMap = mpAtlas->GetCurrentMap();
   // DEBUG LBA
   std::set<long unsigned int> sOptKFs = pActiveMap->msOptKFs;
   std::set<long unsigned int> sFixedKFs = pActiveMap->msFixedKFs;
@@ -295,10 +295,10 @@ void MapDrawer::DrawKeyFrames(const bool bDrawKF, const bool bDrawGraph,
     glEnd();
   }
 
-  vector<Map *> vpMaps = mpAtlas->GetAllMaps();
+  vector<std::shared_ptr<Map>> vpMaps = mpAtlas->GetAllMaps();
 
   if (bDrawKF) {
-    for (Map *pMap : vpMaps) {
+    for (std::shared_ptr<Map> pMap : vpMaps) {
       if (pMap == pActiveMap) continue;
 
       vector<KeyFrame *> vpKFs = pMap->GetAllKeyFrames();
diff --git a/src/MapPoint.cc b/src/MapPoint.cc
index 4c23208c..720436f1 100644
--- a/src/MapPoint.cc
+++ b/src/MapPoint.cc
@@ -49,7 +49,7 @@ MapPoint::MapPoint()
   mpReplaced = nullptr;
 }
 
-MapPoint::MapPoint(const Eigen::Vector3f& Pos, KeyFrame* pRefKF, Map* pMap)
+MapPoint::MapPoint(const Eigen::Vector3f& Pos, KeyFrame* pRefKF, std::shared_ptr<Map> pMap)
     : mnFirstKFid(pRefKF->mnId),
       mnFirstFrame(pRefKF->mnFrameId),
       nObs(0),
@@ -84,7 +84,7 @@ MapPoint::MapPoint(const Eigen::Vector3f& Pos, KeyFrame* pRefKF, Map* pMap)
 }
 
 MapPoint::MapPoint(const double invDepth, cv::Point2f uv_init, KeyFrame* pRefKF,
-                   KeyFrame* pHostKF, Map* pMap)
+                   KeyFrame* pHostKF, std::shared_ptr<Map> pMap)
     : mnFirstKFid(pRefKF->mnId),
       mnFirstFrame(pRefKF->mnFrameId),
       nObs(0),
@@ -119,7 +119,7 @@ MapPoint::MapPoint(const double invDepth, cv::Point2f uv_init, KeyFrame* pRefKF,
   mnId = nNextId++;
 }
 
-MapPoint::MapPoint(const Eigen::Vector3f& Pos, Map* pMap, Frame* pFrame,
+MapPoint::MapPoint(const Eigen::Vector3f& Pos, std::shared_ptr<Map> pMap, Frame* pFrame,
                    const int& idxF)
     : mnFirstKFid(-1),
       mnFirstFrame(pFrame->mnId),
@@ -578,12 +578,12 @@ void MapPoint::PrintObservations() {
   }
 }
 
-Map* MapPoint::GetMap() {
+std::shared_ptr<Map> MapPoint::GetMap() {
   unique_lock<mutex> lock(mMutexMap);
   return mpMap;
 }
 
-void MapPoint::UpdateMap(Map* pMap) {
+void MapPoint::UpdateMap(std::shared_ptr<Map> pMap) {
   unique_lock<mutex> lock(mMutexMap);
   mpMap = pMap;
 }
diff --git a/src/Optimizer.cc b/src/Optimizer.cc
index c0a849ac..1cf32a4b 100644
--- a/src/Optimizer.cc
+++ b/src/Optimizer.cc
@@ -44,7 +44,7 @@ bool sortByVal(const pair<MapPoint*, int>& a, const pair<MapPoint*, int>& b) {
   return (a.second < b.second);
 }
 
-void Optimizer::GlobalBundleAdjustemnt(Map* pMap, int nIterations,
+void Optimizer::GlobalBundleAdjustemnt(std::shared_ptr<Map> pMap, int nIterations,
                                        bool* pbStopFlag,
                                        const unsigned long nLoopKF,
                                        const bool bRobust) {
@@ -60,7 +60,7 @@ void Optimizer::BundleAdjustment(const vector<KeyFrame*>& vpKFs,
   vector<bool> vbNotIncludedMP;
   vbNotIncludedMP.resize(vpMP.size());
 
-  Map* pMap = vpKFs[0]->GetMap();
+  std::shared_ptr<Map> pMap = vpKFs[0]->GetMap();
 
   g2o::SparseOptimizer optimizer;
   g2o::BlockSolver_6_3::LinearSolverType* linearSolver;
@@ -361,7 +361,7 @@ void Optimizer::BundleAdjustment(const vector<KeyFrame*>& vpKFs,
   }
 }
 
-void Optimizer::FullInertialBA(Map* pMap, int its, const bool bFixLocal,
+void Optimizer::FullInertialBA(std::shared_ptr<Map> pMap, int its, const bool bFixLocal,
                                const long unsigned int nLoopId,
                                bool* pbStopFlag, bool bInit, float priorG,
                                float priorA, Eigen::VectorXd* vSingVal,
@@ -1051,7 +1051,7 @@ int Optimizer::PoseOptimization(Frame* pFrame) {
 }
 
 void Optimizer::LocalBundleAdjustment(KeyFrame* pKF, bool* pbStopFlag,
-                                      Map* pMap, int& num_fixedKF,
+                                      std::shared_ptr<Map> pMap, int& num_fixedKF,
                                       int& num_OptKF, int& num_MPs,
                                       int& num_edges) {
   // Local KeyFrames: First Breath Search from Current Keyframe
@@ -1059,7 +1059,7 @@ void Optimizer::LocalBundleAdjustment(KeyFrame* pKF, bool* pbStopFlag,
 
   lLocalKeyFrames.push_back(pKF);
   pKF->mnBALocalForKF = pKF->mnId;
-  Map* pCurrentMap = pKF->GetMap();
+  std::shared_ptr<Map> pCurrentMap = pKF->GetMap();
 
   const vector<KeyFrame*> vNeighKFs = pKF->GetVectorCovisibleKeyFrames();
   for (int i = 0, iend = vNeighKFs.size(); i < iend; i++) {
@@ -1441,7 +1441,7 @@ void Optimizer::LocalBundleAdjustment(KeyFrame* pKF, bool* pbStopFlag,
 }
 
 void Optimizer::OptimizeEssentialGraph(
-    Map* pMap, KeyFrame* pLoopKF, KeyFrame* pCurKF,
+    std::shared_ptr<Map> pMap, KeyFrame* pLoopKF, KeyFrame* pCurKF,
     const LoopClosing::KeyFrameAndPose& NonCorrectedSim3,
     const LoopClosing::KeyFrameAndPose& CorrectedSim3,
     const map<KeyFrame*, set<KeyFrame*>>& LoopConnections,
@@ -1767,7 +1767,7 @@ void Optimizer::OptimizeEssentialGraph(KeyFrame* pCurKF,
   solver->setUserLambdaInit(1e-16);
   optimizer.setAlgorithm(solver);
 
-  Map* pMap = pCurKF->GetMap();
+  std::shared_ptr<Map> pMap = pCurKF->GetMap();
   const unsigned int nMaxKFid = pMap->GetMaxKFid();
 
   vector<g2o::Sim3, Eigen::aligned_allocator<g2o::Sim3>> vScw(nMaxKFid + 1);
@@ -2321,10 +2321,10 @@ int Optimizer::OptimizeSim3(KeyFrame* pKF1, KeyFrame* pKF2,
   return nIn;
 }
 
-void Optimizer::LocalInertialBA(KeyFrame* pKF, bool* pbStopFlag, Map* pMap,
+void Optimizer::LocalInertialBA(KeyFrame* pKF, bool* pbStopFlag, std::shared_ptr<Map> pMap,
                                 int& num_fixedKF, int& num_OptKF, int& num_MPs,
                                 int& num_edges, bool bLarge, bool bRecInit) {
-  Map* pCurrentMap = pKF->GetMap();
+  std::shared_ptr<Map> pCurrentMap = pKF->GetMap();
 
   int maxOpt = 10;
   int opt_it = 10;
@@ -2976,7 +2976,7 @@ Eigen::MatrixXd Optimizer::Marginalize(const Eigen::MatrixXd& H,
   return res;
 }
 
-void Optimizer::InertialOptimization(Map* pMap, Eigen::Matrix3d& Rwg,
+void Optimizer::InertialOptimization(std::shared_ptr<Map> pMap, Eigen::Matrix3d& Rwg,
                                      double& scale, Eigen::Vector3d& bg,
                                      Eigen::Vector3d& ba, bool bMono,
                                      Eigen::MatrixXd& covInertial,
@@ -3155,7 +3155,7 @@ void Optimizer::InertialOptimization(Map* pMap, Eigen::Matrix3d& Rwg,
   }
 }
 
-void Optimizer::InertialOptimization(Map* pMap, Eigen::Vector3d& bg,
+void Optimizer::InertialOptimization(std::shared_ptr<Map> pMap, Eigen::Vector3d& bg,
                                      Eigen::Vector3d& ba, float priorG,
                                      float priorA) {
   int its = 200;  // Check number of iterations
@@ -3313,7 +3313,7 @@ void Optimizer::InertialOptimization(Map* pMap, Eigen::Vector3d& bg,
   }
 }
 
-void Optimizer::InertialOptimization(Map* pMap, Eigen::Matrix3d& Rwg,
+void Optimizer::InertialOptimization(std::shared_ptr<Map> pMap, Eigen::Matrix3d& Rwg,
                                      double& scale) {
   int its = 10;
   long unsigned int maxKFid = pMap->GetMaxKFid();
@@ -3454,7 +3454,7 @@ void Optimizer::LocalBundleAdjustment(KeyFrame* pMainKF,
   long unsigned int maxKFid = 0;
   set<KeyFrame*> spKeyFrameBA;
 
-  Map* pCurrentMap = pMainKF->GetMap();
+  std::shared_ptr<Map> pCurrentMap = pMainKF->GetMap();
 
   // Set fixed KeyFrame vertices
   int numInsertedPoints = 0;
@@ -3851,7 +3851,7 @@ void Optimizer::LocalBundleAdjustment(KeyFrame* pMainKF,
 }
 
 void Optimizer::MergeInertialBA(KeyFrame* pCurrKF, KeyFrame* pMergeKF,
-                                bool* pbStopFlag, Map* pMap,
+                                bool* pbStopFlag, std::shared_ptr<Map> pMap,
                                 LoopClosing::KeyFrameAndPose& corrPoses) {
   const int Nd = 6;
 
@@ -5160,7 +5160,7 @@ int Optimizer::PoseInertialOptimizationLastFrame(Frame* pFrame, bool bRecInit) {
 }
 
 void Optimizer::OptimizeEssentialGraph4DoF(
-    Map* pMap, KeyFrame* pLoopKF, KeyFrame* pCurKF,
+    std::shared_ptr<Map> pMap, KeyFrame* pLoopKF, KeyFrame* pCurKF,
     const LoopClosing::KeyFrameAndPose& NonCorrectedSim3,
     const LoopClosing::KeyFrameAndPose& CorrectedSim3,
     const map<KeyFrame*, set<KeyFrame*>>& LoopConnections) {
diff --git a/src/System.cc b/src/System.cc
index f5a383ae..ba2f7b67 100644
--- a/src/System.cc
+++ b/src/System.cc
@@ -630,12 +630,12 @@ void System::SaveTrajectoryEuRoC(const string& filename) {
   endl; return;
   }*/
 
-  vector<Map*> vpMaps = mpAtlas->GetAllMaps();
+  vector<std::shared_ptr<Map>> vpMaps = mpAtlas->GetAllMaps();
   size_t numMaxKFs = 0;
-  Map* pBiggerMap = nullptr;
+  std::shared_ptr<Map> pBiggerMap = nullptr;
   std::cout << "There are " << std::to_string(vpMaps.size())
             << " maps in the atlas" << std::endl;
-  for (Map* pMap : vpMaps) {
+  for (std::shared_ptr<Map> pMap : vpMaps) {
     std::cout << "  Map " << std::to_string(pMap->GetId()) << " has "
               << std::to_string(pMap->GetAllKeyFrames().size()) << " KFs"
               << std::endl;
@@ -744,7 +744,7 @@ void System::SaveTrajectoryEuRoC(const string& filename) {
   cout << endl << "End of saving trajectory to " << filename << " ..." << endl;
 }
 
-void System::SaveTrajectoryEuRoC(const string& filename, Map* pMap) {
+void System::SaveTrajectoryEuRoC(const string& filename, std::shared_ptr<Map> pMap) {
   cout << endl
        << "Saving trajectory of map " << pMap->GetId() << " to " << filename
        << " ..." << endl;
@@ -1046,10 +1046,10 @@ void System::SaveKeyFrameTrajectoryEuRoC(const string& filename) {
   cout << endl
        << "Saving keyframe trajectory to " << filename << " ... Euroc" << endl;
 
-  vector<Map*> vpMaps = mpAtlas->GetAllMaps();
-  Map* pBiggerMap = nullptr;
+  vector<std::shared_ptr<Map>> vpMaps = mpAtlas->GetAllMaps();
+  std::shared_ptr<Map> pBiggerMap = nullptr;
   size_t numMaxKFs = 0;
-  for (Map* pMap : vpMaps) {
+  for (std::shared_ptr<Map> pMap : vpMaps) {
     if (pMap && pMap->GetAllKeyFrames().size() > numMaxKFs) {
       numMaxKFs = pMap->GetAllKeyFrames().size();
       pBiggerMap = pMap;
@@ -1098,7 +1098,7 @@ void System::SaveKeyFrameTrajectoryEuRoC(const string& filename) {
   f.close();
 }
 
-void System::SaveKeyFrameTrajectoryEuRoC(const string& filename, Map* pMap) {
+void System::SaveKeyFrameTrajectoryEuRoC(const string& filename, std::shared_ptr<Map> pMap) {
   cout << endl
        << "Saving keyframe trajectory of map " << pMap->GetId() << " to "
        << filename << " ..." << endl;
diff --git a/src/Tracking.cc b/src/Tracking.cc
index 21bcaf10..51e455b0 100644
--- a/src/Tracking.cc
+++ b/src/Tracking.cc
@@ -1684,7 +1684,7 @@ void Tracking::Track() {
     return;
   }
 
-  Map* pCurrentMap = mpAtlas->GetCurrentMap(mpSystem);
+  std::shared_ptr<Map> pCurrentMap = mpAtlas->GetCurrentMap(mpSystem);
   if (!pCurrentMap) {
     // if (!mpSystem->isShutDown()){
     //   cout << "ERROR: There is not an active map in the atlas" << endl;
@@ -3519,7 +3519,7 @@ void Tracking::Reset(bool bLocMap) {
 void Tracking::ResetActiveMap(bool bLocMap) {
   Verbose::PrintMess("Active map Reseting", Verbose::VERBOSITY_NORMAL);
 
-  Map* pMap = mpAtlas->GetCurrentMap();
+  std::shared_ptr<Map> pMap = mpAtlas->GetCurrentMap();
 
   if (!bLocMap) {
     Verbose::PrintMess("Reseting Local Mapper...",
@@ -3553,7 +3553,7 @@ void Tracking::ResetActiveMap(bool bLocMap) {
   // lbLost.reserve(mlbLost.size());
   unsigned int index = mnFirstFrameId;
   cout << "mnFirstFrameId = " << mnFirstFrameId << endl;
-  for (Map* pMap : mpAtlas->GetAllMaps()) {
+  for (std::shared_ptr<Map> pMap : mpAtlas->GetAllMaps()) {
     if (pMap->GetAllKeyFrames().size() > 0) {
       if (index > pMap->GetLowerKFID()) index = pMap->GetLowerKFID();
     }
@@ -3635,7 +3635,7 @@ void Tracking::InformOnlyTracking(const bool& flag) { mbOnlyTracking = flag; }
 
 void Tracking::UpdateFrameIMU(const float s, const IMU::Bias& b,
                               KeyFrame* pCurrentKeyFrame) {
-  Map* pMap = pCurrentKeyFrame->GetMap();
+  std::shared_ptr<Map> pMap = pCurrentKeyFrame->GetMap();
   // unsigned int index = mnFirstFrameId; // UNUSED
   list<ORB_SLAM3::KeyFrame*>::iterator lRit = mlpReferences.begin();
   list<bool>::iterator lbL = mlbLost.begin();
@@ -3719,7 +3719,7 @@ void Tracking::SaveSubTrajectory(string strNameFile_frames,
 }
 
 void Tracking::SaveSubTrajectory(string strNameFile_frames,
-                                 string strNameFile_kf, Map* pMap) {
+                                 string strNameFile_kf, std::shared_ptr<Map> pMap) {
   mpSystem->SaveTrajectoryEuRoC(strNameFile_frames, pMap);
   if (!strNameFile_kf.empty())
     mpSystem->SaveKeyFrameTrajectoryEuRoC(strNameFile_kf, pMap);

From 75eb60a7090cd6aeb0c0f925510b5b5cd07b1d52 Mon Sep 17 00:00:00 2001
From: DavidPetkovsek <david.petkovsek@martinrea.com>
Date: Wed, 3 Aug 2022 16:19:28 -0400
Subject: [PATCH 21/26] Fix immediate sophus error Co-authored: ethanseq
 <sequeira.ethan@gmail.com

---
 include/Map.h |  1 +
 src/Map.cc    | 14 ++++++++++++--
 2 files changed, 13 insertions(+), 2 deletions(-)

diff --git a/include/Map.h b/include/Map.h
index a8a27b98..fb40964d 100644
--- a/include/Map.h
+++ b/include/Map.h
@@ -27,6 +27,7 @@
 #include <mutex>
 #include <memory>
 #include <boost/serialization/base_object.hpp>
+#include <stdexcept>
 
 
 namespace ORB_SLAM3
diff --git a/src/Map.cc b/src/Map.cc
index ad4627f2..06cfb31e 100644
--- a/src/Map.cc
+++ b/src/Map.cc
@@ -320,6 +320,11 @@ void Map::SetLastMapChange(int currentChangeId) {
 }
 
 void Map::PreSave(std::set<std::shared_ptr<GeometricCamera>>& spCams, std::shared_ptr<Map> sharedMap) {
+
+  if(this != sharedMap.get()){
+    throw std::runtime_error("The shared map is not equivalent to this");
+  }
+
   int nMPWithoutObs = 0;
 
   std::set<MapPoint*> tmp_mspMapPoints;
@@ -336,7 +341,7 @@ void Map::PreSave(std::set<std::shared_ptr<GeometricCamera>>& spCams, std::share
     for (map<KeyFrame*, std::tuple<int, int>>::iterator it = mpObs.begin(),
                                                         end = mpObs.end();
          it != end; ++it) {
-      if ((it->first->GetMap() != sharedMap && this == sharedMap.get()) || it->first->isBad()) {
+      if ((it->first->GetMap() != sharedMap) || it->first->isBad()) {
         pMPi->EraseObservation(it->first);
       }
     }
@@ -387,6 +392,11 @@ void Map::PostLoad(
     ORBVocabulary*
         pORBVoc /*, map<long unsigned int, KeyFrame*>& mpKeyFrameId*/,
     map<unsigned int, std::shared_ptr<GeometricCamera>>& mpCams, std::shared_ptr<Map> sharedMap) {
+
+  if(this != sharedMap.get()){
+  throw std::runtime_error("The shared map is not equivalent to this");
+  }
+
   std::copy(mvpBackupMapPoints.begin(), mvpBackupMapPoints.end(),
             std::inserter(mspMapPoints, mspMapPoints.begin()));
   std::copy(mvpBackupKeyFrames.begin(), mvpBackupKeyFrames.end(),
@@ -394,7 +404,7 @@ void Map::PostLoad(
 
   map<long unsigned int, MapPoint*> mpMapPointId;
   for (MapPoint* pMPi : mspMapPoints) {
-    if ((!pMPi || pMPi->isBad()) || this != sharedMap.get()) continue;
+    if (!pMPi || pMPi->isBad()) continue;
 
     pMPi->UpdateMap(sharedMap);
     mpMapPointId[pMPi->mnId] = pMPi;

From fca375388ae988f9478e0c2205d06b2762c80a12 Mon Sep 17 00:00:00 2001
From: DavidPetkovsek <david.petkovsek@martinrea.com>
Date: Fri, 5 Aug 2022 14:19:35 -0400
Subject: [PATCH 22/26] No more sophus error Co-authored: ethanseq
 <sequeira.ethan@gmail.com

---
 Examples/Stereo-Inertial/RealSense_D435i.yaml |  8 ++--
 .../stereo_inertial_realsense_D435i.cc        |  3 ++
 build.sh                                      |  6 +--
 src/LocalMapping.cc                           | 26 +++++++++++-
 src/Map.cc                                    |  2 +
 src/Tracking.cc                               | 40 ++++++++++---------
 6 files changed, 58 insertions(+), 27 deletions(-)
 mode change 100644 => 100755 Examples/Stereo-Inertial/stereo_inertial_realsense_D435i.cc

diff --git a/Examples/Stereo-Inertial/RealSense_D435i.yaml b/Examples/Stereo-Inertial/RealSense_D435i.yaml
index ff368a04..e57e3aff 100755
--- a/Examples/Stereo-Inertial/RealSense_D435i.yaml
+++ b/Examples/Stereo-Inertial/RealSense_D435i.yaml
@@ -42,10 +42,10 @@ IMU.T_b_c1: !!opencv-matrix
 IMU.InsertKFsWhenLost: 0
 
 # IMU noise (Use those from VINS-mono)
-IMU.NoiseGyro: 1e-3 # 2.44e-4 #1e-3 # rad/s^0.5
-IMU.NoiseAcc: 1e-2 # 1.47e-3 #1e-2 # m/s^1.5
-IMU.GyroWalk: 1e-6 # rad/s^1.5
-IMU.AccWalk: 1e-4 # m/s^2.5
+IMU.NoiseGyro:  0.008449 # 1e-3 # rad/s^0.5 # 1e-3 2.44e-4
+IMU.NoiseAcc: 0.01066 # 1e-2 # m/s^1.5 # 1e-2 1.47e-3
+IMU.GyroWalk: 0.000754 # rad/s^1.5  1e-6 
+IMU.AccWalk: 0.00930 # m/s^2.5  1e-4 
 IMU.Frequency: 200.0
 
 #--------------------------------------------------------------------------------------------
diff --git a/Examples/Stereo-Inertial/stereo_inertial_realsense_D435i.cc b/Examples/Stereo-Inertial/stereo_inertial_realsense_D435i.cc
old mode 100644
new mode 100755
index f7ab875c..a5925dfa
--- a/Examples/Stereo-Inertial/stereo_inertial_realsense_D435i.cc
+++ b/Examples/Stereo-Inertial/stereo_inertial_realsense_D435i.cc
@@ -416,6 +416,9 @@ int main(int argc, char **argv) {
 #endif
         // Stereo images are already rectified.
         auto pos = SLAM->TrackStereo(im, imRight, timestamp, vImuMeas);
+        std::cout << "XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX: " << pos.translation()[0] << std::endl;
+        std::cout << "YYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYY: " << pos.translation()[1] << std::endl;
+        std::cout << "ZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZ: " << pos.translation()[2] << std::endl;
 #ifdef REGISTER_TIMES
         std::chrono::steady_clock::time_point t_End_Track = std::chrono::steady_clock::now();
         t_track = t_resize + std::chrono::duration_cast<std::chrono::duration<double,std::milli> >(t_End_Track - t_Start_Track).count();
diff --git a/build.sh b/build.sh
index f5c2b3f7..911991c3 100755
--- a/build.sh
+++ b/build.sh
@@ -10,7 +10,7 @@ echo "Configuring and building Thirdparty/DBoW2 ..."
 cd Thirdparty/DBoW2
 mkdir build 2> /dev/null
 cd build
-cmake .. -DCMAKE_BUILD_TYPE=Release
+cmake .. -DCMAKE_BUILD_TYPE=RelWithDebInfo
 make ${jobs}
 
 cd ../../g2o
@@ -19,7 +19,7 @@ echo "Configuring and building Thirdparty/g2o ..."
 
 mkdir build 2> /dev/null
 cd build
-cmake .. -DCMAKE_BUILD_TYPE=Release
+cmake .. -DCMAKE_BUILD_TYPE=RelWithDebInfo
 make ${jobs}
 
 cd ../../Sophus
@@ -28,7 +28,7 @@ echo "Configuring and building Thirdparty/Sophus ..."
 
 mkdir build 2> /dev/null
 cd build
-cmake .. -DCMAKE_BUILD_TYPE=Release
+cmake .. -DCMAKE_BUILD_TYPE=RelWithDebInfo
 make ${jobs}
 
 cd ../../../
diff --git a/src/LocalMapping.cc b/src/LocalMapping.cc
index 29bacdd6..29b20fd4 100644
--- a/src/LocalMapping.cc
+++ b/src/LocalMapping.cc
@@ -30,6 +30,7 @@
 #include "ORBmatcher.h"
 #include "Optimizer.h"
 
+#include <math.h> 
 namespace ORB_SLAM3 {
 
 LocalMapping::LocalMapping(System* pSys, const Atlas_ptr &pAtlas, const float bMonocular,
@@ -1167,13 +1168,21 @@ void LocalMapping::InitializeIMU(float priorG, float priorA, bool bFIBA) {
     Eigen::Matrix3f Rwg;
     Eigen::Vector3f dirG;
     dirG.setZero();
+    // std::cout << "Keyframes---------------------------------------------: " << N << std::endl;
     for (vector<KeyFrame*>::iterator itKF = vpKF.begin(); itKF != vpKF.end();
          itKF++) {
-      if (!(*itKF)->mpImuPreintegrated) continue;
-      if (!(*itKF)->mPrevKF) continue;
+
+      // std::cout << "Hello" << std::endl;
+      if (!(*itKF)->mpImuPreintegrated) continue; // || isnan((*itKF)->mpImuPreintegrated->GetUpdatedDeltaVelocity().sum())
+      if (!(*itKF)->mPrevKF) continue; //  || isnan((*itKF)->mPrevKF->GetImuRotation().sum()
+
+      // std::cout << "initDirG------------------------------------------------------: " << dirG << std::endl;
+      // std::cout << "getImuRot------------------------------------------------------: " << (*itKF)->mPrevKF->GetImuRotation() << std::endl;
+      // std::cout << "getUpdDeltaV------------------------------------------------------: " <<(*itKF)->mpImuPreintegrated->GetUpdatedDeltaVelocity() << std::endl;
 
       dirG -= (*itKF)->mPrevKF->GetImuRotation() *
               (*itKF)->mpImuPreintegrated->GetUpdatedDeltaVelocity();
+      // std::cout << "dirGLoop------------------------------------------------------: " << dirG << std::endl;
       Eigen::Vector3f _vel =
           ((*itKF)->GetImuPosition() - (*itKF)->mPrevKF->GetImuPosition()) /
           (*itKF)->mpImuPreintegrated->dT;
@@ -1181,16 +1190,29 @@ void LocalMapping::InitializeIMU(float priorG, float priorA, bool bFIBA) {
       (*itKF)->mPrevKF->SetVelocity(_vel);
     }
 
+    // if(dirG.sum() == 0){ mRwg << 0.99600422382354736,0.059227496385574341,0.066840663552284241,
+    //   0.059227496385574341,0.12209725379943848,-0.99074935913085938,
+    //   -0.066840663552284241,0.99074935913085938,0.11810147762298584;
+    // } else{
+    std::cout << "dirGBeforeNorm------------------------------------------------------: " << dirG << std::endl;
     dirG = dirG / dirG.norm();
+    std::cout << "dirGAfterNorm------------------------------------------------------: " << dirG << std::endl;
     Eigen::Vector3f gI(0.0f, 0.0f, -1.0f);
     Eigen::Vector3f v = gI.cross(dirG);
     const float nv = v.norm();
+    
     const float cosg = gI.dot(dirG);
     const float ang = acos(cosg);
+
+    std::cout << "v------------------------------------------------------: " << v << std::endl;
+    std::cout << "ang------------------------------------------------------: " << ang << std::endl;
+    std::cout << "nv------------------------------------------------------: " << nv << std::endl;
+
     Eigen::Vector3f vzg = v * ang / nv;
     Rwg = Sophus::SO3f::exp(vzg).matrix();
     mRwg = Rwg.cast<double>();
     mTinit = mpCurrentKeyFrame->mTimeStamp - mFirstTs;
+//}
   } else {
     mRwg = Eigen::Matrix3d::Identity();
     mbg = mpCurrentKeyFrame->GetGyroBias().cast<double>();
diff --git a/src/Map.cc b/src/Map.cc
index 06cfb31e..10f32927 100644
--- a/src/Map.cc
+++ b/src/Map.cc
@@ -153,11 +153,13 @@ int Map::GetLastBigChangeIdx() {
 
 vector<KeyFrame*> Map::GetAllKeyFrames() {
   unique_lock<mutex> lock(mMutexMap);
+  // std::cout << "Size of mspKeyFrames: " << mspKeyFrames.size() << std::endl;
   return vector<KeyFrame*>(mspKeyFrames.begin(), mspKeyFrames.end());
 }
 
 vector<MapPoint*> Map::GetAllMapPoints() {
   unique_lock<mutex> lock(mMutexMap);
+  // std::cout << "Size of mspMapPoints: " << mspMapPoints.size() << std::endl;
   return vector<MapPoint*>(mspMapPoints.begin(), mspMapPoints.end());
 }
 
diff --git a/src/Tracking.cc b/src/Tracking.cc
index 51e455b0..c1d62a98 100644
--- a/src/Tracking.cc
+++ b/src/Tracking.cc
@@ -1527,35 +1527,39 @@ void Tracking::PreintegrateIMU() {
     return;
   }
 
-  while (true) {
+ while (true) {
     unique_lock<mutex> lock(mMutexImuQueue);
-    if(mlQueueImuData.empty()) break;
-
-    IMU::Point* m = &mlQueueImuData.front();
-    if (m->t < mCurrentFrame.mpPrevFrame->mTimeStamp - mImuPer) {
-      // m is old and therefore invalid
-      mlQueueImuData.pop_front();
-    } else if (m->t < mCurrentFrame.mTimeStamp - mImuPer) {
-      // m is valid
-      mvImuFromLastFrame.push_back(*m);
-      mlQueueImuData.pop_front();
-    } else {
-      // m is newer than the current frame
-      mvImuFromLastFrame.push_back(*m);
-      break;
+    bool bSleep = false;
+    {
+      if (!mlQueueImuData.empty()) {
+        IMU::Point* m = &mlQueueImuData.front();
+        cout.precision(17);
+        if (m->t < mCurrentFrame.mpPrevFrame->mTimeStamp - mImuPer) {
+          mlQueueImuData.pop_front();
+        } else if (m->t < mCurrentFrame.mTimeStamp - mImuPer) {
+          mvImuFromLastFrame.push_back(*m);
+          mlQueueImuData.pop_front();
+        } else {
+          mvImuFromLastFrame.push_back(*m);
+          break;
+        }
+      } else {
+        break;
+        bSleep = true;
+      }
     }
+    if (bSleep) usleep(500);
   }
 
-  if (mvImuFromLastFrame.empty()) {
+  const int n = mvImuFromLastFrame.size() - 1;
+  if (n == 0) {
     cout << "Empty IMU measurements vector!!!\n";
     return;
   }
 
-  const int n = mvImuFromLastFrame.size();
   IMU::Preintegrated* pImuPreintegratedFromLastFrame =
       new IMU::Preintegrated(mLastFrame.mImuBias, mCurrentFrame.mImuCalib);
 
-
   for (int i = 0; i < n; i++) {
     float tstep;
     Eigen::Vector3f acc, angVel;

From bb3e5414a321f8c186df7172fe1fbc9ad695f9d4 Mon Sep 17 00:00:00 2001
From: Soldann <guotl321@gmail.com>
Date: Mon, 8 Aug 2022 05:23:41 -1000
Subject: [PATCH 23/26] Changed namespace from ORB_SLAM3 to MORB_SLAM and got
 correct pose values Co-authored: ethanseq <sequeira.ethan@gmail.com>

---
 .gitignore                                    |  16 +-
 .../Monocular-Inertial/mono_inertial_euroc.cc |   8 +-
 .../mono_inertial_realsense_D435i.cc          |   8 +-
 .../mono_inertial_realsense_t265.cc           |   8 +-
 .../mono_inertial_tum_vi.cc                   |   8 +-
 Examples/Monocular/mono_euroc.cc              |   4 +-
 Examples/Monocular/mono_kitti.cc              |   6 +-
 Examples/Monocular/mono_realsense_D435i.cc    |   4 +-
 Examples/Monocular/mono_realsense_t265.cc     |   4 +-
 Examples/Monocular/mono_tum.cc                |   4 +-
 Examples/Monocular/mono_tum_vi.cc             |   4 +-
 .../rgbd_inertial_realsense_D435i.cc          |   8 +-
 Examples/RGB-D/rgbd_realsense_D435i.cc        |   4 +-
 Examples/RGB-D/rgbd_tum.cc                    |   4 +-
 Examples/Stereo-Inertial/RealSense_D435i.yaml |   2 +-
 .../Stereo-Inertial/stereo_inertial_euroc.cc  |   8 +-
 .../stereo_inertial_realsense_D435i.cc        |  12 +-
 .../stereo_inertial_realsense_t265.cc         |   8 +-
 .../Stereo-Inertial/stereo_inertial_tum_vi.cc |  10 +-
 Examples/Stereo/stereo_euroc.cc               |   6 +-
 Examples/Stereo/stereo_kitti.cc               |   4 +-
 Examples/Stereo/stereo_realsense_D435i.cc     |   4 +-
 Examples/Stereo/stereo_realsense_t265.cc      |   6 +-
 Examples/Stereo/stereo_tum_vi.cc              |   4 +-
 README.md                                     |  38 ++--
 build.sh                                      |   2 +-
 build_ros.sh                                  |   2 +-
 clean.sh                                      |  14 +-
 include/Atlas.h                               |   4 +-
 include/Camera.hpp                            |   4 +-
 include/CameraModels/GeometricCamera.h        |   4 +-
 include/CameraModels/KannalaBrandt8.h         |   4 +-
 include/CameraModels/Pinhole.h                |   4 +-
 include/Converter.h                           |   4 +-
 include/Frame.h                               |   2 +-
 include/FrameDrawer.h                         |   4 +-
 include/G2oTypes.h                            |   2 +-
 include/GeometricTools.h                      |   4 +-
 include/ImprovedTypes.hpp                     |   2 +-
 include/ImuTypes.h                            |   4 +-
 include/KeyFrame.h                            |   4 +-
 include/KeyFrameDatabase.h                    |   4 +-
 include/LocalMapping.h                        |   4 +-
 include/LoopClosing.h                         |   2 +-
 include/MLPnPsolver.h                         |   4 +-
 include/Map.h                                 |   4 +-
 include/MapDrawer.h                           |   2 +-
 include/MapPoint.h                            |   2 +-
 include/ORBVocabulary.h                       |   4 +-
 include/ORBextractor.h                        |   4 +-
 include/ORBmatcher.h                          |   4 +-
 include/OptimizableTypes.h                    |  14 +-
 include/Optimizer.h                           |   4 +-
 include/SerializationUtils.h                  |   4 +-
 include/Settings.h                            |   4 +-
 include/Sim3Solver.h                          |   4 +-
 include/System.h                              |   2 +-
 include/Tracking.h                            |   4 +-
 include/TwoViewReconstruction.h               |   2 +-
 include/Viewer.h                              |   4 +-
 output.txt                                    | 170 +++++++++---------
 src/Atlas.cc                                  |   4 +-
 src/Camera.cpp                                |   4 +-
 src/CameraModels/KannalaBrandt8.cpp           |   6 +-
 src/CameraModels/Pinhole.cpp                  |   6 +-
 src/Converter.cc                              |   4 +-
 src/Frame.cc                                  |   5 +-
 src/FrameDrawer.cc                            |   4 +-
 src/G2oTypes.cc                               |   4 +-
 src/GeometricTools.cc                         |   4 +-
 src/ImuTypes.cc                               |   4 +-
 src/KeyFrame.cc                               |   4 +-
 src/KeyFrameDatabase.cc                       |   4 +-
 src/LocalMapping.cc                           |   4 +-
 src/LoopClosing.cc                            |   4 +-
 src/MLPnPsolver.cpp                           |   2 +-
 src/Map.cc                                    |   4 +-
 src/MapDrawer.cc                              |   4 +-
 src/MapPoint.cc                               |   4 +-
 src/ORBextractor.cc                           |   4 +-
 src/ORBmatcher.cc                             |   4 +-
 src/OptimizableTypes.cpp                      |   4 +-
 src/Optimizer.cc                              |  84 ++++-----
 src/Settings.cc                               |  44 ++---
 src/Sim3Solver.cc                             |   4 +-
 src/System.cc                                 |  24 +--
 src/Tracking.cc                               |   6 +-
 src/TwoViewReconstruction.cc                  |   4 +-
 src/Viewer.cc                                 |   4 +-
 89 files changed, 379 insertions(+), 380 deletions(-)

diff --git a/.gitignore b/.gitignore
index e1edf598..12736869 100644
--- a/.gitignore
+++ b/.gitignore
@@ -45,20 +45,20 @@ Examples/Stereo-Inertial/stereo_inertial_realsense_t265
 Examples/Stereo-Inertial/stereo_inertial_realsense_D435i
 
 
-Examples/ROS/ORB_SLAM3/Mono
-Examples/ROS/ORB_SLAM3/Mono_Inertial
-Examples/ROS/ORB_SLAM3/MonoAR
-Examples/ROS/ORB_SLAM3/RGBD
-Examples/ROS/ORB_SLAM3/Stereo
-Examples/ROS/ORB_SLAM3/Stereo_Inertial
+Examples/ROS/MORB_SLAM/Mono
+Examples/ROS/MORB_SLAM/Mono_Inertial
+Examples/ROS/MORB_SLAM/MonoAR
+Examples/ROS/MORB_SLAM/RGBD
+Examples/ROS/MORB_SLAM/Stereo
+Examples/ROS/MORB_SLAM/Stereo_Inertial
 
 Examples/Calibration/recorder_realsense_D435i
 
 Examples/Tests/viewer_dataset
 Examples/Tests/sophus_test
 
-Examples/ROS/ORB_SLAM3/CMakeLists.txt.user
-Examples/ROS/ORB_SLAM3/build/
+Examples/ROS/MORB_SLAM/CMakeLists.txt.user
+Examples/ROS/MORB_SLAM/build/
 
 KeyFrameTrajectory.txt
 CameraTrajectory.txt
diff --git a/Examples/Monocular-Inertial/mono_inertial_euroc.cc b/Examples/Monocular-Inertial/mono_inertial_euroc.cc
index 8c02bb52..873f4be5 100644
--- a/Examples/Monocular-Inertial/mono_inertial_euroc.cc
+++ b/Examples/Monocular-Inertial/mono_inertial_euroc.cc
@@ -118,8 +118,8 @@ int main(int argc, char *argv[])
     cout.precision(17);
 
     // Create SLAM system. It initializes all system threads and gets ready to process frames.
-    ORB_SLAM3::System_ptr SLAM = std::make_shared<ORB_SLAM3::System>(argv[1],argv[2],ORB_SLAM3::CameraType::IMU_MONOCULAR);
-    ORB_SLAM3::Viewer viewer(SLAM, argv[2]);
+    MORB_SLAM::System_ptr SLAM = std::make_shared<MORB_SLAM::System>(argv[1],argv[2],MORB_SLAM::CameraType::IMU_MONOCULAR);
+    MORB_SLAM::Viewer viewer(SLAM, argv[2]);
     float imageScale = SLAM->GetImageScale();
 
 #ifdef REGISTER_TIMES
@@ -132,7 +132,7 @@ int main(int argc, char *argv[])
 
         // Main loop
         cv::Mat im;
-        vector<ORB_SLAM3::IMU::Point> vImuMeas;
+        vector<MORB_SLAM::IMU::Point> vImuMeas;
         proccIm = 0;
         for(int ni=0; ni<nImages[seq]; ni++, proccIm++)
         {
@@ -172,7 +172,7 @@ int main(int argc, char *argv[])
 
                 while(vTimestampsImu[seq][first_imu[seq]]<=vTimestampsCam[seq][ni])
                 {
-                    vImuMeas.push_back(ORB_SLAM3::IMU::Point(vAcc[seq][first_imu[seq]].x,vAcc[seq][first_imu[seq]].y,vAcc[seq][first_imu[seq]].z,
+                    vImuMeas.push_back(MORB_SLAM::IMU::Point(vAcc[seq][first_imu[seq]].x,vAcc[seq][first_imu[seq]].y,vAcc[seq][first_imu[seq]].z,
                                                              vGyro[seq][first_imu[seq]].x,vGyro[seq][first_imu[seq]].y,vGyro[seq][first_imu[seq]].z,
                                                              vTimestampsImu[seq][first_imu[seq]]));
                     first_imu[seq]++;
diff --git a/Examples/Monocular-Inertial/mono_inertial_realsense_D435i.cc b/Examples/Monocular-Inertial/mono_inertial_realsense_D435i.cc
index 63d13e2f..c8f057e0 100644
--- a/Examples/Monocular-Inertial/mono_inertial_realsense_D435i.cc
+++ b/Examples/Monocular-Inertial/mono_inertial_realsense_D435i.cc
@@ -258,7 +258,7 @@ int main(int argc, char **argv) {
 
     rs2::pipeline_profile pipe_profile = pipe.start(cfg, imu_callback);
 
-    vector<ORB_SLAM3::IMU::Point> vImuMeas;
+    vector<MORB_SLAM::IMU::Point> vImuMeas;
     rs2::stream_profile cam_stream = pipe_profile.get_stream(RS2_STREAM_INFRARED, 1);
 
 
@@ -288,8 +288,8 @@ int main(int argc, char **argv) {
 
 
     // Create SLAM system. It initializes all system threads and gets ready to process frames.
-    ORB_SLAM3::System_ptr SLAM = std::make_shared<ORB_SLAM3::System>(argv[1],argv[2],ORB_SLAM3::CameraType::IMU_MONOCULAR, file_name);
-    ORB_SLAM3::Viewer viewer(SLAM, argv[2]);
+    MORB_SLAM::System_ptr SLAM = std::make_shared<MORB_SLAM::System>(argv[1],argv[2],MORB_SLAM::CameraType::IMU_MONOCULAR, file_name);
+    MORB_SLAM::Viewer viewer(SLAM, argv[2]);
     float imageScale = SLAM->GetImageScale();
 
     double timestamp;
@@ -356,7 +356,7 @@ int main(int argc, char **argv) {
 
         for(size_t i=0; i<vGyro.size(); ++i)
         {
-            ORB_SLAM3::IMU::Point lastPoint(vAccel[i].x, vAccel[i].y, vAccel[i].z,
+            MORB_SLAM::IMU::Point lastPoint(vAccel[i].x, vAccel[i].y, vAccel[i].z,
                                   vGyro[i].x, vGyro[i].y, vGyro[i].z,
                                   vGyro_times[i]);
             vImuMeas.push_back(lastPoint);
diff --git a/Examples/Monocular-Inertial/mono_inertial_realsense_t265.cc b/Examples/Monocular-Inertial/mono_inertial_realsense_t265.cc
index 71addae1..08c7ee16 100644
--- a/Examples/Monocular-Inertial/mono_inertial_realsense_t265.cc
+++ b/Examples/Monocular-Inertial/mono_inertial_realsense_t265.cc
@@ -65,8 +65,8 @@ int main(int argc, char **argv)
         // bFileName = true; // UNUSED
     }
 
-    ORB_SLAM3::System_ptr SLAM = std::make_shared<ORB_SLAM3::System>(argv[1],argv[2],ORB_SLAM3::CameraType::IMU_MONOCULAR, file_name);
-    ORB_SLAM3::Viewer viewer(SLAM, argv[2]);
+    MORB_SLAM::System_ptr SLAM = std::make_shared<MORB_SLAM::System>(argv[1],argv[2],MORB_SLAM::CameraType::IMU_MONOCULAR, file_name);
+    MORB_SLAM::Viewer viewer(SLAM, argv[2]);
     float imageScale = SLAM->GetImageScale();
 
     struct sigaction sigIntHandler;
@@ -206,7 +206,7 @@ int main(int argc, char **argv)
 
 
     // Create SLAM system. It initializes all system threads and gets ready to process frames.
-    vector<ORB_SLAM3::IMU::Point> vImuMeas;
+    vector<MORB_SLAM::IMU::Point> vImuMeas;
 
     double timestamp;
     cv::Mat im;
@@ -286,7 +286,7 @@ int main(int argc, char **argv)
 
 
         for(size_t i=0; i<vGyro.size(); ++i){
-            ORB_SLAM3::IMU::Point lastPoint(vAccel[i].x, vAccel[i].y, vAccel[i].z,
+            MORB_SLAM::IMU::Point lastPoint(vAccel[i].x, vAccel[i].y, vAccel[i].z,
                                             vGyro[i].x, vGyro[i].y, vGyro[i].z,
                                             vGyro_times[i]);
             vImuMeas.push_back(lastPoint);
diff --git a/Examples/Monocular-Inertial/mono_inertial_tum_vi.cc b/Examples/Monocular-Inertial/mono_inertial_tum_vi.cc
index bb6dc172..d7500d14 100644
--- a/Examples/Monocular-Inertial/mono_inertial_tum_vi.cc
+++ b/Examples/Monocular-Inertial/mono_inertial_tum_vi.cc
@@ -117,8 +117,8 @@ int main(int argc, char **argv)
     cout << "IMU data in the sequence: " << nImu << endl << endl;*/
 
     // Create SLAM system. It initializes all system threads and gets ready to process frames.
-    ORB_SLAM3::System_ptr SLAM = std::make_shared<ORB_SLAM3::System>(argv[1],argv[2],ORB_SLAM3::CameraType::IMU_MONOCULAR, file_name);
-    ORB_SLAM3::Viewer viewer(SLAM, argv[2]);
+    MORB_SLAM::System_ptr SLAM = std::make_shared<MORB_SLAM::System>(argv[1],argv[2],MORB_SLAM::CameraType::IMU_MONOCULAR, file_name);
+    MORB_SLAM::Viewer viewer(SLAM, argv[2]);
     float imageScale = SLAM->GetImageScale();
 
 #ifdef REGISTER_TIMES
@@ -131,7 +131,7 @@ int main(int argc, char **argv)
 
         // Main loop
         cv::Mat im;
-        vector<ORB_SLAM3::IMU::Point> vImuMeas;
+        vector<MORB_SLAM::IMU::Point> vImuMeas;
         proccIm = 0;
         cv::Ptr<cv::CLAHE> clahe = cv::createCLAHE(3.0, cv::Size(8, 8));
         for(int ni=0; ni<nImages[seq]; ni++, proccIm++)
@@ -164,7 +164,7 @@ int main(int argc, char **argv)
 
                 while(vTimestampsImu[seq][first_imu[seq]]<=vTimestampsCam[seq][ni])
                 {
-                    vImuMeas.push_back(ORB_SLAM3::IMU::Point(vAcc[seq][first_imu[seq]].x,vAcc[seq][first_imu[seq]].y,vAcc[seq][first_imu[seq]].z,
+                    vImuMeas.push_back(MORB_SLAM::IMU::Point(vAcc[seq][first_imu[seq]].x,vAcc[seq][first_imu[seq]].y,vAcc[seq][first_imu[seq]].z,
                                                              vGyro[seq][first_imu[seq]].x,vGyro[seq][first_imu[seq]].y,vGyro[seq][first_imu[seq]].z,
                                                              vTimestampsImu[seq][first_imu[seq]]));
                     // cout << "t_imu = " << fixed << vImuMeas.back().t << endl;
diff --git a/Examples/Monocular/mono_euroc.cc b/Examples/Monocular/mono_euroc.cc
index df6eb16e..3077df02 100644
--- a/Examples/Monocular/mono_euroc.cc
+++ b/Examples/Monocular/mono_euroc.cc
@@ -81,8 +81,8 @@ int main(int argc, char **argv)
     // int fps = 20; // UNUSED
     // float dT = 1.f/fps; // UNUSED
     // Create SLAM system. It initializes all system threads and gets ready to process frames.
-    ORB_SLAM3::System_ptr SLAM = std::make_shared<ORB_SLAM3::System>(argv[1],argv[2],ORB_SLAM3::CameraType::MONOCULAR);
-    ORB_SLAM3::Viewer viewer(SLAM, argv[2]);
+    MORB_SLAM::System_ptr SLAM = std::make_shared<MORB_SLAM::System>(argv[1],argv[2],MORB_SLAM::CameraType::MONOCULAR);
+    MORB_SLAM::Viewer viewer(SLAM, argv[2]);
     float imageScale = SLAM->GetImageScale();
 
 #ifdef REGISTER_TIMES
diff --git a/Examples/Monocular/mono_kitti.cc b/Examples/Monocular/mono_kitti.cc
index 567bdded..337c6d2f 100644
--- a/Examples/Monocular/mono_kitti.cc
+++ b/Examples/Monocular/mono_kitti.cc
@@ -48,8 +48,8 @@ int main(int argc, char **argv)
     int nImages = vstrImageFilenames.size();
 
     // Create SLAM system. It initializes all system threads and gets ready to process frames.
-    ORB_SLAM3::System_ptr SLAM = std::make_shared<ORB_SLAM3::System>(argv[1],argv[2],ORB_SLAM3::CameraType::MONOCULAR);
-    ORB_SLAM3::Viewer viewer(SLAM, argv[2]);
+    MORB_SLAM::System_ptr SLAM = std::make_shared<MORB_SLAM::System>(argv[1],argv[2],MORB_SLAM::CameraType::MONOCULAR);
+    MORB_SLAM::Viewer viewer(SLAM, argv[2]);
     float imageScale = SLAM->GetImageScale();
 
     // Vector for tracking time statistics
@@ -97,7 +97,7 @@ int main(int argc, char **argv)
         std::chrono::steady_clock::time_point t1 = std::chrono::steady_clock::now();
 
         // Pass the image to the SLAM system
-        auto pos = SLAM->TrackMonocular(im,tframe,vector<ORB_SLAM3::IMU::Point>(), vstrImageFilenames[ni]);
+        auto pos = SLAM->TrackMonocular(im,tframe,vector<MORB_SLAM::IMU::Point>(), vstrImageFilenames[ni]);
 
         std::chrono::steady_clock::time_point t2 = std::chrono::steady_clock::now();
 
diff --git a/Examples/Monocular/mono_realsense_D435i.cc b/Examples/Monocular/mono_realsense_D435i.cc
index 422cd62c..b32e5d65 100644
--- a/Examples/Monocular/mono_realsense_D435i.cc
+++ b/Examples/Monocular/mono_realsense_D435i.cc
@@ -208,8 +208,8 @@ int main(int argc, char **argv) {
     std::cout << " Model = " << intrinsics_left.model << std::endl;
 
     // Create SLAM system. It initializes all system threads and gets ready to process frames.
-    ORB_SLAM3::System_ptr SLAM = std::make_shared<ORB_SLAM3::System>(argv[1],argv[2],ORB_SLAM3::CameraType::MONOCULAR, file_name);
-    ORB_SLAM3::Viewer viewer(SLAM, argv[2]);
+    MORB_SLAM::System_ptr SLAM = std::make_shared<MORB_SLAM::System>(argv[1],argv[2],MORB_SLAM::CameraType::MONOCULAR, file_name);
+    MORB_SLAM::Viewer viewer(SLAM, argv[2]);
     float imageScale = SLAM->GetImageScale();
 
     double timestamp;
diff --git a/Examples/Monocular/mono_realsense_t265.cc b/Examples/Monocular/mono_realsense_t265.cc
index 39007c30..91f93962 100644
--- a/Examples/Monocular/mono_realsense_t265.cc
+++ b/Examples/Monocular/mono_realsense_t265.cc
@@ -88,8 +88,8 @@ int main(int argc, char **argv)
     cout << "IMU data in the sequence: " << nImu << endl << endl;*/
 
     // Create SLAM system. It initializes all system threads and gets ready to process frames.
-    ORB_SLAM3::System_ptr SLAM = std::make_shared<ORB_SLAM3::System>(argv[1],argv[2],ORB_SLAM3::CameraType::MONOCULAR);
-    ORB_SLAM3::Viewer viewer(SLAM, argv[2]);
+    MORB_SLAM::System_ptr SLAM = std::make_shared<MORB_SLAM::System>(argv[1],argv[2],MORB_SLAM::CameraType::MONOCULAR);
+    MORB_SLAM::Viewer viewer(SLAM, argv[2]);
     float imageScale = SLAM->GetImageScale();
 
     cv::Mat imCV;
diff --git a/Examples/Monocular/mono_tum.cc b/Examples/Monocular/mono_tum.cc
index f528f93d..d76eadba 100644
--- a/Examples/Monocular/mono_tum.cc
+++ b/Examples/Monocular/mono_tum.cc
@@ -48,8 +48,8 @@ int main(int argc, char **argv)
     int nImages = vstrImageFilenames.size();
 
     // Create SLAM system. It initializes all system threads and gets ready to process frames.
-    ORB_SLAM3::System_ptr SLAM = std::make_shared<ORB_SLAM3::System>(argv[1],argv[2],ORB_SLAM3::CameraType::MONOCULAR);
-    ORB_SLAM3::Viewer viewer(SLAM, argv[2]);
+    MORB_SLAM::System_ptr SLAM = std::make_shared<MORB_SLAM::System>(argv[1],argv[2],MORB_SLAM::CameraType::MONOCULAR);
+    MORB_SLAM::Viewer viewer(SLAM, argv[2]);
     float imageScale = SLAM->GetImageScale();
 
     // Vector for tracking time statistics
diff --git a/Examples/Monocular/mono_tum_vi.cc b/Examples/Monocular/mono_tum_vi.cc
index a2faedb4..6995bff6 100644
--- a/Examples/Monocular/mono_tum_vi.cc
+++ b/Examples/Monocular/mono_tum_vi.cc
@@ -90,8 +90,8 @@ int main(int argc, char **argv)
     cout.precision(17);
 
     // Create SLAM system. It initializes all system threads and gets ready to process frames.
-    ORB_SLAM3::System_ptr SLAM = std::make_shared<ORB_SLAM3::System>(argv[1],argv[2],ORB_SLAM3::CameraType::MONOCULAR, file_name);
-    ORB_SLAM3::Viewer viewer(SLAM, argv[2]);
+    MORB_SLAM::System_ptr SLAM = std::make_shared<MORB_SLAM::System>(argv[1],argv[2],MORB_SLAM::CameraType::MONOCULAR, file_name);
+    MORB_SLAM::Viewer viewer(SLAM, argv[2]);
     float imageScale = SLAM->GetImageScale();
 
 #ifdef REGISTER_TIMES
diff --git a/Examples/RGB-D-Inertial/rgbd_inertial_realsense_D435i.cc b/Examples/RGB-D-Inertial/rgbd_inertial_realsense_D435i.cc
index 68e6adbf..82d7db96 100644
--- a/Examples/RGB-D-Inertial/rgbd_inertial_realsense_D435i.cc
+++ b/Examples/RGB-D-Inertial/rgbd_inertial_realsense_D435i.cc
@@ -291,7 +291,7 @@ int main(int argc, char **argv) {
     pipe_profile = pipe.start(cfg, imu_callback);
 
 
-    vector<ORB_SLAM3::IMU::Point> vImuMeas;
+    vector<MORB_SLAM::IMU::Point> vImuMeas;
     rs2::stream_profile cam_stream = pipe_profile.get_stream(RS2_STREAM_COLOR);
 
     rs2::stream_profile imu_stream = pipe_profile.get_stream(RS2_STREAM_GYRO);
@@ -319,8 +319,8 @@ int main(int argc, char **argv) {
 
 
     // Create SLAM system. It initializes all system threads and gets ready to process frames.
-    ORB_SLAM3::System_ptr SLAM = std::make_shared<ORB_SLAM3::System>(argv[1],argv[2],ORB_SLAM3::CameraType::IMU_RGBD, file_name);
-    ORB_SLAM3::Viewer viewer(SLAM, argv[2]);
+    MORB_SLAM::System_ptr SLAM = std::make_shared<MORB_SLAM::System>(argv[1],argv[2],MORB_SLAM::CameraType::IMU_RGBD, file_name);
+    MORB_SLAM::Viewer viewer(SLAM, argv[2]);
     float imageScale = SLAM->GetImageScale();
 
     double timestamp;
@@ -404,7 +404,7 @@ int main(int argc, char **argv) {
 
         for(size_t i=0; i<vGyro.size(); ++i)
         {
-            ORB_SLAM3::IMU::Point lastPoint(vAccel[i].x, vAccel[i].y, vAccel[i].z,
+            MORB_SLAM::IMU::Point lastPoint(vAccel[i].x, vAccel[i].y, vAccel[i].z,
                                             vGyro[i].x, vGyro[i].y, vGyro[i].z,
                                             vGyro_times[i]);
             vImuMeas.push_back(lastPoint);
diff --git a/Examples/RGB-D/rgbd_realsense_D435i.cc b/Examples/RGB-D/rgbd_realsense_D435i.cc
index e2d9d322..b677ebae 100644
--- a/Examples/RGB-D/rgbd_realsense_D435i.cc
+++ b/Examples/RGB-D/rgbd_realsense_D435i.cc
@@ -307,8 +307,8 @@ int main(int argc, char **argv) {
 
 
     // Create SLAM system. It initializes all system threads and gets ready to process frames.
-    ORB_SLAM3::System_ptr SLAM = std::make_shared<ORB_SLAM3::System>(argv[1],argv[2],ORB_SLAM3::CameraType::RGBD, file_name);
-    ORB_SLAM3::Viewer viewer(SLAM, argv[2]);
+    MORB_SLAM::System_ptr SLAM = std::make_shared<MORB_SLAM::System>(argv[1],argv[2],MORB_SLAM::CameraType::RGBD, file_name);
+    MORB_SLAM::Viewer viewer(SLAM, argv[2]);
     float imageScale = SLAM->GetImageScale();
 
     double timestamp;
diff --git a/Examples/RGB-D/rgbd_tum.cc b/Examples/RGB-D/rgbd_tum.cc
index 6274e76c..06c6081c 100644
--- a/Examples/RGB-D/rgbd_tum.cc
+++ b/Examples/RGB-D/rgbd_tum.cc
@@ -60,8 +60,8 @@ int main(int argc, char **argv)
     }
 
     // Create SLAM system. It initializes all system threads and gets ready to process frames.
-    ORB_SLAM3::System_ptr SLAM = std::make_shared<ORB_SLAM3::System>(argv[1],argv[2],ORB_SLAM3::CameraType::RGBD);
-    ORB_SLAM3::Viewer viewer(SLAM, argv[2]);
+    MORB_SLAM::System_ptr SLAM = std::make_shared<MORB_SLAM::System>(argv[1],argv[2],MORB_SLAM::CameraType::RGBD);
+    MORB_SLAM::Viewer viewer(SLAM, argv[2]);
     float imageScale = SLAM->GetImageScale();
 
     // Vector for tracking time statistics
diff --git a/Examples/Stereo-Inertial/RealSense_D435i.yaml b/Examples/Stereo-Inertial/RealSense_D435i.yaml
index e57e3aff..0f5dfc20 100755
--- a/Examples/Stereo-Inertial/RealSense_D435i.yaml
+++ b/Examples/Stereo-Inertial/RealSense_D435i.yaml
@@ -82,7 +82,7 @@ Viewer.ViewpointZ: -3.5
 Viewer.ViewpointF: 500.0
 
 # Save/load Map
-# System.LoadAtlasFromFile: "/home/ryan/MartinRea/MORB_SLAM/foxtrot5"
+System.LoadAtlasFromFile: "/home/landson/MORB_SLAM/trial"
 # System.LoadAtlasFromFile: "/home/ryan/MartinRea/MORB_SLAM/loopy"
 # System.SaveAtlasToFile: "/home/ryan/MartinRea/MORB_SLAM/rando"
 
diff --git a/Examples/Stereo-Inertial/stereo_inertial_euroc.cc b/Examples/Stereo-Inertial/stereo_inertial_euroc.cc
index fdfa1aec..a961fdb6 100644
--- a/Examples/Stereo-Inertial/stereo_inertial_euroc.cc
+++ b/Examples/Stereo-Inertial/stereo_inertial_euroc.cc
@@ -130,14 +130,14 @@ int main(int argc, char **argv)
     cout.precision(17);
 
     // Create SLAM system. It initializes all system threads and gets ready to process frames.
-    ORB_SLAM3::System_ptr SLAM = std::make_shared<ORB_SLAM3::System>(argv[1],argv[2],ORB_SLAM3::CameraType::IMU_STEREO);
-    ORB_SLAM3::Viewer viewer(SLAM, argv[2]);
+    MORB_SLAM::System_ptr SLAM = std::make_shared<MORB_SLAM::System>(argv[1],argv[2],MORB_SLAM::CameraType::IMU_STEREO);
+    MORB_SLAM::Viewer viewer(SLAM, argv[2]);
 
     cv::Mat imLeft, imRight;
     for (seq = 0; seq<num_seq; seq++)
     {
         // Seq loop
-        vector<ORB_SLAM3::IMU::Point> vImuMeas;
+        vector<MORB_SLAM::IMU::Point> vImuMeas;
 
 #ifdef REGISTER_TIMES
         double t_rect = 0.f;
@@ -174,7 +174,7 @@ int main(int argc, char **argv)
             if(ni>0)
                 while(vTimestampsImu[seq][first_imu[seq]]<=vTimestampsCam[seq][ni]) // while(vTimestampsImu[first_imu]<=vTimestampsCam[ni])
                 {
-                    vImuMeas.push_back(ORB_SLAM3::IMU::Point(vAcc[seq][first_imu[seq]].x,vAcc[seq][first_imu[seq]].y,vAcc[seq][first_imu[seq]].z,
+                    vImuMeas.push_back(MORB_SLAM::IMU::Point(vAcc[seq][first_imu[seq]].x,vAcc[seq][first_imu[seq]].y,vAcc[seq][first_imu[seq]].z,
                                                              vGyro[seq][first_imu[seq]].x,vGyro[seq][first_imu[seq]].y,vGyro[seq][first_imu[seq]].z,
                                                              vTimestampsImu[seq][first_imu[seq]]));
                     first_imu[seq]++;
diff --git a/Examples/Stereo-Inertial/stereo_inertial_realsense_D435i.cc b/Examples/Stereo-Inertial/stereo_inertial_realsense_D435i.cc
index a5925dfa..3b8f6228 100755
--- a/Examples/Stereo-Inertial/stereo_inertial_realsense_D435i.cc
+++ b/Examples/Stereo-Inertial/stereo_inertial_realsense_D435i.cc
@@ -262,7 +262,7 @@ int main(int argc, char **argv) {
 
     rs2::pipeline_profile pipe_profile = pipe.start(cfg, imu_callback);
 
-    vector<ORB_SLAM3::IMU::Point> vImuMeas;
+    vector<MORB_SLAM::IMU::Point> vImuMeas;
     rs2::stream_profile cam_left = pipe_profile.get_stream(RS2_STREAM_INFRARED, 1);
     rs2::stream_profile cam_right = pipe_profile.get_stream(RS2_STREAM_INFRARED, 2);
 
@@ -318,8 +318,8 @@ int main(int argc, char **argv) {
 
 
     // Create SLAM system. It initializes all system threads and gets ready to process frames.
-    ORB_SLAM3::System_ptr SLAM = std::make_shared<ORB_SLAM3::System>(argv[1],argv[2],ORB_SLAM3::CameraType::IMU_STEREO, file_name);
-    ORB_SLAM3::Viewer viewer(SLAM, argv[2]);
+    MORB_SLAM::System_ptr SLAM = std::make_shared<MORB_SLAM::System>(argv[1],argv[2],MORB_SLAM::CameraType::IMU_STEREO, file_name);
+    MORB_SLAM::Viewer viewer(SLAM, argv[2]);
     float imageScale = SLAM->GetImageScale();
 
     double timestamp;
@@ -388,7 +388,7 @@ int main(int argc, char **argv) {
 
         for(size_t i=0; i<vGyro.size(); ++i)
         {
-            ORB_SLAM3::IMU::Point lastPoint(vAccel[i].x, vAccel[i].y, vAccel[i].z,
+            MORB_SLAM::IMU::Point lastPoint(vAccel[i].x, vAccel[i].y, vAccel[i].z,
                                   vGyro[i].x, vGyro[i].y, vGyro[i].z,
                                   vGyro_times[i]);
             vImuMeas.push_back(lastPoint);
@@ -416,9 +416,7 @@ int main(int argc, char **argv) {
 #endif
         // Stereo images are already rectified.
         auto pos = SLAM->TrackStereo(im, imRight, timestamp, vImuMeas);
-        std::cout << "XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX: " << pos.translation()[0] << std::endl;
-        std::cout << "YYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYY: " << pos.translation()[1] << std::endl;
-        std::cout << "ZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZ: " << pos.translation()[2] << std::endl;
+        std::cout << "POSE: " << std::endl << pos.inverse().translation() << std::endl;
 #ifdef REGISTER_TIMES
         std::chrono::steady_clock::time_point t_End_Track = std::chrono::steady_clock::now();
         t_track = t_resize + std::chrono::duration_cast<std::chrono::duration<double,std::milli> >(t_End_Track - t_Start_Track).count();
diff --git a/Examples/Stereo-Inertial/stereo_inertial_realsense_t265.cc b/Examples/Stereo-Inertial/stereo_inertial_realsense_t265.cc
index a3e2fa99..0a673edf 100644
--- a/Examples/Stereo-Inertial/stereo_inertial_realsense_t265.cc
+++ b/Examples/Stereo-Inertial/stereo_inertial_realsense_t265.cc
@@ -67,8 +67,8 @@ int main(int argc, char **argv)
         // bFileName = true; // UNUSED
     }
 
-    ORB_SLAM3::System_ptr SLAM = std::make_shared<ORB_SLAM3::System>(argv[1],argv[2],ORB_SLAM3::CameraType::IMU_STEREO, file_name);
-    ORB_SLAM3::Viewer viewer(SLAM, argv[2]);
+    MORB_SLAM::System_ptr SLAM = std::make_shared<MORB_SLAM::System>(argv[1],argv[2],MORB_SLAM::CameraType::IMU_STEREO, file_name);
+    MORB_SLAM::Viewer viewer(SLAM, argv[2]);
     float imageScale = SLAM->GetImageScale();
 
     struct sigaction sigIntHandler;
@@ -196,7 +196,7 @@ int main(int argc, char **argv)
     }
 
     // Create SLAM system. It initializes all system threads and gets ready to process frames.
-    vector<ORB_SLAM3::IMU::Point> vImuMeas;
+    vector<MORB_SLAM::IMU::Point> vImuMeas;
 
     double timestamp;
     cv::Mat im,imright;
@@ -278,7 +278,7 @@ int main(int argc, char **argv)
 
 
         for(size_t i=0; i<vGyro.size(); ++i){
-            ORB_SLAM3::IMU::Point lastPoint(vAccel[i].x, vAccel[i].y, vAccel[i].z,
+            MORB_SLAM::IMU::Point lastPoint(vAccel[i].x, vAccel[i].y, vAccel[i].z,
                                             vGyro[i].x, vGyro[i].y, vGyro[i].z,
                                             vGyro_times[i]);
             vImuMeas.push_back(lastPoint);
diff --git a/Examples/Stereo-Inertial/stereo_inertial_tum_vi.cc b/Examples/Stereo-Inertial/stereo_inertial_tum_vi.cc
index ae068c40..d7b9b33b 100644
--- a/Examples/Stereo-Inertial/stereo_inertial_tum_vi.cc
+++ b/Examples/Stereo-Inertial/stereo_inertial_tum_vi.cc
@@ -120,8 +120,8 @@ int main(int argc, char **argv)
     cout << "IMU data in the sequence: " << nImu << endl << endl;*/
 
     // Create SLAM system. It initializes all system threads and gets ready to process frames.
-    ORB_SLAM3::System_ptr SLAM = std::make_shared<ORB_SLAM3::System>(argv[1],argv[2],ORB_SLAM3::CameraType::IMU_STEREO, file_name);
-    ORB_SLAM3::Viewer viewer(SLAM, argv[2]);
+    MORB_SLAM::System_ptr SLAM = std::make_shared<MORB_SLAM::System>(argv[1],argv[2],MORB_SLAM::CameraType::IMU_STEREO, file_name);
+    MORB_SLAM::Viewer viewer(SLAM, argv[2]);
     float imageScale = SLAM->GetImageScale();
 
 #ifdef REGISTER_TIMES
@@ -135,7 +135,7 @@ int main(int argc, char **argv)
 
         // Main loop
         cv::Mat imLeft, imRight;
-        vector<ORB_SLAM3::IMU::Point> vImuMeas;
+        vector<MORB_SLAM::IMU::Point> vImuMeas;
         proccIm = 0;
         cv::Ptr<cv::CLAHE> clahe = cv::createCLAHE(3.0, cv::Size(8, 8));
         for(int ni=0; ni<nImages[seq]; ni++, proccIm++)
@@ -184,8 +184,8 @@ int main(int argc, char **argv)
 
                 while(vTimestampsImu[seq][first_imu[seq]]<=vTimestampsCam[seq][ni])
                 {
-                    // vImuMeas.push_back(ORB_SLAM3::IMU::Point(vAcc[first_imu],vGyro[first_imu],vTimestampsImu[first_imu]));
-                    vImuMeas.push_back(ORB_SLAM3::IMU::Point(vAcc[seq][first_imu[seq]].x,vAcc[seq][first_imu[seq]].y,vAcc[seq][first_imu[seq]].z,
+                    // vImuMeas.push_back(MORB_SLAM::IMU::Point(vAcc[first_imu],vGyro[first_imu],vTimestampsImu[first_imu]));
+                    vImuMeas.push_back(MORB_SLAM::IMU::Point(vAcc[seq][first_imu[seq]].x,vAcc[seq][first_imu[seq]].y,vAcc[seq][first_imu[seq]].z,
                                                              vGyro[seq][first_imu[seq]].x,vGyro[seq][first_imu[seq]].y,vGyro[seq][first_imu[seq]].z,
                                                              vTimestampsImu[seq][first_imu[seq]]));
                     // cout << "t_imu = " << fixed << vImuMeas.back().t << endl;
diff --git a/Examples/Stereo/stereo_euroc.cc b/Examples/Stereo/stereo_euroc.cc
index b1a7c422..2ea855ef 100644
--- a/Examples/Stereo/stereo_euroc.cc
+++ b/Examples/Stereo/stereo_euroc.cc
@@ -89,8 +89,8 @@ int main(int argc, char **argv)
     cout.precision(17);
 
     // Create SLAM system. It initializes all system threads and gets ready to process frames.
-    ORB_SLAM3::System_ptr SLAM = std::make_shared<ORB_SLAM3::System>(argv[1],argv[2],ORB_SLAM3::CameraType::STEREO);
-    ORB_SLAM3::Viewer viewer(SLAM, argv[2]);
+    MORB_SLAM::System_ptr SLAM = std::make_shared<MORB_SLAM::System>(argv[1],argv[2],MORB_SLAM::CameraType::STEREO);
+    MORB_SLAM::Viewer viewer(SLAM, argv[2]);
 
     cv::Mat imLeft, imRight;
     for (seq = 0; seq<num_seq; seq++)
@@ -129,7 +129,7 @@ int main(int argc, char **argv)
             std::chrono::steady_clock::time_point t1 = std::chrono::steady_clock::now();
 
             // Pass the images to the SLAM system
-            auto pos = SLAM->TrackStereo(imLeft,imRight,tframe, vector<ORB_SLAM3::IMU::Point>(), vstrImageLeft[seq][ni]);
+            auto pos = SLAM->TrackStereo(imLeft,imRight,tframe, vector<MORB_SLAM::IMU::Point>(), vstrImageLeft[seq][ni]);
 
             std::chrono::steady_clock::time_point t2 = std::chrono::steady_clock::now();
 
diff --git a/Examples/Stereo/stereo_kitti.cc b/Examples/Stereo/stereo_kitti.cc
index cd658cde..509ea694 100644
--- a/Examples/Stereo/stereo_kitti.cc
+++ b/Examples/Stereo/stereo_kitti.cc
@@ -49,8 +49,8 @@ int main(int argc, char **argv)
     const int nImages = vstrImageLeft.size();
 
     // Create SLAM system. It initializes all system threads and gets ready to process frames.
-    ORB_SLAM3::System_ptr SLAM = std::make_shared<ORB_SLAM3::System>(argv[1],argv[2],ORB_SLAM3::CameraType::STEREO);
-    ORB_SLAM3::Viewer viewer(SLAM, argv[2]);
+    MORB_SLAM::System_ptr SLAM = std::make_shared<MORB_SLAM::System>(argv[1],argv[2],MORB_SLAM::CameraType::STEREO);
+    MORB_SLAM::Viewer viewer(SLAM, argv[2]);
 
     float imageScale = SLAM->GetImageScale();
 
diff --git a/Examples/Stereo/stereo_realsense_D435i.cc b/Examples/Stereo/stereo_realsense_D435i.cc
index c39d430c..cae1aee4 100644
--- a/Examples/Stereo/stereo_realsense_D435i.cc
+++ b/Examples/Stereo/stereo_realsense_D435i.cc
@@ -243,9 +243,9 @@ int main(int argc, char** argv) {
 
   // Create SLAM system. It initializes all system threads and gets ready to
   // process frames.
-  ORB_SLAM3::System_ptr SLAM = std::make_shared<ORB_SLAM3::System>(argv[1], argv[2], ORB_SLAM3::CameraType::STEREO, /*0,*/
+  MORB_SLAM::System_ptr SLAM = std::make_shared<MORB_SLAM::System>(argv[1], argv[2], MORB_SLAM::CameraType::STEREO, /*0,*/
                          file_name);
-  ORB_SLAM3::Viewer viewer(SLAM, argv[2]);
+  MORB_SLAM::Viewer viewer(SLAM, argv[2]);
   float imageScale = SLAM->GetImageScale();
 
   double timestamp;
diff --git a/Examples/Stereo/stereo_realsense_t265.cc b/Examples/Stereo/stereo_realsense_t265.cc
index b36d4c0b..5f1da4a7 100644
--- a/Examples/Stereo/stereo_realsense_t265.cc
+++ b/Examples/Stereo/stereo_realsense_t265.cc
@@ -90,12 +90,12 @@ int main(int argc, char **argv)
     cout << "IMU data in the sequence: " << nImu << endl << endl;*/
 
     // Create SLAM system. It initializes all system threads and gets ready to process frames.
-    ORB_SLAM3::System_ptr SLAM = std::make_shared<ORB_SLAM3::System>(argv[1],argv[2],ORB_SLAM3::CameraType::STEREO, /*0, */file_name);
-    ORB_SLAM3::Viewer viewer(SLAM, argv[2]);
+    MORB_SLAM::System_ptr SLAM = std::make_shared<MORB_SLAM::System>(argv[1],argv[2],MORB_SLAM::CameraType::STEREO, /*0, */file_name);
+    MORB_SLAM::Viewer viewer(SLAM, argv[2]);
     float imageScale = SLAM->GetImageScale();
 
     cv::Mat imLeft, imRight;
-    vector<ORB_SLAM3::IMU::Point> vImuMeas;
+    vector<MORB_SLAM::IMU::Point> vImuMeas;
 
     rs2::stream_profile fisheye_stream_left = pipe_profile.get_stream(RS2_STREAM_FISHEYE, 1);
     rs2_intrinsics intrinsics_left = fisheye_stream_left.as<rs2::video_stream_profile>().get_intrinsics();
diff --git a/Examples/Stereo/stereo_tum_vi.cc b/Examples/Stereo/stereo_tum_vi.cc
index 85719981..c498d137 100644
--- a/Examples/Stereo/stereo_tum_vi.cc
+++ b/Examples/Stereo/stereo_tum_vi.cc
@@ -91,8 +91,8 @@ int main(int argc, char **argv)
     cout.precision(17);
 
     // Create SLAM system. It initializes all system threads and gets ready to process frames.
-    ORB_SLAM3::System_ptr SLAM = std::make_shared<ORB_SLAM3::System>(argv[1],argv[2],ORB_SLAM3::CameraType::STEREO);
-    ORB_SLAM3::Viewer viewer(SLAM, argv[2]);
+    MORB_SLAM::System_ptr SLAM = std::make_shared<MORB_SLAM::System>(argv[1],argv[2],MORB_SLAM::CameraType::STEREO);
+    MORB_SLAM::Viewer viewer(SLAM, argv[2]);
     float imageScale = SLAM->GetImageScale();
 
     cout << endl << "-------" << endl;
diff --git a/README.md b/README.md
index e4907a44..3bda943d 100644
--- a/README.md
+++ b/README.md
@@ -2,7 +2,7 @@
 
 [![CMake Build](https://github.com/Soldann/MORB_SLAM/actions/workflows/cmake.yml/badge.svg)](https://github.com/Soldann/MORB_SLAM/actions/workflows/cmake.yml)
 
-This fork of [ORB_SLAM3](https://github.com/UZ-SLAMLab/ORB_SLAM3) converts it into a CMake package that can be imported into other projects. Run `morbslam_installer.sh` to install.
+This fork of [MORB_SLAM](https://github.com/UZ-SLAMLab/MORB_SLAM) converts it into a CMake package that can be imported into other projects. Run `morbslam_installer.sh` to install.
 
 In your other projects, import using:
 ```
@@ -17,7 +17,7 @@ TARGET_LINK_LIBRARIES(${PROJECT_NAME} MORB_SLAM::MORB_SLAM)
 ### V1.0, December 22th, 2021
 **Authors:** Carlos Campos, Richard Elvira, Juan J. Gómez Rodríguez, [José M. M. Montiel](http://webdiis.unizar.es/~josemari/), [Juan D. Tardos](http://webdiis.unizar.es/~jdtardos/).
 
-The [Changelog](https://github.com/UZ-SLAMLab/ORB_SLAM3/blob/master/Changelog.md) describes the features of each version.
+The [Changelog](https://github.com/UZ-SLAMLab/MORB_SLAM/blob/master/Changelog.md) describes the features of each version.
 
 ORB-SLAM3 is the first real-time SLAM library able to perform **Visual, Visual-Inertial and Multi-Map SLAM** with **monocular, stereo and RGB-D** cameras, using **pin-hole and fisheye** lens models. In all sensor configurations, ORB-SLAM3 is as robust as the best systems available in the literature, and significantly more accurate. 
 
@@ -46,7 +46,7 @@ alt="ORB-SLAM3" width="240" height="180" border="10" /></a>
 
 # 1. License
 
-ORB-SLAM3 is released under [GPLv3 license](https://github.com/UZ-SLAMLab/ORB_SLAM3/LICENSE). For a list of all code/library dependencies (and associated licenses), please see [Dependencies.md](https://github.com/UZ-SLAMLab/ORB_SLAM3/blob/master/Dependencies.md).
+ORB-SLAM3 is released under [GPLv3 license](https://github.com/UZ-SLAMLab/MORB_SLAM/LICENSE). For a list of all code/library dependencies (and associated licenses), please see [Dependencies.md](https://github.com/UZ-SLAMLab/MORB_SLAM/blob/master/Dependencies.md).
 
 For a closed-source version of ORB-SLAM3 for commercial purposes, please contact the authors: orbslam (at) unizar (dot) es.
 
@@ -97,12 +97,12 @@ We provide some examples to process input of a monocular, monocular-inertial, st
 
 Clone the repository:
 ```
-git clone https://github.com/UZ-SLAMLab/ORB_SLAM3.git ORB_SLAM3
+git clone https://github.com/UZ-SLAMLab/MORB_SLAM.git MORB_SLAM
 ```
 
 We provide a script `build.sh` to build the *Thirdparty* libraries and *ORB-SLAM3*. Please make sure you have installed all required dependencies (see section 2). Execute:
 ```
-cd ORB_SLAM3
+cd MORB_SLAM
 chmod +x build.sh
 ./build.sh
 ```
@@ -170,14 +170,14 @@ Execute the following script to process sequences and compute the RMS ATE:
 ### Building the nodes for mono, mono-inertial, stereo, stereo-inertial and RGB-D
 Tested with ROS Melodic and ubuntu 18.04.
 
-1. Add the path including *Examples/ROS/ORB_SLAM3* to the ROS_PACKAGE_PATH environment variable. Open .bashrc file:
+1. Add the path including *Examples/ROS/MORB_SLAM* to the ROS_PACKAGE_PATH environment variable. Open .bashrc file:
   ```
   gedit ~/.bashrc
   ```
-and add at the end the following line. Replace PATH by the folder where you cloned ORB_SLAM3:
+and add at the end the following line. Replace PATH by the folder where you cloned MORB_SLAM:
 
   ```
-  export ROS_PACKAGE_PATH=${ROS_PACKAGE_PATH}:PATH/ORB_SLAM3/Examples/ROS
+  export ROS_PACKAGE_PATH=${ROS_PACKAGE_PATH}:PATH/MORB_SLAM/Examples/ROS
   ```
   
 2. Execute `build_ros.sh` script:
@@ -188,38 +188,38 @@ and add at the end the following line. Replace PATH by the folder where you clon
   ```
   
 ### Running Monocular Node
-For a monocular input from topic `/camera/image_raw` run node ORB_SLAM3/Mono. You will need to provide the vocabulary file and a settings file. See the monocular examples above.
+For a monocular input from topic `/camera/image_raw` run node MORB_SLAM/Mono. You will need to provide the vocabulary file and a settings file. See the monocular examples above.
 
   ```
-  rosrun ORB_SLAM3 Mono PATH_TO_VOCABULARY PATH_TO_SETTINGS_FILE
+  rosrun MORB_SLAM Mono PATH_TO_VOCABULARY PATH_TO_SETTINGS_FILE
   ```
 
 ### Running Monocular-Inertial Node
-For a monocular input from topic `/camera/image_raw` and an inertial input from topic `/imu`, run node ORB_SLAM3/Mono_Inertial. Setting the optional third argument to true will apply CLAHE equalization to images (Mainly for TUM-VI dataset).
+For a monocular input from topic `/camera/image_raw` and an inertial input from topic `/imu`, run node MORB_SLAM/Mono_Inertial. Setting the optional third argument to true will apply CLAHE equalization to images (Mainly for TUM-VI dataset).
 
   ```
-  rosrun ORB_SLAM3 Mono PATH_TO_VOCABULARY PATH_TO_SETTINGS_FILE [EQUALIZATION]	
+  rosrun MORB_SLAM Mono PATH_TO_VOCABULARY PATH_TO_SETTINGS_FILE [EQUALIZATION]	
   ```
 
 ### Running Stereo Node
-For a stereo input from topic `/camera/left/image_raw` and `/camera/right/image_raw` run node ORB_SLAM3/Stereo. You will need to provide the vocabulary file and a settings file. For Pinhole camera model, if you **provide rectification matrices** (see Examples/Stereo/EuRoC.yaml example), the node will recitify the images online, **otherwise images must be pre-rectified**. For FishEye camera model, rectification is not required since system works with original images:
+For a stereo input from topic `/camera/left/image_raw` and `/camera/right/image_raw` run node MORB_SLAM/Stereo. You will need to provide the vocabulary file and a settings file. For Pinhole camera model, if you **provide rectification matrices** (see Examples/Stereo/EuRoC.yaml example), the node will recitify the images online, **otherwise images must be pre-rectified**. For FishEye camera model, rectification is not required since system works with original images:
 
   ```
-  rosrun ORB_SLAM3 Stereo PATH_TO_VOCABULARY PATH_TO_SETTINGS_FILE ONLINE_RECTIFICATION
+  rosrun MORB_SLAM Stereo PATH_TO_VOCABULARY PATH_TO_SETTINGS_FILE ONLINE_RECTIFICATION
   ```
 
 ### Running Stereo-Inertial Node
-For a stereo input from topics `/camera/left/image_raw` and `/camera/right/image_raw`, and an inertial input from topic `/imu`, run node ORB_SLAM3/Stereo_Inertial. You will need to provide the vocabulary file and a settings file, including rectification matrices if required in a similar way to Stereo case:
+For a stereo input from topics `/camera/left/image_raw` and `/camera/right/image_raw`, and an inertial input from topic `/imu`, run node MORB_SLAM/Stereo_Inertial. You will need to provide the vocabulary file and a settings file, including rectification matrices if required in a similar way to Stereo case:
 
   ```
-  rosrun ORB_SLAM3 Stereo_Inertial PATH_TO_VOCABULARY PATH_TO_SETTINGS_FILE ONLINE_RECTIFICATION [EQUALIZATION]	
+  rosrun MORB_SLAM Stereo_Inertial PATH_TO_VOCABULARY PATH_TO_SETTINGS_FILE ONLINE_RECTIFICATION [EQUALIZATION]	
   ```
   
 ### Running RGB_D Node
-For an RGB-D input from topics `/camera/rgb/image_raw` and `/camera/depth_registered/image_raw`, run node ORB_SLAM3/RGBD. You will need to provide the vocabulary file and a settings file. See the RGB-D example above.
+For an RGB-D input from topics `/camera/rgb/image_raw` and `/camera/depth_registered/image_raw`, run node MORB_SLAM/RGBD. You will need to provide the vocabulary file and a settings file. See the RGB-D example above.
 
   ```
-  rosrun ORB_SLAM3 RGBD PATH_TO_VOCABULARY PATH_TO_SETTINGS_FILE
+  rosrun MORB_SLAM RGBD PATH_TO_VOCABULARY PATH_TO_SETTINGS_FILE
   ```
 
 **Running ROS example:** Download a rosbag (e.g. V1_02_medium.bag) from the EuRoC dataset (http://projects.asl.ethz.ch/datasets/doku.php?id=kmavvisualinertialdatasets). Open 3 tabs on the terminal and run the following command at each tab for a Stereo-Inertial configuration:
@@ -228,7 +228,7 @@ For an RGB-D input from topics `/camera/rgb/image_raw` and `/camera/depth_regist
   ```
   
   ```
-  rosrun ORB_SLAM3 Stereo_Inertial Vocabulary/ORBvoc.txt Examples/Stereo-Inertial/EuRoC.yaml true
+  rosrun MORB_SLAM Stereo_Inertial Vocabulary/ORBvoc.txt Examples/Stereo-Inertial/EuRoC.yaml true
   ```
   
   ```
diff --git a/build.sh b/build.sh
index 911991c3..e95d3111 100755
--- a/build.sh
+++ b/build.sh
@@ -41,7 +41,7 @@ if [ ! -f "ORBvoc.txt" ]; then
 fi
 cd ..
 
-echo "Configuring and building ORB_SLAM3 ..."
+echo "Configuring and building MORB_SLAM ..."
 
 mkdir build 2> /dev/null
 cd build
diff --git a/build_ros.sh b/build_ros.sh
index 1f13d215..886b5b1c 100755
--- a/build_ros.sh
+++ b/build_ros.sh
@@ -1,6 +1,6 @@
 echo "Building ROS nodes"
 
-cd Examples/ROS/ORB_SLAM3
+cd Examples/ROS/MORB_SLAM
 mkdir build
 cd build
 cmake .. -DROS_BUILD_TYPE=Release
diff --git a/clean.sh b/clean.sh
index c7a11891..d1b72f2d 100755
--- a/clean.sh
+++ b/clean.sh
@@ -40,18 +40,18 @@ rm Examples/RGB-D/rgbd_tum \
     Examples/Stereo-Inertial/stereo_inertial_tum_vi \
     Examples/Stereo-Inertial/stereo_inertial_realsense_t265 \
     Examples/Stereo-Inertial/stereo_inertial_realsense_D435i \
-    Examples/ROS/ORB_SLAM3/Mono \
-    Examples/ROS/ORB_SLAM3/Mono_Inertial \
-    Examples/ROS/ORB_SLAM3/MonoAR \
-    Examples/ROS/ORB_SLAM3/RGBD \
-    Examples/ROS/ORB_SLAM3/Stereo \
-    Examples/ROS/ORB_SLAM3/Stereo_Inertial \
+    Examples/ROS/MORB_SLAM/Mono \
+    Examples/ROS/MORB_SLAM/Mono_Inertial \
+    Examples/ROS/MORB_SLAM/MonoAR \
+    Examples/ROS/MORB_SLAM/RGBD \
+    Examples/ROS/MORB_SLAM/Stereo \
+    Examples/ROS/MORB_SLAM/Stereo_Inertial \
     Examples/Calibration/recorder_realsense_D435i \
     Examples/Calibration/recorder_realsense_T265 \
     Examples/Tests/viewer_dataset \
     Examples/Tests/sophus_test \
     2> /dev/null
 
-rm -r Examples/ROS/ORB_SLAM3/build 2> /dev/null
+rm -r Examples/ROS/MORB_SLAM/build 2> /dev/null
 
 echo cleaning complete
\ No newline at end of file
diff --git a/include/Atlas.h b/include/Atlas.h
index 25086845..0eb4efeb 100644
--- a/include/Atlas.h
+++ b/include/Atlas.h
@@ -36,7 +36,7 @@
 #include "MapPoint.h"
 #include "Pinhole.h"
 
-namespace ORB_SLAM3 {
+namespace MORB_SLAM {
 class Map;
 class MapPoint;
 class KeyFrame;
@@ -160,4 +160,4 @@ class Atlas {
 
 };  // class Atlas
 
-}  // namespace ORB_SLAM3
+}  // namespace MORB_SLAM
diff --git a/include/Camera.hpp b/include/Camera.hpp
index 416f71e0..7d94ca1e 100644
--- a/include/Camera.hpp
+++ b/include/Camera.hpp
@@ -12,7 +12,7 @@
 #include <deque>
 #include <utility>
 
-namespace ORB_SLAM3{
+namespace MORB_SLAM{
 
 
 template<typename T>
@@ -89,4 +89,4 @@ class Camera{
 typedef std::shared_ptr<Camera> Camera_ptr;
 typedef std::weak_ptr<Camera> Camera_wptr;
 
-} // namespace ORB_SLAM3
\ No newline at end of file
+} // namespace MORB_SLAM
\ No newline at end of file
diff --git a/include/CameraModels/GeometricCamera.h b/include/CameraModels/GeometricCamera.h
index 553d63ca..0726cf90 100644
--- a/include/CameraModels/GeometricCamera.h
+++ b/include/CameraModels/GeometricCamera.h
@@ -37,7 +37,7 @@
 #include "Converter.h"
 #include "GeometricTools.h"
 
-namespace ORB_SLAM3 {
+namespace MORB_SLAM {
 class GeometricCamera {
   friend class boost::serialization::access;
 
@@ -112,4 +112,4 @@ class GeometricCamera {
 
   unsigned int mnType;
 };
-}  // namespace ORB_SLAM3
+}  // namespace MORB_SLAM
diff --git a/include/CameraModels/KannalaBrandt8.h b/include/CameraModels/KannalaBrandt8.h
index 05028ac1..ae6194c7 100644
--- a/include/CameraModels/KannalaBrandt8.h
+++ b/include/CameraModels/KannalaBrandt8.h
@@ -26,7 +26,7 @@
 #include "GeometricCamera.h"
 #include "TwoViewReconstruction.h"
 
-namespace ORB_SLAM3 {
+namespace MORB_SLAM {
 class KannalaBrandt8 : public GeometricCamera {
   friend class boost::serialization::access;
 
@@ -130,5 +130,5 @@ class KannalaBrandt8 : public GeometricCamera {
                    const Eigen::Matrix<float, 3, 4>& Tcw2,
                    Eigen::Vector3f& x3D);
 };
-}  // namespace ORB_SLAM3
+}  // namespace MORB_SLAM
 
diff --git a/include/CameraModels/Pinhole.h b/include/CameraModels/Pinhole.h
index f335ee74..48112ce8 100644
--- a/include/CameraModels/Pinhole.h
+++ b/include/CameraModels/Pinhole.h
@@ -26,7 +26,7 @@
 #include "GeometricCamera.h"
 #include "TwoViewReconstruction.h"
 
-namespace ORB_SLAM3 {
+namespace MORB_SLAM {
 class Pinhole : public GeometricCamera {
   friend class boost::serialization::access;
 
@@ -104,7 +104,7 @@ class Pinhole : public GeometricCamera {
   //      [fx, fy, cx, cy]
   TwoViewReconstruction* tvr;
 };
-}  // namespace ORB_SLAM3
+}  // namespace MORB_SLAM
 
 // BOOST_CLASS_EXPORT_KEY(ORBSLAM2::Pinhole)
 
diff --git a/include/Converter.h b/include/Converter.h
index 0978d7d9..2f162d47 100644
--- a/include/Converter.h
+++ b/include/Converter.h
@@ -29,7 +29,7 @@
 #include "sophus/geometry.hpp"
 #include "sophus/sim3.hpp"
 
-namespace ORB_SLAM3 {
+namespace MORB_SLAM {
 
 class Converter {
  public:
@@ -75,4 +75,4 @@ class Converter {
   static Sophus::Sim3f toSophus(const g2o::Sim3 &S);
 };
 
-}  // namespace ORB_SLAM3
+}  // namespace MORB_SLAM
diff --git a/include/Frame.h b/include/Frame.h
index 639e819a..0b1c5f15 100644
--- a/include/Frame.h
+++ b/include/Frame.h
@@ -40,7 +40,7 @@
 
 #include "Camera.hpp"
 
-namespace ORB_SLAM3
+namespace MORB_SLAM
 {
 #define FRAME_GRID_ROWS 48
 #define FRAME_GRID_COLS 64
diff --git a/include/FrameDrawer.h b/include/FrameDrawer.h
index d5954762..f6babdd8 100644
--- a/include/FrameDrawer.h
+++ b/include/FrameDrawer.h
@@ -26,7 +26,7 @@
 #include <vector>
 #include "ImprovedTypes.hpp"
 
-namespace ORB_SLAM3 {
+namespace MORB_SLAM {
 
 class MapPoint;
 class Viewer;
@@ -68,4 +68,4 @@ class FrameDrawer {
   std::vector<std::pair<cv::Point2f, cv::Point2f>> mvTracks;
 };
 
-}  // namespace ORB_SLAM3
+}  // namespace MORB_SLAM
diff --git a/include/G2oTypes.h b/include/G2oTypes.h
index 6086d622..87c48a33 100644
--- a/include/G2oTypes.h
+++ b/include/G2oTypes.h
@@ -36,7 +36,7 @@
 #include"Converter.h"
 #include <math.h>
 
-namespace ORB_SLAM3
+namespace MORB_SLAM
 {
 
 class KeyFrame;
diff --git a/include/GeometricTools.h b/include/GeometricTools.h
index 8c2651b9..c1d888d0 100644
--- a/include/GeometricTools.h
+++ b/include/GeometricTools.h
@@ -25,7 +25,7 @@
 #include <opencv2/core/core.hpp>
 #include <sophus/se3.hpp>
 
-namespace ORB_SLAM3 {
+namespace MORB_SLAM {
 
 class KeyFrame;
 
@@ -77,5 +77,5 @@ class GeometricTools {
   }
 };
 
-}  // namespace ORB_SLAM3
+}  // namespace MORB_SLAM
 
diff --git a/include/ImprovedTypes.hpp b/include/ImprovedTypes.hpp
index 66a5688f..a66a18a9 100644
--- a/include/ImprovedTypes.hpp
+++ b/include/ImprovedTypes.hpp
@@ -4,7 +4,7 @@
 #include <utility>
 #include <unordered_map>
 
-namespace ORB_SLAM3{
+namespace MORB_SLAM{
 
 class System;
 class Atlas;
diff --git a/include/ImuTypes.h b/include/ImuTypes.h
index f9738759..4fb655fc 100644
--- a/include/ImuTypes.h
+++ b/include/ImuTypes.h
@@ -34,7 +34,7 @@
 
 #include "SerializationUtils.h"
 
-namespace ORB_SLAM3 {
+namespace MORB_SLAM {
 
 namespace IMU {
 
@@ -269,4 +269,4 @@ Eigen::Matrix3f NormalizeRotation(const Eigen::Matrix3f &R);
 
 }  // namespace IMU
 
-}  // namespace ORB_SLAM3
+}  // namespace MORB_SLAM
diff --git a/include/KeyFrame.h b/include/KeyFrame.h
index b4cabf46..8f5d7e3e 100644
--- a/include/KeyFrame.h
+++ b/include/KeyFrame.h
@@ -37,7 +37,7 @@
 #include "ORBextractor.h"
 #include "SerializationUtils.h"
 
-namespace ORB_SLAM3 {
+namespace MORB_SLAM {
 
 class Map;
 class MapPoint;
@@ -538,5 +538,5 @@ class KeyFrame {
   }
 };
 
-}  // namespace ORB_SLAM3
+}  // namespace MORB_SLAM
 
diff --git a/include/KeyFrameDatabase.h b/include/KeyFrameDatabase.h
index 52b46faf..370116de 100644
--- a/include/KeyFrameDatabase.h
+++ b/include/KeyFrameDatabase.h
@@ -34,7 +34,7 @@
 #include "Map.h"
 #include "ORBVocabulary.h"
 
-namespace ORB_SLAM3 {
+namespace MORB_SLAM {
 
 class KeyFrame;
 class Frame;
@@ -95,5 +95,5 @@ class KeyFrameDatabase {
   std::mutex mMutex;
 };
 
-}  // namespace ORB_SLAM3
+}  // namespace MORB_SLAM
 
diff --git a/include/LocalMapping.h b/include/LocalMapping.h
index 77a9a6f4..397d1f41 100644
--- a/include/LocalMapping.h
+++ b/include/LocalMapping.h
@@ -31,7 +31,7 @@
 #include "Settings.h"
 #include "Tracking.h"
 
-namespace ORB_SLAM3 {
+namespace MORB_SLAM {
 
 class System;
 class Tracking;
@@ -194,5 +194,5 @@ class LocalMapping {
   ofstream f_lm;
 };
 
-}  // namespace ORB_SLAM3
+}  // namespace MORB_SLAM
 
diff --git a/include/LoopClosing.h b/include/LoopClosing.h
index 35ba0980..d6265a66 100644
--- a/include/LoopClosing.h
+++ b/include/LoopClosing.h
@@ -33,7 +33,7 @@
 #include <mutex>
 #include "g2o/types/types_seven_dof_expmap.h"
 
-namespace ORB_SLAM3
+namespace MORB_SLAM
 {
 
 class Tracking;
diff --git a/include/MLPnPsolver.h b/include/MLPnPsolver.h
index a160db1c..84a8a395 100644
--- a/include/MLPnPsolver.h
+++ b/include/MLPnPsolver.h
@@ -57,7 +57,7 @@
 #include "Frame.h"
 #include "MapPoint.h"
 
-namespace ORB_SLAM3 {
+namespace MORB_SLAM {
 class MLPnPsolver {
  public:
   
@@ -240,4 +240,4 @@ class MLPnPsolver {
   std::shared_ptr<GeometricCamera> mpCamera;
 };
 
-}  // namespace ORB_SLAM3
+}  // namespace MORB_SLAM
diff --git a/include/Map.h b/include/Map.h
index fb40964d..5d8301db 100644
--- a/include/Map.h
+++ b/include/Map.h
@@ -30,7 +30,7 @@
 #include <stdexcept>
 
 
-namespace ORB_SLAM3
+namespace MORB_SLAM
 {
 
 class MapPoint;
@@ -202,4 +202,4 @@ class Map
 
 };
 
-} //namespace ORB_SLAM3
+} //namespace MORB_SLAM
diff --git a/include/MapDrawer.h b/include/MapDrawer.h
index 03f78f77..dc6259fd 100644
--- a/include/MapDrawer.h
+++ b/include/MapDrawer.h
@@ -25,7 +25,7 @@
 
 #include<mutex>
 
-namespace ORB_SLAM3
+namespace MORB_SLAM
 {
 
 class Settings;
diff --git a/include/MapPoint.h b/include/MapPoint.h
index 5725fa18..87342477 100644
--- a/include/MapPoint.h
+++ b/include/MapPoint.h
@@ -33,7 +33,7 @@
 #include <boost/serialization/array.hpp>
 #include <boost/serialization/map.hpp>
 
-namespace ORB_SLAM3
+namespace MORB_SLAM
 {
 
 class KeyFrame;
diff --git a/include/ORBVocabulary.h b/include/ORBVocabulary.h
index c5ab1805..0558266d 100644
--- a/include/ORBVocabulary.h
+++ b/include/ORBVocabulary.h
@@ -24,9 +24,9 @@
 #include "DBoW2/FORB.h"
 #include "DBoW2/TemplatedVocabulary.h"
 
-namespace ORB_SLAM3 {
+namespace MORB_SLAM {
 
 typedef DBoW2::TemplatedVocabulary<DBoW2::FORB::TDescriptor, DBoW2::FORB>
     ORBVocabulary;
 
-}  // namespace ORB_SLAM3
+}  // namespace MORB_SLAM
diff --git a/include/ORBextractor.h b/include/ORBextractor.h
index bc67cb3a..fafa90a9 100644
--- a/include/ORBextractor.h
+++ b/include/ORBextractor.h
@@ -25,7 +25,7 @@
 #include <opencv2/opencv.hpp>
 #include <vector>
 
-namespace ORB_SLAM3 {
+namespace MORB_SLAM {
 
 class ExtractorNode {
  public:
@@ -99,4 +99,4 @@ class ORBextractor {
   std::vector<float> mvInvLevelSigma2;
 };
 
-}  // namespace ORB_SLAM3
+}  // namespace MORB_SLAM
diff --git a/include/ORBmatcher.h b/include/ORBmatcher.h
index c44c2ac8..f7a94f56 100644
--- a/include/ORBmatcher.h
+++ b/include/ORBmatcher.h
@@ -30,7 +30,7 @@
 #include "MapPoint.h"
 #include "sophus/sim3.hpp"
 
-namespace ORB_SLAM3 {
+namespace MORB_SLAM {
 
 class ORBmatcher {
  public:
@@ -127,5 +127,5 @@ class ORBmatcher {
   bool mbCheckOrientation;
 };
 
-}  // namespace ORB_SLAM3
+}  // namespace MORB_SLAM
 
diff --git a/include/OptimizableTypes.h b/include/OptimizableTypes.h
index a0edcccd..601e2952 100644
--- a/include/OptimizableTypes.h
+++ b/include/OptimizableTypes.h
@@ -29,7 +29,7 @@
 
 #include "g2o/core/base_unary_edge.h"
 
-namespace ORB_SLAM3 {
+namespace MORB_SLAM {
 class EdgeSE3ProjectXYZOnlyPose
     : public g2o::BaseUnaryEdge<2, Eigen::Vector2d, g2o::VertexSE3Expmap> {
  public:
@@ -187,7 +187,7 @@ class VertexSim3Expmap : public g2o::BaseVertex<7, g2o::Sim3> {
 
 class EdgeSim3ProjectXYZ
     : public g2o::BaseBinaryEdge<2, Eigen::Vector2d, g2o::VertexSBAPointXYZ,
-                                 ORB_SLAM3::VertexSim3Expmap> {
+                                 MORB_SLAM::VertexSim3Expmap> {
  public:
   
   EdgeSim3ProjectXYZ();
@@ -195,8 +195,8 @@ class EdgeSim3ProjectXYZ
   virtual bool write(std::ostream& os) const;
 
   void computeError() {
-    const ORB_SLAM3::VertexSim3Expmap* v1 =
-        static_cast<const ORB_SLAM3::VertexSim3Expmap*>(_vertices[1]);
+    const MORB_SLAM::VertexSim3Expmap* v1 =
+        static_cast<const MORB_SLAM::VertexSim3Expmap*>(_vertices[1]);
     const g2o::VertexSBAPointXYZ* v2 =
         static_cast<const g2o::VertexSBAPointXYZ*>(_vertices[0]);
 
@@ -217,8 +217,8 @@ class EdgeInverseSim3ProjectXYZ
   virtual bool write(std::ostream& os) const;
 
   void computeError() {
-    const ORB_SLAM3::VertexSim3Expmap* v1 =
-        static_cast<const ORB_SLAM3::VertexSim3Expmap*>(_vertices[1]);
+    const MORB_SLAM::VertexSim3Expmap* v1 =
+        static_cast<const MORB_SLAM::VertexSim3Expmap*>(_vertices[1]);
     const g2o::VertexSBAPointXYZ* v2 =
         static_cast<const g2o::VertexSBAPointXYZ*>(_vertices[0]);
 
@@ -230,4 +230,4 @@ class EdgeInverseSim3ProjectXYZ
   // virtual void linearizeOplus();
 };
 
-}  // namespace ORB_SLAM3
+}  // namespace MORB_SLAM
diff --git a/include/Optimizer.h b/include/Optimizer.h
index 48bcb16e..cfb5ea43 100644
--- a/include/Optimizer.h
+++ b/include/Optimizer.h
@@ -38,7 +38,7 @@
 #include "g2o/types/types_seven_dof_expmap.h"
 #include "g2o/types/types_six_dof_expmap.h"
 
-namespace ORB_SLAM3 {
+namespace MORB_SLAM {
 
 class LoopClosing;
 
@@ -137,5 +137,5 @@ class Optimizer {
   ;
 };
 
-}  // namespace ORB_SLAM3
+}  // namespace MORB_SLAM
 
diff --git a/include/SerializationUtils.h b/include/SerializationUtils.h
index 11185c13..0f12eca3 100644
--- a/include/SerializationUtils.h
+++ b/include/SerializationUtils.h
@@ -29,7 +29,7 @@
 #include <sophus/se3.hpp>
 #include <vector>
 
-namespace ORB_SLAM3 {
+namespace MORB_SLAM {
 
 template <class Archive>
 void serializeSophusSE3(Archive& ar, Sophus::SE3f& T,
@@ -150,5 +150,5 @@ void serializeVectorKeyPoints(Archive& ar, const std::vector<cv::KeyPoint>& vKP,
   }
 }
 
-}  // namespace ORB_SLAM3
+}  // namespace MORB_SLAM
 
diff --git a/include/Settings.h b/include/Settings.h
index 3a369530..81e92531 100644
--- a/include/Settings.h
+++ b/include/Settings.h
@@ -34,7 +34,7 @@
 
 #include "CameraModels/GeometricCamera.h"
 
-namespace ORB_SLAM3 {
+namespace MORB_SLAM {
 
 class System;
 
@@ -235,5 +235,5 @@ class Settings {
    */
   float thFarPoints_;
 };
-}  // namespace ORB_SLAM3
+}  // namespace MORB_SLAM
 
diff --git a/include/Sim3Solver.h b/include/Sim3Solver.h
index adf512bd..47dff9d4 100644
--- a/include/Sim3Solver.h
+++ b/include/Sim3Solver.h
@@ -26,7 +26,7 @@
 
 #include "KeyFrame.h"
 
-namespace ORB_SLAM3 {
+namespace MORB_SLAM {
 
 class Sim3Solver {
  public:
@@ -134,4 +134,4 @@ class Sim3Solver {
   std::shared_ptr<GeometricCamera> pCamera1, pCamera2;
 };
 
-}  // namespace ORB_SLAM3
+}  // namespace MORB_SLAM
diff --git a/include/System.h b/include/System.h
index 6ea7b460..83555ce3 100644
--- a/include/System.h
+++ b/include/System.h
@@ -39,7 +39,7 @@
 #include "Camera.hpp"
 
 
-namespace ORB_SLAM3
+namespace MORB_SLAM
 {
 
 class Verbose
diff --git a/include/Tracking.h b/include/Tracking.h
index b5c21db7..2297c644 100644
--- a/include/Tracking.h
+++ b/include/Tracking.h
@@ -40,7 +40,7 @@
 #include "ImprovedTypes.hpp"
 #include "Camera.hpp"
 
-namespace ORB_SLAM3 {
+namespace MORB_SLAM {
 
 class Atlas;
 class LocalMapping;
@@ -360,5 +360,5 @@ class Tracking {
   cv::Mat mImRight;
 };
 
-}  // namespace ORB_SLAM3
+}  // namespace MORB_SLAM
 
diff --git a/include/TwoViewReconstruction.h b/include/TwoViewReconstruction.h
index e275d531..92bb39de 100644
--- a/include/TwoViewReconstruction.h
+++ b/include/TwoViewReconstruction.h
@@ -24,7 +24,7 @@
 
 #include <sophus/se3.hpp>
 
-namespace ORB_SLAM3
+namespace MORB_SLAM
 {
 
     class TwoViewReconstruction
diff --git a/include/Viewer.h b/include/Viewer.h
index 37f5db36..91aae010 100644
--- a/include/Viewer.h
+++ b/include/Viewer.h
@@ -29,7 +29,7 @@
 #include "MapDrawer.h"
 #include "Settings.h"
 
-namespace ORB_SLAM3 {
+namespace MORB_SLAM {
 
 class Viewer {
   void newParameterLoader(const Settings& settings);
@@ -74,4 +74,4 @@ class Viewer {
   bool mbClosed;
 };
 
-}  // namespace ORB_SLAM3
+}  // namespace MORB_SLAM
diff --git a/output.txt b/output.txt
index 3c6c406f..1cf3a992 100644
--- a/output.txt
+++ b/output.txt
@@ -65,7 +65,7 @@ Consolidate compiler generated dependencies of target test_ceres_se3.cpp
 [ 92%] Built target test_so3
 Consolidate compiler generated dependencies of target HelloSO3
 [100%] Built target HelloSO3
-Configuring and building ORB_SLAM3 ...
+Configuring and building MORB_SLAM ...
 -- Build type: RelWithDebInfo
 -- OPENCV VERSION:
 -- 4.5.4
@@ -96,13 +96,13 @@ Configuring and building ORB_SLAM3 ...
 [ 42%] Building CXX object CMakeFiles/MORB_SLAM.dir/src/Map.cc.o
 [ 43%] Building CXX object CMakeFiles/MORB_SLAM.dir/src/MapDrawer.cc.o
 [ 44%] Building CXX object CMakeFiles/MORB_SLAM.dir/src/Optimizer.cc.o
-/orin_ssd/MORB_SLAM/src/Map.cc: In member function ‘void ORB_SLAM3::Map::PreSave(std::set<std::shared_ptr<ORB_SLAM3::GeometricCamera> >&, std::shared_ptr<ORB_SLAM3::Map>)’:
-/orin_ssd/MORB_SLAM/src/Map.cc:339:32: error: no match for ‘operator!=’ (operand types are ‘std::shared_ptr<ORB_SLAM3::Map>’ and ‘std::__shared_ptr<ORB_SLAM3::Map, __gnu_cxx::_S_atomic>::element_type*’ {aka ‘ORB_SLAM3::Map*’})
+/orin_ssd/MORB_SLAM/src/Map.cc: In member function ‘void MORB_SLAM::Map::PreSave(std::set<std::shared_ptr<MORB_SLAM::GeometricCamera> >&, std::shared_ptr<MORB_SLAM::Map>)’:
+/orin_ssd/MORB_SLAM/src/Map.cc:339:32: error: no match for ‘operator!=’ (operand types are ‘std::shared_ptr<MORB_SLAM::Map>’ and ‘std::__shared_ptr<MORB_SLAM::Map, __gnu_cxx::_S_atomic>::element_type*’ {aka ‘MORB_SLAM::Map*’})
   339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
       |            ~~~~~~~~~~~~~~~~~~~ ^~ ~~~~~~~~~~~~~~~
       |                             |                  |
-      |                             |                  std::__shared_ptr<ORB_SLAM3::Map, __gnu_cxx::_S_atomic>::element_type* {aka ORB_SLAM3::Map*}
-      |                             std::shared_ptr<ORB_SLAM3::Map>
+      |                             |                  std::__shared_ptr<MORB_SLAM::Map, __gnu_cxx::_S_atomic>::element_type* {aka MORB_SLAM::Map*}
+      |                             std::shared_ptr<MORB_SLAM::Map>
 In file included from /usr/include/c++/9/bits/stl_algobase.h:64,
                  from /usr/include/c++/9/bits/stl_tree.h:63,
                  from /usr/include/c++/9/map:60,
@@ -115,7 +115,7 @@ In file included from /usr/include/c++/9/bits/stl_algobase.h:64,
   461 |     operator!=(const pair<_T1, _T2>& __x, const pair<_T1, _T2>& __y)
       |     ^~~~~~~~
 /usr/include/c++/9/bits/stl_pair.h:461:5: note:   template argument deduction/substitution failed:
-/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<ORB_SLAM3::Map>’ is not derived from ‘const std::pair<_T1, _T2>’
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<MORB_SLAM::Map>’ is not derived from ‘const std::pair<_T1, _T2>’
   339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
       |                                                 ^
 In file included from /usr/include/c++/9/bits/stl_algobase.h:67,
@@ -130,7 +130,7 @@ In file included from /usr/include/c++/9/bits/stl_algobase.h:67,
   337 |     operator!=(const reverse_iterator<_Iterator>& __x,
       |     ^~~~~~~~
 /usr/include/c++/9/bits/stl_iterator.h:337:5: note:   template argument deduction/substitution failed:
-/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<ORB_SLAM3::Map>’ is not derived from ‘const std::reverse_iterator<_Iterator>’
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<MORB_SLAM::Map>’ is not derived from ‘const std::reverse_iterator<_Iterator>’
   339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
       |                                                 ^
 In file included from /usr/include/c++/9/bits/stl_algobase.h:67,
@@ -145,7 +145,7 @@ In file included from /usr/include/c++/9/bits/stl_algobase.h:67,
   375 |     operator!=(const reverse_iterator<_IteratorL>& __x,
       |     ^~~~~~~~
 /usr/include/c++/9/bits/stl_iterator.h:375:5: note:   template argument deduction/substitution failed:
-/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<ORB_SLAM3::Map>’ is not derived from ‘const std::reverse_iterator<_Iterator>’
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<MORB_SLAM::Map>’ is not derived from ‘const std::reverse_iterator<_Iterator>’
   339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
       |                                                 ^
 In file included from /usr/include/c++/9/bits/stl_algobase.h:67,
@@ -160,7 +160,7 @@ In file included from /usr/include/c++/9/bits/stl_algobase.h:67,
  1148 |     operator!=(const move_iterator<_IteratorL>& __x,
       |     ^~~~~~~~
 /usr/include/c++/9/bits/stl_iterator.h:1148:5: note:   template argument deduction/substitution failed:
-/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<ORB_SLAM3::Map>’ is not derived from ‘const std::move_iterator<_IteratorL>’
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<MORB_SLAM::Map>’ is not derived from ‘const std::move_iterator<_IteratorL>’
   339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
       |                                                 ^
 In file included from /usr/include/c++/9/bits/stl_algobase.h:67,
@@ -175,7 +175,7 @@ In file included from /usr/include/c++/9/bits/stl_algobase.h:67,
  1154 |     operator!=(const move_iterator<_Iterator>& __x,
       |     ^~~~~~~~
 /usr/include/c++/9/bits/stl_iterator.h:1154:5: note:   template argument deduction/substitution failed:
-/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<ORB_SLAM3::Map>’ is not derived from ‘const std::move_iterator<_IteratorL>’
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<MORB_SLAM::Map>’ is not derived from ‘const std::move_iterator<_IteratorL>’
   339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
       |                                                 ^
 In file included from /usr/include/c++/9/bits/stl_tree.h:64,
@@ -189,7 +189,7 @@ In file included from /usr/include/c++/9/bits/stl_tree.h:64,
   173 |     operator!=(const allocator<_T1>&, const allocator<_T2>&)
       |     ^~~~~~~~
 /usr/include/c++/9/bits/allocator.h:173:5: note:   template argument deduction/substitution failed:
-/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<ORB_SLAM3::Map>’ is not derived from ‘const std::allocator<_Tp1>’
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<MORB_SLAM::Map>’ is not derived from ‘const std::allocator<_Tp1>’
   339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
       |                                                 ^
 In file included from /usr/include/c++/9/bits/char_traits.h:40,
@@ -208,7 +208,7 @@ In file included from /usr/include/c++/9/bits/char_traits.h:40,
   227 |     operator!=(const fpos<_StateT>& __lhs, const fpos<_StateT>& __rhs)
       |     ^~~~~~~~
 /usr/include/c++/9/bits/postypes.h:227:5: note:   template argument deduction/substitution failed:
-/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<ORB_SLAM3::Map>’ is not derived from ‘const std::fpos<_StateT>’
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<MORB_SLAM::Map>’ is not derived from ‘const std::fpos<_StateT>’
   339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
       |                                                 ^
 In file included from /usr/include/c++/9/bits/basic_string.h:48,
@@ -227,7 +227,7 @@ In file included from /usr/include/c++/9/bits/basic_string.h:48,
   489 |     operator!=(basic_string_view<_CharT, _Traits> __x,
       |     ^~~~~~~~
 /usr/include/c++/9/string_view:489:5: note:   template argument deduction/substitution failed:
-/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<ORB_SLAM3::Map>’ is not derived from ‘std::basic_string_view<_CharT, _Traits>’
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<MORB_SLAM::Map>’ is not derived from ‘std::basic_string_view<_CharT, _Traits>’
   339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
       |                                                 ^
 In file included from /usr/include/c++/9/bits/basic_string.h:48,
@@ -246,7 +246,7 @@ In file included from /usr/include/c++/9/bits/basic_string.h:48,
   495 |     operator!=(basic_string_view<_CharT, _Traits> __x,
       |     ^~~~~~~~
 /usr/include/c++/9/string_view:495:5: note:   template argument deduction/substitution failed:
-/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<ORB_SLAM3::Map>’ is not derived from ‘std::basic_string_view<_CharT, _Traits>’
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<MORB_SLAM::Map>’ is not derived from ‘std::basic_string_view<_CharT, _Traits>’
   339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
       |                                                 ^
 In file included from /usr/include/c++/9/bits/basic_string.h:48,
@@ -265,7 +265,7 @@ In file included from /usr/include/c++/9/bits/basic_string.h:48,
   501 |     operator!=(__detail::__idt<basic_string_view<_CharT, _Traits>> __x,
       |     ^~~~~~~~
 /usr/include/c++/9/string_view:501:5: note:   template argument deduction/substitution failed:
-/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   mismatched types ‘std::basic_string_view<_CharT, _Traits>’ and ‘std::__shared_ptr<ORB_SLAM3::Map, __gnu_cxx::_S_atomic>::element_type*’ {aka ‘ORB_SLAM3::Map*’}
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   mismatched types ‘std::basic_string_view<_CharT, _Traits>’ and ‘std::__shared_ptr<MORB_SLAM::Map, __gnu_cxx::_S_atomic>::element_type*’ {aka ‘MORB_SLAM::Map*’}
   339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
       |                                                 ^
 In file included from /usr/include/c++/9/string:55,
@@ -283,7 +283,7 @@ In file included from /usr/include/c++/9/string:55,
  6191 |     operator!=(const basic_string<_CharT, _Traits, _Alloc>& __lhs,
       |     ^~~~~~~~
 /usr/include/c++/9/bits/basic_string.h:6191:5: note:   template argument deduction/substitution failed:
-/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<ORB_SLAM3::Map>’ is not derived from ‘const std::__cxx11::basic_string<_CharT, _Traits, _Allocator>’
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<MORB_SLAM::Map>’ is not derived from ‘const std::__cxx11::basic_string<_CharT, _Traits, _Allocator>’
   339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
       |                                                 ^
 In file included from /usr/include/c++/9/string:55,
@@ -301,7 +301,7 @@ In file included from /usr/include/c++/9/string:55,
  6204 |     operator!=(const _CharT* __lhs,
       |     ^~~~~~~~
 /usr/include/c++/9/bits/basic_string.h:6204:5: note:   template argument deduction/substitution failed:
-/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   mismatched types ‘const _CharT*’ and ‘std::shared_ptr<ORB_SLAM3::Map>’
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   mismatched types ‘const _CharT*’ and ‘std::shared_ptr<MORB_SLAM::Map>’
   339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
       |                                                 ^
 In file included from /usr/include/c++/9/string:55,
@@ -319,7 +319,7 @@ In file included from /usr/include/c++/9/string:55,
  6216 |     operator!=(const basic_string<_CharT, _Traits, _Alloc>& __lhs,
       |     ^~~~~~~~
 /usr/include/c++/9/bits/basic_string.h:6216:5: note:   template argument deduction/substitution failed:
-/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<ORB_SLAM3::Map>’ is not derived from ‘const std::__cxx11::basic_string<_CharT, _Traits, _Allocator>’
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<MORB_SLAM::Map>’ is not derived from ‘const std::__cxx11::basic_string<_CharT, _Traits, _Allocator>’
   339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
       |                                                 ^
 In file included from /usr/include/c++/9/bits/node_handle.h:39,
@@ -334,7 +334,7 @@ In file included from /usr/include/c++/9/bits/node_handle.h:39,
   992 |     operator!=(const optional<_Tp>& __lhs, const optional<_Up>& __rhs)
       |     ^~~~~~~~
 /usr/include/c++/9/optional:992:5: note:   template argument deduction/substitution failed:
-/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<ORB_SLAM3::Map>’ is not derived from ‘const std::optional<_Tp>’
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<MORB_SLAM::Map>’ is not derived from ‘const std::optional<_Tp>’
   339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
       |                                                 ^
 In file included from /usr/include/c++/9/bits/node_handle.h:39,
@@ -349,7 +349,7 @@ In file included from /usr/include/c++/9/bits/node_handle.h:39,
  1044 |     operator!=(const optional<_Tp>& __lhs, nullopt_t) noexcept
       |     ^~~~~~~~
 /usr/include/c++/9/optional:1044:5: note:   template argument deduction/substitution failed:
-/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<ORB_SLAM3::Map>’ is not derived from ‘const std::optional<_Tp>’
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<MORB_SLAM::Map>’ is not derived from ‘const std::optional<_Tp>’
   339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
       |                                                 ^
 In file included from /usr/include/c++/9/bits/node_handle.h:39,
@@ -364,7 +364,7 @@ In file included from /usr/include/c++/9/bits/node_handle.h:39,
  1049 |     operator!=(nullopt_t, const optional<_Tp>& __rhs) noexcept
       |     ^~~~~~~~
 /usr/include/c++/9/optional:1049:5: note:   template argument deduction/substitution failed:
-/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   mismatched types ‘const std::optional<_Tp>’ and ‘std::__shared_ptr<ORB_SLAM3::Map, __gnu_cxx::_S_atomic>::element_type*’ {aka ‘ORB_SLAM3::Map*’}
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   mismatched types ‘const std::optional<_Tp>’ and ‘std::__shared_ptr<MORB_SLAM::Map, __gnu_cxx::_S_atomic>::element_type*’ {aka ‘MORB_SLAM::Map*’}
   339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
       |                                                 ^
 In file included from /usr/include/c++/9/bits/node_handle.h:39,
@@ -379,7 +379,7 @@ In file included from /usr/include/c++/9/bits/node_handle.h:39,
  1107 |     operator!=(const optional<_Tp>& __lhs, const _Up& __rhs)
       |     ^~~~~~~~
 /usr/include/c++/9/optional:1107:5: note:   template argument deduction/substitution failed:
-/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<ORB_SLAM3::Map>’ is not derived from ‘const std::optional<_Tp>’
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<MORB_SLAM::Map>’ is not derived from ‘const std::optional<_Tp>’
   339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
       |                                                 ^
 In file included from /usr/include/c++/9/bits/node_handle.h:39,
@@ -394,7 +394,7 @@ In file included from /usr/include/c++/9/bits/node_handle.h:39,
  1113 |     operator!=(const _Up& __lhs, const optional<_Tp>& __rhs)
       |     ^~~~~~~~
 /usr/include/c++/9/optional:1113:5: note:   template argument deduction/substitution failed:
-/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   mismatched types ‘const std::optional<_Tp>’ and ‘std::__shared_ptr<ORB_SLAM3::Map, __gnu_cxx::_S_atomic>::element_type*’ {aka ‘ORB_SLAM3::Map*’}
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   mismatched types ‘const std::optional<_Tp>’ and ‘std::__shared_ptr<MORB_SLAM::Map, __gnu_cxx::_S_atomic>::element_type*’ {aka ‘MORB_SLAM::Map*’}
   339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
       |                                                 ^
 In file included from /usr/include/c++/9/tuple:39,
@@ -409,7 +409,7 @@ In file included from /usr/include/c++/9/tuple:39,
   257 |     operator!=(const array<_Tp, _Nm>& __one, const array<_Tp, _Nm>& __two)
       |     ^~~~~~~~
 /usr/include/c++/9/array:257:5: note:   template argument deduction/substitution failed:
-/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<ORB_SLAM3::Map>’ is not derived from ‘const std::array<_Tp, _Nm>’
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<MORB_SLAM::Map>’ is not derived from ‘const std::array<_Tp, _Nm>’
   339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
       |                                                 ^
 In file included from /usr/include/c++/9/bits/stl_map.h:63,
@@ -423,7 +423,7 @@ In file included from /usr/include/c++/9/bits/stl_map.h:63,
  1445 |     operator!=(const tuple<_TElements...>& __t,
       |     ^~~~~~~~
 /usr/include/c++/9/tuple:1445:5: note:   template argument deduction/substitution failed:
-/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<ORB_SLAM3::Map>’ is not derived from ‘const std::tuple<_Tps ...>’
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<MORB_SLAM::Map>’ is not derived from ‘const std::tuple<_Tps ...>’
   339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
       |                                                 ^
 In file included from /usr/include/c++/9/map:61,
@@ -436,7 +436,7 @@ In file included from /usr/include/c++/9/map:61,
  1479 |     operator!=(const map<_Key, _Tp, _Compare, _Alloc>& __x,
       |     ^~~~~~~~
 /usr/include/c++/9/bits/stl_map.h:1479:5: note:   template argument deduction/substitution failed:
-/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<ORB_SLAM3::Map>’ is not derived from ‘const std::map<_Key, _Tp, _Compare, _Allocator>’
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<MORB_SLAM::Map>’ is not derived from ‘const std::map<_Key, _Tp, _Compare, _Allocator>’
   339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
       |                                                 ^
 In file included from /usr/include/c++/9/map:62,
@@ -449,7 +449,7 @@ In file included from /usr/include/c++/9/map:62,
  1143 |     operator!=(const multimap<_Key, _Tp, _Compare, _Alloc>& __x,
       |     ^~~~~~~~
 /usr/include/c++/9/bits/stl_multimap.h:1143:5: note:   template argument deduction/substitution failed:
-/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<ORB_SLAM3::Map>’ is not derived from ‘const std::multimap<_Key, _Tp, _Compare, _Allocator>’
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<MORB_SLAM::Map>’ is not derived from ‘const std::multimap<_Key, _Tp, _Compare, _Allocator>’
   339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
       |                                                 ^
 In file included from /usr/include/c++/9/memory:80,
@@ -465,7 +465,7 @@ In file included from /usr/include/c++/9/memory:80,
   732 |     operator!=(const unique_ptr<_Tp, _Dp>& __x,
       |     ^~~~~~~~
 /usr/include/c++/9/bits/unique_ptr.h:732:5: note:   template argument deduction/substitution failed:
-/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<ORB_SLAM3::Map>’ is not derived from ‘const std::unique_ptr<_Tp, _Dp>’
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<MORB_SLAM::Map>’ is not derived from ‘const std::unique_ptr<_Tp, _Dp>’
   339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
       |                                                 ^
 In file included from /usr/include/c++/9/memory:80,
@@ -481,7 +481,7 @@ In file included from /usr/include/c++/9/memory:80,
   738 |     operator!=(const unique_ptr<_Tp, _Dp>& __x, nullptr_t) noexcept
       |     ^~~~~~~~
 /usr/include/c++/9/bits/unique_ptr.h:738:5: note:   template argument deduction/substitution failed:
-/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<ORB_SLAM3::Map>’ is not derived from ‘const std::unique_ptr<_Tp, _Dp>’
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<MORB_SLAM::Map>’ is not derived from ‘const std::unique_ptr<_Tp, _Dp>’
   339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
       |                                                 ^
 In file included from /usr/include/c++/9/memory:80,
@@ -497,7 +497,7 @@ In file included from /usr/include/c++/9/memory:80,
   743 |     operator!=(nullptr_t, const unique_ptr<_Tp, _Dp>& __x) noexcept
       |     ^~~~~~~~
 /usr/include/c++/9/bits/unique_ptr.h:743:5: note:   template argument deduction/substitution failed:
-/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   mismatched types ‘const std::unique_ptr<_Tp, _Dp>’ and ‘std::__shared_ptr<ORB_SLAM3::Map, __gnu_cxx::_S_atomic>::element_type*’ {aka ‘ORB_SLAM3::Map*’}
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   mismatched types ‘const std::unique_ptr<_Tp, _Dp>’ and ‘std::__shared_ptr<MORB_SLAM::Map, __gnu_cxx::_S_atomic>::element_type*’ {aka ‘MORB_SLAM::Map*’}
   339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
       |                                                 ^
 In file included from /usr/include/c++/9/bits/shared_ptr.h:52,
@@ -514,7 +514,7 @@ In file included from /usr/include/c++/9/bits/shared_ptr.h:52,
  1428 |     operator!=(const __shared_ptr<_Tp1, _Lp>& __a,
       |     ^~~~~~~~
 /usr/include/c++/9/bits/shared_ptr_base.h:1428:5: note:   template argument deduction/substitution failed:
-/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   mismatched types ‘const std::__shared_ptr<_Tp2, _Lp>’ and ‘std::__shared_ptr<ORB_SLAM3::Map, __gnu_cxx::_S_atomic>::element_type*’ {aka ‘ORB_SLAM3::Map*’}
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   mismatched types ‘const std::__shared_ptr<_Tp2, _Lp>’ and ‘std::__shared_ptr<MORB_SLAM::Map, __gnu_cxx::_S_atomic>::element_type*’ {aka ‘MORB_SLAM::Map*’}
   339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
       |                                                 ^
 In file included from /usr/include/c++/9/bits/shared_ptr.h:52,
@@ -531,7 +531,7 @@ In file included from /usr/include/c++/9/bits/shared_ptr.h:52,
  1434 |     operator!=(const __shared_ptr<_Tp, _Lp>& __a, nullptr_t) noexcept
       |     ^~~~~~~~
 /usr/include/c++/9/bits/shared_ptr_base.h:1434:5: note:   template argument deduction/substitution failed:
-/orin_ssd/MORB_SLAM/src/Map.cc:339:48: note:   cannot convert ‘sharedMap.std::shared_ptr<ORB_SLAM3::Map>::<anonymous>.std::__shared_ptr<ORB_SLAM3::Map, __gnu_cxx::_S_atomic>::get()’ (type ‘std::__shared_ptr<ORB_SLAM3::Map, __gnu_cxx::_S_atomic>::element_type*’ {aka ‘ORB_SLAM3::Map*’}) to type ‘std::nullptr_t’
+/orin_ssd/MORB_SLAM/src/Map.cc:339:48: note:   cannot convert ‘sharedMap.std::shared_ptr<MORB_SLAM::Map>::<anonymous>.std::__shared_ptr<MORB_SLAM::Map, __gnu_cxx::_S_atomic>::get()’ (type ‘std::__shared_ptr<MORB_SLAM::Map, __gnu_cxx::_S_atomic>::element_type*’ {aka ‘MORB_SLAM::Map*’}) to type ‘std::nullptr_t’
   339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
       |                                   ~~~~~~~~~~~~~^~
 In file included from /usr/include/c++/9/bits/shared_ptr.h:52,
@@ -548,7 +548,7 @@ In file included from /usr/include/c++/9/bits/shared_ptr.h:52,
  1439 |     operator!=(nullptr_t, const __shared_ptr<_Tp, _Lp>& __a) noexcept
       |     ^~~~~~~~
 /usr/include/c++/9/bits/shared_ptr_base.h:1439:5: note:   template argument deduction/substitution failed:
-/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   mismatched types ‘const std::__shared_ptr<_Tp, _Lp>’ and ‘std::__shared_ptr<ORB_SLAM3::Map, __gnu_cxx::_S_atomic>::element_type*’ {aka ‘ORB_SLAM3::Map*’}
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   mismatched types ‘const std::__shared_ptr<_Tp, _Lp>’ and ‘std::__shared_ptr<MORB_SLAM::Map, __gnu_cxx::_S_atomic>::element_type*’ {aka ‘MORB_SLAM::Map*’}
   339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
       |                                                 ^
 In file included from /usr/include/c++/9/memory:81,
@@ -564,7 +564,7 @@ In file included from /usr/include/c++/9/memory:81,
   398 |     operator!=(const shared_ptr<_Tp>& __a, const shared_ptr<_Up>& __b) noexcept
       |     ^~~~~~~~
 /usr/include/c++/9/bits/shared_ptr.h:398:5: note:   template argument deduction/substitution failed:
-/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   mismatched types ‘const std::shared_ptr<_Tp>’ and ‘std::__shared_ptr<ORB_SLAM3::Map, __gnu_cxx::_S_atomic>::element_type*’ {aka ‘ORB_SLAM3::Map*’}
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   mismatched types ‘const std::shared_ptr<_Tp>’ and ‘std::__shared_ptr<MORB_SLAM::Map, __gnu_cxx::_S_atomic>::element_type*’ {aka ‘MORB_SLAM::Map*’}
   339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
       |                                                 ^
 In file included from /usr/include/c++/9/memory:81,
@@ -580,7 +580,7 @@ In file included from /usr/include/c++/9/memory:81,
   403 |     operator!=(const shared_ptr<_Tp>& __a, nullptr_t) noexcept
       |     ^~~~~~~~
 /usr/include/c++/9/bits/shared_ptr.h:403:5: note:   template argument deduction/substitution failed:
-/orin_ssd/MORB_SLAM/src/Map.cc:339:48: note:   cannot convert ‘sharedMap.std::shared_ptr<ORB_SLAM3::Map>::<anonymous>.std::__shared_ptr<ORB_SLAM3::Map, __gnu_cxx::_S_atomic>::get()’ (type ‘std::__shared_ptr<ORB_SLAM3::Map, __gnu_cxx::_S_atomic>::element_type*’ {aka ‘ORB_SLAM3::Map*’}) to type ‘std::nullptr_t’
+/orin_ssd/MORB_SLAM/src/Map.cc:339:48: note:   cannot convert ‘sharedMap.std::shared_ptr<MORB_SLAM::Map>::<anonymous>.std::__shared_ptr<MORB_SLAM::Map, __gnu_cxx::_S_atomic>::get()’ (type ‘std::__shared_ptr<MORB_SLAM::Map, __gnu_cxx::_S_atomic>::element_type*’ {aka ‘MORB_SLAM::Map*’}) to type ‘std::nullptr_t’
   339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
       |                                   ~~~~~~~~~~~~~^~
 In file included from /usr/include/c++/9/memory:81,
@@ -596,7 +596,7 @@ In file included from /usr/include/c++/9/memory:81,
   408 |     operator!=(nullptr_t, const shared_ptr<_Tp>& __a) noexcept
       |     ^~~~~~~~
 /usr/include/c++/9/bits/shared_ptr.h:408:5: note:   template argument deduction/substitution failed:
-/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   mismatched types ‘const std::shared_ptr<_Tp>’ and ‘std::__shared_ptr<ORB_SLAM3::Map, __gnu_cxx::_S_atomic>::element_type*’ {aka ‘ORB_SLAM3::Map*’}
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   mismatched types ‘const std::shared_ptr<_Tp>’ and ‘std::__shared_ptr<MORB_SLAM::Map, __gnu_cxx::_S_atomic>::element_type*’ {aka ‘MORB_SLAM::Map*’}
   339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
       |                                                 ^
 In file included from /usr/include/c++/9/vector:67,
@@ -611,7 +611,7 @@ In file included from /usr/include/c++/9/vector:67,
  1912 |     operator!=(const vector<_Tp, _Alloc>& __x, const vector<_Tp, _Alloc>& __y)
       |     ^~~~~~~~
 /usr/include/c++/9/bits/stl_vector.h:1912:5: note:   template argument deduction/substitution failed:
-/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<ORB_SLAM3::Map>’ is not derived from ‘const std::vector<_Tp, _Alloc>’
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<MORB_SLAM::Map>’ is not derived from ‘const std::vector<_Tp, _Alloc>’
   339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
       |                                                 ^
 In file included from /usr/include/c++/9/functional:59,
@@ -628,7 +628,7 @@ In file included from /usr/include/c++/9/functional:59,
   764 |     operator!=(const function<_Res(_Args...)>& __f, nullptr_t) noexcept
       |     ^~~~~~~~
 /usr/include/c++/9/bits/std_function.h:764:5: note:   template argument deduction/substitution failed:
-/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<ORB_SLAM3::Map>’ is not derived from ‘const std::function<_Res(_ArgTypes ...)>’
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<MORB_SLAM::Map>’ is not derived from ‘const std::function<_Res(_ArgTypes ...)>’
   339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
       |                                                 ^
 In file included from /usr/include/c++/9/functional:59,
@@ -645,7 +645,7 @@ In file included from /usr/include/c++/9/functional:59,
   770 |     operator!=(nullptr_t, const function<_Res(_Args...)>& __f) noexcept
       |     ^~~~~~~~
 /usr/include/c++/9/bits/std_function.h:770:5: note:   template argument deduction/substitution failed:
-/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   mismatched types ‘const std::function<_Res(_ArgTypes ...)>’ and ‘std::__shared_ptr<ORB_SLAM3::Map, __gnu_cxx::_S_atomic>::element_type*’ {aka ‘ORB_SLAM3::Map*’}
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   mismatched types ‘const std::function<_Res(_ArgTypes ...)>’ and ‘std::__shared_ptr<MORB_SLAM::Map, __gnu_cxx::_S_atomic>::element_type*’ {aka ‘MORB_SLAM::Map*’}
   339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
       |                                                 ^
 In file included from /usr/include/c++/9/unordered_map:47,
@@ -663,7 +663,7 @@ In file included from /usr/include/c++/9/unordered_map:47,
  2099 |     operator!=(const unordered_map<_Key, _Tp, _Hash, _Pred, _Alloc>& __x,
       |     ^~~~~~~~
 /usr/include/c++/9/bits/unordered_map.h:2099:5: note:   template argument deduction/substitution failed:
-/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<ORB_SLAM3::Map>’ is not derived from ‘const std::unordered_map<_Key1, _Tp1, _Hash1, _Pred1, _Alloc1>’
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<MORB_SLAM::Map>’ is not derived from ‘const std::unordered_map<_Key1, _Tp1, _Hash1, _Pred1, _Alloc1>’
   339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
       |                                                 ^
 In file included from /usr/include/c++/9/unordered_map:47,
@@ -681,7 +681,7 @@ In file included from /usr/include/c++/9/unordered_map:47,
  2111 |     operator!=(const unordered_multimap<_Key, _Tp, _Hash, _Pred, _Alloc>& __x,
       |     ^~~~~~~~
 /usr/include/c++/9/bits/unordered_map.h:2111:5: note:   template argument deduction/substitution failed:
-/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<ORB_SLAM3::Map>’ is not derived from ‘const std::unordered_multimap<_Key1, _Tp1, _Hash1, _Pred1, _Alloc1>’
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<MORB_SLAM::Map>’ is not derived from ‘const std::unordered_multimap<_Key1, _Tp1, _Hash1, _Pred1, _Alloc1>’
   339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
       |                                                 ^
 In file included from /usr/include/c++/9/bits/ios_base.h:46,
@@ -699,25 +699,25 @@ In file included from /usr/include/c++/9/bits/ios_base.h:46,
 /usr/include/c++/9/system_error:319:3: note: candidate: ‘bool std::operator!=(const std::error_code&, const std::error_code&)’
   319 |   operator!=(const error_code& __lhs, const error_code& __rhs) noexcept
       |   ^~~~~~~~
-/usr/include/c++/9/system_error:319:32: note:   no known conversion for argument 1 from ‘std::shared_ptr<ORB_SLAM3::Map>’ to ‘const std::error_code&’
+/usr/include/c++/9/system_error:319:32: note:   no known conversion for argument 1 from ‘std::shared_ptr<MORB_SLAM::Map>’ to ‘const std::error_code&’
   319 |   operator!=(const error_code& __lhs, const error_code& __rhs) noexcept
       |              ~~~~~~~~~~~~~~~~~~^~~~~
 /usr/include/c++/9/system_error:323:3: note: candidate: ‘bool std::operator!=(const std::error_code&, const std::error_condition&)’
   323 |   operator!=(const error_code& __lhs, const error_condition& __rhs) noexcept
       |   ^~~~~~~~
-/usr/include/c++/9/system_error:323:32: note:   no known conversion for argument 1 from ‘std::shared_ptr<ORB_SLAM3::Map>’ to ‘const std::error_code&’
+/usr/include/c++/9/system_error:323:32: note:   no known conversion for argument 1 from ‘std::shared_ptr<MORB_SLAM::Map>’ to ‘const std::error_code&’
   323 |   operator!=(const error_code& __lhs, const error_condition& __rhs) noexcept
       |              ~~~~~~~~~~~~~~~~~~^~~~~
 /usr/include/c++/9/system_error:327:3: note: candidate: ‘bool std::operator!=(const std::error_condition&, const std::error_code&)’
   327 |   operator!=(const error_condition& __lhs, const error_code& __rhs) noexcept
       |   ^~~~~~~~
-/usr/include/c++/9/system_error:327:37: note:   no known conversion for argument 1 from ‘std::shared_ptr<ORB_SLAM3::Map>’ to ‘const std::error_condition&’
+/usr/include/c++/9/system_error:327:37: note:   no known conversion for argument 1 from ‘std::shared_ptr<MORB_SLAM::Map>’ to ‘const std::error_condition&’
   327 |   operator!=(const error_condition& __lhs, const error_code& __rhs) noexcept
       |              ~~~~~~~~~~~~~~~~~~~~~~~^~~~~
 /usr/include/c++/9/system_error:331:3: note: candidate: ‘bool std::operator!=(const std::error_condition&, const std::error_condition&)’
   331 |   operator!=(const error_condition& __lhs,
       |   ^~~~~~~~
-/usr/include/c++/9/system_error:331:37: note:   no known conversion for argument 1 from ‘std::shared_ptr<ORB_SLAM3::Map>’ to ‘const std::error_condition&’
+/usr/include/c++/9/system_error:331:37: note:   no known conversion for argument 1 from ‘std::shared_ptr<MORB_SLAM::Map>’ to ‘const std::error_condition&’
   331 |   operator!=(const error_condition& __lhs,
       |              ~~~~~~~~~~~~~~~~~~~~~~~^~~~~
 In file included from /usr/include/c++/9/bits/locale_facets.h:48,
@@ -737,7 +737,7 @@ In file included from /usr/include/c++/9/bits/locale_facets.h:48,
   214 |     operator!=(const istreambuf_iterator<_CharT, _Traits>& __a,
       |     ^~~~~~~~
 /usr/include/c++/9/bits/streambuf_iterator.h:214:5: note:   template argument deduction/substitution failed:
-/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<ORB_SLAM3::Map>’ is not derived from ‘const std::istreambuf_iterator<_CharT, _Traits>’
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<MORB_SLAM::Map>’ is not derived from ‘const std::istreambuf_iterator<_CharT, _Traits>’
   339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
       |                                                 ^
 In file included from /usr/include/c++/9/iterator:66,
@@ -753,7 +753,7 @@ In file included from /usr/include/c++/9/iterator:66,
   141 |     operator!=(const istream_iterator<_Tp, _CharT, _Traits, _Dist>& __x,
       |     ^~~~~~~~
 /usr/include/c++/9/bits/stream_iterator.h:141:5: note:   template argument deduction/substitution failed:
-/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<ORB_SLAM3::Map>’ is not derived from ‘const std::istream_iterator<_Tp, _CharT, _Traits, _Dist>’
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<MORB_SLAM::Map>’ is not derived from ‘const std::istream_iterator<_Tp, _CharT, _Traits, _Dist>’
   339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
       |                                                 ^
 In file included from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/so2.hpp:7,
@@ -768,7 +768,7 @@ In file included from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/so2.hpp:7,
   481 |     operator!=(const complex<_Tp>& __x, const complex<_Tp>& __y)
       |     ^~~~~~~~
 /usr/include/c++/9/complex:481:5: note:   template argument deduction/substitution failed:
-/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<ORB_SLAM3::Map>’ is not derived from ‘const std::complex<_Tp>’
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<MORB_SLAM::Map>’ is not derived from ‘const std::complex<_Tp>’
   339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
       |                                                 ^
 In file included from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/so2.hpp:7,
@@ -783,7 +783,7 @@ In file included from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/so2.hpp:7,
   486 |     operator!=(const complex<_Tp>& __x, const _Tp& __y)
       |     ^~~~~~~~
 /usr/include/c++/9/complex:486:5: note:   template argument deduction/substitution failed:
-/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<ORB_SLAM3::Map>’ is not derived from ‘const std::complex<_Tp>’
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<MORB_SLAM::Map>’ is not derived from ‘const std::complex<_Tp>’
   339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
       |                                                 ^
 In file included from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/so2.hpp:7,
@@ -798,7 +798,7 @@ In file included from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/so2.hpp:7,
   491 |     operator!=(const _Tp& __x, const complex<_Tp>& __y)
       |     ^~~~~~~~
 /usr/include/c++/9/complex:491:5: note:   template argument deduction/substitution failed:
-/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   mismatched types ‘const std::complex<_Tp>’ and ‘std::__shared_ptr<ORB_SLAM3::Map, __gnu_cxx::_S_atomic>::element_type*’ {aka ‘ORB_SLAM3::Map*’}
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   mismatched types ‘const std::complex<_Tp>’ and ‘std::__shared_ptr<MORB_SLAM::Map, __gnu_cxx::_S_atomic>::element_type*’ {aka ‘MORB_SLAM::Map*’}
   339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
       |                                                 ^
 In file included from /usr/include/c++/9/random:49,
@@ -817,7 +817,7 @@ In file included from /usr/include/c++/9/random:49,
   421 |     operator!=(const std::linear_congruential_engine<_UIntType, __a,
       |     ^~~~~~~~
 /usr/include/c++/9/bits/random.h:421:5: note:   template argument deduction/substitution failed:
-/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<ORB_SLAM3::Map>’ is not derived from ‘const std::linear_congruential_engine<_UIntType, __a, __c, __m>’
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<MORB_SLAM::Map>’ is not derived from ‘const std::linear_congruential_engine<_UIntType, __a, __c, __m>’
   339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
       |                                                 ^
 In file included from /usr/include/c++/9/random:49,
@@ -836,7 +836,7 @@ In file included from /usr/include/c++/9/random:49,
   658 |     operator!=(const std::mersenne_twister_engine<_UIntType, __w, __n, __m,
       |     ^~~~~~~~
 /usr/include/c++/9/bits/random.h:658:5: note:   template argument deduction/substitution failed:
-/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<ORB_SLAM3::Map>’ is not derived from ‘const std::mersenne_twister_engine<_UIntType, __w, __n, __m, __r, __a, __u, __d, __s, __b, __t, __c, __l, __f>’
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<MORB_SLAM::Map>’ is not derived from ‘const std::mersenne_twister_engine<_UIntType, __w, __n, __m, __r, __a, __u, __d, __s, __b, __t, __c, __l, __f>’
   339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
       |                                                 ^
 In file included from /usr/include/c++/9/random:49,
@@ -855,7 +855,7 @@ In file included from /usr/include/c++/9/random:49,
   859 |     operator!=(const std::subtract_with_carry_engine<_UIntType, __w,
       |     ^~~~~~~~
 /usr/include/c++/9/bits/random.h:859:5: note:   template argument deduction/substitution failed:
-/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<ORB_SLAM3::Map>’ is not derived from ‘const std::subtract_with_carry_engine<_UIntType, __w, __s, __r>’
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<MORB_SLAM::Map>’ is not derived from ‘const std::subtract_with_carry_engine<_UIntType, __w, __s, __r>’
   339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
       |                                                 ^
 In file included from /usr/include/c++/9/random:49,
@@ -874,7 +874,7 @@ In file included from /usr/include/c++/9/random:49,
  1082 |     operator!=(const std::discard_block_engine<_RandomNumberEngine, __p,
       |     ^~~~~~~~
 /usr/include/c++/9/bits/random.h:1082:5: note:   template argument deduction/substitution failed:
-/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<ORB_SLAM3::Map>’ is not derived from ‘const std::discard_block_engine<_RandomNumberEngine, __p, __r>’
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<MORB_SLAM::Map>’ is not derived from ‘const std::discard_block_engine<_RandomNumberEngine, __p, __r>’
   339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
       |                                                 ^
 In file included from /usr/include/c++/9/random:49,
@@ -893,7 +893,7 @@ In file included from /usr/include/c++/9/random:49,
  1279 |     operator!=(const std::independent_bits_engine<_RandomNumberEngine, __w,
       |     ^~~~~~~~
 /usr/include/c++/9/bits/random.h:1279:5: note:   template argument deduction/substitution failed:
-/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<ORB_SLAM3::Map>’ is not derived from ‘const std::independent_bits_engine<_RandomNumberEngine, __w, _UIntType>’
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<MORB_SLAM::Map>’ is not derived from ‘const std::independent_bits_engine<_RandomNumberEngine, __w, _UIntType>’
   339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
       |                                                 ^
 In file included from /usr/include/c++/9/random:49,
@@ -912,7 +912,7 @@ In file included from /usr/include/c++/9/random:49,
  1532 |     operator!=(const std::shuffle_order_engine<_RandomNumberEngine,
       |     ^~~~~~~~
 /usr/include/c++/9/bits/random.h:1532:5: note:   template argument deduction/substitution failed:
-/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<ORB_SLAM3::Map>’ is not derived from ‘const std::shuffle_order_engine<_RandomNumberEngine, __k>’
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<MORB_SLAM::Map>’ is not derived from ‘const std::shuffle_order_engine<_RandomNumberEngine, __k>’
   339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
       |                                                 ^
 In file included from /usr/include/c++/9/random:49,
@@ -931,7 +931,7 @@ In file included from /usr/include/c++/9/random:49,
  1692 |     operator!=(const std::uniform_int_distribution<_IntType>& __d1,
       |     ^~~~~~~~
 /usr/include/c++/9/bits/random.h:1692:5: note:   template argument deduction/substitution failed:
-/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<ORB_SLAM3::Map>’ is not derived from ‘const std::uniform_int_distribution<_IntType>’
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<MORB_SLAM::Map>’ is not derived from ‘const std::uniform_int_distribution<_IntType>’
   339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
       |                                                 ^
 In file included from /usr/include/c++/9/random:49,
@@ -950,7 +950,7 @@ In file included from /usr/include/c++/9/random:49,
  1913 |     operator!=(const std::uniform_real_distribution<_IntType>& __d1,
       |     ^~~~~~~~
 /usr/include/c++/9/bits/random.h:1913:5: note:   template argument deduction/substitution failed:
-/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<ORB_SLAM3::Map>’ is not derived from ‘const std::uniform_real_distribution<_IntType>’
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<MORB_SLAM::Map>’ is not derived from ‘const std::uniform_real_distribution<_IntType>’
   339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
       |                                                 ^
 In file included from /usr/include/c++/9/random:49,
@@ -969,7 +969,7 @@ In file included from /usr/include/c++/9/random:49,
  2170 |     operator!=(const std::normal_distribution<_RealType>& __d1,
       |     ^~~~~~~~
 /usr/include/c++/9/bits/random.h:2170:5: note:   template argument deduction/substitution failed:
-/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<ORB_SLAM3::Map>’ is not derived from ‘const std::normal_distribution<_RealType>’
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<MORB_SLAM::Map>’ is not derived from ‘const std::normal_distribution<_RealType>’
   339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
       |                                                 ^
 In file included from /usr/include/c++/9/random:49,
@@ -988,7 +988,7 @@ In file included from /usr/include/c++/9/random:49,
  2381 |     operator!=(const std::lognormal_distribution<_RealType>& __d1,
       |     ^~~~~~~~
 /usr/include/c++/9/bits/random.h:2381:5: note:   template argument deduction/substitution failed:
-/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<ORB_SLAM3::Map>’ is not derived from ‘const std::lognormal_distribution<_RealType>’
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<MORB_SLAM::Map>’ is not derived from ‘const std::lognormal_distribution<_RealType>’
   339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
       |                                                 ^
 In file included from /usr/include/c++/9/random:49,
@@ -1007,7 +1007,7 @@ In file included from /usr/include/c++/9/random:49,
  2612 |      operator!=(const std::gamma_distribution<_RealType>& __d1,
       |      ^~~~~~~~
 /usr/include/c++/9/bits/random.h:2612:6: note:   template argument deduction/substitution failed:
-/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<ORB_SLAM3::Map>’ is not derived from ‘const std::gamma_distribution<_RealType>’
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<MORB_SLAM::Map>’ is not derived from ‘const std::gamma_distribution<_RealType>’
   339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
       |                                                 ^
 In file included from /usr/include/c++/9/random:49,
@@ -1026,7 +1026,7 @@ In file included from /usr/include/c++/9/random:49,
  2836 |     operator!=(const std::chi_squared_distribution<_RealType>& __d1,
       |     ^~~~~~~~
 /usr/include/c++/9/bits/random.h:2836:5: note:   template argument deduction/substitution failed:
-/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<ORB_SLAM3::Map>’ is not derived from ‘const std::chi_squared_distribution<_RealType>’
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<MORB_SLAM::Map>’ is not derived from ‘const std::chi_squared_distribution<_RealType>’
   339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
       |                                                 ^
 In file included from /usr/include/c++/9/random:49,
@@ -1045,7 +1045,7 @@ In file included from /usr/include/c++/9/random:49,
  3010 |     operator!=(const std::cauchy_distribution<_RealType>& __d1,
       |     ^~~~~~~~
 /usr/include/c++/9/bits/random.h:3010:5: note:   template argument deduction/substitution failed:
-/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<ORB_SLAM3::Map>’ is not derived from ‘const std::cauchy_distribution<_RealType>’
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<MORB_SLAM::Map>’ is not derived from ‘const std::cauchy_distribution<_RealType>’
   339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
       |                                                 ^
 In file included from /usr/include/c++/9/random:49,
@@ -1064,7 +1064,7 @@ In file included from /usr/include/c++/9/random:49,
  3274 |     operator!=(const std::fisher_f_distribution<_RealType>& __d1,
       |     ^~~~~~~~
 /usr/include/c++/9/bits/random.h:3274:5: note:   template argument deduction/substitution failed:
-/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<ORB_SLAM3::Map>’ is not derived from ‘const std::fisher_f_distribution<_RealType>’
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<MORB_SLAM::Map>’ is not derived from ‘const std::fisher_f_distribution<_RealType>’
   339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
       |                                                 ^
 In file included from /usr/include/c++/9/random:49,
@@ -1083,7 +1083,7 @@ In file included from /usr/include/c++/9/random:49,
  3496 |     operator!=(const std::student_t_distribution<_RealType>& __d1,
       |     ^~~~~~~~
 /usr/include/c++/9/bits/random.h:3496:5: note:   template argument deduction/substitution failed:
-/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<ORB_SLAM3::Map>’ is not derived from ‘const std::student_t_distribution<_RealType>’
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<MORB_SLAM::Map>’ is not derived from ‘const std::student_t_distribution<_RealType>’
   339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
       |                                                 ^
 In file included from /usr/include/c++/9/random:49,
@@ -1101,14 +1101,14 @@ In file included from /usr/include/c++/9/random:49,
 /usr/include/c++/9/bits/random.h:3684:3: note: candidate: ‘bool std::operator!=(const std::bernoulli_distribution&, const std::bernoulli_distribution&)’
  3684 |   operator!=(const std::bernoulli_distribution& __d1,
       |   ^~~~~~~~
-/usr/include/c++/9/bits/random.h:3684:49: note:   no known conversion for argument 1 from ‘std::shared_ptr<ORB_SLAM3::Map>’ to ‘const std::bernoulli_distribution&’
+/usr/include/c++/9/bits/random.h:3684:49: note:   no known conversion for argument 1 from ‘std::shared_ptr<MORB_SLAM::Map>’ to ‘const std::bernoulli_distribution&’
  3684 |   operator!=(const std::bernoulli_distribution& __d1,
       |              ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~^~~~
 /usr/include/c++/9/bits/random.h:3959:5: note: candidate: ‘template<class _IntType> bool std::operator!=(const std::binomial_distribution<_IntType>&, const std::binomial_distribution<_IntType>&)’
  3959 |     operator!=(const std::binomial_distribution<_IntType>& __d1,
       |     ^~~~~~~~
 /usr/include/c++/9/bits/random.h:3959:5: note:   template argument deduction/substitution failed:
-/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<ORB_SLAM3::Map>’ is not derived from ‘const std::binomial_distribution<_IntType>’
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<MORB_SLAM::Map>’ is not derived from ‘const std::binomial_distribution<_IntType>’
   339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
       |                                                 ^
 In file included from /usr/include/c++/9/random:49,
@@ -1127,7 +1127,7 @@ In file included from /usr/include/c++/9/random:49,
  4138 |     operator!=(const std::geometric_distribution<_IntType>& __d1,
       |     ^~~~~~~~
 /usr/include/c++/9/bits/random.h:4138:5: note:   template argument deduction/substitution failed:
-/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<ORB_SLAM3::Map>’ is not derived from ‘const std::geometric_distribution<_IntType>’
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<MORB_SLAM::Map>’ is not derived from ‘const std::geometric_distribution<_IntType>’
   339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
       |                                                 ^
 In file included from /usr/include/c++/9/random:49,
@@ -1146,7 +1146,7 @@ In file included from /usr/include/c++/9/random:49,
  4392 |     operator!=(const std::negative_binomial_distribution<_IntType>& __d1,
       |     ^~~~~~~~
 /usr/include/c++/9/bits/random.h:4392:5: note:   template argument deduction/substitution failed:
-/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<ORB_SLAM3::Map>’ is not derived from ‘const std::negative_binomial_distribution<_IntType>’
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<MORB_SLAM::Map>’ is not derived from ‘const std::negative_binomial_distribution<_IntType>’
   339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
       |                                                 ^
 In file included from /usr/include/c++/9/random:49,
@@ -1165,7 +1165,7 @@ In file included from /usr/include/c++/9/random:49,
  4618 |     operator!=(const std::poisson_distribution<_IntType>& __d1,
       |     ^~~~~~~~
 /usr/include/c++/9/bits/random.h:4618:5: note:   template argument deduction/substitution failed:
-/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<ORB_SLAM3::Map>’ is not derived from ‘const std::poisson_distribution<_IntType>’
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<MORB_SLAM::Map>’ is not derived from ‘const std::poisson_distribution<_IntType>’
   339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
       |                                                 ^
 In file included from /usr/include/c++/9/random:49,
@@ -1184,7 +1184,7 @@ In file included from /usr/include/c++/9/random:49,
  4809 |     operator!=(const std::exponential_distribution<_RealType>& __d1,
       |     ^~~~~~~~
 /usr/include/c++/9/bits/random.h:4809:5: note:   template argument deduction/substitution failed:
-/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<ORB_SLAM3::Map>’ is not derived from ‘const std::exponential_distribution<_RealType>’
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<MORB_SLAM::Map>’ is not derived from ‘const std::exponential_distribution<_RealType>’
   339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
       |                                                 ^
 In file included from /usr/include/c++/9/random:49,
@@ -1203,7 +1203,7 @@ In file included from /usr/include/c++/9/random:49,
  5019 |     operator!=(const std::weibull_distribution<_RealType>& __d1,
       |     ^~~~~~~~
 /usr/include/c++/9/bits/random.h:5019:5: note:   template argument deduction/substitution failed:
-/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<ORB_SLAM3::Map>’ is not derived from ‘const std::weibull_distribution<_RealType>’
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<MORB_SLAM::Map>’ is not derived from ‘const std::weibull_distribution<_RealType>’
   339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
       |                                                 ^
 In file included from /usr/include/c++/9/random:49,
@@ -1222,7 +1222,7 @@ In file included from /usr/include/c++/9/random:49,
  5229 |     operator!=(const std::extreme_value_distribution<_RealType>& __d1,
       |     ^~~~~~~~
 /usr/include/c++/9/bits/random.h:5229:5: note:   template argument deduction/substitution failed:
-/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<ORB_SLAM3::Map>’ is not derived from ‘const std::extreme_value_distribution<_RealType>’
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<MORB_SLAM::Map>’ is not derived from ‘const std::extreme_value_distribution<_RealType>’
   339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
       |                                                 ^
 In file included from /usr/include/c++/9/random:49,
@@ -1241,7 +1241,7 @@ In file included from /usr/include/c++/9/random:49,
  5494 |     operator!=(const std::discrete_distribution<_IntType>& __d1,
       |     ^~~~~~~~
 /usr/include/c++/9/bits/random.h:5494:5: note:   template argument deduction/substitution failed:
-/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<ORB_SLAM3::Map>’ is not derived from ‘const std::discrete_distribution<_IntType>’
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<MORB_SLAM::Map>’ is not derived from ‘const std::discrete_distribution<_IntType>’
   339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
       |                                                 ^
 In file included from /usr/include/c++/9/random:49,
@@ -1260,7 +1260,7 @@ In file included from /usr/include/c++/9/random:49,
  5765 |     operator!=(const std::piecewise_constant_distribution<_RealType>& __d1,
       |     ^~~~~~~~
 /usr/include/c++/9/bits/random.h:5765:5: note:   template argument deduction/substitution failed:
-/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<ORB_SLAM3::Map>’ is not derived from ‘const std::piecewise_constant_distribution<_RealType>’
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<MORB_SLAM::Map>’ is not derived from ‘const std::piecewise_constant_distribution<_RealType>’
   339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
       |                                                 ^
 In file included from /usr/include/c++/9/random:49,
@@ -1279,7 +1279,7 @@ In file included from /usr/include/c++/9/random:49,
  6038 |     operator!=(const std::piecewise_linear_distribution<_RealType>& __d1,
       |     ^~~~~~~~
 /usr/include/c++/9/bits/random.h:6038:5: note:   template argument deduction/substitution failed:
-/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<ORB_SLAM3::Map>’ is not derived from ‘const std::piecewise_linear_distribution<_RealType>’
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<MORB_SLAM::Map>’ is not derived from ‘const std::piecewise_linear_distribution<_RealType>’
   339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
       |                                                 ^
 In file included from /usr/include/c++/9/set:61,
@@ -1296,7 +1296,7 @@ In file included from /usr/include/c++/9/set:61,
  1003 |     operator!=(const set<_Key, _Compare, _Alloc>& __x,
       |     ^~~~~~~~
 /usr/include/c++/9/bits/stl_set.h:1003:5: note:   template argument deduction/substitution failed:
-/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<ORB_SLAM3::Map>’ is not derived from ‘const std::set<_Key, _Compare, _Allocator>’
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<MORB_SLAM::Map>’ is not derived from ‘const std::set<_Key, _Compare, _Allocator>’
   339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
       |                                                 ^
 In file included from /usr/include/c++/9/set:62,
@@ -1313,7 +1313,7 @@ In file included from /usr/include/c++/9/set:62,
   988 |     operator!=(const multiset<_Key, _Compare, _Alloc>& __x,
       |     ^~~~~~~~
 /usr/include/c++/9/bits/stl_multiset.h:988:5: note:   template argument deduction/substitution failed:
-/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<ORB_SLAM3::Map>’ is not derived from ‘const std::multiset<_Key, _Compare, _Allocator>’
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<MORB_SLAM::Map>’ is not derived from ‘const std::multiset<_Key, _Compare, _Allocator>’
   339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
       |                                                 ^
 In file included from /usr/include/c++/9/list:63,
@@ -1330,7 +1330,7 @@ In file included from /usr/include/c++/9/list:63,
  2032 |     operator!=(const list<_Tp, _Alloc>& __x, const list<_Tp, _Alloc>& __y)
       |     ^~~~~~~~
 /usr/include/c++/9/bits/stl_list.h:2032:5: note:   template argument deduction/substitution failed:
-/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<ORB_SLAM3::Map>’ is not derived from ‘const std::__cxx11::list<_Tp, _Alloc>’
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<MORB_SLAM::Map>’ is not derived from ‘const std::__cxx11::list<_Tp, _Alloc>’
   339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
       |                                                 ^
 In file included from /usr/include/c++/9/deque:67,
@@ -1347,7 +1347,7 @@ In file included from /usr/include/c++/9/deque:67,
   299 |     operator!=(const _Deque_iterator<_Tp, _Ref, _Ptr>& __x,
       |     ^~~~~~~~
 /usr/include/c++/9/bits/stl_deque.h:299:5: note:   template argument deduction/substitution failed:
-/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<ORB_SLAM3::Map>’ is not derived from ‘const std::_Deque_iterator<_Tp, _Ref, _Ptr>’
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<MORB_SLAM::Map>’ is not derived from ‘const std::_Deque_iterator<_Tp, _Ref, _Ptr>’
   339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
       |                                                 ^
 In file included from /usr/include/c++/9/deque:67,
@@ -1364,7 +1364,7 @@ In file included from /usr/include/c++/9/deque:67,
   306 |     operator!=(const _Deque_iterator<_Tp, _RefL, _PtrL>& __x,
       |     ^~~~~~~~
 /usr/include/c++/9/bits/stl_deque.h:306:5: note:   template argument deduction/substitution failed:
-/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<ORB_SLAM3::Map>’ is not derived from ‘const std::_Deque_iterator<_Tp, _Ref, _Ptr>’
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<MORB_SLAM::Map>’ is not derived from ‘const std::_Deque_iterator<_Tp, _Ref, _Ptr>’
   339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
       |                                                 ^
 In file included from /usr/include/c++/9/deque:67,
@@ -1381,7 +1381,7 @@ In file included from /usr/include/c++/9/deque:67,
  2338 |     operator!=(const deque<_Tp, _Alloc>& __x,
       |     ^~~~~~~~
 /usr/include/c++/9/bits/stl_deque.h:2338:5: note:   template argument deduction/substitution failed:
-/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<ORB_SLAM3::Map>’ is not derived from ‘const std::deque<_Tp, _Alloc>’
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<MORB_SLAM::Map>’ is not derived from ‘const std::deque<_Tp, _Alloc>’
   339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
       |                                                 ^
 In file included from /usr/include/c++/9/stack:61,
@@ -1397,7 +1397,7 @@ In file included from /usr/include/c++/9/stack:61,
   337 |     operator!=(const stack<_Tp, _Seq>& __x, const stack<_Tp, _Seq>& __y)
       |     ^~~~~~~~
 /usr/include/c++/9/bits/stl_stack.h:337:5: note:   template argument deduction/substitution failed:
-/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<ORB_SLAM3::Map>’ is not derived from ‘const std::stack<_Tp, _Seq>’
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<MORB_SLAM::Map>’ is not derived from ‘const std::stack<_Tp, _Seq>’
   339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
       |                                                 ^
 In file included from /usr/include/c++/9/queue:64,
@@ -1415,7 +1415,7 @@ In file included from /usr/include/c++/9/queue:64,
   362 |     operator!=(const queue<_Tp, _Seq>& __x, const queue<_Tp, _Seq>& __y)
       |     ^~~~~~~~
 /usr/include/c++/9/bits/stl_queue.h:362:5: note:   template argument deduction/substitution failed:
-/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<ORB_SLAM3::Map>’ is not derived from ‘const std::queue<_Tp, _Seq>’
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<MORB_SLAM::Map>’ is not derived from ‘const std::queue<_Tp, _Seq>’
   339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
       |                                                 ^
 In file included from /orin_ssd/MORB_SLAM/include/Camera.hpp:2,
@@ -1427,7 +1427,7 @@ In file included from /orin_ssd/MORB_SLAM/include/Camera.hpp:2,
 /usr/include/c++/9/thread:286:3: note: candidate: ‘bool std::operator!=(std::thread::id, std::thread::id)’
   286 |   operator!=(thread::id __x, thread::id __y) noexcept
       |   ^~~~~~~~
-/usr/include/c++/9/thread:286:25: note:   no known conversion for argument 1 from ‘std::shared_ptr<ORB_SLAM3::Map>’ to ‘std::thread::id’
+/usr/include/c++/9/thread:286:25: note:   no known conversion for argument 1 from ‘std::shared_ptr<MORB_SLAM::Map>’ to ‘std::thread::id’
   286 |   operator!=(thread::id __x, thread::id __y) noexcept
       |              ~~~~~~~~~~~^~~
 make[2]: *** [CMakeFiles/MORB_SLAM.dir/build.make:244: CMakeFiles/MORB_SLAM.dir/src/Map.cc.o] Error 1
diff --git a/src/Atlas.cc b/src/Atlas.cc
index 4313a60a..49131adb 100644
--- a/src/Atlas.cc
+++ b/src/Atlas.cc
@@ -25,7 +25,7 @@
 #include "KannalaBrandt8.h"
 #include "Pinhole.h"
 
-namespace ORB_SLAM3 {
+namespace MORB_SLAM {
 
 Atlas::Atlas() { mpCurrentMap = nullptr; }
 
@@ -325,4 +325,4 @@ map<long unsigned int, KeyFrame*> Atlas::GetAtlasKeyframes() {
   return mpIdKFs;
 }
 
-}  // namespace ORB_SLAM3
+}  // namespace MORB_SLAM
diff --git a/src/Camera.cpp b/src/Camera.cpp
index 508a4af8..57a3ff2e 100644
--- a/src/Camera.cpp
+++ b/src/Camera.cpp
@@ -1,6 +1,6 @@
 #include "Camera.hpp"
 
-namespace ORB_SLAM3{
+namespace MORB_SLAM{
 
 Camera::Camera(CameraType::eSensor type, const std::string &name): ljobs{}, rjobs{}, name{name}, type{type},
     shouldStop{false}, lthread{&Camera::threadExec, this, &ljobs}, rthread{&Camera::threadExec, this, &rjobs} {
@@ -66,4 +66,4 @@ ManagedFuture<bool> Camera::queueRight(const std::function<void(void)> &func){ r
 CameraType::eSensor Camera::getType() const { return type; }
 const std::string &Camera::getName() const { return name; }
 
-} // namespace ORB_SLAM3
\ No newline at end of file
+} // namespace MORB_SLAM
\ No newline at end of file
diff --git a/src/CameraModels/KannalaBrandt8.cpp b/src/CameraModels/KannalaBrandt8.cpp
index 4952082e..54c40930 100644
--- a/src/CameraModels/KannalaBrandt8.cpp
+++ b/src/CameraModels/KannalaBrandt8.cpp
@@ -23,9 +23,9 @@
 
 #include <boost/serialization/export.hpp>
 
-// BOOST_CLASS_EXPORT_IMPLEMENT(ORB_SLAM3::KannalaBrandt8)
+// BOOST_CLASS_EXPORT_IMPLEMENT(MORB_SLAM::KannalaBrandt8)
 
-namespace ORB_SLAM3 {
+namespace MORB_SLAM {
 // BOOST_CLASS_EXPORT_GUID(KannalaBrandt8, "KannalaBrandt8")
 
 cv::Point2f KannalaBrandt8::project(const cv::Point3f &p3D) const {
@@ -446,4 +446,4 @@ bool KannalaBrandt8::IsEqual(const std::shared_ptr<GeometricCamera> &pCam) {
   return is_same_camera;
 }
 
-}  // namespace ORB_SLAM3
+}  // namespace MORB_SLAM
diff --git a/src/CameraModels/Pinhole.cpp b/src/CameraModels/Pinhole.cpp
index e83e1948..4c554a26 100644
--- a/src/CameraModels/Pinhole.cpp
+++ b/src/CameraModels/Pinhole.cpp
@@ -23,9 +23,9 @@
 
 #include <boost/serialization/export.hpp>
 
-// BOOST_CLASS_EXPORT_IMPLEMENT(ORB_SLAM3::Pinhole)
+// BOOST_CLASS_EXPORT_IMPLEMENT(MORB_SLAM::Pinhole)
 
-namespace ORB_SLAM3 {
+namespace MORB_SLAM {
 // BOOST_CLASS_EXPORT_GUID(Pinhole, "Pinhole")
 
 long unsigned int GeometricCamera::nNextId = 0;
@@ -170,4 +170,4 @@ bool Pinhole::IsEqual(const std::shared_ptr<GeometricCamera> &pCam) {
   }
   return is_same_camera;
 }
-}  // namespace ORB_SLAM3
+}  // namespace MORB_SLAM
diff --git a/src/Converter.cc b/src/Converter.cc
index c842918d..5bf2d2ab 100644
--- a/src/Converter.cc
+++ b/src/Converter.cc
@@ -21,7 +21,7 @@
 
 #include "Converter.h"
 
-namespace ORB_SLAM3 {
+namespace MORB_SLAM {
 
 std::vector<cv::Mat> Converter::toDescriptorVector(const cv::Mat &Descriptors) {
   std::vector<cv::Mat> vDesc;
@@ -282,4 +282,4 @@ Sophus::Sim3f Converter::toSophus(const g2o::Sim3 &S) {
       S.translation().cast<float>());
 }
 
-}  // namespace ORB_SLAM3
+}  // namespace MORB_SLAM
diff --git a/src/Frame.cc b/src/Frame.cc
index 9c61ad9e..97a89973 100644
--- a/src/Frame.cc
+++ b/src/Frame.cc
@@ -34,7 +34,7 @@
 #include "ORBextractor.h"
 #include "ORBmatcher.h"
 
-namespace ORB_SLAM3 {
+namespace MORB_SLAM {
 
 long unsigned int Frame::nNextId = 0;
 bool Frame::mbInitialComputations = true;
@@ -1099,6 +1099,7 @@ bool Frame::imuIsPreintegrated() {
 
 void Frame::setIntegrated() {
   unique_lock<std::mutex> lock(*mpMutexImu);
+  std::cout << "-----------------------------------------------------------" << std::endl;
   mbImuPreintegrated = true;
 }
 
@@ -1364,4 +1365,4 @@ Eigen::Vector3f Frame::UnprojectStereoFishEye(const int &i) {
   return mRwc * mvStereo3Dpoints[i] + mOw;
 }
 
-}  // namespace ORB_SLAM3
+}  // namespace MORB_SLAM
diff --git a/src/FrameDrawer.cc b/src/FrameDrawer.cc
index 9a75cbdd..4e098b41 100644
--- a/src/FrameDrawer.cc
+++ b/src/FrameDrawer.cc
@@ -28,7 +28,7 @@
 #include "Atlas.h"
 #include "Tracking.h"
 
-namespace ORB_SLAM3 {
+namespace MORB_SLAM {
 
 FrameDrawer::FrameDrawer(const Atlas_ptr &pAtlas) : both(false), mpAtlas(pAtlas) {
   mState = Tracker::SYSTEM_NOT_READY;
@@ -336,4 +336,4 @@ void FrameDrawer::Update(const Tracking_ptr &pTracker) {
   mState = static_cast<int>(pTracker->mLastProcessedState);
 }
 
-}  // namespace ORB_SLAM3
+}  // namespace MORB_SLAM
diff --git a/src/G2oTypes.cc b/src/G2oTypes.cc
index ba1d26ef..ab47d0eb 100644
--- a/src/G2oTypes.cc
+++ b/src/G2oTypes.cc
@@ -23,7 +23,7 @@
 
 #include "Converter.h"
 #include "ImuTypes.h"
-namespace ORB_SLAM3 {
+namespace MORB_SLAM {
 
 ImuCamPose::ImuCamPose(KeyFrame* pKF) : its(0) {
   // Load IMU pose
@@ -853,4 +853,4 @@ Eigen::Matrix3d Skew(const Eigen::Vector3d& w) {
   return W;
 }
 
-}  // namespace ORB_SLAM3
+}  // namespace MORB_SLAM
diff --git a/src/GeometricTools.cc b/src/GeometricTools.cc
index a97002c3..97b3d796 100644
--- a/src/GeometricTools.cc
+++ b/src/GeometricTools.cc
@@ -23,7 +23,7 @@
 
 #include "KeyFrame.h"
 
-namespace ORB_SLAM3 {
+namespace MORB_SLAM {
 
 Eigen::Matrix3f GeometricTools::ComputeF12(KeyFrame *&pKF1, KeyFrame *&pKF2) {
   Sophus::SE3<float> Tc1w = pKF1->GetPose();
@@ -71,4 +71,4 @@ bool GeometricTools::Triangulate(Eigen::Vector3f &x_c1, Eigen::Vector3f &x_c2,
   return true;
 }
 
-}  // namespace ORB_SLAM3
+}  // namespace MORB_SLAM
diff --git a/src/ImuTypes.cc b/src/ImuTypes.cc
index b9b3eb12..fd4ff0be 100644
--- a/src/ImuTypes.cc
+++ b/src/ImuTypes.cc
@@ -26,7 +26,7 @@
 #include "Converter.h"
 #include "GeometricTools.h"
 
-namespace ORB_SLAM3 {
+namespace MORB_SLAM {
 
 namespace IMU {
 
@@ -406,4 +406,4 @@ Calib::Calib(const Calib &calib) {
 
 }  // namespace IMU
 
-}  // namespace ORB_SLAM3
+}  // namespace MORB_SLAM
diff --git a/src/KeyFrame.cc b/src/KeyFrame.cc
index 18b42df0..a373d048 100644
--- a/src/KeyFrame.cc
+++ b/src/KeyFrame.cc
@@ -26,7 +26,7 @@
 #include "Converter.h"
 #include "ImuTypes.h"
 
-namespace ORB_SLAM3 {
+namespace MORB_SLAM {
 
 long unsigned int KeyFrame::nNextId = 0;
 
@@ -1157,4 +1157,4 @@ void KeyFrame::SetKeyFrameDatabase(KeyFrameDatabase *pKFDB) {
   mpKeyFrameDB = pKFDB;
 }
 
-}  // namespace ORB_SLAM3
+}  // namespace MORB_SLAM
diff --git a/src/KeyFrameDatabase.cc b/src/KeyFrameDatabase.cc
index 9ed18483..6b935721 100644
--- a/src/KeyFrameDatabase.cc
+++ b/src/KeyFrameDatabase.cc
@@ -28,7 +28,7 @@
 
 using namespace std;
 
-namespace ORB_SLAM3 {
+namespace MORB_SLAM {
 
 KeyFrameDatabase::KeyFrameDatabase(const ORBVocabulary& voc) : mpVoc(&voc) {
   mvInvertedFile.resize(voc.size());
@@ -822,4 +822,4 @@ void KeyFrameDatabase::SetORBVocabulary(ORBVocabulary* pORBVoc) {
   mvInvertedFile.resize(mpVoc->size());
 }
 
-}  // namespace ORB_SLAM3
+}  // namespace MORB_SLAM
diff --git a/src/LocalMapping.cc b/src/LocalMapping.cc
index 29b20fd4..8eb741bc 100644
--- a/src/LocalMapping.cc
+++ b/src/LocalMapping.cc
@@ -31,7 +31,7 @@
 #include "Optimizer.h"
 
 #include <math.h> 
-namespace ORB_SLAM3 {
+namespace MORB_SLAM {
 
 LocalMapping::LocalMapping(System* pSys, const Atlas_ptr &pAtlas, const float bMonocular,
                            bool bInertial, const string& _strSeqName)
@@ -1461,4 +1461,4 @@ double LocalMapping::GetCurrKFTime() {
 
 KeyFrame* LocalMapping::GetCurrKF() { return mpCurrentKeyFrame; }
 
-}  // namespace ORB_SLAM3
+}  // namespace MORB_SLAM
diff --git a/src/LoopClosing.cc b/src/LoopClosing.cc
index 6cebc6ee..0aa240a5 100644
--- a/src/LoopClosing.cc
+++ b/src/LoopClosing.cc
@@ -31,7 +31,7 @@
 #include "Optimizer.h"
 #include "Sim3Solver.h"
 
-namespace ORB_SLAM3 {
+namespace MORB_SLAM {
 
 LoopClosing::LoopClosing(const Atlas_ptr &pAtlas, KeyFrameDatabase* pDB,
                          ORBVocabulary* pVoc, const bool bFixScale,
@@ -2555,4 +2555,4 @@ bool LoopClosing::isFinished() {
   return mbFinished;
 }
 
-}  // namespace ORB_SLAM3
+}  // namespace MORB_SLAM
diff --git a/src/MLPnPsolver.cpp b/src/MLPnPsolver.cpp
index 0e31d5f8..ed8291d4 100644
--- a/src/MLPnPsolver.cpp
+++ b/src/MLPnPsolver.cpp
@@ -51,7 +51,7 @@
 #include <Eigen/Sparse>
 
 
-namespace ORB_SLAM3 {
+namespace MORB_SLAM {
     MLPnPsolver::MLPnPsolver(const Frame &F, const vector<MapPoint *> &vpMapPointMatches):
             mnInliersi(0), mnIterations(0), mnBestInliers(0), N(0), mpCamera(F.mpCamera){
         mvpMapPointMatches = vpMapPointMatches;
diff --git a/src/Map.cc b/src/Map.cc
index 10f32927..ef6b562e 100644
--- a/src/Map.cc
+++ b/src/Map.cc
@@ -23,7 +23,7 @@
 
 #include <mutex>
 
-namespace ORB_SLAM3 {
+namespace MORB_SLAM {
 
 long unsigned int Map::nNextId = 0;
 
@@ -453,4 +453,4 @@ void Map::PostLoad(
   mvpBackupMapPoints.clear();
 }
 
-}  // namespace ORB_SLAM3
+}  // namespace MORB_SLAM
diff --git a/src/MapDrawer.cc b/src/MapDrawer.cc
index 1592bee6..8d47ef15 100644
--- a/src/MapDrawer.cc
+++ b/src/MapDrawer.cc
@@ -30,7 +30,7 @@
 #include "KeyFrame.h"
 #include "MapPoint.h"
 
-namespace ORB_SLAM3 {
+namespace MORB_SLAM {
 
 MapDrawer::MapDrawer(const Atlas_ptr &pAtlas, const std::string &strSettingPath): mpAtlas(pAtlas){
     cv::FileStorage fSettings(strSettingPath, cv::FileStorage::READ);
@@ -419,4 +419,4 @@ void MapDrawer::GetCurrentOpenGLCameraMatrix(pangolin::OpenGlMatrix &M,
   MOw.m[13] = Twc(1, 3);
   MOw.m[14] = Twc(2, 3);
 }
-}  // namespace ORB_SLAM3
+}  // namespace MORB_SLAM
diff --git a/src/MapPoint.cc b/src/MapPoint.cc
index 720436f1..53515c78 100644
--- a/src/MapPoint.cc
+++ b/src/MapPoint.cc
@@ -25,7 +25,7 @@
 
 #include "ORBmatcher.h"
 
-namespace ORB_SLAM3 {
+namespace MORB_SLAM {
 
 long unsigned int MapPoint::nNextId = 0;
 mutex MapPoint::mGlobalMutex;
@@ -654,4 +654,4 @@ void MapPoint::PostLoad(map<long unsigned int, KeyFrame*>& mpKFid,
   mBackupObservationsId2.clear();
 }
 
-}  // namespace ORB_SLAM3
+}  // namespace MORB_SLAM
diff --git a/src/ORBextractor.cc b/src/ORBextractor.cc
index 95716288..71049039 100644
--- a/src/ORBextractor.cc
+++ b/src/ORBextractor.cc
@@ -66,7 +66,7 @@
 using namespace cv;
 using namespace std;
 
-namespace ORB_SLAM3 {
+namespace MORB_SLAM {
 
 const int PATCH_SIZE = 31;
 const int HALF_PATCH_SIZE = 15;
@@ -958,4 +958,4 @@ void ORBextractor::ComputePyramid(cv::Mat image) {
   }
 }
 
-}  // namespace ORB_SLAM3
+}  // namespace MORB_SLAM
diff --git a/src/ORBmatcher.cc b/src/ORBmatcher.cc
index 7fd764f7..a848650e 100644
--- a/src/ORBmatcher.cc
+++ b/src/ORBmatcher.cc
@@ -30,7 +30,7 @@
 
 using namespace std;
 
-namespace ORB_SLAM3 {
+namespace MORB_SLAM {
 
 const int ORBmatcher::TH_HIGH = 100;
 const int ORBmatcher::TH_LOW = 50;
@@ -1893,4 +1893,4 @@ int ORBmatcher::DescriptorDistance(const cv::Mat &a, const cv::Mat &b) {
   return dist;
 }
 
-}  // namespace ORB_SLAM3
+}  // namespace MORB_SLAM
diff --git a/src/OptimizableTypes.cpp b/src/OptimizableTypes.cpp
index 40160beb..000da75a 100644
--- a/src/OptimizableTypes.cpp
+++ b/src/OptimizableTypes.cpp
@@ -21,7 +21,7 @@
 
 #include "OptimizableTypes.h"
 
-namespace ORB_SLAM3 {
+namespace MORB_SLAM {
 bool EdgeSE3ProjectXYZOnlyPose::read(std::istream& is) {
   for (int i = 0; i < 2; i++) {
     is >> _measurement[i];
@@ -310,4 +310,4 @@ bool EdgeInverseSim3ProjectXYZ::write(std::ostream& os) const {
   return os.good();
 }
 
-}  // namespace ORB_SLAM3
+}  // namespace MORB_SLAM
diff --git a/src/Optimizer.cc b/src/Optimizer.cc
index 1cf32a4b..b86a7321 100644
--- a/src/Optimizer.cc
+++ b/src/Optimizer.cc
@@ -39,7 +39,7 @@
 #include "g2o/solvers/linear_solver_eigen.h"
 #include "g2o/types/types_six_dof_expmap.h"
 
-namespace ORB_SLAM3 {
+namespace MORB_SLAM {
 bool sortByVal(const pair<MapPoint*, int>& a, const pair<MapPoint*, int>& b) {
   return (a.second < b.second);
 }
@@ -81,10 +81,10 @@ void Optimizer::BundleAdjustment(const vector<KeyFrame*>& vpKFs,
 
   const int nExpectedSize = (vpKFs.size()) * vpMP.size();
 
-  vector<ORB_SLAM3::EdgeSE3ProjectXYZ*> vpEdgesMono;
+  vector<MORB_SLAM::EdgeSE3ProjectXYZ*> vpEdgesMono;
   vpEdgesMono.reserve(nExpectedSize);
 
-  vector<ORB_SLAM3::EdgeSE3ProjectXYZToBody*> vpEdgesBody;
+  vector<MORB_SLAM::EdgeSE3ProjectXYZToBody*> vpEdgesBody;
   vpEdgesBody.reserve(nExpectedSize);
 
   vector<KeyFrame*> vpEdgeKFMono;
@@ -158,7 +158,7 @@ void Optimizer::BundleAdjustment(const vector<KeyFrame*>& vpKFs,
         Eigen::Matrix<double, 2, 1> obs;
         obs << kpUn.pt.x, kpUn.pt.y;
 
-        ORB_SLAM3::EdgeSE3ProjectXYZ* e = new ORB_SLAM3::EdgeSE3ProjectXYZ();
+        MORB_SLAM::EdgeSE3ProjectXYZ* e = new MORB_SLAM::EdgeSE3ProjectXYZ();
 
         e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(
                             optimizer.vertex(id)));
@@ -230,8 +230,8 @@ void Optimizer::BundleAdjustment(const vector<KeyFrame*>& vpKFs,
           cv::KeyPoint kp = pKF->mvKeysRight[rightIndex];
           obs << kp.pt.x, kp.pt.y;
 
-          ORB_SLAM3::EdgeSE3ProjectXYZToBody* e =
-              new ORB_SLAM3::EdgeSE3ProjectXYZToBody();
+          MORB_SLAM::EdgeSE3ProjectXYZToBody* e =
+              new MORB_SLAM::EdgeSE3ProjectXYZToBody();
 
           e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(
                               optimizer.vertex(id)));
@@ -300,7 +300,7 @@ void Optimizer::BundleAdjustment(const vector<KeyFrame*>& vpKFs,
         vector<MapPoint*> vpMonoMPsOpt, vpStereoMPsOpt;
 
         for (size_t i2 = 0, iend = vpEdgesMono.size(); i2 < iend; i2++) {
-          ORB_SLAM3::EdgeSE3ProjectXYZ* e = vpEdgesMono[i2];
+          MORB_SLAM::EdgeSE3ProjectXYZ* e = vpEdgesMono[i2];
           MapPoint* pMP = vpMapPointEdgeMono[i2];
           KeyFrame* pKFedge = vpEdgeKFMono[i2];
 
@@ -786,8 +786,8 @@ int Optimizer::PoseOptimization(Frame* pFrame) {
   // Set MapPoint vertices
   const int N = pFrame->N;
 
-  vector<ORB_SLAM3::EdgeSE3ProjectXYZOnlyPose*> vpEdgesMono;
-  vector<ORB_SLAM3::EdgeSE3ProjectXYZOnlyPoseToBody*> vpEdgesMono_FHR;
+  vector<MORB_SLAM::EdgeSE3ProjectXYZOnlyPose*> vpEdgesMono;
+  vector<MORB_SLAM::EdgeSE3ProjectXYZOnlyPoseToBody*> vpEdgesMono_FHR;
   vector<size_t> vnIndexEdgeMono, vnIndexEdgeRight;
   vpEdgesMono.reserve(N);
   vpEdgesMono_FHR.reserve(N);
@@ -819,8 +819,8 @@ int Optimizer::PoseOptimization(Frame* pFrame) {
             const cv::KeyPoint& kpUn = pFrame->mvKeysUn[i];
             obs << kpUn.pt.x, kpUn.pt.y;
 
-            ORB_SLAM3::EdgeSE3ProjectXYZOnlyPose* e =
-                new ORB_SLAM3::EdgeSE3ProjectXYZOnlyPose();
+            MORB_SLAM::EdgeSE3ProjectXYZOnlyPose* e =
+                new MORB_SLAM::EdgeSE3ProjectXYZOnlyPose();
 
             e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(
                                 optimizer.vertex(0)));
@@ -890,8 +890,8 @@ int Optimizer::PoseOptimization(Frame* pFrame) {
             Eigen::Matrix<double, 2, 1> obs;
             obs << kpUn.pt.x, kpUn.pt.y;
 
-            ORB_SLAM3::EdgeSE3ProjectXYZOnlyPose* e =
-                new ORB_SLAM3::EdgeSE3ProjectXYZOnlyPose();
+            MORB_SLAM::EdgeSE3ProjectXYZOnlyPose* e =
+                new MORB_SLAM::EdgeSE3ProjectXYZOnlyPose();
 
             e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(
                                 optimizer.vertex(0)));
@@ -918,8 +918,8 @@ int Optimizer::PoseOptimization(Frame* pFrame) {
 
             pFrame->mvbOutlier[i] = false;
 
-            ORB_SLAM3::EdgeSE3ProjectXYZOnlyPoseToBody* e =
-                new ORB_SLAM3::EdgeSE3ProjectXYZOnlyPoseToBody();
+            MORB_SLAM::EdgeSE3ProjectXYZOnlyPoseToBody* e =
+                new MORB_SLAM::EdgeSE3ProjectXYZOnlyPoseToBody();
 
             e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(
                                 optimizer.vertex(0)));
@@ -968,7 +968,7 @@ int Optimizer::PoseOptimization(Frame* pFrame) {
 
     nBad = 0;
     for (size_t i = 0, iend = vpEdgesMono.size(); i < iend; i++) {
-      ORB_SLAM3::EdgeSE3ProjectXYZOnlyPose* e = vpEdgesMono[i];
+      MORB_SLAM::EdgeSE3ProjectXYZOnlyPose* e = vpEdgesMono[i];
 
       const size_t idx = vnIndexEdgeMono[i];
 
@@ -991,7 +991,7 @@ int Optimizer::PoseOptimization(Frame* pFrame) {
     }
 
     for (size_t i = 0, iend = vpEdgesMono_FHR.size(); i < iend; i++) {
-      ORB_SLAM3::EdgeSE3ProjectXYZOnlyPoseToBody* e = vpEdgesMono_FHR[i];
+      MORB_SLAM::EdgeSE3ProjectXYZOnlyPoseToBody* e = vpEdgesMono_FHR[i];
 
       const size_t idx = vnIndexEdgeRight[i];
 
@@ -1186,10 +1186,10 @@ void Optimizer::LocalBundleAdjustment(KeyFrame* pKF, bool* pbStopFlag,
   const int nExpectedSize =
       (lLocalKeyFrames.size() + lFixedCameras.size()) * lLocalMapPoints.size();
 
-  vector<ORB_SLAM3::EdgeSE3ProjectXYZ*> vpEdgesMono;
+  vector<MORB_SLAM::EdgeSE3ProjectXYZ*> vpEdgesMono;
   vpEdgesMono.reserve(nExpectedSize);
 
-  vector<ORB_SLAM3::EdgeSE3ProjectXYZToBody*> vpEdgesBody;
+  vector<MORB_SLAM::EdgeSE3ProjectXYZToBody*> vpEdgesBody;
   vpEdgesBody.reserve(nExpectedSize);
 
   vector<KeyFrame*> vpEdgeKFMono;
@@ -1250,7 +1250,7 @@ void Optimizer::LocalBundleAdjustment(KeyFrame* pKF, bool* pbStopFlag,
           Eigen::Matrix<double, 2, 1> obs;
           obs << kpUn.pt.x, kpUn.pt.y;
 
-          ORB_SLAM3::EdgeSE3ProjectXYZ* e = new ORB_SLAM3::EdgeSE3ProjectXYZ();
+          MORB_SLAM::EdgeSE3ProjectXYZ* e = new MORB_SLAM::EdgeSE3ProjectXYZ();
 
           e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(
                               optimizer.vertex(id)));
@@ -1319,8 +1319,8 @@ void Optimizer::LocalBundleAdjustment(KeyFrame* pKF, bool* pbStopFlag,
             cv::KeyPoint kp = pKFi->mvKeysRight[rightIndex];
             obs << kp.pt.x, kp.pt.y;
 
-            ORB_SLAM3::EdgeSE3ProjectXYZToBody* e =
-                new ORB_SLAM3::EdgeSE3ProjectXYZToBody();
+            MORB_SLAM::EdgeSE3ProjectXYZToBody* e =
+                new MORB_SLAM::EdgeSE3ProjectXYZToBody();
 
             e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(
                                 optimizer.vertex(id)));
@@ -1365,7 +1365,7 @@ void Optimizer::LocalBundleAdjustment(KeyFrame* pKF, bool* pbStopFlag,
 
   // Check inlier observations
   for (size_t i = 0, iend = vpEdgesMono.size(); i < iend; i++) {
-    ORB_SLAM3::EdgeSE3ProjectXYZ* e = vpEdgesMono[i];
+    MORB_SLAM::EdgeSE3ProjectXYZ* e = vpEdgesMono[i];
     MapPoint* pMP = vpMapPointEdgeMono[i];
 
     if (pMP->isBad()) continue;
@@ -1377,7 +1377,7 @@ void Optimizer::LocalBundleAdjustment(KeyFrame* pKF, bool* pbStopFlag,
   }
 
   for (size_t i = 0, iend = vpEdgesBody.size(); i < iend; i++) {
-    ORB_SLAM3::EdgeSE3ProjectXYZToBody* e = vpEdgesBody[i];
+    MORB_SLAM::EdgeSE3ProjectXYZToBody* e = vpEdgesBody[i];
     MapPoint* pMP = vpMapPointEdgeBody[i];
 
     if (pMP->isBad()) continue;
@@ -2086,7 +2086,7 @@ int Optimizer::OptimizeSim3(KeyFrame* pKF1, KeyFrame* pKF2,
   const Eigen::Vector3f t2w = pKF2->GetTranslation();
 
   // Set Sim3 vertex
-  ORB_SLAM3::VertexSim3Expmap* vSim3 = new ORB_SLAM3::VertexSim3Expmap();
+  MORB_SLAM::VertexSim3Expmap* vSim3 = new MORB_SLAM::VertexSim3Expmap();
   vSim3->_fix_scale = bFixScale;
   vSim3->setEstimate(g2oS12);
   vSim3->setId(0);
@@ -2098,8 +2098,8 @@ int Optimizer::OptimizeSim3(KeyFrame* pKF1, KeyFrame* pKF2,
   // Set MapPoint vertices
   const int N = vpMatches1.size();
   const vector<MapPoint*> vpMapPoints1 = pKF1->GetMapPointMatches();
-  vector<ORB_SLAM3::EdgeSim3ProjectXYZ*> vpEdges12;
-  vector<ORB_SLAM3::EdgeInverseSim3ProjectXYZ*> vpEdges21;
+  vector<MORB_SLAM::EdgeSim3ProjectXYZ*> vpEdges12;
+  vector<MORB_SLAM::EdgeInverseSim3ProjectXYZ*> vpEdges21;
   vector<size_t> vnIndexEdge;
   vector<bool> vbIsInKF2;
 
@@ -2191,7 +2191,7 @@ int Optimizer::OptimizeSim3(KeyFrame* pKF1, KeyFrame* pKF2,
     const cv::KeyPoint& kpUn1 = pKF1->mvKeysUn[i];
     obs1 << kpUn1.pt.x, kpUn1.pt.y;
 
-    ORB_SLAM3::EdgeSim3ProjectXYZ* e12 = new ORB_SLAM3::EdgeSim3ProjectXYZ();
+    MORB_SLAM::EdgeSim3ProjectXYZ* e12 = new MORB_SLAM::EdgeSim3ProjectXYZ();
 
     e12->setVertex(
         0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(id2)));
@@ -2228,8 +2228,8 @@ int Optimizer::OptimizeSim3(KeyFrame* pKF1, KeyFrame* pKF2,
       nOutKF2++;
     }
 
-    ORB_SLAM3::EdgeInverseSim3ProjectXYZ* e21 =
-        new ORB_SLAM3::EdgeInverseSim3ProjectXYZ();
+    MORB_SLAM::EdgeInverseSim3ProjectXYZ* e21 =
+        new MORB_SLAM::EdgeInverseSim3ProjectXYZ();
 
     e21->setVertex(
         0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(id1)));
@@ -2259,8 +2259,8 @@ int Optimizer::OptimizeSim3(KeyFrame* pKF1, KeyFrame* pKF2,
   int nBad = 0;
   int nBadOutKF2 = 0;
   for (size_t i = 0; i < vpEdges12.size(); i++) {
-    ORB_SLAM3::EdgeSim3ProjectXYZ* e12 = vpEdges12[i];
-    ORB_SLAM3::EdgeInverseSim3ProjectXYZ* e21 = vpEdges21[i];
+    MORB_SLAM::EdgeSim3ProjectXYZ* e12 = vpEdges12[i];
+    MORB_SLAM::EdgeInverseSim3ProjectXYZ* e21 = vpEdges21[i];
     if (!e12 || !e21) continue;
 
     if (e12->chi2() > th2 || e21->chi2() > th2) {
@@ -2268,8 +2268,8 @@ int Optimizer::OptimizeSim3(KeyFrame* pKF1, KeyFrame* pKF2,
       vpMatches1[idx] = nullptr;
       optimizer.removeEdge(e12);
       optimizer.removeEdge(e21);
-      vpEdges12[i] = static_cast<ORB_SLAM3::EdgeSim3ProjectXYZ*>(nullptr);
-      vpEdges21[i] = static_cast<ORB_SLAM3::EdgeInverseSim3ProjectXYZ*>(nullptr);
+      vpEdges12[i] = static_cast<MORB_SLAM::EdgeSim3ProjectXYZ*>(nullptr);
+      vpEdges21[i] = static_cast<MORB_SLAM::EdgeInverseSim3ProjectXYZ*>(nullptr);
       nBad++;
 
       if (!vbIsInKF2[i]) {
@@ -2298,8 +2298,8 @@ int Optimizer::OptimizeSim3(KeyFrame* pKF1, KeyFrame* pKF2,
   int nIn = 0;
   mAcumHessian = Eigen::MatrixXd::Zero(7, 7);
   for (size_t i = 0; i < vpEdges12.size(); i++) {
-    ORB_SLAM3::EdgeSim3ProjectXYZ* e12 = vpEdges12[i];
-    ORB_SLAM3::EdgeInverseSim3ProjectXYZ* e21 = vpEdges21[i];
+    MORB_SLAM::EdgeSim3ProjectXYZ* e12 = vpEdges12[i];
+    MORB_SLAM::EdgeInverseSim3ProjectXYZ* e21 = vpEdges21[i];
     if (!e12 || !e21) continue;
 
     e12->computeError();
@@ -3526,7 +3526,7 @@ void Optimizer::LocalBundleAdjustment(KeyFrame* pMainKF,
   const int nExpectedSize =
       (vpAdjustKF.size() + vpFixedKF.size()) * vpMPs.size();
 
-  vector<ORB_SLAM3::EdgeSE3ProjectXYZ*> vpEdgesMono;
+  vector<MORB_SLAM::EdgeSE3ProjectXYZ*> vpEdgesMono;
   vpEdgesMono.reserve(nExpectedSize);
 
   vector<KeyFrame*> vpEdgeKFMono;
@@ -3585,7 +3585,7 @@ void Optimizer::LocalBundleAdjustment(KeyFrame* pMainKF,
         Eigen::Matrix<double, 2, 1> obs;
         obs << kpUn.pt.x, kpUn.pt.y;
 
-        ORB_SLAM3::EdgeSE3ProjectXYZ* e = new ORB_SLAM3::EdgeSE3ProjectXYZ();
+        MORB_SLAM::EdgeSE3ProjectXYZ* e = new MORB_SLAM::EdgeSE3ProjectXYZ();
 
         e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(
                             optimizer.vertex(id)));
@@ -3663,7 +3663,7 @@ void Optimizer::LocalBundleAdjustment(KeyFrame* pMainKF,
     // Check inlier observations
     int badMonoMP = 0, badStereoMP = 0;
     for (size_t i = 0, iend = vpEdgesMono.size(); i < iend; i++) {
-      ORB_SLAM3::EdgeSE3ProjectXYZ* e = vpEdgesMono[i];
+      MORB_SLAM::EdgeSE3ProjectXYZ* e = vpEdgesMono[i];
       MapPoint* pMP = vpMapPointEdgeMono[i];
 
       if (pMP->isBad()) continue;
@@ -3705,7 +3705,7 @@ void Optimizer::LocalBundleAdjustment(KeyFrame* pMainKF,
   // Check inlier observations
   int badMonoMP = 0, badStereoMP = 0;
   for (size_t i = 0, iend = vpEdgesMono.size(); i < iend; i++) {
-    ORB_SLAM3::EdgeSE3ProjectXYZ* e = vpEdgesMono[i];
+    MORB_SLAM::EdgeSE3ProjectXYZ* e = vpEdgesMono[i];
     MapPoint* pMP = vpMapPointEdgeMono[i];
 
     if (pMP->isBad()) continue;
@@ -3796,7 +3796,7 @@ void Optimizer::LocalBundleAdjustment(KeyFrame* pMainKF,
     vector<MapPoint*> vpMonoMPsBad, vpStereoMPsBad;
 
     for (size_t i = 0, iend = vpEdgesMono.size(); i < iend; i++) {
-      ORB_SLAM3::EdgeSE3ProjectXYZ* e = vpEdgesMono[i];
+      MORB_SLAM::EdgeSE3ProjectXYZ* e = vpEdgesMono[i];
       MapPoint* pMP = vpMapPointEdgeMono[i];
       KeyFrame* pKFedge = vpEdgeKFMono[i];
 
@@ -5470,4 +5470,4 @@ void Optimizer::OptimizeEssentialGraph4DoF(
   pMap->IncreaseChangeIndex();
 }
 
-}  // namespace ORB_SLAM3
+}  // namespace MORB_SLAM
diff --git a/src/Settings.cc b/src/Settings.cc
index 8d4c5b8f..372e2373 100644
--- a/src/Settings.cc
+++ b/src/Settings.cc
@@ -30,7 +30,7 @@
 #include "CameraModels/KannalaBrandt8.h"
 #include "CameraModels/Pinhole.h"
 
-namespace ORB_SLAM3 {
+namespace MORB_SLAM {
 
 template <>
 float Settings::readParameter<float>(cv::FileStorage& fSettings,
@@ -152,7 +152,7 @@ Settings::Settings(const std::string& configFile, const int& sensor)
   std::cout << "\t-Loaded camera 1" << std::endl;
 
   // Read second camera if stereo (not rectified)
-  if (sensor_ == ORB_SLAM3::CameraType::STEREO || sensor_ == ORB_SLAM3::CameraType::IMU_STEREO) {
+  if (sensor_ == MORB_SLAM::CameraType::STEREO || sensor_ == MORB_SLAM::CameraType::IMU_STEREO) {
     readCamera2(fSettings);
     std::cout << "\t-Loaded camera 2" << std::endl;
   }
@@ -161,13 +161,13 @@ Settings::Settings(const std::string& configFile, const int& sensor)
   readImageInfo(fSettings);
   std::cout << "\t-Loaded image info" << std::endl;
 
-  if (sensor_ == ORB_SLAM3::CameraType::IMU_MONOCULAR || sensor_ == ORB_SLAM3::CameraType::IMU_STEREO ||
-      sensor_ == ORB_SLAM3::CameraType::IMU_RGBD) {
+  if (sensor_ == MORB_SLAM::CameraType::IMU_MONOCULAR || sensor_ == MORB_SLAM::CameraType::IMU_STEREO ||
+      sensor_ == MORB_SLAM::CameraType::IMU_RGBD) {
     readIMU(fSettings);
     std::cout << "\t-Loaded IMU calibration" << std::endl;
   }
 
-  if (sensor_ == ORB_SLAM3::CameraType::RGBD || sensor_ == ORB_SLAM3::CameraType::IMU_RGBD) {
+  if (sensor_ == MORB_SLAM::CameraType::RGBD || sensor_ == MORB_SLAM::CameraType::IMU_RGBD) {
     readRGBD(fSettings);
     std::cout << "\t-Loaded RGB-D calibration" << std::endl;
   }
@@ -232,7 +232,7 @@ void Settings::readCamera1(cv::FileStorage& fSettings) {
     }
 
     // Check if we need to correct distortion from the images
-    if ((sensor_ == ORB_SLAM3::CameraType::MONOCULAR || sensor_ == ORB_SLAM3::CameraType::IMU_MONOCULAR) &&
+    if ((sensor_ == MORB_SLAM::CameraType::MONOCULAR || sensor_ == MORB_SLAM::CameraType::IMU_MONOCULAR) &&
         vPinHoleDistorsion1_.size() != 0) {
       bNeedToUndistort_ = true;
     }
@@ -270,7 +270,7 @@ void Settings::readCamera1(cv::FileStorage& fSettings) {
     calibration1_ = std::make_shared<KannalaBrandt8>(vCalibration);
     originalCalib1_ = new KannalaBrandt8(vCalibration);
 
-    if (sensor_ == ORB_SLAM3::CameraType::STEREO || sensor_ == ORB_SLAM3::CameraType::IMU_STEREO) {
+    if (sensor_ == MORB_SLAM::CameraType::STEREO || sensor_ == MORB_SLAM::CameraType::IMU_STEREO) {
       int colBegin =
           readParameter<int>(fSettings, "Camera1.overlappingBegin", found);
       int colEnd =
@@ -388,7 +388,7 @@ void Settings::readImageInfo(cv::FileStorage& fSettings) {
       calibration1_->setParameter(
           calibration1_->getParameter(3) * scaleRowFactor, 3);
 
-      if ((sensor_ == ORB_SLAM3::CameraType::STEREO || sensor_ == ORB_SLAM3::CameraType::IMU_STEREO) &&
+      if ((sensor_ == MORB_SLAM::CameraType::STEREO || sensor_ == MORB_SLAM::CameraType::IMU_STEREO) &&
           cameraType_ != Rectified) {
         calibration2_->setParameter(
             calibration2_->getParameter(1) * scaleRowFactor, 1);
@@ -412,7 +412,7 @@ void Settings::readImageInfo(cv::FileStorage& fSettings) {
       calibration1_->setParameter(
           calibration1_->getParameter(2) * scaleColFactor, 2);
 
-      if ((sensor_ == ORB_SLAM3::CameraType::STEREO || sensor_ == ORB_SLAM3::CameraType::IMU_STEREO) &&
+      if ((sensor_ == MORB_SLAM::CameraType::STEREO || sensor_ == MORB_SLAM::CameraType::IMU_STEREO) &&
           cameraType_ != Rectified) {
         calibration2_->setParameter(
             calibration2_->getParameter(0) * scaleColFactor, 0);
@@ -554,7 +554,7 @@ void Settings::precomputeRectificationMaps() {
   bf_ = b_ * P1.at<double>(0, 0);
 
   // Update relative pose between camera 1 and IMU if necessary
-  if (sensor_ == ORB_SLAM3::CameraType::IMU_STEREO) {
+  if (sensor_ == MORB_SLAM::CameraType::IMU_STEREO) {
     Eigen::Matrix3f eigenR_r1_u1;
     cv::cv2eigen(R_r1_u1, eigenR_r1_u1);
     Sophus::SE3f T_r1_u1(eigenR_r1_u1, Eigen::Vector3f::Zero());
@@ -587,8 +587,8 @@ std::ostream& operator<<(std::ostream& output, const Settings& settings) {
     output << " ]" << std::endl;
   }
   std::cout << "bout to start camera 2 stuff\n";
-  if (settings.sensor_ == ORB_SLAM3::CameraType::STEREO ||
-      settings.sensor_ == ORB_SLAM3::CameraType::IMU_STEREO) {
+  if (settings.sensor_ == MORB_SLAM::CameraType::STEREO ||
+      settings.sensor_ == MORB_SLAM::CameraType::IMU_STEREO) {
     output << "\t-Camera 2 parameters (";
     if (settings.cameraType_ == Settings::PinHole ||
         settings.cameraType_ == Settings::Rectified) {
@@ -630,8 +630,8 @@ std::ostream& operator<<(std::ostream& output, const Settings& settings) {
     }
     output << " ]" << std::endl;
 
-    if ((settings.sensor_ == ORB_SLAM3::CameraType::STEREO ||
-         settings.sensor_ == ORB_SLAM3::CameraType::IMU_STEREO) &&
+    if ((settings.sensor_ == MORB_SLAM::CameraType::STEREO ||
+         settings.sensor_ == MORB_SLAM::CameraType::IMU_STEREO) &&
         settings.cameraType_ == Settings::KannalaBrandt) {
       output << "\t-Camera 2 parameters after resize: [ ";
       for (size_t i = 0; i < settings.calibration2_->size(); i++) {
@@ -644,8 +644,8 @@ std::ostream& operator<<(std::ostream& output, const Settings& settings) {
   output << "\t-Sequence FPS: " << settings.fps_ << std::endl;
 
   // Stereo stuff
-  if (settings.sensor_ == ORB_SLAM3::CameraType::STEREO ||
-      settings.sensor_ == ORB_SLAM3::CameraType::IMU_STEREO) {
+  if (settings.sensor_ == MORB_SLAM::CameraType::STEREO ||
+      settings.sensor_ == MORB_SLAM::CameraType::IMU_STEREO) {
     output << "\t-Stereo baseline: " << settings.b_ << std::endl;
     output << "\t-Stereo depth threshold : " << settings.thDepth_ << std::endl;
 
@@ -661,9 +661,9 @@ std::ostream& operator<<(std::ostream& output, const Settings& settings) {
     }
   }
 
-  if (settings.sensor_ == ORB_SLAM3::CameraType::IMU_MONOCULAR ||
-      settings.sensor_ == ORB_SLAM3::CameraType::IMU_STEREO ||
-      settings.sensor_ == ORB_SLAM3::CameraType::IMU_RGBD) {
+  if (settings.sensor_ == MORB_SLAM::CameraType::IMU_MONOCULAR ||
+      settings.sensor_ == MORB_SLAM::CameraType::IMU_STEREO ||
+      settings.sensor_ == MORB_SLAM::CameraType::IMU_RGBD) {
     output << "\t-Gyro noise: " << settings.noiseGyro_ << std::endl;
     output << "\t-Accelerometer noise: " << settings.noiseAcc_ << std::endl;
     output << "\t-Gyro walk: " << settings.gyroWalk_ << std::endl;
@@ -671,8 +671,8 @@ std::ostream& operator<<(std::ostream& output, const Settings& settings) {
     output << "\t-IMU frequency: " << settings.imuFrequency_ << std::endl;
   }
 
-  if (settings.sensor_ == ORB_SLAM3::CameraType::RGBD ||
-      settings.sensor_ == ORB_SLAM3::CameraType::IMU_RGBD) {
+  if (settings.sensor_ == MORB_SLAM::CameraType::RGBD ||
+      settings.sensor_ == MORB_SLAM::CameraType::IMU_RGBD) {
     output << "\t-RGB-D depth map factor: " << settings.depthMapFactor_ << std::endl;
   }
 
@@ -684,4 +684,4 @@ std::ostream& operator<<(std::ostream& output, const Settings& settings) {
 
   return output;
 }
-};  // namespace ORB_SLAM3
+};  // namespace MORB_SLAM
diff --git a/src/Sim3Solver.cc b/src/Sim3Solver.cc
index c1c37fe4..0f50c60c 100644
--- a/src/Sim3Solver.cc
+++ b/src/Sim3Solver.cc
@@ -29,7 +29,7 @@
 #include "KeyFrame.h"
 #include "ORBmatcher.h"
 
-namespace ORB_SLAM3 {
+namespace MORB_SLAM {
 
 Sim3Solver::Sim3Solver(KeyFrame *pKF1, KeyFrame *pKF2,
                        const vector<MapPoint *> &vpMatched12,
@@ -451,4 +451,4 @@ void Sim3Solver::FromCameraToImage(const vector<Eigen::Vector3f> &vP3Dc,
   }
 }
 
-}  // namespace ORB_SLAM3
+}  // namespace MORB_SLAM
diff --git a/src/System.cc b/src/System.cc
index ba2f7b67..c6bd3b35 100644
--- a/src/System.cc
+++ b/src/System.cc
@@ -43,7 +43,7 @@
 
 #include "Converter.h"
 
-namespace ORB_SLAM3 {
+namespace MORB_SLAM {
 
 Verbose::eLevel Verbose::th = Verbose::VERBOSITY_NORMAL;
 
@@ -208,7 +208,7 @@ System::System(const std::string& strVocFile, const std::string& strSettingsFile
       this, mpAtlas, mSensor == CameraType::MONOCULAR || mSensor == CameraType::IMU_MONOCULAR,
       mSensor == CameraType::IMU_MONOCULAR || mSensor == CameraType::IMU_STEREO || mSensor == CameraType::IMU_RGBD,
       strSequence);
-  mptLocalMapping = new thread(&ORB_SLAM3::LocalMapping::Run, mpLocalMapper);
+  mptLocalMapping = new thread(&MORB_SLAM::LocalMapping::Run, mpLocalMapper);
   if (settings_)
     mpLocalMapper->mThFarPoints = settings_->thFarPoints();
   else
@@ -225,7 +225,7 @@ System::System(const std::string& strVocFile, const std::string& strSettingsFile
   mpLoopCloser =
       new LoopClosing(mpAtlas, mpKeyFrameDatabase, mpVocabulary,
                       mSensor != CameraType::MONOCULAR, activeLC);  // mSensor!=CameraType::MONOCULAR);
-  mptLoopClosing = new thread(&ORB_SLAM3::LoopClosing::Run, mpLoopCloser);
+  mptLoopClosing = new thread(&MORB_SLAM::LoopClosing::Run, mpLoopCloser);
 
   // Set pointers between threads
   mpTracker->SetLocalMapper(mpLocalMapper);
@@ -555,7 +555,7 @@ void System::SaveTrajectoryTUM(const string& filename) {
 
   // 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<MORB_SLAM::KeyFrame*>::iterator lRit = mpTracker->mlpReferences.begin();
   list<double>::iterator lT = mpTracker->mlFrameTimes.begin();
   list<bool>::iterator lbL = mpTracker->mlbLost.begin();
   for (list<Sophus::SE3f>::iterator
@@ -674,7 +674,7 @@ void System::SaveTrajectoryEuRoC(const string& filename) {
 
   // 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<MORB_SLAM::KeyFrame*>::iterator lRit = mpTracker->mlpReferences.begin();
   list<double>::iterator lT = mpTracker->mlFrameTimes.begin();
   list<bool>::iterator lbL = mpTracker->mlbLost.begin();
 
@@ -780,7 +780,7 @@ void System::SaveTrajectoryEuRoC(const string& filename, std::shared_ptr<Map> pM
 
   // 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<MORB_SLAM::KeyFrame*>::iterator lRit = mpTracker->mlpReferences.begin();
   list<double>::iterator lT = mpTracker->mlFrameTimes.begin();
   list<bool>::iterator lbL = mpTracker->mlbLost.begin();
 
@@ -895,7 +895,7 @@ transformation.
     // 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 =
+    list<MORB_SLAM::KeyFrame*>::iterator lRit =
 mpTracker->mlpReferences.begin(); list<double>::iterator lT =
 mpTracker->mlFrameTimes.begin(); list<bool>::iterator lbL =
 mpTracker->mlbLost.begin();
@@ -1166,13 +1166,13 @@ transformation.
     // 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 =
+    list<MORB_SLAM::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;
+        MORB_SLAM::KeyFrame* pKF = *lRit;
 
         cv::Mat Trw = cv::Mat::eye(4,4,CV_32F);
 
@@ -1223,13 +1223,13 @@ void System::SaveTrajectoryKITTI(const string& filename) {
 
   // 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<MORB_SLAM::KeyFrame*>::iterator lRit = mpTracker->mlpReferences.begin();
   list<double>::iterator lT = mpTracker->mlFrameTimes.begin();
   for (list<Sophus::SE3f>::iterator
            lit = mpTracker->mlRelativeFramePoses.begin(),
            lend = mpTracker->mlRelativeFramePoses.end();
        lit != lend; lit++, lRit++, lT++) {
-    ORB_SLAM3::KeyFrame* pKF = *lRit;
+    MORB_SLAM::KeyFrame* pKF = *lRit;
 
     Sophus::SE3f Trw;
 
@@ -1550,4 +1550,4 @@ string System::CalculateCheckSum(string filename, int type) {
   return checksum;
 }
 
-}  // namespace ORB_SLAM3
+}  // namespace MORB_SLAM
diff --git a/src/Tracking.cc b/src/Tracking.cc
index c1d62a98..01a146ca 100644
--- a/src/Tracking.cc
+++ b/src/Tracking.cc
@@ -38,7 +38,7 @@
 
 using namespace std;
 
-namespace ORB_SLAM3 {
+namespace MORB_SLAM {
 
 Tracking::Tracking(System* pSys, ORBVocabulary* pVoc, const Atlas_ptr &pAtlas,
                    KeyFrameDatabase* pKFDB, const string& strSettingPath,
@@ -3641,7 +3641,7 @@ void Tracking::UpdateFrameIMU(const float s, const IMU::Bias& b,
                               KeyFrame* pCurrentKeyFrame) {
   std::shared_ptr<Map> pMap = pCurrentKeyFrame->GetMap();
   // unsigned int index = mnFirstFrameId; // UNUSED
-  list<ORB_SLAM3::KeyFrame*>::iterator lRit = mlpReferences.begin();
+  list<MORB_SLAM::KeyFrame*>::iterator lRit = mlpReferences.begin();
   list<bool>::iterator lbL = mlbLost.begin();
   for (auto lit = mlRelativeFramePoses.begin(),
             lend = mlRelativeFramePoses.end();
@@ -3765,4 +3765,4 @@ void Tracking::Release() {
 }
 #endif
 
-}  // namespace ORB_SLAM3
+}  // namespace MORB_SLAM
diff --git a/src/TwoViewReconstruction.cc b/src/TwoViewReconstruction.cc
index 36f603ae..d42c910d 100644
--- a/src/TwoViewReconstruction.cc
+++ b/src/TwoViewReconstruction.cc
@@ -28,7 +28,7 @@
 #include "GeometricTools.h"
 
 using namespace std;
-namespace ORB_SLAM3 {
+namespace MORB_SLAM {
 TwoViewReconstruction::TwoViewReconstruction(const Eigen::Matrix3f &k,
                                              float sigma, int iterations) {
   mK = k;
@@ -904,4 +904,4 @@ void TwoViewReconstruction::DecomposeE(const Eigen::Matrix3f &E,
   if (R2.determinant() < 0) R2 = -R2;
 }
 
-}  // namespace ORB_SLAM3
+}  // namespace MORB_SLAM
diff --git a/src/Viewer.cc b/src/Viewer.cc
index d7a1fa55..fd680d92 100644
--- a/src/Viewer.cc
+++ b/src/Viewer.cc
@@ -34,7 +34,7 @@
 #include "Atlas.h"
 #include "Tracking.h"
 
-namespace ORB_SLAM3 {
+namespace MORB_SLAM {
 
 void Viewer::setBoth(const bool b){
   both = true;
@@ -367,4 +367,4 @@ void Viewer::close(){
   mbClosed = true;
 }
 
-}  // namespace ORB_SLAM3
+}  // namespace MORB_SLAM

From 444f9c957afdad6bc53099831a0b75a5fab11a79 Mon Sep 17 00:00:00 2001
From: DavidPetkovsek <david.petkovsek@martinrea.com>
Date: Mon, 8 Aug 2022 14:38:06 -0400
Subject: [PATCH 24/26] Changed prefix for project header files to MORB_SLAM

---
 .gitignore                                    |  16 +-
 CMakeLists.txt                                |  60 +-
 .../Monocular-Inertial/mono_inertial_euroc.cc |  14 +-
 .../mono_inertial_realsense_D435i.cc          |  12 +-
 .../mono_inertial_realsense_t265.cc           |  14 +-
 .../mono_inertial_tum_vi.cc                   |  14 +-
 Examples/Monocular/mono_euroc.cc              |   8 +-
 Examples/Monocular/mono_kitti.cc              |  10 +-
 Examples/Monocular/mono_realsense_D435i.cc    |   8 +-
 Examples/Monocular/mono_realsense_t265.cc     |   8 +-
 Examples/Monocular/mono_tum.cc                |   8 +-
 Examples/Monocular/mono_tum_vi.cc             |  10 +-
 .../rgbd_inertial_realsense_D435i.cc          |  12 +-
 Examples/RGB-D/rgbd_realsense_D435i.cc        |   8 +-
 Examples/RGB-D/rgbd_tum.cc                    |   8 +-
 .../Stereo-Inertial/stereo_inertial_euroc.cc  |  16 +-
 .../stereo_inertial_realsense_D435i.cc        |  12 +-
 .../stereo_inertial_realsense_t265.cc         |  14 +-
 .../Stereo-Inertial/stereo_inertial_tum_vi.cc |  16 +-
 Examples/Stereo/stereo_euroc.cc               |  10 +-
 Examples/Stereo/stereo_kitti.cc               |   8 +-
 Examples/Stereo/stereo_realsense_D435i.cc     |   8 +-
 Examples/Stereo/stereo_realsense_t265.cc      |  10 +-
 Examples/Stereo/stereo_tum_vi.cc              |   8 +-
 README.md                                     |  38 +-
 build.sh                                      |   2 +-
 build_ros.sh                                  |   2 +-
 clean.sh                                      |  14 +-
 include/Atlas.h                               | 163 ----
 include/Camera.hpp                            |  92 --
 include/CameraModels/GeometricCamera.h        | 115 ---
 include/CameraModels/KannalaBrandt8.h         | 134 ---
 include/CameraModels/Pinhole.h                | 110 ---
 include/Converter.h                           |  78 --
 include/Frame.h                               | 376 --------
 include/FrameDrawer.h                         |  71 --
 include/G2oTypes.h                            | 844 ------------------
 include/GeometricTools.h                      |  81 --
 include/ImprovedTypes.hpp                     |  51 --
 include/ImuTypes.h                            | 272 ------
 include/KeyFrame.h                            | 542 -----------
 include/KeyFrameDatabase.h                    |  99 --
 include/LocalMapping.h                        | 198 ----
 include/LoopClosing.h                         | 245 -----
 include/MLPnPsolver.h                         | 243 -----
 include/Map.h                                 | 205 -----
 include/MapDrawer.h                           |  73 --
 include/MapPoint.h                            | 253 ------
 include/ORBVocabulary.h                       |  32 -
 include/ORBextractor.h                        | 102 ---
 include/ORBmatcher.h                          | 131 ---
 include/OptimizableTypes.h                    | 233 -----
 include/Optimizer.h                           | 141 ---
 include/SerializationUtils.h                  | 154 ----
 include/Settings.h                            | 239 -----
 include/Sim3Solver.h                          | 137 ---
 include/System.h                              | 245 -----
 include/Tracking.h                            | 364 --------
 include/TwoViewReconstruction.h               |  96 --
 include/Viewer.h                              |  77 --
 output.txt                                    | 170 ++--
 src/Atlas.cc                                  |  12 +-
 src/Camera.cpp                                |   6 +-
 src/CameraModels/KannalaBrandt8.cpp           |   8 +-
 src/CameraModels/Pinhole.cpp                  |   8 +-
 src/Converter.cc                              |   6 +-
 src/Frame.cc                                  |  24 +-
 src/FrameDrawer.cc                            |  14 +-
 src/G2oTypes.cc                               |  10 +-
 src/GeometricTools.cc                         |   8 +-
 src/ImuTypes.cc                               |  10 +-
 src/KeyFrame.cc                               |  10 +-
 src/KeyFrameDatabase.cc                       |   8 +-
 src/LocalMapping.cc                           |  16 +-
 src/LoopClosing.cc                            |  18 +-
 src/MLPnPsolver.cpp                           |   4 +-
 src/Map.cc                                    |   6 +-
 src/MapDrawer.cc                              |  12 +-
 src/MapPoint.cc                               |   8 +-
 src/ORBextractor.cc                           |   6 +-
 src/ORBmatcher.cc                             |   6 +-
 src/OptimizableTypes.cpp                      |   6 +-
 src/Optimizer.cc                              |  92 +-
 src/Settings.cc                               |  52 +-
 src/Sim3Solver.cc                             |  10 +-
 src/System.cc                                 |  30 +-
 src/Tracking.cc                               |  26 +-
 src/TwoViewReconstruction.cc                  |  10 +-
 src/Viewer.cc                                 |  14 +-
 89 files changed, 489 insertions(+), 6685 deletions(-)
 delete mode 100644 include/Atlas.h
 delete mode 100644 include/Camera.hpp
 delete mode 100644 include/CameraModels/GeometricCamera.h
 delete mode 100644 include/CameraModels/KannalaBrandt8.h
 delete mode 100644 include/CameraModels/Pinhole.h
 delete mode 100644 include/Converter.h
 delete mode 100644 include/Frame.h
 delete mode 100644 include/FrameDrawer.h
 delete mode 100644 include/G2oTypes.h
 delete mode 100644 include/GeometricTools.h
 delete mode 100644 include/ImprovedTypes.hpp
 delete mode 100644 include/ImuTypes.h
 delete mode 100644 include/KeyFrame.h
 delete mode 100644 include/KeyFrameDatabase.h
 delete mode 100644 include/LocalMapping.h
 delete mode 100644 include/LoopClosing.h
 delete mode 100644 include/MLPnPsolver.h
 delete mode 100644 include/Map.h
 delete mode 100644 include/MapDrawer.h
 delete mode 100644 include/MapPoint.h
 delete mode 100644 include/ORBVocabulary.h
 delete mode 100644 include/ORBextractor.h
 delete mode 100644 include/ORBmatcher.h
 delete mode 100644 include/OptimizableTypes.h
 delete mode 100644 include/Optimizer.h
 delete mode 100644 include/SerializationUtils.h
 delete mode 100644 include/Settings.h
 delete mode 100644 include/Sim3Solver.h
 delete mode 100644 include/System.h
 delete mode 100644 include/Tracking.h
 delete mode 100644 include/TwoViewReconstruction.h
 delete mode 100644 include/Viewer.h

diff --git a/.gitignore b/.gitignore
index e1edf598..12736869 100644
--- a/.gitignore
+++ b/.gitignore
@@ -45,20 +45,20 @@ Examples/Stereo-Inertial/stereo_inertial_realsense_t265
 Examples/Stereo-Inertial/stereo_inertial_realsense_D435i
 
 
-Examples/ROS/ORB_SLAM3/Mono
-Examples/ROS/ORB_SLAM3/Mono_Inertial
-Examples/ROS/ORB_SLAM3/MonoAR
-Examples/ROS/ORB_SLAM3/RGBD
-Examples/ROS/ORB_SLAM3/Stereo
-Examples/ROS/ORB_SLAM3/Stereo_Inertial
+Examples/ROS/MORB_SLAM/Mono
+Examples/ROS/MORB_SLAM/Mono_Inertial
+Examples/ROS/MORB_SLAM/MonoAR
+Examples/ROS/MORB_SLAM/RGBD
+Examples/ROS/MORB_SLAM/Stereo
+Examples/ROS/MORB_SLAM/Stereo_Inertial
 
 Examples/Calibration/recorder_realsense_D435i
 
 Examples/Tests/viewer_dataset
 Examples/Tests/sophus_test
 
-Examples/ROS/ORB_SLAM3/CMakeLists.txt.user
-Examples/ROS/ORB_SLAM3/build/
+Examples/ROS/MORB_SLAM/CMakeLists.txt.user
+Examples/ROS/MORB_SLAM/build/
 
 KeyFrameTrajectory.txt
 CameraTrajectory.txt
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 66d18fd1..c027fa14 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -77,36 +77,36 @@ src/MLPnPsolver.cpp
 src/GeometricTools.cc
 src/TwoViewReconstruction.cc
 src/Settings.cc
-include/System.h
-include/Camera.hpp
-include/Tracking.h
-include/LocalMapping.h
-include/LoopClosing.h
-include/ORBextractor.h
-include/ORBmatcher.h
-include/FrameDrawer.h
-include/Converter.h
-include/MapPoint.h
-include/KeyFrame.h
-include/Atlas.h
-include/Map.h
-include/MapDrawer.h
-include/Optimizer.h
-include/Frame.h
-include/KeyFrameDatabase.h
-include/Sim3Solver.h
-include/Viewer.h
-include/ImuTypes.h
-include/G2oTypes.h
-include/CameraModels/GeometricCamera.h
-include/CameraModels/Pinhole.h
-include/CameraModels/KannalaBrandt8.h
-include/OptimizableTypes.h
-include/MLPnPsolver.h
-include/GeometricTools.h
-include/TwoViewReconstruction.h
-include/SerializationUtils.h
-include/Settings.h)
+include/MORB_SLAM/System.h
+include/MORB_SLAM/Camera.hpp
+include/MORB_SLAM/Tracking.h
+include/MORB_SLAM/LocalMapping.h
+include/MORB_SLAM/LoopClosing.h
+include/MORB_SLAM/ORBextractor.h
+include/MORB_SLAM/ORBmatcher.h
+include/MORB_SLAM/FrameDrawer.h
+include/MORB_SLAM/Converter.h
+include/MORB_SLAM/MapPoint.h
+include/MORB_SLAM/KeyFrame.h
+include/MORB_SLAM/Atlas.h
+include/MORB_SLAM/Map.h
+include/MORB_SLAM/MapDrawer.h
+include/MORB_SLAM/Optimizer.h
+include/MORB_SLAM/Frame.h
+include/MORB_SLAM/KeyFrameDatabase.h
+include/MORB_SLAM/Sim3Solver.h
+include/MORB_SLAM/Viewer.h
+include/MORB_SLAM/ImuTypes.h
+include/MORB_SLAM/G2oTypes.h
+include/MORB_SLAM/CameraModels/GeometricCamera.h
+include/MORB_SLAM/CameraModels/Pinhole.h
+include/MORB_SLAM/CameraModels/KannalaBrandt8.h
+include/MORB_SLAM/OptimizableTypes.h
+include/MORB_SLAM/MLPnPsolver.h
+include/MORB_SLAM/GeometricTools.h
+include/MORB_SLAM/TwoViewReconstruction.h
+include/MORB_SLAM/SerializationUtils.h
+include/MORB_SLAM/Settings.h)
 
 
 add_subdirectory(Thirdparty/g2o)
diff --git a/Examples/Monocular-Inertial/mono_inertial_euroc.cc b/Examples/Monocular-Inertial/mono_inertial_euroc.cc
index 8c02bb52..0ae51f69 100644
--- a/Examples/Monocular-Inertial/mono_inertial_euroc.cc
+++ b/Examples/Monocular-Inertial/mono_inertial_euroc.cc
@@ -26,9 +26,9 @@
 
 #include<opencv2/opencv.hpp>
 
-#include<System.h>
-#include<Viewer.h>
-#include "ImuTypes.h"
+#include<MORB_SLAM/System.h>
+#include<MORB_SLAM/Viewer.h>
+#include "MORB_SLAM/ImuTypes.h"
 
 using namespace std;
 
@@ -118,8 +118,8 @@ int main(int argc, char *argv[])
     cout.precision(17);
 
     // Create SLAM system. It initializes all system threads and gets ready to process frames.
-    ORB_SLAM3::System_ptr SLAM = std::make_shared<ORB_SLAM3::System>(argv[1],argv[2],ORB_SLAM3::CameraType::IMU_MONOCULAR);
-    ORB_SLAM3::Viewer viewer(SLAM, argv[2]);
+    MORB_SLAM::System_ptr SLAM = std::make_shared<MORB_SLAM::System>(argv[1],argv[2],MORB_SLAM::CameraType::IMU_MONOCULAR);
+    MORB_SLAM::Viewer viewer(SLAM, argv[2]);
     float imageScale = SLAM->GetImageScale();
 
 #ifdef REGISTER_TIMES
@@ -132,7 +132,7 @@ int main(int argc, char *argv[])
 
         // Main loop
         cv::Mat im;
-        vector<ORB_SLAM3::IMU::Point> vImuMeas;
+        vector<MORB_SLAM::IMU::Point> vImuMeas;
         proccIm = 0;
         for(int ni=0; ni<nImages[seq]; ni++, proccIm++)
         {
@@ -172,7 +172,7 @@ int main(int argc, char *argv[])
 
                 while(vTimestampsImu[seq][first_imu[seq]]<=vTimestampsCam[seq][ni])
                 {
-                    vImuMeas.push_back(ORB_SLAM3::IMU::Point(vAcc[seq][first_imu[seq]].x,vAcc[seq][first_imu[seq]].y,vAcc[seq][first_imu[seq]].z,
+                    vImuMeas.push_back(MORB_SLAM::IMU::Point(vAcc[seq][first_imu[seq]].x,vAcc[seq][first_imu[seq]].y,vAcc[seq][first_imu[seq]].z,
                                                              vGyro[seq][first_imu[seq]].x,vGyro[seq][first_imu[seq]].y,vGyro[seq][first_imu[seq]].z,
                                                              vTimestampsImu[seq][first_imu[seq]]));
                     first_imu[seq]++;
diff --git a/Examples/Monocular-Inertial/mono_inertial_realsense_D435i.cc b/Examples/Monocular-Inertial/mono_inertial_realsense_D435i.cc
index 63d13e2f..e350633f 100644
--- a/Examples/Monocular-Inertial/mono_inertial_realsense_D435i.cc
+++ b/Examples/Monocular-Inertial/mono_inertial_realsense_D435i.cc
@@ -32,8 +32,8 @@
 #include <librealsense2/rs.hpp>
 #include "librealsense2/rsutil.h"
 
-#include <System.h>
-#include <Viewer.h>
+#include <MORB_SLAM/System.h>
+#include <MORB_SLAM/Viewer.h>
 
 using namespace std;
 
@@ -258,7 +258,7 @@ int main(int argc, char **argv) {
 
     rs2::pipeline_profile pipe_profile = pipe.start(cfg, imu_callback);
 
-    vector<ORB_SLAM3::IMU::Point> vImuMeas;
+    vector<MORB_SLAM::IMU::Point> vImuMeas;
     rs2::stream_profile cam_stream = pipe_profile.get_stream(RS2_STREAM_INFRARED, 1);
 
 
@@ -288,8 +288,8 @@ int main(int argc, char **argv) {
 
 
     // Create SLAM system. It initializes all system threads and gets ready to process frames.
-    ORB_SLAM3::System_ptr SLAM = std::make_shared<ORB_SLAM3::System>(argv[1],argv[2],ORB_SLAM3::CameraType::IMU_MONOCULAR, file_name);
-    ORB_SLAM3::Viewer viewer(SLAM, argv[2]);
+    MORB_SLAM::System_ptr SLAM = std::make_shared<MORB_SLAM::System>(argv[1],argv[2],MORB_SLAM::CameraType::IMU_MONOCULAR, file_name);
+    MORB_SLAM::Viewer viewer(SLAM, argv[2]);
     float imageScale = SLAM->GetImageScale();
 
     double timestamp;
@@ -356,7 +356,7 @@ int main(int argc, char **argv) {
 
         for(size_t i=0; i<vGyro.size(); ++i)
         {
-            ORB_SLAM3::IMU::Point lastPoint(vAccel[i].x, vAccel[i].y, vAccel[i].z,
+            MORB_SLAM::IMU::Point lastPoint(vAccel[i].x, vAccel[i].y, vAccel[i].z,
                                   vGyro[i].x, vGyro[i].y, vGyro[i].z,
                                   vGyro_times[i]);
             vImuMeas.push_back(lastPoint);
diff --git a/Examples/Monocular-Inertial/mono_inertial_realsense_t265.cc b/Examples/Monocular-Inertial/mono_inertial_realsense_t265.cc
index 71addae1..dd17eb66 100644
--- a/Examples/Monocular-Inertial/mono_inertial_realsense_t265.cc
+++ b/Examples/Monocular-Inertial/mono_inertial_realsense_t265.cc
@@ -29,10 +29,10 @@
 
 #include <librealsense2/rs.hpp>
 
-#include <System.h>
-#include <Viewer.h>
+#include <MORB_SLAM/System.h>
+#include <MORB_SLAM/Viewer.h>
 #include <condition_variable>
-#include "ImuTypes.h"
+#include "MORB_SLAM/ImuTypes.h"
 
 using namespace std;
 
@@ -65,8 +65,8 @@ int main(int argc, char **argv)
         // bFileName = true; // UNUSED
     }
 
-    ORB_SLAM3::System_ptr SLAM = std::make_shared<ORB_SLAM3::System>(argv[1],argv[2],ORB_SLAM3::CameraType::IMU_MONOCULAR, file_name);
-    ORB_SLAM3::Viewer viewer(SLAM, argv[2]);
+    MORB_SLAM::System_ptr SLAM = std::make_shared<MORB_SLAM::System>(argv[1],argv[2],MORB_SLAM::CameraType::IMU_MONOCULAR, file_name);
+    MORB_SLAM::Viewer viewer(SLAM, argv[2]);
     float imageScale = SLAM->GetImageScale();
 
     struct sigaction sigIntHandler;
@@ -206,7 +206,7 @@ int main(int argc, char **argv)
 
 
     // Create SLAM system. It initializes all system threads and gets ready to process frames.
-    vector<ORB_SLAM3::IMU::Point> vImuMeas;
+    vector<MORB_SLAM::IMU::Point> vImuMeas;
 
     double timestamp;
     cv::Mat im;
@@ -286,7 +286,7 @@ int main(int argc, char **argv)
 
 
         for(size_t i=0; i<vGyro.size(); ++i){
-            ORB_SLAM3::IMU::Point lastPoint(vAccel[i].x, vAccel[i].y, vAccel[i].z,
+            MORB_SLAM::IMU::Point lastPoint(vAccel[i].x, vAccel[i].y, vAccel[i].z,
                                             vGyro[i].x, vGyro[i].y, vGyro[i].z,
                                             vGyro_times[i]);
             vImuMeas.push_back(lastPoint);
diff --git a/Examples/Monocular-Inertial/mono_inertial_tum_vi.cc b/Examples/Monocular-Inertial/mono_inertial_tum_vi.cc
index bb6dc172..43a726a2 100644
--- a/Examples/Monocular-Inertial/mono_inertial_tum_vi.cc
+++ b/Examples/Monocular-Inertial/mono_inertial_tum_vi.cc
@@ -25,9 +25,9 @@
 
 #include<opencv2/opencv.hpp>
 
-#include<System.h>
-#include<Viewer.h>
-#include "ImuTypes.h"
+#include<MORB_SLAM/System.h>
+#include<MORB_SLAM/Viewer.h>
+#include "MORB_SLAM/ImuTypes.h"
 
 using namespace std;
 
@@ -117,8 +117,8 @@ int main(int argc, char **argv)
     cout << "IMU data in the sequence: " << nImu << endl << endl;*/
 
     // Create SLAM system. It initializes all system threads and gets ready to process frames.
-    ORB_SLAM3::System_ptr SLAM = std::make_shared<ORB_SLAM3::System>(argv[1],argv[2],ORB_SLAM3::CameraType::IMU_MONOCULAR, file_name);
-    ORB_SLAM3::Viewer viewer(SLAM, argv[2]);
+    MORB_SLAM::System_ptr SLAM = std::make_shared<MORB_SLAM::System>(argv[1],argv[2],MORB_SLAM::CameraType::IMU_MONOCULAR, file_name);
+    MORB_SLAM::Viewer viewer(SLAM, argv[2]);
     float imageScale = SLAM->GetImageScale();
 
 #ifdef REGISTER_TIMES
@@ -131,7 +131,7 @@ int main(int argc, char **argv)
 
         // Main loop
         cv::Mat im;
-        vector<ORB_SLAM3::IMU::Point> vImuMeas;
+        vector<MORB_SLAM::IMU::Point> vImuMeas;
         proccIm = 0;
         cv::Ptr<cv::CLAHE> clahe = cv::createCLAHE(3.0, cv::Size(8, 8));
         for(int ni=0; ni<nImages[seq]; ni++, proccIm++)
@@ -164,7 +164,7 @@ int main(int argc, char **argv)
 
                 while(vTimestampsImu[seq][first_imu[seq]]<=vTimestampsCam[seq][ni])
                 {
-                    vImuMeas.push_back(ORB_SLAM3::IMU::Point(vAcc[seq][first_imu[seq]].x,vAcc[seq][first_imu[seq]].y,vAcc[seq][first_imu[seq]].z,
+                    vImuMeas.push_back(MORB_SLAM::IMU::Point(vAcc[seq][first_imu[seq]].x,vAcc[seq][first_imu[seq]].y,vAcc[seq][first_imu[seq]].z,
                                                              vGyro[seq][first_imu[seq]].x,vGyro[seq][first_imu[seq]].y,vGyro[seq][first_imu[seq]].z,
                                                              vTimestampsImu[seq][first_imu[seq]]));
                     // cout << "t_imu = " << fixed << vImuMeas.back().t << endl;
diff --git a/Examples/Monocular/mono_euroc.cc b/Examples/Monocular/mono_euroc.cc
index df6eb16e..069eb8a7 100644
--- a/Examples/Monocular/mono_euroc.cc
+++ b/Examples/Monocular/mono_euroc.cc
@@ -23,8 +23,8 @@
 
 #include<opencv2/opencv.hpp>
 
-#include<System.h>
-#include<Viewer.h>
+#include<MORB_SLAM/System.h>
+#include<MORB_SLAM/Viewer.h>
 
 using namespace std;
 
@@ -81,8 +81,8 @@ int main(int argc, char **argv)
     // int fps = 20; // UNUSED
     // float dT = 1.f/fps; // UNUSED
     // Create SLAM system. It initializes all system threads and gets ready to process frames.
-    ORB_SLAM3::System_ptr SLAM = std::make_shared<ORB_SLAM3::System>(argv[1],argv[2],ORB_SLAM3::CameraType::MONOCULAR);
-    ORB_SLAM3::Viewer viewer(SLAM, argv[2]);
+    MORB_SLAM::System_ptr SLAM = std::make_shared<MORB_SLAM::System>(argv[1],argv[2],MORB_SLAM::CameraType::MONOCULAR);
+    MORB_SLAM::Viewer viewer(SLAM, argv[2]);
     float imageScale = SLAM->GetImageScale();
 
 #ifdef REGISTER_TIMES
diff --git a/Examples/Monocular/mono_kitti.cc b/Examples/Monocular/mono_kitti.cc
index 567bdded..c64890f2 100644
--- a/Examples/Monocular/mono_kitti.cc
+++ b/Examples/Monocular/mono_kitti.cc
@@ -24,8 +24,8 @@
 
 #include <opencv2/opencv.hpp>
 
-#include <System.h>
-#include <Viewer.h>
+#include <MORB_SLAM/System.h>
+#include <MORB_SLAM/Viewer.h>
 
 using namespace std;
 
@@ -48,8 +48,8 @@ int main(int argc, char **argv)
     int nImages = vstrImageFilenames.size();
 
     // Create SLAM system. It initializes all system threads and gets ready to process frames.
-    ORB_SLAM3::System_ptr SLAM = std::make_shared<ORB_SLAM3::System>(argv[1],argv[2],ORB_SLAM3::CameraType::MONOCULAR);
-    ORB_SLAM3::Viewer viewer(SLAM, argv[2]);
+    MORB_SLAM::System_ptr SLAM = std::make_shared<MORB_SLAM::System>(argv[1],argv[2],MORB_SLAM::CameraType::MONOCULAR);
+    MORB_SLAM::Viewer viewer(SLAM, argv[2]);
     float imageScale = SLAM->GetImageScale();
 
     // Vector for tracking time statistics
@@ -97,7 +97,7 @@ int main(int argc, char **argv)
         std::chrono::steady_clock::time_point t1 = std::chrono::steady_clock::now();
 
         // Pass the image to the SLAM system
-        auto pos = SLAM->TrackMonocular(im,tframe,vector<ORB_SLAM3::IMU::Point>(), vstrImageFilenames[ni]);
+        auto pos = SLAM->TrackMonocular(im,tframe,vector<MORB_SLAM::IMU::Point>(), vstrImageFilenames[ni]);
 
         std::chrono::steady_clock::time_point t2 = std::chrono::steady_clock::now();
 
diff --git a/Examples/Monocular/mono_realsense_D435i.cc b/Examples/Monocular/mono_realsense_D435i.cc
index 422cd62c..9e8107a8 100644
--- a/Examples/Monocular/mono_realsense_D435i.cc
+++ b/Examples/Monocular/mono_realsense_D435i.cc
@@ -32,8 +32,8 @@
 #include <librealsense2/rs.hpp>
 #include "librealsense2/rsutil.h"
 
-#include <System.h>
-#include <Viewer.h>
+#include <MORB_SLAM/System.h>
+#include <MORB_SLAM/Viewer.h>
 
 using namespace std;
 
@@ -208,8 +208,8 @@ int main(int argc, char **argv) {
     std::cout << " Model = " << intrinsics_left.model << std::endl;
 
     // Create SLAM system. It initializes all system threads and gets ready to process frames.
-    ORB_SLAM3::System_ptr SLAM = std::make_shared<ORB_SLAM3::System>(argv[1],argv[2],ORB_SLAM3::CameraType::MONOCULAR, file_name);
-    ORB_SLAM3::Viewer viewer(SLAM, argv[2]);
+    MORB_SLAM::System_ptr SLAM = std::make_shared<MORB_SLAM::System>(argv[1],argv[2],MORB_SLAM::CameraType::MONOCULAR, file_name);
+    MORB_SLAM::Viewer viewer(SLAM, argv[2]);
     float imageScale = SLAM->GetImageScale();
 
     double timestamp;
diff --git a/Examples/Monocular/mono_realsense_t265.cc b/Examples/Monocular/mono_realsense_t265.cc
index 39007c30..f691e170 100644
--- a/Examples/Monocular/mono_realsense_t265.cc
+++ b/Examples/Monocular/mono_realsense_t265.cc
@@ -29,8 +29,8 @@
 
 #include <librealsense2/rs.hpp>
 
-#include <System.h>
-#include <Viewer.h>
+#include <MORB_SLAM/System.h>
+#include <MORB_SLAM/Viewer.h>
 
 using namespace std;
 
@@ -88,8 +88,8 @@ int main(int argc, char **argv)
     cout << "IMU data in the sequence: " << nImu << endl << endl;*/
 
     // Create SLAM system. It initializes all system threads and gets ready to process frames.
-    ORB_SLAM3::System_ptr SLAM = std::make_shared<ORB_SLAM3::System>(argv[1],argv[2],ORB_SLAM3::CameraType::MONOCULAR);
-    ORB_SLAM3::Viewer viewer(SLAM, argv[2]);
+    MORB_SLAM::System_ptr SLAM = std::make_shared<MORB_SLAM::System>(argv[1],argv[2],MORB_SLAM::CameraType::MONOCULAR);
+    MORB_SLAM::Viewer viewer(SLAM, argv[2]);
     float imageScale = SLAM->GetImageScale();
 
     cv::Mat imCV;
diff --git a/Examples/Monocular/mono_tum.cc b/Examples/Monocular/mono_tum.cc
index f528f93d..4eb56d54 100644
--- a/Examples/Monocular/mono_tum.cc
+++ b/Examples/Monocular/mono_tum.cc
@@ -23,8 +23,8 @@
 
 #include<opencv2/opencv.hpp>
 
-#include<System.h>
-#include<Viewer.h>
+#include<MORB_SLAM/System.h>
+#include<MORB_SLAM/Viewer.h>
 
 using namespace std;
 
@@ -48,8 +48,8 @@ int main(int argc, char **argv)
     int nImages = vstrImageFilenames.size();
 
     // Create SLAM system. It initializes all system threads and gets ready to process frames.
-    ORB_SLAM3::System_ptr SLAM = std::make_shared<ORB_SLAM3::System>(argv[1],argv[2],ORB_SLAM3::CameraType::MONOCULAR);
-    ORB_SLAM3::Viewer viewer(SLAM, argv[2]);
+    MORB_SLAM::System_ptr SLAM = std::make_shared<MORB_SLAM::System>(argv[1],argv[2],MORB_SLAM::CameraType::MONOCULAR);
+    MORB_SLAM::Viewer viewer(SLAM, argv[2]);
     float imageScale = SLAM->GetImageScale();
 
     // Vector for tracking time statistics
diff --git a/Examples/Monocular/mono_tum_vi.cc b/Examples/Monocular/mono_tum_vi.cc
index a2faedb4..4b184755 100644
--- a/Examples/Monocular/mono_tum_vi.cc
+++ b/Examples/Monocular/mono_tum_vi.cc
@@ -25,9 +25,9 @@
 
 #include<opencv2/opencv.hpp>
 
-#include <System.h>
-#include <Viewer.h>
-#include "Converter.h"
+#include <MORB_SLAM/System.h>
+#include <MORB_SLAM/Viewer.h>
+#include "MORB_SLAM/Converter.h"
 
 using namespace std;
 
@@ -90,8 +90,8 @@ int main(int argc, char **argv)
     cout.precision(17);
 
     // Create SLAM system. It initializes all system threads and gets ready to process frames.
-    ORB_SLAM3::System_ptr SLAM = std::make_shared<ORB_SLAM3::System>(argv[1],argv[2],ORB_SLAM3::CameraType::MONOCULAR, file_name);
-    ORB_SLAM3::Viewer viewer(SLAM, argv[2]);
+    MORB_SLAM::System_ptr SLAM = std::make_shared<MORB_SLAM::System>(argv[1],argv[2],MORB_SLAM::CameraType::MONOCULAR, file_name);
+    MORB_SLAM::Viewer viewer(SLAM, argv[2]);
     float imageScale = SLAM->GetImageScale();
 
 #ifdef REGISTER_TIMES
diff --git a/Examples/RGB-D-Inertial/rgbd_inertial_realsense_D435i.cc b/Examples/RGB-D-Inertial/rgbd_inertial_realsense_D435i.cc
index 68e6adbf..c328e3ad 100644
--- a/Examples/RGB-D-Inertial/rgbd_inertial_realsense_D435i.cc
+++ b/Examples/RGB-D-Inertial/rgbd_inertial_realsense_D435i.cc
@@ -33,8 +33,8 @@
 #include "librealsense2/rsutil.h"
 
 
-#include <System.h>
-#include <Viewer.h>
+#include <MORB_SLAM/System.h>
+#include <MORB_SLAM/Viewer.h>
 
 using namespace std;
 
@@ -291,7 +291,7 @@ int main(int argc, char **argv) {
     pipe_profile = pipe.start(cfg, imu_callback);
 
 
-    vector<ORB_SLAM3::IMU::Point> vImuMeas;
+    vector<MORB_SLAM::IMU::Point> vImuMeas;
     rs2::stream_profile cam_stream = pipe_profile.get_stream(RS2_STREAM_COLOR);
 
     rs2::stream_profile imu_stream = pipe_profile.get_stream(RS2_STREAM_GYRO);
@@ -319,8 +319,8 @@ int main(int argc, char **argv) {
 
 
     // Create SLAM system. It initializes all system threads and gets ready to process frames.
-    ORB_SLAM3::System_ptr SLAM = std::make_shared<ORB_SLAM3::System>(argv[1],argv[2],ORB_SLAM3::CameraType::IMU_RGBD, file_name);
-    ORB_SLAM3::Viewer viewer(SLAM, argv[2]);
+    MORB_SLAM::System_ptr SLAM = std::make_shared<MORB_SLAM::System>(argv[1],argv[2],MORB_SLAM::CameraType::IMU_RGBD, file_name);
+    MORB_SLAM::Viewer viewer(SLAM, argv[2]);
     float imageScale = SLAM->GetImageScale();
 
     double timestamp;
@@ -404,7 +404,7 @@ int main(int argc, char **argv) {
 
         for(size_t i=0; i<vGyro.size(); ++i)
         {
-            ORB_SLAM3::IMU::Point lastPoint(vAccel[i].x, vAccel[i].y, vAccel[i].z,
+            MORB_SLAM::IMU::Point lastPoint(vAccel[i].x, vAccel[i].y, vAccel[i].z,
                                             vGyro[i].x, vGyro[i].y, vGyro[i].z,
                                             vGyro_times[i]);
             vImuMeas.push_back(lastPoint);
diff --git a/Examples/RGB-D/rgbd_realsense_D435i.cc b/Examples/RGB-D/rgbd_realsense_D435i.cc
index e2d9d322..f66d1e3c 100644
--- a/Examples/RGB-D/rgbd_realsense_D435i.cc
+++ b/Examples/RGB-D/rgbd_realsense_D435i.cc
@@ -33,8 +33,8 @@
 #include "librealsense2/rsutil.h"
 
 
-#include <System.h>
-#include <Viewer.h>
+#include <MORB_SLAM/System.h>
+#include <MORB_SLAM/Viewer.h>
 
 using namespace std;
 
@@ -307,8 +307,8 @@ int main(int argc, char **argv) {
 
 
     // Create SLAM system. It initializes all system threads and gets ready to process frames.
-    ORB_SLAM3::System_ptr SLAM = std::make_shared<ORB_SLAM3::System>(argv[1],argv[2],ORB_SLAM3::CameraType::RGBD, file_name);
-    ORB_SLAM3::Viewer viewer(SLAM, argv[2]);
+    MORB_SLAM::System_ptr SLAM = std::make_shared<MORB_SLAM::System>(argv[1],argv[2],MORB_SLAM::CameraType::RGBD, file_name);
+    MORB_SLAM::Viewer viewer(SLAM, argv[2]);
     float imageScale = SLAM->GetImageScale();
 
     double timestamp;
diff --git a/Examples/RGB-D/rgbd_tum.cc b/Examples/RGB-D/rgbd_tum.cc
index 6274e76c..de6196c2 100644
--- a/Examples/RGB-D/rgbd_tum.cc
+++ b/Examples/RGB-D/rgbd_tum.cc
@@ -23,8 +23,8 @@
 
 #include <opencv2/opencv.hpp>
 
-#include <System.h>
-#include <Viewer.h>
+#include <MORB_SLAM/System.h>
+#include <MORB_SLAM/Viewer.h>
 
 using namespace std;
 
@@ -60,8 +60,8 @@ int main(int argc, char **argv)
     }
 
     // Create SLAM system. It initializes all system threads and gets ready to process frames.
-    ORB_SLAM3::System_ptr SLAM = std::make_shared<ORB_SLAM3::System>(argv[1],argv[2],ORB_SLAM3::CameraType::RGBD);
-    ORB_SLAM3::Viewer viewer(SLAM, argv[2]);
+    MORB_SLAM::System_ptr SLAM = std::make_shared<MORB_SLAM::System>(argv[1],argv[2],MORB_SLAM::CameraType::RGBD);
+    MORB_SLAM::Viewer viewer(SLAM, argv[2]);
     float imageScale = SLAM->GetImageScale();
 
     // Vector for tracking time statistics
diff --git a/Examples/Stereo-Inertial/stereo_inertial_euroc.cc b/Examples/Stereo-Inertial/stereo_inertial_euroc.cc
index fdfa1aec..4b20a4ce 100644
--- a/Examples/Stereo-Inertial/stereo_inertial_euroc.cc
+++ b/Examples/Stereo-Inertial/stereo_inertial_euroc.cc
@@ -27,10 +27,10 @@
 #include <opencv2/opencv.hpp>
 
 
-#include<System.h>
-#include<Viewer.h>
-#include "ImuTypes.h"
-#include "Optimizer.h"
+#include<MORB_SLAM/System.h>
+#include<MORB_SLAM/Viewer.h>
+#include "MORB_SLAM/ImuTypes.h"
+#include "MORB_SLAM/Optimizer.h"
 
 using namespace std;
 
@@ -130,14 +130,14 @@ int main(int argc, char **argv)
     cout.precision(17);
 
     // Create SLAM system. It initializes all system threads and gets ready to process frames.
-    ORB_SLAM3::System_ptr SLAM = std::make_shared<ORB_SLAM3::System>(argv[1],argv[2],ORB_SLAM3::CameraType::IMU_STEREO);
-    ORB_SLAM3::Viewer viewer(SLAM, argv[2]);
+    MORB_SLAM::System_ptr SLAM = std::make_shared<MORB_SLAM::System>(argv[1],argv[2],MORB_SLAM::CameraType::IMU_STEREO);
+    MORB_SLAM::Viewer viewer(SLAM, argv[2]);
 
     cv::Mat imLeft, imRight;
     for (seq = 0; seq<num_seq; seq++)
     {
         // Seq loop
-        vector<ORB_SLAM3::IMU::Point> vImuMeas;
+        vector<MORB_SLAM::IMU::Point> vImuMeas;
 
 #ifdef REGISTER_TIMES
         double t_rect = 0.f;
@@ -174,7 +174,7 @@ int main(int argc, char **argv)
             if(ni>0)
                 while(vTimestampsImu[seq][first_imu[seq]]<=vTimestampsCam[seq][ni]) // while(vTimestampsImu[first_imu]<=vTimestampsCam[ni])
                 {
-                    vImuMeas.push_back(ORB_SLAM3::IMU::Point(vAcc[seq][first_imu[seq]].x,vAcc[seq][first_imu[seq]].y,vAcc[seq][first_imu[seq]].z,
+                    vImuMeas.push_back(MORB_SLAM::IMU::Point(vAcc[seq][first_imu[seq]].x,vAcc[seq][first_imu[seq]].y,vAcc[seq][first_imu[seq]].z,
                                                              vGyro[seq][first_imu[seq]].x,vGyro[seq][first_imu[seq]].y,vGyro[seq][first_imu[seq]].z,
                                                              vTimestampsImu[seq][first_imu[seq]]));
                     first_imu[seq]++;
diff --git a/Examples/Stereo-Inertial/stereo_inertial_realsense_D435i.cc b/Examples/Stereo-Inertial/stereo_inertial_realsense_D435i.cc
index a5925dfa..91ee04dd 100755
--- a/Examples/Stereo-Inertial/stereo_inertial_realsense_D435i.cc
+++ b/Examples/Stereo-Inertial/stereo_inertial_realsense_D435i.cc
@@ -32,8 +32,8 @@
 #include <librealsense2/rs.hpp>
 #include "librealsense2/rsutil.h"
 
-#include <System.h>
-#include <Viewer.h>
+#include <MORB_SLAM/System.h>
+#include <MORB_SLAM/Viewer.h>
 
 using namespace std;
 
@@ -262,7 +262,7 @@ int main(int argc, char **argv) {
 
     rs2::pipeline_profile pipe_profile = pipe.start(cfg, imu_callback);
 
-    vector<ORB_SLAM3::IMU::Point> vImuMeas;
+    vector<MORB_SLAM::IMU::Point> vImuMeas;
     rs2::stream_profile cam_left = pipe_profile.get_stream(RS2_STREAM_INFRARED, 1);
     rs2::stream_profile cam_right = pipe_profile.get_stream(RS2_STREAM_INFRARED, 2);
 
@@ -318,8 +318,8 @@ int main(int argc, char **argv) {
 
 
     // Create SLAM system. It initializes all system threads and gets ready to process frames.
-    ORB_SLAM3::System_ptr SLAM = std::make_shared<ORB_SLAM3::System>(argv[1],argv[2],ORB_SLAM3::CameraType::IMU_STEREO, file_name);
-    ORB_SLAM3::Viewer viewer(SLAM, argv[2]);
+    MORB_SLAM::System_ptr SLAM = std::make_shared<MORB_SLAM::System>(argv[1],argv[2],MORB_SLAM::CameraType::IMU_STEREO, file_name);
+    MORB_SLAM::Viewer viewer(SLAM, argv[2]);
     float imageScale = SLAM->GetImageScale();
 
     double timestamp;
@@ -388,7 +388,7 @@ int main(int argc, char **argv) {
 
         for(size_t i=0; i<vGyro.size(); ++i)
         {
-            ORB_SLAM3::IMU::Point lastPoint(vAccel[i].x, vAccel[i].y, vAccel[i].z,
+            MORB_SLAM::IMU::Point lastPoint(vAccel[i].x, vAccel[i].y, vAccel[i].z,
                                   vGyro[i].x, vGyro[i].y, vGyro[i].z,
                                   vGyro_times[i]);
             vImuMeas.push_back(lastPoint);
diff --git a/Examples/Stereo-Inertial/stereo_inertial_realsense_t265.cc b/Examples/Stereo-Inertial/stereo_inertial_realsense_t265.cc
index a3e2fa99..be3422b5 100644
--- a/Examples/Stereo-Inertial/stereo_inertial_realsense_t265.cc
+++ b/Examples/Stereo-Inertial/stereo_inertial_realsense_t265.cc
@@ -29,10 +29,10 @@
 
 #include <librealsense2/rs.hpp>
 
-#include <System.h>
-#include <Viewer.h>
+#include <MORB_SLAM/System.h>
+#include <MORB_SLAM/Viewer.h>
 #include <condition_variable>
-#include "ImuTypes.h"
+#include "MORB_SLAM/ImuTypes.h"
 
 using namespace std;
 
@@ -67,8 +67,8 @@ int main(int argc, char **argv)
         // bFileName = true; // UNUSED
     }
 
-    ORB_SLAM3::System_ptr SLAM = std::make_shared<ORB_SLAM3::System>(argv[1],argv[2],ORB_SLAM3::CameraType::IMU_STEREO, file_name);
-    ORB_SLAM3::Viewer viewer(SLAM, argv[2]);
+    MORB_SLAM::System_ptr SLAM = std::make_shared<MORB_SLAM::System>(argv[1],argv[2],MORB_SLAM::CameraType::IMU_STEREO, file_name);
+    MORB_SLAM::Viewer viewer(SLAM, argv[2]);
     float imageScale = SLAM->GetImageScale();
 
     struct sigaction sigIntHandler;
@@ -196,7 +196,7 @@ int main(int argc, char **argv)
     }
 
     // Create SLAM system. It initializes all system threads and gets ready to process frames.
-    vector<ORB_SLAM3::IMU::Point> vImuMeas;
+    vector<MORB_SLAM::IMU::Point> vImuMeas;
 
     double timestamp;
     cv::Mat im,imright;
@@ -278,7 +278,7 @@ int main(int argc, char **argv)
 
 
         for(size_t i=0; i<vGyro.size(); ++i){
-            ORB_SLAM3::IMU::Point lastPoint(vAccel[i].x, vAccel[i].y, vAccel[i].z,
+            MORB_SLAM::IMU::Point lastPoint(vAccel[i].x, vAccel[i].y, vAccel[i].z,
                                             vGyro[i].x, vGyro[i].y, vGyro[i].z,
                                             vGyro_times[i]);
             vImuMeas.push_back(lastPoint);
diff --git a/Examples/Stereo-Inertial/stereo_inertial_tum_vi.cc b/Examples/Stereo-Inertial/stereo_inertial_tum_vi.cc
index ae068c40..4138a53f 100644
--- a/Examples/Stereo-Inertial/stereo_inertial_tum_vi.cc
+++ b/Examples/Stereo-Inertial/stereo_inertial_tum_vi.cc
@@ -25,9 +25,9 @@
 
 #include<opencv2/opencv.hpp>
 
-#include<System.h>
-#include<Viewer.h>
-#include "ImuTypes.h"
+#include<MORB_SLAM/System.h>
+#include<MORB_SLAM/Viewer.h>
+#include "MORB_SLAM/ImuTypes.h"
 
 using namespace std;
 
@@ -120,8 +120,8 @@ int main(int argc, char **argv)
     cout << "IMU data in the sequence: " << nImu << endl << endl;*/
 
     // Create SLAM system. It initializes all system threads and gets ready to process frames.
-    ORB_SLAM3::System_ptr SLAM = std::make_shared<ORB_SLAM3::System>(argv[1],argv[2],ORB_SLAM3::CameraType::IMU_STEREO, file_name);
-    ORB_SLAM3::Viewer viewer(SLAM, argv[2]);
+    MORB_SLAM::System_ptr SLAM = std::make_shared<MORB_SLAM::System>(argv[1],argv[2],MORB_SLAM::CameraType::IMU_STEREO, file_name);
+    MORB_SLAM::Viewer viewer(SLAM, argv[2]);
     float imageScale = SLAM->GetImageScale();
 
 #ifdef REGISTER_TIMES
@@ -135,7 +135,7 @@ int main(int argc, char **argv)
 
         // Main loop
         cv::Mat imLeft, imRight;
-        vector<ORB_SLAM3::IMU::Point> vImuMeas;
+        vector<MORB_SLAM::IMU::Point> vImuMeas;
         proccIm = 0;
         cv::Ptr<cv::CLAHE> clahe = cv::createCLAHE(3.0, cv::Size(8, 8));
         for(int ni=0; ni<nImages[seq]; ni++, proccIm++)
@@ -184,8 +184,8 @@ int main(int argc, char **argv)
 
                 while(vTimestampsImu[seq][first_imu[seq]]<=vTimestampsCam[seq][ni])
                 {
-                    // vImuMeas.push_back(ORB_SLAM3::IMU::Point(vAcc[first_imu],vGyro[first_imu],vTimestampsImu[first_imu]));
-                    vImuMeas.push_back(ORB_SLAM3::IMU::Point(vAcc[seq][first_imu[seq]].x,vAcc[seq][first_imu[seq]].y,vAcc[seq][first_imu[seq]].z,
+                    // vImuMeas.push_back(MORB_SLAM::IMU::Point(vAcc[first_imu],vGyro[first_imu],vTimestampsImu[first_imu]));
+                    vImuMeas.push_back(MORB_SLAM::IMU::Point(vAcc[seq][first_imu[seq]].x,vAcc[seq][first_imu[seq]].y,vAcc[seq][first_imu[seq]].z,
                                                              vGyro[seq][first_imu[seq]].x,vGyro[seq][first_imu[seq]].y,vGyro[seq][first_imu[seq]].z,
                                                              vTimestampsImu[seq][first_imu[seq]]));
                     // cout << "t_imu = " << fixed << vImuMeas.back().t << endl;
diff --git a/Examples/Stereo/stereo_euroc.cc b/Examples/Stereo/stereo_euroc.cc
index b1a7c422..f8e6235c 100644
--- a/Examples/Stereo/stereo_euroc.cc
+++ b/Examples/Stereo/stereo_euroc.cc
@@ -24,8 +24,8 @@
 
 #include<opencv2/opencv.hpp>
 
-#include<System.h>
-#include<Viewer.h>
+#include<MORB_SLAM/System.h>
+#include<MORB_SLAM/Viewer.h>
 
 using namespace std;
 
@@ -89,8 +89,8 @@ int main(int argc, char **argv)
     cout.precision(17);
 
     // Create SLAM system. It initializes all system threads and gets ready to process frames.
-    ORB_SLAM3::System_ptr SLAM = std::make_shared<ORB_SLAM3::System>(argv[1],argv[2],ORB_SLAM3::CameraType::STEREO);
-    ORB_SLAM3::Viewer viewer(SLAM, argv[2]);
+    MORB_SLAM::System_ptr SLAM = std::make_shared<MORB_SLAM::System>(argv[1],argv[2],MORB_SLAM::CameraType::STEREO);
+    MORB_SLAM::Viewer viewer(SLAM, argv[2]);
 
     cv::Mat imLeft, imRight;
     for (seq = 0; seq<num_seq; seq++)
@@ -129,7 +129,7 @@ int main(int argc, char **argv)
             std::chrono::steady_clock::time_point t1 = std::chrono::steady_clock::now();
 
             // Pass the images to the SLAM system
-            auto pos = SLAM->TrackStereo(imLeft,imRight,tframe, vector<ORB_SLAM3::IMU::Point>(), vstrImageLeft[seq][ni]);
+            auto pos = SLAM->TrackStereo(imLeft,imRight,tframe, vector<MORB_SLAM::IMU::Point>(), vstrImageLeft[seq][ni]);
 
             std::chrono::steady_clock::time_point t2 = std::chrono::steady_clock::now();
 
diff --git a/Examples/Stereo/stereo_kitti.cc b/Examples/Stereo/stereo_kitti.cc
index cd658cde..39e0d2e7 100644
--- a/Examples/Stereo/stereo_kitti.cc
+++ b/Examples/Stereo/stereo_kitti.cc
@@ -24,8 +24,8 @@
 
 #include<opencv2/opencv.hpp>
 
-#include<System.h>
-#include<Viewer.h>
+#include<MORB_SLAM/System.h>
+#include<MORB_SLAM/Viewer.h>
 
 using namespace std;
 
@@ -49,8 +49,8 @@ int main(int argc, char **argv)
     const int nImages = vstrImageLeft.size();
 
     // Create SLAM system. It initializes all system threads and gets ready to process frames.
-    ORB_SLAM3::System_ptr SLAM = std::make_shared<ORB_SLAM3::System>(argv[1],argv[2],ORB_SLAM3::CameraType::STEREO);
-    ORB_SLAM3::Viewer viewer(SLAM, argv[2]);
+    MORB_SLAM::System_ptr SLAM = std::make_shared<MORB_SLAM::System>(argv[1],argv[2],MORB_SLAM::CameraType::STEREO);
+    MORB_SLAM::Viewer viewer(SLAM, argv[2]);
 
     float imageScale = SLAM->GetImageScale();
 
diff --git a/Examples/Stereo/stereo_realsense_D435i.cc b/Examples/Stereo/stereo_realsense_D435i.cc
index c39d430c..698d74ee 100644
--- a/Examples/Stereo/stereo_realsense_D435i.cc
+++ b/Examples/Stereo/stereo_realsense_D435i.cc
@@ -19,8 +19,8 @@
  * ORB-SLAM3. If not, see <http://www.gnu.org/licenses/>.
  */
 
-#include <System.h>
-#include <Viewer.h>
+#include <MORB_SLAM/System.h>
+#include <MORB_SLAM/Viewer.h>
 #include <signal.h>
 #include <stdlib.h>
 
@@ -243,9 +243,9 @@ int main(int argc, char** argv) {
 
   // Create SLAM system. It initializes all system threads and gets ready to
   // process frames.
-  ORB_SLAM3::System_ptr SLAM = std::make_shared<ORB_SLAM3::System>(argv[1], argv[2], ORB_SLAM3::CameraType::STEREO, /*0,*/
+  MORB_SLAM::System_ptr SLAM = std::make_shared<MORB_SLAM::System>(argv[1], argv[2], MORB_SLAM::CameraType::STEREO, /*0,*/
                          file_name);
-  ORB_SLAM3::Viewer viewer(SLAM, argv[2]);
+  MORB_SLAM::Viewer viewer(SLAM, argv[2]);
   float imageScale = SLAM->GetImageScale();
 
   double timestamp;
diff --git a/Examples/Stereo/stereo_realsense_t265.cc b/Examples/Stereo/stereo_realsense_t265.cc
index b36d4c0b..4258b0b6 100644
--- a/Examples/Stereo/stereo_realsense_t265.cc
+++ b/Examples/Stereo/stereo_realsense_t265.cc
@@ -29,8 +29,8 @@
 
 #include <librealsense2/rs.hpp>
 
-#include <System.h>
-#include <Viewer.h>
+#include <MORB_SLAM/System.h>
+#include <MORB_SLAM/Viewer.h>
 
 using namespace std;
 
@@ -90,12 +90,12 @@ int main(int argc, char **argv)
     cout << "IMU data in the sequence: " << nImu << endl << endl;*/
 
     // Create SLAM system. It initializes all system threads and gets ready to process frames.
-    ORB_SLAM3::System_ptr SLAM = std::make_shared<ORB_SLAM3::System>(argv[1],argv[2],ORB_SLAM3::CameraType::STEREO, /*0, */file_name);
-    ORB_SLAM3::Viewer viewer(SLAM, argv[2]);
+    MORB_SLAM::System_ptr SLAM = std::make_shared<MORB_SLAM::System>(argv[1],argv[2],MORB_SLAM::CameraType::STEREO, /*0, */file_name);
+    MORB_SLAM::Viewer viewer(SLAM, argv[2]);
     float imageScale = SLAM->GetImageScale();
 
     cv::Mat imLeft, imRight;
-    vector<ORB_SLAM3::IMU::Point> vImuMeas;
+    vector<MORB_SLAM::IMU::Point> vImuMeas;
 
     rs2::stream_profile fisheye_stream_left = pipe_profile.get_stream(RS2_STREAM_FISHEYE, 1);
     rs2_intrinsics intrinsics_left = fisheye_stream_left.as<rs2::video_stream_profile>().get_intrinsics();
diff --git a/Examples/Stereo/stereo_tum_vi.cc b/Examples/Stereo/stereo_tum_vi.cc
index 85719981..dd64c5ff 100644
--- a/Examples/Stereo/stereo_tum_vi.cc
+++ b/Examples/Stereo/stereo_tum_vi.cc
@@ -25,8 +25,8 @@
 
 #include<opencv2/opencv.hpp>
 
-#include<System.h>
-#include<Viewer.h>
+#include<MORB_SLAM/System.h>
+#include<MORB_SLAM/Viewer.h>
 
 using namespace std;
 
@@ -91,8 +91,8 @@ int main(int argc, char **argv)
     cout.precision(17);
 
     // Create SLAM system. It initializes all system threads and gets ready to process frames.
-    ORB_SLAM3::System_ptr SLAM = std::make_shared<ORB_SLAM3::System>(argv[1],argv[2],ORB_SLAM3::CameraType::STEREO);
-    ORB_SLAM3::Viewer viewer(SLAM, argv[2]);
+    MORB_SLAM::System_ptr SLAM = std::make_shared<MORB_SLAM::System>(argv[1],argv[2],MORB_SLAM::CameraType::STEREO);
+    MORB_SLAM::Viewer viewer(SLAM, argv[2]);
     float imageScale = SLAM->GetImageScale();
 
     cout << endl << "-------" << endl;
diff --git a/README.md b/README.md
index e4907a44..3bda943d 100644
--- a/README.md
+++ b/README.md
@@ -2,7 +2,7 @@
 
 [![CMake Build](https://github.com/Soldann/MORB_SLAM/actions/workflows/cmake.yml/badge.svg)](https://github.com/Soldann/MORB_SLAM/actions/workflows/cmake.yml)
 
-This fork of [ORB_SLAM3](https://github.com/UZ-SLAMLab/ORB_SLAM3) converts it into a CMake package that can be imported into other projects. Run `morbslam_installer.sh` to install.
+This fork of [MORB_SLAM](https://github.com/UZ-SLAMLab/MORB_SLAM) converts it into a CMake package that can be imported into other projects. Run `morbslam_installer.sh` to install.
 
 In your other projects, import using:
 ```
@@ -17,7 +17,7 @@ TARGET_LINK_LIBRARIES(${PROJECT_NAME} MORB_SLAM::MORB_SLAM)
 ### V1.0, December 22th, 2021
 **Authors:** Carlos Campos, Richard Elvira, Juan J. Gómez Rodríguez, [José M. M. Montiel](http://webdiis.unizar.es/~josemari/), [Juan D. Tardos](http://webdiis.unizar.es/~jdtardos/).
 
-The [Changelog](https://github.com/UZ-SLAMLab/ORB_SLAM3/blob/master/Changelog.md) describes the features of each version.
+The [Changelog](https://github.com/UZ-SLAMLab/MORB_SLAM/blob/master/Changelog.md) describes the features of each version.
 
 ORB-SLAM3 is the first real-time SLAM library able to perform **Visual, Visual-Inertial and Multi-Map SLAM** with **monocular, stereo and RGB-D** cameras, using **pin-hole and fisheye** lens models. In all sensor configurations, ORB-SLAM3 is as robust as the best systems available in the literature, and significantly more accurate. 
 
@@ -46,7 +46,7 @@ alt="ORB-SLAM3" width="240" height="180" border="10" /></a>
 
 # 1. License
 
-ORB-SLAM3 is released under [GPLv3 license](https://github.com/UZ-SLAMLab/ORB_SLAM3/LICENSE). For a list of all code/library dependencies (and associated licenses), please see [Dependencies.md](https://github.com/UZ-SLAMLab/ORB_SLAM3/blob/master/Dependencies.md).
+ORB-SLAM3 is released under [GPLv3 license](https://github.com/UZ-SLAMLab/MORB_SLAM/LICENSE). For a list of all code/library dependencies (and associated licenses), please see [Dependencies.md](https://github.com/UZ-SLAMLab/MORB_SLAM/blob/master/Dependencies.md).
 
 For a closed-source version of ORB-SLAM3 for commercial purposes, please contact the authors: orbslam (at) unizar (dot) es.
 
@@ -97,12 +97,12 @@ We provide some examples to process input of a monocular, monocular-inertial, st
 
 Clone the repository:
 ```
-git clone https://github.com/UZ-SLAMLab/ORB_SLAM3.git ORB_SLAM3
+git clone https://github.com/UZ-SLAMLab/MORB_SLAM.git MORB_SLAM
 ```
 
 We provide a script `build.sh` to build the *Thirdparty* libraries and *ORB-SLAM3*. Please make sure you have installed all required dependencies (see section 2). Execute:
 ```
-cd ORB_SLAM3
+cd MORB_SLAM
 chmod +x build.sh
 ./build.sh
 ```
@@ -170,14 +170,14 @@ Execute the following script to process sequences and compute the RMS ATE:
 ### Building the nodes for mono, mono-inertial, stereo, stereo-inertial and RGB-D
 Tested with ROS Melodic and ubuntu 18.04.
 
-1. Add the path including *Examples/ROS/ORB_SLAM3* to the ROS_PACKAGE_PATH environment variable. Open .bashrc file:
+1. Add the path including *Examples/ROS/MORB_SLAM* to the ROS_PACKAGE_PATH environment variable. Open .bashrc file:
   ```
   gedit ~/.bashrc
   ```
-and add at the end the following line. Replace PATH by the folder where you cloned ORB_SLAM3:
+and add at the end the following line. Replace PATH by the folder where you cloned MORB_SLAM:
 
   ```
-  export ROS_PACKAGE_PATH=${ROS_PACKAGE_PATH}:PATH/ORB_SLAM3/Examples/ROS
+  export ROS_PACKAGE_PATH=${ROS_PACKAGE_PATH}:PATH/MORB_SLAM/Examples/ROS
   ```
   
 2. Execute `build_ros.sh` script:
@@ -188,38 +188,38 @@ and add at the end the following line. Replace PATH by the folder where you clon
   ```
   
 ### Running Monocular Node
-For a monocular input from topic `/camera/image_raw` run node ORB_SLAM3/Mono. You will need to provide the vocabulary file and a settings file. See the monocular examples above.
+For a monocular input from topic `/camera/image_raw` run node MORB_SLAM/Mono. You will need to provide the vocabulary file and a settings file. See the monocular examples above.
 
   ```
-  rosrun ORB_SLAM3 Mono PATH_TO_VOCABULARY PATH_TO_SETTINGS_FILE
+  rosrun MORB_SLAM Mono PATH_TO_VOCABULARY PATH_TO_SETTINGS_FILE
   ```
 
 ### Running Monocular-Inertial Node
-For a monocular input from topic `/camera/image_raw` and an inertial input from topic `/imu`, run node ORB_SLAM3/Mono_Inertial. Setting the optional third argument to true will apply CLAHE equalization to images (Mainly for TUM-VI dataset).
+For a monocular input from topic `/camera/image_raw` and an inertial input from topic `/imu`, run node MORB_SLAM/Mono_Inertial. Setting the optional third argument to true will apply CLAHE equalization to images (Mainly for TUM-VI dataset).
 
   ```
-  rosrun ORB_SLAM3 Mono PATH_TO_VOCABULARY PATH_TO_SETTINGS_FILE [EQUALIZATION]	
+  rosrun MORB_SLAM Mono PATH_TO_VOCABULARY PATH_TO_SETTINGS_FILE [EQUALIZATION]	
   ```
 
 ### Running Stereo Node
-For a stereo input from topic `/camera/left/image_raw` and `/camera/right/image_raw` run node ORB_SLAM3/Stereo. You will need to provide the vocabulary file and a settings file. For Pinhole camera model, if you **provide rectification matrices** (see Examples/Stereo/EuRoC.yaml example), the node will recitify the images online, **otherwise images must be pre-rectified**. For FishEye camera model, rectification is not required since system works with original images:
+For a stereo input from topic `/camera/left/image_raw` and `/camera/right/image_raw` run node MORB_SLAM/Stereo. You will need to provide the vocabulary file and a settings file. For Pinhole camera model, if you **provide rectification matrices** (see Examples/Stereo/EuRoC.yaml example), the node will recitify the images online, **otherwise images must be pre-rectified**. For FishEye camera model, rectification is not required since system works with original images:
 
   ```
-  rosrun ORB_SLAM3 Stereo PATH_TO_VOCABULARY PATH_TO_SETTINGS_FILE ONLINE_RECTIFICATION
+  rosrun MORB_SLAM Stereo PATH_TO_VOCABULARY PATH_TO_SETTINGS_FILE ONLINE_RECTIFICATION
   ```
 
 ### Running Stereo-Inertial Node
-For a stereo input from topics `/camera/left/image_raw` and `/camera/right/image_raw`, and an inertial input from topic `/imu`, run node ORB_SLAM3/Stereo_Inertial. You will need to provide the vocabulary file and a settings file, including rectification matrices if required in a similar way to Stereo case:
+For a stereo input from topics `/camera/left/image_raw` and `/camera/right/image_raw`, and an inertial input from topic `/imu`, run node MORB_SLAM/Stereo_Inertial. You will need to provide the vocabulary file and a settings file, including rectification matrices if required in a similar way to Stereo case:
 
   ```
-  rosrun ORB_SLAM3 Stereo_Inertial PATH_TO_VOCABULARY PATH_TO_SETTINGS_FILE ONLINE_RECTIFICATION [EQUALIZATION]	
+  rosrun MORB_SLAM Stereo_Inertial PATH_TO_VOCABULARY PATH_TO_SETTINGS_FILE ONLINE_RECTIFICATION [EQUALIZATION]	
   ```
   
 ### Running RGB_D Node
-For an RGB-D input from topics `/camera/rgb/image_raw` and `/camera/depth_registered/image_raw`, run node ORB_SLAM3/RGBD. You will need to provide the vocabulary file and a settings file. See the RGB-D example above.
+For an RGB-D input from topics `/camera/rgb/image_raw` and `/camera/depth_registered/image_raw`, run node MORB_SLAM/RGBD. You will need to provide the vocabulary file and a settings file. See the RGB-D example above.
 
   ```
-  rosrun ORB_SLAM3 RGBD PATH_TO_VOCABULARY PATH_TO_SETTINGS_FILE
+  rosrun MORB_SLAM RGBD PATH_TO_VOCABULARY PATH_TO_SETTINGS_FILE
   ```
 
 **Running ROS example:** Download a rosbag (e.g. V1_02_medium.bag) from the EuRoC dataset (http://projects.asl.ethz.ch/datasets/doku.php?id=kmavvisualinertialdatasets). Open 3 tabs on the terminal and run the following command at each tab for a Stereo-Inertial configuration:
@@ -228,7 +228,7 @@ For an RGB-D input from topics `/camera/rgb/image_raw` and `/camera/depth_regist
   ```
   
   ```
-  rosrun ORB_SLAM3 Stereo_Inertial Vocabulary/ORBvoc.txt Examples/Stereo-Inertial/EuRoC.yaml true
+  rosrun MORB_SLAM Stereo_Inertial Vocabulary/ORBvoc.txt Examples/Stereo-Inertial/EuRoC.yaml true
   ```
   
   ```
diff --git a/build.sh b/build.sh
index 911991c3..e95d3111 100755
--- a/build.sh
+++ b/build.sh
@@ -41,7 +41,7 @@ if [ ! -f "ORBvoc.txt" ]; then
 fi
 cd ..
 
-echo "Configuring and building ORB_SLAM3 ..."
+echo "Configuring and building MORB_SLAM ..."
 
 mkdir build 2> /dev/null
 cd build
diff --git a/build_ros.sh b/build_ros.sh
index 1f13d215..886b5b1c 100755
--- a/build_ros.sh
+++ b/build_ros.sh
@@ -1,6 +1,6 @@
 echo "Building ROS nodes"
 
-cd Examples/ROS/ORB_SLAM3
+cd Examples/ROS/MORB_SLAM
 mkdir build
 cd build
 cmake .. -DROS_BUILD_TYPE=Release
diff --git a/clean.sh b/clean.sh
index c7a11891..d1b72f2d 100755
--- a/clean.sh
+++ b/clean.sh
@@ -40,18 +40,18 @@ rm Examples/RGB-D/rgbd_tum \
     Examples/Stereo-Inertial/stereo_inertial_tum_vi \
     Examples/Stereo-Inertial/stereo_inertial_realsense_t265 \
     Examples/Stereo-Inertial/stereo_inertial_realsense_D435i \
-    Examples/ROS/ORB_SLAM3/Mono \
-    Examples/ROS/ORB_SLAM3/Mono_Inertial \
-    Examples/ROS/ORB_SLAM3/MonoAR \
-    Examples/ROS/ORB_SLAM3/RGBD \
-    Examples/ROS/ORB_SLAM3/Stereo \
-    Examples/ROS/ORB_SLAM3/Stereo_Inertial \
+    Examples/ROS/MORB_SLAM/Mono \
+    Examples/ROS/MORB_SLAM/Mono_Inertial \
+    Examples/ROS/MORB_SLAM/MonoAR \
+    Examples/ROS/MORB_SLAM/RGBD \
+    Examples/ROS/MORB_SLAM/Stereo \
+    Examples/ROS/MORB_SLAM/Stereo_Inertial \
     Examples/Calibration/recorder_realsense_D435i \
     Examples/Calibration/recorder_realsense_T265 \
     Examples/Tests/viewer_dataset \
     Examples/Tests/sophus_test \
     2> /dev/null
 
-rm -r Examples/ROS/ORB_SLAM3/build 2> /dev/null
+rm -r Examples/ROS/MORB_SLAM/build 2> /dev/null
 
 echo cleaning complete
\ No newline at end of file
diff --git a/include/Atlas.h b/include/Atlas.h
deleted file mode 100644
index 25086845..00000000
--- a/include/Atlas.h
+++ /dev/null
@@ -1,163 +0,0 @@
-/**
- * This file is part of ORB-SLAM3
- *
- * Copyright (C) 2017-2021 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/>.
- */
-
-#pragma once
-
-#include <boost/serialization/export.hpp>
-#include <boost/serialization/vector.hpp>
-#include <boost/serialization/shared_ptr.hpp>
-#include <mutex>
-#include <set>
-#include <map>
-#include <vector>
-
-#include "GeometricCamera.h"
-#include "KannalaBrandt8.h"
-#include "KeyFrame.h"
-#include "Map.h"
-#include "MapPoint.h"
-#include "Pinhole.h"
-
-namespace ORB_SLAM3 {
-class Map;
-class MapPoint;
-class KeyFrame;
-class KeyFrameDatabase;
-class Frame;
-class KannalaBrandt8;
-class Pinhole;
-
-// BOOST_CLASS_EXPORT_GUID(Pinhole, "Pinhole")
-// BOOST_CLASS_EXPORT_GUID(KannalaBrandt8, "KannalaBrandt8")
-
-class Atlas {
-  friend class boost::serialization::access;
-
-  template <class Archive>
-  void serialize(Archive& ar, const unsigned int version) {
-    ar.template register_type<Pinhole>();
-    ar.template register_type<KannalaBrandt8>();
-
-    // Save/load a set structure, the set structure is broken in libboost 1.58
-    // for ubuntu 16.04, a vector is serializated
-    // ar & mspMaps;
-    ar& mvpBackupMaps;
-    ar& mvpCameras;
-    // Need to save/load the static Id from Frame, KeyFrame, MapPoint and Map
-    ar& Map::nNextId;
-    ar& Frame::nNextId;
-    ar& KeyFrame::nNextId;
-    ar& MapPoint::nNextId;
-    ar& GeometricCamera::nNextId;
-    ar& mnLastInitKFidMap;
-  }
-
- public:
-  
-
-  Atlas();
-  Atlas(int initKFid);  // When its initialization the first map is created
-  ~Atlas();
-
-  void CreateNewMap();
-  void ChangeMap(std::shared_ptr<Map> pMap);
-
-  unsigned long int GetLastInitKFid();
-
-  // Method for change components in the current map
-  void AddKeyFrame(KeyFrame* pKF);
-  void AddMapPoint(MapPoint* pMP);
-  // void EraseMapPoint(MapPoint* pMP);
-  // void EraseKeyFrame(KeyFrame* pKF);
-
-  std::shared_ptr<GeometricCamera> AddCamera(const std::shared_ptr<GeometricCamera> &pCam);
-  std::vector<std::shared_ptr<GeometricCamera>> GetAllCameras();
-
-  /* All methods without Map pointer work on current map */
-  void SetReferenceMapPoints(const std::vector<MapPoint*>& vpMPs);
-  void InformNewBigChange();
-  int GetLastBigChangeIdx();
-
-  long unsigned int MapPointsInMap();
-  long unsigned KeyFramesInMap();
-
-  // Method for get data in current map
-  std::vector<KeyFrame*> GetAllKeyFrames();
-  std::vector<MapPoint*> GetAllMapPoints();
-  std::vector<MapPoint*> GetReferenceMapPoints();
-
-  std::vector<std::shared_ptr<Map>> GetAllMaps();
-
-  int CountMaps();
-
-  void clearMap();
-
-  void clearAtlas();
-
-  std::shared_ptr<Map> GetCurrentMap(System* sys = nullptr);
-
-  void SetMapBad(std::shared_ptr<Map> pMap);
-  void RemoveBadMaps();
-
-  bool isInertial();
-  void SetInertialSensor();
-  void SetImuInitialized();
-  bool isImuInitialized();
-
-  // Function for garantee the correction of serialization of this object
-  void PreSave();
-  void PostLoad();
-
-  std::map<long unsigned int, KeyFrame*> GetAtlasKeyframes();
-
-  void SetKeyFrameDababase(KeyFrameDatabase* pKFDB);
-  KeyFrameDatabase* GetKeyFrameDatabase();
-
-  void SetORBVocabulary(ORBVocabulary* pORBVoc);
-  ORBVocabulary* GetORBVocabulary();
-
-  long unsigned int GetNumLivedKF();
-
-  long unsigned int GetNumLivedMP();
-
- protected:
-  std::set<std::shared_ptr<Map>> mspMaps;
-  std::set<std::shared_ptr<Map>> mspBadMaps;
-  // Its necessary change the container from set to vector because libboost 1.58
-  // and Ubuntu 16.04 have an error with this cointainer
-  std::vector<std::shared_ptr<Map>> mvpBackupMaps;
-
-  std::shared_ptr<Map> mpCurrentMap;
-
-  std::vector<std::shared_ptr<GeometricCamera>> mvpCameras;
-
-  unsigned long int mnLastInitKFidMap;
-
-  // Class references for the map reconstruction from the save file
-  KeyFrameDatabase* mpKeyFrameDB;
-  ORBVocabulary* mpORBVocabulary;
-
-  // Mutex
-  std::mutex mMutexAtlas;
-
-};  // class Atlas
-
-}  // namespace ORB_SLAM3
diff --git a/include/Camera.hpp b/include/Camera.hpp
deleted file mode 100644
index 416f71e0..00000000
--- a/include/Camera.hpp
+++ /dev/null
@@ -1,92 +0,0 @@
-#pragma once
-#include <thread>
-#include <string>
-#include "ImprovedTypes.hpp"
-#include <mutex>
-#include <condition_variable>
-#include <future>
-#include <functional>
-#include <memory>
-#include <exception>
-#include <stdexcept>
-#include <deque>
-#include <utility>
-
-namespace ORB_SLAM3{
-
-
-template<typename T>
-class ManagedFuture{
-protected:
-    std::shared_ptr<std::promise<T>> promise;
-    std::shared_future<T> future;
-
-   ManagedFuture(): promise{std::make_shared<std::promise<T>>()}, future{promise->get_future()} {} // make default constructor protected
-public:
-    virtual ~ManagedFuture(){}
-
-    // functions from std::shared_future<T>
-
-    const T& get() const { return future.get(); }
-    void wait() const { future.wait(); }
-
-    template<class Rep, class Period>
-    std::future_status wait_for( const std::chrono::duration<Rep,Period>& timeout_duration ) const { return future.wait_for(timeout_duration); }
-
-    template<class Clock, class Duration>
-    std::future_status wait_until( const std::chrono::time_point<Clock,Duration>& timeout_time ) const { return future.wait_until(timeout_time); }
-};
-
-template<typename T>
-class ManagedPromise: public ManagedFuture<T>{
-    std::shared_ptr<std::mutex> mutex;
-public:
-    ManagedPromise(): mutex{std::make_shared<std::mutex>()} {}
-    virtual ~ManagedPromise(){}
-    
-    // functions from std::promise<T>
-
-    void set_value_at_thread_exit(const T& value){ std::scoped_lock<std::mutex> lock(*mutex); ManagedFuture<T>::promise->set_value_at_thread_exit(value); }
-    void set_value_at_thread_exit(T&& value){ std::scoped_lock<std::mutex> lock(*mutex); ManagedFuture<T>::promise->set_value_at_thread_exit(value); }
-    void set_value_at_thread_exit(T& value){ std::scoped_lock<std::mutex> lock(*mutex); ManagedFuture<T>::promise->set_value_at_thread_exit(value); }
-    void set_value_at_thread_exit(){ std::scoped_lock<std::mutex> lock(*mutex); ManagedFuture<T>::promise->set_value_at_thread_exit(); }
-    void set_exception_at_thread_exit(const std::exception_ptr &p ){ std::scoped_lock<std::mutex> lock(*mutex); ManagedFuture<T>::promise->set_exception_at_thread_exit(p); }
-    void set_exception(const std::exception_ptr &p ){ std::scoped_lock<std::mutex> lock(*mutex); ManagedFuture<T>::promise->set_exception(p); }
-    void set_value(const T& value){ std::scoped_lock<std::mutex> lock(*mutex); ManagedFuture<T>::promise->set_value(value); }
-    void set_value(T&& value){ std::scoped_lock<std::mutex> lock(*mutex); ManagedFuture<T>::promise->set_value(value); }
-    void set_value(T& value){ std::scoped_lock<std::mutex> lock(*mutex); ManagedFuture<T>::promise->set_value(value); }
-    void set_value(){ std::scoped_lock<std::mutex> lock(*mutex); ManagedFuture<T>::promise->set_value(); }
-};
-
-class Camera{
-    std::deque<std::pair<ManagedPromise<bool>, std::function<void(void)>>> ljobs;
-    std::deque<std::pair<ManagedPromise<bool>, std::function<void(void)>>> rjobs;
-
-    std::string name;
-    CameraType::eSensor type;
-    std::mutex camMutex;
-    
-    std::condition_variable camCV;
-
-    bool shouldStop;
-    
-    std::thread lthread;
-    std::thread rthread;
-
-    void threadExec(std::deque<std::pair<ManagedPromise<bool>, std::function<void(void)>>> *jobs);
-public:
-    Camera(CameraType::eSensor type, const std::string &name="camera");
-    virtual ~Camera();
-
-    ManagedFuture<bool> queue(std::function<void(void)> func, bool isLeft=true);
-    ManagedFuture<bool> queueLeft(const std::function<void(void)> &func);
-    ManagedFuture<bool> queueRight(const std::function<void(void)> &func);
-
-    CameraType::eSensor getType() const;
-    const std::string &getName() const;
-};
-
-typedef std::shared_ptr<Camera> Camera_ptr;
-typedef std::weak_ptr<Camera> Camera_wptr;
-
-} // namespace ORB_SLAM3
\ No newline at end of file
diff --git a/include/CameraModels/GeometricCamera.h b/include/CameraModels/GeometricCamera.h
deleted file mode 100644
index 553d63ca..00000000
--- a/include/CameraModels/GeometricCamera.h
+++ /dev/null
@@ -1,115 +0,0 @@
-/**
- * This file is part of ORB-SLAM3
- *
- * Copyright (C) 2017-2021 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/>.
- */
-
-#pragma once
-
-#include <Eigen/Geometry>
-#include <boost/serialization/access.hpp>
-#include <boost/serialization/assume_abstract.hpp>
-#include <boost/serialization/base_object.hpp>
-#include <boost/serialization/export.hpp>
-#include <boost/serialization/serialization.hpp>
-#include <boost/serialization/vector.hpp>
-#include <opencv2/core/core.hpp>
-#include <opencv2/features2d/features2d.hpp>
-#include <opencv2/imgproc/imgproc.hpp>
-#include <sophus/se3.hpp>
-#include <vector>
-
-#include "Converter.h"
-#include "GeometricTools.h"
-
-namespace ORB_SLAM3 {
-class GeometricCamera {
-  friend class boost::serialization::access;
-
-  template <class Archive>
-  void serialize(Archive& ar, const unsigned int version) {
-    ar& mnId;
-    ar& mnType;
-    ar& mvParameters;
-  }
-
- public:
-  GeometricCamera() {}
-  GeometricCamera(const std::vector<float>& _vParameters)
-      : mvParameters(_vParameters) {}
-  ~GeometricCamera() {}
-
-  virtual cv::Point2f project(const cv::Point3f& p3D) const = 0;
-  virtual Eigen::Vector2d project(const Eigen::Vector3d& v3D) const = 0;
-  virtual Eigen::Vector2f project(const Eigen::Vector3f& v3D) const = 0;
-  virtual Eigen::Vector2f projectMat(const cv::Point3f& p3D) const = 0;
-
-  virtual float uncertainty2(const Eigen::Matrix<double, 2, 1>& p2D) = 0;
-
-  virtual Eigen::Vector3f unprojectEig(const cv::Point2f& p2D) const = 0;
-  virtual cv::Point3f unproject(const cv::Point2f& p2D) const = 0;
-
-  virtual Eigen::Matrix<double, 2, 3> projectJac(
-      const Eigen::Vector3d& v3D) = 0;
-
-  virtual bool ReconstructWithTwoViews(const std::vector<cv::KeyPoint>& vKeys1,
-                                       const std::vector<cv::KeyPoint>& vKeys2,
-                                       const std::vector<int>& vMatches12,
-                                       Sophus::SE3f& T21,
-                                       std::vector<cv::Point3f>& vP3D,
-                                       std::vector<bool>& vbTriangulated) = 0;
-
-  virtual cv::Mat toK() const = 0;
-  virtual Eigen::Matrix3f toK_() const = 0;
-
-  virtual bool epipolarConstrain(const std::shared_ptr<GeometricCamera> &otherCamera,
-                                 const cv::KeyPoint& kp1,
-                                 const cv::KeyPoint& kp2,
-                                 const Eigen::Matrix3f& R12,
-                                 const Eigen::Vector3f& t12,
-                                 const float sigmaLevel, const float unc) = 0;
-
-  float getParameter(const int i) { return mvParameters[i]; }
-  void setParameter(const float p, const size_t i) { mvParameters[i] = p; }
-
-  size_t size() { return mvParameters.size(); }
-
-  virtual bool matchAndtriangulate(const cv::KeyPoint& kp1,
-                                   const cv::KeyPoint& kp2,
-                                   GeometricCamera* pOther, Sophus::SE3f& Tcw1,
-                                   Sophus::SE3f& Tcw2, const float sigmaLevel1,
-                                   const float sigmaLevel2,
-                                   Eigen::Vector3f& x3Dtriangulated) = 0;
-
-  unsigned int GetId() { return mnId; }
-
-  unsigned int GetType() { return mnType; }
-
-  const static unsigned int CAM_PINHOLE = 0;
-  const static unsigned int CAM_FISHEYE = 1;
-
-  static long unsigned int nNextId;
-
- protected:
-  std::vector<float> mvParameters;
-
-  unsigned int mnId;
-
-  unsigned int mnType;
-};
-}  // namespace ORB_SLAM3
diff --git a/include/CameraModels/KannalaBrandt8.h b/include/CameraModels/KannalaBrandt8.h
deleted file mode 100644
index 05028ac1..00000000
--- a/include/CameraModels/KannalaBrandt8.h
+++ /dev/null
@@ -1,134 +0,0 @@
-/**
- * This file is part of ORB-SLAM3
- *
- * Copyright (C) 2017-2021 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/>.
- */
-
-#pragma once
-
-#include <assert.h>
-#include <memory>
-#include "GeometricCamera.h"
-#include "TwoViewReconstruction.h"
-
-namespace ORB_SLAM3 {
-class KannalaBrandt8 : public GeometricCamera {
-  friend class boost::serialization::access;
-
-  template <class Archive>
-  void serialize(Archive& ar, const unsigned int version) {
-    ar& boost::serialization::base_object<GeometricCamera>(*this);
-    ar& const_cast<float&>(precision);
-  }
-
- public:
-  KannalaBrandt8() : precision(1e-6) {
-    mvParameters.resize(8);
-    mnId = nNextId++;
-    mnType = CAM_FISHEYE;
-  }
-  KannalaBrandt8(const std::vector<float> _vParameters)
-      : GeometricCamera(_vParameters),
-        mvLappingArea(2, 0),
-        precision(1e-6),
-        tvr(nullptr) {
-    assert(mvParameters.size() == 8);
-    mnId = nNextId++;
-    mnType = CAM_FISHEYE;
-  }
-
-  KannalaBrandt8(const std::vector<float> _vParameters, const float _precision)
-      : GeometricCamera(_vParameters),
-        mvLappingArea(2, 0),
-        precision(_precision) {
-    assert(mvParameters.size() == 8);
-    mnId = nNextId++;
-    mnType = CAM_FISHEYE;
-  }
-  KannalaBrandt8(KannalaBrandt8* pKannala)
-      : GeometricCamera(pKannala->mvParameters),
-        mvLappingArea(2, 0),
-        precision(pKannala->precision),
-        tvr(nullptr) {
-    assert(mvParameters.size() == 8);
-    mnId = nNextId++;
-    mnType = CAM_FISHEYE;
-  }
-
-  cv::Point2f project(const cv::Point3f& p3D) const;
-  Eigen::Vector2d project(const Eigen::Vector3d& v3D) const;
-  Eigen::Vector2f project(const Eigen::Vector3f& v3D) const;
-  Eigen::Vector2f projectMat(const cv::Point3f& p3D) const;
-
-  float uncertainty2(const Eigen::Matrix<double, 2, 1>& p2D);
-
-  Eigen::Vector3f unprojectEig(const cv::Point2f& p2D) const;
-  cv::Point3f unproject(const cv::Point2f& p2D) const;
-
-  Eigen::Matrix<double, 2, 3> projectJac(const Eigen::Vector3d& v3D);
-
-  bool ReconstructWithTwoViews(const std::vector<cv::KeyPoint>& vKeys1,
-                               const std::vector<cv::KeyPoint>& vKeys2,
-                               const std::vector<int>& vMatches12,
-                               Sophus::SE3f& T21,
-                               std::vector<cv::Point3f>& vP3D,
-                               std::vector<bool>& vbTriangulated);
-
-  cv::Mat toK() const;
-  Eigen::Matrix3f toK_() const;
-
-  bool epipolarConstrain(const std::shared_ptr<GeometricCamera> &pCamera2, const cv::KeyPoint& kp1,
-                         const cv::KeyPoint& kp2, const Eigen::Matrix3f& R12,
-                         const Eigen::Vector3f& t12, const float sigmaLevel,
-                         const float unc);
-
-  float TriangulateMatches(const std::shared_ptr<GeometricCamera> &pCamera2, const cv::KeyPoint& kp1,
-                           const cv::KeyPoint& kp2, const Eigen::Matrix3f& R12,
-                           const Eigen::Vector3f& t12, const float sigmaLevel,
-                           const float unc, Eigen::Vector3f& p3D);
-
-  std::vector<int> mvLappingArea;
-
-  bool matchAndtriangulate(const cv::KeyPoint& kp1, const cv::KeyPoint& kp2,
-                           GeometricCamera* pOther, Sophus::SE3f& Tcw1,
-                           Sophus::SE3f& Tcw2, const float sigmaLevel1,
-                           const float sigmaLevel2,
-                           Eigen::Vector3f& x3Dtriangulated);
-
-  friend std::ostream& operator<<(std::ostream& os, const KannalaBrandt8& kb);
-  friend std::istream& operator>>(std::istream& is, KannalaBrandt8& kb);
-
-  float GetPrecision() { return precision; }
-
-  bool IsEqual(const std::shared_ptr<GeometricCamera> &pCam);
-
- private:
-  const float precision;
-
-  // Parameters vector corresponds to
-  //[fx, fy, cx, cy, k0, k1, k2, k3]
-
-  TwoViewReconstruction* tvr;
-
-  void Triangulate(const cv::Point2f& p1, const cv::Point2f& p2,
-                   const Eigen::Matrix<float, 3, 4>& Tcw1,
-                   const Eigen::Matrix<float, 3, 4>& Tcw2,
-                   Eigen::Vector3f& x3D);
-};
-}  // namespace ORB_SLAM3
-
diff --git a/include/CameraModels/Pinhole.h b/include/CameraModels/Pinhole.h
deleted file mode 100644
index f335ee74..00000000
--- a/include/CameraModels/Pinhole.h
+++ /dev/null
@@ -1,110 +0,0 @@
-/**
- * This file is part of ORB-SLAM3
- *
- * Copyright (C) 2017-2021 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/>.
- */
-
-#pragma once
-
-#include <assert.h>
-#include <memory>
-#include "GeometricCamera.h"
-#include "TwoViewReconstruction.h"
-
-namespace ORB_SLAM3 {
-class Pinhole : public GeometricCamera {
-  friend class boost::serialization::access;
-
-  template <class Archive>
-  void serialize(Archive& ar, const unsigned int version) {
-    ar& boost::serialization::base_object<GeometricCamera>(*this);
-  }
-
- public:
-  Pinhole() {
-    mvParameters.resize(4);
-    mnId = nNextId++;
-    mnType = CAM_PINHOLE;
-  }
-  Pinhole(const std::vector<float> _vParameters)
-      : GeometricCamera(_vParameters), tvr(nullptr) {
-    assert(mvParameters.size() == 4);
-    mnId = nNextId++;
-    mnType = CAM_PINHOLE;
-  }
-
-  Pinhole(Pinhole* pPinhole)
-      : GeometricCamera(pPinhole->mvParameters), tvr(nullptr) {
-    assert(mvParameters.size() == 4);
-    mnId = nNextId++;
-    mnType = CAM_PINHOLE;
-  }
-
-  ~Pinhole() {
-    if (tvr) delete tvr;
-  }
-
-  cv::Point2f project(const cv::Point3f& p3D) const;
-  Eigen::Vector2d project(const Eigen::Vector3d& v3D) const;
-  Eigen::Vector2f project(const Eigen::Vector3f& v3D) const;
-  Eigen::Vector2f projectMat(const cv::Point3f& p3D) const;
-
-  float uncertainty2(const Eigen::Matrix<double, 2, 1>& p2D);
-
-  Eigen::Vector3f unprojectEig(const cv::Point2f& p2D) const;
-  cv::Point3f unproject(const cv::Point2f& p2D) const;
-
-  Eigen::Matrix<double, 2, 3> projectJac(const Eigen::Vector3d& v3D);
-
-  bool ReconstructWithTwoViews(const std::vector<cv::KeyPoint>& vKeys1,
-                               const std::vector<cv::KeyPoint>& vKeys2,
-                               const std::vector<int>& vMatches12,
-                               Sophus::SE3f& T21,
-                               std::vector<cv::Point3f>& vP3D,
-                               std::vector<bool>& vbTriangulated);
-
-  cv::Mat toK() const;
-  Eigen::Matrix3f toK_()const;
-
-  bool epipolarConstrain(const std::shared_ptr<GeometricCamera> &pCamera2, const cv::KeyPoint& kp1,
-                         const cv::KeyPoint& kp2, const Eigen::Matrix3f& R12,
-                         const Eigen::Vector3f& t12, const float sigmaLevel,
-                         const float unc);
-
-  bool matchAndtriangulate(const cv::KeyPoint& kp1, const cv::KeyPoint& kp2,
-                           GeometricCamera* pOther, Sophus::SE3f& Tcw1,
-                           Sophus::SE3f& Tcw2, const float sigmaLevel1,
-                           const float sigmaLevel2,
-                           Eigen::Vector3f& x3Dtriangulated) {
-    return false;
-  }
-
-  friend std::ostream& operator<<(std::ostream& os, const Pinhole& ph);
-  friend std::istream& operator>>(std::istream& os, Pinhole& ph);
-
-  bool IsEqual(const std::shared_ptr<GeometricCamera> &pCam);
-
- private:
-  // Parameters vector corresponds to
-  //      [fx, fy, cx, cy]
-  TwoViewReconstruction* tvr;
-};
-}  // namespace ORB_SLAM3
-
-// BOOST_CLASS_EXPORT_KEY(ORBSLAM2::Pinhole)
-
diff --git a/include/Converter.h b/include/Converter.h
deleted file mode 100644
index 0978d7d9..00000000
--- a/include/Converter.h
+++ /dev/null
@@ -1,78 +0,0 @@
-/**
- * This file is part of ORB-SLAM3
- *
- * Copyright (C) 2017-2021 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/>.
- */
-
-#pragma once
-
-#include <Eigen/Dense>
-#include <opencv2/core/core.hpp>
-
-#include "g2o/types/types_seven_dof_expmap.h"
-#include "g2o/types/types_six_dof_expmap.h"
-#include "sophus/geometry.hpp"
-#include "sophus/sim3.hpp"
-
-namespace ORB_SLAM3 {
-
-class Converter {
- public:
-  
-  static std::vector<cv::Mat> toDescriptorVector(const cv::Mat &Descriptors);
-
-  static g2o::SE3Quat toSE3Quat(const cv::Mat &cvT);
-  static g2o::SE3Quat toSE3Quat(const Sophus::SE3f &T);
-  static g2o::SE3Quat toSE3Quat(const g2o::Sim3 &gSim3);
-
-  // TODO templetize these functions
-  static cv::Mat toCvMat(const g2o::SE3Quat &SE3);
-  static cv::Mat toCvMat(const g2o::Sim3 &Sim3);
-  static cv::Mat toCvMat(const Eigen::Matrix<double, 4, 4> &m);
-  static cv::Mat toCvMat(const Eigen::Matrix<float, 4, 4> &m);
-  static cv::Mat toCvMat(const Eigen::Matrix<float, 3, 4> &m);
-  static cv::Mat toCvMat(const Eigen::Matrix3d &m);
-  static cv::Mat toCvMat(const Eigen::Matrix<double, 3, 1> &m);
-  static cv::Mat toCvMat(const Eigen::Matrix<float, 3, 1> &m);
-  static cv::Mat toCvMat(const Eigen::Matrix<float, 3, 3> &m);
-
-  static cv::Mat toCvMat(const Eigen::MatrixXf &m);
-  static cv::Mat toCvMat(const Eigen::MatrixXd &m);
-
-  static cv::Mat toCvSE3(const Eigen::Matrix<double, 3, 3> &R,
-                         const Eigen::Matrix<double, 3, 1> &t);
-  static cv::Mat tocvSkewMatrix(const cv::Mat &v);
-
-  static Eigen::Matrix<double, 3, 1> toVector3d(const cv::Mat &cvVector);
-  static Eigen::Matrix<float, 3, 1> toVector3f(const cv::Mat &cvVector);
-  static Eigen::Matrix<double, 3, 1> toVector3d(const cv::Point3f &cvPoint);
-  static Eigen::Matrix<double, 3, 3> toMatrix3d(const cv::Mat &cvMat3);
-  static Eigen::Matrix<double, 4, 4> toMatrix4d(const cv::Mat &cvMat4);
-  static Eigen::Matrix<float, 3, 3> toMatrix3f(const cv::Mat &cvMat3);
-  static Eigen::Matrix<float, 4, 4> toMatrix4f(const cv::Mat &cvMat4);
-  static std::vector<float> toQuaternion(const cv::Mat &M);
-
-  static bool isRotationMatrix(const cv::Mat &R);
-  static std::vector<float> toEuler(const cv::Mat &R);
-
-  // TODO: Sophus migration, to be deleted in the future
-  static Sophus::SE3<float> toSophus(const cv::Mat &T);
-  static Sophus::Sim3f toSophus(const g2o::Sim3 &S);
-};
-
-}  // namespace ORB_SLAM3
diff --git a/include/Frame.h b/include/Frame.h
deleted file mode 100644
index 639e819a..00000000
--- a/include/Frame.h
+++ /dev/null
@@ -1,376 +0,0 @@
-/**
-* This file is part of ORB-SLAM3
-*
-* Copyright (C) 2017-2021 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/>.
-*/
-
-
-#pragma once
-
-#include<vector>
-
-#include "DBoW2/BowVector.h"
-#include "DBoW2/FeatureVector.h"
-
-#include "sophus/geometry.hpp"
-
-#include "ImuTypes.h"
-#include "ORBVocabulary.h"
-
-#include "Converter.h"
-#include "Settings.h"
-
-#include <mutex>
-#include <opencv2/opencv.hpp>
-
-#include "Eigen/Core"
-#include "sophus/se3.hpp"
-
-#include "Camera.hpp"
-
-namespace ORB_SLAM3
-{
-#define FRAME_GRID_ROWS 48
-#define FRAME_GRID_COLS 64
-
-class MapPoint;
-class KeyFrame;
-class ConstraintPoseImu;
-class GeometricCamera;
-class ORBextractor;
-
-class Frame
-{
-public:
-    Frame();
-
-    // Copy constructor.
-    Frame(const Frame &frame);
-
-    // Constructor for stereo cameras.
-    Frame(const Camera_ptr &cam, const cv::Mat &imLeft, const cv::Mat &imRight, const double &timeStamp, const std::shared_ptr<ORBextractor> &extractorLeft, const std::shared_ptr<ORBextractor> &extractorRight, ORBVocabulary* voc, cv::Mat &K, cv::Mat &distCoef, const float &bf, const float &thDepth, const std::shared_ptr<GeometricCamera> &pCamera, const std::string &pNameFile, int pnNumDataset, Frame* pPrevF = nullptr, const IMU::Calib &ImuCalib = IMU::Calib());
-
-    // Constructor for RGB-D cameras.
-    Frame(const Camera_ptr &cam, const cv::Mat &imGray, const cv::Mat &imDepth, const double &timeStamp, const std::shared_ptr<ORBextractor> &extractor,ORBVocabulary* voc, cv::Mat &K, cv::Mat &distCoef, const float &bf, const float &thDepth, const std::shared_ptr<GeometricCamera> &pCamera, const std::string &pNameFile, int pnNumDataset, Frame* pPrevF = nullptr, const IMU::Calib &ImuCalib = IMU::Calib());
-
-    // Constructor for Monocular cameras.
-    Frame(const Camera_ptr &cam, const cv::Mat &imGray, const double &timeStamp, const std::shared_ptr<ORBextractor> &extractor,ORBVocabulary* voc, const std::shared_ptr<GeometricCamera> &pCamera, cv::Mat &distCoef, const float &bf, const float &thDepth, const std::string &pNameFile, int pnNumDataset,  Frame* pPrevF = nullptr, const IMU::Calib &ImuCalib = IMU::Calib());
-
-    // Destructor
-    virtual ~Frame();
-
-    // Extract ORB on the image. 0 for left image and 1 for right image.
-    void ExtractORB(bool isLeft, const cv::Mat &im, const int x0, const int x1);
-
-    // Compute Bag of Words representation.
-    void ComputeBoW();
-
-    // Set the camera pose. (Imu pose is not modified!)
-    void SetPose(const Sophus::SE3<float> &Tcw);
-
-    // Set IMU velocity
-    void SetVelocity(Eigen::Vector3f Vw);
-
-    Eigen::Vector3f GetVelocity() const;
-
-    // Set IMU pose and velocity (implicitly changes camera pose)
-    void SetImuPoseVelocity(const Eigen::Matrix3f &Rwb, const Eigen::Vector3f &twb, const Eigen::Vector3f &Vwb);
-
-    Eigen::Matrix<float,3,1> GetImuPosition() const;
-    Eigen::Matrix<float,3,3> GetImuRotation();
-    Sophus::SE3<float> GetImuPose();
-
-    Sophus::SE3f GetRelativePoseTrl();
-    Sophus::SE3f GetRelativePoseTlr();
-    Eigen::Matrix3f GetRelativePoseTlr_rotation();
-    Eigen::Vector3f GetRelativePoseTlr_translation();
-
-    void SetNewBias(const IMU::Bias &b);
-
-    // Check if a MapPoint is in the frustum of the camera
-    // and fill variables of the MapPoint to be used by the tracking
-    bool isInFrustum(MapPoint* pMP, float viewingCosLimit);
-
-    bool ProjectPointDistort(MapPoint* pMP, cv::Point2f &kp, float &u, float &v);
-
-    Eigen::Vector3f inRefCoordinates(Eigen::Vector3f pCw);
-
-    // Compute the cell of a keypoint (return false if outside the grid)
-    bool PosInGrid(const cv::KeyPoint &kp, int &posX, int &posY);
-
-    vector<size_t> GetFeaturesInArea(const float &x, const float  &y, const float  &r, const int minLevel=-1, const int maxLevel=-1, const bool bRight = false) const;
-
-    // Search a match for each keypoint in the left image to a keypoint in the right image.
-    // If there is a match, depth is computed and the right coordinate associated to the left keypoint is stored.
-    void ComputeStereoMatches();
-
-    // Associate a "right" coordinate to a keypoint if there is valid depth in the depthmap.
-    void ComputeStereoFromRGBD(const cv::Mat &imDepth);
-
-    // Backprojects a keypoint (if stereo/depth info available) into 3D world coordinates.
-    bool UnprojectStereo(const int &i, Eigen::Vector3f &x3D);
-
-    std::shared_ptr<ConstraintPoseImu> mpcpi;
-
-    bool imuIsPreintegrated();
-    void setIntegrated();
-
-    bool isSet() const;
-
-    // Computes rotation, translation and camera center matrices from the camera pose.
-    void UpdatePoseMatrices();
-
-    // Returns the camera center.
-    inline Eigen::Vector3f GetCameraCenter(){
-        return mOw;
-    }
-
-    // Returns inverse of rotation
-    inline Eigen::Matrix3f GetRotationInverse(){
-        return mRwc;
-    }
-
-    inline Sophus::SE3<float> GetPose() const {
-        //TODO: can the Frame pose be accsessed from several threads? should this be protected somehow?
-        return mTcw;
-    }
-
-    inline Eigen::Matrix3f GetRwc() const {
-        return mRwc;
-    }
-
-    inline Eigen::Vector3f GetOw() const {
-        return mOw;
-    }
-
-    inline bool HasPose() const {
-        return mbHasPose;
-    }
-
-    inline bool HasVelocity() const {
-        return mbHasVelocity;
-    }
-
-
-
-private:
-    //Sophus/Eigen migration
-    Sophus::SE3<float> mTcw;
-    Eigen::Matrix<float,3,3> mRwc;
-    Eigen::Matrix<float,3,1> mOw;
-    Eigen::Matrix<float,3,3> mRcw;
-    Eigen::Matrix<float,3,1> mtcw;
-    bool mbHasPose;
-
-    //Rcw_ not necessary as Sophus has a method for extracting the rotation matrix: Tcw_.rotationMatrix()
-    //tcw_ not necessary as Sophus has a method for extracting the translation vector: Tcw_.translation()
-    //Twc_ not necessary as Sophus has a method for easily computing the inverse pose: Tcw_.inverse()
-
-    Sophus::SE3<float> mTlr, mTrl;
-    Eigen::Matrix<float,3,3> mRlr;
-    Eigen::Vector3f mtlr;
-
-
-    // IMU linear velocity
-    Eigen::Vector3f mVw;
-    bool mbHasVelocity;
-
-public:
-    
-
-    // Vocabulary used for relocalization.
-    ORBVocabulary* mpORBvocabulary;
-
-    // Feature extractor. The right is used only in the stereo case.
-    std::shared_ptr<ORBextractor> mpORBextractorLeft;
-    std::shared_ptr<ORBextractor> mpORBextractorRight;
-
-
-    // Frame timestamp.
-    double mTimeStamp;
-
-    // Calibration matrix and OpenCV distortion parameters.
-    cv::Mat mK;
-    Eigen::Matrix3f mK_;
-    static float fx;
-    static float fy;
-    static float cx;
-    static float cy;
-    static float invfx;
-    static float invfy;
-    cv::Mat mDistCoef;
-
-    // Stereo baseline multiplied by fx.
-    float mbf;
-
-    // Stereo baseline in meters.
-    float mb;
-
-    // Threshold close/far points. Close points are inserted from 1 view.
-    // Far points are inserted as in the monocular case from 2 views.
-    float mThDepth;
-
-    // Number of KeyPoints.
-    int N;
-
-    // Vector of keypoints (original for visualization) and undistorted (actually used by the system).
-    // In the stereo case, mvKeysUn is redundant as images must be rectified.
-    // In the RGB-D case, RGB images can be distorted.
-    std::vector<cv::KeyPoint> mvKeys, mvKeysRight;
-    std::vector<cv::KeyPoint> mvKeysUn;
-
-    // Corresponding stereo coordinate and depth for each keypoint.
-    std::vector<MapPoint*> mvpMapPoints;
-    // "Monocular" keypoints have a negative value.
-    std::vector<float> mvuRight;
-    std::vector<float> mvDepth;
-
-    // Bag of Words Vector structures.
-    DBoW2::BowVector mBowVec;
-    DBoW2::FeatureVector mFeatVec;
-
-    // ORB descriptor, each row associated to a keypoint.
-    cv::Mat mDescriptors, mDescriptorsRight;
-
-    // MapPoints associated to keypoints, nullptr pointer if no association.
-    // Flag to identify outlier associations.
-    std::vector<bool> mvbOutlier;
-    int mnCloseMPs;
-
-    // Keypoints are assigned to cells in a grid to reduce matching complexity when projecting MapPoints.
-    static float mfGridElementWidthInv;
-    static float mfGridElementHeightInv;
-    std::vector<std::size_t> mGrid[FRAME_GRID_COLS][FRAME_GRID_ROWS];
-
-    IMU::Bias mPredBias;
-
-    // IMU bias
-    IMU::Bias mImuBias;
-
-    // Imu calibration
-    IMU::Calib mImuCalib;
-
-    // Imu preintegration from last keyframe
-    IMU::Preintegrated* mpImuPreintegrated;
-    KeyFrame* mpLastKeyFrame;
-
-    // Pointer to previous frame
-    Frame* mpPrevFrame;
-    IMU::Preintegrated* mpImuPreintegratedFrame;
-
-    // Current and Next Frame id.
-    static long unsigned int nNextId;
-    long unsigned int mnId;
-
-    // Reference Keyframe.
-    KeyFrame* mpReferenceKF;
-
-    // Scale pyramid info.
-    int mnScaleLevels;
-    float mfScaleFactor;
-    float mfLogScaleFactor;
-    vector<float> mvScaleFactors;
-    vector<float> mvInvScaleFactors;
-    vector<float> mvLevelSigma2;
-    vector<float> mvInvLevelSigma2;
-
-    // Undistorted Image Bounds (computed once).
-    static float mnMinX;
-    static float mnMaxX;
-    static float mnMinY;
-    static float mnMaxY;
-
-    static bool mbInitialComputations;
-
-    map<long unsigned int, cv::Point2f> mmProjectPoints;
-    map<long unsigned int, cv::Point2f> mmMatchedInImage;
-
-    string mNameFile;
-
-    int mnDataset;
-
-#ifdef REGISTER_TIMES
-    double mTimeORB_Ext;
-    double mTimeStereoMatch;
-#endif
-
-private:
-
-    // Undistort keypoints given OpenCV distortion parameters.
-    // Only for the RGB-D case. Stereo must be already rectified!
-    // (called in the constructor).
-    void UndistortKeyPoints();
-
-    // Computes image bounds for the undistorted image (called in the constructor).
-    void ComputeImageBounds(const cv::Mat &imLeft);
-
-    // Assign keypoints to the grid for speed up feature matching (called in the constructor).
-    void AssignFeaturesToGrid();
-
-    bool mbIsSet;
-
-    bool mbImuPreintegrated;
-
-    std::shared_ptr<std::mutex> mpMutexImu;
-
-public:
-    Camera_ptr camera; 
-    std::shared_ptr<GeometricCamera> mpCamera, mpCamera2;
-
-    //Number of KeyPoints extracted in the left and right images
-    int Nleft, Nright;
-    //Number of Non Lapping Keypoints
-    int monoLeft, monoRight;
-
-    //For stereo matching
-    std::vector<int> mvLeftToRightMatch, mvRightToLeftMatch;
-
-    //For stereo fisheye matching
-    static cv::BFMatcher BFmatcher;
-
-    //Triangulated stereo observations using as reference the left camera. These are
-    //computed during ComputeStereoFishEyeMatches
-    std::vector<Eigen::Vector3f> mvStereo3Dpoints;
-
-    //Grid for the right image
-    std::vector<std::size_t> mGridRight[FRAME_GRID_COLS][FRAME_GRID_ROWS];
-
-    Frame(const Camera_ptr &cam, const cv::Mat &imLeft, const cv::Mat &imRight, const double &timeStamp, const std::shared_ptr<ORBextractor> &extractorLeft, const std::shared_ptr<ORBextractor> &extractorRight, ORBVocabulary* voc, cv::Mat &K, cv::Mat &distCoef, const float &bf, const float &thDepth, const std::shared_ptr<GeometricCamera> &pCamera, const std::shared_ptr<GeometricCamera> &pCamera2, const std::string &pNameFile, int pnNumDataset, Sophus::SE3f& Tlr,Frame* pPrevF = nullptr, const IMU::Calib &ImuCalib = IMU::Calib());
-
-    //Stereo fisheye
-    void ComputeStereoFishEyeMatches();
-
-    bool isInFrustumChecks(MapPoint* pMP, float viewingCosLimit, bool bRight = false);
-
-    Eigen::Vector3f UnprojectStereoFishEye(const int &i);
-
-    cv::Mat imgLeft, imgRight;
-
-    void PrintPointDistribution(){
-        int left = 0, right = 0;
-        int Nlim = (Nleft != -1) ? Nleft : N;
-        for(int i = 0; i < N; i++){
-            if(mvpMapPoints[i] && !mvbOutlier[i]){
-                if(i < Nlim) left++;
-                else right++;
-            }
-        }
-        cout << "Point distribution in Frame: left-> " << left << " --- right-> " << right << endl;
-    }
-
-    Sophus::SE3<double> T_test;
-};
-
-}// namespace ORB_SLAM
diff --git a/include/FrameDrawer.h b/include/FrameDrawer.h
deleted file mode 100644
index d5954762..00000000
--- a/include/FrameDrawer.h
+++ /dev/null
@@ -1,71 +0,0 @@
-/**
- * This file is part of ORB-SLAM3
- *
- * Copyright (C) 2017-2021 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/>.
- */
-
-#pragma once
-
-#include <mutex>
-#include <opencv2/opencv.hpp>
-#include <vector>
-#include "ImprovedTypes.hpp"
-
-namespace ORB_SLAM3 {
-
-class MapPoint;
-class Viewer;
-
-class FrameDrawer {
- public:
-  
-  FrameDrawer(const Atlas_ptr &pAtlas);
-
-  // Update info from the last processed frame.
-  void Update(const Tracking_ptr &pTracker);
-
-  // Draw last processed frame.
-  cv::Mat DrawFrame(float imageScale = 1.f);
-  cv::Mat DrawRightFrame(float imageScale = 1.f);
-
-  friend Viewer;
-
- protected:
-  bool both;
-  void DrawTextInfo(cv::Mat &im, int nState, cv::Mat &imText);
-
-  // Info of the frame to be drawn
-  cv::Mat mIm, mImRight;
-  int N;
-  std::vector<cv::KeyPoint> mvCurrentKeys, mvCurrentKeysRight;
-  std::vector<bool> mvbMap, mvbVO;
-  bool mbOnlyTracking;
-  int mnTracked, mnTrackedVO;
-  std::vector<cv::KeyPoint> mvIniKeys;
-  std::vector<int> mvIniMatches;
-  int mState;
-  std::vector<float> mvCurrentDepth;
-  float mThDepth;
-
-  Atlas_ptr mpAtlas;
-
-  std::mutex mMutex;
-  std::vector<std::pair<cv::Point2f, cv::Point2f>> mvTracks;
-};
-
-}  // namespace ORB_SLAM3
diff --git a/include/G2oTypes.h b/include/G2oTypes.h
deleted file mode 100644
index 6086d622..00000000
--- a/include/G2oTypes.h
+++ /dev/null
@@ -1,844 +0,0 @@
-/**
-* This file is part of ORB-SLAM3
-*
-* Copyright (C) 2017-2021 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/>.
-*/
-
-#pragma once
-
-#include "g2o/core/base_vertex.h"
-#include "g2o/core/base_binary_edge.h"
-#include "g2o/types/types_sba.h"
-#include "g2o/core/base_multi_edge.h"
-#include "g2o/core/base_unary_edge.h"
-
-#include<opencv2/core/core.hpp>
-
-#include <Eigen/Core>
-#include <Eigen/Geometry>
-#include <Eigen/Dense>
-
-#include <Frame.h>
-#include <KeyFrame.h>
-
-#include"Converter.h"
-#include <math.h>
-
-namespace ORB_SLAM3
-{
-
-class KeyFrame;
-class Frame;
-class GeometricCamera;
-
-typedef Eigen::Matrix<double, 6, 1> Vector6d;
-typedef Eigen::Matrix<double, 9, 1> Vector9d;
-typedef Eigen::Matrix<double, 12, 1> Vector12d;
-typedef Eigen::Matrix<double, 15, 1> Vector15d;
-typedef Eigen::Matrix<double, 12, 12> Matrix12d;
-typedef Eigen::Matrix<double, 15, 15> Matrix15d;
-typedef Eigen::Matrix<double, 9, 9> Matrix9d;
-
-Eigen::Matrix3d ExpSO3(const double x, const double y, const double z);
-Eigen::Matrix3d ExpSO3(const Eigen::Vector3d &w);
-
-Eigen::Vector3d LogSO3(const Eigen::Matrix3d &R);
-
-Eigen::Matrix3d InverseRightJacobianSO3(const Eigen::Vector3d &v);
-Eigen::Matrix3d RightJacobianSO3(const Eigen::Vector3d &v);
-Eigen::Matrix3d RightJacobianSO3(const double x, const double y, const double z);
-
-Eigen::Matrix3d Skew(const Eigen::Vector3d &w);
-Eigen::Matrix3d InverseRightJacobianSO3(const double x, const double y, const double z);
-
-template<typename T = double>
-Eigen::Matrix<T,3,3> NormalizeRotation(const Eigen::Matrix<T,3,3> &R) {
-    Eigen::JacobiSVD<Eigen::Matrix<T,3,3>> svd(R,Eigen::ComputeFullU | Eigen::ComputeFullV);
-    return svd.matrixU() * svd.matrixV().transpose();
-}
-
-
-class ImuCamPose
-{
-public:
-    
-    ImuCamPose(){}
-    ImuCamPose(KeyFrame* pKF);
-    ImuCamPose(Frame* pF);
-    ImuCamPose(Eigen::Matrix3d &_Rwc, Eigen::Vector3d &_twc, KeyFrame* pKF);
-
-    void 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);
-
-    void Update(const double *pu); // update in the imu reference
-    void UpdateW(const double *pu); // update in the world reference
-    Eigen::Vector2d Project(const Eigen::Vector3d &Xw, int cam_idx=0) const; // Mono
-    Eigen::Vector3d ProjectStereo(const Eigen::Vector3d &Xw, int cam_idx=0) const; // Stereo
-    bool isDepthPositive(const Eigen::Vector3d &Xw, int cam_idx=0) const;
-
-public:
-    // For IMU
-    Eigen::Matrix3d Rwb;
-    Eigen::Vector3d twb;
-
-    // For set of cameras
-    std::vector<Eigen::Matrix3d> Rcw;
-    std::vector<Eigen::Vector3d> tcw;
-    std::vector<Eigen::Matrix3d> Rcb, Rbc;
-    std::vector<Eigen::Vector3d> tcb, tbc;
-    double bf;
-    std::vector<std::shared_ptr<GeometricCamera>> pCamera;
-
-    // For posegraph 4DoF
-    Eigen::Matrix3d Rwb0;
-    Eigen::Matrix3d DR;
-
-    int its;
-};
-
-class InvDepthPoint
-{
-public:
-    
-    InvDepthPoint(){}
-    InvDepthPoint(double _rho, double _u, double _v, KeyFrame* pHostKF);
-
-    void Update(const double *pu);
-
-    double rho;
-    double u, v; // they are not variables, observation in the host frame
-
-    double fx, fy, cx, cy, bf; // from host frame
-
-    int its;
-};
-
-// Optimizable parameters are IMU pose
-class VertexPose : public g2o::BaseVertex<6,ImuCamPose>
-{
-public:
-    
-    VertexPose(){}
-    VertexPose(KeyFrame* pKF){
-        setEstimate(ImuCamPose(pKF));
-    }
-    VertexPose(Frame* pF){
-        setEstimate(ImuCamPose(pF));
-    }
-
-
-    virtual bool read(std::istream& is);
-    virtual bool write(std::ostream& os) const;
-
-    virtual void setToOriginImpl() {
-        }
-
-    virtual void oplusImpl(const double* update_){
-        _estimate.Update(update_);
-        updateCache();
-    }
-};
-
-class VertexPose4DoF : public g2o::BaseVertex<4,ImuCamPose>
-{
-    // Translation and yaw are the only optimizable variables
-public:
-    
-    VertexPose4DoF(){}
-    VertexPose4DoF(KeyFrame* pKF){
-        setEstimate(ImuCamPose(pKF));
-    }
-    VertexPose4DoF(Frame* pF){
-        setEstimate(ImuCamPose(pF));
-    }
-    VertexPose4DoF(Eigen::Matrix3d &_Rwc, Eigen::Vector3d &_twc, KeyFrame* pKF){
-
-        setEstimate(ImuCamPose(_Rwc, _twc, pKF));
-    }
-
-    virtual bool read(std::istream& is){return false;}
-    virtual bool write(std::ostream& os) const{return false;}
-
-    virtual void setToOriginImpl() {
-        }
-
-    virtual void oplusImpl(const double* update_){
-        double update6DoF[6];
-        update6DoF[0] = 0;
-        update6DoF[1] = 0;
-        update6DoF[2] = update_[0];
-        update6DoF[3] = update_[1];
-        update6DoF[4] = update_[2];
-        update6DoF[5] = update_[3];
-        _estimate.UpdateW(update6DoF);
-        updateCache();
-    }
-};
-
-class VertexVelocity : public g2o::BaseVertex<3,Eigen::Vector3d>
-{
-public:
-    
-    VertexVelocity(){}
-    VertexVelocity(KeyFrame* pKF);
-    VertexVelocity(Frame* pF);
-
-    virtual bool read(std::istream& is){return false;}
-    virtual bool write(std::ostream& os) const{return false;}
-
-    virtual void setToOriginImpl() {
-        }
-
-    virtual void oplusImpl(const double* update_){
-        Eigen::Vector3d uv;
-        uv << update_[0], update_[1], update_[2];
-        setEstimate(estimate()+uv);
-    }
-};
-
-class VertexGyroBias : public g2o::BaseVertex<3,Eigen::Vector3d>
-{
-public:
-    
-    VertexGyroBias(){}
-    VertexGyroBias(KeyFrame* pKF);
-    VertexGyroBias(Frame* pF);
-
-    virtual bool read(std::istream& is){return false;}
-    virtual bool write(std::ostream& os) const{return false;}
-
-    virtual void setToOriginImpl() {
-        }
-
-    virtual void oplusImpl(const double* update_){
-        Eigen::Vector3d ubg;
-        ubg << update_[0], update_[1], update_[2];
-        setEstimate(estimate()+ubg);
-    }
-};
-
-
-class VertexAccBias : public g2o::BaseVertex<3,Eigen::Vector3d>
-{
-public:
-    
-    VertexAccBias(){}
-    VertexAccBias(KeyFrame* pKF);
-    VertexAccBias(Frame* pF);
-
-    virtual bool read(std::istream& is){return false;}
-    virtual bool write(std::ostream& os) const{return false;}
-
-    virtual void setToOriginImpl() {
-        }
-
-    virtual void oplusImpl(const double* update_){
-        Eigen::Vector3d uba;
-        uba << update_[0], update_[1], update_[2];
-        setEstimate(estimate()+uba);
-    }
-};
-
-
-// Gravity direction vertex
-class GDirection
-{
-public:
-    
-    GDirection(){}
-    GDirection(Eigen::Matrix3d pRwg): Rwg(pRwg){}
-
-    void Update(const double *pu)
-    {
-        Rwg=Rwg*ExpSO3(pu[0],pu[1],0.0);
-    }
-
-    Eigen::Matrix3d Rwg, Rgw;
-
-    int its;
-};
-
-class VertexGDir : public g2o::BaseVertex<2,GDirection>
-{
-public:
-    
-    VertexGDir(){}
-    VertexGDir(Eigen::Matrix3d pRwg){
-        setEstimate(GDirection(pRwg));
-    }
-
-    virtual bool read(std::istream& is){return false;}
-    virtual bool write(std::ostream& os) const{return false;}
-
-    virtual void setToOriginImpl() {
-        }
-
-    virtual void oplusImpl(const double* update_){
-        _estimate.Update(update_);
-        updateCache();
-    }
-};
-
-// scale vertex
-class VertexScale : public g2o::BaseVertex<1,double>
-{
-public:
-    
-    VertexScale(){
-        setEstimate(1.0);
-    }
-    VertexScale(double ps){
-        setEstimate(ps);
-    }
-
-    virtual bool read(std::istream& is){return false;}
-    virtual bool write(std::ostream& os) const{return false;}
-
-    virtual void setToOriginImpl(){
-        setEstimate(1.0);
-    }
-
-    virtual void oplusImpl(const double *update_){
-        setEstimate(estimate()*exp(*update_));
-    }
-};
-
-
-// Inverse depth point (just one parameter, inverse depth at the host frame)
-class VertexInvDepth : public g2o::BaseVertex<1,InvDepthPoint>
-{
-public:
-    
-    VertexInvDepth(){}
-    VertexInvDepth(double invDepth, double u, double v, KeyFrame* pHostKF){
-        setEstimate(InvDepthPoint(invDepth, u, v, pHostKF));
-    }
-
-    virtual bool read(std::istream& is){return false;}
-    virtual bool write(std::ostream& os) const{return false;}
-
-    virtual void setToOriginImpl() {
-        }
-
-    virtual void oplusImpl(const double* update_){
-        _estimate.Update(update_);
-        updateCache();
-    }
-};
-
-class EdgeMono : public g2o::BaseBinaryEdge<2,Eigen::Vector2d,g2o::VertexSBAPointXYZ,VertexPose>
-{
-public:
-    
-
-    EdgeMono(int cam_idx_=0): cam_idx(cam_idx_){
-    }
-
-    virtual bool read(std::istream& is){return false;}
-    virtual bool write(std::ostream& os) const{return false;}
-
-    void computeError(){
-        const g2o::VertexSBAPointXYZ* VPoint = static_cast<const g2o::VertexSBAPointXYZ*>(_vertices[0]);
-        const VertexPose* VPose = static_cast<const VertexPose*>(_vertices[1]);
-        const Eigen::Vector2d obs(_measurement);
-        _error = obs - VPose->estimate().Project(VPoint->estimate(),cam_idx);
-    }
-
-
-    virtual void linearizeOplus();
-
-    bool isDepthPositive()
-    {
-        const g2o::VertexSBAPointXYZ* VPoint = static_cast<const g2o::VertexSBAPointXYZ*>(_vertices[0]);
-        const VertexPose* VPose = static_cast<const VertexPose*>(_vertices[1]);
-        return VPose->estimate().isDepthPositive(VPoint->estimate(),cam_idx);
-    }
-
-    Eigen::Matrix<double,2,9> GetJacobian(){
-        linearizeOplus();
-        Eigen::Matrix<double,2,9> J;
-        J.block<2,3>(0,0) = _jacobianOplusXi;
-        J.block<2,6>(0,3) = _jacobianOplusXj;
-        return J;
-    }
-
-    Eigen::Matrix<double,9,9> GetHessian(){
-        linearizeOplus();
-        Eigen::Matrix<double,2,9> J;
-        J.block<2,3>(0,0) = _jacobianOplusXi;
-        J.block<2,6>(0,3) = _jacobianOplusXj;
-        return J.transpose()*information()*J;
-    }
-
-public:
-    const int cam_idx;
-};
-
-class EdgeMonoOnlyPose : public g2o::BaseUnaryEdge<2,Eigen::Vector2d,VertexPose>
-{
-public:
-    
-
-    EdgeMonoOnlyPose(const Eigen::Vector3f &Xw_, int cam_idx_=0):Xw(Xw_.cast<double>()),
-        cam_idx(cam_idx_){}
-
-    virtual bool read(std::istream& is){return false;}
-    virtual bool write(std::ostream& os) const{return false;}
-
-    void computeError(){
-        const VertexPose* VPose = static_cast<const VertexPose*>(_vertices[0]);
-        const Eigen::Vector2d obs(_measurement);
-        _error = obs - VPose->estimate().Project(Xw,cam_idx);
-    }
-
-    virtual void linearizeOplus();
-
-    bool isDepthPositive()
-    {
-        const VertexPose* VPose = static_cast<const VertexPose*>(_vertices[0]);
-        return VPose->estimate().isDepthPositive(Xw,cam_idx);
-    }
-
-    Eigen::Matrix<double,6,6> GetHessian(){
-        linearizeOplus();
-        return _jacobianOplusXi.transpose()*information()*_jacobianOplusXi;
-    }
-
-public:
-    const Eigen::Vector3d Xw;
-    const int cam_idx;
-};
-
-class EdgeStereo : public g2o::BaseBinaryEdge<3,Eigen::Vector3d,g2o::VertexSBAPointXYZ,VertexPose>
-{
-public:
-    
-
-    EdgeStereo(int cam_idx_=0): cam_idx(cam_idx_){}
-
-    virtual bool read(std::istream& is){return false;}
-    virtual bool write(std::ostream& os) const{return false;}
-
-    void computeError(){
-        const g2o::VertexSBAPointXYZ* VPoint = static_cast<const g2o::VertexSBAPointXYZ*>(_vertices[0]);
-        const VertexPose* VPose = static_cast<const VertexPose*>(_vertices[1]);
-        const Eigen::Vector3d obs(_measurement);
-        _error = obs - VPose->estimate().ProjectStereo(VPoint->estimate(),cam_idx);
-    }
-
-
-    virtual void linearizeOplus();
-
-    Eigen::Matrix<double,3,9> GetJacobian(){
-        linearizeOplus();
-        Eigen::Matrix<double,3,9> J;
-        J.block<3,3>(0,0) = _jacobianOplusXi;
-        J.block<3,6>(0,3) = _jacobianOplusXj;
-        return J;
-    }
-
-    Eigen::Matrix<double,9,9> GetHessian(){
-        linearizeOplus();
-        Eigen::Matrix<double,3,9> J;
-        J.block<3,3>(0,0) = _jacobianOplusXi;
-        J.block<3,6>(0,3) = _jacobianOplusXj;
-        return J.transpose()*information()*J;
-    }
-
-public:
-    const int cam_idx;
-};
-
-
-class EdgeStereoOnlyPose : public g2o::BaseUnaryEdge<3,Eigen::Vector3d,VertexPose>
-{
-public:
-    
-
-    EdgeStereoOnlyPose(const Eigen::Vector3f &Xw_, int cam_idx_=0):
-        Xw(Xw_.cast<double>()), cam_idx(cam_idx_){}
-
-    virtual bool read(std::istream& is){return false;}
-    virtual bool write(std::ostream& os) const{return false;}
-
-    void computeError(){
-        const VertexPose* VPose = static_cast<const VertexPose*>(_vertices[0]);
-        const Eigen::Vector3d obs(_measurement);
-        _error = obs - VPose->estimate().ProjectStereo(Xw, cam_idx);
-    }
-
-    virtual void linearizeOplus();
-
-    Eigen::Matrix<double,6,6> GetHessian(){
-        linearizeOplus();
-        return _jacobianOplusXi.transpose()*information()*_jacobianOplusXi;
-    }
-
-public:
-    const Eigen::Vector3d Xw; // 3D point coordinates
-    const int cam_idx;
-};
-
-class EdgeInertial : public g2o::BaseMultiEdge<9,Vector9d>
-{
-public:
-    
-
-    EdgeInertial(IMU::Preintegrated* pInt);
-
-    virtual bool read(std::istream& is){return false;}
-    virtual bool write(std::ostream& os) const{return false;}
-
-    void computeError();
-    virtual void linearizeOplus();
-
-    Eigen::Matrix<double,24,24> GetHessian(){
-        linearizeOplus();
-        Eigen::Matrix<double,9,24> J;
-        J.block<9,6>(0,0) = _jacobianOplus[0];
-        J.block<9,3>(0,6) = _jacobianOplus[1];
-        J.block<9,3>(0,9) = _jacobianOplus[2];
-        J.block<9,3>(0,12) = _jacobianOplus[3];
-        J.block<9,6>(0,15) = _jacobianOplus[4];
-        J.block<9,3>(0,21) = _jacobianOplus[5];
-        return J.transpose()*information()*J;
-    }
-
-    Eigen::Matrix<double,18,18> GetHessianNoPose1(){
-        linearizeOplus();
-        Eigen::Matrix<double,9,18> J;
-        J.block<9,3>(0,0) = _jacobianOplus[1];
-        J.block<9,3>(0,3) = _jacobianOplus[2];
-        J.block<9,3>(0,6) = _jacobianOplus[3];
-        J.block<9,6>(0,9) = _jacobianOplus[4];
-        J.block<9,3>(0,15) = _jacobianOplus[5];
-        return J.transpose()*information()*J;
-    }
-
-    Eigen::Matrix<double,9,9> GetHessian2(){
-        linearizeOplus();
-        Eigen::Matrix<double,9,9> J;
-        J.block<9,6>(0,0) = _jacobianOplus[4];
-        J.block<9,3>(0,6) = _jacobianOplus[5];
-        return J.transpose()*information()*J;
-    }
-
-    const Eigen::Matrix3d JRg, JVg, JPg;
-    const Eigen::Matrix3d JVa, JPa;
-    IMU::Preintegrated* mpInt;
-    const double dt;
-    Eigen::Vector3d g;
-};
-
-
-// Edge inertial whre gravity is included as optimizable variable and it is not supposed to be pointing in -z axis, as well as scale
-class EdgeInertialGS : public g2o::BaseMultiEdge<9,Vector9d>
-{
-public:
-    
-
-    // EdgeInertialGS(IMU::Preintegrated* pInt);
-    EdgeInertialGS(IMU::Preintegrated* pInt);
-
-    virtual bool read(std::istream& is){return false;}
-    virtual bool write(std::ostream& os) const{return false;}
-
-    void computeError();
-    virtual void linearizeOplus();
-
-    const Eigen::Matrix3d JRg, JVg, JPg;
-    const Eigen::Matrix3d JVa, JPa;
-    IMU::Preintegrated* mpInt;
-    const double dt;
-    Eigen::Vector3d g, gI;
-
-    Eigen::Matrix<double,27,27> GetHessian(){
-        linearizeOplus();
-        Eigen::Matrix<double,9,27> J;
-        J.block<9,6>(0,0) = _jacobianOplus[0];
-        J.block<9,3>(0,6) = _jacobianOplus[1];
-        J.block<9,3>(0,9) = _jacobianOplus[2];
-        J.block<9,3>(0,12) = _jacobianOplus[3];
-        J.block<9,6>(0,15) = _jacobianOplus[4];
-        J.block<9,3>(0,21) = _jacobianOplus[5];
-        J.block<9,2>(0,24) = _jacobianOplus[6];
-        J.block<9,1>(0,26) = _jacobianOplus[7];
-        return J.transpose()*information()*J;
-    }
-
-    Eigen::Matrix<double,27,27> GetHessian2(){
-        linearizeOplus();
-        Eigen::Matrix<double,9,27> J;
-        J.block<9,3>(0,0) = _jacobianOplus[2];
-        J.block<9,3>(0,3) = _jacobianOplus[3];
-        J.block<9,2>(0,6) = _jacobianOplus[6];
-        J.block<9,1>(0,8) = _jacobianOplus[7];
-        J.block<9,3>(0,9) = _jacobianOplus[1];
-        J.block<9,3>(0,12) = _jacobianOplus[5];
-        J.block<9,6>(0,15) = _jacobianOplus[0];
-        J.block<9,6>(0,21) = _jacobianOplus[4];
-        return J.transpose()*information()*J;
-    }
-
-    Eigen::Matrix<double,9,9> GetHessian3(){
-        linearizeOplus();
-        Eigen::Matrix<double,9,9> J;
-        J.block<9,3>(0,0) = _jacobianOplus[2];
-        J.block<9,3>(0,3) = _jacobianOplus[3];
-        J.block<9,2>(0,6) = _jacobianOplus[6];
-        J.block<9,1>(0,8) = _jacobianOplus[7];
-        return J.transpose()*information()*J;
-    }
-
-
-
-    Eigen::Matrix<double,1,1> GetHessianScale(){
-        linearizeOplus();
-        Eigen::Matrix<double,9,1> J = _jacobianOplus[7];
-        return J.transpose()*information()*J;
-    }
-
-    Eigen::Matrix<double,3,3> GetHessianBiasGyro(){
-        linearizeOplus();
-        Eigen::Matrix<double,9,3> J = _jacobianOplus[2];
-        return J.transpose()*information()*J;
-    }
-
-    Eigen::Matrix<double,3,3> GetHessianBiasAcc(){
-        linearizeOplus();
-        Eigen::Matrix<double,9,3> J = _jacobianOplus[3];
-        return J.transpose()*information()*J;
-    }
-
-    Eigen::Matrix<double,2,2> GetHessianGDir(){
-        linearizeOplus();
-        Eigen::Matrix<double,9,2> J = _jacobianOplus[6];
-        return J.transpose()*information()*J;
-    }
-};
-
-
-
-class EdgeGyroRW : public g2o::BaseBinaryEdge<3,Eigen::Vector3d,VertexGyroBias,VertexGyroBias>
-{
-public:
-    
-
-    EdgeGyroRW(){}
-
-    virtual bool read(std::istream& is){return false;}
-    virtual bool write(std::ostream& os) const{return false;}
-
-    void computeError(){
-        const VertexGyroBias* VG1= static_cast<const VertexGyroBias*>(_vertices[0]);
-        const VertexGyroBias* VG2= static_cast<const VertexGyroBias*>(_vertices[1]);
-        _error = VG2->estimate()-VG1->estimate();
-    }
-
-    virtual void linearizeOplus(){
-        _jacobianOplusXi = -Eigen::Matrix3d::Identity();
-        _jacobianOplusXj.setIdentity();
-    }
-
-    Eigen::Matrix<double,6,6> GetHessian(){
-        linearizeOplus();
-        Eigen::Matrix<double,3,6> J;
-        J.block<3,3>(0,0) = _jacobianOplusXi;
-        J.block<3,3>(0,3) = _jacobianOplusXj;
-        return J.transpose()*information()*J;
-    }
-
-    Eigen::Matrix3d GetHessian2(){
-        linearizeOplus();
-        return _jacobianOplusXj.transpose()*information()*_jacobianOplusXj;
-    }
-};
-
-
-class EdgeAccRW : public g2o::BaseBinaryEdge<3,Eigen::Vector3d,VertexAccBias,VertexAccBias>
-{
-public:
-    
-
-    EdgeAccRW(){}
-
-    virtual bool read(std::istream& is){return false;}
-    virtual bool write(std::ostream& os) const{return false;}
-
-    void computeError(){
-        const VertexAccBias* VA1= static_cast<const VertexAccBias*>(_vertices[0]);
-        const VertexAccBias* VA2= static_cast<const VertexAccBias*>(_vertices[1]);
-        _error = VA2->estimate()-VA1->estimate();
-    }
-
-    virtual void linearizeOplus(){
-        _jacobianOplusXi = -Eigen::Matrix3d::Identity();
-        _jacobianOplusXj.setIdentity();
-    }
-
-    Eigen::Matrix<double,6,6> GetHessian(){
-        linearizeOplus();
-        Eigen::Matrix<double,3,6> J;
-        J.block<3,3>(0,0) = _jacobianOplusXi;
-        J.block<3,3>(0,3) = _jacobianOplusXj;
-        return J.transpose()*information()*J;
-    }
-
-    Eigen::Matrix3d GetHessian2(){
-        linearizeOplus();
-        return _jacobianOplusXj.transpose()*information()*_jacobianOplusXj;
-    }
-};
-
-class ConstraintPoseImu
-{
-public:
-    
-
-    ConstraintPoseImu(const Eigen::Matrix3d &Rwb_, const Eigen::Vector3d &twb_, const Eigen::Vector3d &vwb_,
-                       const Eigen::Vector3d &bg_, const Eigen::Vector3d &ba_, const Matrix15d &H_):
-                       Rwb(Rwb_), twb(twb_), vwb(vwb_), bg(bg_), ba(ba_), H(H_)
-    {
-        Eigen::SelfAdjointEigenSolver<Eigen::Matrix<double,15,15> > es(H);
-        Eigen::Matrix<double,15,1> eigs = es.eigenvalues();
-        for(int i=0;i<15;i++)
-            if(eigs[i]<1e-12)
-                eigs[i]=0;
-        H = es.eigenvectors()*eigs.asDiagonal()*es.eigenvectors().transpose();
-    }
-
-    Eigen::Matrix3d Rwb;
-    Eigen::Vector3d twb;
-    Eigen::Vector3d vwb;
-    Eigen::Vector3d bg;
-    Eigen::Vector3d ba;
-    Matrix15d H;
-};
-
-class EdgePriorPoseImu : public g2o::BaseMultiEdge<15,Vector15d>
-{
-public:
-        
-        EdgePriorPoseImu(std::shared_ptr<ConstraintPoseImu> c);
-
-        virtual bool read(std::istream& is){return false;}
-        virtual bool write(std::ostream& os) const{return false;}
-
-        void computeError();
-        virtual void linearizeOplus();
-
-        Eigen::Matrix<double,15,15> GetHessian(){
-            linearizeOplus();
-            Eigen::Matrix<double,15,15> J;
-            J.block<15,6>(0,0) = _jacobianOplus[0];
-            J.block<15,3>(0,6) = _jacobianOplus[1];
-            J.block<15,3>(0,9) = _jacobianOplus[2];
-            J.block<15,3>(0,12) = _jacobianOplus[3];
-            return J.transpose()*information()*J;
-        }
-
-        Eigen::Matrix<double,9,9> GetHessianNoPose(){
-            linearizeOplus();
-            Eigen::Matrix<double,15,9> J;
-            J.block<15,3>(0,0) = _jacobianOplus[1];
-            J.block<15,3>(0,3) = _jacobianOplus[2];
-            J.block<15,3>(0,6) = _jacobianOplus[3];
-            return J.transpose()*information()*J;
-        }
-        Eigen::Matrix3d Rwb;
-        Eigen::Vector3d twb, vwb;
-        Eigen::Vector3d bg, ba;
-};
-
-// Priors for biases
-class EdgePriorAcc : public g2o::BaseUnaryEdge<3,Eigen::Vector3d,VertexAccBias>
-{
-public:
-    
-
-    EdgePriorAcc(const Eigen::Vector3f &bprior_):bprior(bprior_.cast<double>()){}
-
-    virtual bool read(std::istream& is){return false;}
-    virtual bool write(std::ostream& os) const{return false;}
-
-    void computeError(){
-        const VertexAccBias* VA = static_cast<const VertexAccBias*>(_vertices[0]);
-        _error = bprior - VA->estimate();
-    }
-    virtual void linearizeOplus();
-
-    Eigen::Matrix<double,3,3> GetHessian(){
-        linearizeOplus();
-        return _jacobianOplusXi.transpose()*information()*_jacobianOplusXi;
-    }
-
-    const Eigen::Vector3d bprior;
-};
-
-class EdgePriorGyro : public g2o::BaseUnaryEdge<3,Eigen::Vector3d,VertexGyroBias>
-{
-public:
-    
-
-    EdgePriorGyro(const Eigen::Vector3f &bprior_):bprior(bprior_.cast<double>()){}
-
-    virtual bool read(std::istream& is){return false;}
-    virtual bool write(std::ostream& os) const{return false;}
-
-    void computeError(){
-        const VertexGyroBias* VG = static_cast<const VertexGyroBias*>(_vertices[0]);
-        _error = bprior - VG->estimate();
-    }
-    virtual void linearizeOplus();
-
-    Eigen::Matrix<double,3,3> GetHessian(){
-        linearizeOplus();
-        return _jacobianOplusXi.transpose()*information()*_jacobianOplusXi;
-    }
-
-    const Eigen::Vector3d bprior;
-};
-
-
-class Edge4DoF : public g2o::BaseBinaryEdge<6,Vector6d,VertexPose4DoF,VertexPose4DoF>
-{
-public:
-    
-
-    Edge4DoF(const Eigen::Matrix4d &deltaT){
-        dTij = deltaT;
-        dRij = deltaT.block<3,3>(0,0);
-        dtij = deltaT.block<3,1>(0,3);
-    }
-
-    virtual bool read(std::istream& is){return false;}
-    virtual bool write(std::ostream& os) const{return false;}
-
-    void computeError(){
-        const VertexPose4DoF* VPi = static_cast<const VertexPose4DoF*>(_vertices[0]);
-        const VertexPose4DoF* VPj = static_cast<const VertexPose4DoF*>(_vertices[1]);
-        _error << LogSO3(VPi->estimate().Rcw[0]*VPj->estimate().Rcw[0].transpose()*dRij.transpose()),
-                 VPi->estimate().Rcw[0]*(-VPj->estimate().Rcw[0].transpose()*VPj->estimate().tcw[0])+VPi->estimate().tcw[0] - dtij;
-    }
-
-    // virtual void linearizeOplus(); // numerical implementation
-
-    Eigen::Matrix4d dTij;
-    Eigen::Matrix3d dRij;
-    Eigen::Vector3d dtij;
-};
-
-} //namespace ORB_SLAM2
-
diff --git a/include/GeometricTools.h b/include/GeometricTools.h
deleted file mode 100644
index 8c2651b9..00000000
--- a/include/GeometricTools.h
+++ /dev/null
@@ -1,81 +0,0 @@
-/**
- * This file is part of ORB-SLAM3
- *
- * Copyright (C) 2017-2021 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/>.
- */
-
-#pragma once
-
-#include <Eigen/Core>
-#include <opencv2/core/core.hpp>
-#include <sophus/se3.hpp>
-
-namespace ORB_SLAM3 {
-
-class KeyFrame;
-
-class GeometricTools {
- public:
-  
-  // Compute the Fundamental matrix between KF1 and KF2
-  static Eigen::Matrix3f ComputeF12(KeyFrame *&pKF1, KeyFrame *&pKF2);
-
-  // Triangulate point with KF1 and KF2
-  static bool Triangulate(Eigen::Vector3f &x_c1, Eigen::Vector3f &x_c2,
-                          Eigen::Matrix<float, 3, 4> &Tc1w,
-                          Eigen::Matrix<float, 3, 4> &Tc2w,
-                          Eigen::Vector3f &x3D);
-
-  template <int rows, int cols>
-  static bool CheckMatrices(const cv::Mat &cvMat,
-                            const Eigen::Matrix<float, rows, cols> &eigMat) {
-    const float epsilon = 1e-3;
-    // std::cout << cvMat.cols - cols << cvMat.rows - rows << std::endl;
-    if (rows != cvMat.rows || cols != cvMat.cols) {
-      std::cout << "wrong cvmat size\n";
-      return false;
-    }
-    for (int i = 0; i < rows; i++)
-      for (int j = 0; j < cols; j++)
-        if ((cvMat.at<float>(i, j) > (eigMat(i, j) + epsilon)) ||
-            (cvMat.at<float>(i, j) < (eigMat(i, j) - epsilon))) {
-          std::cout << "cv mat:\n" << cvMat << std::endl;
-          std::cout << "eig mat:\n" << eigMat << std::endl;
-          return false;
-        }
-    return true;
-  }
-
-  template <typename T, int rows, int cols>
-  static bool CheckMatrices(const Eigen::Matrix<T, rows, cols> &eigMat1,
-                            const Eigen::Matrix<T, rows, cols> &eigMat2) {
-    const float epsilon = 1e-3;
-    for (int i = 0; i < rows; i++)
-      for (int j = 0; j < cols; j++)
-        if ((eigMat1(i, j) > (eigMat2(i, j) + epsilon)) ||
-            (eigMat1(i, j) < (eigMat2(i, j) - epsilon))) {
-          std::cout << "eig mat 1:\n" << eigMat1 << std::endl;
-          std::cout << "eig mat 2:\n" << eigMat2 << std::endl;
-          return false;
-        }
-    return true;
-  }
-};
-
-}  // namespace ORB_SLAM3
-
diff --git a/include/ImprovedTypes.hpp b/include/ImprovedTypes.hpp
deleted file mode 100644
index 66a5688f..00000000
--- a/include/ImprovedTypes.hpp
+++ /dev/null
@@ -1,51 +0,0 @@
-#pragma once
-
-#include <memory>
-#include <utility>
-#include <unordered_map>
-
-namespace ORB_SLAM3{
-
-class System;
-class Atlas;
-class Tracking;
-typedef std::shared_ptr<System> System_ptr;
-typedef std::weak_ptr<System> System_wptr;
-typedef std::shared_ptr<Atlas> Atlas_ptr;
-typedef std::weak_ptr<Atlas> Atlas_wptr;
-typedef std::shared_ptr<Tracking> Tracking_ptr;
-typedef std::weak_ptr<Tracking> Tracking_wptr;
-
-typedef std::pair<int, int> IntPair;
-template<typename KEY, typename VALUE> using umap = std::unordered_map<KEY,VALUE>;
-
-
-
-  namespace Tracker{
-    // Tracking states
-    enum eTrackingState {
-        SYSTEM_NOT_READY = -1,
-        NO_IMAGES_YET = 0,
-        NOT_INITIALIZED = 1,
-        OK = 2,
-        RECENTLY_LOST = 3,
-        LOST = 4,
-        OK_KLT = 5
-    };
-  }
-
-  namespace CameraType{
-    // Input sensor
-    enum eSensor{
-        MONOCULAR=0,
-        STEREO=1,
-        RGBD=2,
-        IMU_MONOCULAR=3,
-        IMU_STEREO=4,
-        IMU_RGBD=5,
-    };
-
-    inline bool isInertial(eSensor sensor) { return sensor == IMU_MONOCULAR || sensor == IMU_STEREO || sensor == IMU_RGBD; }
-    inline bool hasMulticam(eSensor sensor) { return sensor == STEREO || sensor == RGBD || sensor == IMU_STEREO || sensor == IMU_RGBD; }
-  }
-}
\ No newline at end of file
diff --git a/include/ImuTypes.h b/include/ImuTypes.h
deleted file mode 100644
index f9738759..00000000
--- a/include/ImuTypes.h
+++ /dev/null
@@ -1,272 +0,0 @@
-/**
- * This file is part of ORB-SLAM3
- *
- * Copyright (C) 2017-2021 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/>.
- */
-
-#pragma once
-
-#include <Eigen/Core>
-#include <Eigen/Dense>
-#include <Eigen/Geometry>
-#include <boost/serialization/serialization.hpp>
-#include <boost/serialization/vector.hpp>
-#include <mutex>
-#include <opencv2/core/core.hpp>
-#include <sophus/se3.hpp>
-#include <utility>
-#include <vector>
-
-#include "SerializationUtils.h"
-
-namespace ORB_SLAM3 {
-
-namespace IMU {
-
-const float GRAVITY_VALUE = 9.81;
-
-// IMU measurement (gyro, accelerometer and timestamp)
-class Point {
- public:
-  Point(const float &acc_x, const float &acc_y, const float &acc_z,
-        const float &ang_vel_x, const float &ang_vel_y, const float &ang_vel_z,
-        const double &timestamp)
-      : a(acc_x, acc_y, acc_z),
-        w(ang_vel_x, ang_vel_y, ang_vel_z),
-        t(timestamp) {}
-  Point(const cv::Point3f Acc, const cv::Point3f Gyro, const double &timestamp)
-      : a(Acc.x, Acc.y, Acc.z), w(Gyro.x, Gyro.y, Gyro.z), t(timestamp) {}
-
- public:
-  Eigen::Vector3f a;
-  Eigen::Vector3f w;
-  double t;
-  
-};
-
-// IMU biases (gyro and accelerometer)
-class Bias {
-  friend class boost::serialization::access;
-  template <class Archive>
-  void serialize(Archive &ar, const unsigned int version) {
-    ar &bax;
-    ar &bay;
-    ar &baz;
-
-    ar &bwx;
-    ar &bwy;
-    ar &bwz;
-  }
-
- public:
-  Bias() : bax(0), bay(0), baz(0), bwx(0), bwy(0), bwz(0) {}
-  Bias(const float &b_acc_x, const float &b_acc_y, const float &b_acc_z,
-       const float &b_ang_vel_x, const float &b_ang_vel_y,
-       const float &b_ang_vel_z)
-      : bax(b_acc_x),
-        bay(b_acc_y),
-        baz(b_acc_z),
-        bwx(b_ang_vel_x),
-        bwy(b_ang_vel_y),
-        bwz(b_ang_vel_z) {}
-  void CopyFrom(Bias &b);
-  friend std::ostream &operator<<(std::ostream &out, const Bias &b);
-
- public:
-  float bax, bay, baz;
-  float bwx, bwy, bwz;
-  
-};
-
-// IMU calibration (Tbc, Tcb, noise)
-class Calib {
-  friend class boost::serialization::access;
-  template <class Archive>
-  void serialize(Archive &ar, const unsigned int version) {
-    serializeSophusSE3(ar, mTcb, version);
-    serializeSophusSE3(ar, mTbc, version);
-
-    ar &boost::serialization::make_array(Cov.diagonal().data(),
-                                         Cov.diagonal().size());
-    ar &boost::serialization::make_array(CovWalk.diagonal().data(),
-                                         CovWalk.diagonal().size());
-
-    ar &mbIsSet;
-  }
-
- public:
-  Calib(const Sophus::SE3<float> &Tbc, const float &ng, const float &na,
-        const float &ngw, const float &naw) {
-    Set(Tbc, ng, na, ngw, naw);
-  }
-
-  Calib(const Calib &calib);
-  Calib() { mbIsSet = false; }
-
-  // void Set(const cv::Mat &cvTbc, const float &ng, const float &na, const
-  // float &ngw, const float &naw);
-  void Set(const Sophus::SE3<float> &sophTbc, const float &ng, const float &na,
-           const float &ngw, const float &naw);
-
- public:
-  // Sophus/Eigen implementation
-  Sophus::SE3<float> mTcb;
-  Sophus::SE3<float> mTbc;
-  Eigen::DiagonalMatrix<float, 6> Cov, CovWalk;
-  bool mbIsSet;
-};
-
-// Integration of 1 gyro measurement
-class IntegratedRotation {
- public:
-  IntegratedRotation() {}
-  IntegratedRotation(const Eigen::Vector3f &angVel, const Bias &imuBias,
-                     const float &time);
-
- public:
-  float deltaT;  // integration time
-  Eigen::Matrix3f deltaR;
-  Eigen::Matrix3f rightJ;  // right jacobian
-  
-};
-
-// Preintegration of Imu Measurements
-class Preintegrated {
-  friend class boost::serialization::access;
-  template <class Archive>
-  void serialize(Archive &ar, const unsigned int version) {
-    ar &dT;
-    ar &boost::serialization::make_array(C.data(), C.size());
-    ar &boost::serialization::make_array(Info.data(), Info.size());
-    ar &boost::serialization::make_array(Nga.diagonal().data(),
-                                         Nga.diagonal().size());
-    ar &boost::serialization::make_array(NgaWalk.diagonal().data(),
-                                         NgaWalk.diagonal().size());
-    ar &b;
-    ar &boost::serialization::make_array(dR.data(), dR.size());
-    ar &boost::serialization::make_array(dV.data(), dV.size());
-    ar &boost::serialization::make_array(dP.data(), dP.size());
-    ar &boost::serialization::make_array(JRg.data(), JRg.size());
-    ar &boost::serialization::make_array(JVg.data(), JVg.size());
-    ar &boost::serialization::make_array(JVa.data(), JVa.size());
-    ar &boost::serialization::make_array(JPg.data(), JPg.size());
-    ar &boost::serialization::make_array(JPa.data(), JPa.size());
-    ar &boost::serialization::make_array(avgA.data(), avgA.size());
-    ar &boost::serialization::make_array(avgW.data(), avgW.size());
-
-    ar &bu;
-    ar &boost::serialization::make_array(db.data(), db.size());
-    ar &mvMeasurements;
-  }
-
- public:
-  
-  Preintegrated(const Bias &b_, const Calib &calib);
-  Preintegrated(Preintegrated *pImuPre);
-  Preintegrated() {}
-  ~Preintegrated() {}
-  void CopyFrom(Preintegrated *pImuPre);
-  void Initialize(const Bias &b_);
-  void IntegrateNewMeasurement(const Eigen::Vector3f &acceleration,
-                               const Eigen::Vector3f &angVel, const float &dt);
-  void Reintegrate();
-  void MergePrevious(Preintegrated *pPrev);
-  void SetNewBias(const Bias &bu_);
-  IMU::Bias GetDeltaBias(const Bias &b_);
-
-  Eigen::Matrix3f GetDeltaRotation(const Bias &b_);
-  Eigen::Vector3f GetDeltaVelocity(const Bias &b_);
-  Eigen::Vector3f GetDeltaPosition(const Bias &b_);
-
-  Eigen::Matrix3f GetUpdatedDeltaRotation();
-  Eigen::Vector3f GetUpdatedDeltaVelocity();
-  Eigen::Vector3f GetUpdatedDeltaPosition();
-
-  Eigen::Matrix3f GetOriginalDeltaRotation();
-  Eigen::Vector3f GetOriginalDeltaVelocity();
-  Eigen::Vector3f GetOriginalDeltaPosition();
-
-  Eigen::Matrix<float, 6, 1> GetDeltaBias();
-
-  Bias GetOriginalBias();
-  Bias GetUpdatedBias();
-
-  void printMeasurements() const {
-    std::cout << "pint meas:\n";
-    for (size_t i = 0; i < mvMeasurements.size(); i++) {
-      std::cout << "meas " << mvMeasurements[i].t << std::endl;
-    }
-    std::cout << "end pint meas:\n";
-  }
-
- public:
-  float dT;
-  Eigen::Matrix<float, 15, 15> C;
-  Eigen::Matrix<float, 15, 15> Info;
-  Eigen::DiagonalMatrix<float, 6> Nga, NgaWalk;
-
-  // Values for the original bias (when integration was computed)
-  Bias b;
-  Eigen::Matrix3f dR;
-  Eigen::Vector3f dV, dP;
-  Eigen::Matrix3f JRg, JVg, JVa, JPg, JPa;
-  Eigen::Vector3f avgA, avgW;
-
- private:
-  // Updated bias
-  Bias bu;
-  // Dif between original and updated bias
-  // This is used to compute the updated values of the preintegration
-  Eigen::Matrix<float, 6, 1> db;
-
-  struct integrable {
-    template <class Archive>
-    void serialize(Archive &ar, const unsigned int version) {
-      ar &boost::serialization::make_array(a.data(), a.size());
-      ar &boost::serialization::make_array(w.data(), w.size());
-      ar &t;
-    }
-
-    
-    integrable() {}
-    integrable(const Eigen::Vector3f &a_, const Eigen::Vector3f &w_,
-               const float &t_)
-        : a(a_), w(w_), t(t_) {}
-    Eigen::Vector3f a, w;
-    float t;
-  };
-
-  std::vector<integrable> mvMeasurements;
-
-  std::mutex mMutex;
-};
-
-// Lie Algebra Functions
-Eigen::Matrix3f RightJacobianSO3(const float &x, const float &y,
-                                 const float &z);
-Eigen::Matrix3f RightJacobianSO3(const Eigen::Vector3f &v);
-
-Eigen::Matrix3f InverseRightJacobianSO3(const float &x, const float &y,
-                                        const float &z);
-Eigen::Matrix3f InverseRightJacobianSO3(const Eigen::Vector3f &v);
-
-Eigen::Matrix3f NormalizeRotation(const Eigen::Matrix3f &R);
-
-}  // namespace IMU
-
-}  // namespace ORB_SLAM3
diff --git a/include/KeyFrame.h b/include/KeyFrame.h
deleted file mode 100644
index b4cabf46..00000000
--- a/include/KeyFrame.h
+++ /dev/null
@@ -1,542 +0,0 @@
-/**
- * This file is part of ORB-SLAM3
- *
- * Copyright (C) 2017-2021 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/>.
- */
-
-#pragma once
-
-#include <boost/serialization/base_object.hpp>
-#include <boost/serialization/map.hpp>
-#include <boost/serialization/vector.hpp>
-#include <mutex>
-
-#include "DBoW2/BowVector.h"
-#include "DBoW2/FeatureVector.h"
-#include "Frame.h"
-#include "GeometricCamera.h"
-#include "ImuTypes.h"
-#include "KeyFrameDatabase.h"
-#include "MapPoint.h"
-#include "ORBVocabulary.h"
-#include "ORBextractor.h"
-#include "SerializationUtils.h"
-
-namespace ORB_SLAM3 {
-
-class Map;
-class MapPoint;
-class Frame;
-class KeyFrameDatabase;
-
-class GeometricCamera;
-
-class KeyFrame {
-  friend class boost::serialization::access;
-
-  template <class Archive>
-  void serialize(Archive& ar, const unsigned int version) {
-    ar& mnId;
-    ar& const_cast<long unsigned int&>(mnFrameId);
-    ar& const_cast<double&>(mTimeStamp);
-    // Grid
-    ar& const_cast<int&>(mnGridCols);
-    ar& const_cast<int&>(mnGridRows);
-    ar& const_cast<float&>(mfGridElementWidthInv);
-    ar& const_cast<float&>(mfGridElementHeightInv);
-
-    // Variables of tracking
-    // ar & mnTrackReferenceForFrame;
-    // ar & mnFuseTargetForKF;
-    // Variables of local mapping
-    // ar & mnBALocalForKF;
-    // ar & mnBAFixedForKF;
-    // ar & mnNumberOfOpt;
-    // Variables used by KeyFrameDatabase
-    // ar & mnLoopQuery;
-    // ar & mnLoopWords;
-    // ar & mLoopScore;
-    // ar & mnRelocQuery;
-    // ar & mnRelocWords;
-    // ar & mRelocScore;
-    // ar & mnMergeQuery;
-    // ar & mnMergeWords;
-    // ar & mMergeScore;
-    // ar & mnPlaceRecognitionQuery;
-    // ar & mnPlaceRecognitionWords;
-    // ar & mPlaceRecognitionScore;
-    // ar & mbCurrentPlaceRecognition;
-    // Variables of loop closing
-    // serializeMatrix(ar,mTcwGBA,version);
-    // serializeMatrix(ar,mTcwBefGBA,version);
-    // serializeMatrix(ar,mVwbGBA,version);
-    // serializeMatrix(ar,mVwbBefGBA,version);
-    // ar & mBiasGBA;
-    // ar & mnBAGlobalForKF;
-    // Variables of Merging
-    // serializeMatrix(ar,mTcwMerge,version);
-    // serializeMatrix(ar,mTcwBefMerge,version);
-    // serializeMatrix(ar,mTwcBefMerge,version);
-    // serializeMatrix(ar,mVwbMerge,version);
-    // serializeMatrix(ar,mVwbBefMerge,version);
-    // ar & mBiasMerge;
-    // ar & mnMergeCorrectedForKF;
-    // ar & mnMergeForKF;
-    // ar & mfScaleMerge;
-    // ar & mnBALocalForMerge;
-
-    // Scale
-    ar& mfScale;
-    // Calibration parameters
-    ar& const_cast<float&>(fx);
-    ar& const_cast<float&>(fy);
-    ar& const_cast<float&>(invfx);
-    ar& const_cast<float&>(invfy);
-    ar& const_cast<float&>(cx);
-    ar& const_cast<float&>(cy);
-    ar& const_cast<float&>(mbf);
-    ar& const_cast<float&>(mb);
-    ar& const_cast<float&>(mThDepth);
-    serializeMatrix(ar, mDistCoef, version);
-    // Number of Keypoints
-    ar& const_cast<int&>(N);
-    // KeyPoints
-    serializeVectorKeyPoints<Archive>(ar, mvKeys, version);
-    serializeVectorKeyPoints<Archive>(ar, mvKeysUn, version);
-    ar& const_cast<vector<float>&>(mvuRight);
-    ar& const_cast<vector<float>&>(mvDepth);
-    serializeMatrix<Archive>(ar, mDescriptors, version);
-    // BOW
-    ar& mBowVec;
-    ar& mFeatVec;
-    // Pose relative to parent
-    serializeSophusSE3<Archive>(ar, mTcp, version);
-    // Scale
-    ar& const_cast<int&>(mnScaleLevels);
-    ar& const_cast<float&>(mfScaleFactor);
-    ar& const_cast<float&>(mfLogScaleFactor);
-    ar& const_cast<vector<float>&>(mvScaleFactors);
-    ar& const_cast<vector<float>&>(mvLevelSigma2);
-    ar& const_cast<vector<float>&>(mvInvLevelSigma2);
-    // Image bounds and calibration
-    ar& const_cast<int&>(mnMinX);
-    ar& const_cast<int&>(mnMinY);
-    ar& const_cast<int&>(mnMaxX);
-    ar& const_cast<int&>(mnMaxY);
-    ar& boost::serialization::make_array(mK_.data(), mK_.size());
-    // Pose
-    serializeSophusSE3<Archive>(ar, mTcw, version);
-    // MapPointsId associated to keypoints
-    ar& mvBackupMapPointsId;
-    // Grid
-    ar& mGrid;
-    // Connected KeyFrameWeight
-    ar& mBackupConnectedKeyFrameIdWeights;
-    // Spanning Tree and Loop Edges
-    ar& mbFirstConnection;
-    ar& mBackupParentId;
-    ar& mvBackupChildrensId;
-    ar& mvBackupLoopEdgesId;
-    ar& mvBackupMergeEdgesId;
-    // Bad flags
-    ar& mbNotErase;
-    ar& mbToBeErased;
-    ar& mbBad;
-
-    ar& mHalfBaseline;
-
-    ar& mnOriginMapId;
-
-    // Camera variables
-    ar& mnBackupIdCamera;
-    ar& mnBackupIdCamera2;
-
-    // Fisheye variables
-    ar& mvLeftToRightMatch;
-    ar& mvRightToLeftMatch;
-    ar& const_cast<int&>(NLeft);
-    ar& const_cast<int&>(NRight);
-    serializeSophusSE3<Archive>(ar, mTlr, version);
-    serializeVectorKeyPoints<Archive>(ar, mvKeysRight, version);
-    ar& mGridRight;
-
-    // Inertial variables
-    ar& mImuBias;
-    ar& mBackupImuPreintegrated;
-    ar& mImuCalib;
-    ar& mBackupPrevKFId;
-    ar& mBackupNextKFId;
-    ar& bImu;
-    ar& boost::serialization::make_array(mVw.data(), mVw.size());
-    ar& boost::serialization::make_array(mOwb.data(), mOwb.size());
-    ar& mbHasVelocity;
-  }
-
- public:
-  
-  KeyFrame();
-  KeyFrame(Frame& F, std::shared_ptr<Map> pMap, KeyFrameDatabase* pKFDB);
-
-  // Pose functions
-  void SetPose(const Sophus::SE3f& Tcw);
-  void SetVelocity(const Eigen::Vector3f& Vw_);
-
-  Sophus::SE3f GetPose();
-
-  Sophus::SE3f GetPoseInverse();
-  Eigen::Vector3f GetCameraCenter();
-
-  Eigen::Vector3f GetImuPosition();
-  Eigen::Matrix3f GetImuRotation();
-  Sophus::SE3f GetImuPose();
-  Eigen::Matrix3f GetRotation();
-  Eigen::Vector3f GetTranslation();
-  Eigen::Vector3f GetVelocity();
-  bool isVelocitySet();
-
-  // Bag of Words Representation
-  void ComputeBoW();
-
-  // Covisibility graph functions
-  void AddConnection(KeyFrame* pKF, const int& weight);
-  void EraseConnection(KeyFrame* pKF);
-
-  void UpdateConnections(bool upParent = true);
-  void UpdateBestCovisibles();
-  std::set<KeyFrame*> GetConnectedKeyFrames();
-  std::vector<KeyFrame*> GetVectorCovisibleKeyFrames();
-  std::vector<KeyFrame*> GetBestCovisibilityKeyFrames(const int& N);
-  std::vector<KeyFrame*> GetCovisiblesByWeight(const int& w);
-  int GetWeight(KeyFrame* pKF);
-
-  // Spanning tree functions
-  void AddChild(KeyFrame* pKF);
-  void EraseChild(KeyFrame* pKF);
-  void ChangeParent(KeyFrame* pKF);
-  std::set<KeyFrame*> GetChilds();
-  KeyFrame* GetParent();
-  bool hasChild(KeyFrame* pKF);
-  void SetFirstConnection(bool bFirst);
-
-  // Loop Edges
-  void AddLoopEdge(KeyFrame* pKF);
-  std::set<KeyFrame*> GetLoopEdges();
-
-  // Merge Edges
-  void AddMergeEdge(KeyFrame* pKF);
-  set<KeyFrame*> GetMergeEdges();
-
-  // MapPoint observation functions
-  int GetNumberMPs();
-  void AddMapPoint(MapPoint* pMP, const size_t& idx);
-  void EraseMapPointMatch(const int& idx);
-  void EraseMapPointMatch(MapPoint* pMP);
-  void ReplaceMapPointMatch(const int& idx, MapPoint* pMP);
-  std::set<MapPoint*> GetMapPoints();
-  std::vector<MapPoint*> GetMapPointMatches();
-  int TrackedMapPoints(const int& minObs);
-  MapPoint* GetMapPoint(const size_t& idx);
-
-  // KeyPoint functions
-  std::vector<size_t> GetFeaturesInArea(const float& x, const float& y,
-                                        const float& r,
-                                        const bool bRight = false) const;
-  bool UnprojectStereo(int i, Eigen::Vector3f& x3D);
-
-  // Image
-  bool IsInImage(const float& x, const float& y) const;
-
-  // Enable/Disable bad flag changes
-  void SetNotErase();
-  void SetErase();
-
-  // Set/check bad flag
-  void SetBadFlag();
-  bool isBad();
-
-  // Compute Scene Depth (q=2 median). Used in monocular.
-  float ComputeSceneMedianDepth(const int q);
-
-  static bool weightComp(int a, int b) { return a > b; }
-
-  static bool lId(KeyFrame* pKF1, KeyFrame* pKF2) {
-    return pKF1->mnId < pKF2->mnId;
-  }
-
-  std::shared_ptr<Map> GetMap();
-  void UpdateMap(std::shared_ptr<Map> pMap);
-
-  void SetNewBias(const IMU::Bias& b);
-  Eigen::Vector3f GetGyroBias();
-
-  Eigen::Vector3f GetAccBias();
-
-  IMU::Bias GetImuBias();
-
-  bool ProjectPointDistort(MapPoint* pMP, cv::Point2f& kp, float& u, float& v);
-  bool ProjectPointUnDistort(MapPoint* pMP, cv::Point2f& kp, float& u,
-                             float& v);
-
-  void PreSave(set<KeyFrame*>& spKF, set<MapPoint*>& spMP,
-               set<shared_ptr<GeometricCamera>>& spCam);
-  void PostLoad(map<long unsigned int, KeyFrame*>& mpKFid,
-                map<long unsigned int, MapPoint*>& mpMPid,
-                map<unsigned int, std::shared_ptr<GeometricCamera>>& mpCamId);
-
-  void SetORBVocabulary(ORBVocabulary* pORBVoc);
-  void SetKeyFrameDatabase(KeyFrameDatabase* pKFDB);
-
-  bool bImu;
-
-  // The following variables are accesed from only 1 thread or never change (no
-  // mutex needed).
- public:
-  static long unsigned int nNextId;
-  long unsigned int mnId;
-  const long unsigned int mnFrameId;
-
-  const double mTimeStamp;
-
-  // Grid (to speed up feature matching)
-  const int mnGridCols;
-  const int mnGridRows;
-  const float mfGridElementWidthInv;
-  const float mfGridElementHeightInv;
-
-  // Variables used by the tracking
-  long unsigned int mnTrackReferenceForFrame;
-  long unsigned int mnFuseTargetForKF;
-
-  // Variables used by the local mapping
-  long unsigned int mnBALocalForKF;
-  long unsigned int mnBAFixedForKF;
-
-  // Number of optimizations by BA(amount of iterations in BA)
-  long unsigned int mnNumberOfOpt;
-
-  // Variables used by the keyframe database
-  long unsigned int mnLoopQuery;
-  int mnLoopWords;
-  float mLoopScore;
-  long unsigned int mnRelocQuery;
-  int mnRelocWords;
-  float mRelocScore;
-  long unsigned int mnMergeQuery;
-  int mnMergeWords;
-  float mMergeScore;
-  long unsigned int mnPlaceRecognitionQuery;
-  int mnPlaceRecognitionWords;
-  float mPlaceRecognitionScore;
-
-  bool mbCurrentPlaceRecognition;
-
-  // Variables used by loop closing
-  Sophus::SE3f mTcwGBA;
-  Sophus::SE3f mTcwBefGBA;
-  Eigen::Vector3f mVwbGBA;
-  Eigen::Vector3f mVwbBefGBA;
-  IMU::Bias mBiasGBA;
-  long unsigned int mnBAGlobalForKF;
-
-  // Variables used by merging
-  Sophus::SE3f mTcwMerge;
-  Sophus::SE3f mTcwBefMerge;
-  Sophus::SE3f mTwcBefMerge;
-  Eigen::Vector3f mVwbMerge;
-  Eigen::Vector3f mVwbBefMerge;
-  IMU::Bias mBiasMerge;
-  long unsigned int mnMergeCorrectedForKF;
-  long unsigned int mnMergeForKF;
-  float mfScaleMerge;
-  long unsigned int mnBALocalForMerge;
-
-  float mfScale;
-
-  // Calibration parameters
-  const float fx, fy, cx, cy, invfx, invfy, mbf, mb, mThDepth;
-  cv::Mat mDistCoef;
-
-  // Number of KeyPoints
-  const int N;
-
-  // KeyPoints, stereo coordinate and descriptors (all associated by an index)
-  const std::vector<cv::KeyPoint> mvKeys;
-  const std::vector<cv::KeyPoint> mvKeysUn;
-  const std::vector<float> mvuRight;  // negative value for monocular points
-  const std::vector<float> mvDepth;   // negative value for monocular points
-  const cv::Mat mDescriptors;
-
-  // BoW
-  DBoW2::BowVector mBowVec;
-  DBoW2::FeatureVector mFeatVec;
-
-  // Pose relative to parent (this is computed when bad flag is activated)
-  Sophus::SE3f mTcp;
-
-  // Scale
-  const int mnScaleLevels;
-  const float mfScaleFactor;
-  const float mfLogScaleFactor;
-  const std::vector<float> mvScaleFactors;
-  const std::vector<float> mvLevelSigma2;
-  const std::vector<float> mvInvLevelSigma2;
-
-  // Image bounds and calibration
-  const int mnMinX;
-  const int mnMinY;
-  const int mnMaxX;
-  const int mnMaxY;
-
-  // Preintegrated IMU measurements from previous keyframe
-  KeyFrame* mPrevKF;
-  KeyFrame* mNextKF;
-
-  IMU::Preintegrated* mpImuPreintegrated;
-  IMU::Calib mImuCalib;
-
-  unsigned int mnOriginMapId;
-
-  string mNameFile;
-
-  int mnDataset;
-
-  std::vector<KeyFrame*> mvpLoopCandKFs;
-  std::vector<KeyFrame*> mvpMergeCandKFs;
-
-  // bool mbHasHessian;
-  // cv::Mat mHessianPose;
-
-  // The following variables need to be accessed trough a mutex to be thread
-  // safe.
- protected:
-  // sophus poses
-  Sophus::SE3<float> mTcw;
-  Eigen::Matrix3f mRcw;
-  Sophus::SE3<float> mTwc;
-  Eigen::Matrix3f mRwc;
-
-  // IMU position
-  Eigen::Vector3f mOwb;
-  // Velocity (Only used for inertial SLAM)
-  Eigen::Vector3f mVw;
-  bool mbHasVelocity;
-
-  // Transformation matrix between cameras in stereo fisheye
-  Sophus::SE3<float> mTlr;
-  Sophus::SE3<float> mTrl;
-
-  // Imu bias
-  IMU::Bias mImuBias;
-
-  // MapPoints associated to keypoints
-  std::vector<MapPoint*> mvpMapPoints;
-  // For save relation without pointer, this is necessary for save/load function
-  std::vector<long long int> mvBackupMapPointsId;
-
-  // BoW
-  KeyFrameDatabase* mpKeyFrameDB;
-  ORBVocabulary* mpORBvocabulary;
-
-  // Grid over the image to speed up feature matching
-  std::vector<std::vector<std::vector<size_t> > > mGrid;
-
-  std::map<KeyFrame*, int> mConnectedKeyFrameWeights;
-  std::vector<KeyFrame*> mvpOrderedConnectedKeyFrames;
-  std::vector<int> mvOrderedWeights;
-  // For save relation without pointer, this is necessary for save/load function
-  std::map<long unsigned int, int> mBackupConnectedKeyFrameIdWeights;
-
-  // Spanning Tree and Loop Edges
-  bool mbFirstConnection;
-  KeyFrame* mpParent;
-  std::set<KeyFrame*> mspChildrens;
-  std::set<KeyFrame*> mspLoopEdges;
-  std::set<KeyFrame*> mspMergeEdges;
-  // For save relation without pointer, this is necessary for save/load function
-  long long int mBackupParentId;
-  std::vector<long unsigned int> mvBackupChildrensId;
-  std::vector<long unsigned int> mvBackupLoopEdgesId;
-  std::vector<long unsigned int> mvBackupMergeEdgesId;
-
-  // Bad flags
-  bool mbNotErase;
-  bool mbToBeErased;
-  bool mbBad;
-
-  float mHalfBaseline;  // Only for visualization
-
-  std::shared_ptr<Map> mpMap;
-
-  // Backup variables for inertial
-  long long int mBackupPrevKFId;
-  long long int mBackupNextKFId;
-  IMU::Preintegrated mBackupImuPreintegrated;
-
-  // Backup for Cameras
-  unsigned int mnBackupIdCamera, mnBackupIdCamera2;
-
-  // Calibration
-  Eigen::Matrix3f mK_;
-
-  // Mutex
-  std::mutex mMutexPose;  // for pose, velocity and biases
-  std::mutex mMutexConnections;
-  std::mutex mMutexFeatures;
-  std::mutex mMutexMap;
-
- public:
-  std::shared_ptr<GeometricCamera> mpCamera, mpCamera2;
-
-  // Indexes of stereo observations correspondences
-  std::vector<int> mvLeftToRightMatch, mvRightToLeftMatch;
-
-  Sophus::SE3f GetRelativePoseTrl();
-  Sophus::SE3f GetRelativePoseTlr();
-
-  // KeyPoints in the right image (for stereo fisheye, coordinates are needed)
-  const std::vector<cv::KeyPoint> mvKeysRight;
-
-  const int NLeft, NRight;
-
-  std::vector<std::vector<std::vector<size_t> > > mGridRight;
-
-  Sophus::SE3<float> GetRightPose();
-  Sophus::SE3<float> GetRightPoseInverse();
-
-  Eigen::Vector3f GetRightCameraCenter();
-  Eigen::Matrix<float, 3, 3> GetRightRotation();
-  Eigen::Vector3f GetRightTranslation();
-
-  void PrintPointDistribution() {
-    int left = 0, right = 0;
-    int Nlim = (NLeft != -1) ? NLeft : N;
-    for (int i = 0; i < N; i++) {
-      if (mvpMapPoints[i]) {
-        if (i < Nlim)
-          left++;
-        else
-          right++;
-      }
-    }
-    cout << "Point distribution in KeyFrame: left-> " << left << " --- right-> "
-         << right << endl;
-  }
-};
-
-}  // namespace ORB_SLAM3
-
diff --git a/include/KeyFrameDatabase.h b/include/KeyFrameDatabase.h
deleted file mode 100644
index 52b46faf..00000000
--- a/include/KeyFrameDatabase.h
+++ /dev/null
@@ -1,99 +0,0 @@
-/**
- * This file is part of ORB-SLAM3
- *
- * Copyright (C) 2017-2021 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/>.
- */
-
-#pragma once
-
-#include <boost/serialization/base_object.hpp>
-#include <boost/serialization/list.hpp>
-#include <boost/serialization/vector.hpp>
-#include <list>
-#include <mutex>
-#include <set>
-#include <vector>
-
-#include "Frame.h"
-#include "KeyFrame.h"
-#include "Map.h"
-#include "ORBVocabulary.h"
-
-namespace ORB_SLAM3 {
-
-class KeyFrame;
-class Frame;
-class Map;
-
-class KeyFrameDatabase {
-  friend class boost::serialization::access;
-
-  template <class Archive>
-  void serialize(Archive& ar, const unsigned int version) {
-    ar& mvBackupInvertedFileId;
-  }
-
- public:
-  
-
-  KeyFrameDatabase() {}
-  KeyFrameDatabase(const ORBVocabulary& voc);
-
-  void add(KeyFrame* pKF);
-
-  void erase(KeyFrame* pKF);
-
-  void clear();
-  void clearMap(std::shared_ptr<Map> pMap);
-
-  // Loop Detection(DEPRECATED)
-  std::vector<KeyFrame*> DetectLoopCandidates(KeyFrame* pKF, float minScore);
-
-  // Loop and Merge Detection
-  void DetectCandidates(KeyFrame* pKF, float minScore,
-                        vector<KeyFrame*>& vpLoopCand,
-                        vector<KeyFrame*>& vpMergeCand);
-  void DetectBestCandidates(KeyFrame* pKF, vector<KeyFrame*>& vpLoopCand,
-                            vector<KeyFrame*>& vpMergeCand, int nMinWords);
-  void DetectNBestCandidates(KeyFrame* pKF, vector<KeyFrame*>& vpLoopCand,
-                             vector<KeyFrame*>& vpMergeCand,
-                             int nNumCandidates);
-
-  // Relocalization
-  std::vector<KeyFrame*> DetectRelocalizationCandidates(Frame* F, std::shared_ptr<Map> pMap);
-
-  void PreSave();
-  void PostLoad(map<long unsigned int, KeyFrame*> mpKFid);
-  void SetORBVocabulary(ORBVocabulary* pORBVoc);
-
- protected:
-  // Associated vocabulary
-  const ORBVocabulary* mpVoc;
-
-  // Inverted file
-  std::vector<list<KeyFrame*> > mvInvertedFile;
-
-  // For save relation without pointer, this is necessary for save/load function
-  std::vector<list<long unsigned int> > mvBackupInvertedFileId;
-
-  // Mutex
-  std::mutex mMutex;
-};
-
-}  // namespace ORB_SLAM3
-
diff --git a/include/LocalMapping.h b/include/LocalMapping.h
deleted file mode 100644
index 77a9a6f4..00000000
--- a/include/LocalMapping.h
+++ /dev/null
@@ -1,198 +0,0 @@
-/**
- * This file is part of ORB-SLAM3
- *
- * Copyright (C) 2017-2021 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/>.
- */
-
-#pragma once
-
-#include <mutex>
-
-#include "ImprovedTypes.hpp"
-#include "Atlas.h"
-#include "KeyFrame.h"
-#include "KeyFrameDatabase.h"
-#include "LoopClosing.h"
-#include "Settings.h"
-#include "Tracking.h"
-
-namespace ORB_SLAM3 {
-
-class System;
-class Tracking;
-class LoopClosing;
-class Atlas;
-
-class LocalMapping {
- public:
-  
-  LocalMapping(System* pSys, const Atlas_ptr &pAtlas, const float bMonocular,
-               bool bInertial, const string& _strSeqName = std::string());
-
-  void SetLoopCloser(LoopClosing* pLoopCloser);
-
-  void SetTracker(Tracking* pTracker);
-
-  // Main function
-  void Run();
-
-  void InsertKeyFrame(KeyFrame* pKF);
-  void EmptyQueue();
-
-  // Thread Synch
-  void RequestStop();
-  void RequestReset();
-  void RequestResetActiveMap(std::shared_ptr<Map> pMap);
-  bool Stop();
-  void Release();
-  bool isStopped();
-  bool stopRequested();
-  bool AcceptKeyFrames();
-  void SetAcceptKeyFrames(bool flag);
-  bool SetNotStop(bool flag);
-
-  void InterruptBA();
-
-  void RequestFinish();
-  bool isFinished();
-
-  int KeyframesInQueue() {
-    unique_lock<std::mutex> lock(mMutexNewKFs);
-    return mlNewKeyFrames.size();
-  }
-
-  bool IsInitializing();
-  double GetCurrKFTime();
-  KeyFrame* GetCurrKF();
-
-  std::mutex mMutexImuInit;
-
-  Eigen::MatrixXd mcovInertial;
-  Eigen::Matrix3d mRwg;
-  Eigen::Vector3d mbg;
-  Eigen::Vector3d mba;
-  double mScale;
-  double mInitTime;
-  double mCostTime;
-
-  unsigned int mInitSect;
-  unsigned int mIdxInit;
-  unsigned int mnKFs;
-  double mFirstTs;
-  int mnMatchesInliers;
-
-  // For debugging (erase in normal mode)
-  int mIdxIteration;
-  string strSequence;
-
-  bool mbNotBA1;
-  bool mbNotBA2;
-  bool mbBadImu;
-
-  bool mbWriteStats;
-
-  // not consider far points (clouds)
-  bool mbFarPoints;
-  float mThFarPoints;
-
-#ifdef REGISTER_TIMES
-  vector<double> vdKFInsert_ms;
-  vector<double> vdMPCulling_ms;
-  vector<double> vdMPCreation_ms;
-  vector<double> vdLBA_ms;
-  vector<double> vdKFCulling_ms;
-  vector<double> vdLMTotal_ms;
-
-  vector<double> vdLBASync_ms;
-  vector<double> vdKFCullingSync_ms;
-  vector<int> vnLBA_edges;
-  vector<int> vnLBA_KFopt;
-  vector<int> vnLBA_KFfixed;
-  vector<int> vnLBA_MPs;
-  int nLBA_exec;
-  int nLBA_abort;
-#endif
- protected:
-  bool CheckNewKeyFrames();
-  void ProcessNewKeyFrame();
-  void CreateNewMapPoints();
-
-  void MapPointCulling();
-  void SearchInNeighbors();
-  void KeyFrameCulling();
-
-  System* mpSystem;
-
-  bool mbMonocular;
-  bool mbInertial;
-
-  void ResetIfRequested();
-  bool mbResetRequested;
-  bool mbResetRequestedActiveMap;
-  std::shared_ptr<Map> mpMapToReset;
-  std::mutex mMutexReset;
-
-  bool CheckFinish();
-  void SetFinish();
-  bool mbFinishRequested;
-  bool mbFinished;
-  std::mutex mMutexFinish;
-
-  Atlas_ptr mpAtlas;
-
-  LoopClosing* mpLoopCloser;
-  Tracking* mpTracker;
-
-  std::list<KeyFrame*> mlNewKeyFrames;
-
-  KeyFrame* mpCurrentKeyFrame;
-
-  std::list<MapPoint*> mlpRecentAddedMapPoints;
-
-  std::mutex mMutexNewKFs;
-
-  bool mbAbortBA;
-
-  bool mbStopped;
-  bool mbStopRequested;
-  bool mbNotStop;
-  std::mutex mMutexStop;
-
-  bool mbAcceptKeyFrames;
-  std::mutex mMutexAccept;
-
-  void InitializeIMU(float priorG = 1e2, float priorA = 1e6,
-                     bool bFirst = false);
-  void ScaleRefinement();
-
-  bool bInitializing;
-
-  Eigen::MatrixXd infoInertial;
-  int mNumLM;
-  int mNumKFCulling;
-
-  float mTinit;
-
-  int countRefinement;
-
-  // DEBUG
-  ofstream f_lm;
-};
-
-}  // namespace ORB_SLAM3
-
diff --git a/include/LoopClosing.h b/include/LoopClosing.h
deleted file mode 100644
index 35ba0980..00000000
--- a/include/LoopClosing.h
+++ /dev/null
@@ -1,245 +0,0 @@
-/**
-* This file is part of ORB-SLAM3
-*
-* Copyright (C) 2017-2021 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/>.
-*/
-
-
-#pragma once
-
-#include "ImprovedTypes.hpp"
-#include "KeyFrame.h"
-#include "LocalMapping.h"
-#include "Atlas.h"
-#include "ORBVocabulary.h"
-#include "Tracking.h"
-
-#include "KeyFrameDatabase.h"
-
-#include <boost/algorithm/string.hpp>
-#include <thread>
-#include <mutex>
-#include "g2o/types/types_seven_dof_expmap.h"
-
-namespace ORB_SLAM3
-{
-
-class Tracking;
-class LocalMapping;
-class KeyFrameDatabase;
-class Map;
-
-
-class LoopClosing
-{
-public:
-
-    typedef pair<set<KeyFrame*>,int> ConsistentGroup;
-    typedef map<KeyFrame*,g2o::Sim3,std::less<KeyFrame*>,
-        Eigen::aligned_allocator<std::pair<KeyFrame* const, g2o::Sim3> > > KeyFrameAndPose;
-
-public:
-
-    LoopClosing(const Atlas_ptr &pAtlas, KeyFrameDatabase* pDB, ORBVocabulary* pVoc,const bool bFixScale, const bool bActiveLC);
-
-    void SetTracker(Tracking* pTracker);
-
-    void SetLocalMapper(LocalMapping* pLocalMapper);
-
-    // Main function
-    void Run();
-
-    void InsertKeyFrame(KeyFrame *pKF);
-
-    void RequestReset();
-    void RequestResetActiveMap(std::shared_ptr<Map> pMap);
-
-    // This function will run in a separate thread
-    void RunGlobalBundleAdjustment(std::shared_ptr<Map> pActiveMap, unsigned long nLoopKF);
-
-    bool isRunningGBA(){
-        unique_lock<std::mutex> lock(mMutexGBA);
-        return mbRunningGBA;
-    }
-    bool isFinishedGBA(){
-        unique_lock<std::mutex> lock(mMutexGBA);
-        return mbFinishedGBA;
-    }
-
-    void RequestFinish();
-
-    bool isFinished();
-
-#ifdef REGISTER_TIMES
-
-    vector<double> vdDataQuery_ms;
-    vector<double> vdEstSim3_ms;
-    vector<double> vdPRTotal_ms;
-
-    vector<double> vdMergeMaps_ms;
-    vector<double> vdWeldingBA_ms;
-    vector<double> vdMergeOptEss_ms;
-    vector<double> vdMergeTotal_ms;
-    vector<int> vnMergeKFs;
-    vector<int> vnMergeMPs;
-    int nMerges;
-
-    vector<double> vdLoopFusion_ms;
-    vector<double> vdLoopOptEss_ms;
-    vector<double> vdLoopTotal_ms;
-    vector<int> vnLoopKFs;
-    int nLoop;
-
-    vector<double> vdGBA_ms;
-    vector<double> vdUpdateMap_ms;
-    vector<double> vdFGBATotal_ms;
-    vector<int> vnGBAKFs;
-    vector<int> vnGBAMPs;
-    int nFGBA_exec;
-    int nFGBA_abort;
-
-#endif
-
-    
-
-protected:
-
-    bool CheckNewKeyFrames();
-
-
-    //Methods to implement the new place recognition algorithm
-    bool NewDetectCommonRegions();
-    bool DetectAndReffineSim3FromLastKF(KeyFrame* pCurrentKF, KeyFrame* pMatchedKF, g2o::Sim3 &gScw, int &nNumProjMatches,
-                                        std::vector<MapPoint*> &vpMPs, std::vector<MapPoint*> &vpMatchedMPs);
-    bool DetectCommonRegionsFromBoW(std::vector<KeyFrame*> &vpBowCand, KeyFrame* &pMatchedKF, KeyFrame* &pLastCurrentKF, g2o::Sim3 &g2oScw,
-                                     int &nNumCoincidences, std::vector<MapPoint*> &vpMPs, std::vector<MapPoint*> &vpMatchedMPs);
-    bool DetectCommonRegionsFromLastKF(KeyFrame* pCurrentKF, KeyFrame* pMatchedKF, g2o::Sim3 &gScw, int &nNumProjMatches,
-                                            std::vector<MapPoint*> &vpMPs, std::vector<MapPoint*> &vpMatchedMPs);
-    int FindMatchesByProjection(KeyFrame* pCurrentKF, KeyFrame* pMatchedKFw, g2o::Sim3 &g2oScw,
-                                set<MapPoint*> &spMatchedMPinOrigin, vector<MapPoint*> &vpMapPoints,
-                                vector<MapPoint*> &vpMatchedMapPoints);
-
-
-    void SearchAndFuse(const KeyFrameAndPose &CorrectedPosesMap, vector<MapPoint*> &vpMapPoints);
-    void SearchAndFuse(const vector<KeyFrame*> &vConectedKFs, vector<MapPoint*> &vpMapPoints);
-
-    void CorrectLoop();
-
-    void MergeLocal();
-    void MergeLocal2();
-
-    void CheckObservations(set<KeyFrame*> &spKFsMap1, set<KeyFrame*> &spKFsMap2);
-
-    void ResetIfRequested();
-    bool mbResetRequested;
-    bool mbResetActiveMapRequested;
-    std::shared_ptr<Map> mpMapToReset;
-    std::mutex mMutexReset;
-
-    bool CheckFinish();
-    void SetFinish();
-    bool mbFinishRequested;
-    bool mbFinished;
-    std::mutex mMutexFinish;
-
-    Atlas_ptr mpAtlas;
-    Tracking* mpTracker;
-
-    KeyFrameDatabase* mpKeyFrameDB;
-    ORBVocabulary* mpORBVocabulary;
-
-    LocalMapping *mpLocalMapper;
-
-    std::list<KeyFrame*> mlpLoopKeyFrameQueue;
-
-    std::mutex mMutexLoopQueue;
-
-    // Loop detector parameters
-    float mnCovisibilityConsistencyTh;
-
-    // Loop detector variables
-    KeyFrame* mpCurrentKF;
-    KeyFrame* mpLastCurrentKF;
-    KeyFrame* mpMatchedKF;
-    std::vector<ConsistentGroup> mvConsistentGroups;
-    std::vector<KeyFrame*> mvpEnoughConsistentCandidates;
-    std::vector<KeyFrame*> mvpCurrentConnectedKFs;
-    std::vector<MapPoint*> mvpCurrentMatchedPoints;
-    std::vector<MapPoint*> mvpLoopMapPoints;
-    cv::Mat mScw;
-    g2o::Sim3 mg2oScw;
-
-    //-------
-    std::shared_ptr<Map> mpLastMap;
-
-    bool mbLoopDetected;
-    int mnLoopNumCoincidences;
-    int mnLoopNumNotFound;
-    KeyFrame* mpLoopLastCurrentKF;
-    g2o::Sim3 mg2oLoopSlw;
-    g2o::Sim3 mg2oLoopScw;
-    KeyFrame* mpLoopMatchedKF;
-    std::vector<MapPoint*> mvpLoopMPs;
-    std::vector<MapPoint*> mvpLoopMatchedMPs;
-    bool mbMergeDetected;
-    int mnMergeNumCoincidences;
-    int mnMergeNumNotFound;
-    KeyFrame* mpMergeLastCurrentKF;
-    g2o::Sim3 mg2oMergeSlw;
-    g2o::Sim3 mg2oMergeSmw;
-    g2o::Sim3 mg2oMergeScw;
-    KeyFrame* mpMergeMatchedKF;
-    std::vector<MapPoint*> mvpMergeMPs;
-    std::vector<MapPoint*> mvpMergeMatchedMPs;
-    std::vector<KeyFrame*> mvpMergeConnectedKFs;
-
-    g2o::Sim3 mSold_new;
-    //-------
-
-    // Variables related to Global Bundle Adjustment
-    bool mbRunningGBA;
-    bool mbFinishedGBA;
-    bool mbStopGBA;
-    std::mutex mMutexGBA;
-    std::thread* mpThreadGBA;
-
-    // Fix scale in the stereo/RGB-D case
-    bool mbFixScale;
-
-
-    int mnFullBAIdx;
-
-
-
-    vector<double> vdPR_CurrentTime;
-    vector<double> vdPR_MatchedTime;
-    vector<int> vnPR_TypeRecogn;
-
-    //DEBUG
-    string mstrFolderSubTraj;
-    int mnNumCorrection;
-    int mnCorrectionGBA;
-
-
-    // To (de)activate LC
-    bool mbActiveLC = true;
-
-#ifdef REGISTER_LOOP
-    string mstrFolderLoop;
-#endif
-};
-
-} //namespace ORB_SLAM
-
diff --git a/include/MLPnPsolver.h b/include/MLPnPsolver.h
deleted file mode 100644
index a160db1c..00000000
--- a/include/MLPnPsolver.h
+++ /dev/null
@@ -1,243 +0,0 @@
-/**
- * This file is part of ORB-SLAM3
- *
- * Copyright (C) 2017-2021 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/>.
- */
-
-/******************************************************************************
- * Author:   Steffen Urban                                              *
- * Contact:  urbste@gmail.com                                          *
- * License:  Copyright (c) 2016 Steffen Urban, ANU. All rights reserved.      *
- *                                                                            *
- * Redistribution and use in source and binary forms, with or without         *
- * modification, are permitted provided that the following conditions         *
- * are met:                                                                   *
- * * Redistributions of source code must retain the above copyright           *
- *   notice, this list of conditions and the following disclaimer.            *
- * * Redistributions in binary form must reproduce the above copyright        *
- *   notice, this list of conditions and the following disclaimer in the      *
- *   documentation and/or other materials provided with the distribution.     *
- * * Neither the name of ANU nor the names of its contributors may be         *
- *   used to endorse or promote products derived from this software without   *
- *   specific prior written permission.                                       *
- *                                                                            *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"*
- * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE  *
- * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE *
- * ARE DISCLAIMED. IN NO EVENT SHALL ANU OR THE CONTRIBUTORS BE LIABLE        *
- * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL *
- * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR *
- * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER *
- * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT         *
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY  *
- * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF     *
- * SUCH DAMAGE.                                                               *
- ******************************************************************************/
-
-#pragma once
-
-#include <Eigen/Dense>
-#include <Eigen/Sparse>
-
-#include "Frame.h"
-#include "MapPoint.h"
-
-namespace ORB_SLAM3 {
-class MLPnPsolver {
- public:
-  
-
-  MLPnPsolver(const Frame& F, const vector<MapPoint*>& vpMapPointMatches);
-
-  ~MLPnPsolver();
-
-  void SetRansacParameters(double probability = 0.99, int minInliers = 8,
-                           int maxIterations = 300, int minSet = 6,
-                           float epsilon = 0.4, float th2 = 5.991);
-
-  // Find metod is necessary?
-
-  bool iterate(int nIterations, bool& bNoMore, vector<bool>& vbInliers,
-               int& nInliers, Eigen::Matrix4f& Tout);
-
-  // Type definitions needed by the original code
-
-  /** A 3-vector of unit length used to describe landmark observations/bearings
-   *  in camera frames (always expressed in camera frames)
-   */
-  typedef Eigen::Vector3d bearingVector_t;
-
-  /** An array of bearing-vectors */
-  typedef std::vector<bearingVector_t,
-                      Eigen::aligned_allocator<bearingVector_t> >
-      bearingVectors_t;
-
-  /** A 2-matrix containing the 2D covariance information of a bearing vector
-   */
-  typedef Eigen::Matrix2d cov2_mat_t;
-
-  /** A 3-matrix containing the 3D covariance information of a bearing vector */
-  typedef Eigen::Matrix3d cov3_mat_t;
-
-  /** An array of 3D covariance matrices */
-  typedef std::vector<cov3_mat_t, Eigen::aligned_allocator<cov3_mat_t> >
-      cov3_mats_t;
-
-  /** A 3-vector describing a point in 3D-space */
-  typedef Eigen::Vector3d point_t;
-
-  /** An array of 3D-points */
-  typedef std::vector<point_t, Eigen::aligned_allocator<point_t> > points_t;
-
-  /** A homogeneous 3-vector describing a point in 3D-space */
-  typedef Eigen::Vector4d point4_t;
-
-  /** An array of homogeneous 3D-points */
-  typedef std::vector<point4_t, Eigen::aligned_allocator<point4_t> > points4_t;
-
-  /** A 3-vector containing the rodrigues parameters of a rotation matrix */
-  typedef Eigen::Vector3d rodrigues_t;
-
-  /** A rotation matrix */
-  typedef Eigen::Matrix3d rotation_t;
-
-  /** A 3x4 transformation matrix containing rotation \f$ \mathbf{R} \f$ and
-   *  translation \f$ \mathbf{t} \f$ as follows:
-   *  \f$ \left( \begin{array}{cc} \mathbf{R} & \mathbf{t} \end{array} \right)
-   * \f$
-   */
-  typedef Eigen::Matrix<double, 3, 4> transformation_t;
-
-  /** A 3-vector describing a translation/camera position */
-  typedef Eigen::Vector3d translation_t;
-
- private:
-  void CheckInliers();
-  bool Refine();
-
-  // Functions from de original MLPnP code
-
-  /*
-   * Computes the camera pose given 3D points coordinates (in the camera
-   * reference system), the camera rays and (optionally) the covariance matrix
-   * of those camera rays. Result is stored in solution
-   */
-  void computePose(const bearingVectors_t& f, const points_t& p,
-                   const cov3_mats_t& covMats, const std::vector<int>& indices,
-                   transformation_t& result);
-
-  void mlpnp_gn(Eigen::VectorXd& x, const points_t& pts,
-                const std::vector<Eigen::MatrixXd>& nullspaces,
-                const Eigen::SparseMatrix<double> Kll, bool use_cov);
-
-  void mlpnp_residuals_and_jacs(const Eigen::VectorXd& x, const points_t& pts,
-                                const std::vector<Eigen::MatrixXd>& nullspaces,
-                                Eigen::VectorXd& r, Eigen::MatrixXd& fjac,
-                                bool getJacs);
-
-  void mlpnpJacs(const point_t& pt, const Eigen::Vector3d& nullspace_r,
-                 const Eigen::Vector3d& nullspace_s, const rodrigues_t& w,
-                 const translation_t& t, Eigen::MatrixXd& jacs);
-
-  // Auxiliar methods
-
-  /**
-   * \brief Compute a rotation matrix from Rodrigues axis angle.
-   *
-   * \param[in] omega The Rodrigues-parameters of a rotation.
-   * \return The 3x3 rotation matrix.
-   */
-  Eigen::Matrix3d rodrigues2rot(const Eigen::Vector3d& omega);
-
-  /**
-   * \brief Compute the Rodrigues-parameters of a rotation matrix.
-   *
-   * \param[in] R The 3x3 rotation matrix.
-   * \return The Rodrigues-parameters.
-   */
-  Eigen::Vector3d rot2rodrigues(const Eigen::Matrix3d& R);
-
-  //----------------------------------------------------
-  // Fields of the solver
-  //----------------------------------------------------
-  vector<MapPoint*> mvpMapPointMatches;
-
-  // 2D Points
-  vector<cv::Point2f> mvP2D;
-  // Substitued by bearing vectors
-  bearingVectors_t mvBearingVecs;
-
-  vector<float> mvSigma2;
-
-  // 3D Points
-  // vector<cv::Point3f> mvP3Dw;
-  points_t mvP3Dw;
-
-  // Index in Frame
-  vector<size_t> mvKeyPointIndices;
-
-  // Current Estimation
-  double mRi[3][3];
-  double mti[3];
-  Eigen::Matrix4f mTcwi;
-  vector<bool> mvbInliersi;
-  int mnInliersi;
-
-  // Current Ransac State
-  int mnIterations;
-  vector<bool> mvbBestInliers;
-  int mnBestInliers;
-  Eigen::Matrix4f mBestTcw;
-
-  // Refined
-  Eigen::Matrix4f mRefinedTcw;
-  vector<bool> mvbRefinedInliers;
-  int mnRefinedInliers;
-
-  // Number of Correspondences
-  int N;
-
-  // Indices for random selection [0 .. N-1]
-  vector<size_t> mvAllIndices;
-
-  // RANSAC probability
-  double mRansacProb;
-
-  // RANSAC min inliers
-  int mRansacMinInliers;
-
-  // RANSAC max iterations
-  int mRansacMaxIts;
-
-  // RANSAC expected inliers/total ratio
-  float mRansacEpsilon;
-
-  // RANSAC Threshold inlier/outlier. Max error e = dist(P1,T_12*P2)^2
-  float mRansacTh;
-
-  // RANSAC Minimun Set used at each iteration
-  int mRansacMinSet;
-
-  // Max square error associated with scale level. Max error =
-  // th*th*sigma(level)*sigma(level)
-  vector<float> mvMaxError;
-
-  std::shared_ptr<GeometricCamera> mpCamera;
-};
-
-}  // namespace ORB_SLAM3
diff --git a/include/Map.h b/include/Map.h
deleted file mode 100644
index fb40964d..00000000
--- a/include/Map.h
+++ /dev/null
@@ -1,205 +0,0 @@
-/**
-* This file is part of ORB-SLAM3
-*
-* Copyright (C) 2017-2021 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/>.
-*/
-
-
-#pragma once
-
-#include "MapPoint.h"
-#include "KeyFrame.h"
-
-#include <set>
-#include <pangolin/pangolin.h>
-#include <mutex>
-#include <memory>
-#include <boost/serialization/base_object.hpp>
-#include <stdexcept>
-
-
-namespace ORB_SLAM3
-{
-
-class MapPoint;
-class KeyFrame;
-class Atlas;
-class KeyFrameDatabase;
-
-class Map
-{
-    friend class boost::serialization::access;
-
-    template<class Archive>
-    void serialize(Archive &ar, const unsigned int version)
-    {
-        ar & mnId;
-        ar & mnInitKFid;
-        ar & mnMaxKFid;
-        ar & mnBigChangeIdx;
-
-        // Save/load a set structure, the set structure is broken in libboost 1.58 for ubuntu 16.04, a vector is serializated
-        //ar & mspKeyFrames;
-        //ar & mspMapPoints;
-        ar & mvpBackupKeyFrames;
-        ar & mvpBackupMapPoints;
-
-        ar & mvBackupKeyFrameOriginsId;
-
-        ar & mnBackupKFinitialID;
-        ar & mnBackupKFlowerID;
-
-        ar & mbImuInitialized;
-        ar & mbIsInertial;
-        ar & mbIMU_BA1;
-        ar & mbIMU_BA2;
-    }
-
-public:
-    
-    Map();
-    Map(int initKFid);
-    ~Map();
-
-    void AddKeyFrame(KeyFrame* pKF);
-    void AddMapPoint(MapPoint* pMP);
-    void EraseMapPoint(MapPoint* pMP);
-    void EraseKeyFrame(KeyFrame* pKF);
-    void SetReferenceMapPoints(const std::vector<MapPoint*> &vpMPs);
-    void InformNewBigChange();
-    int GetLastBigChangeIdx();
-
-    std::vector<KeyFrame*> GetAllKeyFrames();
-    std::vector<MapPoint*> GetAllMapPoints();
-    std::vector<MapPoint*> GetReferenceMapPoints();
-
-    long unsigned int MapPointsInMap();
-    long unsigned  KeyFramesInMap();
-
-    long unsigned int GetId();
-
-    long unsigned int GetInitKFid();
-    void SetInitKFid(long unsigned int initKFif);
-    long unsigned int GetMaxKFid();
-
-    KeyFrame* GetOriginKF();
-
-    void SetCurrentMap();
-    void SetStoredMap();
-
-    bool HasThumbnail();
-    bool IsInUse();
-
-    void SetBad();
-    bool IsBad();
-
-    void clear();
-
-    int GetMapChangeIndex();
-    void IncreaseChangeIndex();
-    int GetLastMapChange();
-    void SetLastMapChange(int currentChangeId);
-
-    void SetImuInitialized();
-    bool isImuInitialized();
-
-    void ApplyScaledRotation(const Sophus::SE3f &T, const float s, const bool bScaledVel=false);
-
-    void SetInertialSensor();
-    bool IsInertial();
-    void SetIniertialBA1();
-    void SetIniertialBA2();
-    bool GetIniertialBA1();
-    bool GetIniertialBA2();
-
-    void PrintEssentialGraph();
-    bool CheckEssentialGraph();
-    void ChangeId(long unsigned int nId);
-
-    unsigned int GetLowerKFID();
-
-    void PreSave(std::set<std::shared_ptr<GeometricCamera>> &spCams, std::shared_ptr<Map> sharedMap);
-    void PostLoad(KeyFrameDatabase* pKFDB, ORBVocabulary* pORBVoc/*, map<long unsigned int, KeyFrame*>& mpKeyFrameId*/, map<unsigned int, std::shared_ptr<GeometricCamera>> &mpCams, std::shared_ptr<Map> sharedMap);
-
-    void printReprojectionError(list<KeyFrame*> &lpLocalWindowKFs, KeyFrame* mpCurrentKF, string &name, string &name_folder);
-
-    vector<KeyFrame*> mvpKeyFrameOrigins;
-    vector<unsigned long int> mvBackupKeyFrameOriginsId;
-    KeyFrame* mpFirstRegionKF;
-    std::mutex mMutexMapUpdate;
-
-    // This avoid that two points are created simultaneously in separate threads (id conflict)
-    std::mutex mMutexPointCreation;
-
-    bool mbFail;
-
-    // Size of the thumbnail (always in power of 2)
-    static const int THUMB_WIDTH = 512;
-    static const int THUMB_HEIGHT = 512;
-
-    static long unsigned int nNextId;
-
-    // DEBUG: show KFs which are used in LBA
-    std::set<long unsigned int> msOptKFs;
-    std::set<long unsigned int> msFixedKFs;
-
-protected:
-
-    long unsigned int mnId;
-
-    std::set<MapPoint*> mspMapPoints;
-    std::set<KeyFrame*> mspKeyFrames;
-
-    // Save/load, the set structure is broken in libboost 1.58 for ubuntu 16.04, a vector is serializated
-    std::vector<MapPoint*> mvpBackupMapPoints;
-    std::vector<KeyFrame*> mvpBackupKeyFrames;
-
-    KeyFrame* mpKFinitial;
-    KeyFrame* mpKFlowerID;
-
-    long int mnBackupKFinitialID;
-    long int mnBackupKFlowerID;
-
-    std::vector<MapPoint*> mvpReferenceMapPoints;
-
-    bool mbImuInitialized;
-
-    int mnMapChange;
-    int mnMapChangeNotified;
-
-    long unsigned int mnInitKFid;
-    long unsigned int mnMaxKFid;
-
-    // Index related to a big change in the map (loop closure, global BA)
-    int mnBigChangeIdx;
-
-
-    // View of the map in aerial sight (for the AtlasViewer)
-    GLubyte* mThumbnail;
-
-    bool mIsInUse;
-    bool mHasTumbnail;
-    bool mbBad = false;
-
-    bool mbIsInertial;
-    bool mbIMU_BA1;
-    bool mbIMU_BA2;
-
-    // Mutex
-    std::mutex mMutexMap;
-
-};
-
-} //namespace ORB_SLAM3
diff --git a/include/MapDrawer.h b/include/MapDrawer.h
deleted file mode 100644
index 03f78f77..00000000
--- a/include/MapDrawer.h
+++ /dev/null
@@ -1,73 +0,0 @@
-/**
-* This file is part of ORB-SLAM3
-*
-* Copyright (C) 2017-2021 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/>.
-*/
-
-
-#pragma once
-
-#include "ImprovedTypes.hpp"
-#include "Settings.h"
-#include<pangolin/pangolin.h>
-
-#include<mutex>
-
-namespace ORB_SLAM3
-{
-
-class Settings;
-class KeyFrame;
-
-class MapDrawer
-{
-    void newParameterLoader(const Settings& settings);
-    Atlas_ptr mpAtlas;
-    
-    bool ParseViewerParamFile(cv::FileStorage &fSettings);
-
-    float mKeyFrameSize;
-    float mKeyFrameLineWidth;
-    float mGraphLineWidth;
-    float mPointSize;
-    float mCameraSize;
-    float mCameraLineWidth;
-
-    Sophus::SE3f mCameraPose;
-
-    std::mutex mMutexCamera;
-
-    float mfFrameColors[6][3] = {{0.0f, 0.0f, 1.0f},
-                                {0.8f, 0.4f, 1.0f},
-                                {1.0f, 0.2f, 0.4f},
-                                {0.6f, 0.0f, 1.0f},
-                                {1.0f, 1.0f, 0.0f},
-                                {0.0f, 1.0f, 1.0f}};
-
-public:
-    MapDrawer(const Atlas_ptr &pAtlas, const std::string &strSettingPath);
-    MapDrawer(const Atlas_ptr &pAtlas, const Settings& settings);
-
-    void DrawMapPoints();
-    void DrawKeyFrames(const bool bDrawKF, const bool bDrawGraph, const bool bDrawInertialGraph, const bool bDrawOptLba);
-    void DrawCurrentCamera(pangolin::OpenGlMatrix &Twc);
-    void SetCurrentCameraPose(const Sophus::SE3f &Tcw);
-    void SetReferenceKeyFrame(KeyFrame *pKF);
-    void GetCurrentOpenGLCameraMatrix(pangolin::OpenGlMatrix &M, pangolin::OpenGlMatrix &MOw);
-
-};
-
-} //namespace ORB_SLAM
-
diff --git a/include/MapPoint.h b/include/MapPoint.h
deleted file mode 100644
index 5725fa18..00000000
--- a/include/MapPoint.h
+++ /dev/null
@@ -1,253 +0,0 @@
-/**
-* This file is part of ORB-SLAM3
-*
-* Copyright (C) 2017-2021 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/>.
-*/
-
-
-#pragma once
-
-#include "KeyFrame.h"
-#include "Frame.h"
-#include "Map.h"
-#include "Converter.h"
-
-#include "SerializationUtils.h"
-
-#include <opencv2/core/core.hpp>
-#include <mutex>
-
-#include <boost/serialization/serialization.hpp>
-#include <boost/serialization/array.hpp>
-#include <boost/serialization/map.hpp>
-
-namespace ORB_SLAM3
-{
-
-class KeyFrame;
-class Map;
-class Frame;
-
-class MapPoint
-{
-
-    friend class boost::serialization::access;
-    template<class Archive>
-    void serialize(Archive & ar, const unsigned int version)
-    {
-        ar & mnId;
-        ar & mnFirstKFid;
-        ar & mnFirstFrame;
-        ar & nObs;
-        // Variables used by the tracking
-        //ar & mTrackProjX;
-        //ar & mTrackProjY;
-        //ar & mTrackDepth;
-        //ar & mTrackDepthR;
-        //ar & mTrackProjXR;
-        //ar & mTrackProjYR;
-        //ar & mbTrackInView;
-        //ar & mbTrackInViewR;
-        //ar & mnTrackScaleLevel;
-        //ar & mnTrackScaleLevelR;
-        //ar & mTrackViewCos;
-        //ar & mTrackViewCosR;
-        //ar & mnTrackReferenceForFrame;
-        //ar & mnLastFrameSeen;
-
-        // Variables used by local mapping
-        //ar & mnBALocalForKF;
-        //ar & mnFuseCandidateForKF;
-
-        // Variables used by loop closing and merging
-        //ar & mnLoopPointForKF;
-        //ar & mnCorrectedByKF;
-        //ar & mnCorrectedReference;
-        //serializeMatrix(ar,mPosGBA,version);
-        //ar & mnBAGlobalForKF;
-        //ar & mnBALocalForMerge;
-        //serializeMatrix(ar,mPosMerge,version);
-        //serializeMatrix(ar,mNormalVectorMerge,version);
-
-        // Protected variables
-        ar & boost::serialization::make_array(mWorldPos.data(), mWorldPos.size());
-        ar & boost::serialization::make_array(mNormalVector.data(), mNormalVector.size());
-        //ar & BOOST_SERIALIZATION_NVP(mBackupObservationsId);
-        //ar & mObservations;
-        ar & mBackupObservationsId1;
-        ar & mBackupObservationsId2;
-        serializeMatrix(ar,mDescriptor,version);
-        ar & mBackupRefKFId;
-        //ar & mnVisible;
-        //ar & mnFound;
-
-        ar & mbBad;
-        ar & mBackupReplacedId;
-
-        ar & mfMinDistance;
-        ar & mfMaxDistance;
-
-    }
-
-
-public:
-    
-    MapPoint();
-
-    MapPoint(const Eigen::Vector3f &Pos, KeyFrame* pRefKF, std::shared_ptr<Map> pMap);
-    MapPoint(const double invDepth, cv::Point2f uv_init, KeyFrame* pRefKF, KeyFrame* pHostKF, std::shared_ptr<Map> pMap);
-    MapPoint(const Eigen::Vector3f &Pos,  std::shared_ptr<Map> pMap, Frame* pFrame, const int &idxF);
-
-    void SetWorldPos(const Eigen::Vector3f &Pos);
-    Eigen::Vector3f GetWorldPos();
-
-    Eigen::Vector3f GetNormal();
-    void SetNormalVector(const Eigen::Vector3f& normal);
-
-    KeyFrame* GetReferenceKeyFrame();
-
-    std::map<KeyFrame*,std::tuple<int,int>> GetObservations();
-    int Observations();
-
-    void AddObservation(KeyFrame* pKF,int idx);
-    void EraseObservation(KeyFrame* pKF);
-
-    std::tuple<int,int> GetIndexInKeyFrame(KeyFrame* pKF);
-    bool IsInKeyFrame(KeyFrame* pKF);
-
-    void SetBadFlag();
-    bool isBad();
-
-    void Replace(MapPoint* pMP);
-    MapPoint* GetReplaced();
-
-    void IncreaseVisible(int n=1);
-    void IncreaseFound(int n=1);
-    float GetFoundRatio();
-    inline int GetFound(){
-        return mnFound;
-    }
-
-    void ComputeDistinctiveDescriptors();
-
-    cv::Mat GetDescriptor();
-
-    void UpdateNormalAndDepth();
-
-    float GetMinDistanceInvariance();
-    float GetMaxDistanceInvariance();
-    int PredictScale(const float &currentDist, KeyFrame*pKF);
-    int PredictScale(const float &currentDist, Frame* pF);
-
-    std::shared_ptr<Map> GetMap();
-    void UpdateMap(std::shared_ptr<Map> pMap);
-
-    void PrintObservations();
-
-    void PreSave(set<KeyFrame*>& spKF,set<MapPoint*>& spMP);
-    void PostLoad(map<long unsigned int, KeyFrame*>& mpKFid, map<long unsigned int, MapPoint*>& mpMPid);
-
-public:
-    long unsigned int mnId;
-    static long unsigned int nNextId;
-    long int mnFirstKFid;
-    long int mnFirstFrame;
-    int nObs;
-
-    // Variables used by the tracking
-    float mTrackProjX;
-    float mTrackProjY;
-    float mTrackDepth;
-    float mTrackDepthR;
-    float mTrackProjXR;
-    float mTrackProjYR;
-    bool mbTrackInView, mbTrackInViewR;
-    int mnTrackScaleLevel, mnTrackScaleLevelR;
-    float mTrackViewCos, mTrackViewCosR;
-    long unsigned int mnTrackReferenceForFrame;
-    long unsigned int mnLastFrameSeen;
-
-    // Variables used by local mapping
-    long unsigned int mnBALocalForKF;
-    long unsigned int mnFuseCandidateForKF;
-
-    // Variables used by loop closing
-    long unsigned int mnLoopPointForKF;
-    long unsigned int mnCorrectedByKF;
-    long unsigned int mnCorrectedReference;
-    Eigen::Vector3f mPosGBA;
-    long unsigned int mnBAGlobalForKF;
-    long unsigned int mnBALocalForMerge;
-
-    // Variable used by merging
-    Eigen::Vector3f mPosMerge;
-    Eigen::Vector3f mNormalVectorMerge;
-
-
-    // Fopr inverse depth optimization
-    double mInvDepth;
-    double mInitU;
-    double mInitV;
-    KeyFrame* mpHostKF;
-
-    static std::mutex mGlobalMutex;
-
-    unsigned int mnOriginMapId;
-
-protected:
-
-     // Position in absolute coordinates
-     Eigen::Vector3f mWorldPos;
-
-     // Keyframes observing the point and associated index in keyframe
-     std::map<KeyFrame*,std::tuple<int,int> > mObservations;
-     // For save relation without pointer, this is necessary for save/load function
-     std::map<long unsigned int, int> mBackupObservationsId1;
-     std::map<long unsigned int, int> mBackupObservationsId2;
-
-     // Mean viewing direction
-     Eigen::Vector3f mNormalVector;
-
-     // Best descriptor to fast matching
-     cv::Mat mDescriptor;
-
-     // Reference KeyFrame
-     KeyFrame* mpRefKF;
-     long unsigned int mBackupRefKFId;
-
-     // Tracking counters
-     int mnVisible;
-     int mnFound;
-
-     // Bad flag (we do not currently erase MapPoint from memory)
-     bool mbBad;
-     MapPoint* mpReplaced;
-     // For save relation without pointer, this is necessary for save/load function
-     long long int mBackupReplacedId;
-
-     // Scale invariance distances
-     float mfMinDistance;
-     float mfMaxDistance;
-
-     std::shared_ptr<Map> mpMap;
-
-     // Mutex
-     std::mutex mMutexPos;
-     std::mutex mMutexFeatures;
-     std::mutex mMutexMap;
-
-};
-
-} //namespace ORB_SLAM
diff --git a/include/ORBVocabulary.h b/include/ORBVocabulary.h
deleted file mode 100644
index c5ab1805..00000000
--- a/include/ORBVocabulary.h
+++ /dev/null
@@ -1,32 +0,0 @@
-/**
- * This file is part of ORB-SLAM3
- *
- * Copyright (C) 2017-2021 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/>.
- */
-
-#pragma once
-
-#include "DBoW2/FORB.h"
-#include "DBoW2/TemplatedVocabulary.h"
-
-namespace ORB_SLAM3 {
-
-typedef DBoW2::TemplatedVocabulary<DBoW2::FORB::TDescriptor, DBoW2::FORB>
-    ORBVocabulary;
-
-}  // namespace ORB_SLAM3
diff --git a/include/ORBextractor.h b/include/ORBextractor.h
deleted file mode 100644
index bc67cb3a..00000000
--- a/include/ORBextractor.h
+++ /dev/null
@@ -1,102 +0,0 @@
-/**
- * This file is part of ORB-SLAM3
- *
- * Copyright (C) 2017-2021 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/>.
- */
-
-#pragma once
-
-#include <list>
-#include <opencv2/opencv.hpp>
-#include <vector>
-
-namespace ORB_SLAM3 {
-
-class ExtractorNode {
- public:
-  ExtractorNode() : bNoMore(false) {}
-  ExtractorNode(const cv::Point2i &UL, const cv::Point2i &UR, const cv::Point2i &BL, const cv::Point2i &BR, int keySize) :
-    UL{UL}, UR{UR}, BL{BL}, BR{BR}, bNoMore(false) { vKeys.reserve(keySize); }
-
-  void DivideNode(ExtractorNode &n1, ExtractorNode &n2, ExtractorNode &n3,
-                  ExtractorNode &n4);
-
-  std::vector<cv::KeyPoint> vKeys;
-  cv::Point2i UL, UR, BL, BR;
-  std::list<ExtractorNode>::iterator lit;
-  bool bNoMore;
-};
-
-class ORBextractor {
- public:
-  ORBextractor(int nfeatures, float scaleFactor, int nlevels, int iniThFAST,
-               int minThFAST);
-
-  // Compute the ORB features and descriptors on an image.
-  // ORB are dispersed on the image using an octree.
-  // Mask is ignored in the current implementation.
-  int operator()(cv::InputArray _image, cv::InputArray _mask,
-                 std::vector<cv::KeyPoint> &_keypoints,
-                 cv::OutputArray _descriptors, std::vector<int> &vLappingArea);
-
-  int inline GetLevels() { return nlevels; }
-
-  float inline GetScaleFactor() { return scaleFactor; }
-
-  std::vector<float> inline GetScaleFactors() { return mvScaleFactor; }
-
-  std::vector<float> inline GetInverseScaleFactors() {
-    return mvInvScaleFactor;
-  }
-
-  std::vector<float> inline GetScaleSigmaSquares() { return mvLevelSigma2; }
-
-  std::vector<float> inline GetInverseScaleSigmaSquares() {
-    return mvInvLevelSigma2;
-  }
-
-  std::vector<cv::Mat> mvImagePyramid;
-
- protected:
-  void ComputePyramid(cv::Mat image);
-  void ComputeKeyPointsOctTree(
-      std::vector<std::vector<cv::KeyPoint> > &allKeypoints);
-  std::vector<cv::KeyPoint> DistributeOctTree(
-      const std::vector<cv::KeyPoint> &vToDistributeKeys, const int &minX,
-      const int &maxX, const int &minY, const int &maxY, const int &nFeatures,
-      const int &level);
-
-  std::vector<cv::Point> pattern;
-
-  int nfeatures;
-  double scaleFactor;
-  int nlevels;
-  int iniThFAST;
-  int minThFAST;
-
-  std::vector<int> mnFeaturesPerLevel;
-
-  std::vector<int> umax;
-
-  std::vector<float> mvScaleFactor;
-  std::vector<float> mvInvScaleFactor;
-  std::vector<float> mvLevelSigma2;
-  std::vector<float> mvInvLevelSigma2;
-};
-
-}  // namespace ORB_SLAM3
diff --git a/include/ORBmatcher.h b/include/ORBmatcher.h
deleted file mode 100644
index c44c2ac8..00000000
--- a/include/ORBmatcher.h
+++ /dev/null
@@ -1,131 +0,0 @@
-/**
- * This file is part of ORB-SLAM3
- *
- * Copyright (C) 2017-2021 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/>.
- */
-
-#pragma once
-
-#include <opencv2/core/core.hpp>
-#include <opencv2/features2d/features2d.hpp>
-#include <vector>
-
-#include "Frame.h"
-#include "KeyFrame.h"
-#include "MapPoint.h"
-#include "sophus/sim3.hpp"
-
-namespace ORB_SLAM3 {
-
-class ORBmatcher {
- public:
-  ORBmatcher(float nnratio = 0.6, bool checkOri = true);
-
-  // Computes the Hamming distance between two ORB descriptors
-  static int DescriptorDistance(const cv::Mat &a, const cv::Mat &b);
-
-  // Search matches between Frame keypoints and projected MapPoints. Returns
-  // number of matches Used to track the local map (Tracking)
-  int SearchByProjection(Frame &F, const std::vector<MapPoint *> &vpMapPoints,
-                         const float th = 3, const bool bFarPoints = false,
-                         const float thFarPoints = 50.0f);
-
-  // Project MapPoints tracked in last frame into the current frame and search
-  // matches. Used to track from previous frame (Tracking)
-  int SearchByProjection(Frame &CurrentFrame, const Frame &LastFrame,
-                         const float th, const bool bMono);
-
-  // Project MapPoints seen in KeyFrame into the Frame and search matches.
-  // Used in relocalisation (Tracking)
-  int SearchByProjection(Frame &CurrentFrame, KeyFrame *pKF,
-                         const std::set<MapPoint *> &sAlreadyFound,
-                         const float th, const int ORBdist);
-
-  // Project MapPoints using a Similarity Transformation and search matches.
-  // Used in loop detection (Loop Closing)
-  int SearchByProjection(KeyFrame *pKF, Sophus::Sim3<float> &Scw,
-                         const std::vector<MapPoint *> &vpPoints,
-                         std::vector<MapPoint *> &vpMatched, int th,
-                         float ratioHamming = 1.0);
-
-  // Project MapPoints using a Similarity Transformation and search matches.
-  // Used in Place Recognition (Loop Closing and Merging)
-  int SearchByProjection(KeyFrame *pKF, Sophus::Sim3<float> &Scw,
-                         const std::vector<MapPoint *> &vpPoints,
-                         const std::vector<KeyFrame *> &vpPointsKFs,
-                         std::vector<MapPoint *> &vpMatched,
-                         std::vector<KeyFrame *> &vpMatchedKF, int th,
-                         float ratioHamming = 1.0);
-
-  // Search matches between MapPoints in a KeyFrame and ORB in a Frame.
-  // Brute force constrained to ORB that belong to the same vocabulary node (at
-  // a certain level) Used in Relocalisation and Loop Detection
-  int SearchByBoW(KeyFrame *pKF, Frame &F,
-                  std::vector<MapPoint *> &vpMapPointMatches);
-  int SearchByBoW(KeyFrame *pKF1, KeyFrame *pKF2,
-                  std::vector<MapPoint *> &vpMatches12);
-
-  // Matching for the Map Initialization (only used in the monocular case)
-  int SearchForInitialization(Frame &F1, Frame &F2,
-                              std::vector<cv::Point2f> &vbPrevMatched,
-                              std::vector<int> &vnMatches12,
-                              int windowSize = 10);
-
-  // Matching to triangulate new MapPoints. Check Epipolar Constraint.
-  int SearchForTriangulation(KeyFrame *pKF1, KeyFrame *pKF2,
-                             std::vector<pair<size_t, size_t> > &vMatchedPairs,
-                             const bool bOnlyStereo,
-                             const bool bCoarse = false);
-
-  // Search matches between MapPoints seen in KF1 and KF2 transforming by a Sim3
-  // [s12*R12|t12] In the stereo and RGB-D case, s12=1 int
-  // SearchBySim3(KeyFrame* pKF1, KeyFrame* pKF2, std::vector<MapPoint *>
-  // &vpMatches12, const float &s12, const cv::Mat &R12, const cv::Mat &t12,
-  // const float th);
-  int SearchBySim3(KeyFrame *pKF1, KeyFrame *pKF2,
-                   std::vector<MapPoint *> &vpMatches12,
-                   const Sophus::Sim3f &S12, const float th);
-
-  // Project MapPoints into KeyFrame and search for duplicated MapPoints.
-  int Fuse(KeyFrame *pKF, const vector<MapPoint *> &vpMapPoints,
-           const float th = 3.0, const bool bRight = false);
-
-  // Project MapPoints into KeyFrame using a given Sim3 and search for
-  // duplicated MapPoints.
-  int Fuse(KeyFrame *pKF, Sophus::Sim3f &Scw,
-           const std::vector<MapPoint *> &vpPoints, float th,
-           vector<MapPoint *> &vpReplacePoint);
-
- public:
-  static const int TH_LOW;
-  static const int TH_HIGH;
-  static const int HISTO_LENGTH;
-  
-
- protected:
-  float RadiusByViewingCos(const float &viewCos);
-
-  void ComputeThreeMaxima(std::vector<int> *histo, const int L, int &ind1,
-                          int &ind2, int &ind3);
-
-  float mfNNratio;
-  bool mbCheckOrientation;
-};
-
-}  // namespace ORB_SLAM3
-
diff --git a/include/OptimizableTypes.h b/include/OptimizableTypes.h
deleted file mode 100644
index a0edcccd..00000000
--- a/include/OptimizableTypes.h
+++ /dev/null
@@ -1,233 +0,0 @@
-/**
- * This file is part of ORB-SLAM3
- *
- * Copyright (C) 2017-2021 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/>.
- */
-
-#pragma once
-
-#include <GeometricCamera.h>
-#include <g2o/types/sim3.h>
-#include <g2o/types/types_six_dof_expmap.h>
-
-#include <Eigen/Geometry>
-
-#include "g2o/core/base_unary_edge.h"
-
-namespace ORB_SLAM3 {
-class EdgeSE3ProjectXYZOnlyPose
-    : public g2o::BaseUnaryEdge<2, Eigen::Vector2d, g2o::VertexSE3Expmap> {
- public:
-  
-
-  EdgeSE3ProjectXYZOnlyPose() {}
-
-  bool read(std::istream& is);
-
-  bool write(std::ostream& os) const;
-
-  void computeError() {
-    const g2o::VertexSE3Expmap* v1 =
-        static_cast<const g2o::VertexSE3Expmap*>(_vertices[0]);
-    Eigen::Vector2d obs(_measurement);
-    _error = obs - pCamera->project(v1->estimate().map(Xw));
-  }
-
-  bool isDepthPositive() {
-    const g2o::VertexSE3Expmap* v1 =
-        static_cast<const g2o::VertexSE3Expmap*>(_vertices[0]);
-    return (v1->estimate().map(Xw))(2) > 0.0;
-  }
-
-  virtual void linearizeOplus();
-
-  Eigen::Vector3d Xw;
-  std::shared_ptr<GeometricCamera> pCamera;
-};
-
-class EdgeSE3ProjectXYZOnlyPoseToBody
-    : public g2o::BaseUnaryEdge<2, Eigen::Vector2d, g2o::VertexSE3Expmap> {
- public:
-  
-
-  EdgeSE3ProjectXYZOnlyPoseToBody() {}
-
-  bool read(std::istream& is);
-
-  bool write(std::ostream& os) const;
-
-  void computeError() {
-    const g2o::VertexSE3Expmap* v1 =
-        static_cast<const g2o::VertexSE3Expmap*>(_vertices[0]);
-    Eigen::Vector2d obs(_measurement);
-    _error = obs - pCamera->project((mTrl * v1->estimate()).map(Xw));
-  }
-
-  bool isDepthPositive() {
-    const g2o::VertexSE3Expmap* v1 =
-        static_cast<const g2o::VertexSE3Expmap*>(_vertices[0]);
-    return ((mTrl * v1->estimate()).map(Xw))(2) > 0.0;
-  }
-
-  virtual void linearizeOplus();
-
-  Eigen::Vector3d Xw;
-  std::shared_ptr<GeometricCamera> pCamera;
-
-  g2o::SE3Quat mTrl;
-};
-
-class EdgeSE3ProjectXYZ
-    : public g2o::BaseBinaryEdge<2, Eigen::Vector2d, g2o::VertexSBAPointXYZ,
-                                 g2o::VertexSE3Expmap> {
- public:
-  
-
-  EdgeSE3ProjectXYZ();
-
-  bool read(std::istream& is);
-
-  bool write(std::ostream& os) const;
-
-  void computeError() {
-    const g2o::VertexSE3Expmap* v1 =
-        static_cast<const g2o::VertexSE3Expmap*>(_vertices[1]);
-    const g2o::VertexSBAPointXYZ* v2 =
-        static_cast<const g2o::VertexSBAPointXYZ*>(_vertices[0]);
-    Eigen::Vector2d obs(_measurement);
-    _error = obs - pCamera->project(v1->estimate().map(v2->estimate()));
-  }
-
-  bool isDepthPositive() {
-    const g2o::VertexSE3Expmap* v1 =
-        static_cast<const g2o::VertexSE3Expmap*>(_vertices[1]);
-    const g2o::VertexSBAPointXYZ* v2 =
-        static_cast<const g2o::VertexSBAPointXYZ*>(_vertices[0]);
-    return ((v1->estimate().map(v2->estimate()))(2) > 0.0);
-  }
-
-  virtual void linearizeOplus();
-
-  std::shared_ptr<GeometricCamera> pCamera;
-};
-
-class EdgeSE3ProjectXYZToBody
-    : public g2o::BaseBinaryEdge<2, Eigen::Vector2d, g2o::VertexSBAPointXYZ,
-                                 g2o::VertexSE3Expmap> {
- public:
-  
-
-  EdgeSE3ProjectXYZToBody();
-
-  bool read(std::istream& is);
-
-  bool write(std::ostream& os) const;
-
-  void computeError() {
-    const g2o::VertexSE3Expmap* v1 =
-        static_cast<const g2o::VertexSE3Expmap*>(_vertices[1]);
-    const g2o::VertexSBAPointXYZ* v2 =
-        static_cast<const g2o::VertexSBAPointXYZ*>(_vertices[0]);
-    Eigen::Vector2d obs(_measurement);
-    _error =
-        obs - pCamera->project((mTrl * v1->estimate()).map(v2->estimate()));
-  }
-
-  bool isDepthPositive() {
-    const g2o::VertexSE3Expmap* v1 =
-        static_cast<const g2o::VertexSE3Expmap*>(_vertices[1]);
-    const g2o::VertexSBAPointXYZ* v2 =
-        static_cast<const g2o::VertexSBAPointXYZ*>(_vertices[0]);
-    return ((mTrl * v1->estimate()).map(v2->estimate()))(2) > 0.0;
-  }
-
-  virtual void linearizeOplus();
-
-  std::shared_ptr<GeometricCamera> pCamera;
-  g2o::SE3Quat mTrl;
-};
-
-class VertexSim3Expmap : public g2o::BaseVertex<7, g2o::Sim3> {
- public:
-  
-  VertexSim3Expmap();
-  virtual bool read(std::istream& is);
-  virtual bool write(std::ostream& os) const;
-
-  virtual void setToOriginImpl() { _estimate = g2o::Sim3(); }
-
-  virtual void oplusImpl(const double* update_) {
-    Eigen::Map<g2o::Vector7d> update(const_cast<double*>(update_));
-
-    if (_fix_scale) update[6] = 0;
-
-    g2o::Sim3 s(update);
-    setEstimate(s * estimate());
-  }
-
-  std::shared_ptr<GeometricCamera> pCamera1, pCamera2;
-
-  bool _fix_scale;
-};
-
-class EdgeSim3ProjectXYZ
-    : public g2o::BaseBinaryEdge<2, Eigen::Vector2d, g2o::VertexSBAPointXYZ,
-                                 ORB_SLAM3::VertexSim3Expmap> {
- public:
-  
-  EdgeSim3ProjectXYZ();
-  virtual bool read(std::istream& is);
-  virtual bool write(std::ostream& os) const;
-
-  void computeError() {
-    const ORB_SLAM3::VertexSim3Expmap* v1 =
-        static_cast<const ORB_SLAM3::VertexSim3Expmap*>(_vertices[1]);
-    const g2o::VertexSBAPointXYZ* v2 =
-        static_cast<const g2o::VertexSBAPointXYZ*>(_vertices[0]);
-
-    Eigen::Vector2d obs(_measurement);
-    _error = obs - v1->pCamera1->project(v1->estimate().map(v2->estimate()));
-  }
-
-  // virtual void linearizeOplus();
-};
-
-class EdgeInverseSim3ProjectXYZ
-    : public g2o::BaseBinaryEdge<2, Eigen::Vector2d, g2o::VertexSBAPointXYZ,
-                                 VertexSim3Expmap> {
- public:
-  
-  EdgeInverseSim3ProjectXYZ();
-  virtual bool read(std::istream& is);
-  virtual bool write(std::ostream& os) const;
-
-  void computeError() {
-    const ORB_SLAM3::VertexSim3Expmap* v1 =
-        static_cast<const ORB_SLAM3::VertexSim3Expmap*>(_vertices[1]);
-    const g2o::VertexSBAPointXYZ* v2 =
-        static_cast<const g2o::VertexSBAPointXYZ*>(_vertices[0]);
-
-    Eigen::Vector2d obs(_measurement);
-    _error = obs - v1->pCamera2->project(
-                       (v1->estimate().inverse().map(v2->estimate())));
-  }
-
-  // virtual void linearizeOplus();
-};
-
-}  // namespace ORB_SLAM3
diff --git a/include/Optimizer.h b/include/Optimizer.h
deleted file mode 100644
index 48bcb16e..00000000
--- a/include/Optimizer.h
+++ /dev/null
@@ -1,141 +0,0 @@
-/**
- * This file is part of ORB-SLAM3
- *
- * Copyright (C) 2017-2021 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/>.
- */
-
-#pragma once
-
-#include <math.h>
-
-#include "Frame.h"
-#include "KeyFrame.h"
-#include "LoopClosing.h"
-#include "Map.h"
-#include "MapPoint.h"
-#include "g2o/core/block_solver.h"
-#include "g2o/core/optimization_algorithm_gauss_newton.h"
-#include "g2o/core/optimization_algorithm_levenberg.h"
-#include "g2o/core/robust_kernel_impl.h"
-#include "g2o/core/sparse_block_matrix.h"
-#include "g2o/solvers/linear_solver_dense.h"
-#include "g2o/solvers/linear_solver_eigen.h"
-#include "g2o/types/types_seven_dof_expmap.h"
-#include "g2o/types/types_six_dof_expmap.h"
-
-namespace ORB_SLAM3 {
-
-class LoopClosing;
-
-class Optimizer {
- public:
-  void static BundleAdjustment(const std::vector<KeyFrame *> &vpKF,
-                               const std::vector<MapPoint *> &vpMP,
-                               int nIterations = 5, bool *pbStopFlag = nullptr,
-                               const unsigned long nLoopKF = 0,
-                               const bool bRobust = true);
-  void static GlobalBundleAdjustemnt(std::shared_ptr<Map> pMap, int nIterations = 5,
-                                     bool *pbStopFlag = nullptr,
-                                     const unsigned long nLoopKF = 0,
-                                     const bool bRobust = true);
-  void static FullInertialBA(std::shared_ptr<Map> pMap, int its, const bool bFixLocal = false,
-                             const unsigned long nLoopKF = 0,
-                             bool *pbStopFlag = nullptr, bool bInit = false,
-                             float priorG = 1e2, float priorA = 1e6,
-                             Eigen::VectorXd *vSingVal = nullptr,
-                             bool *bHess = nullptr);
-
-  void static LocalBundleAdjustment(KeyFrame *pKF, bool *pbStopFlag, std::shared_ptr<Map> pMap,
-                                    int &num_fixedKF, int &num_OptKF,
-                                    int &num_MPs, int &num_edges);
-
-  int static PoseOptimization(Frame *pFrame);
-  int static PoseInertialOptimizationLastKeyFrame(Frame *pFrame,
-                                                  bool bRecInit = false);
-  int static PoseInertialOptimizationLastFrame(Frame *pFrame,
-                                               bool bRecInit = false);
-
-  // if bFixScale is true, 6DoF optimization (stereo,rgbd), 7DoF otherwise
-  // (mono)
-  void static OptimizeEssentialGraph(
-      std::shared_ptr<Map> pMap, KeyFrame *pLoopKF, KeyFrame *pCurKF,
-      const LoopClosing::KeyFrameAndPose &NonCorrectedSim3,
-      const LoopClosing::KeyFrameAndPose &CorrectedSim3,
-      const map<KeyFrame *, set<KeyFrame *> > &LoopConnections,
-      const bool &bFixScale);
-  void static OptimizeEssentialGraph(KeyFrame *pCurKF,
-                                     vector<KeyFrame *> &vpFixedKFs,
-                                     vector<KeyFrame *> &vpFixedCorrectedKFs,
-                                     vector<KeyFrame *> &vpNonFixedKFs,
-                                     vector<MapPoint *> &vpNonCorrectedMPs);
-
-  // For inertial loopclosing
-  void static OptimizeEssentialGraph4DoF(
-      std::shared_ptr<Map> pMap, KeyFrame *pLoopKF, KeyFrame *pCurKF,
-      const LoopClosing::KeyFrameAndPose &NonCorrectedSim3,
-      const LoopClosing::KeyFrameAndPose &CorrectedSim3,
-      const map<KeyFrame *, set<KeyFrame *> > &LoopConnections);
-
-  // if bFixScale is true, optimize SE3 (stereo,rgbd), Sim3 otherwise (mono)
-  // (NEW)
-  static int OptimizeSim3(KeyFrame *pKF1, KeyFrame *pKF2,
-                          std::vector<MapPoint *> &vpMatches1,
-                          g2o::Sim3 &g2oS12, const float th2,
-                          const bool bFixScale,
-                          Eigen::Matrix<double, 7, 7> &mAcumHessian,
-                          const bool bAllPoints = false);
-
-  // For inertial systems
-
-  void static LocalInertialBA(KeyFrame *pKF, bool *pbStopFlag, std::shared_ptr<Map> pMap,
-                              int &num_fixedKF, int &num_OptKF, int &num_MPs,
-                              int &num_edges, bool bLarge = false,
-                              bool bRecInit = false);
-  void static MergeInertialBA(KeyFrame *pCurrKF, KeyFrame *pMergeKF,
-                              bool *pbStopFlag, std::shared_ptr<Map> pMap,
-                              LoopClosing::KeyFrameAndPose &corrPoses);
-
-  // Local BA in welding area when two maps are merged
-  void static LocalBundleAdjustment(KeyFrame *pMainKF,
-                                    vector<KeyFrame *> vpAdjustKF,
-                                    vector<KeyFrame *> vpFixedKF,
-                                    bool *pbStopFlag);
-
-  // Marginalize block element (start:end,start:end). Perform Schur complement.
-  // Marginalized elements are filled with zeros.
-  static Eigen::MatrixXd Marginalize(const Eigen::MatrixXd &H, const int &start,
-                                     const int &end);
-
-  // Inertial pose-graph
-  void static InertialOptimization(std::shared_ptr<Map> pMap, Eigen::Matrix3d &Rwg,
-                                   double &scale, Eigen::Vector3d &bg,
-                                   Eigen::Vector3d &ba, bool bMono,
-                                   Eigen::MatrixXd &covInertial,
-                                   bool bFixedVel = false, bool bGauss = false,
-                                   float priorG = 1e2, float priorA = 1e6);
-  void static InertialOptimization(std::shared_ptr<Map> pMap, Eigen::Vector3d &bg,
-                                   Eigen::Vector3d &ba, float priorG = 1e2,
-                                   float priorA = 1e6);
-  void static InertialOptimization(std::shared_ptr<Map> pMap, Eigen::Matrix3d &Rwg,
-                                   double &scale);
-
-  ;
-};
-
-}  // namespace ORB_SLAM3
-
diff --git a/include/SerializationUtils.h b/include/SerializationUtils.h
deleted file mode 100644
index 11185c13..00000000
--- a/include/SerializationUtils.h
+++ /dev/null
@@ -1,154 +0,0 @@
-/**
- * This file is part of ORB-SLAM3
- *
- * Copyright (C) 2017-2021 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/>.
- */
-
-#pragma once
-
-#include <Eigen/Core>
-#include <boost/serialization/serialization.hpp>
-#include <boost/serialization/vector.hpp>
-#include <opencv2/core/core.hpp>
-#include <opencv2/features2d/features2d.hpp>
-#include <sophus/se3.hpp>
-#include <vector>
-
-namespace ORB_SLAM3 {
-
-template <class Archive>
-void serializeSophusSE3(Archive& ar, Sophus::SE3f& T,
-                        const unsigned int version) {
-  Eigen::Vector4f quat;
-  Eigen::Vector3f transl;
-
-  if (Archive::is_saving::value) {
-    Eigen::Quaternionf q = T.unit_quaternion();
-    quat << q.w(), q.x(), q.y(), q.z();
-    transl = T.translation();
-  }
-
-  ar& boost::serialization::make_array(quat.data(), quat.size());
-  ar& boost::serialization::make_array(transl.data(), transl.size());
-
-  if (Archive::is_loading::value) {
-    Eigen::Quaternionf q(quat[0], quat[1], quat[2], quat[3]);
-    T = Sophus::SE3f(q, transl);
-  }
-}
-
-/*template <class Archive, size_t dim>
-void serializeDiagonalMatrix(Archive &ar, Eigen::DiagonalMatrix<float, dim> &D,
-const unsigned int version)
-{
-    Eigen::Matrix<float,dim,dim> dense;
-    if(Archive::is_saving::value)
-    {
-        dense = D.toDenseMatrix();
-    }
-
-    ar & boost::serialization::make_array(dense.data(), dense.size());
-
-    if (Archive::is_loading::value)
-    {
-        D = dense.diagonal().asDiagonal();
-    }
-}*/
-
-template <class Archive>
-void serializeMatrix(Archive& ar, cv::Mat& mat, const unsigned int version) {
-  int cols, rows, type;
-  bool continuous;
-
-  if (Archive::is_saving::value) {
-    cols = mat.cols;
-    rows = mat.rows;
-    type = mat.type();
-    continuous = mat.isContinuous();
-  }
-
-  ar& cols& rows& type& continuous;
-
-  if (Archive::is_loading::value) mat.create(rows, cols, type);
-
-  if (continuous) {
-    const unsigned int data_size = rows * cols * mat.elemSize();
-    ar& boost::serialization::make_array(mat.ptr(), data_size);
-  } else {
-    const unsigned int row_size = cols * mat.elemSize();
-    for (int i = 0; i < rows; i++) {
-      ar& boost::serialization::make_array(mat.ptr(i), row_size);
-    }
-  }
-}
-
-template <class Archive>
-void serializeMatrix(Archive& ar, const cv::Mat& mat,
-                     const unsigned int version) {
-  cv::Mat matAux = mat;
-
-  serializeMatrix(ar, matAux, version);
-
-  if (Archive::is_loading::value) {
-    cv::Mat* ptr;
-    ptr = (cv::Mat*)(&mat);
-    *ptr = matAux;
-  }
-}
-
-template <class Archive>
-void serializeVectorKeyPoints(Archive& ar, const std::vector<cv::KeyPoint>& vKP,
-                              const unsigned int version) {
-  int NumEl;
-
-  if (Archive::is_saving::value) {
-    NumEl = vKP.size();
-  }
-
-  ar& NumEl;
-
-  std::vector<cv::KeyPoint> vKPaux = vKP;
-  if (Archive::is_loading::value) vKPaux.reserve(NumEl);
-
-  for (int i = 0; i < NumEl; ++i) {
-    cv::KeyPoint KPi;
-
-    if (Archive::is_loading::value) KPi = cv::KeyPoint();
-
-    if (Archive::is_saving::value) KPi = vKPaux[i];
-
-    ar& KPi.angle;
-    ar& KPi.response;
-    ar& KPi.size;
-    ar& KPi.pt.x;
-    ar& KPi.pt.y;
-    ar& KPi.class_id;
-    ar& KPi.octave;
-
-    if (Archive::is_loading::value) vKPaux.push_back(KPi);
-  }
-
-  if (Archive::is_loading::value) {
-    std::vector<cv::KeyPoint>* ptr;
-    ptr = (std::vector<cv::KeyPoint>*)(&vKP);
-    *ptr = vKPaux;
-  }
-}
-
-}  // namespace ORB_SLAM3
-
diff --git a/include/Settings.h b/include/Settings.h
deleted file mode 100644
index 3a369530..00000000
--- a/include/Settings.h
+++ /dev/null
@@ -1,239 +0,0 @@
-/**
- * This file is part of ORB-SLAM3
- *
- * Copyright (C) 2017-2021 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/>.
- */
-
-#pragma once
-
-// Flag to activate the measurement of time in each process (track,localmap,
-// place recognition).
-//#define REGISTER_TIMES
-
-#include <stdio.h>
-#include <stdlib.h>
-#include <unistd.h>
-
-#include <string>
-#include <memory>
-
-#include "CameraModels/GeometricCamera.h"
-
-namespace ORB_SLAM3 {
-
-class System;
-
-// TODO: change to double instead of float
-
-class Settings {
- public:
-  /*
-   * Enum for the different camera types implemented
-   */
-  enum CameraType { PinHole = 0, Rectified = 1, KannalaBrandt = 2 };
-
-  /*
-   * Delete default constructor
-   */
-  Settings() = delete;
-
-  /*
-   * Constructor from file
-   */
-  Settings(const std::string& configFile, const int& sensor);
-
-  /*
-   * Ostream operator overloading to dump settings to the terminal
-   */
-  friend std::ostream& operator<<(std::ostream& output, const Settings& s);
-
-  /*
-   * Getter methods
-   */
-  CameraType cameraType() const { return cameraType_; }
-  std::shared_ptr<GeometricCamera> camera1() { return calibration1_; }
-  std::shared_ptr<GeometricCamera> camera2() { return calibration2_; }
-  cv::Mat camera1DistortionCoef() {
-    return cv::Mat(vPinHoleDistorsion1_.size(), 1, CV_32F,
-                   vPinHoleDistorsion1_.data());
-  }
-  cv::Mat camera2DistortionCoef() {
-    return cv::Mat(vPinHoleDistorsion2_.size(), 1, CV_32F,
-                   vPinHoleDistorsion2_.data());
-  }
-
-  const Sophus::SE3f &Tlr() const { return Tlr_; }
-  float bf() const { return bf_; }
-  float b() const { return b_; }
-  float thDepth() const { return thDepth_; }
-
-  bool needToUndistort() const { return bNeedToUndistort_; }
-
-  const cv::Size &newImSize() const { return newImSize_; }
-  float fps() const { return fps_; }
-  bool rgb() const { return bRGB_; }
-  bool needToResize() const { return bNeedToResize1_; }
-  bool needToRectify() const { return bNeedToRectify_; }
-
-  float noiseGyro() const { return noiseGyro_; }
-  float noiseAcc() const { return noiseAcc_; }
-  float gyroWalk() const { return gyroWalk_; }
-  float accWalk() const { return accWalk_; }
-  float imuFrequency() const { return imuFrequency_; }
-  const Sophus::SE3f &Tbc() const { return Tbc_; }
-  bool insertKFsWhenLost() const { return insertKFsWhenLost_; }
-
-  float depthMapFactor() const { return depthMapFactor_; }
-
-  int nFeatures() const { return nFeatures_; }
-  int nLevels() const { return nLevels_; }
-  float initThFAST() const { return initThFAST_; }
-  float minThFAST() const { return minThFAST_; }
-  float scaleFactor() const { return scaleFactor_; }
-
-  float keyFrameSize() const { return keyFrameSize_; }
-  float keyFrameLineWidth() const { return keyFrameLineWidth_; }
-  float graphLineWidth() const { return graphLineWidth_; }
-  float pointSize() const { return pointSize_; }
-  float cameraSize() const { return cameraSize_; }
-  float cameraLineWidth() const { return cameraLineWidth_; }
-  float viewPointX() const { return viewPointX_; }
-  float viewPointY() const { return viewPointY_; }
-  float viewPointZ() const { return viewPointZ_; }
-  float viewPointF() const { return viewPointF_; }
-  float imageViewerScale() const { return imageViewerScale_; }
-
-  const std::string &atlasLoadFile() const { return sLoadFrom_; }
-  const std::string &atlasSaveFile() const { return sSaveto_; }
-
-  float thFarPoints() const { return thFarPoints_; }
-
-  const cv::Mat &M1l() const { return M1l_; }
-  const cv::Mat &M2l() const { return M2l_; }
-  const cv::Mat &M1r() const { return M1r_; }
-  const cv::Mat &M2r() const { return M2r_; }
-
- private:
-  template <typename T>
-  T readParameter(cv::FileStorage& fSettings, const std::string& name,
-                  bool& found, const bool required = true) {
-    cv::FileNode node = fSettings[name];
-    if (node.empty()) {
-      if (required) {
-        std::cerr << name << " required parameter does not exist, aborting..."
-                  << std::endl;
-        exit(-1);
-      } else {
-        std::cerr << name << " optional parameter does not exist..."
-                  << std::endl;
-        found = false;
-        return T();
-      }
-
-    } else {
-      found = true;
-      return (T)node;
-    }
-  }
-
-  void readCamera1(cv::FileStorage& fSettings);
-  void readCamera2(cv::FileStorage& fSettings);
-  void readImageInfo(cv::FileStorage& fSettings);
-  void readIMU(cv::FileStorage& fSettings);
-  void readRGBD(cv::FileStorage& fSettings);
-  void readORB(cv::FileStorage& fSettings);
-  void readViewer(cv::FileStorage& fSettings);
-  void readLoadAndSave(cv::FileStorage& fSettings);
-  void readOtherParameters(cv::FileStorage& fSettings);
-
-  void precomputeRectificationMaps();
-
-  int sensor_;
-  CameraType cameraType_;  // Camera type
-
-  /*
-   * Visual stuff
-   */
-  std::shared_ptr<GeometricCamera> calibration1_, calibration2_;  // Camera calibration
-  GeometricCamera *originalCalib1_, *originalCalib2_;
-  std::vector<float> vPinHoleDistorsion1_, vPinHoleDistorsion2_;
-
-  cv::Size originalImSize_, newImSize_;
-  float fps_;
-  bool bRGB_;
-
-  bool bNeedToUndistort_;
-  bool bNeedToRectify_;
-  bool bNeedToResize1_, bNeedToResize2_;
-
-  Sophus::SE3f Tlr_;
-  float thDepth_;
-  float bf_, b_;
-
-  /*
-   * Rectification stuff
-   */
-  cv::Mat M1l_, M2l_;
-  cv::Mat M1r_, M2r_;
-
-  /*
-   * Inertial stuff
-   */
-  float noiseGyro_, noiseAcc_;
-  float gyroWalk_, accWalk_;
-  float imuFrequency_;
-  Sophus::SE3f Tbc_;
-  bool insertKFsWhenLost_;
-
-  /*
-   * RGBD stuff
-   */
-  float depthMapFactor_;
-
-  /*
-   * ORB stuff
-   */
-  int nFeatures_;
-  float scaleFactor_;
-  int nLevels_;
-  int initThFAST_, minThFAST_;
-
-  /*
-   * Viewer stuff
-   */
-  float keyFrameSize_;
-  float keyFrameLineWidth_;
-  float graphLineWidth_;
-  float pointSize_;
-  float cameraSize_;
-  float cameraLineWidth_;
-  float viewPointX_, viewPointY_, viewPointZ_, viewPointF_;
-  float imageViewerScale_;
-
-  /*
-   * Save & load maps
-   */
-  std::string sLoadFrom_, sSaveto_;
-
-  /*
-   * Other stuff
-   */
-  float thFarPoints_;
-};
-}  // namespace ORB_SLAM3
-
diff --git a/include/Sim3Solver.h b/include/Sim3Solver.h
deleted file mode 100644
index adf512bd..00000000
--- a/include/Sim3Solver.h
+++ /dev/null
@@ -1,137 +0,0 @@
-/**
- * This file is part of ORB-SLAM3
- *
- * Copyright (C) 2017-2021 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/>.
- */
-
-#pragma once
-
-#include <opencv2/opencv.hpp>
-#include <vector>
-
-#include "KeyFrame.h"
-
-namespace ORB_SLAM3 {
-
-class Sim3Solver {
- public:
-  
-  Sim3Solver(
-      KeyFrame *pKF1, KeyFrame *pKF2,
-      const std::vector<MapPoint *> &vpMatched12, const bool bFixScale = true,
-      const vector<KeyFrame *> vpKeyFrameMatchedMP = vector<KeyFrame *>());
-
-  void SetRansacParameters(double probability = 0.99, int minInliers = 6,
-                           int maxIterations = 300);
-
-  Eigen::Matrix4f find(std::vector<bool> &vbInliers12, int &nInliers);
-
-  Eigen::Matrix4f iterate(int nIterations, bool &bNoMore,
-                          std::vector<bool> &vbInliers, int &nInliers);
-  Eigen::Matrix4f iterate(int nIterations, bool &bNoMore,
-                          vector<bool> &vbInliers, int &nInliers,
-                          bool &bConverge);
-
-  Eigen::Matrix4f GetEstimatedTransformation();
-  Eigen::Matrix3f GetEstimatedRotation();
-  Eigen::Vector3f GetEstimatedTranslation();
-  float GetEstimatedScale();
-
- protected:
-  void ComputeCentroid(Eigen::Matrix3f &P, Eigen::Matrix3f &Pr,
-                       Eigen::Vector3f &C);
-
-  void ComputeSim3(Eigen::Matrix3f &P1, Eigen::Matrix3f &P2);
-
-  void CheckInliers();
-
-  void Project(const std::vector<Eigen::Vector3f> &vP3Dw,
-               std::vector<Eigen::Vector2f> &vP2D, Eigen::Matrix4f Tcw,
-               const std::shared_ptr<GeometricCamera> &pCamera);
-  void FromCameraToImage(const std::vector<Eigen::Vector3f> &vP3Dc,
-                         std::vector<Eigen::Vector2f> &vP2D,
-                         const std::shared_ptr<GeometricCamera> &pCamera);
-
- protected:
-  // KeyFrames and matches
-  KeyFrame *mpKF1;
-  KeyFrame *mpKF2;
-
-  std::vector<Eigen::Vector3f> mvX3Dc1;
-  std::vector<Eigen::Vector3f> mvX3Dc2;
-  std::vector<MapPoint *> mvpMapPoints1;
-  std::vector<MapPoint *> mvpMapPoints2;
-  std::vector<MapPoint *> mvpMatches12;
-  std::vector<size_t> mvnIndices1;
-  std::vector<size_t> mvSigmaSquare1;
-  std::vector<size_t> mvSigmaSquare2;
-  std::vector<size_t> mvnMaxError1;
-  std::vector<size_t> mvnMaxError2;
-
-  int N;
-  int mN1;
-
-  // Current Estimation
-  Eigen::Matrix3f mR12i;
-  Eigen::Vector3f mt12i;
-  float ms12i;
-  Eigen::Matrix4f mT12i;
-  Eigen::Matrix4f mT21i;
-  std::vector<bool> mvbInliersi;
-  int mnInliersi;
-
-  // Current Ransac State
-  int mnIterations;
-  std::vector<bool> mvbBestInliers;
-  int mnBestInliers;
-  Eigen::Matrix4f mBestT12;
-  Eigen::Matrix3f mBestRotation;
-  Eigen::Vector3f mBestTranslation;
-  float mBestScale;
-
-  // Scale is fixed to 1 in the stereo/RGBD case
-  bool mbFixScale;
-
-  // Indices for random selection
-  std::vector<size_t> mvAllIndices;
-
-  // Projections
-  std::vector<Eigen::Vector2f> mvP1im1;
-  std::vector<Eigen::Vector2f> mvP2im2;
-
-  // RANSAC probability
-  double mRansacProb;
-
-  // RANSAC min inliers
-  int mRansacMinInliers;
-
-  // RANSAC max iterations
-  int mRansacMaxIts;
-
-  // Threshold inlier/outlier. e = dist(Pi,T_ij*Pj)^2 < 5.991*mSigma2
-  float mTh;
-  float mSigma2;
-
-  // Calibration
-  // cv::Mat mK1;
-  // cv::Mat mK2;
-
-  std::shared_ptr<GeometricCamera> pCamera1, pCamera2;
-};
-
-}  // namespace ORB_SLAM3
diff --git a/include/System.h b/include/System.h
deleted file mode 100644
index 6ea7b460..00000000
--- a/include/System.h
+++ /dev/null
@@ -1,245 +0,0 @@
-/**
-* This file is part of ORB-SLAM3
-*
-* Copyright (C) 2017-2021 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/>.
-*/
-
-
-#pragma once
-
-
-#include <unistd.h>
-#include <stdio.h>
-#include <stdlib.h>
-#include <string>
-#include <thread>
-#include <opencv2/core/core.hpp>
-
-#include "ImprovedTypes.hpp"
-#include "Tracking.h"
-#include "Atlas.h"
-#include "LocalMapping.h"
-#include "LoopClosing.h"
-#include "KeyFrameDatabase.h"
-#include "ORBVocabulary.h"
-#include "ImuTypes.h"
-#include "Settings.h"
-#include "Camera.hpp"
-
-
-namespace ORB_SLAM3
-{
-
-class Verbose
-{
-public:
-    enum eLevel
-    {
-        VERBOSITY_QUIET=0,
-        VERBOSITY_NORMAL=1,
-        VERBOSITY_VERBOSE=2,
-        VERBOSITY_VERY_VERBOSE=3,
-        VERBOSITY_DEBUG=4
-    };
-
-    static eLevel th;
-
-public:
-    static void PrintMess(std::string str, eLevel lev)
-    {
-        if(lev <= th)
-            cout << str << endl;
-    }
-
-    static void SetTh(eLevel _th)
-    {
-        th = _th;
-    }
-};
-
-class Viewer;
-class Atlas;
-class Tracking;
-class LocalMapping;
-class LoopClosing;
-class Settings;
-
-class System
-{
-public:
-
-    // File type
-    enum FileType{
-        TEXT_FILE=0,
-        BINARY_FILE=1,
-    };
-
-public:
-    
-    // Initialize the SLAM system. It launches the Local Mapping, Loop Closing and Viewer threads.
-    System(const string &strVocFile, const string &strSettingsFile, const CameraType::eSensor sensor, const string &strSequence = std::string());
-
-    // Proccess the given stereo frame. Images must be synchronized and rectified.
-    // Input images: RGB (CV_8UC3) or grayscale (CV_8U). RGB is converted to grayscale.
-    // Returns the camera pose (empty if tracking fails).
-    Sophus::SE3f TrackStereo(const cv::Mat &imLeft, const cv::Mat &imRight, const double &timestamp, const vector<IMU::Point>& vImuMeas = vector<IMU::Point>(), string filename="");
-
-    // Process the given rgbd frame. Depthmap must be registered to the RGB frame.
-    // Input image: RGB (CV_8UC3) or grayscale (CV_8U). RGB is converted to grayscale.
-    // Input depthmap: Float (CV_32F).
-    // Returns the camera pose (empty if tracking fails).
-    Sophus::SE3f TrackRGBD(const cv::Mat &im, const cv::Mat &depthmap, const double &timestamp, const vector<IMU::Point>& vImuMeas = vector<IMU::Point>(), string filename="");
-
-    // Proccess the given monocular frame and optionally imu data
-    // Input images: RGB (CV_8UC3) or grayscale (CV_8U). RGB is converted to grayscale.
-    // Returns the camera pose (empty if tracking fails).
-    Sophus::SE3f TrackMonocular(const cv::Mat &im, const double &timestamp, const vector<IMU::Point>& vImuMeas = vector<IMU::Point>(), string filename="");
-
-
-    // This stops local mapping thread (map building) and performs only camera tracking.
-    void ActivateLocalizationMode();
-    // This resumes local mapping thread and performs SLAM again.
-    void DeactivateLocalizationMode();
-
-    // Returns true if there have been a big map change (loop closure, global BA)
-    // since last call to this function
-    bool MapChanged();
-
-    // Reset the system (clear Atlas or the active map)
-    void Reset();
-    void ResetActiveMap();
-
-    // All threads will be requested to finish.
-    // It waits until all threads have finished.
-    // This function must be called before saving the trajectory.
-    virtual ~System();
-
-    // Save camera trajectory in the TUM RGB-D dataset format.
-    // Only for stereo and RGB-D. This method does not work for monocular.
-    // Call first Shutdown()
-    // See format details at: http://vision.in.tum.de/data/datasets/rgbd-dataset
-    void SaveTrajectoryTUM(const string &filename);
-
-    // Save keyframe poses in the TUM RGB-D dataset format.
-    // This method works for all sensor input.
-    // Call first Shutdown()
-    // See format details at: http://vision.in.tum.de/data/datasets/rgbd-dataset
-    void SaveKeyFrameTrajectoryTUM(const string &filename);
-
-    void SaveTrajectoryEuRoC(const string &filename);
-    void SaveKeyFrameTrajectoryEuRoC(const string &filename);
-
-    void SaveTrajectoryEuRoC(const string &filename, std::shared_ptr<Map> pMap);
-    void SaveKeyFrameTrajectoryEuRoC(const string &filename, std::shared_ptr<Map> pMap);
-
-    // Save data used for initialization debug
-    void SaveDebugData(const int &iniIdx);
-
-    // Save camera trajectory in the KITTI dataset format.
-    // Only for stereo and RGB-D. This method does not work for monocular.
-    // Call first Shutdown()
-    // See format details at: http://www.cvlibs.net/datasets/kitti/eval_odometry.php
-    void SaveTrajectoryKITTI(const string &filename);
-
-    // TODO: Save/Load functions
-    // SaveMap(const string &filename);
-    // LoadMap(const string &filename);
-
-    // Information from most recent processed frame
-    // You can call this right after TrackMonocular (or stereo or RGBD)
-    int GetTrackingState();
-    std::vector<MapPoint*> GetTrackedMapPoints();
-    std::vector<cv::KeyPoint> GetTrackedKeyPointsUn();
-
-    // For debugging
-    double GetTimeFromIMUInit();
-    bool isLost();
-    bool isFinished();
-
-    void ChangeDataset();
-
-    float GetImageScale();
-
-#ifdef REGISTER_TIMES
-    void InsertRectTime(double& time);
-    void InsertResizeTime(double& time);
-    void InsertTrackTime(double& time);
-#endif
-
-    friend Viewer;
-private:
-
-    void SaveAtlas(int type);
-    bool LoadAtlas(int type);
-
-    string CalculateCheckSum(string filename, int type);
-
-    // Input sensor
-    CameraType::eSensor mSensor;
-    vector<Camera_ptr> cameras;
-
-    // ORB vocabulary used for place recognition and feature matching.
-    ORBVocabulary* mpVocabulary;
-
-    // KeyFrame database for place recognition (relocalization and loop detection).
-    KeyFrameDatabase* mpKeyFrameDatabase;
-
-    // Map structure that stores the pointers to all KeyFrames and MapPoints.
-    //Map* mpMap;
-    Atlas_ptr mpAtlas;
-
-    // Tracker. It receives a frame and computes the associated camera pose.
-    // It also decides when to insert a new keyframe, create some new MapPoints and
-    // performs relocalization if tracking fails.
-    Tracking* mpTracker;
-
-    // Local Mapper. It manages the local map and performs local bundle adjustment.
-    LocalMapping* mpLocalMapper;
-
-    // Loop Closer. It searches loops with every new keyframe. If there is a loop it performs
-    // a pose graph optimization and full bundle adjustment (in a new thread) afterwards.
-    LoopClosing* mpLoopCloser;
-
-    // System threads: Local Mapping, Loop Closing, Viewer.
-    // The Tracking thread "lives" in the main execution thread that creates the System object.
-    std::thread* mptLocalMapping;
-    std::thread* mptLoopClosing;
-
-    // Reset flag
-    std::mutex mMutexReset;
-    bool mbReset;
-    bool mbResetActiveMap;
-
-    // Change mode flags
-    std::mutex mMutexMode;
-    bool mbActivateLocalizationMode;
-    bool mbDeactivateLocalizationMode;
-
-    // Tracking state
-    int mTrackingState;
-    std::vector<MapPoint*> mTrackedMapPoints;
-    std::vector<cv::KeyPoint> mTrackedKeyPointsUn;
-    std::mutex mMutexState;
-
-    //
-    string mStrLoadAtlasFromFile;
-    string mStrSaveAtlasToFile;
-
-    string mStrVocabularyFilePath;
-
-    Settings* settings_;
-};
-
-}// namespace ORB_SLAM
diff --git a/include/Tracking.h b/include/Tracking.h
deleted file mode 100644
index b5c21db7..00000000
--- a/include/Tracking.h
+++ /dev/null
@@ -1,364 +0,0 @@
-/**
- * This file is part of ORB-SLAM3
- *
- * Copyright (C) 2017-2021 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/>.
- */
-
-#pragma once
-
-#include <mutex>
-#include <opencv2/core/core.hpp>
-#include <opencv2/features2d/features2d.hpp>
-#include <unordered_set>
-
-#include "Atlas.h"
-#include "Frame.h"
-#include "GeometricCamera.h"
-#include "ImuTypes.h"
-#include "KeyFrameDatabase.h"
-#include "LocalMapping.h"
-#include "LoopClosing.h"
-#include "ORBVocabulary.h"
-#include "ORBextractor.h"
-#include "Settings.h"
-#include "System.h"
-#include "ImprovedTypes.hpp"
-#include "Camera.hpp"
-
-namespace ORB_SLAM3 {
-
-class Atlas;
-class LocalMapping;
-class LoopClosing;
-class System;
-class Settings;
-
-class Tracking {
- public:
-  
-  Tracking(System* pSys, ORBVocabulary* pVoc,
-           const Atlas_ptr &pAtlas, KeyFrameDatabase* pKFDB,
-           const string& strSettingPath, const CameraType::eSensor sensor, Settings* settings,
-           const string& _nameSeq = std::string());
-
-  ~Tracking();
-
-  // Parse the config file
-  bool ParseCamParamFile(cv::FileStorage& fSettings);
-  bool ParseORBParamFile(cv::FileStorage& fSettings);
-  bool ParseIMUParamFile(cv::FileStorage& fSettings);
-
-  // Preprocess the input and call Track(). Extract features and performs stereo
-  // matching.
-  Sophus::SE3f GrabImageStereo(const cv::Mat& imRectLeft,
-                               const cv::Mat& imRectRight,
-                               const double& timestamp, const string &filename,
-                               const Camera_ptr &cam);
-  Sophus::SE3f GrabImageRGBD(const cv::Mat& imRGB, const cv::Mat& imD,
-                             const double& timestamp, const string &filename,
-                             const Camera_ptr &cam);
-  Sophus::SE3f GrabImageMonocular(const cv::Mat& im, const double& timestamp,
-                                  const string &filename, const Camera_ptr &cam);
-
-  void GrabImuData(const IMU::Point& imuMeasurement);
-
-  void SetLocalMapper(LocalMapping* pLocalMapper);
-  void SetLoopClosing(LoopClosing* pLoopClosing);
-
-  // Load new settings
-  // The focal lenght should be similar or scale prediction will fail when
-  // projecting points
-  void ChangeCalibration(const string& strSettingPath);
-
-  // Use this function if you have deactivated local mapping and you only want
-  // to localize the camera.
-  void InformOnlyTracking(const bool& flag);
-
-  void UpdateFrameIMU(const float s, const IMU::Bias& b,
-                      KeyFrame* pCurrentKeyFrame);
-  KeyFrame* GetLastKeyFrame() { return mpLastKeyFrame; }
-
-  void CreateMapInAtlas();
-  // std::mutex mMutexTracks;
-
-  //--
-  void NewDataset();
-  int GetNumberDataset();
-  int GetMatchesInliers();
-
-  // DEBUG
-  void SaveSubTrajectory(string strNameFile_frames, string strNameFile_kf,
-                         string strFolder = "");
-  void SaveSubTrajectory(string strNameFile_frames, string strNameFile_kf,
-                         std::shared_ptr<Map> pMap);
-
-  float GetImageScale();
-
-#ifdef REGISTER_LOOP
-  void RequestStop();
-  bool isStopped();
-  void Release();
-  bool stopRequested();
-#endif
-
- public:
-
-  Tracker::eTrackingState mState;
-  Tracker::eTrackingState mLastProcessedState;
-
-  // Input sensor
-  CameraType::eSensor mSensor;
-
-  // Current Frame
-  Frame mCurrentFrame;
-  Frame mLastFrame;
-
-  cv::Mat mImGray;
-
-  // Initialization Variables (Monocular)
-  std::vector<int> mvIniLastMatches;
-  std::vector<int> mvIniMatches;
-  std::vector<cv::Point2f> mvbPrevMatched;
-  std::vector<cv::Point3f> mvIniP3D;
-  Frame mInitialFrame;
-
-  // Lists used to recover the full camera trajectory at the end of the
-  // execution. Basically we store the reference keyframe for each frame and its
-  // relative transformation
-  list<Sophus::SE3f> mlRelativeFramePoses;
-  list<KeyFrame*> mlpReferences;
-  list<double> mlFrameTimes;
-  list<bool> mlbLost;
-
-  // frames with estimated pose
-  int mTrackedFr;
-  bool mbStep;
-
-  // True if local mapping is deactivated and we are performing only
-  // localization
-  bool mbOnlyTracking;
-
-  void Reset(bool bLocMap = false);
-  void ResetActiveMap(bool bLocMap = false);
-
-  float mMeanTrack;
-  bool mbInitWith3KFs;
-  double t0;     // time-stamp of first read frame
-  double t0vis;  // time-stamp of first inserted keyframe
-  double t0IMU;  // time-stamp of IMU initialization
-  bool mFastInit = false;
-
-  vector<MapPoint*> GetLocalMapMPS();
-
-  bool mbWriteStats;
-
-#ifdef REGISTER_TIMES
-  void LocalMapStats2File();
-  void TrackStats2File();
-  void PrintTimeStats();
-
-  vector<double> vdRectStereo_ms;
-  vector<double> vdResizeImage_ms;
-  vector<double> vdORBExtract_ms;
-  vector<double> vdStereoMatch_ms;
-  vector<double> vdIMUInteg_ms;
-  vector<double> vdPosePred_ms;
-  vector<double> vdLMTrack_ms;
-  vector<double> vdNewKF_ms;
-  vector<double> vdTrackTotal_ms;
-#endif
-
- protected:
-  // Main tracking function. It is independent of the input sensor.
-  void Track();
-
-  // Map initialization for stereo and RGB-D
-  void StereoInitialization();
-
-  // Map initialization for monocular
-  void MonocularInitialization();
-  // void CreateNewMapPoints();
-  void CreateInitialMapMonocular();
-
-  void CheckReplacedInLastFrame();
-  bool TrackReferenceKeyFrame();
-  void UpdateLastFrame();
-  bool TrackWithMotionModel();
-  bool PredictStateIMU();
-
-  bool Relocalization();
-
-  void UpdateLocalMap();
-  void UpdateLocalPoints();
-  void UpdateLocalKeyFrames();
-
-  bool TrackLocalMap();
-  void SearchLocalPoints();
-
-  bool NeedNewKeyFrame();
-  void CreateNewKeyFrame();
-
-  // Perform preintegration from last frame
-  void PreintegrateIMU();
-
-  // Reset IMU biases and compute frame velocity
-  void ResetFrameIMU();
-
-  bool mbMapUpdated;
-
-  // Imu preintegration from last frame
-  IMU::Preintegrated* mpImuPreintegratedFromLastKF;
-
-  // Queue of IMU measurements between frames
-  std::list<IMU::Point> mlQueueImuData;
-
-  // Vector of IMU measurements from previous to current frame (to be filled by
-  // PreintegrateIMU)
-  std::vector<IMU::Point> mvImuFromLastFrame;
-  std::mutex mMutexImuQueue;
-
-  // Imu calibration parameters
-  std::shared_ptr<IMU::Calib> mpImuCalib;
-
-  // Last Bias Estimation (at keyframe creation)
-  IMU::Bias mLastBias;
-
-  // In case of performing only localization, this flag is true when there are
-  // no matches to points in the map. Still tracking will continue if there are
-  // enough matches with temporal points. In that case we are doing visual
-  // odometry. The system will try to do relocalization to recover "zero-drift"
-  // localization to the map.
-  bool mbVO;
-
-  // Other Thread Pointers
-  LocalMapping* mpLocalMapper;
-  LoopClosing* mpLoopClosing;
-
-  // ORB
-  std::shared_ptr<ORBextractor> mpORBextractorLeft;
-  std::shared_ptr<ORBextractor> mpORBextractorRight;
-  std::shared_ptr<ORBextractor> mpIniORBextractor;
-
-  // BoW
-  ORBVocabulary* mpORBVocabulary;
-  KeyFrameDatabase* mpKeyFrameDB;
-
-  // Initalization (only for monocular)
-  bool mbReadyToInitialize;
-  bool mbSetInit;
-
-  // Local Map
-  KeyFrame* mpReferenceKF;
-  std::vector<KeyFrame*> mvpLocalKeyFrames;
-  std::vector<MapPoint*> mvpLocalMapPoints;
-
-  // System
-  System* mpSystem;
-
-  // Atlas
-  Atlas_ptr mpAtlas;
-
-  // Calibration matrix
-  cv::Mat mK;
-  Eigen::Matrix3f mK_;
-  cv::Mat mDistCoef;
-  float mbf;
-  float mImageScale;
-
-  float mImuFreq;
-  double mImuPer;
-  bool mInsertKFsLost;
-
-  // New KeyFrame rules (according to fps)
-  int mMinFrames;
-  int mMaxFrames;
-
-  int mnFirstImuFrameId;
-  int mnFramesToResetIMU;
-
-  // Threshold close/far points
-  // Points seen as close by the stereo/RGBD sensor are considered reliable
-  // and inserted from just one frame. Far points requiere a match in two
-  // keyframes.
-  float mThDepth;
-
-  // For RGB-D inputs only. For some datasets (e.g. TUM) the depthmap values are
-  // scaled.
-  float mDepthMapFactor;
-
-  // Current matches in frame
-  int mnMatchesInliers;
-
-  // Last Frame, KeyFrame and Relocalisation Info
-  KeyFrame* mpLastKeyFrame;
-  unsigned int mnLastKeyFrameId;
-  unsigned int mnLastRelocFrameId;
-  double mTimeStampLost;
-  double time_recently_lost;
-
-  unsigned int mnFirstFrameId;
-  unsigned int mnInitialFrameId;
-  unsigned int mnLastInitFrameId;
-
-  bool mbCreatedMap;
-
-  // Motion Model
-  bool mbVelocity{false};
-  Sophus::SE3f mVelocity;
-
-  // Color order (true RGB, false BGR, ignored if grayscale)
-  bool mbRGB;
-
-  list<MapPoint*> mlpTemporalPoints;
-
-  // int nMapChangeIndex;
-
-  int mnNumDataset;
-
-  ofstream f_track_stats;
-
-  ofstream f_track_times;
-  double mTime_PreIntIMU;
-  double mTime_PosePred;
-  double mTime_LocalMapTrack;
-  double mTime_NewKF_Dec;
-
-  std::shared_ptr<GeometricCamera> mpCamera;
-  std::shared_ptr<GeometricCamera> mpCamera2;
-
-  int initID, lastID;
-
-  Sophus::SE3f mTlr;
-
-  void newParameterLoader(Settings* settings);
-
-#ifdef REGISTER_LOOP
-  bool Stop();
-
-  bool mbStopped;
-  bool mbStopRequested;
-  bool mbNotStop;
-  std::mutex mMutexStop;
-#endif
-
- public:
-  cv::Mat mImRight;
-};
-
-}  // namespace ORB_SLAM3
-
diff --git a/include/TwoViewReconstruction.h b/include/TwoViewReconstruction.h
deleted file mode 100644
index e275d531..00000000
--- a/include/TwoViewReconstruction.h
+++ /dev/null
@@ -1,96 +0,0 @@
-/**
-* This file is part of ORB-SLAM3
-*
-* Copyright (C) 2017-2021 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/>.
-*/
-
-#pragma once
-
-#include <opencv2/opencv.hpp>
-#include <Eigen/Core>
-#include <unordered_set>
-
-#include <sophus/se3.hpp>
-
-namespace ORB_SLAM3
-{
-
-    class TwoViewReconstruction
-    {
-        typedef std::pair<int,int> Match;
-
-    public:
-        
-        // Fix the reference frame
-        TwoViewReconstruction(const Eigen::Matrix3f& k, float sigma = 1.0, int iterations = 200);
-
-        // Computes in parallel a fundamental matrix and a homography
-        // Selects a model and tries to recover the motion and the structure from motion
-        bool Reconstruct(const std::vector<cv::KeyPoint>& vKeys1, const std::vector<cv::KeyPoint>& vKeys2, const std::vector<int> &vMatches12,
-                          Sophus::SE3f &T21, std::vector<cv::Point3f> &vP3D, std::vector<bool> &vbTriangulated);
-
-    private:
-
-        void FindHomography(std::vector<bool> &vbMatchesInliers, float &score, Eigen::Matrix3f &H21);
-        void FindFundamental(std::vector<bool> &vbInliers, float &score, Eigen::Matrix3f &F21);
-
-        Eigen::Matrix3f ComputeH21(const std::vector<cv::Point2f> &vP1, const std::vector<cv::Point2f> &vP2);
-        Eigen::Matrix3f ComputeF21(const std::vector<cv::Point2f> &vP1, const std::vector<cv::Point2f> &vP2);
-
-        float CheckHomography(const Eigen::Matrix3f &H21, const Eigen::Matrix3f &H12, std::vector<bool> &vbMatchesInliers, float sigma);
-
-        float CheckFundamental(const Eigen::Matrix3f &F21, std::vector<bool> &vbMatchesInliers, float sigma);
-
-        bool ReconstructF(std::vector<bool> &vbMatchesInliers, Eigen::Matrix3f &F21, Eigen::Matrix3f &K,
-                          Sophus::SE3f &T21, std::vector<cv::Point3f> &vP3D, std::vector<bool> &vbTriangulated, float minParallax, int minTriangulated);
-
-        bool ReconstructH(std::vector<bool> &vbMatchesInliers, Eigen::Matrix3f &H21, Eigen::Matrix3f &K,
-                          Sophus::SE3f &T21, std::vector<cv::Point3f> &vP3D,std:: vector<bool> &vbTriangulated, float minParallax, int minTriangulated);
-
-        void Normalize(const std::vector<cv::KeyPoint> &vKeys, std::vector<cv::Point2f> &vNormalizedPoints, Eigen::Matrix3f &T);
-
-
-        int CheckRT(const Eigen::Matrix3f &R, const Eigen::Vector3f &t, const std::vector<cv::KeyPoint> &vKeys1, const std::vector<cv::KeyPoint> &vKeys2,
-                    const std::vector<Match> &vMatches12, std::vector<bool> &vbMatchesInliers,
-                    const Eigen::Matrix3f &K, std::vector<cv::Point3f> &vP3D, float th2, std::vector<bool> &vbGood, float &parallax);
-
-        void DecomposeE(const Eigen::Matrix3f &E, Eigen::Matrix3f &R1, Eigen::Matrix3f &R2, Eigen::Vector3f &t);
-
-
-        // Keypoints from Reference Frame (Frame 1)
-        std::vector<cv::KeyPoint> mvKeys1;
-
-        // Keypoints from Current Frame (Frame 2)
-        std::vector<cv::KeyPoint> mvKeys2;
-
-        // Current Matches from Reference to Current
-        std::vector<Match> mvMatches12;
-        std::vector<bool> mvbMatched1;
-
-        // Calibration
-        Eigen::Matrix3f mK;
-
-        // Standard Deviation and Variance
-        float mSigma, mSigma2;
-
-        // Ransac max iterations
-        int mMaxIterations;
-
-        // Ransac sets
-        std::vector<std::vector<size_t> > mvSets;
-
-    };
-
-} //namespace ORB_SLAM
diff --git a/include/Viewer.h b/include/Viewer.h
deleted file mode 100644
index 37f5db36..00000000
--- a/include/Viewer.h
+++ /dev/null
@@ -1,77 +0,0 @@
-/**
- * This file is part of ORB-SLAM3
- *
- * Copyright (C) 2017-2021 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/>.
- */
-
-#pragma once
-
-#include <mutex>
-#include <memory>
-
-#include "ImprovedTypes.hpp"
-#include "FrameDrawer.h"
-#include "MapDrawer.h"
-#include "Settings.h"
-
-namespace ORB_SLAM3 {
-
-class Viewer {
-  void newParameterLoader(const Settings& settings);
-  // Main thread function. Draw points, keyframes, the current camera pose and
-  // the last processed frame. Drawing is refreshed according to the camera fps.
-  // We use Pangolin.
-  void Run();
- public:
-
-  Viewer(const System_ptr &pSystem, const std::string &strSettingPath);
-  Viewer(const System_ptr &pSystem, const Settings &settings);
-
-  virtual ~Viewer();
-
-  void update(const Sophus::SE3f &pose);
-
-  void close();
-  bool isClosed() const;
-  bool isOpen() const;
-
-
- private:
-  void setBoth(const bool b);
-  bool ParseViewerParamFile(cv::FileStorage& fSettings);
-
-  bool Stop();
-
-  System_ptr mpSystem;
-  FrameDrawer mpFrameDrawer;
-  MapDrawer mpMapDrawer;
-  Tracking_ptr mpTracker;
-  std::thread mptViewer;
-  bool both;
-
-  // 1/fps in ms
-  double mT;
-  float mImageWidth, mImageHeight;
-  float mImageViewerScale;
-
-  float mViewpointX, mViewpointY, mViewpointZ, mViewpointF;
-
-  bool mbClosed;
-};
-
-}  // namespace ORB_SLAM3
diff --git a/output.txt b/output.txt
index 3c6c406f..1cf3a992 100644
--- a/output.txt
+++ b/output.txt
@@ -65,7 +65,7 @@ Consolidate compiler generated dependencies of target test_ceres_se3.cpp
 [ 92%] Built target test_so3
 Consolidate compiler generated dependencies of target HelloSO3
 [100%] Built target HelloSO3
-Configuring and building ORB_SLAM3 ...
+Configuring and building MORB_SLAM ...
 -- Build type: RelWithDebInfo
 -- OPENCV VERSION:
 -- 4.5.4
@@ -96,13 +96,13 @@ Configuring and building ORB_SLAM3 ...
 [ 42%] Building CXX object CMakeFiles/MORB_SLAM.dir/src/Map.cc.o
 [ 43%] Building CXX object CMakeFiles/MORB_SLAM.dir/src/MapDrawer.cc.o
 [ 44%] Building CXX object CMakeFiles/MORB_SLAM.dir/src/Optimizer.cc.o
-/orin_ssd/MORB_SLAM/src/Map.cc: In member function ‘void ORB_SLAM3::Map::PreSave(std::set<std::shared_ptr<ORB_SLAM3::GeometricCamera> >&, std::shared_ptr<ORB_SLAM3::Map>)’:
-/orin_ssd/MORB_SLAM/src/Map.cc:339:32: error: no match for ‘operator!=’ (operand types are ‘std::shared_ptr<ORB_SLAM3::Map>’ and ‘std::__shared_ptr<ORB_SLAM3::Map, __gnu_cxx::_S_atomic>::element_type*’ {aka ‘ORB_SLAM3::Map*’})
+/orin_ssd/MORB_SLAM/src/Map.cc: In member function ‘void MORB_SLAM::Map::PreSave(std::set<std::shared_ptr<MORB_SLAM::GeometricCamera> >&, std::shared_ptr<MORB_SLAM::Map>)’:
+/orin_ssd/MORB_SLAM/src/Map.cc:339:32: error: no match for ‘operator!=’ (operand types are ‘std::shared_ptr<MORB_SLAM::Map>’ and ‘std::__shared_ptr<MORB_SLAM::Map, __gnu_cxx::_S_atomic>::element_type*’ {aka ‘MORB_SLAM::Map*’})
   339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
       |            ~~~~~~~~~~~~~~~~~~~ ^~ ~~~~~~~~~~~~~~~
       |                             |                  |
-      |                             |                  std::__shared_ptr<ORB_SLAM3::Map, __gnu_cxx::_S_atomic>::element_type* {aka ORB_SLAM3::Map*}
-      |                             std::shared_ptr<ORB_SLAM3::Map>
+      |                             |                  std::__shared_ptr<MORB_SLAM::Map, __gnu_cxx::_S_atomic>::element_type* {aka MORB_SLAM::Map*}
+      |                             std::shared_ptr<MORB_SLAM::Map>
 In file included from /usr/include/c++/9/bits/stl_algobase.h:64,
                  from /usr/include/c++/9/bits/stl_tree.h:63,
                  from /usr/include/c++/9/map:60,
@@ -115,7 +115,7 @@ In file included from /usr/include/c++/9/bits/stl_algobase.h:64,
   461 |     operator!=(const pair<_T1, _T2>& __x, const pair<_T1, _T2>& __y)
       |     ^~~~~~~~
 /usr/include/c++/9/bits/stl_pair.h:461:5: note:   template argument deduction/substitution failed:
-/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<ORB_SLAM3::Map>’ is not derived from ‘const std::pair<_T1, _T2>’
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<MORB_SLAM::Map>’ is not derived from ‘const std::pair<_T1, _T2>’
   339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
       |                                                 ^
 In file included from /usr/include/c++/9/bits/stl_algobase.h:67,
@@ -130,7 +130,7 @@ In file included from /usr/include/c++/9/bits/stl_algobase.h:67,
   337 |     operator!=(const reverse_iterator<_Iterator>& __x,
       |     ^~~~~~~~
 /usr/include/c++/9/bits/stl_iterator.h:337:5: note:   template argument deduction/substitution failed:
-/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<ORB_SLAM3::Map>’ is not derived from ‘const std::reverse_iterator<_Iterator>’
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<MORB_SLAM::Map>’ is not derived from ‘const std::reverse_iterator<_Iterator>’
   339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
       |                                                 ^
 In file included from /usr/include/c++/9/bits/stl_algobase.h:67,
@@ -145,7 +145,7 @@ In file included from /usr/include/c++/9/bits/stl_algobase.h:67,
   375 |     operator!=(const reverse_iterator<_IteratorL>& __x,
       |     ^~~~~~~~
 /usr/include/c++/9/bits/stl_iterator.h:375:5: note:   template argument deduction/substitution failed:
-/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<ORB_SLAM3::Map>’ is not derived from ‘const std::reverse_iterator<_Iterator>’
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<MORB_SLAM::Map>’ is not derived from ‘const std::reverse_iterator<_Iterator>’
   339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
       |                                                 ^
 In file included from /usr/include/c++/9/bits/stl_algobase.h:67,
@@ -160,7 +160,7 @@ In file included from /usr/include/c++/9/bits/stl_algobase.h:67,
  1148 |     operator!=(const move_iterator<_IteratorL>& __x,
       |     ^~~~~~~~
 /usr/include/c++/9/bits/stl_iterator.h:1148:5: note:   template argument deduction/substitution failed:
-/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<ORB_SLAM3::Map>’ is not derived from ‘const std::move_iterator<_IteratorL>’
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<MORB_SLAM::Map>’ is not derived from ‘const std::move_iterator<_IteratorL>’
   339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
       |                                                 ^
 In file included from /usr/include/c++/9/bits/stl_algobase.h:67,
@@ -175,7 +175,7 @@ In file included from /usr/include/c++/9/bits/stl_algobase.h:67,
  1154 |     operator!=(const move_iterator<_Iterator>& __x,
       |     ^~~~~~~~
 /usr/include/c++/9/bits/stl_iterator.h:1154:5: note:   template argument deduction/substitution failed:
-/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<ORB_SLAM3::Map>’ is not derived from ‘const std::move_iterator<_IteratorL>’
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<MORB_SLAM::Map>’ is not derived from ‘const std::move_iterator<_IteratorL>’
   339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
       |                                                 ^
 In file included from /usr/include/c++/9/bits/stl_tree.h:64,
@@ -189,7 +189,7 @@ In file included from /usr/include/c++/9/bits/stl_tree.h:64,
   173 |     operator!=(const allocator<_T1>&, const allocator<_T2>&)
       |     ^~~~~~~~
 /usr/include/c++/9/bits/allocator.h:173:5: note:   template argument deduction/substitution failed:
-/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<ORB_SLAM3::Map>’ is not derived from ‘const std::allocator<_Tp1>’
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<MORB_SLAM::Map>’ is not derived from ‘const std::allocator<_Tp1>’
   339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
       |                                                 ^
 In file included from /usr/include/c++/9/bits/char_traits.h:40,
@@ -208,7 +208,7 @@ In file included from /usr/include/c++/9/bits/char_traits.h:40,
   227 |     operator!=(const fpos<_StateT>& __lhs, const fpos<_StateT>& __rhs)
       |     ^~~~~~~~
 /usr/include/c++/9/bits/postypes.h:227:5: note:   template argument deduction/substitution failed:
-/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<ORB_SLAM3::Map>’ is not derived from ‘const std::fpos<_StateT>’
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<MORB_SLAM::Map>’ is not derived from ‘const std::fpos<_StateT>’
   339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
       |                                                 ^
 In file included from /usr/include/c++/9/bits/basic_string.h:48,
@@ -227,7 +227,7 @@ In file included from /usr/include/c++/9/bits/basic_string.h:48,
   489 |     operator!=(basic_string_view<_CharT, _Traits> __x,
       |     ^~~~~~~~
 /usr/include/c++/9/string_view:489:5: note:   template argument deduction/substitution failed:
-/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<ORB_SLAM3::Map>’ is not derived from ‘std::basic_string_view<_CharT, _Traits>’
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<MORB_SLAM::Map>’ is not derived from ‘std::basic_string_view<_CharT, _Traits>’
   339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
       |                                                 ^
 In file included from /usr/include/c++/9/bits/basic_string.h:48,
@@ -246,7 +246,7 @@ In file included from /usr/include/c++/9/bits/basic_string.h:48,
   495 |     operator!=(basic_string_view<_CharT, _Traits> __x,
       |     ^~~~~~~~
 /usr/include/c++/9/string_view:495:5: note:   template argument deduction/substitution failed:
-/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<ORB_SLAM3::Map>’ is not derived from ‘std::basic_string_view<_CharT, _Traits>’
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<MORB_SLAM::Map>’ is not derived from ‘std::basic_string_view<_CharT, _Traits>’
   339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
       |                                                 ^
 In file included from /usr/include/c++/9/bits/basic_string.h:48,
@@ -265,7 +265,7 @@ In file included from /usr/include/c++/9/bits/basic_string.h:48,
   501 |     operator!=(__detail::__idt<basic_string_view<_CharT, _Traits>> __x,
       |     ^~~~~~~~
 /usr/include/c++/9/string_view:501:5: note:   template argument deduction/substitution failed:
-/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   mismatched types ‘std::basic_string_view<_CharT, _Traits>’ and ‘std::__shared_ptr<ORB_SLAM3::Map, __gnu_cxx::_S_atomic>::element_type*’ {aka ‘ORB_SLAM3::Map*’}
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   mismatched types ‘std::basic_string_view<_CharT, _Traits>’ and ‘std::__shared_ptr<MORB_SLAM::Map, __gnu_cxx::_S_atomic>::element_type*’ {aka ‘MORB_SLAM::Map*’}
   339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
       |                                                 ^
 In file included from /usr/include/c++/9/string:55,
@@ -283,7 +283,7 @@ In file included from /usr/include/c++/9/string:55,
  6191 |     operator!=(const basic_string<_CharT, _Traits, _Alloc>& __lhs,
       |     ^~~~~~~~
 /usr/include/c++/9/bits/basic_string.h:6191:5: note:   template argument deduction/substitution failed:
-/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<ORB_SLAM3::Map>’ is not derived from ‘const std::__cxx11::basic_string<_CharT, _Traits, _Allocator>’
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<MORB_SLAM::Map>’ is not derived from ‘const std::__cxx11::basic_string<_CharT, _Traits, _Allocator>’
   339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
       |                                                 ^
 In file included from /usr/include/c++/9/string:55,
@@ -301,7 +301,7 @@ In file included from /usr/include/c++/9/string:55,
  6204 |     operator!=(const _CharT* __lhs,
       |     ^~~~~~~~
 /usr/include/c++/9/bits/basic_string.h:6204:5: note:   template argument deduction/substitution failed:
-/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   mismatched types ‘const _CharT*’ and ‘std::shared_ptr<ORB_SLAM3::Map>’
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   mismatched types ‘const _CharT*’ and ‘std::shared_ptr<MORB_SLAM::Map>’
   339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
       |                                                 ^
 In file included from /usr/include/c++/9/string:55,
@@ -319,7 +319,7 @@ In file included from /usr/include/c++/9/string:55,
  6216 |     operator!=(const basic_string<_CharT, _Traits, _Alloc>& __lhs,
       |     ^~~~~~~~
 /usr/include/c++/9/bits/basic_string.h:6216:5: note:   template argument deduction/substitution failed:
-/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<ORB_SLAM3::Map>’ is not derived from ‘const std::__cxx11::basic_string<_CharT, _Traits, _Allocator>’
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<MORB_SLAM::Map>’ is not derived from ‘const std::__cxx11::basic_string<_CharT, _Traits, _Allocator>’
   339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
       |                                                 ^
 In file included from /usr/include/c++/9/bits/node_handle.h:39,
@@ -334,7 +334,7 @@ In file included from /usr/include/c++/9/bits/node_handle.h:39,
   992 |     operator!=(const optional<_Tp>& __lhs, const optional<_Up>& __rhs)
       |     ^~~~~~~~
 /usr/include/c++/9/optional:992:5: note:   template argument deduction/substitution failed:
-/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<ORB_SLAM3::Map>’ is not derived from ‘const std::optional<_Tp>’
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<MORB_SLAM::Map>’ is not derived from ‘const std::optional<_Tp>’
   339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
       |                                                 ^
 In file included from /usr/include/c++/9/bits/node_handle.h:39,
@@ -349,7 +349,7 @@ In file included from /usr/include/c++/9/bits/node_handle.h:39,
  1044 |     operator!=(const optional<_Tp>& __lhs, nullopt_t) noexcept
       |     ^~~~~~~~
 /usr/include/c++/9/optional:1044:5: note:   template argument deduction/substitution failed:
-/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<ORB_SLAM3::Map>’ is not derived from ‘const std::optional<_Tp>’
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<MORB_SLAM::Map>’ is not derived from ‘const std::optional<_Tp>’
   339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
       |                                                 ^
 In file included from /usr/include/c++/9/bits/node_handle.h:39,
@@ -364,7 +364,7 @@ In file included from /usr/include/c++/9/bits/node_handle.h:39,
  1049 |     operator!=(nullopt_t, const optional<_Tp>& __rhs) noexcept
       |     ^~~~~~~~
 /usr/include/c++/9/optional:1049:5: note:   template argument deduction/substitution failed:
-/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   mismatched types ‘const std::optional<_Tp>’ and ‘std::__shared_ptr<ORB_SLAM3::Map, __gnu_cxx::_S_atomic>::element_type*’ {aka ‘ORB_SLAM3::Map*’}
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   mismatched types ‘const std::optional<_Tp>’ and ‘std::__shared_ptr<MORB_SLAM::Map, __gnu_cxx::_S_atomic>::element_type*’ {aka ‘MORB_SLAM::Map*’}
   339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
       |                                                 ^
 In file included from /usr/include/c++/9/bits/node_handle.h:39,
@@ -379,7 +379,7 @@ In file included from /usr/include/c++/9/bits/node_handle.h:39,
  1107 |     operator!=(const optional<_Tp>& __lhs, const _Up& __rhs)
       |     ^~~~~~~~
 /usr/include/c++/9/optional:1107:5: note:   template argument deduction/substitution failed:
-/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<ORB_SLAM3::Map>’ is not derived from ‘const std::optional<_Tp>’
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<MORB_SLAM::Map>’ is not derived from ‘const std::optional<_Tp>’
   339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
       |                                                 ^
 In file included from /usr/include/c++/9/bits/node_handle.h:39,
@@ -394,7 +394,7 @@ In file included from /usr/include/c++/9/bits/node_handle.h:39,
  1113 |     operator!=(const _Up& __lhs, const optional<_Tp>& __rhs)
       |     ^~~~~~~~
 /usr/include/c++/9/optional:1113:5: note:   template argument deduction/substitution failed:
-/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   mismatched types ‘const std::optional<_Tp>’ and ‘std::__shared_ptr<ORB_SLAM3::Map, __gnu_cxx::_S_atomic>::element_type*’ {aka ‘ORB_SLAM3::Map*’}
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   mismatched types ‘const std::optional<_Tp>’ and ‘std::__shared_ptr<MORB_SLAM::Map, __gnu_cxx::_S_atomic>::element_type*’ {aka ‘MORB_SLAM::Map*’}
   339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
       |                                                 ^
 In file included from /usr/include/c++/9/tuple:39,
@@ -409,7 +409,7 @@ In file included from /usr/include/c++/9/tuple:39,
   257 |     operator!=(const array<_Tp, _Nm>& __one, const array<_Tp, _Nm>& __two)
       |     ^~~~~~~~
 /usr/include/c++/9/array:257:5: note:   template argument deduction/substitution failed:
-/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<ORB_SLAM3::Map>’ is not derived from ‘const std::array<_Tp, _Nm>’
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<MORB_SLAM::Map>’ is not derived from ‘const std::array<_Tp, _Nm>’
   339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
       |                                                 ^
 In file included from /usr/include/c++/9/bits/stl_map.h:63,
@@ -423,7 +423,7 @@ In file included from /usr/include/c++/9/bits/stl_map.h:63,
  1445 |     operator!=(const tuple<_TElements...>& __t,
       |     ^~~~~~~~
 /usr/include/c++/9/tuple:1445:5: note:   template argument deduction/substitution failed:
-/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<ORB_SLAM3::Map>’ is not derived from ‘const std::tuple<_Tps ...>’
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<MORB_SLAM::Map>’ is not derived from ‘const std::tuple<_Tps ...>’
   339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
       |                                                 ^
 In file included from /usr/include/c++/9/map:61,
@@ -436,7 +436,7 @@ In file included from /usr/include/c++/9/map:61,
  1479 |     operator!=(const map<_Key, _Tp, _Compare, _Alloc>& __x,
       |     ^~~~~~~~
 /usr/include/c++/9/bits/stl_map.h:1479:5: note:   template argument deduction/substitution failed:
-/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<ORB_SLAM3::Map>’ is not derived from ‘const std::map<_Key, _Tp, _Compare, _Allocator>’
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<MORB_SLAM::Map>’ is not derived from ‘const std::map<_Key, _Tp, _Compare, _Allocator>’
   339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
       |                                                 ^
 In file included from /usr/include/c++/9/map:62,
@@ -449,7 +449,7 @@ In file included from /usr/include/c++/9/map:62,
  1143 |     operator!=(const multimap<_Key, _Tp, _Compare, _Alloc>& __x,
       |     ^~~~~~~~
 /usr/include/c++/9/bits/stl_multimap.h:1143:5: note:   template argument deduction/substitution failed:
-/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<ORB_SLAM3::Map>’ is not derived from ‘const std::multimap<_Key, _Tp, _Compare, _Allocator>’
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<MORB_SLAM::Map>’ is not derived from ‘const std::multimap<_Key, _Tp, _Compare, _Allocator>’
   339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
       |                                                 ^
 In file included from /usr/include/c++/9/memory:80,
@@ -465,7 +465,7 @@ In file included from /usr/include/c++/9/memory:80,
   732 |     operator!=(const unique_ptr<_Tp, _Dp>& __x,
       |     ^~~~~~~~
 /usr/include/c++/9/bits/unique_ptr.h:732:5: note:   template argument deduction/substitution failed:
-/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<ORB_SLAM3::Map>’ is not derived from ‘const std::unique_ptr<_Tp, _Dp>’
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<MORB_SLAM::Map>’ is not derived from ‘const std::unique_ptr<_Tp, _Dp>’
   339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
       |                                                 ^
 In file included from /usr/include/c++/9/memory:80,
@@ -481,7 +481,7 @@ In file included from /usr/include/c++/9/memory:80,
   738 |     operator!=(const unique_ptr<_Tp, _Dp>& __x, nullptr_t) noexcept
       |     ^~~~~~~~
 /usr/include/c++/9/bits/unique_ptr.h:738:5: note:   template argument deduction/substitution failed:
-/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<ORB_SLAM3::Map>’ is not derived from ‘const std::unique_ptr<_Tp, _Dp>’
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<MORB_SLAM::Map>’ is not derived from ‘const std::unique_ptr<_Tp, _Dp>’
   339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
       |                                                 ^
 In file included from /usr/include/c++/9/memory:80,
@@ -497,7 +497,7 @@ In file included from /usr/include/c++/9/memory:80,
   743 |     operator!=(nullptr_t, const unique_ptr<_Tp, _Dp>& __x) noexcept
       |     ^~~~~~~~
 /usr/include/c++/9/bits/unique_ptr.h:743:5: note:   template argument deduction/substitution failed:
-/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   mismatched types ‘const std::unique_ptr<_Tp, _Dp>’ and ‘std::__shared_ptr<ORB_SLAM3::Map, __gnu_cxx::_S_atomic>::element_type*’ {aka ‘ORB_SLAM3::Map*’}
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   mismatched types ‘const std::unique_ptr<_Tp, _Dp>’ and ‘std::__shared_ptr<MORB_SLAM::Map, __gnu_cxx::_S_atomic>::element_type*’ {aka ‘MORB_SLAM::Map*’}
   339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
       |                                                 ^
 In file included from /usr/include/c++/9/bits/shared_ptr.h:52,
@@ -514,7 +514,7 @@ In file included from /usr/include/c++/9/bits/shared_ptr.h:52,
  1428 |     operator!=(const __shared_ptr<_Tp1, _Lp>& __a,
       |     ^~~~~~~~
 /usr/include/c++/9/bits/shared_ptr_base.h:1428:5: note:   template argument deduction/substitution failed:
-/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   mismatched types ‘const std::__shared_ptr<_Tp2, _Lp>’ and ‘std::__shared_ptr<ORB_SLAM3::Map, __gnu_cxx::_S_atomic>::element_type*’ {aka ‘ORB_SLAM3::Map*’}
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   mismatched types ‘const std::__shared_ptr<_Tp2, _Lp>’ and ‘std::__shared_ptr<MORB_SLAM::Map, __gnu_cxx::_S_atomic>::element_type*’ {aka ‘MORB_SLAM::Map*’}
   339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
       |                                                 ^
 In file included from /usr/include/c++/9/bits/shared_ptr.h:52,
@@ -531,7 +531,7 @@ In file included from /usr/include/c++/9/bits/shared_ptr.h:52,
  1434 |     operator!=(const __shared_ptr<_Tp, _Lp>& __a, nullptr_t) noexcept
       |     ^~~~~~~~
 /usr/include/c++/9/bits/shared_ptr_base.h:1434:5: note:   template argument deduction/substitution failed:
-/orin_ssd/MORB_SLAM/src/Map.cc:339:48: note:   cannot convert ‘sharedMap.std::shared_ptr<ORB_SLAM3::Map>::<anonymous>.std::__shared_ptr<ORB_SLAM3::Map, __gnu_cxx::_S_atomic>::get()’ (type ‘std::__shared_ptr<ORB_SLAM3::Map, __gnu_cxx::_S_atomic>::element_type*’ {aka ‘ORB_SLAM3::Map*’}) to type ‘std::nullptr_t’
+/orin_ssd/MORB_SLAM/src/Map.cc:339:48: note:   cannot convert ‘sharedMap.std::shared_ptr<MORB_SLAM::Map>::<anonymous>.std::__shared_ptr<MORB_SLAM::Map, __gnu_cxx::_S_atomic>::get()’ (type ‘std::__shared_ptr<MORB_SLAM::Map, __gnu_cxx::_S_atomic>::element_type*’ {aka ‘MORB_SLAM::Map*’}) to type ‘std::nullptr_t’
   339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
       |                                   ~~~~~~~~~~~~~^~
 In file included from /usr/include/c++/9/bits/shared_ptr.h:52,
@@ -548,7 +548,7 @@ In file included from /usr/include/c++/9/bits/shared_ptr.h:52,
  1439 |     operator!=(nullptr_t, const __shared_ptr<_Tp, _Lp>& __a) noexcept
       |     ^~~~~~~~
 /usr/include/c++/9/bits/shared_ptr_base.h:1439:5: note:   template argument deduction/substitution failed:
-/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   mismatched types ‘const std::__shared_ptr<_Tp, _Lp>’ and ‘std::__shared_ptr<ORB_SLAM3::Map, __gnu_cxx::_S_atomic>::element_type*’ {aka ‘ORB_SLAM3::Map*’}
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   mismatched types ‘const std::__shared_ptr<_Tp, _Lp>’ and ‘std::__shared_ptr<MORB_SLAM::Map, __gnu_cxx::_S_atomic>::element_type*’ {aka ‘MORB_SLAM::Map*’}
   339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
       |                                                 ^
 In file included from /usr/include/c++/9/memory:81,
@@ -564,7 +564,7 @@ In file included from /usr/include/c++/9/memory:81,
   398 |     operator!=(const shared_ptr<_Tp>& __a, const shared_ptr<_Up>& __b) noexcept
       |     ^~~~~~~~
 /usr/include/c++/9/bits/shared_ptr.h:398:5: note:   template argument deduction/substitution failed:
-/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   mismatched types ‘const std::shared_ptr<_Tp>’ and ‘std::__shared_ptr<ORB_SLAM3::Map, __gnu_cxx::_S_atomic>::element_type*’ {aka ‘ORB_SLAM3::Map*’}
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   mismatched types ‘const std::shared_ptr<_Tp>’ and ‘std::__shared_ptr<MORB_SLAM::Map, __gnu_cxx::_S_atomic>::element_type*’ {aka ‘MORB_SLAM::Map*’}
   339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
       |                                                 ^
 In file included from /usr/include/c++/9/memory:81,
@@ -580,7 +580,7 @@ In file included from /usr/include/c++/9/memory:81,
   403 |     operator!=(const shared_ptr<_Tp>& __a, nullptr_t) noexcept
       |     ^~~~~~~~
 /usr/include/c++/9/bits/shared_ptr.h:403:5: note:   template argument deduction/substitution failed:
-/orin_ssd/MORB_SLAM/src/Map.cc:339:48: note:   cannot convert ‘sharedMap.std::shared_ptr<ORB_SLAM3::Map>::<anonymous>.std::__shared_ptr<ORB_SLAM3::Map, __gnu_cxx::_S_atomic>::get()’ (type ‘std::__shared_ptr<ORB_SLAM3::Map, __gnu_cxx::_S_atomic>::element_type*’ {aka ‘ORB_SLAM3::Map*’}) to type ‘std::nullptr_t’
+/orin_ssd/MORB_SLAM/src/Map.cc:339:48: note:   cannot convert ‘sharedMap.std::shared_ptr<MORB_SLAM::Map>::<anonymous>.std::__shared_ptr<MORB_SLAM::Map, __gnu_cxx::_S_atomic>::get()’ (type ‘std::__shared_ptr<MORB_SLAM::Map, __gnu_cxx::_S_atomic>::element_type*’ {aka ‘MORB_SLAM::Map*’}) to type ‘std::nullptr_t’
   339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
       |                                   ~~~~~~~~~~~~~^~
 In file included from /usr/include/c++/9/memory:81,
@@ -596,7 +596,7 @@ In file included from /usr/include/c++/9/memory:81,
   408 |     operator!=(nullptr_t, const shared_ptr<_Tp>& __a) noexcept
       |     ^~~~~~~~
 /usr/include/c++/9/bits/shared_ptr.h:408:5: note:   template argument deduction/substitution failed:
-/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   mismatched types ‘const std::shared_ptr<_Tp>’ and ‘std::__shared_ptr<ORB_SLAM3::Map, __gnu_cxx::_S_atomic>::element_type*’ {aka ‘ORB_SLAM3::Map*’}
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   mismatched types ‘const std::shared_ptr<_Tp>’ and ‘std::__shared_ptr<MORB_SLAM::Map, __gnu_cxx::_S_atomic>::element_type*’ {aka ‘MORB_SLAM::Map*’}
   339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
       |                                                 ^
 In file included from /usr/include/c++/9/vector:67,
@@ -611,7 +611,7 @@ In file included from /usr/include/c++/9/vector:67,
  1912 |     operator!=(const vector<_Tp, _Alloc>& __x, const vector<_Tp, _Alloc>& __y)
       |     ^~~~~~~~
 /usr/include/c++/9/bits/stl_vector.h:1912:5: note:   template argument deduction/substitution failed:
-/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<ORB_SLAM3::Map>’ is not derived from ‘const std::vector<_Tp, _Alloc>’
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<MORB_SLAM::Map>’ is not derived from ‘const std::vector<_Tp, _Alloc>’
   339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
       |                                                 ^
 In file included from /usr/include/c++/9/functional:59,
@@ -628,7 +628,7 @@ In file included from /usr/include/c++/9/functional:59,
   764 |     operator!=(const function<_Res(_Args...)>& __f, nullptr_t) noexcept
       |     ^~~~~~~~
 /usr/include/c++/9/bits/std_function.h:764:5: note:   template argument deduction/substitution failed:
-/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<ORB_SLAM3::Map>’ is not derived from ‘const std::function<_Res(_ArgTypes ...)>’
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<MORB_SLAM::Map>’ is not derived from ‘const std::function<_Res(_ArgTypes ...)>’
   339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
       |                                                 ^
 In file included from /usr/include/c++/9/functional:59,
@@ -645,7 +645,7 @@ In file included from /usr/include/c++/9/functional:59,
   770 |     operator!=(nullptr_t, const function<_Res(_Args...)>& __f) noexcept
       |     ^~~~~~~~
 /usr/include/c++/9/bits/std_function.h:770:5: note:   template argument deduction/substitution failed:
-/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   mismatched types ‘const std::function<_Res(_ArgTypes ...)>’ and ‘std::__shared_ptr<ORB_SLAM3::Map, __gnu_cxx::_S_atomic>::element_type*’ {aka ‘ORB_SLAM3::Map*’}
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   mismatched types ‘const std::function<_Res(_ArgTypes ...)>’ and ‘std::__shared_ptr<MORB_SLAM::Map, __gnu_cxx::_S_atomic>::element_type*’ {aka ‘MORB_SLAM::Map*’}
   339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
       |                                                 ^
 In file included from /usr/include/c++/9/unordered_map:47,
@@ -663,7 +663,7 @@ In file included from /usr/include/c++/9/unordered_map:47,
  2099 |     operator!=(const unordered_map<_Key, _Tp, _Hash, _Pred, _Alloc>& __x,
       |     ^~~~~~~~
 /usr/include/c++/9/bits/unordered_map.h:2099:5: note:   template argument deduction/substitution failed:
-/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<ORB_SLAM3::Map>’ is not derived from ‘const std::unordered_map<_Key1, _Tp1, _Hash1, _Pred1, _Alloc1>’
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<MORB_SLAM::Map>’ is not derived from ‘const std::unordered_map<_Key1, _Tp1, _Hash1, _Pred1, _Alloc1>’
   339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
       |                                                 ^
 In file included from /usr/include/c++/9/unordered_map:47,
@@ -681,7 +681,7 @@ In file included from /usr/include/c++/9/unordered_map:47,
  2111 |     operator!=(const unordered_multimap<_Key, _Tp, _Hash, _Pred, _Alloc>& __x,
       |     ^~~~~~~~
 /usr/include/c++/9/bits/unordered_map.h:2111:5: note:   template argument deduction/substitution failed:
-/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<ORB_SLAM3::Map>’ is not derived from ‘const std::unordered_multimap<_Key1, _Tp1, _Hash1, _Pred1, _Alloc1>’
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<MORB_SLAM::Map>’ is not derived from ‘const std::unordered_multimap<_Key1, _Tp1, _Hash1, _Pred1, _Alloc1>’
   339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
       |                                                 ^
 In file included from /usr/include/c++/9/bits/ios_base.h:46,
@@ -699,25 +699,25 @@ In file included from /usr/include/c++/9/bits/ios_base.h:46,
 /usr/include/c++/9/system_error:319:3: note: candidate: ‘bool std::operator!=(const std::error_code&, const std::error_code&)’
   319 |   operator!=(const error_code& __lhs, const error_code& __rhs) noexcept
       |   ^~~~~~~~
-/usr/include/c++/9/system_error:319:32: note:   no known conversion for argument 1 from ‘std::shared_ptr<ORB_SLAM3::Map>’ to ‘const std::error_code&’
+/usr/include/c++/9/system_error:319:32: note:   no known conversion for argument 1 from ‘std::shared_ptr<MORB_SLAM::Map>’ to ‘const std::error_code&’
   319 |   operator!=(const error_code& __lhs, const error_code& __rhs) noexcept
       |              ~~~~~~~~~~~~~~~~~~^~~~~
 /usr/include/c++/9/system_error:323:3: note: candidate: ‘bool std::operator!=(const std::error_code&, const std::error_condition&)’
   323 |   operator!=(const error_code& __lhs, const error_condition& __rhs) noexcept
       |   ^~~~~~~~
-/usr/include/c++/9/system_error:323:32: note:   no known conversion for argument 1 from ‘std::shared_ptr<ORB_SLAM3::Map>’ to ‘const std::error_code&’
+/usr/include/c++/9/system_error:323:32: note:   no known conversion for argument 1 from ‘std::shared_ptr<MORB_SLAM::Map>’ to ‘const std::error_code&’
   323 |   operator!=(const error_code& __lhs, const error_condition& __rhs) noexcept
       |              ~~~~~~~~~~~~~~~~~~^~~~~
 /usr/include/c++/9/system_error:327:3: note: candidate: ‘bool std::operator!=(const std::error_condition&, const std::error_code&)’
   327 |   operator!=(const error_condition& __lhs, const error_code& __rhs) noexcept
       |   ^~~~~~~~
-/usr/include/c++/9/system_error:327:37: note:   no known conversion for argument 1 from ‘std::shared_ptr<ORB_SLAM3::Map>’ to ‘const std::error_condition&’
+/usr/include/c++/9/system_error:327:37: note:   no known conversion for argument 1 from ‘std::shared_ptr<MORB_SLAM::Map>’ to ‘const std::error_condition&’
   327 |   operator!=(const error_condition& __lhs, const error_code& __rhs) noexcept
       |              ~~~~~~~~~~~~~~~~~~~~~~~^~~~~
 /usr/include/c++/9/system_error:331:3: note: candidate: ‘bool std::operator!=(const std::error_condition&, const std::error_condition&)’
   331 |   operator!=(const error_condition& __lhs,
       |   ^~~~~~~~
-/usr/include/c++/9/system_error:331:37: note:   no known conversion for argument 1 from ‘std::shared_ptr<ORB_SLAM3::Map>’ to ‘const std::error_condition&’
+/usr/include/c++/9/system_error:331:37: note:   no known conversion for argument 1 from ‘std::shared_ptr<MORB_SLAM::Map>’ to ‘const std::error_condition&’
   331 |   operator!=(const error_condition& __lhs,
       |              ~~~~~~~~~~~~~~~~~~~~~~~^~~~~
 In file included from /usr/include/c++/9/bits/locale_facets.h:48,
@@ -737,7 +737,7 @@ In file included from /usr/include/c++/9/bits/locale_facets.h:48,
   214 |     operator!=(const istreambuf_iterator<_CharT, _Traits>& __a,
       |     ^~~~~~~~
 /usr/include/c++/9/bits/streambuf_iterator.h:214:5: note:   template argument deduction/substitution failed:
-/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<ORB_SLAM3::Map>’ is not derived from ‘const std::istreambuf_iterator<_CharT, _Traits>’
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<MORB_SLAM::Map>’ is not derived from ‘const std::istreambuf_iterator<_CharT, _Traits>’
   339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
       |                                                 ^
 In file included from /usr/include/c++/9/iterator:66,
@@ -753,7 +753,7 @@ In file included from /usr/include/c++/9/iterator:66,
   141 |     operator!=(const istream_iterator<_Tp, _CharT, _Traits, _Dist>& __x,
       |     ^~~~~~~~
 /usr/include/c++/9/bits/stream_iterator.h:141:5: note:   template argument deduction/substitution failed:
-/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<ORB_SLAM3::Map>’ is not derived from ‘const std::istream_iterator<_Tp, _CharT, _Traits, _Dist>’
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<MORB_SLAM::Map>’ is not derived from ‘const std::istream_iterator<_Tp, _CharT, _Traits, _Dist>’
   339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
       |                                                 ^
 In file included from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/so2.hpp:7,
@@ -768,7 +768,7 @@ In file included from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/so2.hpp:7,
   481 |     operator!=(const complex<_Tp>& __x, const complex<_Tp>& __y)
       |     ^~~~~~~~
 /usr/include/c++/9/complex:481:5: note:   template argument deduction/substitution failed:
-/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<ORB_SLAM3::Map>’ is not derived from ‘const std::complex<_Tp>’
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<MORB_SLAM::Map>’ is not derived from ‘const std::complex<_Tp>’
   339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
       |                                                 ^
 In file included from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/so2.hpp:7,
@@ -783,7 +783,7 @@ In file included from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/so2.hpp:7,
   486 |     operator!=(const complex<_Tp>& __x, const _Tp& __y)
       |     ^~~~~~~~
 /usr/include/c++/9/complex:486:5: note:   template argument deduction/substitution failed:
-/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<ORB_SLAM3::Map>’ is not derived from ‘const std::complex<_Tp>’
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<MORB_SLAM::Map>’ is not derived from ‘const std::complex<_Tp>’
   339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
       |                                                 ^
 In file included from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/so2.hpp:7,
@@ -798,7 +798,7 @@ In file included from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/so2.hpp:7,
   491 |     operator!=(const _Tp& __x, const complex<_Tp>& __y)
       |     ^~~~~~~~
 /usr/include/c++/9/complex:491:5: note:   template argument deduction/substitution failed:
-/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   mismatched types ‘const std::complex<_Tp>’ and ‘std::__shared_ptr<ORB_SLAM3::Map, __gnu_cxx::_S_atomic>::element_type*’ {aka ‘ORB_SLAM3::Map*’}
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   mismatched types ‘const std::complex<_Tp>’ and ‘std::__shared_ptr<MORB_SLAM::Map, __gnu_cxx::_S_atomic>::element_type*’ {aka ‘MORB_SLAM::Map*’}
   339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
       |                                                 ^
 In file included from /usr/include/c++/9/random:49,
@@ -817,7 +817,7 @@ In file included from /usr/include/c++/9/random:49,
   421 |     operator!=(const std::linear_congruential_engine<_UIntType, __a,
       |     ^~~~~~~~
 /usr/include/c++/9/bits/random.h:421:5: note:   template argument deduction/substitution failed:
-/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<ORB_SLAM3::Map>’ is not derived from ‘const std::linear_congruential_engine<_UIntType, __a, __c, __m>’
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<MORB_SLAM::Map>’ is not derived from ‘const std::linear_congruential_engine<_UIntType, __a, __c, __m>’
   339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
       |                                                 ^
 In file included from /usr/include/c++/9/random:49,
@@ -836,7 +836,7 @@ In file included from /usr/include/c++/9/random:49,
   658 |     operator!=(const std::mersenne_twister_engine<_UIntType, __w, __n, __m,
       |     ^~~~~~~~
 /usr/include/c++/9/bits/random.h:658:5: note:   template argument deduction/substitution failed:
-/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<ORB_SLAM3::Map>’ is not derived from ‘const std::mersenne_twister_engine<_UIntType, __w, __n, __m, __r, __a, __u, __d, __s, __b, __t, __c, __l, __f>’
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<MORB_SLAM::Map>’ is not derived from ‘const std::mersenne_twister_engine<_UIntType, __w, __n, __m, __r, __a, __u, __d, __s, __b, __t, __c, __l, __f>’
   339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
       |                                                 ^
 In file included from /usr/include/c++/9/random:49,
@@ -855,7 +855,7 @@ In file included from /usr/include/c++/9/random:49,
   859 |     operator!=(const std::subtract_with_carry_engine<_UIntType, __w,
       |     ^~~~~~~~
 /usr/include/c++/9/bits/random.h:859:5: note:   template argument deduction/substitution failed:
-/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<ORB_SLAM3::Map>’ is not derived from ‘const std::subtract_with_carry_engine<_UIntType, __w, __s, __r>’
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<MORB_SLAM::Map>’ is not derived from ‘const std::subtract_with_carry_engine<_UIntType, __w, __s, __r>’
   339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
       |                                                 ^
 In file included from /usr/include/c++/9/random:49,
@@ -874,7 +874,7 @@ In file included from /usr/include/c++/9/random:49,
  1082 |     operator!=(const std::discard_block_engine<_RandomNumberEngine, __p,
       |     ^~~~~~~~
 /usr/include/c++/9/bits/random.h:1082:5: note:   template argument deduction/substitution failed:
-/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<ORB_SLAM3::Map>’ is not derived from ‘const std::discard_block_engine<_RandomNumberEngine, __p, __r>’
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<MORB_SLAM::Map>’ is not derived from ‘const std::discard_block_engine<_RandomNumberEngine, __p, __r>’
   339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
       |                                                 ^
 In file included from /usr/include/c++/9/random:49,
@@ -893,7 +893,7 @@ In file included from /usr/include/c++/9/random:49,
  1279 |     operator!=(const std::independent_bits_engine<_RandomNumberEngine, __w,
       |     ^~~~~~~~
 /usr/include/c++/9/bits/random.h:1279:5: note:   template argument deduction/substitution failed:
-/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<ORB_SLAM3::Map>’ is not derived from ‘const std::independent_bits_engine<_RandomNumberEngine, __w, _UIntType>’
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<MORB_SLAM::Map>’ is not derived from ‘const std::independent_bits_engine<_RandomNumberEngine, __w, _UIntType>’
   339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
       |                                                 ^
 In file included from /usr/include/c++/9/random:49,
@@ -912,7 +912,7 @@ In file included from /usr/include/c++/9/random:49,
  1532 |     operator!=(const std::shuffle_order_engine<_RandomNumberEngine,
       |     ^~~~~~~~
 /usr/include/c++/9/bits/random.h:1532:5: note:   template argument deduction/substitution failed:
-/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<ORB_SLAM3::Map>’ is not derived from ‘const std::shuffle_order_engine<_RandomNumberEngine, __k>’
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<MORB_SLAM::Map>’ is not derived from ‘const std::shuffle_order_engine<_RandomNumberEngine, __k>’
   339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
       |                                                 ^
 In file included from /usr/include/c++/9/random:49,
@@ -931,7 +931,7 @@ In file included from /usr/include/c++/9/random:49,
  1692 |     operator!=(const std::uniform_int_distribution<_IntType>& __d1,
       |     ^~~~~~~~
 /usr/include/c++/9/bits/random.h:1692:5: note:   template argument deduction/substitution failed:
-/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<ORB_SLAM3::Map>’ is not derived from ‘const std::uniform_int_distribution<_IntType>’
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<MORB_SLAM::Map>’ is not derived from ‘const std::uniform_int_distribution<_IntType>’
   339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
       |                                                 ^
 In file included from /usr/include/c++/9/random:49,
@@ -950,7 +950,7 @@ In file included from /usr/include/c++/9/random:49,
  1913 |     operator!=(const std::uniform_real_distribution<_IntType>& __d1,
       |     ^~~~~~~~
 /usr/include/c++/9/bits/random.h:1913:5: note:   template argument deduction/substitution failed:
-/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<ORB_SLAM3::Map>’ is not derived from ‘const std::uniform_real_distribution<_IntType>’
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<MORB_SLAM::Map>’ is not derived from ‘const std::uniform_real_distribution<_IntType>’
   339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
       |                                                 ^
 In file included from /usr/include/c++/9/random:49,
@@ -969,7 +969,7 @@ In file included from /usr/include/c++/9/random:49,
  2170 |     operator!=(const std::normal_distribution<_RealType>& __d1,
       |     ^~~~~~~~
 /usr/include/c++/9/bits/random.h:2170:5: note:   template argument deduction/substitution failed:
-/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<ORB_SLAM3::Map>’ is not derived from ‘const std::normal_distribution<_RealType>’
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<MORB_SLAM::Map>’ is not derived from ‘const std::normal_distribution<_RealType>’
   339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
       |                                                 ^
 In file included from /usr/include/c++/9/random:49,
@@ -988,7 +988,7 @@ In file included from /usr/include/c++/9/random:49,
  2381 |     operator!=(const std::lognormal_distribution<_RealType>& __d1,
       |     ^~~~~~~~
 /usr/include/c++/9/bits/random.h:2381:5: note:   template argument deduction/substitution failed:
-/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<ORB_SLAM3::Map>’ is not derived from ‘const std::lognormal_distribution<_RealType>’
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<MORB_SLAM::Map>’ is not derived from ‘const std::lognormal_distribution<_RealType>’
   339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
       |                                                 ^
 In file included from /usr/include/c++/9/random:49,
@@ -1007,7 +1007,7 @@ In file included from /usr/include/c++/9/random:49,
  2612 |      operator!=(const std::gamma_distribution<_RealType>& __d1,
       |      ^~~~~~~~
 /usr/include/c++/9/bits/random.h:2612:6: note:   template argument deduction/substitution failed:
-/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<ORB_SLAM3::Map>’ is not derived from ‘const std::gamma_distribution<_RealType>’
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<MORB_SLAM::Map>’ is not derived from ‘const std::gamma_distribution<_RealType>’
   339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
       |                                                 ^
 In file included from /usr/include/c++/9/random:49,
@@ -1026,7 +1026,7 @@ In file included from /usr/include/c++/9/random:49,
  2836 |     operator!=(const std::chi_squared_distribution<_RealType>& __d1,
       |     ^~~~~~~~
 /usr/include/c++/9/bits/random.h:2836:5: note:   template argument deduction/substitution failed:
-/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<ORB_SLAM3::Map>’ is not derived from ‘const std::chi_squared_distribution<_RealType>’
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<MORB_SLAM::Map>’ is not derived from ‘const std::chi_squared_distribution<_RealType>’
   339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
       |                                                 ^
 In file included from /usr/include/c++/9/random:49,
@@ -1045,7 +1045,7 @@ In file included from /usr/include/c++/9/random:49,
  3010 |     operator!=(const std::cauchy_distribution<_RealType>& __d1,
       |     ^~~~~~~~
 /usr/include/c++/9/bits/random.h:3010:5: note:   template argument deduction/substitution failed:
-/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<ORB_SLAM3::Map>’ is not derived from ‘const std::cauchy_distribution<_RealType>’
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<MORB_SLAM::Map>’ is not derived from ‘const std::cauchy_distribution<_RealType>’
   339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
       |                                                 ^
 In file included from /usr/include/c++/9/random:49,
@@ -1064,7 +1064,7 @@ In file included from /usr/include/c++/9/random:49,
  3274 |     operator!=(const std::fisher_f_distribution<_RealType>& __d1,
       |     ^~~~~~~~
 /usr/include/c++/9/bits/random.h:3274:5: note:   template argument deduction/substitution failed:
-/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<ORB_SLAM3::Map>’ is not derived from ‘const std::fisher_f_distribution<_RealType>’
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<MORB_SLAM::Map>’ is not derived from ‘const std::fisher_f_distribution<_RealType>’
   339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
       |                                                 ^
 In file included from /usr/include/c++/9/random:49,
@@ -1083,7 +1083,7 @@ In file included from /usr/include/c++/9/random:49,
  3496 |     operator!=(const std::student_t_distribution<_RealType>& __d1,
       |     ^~~~~~~~
 /usr/include/c++/9/bits/random.h:3496:5: note:   template argument deduction/substitution failed:
-/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<ORB_SLAM3::Map>’ is not derived from ‘const std::student_t_distribution<_RealType>’
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<MORB_SLAM::Map>’ is not derived from ‘const std::student_t_distribution<_RealType>’
   339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
       |                                                 ^
 In file included from /usr/include/c++/9/random:49,
@@ -1101,14 +1101,14 @@ In file included from /usr/include/c++/9/random:49,
 /usr/include/c++/9/bits/random.h:3684:3: note: candidate: ‘bool std::operator!=(const std::bernoulli_distribution&, const std::bernoulli_distribution&)’
  3684 |   operator!=(const std::bernoulli_distribution& __d1,
       |   ^~~~~~~~
-/usr/include/c++/9/bits/random.h:3684:49: note:   no known conversion for argument 1 from ‘std::shared_ptr<ORB_SLAM3::Map>’ to ‘const std::bernoulli_distribution&’
+/usr/include/c++/9/bits/random.h:3684:49: note:   no known conversion for argument 1 from ‘std::shared_ptr<MORB_SLAM::Map>’ to ‘const std::bernoulli_distribution&’
  3684 |   operator!=(const std::bernoulli_distribution& __d1,
       |              ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~^~~~
 /usr/include/c++/9/bits/random.h:3959:5: note: candidate: ‘template<class _IntType> bool std::operator!=(const std::binomial_distribution<_IntType>&, const std::binomial_distribution<_IntType>&)’
  3959 |     operator!=(const std::binomial_distribution<_IntType>& __d1,
       |     ^~~~~~~~
 /usr/include/c++/9/bits/random.h:3959:5: note:   template argument deduction/substitution failed:
-/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<ORB_SLAM3::Map>’ is not derived from ‘const std::binomial_distribution<_IntType>’
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<MORB_SLAM::Map>’ is not derived from ‘const std::binomial_distribution<_IntType>’
   339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
       |                                                 ^
 In file included from /usr/include/c++/9/random:49,
@@ -1127,7 +1127,7 @@ In file included from /usr/include/c++/9/random:49,
  4138 |     operator!=(const std::geometric_distribution<_IntType>& __d1,
       |     ^~~~~~~~
 /usr/include/c++/9/bits/random.h:4138:5: note:   template argument deduction/substitution failed:
-/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<ORB_SLAM3::Map>’ is not derived from ‘const std::geometric_distribution<_IntType>’
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<MORB_SLAM::Map>’ is not derived from ‘const std::geometric_distribution<_IntType>’
   339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
       |                                                 ^
 In file included from /usr/include/c++/9/random:49,
@@ -1146,7 +1146,7 @@ In file included from /usr/include/c++/9/random:49,
  4392 |     operator!=(const std::negative_binomial_distribution<_IntType>& __d1,
       |     ^~~~~~~~
 /usr/include/c++/9/bits/random.h:4392:5: note:   template argument deduction/substitution failed:
-/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<ORB_SLAM3::Map>’ is not derived from ‘const std::negative_binomial_distribution<_IntType>’
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<MORB_SLAM::Map>’ is not derived from ‘const std::negative_binomial_distribution<_IntType>’
   339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
       |                                                 ^
 In file included from /usr/include/c++/9/random:49,
@@ -1165,7 +1165,7 @@ In file included from /usr/include/c++/9/random:49,
  4618 |     operator!=(const std::poisson_distribution<_IntType>& __d1,
       |     ^~~~~~~~
 /usr/include/c++/9/bits/random.h:4618:5: note:   template argument deduction/substitution failed:
-/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<ORB_SLAM3::Map>’ is not derived from ‘const std::poisson_distribution<_IntType>’
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<MORB_SLAM::Map>’ is not derived from ‘const std::poisson_distribution<_IntType>’
   339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
       |                                                 ^
 In file included from /usr/include/c++/9/random:49,
@@ -1184,7 +1184,7 @@ In file included from /usr/include/c++/9/random:49,
  4809 |     operator!=(const std::exponential_distribution<_RealType>& __d1,
       |     ^~~~~~~~
 /usr/include/c++/9/bits/random.h:4809:5: note:   template argument deduction/substitution failed:
-/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<ORB_SLAM3::Map>’ is not derived from ‘const std::exponential_distribution<_RealType>’
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<MORB_SLAM::Map>’ is not derived from ‘const std::exponential_distribution<_RealType>’
   339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
       |                                                 ^
 In file included from /usr/include/c++/9/random:49,
@@ -1203,7 +1203,7 @@ In file included from /usr/include/c++/9/random:49,
  5019 |     operator!=(const std::weibull_distribution<_RealType>& __d1,
       |     ^~~~~~~~
 /usr/include/c++/9/bits/random.h:5019:5: note:   template argument deduction/substitution failed:
-/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<ORB_SLAM3::Map>’ is not derived from ‘const std::weibull_distribution<_RealType>’
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<MORB_SLAM::Map>’ is not derived from ‘const std::weibull_distribution<_RealType>’
   339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
       |                                                 ^
 In file included from /usr/include/c++/9/random:49,
@@ -1222,7 +1222,7 @@ In file included from /usr/include/c++/9/random:49,
  5229 |     operator!=(const std::extreme_value_distribution<_RealType>& __d1,
       |     ^~~~~~~~
 /usr/include/c++/9/bits/random.h:5229:5: note:   template argument deduction/substitution failed:
-/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<ORB_SLAM3::Map>’ is not derived from ‘const std::extreme_value_distribution<_RealType>’
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<MORB_SLAM::Map>’ is not derived from ‘const std::extreme_value_distribution<_RealType>’
   339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
       |                                                 ^
 In file included from /usr/include/c++/9/random:49,
@@ -1241,7 +1241,7 @@ In file included from /usr/include/c++/9/random:49,
  5494 |     operator!=(const std::discrete_distribution<_IntType>& __d1,
       |     ^~~~~~~~
 /usr/include/c++/9/bits/random.h:5494:5: note:   template argument deduction/substitution failed:
-/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<ORB_SLAM3::Map>’ is not derived from ‘const std::discrete_distribution<_IntType>’
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<MORB_SLAM::Map>’ is not derived from ‘const std::discrete_distribution<_IntType>’
   339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
       |                                                 ^
 In file included from /usr/include/c++/9/random:49,
@@ -1260,7 +1260,7 @@ In file included from /usr/include/c++/9/random:49,
  5765 |     operator!=(const std::piecewise_constant_distribution<_RealType>& __d1,
       |     ^~~~~~~~
 /usr/include/c++/9/bits/random.h:5765:5: note:   template argument deduction/substitution failed:
-/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<ORB_SLAM3::Map>’ is not derived from ‘const std::piecewise_constant_distribution<_RealType>’
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<MORB_SLAM::Map>’ is not derived from ‘const std::piecewise_constant_distribution<_RealType>’
   339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
       |                                                 ^
 In file included from /usr/include/c++/9/random:49,
@@ -1279,7 +1279,7 @@ In file included from /usr/include/c++/9/random:49,
  6038 |     operator!=(const std::piecewise_linear_distribution<_RealType>& __d1,
       |     ^~~~~~~~
 /usr/include/c++/9/bits/random.h:6038:5: note:   template argument deduction/substitution failed:
-/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<ORB_SLAM3::Map>’ is not derived from ‘const std::piecewise_linear_distribution<_RealType>’
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<MORB_SLAM::Map>’ is not derived from ‘const std::piecewise_linear_distribution<_RealType>’
   339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
       |                                                 ^
 In file included from /usr/include/c++/9/set:61,
@@ -1296,7 +1296,7 @@ In file included from /usr/include/c++/9/set:61,
  1003 |     operator!=(const set<_Key, _Compare, _Alloc>& __x,
       |     ^~~~~~~~
 /usr/include/c++/9/bits/stl_set.h:1003:5: note:   template argument deduction/substitution failed:
-/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<ORB_SLAM3::Map>’ is not derived from ‘const std::set<_Key, _Compare, _Allocator>’
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<MORB_SLAM::Map>’ is not derived from ‘const std::set<_Key, _Compare, _Allocator>’
   339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
       |                                                 ^
 In file included from /usr/include/c++/9/set:62,
@@ -1313,7 +1313,7 @@ In file included from /usr/include/c++/9/set:62,
   988 |     operator!=(const multiset<_Key, _Compare, _Alloc>& __x,
       |     ^~~~~~~~
 /usr/include/c++/9/bits/stl_multiset.h:988:5: note:   template argument deduction/substitution failed:
-/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<ORB_SLAM3::Map>’ is not derived from ‘const std::multiset<_Key, _Compare, _Allocator>’
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<MORB_SLAM::Map>’ is not derived from ‘const std::multiset<_Key, _Compare, _Allocator>’
   339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
       |                                                 ^
 In file included from /usr/include/c++/9/list:63,
@@ -1330,7 +1330,7 @@ In file included from /usr/include/c++/9/list:63,
  2032 |     operator!=(const list<_Tp, _Alloc>& __x, const list<_Tp, _Alloc>& __y)
       |     ^~~~~~~~
 /usr/include/c++/9/bits/stl_list.h:2032:5: note:   template argument deduction/substitution failed:
-/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<ORB_SLAM3::Map>’ is not derived from ‘const std::__cxx11::list<_Tp, _Alloc>’
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<MORB_SLAM::Map>’ is not derived from ‘const std::__cxx11::list<_Tp, _Alloc>’
   339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
       |                                                 ^
 In file included from /usr/include/c++/9/deque:67,
@@ -1347,7 +1347,7 @@ In file included from /usr/include/c++/9/deque:67,
   299 |     operator!=(const _Deque_iterator<_Tp, _Ref, _Ptr>& __x,
       |     ^~~~~~~~
 /usr/include/c++/9/bits/stl_deque.h:299:5: note:   template argument deduction/substitution failed:
-/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<ORB_SLAM3::Map>’ is not derived from ‘const std::_Deque_iterator<_Tp, _Ref, _Ptr>’
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<MORB_SLAM::Map>’ is not derived from ‘const std::_Deque_iterator<_Tp, _Ref, _Ptr>’
   339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
       |                                                 ^
 In file included from /usr/include/c++/9/deque:67,
@@ -1364,7 +1364,7 @@ In file included from /usr/include/c++/9/deque:67,
   306 |     operator!=(const _Deque_iterator<_Tp, _RefL, _PtrL>& __x,
       |     ^~~~~~~~
 /usr/include/c++/9/bits/stl_deque.h:306:5: note:   template argument deduction/substitution failed:
-/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<ORB_SLAM3::Map>’ is not derived from ‘const std::_Deque_iterator<_Tp, _Ref, _Ptr>’
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<MORB_SLAM::Map>’ is not derived from ‘const std::_Deque_iterator<_Tp, _Ref, _Ptr>’
   339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
       |                                                 ^
 In file included from /usr/include/c++/9/deque:67,
@@ -1381,7 +1381,7 @@ In file included from /usr/include/c++/9/deque:67,
  2338 |     operator!=(const deque<_Tp, _Alloc>& __x,
       |     ^~~~~~~~
 /usr/include/c++/9/bits/stl_deque.h:2338:5: note:   template argument deduction/substitution failed:
-/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<ORB_SLAM3::Map>’ is not derived from ‘const std::deque<_Tp, _Alloc>’
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<MORB_SLAM::Map>’ is not derived from ‘const std::deque<_Tp, _Alloc>’
   339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
       |                                                 ^
 In file included from /usr/include/c++/9/stack:61,
@@ -1397,7 +1397,7 @@ In file included from /usr/include/c++/9/stack:61,
   337 |     operator!=(const stack<_Tp, _Seq>& __x, const stack<_Tp, _Seq>& __y)
       |     ^~~~~~~~
 /usr/include/c++/9/bits/stl_stack.h:337:5: note:   template argument deduction/substitution failed:
-/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<ORB_SLAM3::Map>’ is not derived from ‘const std::stack<_Tp, _Seq>’
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<MORB_SLAM::Map>’ is not derived from ‘const std::stack<_Tp, _Seq>’
   339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
       |                                                 ^
 In file included from /usr/include/c++/9/queue:64,
@@ -1415,7 +1415,7 @@ In file included from /usr/include/c++/9/queue:64,
   362 |     operator!=(const queue<_Tp, _Seq>& __x, const queue<_Tp, _Seq>& __y)
       |     ^~~~~~~~
 /usr/include/c++/9/bits/stl_queue.h:362:5: note:   template argument deduction/substitution failed:
-/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<ORB_SLAM3::Map>’ is not derived from ‘const std::queue<_Tp, _Seq>’
+/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note:   ‘std::shared_ptr<MORB_SLAM::Map>’ is not derived from ‘const std::queue<_Tp, _Seq>’
   339 |       if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) {
       |                                                 ^
 In file included from /orin_ssd/MORB_SLAM/include/Camera.hpp:2,
@@ -1427,7 +1427,7 @@ In file included from /orin_ssd/MORB_SLAM/include/Camera.hpp:2,
 /usr/include/c++/9/thread:286:3: note: candidate: ‘bool std::operator!=(std::thread::id, std::thread::id)’
   286 |   operator!=(thread::id __x, thread::id __y) noexcept
       |   ^~~~~~~~
-/usr/include/c++/9/thread:286:25: note:   no known conversion for argument 1 from ‘std::shared_ptr<ORB_SLAM3::Map>’ to ‘std::thread::id’
+/usr/include/c++/9/thread:286:25: note:   no known conversion for argument 1 from ‘std::shared_ptr<MORB_SLAM::Map>’ to ‘std::thread::id’
   286 |   operator!=(thread::id __x, thread::id __y) noexcept
       |              ~~~~~~~~~~~^~~
 make[2]: *** [CMakeFiles/MORB_SLAM.dir/build.make:244: CMakeFiles/MORB_SLAM.dir/src/Map.cc.o] Error 1
diff --git a/src/Atlas.cc b/src/Atlas.cc
index 4313a60a..acbf0df4 100644
--- a/src/Atlas.cc
+++ b/src/Atlas.cc
@@ -19,13 +19,13 @@
  * ORB-SLAM3. If not, see <http://www.gnu.org/licenses/>.
  */
 
-#include "Atlas.h"
+#include "MORB_SLAM/Atlas.h"
 
-#include "GeometricCamera.h"
-#include "KannalaBrandt8.h"
-#include "Pinhole.h"
+#include "MORB_SLAM/CameraModels/GeometricCamera.h"
+#include "MORB_SLAM/CameraModels/KannalaBrandt8.h"
+#include "MORB_SLAM/CameraModels/Pinhole.h"
 
-namespace ORB_SLAM3 {
+namespace MORB_SLAM {
 
 Atlas::Atlas() { mpCurrentMap = nullptr; }
 
@@ -325,4 +325,4 @@ map<long unsigned int, KeyFrame*> Atlas::GetAtlasKeyframes() {
   return mpIdKFs;
 }
 
-}  // namespace ORB_SLAM3
+}  // namespace MORB_SLAM
diff --git a/src/Camera.cpp b/src/Camera.cpp
index 508a4af8..42c2de0b 100644
--- a/src/Camera.cpp
+++ b/src/Camera.cpp
@@ -1,6 +1,6 @@
-#include "Camera.hpp"
+#include "MORB_SLAM/Camera.hpp"
 
-namespace ORB_SLAM3{
+namespace MORB_SLAM{
 
 Camera::Camera(CameraType::eSensor type, const std::string &name): ljobs{}, rjobs{}, name{name}, type{type},
     shouldStop{false}, lthread{&Camera::threadExec, this, &ljobs}, rthread{&Camera::threadExec, this, &rjobs} {
@@ -66,4 +66,4 @@ ManagedFuture<bool> Camera::queueRight(const std::function<void(void)> &func){ r
 CameraType::eSensor Camera::getType() const { return type; }
 const std::string &Camera::getName() const { return name; }
 
-} // namespace ORB_SLAM3
\ No newline at end of file
+} // namespace MORB_SLAM
\ No newline at end of file
diff --git a/src/CameraModels/KannalaBrandt8.cpp b/src/CameraModels/KannalaBrandt8.cpp
index 4952082e..71de141c 100644
--- a/src/CameraModels/KannalaBrandt8.cpp
+++ b/src/CameraModels/KannalaBrandt8.cpp
@@ -19,13 +19,13 @@
  * ORB-SLAM3. If not, see <http://www.gnu.org/licenses/>.
  */
 
-#include "KannalaBrandt8.h"
+#include "MORB_SLAM/CameraModels/KannalaBrandt8.h"
 
 #include <boost/serialization/export.hpp>
 
-// BOOST_CLASS_EXPORT_IMPLEMENT(ORB_SLAM3::KannalaBrandt8)
+// BOOST_CLASS_EXPORT_IMPLEMENT(MORB_SLAM::KannalaBrandt8)
 
-namespace ORB_SLAM3 {
+namespace MORB_SLAM {
 // BOOST_CLASS_EXPORT_GUID(KannalaBrandt8, "KannalaBrandt8")
 
 cv::Point2f KannalaBrandt8::project(const cv::Point3f &p3D) const {
@@ -446,4 +446,4 @@ bool KannalaBrandt8::IsEqual(const std::shared_ptr<GeometricCamera> &pCam) {
   return is_same_camera;
 }
 
-}  // namespace ORB_SLAM3
+}  // namespace MORB_SLAM
diff --git a/src/CameraModels/Pinhole.cpp b/src/CameraModels/Pinhole.cpp
index e83e1948..6df3f00f 100644
--- a/src/CameraModels/Pinhole.cpp
+++ b/src/CameraModels/Pinhole.cpp
@@ -19,13 +19,13 @@
  * ORB-SLAM3. If not, see <http://www.gnu.org/licenses/>.
  */
 
-#include "Pinhole.h"
+#include "MORB_SLAM/CameraModels/Pinhole.h"
 
 #include <boost/serialization/export.hpp>
 
-// BOOST_CLASS_EXPORT_IMPLEMENT(ORB_SLAM3::Pinhole)
+// BOOST_CLASS_EXPORT_IMPLEMENT(MORB_SLAM::Pinhole)
 
-namespace ORB_SLAM3 {
+namespace MORB_SLAM {
 // BOOST_CLASS_EXPORT_GUID(Pinhole, "Pinhole")
 
 long unsigned int GeometricCamera::nNextId = 0;
@@ -170,4 +170,4 @@ bool Pinhole::IsEqual(const std::shared_ptr<GeometricCamera> &pCam) {
   }
   return is_same_camera;
 }
-}  // namespace ORB_SLAM3
+}  // namespace MORB_SLAM
diff --git a/src/Converter.cc b/src/Converter.cc
index c842918d..e2c3ab8d 100644
--- a/src/Converter.cc
+++ b/src/Converter.cc
@@ -19,9 +19,9 @@
  * ORB-SLAM3. If not, see <http://www.gnu.org/licenses/>.
  */
 
-#include "Converter.h"
+#include "MORB_SLAM/Converter.h"
 
-namespace ORB_SLAM3 {
+namespace MORB_SLAM {
 
 std::vector<cv::Mat> Converter::toDescriptorVector(const cv::Mat &Descriptors) {
   std::vector<cv::Mat> vDesc;
@@ -282,4 +282,4 @@ Sophus::Sim3f Converter::toSophus(const g2o::Sim3 &S) {
       S.translation().cast<float>());
 }
 
-}  // namespace ORB_SLAM3
+}  // namespace MORB_SLAM
diff --git a/src/Frame.cc b/src/Frame.cc
index 9c61ad9e..2f84cc1c 100644
--- a/src/Frame.cc
+++ b/src/Frame.cc
@@ -19,22 +19,22 @@
  * ORB-SLAM3. If not, see <http://www.gnu.org/licenses/>.
  */
 
-#include "Frame.h"
+#include "MORB_SLAM/Frame.h"
 
-#include <KannalaBrandt8.h>
-#include <Pinhole.h>
+#include <MORB_SLAM/CameraModels/KannalaBrandt8.h>
+#include <MORB_SLAM/CameraModels/Pinhole.h>
 
 #include <thread>
 
-#include "Converter.h"
-#include "G2oTypes.h"
-#include "GeometricCamera.h"
-#include "KeyFrame.h"
-#include "MapPoint.h"
-#include "ORBextractor.h"
-#include "ORBmatcher.h"
+#include "MORB_SLAM/Converter.h"
+#include "MORB_SLAM/G2oTypes.h"
+#include "MORB_SLAM/CameraModels/GeometricCamera.h"
+#include "MORB_SLAM/KeyFrame.h"
+#include "MORB_SLAM/MapPoint.h"
+#include "MORB_SLAM/ORBextractor.h"
+#include "MORB_SLAM/ORBmatcher.h"
 
-namespace ORB_SLAM3 {
+namespace MORB_SLAM {
 
 long unsigned int Frame::nNextId = 0;
 bool Frame::mbInitialComputations = true;
@@ -1364,4 +1364,4 @@ Eigen::Vector3f Frame::UnprojectStereoFishEye(const int &i) {
   return mRwc * mvStereo3Dpoints[i] + mOw;
 }
 
-}  // namespace ORB_SLAM3
+}  // namespace MORB_SLAM
diff --git a/src/FrameDrawer.cc b/src/FrameDrawer.cc
index 9a75cbdd..bd83c689 100644
--- a/src/FrameDrawer.cc
+++ b/src/FrameDrawer.cc
@@ -19,16 +19,16 @@
  * ORB-SLAM3. If not, see <http://www.gnu.org/licenses/>.
  */
 
-#include "FrameDrawer.h"
+#include "MORB_SLAM/FrameDrawer.h"
 
 #include <mutex>
 #include <opencv2/opencv.hpp>
-#include "ImprovedTypes.hpp"
-#include "MapPoint.h"
-#include "Atlas.h"
-#include "Tracking.h"
+#include "MORB_SLAM/ImprovedTypes.hpp"
+#include "MORB_SLAM/MapPoint.h"
+#include "MORB_SLAM/Atlas.h"
+#include "MORB_SLAM/Tracking.h"
 
-namespace ORB_SLAM3 {
+namespace MORB_SLAM {
 
 FrameDrawer::FrameDrawer(const Atlas_ptr &pAtlas) : both(false), mpAtlas(pAtlas) {
   mState = Tracker::SYSTEM_NOT_READY;
@@ -336,4 +336,4 @@ void FrameDrawer::Update(const Tracking_ptr &pTracker) {
   mState = static_cast<int>(pTracker->mLastProcessedState);
 }
 
-}  // namespace ORB_SLAM3
+}  // namespace MORB_SLAM
diff --git a/src/G2oTypes.cc b/src/G2oTypes.cc
index ba1d26ef..00371816 100644
--- a/src/G2oTypes.cc
+++ b/src/G2oTypes.cc
@@ -19,11 +19,11 @@
  * ORB-SLAM3. If not, see <http://www.gnu.org/licenses/>.
  */
 
-#include "G2oTypes.h"
+#include "MORB_SLAM/G2oTypes.h"
 
-#include "Converter.h"
-#include "ImuTypes.h"
-namespace ORB_SLAM3 {
+#include "MORB_SLAM/Converter.h"
+#include "MORB_SLAM/ImuTypes.h"
+namespace MORB_SLAM {
 
 ImuCamPose::ImuCamPose(KeyFrame* pKF) : its(0) {
   // Load IMU pose
@@ -853,4 +853,4 @@ Eigen::Matrix3d Skew(const Eigen::Vector3d& w) {
   return W;
 }
 
-}  // namespace ORB_SLAM3
+}  // namespace MORB_SLAM
diff --git a/src/GeometricTools.cc b/src/GeometricTools.cc
index a97002c3..6c66991f 100644
--- a/src/GeometricTools.cc
+++ b/src/GeometricTools.cc
@@ -19,11 +19,11 @@
  * ORB-SLAM3. If not, see <http://www.gnu.org/licenses/>.
  */
 
-#include "GeometricTools.h"
+#include "MORB_SLAM/GeometricTools.h"
 
-#include "KeyFrame.h"
+#include "MORB_SLAM/KeyFrame.h"
 
-namespace ORB_SLAM3 {
+namespace MORB_SLAM {
 
 Eigen::Matrix3f GeometricTools::ComputeF12(KeyFrame *&pKF1, KeyFrame *&pKF2) {
   Sophus::SE3<float> Tc1w = pKF1->GetPose();
@@ -71,4 +71,4 @@ bool GeometricTools::Triangulate(Eigen::Vector3f &x_c1, Eigen::Vector3f &x_c2,
   return true;
 }
 
-}  // namespace ORB_SLAM3
+}  // namespace MORB_SLAM
diff --git a/src/ImuTypes.cc b/src/ImuTypes.cc
index b9b3eb12..ef0536a9 100644
--- a/src/ImuTypes.cc
+++ b/src/ImuTypes.cc
@@ -19,14 +19,14 @@
  * ORB-SLAM3. If not, see <http://www.gnu.org/licenses/>.
  */
 
-#include "ImuTypes.h"
+#include "MORB_SLAM/ImuTypes.h"
 
 #include <iostream>
 
-#include "Converter.h"
-#include "GeometricTools.h"
+#include "MORB_SLAM/Converter.h"
+#include "MORB_SLAM/GeometricTools.h"
 
-namespace ORB_SLAM3 {
+namespace MORB_SLAM {
 
 namespace IMU {
 
@@ -406,4 +406,4 @@ Calib::Calib(const Calib &calib) {
 
 }  // namespace IMU
 
-}  // namespace ORB_SLAM3
+}  // namespace MORB_SLAM
diff --git a/src/KeyFrame.cc b/src/KeyFrame.cc
index 18b42df0..034ba7a0 100644
--- a/src/KeyFrame.cc
+++ b/src/KeyFrame.cc
@@ -19,14 +19,14 @@
  * ORB-SLAM3. If not, see <http://www.gnu.org/licenses/>.
  */
 
-#include "KeyFrame.h"
+#include "MORB_SLAM/KeyFrame.h"
 
 #include <mutex>
 
-#include "Converter.h"
-#include "ImuTypes.h"
+#include "MORB_SLAM/Converter.h"
+#include "MORB_SLAM/ImuTypes.h"
 
-namespace ORB_SLAM3 {
+namespace MORB_SLAM {
 
 long unsigned int KeyFrame::nNextId = 0;
 
@@ -1157,4 +1157,4 @@ void KeyFrame::SetKeyFrameDatabase(KeyFrameDatabase *pKFDB) {
   mpKeyFrameDB = pKFDB;
 }
 
-}  // namespace ORB_SLAM3
+}  // namespace MORB_SLAM
diff --git a/src/KeyFrameDatabase.cc b/src/KeyFrameDatabase.cc
index 9ed18483..bed20300 100644
--- a/src/KeyFrameDatabase.cc
+++ b/src/KeyFrameDatabase.cc
@@ -19,16 +19,16 @@
  * ORB-SLAM3. If not, see <http://www.gnu.org/licenses/>.
  */
 
-#include "KeyFrameDatabase.h"
+#include "MORB_SLAM/KeyFrameDatabase.h"
 
 #include <mutex>
 
 #include "DBoW2/BowVector.h"
-#include "KeyFrame.h"
+#include "MORB_SLAM/KeyFrame.h"
 
 using namespace std;
 
-namespace ORB_SLAM3 {
+namespace MORB_SLAM {
 
 KeyFrameDatabase::KeyFrameDatabase(const ORBVocabulary& voc) : mpVoc(&voc) {
   mvInvertedFile.resize(voc.size());
@@ -822,4 +822,4 @@ void KeyFrameDatabase::SetORBVocabulary(ORBVocabulary* pORBVoc) {
   mvInvertedFile.resize(mpVoc->size());
 }
 
-}  // namespace ORB_SLAM3
+}  // namespace MORB_SLAM
diff --git a/src/LocalMapping.cc b/src/LocalMapping.cc
index 29b20fd4..17e8183e 100644
--- a/src/LocalMapping.cc
+++ b/src/LocalMapping.cc
@@ -19,19 +19,19 @@
  * ORB-SLAM3. If not, see <http://www.gnu.org/licenses/>.
  */
 
-#include "LocalMapping.h"
+#include "MORB_SLAM/LocalMapping.h"
 
 #include <chrono>
 #include <mutex>
 
-#include "Converter.h"
-#include "GeometricTools.h"
-#include "LoopClosing.h"
-#include "ORBmatcher.h"
-#include "Optimizer.h"
+#include "MORB_SLAM/Converter.h"
+#include "MORB_SLAM/GeometricTools.h"
+#include "MORB_SLAM/LoopClosing.h"
+#include "MORB_SLAM/ORBmatcher.h"
+#include "MORB_SLAM/Optimizer.h"
 
 #include <math.h> 
-namespace ORB_SLAM3 {
+namespace MORB_SLAM {
 
 LocalMapping::LocalMapping(System* pSys, const Atlas_ptr &pAtlas, const float bMonocular,
                            bool bInertial, const string& _strSeqName)
@@ -1461,4 +1461,4 @@ double LocalMapping::GetCurrKFTime() {
 
 KeyFrame* LocalMapping::GetCurrKF() { return mpCurrentKeyFrame; }
 
-}  // namespace ORB_SLAM3
+}  // namespace MORB_SLAM
diff --git a/src/LoopClosing.cc b/src/LoopClosing.cc
index 6cebc6ee..d47542c7 100644
--- a/src/LoopClosing.cc
+++ b/src/LoopClosing.cc
@@ -19,19 +19,19 @@
  * ORB-SLAM3. If not, see <http://www.gnu.org/licenses/>.
  */
 
-#include "LoopClosing.h"
+#include "MORB_SLAM/LoopClosing.h"
 
 #include <mutex>
 #include <thread>
 
-#include "ImprovedTypes.hpp"
-#include "Converter.h"
-#include "G2oTypes.h"
-#include "ORBmatcher.h"
-#include "Optimizer.h"
-#include "Sim3Solver.h"
+#include "MORB_SLAM/ImprovedTypes.hpp"
+#include "MORB_SLAM/Converter.h"
+#include "MORB_SLAM/G2oTypes.h"
+#include "MORB_SLAM/ORBmatcher.h"
+#include "MORB_SLAM/Optimizer.h"
+#include "MORB_SLAM/Sim3Solver.h"
 
-namespace ORB_SLAM3 {
+namespace MORB_SLAM {
 
 LoopClosing::LoopClosing(const Atlas_ptr &pAtlas, KeyFrameDatabase* pDB,
                          ORBVocabulary* pVoc, const bool bFixScale,
@@ -2555,4 +2555,4 @@ bool LoopClosing::isFinished() {
   return mbFinished;
 }
 
-}  // namespace ORB_SLAM3
+}  // namespace MORB_SLAM
diff --git a/src/MLPnPsolver.cpp b/src/MLPnPsolver.cpp
index 0e31d5f8..a8f6dc8e 100644
--- a/src/MLPnPsolver.cpp
+++ b/src/MLPnPsolver.cpp
@@ -46,12 +46,12 @@
 * SUCH DAMAGE.                                                               *
 ******************************************************************************/
 
-#include "MLPnPsolver.h"
+#include "MORB_SLAM/MLPnPsolver.h"
 
 #include <Eigen/Sparse>
 
 
-namespace ORB_SLAM3 {
+namespace MORB_SLAM {
     MLPnPsolver::MLPnPsolver(const Frame &F, const vector<MapPoint *> &vpMapPointMatches):
             mnInliersi(0), mnIterations(0), mnBestInliers(0), N(0), mpCamera(F.mpCamera){
         mvpMapPointMatches = vpMapPointMatches;
diff --git a/src/Map.cc b/src/Map.cc
index 10f32927..a5e1eb62 100644
--- a/src/Map.cc
+++ b/src/Map.cc
@@ -19,11 +19,11 @@
  * ORB-SLAM3. If not, see <http://www.gnu.org/licenses/>.
  */
 
-#include "Map.h"
+#include "MORB_SLAM/Map.h"
 
 #include <mutex>
 
-namespace ORB_SLAM3 {
+namespace MORB_SLAM {
 
 long unsigned int Map::nNextId = 0;
 
@@ -453,4 +453,4 @@ void Map::PostLoad(
   mvpBackupMapPoints.clear();
 }
 
-}  // namespace ORB_SLAM3
+}  // namespace MORB_SLAM
diff --git a/src/MapDrawer.cc b/src/MapDrawer.cc
index 1592bee6..5e34ccb2 100644
--- a/src/MapDrawer.cc
+++ b/src/MapDrawer.cc
@@ -19,18 +19,18 @@
  * ORB-SLAM3. If not, see <http://www.gnu.org/licenses/>.
  */
 
-#include "MapDrawer.h"
+#include "MORB_SLAM/MapDrawer.h"
 
 #include <pangolin/pangolin.h>
 
 #include <stdexcept>
 #include <mutex>
 
-#include "Atlas.h"
-#include "KeyFrame.h"
-#include "MapPoint.h"
+#include "MORB_SLAM/Atlas.h"
+#include "MORB_SLAM/KeyFrame.h"
+#include "MORB_SLAM/MapPoint.h"
 
-namespace ORB_SLAM3 {
+namespace MORB_SLAM {
 
 MapDrawer::MapDrawer(const Atlas_ptr &pAtlas, const std::string &strSettingPath): mpAtlas(pAtlas){
     cv::FileStorage fSettings(strSettingPath, cv::FileStorage::READ);
@@ -419,4 +419,4 @@ void MapDrawer::GetCurrentOpenGLCameraMatrix(pangolin::OpenGlMatrix &M,
   MOw.m[13] = Twc(1, 3);
   MOw.m[14] = Twc(2, 3);
 }
-}  // namespace ORB_SLAM3
+}  // namespace MORB_SLAM
diff --git a/src/MapPoint.cc b/src/MapPoint.cc
index 720436f1..87ec00cc 100644
--- a/src/MapPoint.cc
+++ b/src/MapPoint.cc
@@ -19,13 +19,13 @@
  * ORB-SLAM3. If not, see <http://www.gnu.org/licenses/>.
  */
 
-#include "MapPoint.h"
+#include "MORB_SLAM/MapPoint.h"
 
 #include <mutex>
 
-#include "ORBmatcher.h"
+#include "MORB_SLAM/ORBmatcher.h"
 
-namespace ORB_SLAM3 {
+namespace MORB_SLAM {
 
 long unsigned int MapPoint::nNextId = 0;
 mutex MapPoint::mGlobalMutex;
@@ -654,4 +654,4 @@ void MapPoint::PostLoad(map<long unsigned int, KeyFrame*>& mpKFid,
   mBackupObservationsId2.clear();
 }
 
-}  // namespace ORB_SLAM3
+}  // namespace MORB_SLAM
diff --git a/src/ORBextractor.cc b/src/ORBextractor.cc
index 95716288..5c4c9d3f 100644
--- a/src/ORBextractor.cc
+++ b/src/ORBextractor.cc
@@ -54,7 +54,7 @@
  *
  */
 
-#include "ORBextractor.h"
+#include "MORB_SLAM/ORBextractor.h"
 
 #include <iostream>
 #include <opencv2/core/core.hpp>
@@ -66,7 +66,7 @@
 using namespace cv;
 using namespace std;
 
-namespace ORB_SLAM3 {
+namespace MORB_SLAM {
 
 const int PATCH_SIZE = 31;
 const int HALF_PATCH_SIZE = 15;
@@ -958,4 +958,4 @@ void ORBextractor::ComputePyramid(cv::Mat image) {
   }
 }
 
-}  // namespace ORB_SLAM3
+}  // namespace MORB_SLAM
diff --git a/src/ORBmatcher.cc b/src/ORBmatcher.cc
index 7fd764f7..e0c6b590 100644
--- a/src/ORBmatcher.cc
+++ b/src/ORBmatcher.cc
@@ -19,7 +19,7 @@
  * ORB-SLAM3. If not, see <http://www.gnu.org/licenses/>.
  */
 
-#include "ORBmatcher.h"
+#include "MORB_SLAM/ORBmatcher.h"
 
 #include <limits.h>
 #include <stdint-gcc.h>
@@ -30,7 +30,7 @@
 
 using namespace std;
 
-namespace ORB_SLAM3 {
+namespace MORB_SLAM {
 
 const int ORBmatcher::TH_HIGH = 100;
 const int ORBmatcher::TH_LOW = 50;
@@ -1893,4 +1893,4 @@ int ORBmatcher::DescriptorDistance(const cv::Mat &a, const cv::Mat &b) {
   return dist;
 }
 
-}  // namespace ORB_SLAM3
+}  // namespace MORB_SLAM
diff --git a/src/OptimizableTypes.cpp b/src/OptimizableTypes.cpp
index 40160beb..23cb892e 100644
--- a/src/OptimizableTypes.cpp
+++ b/src/OptimizableTypes.cpp
@@ -19,9 +19,9 @@
  * ORB-SLAM3. If not, see <http://www.gnu.org/licenses/>.
  */
 
-#include "OptimizableTypes.h"
+#include "MORB_SLAM/OptimizableTypes.h"
 
-namespace ORB_SLAM3 {
+namespace MORB_SLAM {
 bool EdgeSE3ProjectXYZOnlyPose::read(std::istream& is) {
   for (int i = 0; i < 2; i++) {
     is >> _measurement[i];
@@ -310,4 +310,4 @@ bool EdgeInverseSim3ProjectXYZ::write(std::ostream& os) const {
   return os.good();
 }
 
-}  // namespace ORB_SLAM3
+}  // namespace MORB_SLAM
diff --git a/src/Optimizer.cc b/src/Optimizer.cc
index 1cf32a4b..608911ea 100644
--- a/src/Optimizer.cc
+++ b/src/Optimizer.cc
@@ -19,7 +19,7 @@
  * ORB-SLAM3. If not, see <http://www.gnu.org/licenses/>.
  */
 
-#include "Optimizer.h"
+#include "MORB_SLAM/Optimizer.h"
 
 #include <Eigen/Dense>
 #include <Eigen/StdVector>
@@ -27,9 +27,9 @@
 #include <mutex>
 #include <unsupported/Eigen/MatrixFunctions>
 
-#include "Converter.h"
-#include "G2oTypes.h"
-#include "OptimizableTypes.h"
+#include "MORB_SLAM/Converter.h"
+#include "MORB_SLAM/G2oTypes.h"
+#include "MORB_SLAM/OptimizableTypes.h"
 #include "g2o/core/block_solver.h"
 #include "g2o/core/optimization_algorithm_gauss_newton.h"
 #include "g2o/core/optimization_algorithm_levenberg.h"
@@ -39,7 +39,7 @@
 #include "g2o/solvers/linear_solver_eigen.h"
 #include "g2o/types/types_six_dof_expmap.h"
 
-namespace ORB_SLAM3 {
+namespace MORB_SLAM {
 bool sortByVal(const pair<MapPoint*, int>& a, const pair<MapPoint*, int>& b) {
   return (a.second < b.second);
 }
@@ -81,10 +81,10 @@ void Optimizer::BundleAdjustment(const vector<KeyFrame*>& vpKFs,
 
   const int nExpectedSize = (vpKFs.size()) * vpMP.size();
 
-  vector<ORB_SLAM3::EdgeSE3ProjectXYZ*> vpEdgesMono;
+  vector<MORB_SLAM::EdgeSE3ProjectXYZ*> vpEdgesMono;
   vpEdgesMono.reserve(nExpectedSize);
 
-  vector<ORB_SLAM3::EdgeSE3ProjectXYZToBody*> vpEdgesBody;
+  vector<MORB_SLAM::EdgeSE3ProjectXYZToBody*> vpEdgesBody;
   vpEdgesBody.reserve(nExpectedSize);
 
   vector<KeyFrame*> vpEdgeKFMono;
@@ -158,7 +158,7 @@ void Optimizer::BundleAdjustment(const vector<KeyFrame*>& vpKFs,
         Eigen::Matrix<double, 2, 1> obs;
         obs << kpUn.pt.x, kpUn.pt.y;
 
-        ORB_SLAM3::EdgeSE3ProjectXYZ* e = new ORB_SLAM3::EdgeSE3ProjectXYZ();
+        MORB_SLAM::EdgeSE3ProjectXYZ* e = new MORB_SLAM::EdgeSE3ProjectXYZ();
 
         e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(
                             optimizer.vertex(id)));
@@ -230,8 +230,8 @@ void Optimizer::BundleAdjustment(const vector<KeyFrame*>& vpKFs,
           cv::KeyPoint kp = pKF->mvKeysRight[rightIndex];
           obs << kp.pt.x, kp.pt.y;
 
-          ORB_SLAM3::EdgeSE3ProjectXYZToBody* e =
-              new ORB_SLAM3::EdgeSE3ProjectXYZToBody();
+          MORB_SLAM::EdgeSE3ProjectXYZToBody* e =
+              new MORB_SLAM::EdgeSE3ProjectXYZToBody();
 
           e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(
                               optimizer.vertex(id)));
@@ -300,7 +300,7 @@ void Optimizer::BundleAdjustment(const vector<KeyFrame*>& vpKFs,
         vector<MapPoint*> vpMonoMPsOpt, vpStereoMPsOpt;
 
         for (size_t i2 = 0, iend = vpEdgesMono.size(); i2 < iend; i2++) {
-          ORB_SLAM3::EdgeSE3ProjectXYZ* e = vpEdgesMono[i2];
+          MORB_SLAM::EdgeSE3ProjectXYZ* e = vpEdgesMono[i2];
           MapPoint* pMP = vpMapPointEdgeMono[i2];
           KeyFrame* pKFedge = vpEdgeKFMono[i2];
 
@@ -786,8 +786,8 @@ int Optimizer::PoseOptimization(Frame* pFrame) {
   // Set MapPoint vertices
   const int N = pFrame->N;
 
-  vector<ORB_SLAM3::EdgeSE3ProjectXYZOnlyPose*> vpEdgesMono;
-  vector<ORB_SLAM3::EdgeSE3ProjectXYZOnlyPoseToBody*> vpEdgesMono_FHR;
+  vector<MORB_SLAM::EdgeSE3ProjectXYZOnlyPose*> vpEdgesMono;
+  vector<MORB_SLAM::EdgeSE3ProjectXYZOnlyPoseToBody*> vpEdgesMono_FHR;
   vector<size_t> vnIndexEdgeMono, vnIndexEdgeRight;
   vpEdgesMono.reserve(N);
   vpEdgesMono_FHR.reserve(N);
@@ -819,8 +819,8 @@ int Optimizer::PoseOptimization(Frame* pFrame) {
             const cv::KeyPoint& kpUn = pFrame->mvKeysUn[i];
             obs << kpUn.pt.x, kpUn.pt.y;
 
-            ORB_SLAM3::EdgeSE3ProjectXYZOnlyPose* e =
-                new ORB_SLAM3::EdgeSE3ProjectXYZOnlyPose();
+            MORB_SLAM::EdgeSE3ProjectXYZOnlyPose* e =
+                new MORB_SLAM::EdgeSE3ProjectXYZOnlyPose();
 
             e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(
                                 optimizer.vertex(0)));
@@ -890,8 +890,8 @@ int Optimizer::PoseOptimization(Frame* pFrame) {
             Eigen::Matrix<double, 2, 1> obs;
             obs << kpUn.pt.x, kpUn.pt.y;
 
-            ORB_SLAM3::EdgeSE3ProjectXYZOnlyPose* e =
-                new ORB_SLAM3::EdgeSE3ProjectXYZOnlyPose();
+            MORB_SLAM::EdgeSE3ProjectXYZOnlyPose* e =
+                new MORB_SLAM::EdgeSE3ProjectXYZOnlyPose();
 
             e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(
                                 optimizer.vertex(0)));
@@ -918,8 +918,8 @@ int Optimizer::PoseOptimization(Frame* pFrame) {
 
             pFrame->mvbOutlier[i] = false;
 
-            ORB_SLAM3::EdgeSE3ProjectXYZOnlyPoseToBody* e =
-                new ORB_SLAM3::EdgeSE3ProjectXYZOnlyPoseToBody();
+            MORB_SLAM::EdgeSE3ProjectXYZOnlyPoseToBody* e =
+                new MORB_SLAM::EdgeSE3ProjectXYZOnlyPoseToBody();
 
             e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(
                                 optimizer.vertex(0)));
@@ -968,7 +968,7 @@ int Optimizer::PoseOptimization(Frame* pFrame) {
 
     nBad = 0;
     for (size_t i = 0, iend = vpEdgesMono.size(); i < iend; i++) {
-      ORB_SLAM3::EdgeSE3ProjectXYZOnlyPose* e = vpEdgesMono[i];
+      MORB_SLAM::EdgeSE3ProjectXYZOnlyPose* e = vpEdgesMono[i];
 
       const size_t idx = vnIndexEdgeMono[i];
 
@@ -991,7 +991,7 @@ int Optimizer::PoseOptimization(Frame* pFrame) {
     }
 
     for (size_t i = 0, iend = vpEdgesMono_FHR.size(); i < iend; i++) {
-      ORB_SLAM3::EdgeSE3ProjectXYZOnlyPoseToBody* e = vpEdgesMono_FHR[i];
+      MORB_SLAM::EdgeSE3ProjectXYZOnlyPoseToBody* e = vpEdgesMono_FHR[i];
 
       const size_t idx = vnIndexEdgeRight[i];
 
@@ -1186,10 +1186,10 @@ void Optimizer::LocalBundleAdjustment(KeyFrame* pKF, bool* pbStopFlag,
   const int nExpectedSize =
       (lLocalKeyFrames.size() + lFixedCameras.size()) * lLocalMapPoints.size();
 
-  vector<ORB_SLAM3::EdgeSE3ProjectXYZ*> vpEdgesMono;
+  vector<MORB_SLAM::EdgeSE3ProjectXYZ*> vpEdgesMono;
   vpEdgesMono.reserve(nExpectedSize);
 
-  vector<ORB_SLAM3::EdgeSE3ProjectXYZToBody*> vpEdgesBody;
+  vector<MORB_SLAM::EdgeSE3ProjectXYZToBody*> vpEdgesBody;
   vpEdgesBody.reserve(nExpectedSize);
 
   vector<KeyFrame*> vpEdgeKFMono;
@@ -1250,7 +1250,7 @@ void Optimizer::LocalBundleAdjustment(KeyFrame* pKF, bool* pbStopFlag,
           Eigen::Matrix<double, 2, 1> obs;
           obs << kpUn.pt.x, kpUn.pt.y;
 
-          ORB_SLAM3::EdgeSE3ProjectXYZ* e = new ORB_SLAM3::EdgeSE3ProjectXYZ();
+          MORB_SLAM::EdgeSE3ProjectXYZ* e = new MORB_SLAM::EdgeSE3ProjectXYZ();
 
           e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(
                               optimizer.vertex(id)));
@@ -1319,8 +1319,8 @@ void Optimizer::LocalBundleAdjustment(KeyFrame* pKF, bool* pbStopFlag,
             cv::KeyPoint kp = pKFi->mvKeysRight[rightIndex];
             obs << kp.pt.x, kp.pt.y;
 
-            ORB_SLAM3::EdgeSE3ProjectXYZToBody* e =
-                new ORB_SLAM3::EdgeSE3ProjectXYZToBody();
+            MORB_SLAM::EdgeSE3ProjectXYZToBody* e =
+                new MORB_SLAM::EdgeSE3ProjectXYZToBody();
 
             e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(
                                 optimizer.vertex(id)));
@@ -1365,7 +1365,7 @@ void Optimizer::LocalBundleAdjustment(KeyFrame* pKF, bool* pbStopFlag,
 
   // Check inlier observations
   for (size_t i = 0, iend = vpEdgesMono.size(); i < iend; i++) {
-    ORB_SLAM3::EdgeSE3ProjectXYZ* e = vpEdgesMono[i];
+    MORB_SLAM::EdgeSE3ProjectXYZ* e = vpEdgesMono[i];
     MapPoint* pMP = vpMapPointEdgeMono[i];
 
     if (pMP->isBad()) continue;
@@ -1377,7 +1377,7 @@ void Optimizer::LocalBundleAdjustment(KeyFrame* pKF, bool* pbStopFlag,
   }
 
   for (size_t i = 0, iend = vpEdgesBody.size(); i < iend; i++) {
-    ORB_SLAM3::EdgeSE3ProjectXYZToBody* e = vpEdgesBody[i];
+    MORB_SLAM::EdgeSE3ProjectXYZToBody* e = vpEdgesBody[i];
     MapPoint* pMP = vpMapPointEdgeBody[i];
 
     if (pMP->isBad()) continue;
@@ -2086,7 +2086,7 @@ int Optimizer::OptimizeSim3(KeyFrame* pKF1, KeyFrame* pKF2,
   const Eigen::Vector3f t2w = pKF2->GetTranslation();
 
   // Set Sim3 vertex
-  ORB_SLAM3::VertexSim3Expmap* vSim3 = new ORB_SLAM3::VertexSim3Expmap();
+  MORB_SLAM::VertexSim3Expmap* vSim3 = new MORB_SLAM::VertexSim3Expmap();
   vSim3->_fix_scale = bFixScale;
   vSim3->setEstimate(g2oS12);
   vSim3->setId(0);
@@ -2098,8 +2098,8 @@ int Optimizer::OptimizeSim3(KeyFrame* pKF1, KeyFrame* pKF2,
   // Set MapPoint vertices
   const int N = vpMatches1.size();
   const vector<MapPoint*> vpMapPoints1 = pKF1->GetMapPointMatches();
-  vector<ORB_SLAM3::EdgeSim3ProjectXYZ*> vpEdges12;
-  vector<ORB_SLAM3::EdgeInverseSim3ProjectXYZ*> vpEdges21;
+  vector<MORB_SLAM::EdgeSim3ProjectXYZ*> vpEdges12;
+  vector<MORB_SLAM::EdgeInverseSim3ProjectXYZ*> vpEdges21;
   vector<size_t> vnIndexEdge;
   vector<bool> vbIsInKF2;
 
@@ -2191,7 +2191,7 @@ int Optimizer::OptimizeSim3(KeyFrame* pKF1, KeyFrame* pKF2,
     const cv::KeyPoint& kpUn1 = pKF1->mvKeysUn[i];
     obs1 << kpUn1.pt.x, kpUn1.pt.y;
 
-    ORB_SLAM3::EdgeSim3ProjectXYZ* e12 = new ORB_SLAM3::EdgeSim3ProjectXYZ();
+    MORB_SLAM::EdgeSim3ProjectXYZ* e12 = new MORB_SLAM::EdgeSim3ProjectXYZ();
 
     e12->setVertex(
         0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(id2)));
@@ -2228,8 +2228,8 @@ int Optimizer::OptimizeSim3(KeyFrame* pKF1, KeyFrame* pKF2,
       nOutKF2++;
     }
 
-    ORB_SLAM3::EdgeInverseSim3ProjectXYZ* e21 =
-        new ORB_SLAM3::EdgeInverseSim3ProjectXYZ();
+    MORB_SLAM::EdgeInverseSim3ProjectXYZ* e21 =
+        new MORB_SLAM::EdgeInverseSim3ProjectXYZ();
 
     e21->setVertex(
         0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(id1)));
@@ -2259,8 +2259,8 @@ int Optimizer::OptimizeSim3(KeyFrame* pKF1, KeyFrame* pKF2,
   int nBad = 0;
   int nBadOutKF2 = 0;
   for (size_t i = 0; i < vpEdges12.size(); i++) {
-    ORB_SLAM3::EdgeSim3ProjectXYZ* e12 = vpEdges12[i];
-    ORB_SLAM3::EdgeInverseSim3ProjectXYZ* e21 = vpEdges21[i];
+    MORB_SLAM::EdgeSim3ProjectXYZ* e12 = vpEdges12[i];
+    MORB_SLAM::EdgeInverseSim3ProjectXYZ* e21 = vpEdges21[i];
     if (!e12 || !e21) continue;
 
     if (e12->chi2() > th2 || e21->chi2() > th2) {
@@ -2268,8 +2268,8 @@ int Optimizer::OptimizeSim3(KeyFrame* pKF1, KeyFrame* pKF2,
       vpMatches1[idx] = nullptr;
       optimizer.removeEdge(e12);
       optimizer.removeEdge(e21);
-      vpEdges12[i] = static_cast<ORB_SLAM3::EdgeSim3ProjectXYZ*>(nullptr);
-      vpEdges21[i] = static_cast<ORB_SLAM3::EdgeInverseSim3ProjectXYZ*>(nullptr);
+      vpEdges12[i] = static_cast<MORB_SLAM::EdgeSim3ProjectXYZ*>(nullptr);
+      vpEdges21[i] = static_cast<MORB_SLAM::EdgeInverseSim3ProjectXYZ*>(nullptr);
       nBad++;
 
       if (!vbIsInKF2[i]) {
@@ -2298,8 +2298,8 @@ int Optimizer::OptimizeSim3(KeyFrame* pKF1, KeyFrame* pKF2,
   int nIn = 0;
   mAcumHessian = Eigen::MatrixXd::Zero(7, 7);
   for (size_t i = 0; i < vpEdges12.size(); i++) {
-    ORB_SLAM3::EdgeSim3ProjectXYZ* e12 = vpEdges12[i];
-    ORB_SLAM3::EdgeInverseSim3ProjectXYZ* e21 = vpEdges21[i];
+    MORB_SLAM::EdgeSim3ProjectXYZ* e12 = vpEdges12[i];
+    MORB_SLAM::EdgeInverseSim3ProjectXYZ* e21 = vpEdges21[i];
     if (!e12 || !e21) continue;
 
     e12->computeError();
@@ -3526,7 +3526,7 @@ void Optimizer::LocalBundleAdjustment(KeyFrame* pMainKF,
   const int nExpectedSize =
       (vpAdjustKF.size() + vpFixedKF.size()) * vpMPs.size();
 
-  vector<ORB_SLAM3::EdgeSE3ProjectXYZ*> vpEdgesMono;
+  vector<MORB_SLAM::EdgeSE3ProjectXYZ*> vpEdgesMono;
   vpEdgesMono.reserve(nExpectedSize);
 
   vector<KeyFrame*> vpEdgeKFMono;
@@ -3585,7 +3585,7 @@ void Optimizer::LocalBundleAdjustment(KeyFrame* pMainKF,
         Eigen::Matrix<double, 2, 1> obs;
         obs << kpUn.pt.x, kpUn.pt.y;
 
-        ORB_SLAM3::EdgeSE3ProjectXYZ* e = new ORB_SLAM3::EdgeSE3ProjectXYZ();
+        MORB_SLAM::EdgeSE3ProjectXYZ* e = new MORB_SLAM::EdgeSE3ProjectXYZ();
 
         e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(
                             optimizer.vertex(id)));
@@ -3663,7 +3663,7 @@ void Optimizer::LocalBundleAdjustment(KeyFrame* pMainKF,
     // Check inlier observations
     int badMonoMP = 0, badStereoMP = 0;
     for (size_t i = 0, iend = vpEdgesMono.size(); i < iend; i++) {
-      ORB_SLAM3::EdgeSE3ProjectXYZ* e = vpEdgesMono[i];
+      MORB_SLAM::EdgeSE3ProjectXYZ* e = vpEdgesMono[i];
       MapPoint* pMP = vpMapPointEdgeMono[i];
 
       if (pMP->isBad()) continue;
@@ -3705,7 +3705,7 @@ void Optimizer::LocalBundleAdjustment(KeyFrame* pMainKF,
   // Check inlier observations
   int badMonoMP = 0, badStereoMP = 0;
   for (size_t i = 0, iend = vpEdgesMono.size(); i < iend; i++) {
-    ORB_SLAM3::EdgeSE3ProjectXYZ* e = vpEdgesMono[i];
+    MORB_SLAM::EdgeSE3ProjectXYZ* e = vpEdgesMono[i];
     MapPoint* pMP = vpMapPointEdgeMono[i];
 
     if (pMP->isBad()) continue;
@@ -3796,7 +3796,7 @@ void Optimizer::LocalBundleAdjustment(KeyFrame* pMainKF,
     vector<MapPoint*> vpMonoMPsBad, vpStereoMPsBad;
 
     for (size_t i = 0, iend = vpEdgesMono.size(); i < iend; i++) {
-      ORB_SLAM3::EdgeSE3ProjectXYZ* e = vpEdgesMono[i];
+      MORB_SLAM::EdgeSE3ProjectXYZ* e = vpEdgesMono[i];
       MapPoint* pMP = vpMapPointEdgeMono[i];
       KeyFrame* pKFedge = vpEdgeKFMono[i];
 
@@ -5470,4 +5470,4 @@ void Optimizer::OptimizeEssentialGraph4DoF(
   pMap->IncreaseChangeIndex();
 }
 
-}  // namespace ORB_SLAM3
+}  // namespace MORB_SLAM
diff --git a/src/Settings.cc b/src/Settings.cc
index 8d4c5b8f..bb7349c9 100644
--- a/src/Settings.cc
+++ b/src/Settings.cc
@@ -19,18 +19,18 @@
  * ORB-SLAM3. If not, see <http://www.gnu.org/licenses/>.
  */
 
-#include "Settings.h"
+#include "MORB_SLAM/Settings.h"
 
-#include "ImprovedTypes.hpp"
+#include "MORB_SLAM/ImprovedTypes.hpp"
 #include <iostream>
 #include <opencv2/opencv.hpp>
 #include <opencv2/core/eigen.hpp>
 #include <opencv2/core/persistence.hpp>
 
-#include "CameraModels/KannalaBrandt8.h"
-#include "CameraModels/Pinhole.h"
+#include "MORB_SLAM/CameraModels/KannalaBrandt8.h"
+#include "MORB_SLAM/CameraModels/Pinhole.h"
 
-namespace ORB_SLAM3 {
+namespace MORB_SLAM {
 
 template <>
 float Settings::readParameter<float>(cv::FileStorage& fSettings,
@@ -152,7 +152,7 @@ Settings::Settings(const std::string& configFile, const int& sensor)
   std::cout << "\t-Loaded camera 1" << std::endl;
 
   // Read second camera if stereo (not rectified)
-  if (sensor_ == ORB_SLAM3::CameraType::STEREO || sensor_ == ORB_SLAM3::CameraType::IMU_STEREO) {
+  if (sensor_ == MORB_SLAM::CameraType::STEREO || sensor_ == MORB_SLAM::CameraType::IMU_STEREO) {
     readCamera2(fSettings);
     std::cout << "\t-Loaded camera 2" << std::endl;
   }
@@ -161,13 +161,13 @@ Settings::Settings(const std::string& configFile, const int& sensor)
   readImageInfo(fSettings);
   std::cout << "\t-Loaded image info" << std::endl;
 
-  if (sensor_ == ORB_SLAM3::CameraType::IMU_MONOCULAR || sensor_ == ORB_SLAM3::CameraType::IMU_STEREO ||
-      sensor_ == ORB_SLAM3::CameraType::IMU_RGBD) {
+  if (sensor_ == MORB_SLAM::CameraType::IMU_MONOCULAR || sensor_ == MORB_SLAM::CameraType::IMU_STEREO ||
+      sensor_ == MORB_SLAM::CameraType::IMU_RGBD) {
     readIMU(fSettings);
     std::cout << "\t-Loaded IMU calibration" << std::endl;
   }
 
-  if (sensor_ == ORB_SLAM3::CameraType::RGBD || sensor_ == ORB_SLAM3::CameraType::IMU_RGBD) {
+  if (sensor_ == MORB_SLAM::CameraType::RGBD || sensor_ == MORB_SLAM::CameraType::IMU_RGBD) {
     readRGBD(fSettings);
     std::cout << "\t-Loaded RGB-D calibration" << std::endl;
   }
@@ -232,7 +232,7 @@ void Settings::readCamera1(cv::FileStorage& fSettings) {
     }
 
     // Check if we need to correct distortion from the images
-    if ((sensor_ == ORB_SLAM3::CameraType::MONOCULAR || sensor_ == ORB_SLAM3::CameraType::IMU_MONOCULAR) &&
+    if ((sensor_ == MORB_SLAM::CameraType::MONOCULAR || sensor_ == MORB_SLAM::CameraType::IMU_MONOCULAR) &&
         vPinHoleDistorsion1_.size() != 0) {
       bNeedToUndistort_ = true;
     }
@@ -270,7 +270,7 @@ void Settings::readCamera1(cv::FileStorage& fSettings) {
     calibration1_ = std::make_shared<KannalaBrandt8>(vCalibration);
     originalCalib1_ = new KannalaBrandt8(vCalibration);
 
-    if (sensor_ == ORB_SLAM3::CameraType::STEREO || sensor_ == ORB_SLAM3::CameraType::IMU_STEREO) {
+    if (sensor_ == MORB_SLAM::CameraType::STEREO || sensor_ == MORB_SLAM::CameraType::IMU_STEREO) {
       int colBegin =
           readParameter<int>(fSettings, "Camera1.overlappingBegin", found);
       int colEnd =
@@ -388,7 +388,7 @@ void Settings::readImageInfo(cv::FileStorage& fSettings) {
       calibration1_->setParameter(
           calibration1_->getParameter(3) * scaleRowFactor, 3);
 
-      if ((sensor_ == ORB_SLAM3::CameraType::STEREO || sensor_ == ORB_SLAM3::CameraType::IMU_STEREO) &&
+      if ((sensor_ == MORB_SLAM::CameraType::STEREO || sensor_ == MORB_SLAM::CameraType::IMU_STEREO) &&
           cameraType_ != Rectified) {
         calibration2_->setParameter(
             calibration2_->getParameter(1) * scaleRowFactor, 1);
@@ -412,7 +412,7 @@ void Settings::readImageInfo(cv::FileStorage& fSettings) {
       calibration1_->setParameter(
           calibration1_->getParameter(2) * scaleColFactor, 2);
 
-      if ((sensor_ == ORB_SLAM3::CameraType::STEREO || sensor_ == ORB_SLAM3::CameraType::IMU_STEREO) &&
+      if ((sensor_ == MORB_SLAM::CameraType::STEREO || sensor_ == MORB_SLAM::CameraType::IMU_STEREO) &&
           cameraType_ != Rectified) {
         calibration2_->setParameter(
             calibration2_->getParameter(0) * scaleColFactor, 0);
@@ -554,7 +554,7 @@ void Settings::precomputeRectificationMaps() {
   bf_ = b_ * P1.at<double>(0, 0);
 
   // Update relative pose between camera 1 and IMU if necessary
-  if (sensor_ == ORB_SLAM3::CameraType::IMU_STEREO) {
+  if (sensor_ == MORB_SLAM::CameraType::IMU_STEREO) {
     Eigen::Matrix3f eigenR_r1_u1;
     cv::cv2eigen(R_r1_u1, eigenR_r1_u1);
     Sophus::SE3f T_r1_u1(eigenR_r1_u1, Eigen::Vector3f::Zero());
@@ -587,8 +587,8 @@ std::ostream& operator<<(std::ostream& output, const Settings& settings) {
     output << " ]" << std::endl;
   }
   std::cout << "bout to start camera 2 stuff\n";
-  if (settings.sensor_ == ORB_SLAM3::CameraType::STEREO ||
-      settings.sensor_ == ORB_SLAM3::CameraType::IMU_STEREO) {
+  if (settings.sensor_ == MORB_SLAM::CameraType::STEREO ||
+      settings.sensor_ == MORB_SLAM::CameraType::IMU_STEREO) {
     output << "\t-Camera 2 parameters (";
     if (settings.cameraType_ == Settings::PinHole ||
         settings.cameraType_ == Settings::Rectified) {
@@ -630,8 +630,8 @@ std::ostream& operator<<(std::ostream& output, const Settings& settings) {
     }
     output << " ]" << std::endl;
 
-    if ((settings.sensor_ == ORB_SLAM3::CameraType::STEREO ||
-         settings.sensor_ == ORB_SLAM3::CameraType::IMU_STEREO) &&
+    if ((settings.sensor_ == MORB_SLAM::CameraType::STEREO ||
+         settings.sensor_ == MORB_SLAM::CameraType::IMU_STEREO) &&
         settings.cameraType_ == Settings::KannalaBrandt) {
       output << "\t-Camera 2 parameters after resize: [ ";
       for (size_t i = 0; i < settings.calibration2_->size(); i++) {
@@ -644,8 +644,8 @@ std::ostream& operator<<(std::ostream& output, const Settings& settings) {
   output << "\t-Sequence FPS: " << settings.fps_ << std::endl;
 
   // Stereo stuff
-  if (settings.sensor_ == ORB_SLAM3::CameraType::STEREO ||
-      settings.sensor_ == ORB_SLAM3::CameraType::IMU_STEREO) {
+  if (settings.sensor_ == MORB_SLAM::CameraType::STEREO ||
+      settings.sensor_ == MORB_SLAM::CameraType::IMU_STEREO) {
     output << "\t-Stereo baseline: " << settings.b_ << std::endl;
     output << "\t-Stereo depth threshold : " << settings.thDepth_ << std::endl;
 
@@ -661,9 +661,9 @@ std::ostream& operator<<(std::ostream& output, const Settings& settings) {
     }
   }
 
-  if (settings.sensor_ == ORB_SLAM3::CameraType::IMU_MONOCULAR ||
-      settings.sensor_ == ORB_SLAM3::CameraType::IMU_STEREO ||
-      settings.sensor_ == ORB_SLAM3::CameraType::IMU_RGBD) {
+  if (settings.sensor_ == MORB_SLAM::CameraType::IMU_MONOCULAR ||
+      settings.sensor_ == MORB_SLAM::CameraType::IMU_STEREO ||
+      settings.sensor_ == MORB_SLAM::CameraType::IMU_RGBD) {
     output << "\t-Gyro noise: " << settings.noiseGyro_ << std::endl;
     output << "\t-Accelerometer noise: " << settings.noiseAcc_ << std::endl;
     output << "\t-Gyro walk: " << settings.gyroWalk_ << std::endl;
@@ -671,8 +671,8 @@ std::ostream& operator<<(std::ostream& output, const Settings& settings) {
     output << "\t-IMU frequency: " << settings.imuFrequency_ << std::endl;
   }
 
-  if (settings.sensor_ == ORB_SLAM3::CameraType::RGBD ||
-      settings.sensor_ == ORB_SLAM3::CameraType::IMU_RGBD) {
+  if (settings.sensor_ == MORB_SLAM::CameraType::RGBD ||
+      settings.sensor_ == MORB_SLAM::CameraType::IMU_RGBD) {
     output << "\t-RGB-D depth map factor: " << settings.depthMapFactor_ << std::endl;
   }
 
@@ -684,4 +684,4 @@ std::ostream& operator<<(std::ostream& output, const Settings& settings) {
 
   return output;
 }
-};  // namespace ORB_SLAM3
+};  // namespace MORB_SLAM
diff --git a/src/Sim3Solver.cc b/src/Sim3Solver.cc
index c1c37fe4..8ef9c91c 100644
--- a/src/Sim3Solver.cc
+++ b/src/Sim3Solver.cc
@@ -19,17 +19,17 @@
  * ORB-SLAM3. If not, see <http://www.gnu.org/licenses/>.
  */
 
-#include "Sim3Solver.h"
+#include "MORB_SLAM/Sim3Solver.h"
 
 #include <cmath>
 #include <opencv2/core/core.hpp>
 #include <vector>
 
 #include "DUtils/Random.h"
-#include "KeyFrame.h"
-#include "ORBmatcher.h"
+#include "MORB_SLAM/KeyFrame.h"
+#include "MORB_SLAM/ORBmatcher.h"
 
-namespace ORB_SLAM3 {
+namespace MORB_SLAM {
 
 Sim3Solver::Sim3Solver(KeyFrame *pKF1, KeyFrame *pKF2,
                        const vector<MapPoint *> &vpMatched12,
@@ -451,4 +451,4 @@ void Sim3Solver::FromCameraToImage(const vector<Eigen::Vector3f> &vP3Dc,
   }
 }
 
-}  // namespace ORB_SLAM3
+}  // namespace MORB_SLAM
diff --git a/src/System.cc b/src/System.cc
index ba2f7b67..79c3b81c 100644
--- a/src/System.cc
+++ b/src/System.cc
@@ -19,13 +19,13 @@
  * ORB-SLAM3. If not, see <http://www.gnu.org/licenses/>.
  */
 
-#include "System.h"
+#include "MORB_SLAM/System.h"
 
 #include <openssl/md5.h>
 #include <opencv2/opencv.hpp>
 #include <pangolin/pangolin.h>
 
-#include "ImprovedTypes.hpp"
+#include "MORB_SLAM/ImprovedTypes.hpp"
 #include <boost/archive/binary_iarchive.hpp>
 #include <boost/archive/binary_oarchive.hpp>
 #include <boost/archive/text_iarchive.hpp>
@@ -41,9 +41,9 @@
 #include <string>
 #include <iostream>
 
-#include "Converter.h"
+#include "MORB_SLAM/Converter.h"
 
-namespace ORB_SLAM3 {
+namespace MORB_SLAM {
 
 Verbose::eLevel Verbose::th = Verbose::VERBOSITY_NORMAL;
 
@@ -208,7 +208,7 @@ System::System(const std::string& strVocFile, const std::string& strSettingsFile
       this, mpAtlas, mSensor == CameraType::MONOCULAR || mSensor == CameraType::IMU_MONOCULAR,
       mSensor == CameraType::IMU_MONOCULAR || mSensor == CameraType::IMU_STEREO || mSensor == CameraType::IMU_RGBD,
       strSequence);
-  mptLocalMapping = new thread(&ORB_SLAM3::LocalMapping::Run, mpLocalMapper);
+  mptLocalMapping = new thread(&MORB_SLAM::LocalMapping::Run, mpLocalMapper);
   if (settings_)
     mpLocalMapper->mThFarPoints = settings_->thFarPoints();
   else
@@ -225,7 +225,7 @@ System::System(const std::string& strVocFile, const std::string& strSettingsFile
   mpLoopCloser =
       new LoopClosing(mpAtlas, mpKeyFrameDatabase, mpVocabulary,
                       mSensor != CameraType::MONOCULAR, activeLC);  // mSensor!=CameraType::MONOCULAR);
-  mptLoopClosing = new thread(&ORB_SLAM3::LoopClosing::Run, mpLoopCloser);
+  mptLoopClosing = new thread(&MORB_SLAM::LoopClosing::Run, mpLoopCloser);
 
   // Set pointers between threads
   mpTracker->SetLocalMapper(mpLocalMapper);
@@ -555,7 +555,7 @@ void System::SaveTrajectoryTUM(const string& filename) {
 
   // 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<MORB_SLAM::KeyFrame*>::iterator lRit = mpTracker->mlpReferences.begin();
   list<double>::iterator lT = mpTracker->mlFrameTimes.begin();
   list<bool>::iterator lbL = mpTracker->mlbLost.begin();
   for (list<Sophus::SE3f>::iterator
@@ -674,7 +674,7 @@ void System::SaveTrajectoryEuRoC(const string& filename) {
 
   // 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<MORB_SLAM::KeyFrame*>::iterator lRit = mpTracker->mlpReferences.begin();
   list<double>::iterator lT = mpTracker->mlFrameTimes.begin();
   list<bool>::iterator lbL = mpTracker->mlbLost.begin();
 
@@ -780,7 +780,7 @@ void System::SaveTrajectoryEuRoC(const string& filename, std::shared_ptr<Map> pM
 
   // 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<MORB_SLAM::KeyFrame*>::iterator lRit = mpTracker->mlpReferences.begin();
   list<double>::iterator lT = mpTracker->mlFrameTimes.begin();
   list<bool>::iterator lbL = mpTracker->mlbLost.begin();
 
@@ -895,7 +895,7 @@ transformation.
     // 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 =
+    list<MORB_SLAM::KeyFrame*>::iterator lRit =
 mpTracker->mlpReferences.begin(); list<double>::iterator lT =
 mpTracker->mlFrameTimes.begin(); list<bool>::iterator lbL =
 mpTracker->mlbLost.begin();
@@ -1166,13 +1166,13 @@ transformation.
     // 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 =
+    list<MORB_SLAM::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;
+        MORB_SLAM::KeyFrame* pKF = *lRit;
 
         cv::Mat Trw = cv::Mat::eye(4,4,CV_32F);
 
@@ -1223,13 +1223,13 @@ void System::SaveTrajectoryKITTI(const string& filename) {
 
   // 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<MORB_SLAM::KeyFrame*>::iterator lRit = mpTracker->mlpReferences.begin();
   list<double>::iterator lT = mpTracker->mlFrameTimes.begin();
   for (list<Sophus::SE3f>::iterator
            lit = mpTracker->mlRelativeFramePoses.begin(),
            lend = mpTracker->mlRelativeFramePoses.end();
        lit != lend; lit++, lRit++, lT++) {
-    ORB_SLAM3::KeyFrame* pKF = *lRit;
+    MORB_SLAM::KeyFrame* pKF = *lRit;
 
     Sophus::SE3f Trw;
 
@@ -1550,4 +1550,4 @@ string System::CalculateCheckSum(string filename, int type) {
   return checksum;
 }
 
-}  // namespace ORB_SLAM3
+}  // namespace MORB_SLAM
diff --git a/src/Tracking.cc b/src/Tracking.cc
index c1d62a98..e72d80e3 100644
--- a/src/Tracking.cc
+++ b/src/Tracking.cc
@@ -19,26 +19,26 @@
  * ORB-SLAM3. If not, see <http://www.gnu.org/licenses/>.
  */
 
-#include "Tracking.h"
+#include "MORB_SLAM/Tracking.h"
 
-#include "ImprovedTypes.hpp"
+#include "MORB_SLAM/ImprovedTypes.hpp"
 #include <chrono>
 #include <iostream>
 #include <mutex>
 #include <stdexcept>
 
-#include "Converter.h"
-#include "G2oTypes.h"
-#include "GeometricTools.h"
-#include "KannalaBrandt8.h"
-#include "MLPnPsolver.h"
-#include "ORBmatcher.h"
-#include "Optimizer.h"
-#include "Pinhole.h"
+#include "MORB_SLAM/Converter.h"
+#include "MORB_SLAM/G2oTypes.h"
+#include "MORB_SLAM/GeometricTools.h"
+#include "MORB_SLAM/CameraModels/KannalaBrandt8.h"
+#include "MORB_SLAM/MLPnPsolver.h"
+#include "MORB_SLAM/ORBmatcher.h"
+#include "MORB_SLAM/Optimizer.h"
+#include "MORB_SLAM/CameraModels/Pinhole.h"
 
 using namespace std;
 
-namespace ORB_SLAM3 {
+namespace MORB_SLAM {
 
 Tracking::Tracking(System* pSys, ORBVocabulary* pVoc, const Atlas_ptr &pAtlas,
                    KeyFrameDatabase* pKFDB, const string& strSettingPath,
@@ -3641,7 +3641,7 @@ void Tracking::UpdateFrameIMU(const float s, const IMU::Bias& b,
                               KeyFrame* pCurrentKeyFrame) {
   std::shared_ptr<Map> pMap = pCurrentKeyFrame->GetMap();
   // unsigned int index = mnFirstFrameId; // UNUSED
-  list<ORB_SLAM3::KeyFrame*>::iterator lRit = mlpReferences.begin();
+  list<MORB_SLAM::KeyFrame*>::iterator lRit = mlpReferences.begin();
   list<bool>::iterator lbL = mlbLost.begin();
   for (auto lit = mlRelativeFramePoses.begin(),
             lend = mlRelativeFramePoses.end();
@@ -3765,4 +3765,4 @@ void Tracking::Release() {
 }
 #endif
 
-}  // namespace ORB_SLAM3
+}  // namespace MORB_SLAM
diff --git a/src/TwoViewReconstruction.cc b/src/TwoViewReconstruction.cc
index 36f603ae..26f97968 100644
--- a/src/TwoViewReconstruction.cc
+++ b/src/TwoViewReconstruction.cc
@@ -19,16 +19,16 @@
  * ORB-SLAM3. If not, see <http://www.gnu.org/licenses/>.
  */
 
-#include "TwoViewReconstruction.h"
+#include "MORB_SLAM/TwoViewReconstruction.h"
 
 #include <thread>
 
-#include "Converter.h"
+#include "MORB_SLAM/Converter.h"
 #include "DUtils/Random.h"
-#include "GeometricTools.h"
+#include "MORB_SLAM/GeometricTools.h"
 
 using namespace std;
-namespace ORB_SLAM3 {
+namespace MORB_SLAM {
 TwoViewReconstruction::TwoViewReconstruction(const Eigen::Matrix3f &k,
                                              float sigma, int iterations) {
   mK = k;
@@ -904,4 +904,4 @@ void TwoViewReconstruction::DecomposeE(const Eigen::Matrix3f &E,
   if (R2.determinant() < 0) R2 = -R2;
 }
 
-}  // namespace ORB_SLAM3
+}  // namespace MORB_SLAM
diff --git a/src/Viewer.cc b/src/Viewer.cc
index d7a1fa55..76c09cf5 100644
--- a/src/Viewer.cc
+++ b/src/Viewer.cc
@@ -19,22 +19,22 @@
  * ORB-SLAM3. If not, see <http://www.gnu.org/licenses/>.
  */
 
-#include "Viewer.h"
+#include "MORB_SLAM/Viewer.h"
 
 #include <pangolin/pangolin.h>
 
-#include "ImprovedTypes.hpp"
+#include "MORB_SLAM/ImprovedTypes.hpp"
 #include <chrono>
 #include <ctime>
 #include <mutex>
 #include <stdexcept>
 #include <iostream>
 #include <string>
-#include "System.h"
-#include "Atlas.h"
-#include "Tracking.h"
+#include "MORB_SLAM/System.h"
+#include "MORB_SLAM/Atlas.h"
+#include "MORB_SLAM/Tracking.h"
 
-namespace ORB_SLAM3 {
+namespace MORB_SLAM {
 
 void Viewer::setBoth(const bool b){
   both = true;
@@ -367,4 +367,4 @@ void Viewer::close(){
   mbClosed = true;
 }
 
-}  // namespace ORB_SLAM3
+}  // namespace MORB_SLAM

From 9cefbe994075304e9ce2b0ee35c7d0f617a524e0 Mon Sep 17 00:00:00 2001
From: Soldann <guotl321@gmail.com>
Date: Mon, 8 Aug 2022 09:33:52 -1000
Subject: [PATCH 25/26] revert header files from merge Co-authored: ethanseq
 <sequeira.ethan@gmail.com>

---
 include/Atlas.h                        | 163 -----
 include/Camera.hpp                     |  92 ---
 include/CameraModels/GeometricCamera.h | 115 ----
 include/CameraModels/KannalaBrandt8.h  | 134 ----
 include/CameraModels/Pinhole.h         | 110 ----
 include/Converter.h                    |  78 ---
 include/Frame.h                        | 376 -----------
 include/FrameDrawer.h                  |  71 ---
 include/G2oTypes.h                     | 844 -------------------------
 include/GeometricTools.h               |  81 ---
 include/ImprovedTypes.hpp              |  51 --
 include/ImuTypes.h                     | 272 --------
 include/KeyFrame.h                     | 542 ----------------
 include/KeyFrameDatabase.h             |  99 ---
 include/LocalMapping.h                 | 198 ------
 include/LoopClosing.h                  | 245 -------
 include/MLPnPsolver.h                  | 243 -------
 include/Map.h                          | 205 ------
 include/MapDrawer.h                    |  73 ---
 include/MapPoint.h                     | 253 --------
 include/ORBVocabulary.h                |  32 -
 include/ORBextractor.h                 | 102 ---
 include/ORBmatcher.h                   | 131 ----
 include/OptimizableTypes.h             | 233 -------
 include/Optimizer.h                    | 141 -----
 include/SerializationUtils.h           | 154 -----
 include/Settings.h                     | 239 -------
 include/Sim3Solver.h                   | 137 ----
 include/System.h                       | 245 -------
 include/Tracking.h                     | 364 -----------
 include/TwoViewReconstruction.h        |  96 ---
 include/Viewer.h                       |  77 ---
 32 files changed, 6196 deletions(-)
 delete mode 100644 include/Atlas.h
 delete mode 100644 include/Camera.hpp
 delete mode 100644 include/CameraModels/GeometricCamera.h
 delete mode 100644 include/CameraModels/KannalaBrandt8.h
 delete mode 100644 include/CameraModels/Pinhole.h
 delete mode 100644 include/Converter.h
 delete mode 100644 include/Frame.h
 delete mode 100644 include/FrameDrawer.h
 delete mode 100644 include/G2oTypes.h
 delete mode 100644 include/GeometricTools.h
 delete mode 100644 include/ImprovedTypes.hpp
 delete mode 100644 include/ImuTypes.h
 delete mode 100644 include/KeyFrame.h
 delete mode 100644 include/KeyFrameDatabase.h
 delete mode 100644 include/LocalMapping.h
 delete mode 100644 include/LoopClosing.h
 delete mode 100644 include/MLPnPsolver.h
 delete mode 100644 include/Map.h
 delete mode 100644 include/MapDrawer.h
 delete mode 100644 include/MapPoint.h
 delete mode 100644 include/ORBVocabulary.h
 delete mode 100644 include/ORBextractor.h
 delete mode 100644 include/ORBmatcher.h
 delete mode 100644 include/OptimizableTypes.h
 delete mode 100644 include/Optimizer.h
 delete mode 100644 include/SerializationUtils.h
 delete mode 100644 include/Settings.h
 delete mode 100644 include/Sim3Solver.h
 delete mode 100644 include/System.h
 delete mode 100644 include/Tracking.h
 delete mode 100644 include/TwoViewReconstruction.h
 delete mode 100644 include/Viewer.h

diff --git a/include/Atlas.h b/include/Atlas.h
deleted file mode 100644
index 0eb4efeb..00000000
--- a/include/Atlas.h
+++ /dev/null
@@ -1,163 +0,0 @@
-/**
- * This file is part of ORB-SLAM3
- *
- * Copyright (C) 2017-2021 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/>.
- */
-
-#pragma once
-
-#include <boost/serialization/export.hpp>
-#include <boost/serialization/vector.hpp>
-#include <boost/serialization/shared_ptr.hpp>
-#include <mutex>
-#include <set>
-#include <map>
-#include <vector>
-
-#include "GeometricCamera.h"
-#include "KannalaBrandt8.h"
-#include "KeyFrame.h"
-#include "Map.h"
-#include "MapPoint.h"
-#include "Pinhole.h"
-
-namespace MORB_SLAM {
-class Map;
-class MapPoint;
-class KeyFrame;
-class KeyFrameDatabase;
-class Frame;
-class KannalaBrandt8;
-class Pinhole;
-
-// BOOST_CLASS_EXPORT_GUID(Pinhole, "Pinhole")
-// BOOST_CLASS_EXPORT_GUID(KannalaBrandt8, "KannalaBrandt8")
-
-class Atlas {
-  friend class boost::serialization::access;
-
-  template <class Archive>
-  void serialize(Archive& ar, const unsigned int version) {
-    ar.template register_type<Pinhole>();
-    ar.template register_type<KannalaBrandt8>();
-
-    // Save/load a set structure, the set structure is broken in libboost 1.58
-    // for ubuntu 16.04, a vector is serializated
-    // ar & mspMaps;
-    ar& mvpBackupMaps;
-    ar& mvpCameras;
-    // Need to save/load the static Id from Frame, KeyFrame, MapPoint and Map
-    ar& Map::nNextId;
-    ar& Frame::nNextId;
-    ar& KeyFrame::nNextId;
-    ar& MapPoint::nNextId;
-    ar& GeometricCamera::nNextId;
-    ar& mnLastInitKFidMap;
-  }
-
- public:
-  
-
-  Atlas();
-  Atlas(int initKFid);  // When its initialization the first map is created
-  ~Atlas();
-
-  void CreateNewMap();
-  void ChangeMap(std::shared_ptr<Map> pMap);
-
-  unsigned long int GetLastInitKFid();
-
-  // Method for change components in the current map
-  void AddKeyFrame(KeyFrame* pKF);
-  void AddMapPoint(MapPoint* pMP);
-  // void EraseMapPoint(MapPoint* pMP);
-  // void EraseKeyFrame(KeyFrame* pKF);
-
-  std::shared_ptr<GeometricCamera> AddCamera(const std::shared_ptr<GeometricCamera> &pCam);
-  std::vector<std::shared_ptr<GeometricCamera>> GetAllCameras();
-
-  /* All methods without Map pointer work on current map */
-  void SetReferenceMapPoints(const std::vector<MapPoint*>& vpMPs);
-  void InformNewBigChange();
-  int GetLastBigChangeIdx();
-
-  long unsigned int MapPointsInMap();
-  long unsigned KeyFramesInMap();
-
-  // Method for get data in current map
-  std::vector<KeyFrame*> GetAllKeyFrames();
-  std::vector<MapPoint*> GetAllMapPoints();
-  std::vector<MapPoint*> GetReferenceMapPoints();
-
-  std::vector<std::shared_ptr<Map>> GetAllMaps();
-
-  int CountMaps();
-
-  void clearMap();
-
-  void clearAtlas();
-
-  std::shared_ptr<Map> GetCurrentMap(System* sys = nullptr);
-
-  void SetMapBad(std::shared_ptr<Map> pMap);
-  void RemoveBadMaps();
-
-  bool isInertial();
-  void SetInertialSensor();
-  void SetImuInitialized();
-  bool isImuInitialized();
-
-  // Function for garantee the correction of serialization of this object
-  void PreSave();
-  void PostLoad();
-
-  std::map<long unsigned int, KeyFrame*> GetAtlasKeyframes();
-
-  void SetKeyFrameDababase(KeyFrameDatabase* pKFDB);
-  KeyFrameDatabase* GetKeyFrameDatabase();
-
-  void SetORBVocabulary(ORBVocabulary* pORBVoc);
-  ORBVocabulary* GetORBVocabulary();
-
-  long unsigned int GetNumLivedKF();
-
-  long unsigned int GetNumLivedMP();
-
- protected:
-  std::set<std::shared_ptr<Map>> mspMaps;
-  std::set<std::shared_ptr<Map>> mspBadMaps;
-  // Its necessary change the container from set to vector because libboost 1.58
-  // and Ubuntu 16.04 have an error with this cointainer
-  std::vector<std::shared_ptr<Map>> mvpBackupMaps;
-
-  std::shared_ptr<Map> mpCurrentMap;
-
-  std::vector<std::shared_ptr<GeometricCamera>> mvpCameras;
-
-  unsigned long int mnLastInitKFidMap;
-
-  // Class references for the map reconstruction from the save file
-  KeyFrameDatabase* mpKeyFrameDB;
-  ORBVocabulary* mpORBVocabulary;
-
-  // Mutex
-  std::mutex mMutexAtlas;
-
-};  // class Atlas
-
-}  // namespace MORB_SLAM
diff --git a/include/Camera.hpp b/include/Camera.hpp
deleted file mode 100644
index 7d94ca1e..00000000
--- a/include/Camera.hpp
+++ /dev/null
@@ -1,92 +0,0 @@
-#pragma once
-#include <thread>
-#include <string>
-#include "ImprovedTypes.hpp"
-#include <mutex>
-#include <condition_variable>
-#include <future>
-#include <functional>
-#include <memory>
-#include <exception>
-#include <stdexcept>
-#include <deque>
-#include <utility>
-
-namespace MORB_SLAM{
-
-
-template<typename T>
-class ManagedFuture{
-protected:
-    std::shared_ptr<std::promise<T>> promise;
-    std::shared_future<T> future;
-
-   ManagedFuture(): promise{std::make_shared<std::promise<T>>()}, future{promise->get_future()} {} // make default constructor protected
-public:
-    virtual ~ManagedFuture(){}
-
-    // functions from std::shared_future<T>
-
-    const T& get() const { return future.get(); }
-    void wait() const { future.wait(); }
-
-    template<class Rep, class Period>
-    std::future_status wait_for( const std::chrono::duration<Rep,Period>& timeout_duration ) const { return future.wait_for(timeout_duration); }
-
-    template<class Clock, class Duration>
-    std::future_status wait_until( const std::chrono::time_point<Clock,Duration>& timeout_time ) const { return future.wait_until(timeout_time); }
-};
-
-template<typename T>
-class ManagedPromise: public ManagedFuture<T>{
-    std::shared_ptr<std::mutex> mutex;
-public:
-    ManagedPromise(): mutex{std::make_shared<std::mutex>()} {}
-    virtual ~ManagedPromise(){}
-    
-    // functions from std::promise<T>
-
-    void set_value_at_thread_exit(const T& value){ std::scoped_lock<std::mutex> lock(*mutex); ManagedFuture<T>::promise->set_value_at_thread_exit(value); }
-    void set_value_at_thread_exit(T&& value){ std::scoped_lock<std::mutex> lock(*mutex); ManagedFuture<T>::promise->set_value_at_thread_exit(value); }
-    void set_value_at_thread_exit(T& value){ std::scoped_lock<std::mutex> lock(*mutex); ManagedFuture<T>::promise->set_value_at_thread_exit(value); }
-    void set_value_at_thread_exit(){ std::scoped_lock<std::mutex> lock(*mutex); ManagedFuture<T>::promise->set_value_at_thread_exit(); }
-    void set_exception_at_thread_exit(const std::exception_ptr &p ){ std::scoped_lock<std::mutex> lock(*mutex); ManagedFuture<T>::promise->set_exception_at_thread_exit(p); }
-    void set_exception(const std::exception_ptr &p ){ std::scoped_lock<std::mutex> lock(*mutex); ManagedFuture<T>::promise->set_exception(p); }
-    void set_value(const T& value){ std::scoped_lock<std::mutex> lock(*mutex); ManagedFuture<T>::promise->set_value(value); }
-    void set_value(T&& value){ std::scoped_lock<std::mutex> lock(*mutex); ManagedFuture<T>::promise->set_value(value); }
-    void set_value(T& value){ std::scoped_lock<std::mutex> lock(*mutex); ManagedFuture<T>::promise->set_value(value); }
-    void set_value(){ std::scoped_lock<std::mutex> lock(*mutex); ManagedFuture<T>::promise->set_value(); }
-};
-
-class Camera{
-    std::deque<std::pair<ManagedPromise<bool>, std::function<void(void)>>> ljobs;
-    std::deque<std::pair<ManagedPromise<bool>, std::function<void(void)>>> rjobs;
-
-    std::string name;
-    CameraType::eSensor type;
-    std::mutex camMutex;
-    
-    std::condition_variable camCV;
-
-    bool shouldStop;
-    
-    std::thread lthread;
-    std::thread rthread;
-
-    void threadExec(std::deque<std::pair<ManagedPromise<bool>, std::function<void(void)>>> *jobs);
-public:
-    Camera(CameraType::eSensor type, const std::string &name="camera");
-    virtual ~Camera();
-
-    ManagedFuture<bool> queue(std::function<void(void)> func, bool isLeft=true);
-    ManagedFuture<bool> queueLeft(const std::function<void(void)> &func);
-    ManagedFuture<bool> queueRight(const std::function<void(void)> &func);
-
-    CameraType::eSensor getType() const;
-    const std::string &getName() const;
-};
-
-typedef std::shared_ptr<Camera> Camera_ptr;
-typedef std::weak_ptr<Camera> Camera_wptr;
-
-} // namespace MORB_SLAM
\ No newline at end of file
diff --git a/include/CameraModels/GeometricCamera.h b/include/CameraModels/GeometricCamera.h
deleted file mode 100644
index 0726cf90..00000000
--- a/include/CameraModels/GeometricCamera.h
+++ /dev/null
@@ -1,115 +0,0 @@
-/**
- * This file is part of ORB-SLAM3
- *
- * Copyright (C) 2017-2021 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/>.
- */
-
-#pragma once
-
-#include <Eigen/Geometry>
-#include <boost/serialization/access.hpp>
-#include <boost/serialization/assume_abstract.hpp>
-#include <boost/serialization/base_object.hpp>
-#include <boost/serialization/export.hpp>
-#include <boost/serialization/serialization.hpp>
-#include <boost/serialization/vector.hpp>
-#include <opencv2/core/core.hpp>
-#include <opencv2/features2d/features2d.hpp>
-#include <opencv2/imgproc/imgproc.hpp>
-#include <sophus/se3.hpp>
-#include <vector>
-
-#include "Converter.h"
-#include "GeometricTools.h"
-
-namespace MORB_SLAM {
-class GeometricCamera {
-  friend class boost::serialization::access;
-
-  template <class Archive>
-  void serialize(Archive& ar, const unsigned int version) {
-    ar& mnId;
-    ar& mnType;
-    ar& mvParameters;
-  }
-
- public:
-  GeometricCamera() {}
-  GeometricCamera(const std::vector<float>& _vParameters)
-      : mvParameters(_vParameters) {}
-  ~GeometricCamera() {}
-
-  virtual cv::Point2f project(const cv::Point3f& p3D) const = 0;
-  virtual Eigen::Vector2d project(const Eigen::Vector3d& v3D) const = 0;
-  virtual Eigen::Vector2f project(const Eigen::Vector3f& v3D) const = 0;
-  virtual Eigen::Vector2f projectMat(const cv::Point3f& p3D) const = 0;
-
-  virtual float uncertainty2(const Eigen::Matrix<double, 2, 1>& p2D) = 0;
-
-  virtual Eigen::Vector3f unprojectEig(const cv::Point2f& p2D) const = 0;
-  virtual cv::Point3f unproject(const cv::Point2f& p2D) const = 0;
-
-  virtual Eigen::Matrix<double, 2, 3> projectJac(
-      const Eigen::Vector3d& v3D) = 0;
-
-  virtual bool ReconstructWithTwoViews(const std::vector<cv::KeyPoint>& vKeys1,
-                                       const std::vector<cv::KeyPoint>& vKeys2,
-                                       const std::vector<int>& vMatches12,
-                                       Sophus::SE3f& T21,
-                                       std::vector<cv::Point3f>& vP3D,
-                                       std::vector<bool>& vbTriangulated) = 0;
-
-  virtual cv::Mat toK() const = 0;
-  virtual Eigen::Matrix3f toK_() const = 0;
-
-  virtual bool epipolarConstrain(const std::shared_ptr<GeometricCamera> &otherCamera,
-                                 const cv::KeyPoint& kp1,
-                                 const cv::KeyPoint& kp2,
-                                 const Eigen::Matrix3f& R12,
-                                 const Eigen::Vector3f& t12,
-                                 const float sigmaLevel, const float unc) = 0;
-
-  float getParameter(const int i) { return mvParameters[i]; }
-  void setParameter(const float p, const size_t i) { mvParameters[i] = p; }
-
-  size_t size() { return mvParameters.size(); }
-
-  virtual bool matchAndtriangulate(const cv::KeyPoint& kp1,
-                                   const cv::KeyPoint& kp2,
-                                   GeometricCamera* pOther, Sophus::SE3f& Tcw1,
-                                   Sophus::SE3f& Tcw2, const float sigmaLevel1,
-                                   const float sigmaLevel2,
-                                   Eigen::Vector3f& x3Dtriangulated) = 0;
-
-  unsigned int GetId() { return mnId; }
-
-  unsigned int GetType() { return mnType; }
-
-  const static unsigned int CAM_PINHOLE = 0;
-  const static unsigned int CAM_FISHEYE = 1;
-
-  static long unsigned int nNextId;
-
- protected:
-  std::vector<float> mvParameters;
-
-  unsigned int mnId;
-
-  unsigned int mnType;
-};
-}  // namespace MORB_SLAM
diff --git a/include/CameraModels/KannalaBrandt8.h b/include/CameraModels/KannalaBrandt8.h
deleted file mode 100644
index ae6194c7..00000000
--- a/include/CameraModels/KannalaBrandt8.h
+++ /dev/null
@@ -1,134 +0,0 @@
-/**
- * This file is part of ORB-SLAM3
- *
- * Copyright (C) 2017-2021 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/>.
- */
-
-#pragma once
-
-#include <assert.h>
-#include <memory>
-#include "GeometricCamera.h"
-#include "TwoViewReconstruction.h"
-
-namespace MORB_SLAM {
-class KannalaBrandt8 : public GeometricCamera {
-  friend class boost::serialization::access;
-
-  template <class Archive>
-  void serialize(Archive& ar, const unsigned int version) {
-    ar& boost::serialization::base_object<GeometricCamera>(*this);
-    ar& const_cast<float&>(precision);
-  }
-
- public:
-  KannalaBrandt8() : precision(1e-6) {
-    mvParameters.resize(8);
-    mnId = nNextId++;
-    mnType = CAM_FISHEYE;
-  }
-  KannalaBrandt8(const std::vector<float> _vParameters)
-      : GeometricCamera(_vParameters),
-        mvLappingArea(2, 0),
-        precision(1e-6),
-        tvr(nullptr) {
-    assert(mvParameters.size() == 8);
-    mnId = nNextId++;
-    mnType = CAM_FISHEYE;
-  }
-
-  KannalaBrandt8(const std::vector<float> _vParameters, const float _precision)
-      : GeometricCamera(_vParameters),
-        mvLappingArea(2, 0),
-        precision(_precision) {
-    assert(mvParameters.size() == 8);
-    mnId = nNextId++;
-    mnType = CAM_FISHEYE;
-  }
-  KannalaBrandt8(KannalaBrandt8* pKannala)
-      : GeometricCamera(pKannala->mvParameters),
-        mvLappingArea(2, 0),
-        precision(pKannala->precision),
-        tvr(nullptr) {
-    assert(mvParameters.size() == 8);
-    mnId = nNextId++;
-    mnType = CAM_FISHEYE;
-  }
-
-  cv::Point2f project(const cv::Point3f& p3D) const;
-  Eigen::Vector2d project(const Eigen::Vector3d& v3D) const;
-  Eigen::Vector2f project(const Eigen::Vector3f& v3D) const;
-  Eigen::Vector2f projectMat(const cv::Point3f& p3D) const;
-
-  float uncertainty2(const Eigen::Matrix<double, 2, 1>& p2D);
-
-  Eigen::Vector3f unprojectEig(const cv::Point2f& p2D) const;
-  cv::Point3f unproject(const cv::Point2f& p2D) const;
-
-  Eigen::Matrix<double, 2, 3> projectJac(const Eigen::Vector3d& v3D);
-
-  bool ReconstructWithTwoViews(const std::vector<cv::KeyPoint>& vKeys1,
-                               const std::vector<cv::KeyPoint>& vKeys2,
-                               const std::vector<int>& vMatches12,
-                               Sophus::SE3f& T21,
-                               std::vector<cv::Point3f>& vP3D,
-                               std::vector<bool>& vbTriangulated);
-
-  cv::Mat toK() const;
-  Eigen::Matrix3f toK_() const;
-
-  bool epipolarConstrain(const std::shared_ptr<GeometricCamera> &pCamera2, const cv::KeyPoint& kp1,
-                         const cv::KeyPoint& kp2, const Eigen::Matrix3f& R12,
-                         const Eigen::Vector3f& t12, const float sigmaLevel,
-                         const float unc);
-
-  float TriangulateMatches(const std::shared_ptr<GeometricCamera> &pCamera2, const cv::KeyPoint& kp1,
-                           const cv::KeyPoint& kp2, const Eigen::Matrix3f& R12,
-                           const Eigen::Vector3f& t12, const float sigmaLevel,
-                           const float unc, Eigen::Vector3f& p3D);
-
-  std::vector<int> mvLappingArea;
-
-  bool matchAndtriangulate(const cv::KeyPoint& kp1, const cv::KeyPoint& kp2,
-                           GeometricCamera* pOther, Sophus::SE3f& Tcw1,
-                           Sophus::SE3f& Tcw2, const float sigmaLevel1,
-                           const float sigmaLevel2,
-                           Eigen::Vector3f& x3Dtriangulated);
-
-  friend std::ostream& operator<<(std::ostream& os, const KannalaBrandt8& kb);
-  friend std::istream& operator>>(std::istream& is, KannalaBrandt8& kb);
-
-  float GetPrecision() { return precision; }
-
-  bool IsEqual(const std::shared_ptr<GeometricCamera> &pCam);
-
- private:
-  const float precision;
-
-  // Parameters vector corresponds to
-  //[fx, fy, cx, cy, k0, k1, k2, k3]
-
-  TwoViewReconstruction* tvr;
-
-  void Triangulate(const cv::Point2f& p1, const cv::Point2f& p2,
-                   const Eigen::Matrix<float, 3, 4>& Tcw1,
-                   const Eigen::Matrix<float, 3, 4>& Tcw2,
-                   Eigen::Vector3f& x3D);
-};
-}  // namespace MORB_SLAM
-
diff --git a/include/CameraModels/Pinhole.h b/include/CameraModels/Pinhole.h
deleted file mode 100644
index 48112ce8..00000000
--- a/include/CameraModels/Pinhole.h
+++ /dev/null
@@ -1,110 +0,0 @@
-/**
- * This file is part of ORB-SLAM3
- *
- * Copyright (C) 2017-2021 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/>.
- */
-
-#pragma once
-
-#include <assert.h>
-#include <memory>
-#include "GeometricCamera.h"
-#include "TwoViewReconstruction.h"
-
-namespace MORB_SLAM {
-class Pinhole : public GeometricCamera {
-  friend class boost::serialization::access;
-
-  template <class Archive>
-  void serialize(Archive& ar, const unsigned int version) {
-    ar& boost::serialization::base_object<GeometricCamera>(*this);
-  }
-
- public:
-  Pinhole() {
-    mvParameters.resize(4);
-    mnId = nNextId++;
-    mnType = CAM_PINHOLE;
-  }
-  Pinhole(const std::vector<float> _vParameters)
-      : GeometricCamera(_vParameters), tvr(nullptr) {
-    assert(mvParameters.size() == 4);
-    mnId = nNextId++;
-    mnType = CAM_PINHOLE;
-  }
-
-  Pinhole(Pinhole* pPinhole)
-      : GeometricCamera(pPinhole->mvParameters), tvr(nullptr) {
-    assert(mvParameters.size() == 4);
-    mnId = nNextId++;
-    mnType = CAM_PINHOLE;
-  }
-
-  ~Pinhole() {
-    if (tvr) delete tvr;
-  }
-
-  cv::Point2f project(const cv::Point3f& p3D) const;
-  Eigen::Vector2d project(const Eigen::Vector3d& v3D) const;
-  Eigen::Vector2f project(const Eigen::Vector3f& v3D) const;
-  Eigen::Vector2f projectMat(const cv::Point3f& p3D) const;
-
-  float uncertainty2(const Eigen::Matrix<double, 2, 1>& p2D);
-
-  Eigen::Vector3f unprojectEig(const cv::Point2f& p2D) const;
-  cv::Point3f unproject(const cv::Point2f& p2D) const;
-
-  Eigen::Matrix<double, 2, 3> projectJac(const Eigen::Vector3d& v3D);
-
-  bool ReconstructWithTwoViews(const std::vector<cv::KeyPoint>& vKeys1,
-                               const std::vector<cv::KeyPoint>& vKeys2,
-                               const std::vector<int>& vMatches12,
-                               Sophus::SE3f& T21,
-                               std::vector<cv::Point3f>& vP3D,
-                               std::vector<bool>& vbTriangulated);
-
-  cv::Mat toK() const;
-  Eigen::Matrix3f toK_()const;
-
-  bool epipolarConstrain(const std::shared_ptr<GeometricCamera> &pCamera2, const cv::KeyPoint& kp1,
-                         const cv::KeyPoint& kp2, const Eigen::Matrix3f& R12,
-                         const Eigen::Vector3f& t12, const float sigmaLevel,
-                         const float unc);
-
-  bool matchAndtriangulate(const cv::KeyPoint& kp1, const cv::KeyPoint& kp2,
-                           GeometricCamera* pOther, Sophus::SE3f& Tcw1,
-                           Sophus::SE3f& Tcw2, const float sigmaLevel1,
-                           const float sigmaLevel2,
-                           Eigen::Vector3f& x3Dtriangulated) {
-    return false;
-  }
-
-  friend std::ostream& operator<<(std::ostream& os, const Pinhole& ph);
-  friend std::istream& operator>>(std::istream& os, Pinhole& ph);
-
-  bool IsEqual(const std::shared_ptr<GeometricCamera> &pCam);
-
- private:
-  // Parameters vector corresponds to
-  //      [fx, fy, cx, cy]
-  TwoViewReconstruction* tvr;
-};
-}  // namespace MORB_SLAM
-
-// BOOST_CLASS_EXPORT_KEY(ORBSLAM2::Pinhole)
-
diff --git a/include/Converter.h b/include/Converter.h
deleted file mode 100644
index 2f162d47..00000000
--- a/include/Converter.h
+++ /dev/null
@@ -1,78 +0,0 @@
-/**
- * This file is part of ORB-SLAM3
- *
- * Copyright (C) 2017-2021 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/>.
- */
-
-#pragma once
-
-#include <Eigen/Dense>
-#include <opencv2/core/core.hpp>
-
-#include "g2o/types/types_seven_dof_expmap.h"
-#include "g2o/types/types_six_dof_expmap.h"
-#include "sophus/geometry.hpp"
-#include "sophus/sim3.hpp"
-
-namespace MORB_SLAM {
-
-class Converter {
- public:
-  
-  static std::vector<cv::Mat> toDescriptorVector(const cv::Mat &Descriptors);
-
-  static g2o::SE3Quat toSE3Quat(const cv::Mat &cvT);
-  static g2o::SE3Quat toSE3Quat(const Sophus::SE3f &T);
-  static g2o::SE3Quat toSE3Quat(const g2o::Sim3 &gSim3);
-
-  // TODO templetize these functions
-  static cv::Mat toCvMat(const g2o::SE3Quat &SE3);
-  static cv::Mat toCvMat(const g2o::Sim3 &Sim3);
-  static cv::Mat toCvMat(const Eigen::Matrix<double, 4, 4> &m);
-  static cv::Mat toCvMat(const Eigen::Matrix<float, 4, 4> &m);
-  static cv::Mat toCvMat(const Eigen::Matrix<float, 3, 4> &m);
-  static cv::Mat toCvMat(const Eigen::Matrix3d &m);
-  static cv::Mat toCvMat(const Eigen::Matrix<double, 3, 1> &m);
-  static cv::Mat toCvMat(const Eigen::Matrix<float, 3, 1> &m);
-  static cv::Mat toCvMat(const Eigen::Matrix<float, 3, 3> &m);
-
-  static cv::Mat toCvMat(const Eigen::MatrixXf &m);
-  static cv::Mat toCvMat(const Eigen::MatrixXd &m);
-
-  static cv::Mat toCvSE3(const Eigen::Matrix<double, 3, 3> &R,
-                         const Eigen::Matrix<double, 3, 1> &t);
-  static cv::Mat tocvSkewMatrix(const cv::Mat &v);
-
-  static Eigen::Matrix<double, 3, 1> toVector3d(const cv::Mat &cvVector);
-  static Eigen::Matrix<float, 3, 1> toVector3f(const cv::Mat &cvVector);
-  static Eigen::Matrix<double, 3, 1> toVector3d(const cv::Point3f &cvPoint);
-  static Eigen::Matrix<double, 3, 3> toMatrix3d(const cv::Mat &cvMat3);
-  static Eigen::Matrix<double, 4, 4> toMatrix4d(const cv::Mat &cvMat4);
-  static Eigen::Matrix<float, 3, 3> toMatrix3f(const cv::Mat &cvMat3);
-  static Eigen::Matrix<float, 4, 4> toMatrix4f(const cv::Mat &cvMat4);
-  static std::vector<float> toQuaternion(const cv::Mat &M);
-
-  static bool isRotationMatrix(const cv::Mat &R);
-  static std::vector<float> toEuler(const cv::Mat &R);
-
-  // TODO: Sophus migration, to be deleted in the future
-  static Sophus::SE3<float> toSophus(const cv::Mat &T);
-  static Sophus::Sim3f toSophus(const g2o::Sim3 &S);
-};
-
-}  // namespace MORB_SLAM
diff --git a/include/Frame.h b/include/Frame.h
deleted file mode 100644
index 0b1c5f15..00000000
--- a/include/Frame.h
+++ /dev/null
@@ -1,376 +0,0 @@
-/**
-* This file is part of ORB-SLAM3
-*
-* Copyright (C) 2017-2021 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/>.
-*/
-
-
-#pragma once
-
-#include<vector>
-
-#include "DBoW2/BowVector.h"
-#include "DBoW2/FeatureVector.h"
-
-#include "sophus/geometry.hpp"
-
-#include "ImuTypes.h"
-#include "ORBVocabulary.h"
-
-#include "Converter.h"
-#include "Settings.h"
-
-#include <mutex>
-#include <opencv2/opencv.hpp>
-
-#include "Eigen/Core"
-#include "sophus/se3.hpp"
-
-#include "Camera.hpp"
-
-namespace MORB_SLAM
-{
-#define FRAME_GRID_ROWS 48
-#define FRAME_GRID_COLS 64
-
-class MapPoint;
-class KeyFrame;
-class ConstraintPoseImu;
-class GeometricCamera;
-class ORBextractor;
-
-class Frame
-{
-public:
-    Frame();
-
-    // Copy constructor.
-    Frame(const Frame &frame);
-
-    // Constructor for stereo cameras.
-    Frame(const Camera_ptr &cam, const cv::Mat &imLeft, const cv::Mat &imRight, const double &timeStamp, const std::shared_ptr<ORBextractor> &extractorLeft, const std::shared_ptr<ORBextractor> &extractorRight, ORBVocabulary* voc, cv::Mat &K, cv::Mat &distCoef, const float &bf, const float &thDepth, const std::shared_ptr<GeometricCamera> &pCamera, const std::string &pNameFile, int pnNumDataset, Frame* pPrevF = nullptr, const IMU::Calib &ImuCalib = IMU::Calib());
-
-    // Constructor for RGB-D cameras.
-    Frame(const Camera_ptr &cam, const cv::Mat &imGray, const cv::Mat &imDepth, const double &timeStamp, const std::shared_ptr<ORBextractor> &extractor,ORBVocabulary* voc, cv::Mat &K, cv::Mat &distCoef, const float &bf, const float &thDepth, const std::shared_ptr<GeometricCamera> &pCamera, const std::string &pNameFile, int pnNumDataset, Frame* pPrevF = nullptr, const IMU::Calib &ImuCalib = IMU::Calib());
-
-    // Constructor for Monocular cameras.
-    Frame(const Camera_ptr &cam, const cv::Mat &imGray, const double &timeStamp, const std::shared_ptr<ORBextractor> &extractor,ORBVocabulary* voc, const std::shared_ptr<GeometricCamera> &pCamera, cv::Mat &distCoef, const float &bf, const float &thDepth, const std::string &pNameFile, int pnNumDataset,  Frame* pPrevF = nullptr, const IMU::Calib &ImuCalib = IMU::Calib());
-
-    // Destructor
-    virtual ~Frame();
-
-    // Extract ORB on the image. 0 for left image and 1 for right image.
-    void ExtractORB(bool isLeft, const cv::Mat &im, const int x0, const int x1);
-
-    // Compute Bag of Words representation.
-    void ComputeBoW();
-
-    // Set the camera pose. (Imu pose is not modified!)
-    void SetPose(const Sophus::SE3<float> &Tcw);
-
-    // Set IMU velocity
-    void SetVelocity(Eigen::Vector3f Vw);
-
-    Eigen::Vector3f GetVelocity() const;
-
-    // Set IMU pose and velocity (implicitly changes camera pose)
-    void SetImuPoseVelocity(const Eigen::Matrix3f &Rwb, const Eigen::Vector3f &twb, const Eigen::Vector3f &Vwb);
-
-    Eigen::Matrix<float,3,1> GetImuPosition() const;
-    Eigen::Matrix<float,3,3> GetImuRotation();
-    Sophus::SE3<float> GetImuPose();
-
-    Sophus::SE3f GetRelativePoseTrl();
-    Sophus::SE3f GetRelativePoseTlr();
-    Eigen::Matrix3f GetRelativePoseTlr_rotation();
-    Eigen::Vector3f GetRelativePoseTlr_translation();
-
-    void SetNewBias(const IMU::Bias &b);
-
-    // Check if a MapPoint is in the frustum of the camera
-    // and fill variables of the MapPoint to be used by the tracking
-    bool isInFrustum(MapPoint* pMP, float viewingCosLimit);
-
-    bool ProjectPointDistort(MapPoint* pMP, cv::Point2f &kp, float &u, float &v);
-
-    Eigen::Vector3f inRefCoordinates(Eigen::Vector3f pCw);
-
-    // Compute the cell of a keypoint (return false if outside the grid)
-    bool PosInGrid(const cv::KeyPoint &kp, int &posX, int &posY);
-
-    vector<size_t> GetFeaturesInArea(const float &x, const float  &y, const float  &r, const int minLevel=-1, const int maxLevel=-1, const bool bRight = false) const;
-
-    // Search a match for each keypoint in the left image to a keypoint in the right image.
-    // If there is a match, depth is computed and the right coordinate associated to the left keypoint is stored.
-    void ComputeStereoMatches();
-
-    // Associate a "right" coordinate to a keypoint if there is valid depth in the depthmap.
-    void ComputeStereoFromRGBD(const cv::Mat &imDepth);
-
-    // Backprojects a keypoint (if stereo/depth info available) into 3D world coordinates.
-    bool UnprojectStereo(const int &i, Eigen::Vector3f &x3D);
-
-    std::shared_ptr<ConstraintPoseImu> mpcpi;
-
-    bool imuIsPreintegrated();
-    void setIntegrated();
-
-    bool isSet() const;
-
-    // Computes rotation, translation and camera center matrices from the camera pose.
-    void UpdatePoseMatrices();
-
-    // Returns the camera center.
-    inline Eigen::Vector3f GetCameraCenter(){
-        return mOw;
-    }
-
-    // Returns inverse of rotation
-    inline Eigen::Matrix3f GetRotationInverse(){
-        return mRwc;
-    }
-
-    inline Sophus::SE3<float> GetPose() const {
-        //TODO: can the Frame pose be accsessed from several threads? should this be protected somehow?
-        return mTcw;
-    }
-
-    inline Eigen::Matrix3f GetRwc() const {
-        return mRwc;
-    }
-
-    inline Eigen::Vector3f GetOw() const {
-        return mOw;
-    }
-
-    inline bool HasPose() const {
-        return mbHasPose;
-    }
-
-    inline bool HasVelocity() const {
-        return mbHasVelocity;
-    }
-
-
-
-private:
-    //Sophus/Eigen migration
-    Sophus::SE3<float> mTcw;
-    Eigen::Matrix<float,3,3> mRwc;
-    Eigen::Matrix<float,3,1> mOw;
-    Eigen::Matrix<float,3,3> mRcw;
-    Eigen::Matrix<float,3,1> mtcw;
-    bool mbHasPose;
-
-    //Rcw_ not necessary as Sophus has a method for extracting the rotation matrix: Tcw_.rotationMatrix()
-    //tcw_ not necessary as Sophus has a method for extracting the translation vector: Tcw_.translation()
-    //Twc_ not necessary as Sophus has a method for easily computing the inverse pose: Tcw_.inverse()
-
-    Sophus::SE3<float> mTlr, mTrl;
-    Eigen::Matrix<float,3,3> mRlr;
-    Eigen::Vector3f mtlr;
-
-
-    // IMU linear velocity
-    Eigen::Vector3f mVw;
-    bool mbHasVelocity;
-
-public:
-    
-
-    // Vocabulary used for relocalization.
-    ORBVocabulary* mpORBvocabulary;
-
-    // Feature extractor. The right is used only in the stereo case.
-    std::shared_ptr<ORBextractor> mpORBextractorLeft;
-    std::shared_ptr<ORBextractor> mpORBextractorRight;
-
-
-    // Frame timestamp.
-    double mTimeStamp;
-
-    // Calibration matrix and OpenCV distortion parameters.
-    cv::Mat mK;
-    Eigen::Matrix3f mK_;
-    static float fx;
-    static float fy;
-    static float cx;
-    static float cy;
-    static float invfx;
-    static float invfy;
-    cv::Mat mDistCoef;
-
-    // Stereo baseline multiplied by fx.
-    float mbf;
-
-    // Stereo baseline in meters.
-    float mb;
-
-    // Threshold close/far points. Close points are inserted from 1 view.
-    // Far points are inserted as in the monocular case from 2 views.
-    float mThDepth;
-
-    // Number of KeyPoints.
-    int N;
-
-    // Vector of keypoints (original for visualization) and undistorted (actually used by the system).
-    // In the stereo case, mvKeysUn is redundant as images must be rectified.
-    // In the RGB-D case, RGB images can be distorted.
-    std::vector<cv::KeyPoint> mvKeys, mvKeysRight;
-    std::vector<cv::KeyPoint> mvKeysUn;
-
-    // Corresponding stereo coordinate and depth for each keypoint.
-    std::vector<MapPoint*> mvpMapPoints;
-    // "Monocular" keypoints have a negative value.
-    std::vector<float> mvuRight;
-    std::vector<float> mvDepth;
-
-    // Bag of Words Vector structures.
-    DBoW2::BowVector mBowVec;
-    DBoW2::FeatureVector mFeatVec;
-
-    // ORB descriptor, each row associated to a keypoint.
-    cv::Mat mDescriptors, mDescriptorsRight;
-
-    // MapPoints associated to keypoints, nullptr pointer if no association.
-    // Flag to identify outlier associations.
-    std::vector<bool> mvbOutlier;
-    int mnCloseMPs;
-
-    // Keypoints are assigned to cells in a grid to reduce matching complexity when projecting MapPoints.
-    static float mfGridElementWidthInv;
-    static float mfGridElementHeightInv;
-    std::vector<std::size_t> mGrid[FRAME_GRID_COLS][FRAME_GRID_ROWS];
-
-    IMU::Bias mPredBias;
-
-    // IMU bias
-    IMU::Bias mImuBias;
-
-    // Imu calibration
-    IMU::Calib mImuCalib;
-
-    // Imu preintegration from last keyframe
-    IMU::Preintegrated* mpImuPreintegrated;
-    KeyFrame* mpLastKeyFrame;
-
-    // Pointer to previous frame
-    Frame* mpPrevFrame;
-    IMU::Preintegrated* mpImuPreintegratedFrame;
-
-    // Current and Next Frame id.
-    static long unsigned int nNextId;
-    long unsigned int mnId;
-
-    // Reference Keyframe.
-    KeyFrame* mpReferenceKF;
-
-    // Scale pyramid info.
-    int mnScaleLevels;
-    float mfScaleFactor;
-    float mfLogScaleFactor;
-    vector<float> mvScaleFactors;
-    vector<float> mvInvScaleFactors;
-    vector<float> mvLevelSigma2;
-    vector<float> mvInvLevelSigma2;
-
-    // Undistorted Image Bounds (computed once).
-    static float mnMinX;
-    static float mnMaxX;
-    static float mnMinY;
-    static float mnMaxY;
-
-    static bool mbInitialComputations;
-
-    map<long unsigned int, cv::Point2f> mmProjectPoints;
-    map<long unsigned int, cv::Point2f> mmMatchedInImage;
-
-    string mNameFile;
-
-    int mnDataset;
-
-#ifdef REGISTER_TIMES
-    double mTimeORB_Ext;
-    double mTimeStereoMatch;
-#endif
-
-private:
-
-    // Undistort keypoints given OpenCV distortion parameters.
-    // Only for the RGB-D case. Stereo must be already rectified!
-    // (called in the constructor).
-    void UndistortKeyPoints();
-
-    // Computes image bounds for the undistorted image (called in the constructor).
-    void ComputeImageBounds(const cv::Mat &imLeft);
-
-    // Assign keypoints to the grid for speed up feature matching (called in the constructor).
-    void AssignFeaturesToGrid();
-
-    bool mbIsSet;
-
-    bool mbImuPreintegrated;
-
-    std::shared_ptr<std::mutex> mpMutexImu;
-
-public:
-    Camera_ptr camera; 
-    std::shared_ptr<GeometricCamera> mpCamera, mpCamera2;
-
-    //Number of KeyPoints extracted in the left and right images
-    int Nleft, Nright;
-    //Number of Non Lapping Keypoints
-    int monoLeft, monoRight;
-
-    //For stereo matching
-    std::vector<int> mvLeftToRightMatch, mvRightToLeftMatch;
-
-    //For stereo fisheye matching
-    static cv::BFMatcher BFmatcher;
-
-    //Triangulated stereo observations using as reference the left camera. These are
-    //computed during ComputeStereoFishEyeMatches
-    std::vector<Eigen::Vector3f> mvStereo3Dpoints;
-
-    //Grid for the right image
-    std::vector<std::size_t> mGridRight[FRAME_GRID_COLS][FRAME_GRID_ROWS];
-
-    Frame(const Camera_ptr &cam, const cv::Mat &imLeft, const cv::Mat &imRight, const double &timeStamp, const std::shared_ptr<ORBextractor> &extractorLeft, const std::shared_ptr<ORBextractor> &extractorRight, ORBVocabulary* voc, cv::Mat &K, cv::Mat &distCoef, const float &bf, const float &thDepth, const std::shared_ptr<GeometricCamera> &pCamera, const std::shared_ptr<GeometricCamera> &pCamera2, const std::string &pNameFile, int pnNumDataset, Sophus::SE3f& Tlr,Frame* pPrevF = nullptr, const IMU::Calib &ImuCalib = IMU::Calib());
-
-    //Stereo fisheye
-    void ComputeStereoFishEyeMatches();
-
-    bool isInFrustumChecks(MapPoint* pMP, float viewingCosLimit, bool bRight = false);
-
-    Eigen::Vector3f UnprojectStereoFishEye(const int &i);
-
-    cv::Mat imgLeft, imgRight;
-
-    void PrintPointDistribution(){
-        int left = 0, right = 0;
-        int Nlim = (Nleft != -1) ? Nleft : N;
-        for(int i = 0; i < N; i++){
-            if(mvpMapPoints[i] && !mvbOutlier[i]){
-                if(i < Nlim) left++;
-                else right++;
-            }
-        }
-        cout << "Point distribution in Frame: left-> " << left << " --- right-> " << right << endl;
-    }
-
-    Sophus::SE3<double> T_test;
-};
-
-}// namespace ORB_SLAM
diff --git a/include/FrameDrawer.h b/include/FrameDrawer.h
deleted file mode 100644
index f6babdd8..00000000
--- a/include/FrameDrawer.h
+++ /dev/null
@@ -1,71 +0,0 @@
-/**
- * This file is part of ORB-SLAM3
- *
- * Copyright (C) 2017-2021 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/>.
- */
-
-#pragma once
-
-#include <mutex>
-#include <opencv2/opencv.hpp>
-#include <vector>
-#include "ImprovedTypes.hpp"
-
-namespace MORB_SLAM {
-
-class MapPoint;
-class Viewer;
-
-class FrameDrawer {
- public:
-  
-  FrameDrawer(const Atlas_ptr &pAtlas);
-
-  // Update info from the last processed frame.
-  void Update(const Tracking_ptr &pTracker);
-
-  // Draw last processed frame.
-  cv::Mat DrawFrame(float imageScale = 1.f);
-  cv::Mat DrawRightFrame(float imageScale = 1.f);
-
-  friend Viewer;
-
- protected:
-  bool both;
-  void DrawTextInfo(cv::Mat &im, int nState, cv::Mat &imText);
-
-  // Info of the frame to be drawn
-  cv::Mat mIm, mImRight;
-  int N;
-  std::vector<cv::KeyPoint> mvCurrentKeys, mvCurrentKeysRight;
-  std::vector<bool> mvbMap, mvbVO;
-  bool mbOnlyTracking;
-  int mnTracked, mnTrackedVO;
-  std::vector<cv::KeyPoint> mvIniKeys;
-  std::vector<int> mvIniMatches;
-  int mState;
-  std::vector<float> mvCurrentDepth;
-  float mThDepth;
-
-  Atlas_ptr mpAtlas;
-
-  std::mutex mMutex;
-  std::vector<std::pair<cv::Point2f, cv::Point2f>> mvTracks;
-};
-
-}  // namespace MORB_SLAM
diff --git a/include/G2oTypes.h b/include/G2oTypes.h
deleted file mode 100644
index 87c48a33..00000000
--- a/include/G2oTypes.h
+++ /dev/null
@@ -1,844 +0,0 @@
-/**
-* This file is part of ORB-SLAM3
-*
-* Copyright (C) 2017-2021 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/>.
-*/
-
-#pragma once
-
-#include "g2o/core/base_vertex.h"
-#include "g2o/core/base_binary_edge.h"
-#include "g2o/types/types_sba.h"
-#include "g2o/core/base_multi_edge.h"
-#include "g2o/core/base_unary_edge.h"
-
-#include<opencv2/core/core.hpp>
-
-#include <Eigen/Core>
-#include <Eigen/Geometry>
-#include <Eigen/Dense>
-
-#include <Frame.h>
-#include <KeyFrame.h>
-
-#include"Converter.h"
-#include <math.h>
-
-namespace MORB_SLAM
-{
-
-class KeyFrame;
-class Frame;
-class GeometricCamera;
-
-typedef Eigen::Matrix<double, 6, 1> Vector6d;
-typedef Eigen::Matrix<double, 9, 1> Vector9d;
-typedef Eigen::Matrix<double, 12, 1> Vector12d;
-typedef Eigen::Matrix<double, 15, 1> Vector15d;
-typedef Eigen::Matrix<double, 12, 12> Matrix12d;
-typedef Eigen::Matrix<double, 15, 15> Matrix15d;
-typedef Eigen::Matrix<double, 9, 9> Matrix9d;
-
-Eigen::Matrix3d ExpSO3(const double x, const double y, const double z);
-Eigen::Matrix3d ExpSO3(const Eigen::Vector3d &w);
-
-Eigen::Vector3d LogSO3(const Eigen::Matrix3d &R);
-
-Eigen::Matrix3d InverseRightJacobianSO3(const Eigen::Vector3d &v);
-Eigen::Matrix3d RightJacobianSO3(const Eigen::Vector3d &v);
-Eigen::Matrix3d RightJacobianSO3(const double x, const double y, const double z);
-
-Eigen::Matrix3d Skew(const Eigen::Vector3d &w);
-Eigen::Matrix3d InverseRightJacobianSO3(const double x, const double y, const double z);
-
-template<typename T = double>
-Eigen::Matrix<T,3,3> NormalizeRotation(const Eigen::Matrix<T,3,3> &R) {
-    Eigen::JacobiSVD<Eigen::Matrix<T,3,3>> svd(R,Eigen::ComputeFullU | Eigen::ComputeFullV);
-    return svd.matrixU() * svd.matrixV().transpose();
-}
-
-
-class ImuCamPose
-{
-public:
-    
-    ImuCamPose(){}
-    ImuCamPose(KeyFrame* pKF);
-    ImuCamPose(Frame* pF);
-    ImuCamPose(Eigen::Matrix3d &_Rwc, Eigen::Vector3d &_twc, KeyFrame* pKF);
-
-    void 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);
-
-    void Update(const double *pu); // update in the imu reference
-    void UpdateW(const double *pu); // update in the world reference
-    Eigen::Vector2d Project(const Eigen::Vector3d &Xw, int cam_idx=0) const; // Mono
-    Eigen::Vector3d ProjectStereo(const Eigen::Vector3d &Xw, int cam_idx=0) const; // Stereo
-    bool isDepthPositive(const Eigen::Vector3d &Xw, int cam_idx=0) const;
-
-public:
-    // For IMU
-    Eigen::Matrix3d Rwb;
-    Eigen::Vector3d twb;
-
-    // For set of cameras
-    std::vector<Eigen::Matrix3d> Rcw;
-    std::vector<Eigen::Vector3d> tcw;
-    std::vector<Eigen::Matrix3d> Rcb, Rbc;
-    std::vector<Eigen::Vector3d> tcb, tbc;
-    double bf;
-    std::vector<std::shared_ptr<GeometricCamera>> pCamera;
-
-    // For posegraph 4DoF
-    Eigen::Matrix3d Rwb0;
-    Eigen::Matrix3d DR;
-
-    int its;
-};
-
-class InvDepthPoint
-{
-public:
-    
-    InvDepthPoint(){}
-    InvDepthPoint(double _rho, double _u, double _v, KeyFrame* pHostKF);
-
-    void Update(const double *pu);
-
-    double rho;
-    double u, v; // they are not variables, observation in the host frame
-
-    double fx, fy, cx, cy, bf; // from host frame
-
-    int its;
-};
-
-// Optimizable parameters are IMU pose
-class VertexPose : public g2o::BaseVertex<6,ImuCamPose>
-{
-public:
-    
-    VertexPose(){}
-    VertexPose(KeyFrame* pKF){
-        setEstimate(ImuCamPose(pKF));
-    }
-    VertexPose(Frame* pF){
-        setEstimate(ImuCamPose(pF));
-    }
-
-
-    virtual bool read(std::istream& is);
-    virtual bool write(std::ostream& os) const;
-
-    virtual void setToOriginImpl() {
-        }
-
-    virtual void oplusImpl(const double* update_){
-        _estimate.Update(update_);
-        updateCache();
-    }
-};
-
-class VertexPose4DoF : public g2o::BaseVertex<4,ImuCamPose>
-{
-    // Translation and yaw are the only optimizable variables
-public:
-    
-    VertexPose4DoF(){}
-    VertexPose4DoF(KeyFrame* pKF){
-        setEstimate(ImuCamPose(pKF));
-    }
-    VertexPose4DoF(Frame* pF){
-        setEstimate(ImuCamPose(pF));
-    }
-    VertexPose4DoF(Eigen::Matrix3d &_Rwc, Eigen::Vector3d &_twc, KeyFrame* pKF){
-
-        setEstimate(ImuCamPose(_Rwc, _twc, pKF));
-    }
-
-    virtual bool read(std::istream& is){return false;}
-    virtual bool write(std::ostream& os) const{return false;}
-
-    virtual void setToOriginImpl() {
-        }
-
-    virtual void oplusImpl(const double* update_){
-        double update6DoF[6];
-        update6DoF[0] = 0;
-        update6DoF[1] = 0;
-        update6DoF[2] = update_[0];
-        update6DoF[3] = update_[1];
-        update6DoF[4] = update_[2];
-        update6DoF[5] = update_[3];
-        _estimate.UpdateW(update6DoF);
-        updateCache();
-    }
-};
-
-class VertexVelocity : public g2o::BaseVertex<3,Eigen::Vector3d>
-{
-public:
-    
-    VertexVelocity(){}
-    VertexVelocity(KeyFrame* pKF);
-    VertexVelocity(Frame* pF);
-
-    virtual bool read(std::istream& is){return false;}
-    virtual bool write(std::ostream& os) const{return false;}
-
-    virtual void setToOriginImpl() {
-        }
-
-    virtual void oplusImpl(const double* update_){
-        Eigen::Vector3d uv;
-        uv << update_[0], update_[1], update_[2];
-        setEstimate(estimate()+uv);
-    }
-};
-
-class VertexGyroBias : public g2o::BaseVertex<3,Eigen::Vector3d>
-{
-public:
-    
-    VertexGyroBias(){}
-    VertexGyroBias(KeyFrame* pKF);
-    VertexGyroBias(Frame* pF);
-
-    virtual bool read(std::istream& is){return false;}
-    virtual bool write(std::ostream& os) const{return false;}
-
-    virtual void setToOriginImpl() {
-        }
-
-    virtual void oplusImpl(const double* update_){
-        Eigen::Vector3d ubg;
-        ubg << update_[0], update_[1], update_[2];
-        setEstimate(estimate()+ubg);
-    }
-};
-
-
-class VertexAccBias : public g2o::BaseVertex<3,Eigen::Vector3d>
-{
-public:
-    
-    VertexAccBias(){}
-    VertexAccBias(KeyFrame* pKF);
-    VertexAccBias(Frame* pF);
-
-    virtual bool read(std::istream& is){return false;}
-    virtual bool write(std::ostream& os) const{return false;}
-
-    virtual void setToOriginImpl() {
-        }
-
-    virtual void oplusImpl(const double* update_){
-        Eigen::Vector3d uba;
-        uba << update_[0], update_[1], update_[2];
-        setEstimate(estimate()+uba);
-    }
-};
-
-
-// Gravity direction vertex
-class GDirection
-{
-public:
-    
-    GDirection(){}
-    GDirection(Eigen::Matrix3d pRwg): Rwg(pRwg){}
-
-    void Update(const double *pu)
-    {
-        Rwg=Rwg*ExpSO3(pu[0],pu[1],0.0);
-    }
-
-    Eigen::Matrix3d Rwg, Rgw;
-
-    int its;
-};
-
-class VertexGDir : public g2o::BaseVertex<2,GDirection>
-{
-public:
-    
-    VertexGDir(){}
-    VertexGDir(Eigen::Matrix3d pRwg){
-        setEstimate(GDirection(pRwg));
-    }
-
-    virtual bool read(std::istream& is){return false;}
-    virtual bool write(std::ostream& os) const{return false;}
-
-    virtual void setToOriginImpl() {
-        }
-
-    virtual void oplusImpl(const double* update_){
-        _estimate.Update(update_);
-        updateCache();
-    }
-};
-
-// scale vertex
-class VertexScale : public g2o::BaseVertex<1,double>
-{
-public:
-    
-    VertexScale(){
-        setEstimate(1.0);
-    }
-    VertexScale(double ps){
-        setEstimate(ps);
-    }
-
-    virtual bool read(std::istream& is){return false;}
-    virtual bool write(std::ostream& os) const{return false;}
-
-    virtual void setToOriginImpl(){
-        setEstimate(1.0);
-    }
-
-    virtual void oplusImpl(const double *update_){
-        setEstimate(estimate()*exp(*update_));
-    }
-};
-
-
-// Inverse depth point (just one parameter, inverse depth at the host frame)
-class VertexInvDepth : public g2o::BaseVertex<1,InvDepthPoint>
-{
-public:
-    
-    VertexInvDepth(){}
-    VertexInvDepth(double invDepth, double u, double v, KeyFrame* pHostKF){
-        setEstimate(InvDepthPoint(invDepth, u, v, pHostKF));
-    }
-
-    virtual bool read(std::istream& is){return false;}
-    virtual bool write(std::ostream& os) const{return false;}
-
-    virtual void setToOriginImpl() {
-        }
-
-    virtual void oplusImpl(const double* update_){
-        _estimate.Update(update_);
-        updateCache();
-    }
-};
-
-class EdgeMono : public g2o::BaseBinaryEdge<2,Eigen::Vector2d,g2o::VertexSBAPointXYZ,VertexPose>
-{
-public:
-    
-
-    EdgeMono(int cam_idx_=0): cam_idx(cam_idx_){
-    }
-
-    virtual bool read(std::istream& is){return false;}
-    virtual bool write(std::ostream& os) const{return false;}
-
-    void computeError(){
-        const g2o::VertexSBAPointXYZ* VPoint = static_cast<const g2o::VertexSBAPointXYZ*>(_vertices[0]);
-        const VertexPose* VPose = static_cast<const VertexPose*>(_vertices[1]);
-        const Eigen::Vector2d obs(_measurement);
-        _error = obs - VPose->estimate().Project(VPoint->estimate(),cam_idx);
-    }
-
-
-    virtual void linearizeOplus();
-
-    bool isDepthPositive()
-    {
-        const g2o::VertexSBAPointXYZ* VPoint = static_cast<const g2o::VertexSBAPointXYZ*>(_vertices[0]);
-        const VertexPose* VPose = static_cast<const VertexPose*>(_vertices[1]);
-        return VPose->estimate().isDepthPositive(VPoint->estimate(),cam_idx);
-    }
-
-    Eigen::Matrix<double,2,9> GetJacobian(){
-        linearizeOplus();
-        Eigen::Matrix<double,2,9> J;
-        J.block<2,3>(0,0) = _jacobianOplusXi;
-        J.block<2,6>(0,3) = _jacobianOplusXj;
-        return J;
-    }
-
-    Eigen::Matrix<double,9,9> GetHessian(){
-        linearizeOplus();
-        Eigen::Matrix<double,2,9> J;
-        J.block<2,3>(0,0) = _jacobianOplusXi;
-        J.block<2,6>(0,3) = _jacobianOplusXj;
-        return J.transpose()*information()*J;
-    }
-
-public:
-    const int cam_idx;
-};
-
-class EdgeMonoOnlyPose : public g2o::BaseUnaryEdge<2,Eigen::Vector2d,VertexPose>
-{
-public:
-    
-
-    EdgeMonoOnlyPose(const Eigen::Vector3f &Xw_, int cam_idx_=0):Xw(Xw_.cast<double>()),
-        cam_idx(cam_idx_){}
-
-    virtual bool read(std::istream& is){return false;}
-    virtual bool write(std::ostream& os) const{return false;}
-
-    void computeError(){
-        const VertexPose* VPose = static_cast<const VertexPose*>(_vertices[0]);
-        const Eigen::Vector2d obs(_measurement);
-        _error = obs - VPose->estimate().Project(Xw,cam_idx);
-    }
-
-    virtual void linearizeOplus();
-
-    bool isDepthPositive()
-    {
-        const VertexPose* VPose = static_cast<const VertexPose*>(_vertices[0]);
-        return VPose->estimate().isDepthPositive(Xw,cam_idx);
-    }
-
-    Eigen::Matrix<double,6,6> GetHessian(){
-        linearizeOplus();
-        return _jacobianOplusXi.transpose()*information()*_jacobianOplusXi;
-    }
-
-public:
-    const Eigen::Vector3d Xw;
-    const int cam_idx;
-};
-
-class EdgeStereo : public g2o::BaseBinaryEdge<3,Eigen::Vector3d,g2o::VertexSBAPointXYZ,VertexPose>
-{
-public:
-    
-
-    EdgeStereo(int cam_idx_=0): cam_idx(cam_idx_){}
-
-    virtual bool read(std::istream& is){return false;}
-    virtual bool write(std::ostream& os) const{return false;}
-
-    void computeError(){
-        const g2o::VertexSBAPointXYZ* VPoint = static_cast<const g2o::VertexSBAPointXYZ*>(_vertices[0]);
-        const VertexPose* VPose = static_cast<const VertexPose*>(_vertices[1]);
-        const Eigen::Vector3d obs(_measurement);
-        _error = obs - VPose->estimate().ProjectStereo(VPoint->estimate(),cam_idx);
-    }
-
-
-    virtual void linearizeOplus();
-
-    Eigen::Matrix<double,3,9> GetJacobian(){
-        linearizeOplus();
-        Eigen::Matrix<double,3,9> J;
-        J.block<3,3>(0,0) = _jacobianOplusXi;
-        J.block<3,6>(0,3) = _jacobianOplusXj;
-        return J;
-    }
-
-    Eigen::Matrix<double,9,9> GetHessian(){
-        linearizeOplus();
-        Eigen::Matrix<double,3,9> J;
-        J.block<3,3>(0,0) = _jacobianOplusXi;
-        J.block<3,6>(0,3) = _jacobianOplusXj;
-        return J.transpose()*information()*J;
-    }
-
-public:
-    const int cam_idx;
-};
-
-
-class EdgeStereoOnlyPose : public g2o::BaseUnaryEdge<3,Eigen::Vector3d,VertexPose>
-{
-public:
-    
-
-    EdgeStereoOnlyPose(const Eigen::Vector3f &Xw_, int cam_idx_=0):
-        Xw(Xw_.cast<double>()), cam_idx(cam_idx_){}
-
-    virtual bool read(std::istream& is){return false;}
-    virtual bool write(std::ostream& os) const{return false;}
-
-    void computeError(){
-        const VertexPose* VPose = static_cast<const VertexPose*>(_vertices[0]);
-        const Eigen::Vector3d obs(_measurement);
-        _error = obs - VPose->estimate().ProjectStereo(Xw, cam_idx);
-    }
-
-    virtual void linearizeOplus();
-
-    Eigen::Matrix<double,6,6> GetHessian(){
-        linearizeOplus();
-        return _jacobianOplusXi.transpose()*information()*_jacobianOplusXi;
-    }
-
-public:
-    const Eigen::Vector3d Xw; // 3D point coordinates
-    const int cam_idx;
-};
-
-class EdgeInertial : public g2o::BaseMultiEdge<9,Vector9d>
-{
-public:
-    
-
-    EdgeInertial(IMU::Preintegrated* pInt);
-
-    virtual bool read(std::istream& is){return false;}
-    virtual bool write(std::ostream& os) const{return false;}
-
-    void computeError();
-    virtual void linearizeOplus();
-
-    Eigen::Matrix<double,24,24> GetHessian(){
-        linearizeOplus();
-        Eigen::Matrix<double,9,24> J;
-        J.block<9,6>(0,0) = _jacobianOplus[0];
-        J.block<9,3>(0,6) = _jacobianOplus[1];
-        J.block<9,3>(0,9) = _jacobianOplus[2];
-        J.block<9,3>(0,12) = _jacobianOplus[3];
-        J.block<9,6>(0,15) = _jacobianOplus[4];
-        J.block<9,3>(0,21) = _jacobianOplus[5];
-        return J.transpose()*information()*J;
-    }
-
-    Eigen::Matrix<double,18,18> GetHessianNoPose1(){
-        linearizeOplus();
-        Eigen::Matrix<double,9,18> J;
-        J.block<9,3>(0,0) = _jacobianOplus[1];
-        J.block<9,3>(0,3) = _jacobianOplus[2];
-        J.block<9,3>(0,6) = _jacobianOplus[3];
-        J.block<9,6>(0,9) = _jacobianOplus[4];
-        J.block<9,3>(0,15) = _jacobianOplus[5];
-        return J.transpose()*information()*J;
-    }
-
-    Eigen::Matrix<double,9,9> GetHessian2(){
-        linearizeOplus();
-        Eigen::Matrix<double,9,9> J;
-        J.block<9,6>(0,0) = _jacobianOplus[4];
-        J.block<9,3>(0,6) = _jacobianOplus[5];
-        return J.transpose()*information()*J;
-    }
-
-    const Eigen::Matrix3d JRg, JVg, JPg;
-    const Eigen::Matrix3d JVa, JPa;
-    IMU::Preintegrated* mpInt;
-    const double dt;
-    Eigen::Vector3d g;
-};
-
-
-// Edge inertial whre gravity is included as optimizable variable and it is not supposed to be pointing in -z axis, as well as scale
-class EdgeInertialGS : public g2o::BaseMultiEdge<9,Vector9d>
-{
-public:
-    
-
-    // EdgeInertialGS(IMU::Preintegrated* pInt);
-    EdgeInertialGS(IMU::Preintegrated* pInt);
-
-    virtual bool read(std::istream& is){return false;}
-    virtual bool write(std::ostream& os) const{return false;}
-
-    void computeError();
-    virtual void linearizeOplus();
-
-    const Eigen::Matrix3d JRg, JVg, JPg;
-    const Eigen::Matrix3d JVa, JPa;
-    IMU::Preintegrated* mpInt;
-    const double dt;
-    Eigen::Vector3d g, gI;
-
-    Eigen::Matrix<double,27,27> GetHessian(){
-        linearizeOplus();
-        Eigen::Matrix<double,9,27> J;
-        J.block<9,6>(0,0) = _jacobianOplus[0];
-        J.block<9,3>(0,6) = _jacobianOplus[1];
-        J.block<9,3>(0,9) = _jacobianOplus[2];
-        J.block<9,3>(0,12) = _jacobianOplus[3];
-        J.block<9,6>(0,15) = _jacobianOplus[4];
-        J.block<9,3>(0,21) = _jacobianOplus[5];
-        J.block<9,2>(0,24) = _jacobianOplus[6];
-        J.block<9,1>(0,26) = _jacobianOplus[7];
-        return J.transpose()*information()*J;
-    }
-
-    Eigen::Matrix<double,27,27> GetHessian2(){
-        linearizeOplus();
-        Eigen::Matrix<double,9,27> J;
-        J.block<9,3>(0,0) = _jacobianOplus[2];
-        J.block<9,3>(0,3) = _jacobianOplus[3];
-        J.block<9,2>(0,6) = _jacobianOplus[6];
-        J.block<9,1>(0,8) = _jacobianOplus[7];
-        J.block<9,3>(0,9) = _jacobianOplus[1];
-        J.block<9,3>(0,12) = _jacobianOplus[5];
-        J.block<9,6>(0,15) = _jacobianOplus[0];
-        J.block<9,6>(0,21) = _jacobianOplus[4];
-        return J.transpose()*information()*J;
-    }
-
-    Eigen::Matrix<double,9,9> GetHessian3(){
-        linearizeOplus();
-        Eigen::Matrix<double,9,9> J;
-        J.block<9,3>(0,0) = _jacobianOplus[2];
-        J.block<9,3>(0,3) = _jacobianOplus[3];
-        J.block<9,2>(0,6) = _jacobianOplus[6];
-        J.block<9,1>(0,8) = _jacobianOplus[7];
-        return J.transpose()*information()*J;
-    }
-
-
-
-    Eigen::Matrix<double,1,1> GetHessianScale(){
-        linearizeOplus();
-        Eigen::Matrix<double,9,1> J = _jacobianOplus[7];
-        return J.transpose()*information()*J;
-    }
-
-    Eigen::Matrix<double,3,3> GetHessianBiasGyro(){
-        linearizeOplus();
-        Eigen::Matrix<double,9,3> J = _jacobianOplus[2];
-        return J.transpose()*information()*J;
-    }
-
-    Eigen::Matrix<double,3,3> GetHessianBiasAcc(){
-        linearizeOplus();
-        Eigen::Matrix<double,9,3> J = _jacobianOplus[3];
-        return J.transpose()*information()*J;
-    }
-
-    Eigen::Matrix<double,2,2> GetHessianGDir(){
-        linearizeOplus();
-        Eigen::Matrix<double,9,2> J = _jacobianOplus[6];
-        return J.transpose()*information()*J;
-    }
-};
-
-
-
-class EdgeGyroRW : public g2o::BaseBinaryEdge<3,Eigen::Vector3d,VertexGyroBias,VertexGyroBias>
-{
-public:
-    
-
-    EdgeGyroRW(){}
-
-    virtual bool read(std::istream& is){return false;}
-    virtual bool write(std::ostream& os) const{return false;}
-
-    void computeError(){
-        const VertexGyroBias* VG1= static_cast<const VertexGyroBias*>(_vertices[0]);
-        const VertexGyroBias* VG2= static_cast<const VertexGyroBias*>(_vertices[1]);
-        _error = VG2->estimate()-VG1->estimate();
-    }
-
-    virtual void linearizeOplus(){
-        _jacobianOplusXi = -Eigen::Matrix3d::Identity();
-        _jacobianOplusXj.setIdentity();
-    }
-
-    Eigen::Matrix<double,6,6> GetHessian(){
-        linearizeOplus();
-        Eigen::Matrix<double,3,6> J;
-        J.block<3,3>(0,0) = _jacobianOplusXi;
-        J.block<3,3>(0,3) = _jacobianOplusXj;
-        return J.transpose()*information()*J;
-    }
-
-    Eigen::Matrix3d GetHessian2(){
-        linearizeOplus();
-        return _jacobianOplusXj.transpose()*information()*_jacobianOplusXj;
-    }
-};
-
-
-class EdgeAccRW : public g2o::BaseBinaryEdge<3,Eigen::Vector3d,VertexAccBias,VertexAccBias>
-{
-public:
-    
-
-    EdgeAccRW(){}
-
-    virtual bool read(std::istream& is){return false;}
-    virtual bool write(std::ostream& os) const{return false;}
-
-    void computeError(){
-        const VertexAccBias* VA1= static_cast<const VertexAccBias*>(_vertices[0]);
-        const VertexAccBias* VA2= static_cast<const VertexAccBias*>(_vertices[1]);
-        _error = VA2->estimate()-VA1->estimate();
-    }
-
-    virtual void linearizeOplus(){
-        _jacobianOplusXi = -Eigen::Matrix3d::Identity();
-        _jacobianOplusXj.setIdentity();
-    }
-
-    Eigen::Matrix<double,6,6> GetHessian(){
-        linearizeOplus();
-        Eigen::Matrix<double,3,6> J;
-        J.block<3,3>(0,0) = _jacobianOplusXi;
-        J.block<3,3>(0,3) = _jacobianOplusXj;
-        return J.transpose()*information()*J;
-    }
-
-    Eigen::Matrix3d GetHessian2(){
-        linearizeOplus();
-        return _jacobianOplusXj.transpose()*information()*_jacobianOplusXj;
-    }
-};
-
-class ConstraintPoseImu
-{
-public:
-    
-
-    ConstraintPoseImu(const Eigen::Matrix3d &Rwb_, const Eigen::Vector3d &twb_, const Eigen::Vector3d &vwb_,
-                       const Eigen::Vector3d &bg_, const Eigen::Vector3d &ba_, const Matrix15d &H_):
-                       Rwb(Rwb_), twb(twb_), vwb(vwb_), bg(bg_), ba(ba_), H(H_)
-    {
-        Eigen::SelfAdjointEigenSolver<Eigen::Matrix<double,15,15> > es(H);
-        Eigen::Matrix<double,15,1> eigs = es.eigenvalues();
-        for(int i=0;i<15;i++)
-            if(eigs[i]<1e-12)
-                eigs[i]=0;
-        H = es.eigenvectors()*eigs.asDiagonal()*es.eigenvectors().transpose();
-    }
-
-    Eigen::Matrix3d Rwb;
-    Eigen::Vector3d twb;
-    Eigen::Vector3d vwb;
-    Eigen::Vector3d bg;
-    Eigen::Vector3d ba;
-    Matrix15d H;
-};
-
-class EdgePriorPoseImu : public g2o::BaseMultiEdge<15,Vector15d>
-{
-public:
-        
-        EdgePriorPoseImu(std::shared_ptr<ConstraintPoseImu> c);
-
-        virtual bool read(std::istream& is){return false;}
-        virtual bool write(std::ostream& os) const{return false;}
-
-        void computeError();
-        virtual void linearizeOplus();
-
-        Eigen::Matrix<double,15,15> GetHessian(){
-            linearizeOplus();
-            Eigen::Matrix<double,15,15> J;
-            J.block<15,6>(0,0) = _jacobianOplus[0];
-            J.block<15,3>(0,6) = _jacobianOplus[1];
-            J.block<15,3>(0,9) = _jacobianOplus[2];
-            J.block<15,3>(0,12) = _jacobianOplus[3];
-            return J.transpose()*information()*J;
-        }
-
-        Eigen::Matrix<double,9,9> GetHessianNoPose(){
-            linearizeOplus();
-            Eigen::Matrix<double,15,9> J;
-            J.block<15,3>(0,0) = _jacobianOplus[1];
-            J.block<15,3>(0,3) = _jacobianOplus[2];
-            J.block<15,3>(0,6) = _jacobianOplus[3];
-            return J.transpose()*information()*J;
-        }
-        Eigen::Matrix3d Rwb;
-        Eigen::Vector3d twb, vwb;
-        Eigen::Vector3d bg, ba;
-};
-
-// Priors for biases
-class EdgePriorAcc : public g2o::BaseUnaryEdge<3,Eigen::Vector3d,VertexAccBias>
-{
-public:
-    
-
-    EdgePriorAcc(const Eigen::Vector3f &bprior_):bprior(bprior_.cast<double>()){}
-
-    virtual bool read(std::istream& is){return false;}
-    virtual bool write(std::ostream& os) const{return false;}
-
-    void computeError(){
-        const VertexAccBias* VA = static_cast<const VertexAccBias*>(_vertices[0]);
-        _error = bprior - VA->estimate();
-    }
-    virtual void linearizeOplus();
-
-    Eigen::Matrix<double,3,3> GetHessian(){
-        linearizeOplus();
-        return _jacobianOplusXi.transpose()*information()*_jacobianOplusXi;
-    }
-
-    const Eigen::Vector3d bprior;
-};
-
-class EdgePriorGyro : public g2o::BaseUnaryEdge<3,Eigen::Vector3d,VertexGyroBias>
-{
-public:
-    
-
-    EdgePriorGyro(const Eigen::Vector3f &bprior_):bprior(bprior_.cast<double>()){}
-
-    virtual bool read(std::istream& is){return false;}
-    virtual bool write(std::ostream& os) const{return false;}
-
-    void computeError(){
-        const VertexGyroBias* VG = static_cast<const VertexGyroBias*>(_vertices[0]);
-        _error = bprior - VG->estimate();
-    }
-    virtual void linearizeOplus();
-
-    Eigen::Matrix<double,3,3> GetHessian(){
-        linearizeOplus();
-        return _jacobianOplusXi.transpose()*information()*_jacobianOplusXi;
-    }
-
-    const Eigen::Vector3d bprior;
-};
-
-
-class Edge4DoF : public g2o::BaseBinaryEdge<6,Vector6d,VertexPose4DoF,VertexPose4DoF>
-{
-public:
-    
-
-    Edge4DoF(const Eigen::Matrix4d &deltaT){
-        dTij = deltaT;
-        dRij = deltaT.block<3,3>(0,0);
-        dtij = deltaT.block<3,1>(0,3);
-    }
-
-    virtual bool read(std::istream& is){return false;}
-    virtual bool write(std::ostream& os) const{return false;}
-
-    void computeError(){
-        const VertexPose4DoF* VPi = static_cast<const VertexPose4DoF*>(_vertices[0]);
-        const VertexPose4DoF* VPj = static_cast<const VertexPose4DoF*>(_vertices[1]);
-        _error << LogSO3(VPi->estimate().Rcw[0]*VPj->estimate().Rcw[0].transpose()*dRij.transpose()),
-                 VPi->estimate().Rcw[0]*(-VPj->estimate().Rcw[0].transpose()*VPj->estimate().tcw[0])+VPi->estimate().tcw[0] - dtij;
-    }
-
-    // virtual void linearizeOplus(); // numerical implementation
-
-    Eigen::Matrix4d dTij;
-    Eigen::Matrix3d dRij;
-    Eigen::Vector3d dtij;
-};
-
-} //namespace ORB_SLAM2
-
diff --git a/include/GeometricTools.h b/include/GeometricTools.h
deleted file mode 100644
index c1d888d0..00000000
--- a/include/GeometricTools.h
+++ /dev/null
@@ -1,81 +0,0 @@
-/**
- * This file is part of ORB-SLAM3
- *
- * Copyright (C) 2017-2021 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/>.
- */
-
-#pragma once
-
-#include <Eigen/Core>
-#include <opencv2/core/core.hpp>
-#include <sophus/se3.hpp>
-
-namespace MORB_SLAM {
-
-class KeyFrame;
-
-class GeometricTools {
- public:
-  
-  // Compute the Fundamental matrix between KF1 and KF2
-  static Eigen::Matrix3f ComputeF12(KeyFrame *&pKF1, KeyFrame *&pKF2);
-
-  // Triangulate point with KF1 and KF2
-  static bool Triangulate(Eigen::Vector3f &x_c1, Eigen::Vector3f &x_c2,
-                          Eigen::Matrix<float, 3, 4> &Tc1w,
-                          Eigen::Matrix<float, 3, 4> &Tc2w,
-                          Eigen::Vector3f &x3D);
-
-  template <int rows, int cols>
-  static bool CheckMatrices(const cv::Mat &cvMat,
-                            const Eigen::Matrix<float, rows, cols> &eigMat) {
-    const float epsilon = 1e-3;
-    // std::cout << cvMat.cols - cols << cvMat.rows - rows << std::endl;
-    if (rows != cvMat.rows || cols != cvMat.cols) {
-      std::cout << "wrong cvmat size\n";
-      return false;
-    }
-    for (int i = 0; i < rows; i++)
-      for (int j = 0; j < cols; j++)
-        if ((cvMat.at<float>(i, j) > (eigMat(i, j) + epsilon)) ||
-            (cvMat.at<float>(i, j) < (eigMat(i, j) - epsilon))) {
-          std::cout << "cv mat:\n" << cvMat << std::endl;
-          std::cout << "eig mat:\n" << eigMat << std::endl;
-          return false;
-        }
-    return true;
-  }
-
-  template <typename T, int rows, int cols>
-  static bool CheckMatrices(const Eigen::Matrix<T, rows, cols> &eigMat1,
-                            const Eigen::Matrix<T, rows, cols> &eigMat2) {
-    const float epsilon = 1e-3;
-    for (int i = 0; i < rows; i++)
-      for (int j = 0; j < cols; j++)
-        if ((eigMat1(i, j) > (eigMat2(i, j) + epsilon)) ||
-            (eigMat1(i, j) < (eigMat2(i, j) - epsilon))) {
-          std::cout << "eig mat 1:\n" << eigMat1 << std::endl;
-          std::cout << "eig mat 2:\n" << eigMat2 << std::endl;
-          return false;
-        }
-    return true;
-  }
-};
-
-}  // namespace MORB_SLAM
-
diff --git a/include/ImprovedTypes.hpp b/include/ImprovedTypes.hpp
deleted file mode 100644
index a66a18a9..00000000
--- a/include/ImprovedTypes.hpp
+++ /dev/null
@@ -1,51 +0,0 @@
-#pragma once
-
-#include <memory>
-#include <utility>
-#include <unordered_map>
-
-namespace MORB_SLAM{
-
-class System;
-class Atlas;
-class Tracking;
-typedef std::shared_ptr<System> System_ptr;
-typedef std::weak_ptr<System> System_wptr;
-typedef std::shared_ptr<Atlas> Atlas_ptr;
-typedef std::weak_ptr<Atlas> Atlas_wptr;
-typedef std::shared_ptr<Tracking> Tracking_ptr;
-typedef std::weak_ptr<Tracking> Tracking_wptr;
-
-typedef std::pair<int, int> IntPair;
-template<typename KEY, typename VALUE> using umap = std::unordered_map<KEY,VALUE>;
-
-
-
-  namespace Tracker{
-    // Tracking states
-    enum eTrackingState {
-        SYSTEM_NOT_READY = -1,
-        NO_IMAGES_YET = 0,
-        NOT_INITIALIZED = 1,
-        OK = 2,
-        RECENTLY_LOST = 3,
-        LOST = 4,
-        OK_KLT = 5
-    };
-  }
-
-  namespace CameraType{
-    // Input sensor
-    enum eSensor{
-        MONOCULAR=0,
-        STEREO=1,
-        RGBD=2,
-        IMU_MONOCULAR=3,
-        IMU_STEREO=4,
-        IMU_RGBD=5,
-    };
-
-    inline bool isInertial(eSensor sensor) { return sensor == IMU_MONOCULAR || sensor == IMU_STEREO || sensor == IMU_RGBD; }
-    inline bool hasMulticam(eSensor sensor) { return sensor == STEREO || sensor == RGBD || sensor == IMU_STEREO || sensor == IMU_RGBD; }
-  }
-}
\ No newline at end of file
diff --git a/include/ImuTypes.h b/include/ImuTypes.h
deleted file mode 100644
index 4fb655fc..00000000
--- a/include/ImuTypes.h
+++ /dev/null
@@ -1,272 +0,0 @@
-/**
- * This file is part of ORB-SLAM3
- *
- * Copyright (C) 2017-2021 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/>.
- */
-
-#pragma once
-
-#include <Eigen/Core>
-#include <Eigen/Dense>
-#include <Eigen/Geometry>
-#include <boost/serialization/serialization.hpp>
-#include <boost/serialization/vector.hpp>
-#include <mutex>
-#include <opencv2/core/core.hpp>
-#include <sophus/se3.hpp>
-#include <utility>
-#include <vector>
-
-#include "SerializationUtils.h"
-
-namespace MORB_SLAM {
-
-namespace IMU {
-
-const float GRAVITY_VALUE = 9.81;
-
-// IMU measurement (gyro, accelerometer and timestamp)
-class Point {
- public:
-  Point(const float &acc_x, const float &acc_y, const float &acc_z,
-        const float &ang_vel_x, const float &ang_vel_y, const float &ang_vel_z,
-        const double &timestamp)
-      : a(acc_x, acc_y, acc_z),
-        w(ang_vel_x, ang_vel_y, ang_vel_z),
-        t(timestamp) {}
-  Point(const cv::Point3f Acc, const cv::Point3f Gyro, const double &timestamp)
-      : a(Acc.x, Acc.y, Acc.z), w(Gyro.x, Gyro.y, Gyro.z), t(timestamp) {}
-
- public:
-  Eigen::Vector3f a;
-  Eigen::Vector3f w;
-  double t;
-  
-};
-
-// IMU biases (gyro and accelerometer)
-class Bias {
-  friend class boost::serialization::access;
-  template <class Archive>
-  void serialize(Archive &ar, const unsigned int version) {
-    ar &bax;
-    ar &bay;
-    ar &baz;
-
-    ar &bwx;
-    ar &bwy;
-    ar &bwz;
-  }
-
- public:
-  Bias() : bax(0), bay(0), baz(0), bwx(0), bwy(0), bwz(0) {}
-  Bias(const float &b_acc_x, const float &b_acc_y, const float &b_acc_z,
-       const float &b_ang_vel_x, const float &b_ang_vel_y,
-       const float &b_ang_vel_z)
-      : bax(b_acc_x),
-        bay(b_acc_y),
-        baz(b_acc_z),
-        bwx(b_ang_vel_x),
-        bwy(b_ang_vel_y),
-        bwz(b_ang_vel_z) {}
-  void CopyFrom(Bias &b);
-  friend std::ostream &operator<<(std::ostream &out, const Bias &b);
-
- public:
-  float bax, bay, baz;
-  float bwx, bwy, bwz;
-  
-};
-
-// IMU calibration (Tbc, Tcb, noise)
-class Calib {
-  friend class boost::serialization::access;
-  template <class Archive>
-  void serialize(Archive &ar, const unsigned int version) {
-    serializeSophusSE3(ar, mTcb, version);
-    serializeSophusSE3(ar, mTbc, version);
-
-    ar &boost::serialization::make_array(Cov.diagonal().data(),
-                                         Cov.diagonal().size());
-    ar &boost::serialization::make_array(CovWalk.diagonal().data(),
-                                         CovWalk.diagonal().size());
-
-    ar &mbIsSet;
-  }
-
- public:
-  Calib(const Sophus::SE3<float> &Tbc, const float &ng, const float &na,
-        const float &ngw, const float &naw) {
-    Set(Tbc, ng, na, ngw, naw);
-  }
-
-  Calib(const Calib &calib);
-  Calib() { mbIsSet = false; }
-
-  // void Set(const cv::Mat &cvTbc, const float &ng, const float &na, const
-  // float &ngw, const float &naw);
-  void Set(const Sophus::SE3<float> &sophTbc, const float &ng, const float &na,
-           const float &ngw, const float &naw);
-
- public:
-  // Sophus/Eigen implementation
-  Sophus::SE3<float> mTcb;
-  Sophus::SE3<float> mTbc;
-  Eigen::DiagonalMatrix<float, 6> Cov, CovWalk;
-  bool mbIsSet;
-};
-
-// Integration of 1 gyro measurement
-class IntegratedRotation {
- public:
-  IntegratedRotation() {}
-  IntegratedRotation(const Eigen::Vector3f &angVel, const Bias &imuBias,
-                     const float &time);
-
- public:
-  float deltaT;  // integration time
-  Eigen::Matrix3f deltaR;
-  Eigen::Matrix3f rightJ;  // right jacobian
-  
-};
-
-// Preintegration of Imu Measurements
-class Preintegrated {
-  friend class boost::serialization::access;
-  template <class Archive>
-  void serialize(Archive &ar, const unsigned int version) {
-    ar &dT;
-    ar &boost::serialization::make_array(C.data(), C.size());
-    ar &boost::serialization::make_array(Info.data(), Info.size());
-    ar &boost::serialization::make_array(Nga.diagonal().data(),
-                                         Nga.diagonal().size());
-    ar &boost::serialization::make_array(NgaWalk.diagonal().data(),
-                                         NgaWalk.diagonal().size());
-    ar &b;
-    ar &boost::serialization::make_array(dR.data(), dR.size());
-    ar &boost::serialization::make_array(dV.data(), dV.size());
-    ar &boost::serialization::make_array(dP.data(), dP.size());
-    ar &boost::serialization::make_array(JRg.data(), JRg.size());
-    ar &boost::serialization::make_array(JVg.data(), JVg.size());
-    ar &boost::serialization::make_array(JVa.data(), JVa.size());
-    ar &boost::serialization::make_array(JPg.data(), JPg.size());
-    ar &boost::serialization::make_array(JPa.data(), JPa.size());
-    ar &boost::serialization::make_array(avgA.data(), avgA.size());
-    ar &boost::serialization::make_array(avgW.data(), avgW.size());
-
-    ar &bu;
-    ar &boost::serialization::make_array(db.data(), db.size());
-    ar &mvMeasurements;
-  }
-
- public:
-  
-  Preintegrated(const Bias &b_, const Calib &calib);
-  Preintegrated(Preintegrated *pImuPre);
-  Preintegrated() {}
-  ~Preintegrated() {}
-  void CopyFrom(Preintegrated *pImuPre);
-  void Initialize(const Bias &b_);
-  void IntegrateNewMeasurement(const Eigen::Vector3f &acceleration,
-                               const Eigen::Vector3f &angVel, const float &dt);
-  void Reintegrate();
-  void MergePrevious(Preintegrated *pPrev);
-  void SetNewBias(const Bias &bu_);
-  IMU::Bias GetDeltaBias(const Bias &b_);
-
-  Eigen::Matrix3f GetDeltaRotation(const Bias &b_);
-  Eigen::Vector3f GetDeltaVelocity(const Bias &b_);
-  Eigen::Vector3f GetDeltaPosition(const Bias &b_);
-
-  Eigen::Matrix3f GetUpdatedDeltaRotation();
-  Eigen::Vector3f GetUpdatedDeltaVelocity();
-  Eigen::Vector3f GetUpdatedDeltaPosition();
-
-  Eigen::Matrix3f GetOriginalDeltaRotation();
-  Eigen::Vector3f GetOriginalDeltaVelocity();
-  Eigen::Vector3f GetOriginalDeltaPosition();
-
-  Eigen::Matrix<float, 6, 1> GetDeltaBias();
-
-  Bias GetOriginalBias();
-  Bias GetUpdatedBias();
-
-  void printMeasurements() const {
-    std::cout << "pint meas:\n";
-    for (size_t i = 0; i < mvMeasurements.size(); i++) {
-      std::cout << "meas " << mvMeasurements[i].t << std::endl;
-    }
-    std::cout << "end pint meas:\n";
-  }
-
- public:
-  float dT;
-  Eigen::Matrix<float, 15, 15> C;
-  Eigen::Matrix<float, 15, 15> Info;
-  Eigen::DiagonalMatrix<float, 6> Nga, NgaWalk;
-
-  // Values for the original bias (when integration was computed)
-  Bias b;
-  Eigen::Matrix3f dR;
-  Eigen::Vector3f dV, dP;
-  Eigen::Matrix3f JRg, JVg, JVa, JPg, JPa;
-  Eigen::Vector3f avgA, avgW;
-
- private:
-  // Updated bias
-  Bias bu;
-  // Dif between original and updated bias
-  // This is used to compute the updated values of the preintegration
-  Eigen::Matrix<float, 6, 1> db;
-
-  struct integrable {
-    template <class Archive>
-    void serialize(Archive &ar, const unsigned int version) {
-      ar &boost::serialization::make_array(a.data(), a.size());
-      ar &boost::serialization::make_array(w.data(), w.size());
-      ar &t;
-    }
-
-    
-    integrable() {}
-    integrable(const Eigen::Vector3f &a_, const Eigen::Vector3f &w_,
-               const float &t_)
-        : a(a_), w(w_), t(t_) {}
-    Eigen::Vector3f a, w;
-    float t;
-  };
-
-  std::vector<integrable> mvMeasurements;
-
-  std::mutex mMutex;
-};
-
-// Lie Algebra Functions
-Eigen::Matrix3f RightJacobianSO3(const float &x, const float &y,
-                                 const float &z);
-Eigen::Matrix3f RightJacobianSO3(const Eigen::Vector3f &v);
-
-Eigen::Matrix3f InverseRightJacobianSO3(const float &x, const float &y,
-                                        const float &z);
-Eigen::Matrix3f InverseRightJacobianSO3(const Eigen::Vector3f &v);
-
-Eigen::Matrix3f NormalizeRotation(const Eigen::Matrix3f &R);
-
-}  // namespace IMU
-
-}  // namespace MORB_SLAM
diff --git a/include/KeyFrame.h b/include/KeyFrame.h
deleted file mode 100644
index 8f5d7e3e..00000000
--- a/include/KeyFrame.h
+++ /dev/null
@@ -1,542 +0,0 @@
-/**
- * This file is part of ORB-SLAM3
- *
- * Copyright (C) 2017-2021 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/>.
- */
-
-#pragma once
-
-#include <boost/serialization/base_object.hpp>
-#include <boost/serialization/map.hpp>
-#include <boost/serialization/vector.hpp>
-#include <mutex>
-
-#include "DBoW2/BowVector.h"
-#include "DBoW2/FeatureVector.h"
-#include "Frame.h"
-#include "GeometricCamera.h"
-#include "ImuTypes.h"
-#include "KeyFrameDatabase.h"
-#include "MapPoint.h"
-#include "ORBVocabulary.h"
-#include "ORBextractor.h"
-#include "SerializationUtils.h"
-
-namespace MORB_SLAM {
-
-class Map;
-class MapPoint;
-class Frame;
-class KeyFrameDatabase;
-
-class GeometricCamera;
-
-class KeyFrame {
-  friend class boost::serialization::access;
-
-  template <class Archive>
-  void serialize(Archive& ar, const unsigned int version) {
-    ar& mnId;
-    ar& const_cast<long unsigned int&>(mnFrameId);
-    ar& const_cast<double&>(mTimeStamp);
-    // Grid
-    ar& const_cast<int&>(mnGridCols);
-    ar& const_cast<int&>(mnGridRows);
-    ar& const_cast<float&>(mfGridElementWidthInv);
-    ar& const_cast<float&>(mfGridElementHeightInv);
-
-    // Variables of tracking
-    // ar & mnTrackReferenceForFrame;
-    // ar & mnFuseTargetForKF;
-    // Variables of local mapping
-    // ar & mnBALocalForKF;
-    // ar & mnBAFixedForKF;
-    // ar & mnNumberOfOpt;
-    // Variables used by KeyFrameDatabase
-    // ar & mnLoopQuery;
-    // ar & mnLoopWords;
-    // ar & mLoopScore;
-    // ar & mnRelocQuery;
-    // ar & mnRelocWords;
-    // ar & mRelocScore;
-    // ar & mnMergeQuery;
-    // ar & mnMergeWords;
-    // ar & mMergeScore;
-    // ar & mnPlaceRecognitionQuery;
-    // ar & mnPlaceRecognitionWords;
-    // ar & mPlaceRecognitionScore;
-    // ar & mbCurrentPlaceRecognition;
-    // Variables of loop closing
-    // serializeMatrix(ar,mTcwGBA,version);
-    // serializeMatrix(ar,mTcwBefGBA,version);
-    // serializeMatrix(ar,mVwbGBA,version);
-    // serializeMatrix(ar,mVwbBefGBA,version);
-    // ar & mBiasGBA;
-    // ar & mnBAGlobalForKF;
-    // Variables of Merging
-    // serializeMatrix(ar,mTcwMerge,version);
-    // serializeMatrix(ar,mTcwBefMerge,version);
-    // serializeMatrix(ar,mTwcBefMerge,version);
-    // serializeMatrix(ar,mVwbMerge,version);
-    // serializeMatrix(ar,mVwbBefMerge,version);
-    // ar & mBiasMerge;
-    // ar & mnMergeCorrectedForKF;
-    // ar & mnMergeForKF;
-    // ar & mfScaleMerge;
-    // ar & mnBALocalForMerge;
-
-    // Scale
-    ar& mfScale;
-    // Calibration parameters
-    ar& const_cast<float&>(fx);
-    ar& const_cast<float&>(fy);
-    ar& const_cast<float&>(invfx);
-    ar& const_cast<float&>(invfy);
-    ar& const_cast<float&>(cx);
-    ar& const_cast<float&>(cy);
-    ar& const_cast<float&>(mbf);
-    ar& const_cast<float&>(mb);
-    ar& const_cast<float&>(mThDepth);
-    serializeMatrix(ar, mDistCoef, version);
-    // Number of Keypoints
-    ar& const_cast<int&>(N);
-    // KeyPoints
-    serializeVectorKeyPoints<Archive>(ar, mvKeys, version);
-    serializeVectorKeyPoints<Archive>(ar, mvKeysUn, version);
-    ar& const_cast<vector<float>&>(mvuRight);
-    ar& const_cast<vector<float>&>(mvDepth);
-    serializeMatrix<Archive>(ar, mDescriptors, version);
-    // BOW
-    ar& mBowVec;
-    ar& mFeatVec;
-    // Pose relative to parent
-    serializeSophusSE3<Archive>(ar, mTcp, version);
-    // Scale
-    ar& const_cast<int&>(mnScaleLevels);
-    ar& const_cast<float&>(mfScaleFactor);
-    ar& const_cast<float&>(mfLogScaleFactor);
-    ar& const_cast<vector<float>&>(mvScaleFactors);
-    ar& const_cast<vector<float>&>(mvLevelSigma2);
-    ar& const_cast<vector<float>&>(mvInvLevelSigma2);
-    // Image bounds and calibration
-    ar& const_cast<int&>(mnMinX);
-    ar& const_cast<int&>(mnMinY);
-    ar& const_cast<int&>(mnMaxX);
-    ar& const_cast<int&>(mnMaxY);
-    ar& boost::serialization::make_array(mK_.data(), mK_.size());
-    // Pose
-    serializeSophusSE3<Archive>(ar, mTcw, version);
-    // MapPointsId associated to keypoints
-    ar& mvBackupMapPointsId;
-    // Grid
-    ar& mGrid;
-    // Connected KeyFrameWeight
-    ar& mBackupConnectedKeyFrameIdWeights;
-    // Spanning Tree and Loop Edges
-    ar& mbFirstConnection;
-    ar& mBackupParentId;
-    ar& mvBackupChildrensId;
-    ar& mvBackupLoopEdgesId;
-    ar& mvBackupMergeEdgesId;
-    // Bad flags
-    ar& mbNotErase;
-    ar& mbToBeErased;
-    ar& mbBad;
-
-    ar& mHalfBaseline;
-
-    ar& mnOriginMapId;
-
-    // Camera variables
-    ar& mnBackupIdCamera;
-    ar& mnBackupIdCamera2;
-
-    // Fisheye variables
-    ar& mvLeftToRightMatch;
-    ar& mvRightToLeftMatch;
-    ar& const_cast<int&>(NLeft);
-    ar& const_cast<int&>(NRight);
-    serializeSophusSE3<Archive>(ar, mTlr, version);
-    serializeVectorKeyPoints<Archive>(ar, mvKeysRight, version);
-    ar& mGridRight;
-
-    // Inertial variables
-    ar& mImuBias;
-    ar& mBackupImuPreintegrated;
-    ar& mImuCalib;
-    ar& mBackupPrevKFId;
-    ar& mBackupNextKFId;
-    ar& bImu;
-    ar& boost::serialization::make_array(mVw.data(), mVw.size());
-    ar& boost::serialization::make_array(mOwb.data(), mOwb.size());
-    ar& mbHasVelocity;
-  }
-
- public:
-  
-  KeyFrame();
-  KeyFrame(Frame& F, std::shared_ptr<Map> pMap, KeyFrameDatabase* pKFDB);
-
-  // Pose functions
-  void SetPose(const Sophus::SE3f& Tcw);
-  void SetVelocity(const Eigen::Vector3f& Vw_);
-
-  Sophus::SE3f GetPose();
-
-  Sophus::SE3f GetPoseInverse();
-  Eigen::Vector3f GetCameraCenter();
-
-  Eigen::Vector3f GetImuPosition();
-  Eigen::Matrix3f GetImuRotation();
-  Sophus::SE3f GetImuPose();
-  Eigen::Matrix3f GetRotation();
-  Eigen::Vector3f GetTranslation();
-  Eigen::Vector3f GetVelocity();
-  bool isVelocitySet();
-
-  // Bag of Words Representation
-  void ComputeBoW();
-
-  // Covisibility graph functions
-  void AddConnection(KeyFrame* pKF, const int& weight);
-  void EraseConnection(KeyFrame* pKF);
-
-  void UpdateConnections(bool upParent = true);
-  void UpdateBestCovisibles();
-  std::set<KeyFrame*> GetConnectedKeyFrames();
-  std::vector<KeyFrame*> GetVectorCovisibleKeyFrames();
-  std::vector<KeyFrame*> GetBestCovisibilityKeyFrames(const int& N);
-  std::vector<KeyFrame*> GetCovisiblesByWeight(const int& w);
-  int GetWeight(KeyFrame* pKF);
-
-  // Spanning tree functions
-  void AddChild(KeyFrame* pKF);
-  void EraseChild(KeyFrame* pKF);
-  void ChangeParent(KeyFrame* pKF);
-  std::set<KeyFrame*> GetChilds();
-  KeyFrame* GetParent();
-  bool hasChild(KeyFrame* pKF);
-  void SetFirstConnection(bool bFirst);
-
-  // Loop Edges
-  void AddLoopEdge(KeyFrame* pKF);
-  std::set<KeyFrame*> GetLoopEdges();
-
-  // Merge Edges
-  void AddMergeEdge(KeyFrame* pKF);
-  set<KeyFrame*> GetMergeEdges();
-
-  // MapPoint observation functions
-  int GetNumberMPs();
-  void AddMapPoint(MapPoint* pMP, const size_t& idx);
-  void EraseMapPointMatch(const int& idx);
-  void EraseMapPointMatch(MapPoint* pMP);
-  void ReplaceMapPointMatch(const int& idx, MapPoint* pMP);
-  std::set<MapPoint*> GetMapPoints();
-  std::vector<MapPoint*> GetMapPointMatches();
-  int TrackedMapPoints(const int& minObs);
-  MapPoint* GetMapPoint(const size_t& idx);
-
-  // KeyPoint functions
-  std::vector<size_t> GetFeaturesInArea(const float& x, const float& y,
-                                        const float& r,
-                                        const bool bRight = false) const;
-  bool UnprojectStereo(int i, Eigen::Vector3f& x3D);
-
-  // Image
-  bool IsInImage(const float& x, const float& y) const;
-
-  // Enable/Disable bad flag changes
-  void SetNotErase();
-  void SetErase();
-
-  // Set/check bad flag
-  void SetBadFlag();
-  bool isBad();
-
-  // Compute Scene Depth (q=2 median). Used in monocular.
-  float ComputeSceneMedianDepth(const int q);
-
-  static bool weightComp(int a, int b) { return a > b; }
-
-  static bool lId(KeyFrame* pKF1, KeyFrame* pKF2) {
-    return pKF1->mnId < pKF2->mnId;
-  }
-
-  std::shared_ptr<Map> GetMap();
-  void UpdateMap(std::shared_ptr<Map> pMap);
-
-  void SetNewBias(const IMU::Bias& b);
-  Eigen::Vector3f GetGyroBias();
-
-  Eigen::Vector3f GetAccBias();
-
-  IMU::Bias GetImuBias();
-
-  bool ProjectPointDistort(MapPoint* pMP, cv::Point2f& kp, float& u, float& v);
-  bool ProjectPointUnDistort(MapPoint* pMP, cv::Point2f& kp, float& u,
-                             float& v);
-
-  void PreSave(set<KeyFrame*>& spKF, set<MapPoint*>& spMP,
-               set<shared_ptr<GeometricCamera>>& spCam);
-  void PostLoad(map<long unsigned int, KeyFrame*>& mpKFid,
-                map<long unsigned int, MapPoint*>& mpMPid,
-                map<unsigned int, std::shared_ptr<GeometricCamera>>& mpCamId);
-
-  void SetORBVocabulary(ORBVocabulary* pORBVoc);
-  void SetKeyFrameDatabase(KeyFrameDatabase* pKFDB);
-
-  bool bImu;
-
-  // The following variables are accesed from only 1 thread or never change (no
-  // mutex needed).
- public:
-  static long unsigned int nNextId;
-  long unsigned int mnId;
-  const long unsigned int mnFrameId;
-
-  const double mTimeStamp;
-
-  // Grid (to speed up feature matching)
-  const int mnGridCols;
-  const int mnGridRows;
-  const float mfGridElementWidthInv;
-  const float mfGridElementHeightInv;
-
-  // Variables used by the tracking
-  long unsigned int mnTrackReferenceForFrame;
-  long unsigned int mnFuseTargetForKF;
-
-  // Variables used by the local mapping
-  long unsigned int mnBALocalForKF;
-  long unsigned int mnBAFixedForKF;
-
-  // Number of optimizations by BA(amount of iterations in BA)
-  long unsigned int mnNumberOfOpt;
-
-  // Variables used by the keyframe database
-  long unsigned int mnLoopQuery;
-  int mnLoopWords;
-  float mLoopScore;
-  long unsigned int mnRelocQuery;
-  int mnRelocWords;
-  float mRelocScore;
-  long unsigned int mnMergeQuery;
-  int mnMergeWords;
-  float mMergeScore;
-  long unsigned int mnPlaceRecognitionQuery;
-  int mnPlaceRecognitionWords;
-  float mPlaceRecognitionScore;
-
-  bool mbCurrentPlaceRecognition;
-
-  // Variables used by loop closing
-  Sophus::SE3f mTcwGBA;
-  Sophus::SE3f mTcwBefGBA;
-  Eigen::Vector3f mVwbGBA;
-  Eigen::Vector3f mVwbBefGBA;
-  IMU::Bias mBiasGBA;
-  long unsigned int mnBAGlobalForKF;
-
-  // Variables used by merging
-  Sophus::SE3f mTcwMerge;
-  Sophus::SE3f mTcwBefMerge;
-  Sophus::SE3f mTwcBefMerge;
-  Eigen::Vector3f mVwbMerge;
-  Eigen::Vector3f mVwbBefMerge;
-  IMU::Bias mBiasMerge;
-  long unsigned int mnMergeCorrectedForKF;
-  long unsigned int mnMergeForKF;
-  float mfScaleMerge;
-  long unsigned int mnBALocalForMerge;
-
-  float mfScale;
-
-  // Calibration parameters
-  const float fx, fy, cx, cy, invfx, invfy, mbf, mb, mThDepth;
-  cv::Mat mDistCoef;
-
-  // Number of KeyPoints
-  const int N;
-
-  // KeyPoints, stereo coordinate and descriptors (all associated by an index)
-  const std::vector<cv::KeyPoint> mvKeys;
-  const std::vector<cv::KeyPoint> mvKeysUn;
-  const std::vector<float> mvuRight;  // negative value for monocular points
-  const std::vector<float> mvDepth;   // negative value for monocular points
-  const cv::Mat mDescriptors;
-
-  // BoW
-  DBoW2::BowVector mBowVec;
-  DBoW2::FeatureVector mFeatVec;
-
-  // Pose relative to parent (this is computed when bad flag is activated)
-  Sophus::SE3f mTcp;
-
-  // Scale
-  const int mnScaleLevels;
-  const float mfScaleFactor;
-  const float mfLogScaleFactor;
-  const std::vector<float> mvScaleFactors;
-  const std::vector<float> mvLevelSigma2;
-  const std::vector<float> mvInvLevelSigma2;
-
-  // Image bounds and calibration
-  const int mnMinX;
-  const int mnMinY;
-  const int mnMaxX;
-  const int mnMaxY;
-
-  // Preintegrated IMU measurements from previous keyframe
-  KeyFrame* mPrevKF;
-  KeyFrame* mNextKF;
-
-  IMU::Preintegrated* mpImuPreintegrated;
-  IMU::Calib mImuCalib;
-
-  unsigned int mnOriginMapId;
-
-  string mNameFile;
-
-  int mnDataset;
-
-  std::vector<KeyFrame*> mvpLoopCandKFs;
-  std::vector<KeyFrame*> mvpMergeCandKFs;
-
-  // bool mbHasHessian;
-  // cv::Mat mHessianPose;
-
-  // The following variables need to be accessed trough a mutex to be thread
-  // safe.
- protected:
-  // sophus poses
-  Sophus::SE3<float> mTcw;
-  Eigen::Matrix3f mRcw;
-  Sophus::SE3<float> mTwc;
-  Eigen::Matrix3f mRwc;
-
-  // IMU position
-  Eigen::Vector3f mOwb;
-  // Velocity (Only used for inertial SLAM)
-  Eigen::Vector3f mVw;
-  bool mbHasVelocity;
-
-  // Transformation matrix between cameras in stereo fisheye
-  Sophus::SE3<float> mTlr;
-  Sophus::SE3<float> mTrl;
-
-  // Imu bias
-  IMU::Bias mImuBias;
-
-  // MapPoints associated to keypoints
-  std::vector<MapPoint*> mvpMapPoints;
-  // For save relation without pointer, this is necessary for save/load function
-  std::vector<long long int> mvBackupMapPointsId;
-
-  // BoW
-  KeyFrameDatabase* mpKeyFrameDB;
-  ORBVocabulary* mpORBvocabulary;
-
-  // Grid over the image to speed up feature matching
-  std::vector<std::vector<std::vector<size_t> > > mGrid;
-
-  std::map<KeyFrame*, int> mConnectedKeyFrameWeights;
-  std::vector<KeyFrame*> mvpOrderedConnectedKeyFrames;
-  std::vector<int> mvOrderedWeights;
-  // For save relation without pointer, this is necessary for save/load function
-  std::map<long unsigned int, int> mBackupConnectedKeyFrameIdWeights;
-
-  // Spanning Tree and Loop Edges
-  bool mbFirstConnection;
-  KeyFrame* mpParent;
-  std::set<KeyFrame*> mspChildrens;
-  std::set<KeyFrame*> mspLoopEdges;
-  std::set<KeyFrame*> mspMergeEdges;
-  // For save relation without pointer, this is necessary for save/load function
-  long long int mBackupParentId;
-  std::vector<long unsigned int> mvBackupChildrensId;
-  std::vector<long unsigned int> mvBackupLoopEdgesId;
-  std::vector<long unsigned int> mvBackupMergeEdgesId;
-
-  // Bad flags
-  bool mbNotErase;
-  bool mbToBeErased;
-  bool mbBad;
-
-  float mHalfBaseline;  // Only for visualization
-
-  std::shared_ptr<Map> mpMap;
-
-  // Backup variables for inertial
-  long long int mBackupPrevKFId;
-  long long int mBackupNextKFId;
-  IMU::Preintegrated mBackupImuPreintegrated;
-
-  // Backup for Cameras
-  unsigned int mnBackupIdCamera, mnBackupIdCamera2;
-
-  // Calibration
-  Eigen::Matrix3f mK_;
-
-  // Mutex
-  std::mutex mMutexPose;  // for pose, velocity and biases
-  std::mutex mMutexConnections;
-  std::mutex mMutexFeatures;
-  std::mutex mMutexMap;
-
- public:
-  std::shared_ptr<GeometricCamera> mpCamera, mpCamera2;
-
-  // Indexes of stereo observations correspondences
-  std::vector<int> mvLeftToRightMatch, mvRightToLeftMatch;
-
-  Sophus::SE3f GetRelativePoseTrl();
-  Sophus::SE3f GetRelativePoseTlr();
-
-  // KeyPoints in the right image (for stereo fisheye, coordinates are needed)
-  const std::vector<cv::KeyPoint> mvKeysRight;
-
-  const int NLeft, NRight;
-
-  std::vector<std::vector<std::vector<size_t> > > mGridRight;
-
-  Sophus::SE3<float> GetRightPose();
-  Sophus::SE3<float> GetRightPoseInverse();
-
-  Eigen::Vector3f GetRightCameraCenter();
-  Eigen::Matrix<float, 3, 3> GetRightRotation();
-  Eigen::Vector3f GetRightTranslation();
-
-  void PrintPointDistribution() {
-    int left = 0, right = 0;
-    int Nlim = (NLeft != -1) ? NLeft : N;
-    for (int i = 0; i < N; i++) {
-      if (mvpMapPoints[i]) {
-        if (i < Nlim)
-          left++;
-        else
-          right++;
-      }
-    }
-    cout << "Point distribution in KeyFrame: left-> " << left << " --- right-> "
-         << right << endl;
-  }
-};
-
-}  // namespace MORB_SLAM
-
diff --git a/include/KeyFrameDatabase.h b/include/KeyFrameDatabase.h
deleted file mode 100644
index 370116de..00000000
--- a/include/KeyFrameDatabase.h
+++ /dev/null
@@ -1,99 +0,0 @@
-/**
- * This file is part of ORB-SLAM3
- *
- * Copyright (C) 2017-2021 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/>.
- */
-
-#pragma once
-
-#include <boost/serialization/base_object.hpp>
-#include <boost/serialization/list.hpp>
-#include <boost/serialization/vector.hpp>
-#include <list>
-#include <mutex>
-#include <set>
-#include <vector>
-
-#include "Frame.h"
-#include "KeyFrame.h"
-#include "Map.h"
-#include "ORBVocabulary.h"
-
-namespace MORB_SLAM {
-
-class KeyFrame;
-class Frame;
-class Map;
-
-class KeyFrameDatabase {
-  friend class boost::serialization::access;
-
-  template <class Archive>
-  void serialize(Archive& ar, const unsigned int version) {
-    ar& mvBackupInvertedFileId;
-  }
-
- public:
-  
-
-  KeyFrameDatabase() {}
-  KeyFrameDatabase(const ORBVocabulary& voc);
-
-  void add(KeyFrame* pKF);
-
-  void erase(KeyFrame* pKF);
-
-  void clear();
-  void clearMap(std::shared_ptr<Map> pMap);
-
-  // Loop Detection(DEPRECATED)
-  std::vector<KeyFrame*> DetectLoopCandidates(KeyFrame* pKF, float minScore);
-
-  // Loop and Merge Detection
-  void DetectCandidates(KeyFrame* pKF, float minScore,
-                        vector<KeyFrame*>& vpLoopCand,
-                        vector<KeyFrame*>& vpMergeCand);
-  void DetectBestCandidates(KeyFrame* pKF, vector<KeyFrame*>& vpLoopCand,
-                            vector<KeyFrame*>& vpMergeCand, int nMinWords);
-  void DetectNBestCandidates(KeyFrame* pKF, vector<KeyFrame*>& vpLoopCand,
-                             vector<KeyFrame*>& vpMergeCand,
-                             int nNumCandidates);
-
-  // Relocalization
-  std::vector<KeyFrame*> DetectRelocalizationCandidates(Frame* F, std::shared_ptr<Map> pMap);
-
-  void PreSave();
-  void PostLoad(map<long unsigned int, KeyFrame*> mpKFid);
-  void SetORBVocabulary(ORBVocabulary* pORBVoc);
-
- protected:
-  // Associated vocabulary
-  const ORBVocabulary* mpVoc;
-
-  // Inverted file
-  std::vector<list<KeyFrame*> > mvInvertedFile;
-
-  // For save relation without pointer, this is necessary for save/load function
-  std::vector<list<long unsigned int> > mvBackupInvertedFileId;
-
-  // Mutex
-  std::mutex mMutex;
-};
-
-}  // namespace MORB_SLAM
-
diff --git a/include/LocalMapping.h b/include/LocalMapping.h
deleted file mode 100644
index 397d1f41..00000000
--- a/include/LocalMapping.h
+++ /dev/null
@@ -1,198 +0,0 @@
-/**
- * This file is part of ORB-SLAM3
- *
- * Copyright (C) 2017-2021 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/>.
- */
-
-#pragma once
-
-#include <mutex>
-
-#include "ImprovedTypes.hpp"
-#include "Atlas.h"
-#include "KeyFrame.h"
-#include "KeyFrameDatabase.h"
-#include "LoopClosing.h"
-#include "Settings.h"
-#include "Tracking.h"
-
-namespace MORB_SLAM {
-
-class System;
-class Tracking;
-class LoopClosing;
-class Atlas;
-
-class LocalMapping {
- public:
-  
-  LocalMapping(System* pSys, const Atlas_ptr &pAtlas, const float bMonocular,
-               bool bInertial, const string& _strSeqName = std::string());
-
-  void SetLoopCloser(LoopClosing* pLoopCloser);
-
-  void SetTracker(Tracking* pTracker);
-
-  // Main function
-  void Run();
-
-  void InsertKeyFrame(KeyFrame* pKF);
-  void EmptyQueue();
-
-  // Thread Synch
-  void RequestStop();
-  void RequestReset();
-  void RequestResetActiveMap(std::shared_ptr<Map> pMap);
-  bool Stop();
-  void Release();
-  bool isStopped();
-  bool stopRequested();
-  bool AcceptKeyFrames();
-  void SetAcceptKeyFrames(bool flag);
-  bool SetNotStop(bool flag);
-
-  void InterruptBA();
-
-  void RequestFinish();
-  bool isFinished();
-
-  int KeyframesInQueue() {
-    unique_lock<std::mutex> lock(mMutexNewKFs);
-    return mlNewKeyFrames.size();
-  }
-
-  bool IsInitializing();
-  double GetCurrKFTime();
-  KeyFrame* GetCurrKF();
-
-  std::mutex mMutexImuInit;
-
-  Eigen::MatrixXd mcovInertial;
-  Eigen::Matrix3d mRwg;
-  Eigen::Vector3d mbg;
-  Eigen::Vector3d mba;
-  double mScale;
-  double mInitTime;
-  double mCostTime;
-
-  unsigned int mInitSect;
-  unsigned int mIdxInit;
-  unsigned int mnKFs;
-  double mFirstTs;
-  int mnMatchesInliers;
-
-  // For debugging (erase in normal mode)
-  int mIdxIteration;
-  string strSequence;
-
-  bool mbNotBA1;
-  bool mbNotBA2;
-  bool mbBadImu;
-
-  bool mbWriteStats;
-
-  // not consider far points (clouds)
-  bool mbFarPoints;
-  float mThFarPoints;
-
-#ifdef REGISTER_TIMES
-  vector<double> vdKFInsert_ms;
-  vector<double> vdMPCulling_ms;
-  vector<double> vdMPCreation_ms;
-  vector<double> vdLBA_ms;
-  vector<double> vdKFCulling_ms;
-  vector<double> vdLMTotal_ms;
-
-  vector<double> vdLBASync_ms;
-  vector<double> vdKFCullingSync_ms;
-  vector<int> vnLBA_edges;
-  vector<int> vnLBA_KFopt;
-  vector<int> vnLBA_KFfixed;
-  vector<int> vnLBA_MPs;
-  int nLBA_exec;
-  int nLBA_abort;
-#endif
- protected:
-  bool CheckNewKeyFrames();
-  void ProcessNewKeyFrame();
-  void CreateNewMapPoints();
-
-  void MapPointCulling();
-  void SearchInNeighbors();
-  void KeyFrameCulling();
-
-  System* mpSystem;
-
-  bool mbMonocular;
-  bool mbInertial;
-
-  void ResetIfRequested();
-  bool mbResetRequested;
-  bool mbResetRequestedActiveMap;
-  std::shared_ptr<Map> mpMapToReset;
-  std::mutex mMutexReset;
-
-  bool CheckFinish();
-  void SetFinish();
-  bool mbFinishRequested;
-  bool mbFinished;
-  std::mutex mMutexFinish;
-
-  Atlas_ptr mpAtlas;
-
-  LoopClosing* mpLoopCloser;
-  Tracking* mpTracker;
-
-  std::list<KeyFrame*> mlNewKeyFrames;
-
-  KeyFrame* mpCurrentKeyFrame;
-
-  std::list<MapPoint*> mlpRecentAddedMapPoints;
-
-  std::mutex mMutexNewKFs;
-
-  bool mbAbortBA;
-
-  bool mbStopped;
-  bool mbStopRequested;
-  bool mbNotStop;
-  std::mutex mMutexStop;
-
-  bool mbAcceptKeyFrames;
-  std::mutex mMutexAccept;
-
-  void InitializeIMU(float priorG = 1e2, float priorA = 1e6,
-                     bool bFirst = false);
-  void ScaleRefinement();
-
-  bool bInitializing;
-
-  Eigen::MatrixXd infoInertial;
-  int mNumLM;
-  int mNumKFCulling;
-
-  float mTinit;
-
-  int countRefinement;
-
-  // DEBUG
-  ofstream f_lm;
-};
-
-}  // namespace MORB_SLAM
-
diff --git a/include/LoopClosing.h b/include/LoopClosing.h
deleted file mode 100644
index d6265a66..00000000
--- a/include/LoopClosing.h
+++ /dev/null
@@ -1,245 +0,0 @@
-/**
-* This file is part of ORB-SLAM3
-*
-* Copyright (C) 2017-2021 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/>.
-*/
-
-
-#pragma once
-
-#include "ImprovedTypes.hpp"
-#include "KeyFrame.h"
-#include "LocalMapping.h"
-#include "Atlas.h"
-#include "ORBVocabulary.h"
-#include "Tracking.h"
-
-#include "KeyFrameDatabase.h"
-
-#include <boost/algorithm/string.hpp>
-#include <thread>
-#include <mutex>
-#include "g2o/types/types_seven_dof_expmap.h"
-
-namespace MORB_SLAM
-{
-
-class Tracking;
-class LocalMapping;
-class KeyFrameDatabase;
-class Map;
-
-
-class LoopClosing
-{
-public:
-
-    typedef pair<set<KeyFrame*>,int> ConsistentGroup;
-    typedef map<KeyFrame*,g2o::Sim3,std::less<KeyFrame*>,
-        Eigen::aligned_allocator<std::pair<KeyFrame* const, g2o::Sim3> > > KeyFrameAndPose;
-
-public:
-
-    LoopClosing(const Atlas_ptr &pAtlas, KeyFrameDatabase* pDB, ORBVocabulary* pVoc,const bool bFixScale, const bool bActiveLC);
-
-    void SetTracker(Tracking* pTracker);
-
-    void SetLocalMapper(LocalMapping* pLocalMapper);
-
-    // Main function
-    void Run();
-
-    void InsertKeyFrame(KeyFrame *pKF);
-
-    void RequestReset();
-    void RequestResetActiveMap(std::shared_ptr<Map> pMap);
-
-    // This function will run in a separate thread
-    void RunGlobalBundleAdjustment(std::shared_ptr<Map> pActiveMap, unsigned long nLoopKF);
-
-    bool isRunningGBA(){
-        unique_lock<std::mutex> lock(mMutexGBA);
-        return mbRunningGBA;
-    }
-    bool isFinishedGBA(){
-        unique_lock<std::mutex> lock(mMutexGBA);
-        return mbFinishedGBA;
-    }
-
-    void RequestFinish();
-
-    bool isFinished();
-
-#ifdef REGISTER_TIMES
-
-    vector<double> vdDataQuery_ms;
-    vector<double> vdEstSim3_ms;
-    vector<double> vdPRTotal_ms;
-
-    vector<double> vdMergeMaps_ms;
-    vector<double> vdWeldingBA_ms;
-    vector<double> vdMergeOptEss_ms;
-    vector<double> vdMergeTotal_ms;
-    vector<int> vnMergeKFs;
-    vector<int> vnMergeMPs;
-    int nMerges;
-
-    vector<double> vdLoopFusion_ms;
-    vector<double> vdLoopOptEss_ms;
-    vector<double> vdLoopTotal_ms;
-    vector<int> vnLoopKFs;
-    int nLoop;
-
-    vector<double> vdGBA_ms;
-    vector<double> vdUpdateMap_ms;
-    vector<double> vdFGBATotal_ms;
-    vector<int> vnGBAKFs;
-    vector<int> vnGBAMPs;
-    int nFGBA_exec;
-    int nFGBA_abort;
-
-#endif
-
-    
-
-protected:
-
-    bool CheckNewKeyFrames();
-
-
-    //Methods to implement the new place recognition algorithm
-    bool NewDetectCommonRegions();
-    bool DetectAndReffineSim3FromLastKF(KeyFrame* pCurrentKF, KeyFrame* pMatchedKF, g2o::Sim3 &gScw, int &nNumProjMatches,
-                                        std::vector<MapPoint*> &vpMPs, std::vector<MapPoint*> &vpMatchedMPs);
-    bool DetectCommonRegionsFromBoW(std::vector<KeyFrame*> &vpBowCand, KeyFrame* &pMatchedKF, KeyFrame* &pLastCurrentKF, g2o::Sim3 &g2oScw,
-                                     int &nNumCoincidences, std::vector<MapPoint*> &vpMPs, std::vector<MapPoint*> &vpMatchedMPs);
-    bool DetectCommonRegionsFromLastKF(KeyFrame* pCurrentKF, KeyFrame* pMatchedKF, g2o::Sim3 &gScw, int &nNumProjMatches,
-                                            std::vector<MapPoint*> &vpMPs, std::vector<MapPoint*> &vpMatchedMPs);
-    int FindMatchesByProjection(KeyFrame* pCurrentKF, KeyFrame* pMatchedKFw, g2o::Sim3 &g2oScw,
-                                set<MapPoint*> &spMatchedMPinOrigin, vector<MapPoint*> &vpMapPoints,
-                                vector<MapPoint*> &vpMatchedMapPoints);
-
-
-    void SearchAndFuse(const KeyFrameAndPose &CorrectedPosesMap, vector<MapPoint*> &vpMapPoints);
-    void SearchAndFuse(const vector<KeyFrame*> &vConectedKFs, vector<MapPoint*> &vpMapPoints);
-
-    void CorrectLoop();
-
-    void MergeLocal();
-    void MergeLocal2();
-
-    void CheckObservations(set<KeyFrame*> &spKFsMap1, set<KeyFrame*> &spKFsMap2);
-
-    void ResetIfRequested();
-    bool mbResetRequested;
-    bool mbResetActiveMapRequested;
-    std::shared_ptr<Map> mpMapToReset;
-    std::mutex mMutexReset;
-
-    bool CheckFinish();
-    void SetFinish();
-    bool mbFinishRequested;
-    bool mbFinished;
-    std::mutex mMutexFinish;
-
-    Atlas_ptr mpAtlas;
-    Tracking* mpTracker;
-
-    KeyFrameDatabase* mpKeyFrameDB;
-    ORBVocabulary* mpORBVocabulary;
-
-    LocalMapping *mpLocalMapper;
-
-    std::list<KeyFrame*> mlpLoopKeyFrameQueue;
-
-    std::mutex mMutexLoopQueue;
-
-    // Loop detector parameters
-    float mnCovisibilityConsistencyTh;
-
-    // Loop detector variables
-    KeyFrame* mpCurrentKF;
-    KeyFrame* mpLastCurrentKF;
-    KeyFrame* mpMatchedKF;
-    std::vector<ConsistentGroup> mvConsistentGroups;
-    std::vector<KeyFrame*> mvpEnoughConsistentCandidates;
-    std::vector<KeyFrame*> mvpCurrentConnectedKFs;
-    std::vector<MapPoint*> mvpCurrentMatchedPoints;
-    std::vector<MapPoint*> mvpLoopMapPoints;
-    cv::Mat mScw;
-    g2o::Sim3 mg2oScw;
-
-    //-------
-    std::shared_ptr<Map> mpLastMap;
-
-    bool mbLoopDetected;
-    int mnLoopNumCoincidences;
-    int mnLoopNumNotFound;
-    KeyFrame* mpLoopLastCurrentKF;
-    g2o::Sim3 mg2oLoopSlw;
-    g2o::Sim3 mg2oLoopScw;
-    KeyFrame* mpLoopMatchedKF;
-    std::vector<MapPoint*> mvpLoopMPs;
-    std::vector<MapPoint*> mvpLoopMatchedMPs;
-    bool mbMergeDetected;
-    int mnMergeNumCoincidences;
-    int mnMergeNumNotFound;
-    KeyFrame* mpMergeLastCurrentKF;
-    g2o::Sim3 mg2oMergeSlw;
-    g2o::Sim3 mg2oMergeSmw;
-    g2o::Sim3 mg2oMergeScw;
-    KeyFrame* mpMergeMatchedKF;
-    std::vector<MapPoint*> mvpMergeMPs;
-    std::vector<MapPoint*> mvpMergeMatchedMPs;
-    std::vector<KeyFrame*> mvpMergeConnectedKFs;
-
-    g2o::Sim3 mSold_new;
-    //-------
-
-    // Variables related to Global Bundle Adjustment
-    bool mbRunningGBA;
-    bool mbFinishedGBA;
-    bool mbStopGBA;
-    std::mutex mMutexGBA;
-    std::thread* mpThreadGBA;
-
-    // Fix scale in the stereo/RGB-D case
-    bool mbFixScale;
-
-
-    int mnFullBAIdx;
-
-
-
-    vector<double> vdPR_CurrentTime;
-    vector<double> vdPR_MatchedTime;
-    vector<int> vnPR_TypeRecogn;
-
-    //DEBUG
-    string mstrFolderSubTraj;
-    int mnNumCorrection;
-    int mnCorrectionGBA;
-
-
-    // To (de)activate LC
-    bool mbActiveLC = true;
-
-#ifdef REGISTER_LOOP
-    string mstrFolderLoop;
-#endif
-};
-
-} //namespace ORB_SLAM
-
diff --git a/include/MLPnPsolver.h b/include/MLPnPsolver.h
deleted file mode 100644
index 84a8a395..00000000
--- a/include/MLPnPsolver.h
+++ /dev/null
@@ -1,243 +0,0 @@
-/**
- * This file is part of ORB-SLAM3
- *
- * Copyright (C) 2017-2021 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/>.
- */
-
-/******************************************************************************
- * Author:   Steffen Urban                                              *
- * Contact:  urbste@gmail.com                                          *
- * License:  Copyright (c) 2016 Steffen Urban, ANU. All rights reserved.      *
- *                                                                            *
- * Redistribution and use in source and binary forms, with or without         *
- * modification, are permitted provided that the following conditions         *
- * are met:                                                                   *
- * * Redistributions of source code must retain the above copyright           *
- *   notice, this list of conditions and the following disclaimer.            *
- * * Redistributions in binary form must reproduce the above copyright        *
- *   notice, this list of conditions and the following disclaimer in the      *
- *   documentation and/or other materials provided with the distribution.     *
- * * Neither the name of ANU nor the names of its contributors may be         *
- *   used to endorse or promote products derived from this software without   *
- *   specific prior written permission.                                       *
- *                                                                            *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"*
- * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE  *
- * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE *
- * ARE DISCLAIMED. IN NO EVENT SHALL ANU OR THE CONTRIBUTORS BE LIABLE        *
- * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL *
- * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR *
- * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER *
- * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT         *
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY  *
- * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF     *
- * SUCH DAMAGE.                                                               *
- ******************************************************************************/
-
-#pragma once
-
-#include <Eigen/Dense>
-#include <Eigen/Sparse>
-
-#include "Frame.h"
-#include "MapPoint.h"
-
-namespace MORB_SLAM {
-class MLPnPsolver {
- public:
-  
-
-  MLPnPsolver(const Frame& F, const vector<MapPoint*>& vpMapPointMatches);
-
-  ~MLPnPsolver();
-
-  void SetRansacParameters(double probability = 0.99, int minInliers = 8,
-                           int maxIterations = 300, int minSet = 6,
-                           float epsilon = 0.4, float th2 = 5.991);
-
-  // Find metod is necessary?
-
-  bool iterate(int nIterations, bool& bNoMore, vector<bool>& vbInliers,
-               int& nInliers, Eigen::Matrix4f& Tout);
-
-  // Type definitions needed by the original code
-
-  /** A 3-vector of unit length used to describe landmark observations/bearings
-   *  in camera frames (always expressed in camera frames)
-   */
-  typedef Eigen::Vector3d bearingVector_t;
-
-  /** An array of bearing-vectors */
-  typedef std::vector<bearingVector_t,
-                      Eigen::aligned_allocator<bearingVector_t> >
-      bearingVectors_t;
-
-  /** A 2-matrix containing the 2D covariance information of a bearing vector
-   */
-  typedef Eigen::Matrix2d cov2_mat_t;
-
-  /** A 3-matrix containing the 3D covariance information of a bearing vector */
-  typedef Eigen::Matrix3d cov3_mat_t;
-
-  /** An array of 3D covariance matrices */
-  typedef std::vector<cov3_mat_t, Eigen::aligned_allocator<cov3_mat_t> >
-      cov3_mats_t;
-
-  /** A 3-vector describing a point in 3D-space */
-  typedef Eigen::Vector3d point_t;
-
-  /** An array of 3D-points */
-  typedef std::vector<point_t, Eigen::aligned_allocator<point_t> > points_t;
-
-  /** A homogeneous 3-vector describing a point in 3D-space */
-  typedef Eigen::Vector4d point4_t;
-
-  /** An array of homogeneous 3D-points */
-  typedef std::vector<point4_t, Eigen::aligned_allocator<point4_t> > points4_t;
-
-  /** A 3-vector containing the rodrigues parameters of a rotation matrix */
-  typedef Eigen::Vector3d rodrigues_t;
-
-  /** A rotation matrix */
-  typedef Eigen::Matrix3d rotation_t;
-
-  /** A 3x4 transformation matrix containing rotation \f$ \mathbf{R} \f$ and
-   *  translation \f$ \mathbf{t} \f$ as follows:
-   *  \f$ \left( \begin{array}{cc} \mathbf{R} & \mathbf{t} \end{array} \right)
-   * \f$
-   */
-  typedef Eigen::Matrix<double, 3, 4> transformation_t;
-
-  /** A 3-vector describing a translation/camera position */
-  typedef Eigen::Vector3d translation_t;
-
- private:
-  void CheckInliers();
-  bool Refine();
-
-  // Functions from de original MLPnP code
-
-  /*
-   * Computes the camera pose given 3D points coordinates (in the camera
-   * reference system), the camera rays and (optionally) the covariance matrix
-   * of those camera rays. Result is stored in solution
-   */
-  void computePose(const bearingVectors_t& f, const points_t& p,
-                   const cov3_mats_t& covMats, const std::vector<int>& indices,
-                   transformation_t& result);
-
-  void mlpnp_gn(Eigen::VectorXd& x, const points_t& pts,
-                const std::vector<Eigen::MatrixXd>& nullspaces,
-                const Eigen::SparseMatrix<double> Kll, bool use_cov);
-
-  void mlpnp_residuals_and_jacs(const Eigen::VectorXd& x, const points_t& pts,
-                                const std::vector<Eigen::MatrixXd>& nullspaces,
-                                Eigen::VectorXd& r, Eigen::MatrixXd& fjac,
-                                bool getJacs);
-
-  void mlpnpJacs(const point_t& pt, const Eigen::Vector3d& nullspace_r,
-                 const Eigen::Vector3d& nullspace_s, const rodrigues_t& w,
-                 const translation_t& t, Eigen::MatrixXd& jacs);
-
-  // Auxiliar methods
-
-  /**
-   * \brief Compute a rotation matrix from Rodrigues axis angle.
-   *
-   * \param[in] omega The Rodrigues-parameters of a rotation.
-   * \return The 3x3 rotation matrix.
-   */
-  Eigen::Matrix3d rodrigues2rot(const Eigen::Vector3d& omega);
-
-  /**
-   * \brief Compute the Rodrigues-parameters of a rotation matrix.
-   *
-   * \param[in] R The 3x3 rotation matrix.
-   * \return The Rodrigues-parameters.
-   */
-  Eigen::Vector3d rot2rodrigues(const Eigen::Matrix3d& R);
-
-  //----------------------------------------------------
-  // Fields of the solver
-  //----------------------------------------------------
-  vector<MapPoint*> mvpMapPointMatches;
-
-  // 2D Points
-  vector<cv::Point2f> mvP2D;
-  // Substitued by bearing vectors
-  bearingVectors_t mvBearingVecs;
-
-  vector<float> mvSigma2;
-
-  // 3D Points
-  // vector<cv::Point3f> mvP3Dw;
-  points_t mvP3Dw;
-
-  // Index in Frame
-  vector<size_t> mvKeyPointIndices;
-
-  // Current Estimation
-  double mRi[3][3];
-  double mti[3];
-  Eigen::Matrix4f mTcwi;
-  vector<bool> mvbInliersi;
-  int mnInliersi;
-
-  // Current Ransac State
-  int mnIterations;
-  vector<bool> mvbBestInliers;
-  int mnBestInliers;
-  Eigen::Matrix4f mBestTcw;
-
-  // Refined
-  Eigen::Matrix4f mRefinedTcw;
-  vector<bool> mvbRefinedInliers;
-  int mnRefinedInliers;
-
-  // Number of Correspondences
-  int N;
-
-  // Indices for random selection [0 .. N-1]
-  vector<size_t> mvAllIndices;
-
-  // RANSAC probability
-  double mRansacProb;
-
-  // RANSAC min inliers
-  int mRansacMinInliers;
-
-  // RANSAC max iterations
-  int mRansacMaxIts;
-
-  // RANSAC expected inliers/total ratio
-  float mRansacEpsilon;
-
-  // RANSAC Threshold inlier/outlier. Max error e = dist(P1,T_12*P2)^2
-  float mRansacTh;
-
-  // RANSAC Minimun Set used at each iteration
-  int mRansacMinSet;
-
-  // Max square error associated with scale level. Max error =
-  // th*th*sigma(level)*sigma(level)
-  vector<float> mvMaxError;
-
-  std::shared_ptr<GeometricCamera> mpCamera;
-};
-
-}  // namespace MORB_SLAM
diff --git a/include/Map.h b/include/Map.h
deleted file mode 100644
index 5d8301db..00000000
--- a/include/Map.h
+++ /dev/null
@@ -1,205 +0,0 @@
-/**
-* This file is part of ORB-SLAM3
-*
-* Copyright (C) 2017-2021 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/>.
-*/
-
-
-#pragma once
-
-#include "MapPoint.h"
-#include "KeyFrame.h"
-
-#include <set>
-#include <pangolin/pangolin.h>
-#include <mutex>
-#include <memory>
-#include <boost/serialization/base_object.hpp>
-#include <stdexcept>
-
-
-namespace MORB_SLAM
-{
-
-class MapPoint;
-class KeyFrame;
-class Atlas;
-class KeyFrameDatabase;
-
-class Map
-{
-    friend class boost::serialization::access;
-
-    template<class Archive>
-    void serialize(Archive &ar, const unsigned int version)
-    {
-        ar & mnId;
-        ar & mnInitKFid;
-        ar & mnMaxKFid;
-        ar & mnBigChangeIdx;
-
-        // Save/load a set structure, the set structure is broken in libboost 1.58 for ubuntu 16.04, a vector is serializated
-        //ar & mspKeyFrames;
-        //ar & mspMapPoints;
-        ar & mvpBackupKeyFrames;
-        ar & mvpBackupMapPoints;
-
-        ar & mvBackupKeyFrameOriginsId;
-
-        ar & mnBackupKFinitialID;
-        ar & mnBackupKFlowerID;
-
-        ar & mbImuInitialized;
-        ar & mbIsInertial;
-        ar & mbIMU_BA1;
-        ar & mbIMU_BA2;
-    }
-
-public:
-    
-    Map();
-    Map(int initKFid);
-    ~Map();
-
-    void AddKeyFrame(KeyFrame* pKF);
-    void AddMapPoint(MapPoint* pMP);
-    void EraseMapPoint(MapPoint* pMP);
-    void EraseKeyFrame(KeyFrame* pKF);
-    void SetReferenceMapPoints(const std::vector<MapPoint*> &vpMPs);
-    void InformNewBigChange();
-    int GetLastBigChangeIdx();
-
-    std::vector<KeyFrame*> GetAllKeyFrames();
-    std::vector<MapPoint*> GetAllMapPoints();
-    std::vector<MapPoint*> GetReferenceMapPoints();
-
-    long unsigned int MapPointsInMap();
-    long unsigned  KeyFramesInMap();
-
-    long unsigned int GetId();
-
-    long unsigned int GetInitKFid();
-    void SetInitKFid(long unsigned int initKFif);
-    long unsigned int GetMaxKFid();
-
-    KeyFrame* GetOriginKF();
-
-    void SetCurrentMap();
-    void SetStoredMap();
-
-    bool HasThumbnail();
-    bool IsInUse();
-
-    void SetBad();
-    bool IsBad();
-
-    void clear();
-
-    int GetMapChangeIndex();
-    void IncreaseChangeIndex();
-    int GetLastMapChange();
-    void SetLastMapChange(int currentChangeId);
-
-    void SetImuInitialized();
-    bool isImuInitialized();
-
-    void ApplyScaledRotation(const Sophus::SE3f &T, const float s, const bool bScaledVel=false);
-
-    void SetInertialSensor();
-    bool IsInertial();
-    void SetIniertialBA1();
-    void SetIniertialBA2();
-    bool GetIniertialBA1();
-    bool GetIniertialBA2();
-
-    void PrintEssentialGraph();
-    bool CheckEssentialGraph();
-    void ChangeId(long unsigned int nId);
-
-    unsigned int GetLowerKFID();
-
-    void PreSave(std::set<std::shared_ptr<GeometricCamera>> &spCams, std::shared_ptr<Map> sharedMap);
-    void PostLoad(KeyFrameDatabase* pKFDB, ORBVocabulary* pORBVoc/*, map<long unsigned int, KeyFrame*>& mpKeyFrameId*/, map<unsigned int, std::shared_ptr<GeometricCamera>> &mpCams, std::shared_ptr<Map> sharedMap);
-
-    void printReprojectionError(list<KeyFrame*> &lpLocalWindowKFs, KeyFrame* mpCurrentKF, string &name, string &name_folder);
-
-    vector<KeyFrame*> mvpKeyFrameOrigins;
-    vector<unsigned long int> mvBackupKeyFrameOriginsId;
-    KeyFrame* mpFirstRegionKF;
-    std::mutex mMutexMapUpdate;
-
-    // This avoid that two points are created simultaneously in separate threads (id conflict)
-    std::mutex mMutexPointCreation;
-
-    bool mbFail;
-
-    // Size of the thumbnail (always in power of 2)
-    static const int THUMB_WIDTH = 512;
-    static const int THUMB_HEIGHT = 512;
-
-    static long unsigned int nNextId;
-
-    // DEBUG: show KFs which are used in LBA
-    std::set<long unsigned int> msOptKFs;
-    std::set<long unsigned int> msFixedKFs;
-
-protected:
-
-    long unsigned int mnId;
-
-    std::set<MapPoint*> mspMapPoints;
-    std::set<KeyFrame*> mspKeyFrames;
-
-    // Save/load, the set structure is broken in libboost 1.58 for ubuntu 16.04, a vector is serializated
-    std::vector<MapPoint*> mvpBackupMapPoints;
-    std::vector<KeyFrame*> mvpBackupKeyFrames;
-
-    KeyFrame* mpKFinitial;
-    KeyFrame* mpKFlowerID;
-
-    long int mnBackupKFinitialID;
-    long int mnBackupKFlowerID;
-
-    std::vector<MapPoint*> mvpReferenceMapPoints;
-
-    bool mbImuInitialized;
-
-    int mnMapChange;
-    int mnMapChangeNotified;
-
-    long unsigned int mnInitKFid;
-    long unsigned int mnMaxKFid;
-
-    // Index related to a big change in the map (loop closure, global BA)
-    int mnBigChangeIdx;
-
-
-    // View of the map in aerial sight (for the AtlasViewer)
-    GLubyte* mThumbnail;
-
-    bool mIsInUse;
-    bool mHasTumbnail;
-    bool mbBad = false;
-
-    bool mbIsInertial;
-    bool mbIMU_BA1;
-    bool mbIMU_BA2;
-
-    // Mutex
-    std::mutex mMutexMap;
-
-};
-
-} //namespace MORB_SLAM
diff --git a/include/MapDrawer.h b/include/MapDrawer.h
deleted file mode 100644
index dc6259fd..00000000
--- a/include/MapDrawer.h
+++ /dev/null
@@ -1,73 +0,0 @@
-/**
-* This file is part of ORB-SLAM3
-*
-* Copyright (C) 2017-2021 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/>.
-*/
-
-
-#pragma once
-
-#include "ImprovedTypes.hpp"
-#include "Settings.h"
-#include<pangolin/pangolin.h>
-
-#include<mutex>
-
-namespace MORB_SLAM
-{
-
-class Settings;
-class KeyFrame;
-
-class MapDrawer
-{
-    void newParameterLoader(const Settings& settings);
-    Atlas_ptr mpAtlas;
-    
-    bool ParseViewerParamFile(cv::FileStorage &fSettings);
-
-    float mKeyFrameSize;
-    float mKeyFrameLineWidth;
-    float mGraphLineWidth;
-    float mPointSize;
-    float mCameraSize;
-    float mCameraLineWidth;
-
-    Sophus::SE3f mCameraPose;
-
-    std::mutex mMutexCamera;
-
-    float mfFrameColors[6][3] = {{0.0f, 0.0f, 1.0f},
-                                {0.8f, 0.4f, 1.0f},
-                                {1.0f, 0.2f, 0.4f},
-                                {0.6f, 0.0f, 1.0f},
-                                {1.0f, 1.0f, 0.0f},
-                                {0.0f, 1.0f, 1.0f}};
-
-public:
-    MapDrawer(const Atlas_ptr &pAtlas, const std::string &strSettingPath);
-    MapDrawer(const Atlas_ptr &pAtlas, const Settings& settings);
-
-    void DrawMapPoints();
-    void DrawKeyFrames(const bool bDrawKF, const bool bDrawGraph, const bool bDrawInertialGraph, const bool bDrawOptLba);
-    void DrawCurrentCamera(pangolin::OpenGlMatrix &Twc);
-    void SetCurrentCameraPose(const Sophus::SE3f &Tcw);
-    void SetReferenceKeyFrame(KeyFrame *pKF);
-    void GetCurrentOpenGLCameraMatrix(pangolin::OpenGlMatrix &M, pangolin::OpenGlMatrix &MOw);
-
-};
-
-} //namespace ORB_SLAM
-
diff --git a/include/MapPoint.h b/include/MapPoint.h
deleted file mode 100644
index 87342477..00000000
--- a/include/MapPoint.h
+++ /dev/null
@@ -1,253 +0,0 @@
-/**
-* This file is part of ORB-SLAM3
-*
-* Copyright (C) 2017-2021 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/>.
-*/
-
-
-#pragma once
-
-#include "KeyFrame.h"
-#include "Frame.h"
-#include "Map.h"
-#include "Converter.h"
-
-#include "SerializationUtils.h"
-
-#include <opencv2/core/core.hpp>
-#include <mutex>
-
-#include <boost/serialization/serialization.hpp>
-#include <boost/serialization/array.hpp>
-#include <boost/serialization/map.hpp>
-
-namespace MORB_SLAM
-{
-
-class KeyFrame;
-class Map;
-class Frame;
-
-class MapPoint
-{
-
-    friend class boost::serialization::access;
-    template<class Archive>
-    void serialize(Archive & ar, const unsigned int version)
-    {
-        ar & mnId;
-        ar & mnFirstKFid;
-        ar & mnFirstFrame;
-        ar & nObs;
-        // Variables used by the tracking
-        //ar & mTrackProjX;
-        //ar & mTrackProjY;
-        //ar & mTrackDepth;
-        //ar & mTrackDepthR;
-        //ar & mTrackProjXR;
-        //ar & mTrackProjYR;
-        //ar & mbTrackInView;
-        //ar & mbTrackInViewR;
-        //ar & mnTrackScaleLevel;
-        //ar & mnTrackScaleLevelR;
-        //ar & mTrackViewCos;
-        //ar & mTrackViewCosR;
-        //ar & mnTrackReferenceForFrame;
-        //ar & mnLastFrameSeen;
-
-        // Variables used by local mapping
-        //ar & mnBALocalForKF;
-        //ar & mnFuseCandidateForKF;
-
-        // Variables used by loop closing and merging
-        //ar & mnLoopPointForKF;
-        //ar & mnCorrectedByKF;
-        //ar & mnCorrectedReference;
-        //serializeMatrix(ar,mPosGBA,version);
-        //ar & mnBAGlobalForKF;
-        //ar & mnBALocalForMerge;
-        //serializeMatrix(ar,mPosMerge,version);
-        //serializeMatrix(ar,mNormalVectorMerge,version);
-
-        // Protected variables
-        ar & boost::serialization::make_array(mWorldPos.data(), mWorldPos.size());
-        ar & boost::serialization::make_array(mNormalVector.data(), mNormalVector.size());
-        //ar & BOOST_SERIALIZATION_NVP(mBackupObservationsId);
-        //ar & mObservations;
-        ar & mBackupObservationsId1;
-        ar & mBackupObservationsId2;
-        serializeMatrix(ar,mDescriptor,version);
-        ar & mBackupRefKFId;
-        //ar & mnVisible;
-        //ar & mnFound;
-
-        ar & mbBad;
-        ar & mBackupReplacedId;
-
-        ar & mfMinDistance;
-        ar & mfMaxDistance;
-
-    }
-
-
-public:
-    
-    MapPoint();
-
-    MapPoint(const Eigen::Vector3f &Pos, KeyFrame* pRefKF, std::shared_ptr<Map> pMap);
-    MapPoint(const double invDepth, cv::Point2f uv_init, KeyFrame* pRefKF, KeyFrame* pHostKF, std::shared_ptr<Map> pMap);
-    MapPoint(const Eigen::Vector3f &Pos,  std::shared_ptr<Map> pMap, Frame* pFrame, const int &idxF);
-
-    void SetWorldPos(const Eigen::Vector3f &Pos);
-    Eigen::Vector3f GetWorldPos();
-
-    Eigen::Vector3f GetNormal();
-    void SetNormalVector(const Eigen::Vector3f& normal);
-
-    KeyFrame* GetReferenceKeyFrame();
-
-    std::map<KeyFrame*,std::tuple<int,int>> GetObservations();
-    int Observations();
-
-    void AddObservation(KeyFrame* pKF,int idx);
-    void EraseObservation(KeyFrame* pKF);
-
-    std::tuple<int,int> GetIndexInKeyFrame(KeyFrame* pKF);
-    bool IsInKeyFrame(KeyFrame* pKF);
-
-    void SetBadFlag();
-    bool isBad();
-
-    void Replace(MapPoint* pMP);
-    MapPoint* GetReplaced();
-
-    void IncreaseVisible(int n=1);
-    void IncreaseFound(int n=1);
-    float GetFoundRatio();
-    inline int GetFound(){
-        return mnFound;
-    }
-
-    void ComputeDistinctiveDescriptors();
-
-    cv::Mat GetDescriptor();
-
-    void UpdateNormalAndDepth();
-
-    float GetMinDistanceInvariance();
-    float GetMaxDistanceInvariance();
-    int PredictScale(const float &currentDist, KeyFrame*pKF);
-    int PredictScale(const float &currentDist, Frame* pF);
-
-    std::shared_ptr<Map> GetMap();
-    void UpdateMap(std::shared_ptr<Map> pMap);
-
-    void PrintObservations();
-
-    void PreSave(set<KeyFrame*>& spKF,set<MapPoint*>& spMP);
-    void PostLoad(map<long unsigned int, KeyFrame*>& mpKFid, map<long unsigned int, MapPoint*>& mpMPid);
-
-public:
-    long unsigned int mnId;
-    static long unsigned int nNextId;
-    long int mnFirstKFid;
-    long int mnFirstFrame;
-    int nObs;
-
-    // Variables used by the tracking
-    float mTrackProjX;
-    float mTrackProjY;
-    float mTrackDepth;
-    float mTrackDepthR;
-    float mTrackProjXR;
-    float mTrackProjYR;
-    bool mbTrackInView, mbTrackInViewR;
-    int mnTrackScaleLevel, mnTrackScaleLevelR;
-    float mTrackViewCos, mTrackViewCosR;
-    long unsigned int mnTrackReferenceForFrame;
-    long unsigned int mnLastFrameSeen;
-
-    // Variables used by local mapping
-    long unsigned int mnBALocalForKF;
-    long unsigned int mnFuseCandidateForKF;
-
-    // Variables used by loop closing
-    long unsigned int mnLoopPointForKF;
-    long unsigned int mnCorrectedByKF;
-    long unsigned int mnCorrectedReference;
-    Eigen::Vector3f mPosGBA;
-    long unsigned int mnBAGlobalForKF;
-    long unsigned int mnBALocalForMerge;
-
-    // Variable used by merging
-    Eigen::Vector3f mPosMerge;
-    Eigen::Vector3f mNormalVectorMerge;
-
-
-    // Fopr inverse depth optimization
-    double mInvDepth;
-    double mInitU;
-    double mInitV;
-    KeyFrame* mpHostKF;
-
-    static std::mutex mGlobalMutex;
-
-    unsigned int mnOriginMapId;
-
-protected:
-
-     // Position in absolute coordinates
-     Eigen::Vector3f mWorldPos;
-
-     // Keyframes observing the point and associated index in keyframe
-     std::map<KeyFrame*,std::tuple<int,int> > mObservations;
-     // For save relation without pointer, this is necessary for save/load function
-     std::map<long unsigned int, int> mBackupObservationsId1;
-     std::map<long unsigned int, int> mBackupObservationsId2;
-
-     // Mean viewing direction
-     Eigen::Vector3f mNormalVector;
-
-     // Best descriptor to fast matching
-     cv::Mat mDescriptor;
-
-     // Reference KeyFrame
-     KeyFrame* mpRefKF;
-     long unsigned int mBackupRefKFId;
-
-     // Tracking counters
-     int mnVisible;
-     int mnFound;
-
-     // Bad flag (we do not currently erase MapPoint from memory)
-     bool mbBad;
-     MapPoint* mpReplaced;
-     // For save relation without pointer, this is necessary for save/load function
-     long long int mBackupReplacedId;
-
-     // Scale invariance distances
-     float mfMinDistance;
-     float mfMaxDistance;
-
-     std::shared_ptr<Map> mpMap;
-
-     // Mutex
-     std::mutex mMutexPos;
-     std::mutex mMutexFeatures;
-     std::mutex mMutexMap;
-
-};
-
-} //namespace ORB_SLAM
diff --git a/include/ORBVocabulary.h b/include/ORBVocabulary.h
deleted file mode 100644
index 0558266d..00000000
--- a/include/ORBVocabulary.h
+++ /dev/null
@@ -1,32 +0,0 @@
-/**
- * This file is part of ORB-SLAM3
- *
- * Copyright (C) 2017-2021 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/>.
- */
-
-#pragma once
-
-#include "DBoW2/FORB.h"
-#include "DBoW2/TemplatedVocabulary.h"
-
-namespace MORB_SLAM {
-
-typedef DBoW2::TemplatedVocabulary<DBoW2::FORB::TDescriptor, DBoW2::FORB>
-    ORBVocabulary;
-
-}  // namespace MORB_SLAM
diff --git a/include/ORBextractor.h b/include/ORBextractor.h
deleted file mode 100644
index fafa90a9..00000000
--- a/include/ORBextractor.h
+++ /dev/null
@@ -1,102 +0,0 @@
-/**
- * This file is part of ORB-SLAM3
- *
- * Copyright (C) 2017-2021 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/>.
- */
-
-#pragma once
-
-#include <list>
-#include <opencv2/opencv.hpp>
-#include <vector>
-
-namespace MORB_SLAM {
-
-class ExtractorNode {
- public:
-  ExtractorNode() : bNoMore(false) {}
-  ExtractorNode(const cv::Point2i &UL, const cv::Point2i &UR, const cv::Point2i &BL, const cv::Point2i &BR, int keySize) :
-    UL{UL}, UR{UR}, BL{BL}, BR{BR}, bNoMore(false) { vKeys.reserve(keySize); }
-
-  void DivideNode(ExtractorNode &n1, ExtractorNode &n2, ExtractorNode &n3,
-                  ExtractorNode &n4);
-
-  std::vector<cv::KeyPoint> vKeys;
-  cv::Point2i UL, UR, BL, BR;
-  std::list<ExtractorNode>::iterator lit;
-  bool bNoMore;
-};
-
-class ORBextractor {
- public:
-  ORBextractor(int nfeatures, float scaleFactor, int nlevels, int iniThFAST,
-               int minThFAST);
-
-  // Compute the ORB features and descriptors on an image.
-  // ORB are dispersed on the image using an octree.
-  // Mask is ignored in the current implementation.
-  int operator()(cv::InputArray _image, cv::InputArray _mask,
-                 std::vector<cv::KeyPoint> &_keypoints,
-                 cv::OutputArray _descriptors, std::vector<int> &vLappingArea);
-
-  int inline GetLevels() { return nlevels; }
-
-  float inline GetScaleFactor() { return scaleFactor; }
-
-  std::vector<float> inline GetScaleFactors() { return mvScaleFactor; }
-
-  std::vector<float> inline GetInverseScaleFactors() {
-    return mvInvScaleFactor;
-  }
-
-  std::vector<float> inline GetScaleSigmaSquares() { return mvLevelSigma2; }
-
-  std::vector<float> inline GetInverseScaleSigmaSquares() {
-    return mvInvLevelSigma2;
-  }
-
-  std::vector<cv::Mat> mvImagePyramid;
-
- protected:
-  void ComputePyramid(cv::Mat image);
-  void ComputeKeyPointsOctTree(
-      std::vector<std::vector<cv::KeyPoint> > &allKeypoints);
-  std::vector<cv::KeyPoint> DistributeOctTree(
-      const std::vector<cv::KeyPoint> &vToDistributeKeys, const int &minX,
-      const int &maxX, const int &minY, const int &maxY, const int &nFeatures,
-      const int &level);
-
-  std::vector<cv::Point> pattern;
-
-  int nfeatures;
-  double scaleFactor;
-  int nlevels;
-  int iniThFAST;
-  int minThFAST;
-
-  std::vector<int> mnFeaturesPerLevel;
-
-  std::vector<int> umax;
-
-  std::vector<float> mvScaleFactor;
-  std::vector<float> mvInvScaleFactor;
-  std::vector<float> mvLevelSigma2;
-  std::vector<float> mvInvLevelSigma2;
-};
-
-}  // namespace MORB_SLAM
diff --git a/include/ORBmatcher.h b/include/ORBmatcher.h
deleted file mode 100644
index f7a94f56..00000000
--- a/include/ORBmatcher.h
+++ /dev/null
@@ -1,131 +0,0 @@
-/**
- * This file is part of ORB-SLAM3
- *
- * Copyright (C) 2017-2021 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/>.
- */
-
-#pragma once
-
-#include <opencv2/core/core.hpp>
-#include <opencv2/features2d/features2d.hpp>
-#include <vector>
-
-#include "Frame.h"
-#include "KeyFrame.h"
-#include "MapPoint.h"
-#include "sophus/sim3.hpp"
-
-namespace MORB_SLAM {
-
-class ORBmatcher {
- public:
-  ORBmatcher(float nnratio = 0.6, bool checkOri = true);
-
-  // Computes the Hamming distance between two ORB descriptors
-  static int DescriptorDistance(const cv::Mat &a, const cv::Mat &b);
-
-  // Search matches between Frame keypoints and projected MapPoints. Returns
-  // number of matches Used to track the local map (Tracking)
-  int SearchByProjection(Frame &F, const std::vector<MapPoint *> &vpMapPoints,
-                         const float th = 3, const bool bFarPoints = false,
-                         const float thFarPoints = 50.0f);
-
-  // Project MapPoints tracked in last frame into the current frame and search
-  // matches. Used to track from previous frame (Tracking)
-  int SearchByProjection(Frame &CurrentFrame, const Frame &LastFrame,
-                         const float th, const bool bMono);
-
-  // Project MapPoints seen in KeyFrame into the Frame and search matches.
-  // Used in relocalisation (Tracking)
-  int SearchByProjection(Frame &CurrentFrame, KeyFrame *pKF,
-                         const std::set<MapPoint *> &sAlreadyFound,
-                         const float th, const int ORBdist);
-
-  // Project MapPoints using a Similarity Transformation and search matches.
-  // Used in loop detection (Loop Closing)
-  int SearchByProjection(KeyFrame *pKF, Sophus::Sim3<float> &Scw,
-                         const std::vector<MapPoint *> &vpPoints,
-                         std::vector<MapPoint *> &vpMatched, int th,
-                         float ratioHamming = 1.0);
-
-  // Project MapPoints using a Similarity Transformation and search matches.
-  // Used in Place Recognition (Loop Closing and Merging)
-  int SearchByProjection(KeyFrame *pKF, Sophus::Sim3<float> &Scw,
-                         const std::vector<MapPoint *> &vpPoints,
-                         const std::vector<KeyFrame *> &vpPointsKFs,
-                         std::vector<MapPoint *> &vpMatched,
-                         std::vector<KeyFrame *> &vpMatchedKF, int th,
-                         float ratioHamming = 1.0);
-
-  // Search matches between MapPoints in a KeyFrame and ORB in a Frame.
-  // Brute force constrained to ORB that belong to the same vocabulary node (at
-  // a certain level) Used in Relocalisation and Loop Detection
-  int SearchByBoW(KeyFrame *pKF, Frame &F,
-                  std::vector<MapPoint *> &vpMapPointMatches);
-  int SearchByBoW(KeyFrame *pKF1, KeyFrame *pKF2,
-                  std::vector<MapPoint *> &vpMatches12);
-
-  // Matching for the Map Initialization (only used in the monocular case)
-  int SearchForInitialization(Frame &F1, Frame &F2,
-                              std::vector<cv::Point2f> &vbPrevMatched,
-                              std::vector<int> &vnMatches12,
-                              int windowSize = 10);
-
-  // Matching to triangulate new MapPoints. Check Epipolar Constraint.
-  int SearchForTriangulation(KeyFrame *pKF1, KeyFrame *pKF2,
-                             std::vector<pair<size_t, size_t> > &vMatchedPairs,
-                             const bool bOnlyStereo,
-                             const bool bCoarse = false);
-
-  // Search matches between MapPoints seen in KF1 and KF2 transforming by a Sim3
-  // [s12*R12|t12] In the stereo and RGB-D case, s12=1 int
-  // SearchBySim3(KeyFrame* pKF1, KeyFrame* pKF2, std::vector<MapPoint *>
-  // &vpMatches12, const float &s12, const cv::Mat &R12, const cv::Mat &t12,
-  // const float th);
-  int SearchBySim3(KeyFrame *pKF1, KeyFrame *pKF2,
-                   std::vector<MapPoint *> &vpMatches12,
-                   const Sophus::Sim3f &S12, const float th);
-
-  // Project MapPoints into KeyFrame and search for duplicated MapPoints.
-  int Fuse(KeyFrame *pKF, const vector<MapPoint *> &vpMapPoints,
-           const float th = 3.0, const bool bRight = false);
-
-  // Project MapPoints into KeyFrame using a given Sim3 and search for
-  // duplicated MapPoints.
-  int Fuse(KeyFrame *pKF, Sophus::Sim3f &Scw,
-           const std::vector<MapPoint *> &vpPoints, float th,
-           vector<MapPoint *> &vpReplacePoint);
-
- public:
-  static const int TH_LOW;
-  static const int TH_HIGH;
-  static const int HISTO_LENGTH;
-  
-
- protected:
-  float RadiusByViewingCos(const float &viewCos);
-
-  void ComputeThreeMaxima(std::vector<int> *histo, const int L, int &ind1,
-                          int &ind2, int &ind3);
-
-  float mfNNratio;
-  bool mbCheckOrientation;
-};
-
-}  // namespace MORB_SLAM
-
diff --git a/include/OptimizableTypes.h b/include/OptimizableTypes.h
deleted file mode 100644
index 601e2952..00000000
--- a/include/OptimizableTypes.h
+++ /dev/null
@@ -1,233 +0,0 @@
-/**
- * This file is part of ORB-SLAM3
- *
- * Copyright (C) 2017-2021 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/>.
- */
-
-#pragma once
-
-#include <GeometricCamera.h>
-#include <g2o/types/sim3.h>
-#include <g2o/types/types_six_dof_expmap.h>
-
-#include <Eigen/Geometry>
-
-#include "g2o/core/base_unary_edge.h"
-
-namespace MORB_SLAM {
-class EdgeSE3ProjectXYZOnlyPose
-    : public g2o::BaseUnaryEdge<2, Eigen::Vector2d, g2o::VertexSE3Expmap> {
- public:
-  
-
-  EdgeSE3ProjectXYZOnlyPose() {}
-
-  bool read(std::istream& is);
-
-  bool write(std::ostream& os) const;
-
-  void computeError() {
-    const g2o::VertexSE3Expmap* v1 =
-        static_cast<const g2o::VertexSE3Expmap*>(_vertices[0]);
-    Eigen::Vector2d obs(_measurement);
-    _error = obs - pCamera->project(v1->estimate().map(Xw));
-  }
-
-  bool isDepthPositive() {
-    const g2o::VertexSE3Expmap* v1 =
-        static_cast<const g2o::VertexSE3Expmap*>(_vertices[0]);
-    return (v1->estimate().map(Xw))(2) > 0.0;
-  }
-
-  virtual void linearizeOplus();
-
-  Eigen::Vector3d Xw;
-  std::shared_ptr<GeometricCamera> pCamera;
-};
-
-class EdgeSE3ProjectXYZOnlyPoseToBody
-    : public g2o::BaseUnaryEdge<2, Eigen::Vector2d, g2o::VertexSE3Expmap> {
- public:
-  
-
-  EdgeSE3ProjectXYZOnlyPoseToBody() {}
-
-  bool read(std::istream& is);
-
-  bool write(std::ostream& os) const;
-
-  void computeError() {
-    const g2o::VertexSE3Expmap* v1 =
-        static_cast<const g2o::VertexSE3Expmap*>(_vertices[0]);
-    Eigen::Vector2d obs(_measurement);
-    _error = obs - pCamera->project((mTrl * v1->estimate()).map(Xw));
-  }
-
-  bool isDepthPositive() {
-    const g2o::VertexSE3Expmap* v1 =
-        static_cast<const g2o::VertexSE3Expmap*>(_vertices[0]);
-    return ((mTrl * v1->estimate()).map(Xw))(2) > 0.0;
-  }
-
-  virtual void linearizeOplus();
-
-  Eigen::Vector3d Xw;
-  std::shared_ptr<GeometricCamera> pCamera;
-
-  g2o::SE3Quat mTrl;
-};
-
-class EdgeSE3ProjectXYZ
-    : public g2o::BaseBinaryEdge<2, Eigen::Vector2d, g2o::VertexSBAPointXYZ,
-                                 g2o::VertexSE3Expmap> {
- public:
-  
-
-  EdgeSE3ProjectXYZ();
-
-  bool read(std::istream& is);
-
-  bool write(std::ostream& os) const;
-
-  void computeError() {
-    const g2o::VertexSE3Expmap* v1 =
-        static_cast<const g2o::VertexSE3Expmap*>(_vertices[1]);
-    const g2o::VertexSBAPointXYZ* v2 =
-        static_cast<const g2o::VertexSBAPointXYZ*>(_vertices[0]);
-    Eigen::Vector2d obs(_measurement);
-    _error = obs - pCamera->project(v1->estimate().map(v2->estimate()));
-  }
-
-  bool isDepthPositive() {
-    const g2o::VertexSE3Expmap* v1 =
-        static_cast<const g2o::VertexSE3Expmap*>(_vertices[1]);
-    const g2o::VertexSBAPointXYZ* v2 =
-        static_cast<const g2o::VertexSBAPointXYZ*>(_vertices[0]);
-    return ((v1->estimate().map(v2->estimate()))(2) > 0.0);
-  }
-
-  virtual void linearizeOplus();
-
-  std::shared_ptr<GeometricCamera> pCamera;
-};
-
-class EdgeSE3ProjectXYZToBody
-    : public g2o::BaseBinaryEdge<2, Eigen::Vector2d, g2o::VertexSBAPointXYZ,
-                                 g2o::VertexSE3Expmap> {
- public:
-  
-
-  EdgeSE3ProjectXYZToBody();
-
-  bool read(std::istream& is);
-
-  bool write(std::ostream& os) const;
-
-  void computeError() {
-    const g2o::VertexSE3Expmap* v1 =
-        static_cast<const g2o::VertexSE3Expmap*>(_vertices[1]);
-    const g2o::VertexSBAPointXYZ* v2 =
-        static_cast<const g2o::VertexSBAPointXYZ*>(_vertices[0]);
-    Eigen::Vector2d obs(_measurement);
-    _error =
-        obs - pCamera->project((mTrl * v1->estimate()).map(v2->estimate()));
-  }
-
-  bool isDepthPositive() {
-    const g2o::VertexSE3Expmap* v1 =
-        static_cast<const g2o::VertexSE3Expmap*>(_vertices[1]);
-    const g2o::VertexSBAPointXYZ* v2 =
-        static_cast<const g2o::VertexSBAPointXYZ*>(_vertices[0]);
-    return ((mTrl * v1->estimate()).map(v2->estimate()))(2) > 0.0;
-  }
-
-  virtual void linearizeOplus();
-
-  std::shared_ptr<GeometricCamera> pCamera;
-  g2o::SE3Quat mTrl;
-};
-
-class VertexSim3Expmap : public g2o::BaseVertex<7, g2o::Sim3> {
- public:
-  
-  VertexSim3Expmap();
-  virtual bool read(std::istream& is);
-  virtual bool write(std::ostream& os) const;
-
-  virtual void setToOriginImpl() { _estimate = g2o::Sim3(); }
-
-  virtual void oplusImpl(const double* update_) {
-    Eigen::Map<g2o::Vector7d> update(const_cast<double*>(update_));
-
-    if (_fix_scale) update[6] = 0;
-
-    g2o::Sim3 s(update);
-    setEstimate(s * estimate());
-  }
-
-  std::shared_ptr<GeometricCamera> pCamera1, pCamera2;
-
-  bool _fix_scale;
-};
-
-class EdgeSim3ProjectXYZ
-    : public g2o::BaseBinaryEdge<2, Eigen::Vector2d, g2o::VertexSBAPointXYZ,
-                                 MORB_SLAM::VertexSim3Expmap> {
- public:
-  
-  EdgeSim3ProjectXYZ();
-  virtual bool read(std::istream& is);
-  virtual bool write(std::ostream& os) const;
-
-  void computeError() {
-    const MORB_SLAM::VertexSim3Expmap* v1 =
-        static_cast<const MORB_SLAM::VertexSim3Expmap*>(_vertices[1]);
-    const g2o::VertexSBAPointXYZ* v2 =
-        static_cast<const g2o::VertexSBAPointXYZ*>(_vertices[0]);
-
-    Eigen::Vector2d obs(_measurement);
-    _error = obs - v1->pCamera1->project(v1->estimate().map(v2->estimate()));
-  }
-
-  // virtual void linearizeOplus();
-};
-
-class EdgeInverseSim3ProjectXYZ
-    : public g2o::BaseBinaryEdge<2, Eigen::Vector2d, g2o::VertexSBAPointXYZ,
-                                 VertexSim3Expmap> {
- public:
-  
-  EdgeInverseSim3ProjectXYZ();
-  virtual bool read(std::istream& is);
-  virtual bool write(std::ostream& os) const;
-
-  void computeError() {
-    const MORB_SLAM::VertexSim3Expmap* v1 =
-        static_cast<const MORB_SLAM::VertexSim3Expmap*>(_vertices[1]);
-    const g2o::VertexSBAPointXYZ* v2 =
-        static_cast<const g2o::VertexSBAPointXYZ*>(_vertices[0]);
-
-    Eigen::Vector2d obs(_measurement);
-    _error = obs - v1->pCamera2->project(
-                       (v1->estimate().inverse().map(v2->estimate())));
-  }
-
-  // virtual void linearizeOplus();
-};
-
-}  // namespace MORB_SLAM
diff --git a/include/Optimizer.h b/include/Optimizer.h
deleted file mode 100644
index cfb5ea43..00000000
--- a/include/Optimizer.h
+++ /dev/null
@@ -1,141 +0,0 @@
-/**
- * This file is part of ORB-SLAM3
- *
- * Copyright (C) 2017-2021 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/>.
- */
-
-#pragma once
-
-#include <math.h>
-
-#include "Frame.h"
-#include "KeyFrame.h"
-#include "LoopClosing.h"
-#include "Map.h"
-#include "MapPoint.h"
-#include "g2o/core/block_solver.h"
-#include "g2o/core/optimization_algorithm_gauss_newton.h"
-#include "g2o/core/optimization_algorithm_levenberg.h"
-#include "g2o/core/robust_kernel_impl.h"
-#include "g2o/core/sparse_block_matrix.h"
-#include "g2o/solvers/linear_solver_dense.h"
-#include "g2o/solvers/linear_solver_eigen.h"
-#include "g2o/types/types_seven_dof_expmap.h"
-#include "g2o/types/types_six_dof_expmap.h"
-
-namespace MORB_SLAM {
-
-class LoopClosing;
-
-class Optimizer {
- public:
-  void static BundleAdjustment(const std::vector<KeyFrame *> &vpKF,
-                               const std::vector<MapPoint *> &vpMP,
-                               int nIterations = 5, bool *pbStopFlag = nullptr,
-                               const unsigned long nLoopKF = 0,
-                               const bool bRobust = true);
-  void static GlobalBundleAdjustemnt(std::shared_ptr<Map> pMap, int nIterations = 5,
-                                     bool *pbStopFlag = nullptr,
-                                     const unsigned long nLoopKF = 0,
-                                     const bool bRobust = true);
-  void static FullInertialBA(std::shared_ptr<Map> pMap, int its, const bool bFixLocal = false,
-                             const unsigned long nLoopKF = 0,
-                             bool *pbStopFlag = nullptr, bool bInit = false,
-                             float priorG = 1e2, float priorA = 1e6,
-                             Eigen::VectorXd *vSingVal = nullptr,
-                             bool *bHess = nullptr);
-
-  void static LocalBundleAdjustment(KeyFrame *pKF, bool *pbStopFlag, std::shared_ptr<Map> pMap,
-                                    int &num_fixedKF, int &num_OptKF,
-                                    int &num_MPs, int &num_edges);
-
-  int static PoseOptimization(Frame *pFrame);
-  int static PoseInertialOptimizationLastKeyFrame(Frame *pFrame,
-                                                  bool bRecInit = false);
-  int static PoseInertialOptimizationLastFrame(Frame *pFrame,
-                                               bool bRecInit = false);
-
-  // if bFixScale is true, 6DoF optimization (stereo,rgbd), 7DoF otherwise
-  // (mono)
-  void static OptimizeEssentialGraph(
-      std::shared_ptr<Map> pMap, KeyFrame *pLoopKF, KeyFrame *pCurKF,
-      const LoopClosing::KeyFrameAndPose &NonCorrectedSim3,
-      const LoopClosing::KeyFrameAndPose &CorrectedSim3,
-      const map<KeyFrame *, set<KeyFrame *> > &LoopConnections,
-      const bool &bFixScale);
-  void static OptimizeEssentialGraph(KeyFrame *pCurKF,
-                                     vector<KeyFrame *> &vpFixedKFs,
-                                     vector<KeyFrame *> &vpFixedCorrectedKFs,
-                                     vector<KeyFrame *> &vpNonFixedKFs,
-                                     vector<MapPoint *> &vpNonCorrectedMPs);
-
-  // For inertial loopclosing
-  void static OptimizeEssentialGraph4DoF(
-      std::shared_ptr<Map> pMap, KeyFrame *pLoopKF, KeyFrame *pCurKF,
-      const LoopClosing::KeyFrameAndPose &NonCorrectedSim3,
-      const LoopClosing::KeyFrameAndPose &CorrectedSim3,
-      const map<KeyFrame *, set<KeyFrame *> > &LoopConnections);
-
-  // if bFixScale is true, optimize SE3 (stereo,rgbd), Sim3 otherwise (mono)
-  // (NEW)
-  static int OptimizeSim3(KeyFrame *pKF1, KeyFrame *pKF2,
-                          std::vector<MapPoint *> &vpMatches1,
-                          g2o::Sim3 &g2oS12, const float th2,
-                          const bool bFixScale,
-                          Eigen::Matrix<double, 7, 7> &mAcumHessian,
-                          const bool bAllPoints = false);
-
-  // For inertial systems
-
-  void static LocalInertialBA(KeyFrame *pKF, bool *pbStopFlag, std::shared_ptr<Map> pMap,
-                              int &num_fixedKF, int &num_OptKF, int &num_MPs,
-                              int &num_edges, bool bLarge = false,
-                              bool bRecInit = false);
-  void static MergeInertialBA(KeyFrame *pCurrKF, KeyFrame *pMergeKF,
-                              bool *pbStopFlag, std::shared_ptr<Map> pMap,
-                              LoopClosing::KeyFrameAndPose &corrPoses);
-
-  // Local BA in welding area when two maps are merged
-  void static LocalBundleAdjustment(KeyFrame *pMainKF,
-                                    vector<KeyFrame *> vpAdjustKF,
-                                    vector<KeyFrame *> vpFixedKF,
-                                    bool *pbStopFlag);
-
-  // Marginalize block element (start:end,start:end). Perform Schur complement.
-  // Marginalized elements are filled with zeros.
-  static Eigen::MatrixXd Marginalize(const Eigen::MatrixXd &H, const int &start,
-                                     const int &end);
-
-  // Inertial pose-graph
-  void static InertialOptimization(std::shared_ptr<Map> pMap, Eigen::Matrix3d &Rwg,
-                                   double &scale, Eigen::Vector3d &bg,
-                                   Eigen::Vector3d &ba, bool bMono,
-                                   Eigen::MatrixXd &covInertial,
-                                   bool bFixedVel = false, bool bGauss = false,
-                                   float priorG = 1e2, float priorA = 1e6);
-  void static InertialOptimization(std::shared_ptr<Map> pMap, Eigen::Vector3d &bg,
-                                   Eigen::Vector3d &ba, float priorG = 1e2,
-                                   float priorA = 1e6);
-  void static InertialOptimization(std::shared_ptr<Map> pMap, Eigen::Matrix3d &Rwg,
-                                   double &scale);
-
-  ;
-};
-
-}  // namespace MORB_SLAM
-
diff --git a/include/SerializationUtils.h b/include/SerializationUtils.h
deleted file mode 100644
index 0f12eca3..00000000
--- a/include/SerializationUtils.h
+++ /dev/null
@@ -1,154 +0,0 @@
-/**
- * This file is part of ORB-SLAM3
- *
- * Copyright (C) 2017-2021 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/>.
- */
-
-#pragma once
-
-#include <Eigen/Core>
-#include <boost/serialization/serialization.hpp>
-#include <boost/serialization/vector.hpp>
-#include <opencv2/core/core.hpp>
-#include <opencv2/features2d/features2d.hpp>
-#include <sophus/se3.hpp>
-#include <vector>
-
-namespace MORB_SLAM {
-
-template <class Archive>
-void serializeSophusSE3(Archive& ar, Sophus::SE3f& T,
-                        const unsigned int version) {
-  Eigen::Vector4f quat;
-  Eigen::Vector3f transl;
-
-  if (Archive::is_saving::value) {
-    Eigen::Quaternionf q = T.unit_quaternion();
-    quat << q.w(), q.x(), q.y(), q.z();
-    transl = T.translation();
-  }
-
-  ar& boost::serialization::make_array(quat.data(), quat.size());
-  ar& boost::serialization::make_array(transl.data(), transl.size());
-
-  if (Archive::is_loading::value) {
-    Eigen::Quaternionf q(quat[0], quat[1], quat[2], quat[3]);
-    T = Sophus::SE3f(q, transl);
-  }
-}
-
-/*template <class Archive, size_t dim>
-void serializeDiagonalMatrix(Archive &ar, Eigen::DiagonalMatrix<float, dim> &D,
-const unsigned int version)
-{
-    Eigen::Matrix<float,dim,dim> dense;
-    if(Archive::is_saving::value)
-    {
-        dense = D.toDenseMatrix();
-    }
-
-    ar & boost::serialization::make_array(dense.data(), dense.size());
-
-    if (Archive::is_loading::value)
-    {
-        D = dense.diagonal().asDiagonal();
-    }
-}*/
-
-template <class Archive>
-void serializeMatrix(Archive& ar, cv::Mat& mat, const unsigned int version) {
-  int cols, rows, type;
-  bool continuous;
-
-  if (Archive::is_saving::value) {
-    cols = mat.cols;
-    rows = mat.rows;
-    type = mat.type();
-    continuous = mat.isContinuous();
-  }
-
-  ar& cols& rows& type& continuous;
-
-  if (Archive::is_loading::value) mat.create(rows, cols, type);
-
-  if (continuous) {
-    const unsigned int data_size = rows * cols * mat.elemSize();
-    ar& boost::serialization::make_array(mat.ptr(), data_size);
-  } else {
-    const unsigned int row_size = cols * mat.elemSize();
-    for (int i = 0; i < rows; i++) {
-      ar& boost::serialization::make_array(mat.ptr(i), row_size);
-    }
-  }
-}
-
-template <class Archive>
-void serializeMatrix(Archive& ar, const cv::Mat& mat,
-                     const unsigned int version) {
-  cv::Mat matAux = mat;
-
-  serializeMatrix(ar, matAux, version);
-
-  if (Archive::is_loading::value) {
-    cv::Mat* ptr;
-    ptr = (cv::Mat*)(&mat);
-    *ptr = matAux;
-  }
-}
-
-template <class Archive>
-void serializeVectorKeyPoints(Archive& ar, const std::vector<cv::KeyPoint>& vKP,
-                              const unsigned int version) {
-  int NumEl;
-
-  if (Archive::is_saving::value) {
-    NumEl = vKP.size();
-  }
-
-  ar& NumEl;
-
-  std::vector<cv::KeyPoint> vKPaux = vKP;
-  if (Archive::is_loading::value) vKPaux.reserve(NumEl);
-
-  for (int i = 0; i < NumEl; ++i) {
-    cv::KeyPoint KPi;
-
-    if (Archive::is_loading::value) KPi = cv::KeyPoint();
-
-    if (Archive::is_saving::value) KPi = vKPaux[i];
-
-    ar& KPi.angle;
-    ar& KPi.response;
-    ar& KPi.size;
-    ar& KPi.pt.x;
-    ar& KPi.pt.y;
-    ar& KPi.class_id;
-    ar& KPi.octave;
-
-    if (Archive::is_loading::value) vKPaux.push_back(KPi);
-  }
-
-  if (Archive::is_loading::value) {
-    std::vector<cv::KeyPoint>* ptr;
-    ptr = (std::vector<cv::KeyPoint>*)(&vKP);
-    *ptr = vKPaux;
-  }
-}
-
-}  // namespace MORB_SLAM
-
diff --git a/include/Settings.h b/include/Settings.h
deleted file mode 100644
index 81e92531..00000000
--- a/include/Settings.h
+++ /dev/null
@@ -1,239 +0,0 @@
-/**
- * This file is part of ORB-SLAM3
- *
- * Copyright (C) 2017-2021 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/>.
- */
-
-#pragma once
-
-// Flag to activate the measurement of time in each process (track,localmap,
-// place recognition).
-//#define REGISTER_TIMES
-
-#include <stdio.h>
-#include <stdlib.h>
-#include <unistd.h>
-
-#include <string>
-#include <memory>
-
-#include "CameraModels/GeometricCamera.h"
-
-namespace MORB_SLAM {
-
-class System;
-
-// TODO: change to double instead of float
-
-class Settings {
- public:
-  /*
-   * Enum for the different camera types implemented
-   */
-  enum CameraType { PinHole = 0, Rectified = 1, KannalaBrandt = 2 };
-
-  /*
-   * Delete default constructor
-   */
-  Settings() = delete;
-
-  /*
-   * Constructor from file
-   */
-  Settings(const std::string& configFile, const int& sensor);
-
-  /*
-   * Ostream operator overloading to dump settings to the terminal
-   */
-  friend std::ostream& operator<<(std::ostream& output, const Settings& s);
-
-  /*
-   * Getter methods
-   */
-  CameraType cameraType() const { return cameraType_; }
-  std::shared_ptr<GeometricCamera> camera1() { return calibration1_; }
-  std::shared_ptr<GeometricCamera> camera2() { return calibration2_; }
-  cv::Mat camera1DistortionCoef() {
-    return cv::Mat(vPinHoleDistorsion1_.size(), 1, CV_32F,
-                   vPinHoleDistorsion1_.data());
-  }
-  cv::Mat camera2DistortionCoef() {
-    return cv::Mat(vPinHoleDistorsion2_.size(), 1, CV_32F,
-                   vPinHoleDistorsion2_.data());
-  }
-
-  const Sophus::SE3f &Tlr() const { return Tlr_; }
-  float bf() const { return bf_; }
-  float b() const { return b_; }
-  float thDepth() const { return thDepth_; }
-
-  bool needToUndistort() const { return bNeedToUndistort_; }
-
-  const cv::Size &newImSize() const { return newImSize_; }
-  float fps() const { return fps_; }
-  bool rgb() const { return bRGB_; }
-  bool needToResize() const { return bNeedToResize1_; }
-  bool needToRectify() const { return bNeedToRectify_; }
-
-  float noiseGyro() const { return noiseGyro_; }
-  float noiseAcc() const { return noiseAcc_; }
-  float gyroWalk() const { return gyroWalk_; }
-  float accWalk() const { return accWalk_; }
-  float imuFrequency() const { return imuFrequency_; }
-  const Sophus::SE3f &Tbc() const { return Tbc_; }
-  bool insertKFsWhenLost() const { return insertKFsWhenLost_; }
-
-  float depthMapFactor() const { return depthMapFactor_; }
-
-  int nFeatures() const { return nFeatures_; }
-  int nLevels() const { return nLevels_; }
-  float initThFAST() const { return initThFAST_; }
-  float minThFAST() const { return minThFAST_; }
-  float scaleFactor() const { return scaleFactor_; }
-
-  float keyFrameSize() const { return keyFrameSize_; }
-  float keyFrameLineWidth() const { return keyFrameLineWidth_; }
-  float graphLineWidth() const { return graphLineWidth_; }
-  float pointSize() const { return pointSize_; }
-  float cameraSize() const { return cameraSize_; }
-  float cameraLineWidth() const { return cameraLineWidth_; }
-  float viewPointX() const { return viewPointX_; }
-  float viewPointY() const { return viewPointY_; }
-  float viewPointZ() const { return viewPointZ_; }
-  float viewPointF() const { return viewPointF_; }
-  float imageViewerScale() const { return imageViewerScale_; }
-
-  const std::string &atlasLoadFile() const { return sLoadFrom_; }
-  const std::string &atlasSaveFile() const { return sSaveto_; }
-
-  float thFarPoints() const { return thFarPoints_; }
-
-  const cv::Mat &M1l() const { return M1l_; }
-  const cv::Mat &M2l() const { return M2l_; }
-  const cv::Mat &M1r() const { return M1r_; }
-  const cv::Mat &M2r() const { return M2r_; }
-
- private:
-  template <typename T>
-  T readParameter(cv::FileStorage& fSettings, const std::string& name,
-                  bool& found, const bool required = true) {
-    cv::FileNode node = fSettings[name];
-    if (node.empty()) {
-      if (required) {
-        std::cerr << name << " required parameter does not exist, aborting..."
-                  << std::endl;
-        exit(-1);
-      } else {
-        std::cerr << name << " optional parameter does not exist..."
-                  << std::endl;
-        found = false;
-        return T();
-      }
-
-    } else {
-      found = true;
-      return (T)node;
-    }
-  }
-
-  void readCamera1(cv::FileStorage& fSettings);
-  void readCamera2(cv::FileStorage& fSettings);
-  void readImageInfo(cv::FileStorage& fSettings);
-  void readIMU(cv::FileStorage& fSettings);
-  void readRGBD(cv::FileStorage& fSettings);
-  void readORB(cv::FileStorage& fSettings);
-  void readViewer(cv::FileStorage& fSettings);
-  void readLoadAndSave(cv::FileStorage& fSettings);
-  void readOtherParameters(cv::FileStorage& fSettings);
-
-  void precomputeRectificationMaps();
-
-  int sensor_;
-  CameraType cameraType_;  // Camera type
-
-  /*
-   * Visual stuff
-   */
-  std::shared_ptr<GeometricCamera> calibration1_, calibration2_;  // Camera calibration
-  GeometricCamera *originalCalib1_, *originalCalib2_;
-  std::vector<float> vPinHoleDistorsion1_, vPinHoleDistorsion2_;
-
-  cv::Size originalImSize_, newImSize_;
-  float fps_;
-  bool bRGB_;
-
-  bool bNeedToUndistort_;
-  bool bNeedToRectify_;
-  bool bNeedToResize1_, bNeedToResize2_;
-
-  Sophus::SE3f Tlr_;
-  float thDepth_;
-  float bf_, b_;
-
-  /*
-   * Rectification stuff
-   */
-  cv::Mat M1l_, M2l_;
-  cv::Mat M1r_, M2r_;
-
-  /*
-   * Inertial stuff
-   */
-  float noiseGyro_, noiseAcc_;
-  float gyroWalk_, accWalk_;
-  float imuFrequency_;
-  Sophus::SE3f Tbc_;
-  bool insertKFsWhenLost_;
-
-  /*
-   * RGBD stuff
-   */
-  float depthMapFactor_;
-
-  /*
-   * ORB stuff
-   */
-  int nFeatures_;
-  float scaleFactor_;
-  int nLevels_;
-  int initThFAST_, minThFAST_;
-
-  /*
-   * Viewer stuff
-   */
-  float keyFrameSize_;
-  float keyFrameLineWidth_;
-  float graphLineWidth_;
-  float pointSize_;
-  float cameraSize_;
-  float cameraLineWidth_;
-  float viewPointX_, viewPointY_, viewPointZ_, viewPointF_;
-  float imageViewerScale_;
-
-  /*
-   * Save & load maps
-   */
-  std::string sLoadFrom_, sSaveto_;
-
-  /*
-   * Other stuff
-   */
-  float thFarPoints_;
-};
-}  // namespace MORB_SLAM
-
diff --git a/include/Sim3Solver.h b/include/Sim3Solver.h
deleted file mode 100644
index 47dff9d4..00000000
--- a/include/Sim3Solver.h
+++ /dev/null
@@ -1,137 +0,0 @@
-/**
- * This file is part of ORB-SLAM3
- *
- * Copyright (C) 2017-2021 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/>.
- */
-
-#pragma once
-
-#include <opencv2/opencv.hpp>
-#include <vector>
-
-#include "KeyFrame.h"
-
-namespace MORB_SLAM {
-
-class Sim3Solver {
- public:
-  
-  Sim3Solver(
-      KeyFrame *pKF1, KeyFrame *pKF2,
-      const std::vector<MapPoint *> &vpMatched12, const bool bFixScale = true,
-      const vector<KeyFrame *> vpKeyFrameMatchedMP = vector<KeyFrame *>());
-
-  void SetRansacParameters(double probability = 0.99, int minInliers = 6,
-                           int maxIterations = 300);
-
-  Eigen::Matrix4f find(std::vector<bool> &vbInliers12, int &nInliers);
-
-  Eigen::Matrix4f iterate(int nIterations, bool &bNoMore,
-                          std::vector<bool> &vbInliers, int &nInliers);
-  Eigen::Matrix4f iterate(int nIterations, bool &bNoMore,
-                          vector<bool> &vbInliers, int &nInliers,
-                          bool &bConverge);
-
-  Eigen::Matrix4f GetEstimatedTransformation();
-  Eigen::Matrix3f GetEstimatedRotation();
-  Eigen::Vector3f GetEstimatedTranslation();
-  float GetEstimatedScale();
-
- protected:
-  void ComputeCentroid(Eigen::Matrix3f &P, Eigen::Matrix3f &Pr,
-                       Eigen::Vector3f &C);
-
-  void ComputeSim3(Eigen::Matrix3f &P1, Eigen::Matrix3f &P2);
-
-  void CheckInliers();
-
-  void Project(const std::vector<Eigen::Vector3f> &vP3Dw,
-               std::vector<Eigen::Vector2f> &vP2D, Eigen::Matrix4f Tcw,
-               const std::shared_ptr<GeometricCamera> &pCamera);
-  void FromCameraToImage(const std::vector<Eigen::Vector3f> &vP3Dc,
-                         std::vector<Eigen::Vector2f> &vP2D,
-                         const std::shared_ptr<GeometricCamera> &pCamera);
-
- protected:
-  // KeyFrames and matches
-  KeyFrame *mpKF1;
-  KeyFrame *mpKF2;
-
-  std::vector<Eigen::Vector3f> mvX3Dc1;
-  std::vector<Eigen::Vector3f> mvX3Dc2;
-  std::vector<MapPoint *> mvpMapPoints1;
-  std::vector<MapPoint *> mvpMapPoints2;
-  std::vector<MapPoint *> mvpMatches12;
-  std::vector<size_t> mvnIndices1;
-  std::vector<size_t> mvSigmaSquare1;
-  std::vector<size_t> mvSigmaSquare2;
-  std::vector<size_t> mvnMaxError1;
-  std::vector<size_t> mvnMaxError2;
-
-  int N;
-  int mN1;
-
-  // Current Estimation
-  Eigen::Matrix3f mR12i;
-  Eigen::Vector3f mt12i;
-  float ms12i;
-  Eigen::Matrix4f mT12i;
-  Eigen::Matrix4f mT21i;
-  std::vector<bool> mvbInliersi;
-  int mnInliersi;
-
-  // Current Ransac State
-  int mnIterations;
-  std::vector<bool> mvbBestInliers;
-  int mnBestInliers;
-  Eigen::Matrix4f mBestT12;
-  Eigen::Matrix3f mBestRotation;
-  Eigen::Vector3f mBestTranslation;
-  float mBestScale;
-
-  // Scale is fixed to 1 in the stereo/RGBD case
-  bool mbFixScale;
-
-  // Indices for random selection
-  std::vector<size_t> mvAllIndices;
-
-  // Projections
-  std::vector<Eigen::Vector2f> mvP1im1;
-  std::vector<Eigen::Vector2f> mvP2im2;
-
-  // RANSAC probability
-  double mRansacProb;
-
-  // RANSAC min inliers
-  int mRansacMinInliers;
-
-  // RANSAC max iterations
-  int mRansacMaxIts;
-
-  // Threshold inlier/outlier. e = dist(Pi,T_ij*Pj)^2 < 5.991*mSigma2
-  float mTh;
-  float mSigma2;
-
-  // Calibration
-  // cv::Mat mK1;
-  // cv::Mat mK2;
-
-  std::shared_ptr<GeometricCamera> pCamera1, pCamera2;
-};
-
-}  // namespace MORB_SLAM
diff --git a/include/System.h b/include/System.h
deleted file mode 100644
index 83555ce3..00000000
--- a/include/System.h
+++ /dev/null
@@ -1,245 +0,0 @@
-/**
-* This file is part of ORB-SLAM3
-*
-* Copyright (C) 2017-2021 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/>.
-*/
-
-
-#pragma once
-
-
-#include <unistd.h>
-#include <stdio.h>
-#include <stdlib.h>
-#include <string>
-#include <thread>
-#include <opencv2/core/core.hpp>
-
-#include "ImprovedTypes.hpp"
-#include "Tracking.h"
-#include "Atlas.h"
-#include "LocalMapping.h"
-#include "LoopClosing.h"
-#include "KeyFrameDatabase.h"
-#include "ORBVocabulary.h"
-#include "ImuTypes.h"
-#include "Settings.h"
-#include "Camera.hpp"
-
-
-namespace MORB_SLAM
-{
-
-class Verbose
-{
-public:
-    enum eLevel
-    {
-        VERBOSITY_QUIET=0,
-        VERBOSITY_NORMAL=1,
-        VERBOSITY_VERBOSE=2,
-        VERBOSITY_VERY_VERBOSE=3,
-        VERBOSITY_DEBUG=4
-    };
-
-    static eLevel th;
-
-public:
-    static void PrintMess(std::string str, eLevel lev)
-    {
-        if(lev <= th)
-            cout << str << endl;
-    }
-
-    static void SetTh(eLevel _th)
-    {
-        th = _th;
-    }
-};
-
-class Viewer;
-class Atlas;
-class Tracking;
-class LocalMapping;
-class LoopClosing;
-class Settings;
-
-class System
-{
-public:
-
-    // File type
-    enum FileType{
-        TEXT_FILE=0,
-        BINARY_FILE=1,
-    };
-
-public:
-    
-    // Initialize the SLAM system. It launches the Local Mapping, Loop Closing and Viewer threads.
-    System(const string &strVocFile, const string &strSettingsFile, const CameraType::eSensor sensor, const string &strSequence = std::string());
-
-    // Proccess the given stereo frame. Images must be synchronized and rectified.
-    // Input images: RGB (CV_8UC3) or grayscale (CV_8U). RGB is converted to grayscale.
-    // Returns the camera pose (empty if tracking fails).
-    Sophus::SE3f TrackStereo(const cv::Mat &imLeft, const cv::Mat &imRight, const double &timestamp, const vector<IMU::Point>& vImuMeas = vector<IMU::Point>(), string filename="");
-
-    // Process the given rgbd frame. Depthmap must be registered to the RGB frame.
-    // Input image: RGB (CV_8UC3) or grayscale (CV_8U). RGB is converted to grayscale.
-    // Input depthmap: Float (CV_32F).
-    // Returns the camera pose (empty if tracking fails).
-    Sophus::SE3f TrackRGBD(const cv::Mat &im, const cv::Mat &depthmap, const double &timestamp, const vector<IMU::Point>& vImuMeas = vector<IMU::Point>(), string filename="");
-
-    // Proccess the given monocular frame and optionally imu data
-    // Input images: RGB (CV_8UC3) or grayscale (CV_8U). RGB is converted to grayscale.
-    // Returns the camera pose (empty if tracking fails).
-    Sophus::SE3f TrackMonocular(const cv::Mat &im, const double &timestamp, const vector<IMU::Point>& vImuMeas = vector<IMU::Point>(), string filename="");
-
-
-    // This stops local mapping thread (map building) and performs only camera tracking.
-    void ActivateLocalizationMode();
-    // This resumes local mapping thread and performs SLAM again.
-    void DeactivateLocalizationMode();
-
-    // Returns true if there have been a big map change (loop closure, global BA)
-    // since last call to this function
-    bool MapChanged();
-
-    // Reset the system (clear Atlas or the active map)
-    void Reset();
-    void ResetActiveMap();
-
-    // All threads will be requested to finish.
-    // It waits until all threads have finished.
-    // This function must be called before saving the trajectory.
-    virtual ~System();
-
-    // Save camera trajectory in the TUM RGB-D dataset format.
-    // Only for stereo and RGB-D. This method does not work for monocular.
-    // Call first Shutdown()
-    // See format details at: http://vision.in.tum.de/data/datasets/rgbd-dataset
-    void SaveTrajectoryTUM(const string &filename);
-
-    // Save keyframe poses in the TUM RGB-D dataset format.
-    // This method works for all sensor input.
-    // Call first Shutdown()
-    // See format details at: http://vision.in.tum.de/data/datasets/rgbd-dataset
-    void SaveKeyFrameTrajectoryTUM(const string &filename);
-
-    void SaveTrajectoryEuRoC(const string &filename);
-    void SaveKeyFrameTrajectoryEuRoC(const string &filename);
-
-    void SaveTrajectoryEuRoC(const string &filename, std::shared_ptr<Map> pMap);
-    void SaveKeyFrameTrajectoryEuRoC(const string &filename, std::shared_ptr<Map> pMap);
-
-    // Save data used for initialization debug
-    void SaveDebugData(const int &iniIdx);
-
-    // Save camera trajectory in the KITTI dataset format.
-    // Only for stereo and RGB-D. This method does not work for monocular.
-    // Call first Shutdown()
-    // See format details at: http://www.cvlibs.net/datasets/kitti/eval_odometry.php
-    void SaveTrajectoryKITTI(const string &filename);
-
-    // TODO: Save/Load functions
-    // SaveMap(const string &filename);
-    // LoadMap(const string &filename);
-
-    // Information from most recent processed frame
-    // You can call this right after TrackMonocular (or stereo or RGBD)
-    int GetTrackingState();
-    std::vector<MapPoint*> GetTrackedMapPoints();
-    std::vector<cv::KeyPoint> GetTrackedKeyPointsUn();
-
-    // For debugging
-    double GetTimeFromIMUInit();
-    bool isLost();
-    bool isFinished();
-
-    void ChangeDataset();
-
-    float GetImageScale();
-
-#ifdef REGISTER_TIMES
-    void InsertRectTime(double& time);
-    void InsertResizeTime(double& time);
-    void InsertTrackTime(double& time);
-#endif
-
-    friend Viewer;
-private:
-
-    void SaveAtlas(int type);
-    bool LoadAtlas(int type);
-
-    string CalculateCheckSum(string filename, int type);
-
-    // Input sensor
-    CameraType::eSensor mSensor;
-    vector<Camera_ptr> cameras;
-
-    // ORB vocabulary used for place recognition and feature matching.
-    ORBVocabulary* mpVocabulary;
-
-    // KeyFrame database for place recognition (relocalization and loop detection).
-    KeyFrameDatabase* mpKeyFrameDatabase;
-
-    // Map structure that stores the pointers to all KeyFrames and MapPoints.
-    //Map* mpMap;
-    Atlas_ptr mpAtlas;
-
-    // Tracker. It receives a frame and computes the associated camera pose.
-    // It also decides when to insert a new keyframe, create some new MapPoints and
-    // performs relocalization if tracking fails.
-    Tracking* mpTracker;
-
-    // Local Mapper. It manages the local map and performs local bundle adjustment.
-    LocalMapping* mpLocalMapper;
-
-    // Loop Closer. It searches loops with every new keyframe. If there is a loop it performs
-    // a pose graph optimization and full bundle adjustment (in a new thread) afterwards.
-    LoopClosing* mpLoopCloser;
-
-    // System threads: Local Mapping, Loop Closing, Viewer.
-    // The Tracking thread "lives" in the main execution thread that creates the System object.
-    std::thread* mptLocalMapping;
-    std::thread* mptLoopClosing;
-
-    // Reset flag
-    std::mutex mMutexReset;
-    bool mbReset;
-    bool mbResetActiveMap;
-
-    // Change mode flags
-    std::mutex mMutexMode;
-    bool mbActivateLocalizationMode;
-    bool mbDeactivateLocalizationMode;
-
-    // Tracking state
-    int mTrackingState;
-    std::vector<MapPoint*> mTrackedMapPoints;
-    std::vector<cv::KeyPoint> mTrackedKeyPointsUn;
-    std::mutex mMutexState;
-
-    //
-    string mStrLoadAtlasFromFile;
-    string mStrSaveAtlasToFile;
-
-    string mStrVocabularyFilePath;
-
-    Settings* settings_;
-};
-
-}// namespace ORB_SLAM
diff --git a/include/Tracking.h b/include/Tracking.h
deleted file mode 100644
index 2297c644..00000000
--- a/include/Tracking.h
+++ /dev/null
@@ -1,364 +0,0 @@
-/**
- * This file is part of ORB-SLAM3
- *
- * Copyright (C) 2017-2021 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/>.
- */
-
-#pragma once
-
-#include <mutex>
-#include <opencv2/core/core.hpp>
-#include <opencv2/features2d/features2d.hpp>
-#include <unordered_set>
-
-#include "Atlas.h"
-#include "Frame.h"
-#include "GeometricCamera.h"
-#include "ImuTypes.h"
-#include "KeyFrameDatabase.h"
-#include "LocalMapping.h"
-#include "LoopClosing.h"
-#include "ORBVocabulary.h"
-#include "ORBextractor.h"
-#include "Settings.h"
-#include "System.h"
-#include "ImprovedTypes.hpp"
-#include "Camera.hpp"
-
-namespace MORB_SLAM {
-
-class Atlas;
-class LocalMapping;
-class LoopClosing;
-class System;
-class Settings;
-
-class Tracking {
- public:
-  
-  Tracking(System* pSys, ORBVocabulary* pVoc,
-           const Atlas_ptr &pAtlas, KeyFrameDatabase* pKFDB,
-           const string& strSettingPath, const CameraType::eSensor sensor, Settings* settings,
-           const string& _nameSeq = std::string());
-
-  ~Tracking();
-
-  // Parse the config file
-  bool ParseCamParamFile(cv::FileStorage& fSettings);
-  bool ParseORBParamFile(cv::FileStorage& fSettings);
-  bool ParseIMUParamFile(cv::FileStorage& fSettings);
-
-  // Preprocess the input and call Track(). Extract features and performs stereo
-  // matching.
-  Sophus::SE3f GrabImageStereo(const cv::Mat& imRectLeft,
-                               const cv::Mat& imRectRight,
-                               const double& timestamp, const string &filename,
-                               const Camera_ptr &cam);
-  Sophus::SE3f GrabImageRGBD(const cv::Mat& imRGB, const cv::Mat& imD,
-                             const double& timestamp, const string &filename,
-                             const Camera_ptr &cam);
-  Sophus::SE3f GrabImageMonocular(const cv::Mat& im, const double& timestamp,
-                                  const string &filename, const Camera_ptr &cam);
-
-  void GrabImuData(const IMU::Point& imuMeasurement);
-
-  void SetLocalMapper(LocalMapping* pLocalMapper);
-  void SetLoopClosing(LoopClosing* pLoopClosing);
-
-  // Load new settings
-  // The focal lenght should be similar or scale prediction will fail when
-  // projecting points
-  void ChangeCalibration(const string& strSettingPath);
-
-  // Use this function if you have deactivated local mapping and you only want
-  // to localize the camera.
-  void InformOnlyTracking(const bool& flag);
-
-  void UpdateFrameIMU(const float s, const IMU::Bias& b,
-                      KeyFrame* pCurrentKeyFrame);
-  KeyFrame* GetLastKeyFrame() { return mpLastKeyFrame; }
-
-  void CreateMapInAtlas();
-  // std::mutex mMutexTracks;
-
-  //--
-  void NewDataset();
-  int GetNumberDataset();
-  int GetMatchesInliers();
-
-  // DEBUG
-  void SaveSubTrajectory(string strNameFile_frames, string strNameFile_kf,
-                         string strFolder = "");
-  void SaveSubTrajectory(string strNameFile_frames, string strNameFile_kf,
-                         std::shared_ptr<Map> pMap);
-
-  float GetImageScale();
-
-#ifdef REGISTER_LOOP
-  void RequestStop();
-  bool isStopped();
-  void Release();
-  bool stopRequested();
-#endif
-
- public:
-
-  Tracker::eTrackingState mState;
-  Tracker::eTrackingState mLastProcessedState;
-
-  // Input sensor
-  CameraType::eSensor mSensor;
-
-  // Current Frame
-  Frame mCurrentFrame;
-  Frame mLastFrame;
-
-  cv::Mat mImGray;
-
-  // Initialization Variables (Monocular)
-  std::vector<int> mvIniLastMatches;
-  std::vector<int> mvIniMatches;
-  std::vector<cv::Point2f> mvbPrevMatched;
-  std::vector<cv::Point3f> mvIniP3D;
-  Frame mInitialFrame;
-
-  // Lists used to recover the full camera trajectory at the end of the
-  // execution. Basically we store the reference keyframe for each frame and its
-  // relative transformation
-  list<Sophus::SE3f> mlRelativeFramePoses;
-  list<KeyFrame*> mlpReferences;
-  list<double> mlFrameTimes;
-  list<bool> mlbLost;
-
-  // frames with estimated pose
-  int mTrackedFr;
-  bool mbStep;
-
-  // True if local mapping is deactivated and we are performing only
-  // localization
-  bool mbOnlyTracking;
-
-  void Reset(bool bLocMap = false);
-  void ResetActiveMap(bool bLocMap = false);
-
-  float mMeanTrack;
-  bool mbInitWith3KFs;
-  double t0;     // time-stamp of first read frame
-  double t0vis;  // time-stamp of first inserted keyframe
-  double t0IMU;  // time-stamp of IMU initialization
-  bool mFastInit = false;
-
-  vector<MapPoint*> GetLocalMapMPS();
-
-  bool mbWriteStats;
-
-#ifdef REGISTER_TIMES
-  void LocalMapStats2File();
-  void TrackStats2File();
-  void PrintTimeStats();
-
-  vector<double> vdRectStereo_ms;
-  vector<double> vdResizeImage_ms;
-  vector<double> vdORBExtract_ms;
-  vector<double> vdStereoMatch_ms;
-  vector<double> vdIMUInteg_ms;
-  vector<double> vdPosePred_ms;
-  vector<double> vdLMTrack_ms;
-  vector<double> vdNewKF_ms;
-  vector<double> vdTrackTotal_ms;
-#endif
-
- protected:
-  // Main tracking function. It is independent of the input sensor.
-  void Track();
-
-  // Map initialization for stereo and RGB-D
-  void StereoInitialization();
-
-  // Map initialization for monocular
-  void MonocularInitialization();
-  // void CreateNewMapPoints();
-  void CreateInitialMapMonocular();
-
-  void CheckReplacedInLastFrame();
-  bool TrackReferenceKeyFrame();
-  void UpdateLastFrame();
-  bool TrackWithMotionModel();
-  bool PredictStateIMU();
-
-  bool Relocalization();
-
-  void UpdateLocalMap();
-  void UpdateLocalPoints();
-  void UpdateLocalKeyFrames();
-
-  bool TrackLocalMap();
-  void SearchLocalPoints();
-
-  bool NeedNewKeyFrame();
-  void CreateNewKeyFrame();
-
-  // Perform preintegration from last frame
-  void PreintegrateIMU();
-
-  // Reset IMU biases and compute frame velocity
-  void ResetFrameIMU();
-
-  bool mbMapUpdated;
-
-  // Imu preintegration from last frame
-  IMU::Preintegrated* mpImuPreintegratedFromLastKF;
-
-  // Queue of IMU measurements between frames
-  std::list<IMU::Point> mlQueueImuData;
-
-  // Vector of IMU measurements from previous to current frame (to be filled by
-  // PreintegrateIMU)
-  std::vector<IMU::Point> mvImuFromLastFrame;
-  std::mutex mMutexImuQueue;
-
-  // Imu calibration parameters
-  std::shared_ptr<IMU::Calib> mpImuCalib;
-
-  // Last Bias Estimation (at keyframe creation)
-  IMU::Bias mLastBias;
-
-  // In case of performing only localization, this flag is true when there are
-  // no matches to points in the map. Still tracking will continue if there are
-  // enough matches with temporal points. In that case we are doing visual
-  // odometry. The system will try to do relocalization to recover "zero-drift"
-  // localization to the map.
-  bool mbVO;
-
-  // Other Thread Pointers
-  LocalMapping* mpLocalMapper;
-  LoopClosing* mpLoopClosing;
-
-  // ORB
-  std::shared_ptr<ORBextractor> mpORBextractorLeft;
-  std::shared_ptr<ORBextractor> mpORBextractorRight;
-  std::shared_ptr<ORBextractor> mpIniORBextractor;
-
-  // BoW
-  ORBVocabulary* mpORBVocabulary;
-  KeyFrameDatabase* mpKeyFrameDB;
-
-  // Initalization (only for monocular)
-  bool mbReadyToInitialize;
-  bool mbSetInit;
-
-  // Local Map
-  KeyFrame* mpReferenceKF;
-  std::vector<KeyFrame*> mvpLocalKeyFrames;
-  std::vector<MapPoint*> mvpLocalMapPoints;
-
-  // System
-  System* mpSystem;
-
-  // Atlas
-  Atlas_ptr mpAtlas;
-
-  // Calibration matrix
-  cv::Mat mK;
-  Eigen::Matrix3f mK_;
-  cv::Mat mDistCoef;
-  float mbf;
-  float mImageScale;
-
-  float mImuFreq;
-  double mImuPer;
-  bool mInsertKFsLost;
-
-  // New KeyFrame rules (according to fps)
-  int mMinFrames;
-  int mMaxFrames;
-
-  int mnFirstImuFrameId;
-  int mnFramesToResetIMU;
-
-  // Threshold close/far points
-  // Points seen as close by the stereo/RGBD sensor are considered reliable
-  // and inserted from just one frame. Far points requiere a match in two
-  // keyframes.
-  float mThDepth;
-
-  // For RGB-D inputs only. For some datasets (e.g. TUM) the depthmap values are
-  // scaled.
-  float mDepthMapFactor;
-
-  // Current matches in frame
-  int mnMatchesInliers;
-
-  // Last Frame, KeyFrame and Relocalisation Info
-  KeyFrame* mpLastKeyFrame;
-  unsigned int mnLastKeyFrameId;
-  unsigned int mnLastRelocFrameId;
-  double mTimeStampLost;
-  double time_recently_lost;
-
-  unsigned int mnFirstFrameId;
-  unsigned int mnInitialFrameId;
-  unsigned int mnLastInitFrameId;
-
-  bool mbCreatedMap;
-
-  // Motion Model
-  bool mbVelocity{false};
-  Sophus::SE3f mVelocity;
-
-  // Color order (true RGB, false BGR, ignored if grayscale)
-  bool mbRGB;
-
-  list<MapPoint*> mlpTemporalPoints;
-
-  // int nMapChangeIndex;
-
-  int mnNumDataset;
-
-  ofstream f_track_stats;
-
-  ofstream f_track_times;
-  double mTime_PreIntIMU;
-  double mTime_PosePred;
-  double mTime_LocalMapTrack;
-  double mTime_NewKF_Dec;
-
-  std::shared_ptr<GeometricCamera> mpCamera;
-  std::shared_ptr<GeometricCamera> mpCamera2;
-
-  int initID, lastID;
-
-  Sophus::SE3f mTlr;
-
-  void newParameterLoader(Settings* settings);
-
-#ifdef REGISTER_LOOP
-  bool Stop();
-
-  bool mbStopped;
-  bool mbStopRequested;
-  bool mbNotStop;
-  std::mutex mMutexStop;
-#endif
-
- public:
-  cv::Mat mImRight;
-};
-
-}  // namespace MORB_SLAM
-
diff --git a/include/TwoViewReconstruction.h b/include/TwoViewReconstruction.h
deleted file mode 100644
index 92bb39de..00000000
--- a/include/TwoViewReconstruction.h
+++ /dev/null
@@ -1,96 +0,0 @@
-/**
-* This file is part of ORB-SLAM3
-*
-* Copyright (C) 2017-2021 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/>.
-*/
-
-#pragma once
-
-#include <opencv2/opencv.hpp>
-#include <Eigen/Core>
-#include <unordered_set>
-
-#include <sophus/se3.hpp>
-
-namespace MORB_SLAM
-{
-
-    class TwoViewReconstruction
-    {
-        typedef std::pair<int,int> Match;
-
-    public:
-        
-        // Fix the reference frame
-        TwoViewReconstruction(const Eigen::Matrix3f& k, float sigma = 1.0, int iterations = 200);
-
-        // Computes in parallel a fundamental matrix and a homography
-        // Selects a model and tries to recover the motion and the structure from motion
-        bool Reconstruct(const std::vector<cv::KeyPoint>& vKeys1, const std::vector<cv::KeyPoint>& vKeys2, const std::vector<int> &vMatches12,
-                          Sophus::SE3f &T21, std::vector<cv::Point3f> &vP3D, std::vector<bool> &vbTriangulated);
-
-    private:
-
-        void FindHomography(std::vector<bool> &vbMatchesInliers, float &score, Eigen::Matrix3f &H21);
-        void FindFundamental(std::vector<bool> &vbInliers, float &score, Eigen::Matrix3f &F21);
-
-        Eigen::Matrix3f ComputeH21(const std::vector<cv::Point2f> &vP1, const std::vector<cv::Point2f> &vP2);
-        Eigen::Matrix3f ComputeF21(const std::vector<cv::Point2f> &vP1, const std::vector<cv::Point2f> &vP2);
-
-        float CheckHomography(const Eigen::Matrix3f &H21, const Eigen::Matrix3f &H12, std::vector<bool> &vbMatchesInliers, float sigma);
-
-        float CheckFundamental(const Eigen::Matrix3f &F21, std::vector<bool> &vbMatchesInliers, float sigma);
-
-        bool ReconstructF(std::vector<bool> &vbMatchesInliers, Eigen::Matrix3f &F21, Eigen::Matrix3f &K,
-                          Sophus::SE3f &T21, std::vector<cv::Point3f> &vP3D, std::vector<bool> &vbTriangulated, float minParallax, int minTriangulated);
-
-        bool ReconstructH(std::vector<bool> &vbMatchesInliers, Eigen::Matrix3f &H21, Eigen::Matrix3f &K,
-                          Sophus::SE3f &T21, std::vector<cv::Point3f> &vP3D,std:: vector<bool> &vbTriangulated, float minParallax, int minTriangulated);
-
-        void Normalize(const std::vector<cv::KeyPoint> &vKeys, std::vector<cv::Point2f> &vNormalizedPoints, Eigen::Matrix3f &T);
-
-
-        int CheckRT(const Eigen::Matrix3f &R, const Eigen::Vector3f &t, const std::vector<cv::KeyPoint> &vKeys1, const std::vector<cv::KeyPoint> &vKeys2,
-                    const std::vector<Match> &vMatches12, std::vector<bool> &vbMatchesInliers,
-                    const Eigen::Matrix3f &K, std::vector<cv::Point3f> &vP3D, float th2, std::vector<bool> &vbGood, float &parallax);
-
-        void DecomposeE(const Eigen::Matrix3f &E, Eigen::Matrix3f &R1, Eigen::Matrix3f &R2, Eigen::Vector3f &t);
-
-
-        // Keypoints from Reference Frame (Frame 1)
-        std::vector<cv::KeyPoint> mvKeys1;
-
-        // Keypoints from Current Frame (Frame 2)
-        std::vector<cv::KeyPoint> mvKeys2;
-
-        // Current Matches from Reference to Current
-        std::vector<Match> mvMatches12;
-        std::vector<bool> mvbMatched1;
-
-        // Calibration
-        Eigen::Matrix3f mK;
-
-        // Standard Deviation and Variance
-        float mSigma, mSigma2;
-
-        // Ransac max iterations
-        int mMaxIterations;
-
-        // Ransac sets
-        std::vector<std::vector<size_t> > mvSets;
-
-    };
-
-} //namespace ORB_SLAM
diff --git a/include/Viewer.h b/include/Viewer.h
deleted file mode 100644
index 91aae010..00000000
--- a/include/Viewer.h
+++ /dev/null
@@ -1,77 +0,0 @@
-/**
- * This file is part of ORB-SLAM3
- *
- * Copyright (C) 2017-2021 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/>.
- */
-
-#pragma once
-
-#include <mutex>
-#include <memory>
-
-#include "ImprovedTypes.hpp"
-#include "FrameDrawer.h"
-#include "MapDrawer.h"
-#include "Settings.h"
-
-namespace MORB_SLAM {
-
-class Viewer {
-  void newParameterLoader(const Settings& settings);
-  // Main thread function. Draw points, keyframes, the current camera pose and
-  // the last processed frame. Drawing is refreshed according to the camera fps.
-  // We use Pangolin.
-  void Run();
- public:
-
-  Viewer(const System_ptr &pSystem, const std::string &strSettingPath);
-  Viewer(const System_ptr &pSystem, const Settings &settings);
-
-  virtual ~Viewer();
-
-  void update(const Sophus::SE3f &pose);
-
-  void close();
-  bool isClosed() const;
-  bool isOpen() const;
-
-
- private:
-  void setBoth(const bool b);
-  bool ParseViewerParamFile(cv::FileStorage& fSettings);
-
-  bool Stop();
-
-  System_ptr mpSystem;
-  FrameDrawer mpFrameDrawer;
-  MapDrawer mpMapDrawer;
-  Tracking_ptr mpTracker;
-  std::thread mptViewer;
-  bool both;
-
-  // 1/fps in ms
-  double mT;
-  float mImageWidth, mImageHeight;
-  float mImageViewerScale;
-
-  float mViewpointX, mViewpointY, mViewpointZ, mViewpointF;
-
-  bool mbClosed;
-};
-
-}  // namespace MORB_SLAM

From 88a7ec10db15bcc42e79c73756566a4e08c418da Mon Sep 17 00:00:00 2001
From: Soldann <guotl321@gmail.com>
Date: Mon, 8 Aug 2022 09:37:14 -1000
Subject: [PATCH 26/26] add include

---
 include/MORB_SLAM/Atlas.h                     | 163 ++++
 include/MORB_SLAM/Camera.hpp                  |  92 ++
 .../MORB_SLAM/CameraModels/GeometricCamera.h  | 115 +++
 .../MORB_SLAM/CameraModels/KannalaBrandt8.h   | 134 +++
 include/MORB_SLAM/CameraModels/Pinhole.h      | 110 +++
 include/MORB_SLAM/Converter.h                 |  78 ++
 include/MORB_SLAM/Frame.h                     | 376 ++++++++
 include/MORB_SLAM/FrameDrawer.h               |  71 ++
 include/MORB_SLAM/G2oTypes.h                  | 844 ++++++++++++++++++
 include/MORB_SLAM/GeometricTools.h            |  81 ++
 include/MORB_SLAM/ImprovedTypes.hpp           |  51 ++
 include/MORB_SLAM/ImuTypes.h                  | 272 ++++++
 include/MORB_SLAM/KeyFrame.h                  | 542 +++++++++++
 include/MORB_SLAM/KeyFrameDatabase.h          |  99 ++
 include/MORB_SLAM/LocalMapping.h              | 198 ++++
 include/MORB_SLAM/LoopClosing.h               | 245 +++++
 include/MORB_SLAM/MLPnPsolver.h               | 243 +++++
 include/MORB_SLAM/Map.h                       | 205 +++++
 include/MORB_SLAM/MapDrawer.h                 |  73 ++
 include/MORB_SLAM/MapPoint.h                  | 253 ++++++
 include/MORB_SLAM/ORBVocabulary.h             |  32 +
 include/MORB_SLAM/ORBextractor.h              | 102 +++
 include/MORB_SLAM/ORBmatcher.h                | 131 +++
 include/MORB_SLAM/OptimizableTypes.h          | 233 +++++
 include/MORB_SLAM/Optimizer.h                 | 141 +++
 include/MORB_SLAM/SerializationUtils.h        | 154 ++++
 include/MORB_SLAM/Settings.h                  | 239 +++++
 include/MORB_SLAM/Sim3Solver.h                | 137 +++
 include/MORB_SLAM/System.h                    | 245 +++++
 include/MORB_SLAM/Tracking.h                  | 364 ++++++++
 include/MORB_SLAM/TwoViewReconstruction.h     |  96 ++
 include/MORB_SLAM/Viewer.h                    |  77 ++
 32 files changed, 6196 insertions(+)
 create mode 100644 include/MORB_SLAM/Atlas.h
 create mode 100644 include/MORB_SLAM/Camera.hpp
 create mode 100644 include/MORB_SLAM/CameraModels/GeometricCamera.h
 create mode 100644 include/MORB_SLAM/CameraModels/KannalaBrandt8.h
 create mode 100644 include/MORB_SLAM/CameraModels/Pinhole.h
 create mode 100644 include/MORB_SLAM/Converter.h
 create mode 100644 include/MORB_SLAM/Frame.h
 create mode 100644 include/MORB_SLAM/FrameDrawer.h
 create mode 100644 include/MORB_SLAM/G2oTypes.h
 create mode 100644 include/MORB_SLAM/GeometricTools.h
 create mode 100644 include/MORB_SLAM/ImprovedTypes.hpp
 create mode 100644 include/MORB_SLAM/ImuTypes.h
 create mode 100644 include/MORB_SLAM/KeyFrame.h
 create mode 100644 include/MORB_SLAM/KeyFrameDatabase.h
 create mode 100644 include/MORB_SLAM/LocalMapping.h
 create mode 100644 include/MORB_SLAM/LoopClosing.h
 create mode 100644 include/MORB_SLAM/MLPnPsolver.h
 create mode 100644 include/MORB_SLAM/Map.h
 create mode 100644 include/MORB_SLAM/MapDrawer.h
 create mode 100644 include/MORB_SLAM/MapPoint.h
 create mode 100644 include/MORB_SLAM/ORBVocabulary.h
 create mode 100644 include/MORB_SLAM/ORBextractor.h
 create mode 100644 include/MORB_SLAM/ORBmatcher.h
 create mode 100644 include/MORB_SLAM/OptimizableTypes.h
 create mode 100644 include/MORB_SLAM/Optimizer.h
 create mode 100644 include/MORB_SLAM/SerializationUtils.h
 create mode 100644 include/MORB_SLAM/Settings.h
 create mode 100644 include/MORB_SLAM/Sim3Solver.h
 create mode 100644 include/MORB_SLAM/System.h
 create mode 100644 include/MORB_SLAM/Tracking.h
 create mode 100644 include/MORB_SLAM/TwoViewReconstruction.h
 create mode 100644 include/MORB_SLAM/Viewer.h

diff --git a/include/MORB_SLAM/Atlas.h b/include/MORB_SLAM/Atlas.h
new file mode 100644
index 00000000..27cc9812
--- /dev/null
+++ b/include/MORB_SLAM/Atlas.h
@@ -0,0 +1,163 @@
+/**
+ * This file is part of ORB-SLAM3
+ *
+ * Copyright (C) 2017-2021 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/>.
+ */
+
+#pragma once
+
+#include <boost/serialization/export.hpp>
+#include <boost/serialization/vector.hpp>
+#include <boost/serialization/shared_ptr.hpp>
+#include <mutex>
+#include <set>
+#include <map>
+#include <vector>
+
+#include "MORB_SLAM/CameraModels/GeometricCamera.h"
+#include "MORB_SLAM/CameraModels/KannalaBrandt8.h"
+#include "MORB_SLAM/KeyFrame.h"
+#include "MORB_SLAM/Map.h"
+#include "MORB_SLAM/MapPoint.h"
+#include "MORB_SLAM/CameraModels/Pinhole.h"
+
+namespace MORB_SLAM {
+class Map;
+class MapPoint;
+class KeyFrame;
+class KeyFrameDatabase;
+class Frame;
+class KannalaBrandt8;
+class Pinhole;
+
+// BOOST_CLASS_EXPORT_GUID(Pinhole, "Pinhole")
+// BOOST_CLASS_EXPORT_GUID(KannalaBrandt8, "KannalaBrandt8")
+
+class Atlas {
+  friend class boost::serialization::access;
+
+  template <class Archive>
+  void serialize(Archive& ar, const unsigned int version) {
+    ar.template register_type<Pinhole>();
+    ar.template register_type<KannalaBrandt8>();
+
+    // Save/load a set structure, the set structure is broken in libboost 1.58
+    // for ubuntu 16.04, a vector is serializated
+    // ar & mspMaps;
+    ar& mvpBackupMaps;
+    ar& mvpCameras;
+    // Need to save/load the static Id from Frame, KeyFrame, MapPoint and Map
+    ar& Map::nNextId;
+    ar& Frame::nNextId;
+    ar& KeyFrame::nNextId;
+    ar& MapPoint::nNextId;
+    ar& GeometricCamera::nNextId;
+    ar& mnLastInitKFidMap;
+  }
+
+ public:
+  
+
+  Atlas();
+  Atlas(int initKFid);  // When its initialization the first map is created
+  ~Atlas();
+
+  void CreateNewMap();
+  void ChangeMap(std::shared_ptr<Map> pMap);
+
+  unsigned long int GetLastInitKFid();
+
+  // Method for change components in the current map
+  void AddKeyFrame(KeyFrame* pKF);
+  void AddMapPoint(MapPoint* pMP);
+  // void EraseMapPoint(MapPoint* pMP);
+  // void EraseKeyFrame(KeyFrame* pKF);
+
+  std::shared_ptr<GeometricCamera> AddCamera(const std::shared_ptr<GeometricCamera> &pCam);
+  std::vector<std::shared_ptr<GeometricCamera>> GetAllCameras();
+
+  /* All methods without Map pointer work on current map */
+  void SetReferenceMapPoints(const std::vector<MapPoint*>& vpMPs);
+  void InformNewBigChange();
+  int GetLastBigChangeIdx();
+
+  long unsigned int MapPointsInMap();
+  long unsigned KeyFramesInMap();
+
+  // Method for get data in current map
+  std::vector<KeyFrame*> GetAllKeyFrames();
+  std::vector<MapPoint*> GetAllMapPoints();
+  std::vector<MapPoint*> GetReferenceMapPoints();
+
+  std::vector<std::shared_ptr<Map>> GetAllMaps();
+
+  int CountMaps();
+
+  void clearMap();
+
+  void clearAtlas();
+
+  std::shared_ptr<Map> GetCurrentMap(System* sys = nullptr);
+
+  void SetMapBad(std::shared_ptr<Map> pMap);
+  void RemoveBadMaps();
+
+  bool isInertial();
+  void SetInertialSensor();
+  void SetImuInitialized();
+  bool isImuInitialized();
+
+  // Function for garantee the correction of serialization of this object
+  void PreSave();
+  void PostLoad();
+
+  std::map<long unsigned int, KeyFrame*> GetAtlasKeyframes();
+
+  void SetKeyFrameDababase(KeyFrameDatabase* pKFDB);
+  KeyFrameDatabase* GetKeyFrameDatabase();
+
+  void SetORBVocabulary(ORBVocabulary* pORBVoc);
+  ORBVocabulary* GetORBVocabulary();
+
+  long unsigned int GetNumLivedKF();
+
+  long unsigned int GetNumLivedMP();
+
+ protected:
+  std::set<std::shared_ptr<Map>> mspMaps;
+  std::set<std::shared_ptr<Map>> mspBadMaps;
+  // Its necessary change the container from set to vector because libboost 1.58
+  // and Ubuntu 16.04 have an error with this cointainer
+  std::vector<std::shared_ptr<Map>> mvpBackupMaps;
+
+  std::shared_ptr<Map> mpCurrentMap;
+
+  std::vector<std::shared_ptr<GeometricCamera>> mvpCameras;
+
+  unsigned long int mnLastInitKFidMap;
+
+  // Class references for the map reconstruction from the save file
+  KeyFrameDatabase* mpKeyFrameDB;
+  ORBVocabulary* mpORBVocabulary;
+
+  // Mutex
+  std::mutex mMutexAtlas;
+
+};  // class Atlas
+
+}  // namespace MORB_SLAM
diff --git a/include/MORB_SLAM/Camera.hpp b/include/MORB_SLAM/Camera.hpp
new file mode 100644
index 00000000..7d94ca1e
--- /dev/null
+++ b/include/MORB_SLAM/Camera.hpp
@@ -0,0 +1,92 @@
+#pragma once
+#include <thread>
+#include <string>
+#include "ImprovedTypes.hpp"
+#include <mutex>
+#include <condition_variable>
+#include <future>
+#include <functional>
+#include <memory>
+#include <exception>
+#include <stdexcept>
+#include <deque>
+#include <utility>
+
+namespace MORB_SLAM{
+
+
+template<typename T>
+class ManagedFuture{
+protected:
+    std::shared_ptr<std::promise<T>> promise;
+    std::shared_future<T> future;
+
+   ManagedFuture(): promise{std::make_shared<std::promise<T>>()}, future{promise->get_future()} {} // make default constructor protected
+public:
+    virtual ~ManagedFuture(){}
+
+    // functions from std::shared_future<T>
+
+    const T& get() const { return future.get(); }
+    void wait() const { future.wait(); }
+
+    template<class Rep, class Period>
+    std::future_status wait_for( const std::chrono::duration<Rep,Period>& timeout_duration ) const { return future.wait_for(timeout_duration); }
+
+    template<class Clock, class Duration>
+    std::future_status wait_until( const std::chrono::time_point<Clock,Duration>& timeout_time ) const { return future.wait_until(timeout_time); }
+};
+
+template<typename T>
+class ManagedPromise: public ManagedFuture<T>{
+    std::shared_ptr<std::mutex> mutex;
+public:
+    ManagedPromise(): mutex{std::make_shared<std::mutex>()} {}
+    virtual ~ManagedPromise(){}
+    
+    // functions from std::promise<T>
+
+    void set_value_at_thread_exit(const T& value){ std::scoped_lock<std::mutex> lock(*mutex); ManagedFuture<T>::promise->set_value_at_thread_exit(value); }
+    void set_value_at_thread_exit(T&& value){ std::scoped_lock<std::mutex> lock(*mutex); ManagedFuture<T>::promise->set_value_at_thread_exit(value); }
+    void set_value_at_thread_exit(T& value){ std::scoped_lock<std::mutex> lock(*mutex); ManagedFuture<T>::promise->set_value_at_thread_exit(value); }
+    void set_value_at_thread_exit(){ std::scoped_lock<std::mutex> lock(*mutex); ManagedFuture<T>::promise->set_value_at_thread_exit(); }
+    void set_exception_at_thread_exit(const std::exception_ptr &p ){ std::scoped_lock<std::mutex> lock(*mutex); ManagedFuture<T>::promise->set_exception_at_thread_exit(p); }
+    void set_exception(const std::exception_ptr &p ){ std::scoped_lock<std::mutex> lock(*mutex); ManagedFuture<T>::promise->set_exception(p); }
+    void set_value(const T& value){ std::scoped_lock<std::mutex> lock(*mutex); ManagedFuture<T>::promise->set_value(value); }
+    void set_value(T&& value){ std::scoped_lock<std::mutex> lock(*mutex); ManagedFuture<T>::promise->set_value(value); }
+    void set_value(T& value){ std::scoped_lock<std::mutex> lock(*mutex); ManagedFuture<T>::promise->set_value(value); }
+    void set_value(){ std::scoped_lock<std::mutex> lock(*mutex); ManagedFuture<T>::promise->set_value(); }
+};
+
+class Camera{
+    std::deque<std::pair<ManagedPromise<bool>, std::function<void(void)>>> ljobs;
+    std::deque<std::pair<ManagedPromise<bool>, std::function<void(void)>>> rjobs;
+
+    std::string name;
+    CameraType::eSensor type;
+    std::mutex camMutex;
+    
+    std::condition_variable camCV;
+
+    bool shouldStop;
+    
+    std::thread lthread;
+    std::thread rthread;
+
+    void threadExec(std::deque<std::pair<ManagedPromise<bool>, std::function<void(void)>>> *jobs);
+public:
+    Camera(CameraType::eSensor type, const std::string &name="camera");
+    virtual ~Camera();
+
+    ManagedFuture<bool> queue(std::function<void(void)> func, bool isLeft=true);
+    ManagedFuture<bool> queueLeft(const std::function<void(void)> &func);
+    ManagedFuture<bool> queueRight(const std::function<void(void)> &func);
+
+    CameraType::eSensor getType() const;
+    const std::string &getName() const;
+};
+
+typedef std::shared_ptr<Camera> Camera_ptr;
+typedef std::weak_ptr<Camera> Camera_wptr;
+
+} // namespace MORB_SLAM
\ No newline at end of file
diff --git a/include/MORB_SLAM/CameraModels/GeometricCamera.h b/include/MORB_SLAM/CameraModels/GeometricCamera.h
new file mode 100644
index 00000000..9fdde438
--- /dev/null
+++ b/include/MORB_SLAM/CameraModels/GeometricCamera.h
@@ -0,0 +1,115 @@
+/**
+ * This file is part of ORB-SLAM3
+ *
+ * Copyright (C) 2017-2021 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/>.
+ */
+
+#pragma once
+
+#include <Eigen/Geometry>
+#include <boost/serialization/access.hpp>
+#include <boost/serialization/assume_abstract.hpp>
+#include <boost/serialization/base_object.hpp>
+#include <boost/serialization/export.hpp>
+#include <boost/serialization/serialization.hpp>
+#include <boost/serialization/vector.hpp>
+#include <opencv2/core/core.hpp>
+#include <opencv2/features2d/features2d.hpp>
+#include <opencv2/imgproc/imgproc.hpp>
+#include <sophus/se3.hpp>
+#include <vector>
+
+#include "MORB_SLAM/Converter.h"
+#include "MORB_SLAM/GeometricTools.h"
+
+namespace MORB_SLAM {
+class GeometricCamera {
+  friend class boost::serialization::access;
+
+  template <class Archive>
+  void serialize(Archive& ar, const unsigned int version) {
+    ar& mnId;
+    ar& mnType;
+    ar& mvParameters;
+  }
+
+ public:
+  GeometricCamera() {}
+  GeometricCamera(const std::vector<float>& _vParameters)
+      : mvParameters(_vParameters) {}
+  ~GeometricCamera() {}
+
+  virtual cv::Point2f project(const cv::Point3f& p3D) const = 0;
+  virtual Eigen::Vector2d project(const Eigen::Vector3d& v3D) const = 0;
+  virtual Eigen::Vector2f project(const Eigen::Vector3f& v3D) const = 0;
+  virtual Eigen::Vector2f projectMat(const cv::Point3f& p3D) const = 0;
+
+  virtual float uncertainty2(const Eigen::Matrix<double, 2, 1>& p2D) = 0;
+
+  virtual Eigen::Vector3f unprojectEig(const cv::Point2f& p2D) const = 0;
+  virtual cv::Point3f unproject(const cv::Point2f& p2D) const = 0;
+
+  virtual Eigen::Matrix<double, 2, 3> projectJac(
+      const Eigen::Vector3d& v3D) = 0;
+
+  virtual bool ReconstructWithTwoViews(const std::vector<cv::KeyPoint>& vKeys1,
+                                       const std::vector<cv::KeyPoint>& vKeys2,
+                                       const std::vector<int>& vMatches12,
+                                       Sophus::SE3f& T21,
+                                       std::vector<cv::Point3f>& vP3D,
+                                       std::vector<bool>& vbTriangulated) = 0;
+
+  virtual cv::Mat toK() const = 0;
+  virtual Eigen::Matrix3f toK_() const = 0;
+
+  virtual bool epipolarConstrain(const std::shared_ptr<GeometricCamera> &otherCamera,
+                                 const cv::KeyPoint& kp1,
+                                 const cv::KeyPoint& kp2,
+                                 const Eigen::Matrix3f& R12,
+                                 const Eigen::Vector3f& t12,
+                                 const float sigmaLevel, const float unc) = 0;
+
+  float getParameter(const int i) { return mvParameters[i]; }
+  void setParameter(const float p, const size_t i) { mvParameters[i] = p; }
+
+  size_t size() { return mvParameters.size(); }
+
+  virtual bool matchAndtriangulate(const cv::KeyPoint& kp1,
+                                   const cv::KeyPoint& kp2,
+                                   GeometricCamera* pOther, Sophus::SE3f& Tcw1,
+                                   Sophus::SE3f& Tcw2, const float sigmaLevel1,
+                                   const float sigmaLevel2,
+                                   Eigen::Vector3f& x3Dtriangulated) = 0;
+
+  unsigned int GetId() { return mnId; }
+
+  unsigned int GetType() { return mnType; }
+
+  const static unsigned int CAM_PINHOLE = 0;
+  const static unsigned int CAM_FISHEYE = 1;
+
+  static long unsigned int nNextId;
+
+ protected:
+  std::vector<float> mvParameters;
+
+  unsigned int mnId;
+
+  unsigned int mnType;
+};
+}  // namespace MORB_SLAM
diff --git a/include/MORB_SLAM/CameraModels/KannalaBrandt8.h b/include/MORB_SLAM/CameraModels/KannalaBrandt8.h
new file mode 100644
index 00000000..b77c68af
--- /dev/null
+++ b/include/MORB_SLAM/CameraModels/KannalaBrandt8.h
@@ -0,0 +1,134 @@
+/**
+ * This file is part of ORB-SLAM3
+ *
+ * Copyright (C) 2017-2021 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/>.
+ */
+
+#pragma once
+
+#include <assert.h>
+#include <memory>
+#include "MORB_SLAM/CameraModels/GeometricCamera.h"
+#include "MORB_SLAM/TwoViewReconstruction.h"
+
+namespace MORB_SLAM {
+class KannalaBrandt8 : public GeometricCamera {
+  friend class boost::serialization::access;
+
+  template <class Archive>
+  void serialize(Archive& ar, const unsigned int version) {
+    ar& boost::serialization::base_object<GeometricCamera>(*this);
+    ar& const_cast<float&>(precision);
+  }
+
+ public:
+  KannalaBrandt8() : precision(1e-6) {
+    mvParameters.resize(8);
+    mnId = nNextId++;
+    mnType = CAM_FISHEYE;
+  }
+  KannalaBrandt8(const std::vector<float> _vParameters)
+      : GeometricCamera(_vParameters),
+        mvLappingArea(2, 0),
+        precision(1e-6),
+        tvr(nullptr) {
+    assert(mvParameters.size() == 8);
+    mnId = nNextId++;
+    mnType = CAM_FISHEYE;
+  }
+
+  KannalaBrandt8(const std::vector<float> _vParameters, const float _precision)
+      : GeometricCamera(_vParameters),
+        mvLappingArea(2, 0),
+        precision(_precision) {
+    assert(mvParameters.size() == 8);
+    mnId = nNextId++;
+    mnType = CAM_FISHEYE;
+  }
+  KannalaBrandt8(KannalaBrandt8* pKannala)
+      : GeometricCamera(pKannala->mvParameters),
+        mvLappingArea(2, 0),
+        precision(pKannala->precision),
+        tvr(nullptr) {
+    assert(mvParameters.size() == 8);
+    mnId = nNextId++;
+    mnType = CAM_FISHEYE;
+  }
+
+  cv::Point2f project(const cv::Point3f& p3D) const;
+  Eigen::Vector2d project(const Eigen::Vector3d& v3D) const;
+  Eigen::Vector2f project(const Eigen::Vector3f& v3D) const;
+  Eigen::Vector2f projectMat(const cv::Point3f& p3D) const;
+
+  float uncertainty2(const Eigen::Matrix<double, 2, 1>& p2D);
+
+  Eigen::Vector3f unprojectEig(const cv::Point2f& p2D) const;
+  cv::Point3f unproject(const cv::Point2f& p2D) const;
+
+  Eigen::Matrix<double, 2, 3> projectJac(const Eigen::Vector3d& v3D);
+
+  bool ReconstructWithTwoViews(const std::vector<cv::KeyPoint>& vKeys1,
+                               const std::vector<cv::KeyPoint>& vKeys2,
+                               const std::vector<int>& vMatches12,
+                               Sophus::SE3f& T21,
+                               std::vector<cv::Point3f>& vP3D,
+                               std::vector<bool>& vbTriangulated);
+
+  cv::Mat toK() const;
+  Eigen::Matrix3f toK_() const;
+
+  bool epipolarConstrain(const std::shared_ptr<GeometricCamera> &pCamera2, const cv::KeyPoint& kp1,
+                         const cv::KeyPoint& kp2, const Eigen::Matrix3f& R12,
+                         const Eigen::Vector3f& t12, const float sigmaLevel,
+                         const float unc);
+
+  float TriangulateMatches(const std::shared_ptr<GeometricCamera> &pCamera2, const cv::KeyPoint& kp1,
+                           const cv::KeyPoint& kp2, const Eigen::Matrix3f& R12,
+                           const Eigen::Vector3f& t12, const float sigmaLevel,
+                           const float unc, Eigen::Vector3f& p3D);
+
+  std::vector<int> mvLappingArea;
+
+  bool matchAndtriangulate(const cv::KeyPoint& kp1, const cv::KeyPoint& kp2,
+                           GeometricCamera* pOther, Sophus::SE3f& Tcw1,
+                           Sophus::SE3f& Tcw2, const float sigmaLevel1,
+                           const float sigmaLevel2,
+                           Eigen::Vector3f& x3Dtriangulated);
+
+  friend std::ostream& operator<<(std::ostream& os, const KannalaBrandt8& kb);
+  friend std::istream& operator>>(std::istream& is, KannalaBrandt8& kb);
+
+  float GetPrecision() { return precision; }
+
+  bool IsEqual(const std::shared_ptr<GeometricCamera> &pCam);
+
+ private:
+  const float precision;
+
+  // Parameters vector corresponds to
+  //[fx, fy, cx, cy, k0, k1, k2, k3]
+
+  TwoViewReconstruction* tvr;
+
+  void Triangulate(const cv::Point2f& p1, const cv::Point2f& p2,
+                   const Eigen::Matrix<float, 3, 4>& Tcw1,
+                   const Eigen::Matrix<float, 3, 4>& Tcw2,
+                   Eigen::Vector3f& x3D);
+};
+}  // namespace MORB_SLAM
+
diff --git a/include/MORB_SLAM/CameraModels/Pinhole.h b/include/MORB_SLAM/CameraModels/Pinhole.h
new file mode 100644
index 00000000..f5533854
--- /dev/null
+++ b/include/MORB_SLAM/CameraModels/Pinhole.h
@@ -0,0 +1,110 @@
+/**
+ * This file is part of ORB-SLAM3
+ *
+ * Copyright (C) 2017-2021 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/>.
+ */
+
+#pragma once
+
+#include <assert.h>
+#include <memory>
+#include "MORB_SLAM/CameraModels/GeometricCamera.h"
+#include "MORB_SLAM/TwoViewReconstruction.h"
+
+namespace MORB_SLAM {
+class Pinhole : public GeometricCamera {
+  friend class boost::serialization::access;
+
+  template <class Archive>
+  void serialize(Archive& ar, const unsigned int version) {
+    ar& boost::serialization::base_object<GeometricCamera>(*this);
+  }
+
+ public:
+  Pinhole() {
+    mvParameters.resize(4);
+    mnId = nNextId++;
+    mnType = CAM_PINHOLE;
+  }
+  Pinhole(const std::vector<float> _vParameters)
+      : GeometricCamera(_vParameters), tvr(nullptr) {
+    assert(mvParameters.size() == 4);
+    mnId = nNextId++;
+    mnType = CAM_PINHOLE;
+  }
+
+  Pinhole(Pinhole* pPinhole)
+      : GeometricCamera(pPinhole->mvParameters), tvr(nullptr) {
+    assert(mvParameters.size() == 4);
+    mnId = nNextId++;
+    mnType = CAM_PINHOLE;
+  }
+
+  ~Pinhole() {
+    if (tvr) delete tvr;
+  }
+
+  cv::Point2f project(const cv::Point3f& p3D) const;
+  Eigen::Vector2d project(const Eigen::Vector3d& v3D) const;
+  Eigen::Vector2f project(const Eigen::Vector3f& v3D) const;
+  Eigen::Vector2f projectMat(const cv::Point3f& p3D) const;
+
+  float uncertainty2(const Eigen::Matrix<double, 2, 1>& p2D);
+
+  Eigen::Vector3f unprojectEig(const cv::Point2f& p2D) const;
+  cv::Point3f unproject(const cv::Point2f& p2D) const;
+
+  Eigen::Matrix<double, 2, 3> projectJac(const Eigen::Vector3d& v3D);
+
+  bool ReconstructWithTwoViews(const std::vector<cv::KeyPoint>& vKeys1,
+                               const std::vector<cv::KeyPoint>& vKeys2,
+                               const std::vector<int>& vMatches12,
+                               Sophus::SE3f& T21,
+                               std::vector<cv::Point3f>& vP3D,
+                               std::vector<bool>& vbTriangulated);
+
+  cv::Mat toK() const;
+  Eigen::Matrix3f toK_()const;
+
+  bool epipolarConstrain(const std::shared_ptr<GeometricCamera> &pCamera2, const cv::KeyPoint& kp1,
+                         const cv::KeyPoint& kp2, const Eigen::Matrix3f& R12,
+                         const Eigen::Vector3f& t12, const float sigmaLevel,
+                         const float unc);
+
+  bool matchAndtriangulate(const cv::KeyPoint& kp1, const cv::KeyPoint& kp2,
+                           GeometricCamera* pOther, Sophus::SE3f& Tcw1,
+                           Sophus::SE3f& Tcw2, const float sigmaLevel1,
+                           const float sigmaLevel2,
+                           Eigen::Vector3f& x3Dtriangulated) {
+    return false;
+  }
+
+  friend std::ostream& operator<<(std::ostream& os, const Pinhole& ph);
+  friend std::istream& operator>>(std::istream& os, Pinhole& ph);
+
+  bool IsEqual(const std::shared_ptr<GeometricCamera> &pCam);
+
+ private:
+  // Parameters vector corresponds to
+  //      [fx, fy, cx, cy]
+  TwoViewReconstruction* tvr;
+};
+}  // namespace MORB_SLAM
+
+// BOOST_CLASS_EXPORT_KEY(ORBSLAM2::Pinhole)
+
diff --git a/include/MORB_SLAM/Converter.h b/include/MORB_SLAM/Converter.h
new file mode 100644
index 00000000..2f162d47
--- /dev/null
+++ b/include/MORB_SLAM/Converter.h
@@ -0,0 +1,78 @@
+/**
+ * This file is part of ORB-SLAM3
+ *
+ * Copyright (C) 2017-2021 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/>.
+ */
+
+#pragma once
+
+#include <Eigen/Dense>
+#include <opencv2/core/core.hpp>
+
+#include "g2o/types/types_seven_dof_expmap.h"
+#include "g2o/types/types_six_dof_expmap.h"
+#include "sophus/geometry.hpp"
+#include "sophus/sim3.hpp"
+
+namespace MORB_SLAM {
+
+class Converter {
+ public:
+  
+  static std::vector<cv::Mat> toDescriptorVector(const cv::Mat &Descriptors);
+
+  static g2o::SE3Quat toSE3Quat(const cv::Mat &cvT);
+  static g2o::SE3Quat toSE3Quat(const Sophus::SE3f &T);
+  static g2o::SE3Quat toSE3Quat(const g2o::Sim3 &gSim3);
+
+  // TODO templetize these functions
+  static cv::Mat toCvMat(const g2o::SE3Quat &SE3);
+  static cv::Mat toCvMat(const g2o::Sim3 &Sim3);
+  static cv::Mat toCvMat(const Eigen::Matrix<double, 4, 4> &m);
+  static cv::Mat toCvMat(const Eigen::Matrix<float, 4, 4> &m);
+  static cv::Mat toCvMat(const Eigen::Matrix<float, 3, 4> &m);
+  static cv::Mat toCvMat(const Eigen::Matrix3d &m);
+  static cv::Mat toCvMat(const Eigen::Matrix<double, 3, 1> &m);
+  static cv::Mat toCvMat(const Eigen::Matrix<float, 3, 1> &m);
+  static cv::Mat toCvMat(const Eigen::Matrix<float, 3, 3> &m);
+
+  static cv::Mat toCvMat(const Eigen::MatrixXf &m);
+  static cv::Mat toCvMat(const Eigen::MatrixXd &m);
+
+  static cv::Mat toCvSE3(const Eigen::Matrix<double, 3, 3> &R,
+                         const Eigen::Matrix<double, 3, 1> &t);
+  static cv::Mat tocvSkewMatrix(const cv::Mat &v);
+
+  static Eigen::Matrix<double, 3, 1> toVector3d(const cv::Mat &cvVector);
+  static Eigen::Matrix<float, 3, 1> toVector3f(const cv::Mat &cvVector);
+  static Eigen::Matrix<double, 3, 1> toVector3d(const cv::Point3f &cvPoint);
+  static Eigen::Matrix<double, 3, 3> toMatrix3d(const cv::Mat &cvMat3);
+  static Eigen::Matrix<double, 4, 4> toMatrix4d(const cv::Mat &cvMat4);
+  static Eigen::Matrix<float, 3, 3> toMatrix3f(const cv::Mat &cvMat3);
+  static Eigen::Matrix<float, 4, 4> toMatrix4f(const cv::Mat &cvMat4);
+  static std::vector<float> toQuaternion(const cv::Mat &M);
+
+  static bool isRotationMatrix(const cv::Mat &R);
+  static std::vector<float> toEuler(const cv::Mat &R);
+
+  // TODO: Sophus migration, to be deleted in the future
+  static Sophus::SE3<float> toSophus(const cv::Mat &T);
+  static Sophus::Sim3f toSophus(const g2o::Sim3 &S);
+};
+
+}  // namespace MORB_SLAM
diff --git a/include/MORB_SLAM/Frame.h b/include/MORB_SLAM/Frame.h
new file mode 100644
index 00000000..bcc5127e
--- /dev/null
+++ b/include/MORB_SLAM/Frame.h
@@ -0,0 +1,376 @@
+/**
+* This file is part of ORB-SLAM3
+*
+* Copyright (C) 2017-2021 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/>.
+*/
+
+
+#pragma once
+
+#include<vector>
+
+#include "DBoW2/BowVector.h"
+#include "DBoW2/FeatureVector.h"
+
+#include "sophus/geometry.hpp"
+
+#include "ImuTypes.h"
+#include "ORBVocabulary.h"
+
+#include "Converter.h"
+#include "Settings.h"
+
+#include <mutex>
+#include <opencv2/opencv.hpp>
+
+#include "Eigen/Core"
+#include "sophus/se3.hpp"
+
+#include "MORB_SLAM/Camera.hpp"
+
+namespace MORB_SLAM
+{
+#define FRAME_GRID_ROWS 48
+#define FRAME_GRID_COLS 64
+
+class MapPoint;
+class KeyFrame;
+class ConstraintPoseImu;
+class GeometricCamera;
+class ORBextractor;
+
+class Frame
+{
+public:
+    Frame();
+
+    // Copy constructor.
+    Frame(const Frame &frame);
+
+    // Constructor for stereo cameras.
+    Frame(const Camera_ptr &cam, const cv::Mat &imLeft, const cv::Mat &imRight, const double &timeStamp, const std::shared_ptr<ORBextractor> &extractorLeft, const std::shared_ptr<ORBextractor> &extractorRight, ORBVocabulary* voc, cv::Mat &K, cv::Mat &distCoef, const float &bf, const float &thDepth, const std::shared_ptr<GeometricCamera> &pCamera, const std::string &pNameFile, int pnNumDataset, Frame* pPrevF = nullptr, const IMU::Calib &ImuCalib = IMU::Calib());
+
+    // Constructor for RGB-D cameras.
+    Frame(const Camera_ptr &cam, const cv::Mat &imGray, const cv::Mat &imDepth, const double &timeStamp, const std::shared_ptr<ORBextractor> &extractor,ORBVocabulary* voc, cv::Mat &K, cv::Mat &distCoef, const float &bf, const float &thDepth, const std::shared_ptr<GeometricCamera> &pCamera, const std::string &pNameFile, int pnNumDataset, Frame* pPrevF = nullptr, const IMU::Calib &ImuCalib = IMU::Calib());
+
+    // Constructor for Monocular cameras.
+    Frame(const Camera_ptr &cam, const cv::Mat &imGray, const double &timeStamp, const std::shared_ptr<ORBextractor> &extractor,ORBVocabulary* voc, const std::shared_ptr<GeometricCamera> &pCamera, cv::Mat &distCoef, const float &bf, const float &thDepth, const std::string &pNameFile, int pnNumDataset,  Frame* pPrevF = nullptr, const IMU::Calib &ImuCalib = IMU::Calib());
+
+    // Destructor
+    virtual ~Frame();
+
+    // Extract ORB on the image. 0 for left image and 1 for right image.
+    void ExtractORB(bool isLeft, const cv::Mat &im, const int x0, const int x1);
+
+    // Compute Bag of Words representation.
+    void ComputeBoW();
+
+    // Set the camera pose. (Imu pose is not modified!)
+    void SetPose(const Sophus::SE3<float> &Tcw);
+
+    // Set IMU velocity
+    void SetVelocity(Eigen::Vector3f Vw);
+
+    Eigen::Vector3f GetVelocity() const;
+
+    // Set IMU pose and velocity (implicitly changes camera pose)
+    void SetImuPoseVelocity(const Eigen::Matrix3f &Rwb, const Eigen::Vector3f &twb, const Eigen::Vector3f &Vwb);
+
+    Eigen::Matrix<float,3,1> GetImuPosition() const;
+    Eigen::Matrix<float,3,3> GetImuRotation();
+    Sophus::SE3<float> GetImuPose();
+
+    Sophus::SE3f GetRelativePoseTrl();
+    Sophus::SE3f GetRelativePoseTlr();
+    Eigen::Matrix3f GetRelativePoseTlr_rotation();
+    Eigen::Vector3f GetRelativePoseTlr_translation();
+
+    void SetNewBias(const IMU::Bias &b);
+
+    // Check if a MapPoint is in the frustum of the camera
+    // and fill variables of the MapPoint to be used by the tracking
+    bool isInFrustum(MapPoint* pMP, float viewingCosLimit);
+
+    bool ProjectPointDistort(MapPoint* pMP, cv::Point2f &kp, float &u, float &v);
+
+    Eigen::Vector3f inRefCoordinates(Eigen::Vector3f pCw);
+
+    // Compute the cell of a keypoint (return false if outside the grid)
+    bool PosInGrid(const cv::KeyPoint &kp, int &posX, int &posY);
+
+    vector<size_t> GetFeaturesInArea(const float &x, const float  &y, const float  &r, const int minLevel=-1, const int maxLevel=-1, const bool bRight = false) const;
+
+    // Search a match for each keypoint in the left image to a keypoint in the right image.
+    // If there is a match, depth is computed and the right coordinate associated to the left keypoint is stored.
+    void ComputeStereoMatches();
+
+    // Associate a "right" coordinate to a keypoint if there is valid depth in the depthmap.
+    void ComputeStereoFromRGBD(const cv::Mat &imDepth);
+
+    // Backprojects a keypoint (if stereo/depth info available) into 3D world coordinates.
+    bool UnprojectStereo(const int &i, Eigen::Vector3f &x3D);
+
+    std::shared_ptr<ConstraintPoseImu> mpcpi;
+
+    bool imuIsPreintegrated();
+    void setIntegrated();
+
+    bool isSet() const;
+
+    // Computes rotation, translation and camera center matrices from the camera pose.
+    void UpdatePoseMatrices();
+
+    // Returns the camera center.
+    inline Eigen::Vector3f GetCameraCenter(){
+        return mOw;
+    }
+
+    // Returns inverse of rotation
+    inline Eigen::Matrix3f GetRotationInverse(){
+        return mRwc;
+    }
+
+    inline Sophus::SE3<float> GetPose() const {
+        //TODO: can the Frame pose be accsessed from several threads? should this be protected somehow?
+        return mTcw;
+    }
+
+    inline Eigen::Matrix3f GetRwc() const {
+        return mRwc;
+    }
+
+    inline Eigen::Vector3f GetOw() const {
+        return mOw;
+    }
+
+    inline bool HasPose() const {
+        return mbHasPose;
+    }
+
+    inline bool HasVelocity() const {
+        return mbHasVelocity;
+    }
+
+
+
+private:
+    //Sophus/Eigen migration
+    Sophus::SE3<float> mTcw;
+    Eigen::Matrix<float,3,3> mRwc;
+    Eigen::Matrix<float,3,1> mOw;
+    Eigen::Matrix<float,3,3> mRcw;
+    Eigen::Matrix<float,3,1> mtcw;
+    bool mbHasPose;
+
+    //Rcw_ not necessary as Sophus has a method for extracting the rotation matrix: Tcw_.rotationMatrix()
+    //tcw_ not necessary as Sophus has a method for extracting the translation vector: Tcw_.translation()
+    //Twc_ not necessary as Sophus has a method for easily computing the inverse pose: Tcw_.inverse()
+
+    Sophus::SE3<float> mTlr, mTrl;
+    Eigen::Matrix<float,3,3> mRlr;
+    Eigen::Vector3f mtlr;
+
+
+    // IMU linear velocity
+    Eigen::Vector3f mVw;
+    bool mbHasVelocity;
+
+public:
+    
+
+    // Vocabulary used for relocalization.
+    ORBVocabulary* mpORBvocabulary;
+
+    // Feature extractor. The right is used only in the stereo case.
+    std::shared_ptr<ORBextractor> mpORBextractorLeft;
+    std::shared_ptr<ORBextractor> mpORBextractorRight;
+
+
+    // Frame timestamp.
+    double mTimeStamp;
+
+    // Calibration matrix and OpenCV distortion parameters.
+    cv::Mat mK;
+    Eigen::Matrix3f mK_;
+    static float fx;
+    static float fy;
+    static float cx;
+    static float cy;
+    static float invfx;
+    static float invfy;
+    cv::Mat mDistCoef;
+
+    // Stereo baseline multiplied by fx.
+    float mbf;
+
+    // Stereo baseline in meters.
+    float mb;
+
+    // Threshold close/far points. Close points are inserted from 1 view.
+    // Far points are inserted as in the monocular case from 2 views.
+    float mThDepth;
+
+    // Number of KeyPoints.
+    int N;
+
+    // Vector of keypoints (original for visualization) and undistorted (actually used by the system).
+    // In the stereo case, mvKeysUn is redundant as images must be rectified.
+    // In the RGB-D case, RGB images can be distorted.
+    std::vector<cv::KeyPoint> mvKeys, mvKeysRight;
+    std::vector<cv::KeyPoint> mvKeysUn;
+
+    // Corresponding stereo coordinate and depth for each keypoint.
+    std::vector<MapPoint*> mvpMapPoints;
+    // "Monocular" keypoints have a negative value.
+    std::vector<float> mvuRight;
+    std::vector<float> mvDepth;
+
+    // Bag of Words Vector structures.
+    DBoW2::BowVector mBowVec;
+    DBoW2::FeatureVector mFeatVec;
+
+    // ORB descriptor, each row associated to a keypoint.
+    cv::Mat mDescriptors, mDescriptorsRight;
+
+    // MapPoints associated to keypoints, nullptr pointer if no association.
+    // Flag to identify outlier associations.
+    std::vector<bool> mvbOutlier;
+    int mnCloseMPs;
+
+    // Keypoints are assigned to cells in a grid to reduce matching complexity when projecting MapPoints.
+    static float mfGridElementWidthInv;
+    static float mfGridElementHeightInv;
+    std::vector<std::size_t> mGrid[FRAME_GRID_COLS][FRAME_GRID_ROWS];
+
+    IMU::Bias mPredBias;
+
+    // IMU bias
+    IMU::Bias mImuBias;
+
+    // Imu calibration
+    IMU::Calib mImuCalib;
+
+    // Imu preintegration from last keyframe
+    IMU::Preintegrated* mpImuPreintegrated;
+    KeyFrame* mpLastKeyFrame;
+
+    // Pointer to previous frame
+    Frame* mpPrevFrame;
+    IMU::Preintegrated* mpImuPreintegratedFrame;
+
+    // Current and Next Frame id.
+    static long unsigned int nNextId;
+    long unsigned int mnId;
+
+    // Reference Keyframe.
+    KeyFrame* mpReferenceKF;
+
+    // Scale pyramid info.
+    int mnScaleLevels;
+    float mfScaleFactor;
+    float mfLogScaleFactor;
+    vector<float> mvScaleFactors;
+    vector<float> mvInvScaleFactors;
+    vector<float> mvLevelSigma2;
+    vector<float> mvInvLevelSigma2;
+
+    // Undistorted Image Bounds (computed once).
+    static float mnMinX;
+    static float mnMaxX;
+    static float mnMinY;
+    static float mnMaxY;
+
+    static bool mbInitialComputations;
+
+    map<long unsigned int, cv::Point2f> mmProjectPoints;
+    map<long unsigned int, cv::Point2f> mmMatchedInImage;
+
+    string mNameFile;
+
+    int mnDataset;
+
+#ifdef REGISTER_TIMES
+    double mTimeORB_Ext;
+    double mTimeStereoMatch;
+#endif
+
+private:
+
+    // Undistort keypoints given OpenCV distortion parameters.
+    // Only for the RGB-D case. Stereo must be already rectified!
+    // (called in the constructor).
+    void UndistortKeyPoints();
+
+    // Computes image bounds for the undistorted image (called in the constructor).
+    void ComputeImageBounds(const cv::Mat &imLeft);
+
+    // Assign keypoints to the grid for speed up feature matching (called in the constructor).
+    void AssignFeaturesToGrid();
+
+    bool mbIsSet;
+
+    bool mbImuPreintegrated;
+
+    std::shared_ptr<std::mutex> mpMutexImu;
+
+public:
+    Camera_ptr camera; 
+    std::shared_ptr<GeometricCamera> mpCamera, mpCamera2;
+
+    //Number of KeyPoints extracted in the left and right images
+    int Nleft, Nright;
+    //Number of Non Lapping Keypoints
+    int monoLeft, monoRight;
+
+    //For stereo matching
+    std::vector<int> mvLeftToRightMatch, mvRightToLeftMatch;
+
+    //For stereo fisheye matching
+    static cv::BFMatcher BFmatcher;
+
+    //Triangulated stereo observations using as reference the left camera. These are
+    //computed during ComputeStereoFishEyeMatches
+    std::vector<Eigen::Vector3f> mvStereo3Dpoints;
+
+    //Grid for the right image
+    std::vector<std::size_t> mGridRight[FRAME_GRID_COLS][FRAME_GRID_ROWS];
+
+    Frame(const Camera_ptr &cam, const cv::Mat &imLeft, const cv::Mat &imRight, const double &timeStamp, const std::shared_ptr<ORBextractor> &extractorLeft, const std::shared_ptr<ORBextractor> &extractorRight, ORBVocabulary* voc, cv::Mat &K, cv::Mat &distCoef, const float &bf, const float &thDepth, const std::shared_ptr<GeometricCamera> &pCamera, const std::shared_ptr<GeometricCamera> &pCamera2, const std::string &pNameFile, int pnNumDataset, Sophus::SE3f& Tlr,Frame* pPrevF = nullptr, const IMU::Calib &ImuCalib = IMU::Calib());
+
+    //Stereo fisheye
+    void ComputeStereoFishEyeMatches();
+
+    bool isInFrustumChecks(MapPoint* pMP, float viewingCosLimit, bool bRight = false);
+
+    Eigen::Vector3f UnprojectStereoFishEye(const int &i);
+
+    cv::Mat imgLeft, imgRight;
+
+    void PrintPointDistribution(){
+        int left = 0, right = 0;
+        int Nlim = (Nleft != -1) ? Nleft : N;
+        for(int i = 0; i < N; i++){
+            if(mvpMapPoints[i] && !mvbOutlier[i]){
+                if(i < Nlim) left++;
+                else right++;
+            }
+        }
+        cout << "Point distribution in Frame: left-> " << left << " --- right-> " << right << endl;
+    }
+
+    Sophus::SE3<double> T_test;
+};
+
+}// namespace ORB_SLAM
diff --git a/include/MORB_SLAM/FrameDrawer.h b/include/MORB_SLAM/FrameDrawer.h
new file mode 100644
index 00000000..f420c5ac
--- /dev/null
+++ b/include/MORB_SLAM/FrameDrawer.h
@@ -0,0 +1,71 @@
+/**
+ * This file is part of ORB-SLAM3
+ *
+ * Copyright (C) 2017-2021 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/>.
+ */
+
+#pragma once
+
+#include <mutex>
+#include <opencv2/opencv.hpp>
+#include <vector>
+#include "MORB_SLAM/ImprovedTypes.hpp"
+
+namespace MORB_SLAM {
+
+class MapPoint;
+class Viewer;
+
+class FrameDrawer {
+ public:
+  
+  FrameDrawer(const Atlas_ptr &pAtlas);
+
+  // Update info from the last processed frame.
+  void Update(const Tracking_ptr &pTracker);
+
+  // Draw last processed frame.
+  cv::Mat DrawFrame(float imageScale = 1.f);
+  cv::Mat DrawRightFrame(float imageScale = 1.f);
+
+  friend Viewer;
+
+ protected:
+  bool both;
+  void DrawTextInfo(cv::Mat &im, int nState, cv::Mat &imText);
+
+  // Info of the frame to be drawn
+  cv::Mat mIm, mImRight;
+  int N;
+  std::vector<cv::KeyPoint> mvCurrentKeys, mvCurrentKeysRight;
+  std::vector<bool> mvbMap, mvbVO;
+  bool mbOnlyTracking;
+  int mnTracked, mnTrackedVO;
+  std::vector<cv::KeyPoint> mvIniKeys;
+  std::vector<int> mvIniMatches;
+  int mState;
+  std::vector<float> mvCurrentDepth;
+  float mThDepth;
+
+  Atlas_ptr mpAtlas;
+
+  std::mutex mMutex;
+  std::vector<std::pair<cv::Point2f, cv::Point2f>> mvTracks;
+};
+
+}  // namespace MORB_SLAM
diff --git a/include/MORB_SLAM/G2oTypes.h b/include/MORB_SLAM/G2oTypes.h
new file mode 100644
index 00000000..36633587
--- /dev/null
+++ b/include/MORB_SLAM/G2oTypes.h
@@ -0,0 +1,844 @@
+/**
+* This file is part of ORB-SLAM3
+*
+* Copyright (C) 2017-2021 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/>.
+*/
+
+#pragma once
+
+#include "g2o/core/base_vertex.h"
+#include "g2o/core/base_binary_edge.h"
+#include "g2o/types/types_sba.h"
+#include "g2o/core/base_multi_edge.h"
+#include "g2o/core/base_unary_edge.h"
+
+#include<opencv2/core/core.hpp>
+
+#include <Eigen/Core>
+#include <Eigen/Geometry>
+#include <Eigen/Dense>
+
+#include <MORB_SLAM/Frame.h>
+#include <MORB_SLAM/KeyFrame.h>
+
+#include"MORB_SLAM/Converter.h"
+#include <math.h>
+
+namespace MORB_SLAM
+{
+
+class KeyFrame;
+class Frame;
+class GeometricCamera;
+
+typedef Eigen::Matrix<double, 6, 1> Vector6d;
+typedef Eigen::Matrix<double, 9, 1> Vector9d;
+typedef Eigen::Matrix<double, 12, 1> Vector12d;
+typedef Eigen::Matrix<double, 15, 1> Vector15d;
+typedef Eigen::Matrix<double, 12, 12> Matrix12d;
+typedef Eigen::Matrix<double, 15, 15> Matrix15d;
+typedef Eigen::Matrix<double, 9, 9> Matrix9d;
+
+Eigen::Matrix3d ExpSO3(const double x, const double y, const double z);
+Eigen::Matrix3d ExpSO3(const Eigen::Vector3d &w);
+
+Eigen::Vector3d LogSO3(const Eigen::Matrix3d &R);
+
+Eigen::Matrix3d InverseRightJacobianSO3(const Eigen::Vector3d &v);
+Eigen::Matrix3d RightJacobianSO3(const Eigen::Vector3d &v);
+Eigen::Matrix3d RightJacobianSO3(const double x, const double y, const double z);
+
+Eigen::Matrix3d Skew(const Eigen::Vector3d &w);
+Eigen::Matrix3d InverseRightJacobianSO3(const double x, const double y, const double z);
+
+template<typename T = double>
+Eigen::Matrix<T,3,3> NormalizeRotation(const Eigen::Matrix<T,3,3> &R) {
+    Eigen::JacobiSVD<Eigen::Matrix<T,3,3>> svd(R,Eigen::ComputeFullU | Eigen::ComputeFullV);
+    return svd.matrixU() * svd.matrixV().transpose();
+}
+
+
+class ImuCamPose
+{
+public:
+    
+    ImuCamPose(){}
+    ImuCamPose(KeyFrame* pKF);
+    ImuCamPose(Frame* pF);
+    ImuCamPose(Eigen::Matrix3d &_Rwc, Eigen::Vector3d &_twc, KeyFrame* pKF);
+
+    void 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);
+
+    void Update(const double *pu); // update in the imu reference
+    void UpdateW(const double *pu); // update in the world reference
+    Eigen::Vector2d Project(const Eigen::Vector3d &Xw, int cam_idx=0) const; // Mono
+    Eigen::Vector3d ProjectStereo(const Eigen::Vector3d &Xw, int cam_idx=0) const; // Stereo
+    bool isDepthPositive(const Eigen::Vector3d &Xw, int cam_idx=0) const;
+
+public:
+    // For IMU
+    Eigen::Matrix3d Rwb;
+    Eigen::Vector3d twb;
+
+    // For set of cameras
+    std::vector<Eigen::Matrix3d> Rcw;
+    std::vector<Eigen::Vector3d> tcw;
+    std::vector<Eigen::Matrix3d> Rcb, Rbc;
+    std::vector<Eigen::Vector3d> tcb, tbc;
+    double bf;
+    std::vector<std::shared_ptr<GeometricCamera>> pCamera;
+
+    // For posegraph 4DoF
+    Eigen::Matrix3d Rwb0;
+    Eigen::Matrix3d DR;
+
+    int its;
+};
+
+class InvDepthPoint
+{
+public:
+    
+    InvDepthPoint(){}
+    InvDepthPoint(double _rho, double _u, double _v, KeyFrame* pHostKF);
+
+    void Update(const double *pu);
+
+    double rho;
+    double u, v; // they are not variables, observation in the host frame
+
+    double fx, fy, cx, cy, bf; // from host frame
+
+    int its;
+};
+
+// Optimizable parameters are IMU pose
+class VertexPose : public g2o::BaseVertex<6,ImuCamPose>
+{
+public:
+    
+    VertexPose(){}
+    VertexPose(KeyFrame* pKF){
+        setEstimate(ImuCamPose(pKF));
+    }
+    VertexPose(Frame* pF){
+        setEstimate(ImuCamPose(pF));
+    }
+
+
+    virtual bool read(std::istream& is);
+    virtual bool write(std::ostream& os) const;
+
+    virtual void setToOriginImpl() {
+        }
+
+    virtual void oplusImpl(const double* update_){
+        _estimate.Update(update_);
+        updateCache();
+    }
+};
+
+class VertexPose4DoF : public g2o::BaseVertex<4,ImuCamPose>
+{
+    // Translation and yaw are the only optimizable variables
+public:
+    
+    VertexPose4DoF(){}
+    VertexPose4DoF(KeyFrame* pKF){
+        setEstimate(ImuCamPose(pKF));
+    }
+    VertexPose4DoF(Frame* pF){
+        setEstimate(ImuCamPose(pF));
+    }
+    VertexPose4DoF(Eigen::Matrix3d &_Rwc, Eigen::Vector3d &_twc, KeyFrame* pKF){
+
+        setEstimate(ImuCamPose(_Rwc, _twc, pKF));
+    }
+
+    virtual bool read(std::istream& is){return false;}
+    virtual bool write(std::ostream& os) const{return false;}
+
+    virtual void setToOriginImpl() {
+        }
+
+    virtual void oplusImpl(const double* update_){
+        double update6DoF[6];
+        update6DoF[0] = 0;
+        update6DoF[1] = 0;
+        update6DoF[2] = update_[0];
+        update6DoF[3] = update_[1];
+        update6DoF[4] = update_[2];
+        update6DoF[5] = update_[3];
+        _estimate.UpdateW(update6DoF);
+        updateCache();
+    }
+};
+
+class VertexVelocity : public g2o::BaseVertex<3,Eigen::Vector3d>
+{
+public:
+    
+    VertexVelocity(){}
+    VertexVelocity(KeyFrame* pKF);
+    VertexVelocity(Frame* pF);
+
+    virtual bool read(std::istream& is){return false;}
+    virtual bool write(std::ostream& os) const{return false;}
+
+    virtual void setToOriginImpl() {
+        }
+
+    virtual void oplusImpl(const double* update_){
+        Eigen::Vector3d uv;
+        uv << update_[0], update_[1], update_[2];
+        setEstimate(estimate()+uv);
+    }
+};
+
+class VertexGyroBias : public g2o::BaseVertex<3,Eigen::Vector3d>
+{
+public:
+    
+    VertexGyroBias(){}
+    VertexGyroBias(KeyFrame* pKF);
+    VertexGyroBias(Frame* pF);
+
+    virtual bool read(std::istream& is){return false;}
+    virtual bool write(std::ostream& os) const{return false;}
+
+    virtual void setToOriginImpl() {
+        }
+
+    virtual void oplusImpl(const double* update_){
+        Eigen::Vector3d ubg;
+        ubg << update_[0], update_[1], update_[2];
+        setEstimate(estimate()+ubg);
+    }
+};
+
+
+class VertexAccBias : public g2o::BaseVertex<3,Eigen::Vector3d>
+{
+public:
+    
+    VertexAccBias(){}
+    VertexAccBias(KeyFrame* pKF);
+    VertexAccBias(Frame* pF);
+
+    virtual bool read(std::istream& is){return false;}
+    virtual bool write(std::ostream& os) const{return false;}
+
+    virtual void setToOriginImpl() {
+        }
+
+    virtual void oplusImpl(const double* update_){
+        Eigen::Vector3d uba;
+        uba << update_[0], update_[1], update_[2];
+        setEstimate(estimate()+uba);
+    }
+};
+
+
+// Gravity direction vertex
+class GDirection
+{
+public:
+    
+    GDirection(){}
+    GDirection(Eigen::Matrix3d pRwg): Rwg(pRwg){}
+
+    void Update(const double *pu)
+    {
+        Rwg=Rwg*ExpSO3(pu[0],pu[1],0.0);
+    }
+
+    Eigen::Matrix3d Rwg, Rgw;
+
+    int its;
+};
+
+class VertexGDir : public g2o::BaseVertex<2,GDirection>
+{
+public:
+    
+    VertexGDir(){}
+    VertexGDir(Eigen::Matrix3d pRwg){
+        setEstimate(GDirection(pRwg));
+    }
+
+    virtual bool read(std::istream& is){return false;}
+    virtual bool write(std::ostream& os) const{return false;}
+
+    virtual void setToOriginImpl() {
+        }
+
+    virtual void oplusImpl(const double* update_){
+        _estimate.Update(update_);
+        updateCache();
+    }
+};
+
+// scale vertex
+class VertexScale : public g2o::BaseVertex<1,double>
+{
+public:
+    
+    VertexScale(){
+        setEstimate(1.0);
+    }
+    VertexScale(double ps){
+        setEstimate(ps);
+    }
+
+    virtual bool read(std::istream& is){return false;}
+    virtual bool write(std::ostream& os) const{return false;}
+
+    virtual void setToOriginImpl(){
+        setEstimate(1.0);
+    }
+
+    virtual void oplusImpl(const double *update_){
+        setEstimate(estimate()*exp(*update_));
+    }
+};
+
+
+// Inverse depth point (just one parameter, inverse depth at the host frame)
+class VertexInvDepth : public g2o::BaseVertex<1,InvDepthPoint>
+{
+public:
+    
+    VertexInvDepth(){}
+    VertexInvDepth(double invDepth, double u, double v, KeyFrame* pHostKF){
+        setEstimate(InvDepthPoint(invDepth, u, v, pHostKF));
+    }
+
+    virtual bool read(std::istream& is){return false;}
+    virtual bool write(std::ostream& os) const{return false;}
+
+    virtual void setToOriginImpl() {
+        }
+
+    virtual void oplusImpl(const double* update_){
+        _estimate.Update(update_);
+        updateCache();
+    }
+};
+
+class EdgeMono : public g2o::BaseBinaryEdge<2,Eigen::Vector2d,g2o::VertexSBAPointXYZ,VertexPose>
+{
+public:
+    
+
+    EdgeMono(int cam_idx_=0): cam_idx(cam_idx_){
+    }
+
+    virtual bool read(std::istream& is){return false;}
+    virtual bool write(std::ostream& os) const{return false;}
+
+    void computeError(){
+        const g2o::VertexSBAPointXYZ* VPoint = static_cast<const g2o::VertexSBAPointXYZ*>(_vertices[0]);
+        const VertexPose* VPose = static_cast<const VertexPose*>(_vertices[1]);
+        const Eigen::Vector2d obs(_measurement);
+        _error = obs - VPose->estimate().Project(VPoint->estimate(),cam_idx);
+    }
+
+
+    virtual void linearizeOplus();
+
+    bool isDepthPositive()
+    {
+        const g2o::VertexSBAPointXYZ* VPoint = static_cast<const g2o::VertexSBAPointXYZ*>(_vertices[0]);
+        const VertexPose* VPose = static_cast<const VertexPose*>(_vertices[1]);
+        return VPose->estimate().isDepthPositive(VPoint->estimate(),cam_idx);
+    }
+
+    Eigen::Matrix<double,2,9> GetJacobian(){
+        linearizeOplus();
+        Eigen::Matrix<double,2,9> J;
+        J.block<2,3>(0,0) = _jacobianOplusXi;
+        J.block<2,6>(0,3) = _jacobianOplusXj;
+        return J;
+    }
+
+    Eigen::Matrix<double,9,9> GetHessian(){
+        linearizeOplus();
+        Eigen::Matrix<double,2,9> J;
+        J.block<2,3>(0,0) = _jacobianOplusXi;
+        J.block<2,6>(0,3) = _jacobianOplusXj;
+        return J.transpose()*information()*J;
+    }
+
+public:
+    const int cam_idx;
+};
+
+class EdgeMonoOnlyPose : public g2o::BaseUnaryEdge<2,Eigen::Vector2d,VertexPose>
+{
+public:
+    
+
+    EdgeMonoOnlyPose(const Eigen::Vector3f &Xw_, int cam_idx_=0):Xw(Xw_.cast<double>()),
+        cam_idx(cam_idx_){}
+
+    virtual bool read(std::istream& is){return false;}
+    virtual bool write(std::ostream& os) const{return false;}
+
+    void computeError(){
+        const VertexPose* VPose = static_cast<const VertexPose*>(_vertices[0]);
+        const Eigen::Vector2d obs(_measurement);
+        _error = obs - VPose->estimate().Project(Xw,cam_idx);
+    }
+
+    virtual void linearizeOplus();
+
+    bool isDepthPositive()
+    {
+        const VertexPose* VPose = static_cast<const VertexPose*>(_vertices[0]);
+        return VPose->estimate().isDepthPositive(Xw,cam_idx);
+    }
+
+    Eigen::Matrix<double,6,6> GetHessian(){
+        linearizeOplus();
+        return _jacobianOplusXi.transpose()*information()*_jacobianOplusXi;
+    }
+
+public:
+    const Eigen::Vector3d Xw;
+    const int cam_idx;
+};
+
+class EdgeStereo : public g2o::BaseBinaryEdge<3,Eigen::Vector3d,g2o::VertexSBAPointXYZ,VertexPose>
+{
+public:
+    
+
+    EdgeStereo(int cam_idx_=0): cam_idx(cam_idx_){}
+
+    virtual bool read(std::istream& is){return false;}
+    virtual bool write(std::ostream& os) const{return false;}
+
+    void computeError(){
+        const g2o::VertexSBAPointXYZ* VPoint = static_cast<const g2o::VertexSBAPointXYZ*>(_vertices[0]);
+        const VertexPose* VPose = static_cast<const VertexPose*>(_vertices[1]);
+        const Eigen::Vector3d obs(_measurement);
+        _error = obs - VPose->estimate().ProjectStereo(VPoint->estimate(),cam_idx);
+    }
+
+
+    virtual void linearizeOplus();
+
+    Eigen::Matrix<double,3,9> GetJacobian(){
+        linearizeOplus();
+        Eigen::Matrix<double,3,9> J;
+        J.block<3,3>(0,0) = _jacobianOplusXi;
+        J.block<3,6>(0,3) = _jacobianOplusXj;
+        return J;
+    }
+
+    Eigen::Matrix<double,9,9> GetHessian(){
+        linearizeOplus();
+        Eigen::Matrix<double,3,9> J;
+        J.block<3,3>(0,0) = _jacobianOplusXi;
+        J.block<3,6>(0,3) = _jacobianOplusXj;
+        return J.transpose()*information()*J;
+    }
+
+public:
+    const int cam_idx;
+};
+
+
+class EdgeStereoOnlyPose : public g2o::BaseUnaryEdge<3,Eigen::Vector3d,VertexPose>
+{
+public:
+    
+
+    EdgeStereoOnlyPose(const Eigen::Vector3f &Xw_, int cam_idx_=0):
+        Xw(Xw_.cast<double>()), cam_idx(cam_idx_){}
+
+    virtual bool read(std::istream& is){return false;}
+    virtual bool write(std::ostream& os) const{return false;}
+
+    void computeError(){
+        const VertexPose* VPose = static_cast<const VertexPose*>(_vertices[0]);
+        const Eigen::Vector3d obs(_measurement);
+        _error = obs - VPose->estimate().ProjectStereo(Xw, cam_idx);
+    }
+
+    virtual void linearizeOplus();
+
+    Eigen::Matrix<double,6,6> GetHessian(){
+        linearizeOplus();
+        return _jacobianOplusXi.transpose()*information()*_jacobianOplusXi;
+    }
+
+public:
+    const Eigen::Vector3d Xw; // 3D point coordinates
+    const int cam_idx;
+};
+
+class EdgeInertial : public g2o::BaseMultiEdge<9,Vector9d>
+{
+public:
+    
+
+    EdgeInertial(IMU::Preintegrated* pInt);
+
+    virtual bool read(std::istream& is){return false;}
+    virtual bool write(std::ostream& os) const{return false;}
+
+    void computeError();
+    virtual void linearizeOplus();
+
+    Eigen::Matrix<double,24,24> GetHessian(){
+        linearizeOplus();
+        Eigen::Matrix<double,9,24> J;
+        J.block<9,6>(0,0) = _jacobianOplus[0];
+        J.block<9,3>(0,6) = _jacobianOplus[1];
+        J.block<9,3>(0,9) = _jacobianOplus[2];
+        J.block<9,3>(0,12) = _jacobianOplus[3];
+        J.block<9,6>(0,15) = _jacobianOplus[4];
+        J.block<9,3>(0,21) = _jacobianOplus[5];
+        return J.transpose()*information()*J;
+    }
+
+    Eigen::Matrix<double,18,18> GetHessianNoPose1(){
+        linearizeOplus();
+        Eigen::Matrix<double,9,18> J;
+        J.block<9,3>(0,0) = _jacobianOplus[1];
+        J.block<9,3>(0,3) = _jacobianOplus[2];
+        J.block<9,3>(0,6) = _jacobianOplus[3];
+        J.block<9,6>(0,9) = _jacobianOplus[4];
+        J.block<9,3>(0,15) = _jacobianOplus[5];
+        return J.transpose()*information()*J;
+    }
+
+    Eigen::Matrix<double,9,9> GetHessian2(){
+        linearizeOplus();
+        Eigen::Matrix<double,9,9> J;
+        J.block<9,6>(0,0) = _jacobianOplus[4];
+        J.block<9,3>(0,6) = _jacobianOplus[5];
+        return J.transpose()*information()*J;
+    }
+
+    const Eigen::Matrix3d JRg, JVg, JPg;
+    const Eigen::Matrix3d JVa, JPa;
+    IMU::Preintegrated* mpInt;
+    const double dt;
+    Eigen::Vector3d g;
+};
+
+
+// Edge inertial whre gravity is included as optimizable variable and it is not supposed to be pointing in -z axis, as well as scale
+class EdgeInertialGS : public g2o::BaseMultiEdge<9,Vector9d>
+{
+public:
+    
+
+    // EdgeInertialGS(IMU::Preintegrated* pInt);
+    EdgeInertialGS(IMU::Preintegrated* pInt);
+
+    virtual bool read(std::istream& is){return false;}
+    virtual bool write(std::ostream& os) const{return false;}
+
+    void computeError();
+    virtual void linearizeOplus();
+
+    const Eigen::Matrix3d JRg, JVg, JPg;
+    const Eigen::Matrix3d JVa, JPa;
+    IMU::Preintegrated* mpInt;
+    const double dt;
+    Eigen::Vector3d g, gI;
+
+    Eigen::Matrix<double,27,27> GetHessian(){
+        linearizeOplus();
+        Eigen::Matrix<double,9,27> J;
+        J.block<9,6>(0,0) = _jacobianOplus[0];
+        J.block<9,3>(0,6) = _jacobianOplus[1];
+        J.block<9,3>(0,9) = _jacobianOplus[2];
+        J.block<9,3>(0,12) = _jacobianOplus[3];
+        J.block<9,6>(0,15) = _jacobianOplus[4];
+        J.block<9,3>(0,21) = _jacobianOplus[5];
+        J.block<9,2>(0,24) = _jacobianOplus[6];
+        J.block<9,1>(0,26) = _jacobianOplus[7];
+        return J.transpose()*information()*J;
+    }
+
+    Eigen::Matrix<double,27,27> GetHessian2(){
+        linearizeOplus();
+        Eigen::Matrix<double,9,27> J;
+        J.block<9,3>(0,0) = _jacobianOplus[2];
+        J.block<9,3>(0,3) = _jacobianOplus[3];
+        J.block<9,2>(0,6) = _jacobianOplus[6];
+        J.block<9,1>(0,8) = _jacobianOplus[7];
+        J.block<9,3>(0,9) = _jacobianOplus[1];
+        J.block<9,3>(0,12) = _jacobianOplus[5];
+        J.block<9,6>(0,15) = _jacobianOplus[0];
+        J.block<9,6>(0,21) = _jacobianOplus[4];
+        return J.transpose()*information()*J;
+    }
+
+    Eigen::Matrix<double,9,9> GetHessian3(){
+        linearizeOplus();
+        Eigen::Matrix<double,9,9> J;
+        J.block<9,3>(0,0) = _jacobianOplus[2];
+        J.block<9,3>(0,3) = _jacobianOplus[3];
+        J.block<9,2>(0,6) = _jacobianOplus[6];
+        J.block<9,1>(0,8) = _jacobianOplus[7];
+        return J.transpose()*information()*J;
+    }
+
+
+
+    Eigen::Matrix<double,1,1> GetHessianScale(){
+        linearizeOplus();
+        Eigen::Matrix<double,9,1> J = _jacobianOplus[7];
+        return J.transpose()*information()*J;
+    }
+
+    Eigen::Matrix<double,3,3> GetHessianBiasGyro(){
+        linearizeOplus();
+        Eigen::Matrix<double,9,3> J = _jacobianOplus[2];
+        return J.transpose()*information()*J;
+    }
+
+    Eigen::Matrix<double,3,3> GetHessianBiasAcc(){
+        linearizeOplus();
+        Eigen::Matrix<double,9,3> J = _jacobianOplus[3];
+        return J.transpose()*information()*J;
+    }
+
+    Eigen::Matrix<double,2,2> GetHessianGDir(){
+        linearizeOplus();
+        Eigen::Matrix<double,9,2> J = _jacobianOplus[6];
+        return J.transpose()*information()*J;
+    }
+};
+
+
+
+class EdgeGyroRW : public g2o::BaseBinaryEdge<3,Eigen::Vector3d,VertexGyroBias,VertexGyroBias>
+{
+public:
+    
+
+    EdgeGyroRW(){}
+
+    virtual bool read(std::istream& is){return false;}
+    virtual bool write(std::ostream& os) const{return false;}
+
+    void computeError(){
+        const VertexGyroBias* VG1= static_cast<const VertexGyroBias*>(_vertices[0]);
+        const VertexGyroBias* VG2= static_cast<const VertexGyroBias*>(_vertices[1]);
+        _error = VG2->estimate()-VG1->estimate();
+    }
+
+    virtual void linearizeOplus(){
+        _jacobianOplusXi = -Eigen::Matrix3d::Identity();
+        _jacobianOplusXj.setIdentity();
+    }
+
+    Eigen::Matrix<double,6,6> GetHessian(){
+        linearizeOplus();
+        Eigen::Matrix<double,3,6> J;
+        J.block<3,3>(0,0) = _jacobianOplusXi;
+        J.block<3,3>(0,3) = _jacobianOplusXj;
+        return J.transpose()*information()*J;
+    }
+
+    Eigen::Matrix3d GetHessian2(){
+        linearizeOplus();
+        return _jacobianOplusXj.transpose()*information()*_jacobianOplusXj;
+    }
+};
+
+
+class EdgeAccRW : public g2o::BaseBinaryEdge<3,Eigen::Vector3d,VertexAccBias,VertexAccBias>
+{
+public:
+    
+
+    EdgeAccRW(){}
+
+    virtual bool read(std::istream& is){return false;}
+    virtual bool write(std::ostream& os) const{return false;}
+
+    void computeError(){
+        const VertexAccBias* VA1= static_cast<const VertexAccBias*>(_vertices[0]);
+        const VertexAccBias* VA2= static_cast<const VertexAccBias*>(_vertices[1]);
+        _error = VA2->estimate()-VA1->estimate();
+    }
+
+    virtual void linearizeOplus(){
+        _jacobianOplusXi = -Eigen::Matrix3d::Identity();
+        _jacobianOplusXj.setIdentity();
+    }
+
+    Eigen::Matrix<double,6,6> GetHessian(){
+        linearizeOplus();
+        Eigen::Matrix<double,3,6> J;
+        J.block<3,3>(0,0) = _jacobianOplusXi;
+        J.block<3,3>(0,3) = _jacobianOplusXj;
+        return J.transpose()*information()*J;
+    }
+
+    Eigen::Matrix3d GetHessian2(){
+        linearizeOplus();
+        return _jacobianOplusXj.transpose()*information()*_jacobianOplusXj;
+    }
+};
+
+class ConstraintPoseImu
+{
+public:
+    
+
+    ConstraintPoseImu(const Eigen::Matrix3d &Rwb_, const Eigen::Vector3d &twb_, const Eigen::Vector3d &vwb_,
+                       const Eigen::Vector3d &bg_, const Eigen::Vector3d &ba_, const Matrix15d &H_):
+                       Rwb(Rwb_), twb(twb_), vwb(vwb_), bg(bg_), ba(ba_), H(H_)
+    {
+        Eigen::SelfAdjointEigenSolver<Eigen::Matrix<double,15,15> > es(H);
+        Eigen::Matrix<double,15,1> eigs = es.eigenvalues();
+        for(int i=0;i<15;i++)
+            if(eigs[i]<1e-12)
+                eigs[i]=0;
+        H = es.eigenvectors()*eigs.asDiagonal()*es.eigenvectors().transpose();
+    }
+
+    Eigen::Matrix3d Rwb;
+    Eigen::Vector3d twb;
+    Eigen::Vector3d vwb;
+    Eigen::Vector3d bg;
+    Eigen::Vector3d ba;
+    Matrix15d H;
+};
+
+class EdgePriorPoseImu : public g2o::BaseMultiEdge<15,Vector15d>
+{
+public:
+        
+        EdgePriorPoseImu(std::shared_ptr<ConstraintPoseImu> c);
+
+        virtual bool read(std::istream& is){return false;}
+        virtual bool write(std::ostream& os) const{return false;}
+
+        void computeError();
+        virtual void linearizeOplus();
+
+        Eigen::Matrix<double,15,15> GetHessian(){
+            linearizeOplus();
+            Eigen::Matrix<double,15,15> J;
+            J.block<15,6>(0,0) = _jacobianOplus[0];
+            J.block<15,3>(0,6) = _jacobianOplus[1];
+            J.block<15,3>(0,9) = _jacobianOplus[2];
+            J.block<15,3>(0,12) = _jacobianOplus[3];
+            return J.transpose()*information()*J;
+        }
+
+        Eigen::Matrix<double,9,9> GetHessianNoPose(){
+            linearizeOplus();
+            Eigen::Matrix<double,15,9> J;
+            J.block<15,3>(0,0) = _jacobianOplus[1];
+            J.block<15,3>(0,3) = _jacobianOplus[2];
+            J.block<15,3>(0,6) = _jacobianOplus[3];
+            return J.transpose()*information()*J;
+        }
+        Eigen::Matrix3d Rwb;
+        Eigen::Vector3d twb, vwb;
+        Eigen::Vector3d bg, ba;
+};
+
+// Priors for biases
+class EdgePriorAcc : public g2o::BaseUnaryEdge<3,Eigen::Vector3d,VertexAccBias>
+{
+public:
+    
+
+    EdgePriorAcc(const Eigen::Vector3f &bprior_):bprior(bprior_.cast<double>()){}
+
+    virtual bool read(std::istream& is){return false;}
+    virtual bool write(std::ostream& os) const{return false;}
+
+    void computeError(){
+        const VertexAccBias* VA = static_cast<const VertexAccBias*>(_vertices[0]);
+        _error = bprior - VA->estimate();
+    }
+    virtual void linearizeOplus();
+
+    Eigen::Matrix<double,3,3> GetHessian(){
+        linearizeOplus();
+        return _jacobianOplusXi.transpose()*information()*_jacobianOplusXi;
+    }
+
+    const Eigen::Vector3d bprior;
+};
+
+class EdgePriorGyro : public g2o::BaseUnaryEdge<3,Eigen::Vector3d,VertexGyroBias>
+{
+public:
+    
+
+    EdgePriorGyro(const Eigen::Vector3f &bprior_):bprior(bprior_.cast<double>()){}
+
+    virtual bool read(std::istream& is){return false;}
+    virtual bool write(std::ostream& os) const{return false;}
+
+    void computeError(){
+        const VertexGyroBias* VG = static_cast<const VertexGyroBias*>(_vertices[0]);
+        _error = bprior - VG->estimate();
+    }
+    virtual void linearizeOplus();
+
+    Eigen::Matrix<double,3,3> GetHessian(){
+        linearizeOplus();
+        return _jacobianOplusXi.transpose()*information()*_jacobianOplusXi;
+    }
+
+    const Eigen::Vector3d bprior;
+};
+
+
+class Edge4DoF : public g2o::BaseBinaryEdge<6,Vector6d,VertexPose4DoF,VertexPose4DoF>
+{
+public:
+    
+
+    Edge4DoF(const Eigen::Matrix4d &deltaT){
+        dTij = deltaT;
+        dRij = deltaT.block<3,3>(0,0);
+        dtij = deltaT.block<3,1>(0,3);
+    }
+
+    virtual bool read(std::istream& is){return false;}
+    virtual bool write(std::ostream& os) const{return false;}
+
+    void computeError(){
+        const VertexPose4DoF* VPi = static_cast<const VertexPose4DoF*>(_vertices[0]);
+        const VertexPose4DoF* VPj = static_cast<const VertexPose4DoF*>(_vertices[1]);
+        _error << LogSO3(VPi->estimate().Rcw[0]*VPj->estimate().Rcw[0].transpose()*dRij.transpose()),
+                 VPi->estimate().Rcw[0]*(-VPj->estimate().Rcw[0].transpose()*VPj->estimate().tcw[0])+VPi->estimate().tcw[0] - dtij;
+    }
+
+    // virtual void linearizeOplus(); // numerical implementation
+
+    Eigen::Matrix4d dTij;
+    Eigen::Matrix3d dRij;
+    Eigen::Vector3d dtij;
+};
+
+} //namespace ORB_SLAM2
+
diff --git a/include/MORB_SLAM/GeometricTools.h b/include/MORB_SLAM/GeometricTools.h
new file mode 100644
index 00000000..c1d888d0
--- /dev/null
+++ b/include/MORB_SLAM/GeometricTools.h
@@ -0,0 +1,81 @@
+/**
+ * This file is part of ORB-SLAM3
+ *
+ * Copyright (C) 2017-2021 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/>.
+ */
+
+#pragma once
+
+#include <Eigen/Core>
+#include <opencv2/core/core.hpp>
+#include <sophus/se3.hpp>
+
+namespace MORB_SLAM {
+
+class KeyFrame;
+
+class GeometricTools {
+ public:
+  
+  // Compute the Fundamental matrix between KF1 and KF2
+  static Eigen::Matrix3f ComputeF12(KeyFrame *&pKF1, KeyFrame *&pKF2);
+
+  // Triangulate point with KF1 and KF2
+  static bool Triangulate(Eigen::Vector3f &x_c1, Eigen::Vector3f &x_c2,
+                          Eigen::Matrix<float, 3, 4> &Tc1w,
+                          Eigen::Matrix<float, 3, 4> &Tc2w,
+                          Eigen::Vector3f &x3D);
+
+  template <int rows, int cols>
+  static bool CheckMatrices(const cv::Mat &cvMat,
+                            const Eigen::Matrix<float, rows, cols> &eigMat) {
+    const float epsilon = 1e-3;
+    // std::cout << cvMat.cols - cols << cvMat.rows - rows << std::endl;
+    if (rows != cvMat.rows || cols != cvMat.cols) {
+      std::cout << "wrong cvmat size\n";
+      return false;
+    }
+    for (int i = 0; i < rows; i++)
+      for (int j = 0; j < cols; j++)
+        if ((cvMat.at<float>(i, j) > (eigMat(i, j) + epsilon)) ||
+            (cvMat.at<float>(i, j) < (eigMat(i, j) - epsilon))) {
+          std::cout << "cv mat:\n" << cvMat << std::endl;
+          std::cout << "eig mat:\n" << eigMat << std::endl;
+          return false;
+        }
+    return true;
+  }
+
+  template <typename T, int rows, int cols>
+  static bool CheckMatrices(const Eigen::Matrix<T, rows, cols> &eigMat1,
+                            const Eigen::Matrix<T, rows, cols> &eigMat2) {
+    const float epsilon = 1e-3;
+    for (int i = 0; i < rows; i++)
+      for (int j = 0; j < cols; j++)
+        if ((eigMat1(i, j) > (eigMat2(i, j) + epsilon)) ||
+            (eigMat1(i, j) < (eigMat2(i, j) - epsilon))) {
+          std::cout << "eig mat 1:\n" << eigMat1 << std::endl;
+          std::cout << "eig mat 2:\n" << eigMat2 << std::endl;
+          return false;
+        }
+    return true;
+  }
+};
+
+}  // namespace MORB_SLAM
+
diff --git a/include/MORB_SLAM/ImprovedTypes.hpp b/include/MORB_SLAM/ImprovedTypes.hpp
new file mode 100644
index 00000000..a66a18a9
--- /dev/null
+++ b/include/MORB_SLAM/ImprovedTypes.hpp
@@ -0,0 +1,51 @@
+#pragma once
+
+#include <memory>
+#include <utility>
+#include <unordered_map>
+
+namespace MORB_SLAM{
+
+class System;
+class Atlas;
+class Tracking;
+typedef std::shared_ptr<System> System_ptr;
+typedef std::weak_ptr<System> System_wptr;
+typedef std::shared_ptr<Atlas> Atlas_ptr;
+typedef std::weak_ptr<Atlas> Atlas_wptr;
+typedef std::shared_ptr<Tracking> Tracking_ptr;
+typedef std::weak_ptr<Tracking> Tracking_wptr;
+
+typedef std::pair<int, int> IntPair;
+template<typename KEY, typename VALUE> using umap = std::unordered_map<KEY,VALUE>;
+
+
+
+  namespace Tracker{
+    // Tracking states
+    enum eTrackingState {
+        SYSTEM_NOT_READY = -1,
+        NO_IMAGES_YET = 0,
+        NOT_INITIALIZED = 1,
+        OK = 2,
+        RECENTLY_LOST = 3,
+        LOST = 4,
+        OK_KLT = 5
+    };
+  }
+
+  namespace CameraType{
+    // Input sensor
+    enum eSensor{
+        MONOCULAR=0,
+        STEREO=1,
+        RGBD=2,
+        IMU_MONOCULAR=3,
+        IMU_STEREO=4,
+        IMU_RGBD=5,
+    };
+
+    inline bool isInertial(eSensor sensor) { return sensor == IMU_MONOCULAR || sensor == IMU_STEREO || sensor == IMU_RGBD; }
+    inline bool hasMulticam(eSensor sensor) { return sensor == STEREO || sensor == RGBD || sensor == IMU_STEREO || sensor == IMU_RGBD; }
+  }
+}
\ No newline at end of file
diff --git a/include/MORB_SLAM/ImuTypes.h b/include/MORB_SLAM/ImuTypes.h
new file mode 100644
index 00000000..6374a62c
--- /dev/null
+++ b/include/MORB_SLAM/ImuTypes.h
@@ -0,0 +1,272 @@
+/**
+ * This file is part of ORB-SLAM3
+ *
+ * Copyright (C) 2017-2021 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/>.
+ */
+
+#pragma once
+
+#include <Eigen/Core>
+#include <Eigen/Dense>
+#include <Eigen/Geometry>
+#include <boost/serialization/serialization.hpp>
+#include <boost/serialization/vector.hpp>
+#include <mutex>
+#include <opencv2/core/core.hpp>
+#include <sophus/se3.hpp>
+#include <utility>
+#include <vector>
+
+#include "MORB_SLAM/SerializationUtils.h"
+
+namespace MORB_SLAM {
+
+namespace IMU {
+
+const float GRAVITY_VALUE = 9.81;
+
+// IMU measurement (gyro, accelerometer and timestamp)
+class Point {
+ public:
+  Point(const float &acc_x, const float &acc_y, const float &acc_z,
+        const float &ang_vel_x, const float &ang_vel_y, const float &ang_vel_z,
+        const double &timestamp)
+      : a(acc_x, acc_y, acc_z),
+        w(ang_vel_x, ang_vel_y, ang_vel_z),
+        t(timestamp) {}
+  Point(const cv::Point3f Acc, const cv::Point3f Gyro, const double &timestamp)
+      : a(Acc.x, Acc.y, Acc.z), w(Gyro.x, Gyro.y, Gyro.z), t(timestamp) {}
+
+ public:
+  Eigen::Vector3f a;
+  Eigen::Vector3f w;
+  double t;
+  
+};
+
+// IMU biases (gyro and accelerometer)
+class Bias {
+  friend class boost::serialization::access;
+  template <class Archive>
+  void serialize(Archive &ar, const unsigned int version) {
+    ar &bax;
+    ar &bay;
+    ar &baz;
+
+    ar &bwx;
+    ar &bwy;
+    ar &bwz;
+  }
+
+ public:
+  Bias() : bax(0), bay(0), baz(0), bwx(0), bwy(0), bwz(0) {}
+  Bias(const float &b_acc_x, const float &b_acc_y, const float &b_acc_z,
+       const float &b_ang_vel_x, const float &b_ang_vel_y,
+       const float &b_ang_vel_z)
+      : bax(b_acc_x),
+        bay(b_acc_y),
+        baz(b_acc_z),
+        bwx(b_ang_vel_x),
+        bwy(b_ang_vel_y),
+        bwz(b_ang_vel_z) {}
+  void CopyFrom(Bias &b);
+  friend std::ostream &operator<<(std::ostream &out, const Bias &b);
+
+ public:
+  float bax, bay, baz;
+  float bwx, bwy, bwz;
+  
+};
+
+// IMU calibration (Tbc, Tcb, noise)
+class Calib {
+  friend class boost::serialization::access;
+  template <class Archive>
+  void serialize(Archive &ar, const unsigned int version) {
+    serializeSophusSE3(ar, mTcb, version);
+    serializeSophusSE3(ar, mTbc, version);
+
+    ar &boost::serialization::make_array(Cov.diagonal().data(),
+                                         Cov.diagonal().size());
+    ar &boost::serialization::make_array(CovWalk.diagonal().data(),
+                                         CovWalk.diagonal().size());
+
+    ar &mbIsSet;
+  }
+
+ public:
+  Calib(const Sophus::SE3<float> &Tbc, const float &ng, const float &na,
+        const float &ngw, const float &naw) {
+    Set(Tbc, ng, na, ngw, naw);
+  }
+
+  Calib(const Calib &calib);
+  Calib() { mbIsSet = false; }
+
+  // void Set(const cv::Mat &cvTbc, const float &ng, const float &na, const
+  // float &ngw, const float &naw);
+  void Set(const Sophus::SE3<float> &sophTbc, const float &ng, const float &na,
+           const float &ngw, const float &naw);
+
+ public:
+  // Sophus/Eigen implementation
+  Sophus::SE3<float> mTcb;
+  Sophus::SE3<float> mTbc;
+  Eigen::DiagonalMatrix<float, 6> Cov, CovWalk;
+  bool mbIsSet;
+};
+
+// Integration of 1 gyro measurement
+class IntegratedRotation {
+ public:
+  IntegratedRotation() {}
+  IntegratedRotation(const Eigen::Vector3f &angVel, const Bias &imuBias,
+                     const float &time);
+
+ public:
+  float deltaT;  // integration time
+  Eigen::Matrix3f deltaR;
+  Eigen::Matrix3f rightJ;  // right jacobian
+  
+};
+
+// Preintegration of Imu Measurements
+class Preintegrated {
+  friend class boost::serialization::access;
+  template <class Archive>
+  void serialize(Archive &ar, const unsigned int version) {
+    ar &dT;
+    ar &boost::serialization::make_array(C.data(), C.size());
+    ar &boost::serialization::make_array(Info.data(), Info.size());
+    ar &boost::serialization::make_array(Nga.diagonal().data(),
+                                         Nga.diagonal().size());
+    ar &boost::serialization::make_array(NgaWalk.diagonal().data(),
+                                         NgaWalk.diagonal().size());
+    ar &b;
+    ar &boost::serialization::make_array(dR.data(), dR.size());
+    ar &boost::serialization::make_array(dV.data(), dV.size());
+    ar &boost::serialization::make_array(dP.data(), dP.size());
+    ar &boost::serialization::make_array(JRg.data(), JRg.size());
+    ar &boost::serialization::make_array(JVg.data(), JVg.size());
+    ar &boost::serialization::make_array(JVa.data(), JVa.size());
+    ar &boost::serialization::make_array(JPg.data(), JPg.size());
+    ar &boost::serialization::make_array(JPa.data(), JPa.size());
+    ar &boost::serialization::make_array(avgA.data(), avgA.size());
+    ar &boost::serialization::make_array(avgW.data(), avgW.size());
+
+    ar &bu;
+    ar &boost::serialization::make_array(db.data(), db.size());
+    ar &mvMeasurements;
+  }
+
+ public:
+  
+  Preintegrated(const Bias &b_, const Calib &calib);
+  Preintegrated(Preintegrated *pImuPre);
+  Preintegrated() {}
+  ~Preintegrated() {}
+  void CopyFrom(Preintegrated *pImuPre);
+  void Initialize(const Bias &b_);
+  void IntegrateNewMeasurement(const Eigen::Vector3f &acceleration,
+                               const Eigen::Vector3f &angVel, const float &dt);
+  void Reintegrate();
+  void MergePrevious(Preintegrated *pPrev);
+  void SetNewBias(const Bias &bu_);
+  IMU::Bias GetDeltaBias(const Bias &b_);
+
+  Eigen::Matrix3f GetDeltaRotation(const Bias &b_);
+  Eigen::Vector3f GetDeltaVelocity(const Bias &b_);
+  Eigen::Vector3f GetDeltaPosition(const Bias &b_);
+
+  Eigen::Matrix3f GetUpdatedDeltaRotation();
+  Eigen::Vector3f GetUpdatedDeltaVelocity();
+  Eigen::Vector3f GetUpdatedDeltaPosition();
+
+  Eigen::Matrix3f GetOriginalDeltaRotation();
+  Eigen::Vector3f GetOriginalDeltaVelocity();
+  Eigen::Vector3f GetOriginalDeltaPosition();
+
+  Eigen::Matrix<float, 6, 1> GetDeltaBias();
+
+  Bias GetOriginalBias();
+  Bias GetUpdatedBias();
+
+  void printMeasurements() const {
+    std::cout << "pint meas:\n";
+    for (size_t i = 0; i < mvMeasurements.size(); i++) {
+      std::cout << "meas " << mvMeasurements[i].t << std::endl;
+    }
+    std::cout << "end pint meas:\n";
+  }
+
+ public:
+  float dT;
+  Eigen::Matrix<float, 15, 15> C;
+  Eigen::Matrix<float, 15, 15> Info;
+  Eigen::DiagonalMatrix<float, 6> Nga, NgaWalk;
+
+  // Values for the original bias (when integration was computed)
+  Bias b;
+  Eigen::Matrix3f dR;
+  Eigen::Vector3f dV, dP;
+  Eigen::Matrix3f JRg, JVg, JVa, JPg, JPa;
+  Eigen::Vector3f avgA, avgW;
+
+ private:
+  // Updated bias
+  Bias bu;
+  // Dif between original and updated bias
+  // This is used to compute the updated values of the preintegration
+  Eigen::Matrix<float, 6, 1> db;
+
+  struct integrable {
+    template <class Archive>
+    void serialize(Archive &ar, const unsigned int version) {
+      ar &boost::serialization::make_array(a.data(), a.size());
+      ar &boost::serialization::make_array(w.data(), w.size());
+      ar &t;
+    }
+
+    
+    integrable() {}
+    integrable(const Eigen::Vector3f &a_, const Eigen::Vector3f &w_,
+               const float &t_)
+        : a(a_), w(w_), t(t_) {}
+    Eigen::Vector3f a, w;
+    float t;
+  };
+
+  std::vector<integrable> mvMeasurements;
+
+  std::mutex mMutex;
+};
+
+// Lie Algebra Functions
+Eigen::Matrix3f RightJacobianSO3(const float &x, const float &y,
+                                 const float &z);
+Eigen::Matrix3f RightJacobianSO3(const Eigen::Vector3f &v);
+
+Eigen::Matrix3f InverseRightJacobianSO3(const float &x, const float &y,
+                                        const float &z);
+Eigen::Matrix3f InverseRightJacobianSO3(const Eigen::Vector3f &v);
+
+Eigen::Matrix3f NormalizeRotation(const Eigen::Matrix3f &R);
+
+}  // namespace IMU
+
+}  // namespace MORB_SLAM
diff --git a/include/MORB_SLAM/KeyFrame.h b/include/MORB_SLAM/KeyFrame.h
new file mode 100644
index 00000000..86436ad1
--- /dev/null
+++ b/include/MORB_SLAM/KeyFrame.h
@@ -0,0 +1,542 @@
+/**
+ * This file is part of ORB-SLAM3
+ *
+ * Copyright (C) 2017-2021 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/>.
+ */
+
+#pragma once
+
+#include <boost/serialization/base_object.hpp>
+#include <boost/serialization/map.hpp>
+#include <boost/serialization/vector.hpp>
+#include <mutex>
+
+#include "DBoW2/BowVector.h"
+#include "DBoW2/FeatureVector.h"
+#include "MORB_SLAM/Frame.h"
+#include "MORB_SLAM/CameraModels/GeometricCamera.h"
+#include "MORB_SLAM/ImuTypes.h"
+#include "MORB_SLAM/KeyFrameDatabase.h"
+#include "MORB_SLAM/MapPoint.h"
+#include "MORB_SLAM/ORBVocabulary.h"
+#include "MORB_SLAM/ORBextractor.h"
+#include "MORB_SLAM/SerializationUtils.h"
+
+namespace MORB_SLAM {
+
+class Map;
+class MapPoint;
+class Frame;
+class KeyFrameDatabase;
+
+class GeometricCamera;
+
+class KeyFrame {
+  friend class boost::serialization::access;
+
+  template <class Archive>
+  void serialize(Archive& ar, const unsigned int version) {
+    ar& mnId;
+    ar& const_cast<long unsigned int&>(mnFrameId);
+    ar& const_cast<double&>(mTimeStamp);
+    // Grid
+    ar& const_cast<int&>(mnGridCols);
+    ar& const_cast<int&>(mnGridRows);
+    ar& const_cast<float&>(mfGridElementWidthInv);
+    ar& const_cast<float&>(mfGridElementHeightInv);
+
+    // Variables of tracking
+    // ar & mnTrackReferenceForFrame;
+    // ar & mnFuseTargetForKF;
+    // Variables of local mapping
+    // ar & mnBALocalForKF;
+    // ar & mnBAFixedForKF;
+    // ar & mnNumberOfOpt;
+    // Variables used by KeyFrameDatabase
+    // ar & mnLoopQuery;
+    // ar & mnLoopWords;
+    // ar & mLoopScore;
+    // ar & mnRelocQuery;
+    // ar & mnRelocWords;
+    // ar & mRelocScore;
+    // ar & mnMergeQuery;
+    // ar & mnMergeWords;
+    // ar & mMergeScore;
+    // ar & mnPlaceRecognitionQuery;
+    // ar & mnPlaceRecognitionWords;
+    // ar & mPlaceRecognitionScore;
+    // ar & mbCurrentPlaceRecognition;
+    // Variables of loop closing
+    // serializeMatrix(ar,mTcwGBA,version);
+    // serializeMatrix(ar,mTcwBefGBA,version);
+    // serializeMatrix(ar,mVwbGBA,version);
+    // serializeMatrix(ar,mVwbBefGBA,version);
+    // ar & mBiasGBA;
+    // ar & mnBAGlobalForKF;
+    // Variables of Merging
+    // serializeMatrix(ar,mTcwMerge,version);
+    // serializeMatrix(ar,mTcwBefMerge,version);
+    // serializeMatrix(ar,mTwcBefMerge,version);
+    // serializeMatrix(ar,mVwbMerge,version);
+    // serializeMatrix(ar,mVwbBefMerge,version);
+    // ar & mBiasMerge;
+    // ar & mnMergeCorrectedForKF;
+    // ar & mnMergeForKF;
+    // ar & mfScaleMerge;
+    // ar & mnBALocalForMerge;
+
+    // Scale
+    ar& mfScale;
+    // Calibration parameters
+    ar& const_cast<float&>(fx);
+    ar& const_cast<float&>(fy);
+    ar& const_cast<float&>(invfx);
+    ar& const_cast<float&>(invfy);
+    ar& const_cast<float&>(cx);
+    ar& const_cast<float&>(cy);
+    ar& const_cast<float&>(mbf);
+    ar& const_cast<float&>(mb);
+    ar& const_cast<float&>(mThDepth);
+    serializeMatrix(ar, mDistCoef, version);
+    // Number of Keypoints
+    ar& const_cast<int&>(N);
+    // KeyPoints
+    serializeVectorKeyPoints<Archive>(ar, mvKeys, version);
+    serializeVectorKeyPoints<Archive>(ar, mvKeysUn, version);
+    ar& const_cast<vector<float>&>(mvuRight);
+    ar& const_cast<vector<float>&>(mvDepth);
+    serializeMatrix<Archive>(ar, mDescriptors, version);
+    // BOW
+    ar& mBowVec;
+    ar& mFeatVec;
+    // Pose relative to parent
+    serializeSophusSE3<Archive>(ar, mTcp, version);
+    // Scale
+    ar& const_cast<int&>(mnScaleLevels);
+    ar& const_cast<float&>(mfScaleFactor);
+    ar& const_cast<float&>(mfLogScaleFactor);
+    ar& const_cast<vector<float>&>(mvScaleFactors);
+    ar& const_cast<vector<float>&>(mvLevelSigma2);
+    ar& const_cast<vector<float>&>(mvInvLevelSigma2);
+    // Image bounds and calibration
+    ar& const_cast<int&>(mnMinX);
+    ar& const_cast<int&>(mnMinY);
+    ar& const_cast<int&>(mnMaxX);
+    ar& const_cast<int&>(mnMaxY);
+    ar& boost::serialization::make_array(mK_.data(), mK_.size());
+    // Pose
+    serializeSophusSE3<Archive>(ar, mTcw, version);
+    // MapPointsId associated to keypoints
+    ar& mvBackupMapPointsId;
+    // Grid
+    ar& mGrid;
+    // Connected KeyFrameWeight
+    ar& mBackupConnectedKeyFrameIdWeights;
+    // Spanning Tree and Loop Edges
+    ar& mbFirstConnection;
+    ar& mBackupParentId;
+    ar& mvBackupChildrensId;
+    ar& mvBackupLoopEdgesId;
+    ar& mvBackupMergeEdgesId;
+    // Bad flags
+    ar& mbNotErase;
+    ar& mbToBeErased;
+    ar& mbBad;
+
+    ar& mHalfBaseline;
+
+    ar& mnOriginMapId;
+
+    // Camera variables
+    ar& mnBackupIdCamera;
+    ar& mnBackupIdCamera2;
+
+    // Fisheye variables
+    ar& mvLeftToRightMatch;
+    ar& mvRightToLeftMatch;
+    ar& const_cast<int&>(NLeft);
+    ar& const_cast<int&>(NRight);
+    serializeSophusSE3<Archive>(ar, mTlr, version);
+    serializeVectorKeyPoints<Archive>(ar, mvKeysRight, version);
+    ar& mGridRight;
+
+    // Inertial variables
+    ar& mImuBias;
+    ar& mBackupImuPreintegrated;
+    ar& mImuCalib;
+    ar& mBackupPrevKFId;
+    ar& mBackupNextKFId;
+    ar& bImu;
+    ar& boost::serialization::make_array(mVw.data(), mVw.size());
+    ar& boost::serialization::make_array(mOwb.data(), mOwb.size());
+    ar& mbHasVelocity;
+  }
+
+ public:
+  
+  KeyFrame();
+  KeyFrame(Frame& F, std::shared_ptr<Map> pMap, KeyFrameDatabase* pKFDB);
+
+  // Pose functions
+  void SetPose(const Sophus::SE3f& Tcw);
+  void SetVelocity(const Eigen::Vector3f& Vw_);
+
+  Sophus::SE3f GetPose();
+
+  Sophus::SE3f GetPoseInverse();
+  Eigen::Vector3f GetCameraCenter();
+
+  Eigen::Vector3f GetImuPosition();
+  Eigen::Matrix3f GetImuRotation();
+  Sophus::SE3f GetImuPose();
+  Eigen::Matrix3f GetRotation();
+  Eigen::Vector3f GetTranslation();
+  Eigen::Vector3f GetVelocity();
+  bool isVelocitySet();
+
+  // Bag of Words Representation
+  void ComputeBoW();
+
+  // Covisibility graph functions
+  void AddConnection(KeyFrame* pKF, const int& weight);
+  void EraseConnection(KeyFrame* pKF);
+
+  void UpdateConnections(bool upParent = true);
+  void UpdateBestCovisibles();
+  std::set<KeyFrame*> GetConnectedKeyFrames();
+  std::vector<KeyFrame*> GetVectorCovisibleKeyFrames();
+  std::vector<KeyFrame*> GetBestCovisibilityKeyFrames(const int& N);
+  std::vector<KeyFrame*> GetCovisiblesByWeight(const int& w);
+  int GetWeight(KeyFrame* pKF);
+
+  // Spanning tree functions
+  void AddChild(KeyFrame* pKF);
+  void EraseChild(KeyFrame* pKF);
+  void ChangeParent(KeyFrame* pKF);
+  std::set<KeyFrame*> GetChilds();
+  KeyFrame* GetParent();
+  bool hasChild(KeyFrame* pKF);
+  void SetFirstConnection(bool bFirst);
+
+  // Loop Edges
+  void AddLoopEdge(KeyFrame* pKF);
+  std::set<KeyFrame*> GetLoopEdges();
+
+  // Merge Edges
+  void AddMergeEdge(KeyFrame* pKF);
+  set<KeyFrame*> GetMergeEdges();
+
+  // MapPoint observation functions
+  int GetNumberMPs();
+  void AddMapPoint(MapPoint* pMP, const size_t& idx);
+  void EraseMapPointMatch(const int& idx);
+  void EraseMapPointMatch(MapPoint* pMP);
+  void ReplaceMapPointMatch(const int& idx, MapPoint* pMP);
+  std::set<MapPoint*> GetMapPoints();
+  std::vector<MapPoint*> GetMapPointMatches();
+  int TrackedMapPoints(const int& minObs);
+  MapPoint* GetMapPoint(const size_t& idx);
+
+  // KeyPoint functions
+  std::vector<size_t> GetFeaturesInArea(const float& x, const float& y,
+                                        const float& r,
+                                        const bool bRight = false) const;
+  bool UnprojectStereo(int i, Eigen::Vector3f& x3D);
+
+  // Image
+  bool IsInImage(const float& x, const float& y) const;
+
+  // Enable/Disable bad flag changes
+  void SetNotErase();
+  void SetErase();
+
+  // Set/check bad flag
+  void SetBadFlag();
+  bool isBad();
+
+  // Compute Scene Depth (q=2 median). Used in monocular.
+  float ComputeSceneMedianDepth(const int q);
+
+  static bool weightComp(int a, int b) { return a > b; }
+
+  static bool lId(KeyFrame* pKF1, KeyFrame* pKF2) {
+    return pKF1->mnId < pKF2->mnId;
+  }
+
+  std::shared_ptr<Map> GetMap();
+  void UpdateMap(std::shared_ptr<Map> pMap);
+
+  void SetNewBias(const IMU::Bias& b);
+  Eigen::Vector3f GetGyroBias();
+
+  Eigen::Vector3f GetAccBias();
+
+  IMU::Bias GetImuBias();
+
+  bool ProjectPointDistort(MapPoint* pMP, cv::Point2f& kp, float& u, float& v);
+  bool ProjectPointUnDistort(MapPoint* pMP, cv::Point2f& kp, float& u,
+                             float& v);
+
+  void PreSave(set<KeyFrame*>& spKF, set<MapPoint*>& spMP,
+               set<shared_ptr<GeometricCamera>>& spCam);
+  void PostLoad(map<long unsigned int, KeyFrame*>& mpKFid,
+                map<long unsigned int, MapPoint*>& mpMPid,
+                map<unsigned int, std::shared_ptr<GeometricCamera>>& mpCamId);
+
+  void SetORBVocabulary(ORBVocabulary* pORBVoc);
+  void SetKeyFrameDatabase(KeyFrameDatabase* pKFDB);
+
+  bool bImu;
+
+  // The following variables are accesed from only 1 thread or never change (no
+  // mutex needed).
+ public:
+  static long unsigned int nNextId;
+  long unsigned int mnId;
+  const long unsigned int mnFrameId;
+
+  const double mTimeStamp;
+
+  // Grid (to speed up feature matching)
+  const int mnGridCols;
+  const int mnGridRows;
+  const float mfGridElementWidthInv;
+  const float mfGridElementHeightInv;
+
+  // Variables used by the tracking
+  long unsigned int mnTrackReferenceForFrame;
+  long unsigned int mnFuseTargetForKF;
+
+  // Variables used by the local mapping
+  long unsigned int mnBALocalForKF;
+  long unsigned int mnBAFixedForKF;
+
+  // Number of optimizations by BA(amount of iterations in BA)
+  long unsigned int mnNumberOfOpt;
+
+  // Variables used by the keyframe database
+  long unsigned int mnLoopQuery;
+  int mnLoopWords;
+  float mLoopScore;
+  long unsigned int mnRelocQuery;
+  int mnRelocWords;
+  float mRelocScore;
+  long unsigned int mnMergeQuery;
+  int mnMergeWords;
+  float mMergeScore;
+  long unsigned int mnPlaceRecognitionQuery;
+  int mnPlaceRecognitionWords;
+  float mPlaceRecognitionScore;
+
+  bool mbCurrentPlaceRecognition;
+
+  // Variables used by loop closing
+  Sophus::SE3f mTcwGBA;
+  Sophus::SE3f mTcwBefGBA;
+  Eigen::Vector3f mVwbGBA;
+  Eigen::Vector3f mVwbBefGBA;
+  IMU::Bias mBiasGBA;
+  long unsigned int mnBAGlobalForKF;
+
+  // Variables used by merging
+  Sophus::SE3f mTcwMerge;
+  Sophus::SE3f mTcwBefMerge;
+  Sophus::SE3f mTwcBefMerge;
+  Eigen::Vector3f mVwbMerge;
+  Eigen::Vector3f mVwbBefMerge;
+  IMU::Bias mBiasMerge;
+  long unsigned int mnMergeCorrectedForKF;
+  long unsigned int mnMergeForKF;
+  float mfScaleMerge;
+  long unsigned int mnBALocalForMerge;
+
+  float mfScale;
+
+  // Calibration parameters
+  const float fx, fy, cx, cy, invfx, invfy, mbf, mb, mThDepth;
+  cv::Mat mDistCoef;
+
+  // Number of KeyPoints
+  const int N;
+
+  // KeyPoints, stereo coordinate and descriptors (all associated by an index)
+  const std::vector<cv::KeyPoint> mvKeys;
+  const std::vector<cv::KeyPoint> mvKeysUn;
+  const std::vector<float> mvuRight;  // negative value for monocular points
+  const std::vector<float> mvDepth;   // negative value for monocular points
+  const cv::Mat mDescriptors;
+
+  // BoW
+  DBoW2::BowVector mBowVec;
+  DBoW2::FeatureVector mFeatVec;
+
+  // Pose relative to parent (this is computed when bad flag is activated)
+  Sophus::SE3f mTcp;
+
+  // Scale
+  const int mnScaleLevels;
+  const float mfScaleFactor;
+  const float mfLogScaleFactor;
+  const std::vector<float> mvScaleFactors;
+  const std::vector<float> mvLevelSigma2;
+  const std::vector<float> mvInvLevelSigma2;
+
+  // Image bounds and calibration
+  const int mnMinX;
+  const int mnMinY;
+  const int mnMaxX;
+  const int mnMaxY;
+
+  // Preintegrated IMU measurements from previous keyframe
+  KeyFrame* mPrevKF;
+  KeyFrame* mNextKF;
+
+  IMU::Preintegrated* mpImuPreintegrated;
+  IMU::Calib mImuCalib;
+
+  unsigned int mnOriginMapId;
+
+  string mNameFile;
+
+  int mnDataset;
+
+  std::vector<KeyFrame*> mvpLoopCandKFs;
+  std::vector<KeyFrame*> mvpMergeCandKFs;
+
+  // bool mbHasHessian;
+  // cv::Mat mHessianPose;
+
+  // The following variables need to be accessed trough a mutex to be thread
+  // safe.
+ protected:
+  // sophus poses
+  Sophus::SE3<float> mTcw;
+  Eigen::Matrix3f mRcw;
+  Sophus::SE3<float> mTwc;
+  Eigen::Matrix3f mRwc;
+
+  // IMU position
+  Eigen::Vector3f mOwb;
+  // Velocity (Only used for inertial SLAM)
+  Eigen::Vector3f mVw;
+  bool mbHasVelocity;
+
+  // Transformation matrix between cameras in stereo fisheye
+  Sophus::SE3<float> mTlr;
+  Sophus::SE3<float> mTrl;
+
+  // Imu bias
+  IMU::Bias mImuBias;
+
+  // MapPoints associated to keypoints
+  std::vector<MapPoint*> mvpMapPoints;
+  // For save relation without pointer, this is necessary for save/load function
+  std::vector<long long int> mvBackupMapPointsId;
+
+  // BoW
+  KeyFrameDatabase* mpKeyFrameDB;
+  ORBVocabulary* mpORBvocabulary;
+
+  // Grid over the image to speed up feature matching
+  std::vector<std::vector<std::vector<size_t> > > mGrid;
+
+  std::map<KeyFrame*, int> mConnectedKeyFrameWeights;
+  std::vector<KeyFrame*> mvpOrderedConnectedKeyFrames;
+  std::vector<int> mvOrderedWeights;
+  // For save relation without pointer, this is necessary for save/load function
+  std::map<long unsigned int, int> mBackupConnectedKeyFrameIdWeights;
+
+  // Spanning Tree and Loop Edges
+  bool mbFirstConnection;
+  KeyFrame* mpParent;
+  std::set<KeyFrame*> mspChildrens;
+  std::set<KeyFrame*> mspLoopEdges;
+  std::set<KeyFrame*> mspMergeEdges;
+  // For save relation without pointer, this is necessary for save/load function
+  long long int mBackupParentId;
+  std::vector<long unsigned int> mvBackupChildrensId;
+  std::vector<long unsigned int> mvBackupLoopEdgesId;
+  std::vector<long unsigned int> mvBackupMergeEdgesId;
+
+  // Bad flags
+  bool mbNotErase;
+  bool mbToBeErased;
+  bool mbBad;
+
+  float mHalfBaseline;  // Only for visualization
+
+  std::shared_ptr<Map> mpMap;
+
+  // Backup variables for inertial
+  long long int mBackupPrevKFId;
+  long long int mBackupNextKFId;
+  IMU::Preintegrated mBackupImuPreintegrated;
+
+  // Backup for Cameras
+  unsigned int mnBackupIdCamera, mnBackupIdCamera2;
+
+  // Calibration
+  Eigen::Matrix3f mK_;
+
+  // Mutex
+  std::mutex mMutexPose;  // for pose, velocity and biases
+  std::mutex mMutexConnections;
+  std::mutex mMutexFeatures;
+  std::mutex mMutexMap;
+
+ public:
+  std::shared_ptr<GeometricCamera> mpCamera, mpCamera2;
+
+  // Indexes of stereo observations correspondences
+  std::vector<int> mvLeftToRightMatch, mvRightToLeftMatch;
+
+  Sophus::SE3f GetRelativePoseTrl();
+  Sophus::SE3f GetRelativePoseTlr();
+
+  // KeyPoints in the right image (for stereo fisheye, coordinates are needed)
+  const std::vector<cv::KeyPoint> mvKeysRight;
+
+  const int NLeft, NRight;
+
+  std::vector<std::vector<std::vector<size_t> > > mGridRight;
+
+  Sophus::SE3<float> GetRightPose();
+  Sophus::SE3<float> GetRightPoseInverse();
+
+  Eigen::Vector3f GetRightCameraCenter();
+  Eigen::Matrix<float, 3, 3> GetRightRotation();
+  Eigen::Vector3f GetRightTranslation();
+
+  void PrintPointDistribution() {
+    int left = 0, right = 0;
+    int Nlim = (NLeft != -1) ? NLeft : N;
+    for (int i = 0; i < N; i++) {
+      if (mvpMapPoints[i]) {
+        if (i < Nlim)
+          left++;
+        else
+          right++;
+      }
+    }
+    cout << "Point distribution in KeyFrame: left-> " << left << " --- right-> "
+         << right << endl;
+  }
+};
+
+}  // namespace MORB_SLAM
+
diff --git a/include/MORB_SLAM/KeyFrameDatabase.h b/include/MORB_SLAM/KeyFrameDatabase.h
new file mode 100644
index 00000000..e6892748
--- /dev/null
+++ b/include/MORB_SLAM/KeyFrameDatabase.h
@@ -0,0 +1,99 @@
+/**
+ * This file is part of ORB-SLAM3
+ *
+ * Copyright (C) 2017-2021 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/>.
+ */
+
+#pragma once
+
+#include <boost/serialization/base_object.hpp>
+#include <boost/serialization/list.hpp>
+#include <boost/serialization/vector.hpp>
+#include <list>
+#include <mutex>
+#include <set>
+#include <vector>
+
+#include "MORB_SLAM/Frame.h"
+#include "MORB_SLAM/KeyFrame.h"
+#include "MORB_SLAM/Map.h"
+#include "MORB_SLAM/ORBVocabulary.h"
+
+namespace MORB_SLAM {
+
+class KeyFrame;
+class Frame;
+class Map;
+
+class KeyFrameDatabase {
+  friend class boost::serialization::access;
+
+  template <class Archive>
+  void serialize(Archive& ar, const unsigned int version) {
+    ar& mvBackupInvertedFileId;
+  }
+
+ public:
+  
+
+  KeyFrameDatabase() {}
+  KeyFrameDatabase(const ORBVocabulary& voc);
+
+  void add(KeyFrame* pKF);
+
+  void erase(KeyFrame* pKF);
+
+  void clear();
+  void clearMap(std::shared_ptr<Map> pMap);
+
+  // Loop Detection(DEPRECATED)
+  std::vector<KeyFrame*> DetectLoopCandidates(KeyFrame* pKF, float minScore);
+
+  // Loop and Merge Detection
+  void DetectCandidates(KeyFrame* pKF, float minScore,
+                        vector<KeyFrame*>& vpLoopCand,
+                        vector<KeyFrame*>& vpMergeCand);
+  void DetectBestCandidates(KeyFrame* pKF, vector<KeyFrame*>& vpLoopCand,
+                            vector<KeyFrame*>& vpMergeCand, int nMinWords);
+  void DetectNBestCandidates(KeyFrame* pKF, vector<KeyFrame*>& vpLoopCand,
+                             vector<KeyFrame*>& vpMergeCand,
+                             int nNumCandidates);
+
+  // Relocalization
+  std::vector<KeyFrame*> DetectRelocalizationCandidates(Frame* F, std::shared_ptr<Map> pMap);
+
+  void PreSave();
+  void PostLoad(map<long unsigned int, KeyFrame*> mpKFid);
+  void SetORBVocabulary(ORBVocabulary* pORBVoc);
+
+ protected:
+  // Associated vocabulary
+  const ORBVocabulary* mpVoc;
+
+  // Inverted file
+  std::vector<list<KeyFrame*> > mvInvertedFile;
+
+  // For save relation without pointer, this is necessary for save/load function
+  std::vector<list<long unsigned int> > mvBackupInvertedFileId;
+
+  // Mutex
+  std::mutex mMutex;
+};
+
+}  // namespace MORB_SLAM
+
diff --git a/include/MORB_SLAM/LocalMapping.h b/include/MORB_SLAM/LocalMapping.h
new file mode 100644
index 00000000..69ea75dd
--- /dev/null
+++ b/include/MORB_SLAM/LocalMapping.h
@@ -0,0 +1,198 @@
+/**
+ * This file is part of ORB-SLAM3
+ *
+ * Copyright (C) 2017-2021 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/>.
+ */
+
+#pragma once
+
+#include <mutex>
+
+#include "MORB_SLAM/ImprovedTypes.hpp"
+#include "MORB_SLAM/Atlas.h"
+#include "MORB_SLAM/KeyFrame.h"
+#include "MORB_SLAM/KeyFrameDatabase.h"
+#include "MORB_SLAM/LoopClosing.h"
+#include "MORB_SLAM/Settings.h"
+#include "MORB_SLAM/Tracking.h"
+
+namespace MORB_SLAM {
+
+class System;
+class Tracking;
+class LoopClosing;
+class Atlas;
+
+class LocalMapping {
+ public:
+  
+  LocalMapping(System* pSys, const Atlas_ptr &pAtlas, const float bMonocular,
+               bool bInertial, const string& _strSeqName = std::string());
+
+  void SetLoopCloser(LoopClosing* pLoopCloser);
+
+  void SetTracker(Tracking* pTracker);
+
+  // Main function
+  void Run();
+
+  void InsertKeyFrame(KeyFrame* pKF);
+  void EmptyQueue();
+
+  // Thread Synch
+  void RequestStop();
+  void RequestReset();
+  void RequestResetActiveMap(std::shared_ptr<Map> pMap);
+  bool Stop();
+  void Release();
+  bool isStopped();
+  bool stopRequested();
+  bool AcceptKeyFrames();
+  void SetAcceptKeyFrames(bool flag);
+  bool SetNotStop(bool flag);
+
+  void InterruptBA();
+
+  void RequestFinish();
+  bool isFinished();
+
+  int KeyframesInQueue() {
+    unique_lock<std::mutex> lock(mMutexNewKFs);
+    return mlNewKeyFrames.size();
+  }
+
+  bool IsInitializing();
+  double GetCurrKFTime();
+  KeyFrame* GetCurrKF();
+
+  std::mutex mMutexImuInit;
+
+  Eigen::MatrixXd mcovInertial;
+  Eigen::Matrix3d mRwg;
+  Eigen::Vector3d mbg;
+  Eigen::Vector3d mba;
+  double mScale;
+  double mInitTime;
+  double mCostTime;
+
+  unsigned int mInitSect;
+  unsigned int mIdxInit;
+  unsigned int mnKFs;
+  double mFirstTs;
+  int mnMatchesInliers;
+
+  // For debugging (erase in normal mode)
+  int mIdxIteration;
+  string strSequence;
+
+  bool mbNotBA1;
+  bool mbNotBA2;
+  bool mbBadImu;
+
+  bool mbWriteStats;
+
+  // not consider far points (clouds)
+  bool mbFarPoints;
+  float mThFarPoints;
+
+#ifdef REGISTER_TIMES
+  vector<double> vdKFInsert_ms;
+  vector<double> vdMPCulling_ms;
+  vector<double> vdMPCreation_ms;
+  vector<double> vdLBA_ms;
+  vector<double> vdKFCulling_ms;
+  vector<double> vdLMTotal_ms;
+
+  vector<double> vdLBASync_ms;
+  vector<double> vdKFCullingSync_ms;
+  vector<int> vnLBA_edges;
+  vector<int> vnLBA_KFopt;
+  vector<int> vnLBA_KFfixed;
+  vector<int> vnLBA_MPs;
+  int nLBA_exec;
+  int nLBA_abort;
+#endif
+ protected:
+  bool CheckNewKeyFrames();
+  void ProcessNewKeyFrame();
+  void CreateNewMapPoints();
+
+  void MapPointCulling();
+  void SearchInNeighbors();
+  void KeyFrameCulling();
+
+  System* mpSystem;
+
+  bool mbMonocular;
+  bool mbInertial;
+
+  void ResetIfRequested();
+  bool mbResetRequested;
+  bool mbResetRequestedActiveMap;
+  std::shared_ptr<Map> mpMapToReset;
+  std::mutex mMutexReset;
+
+  bool CheckFinish();
+  void SetFinish();
+  bool mbFinishRequested;
+  bool mbFinished;
+  std::mutex mMutexFinish;
+
+  Atlas_ptr mpAtlas;
+
+  LoopClosing* mpLoopCloser;
+  Tracking* mpTracker;
+
+  std::list<KeyFrame*> mlNewKeyFrames;
+
+  KeyFrame* mpCurrentKeyFrame;
+
+  std::list<MapPoint*> mlpRecentAddedMapPoints;
+
+  std::mutex mMutexNewKFs;
+
+  bool mbAbortBA;
+
+  bool mbStopped;
+  bool mbStopRequested;
+  bool mbNotStop;
+  std::mutex mMutexStop;
+
+  bool mbAcceptKeyFrames;
+  std::mutex mMutexAccept;
+
+  void InitializeIMU(float priorG = 1e2, float priorA = 1e6,
+                     bool bFirst = false);
+  void ScaleRefinement();
+
+  bool bInitializing;
+
+  Eigen::MatrixXd infoInertial;
+  int mNumLM;
+  int mNumKFCulling;
+
+  float mTinit;
+
+  int countRefinement;
+
+  // DEBUG
+  ofstream f_lm;
+};
+
+}  // namespace MORB_SLAM
+
diff --git a/include/MORB_SLAM/LoopClosing.h b/include/MORB_SLAM/LoopClosing.h
new file mode 100644
index 00000000..64039497
--- /dev/null
+++ b/include/MORB_SLAM/LoopClosing.h
@@ -0,0 +1,245 @@
+/**
+* This file is part of ORB-SLAM3
+*
+* Copyright (C) 2017-2021 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/>.
+*/
+
+
+#pragma once
+
+#include "MORB_SLAM/ImprovedTypes.hpp"
+#include "MORB_SLAM/KeyFrame.h"
+#include "MORB_SLAM/LocalMapping.h"
+#include "MORB_SLAM/Atlas.h"
+#include "MORB_SLAM/ORBVocabulary.h"
+#include "MORB_SLAM/Tracking.h"
+
+#include "KeyFrameDatabase.h"
+
+#include <boost/algorithm/string.hpp>
+#include <thread>
+#include <mutex>
+#include "g2o/types/types_seven_dof_expmap.h"
+
+namespace MORB_SLAM
+{
+
+class Tracking;
+class LocalMapping;
+class KeyFrameDatabase;
+class Map;
+
+
+class LoopClosing
+{
+public:
+
+    typedef pair<set<KeyFrame*>,int> ConsistentGroup;
+    typedef map<KeyFrame*,g2o::Sim3,std::less<KeyFrame*>,
+        Eigen::aligned_allocator<std::pair<KeyFrame* const, g2o::Sim3> > > KeyFrameAndPose;
+
+public:
+
+    LoopClosing(const Atlas_ptr &pAtlas, KeyFrameDatabase* pDB, ORBVocabulary* pVoc,const bool bFixScale, const bool bActiveLC);
+
+    void SetTracker(Tracking* pTracker);
+
+    void SetLocalMapper(LocalMapping* pLocalMapper);
+
+    // Main function
+    void Run();
+
+    void InsertKeyFrame(KeyFrame *pKF);
+
+    void RequestReset();
+    void RequestResetActiveMap(std::shared_ptr<Map> pMap);
+
+    // This function will run in a separate thread
+    void RunGlobalBundleAdjustment(std::shared_ptr<Map> pActiveMap, unsigned long nLoopKF);
+
+    bool isRunningGBA(){
+        unique_lock<std::mutex> lock(mMutexGBA);
+        return mbRunningGBA;
+    }
+    bool isFinishedGBA(){
+        unique_lock<std::mutex> lock(mMutexGBA);
+        return mbFinishedGBA;
+    }
+
+    void RequestFinish();
+
+    bool isFinished();
+
+#ifdef REGISTER_TIMES
+
+    vector<double> vdDataQuery_ms;
+    vector<double> vdEstSim3_ms;
+    vector<double> vdPRTotal_ms;
+
+    vector<double> vdMergeMaps_ms;
+    vector<double> vdWeldingBA_ms;
+    vector<double> vdMergeOptEss_ms;
+    vector<double> vdMergeTotal_ms;
+    vector<int> vnMergeKFs;
+    vector<int> vnMergeMPs;
+    int nMerges;
+
+    vector<double> vdLoopFusion_ms;
+    vector<double> vdLoopOptEss_ms;
+    vector<double> vdLoopTotal_ms;
+    vector<int> vnLoopKFs;
+    int nLoop;
+
+    vector<double> vdGBA_ms;
+    vector<double> vdUpdateMap_ms;
+    vector<double> vdFGBATotal_ms;
+    vector<int> vnGBAKFs;
+    vector<int> vnGBAMPs;
+    int nFGBA_exec;
+    int nFGBA_abort;
+
+#endif
+
+    
+
+protected:
+
+    bool CheckNewKeyFrames();
+
+
+    //Methods to implement the new place recognition algorithm
+    bool NewDetectCommonRegions();
+    bool DetectAndReffineSim3FromLastKF(KeyFrame* pCurrentKF, KeyFrame* pMatchedKF, g2o::Sim3 &gScw, int &nNumProjMatches,
+                                        std::vector<MapPoint*> &vpMPs, std::vector<MapPoint*> &vpMatchedMPs);
+    bool DetectCommonRegionsFromBoW(std::vector<KeyFrame*> &vpBowCand, KeyFrame* &pMatchedKF, KeyFrame* &pLastCurrentKF, g2o::Sim3 &g2oScw,
+                                     int &nNumCoincidences, std::vector<MapPoint*> &vpMPs, std::vector<MapPoint*> &vpMatchedMPs);
+    bool DetectCommonRegionsFromLastKF(KeyFrame* pCurrentKF, KeyFrame* pMatchedKF, g2o::Sim3 &gScw, int &nNumProjMatches,
+                                            std::vector<MapPoint*> &vpMPs, std::vector<MapPoint*> &vpMatchedMPs);
+    int FindMatchesByProjection(KeyFrame* pCurrentKF, KeyFrame* pMatchedKFw, g2o::Sim3 &g2oScw,
+                                set<MapPoint*> &spMatchedMPinOrigin, vector<MapPoint*> &vpMapPoints,
+                                vector<MapPoint*> &vpMatchedMapPoints);
+
+
+    void SearchAndFuse(const KeyFrameAndPose &CorrectedPosesMap, vector<MapPoint*> &vpMapPoints);
+    void SearchAndFuse(const vector<KeyFrame*> &vConectedKFs, vector<MapPoint*> &vpMapPoints);
+
+    void CorrectLoop();
+
+    void MergeLocal();
+    void MergeLocal2();
+
+    void CheckObservations(set<KeyFrame*> &spKFsMap1, set<KeyFrame*> &spKFsMap2);
+
+    void ResetIfRequested();
+    bool mbResetRequested;
+    bool mbResetActiveMapRequested;
+    std::shared_ptr<Map> mpMapToReset;
+    std::mutex mMutexReset;
+
+    bool CheckFinish();
+    void SetFinish();
+    bool mbFinishRequested;
+    bool mbFinished;
+    std::mutex mMutexFinish;
+
+    Atlas_ptr mpAtlas;
+    Tracking* mpTracker;
+
+    KeyFrameDatabase* mpKeyFrameDB;
+    ORBVocabulary* mpORBVocabulary;
+
+    LocalMapping *mpLocalMapper;
+
+    std::list<KeyFrame*> mlpLoopKeyFrameQueue;
+
+    std::mutex mMutexLoopQueue;
+
+    // Loop detector parameters
+    float mnCovisibilityConsistencyTh;
+
+    // Loop detector variables
+    KeyFrame* mpCurrentKF;
+    KeyFrame* mpLastCurrentKF;
+    KeyFrame* mpMatchedKF;
+    std::vector<ConsistentGroup> mvConsistentGroups;
+    std::vector<KeyFrame*> mvpEnoughConsistentCandidates;
+    std::vector<KeyFrame*> mvpCurrentConnectedKFs;
+    std::vector<MapPoint*> mvpCurrentMatchedPoints;
+    std::vector<MapPoint*> mvpLoopMapPoints;
+    cv::Mat mScw;
+    g2o::Sim3 mg2oScw;
+
+    //-------
+    std::shared_ptr<Map> mpLastMap;
+
+    bool mbLoopDetected;
+    int mnLoopNumCoincidences;
+    int mnLoopNumNotFound;
+    KeyFrame* mpLoopLastCurrentKF;
+    g2o::Sim3 mg2oLoopSlw;
+    g2o::Sim3 mg2oLoopScw;
+    KeyFrame* mpLoopMatchedKF;
+    std::vector<MapPoint*> mvpLoopMPs;
+    std::vector<MapPoint*> mvpLoopMatchedMPs;
+    bool mbMergeDetected;
+    int mnMergeNumCoincidences;
+    int mnMergeNumNotFound;
+    KeyFrame* mpMergeLastCurrentKF;
+    g2o::Sim3 mg2oMergeSlw;
+    g2o::Sim3 mg2oMergeSmw;
+    g2o::Sim3 mg2oMergeScw;
+    KeyFrame* mpMergeMatchedKF;
+    std::vector<MapPoint*> mvpMergeMPs;
+    std::vector<MapPoint*> mvpMergeMatchedMPs;
+    std::vector<KeyFrame*> mvpMergeConnectedKFs;
+
+    g2o::Sim3 mSold_new;
+    //-------
+
+    // Variables related to Global Bundle Adjustment
+    bool mbRunningGBA;
+    bool mbFinishedGBA;
+    bool mbStopGBA;
+    std::mutex mMutexGBA;
+    std::thread* mpThreadGBA;
+
+    // Fix scale in the stereo/RGB-D case
+    bool mbFixScale;
+
+
+    int mnFullBAIdx;
+
+
+
+    vector<double> vdPR_CurrentTime;
+    vector<double> vdPR_MatchedTime;
+    vector<int> vnPR_TypeRecogn;
+
+    //DEBUG
+    string mstrFolderSubTraj;
+    int mnNumCorrection;
+    int mnCorrectionGBA;
+
+
+    // To (de)activate LC
+    bool mbActiveLC = true;
+
+#ifdef REGISTER_LOOP
+    string mstrFolderLoop;
+#endif
+};
+
+} //namespace ORB_SLAM
+
diff --git a/include/MORB_SLAM/MLPnPsolver.h b/include/MORB_SLAM/MLPnPsolver.h
new file mode 100644
index 00000000..f13d65b4
--- /dev/null
+++ b/include/MORB_SLAM/MLPnPsolver.h
@@ -0,0 +1,243 @@
+/**
+ * This file is part of ORB-SLAM3
+ *
+ * Copyright (C) 2017-2021 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/>.
+ */
+
+/******************************************************************************
+ * Author:   Steffen Urban                                              *
+ * Contact:  urbste@gmail.com                                          *
+ * License:  Copyright (c) 2016 Steffen Urban, ANU. All rights reserved.      *
+ *                                                                            *
+ * Redistribution and use in source and binary forms, with or without         *
+ * modification, are permitted provided that the following conditions         *
+ * are met:                                                                   *
+ * * Redistributions of source code must retain the above copyright           *
+ *   notice, this list of conditions and the following disclaimer.            *
+ * * Redistributions in binary form must reproduce the above copyright        *
+ *   notice, this list of conditions and the following disclaimer in the      *
+ *   documentation and/or other materials provided with the distribution.     *
+ * * Neither the name of ANU nor the names of its contributors may be         *
+ *   used to endorse or promote products derived from this software without   *
+ *   specific prior written permission.                                       *
+ *                                                                            *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"*
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE  *
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE *
+ * ARE DISCLAIMED. IN NO EVENT SHALL ANU OR THE CONTRIBUTORS BE LIABLE        *
+ * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL *
+ * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR *
+ * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER *
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT         *
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY  *
+ * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF     *
+ * SUCH DAMAGE.                                                               *
+ ******************************************************************************/
+
+#pragma once
+
+#include <Eigen/Dense>
+#include <Eigen/Sparse>
+
+#include "MORB_SLAM/Frame.h"
+#include "MORB_SLAM/MapPoint.h"
+
+namespace MORB_SLAM {
+class MLPnPsolver {
+ public:
+  
+
+  MLPnPsolver(const Frame& F, const vector<MapPoint*>& vpMapPointMatches);
+
+  ~MLPnPsolver();
+
+  void SetRansacParameters(double probability = 0.99, int minInliers = 8,
+                           int maxIterations = 300, int minSet = 6,
+                           float epsilon = 0.4, float th2 = 5.991);
+
+  // Find metod is necessary?
+
+  bool iterate(int nIterations, bool& bNoMore, vector<bool>& vbInliers,
+               int& nInliers, Eigen::Matrix4f& Tout);
+
+  // Type definitions needed by the original code
+
+  /** A 3-vector of unit length used to describe landmark observations/bearings
+   *  in camera frames (always expressed in camera frames)
+   */
+  typedef Eigen::Vector3d bearingVector_t;
+
+  /** An array of bearing-vectors */
+  typedef std::vector<bearingVector_t,
+                      Eigen::aligned_allocator<bearingVector_t> >
+      bearingVectors_t;
+
+  /** A 2-matrix containing the 2D covariance information of a bearing vector
+   */
+  typedef Eigen::Matrix2d cov2_mat_t;
+
+  /** A 3-matrix containing the 3D covariance information of a bearing vector */
+  typedef Eigen::Matrix3d cov3_mat_t;
+
+  /** An array of 3D covariance matrices */
+  typedef std::vector<cov3_mat_t, Eigen::aligned_allocator<cov3_mat_t> >
+      cov3_mats_t;
+
+  /** A 3-vector describing a point in 3D-space */
+  typedef Eigen::Vector3d point_t;
+
+  /** An array of 3D-points */
+  typedef std::vector<point_t, Eigen::aligned_allocator<point_t> > points_t;
+
+  /** A homogeneous 3-vector describing a point in 3D-space */
+  typedef Eigen::Vector4d point4_t;
+
+  /** An array of homogeneous 3D-points */
+  typedef std::vector<point4_t, Eigen::aligned_allocator<point4_t> > points4_t;
+
+  /** A 3-vector containing the rodrigues parameters of a rotation matrix */
+  typedef Eigen::Vector3d rodrigues_t;
+
+  /** A rotation matrix */
+  typedef Eigen::Matrix3d rotation_t;
+
+  /** A 3x4 transformation matrix containing rotation \f$ \mathbf{R} \f$ and
+   *  translation \f$ \mathbf{t} \f$ as follows:
+   *  \f$ \left( \begin{array}{cc} \mathbf{R} & \mathbf{t} \end{array} \right)
+   * \f$
+   */
+  typedef Eigen::Matrix<double, 3, 4> transformation_t;
+
+  /** A 3-vector describing a translation/camera position */
+  typedef Eigen::Vector3d translation_t;
+
+ private:
+  void CheckInliers();
+  bool Refine();
+
+  // Functions from de original MLPnP code
+
+  /*
+   * Computes the camera pose given 3D points coordinates (in the camera
+   * reference system), the camera rays and (optionally) the covariance matrix
+   * of those camera rays. Result is stored in solution
+   */
+  void computePose(const bearingVectors_t& f, const points_t& p,
+                   const cov3_mats_t& covMats, const std::vector<int>& indices,
+                   transformation_t& result);
+
+  void mlpnp_gn(Eigen::VectorXd& x, const points_t& pts,
+                const std::vector<Eigen::MatrixXd>& nullspaces,
+                const Eigen::SparseMatrix<double> Kll, bool use_cov);
+
+  void mlpnp_residuals_and_jacs(const Eigen::VectorXd& x, const points_t& pts,
+                                const std::vector<Eigen::MatrixXd>& nullspaces,
+                                Eigen::VectorXd& r, Eigen::MatrixXd& fjac,
+                                bool getJacs);
+
+  void mlpnpJacs(const point_t& pt, const Eigen::Vector3d& nullspace_r,
+                 const Eigen::Vector3d& nullspace_s, const rodrigues_t& w,
+                 const translation_t& t, Eigen::MatrixXd& jacs);
+
+  // Auxiliar methods
+
+  /**
+   * \brief Compute a rotation matrix from Rodrigues axis angle.
+   *
+   * \param[in] omega The Rodrigues-parameters of a rotation.
+   * \return The 3x3 rotation matrix.
+   */
+  Eigen::Matrix3d rodrigues2rot(const Eigen::Vector3d& omega);
+
+  /**
+   * \brief Compute the Rodrigues-parameters of a rotation matrix.
+   *
+   * \param[in] R The 3x3 rotation matrix.
+   * \return The Rodrigues-parameters.
+   */
+  Eigen::Vector3d rot2rodrigues(const Eigen::Matrix3d& R);
+
+  //----------------------------------------------------
+  // Fields of the solver
+  //----------------------------------------------------
+  vector<MapPoint*> mvpMapPointMatches;
+
+  // 2D Points
+  vector<cv::Point2f> mvP2D;
+  // Substitued by bearing vectors
+  bearingVectors_t mvBearingVecs;
+
+  vector<float> mvSigma2;
+
+  // 3D Points
+  // vector<cv::Point3f> mvP3Dw;
+  points_t mvP3Dw;
+
+  // Index in Frame
+  vector<size_t> mvKeyPointIndices;
+
+  // Current Estimation
+  double mRi[3][3];
+  double mti[3];
+  Eigen::Matrix4f mTcwi;
+  vector<bool> mvbInliersi;
+  int mnInliersi;
+
+  // Current Ransac State
+  int mnIterations;
+  vector<bool> mvbBestInliers;
+  int mnBestInliers;
+  Eigen::Matrix4f mBestTcw;
+
+  // Refined
+  Eigen::Matrix4f mRefinedTcw;
+  vector<bool> mvbRefinedInliers;
+  int mnRefinedInliers;
+
+  // Number of Correspondences
+  int N;
+
+  // Indices for random selection [0 .. N-1]
+  vector<size_t> mvAllIndices;
+
+  // RANSAC probability
+  double mRansacProb;
+
+  // RANSAC min inliers
+  int mRansacMinInliers;
+
+  // RANSAC max iterations
+  int mRansacMaxIts;
+
+  // RANSAC expected inliers/total ratio
+  float mRansacEpsilon;
+
+  // RANSAC Threshold inlier/outlier. Max error e = dist(P1,T_12*P2)^2
+  float mRansacTh;
+
+  // RANSAC Minimun Set used at each iteration
+  int mRansacMinSet;
+
+  // Max square error associated with scale level. Max error =
+  // th*th*sigma(level)*sigma(level)
+  vector<float> mvMaxError;
+
+  std::shared_ptr<GeometricCamera> mpCamera;
+};
+
+}  // namespace MORB_SLAM
diff --git a/include/MORB_SLAM/Map.h b/include/MORB_SLAM/Map.h
new file mode 100644
index 00000000..2431758c
--- /dev/null
+++ b/include/MORB_SLAM/Map.h
@@ -0,0 +1,205 @@
+/**
+* This file is part of ORB-SLAM3
+*
+* Copyright (C) 2017-2021 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/>.
+*/
+
+
+#pragma once
+
+#include "MORB_SLAM/MapPoint.h"
+#include "MORB_SLAM/KeyFrame.h"
+
+#include <set>
+#include <pangolin/pangolin.h>
+#include <mutex>
+#include <memory>
+#include <boost/serialization/base_object.hpp>
+#include <stdexcept>
+
+
+namespace MORB_SLAM
+{
+
+class MapPoint;
+class KeyFrame;
+class Atlas;
+class KeyFrameDatabase;
+
+class Map
+{
+    friend class boost::serialization::access;
+
+    template<class Archive>
+    void serialize(Archive &ar, const unsigned int version)
+    {
+        ar & mnId;
+        ar & mnInitKFid;
+        ar & mnMaxKFid;
+        ar & mnBigChangeIdx;
+
+        // Save/load a set structure, the set structure is broken in libboost 1.58 for ubuntu 16.04, a vector is serializated
+        //ar & mspKeyFrames;
+        //ar & mspMapPoints;
+        ar & mvpBackupKeyFrames;
+        ar & mvpBackupMapPoints;
+
+        ar & mvBackupKeyFrameOriginsId;
+
+        ar & mnBackupKFinitialID;
+        ar & mnBackupKFlowerID;
+
+        ar & mbImuInitialized;
+        ar & mbIsInertial;
+        ar & mbIMU_BA1;
+        ar & mbIMU_BA2;
+    }
+
+public:
+    
+    Map();
+    Map(int initKFid);
+    ~Map();
+
+    void AddKeyFrame(KeyFrame* pKF);
+    void AddMapPoint(MapPoint* pMP);
+    void EraseMapPoint(MapPoint* pMP);
+    void EraseKeyFrame(KeyFrame* pKF);
+    void SetReferenceMapPoints(const std::vector<MapPoint*> &vpMPs);
+    void InformNewBigChange();
+    int GetLastBigChangeIdx();
+
+    std::vector<KeyFrame*> GetAllKeyFrames();
+    std::vector<MapPoint*> GetAllMapPoints();
+    std::vector<MapPoint*> GetReferenceMapPoints();
+
+    long unsigned int MapPointsInMap();
+    long unsigned  KeyFramesInMap();
+
+    long unsigned int GetId();
+
+    long unsigned int GetInitKFid();
+    void SetInitKFid(long unsigned int initKFif);
+    long unsigned int GetMaxKFid();
+
+    KeyFrame* GetOriginKF();
+
+    void SetCurrentMap();
+    void SetStoredMap();
+
+    bool HasThumbnail();
+    bool IsInUse();
+
+    void SetBad();
+    bool IsBad();
+
+    void clear();
+
+    int GetMapChangeIndex();
+    void IncreaseChangeIndex();
+    int GetLastMapChange();
+    void SetLastMapChange(int currentChangeId);
+
+    void SetImuInitialized();
+    bool isImuInitialized();
+
+    void ApplyScaledRotation(const Sophus::SE3f &T, const float s, const bool bScaledVel=false);
+
+    void SetInertialSensor();
+    bool IsInertial();
+    void SetIniertialBA1();
+    void SetIniertialBA2();
+    bool GetIniertialBA1();
+    bool GetIniertialBA2();
+
+    void PrintEssentialGraph();
+    bool CheckEssentialGraph();
+    void ChangeId(long unsigned int nId);
+
+    unsigned int GetLowerKFID();
+
+    void PreSave(std::set<std::shared_ptr<GeometricCamera>> &spCams, std::shared_ptr<Map> sharedMap);
+    void PostLoad(KeyFrameDatabase* pKFDB, ORBVocabulary* pORBVoc/*, map<long unsigned int, KeyFrame*>& mpKeyFrameId*/, map<unsigned int, std::shared_ptr<GeometricCamera>> &mpCams, std::shared_ptr<Map> sharedMap);
+
+    void printReprojectionError(list<KeyFrame*> &lpLocalWindowKFs, KeyFrame* mpCurrentKF, string &name, string &name_folder);
+
+    vector<KeyFrame*> mvpKeyFrameOrigins;
+    vector<unsigned long int> mvBackupKeyFrameOriginsId;
+    KeyFrame* mpFirstRegionKF;
+    std::mutex mMutexMapUpdate;
+
+    // This avoid that two points are created simultaneously in separate threads (id conflict)
+    std::mutex mMutexPointCreation;
+
+    bool mbFail;
+
+    // Size of the thumbnail (always in power of 2)
+    static const int THUMB_WIDTH = 512;
+    static const int THUMB_HEIGHT = 512;
+
+    static long unsigned int nNextId;
+
+    // DEBUG: show KFs which are used in LBA
+    std::set<long unsigned int> msOptKFs;
+    std::set<long unsigned int> msFixedKFs;
+
+protected:
+
+    long unsigned int mnId;
+
+    std::set<MapPoint*> mspMapPoints;
+    std::set<KeyFrame*> mspKeyFrames;
+
+    // Save/load, the set structure is broken in libboost 1.58 for ubuntu 16.04, a vector is serializated
+    std::vector<MapPoint*> mvpBackupMapPoints;
+    std::vector<KeyFrame*> mvpBackupKeyFrames;
+
+    KeyFrame* mpKFinitial;
+    KeyFrame* mpKFlowerID;
+
+    long int mnBackupKFinitialID;
+    long int mnBackupKFlowerID;
+
+    std::vector<MapPoint*> mvpReferenceMapPoints;
+
+    bool mbImuInitialized;
+
+    int mnMapChange;
+    int mnMapChangeNotified;
+
+    long unsigned int mnInitKFid;
+    long unsigned int mnMaxKFid;
+
+    // Index related to a big change in the map (loop closure, global BA)
+    int mnBigChangeIdx;
+
+
+    // View of the map in aerial sight (for the AtlasViewer)
+    GLubyte* mThumbnail;
+
+    bool mIsInUse;
+    bool mHasTumbnail;
+    bool mbBad = false;
+
+    bool mbIsInertial;
+    bool mbIMU_BA1;
+    bool mbIMU_BA2;
+
+    // Mutex
+    std::mutex mMutexMap;
+
+};
+
+} //namespace MORB_SLAM
diff --git a/include/MORB_SLAM/MapDrawer.h b/include/MORB_SLAM/MapDrawer.h
new file mode 100644
index 00000000..16e3b5e9
--- /dev/null
+++ b/include/MORB_SLAM/MapDrawer.h
@@ -0,0 +1,73 @@
+/**
+* This file is part of ORB-SLAM3
+*
+* Copyright (C) 2017-2021 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/>.
+*/
+
+
+#pragma once
+
+#include "MORB_SLAM/ImprovedTypes.hpp"
+#include "MORB_SLAM/Settings.h"
+#include<pangolin/pangolin.h>
+
+#include<mutex>
+
+namespace MORB_SLAM
+{
+
+class Settings;
+class KeyFrame;
+
+class MapDrawer
+{
+    void newParameterLoader(const Settings& settings);
+    Atlas_ptr mpAtlas;
+    
+    bool ParseViewerParamFile(cv::FileStorage &fSettings);
+
+    float mKeyFrameSize;
+    float mKeyFrameLineWidth;
+    float mGraphLineWidth;
+    float mPointSize;
+    float mCameraSize;
+    float mCameraLineWidth;
+
+    Sophus::SE3f mCameraPose;
+
+    std::mutex mMutexCamera;
+
+    float mfFrameColors[6][3] = {{0.0f, 0.0f, 1.0f},
+                                {0.8f, 0.4f, 1.0f},
+                                {1.0f, 0.2f, 0.4f},
+                                {0.6f, 0.0f, 1.0f},
+                                {1.0f, 1.0f, 0.0f},
+                                {0.0f, 1.0f, 1.0f}};
+
+public:
+    MapDrawer(const Atlas_ptr &pAtlas, const std::string &strSettingPath);
+    MapDrawer(const Atlas_ptr &pAtlas, const Settings& settings);
+
+    void DrawMapPoints();
+    void DrawKeyFrames(const bool bDrawKF, const bool bDrawGraph, const bool bDrawInertialGraph, const bool bDrawOptLba);
+    void DrawCurrentCamera(pangolin::OpenGlMatrix &Twc);
+    void SetCurrentCameraPose(const Sophus::SE3f &Tcw);
+    void SetReferenceKeyFrame(KeyFrame *pKF);
+    void GetCurrentOpenGLCameraMatrix(pangolin::OpenGlMatrix &M, pangolin::OpenGlMatrix &MOw);
+
+};
+
+} //namespace ORB_SLAM
+
diff --git a/include/MORB_SLAM/MapPoint.h b/include/MORB_SLAM/MapPoint.h
new file mode 100644
index 00000000..ef7ba25f
--- /dev/null
+++ b/include/MORB_SLAM/MapPoint.h
@@ -0,0 +1,253 @@
+/**
+* This file is part of ORB-SLAM3
+*
+* Copyright (C) 2017-2021 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/>.
+*/
+
+
+#pragma once
+
+#include "MORB_SLAM/KeyFrame.h"
+#include "MORB_SLAM/Frame.h"
+#include "MORB_SLAM/Map.h"
+#include "MORB_SLAM/Converter.h"
+
+#include "MORB_SLAM/SerializationUtils.h"
+
+#include <opencv2/core/core.hpp>
+#include <mutex>
+
+#include <boost/serialization/serialization.hpp>
+#include <boost/serialization/array.hpp>
+#include <boost/serialization/map.hpp>
+
+namespace MORB_SLAM
+{
+
+class KeyFrame;
+class Map;
+class Frame;
+
+class MapPoint
+{
+
+    friend class boost::serialization::access;
+    template<class Archive>
+    void serialize(Archive & ar, const unsigned int version)
+    {
+        ar & mnId;
+        ar & mnFirstKFid;
+        ar & mnFirstFrame;
+        ar & nObs;
+        // Variables used by the tracking
+        //ar & mTrackProjX;
+        //ar & mTrackProjY;
+        //ar & mTrackDepth;
+        //ar & mTrackDepthR;
+        //ar & mTrackProjXR;
+        //ar & mTrackProjYR;
+        //ar & mbTrackInView;
+        //ar & mbTrackInViewR;
+        //ar & mnTrackScaleLevel;
+        //ar & mnTrackScaleLevelR;
+        //ar & mTrackViewCos;
+        //ar & mTrackViewCosR;
+        //ar & mnTrackReferenceForFrame;
+        //ar & mnLastFrameSeen;
+
+        // Variables used by local mapping
+        //ar & mnBALocalForKF;
+        //ar & mnFuseCandidateForKF;
+
+        // Variables used by loop closing and merging
+        //ar & mnLoopPointForKF;
+        //ar & mnCorrectedByKF;
+        //ar & mnCorrectedReference;
+        //serializeMatrix(ar,mPosGBA,version);
+        //ar & mnBAGlobalForKF;
+        //ar & mnBALocalForMerge;
+        //serializeMatrix(ar,mPosMerge,version);
+        //serializeMatrix(ar,mNormalVectorMerge,version);
+
+        // Protected variables
+        ar & boost::serialization::make_array(mWorldPos.data(), mWorldPos.size());
+        ar & boost::serialization::make_array(mNormalVector.data(), mNormalVector.size());
+        //ar & BOOST_SERIALIZATION_NVP(mBackupObservationsId);
+        //ar & mObservations;
+        ar & mBackupObservationsId1;
+        ar & mBackupObservationsId2;
+        serializeMatrix(ar,mDescriptor,version);
+        ar & mBackupRefKFId;
+        //ar & mnVisible;
+        //ar & mnFound;
+
+        ar & mbBad;
+        ar & mBackupReplacedId;
+
+        ar & mfMinDistance;
+        ar & mfMaxDistance;
+
+    }
+
+
+public:
+    
+    MapPoint();
+
+    MapPoint(const Eigen::Vector3f &Pos, KeyFrame* pRefKF, std::shared_ptr<Map> pMap);
+    MapPoint(const double invDepth, cv::Point2f uv_init, KeyFrame* pRefKF, KeyFrame* pHostKF, std::shared_ptr<Map> pMap);
+    MapPoint(const Eigen::Vector3f &Pos,  std::shared_ptr<Map> pMap, Frame* pFrame, const int &idxF);
+
+    void SetWorldPos(const Eigen::Vector3f &Pos);
+    Eigen::Vector3f GetWorldPos();
+
+    Eigen::Vector3f GetNormal();
+    void SetNormalVector(const Eigen::Vector3f& normal);
+
+    KeyFrame* GetReferenceKeyFrame();
+
+    std::map<KeyFrame*,std::tuple<int,int>> GetObservations();
+    int Observations();
+
+    void AddObservation(KeyFrame* pKF,int idx);
+    void EraseObservation(KeyFrame* pKF);
+
+    std::tuple<int,int> GetIndexInKeyFrame(KeyFrame* pKF);
+    bool IsInKeyFrame(KeyFrame* pKF);
+
+    void SetBadFlag();
+    bool isBad();
+
+    void Replace(MapPoint* pMP);
+    MapPoint* GetReplaced();
+
+    void IncreaseVisible(int n=1);
+    void IncreaseFound(int n=1);
+    float GetFoundRatio();
+    inline int GetFound(){
+        return mnFound;
+    }
+
+    void ComputeDistinctiveDescriptors();
+
+    cv::Mat GetDescriptor();
+
+    void UpdateNormalAndDepth();
+
+    float GetMinDistanceInvariance();
+    float GetMaxDistanceInvariance();
+    int PredictScale(const float &currentDist, KeyFrame*pKF);
+    int PredictScale(const float &currentDist, Frame* pF);
+
+    std::shared_ptr<Map> GetMap();
+    void UpdateMap(std::shared_ptr<Map> pMap);
+
+    void PrintObservations();
+
+    void PreSave(set<KeyFrame*>& spKF,set<MapPoint*>& spMP);
+    void PostLoad(map<long unsigned int, KeyFrame*>& mpKFid, map<long unsigned int, MapPoint*>& mpMPid);
+
+public:
+    long unsigned int mnId;
+    static long unsigned int nNextId;
+    long int mnFirstKFid;
+    long int mnFirstFrame;
+    int nObs;
+
+    // Variables used by the tracking
+    float mTrackProjX;
+    float mTrackProjY;
+    float mTrackDepth;
+    float mTrackDepthR;
+    float mTrackProjXR;
+    float mTrackProjYR;
+    bool mbTrackInView, mbTrackInViewR;
+    int mnTrackScaleLevel, mnTrackScaleLevelR;
+    float mTrackViewCos, mTrackViewCosR;
+    long unsigned int mnTrackReferenceForFrame;
+    long unsigned int mnLastFrameSeen;
+
+    // Variables used by local mapping
+    long unsigned int mnBALocalForKF;
+    long unsigned int mnFuseCandidateForKF;
+
+    // Variables used by loop closing
+    long unsigned int mnLoopPointForKF;
+    long unsigned int mnCorrectedByKF;
+    long unsigned int mnCorrectedReference;
+    Eigen::Vector3f mPosGBA;
+    long unsigned int mnBAGlobalForKF;
+    long unsigned int mnBALocalForMerge;
+
+    // Variable used by merging
+    Eigen::Vector3f mPosMerge;
+    Eigen::Vector3f mNormalVectorMerge;
+
+
+    // Fopr inverse depth optimization
+    double mInvDepth;
+    double mInitU;
+    double mInitV;
+    KeyFrame* mpHostKF;
+
+    static std::mutex mGlobalMutex;
+
+    unsigned int mnOriginMapId;
+
+protected:
+
+     // Position in absolute coordinates
+     Eigen::Vector3f mWorldPos;
+
+     // Keyframes observing the point and associated index in keyframe
+     std::map<KeyFrame*,std::tuple<int,int> > mObservations;
+     // For save relation without pointer, this is necessary for save/load function
+     std::map<long unsigned int, int> mBackupObservationsId1;
+     std::map<long unsigned int, int> mBackupObservationsId2;
+
+     // Mean viewing direction
+     Eigen::Vector3f mNormalVector;
+
+     // Best descriptor to fast matching
+     cv::Mat mDescriptor;
+
+     // Reference KeyFrame
+     KeyFrame* mpRefKF;
+     long unsigned int mBackupRefKFId;
+
+     // Tracking counters
+     int mnVisible;
+     int mnFound;
+
+     // Bad flag (we do not currently erase MapPoint from memory)
+     bool mbBad;
+     MapPoint* mpReplaced;
+     // For save relation without pointer, this is necessary for save/load function
+     long long int mBackupReplacedId;
+
+     // Scale invariance distances
+     float mfMinDistance;
+     float mfMaxDistance;
+
+     std::shared_ptr<Map> mpMap;
+
+     // Mutex
+     std::mutex mMutexPos;
+     std::mutex mMutexFeatures;
+     std::mutex mMutexMap;
+
+};
+
+} //namespace ORB_SLAM
diff --git a/include/MORB_SLAM/ORBVocabulary.h b/include/MORB_SLAM/ORBVocabulary.h
new file mode 100644
index 00000000..0558266d
--- /dev/null
+++ b/include/MORB_SLAM/ORBVocabulary.h
@@ -0,0 +1,32 @@
+/**
+ * This file is part of ORB-SLAM3
+ *
+ * Copyright (C) 2017-2021 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/>.
+ */
+
+#pragma once
+
+#include "DBoW2/FORB.h"
+#include "DBoW2/TemplatedVocabulary.h"
+
+namespace MORB_SLAM {
+
+typedef DBoW2::TemplatedVocabulary<DBoW2::FORB::TDescriptor, DBoW2::FORB>
+    ORBVocabulary;
+
+}  // namespace MORB_SLAM
diff --git a/include/MORB_SLAM/ORBextractor.h b/include/MORB_SLAM/ORBextractor.h
new file mode 100644
index 00000000..fafa90a9
--- /dev/null
+++ b/include/MORB_SLAM/ORBextractor.h
@@ -0,0 +1,102 @@
+/**
+ * This file is part of ORB-SLAM3
+ *
+ * Copyright (C) 2017-2021 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/>.
+ */
+
+#pragma once
+
+#include <list>
+#include <opencv2/opencv.hpp>
+#include <vector>
+
+namespace MORB_SLAM {
+
+class ExtractorNode {
+ public:
+  ExtractorNode() : bNoMore(false) {}
+  ExtractorNode(const cv::Point2i &UL, const cv::Point2i &UR, const cv::Point2i &BL, const cv::Point2i &BR, int keySize) :
+    UL{UL}, UR{UR}, BL{BL}, BR{BR}, bNoMore(false) { vKeys.reserve(keySize); }
+
+  void DivideNode(ExtractorNode &n1, ExtractorNode &n2, ExtractorNode &n3,
+                  ExtractorNode &n4);
+
+  std::vector<cv::KeyPoint> vKeys;
+  cv::Point2i UL, UR, BL, BR;
+  std::list<ExtractorNode>::iterator lit;
+  bool bNoMore;
+};
+
+class ORBextractor {
+ public:
+  ORBextractor(int nfeatures, float scaleFactor, int nlevels, int iniThFAST,
+               int minThFAST);
+
+  // Compute the ORB features and descriptors on an image.
+  // ORB are dispersed on the image using an octree.
+  // Mask is ignored in the current implementation.
+  int operator()(cv::InputArray _image, cv::InputArray _mask,
+                 std::vector<cv::KeyPoint> &_keypoints,
+                 cv::OutputArray _descriptors, std::vector<int> &vLappingArea);
+
+  int inline GetLevels() { return nlevels; }
+
+  float inline GetScaleFactor() { return scaleFactor; }
+
+  std::vector<float> inline GetScaleFactors() { return mvScaleFactor; }
+
+  std::vector<float> inline GetInverseScaleFactors() {
+    return mvInvScaleFactor;
+  }
+
+  std::vector<float> inline GetScaleSigmaSquares() { return mvLevelSigma2; }
+
+  std::vector<float> inline GetInverseScaleSigmaSquares() {
+    return mvInvLevelSigma2;
+  }
+
+  std::vector<cv::Mat> mvImagePyramid;
+
+ protected:
+  void ComputePyramid(cv::Mat image);
+  void ComputeKeyPointsOctTree(
+      std::vector<std::vector<cv::KeyPoint> > &allKeypoints);
+  std::vector<cv::KeyPoint> DistributeOctTree(
+      const std::vector<cv::KeyPoint> &vToDistributeKeys, const int &minX,
+      const int &maxX, const int &minY, const int &maxY, const int &nFeatures,
+      const int &level);
+
+  std::vector<cv::Point> pattern;
+
+  int nfeatures;
+  double scaleFactor;
+  int nlevels;
+  int iniThFAST;
+  int minThFAST;
+
+  std::vector<int> mnFeaturesPerLevel;
+
+  std::vector<int> umax;
+
+  std::vector<float> mvScaleFactor;
+  std::vector<float> mvInvScaleFactor;
+  std::vector<float> mvLevelSigma2;
+  std::vector<float> mvInvLevelSigma2;
+};
+
+}  // namespace MORB_SLAM
diff --git a/include/MORB_SLAM/ORBmatcher.h b/include/MORB_SLAM/ORBmatcher.h
new file mode 100644
index 00000000..f9c69ab6
--- /dev/null
+++ b/include/MORB_SLAM/ORBmatcher.h
@@ -0,0 +1,131 @@
+/**
+ * This file is part of ORB-SLAM3
+ *
+ * Copyright (C) 2017-2021 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/>.
+ */
+
+#pragma once
+
+#include <opencv2/core/core.hpp>
+#include <opencv2/features2d/features2d.hpp>
+#include <vector>
+
+#include "MORB_SLAM/Frame.h"
+#include "MORB_SLAM/KeyFrame.h"
+#include "MORB_SLAM/MapPoint.h"
+#include "sophus/sim3.hpp"
+
+namespace MORB_SLAM {
+
+class ORBmatcher {
+ public:
+  ORBmatcher(float nnratio = 0.6, bool checkOri = true);
+
+  // Computes the Hamming distance between two ORB descriptors
+  static int DescriptorDistance(const cv::Mat &a, const cv::Mat &b);
+
+  // Search matches between Frame keypoints and projected MapPoints. Returns
+  // number of matches Used to track the local map (Tracking)
+  int SearchByProjection(Frame &F, const std::vector<MapPoint *> &vpMapPoints,
+                         const float th = 3, const bool bFarPoints = false,
+                         const float thFarPoints = 50.0f);
+
+  // Project MapPoints tracked in last frame into the current frame and search
+  // matches. Used to track from previous frame (Tracking)
+  int SearchByProjection(Frame &CurrentFrame, const Frame &LastFrame,
+                         const float th, const bool bMono);
+
+  // Project MapPoints seen in KeyFrame into the Frame and search matches.
+  // Used in relocalisation (Tracking)
+  int SearchByProjection(Frame &CurrentFrame, KeyFrame *pKF,
+                         const std::set<MapPoint *> &sAlreadyFound,
+                         const float th, const int ORBdist);
+
+  // Project MapPoints using a Similarity Transformation and search matches.
+  // Used in loop detection (Loop Closing)
+  int SearchByProjection(KeyFrame *pKF, Sophus::Sim3<float> &Scw,
+                         const std::vector<MapPoint *> &vpPoints,
+                         std::vector<MapPoint *> &vpMatched, int th,
+                         float ratioHamming = 1.0);
+
+  // Project MapPoints using a Similarity Transformation and search matches.
+  // Used in Place Recognition (Loop Closing and Merging)
+  int SearchByProjection(KeyFrame *pKF, Sophus::Sim3<float> &Scw,
+                         const std::vector<MapPoint *> &vpPoints,
+                         const std::vector<KeyFrame *> &vpPointsKFs,
+                         std::vector<MapPoint *> &vpMatched,
+                         std::vector<KeyFrame *> &vpMatchedKF, int th,
+                         float ratioHamming = 1.0);
+
+  // Search matches between MapPoints in a KeyFrame and ORB in a Frame.
+  // Brute force constrained to ORB that belong to the same vocabulary node (at
+  // a certain level) Used in Relocalisation and Loop Detection
+  int SearchByBoW(KeyFrame *pKF, Frame &F,
+                  std::vector<MapPoint *> &vpMapPointMatches);
+  int SearchByBoW(KeyFrame *pKF1, KeyFrame *pKF2,
+                  std::vector<MapPoint *> &vpMatches12);
+
+  // Matching for the Map Initialization (only used in the monocular case)
+  int SearchForInitialization(Frame &F1, Frame &F2,
+                              std::vector<cv::Point2f> &vbPrevMatched,
+                              std::vector<int> &vnMatches12,
+                              int windowSize = 10);
+
+  // Matching to triangulate new MapPoints. Check Epipolar Constraint.
+  int SearchForTriangulation(KeyFrame *pKF1, KeyFrame *pKF2,
+                             std::vector<pair<size_t, size_t> > &vMatchedPairs,
+                             const bool bOnlyStereo,
+                             const bool bCoarse = false);
+
+  // Search matches between MapPoints seen in KF1 and KF2 transforming by a Sim3
+  // [s12*R12|t12] In the stereo and RGB-D case, s12=1 int
+  // SearchBySim3(KeyFrame* pKF1, KeyFrame* pKF2, std::vector<MapPoint *>
+  // &vpMatches12, const float &s12, const cv::Mat &R12, const cv::Mat &t12,
+  // const float th);
+  int SearchBySim3(KeyFrame *pKF1, KeyFrame *pKF2,
+                   std::vector<MapPoint *> &vpMatches12,
+                   const Sophus::Sim3f &S12, const float th);
+
+  // Project MapPoints into KeyFrame and search for duplicated MapPoints.
+  int Fuse(KeyFrame *pKF, const vector<MapPoint *> &vpMapPoints,
+           const float th = 3.0, const bool bRight = false);
+
+  // Project MapPoints into KeyFrame using a given Sim3 and search for
+  // duplicated MapPoints.
+  int Fuse(KeyFrame *pKF, Sophus::Sim3f &Scw,
+           const std::vector<MapPoint *> &vpPoints, float th,
+           vector<MapPoint *> &vpReplacePoint);
+
+ public:
+  static const int TH_LOW;
+  static const int TH_HIGH;
+  static const int HISTO_LENGTH;
+  
+
+ protected:
+  float RadiusByViewingCos(const float &viewCos);
+
+  void ComputeThreeMaxima(std::vector<int> *histo, const int L, int &ind1,
+                          int &ind2, int &ind3);
+
+  float mfNNratio;
+  bool mbCheckOrientation;
+};
+
+}  // namespace MORB_SLAM
+
diff --git a/include/MORB_SLAM/OptimizableTypes.h b/include/MORB_SLAM/OptimizableTypes.h
new file mode 100644
index 00000000..1431c149
--- /dev/null
+++ b/include/MORB_SLAM/OptimizableTypes.h
@@ -0,0 +1,233 @@
+/**
+ * This file is part of ORB-SLAM3
+ *
+ * Copyright (C) 2017-2021 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/>.
+ */
+
+#pragma once
+
+#include <MORB_SLAM/CameraModels/GeometricCamera.h>
+#include <g2o/types/sim3.h>
+#include <g2o/types/types_six_dof_expmap.h>
+
+#include <Eigen/Geometry>
+
+#include "g2o/core/base_unary_edge.h"
+
+namespace MORB_SLAM {
+class EdgeSE3ProjectXYZOnlyPose
+    : public g2o::BaseUnaryEdge<2, Eigen::Vector2d, g2o::VertexSE3Expmap> {
+ public:
+  
+
+  EdgeSE3ProjectXYZOnlyPose() {}
+
+  bool read(std::istream& is);
+
+  bool write(std::ostream& os) const;
+
+  void computeError() {
+    const g2o::VertexSE3Expmap* v1 =
+        static_cast<const g2o::VertexSE3Expmap*>(_vertices[0]);
+    Eigen::Vector2d obs(_measurement);
+    _error = obs - pCamera->project(v1->estimate().map(Xw));
+  }
+
+  bool isDepthPositive() {
+    const g2o::VertexSE3Expmap* v1 =
+        static_cast<const g2o::VertexSE3Expmap*>(_vertices[0]);
+    return (v1->estimate().map(Xw))(2) > 0.0;
+  }
+
+  virtual void linearizeOplus();
+
+  Eigen::Vector3d Xw;
+  std::shared_ptr<GeometricCamera> pCamera;
+};
+
+class EdgeSE3ProjectXYZOnlyPoseToBody
+    : public g2o::BaseUnaryEdge<2, Eigen::Vector2d, g2o::VertexSE3Expmap> {
+ public:
+  
+
+  EdgeSE3ProjectXYZOnlyPoseToBody() {}
+
+  bool read(std::istream& is);
+
+  bool write(std::ostream& os) const;
+
+  void computeError() {
+    const g2o::VertexSE3Expmap* v1 =
+        static_cast<const g2o::VertexSE3Expmap*>(_vertices[0]);
+    Eigen::Vector2d obs(_measurement);
+    _error = obs - pCamera->project((mTrl * v1->estimate()).map(Xw));
+  }
+
+  bool isDepthPositive() {
+    const g2o::VertexSE3Expmap* v1 =
+        static_cast<const g2o::VertexSE3Expmap*>(_vertices[0]);
+    return ((mTrl * v1->estimate()).map(Xw))(2) > 0.0;
+  }
+
+  virtual void linearizeOplus();
+
+  Eigen::Vector3d Xw;
+  std::shared_ptr<GeometricCamera> pCamera;
+
+  g2o::SE3Quat mTrl;
+};
+
+class EdgeSE3ProjectXYZ
+    : public g2o::BaseBinaryEdge<2, Eigen::Vector2d, g2o::VertexSBAPointXYZ,
+                                 g2o::VertexSE3Expmap> {
+ public:
+  
+
+  EdgeSE3ProjectXYZ();
+
+  bool read(std::istream& is);
+
+  bool write(std::ostream& os) const;
+
+  void computeError() {
+    const g2o::VertexSE3Expmap* v1 =
+        static_cast<const g2o::VertexSE3Expmap*>(_vertices[1]);
+    const g2o::VertexSBAPointXYZ* v2 =
+        static_cast<const g2o::VertexSBAPointXYZ*>(_vertices[0]);
+    Eigen::Vector2d obs(_measurement);
+    _error = obs - pCamera->project(v1->estimate().map(v2->estimate()));
+  }
+
+  bool isDepthPositive() {
+    const g2o::VertexSE3Expmap* v1 =
+        static_cast<const g2o::VertexSE3Expmap*>(_vertices[1]);
+    const g2o::VertexSBAPointXYZ* v2 =
+        static_cast<const g2o::VertexSBAPointXYZ*>(_vertices[0]);
+    return ((v1->estimate().map(v2->estimate()))(2) > 0.0);
+  }
+
+  virtual void linearizeOplus();
+
+  std::shared_ptr<GeometricCamera> pCamera;
+};
+
+class EdgeSE3ProjectXYZToBody
+    : public g2o::BaseBinaryEdge<2, Eigen::Vector2d, g2o::VertexSBAPointXYZ,
+                                 g2o::VertexSE3Expmap> {
+ public:
+  
+
+  EdgeSE3ProjectXYZToBody();
+
+  bool read(std::istream& is);
+
+  bool write(std::ostream& os) const;
+
+  void computeError() {
+    const g2o::VertexSE3Expmap* v1 =
+        static_cast<const g2o::VertexSE3Expmap*>(_vertices[1]);
+    const g2o::VertexSBAPointXYZ* v2 =
+        static_cast<const g2o::VertexSBAPointXYZ*>(_vertices[0]);
+    Eigen::Vector2d obs(_measurement);
+    _error =
+        obs - pCamera->project((mTrl * v1->estimate()).map(v2->estimate()));
+  }
+
+  bool isDepthPositive() {
+    const g2o::VertexSE3Expmap* v1 =
+        static_cast<const g2o::VertexSE3Expmap*>(_vertices[1]);
+    const g2o::VertexSBAPointXYZ* v2 =
+        static_cast<const g2o::VertexSBAPointXYZ*>(_vertices[0]);
+    return ((mTrl * v1->estimate()).map(v2->estimate()))(2) > 0.0;
+  }
+
+  virtual void linearizeOplus();
+
+  std::shared_ptr<GeometricCamera> pCamera;
+  g2o::SE3Quat mTrl;
+};
+
+class VertexSim3Expmap : public g2o::BaseVertex<7, g2o::Sim3> {
+ public:
+  
+  VertexSim3Expmap();
+  virtual bool read(std::istream& is);
+  virtual bool write(std::ostream& os) const;
+
+  virtual void setToOriginImpl() { _estimate = g2o::Sim3(); }
+
+  virtual void oplusImpl(const double* update_) {
+    Eigen::Map<g2o::Vector7d> update(const_cast<double*>(update_));
+
+    if (_fix_scale) update[6] = 0;
+
+    g2o::Sim3 s(update);
+    setEstimate(s * estimate());
+  }
+
+  std::shared_ptr<GeometricCamera> pCamera1, pCamera2;
+
+  bool _fix_scale;
+};
+
+class EdgeSim3ProjectXYZ
+    : public g2o::BaseBinaryEdge<2, Eigen::Vector2d, g2o::VertexSBAPointXYZ,
+                                 MORB_SLAM::VertexSim3Expmap> {
+ public:
+  
+  EdgeSim3ProjectXYZ();
+  virtual bool read(std::istream& is);
+  virtual bool write(std::ostream& os) const;
+
+  void computeError() {
+    const MORB_SLAM::VertexSim3Expmap* v1 =
+        static_cast<const MORB_SLAM::VertexSim3Expmap*>(_vertices[1]);
+    const g2o::VertexSBAPointXYZ* v2 =
+        static_cast<const g2o::VertexSBAPointXYZ*>(_vertices[0]);
+
+    Eigen::Vector2d obs(_measurement);
+    _error = obs - v1->pCamera1->project(v1->estimate().map(v2->estimate()));
+  }
+
+  // virtual void linearizeOplus();
+};
+
+class EdgeInverseSim3ProjectXYZ
+    : public g2o::BaseBinaryEdge<2, Eigen::Vector2d, g2o::VertexSBAPointXYZ,
+                                 VertexSim3Expmap> {
+ public:
+  
+  EdgeInverseSim3ProjectXYZ();
+  virtual bool read(std::istream& is);
+  virtual bool write(std::ostream& os) const;
+
+  void computeError() {
+    const MORB_SLAM::VertexSim3Expmap* v1 =
+        static_cast<const MORB_SLAM::VertexSim3Expmap*>(_vertices[1]);
+    const g2o::VertexSBAPointXYZ* v2 =
+        static_cast<const g2o::VertexSBAPointXYZ*>(_vertices[0]);
+
+    Eigen::Vector2d obs(_measurement);
+    _error = obs - v1->pCamera2->project(
+                       (v1->estimate().inverse().map(v2->estimate())));
+  }
+
+  // virtual void linearizeOplus();
+};
+
+}  // namespace MORB_SLAM
diff --git a/include/MORB_SLAM/Optimizer.h b/include/MORB_SLAM/Optimizer.h
new file mode 100644
index 00000000..5bdcd6c3
--- /dev/null
+++ b/include/MORB_SLAM/Optimizer.h
@@ -0,0 +1,141 @@
+/**
+ * This file is part of ORB-SLAM3
+ *
+ * Copyright (C) 2017-2021 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/>.
+ */
+
+#pragma once
+
+#include <math.h>
+
+#include "MORB_SLAM/Frame.h"
+#include "MORB_SLAM/KeyFrame.h"
+#include "MORB_SLAM/LoopClosing.h"
+#include "MORB_SLAM/Map.h"
+#include "MORB_SLAM/MapPoint.h"
+#include "g2o/core/block_solver.h"
+#include "g2o/core/optimization_algorithm_gauss_newton.h"
+#include "g2o/core/optimization_algorithm_levenberg.h"
+#include "g2o/core/robust_kernel_impl.h"
+#include "g2o/core/sparse_block_matrix.h"
+#include "g2o/solvers/linear_solver_dense.h"
+#include "g2o/solvers/linear_solver_eigen.h"
+#include "g2o/types/types_seven_dof_expmap.h"
+#include "g2o/types/types_six_dof_expmap.h"
+
+namespace MORB_SLAM {
+
+class LoopClosing;
+
+class Optimizer {
+ public:
+  void static BundleAdjustment(const std::vector<KeyFrame *> &vpKF,
+                               const std::vector<MapPoint *> &vpMP,
+                               int nIterations = 5, bool *pbStopFlag = nullptr,
+                               const unsigned long nLoopKF = 0,
+                               const bool bRobust = true);
+  void static GlobalBundleAdjustemnt(std::shared_ptr<Map> pMap, int nIterations = 5,
+                                     bool *pbStopFlag = nullptr,
+                                     const unsigned long nLoopKF = 0,
+                                     const bool bRobust = true);
+  void static FullInertialBA(std::shared_ptr<Map> pMap, int its, const bool bFixLocal = false,
+                             const unsigned long nLoopKF = 0,
+                             bool *pbStopFlag = nullptr, bool bInit = false,
+                             float priorG = 1e2, float priorA = 1e6,
+                             Eigen::VectorXd *vSingVal = nullptr,
+                             bool *bHess = nullptr);
+
+  void static LocalBundleAdjustment(KeyFrame *pKF, bool *pbStopFlag, std::shared_ptr<Map> pMap,
+                                    int &num_fixedKF, int &num_OptKF,
+                                    int &num_MPs, int &num_edges);
+
+  int static PoseOptimization(Frame *pFrame);
+  int static PoseInertialOptimizationLastKeyFrame(Frame *pFrame,
+                                                  bool bRecInit = false);
+  int static PoseInertialOptimizationLastFrame(Frame *pFrame,
+                                               bool bRecInit = false);
+
+  // if bFixScale is true, 6DoF optimization (stereo,rgbd), 7DoF otherwise
+  // (mono)
+  void static OptimizeEssentialGraph(
+      std::shared_ptr<Map> pMap, KeyFrame *pLoopKF, KeyFrame *pCurKF,
+      const LoopClosing::KeyFrameAndPose &NonCorrectedSim3,
+      const LoopClosing::KeyFrameAndPose &CorrectedSim3,
+      const map<KeyFrame *, set<KeyFrame *> > &LoopConnections,
+      const bool &bFixScale);
+  void static OptimizeEssentialGraph(KeyFrame *pCurKF,
+                                     vector<KeyFrame *> &vpFixedKFs,
+                                     vector<KeyFrame *> &vpFixedCorrectedKFs,
+                                     vector<KeyFrame *> &vpNonFixedKFs,
+                                     vector<MapPoint *> &vpNonCorrectedMPs);
+
+  // For inertial loopclosing
+  void static OptimizeEssentialGraph4DoF(
+      std::shared_ptr<Map> pMap, KeyFrame *pLoopKF, KeyFrame *pCurKF,
+      const LoopClosing::KeyFrameAndPose &NonCorrectedSim3,
+      const LoopClosing::KeyFrameAndPose &CorrectedSim3,
+      const map<KeyFrame *, set<KeyFrame *> > &LoopConnections);
+
+  // if bFixScale is true, optimize SE3 (stereo,rgbd), Sim3 otherwise (mono)
+  // (NEW)
+  static int OptimizeSim3(KeyFrame *pKF1, KeyFrame *pKF2,
+                          std::vector<MapPoint *> &vpMatches1,
+                          g2o::Sim3 &g2oS12, const float th2,
+                          const bool bFixScale,
+                          Eigen::Matrix<double, 7, 7> &mAcumHessian,
+                          const bool bAllPoints = false);
+
+  // For inertial systems
+
+  void static LocalInertialBA(KeyFrame *pKF, bool *pbStopFlag, std::shared_ptr<Map> pMap,
+                              int &num_fixedKF, int &num_OptKF, int &num_MPs,
+                              int &num_edges, bool bLarge = false,
+                              bool bRecInit = false);
+  void static MergeInertialBA(KeyFrame *pCurrKF, KeyFrame *pMergeKF,
+                              bool *pbStopFlag, std::shared_ptr<Map> pMap,
+                              LoopClosing::KeyFrameAndPose &corrPoses);
+
+  // Local BA in welding area when two maps are merged
+  void static LocalBundleAdjustment(KeyFrame *pMainKF,
+                                    vector<KeyFrame *> vpAdjustKF,
+                                    vector<KeyFrame *> vpFixedKF,
+                                    bool *pbStopFlag);
+
+  // Marginalize block element (start:end,start:end). Perform Schur complement.
+  // Marginalized elements are filled with zeros.
+  static Eigen::MatrixXd Marginalize(const Eigen::MatrixXd &H, const int &start,
+                                     const int &end);
+
+  // Inertial pose-graph
+  void static InertialOptimization(std::shared_ptr<Map> pMap, Eigen::Matrix3d &Rwg,
+                                   double &scale, Eigen::Vector3d &bg,
+                                   Eigen::Vector3d &ba, bool bMono,
+                                   Eigen::MatrixXd &covInertial,
+                                   bool bFixedVel = false, bool bGauss = false,
+                                   float priorG = 1e2, float priorA = 1e6);
+  void static InertialOptimization(std::shared_ptr<Map> pMap, Eigen::Vector3d &bg,
+                                   Eigen::Vector3d &ba, float priorG = 1e2,
+                                   float priorA = 1e6);
+  void static InertialOptimization(std::shared_ptr<Map> pMap, Eigen::Matrix3d &Rwg,
+                                   double &scale);
+
+  ;
+};
+
+}  // namespace MORB_SLAM
+
diff --git a/include/MORB_SLAM/SerializationUtils.h b/include/MORB_SLAM/SerializationUtils.h
new file mode 100644
index 00000000..0f12eca3
--- /dev/null
+++ b/include/MORB_SLAM/SerializationUtils.h
@@ -0,0 +1,154 @@
+/**
+ * This file is part of ORB-SLAM3
+ *
+ * Copyright (C) 2017-2021 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/>.
+ */
+
+#pragma once
+
+#include <Eigen/Core>
+#include <boost/serialization/serialization.hpp>
+#include <boost/serialization/vector.hpp>
+#include <opencv2/core/core.hpp>
+#include <opencv2/features2d/features2d.hpp>
+#include <sophus/se3.hpp>
+#include <vector>
+
+namespace MORB_SLAM {
+
+template <class Archive>
+void serializeSophusSE3(Archive& ar, Sophus::SE3f& T,
+                        const unsigned int version) {
+  Eigen::Vector4f quat;
+  Eigen::Vector3f transl;
+
+  if (Archive::is_saving::value) {
+    Eigen::Quaternionf q = T.unit_quaternion();
+    quat << q.w(), q.x(), q.y(), q.z();
+    transl = T.translation();
+  }
+
+  ar& boost::serialization::make_array(quat.data(), quat.size());
+  ar& boost::serialization::make_array(transl.data(), transl.size());
+
+  if (Archive::is_loading::value) {
+    Eigen::Quaternionf q(quat[0], quat[1], quat[2], quat[3]);
+    T = Sophus::SE3f(q, transl);
+  }
+}
+
+/*template <class Archive, size_t dim>
+void serializeDiagonalMatrix(Archive &ar, Eigen::DiagonalMatrix<float, dim> &D,
+const unsigned int version)
+{
+    Eigen::Matrix<float,dim,dim> dense;
+    if(Archive::is_saving::value)
+    {
+        dense = D.toDenseMatrix();
+    }
+
+    ar & boost::serialization::make_array(dense.data(), dense.size());
+
+    if (Archive::is_loading::value)
+    {
+        D = dense.diagonal().asDiagonal();
+    }
+}*/
+
+template <class Archive>
+void serializeMatrix(Archive& ar, cv::Mat& mat, const unsigned int version) {
+  int cols, rows, type;
+  bool continuous;
+
+  if (Archive::is_saving::value) {
+    cols = mat.cols;
+    rows = mat.rows;
+    type = mat.type();
+    continuous = mat.isContinuous();
+  }
+
+  ar& cols& rows& type& continuous;
+
+  if (Archive::is_loading::value) mat.create(rows, cols, type);
+
+  if (continuous) {
+    const unsigned int data_size = rows * cols * mat.elemSize();
+    ar& boost::serialization::make_array(mat.ptr(), data_size);
+  } else {
+    const unsigned int row_size = cols * mat.elemSize();
+    for (int i = 0; i < rows; i++) {
+      ar& boost::serialization::make_array(mat.ptr(i), row_size);
+    }
+  }
+}
+
+template <class Archive>
+void serializeMatrix(Archive& ar, const cv::Mat& mat,
+                     const unsigned int version) {
+  cv::Mat matAux = mat;
+
+  serializeMatrix(ar, matAux, version);
+
+  if (Archive::is_loading::value) {
+    cv::Mat* ptr;
+    ptr = (cv::Mat*)(&mat);
+    *ptr = matAux;
+  }
+}
+
+template <class Archive>
+void serializeVectorKeyPoints(Archive& ar, const std::vector<cv::KeyPoint>& vKP,
+                              const unsigned int version) {
+  int NumEl;
+
+  if (Archive::is_saving::value) {
+    NumEl = vKP.size();
+  }
+
+  ar& NumEl;
+
+  std::vector<cv::KeyPoint> vKPaux = vKP;
+  if (Archive::is_loading::value) vKPaux.reserve(NumEl);
+
+  for (int i = 0; i < NumEl; ++i) {
+    cv::KeyPoint KPi;
+
+    if (Archive::is_loading::value) KPi = cv::KeyPoint();
+
+    if (Archive::is_saving::value) KPi = vKPaux[i];
+
+    ar& KPi.angle;
+    ar& KPi.response;
+    ar& KPi.size;
+    ar& KPi.pt.x;
+    ar& KPi.pt.y;
+    ar& KPi.class_id;
+    ar& KPi.octave;
+
+    if (Archive::is_loading::value) vKPaux.push_back(KPi);
+  }
+
+  if (Archive::is_loading::value) {
+    std::vector<cv::KeyPoint>* ptr;
+    ptr = (std::vector<cv::KeyPoint>*)(&vKP);
+    *ptr = vKPaux;
+  }
+}
+
+}  // namespace MORB_SLAM
+
diff --git a/include/MORB_SLAM/Settings.h b/include/MORB_SLAM/Settings.h
new file mode 100644
index 00000000..59672896
--- /dev/null
+++ b/include/MORB_SLAM/Settings.h
@@ -0,0 +1,239 @@
+/**
+ * This file is part of ORB-SLAM3
+ *
+ * Copyright (C) 2017-2021 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/>.
+ */
+
+#pragma once
+
+// Flag to activate the measurement of time in each process (track,localmap,
+// place recognition).
+//#define REGISTER_TIMES
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <unistd.h>
+
+#include <string>
+#include <memory>
+
+#include "MORB_SLAM/CameraModels/GeometricCamera.h"
+
+namespace MORB_SLAM {
+
+class System;
+
+// TODO: change to double instead of float
+
+class Settings {
+ public:
+  /*
+   * Enum for the different camera types implemented
+   */
+  enum CameraType { PinHole = 0, Rectified = 1, KannalaBrandt = 2 };
+
+  /*
+   * Delete default constructor
+   */
+  Settings() = delete;
+
+  /*
+   * Constructor from file
+   */
+  Settings(const std::string& configFile, const int& sensor);
+
+  /*
+   * Ostream operator overloading to dump settings to the terminal
+   */
+  friend std::ostream& operator<<(std::ostream& output, const Settings& s);
+
+  /*
+   * Getter methods
+   */
+  CameraType cameraType() const { return cameraType_; }
+  std::shared_ptr<GeometricCamera> camera1() { return calibration1_; }
+  std::shared_ptr<GeometricCamera> camera2() { return calibration2_; }
+  cv::Mat camera1DistortionCoef() {
+    return cv::Mat(vPinHoleDistorsion1_.size(), 1, CV_32F,
+                   vPinHoleDistorsion1_.data());
+  }
+  cv::Mat camera2DistortionCoef() {
+    return cv::Mat(vPinHoleDistorsion2_.size(), 1, CV_32F,
+                   vPinHoleDistorsion2_.data());
+  }
+
+  const Sophus::SE3f &Tlr() const { return Tlr_; }
+  float bf() const { return bf_; }
+  float b() const { return b_; }
+  float thDepth() const { return thDepth_; }
+
+  bool needToUndistort() const { return bNeedToUndistort_; }
+
+  const cv::Size &newImSize() const { return newImSize_; }
+  float fps() const { return fps_; }
+  bool rgb() const { return bRGB_; }
+  bool needToResize() const { return bNeedToResize1_; }
+  bool needToRectify() const { return bNeedToRectify_; }
+
+  float noiseGyro() const { return noiseGyro_; }
+  float noiseAcc() const { return noiseAcc_; }
+  float gyroWalk() const { return gyroWalk_; }
+  float accWalk() const { return accWalk_; }
+  float imuFrequency() const { return imuFrequency_; }
+  const Sophus::SE3f &Tbc() const { return Tbc_; }
+  bool insertKFsWhenLost() const { return insertKFsWhenLost_; }
+
+  float depthMapFactor() const { return depthMapFactor_; }
+
+  int nFeatures() const { return nFeatures_; }
+  int nLevels() const { return nLevels_; }
+  float initThFAST() const { return initThFAST_; }
+  float minThFAST() const { return minThFAST_; }
+  float scaleFactor() const { return scaleFactor_; }
+
+  float keyFrameSize() const { return keyFrameSize_; }
+  float keyFrameLineWidth() const { return keyFrameLineWidth_; }
+  float graphLineWidth() const { return graphLineWidth_; }
+  float pointSize() const { return pointSize_; }
+  float cameraSize() const { return cameraSize_; }
+  float cameraLineWidth() const { return cameraLineWidth_; }
+  float viewPointX() const { return viewPointX_; }
+  float viewPointY() const { return viewPointY_; }
+  float viewPointZ() const { return viewPointZ_; }
+  float viewPointF() const { return viewPointF_; }
+  float imageViewerScale() const { return imageViewerScale_; }
+
+  const std::string &atlasLoadFile() const { return sLoadFrom_; }
+  const std::string &atlasSaveFile() const { return sSaveto_; }
+
+  float thFarPoints() const { return thFarPoints_; }
+
+  const cv::Mat &M1l() const { return M1l_; }
+  const cv::Mat &M2l() const { return M2l_; }
+  const cv::Mat &M1r() const { return M1r_; }
+  const cv::Mat &M2r() const { return M2r_; }
+
+ private:
+  template <typename T>
+  T readParameter(cv::FileStorage& fSettings, const std::string& name,
+                  bool& found, const bool required = true) {
+    cv::FileNode node = fSettings[name];
+    if (node.empty()) {
+      if (required) {
+        std::cerr << name << " required parameter does not exist, aborting..."
+                  << std::endl;
+        exit(-1);
+      } else {
+        std::cerr << name << " optional parameter does not exist..."
+                  << std::endl;
+        found = false;
+        return T();
+      }
+
+    } else {
+      found = true;
+      return (T)node;
+    }
+  }
+
+  void readCamera1(cv::FileStorage& fSettings);
+  void readCamera2(cv::FileStorage& fSettings);
+  void readImageInfo(cv::FileStorage& fSettings);
+  void readIMU(cv::FileStorage& fSettings);
+  void readRGBD(cv::FileStorage& fSettings);
+  void readORB(cv::FileStorage& fSettings);
+  void readViewer(cv::FileStorage& fSettings);
+  void readLoadAndSave(cv::FileStorage& fSettings);
+  void readOtherParameters(cv::FileStorage& fSettings);
+
+  void precomputeRectificationMaps();
+
+  int sensor_;
+  CameraType cameraType_;  // Camera type
+
+  /*
+   * Visual stuff
+   */
+  std::shared_ptr<GeometricCamera> calibration1_, calibration2_;  // Camera calibration
+  GeometricCamera *originalCalib1_, *originalCalib2_;
+  std::vector<float> vPinHoleDistorsion1_, vPinHoleDistorsion2_;
+
+  cv::Size originalImSize_, newImSize_;
+  float fps_;
+  bool bRGB_;
+
+  bool bNeedToUndistort_;
+  bool bNeedToRectify_;
+  bool bNeedToResize1_, bNeedToResize2_;
+
+  Sophus::SE3f Tlr_;
+  float thDepth_;
+  float bf_, b_;
+
+  /*
+   * Rectification stuff
+   */
+  cv::Mat M1l_, M2l_;
+  cv::Mat M1r_, M2r_;
+
+  /*
+   * Inertial stuff
+   */
+  float noiseGyro_, noiseAcc_;
+  float gyroWalk_, accWalk_;
+  float imuFrequency_;
+  Sophus::SE3f Tbc_;
+  bool insertKFsWhenLost_;
+
+  /*
+   * RGBD stuff
+   */
+  float depthMapFactor_;
+
+  /*
+   * ORB stuff
+   */
+  int nFeatures_;
+  float scaleFactor_;
+  int nLevels_;
+  int initThFAST_, minThFAST_;
+
+  /*
+   * Viewer stuff
+   */
+  float keyFrameSize_;
+  float keyFrameLineWidth_;
+  float graphLineWidth_;
+  float pointSize_;
+  float cameraSize_;
+  float cameraLineWidth_;
+  float viewPointX_, viewPointY_, viewPointZ_, viewPointF_;
+  float imageViewerScale_;
+
+  /*
+   * Save & load maps
+   */
+  std::string sLoadFrom_, sSaveto_;
+
+  /*
+   * Other stuff
+   */
+  float thFarPoints_;
+};
+}  // namespace MORB_SLAM
+
diff --git a/include/MORB_SLAM/Sim3Solver.h b/include/MORB_SLAM/Sim3Solver.h
new file mode 100644
index 00000000..eba6c8dc
--- /dev/null
+++ b/include/MORB_SLAM/Sim3Solver.h
@@ -0,0 +1,137 @@
+/**
+ * This file is part of ORB-SLAM3
+ *
+ * Copyright (C) 2017-2021 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/>.
+ */
+
+#pragma once
+
+#include <opencv2/opencv.hpp>
+#include <vector>
+
+#include "MORB_SLAM/KeyFrame.h"
+
+namespace MORB_SLAM {
+
+class Sim3Solver {
+ public:
+  
+  Sim3Solver(
+      KeyFrame *pKF1, KeyFrame *pKF2,
+      const std::vector<MapPoint *> &vpMatched12, const bool bFixScale = true,
+      const vector<KeyFrame *> vpKeyFrameMatchedMP = vector<KeyFrame *>());
+
+  void SetRansacParameters(double probability = 0.99, int minInliers = 6,
+                           int maxIterations = 300);
+
+  Eigen::Matrix4f find(std::vector<bool> &vbInliers12, int &nInliers);
+
+  Eigen::Matrix4f iterate(int nIterations, bool &bNoMore,
+                          std::vector<bool> &vbInliers, int &nInliers);
+  Eigen::Matrix4f iterate(int nIterations, bool &bNoMore,
+                          vector<bool> &vbInliers, int &nInliers,
+                          bool &bConverge);
+
+  Eigen::Matrix4f GetEstimatedTransformation();
+  Eigen::Matrix3f GetEstimatedRotation();
+  Eigen::Vector3f GetEstimatedTranslation();
+  float GetEstimatedScale();
+
+ protected:
+  void ComputeCentroid(Eigen::Matrix3f &P, Eigen::Matrix3f &Pr,
+                       Eigen::Vector3f &C);
+
+  void ComputeSim3(Eigen::Matrix3f &P1, Eigen::Matrix3f &P2);
+
+  void CheckInliers();
+
+  void Project(const std::vector<Eigen::Vector3f> &vP3Dw,
+               std::vector<Eigen::Vector2f> &vP2D, Eigen::Matrix4f Tcw,
+               const std::shared_ptr<GeometricCamera> &pCamera);
+  void FromCameraToImage(const std::vector<Eigen::Vector3f> &vP3Dc,
+                         std::vector<Eigen::Vector2f> &vP2D,
+                         const std::shared_ptr<GeometricCamera> &pCamera);
+
+ protected:
+  // KeyFrames and matches
+  KeyFrame *mpKF1;
+  KeyFrame *mpKF2;
+
+  std::vector<Eigen::Vector3f> mvX3Dc1;
+  std::vector<Eigen::Vector3f> mvX3Dc2;
+  std::vector<MapPoint *> mvpMapPoints1;
+  std::vector<MapPoint *> mvpMapPoints2;
+  std::vector<MapPoint *> mvpMatches12;
+  std::vector<size_t> mvnIndices1;
+  std::vector<size_t> mvSigmaSquare1;
+  std::vector<size_t> mvSigmaSquare2;
+  std::vector<size_t> mvnMaxError1;
+  std::vector<size_t> mvnMaxError2;
+
+  int N;
+  int mN1;
+
+  // Current Estimation
+  Eigen::Matrix3f mR12i;
+  Eigen::Vector3f mt12i;
+  float ms12i;
+  Eigen::Matrix4f mT12i;
+  Eigen::Matrix4f mT21i;
+  std::vector<bool> mvbInliersi;
+  int mnInliersi;
+
+  // Current Ransac State
+  int mnIterations;
+  std::vector<bool> mvbBestInliers;
+  int mnBestInliers;
+  Eigen::Matrix4f mBestT12;
+  Eigen::Matrix3f mBestRotation;
+  Eigen::Vector3f mBestTranslation;
+  float mBestScale;
+
+  // Scale is fixed to 1 in the stereo/RGBD case
+  bool mbFixScale;
+
+  // Indices for random selection
+  std::vector<size_t> mvAllIndices;
+
+  // Projections
+  std::vector<Eigen::Vector2f> mvP1im1;
+  std::vector<Eigen::Vector2f> mvP2im2;
+
+  // RANSAC probability
+  double mRansacProb;
+
+  // RANSAC min inliers
+  int mRansacMinInliers;
+
+  // RANSAC max iterations
+  int mRansacMaxIts;
+
+  // Threshold inlier/outlier. e = dist(Pi,T_ij*Pj)^2 < 5.991*mSigma2
+  float mTh;
+  float mSigma2;
+
+  // Calibration
+  // cv::Mat mK1;
+  // cv::Mat mK2;
+
+  std::shared_ptr<GeometricCamera> pCamera1, pCamera2;
+};
+
+}  // namespace MORB_SLAM
diff --git a/include/MORB_SLAM/System.h b/include/MORB_SLAM/System.h
new file mode 100644
index 00000000..ef9516d6
--- /dev/null
+++ b/include/MORB_SLAM/System.h
@@ -0,0 +1,245 @@
+/**
+* This file is part of ORB-SLAM3
+*
+* Copyright (C) 2017-2021 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/>.
+*/
+
+
+#pragma once
+
+
+#include <unistd.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string>
+#include <thread>
+#include <opencv2/core/core.hpp>
+
+#include "MORB_SLAM/ImprovedTypes.hpp"
+#include "MORB_SLAM/Tracking.h"
+#include "MORB_SLAM/Atlas.h"
+#include "MORB_SLAM/LocalMapping.h"
+#include "MORB_SLAM/LoopClosing.h"
+#include "MORB_SLAM/KeyFrameDatabase.h"
+#include "MORB_SLAM/ORBVocabulary.h"
+#include "MORB_SLAM/ImuTypes.h"
+#include "MORB_SLAM/Settings.h"
+#include "MORB_SLAM/Camera.hpp"
+
+
+namespace MORB_SLAM
+{
+
+class Verbose
+{
+public:
+    enum eLevel
+    {
+        VERBOSITY_QUIET=0,
+        VERBOSITY_NORMAL=1,
+        VERBOSITY_VERBOSE=2,
+        VERBOSITY_VERY_VERBOSE=3,
+        VERBOSITY_DEBUG=4
+    };
+
+    static eLevel th;
+
+public:
+    static void PrintMess(std::string str, eLevel lev)
+    {
+        if(lev <= th)
+            cout << str << endl;
+    }
+
+    static void SetTh(eLevel _th)
+    {
+        th = _th;
+    }
+};
+
+class Viewer;
+class Atlas;
+class Tracking;
+class LocalMapping;
+class LoopClosing;
+class Settings;
+
+class System
+{
+public:
+
+    // File type
+    enum FileType{
+        TEXT_FILE=0,
+        BINARY_FILE=1,
+    };
+
+public:
+    
+    // Initialize the SLAM system. It launches the Local Mapping, Loop Closing and Viewer threads.
+    System(const string &strVocFile, const string &strSettingsFile, const CameraType::eSensor sensor, const string &strSequence = std::string());
+
+    // Proccess the given stereo frame. Images must be synchronized and rectified.
+    // Input images: RGB (CV_8UC3) or grayscale (CV_8U). RGB is converted to grayscale.
+    // Returns the camera pose (empty if tracking fails).
+    Sophus::SE3f TrackStereo(const cv::Mat &imLeft, const cv::Mat &imRight, const double &timestamp, const vector<IMU::Point>& vImuMeas = vector<IMU::Point>(), string filename="");
+
+    // Process the given rgbd frame. Depthmap must be registered to the RGB frame.
+    // Input image: RGB (CV_8UC3) or grayscale (CV_8U). RGB is converted to grayscale.
+    // Input depthmap: Float (CV_32F).
+    // Returns the camera pose (empty if tracking fails).
+    Sophus::SE3f TrackRGBD(const cv::Mat &im, const cv::Mat &depthmap, const double &timestamp, const vector<IMU::Point>& vImuMeas = vector<IMU::Point>(), string filename="");
+
+    // Proccess the given monocular frame and optionally imu data
+    // Input images: RGB (CV_8UC3) or grayscale (CV_8U). RGB is converted to grayscale.
+    // Returns the camera pose (empty if tracking fails).
+    Sophus::SE3f TrackMonocular(const cv::Mat &im, const double &timestamp, const vector<IMU::Point>& vImuMeas = vector<IMU::Point>(), string filename="");
+
+
+    // This stops local mapping thread (map building) and performs only camera tracking.
+    void ActivateLocalizationMode();
+    // This resumes local mapping thread and performs SLAM again.
+    void DeactivateLocalizationMode();
+
+    // Returns true if there have been a big map change (loop closure, global BA)
+    // since last call to this function
+    bool MapChanged();
+
+    // Reset the system (clear Atlas or the active map)
+    void Reset();
+    void ResetActiveMap();
+
+    // All threads will be requested to finish.
+    // It waits until all threads have finished.
+    // This function must be called before saving the trajectory.
+    virtual ~System();
+
+    // Save camera trajectory in the TUM RGB-D dataset format.
+    // Only for stereo and RGB-D. This method does not work for monocular.
+    // Call first Shutdown()
+    // See format details at: http://vision.in.tum.de/data/datasets/rgbd-dataset
+    void SaveTrajectoryTUM(const string &filename);
+
+    // Save keyframe poses in the TUM RGB-D dataset format.
+    // This method works for all sensor input.
+    // Call first Shutdown()
+    // See format details at: http://vision.in.tum.de/data/datasets/rgbd-dataset
+    void SaveKeyFrameTrajectoryTUM(const string &filename);
+
+    void SaveTrajectoryEuRoC(const string &filename);
+    void SaveKeyFrameTrajectoryEuRoC(const string &filename);
+
+    void SaveTrajectoryEuRoC(const string &filename, std::shared_ptr<Map> pMap);
+    void SaveKeyFrameTrajectoryEuRoC(const string &filename, std::shared_ptr<Map> pMap);
+
+    // Save data used for initialization debug
+    void SaveDebugData(const int &iniIdx);
+
+    // Save camera trajectory in the KITTI dataset format.
+    // Only for stereo and RGB-D. This method does not work for monocular.
+    // Call first Shutdown()
+    // See format details at: http://www.cvlibs.net/datasets/kitti/eval_odometry.php
+    void SaveTrajectoryKITTI(const string &filename);
+
+    // TODO: Save/Load functions
+    // SaveMap(const string &filename);
+    // LoadMap(const string &filename);
+
+    // Information from most recent processed frame
+    // You can call this right after TrackMonocular (or stereo or RGBD)
+    int GetTrackingState();
+    std::vector<MapPoint*> GetTrackedMapPoints();
+    std::vector<cv::KeyPoint> GetTrackedKeyPointsUn();
+
+    // For debugging
+    double GetTimeFromIMUInit();
+    bool isLost();
+    bool isFinished();
+
+    void ChangeDataset();
+
+    float GetImageScale();
+
+#ifdef REGISTER_TIMES
+    void InsertRectTime(double& time);
+    void InsertResizeTime(double& time);
+    void InsertTrackTime(double& time);
+#endif
+
+    friend Viewer;
+private:
+
+    void SaveAtlas(int type);
+    bool LoadAtlas(int type);
+
+    string CalculateCheckSum(string filename, int type);
+
+    // Input sensor
+    CameraType::eSensor mSensor;
+    vector<Camera_ptr> cameras;
+
+    // ORB vocabulary used for place recognition and feature matching.
+    ORBVocabulary* mpVocabulary;
+
+    // KeyFrame database for place recognition (relocalization and loop detection).
+    KeyFrameDatabase* mpKeyFrameDatabase;
+
+    // Map structure that stores the pointers to all KeyFrames and MapPoints.
+    //Map* mpMap;
+    Atlas_ptr mpAtlas;
+
+    // Tracker. It receives a frame and computes the associated camera pose.
+    // It also decides when to insert a new keyframe, create some new MapPoints and
+    // performs relocalization if tracking fails.
+    Tracking* mpTracker;
+
+    // Local Mapper. It manages the local map and performs local bundle adjustment.
+    LocalMapping* mpLocalMapper;
+
+    // Loop Closer. It searches loops with every new keyframe. If there is a loop it performs
+    // a pose graph optimization and full bundle adjustment (in a new thread) afterwards.
+    LoopClosing* mpLoopCloser;
+
+    // System threads: Local Mapping, Loop Closing, Viewer.
+    // The Tracking thread "lives" in the main execution thread that creates the System object.
+    std::thread* mptLocalMapping;
+    std::thread* mptLoopClosing;
+
+    // Reset flag
+    std::mutex mMutexReset;
+    bool mbReset;
+    bool mbResetActiveMap;
+
+    // Change mode flags
+    std::mutex mMutexMode;
+    bool mbActivateLocalizationMode;
+    bool mbDeactivateLocalizationMode;
+
+    // Tracking state
+    int mTrackingState;
+    std::vector<MapPoint*> mTrackedMapPoints;
+    std::vector<cv::KeyPoint> mTrackedKeyPointsUn;
+    std::mutex mMutexState;
+
+    //
+    string mStrLoadAtlasFromFile;
+    string mStrSaveAtlasToFile;
+
+    string mStrVocabularyFilePath;
+
+    Settings* settings_;
+};
+
+}// namespace ORB_SLAM
diff --git a/include/MORB_SLAM/Tracking.h b/include/MORB_SLAM/Tracking.h
new file mode 100644
index 00000000..e8ef1b7c
--- /dev/null
+++ b/include/MORB_SLAM/Tracking.h
@@ -0,0 +1,364 @@
+/**
+ * This file is part of ORB-SLAM3
+ *
+ * Copyright (C) 2017-2021 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/>.
+ */
+
+#pragma once
+
+#include <mutex>
+#include <opencv2/core/core.hpp>
+#include <opencv2/features2d/features2d.hpp>
+#include <unordered_set>
+
+#include "MORB_SLAM/Atlas.h"
+#include "MORB_SLAM/Frame.h"
+#include "MORB_SLAM/CameraModels/GeometricCamera.h"
+#include "MORB_SLAM/ImuTypes.h"
+#include "MORB_SLAM/KeyFrameDatabase.h"
+#include "MORB_SLAM/LocalMapping.h"
+#include "MORB_SLAM/LoopClosing.h"
+#include "MORB_SLAM/ORBVocabulary.h"
+#include "MORB_SLAM/ORBextractor.h"
+#include "MORB_SLAM/Settings.h"
+#include "MORB_SLAM/System.h"
+#include "MORB_SLAM/ImprovedTypes.hpp"
+#include "MORB_SLAM/Camera.hpp"
+
+namespace MORB_SLAM {
+
+class Atlas;
+class LocalMapping;
+class LoopClosing;
+class System;
+class Settings;
+
+class Tracking {
+ public:
+  
+  Tracking(System* pSys, ORBVocabulary* pVoc,
+           const Atlas_ptr &pAtlas, KeyFrameDatabase* pKFDB,
+           const string& strSettingPath, const CameraType::eSensor sensor, Settings* settings,
+           const string& _nameSeq = std::string());
+
+  ~Tracking();
+
+  // Parse the config file
+  bool ParseCamParamFile(cv::FileStorage& fSettings);
+  bool ParseORBParamFile(cv::FileStorage& fSettings);
+  bool ParseIMUParamFile(cv::FileStorage& fSettings);
+
+  // Preprocess the input and call Track(). Extract features and performs stereo
+  // matching.
+  Sophus::SE3f GrabImageStereo(const cv::Mat& imRectLeft,
+                               const cv::Mat& imRectRight,
+                               const double& timestamp, const string &filename,
+                               const Camera_ptr &cam);
+  Sophus::SE3f GrabImageRGBD(const cv::Mat& imRGB, const cv::Mat& imD,
+                             const double& timestamp, const string &filename,
+                             const Camera_ptr &cam);
+  Sophus::SE3f GrabImageMonocular(const cv::Mat& im, const double& timestamp,
+                                  const string &filename, const Camera_ptr &cam);
+
+  void GrabImuData(const IMU::Point& imuMeasurement);
+
+  void SetLocalMapper(LocalMapping* pLocalMapper);
+  void SetLoopClosing(LoopClosing* pLoopClosing);
+
+  // Load new settings
+  // The focal lenght should be similar or scale prediction will fail when
+  // projecting points
+  void ChangeCalibration(const string& strSettingPath);
+
+  // Use this function if you have deactivated local mapping and you only want
+  // to localize the camera.
+  void InformOnlyTracking(const bool& flag);
+
+  void UpdateFrameIMU(const float s, const IMU::Bias& b,
+                      KeyFrame* pCurrentKeyFrame);
+  KeyFrame* GetLastKeyFrame() { return mpLastKeyFrame; }
+
+  void CreateMapInAtlas();
+  // std::mutex mMutexTracks;
+
+  //--
+  void NewDataset();
+  int GetNumberDataset();
+  int GetMatchesInliers();
+
+  // DEBUG
+  void SaveSubTrajectory(string strNameFile_frames, string strNameFile_kf,
+                         string strFolder = "");
+  void SaveSubTrajectory(string strNameFile_frames, string strNameFile_kf,
+                         std::shared_ptr<Map> pMap);
+
+  float GetImageScale();
+
+#ifdef REGISTER_LOOP
+  void RequestStop();
+  bool isStopped();
+  void Release();
+  bool stopRequested();
+#endif
+
+ public:
+
+  Tracker::eTrackingState mState;
+  Tracker::eTrackingState mLastProcessedState;
+
+  // Input sensor
+  CameraType::eSensor mSensor;
+
+  // Current Frame
+  Frame mCurrentFrame;
+  Frame mLastFrame;
+
+  cv::Mat mImGray;
+
+  // Initialization Variables (Monocular)
+  std::vector<int> mvIniLastMatches;
+  std::vector<int> mvIniMatches;
+  std::vector<cv::Point2f> mvbPrevMatched;
+  std::vector<cv::Point3f> mvIniP3D;
+  Frame mInitialFrame;
+
+  // Lists used to recover the full camera trajectory at the end of the
+  // execution. Basically we store the reference keyframe for each frame and its
+  // relative transformation
+  list<Sophus::SE3f> mlRelativeFramePoses;
+  list<KeyFrame*> mlpReferences;
+  list<double> mlFrameTimes;
+  list<bool> mlbLost;
+
+  // frames with estimated pose
+  int mTrackedFr;
+  bool mbStep;
+
+  // True if local mapping is deactivated and we are performing only
+  // localization
+  bool mbOnlyTracking;
+
+  void Reset(bool bLocMap = false);
+  void ResetActiveMap(bool bLocMap = false);
+
+  float mMeanTrack;
+  bool mbInitWith3KFs;
+  double t0;     // time-stamp of first read frame
+  double t0vis;  // time-stamp of first inserted keyframe
+  double t0IMU;  // time-stamp of IMU initialization
+  bool mFastInit = false;
+
+  vector<MapPoint*> GetLocalMapMPS();
+
+  bool mbWriteStats;
+
+#ifdef REGISTER_TIMES
+  void LocalMapStats2File();
+  void TrackStats2File();
+  void PrintTimeStats();
+
+  vector<double> vdRectStereo_ms;
+  vector<double> vdResizeImage_ms;
+  vector<double> vdORBExtract_ms;
+  vector<double> vdStereoMatch_ms;
+  vector<double> vdIMUInteg_ms;
+  vector<double> vdPosePred_ms;
+  vector<double> vdLMTrack_ms;
+  vector<double> vdNewKF_ms;
+  vector<double> vdTrackTotal_ms;
+#endif
+
+ protected:
+  // Main tracking function. It is independent of the input sensor.
+  void Track();
+
+  // Map initialization for stereo and RGB-D
+  void StereoInitialization();
+
+  // Map initialization for monocular
+  void MonocularInitialization();
+  // void CreateNewMapPoints();
+  void CreateInitialMapMonocular();
+
+  void CheckReplacedInLastFrame();
+  bool TrackReferenceKeyFrame();
+  void UpdateLastFrame();
+  bool TrackWithMotionModel();
+  bool PredictStateIMU();
+
+  bool Relocalization();
+
+  void UpdateLocalMap();
+  void UpdateLocalPoints();
+  void UpdateLocalKeyFrames();
+
+  bool TrackLocalMap();
+  void SearchLocalPoints();
+
+  bool NeedNewKeyFrame();
+  void CreateNewKeyFrame();
+
+  // Perform preintegration from last frame
+  void PreintegrateIMU();
+
+  // Reset IMU biases and compute frame velocity
+  void ResetFrameIMU();
+
+  bool mbMapUpdated;
+
+  // Imu preintegration from last frame
+  IMU::Preintegrated* mpImuPreintegratedFromLastKF;
+
+  // Queue of IMU measurements between frames
+  std::list<IMU::Point> mlQueueImuData;
+
+  // Vector of IMU measurements from previous to current frame (to be filled by
+  // PreintegrateIMU)
+  std::vector<IMU::Point> mvImuFromLastFrame;
+  std::mutex mMutexImuQueue;
+
+  // Imu calibration parameters
+  std::shared_ptr<IMU::Calib> mpImuCalib;
+
+  // Last Bias Estimation (at keyframe creation)
+  IMU::Bias mLastBias;
+
+  // In case of performing only localization, this flag is true when there are
+  // no matches to points in the map. Still tracking will continue if there are
+  // enough matches with temporal points. In that case we are doing visual
+  // odometry. The system will try to do relocalization to recover "zero-drift"
+  // localization to the map.
+  bool mbVO;
+
+  // Other Thread Pointers
+  LocalMapping* mpLocalMapper;
+  LoopClosing* mpLoopClosing;
+
+  // ORB
+  std::shared_ptr<ORBextractor> mpORBextractorLeft;
+  std::shared_ptr<ORBextractor> mpORBextractorRight;
+  std::shared_ptr<ORBextractor> mpIniORBextractor;
+
+  // BoW
+  ORBVocabulary* mpORBVocabulary;
+  KeyFrameDatabase* mpKeyFrameDB;
+
+  // Initalization (only for monocular)
+  bool mbReadyToInitialize;
+  bool mbSetInit;
+
+  // Local Map
+  KeyFrame* mpReferenceKF;
+  std::vector<KeyFrame*> mvpLocalKeyFrames;
+  std::vector<MapPoint*> mvpLocalMapPoints;
+
+  // System
+  System* mpSystem;
+
+  // Atlas
+  Atlas_ptr mpAtlas;
+
+  // Calibration matrix
+  cv::Mat mK;
+  Eigen::Matrix3f mK_;
+  cv::Mat mDistCoef;
+  float mbf;
+  float mImageScale;
+
+  float mImuFreq;
+  double mImuPer;
+  bool mInsertKFsLost;
+
+  // New KeyFrame rules (according to fps)
+  int mMinFrames;
+  int mMaxFrames;
+
+  int mnFirstImuFrameId;
+  int mnFramesToResetIMU;
+
+  // Threshold close/far points
+  // Points seen as close by the stereo/RGBD sensor are considered reliable
+  // and inserted from just one frame. Far points requiere a match in two
+  // keyframes.
+  float mThDepth;
+
+  // For RGB-D inputs only. For some datasets (e.g. TUM) the depthmap values are
+  // scaled.
+  float mDepthMapFactor;
+
+  // Current matches in frame
+  int mnMatchesInliers;
+
+  // Last Frame, KeyFrame and Relocalisation Info
+  KeyFrame* mpLastKeyFrame;
+  unsigned int mnLastKeyFrameId;
+  unsigned int mnLastRelocFrameId;
+  double mTimeStampLost;
+  double time_recently_lost;
+
+  unsigned int mnFirstFrameId;
+  unsigned int mnInitialFrameId;
+  unsigned int mnLastInitFrameId;
+
+  bool mbCreatedMap;
+
+  // Motion Model
+  bool mbVelocity{false};
+  Sophus::SE3f mVelocity;
+
+  // Color order (true RGB, false BGR, ignored if grayscale)
+  bool mbRGB;
+
+  list<MapPoint*> mlpTemporalPoints;
+
+  // int nMapChangeIndex;
+
+  int mnNumDataset;
+
+  ofstream f_track_stats;
+
+  ofstream f_track_times;
+  double mTime_PreIntIMU;
+  double mTime_PosePred;
+  double mTime_LocalMapTrack;
+  double mTime_NewKF_Dec;
+
+  std::shared_ptr<GeometricCamera> mpCamera;
+  std::shared_ptr<GeometricCamera> mpCamera2;
+
+  int initID, lastID;
+
+  Sophus::SE3f mTlr;
+
+  void newParameterLoader(Settings* settings);
+
+#ifdef REGISTER_LOOP
+  bool Stop();
+
+  bool mbStopped;
+  bool mbStopRequested;
+  bool mbNotStop;
+  std::mutex mMutexStop;
+#endif
+
+ public:
+  cv::Mat mImRight;
+};
+
+}  // namespace MORB_SLAM
+
diff --git a/include/MORB_SLAM/TwoViewReconstruction.h b/include/MORB_SLAM/TwoViewReconstruction.h
new file mode 100644
index 00000000..92bb39de
--- /dev/null
+++ b/include/MORB_SLAM/TwoViewReconstruction.h
@@ -0,0 +1,96 @@
+/**
+* This file is part of ORB-SLAM3
+*
+* Copyright (C) 2017-2021 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/>.
+*/
+
+#pragma once
+
+#include <opencv2/opencv.hpp>
+#include <Eigen/Core>
+#include <unordered_set>
+
+#include <sophus/se3.hpp>
+
+namespace MORB_SLAM
+{
+
+    class TwoViewReconstruction
+    {
+        typedef std::pair<int,int> Match;
+
+    public:
+        
+        // Fix the reference frame
+        TwoViewReconstruction(const Eigen::Matrix3f& k, float sigma = 1.0, int iterations = 200);
+
+        // Computes in parallel a fundamental matrix and a homography
+        // Selects a model and tries to recover the motion and the structure from motion
+        bool Reconstruct(const std::vector<cv::KeyPoint>& vKeys1, const std::vector<cv::KeyPoint>& vKeys2, const std::vector<int> &vMatches12,
+                          Sophus::SE3f &T21, std::vector<cv::Point3f> &vP3D, std::vector<bool> &vbTriangulated);
+
+    private:
+
+        void FindHomography(std::vector<bool> &vbMatchesInliers, float &score, Eigen::Matrix3f &H21);
+        void FindFundamental(std::vector<bool> &vbInliers, float &score, Eigen::Matrix3f &F21);
+
+        Eigen::Matrix3f ComputeH21(const std::vector<cv::Point2f> &vP1, const std::vector<cv::Point2f> &vP2);
+        Eigen::Matrix3f ComputeF21(const std::vector<cv::Point2f> &vP1, const std::vector<cv::Point2f> &vP2);
+
+        float CheckHomography(const Eigen::Matrix3f &H21, const Eigen::Matrix3f &H12, std::vector<bool> &vbMatchesInliers, float sigma);
+
+        float CheckFundamental(const Eigen::Matrix3f &F21, std::vector<bool> &vbMatchesInliers, float sigma);
+
+        bool ReconstructF(std::vector<bool> &vbMatchesInliers, Eigen::Matrix3f &F21, Eigen::Matrix3f &K,
+                          Sophus::SE3f &T21, std::vector<cv::Point3f> &vP3D, std::vector<bool> &vbTriangulated, float minParallax, int minTriangulated);
+
+        bool ReconstructH(std::vector<bool> &vbMatchesInliers, Eigen::Matrix3f &H21, Eigen::Matrix3f &K,
+                          Sophus::SE3f &T21, std::vector<cv::Point3f> &vP3D,std:: vector<bool> &vbTriangulated, float minParallax, int minTriangulated);
+
+        void Normalize(const std::vector<cv::KeyPoint> &vKeys, std::vector<cv::Point2f> &vNormalizedPoints, Eigen::Matrix3f &T);
+
+
+        int CheckRT(const Eigen::Matrix3f &R, const Eigen::Vector3f &t, const std::vector<cv::KeyPoint> &vKeys1, const std::vector<cv::KeyPoint> &vKeys2,
+                    const std::vector<Match> &vMatches12, std::vector<bool> &vbMatchesInliers,
+                    const Eigen::Matrix3f &K, std::vector<cv::Point3f> &vP3D, float th2, std::vector<bool> &vbGood, float &parallax);
+
+        void DecomposeE(const Eigen::Matrix3f &E, Eigen::Matrix3f &R1, Eigen::Matrix3f &R2, Eigen::Vector3f &t);
+
+
+        // Keypoints from Reference Frame (Frame 1)
+        std::vector<cv::KeyPoint> mvKeys1;
+
+        // Keypoints from Current Frame (Frame 2)
+        std::vector<cv::KeyPoint> mvKeys2;
+
+        // Current Matches from Reference to Current
+        std::vector<Match> mvMatches12;
+        std::vector<bool> mvbMatched1;
+
+        // Calibration
+        Eigen::Matrix3f mK;
+
+        // Standard Deviation and Variance
+        float mSigma, mSigma2;
+
+        // Ransac max iterations
+        int mMaxIterations;
+
+        // Ransac sets
+        std::vector<std::vector<size_t> > mvSets;
+
+    };
+
+} //namespace ORB_SLAM
diff --git a/include/MORB_SLAM/Viewer.h b/include/MORB_SLAM/Viewer.h
new file mode 100644
index 00000000..c702ad28
--- /dev/null
+++ b/include/MORB_SLAM/Viewer.h
@@ -0,0 +1,77 @@
+/**
+ * This file is part of ORB-SLAM3
+ *
+ * Copyright (C) 2017-2021 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/>.
+ */
+
+#pragma once
+
+#include <mutex>
+#include <memory>
+
+#include "MORB_SLAM/ImprovedTypes.hpp"
+#include "MORB_SLAM/FrameDrawer.h"
+#include "MORB_SLAM/MapDrawer.h"
+#include "MORB_SLAM/Settings.h"
+
+namespace MORB_SLAM {
+
+class Viewer {
+  void newParameterLoader(const Settings& settings);
+  // Main thread function. Draw points, keyframes, the current camera pose and
+  // the last processed frame. Drawing is refreshed according to the camera fps.
+  // We use Pangolin.
+  void Run();
+ public:
+
+  Viewer(const System_ptr &pSystem, const std::string &strSettingPath);
+  Viewer(const System_ptr &pSystem, const Settings &settings);
+
+  virtual ~Viewer();
+
+  void update(const Sophus::SE3f &pose);
+
+  void close();
+  bool isClosed() const;
+  bool isOpen() const;
+
+
+ private:
+  void setBoth(const bool b);
+  bool ParseViewerParamFile(cv::FileStorage& fSettings);
+
+  bool Stop();
+
+  System_ptr mpSystem;
+  FrameDrawer mpFrameDrawer;
+  MapDrawer mpMapDrawer;
+  Tracking_ptr mpTracker;
+  std::thread mptViewer;
+  bool both;
+
+  // 1/fps in ms
+  double mT;
+  float mImageWidth, mImageHeight;
+  float mImageViewerScale;
+
+  float mViewpointX, mViewpointY, mViewpointZ, mViewpointF;
+
+  bool mbClosed;
+};
+
+}  // namespace MORB_SLAM