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