diff --git a/.appveyor.yml b/.appveyor.yml index a77f758a7e..9f19d0ae55 100644 --- a/.appveyor.yml +++ b/.appveyor.yml @@ -17,6 +17,9 @@ init: - call "C:\Program Files (x86)\Microsoft Visual Studio 14.0\VC\vcvarsall.bat" x86_amd64 install: + # To download from google drive + - set PATH=C:\Python38-x64;C:\Python38-x64\Scripts;%PATH% + - ps: py -m pip --disable-pip-version-check install gdown # Qt - set QTDIR=C:\Qt\5.10.1\msvc2015_64 # make sure Qt bin path is before cmake bin path to avoid copying qt5 dlls from cmake before qt installation @@ -73,7 +76,7 @@ install: - ps: "ls \"C:/Program Files/PCL\"" - set PATH=%PATH%;C:\Program Files\PCL\bin # zlib - - ps: wget 'https://docs.google.com/uc?authuser=0&id=0B46akLGdg-uaYm9MTTI4MUtUcmc&export=download' -outfile zlib-1.2.8-vc2010-x64.zip + - ps: gdown -q 0B46akLGdg-uaYm9MTTI4MUtUcmc - ps: Expand-Archive zlib-1.2.8-vc2010-x64.zip -DestinationPath 'C:\Program Files' - ECHO "Installed zlib:" - ps: "ls \"C:/Program Files/zlib\"" diff --git a/.devcontainer/devcontainer.json b/.devcontainer/devcontainer.json new file mode 100644 index 0000000000..476d9718b7 --- /dev/null +++ b/.devcontainer/devcontainer.json @@ -0,0 +1,8 @@ +{ + "image": "introlab3it/rtabmap:20.04", + "customizations": { + "vscode": { + "extensions": ["ms-vscode.cpptools-themes", "ms-vscode.cmake-tools", "vscjava.vscode-java-pack"] + } + } +} diff --git a/.github/workflows/cmake-ros.yml b/.github/workflows/cmake-ros.yml index 4e29e0ef4f..c300841cf0 100644 --- a/.github/workflows/cmake-ros.yml +++ b/.github/workflows/cmake-ros.yml @@ -2,10 +2,10 @@ name: CMake-ROS on: push: - branches: - - '**' + branches: + - master pull_request: - branches: + branches: - '**' env: @@ -22,33 +22,29 @@ jobs: runs-on: ${{ matrix.os }} strategy: matrix: - ros_distribution: [melodic, noetic, foxy, galactic, humble, rolling] + ros_distribution: [ noetic, humble, iron, rolling] include: - - ros_distribution: 'melodic' - os: ubuntu-18.04 - ros_distribution: 'noetic' os: ubuntu-20.04 - - ros_distribution: 'foxy' - os: ubuntu-20.04 - - ros_distribution: 'galactic' - os: ubuntu-20.04 - ros_distribution: 'humble' os: ubuntu-22.04 + - ros_distribution: 'iron' + os: ubuntu-22.04 - ros_distribution: 'rolling' os: ubuntu-22.04 - steps: - - uses: ros-tooling/setup-ros@v0.4 + steps: + - uses: ros-tooling/setup-ros@v0.6 with: required-ros-distributions: ${{ matrix.ros_distribution }} + - uses: actions/checkout@v2 + - name: Install dependencies run: | - sudo apt-get update - sudo apt-get -y install ros-${{ matrix.ros_distribution }}-rtabmap - sudo apt-get -y remove ros-${{ matrix.ros_distribution }}-rtabmap - - - uses: actions/checkout@v2 + source /opt/ros/${{ matrix.ros_distribution }}/setup.bash + rosdep update + rosdep install --from-paths ${{github.workspace}} -y - name: Configure CMake run: | diff --git a/.github/workflows/cmake.yml b/.github/workflows/cmake.yml index c24c3cff61..99e79ccd74 100644 --- a/.github/workflows/cmake.yml +++ b/.github/workflows/cmake.yml @@ -3,7 +3,7 @@ name: CMake on: push: branches: - - '**' + - master pull_request: branches: - '**' @@ -17,14 +17,14 @@ jobs: runs-on: ${{ matrix.os }} strategy: matrix: - os: [ubuntu-22.04, ubuntu-20.04, ubuntu-18.04] + os: [ubuntu-22.04, ubuntu-20.04] steps: - name: Install dependencies run: | DEBIAN_FRONTEND=noninteractive sudo apt-get update - sudo apt-get -y install libopencv-dev libpcl-dev git cmake software-properties-common + sudo apt-get -y install libopencv-dev libpcl-dev git cmake software-properties-common libyaml-cpp-dev - uses: actions/checkout@v2 diff --git a/.github/workflows/docker.yml b/.github/workflows/docker.yml index e6f65ec933..13a41ecba0 100644 --- a/.github/workflows/docker.yml +++ b/.github/workflows/docker.yml @@ -6,22 +6,72 @@ on: - 'master' jobs: - docker: + docker_deps: runs-on: ubuntu-latest strategy: matrix: - docker_tag: [xenial, bionic, focal, android23, android24, android26, android30] + docker_tag: [focal-deps, jammy-deps, jammy-iron-deps] include: - - docker_tag: xenial + - docker_tag: focal-deps docker_tags: | - introlab3it/rtabmap:xenial - introlab3it/rtabmap:16.04 - docker_args: | - NOT_USED=0 + introlab3it/rtabmap:focal-deps + docker_platforms: | + linux/amd64 + linux/arm64 + docker_path: 'focal/deps' + - docker_tag: jammy-deps + docker_tags: | + introlab3it/rtabmap:jammy-deps docker_platforms: | linux/amd64 - docker_path: 'xenial' + linux/arm64 + docker_path: 'jammy/deps' + - docker_tag: jammy-iron-deps + docker_tags: | + introlab3it/rtabmap:jammy-iron-deps + docker_platforms: | + linux/amd64 + docker_path: 'jammy-iron/deps' + + steps: + - + name: Checkout + uses: actions/checkout@v2 + - + name: Set up QEMU + uses: docker/setup-qemu-action@v1 + with: + platforms: all + - + name: Set up Docker Buildx + uses: docker/setup-buildx-action@v1 + - + name: Login to DockerHub + uses: docker/login-action@v1 + with: + username: ${{ secrets.DOCKERHUB_USERNAME }} + password: ${{ secrets.DOCKERHUB_TOKEN }} + - + name: Build and push + uses: docker/build-push-action@v2 + with: + context: . + push: true + platforms: ${{ matrix.docker_platforms }} + file: ./docker/${{ matrix.docker_path }}/Dockerfile + tags: ${{ matrix.docker_tags }} + cache-from: type=registry,ref=introlab3it/rtabmap:${{ matrix.docker_tag }} + cache-to: type=inline + + docker: + needs: docker_deps + runs-on: ubuntu-latest + + strategy: + matrix: + docker_tag: [bionic, focal, jammy, jammy-iron, android23, android24, android26, android30] + include: - docker_tag: bionic docker_tags: | introlab3it/rtabmap:bionic @@ -43,6 +93,24 @@ jobs: linux/amd64 linux/arm64 docker_path: 'focal' + - docker_tag: jammy + docker_tags: | + introlab3it/rtabmap:jammy + introlab3it/rtabmap:22.04 + docker_args: | + NOT_USED=0 + docker_platforms: | + linux/amd64 + linux/arm64 + docker_path: 'jammy' + - docker_tag: jammy-iron + docker_tags: | + introlab3it/rtabmap:jammy-iron + docker_args: | + NOT_USED=0 + docker_platforms: | + linux/amd64 + docker_path: 'jammy-iron' - docker_tag: android23 docker_tags: | introlab3it/rtabmap:android23 diff --git a/.gitignore b/.gitignore index 797bb421b5..5b6152a911 100644 --- a/.gitignore +++ b/.gitignore @@ -10,3 +10,5 @@ app/android/AndroidManifest.xml app/android/res/raw/ compile_flags.txt tags +build_* +*.bak diff --git a/CMakeLists.txt b/CMakeLists.txt index b58dd88c33..3fdc00062a 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,5 +1,5 @@ # Top-Level CmakeLists.txt -cmake_minimum_required(VERSION 3.5) +cmake_minimum_required(VERSION 3.10) PROJECT( RTABMap ) SET(PROJECT_PREFIX rtabmap) @@ -19,8 +19,8 @@ SET(CMAKE_MODULE_PATH "${PROJECT_SOURCE_DIR}/cmake_modules") # VERSION ####################### SET(RTABMAP_MAJOR_VERSION 0) -SET(RTABMAP_MINOR_VERSION 20) -SET(RTABMAP_PATCH_VERSION 22) +SET(RTABMAP_MINOR_VERSION 21) +SET(RTABMAP_PATCH_VERSION 2) SET(RTABMAP_VERSION ${RTABMAP_MAJOR_VERSION}.${RTABMAP_MINOR_VERSION}.${RTABMAP_PATCH_VERSION}) @@ -69,7 +69,11 @@ if(POLICY CMP0020) cmake_policy(SET CMP0020 NEW) endif() if(POLICY CMP0043) - cmake_policy(SET CMP0043 OLD) + cmake_policy(SET CMP0043 NEW) +endif() +# To suppress g2o related opengl warning +if(POLICY CMP0072) + cmake_policy(SET CMP0072 NEW) endif() IF(MINGW) @@ -106,6 +110,7 @@ if(MSVC) SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} /MP${N}") endif() endif() + add_compile_options("/bigobj") endif() # [Eclipse] Automatic Discovery of Include directories (Optional, but handy) @@ -145,7 +150,7 @@ endif() set(INSTALL_CMAKE_DIR ${DEF_INSTALL_CMAKE_DIR}) ####### BUILD OPTIONS ####### - +include (GenerateExportHeader) # ANDROID_PREBUILD (early exit if true) OPTION( ANDROID_PREBUILD "Set to ON to build rtabmap resource build tool (required for android build)" OFF ) IF(ANDROID_PREBUILD) @@ -182,6 +187,7 @@ option(WITH_G2O "Include g2o support" ON) option(WITH_GTSAM "Include GTSAM support" ON) option(WITH_TORO "Include TORO support" ON) option(WITH_CERES "Include Ceres support" OFF) +option(WITH_MRPT "Include MRPT support" ON) option(WITH_VERTIGO "Include Vertigo support" ON) option(WITH_CVSBA "Include cvsba support" OFF) option(WITH_POINTMATCHER "Include libpointmatcher support" ON) @@ -212,7 +218,7 @@ option(WITH_OPENVINS "Include OpenVINS support" OFF) option(WITH_MADGWICK "Include Madgwick IMU filtering support" ON) option(WITH_FASTCV "Include FastCV support" ON) option(WITH_OPENMP "Include OpenMP support" ON) -option(WITH_OPENGV "Include OpenGV support" OFF) +option(WITH_OPENGV "Include OpenGV support" ON) IF(MOBILE_BUILD) option(PCL_OMP "With PCL OMP implementations" OFF) ELSE() @@ -220,9 +226,9 @@ option(PCL_OMP "With PCL OMP implementations" ON) ENDIF() set(RTABMAP_QT_VERSION AUTO CACHE STRING "Force a specific Qt version.") -set_property(CACHE RTABMAP_QT_VERSION PROPERTY STRINGS AUTO 4 5) +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 OPTIONAL_COMPONENTS aruco xfeatures2d nonfree gpu cudafeatures2d) +FIND_PACKAGE(OpenCV REQUIRED 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) @@ -291,35 +297,55 @@ IF(WITH_QT) ENDIF(NOT VTK_FOUND) # If Qt is here, the GUI will be built - # look for Qt5 (if vtk>5 is installed) before Qt4 - IF("${VTK_MAJOR_VERSION}" GREATER 5) - if(RTABMAP_QT_VERSION STREQUAL "AUTO" OR RTABMAP_QT_VERSION STREQUAL "5") - FIND_PACKAGE(Qt5 COMPONENTS Widgets Core Gui QUIET) - IF(Qt5_FOUND) - FIND_PACKAGE(Qt5 COMPONENTS Widgets Core Gui OPTIONAL_COMPONENTS Svg) - ENDIF(Qt5_FOUND) + IF(NOT(${VTK_MAJOR_VERSION} LESS 9)) + IF(NOT VTK_QT_VERSION) + MESSAGE(FATAL_ERROR "WITH_QT option is ON, but VTK ${VTK_MAJOR_VERSION} has not been built with Qt support, disabling Qt.") + ENDIF() + MESSAGE(STATUS "VTK>=9 detected, will use VTK_QT_VERSION=${VTK_QT_VERSION} for Qt version.") + IF(${VTK_QT_VERSION} EQUAL 6) + FIND_PACKAGE(Qt6 COMPONENTS Widgets Core Gui OpenGL PrintSupport QUIET OPTIONAL_COMPONENTS Svg) + ELSEIF(${VTK_QT_VERSION} EQUAL 5) + FIND_PACKAGE(Qt5 COMPONENTS Widgets Core Gui OpenGL PrintSupport QUIET OPTIONAL_COMPONENTS Svg) + ELSE() + FIND_PACKAGE(Qt4 COMPONENTS QtCore QtGui OPTIONAL_COMPONENTS QtSvg) + ENDIF() + ELSE() + # look for Qt5 (if vtk>5 is installed) before Qt4 + IF("${VTK_MAJOR_VERSION}" GREATER 5) + IF(RTABMAP_QT_VERSION STREQUAL "AUTO" OR RTABMAP_QT_VERSION STREQUAL "5") + FIND_PACKAGE(Qt5 COMPONENTS Widgets Core Gui OpenGL PrintSupport QUIET OPTIONAL_COMPONENTS Svg) ENDIF(RTABMAP_QT_VERSION STREQUAL "AUTO" OR RTABMAP_QT_VERSION STREQUAL "5") - ENDIF("${VTK_MAJOR_VERSION}" GREATER 5) + ENDIF("${VTK_MAJOR_VERSION}" GREATER 5) - IF(NOT Qt5_FOUND) + IF(NOT Qt5_FOUND) IF(RTABMAP_QT_VERSION STREQUAL "AUTO" OR RTABMAP_QT_VERSION STREQUAL "4") - FIND_PACKAGE(Qt4 COMPONENTS QtCore QtGui OPTIONAL_COMPONENTS QtSvg) + FIND_PACKAGE(Qt4 COMPONENTS QtCore QtGui OPTIONAL_COMPONENTS QtSvg) ENDIF(RTABMAP_QT_VERSION STREQUAL "AUTO" OR RTABMAP_QT_VERSION STREQUAL "4") - ENDIF(NOT Qt5_FOUND) + ENDIF(NOT Qt5_FOUND) + ENDIF() - IF(QT4_FOUND OR Qt5_FOUND) + IF(QT4_FOUND OR Qt5_FOUND OR Qt6_FOUND) + # For VCPKG build, set those global variables to off, + # we will enable them for jsut specific targets + set(CMAKE_AUTOMOC OFF) + set(CMAKE_AUTORCC OFF) + set(CMAKE_AUTOUIC OFF) IF("${VTK_MAJOR_VERSION}" EQUAL 5) FIND_PACKAGE(QVTK REQUIRED) # only for VTK 5 ELSE() - list(FIND PCL_LIBRARIES VTK::GUISupportQt value) - IF(value EQUAL -1) + list(FIND PCL_LIBRARIES VTK::GUISupportQt value) + IF(value EQUAL -1) list(FIND PCL_LIBRARIES vtkGUISupportQt value) IF(value EQUAL -1) - SET(PCL_LIBRARIES "${PCL_LIBRARIES};vtkGUISupportQt") + IF(NOT(${VTK_MAJOR_VERSION} LESS 9)) + SET(PCL_LIBRARIES "${PCL_LIBRARIES};VTK::GUISupportQt") + ELSE() + SET(PCL_LIBRARIES "${PCL_LIBRARIES};vtkGUISupportQt") + ENDIF() SET(ADD_VTK_GUI_SUPPORT_QT_TO_CONF TRUE) ENDIF(value EQUAL -1) - ENDIF(value EQUAL -1) - + ENDIF(value EQUAL -1) + MESSAGE(STATUS "VTK_RENDERING_BACKEND=${VTK_RENDERING_BACKEND}") IF(VTK_RENDERING_BACKEND STREQUAL "OpenGL2") @@ -337,17 +363,17 @@ IF(WITH_QT) ENDIF(value EQUAL -1) ELSEIF("${VTK_MAJOR_VERSION}" EQUAL 9) list(FIND PCL_LIBRARIES VTK::RenderingOpenGL2 value) - IF(NOT value EQUAL -1) - list(FIND PCL_LIBRARIES VTK::RenderingVolumeOpenGL2 value) + IF(NOT value EQUAL -1) + list(FIND PCL_LIBRARIES VTK::RenderingVolumeOpenGL2 value) IF(value EQUAL -1) SET(PCL_LIBRARIES "${PCL_LIBRARIES};VTK::RenderingVolumeOpenGL2") ENDIF(value EQUAL -1) - ENDIF(NOT value EQUAL -1) + ENDIF(NOT value EQUAL -1) ENDIF() ENDIF() ADD_DEFINITIONS(-DQT_NO_KEYWORDS) # To avoid conflicts with boost signals/foreach and Qt macros - ENDIF(QT4_FOUND OR Qt5_FOUND) + ENDIF(QT4_FOUND OR Qt5_FOUND OR Qt6_FOUND) ENDIF(WITH_QT) IF(NOT VTK_FOUND) @@ -368,6 +394,7 @@ IF(WITH_PYTHON) FIND_PACKAGE(Python3 COMPONENTS Interpreter Development NumPy) IF(Python3_FOUND) MESSAGE(STATUS "Found Python3") + FIND_PACKAGE(pybind11 REQUIRED) ENDIF(Python3_FOUND) ENDIF(WITH_PYTHON) @@ -405,7 +432,7 @@ ENDIF(WITH_FREENECT2) IF(WITH_K4W2 AND WIN32) FIND_PACKAGE(KinectSDK2 QUIET) - IF(KinectSDK2_FOUND) + IF(KinectSDK2_FOUND) MESSAGE(STATUS "Found Kinect for Windows 2: ${KinectSDK2_INCLUDE_DIRS}") ENDIF(KinectSDK2_FOUND) ENDIF(WITH_K4W2 AND WIN32) @@ -420,7 +447,7 @@ IF(WITH_K4A) SET(k4a_FOUND FALSE) ENDIF(NOT (k4a_FOUND AND k4arecord_FOUND)) ENDIF() - IF(k4a_FOUND) + IF(k4a_FOUND) MESSAGE(STATUS "Found Kinect for Azure: ${k4a_INCLUDE_DIRS}") ENDIF(k4a_FOUND) ENDIF(WITH_K4A) @@ -441,13 +468,13 @@ IF(WITH_DC1394) ENDIF(WITH_DC1394) IF(WITH_G2O) - FIND_PACKAGE(g2o QUIET NO_MODULE) + FIND_PACKAGE(g2o NO_MODULE) IF(g2o_FOUND) MESSAGE(STATUS "Found g2o (targets)") - SET(G2O_FOUND ${g2o_FOUND}) - get_target_property(G2O_INCLUDES g2o::core INTERFACE_INCLUDE_DIRECTORIES) - MESSAGE(STATUS "g2o include dir: ${G2O_INCLUDES}") - FIND_FILE(G2O_FACTORY_FILE g2o/core/factory.h + SET(G2O_FOUND ${g2o_FOUND}) + get_target_property(G2O_INCLUDES g2o::core INTERFACE_INCLUDE_DIRECTORIES) + MESSAGE(STATUS "g2o include dir: ${G2O_INCLUDES}") + FIND_FILE(G2O_FACTORY_FILE g2o/core/factory.h PATHS ${G2O_INCLUDES} NO_DEFAULT_PATH) FILE(READ ${G2O_FACTORY_FILE} TMPTXT) @@ -472,6 +499,14 @@ IF(WITH_GTSAM) FIND_PACKAGE(GTSAM CONFIG QUIET) ENDIF(WITH_GTSAM) +IF(WITH_MRPT) + FIND_PACKAGE(MRPT COMPONENTS poses QUIET) + IF(MRPT_FOUND) + message(STATUS "MRPT_VERSION: ${MRPT_VERSION}") + message(STATUS "MRPT_LIBRARIES: ${MRPT_LIBRARIES}") + ENDIF(MRPT_FOUND) +ENDIF(WITH_MRPT) + IF(WITH_FLYCAPTURE2) FIND_PACKAGE(FlyCapture2 QUIET) IF(FlyCapture2_FOUND) @@ -489,7 +524,14 @@ ENDIF(WITH_CVSBA) IF(WITH_POINTMATCHER) find_package(libpointmatcher QUIET) IF(libpointmatcher_FOUND) - MESSAGE(STATUS "Found libpointmatcher: ${libpointmatcher_INCLUDE_DIRS}") + MESSAGE(STATUS "Found libpointmatcher: ${libpointmatcher_INCLUDE_DIRS}") + string(FIND "${libpointmatcher_LIBRARIES}" "libnabo" value) + IF(value EQUAL -1) + # Find libnabo (Issue #1117): + find_package(libnabo REQUIRED PATHS ${LIBNABO_INSTALL_DIR}) + message(STATUS "libnabo found, version ${libnabo_VERSION} (Config mode)") + SET(libpointmatcher_LIBRARIES "${libpointmatcher_LIBRARIES};libnabo::nabo") + ENDIF(value EQUAL -1) ENDIF(libpointmatcher_FOUND) ENDIF(WITH_POINTMATCHER) @@ -499,8 +541,8 @@ IF(libpointmatcher_FOUND OR GTSAM_FOUND) find_package(Boost COMPONENTS thread filesystem system program_options date_time chrono timer serialization REQUIRED) ENDIF(Boost_MINOR_VERSION GREATER 47) IF(WIN32) - MESSAGE(STATUS "Boost_LIBRARY_DIRS=${Boost_LIBRARY_DIRS}") - link_directories(${Boost_LIBRARY_DIRS}) + MESSAGE(STATUS "Boost_LIBRARY_DIRS=${Boost_LIBRARY_DIRS}") + link_directories(${Boost_LIBRARY_DIRS}) ENDIF(WIN32) ENDIF(libpointmatcher_FOUND OR GTSAM_FOUND) @@ -737,51 +779,63 @@ IF(WITH_ORB_SLAM AND NOT G2O_FOUND) ENDIF(WITH_ORB_SLAM AND NOT G2O_FOUND) IF(NOT MSVC) - IF((NOT WITH_MSCKF_VIO OR NOT msckf_vio_FOUND) AND (loam_velodyne_FOUND OR floam_FOUND OR PCL_VERSION VERSION_GREATER "1.9.1" OR TORCH_FOUND OR G2O_FOUND OR CCCoreLib_FOUND OR Open3D_FOUND)) - #LOAM, PCL>=1.10, latest g2o and CCCoreLib require c++14, but MSCKF_VIO requires c++11 - include(CheckCXXCompilerFlag) - CHECK_CXX_COMPILER_FLAG("-std=c++14" COMPILER_SUPPORTS_CXX14) - IF(COMPILER_SUPPORTS_CXX14) - set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++14") - set(CMAKE_CXX_STANDARD 14) - ELSE() - message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has no C++14 support. Please use a different C++ compiler if you want to use LOAM, latest PCL or g2o.") - ENDIF() - ENDIF() - - IF( (NOT (${CMAKE_CXX_STANDARD} STREQUAL "14")) AND ( - G2O_FOUND OR - GTSAM_FOUND OR - CERES_FOUND OR - ZED_FOUND OR - ZEDOC_FOUND OR - ANDROID OR - RealSense_FOUND OR - realsense2_FOUND OR - ORB_SLAM_FOUND OR - okvis_FOUND OR - open_chisel_FOUND OR - msckf_vio_FOUND OR - vins_FOUND OR - ov_msckf_FOUND OR - libpointmatcher_FOUND)) - #Newest versions require std11 - include(CheckCXXCompilerFlag) - CHECK_CXX_COMPILER_FLAG("-std=c++11" COMPILER_SUPPORTS_CXX11) - CHECK_CXX_COMPILER_FLAG("-std=c++0x" COMPILER_SUPPORTS_CXX0X) - IF(COMPILER_SUPPORTS_CXX11) - set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") - ELSEIF(COMPILER_SUPPORTS_CXX0X) - set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x") - ELSE() - message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has no C++11 support. Please use a different C++ compiler.") - ENDIF() - ENDIF() + IF(Qt6_FOUND OR (G2O_FOUND AND G2O_CPP11 EQUAL 1) OR TORCH_FOUND) + # Qt6 requires c++17 + include(CheckCXXCompilerFlag) + CHECK_CXX_COMPILER_FLAG("-std=c++17" COMPILER_SUPPORTS_CXX17) + IF(COMPILER_SUPPORTS_CXX17) + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++17") + set(CMAKE_CXX_STANDARD 17) + ELSE() + message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has no C++17 support. Please use a different C++ compiler if you want to use Qt6.") + ENDIF() + ENDIF() + IF((NOT (${CMAKE_CXX_STANDARD} STREQUAL "17")) AND (msckf_vio_FOUND OR loam_velodyne_FOUND OR floam_FOUND OR PCL_VERSION VERSION_GREATER "1.9.1" OR G2O_FOUND OR CCCoreLib_FOUND OR Open3D_FOUND)) + #MSCKF_VIO, LOAM, PCL>=1.10, latest g2o and CCCoreLib require c++14 + include(CheckCXXCompilerFlag) + CHECK_CXX_COMPILER_FLAG("-std=c++14" COMPILER_SUPPORTS_CXX14) + IF(COMPILER_SUPPORTS_CXX14) + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++14") + set(CMAKE_CXX_STANDARD 14) + ELSE() + message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has no C++14 support. Please use a different C++ compiler if you want to use LOAM, latest PCL or g2o.") + ENDIF() + ENDIF() + + IF( (NOT (${CMAKE_CXX_STANDARD} STREQUAL "17") AND NOT (${CMAKE_CXX_STANDARD} STREQUAL "14")) AND ( + G2O_FOUND OR + GTSAM_FOUND OR + CERES_FOUND OR + ZED_FOUND OR + ZEDOC_FOUND OR + ANDROID OR + RealSense_FOUND OR + realsense2_FOUND OR + ORB_SLAM_FOUND OR + okvis_FOUND OR + open_chisel_FOUND OR + vins_FOUND OR + ov_msckf_FOUND OR + msckf_vio_FOUND OR + libpointmatcher_FOUND)) + #Newest versions require std11 + include(CheckCXXCompilerFlag) + CHECK_CXX_COMPILER_FLAG("-std=c++11" COMPILER_SUPPORTS_CXX11) + CHECK_CXX_COMPILER_FLAG("-std=c++0x" COMPILER_SUPPORTS_CXX0X) + IF(COMPILER_SUPPORTS_CXX11) + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") + ELSEIF(COMPILER_SUPPORTS_CXX0X) + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x") + ELSE() + message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has no C++11 support. Please use a different C++ compiler.") + ENDIF() + ENDIF() ENDIF() + ####### OSX BUNDLE CMAKE_INSTALL_PREFIX ####### IF(APPLE AND BUILD_AS_BUNDLE) - IF(Qt5_FOUND OR (QT4_FOUND AND QT_QTCORE_FOUND AND QT_QTGUI_FOUND)) + IF(Qt6_FOUND OR Qt5_FOUND OR (QT4_FOUND AND QT_QTCORE_FOUND AND QT_QTGUI_FOUND)) # Required when packaging, and set CMAKE_INSTALL_PREFIX to "/". SET(CMAKE_INSTALL_PREFIX "/") @@ -809,11 +863,6 @@ ENDIF(APPLE AND BUILD_AS_BUNDLE) ####### SOURCES (Projects) ####### -# CONF_DEPENDENCIES contains only dependencies not required by the headers -SET(CONF_DEPENDENCIES - ${ZLIB_LIBRARIES} -) - # OpenCV2 has nonfree if OPENCV_NONFREE_FOUND # OpenCV<=3.4.2 has nonfree if OPENCV_XFEATURES2D_FOUND # OpenCV>3.4.2 has nonfree if OPENCV_XFEATURES2D_FOUND and OPENCV_ENABLE_NONFREE is defined @@ -834,16 +883,16 @@ IF(NOT G2O_FOUND) SET(G2O "//") SET(G2O_CPP_CONF "//") ELSE() - SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${G2O_LIBRARIES}) IF(NOT G2O_CPP11) - SET(G2O_CPP_CONF "//") + SET(G2O_CPP_CONF "//") ENDIF(NOT G2O_CPP11) ENDIF() IF(NOT GTSAM_FOUND) SET(GTSAM "//") -ELSE() - SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${GTSAM_LIBRARIES}) ENDIF() +IF(NOT MRPT_FOUND) + SET(MRPT "//") +ENDIF(NOT MRPT_FOUND) IF(NOT CERES_FOUND) SET(CERES "//") ENDIF(NOT CERES_FOUND) @@ -855,8 +904,6 @@ IF(NOT WITH_VERTIGO) ENDIF(NOT WITH_VERTIGO) IF(NOT cvsba_FOUND) SET(CVSBA "//") -ELSE() - SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${cvsba_LIBRARIES}) ENDIF() IF(NOT libpointmatcher_FOUND) SET(POINTMATCHER "//") @@ -870,9 +917,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) @@ -884,129 +931,109 @@ IF(NOT floam_FOUND) ENDIF(NOT floam_FOUND) IF(NOT Freenect_FOUND) SET(FREENECT "//") -ELSE() - SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${Freenect_LIBRARIES}) ENDIF() IF(NOT freenect2_FOUND) SET(FREENECT2 "//") -ELSE() - SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${freenect2_LIBRARIES}) ENDIF() IF(NOT KinectSDK2_FOUND) SET(K4W2 "//") -ELSE() - SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${KinectSDK2_LIBRARIES}) ENDIF() IF(NOT k4a_FOUND) SET(K4A "//") -ELSE() - SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${k4a_LIBRARIES}) + SET(CONF_WITH_K4A 0) +ELSE() + SET(CONF_WITH_K4A 1) + IF(WIN32) + install( + FILES + "${CMAKE_CURRENT_SOURCE_DIR}/cmake_modules/FindK4A.cmake" + DESTINATION ${INSTALL_CMAKE_DIR}/Modules/. + COMPONENT devel + ) + ENDIF(WIN32) ENDIF() IF(NOT OpenNI2_FOUND) SET(OPENNI2 "//") -ELSE() - SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${OpenNI2_LIBRARIES}) ENDIF() IF(NOT DC1394_FOUND) SET(DC1394 "//") -ELSE() - SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${DC1394_LIBRARIES}) ENDIF() IF(NOT FlyCapture2_FOUND) SET(FLYCAPTURE2 "//") -ELSE() - SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${FlyCapture2_LIBRARIES}) ENDIF() IF(NOT ZED_FOUND) SET(ZED "//") -ELSE() - SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${ZED_LIBRARIES} ${CUDA_LIBRARIES}) ENDIF() IF(NOT ZEDOC_FOUND) SET(ZEDOC "//") -ELSE() - SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${ZEDOC_LIBRARIES}) ENDIF() IF(NOT RealSense_FOUND) SET(REALSENSE "//") -ELSE() - SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${RealSense_LIBRARIES}) ENDIF() IF(NOT RealSenseSlam_FOUND) SET(REALSENSESLAM "//") ENDIF(NOT RealSenseSlam_FOUND) IF(NOT realsense2_FOUND) SET(REALSENSE2 "//") -ELSE() - SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${realsense2_LIBRARIES}) + SET(CONF_WITH_REALSENSE2 0) +ELSE() + SET(CONF_WITH_REALSENSE2 1) + IF(WIN32) + install( + FILES + "${CMAKE_CURRENT_SOURCE_DIR}/cmake_modules/FindRealSense2.cmake" + DESTINATION ${INSTALL_CMAKE_DIR}/Modules/. + COMPONENT devel + ) + ENDIF(WIN32) ENDIF() IF(NOT mynteye_FOUND) SET(MYNTEYE "//") ENDIF(NOT mynteye_FOUND) IF(NOT depthai_FOUND) - SET(CONF_DEPTH_AI OFF) + SET(CONF_WITH_DEPTH_AI 0) SET(DEPTHAI "//") ELSE() - SET(CONF_DEPTH_AI ON) - SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} depthai::core depthai::opencv) + SET(CONF_WITH_DEPTH_AI 1) ENDIF() IF(NOT octomap_FOUND) SET(OCTOMAP "//") + SET(CONF_WITH_OCTOMAP 0) ELSE() - SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${OCTOMAP_LIBRARIES}) + SET(CONF_WITH_OCTOMAP 1) ENDIF() IF(NOT CPUTSDF_FOUND) SET(CPUTSDF "//") -ELSE() - SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${CPUTSDF_LIBRARIES}) ENDIF() IF(NOT open_chisel_FOUND) SET(OPENCHISEL "//") -ELSE() - SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${open_chisel_LIBRARIES}) ENDIF() IF(NOT AliceVision_FOUND) SET(ALICE_VISION "//") ENDIF(NOT AliceVision_FOUND) IF(NOT libfovis_FOUND) SET(FOVIS "//") -ELSE() - SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${libfovis_LIBRARIES}) ENDIF() IF(NOT libviso2_FOUND) SET(VISO2 "//") -ELSE() - SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${libviso2_LIBRARIES}) ENDIF() IF(NOT dvo_core_FOUND) SET(DVO "//") -ELSE() - SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${dvo_core_LIBRARIES}) ENDIF() IF(NOT okvis_FOUND) SET(OKVIS "//") -ELSE() - SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${OKVIS_LIBRARIES}) ENDIF() IF(NOT msckf_vio_FOUND) SET(MSCKF_VIO "//") -ELSE() - SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${msckf_vio_LIBRARIES}) ENDIF() IF(NOT vins_FOUND) SET(VINS "//") -ELSE() - SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${vins_LIBRARIES}) ENDIF() IF(NOT ov_msckf_FOUND) SET(OPENVINS "//") -ELSE() - SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${ov_msckf_LIBRARIES}) ENDIF() IF(NOT ORB_SLAM_FOUND) SET(ORB_SLAM "//") -ELSE() - SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${ORB_SLAM_LIBRARIES}) ENDIF() IF(NOT WITH_ORB_OCTREE) SET(ORB_OCTREE "//") @@ -1016,21 +1043,20 @@ IF(NOT TORCH_FOUND) ENDIF() IF(NOT WITH_PYTHON OR NOT Python3_FOUND) SET(PYTHON "//") + SET(CONF_WITH_PYTHON 0) +ELSE() + SET(CONF_WITH_PYTHON 1) ENDIF() IF(ADD_VTK_GUI_SUPPORT_QT_TO_CONF) SET(CONF_VTK_QT true) - SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} vtkGUISupportQt) ELSE() SET(CONF_VTK_QT false) ENDIF() -IF(VTK_USE_QVTK) - SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${QVTK_LIBRARY}) -ENDIF(VTK_USE_QVTK) IF(NOT WITH_MADGWICK) SET(MADGWICK "//") ENDIF() -CONFIGURE_FILE(Version.h.in ${CMAKE_CURRENT_BINARY_DIR}/corelib/include/${PROJECT_PREFIX}/core/Version.h) +CONFIGURE_FILE(Version.h.in ${CMAKE_CURRENT_BINARY_DIR}/corelib/src/include/${PROJECT_PREFIX}/core/Version.h) ADD_SUBDIRECTORY( utilite ) ADD_SUBDIRECTORY( corelib ) @@ -1039,7 +1065,7 @@ IF(ANDROID) IF(BUILD_APP) ADD_SUBDIRECTORY( app ) ENDIF(BUILD_APP) -ELSEIF(Qt5_FOUND OR (QT4_FOUND AND QT_QTCORE_FOUND AND QT_QTGUI_FOUND)) +ELSEIF(Qt6_FOUND OR Qt5_FOUND OR (QT4_FOUND AND QT_QTCORE_FOUND AND QT_QTGUI_FOUND)) ADD_SUBDIRECTORY( guilib ) IF(BUILD_APP) ADD_SUBDIRECTORY( app ) @@ -1067,55 +1093,84 @@ ADD_CUSTOM_TARGET(uninstall "${CMAKE_COMMAND}" -P "${CMAKE_CURRENT_BINARY_DIR}/cmake_uninstall.cmake") #### -# Setup RTABMapConfig.cmake +# Global Export Target #### -# Create the RTABMapConfig.cmake and RTABMapConfigVersion files -file(RELATIVE_PATH REL_INCLUDE_DIR "${CMAKE_INSTALL_PREFIX}/${INSTALL_CMAKE_DIR}" "${CMAKE_INSTALL_PREFIX}/${INSTALL_INCLUDE_DIR}") -file(RELATIVE_PATH REL_LIB_DIR "${CMAKE_INSTALL_PREFIX}/${INSTALL_CMAKE_DIR}" "${CMAKE_INSTALL_PREFIX}/${CMAKE_INSTALL_LIBDIR}") - -# ... for the build tree -set(CONF_INCLUDE_DIRS "${PROJECT_BINARY_DIR}/corelib/include" - "${PROJECT_SOURCE_DIR}/corelib/include" - "${PROJECT_SOURCE_DIR}/guilib/include" - "${PROJECT_SOURCE_DIR}/utilite/include") -set(CONF_LIB_DIR "${CMAKE_ARCHIVE_OUTPUT_DIRECTORY} ${CMAKE_RUNTIME_OUTPUT_DIRECTORY}") -IF(QT4_FOUND OR Qt5_FOUND) +add_library(rtabmap INTERFACE) +add_library(rtabmap::rtabmap ALIAS rtabmap) +IF(QT4_FOUND OR Qt5_FOUND OR Qt6_FOUND) set(CONF_WITH_GUI ON) IF(QT4_FOUND) set(CONF_QT_VERSION 4) - ELSE() + ELSEIF(Qt5_FOUND) set(CONF_QT_VERSION 5) + ELSE() + set(CONF_QT_VERSION 6) ENDIF() + target_link_libraries(rtabmap INTERFACE rtabmap_utilite rtabmap_core rtabmap_gui) ELSE() set(CONF_WITH_GUI OFF) + target_link_libraries(rtabmap INTERFACE rtabmap_utilite rtabmap_core) ENDIF() -configure_file(RTABMapConfig.cmake.in - "${PROJECT_BINARY_DIR}/RTABMapConfig.cmake" @ONLY) - -# ... for the install tree -set(CONF_INCLUDE_DIRS "\${RTABMap_CMAKE_DIR}/${REL_INCLUDE_DIR}") -set(CONF_LIB_DIR "\${RTABMap_CMAKE_DIR}/${REL_LIB_DIR}") -configure_file(RTABMapConfig.cmake.in - "${PROJECT_BINARY_DIR}${CMAKE_FILES_DIRECTORY}/RTABMapConfig.cmake" @ONLY) +install(TARGETS rtabmap EXPORT rtabmapTargets) +export(EXPORT rtabmapTargets + FILE "${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}Targets.cmake" + NAMESPACE rtabmap:: +) +install(EXPORT rtabmapTargets + FILE + ${PROJECT_NAME}Targets.cmake + DESTINATION + ${INSTALL_CMAKE_DIR} + NAMESPACE rtabmap:: + COMPONENT + devel +) + +#### +# Setup RTABMapConfig.cmake +#### +include(CMakePackageConfigHelpers) +write_basic_package_version_file( + "${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}ConfigVersion.cmake" + VERSION ${PROJECT_VERSION} + COMPATIBILITY AnyNewerVersion +) + +# Build tree: +SET(CONF_MODULES_DIR "../cmake_modules") +configure_file( + ${PROJECT_NAME}Config.cmake.in + "${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}Config.cmake" + @ONLY +) + +# Install tree: +SET(CONF_MODULES_DIR "Modules") +configure_file( + ${PROJECT_NAME}Config.cmake.in + "${CMAKE_CURRENT_BINARY_DIR}${CMAKE_FILES_DIRECTORY}/${PROJECT_NAME}Config.cmake" + @ONLY +) +install( + FILES + "${CMAKE_CURRENT_BINARY_DIR}${CMAKE_FILES_DIRECTORY}/${PROJECT_NAME}Config.cmake" + "${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}ConfigVersion.cmake" + DESTINATION + ${INSTALL_CMAKE_DIR} + COMPONENT + devel +) -# ... for both -configure_file(RTABMapConfigVersion.cmake.in - "${PROJECT_BINARY_DIR}/RTABMapConfigVersion.cmake" @ONLY) - -# Install the RTABMapConfig.cmake and RTABMapConfigVersion.cmake -install(FILES - "${PROJECT_BINARY_DIR}${CMAKE_FILES_DIRECTORY}/RTABMapConfig.cmake" - "${PROJECT_BINARY_DIR}/RTABMapConfigVersion.cmake" - DESTINATION "${INSTALL_CMAKE_DIR}" COMPONENT devel) #### ### Install package.xml for catkin -install(FILES package.xml DESTINATION "${CMAKE_INSTALL_DATAROOTDIR}/${PROJECT_PREFIX}") +install(FILES package.xml DESTINATION "${CMAKE_INSTALL_DATAROOTDIR}/${PROJECT_PREFIX}" COMPONENT devel) ####################### # CPACK (Packaging) ####################### IF(BUILD_AS_BUNDLE) + SET(CMAKE_INSTALL_SYSTEM_RUNTIME_COMPONENT runtime) INCLUDE(InstallRequiredSystemLibraries) ENDIF(BUILD_AS_BUNDLE) @@ -1132,6 +1187,7 @@ SET(CPACK_PACKAGE_VERSION_PATCH "${PROJECT_VERSION_PATCH}") #SET(CPACK_SOURCE_PACKAGE_FILE_NAME "${PROJECT_NAME}-${CPACK_PACKAGE_VERSION_MAJOR}.${CPACK_PACKAGE_VERSION_MINOR}.${CPACK_PACKAGE_VERSION_PATCH}") SET(CPACK_PACKAGE_CONTACT "matlabbe@gmail.com") + set(CPACK_SOURCE_IGNORE_FILES "\\\\.svn/" "\\\\.settings/" @@ -1259,6 +1315,9 @@ MESSAGE(STATUS " With VTK ${VTK_MAJOR_VERSION}.${VTK_MINOR_VERSION} ELSEIF(Qt5_FOUND) MESSAGE(STATUS " With Qt ${Qt5_VERSION} = YES (License: Open Source or Commercial)") MESSAGE(STATUS " With VTK ${VTK_MAJOR_VERSION}.${VTK_MINOR_VERSION} = YES (License: BSD)") +ELSEIF(Qt6_FOUND) +MESSAGE(STATUS " With Qt ${Qt6_VERSION} = YES (License: Open Source or Commercial)") +MESSAGE(STATUS " With VTK ${VTK_MAJOR_VERSION}.${VTK_MINOR_VERSION} = YES (License: BSD)") ELSEIF(NOT WITH_QT) MESSAGE(STATUS " With Qt = NO (WITH_QT=OFF)") @@ -1309,7 +1368,7 @@ MESSAGE(STATUS " With FastCV = NO (FastCV not found)") ENDIF() IF(PDAL_FOUND) -MESSAGE(STATUS " With PDAL = YES (License: BSD)") +MESSAGE(STATUS " With PDAL ${PDAL_VERSION} = YES (License: BSD)") ELSEIF(NOT WITH_PDAL) MESSAGE(STATUS " With PDAL = NO (WITH_PDAL=OFF)") ELSE() @@ -1325,7 +1384,7 @@ MESSAGE(STATUS " With TORO = NO (WITH_TORO=OFF)") ENDIF() IF(G2O_FOUND) -MESSAGE(STATUS " *With g2o = YES (License: BSD)") +MESSAGE(STATUS " *With g2o ${g2o_VERSION} = YES (License: BSD)") ELSEIF(NOT WITH_G2O) MESSAGE(STATUS " *With g2o = NO (WITH_G2O=OFF)") ELSE() @@ -1333,7 +1392,7 @@ MESSAGE(STATUS " *With g2o = NO (g2o not found)") ENDIF() IF(GTSAM_FOUND) -MESSAGE(STATUS " *With GTSAM = YES (License: BSD)") +MESSAGE(STATUS " *With GTSAM ${GTSAM_VERSION} = YES (License: BSD)") ELSEIF(NOT WITH_GTSAM) MESSAGE(STATUS " *With GTSAM = NO (WITH_GTSAM=OFF)") ELSE() @@ -1342,9 +1401,9 @@ ENDIF() IF(CERES_FOUND) IF(WITH_CERES) -MESSAGE(STATUS " *With Ceres ${Ceres_VERSION} = YES (License: BSD)") +MESSAGE(STATUS " *With Ceres ${Ceres_VERSION} = YES (License: BSD)") ELSE() -MESSAGE(STATUS " *With Ceres ${Ceres_VERSION} = YES (License: BSD, WITH_CERES=OFF but it is enabled by okvis or floam dependencies)") +MESSAGE(STATUS " *With Ceres ${Ceres_VERSION} = YES (License: BSD, WITH_CERES=OFF but it is enabled by okvis or floam dependencies)") ENDIF() ELSEIF(NOT WITH_CERES) MESSAGE(STATUS " *With Ceres = NO (WITH_CERES=OFF)") @@ -1352,6 +1411,14 @@ ELSE() MESSAGE(STATUS " *With Ceres = NO (Ceres not found)") ENDIF() +IF(MRPT_FOUND) +MESSAGE(STATUS " With MRPT ${MRPT_VERSION} = YES (License: BSD)") +ELSEIF(NOT WITH_MRPT) +MESSAGE(STATUS " With MRPT = NO (WITH_MRPT=OFF)") +ELSE() +MESSAGE(STATUS " With MRPT = NO (MRPT not found)") +ENDIF() + IF(G2O_FOUND OR GTSAM_FOUND) IF(WITH_VERTIGO) MESSAGE(STATUS " With VERTIGO = YES (License: GPLv3)") @@ -1371,7 +1438,7 @@ MESSAGE(STATUS " With cvsba = NO (cvsba not found)") ENDIF() IF(libpointmatcher_FOUND) -MESSAGE(STATUS " *With libpointmatcher = YES (License: BSD)") +MESSAGE(STATUS " *With libpointmatcher ${libpointmatcher_VERSION} = YES (License: BSD)") ELSEIF(NOT WITH_POINTMATCHER) MESSAGE(STATUS " *With libpointmatcher = NO (WITH_POINTMATCHER=OFF)") ELSE() @@ -1396,8 +1463,8 @@ ELSE() MESSAGE(STATUS " With Open3D = NO (Open3D not found)") ENDIF() -IF(opengv_FOUND) -MESSAGE(STATUS " With OpenGV = YES (License: BSD)") +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)") ELSE() @@ -1407,7 +1474,7 @@ ENDIF() MESSAGE(STATUS "") MESSAGE(STATUS " Reconstruction Approaches:") IF(octomap_FOUND) -MESSAGE(STATUS " With OCTOMAP = YES (License: BSD)") +MESSAGE(STATUS " With OCTOMAP ${octomap_VERSION} = YES (License: BSD)") ELSEIF(NOT WITH_OCTOMAP) MESSAGE(STATUS " With OCTOMAP = NO (WITH_OCTOMAP=OFF)") ELSE() @@ -1528,7 +1595,11 @@ MESSAGE(STATUS " With RealSense = NO (librealsense not found)") ENDIF() IF(realsense2_FOUND) +IF(WIN32) MESSAGE(STATUS " With RealSense2 = YES (License: Apache-2)") +ELSE() +MESSAGE(STATUS " With RealSense2 ${realsense2_VERSION} = YES (License: Apache-2)") +ENDIF() ELSEIF(NOT WITH_REALSENSE2) MESSAGE(STATUS " With RealSense2 = NO (WITH_REALSENSE2=OFF)") ELSE() @@ -1544,7 +1615,7 @@ MESSAGE(STATUS " With MyntEyeS = NO (mynteye s sdk not found)") ENDIF() IF(depthai_FOUND) -MESSAGE(STATUS " With DepthAI = YES (License: MIT)") +MESSAGE(STATUS " With DepthAI ${depthai_VERSION} = YES (License: MIT)") ELSEIF(NOT WITH_DEPTHAI) MESSAGE(STATUS " With DepthAI = NO (WITH_DEPTHAI=OFF)") ELSE() diff --git a/README.md b/README.md index a344baa179..7e40900a89 100644 --- a/README.md +++ b/README.md @@ -4,11 +4,15 @@ rtabmap [![RTAB-Map Logo](https://raw.githubusercontent.com/introlab/rtabmap/master/guilib/src/images/RTAB-Map100.png)](http://introlab.github.io/rtabmap) [![Release][release-image]][releases] +[![Downloads][downloads-image]][downloads] [![License][license-image]][license] -[release-image]: https://img.shields.io/badge/release-0.20.16-green.svg?style=flat +[release-image]: https://img.shields.io/badge/release-0.21.0-green.svg?style=flat [releases]: https://github.com/introlab/rtabmap/releases +[downloads-image]: https://img.shields.io/github/downloads/introlab/rtabmap/total?label=downloads +[downloads]: https://github.com/introlab/rtabmap/releases + [license-image]: https://img.shields.io/badge/license-BSD-green.svg?style=flat [license]: https://github.com/introlab/rtabmap/blob/master/LICENSE @@ -50,31 +54,29 @@ This project is supported by [IntRoLab - Intelligent / Interactive / Integrated - - - - - + - - - - - - - - - + + + + + + + + + +
ROS 1MelodicBuild Status
ROS 1 Noetic Build Status
ROS 2FoxyBuild Status
GalacticBuild Status
ROS 2 Humble Build Status
IronBuild Status
Rolling Build Status
Docker + rtabmap + Docker Pulls
- diff --git a/RTABMapConfig.cmake.in b/RTABMapConfig.cmake.in index da95aeecae..5c6c947e0b 100644 --- a/RTABMapConfig.cmake.in +++ b/RTABMapConfig.cmake.in @@ -1,99 +1,99 @@ -# - Config file for the RTABMap package -# Components: -# core (required) -# gui (optional) -# utilite (required) -# It defines the following variables -# RTABMap_INCLUDE_DIRS - include directories for RTABMap -# RTABMap_LIBRARIES - libraries to link against -# RTABMap_CORE - core library -# RTABMap_UTILITE - utilite library -# RTABMap_GUI - gui library (set if RTABMap is built with Qt) +include(CMakeFindDependencyMacro) -# Compute paths -get_filename_component(RTABMap_CMAKE_DIR "${CMAKE_CURRENT_LIST_FILE}" PATH) -set(RTABMap_INCLUDE_DIRS "@CONF_INCLUDE_DIRS@") +# Mandatory dependencies +find_dependency(OpenCV COMPONENTS core calib3d imgproc highgui stitching photo video OPTIONAL_COMPONENTS aruco xfeatures2d nonfree gpu cudafeatures2d) -#core lib -find_library(RTABMap_CORE_RELEASE NAMES rtabmap_core NO_DEFAULT_PATH HINTS @CONF_LIB_DIR@) -find_library(RTABMap_CORE_DEBUG NAMES rtabmap_cored NO_DEFAULT_PATH HINTS @CONF_LIB_DIR@) - -IF(RTABMap_CORE_DEBUG AND RTABMap_CORE_RELEASE) - SET(RTABMap_CORE - debug ${RTABMap_CORE_DEBUG} - optimized ${RTABMap_CORE_RELEASE} - ) -ELSEIF(RTABMap_CORE_DEBUG) - SET(RTABMap_CORE ${RTABMap_CORE_DEBUG}) +if(EXISTS "${CMAKE_CURRENT_LIST_DIR}/RTABMap_guiTargets.cmake") + find_dependency(PCL 1.7 COMPONENTS common io kdtree search surface filters registration sample_consensus segmentation visualization) + + if(@CONF_QT_VERSION@ EQUAL 6) + find_dependency(Qt6 COMPONENTS Widgets Core Gui OpenGL) + elseif(@CONF_QT_VERSION@ EQUAL 5) + find_dependency(Qt5 COMPONENTS Widgets Core Gui OpenGL) + else() # Qt4 + find_dependency(Qt4 COMPONENTS QtCore QtGui) + endif() + set(RTABMap_QT_VERSION @CONF_QT_VERSION@) ELSE() - SET(RTABMap_CORE ${RTABMap_CORE_RELEASE}) + find_dependency(PCL 1.7 COMPONENTS common io kdtree search surface filters registration sample_consensus segmentation) ENDIF() +set(RTABMap_DEFINITIONS ${PCL_DEFINITIONS}) +add_definitions(${RTABMap_DEFINITIONS}) # To include -march=native if set -#utilite lib -find_library(RTABMap_UTILITE_RELEASE NAMES rtabmap_utilite NO_DEFAULT_PATH HINTS @CONF_LIB_DIR@) -find_library(RTABMap_UTILITE_DEBUG NAMES rtabmap_utilited NO_DEFAULT_PATH HINTS @CONF_LIB_DIR@) +# Optional dependencies +IF(EXISTS "${CMAKE_CURRENT_LIST_DIR}/@CONF_MODULES_DIR@") + list(APPEND CMAKE_MODULE_PATH "${CMAKE_CURRENT_LIST_DIR}/@CONF_MODULES_DIR@") +ENDIF() -IF(RTABMap_UTILITE_DEBUG AND RTABMap_UTILITE_RELEASE) - SET(RTABMap_UTILITE - debug ${RTABMap_UTILITE_DEBUG} - optimized ${RTABMap_UTILITE_RELEASE} - ) -ELSEIF(RTABMap_UTILITE_DEBUG) - SET(RTABMap_UTILITE ${RTABMap_UTILITE_DEBUG}) -ELSE() - SET(RTABMap_UTILITE ${RTABMap_UTILITE_RELEASE}) +IF(@CONF_WITH_REALSENSE2@) + IF(WIN32) + find_dependency(RealSense2) + ELSE() + find_dependency(realsense2) + ENDIF() ENDIF() -set(RTABMap_LIBRARIES ${RTABMap_CORE} ${RTABMap_UTILITE}) +IF(@CONF_WITH_K4A@) + IF(WIN32) + find_dependency(K4A) + ELSE() + find_dependency(k4a) + find_dependency(k4arecord) + ENDIF() +ENDIF() -list(LENGTH RTABMap_FIND_COMPONENTS RTABMap_FIND_COMPONENTS_LENGTH) -set(WITH_GUI ON) -if(${RTABMap_FIND_COMPONENTS_LENGTH} GREATER 0) - list (FIND RTABMap_FIND_COMPONENTS "gui" _index) - if (${_index} EQUAL -1) - set(WITH_GUI OFF) - endif() -endif(${RTABMap_FIND_COMPONENTS_LENGTH} GREATER 0) +IF(@CONF_WITH_DEPTH_AI@) + find_dependency(depthai 2) +ENDIF() + +IF(@CONF_WITH_OCTOMAP@) + find_dependency(octomap) +ENDIF() + +IF(@CONF_WITH_PYTHON@) + find_dependency(Python3 COMPONENTS Interpreter Development NumPy) +ENDIF() +# Provide those for backward compatibilities (e.g., catkin requires them to propagate dependencies) +set(RTABMap_INCLUDE_DIRS "") +set(RTABMap_LIBRARIES "") +set(RTABMap_TARGETS "") -#gui lib (OFF if RTAB-Map is not built with Qt) -if(@CONF_WITH_GUI@ AND ${WITH_GUI}) - find_library(RTABMap_GUI_RELEASE NAMES rtabmap_gui NO_DEFAULT_PATH HINTS @CONF_LIB_DIR@) - find_library(RTABMap_GUI_DEBUG NAMES rtabmap_guid NO_DEFAULT_PATH HINTS @CONF_LIB_DIR@) - - IF(RTABMap_GUI_DEBUG AND RTABMap_GUI_RELEASE) - SET(RTABMap_GUI - debug ${RTABMap_GUI_DEBUG} - optimized ${RTABMap_GUI_RELEASE} - ) - ELSEIF(RTABMap_GUI_RELEASE) - SET(RTABMap_GUI ${RTABMap_GUI_RELEASE}) - ELSEIF(RTABMap_GUI_DEBUG) - SET(RTABMap_GUI ${RTABMap_GUI_DEBUG}) - ENDIF() - - set(RTABMap_LIBRARIES ${RTABMap_LIBRARIES} ${RTABMap_GUI}) -elseif(${WITH_GUI}) - MESSAGE(WARNING "Asked for \"gui\" module but RTABMap hasn't been built with gui support.") -endif() +set(_RTABMap_supported_components utilite core gui) -# Dependencies -if(@CONF_VTK_QT@ AND ${WITH_GUI}) - find_package(VTK COMPONENTS vtkGUISupportQt NO_MODULE) # to define vtkGUISupportQt target -endif(@CONF_VTK_QT@ AND ${WITH_GUI}) -if(@CONF_DEPTH_AI@) - FIND_PACKAGE(depthai 2 QUIET REQUIRED) -endif(@CONF_DEPTH_AI@) -SET(RTABMap_LIBRARIES ${RTABMap_LIBRARIES} "@CONF_DEPENDENCIES@") +foreach(_comp ${_RTABMap_supported_components}) + if(EXISTS "${CMAKE_CURRENT_LIST_DIR}/RTABMap_${_comp}Targets.cmake") + include("${CMAKE_CURRENT_LIST_DIR}/RTABMap_${_comp}Targets.cmake") + set(RTABMap_${_comp}_FOUND True) + set(RTABMap_TARGETS + ${RTABMap_TARGETS} + rtabmap::${_comp}) + get_target_property(RTABMap_${_comp}_INCLUDE_DIRS rtabmap::${_comp} INTERFACE_INCLUDE_DIRECTORIES) + get_target_property(RTABMap_${_comp}_LIBRARIES rtabmap::${_comp} INTERFACE_LINK_LIBRARIES) + set(RTABMap_INCLUDE_DIRS + ${RTABMap_INCLUDE_DIRS} + ${RTABMap_${_comp}_INCLUDE_DIRS}) + set(RTABMap_LIBRARIES + ${RTABMap_LIBRARIES} + rtabmap::${_comp}) + if(RTABMap_${_comp}_LIBRARIES) + set(RTABMap_LIBRARIES + ${RTABMap_LIBRARIES} + ${RTABMap_${_comp}_LIBRARIES}) + endif() + else() + set(RTABMap_${_comp}_FOUND False) + endif() +endforeach() -#backward compatibilities -set(RTABMAP_CORE ${RTABMap_CORE}) -set(RTABMAP_UTILITE ${RTABMap_UTILITE}) -if(RTABMap_GUI) - set(RTABMAP_GUI ${RTABMap_GUI}) - set(RTABMAP_QT_VERSION @CONF_QT_VERSION@) -endif(RTABMap_GUI) +include("${CMAKE_CURRENT_LIST_DIR}/RTABMapTargets.cmake") -include(FindPackageHandleStandardArgs) -find_package_handle_standard_args(RTABMap DEFAULT_MSG RTABMap_LIBRARIES RTABMap_INCLUDE_DIRS) -mark_as_advanced(RTABMap_LIBRARIES RTABMap_INCLUDE_DIRS RTABMap_LIBRARY_DIRS) \ No newline at end of file +foreach(_comp ${RTABMap_FIND_COMPONENTS}) + if (NOT ";${_RTABMap_supported_components};" MATCHES ";${_comp};") + set(RTABMap_${_comp}_FOUND False) + if(${RTABMap_FIND_REQUIRED_${_comp}}) + set(RTABMap_FOUND False) + set(RTABMap_NOT_FOUND_MESSAGE "Unsupported or not found required component: ${_comp}") + endif() + endif() +endforeach() diff --git a/RTABMapConfigVersion.cmake.in b/RTABMapConfigVersion.cmake.in deleted file mode 100644 index 4b0a3ae1f5..0000000000 --- a/RTABMapConfigVersion.cmake.in +++ /dev/null @@ -1,11 +0,0 @@ -set(PACKAGE_VERSION "@RTABMAP_VERSION@") - -# Check whether the requested PACKAGE_FIND_VERSION is compatible -if("${PACKAGE_VERSION}" VERSION_LESS "${PACKAGE_FIND_VERSION}") - set(PACKAGE_VERSION_COMPATIBLE FALSE) -else() - set(PACKAGE_VERSION_COMPATIBLE TRUE) - if ("${PACKAGE_VERSION}" VERSION_EQUAL "${PACKAGE_FIND_VERSION}") - set(PACKAGE_VERSION_EXACT TRUE) - endif() -endif() diff --git a/Version.h.in b/Version.h.in index 42b16a1bfa..97e04b77e0 100644 --- a/Version.h.in +++ b/Version.h.in @@ -43,6 +43,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. @G2O_CPP_CONF@#define RTABMAP_G2O_CPP11 @G2O_CPP11@ @GTSAM@#define RTABMAP_GTSAM @CERES@#define RTABMAP_CERES +@MRPT@#define RTABMAP_MRPT @VERTIGO@#define RTABMAP_VERTIGO @OPENNI2@#define RTABMAP_OPENNI2 @FREENECT@#define RTABMAP_FREENECT diff --git a/app/android/jni/CameraMobile.cpp b/app/android/jni/CameraMobile.cpp index 9e859dce69..98f9491679 100644 --- a/app/android/jni/CameraMobile.cpp +++ b/app/android/jni/CameraMobile.cpp @@ -432,23 +432,25 @@ LaserScan CameraMobile::scanFromPointCloudData( //get color from rgb image cv::Point3f org= pt; pt = util3d::transformPoint(pt, opticalRotationInv); - int u,v; - model.reproject(pt.x, pt.y, pt.z, u, v); - unsigned char r=255,g=255,b=255; - if(model.inFrame(u, v)) + if(pt.z > 0) { - b=rgb.at(v,u).val[0]; - g=rgb.at(v,u).val[1]; - r=rgb.at(v,u).val[2]; - if(kpts) - kpts->push_back(cv::KeyPoint(u,v,kptsSize)); - if(kpts3D) - kpts3D->push_back(org); - - *(int*)&ptr[oi*4 + 3] = int(b) | (int(g) << 8) | (int(r) << 16); - ++oi; + int u,v; + model.reproject(pt.x, pt.y, pt.z, u, v); + unsigned char r=255,g=255,b=255; + if(model.inFrame(u, v)) + { + b=rgb.at(v,u).val[0]; + g=rgb.at(v,u).val[1]; + r=rgb.at(v,u).val[2]; + if(kpts) + kpts->push_back(cv::KeyPoint(u,v,kptsSize)); + if(kpts3D) + kpts3D->push_back(org); + + *(int*)&ptr[oi*4 + 3] = int(b) | (int(g) << 8) | (int(r) << 16); + ++oi; + } } - //confidence //*(int*)&ptr[i*4 + 3] = (int(pointCloudData[i*4 + 3] * 255.0f) << 8) | (int(255) << 16); diff --git a/app/android/jni/RTABMapApp.cpp b/app/android/jni/RTABMapApp.cpp index 51b731a4fe..2346bb43f9 100644 --- a/app/android/jni/RTABMapApp.cpp +++ b/app/android/jni/RTABMapApp.cpp @@ -649,9 +649,9 @@ int RTABMapApp::openDatabase(const std::string & databasePath, bool databaseInMe UWARN("Cloud %d is empty", id); } } - else + else if(!data.depthOrRightCompressed().empty() || !data.laserScanCompressed().isEmpty()) { - UERROR("Failed to uncompress data!"); + UERROR("Failed to uncompress data! (rgb=%d, depth=%d, scan=%d)", data.imageCompressed().cols, data.depthOrRightCompressed().cols, data.laserScanCompressed().size()); status=-2; } } diff --git a/app/ios/RTABMapApp.xcodeproj/project.pbxproj b/app/ios/RTABMapApp.xcodeproj/project.pbxproj index 22dae846d5..ac31de3e37 100644 --- a/app/ios/RTABMapApp.xcodeproj/project.pbxproj +++ b/app/ios/RTABMapApp.xcodeproj/project.pbxproj @@ -1007,7 +1007,7 @@ "$(PROJECT_DIR)/RTABMapApp/Libraries/lib", "$(PROJECT_DIR)/RTABMapApp/Libraries/share/OpenCV/3rdparty/lib", ); - MARKETING_VERSION = 0.20.19; + MARKETING_VERSION = 0.21.0; OTHER_CFLAGS = ""; PRODUCT_BUNDLE_IDENTIFIER = com.introlab.rtabmap; PRODUCT_NAME = "$(TARGET_NAME)"; @@ -1020,7 +1020,7 @@ "\"$(SRCROOT)/RTABMapApp/Libraries/include\"", "\"$(SRCROOT)/RTABMapApp/Libraries/include/eigen3\"", "\"$(SRCROOT)/RTABMapApp/Libraries/include/pcl-1.11\"", - "\"$(SRCROOT)/RTABMapApp/Libraries/include/rtabmap-0.20\"", + "\"$(SRCROOT)/RTABMapApp/Libraries/include/rtabmap-0.21\"", "\"$(SRCROOT)/../android/jni/tango-gl/include\"", "\"$(SRCROOT)/../android/jni/third-party/include\"", "\"$(SRCROOT)/RTABMapApp/Libraries/lib/vtk.framework/Headers\"", @@ -1064,7 +1064,7 @@ "$(PROJECT_DIR)/RTABMapApp/Libraries/lib", "$(PROJECT_DIR)/RTABMapApp/Libraries/share/OpenCV/3rdparty/lib", ); - MARKETING_VERSION = 0.20.19; + MARKETING_VERSION = 0.21.0; ONLY_ACTIVE_ARCH = YES; OTHER_CFLAGS = ""; PRODUCT_BUNDLE_IDENTIFIER = com.introlab.rtabmap; @@ -1078,7 +1078,7 @@ "\"$(SRCROOT)/RTABMapApp/Libraries/include\"", "\"$(SRCROOT)/RTABMapApp/Libraries/include/eigen3\"", "\"$(SRCROOT)/RTABMapApp/Libraries/include/pcl-1.11\"", - "\"$(SRCROOT)/RTABMapApp/Libraries/include/rtabmap-0.20\"", + "\"$(SRCROOT)/RTABMapApp/Libraries/include/rtabmap-0.21\"", "\"$(SRCROOT)/../android/jni/tango-gl/include\"", "\"$(SRCROOT)/../android/jni/third-party/include\"", "\"$(SRCROOT)/RTABMapApp/Libraries/lib/vtk.framework/Headers\"", diff --git a/app/ios/RTABMapApp/NativeWrapper.cpp b/app/ios/RTABMapApp/NativeWrapper.cpp index 145e3a8e30..dfe1259552 100644 --- a/app/ios/RTABMapApp/NativeWrapper.cpp +++ b/app/ios/RTABMapApp/NativeWrapper.cpp @@ -489,13 +489,6 @@ void setGridVisibleNative(const void *object, bool visible) else UERROR("object is null!"); } -void setRawScanSavedNative(const void *object, bool enabled) -{ - if(object) - native(object)->setRawScanSaved(enabled); - else - UERROR("object is null!"); -} void setFullResolutionNative(const void *object, bool enabled) { if(object) diff --git a/app/ios/RTABMapApp/NativeWrapper.hpp b/app/ios/RTABMapApp/NativeWrapper.hpp index 574295117b..3b36a29563 100644 --- a/app/ios/RTABMapApp/NativeWrapper.hpp +++ b/app/ios/RTABMapApp/NativeWrapper.hpp @@ -97,7 +97,6 @@ void setGraphOptimizationNative(const void *object, bool enabled); void setNodesFilteringNative(const void *object, bool enabled); void setGraphVisibleNative(const void *object, bool visible); void setGridVisibleNative(const void *object, bool visible); -void setRawScanSavedNative(const void *object, bool enabled); void setFullResolutionNative(const void *object, bool enabled); void setSmoothingNative(const void *object, bool enabled); void setAppendModeNative(const void *object, bool enabled); diff --git a/app/ios/RTABMapApp/RTABMap.swift b/app/ios/RTABMapApp/RTABMap.swift index 52430941b4..2ca01745f6 100644 --- a/app/ios/RTABMapApp/RTABMap.swift +++ b/app/ios/RTABMapApp/RTABMap.swift @@ -405,9 +405,6 @@ class RTABMap { func setGridVisible(visible: Bool) { setGridVisibleNative(native_rtabmap, visible) } - func setRawScanSaved(enabled: Bool) { - setRawScanSavedNative(native_rtabmap, enabled) - } func setFullResolution(enabled: Bool) { setFullResolutionNative(native_rtabmap, enabled) } diff --git a/app/ios/RTABMapApp/ViewController.swift b/app/ios/RTABMapApp/ViewController.swift index dabfe15063..40ebfbaca9 100644 --- a/app/ios/RTABMapApp/ViewController.swift +++ b/app/ios/RTABMapApp/ViewController.swift @@ -1319,7 +1319,6 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP // update preference rtabmap!.setOnlineBlending(enabled: defaults.bool(forKey: "Blending")); rtabmap!.setNodesFiltering(enabled: defaults.bool(forKey: "NodesFiltering")); - rtabmap!.setRawScanSaved(enabled: defaults.bool(forKey: "SaveRawScan")); rtabmap!.setFullResolution(enabled: defaults.bool(forKey: "HDMode")); rtabmap!.setSmoothing(enabled: defaults.bool(forKey: "Smoothing")); rtabmap!.setAppendMode(enabled: defaults.bool(forKey: "AppendMode")); diff --git a/app/ios/RTABMapApp/install_deps.sh b/app/ios/RTABMapApp/install_deps.sh index 5c0b6de250..0e8debd167 100755 --- a/app/ios/RTABMapApp/install_deps.sh +++ b/app/ios/RTABMapApp/install_deps.sh @@ -122,7 +122,7 @@ git clone https://github.com/PointCloudLibrary/pcl.git cd pcl git checkout tags/pcl-1.11.1 # patch -curl -L https://gist.github.com/matlabbe/f3ba9366eb91e1b855dadd2ddce5746d/raw/4a66ebb9faa1dfe997a0860d733bc5473cff20ee/pcl_1_11_1_vtk_ios_support.patch -o pcl_1_11_1_vtk_ios_support.patch +curl -L https://gist.github.com/matlabbe/f3ba9366eb91e1b855dadd2ddce5746d/raw/6869cf26211ab15492599e557b0e729b23b2c119/pcl_1_11_1_vtk_ios_support.patch -o pcl_1_11_1_vtk_ios_support.patch git apply pcl_1_11_1_vtk_ios_support.patch mkdir build cd build diff --git a/app/ios/Settings.bundle/License.plist b/app/ios/Settings.bundle/License.plist index 3c600aa4a3..dc827e966a 100644 --- a/app/ios/Settings.bundle/License.plist +++ b/app/ios/Settings.bundle/License.plist @@ -13,7 +13,7 @@ ======= RTAB-Map ======= RTAB-Map - https://github.com/introlab/rtabmap -Copyright (c) 2010-2022, Mathieu Labbe - IntRoLab - Universite de Sherbrooke, all rights reserved. +Copyright (c) 2010-2023, Mathieu Labbe - IntRoLab - Universite de Sherbrooke, all rights reserved. Copyright (c) XXX, contributors, all rights reserved. Redistribution and use in source and binary forms, with or without diff --git a/app/ios/Settings.bundle/Mapping.plist b/app/ios/Settings.bundle/Mapping.plist index f84ed83b9c..0f2dc059b6 100644 --- a/app/ios/Settings.bundle/Mapping.plist +++ b/app/ios/Settings.bundle/Mapping.plist @@ -552,50 +552,50 @@ DefaultValue - - Type - PSGroupSpecifier - FooterText - Used only in localization mode (when clicking First-P. View during visualization). This is used to get smoother localizations and to verify localization transforms (when Max Optimization Error is not disabled) to make sure we don't teleport to a location very similar to one we previously localized on. - - - Type - PSMultiValueSpecifier - Title - Maximum odometry cache size - Key - MaximumOdometryCacheSize - DefaultValue - 10 - Titles - - 500 - 200 - 100 - 75 - 50 - 40 - 30 - 20 - 10 - 5 - Disabled - - Values - - 500 - 200 - 100 - 75 - 50 - 40 - 30 - 20 - 10 - 5 - 0 - - + + Type + PSGroupSpecifier + FooterText + Used only in localization mode (when clicking First-P. View during visualization). This is used to get smoother localizations and to verify localization transforms (when Max Optimization Error is not disabled) to make sure we don't teleport to a location very similar to one we previously localized on. + + + Type + PSMultiValueSpecifier + Title + Maximum odometry cache size + Key + MaximumOdometryCacheSize + DefaultValue + 10 + Titles + + 500 + 200 + 100 + 75 + 50 + 40 + 30 + 20 + 10 + 5 + Disabled + + Values + + 500 + 200 + 100 + 75 + 50 + 40 + 30 + 20 + 10 + 5 + 0 + + Type PSGroupSpecifier @@ -752,22 +752,6 @@ DefaultValue - - Type - PSGroupSpecifier - FooterText - Save raw point clouds in database. - - - Type - PSToggleSwitchSpecifier - Title - Save Raw Scan - Key - SaveRawScan - DefaultValue - - Type PSGroupSpecifier diff --git a/app/ios/Settings.bundle/Root.plist b/app/ios/Settings.bundle/Root.plist index 39b71e84be..bc9360fe97 100644 --- a/app/ios/Settings.bundle/Root.plist +++ b/app/ios/Settings.bundle/Root.plist @@ -454,7 +454,7 @@ FooterText - Copyright (c) 2010-2022, Mathieu Labbe - IntRoLab - Université de Sherbrooke. All rights reserved. + Copyright (c) 2010-2023, Mathieu Labbe - IntRoLab - Université de Sherbrooke. All rights reserved. Title About Type @@ -462,7 +462,7 @@ DefaultValue - 0.20.19 + 0.21.0 Key Version Title diff --git a/app/src/CMakeLists.txt b/app/src/CMakeLists.txt index b20a07d42b..5bc79a1529 100644 --- a/app/src/CMakeLists.txt +++ b/app/src/CMakeLists.txt @@ -3,32 +3,6 @@ SET(SRC_FILES main.cpp ) -SET(INCLUDE_DIRS - ${PROJECT_BINARY_DIR}/corelib/include - ${PROJECT_SOURCE_DIR}/utilite/include - ${PROJECT_SOURCE_DIR}/corelib/include - ${PROJECT_SOURCE_DIR}/guilib/include - ${CMAKE_CURRENT_SOURCE_DIR} - ${OpenCV_INCLUDE_DIRS} - ${PCL_INCLUDE_DIRS} -) - -IF(QT4_FOUND) - INCLUDE(${QT_USE_FILE}) -ENDIF(QT4_FOUND) - -SET(LIBRARIES - ${QT_LIBRARIES} - ${OpenCV_LIBS} - ${PCL_LIBRARIES} -) - -# rc.exe has problems with these defintions... commented! -#add_definitions(${PCL_DEFINITIONS}) - -# Make sure the compiler can find include files from our library. -INCLUDE_DIRECTORIES(${INCLUDE_DIRS}) - # For Apple set the icns file containing icons IF(APPLE AND BUILD_AS_BUNDLE) # set how it shows up in the Info.plist file @@ -50,40 +24,37 @@ IF(WIN32) ELSE( MINGW ) SET(SRC_FILES ${SRC_FILES} ${PROJECT_NAME}.rc) ENDIF( MINGW ) -ENDIF(WIN32) +ENDIF(WIN32) + +IF(BUILD_AS_BUNDLE) + ADD_DEFINITIONS("-DBUILD_AS_BUNDLE") +ENDIF() # Add binary IF(APPLE AND BUILD_AS_BUNDLE) - ADD_EXECUTABLE(rtabmap MACOSX_BUNDLE ${SRC_FILES}) -ELSEIF(MINGW) - ADD_EXECUTABLE(rtabmap WIN32 ${SRC_FILES}) + ADD_EXECUTABLE(rtabmap_app MACOSX_BUNDLE ${SRC_FILES}) +ELSEIF(WIN32 AND BUILD_AS_BUNDLE) + ADD_EXECUTABLE(rtabmap_app WIN32 ${SRC_FILES}) ELSE() - ADD_EXECUTABLE(rtabmap ${SRC_FILES}) + ADD_EXECUTABLE(rtabmap_app ${SRC_FILES}) ENDIF() -TARGET_LINK_LIBRARIES(rtabmap rtabmap_core rtabmap_gui rtabmap_utilite ${LIBRARIES}) -IF(Qt5_FOUND) - IF(Qt5Svg_FOUND) - QT5_USE_MODULES(rtabmap Widgets Core Gui Svg PrintSupport) - ELSE() - QT5_USE_MODULES(rtabmap Widgets Core Gui PrintSupport) - ENDIF() -ENDIF(Qt5_FOUND) +TARGET_LINK_LIBRARIES(rtabmap_app rtabmap_gui) IF(APPLE AND BUILD_AS_BUNDLE) - SET_TARGET_PROPERTIES(rtabmap PROPERTIES + SET_TARGET_PROPERTIES(rtabmap_app PROPERTIES OUTPUT_NAME ${CMAKE_BUNDLE_NAME}) ELSEIF(WIN32) - SET_TARGET_PROPERTIES(rtabmap PROPERTIES + SET_TARGET_PROPERTIES(rtabmap_app PROPERTIES OUTPUT_NAME ${PROJECT_NAME}) ELSE() - SET_TARGET_PROPERTIES(rtabmap PROPERTIES + SET_TARGET_PROPERTIES(rtabmap_app PROPERTIES OUTPUT_NAME ${PROJECT_PREFIX}) ENDIF() #--------------------------- # Installation stuff #--------------------------- -INSTALL(TARGETS rtabmap +INSTALL(TARGETS rtabmap_app RUNTIME DESTINATION "${CMAKE_INSTALL_BINDIR}" COMPONENT runtime BUNDLE DESTINATION "${CMAKE_BUNDLE_LOCATION}" COMPONENT runtime) @@ -154,16 +125,32 @@ IF(BUILD_AS_BUNDLE AND (APPLE OR WIN32)) ENDIF(WIN32 AND CUDA_FOUND) ENDIF(Torch_FOUND) - # Install needed Qt plugins by copying directories from the qt installation - # One can cull what gets copied by using 'REGEX "..." EXCLUDE' - # Exclude debug libraries - IF(QT_PLUGINS_DIR) - INSTALL(DIRECTORY "${QT_PLUGINS_DIR}/imageformats" - DESTINATION ${plugin_dest_dir}/plugins - COMPONENT runtime - REGEX ".*d4.dll" EXCLUDE - REGEX ".*d4.a" EXCLUDE) - ELSE() + IF(Qt6_FOUND) + # Reference: https://doc-snapshots.qt.io/qt6-6.4/qt-deploy-runtime-dependencies.html + # The following script must only be executed at install time + set(deploy_script "${CMAKE_CURRENT_BINARY_DIR}/deploy_app$.cmake") + file(GENERATE OUTPUT ${deploy_script} CONTENT " + # Including the file pointed to by QT_DEPLOY_SUPPORT ensures the generated + # deployment script has access to qt_deploy_runtime_dependencies() + include(\"${QT_DEPLOY_SUPPORT}\") + qt_deploy_runtime_dependencies( + EXECUTABLE \"${APPS}\" + VERBOSE + PLUGINS_DIR ${plugin_dest_dir}/plugins + )") + install(SCRIPT "${CMAKE_CURRENT_BINARY_DIR}/deploy_appDebug.cmake" + CONFIGURATIONS Debug + COMPONENT runtime) + install(SCRIPT "${CMAKE_CURRENT_BINARY_DIR}/deploy_appRelease.cmake" + CONFIGURATIONS Release + COMPONENT runtime) + install(SCRIPT "${CMAKE_CURRENT_BINARY_DIR}/deploy_appRelWithDebInfo.cmake" + CONFIGURATIONS RelWithDebInfo + COMPONENT runtime) + install(SCRIPT "${CMAKE_CURRENT_BINARY_DIR}/deploy_appMinSizeRel.cmake" + CONFIGURATIONS MinSizeRel + COMPONENT runtime) + ELSEIF(Qt5_FOUND) #Qt5 foreach(plugin ${Qt5Gui_PLUGINS}) get_target_property(plugin_loc ${plugin} LOCATION) @@ -175,8 +162,8 @@ IF(BUILD_AS_BUNDLE AND (APPLE OR WIN32)) ENDIF(NOT plugin_root) #MESSAGE(STATUS "Qt5 plugin \"${plugin_loc}\" installed in \"${plugin_dest_dir}/plugins${plugin_type}\"") INSTALL(FILES ${plugin_loc} - DESTINATION ${plugin_dest_dir}/plugins${plugin_type} - COMPONENT runtime) + DESTINATION ${plugin_dest_dir}/plugins${plugin_type} + COMPONENT runtime) endforeach() IF(NOT Qt5Widgets_VERSION VERSION_LESS 5.10.0) IF(WIN32) @@ -194,22 +181,31 @@ IF(BUILD_AS_BUNDLE AND (APPLE OR WIN32)) #MESSAGE(STATUS "Qt5 plugin \"${plugin_loc}\" installed in \"${plugin_dest_dir}/plugins${plugin_type}\"") ENDIF(EXISTS ${plugin_loc}) ENDIF(NOT Qt5Widgets_VERSION VERSION_LESS 5.10.0) + ELSEIF(QT_PLUGINS_DIR) # Qt4 + # Install needed Qt plugins by copying directories from the qt installation + # One can cull what gets copied by using 'REGEX "..." EXCLUDE' + # Exclude debug libraries + INSTALL(DIRECTORY "${QT_PLUGINS_DIR}/imageformats" + DESTINATION ${plugin_dest_dir}/plugins + COMPONENT runtime + REGEX ".*d4.dll" EXCLUDE + REGEX ".*d4.a" EXCLUDE) ENDIF() - + # install a qt.conf file # this inserts some cmake code into the install script to write the file SET(QT_CONF_FILE [Paths]\nPlugins=plugins) IF(APPLE) - SET(QT_CONF_FILE [Paths]\nPlugins=MacOS/plugins) + SET(QT_CONF_FILE [Paths]\nPlugins=MacOS/plugins) ENDIF(APPLE) INSTALL(CODE " - file(WRITE \"\$ENV{DESTDIR}\${CMAKE_INSTALL_PREFIX}/${qtconf_dest_dir}/qt.conf\" \"${QT_CONF_FILE}\") - " COMPONENT runtime) - + file(WRITE \"\$ENV{DESTDIR}\${CMAKE_INSTALL_PREFIX}/${qtconf_dest_dir}/qt.conf\" \"${QT_CONF_FILE}\") + " COMPONENT runtime) + # directories to look for dependencies - SET(DIRS ${QT_LIBRARY_DIRS} ${PROJECT_SOURCE_DIR}/bin) + SET(DIRS ${QT_LIBRARY_DIRS} ${PROJECT_BINARY_DIR}/bin) IF(APPLE) - SET(DIRS ${DIRS} /usr/local /usr/local/lib) + SET(DIRS ${DIRS} /usr/local /usr/local/lib /opt/homebrew /opt/homebrew/lib /opt/homebrew/lib/gcc/current) ENDIF(APPLE) # Now the work of copying dependencies into the bundle/package diff --git a/app/src/main.cpp b/app/src/main.cpp index 2ffa401134..3603aca065 100644 --- a/app/src/main.cpp +++ b/app/src/main.cpp @@ -36,8 +36,13 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "rtabmap/utilite/UFile.h" #include "rtabmap/utilite/UConversion.h" +#include #include +#if VTK_MAJOR_VERSION > 9 || (VTK_MAJOR_VERSION==9 && VTK_MINOR_VERSION >= 1) +#include +#endif + using namespace rtabmap; int main(int argc, char* argv[]) @@ -50,9 +55,13 @@ int main(int argc, char* argv[]) CoInitialize(nullptr); #endif -#if VTK_MAJOR_VERSION >= 8 +#if VTK_MAJOR_VERSION >= 8 && defined(BUILD_AS_BUNDLE) vtkObject::GlobalWarningDisplayOff(); #endif +#if VTK_MAJOR_VERSION > 9 || (VTK_MAJOR_VERSION==9 && VTK_MINOR_VERSION >= 1) + // needed to ensure appropriate OpenGL context is created for VTK rendering. + QSurfaceFormat::setDefaultFormat(QVTKRenderWidget::defaultFormat()); +#endif /* Create tasks */ QApplication * app = new QApplication(argc, argv); diff --git a/build/.gitignore b/build/.gitignore index 5e7d2734cf..86d0cb2726 100644 --- a/build/.gitignore +++ b/build/.gitignore @@ -1,4 +1,4 @@ # Ignore everything in this directory * # Except this file -!.gitignore +!.gitignore \ No newline at end of file diff --git a/cmake_modules/FindEigen3.cmake b/cmake_modules/FindEigen3.cmake new file mode 100644 index 0000000000..0b36805e75 --- /dev/null +++ b/cmake_modules/FindEigen3.cmake @@ -0,0 +1,107 @@ +# - Try to find Eigen3 lib +# +# This module supports requiring a minimum version, e.g. you can do +# find_package(Eigen3 3.1.2) +# to require version 3.1.2 or newer of Eigen3. +# +# Once done this will define +# +# EIGEN3_FOUND - system has eigen lib with correct version +# EIGEN3_INCLUDE_DIR - the eigen include directory +# EIGEN3_VERSION - eigen version +# +# and the following imported target: +# +# Eigen3::Eigen - The header-only Eigen library +# +# This module reads hints about search locations from +# the following environment variables: +# +# EIGEN3_ROOT +# EIGEN3_ROOT_DIR + +# Copyright (c) 2006, 2007 Montel Laurent, +# Copyright (c) 2008, 2009 Gael Guennebaud, +# Copyright (c) 2009 Benoit Jacob +# Redistribution and use is allowed according to the terms of the 2-clause BSD license. + +if(NOT Eigen3_FIND_VERSION) + if(NOT Eigen3_FIND_VERSION_MAJOR) + set(Eigen3_FIND_VERSION_MAJOR 2) + endif() + if(NOT Eigen3_FIND_VERSION_MINOR) + set(Eigen3_FIND_VERSION_MINOR 91) + endif() + if(NOT Eigen3_FIND_VERSION_PATCH) + set(Eigen3_FIND_VERSION_PATCH 0) + endif() + + set(Eigen3_FIND_VERSION "${Eigen3_FIND_VERSION_MAJOR}.${Eigen3_FIND_VERSION_MINOR}.${Eigen3_FIND_VERSION_PATCH}") +endif() + +macro(_eigen3_check_version) + file(READ "${EIGEN3_INCLUDE_DIR}/Eigen/src/Core/util/Macros.h" _eigen3_version_header) + + string(REGEX MATCH "define[ \t]+EIGEN_WORLD_VERSION[ \t]+([0-9]+)" _eigen3_world_version_match "${_eigen3_version_header}") + set(EIGEN3_WORLD_VERSION "${CMAKE_MATCH_1}") + string(REGEX MATCH "define[ \t]+EIGEN_MAJOR_VERSION[ \t]+([0-9]+)" _eigen3_major_version_match "${_eigen3_version_header}") + set(EIGEN3_MAJOR_VERSION "${CMAKE_MATCH_1}") + string(REGEX MATCH "define[ \t]+EIGEN_MINOR_VERSION[ \t]+([0-9]+)" _eigen3_minor_version_match "${_eigen3_version_header}") + set(EIGEN3_MINOR_VERSION "${CMAKE_MATCH_1}") + + set(EIGEN3_VERSION ${EIGEN3_WORLD_VERSION}.${EIGEN3_MAJOR_VERSION}.${EIGEN3_MINOR_VERSION}) + if(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION}) + set(EIGEN3_VERSION_OK FALSE) + else() + set(EIGEN3_VERSION_OK TRUE) + endif() + + if(NOT EIGEN3_VERSION_OK) + + message(STATUS "Eigen3 version ${EIGEN3_VERSION} found in ${EIGEN3_INCLUDE_DIR}, " + "but at least version ${Eigen3_FIND_VERSION} is required") + endif() +endmacro() + +if (EIGEN3_INCLUDE_DIR) + + # in cache already + _eigen3_check_version() + set(EIGEN3_FOUND ${EIGEN3_VERSION_OK}) + set(Eigen3_FOUND ${EIGEN3_VERSION_OK}) + +else () + + # search first if an Eigen3Config.cmake is available in the system, + # if successful this would set EIGEN3_INCLUDE_DIR and the rest of + # the script will work as usual + find_package(Eigen3 ${Eigen3_FIND_VERSION} NO_MODULE QUIET) + + if(NOT EIGEN3_INCLUDE_DIR) + find_path(EIGEN3_INCLUDE_DIR NAMES signature_of_eigen3_matrix_library + HINTS + ENV EIGEN3_ROOT + ENV EIGEN3_ROOT_DIR + PATHS + ${CMAKE_INSTALL_PREFIX}/include + ${KDE4_INCLUDE_DIR} + PATH_SUFFIXES eigen3 eigen + ) + endif() + + if(EIGEN3_INCLUDE_DIR) + _eigen3_check_version() + endif() + + include(FindPackageHandleStandardArgs) + find_package_handle_standard_args(Eigen3 DEFAULT_MSG EIGEN3_INCLUDE_DIR EIGEN3_VERSION_OK) + + mark_as_advanced(EIGEN3_INCLUDE_DIR) + +endif() + +if(EIGEN3_FOUND AND NOT TARGET Eigen3::Eigen) + add_library(Eigen3::Eigen INTERFACE IMPORTED) + set_target_properties(Eigen3::Eigen PROPERTIES + INTERFACE_INCLUDE_DIRECTORIES "${EIGEN3_INCLUDE_DIR}") +endif() 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/cmake_modules/FindZEDOC.cmake b/cmake_modules/FindZEDOC.cmake index 76563ed943..8c96c2bd1c 100644 --- a/cmake_modules/FindZEDOC.cmake +++ b/cmake_modules/FindZEDOC.cmake @@ -17,7 +17,7 @@ ENDIF (ZEDOC_INCLUDE_DIR AND ZEDOC_LIBRARY) IF (ZEDOC_FOUND) # show which ZEDOC was found only if not quiet - IF (NOT _FIND_QUIETLY) + IF (NOT ZEDOC_FIND_QUIETLY) MESSAGE(STATUS "Found ZEDOC: ${ZEDOC_LIBRARIES}") ENDIF (NOT ZEDOC_FIND_QUIETLY) ELSE (ZEDOC_FOUND) diff --git a/corelib/include/rtabmap/core/BayesFilter.h b/corelib/include/rtabmap/core/BayesFilter.h index 01f4fe30bf..ac2533903b 100644 --- a/corelib/include/rtabmap/core/BayesFilter.h +++ b/corelib/include/rtabmap/core/BayesFilter.h @@ -28,7 +28,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #ifndef BAYESFILTER_H_ #define BAYESFILTER_H_ -#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines +#include "rtabmap/core/rtabmap_core_export.h" // DLL export/import defines #include #include @@ -41,7 +41,7 @@ namespace rtabmap { class Memory; class Signature; -class RTABMAP_EXP BayesFilter +class RTABMAP_CORE_EXPORT BayesFilter { public: BayesFilter(const ParametersMap & parameters = ParametersMap()); diff --git a/corelib/include/rtabmap/core/Camera.h b/corelib/include/rtabmap/core/Camera.h index 114273bff9..708584809e 100644 --- a/corelib/include/rtabmap/core/Camera.h +++ b/corelib/include/rtabmap/core/Camera.h @@ -27,7 +27,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #pragma once -#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines +#include "rtabmap/core/rtabmap_core_export.h" // DLL export/import defines #include namespace rtabmap @@ -37,7 +37,7 @@ namespace rtabmap * Class Camera * */ -class RTABMAP_EXP Camera : public SensorCapture +class RTABMAP_CORE_EXPORT Camera : public SensorCapture { public: virtual ~Camera() {} diff --git a/corelib/include/rtabmap/core/CameraModel.h b/corelib/include/rtabmap/core/CameraModel.h index ae36a37291..b24b971b4d 100644 --- a/corelib/include/rtabmap/core/CameraModel.h +++ b/corelib/include/rtabmap/core/CameraModel.h @@ -30,12 +30,12 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include -#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines +#include "rtabmap/core/rtabmap_core_export.h" // DLL export/import defines #include "rtabmap/core/Transform.h" namespace rtabmap { -class RTABMAP_EXP CameraModel +class RTABMAP_CORE_EXPORT CameraModel { public: /** @@ -159,7 +159,7 @@ class RTABMAP_EXP CameraModel Transform localTransform_; }; -RTABMAP_EXP std::ostream& operator<<(std::ostream& os, const CameraModel& model); +RTABMAP_CORE_EXPORT std::ostream& operator<<(std::ostream& os, const CameraModel& model); } /* namespace rtabmap */ #endif /* CAMERAMODEL_H_ */ diff --git a/corelib/include/rtabmap/core/Compression.h b/corelib/include/rtabmap/core/Compression.h index dcab29dddd..9ff69a3a71 100644 --- a/corelib/include/rtabmap/core/Compression.h +++ b/corelib/include/rtabmap/core/Compression.h @@ -28,7 +28,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #ifndef COMPRESSION_H_ #define COMPRESSION_H_ -#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines +#include "rtabmap/core/rtabmap_core_export.h" // DLL export/import defines #include #include @@ -52,7 +52,7 @@ namespace rtabmap { * ct.join(); * cv::Mat image = ct.getUncompressedData(); */ -class RTABMAP_EXP CompressionThread : public UThread +class RTABMAP_CORE_EXPORT CompressionThread : public UThread { public: // format : ".png" ".jpg" "" (empty is general) @@ -70,21 +70,21 @@ class RTABMAP_EXP CompressionThread : public UThread bool compressMode_; }; -std::vector RTABMAP_EXP compressImage(const cv::Mat & image, const std::string & format = ".png"); -cv::Mat RTABMAP_EXP compressImage2(const cv::Mat & image, const std::string & format = ".png"); +std::vector RTABMAP_CORE_EXPORT compressImage(const cv::Mat & image, const std::string & format = ".png"); +cv::Mat RTABMAP_CORE_EXPORT compressImage2(const cv::Mat & image, const std::string & format = ".png"); -cv::Mat RTABMAP_EXP uncompressImage(const cv::Mat & bytes); -cv::Mat RTABMAP_EXP uncompressImage(const std::vector & bytes); +cv::Mat RTABMAP_CORE_EXPORT uncompressImage(const cv::Mat & bytes); +cv::Mat RTABMAP_CORE_EXPORT uncompressImage(const std::vector & bytes); -std::vector RTABMAP_EXP compressData(const cv::Mat & data); -cv::Mat RTABMAP_EXP compressData2(const cv::Mat & data); +std::vector RTABMAP_CORE_EXPORT compressData(const cv::Mat & data); +cv::Mat RTABMAP_CORE_EXPORT compressData2(const cv::Mat & data); -cv::Mat RTABMAP_EXP uncompressData(const cv::Mat & bytes); -cv::Mat RTABMAP_EXP uncompressData(const std::vector & bytes); -cv::Mat RTABMAP_EXP uncompressData(const unsigned char * bytes, unsigned long size); +cv::Mat RTABMAP_CORE_EXPORT uncompressData(const cv::Mat & bytes); +cv::Mat RTABMAP_CORE_EXPORT uncompressData(const std::vector & bytes); +cv::Mat RTABMAP_CORE_EXPORT uncompressData(const unsigned char * bytes, unsigned long size); -cv::Mat RTABMAP_EXP compressString(const std::string & str); -std::string RTABMAP_EXP uncompressString(const cv::Mat & bytes); +cv::Mat RTABMAP_CORE_EXPORT compressString(const std::string & str); +std::string RTABMAP_CORE_EXPORT uncompressString(const cv::Mat & bytes); } /* namespace rtabmap */ #endif /* COMPRESSION_H_ */ diff --git a/corelib/include/rtabmap/core/DBDriver.h b/corelib/include/rtabmap/core/DBDriver.h index 7499189c62..1335189272 100644 --- a/corelib/include/rtabmap/core/DBDriver.h +++ b/corelib/include/rtabmap/core/DBDriver.h @@ -28,7 +28,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #ifndef DBDRIVER_H_ #define DBDRIVER_H_ -#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines +#include "rtabmap/core/rtabmap_core_export.h" // DLL export/import defines #include #include @@ -59,7 +59,7 @@ class VisualWord; //but never, never try to use the same connection simultaneously in //two or more threads." // -class RTABMAP_EXP DBDriver : public UThreadNode +class RTABMAP_CORE_EXPORT DBDriver : public UThreadNode { public: static DBDriver * create(const ParametersMap & parameters = ParametersMap()); diff --git a/corelib/include/rtabmap/core/DBDriverSqlite3.h b/corelib/include/rtabmap/core/DBDriverSqlite3.h index 56abb750df..463e916817 100644 --- a/corelib/include/rtabmap/core/DBDriverSqlite3.h +++ b/corelib/include/rtabmap/core/DBDriverSqlite3.h @@ -28,7 +28,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #ifndef DBDRIVERSQLITE3_H_ #define DBDRIVERSQLITE3_H_ -#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines +#include "rtabmap/core/rtabmap_core_export.h" // DLL export/import defines #include "rtabmap/core/DBDriver.h" #include @@ -37,7 +37,7 @@ typedef struct sqlite3 sqlite3; namespace rtabmap { -class RTABMAP_EXP DBDriverSqlite3: public DBDriver { +class RTABMAP_CORE_EXPORT DBDriverSqlite3: public DBDriver { public: DBDriverSqlite3(const ParametersMap & parameters = ParametersMap()); virtual ~DBDriverSqlite3(); diff --git a/corelib/include/rtabmap/core/DBReader.h b/corelib/include/rtabmap/core/DBReader.h index 857935d323..5bdc0e2008 100644 --- a/corelib/include/rtabmap/core/DBReader.h +++ b/corelib/include/rtabmap/core/DBReader.h @@ -28,7 +28,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #ifndef DBREADER_H_ #define DBREADER_H_ -#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines +#include "rtabmap/core/rtabmap_core_export.h" // DLL export/import defines #include #include @@ -43,7 +43,7 @@ namespace rtabmap { class DBDriver; -class RTABMAP_EXP DBReader : public Camera { +class RTABMAP_CORE_EXPORT DBReader : public Camera { public: DBReader(const std::string & databasePath, float frameRate = 0.0f, // -1 = use Database stamps, 0 = inf @@ -57,7 +57,8 @@ class RTABMAP_EXP DBReader : public Camera { bool landmarksIgnored = false, bool featuresIgnored = false, int startMapId = 0, - int stopMapId = -1); + int stopMapId = -1, + bool priorsIgnored = false); DBReader(const std::list & databasePaths, float frameRate = 0.0f, // -1 = use Database stamps, 0 = inf bool odometryIgnored = false, @@ -70,7 +71,8 @@ class RTABMAP_EXP DBReader : public Camera { bool landmarksIgnored = false, bool featuresIgnored = false, int startMapId = 0, - int stopMapId = -1); + int stopMapId = -1, + bool priorsIgnored = false); virtual ~DBReader(); virtual bool init( @@ -100,6 +102,7 @@ class RTABMAP_EXP DBReader : public Camera { bool _intermediateNodesIgnored; bool _landmarksIgnored; bool _featuresIgnored; + bool _priorsIgnored; int _startMapId; int _stopMapId; diff --git a/corelib/include/rtabmap/core/EpipolarGeometry.h b/corelib/include/rtabmap/core/EpipolarGeometry.h index 7f13c734be..e9408cd83c 100644 --- a/corelib/include/rtabmap/core/EpipolarGeometry.h +++ b/corelib/include/rtabmap/core/EpipolarGeometry.h @@ -27,7 +27,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #pragma once -#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines +#include "rtabmap/core/rtabmap_core_export.h" // DLL export/import defines #include "rtabmap/core/Parameters.h" #include "rtabmap/utilite/UStl.h" #include @@ -42,7 +42,7 @@ namespace rtabmap class Signature; -class RTABMAP_EXP EpipolarGeometry +class RTABMAP_CORE_EXPORT EpipolarGeometry { public: EpipolarGeometry(const ParametersMap & parameters = ParametersMap()); diff --git a/corelib/include/rtabmap/core/Features2d.h b/corelib/include/rtabmap/core/Features2d.h index 0d9d838f57..81ac630d72 100644 --- a/corelib/include/rtabmap/core/Features2d.h +++ b/corelib/include/rtabmap/core/Features2d.h @@ -28,7 +28,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #ifndef FEATURES2D_H_ #define FEATURES2D_H_ -#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines +#include "rtabmap/core/rtabmap_core_export.h" // DLL export/import defines #include #include @@ -103,7 +103,7 @@ class CV_ORB; #endif // Feature2D -class RTABMAP_EXP Feature2D { +class RTABMAP_CORE_EXPORT Feature2D { public: enum Type {kFeatureUndef=-1, kFeatureSurf=0, @@ -247,7 +247,7 @@ class RTABMAP_EXP Feature2D { }; //SURF -class RTABMAP_EXP SURF : public Feature2D +class RTABMAP_CORE_EXPORT SURF : public Feature2D { public: SURF(const ParametersMap & parameters = ParametersMap()); @@ -274,7 +274,7 @@ class RTABMAP_EXP SURF : public Feature2D }; //SIFT -class RTABMAP_EXP SIFT : public Feature2D +class RTABMAP_CORE_EXPORT SIFT : public Feature2D { public: SIFT(const ParametersMap & parameters = ParametersMap()); @@ -298,7 +298,7 @@ class RTABMAP_EXP SIFT : public Feature2D }; //ORB -class RTABMAP_EXP ORB : public Feature2D +class RTABMAP_CORE_EXPORT ORB : public Feature2D { public: ORB(const ParametersMap & parameters = ParametersMap()); @@ -329,7 +329,7 @@ class RTABMAP_EXP ORB : public Feature2D }; //FAST -class RTABMAP_EXP FAST : public Feature2D +class RTABMAP_CORE_EXPORT FAST : public Feature2D { public: FAST(const ParametersMap & parameters = ParametersMap()); @@ -365,7 +365,7 @@ class RTABMAP_EXP FAST : public Feature2D }; //FAST_BRIEF -class RTABMAP_EXP FAST_BRIEF : public FAST +class RTABMAP_CORE_EXPORT FAST_BRIEF : public FAST { public: FAST_BRIEF(const ParametersMap & parameters = ParametersMap()); @@ -384,7 +384,7 @@ class RTABMAP_EXP FAST_BRIEF : public FAST }; //FAST_FREAK -class RTABMAP_EXP FAST_FREAK : public FAST +class RTABMAP_CORE_EXPORT FAST_FREAK : public FAST { public: FAST_FREAK(const ParametersMap & parameters = ParametersMap()); @@ -406,7 +406,7 @@ class RTABMAP_EXP FAST_FREAK : public FAST }; //GFTT -class RTABMAP_EXP GFTT : public Feature2D +class RTABMAP_CORE_EXPORT GFTT : public Feature2D { public: GFTT(const ParametersMap & parameters = ParametersMap()); @@ -428,7 +428,7 @@ class RTABMAP_EXP GFTT : public Feature2D }; //GFTT_BRIEF -class RTABMAP_EXP GFTT_BRIEF : public GFTT +class RTABMAP_CORE_EXPORT GFTT_BRIEF : public GFTT { public: GFTT_BRIEF(const ParametersMap & parameters = ParametersMap()); @@ -447,7 +447,7 @@ class RTABMAP_EXP GFTT_BRIEF : public GFTT }; //GFTT_FREAK -class RTABMAP_EXP GFTT_FREAK : public GFTT +class RTABMAP_CORE_EXPORT GFTT_FREAK : public GFTT { public: GFTT_FREAK(const ParametersMap & parameters = ParametersMap()); @@ -469,7 +469,7 @@ class RTABMAP_EXP GFTT_FREAK : public GFTT }; //SURF_FREAK -class RTABMAP_EXP SURF_FREAK : public SURF +class RTABMAP_CORE_EXPORT SURF_FREAK : public SURF { public: SURF_FREAK(const ParametersMap & parameters = ParametersMap()); @@ -491,7 +491,7 @@ class RTABMAP_EXP SURF_FREAK : public SURF }; //GFTT_ORB -class RTABMAP_EXP GFTT_ORB : public GFTT +class RTABMAP_CORE_EXPORT GFTT_ORB : public GFTT { public: GFTT_ORB(const ParametersMap & parameters = ParametersMap()); @@ -508,7 +508,7 @@ class RTABMAP_EXP GFTT_ORB : public GFTT }; //BRISK -class RTABMAP_EXP BRISK : public Feature2D +class RTABMAP_CORE_EXPORT BRISK : public Feature2D { public: BRISK(const ParametersMap & parameters = ParametersMap()); @@ -530,7 +530,7 @@ class RTABMAP_EXP BRISK : public Feature2D }; //KAZE -class RTABMAP_EXP KAZE : public Feature2D +class RTABMAP_CORE_EXPORT KAZE : public Feature2D { public: KAZE(const ParametersMap & parameters = ParametersMap()); @@ -557,7 +557,7 @@ class RTABMAP_EXP KAZE : public Feature2D }; //ORB OCTREE -class RTABMAP_EXP ORBOctree : public Feature2D +class RTABMAP_CORE_EXPORT ORBOctree : public Feature2D { public: ORBOctree(const ParametersMap & parameters = ParametersMap()); @@ -583,7 +583,7 @@ class RTABMAP_EXP ORBOctree : public Feature2D }; //SuperPointTorch -class RTABMAP_EXP SuperPointTorch : public Feature2D +class RTABMAP_CORE_EXPORT SuperPointTorch : public Feature2D { public: SuperPointTorch(const ParametersMap & parameters = ParametersMap()); @@ -606,7 +606,7 @@ class RTABMAP_EXP SuperPointTorch : public Feature2D }; //GFTT_DAISY -class RTABMAP_EXP GFTT_DAISY : public GFTT +class RTABMAP_CORE_EXPORT GFTT_DAISY : public GFTT { public: GFTT_DAISY(const ParametersMap & parameters = ParametersMap()); @@ -630,7 +630,7 @@ class RTABMAP_EXP GFTT_DAISY : public GFTT }; //SURF_DAISY -class RTABMAP_EXP SURF_DAISY : public SURF +class RTABMAP_CORE_EXPORT SURF_DAISY : public SURF { public: SURF_DAISY(const ParametersMap & parameters = ParametersMap()); diff --git a/corelib/include/rtabmap/core/FlannIndex.h b/corelib/include/rtabmap/core/FlannIndex.h index 6552a07109..a1cd253e56 100644 --- a/corelib/include/rtabmap/core/FlannIndex.h +++ b/corelib/include/rtabmap/core/FlannIndex.h @@ -28,23 +28,23 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #ifndef CORELIB_SRC_FLANNINDEX_H_ #define CORELIB_SRC_FLANNINDEX_H_ -#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines +#include "rtabmap/core/rtabmap_core_export.h" // DLL export/import defines #include #include namespace rtabmap { -class RTABMAP_EXP FlannIndex +class RTABMAP_CORE_EXPORT FlannIndex { public: FlannIndex(); virtual ~FlannIndex(); void release(); - unsigned int indexedFeatures() const; + size_t indexedFeatures() const; // return Bytes - unsigned long memoryUsed() const; + size_t memoryUsed() const; // Note that useDistanceL1 doesn't have any effect if LSH is used void buildLinearIndex( diff --git a/corelib/include/rtabmap/core/GainCompensator.h b/corelib/include/rtabmap/core/GainCompensator.h index 27602344d0..ee4e3c6482 100644 --- a/corelib/include/rtabmap/core/GainCompensator.h +++ b/corelib/include/rtabmap/core/GainCompensator.h @@ -28,7 +28,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #ifndef CORELIB_SRC_GAINCOMPENSATOR_H_ #define CORELIB_SRC_GAINCOMPENSATOR_H_ -#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines +#include "rtabmap/core/rtabmap_core_export.h" // DLL export/import defines #include #include @@ -41,7 +41,7 @@ namespace rtabmap { /** * Works like cv::GainCompensator but with point clouds */ -class RTABMAP_EXP GainCompensator { +class RTABMAP_CORE_EXPORT GainCompensator { public: GainCompensator(double maxCorrespondenceDistance = 0.02, double minOverlap = 0.0, double alpha = 0.01, double beta = 10); virtual ~GainCompensator(); diff --git a/corelib/include/rtabmap/core/GeodeticCoords.h b/corelib/include/rtabmap/core/GeodeticCoords.h index 6234dd00a1..d2c67c59ec 100644 --- a/corelib/include/rtabmap/core/GeodeticCoords.h +++ b/corelib/include/rtabmap/core/GeodeticCoords.h @@ -43,13 +43,13 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #ifndef GEODETICCOORDS_H_ #define GEODETICCOORDS_H_ -#include +#include #include namespace rtabmap { -class RTABMAP_EXP GeodeticCoords +class RTABMAP_CORE_EXPORT GeodeticCoords { public: GeodeticCoords(); diff --git a/corelib/include/rtabmap/core/Graph.h b/corelib/include/rtabmap/core/Graph.h index f2d76acd4a..324323d252 100644 --- a/corelib/include/rtabmap/core/Graph.h +++ b/corelib/include/rtabmap/core/Graph.h @@ -28,7 +28,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #ifndef GRAPH_H_ #define GRAPH_H_ -#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines +#include "rtabmap/core/rtabmap_core_export.h" // DLL export/import defines #include #include @@ -46,7 +46,7 @@ namespace graph { // Graph utilities //////////////////////////////////////////// -bool RTABMAP_EXP exportPoses( +bool RTABMAP_CORE_EXPORT exportPoses( const std::string & filePath, int format, // 0=Raw (*.txt), 1=RGBD-SLAM motion capture (*.txt) (10=without change of coordinate frame, 11=10+ID), 2=KITTI (*.txt), 3=TORO (*.graph), 4=g2o (*.g2o) const std::map & poses, @@ -54,14 +54,14 @@ bool RTABMAP_EXP exportPoses( const std::map & stamps = std::map(), // required for format 1 const ParametersMap & parameters = ParametersMap()); // optional for formats 3 and 4 -bool RTABMAP_EXP importPoses( +bool RTABMAP_CORE_EXPORT importPoses( const std::string & filePath, int format, // 0=Raw, 1=RGBD-SLAM motion capture (10=without change of coordinate frame, 11=10+ID), 2=KITTI, 3=TORO, 4=g2o, 5=NewCollege(t,x,y), 6=Malaga Urban GPS, 7=St Lucia INS, 8=Karlsruhe, 9=EuRoC MAV std::map & poses, std::multimap * constraints = 0, // optional for formats 3 and 4 std::map * stamps = 0); // optional for format 1 and 9 -bool RTABMAP_EXP exportGPS( +bool RTABMAP_CORE_EXPORT exportGPS( const std::string & filePath, const std::map & gpsValues, unsigned int rgba = 0xFFFFFFFF); @@ -74,7 +74,7 @@ bool RTABMAP_EXP exportGPS( * @param t_err, Output translation error (%) * @param r_err, Output rotation error (deg/m) */ -void RTABMAP_EXP calcKittiSequenceErrors( +void RTABMAP_CORE_EXPORT calcKittiSequenceErrors( const std::vector &poses_gt, const std::vector &poses_result, float & t_err, @@ -87,7 +87,7 @@ void RTABMAP_EXP calcKittiSequenceErrors( * @param t_err, Output translation error (m) * @param r_err, Output rotation error (deg) */ -void RTABMAP_EXP calcRelativeErrors ( +void RTABMAP_CORE_EXPORT calcRelativeErrors ( const std::vector &poses_gt, const std::vector &poses_result, float & t_err, @@ -101,7 +101,7 @@ void RTABMAP_EXP calcRelativeErrors ( * @param poses, Estimated poses * @return Gt to Map transform */ -Transform RTABMAP_EXP calcRMSE( +Transform RTABMAP_CORE_EXPORT calcRMSE( const std::map &groundTruth, const std::map &poses, float & translational_rmse, @@ -117,7 +117,7 @@ Transform RTABMAP_EXP calcRMSE( float & rotational_min, float & rotational_max); -void RTABMAP_EXP computeMaxGraphErrors( +void RTABMAP_CORE_EXPORT computeMaxGraphErrors( const std::map & poses, const std::multimap & links, float & maxLinearErrorRatio, @@ -128,53 +128,65 @@ void RTABMAP_EXP computeMaxGraphErrors( const Link ** maxAngularErrorLink = 0, bool for3DoF = false); -std::vector RTABMAP_EXP getMaxOdomInf(const std::multimap & links); +std::vector RTABMAP_CORE_EXPORT getMaxOdomInf(const std::multimap & links); -std::multimap::iterator RTABMAP_EXP findLink( +std::multimap::iterator RTABMAP_CORE_EXPORT findLink( std::multimap & links, int from, int to, bool checkBothWays = true, Link::Type type = Link::kUndef); -std::multimap::iterator RTABMAP_EXP findLink( +std::multimap >::iterator RTABMAP_CORE_EXPORT findLink( + std::multimap > & links, + int from, + int to, + bool checkBothWays = true, + Link::Type type = Link::kUndef); +std::multimap::iterator RTABMAP_CORE_EXPORT findLink( std::multimap & links, int from, int to, bool checkBothWays = true); -std::multimap::const_iterator RTABMAP_EXP findLink( +std::multimap::const_iterator RTABMAP_CORE_EXPORT findLink( const std::multimap & links, int from, int to, bool checkBothWays = true, Link::Type type = Link::kUndef); -std::multimap::const_iterator RTABMAP_EXP findLink( +std::multimap >::const_iterator RTABMAP_CORE_EXPORT findLink( + const std::multimap > & links, + int from, + int to, + bool checkBothWays = true, + Link::Type type = Link::kUndef); +std::multimap::const_iterator RTABMAP_CORE_EXPORT findLink( const std::multimap & links, int from, int to, bool checkBothWays = true); -std::list RTABMAP_EXP findLinks( +std::list RTABMAP_CORE_EXPORT findLinks( const std::multimap & links, int from); -std::multimap RTABMAP_EXP filterDuplicateLinks( +std::multimap RTABMAP_CORE_EXPORT filterDuplicateLinks( const std::multimap & links); /** * Return links not of type "filteredType". If inverted=true, return links of type "filteredType". */ -std::multimap RTABMAP_EXP filterLinks( +std::multimap RTABMAP_CORE_EXPORT filterLinks( const std::multimap & links, Link::Type filteredType, bool inverted = false); /** * Return links not of type "filteredType". If inverted=true, return links of type "filteredType". */ -std::map RTABMAP_EXP filterLinks( +std::map RTABMAP_CORE_EXPORT filterLinks( const std::map & links, Link::Type filteredType, bool inverted = false); //Note: This assumes a coordinate system where X is forward, * Y is up, and Z is right. -std::map RTABMAP_EXP frustumPosesFiltering( +std::map RTABMAP_CORE_EXPORT frustumPosesFiltering( const std::map & poses, const Transform & cameraPose, float horizontalFOV = 45.0f, // in degrees, xfov = atan((image_width/2)/fx)*2 @@ -191,7 +203,7 @@ std::map RTABMAP_EXP frustumPosesFiltering( * @param keepLatest keep the latest node if true, otherwise the oldest node is kept * @return A map containing only most recent or older poses in the the defined radius */ -std::map RTABMAP_EXP radiusPosesFiltering( +std::map RTABMAP_CORE_EXPORT radiusPosesFiltering( const std::map & poses, float radius, float angle, @@ -204,7 +216,7 @@ std::map RTABMAP_EXP radiusPosesFiltering( * @param angle Maximum angle (rad, [0,PI]) of accepted neighbor nodes in the radius (0 means ignore angle) * @return A map between each pose id and its neighbors found in the radius */ -std::multimap RTABMAP_EXP radiusPosesClustering( +std::multimap RTABMAP_CORE_EXPORT radiusPosesClustering( const std::map & poses, float radius, float angle); @@ -224,7 +236,7 @@ void reduceGraph( * @param updateNewCosts Keep up-to-date costs while traversing the graph. * @return the path ids from id "from" to id "to" including initial and final nodes. */ -std::list > RTABMAP_EXP computePath( +std::list > RTABMAP_CORE_EXPORT computePath( const std::map & poses, const std::multimap & links, int from, @@ -241,7 +253,7 @@ std::list > RTABMAP_EXP computePath( * @param useSameCostForAllLinks Ignore distance between nodes * @return the path ids from id "from" to id "to" including initial and final nodes. */ -std::list RTABMAP_EXP computePath( +std::list RTABMAP_CORE_EXPORT computePath( const std::multimap & links, int from, int to, @@ -257,7 +269,7 @@ std::list RTABMAP_EXP computePath( * @param updateNewCosts Keep up-to-date costs while traversing the graph. * @return the path ids from id "fromId" to id "toId" including initial and final nodes (Identity pose for the first node). */ -std::list > RTABMAP_EXP computePath( +std::list > RTABMAP_CORE_EXPORT computePath( int fromId, int toId, const Memory * memory, @@ -273,7 +285,7 @@ std::list > RTABMAP_EXP computePath( * @param distance squared distance of the nearest node found (optional) * @return the node id. */ -int RTABMAP_EXP findNearestNode( +int RTABMAP_CORE_EXPORT findNearestNode( const std::map & poses, const rtabmap::Transform & targetPose, float * distance = 0); @@ -286,54 +298,56 @@ int RTABMAP_EXP findNearestNode( * @param k max nearest neighbors (0=all inside the radius) * @return the nodes with squared distance to query node. */ -std::map RTABMAP_EXP findNearestNodes( +std::map RTABMAP_CORE_EXPORT findNearestNodes( int nodeId, const std::map & poses, float radius, float angle = 0.0f, int k=0); -std::map RTABMAP_EXP findNearestNodes( +std::map RTABMAP_CORE_EXPORT findNearestNodes( const Transform & targetPose, const std::map & poses, float radius, float angle = 0.0f, int k=0); -std::map RTABMAP_EXP findNearestPoses( +std::map RTABMAP_CORE_EXPORT findNearestPoses( int nodeId, const std::map & poses, float radius, float angle = 0.0f, int k=0); -std::map RTABMAP_EXP findNearestPoses( +std::map RTABMAP_CORE_EXPORT findNearestPoses( const Transform & targetPose, const std::map & poses, float radius, float angle = 0.0f, int k=0); -// typedef hack to avoid error with RTABMAP_DEPRECATED -typedef std::map _mapIntFloat; -typedef std::map _mapIntTransform; -RTABMAP_DEPRECATED(_mapIntFloat RTABMAP_EXP findNearestNodes(const std::map & nodes, const rtabmap::Transform & targetPose, int k), "Use new findNearestNodes() interface with radius=0, angle=0."); -RTABMAP_DEPRECATED(_mapIntFloat RTABMAP_EXP getNodesInRadius(int nodeId, const std::map & nodes, float radius), "Renamed to findNearestNodes()"); -RTABMAP_DEPRECATED(_mapIntFloat RTABMAP_EXP getNodesInRadius(const Transform & targetPose, const std::map & nodes, float radius), "Renamed to findNearestNodes()"); -RTABMAP_DEPRECATED(_mapIntTransform RTABMAP_EXP getPosesInRadius(int nodeId, const std::map & nodes, float radius, float angle = 0.0f), "Renamed to findNearestNodes()"); -RTABMAP_DEPRECATED(_mapIntTransform RTABMAP_EXP getPosesInRadius(const Transform & targetPose, const std::map & nodes, float radius, float angle = 0.0f), "Renamed to findNearestNodes()"); - -float RTABMAP_EXP computePathLength( +// Use new findNearestNodes() interface with radius=0, angle=0. +RTABMAP_DEPRECATED std::map RTABMAP_CORE_EXPORT findNearestNodes(const std::map & nodes, const rtabmap::Transform & targetPose, int k); +// Renamed to findNearestNodes() +RTABMAP_DEPRECATED std::map RTABMAP_CORE_EXPORT getNodesInRadius(int nodeId, const std::map & nodes, float radius); +// Renamed to findNearestNodes() +RTABMAP_DEPRECATED std::map RTABMAP_CORE_EXPORT getNodesInRadius(const Transform & targetPose, const std::map & nodes, float radius); +// Renamed to findNearestNodes() +RTABMAP_DEPRECATED std::map RTABMAP_CORE_EXPORT getPosesInRadius(int nodeId, const std::map & nodes, float radius, float angle = 0.0f); +// Renamed to findNearestNodes() +RTABMAP_DEPRECATED std::map RTABMAP_CORE_EXPORT getPosesInRadius(const Transform & targetPose, const std::map & nodes, float radius, float angle = 0.0f); + +float RTABMAP_CORE_EXPORT computePathLength( const std::vector > & path, unsigned int fromIndex = 0, unsigned int toIndex = 0); // assuming they are all linked in map order -float RTABMAP_EXP computePathLength( +float RTABMAP_CORE_EXPORT computePathLength( const std::map & path); -std::list > RTABMAP_EXP getPaths( +std::list > RTABMAP_CORE_EXPORT getPaths( std::map poses, const std::multimap & links); -void RTABMAP_EXP computeMinMax(const std::map & poses, +void RTABMAP_CORE_EXPORT computeMinMax(const std::map & poses, cv::Vec3f & min, cv::Vec3f & max); diff --git a/corelib/include/rtabmap/core/IMUThread.h b/corelib/include/rtabmap/core/IMUThread.h index 74d1872743..a7cc834770 100644 --- a/corelib/include/rtabmap/core/IMUThread.h +++ b/corelib/include/rtabmap/core/IMUThread.h @@ -27,9 +27,10 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #pragma once -#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines +#include "rtabmap/core/rtabmap_core_export.h" // DLL export/import defines #include +#include #include #include #include @@ -39,11 +40,13 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. namespace rtabmap { +class IMUFilter; + /** * Class IMUThread * */ -class RTABMAP_EXP IMUThread : +class RTABMAP_CORE_EXPORT IMUThread : public UThread, public UEventsSender { @@ -53,6 +56,8 @@ class RTABMAP_EXP IMUThread : bool init(const std::string & path); void setRate(int rate); + void enableIMUFiltering(int filteringStrategy=1, const ParametersMap & parameters = ParametersMap(), bool baseFrameConversion = false); + void disableIMUFiltering(); private: virtual void mainLoopBegin(); @@ -65,6 +70,8 @@ class RTABMAP_EXP IMUThread : UTimer frameRateTimer_; double captureDelay_; double previousStamp_; + IMUFilter * _imuFilter; + bool _imuBaseFrameConversion; }; } // namespace rtabmap diff --git a/corelib/include/rtabmap/core/Landmark.h b/corelib/include/rtabmap/core/Landmark.h index d28be62ca4..8762a409a0 100644 --- a/corelib/include/rtabmap/core/Landmark.h +++ b/corelib/include/rtabmap/core/Landmark.h @@ -28,7 +28,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #ifndef CORELIB_INCLUDE_RTABMAP_CORE_LANDMARK_H_ #define CORELIB_INCLUDE_RTABMAP_CORE_LANDMARK_H_ -#include +#include #include #include #include @@ -59,7 +59,8 @@ class Landmark UASSERT_MSG(uIsFinite(covariance_.at(4,4)) && covariance_.at(4,4)>0, uFormat("Angular covariance should not be null! Value=%f (set to 9999 if unknown).", covariance_.at(4,4)).c_str()); UASSERT_MSG(uIsFinite(covariance_.at(5,5)) && covariance_.at(5,5)>0, uFormat("Angular covariance should not be null! Value=%f (set to 9999 if unknown).", covariance_.at(5,5)).c_str()); } - RTABMAP_DEPRECATED(Landmark(const int & id, const Transform & pose, const cv::Mat & covariance), "Use constructor with size=0 instead."); + // Use constructor with size=0 instead. + RTABMAP_DEPRECATED Landmark(const int & id, const Transform & pose, const cv::Mat & covariance); virtual ~Landmark() {} diff --git a/corelib/include/rtabmap/core/LaserScan.h b/corelib/include/rtabmap/core/LaserScan.h index 0714c2fcdd..4abda58e3f 100644 --- a/corelib/include/rtabmap/core/LaserScan.h +++ b/corelib/include/rtabmap/core/LaserScan.h @@ -28,13 +28,13 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #ifndef CORELIB_INCLUDE_RTABMAP_CORE_LASERSCAN_H_ #define CORELIB_INCLUDE_RTABMAP_CORE_LASERSCAN_H_ -#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines +#include "rtabmap/core/rtabmap_core_export.h" // DLL export/import defines #include namespace rtabmap { -class RTABMAP_EXP LaserScan +class RTABMAP_CORE_EXPORT LaserScan { public: enum Format{kUnknown=0, @@ -77,24 +77,26 @@ class RTABMAP_EXP LaserScan int maxPoints, float maxRange, const Transform & localTransform = Transform::getIdentity()); - RTABMAP_DEPRECATED(LaserScan(const LaserScan & data, + // Use version without \"format\" argument. + RTABMAP_DEPRECATED LaserScan(const LaserScan & data, int maxPoints, float maxRange, Format format, - const Transform & localTransform = Transform::getIdentity()), "Use version without \"format\" argument."); + const Transform & localTransform = Transform::getIdentity()); LaserScan(const cv::Mat & data, int maxPoints, float maxRange, Format format, const Transform & localTransform = Transform::getIdentity()); - RTABMAP_DEPRECATED(LaserScan(const LaserScan & data, + // Use version without \"format\" argument. + RTABMAP_DEPRECATED LaserScan(const LaserScan & data, Format format, float minRange, float maxRange, float angleMin, float angleMax, float angleIncrement, - const Transform & localTransform = Transform::getIdentity()), "Use version without \"format\" argument."); + const Transform & localTransform = Transform::getIdentity()); LaserScan(const LaserScan & data, float minRange, float maxRange, diff --git a/corelib/include/rtabmap/core/Link.h b/corelib/include/rtabmap/core/Link.h index 47c436b274..d876854a0c 100644 --- a/corelib/include/rtabmap/core/Link.h +++ b/corelib/include/rtabmap/core/Link.h @@ -28,14 +28,14 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #ifndef LINK_H_ #define LINK_H_ -#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines +#include "rtabmap/core/rtabmap_core_export.h" // DLL export/import defines #include #include namespace rtabmap { -class RTABMAP_EXP Link +class RTABMAP_CORE_EXPORT Link { public: enum Type { diff --git a/corelib/include/rtabmap/core/MarkerDetector.h b/corelib/include/rtabmap/core/MarkerDetector.h index ed8dc3837e..d628ca022d 100644 --- a/corelib/include/rtabmap/core/MarkerDetector.h +++ b/corelib/include/rtabmap/core/MarkerDetector.h @@ -56,19 +56,20 @@ class MarkerInfo { Transform pose_; }; -class RTABMAP_EXP MarkerDetector { +class RTABMAP_CORE_EXPORT MarkerDetector { public: MarkerDetector(const ParametersMap & parameters = ParametersMap()); virtual ~MarkerDetector(); void parseParameters(const ParametersMap & parameters); - RTABMAP_DEPRECATED( + // Use the other detect(), in which the returned map contains the length of each marker detected. + RTABMAP_DEPRECATED MapIdPose detect(const cv::Mat & image, const CameraModel & model, const cv::Mat & depth = cv::Mat(), float * estimatedMarkerLength = 0, - cv::Mat * imageWithDetections = 0), "Use the other detect(), in which the returned map contains the length of each marker detected."); + cv::Mat * imageWithDetections = 0); std::map detect(const cv::Mat & image, const std::vector & models, diff --git a/corelib/include/rtabmap/core/Memory.h b/corelib/include/rtabmap/core/Memory.h index a9c6e743df..a0f86e89a3 100644 --- a/corelib/include/rtabmap/core/Memory.h +++ b/corelib/include/rtabmap/core/Memory.h @@ -28,7 +28,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #ifndef MEMORY_H_ #define MEMORY_H_ -#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines +#include "rtabmap/core/rtabmap_core_export.h" // DLL export/import defines #include "rtabmap/utilite/UEventsHandler.h" #include "rtabmap/core/Parameters.h" @@ -60,7 +60,7 @@ class Stereo; class OccupancyGrid; class MarkerDetector; -class RTABMAP_EXP Memory +class RTABMAP_CORE_EXPORT Memory { public: static const int kIdStart; diff --git a/corelib/include/rtabmap/core/OccupancyGrid.h b/corelib/include/rtabmap/core/OccupancyGrid.h index 8e247c33f2..04617c4bfe 100644 --- a/corelib/include/rtabmap/core/OccupancyGrid.h +++ b/corelib/include/rtabmap/core/OccupancyGrid.h @@ -28,7 +28,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #ifndef CORELIB_SRC_OCCUPANCYGRID_H_ #define CORELIB_SRC_OCCUPANCYGRID_H_ -#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines +#include "rtabmap/core/rtabmap_core_export.h" // DLL export/import defines #include #include @@ -37,7 +37,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. namespace rtabmap { -class RTABMAP_EXP OccupancyGrid +class RTABMAP_CORE_EXPORT OccupancyGrid { public: inline static float logodds(double probability) diff --git a/corelib/include/rtabmap/core/OctoMap.h b/corelib/include/rtabmap/core/OctoMap.h index b6252df857..770177175f 100644 --- a/corelib/include/rtabmap/core/OctoMap.h +++ b/corelib/include/rtabmap/core/OctoMap.h @@ -28,7 +28,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #ifndef SRC_OCTOMAP_H_ #define SRC_OCTOMAP_H_ -#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines +#include "rtabmap/core/rtabmap_core_export.h" // DLL export/import defines #include #include @@ -171,7 +171,7 @@ class RtabmapColorOcTree : public octomap::OccupancyOcTreeBase +#include #include #include @@ -39,7 +39,7 @@ namespace rtabmap { class OdometryInfo; class ParticleFilter; -class RTABMAP_EXP Odometry +class RTABMAP_CORE_EXPORT Odometry { public: enum Type { @@ -75,7 +75,8 @@ class RTABMAP_EXP Odometry //getters const Transform & getPose() const {return _pose;} bool isInfoDataFilled() const {return _fillInfoData;} - RTABMAP_DEPRECATED(const Transform & previousVelocityTransform() const, "Use getVelocityGuess() instead."); + // Use getVelocityGuess() instead. + RTABMAP_DEPRECATED const Transform & previousVelocityTransform() const; const Transform & getVelocityGuess() const {return velocityGuess_;} double previousStamp() const {return previousStamp_;} unsigned int framesProcessed() const {return framesProcessed_;} diff --git a/corelib/include/rtabmap/core/OdometryThread.h b/corelib/include/rtabmap/core/OdometryThread.h index 01424134a7..159ee55f98 100644 --- a/corelib/include/rtabmap/core/OdometryThread.h +++ b/corelib/include/rtabmap/core/OdometryThread.h @@ -28,7 +28,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #ifndef ODOMETRYTHREAD_H_ #define ODOMETRYTHREAD_H_ -#include +#include #include #include #include @@ -38,7 +38,7 @@ namespace rtabmap { class Odometry; -class RTABMAP_EXP OdometryThread : public UThread, public UEventsHandler { +class RTABMAP_CORE_EXPORT OdometryThread : public UThread, public UEventsHandler { public: // take ownership of Odometry OdometryThread(Odometry * odometry, unsigned int dataBufferMaxSize = 1); diff --git a/corelib/include/rtabmap/core/Optimizer.h b/corelib/include/rtabmap/core/Optimizer.h index 32729d1468..23051312ee 100644 --- a/corelib/include/rtabmap/core/Optimizer.h +++ b/corelib/include/rtabmap/core/Optimizer.h @@ -28,7 +28,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #ifndef OPTIMIZER_H_ #define OPTIMIZER_H_ -#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines +#include "rtabmap/core/rtabmap_core_export.h" // DLL export/import defines #include #include @@ -58,7 +58,7 @@ class FeatureBA //////////////////////////////////////////// // Graph optimizers //////////////////////////////////////////// -class RTABMAP_EXP Optimizer +class RTABMAP_CORE_EXPORT Optimizer { public: enum Type { @@ -79,8 +79,7 @@ class RTABMAP_EXP Optimizer const std::map & posesIn, const std::multimap & linksIn, std::map & posesOut, - std::multimap & linksOut, - bool adjustPosesWithConstraints = true) const; + std::multimap & linksOut) const; public: virtual ~Optimizer() {} diff --git a/corelib/include/rtabmap/core/Parameters.h b/corelib/include/rtabmap/core/Parameters.h index 5906e32dca..a7d9b43fa6 100644 --- a/corelib/include/rtabmap/core/Parameters.h +++ b/corelib/include/rtabmap/core/Parameters.h @@ -29,7 +29,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #define PARAMETERS_H_ // default parameters -#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines +#include "rtabmap/core/rtabmap_core_export.h" // DLL export/import defines #include "rtabmap/core/Version.h" // DLL export/import defines #include #include @@ -167,7 +167,7 @@ typedef std::pair ParametersPair; * @see getDefaultParameters() * TODO Add a detailed example with simple classes */ -class RTABMAP_EXP Parameters +class RTABMAP_CORE_EXPORT Parameters { // Rtabmap parameters RTABMAP_PARAM(Rtabmap, PublishStats, bool, true, "Publishing statistics."); @@ -376,6 +376,8 @@ class RTABMAP_EXP Parameters RTABMAP_PARAM(RGBD, MarkerDetection, bool, false, "Detect static markers to be added as landmarks for graph optimization. If input data have already landmarks, this will be ignored. See \"Marker\" group for parameters."); RTABMAP_PARAM(RGBD, LoopCovLimited, bool, false, "Limit covariance of non-neighbor links to minimum covariance of neighbor links. In other words, if covariance of a loop closure link is smaller than the minimum covariance of odometry links, its covariance is set to minimum covariance of odometry links."); RTABMAP_PARAM(RGBD, MaxOdomCacheSize, int, 10, uFormat("Maximum odometry cache size. Used only in localization mode (when %s=false). This is used to get smoother localizations and to verify localization transforms (when %s!=0) to make sure we don't teleport to a location very similar to one we previously localized on. Set 0 to disable caching.", kMemIncrementalMemory().c_str(), kRGBDOptimizeMaxError().c_str())); + RTABMAP_PARAM(RGBD, LocalizationSmoothing, bool, true, uFormat("Adjust localization constraints based on optimized odometry cache poses (when %s>0).", kRGBDMaxOdomCacheSize().c_str())); + RTABMAP_PARAM(RGBD, LocalizationPriorError, double, 0.001, uFormat("The corresponding variance (error x error) set to priors of the map's poses during localization (when %s>0).", kRGBDMaxOdomCacheSize().c_str())); // Local/Proximity loop closure detection RTABMAP_PARAM(RGBD, ProximityByTime, bool, false, "Detection over all locations in STM."); @@ -526,12 +528,19 @@ class RTABMAP_EXP 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."); @@ -576,6 +585,68 @@ class RTABMAP_EXP Parameters // Odometry VINS RTABMAP_PARAM_STR(OdomVINS, ConfigPath, "", "Path of VINS config file."); + // Odometry OpenVINS + RTABMAP_PARAM(OdomOpenVINS, UseStereo, bool, true, "If we have more than 1 camera, if we should try to track stereo constraints between pairs"); + RTABMAP_PARAM(OdomOpenVINS, UseKLT, bool, true, "If true we will use KLT, otherwise use a ORB descriptor + robust matching"); + RTABMAP_PARAM(OdomOpenVINS, NumPts, int, 200, "Number of points (per camera) we will extract and try to track"); + RTABMAP_PARAM(OdomOpenVINS, MinPxDist, int, 15, "Eistance between features (features near each other provide less information)"); + RTABMAP_PARAM(OdomOpenVINS, FiTriangulate1d, bool, false, "If we should perform 1d triangulation instead of 3d"); + RTABMAP_PARAM(OdomOpenVINS, FiRefineFeatures, bool, true, "If we should perform Levenberg-Marquardt refinement"); + RTABMAP_PARAM(OdomOpenVINS, FiMaxRuns, int, 5, "Max runs for Levenberg-Marquardt"); + RTABMAP_PARAM(OdomOpenVINS, FiMaxBaseline, double, 40, "Max baseline ratio to accept triangulated features"); + RTABMAP_PARAM(OdomOpenVINS, FiMaxCondNumber, double, 10000, "Max condition number of linear triangulation matrix accept triangulated features"); + + RTABMAP_PARAM(OdomOpenVINS, UseFEJ, bool, true, "If first-estimate Jacobians should be used (enable for good consistency)"); + RTABMAP_PARAM(OdomOpenVINS, Integration, int, 1, "0=discrete, 1=rk4, 2=analytical (if rk4 or analytical used then analytical covariance propagation is used)"); + RTABMAP_PARAM(OdomOpenVINS, CalibCamExtrinsics, bool, false, "Bool to determine whether or not to calibrate imu-to-camera pose"); + RTABMAP_PARAM(OdomOpenVINS, CalibCamIntrinsics, bool, false, "Bool to determine whether or not to calibrate camera intrinsics"); + RTABMAP_PARAM(OdomOpenVINS, CalibCamTimeoffset, bool, false, "Bool to determine whether or not to calibrate camera to IMU time offset"); + RTABMAP_PARAM(OdomOpenVINS, CalibIMUIntrinsics, bool, false, "Bool to determine whether or not to calibrate the IMU intrinsics"); + RTABMAP_PARAM(OdomOpenVINS, CalibIMUGSensitivity, bool, false, "Bool to determine whether or not to calibrate the Gravity sensitivity"); + RTABMAP_PARAM(OdomOpenVINS, MaxClones, int, 11, "Max clone size of sliding window"); + RTABMAP_PARAM(OdomOpenVINS, MaxSLAM, int, 50, "Max number of estimated SLAM features"); + RTABMAP_PARAM(OdomOpenVINS, MaxSLAMInUpdate, int, 25, "Max number of SLAM features we allow to be included in a single EKF update."); + RTABMAP_PARAM(OdomOpenVINS, MaxMSCKFInUpdate, int, 50, "Max number of MSCKF features we will use at a given image timestep."); + RTABMAP_PARAM(OdomOpenVINS, FeatRepMSCKF, int, 0, "What representation our features are in (msckf features)"); + RTABMAP_PARAM(OdomOpenVINS, FeatRepSLAM, int, 4, "What representation our features are in (slam features)"); + RTABMAP_PARAM(OdomOpenVINS, DtSLAMDelay, double, 0.0, "Delay, in seconds, that we should wait from init before we start estimating SLAM features"); + RTABMAP_PARAM(OdomOpenVINS, GravityMag, double, 9.81, "Gravity magnitude in the global frame (i.e. should be 9.81 typically)"); + RTABMAP_PARAM_STR(OdomOpenVINS, LeftMaskPath, "", "Mask for left image"); + RTABMAP_PARAM_STR(OdomOpenVINS, RightMaskPath, "", "Mask for right image"); + + RTABMAP_PARAM(OdomOpenVINS, InitWindowTime, double, 2.0, "Amount of time we will initialize over (seconds)"); + RTABMAP_PARAM(OdomOpenVINS, InitIMUThresh, double, 1.0, "Variance threshold on our acceleration to be classified as moving"); + RTABMAP_PARAM(OdomOpenVINS, InitMaxDisparity, double, 10.0, "Max disparity to consider the platform stationary (dependent on resolution)"); + RTABMAP_PARAM(OdomOpenVINS, InitMaxFeatures, int, 50, "How many features to track during initialization (saves on computation)"); + RTABMAP_PARAM(OdomOpenVINS, InitDynUse, bool, false, "If dynamic initialization should be used"); + RTABMAP_PARAM(OdomOpenVINS, InitDynMLEOptCalib, bool, false, "If we should optimize calibration during intialization (not recommended)"); + RTABMAP_PARAM(OdomOpenVINS, InitDynMLEMaxIter, int, 50, "How many iterations the MLE refinement should use (zero to skip the MLE)"); + RTABMAP_PARAM(OdomOpenVINS, InitDynMLEMaxTime, double, 0.05, "How many seconds the MLE should be completed in"); + RTABMAP_PARAM(OdomOpenVINS, InitDynMLEMaxThreads, int, 6, "How many threads the MLE should use"); + RTABMAP_PARAM(OdomOpenVINS, InitDynNumPose, int, 6, "Number of poses to use within our window time (evenly spaced)"); + RTABMAP_PARAM(OdomOpenVINS, InitDynMinDeg, double, 10.0, "Orientation change needed to try to init"); + RTABMAP_PARAM(OdomOpenVINS, InitDynInflationOri, double, 10.0, "What to inflate the recovered q_GtoI covariance by"); + RTABMAP_PARAM(OdomOpenVINS, InitDynInflationVel, double, 100.0, "What to inflate the recovered v_IinG covariance by"); + RTABMAP_PARAM(OdomOpenVINS, InitDynInflationBg, double, 10.0, "What to inflate the recovered bias_g covariance by"); + RTABMAP_PARAM(OdomOpenVINS, InitDynInflationBa, double, 100.0, "What to inflate the recovered bias_a covariance by"); + RTABMAP_PARAM(OdomOpenVINS, InitDynMinRecCond, double, 1e-15, "Reciprocal condition number thresh for info inversion"); + + RTABMAP_PARAM(OdomOpenVINS, TryZUPT, bool, true, "If we should try to use zero velocity update"); + RTABMAP_PARAM(OdomOpenVINS, ZUPTChi2Multiplier, double, 0.0, "Chi2 multiplier for zero velocity"); + RTABMAP_PARAM(OdomOpenVINS, ZUPTMaxVelodicy, double, 0.1, "Max velocity we will consider to try to do a zupt (i.e. if above this, don't do zupt)"); + RTABMAP_PARAM(OdomOpenVINS, ZUPTNoiseMultiplier, double, 10.0, "Multiplier of our zupt measurement IMU noise matrix (default should be 1.0)"); + RTABMAP_PARAM(OdomOpenVINS, ZUPTMaxDisparity, double, 0.5, "Max disparity we will consider to try to do a zupt (i.e. if above this, don't do zupt)"); + RTABMAP_PARAM(OdomOpenVINS, ZUPTOnlyAtBeginning, bool, false, "If we should only use the zupt at the very beginning static initialization phase"); + + RTABMAP_PARAM(OdomOpenVINS, AccelerometerNoiseDensity, double, 0.01, "[m/s^2/sqrt(Hz)] (accel \"white noise\")"); + RTABMAP_PARAM(OdomOpenVINS, AccelerometerRandomWalk, double, 0.001, "[m/s^3/sqrt(Hz)] (accel bias diffusion)"); + RTABMAP_PARAM(OdomOpenVINS, GyroscopeNoiseDensity, double, 0.001, "[rad/s/sqrt(Hz)] (gyro \"white noise\")"); + RTABMAP_PARAM(OdomOpenVINS, GyroscopeRandomWalk, double, 0.0001, "[rad/s^2/sqrt(Hz)] (gyro bias diffusion)"); + RTABMAP_PARAM(OdomOpenVINS, UpMSCKFSigmaPx, double, 2.0, "Pixel noise for MSCKF features"); + RTABMAP_PARAM(OdomOpenVINS, UpMSCKFChi2Multiplier, double, 1.0, "Chi2 multiplier for MSCKF features"); + RTABMAP_PARAM(OdomOpenVINS, UpSLAMSigmaPx, double, 2.0, "Pixel noise for SLAM features"); + RTABMAP_PARAM(OdomOpenVINS, UpSLAMChi2Multiplier, double, 1.0, "Chi2 multiplier for SLAM features"); + // Odometry Open3D RTABMAP_PARAM(OdomOpen3D, MaxDepth, float, 3.0, "Maximum depth."); RTABMAP_PARAM(OdomOpen3D, Method, int, 0, "Registration method: 0=PointToPlane, 1=Intensity, 2=Hybrid."); @@ -586,54 +657,56 @@ class RTABMAP_EXP Parameters RTABMAP_PARAM(Reg, Force3DoF, bool, false, "Force 3 degrees-of-freedom transform (3Dof: x,y and yaw). Parameters z, roll and pitch will be set to 0."); // Visual registration parameters - RTABMAP_PARAM(Vis, EstimationType, int, 1, "Motion estimation approach: 0:3D->3D, 1:3D->2D (PnP), 2:2D->2D (Epipolar Geometry)"); - RTABMAP_PARAM(Vis, ForwardEstOnly, bool, true, "Forward estimation only (A->B). If false, a transformation is also computed in backward direction (B->A), then the two resulting transforms are merged (middle interpolation between the transforms)."); - RTABMAP_PARAM(Vis, InlierDistance, float, 0.1, uFormat("[%s = 0] Maximum distance for feature correspondences. Used by 3D->3D estimation approach.", kVisEstimationType().c_str())); - RTABMAP_PARAM(Vis, RefineIterations, int, 5, uFormat("[%s = 0] Number of iterations used to refine the transformation found by RANSAC. 0 means that the transformation is not refined.", kVisEstimationType().c_str())); - RTABMAP_PARAM(Vis, PnPReprojError, float, 2, uFormat("[%s = 1] PnP reprojection error.", kVisEstimationType().c_str())); - RTABMAP_PARAM(Vis, PnPFlags, int, 0, uFormat("[%s = 1] PnP flags: 0=Iterative, 1=EPNP, 2=P3P", kVisEstimationType().c_str())); + RTABMAP_PARAM(Vis, EstimationType, int, 1, "Motion estimation approach: 0:3D->3D, 1:3D->2D (PnP), 2:2D->2D (Epipolar Geometry)"); + RTABMAP_PARAM(Vis, ForwardEstOnly, bool, true, "Forward estimation only (A->B). If false, a transformation is also computed in backward direction (B->A), then the two resulting transforms are merged (middle interpolation between the transforms)."); + RTABMAP_PARAM(Vis, InlierDistance, float, 0.1, uFormat("[%s = 0] Maximum distance for feature correspondences. Used by 3D->3D estimation approach.", kVisEstimationType().c_str())); + RTABMAP_PARAM(Vis, RefineIterations, int, 5, uFormat("[%s = 0] Number of iterations used to refine the transformation found by RANSAC. 0 means that the transformation is not refined.", kVisEstimationType().c_str())); + RTABMAP_PARAM(Vis, PnPReprojError, float, 2, uFormat("[%s = 1] PnP reprojection error.", kVisEstimationType().c_str())); + RTABMAP_PARAM(Vis, PnPFlags, int, 0, uFormat("[%s = 1] PnP flags: 0=Iterative, 1=EPNP, 2=P3P", kVisEstimationType().c_str())); #if defined(RTABMAP_G2O) || defined(RTABMAP_ORB_SLAM) - RTABMAP_PARAM(Vis, PnPRefineIterations, int, 0, uFormat("[%s = 1] Refine iterations. Set to 0 if \"%s\" is also used.", kVisEstimationType().c_str(), kVisBundleAdjustment().c_str())); + RTABMAP_PARAM(Vis, PnPRefineIterations, int, 0, uFormat("[%s = 1] Refine iterations. Set to 0 if \"%s\" is also used.", kVisEstimationType().c_str(), kVisBundleAdjustment().c_str())); #else - RTABMAP_PARAM(Vis, PnPRefineIterations, int, 1, uFormat("[%s = 1] Refine iterations. Set to 0 if \"%s\" is also used.", kVisEstimationType().c_str(), kVisBundleAdjustment().c_str())); + RTABMAP_PARAM(Vis, PnPRefineIterations, int, 1, uFormat("[%s = 1] Refine iterations. Set to 0 if \"%s\" is also used.", kVisEstimationType().c_str(), kVisBundleAdjustment().c_str())); #endif - RTABMAP_PARAM(Vis, PnPMaxVariance, float, 0.0, uFormat("[%s = 1] Max linear variance between 3D point correspondences after PnP. 0 means disabled.", kVisEstimationType().c_str())); + RTABMAP_PARAM(Vis, PnPVarianceMedianRatio, int, 4, uFormat("[%s = 1] Ratio used to compute variance of the estimated transformation if 3D correspondences are provided (should be > 1). The higher it is, the smaller the covariance will be. With accurate depth estimation, this could be set to 2. For depth estimated by stereo, 4 or more maybe used to ignore large errors of very far points.", kVisEstimationType().c_str())); + RTABMAP_PARAM(Vis, PnPMaxVariance, float, 0.0, uFormat("[%s = 1] Max linear variance between 3D point correspondences after PnP. 0 means disabled.", kVisEstimationType().c_str())); + RTABMAP_PARAM(Vis, PnPSamplingPolicy, unsigned int, 1, uFormat("[%s = 1] Multi-camera random sampling policy: 0=AUTO, 1=ANY, 2=HOMOGENEOUS. With HOMOGENEOUS policy, RANSAC will be done uniformly against all cameras, so at least 2 matches per camera are required. With ANY policy, RANSAC is not constraint to sample on all cameras at the same time. AUTO policy will use HOMOGENEOUS if there are at least 2 matches per camera, otherwise it will fallback to ANY policy.", kVisEstimationType().c_str()).c_str()); - RTABMAP_PARAM(Vis, EpipolarGeometryVar, float, 0.1, uFormat("[%s = 2] Epipolar geometry maximum variance to accept the transformation.", kVisEstimationType().c_str())); - RTABMAP_PARAM(Vis, MinInliers, int, 20, "Minimum feature correspondences to compute/accept the transformation."); - RTABMAP_PARAM(Vis, MeanInliersDistance, float, 0.0, "Maximum distance (m) of the mean distance of inliers from the camera to accept the transformation. 0 means disabled."); - RTABMAP_PARAM(Vis, MinInliersDistribution, float, 0.0, "Minimum distribution value of the inliers in the image to accept the transformation. The distribution is the second eigen value of the PCA (Principal Component Analysis) on the keypoints of the normalized image [-0.5, 0.5]. The value would be between 0 and 0.5. 0 means disabled."); + RTABMAP_PARAM(Vis, EpipolarGeometryVar, float, 0.1, uFormat("[%s = 2] Epipolar geometry maximum variance to accept the transformation.", kVisEstimationType().c_str())); + RTABMAP_PARAM(Vis, MinInliers, int, 20, "Minimum feature correspondences to compute/accept the transformation."); + RTABMAP_PARAM(Vis, MeanInliersDistance, float, 0.0, "Maximum distance (m) of the mean distance of inliers from the camera to accept the transformation. 0 means disabled."); + RTABMAP_PARAM(Vis, MinInliersDistribution, float, 0.0, "Minimum distribution value of the inliers in the image to accept the transformation. The distribution is the second eigen value of the PCA (Principal Component Analysis) on the keypoints of the normalized image [-0.5, 0.5]. The value would be between 0 and 0.5. 0 means disabled."); - RTABMAP_PARAM(Vis, Iterations, int, 300, "Maximum iterations to compute the transform."); + RTABMAP_PARAM(Vis, Iterations, int, 300, "Maximum iterations to compute the transform."); #if CV_MAJOR_VERSION > 2 && !defined(HAVE_OPENCV_XFEATURES2D) // OpenCV>2 without xFeatures2D module doesn't have BRIEF RTABMAP_PARAM(Vis, FeatureType, int, 8, "0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK 8=GFTT/ORB 9=KAZE 10=ORB-OCTREE 11=SuperPoint 12=SURF/FREAK 13=GFTT/DAISY 14=SURF/DAISY 15=PyDetector"); #else RTABMAP_PARAM(Vis, FeatureType, int, 6, "0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK 8=GFTT/ORB 9=KAZE 10=ORB-OCTREE 11=SuperPoint 12=SURF/FREAK 13=GFTT/DAISY 14=SURF/DAISY 15=PyDetector"); #endif - RTABMAP_PARAM(Vis, MaxFeatures, int, 1000, "0 no limits."); - RTABMAP_PARAM(Vis, MaxDepth, float, 0, "Max depth of the features (0 means no limit)."); - RTABMAP_PARAM(Vis, MinDepth, float, 0, "Min depth of the features (0 means no limit)."); - RTABMAP_PARAM(Vis, DepthAsMask, bool, true, "Use depth image as mask when extracting features."); - RTABMAP_PARAM_STR(Vis, RoiRatios, "0.0 0.0 0.0 0.0", "Region of interest ratios [left, right, top, bottom]."); - RTABMAP_PARAM(Vis, SubPixWinSize, int, 3, "See cv::cornerSubPix()."); - 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, GridRows, int, 1, uFormat("Number of rows of the grid used to extract uniformly \"%s / grid cells\" features from each cell.", kVisMaxFeatures().c_str())); - RTABMAP_PARAM(Vis, GridCols, int, 1, uFormat("Number of columns of the grid used to extract uniformly \"%s / grid cells\" features from each cell.", kVisMaxFeatures().c_str())); - 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, BruteForceCrossCheck=5, SuperGlue=6, GMS=7. Used for features matching approach.", kVisCorType().c_str())); - RTABMAP_PARAM(Vis, CorNNDR, float, 0.8, uFormat("[%s=0] NNDR: nearest neighbor distance ratio. Used for knn features matching approach.", kVisCorType().c_str())); - RTABMAP_PARAM(Vis, CorGuessWinSize, int, 40, 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, CorGuessMatchToProjection, bool, false, uFormat("[%s=0] Match frame's corners to source's projected points (when guess transform is provided) instead of projected points to frame's corners.", 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, CorFlowIterations, int, 30, uFormat("[%s=1] See cv::calcOpticalFlowPyrLK(). Used for optical flow approach.", kVisCorType().c_str())); - RTABMAP_PARAM(Vis, CorFlowEps, float, 0.01, uFormat("[%s=1] See cv::calcOpticalFlowPyrLK(). Used for optical flow approach.", kVisCorType().c_str())); - RTABMAP_PARAM(Vis, CorFlowMaxLevel, int, 3, uFormat("[%s=1] See cv::calcOpticalFlowPyrLK(). Used for optical flow approach.", kVisCorType().c_str())); + RTABMAP_PARAM(Vis, MaxFeatures, int, 1000, "0 no limits."); + RTABMAP_PARAM(Vis, MaxDepth, float, 0, "Max depth of the features (0 means no limit)."); + RTABMAP_PARAM(Vis, MinDepth, float, 0, "Min depth of the features (0 means no limit)."); + RTABMAP_PARAM(Vis, DepthAsMask, bool, true, "Use depth image as mask when extracting features."); + RTABMAP_PARAM_STR(Vis, RoiRatios, "0.0 0.0 0.0 0.0", "Region of interest ratios [left, right, top, bottom]."); + RTABMAP_PARAM(Vis, SubPixWinSize, int, 3, "See cv::cornerSubPix()."); + 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, GridRows, int, 1, uFormat("Number of rows of the grid used to extract uniformly \"%s / grid cells\" features from each cell.", kVisMaxFeatures().c_str())); + RTABMAP_PARAM(Vis, GridCols, int, 1, uFormat("Number of columns of the grid used to extract uniformly \"%s / grid cells\" features from each cell.", kVisMaxFeatures().c_str())); + 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, BruteForceCrossCheck=5, SuperGlue=6, GMS=7. Used for features matching approach.", kVisCorType().c_str())); + RTABMAP_PARAM(Vis, CorNNDR, float, 0.8, uFormat("[%s=0] NNDR: nearest neighbor distance ratio. Used for knn features matching approach.", kVisCorType().c_str())); + RTABMAP_PARAM(Vis, CorGuessWinSize, int, 40, 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, CorGuessMatchToProjection, bool, false, uFormat("[%s=0] Match frame's corners to source's projected points (when guess transform is provided) instead of projected points to frame's corners.", 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, CorFlowIterations, int, 30, uFormat("[%s=1] See cv::calcOpticalFlowPyrLK(). Used for optical flow approach.", kVisCorType().c_str())); + RTABMAP_PARAM(Vis, CorFlowEps, float, 0.01, uFormat("[%s=1] See cv::calcOpticalFlowPyrLK(). Used for optical flow approach.", kVisCorType().c_str())); + RTABMAP_PARAM(Vis, CorFlowMaxLevel, int, 3, uFormat("[%s=1] See cv::calcOpticalFlowPyrLK(). Used for optical flow approach.", kVisCorType().c_str())); #if defined(RTABMAP_G2O) || defined(RTABMAP_ORB_SLAM) - RTABMAP_PARAM(Vis, BundleAdjustment, int, 1, "Optimization with bundle adjustment: 0=disabled, 1=g2o, 2=cvsba, 3=Ceres."); + RTABMAP_PARAM(Vis, BundleAdjustment, int, 1, "Optimization with bundle adjustment: 0=disabled, 1=g2o, 2=cvsba, 3=Ceres."); #else - RTABMAP_PARAM(Vis, BundleAdjustment, int, 0, "Optimization with bundle adjustment: 0=disabled, 1=g2o, 2=cvsba, 3=Ceres."); + RTABMAP_PARAM(Vis, BundleAdjustment, int, 0, "Optimization with bundle adjustment: 0=disabled, 1=g2o, 2=cvsba, 3=Ceres."); #endif // Features matching approaches @@ -664,7 +737,8 @@ class RTABMAP_EXP Parameters #else RTABMAP_PARAM(Icp, MaxCorrespondenceDistance, float, 0.05, "Max distance for point correspondences."); #endif - RTABMAP_PARAM(Icp, Iterations, int, 30, "Max iterations."); + RTABMAP_PARAM(Icp, ReciprocalCorrespondences, bool, true, "To be a valid correspondence, the corresponding point in target cloud to point in source cloud should be both their closest closest correspondence."); + RTABMAP_PARAM(Icp, Iterations, int, 30, "Max iterations."); RTABMAP_PARAM(Icp, Epsilon, float, 0, "Set the transformation epsilon (maximum allowable difference between two consecutive transformations) in order for an optimization to be considered as having converged to the final solution."); RTABMAP_PARAM(Icp, CorrespondenceRatio, float, 0.1, "Ratio of matching correspondences to accept the transform."); RTABMAP_PARAM(Icp, Force4DoF, bool, false, uFormat("Limit ICP to x, y, z and yaw DoF. Available if %s > 0.", kIcpStrategy().c_str())); diff --git a/corelib/include/rtabmap/core/PythonInterface.h b/corelib/include/rtabmap/core/PythonInterface.h index 2216cb5ffa..f2f3a6a272 100644 --- a/corelib/include/rtabmap/core/PythonInterface.h +++ b/corelib/include/rtabmap/core/PythonInterface.h @@ -11,31 +11,31 @@ #include #include -#include + +namespace pybind11 { +class scoped_interpreter; +class gil_scoped_release; +} namespace rtabmap { +/** + * Create a single PythonInterface on main thread at + * global scope before any Python classes. + */ class PythonInterface { public: PythonInterface(); virtual ~PythonInterface(); -protected: - std::string getTraceback(); // should be called between lock() and unlock() - void lock(); - void unlock(); - private: - static UMutex mutex_; - static int refCount_; - -protected: - static PyThreadState * mainThreadState_; - static unsigned long mainThreadID_; - PyThreadState * threadState_; + pybind11::scoped_interpreter* guard_; + pybind11::gil_scoped_release* release_; }; +std::string getPythonTraceback(); + } #endif /* CORELIB_SRC_PYTHON_PYTHONINTERFACE_H_ */ diff --git a/corelib/include/rtabmap/core/Recovery.h b/corelib/include/rtabmap/core/Recovery.h index 6300332df3..2506167bdc 100644 --- a/corelib/include/rtabmap/core/Recovery.h +++ b/corelib/include/rtabmap/core/Recovery.h @@ -28,7 +28,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #ifndef RECOVERY_H_ #define RECOVERY_H_ -#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines +#include "rtabmap/core/rtabmap_core_export.h" // DLL export/import defines #include @@ -44,7 +44,7 @@ class ProgressState; * @param errorMsg error message if the function returns false * @param progressState A ProgressState object used to get status of the recovery process */ -bool RTABMAP_EXP databaseRecovery( +bool RTABMAP_CORE_EXPORT databaseRecovery( const std::string & corruptedDatabase, bool keepCorruptedDatabase = true, std::string * errorMsg = 0, diff --git a/corelib/include/rtabmap/core/Registration.h b/corelib/include/rtabmap/core/Registration.h index 7de6b10b30..6516b5ae32 100644 --- a/corelib/include/rtabmap/core/Registration.h +++ b/corelib/include/rtabmap/core/Registration.h @@ -28,7 +28,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #ifndef RTABMAP_REGISTRATION_H_ #define RTABMAP_REGISTRATION_H_ -#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines +#include "rtabmap/core/rtabmap_core_export.h" // DLL export/import defines #include #include @@ -36,7 +36,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. namespace rtabmap { -class RTABMAP_EXP Registration +class RTABMAP_CORE_EXPORT Registration { public: enum Type { diff --git a/corelib/include/rtabmap/core/RegistrationIcp.h b/corelib/include/rtabmap/core/RegistrationIcp.h index 4d44acca2c..0735cfae1b 100644 --- a/corelib/include/rtabmap/core/RegistrationIcp.h +++ b/corelib/include/rtabmap/core/RegistrationIcp.h @@ -28,7 +28,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #ifndef REGISTRATIONICP_H_ #define REGISTRATIONICP_H_ -#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines +#include "rtabmap/core/rtabmap_core_export.h" // DLL export/import defines #include #include @@ -36,7 +36,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. namespace rtabmap { // Geometrical registration -class RTABMAP_EXP RegistrationIcp : public Registration +class RTABMAP_CORE_EXPORT RegistrationIcp : public Registration { public: // take ownership of child @@ -64,6 +64,7 @@ class RTABMAP_EXP RegistrationIcp : public Registration float _rangeMin; float _rangeMax; float _maxCorrespondenceDistance; + bool _reciprocalCorrespondences; int _maxIterations; float _epsilon; float _correspondenceRatio; diff --git a/corelib/include/rtabmap/core/RegistrationVis.h b/corelib/include/rtabmap/core/RegistrationVis.h index 02d7ac7fc1..aa36d55762 100644 --- a/corelib/include/rtabmap/core/RegistrationVis.h +++ b/corelib/include/rtabmap/core/RegistrationVis.h @@ -28,7 +28,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #ifndef REGISTRATIONVIS_H_ #define REGISTRATIONVIS_H_ -#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines +#include "rtabmap/core/rtabmap_core_export.h" // DLL export/import defines #include #include @@ -42,7 +42,7 @@ class PyMatcher; #endif // Visual registration -class RTABMAP_EXP RegistrationVis : public Registration +class RTABMAP_CORE_EXPORT RegistrationVis : public Registration { public: // take ownership of child @@ -82,7 +82,9 @@ class RTABMAP_EXP RegistrationVis : public Registration float _PnPReprojError; int _PnPFlags; int _PnPRefineIterations; + int _PnPVarMedianRatio; float _PnPMaxVar; + unsigned int _multiSamplingPolicy; int _correspondencesApproach; int _flowWinSize; int _flowIterations; diff --git a/corelib/include/rtabmap/core/Rtabmap.h b/corelib/include/rtabmap/core/Rtabmap.h index efb045659a..48676f5c6a 100644 --- a/corelib/include/rtabmap/core/Rtabmap.h +++ b/corelib/include/rtabmap/core/Rtabmap.h @@ -28,7 +28,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #ifndef RTABMAP_H_ #define RTABMAP_H_ -#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines +#include "rtabmap/core/rtabmap_core_export.h" // DLL export/import defines #include "rtabmap/core/Parameters.h" #include "rtabmap/core/SensorData.h" @@ -51,7 +51,7 @@ class Signature; class Optimizer; class PythonInterface; -class RTABMAP_EXP Rtabmap +class RTABMAP_CORE_EXPORT Rtabmap { public: enum VhStrategy {kVhNone, kVhEpipolar, kVhUndef}; @@ -180,12 +180,13 @@ class RTABMAP_EXP Rtabmap void deleteLastLocation(); void setOptimizedPoses(const std::map & poses, const std::multimap & constraints); Signature getSignatureCopy(int id, bool images, bool scan, bool userData, bool occupancyGrid, bool withWords, bool withGlobalDescriptors) const; - RTABMAP_DEPRECATED( + // Use getGraph() instead with withImages=true, withScan=true, withUserData=true and withGrid=true. + RTABMAP_DEPRECATED void get3DMap(std::map & signatures, std::map & poses, std::multimap & constraints, bool optimized, - bool global) const, "Use getGraph() instead with withImages=true, withScan=true, withUserData=true and withGrid=true."); + bool global) const; void getGraph(std::map & poses, std::multimap & constraints, bool optimized, @@ -325,6 +326,8 @@ class RTABMAP_EXP Rtabmap bool _loopCovLimited; bool _loopGPS; int _maxOdomCacheSize; + bool _localizationSmoothing; + double _localizationPriorInf; bool _createGlobalScanMap; float _markerPriorsLinearVariance; float _markerPriorsAngularVariance; @@ -361,13 +364,13 @@ class RTABMAP_EXP Rtabmap Transform _mapCorrectionBackup; // used in localization mode when odom is lost Transform _lastLocalizationPose; // Corrected odometry pose. In mapping mode, this corresponds to last pose return by getLocalOptimizedPoses(). int _lastLocalizationNodeId; // for localization mode + cv::Mat _localizationCovariance; std::map > _gpsGeocentricCache; bool _currentSessionHasGPS; LaserScan _globalScanMap; std::map _globalScanMapPoses; std::map _odomCachePoses; // used in localization mode to reject loop closures std::multimap _odomCacheConstraints; // used in localization mode to reject loop closures - std::vector _odomCorrectionAcc; std::map _markerPriors; std::set _nodesToRepublish; diff --git a/corelib/include/rtabmap/core/RtabmapThread.h b/corelib/include/rtabmap/core/RtabmapThread.h index 02b741aa5b..96fa214caf 100644 --- a/corelib/include/rtabmap/core/RtabmapThread.h +++ b/corelib/include/rtabmap/core/RtabmapThread.h @@ -28,7 +28,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #ifndef RTABMAPTHREAD_H_ #define RTABMAPTHREAD_H_ -#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines +#include "rtabmap/core/rtabmap_core_export.h" // DLL export/import defines #include #include @@ -48,7 +48,7 @@ namespace rtabmap { class Rtabmap; -class RTABMAP_EXP RtabmapThread : +class RTABMAP_CORE_EXPORT RtabmapThread : public UThreadNode, public UEventsHandler { diff --git a/corelib/include/rtabmap/core/SensorCaptureInfo.h b/corelib/include/rtabmap/core/SensorCaptureInfo.h index d5efd517cb..ec188dc103 100644 --- a/corelib/include/rtabmap/core/SensorCaptureInfo.h +++ b/corelib/include/rtabmap/core/SensorCaptureInfo.h @@ -46,6 +46,7 @@ class SensorCaptureInfo timeMirroring(0.0f), timeStereoExposureCompensation(0.0f), timeImageDecimation(0.0f), + timeHistogramEqualization(0.0f), timeScanFromDepth(0.0f), timeUndistortDepth(0.0f), timeBilateralFiltering(0.0f), @@ -63,6 +64,7 @@ class SensorCaptureInfo float timeMirroring; float timeStereoExposureCompensation; float timeImageDecimation; + float timeHistogramEqualization; float timeScanFromDepth; float timeUndistortDepth; float timeBilateralFiltering; diff --git a/corelib/include/rtabmap/core/SensorCaptureThread.h b/corelib/include/rtabmap/core/SensorCaptureThread.h index 7fabb40a24..69af2b4d1d 100644 --- a/corelib/include/rtabmap/core/SensorCaptureThread.h +++ b/corelib/include/rtabmap/core/SensorCaptureThread.h @@ -27,7 +27,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #pragma once -#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines +#include "rtabmap/core/rtabmap_core_export.h" // DLL export/import defines #include #include @@ -47,12 +47,13 @@ class SensorCaptureInfo; class SensorData; class StereoDense; class IMUFilter; +class Feature2D; /** * Class CameraThread - * + * */ -class RTABMAP_EXP SensorCaptureThread : +class RTABMAP_CORE_EXPORT SensorCaptureThread : public UThread, public UEventsSender { @@ -80,6 +81,7 @@ class RTABMAP_EXP SensorCaptureThread : void setStereoExposureCompensation(bool enabled) {_stereoExposureCompensation = enabled;} void setColorOnly(bool colorOnly) {_colorOnly = colorOnly;} void setImageDecimation(int decimation) {_imageDecimation = decimation;} + void setHistogramMethod(int histogramMethod) {_histogramMethod = histogramMethod;} void setStereoToDepth(bool enabled) {_stereoToDepth = enabled;} void setImageRate(float imageRate); void setDistortionModel(const std::string & path); @@ -87,8 +89,11 @@ class RTABMAP_EXP SensorCaptureThread : 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(); - RTABMAP_DEPRECATED(void setScanParameters( + // Use new version of this function with groundNormalsUp=0.8 for forceGroundNormalsUp=True and groundNormalsUp=0.0 for forceGroundNormalsUp=False. + RTABMAP_DEPRECATED void setScanParameters( bool fromDepth, int downsampleStep, // decimation of the depth image in case the scan is from depth image float rangeMin, @@ -96,7 +101,7 @@ class RTABMAP_EXP SensorCaptureThread : float voxelSize, int normalsK, int normalsRadius, - bool forceGroundNormalsUp) , "Use new version of this function with groundNormalsUp=0.8 for forceGroundNormalsUp=True and groundNormalsUp=0.0 for forceGroundNormalsUp=False."); + bool forceGroundNormalsUp); void setScanParameters( bool fromDepth, int downsampleStep=1, // decimation of the depth image in case the scan is from depth image @@ -133,6 +138,7 @@ class RTABMAP_EXP SensorCaptureThread : bool _stereoExposureCompensation; bool _colorOnly; int _imageDecimation; + int _histogramMethod; bool _stereoToDepth; bool _scanFromDepth; int _scanDownsampleStep; @@ -149,6 +155,8 @@ class RTABMAP_EXP SensorCaptureThread : float _bilateralSigmaR; IMUFilter * _imuFilter; bool _imuBaseFrameConversion; + Feature2D * _featureDetector; + bool _depthAsMask; }; } // namespace rtabmap diff --git a/corelib/include/rtabmap/core/SensorData.h b/corelib/include/rtabmap/core/SensorData.h index 0cbe953194..57d7452ddf 100644 --- a/corelib/include/rtabmap/core/SensorData.h +++ b/corelib/include/rtabmap/core/SensorData.h @@ -28,7 +28,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #ifndef SENSORDATA_H_ #define SENSORDATA_H_ -#include +#include #include #include #include @@ -48,7 +48,7 @@ namespace rtabmap /** * An id is automatically generated if id=0. */ -class RTABMAP_EXP SensorData +class RTABMAP_CORE_EXPORT SensorData { public: // empty constructor @@ -210,10 +210,14 @@ class RTABMAP_EXP SensorData cv::Mat depthRaw() const {return _depthOrRightRaw.type()!=CV_8UC1?_depthOrRightRaw:cv::Mat();} cv::Mat rightRaw() const {return _depthOrRightRaw.type()==CV_8UC1?_depthOrRightRaw:cv::Mat();} - RTABMAP_DEPRECATED(void setImageRaw(const cv::Mat & image), "Use setRGBDImage() or setStereoImage() with clearNotUpdated=false or removeRawData() instead. To be backward compatible, this function doesn't clear compressed data."); - RTABMAP_DEPRECATED(void setDepthOrRightRaw(const cv::Mat & image), "Use setRGBDImage() or setStereoImage() with clearNotUpdated=false or removeRawData() instead. To be backward compatible, this function doesn't clear compressed data."); - RTABMAP_DEPRECATED(void setLaserScanRaw(const LaserScan & scan), "Use setLaserScan() with clearNotUpdated=false or removeRawData() instead. To be backward compatible, this function doesn't clear compressed data."); - RTABMAP_DEPRECATED(void setUserDataRaw(const cv::Mat & data), "Use setUserData() or removeRawData() instead."); + // Use setRGBDImage() or setStereoImage() with clearNotUpdated=false or removeRawData() instead. To be backward compatible, this function doesn't clear compressed data. + RTABMAP_DEPRECATED void setImageRaw(const cv::Mat & image); + // Use setRGBDImage() or setStereoImage() with clearNotUpdated=false or removeRawData() instead. To be backward compatible, this function doesn't clear compressed data. + RTABMAP_DEPRECATED void setDepthOrRightRaw(const cv::Mat & image); + // Use setLaserScan() with clearNotUpdated=false or removeRawData() instead. To be backward compatible, this function doesn't clear compressed data. + RTABMAP_DEPRECATED void setLaserScanRaw(const LaserScan & scan); + // Use setUserData() or removeRawData() instead. + RTABMAP_DEPRECATED void setUserDataRaw(const cv::Mat & data); void uncompressData(); void uncompressData( diff --git a/corelib/include/rtabmap/core/Signature.h b/corelib/include/rtabmap/core/Signature.h index fdb850b4fa..1e393cb235 100644 --- a/corelib/include/rtabmap/core/Signature.h +++ b/corelib/include/rtabmap/core/Signature.h @@ -27,7 +27,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #pragma once -#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines +#include "rtabmap/core/rtabmap_core_export.h" // DLL export/import defines #include #include @@ -45,7 +45,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. namespace rtabmap { -class RTABMAP_EXP Signature +class RTABMAP_CORE_EXPORT Signature { public: diff --git a/corelib/include/rtabmap/core/Statistics.h b/corelib/include/rtabmap/core/Statistics.h index fa72a1afca..743b46bf04 100644 --- a/corelib/include/rtabmap/core/Statistics.h +++ b/corelib/include/rtabmap/core/Statistics.h @@ -28,7 +28,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #ifndef STATISTICS_H_ #define STATISTICS_H_ -#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines +#include "rtabmap/core/rtabmap_core_export.h" // DLL export/import defines #include #include @@ -50,7 +50,7 @@ namespace rtabmap { }; \ Dummy##PREFIX##NAME dummy##PREFIX##NAME -class RTABMAP_EXP Statistics +class RTABMAP_CORE_EXPORT Statistics { RTABMAP_STATS(Loop, Id,); // Combined loop or proximity detection RTABMAP_STATS(Loop, RejectedHypothesis,); @@ -67,7 +67,7 @@ class RTABMAP_EXP Statistics RTABMAP_STATS(Loop, Visual_inliers,); RTABMAP_STATS(Loop, Visual_inliers_ratio,); RTABMAP_STATS(Loop, Visual_matches,); - RTABMAP_STATS(Loop, Distance_since_last_loc,); + RTABMAP_STATS(Loop, Distance_since_last_loc, m); RTABMAP_STATS(Loop, Last_id,); RTABMAP_STATS(Loop, Optimization_max_error, m); RTABMAP_STATS(Loop, Optimization_max_error_ratio, ); @@ -90,15 +90,6 @@ class RTABMAP_EXP Statistics RTABMAP_STATS(Loop, Odom_correction_roll, deg); RTABMAP_STATS(Loop, Odom_correction_pitch, deg); RTABMAP_STATS(Loop, Odom_correction_yaw, deg); - //Odom correction - RTABMAP_STATS(Loop, Odom_correction_acc_norm, m); - RTABMAP_STATS(Loop, Odom_correction_acc_angle, deg); - RTABMAP_STATS(Loop, Odom_correction_acc_x, m); - RTABMAP_STATS(Loop, Odom_correction_acc_y, m); - RTABMAP_STATS(Loop, Odom_correction_acc_z, m); - RTABMAP_STATS(Loop, Odom_correction_acc_roll, deg); - RTABMAP_STATS(Loop, Odom_correction_acc_pitch, deg); - RTABMAP_STATS(Loop, Odom_correction_acc_yaw, deg); // Map to Odom RTABMAP_STATS(Loop, MapToOdom_norm, m); RTABMAP_STATS(Loop, MapToOdom_angle, deg); @@ -115,6 +106,8 @@ class RTABMAP_EXP Statistics RTABMAP_STATS(Loop, MapToBase_roll, deg); RTABMAP_STATS(Loop, MapToBase_pitch, deg); RTABMAP_STATS(Loop, MapToBase_yaw, deg); + RTABMAP_STATS(Loop, MapToBase_lin_std, m); + RTABMAP_STATS(Loop, MapToBase_lin_var, m2); RTABMAP_STATS(Proximity, Time_detections,); RTABMAP_STATS(Proximity, Space_last_detection_id,); @@ -158,6 +151,8 @@ class RTABMAP_EXP Statistics RTABMAP_STATS(Memory, RAM_usage, MB); RTABMAP_STATS(Memory, RAM_estimated, MB); RTABMAP_STATS(Memory, Triangulated_points, ); + RTABMAP_STATS(Memory, Closest_node_distance, m); + RTABMAP_STATS(Memory, Closest_node_angle, rad); RTABMAP_STATS(Timing, Memory_update, ms); RTABMAP_STATS(Timing, Neighbor_link_refining, ms); @@ -241,7 +236,8 @@ class RTABMAP_EXP Statistics void setProximityDetectionMapId(int id) {_proximiyDetectionMapId = id;} void setStamp(double stamp) {_stamp = stamp;} - RTABMAP_DEPRECATED(void setLastSignatureData(const Signature & data) {_signaturesData.insert(std::make_pair(data.id(), data));}, "Use addSignatureData() instead."); + // Use addSignatureData() instead. + RTABMAP_DEPRECATED void setLastSignatureData(const Signature & data); void addSignatureData(const Signature & data) {_signaturesData.insert(std::make_pair(data.id(), data));} void setSignaturesData(const std::map & data) {_signaturesData = data;} diff --git a/corelib/include/rtabmap/core/Stereo.h b/corelib/include/rtabmap/core/Stereo.h index e0cfd94221..9c06e2a1dc 100644 --- a/corelib/include/rtabmap/core/Stereo.h +++ b/corelib/include/rtabmap/core/Stereo.h @@ -28,14 +28,14 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #ifndef STEREO_H_ #define STEREO_H_ -#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines +#include "rtabmap/core/rtabmap_core_export.h" // DLL export/import defines #include #include namespace rtabmap { -class RTABMAP_EXP Stereo { +class RTABMAP_CORE_EXPORT Stereo { public: static Stereo * create(const ParametersMap & parameters = ParametersMap()); @@ -67,7 +67,7 @@ class RTABMAP_EXP Stereo { bool winSSD_; }; -class RTABMAP_EXP StereoOpticalFlow : public Stereo { +class RTABMAP_CORE_EXPORT StereoOpticalFlow : public Stereo { public: StereoOpticalFlow(const ParametersMap & parameters = ParametersMap()); virtual ~StereoOpticalFlow() {} diff --git a/corelib/include/rtabmap/core/StereoCameraModel.h b/corelib/include/rtabmap/core/StereoCameraModel.h index 4fbbd7228f..07cf2e77ad 100644 --- a/corelib/include/rtabmap/core/StereoCameraModel.h +++ b/corelib/include/rtabmap/core/StereoCameraModel.h @@ -32,7 +32,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. namespace rtabmap { -class RTABMAP_EXP StereoCameraModel +class RTABMAP_CORE_EXPORT StereoCameraModel { public: StereoCameraModel() : leftSuffix_("left"), rightSuffix_("right") {} @@ -140,7 +140,7 @@ class RTABMAP_EXP StereoCameraModel cv::Mat F_; }; -RTABMAP_EXP std::ostream& operator<<(std::ostream& os, const StereoCameraModel& model); +RTABMAP_CORE_EXPORT std::ostream& operator<<(std::ostream& os, const StereoCameraModel& model); } // rtabmap diff --git a/corelib/include/rtabmap/core/StereoDense.h b/corelib/include/rtabmap/core/StereoDense.h index 2f77b124b6..fe37e52569 100644 --- a/corelib/include/rtabmap/core/StereoDense.h +++ b/corelib/include/rtabmap/core/StereoDense.h @@ -28,14 +28,14 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #ifndef STEREODENSE_H_ #define STEREODENSE_H_ -#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines +#include "rtabmap/core/rtabmap_core_export.h" // DLL export/import defines #include #include namespace rtabmap { -class RTABMAP_EXP StereoDense { +class RTABMAP_CORE_EXPORT StereoDense { public: enum Type { kTypeBM = 0, diff --git a/corelib/include/rtabmap/core/Transform.h b/corelib/include/rtabmap/core/Transform.h index 542229a445..f973c6398d 100644 --- a/corelib/include/rtabmap/core/Transform.h +++ b/corelib/include/rtabmap/core/Transform.h @@ -28,7 +28,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #ifndef TRANSFORM_H_ #define TRANSFORM_H_ -#include +#include #include #include #include @@ -38,7 +38,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. namespace rtabmap { -class RTABMAP_EXP Transform +class RTABMAP_CORE_EXPORT Transform { public: @@ -168,16 +168,17 @@ class RTABMAP_EXP Transform static Transform getTransform( const std::map & tfBuffer, const double & stamp); - RTABMAP_DEPRECATED(static Transform getClosestTransform( + // Use Transform::getTransform() instead to get always accurate transforms. + RTABMAP_DEPRECATED static Transform getClosestTransform( const std::map & tfBuffer, const double & stamp, - double * stampDiff), "Use Transform::getTransform() instead to get always accurate transforms."); + double * stampDiff); private: cv::Mat data_; }; -RTABMAP_EXP std::ostream& operator<<(std::ostream& os, const Transform& s); +RTABMAP_CORE_EXPORT std::ostream& operator<<(std::ostream& os, const Transform& s); class TransformStamped { diff --git a/corelib/include/rtabmap/core/VWDictionary.h b/corelib/include/rtabmap/core/VWDictionary.h index 5a48006878..fbfa7ba542 100644 --- a/corelib/include/rtabmap/core/VWDictionary.h +++ b/corelib/include/rtabmap/core/VWDictionary.h @@ -27,7 +27,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #pragma once -#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines +#include "rtabmap/core/rtabmap_core_export.h" // DLL export/import defines #include #include @@ -43,7 +43,7 @@ class DBDriver; class VisualWord; class FlannIndex; -class RTABMAP_EXP VWDictionary +class RTABMAP_CORE_EXPORT VWDictionary { public: enum NNStrategy{ diff --git a/corelib/include/rtabmap/core/VisualWord.h b/corelib/include/rtabmap/core/VisualWord.h index 35eba36b0c..795c173783 100644 --- a/corelib/include/rtabmap/core/VisualWord.h +++ b/corelib/include/rtabmap/core/VisualWord.h @@ -27,7 +27,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #pragma once -#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines +#include "rtabmap/core/rtabmap_core_export.h" // DLL export/import defines #include #include @@ -35,7 +35,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. namespace rtabmap { -class RTABMAP_EXP VisualWord +class RTABMAP_CORE_EXPORT VisualWord { public: VisualWord(int id, const cv::Mat & descriptor, int signatureId = 0); diff --git a/corelib/include/rtabmap/core/camera/CameraDepthAI.h b/corelib/include/rtabmap/core/camera/CameraDepthAI.h index e16fcf7173..5860e6830d 100644 --- a/corelib/include/rtabmap/core/camera/CameraDepthAI.h +++ b/corelib/include/rtabmap/core/camera/CameraDepthAI.h @@ -27,8 +27,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #pragma once -#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines - #include "rtabmap/core/StereoCameraModel.h" #include "rtabmap/core/Camera.h" #include "rtabmap/core/Version.h" @@ -43,7 +41,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. namespace rtabmap { -class RTABMAP_EXP CameraDepthAI : +class RTABMAP_CORE_EXPORT CameraDepthAI : public Camera { public: @@ -51,15 +49,23 @@ class RTABMAP_EXP CameraDepthAI : public: CameraDepthAI( - const std::string & deviceSerial = "", + const std::string & mxidOrName = "", int resolution = 1, // 0=720p, 1=800p, 2=400p float imageRate=0.0f, const Transform & localTransform = Transform::getIdentity()); virtual ~CameraDepthAI(); void setOutputDepth(bool enabled, int confidence = 200); - void setIMUFirmwareUpdate(bool enabled); + void setUseSpecTranslation(bool useSpecTranslation); + void setAlphaScaling(float alphaScaling = 0.0f); void setIMUPublished(bool published); + void publishInterIMU(bool enabled); + void setLaserDotBrightness(float dotProjectormA = 0.0f); + void setFloodLightBrightness(float floodLightmA = 200.0f); + void setDetectFeatures(int detectFeatures = 0); + void setBlobPath(const std::string & blobPath); + void setGFTTDetector(bool useHarrisDetector = false, float minDistance = 7.0f, int numTargetFeatures = 1000); + void setSuperPointDetector(float threshold = 0.01f, bool nms = true, int nmsRadius = 4); virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = ""); virtual bool isCalibrated() const; @@ -71,19 +77,33 @@ class RTABMAP_EXP CameraDepthAI : private: #ifdef RTABMAP_DEPTHAI StereoCameraModel stereoModel_; + cv::Size targetSize_; Transform imuLocalTransform_; - std::string deviceSerial_; + std::string mxidOrName_; bool outputDepth_; int depthConfidence_; int resolution_; - bool imuFirmwareUpdate_; + bool useSpecTranslation_; + float alphaScaling_; bool imuPublished_; + bool publishInterIMU_; + float dotProjectormA_; + float floodLightmA_; + int detectFeatures_; + bool useHarrisDetector_; + float minDistance_; + int numTargetFeatures_; + float threshold_; + bool nms_; + int nmsRadius_; + std::string blobPath_; std::shared_ptr device_; std::shared_ptr leftQueue_; std::shared_ptr rightOrDepthQueue_; - std::shared_ptr imuQueue_; + std::shared_ptr featuresQueue_; std::map accBuffer_; std::map gyroBuffer_; + UMutex imuMutex_; #endif }; diff --git a/corelib/include/rtabmap/core/camera/CameraFreenect.h b/corelib/include/rtabmap/core/camera/CameraFreenect.h index 103977fe77..4c7c62ffe9 100644 --- a/corelib/include/rtabmap/core/camera/CameraFreenect.h +++ b/corelib/include/rtabmap/core/camera/CameraFreenect.h @@ -27,7 +27,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #pragma once -#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines +#include "rtabmap/core/rtabmap_core_export.h" // DLL export/import defines #include "rtabmap/core/StereoCameraModel.h" #include "rtabmap/core/Camera.h" @@ -41,7 +41,7 @@ namespace rtabmap class FreenectDevice; -class RTABMAP_EXP CameraFreenect : +class RTABMAP_CORE_EXPORT CameraFreenect : public Camera { public: diff --git a/corelib/include/rtabmap/core/camera/CameraFreenect2.h b/corelib/include/rtabmap/core/camera/CameraFreenect2.h index 82800dff94..d0516074c5 100644 --- a/corelib/include/rtabmap/core/camera/CameraFreenect2.h +++ b/corelib/include/rtabmap/core/camera/CameraFreenect2.h @@ -27,8 +27,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #pragma once -#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines - #include "rtabmap/core/StereoCameraModel.h" #include "rtabmap/core/Camera.h" #include "rtabmap/core/Version.h" @@ -45,7 +43,7 @@ class PacketPipeline; namespace rtabmap { -class RTABMAP_EXP CameraFreenect2 : +class RTABMAP_CORE_EXPORT CameraFreenect2 : public Camera { public: diff --git a/corelib/include/rtabmap/core/camera/CameraImages.h b/corelib/include/rtabmap/core/camera/CameraImages.h index bc5aaf41e9..9b7134dc99 100644 --- a/corelib/include/rtabmap/core/camera/CameraImages.h +++ b/corelib/include/rtabmap/core/camera/CameraImages.h @@ -27,8 +27,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #pragma once -#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines - #include "rtabmap/core/Camera.h" #include "rtabmap/utilite/UTimer.h" #include @@ -38,7 +36,7 @@ class UDirectory; namespace rtabmap { -class RTABMAP_EXP CameraImages : +class RTABMAP_CORE_EXPORT CameraImages : public Camera { public: diff --git a/corelib/include/rtabmap/core/camera/CameraK4A.h b/corelib/include/rtabmap/core/camera/CameraK4A.h index ec0a024364..0629af4693 100644 --- a/corelib/include/rtabmap/core/camera/CameraK4A.h +++ b/corelib/include/rtabmap/core/camera/CameraK4A.h @@ -27,8 +27,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #pragma once -#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines - #include "rtabmap/core/CameraModel.h" #include "rtabmap/core/Camera.h" #include "rtabmap/core/Version.h" @@ -42,7 +40,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. namespace rtabmap { -class RTABMAP_EXP CameraK4A : +class RTABMAP_CORE_EXPORT CameraK4A : public Camera { public: diff --git a/corelib/include/rtabmap/core/camera/CameraK4W2.h b/corelib/include/rtabmap/core/camera/CameraK4W2.h index d3604063c9..a47a6378a4 100644 --- a/corelib/include/rtabmap/core/camera/CameraK4W2.h +++ b/corelib/include/rtabmap/core/camera/CameraK4W2.h @@ -27,8 +27,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #pragma once -#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines - #include "rtabmap/core/CameraModel.h" #include "rtabmap/core/Camera.h" #include "rtabmap/core/Version.h" @@ -43,7 +41,7 @@ typedef struct IMultiSourceFrameReader IMultiSourceFrameReader; namespace rtabmap { -class RTABMAP_EXP CameraK4W2 : +class RTABMAP_CORE_EXPORT CameraK4W2 : public Camera { public: diff --git a/corelib/include/rtabmap/core/camera/CameraMyntEye.h b/corelib/include/rtabmap/core/camera/CameraMyntEye.h index 470728545f..d6037946a7 100644 --- a/corelib/include/rtabmap/core/camera/CameraMyntEye.h +++ b/corelib/include/rtabmap/core/camera/CameraMyntEye.h @@ -27,8 +27,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #pragma once -#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines - #include "rtabmap/core/Camera.h" #include "rtabmap/core/Version.h" #include "rtabmap/utilite/USemaphore.h" @@ -46,7 +44,7 @@ namespace rtabmap * Class CameraMyntEye * */ -class RTABMAP_EXP CameraMyntEye : public Camera +class RTABMAP_CORE_EXPORT CameraMyntEye : public Camera { public: static bool available(); diff --git a/corelib/include/rtabmap/core/camera/CameraOpenNI2.h b/corelib/include/rtabmap/core/camera/CameraOpenNI2.h index dd3854f527..dffbcfef98 100644 --- a/corelib/include/rtabmap/core/camera/CameraOpenNI2.h +++ b/corelib/include/rtabmap/core/camera/CameraOpenNI2.h @@ -27,8 +27,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #pragma once -#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines - #include "rtabmap/core/StereoCameraModel.h" #include "rtabmap/core/Camera.h" #include "rtabmap/core/Version.h" @@ -41,7 +39,7 @@ class VideoStream; namespace rtabmap { -class RTABMAP_EXP CameraOpenNI2 : +class RTABMAP_CORE_EXPORT CameraOpenNI2 : public Camera { diff --git a/corelib/include/rtabmap/core/camera/CameraOpenNICV.h b/corelib/include/rtabmap/core/camera/CameraOpenNICV.h index 5e93f6431c..9cb467d13c 100644 --- a/corelib/include/rtabmap/core/camera/CameraOpenNICV.h +++ b/corelib/include/rtabmap/core/camera/CameraOpenNICV.h @@ -27,15 +27,13 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #pragma once -#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines - #include "rtabmap/core/Camera.h" #include "rtabmap/core/Version.h" namespace rtabmap { -class RTABMAP_EXP CameraOpenNICV : +class RTABMAP_CORE_EXPORT CameraOpenNICV : public Camera { diff --git a/corelib/include/rtabmap/core/camera/CameraOpenni.h b/corelib/include/rtabmap/core/camera/CameraOpenni.h index de353528e9..37a4425921 100644 --- a/corelib/include/rtabmap/core/camera/CameraOpenni.h +++ b/corelib/include/rtabmap/core/camera/CameraOpenni.h @@ -27,8 +27,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #pragma once -#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines - #include "rtabmap/utilite/UMutex.h" #include "rtabmap/utilite/USemaphore.h" #include "rtabmap/core/Camera.h" @@ -56,7 +54,7 @@ class Grabber; namespace rtabmap { -class RTABMAP_EXP CameraOpenni : +class RTABMAP_CORE_EXPORT CameraOpenni : public Camera { public: diff --git a/corelib/include/rtabmap/core/camera/CameraRGBDImages.h b/corelib/include/rtabmap/core/camera/CameraRGBDImages.h index 6735a680a4..ebc8870b14 100644 --- a/corelib/include/rtabmap/core/camera/CameraRGBDImages.h +++ b/corelib/include/rtabmap/core/camera/CameraRGBDImages.h @@ -32,7 +32,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. namespace rtabmap { -class RTABMAP_EXP CameraRGBDImages : +class RTABMAP_CORE_EXPORT CameraRGBDImages : public CameraImages { public: diff --git a/corelib/include/rtabmap/core/camera/CameraRealSense.h b/corelib/include/rtabmap/core/camera/CameraRealSense.h index f3263358b3..9bfd01c8d0 100644 --- a/corelib/include/rtabmap/core/camera/CameraRealSense.h +++ b/corelib/include/rtabmap/core/camera/CameraRealSense.h @@ -27,9 +27,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #pragma once -#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines - - #include "rtabmap/utilite/UMutex.h" #include "rtabmap/utilite/USemaphore.h" #include "rtabmap/core/CameraModel.h" @@ -49,7 +46,7 @@ namespace rtabmap { class slam_event_handler; -class RTABMAP_EXP CameraRealSense : +class RTABMAP_CORE_EXPORT CameraRealSense : public Camera { public: diff --git a/corelib/include/rtabmap/core/camera/CameraRealSense2.h b/corelib/include/rtabmap/core/camera/CameraRealSense2.h index 763d60b418..ad5f4a8ae5 100644 --- a/corelib/include/rtabmap/core/camera/CameraRealSense2.h +++ b/corelib/include/rtabmap/core/camera/CameraRealSense2.h @@ -27,8 +27,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #pragma once -#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines - #include "rtabmap/core/CameraModel.h" #include "rtabmap/core/Camera.h" #include "rtabmap/core/Version.h" @@ -53,7 +51,7 @@ struct rs2_extrinsics; namespace rtabmap { -class RTABMAP_EXP CameraRealSense2 : +class RTABMAP_CORE_EXPORT CameraRealSense2 : public Camera { public: diff --git a/corelib/include/rtabmap/core/camera/CameraStereoDC1394.h b/corelib/include/rtabmap/core/camera/CameraStereoDC1394.h index 2af84bebb8..025566b0e9 100644 --- a/corelib/include/rtabmap/core/camera/CameraStereoDC1394.h +++ b/corelib/include/rtabmap/core/camera/CameraStereoDC1394.h @@ -27,8 +27,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #pragma once -#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines - #include "rtabmap/core/StereoCameraModel.h" #include "rtabmap/core/Camera.h" #include "rtabmap/core/Version.h" @@ -38,7 +36,7 @@ namespace rtabmap class DC1394Device; -class RTABMAP_EXP CameraStereoDC1394 : +class RTABMAP_CORE_EXPORT CameraStereoDC1394 : public Camera { public: diff --git a/corelib/include/rtabmap/core/camera/CameraStereoFlyCapture2.h b/corelib/include/rtabmap/core/camera/CameraStereoFlyCapture2.h index 757732f676..3b42788486 100644 --- a/corelib/include/rtabmap/core/camera/CameraStereoFlyCapture2.h +++ b/corelib/include/rtabmap/core/camera/CameraStereoFlyCapture2.h @@ -27,8 +27,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #pragma once -#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines - #include "rtabmap/core/Camera.h" #include "rtabmap/core/Version.h" @@ -40,7 +38,7 @@ class Camera; namespace rtabmap { -class RTABMAP_EXP CameraStereoFlyCapture2 : +class RTABMAP_CORE_EXPORT CameraStereoFlyCapture2 : public Camera { public: diff --git a/corelib/include/rtabmap/core/camera/CameraStereoImages.h b/corelib/include/rtabmap/core/camera/CameraStereoImages.h index 607cd6dc60..75b43bbf68 100644 --- a/corelib/include/rtabmap/core/camera/CameraStereoImages.h +++ b/corelib/include/rtabmap/core/camera/CameraStereoImages.h @@ -28,7 +28,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #pragma once #include -#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines #include "rtabmap/core/StereoCameraModel.h" #include "rtabmap/core/Version.h" @@ -37,7 +36,7 @@ namespace rtabmap { class CameraImages; -class RTABMAP_EXP CameraStereoImages : +class RTABMAP_CORE_EXPORT CameraStereoImages : public CameraImages { public: diff --git a/corelib/include/rtabmap/core/camera/CameraStereoTara.h b/corelib/include/rtabmap/core/camera/CameraStereoTara.h index 397f97f55a..72f5e12265 100644 --- a/corelib/include/rtabmap/core/camera/CameraStereoTara.h +++ b/corelib/include/rtabmap/core/camera/CameraStereoTara.h @@ -32,8 +32,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #pragma once -#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines - #include "rtabmap/core/StereoCameraModel.h" #include "rtabmap/core/camera/CameraVideo.h" #include "rtabmap/core/Version.h" @@ -42,7 +40,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. namespace rtabmap { -class RTABMAP_EXP CameraStereoTara : +class RTABMAP_CORE_EXPORT CameraStereoTara : public Camera { public: diff --git a/corelib/include/rtabmap/core/camera/CameraStereoVideo.h b/corelib/include/rtabmap/core/camera/CameraStereoVideo.h index 90e2257923..b2e17d518d 100644 --- a/corelib/include/rtabmap/core/camera/CameraStereoVideo.h +++ b/corelib/include/rtabmap/core/camera/CameraStereoVideo.h @@ -27,15 +27,13 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #pragma once -#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines - #include "rtabmap/core/StereoCameraModel.h" #include "rtabmap/core/camera/CameraVideo.h" namespace rtabmap { -class RTABMAP_EXP CameraStereoVideo : +class RTABMAP_CORE_EXPORT CameraStereoVideo : public Camera { public: diff --git a/corelib/include/rtabmap/core/camera/CameraStereoZed.h b/corelib/include/rtabmap/core/camera/CameraStereoZed.h index dc8bc8949a..a177875cbf 100644 --- a/corelib/include/rtabmap/core/camera/CameraStereoZed.h +++ b/corelib/include/rtabmap/core/camera/CameraStereoZed.h @@ -27,8 +27,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #pragma once -#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines - #include "rtabmap/core/StereoCameraModel.h" #include "rtabmap/core/camera/CameraVideo.h" #include "rtabmap/core/Version.h" @@ -42,16 +40,16 @@ namespace rtabmap { class ZedIMUThread; -class RTABMAP_EXP CameraStereoZed : +class RTABMAP_CORE_EXPORT CameraStereoZed : public Camera { public: static bool available(); - + static int sdkVersion(); public: CameraStereoZed( int deviceId, - int resolution = 2, // 0=HD2K, 1=HD1080, 2=HD720, 3=VGA + int resolution = 6, // 0=HD2K, 1=HD1080, 2=HD1200, 3=HD720, 4=SVGA, 5=VGA, 6=AUTO int quality = 1, // 0=NONE, 1=PERFORMANCE, 2=QUALITY int sensingMode = 0,// 0=STANDARD, 1=FILL int confidenceThr = 100, @@ -63,7 +61,7 @@ class RTABMAP_EXP CameraStereoZed : int texturenessConfidenceThr = 90); // introduced with ZED SDK 3 CameraStereoZed( const std::string & svoFilePath, - int quality = 1, // 0=NONE, 1=PERFORMANCE, 2=QUALITY + int quality = 1, // 0=NONE, 1=PERFORMANCE, 2=QUALITY, 3=NEURAL int sensingMode = 0,// 0=STANDARD, 1=FILL int confidenceThr = 100, bool computeOdometry = false, diff --git a/corelib/include/rtabmap/core/camera/CameraStereoZedOC.h b/corelib/include/rtabmap/core/camera/CameraStereoZedOC.h index 369104a4fc..2399b7b174 100644 --- a/corelib/include/rtabmap/core/camera/CameraStereoZedOC.h +++ b/corelib/include/rtabmap/core/camera/CameraStereoZedOC.h @@ -27,8 +27,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #pragma once -#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines - #include "rtabmap/core/StereoCameraModel.h" #include "rtabmap/core/Camera.h" #include "rtabmap/core/Version.h" @@ -46,7 +44,7 @@ namespace rtabmap { class ZedOCThread; -class RTABMAP_EXP CameraStereoZedOC : +class RTABMAP_CORE_EXPORT CameraStereoZedOC : public Camera { public: diff --git a/corelib/include/rtabmap/core/camera/CameraVideo.h b/corelib/include/rtabmap/core/camera/CameraVideo.h index 9d97cb6f6b..d34e0e9b45 100644 --- a/corelib/include/rtabmap/core/camera/CameraVideo.h +++ b/corelib/include/rtabmap/core/camera/CameraVideo.h @@ -27,15 +27,13 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #pragma once -#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines - #include #include "rtabmap/core/Camera.h" namespace rtabmap { -class RTABMAP_EXP CameraVideo : +class RTABMAP_CORE_EXPORT CameraVideo : public Camera { public: diff --git a/corelib/include/rtabmap/core/clams/discrete_depth_distortion_model.h b/corelib/include/rtabmap/core/clams/discrete_depth_distortion_model.h index 7f3e1b704e..ab473f160d 100644 --- a/corelib/include/rtabmap/core/clams/discrete_depth_distortion_model.h +++ b/corelib/include/rtabmap/core/clams/discrete_depth_distortion_model.h @@ -36,11 +36,11 @@ RTAB-Map integration: Mathieu Labbe #include #include #include -#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines +#include "rtabmap/core/rtabmap_core_export.h" // DLL export/import defines namespace clams { - class RTABMAP_EXP DiscreteFrustum + class RTABMAP_CORE_EXPORT DiscreteFrustum { public: DiscreteFrustum(int smoothing = 1, double bin_depth = 1.0, double max_dist = 10.0); @@ -65,7 +65,7 @@ namespace clams friend class DiscreteDepthDistortionModel; }; - class RTABMAP_EXP DiscreteDepthDistortionModel + class RTABMAP_CORE_EXPORT DiscreteDepthDistortionModel { public: // returns all divisors of num diff --git a/corelib/include/rtabmap/core/clams/frame_projector.h b/corelib/include/rtabmap/core/clams/frame_projector.h index 181e0e5fa8..f057103656 100644 --- a/corelib/include/rtabmap/core/clams/frame_projector.h +++ b/corelib/include/rtabmap/core/clams/frame_projector.h @@ -34,7 +34,6 @@ RTAB-Map integration: Mathieu Labbe #include #include #include -#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines #define MAX_MULT 1.3 #define MIN_MULT 0.7 @@ -60,7 +59,7 @@ namespace clams //! This is essentially a pinhole camera model for an RGBD sensor, with //! some extra functions added on for use during calibration. - class RTABMAP_EXP FrameProjector + class RTABMAP_CORE_EXPORT FrameProjector { public: // For storing z values in meters. This is not Euclidean distance. diff --git a/corelib/include/rtabmap/core/clams/slam_calibrator.h b/corelib/include/rtabmap/core/clams/slam_calibrator.h index ee3343ca8c..671e26c08b 100644 --- a/corelib/include/rtabmap/core/clams/slam_calibrator.h +++ b/corelib/include/rtabmap/core/clams/slam_calibrator.h @@ -35,12 +35,10 @@ RTAB-Map integration: Mathieu Labbe #include #include -#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines - namespace clams { -DiscreteDepthDistortionModel RTABMAP_EXP calibrate( +DiscreteDepthDistortionModel RTABMAP_CORE_EXPORT calibrate( const std::map & sequence, const std::map & trajectory, const pcl::PointCloud::Ptr & map, diff --git a/corelib/include/rtabmap/core/odometry/OdometryDVO.h b/corelib/include/rtabmap/core/odometry/OdometryDVO.h index 2411cf7793..35427feadc 100644 --- a/corelib/include/rtabmap/core/odometry/OdometryDVO.h +++ b/corelib/include/rtabmap/core/odometry/OdometryDVO.h @@ -40,7 +40,7 @@ class RgbdCameraPyramid; namespace rtabmap { -class RTABMAP_EXP OdometryDVO : public Odometry +class RTABMAP_CORE_EXPORT OdometryDVO : public Odometry { public: OdometryDVO(const rtabmap::ParametersMap & parameters = rtabmap::ParametersMap()); diff --git a/corelib/include/rtabmap/core/odometry/OdometryF2F.h b/corelib/include/rtabmap/core/odometry/OdometryF2F.h index b3da8e8f4f..a2d3bd0995 100644 --- a/corelib/include/rtabmap/core/odometry/OdometryF2F.h +++ b/corelib/include/rtabmap/core/odometry/OdometryF2F.h @@ -35,7 +35,7 @@ namespace rtabmap { class Registration; -class RTABMAP_EXP OdometryF2F : public Odometry +class RTABMAP_CORE_EXPORT OdometryF2F : public Odometry { public: OdometryF2F(const rtabmap::ParametersMap & parameters = rtabmap::ParametersMap()); diff --git a/corelib/include/rtabmap/core/odometry/OdometryF2M.h b/corelib/include/rtabmap/core/odometry/OdometryF2M.h index 9f9ba67610..ce5c117325 100644 --- a/corelib/include/rtabmap/core/odometry/OdometryF2M.h +++ b/corelib/include/rtabmap/core/odometry/OdometryF2M.h @@ -41,7 +41,7 @@ class Signature; class Registration; class Optimizer; -class RTABMAP_EXP OdometryF2M : public Odometry +class RTABMAP_CORE_EXPORT OdometryF2M : public Odometry { public: OdometryF2M(const rtabmap::ParametersMap & parameters = rtabmap::ParametersMap()); diff --git a/corelib/include/rtabmap/core/odometry/OdometryFLOAM.h b/corelib/include/rtabmap/core/odometry/OdometryFLOAM.h index 1878be5a5c..81e9f1ef39 100644 --- a/corelib/include/rtabmap/core/odometry/OdometryFLOAM.h +++ b/corelib/include/rtabmap/core/odometry/OdometryFLOAM.h @@ -35,7 +35,7 @@ class OdomEstimationClass; namespace rtabmap { -class RTABMAP_EXP OdometryFLOAM : public Odometry +class RTABMAP_CORE_EXPORT OdometryFLOAM : public Odometry { public: OdometryFLOAM(const rtabmap::ParametersMap & parameters = rtabmap::ParametersMap()); diff --git a/corelib/include/rtabmap/core/odometry/OdometryFovis.h b/corelib/include/rtabmap/core/odometry/OdometryFovis.h index 2318517382..06b8a0ac6b 100644 --- a/corelib/include/rtabmap/core/odometry/OdometryFovis.h +++ b/corelib/include/rtabmap/core/odometry/OdometryFovis.h @@ -40,7 +40,7 @@ class StereoDepth; namespace rtabmap { -class RTABMAP_EXP OdometryFovis : public Odometry +class RTABMAP_CORE_EXPORT OdometryFovis : public Odometry { public: OdometryFovis(const rtabmap::ParametersMap & parameters = rtabmap::ParametersMap()); diff --git a/corelib/include/rtabmap/core/odometry/OdometryLOAM.h b/corelib/include/rtabmap/core/odometry/OdometryLOAM.h index 17f7056bd8..2977d2116d 100644 --- a/corelib/include/rtabmap/core/odometry/OdometryLOAM.h +++ b/corelib/include/rtabmap/core/odometry/OdometryLOAM.h @@ -40,7 +40,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. namespace rtabmap { -class RTABMAP_EXP OdometryLOAM : public Odometry +class RTABMAP_CORE_EXPORT OdometryLOAM : public Odometry { public: OdometryLOAM(const rtabmap::ParametersMap & parameters = rtabmap::ParametersMap()); diff --git a/corelib/include/rtabmap/core/odometry/OdometryMSCKF.h b/corelib/include/rtabmap/core/odometry/OdometryMSCKF.h index 7ab609f20f..c38cad6c79 100644 --- a/corelib/include/rtabmap/core/odometry/OdometryMSCKF.h +++ b/corelib/include/rtabmap/core/odometry/OdometryMSCKF.h @@ -35,7 +35,7 @@ namespace rtabmap { class ImageProcessorNoROS; class MsckfVioNoROS; -class RTABMAP_EXP OdometryMSCKF : public Odometry +class RTABMAP_CORE_EXPORT OdometryMSCKF : public Odometry { public: OdometryMSCKF(const rtabmap::ParametersMap & parameters = rtabmap::ParametersMap()); diff --git a/corelib/include/rtabmap/core/odometry/OdometryMono.h b/corelib/include/rtabmap/core/odometry/OdometryMono.h index 898b7dbf90..c0b98ceeb1 100644 --- a/corelib/include/rtabmap/core/odometry/OdometryMono.h +++ b/corelib/include/rtabmap/core/odometry/OdometryMono.h @@ -36,7 +36,7 @@ namespace rtabmap { class Memory; class Feature2D; -class RTABMAP_EXP OdometryMono : public Odometry +class RTABMAP_CORE_EXPORT OdometryMono : public Odometry { public: OdometryMono(const rtabmap::ParametersMap & parameters = rtabmap::ParametersMap()); 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 8f5c1fb5a6..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_EXP 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/RtabmapExp.h b/corelib/include/rtabmap/core/odometry/OdometryORBSLAM3.h similarity index 57% rename from corelib/include/rtabmap/core/RtabmapExp.h rename to corelib/include/rtabmap/core/odometry/OdometryORBSLAM3.h index 440c8021fb..b4651a84c9 100644 --- a/corelib/include/rtabmap/core/RtabmapExp.h +++ b/corelib/include/rtabmap/core/odometry/OdometryORBSLAM3.h @@ -1,50 +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 RTABMAPEXP_H -#define RTABMAPEXP_H - -#if defined(_WIN32) - #if defined(rtabmap_core_EXPORTS) - #define RTABMAP_EXP __declspec( dllexport ) - #else - #define RTABMAP_EXP __declspec( dllimport ) - #endif -#else - #define RTABMAP_EXP -#endif - -#ifdef __GNUC__ -#define RTABMAP_DEPRECATED(func, msg) func __attribute__ ((deprecated(msg))) -#elif defined(_MSC_VER) -#define RTABMAP_DEPRECATED(func, msg) __declspec(deprecated(msg)) func -#else -#pragma message("WARNING: You need to implement DEPRECATED for this compiler") -#define RTABMAP_DEPRECATED(func, msg) func -#endif - -#endif // RTABMAPEXP_H +/* +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/include/rtabmap/core/odometry/OdometryOkvis.h b/corelib/include/rtabmap/core/odometry/OdometryOkvis.h index 5861497050..d3fe2a0589 100644 --- a/corelib/include/rtabmap/core/odometry/OdometryOkvis.h +++ b/corelib/include/rtabmap/core/odometry/OdometryOkvis.h @@ -37,7 +37,7 @@ namespace okvis { namespace rtabmap { class OkvisCallbackHandler; -class RTABMAP_EXP OdometryOkvis : public Odometry +class RTABMAP_CORE_EXPORT OdometryOkvis : public Odometry { public: OdometryOkvis(const rtabmap::ParametersMap & parameters = rtabmap::ParametersMap()); diff --git a/corelib/include/rtabmap/core/odometry/OdometryOpen3D.h b/corelib/include/rtabmap/core/odometry/OdometryOpen3D.h index 7f165642af..d7b7de8985 100644 --- a/corelib/include/rtabmap/core/odometry/OdometryOpen3D.h +++ b/corelib/include/rtabmap/core/odometry/OdometryOpen3D.h @@ -32,7 +32,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. namespace rtabmap { -class RTABMAP_EXP OdometryOpen3D : public Odometry +class RTABMAP_CORE_EXPORT OdometryOpen3D : public Odometry { public: OdometryOpen3D(const rtabmap::ParametersMap & parameters = rtabmap::ParametersMap()); diff --git a/corelib/include/rtabmap/core/odometry/OdometryOpenVINS.h b/corelib/include/rtabmap/core/odometry/OdometryOpenVINS.h index ddbf1650fb..1d17e74558 100644 --- a/corelib/include/rtabmap/core/odometry/OdometryOpenVINS.h +++ b/corelib/include/rtabmap/core/odometry/OdometryOpenVINS.h @@ -32,15 +32,15 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. namespace ov_msckf { class VioManager; +struct VioManagerOptions; } namespace rtabmap { -class RTABMAP_EXP OdometryOpenVINS : public Odometry +class RTABMAP_CORE_EXPORT OdometryOpenVINS : public Odometry { public: OdometryOpenVINS(const rtabmap::ParametersMap & parameters = rtabmap::ParametersMap()); - virtual ~OdometryOpenVINS(); virtual void reset(const Transform & initialPose = Transform::getIdentity()); virtual Odometry::Type getType() {return Odometry::kTypeOpenVINS;} @@ -52,12 +52,12 @@ class RTABMAP_EXP OdometryOpenVINS : public Odometry private: #ifdef RTABMAP_OPENVINS - ov_msckf::VioManager * vioManager_; + std::unique_ptr vioManager_; + std::unique_ptr params_; bool initGravity_; - Transform previousPose_; - Transform previousLocalTransform_; - Transform imuLocalTransform_; - std::map imuBuffer_; + Transform previousPoseInv_; + Transform imuLocalTransformInv_; + Eigen::Matrix Phi_; #endif }; diff --git a/corelib/include/rtabmap/core/odometry/OdometryVINS.h b/corelib/include/rtabmap/core/odometry/OdometryVINS.h index 8dc41ca26d..030de4a338 100644 --- a/corelib/include/rtabmap/core/odometry/OdometryVINS.h +++ b/corelib/include/rtabmap/core/odometry/OdometryVINS.h @@ -34,7 +34,7 @@ namespace rtabmap { class VinsEstimator; -class RTABMAP_EXP OdometryVINS : public Odometry +class RTABMAP_CORE_EXPORT OdometryVINS : public Odometry { public: OdometryVINS(const rtabmap::ParametersMap & parameters = rtabmap::ParametersMap()); diff --git a/corelib/include/rtabmap/core/odometry/OdometryViso2.h b/corelib/include/rtabmap/core/odometry/OdometryViso2.h index 52658a1140..ea8b104db3 100644 --- a/corelib/include/rtabmap/core/odometry/OdometryViso2.h +++ b/corelib/include/rtabmap/core/odometry/OdometryViso2.h @@ -34,7 +34,7 @@ class VisualOdometryStereo; namespace rtabmap { -class RTABMAP_EXP OdometryViso2 : public Odometry +class RTABMAP_CORE_EXPORT OdometryViso2 : public Odometry { public: OdometryViso2(const rtabmap::ParametersMap & parameters = rtabmap::ParametersMap()); diff --git a/corelib/include/rtabmap/core/optimizer/OptimizerCVSBA.h b/corelib/include/rtabmap/core/optimizer/OptimizerCVSBA.h index c2c2c0f81b..692ade7912 100644 --- a/corelib/include/rtabmap/core/optimizer/OptimizerCVSBA.h +++ b/corelib/include/rtabmap/core/optimizer/OptimizerCVSBA.h @@ -28,13 +28,11 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #ifndef OPTIMIZERCVSBA_H_ #define OPTIMIZERCVSBA_H_ -#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines - #include namespace rtabmap { -class RTABMAP_EXP OptimizerCVSBA : public Optimizer +class RTABMAP_CORE_EXPORT OptimizerCVSBA : public Optimizer { public: static bool available(); diff --git a/corelib/include/rtabmap/core/optimizer/OptimizerCeres.h b/corelib/include/rtabmap/core/optimizer/OptimizerCeres.h index 9bac559556..abf2010d7b 100644 --- a/corelib/include/rtabmap/core/optimizer/OptimizerCeres.h +++ b/corelib/include/rtabmap/core/optimizer/OptimizerCeres.h @@ -28,13 +28,11 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #ifndef OPTIMIZERCERES_H_ #define OPTIMIZERCERES_H_ -#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines - #include namespace rtabmap { -class RTABMAP_EXP OptimizerCeres : public Optimizer +class RTABMAP_CORE_EXPORT OptimizerCeres : public Optimizer { public: static bool available(); @@ -65,7 +63,7 @@ class RTABMAP_EXP OptimizerCeres : public Optimizer int rootId, const std::map & poses, const std::multimap & links, - const std::map & models, // in case of stereo, Tx should be set + const std::map > & models, // in case of stereo, Tx should be set std::map & points3DMap, const std::map > & wordReferences, // std::set * outliers = 0); diff --git a/corelib/include/rtabmap/core/optimizer/OptimizerG2O.h b/corelib/include/rtabmap/core/optimizer/OptimizerG2O.h index 206552f983..30117d6e56 100644 --- a/corelib/include/rtabmap/core/optimizer/OptimizerG2O.h +++ b/corelib/include/rtabmap/core/optimizer/OptimizerG2O.h @@ -28,13 +28,13 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #ifndef OPTIMIZERG2O_H_ #define OPTIMIZERG2O_H_ -#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines +#include "rtabmap/core/rtabmap_core_export.h" // DLL export/import defines #include namespace rtabmap { -class RTABMAP_EXP OptimizerG2O : public Optimizer +class RTABMAP_CORE_EXPORT OptimizerG2O : public Optimizer { public: static bool available(); diff --git a/corelib/include/rtabmap/core/optimizer/OptimizerGTSAM.h b/corelib/include/rtabmap/core/optimizer/OptimizerGTSAM.h index 465b13ece3..5cec5c2415 100644 --- a/corelib/include/rtabmap/core/optimizer/OptimizerGTSAM.h +++ b/corelib/include/rtabmap/core/optimizer/OptimizerGTSAM.h @@ -28,13 +28,11 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #ifndef OPTIMIZERGTSAM_H_ #define OPTIMIZERGTSAM_H_ -#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines - #include namespace rtabmap { -class RTABMAP_EXP OptimizerGTSAM : public Optimizer +class RTABMAP_CORE_EXPORT OptimizerGTSAM : public Optimizer { public: static bool available(); diff --git a/corelib/include/rtabmap/core/optimizer/OptimizerTORO.h b/corelib/include/rtabmap/core/optimizer/OptimizerTORO.h index 8456946a4f..f2a3cecc6d 100644 --- a/corelib/include/rtabmap/core/optimizer/OptimizerTORO.h +++ b/corelib/include/rtabmap/core/optimizer/OptimizerTORO.h @@ -28,13 +28,11 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #ifndef OPTIMIZERTORO_H_ #define OPTIMIZERTORO_H_ -#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines - #include namespace rtabmap { -class RTABMAP_EXP OptimizerTORO : public Optimizer +class RTABMAP_CORE_EXPORT OptimizerTORO : public Optimizer { public: static bool available(); diff --git a/corelib/include/rtabmap/core/stereo/StereoBM.h b/corelib/include/rtabmap/core/stereo/StereoBM.h index 4e8ef612b5..99d606af01 100644 --- a/corelib/include/rtabmap/core/stereo/StereoBM.h +++ b/corelib/include/rtabmap/core/stereo/StereoBM.h @@ -28,15 +28,13 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #ifndef STEREOBM_H_ #define STEREOBM_H_ -#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines - #include #include #include namespace rtabmap { -class RTABMAP_EXP StereoBM : public StereoDense { +class RTABMAP_CORE_EXPORT StereoBM : public StereoDense { public: StereoBM(int blockSize, int numDisparities); StereoBM(const ParametersMap & parameters = ParametersMap()); diff --git a/corelib/include/rtabmap/core/stereo/StereoSGBM.h b/corelib/include/rtabmap/core/stereo/StereoSGBM.h index d04fb6c65b..1899cef5b2 100644 --- a/corelib/include/rtabmap/core/stereo/StereoSGBM.h +++ b/corelib/include/rtabmap/core/stereo/StereoSGBM.h @@ -28,15 +28,13 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #ifndef STEREOSGBM_H_ #define STEREOSGBM_H_ -#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines - #include #include #include namespace rtabmap { -class RTABMAP_EXP StereoSGBM : public StereoDense { +class RTABMAP_CORE_EXPORT StereoSGBM : public StereoDense { public: StereoSGBM(const ParametersMap & parameters = ParametersMap()); virtual ~StereoSGBM() {} diff --git a/corelib/include/rtabmap/core/util2d.h b/corelib/include/rtabmap/core/util2d.h index 82e894af0f..0b5a9bc3ab 100644 --- a/corelib/include/rtabmap/core/util2d.h +++ b/corelib/include/rtabmap/core/util2d.h @@ -28,7 +28,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #ifndef UTIL2D_H_ #define UTIL2D_H_ -#include +#include #include #include @@ -42,11 +42,11 @@ namespace util2d { // SSD: Sum of Squared Differences -float RTABMAP_EXP ssd(const cv::Mat & windowLeft, const cv::Mat & windowRight); +float RTABMAP_CORE_EXPORT ssd(const cv::Mat & windowLeft, const cv::Mat & windowRight); // SAD: Sum of Absolute intensity Differences -float RTABMAP_EXP sad(const cv::Mat & windowLeft, const cv::Mat & windowRight); +float RTABMAP_CORE_EXPORT sad(const cv::Mat & windowLeft, const cv::Mat & windowRight); -std::vector RTABMAP_EXP calcStereoCorrespondences( +std::vector RTABMAP_CORE_EXPORT calcStereoCorrespondences( const cv::Mat & leftImage, const cv::Mat & rightImage, const std::vector & leftCorners, @@ -59,7 +59,7 @@ std::vector RTABMAP_EXP calcStereoCorrespondences( bool ssdApproach = true); // SSD by default, otherwise it is SAD // exactly as cv::calcOpticalFlowPyrLK but it should be called with pyramid (from cv::buildOpticalFlowPyramid()) and delta drops the y error. -void RTABMAP_EXP calcOpticalFlowPyrLKStereo( cv::InputArray _prevImg, cv::InputArray _nextImg, +void RTABMAP_CORE_EXPORT calcOpticalFlowPyrLKStereo( cv::InputArray _prevImg, cv::InputArray _nextImg, cv::InputArray _prevPts, cv::InputOutputArray _nextPts, cv::OutputArray _status, cv::OutputArray _err, cv::Size winSize = cv::Size(15,3), int maxLevel = 3, @@ -67,16 +67,16 @@ void RTABMAP_EXP calcOpticalFlowPyrLKStereo( cv::InputArray _prevImg, cv::InputA int flags = 0, double minEigThreshold = 1e-4 ); -cv::Mat RTABMAP_EXP disparityFromStereoImages( +cv::Mat RTABMAP_CORE_EXPORT disparityFromStereoImages( const cv::Mat & leftImage, const cv::Mat & rightImage, const ParametersMap & parameters = ParametersMap()); -cv::Mat RTABMAP_EXP depthFromDisparity(const cv::Mat & disparity, +cv::Mat RTABMAP_CORE_EXPORT depthFromDisparity(const cv::Mat & disparity, float fx, float baseline, int type = CV_32FC1); // CV_32FC1 or CV_16UC1 -cv::Mat RTABMAP_EXP depthFromStereoImages( +cv::Mat RTABMAP_CORE_EXPORT depthFromStereoImages( const cv::Mat & leftImage, const cv::Mat & rightImage, const std::vector & leftCorners, @@ -87,63 +87,63 @@ cv::Mat RTABMAP_EXP depthFromStereoImages( int flowIterations = 20, double flowEps = 0.02); -cv::Mat RTABMAP_EXP disparityFromStereoCorrespondences( +cv::Mat RTABMAP_CORE_EXPORT disparityFromStereoCorrespondences( const cv::Size & disparitySize, const std::vector & leftCorners, const std::vector & rightCorners, const std::vector & mask); -cv::Mat RTABMAP_EXP depthFromStereoCorrespondences( +cv::Mat RTABMAP_CORE_EXPORT depthFromStereoCorrespondences( const cv::Mat & leftImage, const std::vector & leftCorners, const std::vector & rightCorners, const std::vector & mask, float fx, float baseline); -cv::Mat RTABMAP_EXP cvtDepthFromFloat(const cv::Mat & depth32F); -cv::Mat RTABMAP_EXP cvtDepthToFloat(const cv::Mat & depth16U); +cv::Mat RTABMAP_CORE_EXPORT cvtDepthFromFloat(const cv::Mat & depth32F); +cv::Mat RTABMAP_CORE_EXPORT cvtDepthToFloat(const cv::Mat & depth16U); -float RTABMAP_EXP getDepth( +float RTABMAP_CORE_EXPORT getDepth( const cv::Mat & depthImage, float x, float y, bool smoothing, float depthErrorRatio = 0.02f, //ratio bool estWithNeighborsIfNull = false); -cv::Rect RTABMAP_EXP computeRoi(const cv::Mat & image, const std::string & roiRatios); -cv::Rect RTABMAP_EXP computeRoi(const cv::Size & imageSize, const std::string & roiRatios); -cv::Rect RTABMAP_EXP computeRoi(const cv::Mat & image, const std::vector & roiRatios); -cv::Rect RTABMAP_EXP computeRoi(const cv::Size & imageSize, const std::vector & roiRatios); +cv::Rect RTABMAP_CORE_EXPORT computeRoi(const cv::Mat & image, const std::string & roiRatios); +cv::Rect RTABMAP_CORE_EXPORT computeRoi(const cv::Size & imageSize, const std::string & roiRatios); +cv::Rect RTABMAP_CORE_EXPORT computeRoi(const cv::Mat & image, const std::vector & roiRatios); +cv::Rect RTABMAP_CORE_EXPORT computeRoi(const cv::Size & imageSize, const std::vector & roiRatios); -cv::Mat RTABMAP_EXP decimate(const cv::Mat & image, int d); -cv::Mat RTABMAP_EXP interpolate(const cv::Mat & image, int factor, float depthErrorRatio = 0.02f); +cv::Mat RTABMAP_CORE_EXPORT decimate(const cv::Mat & image, int d); +cv::Mat RTABMAP_CORE_EXPORT interpolate(const cv::Mat & image, int factor, float depthErrorRatio = 0.02f); // Registration Depth to RGB (return registered depth image) -cv::Mat RTABMAP_EXP registerDepth( +cv::Mat RTABMAP_CORE_EXPORT registerDepth( const cv::Mat & depth, const cv::Mat & depthK, const cv::Size & colorSize, const cv::Mat & colorK, const rtabmap::Transform & transform); -cv::Mat RTABMAP_EXP fillDepthHoles( +cv::Mat RTABMAP_CORE_EXPORT fillDepthHoles( const cv::Mat & depth, int maximumHoleSize = 1, float errorRatio = 0.02f); -void RTABMAP_EXP fillRegisteredDepthHoles( +void RTABMAP_CORE_EXPORT fillRegisteredDepthHoles( cv::Mat & depthRegistered, bool vertical, bool horizontal, bool fillDoubleHoles = false); -cv::Mat RTABMAP_EXP fastBilateralFiltering( +cv::Mat RTABMAP_CORE_EXPORT fastBilateralFiltering( const cv::Mat & depth, float sigmaS = 15.0f, float sigmaR = 0.05f, bool earlyDivision = false); -cv::Mat RTABMAP_EXP brightnessAndContrastAuto( +cv::Mat RTABMAP_CORE_EXPORT brightnessAndContrastAuto( const cv::Mat & src, const cv::Mat & mask, float clipLowHistPercent=0, @@ -151,10 +151,17 @@ cv::Mat RTABMAP_EXP brightnessAndContrastAuto( float * alphaOut = 0, float * betaOut = 0); -cv::Mat RTABMAP_EXP exposureFusion( +cv::Mat RTABMAP_CORE_EXPORT exposureFusion( const std::vector & images); -void RTABMAP_EXP HSVtoRGB( float *r, float *g, float *b, float h, float s, float v ); +void RTABMAP_CORE_EXPORT HSVtoRGB( float *r, float *g, float *b, float h, float s, float v ); + +void RTABMAP_CORE_EXPORT NMS( + const std::vector & ptsIn, + const cv::Mat & descriptorsIn, + std::vector & ptsOut, + cv::Mat & descriptorsOut, + int border, int dist_thresh, int img_width, int img_height); } // namespace util3d } // namespace rtabmap diff --git a/corelib/include/rtabmap/core/util3d.h b/corelib/include/rtabmap/core/util3d.h index eed9dbb1e0..9a903b7bd0 100644 --- a/corelib/include/rtabmap/core/util3d.h +++ b/corelib/include/rtabmap/core/util3d.h @@ -28,7 +28,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #ifndef UTIL3D_H_ #define UTIL3D_H_ -#include "rtabmap/core/RtabmapExp.h" +#include "rtabmap/core/rtabmap_core_export.h" #include #include @@ -48,17 +48,17 @@ namespace rtabmap namespace util3d { -cv::Mat RTABMAP_EXP rgbFromCloud( +cv::Mat RTABMAP_CORE_EXPORT rgbFromCloud( const pcl::PointCloud & cloud, bool bgrOrder = true); -cv::Mat RTABMAP_EXP depthFromCloud( +cv::Mat RTABMAP_CORE_EXPORT depthFromCloud( const pcl::PointCloud & cloud, float & fx, float & fy, bool depth16U = true); -void RTABMAP_EXP rgbdFromCloud( +void RTABMAP_CORE_EXPORT rgbdFromCloud( const pcl::PointCloud & cloud, cv::Mat & rgb, cv::Mat & depth, @@ -67,7 +67,7 @@ void RTABMAP_EXP rgbdFromCloud( bool bgrOrder = true, bool depth16U = true); -pcl::PointXYZ RTABMAP_EXP projectDepthTo3D( +pcl::PointXYZ RTABMAP_CORE_EXPORT projectDepthTo3D( const cv::Mat & depthImage, float x, float y, float cx, float cy, @@ -75,21 +75,22 @@ pcl::PointXYZ RTABMAP_EXP projectDepthTo3D( bool smoothing, float depthErrorRatio = 0.02f); -Eigen::Vector3f RTABMAP_EXP projectDepthTo3DRay( +Eigen::Vector3f RTABMAP_CORE_EXPORT projectDepthTo3DRay( const cv::Size & imageSize, float x, float y, float cx, float cy, float fx, float fy); -RTABMAP_DEPRECATED (pcl::PointCloud::Ptr RTABMAP_EXP cloudFromDepth( +// Use cloudFromDepth with CameraModel interface. +RTABMAP_DEPRECATED pcl::PointCloud::Ptr RTABMAP_CORE_EXPORT cloudFromDepth( const cv::Mat & imageDepth, float cx, float cy, float fx, float fy, int decimation = 1, float maxDepth = 0.0f, float minDepth = 0.0f, - std::vector * validIndices = 0), "Use cloudFromDepth with CameraModel interface."); -pcl::PointCloud::Ptr RTABMAP_EXP cloudFromDepth( + std::vector * validIndices = 0); +pcl::PointCloud::Ptr RTABMAP_CORE_EXPORT cloudFromDepth( const cv::Mat & imageDepth, const CameraModel & model, int decimation = 1, @@ -97,7 +98,8 @@ pcl::PointCloud::Ptr RTABMAP_EXP cloudFromDepth( float minDepth = 0.0f, std::vector * validIndices = 0); -RTABMAP_DEPRECATED (pcl::PointCloud::Ptr RTABMAP_EXP cloudFromDepthRGB( +// Use cloudFromDepthRGB with CameraModel interface. +RTABMAP_DEPRECATED pcl::PointCloud::Ptr RTABMAP_CORE_EXPORT cloudFromDepthRGB( const cv::Mat & imageRgb, const cv::Mat & imageDepth, float cx, float cy, @@ -105,8 +107,8 @@ RTABMAP_DEPRECATED (pcl::PointCloud::Ptr RTABMAP_EXP cloudFrom int decimation = 1, float maxDepth = 0.0f, float minDepth = 0.0f, - std::vector * validIndices = 0), "Use cloudFromDepthRGB with CameraModel interface."); -pcl::PointCloud::Ptr RTABMAP_EXP cloudFromDepthRGB( + std::vector * validIndices = 0); +pcl::PointCloud::Ptr RTABMAP_CORE_EXPORT cloudFromDepthRGB( const cv::Mat & imageRgb, const cv::Mat & imageDepth, const CameraModel & model, @@ -115,7 +117,7 @@ pcl::PointCloud::Ptr RTABMAP_EXP cloudFromDepthRGB( float minDepth = 0.0f, std::vector * validIndices = 0); -pcl::PointCloud::Ptr RTABMAP_EXP cloudFromDisparity( +pcl::PointCloud::Ptr RTABMAP_CORE_EXPORT cloudFromDisparity( const cv::Mat & imageDisparity, const StereoCameraModel & model, int decimation = 1, @@ -123,7 +125,7 @@ pcl::PointCloud::Ptr RTABMAP_EXP cloudFromDisparity( float minDepth = 0.0f, std::vector * validIndices = 0); -pcl::PointCloud::Ptr RTABMAP_EXP cloudFromDisparityRGB( +pcl::PointCloud::Ptr RTABMAP_CORE_EXPORT cloudFromDisparityRGB( const cv::Mat & imageRgb, const cv::Mat & imageDisparity, const StereoCameraModel & model, @@ -132,7 +134,7 @@ pcl::PointCloud::Ptr RTABMAP_EXP cloudFromDisparityRGB( float minDepth = 0.0f, std::vector * validIndices = 0); -pcl::PointCloud::Ptr RTABMAP_EXP cloudFromStereoImages( +pcl::PointCloud::Ptr RTABMAP_CORE_EXPORT cloudFromStereoImages( const cv::Mat & imageLeft, const cv::Mat & imageRight, const StereoCameraModel & model, @@ -142,7 +144,7 @@ pcl::PointCloud::Ptr RTABMAP_EXP cloudFromStereoImages( std::vector * validIndices = 0, const ParametersMap & parameters = ParametersMap()); -pcl::PointCloud::Ptr RTABMAP_EXP cloudFromSensorData( +pcl::PointCloud::Ptr RTABMAP_CORE_EXPORT cloudFromSensorData( const SensorData & sensorData, int decimation = 1, float maxDepth = 0.0f, @@ -165,7 +167,7 @@ pcl::PointCloud::Ptr RTABMAP_EXP cloudFromSensorData( * @param roiRatios, [left, right, top, bottom] region of interest (in ratios) of the image projected. * @return a RGB cloud. */ -pcl::PointCloud::Ptr RTABMAP_EXP cloudRGBFromSensorData( +pcl::PointCloud::Ptr RTABMAP_CORE_EXPORT cloudRGBFromSensorData( const SensorData & sensorData, int decimation = 1, float maxDepth = 0.0f, @@ -177,7 +179,7 @@ pcl::PointCloud::Ptr RTABMAP_EXP cloudRGBFromSensorData( /** * Simulate a laser scan rotating counterclockwise, using middle line of the depth image. */ -pcl::PointCloud RTABMAP_EXP laserScanFromDepthImage( +pcl::PointCloud RTABMAP_CORE_EXPORT laserScanFromDepthImage( const cv::Mat & depthImage, float fx, float fy, @@ -191,7 +193,7 @@ pcl::PointCloud RTABMAP_EXP laserScanFromDepthImage( * The last value of the scan is the most left value of the first depth image. The first value of the scan is the most right value of the last depth image. * */ -pcl::PointCloud RTABMAP_EXP laserScanFromDepthImages( +pcl::PointCloud RTABMAP_CORE_EXPORT laserScanFromDepthImages( const cv::Mat & depthImages, const std::vector & cameraModels, float maxDepth, @@ -200,104 +202,104 @@ pcl::PointCloud RTABMAP_EXP laserScanFromDepthImages( template LaserScan laserScanFromPointCloud(const PointCloud2T & cloud, bool filterNaNs = true, bool is2D = false, const Transform & transform = Transform()); // return CV_32FC3 (x,y,z) -LaserScan RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud & cloud, const Transform & transform = Transform(), bool filterNaNs = true); -LaserScan RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud & cloud, const pcl::IndicesPtr & indices, const Transform & transform = Transform(), bool filterNaNs = true); +LaserScan RTABMAP_CORE_EXPORT laserScanFromPointCloud(const pcl::PointCloud & cloud, const Transform & transform = Transform(), bool filterNaNs = true); +LaserScan RTABMAP_CORE_EXPORT laserScanFromPointCloud(const pcl::PointCloud & cloud, const pcl::IndicesPtr & indices, const Transform & transform = Transform(), bool filterNaNs = true); // return CV_32FC6 (x,y,z,normal_x,normal_y,normal_z) -LaserScan RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud & cloud, const Transform & transform = Transform(), bool filterNaNs = true); -LaserScan RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud & cloud, const pcl::IndicesPtr & indices, const Transform & transform = Transform(), bool filterNaNs = true); -LaserScan RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud & cloud, const pcl::PointCloud & normals, const Transform & transform = Transform(), bool filterNaNs = true); +LaserScan RTABMAP_CORE_EXPORT laserScanFromPointCloud(const pcl::PointCloud & cloud, const Transform & transform = Transform(), bool filterNaNs = true); +LaserScan RTABMAP_CORE_EXPORT laserScanFromPointCloud(const pcl::PointCloud & cloud, const pcl::IndicesPtr & indices, const Transform & transform = Transform(), bool filterNaNs = true); +LaserScan RTABMAP_CORE_EXPORT laserScanFromPointCloud(const pcl::PointCloud & cloud, const pcl::PointCloud & normals, const Transform & transform = Transform(), bool filterNaNs = true); // return CV_32FC4 (x,y,z,rgb) -LaserScan RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud & cloud, const Transform & transform = Transform(), bool filterNaNs = true); -LaserScan RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud & cloud, const pcl::IndicesPtr & indices, const Transform & transform = Transform(), bool filterNaNs = true); +LaserScan RTABMAP_CORE_EXPORT laserScanFromPointCloud(const pcl::PointCloud & cloud, const Transform & transform = Transform(), bool filterNaNs = true); +LaserScan RTABMAP_CORE_EXPORT laserScanFromPointCloud(const pcl::PointCloud & cloud, const pcl::IndicesPtr & indices, const Transform & transform = Transform(), bool filterNaNs = true); // return CV_32FC4 (x,y,z,I) -LaserScan RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud & cloud, const Transform & transform = Transform(), bool filterNaNs = true); -LaserScan RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud & cloud, const pcl::IndicesPtr & indices, const Transform & transform = Transform(), bool filterNaNs = true); +LaserScan RTABMAP_CORE_EXPORT laserScanFromPointCloud(const pcl::PointCloud & cloud, const Transform & transform = Transform(), bool filterNaNs = true); +LaserScan RTABMAP_CORE_EXPORT laserScanFromPointCloud(const pcl::PointCloud & cloud, const pcl::IndicesPtr & indices, const Transform & transform = Transform(), bool filterNaNs = true); // return CV_32FC7 (x,y,z,rgb,normal_x,normal_y,normal_z) -LaserScan RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud & cloud, const pcl::PointCloud & normals, const Transform & transform = Transform(), bool filterNaNs = true); -LaserScan RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud & cloud, const Transform & transform = Transform(), bool filterNaNs = true); -LaserScan RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud & cloud, const pcl::IndicesPtr & indices, const Transform & transform = Transform(), bool filterNaNs = true); +LaserScan RTABMAP_CORE_EXPORT laserScanFromPointCloud(const pcl::PointCloud & cloud, const pcl::PointCloud & normals, const Transform & transform = Transform(), bool filterNaNs = true); +LaserScan RTABMAP_CORE_EXPORT laserScanFromPointCloud(const pcl::PointCloud & cloud, const Transform & transform = Transform(), bool filterNaNs = true); +LaserScan RTABMAP_CORE_EXPORT laserScanFromPointCloud(const pcl::PointCloud & cloud, const pcl::IndicesPtr & indices, const Transform & transform = Transform(), bool filterNaNs = true); // return CV_32FC7 (x,y,z,I,normal_x,normal_y,normal_z) -LaserScan RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud & cloud, const pcl::PointCloud & normals, const Transform & transform = Transform(), bool filterNaNs = true); -LaserScan RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud & cloud, const Transform & transform = Transform(), bool filterNaNs = true); -LaserScan RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud & cloud, const pcl::IndicesPtr & indices, const Transform & transform = Transform(), bool filterNaNs = true); +LaserScan RTABMAP_CORE_EXPORT laserScanFromPointCloud(const pcl::PointCloud & cloud, const pcl::PointCloud & normals, const Transform & transform = Transform(), bool filterNaNs = true); +LaserScan RTABMAP_CORE_EXPORT laserScanFromPointCloud(const pcl::PointCloud & cloud, const Transform & transform = Transform(), bool filterNaNs = true); +LaserScan RTABMAP_CORE_EXPORT laserScanFromPointCloud(const pcl::PointCloud & cloud, const pcl::IndicesPtr & indices, const Transform & transform = Transform(), bool filterNaNs = true); // return CV_32FC2 (x,y) -LaserScan RTABMAP_EXP laserScan2dFromPointCloud(const pcl::PointCloud & cloud, const Transform & transform = Transform(), bool filterNaNs = true); +LaserScan RTABMAP_CORE_EXPORT laserScan2dFromPointCloud(const pcl::PointCloud & cloud, const Transform & transform = Transform(), bool filterNaNs = true); // return CV_32FC3 (x,y,I) -LaserScan RTABMAP_EXP laserScan2dFromPointCloud(const pcl::PointCloud & cloud, const Transform & transform = Transform(), bool filterNaNs = true); +LaserScan RTABMAP_CORE_EXPORT laserScan2dFromPointCloud(const pcl::PointCloud & cloud, const Transform & transform = Transform(), bool filterNaNs = true); // return CV_32FC5 (x,y,normal_x, normal_y, normal_z) -LaserScan RTABMAP_EXP laserScan2dFromPointCloud(const pcl::PointCloud & cloud, const Transform & transform = Transform(), bool filterNaNs = true); -LaserScan RTABMAP_EXP laserScan2dFromPointCloud(const pcl::PointCloud & cloud, const pcl::PointCloud & normals, const Transform & transform = Transform(), bool filterNaNs = true); +LaserScan RTABMAP_CORE_EXPORT laserScan2dFromPointCloud(const pcl::PointCloud & cloud, const Transform & transform = Transform(), bool filterNaNs = true); +LaserScan RTABMAP_CORE_EXPORT laserScan2dFromPointCloud(const pcl::PointCloud & cloud, const pcl::PointCloud & normals, const Transform & transform = Transform(), bool filterNaNs = true); // return CV_32FC6 (x,y,I,normal_x, normal_y, normal_z) -LaserScan RTABMAP_EXP laserScan2dFromPointCloud(const pcl::PointCloud & cloud, const Transform & transform = Transform(), bool filterNaNs = true); -LaserScan RTABMAP_EXP laserScan2dFromPointCloud(const pcl::PointCloud & cloud, const pcl::PointCloud & normals, const Transform & transform = Transform(), bool filterNaNs = true); +LaserScan RTABMAP_CORE_EXPORT laserScan2dFromPointCloud(const pcl::PointCloud & cloud, const Transform & transform = Transform(), bool filterNaNs = true); +LaserScan RTABMAP_CORE_EXPORT laserScan2dFromPointCloud(const pcl::PointCloud & cloud, const pcl::PointCloud & normals, const Transform & transform = Transform(), bool filterNaNs = true); -pcl::PCLPointCloud2::Ptr RTABMAP_EXP laserScanToPointCloud2(const LaserScan & laserScan, const Transform & transform = Transform()); +pcl::PCLPointCloud2::Ptr RTABMAP_CORE_EXPORT laserScanToPointCloud2(const LaserScan & laserScan, const Transform & transform = Transform()); // For 2d laserScan, z is set to null. -pcl::PointCloud::Ptr RTABMAP_EXP laserScanToPointCloud(const LaserScan & laserScan, const Transform & transform = Transform()); +pcl::PointCloud::Ptr RTABMAP_CORE_EXPORT laserScanToPointCloud(const LaserScan & laserScan, const Transform & transform = Transform()); // For laserScan without normals, normals are set to null. -pcl::PointCloud::Ptr RTABMAP_EXP laserScanToPointCloudNormal(const LaserScan & laserScan, const Transform & transform = Transform()); +pcl::PointCloud::Ptr RTABMAP_CORE_EXPORT laserScanToPointCloudNormal(const LaserScan & laserScan, const Transform & transform = Transform()); // For laserScan without rgb, rgb is set to default r,g,b parameters. -pcl::PointCloud::Ptr RTABMAP_EXP laserScanToPointCloudRGB(const LaserScan & laserScan, const Transform & transform = Transform(), unsigned char r = 100, unsigned char g = 100, unsigned char b = 100); +pcl::PointCloud::Ptr RTABMAP_CORE_EXPORT laserScanToPointCloudRGB(const LaserScan & laserScan, const Transform & transform = Transform(), unsigned char r = 100, unsigned char g = 100, unsigned char b = 100); // For laserScan without intensity, intensity is set to intensity parameter. -pcl::PointCloud::Ptr RTABMAP_EXP laserScanToPointCloudI(const LaserScan & laserScan, const Transform & transform = Transform(), float intensity = 0.0f); +pcl::PointCloud::Ptr RTABMAP_CORE_EXPORT laserScanToPointCloudI(const LaserScan & laserScan, const Transform & transform = Transform(), float intensity = 0.0f); // For laserScan without rgb, rgb is set to default r,g,b parameters. // For laserScan without normals, normals are set to null. -pcl::PointCloud::Ptr RTABMAP_EXP laserScanToPointCloudRGBNormal(const LaserScan & laserScan, const Transform & transform = Transform(), unsigned char r = 100, unsigned char g = 100, unsigned char b = 100); +pcl::PointCloud::Ptr RTABMAP_CORE_EXPORT laserScanToPointCloudRGBNormal(const LaserScan & laserScan, const Transform & transform = Transform(), unsigned char r = 100, unsigned char g = 100, unsigned char b = 100); // For laserScan without intensity, intensity is set to default intensity parameter. // For laserScan without normals, normals are set to null. -pcl::PointCloud::Ptr RTABMAP_EXP laserScanToPointCloudINormal(const LaserScan & laserScan, const Transform & transform = Transform(), float intensity = 0.0f); +pcl::PointCloud::Ptr RTABMAP_CORE_EXPORT laserScanToPointCloudINormal(const LaserScan & laserScan, const Transform & transform = Transform(), float intensity = 0.0f); // For 2d laserScan, z is set to null. -pcl::PointXYZ RTABMAP_EXP laserScanToPoint(const LaserScan & laserScan, int index); +pcl::PointXYZ RTABMAP_CORE_EXPORT laserScanToPoint(const LaserScan & laserScan, int index); // For laserScan without normals, normals are set to null. -pcl::PointNormal RTABMAP_EXP laserScanToPointNormal(const LaserScan & laserScan, int index); +pcl::PointNormal RTABMAP_CORE_EXPORT laserScanToPointNormal(const LaserScan & laserScan, int index); // For laserScan without rgb, rgb is set to default r,g,b parameters. -pcl::PointXYZRGB RTABMAP_EXP laserScanToPointRGB(const LaserScan & laserScan, int index, unsigned char r = 100, unsigned char g = 100, unsigned char b = 100); +pcl::PointXYZRGB RTABMAP_CORE_EXPORT laserScanToPointRGB(const LaserScan & laserScan, int index, unsigned char r = 100, unsigned char g = 100, unsigned char b = 100); // For laserScan without intensity, intensity is set to intensity parameter. -pcl::PointXYZI RTABMAP_EXP laserScanToPointI(const LaserScan & laserScan, int index, float intensity); +pcl::PointXYZI RTABMAP_CORE_EXPORT laserScanToPointI(const LaserScan & laserScan, int index, float intensity); // For laserScan without rgb, rgb is set to default r,g,b parameters. // For laserScan without normals, normals are set to null. -pcl::PointXYZRGBNormal RTABMAP_EXP laserScanToPointRGBNormal(const LaserScan & laserScan, int index, unsigned char r, unsigned char g, unsigned char b); +pcl::PointXYZRGBNormal RTABMAP_CORE_EXPORT laserScanToPointRGBNormal(const LaserScan & laserScan, int index, unsigned char r, unsigned char g, unsigned char b); // For laserScan without intensity, intensity is set to default intensity parameter. // For laserScan without normals, normals are set to null. -pcl::PointXYZINormal RTABMAP_EXP laserScanToPointINormal(const LaserScan & laserScan, int index, float intensity); +pcl::PointXYZINormal RTABMAP_CORE_EXPORT laserScanToPointINormal(const LaserScan & laserScan, int index, float intensity); -void RTABMAP_EXP getMinMax3D(const cv::Mat & laserScan, cv::Point3f & min, cv::Point3f & max); -void RTABMAP_EXP getMinMax3D(const cv::Mat & laserScan, pcl::PointXYZ & min, pcl::PointXYZ & max); +void RTABMAP_CORE_EXPORT getMinMax3D(const cv::Mat & laserScan, cv::Point3f & min, cv::Point3f & max); +void RTABMAP_CORE_EXPORT getMinMax3D(const cv::Mat & laserScan, pcl::PointXYZ & min, pcl::PointXYZ & max); -cv::Point3f RTABMAP_EXP projectDisparityTo3D( +cv::Point3f RTABMAP_CORE_EXPORT projectDisparityTo3D( const cv::Point2f & pt, float disparity, const StereoCameraModel & model); -cv::Point3f RTABMAP_EXP projectDisparityTo3D( +cv::Point3f RTABMAP_CORE_EXPORT projectDisparityTo3D( const cv::Point2f & pt, const cv::Mat & disparity, const StereoCameraModel & model); // Register point cloud to camera (return registered depth image 32FC1) -cv::Mat RTABMAP_EXP projectCloudToCamera( +cv::Mat RTABMAP_CORE_EXPORT projectCloudToCamera( const cv::Size & imageSize, const cv::Mat & cameraMatrixK, const cv::Mat & laserScan, // assuming points are already in /base_link coordinate const rtabmap::Transform & cameraTransform); // /base_link -> /camera_link // Register point cloud to camera (return registered depth image 32FC1) -cv::Mat RTABMAP_EXP projectCloudToCamera( +cv::Mat RTABMAP_CORE_EXPORT projectCloudToCamera( const cv::Size & imageSize, const cv::Mat & cameraMatrixK, const pcl::PointCloud::Ptr laserScan, // assuming points are already in /base_link coordinate const rtabmap::Transform & cameraTransform); // /base_link -> /camera_link // Register point cloud to camera (return registered depth image 32FC1) -cv::Mat RTABMAP_EXP projectCloudToCamera( +cv::Mat RTABMAP_CORE_EXPORT projectCloudToCamera( const cv::Size & imageSize, const cv::Mat & cameraMatrixK, const pcl::PCLPointCloud2::Ptr laserScan, // assuming points are already in /base_link coordinate const rtabmap::Transform & cameraTransform); // /base_link -> /camera_link // Direction vertical (>=0), horizontal (<0) -void RTABMAP_EXP fillProjectedCloudHoles( +void RTABMAP_CORE_EXPORT fillProjectedCloudHoles( cv::Mat & depthRegistered, bool verticalDirection, bool fillToBorder); @@ -306,7 +308,7 @@ void RTABMAP_EXP fillProjectedCloudHoles( * For each point, return pixel of the best camera (NodeID->CameraIndex) * looking at it based on the policy and parameters */ -std::vector, pcl::PointXY> > RTABMAP_EXP projectCloudToCameras ( +std::vector, pcl::PointXY> > RTABMAP_CORE_EXPORT projectCloudToCameras ( const pcl::PointCloud & cloud, const std::map & cameraPoses, const std::map > & cameraModels, @@ -320,7 +322,7 @@ std::vector, pcl::PointXY> > RTABMAP_EXP projectC * For each point, return pixel of the best camera (NodeID->CameraIndex) * looking at it based on the policy and parameters */ -std::vector, pcl::PointXY> > RTABMAP_EXP projectCloudToCameras ( +std::vector, pcl::PointXY> > RTABMAP_CORE_EXPORT projectCloudToCameras ( const pcl::PointCloud & cloud, const std::map & cameraPoses, const std::map > & cameraModels, @@ -331,11 +333,11 @@ std::vector, pcl::PointXY> > RTABMAP_EXP projectC bool distanceToCamPolicy = false, const ProgressState * state = 0); -bool RTABMAP_EXP isFinite(const cv::Point3f & pt); +bool RTABMAP_CORE_EXPORT isFinite(const cv::Point3f & pt); -pcl::PointCloud::Ptr RTABMAP_EXP concatenateClouds( +pcl::PointCloud::Ptr RTABMAP_CORE_EXPORT concatenateClouds( const std::list::Ptr> & clouds); -pcl::PointCloud::Ptr RTABMAP_EXP concatenateClouds( +pcl::PointCloud::Ptr RTABMAP_CORE_EXPORT concatenateClouds( const std::list::Ptr> & clouds); /** @@ -347,7 +349,7 @@ pcl::PointCloud::Ptr RTABMAP_EXP concatenateClouds( * two set of indices set are disjoint and/or you need sorted indices, the use of mergeIndices(). * @return the indices concatenated. */ -pcl::IndicesPtr RTABMAP_EXP concatenate( +pcl::IndicesPtr RTABMAP_CORE_EXPORT concatenate( const std::vector & indices); /** @@ -360,16 +362,16 @@ pcl::IndicesPtr RTABMAP_EXP concatenate( * two set of indices set are disjoint and/or you need sorted indices, the use of mergeIndices(). * @return the indices concatenated. */ -pcl::IndicesPtr RTABMAP_EXP concatenate( +pcl::IndicesPtr RTABMAP_CORE_EXPORT concatenate( const pcl::IndicesPtr & indicesA, const pcl::IndicesPtr & indicesB); -void RTABMAP_EXP savePCDWords( +void RTABMAP_CORE_EXPORT savePCDWords( const std::string & fileName, const std::multimap & words, const Transform & transform = Transform::getIdentity()); -void RTABMAP_EXP savePCDWords( +void RTABMAP_CORE_EXPORT savePCDWords( const std::string & fileName, const std::multimap & words, const Transform & transform = Transform::getIdentity()); @@ -378,18 +380,20 @@ void RTABMAP_EXP savePCDWords( * Assume KITTI velodyne format * Return scan 4 channels (format=XYZI). */ -cv::Mat RTABMAP_EXP loadBINScan(const std::string & fileName); -pcl::PointCloud::Ptr RTABMAP_EXP loadBINCloud(const std::string & fileName); -RTABMAP_DEPRECATED(pcl::PointCloud::Ptr RTABMAP_EXP loadBINCloud(const std::string & fileName, int dim), "Use interface without dim argument."); +cv::Mat RTABMAP_CORE_EXPORT loadBINScan(const std::string & fileName); +pcl::PointCloud::Ptr RTABMAP_CORE_EXPORT loadBINCloud(const std::string & fileName); +// Use interface without dim argument. +RTABMAP_DEPRECATED pcl::PointCloud::Ptr RTABMAP_CORE_EXPORT loadBINCloud(const std::string & fileName, int dim); // Load *.pcd, *.ply or *.bin (KITTI format). -LaserScan RTABMAP_EXP loadScan(const std::string & path); +LaserScan RTABMAP_CORE_EXPORT loadScan(const std::string & path); -RTABMAP_DEPRECATED(pcl::PointCloud::Ptr RTABMAP_EXP loadCloud( +// Use loadScan() instead. +RTABMAP_DEPRECATED pcl::PointCloud::Ptr RTABMAP_CORE_EXPORT loadCloud( const std::string & path, const Transform & transform = Transform::getIdentity(), int downsampleStep = 1, - float voxelSize = 0.0f), "Use loadScan() instead."); + float voxelSize = 0.0f); /** * @brief Lidar deskewing diff --git a/corelib/include/rtabmap/core/util3d_correspondences.h b/corelib/include/rtabmap/core/util3d_correspondences.h index f73a59fb36..6bbaeb2d48 100644 --- a/corelib/include/rtabmap/core/util3d_correspondences.h +++ b/corelib/include/rtabmap/core/util3d_correspondences.h @@ -28,7 +28,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #ifndef UTIL3D_CORRESPONDENCES_H_ #define UTIL3D_CORRESPONDENCES_H_ -#include +#include #include #include @@ -44,12 +44,12 @@ namespace util3d { -void RTABMAP_EXP findCorrespondences( +void RTABMAP_CORE_EXPORT findCorrespondences( const std::multimap & wordsA, const std::multimap & wordsB, std::list > & pairs); -void RTABMAP_EXP findCorrespondences( +void RTABMAP_CORE_EXPORT findCorrespondences( const std::multimap & words1, const std::multimap & words2, std::vector & inliers1, @@ -57,7 +57,7 @@ void RTABMAP_EXP findCorrespondences( float maxDepth, std::vector * uniqueCorrespondences = 0); -void RTABMAP_EXP findCorrespondences( +void RTABMAP_CORE_EXPORT findCorrespondences( const std::map & words1, const std::map & words2, std::vector & inliers1, @@ -66,17 +66,17 @@ void RTABMAP_EXP findCorrespondences( std::vector * correspondences = 0); // remove depth by z axis -void RTABMAP_EXP extractXYZCorrespondences(const std::multimap & words1, +void RTABMAP_CORE_EXPORT extractXYZCorrespondences(const std::multimap & words1, const std::multimap & words2, pcl::PointCloud & cloud1, pcl::PointCloud & cloud2); -void RTABMAP_EXP extractXYZCorrespondencesRANSAC(const std::multimap & words1, +void RTABMAP_CORE_EXPORT extractXYZCorrespondencesRANSAC(const std::multimap & words1, const std::multimap & words2, pcl::PointCloud & cloud1, pcl::PointCloud & cloud2); -void RTABMAP_EXP extractXYZCorrespondences(const std::list > & correspondences, +void RTABMAP_CORE_EXPORT extractXYZCorrespondences(const std::list > & correspondences, const cv::Mat & depthImage1, const cv::Mat & depthImage2, float cx, float cy, @@ -85,23 +85,23 @@ void RTABMAP_EXP extractXYZCorrespondences(const std::list & cloud1, pcl::PointCloud & cloud2); -void RTABMAP_EXP extractXYZCorrespondences(const std::list > & correspondences, +void RTABMAP_CORE_EXPORT extractXYZCorrespondences(const std::list > & correspondences, const pcl::PointCloud & cloud1, const pcl::PointCloud & cloud2, pcl::PointCloud & inliers1, pcl::PointCloud & inliers2, char depthAxis); -void RTABMAP_EXP extractXYZCorrespondences(const std::list > & correspondences, +void RTABMAP_CORE_EXPORT extractXYZCorrespondences(const std::list > & correspondences, const pcl::PointCloud & cloud1, const pcl::PointCloud & cloud2, pcl::PointCloud & inliers1, pcl::PointCloud & inliers2, char depthAxis); -int RTABMAP_EXP countUniquePairs(const std::multimap & wordsA, +int RTABMAP_CORE_EXPORT countUniquePairs(const std::multimap & wordsA, const std::multimap & wordsB); -void RTABMAP_EXP filterMaxDepth(pcl::PointCloud & inliers1, +void RTABMAP_CORE_EXPORT filterMaxDepth(pcl::PointCloud & inliers1, pcl::PointCloud & inliers2, float maxDepth, char depthAxis, diff --git a/corelib/include/rtabmap/core/util3d_features.h b/corelib/include/rtabmap/core/util3d_features.h index 3f22cc12e8..2a02bf2eb7 100644 --- a/corelib/include/rtabmap/core/util3d_features.h +++ b/corelib/include/rtabmap/core/util3d_features.h @@ -28,7 +28,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #ifndef UTIL3D_FEATURES_H_ #define UTIL3D_FEATURES_H_ -#include +#include #include #include @@ -44,28 +44,28 @@ namespace util3d { -std::vector RTABMAP_EXP generateKeypoints3DDepth( +std::vector RTABMAP_CORE_EXPORT generateKeypoints3DDepth( const std::vector & keypoints, const cv::Mat & depth, const CameraModel & cameraModel, float minDepth = 0, float maxDepth = 0); -std::vector RTABMAP_EXP generateKeypoints3DDepth( +std::vector RTABMAP_CORE_EXPORT generateKeypoints3DDepth( const std::vector & keypoints, const cv::Mat & depth, const std::vector & cameraModels, float minDepth = 0, float maxDepth = 0); -std::vector RTABMAP_EXP generateKeypoints3DDisparity( +std::vector RTABMAP_CORE_EXPORT generateKeypoints3DDisparity( const std::vector & keypoints, const cv::Mat & disparity, const StereoCameraModel & stereoCameraModel, float minDepth = 0, float maxDepth = 0); -std::vector RTABMAP_EXP generateKeypoints3DStereo( +std::vector RTABMAP_CORE_EXPORT generateKeypoints3DStereo( const std::vector & leftCorners, const std::vector & rightCorners, const StereoCameraModel & model, @@ -73,7 +73,7 @@ std::vector RTABMAP_EXP generateKeypoints3DStereo( float minDepth = 0, float maxDepth = 0); -std::map RTABMAP_EXP generateWords3DMono( +std::map RTABMAP_CORE_EXPORT generateWords3DMono( const std::map & kpts, const std::map & previousKpts, const CameraModel & cameraModel, @@ -84,7 +84,7 @@ std::map RTABMAP_EXP generateWords3DMono( double * variance = 0, std::vector * matchesOut = 0); -std::multimap RTABMAP_EXP aggregate( +std::multimap RTABMAP_CORE_EXPORT aggregate( const std::list & wordIds, const std::vector & keypoints); diff --git a/corelib/include/rtabmap/core/util3d_filtering.h b/corelib/include/rtabmap/core/util3d_filtering.h index 7c365f6409..977a3b91d3 100644 --- a/corelib/include/rtabmap/core/util3d_filtering.h +++ b/corelib/include/rtabmap/core/util3d_filtering.h @@ -28,7 +28,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #ifndef UTIL3D_FILTERING_H_ #define UTIL3D_FILTERING_H_ -#include +#include #include #include #include @@ -48,7 +48,7 @@ namespace util3d * operations like computing normals while the scan has already * normals and voxel filtering is not used. */ -LaserScan RTABMAP_EXP commonFiltering( +LaserScan RTABMAP_CORE_EXPORT commonFiltering( const LaserScan & scan, int downsamplingStep, float rangeMin = 0.0f, @@ -57,7 +57,8 @@ LaserScan RTABMAP_EXP commonFiltering( int normalK = 0, float normalRadius = 0.0f, float groundNormalsUp = 0.0f); -RTABMAP_DEPRECATED(LaserScan RTABMAP_EXP commonFiltering( +// Use version with groundNormalsUp as float. For forceGroundNormalsUp=true, set groundNormalsUp=0.8, otherwise set groundNormalsUp=0.0. +RTABMAP_DEPRECATED LaserScan RTABMAP_CORE_EXPORT commonFiltering( const LaserScan & scan, int downsamplingStep, float rangeMin, @@ -65,75 +66,75 @@ RTABMAP_DEPRECATED(LaserScan RTABMAP_EXP commonFiltering( float voxelSize, int normalK, float normalRadius, - bool forceGroundNormalsUp), "Use version with groundNormalsUp as float. For forceGroundNormalsUp=true, set groundNormalsUp=0.8, otherwise set groundNormalsUp=0.0."); + bool forceGroundNormalsUp); -LaserScan RTABMAP_EXP rangeFiltering( +LaserScan RTABMAP_CORE_EXPORT rangeFiltering( const LaserScan & scan, float rangeMin, float rangeMax); -LaserScan RTABMAP_EXP downsample( +LaserScan RTABMAP_CORE_EXPORT downsample( const LaserScan & cloud, int step); -pcl::PointCloud::Ptr RTABMAP_EXP downsample( +pcl::PointCloud::Ptr RTABMAP_CORE_EXPORT downsample( const pcl::PointCloud::Ptr & cloud, int step); -pcl::PointCloud::Ptr RTABMAP_EXP downsample( +pcl::PointCloud::Ptr RTABMAP_CORE_EXPORT downsample( const pcl::PointCloud::Ptr & cloud, int step); -pcl::PointCloud::Ptr RTABMAP_EXP downsample( +pcl::PointCloud::Ptr RTABMAP_CORE_EXPORT downsample( const pcl::PointCloud::Ptr & cloud, int step); -pcl::PointCloud::Ptr RTABMAP_EXP downsample( +pcl::PointCloud::Ptr RTABMAP_CORE_EXPORT downsample( const pcl::PointCloud::Ptr & cloud, int step); -pcl::PointCloud::Ptr RTABMAP_EXP downsample( +pcl::PointCloud::Ptr RTABMAP_CORE_EXPORT downsample( const pcl::PointCloud::Ptr & cloud, int step); -pcl::PointCloud::Ptr RTABMAP_EXP downsample( +pcl::PointCloud::Ptr RTABMAP_CORE_EXPORT downsample( const pcl::PointCloud::Ptr & cloud, int step); -pcl::PointCloud::Ptr RTABMAP_EXP voxelize( +pcl::PointCloud::Ptr RTABMAP_CORE_EXPORT voxelize( const pcl::PointCloud::Ptr & cloud, const pcl::IndicesPtr & indices, float voxelSize); -pcl::PointCloud::Ptr RTABMAP_EXP voxelize( +pcl::PointCloud::Ptr RTABMAP_CORE_EXPORT voxelize( const pcl::PointCloud::Ptr & cloud, const pcl::IndicesPtr & indices, float voxelSize); -pcl::PointCloud::Ptr RTABMAP_EXP voxelize( +pcl::PointCloud::Ptr RTABMAP_CORE_EXPORT voxelize( const pcl::PointCloud::Ptr & cloud, const pcl::IndicesPtr & indices, float voxelSize); -pcl::PointCloud::Ptr RTABMAP_EXP voxelize( +pcl::PointCloud::Ptr RTABMAP_CORE_EXPORT voxelize( const pcl::PointCloud::Ptr & cloud, const pcl::IndicesPtr & indices, float voxelSize); -pcl::PointCloud::Ptr RTABMAP_EXP voxelize( +pcl::PointCloud::Ptr RTABMAP_CORE_EXPORT voxelize( const pcl::PointCloud::Ptr & cloud, const pcl::IndicesPtr & indices, float voxelSize); -pcl::PointCloud::Ptr RTABMAP_EXP voxelize( +pcl::PointCloud::Ptr RTABMAP_CORE_EXPORT voxelize( const pcl::PointCloud::Ptr & cloud, const pcl::IndicesPtr & indices, float voxelSize); -pcl::PointCloud::Ptr RTABMAP_EXP voxelize( +pcl::PointCloud::Ptr RTABMAP_CORE_EXPORT voxelize( const pcl::PointCloud::Ptr & cloud, float voxelSize); -pcl::PointCloud::Ptr RTABMAP_EXP voxelize( +pcl::PointCloud::Ptr RTABMAP_CORE_EXPORT voxelize( const pcl::PointCloud::Ptr & cloud, float voxelSize); -pcl::PointCloud::Ptr RTABMAP_EXP voxelize( +pcl::PointCloud::Ptr RTABMAP_CORE_EXPORT voxelize( const pcl::PointCloud::Ptr & cloud, float voxelSize); -pcl::PointCloud::Ptr RTABMAP_EXP voxelize( +pcl::PointCloud::Ptr RTABMAP_CORE_EXPORT voxelize( const pcl::PointCloud::Ptr & cloud, float voxelSize); -pcl::PointCloud::Ptr RTABMAP_EXP voxelize( +pcl::PointCloud::Ptr RTABMAP_CORE_EXPORT voxelize( const pcl::PointCloud::Ptr & cloud, float voxelSize); -pcl::PointCloud::Ptr RTABMAP_EXP voxelize( +pcl::PointCloud::Ptr RTABMAP_CORE_EXPORT voxelize( const pcl::PointCloud::Ptr & cloud, float voxelSize); @@ -157,185 +158,185 @@ inline pcl::PointCloud::Ptr uniformSampling( } -pcl::PointCloud::Ptr RTABMAP_EXP randomSampling( +pcl::PointCloud::Ptr RTABMAP_CORE_EXPORT randomSampling( const pcl::PointCloud::Ptr & cloud, int samples); -pcl::PointCloud::Ptr RTABMAP_EXP randomSampling( +pcl::PointCloud::Ptr RTABMAP_CORE_EXPORT randomSampling( const pcl::PointCloud::Ptr & cloud, int samples); -pcl::PointCloud::Ptr RTABMAP_EXP randomSampling( +pcl::PointCloud::Ptr RTABMAP_CORE_EXPORT randomSampling( const pcl::PointCloud::Ptr & cloud, int samples); -pcl::PointCloud::Ptr RTABMAP_EXP randomSampling( +pcl::PointCloud::Ptr RTABMAP_CORE_EXPORT randomSampling( const pcl::PointCloud::Ptr & cloud, int samples); -pcl::PointCloud::Ptr RTABMAP_EXP randomSampling( +pcl::PointCloud::Ptr RTABMAP_CORE_EXPORT randomSampling( const pcl::PointCloud::Ptr & cloud, int samples); -pcl::PointCloud::Ptr RTABMAP_EXP randomSampling( +pcl::PointCloud::Ptr RTABMAP_CORE_EXPORT randomSampling( const pcl::PointCloud::Ptr & cloud, int samples); -pcl::IndicesPtr RTABMAP_EXP passThrough( +pcl::IndicesPtr RTABMAP_CORE_EXPORT passThrough( const pcl::PointCloud::Ptr & cloud, const pcl::IndicesPtr & indices, const std::string & axis, float min, float max, bool negative = false); -pcl::IndicesPtr RTABMAP_EXP passThrough( +pcl::IndicesPtr RTABMAP_CORE_EXPORT passThrough( const pcl::PointCloud::Ptr & cloud, const pcl::IndicesPtr & indices, const std::string & axis, float min, float max, bool negative = false); -pcl::IndicesPtr RTABMAP_EXP passThrough( +pcl::IndicesPtr RTABMAP_CORE_EXPORT passThrough( const pcl::PointCloud::Ptr & cloud, const pcl::IndicesPtr & indices, const std::string & axis, float min, float max, bool negative = false); -pcl::IndicesPtr RTABMAP_EXP passThrough( +pcl::IndicesPtr RTABMAP_CORE_EXPORT passThrough( const pcl::PointCloud::Ptr & cloud, const pcl::IndicesPtr & indices, const std::string & axis, float min, float max, bool negative = false); -pcl::IndicesPtr RTABMAP_EXP passThrough( +pcl::IndicesPtr RTABMAP_CORE_EXPORT passThrough( const pcl::PointCloud::Ptr & cloud, const pcl::IndicesPtr & indices, const std::string & axis, float min, float max, bool negative = false); -pcl::IndicesPtr RTABMAP_EXP passThrough( +pcl::IndicesPtr RTABMAP_CORE_EXPORT passThrough( const pcl::PointCloud::Ptr & cloud, const pcl::IndicesPtr & indices, const std::string & axis, float min, float max, bool negative = false); -pcl::PointCloud::Ptr RTABMAP_EXP passThrough( +pcl::PointCloud::Ptr RTABMAP_CORE_EXPORT passThrough( const pcl::PointCloud::Ptr & cloud, const std::string & axis, float min, float max, bool negative = false); -pcl::PointCloud::Ptr RTABMAP_EXP passThrough( +pcl::PointCloud::Ptr RTABMAP_CORE_EXPORT passThrough( const pcl::PointCloud::Ptr & cloud, const std::string & axis, float min, float max, bool negative = false); -pcl::PointCloud::Ptr RTABMAP_EXP passThrough( +pcl::PointCloud::Ptr RTABMAP_CORE_EXPORT passThrough( const pcl::PointCloud::Ptr & cloud, const std::string & axis, float min, float max, bool negative = false); -pcl::PointCloud::Ptr RTABMAP_EXP passThrough( +pcl::PointCloud::Ptr RTABMAP_CORE_EXPORT passThrough( const pcl::PointCloud::Ptr & cloud, const std::string & axis, float min, float max, bool negative = false); -pcl::PointCloud::Ptr RTABMAP_EXP passThrough( +pcl::PointCloud::Ptr RTABMAP_CORE_EXPORT passThrough( const pcl::PointCloud::Ptr & cloud, const std::string & axis, float min, float max, bool negative = false); -pcl::PointCloud::Ptr RTABMAP_EXP passThrough( +pcl::PointCloud::Ptr RTABMAP_CORE_EXPORT passThrough( const pcl::PointCloud::Ptr & cloud, const std::string & axis, float min, float max, bool negative = false); -pcl::IndicesPtr RTABMAP_EXP cropBox( +pcl::IndicesPtr RTABMAP_CORE_EXPORT cropBox( const pcl::PCLPointCloud2::Ptr & cloud, const pcl::IndicesPtr & indices, const Eigen::Vector4f & min, const Eigen::Vector4f & max, const Transform & transform = Transform::getIdentity(), bool negative = false); -pcl::IndicesPtr RTABMAP_EXP cropBox( +pcl::IndicesPtr RTABMAP_CORE_EXPORT cropBox( const pcl::PointCloud::Ptr & cloud, const pcl::IndicesPtr & indices, const Eigen::Vector4f & min, const Eigen::Vector4f & max, const Transform & transform = Transform::getIdentity(), bool negative = false); -pcl::IndicesPtr RTABMAP_EXP cropBox( +pcl::IndicesPtr RTABMAP_CORE_EXPORT cropBox( const pcl::PointCloud::Ptr & cloud, const pcl::IndicesPtr & indices, const Eigen::Vector4f & min, const Eigen::Vector4f & max, const Transform & transform = Transform::getIdentity(), bool negative = false); -pcl::IndicesPtr RTABMAP_EXP cropBox( +pcl::IndicesPtr RTABMAP_CORE_EXPORT cropBox( const pcl::PointCloud::Ptr & cloud, const pcl::IndicesPtr & indices, const Eigen::Vector4f & min, const Eigen::Vector4f & max, const Transform & transform = Transform::getIdentity(), bool negative = false); -pcl::IndicesPtr RTABMAP_EXP cropBox( +pcl::IndicesPtr RTABMAP_CORE_EXPORT cropBox( const pcl::PointCloud::Ptr & cloud, const pcl::IndicesPtr & indices, const Eigen::Vector4f & min, const Eigen::Vector4f & max, const Transform & transform = Transform::getIdentity(), bool negative = false); -pcl::IndicesPtr RTABMAP_EXP cropBox( +pcl::IndicesPtr RTABMAP_CORE_EXPORT cropBox( const pcl::PointCloud::Ptr & cloud, const pcl::IndicesPtr & indices, const Eigen::Vector4f & min, const Eigen::Vector4f & max, const Transform & transform = Transform::getIdentity(), bool negative = false); -pcl::IndicesPtr RTABMAP_EXP cropBox( +pcl::IndicesPtr RTABMAP_CORE_EXPORT cropBox( const pcl::PointCloud::Ptr & cloud, const pcl::IndicesPtr & indices, const Eigen::Vector4f & min, const Eigen::Vector4f & max, const Transform & transform = Transform::getIdentity(), bool negative = false); -pcl::PointCloud::Ptr RTABMAP_EXP cropBox( +pcl::PointCloud::Ptr RTABMAP_CORE_EXPORT cropBox( const pcl::PointCloud::Ptr & cloud, const Eigen::Vector4f & min, const Eigen::Vector4f & max, const Transform & transform = Transform::getIdentity(), bool negative = false); -pcl::PointCloud::Ptr RTABMAP_EXP cropBox( +pcl::PointCloud::Ptr RTABMAP_CORE_EXPORT cropBox( const pcl::PointCloud::Ptr & cloud, const Eigen::Vector4f & min, const Eigen::Vector4f & max, const Transform & transform = Transform::getIdentity(), bool negative = false); -pcl::PointCloud::Ptr RTABMAP_EXP cropBox( +pcl::PointCloud::Ptr RTABMAP_CORE_EXPORT cropBox( const pcl::PointCloud::Ptr & cloud, const Eigen::Vector4f & min, const Eigen::Vector4f & max, const Transform & transform = Transform::getIdentity(), bool negative = false); -pcl::PointCloud::Ptr RTABMAP_EXP cropBox( +pcl::PointCloud::Ptr RTABMAP_CORE_EXPORT cropBox( const pcl::PointCloud::Ptr & cloud, const Eigen::Vector4f & min, const Eigen::Vector4f & max, const Transform & transform = Transform::getIdentity(), bool negative = false); -pcl::PointCloud::Ptr RTABMAP_EXP cropBox( +pcl::PointCloud::Ptr RTABMAP_CORE_EXPORT cropBox( const pcl::PointCloud::Ptr & cloud, const Eigen::Vector4f & min, const Eigen::Vector4f & max, const Transform & transform = Transform::getIdentity(), bool negative = false); -pcl::PointCloud::Ptr RTABMAP_EXP cropBox( +pcl::PointCloud::Ptr RTABMAP_CORE_EXPORT cropBox( const pcl::PointCloud::Ptr & cloud, const Eigen::Vector4f & min, const Eigen::Vector4f & max, @@ -343,7 +344,7 @@ pcl::PointCloud::Ptr RTABMAP_EXP cropBox( bool negative = false); //Note: This assumes a coordinate system where X is forward, * Y is up, and Z is right. -pcl::IndicesPtr RTABMAP_EXP frustumFiltering( +pcl::IndicesPtr RTABMAP_CORE_EXPORT frustumFiltering( const pcl::PointCloud::Ptr & cloud, const pcl::IndicesPtr & indices, const Transform & cameraPose, @@ -353,7 +354,7 @@ pcl::IndicesPtr RTABMAP_EXP frustumFiltering( float farClipPlaneDistance, bool negative = false); //Note: This assumes a coordinate system where X is forward, * Y is up, and Z is right. -pcl::PointCloud::Ptr RTABMAP_EXP frustumFiltering( +pcl::PointCloud::Ptr RTABMAP_CORE_EXPORT frustumFiltering( const pcl::PointCloud::Ptr & cloud, const Transform & cameraPose, float horizontalFOV, // in degrees, xfov = atan((image_width/2)/fx)*2 @@ -362,7 +363,7 @@ pcl::PointCloud::Ptr RTABMAP_EXP frustumFiltering( float farClipPlaneDistance, bool negative = false); //Note: This assumes a coordinate system where X is forward, * Y is up, and Z is right. -pcl::PointCloud::Ptr RTABMAP_EXP frustumFiltering( +pcl::PointCloud::Ptr RTABMAP_CORE_EXPORT frustumFiltering( const pcl::PointCloud::Ptr & cloud, const Transform & cameraPose, float horizontalFOV, // in degrees, xfov = atan((image_width/2)/fx)*2 @@ -372,48 +373,48 @@ pcl::PointCloud::Ptr RTABMAP_EXP frustumFiltering( bool negative = false); -pcl::PointCloud::Ptr RTABMAP_EXP removeNaNFromPointCloud( +pcl::PointCloud::Ptr RTABMAP_CORE_EXPORT removeNaNFromPointCloud( const pcl::PointCloud::Ptr & cloud); -pcl::PointCloud::Ptr RTABMAP_EXP removeNaNFromPointCloud( +pcl::PointCloud::Ptr RTABMAP_CORE_EXPORT removeNaNFromPointCloud( const pcl::PointCloud::Ptr & cloud); -pcl::PointCloud::Ptr RTABMAP_EXP removeNaNFromPointCloud( +pcl::PointCloud::Ptr RTABMAP_CORE_EXPORT removeNaNFromPointCloud( const pcl::PointCloud::Ptr & cloud); -pcl::PCLPointCloud2::Ptr RTABMAP_EXP removeNaNFromPointCloud( +pcl::PCLPointCloud2::Ptr RTABMAP_CORE_EXPORT removeNaNFromPointCloud( const pcl::PCLPointCloud2::Ptr & cloud); -pcl::PointCloud::Ptr RTABMAP_EXP removeNaNNormalsFromPointCloud( +pcl::PointCloud::Ptr RTABMAP_CORE_EXPORT removeNaNNormalsFromPointCloud( const pcl::PointCloud::Ptr & cloud); -pcl::PointCloud::Ptr RTABMAP_EXP removeNaNNormalsFromPointCloud( +pcl::PointCloud::Ptr RTABMAP_CORE_EXPORT removeNaNNormalsFromPointCloud( const pcl::PointCloud::Ptr & cloud); -pcl::PointCloud::Ptr RTABMAP_EXP removeNaNNormalsFromPointCloud( +pcl::PointCloud::Ptr RTABMAP_CORE_EXPORT removeNaNNormalsFromPointCloud( const pcl::PointCloud::Ptr & cloud); /** * For convenience. */ -pcl::IndicesPtr RTABMAP_EXP radiusFiltering( +pcl::IndicesPtr RTABMAP_CORE_EXPORT radiusFiltering( const pcl::PointCloud::Ptr & cloud, float radiusSearch, int minNeighborsInRadius); -pcl::IndicesPtr RTABMAP_EXP radiusFiltering( +pcl::IndicesPtr RTABMAP_CORE_EXPORT radiusFiltering( const pcl::PointCloud::Ptr & cloud, float radiusSearch, int minNeighborsInRadius); -pcl::IndicesPtr RTABMAP_EXP radiusFiltering( +pcl::IndicesPtr RTABMAP_CORE_EXPORT radiusFiltering( const pcl::PointCloud::Ptr & cloud, float radiusSearch, int minNeighborsInRadius); -pcl::IndicesPtr RTABMAP_EXP radiusFiltering( +pcl::IndicesPtr RTABMAP_CORE_EXPORT radiusFiltering( const pcl::PointCloud::Ptr & cloud, float radiusSearch, int minNeighborsInRadius); -pcl::IndicesPtr RTABMAP_EXP radiusFiltering( +pcl::IndicesPtr RTABMAP_CORE_EXPORT radiusFiltering( const pcl::PointCloud::Ptr & cloud, float radiusSearch, int minNeighborsInRadius); -pcl::IndicesPtr RTABMAP_EXP radiusFiltering( +pcl::IndicesPtr RTABMAP_CORE_EXPORT radiusFiltering( const pcl::PointCloud::Ptr & cloud, float radiusSearch, int minNeighborsInRadius); @@ -430,69 +431,69 @@ pcl::IndicesPtr RTABMAP_EXP radiusFiltering( * @return the indices of the points satisfying the parameters. */ -pcl::IndicesPtr RTABMAP_EXP radiusFiltering( +pcl::IndicesPtr RTABMAP_CORE_EXPORT radiusFiltering( const pcl::PointCloud::Ptr & cloud, const pcl::IndicesPtr & indices, float radiusSearch, int minNeighborsInRadius); -pcl::IndicesPtr RTABMAP_EXP radiusFiltering( +pcl::IndicesPtr RTABMAP_CORE_EXPORT radiusFiltering( const pcl::PointCloud::Ptr & cloud, const pcl::IndicesPtr & indices, float radiusSearch, int minNeighborsInRadius); -pcl::IndicesPtr RTABMAP_EXP radiusFiltering( +pcl::IndicesPtr RTABMAP_CORE_EXPORT radiusFiltering( const pcl::PointCloud::Ptr & cloud, const pcl::IndicesPtr & indices, float radiusSearch, int minNeighborsInRadius); -pcl::IndicesPtr RTABMAP_EXP radiusFiltering( +pcl::IndicesPtr RTABMAP_CORE_EXPORT radiusFiltering( const pcl::PointCloud::Ptr & cloud, const pcl::IndicesPtr & indices, float radiusSearch, int minNeighborsInRadius); -pcl::IndicesPtr RTABMAP_EXP radiusFiltering( +pcl::IndicesPtr RTABMAP_CORE_EXPORT radiusFiltering( const pcl::PointCloud::Ptr & cloud, const pcl::IndicesPtr & indices, float radiusSearch, int minNeighborsInRadius); -pcl::IndicesPtr RTABMAP_EXP radiusFiltering( +pcl::IndicesPtr RTABMAP_CORE_EXPORT radiusFiltering( const pcl::PointCloud::Ptr & cloud, const pcl::IndicesPtr & indices, float radiusSearch, int minNeighborsInRadius); /* for convenience */ -pcl::IndicesPtr RTABMAP_EXP proportionalRadiusFiltering( +pcl::IndicesPtr RTABMAP_CORE_EXPORT proportionalRadiusFiltering( const pcl::PointCloud::Ptr & cloud, const std::vector & viewpointIndices, const std::map & viewpoints, float factor=0.01f, float neighborScale=2.0f); -pcl::IndicesPtr RTABMAP_EXP proportionalRadiusFiltering( +pcl::IndicesPtr RTABMAP_CORE_EXPORT proportionalRadiusFiltering( const pcl::PointCloud::Ptr & cloud, const std::vector & viewpointIndices, const std::map & viewpoints, float factor=0.01f, float neighborScale=2.0f); -pcl::IndicesPtr RTABMAP_EXP proportionalRadiusFiltering( +pcl::IndicesPtr RTABMAP_CORE_EXPORT proportionalRadiusFiltering( const pcl::PointCloud::Ptr & cloud, const std::vector & viewpointIndices, const std::map & viewpoints, float factor=0.01f, float neighborScale=2.0f); -pcl::IndicesPtr RTABMAP_EXP proportionalRadiusFiltering( +pcl::IndicesPtr RTABMAP_CORE_EXPORT proportionalRadiusFiltering( const pcl::PointCloud::Ptr & cloud, const std::vector & viewpointIndices, const std::map & viewpoints, float factor=0.01f, float neighborScale=2.0f); -pcl::IndicesPtr RTABMAP_EXP proportionalRadiusFiltering( +pcl::IndicesPtr RTABMAP_CORE_EXPORT proportionalRadiusFiltering( const pcl::PointCloud::Ptr & cloud, const std::vector & viewpointIndices, const std::map & viewpoints, float factor=0.01f, float neighborScale=2.0f); -pcl::IndicesPtr RTABMAP_EXP proportionalRadiusFiltering( +pcl::IndicesPtr RTABMAP_CORE_EXPORT proportionalRadiusFiltering( const pcl::PointCloud::Ptr & cloud, const std::vector & viewpointIndices, const std::map & viewpoints, @@ -511,42 +512,42 @@ pcl::IndicesPtr RTABMAP_EXP proportionalRadiusFiltering( * @return the indices of the points satisfying the parameters. */ -pcl::IndicesPtr RTABMAP_EXP proportionalRadiusFiltering( +pcl::IndicesPtr RTABMAP_CORE_EXPORT proportionalRadiusFiltering( const pcl::PointCloud::Ptr & cloud, const pcl::IndicesPtr & indices, const std::vector & viewpointIndices, const std::map & viewpoints, float factor=0.01f, float neighborScale=2.0f); -pcl::IndicesPtr RTABMAP_EXP proportionalRadiusFiltering( +pcl::IndicesPtr RTABMAP_CORE_EXPORT proportionalRadiusFiltering( const pcl::PointCloud::Ptr & cloud, const pcl::IndicesPtr & indices, const std::vector & viewpointIndices, const std::map & viewpoints, float factor=0.01f, float neighborScale=2.0f); -pcl::IndicesPtr RTABMAP_EXP proportionalRadiusFiltering( +pcl::IndicesPtr RTABMAP_CORE_EXPORT proportionalRadiusFiltering( const pcl::PointCloud::Ptr & cloud, const pcl::IndicesPtr & indices, const std::vector & viewpointIndices, const std::map & viewpoints, float factor=0.01f, float neighborScale=2.0f); -pcl::IndicesPtr RTABMAP_EXP proportionalRadiusFiltering( +pcl::IndicesPtr RTABMAP_CORE_EXPORT proportionalRadiusFiltering( const pcl::PointCloud::Ptr & cloud, const pcl::IndicesPtr & indices, const std::vector & viewpointIndices, const std::map & viewpoints, float factor=0.01f, float neighborScale=2.0f); -pcl::IndicesPtr RTABMAP_EXP proportionalRadiusFiltering( +pcl::IndicesPtr RTABMAP_CORE_EXPORT proportionalRadiusFiltering( const pcl::PointCloud::Ptr & cloud, const pcl::IndicesPtr & indices, const std::vector & viewpointIndices, const std::map & viewpoints, float factor=0.01f, float neighborScale=2.0f); -pcl::IndicesPtr RTABMAP_EXP proportionalRadiusFiltering( +pcl::IndicesPtr RTABMAP_CORE_EXPORT proportionalRadiusFiltering( const pcl::PointCloud::Ptr & cloud, const pcl::IndicesPtr & indices, const std::vector & viewpointIndices, @@ -557,7 +558,7 @@ pcl::IndicesPtr RTABMAP_EXP proportionalRadiusFiltering( /** * For convenience. */ -pcl::PointCloud::Ptr RTABMAP_EXP subtractFiltering( +pcl::PointCloud::Ptr RTABMAP_CORE_EXPORT subtractFiltering( const pcl::PointCloud::Ptr & cloud, const pcl::PointCloud::Ptr & substractCloud, float radiusSearch, @@ -572,7 +573,7 @@ pcl::PointCloud::Ptr RTABMAP_EXP subtractFiltering( * @param radiusSearch the radius in meter. * @return the indices of the points satisfying the parameters. */ -pcl::IndicesPtr RTABMAP_EXP subtractFiltering( +pcl::IndicesPtr RTABMAP_CORE_EXPORT subtractFiltering( const pcl::PointCloud::Ptr & cloud, const pcl::IndicesPtr & indices, const pcl::PointCloud::Ptr & substractCloud, @@ -583,7 +584,7 @@ pcl::IndicesPtr RTABMAP_EXP subtractFiltering( /** * For convenience. */ -pcl::PointCloud::Ptr RTABMAP_EXP subtractFiltering( +pcl::PointCloud::Ptr RTABMAP_CORE_EXPORT subtractFiltering( const pcl::PointCloud::Ptr & cloud, const pcl::PointCloud::Ptr & substractCloud, float radiusSearch, @@ -599,7 +600,7 @@ pcl::PointCloud::Ptr RTABMAP_EXP subtractFiltering( * @param radiusSearch the radius in meter. * @return the indices of the points satisfying the parameters. */ -pcl::IndicesPtr RTABMAP_EXP subtractFiltering( +pcl::IndicesPtr RTABMAP_CORE_EXPORT subtractFiltering( const pcl::PointCloud::Ptr & cloud, const pcl::IndicesPtr & indices, const pcl::PointCloud::Ptr & substractCloud, @@ -611,13 +612,13 @@ pcl::IndicesPtr RTABMAP_EXP subtractFiltering( /** * For convenience. */ -pcl::PointCloud::Ptr RTABMAP_EXP subtractFiltering( +pcl::PointCloud::Ptr RTABMAP_CORE_EXPORT subtractFiltering( const pcl::PointCloud::Ptr & cloud, const pcl::PointCloud::Ptr & substractCloud, float radiusSearch, float maxAngle = M_PI/4.0f, int minNeighborsInRadius = 1); -pcl::PointCloud::Ptr RTABMAP_EXP subtractFiltering( +pcl::PointCloud::Ptr RTABMAP_CORE_EXPORT subtractFiltering( const pcl::PointCloud::Ptr & cloud, const pcl::PointCloud::Ptr & substractCloud, float radiusSearch, @@ -633,7 +634,7 @@ pcl::PointCloud::Ptr RTABMAP_EXP subtractFiltering( * @param radiusSearch the radius in meter. * @return the indices of the points satisfying the parameters. */ -pcl::IndicesPtr RTABMAP_EXP subtractFiltering( +pcl::IndicesPtr RTABMAP_CORE_EXPORT subtractFiltering( const pcl::PointCloud::Ptr & cloud, const pcl::IndicesPtr & indices, const pcl::PointCloud::Ptr & substractCloud, @@ -641,7 +642,7 @@ pcl::IndicesPtr RTABMAP_EXP subtractFiltering( float radiusSearch, float maxAngle = M_PI/4.0f, int minNeighborsInRadius = 1); -pcl::IndicesPtr RTABMAP_EXP subtractFiltering( +pcl::IndicesPtr RTABMAP_CORE_EXPORT subtractFiltering( const pcl::PointCloud::Ptr & cloud, const pcl::IndicesPtr & indices, const pcl::PointCloud::Ptr & substractCloud, @@ -659,7 +660,7 @@ pcl::IndicesPtr RTABMAP_EXP subtractFiltering( * @param radiusSearchRatio the ratio used to compute the radius at different distances (e.g., a ratio of 0.1 at 4 m results in a radius of 4 cm). * @return the indices of the points satisfying the parameters. */ -pcl::IndicesPtr RTABMAP_EXP subtractAdaptiveFiltering( +pcl::IndicesPtr RTABMAP_CORE_EXPORT subtractAdaptiveFiltering( const pcl::PointCloud::Ptr & cloud, const pcl::IndicesPtr & indices, const pcl::PointCloud::Ptr & substractCloud, @@ -677,7 +678,7 @@ pcl::IndicesPtr RTABMAP_EXP subtractAdaptiveFiltering( * @param radiusSearchRatio the ratio used to compute the radius at different distances (e.g., a ratio of 0.01 at 4 m results in a radius of 4 cm). * @return the indices of the points satisfying the parameters. */ -pcl::IndicesPtr RTABMAP_EXP subtractAdaptiveFiltering( +pcl::IndicesPtr RTABMAP_CORE_EXPORT subtractAdaptiveFiltering( const pcl::PointCloud::Ptr & cloud, const pcl::IndicesPtr & indices, const pcl::PointCloud::Ptr & substractCloud, @@ -692,14 +693,14 @@ pcl::IndicesPtr RTABMAP_EXP subtractAdaptiveFiltering( * For convenience. */ -pcl::IndicesPtr RTABMAP_EXP normalFiltering( +pcl::IndicesPtr RTABMAP_CORE_EXPORT normalFiltering( const pcl::PointCloud::Ptr & cloud, float angleMax, const Eigen::Vector4f & normal, int normalKSearch, const Eigen::Vector4f & viewpoint, float groundNormalsUp = 0.0f); -pcl::IndicesPtr RTABMAP_EXP normalFiltering( +pcl::IndicesPtr RTABMAP_CORE_EXPORT normalFiltering( const pcl::PointCloud::Ptr & cloud, float angleMax, const Eigen::Vector4f & normal, @@ -723,7 +724,7 @@ pcl::IndicesPtr RTABMAP_EXP normalFiltering( * @param viewpoint from which viewpoint the normals should be estimated (see pcl::NormalEstimation). * @return the indices of the points which respect the normal constraint. */ -pcl::IndicesPtr RTABMAP_EXP normalFiltering( +pcl::IndicesPtr RTABMAP_CORE_EXPORT normalFiltering( const pcl::PointCloud::Ptr & cloud, const pcl::IndicesPtr & indices, float angleMax, @@ -731,7 +732,7 @@ pcl::IndicesPtr RTABMAP_EXP normalFiltering( int normalKSearch, const Eigen::Vector4f & viewpoint, float groundNormalsUp = 0.0f); -pcl::IndicesPtr RTABMAP_EXP normalFiltering( +pcl::IndicesPtr RTABMAP_CORE_EXPORT normalFiltering( const pcl::PointCloud::Ptr & cloud, const pcl::IndicesPtr & indices, float angleMax, @@ -739,7 +740,7 @@ pcl::IndicesPtr RTABMAP_EXP normalFiltering( int normalKSearch, const Eigen::Vector4f & viewpoint, float groundNormalsUp = 0.0f); -pcl::IndicesPtr RTABMAP_EXP normalFiltering( +pcl::IndicesPtr RTABMAP_CORE_EXPORT normalFiltering( const pcl::PointCloud::Ptr & cloud, const pcl::IndicesPtr & indices, float angleMax, @@ -747,7 +748,7 @@ pcl::IndicesPtr RTABMAP_EXP normalFiltering( int normalKSearch, const Eigen::Vector4f & viewpoint, float groundNormalsUp = 0.0f); -pcl::IndicesPtr RTABMAP_EXP normalFiltering( +pcl::IndicesPtr RTABMAP_CORE_EXPORT normalFiltering( const pcl::PointCloud::Ptr & cloud, const pcl::IndicesPtr & indices, float angleMax, @@ -755,7 +756,7 @@ pcl::IndicesPtr RTABMAP_EXP normalFiltering( int normalKSearch, const Eigen::Vector4f & viewpoint, float groundNormalsUp = 0.0f); -pcl::IndicesPtr RTABMAP_EXP normalFiltering( +pcl::IndicesPtr RTABMAP_CORE_EXPORT normalFiltering( const pcl::PointCloud::Ptr & cloud, const pcl::IndicesPtr & indices, float angleMax, @@ -763,7 +764,7 @@ pcl::IndicesPtr RTABMAP_EXP normalFiltering( int normalKSearch, const Eigen::Vector4f & viewpoint, float groundNormalsUp = 0.0f); -pcl::IndicesPtr RTABMAP_EXP normalFiltering( +pcl::IndicesPtr RTABMAP_CORE_EXPORT normalFiltering( const pcl::PointCloud::Ptr & cloud, const pcl::IndicesPtr & indices, float angleMax, @@ -775,13 +776,13 @@ pcl::IndicesPtr RTABMAP_EXP normalFiltering( /** * For convenience. */ -std::vector RTABMAP_EXP extractClusters( +std::vector RTABMAP_CORE_EXPORT extractClusters( const pcl::PointCloud::Ptr & cloud, float clusterTolerance, int minClusterSize, int maxClusterSize = std::numeric_limits::max(), int * biggestClusterIndex = 0); -std::vector RTABMAP_EXP extractClusters( +std::vector RTABMAP_CORE_EXPORT extractClusters( const pcl::PointCloud::Ptr & cloud, float clusterTolerance, int minClusterSize, @@ -800,42 +801,42 @@ std::vector RTABMAP_EXP extractClusters( * @param biggestClusterIndex the index of the biggest cluster, if the clusters are empty, a negative index is set. * @return the indices of each cluster found. */ -std::vector RTABMAP_EXP extractClusters( +std::vector RTABMAP_CORE_EXPORT extractClusters( const pcl::PointCloud::Ptr & cloud, const pcl::IndicesPtr & indices, float clusterTolerance, int minClusterSize, int maxClusterSize = std::numeric_limits::max(), int * biggestClusterIndex = 0); -std::vector RTABMAP_EXP extractClusters( +std::vector RTABMAP_CORE_EXPORT extractClusters( const pcl::PointCloud::Ptr & cloud, const pcl::IndicesPtr & indices, float clusterTolerance, int minClusterSize, int maxClusterSize = std::numeric_limits::max(), int * biggestClusterIndex = 0); -std::vector RTABMAP_EXP extractClusters( +std::vector RTABMAP_CORE_EXPORT extractClusters( const pcl::PointCloud::Ptr & cloud, const pcl::IndicesPtr & indices, float clusterTolerance, int minClusterSize, int maxClusterSize = std::numeric_limits::max(), int * biggestClusterIndex = 0); -std::vector RTABMAP_EXP extractClusters( +std::vector RTABMAP_CORE_EXPORT extractClusters( const pcl::PointCloud::Ptr & cloud, const pcl::IndicesPtr & indices, float clusterTolerance, int minClusterSize, int maxClusterSize = std::numeric_limits::max(), int * biggestClusterIndex = 0); -std::vector RTABMAP_EXP extractClusters( +std::vector RTABMAP_CORE_EXPORT extractClusters( const pcl::PointCloud::Ptr & cloud, const pcl::IndicesPtr & indices, float clusterTolerance, int minClusterSize, int maxClusterSize = std::numeric_limits::max(), int * biggestClusterIndex = 0); -std::vector RTABMAP_EXP extractClusters( +std::vector RTABMAP_CORE_EXPORT extractClusters( const pcl::PointCloud::Ptr & cloud, const pcl::IndicesPtr & indices, float clusterTolerance, @@ -843,58 +844,58 @@ std::vector RTABMAP_EXP extractClusters( int maxClusterSize = std::numeric_limits::max(), int * biggestClusterIndex = 0); -pcl::IndicesPtr RTABMAP_EXP extractIndices( +pcl::IndicesPtr RTABMAP_CORE_EXPORT extractIndices( const pcl::PointCloud::Ptr & cloud, const pcl::IndicesPtr & indices, bool negative); -pcl::IndicesPtr RTABMAP_EXP extractIndices( +pcl::IndicesPtr RTABMAP_CORE_EXPORT extractIndices( const pcl::PointCloud::Ptr & cloud, const pcl::IndicesPtr & indices, bool negative); -pcl::IndicesPtr RTABMAP_EXP extractIndices( +pcl::IndicesPtr RTABMAP_CORE_EXPORT extractIndices( const pcl::PointCloud::Ptr & cloud, const pcl::IndicesPtr & indices, bool negative); -pcl::IndicesPtr RTABMAP_EXP extractIndices( +pcl::IndicesPtr RTABMAP_CORE_EXPORT extractIndices( const pcl::PointCloud::Ptr & cloud, const pcl::IndicesPtr & indices, bool negative); -pcl::IndicesPtr RTABMAP_EXP extractIndices( +pcl::IndicesPtr RTABMAP_CORE_EXPORT extractIndices( const pcl::PointCloud::Ptr & cloud, const pcl::IndicesPtr & indices, bool negative); -pcl::IndicesPtr RTABMAP_EXP extractIndices( +pcl::IndicesPtr RTABMAP_CORE_EXPORT extractIndices( const pcl::PointCloud::Ptr & cloud, const pcl::IndicesPtr & indices, bool negative); -pcl::PointCloud::Ptr RTABMAP_EXP extractIndices( +pcl::PointCloud::Ptr RTABMAP_CORE_EXPORT extractIndices( const pcl::PointCloud::Ptr & cloud, const pcl::IndicesPtr & indices, bool negative, bool keepOrganized); -pcl::PointCloud::Ptr RTABMAP_EXP extractIndices( +pcl::PointCloud::Ptr RTABMAP_CORE_EXPORT extractIndices( const pcl::PointCloud::Ptr & cloud, const pcl::IndicesPtr & indices, bool negative, bool keepOrganized); // PCL default lacks of pcl::PointNormal type support -//pcl::PointCloud::Ptr RTABMAP_EXP extractIndices( +//pcl::PointCloud::Ptr RTABMAP_CORE_EXPORT extractIndices( // const pcl::PointCloud::Ptr & cloud, // const pcl::IndicesPtr & indices, // bool negative, // bool keepOrganized); -pcl::PointCloud::Ptr RTABMAP_EXP extractIndices( +pcl::PointCloud::Ptr RTABMAP_CORE_EXPORT extractIndices( const pcl::PointCloud::Ptr & cloud, const pcl::IndicesPtr & indices, bool negative, bool keepOrganized); -pcl::PointCloud::Ptr RTABMAP_EXP extractIndices( +pcl::PointCloud::Ptr RTABMAP_CORE_EXPORT extractIndices( const pcl::PointCloud::Ptr & cloud, const pcl::IndicesPtr & indices, bool negative, bool keepOrganized); -pcl::PointCloud::Ptr RTABMAP_EXP extractIndices( +pcl::PointCloud::Ptr RTABMAP_CORE_EXPORT extractIndices( const pcl::PointCloud::Ptr & cloud, const pcl::IndicesPtr & indices, bool negative, diff --git a/corelib/include/rtabmap/core/util3d_mapping.h b/corelib/include/rtabmap/core/util3d_mapping.h index 6ca8668df0..b1c865d7cd 100644 --- a/corelib/include/rtabmap/core/util3d_mapping.h +++ b/corelib/include/rtabmap/core/util3d_mapping.h @@ -28,7 +28,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #ifndef UTIL3D_MAPPING_H_ #define UTIL3D_MAPPING_H_ -#include "rtabmap/core/RtabmapExp.h" +#include "rtabmap/core/rtabmap_core_export.h" #include #include @@ -43,24 +43,26 @@ namespace rtabmap namespace util3d { -RTABMAP_DEPRECATED(void RTABMAP_EXP occupancy2DFromLaserScan( +// Use interface with \"viewpoint\" parameter to make sure the ray tracing origin is from the sensor and not the base. +RTABMAP_DEPRECATED void RTABMAP_CORE_EXPORT occupancy2DFromLaserScan( const cv::Mat & scan, // in /base_link frame cv::Mat & empty, cv::Mat & occupied, float cellSize, bool unknownSpaceFilled = false, - float scanMaxRange = 0.0f), "Use interface with \"viewpoint\" parameter to make sure the ray tracing origin is from the sensor and not the base."); + float scanMaxRange = 0.0f); -RTABMAP_DEPRECATED(void RTABMAP_EXP occupancy2DFromLaserScan( +// Use interface with scanHit/scanNoHit parameters: scanNoHit set to null matrix has the same functionality than this method. +RTABMAP_DEPRECATED void RTABMAP_CORE_EXPORT occupancy2DFromLaserScan( const cv::Mat & scan, // in /base_link frame const cv::Point3f & viewpoint, // /base_link -> /base_scan cv::Mat & empty, cv::Mat & occupied, float cellSize, bool unknownSpaceFilled = false, - float scanMaxRange = 0.0f), "Use interface with scanHit/scanNoHit parameters: scanNoHit set to null matrix has the same functionality than this method."); + float scanMaxRange = 0.0f); -void RTABMAP_EXP occupancy2DFromLaserScan( +void RTABMAP_CORE_EXPORT occupancy2DFromLaserScan( const cv::Mat & scanHit, // in /base_link frame const cv::Mat & scanNoHit, // in /base_link frame const cv::Point3f & viewpoint, // /base_link -> /base_scan @@ -70,7 +72,7 @@ void RTABMAP_EXP occupancy2DFromLaserScan( bool unknownSpaceFilled = false, float scanMaxRange = 0.0f); // would be set if unknownSpaceFilled=true -cv::Mat RTABMAP_EXP create2DMapFromOccupancyLocalMaps( +cv::Mat RTABMAP_CORE_EXPORT create2DMapFromOccupancyLocalMaps( const std::map & poses, const std::map > & occupancy, float cellSize, @@ -80,16 +82,18 @@ cv::Mat RTABMAP_EXP create2DMapFromOccupancyLocalMaps( bool erode = false, float footprintRadius = 0.0f); -RTABMAP_DEPRECATED(cv::Mat RTABMAP_EXP create2DMap(const std::map & poses, +// Use interface with \"viewpoints\" parameter to make sure the ray tracing origin is from the sensor and not the base. +RTABMAP_DEPRECATED cv::Mat RTABMAP_CORE_EXPORT create2DMap(const std::map & poses, const std::map::Ptr > & scans, // in /base_link frame float cellSize, bool unknownSpaceFilled, float & xMin, float & yMin, float minMapSize = 0.0f, - float scanMaxRange = 0.0f), "Use interface with \"viewpoints\" parameter to make sure the ray tracing origin is from the sensor and not the base."); + float scanMaxRange = 0.0f); -RTABMAP_DEPRECATED(cv::Mat RTABMAP_EXP create2DMap(const std::map & poses, +// Use interface with cv::Mat scans. +RTABMAP_DEPRECATED cv::Mat RTABMAP_CORE_EXPORT create2DMap(const std::map & poses, const std::map::Ptr > & scans, // in /base_link frame const std::map & viewpoints, // /base_link -> /base_scan float cellSize, @@ -97,7 +101,7 @@ RTABMAP_DEPRECATED(cv::Mat RTABMAP_EXP create2DMap(const std::map & poses, +cv::Mat RTABMAP_CORE_EXPORT create2DMap(const std::map & poses, const std::map > & scans, // >, in /base_link frame const std::map & viewpoints, // /base_link -> /base_scan float cellSize, @@ -124,15 +128,15 @@ cv::Mat RTABMAP_EXP create2DMap(const std::map & poses, float minMapSize = 0.0f, float scanMaxRange = 0.0f); // would be set if unknownSpaceFilled=true -void RTABMAP_EXP rayTrace(const cv::Point2i & start, +void RTABMAP_CORE_EXPORT rayTrace(const cv::Point2i & start, const cv::Point2i & end, cv::Mat & grid, bool stopOnObstacle); -cv::Mat RTABMAP_EXP convertMap2Image8U(const cv::Mat & map8S, bool pgmFormat = false); -cv::Mat RTABMAP_EXP convertImage8U2Map(const cv::Mat & map8U, bool pgmFormat = false); +cv::Mat RTABMAP_CORE_EXPORT convertMap2Image8U(const cv::Mat & map8S, bool pgmFormat = false); +cv::Mat RTABMAP_CORE_EXPORT convertImage8U2Map(const cv::Mat & map8U, bool pgmFormat = false); -cv::Mat RTABMAP_EXP erodeMap(const cv::Mat & map); +cv::Mat RTABMAP_CORE_EXPORT erodeMap(const cv::Mat & map); template typename pcl::PointCloud::Ptr projectCloudOnXYPlane( diff --git a/corelib/include/rtabmap/core/util3d_motion_estimation.h b/corelib/include/rtabmap/core/util3d_motion_estimation.h index 02cd15c988..d6409c5015 100644 --- a/corelib/include/rtabmap/core/util3d_motion_estimation.h +++ b/corelib/include/rtabmap/core/util3d_motion_estimation.h @@ -28,7 +28,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #ifndef UTIL3D_MOTION_ESTIMATION_H_ #define UTIL3D_MOTION_ESTIMATION_H_ -#include +#include #include #include @@ -39,7 +39,7 @@ namespace rtabmap namespace util3d { -Transform RTABMAP_EXP estimateMotion3DTo2D( +Transform RTABMAP_CORE_EXPORT estimateMotion3DTo2D( const std::map & words3A, const std::map & words2B, const CameraModel & cameraModel, @@ -48,6 +48,7 @@ Transform RTABMAP_EXP estimateMotion3DTo2D( double reprojError = 5., int flagsPnP = 0, int pnpRefineIterations = 1, + int varianceMedianRatio = 4, float maxVariance = 0, const Transform & guess = Transform::getIdentity(), const std::map & words3B = std::map(), @@ -55,15 +56,17 @@ Transform RTABMAP_EXP estimateMotion3DTo2D( std::vector * matchesOut = 0, std::vector * inliersOut = 0); -Transform RTABMAP_EXP estimateMotion3DTo2D( +Transform RTABMAP_CORE_EXPORT estimateMotion3DTo2D( const std::map & words3A, const std::map & words2B, const std::vector & cameraModels, + unsigned int samplingPolicy = 0, // 0=AUTO, 1=ANY, 2=HOMOGENEOUS int minInliers = 10, int iterations = 100, double reprojError = 5., int flagsPnP = 0, int pnpRefineIterations = 1, + int varianceMedianRatio = 4, float maxVariance = 0, const Transform & guess = Transform::getIdentity(), const std::map & words3B = std::map(), @@ -71,7 +74,7 @@ Transform RTABMAP_EXP estimateMotion3DTo2D( std::vector * matchesOut = 0, std::vector * inliersOut = 0); -Transform RTABMAP_EXP estimateMotion3DTo3D( +Transform RTABMAP_CORE_EXPORT estimateMotion3DTo3D( const std::map & words3A, const std::map & words3B, int minInliers = 10, @@ -82,7 +85,7 @@ Transform RTABMAP_EXP estimateMotion3DTo3D( std::vector * matchesOut = 0, std::vector * inliersOut = 0); -void RTABMAP_EXP solvePnPRansac( +void RTABMAP_CORE_EXPORT solvePnPRansac( const std::vector & objectPoints, const std::vector & imagePoints, const cv::Mat & cameraMatrix, diff --git a/corelib/include/rtabmap/core/util3d_registration.h b/corelib/include/rtabmap/core/util3d_registration.h index 8c1d37d706..639c8e890e 100644 --- a/corelib/include/rtabmap/core/util3d_registration.h +++ b/corelib/include/rtabmap/core/util3d_registration.h @@ -28,7 +28,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #ifndef UTIL3D_REGISTRATION_H_ #define UTIL3D_REGISTRATION_H_ -#include +#include #include #include @@ -41,15 +41,15 @@ namespace rtabmap namespace util3d { -int RTABMAP_EXP getCorrespondencesCount(const pcl::PointCloud::ConstPtr & cloud_source, +int RTABMAP_CORE_EXPORT getCorrespondencesCount(const pcl::PointCloud::ConstPtr & cloud_source, const pcl::PointCloud::ConstPtr & cloud_target, float maxDistance); -Transform RTABMAP_EXP transformFromXYZCorrespondencesSVD( +Transform RTABMAP_CORE_EXPORT transformFromXYZCorrespondencesSVD( const pcl::PointCloud & cloud1, const pcl::PointCloud & cloud2); -Transform RTABMAP_EXP transformFromXYZCorrespondences( +Transform RTABMAP_CORE_EXPORT transformFromXYZCorrespondences( const pcl::PointCloud::ConstPtr & cloud1, const pcl::PointCloud::ConstPtr & cloud2, double inlierThreshold = 0.02, @@ -59,34 +59,38 @@ Transform RTABMAP_EXP transformFromXYZCorrespondences( std::vector * inliers = 0, cv::Mat * variance = 0); -void RTABMAP_EXP computeVarianceAndCorrespondences( +void RTABMAP_CORE_EXPORT computeVarianceAndCorrespondences( const pcl::PointCloud::ConstPtr & cloudA, const pcl::PointCloud::ConstPtr & cloudB, double maxCorrespondenceDistance, double maxCorrespondenceAngle, // <=0 means that we don't care about normal angle difference double & variance, - int & correspondencesOut); -void RTABMAP_EXP computeVarianceAndCorrespondences( + int & correspondencesOut, + bool reciprocal); +void RTABMAP_CORE_EXPORT computeVarianceAndCorrespondences( const pcl::PointCloud::ConstPtr & cloudA, const pcl::PointCloud::ConstPtr & cloudB, double maxCorrespondenceDistance, double maxCorrespondenceAngle, // <=0 means that we don't care about normal angle difference double & variance, - int & correspondencesOut); -void RTABMAP_EXP computeVarianceAndCorrespondences( + int & correspondencesOut, + bool reciprocal); +void RTABMAP_CORE_EXPORT computeVarianceAndCorrespondences( const pcl::PointCloud::ConstPtr & cloudA, const pcl::PointCloud::ConstPtr & cloudB, double maxCorrespondenceDistance, double & variance, - int & correspondencesOut); -void RTABMAP_EXP computeVarianceAndCorrespondences( + int & correspondencesOut, + bool reciprocal); +void RTABMAP_CORE_EXPORT computeVarianceAndCorrespondences( const pcl::PointCloud::ConstPtr & cloudA, const pcl::PointCloud::ConstPtr & cloudB, double maxCorrespondenceDistance, double & variance, - int & correspondencesOut); + int & correspondencesOut, + bool reciprocal); -Transform RTABMAP_EXP icp( +Transform RTABMAP_CORE_EXPORT icp( const pcl::PointCloud::ConstPtr & cloud_source, const pcl::PointCloud::ConstPtr & cloud_target, double maxCorrespondenceDistance, @@ -95,7 +99,7 @@ Transform RTABMAP_EXP icp( pcl::PointCloud & cloud_source_registered, float epsilon = 0.0f, bool icp2D = false); -Transform RTABMAP_EXP icp( +Transform RTABMAP_CORE_EXPORT icp( const pcl::PointCloud::ConstPtr & cloud_source, const pcl::PointCloud::ConstPtr & cloud_target, double maxCorrespondenceDistance, @@ -105,7 +109,7 @@ Transform RTABMAP_EXP icp( float epsilon = 0.0f, bool icp2D = false); -Transform RTABMAP_EXP icpPointToPlane( +Transform RTABMAP_CORE_EXPORT icpPointToPlane( const pcl::PointCloud::ConstPtr & cloud_source, const pcl::PointCloud::ConstPtr & cloud_target, double maxCorrespondenceDistance, @@ -114,7 +118,7 @@ Transform RTABMAP_EXP icpPointToPlane( pcl::PointCloud & cloud_source_registered, float epsilon = 0.0f, bool icp2D = false); -Transform RTABMAP_EXP icpPointToPlane( +Transform RTABMAP_CORE_EXPORT icpPointToPlane( const pcl::PointCloud::ConstPtr & cloud_source, const pcl::PointCloud::ConstPtr & cloud_target, double maxCorrespondenceDistance, diff --git a/corelib/include/rtabmap/core/util3d_surface.h b/corelib/include/rtabmap/core/util3d_surface.h index 4068129751..080cd8b7e1 100644 --- a/corelib/include/rtabmap/core/util3d_surface.h +++ b/corelib/include/rtabmap/core/util3d_surface.h @@ -28,7 +28,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #ifndef UTIL3D_SURFACE_H_ #define UTIL3D_SURFACE_H_ -#include +#include #include #include @@ -61,74 +61,74 @@ namespace util3d * @param neighborPolygons returned index from polygons to neighbor polygons (index size = polygons size). * @param vertexPolygons returned index from vertices to polygons (index size = cloudSize). */ -void RTABMAP_EXP createPolygonIndexes( +void RTABMAP_CORE_EXPORT createPolygonIndexes( const std::vector & polygons, int cloudSize, std::vector > & neighborPolygons, std::vector > & vertexPolygons); -std::list > RTABMAP_EXP clusterPolygons( +std::list > RTABMAP_CORE_EXPORT clusterPolygons( const std::vector > & neighborPolygons, int minClusterSize = 0); -std::vector RTABMAP_EXP organizedFastMesh( +std::vector RTABMAP_CORE_EXPORT organizedFastMesh( const pcl::PointCloud::Ptr & cloud, double angleTolerance, bool quad, int trianglePixelSize, const Eigen::Vector3f & viewpoint = Eigen::Vector3f(0,0,0)); -std::vector RTABMAP_EXP organizedFastMesh( +std::vector RTABMAP_CORE_EXPORT organizedFastMesh( const pcl::PointCloud::Ptr & cloud, double angleTolerance = M_PI/16, bool quad=true, int trianglePixelSize = 2, const Eigen::Vector3f & viewpoint = Eigen::Vector3f(0,0,0)); -std::vector RTABMAP_EXP organizedFastMesh( +std::vector RTABMAP_CORE_EXPORT organizedFastMesh( const pcl::PointCloud::Ptr & cloud, double angleTolerance = M_PI/16, bool quad=true, int trianglePixelSize = 2, const Eigen::Vector3f & viewpoint = Eigen::Vector3f(0,0,0)); -void RTABMAP_EXP appendMesh( +void RTABMAP_CORE_EXPORT appendMesh( pcl::PointCloud & cloudA, std::vector & polygonsA, const pcl::PointCloud & cloudB, const std::vector & polygonsB); -void RTABMAP_EXP appendMesh( +void RTABMAP_CORE_EXPORT appendMesh( pcl::PointCloud & cloudA, std::vector & polygonsA, const pcl::PointCloud & cloudB, const std::vector & polygonsB); // return map from new to old polygon indices -std::vector RTABMAP_EXP filterNotUsedVerticesFromMesh( +std::vector RTABMAP_CORE_EXPORT filterNotUsedVerticesFromMesh( const pcl::PointCloud & cloud, const std::vector & polygons, pcl::PointCloud & outputCloud, std::vector & outputPolygons); -std::vector RTABMAP_EXP filterNotUsedVerticesFromMesh( +std::vector RTABMAP_CORE_EXPORT filterNotUsedVerticesFromMesh( const pcl::PointCloud & cloud, const std::vector & polygons, pcl::PointCloud & outputCloud, std::vector & outputPolygons); -std::vector RTABMAP_EXP filterNaNPointsFromMesh( +std::vector RTABMAP_CORE_EXPORT filterNaNPointsFromMesh( const pcl::PointCloud & cloud, const std::vector & polygons, pcl::PointCloud & outputCloud, std::vector & outputPolygons); -std::vector RTABMAP_EXP filterCloseVerticesFromMesh( +std::vector RTABMAP_CORE_EXPORT filterCloseVerticesFromMesh( const pcl::PointCloud::Ptr cloud, const std::vector & polygons, float radius, float angle, bool keepLatestInRadius); -std::vector RTABMAP_EXP filterInvalidPolygons( +std::vector RTABMAP_CORE_EXPORT filterInvalidPolygons( const std::vector & polygons); -pcl::PolygonMesh::Ptr RTABMAP_EXP createMesh( +pcl::PolygonMesh::Ptr RTABMAP_CORE_EXPORT createMesh( const pcl::PointCloud::Ptr & cloudWithNormals, float gp3SearchRadius = 0.025, float gp3Mu = 2.5, @@ -138,7 +138,7 @@ pcl::PolygonMesh::Ptr RTABMAP_EXP createMesh( float gp3MaximumAngle = 2*M_PI/3, bool gp3NormalConsistency = true); -pcl::TextureMesh::Ptr RTABMAP_EXP createTextureMesh( +pcl::TextureMesh::Ptr RTABMAP_CORE_EXPORT createTextureMesh( const pcl::PolygonMesh::Ptr & mesh, const std::map & poses, const std::map & cameraModels, @@ -151,7 +151,7 @@ pcl::TextureMesh::Ptr RTABMAP_EXP createTextureMesh( const ProgressState * state = 0, std::vector > * vertexToPixels = 0, // For each point, we have a list of cameras with corresponding pixel in it. Beware that the camera ids don't correspond to pose ids, they are indexes from 0 to total camera models and texture's materials. bool distanceToCamPolicy = false); -pcl::TextureMesh::Ptr RTABMAP_EXP createTextureMesh( +pcl::TextureMesh::Ptr RTABMAP_CORE_EXPORT createTextureMesh( const pcl::PolygonMesh::Ptr & mesh, const std::map & poses, const std::map > & cameraModels, @@ -168,26 +168,26 @@ pcl::TextureMesh::Ptr RTABMAP_EXP createTextureMesh( /** * Remove not textured polygon clusters. If minClusterSize<0, only the largest cluster is kept. */ -void RTABMAP_EXP cleanTextureMesh( +void RTABMAP_CORE_EXPORT cleanTextureMesh( pcl::TextureMesh & textureMesh, int minClusterSize); -pcl::TextureMesh::Ptr RTABMAP_EXP concatenateTextureMeshes( +pcl::TextureMesh::Ptr RTABMAP_CORE_EXPORT concatenateTextureMeshes( const std::list & meshes); -void RTABMAP_EXP concatenateTextureMaterials( +void RTABMAP_CORE_EXPORT concatenateTextureMaterials( pcl::TextureMesh & mesh, const cv::Size & imageSize, int textureSize, int maxTextures, float & scale, std::vector * materialsKept=0); -std::vector > RTABMAP_EXP convertPolygonsFromPCL( +std::vector > RTABMAP_CORE_EXPORT convertPolygonsFromPCL( const std::vector & polygons); -std::vector > > RTABMAP_EXP convertPolygonsFromPCL( +std::vector > > RTABMAP_CORE_EXPORT convertPolygonsFromPCL( const std::vector > & polygons); -std::vector RTABMAP_EXP convertPolygonsToPCL( +std::vector RTABMAP_CORE_EXPORT convertPolygonsToPCL( const std::vector > & polygons); -std::vector > RTABMAP_EXP convertPolygonsToPCL( +std::vector > RTABMAP_CORE_EXPORT convertPolygonsToPCL( const std::vector > > & tex_polygons); -pcl::TextureMesh::Ptr RTABMAP_EXP assembleTextureMesh( +pcl::TextureMesh::Ptr RTABMAP_CORE_EXPORT assembleTextureMesh( const cv::Mat & cloudMat, const std::vector > > & polygons, #if PCL_VERSION_COMPARE(>=, 1, 8, 0) @@ -198,7 +198,7 @@ pcl::TextureMesh::Ptr RTABMAP_EXP assembleTextureMesh( cv::Mat & textures, bool mergeTextures = false); -pcl::PolygonMesh::Ptr RTABMAP_EXP assemblePolygonMesh( +pcl::PolygonMesh::Ptr RTABMAP_CORE_EXPORT assemblePolygonMesh( const cv::Mat & cloudMat, const std::vector > & polygons); @@ -206,7 +206,7 @@ pcl::PolygonMesh::Ptr RTABMAP_EXP assemblePolygonMesh( * Merge all textures in the mesh into "textureCount" textures of size "textureSize". * @return merged textures corresponding to new materials set in TextureMesh (height=textureSize, width=textureSize*materials) */ -cv::Mat RTABMAP_EXP mergeTextures( +cv::Mat RTABMAP_CORE_EXPORT mergeTextures( pcl::TextureMesh & mesh, const std::map & images, // raw or compressed, can be empty if memory or dbDriver should be used const std::map & calibrations, // Should match images @@ -228,7 +228,7 @@ cv::Mat RTABMAP_EXP mergeTextures( std::map > * gains = 0, // std::map > * blendingGains = 0, // std::pair * contrastValues = 0); // Alpha/beta contrast values -cv::Mat RTABMAP_EXP mergeTextures( +cv::Mat RTABMAP_CORE_EXPORT mergeTextures( pcl::TextureMesh & mesh, const std::map & images, // raw or compressed, can be empty if memory or dbDriver should be used const std::map > & calibrations, // Should match images @@ -251,9 +251,10 @@ cv::Mat RTABMAP_EXP mergeTextures( std::map > * blendingGains = 0, // std::pair * contrastValues = 0); // Alpha/beta contrast values -void RTABMAP_EXP fixTextureMeshForVisualization(pcl::TextureMesh & textureMesh); +void RTABMAP_CORE_EXPORT fixTextureMeshForVisualization(pcl::TextureMesh & textureMesh); -RTABMAP_DEPRECATED(bool RTABMAP_EXP multiBandTexturing( +// Use the same method with 22 parameters instead. +RTABMAP_DEPRECATED bool RTABMAP_CORE_EXPORT multiBandTexturing( const std::string & outputOBJPath, const pcl::PCLPointCloud2 & cloud, const std::vector & polygons, @@ -268,7 +269,7 @@ RTABMAP_DEPRECATED(bool RTABMAP_EXP multiBandTexturing( const std::map > & gains = std::map >(), // optional output of util3d::mergeTextures() const std::map > & blendingGains = std::map >(), // optional output of util3d::mergeTextures() const std::pair & contrastValues = std::pair(0,0), // optional output of util3d::mergeTextures() - bool gainRGB = true), "Use the same method with 22 parameters instead."); + bool gainRGB = true); /** * Texture mesh with AliceVision's multiband texturing approach. See also https://meshroom-manual.readthedocs.io/en/bibtex1/node-reference/nodes/Texturing.html. @@ -296,7 +297,7 @@ RTABMAP_DEPRECATED(bool RTABMAP_EXP multiBandTexturing( * @param angleHardThreshold 0.0 to disable angle hard threshold filtering (0.0, 180.0). * @param forceVisibleByAllVertices Triangle visibility is based on the union of vertices visibility. */ -bool RTABMAP_EXP multiBandTexturing( +bool RTABMAP_CORE_EXPORT multiBandTexturing( const std::string & outputOBJPath, const pcl::PCLPointCloud2 & cloud, const std::vector & polygons, @@ -321,108 +322,108 @@ bool RTABMAP_EXP multiBandTexturing( double angleHardThreshold = 90.0, bool forceVisibleByAllVertices = false); -cv::Mat RTABMAP_EXP computeNormals( +cv::Mat RTABMAP_CORE_EXPORT computeNormals( const cv::Mat & laserScan, int searchK, float searchRadius); -pcl::PointCloud::Ptr RTABMAP_EXP computeNormals( +pcl::PointCloud::Ptr RTABMAP_CORE_EXPORT computeNormals( const pcl::PointCloud::Ptr & cloud, int searchK = 20, float searchRadius = 0.0f, const Eigen::Vector3f & viewPoint = Eigen::Vector3f(0,0,0)); -pcl::PointCloud::Ptr RTABMAP_EXP computeNormals( +pcl::PointCloud::Ptr RTABMAP_CORE_EXPORT computeNormals( const pcl::PointCloud::Ptr & cloud, int searchK = 20, float searchRadius = 0.0f, const Eigen::Vector3f & viewPoint = Eigen::Vector3f(0,0,0)); -pcl::PointCloud::Ptr RTABMAP_EXP computeNormals( +pcl::PointCloud::Ptr RTABMAP_CORE_EXPORT computeNormals( const pcl::PointCloud::Ptr & cloud, int searchK = 20, float searchRadius = 0.0f, const Eigen::Vector3f & viewPoint = Eigen::Vector3f(0,0,0)); -pcl::PointCloud::Ptr RTABMAP_EXP computeNormals( +pcl::PointCloud::Ptr RTABMAP_CORE_EXPORT computeNormals( const pcl::PointCloud::Ptr & cloud, const pcl::IndicesPtr & indices, int searchK = 20, float searchRadius = 0.0f, const Eigen::Vector3f & viewPoint = Eigen::Vector3f(0,0,0)); -pcl::PointCloud::Ptr RTABMAP_EXP computeNormals( +pcl::PointCloud::Ptr RTABMAP_CORE_EXPORT computeNormals( const pcl::PointCloud::Ptr & cloud, const pcl::IndicesPtr & indices, int searchK = 20, float searchRadius = 0.0f, const Eigen::Vector3f & viewPoint = Eigen::Vector3f(0,0,0)); -pcl::PointCloud::Ptr RTABMAP_EXP computeNormals( +pcl::PointCloud::Ptr RTABMAP_CORE_EXPORT computeNormals( const pcl::PointCloud::Ptr & cloud, const pcl::IndicesPtr & indices, int searchK = 20, float searchRadius = 0.0f, const Eigen::Vector3f & viewPoint = Eigen::Vector3f(0,0,0)); -pcl::PointCloud::Ptr RTABMAP_EXP computeNormals2D( +pcl::PointCloud::Ptr RTABMAP_CORE_EXPORT computeNormals2D( const pcl::PointCloud::Ptr & cloud, int searchK = 5, float searchRadius = 0.0f, const Eigen::Vector3f & viewPoint = Eigen::Vector3f(0,0,0)); -pcl::PointCloud::Ptr RTABMAP_EXP computeNormals2D( +pcl::PointCloud::Ptr RTABMAP_CORE_EXPORT computeNormals2D( const pcl::PointCloud::Ptr & cloud, int searchK = 5, float searchRadius = 0.0f, const Eigen::Vector3f & viewPoint = Eigen::Vector3f(0,0,0)); -pcl::PointCloud::Ptr RTABMAP_EXP computeFastOrganizedNormals2D( +pcl::PointCloud::Ptr RTABMAP_CORE_EXPORT computeFastOrganizedNormals2D( const pcl::PointCloud::Ptr & cloud, int searchK = 5, float searchRadius = 0.0f, const Eigen::Vector3f & viewPoint = Eigen::Vector3f(0,0,0)); -pcl::PointCloud::Ptr RTABMAP_EXP computeFastOrganizedNormals2D( +pcl::PointCloud::Ptr RTABMAP_CORE_EXPORT computeFastOrganizedNormals2D( const pcl::PointCloud::Ptr & cloud, int searchK = 5, float searchRadius = 0.0f, const Eigen::Vector3f & viewPoint = Eigen::Vector3f(0,0,0)); -pcl::PointCloud::Ptr RTABMAP_EXP computeFastOrganizedNormals( +pcl::PointCloud::Ptr RTABMAP_CORE_EXPORT computeFastOrganizedNormals( const pcl::PointCloud::Ptr & cloud, float maxDepthChangeFactor = 0.02f, float normalSmoothingSize = 10.0f, const Eigen::Vector3f & viewPoint = Eigen::Vector3f(0,0,0)); -pcl::PointCloud::Ptr RTABMAP_EXP computeFastOrganizedNormals( +pcl::PointCloud::Ptr RTABMAP_CORE_EXPORT computeFastOrganizedNormals( const pcl::PointCloud::Ptr & cloud, const pcl::IndicesPtr & indices, float maxDepthChangeFactor = 0.02f, float normalSmoothingSize = 10.0f, const Eigen::Vector3f & viewPoint = Eigen::Vector3f(0,0,0)); -float RTABMAP_EXP computeNormalsComplexity( +float RTABMAP_CORE_EXPORT computeNormalsComplexity( const LaserScan & scan, const Transform & t = Transform::getIdentity(), cv::Mat * pcaEigenVectors = 0, cv::Mat * pcaEigenValues = 0); -float RTABMAP_EXP computeNormalsComplexity( +float RTABMAP_CORE_EXPORT computeNormalsComplexity( const pcl::PointCloud & normals, const Transform & t = Transform::getIdentity(), bool is2d = false, cv::Mat * pcaEigenVectors = 0, cv::Mat * pcaEigenValues = 0); -float RTABMAP_EXP computeNormalsComplexity( +float RTABMAP_CORE_EXPORT computeNormalsComplexity( const pcl::PointCloud & cloud, const Transform & t = Transform::getIdentity(), bool is2d = false, cv::Mat * pcaEigenVectors = 0, cv::Mat * pcaEigenValues = 0); -float RTABMAP_EXP computeNormalsComplexity( +float RTABMAP_CORE_EXPORT computeNormalsComplexity( const pcl::PointCloud & cloud, const Transform & t = Transform::getIdentity(), bool is2d = false, cv::Mat * pcaEigenVectors = 0, cv::Mat * pcaEigenValues = 0); -float RTABMAP_EXP computeNormalsComplexity( +float RTABMAP_CORE_EXPORT computeNormalsComplexity( const pcl::PointCloud & cloud, const Transform & t = Transform::getIdentity(), bool is2d = false, cv::Mat * pcaEigenVectors = 0, cv::Mat * pcaEigenValues = 0); -pcl::PointCloud::Ptr RTABMAP_EXP mls( +pcl::PointCloud::Ptr RTABMAP_CORE_EXPORT mls( const pcl::PointCloud::Ptr & cloud, float searchRadius = 0.0f, int polygonialOrder = 2, @@ -432,7 +433,7 @@ pcl::PointCloud::Ptr RTABMAP_EXP mls( int pointDensity = 0, // RANDOM_UNIFORM_DENSITY float dilationVoxelSize = 1.0f, // VOXEL_GRID_DILATION int dilationIterations = 0); // VOXEL_GRID_DILATION -pcl::PointCloud::Ptr RTABMAP_EXP mls( +pcl::PointCloud::Ptr RTABMAP_CORE_EXPORT mls( const pcl::PointCloud::Ptr & cloud, const pcl::IndicesPtr & indices, float searchRadius = 0.0f, @@ -444,66 +445,70 @@ pcl::PointCloud::Ptr RTABMAP_EXP mls( float dilationVoxelSize = 1.0f, // VOXEL_GRID_DILATION int dilationIterations = 0); // VOXEL_GRID_DILATION -RTABMAP_DEPRECATED(LaserScan RTABMAP_EXP adjustNormalsToViewPoint( +// Use version with groundNormalsUp as float. For forceGroundNormalsUp=true, set groundNormalsUp to 0.8f, otherwise set groundNormalsUp to 0.0f. +RTABMAP_DEPRECATED LaserScan RTABMAP_CORE_EXPORT adjustNormalsToViewPoint( const LaserScan & scan, const Eigen::Vector3f & viewpoint, - bool forceGroundNormalsUp), "Use version with groundNormalsUp as float. For forceGroundNormalsUp=true, set groundNormalsUp to 0.8f, otherwise set groundNormalsUp to 0.0f."); -LaserScan RTABMAP_EXP adjustNormalsToViewPoint( + bool forceGroundNormalsUp); +LaserScan RTABMAP_CORE_EXPORT adjustNormalsToViewPoint( const LaserScan & scan, const Eigen::Vector3f & viewpoint = Eigen::Vector3f(0,0,0), float groundNormalsUp = 0.0f); -RTABMAP_DEPRECATED(void RTABMAP_EXP adjustNormalsToViewPoint( +// Use version with groundNormalsUp as float. For forceGroundNormalsUp=true, set groundNormalsUp to 0.8f, otherwise set groundNormalsUp to 0.0f. +RTABMAP_DEPRECATED void RTABMAP_CORE_EXPORT adjustNormalsToViewPoint( pcl::PointCloud::Ptr & cloud, const Eigen::Vector3f & viewpoint, - bool forceGroundNormalsUp), "Use version with groundNormalsUp as float. For forceGroundNormalsUp=true, set groundNormalsUp to 0.8f, otherwise set groundNormalsUp to 0.0f."); -void RTABMAP_EXP adjustNormalsToViewPoint( + bool forceGroundNormalsUp); +void RTABMAP_CORE_EXPORT adjustNormalsToViewPoint( pcl::PointCloud::Ptr & cloud, const Eigen::Vector3f & viewpoint = Eigen::Vector3f(0,0,0), float groundNormalsUp = 0.0f); -RTABMAP_DEPRECATED(void RTABMAP_EXP adjustNormalsToViewPoint( +// Use version with groundNormalsUp as float. For forceGroundNormalsUp=true, set groundNormalsUp to 0.8f, otherwise set groundNormalsUp to 0.0f. +RTABMAP_DEPRECATED void RTABMAP_CORE_EXPORT adjustNormalsToViewPoint( pcl::PointCloud::Ptr & cloud, const Eigen::Vector3f & viewpoint, - bool forceGroundNormalsUp), "Use version with groundNormalsUp as float. For forceGroundNormalsUp=true, set groundNormalsUp to 0.8f, otherwise set groundNormalsUp to 0.0f."); -void RTABMAP_EXP adjustNormalsToViewPoint( + bool forceGroundNormalsUp); +void RTABMAP_CORE_EXPORT adjustNormalsToViewPoint( pcl::PointCloud::Ptr & cloud, const Eigen::Vector3f & viewpoint = Eigen::Vector3f(0,0,0), float groundNormalsUp = 0.0f); -RTABMAP_DEPRECATED(void RTABMAP_EXP adjustNormalsToViewPoint( +// Use version with groundNormalsUp as float. For forceGroundNormalsUp=true, set groundNormalsUp to 0.8f, otherwise set groundNormalsUp to 0.0f. +RTABMAP_DEPRECATED void RTABMAP_CORE_EXPORT adjustNormalsToViewPoint( pcl::PointCloud::Ptr & cloud, const Eigen::Vector3f & viewpoint, - bool forceGroundNormalsUp), "Use version with groundNormalsUp as float. For forceGroundNormalsUp=true, set groundNormalsUp to 0.8f, otherwise set groundNormalsUp to 0.0f."); -void RTABMAP_EXP adjustNormalsToViewPoint( + bool forceGroundNormalsUp); +void RTABMAP_CORE_EXPORT adjustNormalsToViewPoint( pcl::PointCloud::Ptr & cloud, const Eigen::Vector3f & viewpoint = Eigen::Vector3f(0,0,0), float groundNormalsUp = 0.0f); -void RTABMAP_EXP adjustNormalsToViewPoints( +void RTABMAP_CORE_EXPORT adjustNormalsToViewPoints( const std::map & poses, const pcl::PointCloud::Ptr & rawCloud, const std::vector & rawCameraIndices, pcl::PointCloud::Ptr & cloud, float groundNormalsUp = 0.0f); -void RTABMAP_EXP adjustNormalsToViewPoints( +void RTABMAP_CORE_EXPORT adjustNormalsToViewPoints( const std::map & poses, const pcl::PointCloud::Ptr & rawCloud, const std::vector & rawCameraIndices, pcl::PointCloud::Ptr & cloud, float groundNormalsUp = 0.0f); -void RTABMAP_EXP adjustNormalsToViewPoints( +void RTABMAP_CORE_EXPORT adjustNormalsToViewPoints( const std::map & poses, const pcl::PointCloud::Ptr & rawCloud, const std::vector & rawCameraIndices, pcl::PointCloud::Ptr & cloud, float groundNormalsUp = 0.0f); -void RTABMAP_EXP adjustNormalsToViewPoints( +void RTABMAP_CORE_EXPORT adjustNormalsToViewPoints( const std::map & viewpoints, const LaserScan & rawScan, const std::vector & viewpointIds, LaserScan & scan, float groundNormalsUp = 0.0f); -pcl::PolygonMesh::Ptr RTABMAP_EXP meshDecimation(const pcl::PolygonMesh::Ptr & mesh, float factor); +pcl::PolygonMesh::Ptr RTABMAP_CORE_EXPORT meshDecimation(const pcl::PolygonMesh::Ptr & mesh, float factor); template std::vector normalizePolygonsSide( @@ -545,7 +550,7 @@ void denseMeshPostProcessing( * * Mathieu: Adapted for PCL format */ -bool RTABMAP_EXP intersectRayTriangle( +bool RTABMAP_CORE_EXPORT intersectRayTriangle( const Eigen::Vector3f & p, const Eigen::Vector3f & dir, const Eigen::Vector3f & v0, diff --git a/corelib/include/rtabmap/core/util3d_transforms.h b/corelib/include/rtabmap/core/util3d_transforms.h index 20f02b8570..cf66753f03 100644 --- a/corelib/include/rtabmap/core/util3d_transforms.h +++ b/corelib/include/rtabmap/core/util3d_transforms.h @@ -28,7 +28,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #ifndef UTIL3D_TRANSFORMS_H_ #define UTIL3D_TRANSFORMS_H_ -#include +#include #include #include @@ -42,76 +42,76 @@ namespace rtabmap namespace util3d { -LaserScan RTABMAP_EXP transformLaserScan( +LaserScan RTABMAP_CORE_EXPORT transformLaserScan( const LaserScan & laserScan, const Transform & transform); -pcl::PointCloud::Ptr RTABMAP_EXP transformPointCloud( +pcl::PointCloud::Ptr RTABMAP_CORE_EXPORT transformPointCloud( const pcl::PointCloud::Ptr & cloud, const Transform & transform); -pcl::PointCloud::Ptr RTABMAP_EXP transformPointCloud( +pcl::PointCloud::Ptr RTABMAP_CORE_EXPORT transformPointCloud( const pcl::PointCloud::Ptr & cloud, const Transform & transform); -pcl::PointCloud::Ptr RTABMAP_EXP transformPointCloud( +pcl::PointCloud::Ptr RTABMAP_CORE_EXPORT transformPointCloud( const pcl::PointCloud::Ptr & cloud, const Transform & transform); -pcl::PointCloud::Ptr RTABMAP_EXP transformPointCloud( +pcl::PointCloud::Ptr RTABMAP_CORE_EXPORT transformPointCloud( const pcl::PointCloud::Ptr & cloud, const Transform & transform); -pcl::PointCloud::Ptr RTABMAP_EXP transformPointCloud( +pcl::PointCloud::Ptr RTABMAP_CORE_EXPORT transformPointCloud( const pcl::PointCloud::Ptr & cloud, const Transform & transform); -pcl::PointCloud::Ptr RTABMAP_EXP transformPointCloud( +pcl::PointCloud::Ptr RTABMAP_CORE_EXPORT transformPointCloud( const pcl::PointCloud::Ptr & cloud, const Transform & transform); -pcl::PointCloud::Ptr RTABMAP_EXP transformPointCloud( +pcl::PointCloud::Ptr RTABMAP_CORE_EXPORT transformPointCloud( const pcl::PointCloud::Ptr & cloud, const pcl::IndicesPtr & indices, const Transform & transform); -pcl::PointCloud::Ptr RTABMAP_EXP transformPointCloud( +pcl::PointCloud::Ptr RTABMAP_CORE_EXPORT transformPointCloud( const pcl::PointCloud::Ptr & cloud, const pcl::IndicesPtr & indices, const Transform & transform); -pcl::PointCloud::Ptr RTABMAP_EXP transformPointCloud( +pcl::PointCloud::Ptr RTABMAP_CORE_EXPORT transformPointCloud( const pcl::PointCloud::Ptr & cloud, const pcl::IndicesPtr & indices, const Transform & transform); -pcl::PointCloud::Ptr RTABMAP_EXP transformPointCloud( +pcl::PointCloud::Ptr RTABMAP_CORE_EXPORT transformPointCloud( const pcl::PointCloud::Ptr & cloud, const pcl::IndicesPtr & indices, const Transform & transform); -pcl::PointCloud::Ptr RTABMAP_EXP transformPointCloud( +pcl::PointCloud::Ptr RTABMAP_CORE_EXPORT transformPointCloud( const pcl::PointCloud::Ptr & cloud, const pcl::IndicesPtr & indices, const Transform & transform); -pcl::PointCloud::Ptr RTABMAP_EXP transformPointCloud( +pcl::PointCloud::Ptr RTABMAP_CORE_EXPORT transformPointCloud( const pcl::PointCloud::Ptr & cloud, const pcl::IndicesPtr & indices, const Transform & transform); -cv::Point3f RTABMAP_EXP transformPoint( +cv::Point3f RTABMAP_CORE_EXPORT transformPoint( const cv::Point3f & pt, const Transform & transform); -cv::Point3d RTABMAP_EXP transformPoint( +cv::Point3d RTABMAP_CORE_EXPORT transformPoint( const cv::Point3d & pt, const Transform & transform); -pcl::PointXYZ RTABMAP_EXP transformPoint( +pcl::PointXYZ RTABMAP_CORE_EXPORT transformPoint( const pcl::PointXYZ & pt, const Transform & transform); -pcl::PointXYZI RTABMAP_EXP transformPoint( +pcl::PointXYZI RTABMAP_CORE_EXPORT transformPoint( const pcl::PointXYZI & pt, const Transform & transform); -pcl::PointXYZRGB RTABMAP_EXP transformPoint( +pcl::PointXYZRGB RTABMAP_CORE_EXPORT transformPoint( const pcl::PointXYZRGB & pt, const Transform & transform); -pcl::PointNormal RTABMAP_EXP transformPoint( +pcl::PointNormal RTABMAP_CORE_EXPORT transformPoint( const pcl::PointNormal & point, const Transform & transform); -pcl::PointXYZRGBNormal RTABMAP_EXP transformPoint( +pcl::PointXYZRGBNormal RTABMAP_CORE_EXPORT transformPoint( const pcl::PointXYZRGBNormal & point, const Transform & transform); -pcl::PointXYZINormal RTABMAP_EXP transformPoint( +pcl::PointXYZINormal RTABMAP_CORE_EXPORT transformPoint( const pcl::PointXYZINormal & point, const Transform & transform); diff --git a/corelib/src/CMakeLists.txt b/corelib/src/CMakeLists.txt index c7c8ceaa2f..208ecd049c 100644 --- a/corelib/src/CMakeLists.txt +++ b/corelib/src/CMakeLists.txt @@ -91,7 +91,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 @@ -147,20 +148,29 @@ IF(MSVC) ENDIF(MSVC) SET(INCLUDE_DIRS - ${CMAKE_CURRENT_BINARY_DIR}/../include - ${PROJECT_SOURCE_DIR}/utilite/include - ${CMAKE_CURRENT_SOURCE_DIR}/../include - ${CMAKE_CURRENT_SOURCE_DIR} - ${CMAKE_CURRENT_BINARY_DIR} - ${OpenCV_INCLUDE_DIRS} - ${PCL_INCLUDE_DIRS} + ${CMAKE_CURRENT_SOURCE_DIR} + ${CMAKE_CURRENT_SOURCE_DIR}/../include + ${CMAKE_CURRENT_BINARY_DIR} + ${CMAKE_CURRENT_BINARY_DIR}/include ${ZLIB_INCLUDE_DIRS} ) +SET(PUBLIC_INCLUDE_DIRS + ${OpenCV_INCLUDE_DIRS} + ${PCL_INCLUDE_DIRS} +) + SET(LIBRARIES - ${OpenCV_LIBS} - ${PCL_LIBRARIES} - ${ZLIB_LIBRARIES} + ${ZLIB_LIBRARIES} +) + +# Issue that qhull dependency uses optimized and debug keywords, +# which are converted to \$<\$> and \$<\$ +# in RTABMap_coreTargets.cmake (not sure why?!). +list(REMOVE_ITEM PCL_LIBRARIES "debug" "optimized") +SET(PUBLIC_LIBRARIES + ${OpenCV_LIBS} + ${PCL_LIBRARIES} ) IF(Sqlite3_FOUND) @@ -200,11 +210,15 @@ IF(TORCH_FOUND) ENDIF(TORCH_FOUND) IF(WITH_PYTHON AND Python3_FOUND) - SET(LIBRARIES - ${LIBRARIES} + SET(PUBLIC_LIBRARIES + ${PUBLIC_LIBRARIES} Python3::Python Python3::NumPy ) + SET(LIBRARIES + ${LIBRARIES} + pybind11::embed + ) SET(SRC_FILES ${SRC_FILES} python/PythonInterface.cpp @@ -268,18 +282,18 @@ IF(KinectSDK2_FOUND) ENDIF(KinectSDK2_FOUND) IF(k4a_FOUND) - SET(INCLUDE_DIRS - ${INCLUDE_DIRS} + SET(PUBLIC_INCLUDE_DIRS + ${PUBLIC_INCLUDE_DIRS} ${k4a_INCLUDE_DIRS} ) IF(WIN32) - SET(LIBRARIES - ${LIBRARIES} + SET(PUBLIC_LIBRARIES + ${PUBLIC_LIBRARIES} ${k4a_LIBRARIES} ) ELSE() - SET(LIBRARIES - ${LIBRARIES} + SET(PUBLIC_LIBRARIES + ${PUBLIC_LIBRARIES} k4a::k4a k4a::k4arecord ) @@ -298,23 +312,23 @@ IF(RealSense_FOUND) ENDIF(RealSense_FOUND) IF(realsense2_FOUND) - SET(INCLUDE_DIRS - ${INCLUDE_DIRS} + SET(PUBLIC_INCLUDE_DIRS + ${PUBLIC_INCLUDE_DIRS} ${realsense2_INCLUDE_DIRS} ) IF(WIN32) - SET(LIBRARIES - ${LIBRARIES} + SET(PUBLIC_LIBRARIES + ${PUBLIC_LIBRARIES} ${RealSense2_LIBRARIES} ) ELSEIF(APPLE) - SET(LIBRARIES - ${LIBRARIES} + SET(PUBLIC_LIBRARIES + ${PUBLIC_LIBRARIES} ${realsense2_LIBRARIES} ) ELSE() - SET(LIBRARIES - ${LIBRARIES} + SET(PUBLIC_LIBRARIES + ${PUBLIC_LIBRARIES} realsense2::realsense2 # target ) ENDIF() @@ -350,8 +364,8 @@ IF(mynteye_FOUND) ENDIF(mynteye_FOUND) IF(depthai_FOUND) - SET(LIBRARIES - ${LIBRARIES} + SET(PUBLIC_LIBRARIES + ${PUBLIC_LIBRARIES} depthai::core depthai::opencv ) @@ -391,7 +405,7 @@ IF(G2O_FOUND) g2o::csparse_extension) ENDIF(TARGET g2o::solver_csparse) IF(TARGET g2o::solver_cholmod) - SET(LIBRARIES ${LIBRARIES} + SET(LIBRARIES ${LIBRARIES} g2o::solver_cholmod) ENDIF(TARGET g2o::solver_cholmod) ELSE() @@ -437,6 +451,13 @@ IF(CERES_FOUND) ) ENDIF(CERES_FOUND) +IF(MRPT_FOUND) + SET(LIBRARIES + ${LIBRARIES} + ${MRPT_LIBRARIES} + ) +ENDIF(MRPT_FOUND) + IF(libpointmatcher_FOUND) SET(INCLUDE_DIRS ${INCLUDE_DIRS} @@ -556,14 +577,21 @@ IF(ZEDOC_FOUND) ENDIF(ZEDOC_FOUND) IF(octomap_FOUND) - SET(INCLUDE_DIRS - ${INCLUDE_DIRS} - ${OCTOMAP_INCLUDE_DIRS} - ) - SET(LIBRARIES - ${LIBRARIES} - ${OCTOMAP_LIBRARIES} - ) + IF(TARGET octomap) + SET(PUBLIC_LIBRARIES + ${PUBLIC_LIBRARIES} + octomap + ) + ELSE() + SET(PUBLIC_INCLUDE_DIRS + ${PUBLIC_INCLUDE_DIRS} + ${OCTOMAP_INCLUDE_DIRS} + ) + SET(PUBLIC_LIBRARIES + ${PUBLIC_LIBRARIES} + ${OCTOMAP_LIBRARIES} + ) + ENDIF() SET(SRC_FILES ${SRC_FILES} OctoMap.cpp @@ -696,6 +724,13 @@ IF(GTSAM_FOUND) ${LIBRARIES} gtsam # Windows: Place static libs at the end ) + IF(WIN32) + #explicitly add metis target on windows (after gtsam target) + SET(LIBRARIES + ${LIBRARIES} + metis + ) + ENDIF(WIN32) ENDIF(GTSAM_FOUND) IF(WITH_MADGWICK) @@ -724,6 +759,7 @@ foreach(arg ${RESOURCES}) get_filename_component(filename ${arg} NAME) string(REPLACE "." "_" output ${filename}) set(RESOURCES_HEADERS "${RESOURCES_HEADERS}" "${CMAKE_CURRENT_BINARY_DIR}/${output}.h") + set_property(SOURCE "${CMAKE_CURRENT_BINARY_DIR}/${output}.h" PROPERTY SKIP_AUTOGEN ON) endforeach(arg ${RESOURCES}) #MESSAGE(STATUS "RESOURCES = ${RESOURCES}") @@ -749,30 +785,63 @@ INCLUDE_DIRECTORIES(${INCLUDE_DIRS}) # Add binary that is built from the source file "main.cpp". # The extension is automatically found. ADD_LIBRARY(rtabmap_core ${SRC_FILES} ${RESOURCES_HEADERS}) -TARGET_LINK_LIBRARIES(rtabmap_core rtabmap_utilite ${LIBRARIES}) +ADD_LIBRARY(rtabmap::core ALIAS rtabmap_core) + +generate_export_header(rtabmap_core + DEPRECATED_MACRO_NAME RTABMAP_DEPRECATED) + +target_include_directories(rtabmap_core PUBLIC + "$" + "$") + +TARGET_LINK_LIBRARIES(rtabmap_core + PUBLIC + rtabmap_utilite + ${PUBLIC_LIBRARIES} + PRIVATE + ${LIBRARIES}) SET_TARGET_PROPERTIES( rtabmap_core PROPERTIES VERSION ${RTABMAP_VERSION} SOVERSION ${RTABMAP_MAJOR_VERSION}.${RTABMAP_MINOR_VERSION} + EXPORT_NAME "core" ) -INSTALL(TARGETS rtabmap_core +INSTALL(TARGETS rtabmap_core EXPORT rtabmap_coreTargets RUNTIME DESTINATION "${CMAKE_INSTALL_BINDIR}" COMPONENT runtime LIBRARY DESTINATION "${CMAKE_INSTALL_LIBDIR}" COMPONENT devel ARCHIVE DESTINATION "${CMAKE_INSTALL_LIBDIR}" COMPONENT devel) - -install(DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/../include/ - DESTINATION "${INSTALL_INCLUDE_DIR}" - COMPONENT devel - FILES_MATCHING PATTERN "*.h" PATTERN "*.hpp" - PATTERN ".svn" EXCLUDE) - -# For generated Version.h -install(DIRECTORY ${CMAKE_CURRENT_BINARY_DIR}/../include/ - DESTINATION "${INSTALL_INCLUDE_DIR}" - COMPONENT devel - FILES_MATCHING PATTERN "*.h" PATTERN "*.hpp" - PATTERN ".svn" EXCLUDE) + +configure_file( + ${CMAKE_CURRENT_BINARY_DIR}/rtabmap_core_export.h + ${CMAKE_CURRENT_BINARY_DIR}/include/${PROJECT_PREFIX}/core/rtabmap_core_export.h + COPYONLY) + +install( + DIRECTORY + ${CMAKE_CURRENT_SOURCE_DIR}/../include/${PROJECT_PREFIX} + ${CMAKE_CURRENT_BINARY_DIR}/include/${PROJECT_PREFIX} + DESTINATION + ${INSTALL_INCLUDE_DIR} + COMPONENT + devel + FILES_MATCHING PATTERN "*.h" PATTERN "*.hpp" +) + +export(EXPORT rtabmap_coreTargets + FILE "${CMAKE_CURRENT_BINARY_DIR}/../../${PROJECT_NAME}_coreTargets.cmake" + NAMESPACE rtabmap:: +) +install(EXPORT rtabmap_coreTargets + FILE + ${PROJECT_NAME}_coreTargets.cmake + DESTINATION + ${INSTALL_CMAKE_DIR} + NAMESPACE rtabmap:: + COMPONENT + devel +) + diff --git a/corelib/src/DBDriverSqlite3.cpp b/corelib/src/DBDriverSqlite3.cpp index 453c52201e..280c228acd 100644 --- a/corelib/src/DBDriverSqlite3.cpp +++ b/corelib/src/DBDriverSqlite3.cpp @@ -4298,9 +4298,9 @@ void DBDriverSqlite3::saveQuery(const std::list & signatures) { _memoryUsedEstimate += (*i)->getMemoryUsed(); // raw data are not kept in database - _memoryUsedEstimate -= (*i)->sensorData().imageRaw().total() * (*i)->sensorData().imageRaw().elemSize(); - _memoryUsedEstimate -= (*i)->sensorData().depthOrRightRaw().total() * (*i)->sensorData().depthOrRightRaw().elemSize(); - _memoryUsedEstimate -= (*i)->sensorData().laserScanRaw().data().total() * (*i)->sensorData().laserScanRaw().data().elemSize(); + _memoryUsedEstimate -= (*i)->sensorData().imageRaw().empty()?0:(*i)->sensorData().imageRaw().total() * (*i)->sensorData().imageRaw().elemSize(); + _memoryUsedEstimate -= (*i)->sensorData().depthOrRightRaw().empty()?0:(*i)->sensorData().depthOrRightRaw().total() * (*i)->sensorData().depthOrRightRaw().elemSize(); + _memoryUsedEstimate -= (*i)->sensorData().laserScanRaw().empty()?0:(*i)->sensorData().laserScanRaw().data().total() * (*i)->sensorData().laserScanRaw().data().elemSize(); stepNode(ppStmt, *i); } diff --git a/corelib/src/DBReader.cpp b/corelib/src/DBReader.cpp index 47892e2803..6eb27f796b 100644 --- a/corelib/src/DBReader.cpp +++ b/corelib/src/DBReader.cpp @@ -54,7 +54,8 @@ DBReader::DBReader(const std::string & databasePath, bool landmarksIgnored, bool featuresIgnored, int startMapId, - int stopMapId) : + int stopMapId, + bool priorsIgnored) : Camera(frameRate), _paths(uSplit(databasePath, ';')), _odometryIgnored(odometryIgnored), @@ -66,6 +67,7 @@ DBReader::DBReader(const std::string & databasePath, _intermediateNodesIgnored(intermediateNodesIgnored), _landmarksIgnored(landmarksIgnored), _featuresIgnored(featuresIgnored), + _priorsIgnored(priorsIgnored), _startMapId(startMapId), _stopMapId(stopMapId), _dbDriver(0), @@ -98,7 +100,8 @@ DBReader::DBReader(const std::list & databasePaths, bool landmarksIgnored, bool featuresIgnored, int startMapId, - int stopMapId) : + int stopMapId, + bool priorsIgnored) : Camera(frameRate), _paths(databasePaths), _odometryIgnored(odometryIgnored), @@ -110,6 +113,7 @@ DBReader::DBReader(const std::list & databasePaths, _intermediateNodesIgnored(intermediateNodesIgnored), _landmarksIgnored(landmarksIgnored), _featuresIgnored(featuresIgnored), + _priorsIgnored(priorsIgnored), _startMapId(startMapId), _stopMapId(stopMapId), _dbDriver(0), @@ -408,21 +412,24 @@ SensorData DBReader::getNextData(SensorCaptureInfo * info) cv::Mat globalPoseCov; std::multimap priorLinks; - _dbDriver->loadLinks(*_currentId, priorLinks, Link::kPosePrior); - if( priorLinks.size() && - !priorLinks.begin()->second.transform().isNull() && - priorLinks.begin()->second.infMatrix().cols == 6 && - priorLinks.begin()->second.infMatrix().rows == 6) + if(!_priorsIgnored) { - globalPose = priorLinks.begin()->second.transform(); - globalPoseCov = priorLinks.begin()->second.infMatrix().inv(); - if(data.gps().stamp() != 0.0 && - globalPoseCov.at(3,3)>=9999 && - globalPoseCov.at(4,4)>=9999 && - globalPoseCov.at(5,5)>=9999) + _dbDriver->loadLinks(*_currentId, priorLinks, Link::kPosePrior); + if( priorLinks.size() && + !priorLinks.begin()->second.transform().isNull() && + priorLinks.begin()->second.infMatrix().cols == 6 && + priorLinks.begin()->second.infMatrix().rows == 6) { - // clear global pose as GPS was used for prior - globalPose.setNull(); + globalPose = priorLinks.begin()->second.transform(); + globalPoseCov = priorLinks.begin()->second.infMatrix().inv(); + if(data.gps().stamp() != 0.0 && + globalPoseCov.at(3,3)>=9999 && + globalPoseCov.at(4,4)>=9999 && + globalPoseCov.at(5,5)>=9999) + { + // clear global pose as GPS was used for prior + globalPose.setNull(); + } } } @@ -516,11 +523,13 @@ SensorData DBReader::getNextData(SensorCaptureInfo * info) { float ratio = -this->getImageRate(); int sleepTime = 1000.0*(s->getStamp()-_previousStamp)/ratio - 1000.0*_timer.getElapsedTime(); + double stamp = s->getStamp(); if(sleepTime > 10000) { UWARN("Detected long delay (%d sec, stamps = %f vs %f). Waiting a maximum of 10 seconds.", sleepTime/1000, _previousStamp, s->getStamp()); sleepTime = 10000; + stamp = _previousStamp+10; } if(sleepTime > 2) { @@ -528,14 +537,14 @@ SensorData DBReader::getNextData(SensorCaptureInfo * info) } // Add precision at the cost of a small overhead - while(_timer.getElapsedTime() < (s->getStamp()-_previousStamp)/ratio-0.000001) + while(_timer.getElapsedTime() < (stamp-_previousStamp)/ratio-0.000001) { // } double slept = _timer.getElapsedTime(); _timer.start(); - UDEBUG("slept=%fs vs target=%fs (ratio=%f)", slept, (s->getStamp()-_previousStamp)/ratio, ratio); + UDEBUG("slept=%fs vs target=%fs (ratio=%f)", slept, (stamp-_previousStamp)/ratio, ratio); } _previousStamp = s->getStamp(); _previousMapID = s->mapId(); diff --git a/corelib/src/Features2d.cpp b/corelib/src/Features2d.cpp index 5df9f05bc8..d87a04e2fe 100644 --- a/corelib/src/Features2d.cpp +++ b/corelib/src/Features2d.cpp @@ -732,19 +732,19 @@ std::vector Feature2D::generateKeypoints(const cv::Mat & image, co for (int j = 0; j sub_keypoints; - sub_keypoints = this->generateKeypointsImpl(image, roi, mask); - limitKeypoints(sub_keypoints, maxFeatures); + std::vector subKeypoints; + subKeypoints = this->generateKeypointsImpl(image, roi, mask); + limitKeypoints(subKeypoints, maxFeatures); if(roi.x || roi.y) { // Adjust keypoint position to raw image - for(std::vector::iterator iter=sub_keypoints.begin(); iter!=sub_keypoints.end(); ++iter) + for(std::vector::iterator iter=subKeypoints.begin(); iter!=subKeypoints.end(); ++iter) { iter->pt.x += roi.x; iter->pt.y += roi.y; } } - keypoints.insert( keypoints.end(), sub_keypoints.begin(), sub_keypoints.end() ); + keypoints.insert( keypoints.end(), subKeypoints.begin(), subKeypoints.end() ); } } UDEBUG("Keypoints extraction time = %f s, keypoints extracted = %d (grid=%dx%d, mask empty=%d)", @@ -2119,6 +2119,24 @@ std::vector ORBOctree::generateKeypointsImpl(const cv::Mat & image (*_orb)(imgRoi, maskRoi, keypoints, descriptors_); + // OrbOctree ignores the mask, so we have to apply it manually here + if(!keypoints.empty() && !maskRoi.empty()) + { + std::vector validKeypoints; + validKeypoints.reserve(keypoints.size()); + cv::Mat validDescriptors; + for(size_t i=0; i(keypoints[i].pt.y+roi.y, keypoints[i].pt.x+roi.x) != 0) + { + validKeypoints.push_back(keypoints[i]); + validDescriptors.push_back(descriptors_.row(i)); + } + } + keypoints = validKeypoints; + descriptors_ = validDescriptors; + } + if((int)keypoints.size() > this->getMaxFeatures()) { limitKeypoints(keypoints, descriptors_, this->getMaxFeatures()); diff --git a/corelib/src/FlannIndex.cpp b/corelib/src/FlannIndex.cpp index c9a38f39d0..a593257780 100644 --- a/corelib/src/FlannIndex.cpp +++ b/corelib/src/FlannIndex.cpp @@ -80,7 +80,7 @@ void FlannIndex::release() UDEBUG(""); } -unsigned int FlannIndex::indexedFeatures() const +size_t FlannIndex::indexedFeatures() const { if(!index_) { @@ -108,13 +108,13 @@ unsigned int FlannIndex::indexedFeatures() const } // return Bytes -unsigned long FlannIndex::memoryUsed() const +size_t FlannIndex::memoryUsed() const { if(!index_) { return 0; } - unsigned long memoryUsage = sizeof(FlannIndex); + size_t memoryUsage = sizeof(FlannIndex); memoryUsage += addedDescriptors_.size() * (sizeof(int) + sizeof(cv::Mat) + sizeof(std::map::iterator)) + sizeof(std::map); memoryUsage += sizeof(std::list) + removedIndexes_.size() * sizeof(int); if(featuresType_ == CV_8UC1) diff --git a/corelib/src/Graph.cpp b/corelib/src/Graph.cpp index a9a645d8ae..da85efcc0c 100644 --- a/corelib/src/Graph.cpp +++ b/corelib/src/Graph.cpp @@ -430,7 +430,7 @@ bool importPoses( else if(format == 1 || format==10 || format==11) // rgbd-slam format { std::list strList = uSplit(str); - if((strList.size() == 8 && format!=11) || (strList.size() == 9 && format==11)) + if((strList.size() >= 8 && format!=11) || (strList.size() == 9 && format==11)) { double stamp = uStr2Double(strList.front()); strList.pop_front(); @@ -902,7 +902,7 @@ void computeMaxGraphErrors( float & maxAngularError, const Link ** maxLinearErrorLink, const Link ** maxAngularErrorLink, - bool for3DoF) + bool force3DoF) { maxLinearErrorRatio = -1; maxAngularErrorRatio = -1; @@ -912,17 +912,44 @@ void computeMaxGraphErrors( UDEBUG("poses=%d links=%d", (int)poses.size(), (int)links.size()); for(std::multimap::const_iterator iter=links.begin(); iter!=links.end(); ++iter) { - // ignore links with high variance, priors and landmarks - if(iter->second.transVariance() <= 1.0 && iter->second.from() != iter->second.to() && iter->second.type() != Link::kLandmark) + // ignore priors + if(iter->second.from() != iter->second.to()) { Transform t1 = uValue(poses, iter->second.from(), Transform()); Transform t2 = uValue(poses, iter->second.to(), Transform()); + + if( t1.isNull() || + t2.isNull() || + !t1.isInvertible() || + !t2.isInvertible()) + { + UWARN("Poses are null or not invertible, aborting optimized graph max error check! (Pose %d=%s Pose %d=%s)", + iter->second.from(), + t1.prettyPrint().c_str(), + iter->second.to(), + t2.prettyPrint().c_str()); + + if(maxLinearErrorLink) + { + *maxLinearErrorLink = 0; + } + if(maxAngularErrorLink) + { + *maxAngularErrorLink = 0; + } + maxLinearErrorRatio = -1; + maxAngularErrorRatio = -1; + maxLinearError = -1; + maxAngularError = -1; + return; + } + Transform t = t1.inverse()*t2; float linearError = uMax3( fabs(iter->second.transform().x() - t.x()), fabs(iter->second.transform().y() - t.y()), - for3DoF?0:fabs(iter->second.transform().z() - t.z())); + force3DoF?0:fabs(iter->second.transform().z() - t.z())); UASSERT(iter->second.transVariance(false)>0.0); float stddevLinear = sqrt(iter->second.transVariance(false)); float linearErrorRatio = linearError/stddevLinear; @@ -936,25 +963,30 @@ void computeMaxGraphErrors( } } - float opt_roll,opt_pitch,opt_yaw; - float link_roll,link_pitch,link_yaw; - t.getEulerAngles(opt_roll, opt_pitch, opt_yaw); - iter->second.transform().getEulerAngles(link_roll, link_pitch, link_yaw); - float angularError = uMax3( - for3DoF?0:fabs(opt_roll - link_roll), - for3DoF?0:fabs(opt_pitch - link_pitch), - fabs(opt_yaw - link_yaw)); - angularError = angularError>M_PI?2*M_PI-angularError:angularError; - UASSERT(iter->second.rotVariance(false)>0.0); - float stddevAngular = sqrt(iter->second.rotVariance(false)); - float angularErrorRatio = angularError/stddevAngular; - if(angularErrorRatio > maxAngularErrorRatio) + // For landmark links, don't compute angular error if it doesn't estimate orientation + if(iter->second.type() != Link::kLandmark || + 1.0 / static_cast(iter->second.infMatrix().at(5,5)) < 9999.0) { - maxAngularError = angularError; - maxAngularErrorRatio = angularErrorRatio; - if(maxAngularErrorLink) + float opt_roll,opt_pitch,opt_yaw; + float link_roll,link_pitch,link_yaw; + t.getEulerAngles(opt_roll, opt_pitch, opt_yaw); + iter->second.transform().getEulerAngles(link_roll, link_pitch, link_yaw); + float angularError = uMax3( + force3DoF?0:fabs(opt_roll - link_roll), + force3DoF?0:fabs(opt_pitch - link_pitch), + fabs(opt_yaw - link_yaw)); + angularError = angularError>M_PI?2*M_PI-angularError:angularError; + UASSERT(iter->second.rotVariance(false)>0.0); + float stddevAngular = sqrt(iter->second.rotVariance(false)); + float angularErrorRatio = angularError/stddevAngular; + if(angularErrorRatio > maxAngularErrorRatio) { - *maxAngularErrorLink = &iter->second; + maxAngularError = angularError; + maxAngularErrorRatio = angularErrorRatio; + if(maxAngularErrorLink) + { + *maxAngularErrorLink = &iter->second; + } } } } @@ -1022,6 +1054,39 @@ std::multimap::iterator findLink( return links.end(); } +std::multimap >::iterator findLink( + std::multimap > & links, + int from, + int to, + bool checkBothWays, + Link::Type type) +{ + std::multimap >::iterator iter = links.find(from); + while(iter != links.end() && iter->first == from) + { + if(iter->second.first == to && (type==Link::kUndef || type == iter->second.second)) + { + return iter; + } + ++iter; + } + + if(checkBothWays) + { + // let's try to -> from + iter = links.find(to); + while(iter != links.end() && iter->first == to) + { + if(iter->second.first == from && (type==Link::kUndef || type == iter->second.second)) + { + return iter; + } + ++iter; + } + } + return links.end(); +} + std::multimap::iterator findLink( std::multimap & links, int from, @@ -1086,6 +1151,39 @@ std::multimap::const_iterator findLink( return links.end(); } +std::multimap >::const_iterator findLink( + const std::multimap > & links, + int from, + int to, + bool checkBothWays, + Link::Type type) +{ + std::multimap >::const_iterator iter = links.find(from); + while(iter != links.end() && iter->first == from) + { + if(iter->second.first == to && (type==Link::kUndef || type == iter->second.second)) + { + return iter; + } + ++iter; + } + + if(checkBothWays) + { + // let's try to -> from + iter = links.find(to); + while(iter != links.end() && iter->first == to) + { + if(iter->second.first == from && (type==Link::kUndef || type == iter->second.second)) + { + return iter; + } + ++iter; + } + } + return links.end(); +} + std::multimap::const_iterator findLink( const std::multimap & links, int from, @@ -1761,7 +1859,7 @@ std::list > computePath( } // Dijksta -std::list RTABMAP_EXP computePath( +std::list computePath( const std::multimap & links, int from, int to, @@ -2218,7 +2316,7 @@ std::map findNearestPoses( { foundPoses.insert(*poses.find(iter->first)); } - UDEBUG("found nodes=%d", (int)foundPoses.size()); + UDEBUG("found nodes=%d/%d (radius=%f, angle=%f, k=%d)", (int)foundPoses.size(), (int)poses.size(), radius, angle, k); return foundPoses; } diff --git a/corelib/src/IMUThread.cpp b/corelib/src/IMUThread.cpp index 2397390cbf..58072ed83c 100644 --- a/corelib/src/IMUThread.cpp +++ b/corelib/src/IMUThread.cpp @@ -27,6 +27,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "rtabmap/core/IMUThread.h" #include "rtabmap/core/IMU.h" +#include "rtabmap/core/IMUFilter.h" #include #include #include @@ -38,13 +39,16 @@ IMUThread::IMUThread(int rate, const Transform & localTransform) : rate_(rate), localTransform_(localTransform), captureDelay_(0.0), - previousStamp_(0.0) + previousStamp_(0.0), + _imuFilter(0), + _imuBaseFrameConversion(false) { } IMUThread::~IMUThread() { imuFile_.close(); + delete _imuFilter; } bool IMUThread::init(const std::string & path) @@ -81,6 +85,19 @@ void IMUThread::setRate(int rate) rate_ = rate; } +void IMUThread::enableIMUFiltering(int filteringStrategy, const ParametersMap & parameters, bool baseFrameConversion) +{ + delete _imuFilter; + _imuFilter = IMUFilter::create((IMUFilter::Type)filteringStrategy, parameters); + _imuBaseFrameConversion = baseFrameConversion; +} + +void IMUThread::disableIMUFiltering() +{ + delete _imuFilter; + _imuFilter = 0; +} + void IMUThread::mainLoopBegin() { ULogger::registerCurrentThread("IMU"); @@ -141,6 +158,60 @@ void IMUThread::mainLoop() previousStamp_ = stamp; IMU imu(gyr, cv::Mat(), acc, cv::Mat(), localTransform_); + + // IMU filtering + if(_imuFilter && !imu.empty()) + { + if(imu.angularVelocity()[0] == 0 && + imu.angularVelocity()[1] == 0 && + imu.angularVelocity()[2] == 0 && + imu.linearAcceleration()[0] == 0 && + imu.linearAcceleration()[1] == 0 && + imu.linearAcceleration()[2] == 0) + { + UWARN("IMU's acc and gyr values are null! Please disable IMU filtering."); + } + else + { + // Transform IMU data in base_link to correctly initialize yaw + if(_imuBaseFrameConversion) + { + UASSERT(!imu.localTransform().isNull()); + imu.convertToBaseFrame(); + + } + _imuFilter->update( + imu.angularVelocity()[0], + imu.angularVelocity()[1], + imu.angularVelocity()[2], + imu.linearAcceleration()[0], + imu.linearAcceleration()[1], + imu.linearAcceleration()[2], + stamp); + double qx,qy,qz,qw; + _imuFilter->getOrientation(qx,qy,qz,qw); + + imu = IMU( + cv::Vec4d(qx,qy,qz,qw), cv::Mat::eye(3,3,CV_64FC1), + imu.angularVelocity(), imu.angularVelocityCovariance(), + imu.linearAcceleration(), imu.linearAccelerationCovariance(), + imu.localTransform()); + + UDEBUG("%f %f %f %f (gyro=%f %f %f, acc=%f %f %f, %fs)", + imu.orientation()[0], + imu.orientation()[1], + imu.orientation()[2], + imu.orientation()[3], + imu.angularVelocity()[0], + imu.angularVelocity()[1], + imu.angularVelocity()[2], + imu.linearAcceleration()[0], + imu.linearAcceleration()[1], + imu.linearAcceleration()[2], + stamp); + } + } + this->post(new IMUEvent(imu, stamp)); } else if(!this->isKilled()) diff --git a/corelib/src/Link.cpp b/corelib/src/Link.cpp index 4b776eb73d..ad15e03fa0 100644 --- a/corelib/src/Link.cpp +++ b/corelib/src/Link.cpp @@ -163,7 +163,7 @@ cv::Mat Link::uncompressUserDataConst() const Link Link::merge(const Link & link, Type outputType) const { - UASSERT(to_ == link.from()); + UASSERT_MSG(to_ == link.from(), uFormat("merging this=%d->%d to link=%d->%d", from_, to_, link.from(), link.to()).c_str()); UASSERT(outputType != Link::kUndef); UASSERT((link.transform().isNull() && transform_.isNull()) || (!link.transform().isNull() && !transform_.isNull())); UASSERT(infMatrix_.cols == 6 && infMatrix_.rows == 6 && infMatrix_.type() == CV_64FC1); diff --git a/corelib/src/MarkerDetector.cpp b/corelib/src/MarkerDetector.cpp index 46f86b5346..f9cf82d8cf 100644 --- a/corelib/src/MarkerDetector.cpp +++ b/corelib/src/MarkerDetector.cpp @@ -39,12 +39,16 @@ MarkerDetector::MarkerDetector(const ParametersMap & parameters) maxRange_ = Parameters::defaultMarkerMaxRange(); minRange_ = Parameters::defaultMarkerMinRange(); dictionaryId_ = Parameters::defaultMarkerDictionary(); -#if CV_MAJOR_VERSION > 3 || (CV_MAJOR_VERSION == 3 && CV_MINOR_VERSION >=2) +#if CV_MAJOR_VERSION > 4 || (CV_MAJOR_VERSION == 4 && CV_MINOR_VERSION >= 7) + detectorParams_.reset(new cv::aruco::DetectorParameters()); +#elif CV_MAJOR_VERSION > 3 || (CV_MAJOR_VERSION == 3 && CV_MINOR_VERSION >=2) detectorParams_ = cv::aruco::DetectorParameters::create(); #else detectorParams_.reset(new cv::aruco::DetectorParameters()); #endif -#if CV_MAJOR_VERSION > 3 || (CV_MAJOR_VERSION == 3 && CV_MINOR_VERSION >=3) +#if CV_MAJOR_VERSION > 4 || (CV_MAJOR_VERSION == 4 && CV_MINOR_VERSION >= 7) + detectorParams_->cornerRefinementMethod = (cv::aruco::CornerRefineMethod) Parameters::defaultMarkerCornerRefinementMethod(); +#elif CV_MAJOR_VERSION > 3 || (CV_MAJOR_VERSION == 3 && CV_MINOR_VERSION >=3) detectorParams_->cornerRefinementMethod = Parameters::defaultMarkerCornerRefinementMethod(); #else detectorParams_->doCornerRefinement = Parameters::defaultMarkerCornerRefinementMethod()!=0; @@ -70,7 +74,11 @@ void MarkerDetector::parseParameters(const ParametersMap & parameters) detectorParams_->minCornerDistanceRate = 0.05; detectorParams_->minDistanceToBorder = 3; detectorParams_->minMarkerDistanceRate = 0.05; -#if CV_MAJOR_VERSION > 3 || (CV_MAJOR_VERSION == 3 && CV_MINOR_VERSION >=3) +#if CV_MAJOR_VERSION > 4 || (CV_MAJOR_VERSION == 4 && CV_MINOR_VERSION >= 7) + int cornerRefinementMethod; + Parameters::parse(parameters, Parameters::kMarkerCornerRefinementMethod(), cornerRefinementMethod); + detectorParams_->cornerRefinementMethod = (cv::aruco::CornerRefineMethod)cornerRefinementMethod; +#elif CV_MAJOR_VERSION > 3 || (CV_MAJOR_VERSION == 3 && CV_MINOR_VERSION >=3) Parameters::parse(parameters, Parameters::kMarkerCornerRefinementMethod(), detectorParams_->cornerRefinementMethod); #else int doCornerRefinement = detectorParams_->doCornerRefinement?1:0; @@ -103,7 +111,10 @@ void MarkerDetector::parseParameters(const ParametersMap & parameters) dictionaryId_ = Parameters::defaultMarkerDictionary(); } #endif -#if CV_MAJOR_VERSION > 3 || (CV_MAJOR_VERSION == 3 && CV_MINOR_VERSION >=2) +#if CV_MAJOR_VERSION > 4 || (CV_MAJOR_VERSION == 4 && CV_MINOR_VERSION >= 7) + dictionary_.reset(new cv::aruco::Dictionary()); + *dictionary_ = cv::aruco::getPredefinedDictionary(cv::aruco::PredefinedDictionaryType(dictionaryId_)); +#elif CV_MAJOR_VERSION > 3 || (CV_MAJOR_VERSION == 3 && CV_MINOR_VERSION >=2) dictionary_ = cv::aruco::getPredefinedDictionary(cv::aruco::PREDEFINED_DICTIONARY_NAME(dictionaryId_)); #else dictionary_.reset(new cv::aruco::Dictionary()); diff --git a/corelib/src/Memory.cpp b/corelib/src/Memory.cpp index 91e5b5ebf8..9de4886d06 100644 --- a/corelib/src/Memory.cpp +++ b/corelib/src/Memory.cpp @@ -283,6 +283,10 @@ void Memory::loadDataFromDb(bool postInitClosingEvents) -landmarkId, inserted.first->second, landmarkSize.at(0,0)); } } + else + { + UDEBUG("Caching landmark size %f for %d", landmarkSize.at(0,0), -landmarkId); + } } std::map >::iterator nter = _landmarksIndex.find(landmarkId); @@ -4966,6 +4970,10 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor if(stats) stats->addStatistic(Statistics::kTimingMemKeypoints_3D(), t*1000.0f); UDEBUG("time keypoints 3D (%d) = %fs", (int)keypoints3D.size(), t); } + if(depthMask.empty() && (_feature2D->getMinDepth() > 0.0f || _feature2D->getMaxDepth() > 0.0f)) + { + _feature2D->filterKeypointsByDepth(keypoints, descriptors, keypoints3D, _feature2D->getMinDepth(), _feature2D->getMaxDepth()); + } } } else if(data.imageRaw().empty()) diff --git a/corelib/src/OccupancyGrid.cpp b/corelib/src/OccupancyGrid.cpp index 5305700a07..dc2d1b5be0 100644 --- a/corelib/src/OccupancyGrid.cpp +++ b/corelib/src/OccupancyGrid.cpp @@ -377,7 +377,7 @@ void OccupancyGrid::createLocalMap( } else { - UWARN("Cannot create local map, scan is empty (node=%d, %s=0).", node.id(), Parameters::kGridSensor().c_str()); + UWARN("Cannot create local map from scan: scan is empty (node=%d, %s=%d).", node.id(), Parameters::kGridSensor().c_str(), occupancySensor_); } } @@ -457,9 +457,12 @@ void OccupancyGrid::createLocalMap( if(occupancySensor_ == 2) { // backup - scanGroundCells = groundCells.clone(); - scanObstacleCells = obstacleCells.clone(); - scanEmptyCells = emptyCells.clone(); + scanGroundCells = groundCells; + scanObstacleCells = obstacleCells; + scanEmptyCells = emptyCells; + groundCells = cv::Mat(); + obstacleCells = cv::Mat(); + emptyCells = cv::Mat(); } createLocalMap(LaserScan(util3d::laserScanFromPointCloud(*cloud, indices), 0, 0.0f), node.getPose(), groundCells, obstacleCells, emptyCells, viewPoint); diff --git a/corelib/src/Odometry.cpp b/corelib/src/Odometry.cpp index b29b49b812..03e07d7363 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); @@ -326,6 +331,10 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet imus_.erase(imus_.begin()); } } + else + { + UWARN("Received IMU doesn't have orientation set! It is ignored."); + } } @@ -738,12 +747,12 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet UASSERT(info->newCorners.size() == info->refCorners.size() || info->refCorners.empty()); for(unsigned int i=0; inewCorners.size(); ++i) { - info->refCorners[i].x *= _imageDecimation; - info->refCorners[i].y *= _imageDecimation; + info->newCorners[i].x *= _imageDecimation; + info->newCorners[i].y *= _imageDecimation; if(!info->refCorners.empty()) { - info->newCorners[i].x *= _imageDecimation; - info->newCorners[i].y *= _imageDecimation; + info->refCorners[i].x *= _imageDecimation; + info->refCorners[i].y *= _imageDecimation; } } for(std::multimap::iterator iter=info->words.begin(); iter!=info->words.end(); ++iter) diff --git a/corelib/src/Optimizer.cpp b/corelib/src/Optimizer.cpp index cf1d4e10e3..9b3b6704e2 100644 --- a/corelib/src/Optimizer.cpp +++ b/corelib/src/Optimizer.cpp @@ -190,8 +190,7 @@ void Optimizer::getConnectedGraph( const std::map & posesIn, const std::multimap & linksIn, std::map & posesOut, - std::multimap & linksOut, - bool adjustPosesWithConstraints) const + std::multimap & linksOut) const { UDEBUG("IN: fromId=%d poses=%d links=%d priorsIgnored=%d landmarksIgnored=%d", fromId, (int)posesIn.size(), (int)linksIn.size(), priorsIgnored()?1:0, landmarksIgnored()?1:0); UASSERT(fromId>0); @@ -202,15 +201,15 @@ void Optimizer::getConnectedGraph( std::set nextPoses; nextPoses.insert(fromId); - std::multimap biLinks; + std::multimap > biLinks; for(std::multimap::const_iterator iter=linksIn.begin(); iter!=linksIn.end(); ++iter) { if(iter->second.from() != iter->second.to()) { - if(graph::findLink(biLinks, iter->second.from(), iter->second.to()) == biLinks.end()) + if(graph::findLink(biLinks, iter->second.from(), iter->second.to(), true, iter->second.type()) == biLinks.end()) { - biLinks.insert(std::make_pair(iter->second.from(), iter->second.to())); - biLinks.insert(std::make_pair(iter->second.to(), iter->second.from())); + biLinks.insert(std::make_pair(iter->second.from(), std::make_pair(iter->second.to(), iter->second.type()))); + biLinks.insert(std::make_pair(iter->second.to(), std::make_pair(iter->second.from(), iter->second.type()))); } } } @@ -234,41 +233,35 @@ void Optimizer::getConnectedGraph( } } - for(std::multimap::const_iterator iter=biLinks.find(currentId); iter!=biLinks.end() && iter->first==currentId; ++iter) + for(std::multimap >::const_iterator iter=biLinks.find(currentId); iter!=biLinks.end() && iter->first==currentId; ++iter) { - int toId = iter->second; + int toId = iter->second.first; + Link::Type type = iter->second.second; if(posesIn.find(toId) != posesIn.end() && (!landmarksIgnored() || toId>0)) { - std::multimap::const_iterator kter = graph::findLink(linksIn, currentId, toId); + std::multimap::const_iterator kter = graph::findLink(linksIn, currentId, toId, true, type); if(nextPoses.find(toId) == nextPoses.end()) { if(!uContains(posesOut, toId)) { - if(adjustPosesWithConstraints) + const Transform & poseToIn = posesIn.at(toId); + Transform t = kter->second.from()==currentId?kter->second.transform():kter->second.transform().inverse(); + if(isSlam2d() && kter->second.type() == Link::kLandmark && toId>0 && (poseToIn.is3DoF() || poseToIn.is4DoF())) { - if(isSlam2d() && kter->second.type() == Link::kLandmark && toId>0) + if(poseToIn.is3DoF()) { - Transform t; - if(kter->second.from()==currentId) - { - t = kter->second.transform(); - } - else - { - t = kter->second.transform().inverse(); - } posesOut.insert(std::make_pair(toId, (posesOut.at(currentId) * t).to3DoF())); } else { - Transform t = posesOut.at(currentId) * (kter->second.from()==currentId?kter->second.transform():kter->second.transform().inverse()); - posesOut.insert(std::make_pair(toId, t)); + posesOut.insert(std::make_pair(toId, (posesOut.at(currentId) * t).to4DoF())); } } else { - posesOut.insert(*posesIn.find(toId)); + posesOut.insert(std::make_pair(toId, posesOut.at(currentId)* t)); } + // add prior links for(std::multimap::const_iterator pter=linksIn.find(toId); pter!=linksIn.end() && pter->first==toId; ++pter) { @@ -282,7 +275,7 @@ void Optimizer::getConnectedGraph( } // only add unique links - if(graph::findLink(linksOut, currentId, toId) == linksOut.end()) + if(graph::findLink(linksOut, currentId, toId, true, kter->second.type()) == linksOut.end()) { if(kter->second.to() < 0) { diff --git a/corelib/src/RegistrationIcp.cpp b/corelib/src/RegistrationIcp.cpp index 35f2dd20ed..11611ae11d 100644 --- a/corelib/src/RegistrationIcp.cpp +++ b/corelib/src/RegistrationIcp.cpp @@ -64,6 +64,7 @@ RegistrationIcp::RegistrationIcp(const ParametersMap & parameters, Registration _rangeMin(Parameters::defaultIcpRangeMin()), _rangeMax(Parameters::defaultIcpRangeMax()), _maxCorrespondenceDistance(Parameters::defaultIcpMaxCorrespondenceDistance()), + _reciprocalCorrespondences(Parameters::defaultIcpReciprocalCorrespondences()), _maxIterations(Parameters::defaultIcpIterations()), _epsilon(Parameters::defaultIcpEpsilon()), _correspondenceRatio(Parameters::defaultIcpCorrespondenceRatio()), @@ -110,6 +111,7 @@ void RegistrationIcp::parseParameters(const ParametersMap & parameters) Parameters::parse(parameters, Parameters::kIcpRangeMin(), _rangeMin); Parameters::parse(parameters, Parameters::kIcpRangeMax(), _rangeMax); Parameters::parse(parameters, Parameters::kIcpMaxCorrespondenceDistance(), _maxCorrespondenceDistance); + Parameters::parse(parameters, Parameters::kIcpReciprocalCorrespondences(), _reciprocalCorrespondences); Parameters::parse(parameters, Parameters::kIcpIterations(), _maxIterations); Parameters::parse(parameters, Parameters::kIcpEpsilon(), _epsilon); Parameters::parse(parameters, Parameters::kIcpCorrespondenceRatio(), _correspondenceRatio); @@ -652,7 +654,8 @@ Transform RegistrationIcp::computeTransformationImpl( _maxCorrespondenceDistance, _maxRotation, variance, - correspondences); + correspondences, + _reciprocalCorrespondences); } } //////////////////// @@ -843,7 +846,8 @@ Transform RegistrationIcp::computeTransformationImpl( toCloud, _maxCorrespondenceDistance, variance, - correspondences); + correspondences, + _reciprocalCorrespondences); } } // END Registration PointToPLane to PointToPoint UDEBUG("ICP (iterations=%d) time = %f s", _maxIterations, timer.ticks()); diff --git a/corelib/src/RegistrationVis.cpp b/corelib/src/RegistrationVis.cpp index 6b0578e1ea..0bdeb0cd34 100644 --- a/corelib/src/RegistrationVis.cpp +++ b/corelib/src/RegistrationVis.cpp @@ -30,6 +30,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include +#include #include #include #include @@ -69,7 +70,9 @@ RegistrationVis::RegistrationVis(const ParametersMap & parameters, Registration _PnPReprojError(Parameters::defaultVisPnPReprojError()), _PnPFlags(Parameters::defaultVisPnPFlags()), _PnPRefineIterations(Parameters::defaultVisPnPRefineIterations()), + _PnPVarMedianRatio(Parameters::defaultVisPnPVarianceMedianRatio()), _PnPMaxVar(Parameters::defaultVisPnPMaxVariance()), + _multiSamplingPolicy(Parameters::defaultVisPnPSamplingPolicy()), _correspondencesApproach(Parameters::defaultVisCorType()), _flowWinSize(Parameters::defaultVisCorFlowWinSize()), _flowIterations(Parameters::defaultVisCorFlowIterations()), @@ -125,7 +128,9 @@ void RegistrationVis::parseParameters(const ParametersMap & parameters) Parameters::parse(parameters, Parameters::kVisPnPReprojError(), _PnPReprojError); Parameters::parse(parameters, Parameters::kVisPnPFlags(), _PnPFlags); Parameters::parse(parameters, Parameters::kVisPnPRefineIterations(), _PnPRefineIterations); + Parameters::parse(parameters, Parameters::kVisPnPVarianceMedianRatio(), _PnPVarMedianRatio); Parameters::parse(parameters, Parameters::kVisPnPMaxVariance(), _PnPMaxVar); + Parameters::parse(parameters, Parameters::kVisPnPSamplingPolicy(), _multiSamplingPolicy); Parameters::parse(parameters, Parameters::kVisCorType(), _correspondencesApproach); Parameters::parse(parameters, Parameters::kVisCorFlowWinSize(), _flowWinSize); Parameters::parse(parameters, Parameters::kVisCorFlowIterations(), _flowIterations); @@ -484,15 +489,16 @@ Transform RegistrationVis::computeTransformationImpl( if(!imageFrom.empty() && !imageTo.empty()) { + UASSERT(!toSignature.sensorData().cameraModels().empty() || !toSignature.sensorData().stereoCameraModels().empty()); std::vector cornersFrom; cv::KeyPoint::convert(kptsFrom, cornersFrom); std::vector cornersTo; bool guessSet = !guess.isIdentity() && !guess.isNull(); if(guessSet) { - if(fromSignature.sensorData().cameraModels().size() == 1 || fromSignature.sensorData().cameraModels().size() == 1) + if(toSignature.sensorData().cameraModels().size() == 1 || toSignature.sensorData().stereoCameraModels().size() == 1) { - Transform localTransform = fromSignature.sensorData().cameraModels().size()?fromSignature.sensorData().cameraModels()[0].localTransform():fromSignature.sensorData().stereoCameraModels()[0].left().localTransform(); + Transform localTransform = toSignature.sensorData().cameraModels().size()?toSignature.sensorData().cameraModels()[0].localTransform():toSignature.sensorData().stereoCameraModels()[0].left().localTransform(); Transform guessCameraRef = (guess * localTransform).inverse(); cv::Mat R = (cv::Mat_(3,3) << (double)guessCameraRef.r11(), (double)guessCameraRef.r12(), (double)guessCameraRef.r13(), @@ -501,12 +507,53 @@ Transform RegistrationVis::computeTransformationImpl( cv::Mat rvec(1,3, CV_64FC1); cv::Rodrigues(R, rvec); cv::Mat tvec = (cv::Mat_(1,3) << (double)guessCameraRef.x(), (double)guessCameraRef.y(), (double)guessCameraRef.z()); - cv::Mat K = fromSignature.sensorData().cameraModels().size()?fromSignature.sensorData().cameraModels()[0].K():fromSignature.sensorData().stereoCameraModels()[0].left().K(); + cv::Mat K = toSignature.sensorData().cameraModels().size()?toSignature.sensorData().cameraModels()[0].K():toSignature.sensorData().stereoCameraModels()[0].left().K(); cv::projectPoints(kptsFrom3D, rvec, tvec, K, cv::Mat(), cornersTo); } else { - UERROR("Optical flow guess with multi-cameras is not implemented, guess ignored..."); + UTimer t; + int nCameras = toSignature.sensorData().cameraModels().size()?toSignature.sensorData().cameraModels().size():toSignature.sensorData().stereoCameraModels().size(); + cornersTo = cornersFrom; + // compute inverse transforms one time + std::vector inverseTransforms(nCameras); + for(int c=0; c %s", c, inverseTransforms[c].prettyPrint().c_str()); + } + // Project 3D points in each camera + int inFrame = 0; + UASSERT(kptsFrom3D.size() == cornersTo.size()); + int subImageWidth = toSignature.sensorData().cameraModels().size()?toSignature.sensorData().cameraModels()[0].imageWidth():toSignature.sensorData().stereoCameraModels()[0].left().imageWidth(); + UASSERT(subImageWidth>0); + for(size_t i=0; i 0) + { + float u,v; + model.reproject(ptsInCamFrame.x, ptsInCamFrame.y, ptsInCamFrame.z, u, v); + if(model.inFrame(u,v)) + { + cornersTo[i].x = u+model.imageWidth()*c; + cornersTo[i].y = v; + ++inFrame; + break; + } + } + } + } + + UDEBUG("Projected %d/%ld points inside %d cameras (time=%fs)", + inFrame, cornersTo.size(), nCameras, t.ticks()); } } @@ -1048,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); @@ -1578,11 +1625,13 @@ Transform RegistrationVis::computeTransformationImpl( words3A, wordsB, models, + _multiSamplingPolicy, _minInliers, _iterations, _PnPReprojError, _PnPFlags, _PnPRefineIterations, + _PnPVarMedianRatio, _PnPMaxVar, dir==0?(!guess.isNull()?guess:Transform::getIdentity()):!transforms[0].isNull()?transforms[0].inverse():(!guess.isNull()?guess.inverse():Transform::getIdentity()), words3B, @@ -1605,6 +1654,7 @@ Transform RegistrationVis::computeTransformationImpl( _PnPReprojError, _PnPFlags, _PnPRefineIterations, + _PnPVarMedianRatio, _PnPMaxVar, dir==0?(!guess.isNull()?guess:Transform::getIdentity()):!transforms[0].isNull()?transforms[0].inverse():(!guess.isNull()?guess.inverse():Transform::getIdentity()), words3B, @@ -2021,29 +2071,31 @@ Transform RegistrationVis::computeTransformationImpl( } for(unsigned int i=0; i0.0f) + std::multimap::const_iterator wordsIter = toSignature.getWords().find(allInliers[i]); + if(wordsIter != toSignature.getWords().end() && !toSignature.getWordsKpts().empty()) { - std::multimap::const_iterator wordsIter = fromSignature.getWords().find(allInliers[i]); - if(wordsIter != fromSignature.getWords().end() && !fromSignature.getWords3().empty()) + const cv::KeyPoint & kpt = toSignature.getWordsKpts()[wordsIter->second]; + int cameraIndex = (int)(kpt.pt.x / cameraModelsTo[0].imageWidth()); + UASSERT_MSG(cameraIndex < (int)cameraModelsTo.size(), uFormat("cameraIndex=%d (x=%f models=%d camera width = %d)", cameraIndex, kpt.pt.x, (int)cameraModelsTo.size(), cameraModelsTo[0].imageWidth()).c_str()); + + if(_maxInliersMeanDistance>0.0f && !toSignature.getWords3().empty()) { - const cv::Point3f & pt = fromSignature.getWords3()[wordsIter->second]; - if(uIsFinite(pt.x)) + const cv::Point3f & pt = toSignature.getWords3()[wordsIter->second]; + if(util3d::isFinite(pt)) { - distances.push_back(util3d::transformPoint(pt, transformInv).x); + UASSERT(cameraModelsTo[cameraIndex].isValidForProjection()); + + float depth = util3d::transformPoint(pt, cameraModelsTo[cameraIndex].localTransform().inverse()).z; + distances.push_back(depth); } } - } - if(!pcaData.empty()) - { - std::multimap::const_iterator wordsIter = toSignature.getWords().find(allInliers[i]); - UASSERT(wordsIter != fromSignature.getWords().end() && !toSignature.getWordsKpts().empty()); - float * ptr = pcaData.ptr(i, 0); - const cv::KeyPoint & kpt = toSignature.getWordsKpts()[wordsIter->second]; - int cameraIndex = (int)(kpt.pt.x / cameraModelsTo[0].imageWidth()); - UASSERT_MSG(cameraIndex < (int)cameraModelsTo.size(), uFormat("cameraIndex=%d (x=%f models=%d camera width = %d)", cameraIndex, kpt.pt.x, (int)cameraModelsTo.size(), cameraModelsTo[0].imageWidth()).c_str()); - ptr[0] = (kpt.pt.x-cameraIndex*cameraModelsTo[cameraIndex].imageWidth()-cameraModelsTo[cameraIndex].cx()) / cameraModelsTo[cameraIndex].imageWidth(); - ptr[1] = (kpt.pt.y-cameraModelsTo[cameraIndex].cy()) / cameraModelsTo[cameraIndex].imageHeight(); + if(!pcaData.empty()) + { + float * ptr = pcaData.ptr(i, 0); + ptr[0] = (kpt.pt.x-cameraIndex*cameraModelsTo[cameraIndex].imageWidth()-cameraModelsTo[cameraIndex].cx()) / cameraModelsTo[cameraIndex].imageWidth(); + ptr[1] = (kpt.pt.y-cameraModelsTo[cameraIndex].cy()) / cameraModelsTo[cameraIndex].imageHeight(); + } } } diff --git a/corelib/src/Rtabmap.cpp b/corelib/src/Rtabmap.cpp index 18223799f6..33bb603915 100644 --- a/corelib/src/Rtabmap.cpp +++ b/corelib/src/Rtabmap.cpp @@ -44,6 +44,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "rtabmap/core/VWDictionary.h" #include "rtabmap/core/BayesFilter.h" #include "rtabmap/core/Compression.h" +#include "rtabmap/core/Registration.h" #include "rtabmap/core/RegistrationInfo.h" #include @@ -57,6 +58,11 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "rtabmap/core/PythonInterface.h" #endif +#ifdef RTABMAP_MRPT +// Used for odometry error propagation +#include +#endif + #include #include #include @@ -141,6 +147,8 @@ Rtabmap::Rtabmap() : _loopCovLimited(Parameters::defaultRGBDLoopCovLimited()), _loopGPS(Parameters::defaultRtabmapLoopGPS()), _maxOdomCacheSize(Parameters::defaultRGBDMaxOdomCacheSize()), + _localizationSmoothing(Parameters::defaultRGBDLocalizationSmoothing()), + _localizationPriorInf(1.0/(Parameters::defaultRGBDLocalizationPriorError()*Parameters::defaultRGBDLocalizationPriorError())), _createGlobalScanMap(Parameters::defaultRGBDProximityGlobalScanMap()), _markerPriorsLinearVariance(Parameters::defaultMarkerPriorsVarianceLinear()), _markerPriorsAngularVariance(Parameters::defaultMarkerPriorsVarianceAngular()), @@ -161,7 +169,6 @@ Rtabmap::Rtabmap() : _mapCorrection(Transform::getIdentity()), _lastLocalizationNodeId(0), _currentSessionHasGPS(false), - _odomCorrectionAcc(6,0), _pathStatus(0), _pathCurrentIndex(0), _pathGoalIndex(0), @@ -461,10 +468,10 @@ void Rtabmap::close(bool databaseSaved, const std::string & ouputDatabasePath) _mapCorrection.setIdentity(); _mapCorrectionBackup.setNull(); + _localizationCovariance = cv::Mat(); _lastLocalizationNodeId = 0; _odomCachePoses.clear(); _odomCacheConstraints.clear(); - _odomCorrectionAcc = std::vector(6,0); _distanceTravelled = 0.0f; _distanceTravelledSinceLastLocalization = 0.0f; _optimizeFromGraphEndChanged = false; @@ -613,6 +620,11 @@ void Rtabmap::parseParameters(const ParametersMap & parameters) Parameters::parse(parameters, Parameters::kRGBDLoopCovLimited(), _loopCovLimited); Parameters::parse(parameters, Parameters::kRtabmapLoopGPS(), _loopGPS); Parameters::parse(parameters, Parameters::kRGBDMaxOdomCacheSize(), _maxOdomCacheSize); + Parameters::parse(parameters, Parameters::kRGBDLocalizationSmoothing(), _localizationSmoothing); + double localizationPriorError = Parameters::defaultRGBDLocalizationPriorError(); + Parameters::parse(parameters, Parameters::kRGBDLocalizationPriorError(), localizationPriorError); + UASSERT(localizationPriorError>0.0); + _localizationPriorInf = 1.0/(localizationPriorError*localizationPriorError); Parameters::parse(parameters, Parameters::kRGBDProximityGlobalScanMap(), _createGlobalScanMap); Parameters::parse(parameters, Parameters::kMarkerPriorsVarianceLinear(), _markerPriorsLinearVariance); @@ -844,10 +856,10 @@ void Rtabmap::setInitialPose(const Transform & initialPose) if(!_memory->isIncremental()) { _lastLocalizationPose = initialPose; + _localizationCovariance = cv::Mat(); _lastLocalizationNodeId = 0; _odomCachePoses.clear(); _odomCacheConstraints.clear(); - _odomCorrectionAcc = std::vector(6,0); _mapCorrection.setIdentity(); _mapCorrectionBackup.setNull(); @@ -870,10 +882,10 @@ int Rtabmap::triggerNewMap() int mapId = -1; if(_memory) { + _localizationCovariance = cv::Mat(); _lastLocalizationNodeId = 0; _odomCachePoses.clear(); _odomCacheConstraints.clear(); - _odomCorrectionAcc = std::vector(6,0); _distanceTravelled = 0.0f; _distanceTravelledSinceLastLocalization = 0.0f; @@ -1053,10 +1065,10 @@ void Rtabmap::resetMemory() _mapCorrection.setIdentity(); _mapCorrectionBackup.setNull(); _lastLocalizationPose.setNull(); + _localizationCovariance = cv::Mat(); _lastLocalizationNodeId = 0; _odomCachePoses.clear(); _odomCacheConstraints.clear(); - _odomCorrectionAcc = std::vector(6,0); _distanceTravelled = 0.0f; _distanceTravelledSinceLastLocalization = 0.0f; _optimizeFromGraphEndChanged = false; @@ -1426,6 +1438,8 @@ bool Rtabmap::process( std::list signaturesRemoved; bool neighborLinkRefined = false; bool addedNewLandmark = false; + float distanceToClosestNodeInTheGraph = 0; + float angleToClosestNodeInTheGraph = 0; if(_rgbdSlamMode) { statistics_.addStatistic(Statistics::kMemoryOdometry_variance_lin(), odomCovariance.empty()?1.0f:(float)odomCovariance.at(0,0)); @@ -1607,6 +1621,28 @@ bool Rtabmap::process( newPose = _mapCorrection * signature->getPose(); } + // Get statistics about the closest node in the graph + if(!_memory->isIncremental()) + { + int closestNode = 0; + float sqrdDistance = 0.0f; + if(_optimizedPoses.begin()->first < 0) + { + std::map poses(_optimizedPoses.lower_bound(1), _optimizedPoses.end()); + closestNode = graph::findNearestNode(poses, newPose, &sqrdDistance); + } + else + { + closestNode = graph::findNearestNode(_optimizedPoses, newPose, &sqrdDistance); + } + if(closestNode>0 && sqrdDistance>0.0f) + { + distanceToClosestNodeInTheGraph = sqrt(sqrdDistance); + UDEBUG("Last localization pose = %s, closest node=%d (%f m)", newPose.prettyPrint().c_str(), closestNode, distanceToClosestNodeInTheGraph); + angleToClosestNodeInTheGraph = (newPose.inverse() * _optimizedPoses.at(closestNode)).getAngle(); + } + } + UDEBUG("Added pose %s (odom=%s)", newPose.prettyPrint().c_str(), signature->getPose().prettyPrint().c_str()); // Update Poses and Constraints _optimizedPoses.insert(std::make_pair(signature->id(), newPose)); @@ -1636,7 +1672,10 @@ bool Rtabmap::process( Link tmp = signature->getLinks().begin()->second.inverse(); - _distanceTravelled += tmp.transform().getNorm(); + if(!smallDisplacement) + { + _distanceTravelled += tmp.transform().getNorm(); + } // if the previous node is an intermediate node, remove it from the local graph if(_constraints.size() && @@ -1654,6 +1693,37 @@ bool Rtabmap::process( _constraints.insert(std::make_pair(tmp.from(), tmp)); } // Localization mode stuff + if( signature->getWeight() >= 0 && + !smallDisplacement && + odomCovariance.cols == 6 && + odomCovariance.rows == 6 && + odomCovariance.type() == CV_64FC1 && + odomCovariance.at(0,0) < 1) + { + if( _memory->isIncremental() && _localizationCovariance.empty()) + { + _localizationCovariance = cv::Mat::zeros(6,6,CV_64FC1); + } + if(_localizationCovariance.total() == 36) + { +#ifdef RTABMAP_MRPT + // Transform odometry covariance (which in base frame) into global frame + // "odometry error propagation law" + Eigen::Quaterniond rotation = _lastLocalizationPose.getQuaterniond(); + mrpt::poses::CPose3D pose = mrpt::poses::CPose3D::FromQuaternion(mrpt::math::CQuaternionDouble(rotation.w(), rotation.x(), rotation.y(), rotation.z())); + mrpt::math::CMatrixDouble66 gaussian; + gaussian.loadFromRawPointer((const double*)odomCovariance.data); + mrpt::poses::CPose3DPDFGaussian gaussianTransformed(mrpt::poses::CPose3D(), gaussian); + gaussianTransformed.changeCoordinatesReference(pose); + _localizationCovariance += cv::Mat(6,6,CV_64FC1, gaussianTransformed.cov.data()); +#else + // Assuming diagonal uniform covariance matrix! + // If variance is different for each axis, + // build rtabmap with MRPT to use approach above. + _localizationCovariance += odomCovariance; +#endif + } + } _lastLocalizationPose = newPose; // keep in cache the latest corrected pose if(!_memory->isIncremental() && signature->getWeight() >= 0) { @@ -1661,7 +1731,10 @@ bool Rtabmap::process( if(!_odomCachePoses.empty()) { float odomDistance = (_odomCachePoses.rbegin()->second.inverse() * signature->getPose()).getNorm(); - _distanceTravelled += odomDistance; + if(!smallDisplacement) + { + _distanceTravelled += odomDistance; + } while(!_odomCachePoses.empty() && (int)_odomCachePoses.size() > _maxOdomCacheSize) { @@ -2457,7 +2530,8 @@ bool Rtabmap::process( { if(_startNewMapOnLoopClosure && _memory->getWorkingMem().size()>=2 && // must have an old map (+1 virtual place) - graph::filterLinks(signature->getLinks(), Link::kSelfRefLink).size() == 0) // alone in new session) + _localizationCovariance.empty() && // if we didn't localize yet + graph::filterLinks(signature->getLinks(), Link::kSelfRefLink).size() == 0) // alone in new session { UINFO("Proximity detection by space disabled as if we force to have a global loop " "closure with previous map before doing proximity detections (%s=true).", @@ -2633,6 +2707,15 @@ bool Rtabmap::process( } } } + else if(!signature->hasLink(nearestId) && proximityFilteringRadius>0.0f) + { + UDEBUG("Skipping path %d as most likely ID %d is too far %f > %f (%s)", + iter->first.id, + nearestId, + _optimizedPoses.at(signature->id()).getDistance(_optimizedPoses.at(nearestId)), + proximityFilteringRadius, + Parameters::kRGBDProximityPathFilteringRadius().c_str()); + } } } @@ -2971,7 +3054,6 @@ bool Rtabmap::process( float maxAngularErrorRatio = 0.0f; double optimizationError = 0.0; int optimizationIterations = 0; - cv::Mat localizationCovariance; Transform previousMapCorrection; bool rejectedLandmark = false; bool delayedLocalization = false; @@ -3060,6 +3142,7 @@ bool Rtabmap::process( { constraints.insert(std::make_pair(iter->second.from(), iter->second)); } + cv::Mat priorInfMat = cv::Mat::eye(6,6, CV_64FC1)*_localizationPriorInf; for(std::multimap::iterator iter=constraints.begin(); iter!=constraints.end(); ++iter) { std::map::iterator iterPose = _optimizedPoses.find(iter->second.to()); @@ -3067,16 +3150,11 @@ bool Rtabmap::process( { poses.insert(*iterPose); // make the poses in the map fixed - constraints.insert(std::make_pair(iterPose->first, Link(iterPose->first, iterPose->first, Link::kPosePrior, iterPose->second, cv::Mat::eye(6,6, CV_64FC1)*1000000))); - UDEBUG("Constraint %d->%d (type=%s)", iterPose->first, iterPose->first, Link::typeName(Link::kPosePrior).c_str()); + constraints.insert(std::make_pair(iterPose->first, Link(iterPose->first, iterPose->first, Link::kPosePrior, iterPose->second, priorInfMat))); + UDEBUG("Constraint %d->%d: %s (type=%s, var=%f)", iterPose->first, iterPose->first, iterPose->second.prettyPrint().c_str(), Link::typeName(Link::kPosePrior).c_str(), 1./_localizationPriorInf); } - UDEBUG("Constraint %d->%d (type=%s, var = %f %f)", iter->second.from(), iter->second.to(), iter->second.typeName().c_str(), iter->second.transVariance(), iter->second.rotVariance()); + UDEBUG("Constraint %d->%d: %s (type=%s, var = %f %f)", iter->second.from(), iter->second.to(), iter->second.transform().prettyPrint().c_str(), iter->second.typeName().c_str(), iter->second.transVariance(), iter->second.rotVariance()); } - for(std::map::iterator iter=poses.begin(); iter!=poses.end(); ++iter) - { - UDEBUG("Pose %d %s", iter->first, iter->second.prettyPrint().c_str()); - } - std::map posesOut; std::multimap edgeConstraintsOut; @@ -3084,8 +3162,25 @@ bool Rtabmap::process( UDEBUG("priorsIgnored was %s", priorsIgnored?"true":"false"); _graphOptimizer->setPriorsIgnored(false); //temporary set false to use priors above to fix nodes of the map // If slam2d: get connected graph while keeping original roll,pitch,z values. - _graphOptimizer->getConnectedGraph(signature->id(), poses, constraints, posesOut, edgeConstraintsOut, !_graphOptimizer->isSlam2d()); - std::map optPoses = _graphOptimizer->optimize(poses.begin()->first, posesOut, edgeConstraintsOut); + _graphOptimizer->getConnectedGraph(signature->id(), poses, constraints, posesOut, edgeConstraintsOut); + if(ULogger::level() == ULogger::kDebug) + { + for(std::map::iterator iter=posesOut.begin(); iter!=posesOut.end(); ++iter) + { + UDEBUG("Pose %d %s", iter->first, iter->second.prettyPrint().c_str()); + } + } + cv::Mat locOptCovariance; + std::map optPoses; + if(!posesOut.empty() && + posesOut.begin()->first < _odomCachePoses.begin()->first) + { + optPoses = _graphOptimizer->optimize(posesOut.begin()->first, posesOut, edgeConstraintsOut, locOptCovariance); + } + else + { + UERROR("Invalid localization constraints"); + } _graphOptimizer->setPriorsIgnored(priorsIgnored); // set back for(std::map::iterator iter=optPoses.begin(); iter!=optPoses.end(); ++iter) { @@ -3097,7 +3192,7 @@ bool Rtabmap::process( UWARN("Optimization failed, rejecting localization!"); rejectLocalization = true; } - else if(_optimizationMaxError > 0.0f) + else { UINFO("Compute max graph errors..."); const Link * maxLinearLink = 0; @@ -3112,10 +3207,10 @@ bool Rtabmap::process( &maxLinearLink, &maxAngularLink, _graphOptimizer->isSlam2d()); - if(maxLinearLink == 0 && maxAngularLink==0 && _maxOdomCacheSize>0) + if(maxLinearLink == 0 && maxAngularLink==0) { - UWARN("Could not compute graph errors! Wrong loop closures could be accepted!"); - optPoses = posesOut; + UWARN("Could not compute graph errors! Rejecting localization!"); + rejectLocalization = true; } if(maxLinearLink) @@ -3127,7 +3222,7 @@ bool Rtabmap::process( maxLinearLink->transVariance(), maxLinearError/sqrt(maxLinearLink->transVariance()), _optimizationMaxError); - if(maxLinearErrorRatio > _optimizationMaxError) + if(_optimizationMaxError > 0.0f && maxLinearErrorRatio > _optimizationMaxError) { UWARN("Rejecting localization (%d <-> %d) in this " "iteration because a wrong loop closure has been " @@ -3146,6 +3241,19 @@ bool Rtabmap::process( _optimizationMaxError); rejectLocalization = true; } + else if(_optimizationMaxError == 0.0f && maxLinearErrorRatio>100) + { + UERROR("Huge optimization error detected!" + "Linear error ratio of %f (edge %d->%d, type=%d, abs error=%f m, stddev=%f). You may consider " + "enabling \"%s\" to reject those bad optimizations by setting it to a non null value!", + maxLinearErrorRatio, + maxLinearLink->from(), + maxLinearLink->to(), + maxLinearLink->type(), + maxLinearError, + sqrt(maxLinearLink->transVariance()), + Parameters::kRGBDOptimizeMaxError().c_str()); + } } if(maxAngularLink) { @@ -3156,7 +3264,7 @@ bool Rtabmap::process( maxAngularLink->rotVariance(), maxAngularError/sqrt(maxAngularLink->rotVariance()), _optimizationMaxError); - if(maxAngularErrorRatio > _optimizationMaxError) + if(_optimizationMaxError > 0.0f && maxAngularErrorRatio > _optimizationMaxError) { UWARN("Rejecting localization (%d <-> %d) in this " "iteration because a wrong loop closure has been " @@ -3175,6 +3283,19 @@ bool Rtabmap::process( _optimizationMaxError); rejectLocalization = true; } + else if(_optimizationMaxError == 0.0f && maxAngularErrorRatio>100) + { + UERROR("Huge optimization error detected!" + "Angular error ratio of %f (edge %d->%d, type=%d, abs error=%f m, stddev=%f). You may consider " + "enabling \"%s\" to reject those bad optimizations by setting it to a non null value!", + maxAngularErrorRatio, + maxAngularLink->from(), + maxAngularLink->to(), + maxAngularLink->type(), + maxAngularError*180.0f/CV_PI, + sqrt(maxAngularLink->rotVariance()), + Parameters::kRGBDOptimizeMaxError().c_str()); + } } } @@ -3198,8 +3319,17 @@ bool Rtabmap::process( UDEBUG("priorsIgnored was %s", priorsIgnored?"true":"false"); _graphOptimizer->setPriorsIgnored(false); //temporary set false to use priors above to fix nodes of the map // If slam2d: get connected graph while keeping original roll,pitch,z values. - _graphOptimizer->getConnectedGraph(signature->id(), poses, constraints, posesOut, edgeConstraintsOut, !_graphOptimizer->isSlam2d()); - optPoses = _graphOptimizer->optimize(poses.begin()->first, posesOut, edgeConstraintsOut); + _graphOptimizer->getConnectedGraph(signature->id(), poses, constraints, posesOut, edgeConstraintsOut); + optPoses.clear(); + if(!posesOut.empty() && + posesOut.begin()->first < _odomCachePoses.begin()->first) + { + optPoses = _graphOptimizer->optimize(posesOut.begin()->first, posesOut, edgeConstraintsOut, locOptCovariance); + } + else + { + UERROR("Invalid localization constraints"); + } _graphOptimizer->setPriorsIgnored(priorsIgnored); // set back for(std::map::iterator iter=optPoses.begin(); iter!=optPoses.end(); ++iter) { @@ -3211,7 +3341,7 @@ bool Rtabmap::process( UWARN("Optimization failed, rejecting localization!"); rejectLocalization = true; } - else if(_optimizationMaxError > 0.0f) + else { UINFO("Compute max graph errors..."); const Link * maxLinearLink = 0; @@ -3226,10 +3356,10 @@ bool Rtabmap::process( &maxLinearLink, &maxAngularLink, _graphOptimizer->isSlam2d()); - if(maxLinearLink == 0 && maxAngularLink==0 && _maxOdomCacheSize>0) + if(maxLinearLink == 0 && maxAngularLink==0) { - UWARN("Could not compute graph errors! Wrong loop closures could be accepted!"); - optPoses = posesOut; + UWARN("Could not compute graph errors! Rejecting localization!"); + rejectLocalization = true; } if(maxLinearLink) @@ -3241,7 +3371,7 @@ bool Rtabmap::process( maxLinearLink->transVariance(), maxLinearError/sqrt(maxLinearLink->transVariance()), _optimizationMaxError); - if(maxLinearErrorRatio > _optimizationMaxError) + if(_optimizationMaxError > 0.0f && maxLinearErrorRatio > _optimizationMaxError) { UWARN("Rejecting localization (%d <-> %d) in this " "iteration because a wrong loop closure has been " @@ -3260,6 +3390,19 @@ bool Rtabmap::process( _optimizationMaxError); rejectLocalization = true; } + else if(_optimizationMaxError == 0.0f && maxLinearErrorRatio>100) + { + UERROR("Huge optimization error detected!" + "Linear error ratio of %f (edge %d->%d, type=%d, abs error=%f m, stddev=%f). You may consider " + "enabling \"%s\" to reject those bad optimizations by setting it to a non null value!", + maxLinearErrorRatio, + maxLinearLink->from(), + maxLinearLink->to(), + maxLinearLink->type(), + maxLinearError, + sqrt(maxLinearLink->transVariance()), + Parameters::kRGBDOptimizeMaxError().c_str()); + } } if(maxAngularLink) { @@ -3270,7 +3413,7 @@ bool Rtabmap::process( maxAngularLink->rotVariance(), maxAngularError/sqrt(maxAngularLink->rotVariance()), _optimizationMaxError); - if(maxAngularErrorRatio > _optimizationMaxError) + if(_optimizationMaxError > 0.0f && maxAngularErrorRatio > _optimizationMaxError) { UWARN("Rejecting localization (%d <-> %d) in this " "iteration because a wrong loop closure has been " @@ -3289,6 +3432,19 @@ bool Rtabmap::process( _optimizationMaxError); rejectLocalization = true; } + else if(_optimizationMaxError == 0.0f && maxAngularErrorRatio>100) + { + UERROR("Huge optimization error detected!" + "Angular error ratio of %f (edge %d->%d, type=%d, abs error=%f m, stddev=%f). You may consider " + "enabling \"%s\" to reject those bad optimizations by setting it to a non null value!", + maxAngularErrorRatio, + maxAngularLink->from(), + maxAngularLink->to(), + maxAngularLink->type(), + maxAngularError*180.0f/CV_PI, + sqrt(maxAngularLink->rotVariance()), + Parameters::kRGBDOptimizeMaxError().c_str()); + } } } } @@ -3334,16 +3490,26 @@ bool Rtabmap::process( Transform newOptPoseInv = optPoses.at(signature->id()).inverse(); for(std::multimap::iterator iter=localizationLinks.begin(); iter!=localizationLinks.end(); ++iter) { - Transform newT = newOptPoseInv * optPoses.at(iter->first); - UDEBUG("Adjusted localization link %d->%d after optimization", iter->second.from(), iter->second.to()); - UDEBUG("from %s", iter->second.transform().prettyPrint().c_str()); - UDEBUG(" to %s", newT.prettyPrint().c_str()); - iter->second.setTransform(newT); - - // Update link in the referred signatures - if(iter->first > 0) - _memory->updateLink(iter->second, false); - + if(!_localizationSmoothing) + { + // Add original link without optimization + UDEBUG("Adding new odom cache constraint %d->%d (%s)", + iter->second.from(), iter->second.to(), iter->second.transform().prettyPrint().c_str()); + } + else + { + // Adjust with optimized poses, this will smooth the localization + Transform newT = newOptPoseInv * optPoses.at(iter->first); + UDEBUG("Adjusted localization link %d->%d after optimization", iter->second.from(), iter->second.to()); + UDEBUG("from %s", iter->second.transform().prettyPrint().c_str()); + UDEBUG(" to %s", newT.prettyPrint().c_str()); + iter->second.setTransform(newT); + + // Update link in the referred signatures + if(iter->first > 0) + _memory->updateLink(iter->second, false); + } + _odomCacheConstraints.insert(std::make_pair(signature->id(), iter->second)); } @@ -3460,7 +3626,7 @@ bool Rtabmap::process( } _optimizedPoses.at(signature->id()) = newPose; } - localizationCovariance = localizationLinks.rbegin()->second.infMatrix().inv(); + _localizationCovariance = locOptCovariance.empty()?localizationLinks.rbegin()->second.infMatrix().inv():locOptCovariance; } else //delayed localization (wait for more than 1 link) { @@ -3515,7 +3681,6 @@ bool Rtabmap::process( rejectedLandmark = true; } else if(_memory->isIncremental() && - _optimizationMaxError > 0.0f && loopClosureLinksAdded.size() && optimizationIterations > 0 && constraints.size()) @@ -3541,7 +3706,7 @@ bool Rtabmap::process( if(maxLinearLink) { UINFO("Max optimization linear error = %f m (link %d->%d, var=%f, ratio error/std=%f)", maxLinearError, maxLinearLink->from(), maxLinearLink->to(), maxLinearLink->transVariance(), maxLinearError/sqrt(maxLinearLink->transVariance())); - if(maxLinearErrorRatio > _optimizationMaxError) + if(_optimizationMaxError > 0.0f && maxLinearErrorRatio > _optimizationMaxError) { UWARN("Rejecting all added loop closures (%d, first is %d <-> %d) in this " "iteration because a wrong loop closure has been " @@ -3561,11 +3726,24 @@ bool Rtabmap::process( _optimizationMaxError); reject = true; } + else if(_optimizationMaxError == 0.0f && maxLinearErrorRatio>100) + { + UERROR("Huge optimization error detected!" + "Linear error ratio of %f (edge %d->%d, type=%d, abs error=%f m, stddev=%f). You may consider " + "enabling \"%s\" to reject those bad optimizations by setting it to a non null value!", + maxLinearErrorRatio, + maxLinearLink->from(), + maxLinearLink->to(), + maxLinearLink->type(), + maxLinearError, + sqrt(maxLinearLink->transVariance()), + Parameters::kRGBDOptimizeMaxError().c_str()); + } } if(maxAngularLink) { UINFO("Max optimization angular error = %f deg (link %d->%d, var=%f, ratio error/std=%f)", maxAngularError*180.0f/CV_PI, maxAngularLink->from(), maxAngularLink->to(), maxAngularLink->rotVariance(), maxAngularError/sqrt(maxAngularLink->rotVariance())); - if(maxAngularErrorRatio > _optimizationMaxError) + if(_optimizationMaxError > 0.0f && maxAngularErrorRatio > _optimizationMaxError) { UWARN("Rejecting all added loop closures (%d, first is %d <-> %d) in this " "iteration because a wrong loop closure has been " @@ -3585,6 +3763,19 @@ bool Rtabmap::process( _optimizationMaxError); reject = true; } + else if(_optimizationMaxError == 0.0f && maxAngularErrorRatio>100) + { + UERROR("Huge optimization error detected!" + "Angular error ratio of %f (edge %d->%d, type=%d, abs error=%f m, stddev=%f). You may consider " + "enabling \"%s\" to reject those bad optimizations by setting it to a non null value!", + maxAngularErrorRatio, + maxAngularLink->from(), + maxAngularLink->to(), + maxAngularLink->type(), + maxAngularError*180.0f/CV_PI, + sqrt(maxAngularLink->rotVariance()), + Parameters::kRGBDOptimizeMaxError().c_str()); + } } if(reject) @@ -3607,7 +3798,7 @@ bool Rtabmap::process( UINFO("Updated local map (old size=%d, new size=%d)", (int)_optimizedPoses.size(), (int)poses.size()); _optimizedPoses = poses; _constraints = constraints; - localizationCovariance = covariance; + _localizationCovariance = covariance; } } @@ -3619,6 +3810,17 @@ bool Rtabmap::process( } previousMapCorrection = _mapCorrection; _mapCorrection = _optimizedPoses.at(signature->id()) * signature->getPose().inverse(); + // Update statistics about the closest node in the graph using the actual loop closure + if(!_memory->isIncremental() && !_lastLocalizationPose.isNull()) + { + int closestNode = _loopClosureHypothesis.first>0?_loopClosureHypothesis.first:lastProximitySpaceClosureId; + if(closestNode>0) + { + distanceToClosestNodeInTheGraph = _lastLocalizationPose.getDistance(_optimizedPoses.at(closestNode)); + UDEBUG("Last localization pose = %s, updated closest node=%d (%f m)", _lastLocalizationPose.prettyPrint().c_str(), closestNode, distanceToClosestNodeInTheGraph); + angleToClosestNodeInTheGraph = (_lastLocalizationPose.inverse() * _optimizedPoses.at(closestNode)).getAngle(); + } + } _lastLocalizationPose = _optimizedPoses.at(signature->id()); // update if(_mapCorrection.getNormSquared() > 0.1f && _optimizeFromGraphEnd) { @@ -3675,6 +3877,14 @@ bool Rtabmap::process( refWordsCount = (int)signature->getWords().size(); refUniqueWordsCount = (int)uUniqueKeys(signature->getWords()).size(); + if(_graphOptimizer->isSlam2d() && _localizationCovariance.total() == 36) + { + // set very small + _localizationCovariance.at(2,2) = Registration::COVARIANCE_LINEAR_EPSILON; + _localizationCovariance.at(3,3) = Registration::COVARIANCE_ANGULAR_EPSILON; + _localizationCovariance.at(4,4) = Registration::COVARIANCE_ANGULAR_EPSILON; + } + // Posterior is empty if a bad signature is detected float vpHypothesis = posterior.size()?posterior.at(Memory::kIdVirtual):0.0f; int loopId = _loopClosureHypothesis.first>0?_loopClosureHypothesis.first:lastProximitySpaceClosureId; @@ -3737,6 +3947,8 @@ bool Rtabmap::process( statistics_.addStatistic(Statistics::kLoopId(), loopId); statistics_.addStatistic(Statistics::kLoopMap_id(), (loopId>0 && sLoop)?sLoop->mapId():-1); + statistics_.addStatistic(Statistics::kLoopDistance_since_last_loc(), _distanceTravelledSinceLastLocalization); + float x,y,z,roll,pitch,yaw; if(_loopClosureHypothesis.first || lastProximitySpaceClosureId || (!rejectedLandmark && !landmarksDetected.empty())) { @@ -3760,7 +3972,7 @@ bool Rtabmap::process( statistics_.addStatistic(Statistics::kGtLocalization_angular_error(), error.getAngle(1,0,0)*180/M_PI); } } - statistics_.addStatistic(Statistics::kLoopDistance_since_last_loc(), _distanceTravelledSinceLastLocalization); + _distanceTravelledSinceLastLocalization = 0.0f; statistics_.addStatistic(Statistics::kLoopMapToOdom_norm(), _mapCorrection.getNorm()); @@ -3786,30 +3998,6 @@ bool Rtabmap::process( statistics_.addStatistic(Statistics::kLoopOdom_correction_roll(), roll*180.0f/M_PI); statistics_.addStatistic(Statistics::kLoopOdom_correction_pitch(), pitch*180.0f/M_PI); statistics_.addStatistic(Statistics::kLoopOdom_correction_yaw(), yaw*180.0f/M_PI); - - _odomCorrectionAcc[0]+=x; - _odomCorrectionAcc[1]+=y; - _odomCorrectionAcc[2]+=z; - _odomCorrectionAcc[3]+=roll; - _odomCorrectionAcc[4]+=pitch; - _odomCorrectionAcc[5]+=yaw; - Transform odomCorrectionAcc( - _odomCorrectionAcc[0], - _odomCorrectionAcc[1], - _odomCorrectionAcc[2], - _odomCorrectionAcc[3], - _odomCorrectionAcc[4], - _odomCorrectionAcc[5]); - - statistics_.addStatistic(Statistics::kLoopOdom_correction_acc_norm(), odomCorrectionAcc.getNorm()); - statistics_.addStatistic(Statistics::kLoopOdom_correction_acc_angle(), odomCorrectionAcc.getAngle()*180.0f/M_PI); - odomCorrectionAcc.getTranslationAndEulerAngles(x, y, z, roll, pitch, yaw); - statistics_.addStatistic(Statistics::kLoopOdom_correction_acc_x(), x); - statistics_.addStatistic(Statistics::kLoopOdom_correction_acc_y(), y); - statistics_.addStatistic(Statistics::kLoopOdom_correction_acc_z(), z); - statistics_.addStatistic(Statistics::kLoopOdom_correction_acc_roll(), roll*180.0f/M_PI); - statistics_.addStatistic(Statistics::kLoopOdom_correction_acc_pitch(), pitch*180.0f/M_PI); - statistics_.addStatistic(Statistics::kLoopOdom_correction_acc_yaw(), yaw*180.0f/M_PI); } } if(!_lastLocalizationPose.isNull() && !_lastLocalizationPose.isIdentity()) @@ -3822,11 +4010,21 @@ bool Rtabmap::process( statistics_.addStatistic(Statistics::kLoopMapToBase_pitch(), pitch*180.0f/M_PI); statistics_.addStatistic(Statistics::kLoopMapToBase_yaw(), yaw*180.0f/M_PI); UINFO("Localization pose = %s", _lastLocalizationPose.prettyPrint().c_str()); + + if(_localizationCovariance.total()==36) + { + double varLin = _graphOptimizer->isSlam2d()? + std::max(_localizationCovariance.at(0,0), _localizationCovariance.at(1,1)): + uMax3(_localizationCovariance.at(0,0), _localizationCovariance.at(1,1), _localizationCovariance.at(2,2)); + + statistics_.addStatistic(Statistics::kLoopMapToBase_lin_std(), sqrt(varLin)); + statistics_.addStatistic(Statistics::kLoopMapToBase_lin_var(), varLin); + } } statistics_.setMapCorrection(_mapCorrection); UINFO("Set map correction = %s", _mapCorrection.prettyPrint().c_str()); - statistics_.setLocalizationCovariance(localizationCovariance); + statistics_.setLocalizationCovariance(_localizationCovariance); // timings... statistics_.addStatistic(Statistics::kTimingMemory_update(), timeMemoryUpdate*1000); @@ -3860,6 +4058,12 @@ bool Rtabmap::process( statistics_.addStatistic(Statistics::kMemoryFast_movement(), tooFastMovement?1.0f:0); statistics_.addStatistic(Statistics::kMemoryNew_landmark(), addedNewLandmark?1.0f:0); + if(distanceToClosestNodeInTheGraph>0.0) + { + statistics_.addStatistic(Statistics::kMemoryClosest_node_distance(), distanceToClosestNodeInTheGraph); + statistics_.addStatistic(Statistics::kMemoryClosest_node_angle(), angleToClosestNodeInTheGraph); + } + if(_publishRAMUsage) { UTimer ramTimer; @@ -3903,15 +4107,16 @@ bool Rtabmap::process( ULOGGER_INFO("Time creating stats = %f...", timeStatsCreation); } - Signature lastSignatureData(signature->id()); + Signature lastSignatureData = *signature; Transform lastSignatureLocalizedPose; if(_optimizedPoses.find(signature->id()) != _optimizedPoses.end()) { lastSignatureLocalizedPose = _optimizedPoses.at(signature->id()); } - if(_publishLastSignatureData) + if(!_publishLastSignatureData) { - lastSignatureData = *signature; + lastSignatureData.sensorData().clearCompressedData(); + lastSignatureData.sensorData().clearRawData(); } if(!_rawDataKept) { @@ -4194,96 +4399,73 @@ bool Rtabmap::process( poses = _optimizedPoses; constraints = _constraints; } - UDEBUG(""); - if(_publishLastSignatureData) - { - UINFO("Adding data %d [%d] (rgb/left=%d depth/right=%d)", lastSignatureData.id(), lastSignatureData.mapId(), lastSignatureData.sensorData().imageRaw().empty()?0:1, lastSignatureData.sensorData().depthOrRightRaw().empty()?0:1); + UINFO("Adding data %d [%d] (rgb/left=%d depth/right=%d)", lastSignatureData.id(), lastSignatureData.mapId(), lastSignatureData.sensorData().imageRaw().empty()?0:1, lastSignatureData.sensorData().depthOrRightRaw().empty()?0:1); statistics_.addSignatureData(lastSignatureData); - if(_nodesToRepublish.size()) - { - std::multimap missingIds; + if(_nodesToRepublish.size()) + { + std::multimap missingIds; - // priority to loopId - int tmpId = loopId>0?loopId:_highestHypothesis.first; - if(tmpId>0 && _nodesToRepublish.find(tmpId) != _nodesToRepublish.end()) - { - missingIds.insert(std::make_pair(-1, tmpId)); - } + // priority to loopId + int tmpId = loopId>0?loopId:_highestHypothesis.first; + if(tmpId>0 && _nodesToRepublish.find(tmpId) != _nodesToRepublish.end()) + { + missingIds.insert(std::make_pair(-1, tmpId)); + } - if(!_lastLocalizationPose.isNull()) + if(!_lastLocalizationPose.isNull()) + { + // Republish data from closest nodes of the current localization + std::map nodesOnly(_optimizedPoses.lower_bound(1), _optimizedPoses.end()); + int id = rtabmap::graph::findNearestNode(nodesOnly, _lastLocalizationPose); + if(id>0) { - // Republish data from closest nodes of the current localization - std::map nodesOnly(_optimizedPoses.lower_bound(1), _optimizedPoses.end()); - int id = rtabmap::graph::findNearestNode(nodesOnly, _lastLocalizationPose); - if(id>0) + std::map ids = _memory->getNeighborsId(id, 0, 0, true, false, true); + for(std::map::iterator iter=ids.begin(); iter!=ids.end(); ++iter) { - std::map ids = _memory->getNeighborsId(id, 0, 0, true, false, true); - for(std::map::iterator iter=ids.begin(); iter!=ids.end(); ++iter) + if(iter->first != loopId && + _nodesToRepublish.find(iter->first) != _nodesToRepublish.end()) { - if(iter->first != loopId && - _nodesToRepublish.find(iter->first) != _nodesToRepublish.end()) - { - missingIds.insert(std::make_pair(iter->second, iter->first)); - } + missingIds.insert(std::make_pair(iter->second, iter->first)); } + } - if(_nodesToRepublish.size() != missingIds.size()) + if(_nodesToRepublish.size() != missingIds.size()) + { + // remove requested nodes not anymore in the graph + for(std::set::iterator iter=_nodesToRepublish.begin(); iter!=_nodesToRepublish.end();) { - // remove requested nodes not anymore in the graph - for(std::set::iterator iter=_nodesToRepublish.begin(); iter!=_nodesToRepublish.end();) + if(ids.find(*iter) == ids.end()) { - if(ids.find(*iter) == ids.end()) - { - iter = _nodesToRepublish.erase(iter); - } - else - { - ++iter; - } + iter = _nodesToRepublish.erase(iter); + } + else + { + ++iter; } } } } + } - int loaded = 0; - std::stringstream stream; - for(std::multimap::iterator iter=missingIds.begin(); iter!=missingIds.end() && loaded<(int)_maxRepublished; ++iter) - { - statistics_.addSignatureData(getSignatureCopy(iter->second, true, true, true, true, true, true)); - _nodesToRepublish.erase(iter->second); - ++loaded; - stream << iter->second << " "; - } - if(loaded) - { - UWARN("Republishing data of requested node(s) %s(%s=%d)", - stream.str().c_str(), - Parameters::kRtabmapMaxRepublished().c_str(), - _maxRepublished); - } + int loaded = 0; + std::stringstream stream; + for(std::multimap::iterator iter=missingIds.begin(); iter!=missingIds.end() && loaded<(int)_maxRepublished; ++iter) + { + statistics_.addSignatureData(getSignatureCopy(iter->second, true, true, true, true, true, true)); + _nodesToRepublish.erase(iter->second); + ++loaded; + stream << iter->second << " "; } - } - else - { - // only copy node info - Signature nodeInfo( - lastSignatureData.id(), - lastSignatureData.mapId(), - lastSignatureData.getWeight(), - lastSignatureData.getStamp(), - lastSignatureData.getLabel(), - lastSignatureData.getPose(), - lastSignatureData.getGroundTruthPose()); - const std::vector & v = lastSignatureData.getVelocity(); - if(v.size() == 6) + if(loaded) { - nodeInfo.setVelocity(v[0], v[1], v[2], v[3], v[4], v[5]); + UWARN("Republishing data of requested node(s) %s(%s=%d)", + stream.str().c_str(), + Parameters::kRtabmapMaxRepublished().c_str(), + _maxRepublished); } - nodeInfo.sensorData().setGPS(lastSignatureData.sensorData().gps()); - nodeInfo.sensorData().setEnvSensors(lastSignatureData.sensorData().envSensors()); - statistics_.addSignatureData(nodeInfo); } + UDEBUG(""); localGraphSize = (int)poses.size(); if(!lastSignatureLocalizedPose.isNull()) @@ -5427,107 +5609,131 @@ int Rtabmap::detectMoreLoopClosures( if(!t.isNull()) { bool updateConstraints = true; - if(_optimizationMaxError > 0.0f) - { - //optimize the graph to see if the new constraint is globally valid - int fromId = from; - int mapId = signatures.at(from).mapId(); - // use first node of the map containing from - for(std::map::iterator ster=signatures.begin(); ster!=signatures.end(); ++ster) + //optimize the graph to see if the new constraint is globally valid + + int fromId = from; + int mapId = signatures.at(from).mapId(); + // use first node of the map containing from + for(std::map::iterator ster=signatures.begin(); ster!=signatures.end(); ++ster) + { + if(ster->second.mapId() == mapId) { - if(ster->second.mapId() == mapId) - { - fromId = ster->first; - break; - } + fromId = ster->first; + break; } - std::multimap linksIn = links; - linksIn.insert(std::make_pair(from, Link(from, to, Link::kUserClosure, t, getInformation(info.covariance)))); - const Link * maxLinearLink = 0; - const Link * maxAngularLink = 0; - float maxLinearError = 0.0f; - float maxAngularError = 0.0f; - float maxLinearErrorRatio = 0.0f; - float maxAngularErrorRatio = 0.0f; - std::map optimizedPoses; - std::multimap links; - UASSERT(poses.find(fromId) != poses.end()); - UASSERT_MSG(poses.find(from) != poses.end(), uFormat("id=%d poses=%d links=%d", from, (int)poses.size(), (int)links.size()).c_str()); - UASSERT_MSG(poses.find(to) != poses.end(), uFormat("id=%d poses=%d links=%d", to, (int)poses.size(), (int)links.size()).c_str()); - _graphOptimizer->getConnectedGraph(fromId, poses, linksIn, optimizedPoses, links); - UASSERT(optimizedPoses.find(fromId) != optimizedPoses.end()); - UASSERT_MSG(optimizedPoses.find(from) != optimizedPoses.end(), uFormat("id=%d poses=%d links=%d", from, (int)optimizedPoses.size(), (int)links.size()).c_str()); - UASSERT_MSG(optimizedPoses.find(to) != optimizedPoses.end(), uFormat("id=%d poses=%d links=%d", to, (int)optimizedPoses.size(), (int)links.size()).c_str()); - UASSERT(graph::findLink(links, from, to) != links.end()); - optimizedPoses = _graphOptimizer->optimize(fromId, optimizedPoses, links); - std::string msg; - if(optimizedPoses.size()) + } + std::multimap linksIn = links; + linksIn.insert(std::make_pair(from, Link(from, to, Link::kUserClosure, t, getInformation(info.covariance)))); + const Link * maxLinearLink = 0; + const Link * maxAngularLink = 0; + float maxLinearError = 0.0f; + float maxAngularError = 0.0f; + float maxLinearErrorRatio = 0.0f; + float maxAngularErrorRatio = 0.0f; + std::map optimizedPoses; + std::multimap links; + UASSERT(poses.find(fromId) != poses.end()); + UASSERT_MSG(poses.find(from) != poses.end(), uFormat("id=%d poses=%d links=%d", from, (int)poses.size(), (int)links.size()).c_str()); + UASSERT_MSG(poses.find(to) != poses.end(), uFormat("id=%d poses=%d links=%d", to, (int)poses.size(), (int)links.size()).c_str()); + _graphOptimizer->getConnectedGraph(fromId, poses, linksIn, optimizedPoses, links); + UASSERT(optimizedPoses.find(fromId) != optimizedPoses.end()); + UASSERT_MSG(optimizedPoses.find(from) != optimizedPoses.end(), uFormat("id=%d poses=%d links=%d", from, (int)optimizedPoses.size(), (int)links.size()).c_str()); + UASSERT_MSG(optimizedPoses.find(to) != optimizedPoses.end(), uFormat("id=%d poses=%d links=%d", to, (int)optimizedPoses.size(), (int)links.size()).c_str()); + UASSERT(graph::findLink(links, from, to) != links.end()); + optimizedPoses = _graphOptimizer->optimize(fromId, optimizedPoses, links); + std::string msg; + if(optimizedPoses.size()) + { + graph::computeMaxGraphErrors( + optimizedPoses, + links, + maxLinearErrorRatio, + maxAngularErrorRatio, + maxLinearError, + maxAngularError, + &maxLinearLink, + &maxAngularLink); + if(maxLinearLink) { - graph::computeMaxGraphErrors( - optimizedPoses, - links, - maxLinearErrorRatio, - maxAngularErrorRatio, - maxLinearError, - maxAngularError, - &maxLinearLink, - &maxAngularLink); - if(maxLinearLink) + UINFO("Max optimization linear error = %f m (link %d->%d)", maxLinearError, maxLinearLink->from(), maxLinearLink->to()); + if(_optimizationMaxError > 0.0f && maxLinearErrorRatio > _optimizationMaxError) { - UINFO("Max optimization linear error = %f m (link %d->%d)", maxLinearError, maxLinearLink->from(), maxLinearLink->to()); - if(maxLinearErrorRatio > _optimizationMaxError) - { - msg = uFormat("Rejecting edge %d->%d because " - "graph error is too large after optimization (%f m for edge %d->%d with ratio %f > std=%f m). " - "\"%s\" is %f.", - from, - to, - maxLinearError, - maxLinearLink->from(), - maxLinearLink->to(), - maxLinearErrorRatio, - sqrt(maxLinearLink->transVariance()), - Parameters::kRGBDOptimizeMaxError().c_str(), - _optimizationMaxError); - } + msg = uFormat("Rejecting edge %d->%d because " + "graph error is too large after optimization (%f m for edge %d->%d with ratio %f > std=%f m). " + "\"%s\" is %f.", + from, + to, + maxLinearError, + maxLinearLink->from(), + maxLinearLink->to(), + maxLinearErrorRatio, + sqrt(maxLinearLink->transVariance()), + Parameters::kRGBDOptimizeMaxError().c_str(), + _optimizationMaxError); } - else if(maxAngularLink) + else if(_optimizationMaxError == 0.0f && maxLinearErrorRatio>100) { - UINFO("Max optimization angular error = %f deg (link %d->%d)", maxAngularError*180.0f/M_PI, maxAngularLink->from(), maxAngularLink->to()); - if(maxAngularErrorRatio > _optimizationMaxError) - { - msg = uFormat("Rejecting edge %d->%d because " - "graph error is too large after optimization (%f deg for edge %d->%d with ratio %f > std=%f deg). " - "\"%s\" is %f m.", - from, - to, - maxAngularError*180.0f/M_PI, - maxAngularLink->from(), - maxAngularLink->to(), - maxAngularErrorRatio, - sqrt(maxAngularLink->rotVariance()), - Parameters::kRGBDOptimizeMaxError().c_str(), - _optimizationMaxError); - } + UERROR("Huge optimization error detected!" + "Linear error ratio of %f (edge %d->%d, type=%d, abs error=%f m, stddev=%f). You may consider " + "enabling \"%s\" to reject those bad optimizations by setting it to a non null value!", + maxLinearErrorRatio, + maxLinearLink->from(), + maxLinearLink->to(), + maxLinearLink->type(), + maxLinearError, + sqrt(maxLinearLink->transVariance()), + Parameters::kRGBDOptimizeMaxError().c_str()); } } - else - { - msg = uFormat("Rejecting edge %d->%d because graph optimization has failed!", - from, - to); - } - if(!msg.empty()) + else if(maxAngularLink) { - UWARN("%s", msg.c_str()); - updateConstraints = false; - } - else - { - poses = optimizedPoses; + UINFO("Max optimization angular error = %f deg (link %d->%d)", maxAngularError*180.0f/M_PI, maxAngularLink->from(), maxAngularLink->to()); + if(_optimizationMaxError > 0.0f && maxAngularErrorRatio > _optimizationMaxError) + { + msg = uFormat("Rejecting edge %d->%d because " + "graph error is too large after optimization (%f deg for edge %d->%d with ratio %f > std=%f deg). " + "\"%s\" is %f m.", + from, + to, + maxAngularError*180.0f/M_PI, + maxAngularLink->from(), + maxAngularLink->to(), + maxAngularErrorRatio, + sqrt(maxAngularLink->rotVariance()), + Parameters::kRGBDOptimizeMaxError().c_str(), + _optimizationMaxError); + } + else if(_optimizationMaxError == 0.0f && maxAngularErrorRatio>100) + { + UERROR("Huge optimization error detected!" + "Angular error ratio of %f (edge %d->%d, type=%d, abs error=%f m, stddev=%f). You may consider " + "enabling \"%s\" to reject those bad optimizations by setting it to a non null value!", + maxAngularErrorRatio, + maxAngularLink->from(), + maxAngularLink->to(), + maxAngularLink->type(), + maxAngularError*180.0f/CV_PI, + sqrt(maxAngularLink->rotVariance()), + Parameters::kRGBDOptimizeMaxError().c_str()); + } } } + else + { + msg = uFormat("Rejecting edge %d->%d because graph optimization has failed!", + from, + to); + } + if(!msg.empty()) + { + UWARN("%s", msg.c_str()); + updateConstraints = false; + } + else + { + poses = optimizedPoses; + } if(updateConstraints) { @@ -5809,7 +6015,7 @@ bool Rtabmap::addLink(const Link & link) { msg = uFormat("Rejecting edge %d->%d because graph optimization has failed!", link.from(), link.to()); } - else if(_optimizationMaxError > 0.0f) + else { float maxLinearError = 0.0f; float maxLinearErrorRatio = 0.0f; @@ -5830,7 +6036,7 @@ bool Rtabmap::addLink(const Link & link) if(maxLinearLink) { UINFO("Max optimization linear error = %f m (link %d->%d)", maxLinearError, maxLinearLink->from(), maxLinearLink->to()); - if(maxLinearErrorRatio > _optimizationMaxError) + if(_optimizationMaxError > 0.0f && maxLinearErrorRatio > _optimizationMaxError) { msg = uFormat("Rejecting edge %d->%d because " "graph error is too large after optimization (%f m for edge %d->%d with ratio %f > std=%f m). " @@ -5845,11 +6051,24 @@ bool Rtabmap::addLink(const Link & link) Parameters::kRGBDOptimizeMaxError().c_str(), _optimizationMaxError); } + else if(_optimizationMaxError == 0.0f && maxLinearErrorRatio>100) + { + UERROR("Huge optimization error detected!" + "Linear error ratio of %f (edge %d->%d, type=%d, abs error=%f m, stddev=%f). You may consider " + "enabling \"%s\" to reject those bad optimizations by setting it to a non null value!", + maxLinearErrorRatio, + maxLinearLink->from(), + maxLinearLink->to(), + maxLinearLink->type(), + maxLinearError, + sqrt(maxLinearLink->transVariance()), + Parameters::kRGBDOptimizeMaxError().c_str()); + } } else if(maxAngularLink) { UINFO("Max optimization angular error = %f deg (link %d->%d)", maxAngularError*180.0f/M_PI, maxAngularLink->from(), maxAngularLink->to()); - if(maxAngularErrorRatio > _optimizationMaxError) + if(_optimizationMaxError > 0.0f && maxAngularErrorRatio > _optimizationMaxError) { msg = uFormat("Rejecting edge %d->%d because " "graph error is too large after optimization (%f deg for edge %d->%d with ratio %f > std=%f deg). " @@ -5864,6 +6083,19 @@ bool Rtabmap::addLink(const Link & link) Parameters::kRGBDOptimizeMaxError().c_str(), _optimizationMaxError); } + else if(_optimizationMaxError == 0.0f && maxAngularErrorRatio>100) + { + UERROR("Huge optimization error detected!" + "Angular error ratio of %f (edge %d->%d, type=%d, abs error=%f m, stddev=%f). You may consider " + "enabling \"%s\" to reject those bad optimizations by setting it to a non null value!", + maxAngularErrorRatio, + maxAngularLink->from(), + maxAngularLink->to(), + maxAngularLink->type(), + maxAngularError*180.0f/CV_PI, + sqrt(maxAngularLink->rotVariance()), + Parameters::kRGBDOptimizeMaxError().c_str()); + } } } if(!msg.empty()) @@ -5968,7 +6200,7 @@ bool Rtabmap::addLink(const Link & link) UWARN("Optimization failed, rejecting localization!"); rejectLocalization = true; } - else if(_optimizationMaxError > 0.0f) + else { UINFO("Compute max graph errors..."); float maxLinearError = 0.0f; @@ -5995,7 +6227,7 @@ bool Rtabmap::addLink(const Link & link) if(maxLinearLink) { UINFO("Max optimization linear error = %f m (link %d->%d, var=%f, ratio error/std=%f)", maxLinearError, maxLinearLink->from(), maxLinearLink->to(), maxLinearLink->transVariance(), maxLinearError/sqrt(maxLinearLink->transVariance())); - if(maxLinearErrorRatio > _optimizationMaxError) + if(_optimizationMaxError > 0.0f && maxLinearErrorRatio > _optimizationMaxError) { UWARN("Rejecting localization (%d <-> %d) in this " "iteration because a wrong loop closure has been " @@ -6014,11 +6246,24 @@ bool Rtabmap::addLink(const Link & link) _optimizationMaxError); rejectLocalization = true; } + else if(_optimizationMaxError == 0.0f && maxLinearErrorRatio>100) + { + UERROR("Huge optimization error detected!" + "Linear error ratio of %f (edge %d->%d, type=%d, abs error=%f m, stddev=%f). You may consider " + "enabling \"%s\" to reject those bad optimizations by setting it to a non null value!", + maxLinearErrorRatio, + maxLinearLink->from(), + maxLinearLink->to(), + maxLinearLink->type(), + maxLinearError, + sqrt(maxLinearLink->transVariance()), + Parameters::kRGBDOptimizeMaxError().c_str()); + } } if(maxAngularLink) { UINFO("Max optimization angular error = %f deg (link %d->%d, var=%f, ratio error/std=%f)", maxAngularError*180.0f/CV_PI, maxAngularLink->from(), maxAngularLink->to(), maxAngularLink->rotVariance(), maxAngularError/sqrt(maxAngularLink->rotVariance())); - if(maxAngularErrorRatio > _optimizationMaxError) + if(_optimizationMaxError > 0.0f && maxAngularErrorRatio > _optimizationMaxError) { UWARN("Rejecting localization (%d <-> %d) in this " "iteration because a wrong loop closure has been " @@ -6037,6 +6282,19 @@ bool Rtabmap::addLink(const Link & link) _optimizationMaxError); rejectLocalization = true; } + else if(_optimizationMaxError == 0.0f && maxAngularErrorRatio>100) + { + UERROR("Huge optimization error detected!" + "Angular error ratio of %f (edge %d->%d, type=%d, abs error=%f m, stddev=%f). You may consider " + "enabling \"%s\" to reject those bad optimizations by setting it to a non null value!", + maxAngularErrorRatio, + maxAngularLink->from(), + maxAngularLink->to(), + maxAngularLink->type(), + maxAngularError*180.0f/CV_PI, + sqrt(maxAngularLink->rotVariance()), + Parameters::kRGBDOptimizeMaxError().c_str()); + } } } diff --git a/corelib/src/SensorCaptureThread.cpp b/corelib/src/SensorCaptureThread.cpp index b234e4aff0..de421da0e9 100644 --- a/corelib/src/SensorCaptureThread.cpp +++ b/corelib/src/SensorCaptureThread.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 @@ -57,6 +59,7 @@ SensorCaptureThread::SensorCaptureThread(SensorCapture * camera, const Parameter _stereoExposureCompensation(false), _colorOnly(false), _imageDecimation(1), + _histogramMethod(0), _stereoToDepth(false), _scanFromDepth(false), _scanDownsampleStep(1), @@ -72,7 +75,9 @@ SensorCaptureThread::SensorCaptureThread(SensorCapture * camera, const Parameter _bilateralSigmaS(10), _bilateralSigmaR(0.1), _imuFilter(0), - _imuBaseFrameConversion(false) + _imuBaseFrameConversion(false), + _featureDetector(0), + _depthAsMask(Parameters::defaultVisDepthAsMask()) { UASSERT(_camera != 0); } @@ -96,6 +101,7 @@ SensorCaptureThread::SensorCaptureThread( _stereoExposureCompensation(false), _colorOnly(false), _imageDecimation(1), + _histogramMethod(0), _stereoToDepth(false), _scanFromDepth(false), _scanDownsampleStep(1), @@ -111,7 +117,9 @@ SensorCaptureThread::SensorCaptureThread( _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()); @@ -134,6 +142,7 @@ SensorCaptureThread::SensorCaptureThread( _stereoExposureCompensation(false), _colorOnly(false), _imageDecimation(1), + _histogramMethod(0), _stereoToDepth(false), _scanFromDepth(false), _scanDownsampleStep(1), @@ -149,7 +158,9 @@ SensorCaptureThread::SensorCaptureThread( _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"); @@ -163,6 +174,7 @@ SensorCaptureThread::~SensorCaptureThread() delete _distortionModel; delete _stereoDense; delete _imuFilter; + delete _featureDetector; } void SensorCaptureThread::setImageRate(float imageRate) @@ -214,6 +226,30 @@ void SensorCaptureThread::disableIMUFiltering() _imuFilter = 0; } +void SensorCaptureThread::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 SensorCaptureThread::disableFeatureDetection() +{ + delete _featureDetector; + _featureDetector = 0; +} + void SensorCaptureThread::setScanParameters( bool fromDepth, int downsampleStep, @@ -455,9 +491,21 @@ void SensorCaptureThread::postUpdate(SensorData * dataPtr, SensorCaptureInfo * i { data.setStereoImage(image, depthOrRight, stereoModels); } + + std::vector kpts = data.keypoints(); + double log2value = log(double(_imageDecimation))/log(2.0); + for(unsigned int i=0; itimeImageDecimation = timer.ticks(); } + if(_mirroring && !data.imageRaw().empty() && data.cameraModels().size()>=1) { if(data.cameraModels().size() == 1) @@ -493,6 +541,50 @@ void SensorCaptureThread::postUpdate(SensorData * dataPtr, SensorCaptureInfo * i } } + if(_histogramMethod && !data.imageRaw().empty()) + { + if(data.imageRaw().type() == CV_8UC1) + { + UDEBUG(""); + UTimer timer; + cv::Mat image; + if(_histogramMethod == 1) + { + cv::equalizeHist(data.imageRaw(), image); + if(!data.depthRaw().empty()) + { + data.setRGBDImage(image, data.depthRaw(), data.cameraModels()); + } + else if(!data.rightRaw().empty()) + { + cv::Mat right; + cv::equalizeHist(data.rightRaw(), right); + data.setStereoImage(image, right, data.stereoCameraModels()[0]); + } + } + else if(_histogramMethod == 2) + { + cv::Ptr clahe = cv::createCLAHE(3.0); + clahe->apply(data.imageRaw(), image); + if(!data.depthRaw().empty()) + { + data.setRGBDImage(image, data.depthRaw(), data.cameraModels()); + } + else if(!data.rightRaw().empty()) + { + cv::Mat right; + clahe->apply(data.rightRaw(), right); + data.setStereoImage(image, right, data.stereoCameraModels()[0]); + } + } + if(info) info->timeHistogramEqualization = timer.ticks(); + } + else + { + UWARN("Histogram equalization only supports grayscale images..."); + } + } + if(_stereoExposureCompensation && !data.imageRaw().empty() && !data.rightRaw().empty()) { if(data.stereoCameraModels().size()==1) @@ -673,6 +765,50 @@ void SensorCaptureThread::postUpdate(SensorData * dataPtr, SensorCaptureInfo * i 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/SensorData.cpp b/corelib/src/SensorData.cpp index 0c0fddbbc9..147e854d69 100644 --- a/corelib/src/SensorData.cpp +++ b/corelib/src/SensorData.cpp @@ -810,23 +810,23 @@ void SensorData::setFeatures(const std::vector & keypoints, const unsigned long SensorData::getMemoryUsed() const // Return memory usage in Bytes { return sizeof(SensorData) + - _imageCompressed.total()*_imageCompressed.elemSize() + - _imageRaw.total()*_imageRaw.elemSize() + - _depthOrRightCompressed.total()*_depthOrRightCompressed.elemSize() + - _depthOrRightRaw.total()*_depthOrRightRaw.elemSize() + - _userDataCompressed.total()*_userDataCompressed.elemSize() + - _userDataRaw.total()*_userDataRaw.elemSize() + - _laserScanCompressed.data().total()*_laserScanCompressed.data().elemSize() + - _laserScanRaw.data().total()*_laserScanRaw.data().elemSize() + - _groundCellsCompressed.total()*_groundCellsCompressed.elemSize() + - _groundCellsRaw.total()*_groundCellsRaw.elemSize() + - _obstacleCellsCompressed.total()*_obstacleCellsCompressed.elemSize() + - _obstacleCellsRaw.total()*_obstacleCellsRaw.elemSize()+ - _emptyCellsCompressed.total()*_emptyCellsCompressed.elemSize() + - _emptyCellsRaw.total()*_emptyCellsRaw.elemSize()+ + (_imageCompressed.empty()?0:_imageCompressed.total()*_imageCompressed.elemSize()) + + (_imageRaw.empty()?0:_imageRaw.total()*_imageRaw.elemSize()) + + (_depthOrRightCompressed.empty()?0:_depthOrRightCompressed.total()*_depthOrRightCompressed.elemSize()) + + (_depthOrRightRaw.empty()?0:_depthOrRightRaw.total()*_depthOrRightRaw.elemSize()) + + (_userDataCompressed.empty()?0:_userDataCompressed.total()*_userDataCompressed.elemSize()) + + (_userDataRaw.empty()?0:_userDataRaw.total()*_userDataRaw.elemSize()) + + (_laserScanCompressed.empty()?0:_laserScanCompressed.data().total()*_laserScanCompressed.data().elemSize()) + + (_laserScanRaw.empty()?0:_laserScanRaw.data().total()*_laserScanRaw.data().elemSize()) + + (_groundCellsCompressed.empty()?0:_groundCellsCompressed.total()*_groundCellsCompressed.elemSize()) + + (_groundCellsRaw.empty()?0:_groundCellsRaw.total()*_groundCellsRaw.elemSize()) + + (_obstacleCellsCompressed.empty()?0:_obstacleCellsCompressed.total()*_obstacleCellsCompressed.elemSize()) + + (_obstacleCellsRaw.empty()?0:_obstacleCellsRaw.total()*_obstacleCellsRaw.elemSize())+ + (_emptyCellsCompressed.empty()?0:_emptyCellsCompressed.total()*_emptyCellsCompressed.elemSize()) + + (_emptyCellsRaw.empty()?0:_emptyCellsRaw.total()*_emptyCellsRaw.elemSize())+ _keypoints.size() * sizeof(cv::KeyPoint) + _keypoints3D.size() * sizeof(cv::Point3f) + - _descriptors.total()*_descriptors.elemSize(); + (_descriptors.empty()?0:_descriptors.total()*_descriptors.elemSize()); } void SensorData::clearCompressedData(bool images, bool scan, bool userData) diff --git a/corelib/src/Signature.cpp b/corelib/src/Signature.cpp index 53f843b2b8..ee08ab6804 100644 --- a/corelib/src/Signature.cpp +++ b/corelib/src/Signature.cpp @@ -348,7 +348,7 @@ unsigned long Signature::getMemoryUsed(bool withSensorData) const // Return memo total += _words.size() * (sizeof(int)*2+sizeof(std::multimap::iterator)) + sizeof(std::multimap); total += _wordsKpts.size() * sizeof(cv::KeyPoint) + sizeof(std::vector); total += _words3.size() * sizeof(cv::Point3f) + sizeof(std::vector); - total += _wordsDescriptors.total() * _wordsDescriptors.elemSize() + sizeof(cv::Mat); + total += _wordsDescriptors.empty()?0:_wordsDescriptors.total() * _wordsDescriptors.elemSize() + sizeof(cv::Mat); total += _wordsChanged.size() * (sizeof(int)*2+sizeof(std::map::iterator)) + sizeof(std::map); if(withSensorData) { diff --git a/corelib/src/SimpleIni.h b/corelib/src/SimpleIni.h index 3fccf9dc12..e787be813c 100644 --- a/corelib/src/SimpleIni.h +++ b/corelib/src/SimpleIni.h @@ -324,7 +324,10 @@ class CSimpleIniTempl #endif /** Strict less ordering by name of key only */ - struct KeyOrder : std::binary_function { + struct KeyOrder { + using result_type = bool; + using first_argument_type = Entry; + using second_argument_type = Entry; bool operator()(const Entry & lhs, const Entry & rhs) const { const static SI_STRLESS isLess = SI_STRLESS(); return isLess(lhs.pItem, rhs.pItem); @@ -332,7 +335,10 @@ class CSimpleIniTempl }; /** Strict less ordering by order, and then name of key */ - struct LoadOrder : std::binary_function { + struct LoadOrder { + using result_type = bool; + using first_argument_type = Entry; + using second_argument_type = Entry; bool operator()(const Entry & lhs, const Entry & rhs) const { if (lhs.nOrder != rhs.nOrder) { return lhs.nOrder < rhs.nOrder; diff --git a/corelib/src/Statistics.cpp b/corelib/src/Statistics.cpp index 81ec48805c..016e9ca179 100644 --- a/corelib/src/Statistics.cpp +++ b/corelib/src/Statistics.cpp @@ -25,7 +25,7 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ -#include "rtabmap/core/Statistics.h" +#include #include #include @@ -95,4 +95,10 @@ void Statistics::addStatistic(const std::string & name, float value) uInsert(_data, std::pair(name, value)); } +//deprecated +void Statistics::setLastSignatureData(const Signature & data) +{ + _signaturesData.insert(std::make_pair(data.id(), data)); +} + } diff --git a/corelib/src/Transform.cpp b/corelib/src/Transform.cpp index 6fc2755e7a..1157b62746 100644 --- a/corelib/src/Transform.cpp +++ b/corelib/src/Transform.cpp @@ -211,14 +211,24 @@ Transform Transform::to3DoF() const { float x,y,z,roll,pitch,yaw; this->getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw); - return Transform(x,y,0, 0,0,yaw); + float A = std::cos(yaw); + float B = std::sin(yaw); + return Transform( + A,-B, 0, x, + B, A, 0, y, + 0, 0, 1, 0); } Transform Transform::to4DoF() const { float x,y,z,roll,pitch,yaw; this->getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw); - return Transform(x,y,z, 0,0,yaw); + float A = std::cos(yaw); + float B = std::sin(yaw); + return Transform( + A,-B, 0, x, + B, A, 0, y, + 0, 0, 1, z); } bool Transform::is3DoF() const @@ -232,7 +242,7 @@ bool Transform::is4DoF() const r23() == 0.0 && r31() == 0.0 && r32() == 0.0 && - r33() == 0.0; + r33() == 1.0; } cv::Mat Transform::rotationMatrix() const diff --git a/corelib/src/camera/CameraDepthAI.cpp b/corelib/src/camera/CameraDepthAI.cpp index 4c14c46318..ebfde65f47 100644 --- a/corelib/src/camera/CameraDepthAI.cpp +++ b/corelib/src/camera/CameraDepthAI.cpp @@ -26,6 +26,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ #include +#include #include #include #include @@ -45,19 +46,30 @@ bool CameraDepthAI::available() } CameraDepthAI::CameraDepthAI( - const std::string & deviceSerial, + const std::string & mxidOrName, int resolution, float imageRate, const Transform & localTransform) : Camera(imageRate, localTransform) #ifdef RTABMAP_DEPTHAI , - deviceSerial_(deviceSerial), + mxidOrName_(mxidOrName), outputDepth_(false), depthConfidence_(200), resolution_(resolution), - imuFirmwareUpdate_(false), - imuPublished_(true) + useSpecTranslation_(false), + alphaScaling_(0.0), + imuPublished_(true), + publishInterIMU_(false), + dotProjectormA_(0.0), + floodLightmA_(200.0), + detectFeatures_(0), + useHarrisDetector_(false), + minDistance_(7.0), + numTargetFeatures_(1000), + threshold_(0.01), + nms_(true), + nmsRadius_(4) #endif { #ifdef RTABMAP_DEPTHAI @@ -88,10 +100,19 @@ void CameraDepthAI::setOutputDepth(bool enabled, int confidence) #endif } -void CameraDepthAI::setIMUFirmwareUpdate(bool enabled) +void CameraDepthAI::setUseSpecTranslation(bool useSpecTranslation) { #ifdef RTABMAP_DEPTHAI - imuFirmwareUpdate_ = enabled; + useSpecTranslation_ = useSpecTranslation; +#else + UERROR("CameraDepthAI: RTAB-Map is not built with depthai-core support!"); +#endif +} + +void CameraDepthAI::setAlphaScaling(float alphaScaling) +{ +#ifdef RTABMAP_DEPTHAI + alphaScaling_ = alphaScaling; #else UERROR("CameraDepthAI: RTAB-Map is not built with depthai-core support!"); #endif @@ -106,50 +127,111 @@ void CameraDepthAI::setIMUPublished(bool published) #endif } +void CameraDepthAI::publishInterIMU(bool enabled) +{ +#ifdef RTABMAP_DEPTHAI + publishInterIMU_ = enabled; +#else + UERROR("CameraDepthAI: RTAB-Map is not built with depthai-core support!"); +#endif +} + +void CameraDepthAI::setLaserDotBrightness(float dotProjectormA) +{ +#ifdef RTABMAP_DEPTHAI + dotProjectormA_ = dotProjectormA; +#else + UERROR("CameraDepthAI: RTAB-Map is not built with depthai-core support!"); +#endif +} + +void CameraDepthAI::setFloodLightBrightness(float floodLightmA) +{ +#ifdef RTABMAP_DEPTHAI + floodLightmA_ = floodLightmA; +#else + UERROR("CameraDepthAI: RTAB-Map is not built with depthai-core support!"); +#endif +} + +void CameraDepthAI::setDetectFeatures(int detectFeatures) +{ +#ifdef RTABMAP_DEPTHAI + detectFeatures_ = detectFeatures; +#else + UERROR("CameraDepthAI: RTAB-Map is not built with depthai-core support!"); +#endif +} + +void CameraDepthAI::setBlobPath(const std::string & blobPath) +{ +#ifdef RTABMAP_DEPTHAI + blobPath_ = blobPath; +#else + UERROR("CameraDepthAI: RTAB-Map is not built with depthai-core support!"); +#endif +} + +void CameraDepthAI::setGFTTDetector(bool useHarrisDetector, float minDistance, int numTargetFeatures) +{ +#ifdef RTABMAP_DEPTHAI + useHarrisDetector_ = useHarrisDetector; + minDistance_ = minDistance; + numTargetFeatures_ = numTargetFeatures; +#else + UERROR("CameraDepthAI: RTAB-Map is not built with depthai-core support!"); +#endif +} + +void CameraDepthAI::setSuperPointDetector(float threshold, bool nms, int nmsRadius) +{ +#ifdef RTABMAP_DEPTHAI + threshold_ = threshold; + nms_ = nms; + nmsRadius_ = nmsRadius; +#else + UERROR("CameraDepthAI: RTAB-Map is not built with depthai-core support!"); +#endif +} + bool CameraDepthAI::init(const std::string & calibrationFolder, const std::string & cameraName) { UDEBUG(""); #ifdef RTABMAP_DEPTHAI std::vector devices = dai::Device::getAllAvailableDevices(); - if(devices.empty()) + if(devices.empty() && mxidOrName_.empty()) { + UERROR("No DepthAI device found or specified"); return false; } if(device_.get()) - { device_->close(); - } + accBuffer_.clear(); gyroBuffer_.clear(); - dai::DeviceInfo deviceToUse; - if(deviceSerial_.empty()) - deviceToUse = devices[0]; - for(size_t i=0; i(); @@ -158,61 +240,126 @@ bool CameraDepthAI::init(const std::string & calibrationFolder, const std::strin std::shared_ptr imu; if(imuPublished_) imu = p.create(); + std::shared_ptr gfttDetector; + std::shared_ptr manip; + std::shared_ptr superPointNetwork; + if(detectFeatures_ == 1) + { + gfttDetector = p.create(); + } + else if(detectFeatures_ == 2) + { + if(!blobPath_.empty()) + { + manip = p.create(); + superPointNetwork = p.create(); + } + else + { + UWARN("Missing SuperPoint blob file!"); + detectFeatures_ = 0; + } + } auto xoutLeft = p.create(); auto xoutDepthOrRight = p.create(); std::shared_ptr xoutIMU; if(imuPublished_) xoutIMU = p.create(); + std::shared_ptr xoutFeatures; + if(detectFeatures_) + xoutFeatures = p.create(); // XLinkOut xoutLeft->setStreamName("rectified_left"); xoutDepthOrRight->setStreamName(outputDepth_?"depth":"rectified_right"); if(imuPublished_) xoutIMU->setStreamName("imu"); + if(detectFeatures_) + xoutFeatures->setStreamName("features"); // MonoCamera monoLeft->setResolution((dai::MonoCameraProperties::SensorResolution)resolution_); - monoLeft->setBoardSocket(dai::CameraBoardSocket::LEFT); + monoLeft->setCamera("left"); monoRight->setResolution((dai::MonoCameraProperties::SensorResolution)resolution_); - monoRight->setBoardSocket(dai::CameraBoardSocket::RIGHT); - if(this->getImageRate()>0) + monoRight->setCamera("right"); + if(detectFeatures_ == 2) + { + if(this->getImageRate() <= 0 || this->getImageRate() > 15) + { + UWARN("On-device SuperPoint enabled, image rate is limited to 15 FPS!"); + monoLeft->setFps(15); + monoRight->setFps(15); + } + } + else if(this->getImageRate() > 0) { monoLeft->setFps(this->getImageRate()); monoRight->setFps(this->getImageRate()); } // StereoDepth + stereo->setDepthAlign(dai::StereoDepthProperties::DepthAlign::RECTIFIED_LEFT); + stereo->setExtendedDisparity(false); + stereo->setRectifyEdgeFillColor(0); // black, to better see the cutout + stereo->enableDistortionCorrection(true); + stereo->setDisparityToDepthUseSpecTranslation(useSpecTranslation_); + stereo->setDepthAlignmentUseSpecTranslation(useSpecTranslation_); + if(alphaScaling_>-1.0f) + stereo->setAlphaScaling(alphaScaling_); stereo->initialConfig.setConfidenceThreshold(depthConfidence_); + stereo->initialConfig.setLeftRightCheck(true); stereo->initialConfig.setLeftRightCheckThreshold(5); - stereo->setRectifyEdgeFillColor(0); // black, to better see the cutout - stereo->setLeftRightCheck(true); - stereo->setSubpixel(false); - stereo->setExtendedDisparity(false); + stereo->initialConfig.setMedianFilter(dai::MedianFilter::KERNEL_7x7); + auto config = stereo->initialConfig.get(); + config.censusTransform.kernelSize = dai::StereoDepthConfig::CensusTransform::KernelSize::KERNEL_7x9; + config.censusTransform.kernelMask = 0X2AA00AA805540155; + config.postProcessing.brightnessFilter.maxBrightness = 255; + stereo->initialConfig.set(config); // Link plugins CAM -> STEREO -> XLINK monoLeft->out.link(stereo->left); monoRight->out.link(stereo->right); - if(outputDepth_) + // Using VideoEncoder on PoE devices, Subpixel is not supported + if(deviceToUse.protocol == X_LINK_TCP_IP || mxidOrName_.find(".") != std::string::npos) { - // Depth is registered to right image by default, so subscribe to right image when depth is used + auto leftEnc = p.create(); + auto depthOrRightEnc = p.create(); + leftEnc->setDefaultProfilePreset(monoLeft->getFps(), dai::VideoEncoderProperties::Profile::MJPEG); + depthOrRightEnc->setDefaultProfilePreset(monoRight->getFps(), dai::VideoEncoderProperties::Profile::MJPEG); + stereo->rectifiedLeft.link(leftEnc->input); if(outputDepth_) - stereo->rectifiedRight.link(xoutLeft->input); + { + depthOrRightEnc->setQuality(100); + stereo->disparity.link(depthOrRightEnc->input); + } else - stereo->rectifiedLeft.link(xoutLeft->input); - stereo->depth.link(xoutDepthOrRight->input); + { + stereo->rectifiedRight.link(depthOrRightEnc->input); + } + leftEnc->bitstream.link(xoutLeft->input); + depthOrRightEnc->bitstream.link(xoutDepthOrRight->input); } else { + stereo->setSubpixel(true); + stereo->setSubpixelFractionalBits(4); + config = stereo->initialConfig.get(); + config.costMatching.disparityWidth = dai::StereoDepthConfig::CostMatching::DisparityWidth::DISPARITY_64; + config.costMatching.enableCompanding = true; + stereo->initialConfig.set(config); stereo->rectifiedLeft.link(xoutLeft->input); - stereo->rectifiedRight.link(xoutDepthOrRight->input); + if(outputDepth_) + stereo->depth.link(xoutDepthOrRight->input); + else + stereo->rectifiedRight.link(xoutDepthOrRight->input); } if(imuPublished_) { - // enable ACCELEROMETER_RAW and GYROSCOPE_RAW at 200 hz rate - imu->enableIMUSensor({dai::IMUSensor::ACCELEROMETER_RAW, dai::IMUSensor::GYROSCOPE_RAW}, 200); + // enable ACCELEROMETER_RAW and GYROSCOPE_RAW at 100 hz rate + imu->enableIMUSensor({dai::IMUSensor::ACCELEROMETER_RAW, dai::IMUSensor::GYROSCOPE_RAW}, 100); // above this threshold packets will be sent in batch of X, if the host is not blocked and USB bandwidth is available imu->setBatchReportThreshold(1); // maximum number of IMU packets in a batch, if it's reached device will block sending until host can receive it @@ -222,39 +369,102 @@ bool CameraDepthAI::init(const std::string & calibrationFolder, const std::strin // Link plugins IMU -> XLINK imu->out.link(xoutIMU->input); + } - imu->enableFirmwareUpdate(imuFirmwareUpdate_); + if(detectFeatures_ == 1) + { + gfttDetector->setHardwareResources(1, 2); + gfttDetector->initialConfig.setCornerDetector( + useHarrisDetector_?dai::FeatureTrackerConfig::CornerDetector::Type::HARRIS:dai::FeatureTrackerConfig::CornerDetector::Type::SHI_THOMASI); + gfttDetector->initialConfig.setNumTargetFeatures(numTargetFeatures_); + gfttDetector->initialConfig.setMotionEstimator(false); + auto cfg = gfttDetector->initialConfig.get(); + cfg.featureMaintainer.minimumDistanceBetweenFeatures = minDistance_ * minDistance_; + gfttDetector->initialConfig.set(cfg); + stereo->rectifiedLeft.link(gfttDetector->inputImage); + gfttDetector->outputFeatures.link(xoutFeatures->input); + } + else if(detectFeatures_ == 2) + { + manip->setKeepAspectRatio(false); + manip->setMaxOutputFrameSize(320 * 200); + manip->initialConfig.setResize(320, 200); + superPointNetwork->setBlobPath(blobPath_); + superPointNetwork->setNumInferenceThreads(2); + superPointNetwork->setNumNCEPerInferenceThread(1); + superPointNetwork->input.setBlocking(false); + stereo->rectifiedLeft.link(manip->inputImage); + manip->out.link(superPointNetwork->input); + superPointNetwork->out.link(xoutFeatures->input); } device_.reset(new dai::Device(p, deviceToUse)); UINFO("Loading eeprom calibration data"); dai::CalibrationHandler calibHandler = device_->readCalibration(); - std::vector > matrix = calibHandler.getCameraIntrinsics(dai::CameraBoardSocket::LEFT, dai::Size2f(targetSize.width, targetSize.height)); - double fx = matrix[0][0]; - double fy = matrix[1][1]; - double cx = matrix[0][2]; - double cy = matrix[1][2]; - matrix = calibHandler.getCameraExtrinsics(dai::CameraBoardSocket::RIGHT, dai::CameraBoardSocket::LEFT); - double baseline = matrix[0][3]/100.0; + + cv::Mat cameraMatrix, distCoeffs, new_camera_matrix; + + std::vector > matrix = calibHandler.getCameraIntrinsics(dai::CameraBoardSocket::CAM_B, dai::Size2f(targetSize_.width, targetSize_.height)); + cameraMatrix = (cv::Mat_(3,3) << + matrix[0][0], matrix[0][1], matrix[0][2], + matrix[1][0], matrix[1][1], matrix[1][2], + matrix[2][0], matrix[2][1], matrix[2][2]); + + std::vector coeffs = calibHandler.getDistortionCoefficients(dai::CameraBoardSocket::CAM_B); + if(calibHandler.getDistortionModel(dai::CameraBoardSocket::CAM_B) == dai::CameraModel::Perspective) + distCoeffs = (cv::Mat_(1,8) << coeffs[0], coeffs[1], coeffs[2], coeffs[3], coeffs[4], coeffs[5], coeffs[6], coeffs[7]); + + if(alphaScaling_>-1.0f) + new_camera_matrix = cv::getOptimalNewCameraMatrix(cameraMatrix, distCoeffs, targetSize_, alphaScaling_); + else + new_camera_matrix = cameraMatrix; + + double fx = new_camera_matrix.at(0, 0); + double fy = new_camera_matrix.at(1, 1); + double cx = new_camera_matrix.at(0, 2); + double cy = new_camera_matrix.at(1, 2); + double baseline = calibHandler.getBaselineDistance(dai::CameraBoardSocket::CAM_C, dai::CameraBoardSocket::CAM_B, useSpecTranslation_)/100.0; UINFO("left: fx=%f fy=%f cx=%f cy=%f baseline=%f", fx, fy, cx, cy, baseline); - stereoModel_ = StereoCameraModel(device_->getMxId(), fx, fy, cx, cy, baseline, this->getLocalTransform(), targetSize); + stereoModel_ = StereoCameraModel(device_->getDeviceName(), fx, fy, cx, cy, baseline, this->getLocalTransform(), targetSize_); if(imuPublished_) { // Cannot test the following, I get "IMU calibration data is not available on device yet." with my camera // Update: now (as March 6, 2022) it crashes in "dai::CalibrationHandler::getImuToCameraExtrinsics(dai::CameraBoardSocket, bool)" - //matrix = calibHandler.getImuToCameraExtrinsics(dai::CameraBoardSocket::LEFT); + //matrix = calibHandler.getImuToCameraExtrinsics(dai::CameraBoardSocket::CAM_B); //imuLocalTransform_ = Transform( // matrix[0][0], matrix[0][1], matrix[0][2], matrix[0][3], // matrix[1][0], matrix[1][1], matrix[1][2], matrix[1][3], // matrix[2][0], matrix[2][1], matrix[2][2], matrix[2][3]); - // Hard-coded: x->down, y->left, z->forward - imuLocalTransform_ = Transform( - 0, 0, 1, 0, - 0, 1, 0, 0, - -1 ,0, 0, 0); - UINFO("IMU local transform = %s", imuLocalTransform_.prettyPrint().c_str()); + auto eeprom = calibHandler.getEepromData(); + if(eeprom.boardName == "OAK-D" || + eeprom.boardName == "BW1098OBC") + { + imuLocalTransform_ = Transform( + 0, -1, 0, 0.0525, + 1, 0, 0, 0.0137, + 0, 0, 1, 0); + } + else if(eeprom.boardName == "DM9098") + { + imuLocalTransform_ = Transform( + 0, 1, 0, 0.075445, + 1, 0, 0, 0.00079, + 0, 0, -1, -0.007); + } + else if(eeprom.boardName == "NG9097") + { + imuLocalTransform_ = Transform( + 0, 1, 0, 0.0775, + 1, 0, 0, 0.020265, + 0, 0, -1, -0.007); + } + else + { + UWARN("Unknown boardName (%s)! Disabling IMU!", eeprom.boardName.c_str()); + imuPublished_ = false; + } } else { @@ -263,10 +473,46 @@ bool CameraDepthAI::init(const std::string & calibrationFolder, const std::strin if(imuPublished_) { - imuQueue_ = device_->getOutputQueue("imu", 50, false); + imuLocalTransform_ = this->getLocalTransform() * imuLocalTransform_; + UINFO("IMU local transform = %s", imuLocalTransform_.prettyPrint().c_str()); + device_->getOutputQueue("imu", 50, false)->addCallback([this](const std::shared_ptr data) { + auto imuData = std::dynamic_pointer_cast(data); + auto imuPackets = imuData->packets; + + for(auto& imuPacket : imuPackets) + { + auto& acceleroValues = imuPacket.acceleroMeter; + auto& gyroValues = imuPacket.gyroscope; + double accStamp = std::chrono::duration(acceleroValues.getTimestampDevice().time_since_epoch()).count(); + double gyroStamp = std::chrono::duration(gyroValues.getTimestampDevice().time_since_epoch()).count(); + + if(publishInterIMU_) + { + IMU imu(cv::Vec3f(gyroValues.x, gyroValues.y, gyroValues.z), cv::Mat::eye(3,3,CV_64FC1), + cv::Vec3f(acceleroValues.x, acceleroValues.y, acceleroValues.z), cv::Mat::eye(3,3,CV_64FC1), + imuLocalTransform_); + UEventsManager::post(new IMUEvent(imu, (accStamp+gyroStamp)/2)); + } + else + { + UScopeMutex lock(imuMutex_); + accBuffer_.emplace_hint(accBuffer_.end(), accStamp, cv::Vec3f(acceleroValues.x, acceleroValues.y, acceleroValues.z)); + gyroBuffer_.emplace_hint(gyroBuffer_.end(), gyroStamp, cv::Vec3f(gyroValues.x, gyroValues.y, gyroValues.z)); + } + } + }); + } + leftQueue_ = device_->getOutputQueue("rectified_left", 8, false); + rightOrDepthQueue_ = device_->getOutputQueue(outputDepth_?"depth":"rectified_right", 8, false); + if(detectFeatures_) + featuresQueue_ = device_->getOutputQueue("features", 8, false); + + std::vector> irDrivers = device_->getIrDrivers(); + if(!irDrivers.empty()) + { + device_->setIrLaserDotProjectorBrightness(dotProjectormA_); + device_->setIrFloodLightBrightness(floodLightmA_); } - leftQueue_ = device_->getOutputQueue("rectified_left", 1, false); - rightOrDepthQueue_ = device_->getOutputQueue(outputDepth_?"depth":"rectified_right", 1, false); uSleep(2000); // avoid bad frames on start @@ -289,7 +535,7 @@ bool CameraDepthAI::isCalibrated() const std::string CameraDepthAI::getSerial() const { #ifdef RTABMAP_DEPTHAI - return deviceSerial_; + return device_->getMxId(); #endif return ""; } @@ -303,173 +549,145 @@ SensorData CameraDepthAI::captureImage(SensorCaptureInfo * info) auto rectifL = leftQueue_->get(); auto rectifRightOrDepth = rightOrDepthQueue_->get(); - if(rectifL.get() && rectifRightOrDepth.get()) - { - auto stampLeft = rectifL->getTimestamp().time_since_epoch().count(); - auto stampRight = rectifRightOrDepth->getTimestamp().time_since_epoch().count(); - double stamp = double(stampLeft)/10e8; - left = rectifL->getCvFrame(); - depthOrRight = rectifRightOrDepth->getCvFrame(); + while(rectifL->getSequenceNum() < rectifRightOrDepth->getSequenceNum()) + rectifL = leftQueue_->get(); + while(rectifL->getSequenceNum() > rectifRightOrDepth->getSequenceNum()) + rectifRightOrDepth = rightOrDepthQueue_->get(); - if(!left.empty() && !depthOrRight.empty()) + double stamp = std::chrono::duration(rectifL->getTimestampDevice(dai::CameraExposureOffset::MIDDLE).time_since_epoch()).count(); + if(device_->getDeviceInfo().protocol == X_LINK_TCP_IP || mxidOrName_.find(".") != std::string::npos) + { + left = cv::imdecode(rectifL->getData(), cv::IMREAD_GRAYSCALE); + depthOrRight = cv::imdecode(rectifRightOrDepth->getData(), cv::IMREAD_GRAYSCALE); + if(outputDepth_) { - if(depthOrRight.type() == CV_8UC1) - { - if(stereoModel_.isValidForRectification()) - { - left = stereoModel_.left().rectifyImage(left); - depthOrRight = stereoModel_.right().rectifyImage(depthOrRight); - } - data = SensorData(left, depthOrRight, stereoModel_, this->getNextSeqID(), stamp); - } - else - { - data = SensorData(left, depthOrRight, stereoModel_.left(), this->getNextSeqID(), stamp); - } - - if(fabs(double(stampLeft)/10e8 - double(stampRight)/10e8) >= 0.0001) //0.1 ms - { - UWARN("Frames are not synchronized! %f vs %f", double(stampLeft)/10e8, double(stampRight)/10e8); - } + cv::Mat disp; + depthOrRight.convertTo(disp, CV_16UC1); + cv::divide(-stereoModel_.right().Tx() * 1000, disp, depthOrRight); + } + } + else + { + left = rectifL->getFrame(true); + depthOrRight = rectifRightOrDepth->getFrame(true); + } - //get imu - double stampStart = UTimer::now(); - while(imuPublished_ && imuQueue_.get()) - { - if(imuQueue_->has()) - { - auto imuData = imuQueue_->get(); - - auto imuPackets = imuData->packets; - double accStamp = 0.0; - double gyroStamp = 0.0; - for(auto& imuPacket : imuPackets) { - auto& acceleroValues = imuPacket.acceleroMeter; - auto& gyroValues = imuPacket.gyroscope; - - accStamp = double(acceleroValues.timestamp.get().time_since_epoch().count())/10e8; - gyroStamp = double(gyroValues.timestamp.get().time_since_epoch().count())/10e8; - accBuffer_.insert(accBuffer_.end(), std::make_pair(accStamp, cv::Vec3f(acceleroValues.x, acceleroValues.y, acceleroValues.z))); - gyroBuffer_.insert(gyroBuffer_.end(), std::make_pair(gyroStamp, cv::Vec3f(gyroValues.x, gyroValues.y, gyroValues.z))); - if(accBuffer_.size() > 1000) - { - accBuffer_.erase(accBuffer_.begin()); - } - if(gyroBuffer_.size() > 1000) - { - gyroBuffer_.erase(gyroBuffer_.begin()); - } - } - if(accStamp >= stamp && gyroStamp >= stamp) - { - break; - } - } - if((UTimer::now() - stampStart) > 0.01) - { - UWARN("Could not received IMU after 10 ms! Disabling IMU!"); - imuPublished_ = false; - } - } + if(outputDepth_) + data = SensorData(left, depthOrRight, stereoModel_.left(), this->getNextSeqID(), stamp); + else + data = SensorData(left, depthOrRight, stereoModel_, this->getNextSeqID(), stamp); - cv::Vec3d acc, gyro; - bool valid = !accBuffer_.empty() && !gyroBuffer_.empty(); - //acc - if(!accBuffer_.empty()) - { - std::map::const_iterator iterB = accBuffer_.lower_bound(stamp); - std::map::const_iterator iterA = iterB; - if(iterA != accBuffer_.begin()) - { - iterA = --iterA; - } - if(iterB == accBuffer_.end()) - { - iterB = --iterB; - } - if(iterA == iterB && stamp == iterA->first) - { - acc[0] = iterA->second[0]; - acc[1] = iterA->second[1]; - acc[2] = iterA->second[2]; - } - else if(stamp >= iterA->first && stamp <= iterB->first) - { - float t = (stamp-iterA->first) / (iterB->first-iterA->first); - acc[0] = iterA->second[0] + t*(iterB->second[0] - iterA->second[0]); - acc[1] = iterA->second[1] + t*(iterB->second[1] - iterA->second[1]); - acc[2] = iterA->second[2] + t*(iterB->second[2] - iterA->second[2]); - } - else - { - valid = false; - if(stamp < iterA->first) - { - UWARN("Could not find acc data to interpolate at image time %f (earliest is %f). Are sensors synchronized?", stamp, iterA->first); - } - else if(stamp > iterB->first) - { - UWARN("Could not find acc data to interpolate at image time %f (latest is %f). Are sensors synchronized?", stamp, iterB->first); - } - else - { - UWARN("Could not find acc data to interpolate at image time %f (between %f and %f). Are sensors synchronized?", stamp, iterA->first, iterB->first); - } - } - } - //gyro - if(!gyroBuffer_.empty()) - { - std::map::const_iterator iterB = gyroBuffer_.lower_bound(stamp); - std::map::const_iterator iterA = iterB; - if(iterA != gyroBuffer_.begin()) - { - iterA = --iterA; - } - if(iterB == gyroBuffer_.end()) - { - iterB = --iterB; - } - if(iterA == iterB && stamp == iterA->first) - { - gyro[0] = iterA->second[0]; - gyro[1] = iterA->second[1]; - gyro[2] = iterA->second[2]; - } - else if(stamp >= iterA->first && stamp <= iterB->first) - { - float t = (stamp-iterA->first) / (iterB->first-iterA->first); - gyro[0] = iterA->second[0] + t*(iterB->second[0] - iterA->second[0]); - gyro[1] = iterA->second[1] + t*(iterB->second[1] - iterA->second[1]); - gyro[2] = iterA->second[2] + t*(iterB->second[2] - iterA->second[2]); - } - else - { - valid = false; - if(stamp < iterA->first) - { - UWARN("Could not find gyro data to interpolate at image time %f (earliest is %f). Are sensors synchronized?", stamp, iterA->first); - } - else if(stamp > iterB->first) - { - UWARN("Could not find gyro data to interpolate at image time %f (latest is %f). Are sensors synchronized?", stamp, iterB->first); - } - else - { - UWARN("Could not find gyro data to interpolate at image time %f (between %f and %f). Are sensors synchronized?", stamp, iterA->first, iterB->first); - } - } - } + if(imuPublished_ && !publishInterIMU_) + { + cv::Vec3d acc, gyro; + std::map::const_iterator iterA, iterB; + + imuMutex_.lock(); + while(accBuffer_.empty() || gyroBuffer_.empty() || accBuffer_.rbegin()->first < stamp || gyroBuffer_.rbegin()->first < stamp) + { + imuMutex_.unlock(); + uSleep(1); + imuMutex_.lock(); + } - if(valid) - { - data.setIMU(IMU(gyro, cv::Mat::eye(3, 3, CV_64FC1), acc, cv::Mat::eye(3, 3, CV_64FC1), imuLocalTransform_)); - } + //acc + iterB = accBuffer_.lower_bound(stamp); + iterA = iterB; + if(iterA != accBuffer_.begin()) + iterA = --iterA; + if(iterA == iterB || stamp == iterB->first) + { + acc = iterB->second; } + else if(stamp > iterA->first && stamp < iterB->first) + { + float t = (stamp-iterA->first) / (iterB->first-iterA->first); + acc = iterA->second + t*(iterB->second - iterA->second); + } + accBuffer_.erase(accBuffer_.begin(), iterB); + + //gyro + iterB = gyroBuffer_.lower_bound(stamp); + iterA = iterB; + if(iterA != gyroBuffer_.begin()) + iterA = --iterA; + if(iterA == iterB || stamp == iterB->first) + { + gyro = iterB->second; + } + else if(stamp > iterA->first && stamp < iterB->first) + { + float t = (stamp-iterA->first) / (iterB->first-iterA->first); + gyro = iterA->second + t*(iterB->second - iterA->second); + } + gyroBuffer_.erase(gyroBuffer_.begin(), iterB); + + imuMutex_.unlock(); + data.setIMU(IMU(gyro, cv::Mat::eye(3, 3, CV_64FC1), acc, cv::Mat::eye(3, 3, CV_64FC1), imuLocalTransform_)); } - else + + if(detectFeatures_ == 1) { - UWARN("Null images received!?"); + auto features = featuresQueue_->get(); + while(features->getSequenceNum() < rectifL->getSequenceNum()) + features = featuresQueue_->get(); + auto detectedFeatures = features->trackedFeatures; + + std::vector keypoints; + for(auto& feature : detectedFeatures) + keypoints.emplace_back(cv::KeyPoint(feature.position.x, feature.position.y, 3)); + data.setFeatures(keypoints, std::vector(), cv::Mat()); + } + else if(detectFeatures_ == 2) + { + auto features = featuresQueue_->get(); + while(features->getSequenceNum() < rectifL->getSequenceNum()) + features = featuresQueue_->get(); + + auto heatmap = features->getLayerFp16("heatmap"); + auto desc = features->getLayerFp16("desc"); + + cv::Mat prob(200, 320, CV_32FC1, heatmap.data()); + cv::resize(prob, prob, targetSize_, 0, 0, cv::INTER_CUBIC); + std::vector kpts; + cv::findNonZero(prob > threshold_, kpts); + std::vector keypoints_no_nms, keypoints; + for(auto& kpt : kpts) + { + float response = prob.at(kpt); + keypoints_no_nms.emplace_back(cv::KeyPoint(kpt, 8, -1, response)); + } + + if(nms_ && !keypoints_no_nms.empty()) + { + cv::Mat descEmpty; + util2d::NMS(keypoints_no_nms, descEmpty, keypoints, descEmpty, 0, nmsRadius_, targetSize_.width, targetSize_.height); + } + else if(!keypoints_no_nms.empty()) + { + keypoints = keypoints_no_nms; + } + + cv::Mat coarse_desc(25, 40, CV_32FC(256), desc.data()); + coarse_desc.forEach>([&](cv::Vec& descriptor, const int position[]) -> void { + cv::normalize(descriptor, descriptor); + }); + cv::Mat mapX(keypoints.size(), 1, CV_32FC1); + cv::Mat mapY(keypoints.size(), 1, CV_32FC1); + for(size_t i=0; i(i) = (keypoints[i].pt.x - (targetSize_.width-1)/2) * 40/targetSize_.width + (40-1)/2; + mapY.at(i) = (keypoints[i].pt.y - (targetSize_.height-1)/2) * 25/targetSize_.height + (25-1)/2; + } + cv::Mat map1, map2, descriptors; + cv::convertMaps(mapX, mapY, map1, map2, CV_16SC2); + cv::remap(coarse_desc, descriptors, map1, map2, cv::INTER_LINEAR); + descriptors.forEach>([&](cv::Vec& descriptor, const int position[]) -> void { + cv::normalize(descriptor, descriptor); + }); + descriptors = descriptors.reshape(1); + + data.setFeatures(keypoints, std::vector(), descriptors); } #else diff --git a/corelib/src/camera/CameraK4A.cpp b/corelib/src/camera/CameraK4A.cpp index 7935c67532..ad5714d744 100644 --- a/corelib/src/camera/CameraK4A.cpp +++ b/corelib/src/camera/CameraK4A.cpp @@ -158,9 +158,25 @@ bool CameraK4A::init(const std::string & calibrationFolder, const std::string & return false; } - uint64_t recording_length = k4a_playback_get_last_timestamp_usec((k4a_playback_t)playbackHandle_); + uint64_t recording_length = k4a_playback_get_recording_length_usec((k4a_playback_t)playbackHandle_); UINFO("Recording is %lld seconds long", recording_length / 1000000); + k4a_record_configuration_t config; + if(k4a_playback_get_record_configuration((k4a_playback_t)playbackHandle_, &config)) + { + UERROR("Failed to get record configuration"); + close(); + return false; + } + if(this->getImageRate() < 0) + { + int rate = + config.camera_fps == K4A_FRAMES_PER_SECOND_5?5: + config.camera_fps == K4A_FRAMES_PER_SECOND_15?15:30; + UINFO("Recording framerate is %d fps", rate); + this->setImageRate(-float(rate)); + } + if (k4a_playback_get_calibration((k4a_playback_t)playbackHandle_, &calibration_)) { UERROR("Failed to get calibration"); @@ -501,6 +517,7 @@ SensorData CameraK4A::captureImage(SensorCaptureInfo * info) } double stamp = UTimer::now(); + uint64_t imageStamp = 0; if(!bgrCV.empty()) { // Retrieve depth image from capture @@ -508,7 +525,8 @@ SensorData CameraK4A::captureImage(SensorCaptureInfo * info) if (depth_image_ != NULL) { - double stampDevice = ((double)k4a_image_get_device_timestamp_usec(depth_image_)) / 1000000.0; + imageStamp = k4a_image_get_device_timestamp_usec(depth_image_); + double stampDevice = ((double)imageStamp) / 1000000.0; if(timestampOffset_ == 0.0) { @@ -557,12 +575,19 @@ SensorData CameraK4A::captureImage(SensorCaptureInfo * info) k4a_capture_release(captureHandle_); + // Get IMU sample if(playbackHandle_) { - // Get IMU sample, clear buffer - // FIXME: not tested, uncomment when tested. - k4a_playback_seek_timestamp(playbackHandle_, stamp* 1000000+1, K4A_PLAYBACK_SEEK_BEGIN); - if(K4A_STREAM_RESULT_SUCCEEDED == k4a_playback_get_previous_imu_sample(playbackHandle_, &imu_sample_)) + k4a_stream_result_t imu_res = K4A_STREAM_RESULT_FAILED; + k4a_imu_sample_t next_imu_sample; + + while((K4A_STREAM_RESULT_SUCCEEDED == (imu_res=k4a_playback_get_next_imu_sample(playbackHandle_, &next_imu_sample))) && + (next_imu_sample.gyro_timestamp_usec < imageStamp)) + { + imu_sample_ = next_imu_sample; + } + + if(imu_res == K4A_STREAM_RESULT_SUCCEEDED) { imu = IMU(cv::Vec3d(imu_sample_.gyro_sample.xyz.x, imu_sample_.gyro_sample.xyz.y, imu_sample_.gyro_sample.xyz.z), cv::Mat::eye(3, 3, CV_64FC1), @@ -577,7 +602,6 @@ SensorData CameraK4A::captureImage(SensorCaptureInfo * info) } else { - // Get IMU sample, clear buffer if(K4A_WAIT_RESULT_SUCCEEDED == k4a_device_get_imu_sample(deviceHandle_, &imu_sample_, 60)) { imu = IMU(cv::Vec3d(imu_sample_.gyro_sample.xyz.x, imu_sample_.gyro_sample.xyz.y, imu_sample_.gyro_sample.xyz.z), @@ -610,8 +634,8 @@ SensorData CameraK4A::captureImage(SensorCaptureInfo * info) } else if (previousStamp_ > 0) { - float ratio = -this->getImageRate(); - int sleepTime = 1000.0*(stamp - previousStamp_) / ratio - 1000.0*timer_.getElapsedTime(); + double rateSec = -1.0/this->getImageRate(); + int sleepTime = 1000.0*(rateSec - timer_.getElapsedTime()); if (sleepTime > 10000) { UWARN("Detected long delay (%d sec, stamps = %f vs %f). Waiting a maximum of 10 seconds.", @@ -624,14 +648,14 @@ SensorData CameraK4A::captureImage(SensorCaptureInfo * info) } // Add precision at the cost of a small overhead - while (timer_.getElapsedTime() < (stamp - previousStamp_) / ratio - 0.000001) + while (timer_.getElapsedTime() < rateSec - 0.000001) { // } double slept = timer_.getElapsedTime(); timer_.start(); - UDEBUG("slept=%fs vs target=%fs (ratio=%f)", slept, (stamp - previousStamp_) / ratio, ratio); + UDEBUG("slept=%fs vs target=%fs", slept, rateSec); } previousStamp_ = stamp; } diff --git a/corelib/src/camera/CameraRealSense2.cpp b/corelib/src/camera/CameraRealSense2.cpp index 58b48b0f7a..130742daee 100644 --- a/corelib/src/camera/CameraRealSense2.cpp +++ b/corelib/src/camera/CameraRealSense2.cpp @@ -1501,7 +1501,7 @@ SensorData CameraRealSense2::captureImage(SensorCaptureInfo * 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/camera/CameraStereoZed.cpp b/corelib/src/camera/CameraStereoZed.cpp index 8ff3f902f2..7213a19b29 100644 --- a/corelib/src/camera/CameraStereoZed.cpp +++ b/corelib/src/camera/CameraStereoZed.cpp @@ -240,6 +240,17 @@ bool CameraStereoZed::available() #endif } + +int CameraStereoZed::sdkVersion() +{ +#ifdef RTABMAP_ZED + return ZED_SDK_MAJOR_VERSION; +#else + return -1; +#endif +} + + CameraStereoZed::CameraStereoZed( int deviceId, int resolution, @@ -274,6 +285,16 @@ CameraStereoZed::CameraStereoZed( { UDEBUG(""); #ifdef RTABMAP_ZED +#if ZED_SDK_MAJOR_VERSION < 4 + if(resolution_ == 3) + { + resolution_ = 2; + } + else if(resolution_ == 5) + { + resolution_ = 3; + } +#endif #if ZED_SDK_MAJOR_VERSION < 3 UASSERT(resolution_ >= sl::RESOLUTION_HD2K && resolution_ = sl::DEPTH_MODE_NONE && quality_ (resolution_); sl::DEPTH_MODE qual = static_cast(quality_); - sl::SENSING_MODE sens = static_cast(sensingMode_); UASSERT(res >= sl::RESOLUTION::HD2K && res < sl::RESOLUTION::LAST); UASSERT(qual >= sl::DEPTH_MODE::NONE && qual < sl::DEPTH_MODE::LAST); +#if ZED_SDK_MAJOR_VERSION < 4 + sl::SENSING_MODE sens = static_cast(sensingMode_); UASSERT(sens >= sl::SENSING_MODE::STANDARD && sens < sl::SENSING_MODE::LAST); +#else + UASSERT(sensingMode_ >= 0 && sensingMode_ < 2); +#endif UASSERT(confidenceThr_ >= 0 && confidenceThr_ <=100); UASSERT(texturenessConfidenceThr_ >= 0 && texturenessConfidenceThr_ <=100); #endif @@ -334,11 +359,15 @@ CameraStereoZed::CameraStereoZed( #else sl::RESOLUTION res = static_cast(resolution_); sl::DEPTH_MODE qual = static_cast(quality_); - sl::SENSING_MODE sens = static_cast(sensingMode_); UASSERT(res >= sl::RESOLUTION::HD2K && res < sl::RESOLUTION::LAST); UASSERT(qual >= sl::DEPTH_MODE::NONE && qual < sl::DEPTH_MODE::LAST); +#if ZED_SDK_MAJOR_VERSION < 4 + sl::SENSING_MODE sens = static_cast(sensingMode_); UASSERT(sens >= sl::SENSING_MODE::STANDARD && sens < sl::SENSING_MODE::LAST); +#else + UASSERT(sensingMode_ >= 0 && sensingMode_ < 2); +#endif UASSERT(confidenceThr_ >= 0 && confidenceThr_ <=100); UASSERT(texturenessConfidenceThr_ >= 0 && texturenessConfidenceThr_ <=100); #endif @@ -465,7 +494,11 @@ bool CameraStereoZed::init(const std::string & calibrationFolder, const std::str } sl::CameraInformation infos = zed_->getCameraInformation(); +#if ZED_SDK_MAJOR_VERSION < 4 sl::CalibrationParameters *stereoParams = &(infos.calibration_parameters ); +#else + sl::CalibrationParameters *stereoParams = &(infos.camera_configuration.calibration_parameters ); +#endif sl::Resolution res = stereoParams->left_cam.image_size; stereoModel_ = StereoCameraModel( @@ -473,7 +506,11 @@ bool CameraStereoZed::init(const std::string & calibrationFolder, const std::str stereoParams->left_cam.fy, stereoParams->left_cam.cx, stereoParams->left_cam.cy, +#if ZED_SDK_MAJOR_VERSION < 4 stereoParams->T[0],//baseline +#else + stereoParams->getCameraBaseline(), +#endif this->getLocalTransform(), cv::Size(res.width, res.height)); @@ -482,7 +519,11 @@ bool CameraStereoZed::init(const std::string & calibrationFolder, const std::str stereoParams->left_cam.fy, stereoParams->left_cam.cx, stereoParams->left_cam.cy, +#if ZED_SDK_MAJOR_VERSION < 4 stereoParams->T[0],//baseline +#else + stereoParams->getCameraBaseline(), +#endif (int)res.width, (int)res.height, this->getLocalTransform().prettyPrint().c_str()); @@ -493,11 +534,18 @@ bool CameraStereoZed::init(const std::string & calibrationFolder, const std::str if(infos.camera_model != sl::MODEL::ZED) #endif { +#if ZED_SDK_MAJOR_VERSION < 4 imuLocalTransform_ = this->getLocalTransform() * zedPoseToTransform(infos.camera_imu_transform).inverse(); +#else + imuLocalTransform_ = this->getLocalTransform() * zedPoseToTransform(infos.sensors_configuration.camera_imu_transform).inverse(); +#endif UINFO("IMU local transform: %s (imu2cam=%s))", - imuLocalTransform_.prettyPrint().c_str(), - zedPoseToTransform(infos.camera_imu_transform).prettyPrint().c_str()); - + imuLocalTransform_.prettyPrint().c_str(), +#if ZED_SDK_MAJOR_VERSION < 4 + zedPoseToTransform(infos.camera_imu_transform).prettyPrint().c_str()); +#else + zedPoseToTransform(infos.sensors_configuration.camera_imu_transform).prettyPrint().c_str()); +#endif if(publishInterIMU_) { imuPublishingThread_ = new ZedIMUThread(200, zed_, imuLocalTransform_, true); @@ -623,8 +671,10 @@ SensorData CameraStereoZed::captureImage(SensorCaptureInfo * info) #ifdef RTABMAP_ZED #if ZED_SDK_MAJOR_VERSION < 3 sl::RuntimeParameters rparam((sl::SENSING_MODE)sensingMode_, quality_ > 0, quality_ > 0, sl::REFERENCE_FRAME_CAMERA); -#else +#elif ZED_SDK_MAJOR_VERSION < 4 sl::RuntimeParameters rparam((sl::SENSING_MODE)sensingMode_, quality_ > 0, confidenceThr_, texturenessConfidenceThr_, sl::REFERENCE_FRAME::CAMERA); +#else + sl::RuntimeParameters rparam(quality_ > 0, sensingMode_ == 1, confidenceThr_, texturenessConfidenceThr_, sl::REFERENCE_FRAME::CAMERA); #endif if(zed_) @@ -732,38 +782,35 @@ SensorData CameraStereoZed::captureImage(SensorCaptureInfo * info) { int trackingConfidence = pose.pose_confidence; // FIXME What does pose_confidence == -1 mean? - if (trackingConfidence>0) + info->odomPose = zedPoseToTransform(pose); + if (!info->odomPose.isNull()) { - info->odomPose = zedPoseToTransform(pose); - if (!info->odomPose.isNull()) + //transform from: + // x->right, y->down, z->forward + //to: + // x->forward, y->left, z->up + info->odomPose = this->getLocalTransform() * info->odomPose * this->getLocalTransform().inverse(); + if(force3DoF_) { - //transform from: - // x->right, y->down, z->forward - //to: - // x->forward, y->left, z->up - info->odomPose = this->getLocalTransform() * info->odomPose * this->getLocalTransform().inverse(); - if(force3DoF_) - { - info->odomPose = info->odomPose.to3DoF(); - } - if (lost_) - { - info->odomCovariance = cv::Mat::eye(6, 6, CV_64FC1) * 9999.0f; // don't know transform with previous pose - lost_ = false; - UDEBUG("Init %s (var=%f)", info->odomPose.prettyPrint().c_str(), 9999.0f); - } - else - { - info->odomCovariance = cv::Mat::eye(6, 6, CV_64FC1) * 1.0f / float(trackingConfidence); - UDEBUG("Run %s (var=%f)", info->odomPose.prettyPrint().c_str(), 1.0f / float(trackingConfidence)); - } + info->odomPose = info->odomPose.to3DoF(); } - else + if (lost_) + { + info->odomCovariance = cv::Mat::eye(6, 6, CV_64FC1) * 9999.0f; // don't know transform with previous pose + lost_ = false; + UDEBUG("Init %s (var=%f)", info->odomPose.prettyPrint().c_str(), 9999.0f); + } + else if(trackingConfidence==0) { info->odomCovariance = cv::Mat::eye(6, 6, CV_64FC1) * 9999.0f; // lost lost_ = true; UWARN("ZED lost! (trackingConfidence=%d)", trackingConfidence); } + else + { + info->odomCovariance = cv::Mat::eye(6, 6, CV_64FC1) * 1.0f / float(trackingConfidence); + UDEBUG("Run %s (var=%f)", info->odomPose.prettyPrint().c_str(), 1.0f / float(trackingConfidence)); + } } else { diff --git a/corelib/src/icp/libpointmatcher.h b/corelib/src/icp/libpointmatcher.h index e2d362a6cd..46a5ef0830 100644 --- a/corelib/src/icp/libpointmatcher.h +++ b/corelib/src/icp/libpointmatcher.h @@ -513,16 +513,28 @@ struct KDTreeMatcherIntensity : public PointMatcher::Matcher for (int i = 0; i < pointsCount; ++i) { float minDistance = std::numeric_limits::max(); + bool minDistFound = false; for(int k=0; kgravitySigma() > 0.0f && !imus().empty()) { imuT = Transform::getTransform(imus(), data.stamp()); + if(data.imu().empty()) + { + Eigen::Quaternionf q = imuT.getQuaternionf(); + data.setIMU(IMU(cv::Vec4d(q.x(), q.y(), q.z(), q.w()), cv::Mat(), cv::Vec3d(), cv::Mat(), cv::Vec3d(), cv::Mat())); + } } RegistrationInfo regInfo; diff --git a/corelib/src/odometry/OdometryMSCKF.cpp b/corelib/src/odometry/OdometryMSCKF.cpp index add7e870af..ac62c25144 100644 --- a/corelib/src/odometry/OdometryMSCKF.cpp +++ b/corelib/src/odometry/OdometryMSCKF.cpp @@ -672,18 +672,17 @@ class MsckfVioNoROS: public msckf_vio::MsckfVio T_i_w.linear() = msckf_vio::quaternionToRotation(imu_state.orientation).transpose(); T_i_w.translation() = imu_state.position; - Eigen::Isometry3d T_b_w = msckf_vio::IMUState::T_imu_body * T_i_w * - msckf_vio::IMUState::T_imu_body.inverse(); + Eigen::Isometry3d T_b_w = T_i_w * msckf_vio::IMUState::T_imu_body.inverse(); Eigen::Vector3d body_velocity = msckf_vio::IMUState::T_imu_body.linear() * imu_state.velocity; // Publish tf /*if (publish_tf) { - tf::Transform T_b_w_tf; - tf::transformEigenToTF(T_b_w, T_b_w_tf); - tf_pub.sendTransform(tf::StampedTransform( - T_b_w_tf, time, fixed_frame_id, child_frame_id)); - }*/ + tf::Transform T_b_w_tf; + tf::transformEigenToTF(T_b_w, T_b_w_tf); + tf_pub.sendTransform(tf::StampedTransform( + T_b_w_tf, time, fixed_frame_id, child_frame_id)); + }*/ // Publish the odometry nav_msgs::Odometry odom_msg; @@ -725,20 +724,18 @@ class MsckfVioNoROS: public msckf_vio::MsckfVio // Publish the 3D positions of the features that // has been initialized. feature_msg_ptr.reset(new pcl::PointCloud()); - feature_msg_ptr->header.frame_id = fixed_frame_id; - feature_msg_ptr->height = 1; - for (const auto& item : map_server) { - const auto& feature = item.second; - if (feature.is_initialized) { - Eigen::Vector3d feature_position = - msckf_vio::IMUState::T_imu_body.linear() * feature.position; - feature_msg_ptr->points.push_back(pcl::PointXYZ( - feature_position(0), feature_position(1), feature_position(2))); - } - } - feature_msg_ptr->width = feature_msg_ptr->points.size(); - - //feature_pub.publish(feature_msg_ptr); + feature_msg_ptr->header.frame_id = fixed_frame_id; + feature_msg_ptr->height = 1; + for (const auto& item : map_server) { + const auto& feature = item.second; + if (feature.is_initialized) { + feature_msg_ptr->points.push_back(pcl::PointXYZ( + feature.position(0), feature.position(1), feature.position(2))); + } + } + feature_msg_ptr->width = feature_msg_ptr->points.size(); + + // feature_pub.publish(feature_msg_ptr); return odom_msg; } @@ -755,7 +752,7 @@ OdometryMSCKF::OdometryMSCKF(const ParametersMap & parameters) : imageProcessor_(0), msckf_(0), parameters_(parameters), -fixPoseRotation_(0, 0, -1, 0, 0, 1, 0, 0, 1, 0, 0, 0), +fixPoseRotation_(1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0), previousPose_(Transform::getIdentity()), initGravity_(false) #endif diff --git a/corelib/src/odometry/OdometryORBSLAM.cpp b/corelib/src/odometry/OdometryORBSLAM2.cpp similarity index 86% rename from corelib/src/odometry/OdometryORBSLAM.cpp rename to corelib/src/odometry/OdometryORBSLAM2.cpp index 4a63abe50c..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.stereoCameraModels()[0].baseline(), imuLocalTransform_)) + if(!orbslam_->init(model, stereo, data.cameraModels().size()==1?0.0f:data.stereoCameraModels()[0].baseline())) { return t; } @@ -1078,7 +953,7 @@ Transform OdometryORBSLAM::computeTransform( } else { - float baseline = data.stereoCameraModels()[0].baseline(); + float baseline = data.cameraModels().size()==1?0.0f:data.stereoCameraModels()[0].baseline(); if(baseline <= 0.0f) { baseline = rtabmap::Parameters::defaultOdomORBSLAMBf(); 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/corelib/src/odometry/OdometryOpenVINS.cpp b/corelib/src/odometry/OdometryOpenVINS.cpp index fda60d38d5..ffde226cfa 100644 --- a/corelib/src/odometry/OdometryOpenVINS.cpp +++ b/corelib/src/odometry/OdometryOpenVINS.cpp @@ -28,22 +28,17 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "rtabmap/core/odometry/OdometryOpenVINS.h" #include "rtabmap/core/OdometryInfo.h" #include "rtabmap/core/util3d_transforms.h" +#include "rtabmap/utilite/UFile.h" #include "rtabmap/utilite/ULogger.h" #include "rtabmap/utilite/UTimer.h" -#include "rtabmap/utilite/UStl.h" -#include "rtabmap/utilite/UThread.h" -#include "rtabmap/utilite/UDirectory.h" +#include #include #ifdef RTABMAP_OPENVINS #include "core/VioManager.h" -#include "core/VioManagerOptions.h" -#include "core/RosVisualizer.h" -#include "utils/dataset_reader.h" -#include "utils/parse_ros.h" -#include "utils/sensor_data.h" +#include "state/Propagator.h" #include "state/State.h" -#include "types/Type.h" +#include "state/StateHelper.h" #endif namespace rtabmap { @@ -52,17 +47,111 @@ OdometryOpenVINS::OdometryOpenVINS(const ParametersMap & parameters) : Odometry(parameters) #ifdef RTABMAP_OPENVINS , - vioManager_(0), initGravity_(false), - previousPose_(Transform::getIdentity()) + previousPoseInv_(Transform::getIdentity()) #endif { -} - -OdometryOpenVINS::~OdometryOpenVINS() -{ #ifdef RTABMAP_OPENVINS - delete vioManager_; + ov_core::Printer::setPrintLevel(ov_core::Printer::PrintLevel(ULogger::level()+1)); + int enum_index; + std::string left_mask_path, right_mask_path; + params_ = std::make_unique(); + Parameters::parse(parameters, Parameters::kOdomOpenVINSUseStereo(), params_->use_stereo); + Parameters::parse(parameters, Parameters::kOdomOpenVINSUseKLT(), params_->use_klt); + Parameters::parse(parameters, Parameters::kOdomOpenVINSNumPts(), params_->num_pts); + Parameters::parse(parameters, Parameters::kFASTThreshold(), params_->fast_threshold); + Parameters::parse(parameters, Parameters::kVisGridCols(), params_->grid_x); + Parameters::parse(parameters, Parameters::kVisGridRows(), params_->grid_y); + Parameters::parse(parameters, Parameters::kOdomOpenVINSMinPxDist(), params_->min_px_dist); + Parameters::parse(parameters, Parameters::kVisCorNNDR(), params_->knn_ratio); + Parameters::parse(parameters, Parameters::kOdomOpenVINSFiTriangulate1d(), params_->featinit_options.triangulate_1d); + Parameters::parse(parameters, Parameters::kOdomOpenVINSFiRefineFeatures(), params_->featinit_options.refine_features); + Parameters::parse(parameters, Parameters::kOdomOpenVINSFiMaxRuns(), params_->featinit_options.max_runs); + Parameters::parse(parameters, Parameters::kVisMinDepth(), params_->featinit_options.min_dist); + Parameters::parse(parameters, Parameters::kVisMaxDepth(), params_->featinit_options.max_dist); + if(params_->featinit_options.max_dist == 0) + params_->featinit_options.max_dist = std::numeric_limits::infinity(); + Parameters::parse(parameters, Parameters::kOdomOpenVINSFiMaxBaseline(), params_->featinit_options.max_baseline); + Parameters::parse(parameters, Parameters::kOdomOpenVINSFiMaxCondNumber(), params_->featinit_options.max_cond_number); + Parameters::parse(parameters, Parameters::kOdomOpenVINSUseFEJ(), params_->state_options.do_fej); + Parameters::parse(parameters, Parameters::kOdomOpenVINSIntegration(), enum_index); + params_->state_options.integration_method = ov_msckf::StateOptions::IntegrationMethod(enum_index); + Parameters::parse(parameters, Parameters::kOdomOpenVINSCalibCamExtrinsics(), params_->state_options.do_calib_camera_pose); + Parameters::parse(parameters, Parameters::kOdomOpenVINSCalibCamIntrinsics(), params_->state_options.do_calib_camera_intrinsics); + Parameters::parse(parameters, Parameters::kOdomOpenVINSCalibCamTimeoffset(), params_->state_options.do_calib_camera_timeoffset); + Parameters::parse(parameters, Parameters::kOdomOpenVINSCalibIMUIntrinsics(), params_->state_options.do_calib_imu_intrinsics); + Parameters::parse(parameters, Parameters::kOdomOpenVINSCalibIMUGSensitivity(), params_->state_options.do_calib_imu_g_sensitivity); + Parameters::parse(parameters, Parameters::kOdomOpenVINSMaxClones(), params_->state_options.max_clone_size); + Parameters::parse(parameters, Parameters::kOdomOpenVINSMaxSLAM(), params_->state_options.max_slam_features); + Parameters::parse(parameters, Parameters::kOdomOpenVINSMaxSLAMInUpdate(), params_->state_options.max_slam_in_update); + Parameters::parse(parameters, Parameters::kOdomOpenVINSMaxMSCKFInUpdate(), params_->state_options.max_msckf_in_update); + Parameters::parse(parameters, Parameters::kOdomOpenVINSFeatRepMSCKF(), enum_index); + params_->state_options.feat_rep_msckf = ov_type::LandmarkRepresentation::Representation(enum_index); + Parameters::parse(parameters, Parameters::kOdomOpenVINSFeatRepSLAM(), enum_index); + params_->state_options.feat_rep_slam = ov_type::LandmarkRepresentation::Representation(enum_index); + Parameters::parse(parameters, Parameters::kOdomOpenVINSDtSLAMDelay(), params_->dt_slam_delay); + Parameters::parse(parameters, Parameters::kOdomOpenVINSGravityMag(), params_->gravity_mag); + Parameters::parse(parameters, Parameters::kVisDepthAsMask(), params_->use_mask); + Parameters::parse(parameters, Parameters::kOdomOpenVINSLeftMaskPath(), left_mask_path); + if(!left_mask_path.empty()) + { + if(!UFile::exists(left_mask_path)) + UWARN("OpenVINS: invalid left mask path: %s", left_mask_path.c_str()); + else + params_->masks.emplace(0, cv::imread(left_mask_path, cv::IMREAD_GRAYSCALE)); + } + Parameters::parse(parameters, Parameters::kOdomOpenVINSRightMaskPath(), right_mask_path); + if(!right_mask_path.empty()) + { + if(!UFile::exists(right_mask_path)) + UWARN("OpenVINS: invalid right mask path: %s", right_mask_path.c_str()); + else + params_->masks.emplace(1, cv::imread(right_mask_path, cv::IMREAD_GRAYSCALE)); + } + Parameters::parse(parameters, Parameters::kOdomOpenVINSInitWindowTime(), params_->init_options.init_window_time); + Parameters::parse(parameters, Parameters::kOdomOpenVINSInitIMUThresh(), params_->init_options.init_imu_thresh); + Parameters::parse(parameters, Parameters::kOdomOpenVINSInitMaxDisparity(), params_->init_options.init_max_disparity); + Parameters::parse(parameters, Parameters::kOdomOpenVINSInitMaxFeatures(), params_->init_options.init_max_features); + Parameters::parse(parameters, Parameters::kOdomOpenVINSInitDynUse(), params_->init_options.init_dyn_use); + Parameters::parse(parameters, Parameters::kOdomOpenVINSInitDynMLEOptCalib(), params_->init_options.init_dyn_mle_opt_calib); + Parameters::parse(parameters, Parameters::kOdomOpenVINSInitDynMLEMaxIter(), params_->init_options.init_dyn_mle_max_iter); + Parameters::parse(parameters, Parameters::kOdomOpenVINSInitDynMLEMaxTime(), params_->init_options.init_dyn_mle_max_time); + Parameters::parse(parameters, Parameters::kOdomOpenVINSInitDynMLEMaxThreads(), params_->init_options.init_dyn_mle_max_threads); + Parameters::parse(parameters, Parameters::kOdomOpenVINSInitDynNumPose(), params_->init_options.init_dyn_num_pose); + Parameters::parse(parameters, Parameters::kOdomOpenVINSInitDynMinDeg(), params_->init_options.init_dyn_min_deg); + Parameters::parse(parameters, Parameters::kOdomOpenVINSInitDynInflationOri(), params_->init_options.init_dyn_inflation_orientation); + Parameters::parse(parameters, Parameters::kOdomOpenVINSInitDynInflationVel(), params_->init_options.init_dyn_inflation_velocity); + Parameters::parse(parameters, Parameters::kOdomOpenVINSInitDynInflationBg(), params_->init_options.init_dyn_inflation_bias_gyro); + Parameters::parse(parameters, Parameters::kOdomOpenVINSInitDynInflationBa(), params_->init_options.init_dyn_inflation_bias_accel); + Parameters::parse(parameters, Parameters::kOdomOpenVINSInitDynMinRecCond(), params_->init_options.init_dyn_min_rec_cond); + Parameters::parse(parameters, Parameters::kOdomOpenVINSTryZUPT(), params_->try_zupt); + Parameters::parse(parameters, Parameters::kOdomOpenVINSZUPTChi2Multiplier(), params_->zupt_options.chi2_multipler); + Parameters::parse(parameters, Parameters::kOdomOpenVINSZUPTMaxVelodicy(), params_->zupt_max_velocity); + Parameters::parse(parameters, Parameters::kOdomOpenVINSZUPTNoiseMultiplier(), params_->zupt_noise_multiplier); + Parameters::parse(parameters, Parameters::kOdomOpenVINSZUPTMaxDisparity(), params_->zupt_max_disparity); + Parameters::parse(parameters, Parameters::kOdomOpenVINSZUPTOnlyAtBeginning(), params_->zupt_only_at_beginning); + Parameters::parse(parameters, Parameters::kOdomOpenVINSAccelerometerNoiseDensity(), params_->imu_noises.sigma_a); + Parameters::parse(parameters, Parameters::kOdomOpenVINSAccelerometerRandomWalk(), params_->imu_noises.sigma_ab); + Parameters::parse(parameters, Parameters::kOdomOpenVINSGyroscopeNoiseDensity(), params_->imu_noises.sigma_w); + Parameters::parse(parameters, Parameters::kOdomOpenVINSGyroscopeRandomWalk(), params_->imu_noises.sigma_wb); + Parameters::parse(parameters, Parameters::kOdomOpenVINSUpMSCKFSigmaPx(), params_->msckf_options.sigma_pix); + Parameters::parse(parameters, Parameters::kOdomOpenVINSUpMSCKFChi2Multiplier(), params_->msckf_options.chi2_multipler); + Parameters::parse(parameters, Parameters::kOdomOpenVINSUpSLAMSigmaPx(), params_->slam_options.sigma_pix); + Parameters::parse(parameters, Parameters::kOdomOpenVINSUpSLAMChi2Multiplier(), params_->slam_options.chi2_multipler); + params_->vec_dw << 1, 0, 0, 1, 0, 1; + params_->vec_da << 1, 0, 0, 1, 0, 1; + params_->vec_tg << 0, 0, 0, 0, 0, 0, 0, 0, 0; + params_->q_ACCtoIMU << 0, 0, 0, 1; + params_->q_GYROtoIMU << 0, 0, 0, 1; + params_->use_aruco = false; + params_->num_opencv_threads = -1; + params_->histogram_method = ov_core::TrackBase::HistogramMethod::NONE; + params_->init_options.sigma_a = params_->imu_noises.sigma_a; + params_->init_options.sigma_ab = params_->imu_noises.sigma_ab; + params_->init_options.sigma_w = params_->imu_noises.sigma_w; + params_->init_options.sigma_wb = params_->imu_noises.sigma_wb; + params_->init_options.sigma_pix = params_->slam_options.sigma_pix; + params_->init_options.gravity_mag = params_->gravity_mag; #endif } @@ -72,11 +161,9 @@ void OdometryOpenVINS::reset(const Transform & initialPose) #ifdef RTABMAP_OPENVINS if(!initGravity_) { - delete vioManager_; - vioManager_ = 0; - previousPose_.setIdentity(); - previousLocalTransform_.setNull(); - imuBuffer_.clear(); + vioManager_.reset(); + previousPoseInv_.setIdentity(); + imuLocalTransformInv_.setNull(); } initGravity_ = false; #endif @@ -90,395 +177,309 @@ Transform OdometryOpenVINS::computeTransform( { Transform t; #ifdef RTABMAP_OPENVINS - UTimer timer; - - // Buffer imus; - if(!data.imu().empty()) - { - imuBuffer_.insert(std::make_pair(data.stamp(), data.imu())); - } - // OpenVINS has to buffer image before computing transformation with IMU stamp > image stamp - if(!data.imageRaw().empty() && !data.rightRaw().empty() && data.stereoCameraModels().size() == 1) + if(!vioManager_) { - if(imuBuffer_.empty()) + if(!data.imu().empty()) { - UWARN("Waiting IMU for initialization..."); - return t; + imuLocalTransformInv_ = data.imu().localTransform().inverse(); + Phi_.setZero(); + Phi_.block(0,0,3,3) = data.imu().localTransform().toEigen4d().block(0,0,3,3); + Phi_.block(3,3,3,3) = data.imu().localTransform().toEigen4d().block(0,0,3,3); } - if(vioManager_ == 0) - { - UINFO("OpenVINS Initialization"); - - // intialize - ov_msckf::VioManagerOptions params; - - // ESTIMATOR ====================================================================== - // Main EKF parameters - //params.state_options.do_fej = true; - //params.state_options.imu_avg =false; - //params.state_options.use_rk4_integration; - //params.state_options.do_calib_camera_pose = false; - //params.state_options.do_calib_camera_intrinsics = false; - //params.state_options.do_calib_camera_timeoffset = false; - //params.state_options.max_clone_size = 11; - //params.state_options.max_slam_features = 25; - //params.state_options.max_slam_in_update = INT_MAX; - //params.state_options.max_msckf_in_update = INT_MAX; - //params.state_options.max_aruco_features = 1024; - params.state_options.num_cameras = 2; - //params.dt_slam_delay = 2; - - // Set what representation we should be using - //params.state_options.feat_rep_msckf = LandmarkRepresentation::from_string("ANCHORED_MSCKF_INVERSE_DEPTH"); // default GLOBAL_3D - //params.state_options.feat_rep_slam = LandmarkRepresentation::from_string("ANCHORED_MSCKF_INVERSE_DEPTH"); // default GLOBAL_3D - //params.state_options.feat_rep_aruco = LandmarkRepresentation::from_string("ANCHORED_MSCKF_INVERSE_DEPTH"); // default GLOBAL_3D - if( params.state_options.feat_rep_msckf == LandmarkRepresentation::Representation::UNKNOWN || - params.state_options.feat_rep_slam == LandmarkRepresentation::Representation::UNKNOWN || - params.state_options.feat_rep_aruco == LandmarkRepresentation::Representation::UNKNOWN) + if(!data.imageRaw().empty() && !imuLocalTransformInv_.isNull()) + { + Transform T_imu_left; + Eigen::VectorXd left_calib(8), right_calib(8); + if(!data.rightRaw().empty()) { - printf(RED "VioManager(): invalid feature representation specified:\n" RESET); - printf(RED "\t- GLOBAL_3D\n" RESET); - printf(RED "\t- GLOBAL_FULL_INVERSE_DEPTH\n" RESET); - printf(RED "\t- ANCHORED_3D\n" RESET); - printf(RED "\t- ANCHORED_FULL_INVERSE_DEPTH\n" RESET); - printf(RED "\t- ANCHORED_MSCKF_INVERSE_DEPTH\n" RESET); - printf(RED "\t- ANCHORED_INVERSE_DEPTH_SINGLE\n" RESET); - std::exit(EXIT_FAILURE); - } - - // Filter initialization - //params.init_window_time = 1; - //params.init_imu_thresh = 1; - - // Zero velocity update - //params.try_zupt = false; - //params.zupt_options.chi2_multipler = 5; - //params.zupt_max_velocity = 1; - //params.zupt_noise_multiplier = 1; - - // NOISE ====================================================================== - - // Our noise values for inertial sensor - //params.imu_noises.sigma_w = 1.6968e-04; - //params.imu_noises.sigma_a = 2.0000e-3; - //params.imu_noises.sigma_wb = 1.9393e-05; - //params.imu_noises.sigma_ab = 3.0000e-03; - - // Read in update parameters - //params.msckf_options.sigma_pix = 1; - //params.msckf_options.chi2_multipler = 5; - //params.slam_options.sigma_pix = 1; - //params.slam_options.chi2_multipler = 5; - //params.aruco_options.sigma_pix = 1; - //params.aruco_options.chi2_multipler = 5; - - - // STATE ====================================================================== - - // Timeoffset from camera to IMU - //params.calib_camimu_dt = 0.0; - - // Global gravity - //params.gravity[2] = 9.81; - - - // TRACKERS ====================================================================== + params_->state_options.num_cameras = params_->init_options.num_cameras = 2; + T_imu_left = imuLocalTransformInv_ * data.stereoCameraModels()[0].localTransform(); - // Tracking flags - params.use_stereo = true; - //params.use_klt = true; - params.use_aruco = false; - //params.downsize_aruco = true; - //params.downsample_cameras = false; - //params.use_multi_threading = true; - - // General parameters - //params.num_pts = 200; - //params.fast_threshold = 10; - //params.grid_x = 10; - //params.grid_y = 5; - //params.min_px_dist = 8; - //params.knn_ratio = 0.7; - - // Feature initializer parameters - //nh.param("fi_triangulate_1d", params.featinit_options.triangulate_1d, params.featinit_options.triangulate_1d); - //nh.param("fi_refine_features", params.featinit_options.refine_features, params.featinit_options.refine_features); - //nh.param("fi_max_runs", params.featinit_options.max_runs, params.featinit_options.max_runs); - //nh.param("fi_init_lamda", params.featinit_options.init_lamda, params.featinit_options.init_lamda); - //nh.param("fi_max_lamda", params.featinit_options.max_lamda, params.featinit_options.max_lamda); - //nh.param("fi_min_dx", params.featinit_options.min_dx, params.featinit_options.min_dx); - ///nh.param("fi_min_dcost", params.featinit_options.min_dcost, params.featinit_options.min_dcost); - //nh.param("fi_lam_mult", params.featinit_options.lam_mult, params.featinit_options.lam_mult); - //nh.param("fi_min_dist", params.featinit_options.min_dist, params.featinit_options.min_dist); - //params.featinit_options.max_dist = 75; - //params.featinit_options.max_baseline = 500; - //params.featinit_options.max_cond_number = 5000; - - - // CAMERA ====================================================================== - bool fisheye = data.stereoCameraModels()[0].left().isFisheye() && !this->imagesAlreadyRectified(); - params.camera_fisheye.insert(std::make_pair(0, fisheye)); - params.camera_fisheye.insert(std::make_pair(1, fisheye)); + bool is_fisheye = data.stereoCameraModels()[0].left().isFisheye() && !this->imagesAlreadyRectified(); + if(is_fisheye) + { + params_->camera_intrinsics.emplace(0, std::make_shared( + data.stereoCameraModels()[0].left().imageWidth(), data.stereoCameraModels()[0].left().imageHeight())); + params_->camera_intrinsics.emplace(1, std::make_shared( + data.stereoCameraModels()[0].right().imageWidth(), data.stereoCameraModels()[0].right().imageHeight())); + } + else + { + params_->camera_intrinsics.emplace(0, std::make_shared( + data.stereoCameraModels()[0].left().imageWidth(), data.stereoCameraModels()[0].left().imageHeight())); + params_->camera_intrinsics.emplace(1, std::make_shared( + data.stereoCameraModels()[0].right().imageWidth(), data.stereoCameraModels()[0].right().imageHeight())); + } - Eigen::VectorXd camLeft(8), camRight(8); - if(this->imagesAlreadyRectified() || data.stereoCameraModels()[0].left().D_raw().empty()) - { - camLeft << data.stereoCameraModels()[0].left().fx(), - data.stereoCameraModels()[0].left().fy(), - data.stereoCameraModels()[0].left().cx(), - data.stereoCameraModels()[0].left().cy(), 0, 0, 0, 0; - camRight << data.stereoCameraModels()[0].right().fx(), - data.stereoCameraModels()[0].right().fy(), - data.stereoCameraModels()[0].right().cx(), - data.stereoCameraModels()[0].right().cy(), 0, 0, 0, 0; + if(this->imagesAlreadyRectified() || data.stereoCameraModels()[0].left().D_raw().empty()) + { + left_calib << data.stereoCameraModels()[0].left().fx(), + data.stereoCameraModels()[0].left().fy(), + data.stereoCameraModels()[0].left().cx(), + data.stereoCameraModels()[0].left().cy(), 0, 0, 0, 0; + right_calib << data.stereoCameraModels()[0].right().fx(), + data.stereoCameraModels()[0].right().fy(), + data.stereoCameraModels()[0].right().cx(), + data.stereoCameraModels()[0].right().cy(), 0, 0, 0, 0; + } + else + { + UASSERT(data.stereoCameraModels()[0].left().D_raw().cols == data.stereoCameraModels()[0].right().D_raw().cols); + UASSERT(data.stereoCameraModels()[0].left().D_raw().cols >= 4); + UASSERT(data.stereoCameraModels()[0].right().D_raw().cols >= 4); + left_calib << data.stereoCameraModels()[0].left().K_raw().at(0,0), + data.stereoCameraModels()[0].left().K_raw().at(1,1), + data.stereoCameraModels()[0].left().K_raw().at(0,2), + data.stereoCameraModels()[0].left().K_raw().at(1,2), + data.stereoCameraModels()[0].left().D_raw().at(0,0), + data.stereoCameraModels()[0].left().D_raw().at(0,1), + data.stereoCameraModels()[0].left().D_raw().at(0,is_fisheye?4:2), + data.stereoCameraModels()[0].left().D_raw().at(0,is_fisheye?5:3); + right_calib << data.stereoCameraModels()[0].right().K_raw().at(0,0), + data.stereoCameraModels()[0].right().K_raw().at(1,1), + data.stereoCameraModels()[0].right().K_raw().at(0,2), + data.stereoCameraModels()[0].right().K_raw().at(1,2), + data.stereoCameraModels()[0].right().D_raw().at(0,0), + data.stereoCameraModels()[0].right().D_raw().at(0,1), + data.stereoCameraModels()[0].right().D_raw().at(0,is_fisheye?4:2), + data.stereoCameraModels()[0].right().D_raw().at(0,is_fisheye?5:3); + } } else { - UASSERT(data.stereoCameraModels()[0].left().D_raw().cols == data.stereoCameraModels()[0].right().D_raw().cols); - UASSERT(data.stereoCameraModels()[0].left().D_raw().cols >= 4); - UASSERT(data.stereoCameraModels()[0].right().D_raw().cols >= 4); + params_->state_options.num_cameras = params_->init_options.num_cameras = 1; + T_imu_left = imuLocalTransformInv_ * data.cameraModels()[0].localTransform(); - //https://github.com/ethz-asl/kalibr/wiki/supported-models - /// radial-tangential (radtan) - // (distortion_coeffs: [k1 k2 r1 r2]) - /// equidistant (equi) - // (distortion_coeffs: [k1 k2 k3 k4]) rtabmap: (k1,k2,p1,p2,k3,k4) + bool is_fisheye = data.cameraModels()[0].isFisheye() && !this->imagesAlreadyRectified(); + if(is_fisheye) + { + params_->camera_intrinsics.emplace(0, std::make_shared( + data.cameraModels()[0].imageWidth(), data.cameraModels()[0].imageHeight())); + } + else + { + params_->camera_intrinsics.emplace(0, std::make_shared( + data.cameraModels()[0].imageWidth(), data.cameraModels()[0].imageHeight())); + } - camLeft << - data.stereoCameraModels()[0].left().K_raw().at(0,0), - data.stereoCameraModels()[0].left().K_raw().at(1,1), - data.stereoCameraModels()[0].left().K_raw().at(0,2), - data.stereoCameraModels()[0].left().K_raw().at(1,2), - data.stereoCameraModels()[0].left().D_raw().at(0,0), - data.stereoCameraModels()[0].left().D_raw().at(0,1), - data.stereoCameraModels()[0].left().D_raw().at(0,fisheye?4:2), - data.stereoCameraModels()[0].left().D_raw().at(0,fisheye?5:3); - camRight << - data.stereoCameraModels()[0].right().K_raw().at(0,0), - data.stereoCameraModels()[0].right().K_raw().at(1,1), - data.stereoCameraModels()[0].right().K_raw().at(0,2), - data.stereoCameraModels()[0].right().K_raw().at(1,2), - data.stereoCameraModels()[0].right().D_raw().at(0,0), - data.stereoCameraModels()[0].right().D_raw().at(0,1), - data.stereoCameraModels()[0].right().D_raw().at(0,fisheye?4:2), - data.stereoCameraModels()[0].right().D_raw().at(0,fisheye?5:3); + if(this->imagesAlreadyRectified() || data.cameraModels()[0].D_raw().empty()) + { + left_calib << data.cameraModels()[0].fx(), + data.cameraModels()[0].fy(), + data.cameraModels()[0].cx(), + data.cameraModels()[0].cy(), 0, 0, 0, 0; + } + else + { + UASSERT(data.cameraModels()[0].D_raw().cols >= 4); + left_calib << data.cameraModels()[0].K_raw().at(0,0), + data.cameraModels()[0].K_raw().at(1,1), + data.cameraModels()[0].K_raw().at(0,2), + data.cameraModels()[0].K_raw().at(1,2), + data.cameraModels()[0].D_raw().at(0,0), + data.cameraModels()[0].D_raw().at(0,1), + data.cameraModels()[0].D_raw().at(0,is_fisheye?4:2), + data.cameraModels()[0].D_raw().at(0,is_fisheye?5:3); + } } - params.camera_intrinsics.insert(std::make_pair(0, camLeft)); - params.camera_intrinsics.insert(std::make_pair(1, camRight)); - const IMU & imu = imuBuffer_.begin()->second; - imuLocalTransform_ = imu.localTransform(); - Transform imuCam0 = imuLocalTransform_.inverse() * data.stereoCameraModels()[0].localTransform(); - Transform cam0cam1; - if(this->imagesAlreadyRectified() || data.stereoCameraModels()[0].stereoTransform().isNull()) + Eigen::Matrix4d T_LtoI = T_imu_left.toEigen4d(); + Eigen::Matrix left_eigen; + left_eigen.block(0,0,4,1) = ov_core::rot_2_quat(T_LtoI.block(0,0,3,3).transpose()); + left_eigen.block(4,0,3,1) = -T_LtoI.block(0,0,3,3).transpose()*T_LtoI.block(0,3,3,1); + params_->camera_intrinsics.at(0)->set_value(left_calib); + params_->camera_extrinsics.emplace(0, left_eigen); + if(!data.rightRaw().empty()) { - cam0cam1 = Transform( + Transform T_left_right; + if(this->imagesAlreadyRectified() || data.stereoCameraModels()[0].stereoTransform().isNull()) + { + T_left_right = Transform( 1, 0, 0, data.stereoCameraModels()[0].baseline(), 0, 1, 0, 0, 0, 0, 1, 0); + } + else + { + T_left_right = data.stereoCameraModels()[0].stereoTransform().inverse(); + } + UASSERT(!T_left_right.isNull()); + Transform T_imu_right = T_imu_left * T_left_right; + Eigen::Matrix4d T_RtoI = T_imu_right.toEigen4d(); + Eigen::Matrix right_eigen; + right_eigen.block(0,0,4,1) = ov_core::rot_2_quat(T_RtoI.block(0,0,3,3).transpose()); + right_eigen.block(4,0,3,1) = -T_RtoI.block(0,0,3,3).transpose()*T_RtoI.block(0,3,3,1); + params_->camera_intrinsics.at(1)->set_value(right_calib); + params_->camera_extrinsics.emplace(1, right_eigen); } - else - { - cam0cam1 = data.stereoCameraModels()[0].stereoTransform().inverse(); - } - UASSERT(!cam0cam1.isNull()); - Transform imuCam1 = imuCam0 * cam0cam1; - Eigen::Matrix4d cam0_eigen = imuCam0.toEigen4d(); - Eigen::Matrix4d cam1_eigen = imuCam1.toEigen4d(); - Eigen::Matrix cam_eigen0; - cam_eigen0.block(0,0,4,1) = rot_2_quat(cam0_eigen.block(0,0,3,3).transpose()); - cam_eigen0.block(4,0,3,1) = -cam0_eigen.block(0,0,3,3).transpose()*cam0_eigen.block(0,3,3,1); - Eigen::Matrix cam_eigen1; - cam_eigen1.block(0,0,4,1) = rot_2_quat(cam1_eigen.block(0,0,3,3).transpose()); - cam_eigen1.block(4,0,3,1) = -cam1_eigen.block(0,0,3,3).transpose()*cam1_eigen.block(0,3,3,1); - params.camera_extrinsics.insert(std::make_pair(0, cam_eigen0)); - params.camera_extrinsics.insert(std::make_pair(1, cam_eigen1)); - - params.camera_wh.insert({0, std::make_pair(data.stereoCameraModels()[0].left().imageWidth(),data.stereoCameraModels()[0].left().imageHeight())}); - params.camera_wh.insert({1, std::make_pair(data.stereoCameraModels()[0].right().imageWidth(),data.stereoCameraModels()[0].right().imageHeight())}); - - vioManager_ = new ov_msckf::VioManager(params); - } - - cv::Mat left; - cv::Mat right; - if(data.imageRaw().type() == CV_8UC3) - { - cv::cvtColor(data.imageRaw(), left, CV_BGR2GRAY); + params_->init_options.camera_intrinsics = params_->camera_intrinsics; + params_->init_options.camera_extrinsics = params_->camera_extrinsics; + vioManager_ = std::make_unique(*params_); } - else if(data.imageRaw().type() == CV_8UC1) - { - left = data.imageRaw().clone(); - } - else - { - UFATAL("Not supported color type!"); - } - if(data.rightRaw().type() == CV_8UC3) - { - cv::cvtColor(data.rightRaw(), right, CV_BGR2GRAY); - } - else if(data.rightRaw().type() == CV_8UC1) - { - right = data.rightRaw().clone(); - } - else - { - UFATAL("Not supported color type!"); - } - - // Create the measurement - ov_core::CameraData message; - message.timestamp = data.stamp(); - message.sensor_ids.push_back(0); - message.sensor_ids.push_back(1); - message.images.push_back(left); - message.images.push_back(right); - message.masks.push_back(cv::Mat::zeros(left.size(), CV_8UC1)); - message.masks.push_back(cv::Mat::zeros(right.size(), CV_8UC1)); - - // send it to our VIO system - vioManager_->feed_measurement_camera(message); - UDEBUG("Image update stamp=%f", data.stamp()); - - double lastIMUstamp = 0.0; - while(!imuBuffer_.empty()) + } + else + { + if(!data.imu().empty()) { - std::map::iterator iter = imuBuffer_.begin(); - - // Process IMU data until stamp is over image stamp ov_core::ImuData message; - message.timestamp = iter->first; - message.wm << iter->second.angularVelocity().val[0], iter->second.angularVelocity().val[1], iter->second.angularVelocity().val[2]; - message.am << iter->second.linearAcceleration().val[0], iter->second.linearAcceleration().val[1], iter->second.linearAcceleration().val[2]; - - UDEBUG("IMU update stamp=%f", message.timestamp); - - // send it to our VIO system + message.timestamp = data.stamp(); + message.wm << data.imu().angularVelocity().val[0], data.imu().angularVelocity().val[1], data.imu().angularVelocity().val[2]; + message.am << data.imu().linearAcceleration().val[0], data.imu().linearAcceleration().val[1], data.imu().linearAcceleration().val[2]; vioManager_->feed_measurement_imu(message); - - lastIMUstamp = iter->first; - - imuBuffer_.erase(iter); - - if(lastIMUstamp > data.stamp()) - { - break; - } } - if(vioManager_->initialized()) + if(!data.imageRaw().empty()) { - // Get the current state - std::shared_ptr state = vioManager_->get_state(); - - if(state->_timestamp != data.stamp()) + bool covFilled = false; + Eigen::Matrix state_plus = Eigen::Matrix::Zero(); + Eigen::Matrix cov_plus = Eigen::Matrix::Zero(); + if(vioManager_->initialized()) + covFilled = vioManager_->get_propagator()->fast_state_propagate(vioManager_->get_state(), data.stamp(), state_plus, cov_plus); + + cv::Mat image; + if(data.imageRaw().type() == CV_8UC3) + cv::cvtColor(data.imageRaw(), image, CV_BGR2GRAY); + else if(data.imageRaw().type() == CV_8UC1) + image = data.imageRaw().clone(); + else + UFATAL("Not supported color type!"); + ov_core::CameraData message; + message.timestamp = data.stamp(); + message.sensor_ids.emplace_back(0); + message.images.emplace_back(image); + if(params_->masks.find(0) != params_->masks.end()) { - UWARN("OpenVINS: Stamp of the current state %f is not the same " - "than last image processed %f (last IMU stamp=%f). There could be " - "a synchronization issue between camera and IMU. ", - state->_timestamp, - data.stamp(), - lastIMUstamp); + message.masks.emplace_back(params_->masks[0]); } - - Transform p( - (float)state->_imu->pos()(0), - (float)state->_imu->pos()(1), - (float)state->_imu->pos()(2), - (float)state->_imu->quat()(0), - (float)state->_imu->quat()(1), - (float)state->_imu->quat()(2), - (float)state->_imu->quat()(3)); - - - // Finally set the covariance in the message (in the order position then orientation as per ros convention) - std::vector> statevars; - statevars.push_back(state->_imu->pose()->p()); - statevars.push_back(state->_imu->pose()->q()); - - cv::Mat covariance = cv::Mat::eye(6,6, CV_64FC1); - if(this->framesProcessed() == 0) + else if(!data.depthRaw().empty() && params_->use_mask) { - covariance *= 9999; + cv::Mat mask; + if(data.depthRaw().type() == CV_32FC1) + cv::inRange(data.depthRaw(), params_->featinit_options.min_dist, + std::isinf(params_->featinit_options.max_dist)?std::numeric_limits::max():params_->featinit_options.max_dist, mask); + else if(data.depthRaw().type() == CV_16UC1) + cv::inRange(data.depthRaw(), params_->featinit_options.min_dist*1000, + std::isinf(params_->featinit_options.max_dist)?std::numeric_limits::max():params_->featinit_options.max_dist*1000, mask); + message.masks.emplace_back(255-mask); } else { - Eigen::Matrix covariance_posori = ov_msckf::StateHelper::get_marginal_covariance(vioManager_->get_state(),statevars); - for(int r=0; r<6; r++) { - for(int c=0; c<6; c++) { - ((double *)covariance.data)[6*r+c] = covariance_posori(r,c); - } - } + message.masks.emplace_back(cv::Mat::zeros(image.size(), CV_8UC1)); } - - if(!p.isNull()) + if(!data.rightRaw().empty()) { - p = p * imuLocalTransform_.inverse(); + if(data.rightRaw().type() == CV_8UC3) + cv::cvtColor(data.rightRaw(), image, CV_BGR2GRAY); + else if(data.rightRaw().type() == CV_8UC1) + image = data.rightRaw().clone(); + else + UFATAL("Not supported color type!"); + message.sensor_ids.emplace_back(1); + message.images.emplace_back(image); + if(params_->masks.find(1) != params_->masks.end()) + message.masks.emplace_back(params_->masks[1]); + else + message.masks.emplace_back(cv::Mat::zeros(image.size(), CV_8UC1)); + } + vioManager_->feed_measurement_camera(message); - if(this->getPose().rotation().isIdentity()) + if(vioManager_->initialized()) + { + std::shared_ptr state = vioManager_->get_state(); + Transform p((float)state->_imu->pos()(0), + (float)state->_imu->pos()(1), + (float)state->_imu->pos()(2), + (float)state->_imu->quat()(0), + (float)state->_imu->quat()(1), + (float)state->_imu->quat()(2), + (float)state->_imu->quat()(3)); + if(!p.isNull()) { - initGravity_ = true; - this->reset(this->getPose()*p.rotation()); - } + p = p * imuLocalTransformInv_; - if(previousPose_.isIdentity()) - { - previousPose_ = p; - } + if(this->getPose().rotation().isIdentity()) + { + initGravity_ = true; + this->reset(this->getPose() * p.rotation()); + } - // make it incremental - Transform previousPoseInv = previousPose_.inverse(); - t = previousPoseInv*p; - previousPose_ = p; + if(previousPoseInv_.isIdentity()) + previousPoseInv_ = p.inverse(); - if(info) - { - info->type = this->getType(); - info->reg.covariance = covariance; + t = previousPoseInv_ * p; - // feature map - Transform fixT = this->getPose()*previousPoseInv; - Transform camLocalTransformInv = data.stereoCameraModels()[0].localTransform().inverse()*this->getPose().inverse(); - for (auto &it_per_id : vioManager_->get_features_SLAM()) + if(info) { - cv::Point3f pt3d; - pt3d.x = it_per_id[0]; - pt3d.y = it_per_id[1]; - pt3d.z = it_per_id[2]; - pt3d = util3d::transformPoint(pt3d, fixT); - info->localMap.insert(std::make_pair(info->localMap.size(), pt3d)); + double timestamp; + std::unordered_map feat_posinG, feat_tracks_uvd; + vioManager_->get_active_tracks(timestamp, feat_posinG, feat_tracks_uvd); + auto features_SLAM = vioManager_->get_features_SLAM(); + auto good_features_MSCKF = vioManager_->get_good_features_MSCKF(); + + info->type = this->getType(); + info->localMapSize = feat_posinG.size(); + info->features = features_SLAM.size() + good_features_MSCKF.size(); + info->reg.covariance = cv::Mat::eye(6, 6, CV_64FC1); + if(covFilled) + { + Eigen::Matrix covariance = Phi_ * cov_plus.block(6,6,6,6) * Phi_.transpose(); + cv::eigen2cv(covariance, info->reg.covariance); + } - if(this->imagesAlreadyRectified()) + if(this->isInfoDataFilled()) { - cv::Point2f pt; - pt3d = util3d::transformPoint(pt3d, camLocalTransformInv); - data.stereoCameraModels()[0].left().reproject(pt3d.x, pt3d.y, pt3d.z, pt.x, pt.y); - info->reg.inliersIDs.push_back(info->newCorners.size()); - info->newCorners.push_back(pt); + Transform fixT = this->getPose() * previousPoseInv_; + Transform camT; + if(!data.rightRaw().empty()) + camT = data.stereoCameraModels()[0].localTransform().inverse() * t.inverse() * this->getPose().inverse() * fixT; + else + camT = data.cameraModels()[0].localTransform().inverse() * t.inverse() * this->getPose().inverse() * fixT; + + for(auto &feature : feat_posinG) + { + cv::Point3f pt3d(feature.second[0], feature.second[1], feature.second[2]); + pt3d = util3d::transformPoint(pt3d, fixT); + info->localMap.emplace(feature.first, pt3d); + } + + if(this->imagesAlreadyRectified()) + { + for(auto &feature : features_SLAM) + { + cv::Point3f pt3d(feature[0], feature[1], feature[2]); + pt3d = util3d::transformPoint(pt3d, camT); + cv::Point2f pt; + if(!data.rightRaw().empty()) + data.stereoCameraModels()[0].left().reproject(pt3d.x, pt3d.y, pt3d.z, pt.x, pt.y); + else + data.cameraModels()[0].reproject(pt3d.x, pt3d.y, pt3d.z, pt.x, pt.y); + info->reg.inliersIDs.emplace_back(info->newCorners.size()); + info->newCorners.emplace_back(pt); + } + + for(auto &feature : good_features_MSCKF) + { + cv::Point3f pt3d(feature[0], feature[1], feature[2]); + pt3d = util3d::transformPoint(pt3d, camT); + cv::Point2f pt; + if(!data.rightRaw().empty()) + data.stereoCameraModels()[0].left().reproject(pt3d.x, pt3d.y, pt3d.z, pt.x, pt.y); + else + data.cameraModels()[0].reproject(pt3d.x, pt3d.y, pt3d.z, pt.x, pt.y); + info->reg.matchesIDs.emplace_back(info->newCorners.size()); + info->newCorners.emplace_back(pt); + } + } } } - info->features = info->newCorners.size(); - info->localMapSize = info->localMap.size(); + + previousPoseInv_ = p.inverse(); } - UINFO("Odom update time = %fs p=%s", timer.elapsed(), p.prettyPrint().c_str()); } } } - else if(!data.imageRaw().empty() && !data.depthRaw().empty()) - { - UERROR("OpenVINS doesn't work with RGB-D data, stereo images are required!"); - } - else if(!data.imageRaw().empty() && data.depthOrRightRaw().empty()) - { - UERROR("OpenVINS requires stereo images!"); - } - else - { - UERROR("OpenVINS requires stereo images (only one stereo camera and should be calibrated)!"); - } #else UERROR("RTAB-Map is not built with OpenVINS support! Select another visual odometry approach."); diff --git a/corelib/src/odometry/OdometryVINS.cpp b/corelib/src/odometry/OdometryVINS.cpp index 534ed25009..aa18393c04 100644 --- a/corelib/src/odometry/OdometryVINS.cpp +++ b/corelib/src/odometry/OdometryVINS.cpp @@ -503,7 +503,7 @@ Transform OdometryVINS::computeTransform( { UERROR("VINS-Fusion requires stereo images!"); } - else + else if(data.imu().empty()) { UERROR("VINS-Fusion requires stereo images (and only one stereo camera with valid calibration)!"); } diff --git a/corelib/src/optimizer/OptimizerCVSBA.cpp b/corelib/src/optimizer/OptimizerCVSBA.cpp index d37f1fbe1f..6f0dcf3200 100644 --- a/corelib/src/optimizer/OptimizerCVSBA.cpp +++ b/corelib/src/optimizer/OptimizerCVSBA.cpp @@ -87,23 +87,29 @@ std::map OptimizerCVSBA::optimizeBA( for(std::map::const_iterator iter=poses.begin(); iter!=poses.end(); ++iter) { // Get camera model - std::map::const_iterator iterModel = models.find(iter->first); - UASSERT(iterModel != models.end() && iterModel->second.isValidForProjection()); + std::map >::const_iterator iterModel = models.find(iter->first); + UASSERT(iterModel != models.end()); + if(iterModel->second.size() != 1) + { + UERROR("Multi-camera BA not implemented for cvsba, only single camera."); + return std::map(); + } + UASSERT(iterModel->second[0].isValidForProjection()); frameIdToIndex.insert(std::make_pair(iter->first, oi)); - cameraMatrix[oi] = iterModel->second.K(); - if(iterModel->second.D().cols != 5) + cameraMatrix[oi] = iterModel->second[0].K(); + if(iterModel->second[0].D().cols != 5) { distCoeffs[oi] = cv::Mat::zeros(1, 5, CV_64FC1); UWARN("Camera model %d: Distortion coefficients are not 5, setting all them to 0 (assuming no distortion)", iter->first); } else { - distCoeffs[oi] = iterModel->second.D(); + distCoeffs[oi] = iterModel->second[0].D(); } - Transform t = (iter->second * iterModel->second.localTransform()).inverse(); + Transform t = (iter->second * iterModel->second[0].localTransform()).inverse(); R[oi] = (cv::Mat_(3,3) << (double)t.r11(), (double)t.r12(), (double)t.r13(), @@ -172,13 +178,13 @@ std::map OptimizerCVSBA::optimizeBA( if(this->isSlam2d()) { - t = (models.at(iter->first).localTransform() * t).inverse(); + t = (models.at(iter->first)[0].localTransform() * t).inverse(); t = iter->second.inverse() * t; iter->second *= t.to3DoF(); } else { - iter->second = (models.at(iter->first).localTransform() * t).inverse(); + iter->second = (models.at(iter->first)[0].localTransform() * t).inverse(); } ++i; diff --git a/corelib/src/optimizer/OptimizerCeres.cpp b/corelib/src/optimizer/OptimizerCeres.cpp index b4e9a8b7ea..5a62283a83 100644 --- a/corelib/src/optimizer/OptimizerCeres.cpp +++ b/corelib/src/optimizer/OptimizerCeres.cpp @@ -238,8 +238,9 @@ std::map OptimizerCeres::optimize( UINFO("Ceres optimizing begin (iterations=%d)", iterations()); ceres::Solver::Options options; + options.linear_solver_type = ceres::ITERATIVE_SCHUR; + options.sparse_linear_algebra_library_type = ceres::SUITE_SPARSE; options.max_num_iterations = iterations(); - options.linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY; options.function_tolerance = this->epsilon(); ceres::Solver::Summary summary; UTimer timer; @@ -317,7 +318,7 @@ std::map OptimizerCeres::optimizeBA( int rootId, const std::map & posesIn, const std::multimap & links, - const std::map & models, + const std::map > & models, std::map & points3DMap, const std::map > & wordReferences, // ) std::set * outliers) @@ -354,10 +355,16 @@ std::map OptimizerCeres::optimizeBA( ++iter) { // Get camera model - std::map::const_iterator iterModel = models.find(iter->first); - UASSERT(iterModel != models.end() && iterModel->second.isValidForProjection()); + std::map >::const_iterator iterModel = models.find(iter->first); + UASSERT(iterModel != models.end()); + if(iterModel->second.size() != 1) + { + UERROR("Multi-camera BA not implemented for Ceres, only single camera."); + return std::map(); + } + UASSERT(iterModel->second[0].isValidForProjection()); - const Transform & t = (iter->second * iterModel->second.localTransform()).inverse(); + const Transform & t = (iter->second * iterModel->second[0].localTransform()).inverse(); cv::Mat R = (cv::Mat_(3,3) << (double)t.r11(), (double)t.r12(), (double)t.r13(), (double)t.r21(), (double)t.r22(), (double)t.r23(), @@ -403,15 +410,21 @@ std::map OptimizerCeres::optimizeBA( jter!=iter->second.end(); ++jter) { - std::map::const_iterator iterModel = models.find(jter->first); - UASSERT(iterModel != models.end() && iterModel->second.isValidForProjection()); + std::map >::const_iterator iterModel = models.find(jter->first); + UASSERT(iterModel != models.end()); + if(iterModel->second.size() != 1) + { + UERROR("Multi-camera BA not implemented for Ceres, only single camera."); + return std::map(); + } + UASSERT(iterModel->second[0].isValidForProjection()); baProblem.camera_index_[oi] = camIdToIndex.at(jter->first); baProblem.point_index_[oi] = pointIdToIndex.at(iter->first); - baProblem.observations_[4*oi] = jter->second.kpt.pt.x - iterModel->second.cx(); - baProblem.observations_[4*oi+1] = jter->second.kpt.pt.y - iterModel->second.cy(); - baProblem.observations_[4*oi+2] = iterModel->second.fx(); - baProblem.observations_[4*oi+3] = iterModel->second.fy(); + baProblem.observations_[4*oi] = jter->second.kpt.pt.x - iterModel->second[0].cx(); + baProblem.observations_[4*oi+1] = jter->second.kpt.pt.y - iterModel->second[0].cy(); + baProblem.observations_[4*oi+2] = iterModel->second[0].fx(); + baProblem.observations_[4*oi+3] = iterModel->second[0].fy(); ++oi; } } @@ -445,7 +458,8 @@ std::map OptimizerCeres::optimizeBA( // standard solver, SPARSE_NORMAL_CHOLESKY, also works fine but it is slower // for standard bundle adjustment problems. ceres::Solver::Options options; - options.linear_solver_type = ceres::DENSE_SCHUR; + options.linear_solver_type = ceres::ITERATIVE_SCHUR; + options.sparse_linear_algebra_library_type = ceres::SUITE_SPARSE; options.max_num_iterations = iterations(); //options.linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY; options.function_tolerance = this->epsilon(); @@ -480,13 +494,13 @@ std::map OptimizerCeres::optimizeBA( if(this->isSlam2d()) { - t = (models.at(iter->first).localTransform() * t).inverse(); + t = (models.at(iter->first)[0].localTransform() * t).inverse(); t = iter->second.inverse() * t; iter->second *= t.to3DoF(); } else { - iter->second = (models.at(iter->first).localTransform() * t).inverse(); + iter->second = (models.at(iter->first)[0].localTransform() * t).inverse(); } } diff --git a/corelib/src/optimizer/OptimizerG2O.cpp b/corelib/src/optimizer/OptimizerG2O.cpp index 753e548f94..2da2f3e5e6 100644 --- a/corelib/src/optimizer/OptimizerG2O.cpp +++ b/corelib/src/optimizer/OptimizerG2O.cpp @@ -153,8 +153,8 @@ OptimizerG2O::OptimizerG2O(const ParametersMap & parameters) : // Issue on android, have to explicitly register this type when using fixed root prior below if(!g2o::Factory::instance()->knowsTag("CACHE_SE3_OFFSET")) { -#if defined(RTABMAP_G2O_CPP11) and RTABMAP_G2O_CPP11 == 1 - g2o::Factory::instance()->registerType("CACHE_SE3_OFFSET", g2o::make_unique >()); +#if defined(RTABMAP_G2O_CPP11) && RTABMAP_G2O_CPP11 == 1 + g2o::Factory::instance()->registerType("CACHE_SE3_OFFSET", std::make_unique >()); #else g2o::Factory::instance()->registerType("CACHE_SE3_OFFSET", new g2o::HyperGraphElementCreator); #endif @@ -250,17 +250,17 @@ std::map OptimizerG2O::optimize( if(solver_ == 3) { //eigen - auto linearSolver = g2o::make_unique(); + auto linearSolver = std::make_unique(); linearSolver->setBlockOrdering(false); - blockSolver = g2o::make_unique(std::move(linearSolver)); + blockSolver = std::make_unique(std::move(linearSolver)); } #ifdef G2O_HAVE_CHOLMOD else if(solver_ == 2) { //chmold - auto linearSolver = g2o::make_unique(); + auto linearSolver = std::make_unique(); linearSolver->setBlockOrdering(false); - blockSolver = g2o::make_unique(std::move(linearSolver)); + blockSolver = std::make_unique(std::move(linearSolver)); } #endif #ifdef G2O_HAVE_CSPARSE @@ -268,16 +268,16 @@ std::map OptimizerG2O::optimize( { //csparse - auto linearSolver = g2o::make_unique(); + auto linearSolver = std::make_unique(); linearSolver->setBlockOrdering(false); - blockSolver = g2o::make_unique(std::move(linearSolver)); + blockSolver = std::make_unique(std::move(linearSolver)); } #endif else { //pcg - auto linearSolver = g2o::make_unique(); - blockSolver = g2o::make_unique(std::move(linearSolver)); + auto linearSolver = std::make_unique(); + blockSolver = std::make_unique(std::move(linearSolver)); } if(optimizer_ == 1) @@ -1414,7 +1414,7 @@ std::map OptimizerG2O::optimizeBA( { g2o::SparseOptimizer optimizer; //optimizer.setVerbose(ULogger::level()==ULogger::kDebug); -#if defined(RTABMAP_G2O_CPP11) and not defined(RTABMAP_ORB_SLAM) +#if defined(RTABMAP_G2O_CPP11) && !defined(RTABMAP_ORB_SLAM) std::unique_ptr linearSolver; #else g2o::BlockSolver_6_3::LinearSolverType * linearSolver = 0; @@ -1427,7 +1427,7 @@ std::map OptimizerG2O::optimizeBA( { //eigen #ifdef RTABMAP_G2O_CPP11 - linearSolver = g2o::make_unique >(); + linearSolver = std::make_unique >(); #else linearSolver = new g2o::LinearSolverEigen(); #endif @@ -1437,7 +1437,7 @@ std::map OptimizerG2O::optimizeBA( { //chmold #ifdef RTABMAP_G2O_CPP11 - linearSolver = g2o::make_unique >(); + linearSolver = std::make_unique >(); #else linearSolver = new g2o::LinearSolverCholmod(); #endif @@ -1448,7 +1448,7 @@ std::map OptimizerG2O::optimizeBA( { //csparse #ifdef RTABMAP_G2O_CPP11 - linearSolver = g2o::make_unique >(); + linearSolver = std::make_unique >(); #else linearSolver = new g2o::LinearSolverCSparse(); #endif @@ -1458,7 +1458,7 @@ std::map OptimizerG2O::optimizeBA( { //pcg #ifdef RTABMAP_G2O_CPP11 - linearSolver = g2o::make_unique >(); + linearSolver = std::make_unique >(); #else linearSolver = new g2o::LinearSolverPCG(); #endif @@ -1470,7 +1470,7 @@ std::map OptimizerG2O::optimizeBA( { #ifdef RTABMAP_G2O_CPP11 optimizer.setAlgorithm(new g2o::OptimizationAlgorithmGaussNewton( - g2o::make_unique(std::move(linearSolver)))); + std::make_unique(std::move(linearSolver)))); #else optimizer.setAlgorithm(new g2o::OptimizationAlgorithmGaussNewton(new g2o::BlockSolver_6_3(linearSolver))); #endif @@ -1478,9 +1478,9 @@ std::map OptimizerG2O::optimizeBA( else #endif { -#if defined(RTABMAP_G2O_CPP11) and not defined(RTABMAP_ORB_SLAM) +#if defined(RTABMAP_G2O_CPP11) && !defined(RTABMAP_ORB_SLAM) optimizer.setAlgorithm(new g2o::OptimizationAlgorithmLevenberg( - g2o::make_unique(std::move(linearSolver)))); + std::make_unique(std::move(linearSolver)))); #else optimizer.setAlgorithm(new g2o::OptimizationAlgorithmLevenberg(new g2o::BlockSolver_6_3(linearSolver))); #endif diff --git a/corelib/src/optimizer/OptimizerGTSAM.cpp b/corelib/src/optimizer/OptimizerGTSAM.cpp index 3abb3da716..ac2afff0a4 100644 --- a/corelib/src/optimizer/OptimizerGTSAM.cpp +++ b/corelib/src/optimizer/OptimizerGTSAM.cpp @@ -303,8 +303,8 @@ std::map OptimizerGTSAM::optimize( Eigen::Matrix mgtsam = Eigen::Matrix::Identity(); mgtsam.block(0,0,3,3) = information.block(3,3,3,3); // cov rotation mgtsam.block(3,3,3,3) = information.block(0,0,3,3); // cov translation - mgtsam.block(0,3,3,3) = information.block(0,3,3,3); // off diagonal - mgtsam.block(3,0,3,3) = information.block(3,0,3,3); // off diagonal + mgtsam.block(0,3,3,3) = information.block(3,0,3,3); // off diagonal + mgtsam.block(3,0,3,3) = information.block(0,3,3,3); // off diagonal gtsam::SharedNoiseModel model = gtsam::noiseModel::Gaussian::Information(mgtsam); graph.add(gtsam::PriorFactor(id1, gtsam::Pose3(iter->second.transform().toEigen4d()), model)); @@ -383,8 +383,8 @@ std::map OptimizerGTSAM::optimize( Eigen::Matrix mgtsam = Eigen::Matrix::Identity(); mgtsam.block(0,0,3,3) = information.block(3,3,3,3); // cov rotation mgtsam.block(3,3,3,3) = information.block(0,0,3,3); // cov translation - mgtsam.block(0,3,3,3) = information.block(0,3,3,3); // off diagonal - mgtsam.block(3,0,3,3) = information.block(3,0,3,3); // off diagonal + mgtsam.block(0,3,3,3) = information.block(3,0,3,3); // off diagonal + mgtsam.block(3,0,3,3) = information.block(0,3,3,3); // off diagonal gtsam::SharedNoiseModel model = gtsam::noiseModel::Gaussian::Information(mgtsam); graph.add(gtsam::BetweenFactor(id1, id2, gtsam::Pose3(t.toEigen4d()), model)); } @@ -471,8 +471,8 @@ std::map OptimizerGTSAM::optimize( Eigen::Matrix mgtsam = Eigen::Matrix::Identity(); mgtsam.block(0,0,3,3) = information.block(3,3,3,3); // cov rotation mgtsam.block(3,3,3,3) = information.block(0,0,3,3); // cov translation - mgtsam.block(0,3,3,3) = information.block(0,3,3,3); // off diagonal - mgtsam.block(3,0,3,3) = information.block(3,0,3,3); // off diagonal + mgtsam.block(0,3,3,3) = information.block(3,0,3,3); // off diagonal + mgtsam.block(3,0,3,3) = information.block(0,3,3,3); // off diagonal gtsam::SharedNoiseModel model = gtsam::noiseModel::Gaussian::Information(mgtsam); #ifdef RTABMAP_VERTIGO @@ -527,7 +527,11 @@ std::map OptimizerGTSAM::optimize( { float x,y,z,roll,pitch,yaw; std::map tmpPoses; +#if GTSAM_VERSION_NUMERIC >= 40200 + for(gtsam::Values::deref_iterator iter=optimizer->values().begin(); iter!=optimizer->values().end(); ++iter) +#else for(gtsam::Values::const_iterator iter=optimizer->values().begin(); iter!=optimizer->values().end(); ++iter) +#endif { if(iter->value.dim() > 1) { @@ -630,7 +634,11 @@ std::map OptimizerGTSAM::optimize( optimizer->iterations(), optimizer->error(), graph.error(initialEstimate), graph.error(optimizer->values()), timer.ticks()); float x,y,z,roll,pitch,yaw; +#if GTSAM_VERSION_NUMERIC >= 40200 + for(gtsam::Values::deref_iterator iter=optimizer->values().begin(); iter!=optimizer->values().end(); ++iter) +#else for(gtsam::Values::const_iterator iter=optimizer->values().begin(); iter!=optimizer->values().end(); ++iter) +#endif { if(iter->value.dim() > 1) { @@ -708,8 +716,8 @@ std::map OptimizerGTSAM::optimize( Eigen::Matrix mgtsam = Eigen::Matrix::Identity(); mgtsam.block(3,3,3,3) = info.block(0,0,3,3); // cov rotation mgtsam.block(0,0,3,3) = info.block(3,3,3,3); // cov translation - mgtsam.block(0,3,3,3) = info.block(0,3,3,3); // off diagonal - mgtsam.block(3,0,3,3) = info.block(3,0,3,3); // off diagonal + mgtsam.block(0,3,3,3) = info.block(3,0,3,3); // off diagonal + mgtsam.block(3,0,3,3) = info.block(0,3,3,3); // off diagonal memcpy(outputCovariance.data, mgtsam.data(), outputCovariance.total()*sizeof(double)); } else diff --git a/corelib/src/optimizer/OptimizerTORO.cpp b/corelib/src/optimizer/OptimizerTORO.cpp index 8342bbffdf..175f616d2d 100644 --- a/corelib/src/optimizer/OptimizerTORO.cpp +++ b/corelib/src/optimizer/OptimizerTORO.cpp @@ -36,8 +36,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #ifdef RTABMAP_TORO -#include "toro3d/treeoptimizer3.hh" -#include "toro3d/treeoptimizer2.hh" +#include "toro3d/treeoptimizer3.h" +#include "toro3d/treeoptimizer2.h" #endif namespace rtabmap { diff --git a/corelib/src/optimizer/gtsam/GravityFactor.h b/corelib/src/optimizer/gtsam/GravityFactor.h index f292072f13..0ca1a75a77 100644 --- a/corelib/src/optimizer/gtsam/GravityFactor.h +++ b/corelib/src/optimizer/gtsam/GravityFactor.h @@ -63,15 +63,21 @@ class GravityFactor { /** vector of errors */ Vector attitudeError(const Rot3& p, - OptionalJacobian<2,3> H = boost::none) const; +#if GTSAM_VERSION_NUMERIC >= 40300 + OptionalJacobian<2,3> H = {}) const; +#else + OptionalJacobian<2,3> H = boost::none) const; +#endif /** Serialization function */ +#if defined(GTSAM_ENABLE_BOOST_SERIALIZATION) || GTSAM_VERSION_NUMERIC < 40300 friend class boost::serialization::access; template void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & boost::serialization::make_nvp("nZ_", const_cast(nZ_)); ar & boost::serialization::make_nvp("bRef_", const_cast(bRef_)); } +#endif }; /** @@ -85,7 +91,11 @@ class Rot3GravityFactor: public NoiseModelFactor1, public GravityFactor { public: /// shorthand for a smart pointer to a factor +#if GTSAM_VERSION_NUMERIC >= 40300 + typedef std::shared_ptr shared_ptr; + #else typedef boost::shared_ptr shared_ptr; +#endif /// Typedef to this class typedef Rot3GravityFactor This; @@ -111,7 +121,11 @@ class Rot3GravityFactor: public NoiseModelFactor1, public GravityFactor { /// @return a deep copy of this factor virtual gtsam::NonlinearFactor::shared_ptr clone() const { +#if GTSAM_VERSION_NUMERIC >= 40300 + return std::static_pointer_cast( +#else return boost::static_pointer_cast( +#endif gtsam::NonlinearFactor::shared_ptr(new This(*this))); } @@ -124,7 +138,11 @@ class Rot3GravityFactor: public NoiseModelFactor1, public GravityFactor { /** vector of errors */ virtual Vector evaluateError(const Rot3& nRb, // - boost::optional H = boost::none) const { +#if GTSAM_VERSION_NUMERIC >= 40300 + OptionalMatrixType H = OptionalNone) const { +#else + boost::optional H = boost::none) const { +#endif return attitudeError(nRb, H); } Unit3 nZ() const { @@ -135,7 +153,7 @@ class Rot3GravityFactor: public NoiseModelFactor1, public GravityFactor { } private: - +#if defined(GTSAM_ENABLE_BOOST_SERIALIZATION) || GTSAM_VERSION_NUMERIC < 40300 /** Serialization function */ friend class boost::serialization::access; template @@ -145,6 +163,7 @@ class Rot3GravityFactor: public NoiseModelFactor1, public GravityFactor { ar & boost::serialization::make_nvp("GravityFactor", boost::serialization::base_object(*this)); } +#endif public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW @@ -163,8 +182,11 @@ class Pose3GravityFactor: public NoiseModelFactor1, public: /// shorthand for a smart pointer to a factor +#if GTSAM_VERSION_NUMERIC >= 40300 + typedef std::shared_ptr shared_ptr; +#else typedef boost::shared_ptr shared_ptr; - +#endif /// Typedef to this class typedef Pose3GravityFactor This; @@ -189,7 +211,11 @@ class Pose3GravityFactor: public NoiseModelFactor1, /// @return a deep copy of this factor virtual gtsam::NonlinearFactor::shared_ptr clone() const { +#if GTSAM_VERSION_NUMERIC >= 40300 + return std::static_pointer_cast( +#else return boost::static_pointer_cast( +#endif gtsam::NonlinearFactor::shared_ptr(new This(*this))); } @@ -202,7 +228,11 @@ class Pose3GravityFactor: public NoiseModelFactor1, /** vector of errors */ virtual Vector evaluateError(const Pose3& nTb, // +#if GTSAM_VERSION_NUMERIC >= 40300 + OptionalMatrixType H = OptionalNone) const { +#else boost::optional H = boost::none) const { +#endif Vector e = attitudeError(nTb.rotation(), H); if (H) { Matrix H23 = *H; @@ -219,7 +249,7 @@ class Pose3GravityFactor: public NoiseModelFactor1, } private: - +#if defined(GTSAM_ENABLE_BOOST_SERIALIZATION) || GTSAM_VERSION_NUMERIC < 40300 /** Serialization function */ friend class boost::serialization::access; template @@ -229,7 +259,7 @@ class Pose3GravityFactor: public NoiseModelFactor1, ar & boost::serialization::make_nvp("GravityFactor", boost::serialization::base_object(*this)); } - +#endif public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; diff --git a/corelib/src/optimizer/gtsam/XYFactor.h b/corelib/src/optimizer/gtsam/XYFactor.h index d13a3bc8a0..8a7c65585c 100644 --- a/corelib/src/optimizer/gtsam/XYFactor.h +++ b/corelib/src/optimizer/gtsam/XYFactor.h @@ -41,7 +41,12 @@ class XYFactor: public gtsam::NoiseModelFactor1 { // error function // @param p the pose in Pose2 // @param H the optional Jacobian matrix, which use boost optional and has default null pointer - gtsam::Vector evaluateError(const VALUE& p, boost::optional H = boost::none) const { + gtsam::Vector evaluateError(const VALUE& p, +#if GTSAM_VERSION_NUMERIC >= 40300 + OptionalMatrixType H = OptionalNone) const { +#else + boost::optional H = boost::none) const { +#endif // note that use boost optional like a pointer // only calculate jacobian matrix when non-null pointer exists diff --git a/corelib/src/optimizer/gtsam/XYZFactor.h b/corelib/src/optimizer/gtsam/XYZFactor.h index d4ed63c01a..42e55d0b83 100644 --- a/corelib/src/optimizer/gtsam/XYZFactor.h +++ b/corelib/src/optimizer/gtsam/XYZFactor.h @@ -41,14 +41,24 @@ class XYZFactor: public gtsam::NoiseModelFactor1 { // error function // @param p the pose in Pose // @param H the optional Jacobian matrix, which use boost optional and has default null pointer - gtsam::Vector evaluateError(const gtsam::Pose3& p, boost::optional H = boost::none) const { + gtsam::Vector evaluateError(const gtsam::Pose3& p, +#if GTSAM_VERSION_NUMERIC >= 40300 + OptionalMatrixType H = OptionalNone) const { +#else + boost::optional H = boost::none) const { +#endif if(H) { p.translation(H); } return (gtsam::Vector3() << p.x() - mx_, p.y() - my_, p.z() - mz_).finished(); } - gtsam::Vector evaluateError(const gtsam::Point3& p, boost::optional H = boost::none) const { + gtsam::Vector evaluateError(const gtsam::Point3& p, +#if GTSAM_VERSION_NUMERIC >= 40300 + OptionalMatrixType H = OptionalNone) const { +#else + boost::optional H = boost::none) const { +#endif return (gtsam::Vector3() << p.x() - mx_, p.y() - my_, p.z() - mz_).finished(); } }; diff --git a/corelib/src/optimizer/toro3d/dmatrix.hh b/corelib/src/optimizer/toro3d/dmatrix.h similarity index 100% rename from corelib/src/optimizer/toro3d/dmatrix.hh rename to corelib/src/optimizer/toro3d/dmatrix.h diff --git a/corelib/src/optimizer/toro3d/posegraph.hh b/corelib/src/optimizer/toro3d/posegraph.h similarity index 100% rename from corelib/src/optimizer/toro3d/posegraph.hh rename to corelib/src/optimizer/toro3d/posegraph.h diff --git a/corelib/src/optimizer/toro3d/posegraph2.cpp b/corelib/src/optimizer/toro3d/posegraph2.cpp index 061a9d9359..d0117279b1 100644 --- a/corelib/src/optimizer/toro3d/posegraph2.cpp +++ b/corelib/src/optimizer/toro3d/posegraph2.cpp @@ -40,7 +40,8 @@ * such as loading, saving, merging constraints, and etc. **/ -#include "posegraph2.hh" +#include "posegraph2.h" + #include #include #include diff --git a/corelib/src/optimizer/toro3d/posegraph2.hh b/corelib/src/optimizer/toro3d/posegraph2.h similarity index 98% rename from corelib/src/optimizer/toro3d/posegraph2.hh rename to corelib/src/optimizer/toro3d/posegraph2.h index e602b81e4e..a999b53187 100644 --- a/corelib/src/optimizer/toro3d/posegraph2.hh +++ b/corelib/src/optimizer/toro3d/posegraph2.h @@ -43,10 +43,10 @@ #ifndef _POSEGRAPH2_HH_ #define _POSEGRAPH2_HH_ -#include "posegraph.hh" -#include "transformation2.hh" #include #include +#include "posegraph.h" +#include "transformation2.h" namespace AISNavigation { diff --git a/corelib/src/optimizer/toro3d/posegraph3.cpp b/corelib/src/optimizer/toro3d/posegraph3.cpp index 0bc013127e..de853fff01 100644 --- a/corelib/src/optimizer/toro3d/posegraph3.cpp +++ b/corelib/src/optimizer/toro3d/posegraph3.cpp @@ -39,7 +39,8 @@ * such as loading, saving, merging constraints, and etc. **/ -#include "posegraph3.hh" +#include "posegraph3.h" + #include #include #include diff --git a/corelib/src/optimizer/toro3d/posegraph3.hh b/corelib/src/optimizer/toro3d/posegraph3.h similarity index 98% rename from corelib/src/optimizer/toro3d/posegraph3.hh rename to corelib/src/optimizer/toro3d/posegraph3.h index ada4fc4819..a5f85cd62c 100644 --- a/corelib/src/optimizer/toro3d/posegraph3.hh +++ b/corelib/src/optimizer/toro3d/posegraph3.h @@ -43,10 +43,10 @@ #ifndef _POSEGRAPH3_HH_ #define _POSEGRAPH3_HH_ -#include "posegraph.hh" -#include "transformation3.hh" #include #include +#include "posegraph.h" +#include "transformation3.h" typedef unsigned int uint; #ifndef M_PI diff --git a/corelib/src/optimizer/toro3d/transformation2.hh b/corelib/src/optimizer/toro3d/transformation2.h similarity index 100% rename from corelib/src/optimizer/toro3d/transformation2.hh rename to corelib/src/optimizer/toro3d/transformation2.h diff --git a/corelib/src/optimizer/toro3d/transformation3.hh b/corelib/src/optimizer/toro3d/transformation3.h similarity index 99% rename from corelib/src/optimizer/toro3d/transformation3.hh rename to corelib/src/optimizer/toro3d/transformation3.h index e81966c42d..d5f29d4c9b 100644 --- a/corelib/src/optimizer/toro3d/transformation3.hh +++ b/corelib/src/optimizer/toro3d/transformation3.h @@ -39,7 +39,8 @@ #include #include -#include "dmatrix.hh" + +#include "dmatrix.h" namespace AISNavigation { diff --git a/corelib/src/optimizer/toro3d/treeoptimizer2.cpp b/corelib/src/optimizer/toro3d/treeoptimizer2.cpp index 1fb9bce69b..f715db26ef 100644 --- a/corelib/src/optimizer/toro3d/treeoptimizer2.cpp +++ b/corelib/src/optimizer/toro3d/treeoptimizer2.cpp @@ -41,7 +41,8 @@ * **/ -#include "treeoptimizer2.hh" +#include "treeoptimizer2.h" + #include #include #include diff --git a/corelib/src/optimizer/toro3d/treeoptimizer2.hh b/corelib/src/optimizer/toro3d/treeoptimizer2.h similarity index 99% rename from corelib/src/optimizer/toro3d/treeoptimizer2.hh rename to corelib/src/optimizer/toro3d/treeoptimizer2.h index 34f1a963b5..358f52d0d0 100644 --- a/corelib/src/optimizer/toro3d/treeoptimizer2.hh +++ b/corelib/src/optimizer/toro3d/treeoptimizer2.h @@ -44,7 +44,7 @@ #ifndef _TREEOPTIMIZER2_HH_ #define _TREEOPTIMIZER2_HH_ -#include "posegraph2.hh" +#include "posegraph2.h" namespace AISNavigation { diff --git a/corelib/src/optimizer/toro3d/treeoptimizer3.cpp b/corelib/src/optimizer/toro3d/treeoptimizer3.cpp index f704e6e02a..3478c77ff6 100644 --- a/corelib/src/optimizer/toro3d/treeoptimizer3.cpp +++ b/corelib/src/optimizer/toro3d/treeoptimizer3.cpp @@ -41,7 +41,8 @@ * **/ -#include "treeoptimizer3.hh" +#include "treeoptimizer3.h" + #include #include #include diff --git a/corelib/src/optimizer/toro3d/treeoptimizer3.hh b/corelib/src/optimizer/toro3d/treeoptimizer3.h similarity index 99% rename from corelib/src/optimizer/toro3d/treeoptimizer3.hh rename to corelib/src/optimizer/toro3d/treeoptimizer3.h index 17f25c36e5..66cb6a5b29 100644 --- a/corelib/src/optimizer/toro3d/treeoptimizer3.hh +++ b/corelib/src/optimizer/toro3d/treeoptimizer3.h @@ -44,7 +44,7 @@ #ifndef _TREEOPTIMIZER3_HH_ #define _TREEOPTIMIZER3_HH_ -#include "posegraph3.hh" +#include "posegraph3.h" namespace AISNavigation { diff --git a/corelib/src/optimizer/toro3d/treeoptimizer3_iteration.cpp b/corelib/src/optimizer/toro3d/treeoptimizer3_iteration.cpp index 2ee07e9453..e308275797 100644 --- a/corelib/src/optimizer/toro3d/treeoptimizer3_iteration.cpp +++ b/corelib/src/optimizer/toro3d/treeoptimizer3_iteration.cpp @@ -34,9 +34,9 @@ * PURPOSE. **********************************************************************/ -#include "treeoptimizer3.hh" #include #include +#include "treeoptimizer3.h" using namespace std; diff --git a/corelib/src/optimizer/vertigo/gtsam/DerivedValue.h b/corelib/src/optimizer/vertigo/gtsam/DerivedValue.h index 66cf6954e9..ea502a4c6a 100644 --- a/corelib/src/optimizer/vertigo/gtsam/DerivedValue.h +++ b/corelib/src/optimizer/vertigo/gtsam/DerivedValue.h @@ -72,9 +72,15 @@ class DerivedValue : public gtsam::Value { /** * Clone this value (normal clone on the heap, delete with 'delete' operator) */ +#if GTSAM_VERSION_NUMERIC >= 40300 + virtual std::shared_ptr clone() const { + return std::make_shared(static_cast(*this)); + } + #else virtual boost::shared_ptr clone() const { return boost::make_shared(static_cast(*this)); } +#endif /// equals implementing generic Value interface virtual bool equals_(const gtsam::Value& p, double tol = 1e-9) const { diff --git a/corelib/src/optimizer/vertigo/gtsam/betweenFactorMaxMix.h b/corelib/src/optimizer/vertigo/gtsam/betweenFactorMaxMix.h index 0ee996f64f..40538fc0f0 100644 --- a/corelib/src/optimizer/vertigo/gtsam/betweenFactorMaxMix.h +++ b/corelib/src/optimizer/vertigo/gtsam/betweenFactorMaxMix.h @@ -12,7 +12,7 @@ #include #include -#if GTSAM_VERSION_MAJOR > 4 || (GTSAM_VERSION_MAJOR==4 && GTSAM_VERSION_MINOR>=1) +#if GTSAM_VERSION_NUMERIC >= 40100 namespace gtsam { gtsam::Matrix inverse(const gtsam::Matrix & matrix) { @@ -49,7 +49,7 @@ namespace vertigo { double nu1 = 1.0/sqrt(gtsam::inverse(info1).determinant()); double l1 = nu1 * exp(-0.5*m1); -#if GTSAM_VERSION_MAJOR > 4 || (GTSAM_VERSION_MAJOR==4 && GTSAM_VERSION_MINOR>=1) +#if GTSAM_VERSION_NUMERIC >= 40100 double m2 = nullHypothesisModel->squaredMahalanobisDistance(error); #else double m2 = nullHypothesisModel->distance(error); diff --git a/corelib/src/optimizer/vertigo/gtsam/betweenFactorSwitchable.h b/corelib/src/optimizer/vertigo/gtsam/betweenFactorSwitchable.h index ab5a443840..de1b4d7364 100644 --- a/corelib/src/optimizer/vertigo/gtsam/betweenFactorSwitchable.h +++ b/corelib/src/optimizer/vertigo/gtsam/betweenFactorSwitchable.h @@ -30,9 +30,15 @@ namespace vertigo { betweenFactor(key1, key2, measured, model) {}; gtsam::Vector evaluateError(const VALUE& p1, const VALUE& p2, const SwitchVariableLinear& s, +#if GTSAM_VERSION_NUMERIC >= 40300 + OptionalMatrixType H1 = OptionalNone, + OptionalMatrixType H2 = OptionalNone, + OptionalMatrixType H3 = OptionalNone) const +#else boost::optional H1 = boost::none, - boost::optional H2 = boost::none, - boost::optional H3 = boost::none) const + boost::optional H2 = boost::none, + boost::optional H3 = boost::none) const +#endif { // calculate error @@ -64,9 +70,15 @@ namespace vertigo { betweenFactor(key1, key2, measured, model) {}; gtsam::Vector evaluateError(const VALUE& p1, const VALUE& p2, const SwitchVariableSigmoid& s, +#if GTSAM_VERSION_NUMERIC >= 40300 + OptionalMatrixType H1 = OptionalNone, + OptionalMatrixType H2 = OptionalNone, + OptionalMatrixType H3 = OptionalNone) const +#else boost::optional H1 = boost::none, - boost::optional H2 = boost::none, - boost::optional H3 = boost::none) const + boost::optional H2 = boost::none, + boost::optional H3 = boost::none) const +#endif { // calculate error diff --git a/corelib/src/optimizer/vertigo/gtsam/switchVariableLinear.h b/corelib/src/optimizer/vertigo/gtsam/switchVariableLinear.h index 2260ea6077..e95e0b568b 100644 --- a/corelib/src/optimizer/vertigo/gtsam/switchVariableLinear.h +++ b/corelib/src/optimizer/vertigo/gtsam/switchVariableLinear.h @@ -76,8 +76,13 @@ namespace vertigo { /** between operation */ inline SwitchVariableLinear between(const SwitchVariableLinear& l2, +#if GTSAM_VERSION_NUMERIC >= 40300 + OptionalMatrixType H1=OptionalNone, + OptionalMatrixType H2=OptionalNone) const { +#else boost::optional H1=boost::none, boost::optional H2=boost::none) const { +#endif if(H1) *H1 = -gtsam::Matrix::Identity(1, 1); if(H2) *H2 = gtsam::Matrix::Identity(1, 1); return SwitchVariableLinear(l2.value() - value()); @@ -116,11 +121,19 @@ template<> struct traits { typedef OptionalJacobian<3, 3> ChartJacobian; typedef gtsam::Vector TangentVector; static TangentVector Local(const vertigo::SwitchVariableLinear& origin, const vertigo::SwitchVariableLinear& other, - ChartJacobian Horigin = boost::none, ChartJacobian Hother = boost::none) { +#if GTSAM_VERSION_NUMERIC >= 40300 + ChartJacobian Horigin = {}, ChartJacobian Hother = {}) { +#else + ChartJacobian Horigin = boost::none, ChartJacobian Hother = boost::none) { +#endif return origin.localCoordinates(other); } static vertigo::SwitchVariableLinear Retract(const vertigo::SwitchVariableLinear& g, const TangentVector& v, - ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) { +#if GTSAM_VERSION_NUMERIC >= 40300 + ChartJacobian H1 = {}, ChartJacobian H2 = {}) { +#else + ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) { +#endif return g.retract(v); } }; diff --git a/corelib/src/optimizer/vertigo/gtsam/switchVariableSigmoid.h b/corelib/src/optimizer/vertigo/gtsam/switchVariableSigmoid.h index 674e897ac7..79e1fca99d 100644 --- a/corelib/src/optimizer/vertigo/gtsam/switchVariableSigmoid.h +++ b/corelib/src/optimizer/vertigo/gtsam/switchVariableSigmoid.h @@ -76,8 +76,13 @@ namespace vertigo { /** between operation */ inline SwitchVariableSigmoid between(const SwitchVariableSigmoid& l2, +#if GTSAM_VERSION_NUMERIC >= 40300 + OptionalMatrixType H1=OptionalNone, + OptionalMatrixType H2=OptionalNone) const { +#else boost::optional H1=boost::none, boost::optional H2=boost::none) const { +#endif if(H1) *H1 = -gtsam::Matrix::Identity(1, 1); if(H2) *H2 = gtsam::Matrix::Identity(1, 1); return SwitchVariableSigmoid(l2.value() - value()); @@ -117,11 +122,19 @@ template<> struct traits { typedef OptionalJacobian<3, 3> ChartJacobian; typedef gtsam::Vector TangentVector; static TangentVector Local(const vertigo::SwitchVariableSigmoid& origin, const vertigo::SwitchVariableSigmoid& other, - ChartJacobian Horigin = boost::none, ChartJacobian Hother = boost::none) { +#if GTSAM_VERSION_NUMERIC >= 40300 + ChartJacobian Horigin = {}, ChartJacobian Hother = {}) { +#else + ChartJacobian Horigin = boost::none, ChartJacobian Hother = boost::none) { +#endif return origin.localCoordinates(other); } static vertigo::SwitchVariableSigmoid Retract(const vertigo::SwitchVariableSigmoid& g, const TangentVector& v, +#if GTSAM_VERSION_NUMERIC >= 40300 + ChartJacobian H1 = {}, ChartJacobian H2 = {}) { +#else ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) { +#endif return g.retract(v); } }; diff --git a/corelib/src/python/PyDetector.cpp b/corelib/src/python/PyDetector.cpp index 9039640207..46f9f25619 100644 --- a/corelib/src/python/PyDetector.cpp +++ b/corelib/src/python/PyDetector.cpp @@ -10,6 +10,8 @@ #include #include +#include + #define NPY_NO_DEPRECATED_API NPY_API_VERSION #include @@ -32,7 +34,7 @@ PyDetector::PyDetector(const ParametersMap & parameters) : return; } - lock(); + pybind11::gil_scoped_acquire acquire; std::string matcherPythonDir = UDirectory::getDir(path_); if(!matcherPythonDir.empty()) @@ -54,15 +56,13 @@ PyDetector::PyDetector(const ParametersMap & parameters) : if(!pModule_) { UERROR("Module \"%s\" could not be imported! (File=\"%s\")", scriptName.c_str(), path_.c_str()); - UERROR("%s", getTraceback().c_str()); + UERROR("%s", getPythonTraceback().c_str()); } - - unlock(); } PyDetector::~PyDetector() { - lock(); + pybind11::gil_scoped_acquire acquire; if(pFunc_) { @@ -72,8 +72,6 @@ PyDetector::~PyDetector() { Py_DECREF(pModule_); } - - unlock(); } void PyDetector::parseParameters(const ParametersMap & parameters) @@ -102,7 +100,7 @@ std::vector PyDetector::generateKeypointsImpl(const cv::Mat & imag return keypoints; } - lock(); + pybind11::gil_scoped_acquire acquire; if(!pFunc_) { @@ -116,7 +114,7 @@ std::vector PyDetector::generateKeypointsImpl(const cv::Mat & imag if(result == NULL) { UERROR("Call to \"init(...)\" in \"%s\" failed!", path_.c_str()); - UERROR("%s", getTraceback().c_str()); + UERROR("%s", getPythonTraceback().c_str()); return keypoints; } Py_DECREF(result); @@ -129,7 +127,7 @@ std::vector PyDetector::generateKeypointsImpl(const cv::Mat & imag else { UERROR("Cannot find method \"detect(...)\" in %s", path_.c_str()); - UERROR("%s", getTraceback().c_str()); + UERROR("%s", getPythonTraceback().c_str()); if(pFunc_) { Py_DECREF(pFunc_); @@ -141,7 +139,7 @@ std::vector PyDetector::generateKeypointsImpl(const cv::Mat & imag else { UERROR("Cannot call method \"init(...)\" in %s", path_.c_str()); - UERROR("%s", getTraceback().c_str()); + UERROR("%s", getPythonTraceback().c_str()); return keypoints; } Py_DECREF(pFunc); @@ -149,7 +147,7 @@ std::vector PyDetector::generateKeypointsImpl(const cv::Mat & imag else { UERROR("Cannot find method \"init(...)\""); - UERROR("%s", getTraceback().c_str()); + UERROR("%s", getPythonTraceback().c_str()); return keypoints; } UDEBUG("init time = %fs", timer.ticks()); @@ -167,7 +165,7 @@ std::vector PyDetector::generateKeypointsImpl(const cv::Mat & imag if(pReturn == NULL) { UERROR("Failed to call match() function!"); - UERROR("%s", getTraceback().c_str()); + UERROR("%s", getPythonTraceback().c_str()); } else { @@ -220,8 +218,6 @@ std::vector PyDetector::generateKeypointsImpl(const cv::Mat & imag Py_DECREF(pImageBuffer); } - unlock(); - return keypoints; } diff --git a/corelib/src/python/PyDetector.h b/corelib/src/python/PyDetector.h index 4512b8eb02..5f292abdc5 100644 --- a/corelib/src/python/PyDetector.h +++ b/corelib/src/python/PyDetector.h @@ -18,7 +18,7 @@ namespace rtabmap { -class PyDetector : public Feature2D, public PythonInterface +class PyDetector : public Feature2D { public: PyDetector(const ParametersMap & parameters = ParametersMap()); diff --git a/corelib/src/python/PyMatcher.cpp b/corelib/src/python/PyMatcher.cpp index 446f28c961..aac482efe1 100644 --- a/corelib/src/python/PyMatcher.cpp +++ b/corelib/src/python/PyMatcher.cpp @@ -10,6 +10,8 @@ #include #include +#include + #define NPY_NO_DEPRECATED_API NPY_API_VERSION #include @@ -39,7 +41,7 @@ PyMatcher::PyMatcher( return; } - lock(); + pybind11::gil_scoped_acquire acquire; std::string matcherPythonDir = UDirectory::getDir(path_); if(!matcherPythonDir.empty()) @@ -59,15 +61,13 @@ PyMatcher::PyMatcher( if(!pModule_) { UERROR("Module \"%s\" could not be imported! (File=\"%s\")", scriptName.c_str(), path_.c_str()); - UERROR("%s", getTraceback().c_str()); + UERROR("%s", getPythonTraceback().c_str()); } - - unlock(); } PyMatcher::~PyMatcher() { - lock(); + pybind11::gil_scoped_acquire acquire; if(pFunc_) { Py_DECREF(pFunc_); @@ -76,7 +76,6 @@ PyMatcher::~PyMatcher() { Py_DECREF(pModule_); } - unlock(); } std::vector PyMatcher::match( @@ -104,7 +103,7 @@ std::vector PyMatcher::match( imageSize.width>0 && imageSize.height>0) { - lock(); + pybind11::gil_scoped_acquire acquire; UDEBUG("matchThreshold=%f, iterations=%d, cuda=%d", matchThreshold_, iterations_, cuda_?1:0); @@ -120,7 +119,7 @@ std::vector PyMatcher::match( if(result == NULL) { UERROR("Call to \"init(...)\" in \"%s\" failed!", path_.c_str()); - UERROR("%s", getTraceback().c_str()); + UERROR("%s", getPythonTraceback().c_str()); return matches; } Py_DECREF(result); @@ -133,7 +132,7 @@ std::vector PyMatcher::match( else { UERROR("Cannot find method \"match(...)\" in %s", path_.c_str()); - UERROR("%s", getTraceback().c_str()); + UERROR("%s", getPythonTraceback().c_str()); if(pFunc_) { Py_DECREF(pFunc_); @@ -145,7 +144,7 @@ std::vector PyMatcher::match( else { UERROR("Cannot call method \"init(...)\" in %s", path_.c_str()); - UERROR("%s", getTraceback().c_str()); + UERROR("%s", getPythonTraceback().c_str()); return matches; } Py_DECREF(pFunc); @@ -153,7 +152,7 @@ std::vector PyMatcher::match( else { UERROR("Cannot find method \"init(...)\""); - UERROR("%s", getTraceback().c_str()); + UERROR("%s", getPythonTraceback().c_str()); return matches; } UDEBUG("init time = %fs", timer.ticks()); @@ -216,7 +215,7 @@ std::vector PyMatcher::match( if(pReturn == NULL) { UERROR("Failed to call match() function!"); - UERROR("%s", getTraceback().c_str()); + UERROR("%s", getPythonTraceback().c_str()); } else { @@ -260,7 +259,6 @@ std::vector PyMatcher::match( UDEBUG("Fill matches (%d/%d) and cleanup time = %fs", matches.size(), std::min(descriptorsQuery.rows, descriptorsTrain.rows), timer.ticks()); } - unlock(); } else { diff --git a/corelib/src/python/PyMatcher.h b/corelib/src/python/PyMatcher.h index 2af25c75cc..da04cd466f 100644 --- a/corelib/src/python/PyMatcher.h +++ b/corelib/src/python/PyMatcher.h @@ -16,7 +16,7 @@ namespace rtabmap { -class PyMatcher : public PythonInterface +class PyMatcher { public: PyMatcher(const std::string & pythonMatcherPath, diff --git a/corelib/src/python/PythonInterface.cpp b/corelib/src/python/PythonInterface.cpp index 7e9afb48b3..0e2d987980 100644 --- a/corelib/src/python/PythonInterface.cpp +++ b/corelib/src/python/PythonInterface.cpp @@ -8,83 +8,26 @@ #include #include #include +#include namespace rtabmap { -UMutex PythonInterface::mutex_; -int PythonInterface::refCount_ = 0; -PyThreadState * PythonInterface::mainThreadState_ = 0; -unsigned long PythonInterface::mainThreadID_ = 0; - -PythonInterface::PythonInterface() : - threadState_(0) +PythonInterface::PythonInterface() { - UScopeMutex lockM(mutex_); - if(refCount_ == 0) - { - UINFO("Py_Initialize() with thread = %d", UThread::currentThreadId()); - // initialize Python - Py_Initialize(); - - // initialize thread support - PyEval_InitThreads(); - Py_DECREF(PyImport_ImportModule("threading")); - - //release the GIL, store thread state, set the current thread state to NULL - mainThreadState_ = PyEval_SaveThread(); - UASSERT(mainThreadState_); - mainThreadID_ = UThread::currentThreadId(); - } - - ++refCount_; + UINFO("Initialize python interpreter"); + guard_ = new pybind11::scoped_interpreter(); + pybind11::module::import("threading"); + release_ = new pybind11::gil_scoped_release(); } PythonInterface::~PythonInterface() { - UScopeMutex lock(mutex_); - if(refCount_>0 && --refCount_==0) - { - // shut down the interpreter - UINFO("Py_Finalize() with thread = %d", UThread::currentThreadId()); - PyEval_RestoreThread(mainThreadState_); - Py_Finalize(); - } -} - -void PythonInterface::lock() -{ - mutex_.lock(); - - UDEBUG("Lock: Current thread=%d (main=%d)", UThread::currentThreadId(), mainThreadID_); - if(UThread::currentThreadId() == mainThreadID_) - { - PyEval_RestoreThread(mainThreadState_); - } - else - { - // create a thread state object for this thread - threadState_ = PyThreadState_New(mainThreadState_->interp); - UASSERT(threadState_); - PyEval_RestoreThread(threadState_); - } -} - -void PythonInterface::unlock() -{ - if(UThread::currentThreadId() == mainThreadID_) - { - mainThreadState_ = PyEval_SaveThread(); - } - else - { - PyThreadState_Clear(threadState_); - PyThreadState_DeleteCurrent(); - } - UDEBUG("Unlock: Current thread=%d (main=%d)", UThread::currentThreadId(), mainThreadID_); - mutex_.unlock(); + UINFO("Finalize python interpreter"); + delete release_; + delete guard_; } -std::string PythonInterface::getTraceback() +std::string getPythonTraceback() { // Author: https://stackoverflow.com/questions/41268061/c-c-python-exception-traceback-not-being-generated diff --git a/corelib/src/rtflann/algorithms/dist.h b/corelib/src/rtflann/algorithms/dist.h index a286f1eeaf..1c1fa98766 100644 --- a/corelib/src/rtflann/algorithms/dist.h +++ b/corelib/src/rtflann/algorithms/dist.h @@ -477,6 +477,10 @@ struct HammingPopcnt ResultType operator()(Iterator1 a, Iterator2 b, size_t size, ResultType /*worst_dist*/ = -1) const { ResultType result = 0; + + //for portability just use unsigned long -- and use the __builtin_popcountll (see docs for __builtin_popcountll) + typedef unsigned long long pop_t; + #if __GNUC__ #if ANDROID && HAVE_NEON static uint64_t features = android_getCpuFeatures(); diff --git a/corelib/src/rtflann/algorithms/kdtree_index.h b/corelib/src/rtflann/algorithms/kdtree_index.h index c2ab188640..6dfc5d1436 100644 --- a/corelib/src/rtflann/algorithms/kdtree_index.h +++ b/corelib/src/rtflann/algorithms/kdtree_index.h @@ -37,6 +37,7 @@ #include #include #include +#include #include "rtflann/general.h" #include "rtflann/algorithms/nn_index.h" @@ -677,7 +678,9 @@ class KDTreeIndex : public NNIndex /* Construct the randomized trees. */ for (int i = 0; i < trees_; i++) { /* Randomize the order of vectors to allow for unbiased sampling. */ - std::random_shuffle(ind.begin(), ind.end()); + std::random_device rd; + std::mt19937 g(rd()); + std::shuffle(ind.begin(), ind.end(), g); tree_roots_[i] = divideTree(&ind[0], int(size_) ); } delete[] mean_; diff --git a/corelib/src/rtflann/util/heap.h b/corelib/src/rtflann/util/heap.h index b104ee3ae7..a9b50550ce 100644 --- a/corelib/src/rtflann/util/heap.h +++ b/corelib/src/rtflann/util/heap.h @@ -115,8 +115,11 @@ class Heap count = 0; } - struct CompareT : public std::binary_function + struct CompareT { + using result_type = bool; + using first_argument_type = T; + using second_argument_type = T; bool operator()(const T& t_1, const T& t_2) const { return t_2 < t_1; diff --git a/corelib/src/rtflann/util/lsh_table.h b/corelib/src/rtflann/util/lsh_table.h index 974bb9e44a..bbd04522c0 100644 --- a/corelib/src/rtflann/util/lsh_table.h +++ b/corelib/src/rtflann/util/lsh_table.h @@ -47,6 +47,7 @@ #endif #include #include +#include #include "rtflann/util/dynamic_bitset.h" #include "rtflann/util/matrix.h" @@ -364,7 +365,9 @@ inline LshTable::LshTable(unsigned int feature_size, unsigned int // A bit brutal but fast to code std::vector indices(feature_size * CHAR_BIT); for (size_t i = 0; i < feature_size * CHAR_BIT; ++i) indices[i] = i; - std::random_shuffle(indices.begin(), indices.end()); + std::random_device rd; + std::mt19937 g(rd()); + std::shuffle(indices.begin(), indices.end(), g); // Generate a random set of order of subsignature_size_ bits for (unsigned int i = 0; i < key_size_; ++i) { diff --git a/corelib/src/rtflann/util/random.h b/corelib/src/rtflann/util/random.h index 871c9d15f3..44aac93bf5 100644 --- a/corelib/src/rtflann/util/random.h +++ b/corelib/src/rtflann/util/random.h @@ -35,6 +35,7 @@ #include #include #include +#include #include "rtflann/general.h" @@ -76,13 +77,6 @@ inline int rand_int(int high = RAND_MAX, int low = 0) } -class RandomGenerator -{ -public: - ptrdiff_t operator() (ptrdiff_t i) { return rand_int(i); } -}; - - /** * Random number generator that returns a distinct number from * the [0,n) interval each time. @@ -110,14 +104,15 @@ class UniqueRandom */ void init(int n) { - static RandomGenerator generator; // create and initialize an array of size n vals_.resize(n); size_ = n; for (int i = 0; i < size_; ++i) vals_[i] = i; // shuffle the elements in the array - std::random_shuffle(vals_.begin(), vals_.end(), generator); + std::random_device rd; + std::mt19937 g(rd()); + std::shuffle(vals_.begin(), vals_.end(), g); counter_ = 0; } diff --git a/corelib/src/stereo/StereoBM.cpp b/corelib/src/stereo/StereoBM.cpp index 7d47494665..93958293f8 100644 --- a/corelib/src/stereo/StereoBM.cpp +++ b/corelib/src/stereo/StereoBM.cpp @@ -121,6 +121,14 @@ cv::Mat StereoBM::computeDisparity( stereo->setDisp12MaxDiff(disp12MaxDiff_); stereo->compute(leftMono, rightImage, disparity); #endif + + if(minDisparity_>0) + { + cv::Mat dst; + cv::threshold(disparity, dst, minDisparity_*16, 0, cv::THRESH_TOZERO); + disparity = dst; + } + return disparity; } diff --git a/corelib/src/stereo/StereoSGBM.cpp b/corelib/src/stereo/StereoSGBM.cpp index f5388b2cf9..02c2fffe4d 100644 --- a/corelib/src/stereo/StereoSGBM.cpp +++ b/corelib/src/stereo/StereoSGBM.cpp @@ -113,6 +113,14 @@ cv::Mat StereoSGBM::computeDisparity( mode_); stereo->compute(leftMono, rightImage, disparity); #endif + + if(minDisparity_>0) + { + cv::Mat dst; + cv::threshold(disparity, dst, minDisparity_*16, 0, cv::THRESH_TOZERO); + disparity = dst; + } + return disparity; } diff --git a/corelib/src/superpoint_torch/SuperPoint.cc b/corelib/src/superpoint_torch/SuperPoint.cc index 6881c4bde6..03e3d5ac02 100644 --- a/corelib/src/superpoint_torch/SuperPoint.cc +++ b/corelib/src/superpoint_torch/SuperPoint.cc @@ -3,6 +3,7 @@ */ #include +#include #include #include #include @@ -40,7 +41,7 @@ SuperPoint::SuperPoint() convDa(torch::nn::Conv2dOptions(c4, c5, 3).stride(1).padding(1)), convDb(torch::nn::Conv2dOptions(c5, d1, 1).stride(1).padding(0)) - { +{ register_module("conv1a", conv1a); register_module("conv1b", conv1b); @@ -58,11 +59,10 @@ SuperPoint::SuperPoint() register_module("convDa", convDa); register_module("convDb", convDb); - } - - -std::vector SuperPoint::forward(torch::Tensor x) { +} +std::vector SuperPoint::forward(torch::Tensor x) +{ x = torch::relu(conv1a->forward(x)); x = torch::relu(conv1b->forward(x)); x = torch::max_pool2d(x, 2, 2); @@ -104,14 +104,7 @@ std::vector SuperPoint::forward(torch::Tensor x) { ret.push_back(desc); return ret; - } - -void NMS(const std::vector & ptsIn, - const cv::Mat & conf, - const cv::Mat & descriptorsIn, - std::vector & ptsOut, - cv::Mat & descriptorsOut, - int border, int dist_thresh, int img_width, int img_height); +} SPDetector::SPDetector(const std::string & modelPath, float threshold, bool nms, int minDistance, bool cuda) : threshold_(threshold), @@ -183,13 +176,6 @@ std::vector SPDetector::detect(const cv::Mat &img, const cv::Mat & detected_ = true; if (nms_ && !keypoints_no_nms.empty()) { - cv::Mat conf(keypoints_no_nms.size(), 1, CV_32F); - for (size_t i = 0; i < keypoints_no_nms.size(); i++) { - int x = keypoints_no_nms[i].pt.x; - int y = keypoints_no_nms[i].pt.y; - conf.at(i, 0) = prob_cpu[y][x].item(); - } - int border = 0; int dist_thresh = minDistance_; int height = img.rows; @@ -197,7 +183,7 @@ std::vector SPDetector::detect(const cv::Mat &img, const cv::Mat & std::vector keypoints; cv::Mat descEmpty; - NMS(keypoints_no_nms, conf, descEmpty, keypoints, descEmpty, border, dist_thresh, width, height); + util2d::NMS(keypoints_no_nms, descEmpty, keypoints, descEmpty, border, dist_thresh, width, height); if(keypoints.size()>1) { return keypoints; @@ -217,7 +203,7 @@ std::vector SPDetector::detect(const cv::Mat &img, const cv::Mat & { UERROR("No model is loaded!"); return std::vector(); - } + } } cv::Mat SPDetector::compute(const std::vector &keypoints) @@ -274,119 +260,4 @@ cv::Mat SPDetector::compute(const std::vector &keypoints) } } -void NMS(const std::vector & ptsIn, - const cv::Mat & conf, - const cv::Mat & descriptorsIn, - std::vector & ptsOut, - cv::Mat & descriptorsOut, - int border, int dist_thresh, int img_width, int img_height) -{ - - std::vector pts_raw; - - for (size_t i = 0; i < ptsIn.size(); i++) - { - int u = (int) ptsIn[i].pt.x; - int v = (int) ptsIn[i].pt.y; - - pts_raw.push_back(cv::Point2f(u, v)); - } - - //Grid Value Legend: - // 255 : Kept. - // 0 : Empty or suppressed. - // 100 : To be processed (converted to either kept or suppressed). - cv::Mat grid = cv::Mat(cv::Size(img_width, img_height), CV_8UC1); - cv::Mat inds = cv::Mat(cv::Size(img_width, img_height), CV_16UC1); - - cv::Mat confidence = cv::Mat(cv::Size(img_width, img_height), CV_32FC1); - - grid.setTo(0); - inds.setTo(0); - confidence.setTo(0); - - for (size_t i = 0; i < pts_raw.size(); i++) - { - int uu = (int) pts_raw[i].x; - int vv = (int) pts_raw[i].y; - - grid.at(vv, uu) = 100; - inds.at(vv, uu) = i; - - confidence.at(vv, uu) = conf.at(i, 0); - } - - // debug - //cv::Mat confidenceVis = confidence.clone() * 255; - //confidenceVis.convertTo(confidenceVis, CV_8UC1); - //cv::imwrite("confidence.bmp", confidenceVis); - //cv::imwrite("grid_in.bmp", grid); - - cv::copyMakeBorder(grid, grid, dist_thresh, dist_thresh, dist_thresh, dist_thresh, cv::BORDER_CONSTANT, 0); - - for (size_t i = 0; i < pts_raw.size(); i++) - { - // account for top left padding - int uu = (int) pts_raw[i].x + dist_thresh; - int vv = (int) pts_raw[i].y + dist_thresh; - float c = confidence.at(vv-dist_thresh, uu-dist_thresh); - - if (grid.at(vv, uu) == 100) // If not yet suppressed. - { - for(int k = -dist_thresh; k < (dist_thresh+1); k++) - { - for(int j = -dist_thresh; j < (dist_thresh+1); j++) - { - if(j==0 && k==0) - continue; - - if ( confidence.at(vv + k - dist_thresh, uu + j - dist_thresh) <= c ) - { - grid.at(vv + k, uu + j) = 0; - } - } - } - grid.at(vv, uu) = 255; - } - } - - size_t valid_cnt = 0; - std::vector select_indice; - - grid = cv::Mat(grid, cv::Rect(dist_thresh, dist_thresh, img_width, img_height)); - - //debug - //cv::imwrite("grid_nms.bmp", grid); - - for (int v = 0; v < img_height; v++) - { - for (int u = 0; u < img_width; u++) - { - if (grid.at(v,u) == 255) - { - int select_ind = (int) inds.at(v, u); - float response = conf.at(select_ind, 0); - ptsOut.push_back(cv::KeyPoint(pts_raw[select_ind], 8.0f, -1, response)); - - select_indice.push_back(select_ind); - valid_cnt++; - } - } - } - - if(!descriptorsIn.empty()) - { - UASSERT(descriptorsIn.rows == (int)ptsIn.size()); - descriptorsOut.create(select_indice.size(), 256, CV_32F); - - for (size_t i=0; i(i, j) = descriptorsIn.at(select_indice[i], j); - } - } - } -} - } diff --git a/corelib/src/util2d.cpp b/corelib/src/util2d.cpp index bcbf13f6c6..8c8528bc8f 100644 --- a/corelib/src/util2d.cpp +++ b/corelib/src/util2d.cpp @@ -2093,6 +2093,115 @@ void HSVtoRGB( float *r, float *g, float *b, float h, float s, float v ) } } +void NMS( + const std::vector & ptsIn, + const cv::Mat & descriptorsIn, + std::vector & ptsOut, + cv::Mat & descriptorsOut, + int border, int dist_thresh, int img_width, int img_height) +{ + std::vector pts_raw; + + for (size_t i = 0; i < ptsIn.size(); i++) + { + int u = (int) ptsIn[i].pt.x; + int v = (int) ptsIn[i].pt.y; + + pts_raw.emplace_back(cv::Point2f(u, v)); + } + + //Grid Value Legend: + // 255 : Kept. + // 0 : Empty or suppressed. + // 100 : To be processed (converted to either kept or suppressed). + cv::Mat grid = cv::Mat(cv::Size(img_width, img_height), CV_8UC1); + cv::Mat inds = cv::Mat(cv::Size(img_width, img_height), CV_16UC1); + + cv::Mat confidence = cv::Mat(cv::Size(img_width, img_height), CV_32FC1); + + grid.setTo(0); + inds.setTo(0); + confidence.setTo(0); + + for (size_t i = 0; i < pts_raw.size(); i++) + { + int uu = (int) pts_raw[i].x; + int vv = (int) pts_raw[i].y; + + grid.at(vv, uu) = 100; + inds.at(vv, uu) = i; + + confidence.at(vv, uu) = ptsIn[i].response; + } + + // debug + //cv::Mat confidenceVis = confidence.clone() * 255; + //confidenceVis.convertTo(confidenceVis, CV_8UC1); + //cv::imwrite("confidence.bmp", confidenceVis); + //cv::imwrite("grid_in.bmp", grid); + + cv::copyMakeBorder(grid, grid, dist_thresh, dist_thresh, dist_thresh, dist_thresh, cv::BORDER_CONSTANT, 0); + + for (size_t i = 0; i < pts_raw.size(); i++) + { + // account for top left padding + int uu = (int) pts_raw[i].x + dist_thresh; + int vv = (int) pts_raw[i].y + dist_thresh; + float c = confidence.at(vv-dist_thresh, uu-dist_thresh); + + if (grid.at(vv, uu) == 100) // If not yet suppressed. + { + for(int k = -dist_thresh; k < (dist_thresh+1); k++) + { + for(int j = -dist_thresh; j < (dist_thresh+1); j++) + { + if((j==0 && k==0) || grid.at(vv + k, uu + j) == 0) + continue; + + if ( confidence.at(vv + k - dist_thresh, uu + j - dist_thresh) <= c ) + { + grid.at(vv + k, uu + j) = 0; + } + } + } + grid.at(vv, uu) = 255; + } + } + + size_t valid_cnt = 0; + std::vector select_indice; + + grid = cv::Mat(grid, cv::Rect(dist_thresh, dist_thresh, img_width, img_height)); + + //debug + //cv::imwrite("grid_nms.bmp", grid); + + for (int v = 0; v < img_height; v++) + { + for (int u = 0; u < img_width; u++) + { + if (grid.at(v,u) == 255) + { + int select_ind = (int) inds.at(v, u); + ptsOut.emplace_back(ptsIn[select_ind]); + select_indice.emplace_back(select_ind); + valid_cnt++; + } + } + } + + if(!descriptorsIn.empty()) + { + UASSERT(descriptorsIn.rows == (int)ptsIn.size()); + descriptorsOut.create(select_indice.size(), 256, CV_32F); + + for (size_t i=0; i::Ptr cloudFromStereoImages( validIndices); } -pcl::PointCloud::Ptr RTABMAP_EXP cloudFromSensorData( +pcl::PointCloud::Ptr cloudFromSensorData( const SensorData & sensorData, int decimation, float maxDepth, @@ -1054,7 +1054,7 @@ pcl::PointCloud::Ptr RTABMAP_EXP cloudFromSensorData( return cloud; } -pcl::PointCloud::Ptr RTABMAP_EXP cloudRGBFromSensorData( +pcl::PointCloud::Ptr cloudRGBFromSensorData( const SensorData & sensorData, int decimation, float maxDepth, @@ -2593,7 +2593,8 @@ cv::Point3f projectDisparityTo3D( c = model.right().cx() - model.left().cx(); } float W = model.baseline()/(disparity + c); - return cv::Point3f((pt.x - model.left().cx())*W, (pt.y - model.left().cy())*W, model.left().fx()*W); + return cv::Point3f((pt.x - model.left().cx())*W, + (pt.y - model.left().cy())*model.left().fx()/model.left().fy()*W, model.left().fx()*W); } float bad_point = std::numeric_limits::quiet_NaN (); return cv::Point3f(bad_point, bad_point, bad_point); diff --git a/corelib/src/util3d_filtering.cpp b/corelib/src/util3d_filtering.cpp index fb61deca13..c32ffb304c 100644 --- a/corelib/src/util3d_filtering.cpp +++ b/corelib/src/util3d_filtering.cpp @@ -1002,7 +1002,7 @@ pcl::PointCloud::Ptr removeNaNFromPointCloud(const pcl::PointClo return removeNaNFromPointCloudImpl(cloud); } -pcl::PCLPointCloud2::Ptr RTABMAP_EXP removeNaNFromPointCloud(const pcl::PCLPointCloud2::Ptr & cloud) +pcl::PCLPointCloud2::Ptr removeNaNFromPointCloud(const pcl::PCLPointCloud2::Ptr & cloud) { pcl::PCLPointCloud2::Ptr output(new pcl::PCLPointCloud2); output->fields = cloud->fields; diff --git a/corelib/src/util3d_motion_estimation.cpp b/corelib/src/util3d_motion_estimation.cpp index 61d924573e..aaeaed914d 100644 --- a/corelib/src/util3d_motion_estimation.cpp +++ b/corelib/src/util3d_motion_estimation.cpp @@ -42,8 +42,11 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #ifdef RTABMAP_OPENGV #include #include +#include #include +#include #include +#include #endif namespace rtabmap @@ -61,6 +64,7 @@ Transform estimateMotion3DTo2D( double reprojError, int flagsPnP, int refineIterations, + int varianceMedianRatio, float maxVariance, const Transform & guess, const std::map & words3B, @@ -70,6 +74,7 @@ Transform estimateMotion3DTo2D( { UASSERT(cameraModel.isValidForProjection()); UASSERT(!guess.isNull()); + UASSERT(varianceMedianRatio>1); Transform transform; std::vector matches, inliers; @@ -191,11 +196,11 @@ Transform estimateMotion3DTo2D( std::sort(errorSqrdDists.begin(), errorSqrdDists.end()); //divide by 4 instead of 2 to ignore very very far features (stereo) - double median_error_sqr_lin = 2.1981 * (double)errorSqrdDists[errorSqrdDists.size () >> 2]; + double median_error_sqr_lin = 2.1981 * (double)errorSqrdDists[errorSqrdDists.size () / varianceMedianRatio]; UASSERT(uIsFinite(median_error_sqr_lin)); (*covariance)(cv::Range(0,3), cv::Range(0,3)) *= median_error_sqr_lin; std::sort(errorSqrdAngles.begin(), errorSqrdAngles.end()); - double median_error_sqr_ang = 2.1981 * (double)errorSqrdAngles[errorSqrdAngles.size () >> 2]; + double median_error_sqr_ang = 2.1981 * (double)errorSqrdAngles[errorSqrdAngles.size () / varianceMedianRatio]; UASSERT(uIsFinite(median_error_sqr_ang)); (*covariance)(cv::Range(3,6), cv::Range(3,6)) *= median_error_sqr_ang; @@ -242,11 +247,13 @@ Transform estimateMotion3DTo2D( const std::map & words3A, const std::map & words2B, const std::vector & cameraModels, + unsigned int samplingPolicy, int minInliers, int iterations, double reprojError, int flagsPnP, int refineIterations, + int varianceMedianRatio, float maxVariance, const Transform & guess, const std::map & words3B, @@ -267,6 +274,7 @@ Transform estimateMotion3DTo2D( } UASSERT(!guess.isNull()); + UASSERT(varianceMedianRatio > 1); std::vector matches, inliers; @@ -308,12 +316,50 @@ Transform estimateMotion3DTo2D( cameraIndexes.resize(oi); matches.resize(oi); - UDEBUG("words3A=%d words2B=%d matches=%d words3B=%d guess=%s reprojError=%f iterations=%d", + UDEBUG("words3A=%d words2B=%d matches=%d words3B=%d guess=%s reprojError=%f iterations=%d samplingPolicy=%ld", (int)words3A.size(), (int)words2B.size(), (int)matches.size(), (int)words3B.size(), - guess.prettyPrint().c_str(), reprojError, iterations); + guess.prettyPrint().c_str(), reprojError, iterations, samplingPolicy); if((int)matches.size() >= minInliers) { + if(samplingPolicy == 0 || samplingPolicy == 2) + { + std::vector cc; + cc.resize(cameraModels.size()); + std::fill(cc.begin(), cc.end(),0); + for(size_t i=0; i(0, 0)); } - // convert 3d points - opengv::points_t points; - // convert 2d-3d correspondences into bearing vectors - opengv::bearingVectors_t bearingVectors; - opengv::absolute_pose::NoncentralAbsoluteAdapter::camCorrespondences_t camCorrespondences; - for(size_t i=0; i> multiPoints; + multiPoints.resize(cameraModels.size()); + // convert 2d-3d correspondences into bearing vectors + std::vector> multiBearingVectors; + multiBearingVectors.resize(cameraModels.size()); + for(size_t i=0; i(); + multiBearingVectors[i] = std::make_shared(); + } + + for(size_t i=0; ipush_back(opengv::point_t(objectPoints[i].x,objectPoints[i].y,objectPoints[i].z)); + cv::Vec3f pt; + cameraModels[cameraIndex].project(imagePoints[i].x, imagePoints[i].y, 1, pt[0], pt[1], pt[2]); + pt = cv::normalize(pt); + multiBearingVectors[cameraIndex]->push_back(opengv::bearingVector_t(pt[0], pt[1], pt[2])); + } + + //create a non-central absolute multi adapter + opengv::absolute_pose::NoncentralAbsoluteMultiAdapter adapter( + multiBearingVectors, + multiPoints, + camOffsets, + camRotations ); + + adapter.setR(guess.toEigen4d().block<3,3>(0, 0)); + adapter.sett(opengv::translation_t(guess.x(), guess.y(), guess.z())); + + //Create a MultiNoncentralAbsolutePoseSacProblem and MultiRansac + //The method is set to GP3P + opengv::sac::MultiRansac ransac; + std::shared_ptr absposeproblem_ptr( + new opengv::sac_problems::absolute_pose::MultiNoncentralAbsolutePoseSacProblem(adapter)); + + ransac.sac_model_ = absposeproblem_ptr; + ransac.threshold_ = 1.0 - cos(atan(reprojError/cameraModels[0].fx())); + ransac.max_iterations_ = iterations; + UDEBUG("Ransac params: threshold = %f (reprojError=%f fx=%f), max iterations=%d", ransac.threshold_, reprojError, cameraModels[0].fx(), ransac.max_iterations_); + + //Run the experiment + ransac.computeModel(); + + pnp = Transform::fromEigen3d(ransac.model_coefficients_); + + UDEBUG("Ransac result: %s", pnp.prettyPrint().c_str()); + UDEBUG("Ransac iterations done: %d", ransac.iterations_); + for (size_t i=0; i < cameraModels.size(); ++i) + { + inliers.insert(inliers.end(), ransac.inliers_[i].begin(), ransac.inliers_[i].end()); + } } + else + { + // convert 3d points + opengv::points_t points; + + // convert 2d-3d correspondences into bearing vectors + opengv::bearingVectors_t bearingVectors; + opengv::absolute_pose::NoncentralAbsoluteAdapter::camCorrespondences_t camCorrespondences; + + for(size_t i=0; i(0, 0)); - adapter.sett(opengv::translation_t(guess.x(), guess.y(), guess.z())); + adapter.setR(guess.toEigen4d().block<3,3>(0, 0)); + adapter.sett(opengv::translation_t(guess.x(), guess.y(), guess.z())); - //Create a AbsolutePoseSacProblem and Ransac - //The method is set to GP3P - opengv::sac::Ransac ransac; - std::shared_ptr absposeproblem_ptr( - new opengv::sac_problems::absolute_pose::AbsolutePoseSacProblem(adapter, opengv::sac_problems::absolute_pose::AbsolutePoseSacProblem::GP3P)); + //Create a AbsolutePoseSacProblem and Ransac + //The method is set to GP3P + opengv::sac::Ransac ransac; + std::shared_ptr absposeproblem_ptr( + new opengv::sac_problems::absolute_pose::AbsolutePoseSacProblem(adapter, opengv::sac_problems::absolute_pose::AbsolutePoseSacProblem::GP3P)); - ransac.sac_model_ = absposeproblem_ptr; - ransac.threshold_ = 1.0 - cos(atan(reprojError/cameraModels[0].fx())); - ransac.max_iterations_ = iterations; - UDEBUG("Ransac params: threshold = %f (reprojError=%f fx=%f), max iterations=%d", ransac.threshold_, reprojError, cameraModels[0].fx(), ransac.max_iterations_); + ransac.sac_model_ = absposeproblem_ptr; + ransac.threshold_ = 1.0 - cos(atan(reprojError/cameraModels[0].fx())); + ransac.max_iterations_ = iterations; + UDEBUG("Ransac params: threshold = %f (reprojError=%f fx=%f), max iterations=%d", ransac.threshold_, reprojError, cameraModels[0].fx(), ransac.max_iterations_); - //Run the experiment - ransac.computeModel(); + //Run the experiment + ransac.computeModel(); - Transform pnp = Transform::fromEigen3d(ransac.model_coefficients_); + pnp = Transform::fromEigen3d(ransac.model_coefficients_); + + UDEBUG("Ransac result: %s", pnp.prettyPrint().c_str()); + UDEBUG("Ransac iterations done: %d", ransac.iterations_); + inliers = ransac.inliers_; + } - UDEBUG("Ransac result: %s", pnp.prettyPrint().c_str()); - UDEBUG("Ransac iterations done: %d", ransac.iterations_); - inliers = ransac.inliers_; UDEBUG("Ransac inliers: %ld", inliers.size()); - if((int)inliers.size() >= minInliers) + if((int)inliers.size() >= minInliers && !pnp.isNull()) { transform = pnp; @@ -424,11 +534,11 @@ Transform estimateMotion3DTo2D( std::sort(errorSqrdDists.begin(), errorSqrdDists.end()); //divide by 4 instead of 2 to ignore very very far features (stereo) - double median_error_sqr_lin = 2.1981 * (double)errorSqrdDists[errorSqrdDists.size () >> 2]; + double median_error_sqr_lin = 2.1981 * (double)errorSqrdDists[errorSqrdDists.size () / varianceMedianRatio]; UASSERT(uIsFinite(median_error_sqr_lin)); (*covariance)(cv::Range(0,3), cv::Range(0,3)) *= median_error_sqr_lin; std::sort(errorSqrdAngles.begin(), errorSqrdAngles.end()); - double median_error_sqr_ang = 2.1981 * (double)errorSqrdAngles[errorSqrdAngles.size () >> 2]; + double median_error_sqr_ang = 2.1981 * (double)errorSqrdAngles[errorSqrdAngles.size () / varianceMedianRatio]; UASSERT(uIsFinite(median_error_sqr_ang)); (*covariance)(cv::Range(3,6), cv::Range(3,6)) *= median_error_sqr_ang; diff --git a/corelib/src/util3d_registration.cpp b/corelib/src/util3d_registration.cpp index 9b61dd9b2c..a424a7e088 100644 --- a/corelib/src/util3d_registration.cpp +++ b/corelib/src/util3d_registration.cpp @@ -244,7 +244,8 @@ void computeVarianceAndCorrespondencesImpl( double maxCorrespondenceDistance, double maxCorrespondenceAngle, double & variance, - int & correspondencesOut) + int & correspondencesOut, + bool reciprocal) { variance = 1; correspondencesOut = 0; @@ -255,7 +256,7 @@ void computeVarianceAndCorrespondencesImpl( est->setInputTarget(target); est->setInputSource(source); pcl::Correspondences correspondences; - est->determineCorrespondences(correspondences, maxCorrespondenceDistance); + est->determineReciprocalCorrespondences(correspondences, maxCorrespondenceDistance); if(correspondences.size()) { @@ -305,9 +306,10 @@ void computeVarianceAndCorrespondences( double maxCorrespondenceDistance, double maxCorrespondenceAngle, double & variance, - int & correspondencesOut) + int & correspondencesOut, + bool reciprocal) { - computeVarianceAndCorrespondencesImpl(cloudA, cloudB, maxCorrespondenceDistance, maxCorrespondenceAngle, variance, correspondencesOut); + computeVarianceAndCorrespondencesImpl(cloudA, cloudB, maxCorrespondenceDistance, maxCorrespondenceAngle, variance, correspondencesOut, reciprocal); } void computeVarianceAndCorrespondences( @@ -316,9 +318,10 @@ void computeVarianceAndCorrespondences( double maxCorrespondenceDistance, double maxCorrespondenceAngle, double & variance, - int & correspondencesOut) + int & correspondencesOut, + bool reciprocal) { - computeVarianceAndCorrespondencesImpl(cloudA, cloudB, maxCorrespondenceDistance, maxCorrespondenceAngle, variance, correspondencesOut); + computeVarianceAndCorrespondencesImpl(cloudA, cloudB, maxCorrespondenceDistance, maxCorrespondenceAngle, variance, correspondencesOut, reciprocal); } template @@ -327,7 +330,8 @@ void computeVarianceAndCorrespondencesImpl( const typename pcl::PointCloud::ConstPtr & cloudB, double maxCorrespondenceDistance, double & variance, - int & correspondencesOut) + int & correspondencesOut, + bool reciprocal) { variance = 1; correspondencesOut = 0; @@ -336,7 +340,7 @@ void computeVarianceAndCorrespondencesImpl( est->setInputTarget(cloudA->size()>cloudB->size()?cloudA:cloudB); est->setInputSource(cloudA->size()>cloudB->size()?cloudB:cloudA); pcl::Correspondences correspondences; - est->determineCorrespondences(correspondences, maxCorrespondenceDistance); + est->determineReciprocalCorrespondences(correspondences, maxCorrespondenceDistance); if(correspondences.size()>=3) { @@ -360,9 +364,10 @@ void computeVarianceAndCorrespondences( const pcl::PointCloud::ConstPtr & cloudB, double maxCorrespondenceDistance, double & variance, - int & correspondencesOut) + int & correspondencesOut, + bool reciprocal) { - computeVarianceAndCorrespondencesImpl(cloudA, cloudB, maxCorrespondenceDistance, variance, correspondencesOut); + computeVarianceAndCorrespondencesImpl(cloudA, cloudB, maxCorrespondenceDistance, variance, correspondencesOut, reciprocal); } void computeVarianceAndCorrespondences( @@ -370,9 +375,10 @@ void computeVarianceAndCorrespondences( const pcl::PointCloud::ConstPtr & cloudB, double maxCorrespondenceDistance, double & variance, - int & correspondencesOut) + int & correspondencesOut, + bool reciprocal) { - computeVarianceAndCorrespondencesImpl(cloudA, cloudB, maxCorrespondenceDistance, variance, correspondencesOut); + computeVarianceAndCorrespondencesImpl(cloudA, cloudB, maxCorrespondenceDistance, variance, correspondencesOut, reciprocal); } // return transform from source to target (All points must be finite!!!) diff --git a/docker/bionic/Dockerfile b/docker/bionic/Dockerfile index f31021abd8..e79f96b35c 100644 --- a/docker/bionic/Dockerfile +++ b/docker/bionic/Dockerfile @@ -5,38 +5,20 @@ FROM ros:melodic-perception # Install build dependencies RUN apt-get update && \ apt-get install -y git software-properties-common ros-melodic-rtabmap-ros && \ - apt-get remove -y ros-melodic-rtabmap && \ - rm -rf /var/lib/apt/lists/ + apt-get remove -y ros-melodic-rtabmap* && \ + apt-get clean && rm -rf /var/lib/apt/lists/ WORKDIR /root/ # GTSAM RUN add-apt-repository ppa:borglab/gtsam-release-4.0 -y -RUN apt install libgtsam-dev libgtsam-unstable-dev -y +RUN apt-get update && apt install libgtsam-dev libgtsam-unstable-dev -y && \ + apt-get clean && rm -rf /var/lib/apt/lists/ -# libpointmatcher -RUN git clone https://github.com/ethz-asl/libnabo.git -#commit February 13 2021 -RUN cd libnabo && \ - git checkout 3cab7eed92bd5d4aed997347b8c8a2692a83a532 && \ - mkdir build && \ - cd build && \ - cmake -DCMAKE_BUILD_TYPE=Release .. && \ - make -j$(nproc) && \ - make install && \ - cd && \ - rm -r libnabo -RUN git clone https://github.com/ethz-asl/libpointmatcher.git -#commit April 6 2021 -RUN cd libpointmatcher && \ - git checkout 76f99fce0fe69e6384102a0343fdf8d262626e1f && \ - mkdir build && \ - cd build && \ - cmake -DCMAKE_BUILD_TYPE=Release .. && \ - make -j$(nproc) && \ - make install && \ - cd && \ - rm -r libpointmatcher +# MRPT +RUN add-apt-repository ppa:joseluisblancoc/mrpt-stable -y +RUN apt-get update && apt install libmrpt-poses-dev -y && \ + apt-get clean && rm -rf /var/lib/apt/lists/ ARG TARGETPLATFORM ENV TARGETPLATFORM=${TARGETPLATFORM:-linux/amd64} @@ -46,65 +28,14 @@ RUN echo "I am building for $TARGETPLATFORM" RUN if [ "$TARGETPLATFORM" = "linux/arm64" ]; then ln -s /usr/bin/cmake ~/cmake; fi # cmake >=3.11 required for amd64 dependencies -RUN if [ "$TARGETPLATFORM" = "linux/amd64" ]; then apt install -y wget && \ +RUN if [ "$TARGETPLATFORM" = "linux/amd64" ]; then 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; fi - -# AliceVision v2.4.0 modified (Sept 13 2021) -RUN if [ "$TARGETPLATFORM" = "linux/amd64" ]; then apt-get update && DEBIAN_FRONTEND=noninteractive apt-get install -y \ - libsuitesparse-dev \ - libceres-dev \ - xorg-dev \ - libglu1-mesa-dev; fi -RUN if [ "$TARGETPLATFORM" = "linux/amd64" ]; then git clone https://github.com/OpenImageIO/oiio.git && \ - cd oiio && \ - git checkout Release-2.0.12 && \ - mkdir build && \ - cd build && \ - cmake -DUSE_PYTHON=OFF -DOIIO_BUILD_TESTS=OFF -DOIIO_BUILD_TOOLS=OFF .. && \ - make -j$(nproc) && \ - make install && \ - cd && \ - rm -r oiio; fi -RUN if [ "$TARGETPLATFORM" = "linux/amd64" ]; then git clone https://github.com/assimp/assimp.git && \ - cd assimp && \ - git checkout 71a87b653cd4b5671104fe49e2e38cf5dd4d8675 && \ - mkdir build && \ - cd build && \ - cmake .. && \ - make -j$(nproc) && \ - make install && \ - cd && \ - rm -r assimp; fi -RUN if [ "$TARGETPLATFORM" = "linux/amd64" ]; then git clone https://github.com/alicevision/geogram.git && \ - cd geogram && \ - git checkout v1.7.6 && \ - wget https://gist.githubusercontent.com/matlabbe/1df724465106c056ca4cc195c81d8cf0/raw/b3ed4cb8f9b270833a40d57d870a259eabfa4415/geogram_8b2ae61.patch && \ - git apply geogram_8b2ae61.patch && \ - ./configure.sh && \ - cd build/Linux64-gcc-dynamic-Release && \ - make -j$(nproc) && \ - make install && \ - cd && \ - rm -r geogram; fi -RUN if [ "$TARGETPLATFORM" = "linux/amd64" ]; then git clone https://github.com/alicevision/AliceVision.git --recursive && \ - cd AliceVision && \ - git checkout 0f6115b6af6183c524aa7fcf26141337c1cf3872 && \ - git submodule update -i && \ - wget https://gist.githubusercontent.com/matlabbe/1df724465106c056ca4cc195c81d8cf0/raw/b3ed4cb8f9b270833a40d57d870a259eabfa4415/alicevision_0f6115b.patch && \ - git apply alicevision_0f6115b.patch && \ - mkdir build && \ - cd build && \ - ~/cmake -DALICEVISION_USE_CUDA=OFF -DALICEVISION_USE_APRILTAG=OFF -DALICEVISION_BUILD_SOFTWARE=OFF .. && \ - make -j$(nproc) && \ - make install && \ - cd && \ - rm -r AliceVision; fi #commit Aug 6 2020 -RUN apt-get update && apt install wget +RUN apt-get update && apt install wget && apt-get clean && rm -rf /var/lib/apt/lists/ RUN git clone https://github.com/laurentkneip/opengv.git && \ cd opengv && \ git checkout 91f4b19c73450833a40e463ad3648aae80b3a7f3 && \ @@ -118,6 +49,9 @@ RUN git clone https://github.com/laurentkneip/opengv.git && \ cd && \ rm -r opengv +# Will be used to read/store databases on host +RUN mkdir -p /root/Documents/RTAB-Map + RUN rm /bin/sh && ln -s /bin/bash /bin/sh # Copy current source code @@ -126,7 +60,7 @@ COPY . /root/rtabmap # Build RTAB-Map project RUN source /ros_entrypoint.sh && \ cd rtabmap/build && \ - ~/cmake -DWITH_ALICE_VISION=ON -DWITH_OPENGV=ON .. && \ + ~/cmake -DWITH_OPENGV=ON .. && \ make -j$(nproc) && \ make install && \ cd ../.. && \ diff --git a/docker/bionic/nvidia/Dockerfile b/docker/bionic/nvidia/Dockerfile deleted file mode 100644 index 797f33de14..0000000000 --- a/docker/bionic/nvidia/Dockerfile +++ /dev/null @@ -1,13 +0,0 @@ -FROM introlab3it/rtabmap:bionic - -# nvidia-container-runtime -ENV NVIDIA_VISIBLE_DEVICES \ - ${NVIDIA_VISIBLE_DEVICES:-all} -ENV NVIDIA_DRIVER_CAPABILITIES \ - ${NVIDIA_DRIVER_CAPABILITIES:+$NVIDIA_DRIVER_CAPABILITIES,}graphics - -# Will be used to read/store databases on host -RUN mkdir -p /root/Documents/RTAB-Map - -# On Nvidia Jetpack, uncomment the following (https://github.com/introlab/rtabmap/issues/776): -# ENV LD_LIBRARY_PATH=$LD_LIBRARY_PATH:/usr/lib/aarch64-linux-gnu/tegra \ No newline at end of file diff --git a/docker/focal-foxy/Dockerfile b/docker/focal-foxy/Dockerfile new file mode 100644 index 0000000000..c75b2ff013 --- /dev/null +++ b/docker/focal-foxy/Dockerfile @@ -0,0 +1,37 @@ +# Image: introlab3it/rtabmap:focal-foxy + +FROM introlab3it/rtabmap:focal-foxy-deps + +# June 19 2023: moved opengv here so that focal-foxy-deps can be built on my computer. Not sure why but on my machine opengv arm64 fails, but not on CI. +#commit Aug 6 2020 +RUN apt-get update && apt install -y wget && \ + apt-get clean && rm -rf /var/lib/apt/lists/ +RUN git clone https://github.com/laurentkneip/opengv.git && \ + cd opengv && \ + git checkout 91f4b19c73450833a40e463ad3648aae80b3a7f3 && \ + wget https://gist.githubusercontent.com/matlabbe/a412cf7c4627253874f81a00745a7fbb/raw/accc3acf465d1ffd0304a46b17741f62d4d354ef/opengv_disable_march_native.patch && \ + git apply opengv_disable_march_native.patch && \ + mkdir build && \ + cd build && \ + cmake -DCMAKE_BUILD_TYPE=Release .. && \ + make -j$(nproc) && \ + make install && \ + cd && \ + rm -r opengv + +# Will be used to read/store databases on host +RUN mkdir -p /root/Documents/RTAB-Map && chmod 777 /root/Documents/RTAB-Map + +# Copy current source code +COPY . /root/rtabmap + +# Build RTAB-Map project +RUN source /ros_entrypoint.sh && \ + cd rtabmap/build && \ + cmake -DWITH_ALICE_VISION=ON -DWITH_OPENGV=ON .. && \ + make -j$(nproc) && \ + make install && \ + cd ../.. && \ + rm -rf rtabmap && \ + ldconfig + diff --git a/docker/focal-foxy/deps/Dockerfile b/docker/focal-foxy/deps/Dockerfile new file mode 100644 index 0000000000..54317f0537 --- /dev/null +++ b/docker/focal-foxy/deps/Dockerfile @@ -0,0 +1,176 @@ +# Image: introlab3it/rtabmap:focal-foxy-deps + +FROM ubuntu:20.04 + +RUN apt update && \ + apt install software-properties-common -y && \ + add-apt-repository universe && \ + apt update && \ + apt install curl -y && \ + curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key -o /usr/share/keyrings/ros-archive-keyring.gpg && \ + echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/ros-archive-keyring.gpg] http://packages.ros.org/ros2/ubuntu $(. /etc/os-release && echo $UBUNTU_CODENAME) main" | tee /etc/apt/sources.list.d/ros2.list > /dev/null && \ + apt-get clean && rm -rf /var/lib/apt/lists/ + +ENV DEBIAN_FRONTEND=noninteractive + +# Install build dependencies +RUN apt-get update && \ + apt upgrade -y && \ + apt-get install -y git ros-foxy-ros-base python3-argcomplete ros-dev-tools ros-foxy-rtabmap-ros && \ + apt-get remove -y ros-foxy-rtabmap && \ + apt-get clean && rm -rf /var/lib/apt/lists/ + +WORKDIR /root/ + +# GTSAM +RUN add-apt-repository ppa:borglab/gtsam-release-4.0 -y +RUN apt-get update && apt install libgtsam-dev libgtsam-unstable-dev -y && \ + apt-get clean && rm -rf /var/lib/apt/lists/ + +# MRPT +RUN add-apt-repository ppa:joseluisblancoc/mrpt-stable -y +RUN apt-get update && apt install libmrpt-poses-dev -y && \ + apt-get clean && rm -rf /var/lib/apt/lists/ + +# PDAL +RUN apt-get update && apt-get install -y libpdal-dev && \ + apt-get clean && rm -rf /var/lib/apt/lists/ + +# RealSense2 +RUN apt-get update && apt-get install -y ros-foxy-librealsense2 && \ + apt-get clean && rm -rf /var/lib/apt/lists/ + +ARG TARGETPLATFORM +ENV TARGETPLATFORM=${TARGETPLATFORM:-linux/amd64} +RUN echo "I am building for $TARGETPLATFORM" + +# Azure Kinect DK +# Taken from https://github.com/microsoft/Azure-Kinect-Sensor-SDK/issues/1190#issuecomment-822772494 +# K4A binaries on 20.04 not released yet, we should take those from 18.04 +RUN if [ "$TARGETPLATFORM" = "linux/amd64" ]; then echo "Installing k4a..." && \ + apt-get update && apt-get install -y curl && \ + echo "Download libk4a1.3_1.3.0_amd64.deb..." && \ + curl -sSL https://packages.microsoft.com/ubuntu/18.04/prod/pool/main/libk/libk4a1.3/libk4a1.3_1.3.0_amd64.deb > /tmp/libk4a1.3_1.3.0_amd64.deb && \ + echo "Download libk4a1.3-dev_1.3.0_amd64.deb..." && \ + curl -sSL https://packages.microsoft.com/ubuntu/18.04/prod/pool/main/libk/libk4a1.3-dev/libk4a1.3-dev_1.3.0_amd64.deb > /tmp/libk4a1.3-dev_1.3.0_amd64.deb && \ + echo "Download libk4abt1.0_1.0.0_amd64.deb..." && \ + curl -sSL https://packages.microsoft.com/ubuntu/18.04/prod/pool/main/libk/libk4abt1.0/libk4abt1.0_1.0.0_amd64.deb > /tmp/libk4abt1.0_1.0.0_amd64.deb && \ + echo "Download libk4abt1.0-dev_1.0.0_amd64.deb..." && \ + curl -sSL https://packages.microsoft.com/ubuntu/18.04/prod/pool/main/libk/libk4abt1.0-dev/libk4abt1.0-dev_1.0.0_amd64.deb > /tmp/libk4abt1.0-dev_1.0.0_amd64.deb && \ + echo "Download k4a-tools_1.3.0_amd64.deb..." && \ + curl -sSL https://packages.microsoft.com/ubuntu/18.04/prod/pool/main/k/k4a-tools/k4a-tools_1.3.0_amd64.deb > /tmp/k4a-tools_1.3.0_amd64.deb && \ + echo "Accept license..." && \ + echo 'libk4a1.3 libk4a1.3/accepted-eula-hash string 0f5d5c5de396e4fee4c0753a21fee0c1ed726cf0316204edda484f08cb266d76' | debconf-set-selections && \ + echo 'libk4abt1.0 libk4abt1.0/accepted-eula-hash string 03a13b63730639eeb6626d24fd45cf25131ee8e8e0df3f1b63f552269b176e38' | debconf-set-selections && \ + dpkg -i /tmp/libk4a1.3_1.3.0_amd64.deb && \ + dpkg -i /tmp/libk4a1.3-dev_1.3.0_amd64.deb && \ + dpkg -i /tmp/libk4abt1.0_1.0.0_amd64.deb && \ + dpkg -i /tmp/libk4abt1.0-dev_1.0.0_amd64.deb && \ + apt-get install -y libsoundio1 && \ + dpkg -i /tmp/k4a-tools_1.3.0_amd64.deb && \ + rm /tmp/libk4a* /tmp/k4a* && \ + apt-get clean && rm -rf /var/lib/apt/lists/; fi + +# libfreenect2 +RUN if [ "$TARGETPLATFORM" = "linux/amd64" ]; then echo "Installing libfreenect2..." && \ + apt-get update && apt-get install -y mesa-utils xserver-xorg-video-all libusb-1.0-0-dev libturbojpeg0-dev libglfw3-dev && \ + apt-get clean && rm -rf /var/lib/apt/lists/ && \ + git clone https://github.com/OpenKinect/libfreenect2 && \ + cd libfreenect2 && \ + mkdir build && \ + cd build && \ + cmake .. && \ + make -j$(nproc) && \ + make install && \ + cd && \ + rm -r libfreenect2; fi + +# zed open capture +RUN if [ "$TARGETPLATFORM" = "linux/amd64" ]; then echo "Installing zed-open-capture..." && \ + apt-get update && apt install -y libusb-1.0-0-dev libhidapi-libusb0 libhidapi-dev wget && \ + apt-get clean && rm -rf /var/lib/apt/lists/ && \ + git clone https://github.com/stereolabs/zed-open-capture.git && \ + cd zed-open-capture && \ + mkdir build && \ + cd build && \ + cmake .. && \ + make -j$(nproc) && \ + make install && \ + cd && \ + rm -r zed-open-capture; fi + +# AliceVision v2.4.0 modified (Sept 13 2021) +RUN if [ "$TARGETPLATFORM" = "linux/amd64" ]; then echo "Installing AliceVision..." && \ + apt-get update && DEBIAN_FRONTEND=noninteractive apt-get install -y \ + libsuitesparse-dev \ + libceres-dev \ + xorg-dev \ + libglu1-mesa-dev \ + wget && apt-get clean && rm -rf /var/lib/apt/lists/; fi +RUN if [ "$TARGETPLATFORM" = "linux/amd64" ]; then git clone https://github.com/OpenImageIO/oiio.git && \ + cd oiio && \ + git checkout Release-2.0.12 && \ + mkdir build && \ + cd build && \ + cmake -DUSE_PYTHON=OFF -DOIIO_BUILD_TESTS=OFF -DOIIO_BUILD_TOOLS=OFF .. && \ + make -j$(nproc) && \ + make install && \ + cd && \ + rm -r oiio; fi +RUN if [ "$TARGETPLATFORM" = "linux/amd64" ]; then git clone https://github.com/assimp/assimp.git && \ + cd assimp && \ + git checkout 71a87b653cd4b5671104fe49e2e38cf5dd4d8675 && \ + mkdir build && \ + cd build && \ + cmake .. && \ + make -j$(nproc) && \ + make install && \ + cd && \ + rm -r assimp; fi +# Ignore assimp installed from ros, as it has invalid "/usr/lib/include" in its target, making rtabmap's cmake fails +RUN if [ "$TARGETPLATFORM" = "linux/amd64" ]; then rm -rf /usr/lib/x86_64-linux-gnu/cmake/assimp*; fi +RUN if [ "$TARGETPLATFORM" = "linux/amd64" ]; then git clone https://github.com/alicevision/geogram.git && \ + cd geogram && \ + git checkout v1.7.6 && \ + wget https://gist.githubusercontent.com/matlabbe/1df724465106c056ca4cc195c81d8cf0/raw/b3ed4cb8f9b270833a40d57d870a259eabfa4415/geogram_8b2ae61.patch && \ + git apply geogram_8b2ae61.patch && \ + ./configure.sh && \ + cd build/Linux64-gcc-dynamic-Release && \ + make -j$(nproc) && \ + make install && \ + cd && \ + rm -r geogram; fi +RUN if [ "$TARGETPLATFORM" = "linux/amd64" ]; then git clone https://github.com/alicevision/AliceVision.git --recursive && \ + cd AliceVision && \ + git checkout 0f6115b6af6183c524aa7fcf26141337c1cf3872 && \ + git submodule update -i && \ + wget https://gist.githubusercontent.com/matlabbe/1df724465106c056ca4cc195c81d8cf0/raw/b3ed4cb8f9b270833a40d57d870a259eabfa4415/alicevision_0f6115b.patch && \ + git apply alicevision_0f6115b.patch && \ + mkdir build && \ + cd build && \ + cmake -DALICEVISION_USE_CUDA=OFF -DALICEVISION_USE_APRILTAG=OFF -DALICEVISION_BUILD_SOFTWARE=OFF .. && \ + make -j$(nproc) && \ + make install && \ + cd && \ + rm -r AliceVision; fi + +RUN git clone --branch 4.2.0 https://github.com/opencv/opencv.git && \ + git clone --branch 4.2.0 https://github.com/opencv/opencv_contrib.git && \ + cd opencv && \ + mkdir build && \ + cd build && \ + cmake -DBUILD_opencv_python3=OFF -DBUILD_opencv_python_bindings_generator=OFF -DBUILD_opencv_python_tests=OFF -DBUILD_PERF_TESTS=OFF -DBUILD_TESTS=OFF -DOPENCV_ENABLE_NONFREE=ON -DOPENCV_EXTRA_MODULES_PATH=/root/opencv_contrib/modules .. && \ + make -j$(nproc) && \ + make install && \ + cd ../.. && \ + rm -rf opencv opencv_contrib + +RUN rm /bin/sh && ln -s /bin/bash /bin/sh + +COPY ./docker/focal-foxy/deps/ros_entrypoint.sh /ros_entrypoint.sh +RUN chmod +x /ros_entrypoint.sh +ENTRYPOINT [ "/ros_entrypoint.sh" ] + +# for jetson (https://github.com/introlab/rtabmap/issues/776) +ENV LD_LIBRARY_PATH=$LD_LIBRARY_PATH:/usr/lib/aarch64-linux-gnu/tegra + diff --git a/docker/focal-foxy/deps/ros_entrypoint.sh b/docker/focal-foxy/deps/ros_entrypoint.sh new file mode 100644 index 0000000000..60b286226e --- /dev/null +++ b/docker/focal-foxy/deps/ros_entrypoint.sh @@ -0,0 +1,6 @@ +#!/bin/bash +set -e + +# setup ros2 environment +source "/opt/ros/foxy/setup.bash" -- +exec "$@" diff --git a/docker/focal/Dockerfile b/docker/focal/Dockerfile index 5cb3686b6f..277f18c920 100644 --- a/docker/focal/Dockerfile +++ b/docker/focal/Dockerfile @@ -1,162 +1,10 @@ # Image: introlab3it/rtabmap:focal -FROM ros:noetic-perception +FROM introlab3it/rtabmap:focal-deps -# Install build dependencies -RUN apt-get update && \ - apt-get install -y git software-properties-common ros-noetic-rtabmap-ros && \ - apt-get remove -y ros-noetic-rtabmap && \ - rm -rf /var/lib/apt/lists/ - -WORKDIR /root/ - -# GTSAM -RUN add-apt-repository ppa:borglab/gtsam-release-4.0 -y -RUN apt install libgtsam-dev libgtsam-unstable-dev -y - -# libpointmatcher -RUN git clone https://github.com/ethz-asl/libnabo.git -#commit February 13 2021 -RUN cd libnabo && \ - git checkout 3cab7eed92bd5d4aed997347b8c8a2692a83a532 && \ - mkdir build && \ - cd build && \ - cmake -DCMAKE_BUILD_TYPE=Release .. && \ - make -j$(nproc) && \ - make install && \ - cd && \ - rm -r libnabo -RUN git clone https://github.com/ethz-asl/libpointmatcher.git -#commit April 6 2021 -RUN cd libpointmatcher && \ - git checkout 76f99fce0fe69e6384102a0343fdf8d262626e1f && \ - mkdir build && \ - cd build && \ - cmake -DCMAKE_BUILD_TYPE=Release .. && \ - make -j$(nproc) && \ - make install && \ - cd && \ - rm -r libpointmatcher - -# PDAL -RUN apt-get install -y libpdal-dev - -# RealSense2 -RUN apt-get install -y ros-noetic-librealsense2 - -ARG TARGETPLATFORM -ENV TARGETPLATFORM=${TARGETPLATFORM:-linux/amd64} -RUN echo "I am building for $TARGETPLATFORM" - -ENV DEBIAN_FRONTEND=noninteractive - -# Azure Kinect DK -# Taken from https://github.com/microsoft/Azure-Kinect-Sensor-SDK/issues/1190#issuecomment-822772494 -# K4A binaries on 20.04 not released yet, we should take those from 18.04 -RUN if [ "$TARGETPLATFORM" = "linux/amd64" ]; then echo "Installing k4a..." && \ - apt-get update && apt-get install -y curl && \ - echo "Download libk4a1.3_1.3.0_amd64.deb..." && \ - curl -sSL https://packages.microsoft.com/ubuntu/18.04/prod/pool/main/libk/libk4a1.3/libk4a1.3_1.3.0_amd64.deb > /tmp/libk4a1.3_1.3.0_amd64.deb && \ - echo "Download libk4a1.3-dev_1.3.0_amd64.deb..." && \ - curl -sSL https://packages.microsoft.com/ubuntu/18.04/prod/pool/main/libk/libk4a1.3-dev/libk4a1.3-dev_1.3.0_amd64.deb > /tmp/libk4a1.3-dev_1.3.0_amd64.deb && \ - echo "Download libk4abt1.0_1.0.0_amd64.deb..." && \ - curl -sSL https://packages.microsoft.com/ubuntu/18.04/prod/pool/main/libk/libk4abt1.0/libk4abt1.0_1.0.0_amd64.deb > /tmp/libk4abt1.0_1.0.0_amd64.deb && \ - echo "Download libk4abt1.0-dev_1.0.0_amd64.deb..." && \ - curl -sSL https://packages.microsoft.com/ubuntu/18.04/prod/pool/main/libk/libk4abt1.0-dev/libk4abt1.0-dev_1.0.0_amd64.deb > /tmp/libk4abt1.0-dev_1.0.0_amd64.deb && \ - echo "Download k4a-tools_1.3.0_amd64.deb..." && \ - curl -sSL https://packages.microsoft.com/ubuntu/18.04/prod/pool/main/k/k4a-tools/k4a-tools_1.3.0_amd64.deb > /tmp/k4a-tools_1.3.0_amd64.deb && \ - echo "Accept license..." && \ - echo 'libk4a1.3 libk4a1.3/accepted-eula-hash string 0f5d5c5de396e4fee4c0753a21fee0c1ed726cf0316204edda484f08cb266d76' | debconf-set-selections && \ - echo 'libk4abt1.0 libk4abt1.0/accepted-eula-hash string 03a13b63730639eeb6626d24fd45cf25131ee8e8e0df3f1b63f552269b176e38' | debconf-set-selections && \ - dpkg -i /tmp/libk4a1.3_1.3.0_amd64.deb && \ - dpkg -i /tmp/libk4a1.3-dev_1.3.0_amd64.deb && \ - dpkg -i /tmp/libk4abt1.0_1.0.0_amd64.deb && \ - dpkg -i /tmp/libk4abt1.0-dev_1.0.0_amd64.deb && \ - apt-get install -y libsoundio1 && \ - dpkg -i /tmp/k4a-tools_1.3.0_amd64.deb && \ - rm /tmp/libk4a* /tmp/k4a*; fi - -# libfreenect2 -RUN if [ "$TARGETPLATFORM" = "linux/amd64" ]; then echo "Installing libfreenect2..." && \ - apt-get update && apt-get install -y mesa-utils xserver-xorg-video-all libusb-1.0-0-dev libturbojpeg0-dev libglfw3-dev && \ - git clone https://github.com/OpenKinect/libfreenect2 && \ - cd libfreenect2 && \ - mkdir build && \ - cd build && \ - cmake .. && \ - make -j$(nproc) && \ - make install && \ - cd && \ - rm -r libfreenect2; fi - -# zed open capture -RUN if [ "$TARGETPLATFORM" = "linux/amd64" ]; then echo "Installing zed-open-capture..." && \ - apt-get update && apt install libusb-1.0-0-dev libhidapi-libusb0 libhidapi-dev wget && \ - git clone https://github.com/stereolabs/zed-open-capture.git && \ - cd zed-open-capture && \ - mkdir build && \ - cd build && \ - cmake .. && \ - make -j$(nproc) && \ - make install && \ - cd && \ - rm -r zed-open-capture; fi - -# AliceVision v2.4.0 modified (Sept 13 2021) -RUN if [ "$TARGETPLATFORM" = "linux/amd64" ]; then echo "Installing AliceVision..." && \ - apt-get update && DEBIAN_FRONTEND=noninteractive apt-get install -y \ - libsuitesparse-dev \ - libceres-dev \ - xorg-dev \ - libglu1-mesa-dev \ - wget; fi -RUN if [ "$TARGETPLATFORM" = "linux/amd64" ]; then git clone https://github.com/OpenImageIO/oiio.git && \ - cd oiio && \ - git checkout Release-2.0.12 && \ - mkdir build && \ - cd build && \ - cmake -DUSE_PYTHON=OFF -DOIIO_BUILD_TESTS=OFF -DOIIO_BUILD_TOOLS=OFF .. && \ - make -j$(nproc) && \ - make install && \ - cd && \ - rm -r oiio; fi -RUN if [ "$TARGETPLATFORM" = "linux/amd64" ]; then git clone https://github.com/assimp/assimp.git && \ - cd assimp && \ - git checkout 71a87b653cd4b5671104fe49e2e38cf5dd4d8675 && \ - mkdir build && \ - cd build && \ - cmake .. && \ - make -j$(nproc) && \ - make install && \ - cd && \ - rm -r assimp; fi -RUN if [ "$TARGETPLATFORM" = "linux/amd64" ]; then git clone https://github.com/alicevision/geogram.git && \ - cd geogram && \ - git checkout v1.7.6 && \ - wget https://gist.githubusercontent.com/matlabbe/1df724465106c056ca4cc195c81d8cf0/raw/b3ed4cb8f9b270833a40d57d870a259eabfa4415/geogram_8b2ae61.patch && \ - git apply geogram_8b2ae61.patch && \ - ./configure.sh && \ - cd build/Linux64-gcc-dynamic-Release && \ - make -j$(nproc) && \ - make install && \ - cd && \ - rm -r geogram; fi -RUN if [ "$TARGETPLATFORM" = "linux/amd64" ]; then git clone https://github.com/alicevision/AliceVision.git --recursive && \ - cd AliceVision && \ - git checkout 0f6115b6af6183c524aa7fcf26141337c1cf3872 && \ - git submodule update -i && \ - wget https://gist.githubusercontent.com/matlabbe/1df724465106c056ca4cc195c81d8cf0/raw/b3ed4cb8f9b270833a40d57d870a259eabfa4415/alicevision_0f6115b.patch && \ - git apply alicevision_0f6115b.patch && \ - mkdir build && \ - cd build && \ - cmake -DALICEVISION_USE_CUDA=OFF -DALICEVISION_USE_APRILTAG=OFF -DALICEVISION_BUILD_SOFTWARE=OFF .. && \ - make -j$(nproc) && \ - make install && \ - cd && \ - rm -r AliceVision; fi - +# June 19 2023: moved opengv here so that focal-deps can be built on my computer. Not sure why but on my machine opengv arm64 fails, but not on CI. #commit Aug 6 2020 -RUN apt-get update && apt install wget +RUN apt-get update && apt install wget && apt-get clean && rm -rf /var/lib/apt/lists/ RUN git clone https://github.com/laurentkneip/opengv.git && \ cd opengv && \ git checkout 91f4b19c73450833a40e463ad3648aae80b3a7f3 && \ @@ -170,7 +18,8 @@ RUN git clone https://github.com/laurentkneip/opengv.git && \ cd && \ rm -r opengv -RUN rm /bin/sh && ln -s /bin/bash /bin/sh +# Will be used to read/store databases on host +RUN mkdir -p /root/Documents/RTAB-Map && chmod 777 /root/Documents/RTAB-Map # Copy current source code COPY . /root/rtabmap @@ -184,4 +33,3 @@ RUN source /ros_entrypoint.sh && \ cd ../.. && \ rm -rf rtabmap && \ ldconfig - diff --git a/docker/focal/deps/Dockerfile b/docker/focal/deps/Dockerfile new file mode 100644 index 0000000000..870d23de1f --- /dev/null +++ b/docker/focal/deps/Dockerfile @@ -0,0 +1,161 @@ +# Image: introlab3it/rtabmap:focal + +FROM ros:noetic-perception + +# Install build dependencies +RUN apt-get update && \ + apt-get install -y git software-properties-common ros-noetic-rtabmap-ros && \ + apt-get remove -y ros-noetic-rtabmap* && \ + apt-get clean && rm -rf /var/lib/apt/lists/ + +WORKDIR /root/ + +# GTSAM +RUN add-apt-repository ppa:borglab/gtsam-release-4.0 -y +RUN apt-get update && apt install libgtsam-dev libgtsam-unstable-dev -y && \ + apt-get clean && rm -rf /var/lib/apt/lists/ + +# MRPT +RUN add-apt-repository ppa:joseluisblancoc/mrpt-stable -y +RUN apt-get update && apt install libmrpt-poses-dev -y && \ + apt-get clean && rm -rf /var/lib/apt/lists/ + +# PDAL +RUN apt-get update && apt-get install -y libpdal-dev && \ + apt-get clean && rm -rf /var/lib/apt/lists/ + +# RealSense2 +RUN apt-get update && apt-get install -y ros-noetic-librealsense2 && \ + apt-get clean && rm -rf /var/lib/apt/lists/ + +ARG TARGETPLATFORM +ENV TARGETPLATFORM=${TARGETPLATFORM:-linux/amd64} +RUN echo "I am building for $TARGETPLATFORM" + +ENV DEBIAN_FRONTEND=noninteractive + +# Azure Kinect DK +# Taken from https://github.com/microsoft/Azure-Kinect-Sensor-SDK/issues/1190#issuecomment-822772494 +# K4A binaries on 20.04 not released yet, we should take those from 18.04 +RUN if [ "$TARGETPLATFORM" = "linux/amd64" ]; then echo "Installing k4a..." && \ + apt-get update && apt-get install -y curl && \ + echo "Download libk4a1.3_1.3.0_amd64.deb..." && \ + curl -sSL https://packages.microsoft.com/ubuntu/18.04/prod/pool/main/libk/libk4a1.3/libk4a1.3_1.3.0_amd64.deb > /tmp/libk4a1.3_1.3.0_amd64.deb && \ + echo "Download libk4a1.3-dev_1.3.0_amd64.deb..." && \ + curl -sSL https://packages.microsoft.com/ubuntu/18.04/prod/pool/main/libk/libk4a1.3-dev/libk4a1.3-dev_1.3.0_amd64.deb > /tmp/libk4a1.3-dev_1.3.0_amd64.deb && \ + echo "Download libk4abt1.0_1.0.0_amd64.deb..." && \ + curl -sSL https://packages.microsoft.com/ubuntu/18.04/prod/pool/main/libk/libk4abt1.0/libk4abt1.0_1.0.0_amd64.deb > /tmp/libk4abt1.0_1.0.0_amd64.deb && \ + echo "Download libk4abt1.0-dev_1.0.0_amd64.deb..." && \ + curl -sSL https://packages.microsoft.com/ubuntu/18.04/prod/pool/main/libk/libk4abt1.0-dev/libk4abt1.0-dev_1.0.0_amd64.deb > /tmp/libk4abt1.0-dev_1.0.0_amd64.deb && \ + echo "Download k4a-tools_1.3.0_amd64.deb..." && \ + curl -sSL https://packages.microsoft.com/ubuntu/18.04/prod/pool/main/k/k4a-tools/k4a-tools_1.3.0_amd64.deb > /tmp/k4a-tools_1.3.0_amd64.deb && \ + echo "Accept license..." && \ + echo 'libk4a1.3 libk4a1.3/accepted-eula-hash string 0f5d5c5de396e4fee4c0753a21fee0c1ed726cf0316204edda484f08cb266d76' | debconf-set-selections && \ + echo 'libk4abt1.0 libk4abt1.0/accepted-eula-hash string 03a13b63730639eeb6626d24fd45cf25131ee8e8e0df3f1b63f552269b176e38' | debconf-set-selections && \ + dpkg -i /tmp/libk4a1.3_1.3.0_amd64.deb && \ + dpkg -i /tmp/libk4a1.3-dev_1.3.0_amd64.deb && \ + dpkg -i /tmp/libk4abt1.0_1.0.0_amd64.deb && \ + dpkg -i /tmp/libk4abt1.0-dev_1.0.0_amd64.deb && \ + apt-get install -y libsoundio1 && \ + dpkg -i /tmp/k4a-tools_1.3.0_amd64.deb && \ + rm /tmp/libk4a* /tmp/k4a* && \ + apt-get clean && rm -rf /var/lib/apt/lists/; fi + +# libfreenect2 +RUN if [ "$TARGETPLATFORM" = "linux/amd64" ]; then echo "Installing libfreenect2..." && \ + apt-get update && apt-get install -y mesa-utils xserver-xorg-video-all libusb-1.0-0-dev libturbojpeg0-dev libglfw3-dev && \ + apt-get clean && rm -rf /var/lib/apt/lists/ && \ + git clone https://github.com/OpenKinect/libfreenect2 && \ + cd libfreenect2 && \ + mkdir build && \ + cd build && \ + cmake .. && \ + make -j$(nproc) && \ + make install && \ + cd && \ + rm -r libfreenect2; fi + +# zed open capture +RUN if [ "$TARGETPLATFORM" = "linux/amd64" ]; then echo "Installing zed-open-capture..." && \ + apt-get update && apt install libusb-1.0-0-dev libhidapi-libusb0 libhidapi-dev wget && \ + apt-get clean && rm -rf /var/lib/apt/lists/ && \ + git clone https://github.com/stereolabs/zed-open-capture.git && \ + cd zed-open-capture && \ + mkdir build && \ + cd build && \ + cmake .. && \ + make -j$(nproc) && \ + make install && \ + cd && \ + rm -r zed-open-capture; fi + +# AliceVision v2.4.0 modified (Sept 13 2021) +RUN if [ "$TARGETPLATFORM" = "linux/amd64" ]; then echo "Installing AliceVision..." && \ + apt-get update && DEBIAN_FRONTEND=noninteractive apt-get install -y \ + libsuitesparse-dev \ + libceres-dev \ + xorg-dev \ + libglu1-mesa-dev \ + wget && apt-get clean && rm -rf /var/lib/apt/lists/; fi +RUN if [ "$TARGETPLATFORM" = "linux/amd64" ]; then git clone https://github.com/OpenImageIO/oiio.git && \ + cd oiio && \ + git checkout Release-2.0.12 && \ + mkdir build && \ + cd build && \ + cmake -DUSE_PYTHON=OFF -DOIIO_BUILD_TESTS=OFF -DOIIO_BUILD_TOOLS=OFF .. && \ + make -j$(nproc) && \ + make install && \ + cd && \ + rm -r oiio; fi +RUN if [ "$TARGETPLATFORM" = "linux/amd64" ]; then git clone https://github.com/assimp/assimp.git && \ + cd assimp && \ + git checkout 71a87b653cd4b5671104fe49e2e38cf5dd4d8675 && \ + mkdir build && \ + cd build && \ + cmake .. && \ + make -j$(nproc) && \ + make install && \ + cd && \ + rm -r assimp; fi +# Ignore assimp installed from ros, as it has invalid "/usr/lib/include" in its target, making rtabmap's cmake fails +RUN if [ "$TARGETPLATFORM" = "linux/amd64" ]; then rm -rf /usr/lib/x86_64-linux-gnu/cmake/assimp*; fi +RUN if [ "$TARGETPLATFORM" = "linux/amd64" ]; then git clone https://github.com/alicevision/geogram.git && \ + cd geogram && \ + git checkout v1.7.6 && \ + wget https://gist.githubusercontent.com/matlabbe/1df724465106c056ca4cc195c81d8cf0/raw/b3ed4cb8f9b270833a40d57d870a259eabfa4415/geogram_8b2ae61.patch && \ + git apply geogram_8b2ae61.patch && \ + ./configure.sh && \ + cd build/Linux64-gcc-dynamic-Release && \ + make -j$(nproc) && \ + make install && \ + cd && \ + rm -r geogram; fi +RUN if [ "$TARGETPLATFORM" = "linux/amd64" ]; then git clone https://github.com/alicevision/AliceVision.git --recursive && \ + cd AliceVision && \ + git checkout 0f6115b6af6183c524aa7fcf26141337c1cf3872 && \ + git submodule update -i && \ + wget https://gist.githubusercontent.com/matlabbe/1df724465106c056ca4cc195c81d8cf0/raw/b3ed4cb8f9b270833a40d57d870a259eabfa4415/alicevision_0f6115b.patch && \ + git apply alicevision_0f6115b.patch && \ + mkdir build && \ + cd build && \ + cmake -DALICEVISION_USE_CUDA=OFF -DALICEVISION_USE_APRILTAG=OFF -DALICEVISION_BUILD_SOFTWARE=OFF .. && \ + make -j$(nproc) && \ + make install && \ + cd && \ + rm -r AliceVision; fi + +RUN git clone --branch 4.2.0 https://github.com/opencv/opencv.git && \ + git clone --branch 4.2.0 https://github.com/opencv/opencv_contrib.git && \ + cd opencv && \ + mkdir build && \ + cd build && \ + cmake -DBUILD_opencv_python3=OFF -DBUILD_opencv_python_bindings_generator=OFF -DBUILD_opencv_python_tests=OFF -DBUILD_PERF_TESTS=OFF -DBUILD_TESTS=OFF -DOPENCV_ENABLE_NONFREE=ON -DOPENCV_EXTRA_MODULES_PATH=/root/opencv_contrib/modules .. && \ + make -j$(nproc) && \ + make install && \ + cd ../.. && \ + rm -rf opencv opencv_contrib + +RUN rm /bin/sh && ln -s /bin/bash /bin/sh + +# for jetson (https://github.com/introlab/rtabmap/issues/776) +ENV LD_LIBRARY_PATH=$LD_LIBRARY_PATH:/usr/lib/aarch64-linux-gnu/tegra diff --git a/docker/focal/hooks/build b/docker/focal/hooks/build deleted file mode 100644 index e1c97536d2..0000000000 --- a/docker/focal/hooks/build +++ /dev/null @@ -1,2 +0,0 @@ -#!/bin/bash -docker build --build-arg CACHE_DATE="$(date)" --cache-from $IMAGE_NAME -f $DOCKERFILE_PATH -t $IMAGE_NAME -t $DOCKER_REPO:20.04 . diff --git a/docker/focal/hooks/post_push b/docker/focal/hooks/post_push deleted file mode 100644 index cf10a22bed..0000000000 --- a/docker/focal/hooks/post_push +++ /dev/null @@ -1,2 +0,0 @@ -#!/bin/bash -docker push $DOCKER_REPO:20.04 diff --git a/docker/focal/nvidia/Dockerfile b/docker/focal/nvidia/Dockerfile deleted file mode 100644 index 49b3783784..0000000000 --- a/docker/focal/nvidia/Dockerfile +++ /dev/null @@ -1,13 +0,0 @@ -FROM introlab3it/rtabmap:focal - -# nvidia-container-runtime -ENV NVIDIA_VISIBLE_DEVICES \ - ${NVIDIA_VISIBLE_DEVICES:-all} -ENV NVIDIA_DRIVER_CAPABILITIES \ - ${NVIDIA_DRIVER_CAPABILITIES:+$NVIDIA_DRIVER_CAPABILITIES,}graphics - -# Will be used to read/store databases on host -RUN mkdir -p /root/Documents/RTAB-Map - -# On Nvidia Jetpack, uncomment the following (https://github.com/introlab/rtabmap/issues/776): -# ENV LD_LIBRARY_PATH=$LD_LIBRARY_PATH:/usr/lib/aarch64-linux-gnu/tegra diff --git a/docker/jammy-iron/Dockerfile b/docker/jammy-iron/Dockerfile new file mode 100644 index 0000000000..8228d9057f --- /dev/null +++ b/docker/jammy-iron/Dockerfile @@ -0,0 +1,37 @@ +# Image: introlab3it/rtabmap:jammy-iron + +FROM introlab3it/rtabmap:jammy-iron-deps + +# June 19 2023: moved opengv here so that jammy-deps can be built on my computer. Not sure why but on my machine opengv arm64 fails, but not on CI. +#commit Aug 6 2020 +RUN apt-get update && apt install -y wget && \ + apt-get clean && rm -rf /var/lib/apt/lists/ +RUN git clone https://github.com/laurentkneip/opengv.git && \ + cd opengv && \ + git checkout 91f4b19c73450833a40e463ad3648aae80b3a7f3 && \ + wget https://gist.githubusercontent.com/matlabbe/a412cf7c4627253874f81a00745a7fbb/raw/accc3acf465d1ffd0304a46b17741f62d4d354ef/opengv_disable_march_native.patch && \ + git apply opengv_disable_march_native.patch && \ + mkdir build && \ + cd build && \ + cmake -DCMAKE_BUILD_TYPE=Release .. && \ + make -j$(nproc) && \ + make install && \ + cd && \ + rm -r opengv + +# Will be used to read/store databases on host +RUN mkdir -p /root/Documents/RTAB-Map && chmod 777 /root/Documents/RTAB-Map + +# Copy current source code +COPY . /root/rtabmap + +# Build RTAB-Map project +RUN source /ros_entrypoint.sh && \ + cd rtabmap/build && \ + cmake -DWITH_OPENGV=ON .. && \ + make -j$(nproc) && \ + make install && \ + cd ../.. && \ + rm -rf rtabmap && \ + ldconfig + diff --git a/docker/jammy-iron/deps/Dockerfile b/docker/jammy-iron/deps/Dockerfile new file mode 100644 index 0000000000..1f525394d3 --- /dev/null +++ b/docker/jammy-iron/deps/Dockerfile @@ -0,0 +1,88 @@ +# Image: introlab3it/rtabmap:jammy-iron-deps + +FROM ubuntu:22.04 + +# Install ROS2 +RUN apt update && \ + apt install software-properties-common -y && \ + add-apt-repository universe && \ + apt update && \ + apt install curl -y && \ + curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key -o /usr/share/keyrings/ros-archive-keyring.gpg && \ + echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/ros-archive-keyring.gpg] http://packages.ros.org/ros2/ubuntu $(. /etc/os-release && echo $UBUNTU_CODENAME) main" | tee /etc/apt/sources.list.d/ros2.list > /dev/null && \ + apt-get clean && rm -rf /var/lib/apt/lists/ + +ENV DEBIAN_FRONTEND=noninteractive + +# Install build dependencies +RUN apt-get update && \ + apt upgrade -y && \ + apt-get install -y git ros-iron-ros-base ros-dev-tools ros-iron-rtabmap-ros && \ + apt-get remove -y ros-iron-rtabmap* && \ + apt-get clean && rm -rf /var/lib/apt/lists/ + +WORKDIR /root/ + +# PDAL +RUN apt-get update && apt-get install -y libpdal-dev && \ + apt-get clean && rm -rf /var/lib/apt/lists/ + +# RealSense2 +RUN apt-get update && apt-get install -y ros-iron-librealsense2 && \ + apt-get clean && rm -rf /var/lib/apt/lists/ + +ARG TARGETPLATFORM +ENV TARGETPLATFORM=${TARGETPLATFORM:-linux/amd64} +RUN echo "I am building for $TARGETPLATFORM" + +# libfreenect2 +RUN if [ "$TARGETPLATFORM" = "linux/amd64" ]; then echo "Installing libfreenect2..." && \ + apt-get update && apt-get install -y mesa-utils xserver-xorg-video-all libusb-1.0-0-dev libturbojpeg0-dev libglfw3-dev && \ + apt-get clean && rm -rf /var/lib/apt/lists/ && \ + git clone https://github.com/OpenKinect/libfreenect2 && \ + cd libfreenect2 && \ + mkdir build && \ + cd build && \ + cmake .. && \ + make -j$(nproc) && \ + make install && \ + cd && \ + rm -r libfreenect2; fi + +# zed open capture +RUN if [ "$TARGETPLATFORM" = "linux/amd64" ]; then echo "Installing zed-open-capture..." && \ + apt-get update && apt install -y libusb-1.0-0-dev libhidapi-libusb0 libhidapi-dev wget && \ + apt-get clean && rm -rf /var/lib/apt/lists/ && \ + git clone https://github.com/stereolabs/zed-open-capture.git && \ + cd zed-open-capture && \ + mkdir build && \ + cd build && \ + cmake .. && \ + make -j$(nproc) && \ + make install && \ + cd && \ + rm -r zed-open-capture; fi + +RUN git clone --branch 4.5.4 https://github.com/opencv/opencv.git && \ + git clone --branch 4.5.4 https://github.com/opencv/opencv_contrib.git && \ + cd opencv && \ + mkdir build && \ + cd build && \ + cmake -DBUILD_opencv_python3=OFF -DBUILD_opencv_python_bindings_generator=OFF -DBUILD_opencv_python_tests=OFF -DBUILD_PERF_TESTS=OFF -DBUILD_TESTS=OFF -DOPENCV_ENABLE_NONFREE=ON -DOPENCV_EXTRA_MODULES_PATH=/root/opencv_contrib/modules .. && \ + make -j$(nproc) && \ + make install && \ + cd ../.. && \ + rm -rf opencv opencv_contrib + +RUN rm /bin/sh && ln -s /bin/bash /bin/sh + +COPY ./docker/jammy-iron/deps/ros_entrypoint.sh /ros_entrypoint.sh +RUN chmod +x /ros_entrypoint.sh +ENTRYPOINT [ "/ros_entrypoint.sh" ] + +# ros2 seems not sourcing by default its multi-arch folders +ENV LD_LIBRARY_PATH=$LD_LIBRARY_PATH:/opt/ros/iron/lib/x86_64-linux-gnu:/opt/ros/iron/lib/aarch64-linux-gnu + +# for jetson (https://github.com/introlab/rtabmap/issues/776) +ENV LD_LIBRARY_PATH=$LD_LIBRARY_PATH:/usr/lib/aarch64-linux-gnu/tegra + diff --git a/docker/jammy-iron/deps/ros_entrypoint.sh b/docker/jammy-iron/deps/ros_entrypoint.sh new file mode 100644 index 0000000000..6285c32365 --- /dev/null +++ b/docker/jammy-iron/deps/ros_entrypoint.sh @@ -0,0 +1,6 @@ +#!/bin/bash +set -e + +# setup ros2 environment +source "/opt/ros/iron/setup.bash" -- +exec "$@" diff --git a/docker/jammy/Dockerfile b/docker/jammy/Dockerfile new file mode 100644 index 0000000000..c7dbc07cae --- /dev/null +++ b/docker/jammy/Dockerfile @@ -0,0 +1,37 @@ +# Image: introlab3it/rtabmap:jammy + +FROM introlab3it/rtabmap:jammy-deps + +# June 19 2023: moved opengv here so that jammy-deps can be built on my computer. Not sure why but on my machine opengv arm64 fails, but not on CI. +#commit Aug 6 2020 +RUN apt-get update && apt install -y wget && \ + apt-get clean && rm -rf /var/lib/apt/lists/ +RUN git clone https://github.com/laurentkneip/opengv.git && \ + cd opengv && \ + git checkout 91f4b19c73450833a40e463ad3648aae80b3a7f3 && \ + wget https://gist.githubusercontent.com/matlabbe/a412cf7c4627253874f81a00745a7fbb/raw/accc3acf465d1ffd0304a46b17741f62d4d354ef/opengv_disable_march_native.patch && \ + git apply opengv_disable_march_native.patch && \ + mkdir build && \ + cd build && \ + cmake -DCMAKE_BUILD_TYPE=Release .. && \ + make -j$(nproc) && \ + make install && \ + cd && \ + rm -r opengv + +# Will be used to read/store databases on host +RUN mkdir -p /root/Documents/RTAB-Map && chmod 777 /root/Documents/RTAB-Map + +# Copy current source code +COPY . /root/rtabmap + +# Build RTAB-Map project +RUN source /ros_entrypoint.sh && \ + cd rtabmap/build && \ + cmake -DWITH_OPENGV=ON .. && \ + make -j$(nproc) && \ + make install && \ + cd ../.. && \ + rm -rf rtabmap && \ + ldconfig + diff --git a/docker/jammy/deps/Dockerfile b/docker/jammy/deps/Dockerfile new file mode 100644 index 0000000000..05d671df9d --- /dev/null +++ b/docker/jammy/deps/Dockerfile @@ -0,0 +1,88 @@ +# Image: introlab3it/rtabmap:jammy-deps + +FROM ubuntu:22.04 + +# Install ROS2 +RUN apt update && \ + apt install software-properties-common -y && \ + add-apt-repository universe && \ + apt update && \ + apt install curl -y && \ + curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key -o /usr/share/keyrings/ros-archive-keyring.gpg && \ + echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/ros-archive-keyring.gpg] http://packages.ros.org/ros2/ubuntu $(. /etc/os-release && echo $UBUNTU_CODENAME) main" | tee /etc/apt/sources.list.d/ros2.list > /dev/null && \ + apt-get clean && rm -rf /var/lib/apt/lists/ + +ENV DEBIAN_FRONTEND=noninteractive + +# Install build dependencies +RUN apt-get update && \ + apt upgrade -y && \ + apt-get install -y git ros-humble-ros-base ros-dev-tools ros-humble-rtabmap-ros && \ + apt-get remove -y ros-humble-rtabmap* && \ + apt-get clean && rm -rf /var/lib/apt/lists/ + +WORKDIR /root/ + +# PDAL +RUN apt-get update && apt-get install -y libpdal-dev && \ + apt-get clean && rm -rf /var/lib/apt/lists/ + +# RealSense2 +RUN apt-get update && apt-get install -y ros-humble-librealsense2 && \ + apt-get clean && rm -rf /var/lib/apt/lists/ + +ARG TARGETPLATFORM +ENV TARGETPLATFORM=${TARGETPLATFORM:-linux/amd64} +RUN echo "I am building for $TARGETPLATFORM" + +# libfreenect2 +RUN if [ "$TARGETPLATFORM" = "linux/amd64" ]; then echo "Installing libfreenect2..." && \ + apt-get update && apt-get install -y mesa-utils xserver-xorg-video-all libusb-1.0-0-dev libturbojpeg0-dev libglfw3-dev && \ + apt-get clean && rm -rf /var/lib/apt/lists/ && \ + git clone https://github.com/OpenKinect/libfreenect2 && \ + cd libfreenect2 && \ + mkdir build && \ + cd build && \ + cmake .. && \ + make -j$(nproc) && \ + make install && \ + cd && \ + rm -r libfreenect2; fi + +# zed open capture +RUN if [ "$TARGETPLATFORM" = "linux/amd64" ]; then echo "Installing zed-open-capture..." && \ + apt-get update && apt install -y libusb-1.0-0-dev libhidapi-libusb0 libhidapi-dev wget && \ + apt-get clean && rm -rf /var/lib/apt/lists/ && \ + git clone https://github.com/stereolabs/zed-open-capture.git && \ + cd zed-open-capture && \ + mkdir build && \ + cd build && \ + cmake .. && \ + make -j$(nproc) && \ + make install && \ + cd && \ + rm -r zed-open-capture; fi + +RUN git clone --branch 4.5.4 https://github.com/opencv/opencv.git && \ + git clone --branch 4.5.4 https://github.com/opencv/opencv_contrib.git && \ + cd opencv && \ + mkdir build && \ + cd build && \ + cmake -DBUILD_opencv_python3=OFF -DBUILD_opencv_python_bindings_generator=OFF -DBUILD_opencv_python_tests=OFF -DBUILD_PERF_TESTS=OFF -DBUILD_TESTS=OFF -DOPENCV_ENABLE_NONFREE=ON -DOPENCV_EXTRA_MODULES_PATH=/root/opencv_contrib/modules .. && \ + make -j$(nproc) && \ + make install && \ + cd ../.. && \ + rm -rf opencv opencv_contrib + +RUN rm /bin/sh && ln -s /bin/bash /bin/sh + +COPY ./docker/jammy/deps/ros_entrypoint.sh /ros_entrypoint.sh +RUN chmod +x /ros_entrypoint.sh +ENTRYPOINT [ "/ros_entrypoint.sh" ] + +# ros2 seems not sourcing by default its multi-arch folders +ENV LD_LIBRARY_PATH=$LD_LIBRARY_PATH:/opt/ros/humble/lib/x86_64-linux-gnu:/opt/ros/humble/lib/aarch64-linux-gnu + +# for jetson (https://github.com/introlab/rtabmap/issues/776) +ENV LD_LIBRARY_PATH=$LD_LIBRARY_PATH:/usr/lib/aarch64-linux-gnu/tegra + diff --git a/docker/jammy/deps/ros_entrypoint.sh b/docker/jammy/deps/ros_entrypoint.sh new file mode 100644 index 0000000000..9d349493c4 --- /dev/null +++ b/docker/jammy/deps/ros_entrypoint.sh @@ -0,0 +1,6 @@ +#!/bin/bash +set -e + +# setup ros2 environment +source "/opt/ros/humble/setup.bash" -- +exec "$@" diff --git a/docker/jfr2018/Dockerfile b/docker/jfr2018/Dockerfile index aa4c1f8522..0eab9008ee 100644 --- a/docker/jfr2018/Dockerfile +++ b/docker/jfr2018/Dockerfile @@ -127,7 +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 -RUN git clone https://github.com/KumarRobotics/msckf_vio.git && cd msckf_vio && git checkout a9386c5 && wget https://gist.githubusercontent.com/matlabbe/f2518d7427e7f6740af2110e540b1f2b/raw/a6d604e730bcbf3dcd7fd8a27302bed4bb94b799/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/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 7e89c41374..9dde6f351f 100644 --- a/docker/jfr2018/latest/Dockerfile +++ b/docker/jfr2018/latest/Dockerfile @@ -137,8 +137,8 @@ RUN /bin/bash -c 'cd /root/catkin_ws;. /opt/ros/${ROS_DISTRO}/setup.bash; catkin RUN git clone https://github.com/tum-vision/dvo_slam.git && cd dvo_slam && git checkout ${ROS_DISTRO}-devel && rm dvo_slam/package.xml && rm dvo_benchmark/package.xml && rm dvo_ros/package.xml # VISO2 RUN git clone https://github.com/srv/viso2.git && cd viso2 && git checkout ${ROS_DISTRO} -# MSCKF-VIO -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 +# 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 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/docker/jfr2018/run_kitti_datasets.sh b/docker/jfr2018/run_kitti_datasets.sh index 18d2cd7fe0..8dd1d636ca 100755 --- a/docker/jfr2018/run_kitti_datasets.sh +++ b/docker/jfr2018/run_kitti_datasets.sh @@ -78,7 +78,7 @@ then fi SCAN="--Icp/PointToPlane false --Icp/Iterations 10 --Odom/ScanKeyFrameThr 0.8 $SCAN" fi - SCAN="--scan --Reg/Strategy $REG --OdomF2M/ScanMaxSize 10000 --OdomF2M/ScanSubtractRadius 0.5 --Odom/LOAMSensor 2 --Icp/MaxTranslation 2 --Icp/Epsilon 0.0001 --Icp/MaxCorrespondenceDistance 1.5 --Icp/CorrespondenceRatio 0.01 --Icp/PM true --Icp/PMOutlierRatio 0.7 --Icp/PMMatcherKnn 3 --Icp/PMMatcherEpsilon 1 $SCAN" + SCAN="--scan --Reg/Strategy $REG --OdomF2M/ScanMaxSize 10000 --OdomF2M/ScanSubtractRadius 0.5 --Odom/LOAMSensor 2 --Icp/MaxTranslation 2 --Icp/Epsilon 0.0001 --Icp/MaxCorrespondenceDistance 1.5 --Icp/CorrespondenceRatio 0.01 --Icp/PM true --Icp/PMOutlierRatio 0.7 --Icp/PMMatcherKnn 3 --Icp/PMMatcherEpsilon 1 --Icp/ReciprocalCorrespondences false $SCAN" fi RTABMAP_KITTI_TOOL="rtabmap-kitti_dataset" diff --git a/docker/latest_deps/Dockerfile b/docker/latest_deps/Dockerfile new file mode 100644 index 0000000000..588c62fe3c --- /dev/null +++ b/docker/latest_deps/Dockerfile @@ -0,0 +1,86 @@ + +FROM osrf/ros:humble-desktop + +# Install build dependencies +RUN apt-get update && \ + apt-get install -y git software-properties-common ros-humble-rtabmap-ros libqt6* qt6* && \ + apt-get remove -y ros-humble-rtabmap* ros-humble-libg2o libpcl* libqt5* qt5* libvtk* libopencv* && \ + apt-get clean && rm -rf /var/lib/apt/lists/ + +WORKDIR /root/ + +RUN rm /bin/sh && ln -s /bin/bash /bin/sh + +# ros2 seems not sourcing by default its multi-arch folders +ENV LD_LIBRARY_PATH=$LD_LIBRARY_PATH:/opt/ros/humble/lib/x86_64-linux-gnu + +# Build latest VTK with Qt6 +RUN git clone https://github.com/Kitware/VTK.git && \ + cd VTK && \ + mkdir build && \ + cd build && \ + cmake -DVTK_GROUP_ENABLE_Qt=YES .. && \ + make -j$(nproc) && \ + make install && \ + cd ../.. && \ + rm -rf VTK + +# Build latest PCL with latest VTK +RUN git clone https://github.com/PointCloudLibrary/pcl.git && \ + cd pcl && \ + mkdir build && \ + cd build && \ + cmake -DBUILD_tools=OFF .. && \ + make -j$(nproc) && \ + make install && \ + cd ../.. && \ + rm -rf pcl + +# Build latest OpenCV +RUN git clone https://github.com/opencv/opencv.git && \ + git clone https://github.com/opencv/opencv_contrib.git && \ + cd opencv && \ + mkdir build && \ + cd build && \ + cmake -DBUILD_opencv_python3=OFF -DBUILD_opencv_python_bindings_generator=OFF -DBUILD_opencv_python_tests=OFF -DBUILD_PERF_TESTS=OFF -DBUILD_TESTS=OFF -DOPENCV_ENABLE_NONFREE=ON -DOPENCV_EXTRA_MODULES_PATH=/root/opencv_contrib/modules .. && \ + make -j$(nproc) && \ + make install && \ + cd ../.. && \ + rm -rf opencv + +# Build latest gtsam +RUN git clone https://github.com/borglab/gtsam.git && \ + cd gtsam && \ + mkdir build && \ + cd build && \ + cmake -DGTSAM_BUILD_EXAMPLES_ALWAYS=OFF -DGTSAM_BUILD_TESTS=OFF -DGTSAM_BUILD_STATIC_LIBRARY=OFF -DGTSAM_BUILD_UNSTABLE=OFF -DGTSAM_INSTALL_CPPUNILITE=OFF -DGTSAM_USE_SYSTEM_EIGEN=ON .. && \ + cmake --build . --config Release --target install && \ + cd ../.. && \ + rm -rf gtsam + +# Build latest g2o +RUN git clone https://github.com/RainerKuemmerle/g2o.git && \ + cd g2o && \ + mkdir build && \ + cd build && \ + cmake -DBUILD_WITH_MARCH_NATIVE=OFF -DG2O_BUILD_APPS=OFF -DG2O_BUILD_EXAMPLES=OFF -DG2O_USE_OPENGL=OFF .. && \ + cmake --build . --config Release --target install && \ + cd ../.. && \ + rm -rf g2o + +RUN mkdir -p /root/Documents/RTAB-Map + +# Copy current source code +COPY . /root/rtabmap + +# Build RTAB-Map project +RUN source /ros_entrypoint.sh && \ + cd rtabmap/build && \ + cmake .. && \ + make -j$(nproc) && \ + make install && \ + cd ../.. && \ + rm -rf rtabmap && \ + ldconfig + + diff --git a/examples/BOWMapping/CMakeLists.txt b/examples/BOWMapping/CMakeLists.txt index 0aaab05d0c..6c56bba49c 100644 --- a/examples/BOWMapping/CMakeLists.txt +++ b/examples/BOWMapping/CMakeLists.txt @@ -1,51 +1,24 @@ -cmake_minimum_required(VERSION 2.8) +cmake_minimum_required(VERSION 3.5) + +if(POLICY CMP0020) + cmake_policy(SET CMP0020 NEW) +endif() IF(DEFINED PROJECT_NAME) set(internal TRUE) ENDIF(DEFINED PROJECT_NAME) -if(internal) - # inside rtabmap project (see below for external build) - SET(RTABMap_INCLUDE_DIRS - ${PROJECT_BINARY_DIR}/corelib/include - ${PROJECT_SOURCE_DIR}/utilite/include - ${PROJECT_SOURCE_DIR}/corelib/include - ) - SET(RTABMap_LIBRARIES - rtabmap_core - rtabmap_utilite - ) -else() +if(NOT internal) # external build PROJECT( MyProject ) FIND_PACKAGE(RTABMap REQUIRED) - FIND_PACKAGE(OpenCV REQUIRED) - FIND_PACKAGE(PCL 1.7 REQUIRED) -endif() - -if(POLICY CMP0020) - cmake_policy(SET CMP0020 NEW) endif() -SET(INCLUDE_DIRS - ${RTABMap_INCLUDE_DIRS} - ${OpenCV_INCLUDE_DIRS} - ${PCL_INCLUDE_DIRS} -) - -SET(LIBRARIES - ${RTABMap_LIBRARIES} - ${OpenCV_LIBRARIES} - ${PCL_LIBRARIES} -) - -INCLUDE_DIRECTORIES(${INCLUDE_DIRS}) - ADD_EXECUTABLE(bow_mapping main.cpp) -TARGET_LINK_LIBRARIES(bow_mapping ${LIBRARIES}) +TARGET_LINK_LIBRARIES(bow_mapping rtabmap::rtabmap) if(internal) SET_TARGET_PROPERTIES( bow_mapping PROPERTIES OUTPUT_NAME ${PROJECT_PREFIX}-bow_mapping) -endif(internal) \ No newline at end of file +endif(internal) diff --git a/examples/NoEventsExample/CMakeLists.txt b/examples/NoEventsExample/CMakeLists.txt index a1ea87d1fb..e7e074d34a 100644 --- a/examples/NoEventsExample/CMakeLists.txt +++ b/examples/NoEventsExample/CMakeLists.txt @@ -1,69 +1,35 @@ -cmake_minimum_required(VERSION 2.8) +cmake_minimum_required(VERSION 3.5) + +if(POLICY CMP0020) + cmake_policy(SET CMP0020 NEW) +endif() IF(DEFINED PROJECT_NAME) set(internal TRUE) ENDIF(DEFINED PROJECT_NAME) -if(internal) - # inside rtabmap project (see below for external build) - SET(RTABMap_INCLUDE_DIRS - ${PROJECT_BINARY_DIR}/corelib/include - ${PROJECT_SOURCE_DIR}/utilite/include - ${PROJECT_SOURCE_DIR}/corelib/include - ${PROJECT_SOURCE_DIR}/guilib/include - ) - SET(RTABMap_LIBRARIES - rtabmap_core - rtabmap_gui - rtabmap_utilite - ) -else() +if(NOT internal) # external build PROJECT( MyProject ) - FIND_PACKAGE(RTABMap REQUIRED) - FIND_PACKAGE(OpenCV REQUIRED) - FIND_PACKAGE(PCL 1.7 REQUIRED) - - # Find Qt5 first - FIND_PACKAGE(Qt5 COMPONENTS Widgets Core Gui Svg QUIET) - IF(NOT Qt5_FOUND) - FIND_PACKAGE(Qt4 COMPONENTS QtCore QtGui QtSvg) - ENDIF(NOT Qt5_FOUND) -endif() - -if(POLICY CMP0020) - cmake_policy(SET CMP0020 NEW) + FIND_PACKAGE(RTABMap REQUIRED COMPONENTS gui) endif() -SET(INCLUDE_DIRS - ${RTABMap_INCLUDE_DIRS} - ${OpenCV_INCLUDE_DIRS} - ${PCL_INCLUDE_DIRS} -) - -IF(QT4_FOUND) - INCLUDE(${QT_USE_FILE}) -ENDIF(QT4_FOUND) - -SET(LIBRARIES - ${RTABMap_LIBRARIES} - ${OpenCV_LIBRARIES} - ${QT_LIBRARIES} - ${PCL_LIBRARIES} -) - -INCLUDE_DIRECTORIES(${INCLUDE_DIRS}) - -IF(QT4_FOUND) - QT4_WRAP_CPP(moc_srcs MapBuilder.h) -ELSE() - QT5_WRAP_CPP(moc_srcs MapBuilder.h) +IF(QT4_FOUND OR Qt5_FOUND OR Qt6_FOUND) + SET(moc_srcs MapBuilder.h) ENDIF() ADD_EXECUTABLE(noEventsExample main.cpp ${moc_srcs}) -TARGET_LINK_LIBRARIES(noEventsExample ${LIBRARIES}) +TARGET_LINK_LIBRARIES(noEventsExample rtabmap::gui) + +SET_TARGET_PROPERTIES( + noEventsExample + PROPERTIES + AUTOUIC ON + AUTOMOC ON + AUTORCC ON +) if(internal) SET_TARGET_PROPERTIES( noEventsExample diff --git a/examples/RGBDMapping/CMakeLists.txt b/examples/RGBDMapping/CMakeLists.txt index 75a862161d..6f6a53104a 100644 --- a/examples/RGBDMapping/CMakeLists.txt +++ b/examples/RGBDMapping/CMakeLists.txt @@ -1,91 +1,35 @@ -cmake_minimum_required(VERSION 2.8) +cmake_minimum_required(VERSION 3.5) + +if(POLICY CMP0020) + cmake_policy(SET CMP0020 NEW) +endif() IF(DEFINED PROJECT_NAME) set(internal TRUE) ENDIF(DEFINED PROJECT_NAME) -if(internal) - # inside rtabmap project (see below for external build) - SET(RTABMap_INCLUDE_DIRS - ${PROJECT_BINARY_DIR}/corelib/include - ${PROJECT_SOURCE_DIR}/utilite/include - ${PROJECT_SOURCE_DIR}/corelib/include - ${PROJECT_SOURCE_DIR}/guilib/include - ) - SET(RTABMap_LIBRARIES - rtabmap_core - rtabmap_gui - rtabmap_utilite - ) -else() +if(NOT internal) # external build PROJECT( MyProject ) - FIND_PACKAGE(RTABMap REQUIRED) - FIND_PACKAGE(OpenCV REQUIRED) - FIND_PACKAGE(PCL 1.7 REQUIRED) - - # Find Qt5 first - FIND_PACKAGE(Qt5 COMPONENTS Widgets Core Gui Svg QUIET) - IF(NOT Qt5_FOUND) - FIND_PACKAGE(Qt4 COMPONENTS QtCore QtGui QtSvg) - ENDIF(NOT Qt5_FOUND) - - # fix libproj.so not found on Xenial - if(NOT "${PCL_LIBRARIES}" STREQUAL "") - list(REMOVE_ITEM PCL_LIBRARIES "vtkproj4") - endif() - -endif() - -if(POLICY CMP0020) - cmake_policy(SET CMP0020 NEW) + FIND_PACKAGE(RTABMap REQUIRED COMPONENTS gui) endif() -SET(INCLUDE_DIRS - ${RTABMap_INCLUDE_DIRS} - ${OpenCV_INCLUDE_DIRS} - ${PCL_INCLUDE_DIRS} -) - -IF(QT4_FOUND) - INCLUDE(${QT_USE_FILE}) -ENDIF(QT4_FOUND) - -SET(LIBRARIES - ${RTABMap_LIBRARIES} - ${OpenCV_LIBRARIES} - ${QT_LIBRARIES} - ${PCL_LIBRARIES} -) - -# Hack as CameraRealsense2.h needs realsense2 include dir -IF(realsense2_FOUND) - SET(INCLUDE_DIRS - ${INCLUDE_DIRS} - ${realsense2_INCLUDE_DIRS} - ) -ENDIF(realsense2_FOUND) - -# Hack as CameraK4A.h needs k4a include dir -IF(k4a_FOUND) - SET(INCLUDE_DIRS - ${INCLUDE_DIRS} - ${k4a_INCLUDE_DIRS} - ) -ENDIF(k4a_FOUND) - -INCLUDE_DIRECTORIES(${INCLUDE_DIRS}) - -IF(QT4_FOUND) - QT4_WRAP_CPP(moc_srcs MapBuilder.h) -ELSE() - QT5_WRAP_CPP(moc_srcs MapBuilder.h) +IF(QT4_FOUND OR Qt5_FOUND OR Qt6_FOUND) + SET(moc_srcs MapBuilder.h) ENDIF() ADD_EXECUTABLE(rgbd_mapping main.cpp ${moc_srcs}) -TARGET_LINK_LIBRARIES(rgbd_mapping ${LIBRARIES}) +TARGET_LINK_LIBRARIES(rgbd_mapping rtabmap::gui) + +SET_TARGET_PROPERTIES( + rgbd_mapping + PROPERTIES + AUTOUIC ON + AUTOMOC ON + AUTORCC ON +) if(internal) SET_TARGET_PROPERTIES( rgbd_mapping diff --git a/examples/WifiMapping/CMakeLists.txt b/examples/WifiMapping/CMakeLists.txt index 11a4e72941..0614c99e1c 100644 --- a/examples/WifiMapping/CMakeLists.txt +++ b/examples/WifiMapping/CMakeLists.txt @@ -1,85 +1,29 @@ -cmake_minimum_required(VERSION 2.8) +cmake_minimum_required(VERSION 3.5) + +if(POLICY CMP0020) + cmake_policy(SET CMP0020 NEW) +endif() IF(DEFINED PROJECT_NAME) set(internal TRUE) ENDIF(DEFINED PROJECT_NAME) -if(internal) - # inside rtabmap project (see below for external build) - SET(RTABMap_INCLUDE_DIRS - ${PROJECT_BINARY_DIR}/corelib/include - ${PROJECT_SOURCE_DIR}/utilite/include - ${PROJECT_SOURCE_DIR}/corelib/include - ${PROJECT_SOURCE_DIR}/guilib/include - ) - SET(RTABMap_LIBRARIES - rtabmap_core - rtabmap_gui - rtabmap_utilite - ) -else() +if(NOT internal) # external build PROJECT( MyProject ) FIND_PACKAGE(RTABMap REQUIRED) - FIND_PACKAGE(OpenCV REQUIRED) - FIND_PACKAGE(PCL 1.7 REQUIRED) - - # Find Qt5 first - FIND_PACKAGE(Qt5 COMPONENTS Widgets Core Gui Svg QUIET) - IF(NOT Qt5_FOUND) - FIND_PACKAGE(Qt4 COMPONENTS QtCore QtGui QtSvg) - ENDIF(NOT Qt5_FOUND) endif() -if(POLICY CMP0020) - cmake_policy(SET CMP0020 NEW) -endif() - -SET(INCLUDE_DIRS - ${RTABMap_INCLUDE_DIRS} - ${OpenCV_INCLUDE_DIRS} - ${PCL_INCLUDE_DIRS} -) - -IF(QT4_FOUND) - INCLUDE(${QT_USE_FILE}) -ENDIF(QT4_FOUND) - -SET(LIBRARIES - ${RTABMap_LIBRARIES} - ${OpenCV_LIBRARIES} - ${QT_LIBRARIES} - ${PCL_LIBRARIES} -) - -# Hack as CameraRealsense2.h needs realsense2 include dir -IF(realsense2_FOUND) - SET(INCLUDE_DIRS - ${INCLUDE_DIRS} - ${realsense2_INCLUDE_DIRS} - ) -ENDIF(realsense2_FOUND) - -# Hack as CameraK4A.h needs k4a include dir -IF(k4a_FOUND) - SET(INCLUDE_DIRS - ${INCLUDE_DIRS} - ${k4a_INCLUDE_DIRS} - ) -ENDIF(k4a_FOUND) - -INCLUDE_DIRECTORIES(${INCLUDE_DIRS}) - -IF(QT4_FOUND) - QT4_WRAP_CPP(moc_srcs MapBuilder.h MapBuilderWifi.h) -ELSE() - QT5_WRAP_CPP(moc_srcs MapBuilder.h MapBuilderWifi.h) +IF(QT4_FOUND OR Qt5_FOUND OR Qt6_FOUND) + SET(moc_srcs MapBuilder.h MapBuilderWifi.h) ENDIF() SET(srcs main.cpp) +set(LIBRARIES "") + IF(APPLE) FIND_LIBRARY(CoreWLAN_LIBRARY CoreWLAN) FIND_LIBRARY(Foundation_LIBRARY Foundation) @@ -96,7 +40,15 @@ IF(APPLE) ENDIF(APPLE) ADD_EXECUTABLE(wifi_mapping ${srcs} ${moc_srcs}) -TARGET_LINK_LIBRARIES(wifi_mapping ${LIBRARIES}) +TARGET_LINK_LIBRARIES(wifi_mapping rtabmap::gui ${LIBRARIES}) + +SET_TARGET_PROPERTIES( + wifi_mapping + PROPERTIES + AUTOUIC ON + AUTOMOC ON + AUTORCC ON +) if(internal) SET_TARGET_PROPERTIES( wifi_mapping diff --git a/guilib/include/rtabmap/gui/AboutDialog.h b/guilib/include/rtabmap/gui/AboutDialog.h index 86689270e0..00cf7afe15 100644 --- a/guilib/include/rtabmap/gui/AboutDialog.h +++ b/guilib/include/rtabmap/gui/AboutDialog.h @@ -28,7 +28,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #ifndef RTABMAP_ABOUTDIALOG_H_ #define RTABMAP_ABOUTDIALOG_H_ -#include "rtabmap/gui/RtabmapGuiExp.h" // DLL export/import defines +#include "rtabmap/gui/rtabmap_gui_export.h" // DLL export/import defines #include #include @@ -37,7 +37,7 @@ class Ui_aboutDialog; namespace rtabmap { -class RTABMAPGUI_EXP AboutDialog : public QDialog +class RTABMAP_GUI_EXPORT AboutDialog : public QDialog { Q_OBJECT diff --git a/guilib/include/rtabmap/gui/CalibrationDialog.h b/guilib/include/rtabmap/gui/CalibrationDialog.h index 5785c501f8..efc66438c1 100644 --- a/guilib/include/rtabmap/gui/CalibrationDialog.h +++ b/guilib/include/rtabmap/gui/CalibrationDialog.h @@ -28,7 +28,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #ifndef RTABMAP_CALIBRATIONDIALOG_H_ #define RTABMAP_CALIBRATIONDIALOG_H_ -#include "rtabmap/gui/RtabmapGuiExp.h" // DLL export/import defines +#include "rtabmap/gui/rtabmap_gui_export.h" // DLL export/import defines #include #include @@ -43,7 +43,7 @@ class Ui_calibrationDialog; namespace rtabmap { -class RTABMAPGUI_EXP CalibrationDialog : public QDialog, public UEventsHandler +class RTABMAP_GUI_EXPORT CalibrationDialog : public QDialog, public UEventsHandler { Q_OBJECT; diff --git a/guilib/include/rtabmap/gui/CameraViewer.h b/guilib/include/rtabmap/gui/CameraViewer.h index 2343aa8ee4..0f227f7b54 100644 --- a/guilib/include/rtabmap/gui/CameraViewer.h +++ b/guilib/include/rtabmap/gui/CameraViewer.h @@ -28,7 +28,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #ifndef RTABMAP_CAMERAVIEWER_H_ #define RTABMAP_CAMERAVIEWER_H_ -#include "rtabmap/gui/RtabmapGuiExp.h" // DLL export/import defines +#include "rtabmap/gui/rtabmap_gui_export.h" // DLL export/import defines #include #include @@ -45,7 +45,7 @@ namespace rtabmap { class ImageView; class CloudViewer; -class RTABMAPGUI_EXP CameraViewer : public QDialog, public UEventsHandler +class RTABMAP_GUI_EXPORT CameraViewer : public QDialog, public UEventsHandler { Q_OBJECT public: diff --git a/guilib/include/rtabmap/gui/CloudViewer.h b/guilib/include/rtabmap/gui/CloudViewer.h index f0d354a94d..a073a2ffe2 100644 --- a/guilib/include/rtabmap/gui/CloudViewer.h +++ b/guilib/include/rtabmap/gui/CloudViewer.h @@ -28,11 +28,12 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #ifndef RTABMAP_CLOUDVIEWER_H_ #define RTABMAP_CLOUDVIEWER_H_ -#include "rtabmap/gui/RtabmapGuiExp.h" // DLL export/import defines +#include "rtabmap/gui/rtabmap_gui_export.h" // DLL export/import defines #include "rtabmap/core/Transform.h" #include "rtabmap/core/StereoCameraModel.h" #include "rtabmap/gui/CloudViewerInteractorStyle.h" +#include #if VTK_MAJOR_VERSION > 8 #ifndef slots @@ -75,7 +76,7 @@ namespace rtabmap { class OctoMap; -class RTABMAPGUI_EXP CloudViewer : public PCLQVTKWidget +class RTABMAP_GUI_EXPORT CloudViewer : public PCLQVTKWidget { Q_OBJECT diff --git a/guilib/include/rtabmap/gui/CloudViewerCellPicker.h b/guilib/include/rtabmap/gui/CloudViewerCellPicker.h index 2781101f20..6cc3c6634a 100644 --- a/guilib/include/rtabmap/gui/CloudViewerCellPicker.h +++ b/guilib/include/rtabmap/gui/CloudViewerCellPicker.h @@ -8,13 +8,13 @@ #ifndef GUILIB_SRC_CLOUDVIEWERCELLPICKER_H_ #define GUILIB_SRC_CLOUDVIEWERCELLPICKER_H_ -#include "rtabmap/gui/RtabmapGuiExp.h" // DLL export/import defines +#include "rtabmap/gui/rtabmap_gui_export.h" // DLL export/import defines #include namespace rtabmap { -class RTABMAPGUI_EXP CloudViewerCellPicker : public vtkCellPicker { +class RTABMAP_GUI_EXPORT CloudViewerCellPicker : public vtkCellPicker { public: public: static CloudViewerCellPicker *New (); diff --git a/guilib/include/rtabmap/gui/CloudViewerInteractorStyle.h b/guilib/include/rtabmap/gui/CloudViewerInteractorStyle.h index 0cbad9e030..0c7d103065 100644 --- a/guilib/include/rtabmap/gui/CloudViewerInteractorStyle.h +++ b/guilib/include/rtabmap/gui/CloudViewerInteractorStyle.h @@ -8,7 +8,7 @@ #ifndef GUILIB_SRC_CLOUDVIEWERINTERACTORSTYLE_H_ #define GUILIB_SRC_CLOUDVIEWERINTERACTORSTYLE_H_ -#include "rtabmap/gui/RtabmapGuiExp.h" // DLL export/import defines +#include "rtabmap/gui/rtabmap_gui_export.h" // DLL export/import defines #include #include @@ -19,7 +19,7 @@ namespace rtabmap { class CloudViewer; -class RTABMAPGUI_EXP CloudViewerInteractorStyle: public pcl::visualization::PCLVisualizerInteractorStyle +class RTABMAP_GUI_EXPORT CloudViewerInteractorStyle: public pcl::visualization::PCLVisualizerInteractorStyle { public: static CloudViewerInteractorStyle *New (); diff --git a/guilib/include/rtabmap/gui/ConsoleWidget.h b/guilib/include/rtabmap/gui/ConsoleWidget.h index c1e7ea533a..cdb62af53e 100644 --- a/guilib/include/rtabmap/gui/ConsoleWidget.h +++ b/guilib/include/rtabmap/gui/ConsoleWidget.h @@ -28,13 +28,14 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #ifndef RTABMAP_CONSOLEWIDGET_H_ #define RTABMAP_CONSOLEWIDGET_H_ -#include "rtabmap/gui/RtabmapGuiExp.h" // DLL export/import defines +#include "rtabmap/gui/rtabmap_gui_export.h" // DLL export/import defines #include #include #include #include #include +#include class Ui_consoleWidget; class QMessageBox; @@ -42,7 +43,7 @@ class QTextCursor; namespace rtabmap { -class RTABMAPGUI_EXP ConsoleWidget : public QWidget, public UEventsHandler +class RTABMAP_GUI_EXPORT ConsoleWidget : public QWidget, public UEventsHandler { Q_OBJECT; @@ -69,7 +70,7 @@ private Q_SLOTS: QMutex _errorMessageMutex; QMutex _msgListMutex; QTimer _timer; - QTime _time; + QElapsedTimer _time; QTextCursor * _textCursor; QList > _msgList; }; diff --git a/guilib/include/rtabmap/gui/CreateSimpleCalibrationDialog.h b/guilib/include/rtabmap/gui/CreateSimpleCalibrationDialog.h index eb43801531..0482195014 100644 --- a/guilib/include/rtabmap/gui/CreateSimpleCalibrationDialog.h +++ b/guilib/include/rtabmap/gui/CreateSimpleCalibrationDialog.h @@ -28,7 +28,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #ifndef RTABMAP_CREATESIMPLECALIBRATIONDIALOG_H_ #define RTABMAP_CREATESIMPLECALIBRATIONDIALOG_H_ -#include "rtabmap/gui/RtabmapGuiExp.h" // DLL export/import defines +#include "rtabmap/gui/rtabmap_gui_export.h" // DLL export/import defines #include #include @@ -37,7 +37,7 @@ class Ui_createSimpleCalibrationDialog; namespace rtabmap { -class RTABMAPGUI_EXP CreateSimpleCalibrationDialog : public QDialog +class RTABMAP_GUI_EXPORT CreateSimpleCalibrationDialog : public QDialog { Q_OBJECT diff --git a/guilib/include/rtabmap/gui/DataRecorder.h b/guilib/include/rtabmap/gui/DataRecorder.h index 7285a4355a..5b718635de 100644 --- a/guilib/include/rtabmap/gui/DataRecorder.h +++ b/guilib/include/rtabmap/gui/DataRecorder.h @@ -28,7 +28,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #ifndef RTABMAP_DATARECORDER_H_ #define RTABMAP_DATARECORDER_H_ -#include "rtabmap/gui/RtabmapGuiExp.h" // DLL export/import defines +#include "rtabmap/gui/rtabmap_gui_export.h" // DLL export/import defines #include #include @@ -43,7 +43,7 @@ namespace rtabmap { class Memory; class ImageView; -class RTABMAPGUI_EXP DataRecorder : public QWidget, public UEventsHandler +class RTABMAP_GUI_EXPORT DataRecorder : public QWidget, public UEventsHandler { Q_OBJECT public: diff --git a/guilib/include/rtabmap/gui/DatabaseViewer.h b/guilib/include/rtabmap/gui/DatabaseViewer.h index 8360575925..4622580dd4 100644 --- a/guilib/include/rtabmap/gui/DatabaseViewer.h +++ b/guilib/include/rtabmap/gui/DatabaseViewer.h @@ -28,7 +28,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #ifndef RTABMAP_DATABASEVIEWER_H_ #define RTABMAP_DATABASEVIEWER_H_ -#include "rtabmap/gui/RtabmapGuiExp.h" // DLL export/import defines +#include "rtabmap/gui/rtabmap_gui_export.h" // DLL export/import defines #include #include @@ -62,7 +62,7 @@ class ExportCloudsDialog; class EditDepthArea; class EditMapArea; -class RTABMAPGUI_EXP DatabaseViewer : public QMainWindow +class RTABMAP_GUI_EXPORT DatabaseViewer : public QMainWindow { Q_OBJECT @@ -137,6 +137,7 @@ private Q_SLOTS: void editConstraint(); void updateGrid(); void updateOctomapView(); + void updateGraphRotation(); void updateGraphView(); void refineConstraint(); void addConstraint(); diff --git a/guilib/include/rtabmap/gui/DepthCalibrationDialog.h b/guilib/include/rtabmap/gui/DepthCalibrationDialog.h index 78b36370d8..01b2d55076 100644 --- a/guilib/include/rtabmap/gui/DepthCalibrationDialog.h +++ b/guilib/include/rtabmap/gui/DepthCalibrationDialog.h @@ -28,7 +28,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #ifndef RTABMAP_DEPTHCALIBRATIONDIALOG_H_ #define RTABMAP_DEPTHCALIBRATIONDIALOG_H_ -#include "rtabmap/gui/RtabmapGuiExp.h" // DLL export/import defines +#include "rtabmap/gui/rtabmap_gui_export.h" // DLL export/import defines #include #include @@ -46,7 +46,7 @@ namespace rtabmap { class ProgressDialog; -class RTABMAPGUI_EXP DepthCalibrationDialog : public QDialog +class RTABMAP_GUI_EXPORT DepthCalibrationDialog : public QDialog { Q_OBJECT diff --git a/guilib/include/rtabmap/gui/EditConstraintDialog.h b/guilib/include/rtabmap/gui/EditConstraintDialog.h index acf7eb0828..8a37be8e76 100644 --- a/guilib/include/rtabmap/gui/EditConstraintDialog.h +++ b/guilib/include/rtabmap/gui/EditConstraintDialog.h @@ -28,7 +28,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #ifndef RTABMAP_EDITCONSTRAINTDIALOG_H_ #define RTABMAP_EDITCONSTRAINTDIALOG_H_ -#include "rtabmap/gui/RtabmapGuiExp.h" // DLL export/import defines +#include "rtabmap/gui/rtabmap_gui_export.h" // DLL export/import defines #include #include @@ -37,7 +37,7 @@ class Ui_EditConstraintDialog; namespace rtabmap { -class RTABMAPGUI_EXP EditConstraintDialog : public QDialog +class RTABMAP_GUI_EXPORT EditConstraintDialog : public QDialog { Q_OBJECT diff --git a/guilib/include/rtabmap/gui/EditDepthArea.h b/guilib/include/rtabmap/gui/EditDepthArea.h index 6878ea77c6..955872da97 100644 --- a/guilib/include/rtabmap/gui/EditDepthArea.h +++ b/guilib/include/rtabmap/gui/EditDepthArea.h @@ -28,7 +28,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #ifndef RTABMAP_EDITDEPTHAREA_H #define RTABMAP_EDITDEPTHAREA_H -#include "rtabmap/gui/RtabmapGuiExp.h" // DLL export/import defines +#include "rtabmap/gui/rtabmap_gui_export.h" // DLL export/import defines #include #include @@ -42,7 +42,7 @@ class QAction; namespace rtabmap { -class RTABMAPGUI_EXP EditDepthArea : public QWidget +class RTABMAP_GUI_EXPORT EditDepthArea : public QWidget { Q_OBJECT diff --git a/guilib/include/rtabmap/gui/EditMapArea.h b/guilib/include/rtabmap/gui/EditMapArea.h index 8e77727676..36793a7e75 100644 --- a/guilib/include/rtabmap/gui/EditMapArea.h +++ b/guilib/include/rtabmap/gui/EditMapArea.h @@ -28,7 +28,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #ifndef RTABMAP_EDITMAPAREA_H #define RTABMAP_EDITMAPAREA_H -#include "rtabmap/gui/RtabmapGuiExp.h" // DLL export/import defines +#include "rtabmap/gui/rtabmap_gui_export.h" // DLL export/import defines #include #include @@ -42,7 +42,7 @@ class QAction; namespace rtabmap { -class RTABMAPGUI_EXP EditMapArea : public QWidget +class RTABMAP_GUI_EXPORT EditMapArea : public QWidget { Q_OBJECT diff --git a/guilib/include/rtabmap/gui/ExportBundlerDialog.h b/guilib/include/rtabmap/gui/ExportBundlerDialog.h index 96f1e62a0b..edd9ac187f 100644 --- a/guilib/include/rtabmap/gui/ExportBundlerDialog.h +++ b/guilib/include/rtabmap/gui/ExportBundlerDialog.h @@ -25,10 +25,10 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ -#ifndef RTABMAP_EXPORTBUNDLERDIALOG_H_ -#define RTABMAP_EXPORTBUNDLERDIALOG_H_ +#ifndef RTABMAP_CORE_EXPORTBUNDLERDIALOG_H_ +#define RTABMAP_CORE_EXPORTBUNDLERDIALOG_H_ -#include "rtabmap/gui/RtabmapGuiExp.h" // DLL export/import defines +#include "rtabmap/gui/rtabmap_gui_export.h" // DLL export/import defines #include #include @@ -39,7 +39,7 @@ class Ui_ExportBundlerDialog; namespace rtabmap { -class RTABMAPGUI_EXP ExportBundlerDialog : public QDialog +class RTABMAP_GUI_EXPORT ExportBundlerDialog : public QDialog { Q_OBJECT diff --git a/guilib/include/rtabmap/gui/ExportCloudsDialog.h b/guilib/include/rtabmap/gui/ExportCloudsDialog.h index b2ffa71589..71bb674c94 100644 --- a/guilib/include/rtabmap/gui/ExportCloudsDialog.h +++ b/guilib/include/rtabmap/gui/ExportCloudsDialog.h @@ -25,10 +25,10 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ -#ifndef RTABMAP_EXPORTCLOUDSDIALOG_H_ -#define RTABMAP_EXPORTCLOUDSDIALOG_H_ +#ifndef RTABMAP_CORE_EXPORTCLOUDSDIALOG_H_ +#define RTABMAP_CORE_EXPORTCLOUDSDIALOG_H_ -#include "rtabmap/gui/RtabmapGuiExp.h" // DLL export/import defines +#include "rtabmap/gui/rtabmap_gui_export.h" // DLL export/import defines #include #include @@ -51,7 +51,7 @@ class ProgressDialog; class GainCompensator; class DBDriver; -class RTABMAPGUI_EXP ExportCloudsDialog : public QDialog +class RTABMAP_GUI_EXPORT ExportCloudsDialog : public QDialog { Q_OBJECT @@ -153,6 +153,10 @@ private Q_SLOTS: GainCompensator * _compensator; const DBDriver * _dbDriver; bool _scansHaveRGB; + + bool saveOBJFile(const QString &path, pcl::TextureMesh::Ptr &mesh) const; + bool saveOBJFile(const QString &path, pcl::PolygonMesh &mesh) const; + }; } diff --git a/guilib/include/rtabmap/gui/ExportDialog.h b/guilib/include/rtabmap/gui/ExportDialog.h index 6316e02ac7..2db6f99203 100644 --- a/guilib/include/rtabmap/gui/ExportDialog.h +++ b/guilib/include/rtabmap/gui/ExportDialog.h @@ -25,10 +25,10 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ -#ifndef RTABMAP_EXPORTDIALOG_H_ -#define RTABMAP_EXPORTDIALOG_H_ +#ifndef RTABMAP_CORE_EXPORTDIALOG_H_ +#define RTABMAP_CORE_EXPORTDIALOG_H_ -#include "rtabmap/gui/RtabmapGuiExp.h" // DLL export/import defines +#include "rtabmap/gui/rtabmap_gui_export.h" // DLL export/import defines #include #include @@ -37,7 +37,7 @@ class Ui_ExportDialog; namespace rtabmap { -class RTABMAPGUI_EXP ExportDialog : public QDialog +class RTABMAP_GUI_EXPORT ExportDialog : public QDialog { Q_OBJECT diff --git a/guilib/include/rtabmap/gui/GraphViewer.h b/guilib/include/rtabmap/gui/GraphViewer.h index dff621a2b4..fac00a0908 100644 --- a/guilib/include/rtabmap/gui/GraphViewer.h +++ b/guilib/include/rtabmap/gui/GraphViewer.h @@ -28,7 +28,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #ifndef RTABMAP_GRAPHVIEWER_H_ #define RTABMAP_GRAPHVIEWER_H_ -#include "rtabmap/gui/RtabmapGuiExp.h" // DLL export/import defines +#include "rtabmap/gui/rtabmap_gui_export.h" // DLL export/import defines #include #include @@ -49,7 +49,7 @@ namespace rtabmap { class NodeItem; class LinkItem; -class RTABMAPGUI_EXP GraphViewer : public QGraphicsView { +class RTABMAP_GUI_EXPORT GraphViewer : public QGraphicsView { Q_OBJECT; diff --git a/guilib/include/rtabmap/gui/ImageView.h b/guilib/include/rtabmap/gui/ImageView.h index c250c65619..3ef4eee63f 100644 --- a/guilib/include/rtabmap/gui/ImageView.h +++ b/guilib/include/rtabmap/gui/ImageView.h @@ -28,7 +28,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #ifndef RTABMAP_IMAGEVIEW_H_ #define RTABMAP_IMAGEVIEW_H_ -#include "rtabmap/gui/RtabmapGuiExp.h" // DLL export/import defines +#include "rtabmap/gui/rtabmap_gui_export.h" // DLL export/import defines #include #include @@ -45,7 +45,7 @@ namespace rtabmap { class KeypointItem; -class RTABMAPGUI_EXP ImageView : public QWidget { +class RTABMAP_GUI_EXPORT ImageView : public QWidget { Q_OBJECT @@ -72,7 +72,8 @@ class RTABMAPGUI_EXP ImageView : public QWidget { const QColor & getDefaultMatchingFeatureColor() const; const QColor & getDefaultMatchingLineColor() const; const QColor & getBackgroundColor() const; - float getDepthColorMapRange() const; + float getDepthColorMapMinRange() const; + float getDepthColorMapMaxRange() const; uCvQtDepthColorMap getDepthColorMap() const; float viewScale() const; @@ -89,7 +90,7 @@ class RTABMAPGUI_EXP ImageView : public QWidget { void setDefaultMatchingFeatureColor(const QColor & color); void setDefaultMatchingLineColor(const QColor & color); void setBackgroundColor(const QColor & color); - void setDepthColorMapRange(float value); + void setDepthColorMapRange(float min, float max); void setFeatures(const std::multimap & refWords, const cv::Mat & depth = cv::Mat(), const QColor & color = Qt::yellow); void setFeatures(const std::vector & features, const cv::Mat & depth = cv::Mat(), const QColor & color = Qt::yellow); @@ -138,7 +139,8 @@ private Q_SLOTS: QColor _defaultFeatureColor; QColor _defaultMatchingFeatureColor; QColor _defaultMatchingLineColor; - float _depthColorMapRange; + float _depthColorMapMinRange; + float _depthColorMapMaxRange; QMenu * _menu; QAction * _showImage; @@ -160,7 +162,8 @@ private Q_SLOTS: QAction * _colorMapBlackToWhite; QAction * _colorMapRedToBlue; QAction * _colorMapBlueToRed; - QAction * _colorMapRange; + QAction * _colorMapMinRange; + QAction * _colorMapMaxRange; QMenu * _featureMenu; QMenu * _scaleMenu; diff --git a/guilib/include/rtabmap/gui/KeypointItem.h b/guilib/include/rtabmap/gui/KeypointItem.h index 006bfaa0bd..7f4f9f6419 100644 --- a/guilib/include/rtabmap/gui/KeypointItem.h +++ b/guilib/include/rtabmap/gui/KeypointItem.h @@ -28,7 +28,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #ifndef RTABMAP_KEYPOINTITEM_H_ #define RTABMAP_KEYPOINTITEM_H_ -#include "rtabmap/gui/RtabmapGuiExp.h" // DLL export/import defines +#include "rtabmap/gui/rtabmap_gui_export.h" // DLL export/import defines #include #include @@ -38,7 +38,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. namespace rtabmap { -class RTABMAPGUI_EXP KeypointItem : public QGraphicsEllipseItem +class RTABMAP_GUI_EXPORT KeypointItem : public QGraphicsEllipseItem { public: KeypointItem(int id, const cv::KeyPoint & kpt, float depth = 0, const QColor & color = Qt::green, QGraphicsItem * parent = 0); diff --git a/guilib/include/rtabmap/gui/LoopClosureViewer.h b/guilib/include/rtabmap/gui/LoopClosureViewer.h index 66a845a945..c269f33f09 100644 --- a/guilib/include/rtabmap/gui/LoopClosureViewer.h +++ b/guilib/include/rtabmap/gui/LoopClosureViewer.h @@ -28,7 +28,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #ifndef RTABMAP_LOOPCLOSUREVIEWER_H_ #define RTABMAP_LOOPCLOSUREVIEWER_H_ -#include "rtabmap/gui/RtabmapGuiExp.h" // DLL export/import defines +#include "rtabmap/gui/rtabmap_gui_export.h" // DLL export/import defines #include #include @@ -40,7 +40,7 @@ class Ui_loopClosureViewer; namespace rtabmap { -class RTABMAPGUI_EXP LoopClosureViewer : public QWidget { +class RTABMAP_GUI_EXPORT LoopClosureViewer : public QWidget { Q_OBJECT diff --git a/guilib/include/rtabmap/gui/MainWindow.h b/guilib/include/rtabmap/gui/MainWindow.h index d2717cfad4..499e5579da 100644 --- a/guilib/include/rtabmap/gui/MainWindow.h +++ b/guilib/include/rtabmap/gui/MainWindow.h @@ -28,11 +28,12 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #ifndef RTABMAP_MAINWINDOW_H_ #define RTABMAP_MAINWINDOW_H_ -#include "rtabmap/gui/RtabmapGuiExp.h" // DLL export/import defines +#include "rtabmap/gui/rtabmap_gui_export.h" // DLL export/import defines #include "rtabmap/utilite/UEventsHandler.h" #include #include +#include #include "rtabmap/core/RtabmapEvent.h" #include "rtabmap/core/SensorData.h" #include "rtabmap/core/OdometryEvent.h" @@ -76,7 +77,7 @@ class DataRecorder; class OctoMap; class MultiSessionLocWidget; -class RTABMAPGUI_EXP MainWindow : public QMainWindow, public UEventsHandler +class RTABMAP_GUI_EXPORT MainWindow : public QMainWindow, public UEventsHandler { Q_OBJECT @@ -400,8 +401,8 @@ protected Q_SLOTS: bool _processingOdometry; QTimer * _oneSecondTimer; - QTime * _elapsedTime; - QTime * _logEventTime; + QElapsedTimer * _elapsedTime; + QElapsedTimer * _logEventTime; PdfPlotCurve * _posteriorCurve; PdfPlotCurve * _likelihoodCurve; diff --git a/guilib/include/rtabmap/gui/MapVisibilityWidget.h b/guilib/include/rtabmap/gui/MapVisibilityWidget.h index 328e27df7b..e4675a3523 100644 --- a/guilib/include/rtabmap/gui/MapVisibilityWidget.h +++ b/guilib/include/rtabmap/gui/MapVisibilityWidget.h @@ -28,7 +28,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #ifndef RTABMAP_MAPVISIBILITYWIDGET_H_ #define RTABMAP_MAPVISIBILITYWIDGET_H_ -#include "rtabmap/gui/RtabmapGuiExp.h" // DLL export/import defines +#include "rtabmap/gui/rtabmap_gui_export.h" // DLL export/import defines #include #include @@ -36,7 +36,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. namespace rtabmap { -class RTABMAPGUI_EXP MapVisibilityWidget : public QWidget { +class RTABMAP_GUI_EXPORT MapVisibilityWidget : public QWidget { Q_OBJECT; public: MapVisibilityWidget(QWidget * parent = 0); diff --git a/guilib/include/rtabmap/gui/MultiSessionLocSubView.h b/guilib/include/rtabmap/gui/MultiSessionLocSubView.h index cc8a1fca6c..dcbc7fff86 100644 --- a/guilib/include/rtabmap/gui/MultiSessionLocSubView.h +++ b/guilib/include/rtabmap/gui/MultiSessionLocSubView.h @@ -28,7 +28,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #ifndef RTABMAP_MULTISESSIONLOCSUBVIEW_H_ #define RTABMAP_MULTISESSIONLOCSUBVIEW_H_ -#include "rtabmap/gui/RtabmapGuiExp.h" // DLL export/import defines +#include "rtabmap/gui/rtabmap_gui_export.h" // DLL export/import defines #include #include @@ -38,7 +38,7 @@ namespace rtabmap { class ImageView; -class RTABMAPGUI_EXP MultiSessionLocSubView : public QWidget +class RTABMAP_GUI_EXPORT MultiSessionLocSubView : public QWidget { Q_OBJECT diff --git a/guilib/include/rtabmap/gui/MultiSessionLocWidget.h b/guilib/include/rtabmap/gui/MultiSessionLocWidget.h index 54cf968263..97361087ef 100644 --- a/guilib/include/rtabmap/gui/MultiSessionLocWidget.h +++ b/guilib/include/rtabmap/gui/MultiSessionLocWidget.h @@ -28,7 +28,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #ifndef RTABMAP_MULTISESSIONLOCWIDGET_H_ #define RTABMAP_MULTISESSIONLOCWIDGET_H_ -#include "rtabmap/gui/RtabmapGuiExp.h" // DLL export/import defines +#include "rtabmap/gui/rtabmap_gui_export.h" // DLL export/import defines #include "rtabmap/core/Statistics.h" #include "rtabmap/core/Signature.h" @@ -42,7 +42,7 @@ namespace rtabmap { class MultiSessionLocSubView; class ImageView; -class RTABMAPGUI_EXP MultiSessionLocWidget : public QWidget +class RTABMAP_GUI_EXPORT MultiSessionLocWidget : public QWidget { Q_OBJECT diff --git a/guilib/include/rtabmap/gui/OdometryViewer.h b/guilib/include/rtabmap/gui/OdometryViewer.h index 97de724653..5c8684665d 100644 --- a/guilib/include/rtabmap/gui/OdometryViewer.h +++ b/guilib/include/rtabmap/gui/OdometryViewer.h @@ -28,7 +28,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #ifndef RTABMAP_ODOMETRYVIEWER_H_ #define RTABMAP_ODOMETRYVIEWER_H_ -#include "rtabmap/gui/RtabmapGuiExp.h" // DLL export/import defines +#include "rtabmap/gui/rtabmap_gui_export.h" // DLL export/import defines #include "rtabmap/core/OdometryEvent.h" #include "rtabmap/core/Parameters.h" @@ -45,7 +45,7 @@ namespace rtabmap { class ImageView; class CloudViewer; -class RTABMAPGUI_EXP OdometryViewer : public QDialog, public UEventsHandler +class RTABMAP_GUI_EXPORT OdometryViewer : public QDialog, public UEventsHandler { Q_OBJECT diff --git a/guilib/include/rtabmap/gui/ParametersToolBox.h b/guilib/include/rtabmap/gui/ParametersToolBox.h index 18bbf596ce..f52c841b0d 100644 --- a/guilib/include/rtabmap/gui/ParametersToolBox.h +++ b/guilib/include/rtabmap/gui/ParametersToolBox.h @@ -32,7 +32,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #ifndef RTABMAP_PARAMETERSTOOLBOX_H_ #define RTABMAP_PARAMETERSTOOLBOX_H_ -#include "rtabmap/gui/RtabmapGuiExp.h" // DLL export/import defines +#include "rtabmap/gui/rtabmap_gui_export.h" // DLL export/import defines #include #include @@ -44,7 +44,7 @@ class QComboBox; namespace rtabmap { -class RTABMAPGUI_EXP ParametersToolBox: public QWidget +class RTABMAP_GUI_EXPORT ParametersToolBox: public QWidget { Q_OBJECT diff --git a/guilib/include/rtabmap/gui/PdfPlot.h b/guilib/include/rtabmap/gui/PdfPlot.h index 900bc89cc8..4a4ca49832 100644 --- a/guilib/include/rtabmap/gui/PdfPlot.h +++ b/guilib/include/rtabmap/gui/PdfPlot.h @@ -28,7 +28,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #ifndef RTABMAP_PDFPLOT_H_ #define RTABMAP_PDFPLOT_H_ -#include "rtabmap/gui/RtabmapGuiExp.h" // DLL export/import defines +#include "rtabmap/gui/rtabmap_gui_export.h" // DLL export/import defines #include #include "opencv2/opencv.hpp" @@ -36,7 +36,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. namespace rtabmap { -class RTABMAPGUI_EXP PdfPlotItem : public UPlotItem +class RTABMAP_GUI_EXPORT PdfPlotItem : public UPlotItem { public: PdfPlotItem(float dataX, float dataY, float width, int childCount = -1); @@ -59,7 +59,7 @@ class RTABMAPGUI_EXP PdfPlotItem : public UPlotItem }; -class RTABMAPGUI_EXP PdfPlotCurve : public UPlotCurve +class RTABMAP_GUI_EXPORT PdfPlotCurve : public UPlotCurve { Q_OBJECT diff --git a/guilib/include/rtabmap/gui/PostProcessingDialog.h b/guilib/include/rtabmap/gui/PostProcessingDialog.h index d7f0ac8815..b0b2bfe893 100644 --- a/guilib/include/rtabmap/gui/PostProcessingDialog.h +++ b/guilib/include/rtabmap/gui/PostProcessingDialog.h @@ -28,7 +28,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #ifndef RTABMAP_POSTPROCESSINGDIALOG_H_ #define RTABMAP_POSTPROCESSINGDIALOG_H_ -#include "rtabmap/gui/RtabmapGuiExp.h" // DLL export/import defines +#include "rtabmap/gui/rtabmap_gui_export.h" // DLL export/import defines #include #include @@ -40,7 +40,7 @@ class QAbstractButton; namespace rtabmap { -class RTABMAPGUI_EXP PostProcessingDialog : public QDialog +class RTABMAP_GUI_EXPORT PostProcessingDialog : public QDialog { Q_OBJECT diff --git a/guilib/include/rtabmap/gui/PreferencesDialog.h b/guilib/include/rtabmap/gui/PreferencesDialog.h index 446b7e674e..37301a0536 100644 --- a/guilib/include/rtabmap/gui/PreferencesDialog.h +++ b/guilib/include/rtabmap/gui/PreferencesDialog.h @@ -28,7 +28,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #ifndef RTABMAP_PREFERENCESDIALOG_H_ #define RTABMAP_PREFERENCESDIALOG_H_ -#include "rtabmap/gui/RtabmapGuiExp.h" // DLL export/import defines +#include "rtabmap/gui/rtabmap_gui_export.h" // DLL export/import defines #include #include @@ -63,7 +63,7 @@ class Camera; class CalibrationDialog; class CreateSimpleCalibrationDialog; -class RTABMAPGUI_EXP PreferencesDialog : public QDialog +class RTABMAP_GUI_EXPORT PreferencesDialog : public QDialog { Q_OBJECT @@ -264,6 +264,8 @@ class RTABMAPGUI_EXP PreferencesDialog : public QDialog double getBilateralSigmaS() const; double getBilateralSigmaR() const; int getSourceImageDecimation() const; + int getSourceHistogramMethod() const; + bool isSourceFeatureDetection() const; bool isSourceStereoDepthGenerated() const; bool isSourceStereoExposureCompensation() const; bool isSourceScanFromDepth() const; @@ -348,6 +350,8 @@ private Q_SLOTS: void changeOdometryORBSLAMVocabulary(); void changeOdometryOKVISConfigPath(); void changeOdometryVINSConfigPath(); + void changeOdometryOpenVINSLeftMask(); + void changeOdometryOpenVINSRightMask(); void changeIcpPMConfigPath(); void changeSuperPointModelPath(); void changePyMatcherPath(); @@ -380,6 +384,7 @@ private Q_SLOTS: void selectSourceMKVPath(); void selectSourceSvoPath(); void selectSourceRealsense2JsonPath(); + void selectSourceDepthaiBlobPath(); void updateSourceGrpVisibility(); void testOdometry(); void testCamera(); diff --git a/guilib/include/rtabmap/gui/ProgressDialog.h b/guilib/include/rtabmap/gui/ProgressDialog.h index db3c4f9a6f..fa287520b9 100644 --- a/guilib/include/rtabmap/gui/ProgressDialog.h +++ b/guilib/include/rtabmap/gui/ProgressDialog.h @@ -28,7 +28,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #ifndef RTABMAP_PROGRESSDIALOG_H_ #define RTABMAP_PROGRESSDIALOG_H_ -#include "rtabmap/gui/RtabmapGuiExp.h" // DLL export/import defines +#include "rtabmap/gui/rtabmap_gui_export.h" // DLL export/import defines #include @@ -40,12 +40,12 @@ class QCheckBox; namespace rtabmap { -class RTABMAPGUI_EXP ProgressDialog : public QDialog +class RTABMAP_GUI_EXPORT ProgressDialog : public QDialog { Q_OBJECT public: - ProgressDialog(QWidget *parent = 0, Qt::WindowFlags flags = 0); + ProgressDialog(QWidget *parent = 0, Qt::WindowFlags flags = Qt::WindowFlags()); virtual ~ProgressDialog(); void setEndMessage(const QString & message) {_endMessage = message;} // Message shown when the progress is finished diff --git a/guilib/include/rtabmap/gui/RtabmapGuiExp.h b/guilib/include/rtabmap/gui/RtabmapGuiExp.h deleted file mode 100644 index b0af5a6d70..0000000000 --- a/guilib/include/rtabmap/gui/RtabmapGuiExp.h +++ /dev/null @@ -1,41 +0,0 @@ -/* -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 RTABMAPGUIEXP_H -#define RTABMAPGUIEXP_H - -#if defined(_WIN32) - #if defined(rtabmap_gui_EXPORTS) - #define RTABMAPGUI_EXP __declspec( dllexport ) - #else - #define RTABMAPGUI_EXP __declspec( dllimport ) - #endif -#else - #define RTABMAPGUI_EXP -#endif - -#endif // RTABMAPGUIEXP_H diff --git a/guilib/include/rtabmap/gui/StatsToolBox.h b/guilib/include/rtabmap/gui/StatsToolBox.h index 0a9b417ab1..86ab8c7be9 100644 --- a/guilib/include/rtabmap/gui/StatsToolBox.h +++ b/guilib/include/rtabmap/gui/StatsToolBox.h @@ -28,7 +28,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #ifndef RTABMAP_STATSTOOLBOX_H_ #define RTABMAP_STATSTOOLBOX_H_ -#include "rtabmap/gui/RtabmapGuiExp.h" // DLL export/import defines +#include "rtabmap/gui/rtabmap_gui_export.h" // DLL export/import defines #include #include @@ -41,7 +41,7 @@ class QToolBox; namespace rtabmap { -class RTABMAPGUI_EXP StatItem : public QWidget +class RTABMAP_GUI_EXPORT StatItem : public QWidget { Q_OBJECT; @@ -87,7 +87,7 @@ private Q_SLOTS: -class RTABMAPGUI_EXP StatsToolBox : public QWidget +class RTABMAP_GUI_EXPORT StatsToolBox : public QWidget { Q_OBJECT; diff --git a/guilib/include/rtabmap/utilite/UCv2Qt.h b/guilib/include/rtabmap/utilite/UCv2Qt.h index b082381403..c9f21a5645 100644 --- a/guilib/include/rtabmap/utilite/UCv2Qt.h +++ b/guilib/include/rtabmap/utilite/UCv2Qt.h @@ -95,9 +95,15 @@ inline QImage uCvMat2QImage( { // Assume depth image (float in meters) const float * data = (const float *)image.data; - float min=depthMax>depthMin?depthMin:data[0], max=depthMax>depthMin?depthMax:data[0]; - if(depthMax <= depthMin) + float min,max; + if(depthMax>depthMin) { + min = depthMin; + max = depthMax; + } + else + { + min = max = data[0]; for(unsigned int i=1; i 0) @@ -112,6 +118,14 @@ inline QImage uCvMat2QImage( } } } + if(depthMax > 0 && depthMax > depthMin) + { + max = depthMax; + } + if(depthMin>0 && (depthMin < depthMax || depthMin < max)) + { + min = depthMin; + } } qtemp = QImage(image.cols, image.rows, QImage::Format_Indexed8); @@ -120,21 +134,25 @@ inline QImage uCvMat2QImage( for(int x = 0; x < image.cols; ++x) { uchar * p = qtemp.scanLine (y) + x; - if(data[x] < min || data[x] > max || !uIsFinite(data[x]) || max == min) + if(!uIsFinite(data[x]) || max == min || data[x] == 0) { *p = 0; } + else if(data[x] < min) + { + *p = 255; + } + else if(data[x] > max) + { + *p=1; + } else { - *p = uchar(std::max(0.0f, std::min(255.0f, 255.0f - ((data[x]-min)*255.0f)/(max-min)))); - if(*p == 255) - { - *p = 0; - } + *p = uchar(std::max(1.0f, std::min(255.0f, 255.0f - ((data[x]-min)*255.0f)/(max-min)))); } if(*p!=0 && (colorMap == uCvQtDepthBlackToWhite || colorMap == uCvQtDepthRedToBlue)) { - *p = 255-*p; + *p = 255-*p+1; } } } @@ -158,9 +176,15 @@ inline QImage uCvMat2QImage( { // Assume depth image (unsigned short in mm) const unsigned short * data = (const unsigned short *)image.data; - unsigned short min=depthMax>depthMin?(unsigned short)(depthMin*1000):data[0], max=depthMax>depthMin?(unsigned short)(depthMax*1000):data[0]; - if(depthMax<=depthMin) + unsigned short min,max; + if(depthMax>depthMin) { + min = depthMin*1000; + max = depthMax*1000; + } + else + { + min = max = data[0]; for(unsigned int i=1; i 0) @@ -175,6 +199,14 @@ inline QImage uCvMat2QImage( } } } + if(depthMax > 0 && depthMax > depthMin) + { + max = depthMax*1000; + } + if(depthMin>0 && (depthMin < depthMax || depthMin*1000 < max)) + { + min = depthMin*1000; + } } qtemp = QImage(image.cols, image.rows, QImage::Format_Indexed8); @@ -183,21 +215,25 @@ inline QImage uCvMat2QImage( for(int x = 0; x < image.cols; ++x) { uchar * p = qtemp.scanLine (y) + x; - if(data[x] < min || data[x] > max || !uIsFinite(data[x]) || max == min) + if(!uIsFinite(data[x]) || max == min || data[x]==0) { *p = 0; } + else if(data[x] < min) + { + *p = 255; + } + else if(data[x] > max) + { + *p = 1; + } else { - *p = uchar(std::max(0.0f, std::min(255.0f, 255.0f - (float(data[x]-min)/float(max-min))*255.0f))); - if(*p == 255) - { - *p = 0; - } + *p = uchar(std::max(1.0f, std::min(255.0f, 255.0f - (float(data[x]-min)/float(max-min))*255.0f))); } if(*p!=0 && (colorMap == uCvQtDepthBlackToWhite || colorMap == uCvQtDepthRedToBlue)) { - *p = 255-*p; + *p = 255-*p+1; } } } diff --git a/guilib/include/rtabmap/utilite/UImageView.h b/guilib/include/rtabmap/utilite/UImageView.h index 437bc82c52..2de08aba6e 100644 --- a/guilib/include/rtabmap/utilite/UImageView.h +++ b/guilib/include/rtabmap/utilite/UImageView.h @@ -8,12 +8,12 @@ #ifndef IMAGEVIEW_H_ #define IMAGEVIEW_H_ -#include "rtabmap/gui/RtabmapGuiExp.h" // DLL export/import defines +#include "rtabmap/gui/rtabmap_gui_export.h" // DLL export/import defines #include #include -class RTABMAPGUI_EXP UImageView : public QWidget +class RTABMAP_GUI_EXPORT UImageView : public QWidget { Q_OBJECT; public: diff --git a/guilib/include/rtabmap/utilite/UPlot.h b/guilib/include/rtabmap/utilite/UPlot.h index 7a2dcae5d6..3d78729211 100644 --- a/guilib/include/rtabmap/utilite/UPlot.h +++ b/guilib/include/rtabmap/utilite/UPlot.h @@ -20,7 +20,7 @@ #ifndef UPLOT_H_ #define UPLOT_H_ -#include "rtabmap/gui/RtabmapGuiExp.h" // DLL export/import defines +#include "rtabmap/gui/rtabmap_gui_export.h" // DLL export/import defines #include #include @@ -32,6 +32,7 @@ #include #include #include +#include class QGraphicsView; class QGraphicsScene; @@ -43,7 +44,7 @@ class QScrollArea; * UPlotItem is a QGraphicsEllipseItem and can be inherited to do custom behaviors * on an hoverEnterEvent() for example. */ -class RTABMAPGUI_EXP UPlotItem : public QGraphicsEllipseItem +class RTABMAP_GUI_EXPORT UPlotItem : public QGraphicsEllipseItem { public: /** @@ -89,7 +90,7 @@ class UPlot; /** * UPlotCurve is a curve used to hold data shown in a UPlot. */ -class RTABMAPGUI_EXP UPlotCurve : public QObject +class RTABMAP_GUI_EXPORT UPlotCurve : public QObject { Q_OBJECT @@ -259,7 +260,7 @@ public Q_SLOTS: /** * A special UPlotCurve that shows as a line at the specified value, spanning all the UPlot. */ -class RTABMAPGUI_EXP UPlotCurveThreshold : public UPlotCurve +class RTABMAP_GUI_EXPORT UPlotCurveThreshold : public UPlotCurve { Q_OBJECT @@ -292,7 +293,7 @@ public Q_SLOTS: /** * The UPlot axis object. */ -class RTABMAPGUI_EXP UPlotAxis : public QWidget +class RTABMAP_GUI_EXPORT UPlotAxis : public QWidget { public: /** @@ -342,7 +343,7 @@ class RTABMAPGUI_EXP UPlotAxis : public QWidget /** * The UPlot legend item. Used internally by UPlot. */ -class RTABMAPGUI_EXP UPlotLegendItem : public QPushButton +class RTABMAP_GUI_EXPORT UPlotLegendItem : public QPushButton { Q_OBJECT @@ -383,7 +384,7 @@ private Q_SLOTS: /** * The UPlot legend. Used internally by UPlot. */ -class RTABMAPGUI_EXP UPlotLegend : public QWidget +class RTABMAP_GUI_EXPORT UPlotLegend : public QWidget { Q_OBJECT @@ -430,7 +431,7 @@ private Q_SLOTS: /** * Orientable QLabel. Inherit QLabel and let you to specify the orientation. */ -class RTABMAPGUI_EXP UOrientableLabel : public QLabel +class RTABMAP_GUI_EXPORT UOrientableLabel : public QLabel { Q_OBJECT @@ -486,7 +487,7 @@ class RTABMAPGUI_EXP UOrientableLabel : public QLabel * * */ -class RTABMAPGUI_EXP UPlot : public QWidget +class RTABMAP_GUI_EXPORT UPlot : public QWidget { Q_OBJECT @@ -602,9 +603,9 @@ private Q_SLOTS: UOrientableLabel * _yLabel; QLabel * _refreshRate; QString _workingDirectory; - QTime _refreshIntervalTime; + QElapsedTimer _refreshIntervalTime; int _lowestRefreshRate; - QTime _refreshStartTime; + QElapsedTimer _refreshStartTime; QString _autoScreenCaptureFormat; QPoint _mousePressedPos; QPoint _mouseCurrentPos; diff --git a/guilib/src/3rdParty/QMultiComboBox.cpp b/guilib/src/3rdParty/QMultiComboBox.cpp index fe07837015..64e19c6858 100644 --- a/guilib/src/3rdParty/QMultiComboBox.cpp +++ b/guilib/src/3rdParty/QMultiComboBox.cpp @@ -11,7 +11,8 @@ #include "QMultiComboBox.h" #include #include -#include +#include +#include #include "rtabmap/utilite/ULogger.h" @@ -46,7 +47,11 @@ QMultiComboBox::~QMultiComboBox() void QMultiComboBox::SetDisplayText(QString text) { m_DisplayText_ = text; +#if QT_VERSION >= QT_VERSION_CHECK(5, 15, 0) + const int textWidth = fontMetrics().horizontalAdvance(text); +#else const int textWidth = fontMetrics().width(text); +#endif setMinimumWidth(textWidth + 30); updateGeometry(); repaint(); @@ -88,7 +93,7 @@ void QMultiComboBox::showPopup() //QRect rec2(p , p + QPoint(rec.width(), rec.height())); // get the two possible list points and height - QRect screen = QApplication::desktop()->screenGeometry(this); + QRect screen = this->window()->windowHandle()->screen()->availableGeometry(); QPoint above = this->mapToGlobal(QPoint(0,0)); int aboveHeight = above.y() - screen.y(); QPoint below = this->mapToGlobal(QPoint(0,rec.height())); @@ -109,8 +114,8 @@ void QMultiComboBox::showPopup() // determine rect int contheight = vlist_.count()*vlist_.sizeHintForRow(0) + 4; // +4 - should be determined by margins? - belowHeight = min(abs(belowHeight)-screenbound_, contheight); - aboveHeight = min(abs(aboveHeight)-screenbound_, contheight); + belowHeight = std::min(abs(belowHeight)-screenbound_, contheight); + aboveHeight = std::min(abs(aboveHeight)-screenbound_, contheight); if (popheight_ > 0) // fixed { rec2.setHeight(popheight_); diff --git a/guilib/src/3rdParty/QMultiComboBox.h b/guilib/src/3rdParty/QMultiComboBox.h index 7d23405fcb..b9dbbf2da8 100644 --- a/guilib/src/3rdParty/QMultiComboBox.h +++ b/guilib/src/3rdParty/QMultiComboBox.h @@ -12,7 +12,6 @@ #define __MULTIBOXCOMBO_H__ #include -using namespace std; #include #include diff --git a/guilib/src/AboutDialog.cpp b/guilib/src/AboutDialog.cpp index 01d1e76d52..a25ac53435 100644 --- a/guilib/src/AboutDialog.cpp +++ b/guilib/src/AboutDialog.cpp @@ -160,6 +160,14 @@ AboutDialog::AboutDialog(QWidget * parent) : _ui->label_ceres->setText(Optimizer::isAvailable(Optimizer::kTypeCeres)?"Yes":"No"); _ui->label_ceres_license->setEnabled(Optimizer::isAvailable(Optimizer::kTypeCeres)?true:false); +#ifdef RTABMAP_MRPT + _ui->label_mrpt->setText("Yes"); + _ui->label_mrpt_license->setEnabled(true); +#else + _ui->label_mrpt->setText("No"); + _ui->label_mrpt_license->setEnabled(false); +#endif + #ifdef RTABMAP_POINTMATCHER _ui->label_libpointmatcher->setText("Yes"); _ui->label_libpointmatcher_license->setEnabled(true); diff --git a/guilib/src/CMakeLists.txt b/guilib/src/CMakeLists.txt index 07efc7cd27..e53c105411 100644 --- a/guilib/src/CMakeLists.txt +++ b/guilib/src/CMakeLists.txt @@ -38,42 +38,12 @@ SET(headers_ui ../include/${PROJECT_PREFIX}/gui/MultiSessionLocSubView.h ) -SET(uis - ./ui/mainWindow.ui - ./ui/preferencesDialog.ui - ./ui/aboutDialog.ui - ./ui/consoleWidget.ui - ./ui/DatabaseViewer.ui - ./ui/loopClosureViewer.ui - ./ui/exportDialog.ui - ./ui/postProcessingDialog.ui - ./ui/exportCloudsDialog.ui - ./ui/calibrationDialog.ui - ./ui/createSimpleCalibrationDialog.ui - ./ui/depthCalibrationDialog.ui - ./ui/exportBundlerDialog.ui - ./ui/editConstraintDialog.ui - ./ui/multiSessionLocSubView.ui -) - SET(qrc ./GuiLib.qrc ) -IF(QT4_FOUND) - # generate rules for building source files from the resources - QT4_ADD_RESOURCES(srcs_qrc ${qrc}) - - #Generate .h files from the .ui files - QT4_WRAP_UI(moc_uis ${uis}) - - #This will generate moc_* for Qt - QT4_WRAP_CPP(moc_srcs ${headers_ui}) - ### Qt Gui stuff end### -ELSE() - QT5_ADD_RESOURCES(srcs_qrc ${qrc}) - QT5_WRAP_UI(moc_uis ${uis}) - QT5_WRAP_CPP(moc_srcs ${headers_ui}) +IF(QT4_FOUND OR Qt5_FOUND OR Qt6_FOUND) + set(CMAKE_AUTOUIC_SEARCH_PATHS ./ui/) ENDIF() @@ -113,10 +83,8 @@ SET(SRC_FILES ./DepthCalibrationDialog.cpp ./3rdParty/QMultiComboBox.cpp ./opencv/vtkImageMatSource.cpp - - ${moc_srcs} - ${moc_uis} - ${srcs_qrc} + ${qrc} + ${headers_ui} ) # to get includes in visual studio @@ -128,51 +96,52 @@ IF(MSVC) ENDIF(MSVC) SET(INCLUDE_DIRS - ${PROJECT_BINARY_DIR}/corelib/include - ${PROJECT_SOURCE_DIR}/corelib/include - ${PROJECT_SOURCE_DIR}/utilite/include ${CMAKE_CURRENT_SOURCE_DIR}/../include ${CMAKE_CURRENT_SOURCE_DIR} - ${OpenCV_INCLUDE_DIRS} ${CMAKE_CURRENT_BINARY_DIR} # for qt ui generated in binary dir - ${PCL_INCLUDE_DIRS} ) -# Hack as CameraRealsense2.h needs realsense2 include dir -IF(realsense2_FOUND) - SET(INCLUDE_DIRS - ${INCLUDE_DIRS} - ${realsense2_INCLUDE_DIRS} - ) -ENDIF(realsense2_FOUND) -# Hack as CameraK4A.h needs k4a include dir -IF(k4a_FOUND) - SET(INCLUDE_DIRS - ${INCLUDE_DIRS} - ${k4a_INCLUDE_DIRS} - ) -ENDIF(k4a_FOUND) - IF(QT4_FOUND) INCLUDE(${QT_USE_FILE}) ENDIF(QT4_FOUND) -SET(LIBRARIES - ${QT_LIBRARIES} - ${OpenCV_LIBS} - ${PCL_LIBRARIES} -) +SET(LIBRARIES "") +SET(PUBLIC_LIBRARIES "") +SET(PUBLIC_INCLUDE_DIRS "") -IF(octomap_FOUND) - SET(INCLUDE_DIRS - ${INCLUDE_DIRS} - ${OCTOMAP_INCLUDE_DIRS} - ) - SET(LIBRARIES - ${LIBRARIES} - ${OCTOMAP_LIBRARIES} - ) -ENDIF(octomap_FOUND) +IF(Qt6_FOUND) + SET(PUBLIC_LIBRARIES ${PUBLIC_LIBRARIES} Qt6::Widgets Qt6::Core Qt6::Gui Qt6::OpenGL) + SET(LIBRARIES ${LIBRARIES} Qt6::PrintSupport) + IF(Qt6Svg_FOUND) + SET(LIBRARIES ${LIBRARIES} Qt6::Svg) + ENDIF() + SET(PUBLIC_INCLUDE_DIRS + ${PUBLIC_INCLUDE_DIRS} + ${Qt6Widgets_INCLUDE_DIRS} + ${Qt6Core_INCLUDE_DIRS} + ${Qt6Gui_INCLUDE_DIRS} + ${Qt6OpenGL_INCLUDE_DIRS}) +ELSEIF(Qt5_FOUND) + SET(PUBLIC_LIBRARIES ${PUBLIC_LIBRARIES} Qt5::Widgets Qt5::Core Qt5::Gui Qt5::OpenGL) + SET(LIBRARIES ${LIBRARIES} Qt5::PrintSupport) + IF(Qt5Svg_FOUND) + SET(LIBRARIES ${LIBRARIES} Qt5::Svg) + ENDIF() + SET(PUBLIC_INCLUDE_DIRS + ${PUBLIC_INCLUDE_DIRS} + ${Qt5Widgets_INCLUDE_DIRS} + ${Qt5Core_INCLUDE_DIRS} + ${Qt5Gui_INCLUDE_DIRS} + ${Qt5OpenGL_INCLUDE_DIRS}) +ELSE() + SET(PUBLIC_LIBRARIES ${PUBLIC_LIBRARIES} ${QTCORE_LIBRARY} ${QTGUI_LIBRARY}) + IF(QTSVG_FOUND) + SET(LIBRARIES ${LIBRARIES} ${QTSVG_LIBRARY}) + ENDIF() + SET(PUBLIC_INCLUDE_DIRS + ${QTCORE_INCLUDE_DIRS} + ${QTGUI_INCLUDE_DIRS}) +ENDIF() IF(CPUTSDF_FOUND) SET(INCLUDE_DIRS @@ -208,27 +177,63 @@ add_definitions(${PCL_DEFINITIONS}) # create a library from the source files ADD_LIBRARY(rtabmap_gui ${SRC_FILES}) -# Linking with Qt libraries +ADD_LIBRARY(rtabmap::gui ALIAS rtabmap_gui) -TARGET_LINK_LIBRARIES(rtabmap_gui rtabmap_core rtabmap_utilite ${LIBRARIES}) -IF(Qt5_FOUND) - IF(Qt5Svg_FOUND) - QT5_USE_MODULES(rtabmap_gui Widgets Core Gui Svg PrintSupport) - ELSE() - QT5_USE_MODULES(rtabmap_gui Widgets Core Gui PrintSupport) - ENDIF() -ENDIF(Qt5_FOUND) +generate_export_header(rtabmap_gui + DEPRECATED_MACRO_NAME RTABMAP_DEPRECATED) + +target_include_directories(rtabmap_gui PUBLIC + "$" + "$") + +TARGET_LINK_LIBRARIES(rtabmap_gui + PUBLIC + rtabmap_core ${PUBLIC_LIBRARIES} + PRIVATE + ${LIBRARIES} +) SET_TARGET_PROPERTIES( rtabmap_gui PROPERTIES VERSION ${RTABMAP_VERSION} SOVERSION ${RTABMAP_MAJOR_VERSION}.${RTABMAP_MINOR_VERSION} + EXPORT_NAME "gui" + AUTOUIC ON + AUTOMOC ON + AUTORCC ON ) -INSTALL(TARGETS rtabmap_gui +INSTALL(TARGETS rtabmap_gui EXPORT rtabmap_guiTargets RUNTIME DESTINATION "${CMAKE_INSTALL_BINDIR}" COMPONENT runtime LIBRARY DESTINATION "${CMAKE_INSTALL_LIBDIR}" COMPONENT devel ARCHIVE DESTINATION "${CMAKE_INSTALL_LIBDIR}" COMPONENT devel) - -install(DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/../include/ DESTINATION "${INSTALL_INCLUDE_DIR}" COMPONENT devel FILES_MATCHING PATTERN "*.h" PATTERN ".svn" EXCLUDE) + +configure_file( + ${CMAKE_CURRENT_BINARY_DIR}/rtabmap_gui_export.h + ${CMAKE_CURRENT_BINARY_DIR}/include/${PROJECT_PREFIX}/gui/rtabmap_gui_export.h + COPYONLY) + +install( + DIRECTORY + ${CMAKE_CURRENT_SOURCE_DIR}/../include/${PROJECT_PREFIX} + ${CMAKE_CURRENT_BINARY_DIR}/include/${PROJECT_PREFIX} + DESTINATION + ${INSTALL_INCLUDE_DIR} + COMPONENT + devel + FILES_MATCHING PATTERN "*.h" PATTERN "*.hpp" +) +export(EXPORT rtabmap_guiTargets + FILE "${CMAKE_CURRENT_BINARY_DIR}/../../${PROJECT_NAME}_guiTargets.cmake" + NAMESPACE rtabmap:: +) +install(EXPORT rtabmap_guiTargets + FILE + ${PROJECT_NAME}_guiTargets.cmake + DESTINATION + ${INSTALL_CMAKE_DIR} + NAMESPACE rtabmap:: + COMPONENT + devel +) diff --git a/guilib/src/CameraViewer.cpp b/guilib/src/CameraViewer.cpp index f257ae590e..786915e90b 100644 --- a/guilib/src/CameraViewer.cpp +++ b/guilib/src/CameraViewer.cpp @@ -59,7 +59,7 @@ CameraViewer::CameraViewer(QWidget * parent, const ParametersMap & parameters) : imageView_->setImageDepthShown(true); imageView_->setMinimumSize(320, 240); QHBoxLayout * layout = new QHBoxLayout(); - layout->setMargin(0); + layout->setContentsMargins(0,0,0,0); layout->addWidget(imageView_,1); layout->addWidget(cloudView_,1); @@ -95,7 +95,7 @@ CameraViewer::CameraViewer(QWidget * parent, const ParametersMap & parameters) : layout2->addWidget(buttonBox); QVBoxLayout * vlayout = new QVBoxLayout(this); - vlayout->setMargin(0); + vlayout->setContentsMargins(0,0,0,0); vlayout->setSpacing(0); vlayout->addLayout(layout, 1); vlayout->addLayout(layout2); diff --git a/guilib/src/CloudViewer.cpp b/guilib/src/CloudViewer.cpp index d2b41d52b5..8ad45a35bb 100644 --- a/guilib/src/CloudViewer.cpp +++ b/guilib/src/CloudViewer.cpp @@ -40,6 +40,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include +#include #include #include #include @@ -81,7 +82,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #endif - #if VTK_MAJOR_VERSION >= 8 #include #endif @@ -139,9 +139,8 @@ CloudViewer::CloudViewer(QWidget *parent, CloudViewerInteractorStyle * style) : _intensityAbsMax(100.0f), _coordinateFrameScale(1.0) { - UDEBUG(""); this->setMinimumSize(200, 200); - + int argc = 0; UASSERT(style!=0); style->setCloudViewer(this); @@ -205,12 +204,13 @@ CloudViewer::CloudViewer(QWidget *parent, CloudViewerInteractorStyle * style) : this->SetRenderWindow(_visualizer->getRenderWindow()); #endif - // Replaced by the second line, to avoid a crash in Mac OS X on close, as well as - // the "Invalid drawable" warning when the view is not visible. - //_visualizer->setupInteractor(this->GetInteractor(), this->GetRenderWindow()); + // Replaced by the second line, to avoid a crash in Mac OS X on close, as well as + // the "Invalid drawable" warning when the view is not visible. #if VTK_MAJOR_VERSION > 8 + //_visualizer->setupInteractor(this->interactor(), this->renderWindow()); this->interactor()->SetInteractorStyle (_visualizer->getInteractorStyle()); #else + //_visualizer->setupInteractor(this->GetInteractor(), this->GetRenderWindow()); this->GetInteractor()->SetInteractorStyle (_visualizer->getInteractorStyle()); #endif // setup a simple point picker @@ -1499,11 +1499,11 @@ bool CloudViewer::addTextureMesh ( // Save the viewpoint transformation matrix to the global actor map (*_visualizer->getCloudActorMap())[id].viewpoint_transformation_ = transformation; #endif - + #if VTK_MAJOR_VERSION >= 7 - actor->GetProperty()->SetAmbient(0.1); -#else - actor->GetProperty()->SetAmbient(0.5); + actor->GetProperty()->SetAmbient(0.1); +#else + actor->GetProperty()->SetAmbient(0.5); #endif actor->GetProperty()->SetLighting(_aSetLighting->isChecked()); actor->GetProperty()->SetInterpolation(_aSetFlatShading->isChecked()?VTK_FLAT:VTK_PHONG); @@ -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/CreateSimpleCalibrationDialog.cpp b/guilib/src/CreateSimpleCalibrationDialog.cpp index ed1086306b..20c4bfc1e6 100644 --- a/guilib/src/CreateSimpleCalibrationDialog.cpp +++ b/guilib/src/CreateSimpleCalibrationDialog.cpp @@ -175,7 +175,7 @@ void CreateSimpleCalibrationDialog::saveCalibration() ui_->doubleSpinBox_fy->value(), ui_->doubleSpinBox_cx->value(), ui_->doubleSpinBox_cy->value(), - Transform::getIdentity(), + CameraModel::opticalRotation(), 0, cv::Size(width, height)); UASSERT(modelLeft.isValidForProjection()); diff --git a/guilib/src/DataRecorder.cpp b/guilib/src/DataRecorder.cpp index 14eabbec7d..f8fd755b80 100644 --- a/guilib/src/DataRecorder.cpp +++ b/guilib/src/DataRecorder.cpp @@ -58,7 +58,7 @@ DataRecorder::DataRecorder(QWidget * parent) : imageView_->setImageDepthShown(true); imageView_->setMinimumSize(320, 240); QVBoxLayout * layout = new QVBoxLayout(this); - layout->setMargin(0); + layout->setContentsMargins(0,0,0,0); layout->addWidget(imageView_); layout->addWidget(label_); layout->setStretch(0,1); diff --git a/guilib/src/DatabaseViewer.cpp b/guilib/src/DatabaseViewer.cpp index 95ef336f8d..4161c5cc78 100644 --- a/guilib/src/DatabaseViewer.cpp +++ b/guilib/src/DatabaseViewer.cpp @@ -31,7 +31,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include -#include #include #include #include @@ -373,7 +372,7 @@ DatabaseViewer::DatabaseViewer(const QString & ini, QWidget * parent) : connect(ui_->horizontalSlider_iterations, SIGNAL(valueChanged(int)), this, SLOT(sliderIterationsValueChanged(int))); connect(ui_->horizontalSlider_iterations, SIGNAL(sliderMoved(int)), this, SLOT(sliderIterationsValueChanged(int))); connect(ui_->spinBox_optimizationsFrom, SIGNAL(editingFinished()), this, SLOT(updateGraphView())); - connect(ui_->checkBox_iterativeOptimization, SIGNAL(stateChanged(int)), this, SLOT(updateGraphView())); + connect(ui_->comboBox_optimizationFlavor, SIGNAL(activated(int)), this, SLOT(updateGraphView())); connect(ui_->checkBox_spanAllMaps, SIGNAL(stateChanged(int)), this, SLOT(updateGraphView())); connect(ui_->checkBox_wmState, SIGNAL(stateChanged(int)), this, SLOT(updateGraphView())); connect(ui_->graphViewer, SIGNAL(mapShownRequested()), this, SLOT(updateGraphView())); @@ -413,6 +412,7 @@ DatabaseViewer::DatabaseViewer(const QString & ini, QWidget * parent) : connect(ui_->graphicsView_B, SIGNAL(configChanged()), this, SLOT(configModified())); connect(ui_->comboBox_logger_level, SIGNAL(currentIndexChanged(int)), this, SLOT(configModified())); connect(ui_->actionVertical_Layout, SIGNAL(toggled(bool)), this, SLOT(configModified())); + connect(ui_->checkBox_alignPosesWithGPS, SIGNAL(stateChanged(int)), this, SLOT(updateGraphView())); connect(ui_->checkBox_alignPosesWithGroundTruth, SIGNAL(stateChanged(int)), this, SLOT(updateGraphView())); connect(ui_->checkBox_alignScansCloudsWithGroundTruth, SIGNAL(stateChanged(int)), this, SLOT(updateGraphView())); connect(ui_->checkBox_ignoreIntermediateNodes, SIGNAL(stateChanged(int)), this, SLOT(configModified())); @@ -428,6 +428,8 @@ DatabaseViewer::DatabaseViewer(const QString & ini, QWidget * parent) : connect(ui_->groupBox_posefiltering, SIGNAL(clicked(bool)), this, SLOT(configModified())); connect(ui_->doubleSpinBox_posefilteringRadius, SIGNAL(valueChanged(double)), this, SLOT(configModified())); connect(ui_->doubleSpinBox_posefilteringAngle, SIGNAL(valueChanged(double)), this, SLOT(configModified())); + connect(ui_->horizontalSlider_rotation, SIGNAL(valueChanged(int)), this, SLOT(updateGraphRotation())); + connect(ui_->pushButton_applyRotation, SIGNAL(clicked()), this, SLOT(updateGraphView())); connect(ui_->spinBox_icp_decimation, SIGNAL(valueChanged(int)), this, SLOT(configModified())); connect(ui_->doubleSpinBox_icp_maxDepth, SIGNAL(valueChanged(double)), this, SLOT(configModified())); @@ -716,12 +718,13 @@ void DatabaseViewer::restoreDefaultSettings() { // reset GUI parameters ui_->comboBox_logger_level->setCurrentIndex(1); + ui_->checkBox_alignPosesWithGPS->setChecked(true); ui_->checkBox_alignPosesWithGroundTruth->setChecked(true); ui_->checkBox_alignScansCloudsWithGroundTruth->setChecked(false); ui_->checkBox_ignoreIntermediateNodes->setChecked(false); ui_->checkBox_timeStats->setChecked(true); - ui_->checkBox_iterativeOptimization->setChecked(true); + ui_->comboBox_optimizationFlavor->setCurrentIndex(0); ui_->checkBox_spanAllMaps->setChecked(true); ui_->checkBox_wmState->setChecked(false); ui_->checkBox_ignorePoseCorrection->setChecked(false); @@ -1069,14 +1072,19 @@ bool DatabaseViewer::closeDatabase() ui_->checkBox_showOptimized->setEnabled(false); ui_->toolBox_statistics->clear(); databaseFileName_.clear(); + ui_->checkBox_alignPosesWithGPS->setVisible(false); + ui_->checkBox_alignPosesWithGPS->setEnabled(false); ui_->checkBox_alignPosesWithGroundTruth->setVisible(false); + ui_->checkBox_alignPosesWithGroundTruth->setEnabled(false); ui_->checkBox_alignScansCloudsWithGroundTruth->setVisible(false); + ui_->checkBox_alignScansCloudsWithGroundTruth->setEnabled(false); ui_->doubleSpinBox_optimizationScale->setVisible(false); ui_->label_scale_title->setVisible(false); ui_->label_rmse->setVisible(false); ui_->label_rmse_title->setVisible(false); ui_->checkBox_ignoreIntermediateNodes->setVisible(false); ui_->label_ignoreINtermediateNdoes->setVisible(false); + ui_->label_alignPosesWithGPS->setVisible(false); ui_->label_alignPosesWithGroundTruth->setVisible(false); ui_->label_alignScansCloudsWithGroundTruth->setVisible(false); ui_->label_optimizeFrom->setText(tr("Root")); @@ -1683,7 +1691,11 @@ void DatabaseViewer::updateIds() UINFO("Loading all IDs..."); std::set ids; dbDriver_->getAllNodeIds(ids); +#if QT_VERSION >= QT_VERSION_CHECK(6, 0, 0) + ids_ = QList(ids.begin(), ids.end()); +#else ids_ = QList::fromStdList(std::list(ids.begin(), ids.end())); +#endif lastWmIds_.clear(); dbDriver_->getLastNodeIds(lastWmIds_); idToIndex_.clear(); @@ -1696,14 +1708,19 @@ void DatabaseViewer::updateIds() gpsValues_.clear(); lastSliderIndexBrowsed_ = 0; ui_->checkBox_wmState->setVisible(false); + ui_->checkBox_alignPosesWithGPS->setVisible(false); + ui_->checkBox_alignPosesWithGPS->setEnabled(false); ui_->checkBox_alignPosesWithGroundTruth->setVisible(false); + ui_->checkBox_alignPosesWithGroundTruth->setEnabled(false); ui_->checkBox_alignScansCloudsWithGroundTruth->setVisible(false); + ui_->checkBox_alignScansCloudsWithGroundTruth->setEnabled(false); ui_->doubleSpinBox_optimizationScale->setVisible(false); ui_->label_scale_title->setVisible(false); ui_->label_rmse->setVisible(false); ui_->label_rmse_title->setVisible(false); ui_->checkBox_ignoreIntermediateNodes->setVisible(false); ui_->label_ignoreINtermediateNdoes->setVisible(false); + ui_->label_alignPosesWithGPS->setVisible(false); ui_->label_alignPosesWithGroundTruth->setVisible(false); ui_->label_alignScansCloudsWithGroundTruth->setVisible(false); ui_->menuEdit->setEnabled(true); @@ -1886,26 +1903,21 @@ void DatabaseViewer::updateIds() uSleep(100); QApplication::processEvents(); - if(!groundTruthPoses_.empty() || !gpsPoses_.empty()) - { - ui_->checkBox_alignPosesWithGroundTruth->setVisible(true); - ui_->doubleSpinBox_optimizationScale->setVisible(true); - ui_->label_scale_title->setVisible(true); - ui_->label_rmse->setVisible(true); - ui_->label_rmse_title->setVisible(true); - ui_->label_alignPosesWithGroundTruth->setVisible(true); + ui_->doubleSpinBox_optimizationScale->setVisible(!groundTruthPoses_.empty()); + ui_->label_scale_title->setVisible(!groundTruthPoses_.empty()); + ui_->label_rmse->setVisible(!groundTruthPoses_.empty()); + ui_->label_rmse_title->setVisible(!groundTruthPoses_.empty()); + + ui_->checkBox_alignPosesWithGPS->setVisible(!gpsPoses_.empty()); + ui_->checkBox_alignPosesWithGPS->setEnabled(!gpsPoses_.empty()); + ui_->label_alignPosesWithGPS->setVisible(!gpsPoses_.empty()); + ui_->checkBox_alignPosesWithGroundTruth->setVisible(!groundTruthPoses_.empty()); + ui_->checkBox_alignPosesWithGroundTruth->setEnabled(!groundTruthPoses_.empty()); + ui_->label_alignPosesWithGroundTruth->setVisible(!groundTruthPoses_.empty()); + ui_->checkBox_alignScansCloudsWithGroundTruth->setVisible(!groundTruthPoses_.empty()); + ui_->checkBox_alignScansCloudsWithGroundTruth->setEnabled(!groundTruthPoses_.empty()); + ui_->label_alignScansCloudsWithGroundTruth->setVisible(!groundTruthPoses_.empty()); - if(!groundTruthPoses_.empty()) - { - ui_->label_alignPosesWithGroundTruth->setText(tr("Align poses with ground truth")); - ui_->checkBox_alignScansCloudsWithGroundTruth->setVisible(true); - ui_->label_alignScansCloudsWithGroundTruth->setVisible(true); - } - else - { - ui_->label_alignPosesWithGroundTruth->setText(tr("Align poses with GPS")); - } - } if(!gpsValues_.empty()) { ui_->menuExport_GPS->setEnabled(true); @@ -2220,10 +2232,18 @@ void DatabaseViewer::updateInfo() ui_->textEdit_info->append(""); ParametersMap parameters = dbDriver_->getLastParameters(); QFontMetrics metrics(ui_->textEdit_info->font()); +#if QT_VERSION >= QT_VERSION_CHECK(5, 15, 0) + int tabW = ui_->textEdit_info->tabStopDistance(); +#else int tabW = ui_->textEdit_info->tabStopWidth(); +#endif for(ParametersMap::iterator iter=parameters.begin(); iter!=parameters.end(); ++iter) { +#if QT_VERSION >= QT_VERSION_CHECK(5, 15, 0) + int strW = metrics.horizontalAdvance(QString(iter->first.c_str()) + "="); +#else int strW = metrics.width(QString(iter->first.c_str()) + "="); +#endif ui_->textEdit_info->append(tr("%1=%2%3") .arg(iter->first.c_str()) .arg(strW < tabW?"\t\t\t\t":strW < tabW*2?"\t\t\t":strW < tabW*3?"\t\t":"\t") @@ -2540,10 +2560,12 @@ void DatabaseViewer::exportPoses(int format) optimizedPoses = uValueAt(graphes_, ui_->horizontalSlider_iterations->value()); } - if(ui_->checkBox_alignPosesWithGroundTruth->isChecked()) + if((ui_->checkBox_alignPosesWithGPS->isEnabled() && ui_->checkBox_alignPosesWithGPS->isChecked()) || + (ui_->checkBox_alignPosesWithGroundTruth->isEnabled() && ui_->checkBox_alignPosesWithGroundTruth->isChecked())) { std::map refPoses = groundTruthPoses_; - if(refPoses.empty()) + if(ui_->checkBox_alignPosesWithGPS->isEnabled() && + ui_->checkBox_alignPosesWithGPS->isChecked()) { refPoses = gpsPoses_; } @@ -2580,7 +2602,7 @@ void DatabaseViewer::exportPoses(int format) rotational_min, rotational_max); - if(ui_->checkBox_alignPosesWithGroundTruth->isChecked() && !gtToMap.isIdentity()) + if(!gtToMap.isIdentity()) { for(std::map::iterator iter=optimizedPoses.begin(); iter!=optimizedPoses.end(); ++iter) { @@ -3133,7 +3155,26 @@ void DatabaseViewer::exportSaved2DMap() path += ".pgm"; } cv::imwrite(path.toStdString(), map8U); - QMessageBox::information(this, tr("Export 2D map"), tr("Exported %1!").arg(path)); + + QFileInfo info(path); + QString yaml = info.absolutePath() + "/" + info.baseName() + ".yaml"; + + float occupancyThr = Parameters::defaultGridGlobalOccupancyThr(); + Parameters::parse(ui_->parameters_toolbox->getParameters(), Parameters::kGridGlobalOccupancyThr(), occupancyThr); + + std::ofstream file; + file.open (yaml.toStdString()); + file << "image: " << info.baseName().toStdString() << ".pgm" << std::endl; + file << "resolution: " << cellSize << std::endl; + file << "origin: [" << xMin << ", " << yMin << ", 0.0]" << std::endl; + file << "negate: 0" << std::endl; + file << "occupied_thresh: " << occupancyThr << std::endl; + file << "free_thresh: 0.196" << std::endl; + file << std::endl; + file.close(); + + + QMessageBox::information(this, tr("Export 2D map"), tr("Exported %1 and %2!").arg(path).arg(yaml)); } } } @@ -3488,7 +3529,9 @@ void DatabaseViewer::updateOptimizedMesh() } std::map optimizedPoses; - if(ui_->checkBox_alignScansCloudsWithGroundTruth->isChecked() && !groundTruthPoses_.empty()) + if(ui_->checkBox_alignScansCloudsWithGroundTruth->isEnabled() && + ui_->checkBox_alignScansCloudsWithGroundTruth->isChecked() + && !groundTruthPoses_.empty()) { optimizedPoses = groundTruthPoses_; } @@ -3868,7 +3911,11 @@ void DatabaseViewer::regenerateCurrentLocalMaps() QSet idsSet; idsSet.insert(ids_.at(ui_->horizontalSlider_A->value())); idsSet.insert(ids_.at(ui_->horizontalSlider_B->value())); +#if QT_VERSION >= QT_VERSION_CHECK(6, 0, 0) + QList ids(idsSet.begin(), idsSet.end()); +#else QList ids = idsSet.toList(); +#endif rtabmap::ProgressDialog progressDialog(this); progressDialog.setMaximumSteps(ids.size()); @@ -4001,7 +4048,9 @@ void DatabaseViewer::view3DMap() } std::map optimizedPoses; - if(ui_->checkBox_alignScansCloudsWithGroundTruth->isChecked() && !groundTruthPoses_.empty()) + if(ui_->checkBox_alignScansCloudsWithGroundTruth->isEnabled() && + ui_->checkBox_alignScansCloudsWithGroundTruth->isChecked() && + !groundTruthPoses_.empty()) { optimizedPoses = groundTruthPoses_; } @@ -4052,7 +4101,9 @@ void DatabaseViewer::generate3DMap() } std::map optimizedPoses; - if(ui_->checkBox_alignScansCloudsWithGroundTruth->isChecked() && !groundTruthPoses_.empty()) + if(ui_->checkBox_alignScansCloudsWithGroundTruth->isEnabled() && + ui_->checkBox_alignScansCloudsWithGroundTruth->isChecked() && + !groundTruthPoses_.empty()) { optimizedPoses = groundTruthPoses_; } @@ -4364,9 +4415,15 @@ void DatabaseViewer::refineAllLinks(const QList & links) { int from = links[i].from(); int to = links[i].to(); - this->refineConstraint(links[i].from(), links[i].to(), true); - - progressDialog->appendText(tr("Refined link %1->%2 (%3/%4)").arg(from).arg(to).arg(i+1).arg(links.size())); + if(from > 0 && to > 0) + { + this->refineConstraint(links[i].from(), links[i].to(), true); + progressDialog->appendText(tr("Refined link %1->%2 (%3/%4)").arg(from).arg(to).arg(i+1).arg(links.size())); + } + else + { + progressDialog->appendText(tr("Ignored link %1->%2 (landmark)").arg(from).arg(to)); + } progressDialog->incrementStep(); QApplication::processEvents(); if(progressDialog->isCanceled()) @@ -6624,7 +6681,7 @@ void DatabaseViewer::sliderIterationsValueChanged(int value) std::map graph = uValueAt(graphes_, value); std::map refPoses = groundTruthPoses_; - if(refPoses.empty()) + if(ui_->checkBox_alignPosesWithGPS->isEnabled() && ui_->checkBox_alignPosesWithGPS->isChecked()) { refPoses = gpsPoses_; } @@ -6688,7 +6745,9 @@ void DatabaseViewer::sliderIterationsValueChanged(int value) UINFO("rotational_min=%f", rotational_min); UINFO("rotational_max=%f", rotational_max); - if(ui_->checkBox_alignPosesWithGroundTruth->isChecked() && !gtToMap.isIdentity()) + if(((ui_->checkBox_alignPosesWithGPS->isEnabled() && ui_->checkBox_alignPosesWithGPS->isChecked()) || + (ui_->checkBox_alignPosesWithGroundTruth->isEnabled() && ui_->checkBox_alignPosesWithGroundTruth->isChecked())) && + !gtToMap.isIdentity()) { for(std::map::iterator iter=graph.begin(); iter!=graph.end(); ++iter) { @@ -6698,7 +6757,9 @@ void DatabaseViewer::sliderIterationsValueChanged(int value) } std::map graphFiltered; - if(ui_->checkBox_alignScansCloudsWithGroundTruth->isChecked() && !groundTruthPoses_.empty()) + if(ui_->checkBox_alignScansCloudsWithGroundTruth->isEnabled() && + ui_->checkBox_alignScansCloudsWithGroundTruth->isChecked() && + !groundTruthPoses_.empty()) { graphFiltered = groundTruthPoses_; } @@ -6805,7 +6866,7 @@ void DatabaseViewer::sliderIterationsValueChanged(int value) if(graph.size() && localMaps.size() && (ui_->graphViewer->isGridMapVisible() || ui_->dockWidget_occupancyGridView->isVisible())) { - QTime time; + QElapsedTimer time; time.start(); #ifdef RTABMAP_OCTOMAP @@ -7111,12 +7172,36 @@ void DatabaseViewer::sliderIterationsValueChanged(int value) ui_->label_pathLength->setNum(length); } } + +void DatabaseViewer::updateGraphRotation() +{ + if(ui_->horizontalSlider_rotation->isEnabled()) + { + float theta = float(ui_->horizontalSlider_rotation->value())*M_PI/1800.0f; + ui_->graphViewer->setWorldMapRotation(theta); + ui_->label_rotation->setText(QString::number(float(-ui_->horizontalSlider_rotation->value())/10.0f, 'f', 1) + " deg"); + } + else + { + ui_->graphViewer->setWorldMapRotation(0); + } +} + void DatabaseViewer::updateGraphView() { ui_->label_loopClosures->clear(); ui_->label_poses->clear(); ui_->label_rmse->clear(); + if(sender() == ui_->checkBox_alignPosesWithGPS && ui_->checkBox_alignPosesWithGPS->isChecked()) + { + ui_->checkBox_alignPosesWithGroundTruth->setChecked(false); + } + else if(sender() == ui_->checkBox_alignPosesWithGroundTruth && ui_->checkBox_alignPosesWithGroundTruth->isChecked()) + { + ui_->checkBox_alignPosesWithGPS->setChecked(false); + } + if(odomPoses_.size()) { int fromId = ui_->spinBox_optimizationsFrom->value(); @@ -7217,6 +7302,49 @@ void DatabaseViewer::updateGraphView() } } + // Marker priors parameters + double markerPriorsLinearVariance = Parameters::defaultMarkerPriorsVarianceLinear(); + double markerPriorsAngularVariance = Parameters::defaultMarkerPriorsVarianceAngular(); + std::map markerPriors; + ParametersMap parameters = ui_->parameters_toolbox->getParameters(); + Parameters::parse(parameters, Parameters::kMarkerPriorsVarianceLinear(), markerPriorsLinearVariance); + UASSERT(markerPriorsLinearVariance>0.0f); + Parameters::parse(parameters, Parameters::kMarkerPriorsVarianceAngular(), markerPriorsAngularVariance); + UASSERT(markerPriorsAngularVariance>0.0f); + std::string markerPriorsStr; + if(Parameters::parse(parameters, Parameters::kMarkerPriors(), markerPriorsStr)) + { + std::list strList = uSplit(markerPriorsStr, '|'); + for(std::list::iterator iter=strList.begin(); iter!=strList.end(); ++iter) + { + std::string markerStr = *iter; + while(!markerStr.empty() && !uIsDigit(markerStr[0])) + { + markerStr.erase(markerStr.begin()); + } + if(!markerStr.empty()) + { + std::string idStr = uSplitNumChar(markerStr).front(); + int id = uStr2Int(idStr); + Transform prior = Transform::fromString(markerStr.substr(idStr.size())); + if(!prior.isNull() && id>0) + { + markerPriors.insert(std::make_pair(-id, prior)); + UDEBUG("Added landmark prior %d: %s", id, prior.prettyPrint().c_str()); + } + else + { + UERROR("Failed to parse element \"%s\" in parameter %s", markerStr.c_str(), Parameters::kMarkerPriors().c_str()); + } + } + else if(!iter->empty()) + { + UERROR("Failed to parse parameter %s, value=\"%s\"", Parameters::kMarkerPriors().c_str(), iter->c_str()); + } + } + } + + // filter links int totalNeighbor = 0; int totalNeighborMerged = 0; @@ -7291,6 +7419,19 @@ void DatabaseViewer::updateGraphView() } loopLinks_.push_back(iter->second); ++totalLandmarks; + + // add landmark priors if there are some + int markerId = iter->second.to(); + if(markerPriors.find(markerId) != markerPriors.end()) + { + cv::Mat infMatrix = cv::Mat::eye(6, 6, CV_64FC1); + infMatrix(cv::Range(0,3), cv::Range(0,3)) /= markerPriorsLinearVariance; + infMatrix(cv::Range(3,6), cv::Range(3,6)) /= markerPriorsAngularVariance; + links.insert(std::make_pair(markerId, Link(markerId, markerId, Link::kPosePrior, markerPriors.at(markerId), infMatrix))); + UDEBUG("Added prior %d : %s (variance: lin=%f ang=%f)", markerId, markerPriors.at(markerId).prettyPrint().c_str(), + markerPriorsLinearVariance, markerPriorsAngularVariance); + ++totalPriors; + } } else if(iter->second.type() == Link::kPosePrior) { @@ -7332,12 +7473,17 @@ void DatabaseViewer::updateGraphView() while(uContains(weights_, link.to()) && weights_.at(link.to()) < 0) { std::multimap::iterator uter = links.find(link.to()); + while(uter != links.end() && + uter->first==link.to() && + uter->second.from()>uter->second.to()) + { + ++uter; + } if(uter != links.end()) { - UASSERT(links.count(link.to()) == 1); poses.erase(link.to()); link = link.merge(uter->second, uter->second.type()); - links.erase(uter); + links.erase(uter->first); } else { @@ -7350,41 +7496,162 @@ void DatabaseViewer::updateGraphView() } } - graphes_.push_back(poses); + bool applyRotation = sender() == ui_->pushButton_applyRotation; + if(applyRotation) + { + float xMin, yMin, cellSize; + bool hasMap = !dbDriver_->load2DMap(xMin, yMin, cellSize).empty(); + if(hasMap || !dbDriver_->loadOptimizedMesh().empty()) + { + QMessageBox::StandardButton r = QMessageBox::question(this, + tr("Rotate Optimized Graph"), + tr("There is a 2D occupancy grid or mesh already saved in " + "database. Applying rotation will clear them (they can be " + "regenerated later from File menu options). " + "Do you want to continue?"), + QMessageBox::Cancel | QMessageBox::Yes, + QMessageBox::Cancel); + if(r != QMessageBox::Yes) + { + applyRotation = false; + } + } + } - Optimizer * optimizer = Optimizer::create(ui_->parameters_toolbox->getParameters()); + std::map optPoses; + Transform lastLocalizationPose; + if(applyRotation || + ui_->comboBox_optimizationFlavor->currentIndex() == 2) + { + optPoses = dbDriver_->loadOptimizedPoses(&lastLocalizationPose); + if(optPoses.empty()) + { + ui_->comboBox_optimizationFlavor->setCurrentIndex(0); + QMessageBox::warning(this, tr("Optimization Flavor"), + tr("There is no local optimized graph in the database, " + "falling back to global iterative optimization.")); + } + } - std::map posesOut; - std::multimap linksOut; - UINFO("Get connected graph from %d (%d poses, %d links)", fromId, (int)poses.size(), (int)links.size()); - optimizer->getConnectedGraph( - fromId, - poses, - links, - posesOut, - linksOut); - UINFO("Connected graph of %d poses and %d links", (int)posesOut.size(), (int)linksOut.size()); - QTime time; - time.start(); - std::map finalPoses = optimizer->optimize(fromId, posesOut, linksOut, ui_->checkBox_iterativeOptimization->isChecked()?&graphes_:0); - ui_->label_timeOptimization->setNum(double(time.elapsed())/1000.0); - graphLinks_ = linksOut; - if(posesOut.size() && finalPoses.empty()) + if(applyRotation || + ui_->comboBox_optimizationFlavor->currentIndex() != 2) { - UWARN("Optimization failed... (poses=%d, links=%d).", (int)posesOut.size(), (int)linksOut.size()); - if(!optimizer->isCovarianceIgnored() || optimizer->type() != Optimizer::kTypeTORO) + if(ui_->horizontalSlider_rotation->value()!=0 && applyRotation) { - QMessageBox::warning(this, tr("Graph optimization error!"), tr("Graph optimization has failed. See the terminal for potential errors. " - "Give it a try with %1=0 and %2=true.").arg(Parameters::kOptimizerStrategy().c_str()).arg(Parameters::kOptimizerVarianceIgnored().c_str())); + float theta = float(-ui_->horizontalSlider_rotation->value())*M_PI/1800.0f; + Transform rotT(0,0,theta); + poses.at(fromId) = rotT * poses.at(fromId); } - else + + graphes_.push_back(poses); + + Optimizer * optimizer = Optimizer::create(parameters); + + std::map posesOut; + std::multimap linksOut; + UINFO("Get connected graph from %d (%d poses, %d links)", fromId, (int)poses.size(), (int)links.size()); + optimizer->getConnectedGraph( + fromId, + poses, + links, + posesOut, + linksOut); + UINFO("Connected graph of %d poses and %d links", (int)posesOut.size(), (int)linksOut.size()); + QElapsedTimer time; + time.start(); + std::map finalPoses = optimizer->optimize(fromId, posesOut, linksOut, ui_->comboBox_optimizationFlavor->currentIndex()==0?&graphes_:0); + ui_->label_timeOptimization->setNum(double(time.elapsed())/1000.0); + graphLinks_ = linksOut; + if(posesOut.size() && finalPoses.empty()) { - QMessageBox::warning(this, tr("Graph optimization error!"), tr("Graph optimization has failed. See the terminal for potential errors.")); + UWARN("Optimization failed... (poses=%d, links=%d).", (int)posesOut.size(), (int)linksOut.size()); + if(!optimizer->isCovarianceIgnored() || optimizer->type() != Optimizer::kTypeTORO) + { + QMessageBox::warning(this, tr("Graph optimization error!"), tr("Graph optimization has failed. See the terminal for potential errors. " + "Give it a try with %1=0 and %2=true.").arg(Parameters::kOptimizerStrategy().c_str()).arg(Parameters::kOptimizerVarianceIgnored().c_str())); + } + else + { + QMessageBox::warning(this, tr("Graph optimization error!"), tr("Graph optimization has failed. See the terminal for potential errors.")); + } + } + ui_->label_poses->setNum((int)finalPoses.size()); + graphes_.push_back(finalPoses); + delete optimizer; + + if(applyRotation && !finalPoses.empty()) + { + ui_->comboBox_optimizationFlavor->setCurrentIndex(2); + graphes_.clear(); + graphes_.push_back(finalPoses); + if(lastLocalizationPose.isNull()) + { + // use last pose by default + lastLocalizationPose = finalPoses.rbegin()->second; + } + dbDriver_->saveOptimizedPoses(finalPoses, lastLocalizationPose); + // reset optimized mesh and map as poses have changed + float xMin, yMin, cellSize; + bool hasMap = !dbDriver_->load2DMap(xMin, yMin, cellSize).empty(); + if(hasMap || !dbDriver_->loadOptimizedMesh().empty()) + { + dbDriver_->saveOptimizedMesh(cv::Mat()); + dbDriver_->save2DMap(cv::Mat(), 0, 0, 0); + QMessageBox::StandardButton r = QMessageBox::question(this, + tr("Rotate Optimized Graph"), + tr("Optimized graph has been rotated and saved back to database. " + "Note that 2D occupancy grid and mesh have been cleared (if set). " + "Do you want to regenerate the 2D occupancy grid now " + "(can be done later from File menu)?"), + QMessageBox::Ignore | QMessageBox::Yes, + QMessageBox::Yes); + ui_->actionEdit_optimized_2D_map->setEnabled(false); + ui_->actionExport_saved_2D_map->setEnabled(false); + ui_->actionImport_2D_map->setEnabled(false); + ui_->actionView_optimized_mesh->setEnabled(false); + ui_->actionExport_optimized_mesh->setEnabled(false); + if(r == QMessageBox::Yes) + { + regenerateSavedMap(); + } + } } } - ui_->label_poses->setNum((int)finalPoses.size()); - graphes_.push_back(finalPoses); - delete optimizer; + + // Update buttons state + if(ui_->comboBox_optimizationFlavor->currentIndex() == 2) + { + // Local optimized graph + if(graphes_.empty()) + { + ui_->label_timeOptimization->setNum(0); + ui_->label_poses->setNum((int)optPoses.size()); + graphes_.push_back(optPoses); + } + ui_->horizontalSlider_rotation->setEnabled(false); + ui_->pushButton_applyRotation->setEnabled(false); + ui_->spinBox_optimizationsFrom->setEnabled(false); + ui_->checkBox_spanAllMaps->setEnabled(false); + ui_->checkBox_wmState->setEnabled(false); + ui_->checkBox_alignPosesWithGPS->setEnabled(false); + ui_->checkBox_alignPosesWithGroundTruth->setEnabled(false); + ui_->checkBox_alignScansCloudsWithGroundTruth->setEnabled(false); + ui_->checkBox_ignoreIntermediateNodes->setEnabled(false); + } + else + { + // Global map re-optimized + ui_->pushButton_applyRotation->setEnabled(true); + ui_->horizontalSlider_rotation->setEnabled(true); + ui_->spinBox_optimizationsFrom->setEnabled(true); + ui_->checkBox_spanAllMaps->setEnabled(true); + ui_->checkBox_wmState->setEnabled(true); + ui_->checkBox_alignPosesWithGPS->setEnabled(ui_->checkBox_alignPosesWithGPS->isVisible()); + ui_->checkBox_alignPosesWithGroundTruth->setEnabled(ui_->checkBox_alignPosesWithGroundTruth->isVisible()); + ui_->checkBox_alignScansCloudsWithGroundTruth->setEnabled(ui_->checkBox_alignScansCloudsWithGroundTruth->isVisible()); + ui_->checkBox_ignoreIntermediateNodes->setEnabled(true); + } + updateGraphRotation(); } if(graphes_.size()) { diff --git a/guilib/src/DepthCalibrationDialog.cpp b/guilib/src/DepthCalibrationDialog.cpp index 9654c664bd..dbdc1135bd 100644 --- a/guilib/src/DepthCalibrationDialog.cpp +++ b/guilib/src/DepthCalibrationDialog.cpp @@ -465,7 +465,7 @@ void DepthCalibrationDialog::calibrate( ImageView * imageView2 = new ImageView(dialog); imageView2->setMinimumSize(320, 240); QVBoxLayout * vlayout = new QVBoxLayout(); - vlayout->setMargin(0); + vlayout->setContentsMargins(0,0,0,0); vlayout->addWidget(imageView1, 1); vlayout->addWidget(imageView2, 1); dialog->setLayout(vlayout); diff --git a/guilib/src/EditDepthArea.cpp b/guilib/src/EditDepthArea.cpp index 4be707ed5d..027bfb7d7b 100644 --- a/guilib/src/EditDepthArea.cpp +++ b/guilib/src/EditDepthArea.cpp @@ -30,6 +30,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include +#include #include #include "rtabmap/gui/EditDepthArea.h" diff --git a/guilib/src/EditMapArea.cpp b/guilib/src/EditMapArea.cpp index d61f882c97..f16217be1f 100644 --- a/guilib/src/EditMapArea.cpp +++ b/guilib/src/EditMapArea.cpp @@ -30,6 +30,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include +#include #include #include "rtabmap/gui/EditMapArea.h" diff --git a/guilib/src/ExportCloudsDialog.cpp b/guilib/src/ExportCloudsDialog.cpp index 9def478149..d85bdcf445 100644 --- a/guilib/src/ExportCloudsDialog.cpp +++ b/guilib/src/ExportCloudsDialog.cpp @@ -62,7 +62,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include -#include +#include +#include #ifdef RTABMAP_CPUTSDF #include @@ -1212,7 +1213,7 @@ void ExportCloudsDialog::viewClouds( } window->setMinimumWidth(120); window->setMinimumHeight(90); - window->resize(QDesktopWidget().availableGeometry(this).size() * 0.7); + window->resize(this->window()->windowHandle()->screen()->availableGeometry().size() * 0.7); CloudViewer * viewer = new CloudViewer(window); if(_ui->comboBox_pipeline->currentIndex() == 0) @@ -2843,7 +2844,7 @@ bool ExportCloudsDialog::getExportedClouds( } if(!image.empty()) { - + if(_ui->spinBox_camProjDecimation->value()>1) { image = util2d::decimate(image, _ui->spinBox_camProjDecimation->value()); @@ -4169,7 +4170,7 @@ void ExportCloudsDialog::saveClouds( #else QString extensions = tr("Save clouds to (*.ply *.pcd)..."); #endif - QString path = QFileDialog::getExistingDirectory(this, extensions, workingDirectory, 0); + QString path = QFileDialog::getExistingDirectory(this, extensions, workingDirectory, QFileDialog::ShowDirsOnly); if(!path.isEmpty()) { bool ok = false; @@ -4368,11 +4369,11 @@ void ExportCloudsDialog::saveMeshes( } else if(QFileInfo(path).suffix() == "obj") { - success = pcl::io::saveOBJFile(path.toStdString(), *meshes.begin()->second) == 0; + success = saveOBJFile(path, *meshes.begin()->second); } else { - UERROR("Extension not recognized! (%s) Should be (*.ply).", QFileInfo(path).suffix().toStdString().c_str()); + UERROR("Extension not recognized! (%s) Should be (*.ply) or (*.obj).", QFileInfo(path).suffix().toStdString().c_str()); } if(success) { @@ -4394,7 +4395,7 @@ void ExportCloudsDialog::saveMeshes( } else if(meshes.size()) { - QString path = QFileDialog::getExistingDirectory(this, tr("Save meshes to (*.ply *.obj)..."), workingDirectory, 0); + QString path = QFileDialog::getExistingDirectory(this, tr("Save meshes to (*.ply *.obj)..."), workingDirectory, QFileDialog::ShowDirsOnly); if(!path.isEmpty()) { bool ok = false; @@ -4454,7 +4455,7 @@ void ExportCloudsDialog::saveMeshes( } else if(suffix == "obj") { - success = pcl::io::saveOBJFile(pathFile.toStdString(), mesh) == 0; + success = saveOBJFile(pathFile, mesh); } else { @@ -4778,8 +4779,7 @@ void ExportCloudsDialog::saveTextureMeshes( } } - success = pcl::io::saveOBJFile(path.toStdString(), *mesh) == 0; - if(success) + if(saveOBJFile(path, mesh)) { _progressDialog->incrementStep(); _progressDialog->appendText(tr("Saving the mesh (with %1 textures)... done.").arg(mesh->tex_materials.size())); @@ -4799,7 +4799,7 @@ void ExportCloudsDialog::saveTextureMeshes( } else if(meshes.size()) { - QString path = QFileDialog::getExistingDirectory(this, tr("Save texture meshes to (*.obj)..."), workingDirectory, 0); + QString path = QFileDialog::getExistingDirectory(this, tr("Save texture meshes to (*.obj)..."), workingDirectory, QFileDialog::ShowDirsOnly); if(!path.isEmpty()) { bool ok = false; @@ -4975,7 +4975,7 @@ void ExportCloudsDialog::saveTextureMeshes( bool success =false; if(suffix == "obj") { - success = pcl::io::saveOBJFile(pathFile.toStdString(), *mesh) == 0; + success = saveOBJFile(pathFile, mesh); } else { @@ -5009,4 +5009,28 @@ void ExportCloudsDialog::saveTextureMeshes( } } + bool ExportCloudsDialog::saveOBJFile(const QString &path, pcl::TextureMesh::Ptr &mesh) const { +#if PCL_VERSION_COMPARE(>=, 1, 13, 0) + mesh->tex_coord_indices = std::vector>(); + auto nr_meshes = static_cast(mesh->tex_polygons.size()); + unsigned f_idx = 0; + for (unsigned m = 0; m < nr_meshes; m++) { + std::vector ci = mesh->tex_polygons[m]; + for(std::size_t i = 0; i < ci.size(); i++) { + for (std::size_t j = 0; j < ci[i].vertices.size(); j++) { + ci[i].vertices[j] = ci[i].vertices.size() * (i + f_idx) + j; + } + } + mesh->tex_coord_indices.push_back(ci); + f_idx += static_cast(mesh->tex_polygons[m].size()); + } +#endif + return pcl::io::saveOBJFile(path.toStdString(), *mesh) == 0; + } + + bool ExportCloudsDialog::saveOBJFile(const QString &path, pcl::PolygonMesh &mesh) const { + return pcl::io::saveOBJFile(path.toStdString(), mesh) == 0; + } + + } diff --git a/guilib/src/GraphViewer.cpp b/guilib/src/GraphViewer.cpp index b4d4eaeb44..c9b1110f16 100644 --- a/guilib/src/GraphViewer.cpp +++ b/guilib/src/GraphViewer.cpp @@ -62,6 +62,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #endif +#include + namespace rtabmap { class NodeItem: public QGraphicsEllipseItem @@ -129,6 +131,13 @@ class NodeItem: public QGraphicsEllipseItem break; } _pose=pose; + float r,p,yaw; + _pose.getEulerAngles(r, p, yaw); + if(_line) + { + float radius = this->rect().width()/2.0f; + _line->setLine(0,0,-radius*sin(yaw),-radius*cos(yaw)); + } } protected: @@ -242,10 +251,10 @@ class LinkItem: public QGraphicsLineItem protected: virtual void hoverEnterEvent ( QGraphicsSceneHoverEvent * event ) { - QString str = QString("%1->%2 (%3 m)").arg(_from).arg(_to).arg(_poseA.getDistance(_poseB)); + QString str = QString("%1->%2 (type=%3 length=%4 m)").arg(_from).arg(_to).arg(_link.type()).arg(_poseA.getDistance(_poseB)); if(!_link.transform().isNull()) { - str.append(QString("\n%1\n%2 %3").arg(_link.transform().prettyPrint().c_str()).arg(_link.transVariance()).arg(_link.rotVariance())); + str.append(QString("\n%1\nvar= %2 %3").arg(_link.transform().prettyPrint().c_str()).arg(_link.transVariance()).arg(_link.rotVariance())); } this->setToolTip(str); QPen pen = this->pen(); @@ -557,7 +566,7 @@ void GraphViewer::updateGraph(const std::map & poses, itemIter = _linkItems.find(iter->first); while(itemIter.key() == idFrom && itemIter != _linkItems.end()) { - if(itemIter.value()->to() == idTo) + if(itemIter.value()->to() == idTo && itemIter.value()->type() == iter->second.type()) { itemIter.value()->setPoses(poseA, poseB, _viewPlane); itemIter.value()->show(); @@ -1704,16 +1713,9 @@ void GraphViewer::setOrientationENU(bool enabled) _orientationENU = enabled; this->rotate(_orientationENU?90:270); } - if(_orientationENU) - { - QTransform t; - t.rotateRadians(_worldMapRotation); - _root->setTransform(t); - } - else - { - _root->resetTransform(); - } + QTransform t; + t.rotateRadians(_worldMapRotation); + _root->setTransform(t); if(_nodeItems.size() || _linkItems.size()) { this->scene()->setSceneRect(this->scene()->itemsBoundingRect()); // Re-shrink the scene to it's bounding contents @@ -1777,7 +1779,7 @@ void GraphViewer::restoreDefaults() void GraphViewer::wheelEvent ( QWheelEvent * event ) { - if(event->delta() < 0) + if(event->angleDelta().y() < 0) { this->scale(0.95, 0.95); } @@ -1798,6 +1800,8 @@ void GraphViewer::contextMenuEvent(QContextMenuEvent * event) { QMenu menu; QAction * aScreenShot = menu.addAction(tr("Take a screenshot...")); + QAction * aExportGridMap = menu.addAction(tr("Export grid map...")); + aExportGridMap->setEnabled(!_gridMap->pixmap().isNull()); menu.addSeparator(); QAction * aChangeNodeColor = menu.addAction(createIcon(_nodeColor), tr("Set node color...")); @@ -2039,7 +2043,7 @@ void GraphViewer::contextMenuEvent(QContextMenuEvent * event) if(QFileInfo(filePath).suffix().compare("pdf") == 0) { QPrinter printer(QPrinter::HighResolution); - printer.setOrientation(QPrinter::Portrait); + printer.setPageOrientation(QPageLayout::Portrait); printer.setOutputFileName( filePath ); QPainter p(&printer); scene()->render(&p); @@ -2095,6 +2099,48 @@ void GraphViewer::contextMenuEvent(QContextMenuEvent * event) } return; // without emitting configChanged } + else if(r == aExportGridMap) + { + float xMin, yMin, cellSize; + + cellSize = _gridCellSize; + xMin = -_gridMap->y()/100.0f; + yMin = -_gridMap->x()/100.0f; + + QString path = QFileDialog::getSaveFileName( + this, + tr("Save File"), + "map.pgm", + tr("Map (*.pgm)")); + + if(!path.isEmpty()) + { + if(QFileInfo(path).suffix() == "") + { + path += ".pgm"; + } + _gridMap->pixmap().save(path); + + QFileInfo info(path); + QString yaml = info.absolutePath() + "/" + info.baseName() + ".yaml"; + + float occupancyThr = Parameters::defaultGridGlobalOccupancyThr(); + + std::ofstream file; + file.open (yaml.toStdString()); + file << "image: " << info.baseName().toStdString() << ".pgm" << std::endl; + file << "resolution: " << cellSize << std::endl; + file << "origin: [" << xMin << ", " << yMin << ", 0.0]" << std::endl; + file << "negate: 0" << std::endl; + file << "occupied_thresh: " << occupancyThr << std::endl; + file << "free_thresh: 0.196" << std::endl; + file << std::endl; + file.close(); + + + QMessageBox::information(this, tr("Export 2D map"), tr("Exported %1 and %2!").arg(path).arg(yaml)); + } + } else if(r == aSetIntraInterSessionColors) { setIntraInterSessionColorsEnabled(aSetIntraInterSessionColors->isChecked()); diff --git a/guilib/src/ImageView.cpp b/guilib/src/ImageView.cpp index 1c07a713f5..118954a8c4 100644 --- a/guilib/src/ImageView.cpp +++ b/guilib/src/ImageView.cpp @@ -33,6 +33,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include +#include #include #include #include @@ -175,7 +176,8 @@ ImageView::ImageView(QWidget * parent) : _defaultFeatureColor(Qt::yellow), _defaultMatchingFeatureColor(Qt::magenta), _defaultMatchingLineColor(Qt::cyan), - _depthColorMapRange(0), + _depthColorMapMinRange(0), + _depthColorMapMaxRange(0), _imageItem(0), _imageDepthItem(0) { @@ -258,13 +260,14 @@ ImageView::ImageView(QWidget * parent) : _colorMapBlueToRed = colorMap->addAction(tr("Blue to red")); _colorMapBlueToRed->setCheckable(true); _colorMapBlueToRed->setChecked(false); - _colorMapRange = colorMap->addAction(tr("Max Range...")); + _colorMapMinRange = colorMap->addAction(tr("Min Range...")); + _colorMapMaxRange = colorMap->addAction(tr("Max Range...")); group = new QActionGroup(this); group->addAction(_colorMapWhiteToBlack); group->addAction(_colorMapBlackToWhite); group->addAction(_colorMapRedToBlue); group->addAction(_colorMapBlueToRed); - group->addAction(_colorMapRange); + group->addAction(_colorMapMaxRange); _saveImage = _menu->addAction(tr("Save picture...")); _saveImage->setEnabled(false); @@ -296,7 +299,8 @@ void ImageView::saveSettings(QSettings & settings, const QString & group) const settings.setValue("graphics_view_scale", this->isGraphicsViewScaled()); settings.setValue("graphics_view_scale_to_height", this->isGraphicsViewScaledToHeight()); settings.setValue("colormap", _colorMapWhiteToBlack->isChecked()?0:_colorMapBlackToWhite->isChecked()?1:_colorMapRedToBlue->isChecked()?2:3); - settings.setValue("colormap_range", this->getDepthColorMapRange()); + settings.setValue("colormap_min_range", this->getDepthColorMapMinRange()); + settings.setValue("colormap_max_range", this->getDepthColorMapMaxRange()); if(!group.isEmpty()) { settings.endGroup(); @@ -328,7 +332,9 @@ void ImageView::loadSettings(QSettings & settings, const QString & group) _colorMapBlackToWhite->setChecked(colorMap==1); _colorMapRedToBlue->setChecked(colorMap==2); _colorMapBlueToRed->setChecked(colorMap==3); - this->setDepthColorMapRange(settings.value("colormap_range", this->getDepthColorMapRange()).toFloat()); + this->setDepthColorMapRange( + settings.value("colormap_min_range", this->getDepthColorMapMinRange()).toFloat(), + settings.value("colormap_max_range", settings.value("colormap_range" /*backward compatibility*/, this->getDepthColorMapMaxRange())).toFloat()); if(!group.isEmpty()) { settings.endGroup(); @@ -392,9 +398,14 @@ const QColor & ImageView::getBackgroundColor() const return _graphicsView->backgroundBrush().color(); } -float ImageView::getDepthColorMapRange() const +float ImageView::getDepthColorMapMinRange() const { - return _depthColorMapRange; + return _depthColorMapMinRange; +} + +float ImageView::getDepthColorMapMaxRange() const +{ + return _depthColorMapMaxRange; } uCvQtDepthColorMap ImageView::getDepthColorMap() const @@ -694,9 +705,10 @@ void ImageView::setBackgroundColor(const QColor & color) } } -void ImageView::setDepthColorMapRange(float value) +void ImageView::setDepthColorMapRange(float min, float max) { - _depthColorMapRange = value; + _depthColorMapMinRange = min; + _depthColorMapMaxRange = max; } @@ -869,12 +881,12 @@ void ImageView::contextMenuEvent(QContextMenuEvent * e) if(QFileInfo(text).suffix().compare("pdf") == 0) { QPrinter printer(QPrinter::HighResolution); - printer.setOrientation(QPrinter::Portrait); + printer.setPageOrientation(QPageLayout::Portrait); printer.setOutputFileName( text ); QPainter p(&printer); p.begin(&printer); - double xscale = printer.pageRect().width()/double(_graphicsView->sceneRect().width()); - double yscale = printer.pageRect().height()/double(_graphicsView->sceneRect().height()); + double xscale = printer.pageLayout().paintRectPixels(printer.resolution()).width()/double(_graphicsView->sceneRect().width()); + double yscale = printer.pageLayout().paintRectPixels(printer.resolution()).height()/double(_graphicsView->sceneRect().height()); double scale = qMin(xscale, yscale); p.scale(scale, scale); _graphicsView->scene()->render(&p, _graphicsView->sceneRect(), _graphicsView->sceneRect()); @@ -973,13 +985,27 @@ void ImageView::contextMenuEvent(QContextMenuEvent * e) this->setImageDepth(_imageDepthCv); Q_EMIT configChanged(); } - else if(action == _colorMapRange) + else if(action == _colorMapMinRange) + { + bool ok = false; + double value = QInputDialog::getDouble(this, tr("Set depth colormap min range"), tr("Range (m), 0=no limit"), _depthColorMapMinRange, 0, 9999, 1, &ok); + if(ok) + { + this->setDepthColorMapRange(value, _depthColorMapMaxRange); + if(!_imageDepthCv.empty()) + this->setImageDepth(_imageDepthCv); + Q_EMIT configChanged(); + } + } + else if(action == _colorMapMaxRange) { bool ok = false; - double value = QInputDialog::getDouble(this, tr("Set depth colormap max range"), tr("Range (m), 0=no limit"), _depthColorMapRange, 0, 9999, 1, &ok); + double value = QInputDialog::getDouble(this, tr("Set depth colormap max range"), tr("Range (m), 0=no limit"), _depthColorMapMaxRange, 0, 9999, 1, &ok); if(ok) { - this->setDepthColorMapRange(value); + this->setDepthColorMapRange(_depthColorMapMinRange, value); + if(!_imageDepthCv.empty()) + this->setImageDepth(_imageDepthCv); Q_EMIT configChanged(); } } @@ -1170,7 +1196,7 @@ void ImageView::setImage(const QImage & image) void ImageView::setImageDepth(const cv::Mat & imageDepth) { _imageDepthCv = imageDepth; - setImageDepth(uCvMat2QImage(_imageDepthCv, true, getDepthColorMap(), 0.0f, _depthColorMapRange)); + setImageDepth(uCvMat2QImage(_imageDepthCv, true, getDepthColorMap(), _depthColorMapMinRange, _depthColorMapMaxRange)); } void ImageView::setImageDepth(const QImage & imageDepth) diff --git a/guilib/src/MainWindow.cpp b/guilib/src/MainWindow.cpp index 606610c9f7..94e76e5b6f 100644 --- a/guilib/src/MainWindow.cpp +++ b/guilib/src/MainWindow.cpp @@ -168,6 +168,7 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent, bool sh _processingOdometry(false), _oneSecondTimer(0), _elapsedTime(0), + _logEventTime(0), _posteriorCurve(0), _likelihoodCurve(0), _rawLikelihoodCurve(0), @@ -266,10 +267,10 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent, bool sh // Timer _oneSecondTimer = new QTimer(this); _oneSecondTimer->setInterval(1000); - _elapsedTime = new QTime(); + _elapsedTime = new QElapsedTimer(); _ui->label_elapsedTime->setText("00:00:00"); connect(_oneSecondTimer, SIGNAL(timeout()), this, SLOT(updateElapsedTime())); - _logEventTime = new QTime(); + _logEventTime = new QElapsedTimer(); _logEventTime->start(); //Graphics scenes @@ -597,10 +598,15 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent, bool sh _ui->statsToolBox->updateStat("Planning/Length/m", false); _ui->statsToolBox->updateStat("Camera/Time capturing/ms", false); + _ui->statsToolBox->updateStat("Camera/Time undistort depth/ms", false); + _ui->statsToolBox->updateStat("Camera/Time bilateral filtering/ms", false); _ui->statsToolBox->updateStat("Camera/Time decimation/ms", false); _ui->statsToolBox->updateStat("Camera/Time disparity/ms", false); _ui->statsToolBox->updateStat("Camera/Time mirroring/ms", false); + _ui->statsToolBox->updateStat("Camera/Time histogram equalization/ms", false); + _ui->statsToolBox->updateStat("Camera/Time exposure compensation/ms", false); _ui->statsToolBox->updateStat("Camera/Time scan from depth/ms", false); + _ui->statsToolBox->updateStat("Camera/Time total/ms", false); _ui->statsToolBox->updateStat("Odometry/ID/", false); _ui->statsToolBox->updateStat("Odometry/Features/", false); @@ -694,6 +700,7 @@ MainWindow::~MainWindow() this->stopDetection(); delete _ui; delete _elapsedTime; + delete _logEventTime; #ifdef RTABMAP_OCTOMAP delete _octomap; #endif @@ -986,15 +993,16 @@ void MainWindow::processCameraInfo(const rtabmap::SensorCaptureInfo & info) } if(_preferencesDialog->isCacheSavedInFigures() || _ui->statsToolBox->isVisible()) { - _ui->statsToolBox->updateStat("Camera/Time total/ms", _preferencesDialog->isTimeUsedInFigures()?info.stamp-_firstStamp:(float)info.id, info.timeTotal*1000.0f, _preferencesDialog->isCacheSavedInFigures()); _ui->statsToolBox->updateStat("Camera/Time capturing/ms", _preferencesDialog->isTimeUsedInFigures()?info.stamp-_firstStamp:(float)info.id, info.timeCapture*1000.0f, _preferencesDialog->isCacheSavedInFigures()); _ui->statsToolBox->updateStat("Camera/Time undistort depth/ms", _preferencesDialog->isTimeUsedInFigures()?info.stamp-_firstStamp:(float)info.id, info.timeUndistortDepth*1000.0f, _preferencesDialog->isCacheSavedInFigures()); _ui->statsToolBox->updateStat("Camera/Time bilateral filtering/ms", _preferencesDialog->isTimeUsedInFigures()?info.stamp-_firstStamp:(float)info.id, info.timeBilateralFiltering*1000.0f, _preferencesDialog->isCacheSavedInFigures()); _ui->statsToolBox->updateStat("Camera/Time decimation/ms", _preferencesDialog->isTimeUsedInFigures()?info.stamp-_firstStamp:(float)info.id, info.timeImageDecimation*1000.0f, _preferencesDialog->isCacheSavedInFigures()); _ui->statsToolBox->updateStat("Camera/Time disparity/ms", _preferencesDialog->isTimeUsedInFigures()?info.stamp-_firstStamp:(float)info.id, info.timeDisparity*1000.0f, _preferencesDialog->isCacheSavedInFigures()); _ui->statsToolBox->updateStat("Camera/Time mirroring/ms", _preferencesDialog->isTimeUsedInFigures()?info.stamp-_firstStamp:(float)info.id, info.timeMirroring*1000.0f, _preferencesDialog->isCacheSavedInFigures()); + _ui->statsToolBox->updateStat("Camera/Time histogram equalization/ms", _preferencesDialog->isTimeUsedInFigures()?info.stamp-_firstStamp:(float)info.id, info.timeHistogramEqualization*1000.0f, _preferencesDialog->isCacheSavedInFigures()); _ui->statsToolBox->updateStat("Camera/Time exposure compensation/ms", _preferencesDialog->isTimeUsedInFigures()?info.stamp-_firstStamp:(float)info.id, info.timeStereoExposureCompensation*1000.0f, _preferencesDialog->isCacheSavedInFigures()); _ui->statsToolBox->updateStat("Camera/Time scan from depth/ms", _preferencesDialog->isTimeUsedInFigures()?info.stamp-_firstStamp:(float)info.id, info.timeScanFromDepth*1000.0f, _preferencesDialog->isCacheSavedInFigures()); + _ui->statsToolBox->updateStat("Camera/Time total/ms", _preferencesDialog->isTimeUsedInFigures()?info.stamp-_firstStamp:(float)info.id, info.timeTotal*1000.0f, _preferencesDialog->isCacheSavedInFigures()); } Q_EMIT(cameraInfoProcessed()); @@ -1010,6 +1018,7 @@ void MainWindow::processOdometry(const rtabmap::OdometryEvent & odom, bool dataI UDEBUG(""); _processingOdometry = true; UTimer time; + UTimer timeTotal; // Process Data // Set color code as tooltip @@ -1705,6 +1714,11 @@ void MainWindow::processOdometry(const rtabmap::OdometryEvent & odom, bool dataI //draw lines UASSERT(odom.info().refCorners.size() == odom.info().newCorners.size()); std::set inliers(odom.info().cornerInliers.begin(), odom.info().cornerInliers.end()); + int subImageWidth = 0; + if(data->cameraModels().size()>1 || data->stereoCameraModels().size()>1) + { + subImageWidth = data->cameraModels().size()?data->cameraModels()[0].imageWidth():data->stereoCameraModels()[0].left().imageWidth(); + } for(unsigned int i=0; iimageView_odometry->isFeaturesShown() && inliers.find(i) != inliers.end()) @@ -1713,12 +1727,16 @@ void MainWindow::processOdometry(const rtabmap::OdometryEvent & odom, bool dataI } if(_ui->imageView_odometry->isLinesShown()) { - _ui->imageView_odometry->addLine( - odom.info().newCorners[i].x, - odom.info().newCorners[i].y, - odom.info().refCorners[i].x, - odom.info().refCorners[i].y, - inliers.find(i) != inliers.end()?Qt::blue:Qt::yellow); + // just draw lines in same camera + if(subImageWidth==0 || int(odom.info().refCorners[i].x/subImageWidth) == int(odom.info().newCorners[i].x/subImageWidth)) + { + _ui->imageView_odometry->addLine( + odom.info().newCorners[i].x, + odom.info().newCorners[i].y, + odom.info().refCorners[i].x, + odom.info().refCorners[i].y, + inliers.find(i) != inliers.end()?Qt::blue:Qt::yellow); + } } } } @@ -1881,7 +1899,7 @@ void MainWindow::processOdometry(const rtabmap::OdometryEvent & odom, bool dataI _ui->statsToolBox->updateStat("Odometry/Distance/m", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), odom.info().distanceTravelled, _preferencesDialog->isCacheSavedInFigures()); } - _ui->statsToolBox->updateStat("GUI/Refresh odom/ms", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), time.elapsed()*1000.0, _preferencesDialog->isCacheSavedInFigures()); + _ui->statsToolBox->updateStat("GUI/Refresh odom/ms", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), timeTotal.elapsed()*1000.0, _preferencesDialog->isCacheSavedInFigures()); UDEBUG("Time updating Stats toolbox: %fs", time.ticks()); } _processingOdometry = false; @@ -1893,7 +1911,7 @@ void MainWindow::processStats(const rtabmap::Statistics & stat) { _processingStatistics = true; ULOGGER_DEBUG(""); - QTime time, totalTime; + QElapsedTimer time, totalTime; time.start(); totalTime.start(); //Affichage des stats et images @@ -2210,7 +2228,9 @@ void MainWindow::processStats(const rtabmap::Statistics & stat) UDEBUG("time= %d ms (update detection ui)", time.restart()); //update image views - if(!signature.sensorData().imageRaw().empty() || signature.getWords().size()) + if(!signature.sensorData().imageRaw().empty() || + !loopSignature.sensorData().imageRaw().empty() || + signature.getWords().size()) { cv::Mat refImage = signature.sensorData().imageRaw(); cv::Mat loopImage = loopSignature.sensorData().imageRaw(); @@ -2391,7 +2411,10 @@ void MainWindow::processStats(const rtabmap::Statistics & stat) //====================== // RGB-D Mapping stuff //====================== - _odometryCorrection = stat.mapCorrection(); + if(!stat.mapCorrection().isNull()) + { + _odometryCorrection = stat.mapCorrection(); + } // update clouds if(stat.poses().size()) { @@ -2615,18 +2638,24 @@ void MainWindow::processStats(const rtabmap::Statistics & stat) { std::vector missingIds; bool ignoreNewData = smallMovement || fastMovement || signature.getWeight()<0; - for(std::map::const_iterator iter=stat.poses().begin(); iter!=stat.poses().end(); ++iter) + std::set ids = uKeysSet(stat.poses()); + if(ids.empty()) { - if(!ignoreNewData || stat.refImageId() != iter->first) + // In appearance-only mode + ids = uKeysSet(stat.posterior()); + } + for(std::set::const_iterator iter=ids.lower_bound(1); iter!=ids.end(); ++iter) + { + if(!ignoreNewData || stat.refImageId() != *iter) { - QMap::iterator ster = _cachedSignatures.find(iter->first); + QMap::iterator ster = _cachedSignatures.find(*iter); if(ster == _cachedSignatures.end() || (ster.value().getWeight() >=0 && // ignore intermediate nodes ster.value().sensorData().imageCompressed().empty() && ster.value().sensorData().depthOrRightCompressed().empty() && ster.value().sensorData().laserScanCompressed().empty())) { - missingIds.push_back(iter->first); + missingIds.push_back(*iter); } } } @@ -4962,7 +4991,12 @@ void MainWindow::drawKeypoints(const std::multimap & refWords { _lastId = (*refWords.rbegin()).first; } - _lastIds = QSet::fromList(QList::fromStdList(uKeysList(refWords))); + std::list kpts = uKeysList(refWords); +#if QT_VERSION >= QT_VERSION_CHECK(6, 0, 0) + _lastIds = QSet(kpts.begin(), kpts.end()); +#else + _lastIds = QSet::fromList(QList::fromStdList(kpts)); +#endif } // Draw lines between corresponding features... @@ -5205,7 +5239,7 @@ QString MainWindow::captureScreen(bool cacheInRAM, bool png) { QString name = (QDateTime::currentDateTime().toString("yyMMddhhmmsszzz") + (png?".png":".jpg")); _ui->statusbar->clearMessage(); - QPixmap figure = QPixmap::grabWidget(this); + QPixmap figure = this->grab(); QString targetDir = _preferencesDialog->getWorkingDirectory() + QDir::separator() + "ScreensCaptured"; QString msg; @@ -5740,6 +5774,11 @@ void MainWindow::startDetection() _camera->setMirroringEnabled(_preferencesDialog->isSourceMirroring()); _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( @@ -5830,27 +5869,20 @@ void MainWindow::startDetection() _preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcImages) && !_preferencesDialog->getIMUPath().isEmpty()) { - if( odomStrategy != Odometry::kTypeOkvis && - odomStrategy != Odometry::kTypeMSCKF && - odomStrategy != Odometry::kTypeVINS && - odomStrategy != Odometry::kTypeOpenVINS) + _imuThread = new IMUThread(_preferencesDialog->getIMURate(), _preferencesDialog->getIMULocalTransform()); + if(_preferencesDialog->getIMUFilteringStrategy()>0) { - QMessageBox::warning(this, tr("Source IMU Path"), - tr("IMU path is set but odometry chosen doesn't support asynchronous IMU, ignoring IMU..."), QMessageBox::Ok); + _imuThread->enableIMUFiltering(_preferencesDialog->getIMUFilteringStrategy()-1, parameters, _preferencesDialog->getIMUFilteringBaseFrameConversion()); } - else + if(!_imuThread->init(_preferencesDialog->getIMUPath().toStdString())) { - _imuThread = new IMUThread(_preferencesDialog->getIMURate(), _preferencesDialog->getIMULocalTransform()); - if(!_imuThread->init(_preferencesDialog->getIMUPath().toStdString())) - { - QMessageBox::warning(this, tr("Source IMU Path"), - tr("Initialization of IMU data has failed! Path=%1.").arg(_preferencesDialog->getIMUPath()), QMessageBox::Ok); - delete _camera; - _camera = 0; - delete _imuThread; - _imuThread = 0; - return; - } + QMessageBox::warning(this, tr("Source IMU Path"), + tr("Initialization of IMU data has failed! Path=%1.").arg(_preferencesDialog->getIMUPath()), QMessageBox::Ok); + delete _camera; + _camera = 0; + delete _imuThread; + _imuThread = 0; + return; } } Odometry * odom = Odometry::create(odomParameters); @@ -7199,6 +7231,11 @@ void MainWindow::updateCacheFromDatabase(const QString & path) signatures.insert(std::make_pair((*iter)->id(), *(*iter))); delete *iter; } + if(_currentPosesMap.empty() && _currentLinksMap.empty()) + { + _currentPosesMap = driver->loadOptimizedPoses(); + driver->getAllLinks(_currentLinksMap, true, true); + } RtabmapEvent3DMap event(signatures, _currentPosesMap, _currentLinksMap); processRtabmapEvent3DMap(event); } diff --git a/guilib/src/OdometryViewer.cpp b/guilib/src/OdometryViewer.cpp index ae46022dcf..27c6125565 100644 --- a/guilib/src/OdometryViewer.cpp +++ b/guilib/src/OdometryViewer.cpp @@ -126,13 +126,13 @@ OdometryViewer::OdometryViewer( //layout QHBoxLayout * layout = new QHBoxLayout(); - layout->setMargin(0); + layout->setContentsMargins(0,0,0,0); layout->setSpacing(0); layout->addWidget(imageView_,1); layout->addWidget(cloudView_,1); QHBoxLayout * hlayout2 = new QHBoxLayout(); - hlayout2->setMargin(0); + hlayout2->setContentsMargins(0,0,0,0); hlayout2->addWidget(maxCloudsLabel); hlayout2->addWidget(maxCloudsSpin_); hlayout2->addWidget(voxelLabel); @@ -151,7 +151,7 @@ OdometryViewer::OdometryViewer( hlayout2->addWidget(closeButton); QVBoxLayout * vlayout = new QVBoxLayout(this); - vlayout->setMargin(0); + vlayout->setContentsMargins(0,0,0,0); vlayout->setSpacing(0); vlayout->addLayout(layout, 1); vlayout->addLayout(hlayout2); diff --git a/guilib/src/ParametersToolBox.cpp b/guilib/src/ParametersToolBox.cpp index d23242056a..1bd583d25e 100644 --- a/guilib/src/ParametersToolBox.cpp +++ b/guilib/src/ParametersToolBox.cpp @@ -296,19 +296,14 @@ void ParametersToolBox::addParameter( { addParameter(layout, key.c_str(), QString::fromStdString(value)); } - else if(type.compare("int") == 0) + else if(type.compare("int") == 0 || + type.compare("uint") == 0 || + type.compare("unsigned int") == 0) { addParameter(layout, key.c_str(), uStr2Int(value)); } - else if(type.compare("uint") == 0) - { - addParameter(layout, key.c_str(), uStr2Int(value)); - } - else if(type.compare("double") == 0) - { - addParameter(layout, key.c_str(), uStr2Double(value)); - } - else if(type.compare("float") == 0) + else if(type.compare("double") == 0 || + type.compare("float") == 0) { addParameter(layout, key.c_str(), uStr2Double(value)); } @@ -316,6 +311,10 @@ void ParametersToolBox::addParameter( { addParameter(layout, key.c_str(), uStr2Bool(value)); } + else + { + UWARN("Not implemented type \"%s\" for parameter \"%s\". Parameter is not added to toolbox.", type.c_str(), key.c_str()); + } } void ParametersToolBox::addParameter(QVBoxLayout * layout, diff --git a/guilib/src/PreferencesDialog.cpp b/guilib/src/PreferencesDialog.cpp index 24d0cd4fdf..662b32a79d 100644 --- a/guilib/src/PreferencesDialog.cpp +++ b/guilib/src/PreferencesDialog.cpp @@ -380,6 +380,13 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) : _ui->comboBox_cameraStereo->setItemData(kSrcStereoZed - kSrcStereo, 0, Qt::UserRole - 1); _ui->comboBox_odom_sensor->setItemData(2, 0, Qt::UserRole - 1); } + else if(CameraStereoZed::sdkVersion() < 4) + { + _ui->comboBox_stereoZed_resolution->setItemData(2, 0, Qt::UserRole - 1); + _ui->comboBox_stereoZed_resolution->setItemData(4, 0, Qt::UserRole - 1); + _ui->comboBox_stereoZed_resolution->setItemData(6, 0, Qt::UserRole - 1); + _ui->comboBox_stereoZed_quality->setItemData(3, 0, Qt::UserRole - 1); + } if (!CameraStereoTara::available()) { _ui->comboBox_cameraStereo->setItemData(kSrcStereoTara - kSrcStereo, 0, Qt::UserRole - 1); @@ -676,6 +683,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) : connect(_ui->source_checkBox_ignoreGoals, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel())); connect(_ui->source_checkBox_ignoreLandmarks, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel())); connect(_ui->source_checkBox_ignoreFeatures, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel())); + connect(_ui->source_checkBox_ignorePriors, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel())); connect(_ui->source_spinBox_databaseStartId, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel())); connect(_ui->source_spinBox_databaseStopId, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel())); connect(_ui->source_checkBox_useDbStamps, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel())); @@ -797,21 +805,29 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) : connect(_ui->comboBox_depthai_resolution, SIGNAL(currentIndexChanged(int)), this, SLOT(makeObsoleteSourcePanel())); connect(_ui->checkBox_depthai_depth, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel())); connect(_ui->spinBox_depthai_confidence, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel())); + connect(_ui->checkBox_depthai_use_spec_translation, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel())); + connect(_ui->doubleSpinBox_depthai_alpha_scaling, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteSourcePanel())); connect(_ui->checkBox_depthai_imu_published, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel())); - connect(_ui->checkBox_depthai_imu_firmware_update, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel())); + connect(_ui->doubleSpinBox_depthai_laser_dot_brightness, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteSourcePanel())); + connect(_ui->doubleSpinBox_depthai_floodlight_brightness, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteSourcePanel())); + connect(_ui->comboBox_depthai_detect_features, SIGNAL(currentIndexChanged(int)), this, SLOT(makeObsoleteSourcePanel())); + connect(_ui->lineEdit_depthai_blob_path, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel())); + connect(_ui->toolButton_depthai_blob_path, SIGNAL(clicked()), this, SLOT(selectSourceDepthaiBlobPath())); 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())); connect(_ui->pushButton_calibrate_simple, SIGNAL(clicked()), this, SLOT(calibrateSimple())); connect(_ui->toolButton_openniOniPath, SIGNAL(clicked()), this, SLOT(selectSourceOniPath())); connect(_ui->toolButton_openni2OniPath, SIGNAL(clicked()), this, SLOT(selectSourceOni2Path())); - connect(_ui->comboBox_k4a_rgb_resolution, SIGNAL(currentIndexChanged(int)), this, SLOT(makeObsoleteSourcePanel())); - connect(_ui->comboBox_k4a_framerate, SIGNAL(currentIndexChanged(int)), this, SLOT(makeObsoleteSourcePanel())); + connect(_ui->comboBox_k4a_rgb_resolution, SIGNAL(currentIndexChanged(int)), this, SLOT(makeObsoleteSourcePanel())); + connect(_ui->comboBox_k4a_framerate, SIGNAL(currentIndexChanged(int)), this, SLOT(makeObsoleteSourcePanel())); connect(_ui->comboBox_k4a_depth_resolution, SIGNAL(currentIndexChanged(int)), this, SLOT(makeObsoleteSourcePanel())); - connect(_ui->checkbox_k4a_irDepth, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel())); + connect(_ui->checkbox_k4a_irDepth, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel())); connect(_ui->toolButton_k4a_mkv, SIGNAL(clicked()), this, SLOT(selectSourceMKVPath())); connect(_ui->toolButton_source_distortionModel, SIGNAL(clicked()), this, SLOT(selectSourceDistortionModel())); connect(_ui->toolButton_distortionModel, SIGNAL(clicked()), this, SLOT(visualizeDistortionModel())); @@ -1138,6 +1154,8 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) : _ui->checkbox_rgbd_createOccupancyGrid->setObjectName(Parameters::kRGBDCreateOccupancyGrid().c_str()); _ui->RGBDMarkerDetection->setObjectName(Parameters::kRGBDMarkerDetection().c_str()); _ui->spinBox_maxOdomCacheSize->setObjectName(Parameters::kRGBDMaxOdomCacheSize().c_str()); + _ui->checkbox_localizationSmoothing->setObjectName(Parameters::kRGBDLocalizationSmoothing().c_str()); + _ui->doubleSpinBox_localizationPriorError->setObjectName(Parameters::kRGBDLocalizationPriorError().c_str()); // Registration _ui->reg_repeatOnce->setObjectName(Parameters::kRegRepeatOnce().c_str()); @@ -1159,7 +1177,9 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) : _ui->loopClosure_pnpReprojError->setObjectName(Parameters::kVisPnPReprojError().c_str()); _ui->loopClosure_pnpFlags->setObjectName(Parameters::kVisPnPFlags().c_str()); _ui->loopClosure_pnpRefineIterations->setObjectName(Parameters::kVisPnPRefineIterations().c_str()); + _ui->loopClosure_pnpVarMedianRatio->setObjectName(Parameters::kVisPnPVarianceMedianRatio().c_str()); _ui->loopClosure_pnpMaxVariance->setObjectName(Parameters::kVisPnPMaxVariance().c_str()); + _ui->loopClosure_pnpSamplingPolicy->setObjectName(Parameters::kVisPnPSamplingPolicy().c_str()); _ui->reextract_nn->setObjectName(Parameters::kVisCorNNType().c_str()); connect(_ui->reextract_nn, SIGNAL(currentIndexChanged(int)), this, SLOT(updateFeatureMatchingVisibility())); _ui->reextract_nndrRatio->setObjectName(Parameters::kVisCorNNDR().c_str()); @@ -1193,6 +1213,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) : _ui->loopClosure_icpRangeMin->setObjectName(Parameters::kIcpRangeMin().c_str()); _ui->loopClosure_icpRangeMax->setObjectName(Parameters::kIcpRangeMax().c_str()); _ui->loopClosure_icpMaxCorrespondenceDistance->setObjectName(Parameters::kIcpMaxCorrespondenceDistance().c_str()); + _ui->loopClosure_icpReciprocalCorrespondences->setObjectName(Parameters::kIcpReciprocalCorrespondences().c_str()); _ui->loopClosure_icpIterations->setObjectName(Parameters::kIcpIterations().c_str()); _ui->loopClosure_icpEpsilon->setObjectName(Parameters::kIcpEpsilon().c_str()); _ui->loopClosure_icpRatio->setObjectName(Parameters::kIcpCorrespondenceRatio().c_str()); @@ -1368,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()); @@ -1414,6 +1441,70 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) : _ui->lineEdit_OdomVinsPath->setObjectName(Parameters::kOdomVINSConfigPath().c_str()); connect(_ui->toolButton_OdomVinsPath, SIGNAL(clicked()), this, SLOT(changeOdometryVINSConfigPath())); + // Odometry OpenVINS + _ui->checkBox_OdomOpenVINSUseStereo->setObjectName(Parameters::kOdomOpenVINSUseStereo().c_str()); + _ui->checkBox_OdomOpenVINSUseKLT->setObjectName(Parameters::kOdomOpenVINSUseKLT().c_str()); + _ui->spinBox_OdomOpenVINSNumPts->setObjectName(Parameters::kOdomOpenVINSNumPts().c_str()); + _ui->spinBox_OdomOpenVINSMinPxDist->setObjectName(Parameters::kOdomOpenVINSMinPxDist().c_str()); + _ui->checkBox_OdomOpenVINSFiTriangulate1d->setObjectName(Parameters::kOdomOpenVINSFiTriangulate1d().c_str()); + _ui->checkBox_OdomOpenVINSFiRefineFeatures->setObjectName(Parameters::kOdomOpenVINSFiRefineFeatures().c_str()); + _ui->spinBox_OdomOpenVINSFiMaxRuns->setObjectName(Parameters::kOdomOpenVINSFiMaxRuns().c_str()); + _ui->doubleSpinBox_OdomOpenVINSFiMaxBaseline->setObjectName(Parameters::kOdomOpenVINSFiMaxBaseline().c_str()); + _ui->doubleSpinBox_OdomOpenVINSFiMaxCondNumber->setObjectName(Parameters::kOdomOpenVINSFiMaxCondNumber().c_str()); + + _ui->checkBox_OdomOpenVINSUseFEJ->setObjectName(Parameters::kOdomOpenVINSUseFEJ().c_str()); + _ui->comboBox_OdomOpenVINSIntegration->setObjectName(Parameters::kOdomOpenVINSIntegration().c_str()); + _ui->checkBox_OdomOpenVINSCalibCamExtrinsics->setObjectName(Parameters::kOdomOpenVINSCalibCamExtrinsics().c_str()); + _ui->checkBox_OdomOpenVINSCalibCamIntrinsics->setObjectName(Parameters::kOdomOpenVINSCalibCamIntrinsics().c_str()); + _ui->checkBox_OdomOpenVINSCalibCamTimeoffset->setObjectName(Parameters::kOdomOpenVINSCalibCamTimeoffset().c_str()); + _ui->checkBox_OdomOpenVINSCalibIMUIntrinsics->setObjectName(Parameters::kOdomOpenVINSCalibIMUIntrinsics().c_str()); + _ui->checkBox_OdomOpenVINSCalibIMUGSensitivity->setObjectName(Parameters::kOdomOpenVINSCalibIMUGSensitivity().c_str()); + _ui->spinBox_OdomOpenVINSMaxClones->setObjectName(Parameters::kOdomOpenVINSMaxClones().c_str()); + _ui->spinBox_OdomOpenVINSMaxSLAM->setObjectName(Parameters::kOdomOpenVINSMaxSLAM().c_str()); + _ui->spinBox_OdomOpenVINSMaxSLAMInUpdate->setObjectName(Parameters::kOdomOpenVINSMaxSLAMInUpdate().c_str()); + _ui->spinBox_OdomOpenVINSMaxMSCKFInUpdate->setObjectName(Parameters::kOdomOpenVINSMaxMSCKFInUpdate().c_str()); + _ui->comboBox_OdomOpenVINSFeatRepMSCKF->setObjectName(Parameters::kOdomOpenVINSFeatRepMSCKF().c_str()); + _ui->comboBox_OdomOpenVINSFeatRepSLAM->setObjectName(Parameters::kOdomOpenVINSFeatRepSLAM().c_str()); + _ui->doubleSpinBox_OdomOpenVINSDtSLAMDelay->setObjectName(Parameters::kOdomOpenVINSDtSLAMDelay().c_str()); + _ui->doubleSpinBox_OdomOpenVINSGravityMag->setObjectName(Parameters::kOdomOpenVINSGravityMag().c_str()); + _ui->lineEdit_OdomOpenVINSLeftMaskPath->setObjectName(Parameters::kOdomOpenVINSLeftMaskPath().c_str()); + connect(_ui->toolButton_OdomOpenVINSLeftMaskPath, SIGNAL(clicked()), this, SLOT(changeOdometryOpenVINSLeftMask())); + _ui->lineEdit_OdomOpenVINSRightMaskPath->setObjectName(Parameters::kOdomOpenVINSRightMaskPath().c_str()); + connect(_ui->toolButton_OdomOpenVINSRightMaskPath, SIGNAL(clicked()), this, SLOT(changeOdometryOpenVINSRightMask())); + + _ui->doubleSpinBox_OdomOpenVINSInitWindowTime->setObjectName(Parameters::kOdomOpenVINSInitWindowTime().c_str()); + _ui->doubleSpinBox_OdomOpenVINSInitIMUThresh->setObjectName(Parameters::kOdomOpenVINSInitIMUThresh().c_str()); + _ui->doubleSpinBox_OdomOpenVINSInitMaxDisparity->setObjectName(Parameters::kOdomOpenVINSInitMaxDisparity().c_str()); + _ui->spinBox_OdomOpenVINSInitMaxFeatures->setObjectName(Parameters::kOdomOpenVINSInitMaxFeatures().c_str()); + _ui->checkBox_OdomOpenVINSInitDynUse->setObjectName(Parameters::kOdomOpenVINSInitDynUse().c_str()); + _ui->checkBox_OdomOpenVINSInitDynMLEOptCalib->setObjectName(Parameters::kOdomOpenVINSInitDynMLEOptCalib().c_str()); + _ui->spinBox_OdomOpenVINSInitDynMLEMaxIter->setObjectName(Parameters::kOdomOpenVINSInitDynMLEMaxIter().c_str()); + _ui->doubleSpinBox_OdomOpenVINSInitDynMLEMaxTime->setObjectName(Parameters::kOdomOpenVINSInitDynMLEMaxTime().c_str()); + _ui->spinBox_OdomOpenVINSInitDynMLEMaxThreads->setObjectName(Parameters::kOdomOpenVINSInitDynMLEMaxThreads().c_str()); + _ui->spinBox_OdomOpenVINSInitDynNumPose->setObjectName(Parameters::kOdomOpenVINSInitDynNumPose().c_str()); + _ui->doubleSpinBox_OdomOpenVINSInitDynMinDeg->setObjectName(Parameters::kOdomOpenVINSInitDynMinDeg().c_str()); + _ui->doubleSpinBox_OdomOpenVINSInitDynInflationOri->setObjectName(Parameters::kOdomOpenVINSInitDynInflationOri().c_str()); + _ui->doubleSpinBox_OdomOpenVINSInitDynInflationVel->setObjectName(Parameters::kOdomOpenVINSInitDynInflationVel().c_str()); + _ui->doubleSpinBox_OdomOpenVINSInitDynInflationBg->setObjectName(Parameters::kOdomOpenVINSInitDynInflationBg().c_str()); + _ui->doubleSpinBox_OdomOpenVINSInitDynInflationBa->setObjectName(Parameters::kOdomOpenVINSInitDynInflationBa().c_str()); + _ui->doubleSpinBox_OdomOpenVINSInitDynMinRecCond->setObjectName(Parameters::kOdomOpenVINSInitDynMinRecCond().c_str()); + + _ui->checkBox_OdomOpenVINSTryZUPT->setObjectName(Parameters::kOdomOpenVINSTryZUPT().c_str()); + _ui->doubleSpinBox_OdomOpenVINSZUPTChi2Multiplier->setObjectName(Parameters::kOdomOpenVINSZUPTChi2Multiplier().c_str()); + _ui->doubleSpinBox_OdomOpenVINSZUPTMaxVelodicy->setObjectName(Parameters::kOdomOpenVINSZUPTMaxVelodicy().c_str()); + _ui->doubleSpinBox_OdomOpenVINSZUPTNoiseMultiplier->setObjectName(Parameters::kOdomOpenVINSZUPTNoiseMultiplier().c_str()); + _ui->doubleSpinBox_OdomOpenVINSZUPTMaxDisparity->setObjectName(Parameters::kOdomOpenVINSZUPTMaxDisparity().c_str()); + _ui->checkBox_OdomOpenVINSZUPTOnlyAtBeginning->setObjectName(Parameters::kOdomOpenVINSZUPTOnlyAtBeginning().c_str()); + + _ui->doubleSpinBox_OdomOpenVINSAccelerometerNoiseDensity->setObjectName(Parameters::kOdomOpenVINSAccelerometerNoiseDensity().c_str()); + _ui->doubleSpinBox_OdomOpenVINSAccelerometerRandomWalk->setObjectName(Parameters::kOdomOpenVINSAccelerometerRandomWalk().c_str()); + _ui->doubleSpinBox_OdomOpenVINSGyroscopeNoiseDensity->setObjectName(Parameters::kOdomOpenVINSGyroscopeNoiseDensity().c_str()); + _ui->doubleSpinBox_OdomOpenVINSGyroscopeRandomWalk->setObjectName(Parameters::kOdomOpenVINSGyroscopeRandomWalk().c_str()); + _ui->doubleSpinBox_OdomOpenVINSUpMSCKFSigmaPx->setObjectName(Parameters::kOdomOpenVINSUpMSCKFSigmaPx().c_str()); + _ui->doubleSpinBox_OdomOpenVINSUpMSCKFChi2Multiplier->setObjectName(Parameters::kOdomOpenVINSUpMSCKFChi2Multiplier().c_str()); + _ui->doubleSpinBox_OdomOpenVINSUpSLAMSigmaPx->setObjectName(Parameters::kOdomOpenVINSUpSLAMSigmaPx().c_str()); + _ui->doubleSpinBox_OdomOpenVINSUpSLAMChi2Multiplier->setObjectName(Parameters::kOdomOpenVINSUpSLAMChi2Multiplier().c_str()); + //Stereo _ui->stereo_winWidth->setObjectName(Parameters::kStereoWinWidth().c_str()); _ui->stereo_winHeight->setObjectName(Parameters::kStereoWinHeight().c_str()); @@ -1930,6 +2021,7 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox) _ui->source_checkBox_ignoreGoals->setChecked(true); _ui->source_checkBox_ignoreLandmarks->setChecked(true); _ui->source_checkBox_ignoreFeatures->setChecked(true); + _ui->source_checkBox_ignorePriors->setChecked(false); _ui->source_spinBox_databaseStartId->setValue(0); _ui->source_spinBox_databaseStopId->setValue(0); _ui->source_spinBox_database_cameraIndex->setValue(-1); @@ -1966,6 +2058,8 @@ 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); @@ -2003,9 +2097,9 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox) _ui->lineEdit_rs2_jsonFile->clear(); _ui->lineEdit_openniOniPath->clear(); _ui->lineEdit_openni2OniPath->clear(); - _ui->comboBox_k4a_rgb_resolution->setCurrentIndex(0); - _ui->comboBox_k4a_framerate->setCurrentIndex(2); - _ui->comboBox_k4a_depth_resolution->setCurrentIndex(2); + _ui->comboBox_k4a_rgb_resolution->setCurrentIndex(0); + _ui->comboBox_k4a_framerate->setCurrentIndex(2); + _ui->comboBox_k4a_depth_resolution->setCurrentIndex(2); _ui->checkbox_k4a_irDepth->setChecked(false); _ui->lineEdit_k4a_mkv->clear(); _ui->source_checkBox_useMKVStamps->setChecked(true); @@ -2030,7 +2124,7 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox) _ui->spinBox_stereo_right_device->setValue(-1); _ui->spinBox_stereousbcam_streamWidth->setValue(0); _ui->spinBox_stereousbcam_streamHeight->setValue(0); - _ui->comboBox_stereoZed_resolution->setCurrentIndex(2); + _ui->comboBox_stereoZed_resolution->setCurrentIndex(CameraStereoZed::sdkVersion()<4?3:6); _ui->comboBox_stereoZed_quality->setCurrentIndex(1); _ui->checkbox_stereoZed_selfCalibration->setChecked(true); _ui->comboBox_stereoZed_sensingMode->setCurrentIndex(0); @@ -2048,8 +2142,13 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox) _ui->comboBox_depthai_resolution->setCurrentIndex(1); _ui->checkBox_depthai_depth->setChecked(false); _ui->spinBox_depthai_confidence->setValue(200); + _ui->checkBox_depthai_use_spec_translation->setChecked(false); + _ui->doubleSpinBox_depthai_alpha_scaling->setValue(0.0); _ui->checkBox_depthai_imu_published->setChecked(true); - _ui->checkBox_depthai_imu_firmware_update->setChecked(false); + _ui->doubleSpinBox_depthai_laser_dot_brightness->setValue(0.0); + _ui->doubleSpinBox_depthai_floodlight_brightness->setValue(200.0); + _ui->comboBox_depthai_detect_features->setCurrentIndex(0); + _ui->lineEdit_depthai_blob_path->clear(); _ui->checkBox_cameraImages_configForEachFrame->setChecked(false); _ui->checkBox_cameraImages_timestamps->setChecked(false); @@ -2395,6 +2494,8 @@ void PreferencesDialog::readCameraSettings(const QString & filePath) _ui->lineEdit_sourceDevice->setText(settings.value("device",_ui->lineEdit_sourceDevice->text()).toString()); _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) { @@ -2454,9 +2555,9 @@ void PreferencesDialog::readCameraSettings(const QString & filePath) settings.endGroup(); // K4W2 settings.beginGroup("K4A"); - _ui->comboBox_k4a_rgb_resolution->setCurrentIndex(settings.value("rgb_resolution", _ui->comboBox_k4a_rgb_resolution->currentIndex()).toInt()); - _ui->comboBox_k4a_framerate->setCurrentIndex(settings.value("framerate", _ui->comboBox_k4a_framerate->currentIndex()).toInt()); - _ui->comboBox_k4a_depth_resolution->setCurrentIndex(settings.value("depth_resolution", _ui->comboBox_k4a_depth_resolution->currentIndex()).toInt()); + _ui->comboBox_k4a_rgb_resolution->setCurrentIndex(settings.value("rgb_resolution", _ui->comboBox_k4a_rgb_resolution->currentIndex()).toInt()); + _ui->comboBox_k4a_framerate->setCurrentIndex(settings.value("framerate", _ui->comboBox_k4a_framerate->currentIndex()).toInt()); + _ui->comboBox_k4a_depth_resolution->setCurrentIndex(settings.value("depth_resolution", _ui->comboBox_k4a_depth_resolution->currentIndex()).toInt()); _ui->checkbox_k4a_irDepth->setChecked(settings.value("ir", _ui->checkbox_k4a_irDepth->isChecked()).toBool()); _ui->lineEdit_k4a_mkv->setText(settings.value("mkvPath", _ui->lineEdit_k4a_mkv->text()).toString()); _ui->source_checkBox_useMKVStamps->setChecked(settings.value("useMkvStamps", _ui->source_checkBox_useMKVStamps->isChecked()).toBool()); @@ -2532,8 +2633,13 @@ void PreferencesDialog::readCameraSettings(const QString & filePath) _ui->comboBox_depthai_resolution->setCurrentIndex(settings.value("resolution", _ui->comboBox_depthai_resolution->currentIndex()).toInt()); _ui->checkBox_depthai_depth->setChecked(settings.value("depth", _ui->checkBox_depthai_depth->isChecked()).toBool()); _ui->spinBox_depthai_confidence->setValue(settings.value("confidence", _ui->spinBox_depthai_confidence->value()).toInt()); + _ui->checkBox_depthai_use_spec_translation->setChecked(settings.value("use_spec_translation", _ui->checkBox_depthai_use_spec_translation->isChecked()).toBool()); + _ui->doubleSpinBox_depthai_alpha_scaling->setValue(settings.value("alpha_scaling", _ui->doubleSpinBox_depthai_alpha_scaling->value()).toDouble()); _ui->checkBox_depthai_imu_published->setChecked(settings.value("imu_published", _ui->checkBox_depthai_imu_published->isChecked()).toBool()); - _ui->checkBox_depthai_imu_firmware_update->setChecked(settings.value("imu_firmware_update", _ui->checkBox_depthai_imu_firmware_update->isChecked()).toBool()); + _ui->doubleSpinBox_depthai_laser_dot_brightness->setValue(settings.value("laser_dot_brightness", _ui->doubleSpinBox_depthai_laser_dot_brightness->value()).toDouble()); + _ui->doubleSpinBox_depthai_floodlight_brightness->setValue(settings.value("floodlight_brightness", _ui->doubleSpinBox_depthai_floodlight_brightness->value()).toDouble()); + _ui->comboBox_depthai_detect_features->setCurrentIndex(settings.value("detect_features", _ui->comboBox_depthai_detect_features->currentIndex()).toInt()); + _ui->lineEdit_depthai_blob_path->setText(settings.value("blob_path", _ui->lineEdit_depthai_blob_path->text()).toString()); settings.endGroup(); // DepthAI settings.beginGroup("Images"); @@ -2617,6 +2723,7 @@ void PreferencesDialog::readCameraSettings(const QString & filePath) _ui->source_checkBox_ignoreGoals->setChecked(settings.value("ignoreGoals", _ui->source_checkBox_ignoreGoals->isChecked()).toBool()); _ui->source_checkBox_ignoreLandmarks->setChecked(settings.value("ignoreLandmarks", _ui->source_checkBox_ignoreLandmarks->isChecked()).toBool()); _ui->source_checkBox_ignoreFeatures->setChecked(settings.value("ignoreFeatures", _ui->source_checkBox_ignoreFeatures->isChecked()).toBool()); + _ui->source_checkBox_ignorePriors->setChecked(settings.value("ignorePriors", _ui->source_checkBox_ignorePriors->isChecked()).toBool()); _ui->source_spinBox_databaseStartId->setValue(settings.value("startId", _ui->source_spinBox_databaseStartId->value()).toInt()); _ui->source_spinBox_databaseStopId->setValue(settings.value("stopId", _ui->source_spinBox_databaseStopId->value()).toInt()); _ui->source_spinBox_database_cameraIndex->setValue(settings.value("cameraIndex", _ui->source_spinBox_database_cameraIndex->value()).toInt()); @@ -2927,6 +3034,8 @@ void PreferencesDialog::writeCameraSettings(const QString & filePath) const settings.setValue("device", _ui->lineEdit_sourceDevice->text()); 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()); @@ -2980,9 +3089,9 @@ void PreferencesDialog::writeCameraSettings(const QString & filePath) const settings.endGroup(); // K4W2 settings.beginGroup("K4A"); - settings.setValue("rgb_resolution", _ui->comboBox_k4a_rgb_resolution->currentIndex()); - settings.setValue("framerate", _ui->comboBox_k4a_framerate->currentIndex()); - settings.setValue("depth_resolution", _ui->comboBox_k4a_depth_resolution->currentIndex()); + settings.setValue("rgb_resolution", _ui->comboBox_k4a_rgb_resolution->currentIndex()); + settings.setValue("framerate", _ui->comboBox_k4a_framerate->currentIndex()); + settings.setValue("depth_resolution", _ui->comboBox_k4a_depth_resolution->currentIndex()); settings.setValue("ir", _ui->checkbox_k4a_irDepth->isChecked()); settings.setValue("mkvPath", _ui->lineEdit_k4a_mkv->text()); settings.setValue("useMkvStamps", _ui->source_checkBox_useMKVStamps->isChecked()); @@ -3055,11 +3164,16 @@ void PreferencesDialog::writeCameraSettings(const QString & filePath) const settings.endGroup(); // MyntEye settings.beginGroup("DepthAI"); - settings.setValue("resolution", _ui->comboBox_depthai_resolution->currentIndex()); - settings.setValue("depth", _ui->checkBox_depthai_depth->isChecked()); - settings.setValue("confidence", _ui->spinBox_depthai_confidence->value()); - settings.setValue("imu_published", _ui->checkBox_depthai_imu_published->isChecked()); - settings.setValue("imu_firmware_update", _ui->checkBox_depthai_imu_firmware_update->isChecked()); + settings.setValue("resolution", _ui->comboBox_depthai_resolution->currentIndex()); + settings.setValue("depth", _ui->checkBox_depthai_depth->isChecked()); + settings.setValue("confidence", _ui->spinBox_depthai_confidence->value()); + settings.setValue("use_spec_translation", _ui->checkBox_depthai_use_spec_translation->isChecked()); + settings.setValue("alpha_scaling", _ui->doubleSpinBox_depthai_alpha_scaling->value()); + settings.setValue("imu_published", _ui->checkBox_depthai_imu_published->isChecked()); + settings.setValue("laser_dot_brightness", _ui->doubleSpinBox_depthai_laser_dot_brightness->value()); + settings.setValue("floodlight_brightness", _ui->doubleSpinBox_depthai_floodlight_brightness->value()); + settings.setValue("detect_features", _ui->comboBox_depthai_detect_features->currentIndex()); + settings.setValue("blob_path", _ui->lineEdit_depthai_blob_path->text()); settings.endGroup(); // DepthAI settings.beginGroup("Images"); @@ -3141,6 +3255,7 @@ void PreferencesDialog::writeCameraSettings(const QString & filePath) const settings.setValue("ignoreGoals", _ui->source_checkBox_ignoreGoals->isChecked()); settings.setValue("ignoreLandmarks", _ui->source_checkBox_ignoreLandmarks->isChecked()); settings.setValue("ignoreFeatures", _ui->source_checkBox_ignoreFeatures->isChecked()); + settings.setValue("ignorePriors", _ui->source_checkBox_ignorePriors->isChecked()); settings.setValue("startId", _ui->source_spinBox_databaseStartId->value()); settings.setValue("stopId", _ui->source_spinBox_databaseStopId->value()); settings.setValue("cameraIndex", _ui->source_spinBox_database_cameraIndex->value()); @@ -3917,12 +4032,16 @@ void PreferencesDialog::selectSourceDriver(Src src, int variant) _ui->spinBox_rs2_width->setValue(1280); _ui->spinBox_rs2_height->setValue(720); _ui->spinBox_rs2_rate->setValue(30); + _ui->checkbox_rs2_irMode->setChecked(false); + _ui->checkbox_rs2_emitter->setChecked(true); } - else + else // D400 { _ui->spinBox_rs2_width->setValue(848); _ui->spinBox_rs2_height->setValue(480); _ui->spinBox_rs2_rate->setValue(60); + _ui->checkbox_rs2_irMode->setChecked(true); + _ui->checkbox_rs2_emitter->setChecked(false); } } } @@ -4387,6 +4506,20 @@ void PreferencesDialog::selectSourceRealsense2JsonPath() } } +void PreferencesDialog::selectSourceDepthaiBlobPath() +{ + QString dir = _ui->lineEdit_depthai_blob_path->text(); + if(dir.isEmpty()) + { + dir = getWorkingDirectory(); + } + QString path = QFileDialog::getOpenFileName(this, tr("Select file"), dir, tr("MyriadX blob (*.blob)")); + if(path.size()) + { + _ui->lineEdit_depthai_blob_path->setText(path); + } +} + void PreferencesDialog::setParameter(const std::string & key, const std::string & value) { UDEBUG("%s=%s", key.c_str(), value.c_str()); @@ -5094,6 +5227,40 @@ void PreferencesDialog::changeOdometryVINSConfigPath() } } +void PreferencesDialog::changeOdometryOpenVINSLeftMask() +{ + QString path; + if(_ui->lineEdit_OdomOpenVINSLeftMaskPath->text().isEmpty()) + { + path = QFileDialog::getOpenFileName(this, tr("Select Left Mask"), this->getWorkingDirectory(), tr("Image mask (*.jpg *.png)")); + } + else + { + path = QFileDialog::getOpenFileName(this, tr("Select Left Mask"), _ui->lineEdit_OdomOpenVINSLeftMaskPath->text(), tr("Image mask (*.jpg *.png)")); + } + if(!path.isEmpty()) + { + _ui->lineEdit_OdomOpenVINSLeftMaskPath->setText(path); + } +} + +void PreferencesDialog::changeOdometryOpenVINSRightMask() +{ + QString path; + if(_ui->lineEdit_OdomOpenVINSRightMaskPath->text().isEmpty()) + { + path = QFileDialog::getOpenFileName(this, tr("Select Right Mask"), this->getWorkingDirectory(), tr("Image mask (*.jpg *.png)")); + } + else + { + path = QFileDialog::getOpenFileName(this, tr("Select Right Mask"), _ui->lineEdit_OdomOpenVINSRightMaskPath->text(), tr("Image mask (*.jpg *.png)")); + } + if(!path.isEmpty()) + { + _ui->lineEdit_OdomOpenVINSRightMaskPath->setText(path); + } +} + void PreferencesDialog::changeIcpPMConfigPath() { QString path; @@ -5867,6 +6034,14 @@ int PreferencesDialog::getSourceImageDecimation() const { return _ui->spinBox_source_imageDecimation->value(); } +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(); @@ -6030,9 +6205,9 @@ Camera * PreferencesDialog::createCamera( } ((CameraK4A*)camera)->setIRDepthFormat(_ui->checkbox_k4a_irDepth->isChecked()); - ((CameraK4A*)camera)->setPreferences(_ui->comboBox_k4a_rgb_resolution->currentIndex(), - _ui->comboBox_k4a_framerate->currentIndex(), - _ui->comboBox_k4a_depth_resolution->currentIndex()); + ((CameraK4A*)camera)->setPreferences(_ui->comboBox_k4a_rgb_resolution->currentIndex(), + _ui->comboBox_k4a_framerate->currentIndex(), + _ui->comboBox_k4a_depth_resolution->currentIndex()); } else if (driver == kSrcRealSense) { @@ -6300,8 +6475,22 @@ Camera * PreferencesDialog::createCamera( this->getGeneralInputRate(), this->getSourceLocalTransform()); ((CameraDepthAI*)camera)->setOutputDepth(_ui->checkBox_depthai_depth->isChecked(), _ui->spinBox_depthai_confidence->value()); - ((CameraDepthAI*)camera)->setIMUFirmwareUpdate(_ui->checkBox_depthai_imu_firmware_update->isChecked()); + ((CameraDepthAI*)camera)->setUseSpecTranslation(_ui->checkBox_depthai_use_spec_translation->isChecked()); + ((CameraDepthAI*)camera)->setAlphaScaling(_ui->doubleSpinBox_depthai_alpha_scaling->value()); ((CameraDepthAI*)camera)->setIMUPublished(_ui->checkBox_depthai_imu_published->isChecked()); + ((CameraDepthAI*)camera)->publishInterIMU(_ui->checkbox_publishInterIMU->isChecked()); + ((CameraDepthAI*)camera)->setLaserDotBrightness(_ui->doubleSpinBox_depthai_laser_dot_brightness->value()); + ((CameraDepthAI*)camera)->setFloodLightBrightness(_ui->doubleSpinBox_depthai_floodlight_brightness->value()); + ((CameraDepthAI*)camera)->setDetectFeatures(_ui->comboBox_depthai_detect_features->currentIndex()); + ((CameraDepthAI*)camera)->setBlobPath(_ui->lineEdit_depthai_blob_path->text().toStdString()); + if(_ui->comboBox_depthai_detect_features->currentIndex() == 1) + { + ((CameraDepthAI*)camera)->setGFTTDetector(_ui->checkBox_GFTT_useHarrisDetector->isChecked(), _ui->doubleSpinBox_GFTT_minDistance->value(), _ui->reextract_maxFeatures->value()); + } + else if(_ui->comboBox_depthai_detect_features->currentIndex() == 2) + { + ((CameraDepthAI*)camera)->setSuperPointDetector(_ui->doubleSpinBox_sptorch_threshold->value(), _ui->checkBox_sptorch_nms->isChecked(), _ui->spinBox_sptorch_minDistance->value()); + } } else if(driver == kSrcUsbDevice) { @@ -6365,7 +6554,10 @@ Camera * PreferencesDialog::createCamera( _ui->source_spinBox_databaseStopId->value(), !_ui->general_checkBox_createIntermediateNodes->isChecked(), _ui->source_checkBox_ignoreLandmarks->isChecked(), - _ui->source_checkBox_ignoreFeatures->isChecked()); + _ui->source_checkBox_ignoreFeatures->isChecked(), + 0, + -1, + _ui->source_checkBox_ignorePriors->isChecked()); } else { @@ -6597,35 +6789,28 @@ void PreferencesDialog::testOdometry() return; } + ParametersMap parameters = this->getAllParameters(); IMUThread * imuThread = 0; if((this->getSourceDriver() == kSrcStereoImages || this->getSourceDriver() == kSrcRGBDImages || this->getSourceDriver() == kSrcImages) && !_ui->lineEdit_cameraImages_path_imu->text().isEmpty()) { - if(this->getOdomStrategy() != Odometry::kTypeOkvis && - this->getOdomStrategy() != Odometry::kTypeMSCKF && - this->getOdomStrategy() != Odometry::kTypeVINS && - this->getOdomStrategy() != Odometry::kTypeOpenVINS) + imuThread = new IMUThread(_ui->spinBox_cameraImages_max_imu_rate->value(), this->getIMULocalTransform()); + if(getIMUFilteringStrategy()>0) { - QMessageBox::warning(this, tr("Source IMU Path"), - tr("IMU path is set but odometry chosen doesn't support asynchronous IMU, ignoring IMU..."), QMessageBox::Ok); + imuThread->enableIMUFiltering(getIMUFilteringStrategy()-1, parameters, getIMUFilteringBaseFrameConversion()); } - else + if(!imuThread->init(_ui->lineEdit_cameraImages_path_imu->text().toStdString())) { - imuThread = new IMUThread(_ui->spinBox_cameraImages_max_imu_rate->value(), this->getIMULocalTransform()); - if(!imuThread->init(_ui->lineEdit_cameraImages_path_imu->text().toStdString())) - { - QMessageBox::warning(this, tr("Source IMU Path"), - tr("Initialization of IMU data has failed! Path=%1.").arg(_ui->lineEdit_cameraImages_path_imu->text()), QMessageBox::Ok); - delete camera; - delete imuThread; - return; - } + QMessageBox::warning(this, tr("Source IMU Path"), + tr("Initialization of IMU data has failed! Path=%1.").arg(_ui->lineEdit_cameraImages_path_imu->text()), QMessageBox::Ok); + delete camera; + delete imuThread; + return; } } - ParametersMap parameters = this->getAllParameters(); if(getOdomRegistrationApproach() < 3) { uInsert(parameters, ParametersPair(Parameters::kRegStrategy(), uNumber2Str(getOdomRegistrationApproach()))); @@ -6661,6 +6846,11 @@ void PreferencesDialog::testOdometry() cameraThread.setMirroringEnabled(isSourceMirroring()); 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( @@ -6732,6 +6922,11 @@ void PreferencesDialog::testCamera() cameraThread.setMirroringEnabled(isSourceMirroring()); 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( @@ -7047,7 +7242,7 @@ void PreferencesDialog::calibrateOdomSensorExtrinsics() return; } - + // 3 steps calibration: RGB -> IR -> Extrinsic QMessageBox::StandardButton button = QMessageBox::question(this, tr("Calibration"), tr("We will calibrate the extrinsics. Important: Make sure " diff --git a/guilib/src/StatsToolBox.cpp b/guilib/src/StatsToolBox.cpp index bda8a167b9..ce0daf3fc8 100644 --- a/guilib/src/StatsToolBox.cpp +++ b/guilib/src/StatsToolBox.cpp @@ -170,7 +170,7 @@ void StatItem::setupUi(QGridLayout * grid) layout->addWidget(_value); layout->addWidget(_unit); layout->addStretch(); - layout->setMargin(0); + layout->setContentsMargins(0,0,0,0); } } @@ -220,7 +220,7 @@ StatsToolBox::StatsToolBox(QWidget * parent) : //Statistics in the GUI (for plotting) _statBox = new QToolBox(this); this->setLayout(new QVBoxLayout()); - this->layout()->setMargin(0); + this->layout()->setContentsMargins(0,0,0,0); this->layout()->addWidget(_statBox); _statBox->layout()->setSpacing(0); _plotMenu = new QMenu(this); diff --git a/guilib/src/opencv/vtkImageMatSource.cpp b/guilib/src/opencv/vtkImageMatSource.cpp index 24a91f347e..2914ffbe32 100644 --- a/guilib/src/opencv/vtkImageMatSource.cpp +++ b/guilib/src/opencv/vtkImageMatSource.cpp @@ -48,6 +48,7 @@ #include #include #include +#include namespace rtabmap { vtkStandardNewMacro(vtkImageMatSource); diff --git a/guilib/src/ui/DatabaseViewer.ui b/guilib/src/ui/DatabaseViewer.ui index 9d11f66886..a363256f0f 100644 --- a/guilib/src/ui/DatabaseViewer.ui +++ b/guilib/src/ui/DatabaseViewer.ui @@ -61,7 +61,7 @@ 0 0 - 293 + 307 380 @@ -355,7 +355,7 @@ 0 0 - 292 + 306 380 @@ -1176,119 +1176,116 @@ - + - + - # + 0.0 deg - + Qt::ClickFocus + + -1799 + + + 1800 + + + 0 + Qt::Horizontal QSlider::TicksAbove + + 100 + - + - Iterative optimization. Intermediate optimization graphes will be computed. Depending on the graph optimizer used, final optimization may not give the same results. + <html><head/><body><p>The rotation will be applied temporary to optimized global graph. To save it to database, do File-&gt;&quot;Regenerate optimized 2D map...&quot;.</p></body></html> - - - - true + Apply Rotation - - - - - - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - + + + - - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + # - - - - - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + Qt::ClickFocus - - - - - - - - - Poses + + Qt::Horizontal - - - - - - Time optimization (s) + + QSlider::TicksAbove - - - - Links (N, NM, G, LS, LT, U, P, LM, GR) + + + + QComboBox::AdjustToContents + + + Global Iterative + + + + + Global Full + + + + + Local Optimized + + - - + + + + + + - Path length (m) + Align poses with ground truth - - - - - - Time grid (s) + + true - + - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + false @@ -1320,7 +1317,31 @@ - + + + + Time grid (s) + + + + + + + RMSE (m) + + + + + + + + + + true + + + + @@ -1330,38 +1351,69 @@ - - + + - Align poses with ground truth + + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + Align scans/clouds with ground truth true - - + + - - false + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - + + - - true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - + + + + Links (N, NM, G, LS, LT, U, P, LM, GR) + + + + + + + Path length (m) + + + + + + + Poses + + + + + @@ -1370,7 +1422,20 @@ - + + + + + + + + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + Ignore intermediate nodes @@ -1380,25 +1445,35 @@ - - + + - RMSE (m) + Time optimization (s) - - + + - Align scans/clouds with ground truth + + + + true + + + + + + + Align poses with GPS true - - + + diff --git a/guilib/src/ui/aboutDialog.ui b/guilib/src/ui/aboutDialog.ui index 8b32ff9431..59ff675454 100644 --- a/guilib/src/ui/aboutDialog.ui +++ b/guilib/src/ui/aboutDialog.ui @@ -162,9 +162,9 @@ p, li { white-space: pre-wrap; } 0 - -636 + -438 596 - 981 + 1004 @@ -186,7 +186,7 @@ p, li { white-space: pre-wrap; } - + @@ -198,31 +198,31 @@ p, li { white-space: pre-wrap; } - - + + - Apache v2 and/or GPLv2 + + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter true - - + + - - - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + Apache-2 true - - + + @@ -234,134 +234,140 @@ p, li { white-space: pre-wrap; } - - + + - With libpointmatcher : + + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter true - - + + - MPL2 + + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter true - - + + - With loam_velodyne : + + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter true - - + + - With K4W2 : + + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter true - - + + - GPLv3 + With DVO : true - - + + - With DVO : + GPLv2 true - - + + - GPLv2 + MIT true - - + + - With Freenect : + With DepthAI : true - - + + - With K4A : + GPLv3 true - - + + - - - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + With Octomap : true - - + + - GPLv3 + With Freenect : true - - + + - <html><head/><body><p><span style=" font-weight:600;">Third Party Libraries</span></p></body></html> - - - Qt::RichText + With stereo dc1394 : true - - + + BSD @@ -370,18 +376,18 @@ p, li { white-space: pre-wrap; } - - + + - Open Source or Commercial + With Zed Open Capture : true - - + + @@ -393,21 +399,18 @@ p, li { white-space: pre-wrap; } - - + + - - - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + With loam_velodyne : true - - + + @@ -419,113 +422,104 @@ p, li { white-space: pre-wrap; } - - + + - - - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + MIT true - - + + - - - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + With AliceVision : true - - + + - - - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + Apache-2 true - - + + - + <html><head/><body><p><span style=" font-weight:600;">Third Party Libraries</span></p></body></html> - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + + Qt::RichText true - - + + - With CCCoreLib : + Creative Commons [Attribution-NonCommercial-ShareAlike] true - - + + - BSD + + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter true - - + + - MIT + With Freenect2 : true - - + + - Creative Commons [Attribution-NonCommercial-ShareAlike] + With g2o : true - - + + - - - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + PSF true - - + + @@ -537,8 +531,8 @@ p, li { white-space: pre-wrap; } - - + + @@ -550,10 +544,10 @@ p, li { white-space: pre-wrap; } - - + + - BSD + MIT true @@ -570,114 +564,117 @@ p, li { white-space: pre-wrap; } - - + + - - - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + With ORB SLAM 2 : true - - + + - With RealSense : + With MSCKF : true - - + + - With MYNTEYE S : + BSD true - - + + - BSD + With Zed SDK : true - - + + - - - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + Apache v2 true - - + + - With Octomap : + + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter true - - + + - With GTSAM : + + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter true - - + + - With Freenect2 : + + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter true - - + + - With stereo dc1394 : + With stereo FlyCapture2 : true - - + + - With OpenChisel : + PCL version : true - - + + BSD @@ -686,18 +683,18 @@ p, li { white-space: pre-wrap; } - - + + - PCL version : + BSD true - - + + @@ -709,8 +706,8 @@ p, li { white-space: pre-wrap; } - - + + @@ -722,8 +719,8 @@ p, li { white-space: pre-wrap; } - - + + @@ -735,81 +732,84 @@ p, li { white-space: pre-wrap; } - - + + - - - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + BSD true - - + + - With VINS-Fusion : + With Viso2 : true - - + + - With FOVIS : + Open Source or Commercial true - - + + - GPLv3 + With ORB Octree : true - - + + - With TORO : + + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter true - - + + - BSD + OpenCV version : true - - + + - With RealSense2 : + + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter true - - + + @@ -821,18 +821,28 @@ p, li { white-space: pre-wrap; } - - + + - LGPL + With GTSAM : true - - + + + + BSD + + + true + + + + + @@ -844,38 +854,41 @@ p, li { white-space: pre-wrap; } - - + + - With OpenNI2 : + GPLv2 true - - + + - With AliceVision : + + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter true - - + + - With Zed Open Capture : + Penn Software License true - - + + @@ -887,91 +900,104 @@ p, li { white-space: pre-wrap; } - - + + - With DepthAI : + + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter true - - + + - Apache v2 + + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter true - - + + - Apache-2 + With Ceres : true - - + + - BSD + With OpenNI2 : true - - + + - MIT + With SuperPoint Torch : true - - + + - + Qt version : - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + + true + + + + + + + GPLv3 true - - + + - With ORB SLAM 2 : + With FOVIS : true - - + + - With SuperPoint Torch : + With MYNTEYE S : true - - + + @@ -983,28 +1009,31 @@ p, li { white-space: pre-wrap; } - - + + - Qt version : + With TORO : true - - + + - With cvsba : + + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter true - - + + @@ -1016,51 +1045,61 @@ p, li { white-space: pre-wrap; } - - + + - + MPL2 - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + + true + + + + + + + <html><head/><body><p><span style=" font-weight:600;">License</span></p></body></html> true - - + + - Apache v2 and/or GPLv2 + GPLv3 true - - + + - Penn Software License + GPLv2 true - - + + - With ORB Octree : + + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter true - - + + GPLv3 @@ -1069,74 +1108,71 @@ p, li { white-space: pre-wrap; } - - + + - - - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + GPLv3 true - - + + - With FastCV : + LGPL true - - + + - With Zed SDK : + + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter true - - + + - BSD + With cvsba : true - - + + - OpenCV version : + GPLv3 true - - + + - - - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + With FastCV : true - - + + @@ -1148,8 +1184,8 @@ p, li { white-space: pre-wrap; } - - + + @@ -1161,81 +1197,78 @@ p, li { white-space: pre-wrap; } - - + + - GPLv2 + With Python3 : true - - + + - With MSCKF : + With K4W2 : true - - + + - - - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + With OpenChisel : true - - + + - GPLv3 + With RealSense2 : true - - + + - GPLv2 + Apache v2 and/or GPLv2 true - - + + - BSD + With OpenVINS : true - - + + - With stereo FlyCapture2 : + GPLv3 true - - + + @@ -1247,31 +1280,28 @@ p, li { white-space: pre-wrap; } - - + + - VTK version : + BSD true - - + + - - - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + With OKVIS : true - - + + BSD @@ -1280,91 +1310,97 @@ p, li { white-space: pre-wrap; } - - + + - GPLv3 + + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter true - - + + - With g2o : + + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter true - - + + - With OKVIS : + With CCCoreLib : true - - + + - - - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + With VINS-Fusion : true - - + + - PSF + + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter true - - + + - With Viso2 : + With K4A : true - - + + - <html><head/><body><p><span style=" font-weight:600;">License</span></p></body></html> + Apache v2 and/or GPLv2 true - - + + - MIT + With CPU-TSDF : true - - + + @@ -1376,18 +1412,18 @@ p, li { white-space: pre-wrap; } - - + + - Apache-2 + BSD true - - + + @@ -1399,41 +1435,41 @@ p, li { white-space: pre-wrap; } - - + + - BSD + + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter true - - + + - - - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + BSD true - - + + - With CPU-TSDF : + With RealSense : true - - + + @@ -1445,61 +1481,58 @@ p, li { white-space: pre-wrap; } - - + + - With Ceres : + With libpointmatcher : true - - + + - - - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + VTK version : true - - + + - With Python3 : + BSD true - - + + - GPLv3 + With MRPT : true - - + + - With OpenVINS : + BSD true - - + + diff --git a/guilib/src/ui/preferencesDialog.ui b/guilib/src/ui/preferencesDialog.ui index cbfd5fbe2f..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 @@ -64,8 +64,8 @@ 0 0 - 756 - 3657 + 713 + 3893 @@ -95,7 +95,7 @@ QFrame::Raised - 0 + 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 - - - @@ -3068,8 +3009,8 @@ 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 - - + + - Only RGB images (for RGB-D cameras) or left images (for stereo cameras) are published. + 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,8 +3162,80 @@ 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 + + + + + + + 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 + + + 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 + + + + + + + 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 + + + + + @@ -5240,6 +5301,11 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki ULTRA + + + NEURAL + + @@ -5400,15 +5466,30 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki - HD720 + HD1200 - VGA + HD720 - + + + SVGA + + + + + VGA + + + + + AUTO + + + @@ -5789,10 +5870,61 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki DepthAI - - + + + + 0 + + + QComboBox::AdjustToContents + + + + 720p + + + + + 800p + + + + + 400p + + + + + 480p + + + + + 1200p + + + + + + + + -1.000000000000000 + + + 1.000000000000000 + + + 1.000000000000000 + + + 0.000000000000000 + + + + + - Resolution. + Depth confidence. true @@ -5802,10 +5934,30 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki - - + + + + mA + + + 1200.000000000000000 + + + 0.000000000000000 + + + + + - Output depth. + + + + + + + + Floodlight brightness. true @@ -5815,10 +5967,36 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki - - + + - Depth confidence. + Laser dot brightness. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + Use the translation information from the board design data (not the calibration data). + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + Resolution. true @@ -5835,39 +6013,17 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki - - - - 0 + + + + Free scaling parameter between 0 (when all the pixels in the undistorted image are valid) and 1 (when all the source image pixels are retained in the undistorted image). On some high distortion lenses, and/or due to rectification (image rotated) invalid areas may appear even with alpha=0, in these cases alpha < 0.0 helps removing invalid areas. If alpha is set to -1, old rectification policy is used. - - QComboBox::AdjustToContents + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - 720p - - - - - 800p - - - - - 400p - - - - - 480p - - - - - 1200p - - @@ -5883,10 +6039,10 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki - - + + - IMU firmware update + IMU published true @@ -5896,17 +6052,17 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki - - + + - - + + - IMU published + Output depth. true @@ -5916,10 +6072,91 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki - - + + + + mA + + + 1500.000000000000000 + + + 200.000000000000000 + + + + + + + 0 + + + QComboBox::AdjustToContents + + + + None + + + + + GFTT + + + + + SuperPoint + + + + + + - + On-device feature detector. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + ... + + + + + + + + 100 + 0 + + + + + + + + + + + + + Path to SuperPoint blob file. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse @@ -6245,50 +6482,33 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki false - - - - Ignore odometry saved in the database, so if RGB-D SLAM is activated, odometry will be recomputed. - - - true + + + + 0 - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + 99999 - - - - ... - - - - - - - Use database stamps as input rate. - - - true + + + + 0 - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + 99999 - - - - - - + + - - + + - Camera index. If the database contains multi-camera data, you can choose which camera to use. -1 means that all cameras are streamed. + Ignore odometry saved in the database, so if RGB-D SLAM is activated, odometry will be recomputed. true @@ -6298,50 +6518,46 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki - - + + - - + + - + Ignore goals saved in the database. - - - - - - Start position (node ID). + + true Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - + + + + Qt::Vertical + + + + 20 + 0 + + + - - + + - - - - 0 - - - 99999 - - - @@ -6356,34 +6572,53 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki - - + + - - - - 0 + + + + Ignore goal delay. - - 99999 + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - -1 + + + + Ignore features. - - 9999 + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - + + + + Use database stamps as input rate. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + Stop position (node ID) is the last node to process. If 0, all nodes after start position are published. @@ -6396,10 +6631,10 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki - - + + - Ignore landmarks. + Camera index. If the database contains multi-camera data, you can choose which camera to use. -1 means that all cameras are streamed. true @@ -6409,6 +6644,27 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki + + + + + + + + + + + ... + + + + + + + + + + @@ -6416,10 +6672,10 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki - - + + - Ignore goal delay. + Ignore landmarks. true @@ -6429,20 +6685,34 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki - - - - Ignore goals saved in the database. + + + + -1 - - true + + 9999 + + + + + + + Start position (node ID). Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - + + + + + + + + If the database contains stereo data, generate disparity image and convert it to depth. The resulting output is a RGB-D image instead of stereo images. Dense disparity parameters can be found under StereoBM tab. @@ -6455,23 +6725,10 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki - - - - Qt::Vertical - - - - 20 - 0 - - - - - - + + - Ignore features. + Ignore priors. true @@ -6481,8 +6738,8 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki - - + + @@ -11061,10 +11318,10 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag Map Update - - + + - Use Identity matrix as guess when computing loop closure transform, otherwise no guess is used (assuming that registration strategy can deal with transformation estimation without guess). + Linear update: Minimum linear displacement to update the map. Note that Weight Update is done prior to this, so weights are still updated. true @@ -11074,20 +11331,10 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - - - m - - - 1.000000000000000 - - - - - + + - Angular update: Minimum angular displacement to update the map. Note that Weight Update is done prior to this, so weights are still updated. + Ratio of working memory for which local nodes are immunized from transfer. true @@ -11097,33 +11344,36 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - + + - rad + m - 2 - - - 3.140000000000000 + 3 0.100000000000000 - - + + - + Localization smoothing. Used only in localization mode. Adjust localization constraints based on optimized odometry cache poses. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - + + - Local radius for nodes selection in the local map. This parameter is used in some approaches of the sub-panels. + Reject loop closures/localizations if the distance from the map is over this distance (0=disabled). true @@ -11133,22 +11383,83 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - - + + + + + + + + + + + + + + + + m + + + 1.000000000000000 + + + + + + + 9999 + + + + + + + rad + 2 - 1.000000000000000 + 3.140000000000000 0.100000000000000 - - 0.100000000000000 + + + + + + Local radius for nodes selection in the local map. This parameter is used in some approaches of the sub-panels. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + Odometry change detected that triggers a new map (0 means whatever the odometry change, the detector will still link the new pose in the current map). Also by default, when an odometry with Identity transformation is detected, a new map is automatically created. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + @@ -11178,10 +11489,10 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - + + - Linear update: Minimum linear displacement to update the map. Note that Weight Update is done prior to this, so weights are still updated. + Limit covariance of non-neighbor links to minimum covariance of neighbor links. In other words, if covariance of a loop closure link is smaller than the minimum covariance of odometry links, its covariance is set to minimum covariance of odometry links. true @@ -11191,10 +11502,20 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - + + + + m + + + 1.000000000000000 + + + + + - Maximum linear speed to update the map (0 means not limit). + Do local bundle adjustment with neighborhood of the loop closure. true @@ -11204,12 +11525,8 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - - - - - + + @@ -11224,23 +11541,10 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - - - m - - - 3 - - - 0.100000000000000 - - - - - + + - Odometry change detected that triggers a new map (0 means whatever the odometry change, the detector will still link the new pose in the current map). Also by default, when an odometry with Identity transformation is detected, a new map is automatically created. + Maximum linear speed to update the map (0 means not limit). true @@ -11250,27 +11554,17 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - - - 9999 - - - - - - - m - - - 1.000000000000000 + + + + - - + + - Ignore off diagonal values of the odometry covariance matrix. + Use Identity matrix as guess when computing loop closure transform, otherwise no guess is used (assuming that registration strategy can deal with transformation estimation without guess). true @@ -11280,23 +11574,17 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - + + - Limit covariance of non-neighbor links to minimum covariance of neighbor links. In other words, if covariance of a loop closure link is smaller than the minimum covariance of odometry links, its covariance is set to minimum covariance of odometry links. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + - - + + - Use odometry instead of IMU orientation to add gravity links to new nodes created. We assume that odometry is already aligned with gravity (e.g., we are using a VIO approach). Gravity constraints are used by graph optimization only if "Optimizer/GravitySigma" is not zero. + Re-extract visual features when computing loop closure transformations. Raw features are not saved in database. true @@ -11306,10 +11594,10 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - + + - Re-extract visual features when computing loop closure transformations. Raw features are not saved in database. + Inverted registration. On loop closure, do registration from the target to reference instead of reference to target. true @@ -11335,8 +11623,21 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - + + + + Neighbor link refining. When a new node is added to the graph, the transformation of its neighbor link (odometry) with the previous node is refined using ICP registration approach (laser scans required). + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + @@ -11355,26 +11656,29 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - - - - - - Do local bundle adjustment with neighborhood of the loop closure. + + + + - - true + + 2 - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + 1.000000000000000 + + + 0.100000000000000 + + + 0.100000000000000 - - + + - If true, rtabmap will assume the robot is restarting from origin of the map. If false, rtabmap will assume the robot is restarting from the last saved localization pose from previous session (the place where it shut down previously). Used only in localization mode. + Use odometry instead of IMU orientation to add gravity links to new nodes created. We assume that odometry is already aligned with gravity (e.g., we are using a VIO approach). Gravity constraints are used by graph optimization only if "Optimizer/GravitySigma" is not zero. true @@ -11384,24 +11688,10 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - - - - - - - - - - - - - - - + + - Reject loop closures/localizations if the distance from the map is over this distance (0=disabled). + Ignore off diagonal values of the odometry covariance matrix. true @@ -11424,13 +11714,6 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - - - - - - @@ -11444,30 +11727,24 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - + + - Ratio of working memory for which local nodes are immunized from transfer. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + - - + + - - + + - Neighbor link refining. When a new node is added to the graph, the transformation of its neighbor link (odometry) with the previous node is refined using ICP registration approach (laser scans required). + Angular update: Minimum angular displacement to update the map. Note that Weight Update is done prior to this, so weights are still updated. true @@ -11477,10 +11754,17 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - + + - Inverted registration. On loop closure, do registration from the target to reference instead of reference to target. + + + + + + + + If true, rtabmap will assume the robot is restarting from origin of the map. If false, rtabmap will assume the robot is restarting from the last saved localization pose from previous session (the place where it shut down previously). Used only in localization mode. true @@ -11490,10 +11774,35 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - + + - + Localization prior error. Used only in localization mode. The corresponding variance (error x error) set to priors of the map's poses during localization. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + m + + + 4 + + + 1.000000000000000 + + + 0.010000000000000 + + + 0.010000000000000 @@ -14024,7 +14333,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag Odometry - + @@ -14465,7 +14774,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - 12 + 5 @@ -14781,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 + + + + @@ -14865,7 +15174,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag [Visual] Correspondences computation: Optical Flow - + @@ -14882,10 +15191,10 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - + + - Window size. + Iterations. true @@ -14895,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 @@ -14937,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 - - - - @@ -15009,7 +15305,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag 20 - 40 + 0 @@ -16368,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 @@ -16403,32 +16702,10 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - - - - - - 1 - - - 0.100000000000000 - - - 999.000000000000000 - - - 1.000000000000000 - - - 40.000000000000000 - - - - - + + - Fake IR projector baseline used only when stereo is not used. + Maximum size of the feature map (0 means infinite). Only supported with ORB_SLAM2. true @@ -16438,13 +16715,10 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - - - - + + - Path to ORB vocabulary (*.txt). + Maximum ORB features extracted per frame. true @@ -16461,10 +16735,10 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - + + - Camera FPS. This parameter is linked to "Input rate" of the Source panel. + Close/Far threshold. Baseline times. true @@ -16474,35 +16748,23 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - - - false - - - - - - 2 - - - 0.000000000000000 - + + - 999.000000000000000 - - - 1.000000000000000 + 99999 - 0.000000000000000 + 1000 - - + + + + + - Maximum ORB features extracted per frame. + Camera FPS. This parameter is linked to "Input rate" of the Source panel. true @@ -16512,20 +16774,10 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - - - 99999 - - - 1000 - - - - - + + - Maximum size of the feature map (0 means infinite). + Path to ORB vocabulary (*.txt). true @@ -16535,6 +16787,50 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag + + + + m + + + 4 + + + 0.000000000000000 + + + 9.999900000000000 + + + 0.010000000000000 + + + 0.076000000000000 + + + + + + + + + + 1 + + + 0.100000000000000 + + + 999.000000000000000 + + + 1.000000000000000 + + + 40.000000000000000 + + + @@ -16547,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 @@ -16871,7 +17328,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - <html><head/><body><p>MSCKF_VIO: <a href="https://github.com/KumarRobotics/msckf_vio"><span style=" text-decoration: underline; color:#0000ff;">https://github.com/KumarRobotics/msckf_vio</span></a></p><p>Stereo and IMU inputs required. Currently tested only with EuRoC data set.</p></body></html> + <html><head/><body><p>MSCKF_VIO: <a href="https://github.com/KumarRobotics/msckf_vio"><span style=" text-decoration: underline; color:#0000ff;">https://github.com/KumarRobotics/msckf_vio</span></a></p><p>Stereo and IMU inputs required.</p></body></html> true @@ -17680,11 +18137,11 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag OpenVINS - + - <html><head/><body><p>OpenVINS: <a href="https://github.com/rpng/open_vins"><span style=" text-decoration: underline; color:#0000ff;">https://github.com/rpng/open_vins</span></a></p><p>Currently tested only with EuRoC data set.</p></body></html> + <html><head/><body><p>OpenVINS: <a href="https://github.com/rpng/open_vins"><span style=" text-decoration: underline; color:#0000ff;">https://github.com/rpng/open_vins</span></a></p></body></html> true @@ -17697,191 +18154,1711 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - - - - - - Qt::Vertical - - - - 20 - 1049 - - - - - - - - - - - - Dummy-FLOAM - - - - - - - - - - - - Open3D - - - - - <html><head/><body><p>Open3D: <a href="https://github.com/isl-org/Open3D"><span style=" text-decoration: underline; color:#0000ff;">https://github.com/isl-org/Open3D</span></a></p><p>Working only with RGB-D cameras, preferably with TOF sensors. Note that visual keyframe threshold ratio parameter above is used with this odometry strategy.</p></body></html> - - - true - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + Tracker - - - - - - - - 0.000000000000000 - - - 9999.000000000000000 - - - 1.000000000000000 - - - 3.000000000000000 - - - - - - - Method. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - QComboBox::AdjustToContents - - + + + - PointToPlane + - - + + + + - Intensity + If we should process two cameras are being stereo or binocular. If binocular, we do monocular feature tracking on each image. - - + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + - Hybrid + - - - - - - - Max depth. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - - - - - - Qt::Vertical - - - - 20 - 984 - - - - - - - - - - - - Mono - - - - - - Mono is for single camera motion estimation (MonoSLAM). On initialization, the camera must be translated on the side until a first transform can be computed. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - Qt::Vertical - - - - 20 - 0 - - - - - - - - Parameters from BOW and OpticalFlow are also used here. PnP parameters on Odometry panel are used too. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - + + + + + + If we should use KLT tracking, or descriptor matcher + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + 0 + + + 10000 + + + 200 + + + + + + + Number of points (per camera) we will extract and try to track + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + 1 + + + 15 + + + + + + + Will check after doing KLT track and remove any features closer than this + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + + + + + + If we should perform 1d triangulation instead of 3d + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + + + + + + If we should perform Levenberg-Marquardt refinement + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + 1 + + + 5 + + + + + + + Max runs for Levenberg-Marquardt + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + 1000.000000000000000 + + + 10.000000000000000 + + + 40.000000000000000 + + + + + + + Max baseline ratio to accept triangulated features + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + 99999.000000000000000 + + + 1000.000000000000000 + + + 10000.000000000000000 + + + + + + + Max condition number of linear triangulation matrix accept triangulated features + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + + State + + + + + + + + + + + + + If first-estimate Jacobians should be used (enable for good consistency) + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + 1 + + + QComboBox::AdjustToContents + + + + DISCRETE + + + + + RK4 + + + + + ANALYTICAL + + + + + + + + Numerical integration methods + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + + + + + + If the transform between camera and IMU should be optimized (R_ItoC, p_CinI) + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + + + + + + If camera intrinsics should be optimized (focal, center, distortion) + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + + + + + + If timeoffset between camera and IMU should be optimized + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + + + + + + If imu intrinsics should be calibrated (rotation and skew-scale matrix) + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + + + + + + If gyroscope gravity sensitivity (Tg) should be calibrated + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + 1 + + + 11 + + + + + + + Max clone size of sliding window + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + 0 + + + 10000 + + + 50 + + + + + + + Max number of estimated SLAM features + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + 1 + + + 10000 + + + 25 + + + + + + + Max number of SLAM features we allow to be included in a single EKF update. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + 0 + + + 10000 + + + 50 + + + + + + + Max number of MSCKF features we will use at a given image timestep. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + 0 + + + QComboBox::AdjustToContents + + + + GLOBAL_3D + + + + + GLOBAL_FULL_INVERSE_DEPTH + + + + + ANCHORED_3D + + + + + ANCHORED_FULL_INVERSE_DEPTH + + + + + ANCHORED_MSCKF_INVERSE_DEPTH + + + + + ANCHORED_INVERSE_DEPTH_SINGLE + + + + + + + + What representation our features are in (msckf features) + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + 4 + + + QComboBox::AdjustToContents + + + + GLOBAL_3D + + + + + GLOBAL_FULL_INVERSE_DEPTH + + + + + ANCHORED_3D + + + + + ANCHORED_FULL_INVERSE_DEPTH + + + + + ANCHORED_MSCKF_INVERSE_DEPTH + + + + + ANCHORED_INVERSE_DEPTH_SINGLE + + + + + + + + What representation our features are in (slam features) + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + s + + + 0.000000000000000 + + + 0.500000000000000 + + + 0.000000000000000 + + + + + + + Delay before initializing (helps with stability from bad initialization...) + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + 5 + + + 0.010000000000000 + + + 9.810000000000000 + + + + + + + Magnitude of gravity in this location + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + ... + + + + + + + + + + + + Mask for left image + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + ... + + + + + + + + + + + + Mask for right image + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + + Initializer + + + + + + s + + + 0.000000000000000 + + + 0.500000000000000 + + + 2.000000000000000 + + + + + + + Amount of time we will initialize over + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + 0.000000000000000 + + + 10.000000000000000 + + + 0.100000000000000 + + + 1.000000000000000 + + + + + + + Variance threshold on our acceleration to be classified as moving + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + 0.000000000000000 + + + 0.100000000000000 + + + 10.000000000000000 + + + + + + + Max disparity to consider the platform stationary (dependent on resolution) + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + 0 + + + 10000 + + + 50 + + + + + + + How many features to track during initialization (saves on computation) + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + + + + + + If we should perform dynamic initialization + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + + + + + + If we should optimize and recover the calibration in our MLE + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + 0 + + + 50 + + + + + + + Max number of MLE iterations for dynamic initialization + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + s + + + 0.000000000000000 + + + 0.010000000000000 + + + 0.050000000000000 + + + + + + + Max time for MLE optimization + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + 1 + + + 6 + + + + + + + Max number of MLE threads for dynamic initialization + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + 4 + + + 6 + + + + + + + Number of poses to use during initialization (max should be cam freq * window) + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + deg + + + 0.000000000000000 + + + 45.000000000000000 + + + 10.000000000000000 + + + + + + + Minimum degrees we need to rotate before we try to init (sum of norm) + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + 1.000000000000000 + + + 999.000000000000000 + + + 10.000000000000000 + + + + + + + Magnitude we will inflate initial covariance of orientation + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + 1.000000000000000 + + + 999.000000000000000 + + + 100.000000000000000 + + + + + + + Magnitude we will inflate initial covariance of velocity + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + 1.000000000000000 + + + 999.000000000000000 + + + 10.000000000000000 + + + + + + + Magnitude we will inflate initial covariance of gyroscope bias + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + 1.000000000000000 + + + 999.000000000000000 + + + 100.000000000000000 + + + + + + + Magnitude we will inflate initial covariance of accelerometer bias + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + 0.000000000000000 + + + 1.000000000000000 + + + 0.000000000001000 + + + 0.000000000000000 + + + + + + + Minimum reciprocal condition number acceptable for our covariance recovery + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + + ZUPT + + + + + + + + + + + + + If we should try to use zero velocity update + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + 0.000000000000000 + + + 10.000000000000000 + + + 0.100000000000000 + + + 0.000000000000000 + + + + + + + Chi2 multiplier for zero velocity + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + 0.000000000000000 + + + 1.000000000000000 + + + 0.100000000000000 + + + 0.100000000000000 + + + + + + + Max velocity we will consider to try to do a zupt (i.e. if above this, don't do zupt) + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + 0.000000000000000 + + + 100.000000000000000 + + + 1.000000000000000 + + + 10.000000000000000 + + + + + + + Multiplier of our zupt measurement IMU noise matrix (default should be 1.0) + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + 0.000000000000000 + + + 10.000000000000000 + + + 0.100000000000000 + + + 0.500000000000000 + + + + + + + Max disparity we will consider to try to do a zupt (i.e. if above this, don't do zupt) + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + + + + + + If we should only use the zupt at the very beginning static initialization phase + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + + Noise / Chi2 + + + + + + m/s^2/sqrt(Hz) + + + 6 + + + 1.000000000000000 + + + 0.000100000000000 + + + 0.010000000000000 + + + + + + + Accel "white noise" + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + m/s^3/sqrt(Hz) + + + 6 + + + 1.000000000000000 + + + 0.000100000000000 + + + 0.001000000000000 + + + + + + + Accel bias diffusion + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + rad/s/sqrt(Hz) + + + 6 + + + 1.000000000000000 + + + 0.000100000000000 + + + 0.001000000000000 + + + + + + + Gyro "white noise" + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + rad/s^2/sqrt(Hz) + + + 6 + + + 1.000000000000000 + + + 0.000100000000000 + + + 0.000100000000000 + + + + + + + Gyro bias diffusion + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + 0.000000000000000 + + + 10.000000000000000 + + + 0.100000000000000 + + + 2.000000000000000 + + + + + + + Pixel noise for MSCKF features + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + 0.000000000000000 + + + 10.000000000000000 + + + 0.100000000000000 + + + 1.000000000000000 + + + + + + + Chi2 multiplier for MSCKF features + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + 0.000000000000000 + + + 10.000000000000000 + + + 0.100000000000000 + + + 2.000000000000000 + + + + + + + Pixel noise for SLAM features + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + 0.000000000000000 + + + 10.000000000000000 + + + 0.100000000000000 + + + 1.000000000000000 + + + + + + + Chi2 multiplier for SLAM features + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + + + + + Qt::Vertical + + + + 20 + 0 + + + + + + + + + + + + Dummy-FLOAM + + + + + + + + + + + + Open3D + + + + + + <html><head/><body><p>Open3D: <a href="https://github.com/isl-org/Open3D"><span style=" text-decoration: underline; color:#0000ff;">https://github.com/isl-org/Open3D</span></a></p><p>Working only with RGB-D cameras, preferably with TOF sensors. Note that visual keyframe threshold ratio parameter above is used with this odometry strategy.</p></body></html> + + + true + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + 0.000000000000000 + + + 9999.000000000000000 + + + 1.000000000000000 + + + 3.000000000000000 + + + + + + + Method. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + QComboBox::AdjustToContents + + + + PointToPlane + + + + + Intensity + + + + + Hybrid + + + + + + + + Max depth. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + + + + Qt::Vertical + + + + 20 + 984 + + + + + + + + + + + + Mono + + + + + + Mono is for single camera motion estimation (MonoSLAM). On initialization, the camera must be translated on the side until a first transform can be computed. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + Parameters from BOW and OpticalFlow are also used here. PnP parameters on Odometry panel are used too. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -18010,6 +19987,19 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag + + + + Qt::Vertical + + + + 20 + 0 + + + + @@ -18020,7 +20010,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - 0 + 2 @@ -18336,16 +20326,6 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - - - - - - true - - - @@ -19239,10 +21219,10 @@ Lower the ratio -> higher the precision. Motion Estimation: 3D to 2D (PnP) - - + + - Refine iterations. + Flags. true @@ -19252,6 +21232,25 @@ Lower the ratio -> higher the precision. + + + + + AUTO + + + + + ANY + + + + + HOMOGENEOUS + + + + @@ -19271,8 +21270,21 @@ Lower the ratio -> higher the precision. + + + + Refine iterations. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + - + Reprojection error. @@ -19284,6 +21296,44 @@ Lower the ratio -> higher the precision. + + + + 0 + + + 999999 + + + 1 + + + 1 + + + + + + + + + + 4 + + + 0.000000000000000 + + + 99.000000000000000 + + + 0.100000000000000 + + + 0.000000000000000 + + + @@ -19303,26 +21353,23 @@ Lower the ratio -> higher the precision. - - - - 0 - - - 999999 + + + + Multi-camera random sampling policy. With HOMOGENEOUS policy, RANSAC will be done uniformly against all cameras, so at least 2 matches per camera are required. With ANY policy, RANSAC is not constraint to sample on all cameras at the same time. AUTO policy will use HOMOGENEOUS if there are at least 2 matches per camera, otherwise it will fallback to ANY policy. - - 1 + + true - - 1 + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - + + - Flags. + Reprojection error. true @@ -19332,7 +21379,7 @@ Lower the ratio -> higher the precision. - + Max linear variance between 3D point correspondences after PnP. 0 means disabled. @@ -19345,25 +21392,29 @@ Lower the ratio -> higher the precision. - - - - + + + + Ratio used to compute variance of the estimated transformation if 3D correspondences are provided (should be > 1). The higher it is, the smaller the covariance will be. With accurate depth estimation, this could be set to 2. For depth estimated by stereo, 4 or more maybe used to ignore large errors of very far points. - - 4 + + true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + - 0.000000000000000 + 2 - 99.000000000000000 - - - 0.100000000000000 + 999999 - 0.000000000000000 + 4 @@ -19649,7 +21700,7 @@ Lower the ratio -> higher the precision. 0.000000000000000 - 5.000000000000000 + 60.000000000000000 @@ -19672,7 +21723,7 @@ Lower the ratio -> higher the precision. 0.000000000000000 - 5.000000000000000 + 0.100000000000000 @@ -19933,45 +21984,45 @@ Lower the ratio -> higher the precision. - - + + + + 3 + + + 1.000000000000000 + + + 0.010000000000000 + + + + + m - 6 + 2 0.000000000000000 - 1.000000000000000 + 999.000000000000000 - 0.010000000000000 + 1.000000000000000 0.000000000000000 - - - - Outlier ratio used when ICP strategy is not PCL. For libpointmatcher, this parameter set TrimmedDistOutlierFilter / ratio for convenience when configuration file is not set. For CCCoreLib, this parameter set the "finalOverlapRatio". - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - + + - Stuctural complexity strategy. If structural complexity is below the minimum complexity threshold: set to 0 to so that the transform is automatically rejected, set to 1 to limit ICP correction in axes with most constraints (e.g., for a corridor-like environment, the resulting transform will be limited in y and yaw, x will taken from the guess), set to 2 to accept "as is" the transform computed by PointToPoint. + Max iterations. true @@ -19981,36 +22032,45 @@ Lower the ratio -> higher the precision. - - - - Maximum ICP rotation correction accepted (>0). A large rotation difference between the visual transformation and ICP transformation results in wrong transformations in most cases. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - + + + + + PCL (Point Cloud Library) + + + + + libpointmatcher + + + + + CCCoreLib (CloudCompare) + + - - + + 1 - 1000 + 999999 + + + 1 - 20 + 1 - - + + - Minimum structural complexity (0.0=low, 1.0=high) of the scan to do PointToPlane registration, otherwise PointToPoint registration is done instead and strategy from %s is used. This check is done only when PointToPlane is enabled. + Outlier ratio used when ICP strategy is not PCL. For libpointmatcher, this parameter set TrimmedDistOutlierFilter / ratio for convenience when configuration file is not set. For CCCoreLib, this parameter set the "finalOverlapRatio". true @@ -20020,10 +22080,10 @@ Lower the ratio -> higher the precision. - - + + - Uniform sampling voxel size. Set to 0 to disable. + Export scans used for ICP in the specified format (a warning on terminal will be shown with the file paths used). Supported formats are "pcd", "ply" or "vtk". If logger level is debug, from and to scans will stamped, so previous files won't be overwritten. true @@ -20033,30 +22093,10 @@ Lower the ratio -> higher the precision. - - - - 1 - - - 1000 - - - 10 - - - - - - - 0.010000000000000 - - - - - + + - Ratio of matching correspondences to accept the transform. If the maximum laser scans is set, this is the minimum ratio of correspondences on laser scan maximum size to accept the transform. + Max distance for point correspondences. true @@ -20066,10 +22106,17 @@ Lower the ratio -> higher the precision. - - + + - Strategy. + + + + + + + + Maximum ICP rotation correction accepted (>0). A large rotation difference between the visual transformation and ICP transformation results in wrong transformations in most cases. true @@ -20079,62 +22126,58 @@ Lower the ratio -> higher the precision. - - - - Maximum ICP translation correction accepted (>0). A large translation difference between the visual transformation and ICP transformation results in wrong transformations in most cases. + + + + 1 - - true + + 1000 - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + 20 - - - - 3 + + + + + + + m - - 0.001000000000000 + + 2 - 9999.989999999999782 + 10.000000000000000 0.010000000000000 - 0.100000000000000 + 0.200000000000000 - - - - Minimum range filtering. Set to 0 to disable. - - - true + + + + 1 - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + 1000 - - - - - - + + 10 - - + + - Maximum range filtering. Set to 0 to disable. + Minimum structural complexity (0.0=low, 1.0=high) of the scan to do PointToPlane registration, otherwise PointToPoint registration is done instead and strategy from %s is used. This check is done only when PointToPlane is enabled. true @@ -20144,39 +22187,23 @@ Lower the ratio -> higher the precision. - - - - 0.000000000000000 - - - 1.000000000000000 - - - 0.010000000000000 - - - 0.700000000000000 - - - - - - - 2 + + + + Downsampling step of the cloud. Set to 1 to disable. This is done before uniform sampling. - - 1.000000000000000 + + true - - 0.010000000000000 + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - + + - Invert normals on ground if they are pointing down (useful for ring-like 3D LiDARs). 0 means disabled, 1 means only normals perfectly aligned with -z axis. This is only done with 3D scans. + Strategy. true @@ -20186,48 +22213,77 @@ Lower the ratio -> higher the precision. - - + + m - 2 + 3 0.000000000000000 - 1.000000000000000 + 0.010000000000000 - 0.000000000000000 + 0.005000000000000 - - + + m - 2 + 6 + + + 0.000000000000000 - 10.000000000000000 + 1.000000000000000 0.010000000000000 - 0.200000000000000 + 0.000000000000000 + + + + + + + - - + + + + + Reject + + + + + Constrained PointToPoint + + + + + PointToPoint + + + + + + - Force 4 DoF: Limit ICP to x, y, z and yaw DoF. Available only with libpointmatcher or CCCoreLib strategies. + Search radius to compute normals for point to plane. Normals won't be recomputed if uniform sampling is disabled and that there are already normals in the laser scans. true @@ -20237,32 +22293,26 @@ Lower the ratio -> higher the precision. - - - - - - - 2 - + + - 0.010000000000000 + 0.000000000000000 1.000000000000000 - 0.100000000000000 + 0.010000000000000 - 0.850000000000000 + 0.700000000000000 - - + + - Number of neighbors to compute normals for point to plane. Normals won't be recomputed if uniform sampling is disabled and that there are already normals in the laser scans. + Force 4 DoF: Limit ICP to x, y, z and yaw DoF. Available only with libpointmatcher or CCCoreLib strategies. true @@ -20272,10 +22322,10 @@ Lower the ratio -> higher the precision. - - + + - Search radius to compute normals for point to plane. Normals won't be recomputed if uniform sampling is disabled and that there are already normals in the laser scans. + Stuctural complexity strategy. If structural complexity is below the minimum complexity threshold: set to 0 to so that the transform is automatically rejected, set to 1 to limit ICP correction in axes with most constraints (e.g., for a corridor-like environment, the resulting transform will be limited in y and yaw, x will taken from the guess), set to 2 to accept "as is" the transform computed by PointToPoint. true @@ -20285,29 +22335,23 @@ Lower the ratio -> higher the precision. - - - - m - + + - 3 + 2 - - 0.000000000000000 + + 1.000000000000000 0.010000000000000 - - 0.005000000000000 - - - + + - Downsampling step of the cloud. Set to 1 to disable. This is done before uniform sampling. + Set the transformation epsilon (maximum allowable difference between two consecutive transformations) in order for an optimization to be considered as having converged to the final solution. true @@ -20317,55 +22361,36 @@ Lower the ratio -> higher the precision. - - - - - Reject - - - - - Constrained PointToPoint - - - - - PointToPoint - - - - - - + + - + Point to plane ICP. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - PCL (Point Cloud Library) - - - - - libpointmatcher - - - - - CCCoreLib (CloudCompare) - - + + + + Number of neighbors to compute normals for point to plane. Normals won't be recomputed if uniform sampling is disabled and that there are already normals in the laser scans. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + - - + + - Set the transformation epsilon (maximum allowable difference between two consecutive transformations) in order for an optimization to be considered as having converged to the final solution. + Ratio of matching correspondences to accept the transform. If the maximum laser scans is set, this is the minimum ratio of correspondences on laser scan maximum size to accept the transform. true @@ -20375,10 +22400,29 @@ Lower the ratio -> higher the precision. - - + + + + rad + + + 2 + + + 3.140000000000000 + + + 0.010000000000000 + + + 0.780000000000000 + + + + + - Point to plane ICP. + Minimum range filtering. Set to 0 to disable. true @@ -20388,10 +22432,10 @@ Lower the ratio -> higher the precision. - - + + - Max iterations. + Maximum ICP translation correction accepted (>0). A large translation difference between the visual transformation and ICP transformation results in wrong transformations in most cases. true @@ -20401,23 +22445,32 @@ Lower the ratio -> higher the precision. - - + + + + + - 3 + 2 + + + 0.010000000000000 1.000000000000000 - 0.010000000000000 + 0.100000000000000 + + + 0.850000000000000 - - + + - Max distance for point correspondences. + Invert normals on ground if they are pointing down (useful for ring-like 3D LiDARs). 0 means disabled, 1 means only normals perfectly aligned with -z axis. This is only done with 3D scans. true @@ -20427,67 +22480,81 @@ Lower the ratio -> higher the precision. - - - - 1 - - - 999999 + + + + Maximum range filtering. Set to 0 to disable. - - 1 + + true - - 1 + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - + + - rad + m 2 - - 3.140000000000000 + + 0.000000000000000 - 0.010000000000000 + 1.000000000000000 - 0.780000000000000 + 0.000000000000000 - - - - m + + + + Uniform sampling voxel size. Set to 0 to disable. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + 0.010000000000000 + + + + + - 2 + 3 - 0.000000000000000 + 0.001000000000000 - 999.000000000000000 + 9999.989999999999782 - 1.000000000000000 + 0.010000000000000 - 0.000000000000000 + 0.100000000000000 - - + + - Export scans used for ICP in the specified format (a warning on terminal will be shown with the file paths used). Supported formats are "pcd", "ply" or "vtk". If logger level is debug, from and to scans will stamped, so previous files won't be overwritten. + Reciprocal correspondences. To be a valid correspondence, the corresponding point in target cloud to point in source cloud should be both their closest closest correspondence. true @@ -20497,8 +22564,12 @@ Lower the ratio -> higher the precision. - - + + + + + + @@ -23824,7 +25895,7 @@ Lower the ratio -> higher the precision. - + diff --git a/guilib/src/utilite/UPlot.cpp b/guilib/src/utilite/UPlot.cpp index 5928d7e3f7..26f275d35c 100644 --- a/guilib/src/utilite/UPlot.cpp +++ b/guilib/src/utilite/UPlot.cpp @@ -37,6 +37,7 @@ #include #include #include +#include #include #include #include @@ -788,28 +789,44 @@ void UPlotCurve::draw(QPainter * painter, const QRect & limits) { QPointF intersection; QLineF::IntersectType type; +#if QT_VERSION >= QT_VERSION_CHECK(6, 0, 0) + type = lineItem->line().intersects(QLineF(limits.topLeft(), limits.bottomLeft()), &intersection); +#else type = lineItem->line().intersect(QLineF(limits.topLeft(), limits.bottomLeft()), &intersection); +#endif if(type == QLineF::BoundedIntersection) { !limits.contains(line.p1())?line.setP1(intersection.toPoint()):line.setP2(intersection.toPoint()); } else { +#if QT_VERSION >= QT_VERSION_CHECK(6, 0, 0) + type = lineItem->line().intersects(QLineF(limits.topLeft(), limits.topRight()), &intersection); +#else type = lineItem->line().intersect(QLineF(limits.topLeft(), limits.topRight()), &intersection); +#endif if(type == QLineF::BoundedIntersection) { !limits.contains(line.p1())?line.setP1(intersection.toPoint()):line.setP2(intersection.toPoint()); } else { +#if QT_VERSION >= QT_VERSION_CHECK(6, 0, 0) + type = lineItem->line().intersects(QLineF(limits.bottomLeft(), limits.bottomRight()), &intersection); +#else type = lineItem->line().intersect(QLineF(limits.bottomLeft(), limits.bottomRight()), &intersection); +#endif if(type == QLineF::BoundedIntersection) { !limits.contains(line.p1())?line.setP1(intersection.toPoint()):line.setP2(intersection.toPoint()); } else { +#if QT_VERSION >= QT_VERSION_CHECK(6, 0, 0) + type = lineItem->line().intersects(QLineF(limits.topRight(), limits.bottomRight()), &intersection); +#else type = lineItem->line().intersect(QLineF(limits.topRight(), limits.bottomRight()), &intersection); +#endif if(type == QLineF::BoundedIntersection) { !limits.contains(line.p1())?line.setP1(intersection.toPoint()):line.setP2(intersection.toPoint()); @@ -968,7 +985,7 @@ void UPlotCurve::setData(const std::vector & x, const std::vector void UPlotCurve::setData(const QVector & y) { - this->setData(y.toStdVector()); + this->setData(std::vector(y.begin(), y.end())); } void UPlotCurve::setData(const std::vector & y) @@ -1194,8 +1211,13 @@ void UPlotAxis::setAxis(qreal & min, qreal & max) } else { +#if QT_VERSION >= QT_VERSION_CHECK(5, 15, 0) + borderMin = this->fontMetrics().horizontalAdvance(QString::number(_min,'g',_gradMaxDigits))/2; + borderMax = this->fontMetrics().horizontalAdvance(QString::number(_max,'g',_gradMaxDigits))/2; +#else borderMin = this->fontMetrics().width(QString::number(_min,'g',_gradMaxDigits))/2; borderMax = this->fontMetrics().width(QString::number(_max,'g',_gradMaxDigits))/2; +#endif } int border = borderMin>borderMax?borderMin:borderMax; int borderDelta; @@ -1303,10 +1325,18 @@ void UPlotAxis::setAxis(qreal & min, qreal & max) for (int i = 0; i <= _count; i+=5) { QString n(QString::number(_min + (i/5)*((_max-_min)/(_count/5)),'g',_gradMaxDigits)); +#if QT_VERSION >= QT_VERSION_CHECK(5, 15, 0) + if(this->fontMetrics().horizontalAdvance(n) > minWidth) + { + minWidth = this->fontMetrics().horizontalAdvance(n); + } +#else if(this->fontMetrics().width(n) > minWidth) { minWidth = this->fontMetrics().width(n); } +#endif + } this->setMinimumWidth(15+minWidth); } @@ -1592,7 +1622,7 @@ void UPlotLegend::addItem(UPlotCurve * curve) QHBoxLayout * hLayout = new QHBoxLayout(); hLayout->addWidget(legendItem); hLayout->addStretch(0); - hLayout->setMargin(0); + hLayout->setContentsMargins(0,0,0,0); // add to the legend ((QVBoxLayout*)_contentLayout)->insertLayout(_contentLayout->count()-1, hLayout); @@ -1644,7 +1674,7 @@ void UPlotLegend::moveUp(UPlotLegendItem * item) QHBoxLayout * hLayout = new QHBoxLayout(); hLayout->addWidget(layoutItem->layout()->itemAt(0)->widget()); hLayout->addStretch(0); - hLayout->setMargin(0); + hLayout->setContentsMargins(0,0,0,0); ((QVBoxLayout*)_contentLayout)->insertLayout(index-1, hLayout); delete layoutItem; Q_EMIT legendItemMoved(item->curve(), index-1); @@ -1671,7 +1701,7 @@ void UPlotLegend::moveDown(UPlotLegendItem * item) QHBoxLayout * hLayout = new QHBoxLayout(); hLayout->addWidget(layoutItem->layout()->itemAt(0)->widget()); hLayout->addStretch(0); - hLayout->setMargin(0); + hLayout->setContentsMargins(0,0,0,0); ((QVBoxLayout*)_contentLayout)->insertLayout(index+1, hLayout); delete layoutItem; Q_EMIT legendItemMoved(item->curve(), index+1); @@ -1694,7 +1724,7 @@ QString UPlotLegend::getAllCurveDataAsText() const xAxisMap.insert(iter.key(), iter.value()); } } - QList xAxis = xAxisMap.uniqueKeys(); + QList xAxis = xAxisMap.keys(); QVector > axes; for(int i=0; iisChecked() && xPos>=0 && yPos>=0 && xPos<_graphicsViewHolder->width() && yPos<_graphicsViewHolder->height())) { +#if QT_VERSION >= QT_VERSION_CHECK(6, 0, 0) + QToolTip::showText(event->globalPosition().toPoint(), QString("%1,%2").arg(x).arg(y)); +#else QToolTip::showText(event->globalPos(), QString("%1,%2").arg(x).arg(y)); +#endif } else { @@ -2831,8 +2865,8 @@ void UPlot::contextMenuEvent(QContextMenuEvent * event) QPalette p(palette()); // Set background color to white - QColor c = p.color(QPalette::Background); - p.setColor(QPalette::Background, Qt::white); + QColor c = p.color(QPalette::Window); + p.setColor(QPalette::Window, Qt::white); setPalette(p); #ifdef QT_SVG_LIB @@ -2858,14 +2892,14 @@ void UPlot::contextMenuEvent(QContextMenuEvent * event) } else { - QPixmap figure = QPixmap::grabWidget(this); + QPixmap figure = this->grab(); figure.save(text); } #ifdef QT_SVG_LIB } #endif // revert background color - p.setColor(QPalette::Background, c); + p.setColor(QPalette::Window, c); setPalette(p); if(flatModified) @@ -2923,7 +2957,7 @@ void UPlot::captureScreen() } targetDir += "/"; QString name = (QDateTime::currentDateTime().toString("yyMMddhhmmsszzz") + ".") + _autoScreenCaptureFormat; - QPixmap figure = QPixmap::grabWidget(this); + QPixmap figure = this->grab(); figure.save(targetDir + name); } @@ -3181,7 +3215,7 @@ void UPlot::removeCurves() void UPlot::removeCurve(const UPlotCurve * curve) { - QList::iterator iter = qFind(_curves.begin(), _curves.end(), curve); + QList::iterator iter = std::find(_curves.begin(), _curves.end(), curve); #if PRINT_DEBUG ULOGGER_DEBUG("Plot=\"%s\" removing curve=\"%s\"", this->objectName().toStdString().c_str(), curve?curve->name().toStdString().c_str():""); #endif @@ -3215,7 +3249,7 @@ void UPlot::removeCurve(const UPlotCurve * curve) void UPlot::showCurve(const UPlotCurve * curve, bool shown) { - QList::iterator iter = qFind(_curves.begin(), _curves.end(), curve); + QList::iterator iter = std::find(_curves.begin(), _curves.end(), curve); if(iter!=_curves.end()) { UPlotCurve * value = *iter; diff --git a/package.xml b/package.xml index 3c7f164823..48e15620e8 100644 --- a/package.xml +++ b/package.xml @@ -1,7 +1,7 @@ rtabmap - 0.20.22 + 0.21.2 RTAB-Map's standalone library. RTAB-Map is a RGB-D SLAM approach with real-time constraints. Mathieu Labbe Mathieu Labbe @@ -14,9 +14,8 @@ proj cv_bridge - libfreenect-dev libg2o - libopenni-dev + gtsam libpcl-all-dev libpointmatcher diff --git a/tools/Calibration/CMakeLists.txt b/tools/Calibration/CMakeLists.txt index 8d76844ea7..9bb3f74f48 100644 --- a/tools/Calibration/CMakeLists.txt +++ b/tools/Calibration/CMakeLists.txt @@ -1,44 +1,6 @@ -SET(INCLUDE_DIRS - ${PROJECT_BINARY_DIR}/corelib/include - ${PROJECT_SOURCE_DIR}/corelib/include - ${PROJECT_SOURCE_DIR}/guilib/include - ${PROJECT_SOURCE_DIR}/utilite/include - ${OpenCV_INCLUDE_DIRS} - ${PCL_INCLUDE_DIRS} -) - -IF(QT4_FOUND) - INCLUDE(${QT_USE_FILE}) -ENDIF(QT4_FOUND) - -SET(LIBRARIES - ${OpenCV_LIBRARIES} - ${PCL_LIBRARIES} - ${QT_LIBRARIES} -) - -add_definitions(${PCL_DEFINITIONS}) - -# Hack as CameraRealsense2.h needs realsense2 include dir -IF(realsense2_FOUND) - SET(INCLUDE_DIRS - ${INCLUDE_DIRS} - ${realsense2_INCLUDE_DIRS} - ) -ENDIF(realsense2_FOUND) - -# Hack as CameraK4A.h needs k4a include dir -IF(k4a_FOUND) - SET(INCLUDE_DIRS - ${INCLUDE_DIRS} - ${k4a_INCLUDE_DIRS} - ) -ENDIF(k4a_FOUND) - -INCLUDE_DIRECTORIES(${INCLUDE_DIRS}) ADD_EXECUTABLE(calibration main.cpp) -TARGET_LINK_LIBRARIES(calibration rtabmap_core rtabmap_gui rtabmap_utilite ${LIBRARIES}) +TARGET_LINK_LIBRARIES(calibration rtabmap_gui) SET_TARGET_PROPERTIES( calibration PROPERTIES OUTPUT_NAME ${PROJECT_PREFIX}-calibration) diff --git a/tools/Camera/CMakeLists.txt b/tools/Camera/CMakeLists.txt index 69add6252f..8100133e1d 100644 --- a/tools/Camera/CMakeLists.txt +++ b/tools/Camera/CMakeLists.txt @@ -1,22 +1,6 @@ -SET(INCLUDE_DIRS - ${PROJECT_BINARY_DIR}/corelib/include - ${PROJECT_SOURCE_DIR}/corelib/include - ${PROJECT_SOURCE_DIR}/utilite/include - ${OpenCV_INCLUDE_DIRS} - ${PCL_INCLUDE_DIRS} -) - -SET(LIBRARIES - ${OpenCV_LIBRARIES} - ${PCL_LIBRARIES} -) - -add_definitions(${PCL_DEFINITIONS}) - -INCLUDE_DIRECTORIES(${INCLUDE_DIRS}) ADD_EXECUTABLE(camera main.cpp) -TARGET_LINK_LIBRARIES(camera rtabmap_core rtabmap_utilite ${LIBRARIES}) +TARGET_LINK_LIBRARIES(camera rtabmap_core) SET_TARGET_PROPERTIES( camera PROPERTIES OUTPUT_NAME ${PROJECT_PREFIX}-camera) diff --git a/tools/CameraRGBD/CMakeLists.txt b/tools/CameraRGBD/CMakeLists.txt index 5549e6fdb5..4b203e5119 100644 --- a/tools/CameraRGBD/CMakeLists.txt +++ b/tools/CameraRGBD/CMakeLists.txt @@ -1,44 +1,6 @@ -IF(NOT WITH_QT) - # visualization module required - FIND_PACKAGE(PCL 1.7 REQUIRED QUIET COMPONENTS common io kdtree search surface filters registration sample_consensus segmentation visualization) -ENDIF(NOT WITH_QT) - -SET(INCLUDE_DIRS - ${PROJECT_BINARY_DIR}/corelib/include - ${PROJECT_SOURCE_DIR}/corelib/include - ${PROJECT_SOURCE_DIR}/utilite/include - ${OpenCV_INCLUDE_DIRS} - ${PCL_INCLUDE_DIRS} -) - -SET(LIBRARIES - ${OpenCV_LIBRARIES} - ${PCL_LIBRARIES} -) - -add_definitions(${PCL_DEFINITIONS}) - -# Hack as CameraRealsense2.h needs realsense2 include dir -IF(realsense2_FOUND) - SET(INCLUDE_DIRS - ${INCLUDE_DIRS} - ${realsense2_INCLUDE_DIRS} - ) -ENDIF(realsense2_FOUND) - -# Hack as CameraK4A.h needs k4a include dir -IF(k4a_FOUND) - SET(INCLUDE_DIRS - ${INCLUDE_DIRS} - ${k4a_INCLUDE_DIRS} - ) -ENDIF(k4a_FOUND) - -INCLUDE_DIRECTORIES(${INCLUDE_DIRS}) - ADD_EXECUTABLE(rgbd_camera main.cpp) -TARGET_LINK_LIBRARIES(rgbd_camera rtabmap_core rtabmap_utilite ${LIBRARIES}) +TARGET_LINK_LIBRARIES(rgbd_camera rtabmap_gui) SET_TARGET_PROPERTIES( rgbd_camera PROPERTIES OUTPUT_NAME ${PROJECT_PREFIX}-rgbd_camera) diff --git a/tools/CleanupLocalGrids/CMakeLists.txt b/tools/CleanupLocalGrids/CMakeLists.txt index 9d05dff6ee..7d16f37ab6 100644 --- a/tools/CleanupLocalGrids/CMakeLists.txt +++ b/tools/CleanupLocalGrids/CMakeLists.txt @@ -1,31 +1,7 @@ -SET(RTABMap_INCLUDE_DIRS - ${PROJECT_BINARY_DIR}/corelib/include - ${PROJECT_SOURCE_DIR}/utilite/include - ${PROJECT_SOURCE_DIR}/corelib/include -) -SET(RTABMap_LIBRARIES - rtabmap_core - rtabmap_utilite -) - -SET(INCLUDE_DIRS - ${RTABMap_INCLUDE_DIRS} - ${OpenCV_INCLUDE_DIRS} - ${PCL_INCLUDE_DIRS} -) - -SET(LIBRARIES - ${RTABMap_LIBRARIES} - ${OpenCV_LIBRARIES} - ${PCL_LIBRARIES} -) - -INCLUDE_DIRECTORIES(${INCLUDE_DIRS}) - ADD_EXECUTABLE(cleanupLocalGrids main.cpp) -TARGET_LINK_LIBRARIES(cleanupLocalGrids ${LIBRARIES}) +TARGET_LINK_LIBRARIES(cleanupLocalGrids rtabmap_core) SET_TARGET_PROPERTIES( cleanupLocalGrids PROPERTIES OUTPUT_NAME ${PROJECT_PREFIX}-cleanupLocalGrids) diff --git a/tools/ConsoleApp/CMakeLists.txt b/tools/ConsoleApp/CMakeLists.txt index 0300e9e35c..818f2bd459 100644 --- a/tools/ConsoleApp/CMakeLists.txt +++ b/tools/ConsoleApp/CMakeLists.txt @@ -1,32 +1,8 @@ -SET(SRC_FILES - main.cpp -) - -SET(INCLUDE_DIRS - ${PROJECT_BINARY_DIR}/corelib/include - ${PROJECT_SOURCE_DIR}/utilite/include - ${PROJECT_SOURCE_DIR}/corelib/include - ${CMAKE_CURRENT_SOURCE_DIR} - ${OpenCV_INCLUDE_DIRS} - ${CMAKE_CURRENT_SOURCE_DIR}/../include - ${PCL_INCLUDE_DIRS} -) - -SET(LIBRARIES - ${OpenCV_LIBRARIES} - ${PCL_LIBRARIES} -) - -add_definitions(${PCL_DEFINITIONS}) - -# Make sure the compiler can find include files from our library. -INCLUDE_DIRECTORIES(${INCLUDE_DIRS}) - # Add binary called "consoleApp" that is built from the source file "main.cpp". # The extension is automatically found. -ADD_EXECUTABLE(consoleApp ${SRC_FILES}) -TARGET_LINK_LIBRARIES(consoleApp rtabmap_core rtabmap_utilite ${LIBRARIES}) +ADD_EXECUTABLE(consoleApp main.cpp) +TARGET_LINK_LIBRARIES(consoleApp rtabmap_core) SET_TARGET_PROPERTIES( consoleApp PROPERTIES OUTPUT_NAME ${PROJECT_PREFIX}-console) diff --git a/tools/DataRecorder/CMakeLists.txt b/tools/DataRecorder/CMakeLists.txt index 3757d30b9a..61b3c53309 100644 --- a/tools/DataRecorder/CMakeLists.txt +++ b/tools/DataRecorder/CMakeLists.txt @@ -1,54 +1,10 @@ -SET(SRC_FILES - main.cpp -) - -SET(INCLUDE_DIRS - ${PROJECT_BINARY_DIR}/corelib/include - ${PROJECT_SOURCE_DIR}/corelib/include - ${PROJECT_SOURCE_DIR}/utilite/include - ${PROJECT_SOURCE_DIR}/guilib/include - ${CMAKE_CURRENT_SOURCE_DIR} - ${OpenCV_INCLUDE_DIRS} - ${PCL_INCLUDE_DIRS} -) - -IF(QT4_FOUND) - INCLUDE(${QT_USE_FILE}) -ENDIF(QT4_FOUND) - -SET(LIBRARIES - ${OpenCV_LIBRARIES} - ${PCL_LIBRARIES} - ${QT_LIBRARIES} -) - -add_definitions(${PCL_DEFINITIONS}) - -# Hack as CameraRealsense2.h needs realsense2 include dir -IF(realsense2_FOUND) - SET(INCLUDE_DIRS - ${INCLUDE_DIRS} - ${realsense2_INCLUDE_DIRS} - ) -ENDIF(realsense2_FOUND) - -# Hack as CameraK4A.h needs k4a include dir -IF(k4a_FOUND) - SET(INCLUDE_DIRS - ${INCLUDE_DIRS} - ${k4a_INCLUDE_DIRS} - ) -ENDIF(k4a_FOUND) - -INCLUDE_DIRECTORIES(${INCLUDE_DIRS}) - IF(MINGW) - ADD_EXECUTABLE(dataRecorder WIN32 ${SRC_FILES}) + ADD_EXECUTABLE(dataRecorder WIN32 main.cpp) ELSE() - ADD_EXECUTABLE(dataRecorder ${SRC_FILES}) + ADD_EXECUTABLE(dataRecorder main.cpp) ENDIF() -TARGET_LINK_LIBRARIES(dataRecorder rtabmap_core rtabmap_gui rtabmap_utilite ${LIBRARIES}) +TARGET_LINK_LIBRARIES(dataRecorder rtabmap_gui) SET_TARGET_PROPERTIES( dataRecorder PROPERTIES OUTPUT_NAME ${PROJECT_PREFIX}-dataRecorder) diff --git a/tools/DataRecorder/main.cpp b/tools/DataRecorder/main.cpp index c6ec68c528..bf76d06bd0 100644 --- a/tools/DataRecorder/main.cpp +++ b/tools/DataRecorder/main.cpp @@ -148,6 +148,7 @@ int main (int argc, char * argv[]) cam->setMirroringEnabled(dialog.isSourceMirroring()); cam->setColorOnly(dialog.isSourceRGBDColorOnly()); cam->setImageDecimation(dialog.getSourceImageDecimation()); + cam->setHistogramMethod(dialog.getSourceHistogramMethod()); cam->setStereoToDepth(dialog.isSourceStereoDepthGenerated()); cam->setStereoExposureCompensation(dialog.isSourceStereoExposureCompensation()); cam->setScanParameters( diff --git a/tools/DatabaseViewer/CMakeLists.txt b/tools/DatabaseViewer/CMakeLists.txt index fc75a8e513..0c1dd55b50 100644 --- a/tools/DatabaseViewer/CMakeLists.txt +++ b/tools/DatabaseViewer/CMakeLists.txt @@ -1,38 +1,11 @@ -SET(SRC_FILES - main.cpp -) - -SET(INCLUDE_DIRS - ${PROJECT_BINARY_DIR}/corelib/include - ${PROJECT_SOURCE_DIR}/utilite/include - ${PROJECT_SOURCE_DIR}/corelib/include - ${PROJECT_SOURCE_DIR}/guilib/include - ${OpenCV_INCLUDE_DIRS} - ${PCL_INCLUDE_DIRS} -) - -IF(QT4_FOUND) - INCLUDE(${QT_USE_FILE}) -ENDIF(QT4_FOUND) - -SET(LIBRARIES - ${OpenCV_LIBRARIES} - ${QT_LIBRARIES} - ${PCL_LIBRARIES} -) - -add_definitions(${PCL_DEFINITIONS}) - -INCLUDE_DIRECTORIES(${INCLUDE_DIRS}) - IF(MINGW) - ADD_EXECUTABLE(databaseViewer WIN32 ${SRC_FILES}) + ADD_EXECUTABLE(databaseViewer WIN32 main.cpp) ELSE() - ADD_EXECUTABLE(databaseViewer ${SRC_FILES}) + ADD_EXECUTABLE(databaseViewer main.cpp) ENDIF() -TARGET_LINK_LIBRARIES(databaseViewer rtabmap_core rtabmap_gui rtabmap_utilite ${LIBRARIES}) +TARGET_LINK_LIBRARIES(databaseViewer rtabmap_gui) SET_TARGET_PROPERTIES( databaseViewer PROPERTIES OUTPUT_NAME ${PROJECT_PREFIX}-databaseViewer) diff --git a/tools/DatabaseViewer/main.cpp b/tools/DatabaseViewer/main.cpp index 4893f85cc3..3872cc4d91 100644 --- a/tools/DatabaseViewer/main.cpp +++ b/tools/DatabaseViewer/main.cpp @@ -30,6 +30,11 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "rtabmap/utilite/ULogger.h" #include +#include + +#if VTK_MAJOR_VERSION > 9 || (VTK_MAJOR_VERSION==9 && VTK_MINOR_VERSION >= 1) +#include +#endif int main(int argc, char * argv[]) { @@ -40,8 +45,9 @@ int main(int argc, char * argv[]) CoInitialize(nullptr); #endif -#if VTK_MAJOR_VERSION >= 8 - vtkObject::GlobalWarningDisplayOff(); +#if VTK_MAJOR_VERSION > 9 || (VTK_MAJOR_VERSION==9 && VTK_MINOR_VERSION >= 1) + // needed to ensure appropriate OpenGL context is created for VTK rendering. + QSurfaceFormat::setDefaultFormat(QVTKRenderWidget::defaultFormat()); #endif QApplication * app = new QApplication(argc, argv); diff --git a/tools/DetectMoreLoopClosures/CMakeLists.txt b/tools/DetectMoreLoopClosures/CMakeLists.txt index f18fe556db..19b2fbc871 100644 --- a/tools/DetectMoreLoopClosures/CMakeLists.txt +++ b/tools/DetectMoreLoopClosures/CMakeLists.txt @@ -1,35 +1,7 @@ -SET(RTABMap_INCLUDE_DIRS - ${PROJECT_BINARY_DIR}/corelib/include - ${PROJECT_SOURCE_DIR}/utilite/include - ${PROJECT_SOURCE_DIR}/corelib/include -) -SET(RTABMap_LIBRARIES - rtabmap_core - rtabmap_utilite -) - -if(POLICY CMP0020) - cmake_policy(SET CMP0020 NEW) -endif() - -SET(INCLUDE_DIRS - ${RTABMap_INCLUDE_DIRS} - ${OpenCV_INCLUDE_DIRS} - ${PCL_INCLUDE_DIRS} -) - -SET(LIBRARIES - ${RTABMap_LIBRARIES} - ${OpenCV_LIBRARIES} - ${PCL_LIBRARIES} -) - -INCLUDE_DIRECTORIES(${INCLUDE_DIRS}) - ADD_EXECUTABLE(detectMoreLoopClosures main.cpp) -TARGET_LINK_LIBRARIES(detectMoreLoopClosures ${LIBRARIES}) +TARGET_LINK_LIBRARIES(detectMoreLoopClosures rtabmap_core) SET_TARGET_PROPERTIES( detectMoreLoopClosures PROPERTIES OUTPUT_NAME ${PROJECT_PREFIX}-detectMoreLoopClosures) diff --git a/tools/EpipolarGeometry/CMakeLists.txt b/tools/EpipolarGeometry/CMakeLists.txt index b4ceba59e8..5f92f7b8b9 100644 --- a/tools/EpipolarGeometry/CMakeLists.txt +++ b/tools/EpipolarGeometry/CMakeLists.txt @@ -1,35 +1,10 @@ -SET(SRC_FILES - main.cpp -) - -SET(INCLUDE_DIRS - ${PROJECT_BINARY_DIR}/corelib/include - ${PROJECT_SOURCE_DIR}/utilite/include - ${PROJECT_SOURCE_DIR}/corelib/include - ${PROJECT_SOURCE_DIR}/guilib/include - ${OpenCV_INCLUDE_DIRS} - ${PCL_INCLUDE_DIRS} -) - -IF(QT4_FOUND) - INCLUDE(${QT_USE_FILE}) -ENDIF(QT4_FOUND) - -SET(LIBRARIES - ${OpenCV_LIBRARIES} - ${PCL_LIBRARIES} - ${QT_LIBRARIES} -) - -INCLUDE_DIRECTORIES(${INCLUDE_DIRS}) - IF(MINGW) - ADD_EXECUTABLE(epipolar_geometry WIN32 ${SRC_FILES}) + ADD_EXECUTABLE(epipolar_geometry WIN32 main.cpp) ELSE() - ADD_EXECUTABLE(epipolar_geometry ${SRC_FILES}) + ADD_EXECUTABLE(epipolar_geometry main.cpp) ENDIF() -TARGET_LINK_LIBRARIES(epipolar_geometry rtabmap_core rtabmap_gui rtabmap_utilite ${LIBRARIES}) +TARGET_LINK_LIBRARIES(epipolar_geometry rtabmap_gui) SET_TARGET_PROPERTIES( epipolar_geometry PROPERTIES OUTPUT_NAME ${PROJECT_PREFIX}-epipolar_geometry) diff --git a/tools/EpipolarGeometry/main.cpp b/tools/EpipolarGeometry/main.cpp index 93fe8ae534..aa309fa97f 100644 --- a/tools/EpipolarGeometry/main.cpp +++ b/tools/EpipolarGeometry/main.cpp @@ -51,6 +51,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include +#include #include @@ -183,11 +184,10 @@ int main(int argc, char** argv) showUsage(); } - QTime timer; + QElapsedTimer timer; timer.start(); // Extract words - timer.start(); VWDictionary dictionary; ParametersMap param; param.insert(ParametersPair(Parameters::kSURFExtended(), "true")); diff --git a/tools/EurocDataset/CMakeLists.txt b/tools/EurocDataset/CMakeLists.txt index 775405be9b..e766ee1321 100644 --- a/tools/EurocDataset/CMakeLists.txt +++ b/tools/EurocDataset/CMakeLists.txt @@ -1,55 +1,41 @@ -cmake_minimum_required(VERSION 2.8) FIND_PACKAGE(yaml-cpp QUIET) -IF(NOT yaml-cpp_FOUND) +IF(yaml-cpp_FOUND) + IF (TARGET yaml-cpp::yaml-cpp) + # yaml-cpp 0.8.0 uses target yaml-cpp::yaml-cpp. + SET(YAML_CPP_LIBRARIES yaml-cpp::yaml-cpp) + ELSEIF (TARGET yaml-cpp) + # yaml-cpp 0.7.0 uses target yaml-cpp (VCPKG). + SET(YAML_CPP_LIBRARIES yaml-cpp) + ENDIF() +ELSE() find_package(PkgConfig QUIET) IF(PKG_CONFIG_FOUND) pkg_check_modules(yaml_cpp QUIET yaml-cpp) IF(yaml_cpp_FOUND) SET(YAML_CPP_LIBRARIES ${yaml_cpp_LIBRARIES}) - SET(YAML_CPP_INCLUDE_DIRS ${yaml_cpp_INCLUDEDIR}) + SET(YAML_CPP_INCLUDE_DIR ${yaml_cpp_INCLUDEDIR}) SET(yaml-cpp_FOUND ${yaml_cpp_FOUND}) ENDIF(yaml_cpp_FOUND) ENDIF(PKG_CONFIG_FOUND) -ENDIF(NOT yaml-cpp_FOUND) +ENDIF(yaml-cpp_FOUND) IF(yaml-cpp_FOUND) -# inside rtabmap project (see below for external build) -SET(RTABMap_INCLUDE_DIRS - ${PROJECT_BINARY_DIR}/corelib/include - ${PROJECT_SOURCE_DIR}/utilite/include - ${PROJECT_SOURCE_DIR}/corelib/include -) -SET(RTABMap_LIBRARIES - rtabmap_core - rtabmap_utilite -) - -if(POLICY CMP0020) - cmake_policy(SET CMP0020 NEW) -endif() - SET(INCLUDE_DIRS - ${RTABMap_INCLUDE_DIRS} - ${OpenCV_INCLUDE_DIRS} - ${PCL_INCLUDE_DIRS} - ${YAML_CPP_INCLUDE_DIRS} + ${YAML_CPP_INCLUDE_DIR} ) SET(LIBRARIES - ${RTABMap_LIBRARIES} - ${OpenCV_LIBRARIES} - ${PCL_LIBRARIES} ${YAML_CPP_LIBRARIES} ) -INCLUDE_DIRECTORIES(${INCLUDE_DIRS}) +INCLUDE_DIRECTORIES(${INCLUDE_DIRS} yaml-cpp) ADD_EXECUTABLE(euroc_dataset main.cpp) -TARGET_LINK_LIBRARIES(euroc_dataset ${LIBRARIES}) +TARGET_LINK_LIBRARIES(euroc_dataset rtabmap_core ${LIBRARIES}) SET_TARGET_PROPERTIES( euroc_dataset PROPERTIES OUTPUT_NAME ${PROJECT_PREFIX}-euroc_dataset) diff --git a/tools/EurocDataset/main.cpp b/tools/EurocDataset/main.cpp index 8592f1dce1..27b5c061b2 100644 --- a/tools/EurocDataset/main.cpp +++ b/tools/EurocDataset/main.cpp @@ -486,6 +486,7 @@ int main(int argc, char * argv[]) externalStats.insert(std::make_pair("Camera/Disparity/ms", cameraInfo.timeDisparity*1000.0f)); externalStats.insert(std::make_pair("Camera/ImageDecimation/ms", cameraInfo.timeImageDecimation*1000.0f)); externalStats.insert(std::make_pair("Camera/Mirroring/ms", cameraInfo.timeMirroring*1000.0f)); + externalStats.insert(std::make_pair("Camera/HistogramEqualization/ms", cameraInfo.timeHistogramEqualization*1000.0f)); externalStats.insert(std::make_pair("Camera/ExposureCompensation/ms", cameraInfo.timeStereoExposureCompensation*1000.0f)); externalStats.insert(std::make_pair("Camera/ScanFromDepth/ms", cameraInfo.timeScanFromDepth*1000.0f)); externalStats.insert(std::make_pair("Camera/TotalTime/ms", cameraInfo.timeTotal*1000.0f)); diff --git a/tools/Export/CMakeLists.txt b/tools/Export/CMakeLists.txt index 792e8b8e14..fb3d5c4843 100644 --- a/tools/Export/CMakeLists.txt +++ b/tools/Export/CMakeLists.txt @@ -1,31 +1,7 @@ -SET(RTABMap_INCLUDE_DIRS - ${PROJECT_BINARY_DIR}/corelib/include - ${PROJECT_SOURCE_DIR}/utilite/include - ${PROJECT_SOURCE_DIR}/corelib/include -) -SET(RTABMap_LIBRARIES - rtabmap_core - rtabmap_utilite -) - -SET(INCLUDE_DIRS - ${RTABMap_INCLUDE_DIRS} - ${OpenCV_INCLUDE_DIRS} - ${PCL_INCLUDE_DIRS} -) - -SET(LIBRARIES - ${RTABMap_LIBRARIES} - ${OpenCV_LIBRARIES} - ${PCL_LIBRARIES} -) - -INCLUDE_DIRECTORIES(${INCLUDE_DIRS}) - ADD_EXECUTABLE(export main.cpp) -TARGET_LINK_LIBRARIES(export ${LIBRARIES}) +TARGET_LINK_LIBRARIES(export rtabmap_core) SET_TARGET_PROPERTIES( export PROPERTIES OUTPUT_NAME ${PROJECT_PREFIX}-export) diff --git a/tools/ExtractObject/CMakeLists.txt b/tools/ExtractObject/CMakeLists.txt index e57bea5858..0be69c08d5 100644 --- a/tools/ExtractObject/CMakeLists.txt +++ b/tools/ExtractObject/CMakeLists.txt @@ -1,26 +1,7 @@ -SET(SRC_FILES - main.cpp -) -SET(INCLUDE_DIRS - ${PCL_INCLUDE_DIRS} - ${PROJECT_SOURCE_DIR}/utilite/include -) - -SET(LIBRARIES - ${PCL_LIBRARIES} -) - -add_definitions(${PCL_DEFINITIONS}) - -# Make sure the compiler can find include files from our library. -INCLUDE_DIRECTORIES(${INCLUDE_DIRS}) - -# Add binary called "consoleApp" that is built from the source file "main.cpp". -# The extension is automatically found. -ADD_EXECUTABLE(extractObject ${SRC_FILES}) -TARGET_LINK_LIBRARIES(extractObject rtabmap_utilite ${LIBRARIES}) +ADD_EXECUTABLE(extractObject main.cpp) +TARGET_LINK_LIBRARIES(extractObject rtabmap_core) SET_TARGET_PROPERTIES( extractObject PROPERTIES OUTPUT_NAME ${PROJECT_PREFIX}-extractObject) diff --git a/tools/GlobalBundleAdjustment/CMakeLists.txt b/tools/GlobalBundleAdjustment/CMakeLists.txt index b2853c3984..988ee0b91a 100644 --- a/tools/GlobalBundleAdjustment/CMakeLists.txt +++ b/tools/GlobalBundleAdjustment/CMakeLists.txt @@ -1,35 +1,7 @@ -SET(RTABMap_INCLUDE_DIRS - ${PROJECT_BINARY_DIR}/corelib/include - ${PROJECT_SOURCE_DIR}/utilite/include - ${PROJECT_SOURCE_DIR}/corelib/include -) -SET(RTABMap_LIBRARIES - rtabmap_core - rtabmap_utilite -) - -if(POLICY CMP0020) - cmake_policy(SET CMP0020 NEW) -endif() - -SET(INCLUDE_DIRS - ${RTABMap_INCLUDE_DIRS} - ${OpenCV_INCLUDE_DIRS} - ${PCL_INCLUDE_DIRS} -) - -SET(LIBRARIES - ${RTABMap_LIBRARIES} - ${OpenCV_LIBRARIES} - ${PCL_LIBRARIES} -) - -INCLUDE_DIRECTORIES(${INCLUDE_DIRS}) - ADD_EXECUTABLE(globalBundleAdjustment main.cpp) -TARGET_LINK_LIBRARIES(globalBundleAdjustment ${LIBRARIES}) +TARGET_LINK_LIBRARIES(globalBundleAdjustment rtabmap_core) SET_TARGET_PROPERTIES( globalBundleAdjustment PROPERTIES OUTPUT_NAME ${PROJECT_PREFIX}-globalBundleAdjustment) diff --git a/tools/ImagesJoiner/CMakeLists.txt b/tools/ImagesJoiner/CMakeLists.txt index c6979bfe71..879441e2da 100644 --- a/tools/ImagesJoiner/CMakeLists.txt +++ b/tools/ImagesJoiner/CMakeLists.txt @@ -1,25 +1,6 @@ -SET(SRC_FILES - main.cpp -) - -SET(INCLUDE_DIRS - ${PROJECT_SOURCE_DIR}/utilite/include - ${CMAKE_CURRENT_SOURCE_DIR} - ${OpenCV_INCLUDE_DIRS} -) - -SET(LIBRARIES - ${OpenCV_LIBRARIES} -) - -# Make sure the compiler can find include files from our library. -INCLUDE_DIRECTORIES(${INCLUDE_DIRS}) - -# Add binary called "imagesJoiner" that is built from the source file "main.cpp". -# The extension is automatically found. -ADD_EXECUTABLE(imagesJoiner ${SRC_FILES}) -TARGET_LINK_LIBRARIES(imagesJoiner rtabmap_utilite ${LIBRARIES}) +ADD_EXECUTABLE(imagesJoiner main.cpp) +TARGET_LINK_LIBRARIES(imagesJoiner rtabmap_core) SET_TARGET_PROPERTIES( imagesJoiner PROPERTIES OUTPUT_NAME ${PROJECT_PREFIX}-imagesJoiner) diff --git a/tools/Info/CMakeLists.txt b/tools/Info/CMakeLists.txt index dafcb77c59..fc2180c995 100644 --- a/tools/Info/CMakeLists.txt +++ b/tools/Info/CMakeLists.txt @@ -1,35 +1,7 @@ -SET(RTABMap_INCLUDE_DIRS - ${PROJECT_BINARY_DIR}/corelib/include - ${PROJECT_SOURCE_DIR}/utilite/include - ${PROJECT_SOURCE_DIR}/corelib/include -) -SET(RTABMap_LIBRARIES - rtabmap_core - rtabmap_utilite -) - -if(POLICY CMP0020) - cmake_policy(SET CMP0020 NEW) -endif() - -SET(INCLUDE_DIRS - ${RTABMap_INCLUDE_DIRS} - ${OpenCV_INCLUDE_DIRS} - ${PCL_INCLUDE_DIRS} -) - -SET(LIBRARIES - ${RTABMap_LIBRARIES} - ${OpenCV_LIBRARIES} - ${PCL_LIBRARIES} -) - -INCLUDE_DIRECTORIES(${INCLUDE_DIRS}) - ADD_EXECUTABLE(info main.cpp) -TARGET_LINK_LIBRARIES(info ${LIBRARIES}) +TARGET_LINK_LIBRARIES(info rtabmap_core) SET_TARGET_PROPERTIES( info PROPERTIES OUTPUT_NAME ${PROJECT_PREFIX}-info) diff --git a/tools/Info/main.cpp b/tools/Info/main.cpp index 3d39cac593..ad993e4bdf 100644 --- a/tools/Info/main.cpp +++ b/tools/Info/main.cpp @@ -26,7 +26,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ #include -#include +#include #include #include @@ -59,6 +59,7 @@ void showUsage() " Options:\n" " --diff Show only modified parameters.\n" " --diff \"other_map.db\" Compare parameters with other database.\n" + " --dump \"config.ini\" Dump parameters in ini file.\n" "\n"); exit(1); } @@ -81,19 +82,33 @@ int main(int argc, char * argv[]) } std::string otherDatabasePath; + std::string dumpFilePath; bool diff = false; for(int i=1; igetLastParameters(); + if(!dumpFilePath.empty()) + { + Parameters::writeINI(dumpFilePath, parameters); + printf("%ld parameters exported to \"%s\".\n", parameters.size(), dumpFilePath.c_str()); + return 0; + } ParametersMap defaultParameters = Parameters::getDefaultParameters(); ParametersMap removedParameters = Parameters::getBackwardCompatibilityMap(); std::string otherDatabasePathName; diff --git a/tools/KittiDataset/CMakeLists.txt b/tools/KittiDataset/CMakeLists.txt index 81bcca771b..27bc793da2 100644 --- a/tools/KittiDataset/CMakeLists.txt +++ b/tools/KittiDataset/CMakeLists.txt @@ -1,37 +1,7 @@ -cmake_minimum_required(VERSION 2.8) - -# inside rtabmap project (see below for external build) -SET(RTABMap_INCLUDE_DIRS - ${PROJECT_BINARY_DIR}/corelib/include - ${PROJECT_SOURCE_DIR}/utilite/include - ${PROJECT_SOURCE_DIR}/corelib/include -) -SET(RTABMap_LIBRARIES - rtabmap_core - rtabmap_utilite -) - -if(POLICY CMP0020) - cmake_policy(SET CMP0020 NEW) -endif() - -SET(INCLUDE_DIRS - ${RTABMap_INCLUDE_DIRS} - ${OpenCV_INCLUDE_DIRS} - ${PCL_INCLUDE_DIRS} -) - -SET(LIBRARIES - ${RTABMap_LIBRARIES} - ${OpenCV_LIBRARIES} - ${PCL_LIBRARIES} -) - -INCLUDE_DIRECTORIES(${INCLUDE_DIRS}) ADD_EXECUTABLE(kitti_dataset main.cpp) -TARGET_LINK_LIBRARIES(kitti_dataset ${LIBRARIES}) +TARGET_LINK_LIBRARIES(kitti_dataset rtabmap_core) SET_TARGET_PROPERTIES( kitti_dataset PROPERTIES OUTPUT_NAME ${PROJECT_PREFIX}-kitti_dataset) diff --git a/tools/KittiDataset/main.cpp b/tools/KittiDataset/main.cpp index 3555df0f20..56c787c5de 100644 --- a/tools/KittiDataset/main.cpp +++ b/tools/KittiDataset/main.cpp @@ -491,6 +491,7 @@ int main(int argc, char * argv[]) externalStats.insert(std::make_pair("Camera/Disparity/ms", cameraInfo.timeDisparity*1000.0f)); externalStats.insert(std::make_pair("Camera/ImageDecimation/ms", cameraInfo.timeImageDecimation*1000.0f)); externalStats.insert(std::make_pair("Camera/Mirroring/ms", cameraInfo.timeMirroring*1000.0f)); + externalStats.insert(std::make_pair("Camera/HistogramEqualization/ms", cameraInfo.timeHistogramEqualization*1000.0f)); externalStats.insert(std::make_pair("Camera/ExposureCompensation/ms", cameraInfo.timeStereoExposureCompensation*1000.0f)); externalStats.insert(std::make_pair("Camera/ScanFromDepth/ms", cameraInfo.timeScanFromDepth*1000.0f)); externalStats.insert(std::make_pair("Camera/TotalTime/ms", cameraInfo.timeTotal*1000.0f)); diff --git a/tools/Matcher/CMakeLists.txt b/tools/Matcher/CMakeLists.txt index 89dbeb80ec..39b490fb16 100644 --- a/tools/Matcher/CMakeLists.txt +++ b/tools/Matcher/CMakeLists.txt @@ -1,28 +1,6 @@ -SET(INCLUDE_DIRS - ${PROJECT_BINARY_DIR}/corelib/include - ${PROJECT_SOURCE_DIR}/corelib/include - ${PROJECT_SOURCE_DIR}/utilite/include - ${PROJECT_SOURCE_DIR}/guilib/include - ${OpenCV_INCLUDE_DIRS} - ${PCL_INCLUDE_DIRS} -) - -IF(QT4_FOUND) - INCLUDE(${QT_USE_FILE}) -ENDIF(QT4_FOUND) - -SET(LIBRARIES - ${OpenCV_LIBRARIES} - ${PCL_LIBRARIES} - ${QT_LIBRARIES} -) - -add_definitions(${PCL_DEFINITIONS}) - -INCLUDE_DIRECTORIES(${INCLUDE_DIRS}) ADD_EXECUTABLE(matcher main.cpp) -TARGET_LINK_LIBRARIES(matcher rtabmap_core rtabmap_utilite rtabmap_gui ${LIBRARIES}) +TARGET_LINK_LIBRARIES(matcher rtabmap_gui) SET_TARGET_PROPERTIES( matcher PROPERTIES OUTPUT_NAME ${PROJECT_PREFIX}-matcher) diff --git a/tools/OdometryViewer/CMakeLists.txt b/tools/OdometryViewer/CMakeLists.txt index e5a1b5a55e..11ff94fc93 100644 --- a/tools/OdometryViewer/CMakeLists.txt +++ b/tools/OdometryViewer/CMakeLists.txt @@ -1,54 +1,10 @@ -SET(INCLUDE_DIRS - ${PROJECT_BINARY_DIR}/corelib/include - ${PROJECT_SOURCE_DIR}/corelib/include - ${PROJECT_SOURCE_DIR}/utilite/include - ${PROJECT_SOURCE_DIR}/guilib/include - ${CMAKE_CURRENT_SOURCE_DIR} - ${OpenCV_INCLUDE_DIRS} - ${PCL_INCLUDE_DIRS} -) - -IF(QT4_FOUND) - INCLUDE(${QT_USE_FILE}) -ENDIF(QT4_FOUND) - -SET(LIBRARIES - ${OpenCV_LIBRARIES} - ${PCL_LIBRARIES} - ${QT_LIBRARIES} -) - -SET(SRC_FILES - main.cpp -) - -add_definitions(${PCL_DEFINITIONS}) - -# Hack as CameraRealsense2.h needs realsense2 include dir -IF(realsense2_FOUND) - SET(INCLUDE_DIRS - ${INCLUDE_DIRS} - ${realsense2_INCLUDE_DIRS} - ) -ENDIF(realsense2_FOUND) - -# Hack as CameraK4A.h needs k4a include dir -IF(k4a_FOUND) - SET(INCLUDE_DIRS - ${INCLUDE_DIRS} - ${k4a_INCLUDE_DIRS} - ) -ENDIF(k4a_FOUND) - -INCLUDE_DIRECTORIES(${INCLUDE_DIRS}) - IF(MINGW) - ADD_EXECUTABLE(odometryViewer WIN32 ${SRC_FILES}) + ADD_EXECUTABLE(odometryViewer WIN32 main.cpp) ELSE() - ADD_EXECUTABLE(odometryViewer ${SRC_FILES}) + ADD_EXECUTABLE(odometryViewer main.cpp) ENDIF() -TARGET_LINK_LIBRARIES(odometryViewer rtabmap_core rtabmap_gui rtabmap_utilite ${LIBRARIES}) +TARGET_LINK_LIBRARIES(odometryViewer rtabmap_gui) SET_TARGET_PROPERTIES( odometryViewer PROPERTIES OUTPUT_NAME ${PROJECT_PREFIX}-odometryViewer) diff --git a/tools/Recovery/CMakeLists.txt b/tools/Recovery/CMakeLists.txt index 61de000012..bd0151f919 100644 --- a/tools/Recovery/CMakeLists.txt +++ b/tools/Recovery/CMakeLists.txt @@ -1,35 +1,7 @@ -SET(RTABMap_INCLUDE_DIRS - ${PROJECT_BINARY_DIR}/corelib/include - ${PROJECT_SOURCE_DIR}/utilite/include - ${PROJECT_SOURCE_DIR}/corelib/include -) -SET(RTABMap_LIBRARIES - rtabmap_core - rtabmap_utilite -) - -if(POLICY CMP0020) - cmake_policy(SET CMP0020 NEW) -endif() - -SET(INCLUDE_DIRS - ${RTABMap_INCLUDE_DIRS} - ${OpenCV_INCLUDE_DIRS} - ${PCL_INCLUDE_DIRS} -) - -SET(LIBRARIES - ${RTABMap_LIBRARIES} - ${OpenCV_LIBRARIES} - ${PCL_LIBRARIES} -) - -INCLUDE_DIRECTORIES(${INCLUDE_DIRS}) - ADD_EXECUTABLE(recovery main.cpp) -TARGET_LINK_LIBRARIES(recovery ${LIBRARIES}) +TARGET_LINK_LIBRARIES(recovery rtabmap_core) SET_TARGET_PROPERTIES( recovery PROPERTIES OUTPUT_NAME ${PROJECT_PREFIX}-recovery) diff --git a/tools/Report/CMakeLists.txt b/tools/Report/CMakeLists.txt index 96bf26c778..c9c6c6e7d3 100644 --- a/tools/Report/CMakeLists.txt +++ b/tools/Report/CMakeLists.txt @@ -1,47 +1,11 @@ -cmake_minimum_required(VERSION 2.8) -# inside rtabmap project (see below for external build) -SET(RTABMap_INCLUDE_DIRS - ${PROJECT_BINARY_DIR}/corelib/include - ${PROJECT_SOURCE_DIR}/utilite/include - ${PROJECT_SOURCE_DIR}/corelib/include - ${PROJECT_SOURCE_DIR}/guilib/include -) -SET(RTABMap_LIBRARIES - rtabmap_core - rtabmap_utilite -) - -if(POLICY CMP0020) - cmake_policy(SET CMP0020 NEW) -endif() - -SET(INCLUDE_DIRS - ${RTABMap_INCLUDE_DIRS} - ${OpenCV_INCLUDE_DIRS} - ${PCL_INCLUDE_DIRS} -) - -SET(LIBRARIES - ${RTABMap_LIBRARIES} - ${OpenCV_LIBRARIES} - ${PCL_LIBRARIES} -) +set(LIBRARIES rtabmap_core) IF(QT4_FOUND OR Qt5_FOUND) - IF(QT4_FOUND) - INCLUDE(${QT_USE_FILE}) - ENDIF(QT4_FOUND) - SET(LIBRARIES - ${LIBRARIES} - ${QT_LIBRARIES} - rtabmap_gui - ) ADD_DEFINITIONS("-DWITH_QT") + set(LIBRARIES ${LIBRARIES} rtabmap_gui) ENDIF(QT4_FOUND OR Qt5_FOUND) -INCLUDE_DIRECTORIES(${INCLUDE_DIRS}) - ADD_EXECUTABLE(report main.cpp) TARGET_LINK_LIBRARIES(report ${LIBRARIES}) diff --git a/tools/Report/main.cpp b/tools/Report/main.cpp index fbc15ac95d..4185eae8ee 100644 --- a/tools/Report/main.cpp +++ b/tools/Report/main.cpp @@ -62,6 +62,12 @@ void showUsage() " and compute error based on the scaled path.\n" " --poses Export poses to [path]_poses.txt, ground truth to [path]_gt.txt\n" " and valid ground truth indices to [path]_indices.txt \n" + " --gt FILE.txt Use this file as ground truth (TUM RGB-D format). It will\n" + " override the ground truth set in database if there is one.\n" + " If extension is *.db, the optimized poses of that database will\n" + " be used as ground truth.\n" + " --gt_max_t # Maximum time interval (sec) to interpolate between a pose and\n" + " corresponding ground truth (default 1 sec).\n" " --inc Incremental optimization. \n" " --stats Show available statistics \"Statistic/Id\" to plot or get localization statistics (if path is a file). \n" #ifdef WITH_QT @@ -147,6 +153,8 @@ int main(int argc, char * argv[]) std::string exportPrefix = "Stat"; int showLoc = 0; float locDelay = 60; + std::string gtFile; + double gtMaxInterval = 1; std::vector statsToShow; #ifdef WITH_QT std::map figures; @@ -227,6 +235,44 @@ int main(int argc, char * argv[]) showUsage(); } } + else if(strcmp(argv[i],"--gt") == 0) + { + ++i; + if(i10) + { + printf("\"--gt_max_t\" option should be between 0 and 10 sec, parsed %f sec.\n", gtMaxInterval); + showUsage(); + } + } + else + { + printf("Missing value for \"--gt\" option.\n"); + showUsage(); + } + } else if(strcmp(argv[i],"--loc") == 0) { ++i; @@ -324,6 +370,57 @@ int main(int argc, char * argv[]) statsToShow.clear(); } + // External Ground Truth + std::map externalGtPoses; + if(!gtFile.empty()) + { + if(UFile::getExtension(gtFile) == "db") + { + DBDriver * driver = DBDriver::create(); + if(driver->openConnection(gtFile)) + { + std::map poses = driver->loadOptimizedPoses(); + for(std::map::iterator iter=poses.begin(); iter!=poses.end(); ++iter) + { + Transform p, gt; + GPS gps; + int m=-1, w=-1; + std::string l; + double s; + std::vector v; + EnvSensors sensors; + if(driver->getNodeInfo(iter->first, p, m, w, l, s, gt, v, gps, sensors)) + { + externalGtPoses.insert(std::make_pair(s, iter->second)); + } + } + } + delete driver; + } + else + { + // 10 can import format 1, 10 and 11. + std::map poses; + std::map stamps; + if(rtabmap::graph::importPoses(gtFile, 10, poses, 0, &stamps)) + { + for(std::map::iterator iter=poses.begin(); iter!=poses.end(); ++iter) + { + externalGtPoses.insert(std::make_pair(stamps.at(iter->first), iter->second)); + } + } + } + if(externalGtPoses.size() < 2) + { + printf("Failed loading ground truth poses from \"%s\" (\"--gt\" option).\n", gtFile.c_str()); + showUsage(); + } + else + { + printf("Loading %ld ground truth poses from \"%s\".\n", externalGtPoses.size(), gtFile.c_str()); + } + } + std::string fileName; std::list paths; paths.push_back(path); @@ -404,6 +501,11 @@ int main(int argc, char * argv[]) ULogger::setLevel(logLevel); std::set ids; driver->getAllNodeIds(ids, false, false, ignoreInterNodes); + std::set allIds = ids; + if(ignoreInterNodes) + { + driver->getAllNodeIds(allIds, false, false, false); + } std::map, double> > stats = driver->getAllStatistics(); std::map odomPoses, gtPoses; std::map odomStamps; @@ -523,7 +625,8 @@ int main(int argc, char * argv[]) std::map > localizationSessionStats; double previousStamp = 0.0; - for(std::set::iterator iter=ids.begin(); iter!=ids.end(); ++iter) + std::map allWeights; + for(std::set::iterator iter=allIds.begin(); iter!=allIds.end(); ++iter) { Transform p, gt; GPS gps; @@ -534,119 +637,137 @@ int main(int argc, char * argv[]) EnvSensors sensors; if(driver->getNodeInfo(*iter, p, m, w, l, s, gt, v, gps, sensors)) { - odomPoses.insert(std::make_pair(*iter, p)); - odomStamps.insert(std::make_pair(*iter, s)); - if(!gt.isNull()) + allWeights.insert(std::make_pair(*iter, w)); + if((!ignoreInterNodes || w!=-1)) { - gtPoses.insert(std::make_pair(*iter, gt)); - } - - if(!localizationMultiStats.empty() && mappingSessionIds.find(m) != mappingSessionIds.end()) - { - continue; - } - - if(*iter >= startIdPerDb && uContains(stats, *iter)) - { - const std::map & stat = stats.at(*iter).first; - if(uContains(stat, Statistics::kGtTranslational_rmse())) + odomPoses.insert(std::make_pair(*iter, p)); + odomStamps.insert(std::make_pair(*iter, s)); + if(!externalGtPoses.empty()) { - rmse = stat.at(Statistics::kGtTranslational_rmse()); - if(maxRMSE==-1 || maxRMSE < rmse) + std::map::iterator nextIter = externalGtPoses.upper_bound(s); + if(nextIter!=externalGtPoses.end()) { - maxRMSE = rmse; + std::map::iterator previousIter = nextIter; + --previousIter; + if(s == previousIter->first || (nextIter->first-s <= gtMaxInterval && s-previousIter->first <= gtMaxInterval)) + { + UASSERT(s-previousIter->first >= 0); + gtPoses.insert(std::make_pair(*iter, previousIter->second.interpolate((s-previousIter->first)/(nextIter->first-previousIter->first),nextIter->second))); + } } } - if(uContains(stat, std::string("Camera/TotalTime/ms"))) + else if(!gt.isNull()) { - cameraTime.push_back(stat.at(std::string("Camera/TotalTime/ms"))); + gtPoses.insert(std::make_pair(*iter, gt)); } - if(uContains(stat, std::string("Odometry/TotalTime/ms"))) - { - odomTime.push_back(stat.at(std::string("Odometry/TotalTime/ms"))); - } - else if(uContains(stat, std::string("Odometry/TimeEstimation/ms"))) + + if(!localizationMultiStats.empty() && mappingSessionIds.find(m) != mappingSessionIds.end()) { - odomTime.push_back(stat.at(std::string("Odometry/TimeEstimation/ms"))); + continue; } - if(uContains(stat, std::string("RtabmapROS/TotalTime/ms"))) + if(*iter >= startIdPerDb && uContains(stats, *iter)) { - if(w!=-1) + const std::map & stat = stats.at(*iter).first; + if(uContains(stat, Statistics::kGtTranslational_rmse())) { - slamTime.push_back(stat.at("RtabmapROS/TotalTime/ms")); + rmse = stat.at(Statistics::kGtTranslational_rmse()); + if(maxRMSE==-1 || maxRMSE < rmse) + { + maxRMSE = rmse; + } } - } - else if(uContains(stat, Statistics::kTimingTotal())) - { - if(w!=-1) + if(uContains(stat, std::string("Camera/TotalTime/ms"))) { - slamTime.push_back(stat.at(Statistics::kTimingTotal())); + cameraTime.push_back(stat.at(std::string("Camera/TotalTime/ms"))); + } + if(uContains(stat, std::string("Odometry/TotalTime/ms"))) + { + odomTime.push_back(stat.at(std::string("Odometry/TotalTime/ms"))); + } + else if(uContains(stat, std::string("Odometry/TimeEstimation/ms"))) + { + odomTime.push_back(stat.at(std::string("Odometry/TimeEstimation/ms"))); } - } - if(uContains(stat, std::string(Statistics::kMemoryRAM_usage()))) - { - float ram = stat.at(Statistics::kMemoryRAM_usage()); - if(maxMapRAM==-1 || maxMapRAM < ram) + if(uContains(stat, std::string("RtabmapROS/TotalTime/ms"))) { - maxMapRAM = ram; + if(w!=-1) + { + slamTime.push_back(stat.at("RtabmapROS/TotalTime/ms")); + } } - } - if(uContains(stat, std::string("Odometry/RAM_usage/MB"))) - { - float ram = stat.at("Odometry/RAM_usage/MB"); - if(maxOdomRAM==-1 || maxOdomRAM < ram) + else if(uContains(stat, Statistics::kTimingTotal())) + { + if(w!=-1) + { + slamTime.push_back(stat.at(Statistics::kTimingTotal())); + } + } + + if(uContains(stat, std::string(Statistics::kMemoryRAM_usage()))) { - maxOdomRAM = ram; + float ram = stat.at(Statistics::kMemoryRAM_usage()); + if(maxMapRAM==-1 || maxMapRAM < ram) + { + maxMapRAM = ram; + } + } + if(uContains(stat, std::string("Odometry/RAM_usage/MB"))) + { + float ram = stat.at("Odometry/RAM_usage/MB"); + if(maxOdomRAM==-1 || maxOdomRAM < ram) + { + maxOdomRAM = ram; + } } - } #ifdef WITH_QT - for(std::map::iterator jter=curves.begin(); jter!=curves.end(); ++jter) - { + for(std::map::iterator jter=curves.begin(); jter!=curves.end(); ++jter) + { #else - for(std::map > > >::iterator jter=localizationMultiStats.begin(); - jter!=localizationMultiStats.end(); - ++jter) - { -#endif - if(uContains(stat, jter->first)) + for(std::map > > >::iterator jter=localizationMultiStats.begin(); + jter!=localizationMultiStats.end(); + ++jter) { - double y = stat.at(jter->first); -#ifdef WITH_QT - double x = s; - if(useIds) +#endif + if(uContains(stat, jter->first)) { - x = *iter; - } - jter->second->addValue(x,y); + double y = stat.at(jter->first); +#ifdef WITH_QT + double x = s; + if(useIds) + { + x = *iter; + } + jter->second->addValue(x,y); #endif - if(!localizationMultiStats.empty()) - { - if(previousStamp > 0 && fabs(s - previousStamp) > locDelay && uContains(localizationSessionStats, jter->first)) + if(!localizationMultiStats.empty()) { - // changed session - for(std::map >::iterator kter=localizationSessionStats.begin(); kter!=localizationSessionStats.end(); ++kter) + if(previousStamp > 0 && fabs(s - previousStamp) > locDelay && uContains(localizationSessionStats, jter->first)) { - LocStats values = LocStats::from(localizationSessionStats.at(kter->first)); - localizationMultiStats.at(kter->first).rbegin()->second.push_back(values); - localizationSessionStats.at(kter->first).clear(); - } + // changed session + for(std::map >::iterator kter=localizationSessionStats.begin(); kter!=localizationSessionStats.end(); ++kter) + { + LocStats values = LocStats::from(localizationSessionStats.at(kter->first)); + localizationMultiStats.at(kter->first).rbegin()->second.push_back(values); + localizationSessionStats.at(kter->first).clear(); + } - previousStamp = s; - } + previousStamp = s; + } - if(!uContains(localizationSessionStats, jter->first)) - { - localizationSessionStats.insert(std::make_pair(jter->first, std::vector())); + if(!uContains(localizationSessionStats, jter->first)) + { + localizationSessionStats.insert(std::make_pair(jter->first, std::vector())); + } + localizationSessionStats.at(jter->first).push_back(y); } - localizationSessionStats.at(jter->first).push_back(y); } } + previousStamp = s; } - previousStamp = s; } } } @@ -667,12 +788,54 @@ int main(int argc, char * argv[]) std::multimap links; std::multimap allLinks; driver->getAllLinks(allLinks, true, true); + if(ignoreInterNodes) + { + std::multimap allBiLinks; + for(std::multimap::iterator jter=allLinks.begin(); jter!=allLinks.end(); ++jter) + { + if(jter->second.from() != jter->second.to() && + (jter->second.type() == Link::kNeighbor || + jter->second.type() == Link::kNeighborMerged)) + { + allBiLinks.insert(std::make_pair(jter->second.to(), jter->second.inverse())); + } + allBiLinks.insert(*jter); + } + allLinks=allBiLinks; + } std::multimap loopClosureLinks; for(std::multimap::iterator jter=allLinks.begin(); jter!=allLinks.end(); ++jter) { if(jter->second.from() == jter->second.to() || graph::findLink(links, jter->second.from(), jter->second.to(), true) == links.end()) { - links.insert(*jter); + Link link = jter->second; + // remove intermediate nodes? + if(link.from() != link.to() && + ignoreInterNodes && + (link.type() == Link::kNeighbor || + link.type() == Link::kNeighborMerged)) + { + while(uContains(allWeights, link.to()) && allWeights.at(link.to()) < 0) + { + std::multimap::iterator uter = allLinks.find(link.to()); + while(uter != allLinks.end() && + uter->first==link.to() && + uter->second.from()>uter->second.to()) + { + ++uter; + } + if(uter != allLinks.end()) + { + link = link.merge(uter->second, uter->second.type()); + allLinks.erase(uter->first); + } + else + { + break; + } + } + } + links.insert(std::make_pair(jter->first, link)); } if( jter->second.type() != Link::kNeighbor && jter->second.type() != Link::kNeighborMerged && @@ -860,19 +1023,19 @@ int main(int argc, char * argv[]) } else { - std::vector gtPoses; + std::vector gtPosesTmp; std::vector rPoses; for(std::map::iterator iter=poses.begin(); iter!=poses.end(); ++iter) { if(groundTruth.find(iter->first) != groundTruth.end()) { - gtPoses.push_back(groundTruth.at(iter->first)); + gtPosesTmp.push_back(groundTruth.at(iter->first)); rPoses.push_back(poses.at(iter->first)); } } - if(!gtPoses.empty()) + if(!gtPosesTmp.empty()) { - graph::calcRelativeErrors(gtPoses, rPoses, relative_t_err, relative_r_err); + graph::calcRelativeErrors(gtPosesTmp, rPoses, relative_t_err, relative_r_err); } } } @@ -1010,12 +1173,12 @@ int main(int argc, char * argv[]) } } } - printf(" %s (%d, s=%.3f):\terror lin=%.3fm (max=%.3fm, odom=%.3fm) ang=%.1fdeg%s%s, %s: avg=%dms (max=%dms) loops=%d%s, odom: avg=%dms (max=%dms), camera: avg=%dms, %smap=%dMB\n", + printf(" %s (%d, s=%.3f):\terror lin=%.3fm (max=%s, odom=%.3fm) ang=%.1fdeg%s%s, %s: avg=%dms (max=%dms) loops=%d%s, odom: avg=%dms (max=%dms), camera: avg=%dms, %smap=%dMB\n", fileName.c_str(), (int)ids.size(), bestScale, bestRMSE, - maxRMSE, + maxRMSE!=-1?uFormat("%.3fm", maxRMSE).c_str():"NA", bestVoRMSE, bestRMSEAng, !outputKittiError?"":uFormat(", KITTI: t_err=%.2f%% r_err=%.2f deg/100m", kitti_t_err, kitti_r_err*100).c_str(), diff --git a/tools/Reprocess/CMakeLists.txt b/tools/Reprocess/CMakeLists.txt index c5e707bd53..18ec5704ce 100644 --- a/tools/Reprocess/CMakeLists.txt +++ b/tools/Reprocess/CMakeLists.txt @@ -1,46 +1,7 @@ -SET(RTABMap_INCLUDE_DIRS - ${PROJECT_BINARY_DIR}/corelib/include - ${PROJECT_SOURCE_DIR}/utilite/include - ${PROJECT_SOURCE_DIR}/corelib/include -) -SET(RTABMap_LIBRARIES - rtabmap_core - rtabmap_utilite -) - -if(POLICY CMP0020) - cmake_policy(SET CMP0020 NEW) -endif() - -SET(INCLUDE_DIRS - ${RTABMap_INCLUDE_DIRS} - ${OpenCV_INCLUDE_DIRS} - ${PCL_INCLUDE_DIRS} -) - -SET(LIBRARIES - ${RTABMap_LIBRARIES} - ${OpenCV_LIBRARIES} - ${PCL_LIBRARIES} -) - -IF(octomap_FOUND) - SET(INCLUDE_DIRS - ${INCLUDE_DIRS} - ${OCTOMAP_INCLUDE_DIRS} - ) - SET(LIBRARIES - ${LIBRARIES} - ${OCTOMAP_LIBRARIES} - ) -ENDIF(octomap_FOUND) - -INCLUDE_DIRECTORIES(${INCLUDE_DIRS}) - ADD_EXECUTABLE(reprocess main.cpp) -TARGET_LINK_LIBRARIES(reprocess ${LIBRARIES}) +TARGET_LINK_LIBRARIES(reprocess rtabmap_core) SET_TARGET_PROPERTIES( reprocess PROPERTIES OUTPUT_NAME ${PROJECT_PREFIX}-reprocess) diff --git a/tools/Reprocess/main.cpp b/tools/Reprocess/main.cpp index f2099dbacb..21b00bc206 100644 --- a/tools/Reprocess/main.cpp +++ b/tools/Reprocess/main.cpp @@ -35,6 +35,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include +#include +#include #include #include #include @@ -67,6 +69,11 @@ void showUsage() " -c \"path.ini\" Configuration file, overwriting parameters read \n" " from the database. If custom parameters are also set as \n" " arguments, they overwrite those in config file and the database.\n" + " -default Input database's parameters are ignored, using default ones instead.\n" + " -odom Recompute odometry. See \"Odom/\" parameters with --params. If -skip option\n" + " is used, it will be applied to odometry frames, not rtabmap frames. Multi-session\n" + " cannot be detected in this mode (assuming the database contains continuous frames\n" + " of a single session).\n" " -start # Start from this node ID.\n" " -stop # Last node to process.\n" " -start_s # Start from this map session ID.\n" @@ -75,6 +82,7 @@ void showUsage() " then next databases are reprocessed on top of the first one.\n" " -cam # Camera index to stream. Ignored if a database doesn't contain multi-camera data.\n" " -nolandmark Don't republish landmarks contained in input database.\n" + " -nopriors Don't republish priors contained in input database.\n" " -pub_loops Republish loop closures contained in input database.\n" " -loc_null On localization mode, reset localization pose to null and map correction to identity between sessions.\n" " -gt When reprocessing a single database, load its original optimized graph, then \n" @@ -229,6 +237,8 @@ int main(int argc, char * argv[]) bool assemble2dOctoMap = false; bool assemble3dOctoMap = false; bool useDatabaseRate = false; + bool useDefaultParameters = false; + bool recomputeOdometry = false; int startId = 0; int stopId = 0; int startMapId = 0; @@ -237,6 +247,7 @@ int main(int argc, char * argv[]) int cameraIndex = -1; int framesToSkip = 0; bool ignoreLandmarks = false; + bool ignorePriors = false; bool republishLoopClosures = false; bool locNull = false; bool originalGraphAsGT = false; @@ -274,6 +285,15 @@ int main(int argc, char * argv[]) showUsage(); } } + else if(strcmp(argv[i], "-default") == 0 || strcmp(argv[i], "--default") == 0) + { + useDefaultParameters = true; + printf("Using default parameters.\n"); + } + else if(strcmp(argv[i], "-odom") == 0 || strcmp(argv[i], "--odom") == 0) + { + recomputeOdometry = true; + } else if (strcmp(argv[i], "-start") == 0 || strcmp(argv[i], "--start") == 0) { ++i; @@ -368,6 +388,11 @@ int main(int argc, char * argv[]) ignoreLandmarks = true; printf("Ignoring landmarks from input database (-nolandmark option).\n"); } + else if(strcmp(argv[i], "-nopriors") == 0 || strcmp(argv[i], "--nopriors") == 0) + { + ignorePriors = true; + printf("Ignoring priors from input database (-nopriors option).\n"); + } else if(strcmp(argv[i], "-pub_loops") == 0 || strcmp(argv[i], "--pub_loops") == 0) { republishLoopClosures = true; @@ -556,13 +581,19 @@ int main(int argc, char * argv[]) return -1; } - ParametersMap parameters = dbDriver->getLastParameters(); - std::string targetVersion = dbDriver->getDatabaseVersion(); - parameters.insert(ParametersPair(Parameters::kDbTargetVersion(), targetVersion)); - if(parameters.empty()) + ParametersMap parameters; + std::string targetVersion; + if(!useDefaultParameters) { - printf("WARNING: Failed getting parameters from database, reprocessing will be done with default parameters! Database version may be too old (%s).\n", dbDriver->getDatabaseVersion().c_str()); + parameters = dbDriver->getLastParameters(); + targetVersion = dbDriver->getDatabaseVersion(); + parameters.insert(ParametersPair(Parameters::kDbTargetVersion(), targetVersion)); + if(parameters.empty()) + { + printf("WARNING: Failed getting parameters from database, reprocessing will be done with default parameters! Database version may be too old (%s).\n", dbDriver->getDatabaseVersion().c_str()); + } } + if(customParameters.size()) { printf("Custom parameters:\n"); @@ -742,7 +773,8 @@ int main(int argc, char * argv[]) ignoreLandmarks, !useOdomFeatures, startMapId, - stopMapId); + stopMapId, + ignorePriors); dbReader->init(); @@ -757,6 +789,28 @@ int main(int argc, char * argv[]) Parameters::parse(parameters, Parameters::kRGBDLinearUpdate(), linearUpdate); Parameters::parse(parameters, Parameters::kRGBDAngularUpdate(), angularUpdate); + Odometry * odometry = 0; + float rtabmapUpdateRate = Parameters::defaultRtabmapDetectionRate(); + double lastUpdateStamp = 0; + if(recomputeOdometry) + { + if(odometryIgnored) + { + printf("odom option is set but %s parameter is false, odometry won't be recomputed...\n", Parameters::kRGBDEnabled().c_str()); + recomputeOdometry = false; + } + else + { + printf("Odometry will be recomputed (odom option is set)\n"); + Parameters::parse(parameters, Parameters::kRtabmapDetectionRate(), rtabmapUpdateRate); + if(rtabmapUpdateRate!=0) + { + rtabmapUpdateRate = 1.0f/rtabmapUpdateRate; + } + odometry = Odometry::create(parameters); + } + } + printf("Reprocessing data of \"%s\"...\n", inputDatabasePath.c_str()); std::map globalMapStats; int processed = 0; @@ -773,6 +827,44 @@ int main(int argc, char * argv[]) bool inMotion = true; while(data.isValid() && g_loopForever) { + if(recomputeOdometry) + { + OdometryInfo odomInfo; + Transform pose = odometry->process(data, &odomInfo); + printf("Processed %d/%d frames (visual=%d/%d lidar=%f lost=%s)... odometry = %dms\n", + processed+1, + totalIds, + odomInfo.reg.inliers, + odomInfo.reg.matches, + odomInfo.reg.icpInliersRatio, + odomInfo.lost?"true":"false", + int(odomInfo.timeEstimation * 1000)); + if(lastUpdateStamp > 0.0 && data.stamp() < lastUpdateStamp + rtabmapUpdateRate) + { + if(framesToSkip>0) + { + int skippedFrames = framesToSkip; + while(skippedFrames-- > 0) + { + ++processed; + data = dbReader->takeImage(); + } + } + + data = dbReader->takeImage(&info); + if(scanFromDepth) + { + data.setLaserScan(LaserScan()); + } + camThread.postUpdate(&data, &info); + ++processed; + continue; + } + info.odomPose = pose; + info.odomCovariance = odomInfo.reg.covariance; + lastUpdateStamp = data.stamp(); + } + UTimer iterationTime; std::string status; if(!odometryIgnored && info.odomPose.isNull()) @@ -986,11 +1078,12 @@ int main(int argc, char * argv[]) Transform odomPose = info.odomPose; - if(framesToSkip>0) + if(framesToSkip>0 && !recomputeOdometry) { int skippedFrames = framesToSkip; while(skippedFrames-- > 0) { + processed++; data = dbReader->takeImage(&info); if(!odometryIgnored && !info.odomCovariance.empty() && info.odomCovariance.at(0,0)>=9999) { @@ -1058,6 +1151,8 @@ int main(int argc, char * argv[]) rtabmap.close(true); printf("Closing database \"%s\"... done!\n", outputDatabasePath.c_str()); + delete odometry; + if(assemble2dMap) { std::string outputPath = outputDatabasePath.substr(0, outputDatabasePath.size()-3) + "_map.pgm"; diff --git a/tools/RgbdDataset/CMakeLists.txt b/tools/RgbdDataset/CMakeLists.txt index 80232c7670..918eb781f2 100644 --- a/tools/RgbdDataset/CMakeLists.txt +++ b/tools/RgbdDataset/CMakeLists.txt @@ -1,54 +1,7 @@ -cmake_minimum_required(VERSION 2.8) - -# inside rtabmap project (see below for external build) -SET(RTABMap_INCLUDE_DIRS - ${PROJECT_BINARY_DIR}/corelib/include - ${PROJECT_SOURCE_DIR}/utilite/include - ${PROJECT_SOURCE_DIR}/corelib/include -) -SET(RTABMap_LIBRARIES - rtabmap_core - rtabmap_utilite -) - -if(POLICY CMP0020) - cmake_policy(SET CMP0020 NEW) -endif() - -SET(INCLUDE_DIRS - ${RTABMap_INCLUDE_DIRS} - ${OpenCV_INCLUDE_DIRS} - ${PCL_INCLUDE_DIRS} -) - -SET(LIBRARIES - ${RTABMap_LIBRARIES} - ${OpenCV_LIBRARIES} - ${PCL_LIBRARIES} -) - -# Hack as CameraRealsense2.h needs realsense2 include dir -IF(realsense2_FOUND) - SET(INCLUDE_DIRS - ${INCLUDE_DIRS} - ${realsense2_INCLUDE_DIRS} - ) -ENDIF(realsense2_FOUND) - -# Hack as CameraK4A.h needs k4a include dir -IF(k4a_FOUND) - SET(INCLUDE_DIRS - ${INCLUDE_DIRS} - ${k4a_INCLUDE_DIRS} - ) -ENDIF(k4a_FOUND) - -INCLUDE_DIRECTORIES(${INCLUDE_DIRS}) ADD_EXECUTABLE(rgbd_dataset main.cpp) -TARGET_LINK_LIBRARIES(rgbd_dataset ${LIBRARIES}) - +TARGET_LINK_LIBRARIES(rgbd_dataset rtabmap_core) SET_TARGET_PROPERTIES( rgbd_dataset PROPERTIES OUTPUT_NAME ${PROJECT_PREFIX}-rgbd_dataset) diff --git a/tools/RgbdDataset/main.cpp b/tools/RgbdDataset/main.cpp index 37e6d36125..4755c72cac 100644 --- a/tools/RgbdDataset/main.cpp +++ b/tools/RgbdDataset/main.cpp @@ -246,7 +246,6 @@ int main(int argc, char * argv[]) ///////////////////////////// // Processing dataset begin ///////////////////////////// - cv::Mat covariance; int odomKeyFrames = 0; double previousStamp = 0.0; int skipCount = 0; @@ -277,6 +276,12 @@ int main(int argc, char * argv[]) odomInfo.reg.covariance = cv::Mat::eye(6,6,CV_64FC1); } } + + if(iteration!=0 && !odomInfo.reg.covariance.empty() && odomInfo.reg.covariance.at(0,0)>=9999) + { + UWARN("Odometry is reset (high variance (%f >=9999 detected). Increment map id!", odomInfo.reg.covariance.at(0,0)); + rtabmap.triggerNewMap(); + } if(odomInfo.keyFrameAdded) { @@ -303,10 +308,6 @@ int main(int argc, char * argv[]) data.setFeatures(std::vector(), std::vector(), cv::Mat());// remove features processData = intermediateNodes; } - if(covariance.empty() || odomInfo.reg.covariance.at(0,0) > covariance.at(0,0)) - { - covariance = odomInfo.reg.covariance; - } timer.restart(); if(processData) @@ -318,6 +319,7 @@ int main(int argc, char * argv[]) externalStats.insert(std::make_pair("Camera/Disparity/ms", cameraInfo.timeDisparity*1000.0f)); externalStats.insert(std::make_pair("Camera/ImageDecimation/ms", cameraInfo.timeImageDecimation*1000.0f)); externalStats.insert(std::make_pair("Camera/Mirroring/ms", cameraInfo.timeMirroring*1000.0f)); + externalStats.insert(std::make_pair("Camera/HistogramEqualization/ms", cameraInfo.timeHistogramEqualization*1000.0f)); externalStats.insert(std::make_pair("Camera/ExposureCompensation/ms", cameraInfo.timeStereoExposureCompensation*1000.0f)); externalStats.insert(std::make_pair("Camera/ScanFromDepth/ms", cameraInfo.timeScanFromDepth*1000.0f)); externalStats.insert(std::make_pair("Camera/TotalTime/ms", cameraInfo.timeTotal*1000.0f)); @@ -337,8 +339,7 @@ int main(int argc, char * argv[]) externalStats.insert(std::make_pair("Odometry/LocalScanMapSize/", odomInfo.localScanMapSize)); OdometryEvent e(SensorData(), Transform(), odomInfo); - rtabmap.process(data, pose, covariance, e.velocity(), externalStats); - covariance = cv::Mat(); + rtabmap.process(data, pose, odomInfo.reg.covariance, e.velocity(), externalStats); } ++iteration; @@ -354,8 +355,6 @@ int main(int argc, char * argv[]) if(rmse >= 0.0f) { - //printf("Iteration %d/%d: camera=%dms, odom(quality=%d/%d, kfs=%d)=%dms, slam=%dms, rmse=%fm, noise stddev=%fm %frad", - // iteration, totalImages, int(cameraInfo.timeTotal*1000.0f), odomInfo.reg.inliers, odomInfo.features, odomKeyFrames, int(odomInfo.timeEstimation*1000.0f), int(slamTime*1000.0f), rmse, sqrt(odomInfo.reg.covariance.at(0,0)), sqrt(odomInfo.reg.covariance.at(3,3))); printf("Iteration %d/%d: camera=%dms, odom(quality=%d/%d, kfs=%d)=%dms, slam=%dms, rmse=%fm", iteration, totalImages, int(cameraInfo.timeTotal*1000.0f), odomInfo.reg.inliers, odomInfo.features, odomKeyFrames, int(odomInfo.timeEstimation*1000.0f), int(slamTime*1000.0f), rmse); } @@ -428,7 +427,7 @@ int main(int argc, char * argv[]) } } - + // compute RMSE statistics float translational_rmse = 0.0f; float translational_mean = 0.0f; diff --git a/tools/StereoEval/CMakeLists.txt b/tools/StereoEval/CMakeLists.txt index 87aa50555e..be6e422110 100644 --- a/tools/StereoEval/CMakeLists.txt +++ b/tools/StereoEval/CMakeLists.txt @@ -1,22 +1,6 @@ -SET(INCLUDE_DIRS - ${PROJECT_BINARY_DIR}/corelib/include - ${PROJECT_SOURCE_DIR}/corelib/include - ${PROJECT_SOURCE_DIR}/utilite/include - ${OpenCV_INCLUDE_DIRS} - ${PCL_INCLUDE_DIRS} -) - -SET(LIBRARIES - ${OpenCV_LIBRARIES} - ${PCL_LIBRARIES} -) - -add_definitions(${PCL_DEFINITIONS}) - -INCLUDE_DIRECTORIES(${INCLUDE_DIRS}) ADD_EXECUTABLE(stereoEval main.cpp) -TARGET_LINK_LIBRARIES(stereoEval rtabmap_core rtabmap_utilite ${LIBRARIES}) +TARGET_LINK_LIBRARIES(stereoEval rtabmap_core) SET_TARGET_PROPERTIES( stereoEval PROPERTIES OUTPUT_NAME ${PROJECT_PREFIX}-stereoEval) diff --git a/tools/VocabularyComparison/CMakeLists.txt b/tools/VocabularyComparison/CMakeLists.txt index ed35daf119..16bbcc076b 100644 --- a/tools/VocabularyComparison/CMakeLists.txt +++ b/tools/VocabularyComparison/CMakeLists.txt @@ -1,19 +1,6 @@ -SET(INCLUDE_DIRS - ${PROJECT_BINARY_DIR}/corelib/include - ${PROJECT_SOURCE_DIR}/utilite/include - ${PROJECT_SOURCE_DIR}/corelib/include - ${OpenCV_INCLUDE_DIRS} -) - -SET(LIBRARIES - ${OpenCV_LIBRARIES} -) - -INCLUDE_DIRECTORIES(${INCLUDE_DIRS}) - ADD_EXECUTABLE(vocabularyComparison main.cpp) -TARGET_LINK_LIBRARIES(vocabularyComparison rtabmap_core rtabmap_utilite ${LIBRARIES}) +TARGET_LINK_LIBRARIES(vocabularyComparison rtabmap_core) SET_TARGET_PROPERTIES( vocabularyComparison PROPERTIES OUTPUT_NAME ${PROJECT_PREFIX}-vocabularyComparison) diff --git a/utilite/include/rtabmap/utilite/UConversion.h b/utilite/include/rtabmap/utilite/UConversion.h index 024d09b0c7..705cee3a72 100644 --- a/utilite/include/rtabmap/utilite/UConversion.h +++ b/utilite/include/rtabmap/utilite/UConversion.h @@ -20,7 +20,7 @@ #ifndef UCONVERSION_H #define UCONVERSION_H -#include "rtabmap/utilite/UtiLiteExp.h" // DLL export/import defines +#include "rtabmap/utilite/utilite_export.h" // DLL export/import defines #include #include @@ -48,7 +48,7 @@ * @param after the new character replacing the old one * @return the modified string */ -std::string UTILITE_EXP uReplaceChar(const std::string & str, char before, char after); +std::string UTILITE_EXPORT uReplaceChar(const std::string & str, char before, char after); /** * Replace old characters in a string with the specified string. @@ -64,7 +64,7 @@ std::string UTILITE_EXP uReplaceChar(const std::string & str, char before, char * @param after the new string replacing the old character * @return the modified string */ -std::string UTILITE_EXP uReplaceChar(const std::string & str, char before, const std::string & after); +std::string UTILITE_EXPORT uReplaceChar(const std::string & str, char before, const std::string & after); /** * Transform characters from a string to upper case. @@ -77,7 +77,7 @@ std::string UTILITE_EXP uReplaceChar(const std::string & str, char before, const * @param str the string * @return the modified string */ -std::string UTILITE_EXP uToUpperCase(const std::string & str); +std::string UTILITE_EXPORT uToUpperCase(const std::string & str); /** * Transform characters from a string to lower case. @@ -90,53 +90,53 @@ std::string UTILITE_EXP uToUpperCase(const std::string & str); * @param str the string * @return the modified string */ -std::string UTILITE_EXP uToLowerCase(const std::string & str); +std::string UTILITE_EXPORT uToLowerCase(const std::string & str); /** * Convert a number (unsigned int) to a string. * @param number the number to convert in a string * @return the string */ -std::string UTILITE_EXP uNumber2Str(unsigned int number); +std::string UTILITE_EXPORT uNumber2Str(unsigned int number); /** * Convert a number (int) to a string. * @param number the number to convert in a string * @return the string */ -std::string UTILITE_EXP uNumber2Str(int number); +std::string UTILITE_EXPORT uNumber2Str(int number); /** * Convert a number (float) to a string. * @param number the number to convert in a string * @return the string */ -std::string UTILITE_EXP uNumber2Str(float number, int precision=6, bool fixed = false); +std::string UTILITE_EXPORT uNumber2Str(float number, int precision=6, bool fixed = false); /** * Convert a number (double) to a string. * @param number the number to convert in a string * @return the string */ -std::string UTILITE_EXP uNumber2Str(double number, int precision=6, bool fixed = false); +std::string UTILITE_EXPORT uNumber2Str(double number, int precision=6, bool fixed = false); /** * Convert a string to an integer. * @param the string * @return the number */ -int UTILITE_EXP uStr2Int(const std::string & str); +int UTILITE_EXPORT uStr2Int(const std::string & str); /** * Convert a string to a float independent of the locale (comma/dot). * @param the string * @return the number */ -float UTILITE_EXP uStr2Float(const std::string & str); +float UTILITE_EXPORT uStr2Float(const std::string & str); /** * Convert a string to a double independent of the locale (comma/dot). * @param the string * @return the number */ -double UTILITE_EXP uStr2Double(const std::string & str); +double UTILITE_EXPORT uStr2Double(const std::string & str); /** @@ -145,7 +145,7 @@ double UTILITE_EXP uStr2Double(const std::string & str); * @param boolean the boolean to convert in a string * @return the string */ -std::string UTILITE_EXP uBool2Str(bool boolean); +std::string UTILITE_EXPORT uBool2Str(bool boolean); /** * Convert a string to a boolean. * The format used is : @@ -153,22 +153,22 @@ std::string UTILITE_EXP uBool2Str(bool boolean); * @param str the string to convert in a boolean * @return the boolean */ -bool UTILITE_EXP uStr2Bool(const char * str); -bool UTILITE_EXP uStr2Bool(const std::string & str); +bool UTILITE_EXPORT uStr2Bool(const char * str); +bool UTILITE_EXPORT uStr2Bool(const std::string & str); /** * Convert a string to an array of bytes including the null character ('\0'). * @param str the string * @return the array of bytes */ -std::vector UTILITE_EXP uStr2Bytes(const std::string & str); +std::vector UTILITE_EXPORT uStr2Bytes(const std::string & str); /** * Convert an array of bytes to string, the array of bytes must end with the null character ('\0'). * @param bytes the array of bytes * @return the string */ -std::string UTILITE_EXP uBytes2Str(const std::vector & bytes); +std::string UTILITE_EXPORT uBytes2Str(const std::vector & bytes); /** * Convert a bytes array to an hexadecimal string. @@ -185,7 +185,7 @@ std::string UTILITE_EXP uBytes2Str(const std::vector & bytes); * @param bytesLen the length of the bytes array * @return the hexadecimal string */ -std::string UTILITE_EXP uBytes2Hex(const char * bytes, unsigned int bytesLen); +std::string UTILITE_EXPORT uBytes2Hex(const char * bytes, unsigned int bytesLen); /** * Convert an hexadecimal string to a bytes array. * The string must be pair length. The hexadecimal @@ -200,7 +200,7 @@ std::string UTILITE_EXP uBytes2Hex(const char * bytes, unsigned int bytesLen); * @param hex the hexadecimal string * @return the bytes array */ -std::vector UTILITE_EXP uHex2Bytes(const std::string & hex); +std::vector UTILITE_EXPORT uHex2Bytes(const std::string & hex); /** * Convert an hexadecimal string to a bytes array. * The string must be pair length. The hexadecimal @@ -215,7 +215,7 @@ std::vector UTILITE_EXP uHex2Bytes(const std::string & hex); * @param bytesLen the hexadecimal string length * @return the bytes array */ -std::vector UTILITE_EXP uHex2Bytes(const char * hex, int hexLen); +std::vector UTILITE_EXPORT uHex2Bytes(const char * hex, int hexLen); /** * Convert an hexadecimal string to an ascii string. A convenient way @@ -233,7 +233,7 @@ std::vector UTILITE_EXP uHex2Bytes(const char * hex, int hexLen); * @param hex the hexadecimal string * @return the ascii string */ -std::string UTILITE_EXP uHex2Str(const std::string & hex); +std::string UTILITE_EXPORT uHex2Str(const std::string & hex); /** * Convert hexadecimal (left or right part) value to an ascii character. @@ -247,7 +247,7 @@ std::string UTILITE_EXP uHex2Str(const std::string & hex); * @param rightPart If we want the character corresponding to the right of left part (4 bits) of the byte value. * @return the ascii character (in upper case) */ -unsigned char UTILITE_EXP uHex2Ascii(const unsigned char & c, bool rightPart); +unsigned char UTILITE_EXPORT uHex2Ascii(const unsigned char & c, bool rightPart); /** * Convert an ascii character to an hexadecimal value (right 4 bits). @@ -261,30 +261,30 @@ unsigned char UTILITE_EXP uHex2Ascii(const unsigned char & c, bool rightPart); * @param c the ascii character * @return the hexadecimal value */ -unsigned char UTILITE_EXP uAscii2Hex(const unsigned char & c); +unsigned char UTILITE_EXPORT uAscii2Hex(const unsigned char & c); /** * Format a string like printf, and return it as a std::string */ -std::string UTILITE_EXP uFormatv (const char *fmt, va_list ap); +std::string UTILITE_EXPORT uFormatv (const char *fmt, va_list ap); /** * Format a string like printf, and return it as a std::string */ -std::string UTILITE_EXP uFormat (const char *fmt, ...); +std::string UTILITE_EXPORT uFormat (const char *fmt, ...); #ifdef _WIN32 /** * Convert multi-byte string to unicode (wide-char) string. * Note that returned whar_t * must be deleted : delete [] wText; */ -UTILITE_EXP wchar_t * createWCharFromChar(const char * text); +UTILITE_EXPORT wchar_t * createWCharFromChar(const char * text); /** * Convert unicode (wide-char) string to multi-byte string. * Note that returned char * must be deleted : delete [] text; */ -UTILITE_EXP char * createCharFromWChar(const wchar_t * wText); +UTILITE_EXPORT char * createCharFromWChar(const wchar_t * wText); #endif #endif /* UCONVERSION_H */ diff --git a/utilite/include/rtabmap/utilite/UDestroyer.h b/utilite/include/rtabmap/utilite/UDestroyer.h index 08e665a6a7..187320f82f 100644 --- a/utilite/include/rtabmap/utilite/UDestroyer.h +++ b/utilite/include/rtabmap/utilite/UDestroyer.h @@ -20,8 +20,6 @@ #ifndef UDESTROYER_H #define UDESTROYER_H -#include "rtabmap/utilite/UtiLiteExp.h" // DLL export/import defines - /** * This class is used to delete a dynamically created * objects. It was mainly designed to remove dynamically created Singleton. diff --git a/utilite/include/rtabmap/utilite/UDirectory.h b/utilite/include/rtabmap/utilite/UDirectory.h index caed174c98..9b52fbc837 100644 --- a/utilite/include/rtabmap/utilite/UDirectory.h +++ b/utilite/include/rtabmap/utilite/UDirectory.h @@ -20,7 +20,7 @@ #ifndef UDIRECTORY_H #define UDIRECTORY_H -#include "rtabmap/utilite/UtiLiteExp.h" // DLL export/import defines +#include "rtabmap/utilite/utilite_export.h" // DLL export/import defines #include #include @@ -31,7 +31,7 @@ * * This class can be used to get file names in a directory. */ -class UTILITE_EXP UDirectory +class UTILITE_EXPORT UDirectory { public: /** diff --git a/utilite/include/rtabmap/utilite/UEvent.h b/utilite/include/rtabmap/utilite/UEvent.h index 6dc5b61d80..68d087cdb4 100644 --- a/utilite/include/rtabmap/utilite/UEvent.h +++ b/utilite/include/rtabmap/utilite/UEvent.h @@ -20,7 +20,7 @@ #ifndef UEVENT_H #define UEVENT_H -#include "rtabmap/utilite/UtiLiteExp.h" // DLL export/import defines +#include "rtabmap/utilite/utilite_export.h" // DLL export/import defines #include @@ -54,7 +54,7 @@ class UEventsHandler; * @see UEventsHandler * @see getClassName() */ -class UTILITE_EXP UEvent{ +class UTILITE_EXPORT UEvent{ public: virtual ~UEvent() {} diff --git a/utilite/include/rtabmap/utilite/UEventsHandler.h b/utilite/include/rtabmap/utilite/UEventsHandler.h index b80b6fea8d..6cbeb2d6bb 100644 --- a/utilite/include/rtabmap/utilite/UEventsHandler.h +++ b/utilite/include/rtabmap/utilite/UEventsHandler.h @@ -20,7 +20,7 @@ #ifndef UEVENTSHANDLER_H #define UEVENTSHANDLER_H -#include "rtabmap/utilite/UtiLiteExp.h" // DLL export/import defines +#include "rtabmap/utilite/utilite_export.h" // DLL export/import defines #include "rtabmap/utilite/UEventsSender.h" class UEvent; @@ -125,7 +125,7 @@ class UEvent; * @see UThreadNode * */ -class UTILITE_EXP UEventsHandler : public UEventsSender { +class UTILITE_EXPORT UEventsHandler : public UEventsSender { public: void registerToEventsManager(); diff --git a/utilite/include/rtabmap/utilite/UEventsManager.h b/utilite/include/rtabmap/utilite/UEventsManager.h index b4a51efae4..02a78de9f1 100644 --- a/utilite/include/rtabmap/utilite/UEventsManager.h +++ b/utilite/include/rtabmap/utilite/UEventsManager.h @@ -20,7 +20,7 @@ #ifndef UEVENTSMANAGER_H #define UEVENTSMANAGER_H -#include "rtabmap/utilite/UtiLiteExp.h" // DLL export/import defines +#include "rtabmap/utilite/utilite_export.h" // DLL export/import defines #include "rtabmap/utilite/UEventsHandler.h" #include "rtabmap/utilite/UThreadNode.h" @@ -75,7 +75,7 @@ class UEventDispatcher : public UThread * @see addHandler() * @see removeHandler() */ -class UTILITE_EXP UEventsManager : public UThread{ +class UTILITE_EXPORT UEventsManager : public UThread{ public: diff --git a/utilite/include/rtabmap/utilite/UEventsSender.h b/utilite/include/rtabmap/utilite/UEventsSender.h index fc5d7eeb51..9dfe98e18f 100644 --- a/utilite/include/rtabmap/utilite/UEventsSender.h +++ b/utilite/include/rtabmap/utilite/UEventsSender.h @@ -8,11 +8,11 @@ #ifndef UEVENTSSENDER_H_ #define UEVENTSSENDER_H_ -#include "rtabmap/utilite/UtiLiteExp.h" // DLL export/import defines +#include "rtabmap/utilite/utilite_export.h" // DLL export/import defines class UEvent; -class UTILITE_EXP UEventsSender +class UTILITE_EXPORT UEventsSender { public: UEventsSender(){} diff --git a/utilite/include/rtabmap/utilite/UFile.h b/utilite/include/rtabmap/utilite/UFile.h index 610178fb77..c5021d41d1 100644 --- a/utilite/include/rtabmap/utilite/UFile.h +++ b/utilite/include/rtabmap/utilite/UFile.h @@ -20,7 +20,7 @@ #ifndef FILE_H #define FILE_H -#include "rtabmap/utilite/UtiLiteExp.h" // DLL export/import defines +#include "rtabmap/utilite/utilite_export.h" // DLL export/import defines #include "rtabmap/utilite/UDirectory.h" #include @@ -30,7 +30,7 @@ * * This class can be used to modify/erase files on hard drive. */ -class UTILITE_EXP UFile +class UTILITE_EXPORT UFile { public: /** diff --git a/utilite/include/rtabmap/utilite/ULogger.h b/utilite/include/rtabmap/utilite/ULogger.h index 20f2f46a7f..dd8dae4114 100644 --- a/utilite/include/rtabmap/utilite/ULogger.h +++ b/utilite/include/rtabmap/utilite/ULogger.h @@ -20,7 +20,7 @@ #ifndef ULOGGER_H #define ULOGGER_H -#include "rtabmap/utilite/UtiLiteExp.h" // DLL export/import defines +#include "rtabmap/utilite/utilite_export.h" // DLL export/import defines #include "rtabmap/utilite/UMutex.h" #include "rtabmap/utilite/UDestroyer.h" @@ -226,7 +226,7 @@ class ULogEvent : public UEvent * @see UDEBUG(), UINFO(), UWARN(), UERROR(), UFATAL() * */ -class UTILITE_EXP ULogger +class UTILITE_EXPORT ULogger { public: diff --git a/utilite/include/rtabmap/utilite/UMath.h b/utilite/include/rtabmap/utilite/UMath.h index bd03632268..ac15c43ae7 100644 --- a/utilite/include/rtabmap/utilite/UMath.h +++ b/utilite/include/rtabmap/utilite/UMath.h @@ -24,8 +24,6 @@ \brief Basic mathematics functions */ -#include "rtabmap/utilite/UtiLiteExp.h" // DLL export/import defines - #include #include #include diff --git a/utilite/include/rtabmap/utilite/UProcessInfo.h b/utilite/include/rtabmap/utilite/UProcessInfo.h index 1b54fd3b38..66d2b3a873 100644 --- a/utilite/include/rtabmap/utilite/UProcessInfo.h +++ b/utilite/include/rtabmap/utilite/UProcessInfo.h @@ -20,13 +20,13 @@ #ifndef UPROCESSINFO_H #define UPROCESSINFO_H -#include "rtabmap/utilite/UtiLiteExp.h" // DLL export/import defines +#include "rtabmap/utilite/utilite_export.h" // DLL export/import defines /** * This class is used to get some informations * about the current process. */ -class UTILITE_EXP UProcessInfo { +class UTILITE_EXPORT UProcessInfo { public: UProcessInfo(); virtual ~UProcessInfo(); diff --git a/utilite/include/rtabmap/utilite/UThread.h b/utilite/include/rtabmap/utilite/UThread.h index c0e3d48fe9..2d5781f2e0 100644 --- a/utilite/include/rtabmap/utilite/UThread.h +++ b/utilite/include/rtabmap/utilite/UThread.h @@ -20,7 +20,7 @@ #ifndef UTHREADNODE_H #define UTHREADNODE_H -#include "rtabmap/utilite/UtiLiteExp.h" // DLL export/import defines +#include "rtabmap/utilite/utilite_export.h" // DLL export/import defines #include "rtabmap/utilite/UThreadC.h" @@ -83,7 +83,7 @@ * @see mainLoop() * */ -class UTILITE_EXP UThread : public UThreadC +class UTILITE_EXPORT UThread : public UThreadC { public: /** diff --git a/utilite/include/rtabmap/utilite/UTimer.h b/utilite/include/rtabmap/utilite/UTimer.h index 6120630d5d..42f6d2dd74 100644 --- a/utilite/include/rtabmap/utilite/UTimer.h +++ b/utilite/include/rtabmap/utilite/UTimer.h @@ -20,7 +20,7 @@ #ifndef UTIMER_H #define UTIMER_H -#include "rtabmap/utilite/UtiLiteExp.h" // DLL export/import defines +#include "rtabmap/utilite/utilite_export.h" // DLL export/import defines #ifdef _WIN32 #include @@ -43,7 +43,7 @@ * ... * @endcode */ -class UTILITE_EXP UTimer +class UTILITE_EXPORT UTimer { public: UTimer(); @@ -81,7 +81,7 @@ class UTILITE_EXP UTimer * @return double the interval in seconds. * @deprecated use elapsed() instead. */ - UTILITE_DEPRECATED(double getInterval()); + UTILITE_DEPRECATED double getInterval(); /** * This method is used to get the interval of diff --git a/utilite/include/rtabmap/utilite/UVariant.h b/utilite/include/rtabmap/utilite/UVariant.h index 94b6ed18fd..faa6e26831 100644 --- a/utilite/include/rtabmap/utilite/UVariant.h +++ b/utilite/include/rtabmap/utilite/UVariant.h @@ -20,14 +20,14 @@ #ifndef UVARIANT_H #define UVARIANT_H -#include "rtabmap/utilite/UtiLiteExp.h" // DLL export/import defines +#include "rtabmap/utilite/utilite_export.h" // DLL export/import defines #include #include /** * Experimental class... */ -class UTILITE_EXP UVariant +class UTILITE_EXPORT UVariant { public: enum Type{ diff --git a/utilite/include/rtabmap/utilite/UtiLiteExp.h b/utilite/include/rtabmap/utilite/UtiLiteExp.h deleted file mode 100644 index 3a208ecfec..0000000000 --- a/utilite/include/rtabmap/utilite/UtiLiteExp.h +++ /dev/null @@ -1,46 +0,0 @@ -/* -* utilite is a cross-platform library with -* useful utilities for fast and small developing. -* Copyright (C) 2010 Mathieu Labbe -* -* utilite is free library: you can redistribute it and/or modify -* it under the terms of the GNU Lesser General Public License as published by -* the Free Software Foundation, either version 3 of the License, or -* (at your option) any later version. -* -* utilite is distributed in the hope that it will be useful, -* but WITHOUT ANY WARRANTY; without even the implied warranty of -* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -* GNU Lesser General Public License for more details. -* -* You should have received a copy of the GNU Lesser General Public License -* along with this program. If not, see . -*/ - -#ifndef UTILITEEXP_H -#define UTILITEEXP_H - -/** - * Used mainly on Windows for dynamic linked libraries (dll). - */ -#if defined(_WIN32) - #if defined(rtabmap_utilite_EXPORTS) - #define UTILITE_EXP __declspec( dllexport ) - #else - #define UTILITE_EXP __declspec( dllimport ) - #endif -#else - #define UTILITE_EXP -#endif - -#ifdef __GNUC__ -#define UTILITE_DEPRECATED(func) func __attribute__ ((deprecated)) -#elif defined(_MSC_VER) -#define UTILITE_DEPRECATED(func) __declspec(deprecated) func -#else -#pragma message("WARNING: You need to implement DEPRECATED for this compiler") -#define UTILITE_DEPRECATED(func) func -#endif - -#endif - diff --git a/utilite/include/rtabmap/utilite/Win32/UThreadC.h b/utilite/include/rtabmap/utilite/Win32/UThreadC.h index 356b1f9f2e..5395e7f70c 100644 --- a/utilite/include/rtabmap/utilite/Win32/UThreadC.h +++ b/utilite/include/rtabmap/utilite/Win32/UThreadC.h @@ -36,6 +36,7 @@ #ifndef _U_Thread_Win32_ #define _U_Thread_Win32_ +#include "rtabmap/utilite/utilite_export.h" #include "rtabmap/utilite/Win32/UWin32.h" #include "rtabmap/utilite/USemaphore.h" #include "rtabmap/utilite/UMutex.h" @@ -87,7 +88,7 @@ template < typename Thread_T > -class UTILITE_EXP UThreadC +class UTILITE_EXPORT UThreadC { private: struct Instance; @@ -241,7 +242,7 @@ class UTILITE_EXP UThreadC // Explicit Specialization of void // template<> -class UTILITE_EXP UThreadC +class UTILITE_EXPORT UThreadC { private: struct Instance; diff --git a/utilite/include/rtabmap/utilite/Win32/UWin32.h b/utilite/include/rtabmap/utilite/Win32/UWin32.h index d21a745125..49d02b6955 100644 --- a/utilite/include/rtabmap/utilite/Win32/UWin32.h +++ b/utilite/include/rtabmap/utilite/Win32/UWin32.h @@ -9,8 +9,6 @@ #ifndef _U_Win32_ #define _U_Win32_ -#include "rtabmap/utilite/UtiLiteExp.h" - #if !defined(_WINDOWS_) // WIN32 Excludes #ifdef WIN32_LEAN_AND_MEAN diff --git a/utilite/src/CMakeLists.txt b/utilite/src/CMakeLists.txt index f174fc2a39..dfdfad56f5 100644 --- a/utilite/src/CMakeLists.txt +++ b/utilite/src/CMakeLists.txt @@ -13,36 +13,62 @@ SET(SRC_FILES UVariant.cpp ) -SET(INCLUDE_DIRS - ${CMAKE_CURRENT_SOURCE_DIR}/../include - ${PTHREADS_INCLUDE_DIR} -) +ADD_LIBRARY(rtabmap_utilite ${SRC_FILES}) +ADD_LIBRARY(rtabmap::utilite ALIAS rtabmap_utilite) -# Make sure the compiler can find include files from our library. -INCLUDE_DIRECTORIES(${INCLUDE_DIRS}) +generate_export_header(rtabmap_utilite + BASE_NAME utilite) + +target_include_directories(rtabmap_utilite PUBLIC + "$" + "$") -ADD_LIBRARY(rtabmap_utilite ${SRC_FILES}) IF(MINGW) - TARGET_LINK_LIBRARIES(rtabmap_utilite ${PTHREADS_LIBRARY} "-lpsapi") + TARGET_LINK_LIBRARIES(rtabmap_utilite PRIVATE ${PTHREADS_LIBRARY} "-lpsapi") ELSEIF(WIN32 OR MSVC) FIND_LIBRARY(PSAPI_LIBRARIES NAMES psapi libpsapi.dll.a libpsapi.a libpsapi.lib ) - TARGET_LINK_LIBRARIES(rtabmap_utilite ${PSAPI_LIBRARIES}) + TARGET_LINK_LIBRARIES(rtabmap_utilite PRIVATE ${PSAPI_LIBRARIES}) ELSE() - TARGET_LINK_LIBRARIES(rtabmap_utilite ${PTHREADS_LIBRARY}) + TARGET_LINK_LIBRARIES(rtabmap_utilite PRIVATE ${PTHREADS_LIBRARY}) ENDIF() - SET_TARGET_PROPERTIES( - rtabmap_utilite + rtabmap_utilite PROPERTIES VERSION ${RTABMAP_VERSION} SOVERSION ${RTABMAP_MAJOR_VERSION}.${RTABMAP_MINOR_VERSION} + EXPORT_NAME "utilite" ) - -INSTALL(TARGETS rtabmap_utilite +INSTALL(TARGETS rtabmap_utilite EXPORT rtabmap_utiliteTargets RUNTIME DESTINATION "${CMAKE_INSTALL_BINDIR}" COMPONENT runtime LIBRARY DESTINATION "${CMAKE_INSTALL_LIBDIR}" COMPONENT devel ARCHIVE DESTINATION "${CMAKE_INSTALL_LIBDIR}" COMPONENT devel) - -install(DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/../include/ DESTINATION "${INSTALL_INCLUDE_DIR}" COMPONENT devel FILES_MATCHING PATTERN "*.h" PATTERN ".svn" EXCLUDE) +configure_file( + ${CMAKE_CURRENT_BINARY_DIR}/utilite_export.h + ${CMAKE_CURRENT_BINARY_DIR}/include/${PROJECT_PREFIX}/utilite/utilite_export.h + COPYONLY) + +install( + DIRECTORY + ${CMAKE_CURRENT_SOURCE_DIR}/../include/${PROJECT_PREFIX} + ${CMAKE_CURRENT_BINARY_DIR}/include/${PROJECT_PREFIX} + DESTINATION + "${INSTALL_INCLUDE_DIR}" + COMPONENT + devel + FILES_MATCHING PATTERN "*.h" PATTERN "*.hpp") + +export(EXPORT rtabmap_utiliteTargets + FILE "${CMAKE_CURRENT_BINARY_DIR}/../../${PROJECT_NAME}_utiliteTargets.cmake" + NAMESPACE rtabmap:: +) +install(EXPORT rtabmap_utiliteTargets + FILE + ${PROJECT_NAME}_utiliteTargets.cmake + DESTINATION + ${INSTALL_CMAKE_DIR} + NAMESPACE rtabmap:: + COMPONENT + devel +)