Updating orbslam3 v1 support (#1152)

* Refactoring ORB_SLAM3 integration. Fixed realsense2 inter IMU stamps.

* Renamed OdometryORBSLAM -> OdometryORBSLAM2

* revert a change

* Fixed build without orb_slam

* Source camera: added feature detection option

* Fixed jfr2018 docker files

CMakeLists.txt

@@ -814,6 +814,7 @@ IF(NOT MSVC)
            open_chisel_FOUND OR
            vins_FOUND OR
            ov_msckf_FOUND OR
+           msckf_vio_FOUND OR
            libpointmatcher_FOUND))
      #Newest versions require std11
      include(CheckCXXCompilerFlag)
@@ -829,6 +830,7 @@ IF(NOT MSVC)
     ENDIF()
 ENDIF()
 
+
 ####### OSX BUNDLE CMAKE_INSTALL_PREFIX #######
 IF(APPLE AND BUILD_AS_BUNDLE)
   IF(Qt6_FOUND OR Qt5_FOUND OR (QT4_FOUND AND QT_QTCORE_FOUND AND QT_QTGUI_FOUND))
@@ -913,9 +915,9 @@ ENDIF(NOT Open3D_FOUND)
 IF(NOT FastCV_FOUND)
    SET(FASTCV "//")
 ENDIF(NOT FastCV_FOUND)
-IF(NOT opengv_FOUND)
+IF(NOT opengv_FOUND OR NOT WITH_OPENGV)
    SET(OPENGV "//")
-ENDIF(NOT opengv_FOUND)
+ENDIF(NOT opengv_FOUND OR NOT WITH_OPENGV)
 IF(NOT PDAL_FOUND)
    SET(PDAL "//")
 ENDIF(NOT PDAL_FOUND)
@@ -1459,7 +1461,7 @@ ELSE()
 MESSAGE(STATUS "  With Open3D               = NO (Open3D not found)")
 ENDIF()
 
-IF(opengv_FOUND)
+IF(opengv_FOUND AND WITH_OPENGV)
 MESSAGE(STATUS "  With OpenGV ${opengv_VERSION}           = YES (License: BSD)")
 ELSEIF(NOT WITH_OPENGV)
 MESSAGE(STATUS "  With OpenGV               = NO (WITH_OPENGV=OFF)")

cmake_modules/FindORB_SLAM.cmake

