Added ORB_SLAM3 support. IMU Filter: added base frame conversion option. (#698)

Referred issues: #655 https://github.com/introlab/rtabmap_ros/issues/492 Note: IMU not supported yet with ORB_SLAM3. Commits: * Added orbslam3 support. UI-Source->IMU filtering: Added base frame conversion option of IMU data to uniformize yaw initialization. Madgwick: fixed yaw initialization accordingly to Z acc. * fixed regression build error with ORB_SLAM2 * Renamed OdometryORBSLAM2 to OdometryORBSLAM (can be 2 or 3 now)
2026-09-13 23:10:20 +08:00 · 2021-03-09 16:00:41 -05:00
parent 736c8aceae
commit db43479e44
26 changed files with 637 additions and 315 deletions
@@ -196,7 +196,7 @@ option(WITH_ALICE_VISION  "Include AliceVision support"          OFF)
 option(WITH_FOVIS         "Include FOVIS support"                ON)
 option(WITH_VISO2         "Include VISO2 support"                ON)
 option(WITH_DVO           "Include DVO support"                  ON)
-option(WITH_ORB_SLAM2     "Include ORB_SLAM2 support"            ON)
+option(WITH_ORB_SLAM      "Include ORB_SLAM2 or ORB_SLAM3  support" ON)
 option(WITH_OKVIS         "Include OKVIS support"                ON)
 option(WITH_MSCKF_VIO     "Include MSCKF_VIO support"           OFF)
 option(WITH_VINS          "Include VINS-Fusion support"          ON)
@@ -634,21 +634,12 @@ IF(WITH_FASTCV)
    ENDIF(FastCV_FOUND)
 ENDIF(WITH_FASTCV)

-IF(WITH_ORB_SLAM2 AND NOT G2O_FOUND)
-    FIND_PACKAGE(ORB_SLAM2 QUIET)
-    IF(ORB_SLAM2_FOUND)
-       MESSAGE(STATUS "Found ORB_SLAM2: ${ORB_SLAM2_INCLUDE_DIRS}")
-       FIND_PACKAGE(Pangolin QUIET)
-       IF(NOT Pangolin_FOUND)
-          SET(ORB_SLAM2_FOUND FALSE)
-          MESSAGE(STATUS "Found ORB_SLAM2 but not Pangolin, disabling ORB_SLAM2.")
-       ELSE()
-          MESSAGE(STATUS "Found Pangolin: ${Pangolin_INCLUDE_DIRS}")
-          SET(ORB_SLAM2_INCLUDE_DIRS ${ORB_SLAM2_INCLUDE_DIRS} ${Pangolin_INCLUDE_DIRS})
-          SET(ORB_SLAM2_LIBRARIES ${ORB_SLAM2_LIBRARIES} ${Pangolin_LIBRARIES})
-       ENDIF()
-    ENDIF(ORB_SLAM2_FOUND)
-ENDIF(WITH_ORB_SLAM2 AND NOT G2O_FOUND)
+IF(WITH_ORB_SLAM AND NOT G2O_FOUND)
+    FIND_PACKAGE(ORB_SLAM QUIET)
+    IF(ORB_SLAM_FOUND)
+       MESSAGE(STATUS "Found ORB_SLAM${ORB_SLAM_VERSION}: ${ORB_SLAM_INCLUDE_DIRS}")
+    ENDIF(ORB_SLAM_FOUND)
+ENDIF(WITH_ORB_SLAM AND NOT G2O_FOUND)

 IF(NOT MSVC)
 	IF(loam_velodyne_FOUND OR PCL_VERSION VERSION_GREATER "1.9.1" OR TORCH_FOUND OR G2O_FOUND)
@@ -672,7 +663,7 @@ IF(NOT MSVC)
 	       ANDROID OR
 	       RealSense_FOUND OR
 	       realsense2_FOUND OR
-	       ORB_SLAM2_FOUND OR
+	       ORB_SLAM_FOUND OR
 	       okvis_FOUND OR
 	       open_chisel_FOUND OR
 	       msckf_vio_FOUND OR
@@ -894,10 +885,10 @@ IF(NOT vins_FOUND)
 ELSE()
   SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${vins_LIBRARIES}) 
 ENDIF()
-IF(NOT ORB_SLAM2_FOUND)
-   SET(ORB_SLAM2 "//")
+IF(NOT ORB_SLAM_FOUND)
+   SET(ORB_SLAM "//")
 ELSE()
-   SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${ORB_SLAM2_LIBRARIES}) 
+   SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${ORB_SLAM_LIBRARIES})
 ENDIF()
 IF(NOT WITH_ORB_OCTREE)
   SET(ORB_OCTREE "//")
@@ -1468,14 +1459,14 @@ ELSE()
 MESSAGE(STATUS "  With VINS-Fusion          = NO (VINS-Fusion not found)")
 ENDIF()

-IF(ORB_SLAM2_FOUND)
-MESSAGE(STATUS "  With ORB_SLAM2            = YES (License: GPLv3)")
-ELSEIF(NOT WITH_ORB_SLAM2)
-MESSAGE(STATUS "  With ORB_SLAM2            = NO (WITH_ORB_SLAM2=OFF)")
+IF(ORB_SLAM_FOUND)
+MESSAGE(STATUS "  With ORB_SLAM${ORB_SLAM_VERSION}            = YES (License: GPLv3)")
+ELSEIF(NOT WITH_ORB_SLAM)
+MESSAGE(STATUS "  With ORB_SLAM             = NO (WITH_ORB_SLAM=OFF)")
 ELSEIF(G2O_FOUND)
-MESSAGE(STATUS "  With ORB_SLAM2            = NO (WITH_G2O should be OFF as ORB_SLAM2 uses its own g2o version)")
+MESSAGE(STATUS "  With ORB_SLAM             = NO (WITH_G2O should be OFF as ORB_SLAM uses its own g2o version)")
 ELSE()
-MESSAGE(STATUS "  With ORB_SLAM2            = NO (ORB_SLAM2 not found, make sure environment variable ORB_SLAM2_ROOT_DIR is set)")
+MESSAGE(STATUS "  With ORB_SLAM             = NO (ORB_SLAM2 and ORB_SLAM3 not found, make sure environment variable ORB_SLAM_ROOT_DIR is set)")
 ENDIF()

 MESSAGE(STATUS "Show all options with: cmake -LA | grep WITH_")
@@ -73,7 +73,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
@OKVIS@#define RTABMAP_OKVIS
@MSCKF_VIO@#define RTABMAP_MSCKF_VIO
@VINS@#define RTABMAP_VINS
-@ORB_SLAM2@#define RTABMAP_ORB_SLAM2
+@ORB_SLAM@#define RTABMAP_ORB_SLAM @ORB_SLAM_VERSION@
@ORB_OCTREE@#define RTABMAP_ORB_OCTREE
@TORCH@#define RTABMAP_TORCH
@PYTHON@#define RTABMAP_PYTHON
@@ -0,0 +1,53 @@
+# - Find ORB_SLAM2 OR ORB_SLAM3
+#
+# It sets the following variables:
+#  ORB_SLAM_FOUND         - Set to false, or undefined, if ORB_SLAM isn't found.
+#  ORB_SLAM_INCLUDE_DIRS  - The ORB_SLAM include directory.
+#  ORB_SLAM_LIBRARIES     - The ORB_SLAM library to link against.
+#  ORB_SLAM_VERSION       - The ORB_SLAM major version.
+#
+#  Set ORB_SLAM_ROOT_DIR environment variable as the path to ORB_SLAM2 or ORB_SLAM3 root folder.
+
+find_path(ORB_SLAM_INCLUDE_DIR NAMES System.h PATHS $ENV{ORB_SLAM_ROOT_DIR}/include)
+find_library(ORB_SLAM2_LIBRARY NAMES ORB_SLAM2 PATHS $ENV{ORB_SLAM_ROOT_DIR}/lib)
+find_library(ORB_SLAM3_LIBRARY NAMES ORB_SLAM3 PATHS $ENV{ORB_SLAM_ROOT_DIR}/lib)
+find_path(g2o_INCLUDE_DIR NAMES g2o/core/sparse_optimizer.h PATHS $ENV{ORB_SLAM_ROOT_DIR}/Thirdparty/g2o NO_DEFAULT_PATH)
+find_library(g2o_LIBRARY NAMES g2o PATHS $ENV{ORB_SLAM_ROOT_DIR}/Thirdparty/g2o/lib NO_DEFAULT_PATH)
+find_library(DBoW2_LIBRARY NAMES DBoW2 PATHS $ENV{ORB_SLAM_ROOT_DIR}/Thirdparty/DBoW2/lib NO_DEFAULT_PATH)
+
+IF(ORB_SLAM2_LIBRARY)
+   SET(ORB_SLAM_VERSION 2)
+   SET(ORB_SLAM_LIBRARY ${ORB_SLAM2_LIBRARY})
+ELSEIF(ORB_SLAM3_LIBRARY)
+   SET(ORB_SLAM_VERSION 3)
+   SET(ORB_SLAM_LIBRARY ${ORB_SLAM3_LIBRARY})
+ENDIF()
+
+IF (ORB_SLAM_INCLUDE_DIR AND ORB_SLAM_LIBRARY AND DBoW2_LIBRARY AND g2o_INCLUDE_DIR AND g2o_LIBRARY)
+   SET(ORB_SLAM_FOUND TRUE)
+   SET(ORB_SLAM_INCLUDE_DIRS ${ORB_SLAM_INCLUDE_DIR} ${ORB_SLAM_INCLUDE_DIR}/CameraModels ${g2o_INCLUDE_DIR} $ENV{ORB_SLAM_ROOT_DIR})
+   SET(ORB_SLAM_LIBRARIES ${g2o_LIBRARY} ${ORB_SLAM_LIBRARY} ${DBoW2_LIBRARY})
+ENDIF (ORB_SLAM_INCLUDE_DIR AND ORB_SLAM_LIBRARY AND DBoW2_LIBRARY AND g2o_INCLUDE_DIR AND g2o_LIBRARY)
+
+FIND_PACKAGE(Pangolin QUIET)
+IF(NOT Pangolin_FOUND)
+  SET(ORB_SLAM_FOUND FALSE)
+  MESSAGE(STATUS "Found ORB_SLAM but not Pangolin, disabling ORB_SLAM.")
+ELSE()
+  MESSAGE(STATUS "Found Pangolin: ${Pangolin_INCLUDE_DIRS}")
+  SET(ORB_SLAM_INCLUDE_DIRS ${ORB_SLAM_INCLUDE_DIRS} ${Pangolin_INCLUDE_DIRS})
+  SET(ORB_SLAM_LIBRARIES ${ORB_SLAM_LIBRARIES} ${Pangolin_LIBRARIES})
+ENDIF()
+
+IF (ORB_SLAM_FOUND)
+   # show which ORB_SLAM was found only if not quiet
+   IF (NOT ORB_SLAM_FIND_QUIETLY)
+      MESSAGE(STATUS "Found ORB_SLAM${ORB_SLAM_VERSION}: ${ORB_SLAM_LIBRARIES}")
+   ENDIF (NOT ORB_SLAM_FIND_QUIETLY)
+ELSE (ORB_SLAM_FOUND)
+   # fatal error if ORB_SLAM is required but not found
+   IF (ORB_SLAM_FIND_REQUIRED)
+      MESSAGE(FATAL_ERROR "Could not find ORB_SLAM")
+   ENDIF (ORB_SLAM_FIND_REQUIRED)
+ENDIF (ORB_SLAM_FOUND)
+
@@ -1,33 +0,0 @@
-# - Find ORB_SLAM2 
-#
-# It sets the following variables:
-#  ORB_SLAM2_FOUND         - Set to false, or undefined, if ORB_SLAM2 isn't found.
-#  ORB_SLAM2_INCLUDE_DIRS  - The ORB_SLAM2 include directory.
-#  ORB_SLAM2_LIBRARIES     - The ORB_SLAM2 library to link against.
-#
-#  Set ORB_SLAM2_ROOT_DIR environment variable as the path to ORB_SLAM2 root folder.
-
-find_path(ORB_SLAM2_INCLUDE_DIR NAMES System.h PATHS $ENV{ORB_SLAM2_ROOT_DIR}/include)
-find_library(ORB_SLAM2_LIBRARY NAMES ORB_SLAM2 PATHS $ENV{ORB_SLAM2_ROOT_DIR}/lib)
-find_path(g2o_INCLUDE_DIR NAMES g2o/core/sparse_optimizer.h PATHS $ENV{ORB_SLAM2_ROOT_DIR}/Thirdparty/g2o NO_DEFAULT_PATH)
-find_library(g2o_LIBRARY NAMES g2o PATHS $ENV{ORB_SLAM2_ROOT_DIR}/Thirdparty/g2o/lib NO_DEFAULT_PATH)
-find_library(DBoW2_LIBRARY NAMES DBoW2 PATHS $ENV{ORB_SLAM2_ROOT_DIR}/Thirdparty/DBoW2/lib NO_DEFAULT_PATH)
-
-IF (ORB_SLAM2_INCLUDE_DIR AND ORB_SLAM2_LIBRARY AND DBoW2_LIBRARY AND g2o_INCLUDE_DIR AND g2o_LIBRARY)
-   SET(ORB_SLAM2_FOUND TRUE)
-   SET(ORB_SLAM2_INCLUDE_DIRS ${ORB_SLAM2_INCLUDE_DIR} ${g2o_INCLUDE_DIR} $ENV{ORB_SLAM2_ROOT_DIR})
-   SET(ORB_SLAM2_LIBRARIES ${g2o_LIBRARY} ${ORB_SLAM2_LIBRARY} ${DBoW2_LIBRARY})
-ENDIF (ORB_SLAM2_INCLUDE_DIR AND ORB_SLAM2_LIBRARY AND DBoW2_LIBRARY AND g2o_INCLUDE_DIR AND g2o_LIBRARY)
-
-IF (ORB_SLAM2_FOUND)
-   # show which ORB_SLAM2 was found only if not quiet
-   IF (NOT ORB_SLAM2_FIND_QUIETLY)
-      MESSAGE(STATUS "Found ORB_SLAM2: ${ORB_SLAM2_LIBRARIES}")
-   ENDIF (NOT ORB_SLAM2_FIND_QUIETLY)
-ELSE (ORB_SLAM2_FOUND)
-   # fatal error if ORB_SLAM2 is required but not found
-   IF (ORB_SLAM2_FIND_REQUIRED)
-      MESSAGE(FATAL_ERROR "Could not find ORB_SLAM2")
-   ENDIF (ORB_SLAM2_FIND_REQUIRED)
-ENDIF (ORB_SLAM2_FOUND)
-
@@ -69,7 +69,7 @@ public:
 	void setDistortionModel(const std::string & path);
 	void enableBilateralFiltering(float sigmaS, float sigmaR);
 	void disableBilateralFiltering() {_bilateralFiltering = false;}
