i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-02 01:20:25 +08:00
Code Issues Packages Projects Releases Wiki Activity

Integrated ORB_SLAM2 as an odometry approach Odom/Strategy=5.

Browse Source
This commit is contained in:
matlabbe
2017-06-05 19:04:23 -04:00
parent de797b644e
commit c4ca6a0bac
17 changed files with 2036 additions and 348 deletions
Download Patch File Download Diff File Expand all files Collapse all files

47
CMakeLists.txt
View File

@@ -155,6 +155,7 @@ option(WITH_CPUTSDF "Include CPUTSDF support" ON)
option(WITH_FOVIS "Include FOVIS support" ON) option(WITH_FOVIS "Include FOVIS support" ON)
option(WITH_VISO2 "Include VISO2 support" ON) option(WITH_VISO2 "Include VISO2 support" ON)
option(WITH_DVO "Include DVO support" ON) option(WITH_DVO "Include DVO support" ON)
option(WITH_ORB_SLAM2 "Include ORB_SLAM2 support" ON)
option(PCL_OMP "With PCL OMP implementations" ON) option(PCL_OMP "With PCL OMP implementations" ON)
FIND_PACKAGE(OpenCV REQUIRED QUIET) FIND_PACKAGE(OpenCV REQUIRED QUIET)
@@ -368,7 +369,26 @@ IF(WITH_DVO)
ENDIF(dvo_core_FOUND) ENDIF(dvo_core_FOUND)
ENDIF(WITH_DVO) ENDIF(WITH_DVO)
IF(G2O_FOUND OR GTSAM_FOUND OR ZED_FOUND OR ANDROID OR RealSense_FOUND) IF(WITH_ORB_SLAM2 AND NOT G2O_FOUND AND NOT RealSense_FOUND)
FIND_PACKAGE(ORB_SLAM2 QUIET)
IF(ORB_SLAM2_FOUND)
MESSAGE(STATUS "Found ORB_SLAM2: ${ORB_SLAM2_INCLUDE_DIRS}")
FIND_PACKAGE(Pangolin QUIET)
IF(NOT Pangolin_FOUND)
SET(ORB_SLAM2_FOUND FALSE)
MESSAGE(STATUS "Found ORB_SLAM2 but not Pangolin, disabling ORB_SLAM2.")
ELSE()
MESSAGE(STATUS "Found Pangolin: ${Pangolin_INCLUDE_DIRS}")
SET(ORB_SLAM2_INCLUDE_DIRS ${ORB_SLAM2_INCLUDE_DIRS} ${Pangolin_INCLUDE_DIRS})
SET(ORB_SLAM2_LIBRARIES ${ORB_SLAM2_LIBRARIES} ${Pangolin_LIBRARIES})
set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -march=native")
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -march=native")
# There could be crash issues if built with RealSense
ENDIF()
ENDIF(ORB_SLAM2_FOUND)
ENDIF(WITH_ORB_SLAM2 AND NOT G2O_FOUND AND NOT RealSense_FOUND)
IF(G2O_FOUND OR GTSAM_FOUND OR ZED_FOUND OR ANDROID OR RealSense_FOUND OR ORB_SLAM2_FOUND)
#Newest versions require std11 #Newest versions require std11
IF(NOT MSVC) IF(NOT MSVC)
include(CheckCXXCompilerFlag) include(CheckCXXCompilerFlag)
@@ -382,7 +402,7 @@ IF(G2O_FOUND OR GTSAM_FOUND OR ZED_FOUND OR ANDROID OR RealSense_FOUND)
message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has no C++11 support. Please use a different C++ compiler if you want to use g2o or gtsam (set \"-DWITH_G2O=OFF -DWITH_GTSAM=OFF\" to build without g2o and gtsam).") message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has no C++11 support. Please use a different C++ compiler if you want to use g2o or gtsam (set \"-DWITH_G2O=OFF -DWITH_GTSAM=OFF\" to build without g2o and gtsam).")
ENDIF() ENDIF()
ENDIF() ENDIF()
ENDIF(G2O_FOUND OR GTSAM_FOUND OR ZED_FOUND OR ANDROID OR RealSense_FOUND) ENDIF(G2O_FOUND OR GTSAM_FOUND OR ZED_FOUND OR ANDROID OR RealSense_FOUND OR ORB_SLAM2_FOUND)
####### OSX BUNDLE CMAKE_INSTALL_PREFIX ####### ####### OSX BUNDLE CMAKE_INSTALL_PREFIX #######
IF(APPLE AND BUILD_AS_BUNDLE) IF(APPLE AND BUILD_AS_BUNDLE)
@@ -505,6 +525,11 @@ IF(NOT dvo_core_FOUND)
ELSE() ELSE()
SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${dvo_core_LIBRARIES}) SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${dvo_core_LIBRARIES})
ENDIF() ENDIF()
IF(NOT ORB_SLAM2_FOUND)
SET(ORB_SLAM2 "//")
ELSE()
SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${ORB_SLAM2_LIBRARIES})
ENDIF()
IF(ADD_VTK_GUI_SUPPORT_QT_TO_CONF) IF(ADD_VTK_GUI_SUPPORT_QT_TO_CONF)
SET(CONF_VTK_QT true) SET(CONF_VTK_QT true)
SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} vtkGUISupportQt) SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} vtkGUISupportQt)
@@ -771,11 +796,15 @@ ELSE()
MESSAGE(STATUS " With GTSAM = NO (GTSAM not found)") MESSAGE(STATUS " With GTSAM = NO (GTSAM not found)")
ENDIF() ENDIF()
IF(G2O_FOUND OR GTSAM_FOUND)
IF(WITH_VERTIGO) IF(WITH_VERTIGO)
MESSAGE(STATUS " With VERTIGO = YES (License: GPLv3)") MESSAGE(STATUS " With VERTIGO = YES (License: GPLv3)")
ELSE() ELSE()
MESSAGE(STATUS " With VERTIGO = NO (WITH_VERTIGO=OFF)") MESSAGE(STATUS " With VERTIGO = NO (WITH_VERTIGO=OFF)")
ENDIF() ENDIF()
ELSE()
MESSAGE(STATUS " With VERTIGO = NO (GTSAM or g2o required)")
ENDIF()
IF(cvsba_FOUND) IF(cvsba_FOUND)
MESSAGE(STATUS " With cvsba = YES (License: GPLv2)") MESSAGE(STATUS " With cvsba = YES (License: GPLv2)")
@@ -852,6 +881,20 @@ ELSE()
MESSAGE(STATUS " With dvo_core = NO (dvo_core not found)") MESSAGE(STATUS " With dvo_core = NO (dvo_core not found)")
ENDIF() ENDIF()
IF(ORB_SLAM2_FOUND)
MESSAGE(STATUS " With ORB_SLAM2 = YES (License: GPLv3)")
ELSEIF(NOT WITH_ORB_SLAM2)
MESSAGE(STATUS " With ORB_SLAM2 = NO (WITH_ORB_SLAM2=OFF)")
ELSEIF(G2O_FOUND AND RealSense_FOUND)
MESSAGE(STATUS " With ORB_SLAM2 = NO (WITH_G2O should be OFF as ORB_SLAM2 uses its own g2o version, WITH_REALSENSE should also be off to avoid eigen errors)")
ELSEIF(G2O_FOUND)
MESSAGE(STATUS " With ORB_SLAM2 = NO (WITH_G2O should be OFF as ORB_SLAM2 uses its own g2o version)")
ELSEIF(RealSense_FOUND)
MESSAGE(STATUS " With ORB_SLAM2 = NO (WITH_REALSENSE should be off to avoid eigen errors)")
ELSE()
MESSAGE(STATUS " With ORB_SLAM2 = NO (ORB_SLAM2 not found, make sure environment variable ORB_SLAM2_ROOT_DIR is set)")
ENDIF()
IF(QT4_FOUND) IF(QT4_FOUND)
MESSAGE(STATUS " With Qt4 = YES (License: Open Source or Commercial)") MESSAGE(STATUS " With Qt4 = YES (License: Open Source or Commercial)")
ELSEIF(Qt5_FOUND) ELSEIF(Qt5_FOUND)

1
Version.h.in
View File

@@ -57,6 +57,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
@FOVIS@#define RTABMAP_FOVIS @FOVIS@#define RTABMAP_FOVIS
@VISO2@#define RTABMAP_VISO2 @VISO2@#define RTABMAP_VISO2
@DVO@#define RTABMAP_DVO @DVO@#define RTABMAP_DVO
@ORB_SLAM2@#define RTABMAP_ORB_SLAM2
#endif /* VERSION_H_ */ #endif /* VERSION_H_ */

30
cmake_modules/FindORB_SLAM2.cmake Normal file
View File

