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 @@ class RTABMAP_CORE_EXPORT CameraThread :
 	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 @@ class RTABMAP_CORE_EXPORT CameraThread :
 	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/OdometryORBSLAM.h → 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