-	void enableIMUFiltering(int filteringStrategy=1, const ParametersMap & parameters = ParametersMap());
+	void enableIMUFiltering(int filteringStrategy=1, const ParametersMap & parameters = ParametersMap(), bool baseFrameConversion = false);
 	void disableIMUFiltering();

 	RTABMAP_DEPRECATED(void setScanParameters(
@@ -125,6 +125,7 @@ private:
 	float _bilateralSigmaS;
 	float _bilateralSigmaR;
 	IMUFilter * _imuFilter;
+	bool _imuBaseFrameConversion;
 };

 } // namespace rtabmap
@@ -9,7 +9,8 @@
 #define IMU_H_

 #include <opencv2/core/core.hpp>
-#include <rtabmap/utilite/ULogger.h>
+#include <rtabmap/utilite/UEvent.h>
+#include <rtabmap/core/Transform.h>

 namespace rtabmap {

@@ -60,6 +61,9 @@ public:

 	const Transform & localTransform() const {return localTransform_;}

+	// apply local transform rotation to data, and set Identity rotation for local transform
+	void convertToBaseFrame();
+
 	bool empty() const
 	{
 		return localTransform_.isNull();
@@ -49,7 +49,7 @@ public:
 		kTypeFovis = 2,
 		kTypeViso2 = 3,
 		kTypeDVO = 4,
-		kTypeORBSLAM2 = 5,
+		kTypeORBSLAM = 5,
 		kTypeOkvis = 6,
 		kTypeLOAM = 7,
 		kTypeMSCKF = 8,
@@ -418,7 +418,7 @@ class RTABMAP_EXP Parameters
    RTABMAP_PARAM(Optimizer, GravitySigma,    float, 0.0,      uFormat("Gravity sigma value (>=0, typically between 0.1 and 0.3). Optimization is done while preserving gravity orientation of the poses. This should be used only with visual/lidar inertial odometry approaches, for which we assume that all odometry poses are aligned with gravity. Set to 0 to disable gravity constraints. Currently supported only with g2o and GTSAM optimization strategies (see %s).", kOptimizerStrategy().c_str()));
 #endif

-#ifdef RTABMAP_ORB_SLAM2
+#ifdef RTABMAP_ORB_SLAM
    RTABMAP_PARAM(g2o, Solver,            int, 3,          "0=csparse 1=pcg 2=cholmod 3=Eigen");
 #else
    RTABMAP_PARAM(g2o, Solver,            int, 0,          "0=csparse 1=pcg 2=cholmod 3=Eigen");
@@ -460,7 +460,7 @@ class RTABMAP_EXP Parameters
    RTABMAP_PARAM(OdomF2M, ScanSubtractAngle,   float, 45,    uFormat("[Geometry] Max angle (degrees) used to filter points of a new added scan to local map (when \"%s\">0). 0 means any angle.", kOdomF2MScanSubtractRadius().c_str()).c_str());
    RTABMAP_PARAM(OdomF2M, ScanRange,           float, 0,     "[Geometry] Distance Range used to filter points of local map (when > 0). 0 means local map is updated using time and not range.");
    RTABMAP_PARAM(OdomF2M, ValidDepthRatio,     float, 0.75,  "If a new frame has points without valid depth, they are added to local feature map only if points with valid depth on total points is over this ratio. Setting to 1 means no points without valid depth are added to local feature map.");
-#if defined(RTABMAP_G2O) || defined(RTABMAP_ORB_SLAM2)
+#if defined(RTABMAP_G2O) || defined(RTABMAP_ORB_SLAM)
    RTABMAP_PARAM(OdomF2M, BundleAdjustment,          int, 1, "Local bundle adjustment: 0=disabled, 1=g2o, 2=cvsba, 3=Ceres.");
 #else
    RTABMAP_PARAM(OdomF2M, BundleAdjustment,          int, 0, "Local bundle adjustment: 0=disabled, 1=g2o, 2=cvsba, 3=Ceres.");
@@ -521,12 +521,12 @@ class RTABMAP_EXP Parameters
    RTABMAP_PARAM(OdomViso2, BucketHeight,              double, 50,  "Height of bucket.");

    // Odometry ORB_SLAM2
-    RTABMAP_PARAM_STR(OdomORBSLAM2, VocPath,             "",    "Path to ORB vocabulary (*.txt).");
-    RTABMAP_PARAM(OdomORBSLAM2, Bf,              double, 0.076, "Fake IR projector baseline (m) used only when stereo is not used.");
-    RTABMAP_PARAM(OdomORBSLAM2, ThDepth,         double, 40.0,  "Close/Far threshold. Baseline times.");
-    RTABMAP_PARAM(OdomORBSLAM2, Fps,             float,  0.0,   "Camera FPS.");
-    RTABMAP_PARAM(OdomORBSLAM2, MaxFeatures,     int,    1000,  "Maximum ORB features extracted per frame.");
-    RTABMAP_PARAM(OdomORBSLAM2, MapSize,         int,    3000,  "Maximum size of the feature map (0 means infinite).");
+    RTABMAP_PARAM_STR(OdomORBSLAM, VocPath,             "",    "Path to ORB vocabulary (*.txt).");
+    RTABMAP_PARAM(OdomORBSLAM, Bf,              double, 0.076, "Fake IR projector baseline (m) used only when stereo is not used.");
+    RTABMAP_PARAM(OdomORBSLAM, ThDepth,         double, 40.0,  "Close/Far threshold. Baseline times.");
+    RTABMAP_PARAM(OdomORBSLAM, Fps,             float,  0.0,   "Camera FPS.");
+    RTABMAP_PARAM(OdomORBSLAM, MaxFeatures,     int,    1000,  "Maximum ORB features extracted per frame.");
+    RTABMAP_PARAM(OdomORBSLAM, MapSize,         int,    3000,  "Maximum size of the feature map (0 means infinite).");

    // Odometry OKVIS
    RTABMAP_PARAM_STR(OdomOKVIS, ConfigPath,     "",  "Path of OKVIS config file.");
@@ -582,7 +582,7 @@ class RTABMAP_EXP Parameters
    RTABMAP_PARAM(Vis, RefineIterations,         int, 5,        uFormat("[%s = 0] Number of iterations used to refine the transformation found by RANSAC. 0 means that the transformation is not refined.", kVisEstimationType().c_str()));
    RTABMAP_PARAM(Vis, PnPReprojError,           float, 2,      uFormat("[%s = 1] PnP reprojection error.", kVisEstimationType().c_str()));
    RTABMAP_PARAM(Vis, PnPFlags,                 int, 0,        uFormat("[%s = 1] PnP flags: 0=Iterative, 1=EPNP, 2=P3P", kVisEstimationType().c_str()));
-#if defined(RTABMAP_G2O) || defined(RTABMAP_ORB_SLAM2)
+#if defined(RTABMAP_G2O) || defined(RTABMAP_ORB_SLAM)
    RTABMAP_PARAM(Vis, PnPRefineIterations,      int, 0,        uFormat("[%s = 1] Refine iterations. Set to 0 if \"%s\" is also used.", kVisEstimationType().c_str(), kVisBundleAdjustment().c_str()));
 #else
    RTABMAP_PARAM(Vis, PnPRefineIterations,      int, 1,        uFormat("[%s = 1] Refine iterations. Set to 0 if \"%s\" is also used.", kVisEstimationType().c_str(), kVisBundleAdjustment().c_str()));
@@ -619,7 +619,7 @@ class RTABMAP_EXP Parameters
    RTABMAP_PARAM(Vis, CorFlowIterations,        int, 30,     uFormat("[%s=1] See cv::calcOpticalFlowPyrLK(). Used for optical flow approach.", kVisCorType().c_str()));
    RTABMAP_PARAM(Vis, CorFlowEps,               float, 0.01, uFormat("[%s=1] See cv::calcOpticalFlowPyrLK(). Used for optical flow approach.", kVisCorType().c_str()));
    RTABMAP_PARAM(Vis, CorFlowMaxLevel,          int, 3,      uFormat("[%s=1] See cv::calcOpticalFlowPyrLK(). Used for optical flow approach.", kVisCorType().c_str()));
-#if defined(RTABMAP_G2O) || defined(RTABMAP_ORB_SLAM2)
+#if defined(RTABMAP_G2O) || defined(RTABMAP_ORB_SLAM)
    RTABMAP_PARAM(Vis, BundleAdjustment,         int, 1,      "Optimization with bundle adjustment: 0=disabled, 1=g2o, 2=cvsba, 3=Ceres.");
 #else
    RTABMAP_PARAM(Vis, BundleAdjustment,         int, 0,      "Optimization with bundle adjustment: 0=disabled, 1=g2o, 2=cvsba, 3=Ceres.");
@@ -25,41 +25,48 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */

-#ifndef ODOMETRYORBSLAM2_H_
-#define ODOMETRYORBSLAM2_H_
+#ifndef ODOMETRYORBSLAM_H_
+#define ODOMETRYORBSLAM_H_

 #include <rtabmap/core/Odometry.h>

+#if RTABMAP_ORB_SLAM == 3
+namespace ORB_SLAM3 {
+#else
 namespace ORB_SLAM2 {
+#endif
 class System;
 }

-class ORBSLAM2System;
+class ORBSLAMSystem;

 namespace rtabmap {

-class RTABMAP_EXP OdometryORBSLAM2 : public Odometry
+class RTABMAP_EXP OdometryORBSLAM : public Odometry
 {
 public:
-	OdometryORBSLAM2(const rtabmap::ParametersMap & parameters = rtabmap::ParametersMap());
-	virtual ~OdometryORBSLAM2();
+	OdometryORBSLAM(const rtabmap::ParametersMap & parameters = rtabmap::ParametersMap());
+	virtual ~OdometryORBSLAM();

 	virtual void reset(const Transform & initialPose = Transform::getIdentity());
-	virtual Odometry::Type getType() {return Odometry::kTypeORBSLAM2;}
+	virtual Odometry::Type getType() {return Odometry::kTypeORBSLAM;}
+	virtual bool canProcessAsyncIMU() const;

 private:
 	virtual Transform computeTransform(SensorData & image, const Transform & guess = Transform(), OdometryInfo * info = 0);

 private:
-#ifdef RTABMAP_ORB_SLAM2
-	ORBSLAM2System * orbslam2_;
+#ifdef RTABMAP_ORB_SLAM
+	ORBSLAMSystem * orbslam_;
 	bool firstFrame_;
 	Transform originLocalTransform_;
 	Transform previousPose_;
+	bool useIMU_;
+	Transform imuLocalTransform_;
 #endif

 };

 }

-#endif /* ODOMETRYORBSLAM2_H_ */
+#endif /* ODOMETRYORBSLAM_H_ */
@@ -87,11 +87,12 @@ SET(SRC_FILES
    odometry/OdometryViso2.cpp
    odometry/OdometryDVO.cpp
    odometry/OdometryOkvis.cpp
-    odometry/OdometryORBSLAM2.cpp
+    odometry/OdometryORBSLAM.cpp
    odometry/OdometryLOAM.cpp
    odometry/OdometryMSCKF.cpp
    odometry/OdometryVINS.cpp
    
+    IMU.cpp
    IMUThread.cpp
    IMUFilter.cpp
    imufilter/ComplementaryFilter.cpp
@@ -601,16 +602,16 @@ IF(vins_FOUND)
 	)
 ENDIF(vins_FOUND)

-IF(ORB_SLAM2_FOUND)
+IF(ORB_SLAM_FOUND)
    SET(INCLUDE_DIRS
-        ${ORB_SLAM2_INCLUDE_DIRS} #before so that g2o includes are taken from ORB_SLAM2 directory before the official g2o one
+        ${ORB_SLAM_INCLUDE_DIRS} #before so that g2o includes are taken from ORB_SLAM directory before the official g2o one
 		${INCLUDE_DIRS}
 	)
 	SET(LIBRARIES
-	    ${ORB_SLAM2_LIBRARIES}
+	    ${ORB_SLAM_LIBRARIES}
 		${LIBRARIES}
 	)
-ENDIF(ORB_SLAM2_FOUND)
+ENDIF(ORB_SLAM_FOUND)

 IF(GTSAM_FOUND)
 	# Make sure GTSAM is built with system Eigen, not the included one in its package
@@ -67,7 +67,8 @@ CameraThread::CameraThread(Camera * camera, const ParametersMap & parameters) :
 		_bilateralFiltering(false),
 		_bilateralSigmaS(10),
 		_bilateralSigmaR(0.1),
-		_imuFilter(0)
+		_imuFilter(0),
+		_imuBaseFrameConversion(false)
 {
 	UASSERT(_camera != 0);
 }
@@ -117,10 +118,11 @@ void CameraThread::enableBilateralFiltering(float sigmaS, float sigmaR)
 	_bilateralSigmaR = sigmaR;
 }

-void CameraThread::enableIMUFiltering(int filteringStrategy, const ParametersMap & parameters)
+void CameraThread::enableIMUFiltering(int filteringStrategy, const ParametersMap & parameters, bool baseFrameConversion)
 {
 	delete _imuFilter;
 	_imuFilter = IMUFilter::create((IMUFilter::Type)filteringStrategy, parameters);
+	_imuBaseFrameConversion = baseFrameConversion;
 }

 void CameraThread::disableIMUFiltering()
@@ -470,21 +472,31 @@ void CameraThread::postUpdate(SensorData * dataPtr, CameraInfo * info) const
 		}
 		else
 		{
+			// Transform IMU data in base_link to correctly initialize yaw
+			IMU imu = data.imu();
+			if(_imuBaseFrameConversion)
+			{
+				UASSERT(!data.imu().localTransform().isNull());
+				imu.convertToBaseFrame();
+
+			}
 			_imuFilter->update(
-					data.imu().angularVelocity()[0],
-					data.imu().angularVelocity()[1],
-					data.imu().angularVelocity()[2],
-					data.imu().linearAcceleration()[0],
-					data.imu().linearAcceleration()[1],
-					data.imu().linearAcceleration()[2],
+					imu.angularVelocity()[0],
+					imu.angularVelocity()[1],
+					imu.angularVelocity()[2],
+					imu.linearAcceleration()[0],
+					imu.linearAcceleration()[1],
+					imu.linearAcceleration()[2],
 					data.stamp());
 			double qx,qy,qz,qw;
 			_imuFilter->getOrientation(qx,qy,qz,qw);
+
 			data.setIMU(IMU(
 					cv::Vec4d(qx,qy,qz,qw), cv::Mat::eye(3,3,CV_64FC1),
-					data.imu().angularVelocity(), data.imu().angularVelocityCovariance(),
-					data.imu().linearAcceleration(), data.imu().linearAccelerationCovariance(),
-					data.imu().localTransform()));
+					imu.angularVelocity(), imu.angularVelocityCovariance(),
+					imu.linearAcceleration(), imu.linearAccelerationCovariance(),
+					imu.localTransform()));
+
 			UDEBUG("%f %f %f %f (gyro=%f %f %f, acc=%f %f %f, %fs)",
 						data.imu().orientation()[0],
 						data.imu().orientation()[1],
@@ -0,0 +1,74 @@
+/*
+Copyright (c) 2010-2021, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+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 the Universite de Sherbrooke 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 THE COPYRIGHT HOLDER OR 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.
+*/
+
+
+#include <rtabmap/core/IMU.h>
+
+namespace rtabmap {
+
+void IMU::convertToBaseFrame()
+{
+	if(!localTransform_.isNull() && !localTransform_.rotation().isIdentity())
+	{
+		cv::Mat rotationMatrix, rotationMatrixT;
+		localTransform_.rotationMatrix().convertTo(rotationMatrix, CV_64FC1);
+		cv::transpose(rotationMatrix, rotationMatrixT);
+
+		cv::Mat_<double> v = rotationMatrix * cv::Mat(linearAcceleration_);
+		linearAcceleration_ = cv::Vec3d(v(0,0), v(0,1), v(0,2));
+		if(!linearAccelerationCovariance_.empty())
+		{
+			linearAccelerationCovariance_ = rotationMatrix * linearAccelerationCovariance_ * rotationMatrixT;
+		}
+
+		v = rotationMatrix * cv::Mat(angularVelocity_);
+		angularVelocity_ = cv::Vec3d(v(0,0), v(0,1), v(0,2));
+		if(!angularVelocityCovariance_.empty())
+		{
+			angularVelocityCovariance_ = rotationMatrix * angularVelocityCovariance_ * rotationMatrixT;
+		}
+
+		if(!(orientation_[0] == 0.0 && orientation_[1] == 0.0 && orientation_[2] == 0.0))
+		{
+			// orientation includes roll and pitch but not yaw in local transform
+			Eigen::Quaterniond qTheta =
+					Eigen::AngleAxisd(0, Eigen::Vector3d::UnitX()) *
+					Eigen::AngleAxisd(0, Eigen::Vector3d::UnitY()) *
+					Eigen::AngleAxisd(localTransform_.theta(), Eigen::Vector3d::UnitZ());
+			Eigen::Quaterniond q = qTheta * Eigen::Quaterniond(orientation_[3], orientation_[0], orientation_[1], orientation_[2]) * localTransform_.getQuaterniond().inverse();
+			orientation_ = cv::Vec4d(q.x(),q.y(),q.z(),q.w());
+			if(!orientationCovariance_.empty())
+			{
+				orientationCovariance_ = rotationMatrix * orientationCovariance_ * rotationMatrixT;
+			}
+		}
+
+		localTransform_ = Transform(localTransform_.x(), localTransform_.y(), localTransform_.z(), 0,0,0);
+	}
+}
+
+} //namespace rtabmap
@@ -32,7 +32,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/core/odometry/OdometryViso2.h"
 #include "rtabmap/core/odometry/OdometryDVO.h"
 #include "rtabmap/core/odometry/OdometryOkvis.h"
-#include "rtabmap/core/odometry/OdometryORBSLAM2.h"
+#include "rtabmap/core/odometry/OdometryORBSLAM.h"
 #include "rtabmap/core/odometry/OdometryLOAM.h"
 #include "rtabmap/core/odometry/OdometryMSCKF.h"
 #include "rtabmap/core/odometry/OdometryVINS.h"
@@ -80,8 +80,8 @@ Odometry * Odometry::create(Odometry::Type & type, const ParametersMap & paramet
 	case Odometry::kTypeDVO:
 		odometry = new OdometryDVO(parameters);
 		break;
-	case Odometry::kTypeORBSLAM2:
-		odometry = new OdometryORBSLAM2(parameters);
+	case Odometry::kTypeORBSLAM:
+		odometry = new OdometryORBSLAM(parameters);
 		break;
 	case Odometry::kTypeOkvis:
 		odometry = new OdometryOkvis(parameters);
@@ -291,7 +291,12 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
 		{
 			Transform orientation(0,0,0, data.imu().orientation()[0], data.imu().orientation()[1], data.imu().orientation()[2], data.imu().orientation()[3]);
 			// orientation includes roll and pitch but not yaw in local transform
-			Transform imuT = Transform(0,0,data.imu().localTransform().theta()) * orientation*data.imu().localTransform().inverse();
+			Transform imuT = Transform(data.imu().localTransform().x(),data.imu().localTransform().y(),data.imu().localTransform().z(), 0,0,data.imu().localTransform().theta()) *
+					orientation*
+					data.imu().localTransform().rotation().inverse();
+
+			IMU imu2 = data.imu();
+			imu2.convertToBaseFrame();

 			if(	this->getPose().r11() == 1.0f && this->getPose().r22() == 1.0f && this->getPose().r33() == 1.0f &&
 				this->framesProcessed() == 0)
@@ -234,6 +234,14 @@ const std::map<std::string, std::pair<bool, std::string> > & Parameters::getRemo
 	{
 		// removed parameters

+		// 0.20.9
+		removedParameters_.insert(std::make_pair("OdomORBSLAM2/VocPath",     std::make_pair(true, Parameters::kOdomORBSLAMVocPath())));
+		removedParameters_.insert(std::make_pair("OdomORBSLAM2/Bf",          std::make_pair(true, Parameters::kOdomORBSLAMBf())));
+		removedParameters_.insert(std::make_pair("OdomORBSLAM2/ThDepth",     std::make_pair(true, Parameters::kOdomORBSLAMThDepth())));
+		removedParameters_.insert(std::make_pair("OdomORBSLAM2/Fps",         std::make_pair(true, Parameters::kOdomORBSLAMFps())));
+		removedParameters_.insert(std::make_pair("OdomORBSLAM2/MaxFeatures", std::make_pair(true, Parameters::kOdomORBSLAMMaxFeatures())));
+		removedParameters_.insert(std::make_pair("OdomORBSLAM2/MapSize",     std::make_pair(true, Parameters::kOdomORBSLAMMapSize())));
+
 		// 0.20.
 		removedParameters_.insert(std::make_pair("SuperGlue/Path",           std::make_pair(true, Parameters::kPyMatcherPath())));
 		removedParameters_.insert(std::make_pair("SuperGlue/Iterations",     std::make_pair(true, Parameters::kPyMatcherIterations())));
@@ -823,8 +831,14 @@ ParametersMap Parameters::parseArguments(int argc, char * argv[], bool onlyParam
 #else
 				std::cout << str << std::setw(spacing - str.size()) << "false" << std::endl;
 #endif
+#if RTABMAP_ORB_SLAM == 3
+				str = "With ORB_SLAM3:";
+#elif RTABMAP_ORB_SLAM == 2
 				str = "With ORB_SLAM2:";
-#ifdef RTABMAP_ORB_SLAM2
+#else
+				str = "With ORB_SLAM:";
+#endif
+#ifdef RTABMAP_ORB_SLAM
 				std::cout << str << std::setw(spacing - str.size()) << "true" << std::endl;
 #else
 				std::cout << str << std::setw(spacing - str.size()) << "false" << std::endl;
@@ -127,7 +127,7 @@ void CameraRealSense2::close()
 	{
 		UINFO("%s", error.what());
 	}
-	
+
 	closing_ = false;
 }

@@ -179,7 +179,7 @@ void CameraRealSense2::pose_callback(rs2::frame frame)
 				pose.rotation.y,
 				pose.rotation.w);

-	UDEBUG("POSE callback! %f %s (confidence=%d)", frame.get_timestamp(), poseT.prettyPrint().c_str(), (int)pose.tracker_confidence);
+	//UDEBUG("POSE callback! %f %s (confidence=%d)", frame.get_timestamp(), poseT.prettyPrint().c_str(), (int)pose.tracker_confidence);

 	UScopeMutex sm(poseMutex_);
 	poseBuffer_.insert(poseBuffer_.end(), std::make_pair(frame.get_timestamp(), std::make_pair(poseT, pose.tracker_confidence)));
@@ -191,7 +191,7 @@ void CameraRealSense2::pose_callback(rs2::frame frame)

 void CameraRealSense2::frame_callback(rs2::frame frame)
 {
-	UDEBUG("Frame callback! %f", frame.get_timestamp());
+	//UDEBUG("Frame callback! %f", frame.get_timestamp());
 	syncer_(frame);
 }
 void CameraRealSense2::multiple_message_callback(rs2::frame frame)
@@ -486,7 +486,7 @@ bool CameraRealSense2::init(const std::string & calibrationFolder, const std::st
 	UINFO("setupDevice...");

 	close();
-	
+
 	clockSyncWarningShown_ = false;
 	imuGlobalSyncWarningShown_ = false;

@@ -498,32 +498,40 @@ bool CameraRealSense2::init(const std::string & calibrationFolder, const std::st
 	}

 	bool found=false;
-	for (rs2::device dev : list)
+	try
 	{
-		auto sn = dev.get_info(RS2_CAMERA_INFO_SERIAL_NUMBER);
-		auto pid_str = dev.get_info(RS2_CAMERA_INFO_PRODUCT_ID);
-		uint16_t pid;
-		std::stringstream ss;
-		ss << std::hex << pid_str;
-		ss >> pid;
-		UINFO("Device with serial number %s was found with product ID=%d.", sn, (int)pid);
-		if(dualMode_ && pid == 0x0B37)
+		for (rs2::device dev : list)
 		{
-			// Dual setup: device[0] = D400, device[1] = T265
-			// T265
-			dev_.resize(2);
-			dev_[1] = dev;
-		}
-		else if (!found && (deviceId_.empty() || deviceId_ == sn))
-		{
-			if(dev_.empty())
+			auto sn = dev.get_info(RS2_CAMERA_INFO_SERIAL_NUMBER);
+			auto pid_str = dev.get_info(RS2_CAMERA_INFO_PRODUCT_ID);
+
+			uint16_t pid;
+			std::stringstream ss;
+			ss << std::hex << pid_str;
+			ss >> pid;
+			UINFO("Device with serial number %s was found with product ID=%d.", sn, (int)pid);
+			if(dualMode_ && pid == 0x0B37)
 			{
-				dev_.resize(1);
+				// Dual setup: device[0] = D400, device[1] = T265
+				// T265
+				dev_.resize(2);
+				dev_[1] = dev;
+			}
+			else if (!found && (deviceId_.empty() || deviceId_ == sn))
+			{
+				if(dev_.empty())
+				{
+					dev_.resize(1);
+				}
+				dev_[0] = dev;
+				found=true;
 			}
-			dev_[0] = dev;
-			found=true;
 		}
 	}
+	catch(const rs2::error & error)
+	{
+		UWARN("%s. Is the camera already used with another app?", error.what());
+	}

 	if (!found)
 	{
@@ -1405,27 +1413,37 @@ SensorData CameraRealSense2::captureImage(CameraInfo * info)
 					{
 						++iterB;
 					}
-					if(iterA != iterB)
+					std::vector<double> stamps;
+					for(;iterA != iterB;++iterA)
 					{
-						int pub = 0;
-						for(;iterA != iterB;++iterA)
-						{
-							Transform tmp;
-							IMU imuTmp;
-							getPoseAndIMU(iterA->first, tmp, confidence, imuTmp);
-							if(!imuTmp.empty())
-							{
-								UEventsManager::post(new IMUEvent(imuTmp, iterA->first/1000.0));
-								pub++;
-							}
-							else
-							{
-								break;
-							}
-						}
-						UDEBUG("inter imu published=%d, %f -> %f", pub, lastImuStamp_, imuStamp);
+						stamps.push_back(iterA->first);
 					}
 					imuMutex_.unlock();
+
+					int pub = 0;
+					for(size_t i=0; i<stamps.size(); ++i)
+					{
+						Transform tmp;
+						IMU imuTmp;
+						getPoseAndIMU(stamps[i], tmp, confidence, imuTmp);
+						if(!imuTmp.empty())
+						{
+							UEventsManager::post(new IMUEvent(imuTmp, iterA->first/1000.0));
+							pub++;
+						}
+						else
+						{
+							break;
+						}
+					}
+					if(stamps.size())
+					{
+						UDEBUG("inter imu published=%d (rate=%fHz), %f -> %f", pub, double(pub)/((stamps.back()-stamps.front())/1000.0), stamps.front()/1000.0, stamps.back()/1000.0);
+					}
+					else
+					{
+						UWARN("No inter imu published!?");
+					}
 				}
 				lastImuStamp_ = imuStamp;
 			}
@@ -217,14 +217,14 @@ static inline bool computeOrientation(
  // magnetic Field E must not be parallel to A,
  // choose an arbitrary orthogonal vector
  Eigen::Vector3f E;
-  if (fabs(A[0]) > 0.1 || fabs(A[1]) > 0.1) {
+  if (fabs(A[2]) > 0.1) {
+  	  E[0] = 0.0;
+  	  E[1] = A[2];
+  	  E[2] = -A[1];
+  } else if (fabs(A[0]) > 0.1 || fabs(A[1]) > 0.1) {
 	  E[0] = A[1];
 	  E[1] = A[0];
 	  E[2] = 0.0;
-  } else if (fabs(A[2]) > 0.1) {
-	  E[0] = 0.0;
-	  E[1] = A[2];
-	  E[2] = A[1];
  } else {
 	  // free fall
 	  return false;
@@ -25,7 +25,6 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */

-#include "rtabmap/core/odometry/OdometryORBSLAM2.h"
 #include "rtabmap/core/OdometryInfo.h"
 #include "rtabmap/core/util2d.h"
 #include "rtabmap/core/util3d_transforms.h"
@@ -35,31 +34,42 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/utilite/UDirectory.h"
 #include <pcl/common/transforms.h>
 #include <opencv2/imgproc/types_c.h>
+#include <rtabmap/core/odometry/OdometryORBSLAM.h>

-#ifdef RTABMAP_ORB_SLAM2
+#ifdef RTABMAP_ORB_SLAM
 #include <System.h>
 #include <thread>

 using namespace std;

+#if RTABMAP_ORB_SLAM == 3
+namespace ORB_SLAM3 {
+#else
 namespace ORB_SLAM2 {
+#endif
 // Override original Tracking object to comment all rendering stuff
 class Tracker: public Tracking
 {
 public:
+#if RTABMAP_ORB_SLAM == 3
+	Tracker(System* pSys, ORBVocabulary* pVoc, FrameDrawer* pFrameDrawer, MapDrawer* pMapDrawer, Atlas* pMap,
+#else
 	Tracker(System* pSys, ORBVocabulary* pVoc, FrameDrawer* pFrameDrawer, MapDrawer* pMapDrawer, Map* pMap,
+#endif
 	             KeyFrameDatabase* pKFDB, const std::string &strSettingPath, const int sensor, long unsigned int maxFeatureMapSize) :
 	            	 Tracking(pSys, pVoc, pFrameDrawer, pMapDrawer, pMap, pKFDB, strSettingPath, sensor),
 	            	 maxFeatureMapSize_(maxFeatureMapSize)
-	{
+	{}

-	}
 private:
 	long unsigned int maxFeatureMapSize_;

 protected:
 	void Track()
 	{
+#if RTABMAP_ORB_SLAM == 3
+		Map* mpMap = mpAtlas->GetCurrentMap();
+#endif
 	    if(mState==NO_IMAGES_YET)
 	    {
 	        mState = NOT_INITIALIZED;
@@ -80,12 +90,23 @@ protected:
 	        //mpFrameDrawer->Update(this);

 	        if(mState!=OK)
+	        {
+#if RTABMAP_ORB_SLAM == 3
+	        	mLastFrame = Frame(mCurrentFrame);
+#endif
 	            return;
+	        }
+#if RTABMAP_ORB_SLAM == 3
+	        if(mpAtlas->GetAllMaps().size() == 1)
+			{
+				mnFirstFrameId = mCurrentFrame.mnId;
+			}
+#endif
 	    }
 	    else
 	    {
 	        // System is initialized. Track Frame.
-	        bool bOK;
+	        bool bOK = true;

 	        // Initial camera pose estimation using motion model or relocalization (if tracking is lost)
 	        if(!mbOnlyTracking)
@@ -190,7 +211,8 @@ protected:
 	            }
 	        }

-	        mCurrentFrame.mpReferenceKF = mpReferenceKF;
+	        if(!mCurrentFrame.mpReferenceKF)
+				   mCurrentFrame.mpReferenceKF = mpReferenceKF;

 	        // If we have an initial estimation of the camera pose and matching. Track the local map.
 	        if(!mbOnlyTracking)
@@ -362,6 +384,9 @@ protected:
 	        // Set Frame pose to the origin
 	        mCurrentFrame.SetPose(cv::Mat::eye(4,4,CV_32F));

+#if RTABMAP_ORB_SLAM == 3
+			Map* mpMap = mpAtlas->GetCurrentMap();
+#endif
 	        // Create KeyFrame
 	        KeyFrame* pKFini = new KeyFrame(mCurrentFrame,mpMap,mpKeyFrameDB);

@@ -459,9 +484,11 @@ public:
 				cvtColor(imGrayRight,imGrayRight,CV_BGRA2GRAY);
 			}
 		}
-
+#if RTABMAP_ORB_SLAM == 3
+	    mCurrentFrame = Frame(mImGray,imGrayRight,timestamp,mpORBextractorLeft,mpORBextractorRight,mpORBVocabulary,mK,mDistCoef,mbf,mThDepth, mpCamera);
+#else
 	    mCurrentFrame = Frame(mImGray,imGrayRight,timestamp,mpORBextractorLeft,mpORBextractorRight,mpORBVocabulary,mK,mDistCoef,mbf,mThDepth);
-
+#endif
 	    Track();

 	    return mCurrentFrame.mTcw.clone();
@@ -489,8 +516,11 @@ public:

 	    UASSERT(imDepth.type()==CV_32F);

+#if RTABMAP_ORB_SLAM == 3
+	    mCurrentFrame = Frame(mImGray,imDepth,timestamp,mpORBextractorLeft,mpORBVocabulary,mK,mDistCoef,mbf,mThDepth, mpCamera);
+#else
 	    mCurrentFrame = Frame(mImGray,imDepth,timestamp,mpORBextractorLeft,mpORBVocabulary,mK,mDistCoef,mbf,mThDepth);
-
+#endif
 	    Track();

 	    return mCurrentFrame.mTcw.clone();
@@ -501,10 +531,13 @@ public:
 class LoopCloser: public LoopClosing
 {
 public:
+#if RTABMAP_ORB_SLAM == 3
+	LoopCloser(Atlas* pMap, KeyFrameDatabase* pDB, ORBVocabulary* pVoc,const bool bFixScale) :
+#else
 	LoopCloser(Map* pMap, KeyFrameDatabase* pDB, ORBVocabulary* pVoc,const bool bFixScale) :
+#endif
 		LoopClosing(pMap, pDB, pVoc, bFixScale)
-	{
-	}
+	{}

 public:
 	void RunNoLoop()
@@ -531,12 +564,18 @@ public:
 	}
 };

-} // namespace ORB_SLAM2
+} // namespace ORB_SLAM

-class ORBSLAM2System
+#if RTABMAP_ORB_SLAM == 3
+using namespace ORB_SLAM3;
+#else
+using namespace ORB_SLAM2;
+#endif
+
+class ORBSLAMSystem
 {
 public:
-	ORBSLAM2System(const rtabmap::ParametersMap & parameters) :
+	ORBSLAMSystem(const rtabmap::ParametersMap & parameters) :
 		mpVocabulary(0),
 		mpKeyFrameDatabase(0),
 		mpMap(0),
@@ -548,14 +587,14 @@ public:
 		parameters_(parameters)
 	{
 		std::string vocabularyPath;
-		rtabmap::Parameters::parse(parameters, rtabmap::Parameters::kOdomORBSLAM2VocPath(), vocabularyPath);
+		rtabmap::Parameters::parse(parameters, rtabmap::Parameters::kOdomORBSLAMVocPath(), vocabularyPath);

 		if(!vocabularyPath.empty())
 		{
 			//Load ORB Vocabulary
 			vocabularyPath = uReplaceChar(vocabularyPath, '~', UDirectory::homeDir());
 			UWARN("Loading ORB Vocabulary: \"%s\". This could take a while...", vocabularyPath.c_str());
-			mpVocabulary = new ORB_SLAM2::ORBVocabulary();
+			mpVocabulary = new ORBVocabulary();
 			bool bVocLoad = mpVocabulary->loadFromTextFile(vocabularyPath);
 			if(!bVocLoad)
 			{
@@ -570,11 +609,11 @@ public:
 		}
 		else
 		{
-			UERROR("ORBSLAM2 vocabulary path should be set! (Parameter name=\"%s\")", rtabmap::Parameters::kOdomORBSLAM2VocPath().c_str());
+			UERROR("ORBSLAM2 vocabulary path should be set! (Parameter name=\"%s\")", rtabmap::Parameters::kOdomORBSLAMVocPath().c_str());
 		}
 	}

-	bool init(const rtabmap::CameraModel & model, bool stereo, double baseline)
+	bool init(const rtabmap::CameraModel & model, bool stereo, double baseline, const rtabmap::Transform & localIMUTransform)
 	{
 		if(!mpVocabulary)
 		{
@@ -591,11 +630,17 @@ public:
 		{
 			workingDir = ".";
 		}
-		std::string configPath = workingDir+"/rtabmap_orbslam2.yaml";
+		std::string configPath = workingDir+"/rtabmap_orbslam.yaml";
 		std::ofstream ofs (configPath, std::ofstream::out);
 		ofs << "%YAML:1.0" << std::endl;
 		ofs << std::endl;

+
+		ofs << "Camera.type: \"PinHole\"" << std::endl;
+		ofs << std::endl;
+
+		ofs << fixed << setprecision(13);
+
 		//# Camera calibration and distortion parameters (OpenCV)
 		ofs << "Camera.fx: " << model.fx() << std::endl;
 		ofs << "Camera.fy: " << model.fy() << std::endl;
@@ -605,13 +650,13 @@ public:

 		if(model.D().cols < 4)
 		{
-			ofs << "Camera.k1: " << 0 << std::endl;
-			ofs << "Camera.k2: " << 0 << std::endl;
-			ofs << "Camera.p1: " << 0 << std::endl;
-			ofs << "Camera.p2: " << 0 << std::endl;
+			ofs << "Camera.k1: " << 0.0 << std::endl;
+			ofs << "Camera.k2: " << 0.0 << std::endl;
+			ofs << "Camera.p1: " << 0.0 << std::endl;
+			ofs << "Camera.p2: " << 0.0 << std::endl;
 			if(!stereo)
 			{
-				ofs << "Camera.k3: " << 0 << std::endl;
+				ofs << "Camera.k3: " << 0.0 << std::endl;
 			}
 		}
 		if(model.D().cols >= 4)
@@ -638,8 +683,8 @@ public:
 		//# IR projector baseline times fx (aprox.)
 		if(baseline <= 0.0)
 		{
-			baseline = rtabmap::Parameters::defaultOdomORBSLAM2Bf();
-			rtabmap::Parameters::parse(parameters_, rtabmap::Parameters::kOdomORBSLAM2Bf(), baseline);
+			baseline = rtabmap::Parameters::defaultOdomORBSLAMBf();
+			rtabmap::Parameters::parse(parameters_, rtabmap::Parameters::kOdomORBSLAMBf(), baseline);
 		}
 		ofs << "Camera.bf: " << model.fx()*baseline << std::endl;
 		ofs << std::endl;
@@ -649,14 +694,14 @@ public:
 		ofs << "Camera.RGB: 1" << std::endl;
 		ofs << std::endl;

-		float fps = rtabmap::Parameters::defaultOdomORBSLAM2Fps();
-		rtabmap::Parameters::parse(parameters_, rtabmap::Parameters::kOdomORBSLAM2Fps(), fps);
+		float fps = rtabmap::Parameters::defaultOdomORBSLAMFps();
+		rtabmap::Parameters::parse(parameters_, rtabmap::Parameters::kOdomORBSLAMFps(), fps);
 		ofs << "Camera.fps: " << fps << std::endl;
 		ofs << std::endl;

 		//# Close/Far threshold. Baseline times.
-		double thDepth = rtabmap::Parameters::defaultOdomORBSLAM2ThDepth();
-		rtabmap::Parameters::parse(parameters_, rtabmap::Parameters::kOdomORBSLAM2ThDepth(), thDepth);
+		double thDepth = rtabmap::Parameters::defaultOdomORBSLAMThDepth();
+		rtabmap::Parameters::parse(parameters_, rtabmap::Parameters::kOdomORBSLAMThDepth(), thDepth);
 		ofs << "ThDepth: " << thDepth << std::endl;
 		ofs << std::endl;

@@ -664,12 +709,37 @@ public:
 		ofs << "DepthMapFactor: " << 1000.0 << std::endl;
 		ofs << std::endl;

+		if(!localIMUTransform.isNull())
+		{
+			//#--------------------------------------------------------------------------------------------
+			//# IMU Parameters TODO: hard-coded, not used
+			//#--------------------------------------------------------------------------------------------
+			//  Transformation from camera 0 to body-frame (imu)
+			rtabmap::Transform camImuT = model.localTransform()*localIMUTransform;
+			ofs << "Tbc: !!opencv-matrix" << std::endl;
+			ofs << "   rows: 4" << std::endl;
+			ofs << "   cols: 4" << std::endl;
+			ofs << "   dt: f" << std::endl;
+			ofs << "   data: [" << camImuT.data()[0] << ", " << camImuT.data()[1] << ", " << camImuT.data()[2]  << ", " << camImuT.data()[3]  << ", " << std::endl;
+			ofs << "         "  << camImuT.data()[4] << ", " << camImuT.data()[5] << ", " << camImuT.data()[6]  << ", " << camImuT.data()[7]  << ", " << std::endl;
+			ofs << "         "  << camImuT.data()[8] << ", " << camImuT.data()[9] << ", " << camImuT.data()[10] << ", " << camImuT.data()[11] << ", " << std::endl;
+			ofs << "         0.0, 0.0, 0.0, 1.0]" << std::endl;
+			ofs << std::endl;
+
+			ofs << "IMU.NoiseGyro: " << 1.7e-4 << std::endl;
+			ofs << "IMU.NoiseAcc: " << 2.0e-3 << std::endl;
+			ofs << "IMU.GyroWalk: " << 1.9393e-5 << std::endl;
+			ofs << "IMU.AccWalk: " << 3.e-3 << std::endl;
+			ofs << "IMU.Frequency: " << 200 << std::endl;
+			ofs << std::endl;
+		}
+
 		//#--------------------------------------------------------------------------------------------
 		//# ORB Parameters
 		//#--------------------------------------------------------------------------------------------
 		//# ORB Extractor: Number of features per image
-		int features = rtabmap::Parameters::defaultOdomORBSLAM2MaxFeatures();
-		rtabmap::Parameters::parse(parameters_, rtabmap::Parameters::kOdomORBSLAM2MaxFeatures(), features);
+		int features = rtabmap::Parameters::defaultOdomORBSLAMMaxFeatures();
+		rtabmap::Parameters::parse(parameters_, rtabmap::Parameters::kOdomORBSLAMMaxFeatures(), features);
 		ofs << "ORBextractor.nFeatures: " << features << std::endl;
 		ofs << std::endl;

@@ -697,29 +767,36 @@ public:
 		ofs << "ORBextractor.minThFAST: " << minThFAST << std::endl;
 		ofs << std::endl;

-		int maxFeatureMapSize = rtabmap::Parameters::defaultOdomORBSLAM2MapSize();
-		rtabmap::Parameters::parse(parameters_, rtabmap::Parameters::kOdomORBSLAM2MapSize(), maxFeatureMapSize);
+		int maxFeatureMapSize = rtabmap::Parameters::defaultOdomORBSLAMMapSize();
+		rtabmap::Parameters::parse(parameters_, rtabmap::Parameters::kOdomORBSLAMMapSize(), maxFeatureMapSize);

 		ofs.close();

 		//Create KeyFrame Database
-		mpKeyFrameDatabase = new ORB_SLAM2::KeyFrameDatabase(*mpVocabulary);
+		mpKeyFrameDatabase = new KeyFrameDatabase(*mpVocabulary);

 		//Create the Map
+#if RTABMAP_ORB_SLAM == 3
+		mpMap = new Atlas(0);
+#else
 		mpMap = new ORB_SLAM2::Map();
+#endif

 		//Initialize the Tracking thread
 		//(it will live in the main thread of execution, the one that called this constructor)
-		mpTracker = new ORB_SLAM2::Tracker(0, mpVocabulary, 0, 0, mpMap, mpKeyFrameDatabase, configPath, stereo?ORB_SLAM2::System::STEREO:ORB_SLAM2::System::RGBD, maxFeatureMapSize);
+		mpTracker = new Tracker(0, mpVocabulary, 0, 0, mpMap, mpKeyFrameDatabase, configPath, stereo?System::STEREO:System::RGBD, maxFeatureMapSize);

 		//Initialize the Local Mapping thread and launch
-		mpLocalMapper = new ORB_SLAM2::LocalMapping(mpMap, false);
-
+#if RTABMAP_ORB_SLAM == 3
+		mpLocalMapper = new LocalMapping(0, mpMap, false, stereo && !localIMUTransform.isNull());
+#else
+		mpLocalMapper = new LocalMapping(mpMap, false);
+#endif
 		//Initialize the Loop Closing thread and launch
-		mpLoopCloser = new ORB_SLAM2::LoopCloser(mpMap, mpKeyFrameDatabase, mpVocabulary, true);
+		mpLoopCloser = new LoopCloser(mpMap, mpKeyFrameDatabase, mpVocabulary, true);

-		mptLocalMapping = new thread(&ORB_SLAM2::LocalMapping::Run, mpLocalMapper);
-		mptLoopClosing = new thread(&ORB_SLAM2::LoopCloser::RunNoLoop, mpLoopCloser);
+		mptLocalMapping = new thread(&LocalMapping::Run, mpLocalMapper);
+		mptLoopClosing = new thread(&LoopCloser::RunNoLoop, mpLoopCloser);

 		//Set pointers between threads
 		mpTracker->SetLocalMapper(mpLocalMapper);
@@ -731,11 +808,15 @@ public:

 		mpLoopCloser->SetTracker(mpTracker);
 		mpLoopCloser->SetLocalMapper(mpLocalMapper);
+#if RTABMAP_ORB_SLAM == 3
+		if(ULogger::level() > ULogger::kInfo)
+			Verbose::SetTh(Verbose::VERBOSITY_QUIET);
+#endif

 		return true;
 	}

-	virtual ~ORBSLAM2System()
+	virtual ~ORBSLAMSystem()
 	{
 		shutdown();
 		delete mpVocabulary;
@@ -776,25 +857,29 @@ public:

 public:
 	// ORB vocabulary used for place recognition and feature matching.
-	ORB_SLAM2::ORBVocabulary* mpVocabulary;
+	ORBVocabulary* mpVocabulary;

 	// KeyFrame database for place recognition (relocalization and loop detection).
-	ORB_SLAM2::KeyFrameDatabase* mpKeyFrameDatabase;
+	KeyFrameDatabase* mpKeyFrameDatabase;

 	// Map structure that stores the pointers to all KeyFrames and MapPoints.
-	ORB_SLAM2::Map* mpMap;
+#if RTABMAP_ORB_SLAM == 3
+	Atlas* mpMap;
+#else
+	Map* mpMap;
+#endif

 	// 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.
-	ORB_SLAM2::Tracker* mpTracker;
+	Tracker* mpTracker;

 	// Local Mapper. It manages the local map and performs local bundle adjustment.
-	ORB_SLAM2::LocalMapping* mpLocalMapper;
+	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.
-	ORB_SLAM2::LoopCloser* mpLoopCloser;
+	LoopCloser* mpLoopCloser;

 	 // System threads: Local Mapping, Loop Closing, Viewer.
 	// The Tracking thread "lives" in the main execution thread that creates the System object.
@@ -807,55 +892,96 @@ public:

 namespace rtabmap {

-OdometryORBSLAM2::OdometryORBSLAM2(const ParametersMap & parameters) :
+OdometryORBSLAM::OdometryORBSLAM(const ParametersMap & parameters) :
 	Odometry(parameters)
-#ifdef RTABMAP_ORB_SLAM2
+#ifdef RTABMAP_ORB_SLAM
    ,
-	orbslam2_(0),
+	orbslam_(0),
 	firstFrame_(true),
-	previousPose_(Transform::getIdentity())
+	previousPose_(Transform::getIdentity()),
+	useIMU_(false) // TODO: Not yet supported with ORB_SLAM3
 #endif
 {
-#ifdef RTABMAP_ORB_SLAM2
-	orbslam2_ = new ORBSLAM2System(parameters);
+#ifdef RTABMAP_ORB_SLAM
+	orbslam_ = new ORBSLAMSystem(parameters);
 #endif
 }

-OdometryORBSLAM2::~OdometryORBSLAM2()
+OdometryORBSLAM::~OdometryORBSLAM()
 {
-#ifdef RTABMAP_ORB_SLAM2
-	if(orbslam2_)
+#ifdef RTABMAP_ORB_SLAM
+	if(orbslam_)
 	{
-		delete orbslam2_;
+		delete orbslam_;
 	}
 #endif
 }

-void OdometryORBSLAM2::reset(const Transform & initialPose)
+void OdometryORBSLAM::reset(const Transform & initialPose)
 {
 	Odometry::reset(initialPose);
-#ifdef RTABMAP_ORB_SLAM2
-	if(orbslam2_)
+#ifdef RTABMAP_ORB_SLAM
+	if(orbslam_)
 	{
-		orbslam2_->shutdown();
+		orbslam_->shutdown();
 	}
 	firstFrame_ = true;
 	originLocalTransform_.setNull();
 	previousPose_.setIdentity();
+	imuLocalTransform_.setNull();
+#endif
+}
+
+bool OdometryORBSLAM::canProcessAsyncIMU() const
+{
+#ifdef RTABMAP_ORB_SLAM
+	return useIMU_;
+#else
+	return false;
 #endif
 }

 // return not null transform if odometry is correctly computed
-Transform OdometryORBSLAM2::computeTransform(
+Transform OdometryORBSLAM::computeTransform(
 		SensorData & data,
 		const Transform & guess,
 		OdometryInfo * info)
 {
 	Transform t;

-#ifdef RTABMAP_ORB_SLAM2
+#ifdef RTABMAP_ORB_SLAM
 	UTimer timer;

+#if RTABMAP_ORB_SLAM == 3
+	if(useIMU_)
+	{
+		if(orbslam_->mpTracker == 0)
+		{
+			if(!data.imu().empty())
+			{
+				imuLocalTransform_ = data.imu().localTransform();
+			}
+		}
+		else if(!data.imu().empty())
+		{
+			ORB_SLAM3::IMU::Point pt(
+					data.imu().linearAcceleration().val[0],
+					data.imu().linearAcceleration().val[1],
+					data.imu().linearAcceleration().val[2],
+					data.imu().angularVelocity().val[0],
+					data.imu().angularVelocity().val[1],
+					data.imu().angularVelocity().val[2],
+					data.stamp());
+			orbslam_->mpTracker->GrabImuData(pt);
+		}
+
+		if(data.imageRaw().empty() || imuLocalTransform_.isNull())
+		{
+			return Transform();
+		}
+	}
+#endif
+
 	if(data.imageRaw().empty() ||
 		data.imageRaw().rows != data.depthOrRightRaw().rows ||
 		data.imageRaw().cols != data.depthOrRightRaw().cols)
@@ -876,12 +1002,18 @@ Transform OdometryORBSLAM2::computeTransform(
 	}

 	bool stereo = data.cameraModels().size() == 0;
+	if(!stereo && useIMU_)
+	{
+		UWARN("Disabling IMU support (ORB_SLAM3 doesn't support IMU with RGB-D mode).");
+		useIMU_ = false;
+		imuLocalTransform_.setNull();
+	}

 	cv::Mat covariance;
-	if(orbslam2_->mpTracker == 0)
+	if(orbslam_->mpTracker == 0)
 	{
 		CameraModel model = data.cameraModels().size()==1?data.cameraModels()[0]:data.stereoCameraModel().left();
-		if(!orbslam2_->init(model, stereo, data.stereoCameraModel().baseline()))
+		if(!orbslam_->init(model, stereo, data.stereoCameraModel().baseline(), imuLocalTransform_))
 		{
 			return t;
 		}
@@ -892,7 +1024,7 @@ Transform OdometryORBSLAM2::computeTransform(
 	if(stereo)
 	{
 		localTransform = data.stereoCameraModel().localTransform();
-		Tcw = ((ORB_SLAM2::Tracker*)orbslam2_->mpTracker)->GrabImageStereo(data.imageRaw(), data.rightRaw(), data.stamp());
+		Tcw = ((Tracker*)orbslam_->mpTracker)->GrabImageStereo(data.imageRaw(), data.rightRaw(), data.stamp());
 	}
 	else
 	{
@@ -906,11 +1038,11 @@ Transform OdometryORBSLAM2::computeTransform(
 		{
 			depth = util2d::cvtDepthToFloat(data.depthRaw());
 		}
-		Tcw = ((ORB_SLAM2::Tracker*)orbslam2_->mpTracker)->GrabImageRGBD(data.imageRaw(), depth, data.stamp());
+		Tcw = ((Tracker*)orbslam_->mpTracker)->GrabImageRGBD(data.imageRaw(), depth, data.stamp());
 	}

 	Transform previousPoseInv = previousPose_.inverse();
-	if(orbslam2_->mpTracker->mState == ORB_SLAM2::Tracking::LOST)
+	if(orbslam_->mpTracker->mState == Tracking::LOST)
 	{
 		covariance = cv::Mat::eye(6,6,CV_64FC1)*9999.0f;
 	}
@@ -944,8 +1076,8 @@ Transform OdometryORBSLAM2::computeTransform(
 			float baseline = data.stereoCameraModel().baseline();
 			if(baseline <= 0.0f)
 			{
-				baseline = rtabmap::Parameters::defaultOdomORBSLAM2Bf();
-				rtabmap::Parameters::parse(orbslam2_->parameters_, rtabmap::Parameters::kOdomORBSLAM2Bf(), baseline);
+				baseline = rtabmap::Parameters::defaultOdomORBSLAMBf();
+				rtabmap::Parameters::parse(orbslam_->parameters_, rtabmap::Parameters::kOdomORBSLAMBf(), baseline);
 			}
 			double linearVar = 0.0001;
 			if(baseline > 0.0f)
@@ -966,39 +1098,39 @@ Transform OdometryORBSLAM2::computeTransform(

 	int totalMapPoints= 0;
 	int totalKfs= 0;
-	if(orbslam2_->mpMap)
+	if(orbslam_->mpMap)
 	{
-		totalMapPoints = orbslam2_->mpMap->MapPointsInMap();
-		totalKfs = orbslam2_->mpMap->KeyFramesInMap();
+		totalMapPoints = orbslam_->mpMap->MapPointsInMap();
+		totalKfs = orbslam_->mpMap->KeyFramesInMap();
 	}

 	if(info)
 	{
 		info->lost = t.isNull();
-		info->type = (int)kTypeORBSLAM2;
+		info->type = (int)kTypeORBSLAM;
 		info->reg.covariance = covariance;
 		info->localMapSize = totalMapPoints;
 		info->localKeyFrames = totalKfs;

-		if(this->isInfoDataFilled() && orbslam2_->mpTracker && orbslam2_->mpMap)
+		if(this->isInfoDataFilled() && orbslam_->mpTracker && orbslam_->mpMap)
 		{
-			const std::vector<cv::KeyPoint> & kpts = orbslam2_->mpTracker->mCurrentFrame.mvKeys;
+			const std::vector<cv::KeyPoint> & kpts = orbslam_->mpTracker->mCurrentFrame.mvKeys;
 			info->reg.matchesIDs.resize(kpts.size());
 			info->reg.inliersIDs.resize(kpts.size());
 			int oi = 0;
 			for (unsigned int i = 0; i < kpts.size(); ++i)
 			{
 				int wordId;
-				if(orbslam2_->mpTracker->mCurrentFrame.mvpMapPoints[i] != 0)
+				if(orbslam_->mpTracker->mCurrentFrame.mvpMapPoints[i] != 0)
 				{
-					wordId = orbslam2_->mpTracker->mCurrentFrame.mvpMapPoints[i]->mnId;
+					wordId = orbslam_->mpTracker->mCurrentFrame.mvpMapPoints[i]->mnId;
 				}
 				else
 				{
 					wordId = -(i+1);
 				}
 				info->words.insert(std::make_pair(wordId, kpts[i]));
-				if(orbslam2_->mpTracker->mCurrentFrame.mvpMapPoints[i] != 0)
+				if(orbslam_->mpTracker->mCurrentFrame.mvpMapPoints[i] != 0)
 				{
 					info->reg.matchesIDs[oi] = wordId;
 					info->reg.inliersIDs[oi] = wordId;
@@ -1010,7 +1142,7 @@ Transform OdometryORBSLAM2::computeTransform(
 			info->reg.inliers = oi;
 			info->reg.matches = oi;

-			std::vector<ORB_SLAM2::MapPoint*> mapPoints = orbslam2_->mpMap->GetAllMapPoints();
+			std::vector<MapPoint*> mapPoints = orbslam_->mpMap->GetAllMapPoints();
 			Eigen::Affine3f fixRot = (this->getPose()*previousPoseInv*originLocalTransform_).toEigen3f();
 			for (unsigned int i = 0; i < mapPoints.size(); ++i)
 			{
@@ -1024,7 +1156,7 @@ Transform OdometryORBSLAM2::computeTransform(
 	UINFO("Odom update time = %fs, map points=%d, keyframes=%d, lost=%s", timer.elapsed(), totalMapPoints, totalKfs, t.isNull()?"true":"false");

 #else
-	UERROR("RTAB-Map is not built with ORB_SLAM2 support! Select another visual odometry approach.");
+	UERROR("RTAB-Map is not built with ORB_SLAM support! Select another visual odometry approach.");
 #endif
 	return t;
 }
@@ -40,7 +40,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/core/util3d.h>


-#if defined(RTABMAP_G2O) || defined(RTABMAP_ORB_SLAM2)
+#if defined(RTABMAP_G2O) || defined(RTABMAP_ORB_SLAM)
 #include "g2o/core/sparse_optimizer.h"
 #include "g2o/core/block_solver.h"
 #include "g2o/core/factory.h"
@@ -71,7 +71,7 @@ typedef Eigen::Matrix<double,Eigen::Dynamic,Eigen::Dynamic,Eigen::ColMajor> Matr
 #endif
 #endif // RTABMAP_G2O

-#ifdef RTABMAP_ORB_SLAM2
+#ifdef RTABMAP_ORB_SLAM
 #include "g2o/types/types_sba.h"
 #include "g2o/types/types_six_dof_expmap.h"
 #include "g2o/solvers/linear_solver_eigen.h"
@@ -102,7 +102,7 @@ typedef VertexPointXYZ VertexSBAPointXYZ;
 #endif
 #endif

-#endif // end defined(RTABMAP_G2O) || defined(RTABMAP_ORB_SLAM2)
+#endif // end defined(RTABMAP_G2O) || defined(RTABMAP_ORB_SLAM)

 enum {
    PARAM_OFFSET=0,
@@ -112,7 +112,7 @@ namespace rtabmap {

 bool OptimizerG2O::available()
 {
-#if defined(RTABMAP_G2O) || defined(RTABMAP_ORB_SLAM2)
+#if defined(RTABMAP_G2O) || defined(RTABMAP_ORB_SLAM)
 	return true;
 #else
 	return false;
@@ -149,10 +149,10 @@ void OptimizerG2O::parseParameters(const ParametersMap & parameters)
 	UASSERT(pixelVariance_ > 0.0);
 	UASSERT(baseline_ >= 0.0);

-#ifdef RTABMAP_ORB_SLAM2
+#ifdef RTABMAP_ORB_SLAM
 	if(solver_ != 3)
 	{
-		UWARN("g2o built with ORB_SLAM2 has only Eigen solver available, using Eigen=3 instead of %d.", solver_);
+		UWARN("g2o built with ORB_SLAM has only Eigen solver available, using Eigen=3 instead of %d.", solver_);
 		solver_ = 3;
 	}
 #else
@@ -1165,8 +1165,8 @@ std::map<int, Transform> OptimizerG2O::optimize(
 	}
 	UDEBUG("Optimizing graph...end!");
 #else
-#ifdef RTABMAP_ORB_SLAM2
-	UERROR("G2O graph optimization cannot be used with g2o built from ORB_SLAM2, only SBA is available.");
+#ifdef RTABMAP_ORB_SLAM
+	UERROR("G2O graph optimization cannot be used with g2o built from ORB_SLAM, only SBA is available.");
 #else
 	UERROR("Not built with G2O support!");
 #endif
@@ -1174,7 +1174,7 @@ std::map<int, Transform> OptimizerG2O::optimize(
 	return optimizedPoses;
 }

-#ifdef RTABMAP_ORB_SLAM2
+#ifdef RTABMAP_ORB_SLAM
 /**
 * \brief 3D edge between two SBAcam
 */
@@ -1259,7 +1259,7 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
 		std::set<int> * outliers)
 {
 	std::map<int, Transform> optimizedPoses;
-#if defined(RTABMAP_G2O) || defined(RTABMAP_ORB_SLAM2)
+#if defined(RTABMAP_G2O) || defined(RTABMAP_ORB_SLAM)
 	UDEBUG("Optimizing graph...");

 	optimizedPoses.clear();
@@ -1267,13 +1267,13 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
 	{
 		g2o::SparseOptimizer optimizer;
 		//optimizer.setVerbose(ULogger::level()==ULogger::kDebug);
-#if defined(RTABMAP_G2O_CPP11) and not defined(RTABMAP_ORB_SLAM2)
+#if defined(RTABMAP_G2O_CPP11) and not defined(RTABMAP_ORB_SLAM)
 		std::unique_ptr<g2o::BlockSolver_6_3::LinearSolverType> linearSolver;
 #else
 		g2o::BlockSolver_6_3::LinearSolverType * linearSolver = 0;
 #endif

-#ifdef RTABMAP_ORB_SLAM2
+#ifdef RTABMAP_ORB_SLAM
 		linearSolver = new g2o::LinearSolverEigen<g2o::BlockSolver_6_3::PoseMatrixType>();
 #else
 		if(solver_ == 3)
@@ -1316,9 +1316,9 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
 			linearSolver = new g2o::LinearSolverPCG<g2o::BlockSolver_6_3::PoseMatrixType>();
 #endif
 		}
-#endif // RTABMAP_ORB_SLAM2
+#endif // RTABMAP_ORB_SLAM

-#ifndef RTABMAP_ORB_SLAM2
+#ifndef RTABMAP_ORB_SLAM
 		if(optimizer_ == 1)
 		{
 #ifdef RTABMAP_G2O_CPP11
@@ -1331,7 +1331,7 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
 		else
 #endif
 		{
-#if defined(RTABMAP_G2O_CPP11) and not defined(RTABMAP_ORB_SLAM2)
+#if defined(RTABMAP_G2O_CPP11) and not defined(RTABMAP_ORB_SLAM)
 			optimizer.setAlgorithm(new g2o::OptimizationAlgorithmLevenberg(
 					g2o::make_unique<g2o::BlockSolver_6_3>(std::move(linearSolver))));
 #else
@@ -1353,14 +1353,14 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(

 				// Add node's pose
 				UASSERT(!camPose.isNull());
-#ifdef RTABMAP_ORB_SLAM2
+#ifdef RTABMAP_ORB_SLAM
 				g2o::VertexSE3Expmap * vCam = new g2o::VertexSE3Expmap();
 #else
 				g2o::VertexCam * vCam = new g2o::VertexCam();
 #endif

 				Eigen::Affine3d a = camPose.toEigen3d();
-#ifdef RTABMAP_ORB_SLAM2
+#ifdef RTABMAP_ORB_SLAM
 				a = a.inverse();
 				vCam->setEstimate(g2o::SE3Quat(a.linear(), a.translation()));
 #else
@@ -1407,7 +1407,7 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(

 				if(id1 == id2)
 				{
-#ifndef RTABMAP_ORB_SLAM2
+#ifndef RTABMAP_ORB_SLAM
 					g2o::HyperGraph::Edge * edge = 0;
 					if(gravitySigma() > 0 && iter->second.type() == Link::kGravity && poses.find(iter->first) != poses.end())
 					{
@@ -1455,7 +1455,7 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
 					//		id1,
 					//		id2,
 					//		camLink.prettyPrint().c_str());
-#ifdef RTABMAP_ORB_SLAM2
+#ifdef RTABMAP_ORB_SLAM
 					EdgeSE3Expmap * e = new EdgeSE3Expmap();
 					g2o::VertexSE3Expmap* v1 = (g2o::VertexSE3Expmap*)optimizer.vertex(id1);
 					g2o::VertexSE3Expmap* v2 = (g2o::VertexSE3Expmap*)optimizer.vertex(id2);
@@ -1537,7 +1537,7 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(

 						g2o::OptimizableGraph::Edge * e;
 						double baseline = 0.0;
-#ifdef RTABMAP_ORB_SLAM2
+#ifdef RTABMAP_ORB_SLAM
 						g2o::VertexSE3Expmap* vcam = dynamic_cast<g2o::VertexSE3Expmap*>(optimizer.vertex(camId));
 						std::map<int, CameraModel>::const_iterator iterModel = models.find(camId);

@@ -1551,7 +1551,7 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
 						if(uIsFinite(depth) && depth > 0.0 && baseline > 0.0)
 						{
 							// stereo edge
-#ifdef RTABMAP_ORB_SLAM2
+#ifdef RTABMAP_ORB_SLAM
 							g2o::EdgeStereoSE3ProjectXYZ* es = new g2o::EdgeStereoSE3ProjectXYZ();
 							float disparity = baseline * iterModel->second.fx() / depth;
 							Eigen::Vector3d obs( pt.kpt.pt.x, pt.kpt.pt.y, pt.kpt.pt.x-disparity);
@@ -1584,7 +1584,7 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
 										vpt3d->id()-stepVertexId, camId, (int)pt.kpt.pt.x, (int)pt.kpt.pt.y, depth);
 							}
 							// mono edge
-#ifdef RTABMAP_ORB_SLAM2
+#ifdef RTABMAP_ORB_SLAM
 							g2o::EdgeSE3ProjectXYZ* em = new g2o::EdgeSE3ProjectXYZ();
 							Eigen::Vector2d obs( pt.kpt.pt.x, pt.kpt.pt.y);
 							em->setMeasurement(obs);
@@ -1662,7 +1662,7 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
 						(*iter)->setLevel(1);
 						++outliersCount;
 						double d = 0.0;
-#ifdef RTABMAP_ORB_SLAM2
+#ifdef RTABMAP_ORB_SLAM
 						if(dynamic_cast<g2o::EdgeStereoSE3ProjectXYZ*>(*iter) != 0)
 						{
 							d = ((g2o::EdgeStereoSE3ProjectXYZ*)(*iter))->measurement()[0]-((g2o::EdgeStereoSE3ProjectXYZ*)(*iter))->measurement()[2];
@@ -1716,7 +1716,7 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
 		{
 			if(iter->first > 0)
 			{
-#ifdef RTABMAP_ORB_SLAM2
+#ifdef RTABMAP_ORB_SLAM
 				const g2o::VertexSE3Expmap* v = (const g2o::VertexSE3Expmap*)optimizer.vertex(iter->first);
 #else
 				const g2o::VertexCam* v = (const g2o::VertexCam*)optimizer.vertex(iter->first);
@@ -1725,7 +1725,7 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
 				{
 					Transform t = Transform::fromEigen3d(v->estimate());

-#ifdef RTABMAP_ORB_SLAM2
+#ifdef RTABMAP_ORB_SLAM
 					t=t.inverse();
 #endif

@@ -254,6 +254,7 @@ public:
 	bool isSourceDatabaseStereoToDepth() const;
 	bool isSourceRGBDColorOnly() const;
 	int getIMUFilteringStrategy() const;
+	bool getIMUFilteringBaseFrameConversion() const;
 	bool isDepthFilteringAvailable() const;
 	QString getSourceDistortionModel() const;
 	bool isBilateralFiltering() const;
@@ -337,7 +338,7 @@ private Q_SLOTS:
 	void useOdomFeatures();
 	void changeWorkingDirectory();
 	void changeDictionaryPath();
-	void changeOdometryORBSLAM2Vocabulary();
+	void changeOdometryORBSLAMVocabulary();
 	void changeOdometryOKVISConfigPath();
 	void changeOdometryVINSConfigPath();
 	void changeIcpPMConfigPath();
@@ -189,12 +189,17 @@ AboutDialog::AboutDialog(QWidget * parent) :
 	_ui->label_dvo->setText("No");
 	_ui->label_dvo_license->setEnabled(false);
 #endif
-#ifdef RTABMAP_ORB_SLAM2
-	_ui->label_orbslam2->setText("Yes");
-	_ui->label_orbslam2_license->setEnabled(true);
+#ifdef RTABMAP_ORB_SLAM
+#if RTABMAP_ORB_SLAM == 3
+	_ui->label_orbslam_title->setText("With ORB SLAM3 :");
+#elif RTABMAP_ORB_SLAM == 2
+	_ui->label_orbslam_title->setText("With ORB SLAM2 :");
+#endif
+	_ui->label_orbslam->setText("Yes");
+	_ui->label_orbslam_license->setEnabled(true);
 #else
-	_ui->label_orbslam2->setText("No");
-	_ui->label_orbslam2_license->setEnabled(false);
+	_ui->label_orbslam->setText("No");
+	_ui->label_orbslam_license->setEnabled(false);
 #endif

 #ifdef RTABMAP_OKVIS
@@ -1466,7 +1466,7 @@ void MainWindow::processOdometry(const rtabmap::OdometryEvent & odom, bool dataI
 	{
 		if(_ui->imageView_odometry->isFeaturesShown())
 		{
-			if(odom.info().type == (int)Odometry::kTypeF2M || odom.info().type == (int)Odometry::kTypeORBSLAM2)
+			if(odom.info().type == (int)Odometry::kTypeF2M || odom.info().type == (int)Odometry::kTypeORBSLAM)
 			{
 				if(_preferencesDialog->isOdomOnlyInliersShown())
 				{
@@ -1539,7 +1539,7 @@ void MainWindow::processOdometry(const rtabmap::OdometryEvent & odom, bool dataI
 			}

 			if( odom.info().type == (int)Odometry::kTypeF2M ||
-				odom.info().type == (int)Odometry::kTypeORBSLAM2 ||
+				odom.info().type == (int)Odometry::kTypeORBSLAM ||
 				odom.info().type == (int)Odometry::kTypeMSCKF ||
 				odom.info().type == (int)Odometry::kTypeVINS)
 			{
@@ -5350,7 +5350,7 @@ void MainWindow::startDetection()
 			(float)_preferencesDialog->getSourceScanForceGroundNormalsUp());
 	if(_preferencesDialog->getIMUFilteringStrategy()>0 && dynamic_cast<DBReader*>(camera) == 0)
 	{
-		_camera->enableIMUFiltering(_preferencesDialog->getIMUFilteringStrategy()-1, parameters);
+		_camera->enableIMUFiltering(_preferencesDialog->getIMUFilteringStrategy()-1, parameters, _preferencesDialog->getIMUFilteringBaseFrameConversion());
 	}
 	if(_preferencesDialog->isDepthFilteringAvailable())
 	{
@@ -395,7 +395,7 @@ void OdometryViewer::processData(const rtabmap::OdometryEvent & odom)

 	if(!odom.data().imageRaw().empty())
 	{
-		if(odom.info().type == (int)Odometry::kTypeF2M || odom.info().type == (int)Odometry::kTypeORBSLAM2)
+		if(odom.info().type == (int)Odometry::kTypeF2M || odom.info().type == (int)Odometry::kTypeORBSLAM)
 		{
 			imageView_->setFeatures(odom.info().words, odom.data().depthRaw(), Qt::yellow);
 		}
@@ -438,7 +438,7 @@ void OdometryViewer::processData(const rtabmap::OdometryEvent & odom)
 			}

 			if( odom.info().type == Odometry::kTypeF2M ||
-				odom.info().type == (int)Odometry::kTypeORBSLAM2 ||
+				odom.info().type == (int)Odometry::kTypeORBSLAM ||
 				odom.info().type == (int)Odometry::kTypeMSCKF)
 			{
 				if(imageView_->isFeaturesShown())
@@ -171,8 +171,14 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 #ifndef RTABMAP_DVO
 	_ui->odom_strategy->setItemData(4, 0, Qt::UserRole - 1);
 #endif
-#ifndef RTABMAP_ORB_SLAM2
+#ifndef RTABMAP_ORB_SLAM
 	_ui->odom_strategy->setItemData(5, 0, Qt::UserRole - 1);
+#elif RTABMAP_ORB_SLAM == 3
+	_ui->odom_strategy->setItemText(5, "ORB SLAM 3");
+	_ui->groupBox_odomORBSLAM->setTitle("ORB SLAM 3");
+#else
+	_ui->odom_strategy->setItemText(5, "ORB SLAM 2");
+	_ui->groupBox_odomORBSLAM->setTitle("ORB SLAM 2");
 #endif
 #ifndef RTABMAP_OKVIS
 	_ui->odom_strategy->setItemData(6, 0, Qt::UserRole - 1);
@@ -266,7 +272,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 		_ui->loopClosure_bundle->setItemData(1, 0, Qt::UserRole - 1);
 		_ui->groupBoxx_g2o->setEnabled(false);
 	}
-#ifdef RTABMAP_ORB_SLAM2
+#ifdef RTABMAP_ORB_SLAM
 	else
 	{
 		// only graph optimization is disabled, g2o (from ORB_SLAM2) is valid only for SBA
@@ -616,7 +622,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :

 	//Source panel
 	connect(_ui->general_doubleSpinBox_imgRate, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteSourcePanel()));
-	connect(_ui->general_doubleSpinBox_imgRate, SIGNAL(valueChanged(double)), _ui->doubleSpinBox_OdomORBSLAM2Fps, SLOT(setValue(double)));
+	connect(_ui->general_doubleSpinBox_imgRate, SIGNAL(valueChanged(double)), _ui->doubleSpinBox_OdomORBSLAMFps, SLOT(setValue(double)));
 	connect(_ui->source_mirroring, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->toolButton_source_path_calibration, SIGNAL(clicked()), this, SLOT(selectCalibrationPath()));
 	connect(_ui->lineEdit_calibrationFile, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel()));
@@ -804,6 +810,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	connect(_ui->comboBox_imuFilter_strategy, SIGNAL(currentIndexChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->comboBox_imuFilter_strategy, SIGNAL(currentIndexChanged(int)), _ui->stackedWidget_imuFilter, SLOT(setCurrentIndex(int)));
 	_ui->stackedWidget_imuFilter->setCurrentIndex(_ui->comboBox_imuFilter_strategy->currentIndex());
+	connect(_ui->checkBox_imuFilter_baseFrameConversion, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->checkbox_publishInterIMU, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));

 	connect(_ui->checkBox_source_scanFromDepth, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
@@ -1312,14 +1319,14 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	_ui->doubleSpinBox_OdomViso2BucketWidth->setObjectName(Parameters::kOdomViso2BucketWidth().c_str());
 	_ui->doubleSpinBox_OdomViso2BucketHeight->setObjectName(Parameters::kOdomViso2BucketHeight().c_str());

-	// Odometry ORBSLAM2
-	_ui->lineEdit_OdomORBSLAM2VocPath->setObjectName(Parameters::kOdomORBSLAM2VocPath().c_str());
-	connect(_ui->toolButton_OdomORBSLAM2VocPath, SIGNAL(clicked()), this, SLOT(changeOdometryORBSLAM2Vocabulary()));
-	_ui->doubleSpinBox_OdomORBSLAM2Bf->setObjectName(Parameters::kOdomORBSLAM2Bf().c_str());
-	_ui->doubleSpinBox_OdomORBSLAM2ThDepth->setObjectName(Parameters::kOdomORBSLAM2ThDepth().c_str());
-	_ui->doubleSpinBox_OdomORBSLAM2Fps->setObjectName(Parameters::kOdomORBSLAM2Fps().c_str());
-	_ui->spinBox_OdomORBSLAM2MaxFeatures->setObjectName(Parameters::kOdomORBSLAM2MaxFeatures().c_str());
-	_ui->spinBox_OdomORBSLAM2MapSize->setObjectName(Parameters::kOdomORBSLAM2MapSize().c_str());
+	// Odometry ORBSLAM
+	_ui->lineEdit_OdomORBSLAMVocPath->setObjectName(Parameters::kOdomORBSLAMVocPath().c_str());
+	connect(_ui->toolButton_OdomORBSLAMVocPath, SIGNAL(clicked()), this, SLOT(changeOdometryORBSLAMVocabulary()));
+	_ui->doubleSpinBox_OdomORBSLAMBf->setObjectName(Parameters::kOdomORBSLAMBf().c_str());
+	_ui->doubleSpinBox_OdomORBSLAMThDepth->setObjectName(Parameters::kOdomORBSLAMThDepth().c_str());
+	_ui->doubleSpinBox_OdomORBSLAMFps->setObjectName(Parameters::kOdomORBSLAMFps().c_str());
+	_ui->spinBox_OdomORBSLAMMaxFeatures->setObjectName(Parameters::kOdomORBSLAMMaxFeatures().c_str());
+	_ui->spinBox_OdomORBSLAMMapSize->setObjectName(Parameters::kOdomORBSLAMMapSize().c_str());

 	// Odometry Okvis
 	_ui->lineEdit_OdomOkvisPath->setObjectName(Parameters::kOdomOKVISConfigPath().c_str());
@@ -2008,13 +2015,14 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox)
 		_ui->lineEdit_cameraImages_imu_transform->setText("0 0 1 0 -1 0 1 0 0");
 		_ui->spinBox_cameraImages_max_imu_rate->setValue(0);

-		_ui->comboBox_imuFilter_strategy->setCurrentIndex(1);
+		_ui->comboBox_imuFilter_strategy->setCurrentIndex(2);
 		_ui->doubleSpinBox_imuFilterMadgwickGain->setValue(Parameters::defaultImuFilterMadgwickGain());
 		_ui->doubleSpinBox_imuFilterMadgwickZeta->setValue(Parameters::defaultImuFilterMadgwickZeta());
 		_ui->doubleSpinBox_imuFilterComplementaryGainAcc->setValue(Parameters::defaultImuFilterComplementaryGainAcc());
 		_ui->doubleSpinBox_imuFilterComplementaryBiasAlpha->setValue(Parameters::defaultImuFilterComplementaryBiasAlpha());
 		_ui->checkBox_imuFilterComplementaryDoAdaptiveGain->setChecked(Parameters::defaultImuFilterComplementaryDoAdpativeGain());
 		_ui->checkBox_imuFilterComplementaryDoBiasEstimation->setChecked(Parameters::defaultImuFilterComplementaryDoBiasEstimation());
+		_ui->checkBox_imuFilter_baseFrameConversion->setChecked(true);
 		_ui->checkbox_publishInterIMU->setChecked(false);

 		_ui->checkBox_source_scanFromDepth->setChecked(false);
@@ -2505,6 +2513,7 @@ void PreferencesDialog::readCameraSettings(const QString & filePath)
 	_ui->doubleSpinBox_imuFilterComplementaryBiasAlpha->setValue(settings.value("complementary_bias_alpha", _ui->doubleSpinBox_imuFilterComplementaryBiasAlpha->value()).toDouble());
 	_ui->checkBox_imuFilterComplementaryDoAdaptiveGain->setChecked(settings.value("complementary_adaptive_gain", _ui->checkBox_imuFilterComplementaryDoAdaptiveGain->isChecked()).toBool());
 	_ui->checkBox_imuFilterComplementaryDoBiasEstimation->setChecked(settings.value("complementary_biais_estimation", _ui->checkBox_imuFilterComplementaryDoBiasEstimation->isChecked()).toBool());
+	_ui->checkBox_imuFilter_baseFrameConversion->setChecked(settings.value("base_frame_conversion", _ui->checkBox_imuFilter_baseFrameConversion->isChecked()).toBool());
 	_ui->checkbox_publishInterIMU->setChecked(settings.value("publish_inter_imu", _ui->checkbox_publishInterIMU->isChecked()).toBool());
 	settings.endGroup();//IMU

@@ -2997,6 +3006,7 @@ void PreferencesDialog::writeCameraSettings(const QString & filePath) const
 	settings.setValue("complementary_bias_alpha",       _ui->doubleSpinBox_imuFilterComplementaryBiasAlpha->value());
 	settings.setValue("complementary_adaptive_gain",    _ui->checkBox_imuFilterComplementaryDoAdaptiveGain->isChecked());
 	settings.setValue("complementary_biais_estimation", _ui->checkBox_imuFilterComplementaryDoBiasEstimation->isChecked());
+	settings.setValue("base_frame_conversion",          _ui->checkBox_imuFilter_baseFrameConversion->isChecked());
 	settings.setValue("publish_inter_imu",              _ui->checkbox_publishInterIMU->isChecked());
 	settings.endGroup();//IMU

@@ -3150,7 +3160,7 @@ bool PreferencesDialog::validateForm()
 					   "with TORO. GTSAM is set instead for graph optimization strategy."));
 			_ui->graphOptimization_type->setCurrentIndex(Optimizer::kTypeGTSAM);
 		}
-#ifndef RTABMAP_ORB_SLAM2
+#ifndef RTABMAP_ORB_SLAM
 		else if(Optimizer::isAvailable(Optimizer::kTypeG2O))
 		{
 			QMessageBox::warning(this, tr("Parameter warning"),
@@ -3160,7 +3170,7 @@ bool PreferencesDialog::validateForm()
 		}
 #endif
 	}
-#ifdef RTABMAP_ORB_SLAM2
+#ifdef RTABMAP_ORB_SLAM
 	if(_ui->graphOptimization_type->currentIndex() == 1)
 #else
 	if(_ui->graphOptimization_type->currentIndex() == 1 && !Optimizer::isAvailable(Optimizer::kTypeG2O))
@@ -3183,7 +3193,7 @@ bool PreferencesDialog::validateForm()
 	}
 	if(_ui->graphOptimization_type->currentIndex() == 2 && !Optimizer::isAvailable(Optimizer::kTypeGTSAM))
 	{
-#ifndef RTABMAP_ORB_SLAM2
+#ifndef RTABMAP_ORB_SLAM
 		if(Optimizer::isAvailable(Optimizer::kTypeG2O))
 		{
 			QMessageBox::warning(this, tr("Parameter warning"),
@@ -3768,7 +3778,10 @@ void PreferencesDialog::updateParameters(const ParametersMap & parameters, bool

 void PreferencesDialog::selectSourceDriver(Src src, int variant)
 {
-	_ui->comboBox_imuFilter_strategy->setCurrentIndex(1);
+	if(_ui->comboBox_imuFilter_strategy->currentIndex()==0)
+	{
+		_ui->comboBox_imuFilter_strategy->setCurrentIndex(2);
+	}
 	_3dRenderingRoiRatios[0]->setText("0.0 0.0 0.0 0.0");
 	_3dRenderingRoiRatios[1]->setText("0.0 0.0 0.0 0.0");

@@ -4309,7 +4322,7 @@ void PreferencesDialog::setParameter(const std::string & key, const std::string
 				}
 #endif
 #endif
-#ifndef RTABMAP_ORB_SLAM2
+#ifndef RTABMAP_ORB_SLAM
 				if(!Optimizer::isAvailable(Optimizer::kTypeG2O))
 #endif
 				{
@@ -4861,20 +4874,20 @@ void PreferencesDialog::changeDictionaryPath()
 	}
 }

-void PreferencesDialog::changeOdometryORBSLAM2Vocabulary()
+void PreferencesDialog::changeOdometryORBSLAMVocabulary()
 {
 	QString path;
-	if(_ui->lineEdit_OdomORBSLAM2VocPath->text().isEmpty())
+	if(_ui->lineEdit_OdomORBSLAMVocPath->text().isEmpty())
 	{
-		path = QFileDialog::getOpenFileName(this, tr("ORBSLAM2 Vocabulary"), this->getWorkingDirectory(), tr("Vocabulary (*.txt)"));
+		path = QFileDialog::getOpenFileName(this, tr("ORBSLAM Vocabulary"), this->getWorkingDirectory(), tr("Vocabulary (*.txt)"));
 	}
 	else
 	{
-		path = QFileDialog::getOpenFileName(this, tr("ORBSLAM2 Vocabulary"), _ui->lineEdit_OdomORBSLAM2VocPath->text(),  tr("Vocabulary (*.txt)"));
+		path = QFileDialog::getOpenFileName(this, tr("ORBSLAM Vocabulary"), _ui->lineEdit_OdomORBSLAMVocPath->text(),  tr("Vocabulary (*.txt)"));
 	}
 	if(!path.isEmpty())
 	{
-		_ui->lineEdit_OdomORBSLAM2VocPath->setText(path);
+		_ui->lineEdit_OdomORBSLAMVocPath->setText(path);
 	}
 }

@@ -5631,6 +5644,10 @@ int PreferencesDialog::getIMUFilteringStrategy() const
 {
 	return _ui->comboBox_imuFilter_strategy->currentIndex();
 }
+bool PreferencesDialog::getIMUFilteringBaseFrameConversion() const
+{
+	return _ui->checkBox_imuFilter_baseFrameConversion->isChecked();
+}
 bool PreferencesDialog::isDepthFilteringAvailable() const
 {
 	return _ui->groupBox_depthImageFiltering->isEnabled();
@@ -6398,7 +6415,7 @@ void PreferencesDialog::testOdometry()
 			(float)_ui->doubleSpinBox_source_scanNormalsForceGroundUp->value());
 	if(_ui->comboBox_imuFilter_strategy->currentIndex()>0 && dynamic_cast<DBReader*>(camera) == 0)
 	{
-		cameraThread.enableIMUFiltering(_ui->comboBox_imuFilter_strategy->currentIndex()-1, this->getAllParameters());
+		cameraThread.enableIMUFiltering(_ui->comboBox_imuFilter_strategy->currentIndex()-1, this->getAllParameters(), _ui->checkBox_imuFilter_baseFrameConversion->isChecked());
 	}
 	if(isDepthFilteringAvailable())
 	{
@@ -6469,7 +6486,7 @@ void PreferencesDialog::testCamera()
 				(float)_ui->doubleSpinBox_source_scanNormalsForceGroundUp->value());
 		if(_ui->comboBox_imuFilter_strategy->currentIndex()>0 && dynamic_cast<DBReader*>(camera) == 0)
 		{
-			cameraThread.enableIMUFiltering(_ui->comboBox_imuFilter_strategy->currentIndex()-1, this->getAllParameters());
+			cameraThread.enableIMUFiltering(_ui->comboBox_imuFilter_strategy->currentIndex()-1, this->getAllParameters(), _ui->checkBox_imuFilter_baseFrameConversion->isChecked());
 		}
 		if(isDepthFilteringAvailable())
 		{
@@ -162,9 +162,9 @@ p, li { white-space: pre-wrap; }
           <property name="geometry">
            <rect>
             <x>0</x>
-             <y>-228</y>
+             <y>-567</y>
             <width>596</width>
-             <height>929</height>
+             <height>912</height>
            </rect>
           </property>
           <layout class="QVBoxLayout" name="verticalLayout_2">
@@ -430,7 +430,7 @@ p, li { white-space: pre-wrap; }
               </widget>
              </item>
              <item row="35" column="0">
-               <widget class="QLabel" name="label_28">
+               <widget class="QLabel" name="label_orbslam_title">
                <property name="text">
                 <string>With ORB SLAM 2 :</string>
                </property>
@@ -450,7 +450,7 @@ p, li { white-space: pre-wrap; }
               </widget>
              </item>
              <item row="35" column="1">
-               <widget class="QLabel" name="label_orbslam2">
+               <widget class="QLabel" name="label_orbslam">
                <property name="text">
                 <string/>
                </property>
@@ -944,7 +944,7 @@ p, li { white-space: pre-wrap; }
               </widget>
              </item>
              <item row="35" column="2">
-               <widget class="QLabel" name="label_orbslam2_license">
+               <widget class="QLabel" name="label_orbslam_license">
                <property name="text">
                 <string>GPLv3</string>
                </property>
@@ -65,7 +65,7 @@
            <x>0</x>
            <y>-265</y>
            <width>686</width>
-            <height>3357</height>
+            <height>3397</height>
           </rect>
          </property>
          <layout class="QVBoxLayout" name="verticalLayout_16">
@@ -7212,7 +7212,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                           </property>
                          </widget>
                         </item>
-                         <item row="1" column="0">
+                         <item row="2" column="0">
                          <widget class="QCheckBox" name="checkbox_publishInterIMU">
                           <property name="text">
                            <string/>
@@ -7222,16 +7222,36 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                           </property>
                          </widget>
                         </item>
-                         <item row="1" column="1">
+                         <item row="2" column="1">
                          <widget class="QLabel" name="label_563">
                           <property name="text">
-                            <string>Publish inter IMU messages from the camera. IMU received between images will be published as separate topic.</string>
+                            <string>Publish inter IMU messages from the camera. IMU received between images will be published as separate topic. IMU filtering strategy will be ignored.</string>
                           </property>
                           <property name="wordWrap">
                            <bool>true</bool>
                           </property>
                          </widget>
                         </item>
+                         <item row="1" column="1">
+                          <widget class="QLabel" name="label_621">
+                           <property name="text">
+                            <string>Convert IMU in base frame before filtering. This can help to initialize correctly the yaw.</string>
+                           </property>
+                           <property name="wordWrap">
+                            <bool>true</bool>
+                           </property>
+                          </widget>
+                         </item>
+                         <item row="1" column="0">
+                          <widget class="QCheckBox" name="checkBox_imuFilter_baseFrameConversion">
+                           <property name="text">
+                            <string/>
+                           </property>
+                           <property name="checked">
+                            <bool>false</bool>
+                           </property>
+                          </widget>
+                         </item>
                        </layout>
                       </item>
                       <item>
@@ -13572,7 +13592,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                        </item>
                        <item>
                         <property name="text">
-                          <string>ORB SLAM 2</string>
+                          <string>ORB SLAM</string>
                         </property>
                        </item>
                        <item>
@@ -13898,7 +13918,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                    <item>
                     <widget class="QStackedWidget" name="stackedWidget_odometryType">
                      <property name="currentIndex">
-                       <number>0</number>
+                       <number>5</number>
                      </property>
                      <widget class="QWidget" name="page_52">
                       <layout class="QVBoxLayout" name="verticalLayout_77">
@@ -15778,15 +15798,15 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                      <widget class="QWidget" name="page_70">
                       <layout class="QVBoxLayout" name="verticalLayout_125">
                        <item>
-                         <widget class="QGroupBox" name="groupBox_odomORBSLAM2">
+                         <widget class="QGroupBox" name="groupBox_odomORBSLAM">
                          <property name="title">
-                           <string>ORB SLAM 2</string>
+                           <string>ORB SLAM</string>
                          </property>
                          <layout class="QVBoxLayout" name="verticalLayout_124">
                           <item>
                            <widget class="QLabel" name="label_412">
                             <property name="text">
-                              <string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;ORB SLAM 2: &lt;a href=&quot;https://github.com/raulmur/ORB_SLAM2&quot;&gt;&lt;span style=&quot; text-decoration: underline; color:#0000ff;&quot;&gt;https://github.com/raulmur/ORB_SLAM2&lt;/span&gt;&lt;/a&gt;&lt;/p&gt;&lt;p&gt;Tracking and local mapping from ORB SLAM 2 is done but loop closure detection is disabled, using RTAB-Map loop closure detection instead. See ORB panel for ORB parameters (pyramid scale and number of pyramids). See FAST panel for FAST parameters (init and min thresholds). Vis/MaxFeatures (see Visual Registration panel) is used as the number of ORB features extracted.&lt;br/&gt;&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
+                              <string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;ORB SLAM2: &lt;a href=&quot;https://github.com/raulmur/ORB_SLAM2&quot;&gt;&lt;span style=&quot; text-decoration: underline; color:#0000ff;&quot;&gt;https://github.com/UZ-SLAMLab/ORB_SLAM3&lt;/span&gt;&lt;/a&gt;&lt;br/&gt;ORB SLAM3: &lt;a href=&quot;https://github.com/UZ-SLAMLab/ORB_SLAM3&quot;&gt;&lt;span style=&quot; text-decoration: underline; color:#0000ff;&quot;&gt;https://github.com/raulmur/ORB_SLAM2&lt;/span&gt;&lt;/a&gt;&lt;/p&gt;&lt;p&gt;Tracking and local mapping from ORB SLAM is done but loop closure detection is disabled, using RTAB-Map loop closure detection instead. See ORB panel for ORB parameters (pyramid scale and number of pyramids). See FAST panel for FAST parameters (init and min thresholds). Vis/MaxFeatures (see Visual Registration panel) is used as the number of ORB features extracted.&lt;br/&gt;&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
                             </property>
                             <property name="wordWrap">
                              <bool>true</bool>
@@ -15802,7 +15822,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                           <item>
                            <layout class="QGridLayout" name="gridLayout_93" columnstretch="0,0,1">
                             <item row="1" column="1">
-                              <widget class="QDoubleSpinBox" name="doubleSpinBox_OdomORBSLAM2Bf">
+                              <widget class="QDoubleSpinBox" name="doubleSpinBox_OdomORBSLAMBf">
                               <property name="suffix">
                                <string> m</string>
                               </property>
@@ -15837,7 +15857,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                              </widget>
                             </item>
                             <item row="2" column="1">
-                              <widget class="QDoubleSpinBox" name="doubleSpinBox_OdomORBSLAM2ThDepth">
+                              <widget class="QDoubleSpinBox" name="doubleSpinBox_OdomORBSLAMThDepth">
                               <property name="suffix">
                                <string/>
                               </property>
@@ -15872,7 +15892,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                              </widget>
                             </item>
                             <item row="0" column="1">
-                              <widget class="QLineEdit" name="lineEdit_OdomORBSLAM2VocPath"/>
+                              <widget class="QLineEdit" name="lineEdit_OdomORBSLAMVocPath"/>
                             </item>
                             <item row="0" column="2">
                              <widget class="QLabel" name="label_415">
@@ -15888,7 +15908,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                              </widget>
                             </item>
                             <item row="0" column="0">
-                              <widget class="QToolButton" name="toolButton_OdomORBSLAM2VocPath">
+                              <widget class="QToolButton" name="toolButton_OdomORBSLAMVocPath">
                               <property name="text">
                                <string>...</string>
                               </property>
@@ -15908,7 +15928,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                              </widget>
                             </item>
                             <item row="3" column="1">
-                              <widget class="QDoubleSpinBox" name="doubleSpinBox_OdomORBSLAM2Fps">
+                              <widget class="QDoubleSpinBox" name="doubleSpinBox_OdomORBSLAMFps">
                               <property name="enabled">
                                <bool>false</bool>
                               </property>
@@ -15946,7 +15966,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                              </widget>
                             </item>
                             <item row="4" column="1">
-                              <widget class="QSpinBox" name="spinBox_OdomORBSLAM2MaxFeatures">
+                              <widget class="QSpinBox" name="spinBox_OdomORBSLAMMaxFeatures">
                               <property name="maximum">
                                <number>99999</number>
                               </property>
@@ -15969,7 +15989,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                              </widget>
                             </item>
                             <item row="5" column="1">
-                              <widget class="QSpinBox" name="spinBox_OdomORBSLAM2MapSize">
+                              <widget class="QSpinBox" name="spinBox_OdomORBSLAMMapSize">
                               <property name="maximum">
                                <number>999999</number>
                               </property>
@@ -161,7 +161,7 @@ int main (int argc, char * argv[])
 			(float)dialog.getSourceScanForceGroundNormalsUp());
 	if(dialog.getIMUFilteringStrategy()>0 && dynamic_cast<DBReader*>(camera) == 0)
 	{
-		cam->enableIMUFiltering(dialog.getIMUFilteringStrategy()-1, parameters);
+		cam->enableIMUFiltering(dialog.getIMUFilteringStrategy()-1, parameters, dialog.getIMUFilteringBaseFrameConversion());
 	}
 	if(dialog.isDepthFilteringAvailable())
 	{