feat:调整CMake

CMakeLists.txt

@@ -14,12 +14,12 @@ set(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS_RELEASE} -march=native")
 
 # Check C++11 or C++0x support
 include(CheckCXXCompilerFlag)
-CHECK_CXX_COMPILER_FLAG("-std=c++11" COMPILER_SUPPORTS_CXX11)
+CHECK_CXX_COMPILER_FLAG("-std=c++14" COMPILER_SUPPORTS_CXX14)
 CHECK_CXX_COMPILER_FLAG("-std=c++0x" COMPILER_SUPPORTS_CXX0X)
 if(COMPILER_SUPPORTS_CXX11)
-   set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11")
+   set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++14")
    add_definitions(-DCOMPILEDWITHC11)
-   message(STATUS "Using flag -std=c++11.")
+   message(STATUS "Using flag -std=c++14.")
 elseif(COMPILER_SUPPORTS_CXX0X)
    set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x")
    add_definitions(-DCOMPILEDWITHC0X)
@@ -32,9 +32,9 @@ LIST(APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake_modules)
 
 # 要保证整个工程的opencv版本一致，包括dbow，源码以及ros相关的
 # 3 4 都可以正常运行
-find_package(OpenCV 3.2)
+find_package(OpenCV 4.2)
    if(NOT OpenCV_FOUND)
-      message(FATAL_ERROR "OpenCV > 3.2 not found.")
+      message(FATAL_ERROR "OpenCV > 4.2 not found.")
    endif()
 
 MESSAGE("OPENCV VERSION:")

Examples/ROS/ORB_SLAM3/CMakeLists.txt

@@ -14,12 +14,12 @@ set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall  -O3 -march=native")
 
 # Check C++11 or C++0x support
 include(CheckCXXCompilerFlag)
-CHECK_CXX_COMPILER_FLAG("-std=c++11" COMPILER_SUPPORTS_CXX11)
+CHECK_CXX_COMPILER_FLAG("-std=c++14" COMPILER_SUPPORTS_CXX14)
 CHECK_CXX_COMPILER_FLAG("-std=c++0x" COMPILER_SUPPORTS_CXX0X)
-if(COMPILER_SUPPORTS_CXX11)
-   set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11")
+if(COMPILER_SUPPORTS_CXX14)
+   set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++14")
    add_definitions(-DCOMPILEDWITHC11)
-   message(STATUS "Using flag -std=c++11.")
+   message(STATUS "Using flag -std=c++14.")
 elseif(COMPILER_SUPPORTS_CXX0X)
    set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x")
    add_definitions(-DCOMPILEDWITHC0X)
@@ -32,9 +32,9 @@ LIST(APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/../../../cmake_modules)
 
 # 要保证整个工程的opencv版本一致，包括dbow，源码以及ros相关的
 # 3 4 都可以正常运行
-find_package(OpenCV 3.2)
+find_package(OpenCV 4.2)
    if(NOT OpenCV_FOUND)
-      message(FATAL_ERROR "OpenCV > 3.2 not found.")
+      message(FATAL_ERROR "OpenCV > 4.2 not found.")
    endif()
 
 find_package(Eigen3 3.1.0 REQUIRED)

Examples/Stereo/KITTI00-02.yaml

@@ -57,7 +57,7 @@ Viewer.PointSize: 2.0
 Viewer.CameraSize: 0.7
 Viewer.CameraLineWidth: 3.0
 Viewer.ViewpointX: 0.0
-Viewer.ViewpointY: -100
+Viewer.ViewpointY: -100.0
 Viewer.ViewpointZ: -0.1
 Viewer.ViewpointF: 2000.0
 

README.md

@@ -1,12 +1,10 @@
 # ORB-SLAM3 超详细注释
 