@@ -12,6 +12,7 @@ find_path(ORB_SLAM_INCLUDE_DIR NAMES System.h PATHS $ENV{ORB_SLAM_ROOT_DIR}/incl
 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_path(sophus_INCLUDE_DIR NAMES sophus/se3.hpp PATHS $ENV{ORB_SLAM_ROOT_DIR}/Thirdparty/Sophus 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)
 
@@ -21,6 +22,9 @@ IF(ORB_SLAM2_LIBRARY)
 ELSEIF(ORB_SLAM3_LIBRARY)
    SET(ORB_SLAM_VERSION 3)
    SET(ORB_SLAM_LIBRARY ${ORB_SLAM3_LIBRARY})
+   IF(g2o_INCLUDE_DIR AND sophus_INCLUDE_DIR) # ORB_SLAM3 v1
+      SET(g2o_INCLUDE_DIR ${g2o_INCLUDE_DIR} ${sophus_INCLUDE_DIR})
+   ENDIF(g2o_INCLUDE_DIR AND sophus_INCLUDE_DIR)
 ENDIF()
 
 IF (ORB_SLAM_INCLUDE_DIR AND ORB_SLAM_LIBRARY AND DBoW2_LIBRARY AND g2o_INCLUDE_DIR AND g2o_LIBRARY)

corelib/include/rtabmap/core/CameraThread.h

@@ -47,6 +47,7 @@ class CameraInfo;
 class SensorData;
 class StereoDense;
 class IMUFilter;
+class Feature2D;
 
 /**
  * Class CameraThread
@@ -88,6 +89,8 @@ public:
 	void disableBilateralFiltering() {_bilateralFiltering = false;}
 	void enableIMUFiltering(int filteringStrategy=1, const ParametersMap & parameters = ParametersMap(), bool baseFrameConversion = false);
 	void disableIMUFiltering();
+	void enableFeatureDetection(const ParametersMap & parameters = ParametersMap());
+	void disableFeatureDetection();
 
 	// Use new version of this function with groundNormalsUp=0.8 for forceGroundNormalsUp=True and groundNormalsUp=0.0 for forceGroundNormalsUp=False.
 	RTABMAP_DEPRECATED void setScanParameters(
@@ -152,6 +155,8 @@ private:
 	float _bilateralSigmaR;
 	IMUFilter * _imuFilter;
 	bool _imuBaseFrameConversion;
+	Feature2D * _featureDetector;
+	bool _depthAsMask;
 };
 
 } // namespace rtabmap

corelib/include/rtabmap/core/Parameters.h

@@ -527,12 +527,19 @@ class RTABMAP_CORE_EXPORT Parameters
     RTABMAP_PARAM(OdomViso2, BucketHeight,              double, 50,  "Height of bucket.");
 
     // Odometry ORB_SLAM2
-    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).");
+    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 (0 to estimate from input data).");
+    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). Only supported with ORB_SLAM2.");
+    RTABMAP_PARAM(OdomORBSLAM, Inertial,        bool,   false,    "Enable IMU. Only supported with ORB_SLAM3.");
+    RTABMAP_PARAM(OdomORBSLAM, GyroNoise,       double, 0.01,     "IMU gyroscope \"white noise\".");
+    RTABMAP_PARAM(OdomORBSLAM, AccNoise,        double, 0.1,      "IMU accelerometer \"white noise\".");
+    RTABMAP_PARAM(OdomORBSLAM, GyroWalk,        double, 0.000001, "IMU gyroscope \"random walk\".");
+    RTABMAP_PARAM(OdomORBSLAM, AccWalk,         double, 0.0001,   "IMU accelerometer \"random walk\".");
+    RTABMAP_PARAM(OdomORBSLAM, SamplingRate,    double, 0,        "IMU sampling rate (0 to estimate from input data).");
+
 
     // Odometry OKVIS
     RTABMAP_PARAM_STR(OdomOKVIS, ConfigPath,     "",  "Path of OKVIS config file.");

corelib/include/rtabmap/core/odometry/OdometryORBSLAM2.h

@@ -25,48 +25,38 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */
 
-#ifndef ODOMETRYORBSLAM_H_
-#define ODOMETRYORBSLAM_H_
+#ifndef ODOMETRYORBSLAM2_H_
+#define ODOMETRYORBSLAM2_H_
 
 #include <rtabmap/core/Odometry.h>
 
-#if RTABMAP_ORB_SLAM == 3
-namespace ORB_SLAM3 {
-#else
-namespace ORB_SLAM2 {
-#endif
-class System;
-}
 
-class ORBSLAMSystem;
+class ORBSLAM2System;
 
 namespace rtabmap {
 
-class RTABMAP_CORE_EXPORT OdometryORBSLAM : public Odometry
+class RTABMAP_CORE_EXPORT OdometryORBSLAM2 : public Odometry
 {
 public:
-	OdometryORBSLAM(const rtabmap::ParametersMap & parameters = rtabmap::ParametersMap());
-	virtual ~OdometryORBSLAM();
+	OdometryORBSLAM2(const rtabmap::ParametersMap & parameters = rtabmap::ParametersMap());
+	virtual ~OdometryORBSLAM2();
 
 	virtual void reset(const Transform & initialPose = Transform::getIdentity());
 	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_SLAM
-	ORBSLAMSystem * orbslam_;
+#if defined(RTABMAP_ORB_SLAM) and RTABMAP_ORB_SLAM == 2
+	ORBSLAM2System * orbslam_;
 	bool firstFrame_;
 	Transform originLocalTransform_;
 	Transform previousPose_;
-	bool useIMU_;
-	Transform imuLocalTransform_;
 #endif
 
 };
 
 }
 
-#endif /* ODOMETRYORBSLAM_H_ */
+#endif /* ODOMETRYORBSLAM2_H_ */

corelib/include/rtabmap/core/odometry/OdometryORBSLAM3.h

@@ -0,0 +1,71 @@
+/*
+Copyright (c) 2010-2016, 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.
+*/
+
+#ifndef ODOMETRYORBSLAM3_H_
+#define ODOMETRYORBSLAM3_H_
+
+#include <rtabmap/core/Odometry.h>
+
+#if defined(RTABMAP_ORB_SLAM) and RTABMAP_ORB_SLAM == 3
+#include <System.h>
+#endif
+
+namespace rtabmap {
+
+class RTABMAP_CORE_EXPORT OdometryORBSLAM3 : public Odometry
+{
+public:
+	OdometryORBSLAM3(const rtabmap::ParametersMap & parameters = rtabmap::ParametersMap());
+	virtual ~OdometryORBSLAM3();
+
+	virtual void reset(const Transform & initialPose = Transform::getIdentity());
+	virtual Odometry::Type getType() {return Odometry::kTypeORBSLAM;}
+	virtual bool canProcessAsyncIMU() const;
+
+private:
+	virtual Transform computeTransform(SensorData & image, const Transform & guess = Transform(), OdometryInfo * info = 0);
+
+	bool init(const rtabmap::CameraModel & model, double stamp, bool stereo, double baseline);
+private:
+#if defined(RTABMAP_ORB_SLAM) and RTABMAP_ORB_SLAM == 3
+	ORB_SLAM3::System * orbslam_;
+	bool firstFrame_;
+	Transform originLocalTransform_;
+	Transform previousPose_;
+	bool useIMU_;
+	Transform imuLocalTransform_;
+	ParametersMap parameters_;
+	std::vector<ORB_SLAM3::IMU::Point> orbslamImus_;
+	double lastImuStamp_;
+	double lastImageStamp_;
+#endif
+
+};
+
+}
+
+#endif /* ODOMETRYORBSLAM_H3_ */

corelib/src/CMakeLists.txt

@@ -87,7 +87,8 @@ SET(SRC_FILES
     odometry/OdometryViso2.cpp
     odometry/OdometryDVO.cpp
     odometry/OdometryOkvis.cpp
-    odometry/OdometryORBSLAM.cpp
+    odometry/OdometryORBSLAM2.cpp
+    odometry/OdometryORBSLAM3.cpp
     odometry/OdometryLOAM.cpp
     odometry/OdometryFLOAM.cpp
     odometry/OdometryMSCKF.cpp

corelib/src/CameraThread.cpp

@@ -36,10 +36,12 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/core/StereoDense.h"
 #include "rtabmap/core/DBReader.h"
 #include "rtabmap/core/IMUFilter.h"
+#include "rtabmap/core/Features2d.h"
 #include "rtabmap/core/clams/discrete_depth_distortion_model.h"
 #include <opencv2/stitching/detail/exposure_compensate.hpp>
 #include <rtabmap/utilite/UTimer.h>
 #include <rtabmap/utilite/ULogger.h>
+#include <rtabmap/utilite/UStl.h>
 
 #include <pcl/io/io.h>
 
@@ -73,7 +75,9 @@ CameraThread::CameraThread(Camera * camera, const ParametersMap & parameters) :
 		_bilateralSigmaS(10),
 		_bilateralSigmaR(0.1),
 		_imuFilter(0),
-		_imuBaseFrameConversion(false)
+		_imuBaseFrameConversion(false),
+		_featureDetector(0),
+		_depthAsMask(Parameters::defaultVisDepthAsMask())
 {
 	UASSERT(_camera != 0);
 }
@@ -113,7 +117,9 @@ CameraThread::CameraThread(
 			_bilateralSigmaS(10),
 			_bilateralSigmaR(0.1),
 			_imuFilter(0),
-			_imuBaseFrameConversion(false)
+			_imuBaseFrameConversion(false),
+			_featureDetector(0),
+			_depthAsMask(Parameters::defaultVisDepthAsMask())
 {
 	UASSERT(_camera != 0 && _odomSensor != 0 && !_extrinsicsOdomToCamera.isNull());
 	UDEBUG("_extrinsicsOdomToCamera=%s", _extrinsicsOdomToCamera.prettyPrint().c_str());
@@ -152,7 +158,9 @@ CameraThread::CameraThread(
 			_bilateralSigmaS(10),
 			_bilateralSigmaR(0.1),
 			_imuFilter(0),
-			_imuBaseFrameConversion(false)
+			_imuBaseFrameConversion(false),
+			_featureDetector(0),
+			_depthAsMask(Parameters::defaultVisDepthAsMask())
 {
 	UASSERT(_camera != 0);
 	UDEBUG("_odomAsGt              =%s", _odomAsGt?"true":"false");
@@ -166,6 +174,7 @@ CameraThread::~CameraThread()
 	delete _distortionModel;
 	delete _stereoDense;
 	delete _imuFilter;
+	delete _featureDetector;
 }
 
 void CameraThread::setImageRate(float imageRate)
@@ -217,6 +226,30 @@ void CameraThread::disableIMUFiltering()
 	_imuFilter = 0;
 }
 
+void CameraThread::enableFeatureDetection(const ParametersMap & parameters)
+{
+	delete _featureDetector;
+	ParametersMap params = parameters;
+	ParametersMap defaultParams = Parameters::getDefaultParameters("Vis");
+	uInsert(params, ParametersPair(Parameters::kKpDetectorStrategy(), uValue(params, Parameters::kVisFeatureType(), defaultParams.at(Parameters::kVisFeatureType()))));
+	uInsert(params, ParametersPair(Parameters::kKpMaxFeatures(), uValue(params, Parameters::kVisMaxFeatures(), defaultParams.at(Parameters::kVisMaxFeatures()))));
+	uInsert(params, ParametersPair(Parameters::kKpMaxDepth(), uValue(params, Parameters::kVisMaxDepth(), defaultParams.at(Parameters::kVisMaxDepth()))));
+	uInsert(params, ParametersPair(Parameters::kKpMinDepth(), uValue(params, Parameters::kVisMinDepth(), defaultParams.at(Parameters::kVisMinDepth()))));
+	uInsert(params, ParametersPair(Parameters::kKpRoiRatios(), uValue(params, Parameters::kVisRoiRatios(), defaultParams.at(Parameters::kVisRoiRatios()))));
+	uInsert(params, ParametersPair(Parameters::kKpSubPixEps(), uValue(params, Parameters::kVisSubPixEps(), defaultParams.at(Parameters::kVisSubPixEps()))));
+	uInsert(params, ParametersPair(Parameters::kKpSubPixIterations(), uValue(params, Parameters::kVisSubPixIterations(), defaultParams.at(Parameters::kVisSubPixIterations()))));
+	uInsert(params, ParametersPair(Parameters::kKpSubPixWinSize(), uValue(params, Parameters::kVisSubPixWinSize(), defaultParams.at(Parameters::kVisSubPixWinSize()))));
+	uInsert(params, ParametersPair(Parameters::kKpGridRows(), uValue(params, Parameters::kVisGridRows(), defaultParams.at(Parameters::kVisGridRows()))));
+	uInsert(params, ParametersPair(Parameters::kKpGridCols(), uValue(params, Parameters::kVisGridCols(), defaultParams.at(Parameters::kVisGridCols()))));
+	_featureDetector = Feature2D::create(params);
+	_depthAsMask = Parameters::parse(params, Parameters::kVisDepthAsMask(), _depthAsMask);
+}
+void CameraThread::disableFeatureDetection()
+{
+	delete _featureDetector;
+	_featureDetector = 0;
+}
+
 void CameraThread::setScanParameters(
 	bool fromDepth,
 	int downsampleStep,
@@ -732,6 +765,50 @@ void CameraThread::postUpdate(SensorData * dataPtr, CameraInfo * info) const
 						data.stamp());
 		}
 	}
+
+	if(_featureDetector && !data.imageRaw().empty())
+	{
+		UDEBUG("Detecting features");
+		cv::Mat grayScaleImg = data.imageRaw();
+		if(data.imageRaw().channels() > 1)
+		{
+			cv::Mat tmp;
+			cv::cvtColor(grayScaleImg, tmp, cv::COLOR_BGR2GRAY);
+			grayScaleImg = tmp;
+		}
+
+		cv::Mat depthMask;
+		if(!data.depthRaw().empty() && _depthAsMask)
+		{
+			if( data.imageRaw().rows % data.depthRaw().rows == 0 &&
+				data.imageRaw().cols % data.depthRaw().cols == 0 &&
+				data.imageRaw().rows/data.depthRaw().rows == data.imageRaw().cols/data.depthRaw().cols)
+			{
+				depthMask = util2d::interpolate(data.depthRaw(), data.imageRaw().rows/data.depthRaw().rows, 0.1f);
+			}
+			else
+			{
+				UWARN("%s is true, but RGB size (%dx%d) modulo depth size (%dx%d) is not 0. Ignoring depth mask for feature detection.",
+						Parameters::kVisDepthAsMask().c_str(),
+						data.imageRaw().rows, data.imageRaw().cols,
+						data.depthRaw().rows, data.depthRaw().cols);
+			}
+		}
+
+		std::vector<cv::KeyPoint> keypoints = _featureDetector->generateKeypoints(grayScaleImg, depthMask);
+		cv::Mat descriptors;
+		std::vector<cv::Point3f> keypoints3D;
+		if(!keypoints.empty())
+		{
+			descriptors = _featureDetector->generateDescriptors(grayScaleImg, keypoints);
+			if(!keypoints.empty())
+			{
+				keypoints3D = _featureDetector->generateKeypoints3D(data, keypoints);
+			}
+		}
+
+		data.setFeatures(keypoints, keypoints3D, descriptors);
+	}
 }
 
 } // namespace rtabmap

corelib/src/Odometry.cpp

@@ -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/OdometryORBSLAM.h"
+#include "rtabmap/core/odometry/OdometryORBSLAM3.h"
 #include "rtabmap/core/odometry/OdometryLOAM.h"
 #include "rtabmap/core/odometry/OdometryFLOAM.h"
 #include "rtabmap/core/odometry/OdometryMSCKF.h"
@@ -51,6 +51,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/core/util2d.h"
 
 #include <pcl/pcl_base.h>
+#include <rtabmap/core/odometry/OdometryORBSLAM2.h>
 
 namespace rtabmap {
 
@@ -84,7 +85,11 @@ Odometry * Odometry::create(Odometry::Type & type, const ParametersMap & paramet
 		odometry = new OdometryDVO(parameters);
 		break;
 	case Odometry::kTypeORBSLAM:
+#if defined(RTABMAP_ORB_SLAM) and RTABMAP_ORB_SLAM == 2
 		odometry = new OdometryORBSLAM(parameters);
+#else
+		odometry = new OdometryORBSLAM3(parameters);
+#endif
 		break;
 	case Odometry::kTypeOkvis:
 		odometry = new OdometryOkvis(parameters);

corelib/src/RegistrationVis.cpp

@@ -1095,7 +1095,7 @@ Transform RegistrationVis::computeTransformationImpl(
 							std::vector<std::vector<float> > dists;
 							float radius = (float)_guessWinSize; // pixels
 							rtflann::Matrix<float> cornersProjectedMat((float*)cornersProjected.data(), cornersProjected.size(), 2);
-							index.radiusSearch(cornersProjectedMat, indices, dists, radius*radius, rtflann::SearchParams());
+							index.radiusSearch(cornersProjectedMat, indices, dists, radius*radius, rtflann::SearchParams(32, 0, false));
 
 							UASSERT(indices.size() == cornersProjectedMat.rows);
 							UASSERT(descriptorsFrom.cols == descriptorsTo.cols);

corelib/src/camera/CameraRealSense2.cpp

@@ -1501,7 +1501,7 @@ SensorData CameraRealSense2::captureImage(CameraInfo * info)
 						getPoseAndIMU(stamps[i], tmp, confidence, imuTmp);
 						if(!imuTmp.empty())
 						{
-							UEventsManager::post(new IMUEvent(imuTmp, iterA->first/1000.0));
+							UEventsManager::post(new IMUEvent(imuTmp, stamps[i]/1000.0));
 							pub++;
 						}
 						else

corelib/src/odometry/OdometryORBSLAM2.cpp

@@ -34,28 +34,21 @@ 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>
+#include <rtabmap/core/odometry/OdometryORBSLAM2.h>
 
-#ifdef RTABMAP_ORB_SLAM
+#if defined(RTABMAP_ORB_SLAM) and RTABMAP_ORB_SLAM == 2
 #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)
@@ -67,9 +60,6 @@ private:
 protected:
 	void Track()
 	{
-#if RTABMAP_ORB_SLAM == 3
-		Map* mpMap = mpAtlas->GetCurrentMap();
-#endif
 	    if(mState==NO_IMAGES_YET)
 	    {
 	        mState = NOT_INITIALIZED;
@@ -91,17 +81,8 @@ protected:
 
 	        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
 	    {
@@ -384,9 +365,6 @@ 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);
 
@@ -484,11 +462,9 @@ 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();
@@ -516,11 +492,8 @@ 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();
@@ -531,11 +504,7 @@ 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)
 	{}
 
@@ -566,16 +535,12 @@ public:
 
 } // namespace ORB_SLAM
 
-#if RTABMAP_ORB_SLAM == 3
-using namespace ORB_SLAM3;
-#else
 using namespace ORB_SLAM2;
-#endif
 
-class ORBSLAMSystem
+class ORBSLAM2System
 {
 public:
-	ORBSLAMSystem(const rtabmap::ParametersMap & parameters) :
+	ORBSLAM2System(const rtabmap::ParametersMap & parameters) :
 		mpVocabulary(0),
 		mpKeyFrameDatabase(0),
 		mpMap(0),
@@ -613,7 +578,7 @@ public:
 		}
 	}
 
-	bool init(const rtabmap::CameraModel & model, bool stereo, double baseline, const rtabmap::Transform & localIMUTransform)
+	bool init(const rtabmap::CameraModel & model, bool stereo, double baseline)
 	{
 		if(!mpVocabulary)
 		{
@@ -709,31 +674,6 @@ 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
 		//#--------------------------------------------------------------------------------------------
@@ -776,22 +716,15 @@ public:
 		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 Tracker(0, mpVocabulary, 0, 0, mpMap, mpKeyFrameDatabase, configPath, stereo?System::STEREO:System::RGBD, maxFeatureMapSize);
 
 		//Initialize the Local Mapping thread and launch
-#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 LoopCloser(mpMap, mpKeyFrameDatabase, mpVocabulary, true);
 
@@ -812,17 +745,10 @@ public:
 		// Reset all static variables
 		Frame::mbInitialComputations = true;
 
-#if RTABMAP_ORB_SLAM == 3
-		if(ULogger::level() > ULogger::kInfo)
-			Verbose::SetTh(Verbose::VERBOSITY_QUIET);
-
-		mpTracker->Reset(true);
-#endif
-
 		return true;
 	}
 
-	virtual ~ORBSLAMSystem()
+	virtual ~ORBSLAM2System()
 	{
 		shutdown();
 		delete mpVocabulary;
@@ -869,11 +795,7 @@ public:
 	KeyFrameDatabase* mpKeyFrameDatabase;
 
 	// Map structure that stores the pointers to all KeyFrames and MapPoints.
-#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
@@ -898,24 +820,23 @@ public:
 
 namespace rtabmap {
 
-OdometryORBSLAM::OdometryORBSLAM(const ParametersMap & parameters) :
+OdometryORBSLAM2::OdometryORBSLAM2(const ParametersMap & parameters) :
 	Odometry(parameters)
-#ifdef RTABMAP_ORB_SLAM
+#if defined(RTABMAP_ORB_SLAM) and RTABMAP_ORB_SLAM == 2
     ,
 	orbslam_(0),
 	firstFrame_(true),
-	previousPose_(Transform::getIdentity()),
-	useIMU_(false) // TODO: Not yet supported with ORB_SLAM3
+	previousPose_(Transform::getIdentity())
 #endif
 {
-#ifdef RTABMAP_ORB_SLAM
-	orbslam_ = new ORBSLAMSystem(parameters);
+#if defined(RTABMAP_ORB_SLAM) and RTABMAP_ORB_SLAM == 2
+	orbslam_ = new ORBSLAM2System(parameters);
 #endif
 }
 
-OdometryORBSLAM::~OdometryORBSLAM()
+OdometryORBSLAM2::~OdometryORBSLAM2()
 {
-#ifdef RTABMAP_ORB_SLAM
+#if defined(RTABMAP_ORB_SLAM) and RTABMAP_ORB_SLAM == 2
 	if(orbslam_)
 	{
 		delete orbslam_;
@@ -923,10 +844,10 @@ OdometryORBSLAM::~OdometryORBSLAM()
 #endif
 }
 
-void OdometryORBSLAM::reset(const Transform & initialPose)
+void OdometryORBSLAM2::reset(const Transform & initialPose)
 {
 	Odometry::reset(initialPose);
-#ifdef RTABMAP_ORB_SLAM
+#if defined(RTABMAP_ORB_SLAM) and RTABMAP_ORB_SLAM == 2
 	if(orbslam_)
 	{
 		orbslam_->shutdown();
@@ -934,60 +855,20 @@ void OdometryORBSLAM::reset(const Transform & initialPose)
 	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 OdometryORBSLAM::computeTransform(
+Transform OdometryORBSLAM2::computeTransform(
 		SensorData & data,
 		const Transform & guess,
 		OdometryInfo * info)
 {
 	Transform t;
 
-#ifdef RTABMAP_ORB_SLAM
+#if defined(RTABMAP_ORB_SLAM) and RTABMAP_ORB_SLAM == 2
 	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)
@@ -1007,18 +888,12 @@ Transform OdometryORBSLAM::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(orbslam_->mpTracker == 0)
 	{
 		CameraModel model = data.cameraModels().size()==1?data.cameraModels()[0]:data.stereoCameraModels()[0].left();
-		if(!orbslam_->init(model, stereo, data.cameraModels().size()==1?0.0f:data.stereoCameraModels()[0].baseline(), imuLocalTransform_))
+		if(!orbslam_->init(model, stereo, data.cameraModels().size()==1?0.0f:data.stereoCameraModels()[0].baseline()))
 		{
 			return t;
 		}

corelib/src/odometry/OdometryORBSLAM3.cpp

@@ -0,0 +1,571 @@
+/*
+Copyright (c) 2010-2016, 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/OdometryInfo.h"
+#include "rtabmap/core/util2d.h"
+#include "rtabmap/core/util3d_transforms.h"
+#include "rtabmap/utilite/ULogger.h"
+#include "rtabmap/utilite/UTimer.h"
+#include "rtabmap/utilite/UStl.h"
+#include "rtabmap/utilite/UDirectory.h"
+#include <pcl/common/transforms.h>
+#include <opencv2/imgproc/types_c.h>
+#include <rtabmap/core/odometry/OdometryORBSLAM3.h>
+
+#if defined(RTABMAP_ORB_SLAM) and RTABMAP_ORB_SLAM == 3
+#include <thread>
+#include <Converter.h>
+
+using namespace std;
+
+#endif
+
+namespace rtabmap {
+
+OdometryORBSLAM3::OdometryORBSLAM3(const ParametersMap & parameters) :
+	Odometry(parameters)
+#if defined(RTABMAP_ORB_SLAM) and RTABMAP_ORB_SLAM == 3
+    ,
+	orbslam_(0),
+	firstFrame_(true),
+	previousPose_(Transform::getIdentity()),
+	useIMU_(Parameters::defaultOdomORBSLAMInertial()),
+	parameters_(parameters),
+	lastImuStamp_(0.0),
+	lastImageStamp_(0.0)
+#endif
+{
+#if defined(RTABMAP_ORB_SLAM) and RTABMAP_ORB_SLAM == 3
+	Parameters::parse(parameters, Parameters::kOdomORBSLAMInertial(), useIMU_);
+#endif
+}
+
+OdometryORBSLAM3::~OdometryORBSLAM3()
+{
+#if defined(RTABMAP_ORB_SLAM) and RTABMAP_ORB_SLAM == 3
+	if(orbslam_)
+	{
+		orbslam_->Shutdown();
+		delete orbslam_;
+	}
+#endif
+}
+
+void OdometryORBSLAM3::reset(const Transform & initialPose)
+{
+	Odometry::reset(initialPose);
+#if defined(RTABMAP_ORB_SLAM) and RTABMAP_ORB_SLAM == 3
+	if(orbslam_)
+	{
+		orbslam_->Shutdown();
+		delete orbslam_;
+		orbslam_=0;
+	}
+	firstFrame_ = true;
+	originLocalTransform_.setNull();
+	previousPose_.setIdentity();
+	imuLocalTransform_.setNull();
+	lastImuStamp_ = 0.0;
+	lastImageStamp_ = 0.0;
+#endif
+}
+
+bool OdometryORBSLAM3::canProcessAsyncIMU() const
+{
+#if defined(RTABMAP_ORB_SLAM) and RTABMAP_ORB_SLAM == 3
+	return useIMU_;
+#else
+	return false;
+#endif
+}
+
+bool OdometryORBSLAM3::init(const rtabmap::CameraModel & model, double stamp,  bool stereo, double baseline)
+{
+#if defined(RTABMAP_ORB_SLAM) and RTABMAP_ORB_SLAM == 3
+	std::string vocabularyPath;
+	rtabmap::Parameters::parse(parameters_, rtabmap::Parameters::kOdomORBSLAMVocPath(), vocabularyPath);
+
+	if(vocabularyPath.empty())
+	{
+		UERROR("ORB_SLAM vocabulary path should be set! (Parameter name=\"%s\")", rtabmap::Parameters::kOdomORBSLAMVocPath().c_str());
+		return false;
+	}
+	//Load ORB Vocabulary
+	vocabularyPath = uReplaceChar(vocabularyPath, '~', UDirectory::homeDir());
+	UWARN("Loading ORB Vocabulary: \"%s\". This could take a while...", vocabularyPath.c_str());
+
+	// Create configuration file
+	std::string workingDir;
+	rtabmap::Parameters::parse(parameters_, rtabmap::Parameters::kRtabmapWorkingDirectory(), workingDir);
+	if(workingDir.empty())
+	{
+		workingDir = ".";
+	}
+	std::string configPath = workingDir+"/rtabmap_orbslam.yaml";
+	std::ofstream ofs (configPath, std::ofstream::out);
+	ofs << "%YAML:1.0" << std::endl;
+	ofs << std::endl;
+
+	ofs << "File.version: \"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 << "Camera1.fx: " << model.fx() << std::endl;
+	ofs << "Camera1.fy: " << model.fy() << std::endl;
+	ofs << "Camera1.cx: " << model.cx() << std::endl;
+	ofs << "Camera1.cy: " << model.cy() << std::endl;
+	ofs << std::endl;
+
+	if(model.D().cols < 4)
+	{
+		ofs << "Camera1.k1: " << 0.0 << std::endl;
+		ofs << "Camera1.k2: " << 0.0 << std::endl;
+		ofs << "Camera1.p1: " << 0.0 << std::endl;
+		ofs << "Camera1.p2: " << 0.0 << std::endl;
+		if(!stereo)
+		{
+			ofs << "Camera1.k3: " << 0.0 << std::endl;
+		}
+	}
+	if(model.D().cols >= 4)
+	{
+		ofs << "Camera1.k1: " << model.D().at<double>(0,0) << std::endl;
+		ofs << "Camera1.k2: " << model.D().at<double>(0,1) << std::endl;
+		ofs << "Camera1.p1: " << model.D().at<double>(0,2) << std::endl;
+		ofs << "Camera1.p2: " << model.D().at<double>(0,3) << std::endl;
+	}
+	if(model.D().cols >= 5)
+	{
+		ofs << "Camera1.k3: " << model.D().at<double>(0,4) << std::endl;
+	}
+	if(model.D().cols > 5)
+	{
+		UWARN("Unhandled camera distortion size %d, only 5 first coefficients used", model.D().cols);
+	}
+	ofs << std::endl;
+
+	//# IR projector baseline times fx (aprox.)
+	if(baseline <= 0.0)
+	{
+		baseline = rtabmap::Parameters::defaultOdomORBSLAMBf();
+		rtabmap::Parameters::parse(parameters_, rtabmap::Parameters::kOdomORBSLAMBf(), baseline);
+	}
+	ofs << "Camera.bf: " << model.fx()*baseline << std::endl;
+
+	ofs << "Camera.width: " << model.imageWidth() << std::endl;
+	ofs << "Camera.height: " << model.imageHeight() << std::endl;
+	ofs << std::endl;
+
+	//# Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale)
+	//Camera.RGB: 1
+	ofs << "Camera.RGB: 1" << std::endl;
+	ofs << std::endl;
+
+	float fps = rtabmap::Parameters::defaultOdomORBSLAMFps();
+	rtabmap::Parameters::parse(parameters_, rtabmap::Parameters::kOdomORBSLAMFps(), fps);
+	if(fps == 0)
+	{
+		UASSERT(stamp > lastImageStamp_);
+		fps = std::round(1./(stamp - lastImageStamp_));
+		UWARN("Camera FPS estimated at %d Hz. If this doesn't look good, "
+			  "set explicitly parameter %s to expected frequency.",
+			  int(fps), Parameters::kOdomORBSLAMFps().c_str());
+	}
+	ofs << "Camera.fps: " << (int)fps << std::endl;
+	ofs << std::endl;
+
+	//# Close/Far threshold. Baseline times.
+	double thDepth = rtabmap::Parameters::defaultOdomORBSLAMThDepth();
+	rtabmap::Parameters::parse(parameters_, rtabmap::Parameters::kOdomORBSLAMThDepth(), thDepth);
+	ofs << "Stereo.ThDepth: " << thDepth << std::endl;
+	ofs << "Stereo.b: " << baseline << std::endl;
+	ofs << std::endl;
+
+	//# Deptmap values factor
+	ofs << "RGBD.DepthMapFactor: " << 1.0 << std::endl;
+	ofs << std::endl;
+
+	bool withIMU = false;
+	if(!imuLocalTransform_.isNull())
+	{
+		withIMU = true;
+		//#--------------------------------------------------------------------------------------------
+		//# IMU Parameters TODO: hard-coded, not used
+		//#--------------------------------------------------------------------------------------------
+		//  Transformation from camera 0 to body-frame (imu)
+		rtabmap::Transform camImuT = model.localTransform()*imuLocalTransform_;
+		ofs << "IMU.T_b_c1: !!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.InsertKFsWhenLost: " << 0 << std::endl;
+		ofs << std::endl;
+
+		double gyroNoise = rtabmap::Parameters::defaultOdomORBSLAMGyroNoise();
+		double accNoise = rtabmap::Parameters::defaultOdomORBSLAMAccNoise();
+		double gyroWalk = rtabmap::Parameters::defaultOdomORBSLAMGyroWalk();
+		double accWalk = rtabmap::Parameters::defaultOdomORBSLAMAccWalk();
+		double samplingRate = rtabmap::Parameters::defaultOdomORBSLAMSamplingRate();
+		rtabmap::Parameters::parse(parameters_, rtabmap::Parameters::kOdomORBSLAMGyroNoise(), gyroNoise);
+		rtabmap::Parameters::parse(parameters_, rtabmap::Parameters::kOdomORBSLAMAccNoise(), accNoise);
+		rtabmap::Parameters::parse(parameters_, rtabmap::Parameters::kOdomORBSLAMGyroWalk(), gyroWalk);
+		rtabmap::Parameters::parse(parameters_, rtabmap::Parameters::kOdomORBSLAMAccWalk(), accWalk);
+		rtabmap::Parameters::parse(parameters_, rtabmap::Parameters::kOdomORBSLAMSamplingRate(), samplingRate);
+
+		ofs << "IMU.NoiseGyro: " << gyroNoise << std::endl; // 1e-2
+		ofs << "IMU.NoiseAcc: " << accNoise << std::endl; // 1e-1
+		ofs << "IMU.GyroWalk: " << gyroWalk << std::endl; // 1e-6
+		ofs << "IMU.AccWalk: " << accWalk << std::endl; // 1e-4
+		if(samplingRate == 0)
+		{
+			// estimate rate from imu received.
+			UASSERT(orbslamImus_.size() > 1 && orbslamImus_[0].t < orbslamImus_[1].t);
+			samplingRate = 1./(orbslamImus_[1].t - orbslamImus_[0].t);
+			samplingRate = std::round(samplingRate);
+			UWARN("IMU sampling rate estimated at %.0f Hz. If this doesn't look good, "
+				  "set explicitly parameter %s to expected frequency.",
+				  samplingRate,	Parameters::kOdomORBSLAMSamplingRate().c_str());
+		}
+		ofs << "IMU.Frequency: " << samplingRate << std::endl; // 200
+		ofs << std::endl;
+	}
+
+
+
+	//#--------------------------------------------------------------------------------------------
+	//# ORB Parameters
+	//#--------------------------------------------------------------------------------------------
+	//# ORB Extractor: Number of features per image
+	int features = rtabmap::Parameters::defaultOdomORBSLAMMaxFeatures();
+	rtabmap::Parameters::parse(parameters_, rtabmap::Parameters::kOdomORBSLAMMaxFeatures(), features);
+	ofs << "ORBextractor.nFeatures: " << features << std::endl;
+	ofs << std::endl;
+
+	//# ORB Extractor: Scale factor between levels in the scale pyramid
+	double scaleFactor = rtabmap::Parameters::defaultORBScaleFactor();
+	rtabmap::Parameters::parse(parameters_, rtabmap::Parameters::kORBScaleFactor(), scaleFactor);
+	ofs << "ORBextractor.scaleFactor: " << scaleFactor << std::endl;
+	ofs << std::endl;
+
+	//# ORB Extractor: Number of levels in the scale pyramid
+	int levels = rtabmap::Parameters::defaultORBNLevels();
+	rtabmap::Parameters::parse(parameters_, rtabmap::Parameters::kORBNLevels(), levels);
+	ofs << "ORBextractor.nLevels: " << levels << std::endl;
+	ofs << std::endl;
+
+	//# ORB Extractor: Fast threshold
+	//# Image is divided in a grid. At each cell FAST are extracted imposing a minimum response.
+	//# Firstly we impose iniThFAST. If no corners are detected we impose a lower value minThFAST
+	//# You can lower these values if your images have low contrast
+	int iniThFAST = rtabmap::Parameters::defaultFASTThreshold();
+	int minThFAST = rtabmap::Parameters::defaultFASTMinThreshold();
+	rtabmap::Parameters::parse(parameters_, rtabmap::Parameters::kFASTThreshold(), iniThFAST);
+	rtabmap::Parameters::parse(parameters_, rtabmap::Parameters::kFASTMinThreshold(), minThFAST);
+	ofs << "ORBextractor.iniThFAST: " << iniThFAST << std::endl;
+	ofs << "ORBextractor.minThFAST: " << minThFAST << std::endl;
+	ofs << std::endl;
+
+	int maxFeatureMapSize = rtabmap::Parameters::defaultOdomORBSLAMMapSize();
+	rtabmap::Parameters::parse(parameters_, rtabmap::Parameters::kOdomORBSLAMMapSize(), maxFeatureMapSize);
+
+	//# Disable loop closure detection
+	ofs << "loopClosing: " << 0 << std::endl;
+	ofs << std::endl;
+
+	//# Set dummy Viewer parameters
+	ofs << "Viewer.KeyFrameSize: " << 0.05 << std::endl;
+	ofs << "Viewer.KeyFrameLineWidth: " << 1.0 << std::endl;
+	ofs << "Viewer.GraphLineWidth: " << 0.9 << std::endl;
+	ofs << "Viewer.PointSize: " << 2.0 << std::endl;
+	ofs << "Viewer.CameraSize: " << 0.08 << std::endl;
+	ofs << "Viewer.CameraLineWidth: " << 3.0 << std::endl;
+	ofs << "Viewer.ViewpointX: " << 0.0 << std::endl;
+	ofs << "Viewer.ViewpointY: " << -0.7 << std::endl;
+	ofs << "Viewer.ViewpointZ: " << -3.5 << std::endl;
+	ofs << "Viewer.ViewpointF: " << 500.0 << std::endl;
+	ofs << std::endl;
+
+	ofs.close();
+
+	orbslam_ = new ORB_SLAM3::System(
+			vocabularyPath,
+			configPath,
+			stereo && withIMU?ORB_SLAM3::System::IMU_STEREO:
+			stereo?ORB_SLAM3::System::STEREO:
+			withIMU?ORB_SLAM3::System::IMU_RGBD:
+					ORB_SLAM3::System::RGBD,
+			false);
+	return true;
+#else
+	UERROR("RTAB-Map is not built with ORB_SLAM support! Select another visual odometry approach.");
+#endif
+	return false;
+}
+
+// return not null transform if odometry is correctly computed
+Transform OdometryORBSLAM3::computeTransform(
+		SensorData & data,
+		const Transform & guess,
+		OdometryInfo * info)
+{
+	Transform t;
+
+#if defined(RTABMAP_ORB_SLAM) and RTABMAP_ORB_SLAM == 3
+	UTimer timer;
+
+	if(useIMU_)
+	{
+		bool added = false;
+		if(!data.imu().empty())
+		{
+			if(lastImuStamp_ == 0.0 || lastImuStamp_ < data.stamp())
+			{
+				orbslamImus_.push_back(ORB_SLAM3::IMU::Point(
+						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()));
+				lastImuStamp_ = data.stamp();
+				added = true;
+			}
+			else
+			{
+				UERROR("Received IMU with stamp (%f) <= than the previous IMU (%f), ignoring it!", data.stamp(), lastImuStamp_);
+			}
+		}
+
+		if(orbslam_ == 0)
+		{
+			// We need two samples to estimate imu frame rate
+			if(orbslamImus_.size()>1 && added)
+			{
+				imuLocalTransform_ = data.imu().localTransform();
+			}
+		}
+
+		if(data.imageRaw().empty() || imuLocalTransform_.isNull())
+		{
+			return Transform();
+		}
+	}
+
+	if(data.imageRaw().empty() ||
+		data.imageRaw().rows != data.depthOrRightRaw().rows ||
+		data.imageRaw().cols != data.depthOrRightRaw().cols)
+	{
+		UERROR("Not supported input! RGB (%dx%d) and depth (%dx%d) should have the same size.",
+				data.imageRaw().cols, data.imageRaw().rows, data.depthOrRightRaw().cols, data.depthOrRightRaw().rows);
+		return t;
+	}
+
+	if(!((data.cameraModels().size() == 1 &&
+			data.cameraModels()[0].isValidForReprojection()) ||
+		(data.stereoCameraModels().size() == 1 &&
+			data.stereoCameraModels()[0].isValidForProjection())))
+	{
+		UERROR("Invalid camera model!");
+		return t;
+	}
+
+	bool stereo = data.cameraModels().size() == 0;
+
+	cv::Mat covariance;
+	if(orbslam_ == 0)
+	{
+		// We need two frames to estimate camera frame rate
+		if(lastImageStamp_ == 0.0)
+		{
+			lastImageStamp_ = data.stamp();
+			return t;
+		}
+
+		CameraModel model = data.cameraModels().size()==1?data.cameraModels()[0]:data.stereoCameraModels()[0].left();
+		if(!init(model, data.stamp(), stereo, data.cameraModels().size()==1?0.0:data.stereoCameraModels()[0].baseline()))
+		{
+			return t;
+		}
+	}
+
+	Sophus::SE3f Tcw;
+	Transform localTransform;
+	if(stereo)
+	{
+		localTransform = data.stereoCameraModels()[0].localTransform();
+
+		Tcw = orbslam_->TrackStereo(data.imageRaw(), data.rightRaw(), data.stamp(), orbslamImus_);
+		orbslamImus_.clear();
+	}
+	else
+	{
+		localTransform = data.cameraModels()[0].localTransform();
+		cv::Mat depth;
+		if(data.depthRaw().type() == CV_32FC1)
+		{
+			depth = data.depthRaw();
+		}
+		else if(data.depthRaw().type() == CV_16UC1)
+		{
+			depth = util2d::cvtDepthToFloat(data.depthRaw());
+		}
+		Tcw = orbslam_->TrackRGBD(data.imageRaw(), depth, data.stamp(), orbslamImus_);
+		orbslamImus_.clear();
+	}
+
+	Transform previousPoseInv = previousPose_.inverse();
+	std::vector<ORB_SLAM3::MapPoint*> mapPoints = orbslam_->GetTrackedMapPoints();
+	if(orbslam_->isLost() || mapPoints.empty())
+	{
+		covariance = cv::Mat::eye(6,6,CV_64FC1)*9999.0f;
+	}
+	else
+	{
+		cv::Mat TcwMat = ORB_SLAM3::Converter::toCvMat(ORB_SLAM3::Converter::toSE3Quat(Tcw)).clone();
+		UASSERT(TcwMat.cols == 4 && TcwMat.rows == 4);
+		Transform p = Transform(cv::Mat(TcwMat, cv::Range(0,3), cv::Range(0,4)));
+
+		if(!p.isNull())
+		{
+			if(!localTransform.isNull())
+			{
+				if(originLocalTransform_.isNull())
+				{
+					originLocalTransform_ = localTransform;
+				}
+				// transform in base frame
+				p = originLocalTransform_ * p.inverse() * localTransform.inverse();
+			}
+			t = previousPoseInv*p;
+		}
+		previousPose_ = p;
+
+		if(firstFrame_)
+		{
+			// just recovered of being lost, set high covariance
+			covariance = cv::Mat::eye(6,6,CV_64FC1)*9999.0f;
+			firstFrame_ = false;
+		}
+		else
+		{
+			float baseline = data.cameraModels().size()==1?0.0f:data.stereoCameraModels()[0].baseline();
+			if(baseline <= 0.0f)
+			{
+				baseline = rtabmap::Parameters::defaultOdomORBSLAMBf();
+				rtabmap::Parameters::parse(parameters_, rtabmap::Parameters::kOdomORBSLAMBf(), baseline);
+			}
+			double linearVar = 0.0001;
+			if(baseline > 0.0f)
+			{
+				linearVar = baseline/8.0;
+				linearVar *= linearVar;
+			}
+
+			covariance = cv::Mat::eye(6,6, CV_64FC1);
+			covariance.at<double>(0,0) = linearVar;
+			covariance.at<double>(1,1) = linearVar;
+			covariance.at<double>(2,2) = linearVar;
+			covariance.at<double>(3,3) = 0.0001;
+			covariance.at<double>(4,4) = 0.0001;
+			covariance.at<double>(5,5) = 0.0001;
+		}
+	}
+
+	if(info)
+	{
+		info->lost = t.isNull();
+		info->type = (int)kTypeORBSLAM;
+		info->reg.covariance = covariance;
+		info->localMapSize = mapPoints.size();
+		info->localKeyFrames = 0;
+
+		if(this->isInfoDataFilled())
+		{
+			std::vector<cv::KeyPoint> kpts = orbslam_->GetTrackedKeyPointsUn();
+			info->reg.matchesIDs.resize(kpts.size());
+			info->reg.inliersIDs.resize(kpts.size());
+			int oi = 0;
+
+			UASSERT(mapPoints.size() == kpts.size());
+			for (unsigned int i = 0; i < kpts.size(); ++i)
+			{
+				int wordId;
+				if(mapPoints[i] != 0)
+				{
+					wordId = mapPoints[i]->mnId;
+				}
+				else
+				{
+					wordId = -(i+1);
+				}
+				info->words.insert(std::make_pair(wordId, kpts[i]));
+				if(mapPoints[i] != 0)
+				{
+					info->reg.matchesIDs[oi] = wordId;
+					info->reg.inliersIDs[oi] = wordId;
+					++oi;
+				}
+			}
+			info->reg.matchesIDs.resize(oi);
+			info->reg.inliersIDs.resize(oi);
+			info->reg.inliers = oi;
+			info->reg.matches = oi;
+
+			Eigen::Affine3f fixRot = (this->getPose()*previousPoseInv*originLocalTransform_).toEigen3f();
+			for (unsigned int i = 0; i < mapPoints.size(); ++i)
+			{
+				if(mapPoints[i])
+				{
+					Eigen::Vector3f pt = mapPoints[i]->GetWorldPos();
+					pcl::PointXYZ ptt = pcl::transformPoint(pcl::PointXYZ(pt[0], pt[1], pt[2]), fixRot);
+					info->localMap.insert(std::make_pair(mapPoints[i]->mnId, cv::Point3f(ptt.x, ptt.y, ptt.z)));
+				}
+			}
+		}
+	}
+
+	UINFO("Odom update time = %fs, map points=%ld, lost=%s", timer.elapsed(), mapPoints.size(), t.isNull()?"true":"false");
+
+#else
+	UERROR("RTAB-Map is not built with ORB_SLAM support! Select another visual odometry approach.");
+#endif
+	return t;
+}
+
+} // namespace rtabmap

docker/jfr2018/Dockerfile

@@ -127,8 +127,7 @@ WORKDIR /root/catkin_ws/src
 RUN /bin/bash -c '. /opt/ros/${ROS_DISTRO}/setup.bash; catkin_init_workspace'
 RUN git clone https://github.com/tum-vision/dvo_slam.git && cd dvo_slam && git checkout kinetic-devel && rm dvo_slam/package.xml && rm dvo_benchmark/package.xml && rm dvo_ros/package.xml
 RUN git clone https://github.com/srv/viso2.git && cd viso2 && git checkout kinetic
-# msckf: using a patched version to be used outside ros and c++14:
-RUN git clone https://github.com/borongyuan/msckf_vio.git && cd msckf_vio && git checkout e69cdb68e9b31a438bbc51bb77cd34fe9b2e80cb
+RUN git clone https://github.com/KumarRobotics/msckf_vio.git && cd msckf_vio && git checkout a9386c5 && wget https://gist.githubusercontent.com/matlabbe/f2518d7427e7f6740af2110e540b1f2b/raw/1b7c96b38e063bacfa77408686e4528f3d27af2e/msckf_vio_a9386c5_ros_commented.patch && git apply --ignore-space-change --ignore-whitespace msckf_vio_a9386c5_ros_commented.patch
 RUN git clone https://github.com/srv/libfovis.git && cd libfovis && git checkout db2fc39451e59317cf8486d92085da1c8e414785
 RUN git clone https://github.com/ros-perception/vision_opencv.git && cd vision_opencv && git checkout kinetic
 RUN git clone https://github.com/laboshinl/loam_velodyne.git && cd loam_velodyne && git checkout a4c364a677647f2a35831439032dc5a58378b3fd

docker/jfr2018/latest/Dockerfile

@@ -138,7 +138,7 @@ RUN git clone https://github.com/tum-vision/dvo_slam.git && cd dvo_slam && git c
 # VISO2
 RUN git clone https://github.com/srv/viso2.git && cd viso2 && git checkout ${ROS_DISTRO}
 # MSCKF-VIO: using a patched version to be used outside ros and c++14:
-RUN git clone https://github.com/borongyuan/msckf_vio.git && cd msckf_vio && git checkout e69cdb68e9b31a438bbc51bb77cd34fe9b2e80cb
+RUN git clone https://github.com/borongyuan/msckf_vio.git && cd msckf_vio && git checkout 916c917e5b481741c60732057f0141e8311962c1
 # FOVIS
 RUN git clone https://github.com/srv/libfovis.git && cd libfovis && git checkout 896acc8425e9fd7c5609153b8bad349ae1abbb50
 # LOAM
@@ -227,21 +227,55 @@ ENV ORB_SLAM2_ROOT_DIR /root/ORB_SLAM2
 
 WORKDIR /root
 
+# To make "source" working
+RUN rm /bin/sh && ln -s /bin/bash /bin/sh
+
+# Need cmake >=3.10
+RUN apt update && apt install -y wget && apt-get clean && rm -rf /var/lib/apt/lists/ && \
+    wget -nv https://github.com/Kitware/CMake/releases/download/v3.17.0/cmake-3.17.0-Linux-x86_64.tar.gz && \
+    tar -xzf cmake-3.17.0-Linux-x86_64.tar.gz && \
+    rm cmake-3.17.0-Linux-x86_64.tar.gz &&\
+    ln -s ~/cmake-3.17.0-Linux-x86_64/bin/cmake ~/cmake
+
 # Copy current source code
 COPY . /root/rtabmap
 
 # Patch for OpenCV 3.1
-RUN cd rtabmap && wget https://gist.githubusercontent.com/matlabbe/ab42b8ea5d5902ffc6177539b98d9d51/raw/87520db7a54bf2673be3fc898578cd6348dd66f0/rtabmap_opencv310_backward_compatibility.patch && git apply --ignore-space-change --ignore-whitespace rtabmap_opencv310_backward_compatibility.patch
+RUN cd rtabmap && git apply --ignore-space-change --ignore-whitespace docker/jfr2018/rtabmap_opencv310_backward_compatibility.patch
 
 # Build RTAB-Map (using standard g2o, then a version for orbslam2, which uses its own g2o version)
 RUN cp -r rtabmap rtabmap_os2 && cp -r rtabmap rtabmap_msckf && cp -r rtabmap rtabmap_loam
-RUN /bin/bash -c '. /root/catkin_ws/devel/setup.bash; cd rtabmap ;  cd build ;  cmake -DWITH_FOVIS=ON -DWITH_VISO2=ON -DWITH_DVO=ON -DWITH_OKVIS=ON -DWITH_VINS=ON -DWITH_OPENVINS=ON .. ;  make -j3 ; make install ; rm -rf * ; ldconfig'
+
+RUN source /root/catkin_ws/devel/setup.bash && \
+    cd rtabmap && \
+    cd build && \
+    ~/cmake -DWITH_FOVIS=ON -DWITH_VISO2=ON -DWITH_DVO=ON -DWITH_OKVIS=ON -DWITH_VINS=ON -DWITH_OPENVINS=ON -DWITH_OPENGV=OFF .. && \
+    make -j3 && \
+    make install && \
+    rm -rf * && \
+    ldconfig
 # Version with orb_slam2 (system g2o disabled)
-RUN /bin/bash -c '. /root/catkin_ws/devel/setup.bash; cd rtabmap_os2 ; cd build ; cmake -DWITH_G2O=OFF -DWITH_ORB_SLAM=ON .. ; make -j3 ; rm -rf *'
+RUN source /root/catkin_ws/devel/setup.bash && \
+    cd rtabmap_os2 && \
+    cd build && \
+    ~/cmake -DWITH_G2O=OFF -DWITH_ORB_SLAM=ON -DWITH_OPENGV=OFF .. && \
+    make -j3 && \
+    rm -rf *
 
 # rtabmap is crashing if LOAM and MSCKF dependencies are used at the same time, so split them
-RUN /bin/bash -c '. /root/catkin_ws/devel/setup.bash; cd rtabmap_loam ;cd build ;cmake  -DWITH_LOAM=ON -DWITH_FLOAM=ON .. ;make -j3 ;rm -rf *'
-RUN /bin/bash -c '. /root/catkin_ws/devel/setup.bash; cd rtabmap_msckf ;cd build ;cmake -DWITH_MSCKF_VIO=ON .. ;make -j3 ;rm -rf *'
+RUN source /root/catkin_ws/devel/setup.bash && \
+    cd rtabmap_loam && \
+    cd build && \
+    ~/cmake  -DWITH_LOAM=ON -DWITH_FLOAM=ON -DWITH_OPENGV=OFF .. && \
+    make -j3 && \
+    rm -rf *
+RUN source /root/catkin_ws/devel/setup.bash && \
+    cd rtabmap_msckf && \
+    git apply --ignore-space-change --ignore-whitespace docker/jfr2018/rtabmap_msckf_ubuntu_16_compatibility.patch && \
+    cd build && \
+    ~/cmake -DWITH_MSCKF_VIO=ON -DWITH_OPENGV=OFF .. && \
+    make -j3 && \
+    rm -rf *
 
 WORKDIR /root
     

docker/jfr2018/rtabmap_msckf_ubuntu_16_compatibility.patch

@@ -0,0 +1,13 @@
+diff --git a/CMakeLists.txt b/CMakeLists.txt
+index c14cb659..2c370200 100644
+--- a/CMakeLists.txt
++++ b/CMakeLists.txt
+@@ -830,6 +830,8 @@ IF(NOT MSVC)
+     ENDIF()
+ ENDIF()
+ 
++set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fext-numeric-literals")
++
+ 
+ ####### OSX BUNDLE CMAKE_INSTALL_PREFIX #######
+ IF(APPLE AND BUILD_AS_BUNDLE)

docker/jfr2018/rtabmap_opencv310_backward_compatibility.patch

@@ -0,0 +1,13 @@
+diff --git a/CMakeLists.txt b/CMakeLists.txt
+index 9f2bfd83..65b03eef 100644
+--- a/CMakeLists.txt
++++ b/CMakeLists.txt
+@@ -228,7 +228,7 @@ ENDIF()
+ set(RTABMAP_QT_VERSION AUTO CACHE STRING "Force a specific Qt version.")
+ set_property(CACHE RTABMAP_QT_VERSION PROPERTY STRINGS AUTO 4 5 6)
+ 
+-FIND_PACKAGE(OpenCV REQUIRED QUIET COMPONENTS core calib3d imgproc highgui stitching photo video videoio OPTIONAL_COMPONENTS aruco xfeatures2d nonfree gpu cudafeatures2d)
++FIND_PACKAGE(OpenCV QUIET COMPONENTS core calib3d imgproc highgui stitching photo video videoio OPTIONAL_COMPONENTS aruco xfeatures2d nonfree gpu cudafeatures2d)
+ 
+ IF(WITH_QT)
+ FIND_PACKAGE(PCL 1.7 REQUIRED QUIET COMPONENTS common io kdtree search surface filters registration sample_consensus segmentation visualization)

guilib/include/rtabmap/gui/PreferencesDialog.h

@@ -265,6 +265,7 @@ public:
 	double getBilateralSigmaR() const;
 	int getSourceImageDecimation() const;
 	int getSourceHistogramMethod() const;
+	bool isSourceFeatureDetection() const;
 	bool isSourceStereoDepthGenerated() const;
 	bool isSourceStereoExposureCompensation() const;
 	bool isSourceScanFromDepth() const;

guilib/src/CloudViewer.cpp

@@ -2835,8 +2835,6 @@ void CloudViewer::setCameraPosition(
 	{
 		renderer->ResetCameraClippingRange(boundingBox);
 	}
-
-	_visualizer->getRenderWindow()->Render ();
 }
 
 void CloudViewer::updateCameraTargetPosition(const Transform & pose)
@@ -2846,14 +2844,6 @@ void CloudViewer::updateCameraTargetPosition(const Transform & pose)
 		Eigen::Affine3f m = pose.toEigen3f();
 		Eigen::Vector3f pos = m.translation();
 
-		Eigen::Vector3f lastPos(0,0,0);
-		if(_trajectory->size())
-		{
-			lastPos[0]=_trajectory->back().x;
-			lastPos[1]=_trajectory->back().y;
-			lastPos[2]=_trajectory->back().z;
-		}
-
 		_trajectory->push_back(pcl::PointXYZ(pos[0], pos[1], pos[2]));
 		if(_maxTrajectorySize>0)
 		{

guilib/src/MainWindow.cpp

@@ -5774,6 +5774,10 @@ void MainWindow::startDetection()
 	_camera->setColorOnly(_preferencesDialog->isSourceRGBDColorOnly());
 	_camera->setImageDecimation(_preferencesDialog->getSourceImageDecimation());
 	_camera->setHistogramMethod(_preferencesDialog->getSourceHistogramMethod());
+	if(_preferencesDialog->isSourceFeatureDetection())
+	{
+		_camera->enableFeatureDetection(parameters);
+	}
 	_camera->setStereoToDepth(_preferencesDialog->isSourceStereoDepthGenerated());
 	_camera->setStereoExposureCompensation(_preferencesDialog->isSourceStereoExposureCompensation());
 	_camera->setScanParameters(

guilib/src/PreferencesDialog.cpp

@@ -817,6 +817,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	connect(_ui->checkbox_rgbd_colorOnly, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->spinBox_source_imageDecimation, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->comboBox_source_histogramMethod, SIGNAL(currentIndexChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
+	connect(_ui->checkbox_source_feature_detection, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->checkbox_stereo_depthGenerated, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->checkBox_stereo_exposureCompensation, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->pushButton_calibrate, SIGNAL(clicked()), this, SLOT(calibrate()));
@@ -1388,6 +1389,12 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	_ui->doubleSpinBox_OdomORBSLAMFps->setObjectName(Parameters::kOdomORBSLAMFps().c_str());
 	_ui->spinBox_OdomORBSLAMMaxFeatures->setObjectName(Parameters::kOdomORBSLAMMaxFeatures().c_str());
 	_ui->spinBox_OdomORBSLAMMapSize->setObjectName(Parameters::kOdomORBSLAMMapSize().c_str());
+	_ui->groupBox_OdomORBSLAMInertial->setObjectName(Parameters::kOdomORBSLAMInertial().c_str());
+	_ui->doubleSpinBox_ORBSLAMGyroNoise->setObjectName(Parameters::kOdomORBSLAMGyroNoise().c_str());
+	_ui->doubleSpinBox_ORBSLAMAccNoise->setObjectName(Parameters::kOdomORBSLAMAccNoise().c_str());
+	_ui->doubleSpinBox_ORBSLAMGyroWalk->setObjectName(Parameters::kOdomORBSLAMGyroWalk().c_str());
+	_ui->doubleSpinBox_ORBSLAMAccWalk->setObjectName(Parameters::kOdomORBSLAMAccWalk().c_str());
+	_ui->doubleSpinBox_ORBSLAMSamplingRate->setObjectName(Parameters::kOdomORBSLAMSamplingRate().c_str());
 
 	// Odometry Okvis
 	_ui->lineEdit_OdomOkvisPath->setObjectName(Parameters::kOdomOKVISConfigPath().c_str());
@@ -2052,6 +2059,7 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox)
 		_ui->checkbox_rgbd_colorOnly->setChecked(false);
 		_ui->spinBox_source_imageDecimation->setValue(1);
 		_ui->comboBox_source_histogramMethod->setCurrentIndex(0);
+		_ui->checkbox_source_feature_detection->setChecked(false);
 		_ui->checkbox_stereo_depthGenerated->setChecked(false);
 		_ui->checkBox_stereo_exposureCompensation->setChecked(false);
 		_ui->openni2_autoWhiteBalance->setChecked(true);
@@ -2487,6 +2495,7 @@ void PreferencesDialog::readCameraSettings(const QString & filePath)
 	_ui->lineEdit_sourceLocalTransform->setText(settings.value("localTransform",_ui->lineEdit_sourceLocalTransform->text()).toString());
 	_ui->spinBox_source_imageDecimation->setValue(settings.value("imageDecimation",_ui->spinBox_source_imageDecimation->value()).toInt());
 	_ui->comboBox_source_histogramMethod->setCurrentIndex(settings.value("histogramMethod", _ui->comboBox_source_histogramMethod->currentIndex()).toInt());
+	_ui->checkbox_source_feature_detection->setChecked(settings.value("featureDetection", _ui->checkbox_source_feature_detection->isChecked()).toBool());
 	// Backward compatibility
 	if(_ui->lineEdit_sourceLocalTransform->text().compare("0 0 1 -1 0 0 0 -1 0") == 0)
 	{
@@ -3026,6 +3035,7 @@ void PreferencesDialog::writeCameraSettings(const QString & filePath) const
 	settings.setValue("localTransform",  _ui->lineEdit_sourceLocalTransform->text());
 	settings.setValue("imageDecimation",  _ui->spinBox_source_imageDecimation->value());
 	settings.setValue("histogramMethod",  _ui->comboBox_source_histogramMethod->currentIndex());
+	settings.setValue("featureDetection",  _ui->checkbox_source_feature_detection->isChecked());
 
 	settings.beginGroup("rgbd");
 	settings.setValue("driver", 	       _ui->comboBox_cameraRGBD->currentIndex());
@@ -6028,6 +6038,10 @@ int PreferencesDialog::getSourceHistogramMethod() const
 {
 	return _ui->comboBox_source_histogramMethod->currentIndex();
 }
+bool PreferencesDialog::isSourceFeatureDetection() const
+{
+	return _ui->checkbox_source_feature_detection->isChecked();
+}
 bool PreferencesDialog::isSourceStereoDepthGenerated() const
 {
 	return _ui->checkbox_stereo_depthGenerated->isChecked();
@@ -6833,6 +6847,10 @@ void PreferencesDialog::testOdometry()
 	cameraThread.setColorOnly(_ui->checkbox_rgbd_colorOnly->isChecked());
 	cameraThread.setImageDecimation(_ui->spinBox_source_imageDecimation->value());
 	cameraThread.setHistogramMethod(_ui->comboBox_source_histogramMethod->currentIndex());
+	if(_ui->checkbox_source_feature_detection->isChecked())
+	{
+		cameraThread.enableFeatureDetection(this->getAllParameters());
+	}
 	cameraThread.setStereoToDepth(_ui->checkbox_stereo_depthGenerated->isChecked());
 	cameraThread.setStereoExposureCompensation(_ui->checkBox_stereo_exposureCompensation->isChecked());
 	cameraThread.setScanParameters(
@@ -6905,6 +6923,10 @@ void PreferencesDialog::testCamera()
 		cameraThread.setColorOnly(_ui->checkbox_rgbd_colorOnly->isChecked());
 		cameraThread.setImageDecimation(_ui->spinBox_source_imageDecimation->value());
 		cameraThread.setHistogramMethod(_ui->comboBox_source_histogramMethod->currentIndex());
+		if(_ui->checkbox_source_feature_detection->isChecked())
+		{
+			cameraThread.enableFeatureDetection(this->getAllParameters());
+		}
 		cameraThread.setStereoToDepth(_ui->checkbox_stereo_depthGenerated->isChecked());
 		cameraThread.setStereoExposureCompensation(_ui->checkBox_stereo_exposureCompensation->isChecked());
 		cameraThread.setScanParameters(