@@ -0,0 +1,30 @@
# - Find ORB_SLAM2
#
# It sets the following variables:
# ORB_SLAM2_FOUND - Set to false, or undefined, if ORB_SLAM2 isn't found.
# ORB_SLAM2_INCLUDE_DIRS - The ORB_SLAM2 include directory.
# ORB_SLAM2_LIBRARIES - The ORB_SLAM2 library to link against.
#
# Set ORB_SLAM2_ROOT_DIR environment variable as the path to ORB_SLAM2 root folder.
find_path(ORB_SLAM2_INCLUDE_DIR NAMES System.h PATHS $ENV{ORB_SLAM2_ROOT_DIR}/include)
find_library(ORB_SLAM2_LIBRARY NAMES ORB_SLAM2 PATHS $ENV{ORB_SLAM2_ROOT_DIR}/lib)
IF (ORB_SLAM2_INCLUDE_DIR AND ORB_SLAM2_LIBRARY)
SET(ORB_SLAM2_FOUND TRUE)
SET(ORB_SLAM2_INCLUDE_DIRS ${ORB_SLAM2_INCLUDE_DIR} $ENV{ORB_SLAM2_ROOT_DIR})
SET(ORB_SLAM2_LIBRARIES ${ORB_SLAM2_LIBRARY})
ENDIF (ORB_SLAM2_INCLUDE_DIR AND ORB_SLAM2_LIBRARY)
IF (ORB_SLAM2_FOUND)
# show which ORB_SLAM2 was found only if not quiet
IF (NOT ORB_SLAM2_FIND_QUIETLY)
MESSAGE(STATUS "Found ORB_SLAM2: ${ORB_SLAM2_LIBRARIES}")
ENDIF (NOT ORB_SLAM2_FIND_QUIETLY)
ELSE (ORB_SLAM2_FOUND)
# fatal error if ORB_SLAM2 is required but not found
IF (ORB_SLAM2_FIND_REQUIRED)
MESSAGE(FATAL_ERROR "Could not find ORB_SLAM2")
ENDIF (ORB_SLAM2_FIND_REQUIRED)
ENDIF (ORB_SLAM2_FOUND)

3
corelib/include/rtabmap/core/Odometry.h
View File

@@ -48,7 +48,8 @@ public:
kTypeF2F = 1, kTypeF2F = 1,
kTypeFovis = 2, kTypeFovis = 2,
kTypeViso2 = 3, kTypeViso2 = 3,
kTypeDVO = 4 kTypeDVO = 4,
kTypeORBSLAM2 = 5
}; };
public: public:

62
corelib/include/rtabmap/core/OdometryORBSLAM2.h Normal file
View File

@@ -0,0 +1,62 @@
/*
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 ODOMETRYORBSLAM2_H_
#define ODOMETRYORBSLAM2_H_
#include <rtabmap/core/Odometry.h>
namespace ORB_SLAM2 {
class System;
}
class ORBSLAM2System;
namespace rtabmap {
class RTABMAP_EXP OdometryORBSLAM2 : public Odometry
{
public:
OdometryORBSLAM2(const rtabmap::ParametersMap & parameters = rtabmap::ParametersMap());
virtual ~OdometryORBSLAM2();
virtual void reset(const Transform & initialPose = Transform::getIdentity());
virtual Odometry::Type getType() {return Odometry::kTypeORBSLAM2;}
private:
virtual Transform computeTransform(SensorData & image, const Transform & guess = Transform(), OdometryInfo * info = 0);
private:
ORBSLAM2System * orbslam2_;
ORB_SLAM2::System * system_;
bool firstFrame_;
};
}
#endif /* ODOMETRYORBSLAM2_H_ */

13
corelib/include/rtabmap/core/Parameters.h
View File

@@ -262,11 +262,11 @@ class RTABMAP_EXP Parameters
RTABMAP_PARAM(BRIEF, Bytes, int, 32, "Bytes is a length of descriptor in bytes. It can be equal 16, 32 or 64 bytes."); RTABMAP_PARAM(BRIEF, Bytes, int, 32, "Bytes is a length of descriptor in bytes. It can be equal 16, 32 or 64 bytes.");
RTABMAP_PARAM(FAST, Threshold, int, 10, "Threshold on difference between intensity of the central pixel and pixels of a circle around this pixel."); RTABMAP_PARAM(FAST, Threshold, int, 20, "Threshold on difference between intensity of the central pixel and pixels of a circle around this pixel.");
RTABMAP_PARAM(FAST, NonmaxSuppression, bool, true, "If true, non-maximum suppression is applied to detected corners (keypoints)."); RTABMAP_PARAM(FAST, NonmaxSuppression, bool, true, "If true, non-maximum suppression is applied to detected corners (keypoints).");
RTABMAP_PARAM(FAST, Gpu, bool, false, "GPU-FAST: Use GPU version of FAST. This option is enabled only if OpenCV is built with CUDA and GPUs are detected."); RTABMAP_PARAM(FAST, Gpu, bool, false, "GPU-FAST: Use GPU version of FAST. This option is enabled only if OpenCV is built with CUDA and GPUs are detected.");
RTABMAP_PARAM(FAST, GpuKeypointsRatio, double, 0.05, "Used with FAST GPU."); RTABMAP_PARAM(FAST, GpuKeypointsRatio, double, 0.05, "Used with FAST GPU.");
RTABMAP_PARAM(FAST, MinThreshold, int, 1, "Minimum threshold. Used only when FAST/GridRows and FAST/GridCols are set."); RTABMAP_PARAM(FAST, MinThreshold, int, 7, "Minimum threshold. Used only when FAST/GridRows and FAST/GridCols are set.");
RTABMAP_PARAM(FAST, MaxThreshold, int, 200, "Maximum threshold. Used only when FAST/GridRows and FAST/GridCols are set."); RTABMAP_PARAM(FAST, MaxThreshold, int, 200, "Maximum threshold. Used only when FAST/GridRows and FAST/GridCols are set.");
RTABMAP_PARAM(FAST, GridRows, int, 4, "Grid rows (0 to disable). Adapts the detector to partition the source image into a grid and detect points in each cell."); RTABMAP_PARAM(FAST, GridRows, int, 4, "Grid rows (0 to disable). Adapts the detector to partition the source image into a grid and detect points in each cell.");
RTABMAP_PARAM(FAST, GridCols, int, 4, "Grid cols (0 to disable). Adapts the detector to partition the source image into a grid and detect points in each cell."); RTABMAP_PARAM(FAST, GridCols, int, 4, "Grid cols (0 to disable). Adapts the detector to partition the source image into a grid and detect points in each cell.");
@@ -363,7 +363,7 @@ class RTABMAP_EXP Parameters
RTABMAP_PARAM(GTSAM, Optimizer, int, 1, "0=Levenberg 1=GaussNewton 2=Dogleg"); RTABMAP_PARAM(GTSAM, Optimizer, int, 1, "0=Levenberg 1=GaussNewton 2=Dogleg");
// Odometry // Odometry
RTABMAP_PARAM(Odom, Strategy, int, 0, "0=Frame-to-Map (F2M) 1=Frame-to-Frame (F2F) 2=Fovis 3=viso2"); RTABMAP_PARAM(Odom, Strategy, int, 0, "0=Frame-to-Map (F2M) 1=Frame-to-Frame (F2F) 2=Fovis 3=viso2 4=DVO-SLAM 5=ORB_SLAM2");
RTABMAP_PARAM(Odom, ResetCountdown, int, 0, "Automatically reset odometry after X consecutive images on which odometry cannot be computed (value=0 disables auto-reset)."); RTABMAP_PARAM(Odom, ResetCountdown, int, 0, "Automatically reset odometry after X consecutive images on which odometry cannot be computed (value=0 disables auto-reset).");
RTABMAP_PARAM(Odom, Holonomic, bool, true, "If the robot is holonomic (strafing commands can be issued). If not, y value will be estimated from x and yaw values (y=x*tan(yaw))."); RTABMAP_PARAM(Odom, Holonomic, bool, true, "If the robot is holonomic (strafing commands can be issued). If not, y value will be estimated from x and yaw values (y=x*tan(yaw)).");
RTABMAP_PARAM(Odom, FillInfoData, bool, true, "Fill info with data (inliers/outliers features)."); RTABMAP_PARAM(Odom, FillInfoData, bool, true, "Fill info with data (inliers/outliers features).");
@@ -444,6 +444,11 @@ class RTABMAP_EXP Parameters
RTABMAP_PARAM(OdomViso2, BucketWidth, double, 50, "Width of bucket."); RTABMAP_PARAM(OdomViso2, BucketWidth, double, 50, "Width of bucket.");
RTABMAP_PARAM(OdomViso2, BucketHeight, double, 50, "Height of bucket."); RTABMAP_PARAM(OdomViso2, BucketHeight, double, 50, "Height of bucket.");
// Odometry ORB_SLAM2
RTABMAP_PARAM_STR(OdomORBSLAM2, VocPath, "", "Path to ORB vocabulary (*.txt).");
RTABMAP_PARAM(OdomORBSLAM2, Bf, double, 0.076, "Fake IR projector baseline (m) used only when stereo is not used.");
RTABMAP_PARAM(OdomORBSLAM2, ThDepth, double, 40.0, "Close/Far threshold. Baseline times.");
// Common registration parameters // Common registration parameters
RTABMAP_PARAM(Reg, VarianceFromInliersCount, bool, false, "Set variance as the inverse of the number of inliers. Otherwise, the variance is computed as the average 3D position error of the inliers."); RTABMAP_PARAM(Reg, VarianceFromInliersCount, bool, false, "Set variance as the inverse of the number of inliers. Otherwise, the variance is computed as the average 3D position error of the inliers.");
RTABMAP_PARAM(Reg, VarianceNormalized, bool, false, "Normalize covariance values. Position variances are multiplied by norm of the transform and orientation variances are multiplied by angle of the transform."); RTABMAP_PARAM(Reg, VarianceNormalized, bool, false, "Normalize covariance values. Position variances are multiplied by norm of the transform and orientation variances are multiplied by angle of the transform.");
@@ -479,7 +484,7 @@ class RTABMAP_EXP Parameters
RTABMAP_PARAM(Vis, SubPixIterations, int, 0, "See cv::cornerSubPix(). 0 disables sub pixel refining."); RTABMAP_PARAM(Vis, SubPixIterations, int, 0, "See cv::cornerSubPix(). 0 disables sub pixel refining.");
RTABMAP_PARAM(Vis, SubPixEps, float, 0.02, "See cv::cornerSubPix()."); RTABMAP_PARAM(Vis, SubPixEps, float, 0.02, "See cv::cornerSubPix().");
RTABMAP_PARAM(Vis, CorType, int, 0, "Correspondences computation approach: 0=Features Matching, 1=Optical Flow"); RTABMAP_PARAM(Vis, CorType, int, 0, "Correspondences computation approach: 0=Features Matching, 1=Optical Flow");
RTABMAP_PARAM(Vis, CorNNType, int, 1, uFormat("[%s=0] kNNFlannNaive=0, kNNFlannKdTree=1, kNNFlannLSH=2, kNNBruteForce=3, kNNBruteForceGPU=4. Used for features matching approach.", kVisCorType().c_str())); RTABMAP_PARAM(Vis, CorNNType, int, 1, uFormat("[%s=0] kNNFlannNaive=0, kNNFlannKdTree=1, kNNFlannLSH=2, kNNBruteForce=3, kNNBruteForceGPU=4. Used for features matching approach.", kVisCorType().c_str()));
RTABMAP_PARAM(Vis, CorNNDR, float, 0.6, uFormat("[%s=0] NNDR: nearest neighbor distance ratio. Used for features matching approach.", kVisCorType().c_str())); RTABMAP_PARAM(Vis, CorNNDR, float, 0.6, uFormat("[%s=0] NNDR: nearest neighbor distance ratio. Used for features matching approach.", kVisCorType().c_str()));
RTABMAP_PARAM(Vis, CorGuessWinSize, int, 20, uFormat("[%s=0] Matching window size (pixels) around projected points when a guess transform is provided to find correspondences. 0 means disabled.", kVisCorType().c_str())); RTABMAP_PARAM(Vis, CorGuessWinSize, int, 20, uFormat("[%s=0] Matching window size (pixels) around projected points when a guess transform is provided to find correspondences. 0 means disabled.", kVisCorType().c_str()));
RTABMAP_PARAM(Vis, CorFlowWinSize, int, 16, uFormat("[%s=1] See cv::calcOpticalFlowPyrLK(). Used for optical flow approach.", kVisCorType().c_str())); RTABMAP_PARAM(Vis, CorFlowWinSize, int, 16, uFormat("[%s=1] See cv::calcOpticalFlowPyrLK(). Used for optical flow approach.", kVisCorType().c_str()));