--by 计算机视觉life 公众号旗下开源学习小组：SLAM研习社
-
 ![DEMO](https://github.com/electech6/ORB_SLAM3_detailed_comments/blob/master/demo.gif)
 
-![课程大纲](https://github.com/electech6/ORB_SLAM3_detailed_comments/blob/master/outline.png)
+独家视频课程《VIO灭霸：ORB-SLAM3公式推导+源码逐行解析+算法改进》！ [点击进入官网](https://cvlife.net/)
 
-[ORB-SLAM3 逐行源码讲解](https://cvlife.net/detail/term_6255372036a53_o5VfgR/25)
+点击进入 [小六的机器人SLAM学习圈](https://mp.weixin.qq.com/s?__biz=MzIxOTczOTM4NA==&mid=2247567299&idx=1&sn=2d6b673b6d5e396bd87ec2a28cd3d27d&chksm=97d52254a0a2ab42ff833b5fa733f2213a1ad1ec8ebdb199d96c237aa4cb21fe8e05cc77db8a#rd)，4500+人都在这里交流！
 
 ----
 # ORB-SLAM3

Thirdparty/DBoW2/CMakeLists.txt

@@ -30,13 +30,10 @@ set(SRCS_DUTILS
 
 # 要保证整个工程的opencv版本一致，包括dbow，源码以及ros相关的
 # 3 4 都可以正常运行
-find_package(OpenCV 3.2 QUIET)
-if(NOT OpenCV_FOUND)
-   find_package(OpenCV 3.0 QUIET)
+find_package(OpenCV 4.2 QUIET)
 if(NOT OpenCV_FOUND)
   message(FATAL_ERROR "OpenCV > 3.0 not found.")
 endif()
-endif()
 
 set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
 

include/LoopClosing.h

@@ -180,7 +180,7 @@ protected:
     std::vector<KeyFrame*> mvpCurrentConnectedKFs;
     std::vector<MapPoint*> mvpCurrentMatchedPoints;
     std::vector<MapPoint*> mvpLoopMapPoints;
-    cv::Mat mScw;
+
     g2o::Sim3 mg2oScw;
 
     //-------

include/MapPoint.h

@@ -123,12 +123,12 @@ public:
     int Observations();
 
     void AddObservation(KeyFrame* pKF,int idx);
-    void EraseObservation(KeyFrame* pKF);
+    void EraseObservation(KeyFrame* pKF, bool erase = true);
 
     std::tuple<int,int> GetIndexInKeyFrame(KeyFrame* pKF);
     bool IsInKeyFrame(KeyFrame* pKF);
 
-    void SetBadFlag();
+    void SetBadFlag(bool erase = true);
     bool isBad();
 
     void Replace(MapPoint* pMP);    

src/CameraModels/KannalaBrandt8.cpp

@@ -323,9 +323,11 @@ bool KannalaBrandt8::epipolarConstrain(GeometricCamera *pCamera2, const cv::KeyP
                                         const Eigen::Matrix3f &R12, const Eigen::Vector3f &t12, const float sigmaLevel, const float unc)
 {
     Eigen::Vector3f p3D;
+    // 用三角化出点并验证的这个过程代替极线验证
     return this->TriangulateMatches(pCamera2, kp1, kp2, R12, t12, sigmaLevel, unc, p3D) > 0.0001f;
 }
 
+// 没用
 bool KannalaBrandt8::matchAndtriangulate(const cv::KeyPoint &kp1, const cv::KeyPoint &kp2, GeometricCamera *pOther,
                                             Sophus::SE3f &Tcw1, Sophus::SE3f &Tcw2,
                                             const float sigmaLevel1, const float sigmaLevel2,
@@ -439,7 +441,7 @@ float KannalaBrandt8::TriangulateMatches(
     const Eigen::Matrix3f &R12, const Eigen::Vector3f &t12, const float sigmaLevel,
     const float unc, Eigen::Vector3f &p3D)
 {
-    // 1. 得到对应特征点的相平面坐标
+    // 1. 得到对应特征点的归一化平面坐标
     Eigen::Vector3f r1 = this->unprojectEig(kp1.pt);
     Eigen::Vector3f r2 = pCamera2->unprojectEig(kp2.pt);
 

src/G2oTypes.cc

@@ -831,8 +831,8 @@ void EdgeInertialGS::linearizeOplus()
     // Jacobians wrt scale factor
     // _jacobianOplus[3] 9*1矩阵 总体来说就是三个残差分别对尺度求导
     _jacobianOplus[7].setZero();
-    _jacobianOplus[7].block<3,1>(3,0) = Rbw1*(VV2->estimate()-VV1->estimate());
-    _jacobianOplus[7].block<3,1>(6,0) = Rbw1*(VP2->estimate().twb-VP1->estimate().twb-VV1->estimate()*dt);
+    _jacobianOplus[7].block<3,1>(3,0) = Rbw1*(VV2->estimate()-VV1->estimate()) * s;
+    _jacobianOplus[7].block<3,1>(6,0) = Rbw1*(VP2->estimate().twb-VP1->estimate().twb-VV1->estimate()*dt) * s;
 }
 
 /** 

src/LocalMapping.cc

@@ -1609,7 +1609,7 @@ void LocalMapping::InitializeIMU(float priorG, float priorA, bool bFIBA)
         dirG = dirG/dirG.norm();
         // 原本的重力方向
         Eigen::Vector3f gI(0.0f, 0.0f, -1.0f);
-        // 求重力在世界坐标系下的方向与重力在重力坐标系下的方向的叉乘
+        // 求“重力在重力坐标系下的方向”与的“重力在世界坐标系（纯视觉）下的方向”叉乘
         Eigen::Vector3f v = gI.cross(dirG);
         // 求叉乘模长
         const float nv = v.norm();

src/LoopClosing.cc

@@ -179,8 +179,8 @@ void LoopClosing::Run()
                             // If inertial, force only yaw
                             // 如果是imu模式并且完成了初始化,强制将焊接变换的 roll 和 pitch 设为0
                             // 通过物理约束来保证两个坐标轴都是水平的
-                            if ((mpTracker->mSensor==System::IMU_MONOCULAR ||mpTracker->mSensor==System::IMU_STEREO) &&
-                                   mpCurrentKF->GetMap()->GetIniertialBA1()) // TODO, maybe with GetIniertialBA1
+                            if ((mpTracker->mSensor==System::IMU_MONOCULAR || mpTracker->mSensor==System::IMU_STEREO || mpTracker->mSensor==System::IMU_RGBD) &&
+                                   mpCurrentKF->GetMap()->GetIniertialBA1())
                             {
                                 Eigen::Vector3d phi = LogSO3(mSold_new.rotation().toRotationMatrix());
                                 phi(0)=0;
@@ -693,8 +693,7 @@ bool LoopClosing::DetectAndReffineSim3FromLastKF(KeyFrame* pCurrentKF, KeyFrame*
         if(numOptMatches > nProjOptMatches)
         {
             //!bug, 以下gScw_estimation应该通过上述sim3优化后的位姿来更新。以下mScw应该改为 gscm * gswm^-1
-            g2o::Sim3 gScw_estimation(Converter::toMatrix3d(mScw.rowRange(0, 3).colRange(0, 3)),
-                           Converter::toVector3d(mScw.rowRange(0, 3).col(3)),1.0);
+            g2o::Sim3 gScw_estimation(gScw.rotation(), gScw.translation(),1.0);
 
             vector<MapPoint*> vpMatchedMP;
             vpMatchedMP.resize(mpCurrentKF->GetMapPointMatches().size(), static_cast<MapPoint*>(NULL));
@@ -1218,11 +1217,16 @@ int LoopClosing::FindMatchesByProjection(
                 {
                     spCheckKFs.insert(vpKFs[j]);
                     ++nInserted;
+
+                    // 改成这样
+                    vpCovKFm.push_back(vpKFs[j]);
                 }
                 ++j;
             }
+            // 这里是原来的代码，这么写不太合适，会出现重复
+            // 所以下面的插入可以改成放在if里面
             // 把每个帧的共视关键帧都加到窗口内
-            vpCovKFm.insert(vpCovKFm.end(), vpKFs.begin(), vpKFs.end());
+            // vpCovKFm.insert(vpCovKFm.end(), vpKFs.begin(), vpKFs.end());  // 已放上面
         }
     }
 
@@ -2275,8 +2279,8 @@ void LoopClosing::MergeLocal()
     // Essential graph 优化后可以重新开始局部建图了
     mpLocalMapper->Release();
 
-    // 全局的BA（永远不会执行）
-    // 这里没有imu, 所以isImuInitialized一定是false, 此时地图融合Atlas至少2个地图，所以第二个条件也一定是false
+    // 全局的BA，后面一串的判断都为true
+    // !pCurrentMap->isImuInitialized()一定是true
     // Step 9 全局BA
     if(bRelaunchBA && (!pCurrentMap->isImuInitialized() || (pCurrentMap->KeyFramesInMap()<200 && mpAtlas->CountMaps()==1)))
     {

src/Map.cc

@@ -311,6 +311,8 @@ void Map::ApplyScaledRotation(const Sophus::SE3f &T, const float s, const bool b
     {
         // 更新每一个mp在世界坐标系下的坐标
         MapPoint *pMP = *sit;
+        if (!pMP || pMP->isBad())
+            continue;
         pMP->SetWorldPos(s * Ryw * pMP->GetWorldPos() + tyw);
         pMP->UpdateNormalAndDepth();
     }
@@ -412,9 +414,9 @@ void Map::PreSave(std::set<GeometricCamera *> &spCams)
         map<KeyFrame *, std::tuple<int, int>> mpObs = pMPi->GetObservations();
         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 || it->first->GetMap() != this || it->first->isBad())
             {
-                pMPi->EraseObservation(it->first);
+                pMPi->EraseObservation(it->first, false);
             }
         }
     }

src/MapPoint.cc

@@ -201,7 +201,7 @@ void MapPoint::AddObservation(KeyFrame* pKF, int idx)
 }
 
 // 删除某个关键帧对当前地图点的观测
-void MapPoint::EraseObservation(KeyFrame* pKF)
+void MapPoint::EraseObservation(KeyFrame* pKF, bool erase)
 {
     bool bBad=false;
     {
@@ -236,7 +236,7 @@ void MapPoint::EraseObservation(KeyFrame* pKF)
     }
     // 告知可以观测到该MapPoint的Frame，该MapPoint已被删除
     if(bBad)
-        SetBadFlag();
+        SetBadFlag(erase);
 }
 
 // 能够观测到当前地图点的所有关键帧及该地图点在KF中的索引
@@ -260,7 +260,7 @@ int MapPoint::Observations()
  * @brief 告知可以观测到该MapPoint的Frame，该MapPoint已被删除
  * 
  */
-void MapPoint::SetBadFlag()
+void MapPoint::SetBadFlag(bool erase)
 {
     map<KeyFrame*, tuple<int,int>> obs;
     {
@@ -285,6 +285,7 @@ void MapPoint::SetBadFlag()
     }
 
     // 擦除该MapPoint申请的内存
+    if (erase)
         mpMap->EraseMapPoint(this);
 }
 
@@ -769,7 +770,9 @@ void MapPoint::PreSave(set<KeyFrame*>& spKF,set<MapPoint*>& spMP)
     mBackupObservationsId1.clear();
     mBackupObservationsId2.clear();
     // Save the id and position in each KF who view it
-    for(std::map<KeyFrame*,std::tuple<int,int> >::const_iterator it = mObservations.begin(), end = mObservations.end(); it != end; ++it)
+    std::vector<KeyFrame*> erase_kfs;
+    for(std::map<KeyFrame*,std::tuple<int,int> >::const_iterator it = mObservations.begin(), 
+        end = mObservations.end(); it != end; ++it)
     {
         KeyFrame* pKFi = it->first;
         if(spKF.find(pKFi) != spKF.end())
@@ -779,10 +782,11 @@ void MapPoint::PreSave(set<KeyFrame*>& spKF,set<MapPoint*>& spMP)
         }
         else
         {
-            EraseObservation(pKFi);
+            erase_kfs.push_back(pKFi); 
         }
     }
-
+    for (auto pKFi : erase_kfs)
+        EraseObservation(pKFi, false);
     // Save the id of the reference KF
     // 3. 备份参考关键帧ID
     if(spKF.find(mpRefKF) != spKF.end())

src/Settings.cc

@@ -30,113 +30,139 @@
 
 using namespace std;
 
-namespace ORB_SLAM3 {
+namespace ORB_SLAM3
+{
 
 template <>
-    float Settings::readParameter<float>(cv::FileStorage& fSettings, const std::string& name, bool& found, const bool required){
+float Settings::readParameter<float>(cv::FileStorage &fSettings, const std::string &name, bool &found, const bool required)
+{
     cv::FileNode node = fSettings[name];
-        if(node.empty()){
-            if(required){
+    if (node.empty())
+    {
+        if (required)
+        {
             std::cerr << name << " required parameter does not exist, aborting..." << std::endl;
             exit(-1);
         }
-            else{
+        else
+        {
             std::cerr << name << " optional parameter does not exist..." << std::endl;
             found = false;
             return 0.0f;
         }
     }
-        else if(!node.isReal()){
+    else if (!node.isReal())
+    {
         std::cerr << name << " parameter must be a real number, aborting..." << std::endl;
         exit(-1);
     }
-        else{
+    else
+    {
         found = true;
         return node.real();
     }
 }
 
 template <>
-    int Settings::readParameter<int>(cv::FileStorage& fSettings, const std::string& name, bool& found, const bool required){
+int Settings::readParameter<int>(cv::FileStorage &fSettings, const std::string &name, bool &found, const bool required)
+{
     cv::FileNode node = fSettings[name];
-        if(node.empty()){
-            if(required){
+    if (node.empty())
+    {
+        if (required)
+        {
             std::cerr << name << " required parameter does not exist, aborting..." << std::endl;
             exit(-1);
         }
-            else{
+        else
+        {
             std::cerr << name << " optional parameter does not exist..." << std::endl;
             found = false;
             return 0;
         }
     }
-        else if(!node.isInt()){
+    else if (!node.isInt())
+    {
         std::cerr << name << " parameter must be an integer number, aborting..." << std::endl;
         exit(-1);
     }
-        else{
+    else
+    {
         found = true;
         return node.operator int();
     }
 }
 
 template <>
-    string Settings::readParameter<string>(cv::FileStorage& fSettings, const std::string& name, bool& found, const bool required){
+string Settings::readParameter<string>(cv::FileStorage &fSettings, const std::string &name, bool &found, const bool required)
+{
     cv::FileNode node = fSettings[name];
-        if(node.empty()){
-            if(required){
+    if (node.empty())
+    {
+        if (required)
+        {
             std::cerr << name << " required parameter does not exist, aborting..." << std::endl;
             exit(-1);
         }
-            else{
+        else
+        {
             std::cerr << name << " optional parameter does not exist..." << std::endl;
             found = false;
             return string();
         }
     }
-        else if(!node.isString()){
+    else if (!node.isString())
+    {
         std::cerr << name << " parameter must be a string, aborting..." << std::endl;
         exit(-1);
     }
-        else{
+    else
+    {
         found = true;
         return node.string();
     }
 }
 
 template <>
-    cv::Mat Settings::readParameter<cv::Mat>(cv::FileStorage& fSettings, const std::string& name, bool& found, const bool required){
+cv::Mat Settings::readParameter<cv::Mat>(cv::FileStorage &fSettings, const std::string &name, bool &found, const bool required)
+{
     cv::FileNode node = fSettings[name];
-        if(node.empty()){
-            if(required){
+    if (node.empty())
+    {
+        if (required)
+        {
             std::cerr << name << " required parameter does not exist, aborting..." << std::endl;
             exit(-1);
         }
-            else{
+        else
+        {
             std::cerr << name << " optional parameter does not exist..." << std::endl;
             found = false;
             return cv::Mat();
         }
     }
-        else{
+    else
+    {
         found = true;
         return node.mat();
     }
 }
 
-    Settings::Settings(const std::string &configFile, const int& sensor) :
-    bNeedToUndistort_(false), bNeedToRectify_(false), bNeedToResize1_(false), bNeedToResize2_(false) {
+Settings::Settings(const std::string &configFile, const int &sensor) : bNeedToUndistort_(false), bNeedToRectify_(false), bNeedToResize1_(false), bNeedToResize2_(false)
+{
     sensor_ = sensor;
 
     // Open settings file
     cv::FileStorage fSettings(configFile, cv::FileStorage::READ);
-        if (!fSettings.isOpened()) {
+    if (!fSettings.isOpened())
+    {
         cerr << "[ERROR]: could not open configuration file at: " << configFile << endl;
         cerr << "Aborting..." << endl;
 
         exit(-1);
     }
-        else{
+    else
+    {
         cout << "Loading settings from " << configFile << endl;
     }
 
@@ -145,7 +171,8 @@ namespace ORB_SLAM3 {
     cout << "\t-Loaded camera 1" << endl;
 
     // Read second camera if stereo (not rectified)
-        if(sensor_ == System::STEREO || sensor_ == System::IMU_STEREO){
+    if (sensor_ == System::STEREO || sensor_ == System::IMU_STEREO)
+    {
         readCamera2(fSettings);
         cout << "\t-Loaded camera 2" << endl;
     }
@@ -154,12 +181,14 @@ namespace ORB_SLAM3 {
     readImageInfo(fSettings);
     cout << "\t-Loaded image info" << endl;
 
-        if(sensor_ == System::IMU_MONOCULAR || sensor_ == System::IMU_STEREO || sensor_ == System::IMU_RGBD){
+    if (sensor_ == System::IMU_MONOCULAR || sensor_ == System::IMU_STEREO || sensor_ == System::IMU_RGBD)
+    {
         readIMU(fSettings);
         cout << "\t-Loaded IMU calibration" << endl;
     }
 
-        if(sensor_ == System::RGBD || sensor_ == System::IMU_RGBD){
+    if (sensor_ == System::RGBD || sensor_ == System::IMU_RGBD)
+    {
         readRGBD(fSettings);
         cout << "\t-Loaded RGB-D calibration" << endl;
     }
@@ -173,7 +202,8 @@ namespace ORB_SLAM3 {
     readOtherParameters(fSettings);
     cout << "\t-Loaded misc parameters" << endl;
 
-        if(bNeedToRectify_){
+    if (bNeedToRectify_)
+    {
         precomputeRectificationMaps();
         cout << "\t-Computed rectification maps" << endl;
     }
@@ -181,14 +211,16 @@ namespace ORB_SLAM3 {
     cout << "----------------------------------" << endl;
 }
 
-    void Settings::readCamera1(cv::FileStorage &fSettings) {
+void Settings::readCamera1(cv::FileStorage &fSettings)
+{
     bool found;
 
     // Read camera model
     string cameraModel = readParameter<string>(fSettings, "Camera.type", found);
 
     vector<float> vCalibration;
-        if (cameraModel == "PinHole") {
+    if (cameraModel == "PinHole")
+    {
         cameraType_ = PinHole;
 
         // Read intrinsic parameters
@@ -204,13 +236,16 @@ namespace ORB_SLAM3 {
 
         // Check if it is a distorted PinHole
         readParameter<float>(fSettings, "Camera1.k1", found, false);
-            if(found){
+        if (found)
+        {
             readParameter<float>(fSettings, "Camera1.k3", found, false);
-                if(found){
+            if (found)
+            {
                 vPinHoleDistorsion1_.resize(5);
                 vPinHoleDistorsion1_[4] = readParameter<float>(fSettings, "Camera1.k3", found);
             }
-                else{
+            else
+            {
                 vPinHoleDistorsion1_.resize(4);
             }
             vPinHoleDistorsion1_[0] = readParameter<float>(fSettings, "Camera1.k1", found);
@@ -220,11 +255,18 @@ namespace ORB_SLAM3 {
         }
 
         // Check if we need to correct distortion from the images
-            if((sensor_ == System::MONOCULAR || sensor_ == System::IMU_MONOCULAR) && vPinHoleDistorsion1_.size() != 0){
+        if (
+            (sensor_ == System::MONOCULAR ||
+                sensor_ == System::IMU_MONOCULAR ||
+                sensor_ == System::RGBD ||
+                sensor_ == System::IMU_RGBD) &&
+            vPinHoleDistorsion1_.size() != 0)
+        {
             bNeedToUndistort_ = true;
         }
     }
-        else if(cameraModel == "Rectified"){
+    else if (cameraModel == "Rectified")
+    {
         cameraType_ = Rectified;
 
         // Read intrinsic parameters
@@ -240,7 +282,8 @@ namespace ORB_SLAM3 {
 
         // Rectified images are assumed to be ideal PinHole images (no distortion)
     }
-        else if(cameraModel == "KannalaBrandt8"){
+    else if (cameraModel == "KannalaBrandt8")
+    {
         cameraType_ = KannalaBrandt;
 
         // Read intrinsic parameters
@@ -259,7 +302,8 @@ namespace ORB_SLAM3 {
         calibration1_ = new KannalaBrandt8(vCalibration);
         originalCalib1_ = new KannalaBrandt8(vCalibration);
 
-            if(sensor_ == System::STEREO || sensor_ == System::IMU_STEREO){
+        if (sensor_ == System::STEREO || sensor_ == System::IMU_STEREO)
+        {
             int colBegin = readParameter<int>(fSettings, "Camera1.overlappingBegin", found);
             int colEnd = readParameter<int>(fSettings, "Camera1.overlappingEnd", found);
             vector<int> vOverlapping = {colBegin, colEnd};
@@ -267,16 +311,19 @@ namespace ORB_SLAM3 {
             static_cast<KannalaBrandt8 *>(calibration1_)->mvLappingArea = vOverlapping;
         }
     }
-        else{
+    else
+    {
         cerr << "Error: " << cameraModel << " not known" << endl;
         exit(-1);
     }
 }
 
-    void Settings::readCamera2(cv::FileStorage &fSettings) {
+void Settings::readCamera2(cv::FileStorage &fSettings)
+{
     bool found;
     vector<float> vCalibration;
-        if (cameraType_ == PinHole) {
+    if (cameraType_ == PinHole)
+    {
         bNeedToRectify_ = true;
 
         // Read intrinsic parameters
@@ -285,7 +332,6 @@ namespace ORB_SLAM3 {
         float cx = readParameter<float>(fSettings, "Camera2.cx", found);
         float cy = readParameter<float>(fSettings, "Camera2.cy", found);
 
-
         vCalibration = {fx, fy, cx, cy};
 
         calibration2_ = new Pinhole(vCalibration);
@@ -293,13 +339,16 @@ namespace ORB_SLAM3 {
 
         // Check if it is a distorted PinHole
         readParameter<float>(fSettings, "Camera2.k1", found, false);
-            if(found){
+        if (found)
+        {
             readParameter<float>(fSettings, "Camera2.k3", found, false);
-                if(found){
+            if (found)
+            {
                 vPinHoleDistorsion2_.resize(5);
                 vPinHoleDistorsion2_[4] = readParameter<float>(fSettings, "Camera2.k3", found);
             }
-                else{
+            else
+            {
                 vPinHoleDistorsion2_.resize(4);
             }
             vPinHoleDistorsion2_[0] = readParameter<float>(fSettings, "Camera2.k1", found);
@@ -308,7 +357,8 @@ namespace ORB_SLAM3 {
             vPinHoleDistorsion2_[3] = readParameter<float>(fSettings, "Camera2.p2", found);
         }
     }
-        else if(cameraType_ == KannalaBrandt){
+    else if (cameraType_ == KannalaBrandt)
+    {
         // Read intrinsic parameters
         float fx = readParameter<float>(fSettings, "Camera2.fx", found);
         float fy = readParameter<float>(fSettings, "Camera2.fy", found);
@@ -320,7 +370,6 @@ namespace ORB_SLAM3 {
         float k2 = readParameter<float>(fSettings, "Camera1.k3", found);
         float k3 = readParameter<float>(fSettings, "Camera1.k4", found);
 
-
         vCalibration = {fx, fy, cx, cy, k0, k1, k2, k3};
 
         calibration2_ = new KannalaBrandt8(vCalibration);
@@ -334,11 +383,13 @@ namespace ORB_SLAM3 {
     }
 
     // Load stereo extrinsic calibration
-        if(cameraType_ == Rectified){
+    if (cameraType_ == Rectified)
+    {
         b_ = readParameter<float>(fSettings, "Stereo.b", found);
         bf_ = b_ * calibration1_->getParameter(0);
     }
-        else{
+    else
+    {
         cv::Mat cvTlr = readParameter<cv::Mat>(fSettings, "Stereo.T_c1_c2", found);
         Tlr_ = Converter::toSophus(cvTlr);
 
@@ -349,11 +400,10 @@ namespace ORB_SLAM3 {
     }
 
     thDepth_ = readParameter<float>(fSettings, "Stereo.ThDepth", found);
-
-
 }
 
-    void Settings::readImageInfo(cv::FileStorage &fSettings) {
+void Settings::readImageInfo(cv::FileStorage &fSettings)
+{
     bool found;
     // Read original and desired image dimensions
     int originalRows = readParameter<int>(fSettings, "Camera.height", found);
@@ -363,18 +413,20 @@ namespace ORB_SLAM3 {
 
     newImSize_ = originalImSize_;
     int newHeigh = readParameter<int>(fSettings, "Camera.newHeight", found, false);
-        if(found){
+    if (found)
+    {
         bNeedToResize1_ = true;
         newImSize_.height = newHeigh;
 
-            if(!bNeedToRectify_){
+        if (!bNeedToRectify_)
+        {
             // Update calibration
             float scaleRowFactor = (float)newImSize_.height / (float)originalImSize_.height;
             calibration1_->setParameter(calibration1_->getParameter(1) * scaleRowFactor, 1);
             calibration1_->setParameter(calibration1_->getParameter(3) * scaleRowFactor, 3);
 
-
-                if((sensor_ == System::STEREO || sensor_ == System::IMU_STEREO) && cameraType_ != Rectified){
+            if ((sensor_ == System::STEREO || sensor_ == System::IMU_STEREO) && cameraType_ != Rectified)
+            {
                 calibration2_->setParameter(calibration2_->getParameter(1) * scaleRowFactor, 1);
                 calibration2_->setParameter(calibration2_->getParameter(3) * scaleRowFactor, 3);
             }
@@ -382,21 +434,25 @@ namespace ORB_SLAM3 {
     }
 
     int newWidth = readParameter<int>(fSettings, "Camera.newWidth", found, false);
-        if(found){
+    if (found)
+    {
         bNeedToResize1_ = true;
         newImSize_.width = newWidth;
 
-            if(!bNeedToRectify_){
+        if (!bNeedToRectify_)
+        {
             // Update calibration
             float scaleColFactor = (float)newImSize_.width / (float)originalImSize_.width;
             calibration1_->setParameter(calibration1_->getParameter(0) * scaleColFactor, 0);
             calibration1_->setParameter(calibration1_->getParameter(2) * scaleColFactor, 2);
 
-                if((sensor_ == System::STEREO || sensor_ == System::IMU_STEREO) && cameraType_ != Rectified){
+            if ((sensor_ == System::STEREO || sensor_ == System::IMU_STEREO) && cameraType_ != Rectified)
+            {
                 calibration2_->setParameter(calibration2_->getParameter(0) * scaleColFactor, 0);
                 calibration2_->setParameter(calibration2_->getParameter(2) * scaleColFactor, 2);
 
-                    if(cameraType_ == KannalaBrandt){
+                if (cameraType_ == KannalaBrandt)
+                {
                     static_cast<KannalaBrandt8 *>(calibration1_)->mvLappingArea[0] *= scaleColFactor;
                     static_cast<KannalaBrandt8 *>(calibration1_)->mvLappingArea[1] *= scaleColFactor;
 
@@ -411,7 +467,8 @@ namespace ORB_SLAM3 {
     bRGB_ = (bool)readParameter<int>(fSettings, "Camera.RGB", found);
 }
 
-    void Settings::readIMU(cv::FileStorage &fSettings) {
+void Settings::readIMU(cv::FileStorage &fSettings)
+{
     bool found;
     noiseGyro_ = readParameter<float>(fSettings, "IMU.NoiseGyro", found);
     noiseAcc_ = readParameter<float>(fSettings, "IMU.NoiseAcc", found);
@@ -423,15 +480,18 @@ namespace ORB_SLAM3 {
     Tbc_ = Converter::toSophus(cvTbc);
 
     readParameter<int>(fSettings, "IMU.InsertKFsWhenLost", found, false);
-        if(found){
+    if (found)
+    {
         insertKFsWhenLost_ = (bool)readParameter<int>(fSettings, "IMU.InsertKFsWhenLost", found, false);
     }
-        else{
+    else
+    {
         insertKFsWhenLost_ = true;
     }
 }
 
-    void Settings::readRGBD(cv::FileStorage& fSettings) {
+void Settings::readRGBD(cv::FileStorage &fSettings)
+{
     bool found;
 
     depthMapFactor_ = readParameter<float>(fSettings, "RGBD.DepthMapFactor", found);
@@ -440,7 +500,8 @@ namespace ORB_SLAM3 {
     bf_ = b_ * calibration1_->getParameter(0);
 }
 
-    void Settings::readORB(cv::FileStorage &fSettings) {
+void Settings::readORB(cv::FileStorage &fSettings)
+{
     bool found;
 
     nFeatures_ = readParameter<int>(fSettings, "ORBextractor.nFeatures", found);
@@ -450,7 +511,8 @@ namespace ORB_SLAM3 {
     minThFAST_ = readParameter<int>(fSettings, "ORBextractor.minThFAST", found);
 }
 
-    void Settings::readViewer(cv::FileStorage &fSettings) {
+void Settings::readViewer(cv::FileStorage &fSettings)
+{
     bool found;
 
     keyFrameSize_ = readParameter<float>(fSettings, "Viewer.KeyFrameSize", found);
@@ -469,20 +531,23 @@ namespace ORB_SLAM3 {
         imageViewerScale_ = 1.0f;
 }
 
-    void Settings::readLoadAndSave(cv::FileStorage &fSettings) {
+void Settings::readLoadAndSave(cv::FileStorage &fSettings)
+{
     bool found;
 
     sLoadFrom_ = readParameter<string>(fSettings, "System.LoadAtlasFromFile", found, false);
     sSaveto_ = readParameter<string>(fSettings, "System.SaveAtlasToFile", found, false);
 }
 
-    void Settings::readOtherParameters(cv::FileStorage& fSettings) {
+void Settings::readOtherParameters(cv::FileStorage &fSettings)
+{
     bool found;
 
     thFarPoints_ = readParameter<float>(fSettings, "System.thFarPoints", found, false);
 }
 
-    void Settings::precomputeRectificationMaps() {
+void Settings::precomputeRectificationMaps()
+{
     // Precompute rectification maps, new calibrations, ...
     cv::Mat K1 = static_cast<Pinhole *>(calibration1_)->toK();
     K1.convertTo(K1, CV_64F);
@@ -518,7 +583,8 @@ namespace ORB_SLAM3 {
     bf_ = b_ * P1.at<double>(0, 0);
 
     // Update relative pose between camera 1 and IMU if necessary
-        if(sensor_ == System::IMU_STEREO){
+    if (sensor_ == System::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());
@@ -526,74 +592,97 @@ namespace ORB_SLAM3 {
     }
 }
 
-    ostream &operator<<(std::ostream& output, const Settings& settings){
+ostream &operator<<(std::ostream &output, const Settings &settings)
+{
     output << "SLAM settings: " << endl;
 
     output << "\t-Camera 1 parameters (";
-        if(settings.cameraType_ == Settings::PinHole || settings.cameraType_ ==  Settings::Rectified){
+    if (settings.cameraType_ == Settings::PinHole || settings.cameraType_ == Settings::Rectified)
+    {
         output << "Pinhole";
     }
-        else{
+    else
+    {
         output << "Kannala-Brandt";
     }
-        output << ")" << ": [";
-        for(size_t i = 0; i < settings.originalCalib1_->size(); i++){
+    output << ")"
+            << ": [";
+    for (size_t i = 0; i < settings.originalCalib1_->size(); i++)
+    {
         output << " " << settings.originalCalib1_->getParameter(i);
     }
     output << " ]" << endl;
 
-        if(!settings.vPinHoleDistorsion1_.empty()){
+    if (!settings.vPinHoleDistorsion1_.empty())
+    {
         output << "\t-Camera 1 distortion parameters: [ ";
-            for(float d : settings.vPinHoleDistorsion1_){
+        for (float d : settings.vPinHoleDistorsion1_)
+        {
             output << " " << d;
         }
         output << " ]" << endl;
     }
 
-        if(settings.sensor_ == System::STEREO || settings.sensor_ == System::IMU_STEREO){
+    if (settings.sensor_ == System::STEREO || settings.sensor_ == System::IMU_STEREO)
+    {
+        if (settings.cameraType_ != Settings::Rectified)
+        {
             output << "\t-Camera 2 parameters (";
-            if(settings.cameraType_ == Settings::PinHole || settings.cameraType_ ==  Settings::Rectified){
+            if (settings.cameraType_ == Settings::PinHole)
+            {
                 output << "Pinhole";
             }
-            else{
+            else
+            {
                 output << "Kannala-Brandt";
             }
-            output << "" << ": [";
-            for(size_t i = 0; i < settings.originalCalib2_->size(); i++){
+            output << ""
+                    << ": [";
+            for (size_t i = 0; i < settings.originalCalib2_->size(); i++)
+            {
                 output << " " << settings.originalCalib2_->getParameter(i);
             }
             output << " ]" << endl;
 
-            if(!settings.vPinHoleDistorsion2_.empty()){
+            if (!settings.vPinHoleDistorsion2_.empty())
+            {
                 output << "\t-Camera 1 distortion parameters: [ ";
-                for(float d : settings.vPinHoleDistorsion2_){
+                for (float d : settings.vPinHoleDistorsion2_)
+                {
                     output << " " << d;
                 }
                 output << " ]" << endl;
             }
         }
+    }
 
     output << "\t-Original image size: [ " << settings.originalImSize_.width << " , " << settings.originalImSize_.height << " ]" << endl;
     output << "\t-Current image size: [ " << settings.newImSize_.width << " , " << settings.newImSize_.height << " ]" << endl;
 
-        if(settings.bNeedToRectify_){
+    if (settings.bNeedToRectify_)
+    {
         output << "\t-Camera 1 parameters after rectification: [ ";
-            for(size_t i = 0; i < settings.calibration1_->size(); i++){
+        for (size_t i = 0; i < settings.calibration1_->size(); i++)
+        {
             output << " " << settings.calibration1_->getParameter(i);
         }
         output << " ]" << endl;
     }
-        else if(settings.bNeedToResize1_){
+    else if (settings.bNeedToResize1_)
+    {
         output << "\t-Camera 1 parameters after resize: [ ";
-            for(size_t i = 0; i < settings.calibration1_->size(); i++){
+        for (size_t i = 0; i < settings.calibration1_->size(); i++)
+        {
             output << " " << settings.calibration1_->getParameter(i);
         }
         output << " ]" << endl;
 
         if ((settings.sensor_ == System::STEREO || settings.sensor_ == System::IMU_STEREO) &&
-                settings.cameraType_ == Settings::KannalaBrandt){
+            settings.cameraType_ == Settings::KannalaBrandt)
+        {
             output << "\t-Camera 2 parameters after resize: [ ";
-                for(size_t i = 0; i < settings.calibration2_->size(); i++){
+            for (size_t i = 0; i < settings.calibration2_->size(); i++)
+            {
                 output << " " << settings.calibration2_->getParameter(i);
             }
             output << " ]" << endl;
@@ -603,11 +692,13 @@ namespace ORB_SLAM3 {
     output << "\t-Sequence FPS: " << settings.fps_ << endl;
 
     // Stereo stuff
-        if(settings.sensor_ == System::STEREO || settings.sensor_ == System::IMU_STEREO){
+    if (settings.sensor_ == System::STEREO || settings.sensor_ == System::IMU_STEREO)
+    {
         output << "\t-Stereo baseline: " << settings.b_ << endl;
         output << "\t-Stereo depth threshold : " << settings.thDepth_ << endl;
 
-            if(settings.cameraType_ == Settings::KannalaBrandt){
+        if (settings.cameraType_ == Settings::KannalaBrandt)
+        {
             auto vOverlapping1 = static_cast<KannalaBrandt8 *>(settings.calibration1_)->mvLappingArea;
             auto vOverlapping2 = static_cast<KannalaBrandt8 *>(settings.calibration2_)->mvLappingArea;
             output << "\t-Camera 1 overlapping area: [ " << vOverlapping1[0] << " , " << vOverlapping1[1] << " ]" << endl;
@@ -615,7 +706,8 @@ namespace ORB_SLAM3 {
         }
     }
 
-        if(settings.sensor_ == System::IMU_MONOCULAR || settings.sensor_ == System::IMU_STEREO || settings.sensor_ == System::IMU_RGBD) {
+    if (settings.sensor_ == System::IMU_MONOCULAR || settings.sensor_ == System::IMU_STEREO || settings.sensor_ == System::IMU_RGBD)
+    {
         output << "\t-Gyro noise: " << settings.noiseGyro_ << endl;
         output << "\t-Accelerometer noise: " << settings.noiseAcc_ << endl;
         output << "\t-Gyro walk: " << settings.gyroWalk_ << endl;
@@ -623,7 +715,8 @@ namespace ORB_SLAM3 {
         output << "\t-IMU frequency: " << settings.imuFrequency_ << endl;
     }
 
-        if(settings.sensor_ == System::RGBD || settings.sensor_ == System::IMU_RGBD){
+    if (settings.sensor_ == System::RGBD || settings.sensor_ == System::IMU_RGBD)
+    {
         output << "\t-RGB-D depth map factor: " << settings.depthMapFactor_ << endl;
     }