orbslam3 v0.4-beta fix for rtabmap integration

orbslam3_v4_rtabmap_fix.patch

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 3c731bb..e6d2c1c 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -9,17 +9,20 @@ MESSAGE("Build type: " ${CMAKE_BUILD_TYPE})
 
 set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS}  -Wall   -O3")
 set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall   -O3")
-set(CMAKE_C_FLAGS_RELEASE "${CMAKE_C_FLAGS_RELEASE} -march=native")
-set(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS_RELEASE} -march=native")
 
 # set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Wall -Wno-deprecated -O3 -march=native ")
 # set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wno-deprecated -O3 -march=native")
 
 # Check C++11 or C++0x support
 include(CheckCXXCompilerFlag)
+CHECK_CXX_COMPILER_FLAG("-std=c++14" COMPILER_SUPPORTS_CXX14)
 CHECK_CXX_COMPILER_FLAG("-std=c++11" COMPILER_SUPPORTS_CXX11)
 CHECK_CXX_COMPILER_FLAG("-std=c++0x" COMPILER_SUPPORTS_CXX0X)
-if(COMPILER_SUPPORTS_CXX11)
+if(COMPILER_SUPPORTS_CXX14)
+   set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++14")
+   add_definitions(-DCOMPILEDWITHC11)
+   message(STATUS "Using flag -std=c++14.")
+elseif(COMPILER_SUPPORTS_CXX11)
    set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11")
    add_definitions(-DCOMPILEDWITHC11)
    message(STATUS "Using flag -std=c++11.")
diff --git a/Thirdparty/DBoW2/CMakeLists.txt b/Thirdparty/DBoW2/CMakeLists.txt
index c561724..a5c1c8e 100644
--- a/Thirdparty/DBoW2/CMakeLists.txt
+++ b/Thirdparty/DBoW2/CMakeLists.txt
@@ -1,8 +1,8 @@
 cmake_minimum_required(VERSION 2.8)
 project(DBoW2)
 
-set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS}  -Wall  -O3 -march=native ")
-set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall  -O3 -march=native")
+set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS}  -Wall")
+set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall")
 
 set(HDRS_DBOW2
   DBoW2/BowVector.h
diff --git a/Thirdparty/g2o/CMakeLists.txt b/Thirdparty/g2o/CMakeLists.txt
index fa3e1e3..bbb243b 100644
--- a/Thirdparty/g2o/CMakeLists.txt
+++ b/Thirdparty/g2o/CMakeLists.txt
@@ -54,8 +54,6 @@ IF(OPENMP_FOUND AND G2O_USE_OPENMP)
 ENDIF(OPENMP_FOUND AND G2O_USE_OPENMP)
 
 # Compiler specific options for gcc
-SET(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS_RELEASE} -O3 -march=native")
-SET(CMAKE_C_FLAGS_RELEASE "${CMAKE_C_FLAGS_RELEASE} -O3 -march=native")
 # SET(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS_RELEASE} -O3")
 # SET(CMAKE_C_FLAGS_RELEASE "${CMAKE_C_FLAGS_RELEASE} -O3")
 
diff --git a/src/CameraModels/KannalaBrandt8.cpp b/src/CameraModels/KannalaBrandt8.cpp
index 6876ad5..6842e96 100644
--- a/src/CameraModels/KannalaBrandt8.cpp
+++ b/src/CameraModels/KannalaBrandt8.cpp
@@ -531,6 +531,6 @@ namespace ORB_SLAM3 {
 
         cv::SVD::compute(A,w,u,vt,cv::SVD::MODIFY_A| cv::SVD::FULL_UV);
         cv::Matx41f x3D_h = vt.row(3).t();
-        x3D = x3D_h.get_minor<3,1>(0,0) / x3D_h(3);
+        x3D = x3D_h.get_minor<3,1>(0,0) * (1.0f/x3D_h(3));
     }
 }
diff --git a/src/LocalMapping.cc b/src/LocalMapping.cc
index effa4e1..819efda 100644
--- a/src/LocalMapping.cc
+++ b/src/LocalMapping.cc
@@ -625,7 +625,7 @@ void LocalMapping::CreateNewMapPoints()
                     continue;
 
                 // Euclidean coordinates
-                x3D = x3D_h.get_minor<3,1>(0,0) / x3D_h(3);
+                x3D = x3D_h.get_minor<3,1>(0,0) * (1.0f/x3D_h(3));
                 bEstimated = true;
 
             }
diff --git a/src/Tracking.cc b/src/Tracking.cc
index 3ecefeb..ecdb6eb 100644
--- a/src/Tracking.cc
+++ b/src/Tracking.cc
@@ -946,7 +946,7 @@ bool Tracking::ParseCamParamFile(cv::FileStorage &fSettings)
         std::cerr << "Check an example configuration file with the desired sensor" << std::endl;
     }
 
-    if(mSensor==System::STEREO || mSensor==System::IMU_STEREO)
+    if(mSensor==System::STEREO || mSensor==System::RGBD || mSensor==System::IMU_STEREO)
     {
         cv::FileNode node = fSettings["Camera.bf"];
         if(!node.empty() && node.isReal())

Usage (tested on this version of ORB_SLAM3, and assuming Pangolin and wget are already installed):

git clone https://github.com/UZ-SLAMLab/ORB_SLAM3.git
cd ORB_SLAM3
# get this patch (you can also download with your browser, then put it in ORB_SLAM3 directory)
wget https://gist.githubusercontent.com/matlabbe/f5cb281304a1305b2824a6ce19792e13/raw/f8bbc796edc29b9f815cbf3c99a0c3e13e23663d/orbslam3_v4_rtabmap_fix.patch
git apply orbslam3_v4_rtabmap_fix.patch
./build.sh

If the patch is successfully applied, you would see those changes:

$ git status
On branch master
Your branch is up to date with 'origin/master'.

Changes not staged for commit:
  (use "git add <file>..." to update what will be committed)
  (use "git restore <file>..." to discard changes in working directory)
	modified:   CMakeLists.txt
	modified:   Thirdparty/DBoW2/CMakeLists.txt
	modified:   Thirdparty/g2o/CMakeLists.txt
	modified:   src/CameraModels/KannalaBrandt8.cpp
	modified:   src/LocalMapping.cc
	modified:   src/Tracking.cc

Untracked files:
  (use "git add <file>..." to include in what will be committed)
	orbslam3_v4_rtabmap_fix.patch

no changes added to commit (use "git add" and/or "git commit -a")