From aaff1abc4f257cf948de5f427d190a89556510ac Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sun, 19 Nov 2023 01:05:52 -0800 Subject: [PATCH] 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 | 8 +- cmake_modules/FindORB_SLAM.cmake | 4 + corelib/include/rtabmap/core/CameraThread.h | 5 + corelib/include/rtabmap/core/Parameters.h | 19 +- .../{OdometryORBSLAM.h => OdometryORBSLAM2.h} | 28 +- .../rtabmap/core/odometry/OdometryORBSLAM3.h | 71 ++ corelib/src/CMakeLists.txt | 3 +- corelib/src/CameraThread.cpp | 83 +- corelib/src/Odometry.cpp | 7 +- corelib/src/RegistrationVis.cpp | 2 +- corelib/src/camera/CameraRealSense2.cpp | 2 +- ...ometryORBSLAM.cpp => OdometryORBSLAM2.cpp} | 171 +--- corelib/src/odometry/OdometryORBSLAM3.cpp | 571 +++++++++++++ docker/jfr2018/Dockerfile | 3 +- docker/jfr2018/latest/Dockerfile | 46 +- ...tabmap_msckf_ubuntu_16_compatibility.patch | 13 + ...map_opencv310_backward_compatibility.patch | 13 + .../include/rtabmap/gui/PreferencesDialog.h | 1 + guilib/src/CloudViewer.cpp | 10 - guilib/src/MainWindow.cpp | 4 + guilib/src/PreferencesDialog.cpp | 22 + guilib/src/ui/preferencesDialog.ui | 763 +++++++++++------- 22 files changed, 1347 insertions(+), 502 deletions(-) rename corelib/include/rtabmap/core/odometry/{OdometryORBSLAM.h => OdometryORBSLAM2.h} (79%) create mode 100644 corelib/include/rtabmap/core/odometry/OdometryORBSLAM3.h rename corelib/src/odometry/{OdometryORBSLAM.cpp => OdometryORBSLAM2.cpp} (87%) create mode 100644 corelib/src/odometry/OdometryORBSLAM3.cpp create mode 100644 docker/jfr2018/rtabmap_msckf_ubuntu_16_compatibility.patch create mode 100644 docker/jfr2018/rtabmap_opencv310_backward_compatibility.patch diff --git a/CMakeLists.txt b/CMakeLists.txt index 9f2bfd8329..c14cb659b7 100644 --- a/CMakeLists.txt +++ b/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)") diff --git a/cmake_modules/FindORB_SLAM.cmake b/cmake_modules/FindORB_SLAM.cmake index 1e89348b73..f9ccdc0d57 100644 --- a/cmake_modules/FindORB_SLAM.cmake +++ b/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) diff --git a/corelib/include/rtabmap/core/CameraThread.h b/corelib/include/rtabmap/core/CameraThread.h index 514fea9f58..b4535fa40f 100644 --- a/corelib/include/rtabmap/core/CameraThread.h +++ b/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 diff --git a/corelib/include/rtabmap/core/Parameters.h b/corelib/include/rtabmap/core/Parameters.h index b93a2ba0b1..046ff7ee60 100644 --- a/corelib/include/rtabmap/core/Parameters.h +++ b/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."); diff --git a/corelib/include/rtabmap/core/odometry/OdometryORBSLAM.h b/corelib/include/rtabmap/core/odometry/OdometryORBSLAM2.h similarity index 79% rename from corelib/include/rtabmap/core/odometry/OdometryORBSLAM.h rename to corelib/include/rtabmap/core/odometry/OdometryORBSLAM2.h index a0af8bad03..0a0adeff4c 100644 --- a/corelib/include/rtabmap/core/odometry/OdometryORBSLAM.h +++ b/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 -#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_ */ diff --git a/corelib/include/rtabmap/core/odometry/OdometryORBSLAM3.h b/corelib/include/rtabmap/core/odometry/OdometryORBSLAM3.h new file mode 100644 index 0000000000..b4651a84c9 --- /dev/null +++ b/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 + +#if defined(RTABMAP_ORB_SLAM) and RTABMAP_ORB_SLAM == 3 +#include +#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 orbslamImus_; + double lastImuStamp_; + double lastImageStamp_; +#endif + +}; + +} + +#endif /* ODOMETRYORBSLAM_H3_ */ diff --git a/corelib/src/CMakeLists.txt b/corelib/src/CMakeLists.txt index b3f1fa434f..15e7af2269 100644 --- a/corelib/src/CMakeLists.txt +++ b/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 diff --git a/corelib/src/CameraThread.cpp b/corelib/src/CameraThread.cpp index b2a981d292..a8c099a26e 100644 --- a/corelib/src/CameraThread.cpp +++ b/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 #include #include +#include #include @@ -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 keypoints = _featureDetector->generateKeypoints(grayScaleImg, depthMask); + cv::Mat descriptors; + std::vector keypoints3D; + if(!keypoints.empty()) + { + descriptors = _featureDetector->generateDescriptors(grayScaleImg, keypoints); + if(!keypoints.empty()) + { + keypoints3D = _featureDetector->generateKeypoints3D(data, keypoints); + } + } + + data.setFeatures(keypoints, keypoints3D, descriptors); + } } } // namespace rtabmap diff --git a/corelib/src/Odometry.cpp b/corelib/src/Odometry.cpp index f6c7762724..8147a927d1 100644 --- a/corelib/src/Odometry.cpp +++ b/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 +#include 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); diff --git a/corelib/src/RegistrationVis.cpp b/corelib/src/RegistrationVis.cpp index a0fce6a635..0bdeb0cd34 100644 --- a/corelib/src/RegistrationVis.cpp +++ b/corelib/src/RegistrationVis.cpp @@ -1095,7 +1095,7 @@ Transform RegistrationVis::computeTransformationImpl( std::vector > dists; float radius = (float)_guessWinSize; // pixels rtflann::Matrix 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); diff --git a/corelib/src/camera/CameraRealSense2.cpp b/corelib/src/camera/CameraRealSense2.cpp index 7694e05433..c16e530c9f 100644 --- a/corelib/src/camera/CameraRealSense2.cpp +++ b/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 diff --git a/corelib/src/odometry/OdometryORBSLAM.cpp b/corelib/src/odometry/OdometryORBSLAM2.cpp similarity index 87% rename from corelib/src/odometry/OdometryORBSLAM.cpp rename to corelib/src/odometry/OdometryORBSLAM2.cpp index 3212ba62b9..1ae10bc197 100644 --- a/corelib/src/odometry/OdometryORBSLAM.cpp +++ b/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 #include -#include +#include -#ifdef RTABMAP_ORB_SLAM +#if defined(RTABMAP_ORB_SLAM) and RTABMAP_ORB_SLAM == 2 #include #include 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 @@ class Tracker: public Tracking 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 @@ class Tracker: public Tracking 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 @@ class Tracker: public Tracking // 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 @@ class Tracker: public Tracking 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 @@ class Tracker: public Tracking 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 @@ class Tracker: public Tracking 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 @@ class LoopCloser: public LoopClosing } // 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 @@ class ORBSLAMSystem } } - 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 @@ class ORBSLAMSystem 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 @@ class ORBSLAMSystem 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 @@ class ORBSLAMSystem // 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 @@ class ORBSLAMSystem 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 @@ class ORBSLAMSystem 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; } diff --git a/corelib/src/odometry/OdometryORBSLAM3.cpp b/corelib/src/odometry/OdometryORBSLAM3.cpp new file mode 100644 index 0000000000..4c6b57abc9 --- /dev/null +++ b/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 +#include +#include + +#if defined(RTABMAP_ORB_SLAM) and RTABMAP_ORB_SLAM == 3 +#include +#include + +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(0,0) << std::endl; + ofs << "Camera1.k2: " << model.D().at(0,1) << std::endl; + ofs << "Camera1.p1: " << model.D().at(0,2) << std::endl; + ofs << "Camera1.p2: " << model.D().at(0,3) << std::endl; + } + if(model.D().cols >= 5) + { + ofs << "Camera1.k3: " << model.D().at(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 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(0,0) = linearVar; + covariance.at(1,1) = linearVar; + covariance.at(2,2) = linearVar; + covariance.at(3,3) = 0.0001; + covariance.at(4,4) = 0.0001; + covariance.at(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 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 diff --git a/docker/jfr2018/Dockerfile b/docker/jfr2018/Dockerfile index 064334f566..0eab9008ee 100644 --- a/docker/jfr2018/Dockerfile +++ b/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 diff --git a/docker/jfr2018/latest/Dockerfile b/docker/jfr2018/latest/Dockerfile index f98a2a89d5..9dde6f351f 100644 --- a/docker/jfr2018/latest/Dockerfile +++ b/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 diff --git a/docker/jfr2018/rtabmap_msckf_ubuntu_16_compatibility.patch b/docker/jfr2018/rtabmap_msckf_ubuntu_16_compatibility.patch new file mode 100644 index 0000000000..a396cde070 --- /dev/null +++ b/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) diff --git a/docker/jfr2018/rtabmap_opencv310_backward_compatibility.patch b/docker/jfr2018/rtabmap_opencv310_backward_compatibility.patch new file mode 100644 index 0000000000..949906ea10 --- /dev/null +++ b/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) diff --git a/guilib/include/rtabmap/gui/PreferencesDialog.h b/guilib/include/rtabmap/gui/PreferencesDialog.h index 7f8f6daa3b..37301a0536 100644 --- a/guilib/include/rtabmap/gui/PreferencesDialog.h +++ b/guilib/include/rtabmap/gui/PreferencesDialog.h @@ -265,6 +265,7 @@ class RTABMAP_GUI_EXPORT PreferencesDialog : public QDialog double getBilateralSigmaR() const; int getSourceImageDecimation() const; int getSourceHistogramMethod() const; + bool isSourceFeatureDetection() const; bool isSourceStereoDepthGenerated() const; bool isSourceStereoExposureCompensation() const; bool isSourceScanFromDepth() const; diff --git a/guilib/src/CloudViewer.cpp b/guilib/src/CloudViewer.cpp index 4f43f0be4b..8ad45a35bb 100644 --- a/guilib/src/CloudViewer.cpp +++ b/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) { diff --git a/guilib/src/MainWindow.cpp b/guilib/src/MainWindow.cpp index a63039de49..8a5908497e 100644 --- a/guilib/src/MainWindow.cpp +++ b/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( diff --git a/guilib/src/PreferencesDialog.cpp b/guilib/src/PreferencesDialog.cpp index d65c0adac9..3476d5be02 100644 --- a/guilib/src/PreferencesDialog.cpp +++ b/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( diff --git a/guilib/src/ui/preferencesDialog.ui b/guilib/src/ui/preferencesDialog.ui index ef9040d00d..643be37779 100644 --- a/guilib/src/ui/preferencesDialog.ui +++ b/guilib/src/ui/preferencesDialog.ui @@ -6,7 +6,7 @@ 0 0 - 1052 + 1009 925 @@ -23,8 +23,8 @@ :/images/RTAB-Map.ico:/images/RTAB-Map.ico - - + + 0 @@ -63,9 +63,9 @@ 0 - -844 - 756 - 4371 + 0 + 713 + 3893 @@ -95,7 +95,7 @@ QFrame::Raised - 12 + 5 @@ -2906,29 +2906,6 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki - - - - Input rate (0 means as fast as possible). - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - Local transform from /base_link to /camera_link. Mouse over the box to show formats. If odometry sensor is enabled, it is the transform to odometry sensor. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - @@ -2942,33 +2919,17 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki - - - - ID of the device, which might be a serial number, bus@address or the index of the device. If empty, the first device found is taken. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + <html><head/><body><p>Format (3 values): x y z<br/>Format (6 values): x y z roll pitch yaw<br/>Format (7 values): x y z qx qy qz qw<br/>Format (9 values, 3x3 rotation): r11 r12 r13 r21 r22 r23 r31 r32 r33<br/>Format (12 values, 3x4 transform): r11 r12 r13 tx r21 r22 r23 ty r31 r32 r33 tz</p><p>KITTI: /base_link to /gray_camera = 0 0 0 0 0 0<br/>KITTI: /base_link to /color_camera = 0 -0.06 0 0 0 0<br/>KITTI: /base_footprint to /gray_camera = 0 0 1.67 0 0 0<br/>KITTI: /base_footprint to /color_camera = 0 -0.06 1.67 0 0 0</p><p>EuRoC MAV: /base_link to /cam0 = T_BS*T_SC0 = 0.999661 0.0257743 -0.00375625 0.00981073 -0.0257154 0.999557 0.0149672 0.064677 0.00414035 -0.0148655 0.999881 -0.0216401</p></body></html> - - - - - Create a calibration file with known intrinsics (fx, fy, cx, cy). Useful if you already know the intrinsics of the source of images. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + 0 0 0 0 0 0 - + @@ -2992,6 +2953,16 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki + + + + + + + false + + + @@ -3011,36 +2982,6 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki - - - - Mirroring mode (flip image horizontally). It has no effect on database source. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - - - - - - - - <html><head/><body><p>Format (3 values): x y z<br/>Format (6 values): x y z roll pitch yaw<br/>Format (7 values): x y z qx qy qz qw<br/>Format (9 values, 3x3 rotation): r11 r12 r13 r21 r22 r23 r31 r32 r33<br/>Format (12 values, 3x4 transform): r11 r12 r13 tx r21 r22 r23 ty r31 r32 r33 tz</p><p>KITTI: /base_link to /gray_camera = 0 0 0 0 0 0<br/>KITTI: /base_link to /color_camera = 0 -0.06 0 0 0 0<br/>KITTI: /base_footprint to /gray_camera = 0 0 1.67 0 0 0<br/>KITTI: /base_footprint to /color_camera = 0 -0.06 1.67 0 0 0</p><p>EuRoC MAV: /base_link to /cam0 = T_BS*T_SC0 = 0.999661 0.0257743 -0.00375625 0.00981073 -0.0257154 0.999557 0.0149672 0.064677 0.00414035 -0.0148655 0.999881 -0.0216401</p></body></html> - - - 0 0 0 0 0 0 - - - @@ -3069,7 +3010,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki - + 0 @@ -3077,11 +3018,24 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki - Test + Create Calibration - + + + + Equalizes the histogram of grayscale images. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + @@ -3094,10 +3048,10 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki - - + + - Calibration files are saved in "camera_info" folder of the working directory. + Create a calibration file with known intrinsics (fx, fy, cx, cy). Useful if you already know the intrinsics of the source of images. true @@ -3107,15 +3061,50 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki - - + + + + 0 + + + QComboBox::AdjustToContents + + + + None + + + + + Histogram + + + + + CLAHE + + + + + + + + Input rate (0 means as fast as possible). + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + - - + + 0 @@ -3123,14 +3112,14 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki - Create Calibration + Test - - + + - Calibration file path (*.yaml). If empty, the GUID of the camera is used (for those having one). OpenNI and Freenect drivers use factory calibration by default (so they ignore this parameter). A calibrated camera is required for RGB-D SLAM mode. + Calibration files are saved in "camera_info" folder of the working directory. true @@ -3140,10 +3129,10 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki - - + + - Image decimation. RGB/Mono and depth images will be resized according to this value (size*1/decimation). Note that if depth images are captured, decimation should be a multiple of the depth image size. If depth images are smaller than RGB images, the decimation is first applied on RGB, if the resulting RGB image is still bigger than depth image, the depth is not decimated. + Only RGB images (for RGB-D cameras) or left images (for stereo cameras) are published. true @@ -3160,10 +3149,10 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki - - + + - Equalizes the histogram of grayscale images. + ID of the device, which might be a serial number, bus@address or the index of the device. If empty, the first device found is taken. true @@ -3173,35 +3162,30 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki - - - - 0 + + + + - - QComboBox::AdjustToContents + + + + + + Mirroring mode (flip image horizontally). It has no effect on database source. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - None - - - - - Histogram - - - - - CLAHE - - - - + + - Only RGB images (for RGB-D cameras) or left images (for stereo cameras) are published. + Image decimation. RGB/Mono and depth images will be resized according to this value (size*1/decimation). Note that if depth images are captured, decimation should be a multiple of the depth image size. If depth images are smaller than RGB images, the decimation is first applied on RGB, if the resulting RGB image is still bigger than depth image, the depth is not decimated. true @@ -3211,8 +3195,47 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki - - + + + + Local transform from /base_link to /camera_link. Mouse over the box to show formats. If odometry sensor is enabled, it is the transform to odometry sensor. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + Calibration file path (*.yaml). If empty, the GUID of the camera is used (for those having one). OpenNI and Freenect drivers use factory calibration by default (so they ignore this parameter). A calibrated camera is required for RGB-D SLAM mode. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + Detect features inside camera thread using parameters set in Visual Registration panel. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + @@ -14310,7 +14333,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag Odometry - + @@ -14751,7 +14774,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - 12 + 5 @@ -15067,22 +15090,22 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - - - Qt::Vertical - - - - 20 - 0 - - - - + + + + Qt::Vertical + + + + 20 + 0 + + + + @@ -15151,7 +15174,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag [Visual] Correspondences computation: Optical Flow - + @@ -15168,10 +15191,10 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - + + - Window size. + Iterations. true @@ -15181,26 +15204,23 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - - - 0 - - - 999999 + + + + Max level. - - 1 + + true - - 3 + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - + + - Iterations. + Window size. true @@ -15223,67 +15243,57 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - + + + + 3 + - 1 + 0.001000000000000 - 999999 + 1.000000000000000 - 1 + 0.010000000000000 - 30 + 0.010000000000000 - - - - Max level. + + + + 0 - - true + + 999999 - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + 1 - - - - - + 3 + + + + - 0.001000000000000 + 1 - 1.000000000000000 + 999999 - 0.010000000000000 + 1 - 0.010000000000000 + 30 - - - - Qt::Vertical - - - - 20 - 0 - - - - @@ -15295,7 +15305,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag 20 - 40 + 0 @@ -16654,32 +16664,35 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - + + + + false + - m + - 4 + 2 0.000000000000000 - 9.999900000000000 + 999.000000000000000 - 0.010000000000000 + 1.000000000000000 - 0.076000000000000 + 0.000000000000000 - - + + - Close/Far threshold. Baseline times. + Fake IR projector baseline used only when stereo is not used. true @@ -16689,32 +16702,23 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - - - - - - 1 - - - 0.100000000000000 - - - 999.000000000000000 + + + + Maximum size of the feature map (0 means infinite). Only supported with ORB_SLAM2. - - 1.000000000000000 + + true - - 40.000000000000000 + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - + + - Fake IR projector baseline used only when stereo is not used. + Maximum ORB features extracted per frame. true @@ -16724,13 +16728,17 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - + + + + ... + + - - + + - Path to ORB vocabulary (*.txt). + Close/Far threshold. Baseline times. true @@ -16740,13 +16748,19 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - - - ... + + + + 99999 + + + 1000 + + + @@ -16760,64 +16774,60 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - - - false + + + + Path to ORB vocabulary (*.txt). + + + true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + - + m - 2 + 4 0.000000000000000 - 999.000000000000000 + 9.999900000000000 - 1.000000000000000 + 0.010000000000000 - 0.000000000000000 + 0.076000000000000 - - - - Maximum ORB features extracted per frame. + + + + - - true + + 1 - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + 0.100000000000000 - - - - - 99999 - - - 1000 - - - - - - - Maximum size of the feature map (0 means infinite). + 999.000000000000000 - - true + + 1.000000000000000 - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + 40.000000000000000 @@ -16833,11 +16843,172 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag + + + + Enable IMU. Only supported with ORB_SLAM3. + + + true + + + + + + + + 8 + + + 0.010000000000000 + + + 0.010000000000000 + + + + + + + Accelerometer "random walk". + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + Accelerometer "white noise". + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + Gyroscope "white noise". + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + 8 + + + 0.100000000000000 + + + 0.100000000000000 + + + + + + + Gyroscope "random walk". + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + IMU sampling rate (0 to estimate from input data). + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + 8 + + + 0.000000000000000 + + + 1.000000000000000 + + + 0.000001000000000 + + + 0.000001000000000 + + + + + + + 8 + + + 0.000000000000000 + + + 1.000000000000000 + + + 0.000100000000000 + + + 0.000100000000000 + + + + + + + 0 + + + 0.000000000000000 + + + 10000.000000000000000 + + + 0.000000000000000 + + + + + + + + - + Qt::Vertical @@ -18201,7 +18372,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag State - + @@ -19527,7 +19698,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag 20 - 1049 + 0 @@ -19677,19 +19848,6 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - - - Qt::Vertical - - - - 20 - 0 - - - - @@ -19829,6 +19987,19 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag + + + + Qt::Vertical + + + + 20 + 0 + + + + @@ -19839,7 +20010,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - 0 + 2 @@ -20155,16 +20326,6 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - - - - - - true - - - @@ -25734,7 +25895,7 @@ Lower the ratio -> higher the precision. - +