12
corelib/src/CMakeLists.txt
View File

@@ -63,6 +63,7 @@ SET(SRC_FILES
OdometryFovis.cpp OdometryFovis.cpp
OdometryViso2.cpp OdometryViso2.cpp
OdometryDVO.cpp OdometryDVO.cpp
OdometryORBSLAM2.cpp
Stereo.cpp Stereo.cpp
StereoDense.cpp StereoDense.cpp
@@ -311,6 +312,17 @@ IF(dvo_core_FOUND)
) )
ENDIF(dvo_core_FOUND) ENDIF(dvo_core_FOUND)
IF(ORB_SLAM2_FOUND)
SET(INCLUDE_DIRS
${INCLUDE_DIRS}
${ORB_SLAM2_INCLUDE_DIRS}
)
SET(LIBRARIES
${LIBRARIES}
${ORB_SLAM2_LIBRARIES}
)
ENDIF(ORB_SLAM2_FOUND)
#################################### ####################################
# Generate resources files # Generate resources files
#################################### ####################################

6
corelib/src/Graph.cpp
View File

@@ -442,7 +442,7 @@ struct errors {
std::vector<float> trajectoryDistances (const std::vector<Transform> &poses) { std::vector<float> trajectoryDistances (const std::vector<Transform> &poses) {
std::vector<float> dist; std::vector<float> dist;
dist.push_back(0); dist.push_back(0);
for (int32_t i=1; i<poses.size(); i++) { for (unsigned int i=1; i<poses.size(); i++) {
Transform P1 = poses[i-1]; Transform P1 = poses[i-1];
Transform P2 = poses[i]; Transform P2 = poses[i];
float dx = P1.x()-P2.x(); float dx = P1.x()-P2.x();
@@ -454,7 +454,7 @@ std::vector<float> trajectoryDistances (const std::vector<Transform> &poses) {
} }
int32_t lastFrameFromSegmentLength(std::vector<float> &dist,int32_t first_frame,float len) { int32_t lastFrameFromSegmentLength(std::vector<float> &dist,int32_t first_frame,float len) {
for (int32_t i=first_frame; i<dist.size(); i++) for (unsigned int i=first_frame; i<dist.size(); i++)
if (dist[i]>dist[first_frame]+len) if (dist[i]>dist[first_frame]+len)
return i; return i;
return -1; return -1;
@@ -493,7 +493,7 @@ void calcKittiSequenceErrors (
std::vector<float> dist = trajectoryDistances(poses_gt); std::vector<float> dist = trajectoryDistances(poses_gt);
// for all start positions do // for all start positions do
for (int32_t first_frame=0; first_frame<poses_gt.size(); first_frame+=step_size) { for (unsigned int first_frame=0; first_frame<poses_gt.size(); first_frame+=step_size) {
// for all segment lengths do // for all segment lengths do
for (int32_t i=0; i<num_lengths; i++) { for (int32_t i=0; i<num_lengths; i++) {

4
corelib/src/Odometry.cpp
View File

@@ -31,6 +31,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "rtabmap/core/OdometryFovis.h" #include "rtabmap/core/OdometryFovis.h"
#include "rtabmap/core/OdometryViso2.h" #include "rtabmap/core/OdometryViso2.h"
#include "rtabmap/core/OdometryDVO.h" #include "rtabmap/core/OdometryDVO.h"
#include "rtabmap/core/OdometryORBSLAM2.h"
#include "rtabmap/core/OdometryInfo.h" #include "rtabmap/core/OdometryInfo.h"
#include "rtabmap/core/util3d.h" #include "rtabmap/core/util3d.h"
#include "rtabmap/core/util3d_mapping.h" #include "rtabmap/core/util3d_mapping.h"
@@ -59,6 +60,9 @@ Odometry * Odometry::create(Odometry::Type & type, const ParametersMap & paramet
Odometry * odometry = 0; Odometry * odometry = 0;
switch(type) switch(type)
{ {
case Odometry::kTypeORBSLAM2:
odometry = new OdometryORBSLAM2(parameters);
break;
case Odometry::kTypeDVO: case Odometry::kTypeDVO:
odometry = new OdometryDVO(parameters); odometry = new OdometryDVO(parameters);
break; break;

935
corelib/src/OdometryORBSLAM2.cpp Normal file
View File

@@ -0,0 +1,935 @@
/*
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/OdometryORBSLAM2.h"
#include "rtabmap/core/OdometryInfo.h"
#include "rtabmap/core/util2d.h"
#include "rtabmap/core/Version.h"
#include "rtabmap/core/util3d_transforms.h"
#include "rtabmap/utilite/ULogger.h"
#include "rtabmap/utilite/UTimer.h"
#include "rtabmap/utilite/UStl.h"
#ifdef RTABMAP_ORB_SLAM2
#include <System.h>
#include <thread>
using namespace std;
namespace ORB_SLAM2 {
// Override original Tracking object to comment all rendering stuff
class Tracker: public Tracking
{
public:
Tracker(System* pSys, ORBVocabulary* pVoc, FrameDrawer* pFrameDrawer, MapDrawer* pMapDrawer, Map* pMap,
KeyFrameDatabase* pKFDB, const std::string &strSettingPath, const int sensor) :
Tracking(pSys, pVoc, pFrameDrawer, pMapDrawer, pMap, pKFDB, strSettingPath, sensor)
{
}
protected:
void Track()
{
if(mState==NO_IMAGES_YET)
{
mState = NOT_INITIALIZED;
}
mLastProcessedState=mState;
// Get Map Mutex -> Map cannot be changed
unique_lock<mutex> lock(mpMap->mMutexMapUpdate);
if(mState==NOT_INITIALIZED)
{
// if(mSensor==System::STEREO || mSensor==System::RGBD)
StereoInitialization();
//else
// MonocularInitialization();
//mpFrameDrawer->Update(this);
if(mState!=OK)
return;
}
else
{
// System is initialized. Track Frame.
bool bOK;
// Initial camera pose estimation using motion model or relocalization (if tracking is lost)
if(!mbOnlyTracking)
{
// Local Mapping is activated. This is the normal behaviour, unless
// you explicitly activate the "only tracking" mode.
if(mState==OK || mState==LOST)
{
// Local Mapping might have changed some MapPoints tracked in last frame
CheckReplacedInLastFrame();
if(mVelocity.empty() || mCurrentFrame.mnId<mnLastRelocFrameId+2)
{
bOK = TrackReferenceKeyFrame();
}
else
{
bOK = TrackWithMotionModel();
if(!bOK)
bOK = TrackReferenceKeyFrame();
}
if(bOK)
{
mState=OK;
}
}
else
{
bOK = Relocalization();
}
}
else
{
// Localization Mode: Local Mapping is deactivated
if(mState==LOST)
{
bOK = Relocalization();
}
else
{
if(!mbVO)
{
// In last frame we tracked enough MapPoints in the map
if(!mVelocity.empty())
{
bOK = TrackWithMotionModel();
}
else
{
bOK = TrackReferenceKeyFrame();
}
}
else
{
// In last frame we tracked mainly "visual odometry" points.
// We compute two camera poses, one from motion model and one doing relocalization.
// If relocalization is sucessfull we choose that solution, otherwise we retain
// the "visual odometry" solution.
bool bOKMM = false;
bool bOKReloc = false;
std::vector<MapPoint*> vpMPsMM;
std::vector<bool> vbOutMM;
cv::Mat TcwMM;
if(!mVelocity.empty())
{
bOKMM = TrackWithMotionModel();
vpMPsMM = mCurrentFrame.mvpMapPoints;
vbOutMM = mCurrentFrame.mvbOutlier;
TcwMM = mCurrentFrame.mTcw.clone();
}
bOKReloc = Relocalization();
if(bOKMM && !bOKReloc)
{
mCurrentFrame.SetPose(TcwMM);
mCurrentFrame.mvpMapPoints = vpMPsMM;
mCurrentFrame.mvbOutlier = vbOutMM;
if(mbVO)
{
for(int i =0; i<mCurrentFrame.N; i++)
{
if(mCurrentFrame.mvpMapPoints[i] && !mCurrentFrame.mvbOutlier[i])
{
mCurrentFrame.mvpMapPoints[i]->IncreaseFound();
}
}
}
}
else if(bOKReloc)
{
mbVO = false;
}
bOK = bOKReloc || bOKMM;
}
}
}
mCurrentFrame.mpReferenceKF = mpReferenceKF;
// If we have an initial estimation of the camera pose and matching. Track the local map.
if(!mbOnlyTracking)
{
if(bOK)
bOK = TrackLocalMap();
}
else
{
// mbVO true means that there are few matches to MapPoints in the map. We cannot retrieve
// a local map and therefore we do not perform TrackLocalMap(). Once the system relocalizes
// the camera we will use the local map again.
if(bOK && !mbVO)
bOK = TrackLocalMap();
}
if(bOK)
mState = OK;
else
mState=LOST;
// Update drawer
//mpFrameDrawer->Update(this);
// If tracking were good, check if we insert a keyframe
if(bOK)
{
// Update motion model
if(!mLastFrame.mTcw.empty())
{
cv::Mat LastTwc = cv::Mat::eye(4,4,CV_32F);
mLastFrame.GetRotationInverse().copyTo(LastTwc.rowRange(0,3).colRange(0,3));
mLastFrame.GetCameraCenter().copyTo(LastTwc.rowRange(0,3).col(3));
mVelocity = mCurrentFrame.mTcw*LastTwc;
}
else
mVelocity = cv::Mat();
//mpMapDrawer->SetCurrentCameraPose(mCurrentFrame.mTcw);
// Clean VO matches
for(int i=0; i<mCurrentFrame.N; i++)
{
MapPoint* pMP = mCurrentFrame.mvpMapPoints[i];
if(pMP)
if(pMP->Observations()<1)
{
mCurrentFrame.mvbOutlier[i] = false;
mCurrentFrame.mvpMapPoints[i]=static_cast<MapPoint*>(NULL);
}
}
// Delete temporal MapPoints
for(list<MapPoint*>::iterator lit = mlpTemporalPoints.begin(), lend = mlpTemporalPoints.end(); lit!=lend; lit++)
{
MapPoint* pMP = *lit;
delete pMP;
}
mlpTemporalPoints.clear();
// Check if we need to insert a new keyframe
if(NeedNewKeyFrame())
CreateNewKeyFrame();
// We allow points with high innovation (considererd outliers by the Huber Function)
// pass to the new keyframe, so that bundle adjustment will finally decide
// if they are outliers or not. We don't want next frame to estimate its position
// with those points so we discard them in the frame.
for(int i=0; i<mCurrentFrame.N;i++)
{
if(mCurrentFrame.mvpMapPoints[i] && mCurrentFrame.mvbOutlier[i])
mCurrentFrame.mvpMapPoints[i]=static_cast<MapPoint*>(NULL);
}
}
// Reset if the camera get lost soon after initialization
if(mState==LOST)
{
//if(mpMap->KeyFramesInMap()<=5)
{
UWARN("Track lost...");
return;
}
}
if(!mCurrentFrame.mpReferenceKF)
mCurrentFrame.mpReferenceKF = mpReferenceKF;
mLastFrame = Frame(mCurrentFrame);
}
// Store frame pose information to retrieve the complete camera trajectory afterwards.
if(!mCurrentFrame.mTcw.empty())
{
cv::Mat Tcr = mCurrentFrame.mTcw*mCurrentFrame.mpReferenceKF->GetPoseInverse();
mlRelativeFramePoses.push_back(Tcr);
mlpReferences.push_back(mpReferenceKF);
mlFrameTimes.push_back(mCurrentFrame.mTimeStamp);
mlbLost.push_back(mState==LOST);
}
else
{
// This can happen if tracking is lost
mlRelativeFramePoses.push_back(mlRelativeFramePoses.back());
mlpReferences.push_back(mlpReferences.back());
mlFrameTimes.push_back(mlFrameTimes.back());
mlbLost.push_back(mState==LOST);
}
}
void StereoInitialization()
{
if(mCurrentFrame.N>500)
{
// Set Frame pose to the origin
mCurrentFrame.SetPose(cv::Mat::eye(4,4,CV_32F));
// Create KeyFrame
KeyFrame* pKFini = new KeyFrame(mCurrentFrame,mpMap,mpKeyFrameDB);
// Insert KeyFrame in the map
mpMap->AddKeyFrame(pKFini);
// Create MapPoints and asscoiate to KeyFrame
for(int i=0; i<mCurrentFrame.N;i++)
{
float z = mCurrentFrame.mvDepth[i];
if(z>0)
{
cv::Mat x3D = mCurrentFrame.UnprojectStereo(i);
MapPoint* pNewMP = new MapPoint(x3D,pKFini,mpMap);
pNewMP->AddObservation(pKFini,i);
pKFini->AddMapPoint(pNewMP,i);
pNewMP->ComputeDistinctiveDescriptors();
pNewMP->UpdateNormalAndDepth();
mpMap->AddMapPoint(pNewMP);
mCurrentFrame.mvpMapPoints[i]=pNewMP;
}
}
cout << "New map created with " << mpMap->MapPointsInMap() << " points" << endl;
mpLocalMapper->InsertKeyFrame(pKFini);
mLastFrame = Frame(mCurrentFrame);
mnLastKeyFrameId=mCurrentFrame.mnId;
mpLastKeyFrame = pKFini;
mvpLocalKeyFrames.push_back(pKFini);
mvpLocalMapPoints=mpMap->GetAllMapPoints();
mpReferenceKF = pKFini;
mCurrentFrame.mpReferenceKF = pKFini;
mpMap->SetReferenceMapPoints(mvpLocalMapPoints);
mpMap->mvpKeyFrameOrigins.push_back(pKFini);
//mpMapDrawer->SetCurrentCameraPose(mCurrentFrame.mTcw);
mState=OK;
}
}
public:
cv::Mat GrabImageStereo(const cv::Mat &imRectLeft, const cv::Mat &imRectRight, const double &timestamp)
{
mImGray = imRectLeft;
cv::Mat imGrayRight = imRectRight;
if(mImGray.channels()==3)
{
if(mbRGB)
{
cvtColor(mImGray,mImGray,CV_RGB2GRAY);
}
else
{
cvtColor(mImGray,mImGray,CV_BGR2GRAY);
}
}
else if(mImGray.channels()==4)
{
if(mbRGB)
{
cvtColor(mImGray,mImGray,CV_RGBA2GRAY);
}
else
{
cvtColor(mImGray,mImGray,CV_BGRA2GRAY);
}
}
if(imGrayRight.channels()==3)
{
if(mbRGB)
{
cvtColor(imGrayRight,imGrayRight,CV_RGB2GRAY);
}
else
{
cvtColor(imGrayRight,imGrayRight,CV_BGR2GRAY);
}
}
else if(imGrayRight.channels()==4)
{
if(mbRGB)
{
cvtColor(imGrayRight,imGrayRight,CV_RGBA2GRAY);
}
else
{
cvtColor(imGrayRight,imGrayRight,CV_BGRA2GRAY);
}
}
mCurrentFrame = Frame(mImGray,imGrayRight,timestamp,mpORBextractorLeft,mpORBextractorRight,mpORBVocabulary,mK,mDistCoef,mbf,mThDepth);
Track();
return mCurrentFrame.mTcw.clone();
}
cv::Mat GrabImageRGBD(const cv::Mat &imRGB,const cv::Mat &imD, const double &timestamp)
{
mImGray = imRGB;
cv::Mat imDepth = imD;
if(mImGray.channels()==3)
{
if(mbRGB)
cvtColor(mImGray,mImGray,CV_RGB2GRAY);
else
cvtColor(mImGray,mImGray,CV_BGR2GRAY);
}
else if(mImGray.channels()==4)
{
if(mbRGB)
cvtColor(mImGray,mImGray,CV_RGBA2GRAY);
else
cvtColor(mImGray,mImGray,CV_BGRA2GRAY);
}
UASSERT(imDepth.type()==CV_32F);
mCurrentFrame = Frame(mImGray,imDepth,timestamp,mpORBextractorLeft,mpORBVocabulary,mK,mDistCoef,mbf,mThDepth);
Track();
return mCurrentFrame.mTcw.clone();
}
};
// Hack to disable loop closing
class LoopCloser: public LoopClosing
{
public:
LoopCloser(Map* pMap, KeyFrameDatabase* pDB, ORBVocabulary* pVoc,const bool bFixScale) :
LoopClosing(pMap, pDB, pVoc, bFixScale)
{
}
public:
void RunNoLoop()
{
mbFinished =false;
while(1)
{
// just clear the buffer
{
unique_lock<mutex> lock(mMutexLoopQueue);
mlpLoopKeyFrameQueue.clear();
}
ResetIfRequested();
if(CheckFinish())
break;
usleep(30000);
}
SetFinish();
}
};
} // namespace ORB_SLAM2
class ORBSLAM2System
{
public:
ORBSLAM2System(const rtabmap::ParametersMap & parameters) :
mpVocabulary(0),
mpKeyFrameDatabase(0),
mpMap(0),
mpTracker(0),
mpLocalMapper(0),
mpLoopCloser(0),
mptLocalMapping(0),
mptLoopClosing(0),
parameters_(parameters)
{
std::string vocabularyPath;
rtabmap::Parameters::parse(parameters, rtabmap::Parameters::kOdomORBSLAM2VocPath(), vocabularyPath);
if(!vocabularyPath.empty())
{
//Load ORB Vocabulary
UWARN("Loading ORB Vocabulary: \"%s\". This could take a while...", vocabularyPath.c_str());
mpVocabulary = new ORB_SLAM2::ORBVocabulary();
bool bVocLoad = mpVocabulary->loadFromTextFile(vocabularyPath);
if(!bVocLoad)
{
UERROR("Failed to open vocabulary at %s", vocabularyPath.c_str());
delete mpVocabulary;
mpVocabulary = 0;
}
else
{
UWARN("Vocabulary loaded!");
}
}
else
{
UERROR("ORBSLAM2 vocabulary path should be set! (Parameter name=\"%s\")", rtabmap::Parameters::kOdomORBSLAM2VocPath().c_str());
}
}
bool init(const rtabmap::CameraModel & model, bool stereo, double baseline)
{
if(!mpVocabulary)
{
UERROR("Vocabulary not loaded!");
return false;
}
this->shutdown();
// Create configuration file
std::string workingDir;
rtabmap::Parameters::parse(parameters_, rtabmap::Parameters::kRtabmapWorkingDirectory(), workingDir);
if(workingDir.empty())
{
workingDir = ".";
}
std::string configPath = workingDir+"/rtabmap_orbslam2.yaml";
std::ofstream ofs (configPath, std::ofstream::out);
ofs << "%YAML:1.0" << std::endl;
ofs << std::endl;
//# Camera calibration and distortion parameters (OpenCV)
ofs << "Camera.fx: " << model.fx() << std::endl;
ofs << "Camera.fy: " << model.fy() << std::endl;
ofs << "Camera.cx: " << model.cx() << std::endl;
ofs << "Camera.cy: " << model.cy() << std::endl;
ofs << std::endl;
if(model.D().cols < 4)
{
ofs << "Camera.k1: " << 0 << std::endl;
ofs << "Camera.k2: " << 0 << std::endl;
ofs << "Camera.p1: " << 0 << std::endl;
ofs << "Camera.p2: " << 0 << std::endl;
if(!stereo)
{
ofs << "Camera.k3: " << 0 << std::endl;
}
}
if(model.D().cols >= 4)
{
ofs << "Camera.k1: " << model.D().at<double>(0,0) << std::endl;
ofs << "Camera.k2: " << model.D().at<double>(0,1) << std::endl;
ofs << "Camera.p1: " << model.D().at<double>(0,2) << std::endl;
ofs << "Camera.p2: " << model.D().at<double>(0,3) << std::endl;
}
if(model.D().cols >= 5)
{
ofs << "Camera.k3: " << model.D().at<double>(0,4) << std::endl;
}
if(model.D().cols > 5)
{
UWARN("Unhandled camera distortion size %d, only 5 first coefficients used", model.D().cols);
}
ofs << std::endl;
ofs << "Camera.width: " << model.imageWidth() << std::endl;
ofs << "Camera.height: " << model.imageHeight() << std::endl;
ofs << std::endl;
//# IR projector baseline times fx (aprox.)
if(baseline <= 0.0)
{
baseline = rtabmap::Parameters::defaultOdomORBSLAM2Bf();
rtabmap::Parameters::parse(parameters_, rtabmap::Parameters::kOdomORBSLAM2Bf(), baseline);
}
ofs << "Camera.bf: " << model.fx()*baseline << 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;
//# Close/Far threshold. Baseline times.
double thDepth = rtabmap::Parameters::defaultOdomORBSLAM2ThDepth();
rtabmap::Parameters::parse(parameters_, rtabmap::Parameters::kOdomORBSLAM2ThDepth(), thDepth);
ofs << "ThDepth: " << thDepth << std::endl;
ofs << std::endl;
//# Deptmap values factor
ofs << "DepthMapFactor: " << 1000.0 << std::endl;
ofs << std::endl;
//#--------------------------------------------------------------------------------------------
//# ORB Parameters
//#--------------------------------------------------------------------------------------------
//# ORB Extractor: Number of features per image
int features = rtabmap::Parameters::defaultVisMaxFeatures();
rtabmap::Parameters::parse(parameters_, rtabmap::Parameters::kVisMaxFeatures(), 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;
ofs.close();
//Create KeyFrame Database
mpKeyFrameDatabase = new ORB_SLAM2::KeyFrameDatabase(*mpVocabulary);
//Create the Map
mpMap = new ORB_SLAM2::Map();
//Initialize the Tracking thread
//(it will live in the main thread of execution, the one that called this constructor)
mpTracker = new ORB_SLAM2::Tracker(0, mpVocabulary, 0, 0, mpMap, mpKeyFrameDatabase, configPath, stereo?ORB_SLAM2::System::STEREO:ORB_SLAM2::System::RGBD);
//Initialize the Local Mapping thread and launch
mpLocalMapper = new ORB_SLAM2::LocalMapping(mpMap, false);
//Initialize the Loop Closing thread and launch
mpLoopCloser = new ORB_SLAM2::LoopCloser(mpMap, mpKeyFrameDatabase, mpVocabulary, true);
mptLocalMapping = new thread(&ORB_SLAM2::LocalMapping::Run, mpLocalMapper);
mptLoopClosing = new thread(&ORB_SLAM2::LoopCloser::RunNoLoop, mpLoopCloser);
//Set pointers between threads
mpTracker->SetLocalMapper(mpLocalMapper);
mpTracker->SetLoopClosing(mpLoopCloser);
mpTracker->SetViewer(0);
mpLocalMapper->SetTracker(mpTracker);
mpLocalMapper->SetLoopCloser(mpLoopCloser);
mpLoopCloser->SetTracker(mpTracker);
mpLoopCloser->SetLocalMapper(mpLocalMapper);
return true;
}
virtual ~ORBSLAM2System()
{
shutdown();
if(mpVocabulary)
{
delete mpVocabulary;
}
}
void shutdown()
{
if(mpMap)
{
mpLocalMapper->RequestFinish();
mpLoopCloser->RequestFinish();
// Wait until all thread have effectively stopped
while(!mpLocalMapper->isFinished() || !mpLoopCloser->isFinished() || mpLoopCloser->isRunningGBA())
{
usleep(5000);
}
//cleanup!
mptLoopClosing->join();
delete mptLoopClosing;
mptLoopClosing = 0;
mptLocalMapping->join();
delete mptLocalMapping;
mptLocalMapping = 0;
delete mpLoopCloser;
mpLoopCloser=0;
delete mpLocalMapper;
mpLocalMapper=0;
delete mpTracker;
mpTracker=0;
delete mpMap;
mpMap=0;
delete mpKeyFrameDatabase;
mpKeyFrameDatabase=0;
}
}
public:
// ORB vocabulary used for place recognition and feature matching.
ORB_SLAM2::ORBVocabulary* mpVocabulary;
// KeyFrame database for place recognition (relocalization and loop detection).
ORB_SLAM2::KeyFrameDatabase* mpKeyFrameDatabase;
// Map structure that stores the pointers to all KeyFrames and MapPoints.
ORB_SLAM2::Map* mpMap;
// Tracker. It receives a frame and computes the associated camera pose.
// It also decides when to insert a new keyframe, create some new MapPoints and
// performs relocalization if tracking fails.
ORB_SLAM2::Tracker* mpTracker;
// Local Mapper. It manages the local map and performs local bundle adjustment.
ORB_SLAM2::LocalMapping* mpLocalMapper;
// Loop Closer. It searches loops with every new keyframe. If there is a loop it performs
// a pose graph optimization and full bundle adjustment (in a new thread) afterwards.
ORB_SLAM2::LoopCloser* mpLoopCloser;
// System threads: Local Mapping, Loop Closing, Viewer.
// The Tracking thread "lives" in the main execution thread that creates the System object.
std::thread* mptLocalMapping;
std::thread* mptLoopClosing;
rtabmap::ParametersMap parameters_;
};
#endif
namespace rtabmap {
OdometryORBSLAM2::OdometryORBSLAM2(const ParametersMap & parameters) :
Odometry(parameters),
orbslam2_(0),
system_(0),
firstFrame_(true)
{
orbslam2_ = new ORBSLAM2System(parameters);
}
OdometryORBSLAM2::~OdometryORBSLAM2()
{
#ifdef RTABMAP_ORB_SLAM2
if(orbslam2_)
{
delete orbslam2_;
}
#endif
}
void OdometryORBSLAM2::reset(const Transform & initialPose)
{
Odometry::reset(initialPose);
#ifdef RTABMAP_ORB_SLAM2
if(orbslam2_)
{
orbslam2_->shutdown();
}
firstFrame_ = true;
#endif
}
// return not null transform if odometry is correctly computed
Transform OdometryORBSLAM2::computeTransform(
SensorData & data,
const Transform & guess,
OdometryInfo * info)
{
Transform t;
#ifdef RTABMAP_ORB_SLAM2
UTimer timer;
if(data.imageRaw().empty() ||
data.imageRaw().rows != data.depthOrRightRaw().rows ||
data.imageRaw().cols != data.depthOrRightRaw().cols)
{
UERROR("Not supported input!");
return t;
}
if(!((data.cameraModels().size() == 1 &&
data.cameraModels()[0].isValidForReprojection()) ||
(data.stereoCameraModel().isValidForProjection() &&
data.stereoCameraModel().left().isValidForReprojection() &&
data.stereoCameraModel().right().isValidForReprojection())))
{
UERROR("Invalid camera model!");
return t;
}
bool stereo = data.cameraModels().size() == 0;
cv::Mat covariance;
if(orbslam2_->mpTracker == 0)
{
CameraModel model = data.cameraModels().size()==1?data.cameraModels()[0]:data.stereoCameraModel().left();
if(!orbslam2_->init(model, stereo, data.stereoCameraModel().baseline()))
{
return t;
}
}
cv::Mat Tcw;
Transform localTransform;
if(stereo)
{
localTransform = data.stereoCameraModel().localTransform();
Tcw = ((ORB_SLAM2::Tracker*)orbslam2_->mpTracker)->GrabImageStereo(data.imageRaw(), data.rightRaw(), data.stamp());
}
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 = ((ORB_SLAM2::Tracker*)orbslam2_->mpTracker)->GrabImageRGBD(data.imageRaw(), depth, data.stamp());
}
if(orbslam2_->mpTracker->mState == ORB_SLAM2::Tracking::LOST)
{
covariance = cv::Mat::eye(6,6,CV_64FC1)*9999.0f;
}
else if(Tcw.cols == 4 && Tcw.rows == 4)
{
t = Transform(cv::Mat(Tcw, cv::Range(0,3), cv::Range(0,4)).clone());
if(!t.isNull() && !t.isIdentity() && !localTransform.isIdentity() && !localTransform.isNull())
{
// from camera frame to base frame
t = localTransform * t.inverse() * localTransform.inverse();
}
t = this->getPose().inverse() * t;
if(firstFrame_)
{
// just recovered of being lost, set high covariance
covariance = cv::Mat::eye(6,6,CV_64FC1)*9999.0f;
firstFrame_ = false;
}
else
{
//based on values set in viso2_ros
covariance = cv::Mat::eye(6,6, CV_64FC1);
covariance.at<double>(0,0) = 0.002;
covariance.at<double>(1,1) = 0.002;
covariance.at<double>(2,2) = 0.05;
covariance.at<double>(3,3) = 0.09;
covariance.at<double>(4,4) = 0.09;
covariance.at<double>(5,5) = 0.09;
}
}
int totalMapPoints= 0;
int totalKfs= 0;
if(orbslam2_->mpMap)
{
totalMapPoints = orbslam2_->mpMap->MapPointsInMap();
totalKfs = orbslam2_->mpMap->KeyFramesInMap();
}
if(info)
{
info->lost = t.isNull();
info->type = (int)kTypeORBSLAM2;
info->covariance = covariance;
info->localMapSize = totalMapPoints;
info->localKeyFrames = totalKfs;
if(this->isInfoDataFilled() && orbslam2_->mpTracker && orbslam2_->mpMap)
{
const std::vector<cv::KeyPoint> & kpts = orbslam2_->mpTracker->mCurrentFrame.mvKeys;
info->wordMatches.resize(kpts.size());
info->wordInliers.resize(kpts.size());
int oi = 0;
for (unsigned int i = 0; i < kpts.size(); ++i)
{
int wordId;
if(orbslam2_->mpTracker->mCurrentFrame.mvpMapPoints[i] != 0)
{
wordId = orbslam2_->mpTracker->mCurrentFrame.mvpMapPoints[i]->mnId;
}
else
{
wordId = -(i+1);
}
info->words.insert(std::make_pair(wordId, kpts[i]));
if(orbslam2_->mpTracker->mCurrentFrame.mvpMapPoints[i] != 0)
{
info->wordMatches[oi] = wordId;
info->wordInliers[oi] = wordId;
++oi;
}
}
info->wordMatches.resize(oi);
info->wordInliers.resize(oi);
std::vector<ORB_SLAM2::MapPoint*> mapPoints = orbslam2_->mpMap->GetAllMapPoints();
for (unsigned int i = 0; i < mapPoints.size(); ++i)
{
cv::Mat pt = mapPoints[i]->GetWorldPos();
info->localMap.insert(std::make_pair(mapPoints[i]->mnId, util3d::transformPoint(cv::Point3f(pt), localTransform)));
}
}
}
UINFO("Odom update time = %fs, map points=%d, keyframes=%d, lost=%s", timer.elapsed(), totalMapPoints, totalKfs, t.isNull()?"true":"false");
#else
UERROR("RTAB-Map is not built with ORB_SLAM2 support! Select another visual odometry approach.");
#endif
return t;
}
} // namespace rtabmap

1
guilib/include/rtabmap/gui/PreferencesDialog.h
View File

@@ -305,6 +305,7 @@ private slots:
void useGridProjRayTracing(); void useGridProjRayTracing();
void changeWorkingDirectory(); void changeWorkingDirectory();
void changeDictionaryPath(); void changeDictionaryPath();
void changeOdometryORBSLAM2Vocabulary();
void readSettingsEnd(); void readSettingsEnd();
void setupTreeView(); void setupTreeView();
void updateBasicParameter(); void updateBasicParameter();

30
guilib/src/AboutDialog.cpp
View File

@@ -49,49 +49,79 @@ AboutDialog::AboutDialog(QWidget * parent) :
QString cv_version = CV_VERSION; QString cv_version = CV_VERSION;
#ifdef RTABMAP_NONFREE #ifdef RTABMAP_NONFREE
cv_version.append(" [With nonfree]"); cv_version.append(" [With nonfree]");
_ui->label_opencv_license->setText("Not Commercial");
#else #else
cv_version.append(" [Without nonfree]"); cv_version.append(" [Without nonfree]");
_ui->label_opencv_license->setText("BSD");
#endif #endif
_ui->label_version->setText(version); _ui->label_version->setText(version);
_ui->label_opencv_version->setText(cv_version); _ui->label_opencv_version->setText(cv_version);
_ui->label_pcl_version->setText(PCL_VERSION_PRETTY); _ui->label_pcl_version->setText(PCL_VERSION_PRETTY);
_ui->label_vtk_version->setText(vtkVersion::GetVTKVersion()); _ui->label_vtk_version->setText(vtkVersion::GetVTKVersion());
_ui->label_qt_version->setText(qVersion());
#ifdef RTABMAP_OCTOMAP #ifdef RTABMAP_OCTOMAP
_ui->label_octomap->setText("Yes"); _ui->label_octomap->setText("Yes");
_ui->label_octomap_license->setEnabled(true);
#else #else
_ui->label_octomap->setText("No"); _ui->label_octomap->setText("No");
_ui->label_octomap_license->setEnabled(false);
#endif #endif
#ifdef RTABMAP_CPUTSDF #ifdef RTABMAP_CPUTSDF
_ui->label_cputsdf->setText("Yes"); _ui->label_cputsdf->setText("Yes");
_ui->label_cputsdf_license->setEnabled(true);
#else #else
_ui->label_cputsdf->setText("No"); _ui->label_cputsdf->setText("No");
_ui->label_cputsdf_license->setEnabled(false);
#endif #endif
_ui->label_freenect->setText(CameraFreenect::available()?"Yes":"No"); _ui->label_freenect->setText(CameraFreenect::available()?"Yes":"No");
_ui->label_freenect_license->setEnabled(CameraFreenect::available());
_ui->label_openni2->setText(CameraOpenNI2::available()?"Yes":"No"); _ui->label_openni2->setText(CameraOpenNI2::available()?"Yes":"No");
_ui->label_openni2_license->setEnabled(CameraOpenNI2::available());
_ui->label_freenect2->setText(CameraFreenect2::available()?"Yes":"No"); _ui->label_freenect2->setText(CameraFreenect2::available()?"Yes":"No");
_ui->label_freenect2_license->setEnabled(CameraFreenect2::available());
_ui->label_realsense->setText(CameraRealSense::available() ? "Yes" : "No"); _ui->label_realsense->setText(CameraRealSense::available() ? "Yes" : "No");
_ui->label_realsense_license->setEnabled(CameraRealSense::available());
_ui->label_dc1394->setText(CameraStereoDC1394::available()?"Yes":"No"); _ui->label_dc1394->setText(CameraStereoDC1394::available()?"Yes":"No");
_ui->label_dc1394_license->setEnabled(CameraStereoDC1394::available());
_ui->label_flycapture2->setText(CameraStereoFlyCapture2::available()?"Yes":"No"); _ui->label_flycapture2->setText(CameraStereoFlyCapture2::available()?"Yes":"No");
_ui->label_zed->setText(CameraStereoZed::available()?"Yes":"No"); _ui->label_zed->setText(CameraStereoZed::available()?"Yes":"No");
_ui->label_toro->setText(Optimizer::isAvailable(Optimizer::kTypeTORO)?"Yes":"No");
_ui->label_toro_license->setEnabled(Optimizer::isAvailable(Optimizer::kTypeTORO)?true:false);
_ui->label_g2o->setText(Optimizer::isAvailable(Optimizer::kTypeG2O)?"Yes":"No"); _ui->label_g2o->setText(Optimizer::isAvailable(Optimizer::kTypeG2O)?"Yes":"No");
_ui->label_g2o_license->setEnabled(Optimizer::isAvailable(Optimizer::kTypeG2O)?true:false);
_ui->label_gtsam->setText(Optimizer::isAvailable(Optimizer::kTypeGTSAM)?"Yes":"No"); _ui->label_gtsam->setText(Optimizer::isAvailable(Optimizer::kTypeGTSAM)?"Yes":"No");
_ui->label_gtsam_license->setEnabled(Optimizer::isAvailable(Optimizer::kTypeGTSAM)?true:false);
_ui->label_cvsba->setText(Optimizer::isAvailable(Optimizer::kTypeCVSBA)?"Yes":"No"); _ui->label_cvsba->setText(Optimizer::isAvailable(Optimizer::kTypeCVSBA)?"Yes":"No");
_ui->label_cvsba_license->setEnabled(Optimizer::isAvailable(Optimizer::kTypeCVSBA)?true:false);
#ifdef RTABMAP_FOVIS #ifdef RTABMAP_FOVIS
_ui->label_fovis->setText("Yes"); _ui->label_fovis->setText("Yes");
_ui->label_fovis_license->setEnabled(true);
#else #else
_ui->label_fovis->setText("No"); _ui->label_fovis->setText("No");
_ui->label_fovis_license->setEnabled(false);
#endif #endif
#ifdef RTABMAP_VISO2 #ifdef RTABMAP_VISO2
_ui->label_viso2->setText("Yes"); _ui->label_viso2->setText("Yes");
_ui->label_viso2_license->setEnabled(true);
#else #else
_ui->label_viso2->setText("No"); _ui->label_viso2->setText("No");
_ui->label_viso2_license->setEnabled(false);
#endif #endif
#ifdef RTABMAP_DVO #ifdef RTABMAP_DVO
_ui->label_dvo->setText("Yes"); _ui->label_dvo->setText("Yes");
_ui->label_dvo_license->setEnabled(true);
#else #else
_ui->label_dvo->setText("No"); _ui->label_dvo->setText("No");
_ui->label_dvo_license->setEnabled(false);
#endif
#ifdef RTABMAP_ORB_SLAM2
_ui->label_orbslam2->setText("Yes");
_ui->label_orbslam2_license->setEnabled(true);
#else
_ui->label_orbslam2->setText("No");
_ui->label_orbslam2_license->setEnabled(false);
#endif #endif
} }

8
guilib/src/MainWindow.cpp
View File

@@ -1188,7 +1188,7 @@ void MainWindow::processOdometry(const rtabmap::OdometryEvent & odom, bool dataI
{ {
if(_ui->imageView_odometry->isFeaturesShown()) if(_ui->imageView_odometry->isFeaturesShown())
{ {
if(odom.info().type == (int)Odometry::kTypeF2M) if(odom.info().type == (int)Odometry::kTypeF2M || odom.info().type == (int)Odometry::kTypeORBSLAM2)
{ {
if(_preferencesDialog->isOdomOnlyInliersShown()) if(_preferencesDialog->isOdomOnlyInliersShown())
{ {
@@ -1223,9 +1223,9 @@ void MainWindow::processOdometry(const rtabmap::OdometryEvent & odom, bool dataI
} }
} }
//detect if it is OdometryMono intitialization //detect if it is OdometryMono initialization
bool monoInitialization = false; bool monoInitialization = false;
if(_preferencesDialog->getOdomStrategy() == 5 && odom.info().type == (int)Odometry::kTypeF2F) if(_preferencesDialog->getOdomStrategy() == 6 && odom.info().type == (int)Odometry::kTypeF2F)
{ {
monoInitialization = true; monoInitialization = true;
} }
@@ -1258,7 +1258,7 @@ void MainWindow::processOdometry(const rtabmap::OdometryEvent & odom, bool dataI
_ui->imageView_odometry->setImageDepth(uCvMat2QImage(odom.data().depthOrRightRaw())); _ui->imageView_odometry->setImageDepth(uCvMat2QImage(odom.data().depthOrRightRaw()));
} }
if(odom.info().type == (int)Odometry::kTypeF2M) if(odom.info().type == (int)Odometry::kTypeF2M || odom.info().type == (int)Odometry::kTypeORBSLAM2)
{ {
if(_ui->imageView_odometry->isFeaturesShown() && !_preferencesDialog->isOdomOnlyInliersShown()) if(_ui->imageView_odometry->isFeaturesShown() && !_preferencesDialog->isOdomOnlyInliersShown())
{ {

4
guilib/src/OdometryViewer.cpp
View File

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

31
guilib/src/PreferencesDialog.cpp
View File

@@ -168,6 +168,9 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
#ifndef RTABMAP_DVO #ifndef RTABMAP_DVO
_ui->odom_strategy->setItemData(4, 0, Qt::UserRole - 1); _ui->odom_strategy->setItemData(4, 0, Qt::UserRole - 1);
#endif #endif
#ifndef RTABMAP_ORB_SLAM2
_ui->odom_strategy->setItemData(5, 0, Qt::UserRole - 1);
#endif
#ifndef RTABMAP_NONFREE #ifndef RTABMAP_NONFREE
_ui->comboBox_detector_strategy->setItemData(0, 0, Qt::UserRole - 1); _ui->comboBox_detector_strategy->setItemData(0, 0, Qt::UserRole - 1);
@@ -972,6 +975,12 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
_ui->doubleSpinBox_OdomViso2BucketWidth->setObjectName(Parameters::kOdomViso2BucketWidth().c_str()); _ui->doubleSpinBox_OdomViso2BucketWidth->setObjectName(Parameters::kOdomViso2BucketWidth().c_str());
_ui->doubleSpinBox_OdomViso2BucketHeight->setObjectName(Parameters::kOdomViso2BucketHeight().c_str()); _ui->doubleSpinBox_OdomViso2BucketHeight->setObjectName(Parameters::kOdomViso2BucketHeight().c_str());
// Odometry ORBSLAM2
_ui->lineEdit_OdomORBSLAM2VocPath->setObjectName(Parameters::kOdomORBSLAM2VocPath().c_str());
connect(_ui->toolButton_OdomORBSLAM2VocPath, SIGNAL(clicked()), this, SLOT(changeOdometryORBSLAM2Vocabulary()));
_ui->doubleSpinBox_OdomORBSLAM2Bf->setObjectName(Parameters::kOdomORBSLAM2Bf().c_str());
_ui->doubleSpinBox_OdomORBSLAM2ThDepth->setObjectName(Parameters::kOdomORBSLAM2ThDepth().c_str());
//Stereo //Stereo
_ui->stereo_winWidth->setObjectName(Parameters::kStereoWinWidth().c_str()); _ui->stereo_winWidth->setObjectName(Parameters::kStereoWinWidth().c_str());
_ui->stereo_winHeight->setObjectName(Parameters::kStereoWinHeight().c_str()); _ui->stereo_winHeight->setObjectName(Parameters::kStereoWinHeight().c_str());
@@ -3885,11 +3894,14 @@ void PreferencesDialog::setupKpRoiPanel()
void PreferencesDialog::updateOdometryVisibility() void PreferencesDialog::updateOdometryVisibility()
{ {
UASSERT(_ui->odom_strategy->count() == 6);
_ui->groupBox_odomF2M->setVisible(_ui->odom_strategy->currentIndex()==0); _ui->groupBox_odomF2M->setVisible(_ui->odom_strategy->currentIndex()==0);
_ui->groupBox_odomF2F->setVisible(_ui->odom_strategy->currentIndex()==1); _ui->groupBox_odomF2F->setVisible(_ui->odom_strategy->currentIndex()==1);
_ui->groupBox_odomFovis->setVisible(_ui->odom_strategy->currentIndex()==2); _ui->groupBox_odomFovis->setVisible(_ui->odom_strategy->currentIndex()==2);
_ui->groupBox_odomViso2->setVisible(_ui->odom_strategy->currentIndex()==3); _ui->groupBox_odomViso2->setVisible(_ui->odom_strategy->currentIndex()==3);
_ui->groupBox_odomMono->setVisible(_ui->odom_strategy->currentIndex()==4); _ui->groupBox_odomDVO->setVisible(_ui->odom_strategy->currentIndex()==4);
_ui->groupBox_odomORBSLAM2->setVisible(_ui->odom_strategy->currentIndex()==5);
_ui->groupBox_odomMono->setVisible(_ui->odom_strategy->currentIndex()==6);
} }
void PreferencesDialog::updateKpROI() void PreferencesDialog::updateKpROI()
@@ -3986,6 +3998,23 @@ void PreferencesDialog::changeDictionaryPath()
} }
} }
void PreferencesDialog::changeOdometryORBSLAM2Vocabulary()
{
QString path;
if(_ui->lineEdit_OdomORBSLAM2VocPath->text().isEmpty())
{
path = QFileDialog::getOpenFileName(this, tr("ORBSLAM2 Vocabulary"), this->getWorkingDirectory());
}
else
{
path = QFileDialog::getOpenFileName(this, tr("ORBSLAM2 Vocabulary"), _ui->lineEdit_OdomORBSLAM2VocPath->text());
}
if(!path.isEmpty())
{
_ui->lineEdit_OdomORBSLAM2VocPath->setText(path);
}
}
void PreferencesDialog::updateSourceGrpVisibility() void PreferencesDialog::updateSourceGrpVisibility()
{ {
_ui->groupBox_sourceRGBD->setVisible(_ui->comboBox_sourceType->currentIndex() == 0); _ui->groupBox_sourceRGBD->setVisible(_ui->comboBox_sourceType->currentIndex() == 0);

988
guilib/src/ui/aboutDialog.ui
View File

File diff suppressed because it is too large Load Diff

209
guilib/src/ui/preferencesDialog.ui
View File

@@ -63,7 +63,7 @@
<property name="geometry"> <property name="geometry">
<rect> <rect>
<x>0</x> <x>0</x>
<y>-34</y> <y>-637</y>
<width>678</width> <width>678</width>
<height>2739</height> <height>2739</height>
</rect> </rect>
@@ -9714,6 +9714,11 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
<string>DVO</string> <string>DVO</string>
</property> </property>
</item> </item>
<item>
<property name="text">
<string>ORB SLAM 2</string>
</property>
</item>
</widget> </widget>
</item> </item>
<item row="2" column="1"> <item row="2" column="1">
@@ -9985,7 +9990,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
<item> <item>
<widget class="QStackedWidget" name="stackedWidget_odometryType"> <widget class="QStackedWidget" name="stackedWidget_odometryType">
<property name="currentIndex"> <property name="currentIndex">
<number>4</number> <number>5</number>
</property> </property>
<widget class="QWidget" name="page_52"> <widget class="QWidget" name="page_52">
<layout class="QVBoxLayout" name="verticalLayout_77"> <layout class="QVBoxLayout" name="verticalLayout_77">
@@ -11512,7 +11517,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
<widget class="QWidget" name="page_69"> <widget class="QWidget" name="page_69">
<layout class="QVBoxLayout" name="verticalLayout_122"> <layout class="QVBoxLayout" name="verticalLayout_122">
<item> <item>
<widget class="QGroupBox" name="groupBox_27"> <widget class="QGroupBox" name="groupBox_odomDVO">
<property name="title"> <property name="title">
<string>DVO</string> <string>DVO</string>
</property> </property>
@@ -11551,6 +11556,145 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</item> </item>
</layout> </layout>
</widget> </widget>
<widget class="QWidget" name="page_70">
<layout class="QVBoxLayout" name="verticalLayout_125">
<item>
<widget class="QGroupBox" name="groupBox_odomORBSLAM2">
<property name="title">
<string>ORB SLAM 2</string>
</property>
<layout class="QVBoxLayout" name="verticalLayout_124">
<item>
<widget class="QLabel" name="label_412">
<property name="text">
<string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;ORB SLAM 2: &lt;a href=&quot;https://github.com/raulmur/ORB_SLAM2&quot;&gt;&lt;span style=&quot; text-decoration: underline; color:#0000ff;&quot;&gt;https://github.com/raulmur/ORB_SLAM2&lt;/span&gt;&lt;/a&gt;&lt;/p&gt;&lt;p&gt;Tracking and local mapping from ORB SLAM 2 is done but loop closure detection is disabled, using RTAB-Map loop closure detection instead. See ORB panel for ORB parameters (pyramid scale and number of pyramids). See FAST panel for FAST parameters (init and min thresholds). Vis/MaxFeatures (see Visual Registration panel) is used as the number of ORB features extracted.&lt;br/&gt;&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="openExternalLinks">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item>
<layout class="QGridLayout" name="gridLayout_93" columnstretch="0,0,1">
<item row="1" column="1">
<widget class="QDoubleSpinBox" name="doubleSpinBox_OdomORBSLAM2Bf">
<property name="suffix">
<string> m</string>
</property>
<property name="decimals">
<number>4</number>
</property>
<property name="minimum">
<double>0.000000000000000</double>
</property>
<property name="maximum">
<double>9.999900000000000</double>
</property>
<property name="singleStep">
<double>0.010000000000000</double>
</property>
<property name="value">
<double>0.076000000000000</double>
</property>
</widget>
</item>
<item row="0" column="2">
<widget class="QLabel" name="label_415">
<property name="text">
<string>Path to ORB vocabulary (*.txt).</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="0" column="1">
<widget class="QLineEdit" name="lineEdit_OdomORBSLAM2VocPath"/>
</item>
<item row="2" column="2">
<widget class="QLabel" name="label_414">
<property name="text">
<string>Close/Far threshold. Baseline times.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="2" column="1">
<widget class="QDoubleSpinBox" name="doubleSpinBox_OdomORBSLAM2ThDepth">
<property name="suffix">
<string/>
</property>
<property name="decimals">
<number>1</number>
</property>
<property name="minimum">
<double>0.100000000000000</double>
</property>
<property name="maximum">
<double>999.000000000000000</double>
</property>
<property name="singleStep">
<double>1.000000000000000</double>
</property>
<property name="value">
<double>40.000000000000000</double>
</property>
</widget>
</item>
<item row="1" column="2">
<widget class="QLabel" name="label_413">
<property name="text">
<string>Fake IR projector baseline used only when stereo is not used.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="0" column="0">
<widget class="QToolButton" name="toolButton_OdomORBSLAM2VocPath">
<property name="text">
<string>...</string>
</property>
</widget>
</item>
</layout>
</item>
</layout>
</widget>
</item>
<item>
<spacer name="verticalSpacer_64">
<property name="orientation">
<enum>Qt::Vertical</enum>
</property>
<property name="sizeHint" stdset="0">
<size>
<width>20</width>
<height>40</height>
</size>
</property>
</spacer>
</item>
</layout>
</widget>
<widget class="QWidget" name="page_26"> <widget class="QWidget" name="page_26">
<layout class="QVBoxLayout" name="verticalLayout_88"> <layout class="QVBoxLayout" name="verticalLayout_88">
<item> <item>
@@ -14933,10 +15077,7 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
</property> </property>
<layout class="QVBoxLayout" name="verticalLayout"> <layout class="QVBoxLayout" name="verticalLayout">
<item> <item>
<layout class="QFormLayout" name="formLayout_2"> <layout class="QGridLayout" name="gridLayout_94" columnstretch="0,1">
<property name="fieldGrowthPolicy">
<enum>QFormLayout::AllNonFixedFieldsGrow</enum>
</property>
<item row="0" column="0"> <item row="0" column="0">
<widget class="QSpinBox" name="fastThreshold"> <widget class="QSpinBox" name="fastThreshold">
<property name="minimum"> <property name="minimum">
@@ -14964,6 +15105,32 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
</widget> </widget>
</item> </item>
<item row="1" column="0"> <item row="1" column="0">
<widget class="QSpinBox" name="fastThresholdMin">
<property name="minimum">
<number>1</number>
</property>
<property name="maximum">
<number>9999</number>
</property>
<property name="value">
<number>50</number>
</property>
</widget>
</item>
<item row="1" column="1">
<widget class="QLabel" name="label_111">
<property name="text">
<string>Minimum threshold. Used when FAST/GridRows and FAST/GridCols are set or if ORBSLAM2 odometry is used.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="2" column="0">
<widget class="QCheckBox" name="fastSuppressNonMax"> <widget class="QCheckBox" name="fastSuppressNonMax">
<property name="text"> <property name="text">
<string/> <string/>
@@ -14973,7 +15140,7 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
</property> </property>
</widget> </widget>
</item> </item>
<item row="1" column="1"> <item row="2" column="1">
<widget class="QLabel" name="label_69"> <widget class="QLabel" name="label_69">
<property name="text"> <property name="text">
<string>If true, non-maximum suppression is applied to detected corners (keypoints).</string> <string>If true, non-maximum suppression is applied to detected corners (keypoints).</string>
@@ -14994,32 +15161,6 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
<string>OpenCV 2</string> <string>OpenCV 2</string>
</property> </property>
<layout class="QGridLayout" name="gridLayout_71" columnstretch="0,1"> <layout class="QGridLayout" name="gridLayout_71" columnstretch="0,1">
<item row="0" column="0">
<widget class="QSpinBox" name="fastThresholdMin">
<property name="minimum">
<number>1</number>
</property>
<property name="maximum">
<number>9999</number>
</property>
<property name="value">
<number>50</number>
</property>
</widget>
</item>
<item row="0" column="1">
<widget class="QLabel" name="label_111">
<property name="text">
<string>Minimum threshold. Used only when FAST/GridRows and FAST/GridCols are set.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="1" column="0"> <item row="1" column="0">
<widget class="QSpinBox" name="fastThresholdMax"> <widget class="QSpinBox" name="fastThresholdMax">
<property name="minimum"> <property name="minimum">