diff --git a/.appveyor.yml b/.appveyor.yml deleted file mode 100644 index 4dc21dc6..00000000 --- a/.appveyor.yml +++ /dev/null @@ -1,152 +0,0 @@ - -branches: - only: - - master - - devel - -os: Visual Studio 2015 - -clone_folder: c:\projects\rtabmap - -platform: x64 -configuration: Release - -init: - - cmake --version - - call "C:\Program Files\Microsoft SDKs\Windows\v7.1\Bin\SetEnv.cmd" /x64 - - 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>=5.1.0 - # 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 - - set PATH=%QTDIR%\bin;%PATH% - # Boost - - set PATH=%PATH%;C:\Libraries\boost_1_62_0\lib64-msvc-14.0 - # Openni2 - - ps: wget 'https://dl.dropboxusercontent.com/s/d98jv79l6oy9fxz/OpenNI2.exe?dl=0' -outfile OpenNI2.exe - - cmd: OpenNI2.exe -o"C:\Program Files" -y - - ECHO "Installed OpenNI2:" - - ps: "ls \"C:/Program Files/OpenNI2\"" - - set PATH=%PATH%;C:\Program Files\OpenNI2\Redist - - set OPENNI2_INCLUDE64=C:\Program Files\OpenNI2\Include - - set OPENNI2_LIB64=C:\Program Files\OpenNI2\Lib - - set OPENNI2_REDIST64=C:\Program Files\OpenNI2\Redist - # OpenCV - #- appveyor-retry appveyor DownloadFile http://downloads.sourceforge.net/project/opencvlibrary/4.5.2/opencv-4.5.2-vc14_vc15.exe - #- cmd: opencv-4.5.2-vc14_vc15.exe -o"C:\Program Files" -y - #- ECHO "Installed OpenCV:" - #- ps: "ls \"C:/Program Files/opencv/build\"" - #- set PATH=%PATH%;C:\Program Files\opencv\build\x64\vc14\bin - - ps: wget 'https://dl.dropboxusercontent.com/s/o6ofn491bc0jso1/opencv450_vc14.exe?dl=0' -outfile opencv.exe - - cmd: opencv.exe -o"C:\Program Files" -y - - ECHO "Installed OpenCV:" - - ps: "ls \"C:/Program Files/opencv\"" - - set PATH=%PATH%;C:\Program Files\opencv\x64\vc14\bin - # VTK (including QVTK) - - ps: wget 'https://dl.dropboxusercontent.com/s/1l33b5l3f3y52gf/VTK-6_3-msvc140.exe?dl=0' -outfile VTK-6_3.exe - - cmd: VTK-6_3.exe -o"C:\Program Files" -y - - ECHO "Installed VTK:" - - ps: "ls \"C:/Program Files/VTK\"" - - set PATH=%PATH%;C:\Program Files\VTK\bin - # QHull - - ps: wget 'https://dl.dropboxusercontent.com/s/9widnk9msdsh2b8/Qhull-msvc140.exe?dl=0' -outfile Qhull.exe - - cmd: Qhull.exe -o"C:\Program Files" -y - - ECHO "Installed QHull:" - - ps: "ls \"C:/Program Files/Qhull\"" - - set PATH=%PATH%;C:\Program Files\Qhull\bin - # FLANN - - ps: wget 'https://dl.dropboxusercontent.com/s/7k58jbmqa51sxmh/FLANN-msvc140.exe?dl=0' -outfile FLANN.exe - - cmd: FLANN.exe -o"C:\Program Files" -y - - ECHO "Installed FLANN:" - - ps: "ls \"C:/Program Files/FLANN\"" - - set PATH=%PATH%;C:\Program Files\FLANN\bin - # Eigen - - ps: wget 'https://dl.dropboxusercontent.com/s/3v6i9i8dxj4o8ji/Eigen.exe?dl=0' -outfile Eigen.exe - - cmd: Eigen.exe -o"C:\Program Files" -y - - ECHO "Installed Eigen:" - - ps: "ls \"C:/Program Files/Eigen\"" - # PCL - - ps: wget 'https://dl.dropboxusercontent.com/s/2iayr4lyqa50i9j/PCL_181_August2018_x64_vc14.exe?dl=0' -outfile PCL_1.8.1.exe - - cmd: PCL_1.8.1.exe -o"C:\Program Files" -y - - ECHO "Installed PCL:" - - ps: "ls \"C:/Program Files/PCL\"" - - set PATH=%PATH%;C:\Program Files\PCL\bin - # zlib - - 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\"" - - set PATH=%PATH%;C:\Program Files\zlib\bin - # g2o - - ps: wget 'https://dl.dropboxusercontent.com/s/ht74s5pa21wokzw/g2o.exe?dl=0' -outfile g2o.exe - - cmd: g2o.exe -o"C:\Program Files" -y - - ECHO "Installed g2o:" - - ps: "ls \"C:/Program Files/g2o\"" - - set PATH=%PATH%;C:\Program Files\g2o\bin - # GTSAM - - ps: wget 'https://dl.dropboxusercontent.com/s/0fpr6r4cgsqmvhf/GTSAM-4_0_0_alpha2-msvc140.exe?dl=0' -outfile GTSAM.exe - - cmd: GTSAM.exe -o"C:\Program Files" -y - - ECHO "Installed GTSAM:" - - ps: "ls \"C:/Program Files/GTSAM\"" - - set PATH=%PATH%;C:\Program Files\GTSAM\bin - # OctoMap - - ps: wget 'https://dl.dropboxusercontent.com/s/6jpxu0nm8ne6e54/octomap_x64_vc14.exe?dl=0' -outfile octomap.exe - - cmd: octomap.exe -o"C:\Program Files" -y - - ECHO "Installed OctoMap:" - - ps: "ls \"C:/Program Files/octomap-distribution\"" - - set PATH=%PATH%;C:\Program Files\octomap-distribution\bin - # CPU-TSDF - - ps: wget 'https://dl.dropboxusercontent.com/s/mgges9va1uzxr0q/cpu_tsdf_sept2015_x64_vc14.exe?dl=0' -outfile cpu_tsdf.exe - - cmd: cpu_tsdf.exe -o"C:\Program Files" -y - - ECHO "Installed CPU-TSDF:" - - ps: "ls \"C:/Program Files/cpu_tsdf\"" - - set PATH=%PATH%;C:\Program Files\cpu_tsdf\bin - # Open Chisel - - ps: wget 'https://dl.dropboxusercontent.com/s/0aaphcde4acrinm/open_chisel_x64_vc14.exe?dl=0' -outfile open_chisel.exe - - cmd: open_chisel.exe -o"C:\Program Files" -y - - ECHO "Installed Open Chisel:" - - ps: "ls \"C:/Program Files/open_chisel\"" - - set PATH=%PATH%;C:\Program Files\open_chisel\bin - # yaml-cpp - - ps: wget 'https://dl.dropboxusercontent.com/s/22qfvftwj6zq8tj/yaml-cpp_x64_vc14.exe?dl=0' -outfile yaml-cpp.exe - - cmd: yaml-cpp.exe -o"C:\Program Files" -y - - ECHO "Installed yaml-cpp:" - - ps: "ls \"C:/Program Files/yaml-cpp\"" - # RealSense2 - - ps: wget 'https://github.com/IntelRealSense/librealsense/releases/download/v2.40.0/Intel.RealSense.SDK-WIN10-2.40.0.2482.exe' -outfile realsense2.exe - - cmd: realsense2.exe /VERYSILENT - - ECHO "Installed RealSense2:" - - ps: "ls \"C:/Program Files (x86)/Intel RealSense SDK 2.0\"" - - set PATH=%PATH%;C:\Program Files (x86)\Intel RealSense SDK 2.0\bin\x64 - - set RealSense2_ROOT_DIR=C:\Program Files (x86)\Intel RealSense SDK 2.0 - # Kinect 4 Azure - - ps: wget 'https://download.microsoft.com/download/3/d/6/3d6d9e99-a251-4cf3-8c6a-8e108e960b4b/Azure%20Kinect%20SDK%201.4.1.exe' -outfile azure.exe - - cmd: azure.exe /quiet - - ECHO "Installed Kinect For Azure:" - - ps: "ls \"C:/Program Files/Azure Kinect SDK v1.4.1\"" - - set PATH=%PATH%;C:\Program Files\Azure Kinect SDK v1.4.1\tools - - set K4A_ROOT_DIR=C:\Program Files\Azure Kinect SDK v1.4.1 - -before_build: - - cd c:\projects\rtabmap\build - - ECHO %PROGRAMFILES% - - ECHO %PATH% - - cmake -G "Visual Studio 14 2015 Win64" -DOpenCV_DIR="C:\Program Files\opencv\build" -DPCL_DIR="C:\Program Files\PCL\cmake" -DCPUTSDF_DIR="C:\Program Files\cpu_tsdf\share\cpu_tsdf" -Dyaml-cpp_DIR="C:\Program Files\yaml-cpp\CMake" -DBUILD_AS_BUNDLE=ON .. - -after_build : - - cmake --build . --config Release --target package - -artifacts: - - path: build\RTABMap-* - -notifications: - - provider: Email - to: - - matlabbe@gmail.com - on_build_success: false - on_build_failure: false - on_build_status_changed: true diff --git a/.devcontainer/focal/Dockerfile b/.devcontainer/focal/Dockerfile new file mode 100644 index 00000000..841a7ff0 --- /dev/null +++ b/.devcontainer/focal/Dockerfile @@ -0,0 +1,21 @@ +FROM introlab3it/rtabmap:focal-deps + +RUN apt-get update && apt-get install -y sudo && \ + apt-get clean && rm -rf /var/lib/apt/lists/ + +ARG USERNAME=vscode +ARG USER_UID=1000 +ARG USER_GID=1000 + +RUN set -ex && \ + groupadd --gid ${USER_GID} ${USERNAME} && \ + useradd --uid ${USER_UID} --gid ${USER_GID} -m ${USERNAME} && \ + usermod -a -G sudo ${USERNAME} && \ + echo "${USERNAME} ALL=(ALL) NOPASSWD:ALL" > /etc/sudoers.d/${USERNAME} && \ + chmod 0440 /etc/sudoers.d/${USERNAME} + +RUN mkdir -p /home/${USERNAME}/Documents/RTAB-Map && chown -R ${USERNAME} /home/${USERNAME} + +RUN echo "source /usr/share/bash-completion/completions/git" >> /home/${USERNAME}/.bashrc + + diff --git a/.devcontainer/focal/devcontainer.json b/.devcontainer/focal/devcontainer.json index 8b96029c..7684b5f4 100644 --- a/.devcontainer/focal/devcontainer.json +++ b/.devcontainer/focal/devcontainer.json @@ -1,8 +1,18 @@ { - "image": "introlab3it/rtabmap:20.04", + "build": { + "dockerfile": "Dockerfile" + }, "customizations": { "vscode": { "extensions": ["ms-vscode.cpptools-themes", "ms-vscode.cmake-tools"] } - } + }, + "workspaceMount": "source=${localWorkspaceFolder},target=/home/vscode/rtabmap,type=bind", + "workspaceFolder": "/home/vscode/rtabmap", + //"mounts": ["source=${localEnv:HOME}/Documents/RTAB-Map,target=/home/vscode/Documents/RTAB-Map,type=bind,consistency=cached"], + "settings": { + "terminal.integrated.defaultProfile.linux": "bash" + }, + "remoteUser": "vscode", + "runArgs": ["--privileged", "--network=host"] } diff --git a/.devcontainer/jammy/Dockerfile b/.devcontainer/jammy/Dockerfile new file mode 100644 index 00000000..83d58d64 --- /dev/null +++ b/.devcontainer/jammy/Dockerfile @@ -0,0 +1,21 @@ +FROM introlab3it/rtabmap:jammy-deps + +RUN apt-get update && apt-get install -y sudo && \ + apt-get clean && rm -rf /var/lib/apt/lists/ + +ARG USERNAME=vscode +ARG USER_UID=1000 +ARG USER_GID=1000 + +RUN set -ex && \ + groupadd --gid ${USER_GID} ${USERNAME} && \ + useradd --uid ${USER_UID} --gid ${USER_GID} -m ${USERNAME} && \ + usermod -a -G sudo ${USERNAME} && \ + echo "${USERNAME} ALL=(ALL) NOPASSWD:ALL" > /etc/sudoers.d/${USERNAME} && \ + chmod 0440 /etc/sudoers.d/${USERNAME} + +RUN mkdir -p /home/${USERNAME}/Documents/RTAB-Map && chown -R ${USERNAME} /home/${USERNAME} + +RUN echo "source /usr/share/bash-completion/completions/git" >> /home/${USERNAME}/.bashrc + + diff --git a/.devcontainer/jammy/devcontainer.json b/.devcontainer/jammy/devcontainer.json index 21c9f2ed..7684b5f4 100644 --- a/.devcontainer/jammy/devcontainer.json +++ b/.devcontainer/jammy/devcontainer.json @@ -1,8 +1,18 @@ { - "image": "introlab3it/rtabmap:22.04", + "build": { + "dockerfile": "Dockerfile" + }, "customizations": { "vscode": { "extensions": ["ms-vscode.cpptools-themes", "ms-vscode.cmake-tools"] } - } + }, + "workspaceMount": "source=${localWorkspaceFolder},target=/home/vscode/rtabmap,type=bind", + "workspaceFolder": "/home/vscode/rtabmap", + //"mounts": ["source=${localEnv:HOME}/Documents/RTAB-Map,target=/home/vscode/Documents/RTAB-Map,type=bind,consistency=cached"], + "settings": { + "terminal.integrated.defaultProfile.linux": "bash" + }, + "remoteUser": "vscode", + "runArgs": ["--privileged", "--network=host"] } diff --git a/.devcontainer/latest-deps-from-source/Dockerfile b/.devcontainer/latest-deps-from-source/Dockerfile new file mode 100644 index 00000000..941967ae --- /dev/null +++ b/.devcontainer/latest-deps-from-source/Dockerfile @@ -0,0 +1,104 @@ +ARG ROS_DISTRO=jazzy + +FROM osrf/ros:${ROS_DISTRO}-desktop + +# Install build dependencies with Qt6 (issue: rtabmap has black window) +#RUN apt-get update && \ +# apt-get install -y git software-properties-common ros-${ROS_DISTRO}-rtabmap-ros libqt6* qt6* qml6* && \ +# apt-get remove -y ros-${ROS_DISTRO}-rtabmap* ros-${ROS_DISTRO}-gtsam ros-${ROS_DISTRO}-libg2o libpcl* libqt5* qt5* libvtk* libopencv* && \ +# apt-get clean && rm -rf /var/lib/apt/lists/ + +# Install build dependencies with Qt5 +RUN apt-get update && \ + apt-get install -y git software-properties-common ros-${ROS_DISTRO}-rtabmap-ros && \ + apt-get remove -y ros-${ROS_DISTRO}-rtabmap* ros-${ROS_DISTRO}-gtsam ros-${ROS_DISTRO}-libg2o libpcl* libvtk* libopencv* && \ + apt-get clean && rm -rf /var/lib/apt/lists/ + +# remove ubuntu user +RUN touch /var/mail/ubuntu && chown ubuntu /var/mail/ubuntu && userdel -r ubuntu + +RUN apt-get update && apt-get install -y sudo && \ + apt-get clean && rm -rf /var/lib/apt/lists/ + +ARG USERNAME=vscode +ARG USER_UID=1000 +ARG USER_GID=1000 + +RUN set -ex && \ + groupadd --gid ${USER_GID} ${USERNAME} && \ + useradd --uid ${USER_UID} --gid ${USER_GID} -m ${USERNAME} && \ + usermod -a -G sudo ${USERNAME} && \ + echo "${USERNAME} ALL=(ALL) NOPASSWD:ALL" > /etc/sudoers.d/${USERNAME} && \ + chmod 0440 /etc/sudoers.d/${USERNAME} + +RUN mkdir -p /home/${USERNAME}/Documents/RTAB-Map + +RUN echo "source /usr/share/bash-completion/completions/git" >> /home/${USERNAME}/.bashrc + +WORKDIR /home/${USERNAME}/ + +RUN rm /bin/sh && ln -s /bin/bash /bin/sh + +# We build main dependencies from source, cleaning up the +# build directory after install but keeping the source code +# to change version/reinstall from inside the dev container +# if needed. + +# Build latest VTK with Qt6 +RUN git clone https://github.com/Kitware/VTK.git +RUN cd VTK && \ + mkdir build && \ + cd build && \ + cmake -DVTK_GROUP_ENABLE_Qt=YES .. && \ + make -j$(nproc) && \ + make install && \ + make clean + +# Build latest PCL with latest VTK +# Make sure all libraries depending on Eigen are built with same CXX standard (17) +RUN git clone https://github.com/PointCloudLibrary/pcl.git +RUN cd pcl && \ + mkdir build && \ + cd build && \ + cmake -DCMAKE_CXX_STANDARD=17 -DBUILD_tools=ON -DPCL_ENABLE_AVX=OFF -DPCL_ENABLE_MARCHNATIVE=OFF -DPCL_ENABLE_SSE=OFF .. && \ + make -j$(nproc) && \ + make install && \ + make clean + +# Build latest OpenCV +RUN git clone https://github.com/opencv/opencv.git +RUN git clone https://github.com/opencv/opencv_contrib.git +RUN 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=../../opencv_contrib/modules .. && \ + make -j$(nproc) && \ + make install && \ + make clean + +# Build latest gtsam +RUN git clone https://github.com/borglab/gtsam.git +RUN cd gtsam && \ + mkdir build && \ + cd build && \ + cmake -DCMAKE_CXX_STANDARD=17 -DGTSAM_BUILD_WITH_MARCH_NATIVE=OFF -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 -DCMAKE_BUILD_TYPE=Release .. && \ + make -j$(nproc) && \ + make install && \ + make clean + +# Build latest g2o +RUN git clone https://github.com/RainerKuemmerle/g2o.git +RUN cd g2o && \ + mkdir build && \ + cd build && \ + cmake -DCMAKE_CXX_STANDARD=17 -DBUILD_WITH_MARCH_NATIVE=OFF -DG2O_BUILD_APPS=OFF -DG2O_BUILD_EXAMPLES=OFF -DG2O_USE_OPENGL=OFF -DCMAKE_BUILD_TYPE=Release .. && \ + make -j$(nproc) && \ + make install && \ + make clean + +# ros2 seems not sourcing by default its multi-arch folders +ENV LD_LIBRARY_PATH=$LD_LIBRARY_PATH:/opt/ros/${ROS_DISTRO}/lib/x86_64-linux-gnu + +RUN ldconfig + +RUN chown -R ${USERNAME} /home/${USERNAME} diff --git a/.devcontainer/latest-deps-from-source/devcontainer.json b/.devcontainer/latest-deps-from-source/devcontainer.json new file mode 100644 index 00000000..c63155a5 --- /dev/null +++ b/.devcontainer/latest-deps-from-source/devcontainer.json @@ -0,0 +1,21 @@ +{ + "build": { + "dockerfile": "Dockerfile", + "args": { + "ROS_DISTRO": "jazzy" + } + }, + "customizations": { + "vscode": { + "extensions": ["ms-vscode.cpptools-themes", "ms-vscode.cmake-tools"] + } + }, + "workspaceMount": "source=${localWorkspaceFolder},target=/home/vscode/rtabmap,type=bind", + "workspaceFolder": "/home/vscode/rtabmap", + //"mounts": ["source=${localEnv:HOME}/Documents/RTAB-Map,target=/home/vscode/Documents/RTAB-Map,type=bind,consistency=cached"], + "settings": { + "terminal.integrated.defaultProfile.linux": "bash" + }, + "remoteUser": "vscode", + "runArgs": ["--privileged", "--network=host"] +} diff --git a/.devcontainer/noble/Dockerfile b/.devcontainer/noble/Dockerfile new file mode 100644 index 00000000..84792a66 --- /dev/null +++ b/.devcontainer/noble/Dockerfile @@ -0,0 +1,25 @@ +FROM introlab3it/rtabmap:noble-deps + +# For devcontainer +# remove ubuntu user +RUN touch /var/mail/ubuntu && chown ubuntu /var/mail/ubuntu && userdel -r ubuntu + +RUN apt-get update && apt-get install -y sudo && \ + apt-get clean && rm -rf /var/lib/apt/lists/ + +ARG USERNAME=vscode +ARG USER_UID=1000 +ARG USER_GID=1000 + +RUN set -ex && \ + groupadd --gid ${USER_GID} ${USERNAME} && \ + useradd --uid ${USER_UID} --gid ${USER_GID} -m ${USERNAME} && \ + usermod -a -G sudo ${USERNAME} && \ + echo "${USERNAME} ALL=(ALL) NOPASSWD:ALL" > /etc/sudoers.d/${USERNAME} && \ + chmod 0440 /etc/sudoers.d/${USERNAME} + +RUN mkdir -p /home/${USERNAME}/Documents/RTAB-Map && chown -R ${USERNAME} /home/${USERNAME} + +RUN echo "source /usr/share/bash-completion/completions/git" >> /home/${USERNAME}/.bashrc + + diff --git a/.devcontainer/noble/devcontainer.json b/.devcontainer/noble/devcontainer.json index 66e9a222..387a00c1 100644 --- a/.devcontainer/noble/devcontainer.json +++ b/.devcontainer/noble/devcontainer.json @@ -1,8 +1,30 @@ { - "image": "introlab3it/rtabmap:24.04", + "build": { + "dockerfile": "Dockerfile" + }, "customizations": { "vscode": { - "extensions": ["ms-vscode.cpptools-themes", "ms-vscode.cmake-tools"] + "extensions": ["ms-vscode.cpptools-themes", "ms-vscode.cmake-tools", "ms-vscode.cpptools-extension-pack"] } - } + }, + "workspaceMount": "source=${localWorkspaceFolder},target=/home/vscode/rtabmap,type=bind", + "workspaceFolder": "/home/vscode/rtabmap", + //"mounts": ["source=${localEnv:HOME}/Documents/RTAB-Map,target=/home/vscode/Documents/RTAB-Map,type=bind,consistency=cached"], + "settings": { + "terminal.integrated.defaultProfile.linux": "bash" + }, + "remoteUser": "vscode", + "hostRequirements": { + "gpu": "optional" + }, + "runArgs": ["--privileged", + "--network=host", + "--gpus=all", + //"--runtime=nvidia", // uncommment this if rtabmap doesn't show up in nvidia-smi on the host computer + "--env=DISPLAY", + "--env=QT_X11_NO_MITSHM=1", + "--volume=/tmp/.X11-unix:/tmp/.X11-unix"], + "containerEnv": { + "NVIDIA_VISIBLE_DEVICES": "all" + } } diff --git a/.devcontainer/resolute/Dockerfile b/.devcontainer/resolute/Dockerfile new file mode 100644 index 00000000..244d9ca4 --- /dev/null +++ b/.devcontainer/resolute/Dockerfile @@ -0,0 +1,25 @@ +FROM introlab3it/rtabmap:resolute-deps + +# For devcontainer +# remove ubuntu user +RUN touch /var/mail/ubuntu && chown ubuntu /var/mail/ubuntu && userdel -r ubuntu + +RUN apt-get update && apt-get install -y sudo && \ + apt-get clean && rm -rf /var/lib/apt/lists/ + +ARG USERNAME=vscode +ARG USER_UID=1000 +ARG USER_GID=1000 + +RUN set -ex && \ + groupadd --gid ${USER_GID} ${USERNAME} && \ + useradd --uid ${USER_UID} --gid ${USER_GID} -m ${USERNAME} && \ + usermod -a -G sudo ${USERNAME} && \ + echo "${USERNAME} ALL=(ALL) NOPASSWD:ALL" > /etc/sudoers.d/${USERNAME} && \ + chmod 0440 /etc/sudoers.d/${USERNAME} + +RUN mkdir -p /home/${USERNAME}/Documents/RTAB-Map && chown -R ${USERNAME} /home/${USERNAME} + +RUN echo "source /usr/share/bash-completion/completions/git" >> /home/${USERNAME}/.bashrc + + diff --git a/.devcontainer/resolute/devcontainer.json b/.devcontainer/resolute/devcontainer.json new file mode 100644 index 00000000..387a00c1 --- /dev/null +++ b/.devcontainer/resolute/devcontainer.json @@ -0,0 +1,30 @@ +{ + "build": { + "dockerfile": "Dockerfile" + }, + "customizations": { + "vscode": { + "extensions": ["ms-vscode.cpptools-themes", "ms-vscode.cmake-tools", "ms-vscode.cpptools-extension-pack"] + } + }, + "workspaceMount": "source=${localWorkspaceFolder},target=/home/vscode/rtabmap,type=bind", + "workspaceFolder": "/home/vscode/rtabmap", + //"mounts": ["source=${localEnv:HOME}/Documents/RTAB-Map,target=/home/vscode/Documents/RTAB-Map,type=bind,consistency=cached"], + "settings": { + "terminal.integrated.defaultProfile.linux": "bash" + }, + "remoteUser": "vscode", + "hostRequirements": { + "gpu": "optional" + }, + "runArgs": ["--privileged", + "--network=host", + "--gpus=all", + //"--runtime=nvidia", // uncommment this if rtabmap doesn't show up in nvidia-smi on the host computer + "--env=DISPLAY", + "--env=QT_X11_NO_MITSHM=1", + "--volume=/tmp/.X11-unix:/tmp/.X11-unix"], + "containerEnv": { + "NVIDIA_VISIBLE_DEVICES": "all" + } +} diff --git a/.devcontainer/rolling/Dockerfile b/.devcontainer/rolling/Dockerfile index 38c18c24..576b75fe 100644 --- a/.devcontainer/rolling/Dockerfile +++ b/.devcontainer/rolling/Dockerfile @@ -6,11 +6,12 @@ ENV DEBIAN_FRONTEND=noninteractive # Install ROS2 RUN apt update && \ apt install software-properties-common -y && \ - add-apt-repository universe && \ + add-apt-repository universe -y && \ 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 && \ + export ROS_APT_SOURCE_VERSION=$(curl -s https://api.github.com/repos/ros-infrastructure/ros-apt-source/releases/latest | grep -F "tag_name" | awk -F\" '{print $4}') && \ + curl -L -o /tmp/ros2-apt-source.deb "https://github.com/ros-infrastructure/ros-apt-source/releases/download/${ROS_APT_SOURCE_VERSION}/ros2-apt-source_${ROS_APT_SOURCE_VERSION}.$(. /etc/os-release && echo ${UBUNTU_CODENAME:-${VERSION_CODENAME}})_all.deb" && \ + apt install /tmp/ros2-apt-source.deb && \ apt-get clean && rm -rf /var/lib/apt/lists/ # Install build dependencies @@ -73,5 +74,7 @@ RUN set -ex && \ echo "${USERNAME} ALL=(ALL) NOPASSWD:ALL" > /etc/sudoers.d/${USERNAME} && \ chmod 0440 /etc/sudoers.d/${USERNAME} +RUN mkdir -p /home/${USERNAME}/Documents/RTAB-Map && chown -R ${USERNAME} /home/${USERNAME} + RUN echo "source /usr/share/bash-completion/completions/git" >> /home/${USERNAME}/.bashrc diff --git a/.devcontainer/rolling/devcontainer.json b/.devcontainer/rolling/devcontainer.json index 95463406..1f59ad49 100644 --- a/.devcontainer/rolling/devcontainer.json +++ b/.devcontainer/rolling/devcontainer.json @@ -9,9 +9,22 @@ }, "workspaceMount": "source=${localWorkspaceFolder},target=/home/vscode/rtabmap,type=bind", "workspaceFolder": "/home/vscode/rtabmap", + //"mounts": ["source=${localEnv:HOME}/Documents/RTAB-Map,target=/home/vscode/Documents/RTAB-Map,type=bind,consistency=cached"], "settings": { "terminal.integrated.defaultProfile.linux": "bash" }, "remoteUser": "vscode", - "runArgs": ["--privileged", "--network=host"] + "hostRequirements": { + "gpu": "optional" + }, + "runArgs": ["--privileged", + "--network=host", + "--gpus=all", + //"--runtime=nvidia", // uncommment this if rtabmap doesn't show up in nvidia-smi on the host computer + "--env=DISPLAY", + "--env=QT_X11_NO_MITSHM=1", + "--volume=/tmp/.X11-unix:/tmp/.X11-unix"], + "containerEnv": { + "NVIDIA_VISIBLE_DEVICES": "all" + } } diff --git a/.github/actions/install-windows-cuda-deps/action.yml b/.github/actions/install-windows-cuda-deps/action.yml new file mode 100644 index 00000000..0027ee58 --- /dev/null +++ b/.github/actions/install-windows-cuda-deps/action.yml @@ -0,0 +1,53 @@ +name: 'Install Windows Dependencies with CUDA' +description: 'Installs PCL, Qt, VTK, g2o and others' +runs: + using: "composite" + steps: + - name: Set up MSVC Developer Command Prompt + uses: ilammy/msvc-dev-cmd@v1 + with: + arch: x64 + + - name: Install CUDA + uses: Jimver/cuda-toolkit@v0.2.30 + id: cuda-toolkit + with: + cuda: '13.0.0' + use-github-cache: True + + - name: Verify CUDA + shell: bash + run: | + nvcc --version + echo "CUDA Path: $CUDA_PATH" + + - name: Cache vcpkg + id: cache-vcpkg + uses: actions/cache@v4 + with: + path: ${{ runner.workspace }}/vcpkg_installed + key: ${{ runner.os }}-vcpkg-export-66c0373d-x64-vs2022-cuda130_v1 + + - name: Download and Install vcpkg + if: steps.cache-vcpkg.outputs.cache-hit != 'true' + shell: pwsh + run: | + $install_dir = "${{ runner.workspace }}\vcpkg_installed" + $archivePath = "${{ runner.workspace }}\vcpkg-export.7z" + + # The file has been built locally with bundle-windows-deps.bat + $url = "https://github.com/introlab/rtabmap/releases/download/0.23.1/vcpkg-export-66c0373d-x64-vs2022-cuda130.7z" + + Invoke-WebRequest -Uri $url -OutFile $archivePath + & 7z x $archivePath "-o$install_dir" -y + + - name: Add vcpkg to PATH and env variable + shell: pwsh + run: | + $vcpkg_path = "${{ runner.workspace }}\vcpkg_installed" + echo "VCPKG_EXPORT_PATH=$vcpkg_path" | Out-File -FilePath $env:GITHUB_ENV -Encoding utf8 -Append + echo "$vcpkg_path\installed\x64-windows-release\bin" | Out-File -FilePath $env:GITHUB_PATH -Encoding utf8 -Append + echo "${{env.CUDA_PATH}}\bin\x64" | Out-File -FilePath $env:GITHUB_PATH -Encoding utf8 -Append + echo "${{env.CUDA_PATH}}\extras\CUPTI\lib64" | Out-File -FilePath $env:GITHUB_PATH -Encoding utf8 -Append + echo "$vcpkg_path\installed\x64-windows-release\tools\python3\Lib\site-packages\torch\lib" | Out-File -FilePath $env:GITHUB_PATH -Encoding utf8 -Append + echo "$vcpkg_path\installed\x64-windows-release\tools\python3\Lib\site-packages\numpy.libs" | Out-File -FilePath $env:GITHUB_PATH -Encoding utf8 -Append diff --git a/.github/actions/install-windows-deps/action.yml b/.github/actions/install-windows-deps/action.yml new file mode 100644 index 00000000..00983b29 --- /dev/null +++ b/.github/actions/install-windows-deps/action.yml @@ -0,0 +1,37 @@ +name: 'Install Windows Dependencies' +description: 'Installs PCL, Qt, VTK, g2o and others' +runs: + using: "composite" + steps: + - name: Set up MSVC Developer Command Prompt + uses: ilammy/msvc-dev-cmd@v1 + with: + arch: x64 + + - name: Cache vcpkg + id: cache-vcpkg + uses: actions/cache@v4 + with: + path: ${{ runner.workspace }}/vcpkg_installed + key: ${{ runner.os }}-vcpkg-export-66c0373d-x64-vs2022-v4 + + - name: Download and Install vcpkg + if: steps.cache-vcpkg.outputs.cache-hit != 'true' + shell: pwsh + run: | + $install_dir = "${{ runner.workspace }}\vcpkg_installed" + $archivePath = "${{ runner.workspace }}\vcpkg-export.7z" + + # The file has been built locally with bundle-windows-deps.bat + $url = "https://github.com/introlab/rtabmap/releases/download/0.23.1/vcpkg-export-66c0373d-x64-vs2022.7z" + + Invoke-WebRequest -Uri $url -OutFile $archivePath + & 7z x $archivePath "-o$install_dir" -y + + - name: Add vcpkg to PATH and env variable + shell: pwsh + run: | + $vcpkg_path = "${{ runner.workspace }}\vcpkg_installed" + echo "VCPKG_EXPORT_PATH=$vcpkg_path" | Out-File -FilePath $env:GITHUB_ENV -Encoding utf8 -Append + echo "$vcpkg_path\installed\x64-windows-release\bin" | Out-File -FilePath $env:GITHUB_PATH -Encoding utf8 -Append + echo "$vcpkg_path\installed\x64-windows-release\tools\python3\Lib\site-packages\numpy.libs" | Out-File -FilePath $env:GITHUB_PATH -Encoding utf8 -Append diff --git a/.github/workflows/cleanup-pr-artifacts.yml b/.github/workflows/cleanup-pr-artifacts.yml new file mode 100644 index 00000000..d5e2789a --- /dev/null +++ b/.github/workflows/cleanup-pr-artifacts.yml @@ -0,0 +1,15 @@ +name: Cleanup PR Artifacts +on: + pull_request: + types: [closed] + +jobs: + delete-artifacts: + runs-on: ubuntu-latest + permissions: + actions: write + steps: + - name: Delete PR Artifacts + uses: geekyeggo/delete-artifact@v5 + with: + name: build-output-* \ No newline at end of file diff --git a/.github/workflows/cmake-linux.yml b/.github/workflows/cmake-linux.yml new file mode 100644 index 00000000..bf2b51fb --- /dev/null +++ b/.github/workflows/cmake-linux.yml @@ -0,0 +1,80 @@ +name: CMake-Linux + +on: + push: + branches: + - master + pull_request: + branches: + - '**' + +env: + BUILD_TYPE: Release + +concurrency: + group: ${{ github.workflow }}-${{ github.event.pull_request.number || github.ref }} + cancel-in-progress: ${{ github.event_name == 'pull_request' }} + +jobs: + build: + name: ${{ matrix.build_name }} + runs-on: ${{ matrix.os }} + strategy: + fail-fast: true + matrix: + build_name: [ubuntu-22.04, ubuntu-24.04, ubuntu-24.04-with-opengv, ubuntu-26.04] + include: + - build_name: ubuntu-22.04 + os: ubuntu-22.04 + extra_deps: "libunwind-dev libceres-dev" + extra_cmake_def: "-DWITH_CERES=ON -DWITH_PYTHON=ON" + - build_name: ubuntu-24.04 + os: ubuntu-24.04 + extra_deps: "libg2o-dev libceres-dev" + extra_cmake_def: "-DWITH_CERES=ON -DWITH_PYTHON=ON" + - build_name: ubuntu-24.04-with-opengv + os: ubuntu-24.04 + extra_deps: "libg2o-dev libceres-dev" + extra_cmake_def: "-DWITH_CERES=ON -DWITH_PYTHON=ON -DBUILD_OPENGV=ON" + - build_name: ubuntu-26.04 + os: ubuntu-26.04 + extra_deps: "libg2o-dev libceres-dev" + extra_cmake_def: "-DWITH_CERES=ON -DWITH_PYTHON=ON -DBUILD_OPENGV=ON" + + steps: + - uses: actions/checkout@v4 + + - name: Install Linux Dependencies + run: | + DEBIAN_FRONTEND=noninteractive + sudo apt-get update + sudo apt-get -y install libopencv-dev libpcl-dev git cmake software-properties-common libyaml-cpp-dev ${{ matrix.extra_deps }} + + - name: Set up Python + uses: actions/setup-python@v5 + with: + python-version: '3.x' + + - name: Install Python Dependencies + run: | + python -m pip install --upgrade pip + pip install numpy pybind11 + + - name: Configure CMake + run: | + cmake -B ${{github.workspace}}/build -DCMAKE_BUILD_TYPE=${{env.BUILD_TYPE}} -DPython3_EXECUTABLE=$(which python3) -Dpybind11_DIR=$(python3 -m pybind11 --cmakedir) ${{ matrix.extra_cmake_def }} + + - name: Build + run: cmake --build ${{github.workspace}}/build --config ${{env.BUILD_TYPE}} + + - name: Info + working-directory: ${{github.workspace}}/build/bin + run: | + ./rtabmap-console --version + +# - name: Test +# working-directory: ${{github.workspace}}/build +# # Execute tests defined by the CMake configuration. +# # See https://cmake.org/cmake/help/latest/manual/ctest.1.html for more detail +# run: ctest -C ${{env.BUILD_TYPE}} + diff --git a/.github/workflows/cmake-macos.yml b/.github/workflows/cmake-macos.yml new file mode 100644 index 00000000..a8d37a3e --- /dev/null +++ b/.github/workflows/cmake-macos.yml @@ -0,0 +1,83 @@ +name: CMake-MacOS + +on: + push: + branches: + - master + pull_request: + branches: + - '**' + +env: + BUILD_TYPE: Release + +concurrency: + group: ${{ github.workflow }}-${{ github.event.pull_request.number || github.ref }} + cancel-in-progress: ${{ github.event_name == 'pull_request' }} + +jobs: + build: + name: ${{ matrix.build_name }} + runs-on: ${{ matrix.os }} + strategy: + fail-fast: true + matrix: + build_name: [macos-sequoia-intel, macos-sequoia-apple-silicon, macos-tahoe-intel, macos-tahoe-apple-silicon] + include: + - build_name: macos-sequoia-intel + os: macos-15-intel + extra_deps: "" + extra_cmake_def: '-DBUILD_AS_BUNDLE=ON' + - build_name: macos-sequoia-apple-silicon + os: macos-15 + extra_deps: "" + extra_cmake_def: '-DBUILD_AS_BUNDLE=ON' + - build_name: macos-tahoe-intel + os: macos-26-intel + extra_deps: "" + extra_cmake_def: '-DBUILD_AS_BUNDLE=ON' + - build_name: macos-tahoe-apple-silicon + os: macos-26 + extra_deps: "" + extra_cmake_def: '-DBUILD_AS_BUNDLE=ON' + + steps: + - uses: actions/checkout@v4 + + - name: Install Brew Dependencies + run: | + # Update brew and install from Brewfile if present, or specific packages + brew install pcl opencv octomap g2o pdal + + - name: Configure CMake + run: | + cmake -B ${{github.workspace}}/build -DCMAKE_BUILD_TYPE=${{env.BUILD_TYPE}} ${{ matrix.extra_cmake_def }} + + - name: Build + run: cmake --build ${{github.workspace}}/build --config ${{env.BUILD_TYPE}} + + - name: Info + working-directory: ${{github.workspace}}/build/bin + run: | + ./rtabmap-console --version + +# - name: Build MacOS Package +# run: | +# cmake --build ${{ github.workspace }}/build --config ${{ env.BUILD_TYPE }} --target package + +# - name: Upload RTABMap Artifacts (DMG) +# uses: actions/upload-artifact@v4 +# with: +# name: RTABMap-Binaries-${{ matrix.build_name }}-zip +# path: | +# build/RTABMap-*.dmg +# compression-level: 0 +# if-no-files-found: warn +# retention-days: ${{ github.event_name == 'pull_request' && 1 || 90 }} + +# - name: Test +# working-directory: ${{github.workspace}}/build +# # Execute tests defined by the CMake configuration. +# # See https://cmake.org/cmake/help/latest/manual/ctest.1.html for more detail +# run: ctest -C ${{env.BUILD_TYPE}} + diff --git a/.github/workflows/cmake-ros.yml b/.github/workflows/cmake-ros.yml index 5fd16ec8..67c03c04 100644 --- a/.github/workflows/cmake-ros.yml +++ b/.github/workflows/cmake-ros.yml @@ -12,57 +12,35 @@ env: # Customize the CMake build type here (Release, Debug, RelWithDebInfo, etc.) BUILD_TYPE: Release +concurrency: + group: ${{ github.workflow }}-${{ github.event.pull_request.number || github.ref }} + cancel-in-progress: ${{ github.event_name == 'pull_request' }} + jobs: build: # The CMake configure and build commands are platform agnostic and should work equally # well on Windows or Mac. You can convert this to a matrix build if you need # cross-platform coverage. # See: https://docs.github.com/en/free-pro-team@latest/actions/learn-github-actions/managing-complex-workflows#using-a-build-matrix - name: Build on ros ${{ matrix.ros_distribution }} and ${{ matrix.os }} - runs-on: ${{ matrix.os }} + name: ${{ matrix.ros_distribution }} + runs-on: ubuntu-latest strategy: fail-fast: false matrix: ros_distribution: [ kilted ] include: - ros_distribution: 'kilted' - os: ubuntu-24.04 - + skip_keys: "" + container: + image: osrf/ros:${{ matrix.ros_distribution }}-desktop-full steps: - - name: Setup ROS2 - # https://docs.ros.org/en/humble/Installation/Ubuntu-Install-Debs.html - run: | - sudo apt install software-properties-common - sudo add-apt-repository universe - sudo apt update && sudo apt install curl -y - export ROS_APT_SOURCE_VERSION=$(curl -s https://api.github.com/repos/ros-infrastructure/ros-apt-source/releases/latest | grep -F "tag_name" | awk -F\" '{print $4}') - curl -L -o /tmp/ros2-apt-source.deb "https://github.com/ros-infrastructure/ros-apt-source/releases/download/${ROS_APT_SOURCE_VERSION}/ros2-apt-source_${ROS_APT_SOURCE_VERSION}.$(. /etc/os-release && echo $VERSION_CODENAME)_all.deb" - sudo apt install /tmp/ros2-apt-source.deb - sudo apt update - + - uses: actions/checkout@v4 - uses: ros-tooling/setup-ros@v0.7 with: required-ros-distributions: ${{ matrix.ros_distribution }} - - - uses: actions/checkout@v4 - - - name: Install dependencies - run: | - source /opt/ros/${{ matrix.ros_distribution }}/setup.bash - rosdep update - rosdep install --from-paths ${{github.workspace}} -y - - - name: Configure CMake - run: | - source /opt/ros/${{ matrix.ros_distribution }}/setup.bash - cmake -B ${{github.workspace}}/build -DCMAKE_BUILD_TYPE=${{env.BUILD_TYPE}} - - - name: Build - run: cmake --build ${{github.workspace}}/build --config ${{env.BUILD_TYPE}} - - - name: Info - working-directory: ${{github.workspace}}/build/bin - run: | - source /opt/ros/${{ matrix.ros_distribution }}/setup.bash - ./rtabmap-console --version + - uses: ros-tooling/action-ros-ci@v0.4 + with: + package-name: rtabmap + target-ros2-distro: ${{ matrix.ros_distribution }} + rosdep-skip-keys: "${{ matrix.skip_keys }}" diff --git a/.github/workflows/cmake-windows.yml b/.github/workflows/cmake-windows.yml new file mode 100644 index 00000000..9af50aa3 --- /dev/null +++ b/.github/workflows/cmake-windows.yml @@ -0,0 +1,107 @@ +name: CMake-Windows + +on: + push: + branches: + - master + pull_request: + branches: + - '**' + +env: + BUILD_TYPE: Release + +concurrency: + group: ${{ github.workflow }}-${{ github.event.pull_request.number || github.ref }} + cancel-in-progress: ${{ github.event_name == 'pull_request' }} + +jobs: + build: + name: ${{ matrix.build_name }} + runs-on: ${{ matrix.os }} + strategy: + fail-fast: true + matrix: + build_name: [windows-2022, windows-2022-cuda] + include: + - build_name: windows-2022 + os: windows-2022 + extra_deps: "" + extra_cmake_def: '-DBUILD_AS_BUNDLE=ON -DWITH_PYTHON=ON -DWITH_TORCH=OFF' + - build_name: windows-2022-cuda + os: windows-2022 + extra_deps: "" + extra_cmake_def: '-DBUILD_AS_BUNDLE=ON -DWITH_PYTHON=ON -DWITH_TORCH=ON' + + steps: + - uses: actions/checkout@v4 + + - name: Install Windows Dependencies + if: matrix.build_name == 'windows-2022' + uses: ./.github/actions/install-windows-deps + + - name: Install Windows Dependencies with CUDA + if: matrix.build_name == 'windows-2022-cuda' + uses: ./.github/actions/install-windows-cuda-deps + + - name: Configure CMake + run: | + cmake ` + -B ${{github.workspace}}/build ` + -DCMAKE_BUILD_TYPE=${{env.BUILD_TYPE}} ` + ${{ matrix.extra_cmake_def }} ` + -DVCPKG_MANIFEST_INSTALL=OFF ` + -DVCPKG_TARGET_TRIPLET=x64-windows-release ` + -DVCPKG_INSTALLED_DIR="${{env.VCPKG_EXPORT_PATH}}/installed" ` + -DCMAKE_TOOLCHAIN_FILE=${{env.VCPKG_EXPORT_PATH}}/scripts/buildsystems/vcpkg.cmake ` + -DTorch_DIR=${{env.VCPKG_EXPORT_PATH}}/installed/x64-windows-release/tools/python3/Lib/site-packages/torch/share/cmake/Torch + + - name: Build + run: cmake --build ${{github.workspace}}/build --config ${{env.BUILD_TYPE}} + + - name: Build Windows Package + shell: pwsh + run: | + if ("${{ github.event_name }}" -eq "pull_request") { + cpack --config build/CPackConfig.cmake -G ZIP -B build + } else { + cmake --build ${{ github.workspace }}/build --config ${{ env.BUILD_TYPE }} --target package + } + + - name: Rename CUDA artifacts + if: matrix.build_name == 'windows-2022-cuda' + shell: pwsh + run: Get-ChildItem -Path "build" -Filter "RTABMap-*" | Rename-Item -NewName { $_.BaseName + "_cuda" + $_.Extension } + + - name: Info + working-directory: ${{github.workspace}}/build/bin + run: | + ./rtabmap-console --version + + - name: Upload RTABMap Artifacts (ZIP) + uses: actions/upload-artifact@v4 + with: + name: RTABMap-Binaries-${{ matrix.build_name }}-zip + path: | + build/RTABMap-*.zip + compression-level: 0 + if-no-files-found: warn + retention-days: ${{ github.event_name == 'pull_request' && 1 || 90 }} + + - name: Upload RTABMap Artifacts (Installer) + if: github.event_name != 'pull_request' + uses: actions/upload-artifact@v4 + with: + name: RTABMap-Binaries-${{ matrix.build_name }}-exe + path: | + build/RTABMap-*.exe + compression-level: 0 + if-no-files-found: warn + retention-days: ${{ github.event_name == 'pull_request' && 1 || 90 }} + +# - name: Test +# working-directory: ${{github.workspace}}/build +# # Execute tests defined by the CMake configuration. +# # See https://cmake.org/cmake/help/latest/manual/ctest.1.html for more detail +# run: ctest -C ${{env.BUILD_TYPE}} + diff --git a/.github/workflows/cmake.yml b/.github/workflows/cmake.yml deleted file mode 100644 index 9a347527..00000000 --- a/.github/workflows/cmake.yml +++ /dev/null @@ -1,56 +0,0 @@ -name: CMake - -on: - push: - branches: - - master - pull_request: - branches: - - '**' - -env: - BUILD_TYPE: Release - -jobs: - build: - name: ${{ matrix.os }} - runs-on: ${{ matrix.os }} - strategy: - fail-fast: false - matrix: - os: [ubuntu-24.04, ubuntu-22.04] - include: - - os: ubuntu-22.04 - extra_deps: "libunwind-dev libceres-dev" - extra_cmake_def: "" - - os: ubuntu-24.04 - extra_deps: "libg2o-dev libceres-dev" - extra_cmake_def: "-DWITH_CERES=ON" - - 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 libyaml-cpp-dev ${{ matrix.extra_deps }} - - - uses: actions/checkout@v4 - - - name: Configure CMake - run: | - cmake -B ${{github.workspace}}/build -DCMAKE_BUILD_TYPE=${{env.BUILD_TYPE}} ${{ matrix.extra_cmake_def }} - - - name: Build - run: cmake --build ${{github.workspace}}/build --config ${{env.BUILD_TYPE}} - - - name: Info - working-directory: ${{github.workspace}}/build/bin - run: | - ./rtabmap-console --version - -# - name: Test -# working-directory: ${{github.workspace}}/build -# # Execute tests defined by the CMake configuration. -# # See https://cmake.org/cmake/help/latest/manual/ctest.1.html for more detail -# run: ctest -C ${{env.BUILD_TYPE}} - diff --git a/.github/workflows/docker.yml b/.github/workflows/docker.yml index 5bcf160c..3fcfd2c1 100644 --- a/.github/workflows/docker.yml +++ b/.github/workflows/docker.yml @@ -4,6 +4,13 @@ on: push: branches: - 'master' + pull_request: + branches: + - '**' + +concurrency: + group: ${{ github.workflow }}-${{ github.event.pull_request.number || github.ref }} + cancel-in-progress: ${{ github.event_name == 'pull_request' }} jobs: docker_deps: @@ -15,14 +22,15 @@ jobs: # $ sudo apt-get upgrade qemu-user-static # $ docker run --rm --privileged multiarch/qemu-user-static --reset -p yes -c yes # More info: https://github.com/introlab/rtabmap/issues/1454 - # if: false + # Skipped on pull requests; built and pushed only on push to master. + if: github.event_name != 'pull_request' runs-on: ubuntu-latest strategy: fail-fast: false matrix: - docker_tag: [focal-deps, jammy-deps, noble-deps, noble-kilted-deps] + docker_tag: [focal-deps, jammy-deps, noble-deps, noble-kilted-deps, resolute-deps] include: - docker_tag: focal-deps docker_tags: | @@ -52,6 +60,13 @@ jobs: linux/amd64 linux/arm64 docker_path: 'noble-kilted/deps' + - docker_tag: resolute-deps + docker_tags: | + introlab3it/rtabmap:resolute-deps + docker_platforms: | + linux/amd64 + linux/arm64 + docker_path: 'resolute/deps' steps: - @@ -85,12 +100,14 @@ jobs: docker: needs: docker_deps + # Run even when docker_deps is skipped (it is, on pull requests). + if: ${{ !cancelled() && !failure() }} runs-on: ubuntu-latest strategy: fail-fast: false matrix: - docker_tag: [bionic, focal, jammy, noble, noble-kilted, android23, android24, android26, android30] + docker_tag: [bionic, focal, jammy, noble, noble-kilted, resolute, android23, android24, android26, android30] include: - docker_tag: bionic docker_tags: | @@ -142,6 +159,16 @@ jobs: linux/amd64 linux/arm64 docker_path: 'noble-kilted' + - docker_tag: resolute + docker_tags: | + introlab3it/rtabmap:resolute + introlab3it/rtabmap:26.04 + docker_args: | + NOT_USED=0 + docker_platforms: | + linux/amd64 + linux/arm64 + docker_path: 'resolute' - docker_tag: android23 docker_tags: | introlab3it/rtabmap:android23 @@ -190,6 +217,9 @@ jobs: uses: docker/setup-buildx-action@v3 - name: Login to DockerHub + # Only needed when pushing; skipped on pull requests (secrets are + # unavailable for fork PRs and we don't push there anyway). + if: github.event_name != 'pull_request' uses: docker/login-action@v3 with: username: ${{ secrets.DOCKERHUB_USERNAME }} @@ -199,8 +229,8 @@ jobs: uses: docker/build-push-action@v6 with: context: . - push: true - platforms: ${{ matrix.docker_platforms }} + push: ${{ github.event_name != 'pull_request' }} + platforms: ${{ github.event_name == 'pull_request' && 'linux/amd64' || matrix.docker_platforms }} file: ./docker/${{ matrix.docker_path }}/Dockerfile build-args: | ${{ matrix.docker_args }} diff --git a/.github/workflows/ios.yml b/.github/workflows/ios.yml new file mode 100644 index 00000000..a035941d --- /dev/null +++ b/.github/workflows/ios.yml @@ -0,0 +1,114 @@ +name: iOS + +on: + push: + branches: + - master + paths: &ios_paths + - '.github/workflows/ios.yml' + - 'app/ios/**' + - 'app/android/jni/**' + - 'corelib/**' + - 'utilite/**' + - 'cmake_modules/**' + - 'CMakeLists.txt' + pull_request: + branches: + - '**' + paths: *ios_paths + +concurrency: + group: ${{ github.workflow }}-${{ github.event.pull_request.number || github.ref }} + cancel-in-progress: ${{ github.event_name == 'pull_request' }} + +env: + # Pre-built iOS dependencies (content of app/ios/RTABMapApp/Libraries, generated by install_deps.sh). + # Bump this when the dependency set changes (must match the Xcode toolchain below). + DEPS_URL: https://github.com/introlab/rtabmap/releases/download/0.23.1/libraries-ios-xcode26.5.zip + XCODE_VERSION: '26.5' + +jobs: + build: + name: build-ios + # macos-26 (Tahoe) ships Xcode 26.x, matching the toolchain used to build the prebuilt libraries. + runs-on: macos-26 + + steps: + - uses: actions/checkout@v4 + + - name: Select Xcode ${{ env.XCODE_VERSION }} + uses: maxim-lobanov/setup-xcode@v1 + with: + xcode-version: ${{ env.XCODE_VERSION }} + + - name: Versions + run: | + xcodebuild -version + cmake --version || brew install cmake + + - name: Cache prebuilt dependencies archive + id: deps-cache + uses: actions/cache@v4 + with: + path: deps.zip + # Keyed on the archive URL (release tag + filename), so the cache is + # reused until DEPS_URL is bumped, regardless of other workflow edits. + key: ${{ runner.os }}-ios-deps-${{ env.DEPS_URL }} + + - name: Download prebuilt dependencies + if: steps.deps-cache.outputs.cache-hit != 'true' + run: curl -L "$DEPS_URL" -o deps.zip + + - name: Extract dependencies into Libraries + run: | + set -eux + mkdir -p app/ios/RTABMapApp/Libraries + rm -rf deps_extract && mkdir -p deps_extract + unzip -q deps.zip -d deps_extract + # The archive holds the *content* of the Libraries folder (include/ lib/ share/), + # but tolerate an extra top-level Libraries/ wrapper just in case. + if [ -d deps_extract/Libraries ]; then + SRC=deps_extract/Libraries + else + SRC=deps_extract + fi + cp -R "$SRC"/. app/ios/RTABMapApp/Libraries/ + test -d app/ios/RTABMapApp/Libraries/include + test -d app/ios/RTABMapApp/Libraries/lib + + - name: Build rtabmap core (third-party deps are skipped, already provided by the archive) + working-directory: app/ios/RTABMapApp + run: ./install_deps.sh + + - name: Build RTABMapApp + run: | + xcodebuild \ + -project app/ios/RTABMapApp.xcodeproj \ + -scheme RTABMapApp \ + -configuration Release \ + -sdk iphoneos \ + -destination 'generic/platform=iOS' \ + -derivedDataPath build \ + CODE_SIGNING_ALLOWED=NO \ + CODE_SIGNING_REQUIRED=NO \ + CODE_SIGN_IDENTITY="" \ + DEVELOPMENT_TEAM="" \ + build + + - name: Package app (unsigned .ipa) + run: | + set -eux + APP_DIR="build/Build/Products/Release-iphoneos" + rm -rf Payload && mkdir Payload + cp -R "$APP_DIR/RTABMapApp.app" Payload/ + # Unsigned .ipa: not installable as-is, but ready for later (re)signing. + zip -q -r RTABMapApp-unsigned.ipa Payload + + - name: Upload app artifact + uses: actions/upload-artifact@v4 + with: + name: RTABMapApp-ios-unsigned + path: RTABMapApp-unsigned.ipa + compression-level: 0 + if-no-files-found: error + retention-days: ${{ github.event_name == 'pull_request' && 1 || 90 }} diff --git a/CMakeLists.txt b/CMakeLists.txt index b7d36de7..3fd1801d 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,5 +1,6 @@ # Top-Level CmakeLists.txt cmake_minimum_required(VERSION 3.14) + PROJECT( RTABMap ) SET(PROJECT_PREFIX rtabmap) @@ -11,6 +12,7 @@ IF(NOT MULTI_ARCH AND NOT DEFINED CMAKE_INSTALL_LIBDIR) ENDIF(NOT MULTI_ARCH AND NOT DEFINED CMAKE_INSTALL_LIBDIR) INCLUDE(GNUInstallDirs) +INCLUDE(FetchContent) ####### local cmake modules ####### SET(CMAKE_MODULE_PATH "${PROJECT_SOURCE_DIR}/cmake_modules") @@ -19,10 +21,18 @@ SET(CMAKE_MODULE_PATH "${PROJECT_SOURCE_DIR}/cmake_modules") # VERSION ####################### SET(RTABMAP_MAJOR_VERSION 0) -SET(RTABMAP_MINOR_VERSION 22) -SET(RTABMAP_PATCH_VERSION 1) +SET(RTABMAP_MINOR_VERSION 23) +SET(RTABMAP_PATCH_VERSION 7) SET(RTABMAP_VERSION ${RTABMAP_MAJOR_VERSION}.${RTABMAP_MINOR_VERSION}.${RTABMAP_PATCH_VERSION}) + +# Make sure we have valid version so that RTABMAP_VERSION_COMPARE logic in Version.h works +IF(RTABMAP_MINOR_VERSION GREATER 99) +MESSAGE(FATAL_ERROR "RTABMAP_MINOR_VERSION must be < 100, bump major version and restart minor to 0!") +ENDIF() +IF(RTABMAP_PATCH_VERSION GREATER 99) +MESSAGE(FATAL_ERROR "RTABMAP_PATCH_VERSION must be < 100, bump minor version and restart patch to 0!") +ENDIF() SET(PROJECT_VERSION "${RTABMAP_VERSION}") @@ -83,14 +93,6 @@ IF(MINGW) SET(CMAKE_SHARED_LINKER_FLAGS "-Wl,--enable-auto-import") ENDIF(MINGW) -# GCC 4 required -IF(UNIX OR MINGW) - EXEC_PROGRAM( gcc ARGS "-dumpversion" OUTPUT_VARIABLE GCC_VERSION ) - IF(GCC_VERSION VERSION_LESS "4.0.0") - MESSAGE(FATAL_ERROR "GCC ${GCC_VERSION} found, but version 4.x.x minimum is required") - ENDIF(GCC_VERSION VERSION_LESS "4.0.0") -ENDIF(UNIX OR MINGW) - #The CDT Error Parser cannot handle error messages that span #more than one line, which is the default gcc behavior. #In order to force gcc to generate single line error messages with no line wrapping @@ -198,15 +200,17 @@ option(WITH_CCCORELIB "Include CCCoreLib support" OFF) option(WITH_OPEN3D "Include Open3D support" OFF) option(WITH_LOAM "Include LOAM support" OFF) option(WITH_FLOAM "Include FLOAM support" OFF) +option(WITH_LIOSAM "Include LIO-SAM support" OFF) option(WITH_FLYCAPTURE2 "Include FlyCapture2/Triclops support" ON) option(WITH_ZED "Include ZED sdk support" ON) option(WITH_ZEDOC "Include ZED Open Capture support" ON) option(WITH_REALSENSE "Include RealSense support" ON) option(WITH_REALSENSE_SLAM "Include RealSenseSlam support" ON) -option(WITH_REALSENSE2 "Include RealSense support" ON) +option(WITH_REALSENSE2 "Include RealSense2 support" ON) option(WITH_MYNTEYE "Include mynteye-s support" ON) option(WITH_DEPTHAI "Include depthai-core support" OFF) option(WITH_XVSDK "Include XVisio SDK support" OFF) +option(WITH_ORBBEC_SDK "Include Orbbec SDK v2 support" OFF) option(WITH_OCTOMAP "Include OctoMap support" ON) option(WITH_GRIDMAP "Include GridMap support" OFF) option(WITH_CPUTSDF "Include CPUTSDF support" OFF) @@ -218,12 +222,15 @@ option(WITH_DVO "Include DVO support" OFF) option(WITH_ORB_SLAM "Include ORB_SLAM2 or ORB_SLAM3 support" OFF) option(WITH_OKVIS "Include OKVIS support" OFF) option(WITH_MSCKF_VIO "Include MSCKF_VIO support" OFF) -option(WITH_VINS "Include VINS-Fusion support" OFF) +option(WITH_VINS_FUSION "Include VINS-Fusion support" OFF) option(WITH_OPENVINS "Include OpenVINS support" OFF) +option(WITH_CUVSLAM "Include cuVSLAM 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" ON) +option(BUILD_OPENGV "Build OpenGV internally instead of using the system one" OFF) +option(WITH_APRILTAG "Include AprilTag support" OFF) IF(MOBILE_BUILD) option(PCL_OMP "With PCL OMP implementations" OFF) ELSE() @@ -396,7 +403,7 @@ IF(NOT VTK_FOUND) ENDIF(NOT VTK_FOUND) IF(WITH_TORCH) - FIND_PACKAGE(Torch QUIET) + FIND_PACKAGE(Torch) IF(TORCH_FOUND) MESSAGE(STATUS "Found Torch: ${TORCH_INCLUDE_DIRS}") ENDIF(TORCH_FOUND) @@ -418,14 +425,14 @@ IF(WITH_PDAL) ENDIF(WITH_PDAL) IF(WITH_LIBLAS) - FIND_PACKAGE(libLAS QUIET) + FIND_PACKAGE(libLAS) IF(libLAS_FOUND) MESSAGE(STATUS "Found libLAS ${libLAS_VERSION}: ${libLAS_INCLUDE_DIRS}") ENDIF(libLAS_FOUND) ENDIF(WITH_LIBLAS) IF(WITH_CUDASIFT) - FIND_PACKAGE(CudaSift 3 QUIET) + FIND_PACKAGE(CudaSift 3) IF(CudaSift_FOUND) MESSAGE(STATUS "Found CudaSift") ENDIF(CudaSift_FOUND) @@ -496,35 +503,34 @@ ENDIF(WITH_DC1394) IF(WITH_G2O) 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 + 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 PATHS ${G2O_INCLUDES} NO_DEFAULT_PATH) - FILE(READ ${G2O_FACTORY_FILE} TMPTXT) - STRING(FIND "${TMPTXT}" "shared_ptr" matchres) - IF(${matchres} EQUAL -1) + FILE(READ ${G2O_FACTORY_FILE} TMPTXT) + STRING(FIND "${TMPTXT}" "shared_ptr" matchres) + IF(${matchres} EQUAL -1) MESSAGE(STATUS "Old g2o factory version detected without shared ptr (factory file: ${G2O_FACTORY_FILE}).") SET(G2O_CPP11 2) - ELSE() + ELSE() MESSAGE(STATUS "Latest g2o factory version detected with shared ptr (factory file: ${G2O_FACTORY_FILE}).") SET(G2O_CPP11 1) - ENDIF() + ENDIF() + FIND_FILE(G2O_SBA_UTILS_FILE g2o/types/sba/sba_utils.h + PATHS ${G2O_INCLUDES} + NO_DEFAULT_PATH) + IF(G2O_SBA_UTILS_FILE) + SET(G2O_WITH_SBA_UTILS 1) + ELSE() + SET(G2O_WITH_SBA_UTILS 0) + ENDIF() ELSE() FIND_PACKAGE(G2O QUIET) IF(G2O_FOUND) MESSAGE(STATUS "Found g2o: ${G2O_INCLUDE_DIRS}") - FIND_FILE(G2O_FACTORY_FILE g2o/core/factory.h - PATHS ${G2O_INCLUDES} - NO_DEFAULT_PATH) - FILE(READ ${G2O_FACTORY_FILE} TMPTXT) - STRING(FIND "${TMPTXT}" "shared_ptr" matchres) - IF(NOT ${matchres} EQUAL -1) - MESSAGE(STATUS "Latest g2o factory version detected with shared ptr (factory file: ${G2O_FACTORY_FILE}).") - SET(G2O_CPP11 1) - ENDIF() ENDIF(G2O_FOUND) ENDIF() ENDIF(WITH_G2O) @@ -532,6 +538,16 @@ ENDIF(WITH_G2O) IF(WITH_GTSAM) # Force config mode to ignore PCL's findGTSAM.cmake file FIND_PACKAGE(GTSAM CONFIG QUIET) + IF(GTSAM_FOUND) + # For issue https://github.com/introlab/rtabmap/pull/1626 + FIND_FILE(GTSAM_NOISE_MODEL_FACTOR_N_FILE gtsam/nonlinear/NoiseModelFactorN.h + PATHS ${GTSAM_INCLUDE_DIR} + NO_DEFAULT_PATH) + IF(GTSAM_NOISE_MODEL_FACTOR_N_FILE) + MESSAGE(STATUS "GTSAM with NoiseModelFactorN.h") + ADD_DEFINITIONS("-DGTSAM_WITH_NOISE_MODEL_FACTOR_N") + ENDIF(GTSAM_NOISE_MODEL_FACTOR_N_FILE) + ENDIF(GTSAM_FOUND) ENDIF(WITH_GTSAM) IF(WITH_MRPT) @@ -550,7 +566,7 @@ IF(WITH_FLYCAPTURE2) ENDIF(WITH_FLYCAPTURE2) IF(WITH_CVSBA) - FIND_PACKAGE(cvsba QUIET) + FIND_PACKAGE(cvsba) IF(cvsba_FOUND) MESSAGE(STATUS "Found cvsba: ${cvsba_INCLUDE_DIRS}") ENDIF(cvsba_FOUND) @@ -576,10 +592,14 @@ IF(WITH_POINTMATCHER) ENDIF(WITH_POINTMATCHER) IF(libpointmatcher_FOUND OR GTSAM_FOUND) - find_package(Boost COMPONENTS thread filesystem system program_options date_time REQUIRED) - IF(Boost_MINOR_VERSION GREATER 47) + find_package(Boost COMPONENTS thread filesystem program_options date_time REQUIRED) + IF(Boost_MINOR_VERSION GREATER 80) + find_package(Boost COMPONENTS thread filesystem program_options date_time chrono timer serialization REQUIRED) + ELSEIF(Boost_MINOR_VERSION GREATER 47) find_package(Boost COMPONENTS thread filesystem system program_options date_time chrono timer serialization REQUIRED) - ENDIF(Boost_MINOR_VERSION GREATER 47) + ELSE() + find_package(Boost COMPONENTS thread filesystem system program_options date_time REQUIRED) + ENDIF() IF(WIN32) MESSAGE(STATUS "Boost_LIBRARY_DIRS=${Boost_LIBRARY_DIRS}") link_directories(${Boost_LIBRARY_DIRS}) @@ -587,7 +607,7 @@ IF(libpointmatcher_FOUND OR GTSAM_FOUND) ENDIF(libpointmatcher_FOUND OR GTSAM_FOUND) IF(WITH_CCCORELIB) - find_package(CCCoreLib QUIET) + find_package(CCCoreLib) IF(CCCoreLib_FOUND) MESSAGE(STATUS "Found CCCoreLib: ${CCCoreLib_INCLUDE_DIRS}") ENDIF(CCCoreLib_FOUND) @@ -599,7 +619,7 @@ IF(WITH_OPEN3D) ELSE() # Build Open3D like this to avoid linker errors in rtabmap: # cmake -DBUILD_SHARED_LIBS=ON -DGLIBCXX_USE_CXX11_ABI=ON -DCMAKE_BUILD_TYPE=Release .. - find_package(Open3D QUIET) + find_package(Open3D) IF(Open3D_FOUND) MESSAGE(STATUS "Found Open3D: ${Open3DINCLUDE_DIRS}") ENDIF(Open3D_FOUND) @@ -607,19 +627,25 @@ IF(WITH_OPEN3D) ENDIF(WITH_OPEN3D) IF(WITH_LOAM) - find_package(loam_velodyne QUIET) + find_package(loam_velodyne) IF(loam_velodyne_FOUND) MESSAGE(STATUS "Found loam_velodyne: ${loam_velodyne_INCLUDE_DIRS}") ENDIF(loam_velodyne_FOUND) ENDIF(WITH_LOAM) IF(WITH_FLOAM) - find_package(floam QUIET) + find_package(floam) IF(floam_FOUND) MESSAGE(STATUS "Found floam: ${floam_INCLUDE_DIRS}") - FIND_PACKAGE(Ceres QUIET REQUIRED) + FIND_PACKAGE(Ceres REQUIRED) ENDIF(floam_FOUND) ENDIF(WITH_FLOAM) +IF(WITH_LIOSAM) + find_package(lio_sam QUIET) + IF(lio_sam_FOUND) + MESSAGE(STATUS "Found lio_sam: ${lio_sam_INCLUDE_DIRS}") + ENDIF(lio_sam_FOUND) +ENDIF(WITH_LIOSAM) SET(ZED_FOUND FALSE) IF(WITH_ZED) @@ -632,7 +658,8 @@ IF(WITH_ZED) IF(CUDA_FOUND) MESSAGE(STATUS "Found CUDA: ${CUDA_INCLUDE_DIRS}") ELSE() - MESSAGE(FATAL_ERROR "CUDA is required to build with Zed sdk! Set -DWITH_ZED=OFF if you don't have CUDA.") + MESSAGE(WARNING "CUDA is required to build with Zed sdk! Set -DWITH_ZED=OFF if you don't have CUDA.") + SET(ZED_FOUND FALSE) ENDIF() ENDIF(ZED_FOUND) ENDIF(WITH_ZED) @@ -684,19 +711,26 @@ IF(WITH_MYNTEYE) ENDIF(WITH_MYNTEYE) IF(WITH_DEPTHAI) - FIND_PACKAGE(depthai 2.24 QUIET) + FIND_PACKAGE(depthai 2.24) IF(depthai_FOUND) MESSAGE(STATUS "Found depthai-core (targets)") ENDIF(depthai_FOUND) ENDIF(WITH_DEPTHAI) IF(WITH_XVSDK) - FIND_PACKAGE(xvsdk QUIET) + FIND_PACKAGE(xvsdk) IF(xvsdk_FOUND) MESSAGE(STATUS "Found xvsdk (targets)") ENDIF(xvsdk_FOUND) ENDIF(WITH_XVSDK) +IF(WITH_ORBBEC_SDK) + FIND_PACKAGE(OrbbecSDK 2) + IF(OrbbecSDK_FOUND) + MESSAGE(STATUS "Found OrbbecSDK v2 (targets)") + ENDIF(OrbbecSDK_FOUND) +ENDIF(WITH_ORBBEC_SDK) + IF(WITH_OCTOMAP) FIND_PACKAGE(octomap QUIET) IF(octomap_FOUND) @@ -708,35 +742,35 @@ IF(WITH_OCTOMAP) ENDIF(WITH_OCTOMAP) IF(WITH_GRIDMAP) - FIND_PACKAGE(grid_map_core QUIET) + FIND_PACKAGE(grid_map_core) IF(grid_map_core_FOUND) MESSAGE(STATUS "Found grid_map_core ${grid_map_core_VERSION}: ${grid_map_core_INCLUDE_DIRS}") ENDIF(grid_map_core_FOUND) ENDIF(WITH_GRIDMAP) IF(WITH_CPUTSDF) - FIND_PACKAGE(CPUTSDF QUIET) + FIND_PACKAGE(CPUTSDF) IF(CPUTSDF_FOUND) MESSAGE(STATUS "Found CPUTSDF: ${CPUTSDF_INCLUDE_DIRS}") ENDIF(CPUTSDF_FOUND) ENDIF(WITH_CPUTSDF) IF(WITH_OPENCHISEL) - find_package(open_chisel QUIET) + find_package(open_chisel) if(open_chisel_FOUND) MESSAGE(STATUS "Found open_chisel: ${open_chisel_INCLUDE_DIRS}") endif(open_chisel_FOUND) ENDIF(WITH_OPENCHISEL) IF(WITH_ALICE_VISION) - find_package(AliceVision CONFIG QUIET) + find_package(AliceVision CONFIG) IF(AliceVision_FOUND) IF(${AliceVision_VERSION} VERSION_LESS_EQUAL "2.2") find_package(Boost COMPONENTS log log_setup container REQUIRED) ENDIF(${AliceVision_VERSION} VERSION_LESS_EQUAL "2.2") SET(CMAKE_MODULE_PATH "${CMAKE_MODULE_PATH};/usr/local/lib/cmake/modules") - find_package(Geogram REQUIRED QUIET) - find_package(assimp QUIET) + find_package(Geogram REQUIRED) + find_package(assimp) add_definitions("-DRTABMAP_ALICE_VISION_MAJOR=${AliceVision_VERSION_MAJOR}") add_definitions("-DRTABMAP_ALICE_VISION_MINOR=${AliceVision_VERSION_MINOR}") add_definitions("-DRTABMAP_ALICE_VISION_PATCH=${AliceVision_VERSION_PATCH}") @@ -744,34 +778,32 @@ IF(WITH_ALICE_VISION) ENDIF(WITH_ALICE_VISION) IF(WITH_FOVIS) - FIND_PACKAGE(libfovis QUIET) + FIND_PACKAGE(libfovis) IF(libfovis_FOUND) MESSAGE(STATUS "Found libfovis: ${libfovis_INCLUDE_DIRS}") ENDIF(libfovis_FOUND) ENDIF(WITH_FOVIS) IF(WITH_VISO2) - FIND_PACKAGE(libviso2 QUIET) + FIND_PACKAGE(libviso2) IF(libviso2_FOUND) MESSAGE(STATUS "Found libviso2: ${libviso2_INCLUDE_DIRS}") ENDIF(libviso2_FOUND) ENDIF(WITH_VISO2) IF(WITH_DVO) - FIND_PACKAGE(dvo_core QUIET) + FIND_PACKAGE(dvo_core) IF(dvo_core_FOUND) MESSAGE(STATUS "Found dvo_core: ${dvo_core_INCLUDE_DIRS}") ENDIF(dvo_core_FOUND) ENDIF(WITH_DVO) IF(WITH_OKVIS) - FIND_PACKAGE(okvis 1.1 QUIET) + FIND_PACKAGE(okvis 1.1) IF(okvis_FOUND) MESSAGE(STATUS "Found okvis: ${OKVIS_INCLUDE_DIRS}") find_package(brisk 2 REQUIRED) MESSAGE(STATUS "Found brisk: ${BRISK_INCLUDE_DIRS}") - find_package(opengv REQUIRED) - MESSAGE(STATUS "Found opengv: ${OPENGV_INCLUDE_DIRS}") find_package(Ceres 1.9.0 REQUIRED EXACT) # OKVIS requires this specific version MESSAGE(STATUS "Found ceres ${Ceres_VERSION}: ${CERES_INCLUDE_DIRS}") ENDIF(okvis_FOUND) @@ -780,7 +812,7 @@ ENDIF(WITH_OKVIS) # If built with okvis, we found already ceres above IF(WITH_CERES) IF(NOT okvis_FOUND AND NOT floam_FOUND) - FIND_PACKAGE(Ceres QUIET) + FIND_PACKAGE(Ceres) MESSAGE(STATUS "Found ceres ${Ceres_VERSION}: ${CERES_INCLUDE_DIRS}") ENDIF(NOT okvis_FOUND AND NOT floam_FOUND) ELSEIF(Ceres_FOUND) @@ -788,39 +820,29 @@ ELSEIF(Ceres_FOUND) ENDIF() IF(WITH_MSCKF_VIO) - FIND_PACKAGE(msckf_vio QUIET) + FIND_PACKAGE(msckf_vio) IF(msckf_vio_FOUND) MESSAGE(STATUS "Found msckf_vio: ${msckf_vio_INCLUDE_DIRS}") ENDIF(msckf_vio_FOUND) ENDIF(WITH_MSCKF_VIO) -IF(WITH_VINS) - FIND_PACKAGE(vins QUIET) +IF(WITH_VINS AND NOT WITH_VINS_FUSION) +message(DEPRECATION "The option WITH_VINS is deprecated and will be removed in a future version. Please use WITH_VINS_FUSION instead.") +set(WITH_VINS_FUSION ON) +ENDIF(WITH_VINS AND NOT WITH_VINS_FUSION) + +IF(WITH_VINS_FUSION) + FIND_PACKAGE(vins) IF(vins_FOUND) - MESSAGE(STATUS "Found vins: ${vins_INCLUDE_DIRS}") + MESSAGE(STATUS "Found vins-fusion: ${vins_INCLUDE_DIRS}") IF(okvis_FOUND) - MESSAGE(WARNING "VINS and OKVIS will be both linked to project, make sure VINS has been built with against same Ceres version than OKVIS to avoid some crashes.") + MESSAGE(WARNING "VINS-Fusion and OKVIS will be both linked to project, make sure VINS-Fusion has been built with against same Ceres version than OKVIS to avoid some crashes.") ENDIF(okvis_FOUND) ENDIF(vins_FOUND) -ENDIF(WITH_VINS) +ENDIF(WITH_VINS_FUSION) IF(WITH_OPENVINS) - FIND_PACKAGE(ov_msckf QUIET) - # On ROS2, the indirect includes and libraries - # are not forwarded by ov_msckf target, append them manually - FIND_PACKAGE(ov_core QUIET) - FIND_PACKAGE(ov_init QUIET) - IF(ov_msckf_FOUND AND ov_core_FOUND AND ov_init_FOUND) - SET(ov_msckf_INCLUDE_DIRS - ${ov_msckf_INCLUDE_DIRS} - ${ov_core_INCLUDE_DIRS} - ${ov_init_INCLUDE_DIRS}) - SET(ov_msckf_LIBRARIES - ${ov_msckf_LIBRARIES} - ${ov_core_LIBRARIES} - ${ov_init_LIBRARIES}) - MESSAGE(STATUS "Found OpenVINS: ${ov_msckf_INCLUDE_DIRS}") - ENDIF() + FIND_PACKAGE(OpenVINS) ENDIF(WITH_OPENVINS) IF(WITH_FASTCV) @@ -830,27 +852,97 @@ IF(WITH_FASTCV) ENDIF(FastCV_FOUND) ENDIF(WITH_FASTCV) -IF(WITH_OPENGV) - FIND_PACKAGE(opengv QUIET) - IF(opengv_FOUND) - MESSAGE(STATUS "Found OpenGV: ${opengv_INCLUDE_DIRS}") - ENDIF(opengv_FOUND) -ENDIF(WITH_OPENGV) +IF(WITH_APRILTAG) + FIND_PACKAGE(apriltag QUIET) + IF(apriltag_FOUND) + get_target_property(APRILTAG_LOCATION apriltag::apriltag LOCATION) + get_target_property(APRILTAG_INCLUDES apriltag::apriltag INTERFACE_INCLUDE_DIRECTORIES) + FIND_FILE(apriltag_aruco_4x4_50_header NAMES tagAruco4x4_50.h PATH_SUFFIXES aruco PATHS ${APRILTAG_INCLUDES} NO_DEFAULT_PATH) + SET(WITH_APRILTAG_ARUCO NO) + IF(apriltag_aruco_4x4_50_header) + SET(WITH_APRILTAG_ARUCO YES) + ENDIF() + MESSAGE(STATUS "Found apriltag (with aruco=${WITH_APRILTAG_ARUCO}): ${APRILTAG_LOCATION} ${APRILTAG_INCLUDES}") + ENDIF(apriltag_FOUND) +ENDIF(WITH_APRILTAG) + +IF(WITH_OPENGV OR okvis_FOUND) + if(NOT BUILD_OPENGV) + FIND_PACKAGE(opengv QUIET) + endif() + + if(opengv_FOUND) + MESSAGE(STATUS "Found system-installed OpenGV: ${opengv_INCLUDE_DIRS}") + elseif(BUILD_OPENGV) + SET(PCL_USING_MARCHNATIVE OFF) + if(PCL_COMPILE_OPTIONS) + if("${PCL_COMPILE_OPTIONS}" MATCHES "-march=native") + set(PCL_USING_MARCHNATIVE ON) + endif() + elseif("${PCL_DEFINITIONS}" MATCHES "-march=native") + set(PCL_USING_MARCHNATIVE ON) + endif() + SET(MSG_EXTRA "without -march-native (not used by PCL)") + if(PCL_USING_MARCHNATIVE) + set(MSG_EXTRA "with -march-native (used by PCL)") + endif() + + message(STATUS "Download/Build OpenGV internally (BUILD_OPENGV=ON) ${MSG_EXTRA}.") + function(add_submodule_opengv) + FetchContent_Declare( + opengv + GIT_REPOSITORY https://github.com/laurentkneip/opengv.git + GIT_TAG 91f4b19c73450833a40e463ad3648aae80b3a7f3 + PATCH_COMMAND ${CMAKE_COMMAND} + -DPATCH_FILE=${CMAKE_CURRENT_LIST_DIR}/patches/opengv_91f4b19c.patch + -P ${CMAKE_CURRENT_LIST_DIR}/patches/apply_patch.cmake + ) + set(BUILD_SHARED_LIBS OFF) + set(BUILD_TESTS OFF) + set(CMAKE_BUILD_TYPE Release) + set(CMAKE_POLICY_DEFAULT_CMP0077 NEW) + # OpenGV's CMakeLists.txt declares cmake_minimum_required(VERSION 2.x), + # which CMake >= 4.0 (e.g. recent Ubuntu) rejects. Allow it to configure. + set(CMAKE_POLICY_VERSION_MINIMUM 3.5) + # Eigen should have been already added by PCL, just populate the compatible variables + IF(EIGEN_INCLUDE_DIRS) + set(EIGEN_INCLUDE_DIRS "${EIGEN_INCLUDE_DIRS}" CACHE PATH "Eigen include dirs" FORCE) + set(EIGEN_INCLUDE_DIR "${EIGEN_INCLUDE_DIRS}" CACHE PATH "Eigen include dir" FORCE) + ELSEIF(Eigen3_INCLUDE_DIRS) + set(EIGEN_INCLUDE_DIRS "${Eigen3_INCLUDE_DIRS}" CACHE PATH "Eigen include dirs" FORCE) + set(EIGEN_INCLUDE_DIR "${Eigen3_INCLUDE_DIRS}" CACHE PATH "Eigen include dir" FORCE) + ENDIF() + set(BUILD_WITH_MARCHNATIVE ${PCL_USING_MARCHNATIVE}) + FetchContent_MakeAvailable(opengv) + endfunction() + + add_submodule_opengv() + set(opengv_FOUND TRUE) + set(opengv_VERSION "internal") + endif() +ENDIF() IF(WITH_ORB_SLAM AND NOT G2O_FOUND) - FIND_PACKAGE(ORB_SLAM QUIET) + FIND_PACKAGE(ORB_SLAM) IF(ORB_SLAM_FOUND) MESSAGE(STATUS "Found ORB_SLAM${ORB_SLAM_VERSION}: ${ORB_SLAM_INCLUDE_DIRS}") ENDIF(ORB_SLAM_FOUND) ENDIF(WITH_ORB_SLAM AND NOT G2O_FOUND) +IF(WITH_CUVSLAM) + FIND_PACKAGE(CuVSLAM 14.0.0) + IF(CUVSLAM_FOUND) + MESSAGE(STATUS "Found cuVSLAM: ${CUVSLAM_INCLUDE_DIRS}") + ENDIF() +ENDIF(WITH_CUVSLAM) + SET(DISABLE_NEW_DTAGS_FLAG "--disable-new-dtags") IF(NOT (APPLE OR WIN32) AND BUILD_WITH_RPATH_NOT_RUNPATH) ADD_LINK_OPTIONS(LINKER:${DISABLE_NEW_DTAGS_FLAG}) ENDIF() IF(NOT MSVC) - IF(Qt6_FOUND OR (G2O_FOUND AND G2O_CPP11 EQUAL 1) OR TORCH_FOUND OR MRPT_FOUND) + IF(Qt6_FOUND OR (G2O_FOUND AND G2O_CPP11 EQUAL 1) OR (GTSAM_FOUND AND GTSAM_VERSION VERSION_GREATER_EQUAL "4.3.0") OR TORCH_FOUND OR MRPT_FOUND) # Qt6 requires c++17 include(CheckCXXCompilerFlag) CHECK_CXX_COMPILER_FLAG("-std=c++17" COMPILER_SUPPORTS_CXX17) @@ -938,10 +1030,14 @@ ENDIF() IF(NOT G2O_FOUND) SET(G2O "//") SET(G2O_CPP_CONF "//") + SET(G2O_WITH_SBA_UTILS "//") ELSE() IF(NOT G2O_CPP11) SET(G2O_CPP_CONF "//") ENDIF(NOT G2O_CPP11) + IF(NOT G2O_WITH_SBA_UTILS) + SET(G2O_WITH_SBA_UTILS_CONF "//") + ENDIF(NOT G2O_WITH_SBA_UTILS) ENDIF() IF(NOT GTSAM_FOUND) SET(GTSAM "//") @@ -973,6 +1069,12 @@ ENDIF(NOT Open3D_FOUND) IF(NOT FastCV_FOUND) SET(FASTCV "//") ENDIF(NOT FastCV_FOUND) +IF(NOT apriltag_FOUND) + SET(APRILTAG "//") + SET(APRILTAG_ARUCO "//") +ELSEIF(NOT WITH_APRILTAG_ARUCO) + SET(APRILTAG_ARUCO "//") +ENDIF() IF(NOT opengv_FOUND OR NOT WITH_OPENGV) SET(OPENGV "//") ENDIF(NOT opengv_FOUND OR NOT WITH_OPENGV) @@ -991,6 +1093,9 @@ ENDIF(NOT loam_velodyne_FOUND) IF(NOT floam_FOUND) SET(FLOAM "//") ENDIF(NOT floam_FOUND) +IF(NOT lio_sam_FOUND) + SET(LIOSAM "//") +ENDIF(NOT lio_sam_FOUND) IF(NOT Freenect_FOUND) SET(FREENECT "//") ENDIF() @@ -1067,6 +1172,9 @@ IF(NOT xvsdk_FOUND) ELSE() SET(CONF_WITH_XVSDK 1) ENDIF() +IF(NOT OrbbecSDK_FOUND) + SET(ORBBEC_SDK "//") +ENDIF(NOT OrbbecSDK_FOUND) IF(NOT octomap_FOUND) SET(OCTOMAP "//") SET(CONF_WITH_OCTOMAP 0) @@ -1101,11 +1209,14 @@ IF(NOT msckf_vio_FOUND) SET(MSCKF_VIO "//") ENDIF() IF(NOT vins_FOUND) - SET(VINS "//") + SET(VINSFUSION "//") ENDIF() -IF(NOT ov_msckf_FOUND) +IF(NOT OpenVINS_FOUND) SET(OPENVINS "//") ENDIF() +IF(NOT CUVSLAM_FOUND) + SET(CUVSLAM "//") +ENDIF() IF(NOT ORB_SLAM_FOUND) SET(ORB_SLAM "//") ENDIF() @@ -1249,7 +1360,17 @@ install(FILES package.xml DESTINATION "${CMAKE_INSTALL_DATAROOTDIR}/${PROJECT_PR ####################### IF(BUILD_AS_BUNDLE) SET(CMAKE_INSTALL_SYSTEM_RUNTIME_COMPONENT runtime) + IF(WIN32) + set(CMAKE_INSTALL_SYSTEM_RUNTIME_LIBS_SKIP TRUE) + set(CPACK_NSIS_EXTRA_INSTALL_COMMANDS " + ExecWait '\\\"$INSTDIR\\\\vc_redist.x64.exe\\\" /quiet /norestart' + ") + ENDIF() INCLUDE(InstallRequiredSystemLibraries) + set(CPACK_NSIS_COMPONENT_INSTALL OFF) + set(CPACK_ARCHIVE_COMPONENT_INSTALL ON) + set(CPACK_COMPONENTS_GROUPING ALL_COMPONENTS_IN_ONE) + set(CPACK_COMPONENTS_ALL runtime) ENDIF(BUILD_AS_BUNDLE) SET(CPACK_PACKAGE_NAME "${PROJECT_NAME}") @@ -1363,28 +1484,33 @@ ENDIF(PCL_COMPILE_OPTIONS) MESSAGE(STATUS "") MESSAGE(STATUS "Optional dependencies ('*' affects some default parameters) :") IF(OpenCV_FOUND) + IF(OPENCV_ARUCO_FOUND) + set(ARUCO_STR "YES") + ELSE() + set(ARUCO_STR "NO") + ENDIF() IF(OpenCV_VERSION_MAJOR EQUAL 2) IF(OPENCV_NONFREE_FOUND) - MESSAGE(STATUS " *With OpenCV 2 nonfree module (SIFT/SURF) = YES (License: Non commercial)") + MESSAGE(STATUS " *With OpenCV 2 nonfree module (SIFT/SURF) = YES, aruco = ${ARUCO_STR} (License: Non commercial)") ELSE() - MESSAGE(STATUS " *With OpenCV 2 nonfree module (SIFT/SURF) = NO (not found, License: BSD)") + MESSAGE(STATUS " *With OpenCV 2 nonfree module (SIFT/SURF) = NO, aruco = ${ARUCO_STR} (not found, License: BSD)") ENDIF() ELSE() IF(OPENCV_XFEATURES2D_FOUND) IF(NONFREE STREQUAL "//") IF((OpenCV_VERSION_MAJOR LESS 4) OR ((OpenCV_VERSION_MAJOR EQUAL 4) AND (OpenCV_VERSION_MINOR LESS 5))) - MESSAGE(STATUS " *With OpenCV ${OpenCV_VERSION} xfeatures2d = YES, nonfree = NO (License: BSD)") + MESSAGE(STATUS " *With OpenCV ${OpenCV_VERSION} xfeatures2d = YES, nonfree = NO, aruco = ${ARUCO_STR} (License: BSD)") ELSE() - MESSAGE(STATUS " *With OpenCV ${OpenCV_VERSION} xfeatures2d = YES, nonfree = NO (License: Apache 2)") + MESSAGE(STATUS " *With OpenCV ${OpenCV_VERSION} xfeatures2d = YES, nonfree = NO, aruco = ${ARUCO_STR} (License: Apache 2)") ENDIF() ELSE() - MESSAGE(STATUS " *With OpenCV ${OpenCV_VERSION} xfeatures2d = YES, nonfree = YES (License: Non commercial)") + MESSAGE(STATUS " *With OpenCV ${OpenCV_VERSION} xfeatures2d = YES, nonfree = YES, aruco = ${ARUCO_STR} (License: Non commercial)") ENDIF() ELSE() IF((OpenCV_VERSION_MAJOR LESS 4) OR ((OpenCV_VERSION_MAJOR EQUAL 4) AND (OpenCV_VERSION_MINOR LESS 5))) - MESSAGE(STATUS " *With OpenCV ${OpenCV_VERSION} xfeatures2d = NO, nonfree = NO (License: BSD)") + MESSAGE(STATUS " *With OpenCV ${OpenCV_VERSION} xfeatures2d = NO, nonfree = NO, aruco = ${ARUCO_STR} (License: BSD)") ELSE() - MESSAGE(STATUS " *With OpenCV ${OpenCV_VERSION} xfeatures2d = NO, nonfree = NO (License: Apache 2)") + MESSAGE(STATUS " *With OpenCV ${OpenCV_VERSION} xfeatures2d = NO, nonfree = NO, aruco = ${ARUCO_STR} (License: Apache 2)") ENDIF() ENDIF() ENDIF() @@ -1419,15 +1545,26 @@ MESSAGE(STATUS " With ORB OcTree = NO (WITH_ORB_OCTREE=OFF)") ENDIF() IF(TORCH_FOUND) -MESSAGE(STATUS " With SuperPoint = YES (License: GPLv3) libtorch=${Torch_VERSION}") +MESSAGE(STATUS " With SuperPoint = YES (License: GPLv3) libtorch=${Torch_VERSION}") ELSEIF(NOT WITH_TORCH) -MESSAGE(STATUS " With SuperPoint = NO (WITH_TORCH=OFF)") +MESSAGE(STATUS " With SuperPoint = NO (WITH_TORCH=OFF)") ELSE() -MESSAGE(STATUS " With SuperPoint = NO (libtorch not found)") +MESSAGE(STATUS " With SuperPoint = NO (libtorch not found)") ENDIF() +IF(TORCH_FOUND AND WITH_PYTHON AND Python3_FOUND) +MESSAGE(STATUS " With Superpoint Rpautrat = YES (Liscense: MIT) libtorch=${Torch_VERSION}") +ELSEIF(NOT WITH_TORCH) +MESSAGE(STATUS " With Superpoint Rpautrat = NO (WITH_TORCH=OFF)") +ELSEIF(NOT WITH_PYTHON) +MESSAGE(STATUS " With Superpoint Rpautrat = NO (WITH_PYTHON=OFF)") +ELSE() +MESSAGE(STATUS " Wtih Superpoint Rpautrat = NO (libtorch and/or python3 not found)") +ENDIF() + + IF(WITH_PYTHON AND Python3_FOUND) -MESSAGE(STATUS " With Python${Python3_VERSION_MAJOR}.${Python3_VERSION_MINOR} = YES (License: PSF)") +MESSAGE(STATUS " With Python${Python3_VERSION_MAJOR}.${Python3_VERSION_MINOR} = YES (License: PSF)") ELSEIF(NOT WITH_PYTHON) MESSAGE(STATUS " With Python3 = NO (WITH_PYTHON=OFF)") ELSE() @@ -1448,6 +1585,14 @@ ELSE() MESSAGE(STATUS " With FastCV = NO (FastCV not found)") ENDIF() +IF(apriltag_FOUND) +MESSAGE(STATUS " With AprilTag ${apriltag_VERSION} = YES (aruco=${WITH_APRILTAG_ARUCO}) (License: BSD 2-Clause License)") +ELSEIF(NOT WITH_APRILTAG) +MESSAGE(STATUS " With AprilTag = NO (WITH_APRILTAG=OFF)") +ELSE() +MESSAGE(STATUS " With AprilTag = NO (apriltag not found)") +ENDIF() + IF(PDAL_FOUND) MESSAGE(STATUS " With PDAL ${PDAL_VERSION} = YES (License: BSD)") ELSEIF(NOT WITH_PDAL) @@ -1557,7 +1702,11 @@ MESSAGE(STATUS " With Open3D = NO (Open3D not found)") ENDIF() IF(opengv_FOUND AND WITH_OPENGV) +IF(opengv_VERSION STREQUAL "internal") +MESSAGE(STATUS " With OpenGV (internal) = YES (License: BSD)") +ELSE() MESSAGE(STATUS " With OpenGV ${opengv_VERSION} = YES (License: BSD)") +ENDIF() ELSEIF(NOT WITH_OPENGV) MESSAGE(STATUS " With OpenGV = NO (WITH_OPENGV=OFF)") ELSE() @@ -1576,7 +1725,7 @@ ENDIF() IF(grid_map_core_FOUND) MESSAGE(STATUS " With GridMap ${grid_map_core_VERSION} = YES (License: BSD)") -ELSEIF(NOT WITH_OCTOMAP) +ELSEIF(NOT WITH_GRIDMAP) MESSAGE(STATUS " With GridMap = NO (WITH_GRIDMAP=OFF)") ELSE() MESSAGE(STATUS " With GridMap = NO (grid_map_core not found)") @@ -1672,7 +1821,7 @@ ELSE() MESSAGE(STATUS " With FlyCapture2/Triclops = NO (Point Grey SDK not found)") ENDIF() -IF(ZED_FOUND AND CUDA_FOUND) +IF(ZED_FOUND) MESSAGE(STATUS " With ZED = YES") ELSEIF(NOT WITH_ZED) MESSAGE(STATUS " With ZED = NO (WITH_ZED=OFF)") @@ -1739,6 +1888,14 @@ ELSE() MESSAGE(STATUS " With XVisio SDK = NO (xvsdk not found)") ENDIF() +IF(OrbbecSDK_FOUND) +MESSAGE(STATUS " With Orbbec SDK ${OrbbecSDK_VERSION} = YES (License: MIT)") +ELSEIF(NOT WITH_ORBBEC_SDK) +MESSAGE(STATUS " With Orbbec SDK = NO (WITH_ORBBEC_SDK=OFF)") +ELSE() +MESSAGE(STATUS " With Orbbec SDK = NO (OrbbecSDK v2 not found)") +ENDIF() + MESSAGE(STATUS "") MESSAGE(STATUS " Odometry Approaches:") IF(loam_velodyne_FOUND) @@ -1756,6 +1913,13 @@ MESSAGE(STATUS " With floam = NO (WITH_FLOAM=OFF)") ELSE() MESSAGE(STATUS " With floam = NO (floam not found)") ENDIF() +IF(lio_sam_FOUND) +MESSAGE(STATUS " With lio_sam = YES (License: BSD)") +ELSEIF(NOT WITH_LIOSAM) +MESSAGE(STATUS " With lio_sam = NO (WITH_LIOSAM=OFF)") +ELSE() +MESSAGE(STATUS " With lio_sam = NO (lio_sam not found)") +ENDIF() IF(libfovis_FOUND) MESSAGE(STATUS " With libfovis = YES (License: GPLv2)") @@ -1800,12 +1964,12 @@ ENDIF() IF(vins_FOUND) MESSAGE(STATUS " With VINS-Fusion = YES (License: GPLv3)") ELSEIF(NOT WITH_VINS) -MESSAGE(STATUS " With VINS-Fusion = NO (WITH_VINS=OFF)") +MESSAGE(STATUS " With VINS-Fusion = NO (WITH_VINS_FUSION=OFF)") ELSE() MESSAGE(STATUS " With VINS-Fusion = NO (VINS-Fusion not found)") ENDIF() -IF(ov_msckf_FOUND) +IF(OpenVINS_FOUND) MESSAGE(STATUS " With OpenVINS = YES (License: GPLv3)") ELSEIF(NOT WITH_OPENVINS) MESSAGE(STATUS " With OpenVINS = NO (WITH_OPENVINS=OFF)") @@ -1823,6 +1987,14 @@ ELSE() MESSAGE(STATUS " With ORB_SLAM = NO (ORB_SLAM2 and ORB_SLAM3 not found, make sure environment variable ORB_SLAM_ROOT_DIR is set)") ENDIF() +IF(CUVSLAM_FOUND) +MESSAGE(STATUS " With cuVSLAM = YES (License: NVIDIA ISAAC ROS SOFTWARE LICENSE)") +ELSEIF(NOT WITH_CUVSLAM) +MESSAGE(STATUS " With cuVSLAM = NO (WITH_CUVSLAM=OFF)") +ELSE() +MESSAGE(STATUS " With cuVSLAM = NO (cuVSLAM not found, make sure cuVSLAM is installed)") +ENDIF() + MESSAGE(STATUS "Show all options with: cmake -LA | grep WITH_") MESSAGE(STATUS "--------------------------------------------") diff --git a/README.md b/README.md index a0978e79..427c7322 100644 --- a/README.md +++ b/README.md @@ -7,7 +7,7 @@ rtabmap [![Downloads][downloads-image]][downloads] [![License][license-image]][license] -[release-image]: https://img.shields.io/badge/release-0.21.4-green.svg?style=flat +[release-image]: https://img.shields.io/badge/release-0.23.1-green.svg?style=flat [releases]: https://github.com/introlab/rtabmap/releases [downloads-image]: https://img.shields.io/github/downloads/introlab/rtabmap/total?label=downloads @@ -35,13 +35,12 @@ This project is supported by [IntRoLab - Intelligent / Interactive / Integrated - - - - - - @@ -59,7 +58,7 @@ This project is supported by [IntRoLab - Intelligent / Interactive / Integrated - + @@ -67,6 +66,10 @@ This project is supported by [IntRoLab - Intelligent / Interactive / Integrated + + + + diff --git a/Version.h.in b/Version.h.in index 29feac8b..2bb9d46a 100644 --- a/Version.h.in +++ b/Version.h.in @@ -35,12 +35,13 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #define RTABMAP_VERSION_MINOR @PROJECT_VERSION_MINOR@ #define RTABMAP_VERSION_PATCH @PROJECT_VERSION_PATCH@ -#define RTABMAP_VERSION_COMPARE(major, minor, patch) (major>=@PROJECT_VERSION_MAJOR@ || (major==@PROJECT_VERSION_MAJOR@ && minor>=@PROJECT_VERSION_MINOR@) || (major==@PROJECT_VERSION_MAJOR@ && minor==@PROJECT_VERSION_MINOR@ && patch >=@PROJECT_VERSION_PATCH@)) +#define RTABMAP_VERSION_COMPARE(OP, MAJOR, MINOR, PATCH) (RTABMAP_VERSION_MAJOR*10000+RTABMAP_VERSION_MINOR*100+RTABMAP_VERSION_PATCH OP MAJOR*10000+MINOR*100+PATCH) @NONFREE@#define RTABMAP_NONFREE @TORO@#define RTABMAP_TORO @G2O@#define RTABMAP_G2O @G2O_CPP_CONF@#define RTABMAP_G2O_CPP11 @G2O_CPP11@ +@G2O_WITH_SBA_UTILS_CONF@#define RTABMAP_G2O_WITH_SBA_UTILS @GTSAM@#define RTABMAP_GTSAM @CERES@#define RTABMAP_CERES @MRPT@#define RTABMAP_MRPT @@ -62,6 +63,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. @CUDASIFT@#define RTABMAP_CUDASIFT @LOAM@#define RTABMAP_LOAM @FLOAM@#define RTABMAP_FLOAM +@LIOSAM@#define RTABMAP_LIOSAM @DC1394@#define RTABMAP_DC1394 @FLYCAPTURE2@#define RTABMAP_FLYCAPTURE2 @ZED@#define RTABMAP_ZED @@ -72,6 +74,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. @MYNTEYE@#define RTABMAP_MYNTEYE @DEPTHAI@#define RTABMAP_DEPTHAI @XVSDK@#define RTABMAP_XVSDK +@ORBBEC_SDK@#define RTABMAP_ORBBEC_SDK @OCTOMAP@#define RTABMAP_OCTOMAP @GRIDMAP@#define RTABMAP_GRIDMAP @CPUTSDF@#define RTABMAP_CPUTSDF @@ -82,13 +85,16 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. @DVO@#define RTABMAP_DVO @OKVIS@#define RTABMAP_OKVIS @MSCKF_VIO@#define RTABMAP_MSCKF_VIO -@VINS@#define RTABMAP_VINS +@VINSFUSION@#define RTABMAP_VINS_FUSION @OPENVINS@#define RTABMAP_OPENVINS +@CUVSLAM@#define RTABMAP_CUVSLAM @ORB_SLAM@#define RTABMAP_ORB_SLAM @ORB_SLAM_VERSION@ @ORB_OCTREE@#define RTABMAP_ORB_OCTREE @TORCH@#define RTABMAP_TORCH @PYTHON@#define RTABMAP_PYTHON @MADGWICK@#define RTABMAP_MADGWICK +@APRILTAG@#define RTABMAP_APRILTAG +@APRILTAG_ARUCO@#define RTABMAP_APRILTAG_WITH_ARUCO #include diff --git a/app/android/jni/RTABMapApp.cpp b/app/android/jni/RTABMapApp.cpp index 52ec7e71..2db3e8d3 100644 --- a/app/android/jni/RTABMapApp.cpp +++ b/app/android/jni/RTABMapApp.cpp @@ -3000,7 +3000,7 @@ void RTABMapApp::setTrajectoryMode(bool enabled) void RTABMapApp::setGraphOptimization(bool enabled) { graphOptimization_ = enabled; - if((sensorCaptureThread_ == 0) && rtabmap_ && rtabmap_->getMemory()->getLastWorkingSignature()!=0) + if((sensorCaptureThread_ == 0) && rtabmap_ && rtabmap_->getMemory()->getLastWorkingSignature(false)!=0) { std::map poses; std::multimap links; diff --git a/app/ios/RTABMapApp.xcodeproj/project.pbxproj b/app/ios/RTABMapApp.xcodeproj/project.pbxproj index f571574a..6fc3ba60 100644 --- a/app/ios/RTABMapApp.xcodeproj/project.pbxproj +++ b/app/ios/RTABMapApp.xcodeproj/project.pbxproj @@ -1065,7 +1065,7 @@ "$(PROJECT_DIR)/RTABMapApp/Libraries/share/OpenCV/3rdparty/lib", "$(PROJECT_DIR)/RTABMapApp/Libraries/lib/opencv4/3rdparty", ); - MARKETING_VERSION = 0.22.0; + MARKETING_VERSION = 0.23.7; OTHER_CFLAGS = ""; PRODUCT_BUNDLE_IDENTIFIER = com.introlab.rtabmap; PRODUCT_NAME = "$(TARGET_NAME)"; @@ -1078,7 +1078,7 @@ "\"$(SRCROOT)/RTABMapApp/Libraries/include\"", "\"$(SRCROOT)/RTABMapApp/Libraries/include/eigen3\"", "\"$(SRCROOT)/RTABMapApp/Libraries/include/pcl-1.15\"", - "\"$(SRCROOT)/RTABMapApp/Libraries/include/rtabmap-0.22\"", + "\"$(SRCROOT)/RTABMapApp/Libraries/include/rtabmap-0.23\"", "\"$(SRCROOT)/../android/jni/tango-gl/include\"", "\"$(SRCROOT)/../android/jni/third-party/include\"", "\"$(SRCROOT)/RTABMapApp/Libraries/lib/vtk.framework/Headers\"", @@ -1125,7 +1125,7 @@ "$(PROJECT_DIR)/RTABMapApp/Libraries/share/OpenCV/3rdparty/lib", "$(PROJECT_DIR)/RTABMapApp/Libraries/lib/opencv4/3rdparty", ); - MARKETING_VERSION = 0.22.0; + MARKETING_VERSION = 0.23.7; ONLY_ACTIVE_ARCH = YES; OTHER_CFLAGS = ""; PRODUCT_BUNDLE_IDENTIFIER = com.introlab.rtabmap; @@ -1139,7 +1139,7 @@ "\"$(SRCROOT)/RTABMapApp/Libraries/include\"", "\"$(SRCROOT)/RTABMapApp/Libraries/include/eigen3\"", "\"$(SRCROOT)/RTABMapApp/Libraries/include/pcl-1.15\"", - "\"$(SRCROOT)/RTABMapApp/Libraries/include/rtabmap-0.22\"", + "\"$(SRCROOT)/RTABMapApp/Libraries/include/rtabmap-0.23\"", "\"$(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/install_deps.sh b/app/ios/RTABMapApp/install_deps.sh index 0c575097..15f67021 100755 --- a/app/ios/RTABMapApp/install_deps.sh +++ b/app/ios/RTABMapApp/install_deps.sh @@ -175,6 +175,18 @@ cd $pwd #rm -rf g2o fi +# g2o's installed CMake config hard-codes the absolute build-time prefix of its +# external dependencies (e.g. suitesparse) in INTERFACE_INCLUDE_DIRECTORIES. That +# path doesn't exist when the prebuilt Libraries archive is unpacked on another +# machine (CI), breaking find_package(g2o) with "includes non-existent path". +# Rewrite those absolute paths to be relocatable (relative to the config file). +# Run unconditionally (outside the build guard above) so it also fixes the prebuilt +# archive in CI, where the g2o build step is skipped. +find "$prefix/lib" -path '*/cmake/g2o/*.cmake' -print0 | while IFS= read -r -d '' f +do + sed -i '' -E 's#[^";]*/Libraries#${CMAKE_CURRENT_LIST_DIR}/../../..#g' "$f" +done + # VTK if [ ! -e $prefix/lib/vtk.framework ] then @@ -288,6 +300,6 @@ cmake -DANDROID_PREBUILD=ON ../../../../.. cmake --build . --config Release mkdir -p ios cd ios -cmake -G Xcode -DCMAKE_SYSTEM_NAME=iOS -DCMAKE_OSX_ARCHITECTURES=arm64 -DCMAKE_OSX_SYSROOT=$sysroot -DBUILD_SHARED_LIBS=OFF -DCMAKE_BUILD_TYPE=Release -DCMAKE_OSX_DEPLOYMENT_TARGET=12.0 -DCMAKE_INSTALL_PREFIX=$prefix -DCMAKE_FIND_ROOT_PATH=$prefix -DWITH_QT=OFF -DBUILD_APP=OFF -DBUILD_TOOLS=OFF -DWITH_TORO=OFF -DWITH_VERTIGO=OFF -DWITH_MADGWICK=OFF -DWITH_ORB_OCTREE=ON -DBUILD_EXAMPLES=OFF -DWITH_LIBLAS=ON ../../../../../.. +cmake -G Xcode -DCMAKE_SYSTEM_NAME=iOS -DCMAKE_OSX_ARCHITECTURES=arm64 -DCMAKE_OSX_SYSROOT=$sysroot -DBUILD_SHARED_LIBS=OFF -DCMAKE_BUILD_TYPE=Release -DCMAKE_OSX_DEPLOYMENT_TARGET=12.0 -DCMAKE_INSTALL_PREFIX=$prefix -DCMAKE_FIND_ROOT_PATH=$prefix -DWITH_QT=OFF -DBUILD_APP=OFF -DBUILD_TOOLS=OFF -DWITH_TORO=OFF -DWITH_VERTIGO=OFF -DWITH_MADGWICK=OFF -DWITH_ORB_OCTREE=ON -DBUILD_EXAMPLES=OFF -DWITH_LIBLAS=ON -DWITH_OPENGV=OFF ../../../../../.. cmake --build . --config Release cmake --build . --config Release --target install diff --git a/app/src/CMakeLists.txt b/app/src/CMakeLists.txt index 7ec7e01c..a58eeb3e 100644 --- a/app/src/CMakeLists.txt +++ b/app/src/CMakeLists.txt @@ -34,7 +34,7 @@ ENDIF() IF(APPLE AND BUILD_AS_BUNDLE) ADD_EXECUTABLE(rtabmap_app MACOSX_BUNDLE ${SRC_FILES}) ELSEIF(WIN32 AND BUILD_AS_BUNDLE) - ADD_EXECUTABLE(rtabmap_app WIN32 ${SRC_FILES}) + ADD_EXECUTABLE(rtabmap_app ${SRC_FILES}) ELSE() ADD_EXECUTABLE(rtabmap_app ${SRC_FILES}) ENDIF() @@ -95,9 +95,11 @@ IF(BUILD_AS_BUNDLE AND (APPLE OR WIN32)) DESTINATION ${thirdparty_dest_dir} COMPONENT runtime REGEX ".*pdb" EXCLUDE) - INSTALL(FILES "${OpenNI2_BIN_DIR}/OpenNI.ini" + IF(NOT WIN32) + INSTALL(FILES "${OpenNI2_BIN_DIR}/OpenNI.ini" DESTINATION ${thirdparty_dest_dir} COMPONENT runtime) + ENDIF() ENDIF(OpenNI2_FOUND) IF(k4a_FOUND) @@ -110,24 +112,135 @@ IF(BUILD_AS_BUNDLE AND (APPLE OR WIN32)) ENDIF(WIN32) ENDIF(k4a_FOUND) + IF(ZED_FOUND) + # Install needed zlibwapi.dll + IF(WIN32) + file(TO_CMAKE_PATH "$ENV{ZED_SDK_ROOT_DIR}" ENV_ZED_SDK_ROOT_DIR) + INSTALL(FILES "${ENV_ZED_SDK_ROOT_DIR}/bin/zlibwapi.dll" + DESTINATION ${thirdparty_dest_dir} + COMPONENT runtime) + ENDIF(WIN32) + ENDIF(ZED_FOUND) + + IF(OrbbecSDK_FOUND) + # Install needed "extensions" folder + IF(WIN32) + find_path(OrbbecSDK_BIN_DIR NAMES OrbbecSDK.dll) + IF(NOT OrbbecSDK_BIN_DIR) + MESSAGE(FATAL_ERROR "OrbbecSDK.dll not found! Verify your PATH.") + ENDIF(NOT OrbbecSDK_BIN_DIR) + MESSAGE(FATAL "OrbbecSDK_BIN_DIR=${OrbbecSDK_BIN_DIR}") + INSTALL(DIRECTORY "${OrbbecSDK_BIN_DIR}/extensions" + DESTINATION ${thirdparty_dest_dir} + COMPONENT runtime + FILES_MATCHING + PATTERN "*.lib" EXCLUDE + PATTERN "*") + ENDIF(WIN32) + ENDIF(OrbbecSDK_FOUND) + IF(Torch_FOUND) - # Install needed cudnn_ops_infer64_8.dll and cudnn_cnn_infer64_8.dll - # TODO: should be a more general way to include them if version is different + # Install needed cudnn dlls IF(WIN32 AND CUDA_FOUND) - find_file(CUDNN_OPS_DLL NAMES cudnn_ops_infer64_8.dll) - find_file(CUDNN_CNN_DLL NAMES cudnn_cnn_infer64_8.dll) - IF(CUDNN_OPS_DLL AND CUDNN_CNN_DLL) - MESSAGE(STATUS "Found ${CUDNN_OPS_DLL}") - MESSAGE(STATUS "Found ${CUDNN_CNN_DLL}") - INSTALL(FILES ${CUDNN_OPS_DLL} ${CUDNN_CNN_DLL} - DESTINATION ${thirdparty_dest_dir} - COMPONENT runtime) - ELSE() - MESSAGE(AUTHOR_WARNING "Using Torch with CUDA, but cudnn_ops_infer64_8.dll and cudnn_cnn_infer64_8.dll are not found on the PATH, so it won't be added to package.") + find_path(cuDNN_BIN_DIR NAMES cudnn.dll cudnn64.dll cudnn64_9.dll) + IF(NOT cuDNN_BIN_DIR) + MESSAGE(FATAL_ERROR "cudnn dlls not found! Make sure the dlls are in a directory on your PATH.") + ENDIF(NOT cuDNN_BIN_DIR) + MESSAGE(STATUS "cuDNN_BIN_DIR = ${cuDNN_BIN_DIR}") + file(GLOB CUDNN_DLLS "${cuDNN_BIN_DIR}/cudnn*.dll") + IF(CUDNN_DLLS) + MESSAGE(STATUS "Found cuDNN DLLs: ${CUDNN_DLLS}") + INSTALL(FILES ${CUDNN_DLLS} + DESTINATION ${thirdparty_dest_dir} + COMPONENT runtime) ENDIF() ENDIF(WIN32 AND CUDA_FOUND) ENDIF(Torch_FOUND) + + set(python_pyd_dir "") + IF(Python3_FOUND) + # bundle python3 + IF(WIN32) + set(python_pyd_dir "bin/Lib/site-packages") + set(PYTHON_ZIP_NAME "python${Python3_VERSION_MAJOR}${Python3_VERSION_MINOR}.zip") + get_filename_component(PYTHON_ROOT "${Python3_EXECUTABLE}" DIRECTORY) + file(TO_CMAKE_PATH "${Python3_STDLIB}" SANITIZED_STDLIB) + MESSAGE(STATUS "Python3_EXECUTABLE=${Python3_EXECUTABLE}") + MESSAGE(STATUS "Python3_STDLIB=${SANITIZED_STDLIB}") + + install(FILES "${Python3_EXECUTABLE}" DESTINATION bin COMPONENT runtime) + + # when using python-opencv, it expects python3.dll, not python312.dll + get_filename_component(VCPKG_TRIPLET_ROOT "${PYTHON_TOOLS_DIR}/../../" ABSOLUTE) + set(VCPKG_BIN_DIR "${VCPKG_TRIPLET_ROOT}/bin") + find_file(PYTHON3_STABLE_DLL + NAMES python3.dll + PATHS "${VCPKG_BIN_DIR}" + NO_DEFAULT_PATH + ) + if(PYTHON3_STABLE_DLL) + message(STATUS "Found python3.dll at: ${PYTHON3_STABLE_DLL}") + install(FILES "${PYTHON3_STABLE_DLL}" DESTINATION bin COMPONENT runtime) + endif() + + # install python Lib in python312.zip (without site-packages, which is installed separatly afterwards) + file(TO_CMAKE_PATH "${Python3_STDLIB}" SANITIZED_STDLIB) + file(GLOB LIB_CONTENTS RELATIVE "${SANITIZED_STDLIB}" "${SANITIZED_STDLIB}/*") + list(REMOVE_ITEM LIB_CONTENTS "site-packages") + install(CODE " + execute_process( + COMMAND \"${CMAKE_COMMAND}\" -E tar cf \"\$ENV{DESTDIR}\${CMAKE_INSTALL_PREFIX}/bin/${PYTHON_ZIP_NAME}\" --format=zip -- ${LIB_CONTENTS} + WORKING_DIRECTORY \"${SANITIZED_STDLIB}\" + ) + " COMPONENT runtime) + set(PYTHON_SITE_PACKAGES "${SANITIZED_STDLIB}/site-packages") + + install(DIRECTORY "${PYTHON_SITE_PACKAGES}/" + DESTINATION "${thirdparty_dest_dir}/Lib/site-packages" + COMPONENT runtime + PATTERN "*.exe" EXCLUDE + PATTERN "*.lib" EXCLUDE + PATTERN "*.hpp" EXCLUDE + PATTERN "*.h" EXCLUDE + PATTERN "*/torch/*" EXCLUDE + ) + if(EXISTS "${PYTHON_SITE_PACKAGES}/torch") + install(DIRECTORY "${PYTHON_SITE_PACKAGES}/torch" + DESTINATION "${thirdparty_dest_dir}/Lib/site-packages/" + COMPONENT runtime + PATTERN "*.exe" EXCLUDE + PATTERN "*.lib" EXCLUDE + PATTERN "*.hpp" EXCLUDE + PATTERN "*.h" EXCLUDE + PATTERN "*.dll" EXCLUDE + ) + file(GLOB_RECURSE PY_DLL_FILES "${PYTHON_SITE_PACKAGES}/torch/*.dll") + if(PY_DLL_FILES) + install(FILES ${PY_DLL_FILES} DESTINATION bin COMPONENT runtime) + endif() + endif() + + # install python DDLs + file(GLOB_RECURSE PY_DLL_FILES \"\$ENV{DESTDIR}\${CMAKE_INSTALL_PREFIX}/${python_pyd_dir}/*.dll\") + install(DIRECTORY "${PYTHON_ROOT}/DLLs" + DESTINATION "${thirdparty_dest_dir}/" + COMPONENT runtime + FILES_MATCHING + PATTERN "*.dll" + PATTERN "*.pyd" + ) + + # install our python scripts in share for convenience + install(DIRECTORY "${PROJECT_SOURCE_DIR}/corelib/src/python/" + DESTINATION share + COMPONENT runtime + FILES_MATCHING + PATTERN "*.py" + ) + ENDIF(WIN32) + ENDIF(Python3_FOUND) + 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 @@ -221,8 +334,8 @@ IF(BUILD_AS_BUNDLE AND (APPLE OR WIN32)) SET(DIRS "${QT_LIBRARY_DIRS}" "\$ENV{DESTDIR}\${CMAKE_INSTALL_PREFIX}/lib") IF(APPLE) SET(DIRS ${DIRS} /usr/local /usr/local/lib /opt/homebrew /opt/homebrew/lib /opt/homebrew/lib/gcc/current) - ENDIF(APPLE) - + ENDIF(APPLE) + # Now the work of copying dependencies into the bundle/package # The quotes are escaped and variables to use at install time have their $ escaped # An alternative is the do a configure_file() on a script and use install(SCRIPT ...). @@ -230,11 +343,26 @@ IF(BUILD_AS_BUNDLE AND (APPLE OR WIN32)) # over. # To find dependencies, cmake use "otool" on Apple and "dumpbin" on Windows (make sure you have one of them). install(CODE " - file(GLOB_RECURSE QTPLUGINS \"\$ENV{DESTDIR}\${CMAKE_INSTALL_PREFIX}/${plugin_dest_dir}/*${CMAKE_SHARED_LIBRARY_SUFFIX}\") + # Glob Qt Plugins + file(GLOB_RECURSE ALL_LIBS \"\$ENV{DESTDIR}\${CMAKE_INSTALL_PREFIX}/${plugin_dest_dir}/*${CMAKE_SHARED_LIBRARY_SUFFIX}\") + + if(NOT \"${python_pyd_dir}\" STREQUAL \"\") + file(GLOB_RECURSE PYD_FILES \"\$ENV{DESTDIR}\${CMAKE_INSTALL_PREFIX}/${python_pyd_dir}/*.pyd\") + if(PYD_FILES) + list(APPEND ALL_LIBS \${PYD_FILES}) + endif() + if(WIN32) + file(GLOB_RECURSE DLL_FILES \"\$ENV{DESTDIR}\${CMAKE_INSTALL_PREFIX}/${python_pyd_dir}/torch/*.dll\") + if(DLL_FILES) + list(APPEND ALL_LIBS \${DLL_FILES}) + endif() + endif() + endif() + set(BU_CHMOD_BUNDLE_ITEMS ON) include(\"BundleUtilities\") - fixup_bundle(\"${APPS}\" \"\${QTPLUGINS}\" \"${DIRS}\") - " COMPONENT runtime) - + fixup_bundle(\"${APPS}\" \"\${ALL_LIBS}\" \"${DIRS}\") + " COMPONENT runtime) + ENDIF(BUILD_AS_BUNDLE AND (APPLE OR WIN32)) diff --git a/archive/2022-IlluminationInvariant/results_common.m b/archive/2022-IlluminationInvariant/results_common.m index e7cb03df..289305d5 100644 --- a/archive/2022-IlluminationInvariant/results_common.m +++ b/archive/2022-IlluminationInvariant/results_common.m @@ -15,7 +15,7 @@ RAMaddOverhead = 0; % Inliers_ratio = 'Loop/Visual_inliers/' ./ 'Keypoint/Current_frame/words' % Odometry_average = 'Memory/Distance_travelled/m'(2:end) - 'Memory/Distance_travelled/m'(1:end-1) -statNames = {'Loop/Odom_correction_norm/m', 'Loop/Visual_inliers/', 'Inliers_ratio_%', 'Timing/Total/ms', 'Memory/RAM_usage/MB', 'Memory/RAM_estimated/MB', 'Keypoint/Current_frame/words', 'Loop/Map_id/', 'Memory/Local_graph_size/', 'Keypoint/Dictionary_size/words', 'Loop/Distance_since_last_loc/'}; % 'Odometry_average' +statNames = {'Loop/Odom_correction_norm/m', 'Loop/Visual_inliers/', 'Inliers_ratio_%', 'Timing/Total/ms', 'Memory/RAM_usage/MB', 'Memory/RAM_estimated/MB', 'Keypoint/Current_frame/words', 'Loop/Map_id/', 'Memory/Local_graph_size/', 'Keypoint/Dictionary_size/words', 'Loop/Distance_since_last_loc/m'}; % 'Odometry_average' datasets = [ 0 1 6 7 9 14 11 111 ]; % 0 1 6 7 9 12 14 11 @@ -26,7 +26,7 @@ if resultsToShow == 2 sep = [0, 1000, 3000, 5000, 7000, 9000]; sepName = {'17:27', '17:54', '18:27', '18:56', '19:35'}; prefix = 'Consecutive'; - statNames = {'Loop/Distance_since_last_loc/', 'Distance_since_last_loc_under_50cm'}; + statNames = {'Loop/Distance_since_last_loc/m', 'Distance_since_last_loc_under_50cm'}; endif MapsN = length(sepName); @@ -52,7 +52,7 @@ if strcmp(statName,'Inliers_ratio_%') elseif strcmp(statName, 'Odometry_average') data = dlmread([dataDir '/' prefix num2str(datasets(d)) '-' 'Memory-Distance_travelled-m' '.txt'], '\t', 1, 0, "emptyvalue", 0); elseif strcmp(statName, 'Distance_since_last_loc_under_50cm') - data = dlmread([dataDir '/' prefix num2str(datasets(d)) '-' 'Loop-Distance_since_last_loc-' '.txt'], '\t', 1, 0, "emptyvalue", 0); + data = dlmread([dataDir '/' prefix num2str(datasets(d)) '-' 'Loop-Distance_since_last_loc-m' '.txt'], '\t', 1, 0, "emptyvalue", 0); else data = dlmread([dataDir '/' prefix num2str(datasets(d)) '-' statName '.txt'], '\t', 1, 0, "emptyvalue", 0); endif diff --git a/archive/2022-IlluminationInvariant/scripts/export_stats.sh b/archive/2022-IlluminationInvariant/scripts/export_stats.sh index ec0128fb..fd31f67b 100755 --- a/archive/2022-IlluminationInvariant/scripts/export_stats.sh +++ b/archive/2022-IlluminationInvariant/scripts/export_stats.sh @@ -13,7 +13,7 @@ source rtabmap_latest.bash for d in "${DETECTOR[@]}" do - rtabmap-report --export --export_prefix "Stat$d" --loc 32 Loop/Odom_correction_norm/m Loop/Visual_inliers/ Timing/Total/ms Timing/Proximity_by_space_visual/ms Timing/Likelihood_computation/ms Timing/Posterior_computation/ms TimingMem/Keypoints_detection/ms TimingMem/Descriptors_extraction/ms TimingMem/Add_new_words/ms Loop/Map_id/ Keypoint/Current_frame/words Memory/RAM_usage/MB Memory/RAM_estimated/MB Memory/Distance_travelled/m Loop/Distance_since_last_loc/ Memory/Local_graph_size/ Keypoint/Dictionary_size/words "$DATA/$d/loc" - rtabmap-report --export --export_prefix "Consecutive$d" --loc 32 Loop/Map_id/ Loop/Distance_since_last_loc/ "$DATA/$d/consecutive_loc" + rtabmap-report --export --export_prefix "Stat$d" --loc 32 Loop/Odom_correction_norm/m Loop/Visual_inliers/ Timing/Total/ms Timing/Proximity_by_space_visual/ms Timing/Likelihood_computation/ms Timing/Posterior_computation/ms TimingMem/Keypoints_detection/ms TimingMem/Descriptors_extraction/ms TimingMem/Add_new_words/ms Loop/Map_id/ Keypoint/Current_frame/words Memory/RAM_usage/MB Memory/RAM_estimated/MB Memory/Distance_travelled/m Loop/Distance_since_last_loc/m Memory/Local_graph_size/ Keypoint/Dictionary_size/words "$DATA/$d/loc" + rtabmap-report --export --export_prefix "Consecutive$d" --loc 32 Loop/Map_id/ Loop/Distance_since_last_loc/m "$DATA/$d/consecutive_loc" done diff --git a/bundle_windows_deps.bat b/bundle_windows_deps.bat new file mode 100644 index 00000000..7f255557 --- /dev/null +++ b/bundle_windows_deps.bat @@ -0,0 +1,227 @@ +@echo off +setlocal enabledelayedexpansion + +:: --- CONFIGURATION --- +set "VCPKG_ROOT=%~dp0vcpkg" +set "EXPORT_DIR=%~dp0vcpkg_binaries" +set "TRIPLET=x64-windows-release" +set "SEVENZIP_EXE=C:\Program Files\7-Zip\7z.exe" + +set "VCPKG_JSON=%~dp0vcpkg.json" +for /f "usebackq tokens=*" %%a in (`powershell -NoProfile -Command "(Get-Content '%VCPKG_JSON%' -Raw | ConvertFrom-Json).'builtin-baseline'"` ) do set "VCPKG_COMMIT=%%a" +echo [+] Detected VCPKG baseline commit: %VCPKG_COMMIT% + +if "%VCPKG_COMMIT%"=="" ( + echo [X] Error: Could not find 'builtin-baseline' in %VCPKG_JSON% + pause + exit /b 1 +) + +set VCPKG_COMMIT_SHORT=%VCPKG_COMMIT:~0,8% + +:: 1. Setup Local vcpkg +if not exist "%VCPKG_ROOT%" ( + echo [+] Local vcpkg not found. Cloning... + git clone https://github.com/microsoft/vcpkg.git "%VCPKG_ROOT%" +) +pushd "%VCPKG_ROOT%" +git checkout %VCPKG_COMMIT% +call .\bootstrap-vcpkg.bat +popd + +:: 2. Install vcpkg dependencies +echo [+] Installing dependencies via vcpkg manifest... +"%VCPKG_ROOT%\vcpkg.exe" install ^ + --triplet=%TRIPLET% ^ + --host-triplet=%TRIPLET% ^ + --clean-after-build ^ + --x-feature=tools ^ + --x-feature=k4w2 ^ + --x-feature=octomap ^ + --x-feature=openmp ^ + --x-feature=realsense2 ^ + --x-feature=openni2 ^ + --x-feature=gtsam-deps ^ + --x-feature=python ^ + --x-feature=libpointmatcher-deps || exit /b !errorlevel! + +:: 3. Export +echo [+] Exporting built binaries to raw folder... +set "VS_LOCATOR=%ProgramFiles(x86)%\Microsoft Visual Studio\Installer\vswhere.exe" +for /f "usebackq tokens=*" %%i in (`"%VS_LOCATOR%" -latest -property catalog_productLineVersion`) do set VS_YEAR=vs%%i +set TARGET_NAME=vcpkg-export-%VCPKG_COMMIT_SHORT%-x64-%VS_YEAR% +set TARGET_FULL_PATH=%EXPORT_DIR%\%TARGET_NAME% +if exist "%TARGET_FULL_PATH%" rd /s /q "%TARGET_FULL_PATH%" +"%VCPKG_ROOT%\vcpkg.exe" export --raw --output-dir="%EXPORT_DIR%" --triplet=%TRIPLET% || exit /b !errorlevel! + +:: Find the actual exported folder name (it usually contains a date/hash) +for /d %%i in ("%EXPORT_DIR%\vcpkg-export-20??????-??????") do set "FINAL_EXPORT_PATH=%%i" + +echo [+] Rename folder %FINAL_EXPORT_PATH% to %TARGET_NAME% +ren "%FINAL_EXPORT_PATH%" "%TARGET_NAME%" || exit /b !errorlevel! +set "FINAL_EXPORT_PATH=%TARGET_FULL_PATH%" + +echo [+] Add numpy... +:: We install numpy<2 to be compatible with SuperPoint and SuperGlue scripts +%FINAL_EXPORT_PATH%/installed/%TRIPLET%/tools/python3/python.exe -m ensurepip --upgrade || exit /b %errorlevel% +%FINAL_EXPORT_PATH%/installed/%TRIPLET%/tools/python3/python.exe -m pip install --upgrade pip || exit /b !errorlevel! +%FINAL_EXPORT_PATH%/installed/%TRIPLET%/tools/python3/python.exe -m pip install "numpy<2" || exit /b !errorlevel! + +:: 4. Other dependencies not in vcpkg +:: libnabo +echo [+] Building libnabo... +if not exist libnabo ( + echo [+] Downloading... + git clone https://github.com/ethz-asl/libnabo.git + cd libnabo + :: Jan 27, 2022 + git checkout c925c47 + git apply ../patches/libnabo_c925c47.patch + cd .. +) +cd libnabo +cmake -S . -B build -GNinja ^ + -DVCPKG_MANIFEST_INSTALL=OFF ^ + -DVCPKG_TARGET_TRIPLET=x64-windows-release ^ + -DVCPKG_INSTALLED_DIR="%FINAL_EXPORT_PATH%\installed" ^ + -DCMAKE_TOOLCHAIN_FILE="%FINAL_EXPORT_PATH%\scripts\buildsystems\vcpkg.cmake" ^ + -DCMAKE_INSTALL_PREFIX="%FINAL_EXPORT_PATH%\installed\%TRIPLET%" ^ + -DCMAKE_BUILD_TYPE=Release ^ + -DSHARED_LIBS=FALSE ^ + -DLIBNABO_BUILD_DOXYGEN=OFF ^ + -DLIBNABO_BUILD_EXAMPLES=OFF ^ + -DLIBNABO_BUILD_PYTHON=OFF ^ + -DLIBNABO_BUILD_TESTS=OFF || exit /b !errorlevel! +cmake --build build --config Release --target install || exit /b !errorlevel! +cd .. + +:: libpointmatcher +echo [+] Building libpointmatcher... +if not exist libpointmatcher ( + echo [+] Downloading and applying patch... + git clone https://github.com/ethz-asl/libpointmatcher.git + cd libpointmatcher + :: Mar 17, 2023 + git checkout 7dc58e5 + git apply ../patches/pointmatcher_7dc58e5.patch + cd .. +) +cd libpointmatcher +cmake -S . -B build -GNinja ^ + -DVCPKG_MANIFEST_INSTALL=OFF ^ + -DVCPKG_TARGET_TRIPLET=x64-windows-release ^ + -DVCPKG_INSTALLED_DIR="%FINAL_EXPORT_PATH%\installed" ^ + -DCMAKE_TOOLCHAIN_FILE="%FINAL_EXPORT_PATH%\scripts\buildsystems\vcpkg.cmake" ^ + -DCMAKE_INSTALL_PREFIX="%FINAL_EXPORT_PATH%\installed\%TRIPLET%" ^ + -DCMAKE_BUILD_TYPE=Release ^ + -DBUILD_TESTS=OFF ^ + -DBUILD_SHARED_LIBS=ON ^ + -DPOINTMATCHER_BUILD_EVALUATIONS=OFF ^ + -DPOINTMATCHER_BUILD_EXAMPLES=OFF ^ + -DCMAKE_CXX_FLAGS="-DBOOST_TIMER_ENABLE_DEPRECATED /EHsc -DBOOST_EXCEPTION_DISABLE" || exit /b !errorlevel! +cmake --build build --config Release --target install || exit /b !errorlevel! +cd .. + +:: We remove the files in the top-level CMake directory to force use of share/libpointmatcher/cmake +if exist "%FINAL_EXPORT_PATH%\installed\%TRIPLET%\CMake\" ( + rd /s /q "%FINAL_EXPORT_PATH%\installed\%TRIPLET%\CMake" +) + +:: gtsam +echo [+] Building gtsam... +if not exist gtsam ( + echo [+] Downloading and applying patch... + git clone https://github.com/borglab/gtsam.git + cd gtsam + :: June 18, 2025 + git checkout 4.3a0-ros + git cherry-pick 18af4e6 + git apply ../patches/gtsam_4_3a0-ros.patch + cd .. +) +cd gtsam +cmake -S . -B build -GNinja ^ + -DVCPKG_MANIFEST_INSTALL=OFF ^ + -DVCPKG_TARGET_TRIPLET=x64-windows-release ^ + -DVCPKG_INSTALLED_DIR="%FINAL_EXPORT_PATH%\installed" ^ + -DCMAKE_TOOLCHAIN_FILE="%FINAL_EXPORT_PATH%\scripts\buildsystems\vcpkg.cmake" ^ + -DCMAKE_INSTALL_PREFIX="%FINAL_EXPORT_PATH%\installed\%TRIPLET%" ^ + -DCMAKE_BUILD_TYPE=Release ^ + -DGTSAM_BUILD_EXAMPLES_ALWAYS=OFF ^ + -DGTSAM_BUILD_TESTS=OFF ^ + -DGTSAM_BUILD_UNSTABLE=OFF ^ + -DGTSAM_USE_SYSTEM_EIGEN=ON ^ + -DGTSAM_BUILD_WITH_PRECOMPILED_HEADERS=OFF ^ + -DGTSAM_UNSTABLE_BUILD_PYTHON=OFF ^ + -DGTSAM_WITH_EIGEN_MKL=OFF ^ + -DGTSAM_WITH_EIGEN_MKL_OPENMP=OFF ^ + -DCMAKE_CXX_FLAGS="-DBOOST_TIMER_ENABLE_DEPRECATED -DBOOST_BIND_GLOBAL_PLACEHOLDERS" || exit /b !errorlevel! +cmake --build build --config Release --target install || exit /b !errorlevel! +cd .. + +:: opengv +echo [+] Building opengv... +if not exist opengv ( + echo [+] Downloading and applying patch... + git clone https://github.com/laurentkneip/opengv.git + cd opengv + :: Aug 6, 2020 + git checkout 91f4b19c73450833a40e463ad3648aae80b3a7f3 + git apply ../patches/opengv_91f4b19c.patch + cd .. +) +cd opengv +cmake -S . -B build -GNinja ^ + -DVCPKG_MANIFEST_INSTALL=OFF ^ + -DVCPKG_TARGET_TRIPLET=x64-windows-release ^ + -DVCPKG_INSTALLED_DIR="%FINAL_EXPORT_PATH%\installed" ^ + -DCMAKE_TOOLCHAIN_FILE="%FINAL_EXPORT_PATH%\scripts\buildsystems\vcpkg.cmake" ^ + -DCMAKE_INSTALL_PREFIX="%FINAL_EXPORT_PATH%\installed\%TRIPLET%" ^ + -DBUILD_TESTS=OFF ^ + -DBUILD_SHARED_LIBS=ON || exit /b !errorlevel! +cmake --build build --config Release --target install || exit /b !errorlevel! +cd .. + +:: 5. ZIP the folder +echo [+] Creating final package with 7-Zip... +:: Rip off pdb files +cd /d "%FINAL_EXPORT_PATH%" +del /s /q /f *.pdb >nul 2>&1 + +cd .. + +set "FINAL_ZIP=%TARGET_NAME%.7z" + +:: compress contents without the root folder +"%SEVENZIP_EXE%" u -t7z -mx9 "%FINAL_ZIP%" "%FINAL_EXPORT_PATH%\*" -up0q0 + +if !errorlevel! EQU 0 ( + echo [!] Success! Package created at %FINAL_ZIP% +) else ( + echo [X] 7-Zip failed with error code !errorlevel! +) + + +:: Example building rtabmap afterwards +goto :EndComment + +set VCPKG_UNZIPPED_EXPORT_PATH=%USERPROFILE%\Downloads\vcpkg-export-########-x64-vs2022 +set PATH=%VCPKG_UNZIPPED_EXPORT_PATH%\installed\x64-windows-release\bin;%PATH% + +cmake -B build -GNinja ^ + -DCMAKE_BUILD_TYPE=Release ^ + -DBUILD_AS_BUNDLE=ON -DWITH_PYTHON=ON ^ + -DWITH_ZED=OFF ^ + -DVCPKG_MANIFEST_INSTALL=OFF ^ + -DVCPKG_TARGET_TRIPLET=x64-windows-release ^ + -DVCPKG_INSTALLED_DIR="%VCPKG_UNZIPPED_EXPORT_PATH%/installed" ^ + -DCMAKE_TOOLCHAIN_FILE=%VCPKG_UNZIPPED_EXPORT_PATH%/scripts/buildsystems/vcpkg.cmake ^ + -DGTSAM_DIR=%VCPKG_UNZIPPED_EXPORT_PATH%\installed\x64-windows-release\CMake + +cmake --build build --config Release --target package + +:: To install CPU pytorch inside rtabmap package afterwards. +:: Note that python.exe is the one in the bin directory of the package, not the system one. +python.exe -m pip install torch torchvision opencv-python-headless "numpy<2" + +:EndComment diff --git a/bundle_windows_deps_cuda.bat b/bundle_windows_deps_cuda.bat new file mode 100644 index 00000000..3ba4d2fc --- /dev/null +++ b/bundle_windows_deps_cuda.bat @@ -0,0 +1,239 @@ +@echo off +setlocal enabledelayedexpansion + +IF NOT DEFINED CUDA_PATH ( + echo [ERROR] CUDA_PATH is not set. + exit /b 1 +) + +set PATH=%CUDA_PATH%\bin;%PATH% +set PATH=%CUDA_PATH%\bin\x64;%PATH% +set PATH=%CUDA_PATH%\extras\CUPTI\lib64;%PATH% + +:: CUDA Toolkit should be manually installed on the computer before running this script +:: We assume also that cuDNN is merged into CUDA installed directory. +where nvcc >nul 2>&1 +if !errorlevel! neq 0 ( + echo [ERROR] nvcc was not found in your PATH. + pause + exit /b +) +for /f "tokens=5" %%a in ('nvcc --version ^| findstr "release"') do ( + set "RAW_VER=%%a" + :: This removes the trailing comma + set "CUDA_VER=!RAW_VER:,=!" + set "CUDA_VER_SHORT=!CUDA_VER:.=!" +) +if "!CUDA_VER!"=="" ( + echo [ERROR] Could not parse CUDA version. + pause + exit /b +) +echo Installed CUDA Toolkit: %CUDA_VER% + +:: --- CONFIGURATION --- +set "VCPKG_ROOT=%~dp0vcpkg" +set "EXPORT_DIR=%~dp0vcpkg_binaries" +set "TRIPLET=x64-windows-release" +set "SEVENZIP_EXE=C:\Program Files\7-Zip\7z.exe" + +set "VCPKG_JSON=%~dp0vcpkg.json" +for /f "usebackq tokens=*" %%a in (`powershell -NoProfile -Command "(Get-Content '%VCPKG_JSON%' -Raw | ConvertFrom-Json).'builtin-baseline'"` ) do set "VCPKG_COMMIT=%%a" + +if "%VCPKG_COMMIT%"=="" ( + echo [X] Error: Could not find 'builtin-baseline' in %VCPKG_JSON% + pause + exit /b 1 +) + +set VCPKG_COMMIT_SHORT=%VCPKG_COMMIT:~0,8% +set "VS_LOCATOR=%ProgramFiles(x86)%\Microsoft Visual Studio\Installer\vswhere.exe" +for /f "usebackq tokens=*" %%i in (`"%VS_LOCATOR%" -latest -property catalog_productLineVersion`) do set VS_YEAR=vs%%i +set ORG_TARGET_NAME=vcpkg-export-%VCPKG_COMMIT_SHORT%-x64-%VS_YEAR% +set TARGET_NAME=%ORG_TARGET_NAME%-cuda%CUDA_VER_SHORT% +set VCPKG_EXPORT_PATH=%EXPORT_DIR%\%ORG_TARGET_NAME% +set FINAL_EXPORT_PATH=%EXPORT_DIR%\%TARGET_NAME% + +if not exist "%FINAL_EXPORT_PATH%" ( + if not exist "%VCPKG_EXPORT_PATH%" ( + call bundle_windows_deps.bat || exit /b !errorlevel! + ) + echo [+] Copying %VCPKG_EXPORT_PATH% to %FINAL_EXPORT_PATH% + xcopy "%VCPKG_EXPORT_PATH%" "%FINAL_EXPORT_PATH%\" /E /I /H /Y /Q || exit /b !errorlevel! + echo [+] Remove opencv built by vcpkg + rmdir /s /q "%FINAL_EXPORT_PATH%\installed\%TRIPLET%\include\opencv4" || exit /b !errorlevel! + rmdir /s /q "%FINAL_EXPORT_PATH%\installed\%TRIPLET%\share\opencv4" || exit /b !errorlevel! + rmdir /s /q "%FINAL_EXPORT_PATH%\installed\%TRIPLET%\share\opencv" || exit /b !errorlevel! + del "%FINAL_EXPORT_PATH%\installed\%TRIPLET%\bin\opencv*" || exit /b !errorlevel! + del "%FINAL_EXPORT_PATH%\installed\%TRIPLET%\lib\opencv*" || exit /b !errorlevel! + :: bundle cudnn runtime libraries + xcopy "%CUDA_PATH%\bin\x64\cudnn*.dll" "%FINAL_EXPORT_PATH%\installed\%TRIPLET%\bin\" /Y +) + +:: pytorch deps +%FINAL_EXPORT_PATH%/installed/%TRIPLET%/tools/python3/python.exe -m pip install numpy packaging "setuptools<82" pyyaml typing_extensions + +git config --global core.longpaths true + +:: pytorch, build with local cuda libraries to avoid duplicating them when we install rtabmap +echo [+] Building pytorch with cuda support... +if not exist pytorch ( + echo [+] Downloading pytorch... + git clone https://github.com/pytorch/pytorch || exit /b !errorlevel! + cd pytorch + :: Jan 21, 2026 + git checkout v2.10.0 + git submodule update --init --recursive || exit /b !errorlevel! + cd .. +) + +set "PYTHONHOME=%FINAL_EXPORT_PATH%\installed\%TRIPLET%\tools\python3" +set "Python_ROOT_DIR=%FINAL_EXPORT_PATH%\installed\%TRIPLET%" +set CMAKE_GENERATOR=Ninja +set BUILD_TEST=0 +set ATEN_NO_TEST=1 +set INSTALL_TEST=OFF +set "LIB=%FINAL_EXPORT_PATH%\installed\%TRIPLET%\lib;%LIB%" +set "%FINAL_EXPORT_PATH%\installed\%TRIPLET%\include;INCLUDE=%INCLUDE%" + +:: check if torch is installed +%PYTHONHOME%/python.exe -m pip show torch >nul 2>&1 +if !errorlevel! neq 0 ( + cd pytorch + %PYTHONHOME%/python.exe setup.py install || exit /b !errorlevel! + cd .. +) +if exist "%PYTHONHOME%\Lib\site-packages\torch\test" rd /s /q %PYTHONHOME%\Lib\site-packages\torch\test" +del "%PYTHONHOME%\Lib\site-packages\torch\bin\test_*" || exit /b !errorlevel! + +echo [+] Building torchvision... +if not exist torchvision ( + echo [+] Downloading torchvision... + git clone https://github.com/pytorch/vision.git torchvision || exit /b !errorlevel! + cd torchvision + :: Jan 6, 2026 + git checkout v0.25.0 + git submodule update --init --recursive || exit /b !errorlevel! + cd .. +) +cd torchvision +set PATH=%FINAL_EXPORT_PATH%\installed\%TRIPLET%\bin;%PATH% +set DISTUTILS_USE_SDK=1 +set TORCHVISION_INCLUDE=%FINAL_EXPORT_PATH%\installed\%TRIPLET%\include +set TORCHVISION_LIBRARY=%FINAL_EXPORT_PATH%\installed\%TRIPLET%\lib +%FINAL_EXPORT_PATH%/installed/%TRIPLET%/tools/python3/python.exe -m pip install . -v --no-build-isolation || exit /b !errorlevel! +cd .. + +:: opencv_cuda +echo [+] Building opencv with cuda support... +if not exist opencv ( + echo [+] Downloading opencv... + git clone https://github.com/opencv/opencv.git || exit /b !errorlevel! + cd opencv + :: 4.13.0 minimum required to be compatible with cuda 13 + :: Dec 31, 2025 + git checkout 4.13.0 + cd .. +) +if not exist opencv_contrib ( + echo [+] Downloading opencv_contrib... + git clone https://github.com/opencv/opencv_contrib.git || exit /b !errorlevel! + cd opencv + :: 4.13.0 minimum required to be compatible with cuda 13 + :: Dec 31, 2025 + git checkout 4.13.0 + cd .. +) +cd opencv +cmake -S . -B build -GNinja ^ + -DVCPKG_MANIFEST_INSTALL=OFF ^ + -DVCPKG_TARGET_TRIPLET=%TRIPLET% ^ + -DVCPKG_INSTALLED_DIR="%FINAL_EXPORT_PATH%\installed" ^ + -DCMAKE_TOOLCHAIN_FILE="%FINAL_EXPORT_PATH%\scripts\buildsystems\vcpkg.cmake" ^ + -DCMAKE_INSTALL_PREFIX="%FINAL_EXPORT_PATH%\installed\%TRIPLET%" ^ + -DCMAKE_BUILD_TYPE=Release ^ + -DOPENCV_BIN_INSTALL_PATH="bin" ^ + -DOPENCV_LIB_INSTALL_PATH="lib" ^ + -DOPENCV_CONFIG_INSTALL_PATH="share/opencv" ^ + -DOPENCV_EXTRA_MODULES_PATH=../opencv_contrib/modules ^ + -DBUILD_SHARED_LIBS=ON ^ + -DBUILD_TESTS=OFF ^ + -DBUILD_PERF_TESTS=OFF ^ + -DOPENCV_ENABLE_NONFREE=ON ^ + -DBUILD_opencv_apps=OFF ^ + -DBUILD_opencv_python3=ON ^ + -DPYTHON3_EXECUTABLE=%FINAL_EXPORT_PATH%/installed/%TRIPLET%/tools/python3/python.exe ^ + -DPYTHON3_PACKAGES_PATH=bin/Lib/site-packages ^ + -DBUILD_opencv_java_bindings_generator=OFF ^ + -DWITH_CUDA=ON ^ + -DWITH_VTK=OFF ^ + -DWITH_TBB=ON || exit /b !errorlevel! +cmake --build build --config Release --target install || exit /b !errorlevel! +:: move cv2 package under tools/python3 +robocopy "%FINAL_EXPORT_PATH%\installed\%TRIPLET%\bin\Lib" "%FINAL_EXPORT_PATH%\installed\%TRIPLET%\tools\python3\Lib" /E /MOVE /NFL /NDL /NJH /NC /NS /NP +cd .. + + +:: 5. ZIP the folder +echo [+] Creating final package with 7-Zip... +:: Rip off pdb files +cd /d "%FINAL_EXPORT_PATH%" +del /s /q /f *.pdb >nul 2>&1 +cd .. + +set "FINAL_ZIP=%TARGET_NAME%.7z" + +:: compress contents without the root folder +"%SEVENZIP_EXE%" u -t7z -mx9 "%FINAL_ZIP%" "%FINAL_EXPORT_PATH%\*" -up0q0 || exit /b !errorlevel! + +if !errorlevel! EQU 0 ( + echo [!] Success! Package created at %FINAL_ZIP% +) else ( + echo [X] 7-Zip failed with error code !errorlevel! +) + +:: Example building rtabmap with opencv cuda and libtorch afterwards +goto :EndComment + +:: Set path of unzipped deps +set VCPKG_UNZIPPED_EXPORT_PATH= + +set TRIPLET=x64-windows-release +set PATH=%VCPKG_UNZIPPED_EXPORT_PATH%\installed\%TRIPLET%\bin;%PATH% +set PATH=%VCPKG_UNZIPPED_EXPORT_PATH%\installed\%TRIPLET%\tools\python3;%PATH% +set PATH=%CUDA_PATH%\bin;%PATH% +set PATH=%CUDA_PATH%\bin\x64;%PATH% +set PATH=%CUDA_PATH%\extras\CUPTI\lib64;%PATH% +set PATH=%VCPKG_UNZIPPED_EXPORT_PATH%\installed\%TRIPLET%\tools\python3\Lib\site-packages\torch\lib;%PATH% +set PATH=%VCPKG_UNZIPPED_EXPORT_PATH%\installed\%TRIPLET%\tools\python3\Lib\site-packages\numpy.libs;%PATH% + +:: Other dependencies + +:: For ZED, modify zed-config.cmake and remove all dependencies +set PATH=%PATH%;%ZED_SDK_ROOT_DIR%\bin + +:: For kinect 4 windows SDK v2, move kinect20.dll from system32 to KINECTSDK20_DIR\bin +:: For kinect 4 windows SDK v1, move kinect10.dll and KinectAudio10.dll to KINECTSDK20_DIR\bin +set PATH=%PATH%;%KINECTSDK20_DIR%\bin + +cmake -B build_cuda -GNinja ^ + -DCMAKE_BUILD_TYPE=Release ^ + -DBUILD_AS_BUNDLE=ON ^ + -DWITH_PYTHON=ON ^ + -DWITH_TORCH=ON ^ + -DWITH_ZED=ON ^ + -DVCPKG_MANIFEST_INSTALL=OFF ^ + -DVCPKG_TARGET_TRIPLET=%TRIPLET% ^ + -DVCPKG_INSTALLED_DIR="%VCPKG_UNZIPPED_EXPORT_PATH%/installed" ^ + -DCMAKE_TOOLCHAIN_FILE=%VCPKG_UNZIPPED_EXPORT_PATH%/scripts/buildsystems/vcpkg.cmake ^ + -DGTSAM_DIR=%VCPKG_UNZIPPED_EXPORT_PATH%\installed\%TRIPLET%\CMake ^ + -DTorch_DIR=%VCPKG_UNZIPPED_EXPORT_PATH%\installed\%TRIPLET%\tools\python3\Lib\site-packages\torch\share\cmake\Torch + +cmake --build build_cuda --config Release --target package + +:: Generate superpoint weights (from share directory of the installed package) +curl -L -O "https://raw.githubusercontent.com/magicleap/SuperPointPretrainedNetwork/master/demo_superpoint.py" +curl -L -O "https://github.com/magicleap/SuperPointPretrainedNetwork/raw/refs/heads/master/superpoint_v1.pth" +..\bin\python.exe rtabmap_trace_superpoint.py + +:EndComment \ No newline at end of file diff --git a/cmake_modules/FindCuVSLAM.cmake b/cmake_modules/FindCuVSLAM.cmake new file mode 100644 index 00000000..ec204b24 --- /dev/null +++ b/cmake_modules/FindCuVSLAM.cmake @@ -0,0 +1,105 @@ +# - Find cuVSLAM library (https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_visual_slam) +# +# CUVSLAM_ROOT_DIR environment variable can be set to find the library. +# +# It sets the following variables: +# CUVSLAM_FOUND - Set to false, or undefined, if cuVSLAM isn't found. +# CUVSLAM_VERSION - The version of cuVSLAM found (e.g., "14.0.0"). +# CUVSLAM_INCLUDE_DIRS - The cuVSLAM include directory. +# CUVSLAM_LIBRARIES - The cuVSLAM library to link against. + +find_package(CUDA REQUIRED) +find_package(Eigen3 REQUIRED) + +find_path(CUVSLAM_INCLUDE_DIRS + NAMES cuvslam.h + PATHS + /usr/include + /usr/local/include + /opt/cuvslam/include + /opt/ros/humble/share/isaac_ros_nitros/cuvslam/include + $ENV{CUVSLAM_ROOT}/include + $ENV{CUVSLAM_ROOT_DIR}/include +) + +find_library(CUVSLAM_LIBRARY + NAMES cuvslam + PATHS + /usr/lib + /usr/local/lib + /opt/cuvslam/lib + /opt/ros/humble/share/isaac_ros_nitros/cuvslam/lib + $ENV{CUVSLAM_ROOT}/lib + $ENV{CUVSLAM_ROOT_DIR}/lib +) + +if(CUVSLAM_INCLUDE_DIRS AND CUVSLAM_LIBRARY) + # Extract version from cuvslam.h header + file(STRINGS "${CUVSLAM_INCLUDE_DIRS}/cuvslam.h" CUVSLAM_VERSION_MAJOR_LINE + REGEX "^#define CUVSLAM_API_VERSION_MAJOR") + file(STRINGS "${CUVSLAM_INCLUDE_DIRS}/cuvslam.h" CUVSLAM_VERSION_MINOR_LINE + REGEX "^#define CUVSLAM_API_VERSION_MINOR") + + if(CUVSLAM_VERSION_MAJOR_LINE AND CUVSLAM_VERSION_MINOR_LINE) + string(REGEX MATCH "[0-9]+" CUVSLAM_VERSION_MAJOR "${CUVSLAM_VERSION_MAJOR_LINE}") + string(REGEX MATCH "[0-9]+" CUVSLAM_VERSION_MINOR "${CUVSLAM_VERSION_MINOR_LINE}") + set(CUVSLAM_VERSION "${CUVSLAM_VERSION_MAJOR}.${CUVSLAM_VERSION_MINOR}.0") + endif() + + set(CUVSLAM_LIBRARIES + ${CUVSLAM_LIBRARY} + ${CUDA_LIBRARIES} + # Eigen3 is header-only, so we don't need to link to it + ) + set(CUVSLAM_INCLUDE_DIRS + ${CUVSLAM_INCLUDE_DIRS} + ${CUDA_INCLUDE_DIRS} + ${EIGEN3_INCLUDE_DIR} + ) +endif() + +# Version compatibility check - cuVSLAM only guarantees API compatibility within the same major version +set(CUVSLAM_VERSION_MISMATCH_REASON "") +if(CuVSLAM_FIND_VERSION AND CUVSLAM_VERSION) + string(REGEX MATCH "^[0-9]+" REQUESTED_MAJOR_VERSION "${CuVSLAM_FIND_VERSION}") + if(NOT CUVSLAM_VERSION_MAJOR EQUAL REQUESTED_MAJOR_VERSION) + set(CUVSLAM_VERSION_MISMATCH_REASON "Major version mismatch: found ${CUVSLAM_VERSION_MAJOR}.x but requested ${REQUESTED_MAJOR_VERSION}.x.\ncuVSLAM only guarantees API compatibility within the same major version.\nPlease install cuVSLAM ${REQUESTED_MAJOR_VERSION}.x or update CMakeLists.txt to request version ${CUVSLAM_VERSION_MAJOR}.0.0") + + if(CuVSLAM_FIND_REQUIRED) + message(FATAL_ERROR + "cuVSLAM major version mismatch: found version ${CUVSLAM_VERSION} but version ${CuVSLAM_FIND_VERSION} is required.\n" + "cuVSLAM only guarantees API compatibility within the same major version.\n" + "Found major version ${CUVSLAM_VERSION_MAJOR} is not compatible with requested major version ${REQUESTED_MAJOR_VERSION}.\n" + "Please install cuVSLAM ${REQUESTED_MAJOR_VERSION}.x or update CMakeLists.txt to request version ${CUVSLAM_VERSION_MAJOR}.x." + ) + else() + # Clear the found variables to indicate incompatibility + unset(CUVSLAM_LIBRARIES) + unset(CUVSLAM_INCLUDE_DIRS) + endif() + endif() +endif() + +# Handle the QUIET and REQUIRED arguments +include(FindPackageHandleStandardArgs) +find_package_handle_standard_args(CuVSLAM + FOUND_VAR CUVSLAM_FOUND + REQUIRED_VARS CUVSLAM_LIBRARIES CUVSLAM_INCLUDE_DIRS + VERSION_VAR CUVSLAM_VERSION + REASON_FAILURE_MESSAGE "${CUVSLAM_VERSION_MISMATCH_REASON}" + HANDLE_COMPONENTS +) + +if(CUVSLAM_FOUND) + # Create imported target for modern CMake usage + if(NOT TARGET cuvslam::cuvslam) + add_library(cuvslam::cuvslam UNKNOWN IMPORTED) + set_target_properties(cuvslam::cuvslam PROPERTIES + IMPORTED_LOCATION "${CUVSLAM_LIBRARY}" + INTERFACE_INCLUDE_DIRECTORIES "${CUVSLAM_INCLUDE_DIRS}" + INTERFACE_LINK_LIBRARIES "${CUVSLAM_LIBRARIES};Eigen3::Eigen" + ) + endif() +endif() + +mark_as_advanced(CUVSLAM_INCLUDE_DIRS CUVSLAM_LIBRARY) diff --git a/cmake_modules/FindEigen3.cmake b/cmake_modules/FindEigen3.cmake index 0b36805e..460de2ae 100644 --- a/cmake_modules/FindEigen3.cmake +++ b/cmake_modules/FindEigen3.cmake @@ -27,10 +27,10 @@ if(NOT Eigen3_FIND_VERSION) if(NOT Eigen3_FIND_VERSION_MAJOR) - set(Eigen3_FIND_VERSION_MAJOR 2) + set(Eigen3_FIND_VERSION_MAJOR 3) endif() if(NOT Eigen3_FIND_VERSION_MINOR) - set(Eigen3_FIND_VERSION_MINOR 91) + set(Eigen3_FIND_VERSION_MINOR 0) endif() if(NOT Eigen3_FIND_VERSION_PATCH) set(Eigen3_FIND_VERSION_PATCH 0) diff --git a/cmake_modules/FindG2O.cmake b/cmake_modules/FindG2O.cmake index ffc9c9a7..1790a26c 100644 --- a/cmake_modules/FindG2O.cmake +++ b/cmake_modules/FindG2O.cmake @@ -26,6 +26,10 @@ FIND_FILE(G2O_FACTORY_FILE g2o/core/factory.h PATHS ${G2O_INCLUDE_DIR} NO_DEFAULT_PATH) +FIND_FILE(G2O_SBA_UTILS_FILE g2o/types/sba/sba_utils.h + PATHS ${G2O_INCLUDE_DIR} + NO_DEFAULT_PATH) + #ifdef G2O_NUMBER_FORMAT_STR #define G2O_CPP11 // we assume that if G2O_NUMBER_FORMAT_STR is defined, this is the new g2o code with c++11 interface #endif @@ -97,16 +101,16 @@ IF(G2O_STUFF_LIBRARY AND G2O_CORE_LIBRARY AND G2O_INCLUDE_DIR AND G2O_CONFIG_FIL ${G2O_TYPES_SBA} ${G2O_STUFF_LIBRARY}) - IF(CSPARSE_FOUND) + IF(CSPARSE_FOUND AND G2O_SOLVER_CSPARSE AND G2O_SOLVER_CSPARSE_EXTENSION) SET(G2O_INCLUDE_DIRS ${G2O_INCLUDE_DIRS} ${CSPARSE_INCLUDE_DIR}) SET(G2O_LIBRARIES ${G2O_LIBRARIES} - ${G2O_SOLVER_CSPARSE} - ${G2O_SOLVER_CSPARSE_EXTENSION} - ${CSPARSE_LIBRARY}) - ENDIF(CSPARSE_FOUND) + ${G2O_SOLVER_CSPARSE} + ${G2O_SOLVER_CSPARSE_EXTENSION} + ${CSPARSE_LIBRARY}) + ENDIF(CSPARSE_FOUND AND G2O_SOLVER_CSPARSE AND G2O_SOLVER_CSPARSE_EXTENSION) IF(G2O_SOLVER_CHOLMOD) SET(G2O_INCLUDE_DIRS @@ -118,22 +122,29 @@ IF(G2O_STUFF_LIBRARY AND G2O_CORE_LIBRARY AND G2O_INCLUDE_DIR AND G2O_CONFIG_FIL ${CHOLMOD_LIB}) ENDIF(G2O_SOLVER_CHOLMOD) - FILE(READ ${G2O_CONFIG_FILE} TMPTXT) - STRING(FIND "${TMPTXT}" "G2O_NUMBER_FORMAT_STR" matchres) + FILE(READ ${G2O_FACTORY_FILE} TMPTXT) + STRING(FIND "${TMPTXT}" "shared_ptr" matchres) IF(${matchres} EQUAL -1) - MESSAGE(STATUS "Old g2o version detected with c++03 interface (config file: ${G2O_CONFIG_FILE}).") - SET(G2O_CPP11 0) - ELSE() - MESSAGE(WARNING "Latest g2o version detected with c++11 interface (config file: ${G2O_CONFIG_FILE}). Make sure g2o is built with \"-DBUILD_WITH_MARCH_NATIVE=OFF\" to avoid segmentation faults caused by Eigen.") - FILE(READ ${G2O_FACTORY_FILE} TMPTXT) - STRING(FIND "${TMPTXT}" "shared_ptr" matchres) + FILE(READ ${G2O_CONFIG_FILE} TMPTXT) + STRING(FIND "${TMPTXT}" "G2O_NUMBER_FORMAT_STR" matchres) IF(${matchres} EQUAL -1) + MESSAGE(STATUS "Old g2o version detected with c++03 interface (config file: ${G2O_CONFIG_FILE}).") + SET(G2O_CPP11 0) + ELSE() + MESSAGE(WARNING "Latest g2o version detected with c++11 interface (config file: ${G2O_CONFIG_FILE}). Make sure g2o is built with \"-DBUILD_WITH_MARCH_NATIVE=OFF\" to avoid segmentation faults caused by Eigen.") MESSAGE(STATUS "Old g2o factory version detected without shared ptr (factory file: ${G2O_FACTORY_FILE}).") SET(G2O_CPP11 2) - ELSE() - MESSAGE(STATUS "Latest g2o factory version detected with shared ptr (factory file: ${G2O_FACTORY_FILE}).") - SET(G2O_CPP11 1) ENDIF() + ELSE() + MESSAGE(WARNING "Latest g2o version detected with c++11 interface (config file: ${G2O_CONFIG_FILE}). Make sure g2o is built with \"-DBUILD_WITH_MARCH_NATIVE=OFF\" to avoid segmentation faults caused by Eigen.") + MESSAGE(STATUS "Latest g2o factory version detected with shared ptr (factory file: ${G2O_FACTORY_FILE}).") + SET(G2O_CPP11 1) + ENDIF() + + IF(G2O_SBA_UTILS_FILE) + SET(G2O_WITH_SBA_UTILS 1) + ELSE() + SET(G2O_WITH_SBA_UTILS 0) ENDIF() SET(G2O_FOUND "YES") diff --git a/cmake_modules/FindORB_SLAM.cmake b/cmake_modules/FindORB_SLAM.cmake index f9ccdc0d..aa5e1124 100644 --- a/cmake_modules/FindORB_SLAM.cmake +++ b/cmake_modules/FindORB_SLAM.cmake @@ -11,6 +11,7 @@ find_path(ORB_SLAM_INCLUDE_DIR NAMES System.h PATHS $ENV{ORB_SLAM_ROOT_DIR}/include) 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(DBoW2_INCLUDE_DIR NAMES DBoW2/BowVector.h PATHS $ENV{ORB_SLAM_ROOT_DIR}/Thirdparty/DBoW2 NO_DEFAULT_PATH) 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) @@ -22,9 +23,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) + IF(g2o_INCLUDE_DIR AND sophus_INCLUDE_DIR AND DBoW2_INCLUDE_DIR) # ORB_SLAM3 v1 + SET(g2o_INCLUDE_DIR ${g2o_INCLUDE_DIR} ${sophus_INCLUDE_DIR} ${DBoW2_INCLUDE_DIR} $ENV{ORB_SLAM_ROOT_DIR}) + ENDIF(g2o_INCLUDE_DIR AND sophus_INCLUDE_DIR AND DBoW2_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/FindOpenVINS.cmake b/cmake_modules/FindOpenVINS.cmake new file mode 100644 index 00000000..66029218 --- /dev/null +++ b/cmake_modules/FindOpenVINS.cmake @@ -0,0 +1,44 @@ +# Find OpenVINS +# +# We search for a vins installation in ROS/ROS2 first, then fallback on +# ros-free library in common install paths + +FIND_PACKAGE(ov_msckf QUIET) +IF(ov_msckf_FOUND) + # On ROS2, the indirect includes and libraries + # are not forwarded by ov_msckf target, append them manually + FIND_PACKAGE(ov_core) + FIND_PACKAGE(ov_init) + IF(ov_msckf_FOUND AND ov_core_FOUND AND ov_init_FOUND) + SET(OpenVINS_FOUND TRUE) + SET(OpenVINS_INCLUDE_DIRS + ${ov_msckf_INCLUDE_DIRS} + ${ov_core_INCLUDE_DIRS} + ${ov_init_INCLUDE_DIRS}) + SET(OpenVINS_LIBRARIES + ${ov_msckf_LIBRARIES} + ${ov_core_LIBRARIES} + ${ov_init_LIBRARIES}) + ENDIF() +ELSE() + find_path(OpenVINS_INCLUDE_DIR NAMES core/VioManager.h PATH_SUFFIXES open_vins) + find_library(OpenVINS_LIBRARY NAMES ov_msckf_lib) + IF (OpenVINS_INCLUDE_DIR AND OpenVINS_LIBRARY) + SET(OpenVINS_FOUND TRUE) + SET(OpenVINS_INCLUDE_DIRS ${OpenVINS_INCLUDE_DIR}) + SET(OpenVINS_LIBRARIES ${OpenVINS_LIBRARY}) + ENDIF() +ENDIF() + +IF (OpenVINS_FOUND) + # show which OpenVINS was found only if not quiet + IF (NOT OpenVINS_FIND_QUIETLY) + MESSAGE(STATUS "Found OpenVINS: ${OpenVINS_LIBRARIES} ${OpenVINS_INCLUDE_DIRS}") + ENDIF (NOT OpenVINS_FIND_QUIETLY) +ELSE (OpenVINS_FOUND) + # fatal error if OpenVINS is required but not found + IF (OpenVINS_FIND_REQUIRED) + MESSAGE(FATAL_ERROR "Could not find OpenVINS") + ENDIF (OpenVINS_FIND_REQUIRED) +ENDIF (OpenVINS_FOUND) + diff --git a/cmake_modules/MacOSXBundleInfo.plist.in b/cmake_modules/MacOSXBundleInfo.plist.in index 7d5f67f8..322b94f9 100644 --- a/cmake_modules/MacOSXBundleInfo.plist.in +++ b/cmake_modules/MacOSXBundleInfo.plist.in @@ -10,8 +10,6 @@ ${MACOSX_BUNDLE_INFO_STRING} CFBundleIconFile ${MACOSX_BUNDLE_ICON_FILE} - CFBundleIdentifier - ${MACOSX_BUNDLE_GUI_IDENTIFIER} CFBundleInfoDictionaryVersion 6.0 CFBundleLongVersionString @@ -32,6 +30,14 @@ NSHumanReadableCopyright ${MACOSX_BUNDLE_COPYRIGHT} + com.apple.security.app-sandbox + + com.apple.security.files.downloads.read-write + + com.apple.security.files.downloads.read-only + + com.apple.security.device.camera + CFBundleDocumentTypes diff --git a/corelib/include/rtabmap/core/Camera.h b/corelib/include/rtabmap/core/Camera.h index a267f140..a76ea3c9 100644 --- a/corelib/include/rtabmap/core/Camera.h +++ b/corelib/include/rtabmap/core/Camera.h @@ -38,7 +38,7 @@ class IMUFilter; /** * Class Camera - * + * */ class RTABMAP_CORE_EXPORT Camera : public SensorCapture { @@ -48,7 +48,7 @@ public: SensorData takeImage(SensorCaptureInfo * info = 0) {return takeData(info);} float getImageRate() const {return getFrameRate();} void setImageRate(float imageRate) {setFrameRate(imageRate);} - void setInterIMUPublishing(bool enabled, IMUFilter * filter = 0); // Take ownership of filter + void setInterIMUPublishing(bool enabled, IMUFilter * filter = 0, bool baseFrameConversion = false); // Take ownership of filter bool isInterIMUPublishing() const {return publishInterIMU_;} bool initFromFile(const std::string & calibrationPath); @@ -73,6 +73,7 @@ private: private: IMUFilter * imuFilter_; bool publishInterIMU_; + bool imuBaseFrameConversion_; }; diff --git a/corelib/include/rtabmap/core/CameraRGBD.h b/corelib/include/rtabmap/core/CameraRGBD.h index afb2acff..11439ce8 100644 --- a/corelib/include/rtabmap/core/CameraRGBD.h +++ b/corelib/include/rtabmap/core/CameraRGBD.h @@ -38,3 +38,4 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include +#include diff --git a/corelib/include/rtabmap/core/DBDriver.h b/corelib/include/rtabmap/core/DBDriver.h index fdbfe285..847caa50 100644 --- a/corelib/include/rtabmap/core/DBDriver.h +++ b/corelib/include/rtabmap/core/DBDriver.h @@ -129,11 +129,12 @@ public: std::vector > * texCoords = 0, #endif cv::Mat * textures = 0) const; + void saveFlannIndex(const std::vector & indexData) const; public: // Mutex-protected methods of abstract versions below - bool openConnection(const std::string & url, bool overwritten = false); + bool openConnection(const std::string & url, bool overwritten = false, bool readOnly = false); void closeConnection(bool save = true, const std::string & outputUrl = ""); bool isConnected() const; unsigned long getMemoryUsed() const; // In bytes @@ -161,19 +162,20 @@ public: void executeNoResult(const std::string & sql) const; // Load objects - void load(VWDictionary * dictionary, bool lastStateOnly = true) const; - void loadLastNodes(std::list & signatures) const; // returned signatures must be freed after usage + void load(VWDictionary & dictionary, bool lastStateOnly = true) const; + void loadLastNodes(std::list & signatures, bool loadWordIdsOnly = false) const; // returned signatures must be freed after usage Signature * loadSignature(int id, bool * loadedFromTrash = 0); // returned signature must be freed after usage, call loadSignatures() instead if more than one signature should be loaded - void loadSignatures(const std::list & ids, std::list & signatures, std::set * loadedFromTrash = 0); // returned signatures must be freed after usage + void loadSignatures(const std::list & ids, std::list & signatures, std::set * loadedFromTrash = 0, bool loadWordIdsOnly = false); // returned signatures must be freed after usage void loadWords(const std::set & wordIds, std::list & vws); // returned words must be freed after usage // Specific queries... - void loadNodeData(Signature * signature, bool images = true, bool scan = true, bool userData = true, bool occupancyGrid = true) const; + void loadNodeData(Signature & signature, bool images = true, bool scan = true, bool userData = true, bool occupancyGrid = true) const; void loadNodeData(std::list & signatures, bool images = true, bool scan = true, bool userData = true, bool occupancyGrid = true) const; void getNodeData(int signatureId, SensorData & data, bool images = true, bool scan = true, bool userData = true, bool occupancyGrid = true) const; bool getCalibration(int signatureId, std::vector & models, std::vector & stereoModels) const; bool getLaserScanInfo(int signatureId, LaserScan & info) const; bool getNodeInfo(int signatureId, Transform & pose, int & mapId, int & weight, std::string & label, double & stamp, Transform & groundTruthPose, std::vector & velocity, GPS & gps, EnvSensors & sensors) const; + void getLocalFeatures(int signatureId, std::multimap & words, std::vector & keypoints, std::vector & points, cv::Mat & descriptors) const; void loadLinks(int signatureId, std::multimap & links, Link::Type type = Link::kUndef) const; void getWeight(int signatureId, int & weight) const; void getLastNodeIds(std::set & ids) const; @@ -191,7 +193,7 @@ public: protected: DBDriver(const ParametersMap & parameters = ParametersMap()); - virtual bool connectDatabaseQuery(const std::string & url, bool overwritten = false) = 0; + virtual bool connectDatabaseQuery(const std::string & url, bool overwritten = false, bool readOnly = false) = 0; virtual void disconnectDatabaseQuery(bool save = true, const std::string & outputUrl = "") = 0; virtual bool isConnectedQuery() const = 0; virtual unsigned long getMemoryUsedQuery() const = 0; // In bytes @@ -274,11 +276,12 @@ protected: std::vector > * texCoords, #endif cv::Mat * textures) const = 0; + virtual void saveFlannIndexQuery(const std::vector & indexData) const = 0; // Load objects - virtual void loadQuery(VWDictionary * dictionary, bool lastStateOnly = true) const = 0; - virtual void loadLastNodesQuery(std::list & signatures) const = 0; - virtual void loadSignaturesQuery(const std::list & ids, std::list & signatures) const = 0; + virtual void loadQuery(VWDictionary & dictionary, bool lastStateOnly = true) const = 0; + virtual void loadLastNodesQuery(std::list & signatures, bool loadWordIdsOnly) const = 0; + virtual void loadSignaturesQuery(const std::list & ids, std::list & signatures, bool loadWordIdsOnly) const = 0; virtual void loadWordsQuery(const std::set & wordIds, std::list & vws) const = 0; virtual void loadLinksQuery(int signatureId, std::multimap & links, Link::Type type = Link::kUndef) const = 0; @@ -286,6 +289,7 @@ protected: virtual bool getCalibrationQuery(int signatureId, std::vector & models, std::vector & stereoModels) const = 0; virtual bool getLaserScanInfoQuery(int signatureId, LaserScan & info) const = 0; virtual bool getNodeInfoQuery(int signatureId, Transform & pose, int & mapId, int & weight, std::string & label, double & stamp, Transform & groundTruthPose, std::vector & velocity, GPS & gps, EnvSensors & sensors) const = 0; + virtual void getLocalFeaturesQuery(int signatureId, std::multimap & words, std::vector & keypoints, std::vector & points, cv::Mat & descriptors) const = 0; virtual void getLastNodeIdsQuery(std::set & ids) const = 0; virtual void getAllNodeIdsQuery(std::set & ids, bool ignoreChildren, bool ignoreBadSignatures, bool ignoreIntermediateNodes) const = 0; virtual void getAllOdomPosesQuery(std::map & poses, bool ignoreChildren, bool ignoreIntermediateNodes) const = 0; diff --git a/corelib/include/rtabmap/core/DBDriverSqlite3.h b/corelib/include/rtabmap/core/DBDriverSqlite3.h index 407ec7e0..a5df8e92 100644 --- a/corelib/include/rtabmap/core/DBDriverSqlite3.h +++ b/corelib/include/rtabmap/core/DBDriverSqlite3.h @@ -51,7 +51,7 @@ public: void setTempStore(int tempStore); protected: - virtual bool connectDatabaseQuery(const std::string & url, bool overwritten = false); + virtual bool connectDatabaseQuery(const std::string & url, bool overwritten = false, bool readOnly = false); virtual void disconnectDatabaseQuery(bool save = true, const std::string & outputUrl = ""); virtual bool isConnectedQuery() const; virtual unsigned long getMemoryUsedQuery() const; // In bytes @@ -135,10 +135,12 @@ protected: #endif cv::Mat * textures) const; + virtual void saveFlannIndexQuery(const std::vector & indexData) const; + // Load objects - virtual void loadQuery(VWDictionary * dictionary, bool lastStateOnly = true) const; - virtual void loadLastNodesQuery(std::list & signatures) const; - virtual void loadSignaturesQuery(const std::list & ids, std::list & signatures) const; + virtual void loadQuery(VWDictionary & dictionary, bool lastStateOnly = true) const; + virtual void loadLastNodesQuery(std::list & signatures, bool loadWordIdsOnly) const; + virtual void loadSignaturesQuery(const std::list & ids, std::list & signatures, bool loadWordIdsOnly) const; virtual void loadWordsQuery(const std::set & wordIds, std::list & vws) const; virtual void loadLinksQuery(int signatureId, std::multimap & links, Link::Type type = Link::kUndef) const; @@ -146,6 +148,7 @@ protected: virtual bool getCalibrationQuery(int signatureId, std::vector & models, std::vector & stereoModels) const; virtual bool getLaserScanInfoQuery(int signatureId, LaserScan & info) const; virtual bool getNodeInfoQuery(int signatureId, Transform & pose, int & mapId, int & weight, std::string & label, double & stamp, Transform & groundTruthPose, std::vector & velocity, GPS & gps, EnvSensors & sensors) const; + virtual void getLocalFeaturesQuery(int signatureId, std::multimap & words, std::vector & keypoints, std::vector & points, cv::Mat & descriptors) const; virtual void getLastNodeIdsQuery(std::set & ids) const; virtual void getAllNodeIdsQuery(std::set & ids, bool ignoreChildren, bool ignoreBadSignatures, bool ignoreIntermediateNodes) const; virtual void getAllOdomPosesQuery(std::map & poses, bool ignoreChildren, bool ignoreIntermediateNodes) const; @@ -190,6 +193,8 @@ private: const cv::Point3f & viewpoint) const; private: + void loadWordsQuery(std::list & signatures) const; + void loadWordIdsQuery(std::list & signatures) const; void loadLinksQuery(std::list & signatures) const; int loadOrSaveDb(sqlite3 *pInMemory, const std::string & fileName, int isSave) const; diff --git a/corelib/include/rtabmap/core/DBReader.h b/corelib/include/rtabmap/core/DBReader.h index 9aae674d..c31da3f4 100644 --- a/corelib/include/rtabmap/core/DBReader.h +++ b/corelib/include/rtabmap/core/DBReader.h @@ -59,6 +59,8 @@ public: int startMapId = 0, int stopMapId = -1, bool priorsIgnored = false, + bool imuIgnored = false, + bool intermediateNodesAreNormalNodes = false, const std::vector & cameraLocalTransformOverrides = std::vector()); DBReader(const std::list & databasePaths, float frameRate = 0.0f, // -1 = use Database stamps, 0 = inf @@ -74,6 +76,8 @@ public: int startMapId = 0, int stopMapId = -1, bool priorsIgnored = false, + bool imuIgnored = false, + bool intermediateNodesAreNormalNodes = false, const std::vector & cameraLocalTransformOverrides = std::vector()); virtual ~DBReader(); @@ -104,9 +108,11 @@ private: int _stopId; std::vector _cameraIndices; bool _intermediateNodesIgnored; + bool _intermediateNodesAreNormalNodes; bool _landmarksIgnored; bool _featuresIgnored; bool _priorsIgnored; + bool _imuIgnored; int _startMapId; int _stopMapId; std::vector _cameraLocalTransformOverrides; diff --git a/corelib/include/rtabmap/core/Features2d.h b/corelib/include/rtabmap/core/Features2d.h index f26f84be..eec3f57b 100644 --- a/corelib/include/rtabmap/core/Features2d.h +++ b/corelib/include/rtabmap/core/Features2d.h @@ -104,6 +104,7 @@ namespace rtabmap { class ORBextractor; class SPDetector; +class SPDetectorRpautrat; class Stereo; #if CV_MAJOR_VERSION < 3 @@ -129,7 +130,8 @@ public: kFeatureSurfFreak=12, //new 0.20.4 kFeatureGfttDaisy=13, //new 0.20.6 kFeatureSurfDaisy=14, //new 0.20.6 - kFeaturePyDetector=15}; //new 0.20.8 + kFeaturePyDetector=15, //new 0.20.8 + kFeatureSuperPointRpautrat=16}; // new 0.23.3 static std::string typeName(Type type) { @@ -164,6 +166,8 @@ public: return "GFTT+Daisy"; case kFeatureSurfDaisy: return "SURF+Daisy"; + case kFeatureSuperPointRpautrat: + return "SUPERPOINT-RPAUTRAT"; default: return "Unknown"; } @@ -305,7 +309,8 @@ private: bool preciseUpscale_; bool rootSIFT_; bool gpu_; - float guaussianThreshold_; + float gaussianThreshold_; + float maxGaussianThreshold_; bool upscale_; cv::Ptr sift_; @@ -626,6 +631,31 @@ private: bool cuda_; }; +//SuperPointRpautrat +class RTABMAP_CORE_EXPORT SuperPointRpautrat : public Feature2D +{ +public: + SuperPointRpautrat(const ParametersMap & parameters = ParametersMap()); + virtual ~SuperPointRpautrat(); + + virtual void parseParameters(const ParametersMap & parameters); + virtual Feature2D::Type getType() const { return kFeatureSuperPointRpautrat; } + +private: + virtual std::vector generateKeypointsImpl(const cv::Mat & image, const cv::Rect & roi, const cv::Mat & mask = cv::Mat()); + virtual cv::Mat generateDescriptorsImpl(const cv::Mat & image, std::vector & keypoints) const; + + cv::Ptr superPoint_; + + std::string superpointWeightsPath_; + std::string superpointModelPath_; + std::string outputDir_; + float threshold_; + bool nms_; + int minDistance_; + bool cuda_; +}; + //GFTT_DAISY class RTABMAP_CORE_EXPORT GFTT_DAISY : public GFTT { diff --git a/corelib/include/rtabmap/core/FlannIndex.h b/corelib/include/rtabmap/core/FlannIndex.h index a1cd253e..8e387400 100644 --- a/corelib/include/rtabmap/core/FlannIndex.h +++ b/corelib/include/rtabmap/core/FlannIndex.h @@ -37,37 +37,48 @@ namespace rtabmap { class RTABMAP_CORE_EXPORT FlannIndex { public: + // A forward of the internal enum, indexes should match. See src/rtflann/defines.h + enum flann_algorithm_t + { + FLANN_INDEX_LINEAR = 0, + FLANN_INDEX_KDTREE = 1, + FLANN_INDEX_KDTREE_SINGLE = 4, + FLANN_INDEX_LSH = 6, + }; + FlannIndex(); virtual ~FlannIndex(); void release(); + std::vector serializeIndex(bool computeChecksum = true) const; + size_t indexedFeatures() const; // return Bytes size_t memoryUsed() const; // Note that useDistanceL1 doesn't have any effect if LSH is used - void buildLinearIndex( + void buildIndex( + flann_algorithm_t algorithm, const cv::Mat & features, bool useDistanceL1 = false, float rebalancingFactor = 2.0f); - void buildKDTreeIndex( - const cv::Mat & features, - int trees = 4, - bool useDistanceL1 = false, - float rebalancingFactor = 2.0f); - void buildKDTreeSingleIndex( - const cv::Mat & features, - int leafMaxSize = 10, - bool reorder = true, - bool useDistanceL1 = false, - float rebalancingFactor = 2.0f); - void buildLSHIndex( - const cv::Mat & features, - unsigned int table_number = 12, - unsigned int key_size = 20, - unsigned int multi_probe_level = 2, - float rebalancingFactor = 2.0f); + // Return false if the indexData doesn't correspond to expected features used and parameters. + bool loadIndex( + const std::vector & indexData, + flann_algorithm_t algorithm, + const cv::Mat & features, + bool useDistanceL1 = false, + float rebalancingFactor = 2.0f, + std::string * errorMsg = NULL); + bool loadIndex( + const unsigned char * indexData, + size_t indexDataSize, + flann_algorithm_t algorithm, + const cv::Mat & features, + bool useDistanceL1 = false, + float rebalancingFactor = 2.0f, + std::string * errorMsg = NULL); bool isBuilt(); @@ -104,9 +115,9 @@ private: unsigned int nextIndex_; int featuresType_; int featuresDim_; - bool isLSH_; bool useDistanceL1_; // true=EUCLEDIAN_L2 false=MANHATTAN_L1 float rebalancingFactor_; + flann_algorithm_t algorithm_; // keep feature in memory until the tree is rebuilt // (in case the word is deleted when removed from the VWDictionary) diff --git a/corelib/include/rtabmap/core/GlobalMap.h b/corelib/include/rtabmap/core/GlobalMap.h index 14092132..8f9e4a2e 100644 --- a/corelib/include/rtabmap/core/GlobalMap.h +++ b/corelib/include/rtabmap/core/GlobalMap.h @@ -53,6 +53,7 @@ public: public: virtual ~GlobalMap(); + bool fullUpdateNeeded(const std::map & poses) const; bool update(const std::map & poses); // return true if map has changed virtual void clear(); diff --git a/corelib/include/rtabmap/core/Graph.h b/corelib/include/rtabmap/core/Graph.h index 974830cd..76d37b80 100644 --- a/corelib/include/rtabmap/core/Graph.h +++ b/corelib/include/rtabmap/core/Graph.h @@ -56,7 +56,7 @@ bool RTABMAP_CORE_EXPORT exportPoses( 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 + 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, 12=rgbd_bonn std::map & poses, std::multimap * constraints = 0, // optional for formats 3 and 4 std::map * stamps = 0); // optional for format 1 and 9 @@ -118,15 +118,18 @@ Transform RTABMAP_CORE_EXPORT calcRMSE( float & rotational_max, bool align2D = false); -void RTABMAP_CORE_EXPORT computeMaxGraphErrors( +struct MaxGraphErrors +{ + float linear=-1.0f; // absolute error (m) of the link with maximum linear error + float angular=-1.0f; // absolute error (rad) of the link with maximum angular error + float linearRatio=-1.0f; // Ratio = absolute error (m) / linear std (m), of the link with maximum linear error + float angularRatio=-1.0f; // Ratio = absolute error (rad) / angular std (rad), of the link with maximum angular error + Link linearLink; // link with maximum linear error + Link angularLink; // link with maximum angular error +}; +MaxGraphErrors RTABMAP_CORE_EXPORT computeMaxGraphErrors( const std::map & poses, const std::multimap & links, - float & maxLinearErrorRatio, - float & maxAngularErrorRatio, - float & maxLinearError, - float & maxAngularError, - const Link ** maxLinearErrorLink = 0, - const Link ** maxAngularErrorLink = 0, bool for3DoF = false); std::vector RTABMAP_CORE_EXPORT getMaxOdomInf(const std::multimap & links); @@ -277,7 +280,8 @@ std::list > RTABMAP_CORE_EXPORT computePath( bool lookInDatabase = true, bool updateNewCosts = false, float linearVelocity = 0.0f, // m/sec - float angularVelocity = 0.0f); // rad/sec + float angularVelocity = 0.0f, // rad/sec + bool ignoreDirectLinks = false); /** * Find the nearest node of the target pose @@ -336,9 +340,7 @@ RTABMAP_DEPRECATED std::map RTABMAP_CORE_EXPORT getPosesInRadius 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); + const std::vector > & path); // assuming they are all linked in map order float RTABMAP_CORE_EXPORT computePathLength( diff --git a/corelib/include/rtabmap/core/LaserScan.h b/corelib/include/rtabmap/core/LaserScan.h index 0e76fdcb..8e2e4f02 100644 --- a/corelib/include/rtabmap/core/LaserScan.h +++ b/corelib/include/rtabmap/core/LaserScan.h @@ -48,7 +48,8 @@ public: kXYZNormal=8, kXYZINormal=9, kXYZRGBNormal=10, - kXYZIT=11}; + kXYZIT=11, + kXYZIRT=12}; static std::string formatName(const Format & format); static int channels(const Format & format); @@ -57,6 +58,7 @@ public: static bool isScanHasRGB(const Format & format); static bool isScanHasIntensity(const Format & format); static bool isScanHasTime(const Format & format); + static bool isScanHasRing(const Format & format); static LaserScan backwardCompatibility( const cv::Mat & oldScanFormat, int maxPoints = 0, @@ -135,6 +137,7 @@ public: bool hasRGB() const {return isScanHasRGB(format_);} bool hasIntensity() const {return isScanHasIntensity(format_);} bool hasTime() const {return isScanHasTime(format_);} + bool hasRing() const {return isScanHasRing(format_);} bool isCompressed() const {return !data_.empty() && data_.type()==CV_8UC1;} bool isOrganized() const {return data_.rows > 1;} LaserScan clone() const; @@ -143,7 +146,8 @@ public: int getIntensityOffset() const {return hasIntensity()?(is2d()?2:3):-1;} int getRGBOffset() const {return hasRGB()?(is2d()?2:3):-1;} int getNormalsOffset() const {return hasNormals()?(2 + (is2d()?0:1) + ((hasRGB() || hasIntensity())?1:0)):-1;} - int getTimeOffset() const {return hasTime()?4:-1;} + int getRingOffset() const {return format_==kXYZIRT?4:-1;} + int getTimeOffset() const {return format_==kXYZIT?4:(format_==kXYZIRT?5:-1);} float & field(unsigned int pointIndex, unsigned int channelOffset); diff --git a/corelib/include/rtabmap/core/MarkerDetector.h b/corelib/include/rtabmap/core/MarkerDetector.h index d628ca02..630bf87f 100644 --- a/corelib/include/rtabmap/core/MarkerDetector.h +++ b/corelib/include/rtabmap/core/MarkerDetector.h @@ -57,6 +57,12 @@ private: }; class RTABMAP_CORE_EXPORT MarkerDetector { + +public: + enum Strategy { + kStrategyOpencv, + kStrategyApriltag + }; public: MarkerDetector(const ParametersMap & parameters = ParametersMap()); @@ -84,15 +90,19 @@ public: cv::Mat * imageWithDetections = 0); private: -#ifdef HAVE_OPENCV_ARUCO - cv::Ptr detectorParams_; - float markerLength_; + Strategy strategy_; + float markerLength_; + std::map markerLengths_; float maxDepthError_; float maxRange_; float minRange_; int dictionaryId_; +#ifdef HAVE_OPENCV_ARUCO + cv::Ptr detectorParams_; cv::Ptr dictionary_; #endif + void * apriltagLibDetector_; + void * apriltagLibFamily_; }; } /* namespace rtabmap */ diff --git a/corelib/include/rtabmap/core/Memory.h b/corelib/include/rtabmap/core/Memory.h index 39996d92..8c3306ad 100644 --- a/corelib/include/rtabmap/core/Memory.h +++ b/corelib/include/rtabmap/core/Memory.h @@ -140,10 +140,12 @@ public: float radius, const std::map & optimizedPoses, int maxGraphDepth) const; + void convertToIntermediate(int locationId); void deleteLocation(int locationId, std::list * deletedWords = 0); void saveLocationData(int locationId); void removeLink(int idA, int idB); - void removeRawData(int id, bool image = true, bool scan = true, bool userData = true); + void removeRawData(int id, bool image = true, bool scan = true, bool userData = true, bool occupancyGrid = true); + int reduceNode(int id, float maxDistance = 0.0f, bool keepLinkedInDb = false, int direction = 0); //getters const std::map & getWorkingMem() const {return _workingMem;} @@ -161,7 +163,7 @@ public: float getSimilarityThreshold() const {return _similarityThreshold;} std::map getWeights() const; int getLastSignatureId() const; - const Signature * getLastWorkingSignature() const; + const Signature * getLastWorkingSignature(bool ignoreIntermediateNodes) const; std::map getNodesObservingLandmark(int landmarkId, bool lookInDatabase) const; int getSignatureIdByLabel(const std::string & label, bool lookInDatabase = true) const; bool labelSignature(int id, const std::string & label); @@ -211,6 +213,7 @@ public: std::set getAllSignatureIds(bool ignoreChildren = true) const; bool memoryChanged() const {return _memoryChanged;} bool isIncremental() const {return _incrementalMemory;} + bool isReadOnly() const {return !_incrementalMemory && _localizationReadOnly;} bool isLocalizationDataSaved() const {return _localizationDataSaved;} const Signature * getSignature(int id) const; bool isInSTM(int signatureId) const {return _stMem.find(signatureId) != _stMem.end();} @@ -264,6 +267,7 @@ private: void addSignatureToStm(Signature * signature, const cv::Mat & covariance); void clear(); void loadDataFromDb(bool postInitClosingEvents); + void saveFlannIndex(bool postInitClosingEvents); void moveToTrash(Signature * s, bool keepLinkedToGraph = true, std::list * deletedWords = 0); void moveSignatureToWMFromSTM(int id, int * reducedTo = 0); @@ -275,6 +279,7 @@ private: void initCountId(); void rehearsal(Signature * signature, Statistics * stats = 0); bool rehearsalMerge(int oldId, int newId); + bool canBeReduced(const Link & link, float maxDistance, int direction); const std::map & getSignatures() const {return _signatures;} @@ -299,13 +304,16 @@ private: float _similarityThreshold; bool _binDataKept; bool _rawDescriptorsKept; + bool _loadVisualLocalFeaturesOnInit; bool _saveDepth16Format; bool _notLinkedNodesKeptInDb; bool _saveIntermediateNodeData; std::string _rgbCompressionFormat; std::string _depthCompressionFormat; bool _incrementalMemory; + bool _localizationReadOnly; bool _localizationDataSaved; + bool _flannIndexSaved; bool _reduceGraph; int _maxStMemSize; float _recentWmRatio; @@ -353,6 +361,7 @@ private: bool _linksChanged; // False by default, become true when links are modified. int _signaturesAdded; bool _allNodesInWM; + bool _receivingOdometryFeatures; GPS _gpsOrigin; std::vector _rectCameraModels; std::vector _rectStereoCameraModels; diff --git a/corelib/include/rtabmap/core/Odometry.h b/corelib/include/rtabmap/core/Odometry.h index 8b897673..463f56b0 100644 --- a/corelib/include/rtabmap/core/Odometry.h +++ b/corelib/include/rtabmap/core/Odometry.h @@ -53,10 +53,12 @@ public: kTypeOkvis = 6, kTypeLOAM = 7, kTypeMSCKF = 8, - kTypeVINS = 9, + kTypeVINSFusion = 9, kTypeOpenVINS = 10, kTypeFLOAM = 11, - kTypeOpen3D = 12 + kTypeOpen3D = 12, + kTypeCuVSLAM = 13, + kTypeLIOSAM = 14 }; public: diff --git a/corelib/include/rtabmap/core/OdometryThread.h b/corelib/include/rtabmap/core/OdometryThread.h index 6d581b73..f9d7082b 100644 --- a/corelib/include/rtabmap/core/OdometryThread.h +++ b/corelib/include/rtabmap/core/OdometryThread.h @@ -29,6 +29,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #define ODOMETRYTHREAD_H_ #include +#include #include #include #include @@ -55,18 +56,19 @@ private: // MAIN LOOP //============================================================ virtual void mainLoop(); - void addData(const SensorData & data); - bool getData(SensorData & data); + void addData(const SensorEvent & data); + bool getData(SensorEvent & data); private: USemaphore _dataAdded; UMutex _dataMutex; - std::list _dataBuffer; + std::list _dataBuffer; std::list _imuBuffer; Odometry * _odometry; unsigned int _dataBufferMaxSize; bool _resetOdometry; Transform _resetPose; + Transform _previousGuessPose; double _oldestAsyncImuStamp; double _newestAsyncImuStamp; }; diff --git a/corelib/include/rtabmap/core/Parameters.h b/corelib/include/rtabmap/core/Parameters.h index 9ddc4f37..e3c8d574 100644 --- a/corelib/include/rtabmap/core/Parameters.h +++ b/corelib/include/rtabmap/core/Parameters.h @@ -204,6 +204,7 @@ class RTABMAP_CORE_EXPORT Parameters RTABMAP_PARAM(Mem, ImageKept, bool, false, "Keep raw images in RAM."); RTABMAP_PARAM(Mem, BinDataKept, bool, true, "Keep binary data in db."); RTABMAP_PARAM(Mem, RawDescriptorsKept, bool, true, "Raw descriptors kept in memory."); + RTABMAP_PARAM(Mem, LoadVisualLocalFeaturesOnInit, bool, true, "Load all local visual features (keypoints, descriptors and 3D points) in RAM when loading an existing database. This can add significant time to initialize the memory but the features will be already loaded before computing loop closure transforms. If false, the features are loaded on-demand from the database when a loop closure transformation should be estimated."); RTABMAP_PARAM(Mem, MapLabelsAdded, bool, true, "Create map labels. The first node of a map will be labeled as \"map#\" where # is the map ID."); RTABMAP_PARAM(Mem, SaveDepth16Format, bool, false, "Save depth image into 16 bits format to reduce memory used. Warning: values over ~65 meters are ignored (maximum 65535 millimeters)."); RTABMAP_PARAM(Mem, NotLinkedNodesKept, bool, true, "Keep not linked nodes in db (rehearsed nodes and deleted nodes)."); @@ -212,17 +213,18 @@ class RTABMAP_CORE_EXPORT Parameters RTABMAP_PARAM_STR(Mem, DepthCompressionFormat, ".rvl", "Depth image compression format for 16UC1 depth type. It should be \".png\" or \".rvl\". If depth type is 32FC1, \".png\" is used."); RTABMAP_PARAM(Mem, STMSize, unsigned int, 10, "Short-term memory size."); RTABMAP_PARAM(Mem, IncrementalMemory, bool, true, "SLAM mode, otherwise it is Localization mode."); - RTABMAP_PARAM(Mem, LocalizationDataSaved, bool, false, uFormat("Save localization data during localization session (when %s=false). When enabled, the database will then also grow in localization mode. This mode would be used only for debugging purpose.", kMemIncrementalMemory().c_str()).c_str()); - RTABMAP_PARAM(Mem, ReduceGraph, bool, false, "Reduce graph. Merge nodes when loop closures are added (ignoring those with user data set)."); + RTABMAP_PARAM(Mem, LocalizationReadOnly, bool, false, uFormat("In localization mode, open the database in read-only mode (ignored if %s=true). Currrenty incompatible with memory management (%s and %s cannot be used) and if there are disjoint sessions in working memory. Last localization pose won't be saved back in the database at the end of the session, so the robot will always restart to original last localization pose, unless %s is used or an external initial pose is provided on initialization.", kMemIncrementalMemory().c_str(), kRtabmapLoopThr().c_str(), kRtabmapMemoryThr().c_str(), kRGBDStartAtOrigin().c_str()).c_str()); + RTABMAP_PARAM(Mem, LocalizationDataSaved, bool, false, uFormat("Save localization data during localization session (when %s=false). When enabled, the database will then also grow in localization mode. This mode would be used only for debugging purpose.", kMemIncrementalMemory().c_str()).c_str()); + RTABMAP_PARAM(Mem, ReduceGraph, bool, false, uFormat("Reduce graph. Merge nodes when loop closures are added (ignoring those with user data). Note that this approach assumes that 100%% of the loop closures accepted are good, so it is highly recommended to enable \"%s\" at the same time.", kRGBDOptimizeMaxError().c_str())); RTABMAP_PARAM(Mem, RecentWmRatio, float, 0.2, "Ratio of locations after the last loop closure in WM that cannot be transferred."); RTABMAP_PARAM(Mem, TransferSortingByWeightId, bool, false, "On transfer, signatures are sorted by weight->ID only (i.e. the oldest of the lowest weighted signatures are transferred first). If false, the signatures are sorted by weight->Age->ID (i.e. the oldest inserted in WM of the lowest weighted signatures are transferred first). Note that retrieval updates the age, not the ID."); - RTABMAP_PARAM(Mem, RehearsalIdUpdatedToNewOne, bool, false, "On merge, update to new id. When false, no copy."); + RTABMAP_PARAM(Mem, RehearsalIdUpdatedToNewOne, bool, false, uFormat("On merge, update to new id. When false, no copy. Keep this disable if %s=true.", kRtabmapCreateIntermediateNodes().c_str())); RTABMAP_PARAM(Mem, RehearsalWeightIgnoredWhileMoving, bool, false, "When the robot is moving, weights are not updated on rehearsal."); RTABMAP_PARAM(Mem, GenerateIds, bool, true, "True=Generate location IDs, False=use input image IDs."); RTABMAP_PARAM(Mem, BadSignaturesIgnored, bool, false, "Bad signatures are ignored."); RTABMAP_PARAM(Mem, InitWMWithAllNodes, bool, false, "Initialize the Working Memory with all nodes in Long-Term Memory. When false, it is initialized with nodes of the previous session."); RTABMAP_PARAM(Mem, DepthAsMask, bool, true, "Use depth image as mask when extracting features for vocabulary."); - RTABMAP_PARAM(Mem, DepthMaskFloorThr, float, 0.0, uFormat("Filter floor from depth mask below specified threshold (m) before extracting features. 0 means disabled, negative means remove all objects above the floor threshold instead. Ignored if %s is false.", kMemDepthAsMask().c_str())); + RTABMAP_PARAM(Mem, DepthMaskFloorThr, float, 0.0, uFormat("Filter floor from depth mask below specified threshold (m) before extracting features. 0 means disabled. Ignored if %s is false.", kMemDepthAsMask().c_str())); RTABMAP_PARAM(Mem, StereoFromMotion, bool, false, uFormat("Triangulate features without depth using stereo from motion (odometry). It would be ignored if %s is true and the feature detector used supports masking.", kMemDepthAsMask().c_str())); RTABMAP_PARAM(Mem, ImagePreDecimation, unsigned int, 1, uFormat("Decimation of the RGB image before visual feature detection. If depth size is larger than decimated RGB size, depth is decimated to be always at most equal to RGB size. If %s is true and if depth is smaller than decimated RGB, depth may be interpolated to match RGB size for feature detection.",kMemDepthAsMask().c_str())); RTABMAP_PARAM(Mem, ImagePostDecimation, unsigned int, 1, uFormat("Decimation of the RGB image before saving it to database. If depth size is larger than decimated RGB size, depth is decimated to be always at most equal to RGB size. Decimation is done from the original image. If set to same value than %s, data already decimated is saved (no need to re-decimate the image).", kMemImagePreDecimation().c_str())); @@ -247,19 +249,21 @@ class RTABMAP_CORE_EXPORT Parameters RTABMAP_PARAM(Kp, MinDepth, float, 0, "Filter extracted keypoints by depth."); RTABMAP_PARAM(Kp, MaxFeatures, int, 500, "Maximum features extracted from the images (0 means not bounded, <0 means no extraction)."); RTABMAP_PARAM(Kp, SSC, bool, false, "If true, SSC (Suppression via Square Covering) is applied to limit keypoints."); - RTABMAP_PARAM(Kp, BadSignRatio, float, 0.5, "Bad signature ratio (less than Ratio x AverageWordsPerImage = bad)."); + RTABMAP_PARAM(Kp, BadSignRatio, float, 0.5, uFormat("Bad signature ratio. If %s=0, the ratio is computed from the average number of words per signature (less than Ratio x AverageWordsPerImage = bad).", kKpMaxFeatures().c_str())); RTABMAP_PARAM(Kp, NndrRatio, float, 0.8, "NNDR ratio (A matching pair is detected, if its distance is closer than X times the distance of the second nearest neighbor.)"); #if CV_MAJOR_VERSION > 2 && !defined(HAVE_OPENCV_XFEATURES2D) // OpenCV>2 without xFeatures2D module doesn't have BRIEF - RTABMAP_PARAM(Kp, DetectorStrategy, 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"); + RTABMAP_PARAM(Kp, DetectorStrategy, 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 16=SuperPoint-Rpautrat"); #else - RTABMAP_PARAM(Kp, DetectorStrategy, 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"); + RTABMAP_PARAM(Kp, DetectorStrategy, 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 16=SuperPoint-Rpautrat"); #endif RTABMAP_PARAM(Kp, TfIdfLikelihoodUsed, bool, true, "Use of the td-idf strategy to compute the likelihood."); RTABMAP_PARAM(Kp, Parallelized, bool, true, "If the dictionary update and signature creation were parallelized."); RTABMAP_PARAM_STR(Kp, RoiRatios, "0.0 0.0 0.0 0.0", "Region of interest ratios [left, right, top, bottom]."); RTABMAP_PARAM_STR(Kp, DictionaryPath, "", "Path of the pre-computed dictionary"); RTABMAP_PARAM(Kp, NewWordsComparedTogether, bool, true, "When adding new words to dictionary, they are compared also with each other (to detect same words in the same signature)."); + RTABMAP_PARAM(Kp, FlannIndexSaved, bool, false, uFormat("Save FLANN index during localization session (when %s=false). The FLANN index will be saved to database after the first time localization mode is used, then on next sessions, the index is reloaded from the database instead of being rebuilt again. This can save significant loading time when the visual word dictionary is big (>1M words). Note that if the dictionary is modified (parameters or data), the index will be rebuilt and saved again on the next session. Ignored on initialization if %s is enabled.", kMemIncrementalMemory().c_str(), kMemInitWMWithAllNodes().c_str()).c_str()); + RTABMAP_PARAM(Kp, SerializeWithChecksum, bool, true, "On serialization of the FLANN index, compute checksum of the data used by the FLANN index. This adds a slight overhead on serialization/deserialization to make sure that the dictionary data correspond to same data used when the index was built."); RTABMAP_PARAM(Kp, SubPixWinSize, int, 3, "See cv::cornerSubPix()."); RTABMAP_PARAM(Kp, SubPixIterations, int, 0, "See cv::cornerSubPix(). 0 disables sub pixel refining."); RTABMAP_PARAM(Kp, SubPixEps, double, 0.02, "See cv::cornerSubPix()."); @@ -290,7 +294,8 @@ class RTABMAP_CORE_EXPORT Parameters RTABMAP_PARAM(SIFT, PreciseUpscale, bool, false, "Whether to enable precise upscaling in the scale pyramid (OpenCV >= 4.8)."); RTABMAP_PARAM(SIFT, RootSIFT, bool, false, "Apply RootSIFT normalization of the descriptors."); RTABMAP_PARAM(SIFT, Gpu, bool, false, "CudaSift: Use GPU version of SIFT. This option is enabled only if RTAB-Map is built with CudaSift dependency and GPUs are detected."); - RTABMAP_PARAM(SIFT, GaussianThreshold, float, 2.0, "CudaSift: Threshold on difference of Gaussians for feature pruning. The higher the threshold, the less features are produced by the detector."); + RTABMAP_PARAM(SIFT, GaussianThreshold, float, 2.0, "CudaSift: Threshold on difference of Gaussians for feature pruning. The higher the threshold, the less features with low response/hessian are produced by the detector."); + RTABMAP_PARAM(SIFT, MaxGaussianThreshold, float, 0.0, uFormat("CudaSift: Maximum threshold on difference of Gaussians for feature pruning (ignored if smaller or equal than %s). The lower the threshold, the less features with high response/hessian are produced by the detector.", kSIFTGaussianThreshold().c_str())); RTABMAP_PARAM(SIFT, Upscale, bool, false, "CudaSift: Whether to enable upscaling."); RTABMAP_PARAM(BRIEF, Bytes, int, 32, "Bytes is a length of descriptor in bytes. It can be equal 16, 32 or 64 bytes."); @@ -343,6 +348,13 @@ class RTABMAP_CORE_EXPORT Parameters RTABMAP_PARAM(SuperPoint, NMSRadius, int, 4, uFormat("[%s=true] Minimum distance (pixels) between keypoints.", kSuperPointNMS().c_str())); RTABMAP_PARAM(SuperPoint, Cuda, bool, true, "Use Cuda device for Torch, otherwise CPU device is used by default."); + RTABMAP_PARAM_STR(SuperPointRpautrat, WeightsPath, "", "[Required] SuperPoint weights file (*.pth)."); + RTABMAP_PARAM_STR(SuperPointRpautrat, ModelPath, "", "[Required] SuperPoint python model file (superpoint_pytorch.py)."); + RTABMAP_PARAM(SuperPointRpautrat, Threshold, float, 0.005, "Detector response threshold to accept keypoint."); + RTABMAP_PARAM(SuperPointRpautrat, NMS, bool, true, "If true, non-maximum suppression is applied to detected keypoints."); + RTABMAP_PARAM(SuperPointRpautrat, NMSRadius, int, 4, uFormat("[%s=true] Minimum distance (pixels) between keypoints.", kSuperPointRpautratNMS().c_str())); + RTABMAP_PARAM(SuperPointRpautrat, Cuda, bool, true, "Use Cuda device for Torch, otherwise CPU device is used by default."); + RTABMAP_PARAM_STR(PyDetector, Path, "", "Path to python script file (see available ones in rtabmap/corelib/src/python/*). See the header to see where the script should be copied."); RTABMAP_PARAM(PyDetector, Cuda, bool, true, "Use cuda."); @@ -359,14 +371,15 @@ class RTABMAP_CORE_EXPORT Parameters // RGB-D SLAM RTABMAP_PARAM(RGBD, Enabled, bool, true, "Activate metric SLAM. If set to false, classic RTAB-Map loop closure detection is done using only images and without any metric information."); - RTABMAP_PARAM(RGBD, LinearUpdate, float, 0.1, "Minimum linear displacement (m) to update the map. Rehearsal is done prior to this, so weights are still updated."); - RTABMAP_PARAM(RGBD, AngularUpdate, float, 0.1, "Minimum angular displacement (rad) to update the map. Rehearsal is done prior to this, so weights are still updated."); + RTABMAP_PARAM(RGBD, LinearUpdate, float, 0.1, uFormat("Minimum linear displacement (m) to update the map. Rehearsal is done prior to this, so weights are still updated. To update the map when not moving, both %s and %s should be set to 0.", Parameters::kRGBDLinearUpdate().c_str(), Parameters::kRGBDAngularUpdate().c_str())); + RTABMAP_PARAM(RGBD, AngularUpdate, float, 0.1, uFormat("Minimum angular displacement (rad) to update the map. Rehearsal is done prior to this, so weights are still updated. To update the map when not moving, both %s and %s should be set to 0.", Parameters::kRGBDLinearUpdate().c_str(), Parameters::kRGBDAngularUpdate().c_str())); RTABMAP_PARAM(RGBD, LinearSpeedUpdate, float, 0.0, "Maximum linear speed (m/s) to update the map (0 means not limit)."); RTABMAP_PARAM(RGBD, AngularSpeedUpdate, float, 0.0, "Maximum angular speed (rad/s) to update the map (0 means not limit)."); RTABMAP_PARAM(RGBD, AggressiveLoopThr, float, 0.05, uFormat("Loop closure threshold used (overriding %s) when a new mapping session is not yet linked to a map of the highest loop closure hypothesis. In localization mode, this threshold is used when there are no loop closure constraints with any map in the cache (%s). In all cases, the goal is to aggressively loop on a previous map in the database. Only used when %s is enabled. Set 1 to disable.", kRtabmapLoopThr().c_str(), kRGBDMaxOdomCacheSize().c_str(), kRGBDEnabled().c_str())); RTABMAP_PARAM(RGBD, NewMapOdomChangeDistance, float, 0, "A new map is created if a change of odometry translation greater than X m is detected (0 m = disabled)."); RTABMAP_PARAM(RGBD, OptimizeFromGraphEnd, bool, false, "Optimize graph from the newest node. If false, the graph is optimized from the oldest node of the current graph (this adds an overhead computation to detect to oldest node of the current graph, but it can be useful to preserve the map referential from the oldest node). Warning when set to false: when some nodes are transferred, the first referential of the local map may change, resulting in momentary changes in robot/map position (which are annoying in teleoperation)."); - RTABMAP_PARAM(RGBD, OptimizeMaxError, float, 3.0, uFormat("Reject loop closures if optimization error ratio is greater than this value (0=disabled). Ratio is computed as absolute error over standard deviation of each link. This will help to detect when a wrong loop closure is added to the graph. Not compatible with \"%s\" if enabled.", kOptimizerRobust().c_str())); + RTABMAP_PARAM(RGBD, OptimizeMaxError, float, 3.0, uFormat("Reject loop closures if optimization error ratio is greater than this value (0=disabled). Ratio is computed as absolute error over standard deviation of each link. This will help to detect when a wrong loop closure is added to the graph. If used with \"%s\", the disabled loop closure links will be removed.", kOptimizerRobust().c_str())); + RTABMAP_PARAM(RGBD, OptimizeMaxErrorRepairRadius, float, 0.0, uFormat("If two consecutive loop closures are rejected by %s on the same old loop closure link, we will remove that old link, and other old links under that radius if necessary, until optimization is accepted. When optimization is accepted, the old loop closure links are removed from the graph. This feature is useful to reject bad loop closures that were accepted previously. Set to 0 to disable this feature.", kRGBDOptimizeMaxError().c_str())); RTABMAP_PARAM(RGBD, MaxLoopClosureDistance, float, 0.0, "Reject loop closures/localizations if the distance from the map is over this distance (0=disabled)."); RTABMAP_PARAM(RGBD, ForceOdom3DoF, bool, true, uFormat("Force odometry pose to be 3DoF if %s=true.", kRegForce3DoF().c_str())); RTABMAP_PARAM(RGBD, StartAtOrigin, bool, false, uFormat("If true, rtabmap will assume the robot is starting 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 (%s=false).", kMemIncrementalMemory().c_str())); @@ -428,7 +441,7 @@ class RTABMAP_CORE_EXPORT Parameters #endif #endif RTABMAP_PARAM(Optimizer, VarianceIgnored, bool, false, "Ignore constraints' variance. If checked, identity information matrix is used for each constraint. Otherwise, an information matrix is generated from the variance saved in the links."); - RTABMAP_PARAM(Optimizer, Robust, bool, false, uFormat("Robust graph optimization using Vertigo (only work for g2o and GTSAM optimization strategies). Not compatible with \"%s\" if enabled.", kRGBDOptimizeMaxError().c_str())); + RTABMAP_PARAM(Optimizer, Robust, bool, false, "Robust graph optimization using Vertigo (only work for g2o and GTSAM optimization strategies)."); RTABMAP_PARAM(Optimizer, PriorsIgnored, bool, true, "Ignore prior constraints (global pose or GPS) while optimizing. Currently only g2o and gtsam optimization supports this."); RTABMAP_PARAM(Optimizer, LandmarksIgnored, bool, false, "Ignore landmark constraints while optimizing. Currently only g2o and gtsam optimization supports this."); #if defined(RTABMAP_G2O) || defined(RTABMAP_GTSAM) @@ -453,8 +466,8 @@ class RTABMAP_CORE_EXPORT Parameters RTABMAP_PARAM(GTSAM, IncRelinearizeSkip, int, 1, "Only relinearize any variables every X calls to ISAM2::update(). See GTSAM::ISAM2 doc for more info."); // Odometry - RTABMAP_PARAM(Odom, Strategy, int, 0, "0=Frame-to-Map (F2M) 1=Frame-to-Frame (F2F) 2=Fovis 3=viso2 4=DVO-SLAM 5=ORB_SLAM2 6=OKVIS 7=LOAM 8=MSCKF_VIO 9=VINS-Fusion 10=OpenVINS 11=FLOAM 12=Open3D"); - RTABMAP_PARAM(Odom, ResetCountdown, int, 0, "Automatically reset odometry after X consecutive images on which odometry cannot be computed (value=0 disables auto-reset)."); + RTABMAP_PARAM(Odom, Strategy, int, 0, "0=Frame-to-Map (F2M) 1=Frame-to-Frame (F2F) 2=Fovis 3=viso2 4=DVO-SLAM 5=ORB_SLAM 6=OKVIS 7=LOAM 8=MSCKF_VIO 9=VINS-Fusion 10=OpenVINS 11=FLOAM 12=Open3D 13=cuVSLAM 14=LIO-SAM"); + RTABMAP_PARAM(Odom, ResetCountdown, int, 0, "Automatically reset odometry after X consecutive images where odometry cannot be computed (a value of 0 disables auto-reset). When a reset occurs, odometry resumes from the last successfully computed pose with large covariance to trigger a new map. If external odometry is used, it will also be reset based on the motion estimated relative to the last computed pose but no large covariance will be received, so that a new map won't be triggered."); RTABMAP_PARAM(Odom, Holonomic, bool, true, "If the robot is holonomic (strafing commands can be issued). If not, y value will be estimated from x and yaw values (y=x*tan(yaw))."); RTABMAP_PARAM(Odom, FillInfoData, bool, true, "Fill info with data (inliers/outliers features)."); RTABMAP_PARAM(Odom, ImageBufferSize, unsigned int, 1, "Data buffer size (0 min inf)."); @@ -548,7 +561,7 @@ class RTABMAP_CORE_EXPORT Parameters RTABMAP_PARAM(OdomViso2, BucketWidth, double, 50, "Width of bucket."); RTABMAP_PARAM(OdomViso2, BucketHeight, double, 50, "Height of bucket."); - // Odometry ORB_SLAM2 + // Odometry ORB_SLAM 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."); @@ -603,75 +616,94 @@ class RTABMAP_CORE_EXPORT Parameters RTABMAP_PARAM(OdomMSCKF, InitCovExTrans, double, 0.000025, ""); RTABMAP_PARAM(OdomMSCKF, MaxCamStateSize, int, 20, ""); - // Odometry VINS - RTABMAP_PARAM_STR(OdomVINS, ConfigPath, "", "Path of VINS config file."); + // Odometry VINS-Fusion + RTABMAP_PARAM_STR(OdomVINSFusion, ConfigPath, "", "Path of VINS-Fusion 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_STR(OdomOpenVINS, ConfigPath, "", "Path of OpenVINS config file (*.yaml). Same format used than OpenVINS library. Note that any parameter from that config file will overwrite the same parameter in OdomOpenVINS group."); + 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, 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, 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, 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, 1.0, "Pixel noise for MSCKF features"); - RTABMAP_PARAM(OdomOpenVINS, UpMSCKFChi2Multiplier, double, 1.0, "Chi2 multiplier for MSCKF features"); - RTABMAP_PARAM(OdomOpenVINS, UpSLAMSigmaPx, double, 1.0, "Pixel noise for SLAM features"); - RTABMAP_PARAM(OdomOpenVINS, UpSLAMChi2Multiplier, double, 1.0, "Chi2 multiplier for SLAM features"); + 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, 1.0, "Pixel noise for MSCKF features."); + RTABMAP_PARAM(OdomOpenVINS, UpMSCKFChi2Multiplier, double, 1.0, "Chi2 multiplier for MSCKF features."); + RTABMAP_PARAM(OdomOpenVINS, UpSLAMSigmaPx, double, 1.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."); + // Odometry cuVSLAM + RTABMAP_PARAM(OdomCuVSLAM, MulticamMode, int, 0, "cuVSLAM multicam_mode setting: 0=moderate, 1=performance, 2=precision."); + + // Odometry LIO-SAM + RTABMAP_PARAM_STR(OdomLIOSAM, ConfigPath, "", "Path to LIO-SAM params.yaml config file. When set, sensor/IMU/feature parameters are loaded from the file and the individual parameters below are ignored."); + RTABMAP_PARAM(OdomLIOSAM, Sensor, int, 0, "LiDAR sensor: 0=Velodyne, 1=Ouster, 2=Livox"); + RTABMAP_PARAM(OdomLIOSAM, NScan, int, 16, "Number of LiDAR channels (16, 32, 64, 128)."); + RTABMAP_PARAM(OdomLIOSAM, HorizonScan, int, 1800, "Horizontal resolution (Velodyne:1800, Ouster:512/1024/2048)."); + RTABMAP_PARAM(OdomLIOSAM, ImuAccNoise, float, 0.01, "IMU accelerometer white noise."); + RTABMAP_PARAM(OdomLIOSAM, ImuGyrNoise, float, 0.001, "IMU gyroscope white noise."); + RTABMAP_PARAM(OdomLIOSAM, ImuAccBiasN, float, 0.0002,"IMU accelerometer bias noise."); + RTABMAP_PARAM(OdomLIOSAM, ImuGyrBiasN, float, 0.00003,"IMU gyroscope bias noise."); + RTABMAP_PARAM(OdomLIOSAM, ImuGravity, float, 9.80511,"Gravity magnitude."); + RTABMAP_PARAM(OdomLIOSAM, EdgeThreshold,float, 1.0, "Edge feature curvature threshold."); + RTABMAP_PARAM(OdomLIOSAM, SurfThreshold,float, 0.1, "Surface feature curvature threshold."); + RTABMAP_PARAM(OdomLIOSAM, LinVar, float, 0.01, "Linear output variance."); + RTABMAP_PARAM(OdomLIOSAM, AngVar, float, 0.01, "Angular output variance."); + // Common registration parameters RTABMAP_PARAM(Reg, RepeatOnce, bool, true, "Do a second registration with the output of the first registration as guess. Only done if no guess was provided for the first registration (like on loop closure). It can be useful if the registration approach used can use a guess to get better matches."); RTABMAP_PARAM(Reg, Strategy, int, 0, "0=Vis, 1=Icp, 2=VisIcp"); @@ -701,16 +733,16 @@ class RTABMAP_CORE_EXPORT Parameters 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"); + 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 16=SuperPoint-Rpautrat"); #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"); + 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 16=SuperPoint-Rpautrat"); #endif RTABMAP_PARAM(Vis, MaxFeatures, int, 1000, "0 no limits."); RTABMAP_PARAM(Vis, SSC, bool, false, "If true, SSC (Suppression via Square Covering) is applied to limit keypoints."); 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(Vis, DepthMaskFloorThr, float, 0.0, uFormat("Filter floor from depth mask below specified threshold (m) before extracting features. 0 means disabled, negative means remove all objects above the floor threshold instead. Ignored if %s is false.", kVisDepthAsMask().c_str())); + RTABMAP_PARAM(Vis, DepthMaskFloorThr, float, 0.0, uFormat("Filter floor from depth mask below specified threshold (m) before extracting features. 0 means disabled. Ignored if %s is false.", kVisDepthAsMask().c_str())); 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."); @@ -726,8 +758,11 @@ class RTABMAP_CORE_EXPORT Parameters 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, CorFlowGpu, bool, false, uFormat("[%s=1] Enable GPU version of the optical flow approach (only available if OpenCV is built with CUDA).", kVisCorType().c_str())); -#if defined(RTABMAP_G2O) || defined(RTABMAP_ORB_SLAM) + RTABMAP_PARAM(Vis, CorFlowUseMinEigenVals, bool, true, uFormat("[%s=1] See cv::calcOpticalFlowPyrLK(). Used for optical flow approach. Use minimum eigen values as an error measure, otherwise L1 distance between patches is used as an error measure.", kVisCorType().c_str())); + RTABMAP_PARAM(Vis, CorFlowMinEigThreshold, float, 1e-4, uFormat("[%s=true] If the minimum eigenvalue of a feature's spatial gradient matrix is less than this threshold, then the feature is filtered out.", kVisCorFlowUseMinEigenVals().c_str())); + RTABMAP_PARAM(Vis, CorFlowErrorThreshold, float, 20, uFormat("[%s=false] Filter out features with error greater than this threshold.", kVisCorFlowUseMinEigenVals().c_str())); + RTABMAP_PARAM(Vis, CorFlowGpu, bool, false, uFormat("[%s=1] Enable GPU version of the optical flow approach (only available if OpenCV is built with CUDA). Note that %s is not used in the GPU implementation.", kVisCorType().c_str(), kVisCorFlowUseMinEigenVals().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."); #else RTABMAP_PARAM(Vis, BundleAdjustment, int, 0, "Optimization with bundle adjustment: 0=disabled, 1=g2o, 2=cvsba, 3=Ceres."); @@ -804,7 +839,10 @@ class RTABMAP_CORE_EXPORT Parameters RTABMAP_PARAM(Stereo, OpticalFlow, bool, true, "Use optical flow to find stereo correspondences, otherwise a simple block matching approach is used."); RTABMAP_PARAM(Stereo, SSD, bool, true, uFormat("[%s=false] Use Sum of Squared Differences (SSD) window, otherwise Sum of Absolute Differences (SAD) window is used.", kStereoOpticalFlow().c_str())); RTABMAP_PARAM(Stereo, Eps, double, 0.01, uFormat("[%s=true] Epsilon stop criterion.", kStereoOpticalFlow().c_str())); - RTABMAP_PARAM(Stereo, Gpu, bool, false, uFormat("[%s=true] Enable GPU version of the optical flow approach (only available if OpenCV is built with CUDA).", kStereoOpticalFlow().c_str())); + RTABMAP_PARAM(Stereo, UseMinEigenVals, bool, true, uFormat("[%s=true] Use minimum eigen values as an error measure, otherwise L1 distance between patches is used as an error measure.", kStereoOpticalFlow().c_str())); + RTABMAP_PARAM(Stereo, MinEigThreshold, double, 1e-4, uFormat("[%s=true] If the minimum eigenvalue of a feature's spatial gradient matrix is less than this threshold, then the feature is filtered out.", kStereoUseMinEigenVals().c_str())); + RTABMAP_PARAM(Stereo, ErrorThreshold, double, 50, uFormat("[%s=false] Filter out features with error greater than this threshold.", kStereoUseMinEigenVals().c_str())); + RTABMAP_PARAM(Stereo, Gpu, bool, false, uFormat("[%s=true] Enable GPU version of the optical flow approach (only available if OpenCV is built with CUDA). Note that %s is not used in the GPU implementation.", kStereoOpticalFlow().c_str(), kStereoUseMinEigenVals().c_str())); RTABMAP_PARAM(Stereo, DenseStrategy, int, 0, "0=cv::StereoBM, 1=cv::StereoSGBM"); @@ -880,19 +918,29 @@ class RTABMAP_CORE_EXPORT Parameters RTABMAP_PARAM(GridGlobal, ProbClampingMax, float, 0.971, "Probability clamping maximum (value between 0 and 1)."); RTABMAP_PARAM(GridGlobal, FloodFillDepth, unsigned int, 0, "Flood fill filter (0=disabled), used to remove empty cells outside the map. The flood fill is done at the specified depth (between 1 and 16) of the OctoMap."); - RTABMAP_PARAM(Marker, Dictionary, int, 0, "Dictionary to use: DICT_ARUCO_4X4_50=0, DICT_ARUCO_4X4_100=1, DICT_ARUCO_4X4_250=2, DICT_ARUCO_4X4_1000=3, DICT_ARUCO_5X5_50=4, DICT_ARUCO_5X5_100=5, DICT_ARUCO_5X5_250=6, DICT_ARUCO_5X5_1000=7, DICT_ARUCO_6X6_50=8, DICT_ARUCO_6X6_100=9, DICT_ARUCO_6X6_250=10, DICT_ARUCO_6X6_1000=11, DICT_ARUCO_7X7_50=12, DICT_ARUCO_7X7_100=13, DICT_ARUCO_7X7_250=14, DICT_ARUCO_7X7_1000=15, DICT_ARUCO_ORIGINAL = 16, DICT_APRILTAG_16h5=17, DICT_APRILTAG_25h9=18, DICT_APRILTAG_36h10=19, DICT_APRILTAG_36h11=20"); - RTABMAP_PARAM(Marker, Length, float, 0, "The length (m) of the markers' side. 0 means automatic marker length estimation using the depth image (the camera should look at the marker perpendicularly for initialization)."); + RTABMAP_PARAM(Marker, Strategy, int, 0, "Marker detection implementation: 0=OpenCV, 1=AprilTag"); + RTABMAP_PARAM(Marker, Dictionary, int, 0, "Dictionary to use: DICT_ARUCO_4X4_50=0, DICT_ARUCO_4X4_100=1, DICT_ARUCO_4X4_250=2, DICT_ARUCO_4X4_1000=3, DICT_ARUCO_5X5_50=4, DICT_ARUCO_5X5_100=5, DICT_ARUCO_5X5_250=6, DICT_ARUCO_5X5_1000=7, DICT_ARUCO_6X6_50=8, DICT_ARUCO_6X6_100=9, DICT_ARUCO_6X6_250=10, DICT_ARUCO_6X6_1000=11, DICT_ARUCO_7X7_50=12, DICT_ARUCO_7X7_100=13, DICT_ARUCO_7X7_250=14, DICT_ARUCO_7X7_1000=15, DICT_ARUCO_ORIGINAL = 16, DICT_APRILTAG_16h5=17, DICT_APRILTAG_25h9=18, DICT_APRILTAG_36h10=19, DICT_APRILTAG_36h11=20, DICT_ARUCO_MIP_36H12=21"); + RTABMAP_PARAM(Marker, Length, float, 0, "The length (m) of the markers' side. Value <=0 means automatic marker length estimation using the depth image (the camera should look at the marker perpendicularly for initialization). If 0, the length is estimated only on the first marker detected, then re-used for all next detections (i.e., this assumes that markers have all the same length). With <0, the length is estimated once for each unique marker, then re-used for next detections with the same marker ID."); + RTABMAP_PARAM_STR(Marker, Lengths, "", uFormat("List of markers to detect. Format is the marker's ID followed by its length (in meters), multiple markers are separated by a vertical line (\"id1 length|id2 length\"). We can also define a range of markers with \"id1:id2 length\" (id2 included). If empty, all markers of the chosen dictionary can be detected and their length is set/estimated based on %s. For example, to detect markers 12 and 14 with lengths of 8 and 15 cm respectively, and all markers between 30 and 40 with a length of 10 cm, set \"12 0.08|14 0.15|30:40 0.1\".", kMarkerLength().c_str()).c_str()); RTABMAP_PARAM(Marker, MaxDepthError, float, 0.01, uFormat("Maximum depth error between all corners of a marker when estimating the marker length (when %s is 0). The smaller it is, the more perpendicular the camera should be toward the marker to initialize the length.", kMarkerLength().c_str())); RTABMAP_PARAM(Marker, VarianceLinear, float, 0.001, uFormat("Linear variance to set on marker detections. If %s is enabled and %s=2 (GTSAM): it is the variance of the range factor, with 9999 to disable range factor and to do only bearing.", kMarkerVarianceOrientationIgnored().c_str(), kOptimizerStrategy().c_str())); RTABMAP_PARAM(Marker, VarianceAngular, float, 0.01, uFormat("Angular variance to set on marker detections. If %s is enabled, it is ignored with %s=1 (g2o) and it corresponds to bearing variance with %s=2 (GTSAM).", kMarkerVarianceOrientationIgnored().c_str(), kOptimizerStrategy().c_str(), kOptimizerStrategy().c_str())); RTABMAP_PARAM(Marker, VarianceOrientationIgnored, bool, false, uFormat("When this setting is false, the landmark's orientation is optimized during graph optimization. When this setting is true, only the position of the landmark is optimized. This can be useful when the landmark's orientation estimation is not reliable. Note that for %s=1 (g2o), only %s needs be set if we ignore orientation. For %s=2 (GTSAM), instead of optimizing the landmark's position directly, a bearing/range factor is used, with %s as the variance of the range factor (with 9999 to optimize the position with only a bearing factor) and %s as the variance of the bearing factor (pitch/yaw).", kOptimizerStrategy().c_str(), kMarkerVarianceLinear().c_str(), kOptimizerStrategy().c_str(), kMarkerVarianceLinear().c_str(), kMarkerVarianceAngular().c_str())); - RTABMAP_PARAM(Marker, CornerRefinementMethod, int, 0, "Corner refinement method (0: None, 1: Subpixel, 2:contour, 3: AprilTag2). For OpenCV <3.3.0, this is \"doCornerRefinement\" parameter: set 0 for false and 1 for true."); RTABMAP_PARAM(Marker, MaxRange, float, 0.0, "Maximum range in which markers will be detected. <=0 for unlimited range."); RTABMAP_PARAM(Marker, MinRange, float, 0.0, "Miniminum range in which markers will be detected. <=0 for unlimited range."); - RTABMAP_PARAM_STR(Marker, Priors, "", "World prior locations of the markers. The map will be transformed in marker's world frame when a tag is detected. Format is the marker's ID followed by its position (angles in rad), markers are separated by vertical line (\"id1 x y z roll pitch yaw|id2 x y z roll pitch yaw\"). Example: \"1 0 0 1 0 0 0|2 1 0 1 0 0 1.57\" (marker 2 is 1 meter forward than marker 1 with 90 deg yaw rotation)."); + RTABMAP_PARAM_STR(Marker, Priors, "", "World prior locations of the markers. The map will be transformed in marker's world frame when a tag is detected. Format is the marker's ID followed by its position (angles in rad), multiple markers are separated by vertical line (\"id1 x y z roll pitch yaw|id2 x y z roll pitch yaw\"). Example: \"1 0 0 1 0 0 0|2 1 0 1 0 0 1.57\" (marker 2 is 1 meter forward than marker 1 with 90 deg yaw rotation)."); RTABMAP_PARAM(Marker, PriorsVarianceLinear, float, 0.001, "Linear variance to set on marker priors."); RTABMAP_PARAM(Marker, PriorsVarianceAngular, float, 0.001, "Angular variance to set on marker priors."); + RTABMAP_PARAM(MarkerAprilTag, NThreads, int, 1, "How many threads should be used?"); + RTABMAP_PARAM(MarkerAprilTag, QuadDecimate, float, 1.0, "Detection of quads can be done on a lower-resolution image, improving speed at a cost of pose accuracy and a slight decrease in detection rate. Decoding the binary payload is still done at full resolution."); + RTABMAP_PARAM(MarkerAprilTag, QuadSigma, float, 0.0, "What Gaussian blur should be applied to the segmented image (used for quad detection?) Parameter is the standard deviation in pixels. Very noisy images benefit from non-zero values (e.g. 0.8)."); + RTABMAP_PARAM(MarkerAprilTag, RefineEdges, bool, true, uFormat("When true, the edges of the each quad are adjusted to \"snap to\" strong gradients nearby. This is useful when decimation is employed, as it can increase the quality of the initial quad estimate substantially. Generally recommended to be on (true). Very computationally inexpensive. Option is ignored if %s = 1.", kMarkerAprilTagQuadDecimate().c_str())); + RTABMAP_PARAM(MarkerAprilTag, DecodeSharpening, double, 0.25, "How much sharpening should be done to decoded images? This can help decode small tags but may or may not help in odd lighting conditions or low light conditions."); + RTABMAP_PARAM(MarkerAprilTag, Debug, bool, false, uFormat("When true, write a variety of debugging images to the working directory where the app started (not %s) at various stages through the detection process. (Somewhat slow).", kRtabmapWorkingDirectory().c_str())); + + RTABMAP_PARAM(MarkerOpenCV, CornerRefinementMethod, int, 0, "Corner refinement method for OpenCV strategy (0: None, 1: Subpixel, 2:contour, 3: AprilTag2). For OpenCV <3.3.0, this is \"doCornerRefinement\" parameter: set 0 for false and 1 for true."); + RTABMAP_PARAM(ImuFilter, MadgwickGain, double, 0.1, "Gain of the filter. Higher values lead to faster convergence but more noise. Lower values lead to slower convergence but smoother signal, belongs in [0, 1]."); RTABMAP_PARAM(ImuFilter, MadgwickZeta, double, 0.0, "Gyro drift gain (approx. rad/s), belongs in [-1, 1]."); diff --git a/corelib/include/rtabmap/core/PythonInterface.h b/corelib/include/rtabmap/core/PythonInterface.h index f2f3a6a2..3129c75e 100644 --- a/corelib/include/rtabmap/core/PythonInterface.h +++ b/corelib/include/rtabmap/core/PythonInterface.h @@ -8,6 +8,7 @@ #ifndef CORELIB_SRC_PYTHON_PYTHONINTERFACE_H_ #define CORELIB_SRC_PYTHON_PYTHONINTERFACE_H_ +#include "rtabmap/core/rtabmap_core_export.h" // DLL export/import defines #include #include @@ -23,7 +24,7 @@ namespace rtabmap { * Create a single PythonInterface on main thread at * global scope before any Python classes. */ -class PythonInterface +class RTABMAP_CORE_EXPORT PythonInterface { public: PythonInterface(); @@ -34,7 +35,7 @@ private: pybind11::gil_scoped_release* release_; }; -std::string getPythonTraceback(); +std::string RTABMAP_CORE_EXPORT getPythonTraceback(); } diff --git a/corelib/include/rtabmap/core/RegistrationInfo.h b/corelib/include/rtabmap/core/RegistrationInfo.h index 75592898..3ec65098 100644 --- a/corelib/include/rtabmap/core/RegistrationInfo.h +++ b/corelib/include/rtabmap/core/RegistrationInfo.h @@ -41,6 +41,7 @@ public: inliersMeanDistance(0.0f), inliersDistribution(0.0f), matches(0), + variance(0.0f), icpInliersRatio(0), icpTranslation(0.0f), icpRotation(0.0f), @@ -64,6 +65,7 @@ public: output.inliersDistribution = inliersDistribution; output.matches = matches; output.matchesPerCam = matchesPerCam; + output.variance = variance; output.icpInliersRatio = icpInliersRatio; output.icpTranslation = icpTranslation; output.icpRotation = icpRotation; @@ -85,6 +87,7 @@ public: float inliersDistribution; std::vector inliersIDs; int matches; + float variance; std::vector matchesIDs; std::vector projectedIDs; // "From" IDs std::vector inliersPerCam; diff --git a/corelib/include/rtabmap/core/RegistrationVis.h b/corelib/include/rtabmap/core/RegistrationVis.h index 3e13d8ba..e87b2229 100644 --- a/corelib/include/rtabmap/core/RegistrationVis.h +++ b/corelib/include/rtabmap/core/RegistrationVis.h @@ -91,6 +91,9 @@ private: float _flowEps; int _flowMaxLevel; bool _flowGpu; + bool _flowUseMinEigenVals; + float _flowMinEigThreshold; + float _flowErrorThreshold; float _nndr; int _nnType; bool _gmsWithRotation; diff --git a/corelib/include/rtabmap/core/Rtabmap.h b/corelib/include/rtabmap/core/Rtabmap.h index f15a8ef4..b3d438e8 100644 --- a/corelib/include/rtabmap/core/Rtabmap.h +++ b/corelib/include/rtabmap/core/Rtabmap.h @@ -35,6 +35,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "rtabmap/core/Statistics.h" #include "rtabmap/core/Link.h" #include "rtabmap/core/ProgressState.h" +#include "rtabmap/core/Graph.h" #include #include @@ -209,7 +210,8 @@ public: bool intraSession = true, bool interSession = true, const ProgressState * state = 0, - float clusterRadiusMin = 0.0f); + float clusterRadiusMin = 0.0f, + int toFromMapId = -1); bool globalBundleAdjustment( int optimizerType = 1 /*g2o*/, bool rematchFeatures = true, @@ -263,6 +265,13 @@ private: std::multimap * constraints = 0, double * error = 0, int * iterationsDone = 0) const; + std::list > repairGraph( + graph::MaxGraphErrors & maxGraphErrors, + std::map & poses, + std::multimap & constraints, + double & optimizationError, + int & optimizationIterations, + cv::Mat & optimizationCovariance); void updateGoalIndex(); bool computePath(int targetNode, std::map nodes, const std::multimap & constraints); @@ -319,6 +328,7 @@ private: std::string _databasePath; bool _optimizeFromGraphEnd; float _optimizationMaxError; + float _optimizationMaxErrorRepairRadius; bool _startNewMapOnLoopClosure; bool _startNewMapOnGoodSignature; float _goalReachedRadius; // meters @@ -378,6 +388,7 @@ private: std::map _odomCachePoses; // used in localization mode to reject loop closures std::multimap _odomCacheConstraints; // used in localization mode to reject loop closures std::map _markerPriors; + std::pair _lastRejectedLoopClosureIds; std::set _nodesToRepublish; diff --git a/corelib/include/rtabmap/core/RtabmapThread.h b/corelib/include/rtabmap/core/RtabmapThread.h index 96fa214c..9cdecd8b 100644 --- a/corelib/include/rtabmap/core/RtabmapThread.h +++ b/corelib/include/rtabmap/core/RtabmapThread.h @@ -106,7 +106,6 @@ private: unsigned int _dataBufferMaxSize; float _rate; bool _createIntermediateNodes; - UTimer * _frameRateTimer; double _previousStamp; Rtabmap * _rtabmap; diff --git a/corelib/include/rtabmap/core/SensorData.h b/corelib/include/rtabmap/core/SensorData.h index 18fc5d36..ea5a5dee 100644 --- a/corelib/include/rtabmap/core/SensorData.h +++ b/corelib/include/rtabmap/core/SensorData.h @@ -212,6 +212,12 @@ public: _userDataCompressed.empty() && _keypoints.size() == 0 && _descriptors.empty() && + _groundCellsRaw.empty() && + _groundCellsCompressed.empty() && + _obstacleCellsRaw.empty() && + _obstacleCellsCompressed.empty() && + _emptyCellsRaw.empty() && + _emptyCellsCompressed.empty() && imu_.empty()); } @@ -309,8 +315,6 @@ public: const cv::Mat & empty, float cellSize, const cv::Point3f & viewPoint); - // remove raw occupancy grids - void clearOccupancyGridRaw() {_groundCellsRaw = cv::Mat(); _obstacleCellsRaw = cv::Mat();} const cv::Mat & gridGroundCellsRaw() const {return _groundCellsRaw;} const cv::Mat & gridGroundCellsCompressed() const {return _groundCellsCompressed;} const cv::Mat & gridObstacleCellsRaw() const {return _obstacleCellsRaw;} @@ -355,12 +359,12 @@ public: * Clear compressed rgb/depth (left/right) images, compressed laser scan and compressed user data. * Raw data are kept is set. */ - void clearCompressedData(bool images = true, bool scan = true, bool userData = true); + void clearCompressedData(bool images = true, bool scan = true, bool userData = true, bool occupancyGrid = true); /** * Clear raw rgb/depth (left/right) images, raw laser scan and raw user data. * Compressed data are kept is set. */ - void clearRawData(bool images = true, bool scan = true, bool userData = true); + void clearRawData(bool images = true, bool scan = true, bool userData = true, bool occupancyGrid = true); bool isPointVisibleFromCameras(const cv::Point3f & pt) const; // assuming point is in robot frame diff --git a/corelib/include/rtabmap/core/Statistics.h b/corelib/include/rtabmap/core/Statistics.h index 61abddea..1cdaf40d 100644 --- a/corelib/include/rtabmap/core/Statistics.h +++ b/corelib/include/rtabmap/core/Statistics.h @@ -67,6 +67,7 @@ class RTABMAP_CORE_EXPORT Statistics RTABMAP_STATS(Loop, Visual_inliers,); RTABMAP_STATS(Loop, Visual_inliers_ratio,); RTABMAP_STATS(Loop, Visual_matches,); + RTABMAP_STATS(Loop, Visual_variance,); RTABMAP_STATS(Loop, Distance_since_last_loc, m); RTABMAP_STATS(Loop, Last_id,); RTABMAP_STATS(Loop, Optimization_max_error, m); @@ -75,6 +76,13 @@ class RTABMAP_CORE_EXPORT Statistics RTABMAP_STATS(Loop, Optimization_max_ang_error_ratio, ); RTABMAP_STATS(Loop, Optimization_error, ); RTABMAP_STATS(Loop, Optimization_iterations, ); + RTABMAP_STATS(Loop, Optimization_max_error_from_id, ); + RTABMAP_STATS(Loop, Optimization_max_error_to_id, ); + RTABMAP_STATS(Loop, Optimization_max_ang_error_from_id, ); + RTABMAP_STATS(Loop, Optimization_max_ang_error_to_id, ); + RTABMAP_STATS(Loop, Optimization_max_error_removed_from_id, ); + RTABMAP_STATS(Loop, Optimization_max_error_removed_to_id, ); + RTABMAP_STATS(Loop, Optimization_max_error_removed_count, ); RTABMAP_STATS(Loop, Linear_variance,); RTABMAP_STATS(Loop, Angular_variance,); RTABMAP_STATS(Loop, Landmark_detected,); diff --git a/corelib/include/rtabmap/core/Stereo.h b/corelib/include/rtabmap/core/Stereo.h index 497f8067..ebf1e44a 100644 --- a/corelib/include/rtabmap/core/Stereo.h +++ b/corelib/include/rtabmap/core/Stereo.h @@ -96,16 +96,23 @@ public: #endif float epsilon() const {return epsilon_;} + bool usingMinEigenVals() const {return useMinEigenVals_;} + float minEigThreshold() const {return minEigThreshold_;} + float errorThreshold() const {return errorThreshold_;} virtual bool isGpuEnabled() const; private: void updateStatus( const std::vector & leftCorners, const std::vector & rightCorners, - std::vector & status) const; + std::vector & status, + std::vector err = {}) const; private: float epsilon_; + bool useMinEigenVals_; + float minEigThreshold_; + float errorThreshold_; bool gpu_; }; diff --git a/corelib/include/rtabmap/core/VWDictionary.h b/corelib/include/rtabmap/core/VWDictionary.h index fbfa7ba5..ebfa3a62 100644 --- a/corelib/include/rtabmap/core/VWDictionary.h +++ b/corelib/include/rtabmap/core/VWDictionary.h @@ -89,7 +89,7 @@ public: std::vector findNN(const std::list & vws) const; std::vector findNN(const cv::Mat & descriptors) const; - void addWordRef(int wordId, int signatureId); + bool addWordRef(int wordId, int signatureId); void removeAllWordRef(int wordId, int signatureId); const VisualWord * getWord(int id) const; VisualWord * getUnusedWord(int id) const; @@ -107,7 +107,11 @@ public: bool isIncrementalFlann() const {return _incrementalFlann;} void setIncrementalDictionary(); void setFixedDictionary(const std::string & dictionaryPath); + bool isModified() const; + std::vector serializeIndex() const; + void deserializeIndex(const std::vector & data); + void deserializeIndex(const unsigned char * data, size_t size); void exportDictionary(const char * fileNameReferences, const char * fileNameDescriptors) const; void clear(bool printWarningsIfNotEmpty = true); @@ -137,10 +141,12 @@ private: std::string _dictionaryPath; // a pre-computed dictionary (.txt or .db) std::string _newDictionaryPath; // a pre-computed dictionary (.txt or .db) bool _newWordsComparedTogether; + bool _serializeWithChecksum; int _lastWordId; bool useDistanceL1_; FlannIndex * _flannIndex; cv::Mat _dataTree; + bool _modified; NNStrategy _strategy; std::map _mapIndexId; std::map _mapIdIndex; diff --git a/corelib/include/rtabmap/core/camera/CameraImages.h b/corelib/include/rtabmap/core/camera/CameraImages.h index 9b7134dc..2e234b9a 100644 --- a/corelib/include/rtabmap/core/camera/CameraImages.h +++ b/corelib/include/rtabmap/core/camera/CameraImages.h @@ -94,18 +94,19 @@ public: _depthFromScanFillHolesFromBorder = fillHolesFromBorder; } - // Format: 0=Raw, 1=RGBD-SLAM, 2=KITTI, 3=TORO, 4=g2o, 5=NewCollege(t,x,y), 6=Malaga Urban GPS, 7=St Lucia INS, 8=Karlsruhe + // 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, 12=rgbd_bonn void setOdometryPath(const std::string & filePath, int format = 0) { _odometryPath = filePath; _odometryFormat = format; } - // Format: 0=Raw, 1=RGBD-SLAM, 2=KITTI, 3=TORO, 4=g2o, 5=NewCollege(t,x,y), 6=Malaga Urban GPS, 7=St Lucia INS, 8=Karlsruhe - void setGroundTruthPath(const std::string & filePath, int format = 0) + // 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, 12=rgbd_bonn + void setGroundTruthPath(const std::string & filePath, int format = 0, const Transform & localTransform = Transform::getIdentity()) { _groundTruthPath = filePath; _groundTruthFormat = format; + _groundTruthLocalTransform = localTransform; } void setMaxPoseTimeDiff(double diff) {_maxPoseTimeDiff = diff;} @@ -164,6 +165,7 @@ private: int _odometryFormat; std::string _groundTruthPath; int _groundTruthFormat; + Transform _groundTruthLocalTransform; double _maxPoseTimeDiff; std::list _stamps; diff --git a/corelib/include/rtabmap/core/camera/CameraOrbbecSDK.h b/corelib/include/rtabmap/core/camera/CameraOrbbecSDK.h new file mode 100644 index 00000000..c14cc78f --- /dev/null +++ b/corelib/include/rtabmap/core/camera/CameraOrbbecSDK.h @@ -0,0 +1,105 @@ +/* +Copyright (c) 2010-2025, 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. +*/ + +#pragma once + +#include "rtabmap/core/CameraModel.h" +#include "rtabmap/core/Camera.h" +#include "rtabmap/core/Version.h" + +#ifdef RTABMAP_ORBBEC_SDK +namespace ob +{ + class Pipeline; + class Align; +} +#endif + +namespace rtabmap +{ + +class RTABMAP_CORE_EXPORT CameraOrbbecSDK : + public Camera +{ +public: + static bool available(); + +public: + // deviceId can be either an index (e.g., "0"), an UID (e.g, "2-1-2" or "gmsl-1") or a serial ("AAA6454S") + CameraOrbbecSDK( + std::string deviceId = "", + unsigned int colorWidth = 800, + unsigned int colorHeight = 600, + unsigned int depthWidth = 800, + unsigned int depthHeight = 600, + float imageRate = 0.0f, + const Transform & localTransform = Transform::getIdentity()); + virtual ~CameraOrbbecSDK(); + + virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = ""); + virtual bool isCalibrated() const; + virtual std::string getSerial() const; + + void close(); + + // Should be set before initializing + void enableColorRectification(bool enabled); + void enableImu(bool enabled); + + void enableDepthMM(bool enabled); + +protected: + virtual SensorData captureImage(SensorCaptureInfo * info = 0); + +private: + +#ifdef RTABMAP_ORBBEC_SDK + std::string deviceId_; + unsigned int colorWidth_; + unsigned int colorHeight_; + unsigned int depthWidth_; + unsigned int depthHeight_; + ob::Pipeline * pipeline_; + ob::Pipeline * imuPipeline_; + ob::Align * alignFilter_; + CameraModel model_; + Transform imuLocalTransform_; + bool imuLocalTransformInitialized_; + uint64_t lastAccStamp_; + uint64_t lastImageStamp_; + bool globalTimestampAvailable_; + bool rectifyColor_; + bool convertDepthToMM_; + bool imuPublished_; + std::map imuBuffer_; + UMutex imuMutex_; +#endif + +}; + + +} // namespace rtabmap diff --git a/corelib/include/rtabmap/core/camera/CameraRealSense2.h b/corelib/include/rtabmap/core/camera/CameraRealSense2.h index 47788f0f..10843a26 100644 --- a/corelib/include/rtabmap/core/camera/CameraRealSense2.h +++ b/corelib/include/rtabmap/core/camera/CameraRealSense2.h @@ -146,6 +146,7 @@ private: Transform dualExtrinsics_; std::string jsonConfig_; bool closing_; + bool playback_; static Transform realsense2PoseRotation_; static Transform realsense2PoseRotationInv_; diff --git a/corelib/include/rtabmap/core/global_map/OccupancyGrid.h b/corelib/include/rtabmap/core/global_map/OccupancyGrid.h index 915c3dc1..ddcd3a9c 100644 --- a/corelib/include/rtabmap/core/global_map/OccupancyGrid.h +++ b/corelib/include/rtabmap/core/global_map/OccupancyGrid.h @@ -57,7 +57,6 @@ protected: private: cv::Mat map_; cv::Mat mapInfo_; - std::map > cellCount_; // float minMapSize_; bool erode_; diff --git a/corelib/include/rtabmap/core/impl/util3d.hpp b/corelib/include/rtabmap/core/impl/util3d.hpp index 1e9e23cb..11fd55aa 100644 --- a/corelib/include/rtabmap/core/impl/util3d.hpp +++ b/corelib/include/rtabmap/core/impl/util3d.hpp @@ -41,11 +41,11 @@ LaserScan laserScanFromPointCloud(const PointCloud2T & cloud, bool filterNaNs, b return LaserScan(); } //determine the output type - int fieldStates[8] = {0}; // x,y,z,normal_x,normal_y,normal_z,rgb,intensity + int fieldStates[10] = {0}; // x,y,z,normal_x,normal_y,normal_z,rgb,intensity,time,ring #if PCL_VERSION_COMPARE(>=, 1, 10, 0) - std::uint32_t fieldOffsets[8] = {0}; + std::uint32_t fieldOffsets[10] = {0}; #else - pcl::uint32_t fieldOffsets[8] = {0}; + pcl::uint32_t fieldOffsets[10] = {0}; #endif for(unsigned int i=0; i +#include +#include +#include + +#ifdef RTABMAP_CUVSLAM +#include +#include +#include +#endif + +namespace rtabmap { + +class RTABMAP_CORE_EXPORT OdometryCuVSLAM : public Odometry +{ +public: + OdometryCuVSLAM(const rtabmap::ParametersMap & parameters = rtabmap::ParametersMap()); + virtual ~OdometryCuVSLAM(); + + virtual void reset(const Transform & initialPose = Transform::getIdentity()); + virtual Odometry::Type getType() {return Odometry::kTypeCuVSLAM;} + +private: + virtual Transform computeTransform(SensorData & image, const Transform & guess = Transform(), OdometryInfo * info = 0); + virtual void cleanupCuVSLAMResources(); + +private: +#ifdef RTABMAP_CUVSLAM + CUVSLAM_TrackerHandle cuvslam_handle_; + CUVSLAM_GroundConstraintHandle ground_constraint_handle_; + + std::vector cuvslam_cameras_; + std::vector> intrinsics_; + + // State tracking + bool initialized_; + bool lost_; + bool tracking_; + bool planar_constraints_; + int multicam_mode_; + Transform previous_pose_; + double last_timestamp_; + + // Configuration Thresholds + double velocity_ratio_threshold_high_ = 1.5; // The maximum velocity ratio of guess / estimated velocity needed to detect lost state. + double velocity_ratio_threshold_low_ = 0.5; // The minimum velocity ratio of guess / estimated velocity needed to detect lost state. + double velocity_difference_threshold_ = 0.1; // The maximum velocity difference between the guess and the estimated velocity needed to detect lost state. + double zero_estimated_velocity_threshold_ = 0.00001; // The minimum cuVSLAM estimated velocity needed to detect lost state. + double min_landmarks_threshold_ = 30; // The minimum number of landmarks needed to start tracking after an initialization. + + // Forward cuVLSAM covariance directly to RTAB-Map. + // When true this disables covariance based lost detection. + bool use_raw_covariance_ = false; + + //visualization + std::vector observations_; + std::vector landmarks_; + + // GPU memory management + std::vector gpu_left_image_data_; // pointers to all gpu images + std::vector gpu_right_image_data_; + std::vector gpu_left_image_sizes_; // size of one image + std::vector gpu_right_image_sizes_; + cudaStream_t cuda_stream_; +#endif +}; + +} + +#endif /* ODOMETRYCUVSLAM_H_ */ \ No newline at end of file diff --git a/corelib/include/rtabmap/core/odometry/OdometryLIOSAM.h b/corelib/include/rtabmap/core/odometry/OdometryLIOSAM.h new file mode 100644 index 00000000..87226e52 --- /dev/null +++ b/corelib/include/rtabmap/core/odometry/OdometryLIOSAM.h @@ -0,0 +1,82 @@ +/* +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 ODOMETRYLIOSAM_H_ +#define ODOMETRYLIOSAM_H_ + +#include + +#ifdef RTABMAP_LIOSAM +#include +#include +#include +namespace lio_sam { class LioSamCore; } +#endif + +namespace rtabmap { + +class RTABMAP_CORE_EXPORT OdometryLIOSAM : public Odometry +{ +public: + OdometryLIOSAM(const rtabmap::ParametersMap & parameters = rtabmap::ParametersMap()); + virtual ~OdometryLIOSAM(); + + virtual void reset(const Transform & initialPose = Transform::getIdentity()); + virtual Odometry::Type getType() {return Odometry::kTypeLIOSAM;} + virtual bool canProcessAsyncIMU() const {return true;} + +private: + virtual Transform computeTransform(SensorData & data, const Transform & guess = Transform(), OdometryInfo * info = 0); + +#ifdef RTABMAP_LIOSAM + bool init(const Transform & imuLocalTransform, const Transform & lidarLocalTransform); +#endif + +private: +#ifdef RTABMAP_LIOSAM + lio_sam::LioSamCore * lioSam_; + Transform lastPose_; + bool lost_; + float linVar_; + float angVar_; + ParametersMap parameters_; + Transform imuLocalTransform_; // base_link -> imu_link (cached for deferred init) + + // Buffered IMU samples received before initialization + struct ImuSample { + double stamp; + Eigen::Vector3d acc; + Eigen::Vector3d gyro; + Eigen::Quaterniond orientation; + }; + std::vector imuBuffer_; +#endif +}; + +} + +#endif /* ODOMETRYLIOSAM_H_ */ diff --git a/corelib/include/rtabmap/core/odometry/OdometryVINS.h b/corelib/include/rtabmap/core/odometry/OdometryVINS.h index 030de4a3..bd1a3e64 100644 --- a/corelib/include/rtabmap/core/odometry/OdometryVINS.h +++ b/corelib/include/rtabmap/core/odometry/OdometryVINS.h @@ -25,39 +25,7 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ -#ifndef ODOMETRYVINS_H_ -#define ODOMETRYVINS_H_ +#pragma once +#pragma message("Warning: OdometryVINS.h is deprecated. Please use OdometryVINSFusion.h instead.") -#include - -namespace rtabmap { - -class VinsEstimator; - -class RTABMAP_CORE_EXPORT OdometryVINS : public Odometry -{ -public: - OdometryVINS(const rtabmap::ParametersMap & parameters = rtabmap::ParametersMap()); - virtual ~OdometryVINS(); - - virtual void reset(const Transform & initialPose = Transform::getIdentity()); - virtual Odometry::Type getType() {return Odometry::kTypeVINS;} - virtual bool canProcessRawImages() const {return true;} - virtual bool canProcessAsyncIMU() const {return true;} - -private: - virtual Transform computeTransform(SensorData & image, const Transform & guess = Transform(), OdometryInfo * info = 0); - -private: -#ifdef RTABMAP_VINS - VinsEstimator * vinsEstimator_; - bool initGravity_; - Transform previousPose_; - Transform previousLocalTransform_; - IMU lastImu_; -#endif -}; - -} - -#endif /* ODOMETRYVINS_H_ */ +#include "rtabmap/core/odometry/OdometryVINSFusion.h" diff --git a/corelib/include/rtabmap/core/odometry/OdometryVINSFusion.h b/corelib/include/rtabmap/core/odometry/OdometryVINSFusion.h new file mode 100644 index 00000000..e5059d22 --- /dev/null +++ b/corelib/include/rtabmap/core/odometry/OdometryVINSFusion.h @@ -0,0 +1,64 @@ +/* +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 ODOMETRYVINSFUSION_H_ +#define ODOMETRYVINSFUSION_H_ + +#include + +namespace rtabmap { + +class VinsFusionEstimator; + +class RTABMAP_CORE_EXPORT OdometryVINSFusion : public Odometry +{ +public: + OdometryVINSFusion(const rtabmap::ParametersMap & parameters = rtabmap::ParametersMap()); + virtual ~OdometryVINSFusion(); + + virtual void reset(const Transform & initialPose = Transform::getIdentity()); + virtual Odometry::Type getType() {return Odometry::kTypeVINSFusion;} + virtual bool canProcessRawImages() const {return true;} + virtual bool canProcessAsyncIMU() const {return true;} + +private: + virtual Transform computeTransform(SensorData & image, const Transform & guess = Transform(), OdometryInfo * info = 0); + +private: +#ifdef RTABMAP_VINS_FUSION + VinsFusionEstimator * vinsEstimator_; + bool initGravity_; + Transform previousPose_; + Transform previousLocalTransform_; + IMU lastImu_; + double lastImuStamp_; +#endif +}; + +} + +#endif /* ODOMETRYVINSFUSION_H_ */ diff --git a/corelib/include/rtabmap/core/optimizer/OptimizerG2O.h b/corelib/include/rtabmap/core/optimizer/OptimizerG2O.h index 30117d6e..d8c95279 100644 --- a/corelib/include/rtabmap/core/optimizer/OptimizerG2O.h +++ b/corelib/include/rtabmap/core/optimizer/OptimizerG2O.h @@ -41,6 +41,12 @@ public: static bool isCSparseAvailable(); static bool isCholmodAvailable(); +public: + static bool loadGraph( + const std::string & fileName, + std::map & poses, + std::multimap & edgeConstraints); + public: OptimizerG2O(const ParametersMap & parameters = ParametersMap()); virtual ~OptimizerG2O() {} diff --git a/corelib/include/rtabmap/core/optimizer/OptimizerGTSAM.h b/corelib/include/rtabmap/core/optimizer/OptimizerGTSAM.h index 2d3eb6ba..c2b5a0f3 100644 --- a/corelib/include/rtabmap/core/optimizer/OptimizerGTSAM.h +++ b/corelib/include/rtabmap/core/optimizer/OptimizerGTSAM.h @@ -77,6 +77,7 @@ private: std::vector lastAddedConstraints_; int lastSwitchId_; std::set addedPoses_; + std::map isLandmarkWithRotation_; // persists across iSAM2 incremental calls std::pair lastRootFactorIndex_; }; diff --git a/corelib/include/rtabmap/core/util3d.h b/corelib/include/rtabmap/core/util3d.h index 58cd82d6..72dc44a5 100644 --- a/corelib/include/rtabmap/core/util3d.h +++ b/corelib/include/rtabmap/core/util3d.h @@ -39,12 +39,26 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include +#include #include #include namespace rtabmap { +// Point type carrying xyz + intensity + ring (laser line index) + time +// (per-point acquisition offset, seconds from the scan start). Matches the +// layout expected by LIO-SAM's Velodyne feature extractor so it can be fed +// directly via util3d::laserScanFromPointCloud(). +struct EIGEN_ALIGN16 PointXYZIRT +{ + PCL_ADD_POINT4D; + float intensity; + std::uint16_t ring; + float time; + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; + namespace util3d { @@ -297,6 +311,9 @@ 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,I,ring,time) +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_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); @@ -512,6 +529,15 @@ LaserScan RTABMAP_CORE_EXPORT deskew( } // namespace util3d } // namespace rtabmap +POINT_CLOUD_REGISTER_POINT_STRUCT(rtabmap::PointXYZIRT, + (float, x, x) + (float, y, y) + (float, z, z) + (float, intensity, intensity) + (std::uint16_t, ring, ring) + (float, time, time) +) + #include "rtabmap/core/impl/util3d.hpp" #endif /* UTIL3D_H_ */ diff --git a/corelib/include/rtabmap/core/util3d_features.h b/corelib/include/rtabmap/core/util3d_features.h index 2a02bf2e..2ea95bf7 100644 --- a/corelib/include/rtabmap/core/util3d_features.h +++ b/corelib/include/rtabmap/core/util3d_features.h @@ -80,6 +80,7 @@ std::map RTABMAP_CORE_EXPORT generateWords3DMono( Transform & cameraTransform, float ransacReprojThreshold = 3.0f, float ransacConfidence = 0.99f, + int varianceMedianRatio = 4, const std::map & refGuess3D = std::map(), double * variance = 0, std::vector * matchesOut = 0); diff --git a/corelib/src/CMakeLists.txt b/corelib/src/CMakeLists.txt index b758ad0f..e6bf74c9 100644 --- a/corelib/src/CMakeLists.txt +++ b/corelib/src/CMakeLists.txt @@ -41,6 +41,7 @@ SET(SRC_FILES camera/CameraMyntEye.cpp camera/CameraDepthAI.cpp camera/CameraSeerSense.cpp + camera/CameraOrbbecSDK.cpp EpipolarGeometry.cpp VisualWord.cpp @@ -97,10 +98,12 @@ SET(SRC_FILES odometry/OdometryORBSLAM3.cpp odometry/OdometryLOAM.cpp odometry/OdometryFLOAM.cpp + odometry/OdometryLIOSAM.cpp odometry/OdometryMSCKF.cpp - odometry/OdometryVINS.cpp + odometry/OdometryVINSFusion.cpp odometry/OdometryOpenVINS.cpp odometry/OdometryOpen3D.cpp + odometry/OdometryCuVSLAM.cpp IMU.cpp IMUThread.cpp @@ -163,10 +166,6 @@ IF(MSVC) ENDIF(MSVC) SET(INCLUDE_DIRS - ${CMAKE_CURRENT_SOURCE_DIR} - ${CMAKE_CURRENT_SOURCE_DIR}/../include - ${CMAKE_CURRENT_BINARY_DIR} - ${CMAKE_CURRENT_BINARY_DIR}/include ${ZLIB_INCLUDE_DIRS} ) @@ -217,11 +216,21 @@ IF(TORCH_FOUND) ${SRC_FILES} superpoint_torch/SuperPoint.cc ) - SET(INCLUDE_DIRS + SET(INCLUDE_DIRS ${TORCH_INCLUDE_DIRS} ${CMAKE_CURRENT_SOURCE_DIR}/superpoint_torch ${INCLUDE_DIRS} ) + IF(WITH_PYTHON AND Python3_FOUND) + SET(SRC_FILES + ${SRC_FILES} + superpoint_rpautrat/SuperpointRpautrat.cpp + ) + SET(INCLUDE_DIRS + ${CMAKE_CURRENT_SOURCE_DIR}/superpoint_rpautrat + ${INCLUDE_DIRS} + ) + ENDIF(WITH_PYTHON AND Python3_FOUND) ENDIF(TORCH_FOUND) IF(WITH_PYTHON AND Python3_FOUND) @@ -394,6 +403,13 @@ IF(xvsdk_FOUND) ) ENDIF(xvsdk_FOUND) +IF(OrbbecSDK_FOUND) + SET(LIBRARIES + ${LIBRARIES} + ob::OrbbecSDK + ) +ENDIF(OrbbecSDK_FOUND) + IF(TARGET OpenMP::OpenMP_CXX) SET(LIBRARIES ${LIBRARIES} @@ -517,6 +533,13 @@ IF(FastCV_FOUND) ) ENDIF(FastCV_FOUND) +IF(apriltag_FOUND) + SET(LIBRARIES + apriltag::apriltag + ${LIBRARIES} + ) +ENDIF(apriltag_FOUND) + IF(opengv_FOUND) SET(LIBRARIES ${LIBRARIES} @@ -589,6 +612,18 @@ IF(floam_FOUND) ) ENDIF(floam_FOUND) +IF(lio_sam_FOUND) + SET(INCLUDE_DIRS + ${INCLUDE_DIRS} + ${lio_sam_INCLUDE_DIRS} + ) + link_directories(${lio_sam_LIBRARY_DIRS}) + SET(LIBRARIES + ${LIBRARIES} + lio_sam_core + ) +ENDIF(lio_sam_FOUND) + IF(ZED_FOUND) SET(INCLUDE_DIRS ${INCLUDE_DIRS} @@ -746,16 +781,16 @@ IF(vins_FOUND) ) ENDIF(vins_FOUND) -IF(ov_msckf_FOUND) +IF(OpenVINS_FOUND) SET(INCLUDE_DIRS - ${ov_msckf_INCLUDE_DIRS} + ${OpenVINS_INCLUDE_DIRS} ${INCLUDE_DIRS} ) SET(LIBRARIES - ${ov_msckf_LIBRARIES} + ${OpenVINS_LIBRARIES} ${LIBRARIES} ) -ENDIF(ov_msckf_FOUND) +ENDIF(OpenVINS_FOUND) IF(ORB_SLAM_FOUND) SET(INCLUDE_DIRS @@ -768,38 +803,18 @@ IF(ORB_SLAM_FOUND) ) ENDIF(ORB_SLAM_FOUND) -IF(GTSAM_FOUND) - # Make sure GTSAM is built with system Eigen, not the included one in its package - IF(GTSAM_INCLUDE_DIR) - SET(INCLUDE_DIRS - ${INCLUDE_DIRS} - ${GTSAM_INCLUDE_DIR} - ) - ELSE() - SET(INCLUDE_DIRS - ${INCLUDE_DIRS} - ${GTSAM_INCLUDE_DIRS} - ) - ENDIF() - SET(SRC_FILES - ${SRC_FILES} - optimizer/gtsam/GravityFactor.cpp +IF(CUVSLAM_FOUND) + SET(LIBRARIES + ${LIBRARIES} + cuvslam::cuvslam ) - IF(WIN32) - # GTSAM should be built in STATIC on Windows to avoid "error C2338: THIS_METHOD_IS_ONLY_FOR_1x1_EXPRESSIONS" when building GTSAM - add_definitions("-DGTSAM_IMPORT_STATIC") - ENDIF(WIN32) +ENDIF(CUVSLAM_FOUND) + +IF(GTSAM_FOUND) SET(LIBRARIES ${LIBRARIES} - gtsam # Windows: Place static libs at the end + gtsam ) - IF(WIN32) - #explicitly add metis target on windows (after gtsam target) - SET(LIBRARIES - ${LIBRARIES} - metis - ) - ENDIF(WIN32) ENDIF(GTSAM_FOUND) IF(WITH_MADGWICK) @@ -816,6 +831,7 @@ CONFIGURE_FILE(${CMAKE_CURRENT_SOURCE_DIR}/resources/DatabaseSchema.sql.in ${CMA SET(RESOURCES ${CMAKE_CURRENT_SOURCE_DIR}/resources/DatabaseSchema.sql + ${CMAKE_CURRENT_SOURCE_DIR}/resources/backward_compatibility/DatabaseSchema_0_22_0.sql ${CMAKE_CURRENT_SOURCE_DIR}/resources/backward_compatibility/DatabaseSchema_0_20_0.sql ${CMAKE_CURRENT_SOURCE_DIR}/resources/backward_compatibility/DatabaseSchema_0_18_3.sql ${CMAKE_CURRENT_SOURCE_DIR}/resources/backward_compatibility/DatabaseSchema_0_18_0.sql @@ -825,6 +841,13 @@ SET(RESOURCES ${CMAKE_CURRENT_SOURCE_DIR}/resources/backward_compatibility/DatabaseSchema_0_16_0.sql ) +IF(TORCH_FOUND AND WITH_PYTHON AND Python3_FOUND) + SET(RESOURCES + ${RESOURCES} + ${CMAKE_CURRENT_SOURCE_DIR}/superpoint_rpautrat/superpoint_to_torchscript.py + ) +ENDIF() + foreach(arg ${RESOURCES}) get_filename_component(filename ${arg} NAME) string(REPLACE "." "_" output ${filename}) @@ -857,8 +880,12 @@ generate_export_header(rtabmap_core DEPRECATED_MACRO_NAME RTABMAP_DEPRECATED) target_include_directories(rtabmap_core PUBLIC - "$" - "$") + "$" + "$") + +target_include_directories(rtabmap_core SYSTEM PUBLIC + "$" + "$") TARGET_LINK_LIBRARIES(rtabmap_core PUBLIC diff --git a/corelib/src/Camera.cpp b/corelib/src/Camera.cpp index b9503c85..b95b0e93 100644 --- a/corelib/src/Camera.cpp +++ b/corelib/src/Camera.cpp @@ -39,7 +39,8 @@ namespace rtabmap Camera::Camera(float imageRate, const Transform & localTransform) : SensorCapture(imageRate, localTransform*CameraModel::opticalRotation()), imuFilter_(0), - publishInterIMU_(false) + publishInterIMU_(false), + imuBaseFrameConversion_(false) {} Camera::~Camera() @@ -52,15 +53,23 @@ bool Camera::initFromFile(const std::string & calibrationPath) return init(UDirectory::getDir(calibrationPath), uSplit(UFile::getName(calibrationPath), '.').front()); } -void Camera::setInterIMUPublishing(bool enabled, IMUFilter * filter) +void Camera::setInterIMUPublishing(bool enabled, IMUFilter * filter, bool baseFrameConversion) { publishInterIMU_ = enabled; delete imuFilter_; imuFilter_ = filter; + imuBaseFrameConversion_ = baseFrameConversion; } -void Camera::postInterIMU(const IMU & imu, double stamp) +void Camera::postInterIMU(const IMU & imu_in, double stamp) { + IMU imu = imu_in; + if(imuBaseFrameConversion_) + { + UASSERT(!imu.localTransform().isNull()); + imu.convertToBaseFrame(); + } + if(imuFilter_) { imuFilter_->update( diff --git a/corelib/src/CameraModel.cpp b/corelib/src/CameraModel.cpp index 3e80d25a..04b77329 100644 --- a/corelib/src/CameraModel.cpp +++ b/corelib/src/CameraModel.cpp @@ -353,6 +353,22 @@ bool CameraModel::load(const std::string & filePath) data[0], data[1], data[2], data[3], data[4], data[5], data[6], data[7], data[8], data[9], data[10], data[11]); + Transform detCheck = localTransform_.clone(); + localTransform_.normalizeRotation(); /// Normalize by default + float det = detCheck.toEigen3f().linear().determinant(); + if(fabs(det - 1.0f) > 0.0001) + { + std::stringstream streamBefore, streamAfter; + streamBefore << detCheck << std::endl; + streamAfter << localTransform_ << std::endl; + UWARN("The camera model's local_transform from \"%s\" doesn't " + "have a normalized rotation matrix (dertminant=%f). We will normalize " + "it for convenience.\nWas:\n%sNow\n%s", + filePath.c_str(), + det, + streamBefore.str().c_str(), + streamAfter.str().c_str()); + } } else { @@ -554,7 +570,7 @@ unsigned int CameraModel::deserialize(const unsigned char * data, unsigned int d int iR = 8; int iP = 9; int iL = 10; - UDEBUG("Header: %d %d %d %d %d %d %d %d %d %d %d", header[0],header[1],header[2],header[3],header[4],header[5],header[6],header[7],header[8],header[9],header[10]); + //UDEBUG("Header: %d %d %d %d %d %d %d %d %d %d %d", header[0],header[1],header[2],header[3],header[4],header[5],header[6],header[7],header[8],header[9],header[10]); unsigned int requiredDataSize = sizeof(int)*headerSize + sizeof(double)*(header[iK]+header[iD]+header[iR]+header[iP]) + sizeof(float)*header[iL]; diff --git a/corelib/src/DBDriver.cpp b/corelib/src/DBDriver.cpp index 2c381164..d71c1400 100644 --- a/corelib/src/DBDriver.cpp +++ b/corelib/src/DBDriver.cpp @@ -45,7 +45,7 @@ DBDriver * DBDriver::create(const ParametersMap & parameters) DBDriver::DBDriver(const ParametersMap & parameters) : _emptyTrashesTime(0), - _timestampUpdate(true) + _timestampUpdate(false) { this->parseParameters(parameters); } @@ -73,7 +73,15 @@ void DBDriver::closeConnection(bool save, const std::string & outputUrl) else { _trashesMutex.lock(); + for(auto & iter: _trashSignatures) + { + delete iter.second; + } _trashSignatures.clear(); + for(auto & iter: _trashVisualWords) + { + delete iter.second; + } _trashVisualWords.clear(); _trashesMutex.unlock(); } @@ -83,12 +91,12 @@ void DBDriver::closeConnection(bool save, const std::string & outputUrl) UDEBUG(""); } -bool DBDriver::openConnection(const std::string & url, bool overwritten) +bool DBDriver::openConnection(const std::string & url, bool overwritten, bool readOnly) { UDEBUG(""); _url = url; _dbSafeAccessMutex.lock(); - if(this->connectDatabaseQuery(url, overwritten)) + if(this->connectDatabaseQuery(url, overwritten, readOnly)) { _dbSafeAccessMutex.unlock(); return true; @@ -383,7 +391,7 @@ void DBDriver::asyncSave(Signature * s) { if(s) { - UDEBUG("s=%d", s->id()); + //UDEBUG("s=%d", s->id()); _trashesMutex.lock(); { _trashSignatures.insert(std::pair(s->id(), s)); @@ -531,17 +539,17 @@ void DBDriver::updateLaserScan(int nodeId, const LaserScan & scan) _dbSafeAccessMutex.unlock(); } -void DBDriver::load(VWDictionary * dictionary, bool lastStateOnly) const +void DBDriver::load(VWDictionary & dictionary, bool lastStateOnly) const { _dbSafeAccessMutex.lock(); this->loadQuery(dictionary, lastStateOnly); _dbSafeAccessMutex.unlock(); } -void DBDriver::loadLastNodes(std::list & signatures) const +void DBDriver::loadLastNodes(std::list & signatures, bool loadWordIdsOnly) const { _dbSafeAccessMutex.lock(); - this->loadLastNodesQuery(signatures); + this->loadLastNodesQuery(signatures, loadWordIdsOnly); _dbSafeAccessMutex.unlock(); } @@ -564,7 +572,8 @@ Signature * DBDriver::loadSignature(int id, bool * loadedFromTrash) } void DBDriver::loadSignatures(const std::list & signIds, std::list & signatures, - std::set * loadedFromTrash) + std::set * loadedFromTrash, + bool loadWordIdsOnly) { UDEBUG(""); // look up in the trash before the database @@ -609,7 +618,7 @@ void DBDriver::loadSignatures(const std::list & signIds, if(ids.size()) { _dbSafeAccessMutex.lock(); - this->loadSignaturesQuery(ids, signatures); + this->loadSignaturesQuery(ids, signatures, loadWordIdsOnly); _dbSafeAccessMutex.unlock(); } } @@ -656,10 +665,10 @@ void DBDriver::loadWords(const std::set & wordIds, std::list } } -void DBDriver::loadNodeData(Signature * signature, bool images, bool scan, bool userData, bool occupancyGrid) const +void DBDriver::loadNodeData(Signature & signature, bool images, bool scan, bool userData, bool occupancyGrid) const { std::list signatures; - signatures.push_back(signature); + signatures.push_back(&signature); this->loadNodeData(signatures, images, scan, userData, occupancyGrid); } @@ -823,6 +832,45 @@ bool DBDriver::getNodeInfo( return found; } +void DBDriver::getLocalFeatures( + int signatureId, + std::multimap & words, + std::vector & keypoints, + std::vector & points, + cv::Mat & descriptors) const +{ + bool found = false; + // look in the trash + _trashesMutex.lock(); + if(uContains(_trashSignatures, signatureId)) + { + const Signature * s = _trashSignatures.at(signatureId); + UASSERT(s != 0); + found = true; + if(!s->getWords().empty()) + { + words = s->getWords(); + if(s->getWordsKpts().empty()){ + found = false; // Force checking the database in case the local features were not loaded in RAM + } + else + { + words = s->getWords(); + keypoints = s->getWordsKpts(); + points = s->getWords3(); + descriptors = s->getWordsDescriptors().clone(); + } + } + } + _trashesMutex.unlock(); + + if(!found) + { + UScopeMutex lock(_dbSafeAccessMutex); + getLocalFeaturesQuery(signatureId, words, keypoints, points, descriptors); + } +} + void DBDriver::loadLinks(int signatureId, std::multimap & links, Link::Type type) const { bool found = false; @@ -1287,6 +1335,13 @@ cv::Mat DBDriver::loadOptimizedMesh( return cloud; } +void DBDriver::saveFlannIndex(const std::vector & indexData) const +{ + _dbSafeAccessMutex.lock(); + saveFlannIndexQuery(indexData); + _dbSafeAccessMutex.unlock(); +} + void DBDriver::generateGraph( const std::string & fileName, const std::set & idsInput, diff --git a/corelib/src/DBDriverSqlite3.cpp b/corelib/src/DBDriverSqlite3.cpp index 85a02637..66986051 100644 --- a/corelib/src/DBDriverSqlite3.cpp +++ b/corelib/src/DBDriverSqlite3.cpp @@ -34,6 +34,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "rtabmap/core/util3d.h" #include "rtabmap/core/Compression.h" #include "DatabaseSchema_sql.h" +#include "DatabaseSchema_0_22_0_sql.h" #include "DatabaseSchema_0_20_0_sql.h" #include "DatabaseSchema_0_18_3_sql.h" #include "DatabaseSchema_0_18_0_sql.h" @@ -319,7 +320,7 @@ bool DBDriverSqlite3::getDatabaseVersionQuery(std::string & version) const return false; } -bool DBDriverSqlite3::connectDatabaseQuery(const std::string & url, bool overwritten) +bool DBDriverSqlite3::connectDatabaseQuery(const std::string & url, bool overwritten, bool readOnly) { this->disconnectDatabaseQuery(); // Open a database connection @@ -331,7 +332,7 @@ bool DBDriverSqlite3::connectDatabaseQuery(const std::string & url, bool overwri if(!url.empty()) { dbFileExist = UFile::exists(url.c_str()); - if(dbFileExist && overwritten) + if(dbFileExist && overwritten && !readOnly) { UINFO("Deleting database %s...", url.c_str()); UASSERT(UFile::erase(url.c_str()) == 0); @@ -353,12 +354,12 @@ bool DBDriverSqlite3::connectDatabaseQuery(const std::string & url, bool overwri { ULOGGER_INFO("Using empty database in the memory."); } - rc = sqlite3_open_v2(":memory:", &_ppDb, SQLITE_OPEN_READWRITE | SQLITE_OPEN_CREATE, 0); + rc = sqlite3_open_v2(":memory:", &_ppDb, readOnly ? SQLITE_OPEN_READONLY : SQLITE_OPEN_READWRITE | SQLITE_OPEN_CREATE, 0); } else { ULOGGER_INFO("Using database \"%s\" from the hard drive.", url.c_str()); - rc = sqlite3_open_v2(url.c_str(), &_ppDb, SQLITE_OPEN_READWRITE | SQLITE_OPEN_CREATE, 0); + rc = sqlite3_open_v2(url.c_str(), &_ppDb, readOnly ? SQLITE_OPEN_READONLY : SQLITE_OPEN_READWRITE | SQLITE_OPEN_CREATE, 0); } if(rc != SQLITE_OK) { @@ -404,6 +405,7 @@ bool DBDriverSqlite3::connectDatabaseQuery(const std::string & url, bool overwri schemas.push_back(std::make_pair("0.18.0", DATABASESCHEMA_0_18_0_SQL)); schemas.push_back(std::make_pair("0.18.3", DATABASESCHEMA_0_18_3_SQL)); schemas.push_back(std::make_pair("0.20.0", DATABASESCHEMA_0_20_0_SQL)); + schemas.push_back(std::make_pair("0.22.0", DATABASESCHEMA_0_22_0_SQL)); schemas.push_back(std::make_pair(uNumber2Str(RTABMAP_VERSION_MAJOR)+"."+uNumber2Str(RTABMAP_VERSION_MINOR), DATABASESCHEMA_SQL)); for(size_t i=0; i > DBDriverSqlite3::getAllStatisticsWmStatesQuery( void DBDriverSqlite3::loadNodeDataQuery(std::list & signatures, bool images, bool scan, bool userData, bool occupancyGrid) const { - UDEBUG("load data for %d signatures images=%d scan=%d userData=%d, grid=%d", - (int)signatures.size(), images?1:0, scan?1:0, userData?1:0, occupancyGrid?1:0); + //UDEBUG("load data for %d signatures images=%d scan=%d userData=%d, grid=%d", + // (int)signatures.size(), images?1:0, scan?1:0, userData?1:0, occupancyGrid?1:0); if(!images && !scan && !userData && !occupancyGrid) { @@ -1445,7 +1447,7 @@ void DBDriverSqlite3::loadNodeDataQuery(std::list & signatures, boo { UASSERT(*iter != 0); - ULOGGER_DEBUG("Loading data for %d...", (*iter)->id()); + //ULOGGER_DEBUG("Loading data for %d...", (*iter)->id()); // bind id rc = sqlite3_bind_int(ppStmt, 1, (*iter)->id()); UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str()); @@ -1874,7 +1876,7 @@ void DBDriverSqlite3::loadNodeDataQuery(std::list & signatures, boo // Finalize (delete) the statement rc = sqlite3_finalize(ppStmt); UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str()); - ULOGGER_DEBUG("Time=%fs", timer.ticks()); + //ULOGGER_DEBUG("Time=%fs", timer.ticks()); } } @@ -2391,6 +2393,23 @@ bool DBDriverSqlite3::getNodeInfoQuery(int signatureId, return found; } +void DBDriverSqlite3::getLocalFeaturesQuery( + int signatureId, + std::multimap & words, + std::vector & keypoints, + std::vector & points, + cv::Mat & descriptors) const +{ + Signature s(signatureId); + std::list ids; + ids.push_back(&s); + this->loadWordsQuery(ids); + words = ids.front()->getWords(); + keypoints = ids.front()->getWordsKpts(); + points = ids.front()->getWords3(); + descriptors = ids.front()->getWordsDescriptors().clone(); +} + void DBDriverSqlite3::getLastNodeIdsQuery(std::set & ids) const { if(_ppDb) @@ -2987,7 +3006,7 @@ void DBDriverSqlite3::getWeightQuery(int nodeId, int & weight) const } //may be slower than the previous version but don't have a limit of words that can be loaded at the same time -void DBDriverSqlite3::loadSignaturesQuery(const std::list & ids, std::list & nodes) const +void DBDriverSqlite3::loadSignaturesQuery(const std::list & ids, std::list & nodes, bool loadWordIdsOnly) const { ULOGGER_DEBUG("count=%d", (int)ids.size()); if(_ppDb && ids.size()) @@ -3151,7 +3170,7 @@ void DBDriverSqlite3::loadSignaturesQuery(const std::list & ids, std::list< // create the node if(id) { - ULOGGER_DEBUG("Creating %d (map=%d, pose=%s)", *iter, mapId, pose.prettyPrint().c_str()); + //ULOGGER_DEBUG("Creating %d (map=%d, pose=%s)", *iter, mapId, pose.prettyPrint().c_str()); Signature * s = new Signature( id, mapId, @@ -3190,175 +3209,17 @@ void DBDriverSqlite3::loadSignaturesQuery(const std::list & ids, std::list< ULOGGER_DEBUG("Time=%fs", timer.ticks()); // Prepare the query... Get the map from signature and visual words - std::stringstream query2; - if(uStrNumCmp(_version, "0.13.0") >= 0) - { - query2 << "SELECT word_id, pos_x, pos_y, size, dir, response, octave, depth_x, depth_y, depth_z, descriptor_size, descriptor " - "FROM Feature " - "WHERE node_id = ? "; + UDEBUG("Loading local features (ids only=%s)....", loadWordIdsOnly?"true":"false"); + if(loadWordIdsOnly) { + this->loadWordIdsQuery(nodes); } - else if(uStrNumCmp(_version, "0.12.0") >= 0) - { - query2 << "SELECT word_id, pos_x, pos_y, size, dir, response, octave, depth_x, depth_y, depth_z, descriptor_size, descriptor " - "FROM Map_Node_Word " - "WHERE node_id = ? "; + else { + this->loadWordsQuery(nodes); } - else if(uStrNumCmp(_version, "0.11.2") >= 0) - { - query2 << "SELECT word_id, pos_x, pos_y, size, dir, response, depth_x, depth_y, depth_z, descriptor_size, descriptor " - "FROM Map_Node_Word " - "WHERE node_id = ? "; - } - else - { - query2 << "SELECT word_id, pos_x, pos_y, size, dir, response, depth_x, depth_y, depth_z " - "FROM Map_Node_Word " - "WHERE node_id = ? "; - } - - query2 << " ORDER BY word_id"; // Needed for fast insertion below - query2 << ";"; - - rc = sqlite3_prepare_v2(_ppDb, query2.str().c_str(), -1, &ppStmt, 0); - UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str()); - - float nanFloat = std::numeric_limits::quiet_NaN (); - - for(std::list::const_iterator iter=nodes.begin(); iter!=nodes.end(); ++iter) - { - //ULOGGER_DEBUG("Loading words of %d...", (*iter)->id()); - // bind id - rc = sqlite3_bind_int(ppStmt, 1, (*iter)->id()); - UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str()); - - int visualWordId = 0; - int descriptorSize = 0; - const void * descriptor = 0; - int dRealSize = 0; - cv::KeyPoint kpt; - std::multimap visualWords; - std::vector visualWordsKpts; - std::vector visualWords3; - cv::Mat descriptors; - bool allWords3NaN = true; - cv::Point3f depth(0,0,0); - - // Process the result if one - rc = sqlite3_step(ppStmt); - while(rc == SQLITE_ROW) - { - int index = 0; - visualWordId = sqlite3_column_int(ppStmt, index++); - kpt.pt.x = sqlite3_column_double(ppStmt, index++); - kpt.pt.y = sqlite3_column_double(ppStmt, index++); - kpt.size = sqlite3_column_int(ppStmt, index++); - kpt.angle = sqlite3_column_double(ppStmt, index++); - kpt.response = sqlite3_column_double(ppStmt, index++); - if(uStrNumCmp(_version, "0.12.0") >= 0) - { - kpt.octave = sqlite3_column_int(ppStmt, index++); - } - - if(sqlite3_column_type(ppStmt, index) == SQLITE_NULL) - { - depth.x = nanFloat; - ++index; - } - else - { - depth.x = sqlite3_column_double(ppStmt, index++); - } - - if(sqlite3_column_type(ppStmt, index) == SQLITE_NULL) - { - depth.y = nanFloat; - ++index; - } - else - { - depth.y = sqlite3_column_double(ppStmt, index++); - } - - if(sqlite3_column_type(ppStmt, index) == SQLITE_NULL) - { - depth.z = nanFloat; - ++index; - } - else - { - depth.z = sqlite3_column_double(ppStmt, index++); - } - - visualWordsKpts.push_back(kpt); - visualWords.insert(visualWords.end(), std::make_pair(visualWordId, visualWordsKpts.size()-1)); - visualWords3.push_back(depth); - - if(allWords3NaN && util3d::isFinite(depth)) - { - allWords3NaN = false; - } - - if(uStrNumCmp(_version, "0.11.2") >= 0) - { - descriptorSize = sqlite3_column_int(ppStmt, index++); // VisualWord descriptor size - descriptor = sqlite3_column_blob(ppStmt, index); // VisualWord descriptor array - dRealSize = sqlite3_column_bytes(ppStmt, index++); - - if(descriptor && descriptorSize>0 && dRealSize>0) - { - cv::Mat d; - if(dRealSize == descriptorSize) - { - // CV_8U binary descriptors - d = cv::Mat(1, descriptorSize, CV_8U); - } - else if(dRealSize/int(sizeof(float)) == descriptorSize) - { - // CV_32F - d = cv::Mat(1, descriptorSize, CV_32F); - } - else - { - UFATAL("Saved buffer size (%d bytes) is not the same as descriptor size (%d)", dRealSize, descriptorSize); - } - - memcpy(d.data, descriptor, dRealSize); - - descriptors.push_back(d); - } - } - - rc = sqlite3_step(ppStmt); - } - UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str()); - - if(visualWords.size()==0) - { - UDEBUG("Empty signature detected! (id=%d)", (*iter)->id()); - } - else - { - if(allWords3NaN) - { - visualWords3.clear(); - } - (*iter)->setWords(visualWords, visualWordsKpts, visualWords3, descriptors); - ULOGGER_DEBUG("Add %d keypoints, %d 3d points and %d descriptors to node %d", (int)visualWords.size(), allWords3NaN?0:(int)visualWords3.size(), (int)descriptors.rows, (*iter)->id()); - } - - //reset - rc = sqlite3_reset(ppStmt); - UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str()); - } - - // Finalize (delete) the statement - rc = sqlite3_finalize(ppStmt); - UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str()); - - ULOGGER_DEBUG("Time=%fs", timer.ticks()); + UDEBUG("Loading local features.... done! (in %f s)", timer.ticks()); this->loadLinksQuery(nodes); - ULOGGER_DEBUG("Time load links=%fs", timer.ticks()); + ULOGGER_DEBUG("Time loading links=%fs", timer.ticks()); for(std::list::iterator iter = nodes.begin(); iter!=nodes.end(); ++iter) { @@ -3626,7 +3487,7 @@ void DBDriverSqlite3::loadSignaturesQuery(const std::list & ids, std::list< } } -void DBDriverSqlite3::loadLastNodesQuery(std::list & nodes) const +void DBDriverSqlite3::loadLastNodesQuery(std::list & nodes, bool loadWordIdsOnly) const { ULOGGER_DEBUG(""); if(_ppDb) @@ -3672,15 +3533,15 @@ void DBDriverSqlite3::loadLastNodesQuery(std::list & nodes) const UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str()); ULOGGER_DEBUG("Loading %d signatures...", ids.size()); - this->loadSignaturesQuery(ids, nodes); + this->loadSignaturesQuery(ids, nodes, loadWordIdsOnly); ULOGGER_DEBUG("loaded=%d, Time=%fs", nodes.size(), timer.ticks()); } } -void DBDriverSqlite3::loadQuery(VWDictionary * dictionary, bool lastStateOnly) const +void DBDriverSqlite3::loadQuery(VWDictionary & dictionary, bool lastStateOnly) const { ULOGGER_DEBUG(""); - if(_ppDb && dictionary) + if(_ppDb) { std::string type; UTimer timer; @@ -3743,11 +3604,11 @@ void DBDriverSqlite3::loadQuery(VWDictionary * dictionary, bool lastStateOnly) c memcpy(d.data, descriptor, dRealSize); VisualWord * vw = new VisualWord(id, d); vw->setSaved(true); - dictionary->addWord(vw); + dictionary.addWord(vw); if(++count % 5000 == 0) { - ULOGGER_DEBUG("Loaded %d words...", count); + //ULOGGER_DEBUG("Loaded %d words...", count); } rc = sqlite3_step(ppStmt); // next result... } @@ -3758,9 +3619,50 @@ void DBDriverSqlite3::loadQuery(VWDictionary * dictionary, bool lastStateOnly) c // Get Last word id getLastWordId(id); - dictionary->setLastWordId(id); + dictionary.setLastWordId(id); - ULOGGER_DEBUG("Time=%fs", timer.ticks()); + if(uStrNumCmp(_version, "0.23.0") >= 0) { + // load dictionary index + std::stringstream query3; + query3 << "SELECT dictionary_index " + << "FROM Admin " + << "WHERE version='" << _version.c_str() + <<"';"; + + rc = sqlite3_prepare_v2(_ppDb, query3.str().c_str(), -1, &ppStmt, 0); + UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str()); + + // Process the result if one + rc = sqlite3_step(ppStmt); + UASSERT_MSG(rc == SQLITE_ROW, uFormat("DB error (%s): Not found first Admin row: query=\"%s\"", _version.c_str(), query3.str().c_str()).c_str()); + if(rc == SQLITE_ROW) + { + const void * data = 0; + int dataSize = 0; + int index = 0; + + //opt_poses + data = sqlite3_column_blob(ppStmt, index); + dataSize = sqlite3_column_bytes(ppStmt, index++); + if(dataSize>4 && data) + { + UDEBUG("A flann index was saved in the database (size=%ld).", dataSize); + dictionary.deserializeIndex((const unsigned char*)data, dataSize); + } + else { + UDEBUG("No flann index was saved in the database."); + } + + rc = sqlite3_step(ppStmt); // next result... + } + UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str()); + + // Finalize (delete) the statement + rc = sqlite3_finalize(ppStmt); + UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str()); + } + + ULOGGER_DEBUG("Loaded %d words... time=%fs", count, timer.ticks()); } } @@ -3860,6 +3762,262 @@ void DBDriverSqlite3::loadWordsQuery(const std::set & wordIds, std::list & signatures) const +{ + if(_ppDb) + { + int rc = SQLITE_OK; + sqlite3_stmt * ppStmt = 0; + std::stringstream query; + + if(uStrNumCmp(_version, "0.13.0") >= 0) + { + query << "SELECT word_id " + "FROM Feature " + "WHERE node_id = ? "; + } + else if(uStrNumCmp(_version, "0.12.0") >= 0) + { + query << "SELECT word_id " + "FROM Map_Node_Word " + "WHERE node_id = ? "; + } + else if(uStrNumCmp(_version, "0.11.2") >= 0) + { + query << "SELECT word_id " + "FROM Map_Node_Word " + "WHERE node_id = ? "; + } + else + { + query << "SELECT word_id " + "FROM Map_Node_Word " + "WHERE node_id = ? "; + } + + query << " ORDER BY word_id"; // Needed for fast insertion below + query << ";"; + + rc = sqlite3_prepare_v2(_ppDb, query.str().c_str(), -1, &ppStmt, 0); + UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str()); + + for(std::list::const_iterator iter=signatures.begin(); iter!=signatures.end(); ++iter) + { + //ULOGGER_DEBUG("Loading words of %d...", (*iter)->id()); + // bind id + rc = sqlite3_bind_int(ppStmt, 1, (*iter)->id()); + UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str()); + + int visualWordId = 0; + std::multimap visualWords; + + // Process the result if one + rc = sqlite3_step(ppStmt); + while(rc == SQLITE_ROW) + { + int index = 0; + visualWordId = sqlite3_column_int(ppStmt, index++); + visualWords.insert(visualWords.end(), std::make_pair(visualWordId, -1)); + + rc = sqlite3_step(ppStmt); + } + UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str()); + + if(visualWords.size()==0) + { + UDEBUG("Empty signature detected! (id=%d)", (*iter)->id()); + } + else + { + (*iter)->setWords(visualWords, std::vector(), std::vector(), cv::Mat()); + //ULOGGER_DEBUG("Add %d keypoints, %d 3d points and %d descriptors to node %d", (int)visualWords.size(), allWords3NaN?0:(int)visualWords3.size(), (int)descriptors.rows, (*iter)->id()); + } + + //reset + rc = sqlite3_reset(ppStmt); + UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str()); + } + + // Finalize (delete) the statement + rc = sqlite3_finalize(ppStmt); + UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str()); + } +} + +void DBDriverSqlite3::loadWordsQuery(std::list & signatures) const +{ + if(_ppDb) + { + int rc = SQLITE_OK; + sqlite3_stmt * ppStmt = 0; + std::stringstream query; + + if(uStrNumCmp(_version, "0.13.0") >= 0) + { + query << "SELECT word_id, pos_x, pos_y, size, dir, response, octave, depth_x, depth_y, depth_z, descriptor_size, descriptor " + "FROM Feature " + "WHERE node_id = ? "; + } + else if(uStrNumCmp(_version, "0.12.0") >= 0) + { + query << "SELECT word_id, pos_x, pos_y, size, dir, response, octave, depth_x, depth_y, depth_z, descriptor_size, descriptor " + "FROM Map_Node_Word " + "WHERE node_id = ? "; + } + else if(uStrNumCmp(_version, "0.11.2") >= 0) + { + query << "SELECT word_id, pos_x, pos_y, size, dir, response, depth_x, depth_y, depth_z, descriptor_size, descriptor " + "FROM Map_Node_Word " + "WHERE node_id = ? "; + } + else + { + query << "SELECT word_id, pos_x, pos_y, size, dir, response, depth_x, depth_y, depth_z " + "FROM Map_Node_Word " + "WHERE node_id = ? "; + } + + query << " ORDER BY word_id"; // Needed for fast insertion below + query << ";"; + + rc = sqlite3_prepare_v2(_ppDb, query.str().c_str(), -1, &ppStmt, 0); + UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str()); + + float nanFloat = std::numeric_limits::quiet_NaN (); + + for(std::list::const_iterator iter=signatures.begin(); iter!=signatures.end(); ++iter) + { + //ULOGGER_DEBUG("Loading words of %d...", (*iter)->id()); + // bind id + rc = sqlite3_bind_int(ppStmt, 1, (*iter)->id()); + UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str()); + + int visualWordId = 0; + int descriptorSize = 0; + const void * descriptor = 0; + int dRealSize = 0; + cv::KeyPoint kpt; + std::multimap visualWords; + std::vector visualWordsKpts; + std::vector visualWords3; + cv::Mat descriptors; + bool allWords3NaN = true; + cv::Point3f depth(0,0,0); + + // Process the result if one + rc = sqlite3_step(ppStmt); + while(rc == SQLITE_ROW) + { + int index = 0; + visualWordId = sqlite3_column_int(ppStmt, index++); + kpt.pt.x = sqlite3_column_double(ppStmt, index++); + kpt.pt.y = sqlite3_column_double(ppStmt, index++); + kpt.size = sqlite3_column_int(ppStmt, index++); + kpt.angle = sqlite3_column_double(ppStmt, index++); + kpt.response = sqlite3_column_double(ppStmt, index++); + if(uStrNumCmp(_version, "0.12.0") >= 0) + { + kpt.octave = sqlite3_column_int(ppStmt, index++); + } + + if(sqlite3_column_type(ppStmt, index) == SQLITE_NULL) + { + depth.x = nanFloat; + ++index; + } + else + { + depth.x = sqlite3_column_double(ppStmt, index++); + } + + if(sqlite3_column_type(ppStmt, index) == SQLITE_NULL) + { + depth.y = nanFloat; + ++index; + } + else + { + depth.y = sqlite3_column_double(ppStmt, index++); + } + + if(sqlite3_column_type(ppStmt, index) == SQLITE_NULL) + { + depth.z = nanFloat; + ++index; + } + else + { + depth.z = sqlite3_column_double(ppStmt, index++); + } + + visualWordsKpts.push_back(kpt); + visualWords.insert(visualWords.end(), std::make_pair(visualWordId, visualWordsKpts.size()-1)); + visualWords3.push_back(depth); + + if(allWords3NaN && util3d::isFinite(depth)) + { + allWords3NaN = false; + } + + if(uStrNumCmp(_version, "0.11.2") >= 0) + { + descriptorSize = sqlite3_column_int(ppStmt, index++); // VisualWord descriptor size + descriptor = sqlite3_column_blob(ppStmt, index); // VisualWord descriptor array + dRealSize = sqlite3_column_bytes(ppStmt, index++); + + if(descriptor && descriptorSize>0 && dRealSize>0) + { + cv::Mat d; + if(dRealSize == descriptorSize) + { + // CV_8U binary descriptors + d = cv::Mat(1, descriptorSize, CV_8U); + } + else if(dRealSize/int(sizeof(float)) == descriptorSize) + { + // CV_32F + d = cv::Mat(1, descriptorSize, CV_32F); + } + else + { + UFATAL("Saved buffer size (%d bytes) is not the same as descriptor size (%d)", dRealSize, descriptorSize); + } + + memcpy(d.data, descriptor, dRealSize); + + descriptors.push_back(d); + } + } + + rc = sqlite3_step(ppStmt); + } + UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str()); + + if(visualWords.size()==0) + { + UDEBUG("Empty signature detected! (id=%d)", (*iter)->id()); + } + else + { + if(allWords3NaN) + { + visualWords3.clear(); + } + (*iter)->setWords(visualWords, visualWordsKpts, visualWords3, descriptors); + //ULOGGER_DEBUG("Add %d keypoints, %d 3d points and %d descriptors to node %d", (int)visualWords.size(), allWords3NaN?0:(int)visualWords3.size(), (int)descriptors.rows, (*iter)->id()); + } + + //reset + rc = sqlite3_reset(ppStmt); + UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str()); + } + + // Finalize (delete) the statement + rc = sqlite3_finalize(ppStmt); + UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str()); + } +} + void DBDriverSqlite3::loadLinksQuery( int signatureId, std::multimap & links, @@ -4174,7 +4332,7 @@ void DBDriverSqlite3::loadLinksQuery(std::list & signatures) const //reset rc = sqlite3_reset(ppStmt); UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str()); - UDEBUG("time=%fs, node=%d, links.size=%d", timer.ticks(), (*iter)->id(), links.size()); + //UDEBUG("time=%fs, node=%d, links.size=%d", timer.ticks(), (*iter)->id(), links.size()); } // Finalize (delete) the statement @@ -4185,7 +4343,7 @@ void DBDriverSqlite3::loadLinksQuery(std::list & signatures) const void DBDriverSqlite3::updateQuery(const std::list & nodes, bool updateTimestamp) const { - UDEBUG("nodes = %d", nodes.size()); + UDEBUG("nodes = %d, updateTimestamp = %s", nodes.size(), updateTimestamp?"true":"false"); if(_ppDb && nodes.size()) { UTimer timer; @@ -4224,7 +4382,7 @@ void DBDriverSqlite3::updateQuery(const std::list & nodes, bool upd { s = *i; int index = 1; - if(s) + if(s && (s->isModified() || updateTimestamp)) { rc = sqlite3_bind_int(ppStmt, index++, s->getWeight()); UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str()); @@ -4248,7 +4406,7 @@ void DBDriverSqlite3::updateQuery(const std::list & nodes, bool upd //step rc=sqlite3_step(ppStmt); - UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str()); + UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s (node id = %d map=%d)", _version.c_str(), sqlite3_errmsg(_ppDb), s->id(), s->mapId()).c_str()); rc = sqlite3_reset(ppStmt); UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str()); @@ -4261,14 +4419,7 @@ void DBDriverSqlite3::updateQuery(const std::list & nodes, bool upd ULOGGER_DEBUG("Update Node table, Time=%fs", timer.ticks()); // Update links part1 - if(uStrNumCmp(_version, "0.18.3") >= 0) - { - query = uFormat("DELETE FROM Link WHERE from_id=? and type!=%d;", (int)Link::kLandmark); - } - else - { - query = uFormat("DELETE FROM Link WHERE from_id=?;"); - } + query = uFormat("DELETE FROM Link WHERE from_id=?;"); rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0); UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str()); for(std::list::const_iterator j=nodes.begin(); j!=nodes.end(); ++j) @@ -4303,6 +4454,12 @@ void DBDriverSqlite3::updateQuery(const std::list & nodes, bool upd { stepLink(ppStmt, i->second); } + // Save landmarks + const std::map & landmarks = (*j)->getLandmarks(); + for(std::map::const_iterator i=landmarks.begin(); i!=landmarks.end(); ++i) + { + stepLink(ppStmt, i->second); + } } } // Finalize (delete) the statement @@ -5117,8 +5274,8 @@ std::map DBDriverSqlite3::loadOptimizedPosesQuery(Transform * la Transform t(serializedPoses.at(i*12), serializedPoses.at(i*12+1), serializedPoses.at(i*12+2), serializedPoses.at(i*12+3), serializedPoses.at(i*12+4), serializedPoses.at(i*12+5), serializedPoses.at(i*12+6), serializedPoses.at(i*12+7), serializedPoses.at(i*12+8), serializedPoses.at(i*12+9), serializedPoses.at(i*12+10), serializedPoses.at(i*12+11)); - poses.insert(std::make_pair(serializedIds.at(i), t)); - UDEBUG("Optimized pose %d: %s", serializedIds.at(i), t.prettyPrint().c_str()); + poses.insert(poses.end(), std::make_pair(serializedIds.at(i), t)); + //UDEBUG("Optimized pose %d: %s", serializedIds.at(i), t.prettyPrint().c_str()); } } @@ -5591,6 +5748,47 @@ cv::Mat DBDriverSqlite3::loadOptimizedMeshQuery( return cloud; } +void DBDriverSqlite3::saveFlannIndexQuery(const std::vector & data) const +{ + UDEBUG("data size = %ld bytes", data.size()); + if(_ppDb && uStrNumCmp(_version, "0.23.0") >= 0) + { + UTimer timer; + timer.start(); + int rc = SQLITE_OK; + sqlite3_stmt * ppStmt = 0; + std::string query; + + // Update table Admin + query = uFormat("UPDATE Admin SET dictionary_index=? WHERE version='%s';", _version.c_str()); + rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0); + UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str()); + + int index = 1; + + if(data.empty()) + { + rc = sqlite3_bind_null(ppStmt, index++); + UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str()); + } + else + { + rc = sqlite3_bind_blob(ppStmt, index++, data.data(), data.size(), SQLITE_STATIC); + UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str()); + } + + //execute query + rc=sqlite3_step(ppStmt); + UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str()); + + // Finalize (delete) the statement + rc = sqlite3_finalize(ppStmt); + UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str()); + + UDEBUG("Time=%fs", timer.ticks()); + } +} + std::string DBDriverSqlite3::queryStepNode() const { if(uStrNumCmp(_version, "0.18.0") >= 0) @@ -6598,7 +6796,7 @@ void DBDriverSqlite3::stepLink( { UFATAL(""); } - UDEBUG("Save link from %d to %d, type=%d", link.from(), link.to(), link.type()); + //UDEBUG("Save link from %d to %d, type=%d", link.from(), link.to(), link.type()); // Don't save virtual links if(link.type()==Link::kVirtualClosure) diff --git a/corelib/src/DBReader.cpp b/corelib/src/DBReader.cpp index aa4d4e44..5f5c0540 100644 --- a/corelib/src/DBReader.cpp +++ b/corelib/src/DBReader.cpp @@ -56,6 +56,8 @@ DBReader::DBReader(const std::string & databasePath, int startMapId, int stopMapId, bool priorsIgnored, + bool imuIgnored, + bool intermediateNodesAreNormalNodes, const std::vector & cameraLocalTransformOverrides) : Camera(frameRate), _paths(uSplit(databasePath, ';')), @@ -66,9 +68,11 @@ DBReader::DBReader(const std::string & databasePath, _stopId(stopId), _cameraIndices(cameraIndices), _intermediateNodesIgnored(intermediateNodesIgnored), + _intermediateNodesAreNormalNodes(intermediateNodesAreNormalNodes), _landmarksIgnored(landmarksIgnored), _featuresIgnored(featuresIgnored), _priorsIgnored(priorsIgnored), + _imuIgnored(imuIgnored), _startMapId(startMapId), _stopMapId(stopMapId), _cameraLocalTransformOverrides(cameraLocalTransformOverrides), @@ -96,6 +100,8 @@ DBReader::DBReader(const std::list & databasePaths, int startMapId, int stopMapId, bool priorsIgnored, + bool imuIgnored, + bool intermediateNodesAreNormalNodes, const std::vector & cameraLocalTransformOverrides) : Camera(frameRate), _paths(databasePaths), @@ -106,9 +112,11 @@ DBReader::DBReader(const std::list & databasePaths, _stopId(stopId), _cameraIndices(cameraIndices), _intermediateNodesIgnored(intermediateNodesIgnored), + _intermediateNodesAreNormalNodes(intermediateNodesAreNormalNodes), _landmarksIgnored(landmarksIgnored), _featuresIgnored(featuresIgnored), _priorsIgnored(priorsIgnored), + _imuIgnored(imuIgnored), _startMapId(startMapId), _stopMapId(stopMapId), _cameraLocalTransformOverrides(cameraLocalTransformOverrides), @@ -260,7 +268,7 @@ bool DBReader::init( else { Signature * s = _dbDriver->loadSignature(*_ids.begin()); - _dbDriver->loadNodeData(s); + _dbDriver->loadNodeData(*s); if( s->sensorData().imageCompressed().empty() && s->getWords().empty() && !s->sensorData().laserScanCompressed().empty()) @@ -463,14 +471,17 @@ SensorData DBReader::getNextData(SensorCaptureInfo * info) } Transform gravityTransform; - std::multimap gravityLinks; - _dbDriver->loadLinks(*_currentId, gravityLinks, Link::kGravity); - if( gravityLinks.size() && - !gravityLinks.begin()->second.transform().isNull() && - gravityLinks.begin()->second.infMatrix().cols == 6 && - gravityLinks.begin()->second.infMatrix().rows == 6) + if(!_imuIgnored) { - gravityTransform = gravityLinks.begin()->second.transform(); + std::multimap gravityLinks; + _dbDriver->loadLinks(*_currentId, gravityLinks, Link::kGravity); + if( gravityLinks.size() && + !gravityLinks.begin()->second.transform().isNull() && + gravityLinks.begin()->second.infMatrix().cols == 6 && + gravityLinks.begin()->second.infMatrix().rows == 6) + { + gravityTransform = gravityLinks.begin()->second.transform(); + } } Landmarks landmarks; @@ -510,22 +521,41 @@ SensorData DBReader::getNextData(SensorCaptureInfo * info) } else { - // if localization data saved in database, covariance will be set in a prior link - _dbDriver->loadLinks(*_currentId, links, Link::kPosePrior); - if(links.size()) - { - // assume the first is the backward neighbor, take its variance - infMatrix = links.begin()->second.infMatrix(); - _previousInfMatrix = infMatrix; - } - else - { - if(_previousInfMatrix.empty()) + // In case the graph was reduced, look for forward neighbor link from previous id + bool covAdded = false; + if(_currentId != _ids.begin()) { + std::set::iterator previousId = _currentId; + --previousId; + std::multimap previousLinks; + _dbDriver->loadLinks(*previousId, previousLinks, Link::kNeighbor); + if(previousLinks.size() && previousLinks.rbegin()->first == *_currentId) { - _previousInfMatrix = cv::Mat::eye(6,6,CV_64FC1); + // assume the last is the forward neighbor pointing to current ID, take its covariance + infMatrix = previousLinks.rbegin()->second.infMatrix(); + _previousInfMatrix = infMatrix; + covAdded = true; + } + } + + if(!covAdded) { + // if localization data saved in database, covariance will be set in a prior link + _dbDriver->loadLinks(*_currentId, links, Link::kPosePrior); + if(links.size()) + { + // assume the first is the backward neighbor, take its variance + infMatrix = links.begin()->second.infMatrix(); + _previousInfMatrix = infMatrix; + } + else + { + if(_previousInfMatrix.empty()) + { + _previousInfMatrix = cv::Mat::eye(6,6,CV_64FC1); + } + // we have a node not linked to map, use last variance + UWARN("The node loaded (%d) doesn't have neighbor, re-using the covariance of the previous link for odometry.", s->id()); + infMatrix = _previousInfMatrix; } - // we have a node not linked to map, use last variance - infMatrix = _previousInfMatrix; } } } @@ -723,7 +753,7 @@ SensorData DBReader::getNextData(SensorCaptureInfo * info) data.setStereoCameraModels(combinedStereoModels); } } - data.setId(seq); + data.setId(!_intermediateNodesAreNormalNodes && s->getWeight()==-1 ? -1 : seq); data.setStamp(s->getStamp()); data.setGroundTruth(s->getGroundTruthPose()); if(!globalPose.isNull()) @@ -837,6 +867,7 @@ SensorData DBReader::getNextData(SensorCaptureInfo * info) if(info) { info->odomPose = pose; + UASSERT(!infMatrix.empty()); info->odomCovariance = infMatrix.inv(); info->odomVelocity = s->getVelocity(); UDEBUG("odom variance = %f/%f", info->odomCovariance.at(0,0), info->odomCovariance.at(5,5)); diff --git a/corelib/src/Features2d.cpp b/corelib/src/Features2d.cpp index 90555db4..da83db71 100644 --- a/corelib/src/Features2d.cpp +++ b/corelib/src/Features2d.cpp @@ -47,6 +47,9 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #ifdef RTABMAP_TORCH #include "superpoint_torch/SuperPoint.h" #endif +#if defined(RTABMAP_TORCH) && defined(RTABMAP_PYTHON) +#include "superpoint_rpautrat/SuperpointRpautrat.h" +#endif #ifdef RTABMAP_PYTHON #include "python/PyDetector.h" @@ -300,7 +303,7 @@ void Feature2D::limitKeypoints(std::vector & keypoints, std::vecto cv::Mat descriptorsTmp; if(ssc) { - ULOGGER_DEBUG("too much words (%d), removing words with SSC", keypoints.size()); + ULOGGER_DEBUG("too many words (%d), removing words with SSC", keypoints.size()); // Sorting keypoints by deacreasing order of strength std::vector responseVector; @@ -416,7 +419,7 @@ void Feature2D::limitKeypoints(const std::vector & keypoints, std: inliers.resize(keypoints.size(), false); if(ssc) { - ULOGGER_DEBUG("too much words (%d), removing words with SSC", keypoints.size()); + ULOGGER_DEBUG("too many words (%d), removing words with SSC", keypoints.size()); // Sorting keypoints by deacreasing order of strength std::vector responseVector; @@ -463,7 +466,7 @@ void Feature2D::limitKeypoints(const std::vector & keypoints, std: minimumHessian = iter->first; } } - ULOGGER_DEBUG("%d keypoints removed, (kept %d), minimum response=%f", removed, maxKeypoints, minimumHessian); + ULOGGER_DEBUG("%d keypoints removed, (kept %d), minimum response=%f", removed, keypoints.size()-removed, minimumHessian); ULOGGER_DEBUG("filter keypoints time = %f s", timer.ticks()); } else @@ -730,9 +733,14 @@ Feature2D * Feature2D::create(Feature2D::Type type, const ParametersMap & parame feature2D = new ORBOctree(parameters); break; #ifdef RTABMAP_TORCH - case Feature2D::kFeatureSuperPointTorch: - feature2D = new SuperPointTorch(parameters); - break; +case Feature2D::kFeatureSuperPointTorch: + feature2D = new SuperPointTorch(parameters); + break; +#endif +#if defined(RTABMAP_TORCH) && defined(RTABMAP_PYTHON) +case Feature2D::kFeatureSuperPointRpautrat: + feature2D = new SuperPointRpautrat(parameters); + break; #endif case Feature2D::kFeatureSurfFreak: feature2D = new SURF_FREAK(parameters); @@ -831,7 +839,7 @@ std::vector Feature2D::generateKeypoints(const cv::Mat & image, co cv::Rect roi(globalRoi.x + j*colSize, globalRoi.y + i*rowSize, colSize, rowSize); std::vector subKeypoints; subKeypoints = this->generateKeypointsImpl(image, roi, mask); - if (this->getType() != Feature2D::Type::kFeaturePyDetector) + if (this->getType() != Feature2D::Type::kFeaturePyDetector && this->getType() != Feature2D::Type::kFeatureSuperPointRpautrat) { limitKeypoints(subKeypoints, maxFeatures, roi.size(), this->getSSC()); } @@ -879,8 +887,17 @@ cv::Mat Feature2D::generateDescriptors( UASSERT(!image.empty()); UASSERT(image.type() == CV_8UC1); descriptors = generateDescriptorsImpl(image, keypoints); - UASSERT_MSG(descriptors.rows == (int)keypoints.size(), uFormat("descriptors=%d, keypoints=%d", descriptors.rows, (int)keypoints.size()).c_str()); - UDEBUG("Descriptors extracted = %d, remaining kpts=%d", descriptors.rows, (int)keypoints.size()); + if(descriptors.rows != (int)keypoints.size()) + { + UWARN("Descriptor extraction returned %d rows for %d keypoints — " + "clearing keypoints to keep them in sync.", + descriptors.rows, (int)keypoints.size()); + keypoints.clear(); + descriptors = cv::Mat(); + } + else { + UDEBUG("Descriptors extracted = %d, remaining kpts=%d", descriptors.rows, (int)keypoints.size()); + } } return descriptors; } @@ -1243,7 +1260,8 @@ SIFT::SIFT(const ParametersMap & parameters) : preciseUpscale_(Parameters::defaultSIFTPreciseUpscale()), rootSIFT_(Parameters::defaultSIFTRootSIFT()), gpu_(Parameters::defaultSIFTGpu()), - guaussianThreshold_(Parameters::defaultSIFTGaussianThreshold()), + gaussianThreshold_(Parameters::defaultSIFTGaussianThreshold()), + maxGaussianThreshold_(Parameters::defaultSIFTMaxGaussianThreshold()), upscale_(Parameters::defaultSIFTUpscale()), cudaSiftData_(0), cudaSiftMemory_(0), @@ -1276,23 +1294,25 @@ void SIFT::parseParameters(const ParametersMap & parameters) Parameters::parse(parameters, Parameters::kSIFTPreciseUpscale(), preciseUpscale_); Parameters::parse(parameters, Parameters::kSIFTRootSIFT(), rootSIFT_); Parameters::parse(parameters, Parameters::kSIFTGpu(), gpu_); - Parameters::parse(parameters, Parameters::kSIFTGaussianThreshold(), guaussianThreshold_); + Parameters::parse(parameters, Parameters::kSIFTGaussianThreshold(), gaussianThreshold_); + Parameters::parse(parameters, Parameters::kSIFTMaxGaussianThreshold(), maxGaussianThreshold_); Parameters::parse(parameters, Parameters::kSIFTUpscale(), upscale_); if(gpu_) { #ifdef RTABMAP_CUDASIFT // Check if there is a cuda device - if(InitCuda(0, ULogger::level() == ULogger::kDebug)) { - UDEBUG("Init SiftData"); - if(cudaSiftData_ == 0) { + if(cudaSiftData_==0) + { + if(InitCuda(0, ULogger::level() == ULogger::kDebug)) { + UDEBUG("Init SiftData"); cudaSiftData_ = new SiftData(); InitSiftData(*cudaSiftData_, 8192, true, true); } - } - else{ - UWARN("No cuda device(s) detected, CudaSift is not available! Using SIFT CPU version instead."); - gpu_ = false; + else{ + UWARN("No cuda device(s) detected, CudaSift is not available! Using SIFT CPU version instead."); + gpu_ = false; + } } #else UWARN("RTAB-Map is not built with CudaSift so %s cannot be used!", Parameters::kSIFTGpu().c_str()); @@ -1355,7 +1375,7 @@ std::vector SIFT::generateKeypointsImpl(const cv::Mat & image, con numOctaves = 7; // hard-coded limit in CudaSift } float initBlur = sigma_; /* Amount of initial Gaussian blurring in standard deviations */ - float thresh = guaussianThreshold_; /* Threshold on difference of Gaussians for feature pruning */ + float thresh = gaussianThreshold_; /* Threshold on difference of Gaussians for feature pruning */ float edgeLimit = edgeThreshold_; float minScale = 0.0f; /* Minimum acceptable scale to remove fine-scale features */ UDEBUG("numOctaves=%d initBlur=%f thresh=%f edgeLimit=%f minScale=%f upScale=%s w=%d h=%d", numOctaves, initBlur, thresh, edgeLimit, minScale, upscale_?"true":"false", w, h); @@ -1380,15 +1400,9 @@ std::vector SIFT::generateKeypointsImpl(const cv::Mat & image, con cudaSiftDescriptors_ = cv::Mat(); if(cudaSiftData_->numPts) { - int maxKeypoints = this->getMaxFeatures(); - if(maxKeypoints == 0 || maxKeypoints > cudaSiftData_->numPts) - { - maxKeypoints = cudaSiftData_->numPts; - } - - // Re-using same implementation of limitKeypoints() directly here to avoid doubling memory copies - // Sort words by hessian - std::multimap hessianMap; // + keypoints.resize(cudaSiftData_->numPts); + cudaSiftDescriptors_ = cv::Mat(cudaSiftData_->numPts, 128, CV_32FC1); + size_t k=0; for(int i=0; inumPts; ++i) { // Ignore keypoints with invalid descriptors @@ -1405,29 +1419,40 @@ std::vector SIFT::generateKeypointsImpl(const cv::Mat & image, con continue; } - //Keep track of the data, to be easier to manage the data in the next step - hessianMap.insert(std::pair(cudaSiftData_->h_data[i].sharpness, i)); - } + if(i>0 && + cudaSiftData_->h_data[i].subsampling == cudaSiftData_->h_data[i-1].subsampling && + fabs(cudaSiftData_->h_data[i].xpos-cudaSiftData_->h_data[i-1].xpos) + + fabs(cudaSiftData_->h_data[i].xpos-cudaSiftData_->h_data[i-1].ypos) < 0.1f) + { + // Same feature, skip doubles + continue; + } - if((int)hessianMap.size() < maxKeypoints) - { - maxKeypoints = hessianMap.size(); - } + float response = abs(cudaSiftData_->h_data[i].sharpness); + if(maxGaussianThreshold_>gaussianThreshold_ && response > maxGaussianThreshold_) + { + continue; + } - std::multimap::reverse_iterator iter = hessianMap.rbegin(); - keypoints.resize(maxKeypoints); - cudaSiftDescriptors_ = cv::Mat(maxKeypoints, 128, CV_32FC1); - for(unsigned int k=0; ksecond; - float *desc = cudaSiftData_->h_data[i].data; cv::Mat(1, 128, CV_32FC1, desc).copyTo(cudaSiftDescriptors_.row(k)); keypoints[k].pt.x = cudaSiftData_->h_data[i].xpos; keypoints[k].pt.y = cudaSiftData_->h_data[i].ypos; keypoints[k].size = 2.0f*cudaSiftData_->h_data[i].scale; // x2 because the scale is more like a radius than a diameter, see CudaSift's ExtractSiftDescriptors function to see how they convert scale to patch size keypoints[k].angle = cudaSiftData_->h_data[i].orientation; - keypoints[k].response = cudaSiftData_->h_data[i].sharpness; + keypoints[k].response = response; keypoints[k].octave = log2(cudaSiftData_->h_data[i].subsampling)-(upscale_?1:0); + ++k; + } + if(k < keypoints.size()) + { + UDEBUG("keypoints extracted = %d, valid=%d", keypoints.size(), k); + keypoints.resize(k); + cudaSiftDescriptors_.resize(k); + } + if(this->getMaxFeatures() != 0 && this->getMaxFeatures() < (int)keypoints.size()) + { + // Call limitKeypoints() now to filter the descriptors. + this->limitKeypoints(keypoints, cudaSiftDescriptors_, this->getMaxFeatures(), cv::Size(w,h), this->getSSC()); } } } @@ -1449,12 +1474,13 @@ std::vector SIFT::generateKeypointsImpl(const cv::Mat & image, con cv::Mat SIFT::generateDescriptorsImpl(const cv::Mat & image, std::vector & keypoints) const { + cv::Mat descriptors; #ifdef RTABMAP_CUDASIFT if(gpu_) { if((int)keypoints.size() == cudaSiftDescriptors_.rows) { - return cudaSiftDescriptors_.clone(); + descriptors = cudaSiftDescriptors_.clone(); } else { @@ -1462,19 +1488,25 @@ cv::Mat SIFT::generateDescriptorsImpl(const cv::Mat & image, std::vectorcompute(image, keypoints, descriptors); + sift_->compute(image, keypoints, descriptors); #else - UWARN("RTAB-Map is not built with OpenCV nonfree module so SIFT cannot be used!"); + UWARN("RTAB-Map is not built with OpenCV nonfree module so SIFT cannot be used!"); #endif #else // >=4.4, >=3.4.11 - sift_->compute(image, keypoints, descriptors); + sift_->compute(image, keypoints, descriptors); #endif + +#ifdef RTABMAP_CUDASIFT + } +#endif + if( rootSIFT_ && !descriptors.empty()) { UDEBUG("Performing RootSIFT..."); @@ -2607,7 +2639,31 @@ cv::Mat SuperPointTorch::generateDescriptorsImpl(const cv::Mat & image, std::vec { #ifdef RTABMAP_TORCH UASSERT(!image.empty() && image.channels() == 1 && image.depth() == CV_8U); - return superPoint_->compute(keypoints); + cv::Mat descriptors; + if(!keypoints.empty()) + { + descriptors = superPoint_->compute(keypoints); + if(descriptors.empty()) + { + // superpoint may have been reset between keypoint detection and now, + // re-detect features to re-inialize the descriptors matrix, then + // re-extract descriptors with original keypoints. + UWARN("Re-initializing superpoint on that image to extract descriptors"); + if(!superPoint_->detect(image).empty()) + { + descriptors = superPoint_->compute(keypoints); + if(descriptors.rows == (int)keypoints.size()) + { + UWARN("Sucessfully re-initialized superpoint, returning %d descriptors.", descriptors.rows); + } + } + else + { + UWARN("Failed to re-initialize superpoint on that image, returning empty descriptors."); + } + } + } + return descriptors; #else UWARN("RTAB-Map is not built with Torch support so SuperPoint Torch feature cannot be used!"); return cv::Mat(); @@ -2615,6 +2671,132 @@ cv::Mat SuperPointTorch::generateDescriptorsImpl(const cv::Mat & image, std::vec } +////////////////////////// +//SuperPointRpautrat +////////////////////////// +SuperPointRpautrat::SuperPointRpautrat(const ParametersMap & parameters) : + superpointWeightsPath_(Parameters::defaultSuperPointRpautratWeightsPath()), + superpointModelPath_(Parameters::defaultSuperPointRpautratModelPath()), + outputDir_(""), + threshold_(Parameters::defaultSuperPointRpautratThreshold()), + nms_(Parameters::defaultSuperPointRpautratNMS()), + minDistance_(Parameters::defaultSuperPointRpautratNMSRadius()), + cuda_(Parameters::defaultSuperPointRpautratCuda()) +{ + parseParameters(parameters); +} + +SuperPointRpautrat::~SuperPointRpautrat() +{ +} + +void SuperPointRpautrat::parseParameters(const ParametersMap & parameters) +{ + Feature2D::parseParameters(parameters); + +#if defined(RTABMAP_TORCH) && defined(RTABMAP_PYTHON) + std::string previousWeightsPath = superpointWeightsPath_; + std::string previousModelPath = superpointModelPath_; + bool previousCuda = cuda_; + float previousThreshold = threshold_; + bool previousNms = nms_; + int previousMinDistance = minDistance_; + + Parameters::parse(parameters, Parameters::kSuperPointRpautratWeightsPath(), superpointWeightsPath_); + Parameters::parse(parameters, Parameters::kSuperPointRpautratModelPath(), superpointModelPath_); + Parameters::parse(parameters, Parameters::kSuperPointRpautratThreshold(), threshold_); + Parameters::parse(parameters, Parameters::kSuperPointRpautratNMS(), nms_); + Parameters::parse(parameters, Parameters::kSuperPointRpautratNMSRadius(), minDistance_); + Parameters::parse(parameters, Parameters::kSuperPointRpautratCuda(), cuda_); + Parameters::parse(parameters, Parameters::kRtabmapWorkingDirectory(), outputDir_); + + // If working directory is not set, use the default + if(outputDir_.empty()) + { + outputDir_ = Parameters::createDefaultWorkingDirectory(); + } + + // Reinitialize detector if model-affecting parameters changed + if(superPoint_.get() == 0 || + superpointWeightsPath_.compare(previousWeightsPath) != 0 || + superpointModelPath_.compare(previousModelPath) != 0 || + previousCuda != cuda_ || + previousThreshold != threshold_ || + previousNms != nms_ || + previousMinDistance != minDistance_) + { + superPoint_ = cv::Ptr(new SPDetectorRpautrat(superpointWeightsPath_, superpointModelPath_, outputDir_, threshold_, nms_, minDistance_, cuda_, this->getMaxFeatures(), this->getSSC())); + } + else if(superPoint_.get() != 0) + { + // Update post-processing parameters without reinitializing + superPoint_->setMaxFeatures(this->getMaxFeatures()); + superPoint_->setSSC(this->getSSC()); + } +#else + UWARN("RTAB-Map is not built with Torch support so SuperPoint Rpautrat feature cannot be used!"); +#endif +} + +std::vector SuperPointRpautrat::generateKeypointsImpl(const cv::Mat & image, const cv::Rect & roi, const cv::Mat & mask) +{ +#if defined(RTABMAP_TORCH) && defined(RTABMAP_PYTHON) + UASSERT(!image.empty() && image.channels() == 1 && image.depth() == CV_8U); + if(roi.x!=0 || roi.y !=0) + { + UERROR("SuperPoint Rpautrat: Not supporting ROI (%d,%d,%d,%d). Make sure %s, %s, %s, %s, %s, %s are all set to default values.", + roi.x, roi.y, roi.width, roi.height, + Parameters::kKpRoiRatios().c_str(), + Parameters::kVisRoiRatios().c_str(), + Parameters::kVisGridRows().c_str(), + Parameters::kVisGridCols().c_str(), + Parameters::kKpGridRows().c_str(), + Parameters::kKpGridCols().c_str()); + return std::vector(); + } + return superPoint_->detect(image, mask); +#else + UWARN("RTAB-Map is not built with Torch support so SuperPoint Rpautrat feature cannot be used!"); + return std::vector(); +#endif +} + +cv::Mat SuperPointRpautrat::generateDescriptorsImpl(const cv::Mat & image, std::vector & keypoints) const +{ +#if defined(RTABMAP_TORCH) && defined(RTABMAP_PYTHON) + UASSERT(!image.empty() && image.channels() == 1 && image.depth() == CV_8U); + cv::Mat descriptors; + if(!keypoints.empty()) + { + descriptors = superPoint_->compute(keypoints); + if(descriptors.empty()) + { + // superpoint may have been reset between keypoint detection and now, + // re-detect features to re-inialize the descriptors matrix, then + // re-extract descriptors with original keypoints. + UWARN("Re-initializing superpoint on that image to extract descriptors"); + if(!superPoint_->detect(image).empty()) + { + descriptors = superPoint_->compute(keypoints); + if(descriptors.rows == (int)keypoints.size()) + { + UWARN("Sucessfully re-initialized superpoint, returning %d descriptors.", descriptors.rows); + } + } + else + { + UWARN("Failed to re-initialize superpoint on that image, returning empty descriptors."); + } + } + } + return descriptors; +#else + UWARN("RTAB-Map is not built with Torch support so SuperPoint Rpautrat feature cannot be used!"); + return cv::Mat(); +#endif +} + + ////////////////////////// //GFTT-DAISY ////////////////////////// diff --git a/corelib/src/FlannIndex.cpp b/corelib/src/FlannIndex.cpp index a5932577..b6b09a4c 100644 --- a/corelib/src/FlannIndex.cpp +++ b/corelib/src/FlannIndex.cpp @@ -27,8 +27,16 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include +#include +#include +#include +#include +#ifdef WIN32 +#include +#endif #include "rtflann/flann.hpp" +#include namespace rtabmap { @@ -37,7 +45,6 @@ FlannIndex::FlannIndex(): nextIndex_(0), featuresType_(0), featuresDim_(0), - isLSH_(false), useDistanceL1_(false), rebalancingFactor_(2.0f) { @@ -49,9 +56,9 @@ FlannIndex::~FlannIndex() void FlannIndex::release() { - UDEBUG(""); if(index_) { + UDEBUG("Clearing flann index..."); if(featuresType_ == CV_8UC1) { delete (rtflann::Index >*)index_; @@ -72,12 +79,139 @@ void FlannIndex::release() } } index_ = 0; + UDEBUG("Clearing flann index... done!"); } nextIndex_ = 0; - isLSH_ = false; addedDescriptors_.clear(); removedIndexes_.clear(); - UDEBUG(""); +} + +#define FLANN_INDEX_HEADER_SIZE 12 + +std::vector FlannIndex::serializeIndex(bool computeChecksum) const { + if(index_ && !addedDescriptors_.empty()) + { +#ifdef WIN32 + UERROR("FLANN index serialization is not yet implemented on Windows. Parameter \"%s\" cannot be used.", Parameters::kKpFlannIndexSaved().c_str()); +#else + UTimer timer; + const int headerSizeBytes = sizeof(int)*FLANN_INDEX_HEADER_SIZE; + std::vector indexData(1024*1024*1024 + headerSizeBytes); // Max 1 GB + FILE* indexDataPtr = fmemopen(indexData.data()+headerSizeBytes, indexData.size() - headerSizeBytes, "wb"); + long bytes_written = 0; + if (indexDataPtr) { + if(featuresType_ == CV_8UC1) + { + ((rtflann::Index >*)index_)->save(indexDataPtr); + } + else + { + if(useDistanceL1_) + { + ((rtflann::Index >*)index_)->save(indexDataPtr);; + } + else if(featuresDim_ <= 3) + { + ((rtflann::Index >*)index_)->save(indexDataPtr);; + } + else + { + ((rtflann::Index >*)index_)->save(indexDataPtr);; + } + } + bytes_written = ftell(indexDataPtr); + fclose(indexDataPtr); + } + if(bytes_written < long(indexData.size()-headerSizeBytes)) + { + //Expected data size and type + int dataRows = 0; + int dataCols = 0; + int dataType = -1; + cv::Mat dataset; + std::set removedDescriptors; + if(computeChecksum){ + removedDescriptors.insert(removedIndexes_.begin(), removedIndexes_.end()); + } + for(const auto & iter: addedDescriptors_) + { + UASSERT(!iter.second.empty()); + dataRows += iter.second.rows; + if(dataCols <= 0) { + dataCols = iter.second.cols; + } + else { + UASSERT(dataCols == iter.second.cols); + } + if(dataType < 0) { + dataType = iter.second.type(); + } + else { + UASSERT(dataType == iter.second.type()); + } + if(computeChecksum){ + if(removedDescriptors.find(iter.first) == removedDescriptors.end()) { + if(dataset.empty()) { + dataset = iter.second.clone(); + } + else { + dataset.push_back(iter.second); + } + } + else { + dataRows -= iter.second.rows; + } + } + } + if(!computeChecksum) { + for(const auto & index: removedIndexes_) + { + dataRows -= addedDescriptors_.at(index).rows; + } + } + + unsigned int crcValue = 0; + if(computeChecksum) { + boost::crc_32_type result; + result.process_bytes(dataset.data, dataset.total()*dataset.elemSize()); + crcValue = result.checksum(); + } + + indexData.resize(bytes_written+headerSizeBytes); + indexData.shrink_to_fit(); + int rebalancingFactorAsInt; + memcpy(&rebalancingFactorAsInt, &rebalancingFactor_, sizeof(rebalancingFactor_)); + int crcValueAsInt; + memcpy(&crcValueAsInt, &crcValue, sizeof(crcValue)); + int header[FLANN_INDEX_HEADER_SIZE] = { + RTABMAP_VERSION_MAJOR, RTABMAP_VERSION_MINOR, RTABMAP_VERSION_PATCH, // 0,1,2 + algorithm_, // 3, + featuresDim_, // 4, + useDistanceL1_?1:0, // 5, + rebalancingFactorAsInt, // 6, + dataRows, // 7, + dataCols, // 8, + dataType, // 9, + crcValueAsInt, // 10 + (int)bytes_written}; // 11 + UDEBUG("Header: \"%d.%d.%d\" alg=%d dim=%d L1=%d factor=%f data(%dx%d type=%d, crc=%X) %d", + header[0],header[1],header[2], + header[3], + header[4], + header[5], + rebalancingFactor_, + header[7], header[8], header[9], crcValueAsInt, + header[11]); + memcpy(indexData.data(), header, headerSizeBytes); + return indexData; + } + else { + UERROR("Target buffer too small to serialize index, aborting."); + } + UDEBUG("Flann serialization: %fs", timer.ticks()); +#endif + } + return std::vector(); } size_t FlannIndex::indexedFeatures() const @@ -139,12 +273,13 @@ size_t FlannIndex::memoryUsed() const return memoryUsage; } -void FlannIndex::buildLinearIndex( +void FlannIndex::buildIndex( + flann_algorithm_t algorithm, const cv::Mat & features, bool useDistanceL1, float rebalancingFactor) { - UDEBUG(""); + UDEBUG("algorithm=%d", (int)algorithm); this->release(); UASSERT(index_ == 0); UASSERT(features.type() == CV_32FC1 || features.type() == CV_8UC1); @@ -152,8 +287,29 @@ void FlannIndex::buildLinearIndex( featuresDim_ = features.cols; useDistanceL1_ = useDistanceL1; rebalancingFactor_ = rebalancingFactor; + algorithm_ = algorithm; - rtflann::LinearIndexParams params; + rtflann::IndexParams params; + + switch (algorithm) + { + case FLANN_INDEX_LINEAR: + params = rtflann::LinearIndexParams(); + break; + case FLANN_INDEX_KDTREE: + params = rtflann::KDTreeIndexParams(4); + break; + case FLANN_INDEX_KDTREE_SINGLE: + params = rtflann::KDTreeSingleIndexParams(10, true); + break; + case FLANN_INDEX_LSH: + UASSERT(features.type() == CV_8UC1); + params = rtflann::LshIndexParams(12, 20, 2); + break; + default: + UFATAL("The flann algorithm type %d is not supported!", (int)algorithm); + break; + } if(featuresType_ == CV_8UC1) { @@ -199,13 +355,140 @@ void FlannIndex::buildLinearIndex( UDEBUG(""); } -void FlannIndex::buildKDTreeIndex( - const cv::Mat & features, - int trees, - bool useDistanceL1, - float rebalancingFactor) +bool FlannIndex::loadIndex( + const std::vector & indexData, + flann_algorithm_t algorithm, + const cv::Mat & features, + bool useDistanceL1, + float rebalancingFactor, + std::string * error) { - UDEBUG(""); + return loadIndex( + indexData.data(), + indexData.size(), + algorithm, + features, + useDistanceL1, + rebalancingFactor), + error; +} +bool FlannIndex::loadIndex( + const unsigned char * indexData, + size_t indexDataSize, + flann_algorithm_t algorithm, + const cv::Mat & features, + bool useDistanceL1, + float rebalancingFactor, + std::string * error) +{ + UASSERT(indexData!=NULL); + if(indexDataSize == 0) { + UWARN("Trying to load empty index...."); + return false; + } + +#ifdef WIN32 + UERROR("FLANN index deserialization is not yet implemented on Windows. Index cannot be loaded from memory buffer."); + return false; +#else + + // Check if the features match the expected data from the index + size_t headerSizeBytes = sizeof(int)*FLANN_INDEX_HEADER_SIZE; + if(indexDataSize < headerSizeBytes) { + if(error) { + *error = uFormat("Wrong header size detected (%ld vs expected %ld).", indexDataSize, headerSizeBytes); + } + return false; + } + const int * header = (const int *)indexData; + + int savedAlgorithm = header[3]; + int savedDim = header[4]; + bool savedDistanceL1 = header[5]==1; + float savedRebalancingFactor; + memcpy(&savedRebalancingFactor, &header[6], sizeof(header[6])); + int savedRows = header[7]; + int savedCols = header[8]; + int savedType = header[9]; + unsigned int savedCrc; + memcpy(&savedCrc, &header[10], sizeof(header[10])); + int savedIndexSize = header[11]; + + UDEBUG("Header: \"%d.%d.%d\" alg=%d dim=%d L1=%d factor=%f data(%dx%d type=%d, crc=%X) %d", + header[0],header[1],header[2], + header[3], + header[4], + header[5], + savedRebalancingFactor, + header[7], header[8], header[9], savedCrc, + header[11]); + + if(savedAlgorithm != algorithm) { + if(error) { + *error = uFormat("Serialized flann algorithm (%d) doesn't match the expected one (%d).", savedAlgorithm, algorithm); + } + return false; + } + if(savedDim != features.cols) { + if(error) { + *error = uFormat("Serialized feature dimension (%d) doesn't match the expected one (%d).", savedDim, features.cols); + } + return false; + } + if(savedDistanceL1 != useDistanceL1) { + if(error) { + *error = uFormat("Serialized \"use distance L1\" (%s) doesn't match the expected one (%s).", savedDistanceL1?"true":"false", useDistanceL1?"true":"false"); + } + return false; + } + if(savedRebalancingFactor != rebalancingFactor) { + if(error) { + *error = uFormat("Serialized \"rebalancing factor\" (%f) doesn't match the expected one (%f).", savedRebalancingFactor, rebalancingFactor); + } + return false; + } + if(savedRows != features.rows) { + if(error) { + *error = uFormat("Serialized feature count (%d) doesn't match the expected one (%d).", savedRows, features.rows); + } + return false; + } + if(savedCols != features.cols) { + if(error) { + *error = uFormat("Serialized feature dimension (%d) doesn't match the expected one (%d).", savedCols, features.cols); + } + return false; + } + if(savedType != features.type()) { + if(error) { + *error = uFormat("Serialized feature type (%d) doesn't match the expected one (%d).", savedType, features.type()); + } + return false; + } + if(savedCrc != 0) { + // Compute checksum and compare + boost::crc_32_type result; + result.process_bytes(features.data, features.total()*features.elemSize()); + if(savedCrc != result.checksum()) { + if(error) { + *error = uFormat("Serialized feature crc (%X) doesn't match the expected one (%X).", savedCrc, result.checksum()); + } + return false; + } + } + if(savedIndexSize != int(indexDataSize - headerSizeBytes)) { + if(error) { + *error = uFormat("Serialized flann index size (%ld) doesn't match the expected one (%ld).", savedIndexSize, indexDataSize - headerSizeBytes); + } + return false; + } + if(savedIndexSize == 0) { + if(error) { + *error = "Serialized flann index is empty."; + } + return false; + } + this->release(); UASSERT(index_ == 0); UASSERT(features.type() == CV_32FC1 || features.type() == CV_8UC1); @@ -213,14 +496,39 @@ void FlannIndex::buildKDTreeIndex( featuresDim_ = features.cols; useDistanceL1_ = useDistanceL1; rebalancingFactor_ = rebalancingFactor; + algorithm_ = algorithm; - rtflann::KDTreeIndexParams params(trees); + UDEBUG("algorithm=%d", (int)algorithm); + + rtflann::IndexParams params; + + switch (algorithm) + { + case FLANN_INDEX_LINEAR: + params = rtflann::LinearIndexParams(); + break; + case FLANN_INDEX_KDTREE: + params = rtflann::KDTreeIndexParams(4); + break; + case FLANN_INDEX_KDTREE_SINGLE: + params = rtflann::KDTreeSingleIndexParams(10, true); + break; + case FLANN_INDEX_LSH: + UASSERT(features.type() == CV_8UC1); + params = rtflann::LshIndexParams(12, 20, 2); + break; + default: + UFATAL("The flann algorithm type %d is not supported!", (int)algorithm); + break; + } + + FILE* indexDataPtr = fmemopen((void*)(indexData+headerSizeBytes), indexDataSize - headerSizeBytes, "r"); if(featuresType_ == CV_8UC1) { rtflann::Matrix dataset(features.data, features.rows, features.cols); index_ = new rtflann::Index >(dataset, params); - ((rtflann::Index >*)index_)->buildIndex(); + ((rtflann::Index >*)index_)->load_saved_index(indexDataPtr); } else { @@ -228,22 +536,24 @@ void FlannIndex::buildKDTreeIndex( if(useDistanceL1_) { index_ = new rtflann::Index >(dataset, params); - ((rtflann::Index >*)index_)->buildIndex(); + ((rtflann::Index >*)index_)->load_saved_index(indexDataPtr); } else if(featuresDim_ <=3) { index_ = new rtflann::Index >(dataset, params); - ((rtflann::Index >*)index_)->buildIndex(); + ((rtflann::Index >*)index_)->load_saved_index(indexDataPtr); } else { index_ = new rtflann::Index >(dataset, params); - ((rtflann::Index >*)index_)->buildIndex(); + ((rtflann::Index >*)index_)->load_saved_index(indexDataPtr); } } + fclose(indexDataPtr); // incremental FLANN: we should add all headers separately in case we remove // some indexes (to keep underlying matrix data allocated) + if(rebalancingFactor_ > 1.0f) { for(int i=0; irelease(); - UASSERT(index_ == 0); - UASSERT(features.type() == CV_32FC1 || features.type() == CV_8UC1); - featuresType_ = features.type(); - featuresDim_ = features.cols; - useDistanceL1_ = useDistanceL1; - rebalancingFactor_ = rebalancingFactor; - - rtflann::KDTreeSingleIndexParams params(leafMaxSize, reorder); - - if(featuresType_ == CV_8UC1) - { - rtflann::Matrix dataset(features.data, features.rows, features.cols); - index_ = new rtflann::Index >(dataset, params); - ((rtflann::Index >*)index_)->buildIndex(); - } - else - { - rtflann::Matrix dataset((float*)features.data, features.rows, features.cols); - if(useDistanceL1_) - { - index_ = new rtflann::Index >(dataset, params); - ((rtflann::Index >*)index_)->buildIndex(); - } - else if(featuresDim_ <=3) - { - index_ = new rtflann::Index >(dataset, params); - ((rtflann::Index >*)index_)->buildIndex(); - } - else - { - index_ = new rtflann::Index >(dataset, params); - ((rtflann::Index >*)index_)->buildIndex(); - } - } - - // incremental FLANN: we should add all headers separately in case we remove - // some indexes (to keep underlying matrix data allocated) - if(rebalancingFactor_ > 1.0f) - { - for(int i=0; irelease(); - UASSERT(index_ == 0); - UASSERT(features.type() == CV_8UC1); - featuresType_ = features.type(); - featuresDim_ = features.cols; - useDistanceL1_ = true; - rebalancingFactor_ = rebalancingFactor; - - rtflann::Matrix dataset(features.data, features.rows, features.cols); - index_ = new rtflann::Index >(dataset, rtflann::LshIndexParams(12, 20, 2)); - ((rtflann::Index >*)index_)->buildIndex(); - - // incremental FLANN: we should add all headers separately in case we remove - // some indexes (to keep underlying matrix data allocated) - if(rebalancingFactor_ > 1.0f) - { - for(int i=0; i & poses) +bool GlobalMap::fullUpdateNeeded(const std::map & poses) const { - UDEBUG("Update (poses=%d addedNodes_=%d)", (int)poses.size(), (int)addedNodes_.size()); - - // First, check of the graph has changed. If so, re-create the octree by moving all occupied nodes. bool graphOptimized = false; // If a loop closure happened (e.g., poses are modified) bool graphChanged = addedNodes_.size()>0; // If the new map doesn't have any node from the previous map float updateErrorSqrd = updateError_*updateError_; - for(std::map::iterator iter=addedNodes_.begin(); iter!=addedNodes_.end(); ++iter) + for(std::map::const_iterator iter=addedNodes_.begin(); iter!=addedNodes_.end(); ++iter) { std::map::const_iterator jter = poses.find(iter->first); if(jter != poses.end()) @@ -125,7 +122,15 @@ bool GlobalMap::update(const std::map & poses) } } - if(graphOptimized || graphChanged) + return graphOptimized || graphChanged; +} + +bool GlobalMap::update(const std::map & poses) +{ + UDEBUG("Update (poses=%d addedNodes_=%d)", (int)poses.size(), (int)addedNodes_.size()); + + // First, check of the graph has changed. If so, re-create the octree by moving all occupied nodes. + if(fullUpdateNeeded(poses)) { // clear all but keep cache clear(); @@ -134,14 +139,25 @@ bool GlobalMap::update(const std::map & poses) std::list > orderedPoses; // add old poses that were not in the current map (they were just retrieved from LTM) + int nodesNotAssembled = 0; + int nodesNotInCache = 0; for(std::map::const_iterator iter=poses.lower_bound(1); iter!=poses.end(); ++iter) { if(!isNodeAssembled(iter->first)) { - UDEBUG("Pose %d not found in current added poses, it will be added to map", iter->first); - orderedPoses.push_back(*iter); + if(uContains(cache(), iter->first)) + { + ++nodesNotAssembled; + //UDEBUG("Pose %d not found in current added poses, it will be added to map", iter->first); + orderedPoses.push_back(*iter); + } + else + { + ++nodesNotInCache; + } } } + UDEBUG("%d nodes will be assembled in the map and %d nodes won't (no local grids in cache for them)", nodesNotAssembled, nodesNotInCache); // insert zero after if(poses.find(0) != poses.end()) diff --git a/corelib/src/Graph.cpp b/corelib/src/Graph.cpp index bf605853..05f2ac91 100644 --- a/corelib/src/Graph.cpp +++ b/corelib/src/Graph.cpp @@ -196,7 +196,7 @@ bool exportPoses( bool 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 + 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, 12=rgbd_bonn std::map & poses, std::multimap * constraints, // optional for formats 3 and 4 std::map * stamps) // optional for format 1 and 9 @@ -218,7 +218,14 @@ bool importPoses( else if(format == 4) // g2o { std::multimap constraintsTmp; - UERROR("Cannot import from g2o format because it is not yet supported!"); + if(OptimizerG2O::loadGraph(filePath, poses, constraintsTmp)) + { + if(constraints) + { + *constraints = constraintsTmp; + } + return true; + } return false; } else @@ -440,7 +447,7 @@ bool importPoses( UERROR("Error parsing \"%s\" with NewCollege format (should have 3 values: stamp x y, found %d)", str.c_str(), (int)strList.size()); } } - else if(format == 1 || format==10 || format==11) // rgbd-slam format + else if(format == 1 || format==10 || format==11 || format==12) // rgbd-slam format { std::list strList = uSplit(str); if((strList.size() >= 8 && format!=11) || (strList.size() == 9 && format==11)) @@ -451,9 +458,12 @@ bool importPoses( } double stamp = uStr2Double(strList.front()); strList.pop_front(); - if(format==11) + if(strList.size() == 8 && (format==10 || format==11 || format==12)) { - id = uStr2Int(strList.back()); + if(format==11) + { + id = uStr2Int(strList.back()); + } strList.pop_back(); } str = uJoin(strList, " "); @@ -481,6 +491,20 @@ bool importPoses( 1, 0, 0, 0); pose = t*pose; } + else if(format == 12) + { + // See https://www.ipb.uni-bonn.de/data/rgbd-dynamic-dataset/index.html + Transform T_ros(-1, 0, 0, 0, + 0, 0, 1, 0, + 0, 1, 0, 0); + Transform T_m( + 1.0157, 0.1828, -0.2389, 0.0113, + 0.0009, -0.8431, -0.6413, -0.00980, + -0.3009, 0.6147, -0.8085, 0.0111); + + // we remove the optical rotation + pose = T_ros*pose*T_ros*T_m*CameraModel::opticalRotation().inverse(); + } poses.insert(std::make_pair(id, pose)); } } @@ -910,21 +934,12 @@ Transform calcRMSE ( return t; } -void computeMaxGraphErrors( +MaxGraphErrors computeMaxGraphErrors( const std::map & poses, const std::multimap & links, - float & maxLinearErrorRatio, - float & maxAngularErrorRatio, - float & maxLinearError, - float & maxAngularError, - const Link ** maxLinearErrorLink, - const Link ** maxAngularErrorLink, bool force3DoF) { - maxLinearErrorRatio = -1; - maxAngularErrorRatio = -1; - maxLinearError = -1; - maxAngularError = -1; + MaxGraphErrors maxError; UDEBUG("poses=%d links=%d", (int)poses.size(), (int)links.size()); for(std::multimap::const_iterator iter=links.begin(); iter!=links.end(); ++iter) @@ -946,19 +961,7 @@ void computeMaxGraphErrors( iter->second.to(), t2.prettyPrint().c_str()); - if(maxLinearErrorLink) - { - *maxLinearErrorLink = 0; - } - if(maxAngularErrorLink) - { - *maxAngularErrorLink = 0; - } - maxLinearErrorRatio = -1; - maxAngularErrorRatio = -1; - maxLinearError = -1; - maxAngularError = -1; - return; + return MaxGraphErrors(); } Transform t; @@ -982,14 +985,11 @@ void computeMaxGraphErrors( UASSERT(iter->second.transVariance(false)>0.0); float stddevLinear = sqrt(iter->second.transVariance(false)); float linearErrorRatio = linearError/stddevLinear; - if(linearErrorRatio > maxLinearErrorRatio) + if(linearErrorRatio > maxError.linearRatio) { - maxLinearError = linearError; - maxLinearErrorRatio = linearErrorRatio; - if(maxLinearErrorLink) - { - *maxLinearErrorLink = &iter->second; - } + maxError.linear = linearError; + maxError.linearRatio = linearErrorRatio; + maxError.linearLink = iter->second; } // For landmark links, don't compute angular error if it doesn't estimate orientation @@ -1014,18 +1014,16 @@ void computeMaxGraphErrors( UASSERT(iter->second.rotVariance(false)>0.0); float stddevAngular = sqrt(iter->second.rotVariance(false)); float angularErrorRatio = angularError/stddevAngular; - if(angularErrorRatio > maxAngularErrorRatio) + if(angularErrorRatio > maxError.angularRatio) { - maxAngularError = angularError; - maxAngularErrorRatio = angularErrorRatio; - if(maxAngularErrorLink) - { - *maxAngularErrorLink = &iter->second; - } + maxError.angular = angularError; + maxError.angularRatio = angularErrorRatio; + maxError.angularLink = iter->second; } } } } + return maxError; } std::vector getMaxOdomInf(const std::multimap & links) @@ -2003,19 +2001,21 @@ std::list > computePath( bool lookInDatabase, bool updateNewCosts, float linearVelocity, // m/sec - float angularVelocity) // rad/sec + float angularVelocity, // rad/sec + bool ignoreDirectLinks) { UASSERT(memory!=0); UASSERT(fromId>=0); UASSERT(toId!=0); std::list > path; - UDEBUG("fromId=%d, toId=%d, lookInDatabase=%d, updateNewCosts=%d, linearVelocity=%f, angularVelocity=%f", + UDEBUG("fromId=%d, toId=%d, lookInDatabase=%d, updateNewCosts=%d, linearVelocity=%f, angularVelocity=%f ignoreDirectLinks=%d", fromId, toId, lookInDatabase?1:0, updateNewCosts?1:0, linearVelocity, - angularVelocity); + angularVelocity, + ignoreDirectLinks?1:0); std::multimap allLinks; if(lookInDatabase) @@ -2093,7 +2093,9 @@ std::list > computePath( } for(std::multimap::const_iterator iter = links.begin(); iter!=links.end(); ++iter) { - if(iter->second.from() != iter->second.to()) + if(iter->second.from() != iter->second.to() && + (!ignoreDirectLinks || + (!(iter->second.from()==fromId && iter->second.to()==toId) && !(iter->second.to()==fromId && iter->second.from()==toId)))) { Transform nextPose = currentNode->pose()*iter->second.transform(); float cost = 0.0f; @@ -2379,26 +2381,15 @@ std::map getPosesInRadius(const Transform & targetPose, const st float computePathLength( - const std::vector > & path, - unsigned int fromIndex, - unsigned int toIndex) + const std::vector > & path) { float length = 0.0f; if(path.size() > 1) { - UASSERT(fromIndex < path.size() && toIndex < path.size() && fromIndex <= toIndex); - if(fromIndex >= toIndex) + for(unsigned int i=0; i 1) { - float x=0, y=0, z=0; std::map::const_iterator iter=path.begin(); Transform previousPose = iter->second; ++iter; for(; iter!=path.end(); ++iter) { const Transform & currentPose = iter->second; - x += fabs(previousPose.x() - currentPose.x()); - y += fabs(previousPose.y() - currentPose.y()); - z += fabs(previousPose.z() - currentPose.z()); + length+=previousPose.getDistance(currentPose); previousPose = currentPose; } - length = sqrt(x*x + y*y + z*z); } return length; } diff --git a/corelib/src/IMUThread.cpp b/corelib/src/IMUThread.cpp index 58072ed8..778c5e42 100644 --- a/corelib/src/IMUThread.cpp +++ b/corelib/src/IMUThread.cpp @@ -135,8 +135,20 @@ void IMUThread::mainLoop() std::stringstream stream(line); std::string s; std::getline(stream, s, ','); - std::string nanoseconds = s.substr(s.size() - 9, 9); - std::string seconds = s.substr(0, s.size() - 9); + + double stamp = 0.0; + if(s.find('.') != std::string::npos) + { + // Normal [epoch] timestamp + stamp = uStr2Double(s); + } + else + { + // Assume EuRoC format + std::string nanoseconds = s.substr(s.size() - 9, 9); + std::string seconds = s.substr(0, s.size() - 9); + stamp = double(uStr2Int(seconds)) + double(uStr2Int(nanoseconds))*1e-9; + } cv::Vec3d gyr; for (int j = 0; j < 3; ++j) { @@ -150,7 +162,6 @@ void IMUThread::mainLoop() acc[j] = uStr2Double(s); } - double stamp = double(uStr2Int(seconds)) + double(uStr2Int(nanoseconds))*1e-9; if(previousStamp_>0 && stamp > previousStamp_) { captureDelay_ = stamp - previousStamp_; diff --git a/corelib/src/LaserScan.cpp b/corelib/src/LaserScan.cpp index 7e875a98..3f136e94 100644 --- a/corelib/src/LaserScan.cpp +++ b/corelib/src/LaserScan.cpp @@ -68,6 +68,9 @@ std::string LaserScan::formatName(const Format & format) case kXYZIT: name = "XYZIT"; break; + case kXYZIRT: + name = "XYZIRT"; + break; default: name = "Unknown"; break; @@ -96,6 +99,7 @@ int LaserScan::channels(const Format & format) break; case kXYZNormal: case kXYINormal: + case kXYZIRT: channels = 6; break; case kXYZINormal: @@ -123,11 +127,15 @@ bool LaserScan::isScanHasRGB(const Format & format) } bool LaserScan::isScanHasIntensity(const Format & format) { - return format==kXYZI || format==kXYZINormal || format == kXYI || format == kXYINormal || format==kXYZIT; + return format==kXYZI || format==kXYZINormal || format == kXYI || format == kXYINormal || format==kXYZIT || format==kXYZIRT; } bool LaserScan::isScanHasTime(const Format & format) { - return format==kXYZIT; + return format==kXYZIT || format==kXYZIRT; +} +bool LaserScan::isScanHasRing(const Format & format) +{ + return format==kXYZIRT; } LaserScan LaserScan::backwardCompatibility( @@ -404,7 +412,7 @@ void LaserScan::init( UASSERT_MSG(data.channels() != 3 || (data.channels() == 3 && (format == kXYZ || format == kXYI)), uFormat("format=%s", LaserScan::formatName(format).c_str()).c_str()); UASSERT_MSG(data.channels() != 4 || (data.channels() == 4 && (format == kXYZI || format == kXYZRGB)), uFormat("format=%s", LaserScan::formatName(format).c_str()).c_str()); UASSERT_MSG(data.channels() != 5 || (data.channels() == 5 && (format == kXYNormal || format == kXYZIT)), uFormat("format=%s", LaserScan::formatName(format).c_str()).c_str()); - UASSERT_MSG(data.channels() != 6 || (data.channels() == 6 && (format == kXYINormal || format == kXYZNormal)), uFormat("format=%s", LaserScan::formatName(format).c_str()).c_str()); + UASSERT_MSG(data.channels() != 6 || (data.channels() == 6 && (format == kXYINormal || format == kXYZNormal || format == kXYZIRT)), uFormat("format=%s", LaserScan::formatName(format).c_str()).c_str()); UASSERT_MSG(data.channels() != 7 || (data.channels() == 7 && (format == kXYZRGBNormal || format == kXYZINormal)), uFormat("format=%s", LaserScan::formatName(format).c_str()).c_str()); } } diff --git a/corelib/src/LocalGrid.cpp b/corelib/src/LocalGrid.cpp index 77e98781..22dbb920 100644 --- a/corelib/src/LocalGrid.cpp +++ b/corelib/src/LocalGrid.cpp @@ -64,8 +64,8 @@ void LocalGridCache::add(int nodeId, void LocalGridCache::add(int nodeId, const LocalGrid & localGrid) { - UDEBUG("nodeId=%d (ground=%d/%d obstacles=%d/%d empty=%d/%d)", - nodeId, localGrid.groundCells.cols, localGrid.groundCells.channels(), localGrid.obstacleCells.cols, localGrid.obstacleCells.channels(), localGrid.emptyCells.cols, localGrid.emptyCells.channels()); + //UDEBUG("nodeId=%d (ground=%d/%d obstacles=%d/%d empty=%d/%d)", + // nodeId, localGrid.groundCells.cols, localGrid.groundCells.channels(), localGrid.obstacleCells.cols, localGrid.obstacleCells.channels(), localGrid.emptyCells.cols, localGrid.emptyCells.channels()); if(nodeId < 0) { UWARN("Cannot add nodes with negative id (nodeId=%d)", nodeId); diff --git a/corelib/src/MarkerDetector.cpp b/corelib/src/MarkerDetector.cpp index 9ff9f694..14840827 100644 --- a/corelib/src/MarkerDetector.cpp +++ b/corelib/src/MarkerDetector.cpp @@ -28,17 +28,179 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include +#include + +#ifdef HAVE_OPENCV_ARUCO +#if CV_MAJOR_VERSION < 4 || (CV_MAJOR_VERSION == 4 && CV_MINOR_VERSION <8) +namespace cv{ + namespace aruco { + static const int DICT_ARUCO_MIP_36h12 = 21; +#if CV_MAJOR_VERSION < 4 || (CV_MAJOR_VERSION == 4 && CV_MINOR_VERSION <7) + typedef PREDEFINED_DICTIONARY_NAME PredefinedDictionaryType; +#endif + } +} +#endif +#endif + +#ifdef RTABMAP_APRILTAG +extern "C" { +#include "apriltag/apriltag.h" +#include "apriltag/apriltag_pose.h" +#ifdef RTABMAP_APRILTAG_WITH_ARUCO +#include "apriltag/aruco/tagAruco4x4_50.h" +#include "apriltag/aruco/tagAruco4x4_100.h" +#include "apriltag/aruco/tagAruco4x4_250.h" +#include "apriltag/aruco/tagAruco4x4_1000.h" +#include "apriltag/aruco/tagAruco5x5_50.h" +#include "apriltag/aruco/tagAruco5x5_100.h" +#include "apriltag/aruco/tagAruco5x5_250.h" +#include "apriltag/aruco/tagAruco5x5_1000.h" +#include "apriltag/aruco/tagAruco6x6_50.h" +#include "apriltag/aruco/tagAruco6x6_100.h" +#include "apriltag/aruco/tagAruco6x6_250.h" +#include "apriltag/aruco/tagAruco6x6_1000.h" +#include "apriltag/aruco/tagAruco7x7_50.h" +#include "apriltag/aruco/tagAruco7x7_100.h" +#include "apriltag/aruco/tagAruco7x7_250.h" +#include "apriltag/aruco/tagAruco7x7_1000.h" +#include "apriltag/aruco/tagArucoMIP36h12.h" +#endif +#include "apriltag/tag16h5.h" +#include "apriltag/tag25h9.h" +#include "apriltag/tag36h10.h" +#include "apriltag/tag36h11.h" +} + +#ifndef HAVE_OPENCV_ARUCO +// To match opencv::aruco module, add opencv::aruco dictionary enum +namespace cv{ +namespace aruco { +enum PredefinedDictionaryType { + DICT_4X4_50 = 0, + DICT_4X4_100, + DICT_4X4_250, + DICT_4X4_1000, + DICT_5X5_50, + DICT_5X5_100, + DICT_5X5_250, + DICT_5X5_1000, + DICT_6X6_50, + DICT_6X6_100, + DICT_6X6_250, + DICT_6X6_1000, + DICT_7X7_50, + DICT_7X7_100, + DICT_7X7_250, + DICT_7X7_1000, + DICT_ARUCO_ORIGINAL, + DICT_APRILTAG_16h5, + DICT_APRILTAG_25h9, + DICT_APRILTAG_36h10, + DICT_APRILTAG_36h11, + DICT_ARUCO_MIP_36h12 +}; +} +} +#endif +#endif namespace rtabmap { -MarkerDetector::MarkerDetector(const ParametersMap & parameters) +#ifdef RTABMAP_APRILTAG + +#define ARUCO_CASE(N, TOTAL) \ +case cv::aruco::DICT_##N##X##N##_##TOTAL: \ + af = tagAruco##N##x##N##_##TOTAL##_create(); \ + break; +#define APRILTAG_CASE(N1, N2) \ +case cv::aruco::DICT_APRILTAG_##N1##h##N2: \ + af = tag##N1##h##N2##_create(); \ + break; + +apriltag_family_t * createAprilTagPredefinedDictionary(int opencvArucoDictionary) +{ + apriltag_family_t * af = NULL; + switch(opencvArucoDictionary) + { + APRILTAG_CASE(16, 5) + APRILTAG_CASE(25, 9) + APRILTAG_CASE(36, 10) + APRILTAG_CASE(36, 11) +#ifdef RTABMAP_APRILTAG_WITH_ARUCO + ARUCO_CASE(4, 50) + ARUCO_CASE(4, 100) + ARUCO_CASE(4, 250) + ARUCO_CASE(4, 1000) + ARUCO_CASE(5, 50) + ARUCO_CASE(5, 100) + ARUCO_CASE(5, 250) + ARUCO_CASE(5, 1000) + ARUCO_CASE(6, 50) + ARUCO_CASE(6, 100) + ARUCO_CASE(6, 250) + ARUCO_CASE(6, 1000) + ARUCO_CASE(7, 50) + ARUCO_CASE(7, 100) + ARUCO_CASE(7, 250) + ARUCO_CASE(7, 1000) + case cv::aruco::DICT_ARUCO_MIP_36h12: + af = tagArucoMIP36h12_create(); + break; +#endif + default: + break; + } + return af; +} +void destroyAprilTagDictionary(apriltag_family_t * dictionary) +{ + if(dictionary == NULL) + { + return; + } + const char* name = dictionary->name; + if(strcmp(name, "tag16h5") == 0) tag16h5_destroy(dictionary); + else if(strcmp(name, "tag25h9") == 0) tag25h9_destroy(dictionary); + else if(strcmp(name, "tag36h10") == 0) tag36h10_destroy(dictionary); + else if(strcmp(name, "tag36h11") == 0) tag36h11_destroy(dictionary); +#ifdef RTABMAP_APRILTAG_WITH_ARUCO + else if(strcmp(name, "tagAruco4x4_50") == 0) tagAruco4x4_50_destroy(dictionary); + else if(strcmp(name, "tagAruco4x4_100") == 0) tagAruco4x4_100_destroy(dictionary); + else if(strcmp(name, "tagAruco4x4_250") == 0) tagAruco4x4_250_destroy(dictionary); + else if(strcmp(name, "tagAruco4x4_1000") == 0) tagAruco4x4_1000_destroy(dictionary); + else if(strcmp(name, "tagAruco5x5_50") == 0) tagAruco5x5_50_destroy(dictionary); + else if(strcmp(name, "tagAruco5x5_100") == 0) tagAruco5x5_100_destroy(dictionary); + else if(strcmp(name, "tagAruco5x5_250") == 0) tagAruco5x5_250_destroy(dictionary); + else if(strcmp(name, "tagAruco5x5_1000") == 0) tagAruco5x5_1000_destroy(dictionary); + else if(strcmp(name, "tagAruco6x6_50") == 0) tagAruco6x6_50_destroy(dictionary); + else if(strcmp(name, "tagAruco6x6_100") == 0) tagAruco6x6_100_destroy(dictionary); + else if(strcmp(name, "tagAruco6x6_250") == 0) tagAruco6x6_250_destroy(dictionary); + else if(strcmp(name, "tagAruco6x6_1000") == 0) tagAruco6x6_1000_destroy(dictionary); + else if(strcmp(name, "tagAruco7x7_50") == 0) tagAruco7x7_50_destroy(dictionary); + else if(strcmp(name, "tagAruco7x7_100") == 0) tagAruco7x7_100_destroy(dictionary); + else if(strcmp(name, "tagAruco7x7_250") == 0) tagAruco7x7_250_destroy(dictionary); + else if(strcmp(name, "tagAruco7x7_1000") == 0) tagAruco7x7_1000_destroy(dictionary); + else if(strcmp(name, "tagArucoMIP_36h12") == 0) tagArucoMIP36h12_destroy(dictionary); +#endif + else + { + UFATAL("AprilTag: Didn't find the right destructor for dictionary \"%s\"", name); + } +} +#endif + +MarkerDetector::MarkerDetector(const ParametersMap & parameters) : + strategy_((Strategy)Parameters::defaultMarkerStrategy()), + markerLength_(Parameters::defaultMarkerLength()), + maxDepthError_(Parameters::defaultMarkerMaxDepthError()), + maxRange_(Parameters::defaultMarkerMaxRange()), + minRange_(Parameters::defaultMarkerMinRange()), + dictionaryId_(Parameters::defaultMarkerDictionary()), + apriltagLibDetector_(NULL), + apriltagLibFamily_(NULL) { #ifdef HAVE_OPENCV_ARUCO - markerLength_ = Parameters::defaultMarkerLength(); - maxDepthError_ = Parameters::defaultMarkerMaxDepthError(); - maxRange_ = Parameters::defaultMarkerMaxRange(); - minRange_ = Parameters::defaultMarkerMinRange(); - dictionaryId_ = Parameters::defaultMarkerDictionary(); #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) @@ -47,22 +209,107 @@ MarkerDetector::MarkerDetector(const ParametersMap & parameters) detectorParams_.reset(new cv::aruco::DetectorParameters()); #endif #if CV_MAJOR_VERSION > 4 || (CV_MAJOR_VERSION == 4 && CV_MINOR_VERSION >= 7) - detectorParams_->cornerRefinementMethod = (cv::aruco::CornerRefineMethod) Parameters::defaultMarkerCornerRefinementMethod(); + detectorParams_->cornerRefinementMethod = (cv::aruco::CornerRefineMethod) Parameters::defaultMarkerOpenCVCornerRefinementMethod(); #elif CV_MAJOR_VERSION > 3 || (CV_MAJOR_VERSION == 3 && CV_MINOR_VERSION >=3) - detectorParams_->cornerRefinementMethod = Parameters::defaultMarkerCornerRefinementMethod(); + detectorParams_->cornerRefinementMethod = Parameters::defaultMarkerOpenCVCornerRefinementMethod(); #else - detectorParams_->doCornerRefinement = Parameters::defaultMarkerCornerRefinementMethod()!=0; + detectorParams_->doCornerRefinement = Parameters::defaultMarkerOpenCVCornerRefinementMethod()!=0; #endif +#endif + parseParameters(parameters); -#endif } MarkerDetector::~MarkerDetector() { - +#ifdef RTABMAP_APRILTAG + if(apriltagLibDetector_) + { + apriltag_detector_destroy(((apriltag_detector_t*)apriltagLibDetector_)); + } + if(apriltagLibFamily_) + { + destroyAprilTagDictionary((apriltag_family_t*)apriltagLibFamily_); + } +#endif } void MarkerDetector::parseParameters(const ParametersMap & parameters) { + int strategy = strategy_; + Parameters::parse(parameters, Parameters::kMarkerStrategy(), strategy); + strategy_ = (Strategy)strategy; + Parameters::parse(parameters, Parameters::kMarkerLength(), markerLength_); + Parameters::parse(parameters, Parameters::kMarkerMaxDepthError(), maxDepthError_); + Parameters::parse(parameters, Parameters::kMarkerMaxRange(), maxRange_); + Parameters::parse(parameters, Parameters::kMarkerMinRange(), minRange_); + Parameters::parse(parameters, Parameters::kMarkerDictionary(), dictionaryId_); + + if(parameters.find(Parameters::kMarkerLengths()) != parameters.end()) + { + markerLengths_.clear(); + std::string strLengths; + Parameters::parse(parameters, Parameters::kMarkerLengths(), strLengths); + std::list strList = uSplit(strLengths, '|'); + for(std::list::iterator iter=strList.begin(); iter!=strList.end(); ++iter) + { + std::list items = uSplit(*iter, ' '); + if(items.size() != 2) + { + UERROR("Invalid string format \"%s\" for parameter %s, make " + "sure the values are separated by single space and/or '|'. See " + "description of the parameter for example. That parameter " + "will be ignored.", + strLengths.c_str(), + Parameters::kMarkerLengths().c_str()); + markerLengths_.clear(); + break; + } + else + { + float length = uStr2Float(items.back()); + if(uStrContains(items.front(), ":")) + { + std::list range = uSplit(items.front(), ':'); + if(range.size() != 2) + { + UERROR("Invalid string format \"%s\" for parameter %s, make " + "sure the values are separated by single space and/or '|'. See " + "description of the parameter for example. That parameter " + "will be ignored.", + strLengths.c_str(), + Parameters::kMarkerLengths().c_str()); + markerLengths_.clear(); + break; + } + int id1 = uStr2Int(range.front()); + int id2 = uStr2Int(range.back()); + UDEBUG("Adding a range of markers %d -> %d with length %f", id1, id2, length); + for(int id=id1; id<=id2; ++id) + { + auto inserted = markerLengths_.insert(std::make_pair(id, length)); + if(!inserted.second) + { + UWARN("Marker %d is already configured with length %f, not overwriting", inserted.first->first, inserted.first->second); + } + } + } + else { + int id = uStr2Int(items.front()); + auto inserted = markerLengths_.insert(std::make_pair(id, length)); + if(!inserted.second) + { + UWARN("Marker %d is already configured with length %f, overwriting with %f", inserted.first->first, inserted.first->second, length); + inserted.first->second = length; + } + else + { + UDEBUG("Added marker %d with length %f", id, length); + } + } + } + } + } + #ifdef HAVE_OPENCV_ARUCO detectorParams_->adaptiveThreshWinSizeMin = 3; detectorParams_->adaptiveThreshWinSizeMax = 23; @@ -76,13 +323,13 @@ void MarkerDetector::parseParameters(const ParametersMap & parameters) detectorParams_->minMarkerDistanceRate = 0.05; #if CV_MAJOR_VERSION > 4 || (CV_MAJOR_VERSION == 4 && CV_MINOR_VERSION >= 7) int cornerRefinementMethod; - Parameters::parse(parameters, Parameters::kMarkerCornerRefinementMethod(), cornerRefinementMethod); + Parameters::parse(parameters, Parameters::kMarkerOpenCVCornerRefinementMethod(), 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); + Parameters::parse(parameters, Parameters::kMarkerOpenCVCornerRefinementMethod(), detectorParams_->cornerRefinementMethod); #else int doCornerRefinement = detectorParams_->doCornerRefinement?1:0; - Parameters::parse(parameters, Parameters::kMarkerCornerRefinementMethod(), doCornerRefinement); + Parameters::parse(parameters, Parameters::kMarkerOpenCVCornerRefinementMethod(), doCornerRefinement); detectorParams_->doCornerRefinement = doCornerRefinement!=0; #endif detectorParams_->cornerRefinementWinSize = 5; @@ -95,13 +342,9 @@ void MarkerDetector::parseParameters(const ParametersMap & parameters) detectorParams_->minOtsuStdDev = 5.0; detectorParams_->errorCorrectionRate = 0.6; - Parameters::parse(parameters, Parameters::kMarkerLength(), markerLength_); - Parameters::parse(parameters, Parameters::kMarkerMaxDepthError(), maxDepthError_); - Parameters::parse(parameters, Parameters::kMarkerMaxRange(), maxRange_); - Parameters::parse(parameters, Parameters::kMarkerMinRange(), minRange_); - Parameters::parse(parameters, Parameters::kMarkerDictionary(), dictionaryId_); + #if CV_MAJOR_VERSION < 3 || (CV_MAJOR_VERSION == 3 && (CV_MINOR_VERSION <4 || (CV_MINOR_VERSION ==4 && CV_SUBMINOR_VERSION<2))) - if(dictionaryId_ >= 17) + if(dictionaryId_ = 17 && strategy_ == 0) { UERROR("Cannot set AprilTag dictionary. OpenCV version should be at least 3.4.2, " "current version is %s. Setting %s to default (%d)", @@ -110,21 +353,108 @@ void MarkerDetector::parseParameters(const ParametersMap & parameters) Parameters::defaultMarkerDictionary()); dictionaryId_ = Parameters::defaultMarkerDictionary(); } +#elif CV_MAJOR_VERSION < 4 || (CV_MAJOR_VERSION == 4 && CV_MINOR_VERSION <8) + if(dictionaryId_ == 21 && strategy_ == 0) + { + UERROR("Cannot set ARUCO_MIP_36h12 dictionary. OpenCV version should be at least 4.8.0, " + "current version is %s. Setting %s to default (%d)", + CV_VERSION, + Parameters::kMarkerDictionary().c_str(), + Parameters::defaultMarkerDictionary()); + dictionaryId_ = Parameters::defaultMarkerDictionary(); + } #endif #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_)); + dictionary_ = cv::aruco::getPredefinedDictionary(cv::aruco::PredefinedDictionaryType(dictionaryId_)); #else dictionary_.reset(new cv::aruco::Dictionary()); - *dictionary_ = cv::aruco::getPredefinedDictionary(cv::aruco::PREDEFINED_DICTIONARY_NAME(dictionaryId_)); + *dictionary_ = cv::aruco::getPredefinedDictionary(cv::aruco::PredefinedDictionaryType(dictionaryId_)); #endif +#else + if(strategy_ == 0) + { +#ifdef RTABMAP_APRILTAG + UERROR("RTAB-Map is not built with opencv-aruco library! Fallback to AprilTag strategy (%s=1).", Parameters::kMarkerStrategy().c_str()); + strategy_ = kStrategyApriltag; +#else + UERROR("RTAB-Map is not built with opencv-aruco library!"); +#endif + } +#endif + +#ifdef RTABMAP_APRILTAG + if(apriltagLibDetector_ && apriltagLibFamily_) { + apriltag_detector_remove_family(((apriltag_detector_t*)apriltagLibDetector_), (apriltag_family_t*)apriltagLibFamily_); + } + if(apriltagLibFamily_) + { + destroyAprilTagDictionary((apriltag_family_t*)apriltagLibFamily_); + apriltagLibFamily_ = NULL; + } + if(strategy_ == 1) + { + if(apriltagLibDetector_ == NULL) + { + apriltagLibDetector_ = apriltag_detector_create(); + } + Parameters::parse(parameters, Parameters::kMarkerAprilTagNThreads(), ((apriltag_detector_t*)apriltagLibDetector_)->nthreads); + Parameters::parse(parameters, Parameters::kMarkerAprilTagQuadDecimate(), ((apriltag_detector_t*)apriltagLibDetector_)->quad_decimate); + Parameters::parse(parameters, Parameters::kMarkerAprilTagQuadSigma(), ((apriltag_detector_t*)apriltagLibDetector_)->quad_sigma); + Parameters::parse(parameters, Parameters::kMarkerAprilTagRefineEdges(), ((apriltag_detector_t*)apriltagLibDetector_)->refine_edges); + Parameters::parse(parameters, Parameters::kMarkerAprilTagDecodeSharpening(), ((apriltag_detector_t*)apriltagLibDetector_)->decode_sharpening); + Parameters::parse(parameters, Parameters::kMarkerAprilTagDebug(), ((apriltag_detector_t*)apriltagLibDetector_)->debug); + + cv::aruco::PredefinedDictionaryType dictFamily = cv::aruco::PredefinedDictionaryType(dictionaryId_); +#ifndef RTABMAP_APRILTAG_WITH_ARUCO + if((dictFamily >= cv::aruco::DICT_4X4_50 && dictFamily < cv::aruco::DICT_APRILTAG_16h5) || dictFamily==cv::aruco::DICT_ARUCO_MIP_36h12) + { + UERROR("Cannot set aruco dictionaries with current AprilTag library version. " + "Use opencv-aruco implementation instead or rebuild/install latest " + "AprilTag from source. Setting %s to default apriltag dictionary (%d)", + Parameters::kMarkerDictionary().c_str(), + cv::aruco::DICT_APRILTAG_36h11); + dictionaryId_ = cv::aruco::DICT_APRILTAG_36h11; + dictFamily = cv::aruco::PredefinedDictionaryType(dictionaryId_); + } +#endif + if(dictFamily == cv::aruco::DICT_ARUCO_ORIGINAL) + { + UERROR("Cannot set ARUCO_ORIGINAL (%d) dictionary with AprilTag library implementation. " + "Use opencv-aruco implementation instead. Setting %s to default apriltag dictionary (%d)", + dictionaryId_, + Parameters::kMarkerDictionary().c_str(), + cv::aruco::DICT_APRILTAG_36h11); + dictionaryId_ = cv::aruco::DICT_APRILTAG_36h11; + dictFamily = cv::aruco::PredefinedDictionaryType(dictionaryId_); + } + apriltagLibFamily_ = createAprilTagPredefinedDictionary(dictFamily); + UASSERT_MSG(apriltagLibFamily_ != NULL, uFormat("AprilTag library cannot be used with dictionary type %d", (int)dictionaryId_).c_str()); + errno = 0; + apriltag_detector_add_family(((apriltag_detector_t*)apriltagLibDetector_), (apriltag_family_t*)apriltagLibFamily_); + if (errno == ENOMEM) { + UFATAL("Unable to add family to detector due to insufficient memory to allocate the tag-family decoder with the default maximum hamming value of 2. Try choosing an alternative tag family."); + } + } +#else + if(strategy_ == 1) + { +#ifdef HAVE_OPENCV_ARUCO + UERROR("RTAB-Map is not built with apriltag library! Fallback to OpenCV (%s=0).", Parameters::kMarkerStrategy().c_str()); + strategy_ = kStrategyOpencv; +#else + UERROR("RTAB-Map is not built with apriltag library!"); +#endif + } #endif } +// deprecated std::map MarkerDetector::detect(const cv::Mat & image, const CameraModel & model, const cv::Mat & depth, float * markerLengthOut, cv::Mat * imageWithDetections) { + UDEBUG(""); std::map detections; std::map infos = detect(image, model, depth, std::map(), imageWithDetections); @@ -144,221 +474,404 @@ std::map MarkerDetector::detect(const cv::Mat & image, const Cam std::map MarkerDetector::detect(const cv::Mat & image, const std::vector & models, const cv::Mat & depth, - const std::map & markerLengths, + const std::map & extraMarkerLengths, cv::Mat * imageWithDetections) { UASSERT(!models.empty() && !image.empty()); UASSERT(int((image.cols/models.size())*models.size()) == image.cols); UASSERT(int((depth.cols/models.size())*models.size()) == depth.cols); int subRGBWidth = image.cols/models.size(); - int subDepthWidth = depth.cols/models.size(); - std::map allInfo; - for(size_t i=0; i subInfo = detect(subImage, model, subDepth, markerLengths, imageWithDetections?&subImageWithDetections:0); - if(ULogger::level() >= ULogger::kWarning) + rgbToDepthFactorX = float(depth.cols) / float(image.cols); + rgbToDepthFactorY = float(depth.rows) / float(image.rows); + } + else if(markerLength_ == 0) + { + if(depth.empty()) { - for(std::map::iterator iter=subInfo.begin(); iter!=subInfo.end(); ++iter) - { - std::pair::iterator, bool> inserted = allInfo.insert(*iter); - if(!inserted.second) - { - UWARN("Marker %d already added by another camera, ignoring detection from camera %d", iter->first, i); - } - } - } - else - { - allInfo.insert(subInfo.begin(), subInfo.end()); - } - if(imageWithDetections) - { - if(i==0) - { - *imageWithDetections = cv::Mat(image.size(), subImageWithDetections.type()); - } - if(!subImageWithDetections.empty()) - { - subImageWithDetections.copyTo(cv::Mat(*imageWithDetections, cv::Rect(subRGBWidth*i, 0, subRGBWidth, image.rows))); - } + UERROR("Depth image is empty, please set %s parameter to non-null.", Parameters::kMarkerLength().c_str()); + return std::map(); } } - return allInfo; -} - -std::map MarkerDetector::detect(const cv::Mat & image, - const CameraModel & model, - const cv::Mat & depth, - const std::map & markerLengths, - cv::Mat * imageWithDetections) -{ - if(!image.empty() && image.cols != model.imageWidth()) - { - UERROR("This method cannot handle multi-camera marker detection, use the other function version supporting it."); - return std::map(); - } - - std::map detections; - -#ifdef HAVE_OPENCV_ARUCO std::vector< int > ids; - std::vector< std::vector< cv::Point2f > > corners, rejected; - std::vector< cv::Vec3d > rvecs, tvecs; + std::vector< int > cams; + std::vector< std::vector< cv::Point2f > > corners; // in stitched image + std::vector poses; // in optical frame // detect markers and estimate pose -#if CV_MAJOR_VERSION > 3 || (CV_MAJOR_VERSION == 3 && CV_MINOR_VERSION >=2) - cv::aruco::detectMarkers(image, dictionary_, corners, ids, detectorParams_, rejected); -#else - cv::aruco::detectMarkers(image, *dictionary_, corners, ids, *detectorParams_, rejected); -#endif - UDEBUG("Markers detected=%d rejected=%d", (int)ids.size(), (int)rejected.size()); - if(ids.size() > 0) + if(strategy_ == kStrategyApriltag) { - float rgbToDepthFactorX = 1.0f; - float rgbToDepthFactorY = 1.0f; - if(!depth.empty()) +#ifdef RTABMAP_APRILTAG + cv::Mat grayImage = image; + if(image.channels() > 1) { - rgbToDepthFactorX = 1.0f/(model.imageWidth()>0?float(model.imageWidth())/float(depth.cols):1.0f); - rgbToDepthFactorY = 1.0f/(model.imageHeight()>0?float(model.imageHeight())/float(depth.rows):1.0f); + cv::cvtColor(image, grayImage, cv::COLOR_BGR2GRAY); } - else if(markerLength_ == 0) - { - if(depth.empty()) - { - UERROR("Depth image is empty, please set %s parameter to non-null.", Parameters::kMarkerLength().c_str()); - return detections; - } - } + image_u8_t im = {grayImage.cols, grayImage.rows, (int)grayImage.step, grayImage.data}; - cv::aruco::estimatePoseSingleMarkers(corners, markerLength_<=0.0?1.0f:markerLength_, model.K(), model.D(), rvecs, tvecs); - std::vector scales; - for(size_t i=0; i::const_iterator findIter = markerLengths.find(ids[i]); - if(!depth.empty() && (markerLength_ == 0 || (markerLength_<0 && findIter==markerLengths.end()))) + errno = 0; + zarray_t *apriltagDetections = apriltag_detector_detect(((apriltag_detector_t*)apriltagLibDetector_), &im); + if (errno == EAGAIN) { + UERROR("Unable to create the %d threads requested.", ((apriltag_detector_t*)apriltagLibDetector_)->nthreads); + if(apriltagDetections) { - float d = util2d::getDepth(depth, (corners[i][0].x + (corners[i][2].x-corners[i][0].x)/2.0f)*rgbToDepthFactorX, (corners[i][0].y + (corners[i][2].y-corners[i][0].y)/2.0f)*rgbToDepthFactorY, true, 0.02f, true); - float d1 = util2d::getDepth(depth, corners[i][0].x*rgbToDepthFactorX, corners[i][0].y*rgbToDepthFactorY, true, 0.02f, true); - float d2 = util2d::getDepth(depth, corners[i][1].x*rgbToDepthFactorX, corners[i][1].y*rgbToDepthFactorY, true, 0.02f, true); - float d3 = util2d::getDepth(depth, corners[i][2].x*rgbToDepthFactorX, corners[i][2].y*rgbToDepthFactorY, true, 0.02f, true); - float d4 = util2d::getDepth(depth, corners[i][3].x*rgbToDepthFactorX, corners[i][3].y*rgbToDepthFactorY, true, 0.02f, true); - // Accept measurement only if all 4 depth values are valid and - // they are at the same depth (camera should be perpendicular to marker for - // best depth estimation) - if(d>0 && d1>0 && d2>0 && d3>0 && d4>0) + apriltag_detections_destroy(apriltagDetections); + } + return std::map(); + } + + std::set idsAdded; + for (int i = 0; i < zarray_size(apriltagDetections); i++) + { + apriltag_detection_t *det; + zarray_get(apriltagDetections, i, &det); + + // Which camera? + int cameraIndex = int(det->c[0]) / subRGBWidth; + if(cameraIndex<0 || cameraIndex>=(int)models.size()) + { + UWARN("Marker %d detected outside the image! camera index=%d (models=%ld subWidth=%d) marker center=(%f,%f). Ignoring...", + det->id, cameraIndex, models.size(), subRGBWidth, det->c[0], det->c[1]); + continue; + } + + if(!markerLengths_.empty() && + markerLengths_.find(det->id) == markerLengths_.end() && + extraMarkerLengths.find(det->id) == extraMarkerLengths.end()) + { + UDEBUG("Ignoring marker %d because it is not in the list of expected markers (see %s)", + det->id, + Parameters::kMarkerLengths().c_str()); + continue; + } + if(idsAdded.find(det->id)!=idsAdded.end()) + { + UDEBUG("Marker %d already added by another camera, ignoring detection from camera %d", det->id, cameraIndex); + continue; + } + + const CameraModel & model = models[cameraIndex]; + float offsetX = cameraIndex*subRGBWidth; + + // Convert detection in local camera + det->c[0] -= offsetX; + for(int i=0; i<4; ++i) + { + det->p[i][0] -= offsetX; + } + + // Shift the homography to match the local-camera pixel frame: + // pre-multiply H by T = [[1,0,-offsetX],[0,1,0],[0,0,1]] so it + // stays consistent with the shifted corners. estimate_tag_pose() + // seeds its iterative refinement from H, so leaving it stale + // produces a bad initial guess (often triggers the AprilTag + // "more than one new minimum found" debug print). + MATD_EL(det->H, 0, 0) -= offsetX * MATD_EL(det->H, 2, 0); + MATD_EL(det->H, 0, 1) -= offsetX * MATD_EL(det->H, 2, 1); + MATD_EL(det->H, 0, 2) -= offsetX * MATD_EL(det->H, 2, 2); + + apriltag_detection_info_t info; + info.det = det; + info.tagsize = 1.0f; + info.fx = model.fx(); + info.fy = model.fy(); + info.cx = model.cx(); + info.cy = model.cy(); + + // Then call estimate_tag_pose. + apriltag_pose_t pose; + double err = estimate_tag_pose(&info, &pose); + + if (pose.R && pose.t) + { + if(MATD_EL(pose.t, 2, 0) > 0) { - float scale = d/tvecs[i].val[2]; - - if( fabs(d-d1) < maxDepthError_ && - fabs(d-d2) < maxDepthError_ && - fabs(d-d3) < maxDepthError_ && - fabs(d-d4) < maxDepthError_) + Transform t(MATD_EL(pose.R, 0, 0), MATD_EL(pose.R, 0, 1), MATD_EL(pose.R, 0, 2), MATD_EL(pose.t, 0, 0), + MATD_EL(pose.R, 1, 0), MATD_EL(pose.R, 1, 1), MATD_EL(pose.R, 1, 2), MATD_EL(pose.t, 1, 0), + MATD_EL(pose.R, 2, 0), MATD_EL(pose.R, 2, 1), MATD_EL(pose.R, 2, 2), MATD_EL(pose.t, 2, 0)); + poses.push_back(t*Transform(-1,0,0,0, 0,1,0,0, 0,0,-1,0)); // Flip tag to match same frame than OpenCV (+x->-x, +z->-z) + + corners.push_back(std::vector(4)); + for(int i=0; i<4; ++i) { - length = scale; - scales.push_back(length); - tvecs[i] *= scales.back(); - UWARN("Automatic marker length estimation: id=%d depth=%fm length=%fm", ids[i], d, scales.back()); - } - else - { - UWARN("The four marker's corners should be " - "perpendicular to camera to estimate correctly " - "the marker's length. Errors: %f, %f, %f > %fm (%s). Four corners: %f %f %f %f (middle=%f). " - "Parameter %s can be set to non-null to skip automatic " - "marker length estimation. Detections are ignored.", - fabs(d1-d2), fabs(d1-d3), fabs(d1-d4), maxDepthError_, Parameters::kMarkerMaxDepthError().c_str(), - d1, d2, d3, d4, d, - Parameters::kMarkerLength().c_str()); - continue; + // reconvert in original stitched image + corners.back()[i].x = det->p[i][0]+offsetX; + corners.back()[i].y = det->p[i][1]; } + ids.push_back(det->id); + cams.push_back(cameraIndex); + UDEBUG("Add marker %d (err = %f)", det->id, err); + idsAdded.insert(det->id); } else { - UWARN("Some depth values (%f,%f,%f,%f, middle=%f) cannot be detected on the " - "marker's corners, cannot initialize marker length. " - "Parameter %s can be set to non-null to skip automatic " - "marker length estimation. Detections are ignored.", - d1,d2,d3,d4,d, - Parameters::kMarkerLength().c_str()); - continue; + UWARN("Skipping %d because its estimated pose is behind the camera %d", det->id, cameraIndex); } } - else if(markerLength_ < 0) - { - if(findIter!=markerLengths.end()) - { - length = findIter->second; - tvecs[i] *= length; - } - else - { - UWARN("Cannot find marker length for marker %d, ignoring this marker (count=%d)", ids[i], (int)markerLengths.size()); - continue; - } - } - else if(markerLength_ > 0) - { - length = markerLength_; - } - else - { - continue; - } - - // Limit the detection range to be between the min / max range. - // If the ranges are -1, allow any detection within that direction. - if((maxRange_ <= 0 || tvecs[i].val[2] < maxRange_) && - (minRange_ <= 0 || tvecs[i].val[2] > minRange_)) + else { + UWARN("Failed to compute pose for marker %d, ignoring...", det->id); + } + + // Free pose memory + if (pose.R) matd_destroy(pose.R); + if (pose.t) matd_destroy(pose.t); + } + + apriltag_detections_destroy(apriltagDetections); +#else + UERROR("RTAB-Map is not built with apriltag library."); +#endif + } + else // opencv + { + std::vector< int > cvIds; + std::vector< std::vector< cv::Point2f > > cvCorners, cvRejected; +#ifdef HAVE_OPENCV_ARUCO +#if CV_MAJOR_VERSION > 3 || (CV_MAJOR_VERSION == 3 && CV_MINOR_VERSION >=2) + cv::aruco::detectMarkers(image, dictionary_, cvCorners, cvIds, detectorParams_, cvRejected); +#else + cv::aruco::detectMarkers(image, *dictionary_, cvCorners, cvIds, *detectorParams_, cvRejected); +#endif + UDEBUG("Markers detected=%d rejected=%d", (int)cvIds.size(), (int)cvRejected.size()); + + // split detections per camera + std::set idsAdded; + std::vector< std::vector< std::vector< cv::Point2f > > > cvCornersPerCam(models.size()); + std::vector< std::vector< int > > cvIdsPerCam(models.size()); + for(size_t i=0; i=(int)models.size()) + { + UWARN("Marker %d detected outside the image! camera index=%d (models=%ld subWidth=%d) marker center=(%f,%f). Ignoring...", + id, cameraIndex, models.size(), subRGBWidth, cvCorners[i][0].x, cvCorners[i][0].y); + continue; + } + + if(!markerLengths_.empty() && + markerLengths_.find(id) == markerLengths_.end() && + extraMarkerLengths.find(id) == extraMarkerLengths.end()) + { + UDEBUG("Ignoring marker %d because it is not in the list of expected markers (see %s)", + id, + Parameters::kMarkerLengths().c_str()); + continue; + } + if(idsAdded.find(id) != idsAdded.end()) + { + UDEBUG("Marker %d already added by another camera, ignoring detection from camera %d", id, cameraIndex); + continue; + } + + float offsetX = cameraIndex*subRGBWidth; + for(size_t j=0; j rvecs, tvecs; + const CameraModel & model = models[cam]; + cv::aruco::estimatePoseSingleMarkers(cvCornersPerCam[cam], 1.0f, model.K(), model.D(), rvecs, tvecs); + float offsetX = cam*subRGBWidth; + for(size_t i=0; i(0,0), R.at(0,1), R.at(0,2), tvecs[i].val[0], - R.at(1,0), R.at(1,1), R.at(1,2), tvecs[i].val[1], - R.at(2,0), R.at(2,1), R.at(2,2), tvecs[i].val[2]); - Transform pose = model.localTransform() * t; - detections.insert(std::make_pair(ids[i], MarkerInfo(ids[i], length, pose))); - UDEBUG("Marker %d detected in base_link: %s, optical_link=%s, local transform=%s", ids[i], pose.prettyPrint().c_str(), t.prettyPrint().c_str(), model.localTransform().prettyPrint().c_str()); - } - } - if(markerLength_ == 0 && !scales.empty()) - { - float sum = 0.0f; - float maxError = 0.0f; - for(size_t i=0; i0) + R.at(1,0), R.at(1,1), R.at(1,2), tvecs[i].val[1], + R.at(2,0), R.at(2,1), R.at(2,2), tvecs[i].val[2]); + poses.push_back(t); + ids.push_back(cvIdsPerCam[cam][i]); + cams.push_back(cam); + // reconvert in original stitched image + for(size_t j=0; j 0.001f) - { - UWARN("The marker's length detected between 2 of the " - "markers doesn't match (%fm vs %fm)." - "Parameter %s can be set to non-null to skip automatic " - "marker length estimation. Detections are ignored.", - scales[i], scales[0], - Parameters::kMarkerLength().c_str()); - detections.clear(); - return detections; - } - if(error > maxError) - { - maxError = error; - } + cvCornersPerCam[cam][i][j].x += offsetX; } - sum += scales[i]; + corners.push_back(cvCornersPerCam[cam][i]); + } + } +#else + UERROR("RTAB-Map is not built with \"aruco\" module from OpenCV."); +#endif + } + + // Estimate scale and fill up output detections + std::vector scales; + std::map detections; + for(size_t i=0; i::const_iterator findIter = extraMarkerLengths.find(ids[i]); + if(markerLengths_.empty() && !depth.empty() && (markerLength_ == 0 || (markerLength_<0 && findIter==extraMarkerLengths.end()))) + { + float d = util2d::getDepth(depth, (corners[i][0].x + (corners[i][2].x-corners[i][0].x)/2.0f)*rgbToDepthFactorX, (corners[i][0].y + (corners[i][2].y-corners[i][0].y)/2.0f)*rgbToDepthFactorY, true, 0.02f, true); + float d1 = util2d::getDepth(depth, corners[i][0].x*rgbToDepthFactorX, corners[i][0].y*rgbToDepthFactorY, true, 0.02f, true); + float d2 = util2d::getDepth(depth, corners[i][1].x*rgbToDepthFactorX, corners[i][1].y*rgbToDepthFactorY, true, 0.02f, true); + float d3 = util2d::getDepth(depth, corners[i][2].x*rgbToDepthFactorX, corners[i][2].y*rgbToDepthFactorY, true, 0.02f, true); + float d4 = util2d::getDepth(depth, corners[i][3].x*rgbToDepthFactorX, corners[i][3].y*rgbToDepthFactorY, true, 0.02f, true); + // Accept measurement only if all 4 depth values are valid and + // they are at the same depth (camera should be perpendicular to marker for + // best depth estimation) + if(d>0 && d1>0 && d2>0 && d3>0 && d4>0) + { + float scale = d / poses[i].z(); + + if( fabs(d-d1) < maxDepthError_ && + fabs(d-d2) < maxDepthError_ && + fabs(d-d3) < maxDepthError_ && + fabs(d-d4) < maxDepthError_) + { + length = scale; + scales.push_back(length); + UWARN("Automatic marker length estimation: id=%d depth=%fm length=%fm", ids[i], d, length); + } + else + { + UWARN("The four marker's corners should be " + "perpendicular to camera to estimate correctly " + "the marker's length. Errors: %f, %f, %f > %fm (%s). Four corners: %f %f %f %f (middle=%f). " + "Parameter %s can be set to non-null to skip automatic " + "marker length estimation. Detections are ignored.", + fabs(d1-d2), fabs(d1-d3), fabs(d1-d4), maxDepthError_, Parameters::kMarkerMaxDepthError().c_str(), + d1, d2, d3, d4, d, + Parameters::kMarkerLength().c_str()); + continue; + } + } + else + { + UWARN("Some depth values (%f,%f,%f,%f, middle=%f) cannot be detected on the " + "marker's corners, cannot initialize marker length. " + "Parameter %s can be set to non-null to skip automatic " + "marker length estimation. Detections are ignored.", + d1,d2,d3,d4,d, + Parameters::kMarkerLength().c_str()); + continue; } - markerLength_ = sum/float(scales.size()); - UWARN("Final marker length estimated = %fm, max error=%fm (used for subsequent detections)", markerLength_, maxError); } + else if(!markerLengths_.empty() || findIter!=extraMarkerLengths.end()) + { + std::map::const_iterator paramIter = markerLengths_.find(ids[i]); + if(paramIter != markerLengths_.end()) + { + if(findIter!=extraMarkerLengths.end() && findIter->second != paramIter->second) + { + UWARN("Marker's length of %d is defined both in extra lengths " + "(%f m) and the parameter %s (%f m), we will use the length " + "from extra lengths.", + findIter->second, + Parameters::kMarkerLengths().c_str(), + paramIter->second); + length = findIter->second; + } + else + { + length = paramIter->second; + } + } + else if(findIter!=extraMarkerLengths.end()) + { + length = findIter->second; + } + else + { + UERROR("Not supposed to reach this case, ignoring detection %d", ids[i]); + continue; + } + } + else if(markerLength_ < 0) + { + UWARN("Cannot find marker length for marker %d, ignoring this marker (count=%d)", ids[i], (int)extraMarkerLengths.size()); + continue; + } + else if(markerLength_ > 0) + { + length = markerLength_; + } + else + { + continue; + } + + UASSERT(length > 0.0f); + poses[i].x() *= length; + poses[i].y() *= length; + poses[i].z() *= length; + + // Limit the detection range to be between the min / max range. + // If the ranges are -1, allow any detection within that direction. + if((maxRange_ <= 0 || poses[i].z() < maxRange_) && + (minRange_ <= 0 || poses[i].z() > minRange_)) + { + Transform pose = models[cams[i]].localTransform() * poses[i]; + detections.insert(std::make_pair(ids[i], MarkerInfo(ids[i], length, pose))); + UDEBUG("Marker %d detected in base_link: %s, optical_link=%s, local transform=%s", + ids[i], + pose.prettyPrint().c_str(), + poses[i].prettyPrint().c_str(), + models[cams[i]].localTransform().prettyPrint().c_str()); + } + else + { + UDEBUG("Filtered marker %d by distance (min=%f max=%f value=%f) from the camera %d", + ids[i], minRange_, maxRange_, poses[i].z(), cams[i]); + } + } + if(markerLength_ == 0 && !scales.empty()) + { + float sum = 0.0f; + float maxError = 0.0f; + for(size_t i=0; i0) + { + float error = fabs(scales[i]-scales[0]); + if(error > 0.001f) + { + UWARN("The marker's length detected between 2 of the " + "markers doesn't match (%fm vs %fm)." + "Parameter %s can be set to non-null to skip automatic " + "marker length estimation or set to a negative value to " + "estimate length for each marker. The current %ld detections " + "are ignored!", + scales[i], scales[0], + Parameters::kMarkerLength().c_str(), + detections.size()); + detections.clear(); + return detections; + } + if(error > maxError) + { + maxError = error; + } + } + sum += scales[i]; + } + markerLength_ = sum/float(scales.size()); + UWARN("Final marker length estimated = %fm, max error=%fm (used for subsequent detections)", markerLength_, maxError); } if(imageWithDetections) @@ -371,31 +884,61 @@ std::map MarkerDetector::detect(const cv::Mat & image, { image.copyTo(*imageWithDetections); } + if(!ids.empty()) { +#ifdef HAVE_OPENCV_ARUCO cv::aruco::drawDetectedMarkers(*imageWithDetections, corners, ids); - +#else + UWARN("RTAB-Map is not built with \"aruco\" module from OpenCV. Cannot draw markers on image."); +#endif for(unsigned int i = 0; i < ids.size(); i++) { std::map::iterator iter = detections.find(ids[i]); if(iter!=detections.end()) { + int cam = cams[i]; + UASSERT(cam >=0 && cam < (int)models.size()); + const CameraModel & model = models[cam]; + int roix = subRGBWidth*cam; + UASSERT(roix >=0 && roix <= imageWithDetections->cols - subRGBWidth); + cv::Mat subImage(*imageWithDetections, cv::Rect(roix, 0, subRGBWidth, imageWithDetections->rows)); + cv::Vec3d rvec; + cv::Vec3d tvec(poses[i].x(), poses[i].y(), poses[i].z()); + cv::Mat R; + poses[i].rotationMatrix().convertTo(R, CV_64F); + cv::Rodrigues(R, rvec); #if CV_MAJOR_VERSION > 4 || (CV_MAJOR_VERSION == 4 && (CV_MINOR_VERSION >1 || (CV_MINOR_VERSION==1 && CV_PATCH_VERSION>=1))) - cv::drawFrameAxes(*imageWithDetections, model.K(), model.D(), rvecs[i], tvecs[i], iter->second.length() * 0.5f); -#else - cv::aruco::drawAxis(*imageWithDetections, model.K(), model.D(), rvecs[i], tvecs[i], iter->second.length() * 0.5f); + cv::drawFrameAxes(subImage, model.K(), model.D(), rvec, tvec, iter->second.length() * 0.5f); +#elif defined(HAVE_OPENCV_ARUCO) + cv::aruco::drawAxis(subImage, model.K(), model.D(), rvec, tvec, iter->second.length() * 0.5f); #endif } } } } -#else - UERROR("RTAB-Map is not built with \"aruco\" module from OpenCV."); -#endif - return detections; } +std::map MarkerDetector::detect(const cv::Mat & image, + const CameraModel & model, + const cv::Mat & depth, + const std::map & markerLengths, + cv::Mat * imageWithDetections) +{ + UDEBUG(""); + if(!image.empty() && image.cols != model.imageWidth()) + { + UERROR("This method cannot handle multi-camera marker detection, use the other function version supporting it."); + return std::map(); + } + + std::vector models; + models.push_back(model); + + return detect(image, models, depth, markerLengths, imageWithDetections); +} + } /* namespace rtabmap */ diff --git a/corelib/src/Memory.cpp b/corelib/src/Memory.cpp index 05563120..37afdbf7 100644 --- a/corelib/src/Memory.cpp +++ b/corelib/src/Memory.cpp @@ -76,13 +76,16 @@ Memory::Memory(const ParametersMap & parameters) : _similarityThreshold(Parameters::defaultMemRehearsalSimilarity()), _binDataKept(Parameters::defaultMemBinDataKept()), _rawDescriptorsKept(Parameters::defaultMemRawDescriptorsKept()), + _loadVisualLocalFeaturesOnInit(Parameters::defaultMemLoadVisualLocalFeaturesOnInit()), _saveDepth16Format(Parameters::defaultMemSaveDepth16Format()), _notLinkedNodesKeptInDb(Parameters::defaultMemNotLinkedNodesKept()), _saveIntermediateNodeData(Parameters::defaultMemIntermediateNodeDataKept()), _rgbCompressionFormat(Parameters::defaultMemImageCompressionFormat()), _depthCompressionFormat(Parameters::defaultMemDepthCompressionFormat()), _incrementalMemory(Parameters::defaultMemIncrementalMemory()), + _localizationReadOnly(Parameters::defaultMemLocalizationReadOnly()), _localizationDataSaved(Parameters::defaultMemLocalizationDataSaved()), + _flannIndexSaved(Parameters::defaultKpFlannIndexSaved()), _reduceGraph(Parameters::defaultMemReduceGraph()), _maxStMemSize(Parameters::defaultMemSTMSize()), _recentWmRatio(Parameters::defaultMemRecentWmRatio()), @@ -129,6 +132,7 @@ Memory::Memory(const ParametersMap & parameters) : _linksChanged(false), _signaturesAdded(0), _allNodesInWM(true), + _receivingOdometryFeatures(false), _badSignRatio(Parameters::defaultKpBadSignRatio()), _tfIdfLikelihoodUsed(Parameters::defaultKpTfIdfLikelihoodUsed()), _parallelized(Parameters::defaultKpParallelized()), @@ -182,10 +186,6 @@ bool Memory::init(const std::string & dbUrl, bool dbOverwritten, const Parameter _dbDriver = 0; // HACK for the clear() below to think that there is no db } } - else if(!_memoryChanged && _linksChanged) - { - _dbDriver->setTimestampUpdateEnabled(false); // update links only - } this->clear(); if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit("Clearing memory, done!")); @@ -209,10 +209,10 @@ bool Memory::init(const std::string & dbUrl, bool dbOverwritten, const Parameter bool success = true; if(_dbDriver) { - _dbDriver->setTimestampUpdateEnabled(true); // make sure that timestamp update is enabled (may be disabled above) + success = false; if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(std::string("Connecting to database \"") + dbUrl + "\"...")); - if(_dbDriver->openConnection(dbUrl, dbOverwritten)) + if(_dbDriver->openConnection(dbUrl, dbOverwritten, isReadOnly())) { success = true; if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(std::string("Connecting to database \"") + dbUrl + "\", done!")); @@ -242,16 +242,18 @@ void Memory::loadDataFromDb(bool postInitClosingEvents) if(loadAllNodesInWM) { + UDEBUG("Loading all nodes to WM..."); if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(std::string("Loading all nodes to WM..."))); std::set ids; _dbDriver->getAllNodeIds(ids, true); - _dbDriver->loadSignatures(std::list(ids.begin(), ids.end()), dbSignatures); + _dbDriver->loadSignatures(std::list(ids.begin(), ids.end()), dbSignatures, 0, !_loadVisualLocalFeaturesOnInit); } else { + UDEBUG("Loading last nodes to WM..."); // load previous session working memory if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(std::string("Loading last nodes to WM..."))); - _dbDriver->loadLastNodes(dbSignatures); + _dbDriver->loadLastNodes(dbSignatures, !_loadVisualLocalFeaturesOnInit); } for(std::list::reverse_iterator iter=dbSignatures.rbegin(); iter!=dbSignatures.rend(); ++iter) { @@ -417,23 +419,26 @@ void Memory::loadDataFromDb(bool postInitClosingEvents) } else { - _dbDriver->load(_vwd, false); + _dbDriver->load(*_vwd, false); } } else { UDEBUG("load words"); // load the last dictionary - _dbDriver->load(_vwd, _vwd->isIncremental()); + _dbDriver->load(*_vwd, _vwd->isIncremental()); } UDEBUG("%d words loaded!", _vwd->getUnusedWordsSize()); _vwd->update(); if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(uFormat("Loading dictionary, done! (%d words)", (int)_vwd->getUnusedWordsSize()))); if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(std::string("Adding word references..."))); + UDEBUG("Adding word references..."); + UTimer timer; // Enable loaded signatures const std::map & signatures = this->getSignatures(); - for(std::map::const_iterator i=signatures.begin(); i!=signatures.end(); ++i) + bool corruptedDictionary = false; + for(std::map::const_iterator i=signatures.begin(); i!=signatures.end() && !corruptedDictionary; ++i) { Signature * s = this->_getSignature(i->first); UASSERT(s != 0); @@ -441,24 +446,126 @@ void Memory::loadDataFromDb(bool postInitClosingEvents) const std::multimap & words = s->getWords(); if(words.size()) { - UDEBUG("node=%d, word references=%d", s->id(), words.size()); + //UDEBUG("node=%d, word references=%d", s->id(), words.size()); for(std::multimap::const_iterator iter = words.begin(); iter!=words.end(); ++iter) { if(iter->first > 0) { - _vwd->addWordRef(iter->first, i->first); + if(!_vwd->addWordRef(iter->first, s->id())) + { + corruptedDictionary = true; + break; + } + } + } + s->setEnabled(!corruptedDictionary); + if(corruptedDictionary) + { + //revert all changes from that signature till it broke above + for(std::multimap::const_iterator iter = words.begin(); iter!=words.end(); ++iter) + { + if(iter->first > 0) + { + _vwd->removeAllWordRef(iter->first, s->id()); + } } } - s->setEnabled(true); } } + if(corruptedDictionary) + { + if(!_vwd->isIncremental()) + { + UERROR("The dictionary is empty or missing some words from nodes in WM, " + "we cannot repair it because it is a fixed dictionary. Make sure you " + "are using the right fixed dictionary that was used to generate the map."); + } + else + { + std::string msg = uFormat( + "The dictionary is empty or missing some words from nodes in WM, " + "we will try to repair it. This can be caused by rtabmap closing before it has time " + "to save the dictionary. Re-creating the dictionary from %ld nodes...", + signatures.size()); + UWARN("%s", msg.c_str()); + if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(msg)); + + //remove all words ref + + const std::map & addedWords = _vwd->getVisualWords(); + int nodesRepaired = 0; + size_t oldSize = addedWords.size(); + std::string assertMsg = + "If we assert here, the problem is maybe deeper. Try " + "to use rtabmap-recovery tool instead to fix the database."; + for(std::map::const_iterator i=signatures.begin(); i!=signatures.end(); ++i) + { + Signature * s = this->_getSignature(i->first); + UASSERT_MSG(s != 0, assertMsg.c_str()); + + if(s->isEnabled()) + { + // Words already in dictionary and references added + continue; + } + + const std::multimap * words = &s->getWords(); + if(words->size()) + { + cv::Mat descriptors = s->getWordsDescriptors(); + std::multimap loadedWords; + if(descriptors.empty()) + { + // We may have started rtabmap without loading features, check in the database + std::multimap w; + std::vector k; + std::vector p; + _dbDriver->getLocalFeatures(s->id(), loadedWords, k, p, descriptors); + UASSERT_MSG(loadedWords.size() == words->size(), assertMsg.c_str()); // Just doublecheck + words = &loadedWords; // The index will be set + UASSERT_MSG(!descriptors.empty(), assertMsg.c_str()); + } + bool repaired = false; + for(std::multimap::const_iterator iter = words->begin(); iter!=words->end(); ++iter) + { + if(iter->first > 0) + { + if(addedWords.find(iter->first) == addedWords.end()) + { + UASSERT_MSG(iter->second >= 0 && iter->second < descriptors.rows, + uFormat("iter->second=%d descriptors.rows=%d (signature=%d word=%d). %s", + iter->second, descriptors.rows, s->id(), iter->first, assertMsg.c_str()).c_str()); + _vwd->addWord(new VisualWord(iter->first, descriptors.row(iter->second).clone())); + repaired = true; + } + UASSERT_MSG(_vwd->addWordRef(iter->first, s->id()), assertMsg.c_str()); + } + } + nodesRepaired += (repaired?1:0); + s->setEnabled(true); + } + } + + msg = uFormat( + "Regenerated the dictionary with %ld missing words (%ld -> %ld) from %d nodes.", + addedWords.size() - oldSize, + oldSize, + addedWords.size(), + nodesRepaired); + UWARN("%s", msg.c_str()); + if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(msg)); + _memoryChanged = true; // This will force rtabmap to save back the dictionary even if we don't process any new data + _vwd->update(); + } + } + if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(uFormat("Adding word references, done! (%d)", _vwd->getTotalActiveReferences()))); if(_vwd->getUnusedWordsSize() && _vwd->isIncremental()) { UWARN("_vwd->getUnusedWordsSize() must be empty... size=%d", _vwd->getUnusedWordsSize()); } - UDEBUG("Total word references added = %d", _vwd->getTotalActiveReferences()); + UDEBUG("Total word references added = %d (in %f s)", _vwd->getTotalActiveReferences(), timer.ticks()); if(_lastSignature == 0) { @@ -482,6 +589,37 @@ void Memory::loadDataFromDb(bool postInitClosingEvents) UDEBUG("map ids start with %d", _idMapCount); } +void Memory::saveFlannIndex(bool postInitClosingEvents) +{ + if(!_dbDriver) { + return; + } + if(uStrNumCmp(_dbDriver->getDatabaseVersion(), "0.23.0") >= 0) { + if(_flannIndexSaved && !_incrementalMemory) { + if(_vwd->isModified()) { + UINFO("Saving flann index to database... (%s=true)", Parameters::kKpFlannIndexSaved().c_str()); + if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit("Saving flann index to database...")); + _dbDriver->saveFlannIndex(_vwd->serializeIndex()); + if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit("Saving flann index to database, done!")); + } + else + { + UDEBUG("The dictionary didn't change since loaded, do not need to save again to database."); + } + } + else { + // clear if exists + _dbDriver->saveFlannIndex(std::vector()); + } + } + else if(_flannIndexSaved) + { + UWARN("Parameter %s is enabled, but database version is too old (%s < 0.23). Flann index cannot be saved.", + Parameters::kKpFlannIndexSaved().c_str(), + _dbDriver->getDatabaseVersion().c_str()); + } +} + void Memory::close(bool databaseSaved, bool postInitClosingEvents, const std::string & ouputDatabasePath) { UINFO("databaseSaved=%d, postInitClosingEvents=%d", databaseSaved?1:0, postInitClosingEvents?1:0); @@ -493,15 +631,25 @@ void Memory::close(bool databaseSaved, bool postInitClosingEvents, const std::st databaseNameChanged = ouputDatabasePath.size() && _dbDriver->getUrl().size() && _dbDriver->getUrl().compare(ouputDatabasePath) != 0?true:false; } - if(!databaseSaved || (!_memoryChanged && !_linksChanged && !databaseNameChanged)) + UDEBUG("_memoryChanged=%d _linksChanged=%d databaseNameChanged=%d", _memoryChanged?1:0, _linksChanged?1:0, databaseNameChanged?1:0); + + if(!databaseSaved || (!_memoryChanged && !_linksChanged && !databaseNameChanged) || this->isReadOnly()) { if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(uFormat("No changes added to database."))); UINFO("No changes added to database."); if(_dbDriver) { + if(!this->isReadOnly()) { + saveFlannIndex(postInitClosingEvents); + } + else if(_memoryChanged || _linksChanged || databaseNameChanged) + { + UWARN("Memory has been modified (nodes=%s links=%s name=%s) but the database is read-only, changes are not saved to database.", + _memoryChanged?"true":"false", _linksChanged?"true":"false", databaseNameChanged?"true":"false"); + } if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(uFormat("Closing database \"%s\"...", _dbDriver->getUrl().c_str()))); - _dbDriver->closeConnection(false, ouputDatabasePath); + _dbDriver->closeConnection(false); delete _dbDriver; _dbDriver = 0; if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit("Closing database, done!")); @@ -514,11 +662,9 @@ void Memory::close(bool databaseSaved, bool postInitClosingEvents, const std::st { UINFO("Saving memory..."); if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit("Saving memory...")); - if(!_memoryChanged && _linksChanged && _dbDriver) + if(!_memoryChanged && _dbDriver) { - // don't update the time stamps! - UDEBUG(""); - _dbDriver->setTimestampUpdateEnabled(false); + saveFlannIndex(postInitClosingEvents); } this->clear(); if(_dbDriver) @@ -565,6 +711,7 @@ void Memory::parseParameters(const ParametersMap & parameters) Parameters::parse(params, Parameters::kMemBinDataKept(), _binDataKept); Parameters::parse(params, Parameters::kMemRawDescriptorsKept(), _rawDescriptorsKept); + Parameters::parse(params, Parameters::kMemLoadVisualLocalFeaturesOnInit(), _loadVisualLocalFeaturesOnInit); Parameters::parse(params, Parameters::kMemSaveDepth16Format(), _saveDepth16Format); Parameters::parse(params, Parameters::kMemReduceGraph(), _reduceGraph); Parameters::parse(params, Parameters::kMemNotLinkedNodesKept(), _notLinkedNodesKeptInDb); @@ -618,6 +765,8 @@ void Memory::parseParameters(const ParametersMap & parameters) Parameters::parse(params, Parameters::kMarkerVarianceAngular(), _markerAngVariance); Parameters::parse(params, Parameters::kMarkerVarianceOrientationIgnored(), _markerOrientationIgnored); Parameters::parse(params, Parameters::kMemLocalizationDataSaved(), _localizationDataSaved); + Parameters::parse(params, Parameters::kKpFlannIndexSaved(), _flannIndexSaved); + Parameters::parse(params, Parameters::kMemLocalizationReadOnly(), _localizationReadOnly); if(_markerAngVariance>=9999) { @@ -654,10 +803,7 @@ void Memory::parseParameters(const ParametersMap & parameters) } // Keypoint stuff - if(_vwd) - { - _vwd->parseParameters(params); - } + _vwd->parseParameters(params); Parameters::parse(params, Parameters::kKpTfIdfLikelihoodUsed(), _tfIdfLikelihoodUsed); Parameters::parse(params, Parameters::kKpParallelized(), _parallelized); @@ -856,7 +1002,7 @@ void Memory::preUpdate() { this->cleanUnusedWords(); } - if(_vwd && !_parallelized) + if(!_parallelized) { //When parallelized, it is done in CreateSignature _vwd->update(); @@ -1114,10 +1260,7 @@ void Memory::addSignatureToStm(Signature * signature, const cv::Mat & covariance } ++_signaturesAdded; - if(_vwd) - { - UDEBUG("%d words ref for the signature %d (weight=%d)", signature->getWords().size(), signature->id(), signature->getWeight()); - } + UDEBUG("%d words ref for the signature %d (weight=%d)", signature->getWords().size(), signature->id(), signature->getWeight()); if(signature->getWords().size()) { signature->setEnabled(true); @@ -1145,113 +1288,187 @@ void Memory::addSignatureToWmFromLTM(Signature * signature) } } -void Memory::moveSignatureToWMFromSTM(int id, int * reducedTo) +bool Memory::canBeReduced(const Link & link, float maxDistance, int direction) +{ + return link.to() != link.from() && + link.type() != Link::kNeighbor && + link.type() != Link::kNeighborMerged && + link.userDataCompressed().empty() && + link.type() != Link::kUndef && + link.type() != Link::kVirtualClosure && + (maxDistance == 0.0f || link.transform().getNorm() < maxDistance) && + (direction == 0 || (direction==-1 && link.to() < link.from()) || (direction==1 && link.to() > link.from())); +} + +int Memory::reduceNode(int id, float maxDistance, bool keepLinkedInDb, int direction) +{ + UDEBUG("Reducing %d (max distance=%f, keep linked in db=%s, direction=%d)", + id, maxDistance, keepLinkedInDb?"true":"false", direction); + Signature * s = this->_getSignature(id); + if(s==0) + { + UWARN("Node %d is not in WM/STM, cannot reduce it.", id); + return 0; + } + + if(!s->getLabel().empty()) + { + // We currently not remove nodes with labels + return 0; + } + + std::multimap links = s->getLinks(); + std::map neighbors; + int reducedTo = 0; + for(std::multimap::const_iterator iter=links.begin(); iter!=links.end(); ++iter) + { + if(canBeReduced(iter->second, maxDistance, direction)) + { + float distance = iter->second.transform().getNorm(); + reducedTo = iter->second.to(); + UDEBUG("Reduce %d to %d (distance=%f)", + s->id(), iter->second.to(), distance); + } + + if(iter->second.type() == Link::kNeighbor) + { + neighbors.insert(*iter); + } + } + if(reducedTo>0) + { + if(maxDistance > 0.0f) + { + // Only reduce if all neighbor merged links are also below maxDistance + for(std::multimap::const_iterator iter=links.begin(); iter!=links.end(); ++iter) + { + if( iter->second.type() == Link::kNeighborMerged && + iter->second.transform().getNorm() > maxDistance) + { + return 0; + } + } + } + + for(std::multimap::const_iterator iter=links.begin(); iter!=links.end(); ++iter) + { + Signature * sTo = this->_getSignature(iter->first); + if(sTo->id()!=s->id()) // Not Prior/Gravity links... + { + UASSERT_MSG(sTo!=0, uFormat("id=%d", iter->first).c_str()); + sTo->removeLink(s->id()); + if(iter->second.type() != Link::kNeighbor && + iter->second.type() != Link::kUndef) + { + if(iter->second.type() == Link::kNeighborMerged) + { + s->removeLink(sTo->id()); + if(maxDistance == 0.0f) + { + // online graph reduction, always skip these links + continue; + } + } + // link to all neighbors + for(std::map::iterator jter=neighbors.begin(); jter!=neighbors.end(); ++jter) + { + if(!sTo->hasLink(jter->second.to())) + { + Link l = iter->second.inverse().merge( + jter->second, + iter->second.userDataCompressed().empty() && iter->second.type() != Link::kVirtualClosure?Link::kNeighborMerged:iter->second.type()); + UDEBUG("Merging link %d->%d (type=%d) to with %d->%d (type %d). Adding %d->%d (type %d) to %d and %d", + iter->second.to(), iter->second.from(), iter->second.type(), + jter->second.from(), jter->second.to(), jter->second.type(), + l.from(), l.to(), l.type(), sTo->id(), l.to()); + sTo->addLink(l); + Signature * sB = this->_getSignature(l.to()); + UASSERT(sB!=0); + UASSERT_MSG(!sB->hasLink(l.from()), uFormat("%d->%d type=%d", sB->id(), l.to(), l.type()).c_str()); + sB->addLink(l.inverse()); + } + } + // link to all landmarks + for(std::map::const_iterator jter=s->getLandmarks().begin(); jter!=s->getLandmarks().end(); ++jter) + { + if(!uContains(sTo->getLandmarks(), jter->first)) + { + UDEBUG("Move landmark observation %d from %d to %d", + jter->first, s->id(), sTo->id()); + Link l = iter->second.inverse().merge( + jter->second, + jter->second.type()); + sTo->addLandmark(l); + // Update landmark index + std::map >::iterator nter = _landmarksIndex.find(jter->first); + if(nter!=_landmarksIndex.end()) + { + nter->second.insert(sTo->id()); + } + else + { + std::set tmp; + tmp.insert(sTo->id()); + _landmarksIndex.insert(std::make_pair(jter->first, tmp)); + } + } + } + } + } + } + + this->moveToTrash(s, keepLinkedInDb); + s = 0; + _linksChanged = true; + _memoryChanged = true; + } + return reducedTo; +} + +void Memory::moveSignatureToWMFromSTM(int id, int * reducedToOut) { UDEBUG("Inserting node %d from STM in WM...", id); UASSERT(_stMem.find(id) != _stMem.end()); - Signature * s = this->_getSignature(id); - UASSERT(s!=0); - + int reducedId = 0; if(_reduceGraph) { - bool merge = false; - const std::multimap & links = s->getLinks(); - std::map neighbors; - for(std::multimap::const_iterator iter=links.begin(); iter!=links.end(); ++iter) + Signature * s = this->_getSignature(id); + UASSERT(s!=0); + if(s->getWeight() == -1) { - if(!merge) - { - merge = iter->second.to() < s->id() && // should be a parent->child link - iter->second.to() != iter->second.from() && - iter->second.type() != Link::kNeighbor && - iter->second.type() != Link::kNeighborMerged && - iter->second.userDataCompressed().empty() && - iter->second.type() != Link::kUndef && - iter->second.type() != Link::kVirtualClosure; - if(merge) - { - UDEBUG("Reduce %d to %d", s->id(), iter->second.to()); - if(reducedTo) - { - *reducedTo = iter->second.to(); - } - } - - } - if(iter->second.type() == Link::kNeighbor) - { - neighbors.insert(*iter); - } + UERROR("Graph reduction with intermediate nodes is not supported."); } - if(merge) + else { - if(s->getLabel().empty()) + std::multimap links = s->getLinks(); + // Setting true to make sure we save all visual + // words that could be referenced in a previously + // transferred node in LTM (#979) + reducedId = reduceNode(s->id(), 0, true); + if(reducedToOut) { + *reducedToOut = reducedId; + } + if(reducedId>0) { - for(std::multimap::const_iterator iter=links.begin(); iter!=links.end(); ++iter) + for(std::multimap::iterator iter=links.begin(); iter!=links.end(); ++iter) { - Signature * sTo = this->_getSignature(iter->first); - if(sTo->id()!=s->id()) // Not Prior/Gravity links... + if(iter->second.type() == Link::kNeighbor) { - UASSERT_MSG(sTo!=0, uFormat("id=%d", iter->first).c_str()); - sTo->removeLink(s->id()); - if(iter->second.type() != Link::kNeighbor && - iter->second.type() != Link::kNeighborMerged && - iter->second.type() != Link::kUndef) + if(_lastGlobalLoopClosureId == s->id()) { - // link to all neighbors - for(std::map::iterator jter=neighbors.begin(); jter!=neighbors.end(); ++jter) - { - if(!sTo->hasLink(jter->second.to())) - { - UDEBUG("Merging link %d->%d (type=%d) to link %d->%d (type %d)", - iter->second.from(), iter->second.to(), iter->second.type(), - jter->second.from(), jter->second.to(), jter->second.type()); - Link l = iter->second.inverse().merge( - jter->second, - iter->second.userDataCompressed().empty() && iter->second.type() != Link::kVirtualClosure?Link::kNeighborMerged:iter->second.type()); - sTo->addLink(l); - Signature * sB = this->_getSignature(l.to()); - UASSERT(sB!=0); - UASSERT_MSG(!sB->hasLink(l.from()), uFormat("%d->%d", sB->id(), l.to()).c_str()); - sB->addLink(l.inverse()); - } - } + _lastGlobalLoopClosureId = iter->first; } } } - - //remove neighbor links - std::multimap linksCopy = links; - for(std::multimap::iterator iter=linksCopy.begin(); iter!=linksCopy.end(); ++iter) - { - if(iter->second.type() == Link::kNeighbor || - iter->second.type() == Link::kNeighborMerged) - { - s->removeLink(iter->first); - if(iter->second.type() == Link::kNeighbor) - { - if(_lastGlobalLoopClosureId == s->id()) - { - _lastGlobalLoopClosureId = iter->first; - } - } - } - } - - // Setting true to make sure we save all visual - // words that could be referenced in a previously - // transferred node in LTM (#979) - this->moveToTrash(s, true); - s = 0; } } } - if(s != 0) + if(reducedId == 0) { _workingMem.insert(_workingMem.end(), std::make_pair(*_stMem.begin(), UTimer::now())); _stMem.erase(*_stMem.begin()); } - // else already removed from STM/WM in moveToTrash() + // else already removed from STM/WM in reduceNode() } const Signature * Memory::getSignature(int id) const @@ -1464,7 +1681,7 @@ std::map Memory::getNeighborsId( ) const { UASSERT(maxGraphDepth >= 0); - //DEBUG("signatureId=%d maxGraphDepth=%d maxCheckedInDatabase=%d incrementMarginOnLoop=%d " + //UDEBUG("signatureId=%d maxGraphDepth=%d maxCheckedInDatabase=%d incrementMarginOnLoop=%d " // "ignoreLoopIds=%d ignoreIntermediateNodes=%d ignoreLocalSpaceLoopIds=%d", // signatureId, maxGraphDepth, maxCheckedInDatabase, incrementMarginOnLoop?1:0, // ignoreLoopIds?1:0, ignoreIntermediateNodes?1:0, ignoreLocalSpaceLoopIds?1:0); @@ -1501,17 +1718,19 @@ std::map Memory::getNeighborsId( std::map tmpLandmarks; const std::multimap * links = &tmpLinks; const std::map * landmarks = &tmpLandmarks; + bool isIntermediateNode = false; if(s) { - if(!ignoreIntermediateNodes || s->getWeight() != -1) + isIntermediateNode = s->getWeight() == -1; + if(!ignoreIntermediateNodes || !isIntermediateNode) { - ids.insert(std::pair(*jter, m)); + int effectiveMargin = m>0 && isIntermediateNode>0 ? m-1 : m; + ids.insert(std::pair(s->id(), effectiveMargin)); } else { - ignoredIds.insert(*jter); + ignoredIds.insert(s->id()); } - links = &s->getLinks(); if(!ignoreLoopIds) { @@ -1520,8 +1739,22 @@ std::map Memory::getNeighborsId( } else if(maxCheckedInDatabase == -1 || (maxCheckedInDatabase > 0 && _dbDriver && nbLoadedFromDb < maxCheckedInDatabase)) { - ++nbLoadedFromDb; - ids.insert(std::pair(*jter, m)); + int weight = 0; + _dbDriver->getWeight(*jter, weight); + isIntermediateNode = weight == -1; + if(!ignoreIntermediateNodes || !isIntermediateNode) + { + int effectiveMargin = m>0 && isIntermediateNode>0 ? m-1 : m; + ids.insert(std::pair(*jter, effectiveMargin)); + if(!isIntermediateNode) + { + ++nbLoadedFromDb; + } + } + else + { + ignoredIds.insert(*jter); + } UTimer timer; _dbDriver->loadLinks(*jter, tmpLinks, ignoreLoopIds?Link::kAllWithoutLandmarks:Link::kAllWithLandmarks); @@ -1565,7 +1798,7 @@ std::map Memory::getNeighborsId( if(iter->second.type() == Link::kNeighbor || iter->second.type() == Link::kNeighborMerged) { - if(ignoreIntermediateNodes && s->getWeight()==-1) + if(isIntermediateNode) { // stay on the same margin if(currentMargin.insert(iter->first).second) @@ -1696,7 +1929,7 @@ int Memory::getNextId() int Memory::incrementMapId(std::map * reducedIds) { //don't increment if there is no location in the current map - const Signature * s = getLastWorkingSignature(); + const Signature * s = getLastWorkingSignature(false); if(s && s->mapId() == _idMapCount) { // New session! move all signatures from the STM to WM @@ -1825,6 +2058,7 @@ void Memory::clear() uInsert(parameters, parameters_); parameters.erase(Parameters::kRtabmapWorkingDirectory()); // don't save working directory as it is machine dependent UDEBUG(""); + _dbDriver->setTimestampUpdateEnabled(true); // Only re-stamp if we updated the memory _dbDriver->addInfoAfterRun(memSize, _lastSignature?_lastSignature->id():0, UProcessInfo::getMemoryUsage(), @@ -1836,13 +2070,24 @@ void Memory::clear() UDEBUG(""); //Get the tree root (parents) - std::map mem = _signatures; - for(std::map::iterator i=mem.begin(); i!=mem.end(); ++i) - { - if(i->second) + if(!_dbDriver) { + // We are not saving to database anyway, just delete now. + for(std::map::iterator iter=_signatures.begin(); iter!=_signatures.end(); ++iter) { - UDEBUG("deleting from the working and the short-term memory: %d", i->first); - this->moveToTrash(i->second); + delete iter->second; + } + _workingMem.clear(); + _signatures.clear(); + } + else { + std::map mem = _signatures; + for(std::map::iterator i=mem.begin(); i!=mem.end(); ++i) + { + if(i->second) + { + //UDEBUG("deleting from the working and the short-term memory: %d", i->first); + this->moveToTrash(i->second); + } } } @@ -1866,6 +2111,7 @@ void Memory::clear() UDEBUG(""); _lastSignature = 0; _lastGlobalLoopClosureId = 0; + _signaturesAdded = 0; _idCount = kIdStart; _idMapCount = kIdStart; _memoryChanged = false; @@ -1879,21 +2125,16 @@ void Memory::clear() _landmarksIndex.clear(); _landmarksSize.clear(); _allNodesInWM = true; + _receivingOdometryFeatures = false; if(_dbDriver) { _dbDriver->join(true); cleanUnusedWords(); _dbDriver->emptyTrashes(); + _dbDriver->setTimestampUpdateEnabled(false); } - else - { - cleanUnusedWords(); - } - if(_vwd) - { - _vwd->clear(); - } + _vwd->clear(_dbDriver!=NULL); UDEBUG(""); } @@ -2068,12 +2309,30 @@ std::list Memory::forget(const std::set & ignoredIds) std::list signatures = this->getRemovableSignatures(1, ignoredIds); if(signatures.size()) { - Signature * s = dynamic_cast(signatures.front()); + Signature * s = dynamic_cast(signatures.front()); if(s) { - signaturesRemoved.push_back(s->id()); + int refId = s->id(); + signaturesRemoved.push_back(refId); + std::multimap neighborLinks = graph::filterLinks(s->getLinks(), Link::kNeighbor, true); this->moveToTrash(s); wordsRemoved = _vwd->getUnusedWordsSize(); + + // Remove all linked intermediate nodes at the same time (in both direction) + std::list idsToCheck(uKeysList(neighborLinks)); + while(!idsToCheck.empty()) + { + int id = idsToCheck.front(); + idsToCheck.pop_front(); + s = this->_getSignature(id); + if(s && s->getWeight() == -1) + { + neighborLinks = graph::filterLinks(s->getLinks(), Link::kNeighbor, true); + uAppend(idsToCheck, uKeysList(neighborLinks)); + signaturesRemoved.push_back(s->id()); + this->moveToTrash(s); + } + } } else { @@ -2092,22 +2351,56 @@ std::list Memory::forget(const std::set & ignoredIds) UDEBUG(""); // Remove one more than total added during the iteration int signaturesAdded = _signaturesAdded; - std::list signatures = getRemovableSignatures(signaturesAdded+1, ignoredIds); - for(std::list::iterator iter=signatures.begin(); iter!=signatures.end(); ++iter) + int intermediateNodesRemoved = 0; + while(int(signaturesRemoved.size()-intermediateNodesRemoved) < signaturesAdded+1) { - signaturesRemoved.push_back((*iter)->id()); - // When a signature is deleted, it notifies the memory - // and it is removed from the memory list - this->moveToTrash(*iter); + std::list signatures = this->getRemovableSignatures(1, ignoredIds); + if(signatures.size()) + { + Signature * s = dynamic_cast(signatures.front()); + if(s) + { + signaturesRemoved.push_back(s->id()); + std::multimap neighborLinks = graph::filterLinks(s->getLinks(), Link::kNeighbor, true); + // When a signature is deleted, it notifies the memory + // and it is removed from the memory list + this->moveToTrash(s); + + // Remove all linked intermediate nodes at the same time (in both direction) + std::list idsToCheck(uKeysList(neighborLinks)); + while(!idsToCheck.empty()) + { + int id = idsToCheck.front(); + idsToCheck.pop_front(); + s = this->_getSignature(id); + if(s && s->getWeight() == -1) + { + ++intermediateNodesRemoved; + neighborLinks = graph::filterLinks(s->getLinks(), Link::kNeighbor, true); + uAppend(idsToCheck, uKeysList(neighborLinks)); + signaturesRemoved.push_back(s->id()); + this->moveToTrash(s); + } + } + } + else + { + break; + } + } + else + { + break; + } } - if((int)signatures.size() < signaturesAdded) + if(int(signaturesRemoved.size() - intermediateNodesRemoved) < signaturesAdded) { - UWARN("Less signatures transferred (%d) than added (%d)! The working memory cannot decrease in size.", - (int)signatures.size(), signaturesAdded); + UWARN("Less signatures transferred (%d, inter=%d) than added (%d)! The working memory cannot decrease in size.", + int(signaturesRemoved.size()-intermediateNodesRemoved), intermediateNodesRemoved, signaturesAdded); } else { - UDEBUG("signaturesRemoved=%d, _signaturesAdded=%d", (int)signatures.size(), signaturesAdded); + UDEBUG("signaturesRemoved=%d (inter=%d), _signaturesAdded=%d", int(signaturesRemoved.size()-intermediateNodesRemoved), intermediateNodesRemoved, signaturesAdded); } } return signaturesRemoved; @@ -2120,7 +2413,9 @@ int Memory::cleanup() int signatureRemoved = 0; // bad signature - if(_lastSignature && ((_lastSignature->isBadSignature() && _badSignaturesIgnored) || !_incrementalMemory)) + if(_lastSignature && + ((_lastSignature->isBadSignature() && _badSignaturesIgnored && _lastSignature->getWeight()!=-1) || + !_incrementalMemory)) { if(_lastSignature->isBadSignature()) { @@ -2360,9 +2655,10 @@ std::list Memory::getRemovableSignatures(int count, const std::set< break; } } - if(!foundInSTM) + if(!foundInSTM && s->getWeight()>=0) { - // less weighted signature priority to be transferred + // Less weighted signature priority to be transferred + // Ignore intermediate nodes weightAgeIdMap.insert(std::make_pair(WeightAgeIdKey(s->getWeight(), _transferSortingByWeightId?0.0:memIter->second, s->id()), s)); } } @@ -2428,7 +2724,7 @@ std::list Memory::getRemovableSignatures(int count, const std::set< */ void Memory::moveToTrash(Signature * s, bool keepLinkedToGraph, std::list * deletedWords) { - UDEBUG("id=%d", s?s->id():0); + //UDEBUG("id=%d", s?s->id():0); if(s) { // Cleanup landmark indexes @@ -2450,7 +2746,11 @@ void Memory::moveToTrash(Signature * s, bool keepLinkedToGraph, std::list * } // it is a bad signature (not saved), remove links! - if(keepLinkedToGraph && (!s->isSaved() && s->isBadSignature() && _badSignaturesIgnored)) + if(keepLinkedToGraph && + !s->isSaved() && + s->isBadSignature() && + _badSignaturesIgnored && + s->getWeight()!=-1) { keepLinkedToGraph = false; } @@ -2458,9 +2758,10 @@ void Memory::moveToTrash(Signature * s, bool keepLinkedToGraph, std::list * // If not saved to database if(!keepLinkedToGraph) { - UASSERT_MSG(this->isInSTM(s->id()), + UASSERT_MSG(this->isInSTM(s->id()) || this->isInWM(s->id()), uFormat("Deleting location (%d) outside the " - "STM is not implemented!", s->id()).c_str()); + "WM/STM is not implemented! STM size=%ld WM size=%ld", + s->id(), this->getStMem().size(), this->getWorkingMem().size()).c_str()); const std::multimap & links = s->getLinks(); for(std::multimap::const_iterator iter=links.begin(); iter!=links.end(); ++iter) { @@ -2471,7 +2772,7 @@ void Memory::moveToTrash(Signature * s, bool keepLinkedToGraph, std::list * UASSERT_MSG(sTo!=0, uFormat("A neighbor (%d) of the deleted location %d is " "not found in WM/STM! Are you deleting a location " - "outside the STM?", iter->first, s->id()).c_str()); + "outside the WM/STM?", iter->first, s->id()).c_str()); if(iter->first > s->id() && links.size()>1 && sTo->hasLink(s->id())) { @@ -2481,7 +2782,7 @@ void Memory::moveToTrash(Signature * s, bool keepLinkedToGraph, std::list * } // child - if(iter->second.type() == Link::kGlobalClosure && s->id() > sTo->id() && s->getWeight()>0) + if(iter->second.type() == Link::kGlobalClosure && s->getWeight()>0) { sTo->setWeight(sTo->getWeight() + s->getWeight()); // copy weight } @@ -2575,9 +2876,21 @@ int Memory::getLastSignatureId() const return _idCount; } -const Signature * Memory::getLastWorkingSignature() const +const Signature * Memory::getLastWorkingSignature(bool ignoreIntermediateNodes) const { - UDEBUG(""); + if(ignoreIntermediateNodes && _lastSignature && _lastSignature->getWeight()==-1) + { + for(std::map::const_reverse_iterator iter=_signatures.rbegin(); + iter!=_signatures.rend(); + ++iter) + { + if(iter->second->getWeight() != -1) + { + return iter->second; + } + } + return 0; + } return _lastSignature; } @@ -2735,6 +3048,29 @@ bool Memory::setUserData(int id, const cv::Mat & data) return false; } +void Memory::convertToIntermediate(int locationId) +{ + UDEBUG("Converting location %d to intermediate node", locationId); + Signature * location = _getSignature(locationId); + if(location) + { + location->setWeight(-1); + location->sensorData().setFeatures(std::vector(), std::vector(), cv::Mat()); + this->disableWordsRef(locationId); // won't be used for loop closure detection anymore + if(!_saveIntermediateNodeData) + { + location->removeAllWords(); + location->sensorData().clearGlobalDescriptors(); + } + + location->sensorData().clearRawData(); + if(!_saveIntermediateNodeData || !this->isBinDataKept()) + { + location->sensorData().clearCompressedData(); + } + } +} + void Memory::deleteLocation(int locationId, std::list * deletedWords) { UDEBUG("Deleting location %d", locationId); @@ -2840,7 +3176,7 @@ void Memory::removeLink(int oldId, int newId) } } -void Memory::removeRawData(int id, bool image, bool scan, bool userData) +void Memory::removeRawData(int id, bool image, bool scan, bool userData, bool occupancyGrid) { UDEBUG("id=%d image=%d scan=%d userData=%d", id, image?1:0, scan?1:0, userData?1:0); Signature * s = this->_getSignature(id); @@ -2849,7 +3185,8 @@ void Memory::removeRawData(int id, bool image, bool scan, bool userData) s->sensorData().clearRawData( image && (!_reextractLoopClosureFeatures || !_registrationPipeline->isImageRequired()), scan && !_registrationPipeline->isScanRequired(), - userData && !_registrationPipeline->isUserDataRequired()); + userData && !_registrationPipeline->isUserDataRequired(), + occupancyGrid); } } @@ -2921,6 +3258,37 @@ Transform Memory::computeTransform( _registrationPipeline->isScanRequired()?&laserBuf:0, _registrationPipeline->isUserDataRequired()?&userBuf:0); + // Load word descriptors and keypoints on-demand if necessary + if( !_reextractLoopClosureFeatures && + (_registrationPipeline->isImageRequired() || guess.isNull()) && + !fromS.getWords().empty() && fromS.getWordsKpts().empty() && + _dbDriver) + { + // We assume "toS" has already features in RAM, so just lookup "fromS" + UDEBUG("Loading local visual features for signature %d", fromS.id()); + std::multimap words; + std::vector keypoints; + std::vector points; + cv::Mat descriptors; + UTimer timer; + _dbDriver->getLocalFeatures(fromS.id(), words, keypoints, points, descriptors); + if(!words.empty() && !keypoints.empty()) { + UASSERT(words.size() == fromS.getWords().size()); + std::map wordsChanged = fromS.getWordsChanged(); + bool wasEnabled = fromS.isEnabled(); + fromS.setWords(words, keypoints, points, descriptors); + for(const auto & iter: wordsChanged) { + fromS.changeWordsRef(iter.first, iter.second); + } + fromS.setEnabled(wasEnabled); + UDEBUG("Loaded %ld local visual features for signature %d! (in %f s)", words.size(), fromS.id(), timer.ticks()); + } + else + { + UDEBUG("Failed to load local visual features for signature %d.", fromS.id()); + } + + } // compute transform fromId -> toId std::vector inliersV; @@ -2980,8 +3348,10 @@ Transform Memory::computeTransform( !_invertedReg && !tmpTo.getWordsDescriptors().empty() && !tmpTo.getWords().empty() && + !tmpTo.getWordsKpts().empty() && !tmpFrom.getWordsDescriptors().empty() && !tmpFrom.getWords().empty() && + !tmpFrom.getWordsKpts().empty() && !tmpFrom.getWords3().empty() && fromS.hasLink(0, Link::kNeighbor)) // If doesn't have neighbors, skip bundle { @@ -3015,8 +3385,12 @@ Transform Memory::computeTransform( if(id != fromS.id() && iter->second.type() == Link::kNeighbor) // assemble only neighbors for the local feature map { const Signature * s = this->getSignature(id); - if(s && !s->getWords3().empty()) + if(s) { + if(s->getWordsKpts().empty() && s->getWords3().empty() && s->getWordsDescriptors().empty()) { + UDEBUG("Signature %d doesn't have features set. Cannot be added in the local feature map.", s->id()); + continue; + } const std::map & wordsTo = uMultimapToMapUnique(s->getWords()); for(std::map::const_iterator jter=wordsTo.begin(); jter!=wordsTo.end(); ++jter) { @@ -3115,6 +3489,11 @@ Transform Memory::computeTransform( bundlePoses.insert(std::make_pair(id, iter->second.transform())); } + if(s->getWordsKpts().empty()) + { + UDEBUG("Signature %d doesn't have features set. Keypoints won't be added in local bundle adjustment.", s->id()); + continue; + } const std::map & words = uMultimapToMapUnique(s->getWords()); for(std::map::const_iterator jter=words.begin(); jter!=words.end(); ++jter) { @@ -3538,7 +3917,7 @@ void Memory::updateLink(const Link & link, bool updateInDatabase) if(oldType!=Link::kVirtualClosure || link.type()!=Link::kVirtualClosure) { - _linksChanged = true; + _linksChanged = _incrementalMemory || (fromS->isSaved() && toS->isSaved()); } } else @@ -3590,7 +3969,7 @@ void Memory::removeAllVirtualLinks() void Memory::removeVirtualLinks(int signatureId) { - UDEBUG(""); + //UDEBUG(""); Signature * s = this->_getSignature(signatureId); if(s) { @@ -3629,10 +4008,7 @@ void Memory::dumpMemory(std::string directory) const void Memory::dumpDictionary(const char * fileNameRef, const char * fileNameDesc) const { - if(_vwd) - { - _vwd->exportDictionary(fileNameRef, fileNameDesc); - } + _vwd->exportDictionary(fileNameRef, fileNameDesc); } void Memory::dumpSignatures(const char * fileNameSign, bool words3D) const @@ -3753,10 +4129,7 @@ unsigned long Memory::getMemoryUsed() const { memoryUsage += iter->second->getMemoryUsed(true); } - if(_vwd) - { - memoryUsage += _vwd->getMemoryUsed(); - } + memoryUsage += _vwd->getMemoryUsed(); memoryUsage += _stMem.size() * (sizeof(int)+sizeof(std::set::iterator)) + sizeof(std::set); memoryUsage += _workingMem.size() * (sizeof(int)+sizeof(double)+sizeof(std::map::iterator)) + sizeof(std::map); memoryUsage += _groundTruths.size() * (sizeof(int)+sizeof(Transform)+12*sizeof(float) + sizeof(std::map::iterator)) + sizeof(std::map); @@ -3974,18 +4347,46 @@ bool Memory::rehearsalMerge(int oldId, int newId) // just update weight int w = oldS->getWeight()>=0?oldS->getWeight():0; newS->setWeight(w + newS->getWeight() + 1); - oldS->setWeight(intermediateMerge?-1:0); // convert to intermediate node + oldS->setWeight(0); if(_lastGlobalLoopClosureId == oldS->id()) { _lastGlobalLoopClosureId = newS->id(); } + if(intermediateMerge) + { + static bool warned = false; + if(!warned) + { + UWARN("A rehearsal was accepted (%d->%d) while not moving but " + "there are intermediate nodes between them in the graph. " + "Because %s=true, the node %d cannot be converted " + "into an intermediate node so it will be kept in the graph " + "even if we are not moving. Set %s=false to handle intermediate " + "nodes with rehearsal enabled so that loop closure hypotheses " + "are propagated correctly. This message is only " + "printed once.", + oldS->id(), + newS->id(), + Parameters::kMemRehearsalIdUpdatedToNewOne().c_str(), + oldS->id(), + Parameters::kMemRehearsalIdUpdatedToNewOne().c_str()); + warned = true; + } + } } else // !_idUpdatedToNewOneRehearsal { int w = newS->getWeight()>=0?newS->getWeight():0; oldS->setWeight(w + oldS->getWeight() + 1); - newS->setWeight(intermediateMerge?-1:0); // convert to intermediate node + if(intermediateMerge) + { + this->convertToIntermediate(newS->id()); + } + else + { + newS->setWeight(0); + } } } } @@ -4194,6 +4595,29 @@ void Memory::getNodeWordsAndGlobalDescriptors(int nodeId, words3 = s->getWords3(); wordsDescriptors = s->getWordsDescriptors(); globalDescriptors = s->sensorData().globalDescriptors(); + + if(!words.empty() && wordsKpts.empty() && _dbDriver) + { + std::multimap tmpWords; + _dbDriver->getLocalFeatures(nodeId, tmpWords, wordsKpts, words3, wordsDescriptors); + if(!tmpWords.empty() && !wordsKpts.empty()) + { + UASSERT(tmpWords.size() == words.size()); + std::map wordsChanged = s->getWordsChanged(); + for(const auto & iter: wordsChanged) { + std::list subwords = uValues(tmpWords, iter.first); // old id + if(subwords.size()) + { + tmpWords.erase(iter.first); + for(std::list::const_iterator jter=subwords.begin(); jter!=subwords.end(); ++jter) + { + tmpWords.insert(std::pair(iter.second, (*jter))); // new id + } + } + } + words = tmpWords; + } + } } else if(_dbDriver) { @@ -4507,6 +4931,11 @@ void Memory::copyData(const Signature * from, Signature * to) } to->sensorData().setId(to->id()); + if(!from->sensorData().globalDescriptors().empty()) + { + to->sensorData().setGlobalDescriptors(from->sensorData().globalDescriptors()); + } + to->setPose(from->getPose()); } else @@ -4539,6 +4968,12 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor bool isIntermediateNode = data.id() < 0; + if(this->getSignatures().empty() && isIntermediateNode) + { + UWARN("Ignoring input data with stamp %s because the first node in memory cannot be an intermediate node.", inputData.stamp()); + return 0; + } + // uncompress data if needed if(!isIntermediateNode) @@ -4761,10 +5196,16 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor int treeSize= int(_workingMem.size() + _stMem.size()); int meanWordsPerLocation = _feature2D->getMaxFeatures()>0?_feature2D->getMaxFeatures():0; - if(treeSize > 1) + if(meanWordsPerLocation==0 && treeSize > 1) { meanWordsPerLocation = _vwd->getTotalActiveReferences() / (treeSize-1); // ignore virtual signature } + else if(_useOdometryFeatures) { + // To not detect first image as bad signature if odometry + // is using less features than feature2D->getMaxFeatures() + meanWordsPerLocation = 0; + } + UDEBUG("ratio=%f, meanWordsPerLocation=%d", _badSignRatio, meanWordsPerLocation); if(_parallelized && !isIntermediateNode) { @@ -4868,10 +5309,12 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor SensorData decimatedData; UDEBUG("Received kpts=%d kpts3D=%d, descriptors=%d _useOdometryFeatures=%s", (int)data.keypoints().size(), (int)data.keypoints3D().size(), data.descriptors().rows, _useOdometryFeatures?"true":"false"); + // TODO: do we still need the third and fouth comparisons? + // TODO: there is significant repetitive code between the if and the else, could we combine them?! if(!_useOdometryFeatures || - data.keypoints().empty() || + (!_receivingOdometryFeatures && data.keypoints().empty()) || (int)data.keypoints().size() != data.descriptors().rows || - (_feature2D->getType() == Feature2D::kFeatureOrbOctree && data.descriptors().empty())) + (!_receivingOdometryFeatures && _feature2D->getType() == Feature2D::kFeatureOrbOctree && data.descriptors().empty())) { if(_feature2D->getMaxFeatures() >= 0 && !data.imageRaw().empty() && !isIntermediateNode) { @@ -4950,16 +5393,7 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor { UASSERT(!decimatedData.cameraModels().empty()); UDEBUG("Masking floor (threshold=%f)", _maskFloorThreshold); - if(_maskFloorThreshold<0.0f) - { - cv::Mat depthBelow; - util3d::filterFloor(depthMask, decimatedData.cameraModels(), _maskFloorThreshold*-1.0f, &depthBelow); - depthMask = depthBelow; - } - else - { - depthMask = util3d::filterFloor(depthMask, decimatedData.cameraModels(), _maskFloorThreshold); - } + depthMask = util3d::filterFloor(depthMask, decimatedData.cameraModels(), _maskFloorThreshold); UDEBUG("Masking floor done."); } @@ -5009,6 +5443,7 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor else { int oldMaxFeatures = _feature2D->getMaxFeatures(); + bool oldSSC = _feature2D->getSSC(); UDEBUG("rawDescriptorsKept=%d, pose=%d, maxFeatures=%d, visMaxFeatures=%d", _rawDescriptorsKept?1:0, pose.isNull()?0:1, _feature2D->getMaxFeatures(), _visMaxFeatures); ParametersMap tmpMaxFeatureParameter; if(_rawDescriptorsKept&&!pose.isNull()&&_feature2D->getMaxFeatures()>0&&_feature2D->getMaxFeatures()<_visMaxFeatures) @@ -5016,6 +5451,7 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor // The total extracted features should match the number of features used for transformation estimation UDEBUG("Changing temporary max features from %d to %d", _feature2D->getMaxFeatures(), _visMaxFeatures); tmpMaxFeatureParameter.insert(ParametersPair(Parameters::kKpMaxFeatures(), uNumber2Str(_visMaxFeatures))); + tmpMaxFeatureParameter.insert(ParametersPair(Parameters::kKpSSC(), uNumber2Str(_visSSC))); _feature2D->parseParameters(tmpMaxFeatureParameter); } @@ -5026,6 +5462,7 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor if(tmpMaxFeatureParameter.size()) { tmpMaxFeatureParameter.at(Parameters::kKpMaxFeatures()) = uNumber2Str(oldMaxFeatures); + tmpMaxFeatureParameter.at(Parameters::kKpSSC()) = uBool2Str(oldSSC); _feature2D->parseParameters(tmpMaxFeatureParameter); // reset back } t = timer.ticks(); @@ -5038,41 +5475,90 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor if(stats) stats->addStatistic(Statistics::kTimingMemDescriptors_extraction(), t*1000.0f); UDEBUG("time descriptors (%d) = %fs", descriptors.rows, t); - UDEBUG("ratio=%f, meanWordsPerLocation=%d", _badSignRatio, meanWordsPerLocation); - if(descriptors.rows && descriptors.rows < _badSignRatio * float(meanWordsPerLocation)) + if(!imagesRectified && decimatedData.cameraModels().size()) { - descriptors = cv::Mat(); - } - else - { - if(!imagesRectified && decimatedData.cameraModels().size()) - { - UASSERT_MSG((int)keypoints.size() == descriptors.rows, uFormat("%d vs %d", (int)keypoints.size(), descriptors.rows).c_str()); - std::vector keypointsValid; - keypointsValid.reserve(keypoints.size()); - cv::Mat descriptorsValid; - descriptorsValid.reserve(descriptors.rows); + UASSERT_MSG((int)keypoints.size() == descriptors.rows, uFormat("%d vs %d", (int)keypoints.size(), descriptors.rows).c_str()); + std::vector keypointsValid; + keypointsValid.reserve(keypoints.size()); + cv::Mat descriptorsValid; + descriptorsValid.reserve(descriptors.rows); - //undistort keypoints before projection (RGB-D) - if(decimatedData.cameraModels().size() == 1) + //undistort keypoints before projection (RGB-D) + if(decimatedData.cameraModels().size() == 1) + { + std::vector pointsIn, pointsOut; + cv::KeyPoint::convert(keypoints,pointsIn); + if(decimatedData.cameraModels()[0].D_raw().cols == 6) { +#if CV_MAJOR_VERSION > 2 or (CV_MAJOR_VERSION == 2 and (CV_MINOR_VERSION >4 or (CV_MINOR_VERSION == 4 and CV_SUBMINOR_VERSION >=10))) + // Equidistant / FishEye + // get only k parameters (k1,k2,p1,p2,k3,k4) + cv::Mat D(1, 4, CV_64FC1); + D.at(0,0) = decimatedData.cameraModels()[0].D_raw().at(0,0); + D.at(0,1) = decimatedData.cameraModels()[0].D_raw().at(0,1); + D.at(0,2) = decimatedData.cameraModels()[0].D_raw().at(0,4); + D.at(0,3) = decimatedData.cameraModels()[0].D_raw().at(0,5); + cv::fisheye::undistortPoints(pointsIn, pointsOut, + decimatedData.cameraModels()[0].K_raw(), + D, + decimatedData.cameraModels()[0].R(), + decimatedData.cameraModels()[0].P()); + } + else +#else + UWARN("Too old opencv version (%d,%d,%d) to support fisheye model (min 2.4.10 required)!", + CV_MAJOR_VERSION, CV_MINOR_VERSION, CV_SUBMINOR_VERSION); + } +#endif + { + //RadialTangential + cv::undistortPoints(pointsIn, pointsOut, + decimatedData.cameraModels()[0].K_raw(), + decimatedData.cameraModels()[0].D_raw(), + decimatedData.cameraModels()[0].R(), + decimatedData.cameraModels()[0].P()); + } + UASSERT(pointsOut.size() == keypoints.size()); + for(unsigned int i=0; i=0 && pointsOut.at(i).x=0 && pointsOut.at(i).y= 0 && cameraIndex < (int)decimatedData.cameraModels().size(), + uFormat("cameraIndex=%d, models=%d, kpt.x=%f, subImageWidth=%f (Camera model image width=%d)", + cameraIndex, (int)decimatedData.cameraModels().size(), keypoints[i].pt.x, subImageWidth, decimatedData.cameraModels()[0].imageWidth()).c_str()); + std::vector pointsIn, pointsOut; - cv::KeyPoint::convert(keypoints,pointsIn); - if(decimatedData.cameraModels()[0].D_raw().cols == 6) + pointsIn.push_back(cv::Point2f(keypoints.at(i).pt.x-subImageWidth*cameraIndex, keypoints.at(i).pt.y)); + if(decimatedData.cameraModels()[cameraIndex].D_raw().cols == 6) { #if CV_MAJOR_VERSION > 2 or (CV_MAJOR_VERSION == 2 and (CV_MINOR_VERSION >4 or (CV_MINOR_VERSION == 4 and CV_SUBMINOR_VERSION >=10))) // Equidistant / FishEye // get only k parameters (k1,k2,p1,p2,k3,k4) cv::Mat D(1, 4, CV_64FC1); - D.at(0,0) = decimatedData.cameraModels()[0].D_raw().at(0,0); - D.at(0,1) = decimatedData.cameraModels()[0].D_raw().at(0,1); - D.at(0,2) = decimatedData.cameraModels()[0].D_raw().at(0,4); - D.at(0,3) = decimatedData.cameraModels()[0].D_raw().at(0,5); + D.at(0,0) = decimatedData.cameraModels()[cameraIndex].D_raw().at(0,0); + D.at(0,1) = decimatedData.cameraModels()[cameraIndex].D_raw().at(0,1); + D.at(0,2) = decimatedData.cameraModels()[cameraIndex].D_raw().at(0,4); + D.at(0,3) = decimatedData.cameraModels()[cameraIndex].D_raw().at(0,5); cv::fisheye::undistortPoints(pointsIn, pointsOut, - decimatedData.cameraModels()[0].K_raw(), + decimatedData.cameraModels()[cameraIndex].K_raw(), D, - decimatedData.cameraModels()[0].R(), - decimatedData.cameraModels()[0].P()); + decimatedData.cameraModels()[cameraIndex].R(), + decimatedData.cameraModels()[cameraIndex].P()); } else #else @@ -5083,115 +5569,58 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor { //RadialTangential cv::undistortPoints(pointsIn, pointsOut, - decimatedData.cameraModels()[0].K_raw(), - decimatedData.cameraModels()[0].D_raw(), - decimatedData.cameraModels()[0].R(), - decimatedData.cameraModels()[0].P()); + decimatedData.cameraModels()[cameraIndex].K_raw(), + decimatedData.cameraModels()[cameraIndex].D_raw(), + decimatedData.cameraModels()[cameraIndex].R(), + decimatedData.cameraModels()[cameraIndex].P()); } - UASSERT(pointsOut.size() == keypoints.size()); - for(unsigned int i=0; i=0 && pointsOut[0].x=0 && pointsOut[0].y=0 && pointsOut.at(i).x=0 && pointsOut.at(i).y= 0 && cameraIndex < (int)decimatedData.cameraModels().size(), - uFormat("cameraIndex=%d, models=%d, kpt.x=%f, subImageWidth=%f (Camera model image width=%d)", - cameraIndex, (int)decimatedData.cameraModels().size(), keypoints[i].pt.x, subImageWidth, decimatedData.cameraModels()[0].imageWidth()).c_str()); - - std::vector pointsIn, pointsOut; - pointsIn.push_back(cv::Point2f(keypoints.at(i).pt.x-subImageWidth*cameraIndex, keypoints.at(i).pt.y)); - if(decimatedData.cameraModels()[cameraIndex].D_raw().cols == 6) - { -#if CV_MAJOR_VERSION > 2 or (CV_MAJOR_VERSION == 2 and (CV_MINOR_VERSION >4 or (CV_MINOR_VERSION == 4 and CV_SUBMINOR_VERSION >=10))) - // Equidistant / FishEye - // get only k parameters (k1,k2,p1,p2,k3,k4) - cv::Mat D(1, 4, CV_64FC1); - D.at(0,0) = decimatedData.cameraModels()[cameraIndex].D_raw().at(0,0); - D.at(0,1) = decimatedData.cameraModels()[cameraIndex].D_raw().at(0,1); - D.at(0,2) = decimatedData.cameraModels()[cameraIndex].D_raw().at(0,4); - D.at(0,3) = decimatedData.cameraModels()[cameraIndex].D_raw().at(0,5); - cv::fisheye::undistortPoints(pointsIn, pointsOut, - decimatedData.cameraModels()[cameraIndex].K_raw(), - D, - decimatedData.cameraModels()[cameraIndex].R(), - decimatedData.cameraModels()[cameraIndex].P()); - } - else -#else - UWARN("Too old opencv version (%d,%d,%d) to support fisheye model (min 2.4.10 required)!", - CV_MAJOR_VERSION, CV_MINOR_VERSION, CV_SUBMINOR_VERSION); - } -#endif - { - //RadialTangential - cv::undistortPoints(pointsIn, pointsOut, - decimatedData.cameraModels()[cameraIndex].K_raw(), - decimatedData.cameraModels()[cameraIndex].D_raw(), - decimatedData.cameraModels()[cameraIndex].R(), - decimatedData.cameraModels()[cameraIndex].P()); - } - - if(pointsOut[0].x>=0 && pointsOut[0].x=0 && pointsOut[0].yaddStatistic(Statistics::kTimingMemRectification(), t*1000.0f); - UDEBUG("time rectification = %fs", t); } - if(useProvided3dPoints && keypoints.size() != data.keypoints3D().size()) + keypoints = keypointsValid; + descriptors = descriptorsValid; + + t = timer.ticks(); + if(stats) stats->addStatistic(Statistics::kTimingMemRectification(), t*1000.0f); + UDEBUG("time rectification = %fs", t); + } + + if(useProvided3dPoints && keypoints.size() != data.keypoints3D().size()) + { + UDEBUG("Using provided 3d points (%d->%d)", (int)data.keypoints3D().size(), (int)keypoints.size()); + keypoints3D.resize(keypoints.size()); + for(size_t i=0; i%d)", (int)data.keypoints3D().size(), (int)keypoints.size()); - keypoints3D.resize(keypoints.size()); - for(size_t i=0; igenerateKeypoints3D(decimatedData, keypoints); - t = timer.ticks(); - 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()); + UASSERT(keypoints[i].class_id < (int)data.keypoints3D().size()); + keypoints3D[i] = data.keypoints3D()[keypoints[i].class_id]; } } + else if(useProvided3dPoints && keypoints.size() == data.keypoints3D().size()) + { + UDEBUG("Using provided 3d points (%d)", (int)data.keypoints3D().size()); + keypoints3D = data.keypoints3D(); + } + else if((!decimatedData.depthRaw().empty() && decimatedData.cameraModels().size() && decimatedData.cameraModels()[0].isValidForProjection()) || + (!decimatedData.rightRaw().empty() && decimatedData.stereoCameraModels().size() && decimatedData.stereoCameraModels()[0].isValidForProjection())) + { + keypoints3D = _feature2D->generateKeypoints3D(decimatedData, keypoints); + t = timer.ticks(); + 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()) { @@ -5206,8 +5635,9 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor UDEBUG("Intermediate node detected, don't extract features!"); } } - else if(_feature2D->getMaxFeatures() >= 0 && !isIntermediateNode) + else { + _receivingOdometryFeatures = true; UINFO("Use odometry features: kpts=%d 3d=%d desc=%d (dim=%d, type=%d)", (int)data.keypoints().size(), (int)data.keypoints3D().size(), @@ -5221,133 +5651,75 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor UASSERT(descriptors.empty() || descriptors.rows == (int)keypoints.size()); UASSERT(keypoints3D.empty() || keypoints3D.size() == keypoints.size()); - int maxFeatures = _rawDescriptorsKept&&!pose.isNull()&&_feature2D->getMaxFeatures()>0&&_feature2D->getMaxFeatures()<_visMaxFeatures?_visMaxFeatures:_feature2D->getMaxFeatures(); - bool ssc = _rawDescriptorsKept&&!pose.isNull()&&_feature2D->getMaxFeatures()>0&&_feature2D->getMaxFeatures()<_visMaxFeatures?_visSSC:_feature2D->getSSC(); - if((int)keypoints.size() > maxFeatures) + if(_feature2D->getMaxFeatures() >= 0 && !isIntermediateNode) { - if(data.cameraModels().size()==1 || data.stereoCameraModels().size()==1) - _feature2D->limitKeypoints(keypoints, keypoints3D, descriptors, maxFeatures, data.cameraModels().size()?data.cameraModels()[0].imageSize():data.stereoCameraModels()[0].left().imageSize(), ssc); - else - _feature2D->limitKeypoints(keypoints, keypoints3D, descriptors, maxFeatures); - } - t = timer.ticks(); - if(stats) stats->addStatistic(Statistics::kTimingMemKeypoints_detection(), t*1000.0f); - UDEBUG("time keypoints (%d) = %fs", (int)keypoints.size(), t); - - if(descriptors.empty()) - { - cv::Mat imageMono; - if(data.imageRaw().channels() == 3) + int maxFeatures = _rawDescriptorsKept&&!pose.isNull()&&_feature2D->getMaxFeatures()>0&&_feature2D->getMaxFeatures()<_visMaxFeatures?_visMaxFeatures:_feature2D->getMaxFeatures(); + if((int)keypoints.size() > maxFeatures) { - cv::cvtColor(data.imageRaw(), imageMono, CV_BGR2GRAY); - } - else - { - imageMono = data.imageRaw(); - } - - UASSERT_MSG(imagesRectified, "Cannot extract descriptors on not rectified image from keypoints which assumed to be undistorted"); - descriptors = _feature2D->generateDescriptors(imageMono, keypoints); - } - else if(!imagesRectified && !data.cameraModels().empty()) - { - std::vector keypointsValid; - keypointsValid.reserve(keypoints.size()); - cv::Mat descriptorsValid; - descriptorsValid.reserve(descriptors.rows); - std::vector keypoints3DValid; - keypoints3DValid.reserve(keypoints3D.size()); - - //undistort keypoints before projection (RGB-D) - if(data.cameraModels().size() == 1) - { - std::vector pointsIn, pointsOut; - cv::KeyPoint::convert(keypoints,pointsIn); - if(data.cameraModels()[0].D_raw().cols == 6) - { -#if CV_MAJOR_VERSION > 2 or (CV_MAJOR_VERSION == 2 and (CV_MINOR_VERSION >4 or (CV_MINOR_VERSION == 4 and CV_SUBMINOR_VERSION >=10))) - // Equidistant / FishEye - // get only k parameters (k1,k2,p1,p2,k3,k4) - cv::Mat D(1, 4, CV_64FC1); - D.at(0,0) = data.cameraModels()[0].D_raw().at(0,0); - D.at(0,1) = data.cameraModels()[0].D_raw().at(0,1); - D.at(0,2) = data.cameraModels()[0].D_raw().at(0,4); - D.at(0,3) = data.cameraModels()[0].D_raw().at(0,5); - cv::fisheye::undistortPoints(pointsIn, pointsOut, - data.cameraModels()[0].K_raw(), - D, - data.cameraModels()[0].R(), - data.cameraModels()[0].P()); - } + bool ssc = _rawDescriptorsKept&&!pose.isNull()&&_feature2D->getMaxFeatures()>0&&_feature2D->getMaxFeatures()<_visMaxFeatures?_visSSC:_feature2D->getSSC(); + if(data.cameraModels().size()>=1 || data.stereoCameraModels().size()>=1) + _feature2D->limitKeypoints(keypoints, + keypoints3D, + descriptors, + maxFeatures, + data.cameraModels().size()?cv::Size(data.cameraModels()[0].imageWidth()*data.cameraModels().size(), + data.cameraModels()[0].imageHeight()):cv::Size(data.stereoCameraModels()[0].left().imageWidth()*data.stereoCameraModels().size(), + data.stereoCameraModels()[0].left().imageHeight()), + ssc); else -#else - UWARN("Too old opencv version (%d,%d,%d) to support fisheye model (min 2.4.10 required)!", - CV_MAJOR_VERSION, CV_MINOR_VERSION, CV_SUBMINOR_VERSION); - } -#endif - { - //RadialTangential - cv::undistortPoints(pointsIn, pointsOut, - data.cameraModels()[0].K_raw(), - data.cameraModels()[0].D_raw(), - data.cameraModels()[0].R(), - data.cameraModels()[0].P()); - } - UASSERT(pointsOut.size() == keypoints.size()); - for(unsigned int i=0; i=0 && pointsOut.at(i).x=0 && pointsOut.at(i).ylimitKeypoints(keypoints, + keypoints3D, + descriptors, + maxFeatures); } - else + t = timer.ticks(); + if(stats) stats->addStatistic(Statistics::kTimingMemKeypoints_detection(), t*1000.0f); + UDEBUG("time keypoints (%d) = %fs", (int)keypoints.size(), t); + + if(descriptors.empty()) { - float subImageWidth; - if(!data.imageRaw().empty()) + cv::Mat imageMono; + if(data.imageRaw().channels() == 3) { - UASSERT(int((data.imageRaw().cols/data.cameraModels().size())*data.cameraModels().size()) == data.imageRaw().cols); - subImageWidth = data.imageRaw().cols/data.cameraModels().size(); + cv::cvtColor(data.imageRaw(), imageMono, CV_BGR2GRAY); } else { - UASSERT(data.cameraModels()[0].imageWidth()>0); - subImageWidth = data.cameraModels()[0].imageWidth(); + imageMono = data.imageRaw(); } - for(unsigned int i=0; i= 0 && cameraIndex < (int)data.cameraModels().size(), - uFormat("cameraIndex=%d, models=%d, kpt.x=%f, subImageWidth=%f (Camera model image width=%d)", - cameraIndex, (int)data.cameraModels().size(), keypoints[i].pt.x, subImageWidth, data.cameraModels()[0].imageWidth()).c_str()); + UASSERT_MSG(imagesRectified, "Cannot extract descriptors on not rectified image from keypoints which assumed to be undistorted"); + descriptors = _feature2D->generateDescriptors(imageMono, keypoints); + } + else if(!imagesRectified && !data.cameraModels().empty()) + { + std::vector keypointsValid; + keypointsValid.reserve(keypoints.size()); + cv::Mat descriptorsValid; + descriptorsValid.reserve(descriptors.rows); + std::vector keypoints3DValid; + keypoints3DValid.reserve(keypoints3D.size()); + //undistort keypoints before projection (RGB-D) + if(data.cameraModels().size() == 1) + { std::vector pointsIn, pointsOut; - pointsIn.push_back(cv::Point2f(keypoints.at(i).pt.x-subImageWidth*cameraIndex, keypoints.at(i).pt.y)); - if(data.cameraModels()[cameraIndex].D_raw().cols == 6) + cv::KeyPoint::convert(keypoints,pointsIn); + if(data.cameraModels()[0].D_raw().cols == 6) { #if CV_MAJOR_VERSION > 2 or (CV_MAJOR_VERSION == 2 and (CV_MINOR_VERSION >4 or (CV_MINOR_VERSION == 4 and CV_SUBMINOR_VERSION >=10))) // Equidistant / FishEye // get only k parameters (k1,k2,p1,p2,k3,k4) cv::Mat D(1, 4, CV_64FC1); - D.at(0,0) = data.cameraModels()[cameraIndex].D_raw().at(0,0); - D.at(0,1) = data.cameraModels()[cameraIndex].D_raw().at(0,1); - D.at(0,2) = data.cameraModels()[cameraIndex].D_raw().at(0,4); - D.at(0,3) = data.cameraModels()[cameraIndex].D_raw().at(0,5); + D.at(0,0) = data.cameraModels()[0].D_raw().at(0,0); + D.at(0,1) = data.cameraModels()[0].D_raw().at(0,1); + D.at(0,2) = data.cameraModels()[0].D_raw().at(0,4); + D.at(0,3) = data.cameraModels()[0].D_raw().at(0,5); cv::fisheye::undistortPoints(pointsIn, pointsOut, - data.cameraModels()[cameraIndex].K_raw(), + data.cameraModels()[0].K_raw(), D, - data.cameraModels()[cameraIndex].R(), - data.cameraModels()[cameraIndex].P()); + data.cameraModels()[0].R(), + data.cameraModels()[0].P()); } else #else @@ -5358,57 +5730,122 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor { //RadialTangential cv::undistortPoints(pointsIn, pointsOut, - data.cameraModels()[cameraIndex].K_raw(), - data.cameraModels()[cameraIndex].D_raw(), - data.cameraModels()[cameraIndex].R(), - data.cameraModels()[cameraIndex].P()); + data.cameraModels()[0].K_raw(), + data.cameraModels()[0].D_raw(), + data.cameraModels()[0].R(), + data.cameraModels()[0].P()); } - - if(pointsOut[0].x>=0 && pointsOut[0].x=0 && pointsOut[0].y=0 && pointsOut.at(i).x=0 && pointsOut.at(i).y0); + subImageWidth = data.cameraModels()[0].imageWidth(); + } + + for(unsigned int i=0; i= 0 && cameraIndex < (int)data.cameraModels().size(), + uFormat("cameraIndex=%d, models=%d, kpt.x=%f, subImageWidth=%f (Camera model image width=%d)", + cameraIndex, (int)data.cameraModels().size(), keypoints[i].pt.x, subImageWidth, data.cameraModels()[0].imageWidth()).c_str()); + + std::vector pointsIn, pointsOut; + pointsIn.push_back(cv::Point2f(keypoints.at(i).pt.x-subImageWidth*cameraIndex, keypoints.at(i).pt.y)); + if(data.cameraModels()[cameraIndex].D_raw().cols == 6) + { +#if CV_MAJOR_VERSION > 2 or (CV_MAJOR_VERSION == 2 and (CV_MINOR_VERSION >4 or (CV_MINOR_VERSION == 4 and CV_SUBMINOR_VERSION >=10))) + // Equidistant / FishEye + // get only k parameters (k1,k2,p1,p2,k3,k4) + cv::Mat D(1, 4, CV_64FC1); + D.at(0,0) = data.cameraModels()[cameraIndex].D_raw().at(0,0); + D.at(0,1) = data.cameraModels()[cameraIndex].D_raw().at(0,1); + D.at(0,2) = data.cameraModels()[cameraIndex].D_raw().at(0,4); + D.at(0,3) = data.cameraModels()[cameraIndex].D_raw().at(0,5); + cv::fisheye::undistortPoints(pointsIn, pointsOut, + data.cameraModels()[cameraIndex].K_raw(), + D, + data.cameraModels()[cameraIndex].R(), + data.cameraModels()[cameraIndex].P()); + } + else +#else + UWARN("Too old opencv version (%d,%d,%d) to support fisheye model (min 2.4.10 required)!", + CV_MAJOR_VERSION, CV_MINOR_VERSION, CV_SUBMINOR_VERSION); + } +#endif + { + //RadialTangential + cv::undistortPoints(pointsIn, pointsOut, + data.cameraModels()[cameraIndex].K_raw(), + data.cameraModels()[cameraIndex].D_raw(), + data.cameraModels()[cameraIndex].R(), + data.cameraModels()[cameraIndex].P()); + } + + if(pointsOut[0].x>=0 && pointsOut[0].x=0 && pointsOut[0].yaddStatistic(Statistics::kTimingMemRectification(), t*1000.0f); + UDEBUG("time rectification = %fs", t); } - - keypoints = keypointsValid; - descriptors = descriptorsValid; - keypoints3D = keypoints3DValid; - t = timer.ticks(); - if(stats) stats->addStatistic(Statistics::kTimingMemRectification(), t*1000.0f); - UDEBUG("time rectification = %fs", t); - } - t = timer.ticks(); - if(stats) stats->addStatistic(Statistics::kTimingMemDescriptors_extraction(), t*1000.0f); - UDEBUG("time descriptors (%d) = %fs", descriptors.rows, t); + if(stats) stats->addStatistic(Statistics::kTimingMemDescriptors_extraction(), t*1000.0f); + UDEBUG("time descriptors (%d) = %fs", descriptors.rows, t); - if(keypoints3D.empty() && - ((!data.depthRaw().empty() && data.cameraModels().size() && data.cameraModels()[0].isValidForProjection()) || - (!data.rightRaw().empty() && data.stereoCameraModels().size() && data.stereoCameraModels()[0].isValidForProjection()))) - { - keypoints3D = _feature2D->generateKeypoints3D(data, keypoints); - } - if(_feature2D->getMinDepth() > 0.0f || _feature2D->getMaxDepth() > 0.0f) - { - _feature2D->filterKeypointsByDepth(keypoints, descriptors, keypoints3D, _feature2D->getMinDepth(), _feature2D->getMaxDepth()); - } - t = timer.ticks(); - if(stats) stats->addStatistic(Statistics::kTimingMemKeypoints_3D(), t*1000.0f); - UDEBUG("time keypoints 3D (%d) = %fs", (int)keypoints3D.size(), t); - - UDEBUG("ratio=%f, meanWordsPerLocation=%d", _badSignRatio, meanWordsPerLocation); - if(descriptors.rows && descriptors.rows < _badSignRatio * float(meanWordsPerLocation)) - { - descriptors = cv::Mat(); + if(keypoints3D.empty() && + ((!data.depthRaw().empty() && data.cameraModels().size() && data.cameraModels()[0].isValidForProjection()) || + (!data.rightRaw().empty() && data.stereoCameraModels().size() && data.stereoCameraModels()[0].isValidForProjection()))) + { + keypoints3D = _feature2D->generateKeypoints3D(data, keypoints); + } + if(_feature2D->getMinDepth() > 0.0f || _feature2D->getMaxDepth() > 0.0f) + { + _feature2D->filterKeypointsByDepth(keypoints, descriptors, keypoints3D, _feature2D->getMinDepth(), _feature2D->getMaxDepth()); + } + t = timer.ticks(); + if(stats) stats->addStatistic(Statistics::kTimingMemKeypoints_3D(), t*1000.0f); + UDEBUG("time keypoints 3D (%d) = %fs", (int)keypoints3D.size(), t); } } @@ -5431,103 +5868,123 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor } std::list wordIds; - if(descriptors.rows) + bool addedToDictionary = false; + if(!keypoints.empty()) { - // In case the number of features we want to do quantization is lower - // than extracted ones (that would be used for transform estimation) - std::vector inliers; - cv::Mat descriptorsForQuantization = descriptors; - std::vector quantizedToRawIndices; - if(_feature2D->getMaxFeatures()>0 && descriptors.rows > _feature2D->getMaxFeatures()) + if(descriptors.rows && + !isIntermediateNode && // don't add intermediate nodes to dictionary + descriptors.rows >= int(_badSignRatio * float(meanWordsPerLocation))) // don't add bad signatures to dictionary { - UASSERT((int)keypoints.size() == descriptors.rows); - int inliersCount = 0; - if((_feature2D->getGridRows() > 1 || _feature2D->getGridCols() > 1) && - (decimatedData.cameraModels().size()==1 || decimatedData.stereoCameraModels().size()==1 || - data.cameraModels().size()==1 || data.stereoCameraModels().size()==1)) + // In case the number of features we want to do quantization is lower + // than extracted ones (that would be used for transform estimation) + std::vector inliers; + cv::Mat descriptorsForQuantization = descriptors; + std::vector quantizedToRawIndices; + if(_feature2D->getMaxFeatures()>0 && descriptors.rows > _feature2D->getMaxFeatures()) { - Feature2D::limitKeypoints(keypoints, inliers, _feature2D->getMaxFeatures(), - decimatedData.cameraModels().size()?decimatedData.cameraModels()[0].imageSize(): - decimatedData.stereoCameraModels().size()?decimatedData.stereoCameraModels()[0].left().imageSize(): - data.cameraModels().size()?data.cameraModels()[0].imageSize():data.stereoCameraModels()[0].left().imageSize(), - _feature2D->getGridRows(), _feature2D->getGridCols(), _feature2D->getSSC()); - } - else - { - if(_feature2D->getGridRows() > 1 || _feature2D->getGridCols() > 1) - { - UWARN("Ignored %s and %s parameters as they cannot be used for multi-cameras setup or uncalibrated camera.", - Parameters::kKpGridCols().c_str(), Parameters::kKpGridRows().c_str()); - } - if(decimatedData.cameraModels().size()==1 || decimatedData.stereoCameraModels().size()==1 || - data.cameraModels().size()==1 || data.stereoCameraModels().size()==1) + UASSERT((int)keypoints.size() == descriptors.rows); + int inliersCount = 0; + if((_feature2D->getGridRows() > 1 || _feature2D->getGridCols() > 1) && + (decimatedData.cameraModels().size()==1 || decimatedData.stereoCameraModels().size()==1 || + data.cameraModels().size()==1 || data.stereoCameraModels().size()==1)) { Feature2D::limitKeypoints(keypoints, inliers, _feature2D->getMaxFeatures(), decimatedData.cameraModels().size()?decimatedData.cameraModels()[0].imageSize(): decimatedData.stereoCameraModels().size()?decimatedData.stereoCameraModels()[0].left().imageSize(): data.cameraModels().size()?data.cameraModels()[0].imageSize():data.stereoCameraModels()[0].left().imageSize(), - _feature2D->getSSC()); + _feature2D->getGridRows(), _feature2D->getGridCols(), _feature2D->getSSC()); } else { - Feature2D::limitKeypoints(keypoints, inliers, _feature2D->getMaxFeatures()); - } - } - for(size_t i=0; igetGridRows() > 1 || _feature2D->getGridCols() > 1) { - memcpy(descriptorsForQuantization.ptr(oi), descriptors.ptr(k), descriptors.cols*sizeof(float)); + UWARN("Ignored %s and %s parameters as they cannot be used for multi-cameras setup or uncalibrated camera.", + Parameters::kKpGridCols().c_str(), Parameters::kKpGridRows().c_str()); + } + if(decimatedData.cameraModels().size()>=1 || decimatedData.stereoCameraModels().size()>=1 || + data.cameraModels().size()>=1 || data.stereoCameraModels().size()>=1) + { + Feature2D::limitKeypoints( + keypoints, + inliers, + _feature2D->getMaxFeatures(), + decimatedData.cameraModels().size()?cv::Size(decimatedData.cameraModels()[0].imageWidth()*decimatedData.cameraModels().size(), decimatedData.cameraModels()[0].imageHeight()): + decimatedData.stereoCameraModels().size()?cv::Size(decimatedData.stereoCameraModels()[0].left().imageWidth()*decimatedData.stereoCameraModels().size(), decimatedData.stereoCameraModels()[0].left().imageWidth()): + data.cameraModels().size()?cv::Size(data.cameraModels()[0].imageWidth()*data.cameraModels().size(), data.cameraModels()[0].imageHeight()): + cv::Size(data.stereoCameraModels()[0].left().imageWidth()*data.stereoCameraModels().size(), data.stereoCameraModels()[0].left().imageHeight()), + _feature2D->getSSC()); } else { - memcpy(descriptorsForQuantization.ptr(oi), descriptors.ptr(k), descriptors.cols*sizeof(char)); + Feature2D::limitKeypoints(keypoints, inliers, _feature2D->getMaxFeatures()); } - quantizedToRawIndices[oi] = k; - ++oi; } - } - UASSERT_MSG((int)oi == inliersCount, - uFormat("oi=%d inliersCount=%d (maxFeatures=%d, grid=%dx%d)", - oi, inliersCount, _feature2D->getMaxFeatures(), _feature2D->getGridCols(), _feature2D->getGridRows()).c_str()); - } - - // Quantization to vocabulary - wordIds = _vwd->addNewWords(descriptorsForQuantization, id); - - // Set ID -1 to features not used for quantization - if(wordIds.size() < keypoints.size()) - { - std::vector allWordIds; - allWordIds.resize(keypoints.size(),-1); - int i=0; - for(std::list::iterator iter=wordIds.begin(); iter!=wordIds.end(); ++iter) - { - allWordIds[quantizedToRawIndices[i]] = *iter; - ++i; - } - int negIndex = -1; - for(i=0; i<(int)allWordIds.size(); ++i) - { - if(allWordIds[i] < 0) + for(size_t i=0; i(oi), descriptors.ptr(k), descriptors.cols*sizeof(float)); + } + else + { + memcpy(descriptorsForQuantization.ptr(oi), descriptors.ptr(k), descriptors.cols*sizeof(char)); + } + quantizedToRawIndices[oi] = k; + ++oi; + } + } + UASSERT_MSG((int)oi == inliersCount, + uFormat("oi=%d inliersCount=%d (maxFeatures=%d, grid=%dx%d)", + oi, inliersCount, _feature2D->getMaxFeatures(), _feature2D->getGridCols(), _feature2D->getGridRows()).c_str()); + } + + // Quantization to vocabulary + wordIds = _vwd->addNewWords(descriptorsForQuantization, id); + addedToDictionary = true; + + // Set ID -1 to features not used for quantization + if(wordIds.size() < keypoints.size()) + { + std::vector allWordIds; + allWordIds.resize(keypoints.size(),-1); + int i=0; + for(std::list::iterator iter=wordIds.begin(); iter!=wordIds.end(); ++iter) + { + allWordIds[quantizedToRawIndices[i]] = *iter; + ++i; + } + int negIndex = -1; + for(i=0; i<(int)allWordIds.size(); ++i) + { + if(allWordIds[i] < 0) + { + allWordIds[i] = negIndex--; + } + } + wordIds = uVectorToList(allWordIds); + } + } + else + { + // Set all words as not used in dictionary + int negIndex = -1; + for(size_t i=0; i 0) { UASSERT(wordIds.size() == keypoints.size()); + UASSERT(descriptors.rows == 0 || descriptors.rows == (int)wordIds.size()); UASSERT(keypoints3D.size() == 0 || keypoints3D.size() == wordIds.size()); unsigned int i=0; float decimationRatio = float(preDecimation) / float(_imagePostDecimation); @@ -5554,7 +6012,7 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor for(std::list::iterator iter=wordIds.begin(); iter!=wordIds.end() && i < keypoints.size(); ++iter, ++i) { cv::KeyPoint kpt = keypoints[i]; - if(preDecimation != _imagePostDecimation) + if(preDecimation != _imagePostDecimation && !isIntermediateNode) { // remap keypoints to final image size kpt.pt.x *= decimationRatio; @@ -5573,7 +6031,7 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor ++words3DValid; } } - if(_rawDescriptorsKept) + if(!descriptors.empty() && _rawDescriptorsKept) { wordsDescriptors.push_back(descriptors.row(i)); } @@ -5581,12 +6039,7 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor } Landmarks landmarks = data.landmarks(); - if(!landmarks.empty() && isIntermediateNode) - { - UDEBUG("Landmarks provided (size=%ld) are ignored because this signature is set as intermediate.", landmarks.size()); - landmarks.clear(); - } - else if(_detectMarkers && !isIntermediateNode && !data.imageRaw().empty()) + if(_detectMarkers && !isIntermediateNode && !data.imageRaw().empty()) { UDEBUG("Detecting markers..."); if(landmarks.empty()) @@ -5718,12 +6171,12 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor UDEBUG("time post-decimation = %fs", t); } - if(_stereoFromMotion && + if(!isIntermediateNode && + _stereoFromMotion && !pose.isNull() && cameraModels.size() == 1 && words.size() && (words3D.size() == 0 || (words.size() == words3D.size() && words3DValid!=(int)words3D.size())) && - _registrationPipeline->isImageRequired() && _signatures.size() && _signatures.rbegin()->second->mapId() == _idMapCount) // same map { @@ -5767,11 +6220,14 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor // The following is used only to re-estimate the correspondences, the returned transform is ignored Transform tmpt; - RegistrationVis reg(parameters_); + ParametersMap tmpParams = parameters_; + // Pure 2D-2D without guess would generate variance=1 + uInsert(tmpParams, ParametersPair(Parameters::kVisEpipolarGeometryVar(), "1")); + RegistrationVis reg(tmpParams); if(_registrationPipeline->isScanRequired()) { // If icp is used, remove it to just do visual registration - RegistrationVis vis(parameters_); + RegistrationVis vis(tmpParams); tmpt = vis.computeTransformationMod(cpCurrent, cpPrevious, cameraTransform); } else @@ -5793,11 +6249,18 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor { previousWords.insert(std::make_pair(iter->first, cpPrevious.getWordsKpts()[iter->second])); } + float reprojError = Parameters::defaultVisPnPReprojError(); + int varianceMedianRatio = Parameters::defaultVisPnPVarianceMedianRatio(); + Parameters::parse(parameters_, Parameters::kVisPnPReprojError(), reprojError); + Parameters::parse(parameters_, Parameters::kVisPnPVarianceMedianRatio(), varianceMedianRatio); std::map inliers = util3d::generateWords3DMono( currentWords, previousWords, cameraModels[0], - cameraTransform); + cameraTransform, + reprojError, + 0.99f, + varianceMedianRatio); UDEBUG("inliers=%d", (int)inliers.size()); @@ -6122,9 +6585,15 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor compressedUserData)); } - s->setWords(words, wordsKpts, - _reextractLoopClosureFeatures?std::vector():words3D, - _reextractLoopClosureFeatures?cv::Mat():wordsDescriptors); + if(!isIntermediateNode || _saveIntermediateNodeData) + { + s->setWords(words, wordsKpts, + _reextractLoopClosureFeatures?std::vector():words3D, + _reextractLoopClosureFeatures?cv::Mat():wordsDescriptors); + + s->sensorData().setLaserScan(laserScan, false); + s->sensorData().setUserData(data.userDataRaw(), false); + } // set raw data if(!cameraModels.empty()) @@ -6135,8 +6604,6 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor { s->sensorData().setStereoImage(image, depthOrRightImage, stereoCameraModels, false); } - s->sensorData().setLaserScan(laserScan, false); - s->sensorData().setUserData(data.userDataRaw(), false); UDEBUG("data.groundTruth() =%s", data.groundTruth().prettyPrint().c_str()); UDEBUG("data.gps() =%s", data.gps().stamp()?"true":"false"); @@ -6146,28 +6613,33 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor s->sensorData().setGPS(data.gps()); s->sensorData().setEnvSensors(data.envSensors()); - if(!isIntermediateNode) + std::vector globalDescriptors = data.globalDescriptors(); + if(!isIntermediateNode && _globalDescriptorExtractor) { - std::vector globalDescriptors = data.globalDescriptors(); - if(_globalDescriptorExtractor) + GlobalDescriptor gdescriptor = _globalDescriptorExtractor->extract(inputData); + if(!gdescriptor.data().empty()) { - GlobalDescriptor gdescriptor = _globalDescriptorExtractor->extract(inputData); - if(!gdescriptor.data().empty()) - { - globalDescriptors.push_back(gdescriptor); - } + globalDescriptors.push_back(gdescriptor); } - s->sensorData().setGlobalDescriptors(globalDescriptors); } - else if(!data.globalDescriptors().empty()) + if(!globalDescriptors.empty()) { - UDEBUG("Global descriptors provided (size=%ld) are ignored because this signature is set as intermediate.", data.globalDescriptors().size()); + if(!isIntermediateNode || _saveIntermediateNodeData) + { + s->sensorData().setGlobalDescriptors(globalDescriptors); + } + else + { + UDEBUG("Global descriptors provided (size=%ld) are ignored because this signature is set as intermediate and %s=false.", + globalDescriptors.size(), + Parameters::kMemIntermediateNodeDataKept().c_str()); + } } t = timer.ticks(); if(stats) stats->addStatistic(Statistics::kTimingMemCompressing_data(), t*1000.0f); UDEBUG("time compressing data (id=%d) %fs", id, t); - if(words.size()) + if(words.size() && addedToDictionary) { s->setEnabled(true); // All references are already activated in the dictionary at this point (see _vwd->addNewWords()) } @@ -6307,16 +6779,19 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor s->addLandmark(landmark); // Update landmark index - std::map >::iterator nter = _landmarksIndex.find(landmarkId); - if(nter!=_landmarksIndex.end()) + if(!isIntermediateNode) { - nter->second.insert(s->id()); - } - else - { - std::set tmp; - tmp.insert(s->id()); - _landmarksIndex.insert(std::make_pair(landmarkId, tmp)); + std::map >::iterator nter = _landmarksIndex.find(landmarkId); + if(nter!=_landmarksIndex.end()) + { + nter->second.insert(s->id()); + } + else + { + std::set tmp; + tmp.insert(s->id()); + _landmarksIndex.insert(std::make_pair(landmarkId, tmp)); + } } } else @@ -6330,7 +6805,7 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor void Memory::disableWordsRef(int signatureId) { - UDEBUG("id=%d", signatureId); + //UDEBUG("id=%d", signatureId); Signature * ss = this->_getSignature(signatureId); if(ss && ss->isEnabled()) @@ -6346,7 +6821,7 @@ void Memory::disableWordsRef(int signatureId) count -= _vwd->getTotalActiveReferences(); ss->setEnabled(false); - UDEBUG("%d words total ref removed from signature %d... (total active ref = %d)", count, ss->id(), _vwd->getTotalActiveReferences()); + //UDEBUG("%d words total ref removed from signature %d... (total active ref = %d)", count, ss->id(), _vwd->getTotalActiveReferences()); } } @@ -6409,7 +6884,7 @@ void Memory::enableWordsRef(const std::list & signatureIds) UDEBUG("oldWordIds.size()=%d, getOldIds time=%fs", oldWordIds.size(), timer.ticks()); - // the words were deleted, so try to math it with an active word + // the words were deleted, so try to match it with an active word std::list vws; if(oldWordIds.size() && _dbDriver) { @@ -6474,7 +6949,10 @@ void Memory::enableWordsRef(const std::list & signatureIds) { if(keys.at(i)>0) { - _vwd->addWordRef(keys.at(i), (*j)->id()); + if(!_vwd->addWordRef(keys.at(i), (*j)->id())) + { + UERROR("Could not add word ref %d to node %d!?", keys.at(i), (*j)->id()); + } } } (*j)->setEnabled(true); @@ -6494,6 +6972,7 @@ std::set Memory::reactivateSignatures(const std::list & ids, unsigned UDEBUG(""); UTimer timer; std::list idsToLoad; + std::list idsInQueue; std::map::iterator wmIter; for(std::list::const_iterator i=ids.begin(); i!=ids.end(); ++i) { @@ -6504,10 +6983,14 @@ std::set Memory::reactivateSignatures(const std::list & ids, unsigned idsToLoad.push_back(*i); UINFO("Loading location %d from database...", *i); } + else if(idsToLoad.size() >= maxLoaded) + { + idsInQueue.push_back(*i); + } } } - UDEBUG("idsToLoad = %d", idsToLoad.size()); + UDEBUG("idsToLoad = %ld (in queue = %ld)", idsToLoad.size(), idsInQueue.size()); std::list reactivatedSigns; if(_dbDriver) @@ -6516,8 +6999,13 @@ std::set Memory::reactivateSignatures(const std::list & ids, unsigned } timeDbAccess = timer.getElapsedTime(); std::list idsLoaded; + int intermediateNodesLoaded = 0; for(std::list::iterator i=reactivatedSigns.begin(); i!=reactivatedSigns.end(); ++i) { + if((*i)->getWeight() == -1) + { + ++intermediateNodesLoaded; + } if(!(*i)->getLandmarks().empty()) { // Update landmark indexes @@ -6561,10 +7049,23 @@ std::set Memory::reactivateSignatures(const std::list & ids, unsigned } this->enableWordsRef(idsLoaded); UDEBUG("time = %fs", timer.ticks()); - return std::set(idsToLoad.begin(), idsToLoad.end()); + + std::set totalLoaded(idsToLoad.begin(), idsToLoad.end()); + + // Ignore intermediate nodes in the total count of signatures loaded, keep loading next in queue + if(intermediateNodesLoaded > 0 && (int)idsInQueue.size() >= intermediateNodesLoaded) + { + double queueTimeDbAccess = 0.0; + std::set queueLoaded = reactivateSignatures(idsInQueue, maxLoaded-intermediateNodesLoaded, queueTimeDbAccess); + timeDbAccess += queueTimeDbAccess; + totalLoaded.insert(queueLoaded.begin(), queueLoaded.end()); + } + + return totalLoaded; } -// return all non-null poses +// returns all non-null poses and links +// if lookInDatabase is false, intermediate nodes are ignored and new neighbor links between non-intermediate nodes are returned // return unique links between nodes (for neighbors: old->new, for loops: parent->child) void Memory::getMetricConstraints( const std::set & ids, @@ -6587,6 +7088,13 @@ void Memory::getMetricConstraints( { if(uContains(poses, *iter)) { + const Signature * s = lookInDatabase?0:this->getSignature(*iter); // If we look in db, we don't ignore intermediate nodes + if(s && s->getWeight() == -1) + { + poses.erase(*iter); + continue; + } + std::multimap tmpLinks = getLinks(*iter, lookInDatabase, true); for(std::multimap::iterator jter=tmpLinks.begin(); jter!=tmpLinks.end(); ++jter) { @@ -6599,14 +7107,15 @@ void Memory::getMetricConstraints( (jter->second.type() == Link::kNeighbor || jter->second.type() == Link::kNeighborMerged)) { - const Signature * s = this->getSignature(jter->first); + s = this->getSignature(jter->first); UASSERT(s!=0); if(s->getWeight() == -1) { + bool validLink = false; Link link = jter->second; while(s && s->getWeight() == -1) { - // skip to next neighbor, well we assume that bad signatures + // skip to next neighbor, well we assume that intermediate signatures // are only linked by max 2 neighbor links. std::multimap n = this->getNeighborLinks(s->id(), false); UASSERT(n.size() <= 2); @@ -6619,15 +7128,29 @@ void Memory::getMetricConstraints( link = link.merge(uter->second, uter->second.type()); poses.erase(s->id()); s = s2; + validLink = s->getWeight() != -1; + } + else + { + validLink = false; + break; } } else { + validLink = false; break; } } - links.insert(std::make_pair(*iter, link)); + if(validLink) + { + links.insert(std::make_pair(*iter, link)); + } + else + { + poses.erase(s->id()); + } } else { diff --git a/corelib/src/Odometry.cpp b/corelib/src/Odometry.cpp index 2b25e15c..ebcd1db1 100644 --- a/corelib/src/Odometry.cpp +++ b/corelib/src/Odometry.cpp @@ -35,10 +35,12 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "rtabmap/core/odometry/OdometryORBSLAM3.h" #include "rtabmap/core/odometry/OdometryLOAM.h" #include "rtabmap/core/odometry/OdometryFLOAM.h" +#include "rtabmap/core/odometry/OdometryLIOSAM.h" #include "rtabmap/core/odometry/OdometryMSCKF.h" -#include "rtabmap/core/odometry/OdometryVINS.h" +#include "rtabmap/core/odometry/OdometryVINSFusion.h" #include "rtabmap/core/odometry/OdometryOpenVINS.h" #include "rtabmap/core/odometry/OdometryOpen3D.h" +#include "rtabmap/core/odometry/OdometryCuVSLAM.h" #include "rtabmap/core/OdometryInfo.h" #include "rtabmap/core/util3d.h" #include "rtabmap/core/util3d_mapping.h" @@ -100,11 +102,14 @@ Odometry * Odometry::create(Odometry::Type & type, const ParametersMap & paramet case Odometry::kTypeFLOAM: odometry = new OdometryFLOAM(parameters); break; + case Odometry::kTypeLIOSAM: + odometry = new OdometryLIOSAM(parameters); + break; case Odometry::kTypeMSCKF: odometry = new OdometryMSCKF(parameters); break; - case Odometry::kTypeVINS: - odometry = new OdometryVINS(parameters); + case Odometry::kTypeVINSFusion: + odometry = new OdometryVINSFusion(parameters); break; case Odometry::kTypeOpenVINS: odometry = new OdometryOpenVINS(parameters); @@ -112,6 +117,9 @@ Odometry * Odometry::create(Odometry::Type & type, const ParametersMap & paramet case Odometry::kTypeOpen3D: odometry = new OdometryOpen3D(parameters); break; + case Odometry::kTypeCuVSLAM: + odometry = new OdometryCuVSLAM(parameters); + break; default: UERROR("Unknown odometry type %d, using F2M instead...", (int)type); odometry = new OdometryF2M(parameters); @@ -625,6 +633,7 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet if(!guessIn.isNull()) { guess = guessIn; + UDEBUG("Using provided guess %s", guessIn.prettyPrint().c_str()); } else if(!imus_.empty()) { @@ -641,12 +650,16 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet { guess = guess.to3DoF(); } + UDEBUG("Adjusting guess from motion with IMU %s", guess.prettyPrint().c_str()); } else if(!imuLastTransform_.isNull()) { UWARN("Could not find imu transform at %f", data.stamp()); } } + else if(!guess.isNull() && (!data.imageRaw().empty() || !data.laserScanRaw().isEmpty())) { + UDEBUG("Using guess from motion %s", guess.prettyPrint().c_str()); + } UTimer time; @@ -1011,21 +1024,28 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet --_resetCurrentCount; if(_resetCurrentCount == 0) { - UWARN("Odometry automatically reset to latest pose!"); - this->reset(_pose); + if(!guess.isNull() && !guessIn.isNull()) { + UWARN("Odometry automatically reset to latest pose (%s) + guess (%s)!", _pose.prettyPrint().c_str(), guess.prettyPrint().c_str()); + this->reset(_pose * guess); + } + else { + UWARN("Odometry automatically reset to latest pose (%s)!", _pose.prettyPrint().c_str()); + this->reset(_pose); + } _resetCurrentCount = _resetCountdown; if(info) { *info = OdometryInfo(); } - return this->computeTransform(data, Transform(), info); + this->computeTransform(data, Transform(), info); + return _pose; } - } previousVelocities_.clear(); velocityGuess_.setNull(); previousStamp_ = 0; +} return Transform(); } @@ -1055,6 +1075,7 @@ void Odometry::initKalmanFilter(const Transform & initialPose, float vx, float v 0, 0, 0, 0, 0, 0.17 } }; static const boost::array STANDARD_TWIST_COVARIANCE = { { 0.05, 0, 0, 0, 0, 0, + } 0, 0.05, 0, 0, 0, 0, 0, 0, 0.05, 0, 0, 0, 0, 0, 0, 0.09, 0, 0, diff --git a/corelib/src/OdometryInfo.cpp b/corelib/src/OdometryInfo.cpp index c06d10d4..4b1d3c90 100644 --- a/corelib/src/OdometryInfo.cpp +++ b/corelib/src/OdometryInfo.cpp @@ -126,10 +126,10 @@ std::map OdometryInfo::statistics(const Transform & pose) stats.insert(std::make_pair("Odometry/ICPStructuralComplexity/", reg.icpStructuralComplexity)); stats.insert(std::make_pair("Odometry/ICPStructuralDistribution/", reg.icpStructuralDistribution)); stats.insert(std::make_pair("Odometry/ICPCorrespondences/", reg.icpCorrespondences)); - stats.insert(std::make_pair("Odometry/StdDevLin/", sqrt((float)reg.covariance.at(0,0)))); - stats.insert(std::make_pair("Odometry/StdDevAng/", sqrt((float)reg.covariance.at(5,5)))); - stats.insert(std::make_pair("Odometry/VarianceLin/", (float)reg.covariance.at(0,0))); - stats.insert(std::make_pair("Odometry/VarianceAng/", (float)reg.covariance.at(5,5))); + stats.insert(std::make_pair("Odometry/StdDevLin/", reg.covariance.empty()?0:sqrt((float)reg.covariance.at(0,0)))); + stats.insert(std::make_pair("Odometry/StdDevAng/", reg.covariance.empty()?0:sqrt((float)reg.covariance.at(5,5)))); + stats.insert(std::make_pair("Odometry/VarianceLin/", reg.covariance.empty()?0:(float)reg.covariance.at(0,0))); + stats.insert(std::make_pair("Odometry/VarianceAng/", reg.covariance.empty()?0:(float)reg.covariance.at(5,5))); stats.insert(std::make_pair("Odometry/TimeEstimation/ms", timeEstimation*1000.0f)); stats.insert(std::make_pair("Odometry/TimeFiltering/ms", timeParticleFiltering*1000.0f)); stats.insert(std::make_pair("Odometry/LocalMapSize/", localMapSize)); diff --git a/corelib/src/OdometryThread.cpp b/corelib/src/OdometryThread.cpp index 490a3753..3c7b079a 100644 --- a/corelib/src/OdometryThread.cpp +++ b/corelib/src/OdometryThread.cpp @@ -32,6 +32,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "rtabmap/core/OdometryInfo.h" #include "rtabmap/core/OdometryEvent.h" #include "rtabmap/utilite/ULogger.h" +#include "rtabmap/utilite/UTimer.h" namespace rtabmap { @@ -63,7 +64,7 @@ bool OdometryThread::handleEvent(UEvent * event) SensorEvent * sensorEvent = (SensorEvent*)event; if(sensorEvent->getCode() == SensorEvent::kCodeData) { - this->addData(sensorEvent->data()); + this->addData(*sensorEvent); } } else if(event->getClassName().compare("IMUEvent") == 0) @@ -112,31 +113,51 @@ void OdometryThread::mainLoop() _imuBuffer.clear(); _oldestAsyncImuStamp = 0.0; _newestAsyncImuStamp = 0.0; + _previousGuessPose.setNull(); } - SensorData data; - if(getData(data)) + SensorEvent event; + if(getData(event)) { OdometryInfo info; UDEBUG("Processing data..."); - Transform pose = _odometry->process(data, &info); + Transform guess; + UDEBUG("event.info().odomPose=%s", event.info().odomPose.prettyPrint().c_str()); + if(!_previousGuessPose.isNull() && !event.info().odomPose.isNull()) { + guess = _previousGuessPose.inverse() * event.info().odomPose; + } + + SensorData data = event.data(); + Transform pose = _odometry->process(data, guess , &info); if(!data.imageRaw().empty() || !data.laserScanRaw().empty() || (pose.isNull() && data.imu().empty())) { UDEBUG("Odom pose = %s", pose.prettyPrint().c_str()); + if(!pose.isNull()) { + _previousGuessPose = event.info().odomPose; + UASSERT(event.info().odomPose.isNull() || !info.reg.covariance.empty()); + if(!event.info().odomPose.isNull() && info.reg.covariance.at(0,0) >= 9999 && + (pose.x() != 0.0f || pose.y() != 0.0f || pose.z() != 0.0f)) // not the first frame + { + // In case of external guess and auto reset, keep reporting lost till we + // process the second frame with valid covariance. This way it + // won't trigger a new map. + pose = Transform(); + } + } // a null pose notify that odometry could not be computed this->post(new OdometryEvent(data, pose, info)); } } } -void OdometryThread::addData(const SensorData & data) +void OdometryThread::addData(const SensorEvent & event) { - if(data.imu().empty()) + if(event.data().imu().empty()) { if(dynamic_cast(_odometry) == 0) { - if((data.imageRaw().empty() || data.depthOrRightRaw().empty() || (data.cameraModels().empty() && data.stereoCameraModels().empty())) && - data.laserScanRaw().empty()) + if((event.data().imageRaw().empty() || event.data().depthOrRightRaw().empty() || (event.data().cameraModels().empty() && event.data().stereoCameraModels().empty())) && + event.data().laserScanRaw().empty()) { ULOGGER_ERROR("Missing some information (images/scans empty or missing calibration)!?"); return; @@ -145,7 +166,7 @@ void OdometryThread::addData(const SensorData & data) else { // Mono can accept RGB only - if(data.imageRaw().empty() || (data.cameraModels().empty() && data.stereoCameraModels().empty())) + if(event.data().imageRaw().empty() || (event.data().cameraModels().empty() && event.data().stereoCameraModels().empty())) { ULOGGER_ERROR("Missing some information (image empty or missing calibration)!?"); return; @@ -156,30 +177,32 @@ void OdometryThread::addData(const SensorData & data) bool notify = true; _dataMutex.lock(); { - if( !data.imageRaw().empty() || - !data.imageCompressed().empty() || - !data.laserScanRaw().isEmpty() || - !data.laserScanCompressed().empty() || - data.imu().empty()) + if( !event.data().imageRaw().empty() || + !event.data().imageCompressed().empty() || + !event.data().laserScanRaw().isEmpty() || + !event.data().laserScanCompressed().empty() || + event.data().imu().empty()) { - if(_oldestAsyncImuStamp > 0.0 && data.stamp() < _oldestAsyncImuStamp) { + if(_oldestAsyncImuStamp > 0.0 && event.data().stamp() < _oldestAsyncImuStamp) { UWARN("Received image/lidar with stamp (%f) older than oldest received imu " "(%f), skipping that frame (imu buffer size=%ld). " "When using async IMU, make sure IMU is published faster " - "than camera/lidar (assuming IMU latency is very small compared to camera/lidar).", - data.stamp(), _oldestAsyncImuStamp, _imuBuffer.size()); + "than camera/lidar (assuming IMU latency is very small compared to camera/lidar)." + "Current camera/lidar delay with system time is %fs.", + event.data().stamp(), _oldestAsyncImuStamp, _imuBuffer.size(), UTimer::now() - event.data().stamp()); notify = false; } - else if(_newestAsyncImuStamp > 0.0 && data.stamp()>=_newestAsyncImuStamp) { + else if(_newestAsyncImuStamp > 0.0 && event.data().stamp()>=_newestAsyncImuStamp) { UWARN("Received image/lidar with stamp (%f) newer than latest received imu " "(%f), skipping that frame (imu buffer size=%ld). " "When using async IMU, make sure IMU is published faster " - "than camera/lidar (assuming IMU latency is very small compared to camera/lidar).", - data.stamp(), _newestAsyncImuStamp, _imuBuffer.size()); + "than camera/lidar (assuming IMU latency is very small compared to camera/lidar). " + "Current camera/lidar delay with system time is %fs.", + event.data().stamp(), _newestAsyncImuStamp, _imuBuffer.size(), UTimer::now() - event.data().stamp()); notify = false; } else { - _dataBuffer.push_back(data); + _dataBuffer.push_back(event); while(_dataBufferMaxSize > 0 && _dataBuffer.size() > _dataBufferMaxSize) { UDEBUG("Data buffer is full, the oldest data is removed to add the new one."); @@ -190,11 +213,11 @@ void OdometryThread::addData(const SensorData & data) } else { - _imuBuffer.push_back(data); + _imuBuffer.push_back(event.data()); if(_oldestAsyncImuStamp == 0) { - _oldestAsyncImuStamp = data.stamp(); + _oldestAsyncImuStamp = event.data().stamp(); } - _newestAsyncImuStamp = data.stamp(); + _newestAsyncImuStamp = event.data().stamp(); } } _dataMutex.unlock(); @@ -205,7 +228,7 @@ void OdometryThread::addData(const SensorData & data) } } -bool OdometryThread::getData(SensorData & data) +bool OdometryThread::getData(SensorEvent & event) { bool dataFilled = false; _dataAdded.acquire(); @@ -219,12 +242,12 @@ bool OdometryThread::getData(SensorData & data) _odometry->process(_imuBuffer.front()); double stamp =_imuBuffer.front().stamp(); _imuBuffer.pop_front(); - if(stamp > _dataBuffer.front().stamp()) { + if(stamp > _dataBuffer.front().data().stamp()) { break; } } - data = _dataBuffer.front(); + event = _dataBuffer.front(); _dataBuffer.pop_front(); dataFilled = true; } diff --git a/corelib/src/Optimizer.cpp b/corelib/src/Optimizer.cpp index f0b563ab..9a76f0a9 100644 --- a/corelib/src/Optimizer.cpp +++ b/corelib/src/Optimizer.cpp @@ -185,6 +185,52 @@ Optimizer * Optimizer::create(Optimizer::Type type, const ParametersMap & parame return optimizer; } +class LinkIdKey +{ +public: +LinkIdKey(int id, Link::Type type) : + id_(id), + type_(type) {} + bool operator<(const LinkIdKey & k) const + { + // landmark, sort by smallest to largest landmark id, after normal links + if(id_ < 0 && k.id_ < 0) + { + return id_ > k.id_; + } + else if(id_ < 0) { + return false; + } + else if(k.id_ < 0) { + return true; + } + + if(type_ == Link::kNeighbor && k.type_ != Link::kNeighbor) + { + return true; + } + else if(type_ != Link::kNeighbor && k.type_ == Link::kNeighbor) + { + return false; + } + else if(type_ == Link::kNeighborMerged && k.type_ != Link::kNeighbor && k.type_ != Link::kNeighborMerged) + { + return true; + } + else if(k.type_ == Link::kNeighborMerged && type_ != Link::kNeighbor && type_ != Link::kNeighborMerged) + { + return false; + } + else + { + // normal link, sort by smallest to largest id + return id_ < k.id_; + } + } + int id_; + Link::Type type_; +}; + void Optimizer::getConnectedGraph( int fromId, const std::map & posesIn, @@ -194,13 +240,14 @@ void Optimizer::getConnectedGraph( { 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); - UASSERT(uContains(posesIn, fromId)); + UASSERT_MSG(uContains(posesIn, fromId), uFormat("poses=%ld (first=%d last=%d) fromId=%d", + posesIn.size(), posesIn.empty()?0:posesIn.begin()->first, posesIn.empty()?0:posesIn.rbegin()->first, fromId).c_str()); posesOut.clear(); linksOut.clear(); - std::set nextPoses; - nextPoses.insert(fromId); + std::map nextPoses; + nextPoses.insert(std::make_pair(LinkIdKey(fromId, Link::kUndef), posesIn.find(fromId)->second)); std::multimap > biLinks; for(std::multimap::const_iterator iter=linksIn.begin(); iter!=linksIn.end(); ++iter) { @@ -214,22 +261,27 @@ void Optimizer::getConnectedGraph( } } - while(nextPoses.size()) + while(!nextPoses.empty()) { - int currentId = *nextPoses.rbegin(); // fill up all nodes before landmarks - nextPoses.erase(*nextPoses.rbegin()); + // Fill up all nodes before landmarks + // For nodes, fill up all neightbor nodes before loop closure ones + int currentId = nextPoses.begin()->first.id_; + Transform currentPose = nextPoses.begin()->second; + nextPoses.erase(nextPoses.begin()); - if(posesOut.empty()) + if(posesOut.find(currentId) != posesOut.end()) { + // Already added from priority list + continue; + } + + posesOut.insert(std::make_pair(currentId, currentPose)); + + // add prior links + for(std::multimap::const_iterator pter=linksIn.find(currentId); pter!=linksIn.end() && pter->first==currentId; ++pter) { - posesOut.insert(std::make_pair(currentId, posesIn.find(currentId)->second)); - - // add prior links - for(std::multimap::const_iterator pter=linksIn.find(currentId); pter!=linksIn.end() && pter->first==currentId; ++pter) + if(pter->second.from() == pter->second.to() && (!priorsIgnored() || pter->second.type() != Link::kPosePrior)) { - if(pter->second.from() == pter->second.to() && (!priorsIgnored() || pter->second.type() != Link::kPosePrior)) - { - linksOut.insert(*pter); - } + linksOut.insert(*pter); } } @@ -240,52 +292,42 @@ void Optimizer::getConnectedGraph( if(posesIn.find(toId) != posesIn.end() && (!landmarksIgnored() || toId>0)) { std::multimap::const_iterator kter = graph::findLink(linksIn, currentId, toId, true, type); - if(nextPoses.find(toId) == nextPoses.end()) + UASSERT(kter!=linksIn.end()); + if(!uContains(posesOut, toId)) { - if(!uContains(posesOut, toId)) + const Transform & poseToIn = posesIn.at(toId); + Transform t = kter->second.from()==currentId?kter->second.transform():kter->second.transform().inverse(); + Transform pose; + if(isSlam2d() && kter->second.type() == Link::kLandmark && toId>0 && (poseToIn.is3DoF() || poseToIn.is4DoF())) { - 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(poseToIn.is3DoF()) { - if(poseToIn.is3DoF()) - { - posesOut.insert(std::make_pair(toId, (posesOut.at(currentId) * t).to3DoF())); - } - else - { - posesOut.insert(std::make_pair(toId, (posesOut.at(currentId) * t).to4DoF())); - } + pose = (posesOut.at(currentId) * t).to3DoF(); } else { - posesOut.insert(std::make_pair(toId, posesOut.at(currentId)* t)); + pose = (posesOut.at(currentId) * t).to4DoF(); } - - // add prior links - for(std::multimap::const_iterator pter=linksIn.find(toId); pter!=linksIn.end() && pter->first==toId; ++pter) - { - if(pter->second.from() == pter->second.to() && (!priorsIgnored() || pter->second.type() != Link::kPosePrior)) - { - linksOut.insert(*pter); - } - } - - nextPoses.insert(toId); + } + else + { + pose = posesOut.at(currentId)* t; } - // only add unique links - if(graph::findLink(linksOut, currentId, toId, true, kter->second.type()) == linksOut.end()) + nextPoses.insert(std::make_pair(LinkIdKey(toId, type), pose)); + } + + // only add unique links + if(graph::findLink(linksOut, currentId, toId, true, kter->second.type()) == linksOut.end()) + { + if(kter->second.to() < 0) { - if(kter->second.to() < 0) - { - // For landmarks, make sure fromId is the landmark - linksOut.insert(std::make_pair(kter->second.to(), kter->second.inverse())); - } - else - { - linksOut.insert(*kter); - } + // For landmarks, make sure fromId is the landmark + linksOut.insert(std::make_pair(kter->second.to(), kter->second.inverse())); + } + else + { + linksOut.insert(*kter); } } } @@ -608,8 +650,8 @@ void Optimizer::computeBACorrespondences( } } - if(sFrom.getWords().size() && - sTo.getWords().size() && + if(sFrom.getWordsKpts().size() && + sTo.getWordsKpts().size() && sFrom.getWords3().size()) { if(!rematchFeatures) diff --git a/corelib/src/Parameters.cpp b/corelib/src/Parameters.cpp index 03d767ca..ed7e981b 100644 --- a/corelib/src/Parameters.cpp +++ b/corelib/src/Parameters.cpp @@ -113,11 +113,14 @@ ParametersMap Parameters::deserialize(const std::string & parameters) std::list tuplets = uSplit(parameters, ';'); for(std::list::iterator iter=tuplets.begin(); iter!=tuplets.end(); ++iter) { - std::list p = uSplit(*iter, ':'); - if(p.size() == 2) + // Split on the FIRST ':' only. Using uSplit() here would discard + // empty tokens, so a tuplet like "Marker/Lengths:" (legitimate empty + // string value) would lose the value side and be dropped entirely. + size_t colonPos = iter->find(':'); + if(colonPos != std::string::npos && colonPos > 0) { - std::string key = p.front(); - std::string value = p.back(); + std::string key = iter->substr(0, colonPos); + std::string value = iter->substr(colonPos + 1); // look for old parameter name bool addParameter = true; @@ -168,6 +171,7 @@ bool Parameters::isFeatureParameter(const std::string & parameter) group.compare("BRISK") == 0 || group.compare("KAZE") == 0 || group.compare("SuperPoint") == 0 || + group.compare("SuperPointRpautrat") == 0 || group.compare("PyDetector") == 0; } @@ -182,6 +186,7 @@ rtabmap::ParametersMap Parameters::getDefaultOdometryParameters(bool stereo, boo (stereo && group.compare("Stereo") == 0) || (icp && group.compare("Icp") == 0) || (vis && Parameters::isFeatureParameter(iter->first)) || + group.compare("OdomCuVSLAM") == 0 || group.compare("Reg") == 0 || group.compare("Optimizer") == 0 || group.compare("g2o") == 0 || @@ -236,6 +241,12 @@ const std::map > & Parameters::getRemo { // removed parameters + // 0.23.7 + removedParameters_.insert(std::make_pair("Marker/CornerRefinementMethod", std::make_pair(true, Parameters::kMarkerOpenCVCornerRefinementMethod()))); + + // 0.23.1 + removedParameters_.insert(std::make_pair("OdomVINS/ConfigPath", std::make_pair(true, Parameters::kOdomVINSFusionConfigPath()))); + // 0.21.13 removedParameters_.insert(std::make_pair("Vis/ForwardEstOnly", std::make_pair(false, ""))); @@ -285,7 +296,7 @@ const std::map > & Parameters::getRemo removedParameters_.insert(std::make_pair("Aruco/MaxDepthError", std::make_pair(true, Parameters::kMarkerMaxDepthError()))); removedParameters_.insert(std::make_pair("Aruco/VarianceLinear", std::make_pair(true, Parameters::kMarkerVarianceLinear()))); removedParameters_.insert(std::make_pair("Aruco/VarianceAngular", std::make_pair(true, Parameters::kMarkerVarianceAngular()))); - removedParameters_.insert(std::make_pair("Aruco/CornerRefinementMethod", std::make_pair(true, Parameters::kMarkerCornerRefinementMethod()))); + removedParameters_.insert(std::make_pair("Aruco/CornerRefinementMethod", std::make_pair(true, Parameters::kMarkerOpenCVCornerRefinementMethod()))); // 0.17.5 removedParameters_.insert(std::make_pair("Grid/OctoMapOccupancyThr", std::make_pair(true, Parameters::kGridGlobalOccupancyThr()))); @@ -658,6 +669,12 @@ ParametersMap Parameters::parseArguments(int argc, char * argv[], bool onlyParam std::cout << str << std::setw(spacing - str.size()) << "true" << std::endl; #else std::cout << str << std::setw(spacing - str.size()) << "false" << std::endl; +#endif + str = "With SuperPoint Rpautrat:"; +#if defined(RTABMAP_TORCH) && defined(RTABMAP_PYTHON) + std::cout << str << std::setw(spacing - str.size()) << "true" << std::endl; +#else + std::cout << str << std::setw(spacing - str.size()) << "false" << std::endl; #endif str = "With Python3:"; #ifdef RTABMAP_PYTHON @@ -670,6 +687,12 @@ ParametersMap Parameters::parseArguments(int argc, char * argv[], bool onlyParam std::cout << str << std::setw(spacing - str.size()) << "true" << std::endl; #else std::cout << str << std::setw(spacing - str.size()) << "false" << std::endl; +#endif + str = "With AprilTag:"; +#ifdef RTABMAP_APRILTAG + std::cout << str << std::setw(spacing - str.size()) << "true" << std::endl; +#else + std::cout << str << std::setw(spacing - str.size()) << "false" << std::endl; #endif str = "With OpenGV:"; #ifdef RTABMAP_OPENGV @@ -892,6 +915,12 @@ ParametersMap Parameters::parseArguments(int argc, char * argv[], bool onlyParam std::cout << str << std::setw(spacing - str.size()) << "true" << std::endl; #else std::cout << str << std::setw(spacing - str.size()) << "false" << std::endl; +#endif + str = "With LIO-SAM:"; +#ifdef RTABMAP_LIOSAM + std::cout << str << std::setw(spacing - str.size()) << "true" << std::endl; +#else + std::cout << str << std::setw(spacing - str.size()) << "false" << std::endl; #endif str = "With FOVIS:"; #ifdef RTABMAP_FOVIS @@ -936,7 +965,7 @@ ParametersMap Parameters::parseArguments(int argc, char * argv[], bool onlyParam std::cout << str << std::setw(spacing - str.size()) << "false" << std::endl; #endif str = "With VINS-Fusion:"; -#ifdef RTABMAP_VINS +#ifdef RTABMAP_VINS_FUSION std::cout << str << std::setw(spacing - str.size()) << "true" << std::endl; #else std::cout << str << std::setw(spacing - str.size()) << "false" << std::endl; @@ -1109,8 +1138,8 @@ ParametersMap Parameters::parseArguments(int argc, char * argv[], bool onlyParam ignore = true; } #endif -#ifndef RTABMAP_ORBSLAM2 - if(group.compare("OdomORBSLAM2") == 0) +#ifndef RTABMAP_ORB_SLAM + if(group.compare("OdomORBSLAM") == 0) { ignore = true; } @@ -1216,13 +1245,13 @@ void readINIImpl(const CSimpleIniA & ini, const std::string & configFilePath, Pa std::vector version = uListToVector(uSplit((*iter).second, '.')); if(version.size() == 3) { - if(!RTABMAP_VERSION_COMPARE(std::atoi(version[0].c_str()), std::atoi(version[1].c_str()), std::atoi(version[2].c_str()))) + if(RTABMAP_VERSION_COMPARE(<, std::atoi(version[0].c_str()), std::atoi(version[1].c_str()), std::atoi(version[2].c_str()))) { if(configFilePath.find(".rtabmap") != std::string::npos) { UWARN("Version in the config file \"%s\" is more recent (\"%s\") than " - "current RTAB-Map version used (\"%s\"). The config file will be upgraded " - "to new version.", + "current RTAB-Map version used (\"%s\"). The config file will be downgraded " + "to current RTAB-Map version if saved.", configFilePath.c_str(), (*iter).second, RTABMAP_VERSION); diff --git a/corelib/src/RegistrationIcp.cpp b/corelib/src/RegistrationIcp.cpp index 11611ae1..29bda086 100644 --- a/corelib/src/RegistrationIcp.cpp +++ b/corelib/src/RegistrationIcp.cpp @@ -529,6 +529,7 @@ Transform RegistrationIcp::computeTransformationImpl( double toComplexity = util3d::computeNormalsComplexity(toScan, guess, &complexityVectorsTo, &complexityValuesTo); float complexity = fromComplexitygetType()); UDEBUG("guess=%s", guess.prettyPrint().c_str()); - UDEBUG("Input(%d): from=%d words, %d 3D words, %d words descriptors, %d kpts, %d kpts3D, %d descriptors, image=%dx%d models=%d stereo=%d", + UDEBUG("Input(%d): from=%d words, %d 3D words, %d words descriptors, %d words kpts, %d kpts, %d kpts3D, %d descriptors, image=%dx%d models=%d stereo=%d", fromSignature.id(), (int)fromSignature.getWords().size(), (int)fromSignature.getWords3().size(), (int)fromSignature.getWordsDescriptors().rows, + (int)fromSignature.getWordsKpts().size(), (int)fromSignature.sensorData().keypoints().size(), (int)fromSignature.sensorData().keypoints3D().size(), fromSignature.sensorData().descriptors().rows, @@ -342,11 +354,12 @@ Transform RegistrationVis::computeTransformationImpl( (int)fromSignature.sensorData().cameraModels().size(), (int)fromSignature.sensorData().stereoCameraModels().size()); - UDEBUG("Input(%d): to=%d words, %d 3D words, %d words descriptors, %d kpts, %d kpts3D, %d descriptors, image=%dx%d models=%d stereo=%d", + UDEBUG("Input(%d): to=%d words, %d 3D words, %d words descriptors, %d words kpts, %d kpts, %d kpts3D, %d descriptors, image=%dx%d models=%d stereo=%d", toSignature.id(), (int)toSignature.getWords().size(), (int)toSignature.getWords3().size(), (int)toSignature.getWordsDescriptors().rows, + (int)toSignature.getWordsKpts().size(), (int)toSignature.sensorData().keypoints().size(), (int)toSignature.sensorData().keypoints3D().size(), toSignature.sensorData().descriptors().rows, @@ -375,6 +388,9 @@ Transform RegistrationVis::computeTransformationImpl( { UDEBUG(""); // just some checks to make sure that input data are ok + UASSERT(fromSignature.getWords().empty() || + fromSignature.getWordsKpts().empty() || + (fromSignature.getWords().size() == fromSignature.getWordsKpts().size())); UASSERT(fromSignature.getWords().empty() || fromSignature.getWords3().empty() || (fromSignature.getWords().size() == fromSignature.getWords3().size())); @@ -382,8 +398,11 @@ Transform RegistrationVis::computeTransformationImpl( (int)fromSignature.getWords().size() == fromSignature.getWordsDescriptors().rows || fromSignature.sensorData().descriptors().empty() || fromSignature.getWordsDescriptors().empty() == 0); - UASSERT((toSignature.getWords().empty() && toSignature.getWords3().empty())|| - (toSignature.getWords().size() && toSignature.getWords3().empty())|| + UASSERT(toSignature.getWords().empty() || + toSignature.getWordsKpts().empty() || + (toSignature.getWords().size() == toSignature.getWordsKpts().size())); + UASSERT(toSignature.getWords().empty() || + toSignature.getWords3().empty() || (toSignature.getWords().size() == toSignature.getWords3().size())); UASSERT((int)toSignature.sensorData().keypoints().size() == toSignature.sensorData().descriptors().rows || (int)toSignature.getWords().size() == toSignature.getWordsDescriptors().rows || @@ -428,16 +447,7 @@ Transform RegistrationVis::computeTransformationImpl( { UASSERT(!fromSignature.sensorData().cameraModels().empty()); UDEBUG("Masking floor (threshold=%f)", _maskFloorThreshold); - if(_maskFloorThreshold<0.0f) - { - cv::Mat depthBelow; - util3d::filterFloor(depthMask, fromSignature.sensorData().cameraModels(), _maskFloorThreshold*-1.0f, &depthBelow); - depthMask = depthBelow; - } - else - { - depthMask = util3d::filterFloor(depthMask, fromSignature.sensorData().cameraModels(), _maskFloorThreshold); - } + depthMask = util3d::filterFloor(depthMask, fromSignature.sensorData().cameraModels(), _maskFloorThreshold); UDEBUG("Masking floor done."); } @@ -643,6 +653,7 @@ Transform RegistrationVis::computeTransformationImpl( // Find features in the new left image UDEBUG("guessSet = %d", guessSet?1:0); std::vector status; + std::vector err; #ifdef HAVE_OPENCV_CUDAOPTFLOW if (_flowGpu) { @@ -672,7 +683,6 @@ Transform RegistrationVis::computeTransformationImpl( else #endif { - std::vector err; UDEBUG("cv::calcOpticalFlowPyrLK() begin"); cv::calcOpticalFlowPyrLK( imageFrom, @@ -684,7 +694,8 @@ Transform RegistrationVis::computeTransformationImpl( cv::Size(_flowWinSize, _flowWinSize), guessSet ? 0 : _flowMaxLevel, cv::TermCriteria(cv::TermCriteria::COUNT + cv::TermCriteria::EPS, _flowIterations, _flowEps), - cv::OPTFLOW_LK_GET_MIN_EIGENVALS | (guessSet ? cv::OPTFLOW_USE_INITIAL_FLOW : 0), 1e-4); + (_flowUseMinEigenVals ? cv::OPTFLOW_LK_GET_MIN_EIGENVALS : 0) | (guessSet ? cv::OPTFLOW_USE_INITIAL_FLOW : 0), + _flowMinEigThreshold); UDEBUG("cv::calcOpticalFlowPyrLK() end"); } @@ -693,11 +704,14 @@ Transform RegistrationVis::computeTransformationImpl( std::vector kptsFrom3DKept(kptsFrom3D.size()); std::vector orignalWordsFromIdsCpy = orignalWordsFromIds; int ki = 0; + UASSERT((status.empty() || cornersTo.size() == status.size()) && + (err.empty() || cornersTo.size() == err.size())); for(unsigned int i=0; i(0, 1); + UDEBUG("Visual distribution: %f (eigen values = %f %f)", info.inliersDistribution, pca_analysis.eigenvalues.at(0, 0), pca_analysis.eigenvalues.at(0, 1)); + if(info.inliersDistribution < _minInliersDistributionThr) { msg = uFormat("The distribution (%f) of inliers is under %s threshold (%f)", @@ -2196,6 +2200,10 @@ Transform RegistrationVis::computeTransformationImpl( info.matches = matchesCount; info.rejectedMsg = msg; info.covariance = covariance; + if(!covariance.empty()) + { + info.variance = covariance.at(0,0); + } UDEBUG("inliers=%d/%d", info.inliers, info.matches); UDEBUG("transform=%s", transform.prettyPrint().c_str()); diff --git a/corelib/src/Rtabmap.cpp b/corelib/src/Rtabmap.cpp index 1c05b8da..775572ed 100644 --- a/corelib/src/Rtabmap.cpp +++ b/corelib/src/Rtabmap.cpp @@ -138,6 +138,7 @@ Rtabmap::Rtabmap() : _databasePath(""), _optimizeFromGraphEnd(Parameters::defaultRGBDOptimizeFromGraphEnd()), _optimizationMaxError(Parameters::defaultRGBDOptimizeMaxError()), + _optimizationMaxErrorRepairRadius(Parameters::defaultRGBDOptimizeMaxErrorRepairRadius()), _startNewMapOnLoopClosure(Parameters::defaultRtabmapStartNewMapOnLoopClosure()), _startNewMapOnGoodSignature(Parameters::defaultRtabmapStartNewMapOnGoodSignature()), _goalReachedRadius(Parameters::defaultRGBDGoalReachedRadius()), @@ -173,6 +174,7 @@ Rtabmap::Rtabmap() : _mapCorrection(Transform::getIdentity()), _lastLocalizationNodeId(0), _currentSessionHasGPS(false), + _lastRejectedLoopClosureIds(0,0), _pathStatus(0), _pathCurrentIndex(0), _pathGoalIndex(0), @@ -382,22 +384,22 @@ void Rtabmap::init(const ParametersMap & parameters, const std::string & databas { cv::Mat cov; this->optimizeCurrentMap( - !_optimizeFromGraphEnd?_memory->getWorkingMem().lower_bound(1)->first:_memory->getWorkingMem().rbegin()->first, + _memory->getLastWorkingSignature(true)->id(), false, _optimizedPoses, cov, &_constraints); } - if(!_optimizedPoses.empty()) + if(_optimizedPoses.lower_bound(1) != _optimizedPoses.end()) { if(_restartAtOrigin) { - UWARN("last localization pose is ignored (%s=true), assuming we start at the origin of the map.", Parameters::kRGBDStartAtOrigin().c_str()); - lastPose = _optimizedPoses.begin()->second; + UWARN("last localization pose is ignored (%s=true), assuming we start at the first node of the map.", Parameters::kRGBDStartAtOrigin().c_str()); + lastPose = _optimizedPoses.lower_bound(1)->second; } _lastLocalizationPose = lastPose; UINFO("Loaded optimizedPoses=%d firstPose %d=%s lastLocalizationPose=%s", _optimizedPoses.size(), - _optimizedPoses.begin()->first, - _optimizedPoses.begin()->second.prettyPrint().c_str(), + _optimizedPoses.lower_bound(1)->first, + _optimizedPoses.lower_bound(1)->second.prettyPrint().c_str(), _lastLocalizationPose.prettyPrint().c_str()); if(_constraints.empty()) @@ -411,7 +413,7 @@ void Rtabmap::init(const ParametersMap & parameters, const std::string & databas UTimer time; std::map likelihood; likelihood.insert(std::make_pair(Memory::kIdVirtual, 1)); - for(std::map::iterator iter=_optimizedPoses.begin(); iter!=_optimizedPoses.end(); ++iter) + for(std::map::iterator iter=_optimizedPoses.lower_bound(1); iter!=_optimizedPoses.end(); ++iter) { if(_memory->getSignature(iter->first)) { @@ -507,6 +509,11 @@ void Rtabmap::close(bool databaseSaved, const std::string & ouputDatabasePath) } if(_memory) { + if(_memory->isReadOnly() && databaseSaved) + { + UWARN("Database is read-only, latest optimized poses, latest localization pose and latest state of the memory are not saved."); + databaseSaved = false; + } if(databaseSaved) { if(_memory->isGraphReduced() && _memory->isIncremental()) @@ -616,6 +623,7 @@ void Rtabmap::parseParameters(const ParametersMap & parameters) _optimizeFromGraphEndChanged = true; } Parameters::parse(parameters, Parameters::kRGBDOptimizeMaxError(), _optimizationMaxError); + Parameters::parse(parameters, Parameters::kRGBDOptimizeMaxErrorRepairRadius(), _optimizationMaxErrorRepairRadius); Parameters::parse(parameters, Parameters::kRtabmapStartNewMapOnLoopClosure(), _startNewMapOnLoopClosure); Parameters::parse(parameters, Parameters::kRtabmapStartNewMapOnGoodSignature(), _startNewMapOnGoodSignature); Parameters::parse(parameters, Parameters::kRGBDGoalReachedRadius(), _goalReachedRadius); @@ -723,29 +731,24 @@ void Rtabmap::parseParameters(const ParametersMap & parameters) isMemIncremental != _memory->isIncremental()) { // Mode has changed from Mapping to Localization, cleanup the local graph - if(_memory->isGraphReduced() && _memory->isIncremental()) + if(_memory->isIncremental()) { - // Force reducing graph, then remove filtered nodes from the optimized poses - std::map reducedIds; - _memory->incrementMapId(&reducedIds); - for(std::map::iterator iter=reducedIds.begin(); iter!=reducedIds.end(); ++iter) + if(_memory->isGraphReduced()) { - _optimizedPoses.erase(iter->first); + // Force reducing graph, then remove filtered nodes from the optimized poses + std::map reducedIds; + _memory->incrementMapId(&reducedIds); + for(std::map::iterator iter=reducedIds.begin(); iter!=reducedIds.end(); ++iter) + { + _optimizedPoses.erase(iter->first); + } } + _odomCachePoses.clear(); + _odomCacheConstraints.clear(); } // In both cases, we save the latest optimized graph and latest localization pose _memory->saveOptimizedPoses(_optimizedPoses, _lastLocalizationPose); - - // Mode changed from Localization to Mapping, clear local graph - if(!_memory->isIncremental()) { - _optimizedPoses.clear(); - _lastLocalizationPose.setNull(); - _mapCorrection.setIdentity(); - _mapCorrectionBackup.setNull(); - _localizationCovariance = cv::Mat(); - _lastLocalizationNodeId = 0; - } } _memory->parseParameters(parameters); @@ -760,12 +763,6 @@ void Rtabmap::parseParameters(const ParametersMap & parameters) { this->createGlobalScanMap(); } - - if(_memory->isIncremental()) - { - _odomCachePoses.clear(); - _odomCacheConstraints.clear(); - } } if(!_epipolarGeometry) @@ -851,7 +848,7 @@ int Rtabmap::getTotalMemSize() const { if(_memory) { - const Signature * s =_memory->getLastWorkingSignature(); + const Signature * s =_memory->getLastWorkingSignature(false); if(s) { return s->id(); @@ -902,11 +899,11 @@ void Rtabmap::setInitialPose(const Transform & initialPose) _mapCorrection.setIdentity(); _mapCorrectionBackup.setNull(); - if(_memory->getLastWorkingSignature()->id() && + if(_memory->getLastWorkingSignature(true)->id() && _optimizedPoses.empty()) { cv::Mat covariance; - this->optimizeCurrentMap(_memory->getLastWorkingSignature()->id(), false, _optimizedPoses, covariance, &_constraints); + this->optimizeCurrentMap(_memory->getLastWorkingSignature(true)->id(), false, _optimizedPoses, covariance, &_constraints); } } else @@ -942,6 +939,7 @@ int Rtabmap::triggerNewMap() UINFO("New map triggered, new map = %d", mapId); _optimizedPoses.clear(); _constraints.clear(); + _lastRejectedLoopClosureIds = std::make_pair(0,0); if(_bayesFilter) { @@ -973,9 +971,9 @@ bool Rtabmap::labelLocation(int id, const std::string & label) { return _memory->labelSignature(id, label); } - else if(_memory->isIncremental() && _memory->getLastWorkingSignature()) + else if(_memory->isIncremental() && _memory->getLastWorkingSignature(true)) { - return _memory->labelSignature(_memory->getLastWorkingSignature()->id(), label); + return _memory->labelSignature(_memory->getLastWorkingSignature(true)->id(), label); } else if(!_memory->isIncremental() && !_lastLocalizationPose.isNull() && !_lastLocalizationPose.isIdentity()) { @@ -1009,9 +1007,9 @@ bool Rtabmap::setUserData(int id, const cv::Mat & data) { return _memory->setUserData(id, data); } - else if(_memory->getLastWorkingSignature()) + else if(_memory->getLastWorkingSignature(true)) { - return _memory->setUserData(_memory->getLastWorkingSignature()->id(), data); + return _memory->setUserData(_memory->getLastWorkingSignature(true)->id(), data); } else { @@ -1055,7 +1053,7 @@ void Rtabmap::generateDOTGraph(const std::string & path, int id, int margin) void Rtabmap::exportPoses(const std::string & path, bool optimized, bool global, int format) { - if(_memory && _memory->getLastWorkingSignature()) + if(_memory && _memory->getLastWorkingSignature(!global)) { std::map poses; std::multimap constraints; @@ -1063,11 +1061,11 @@ void Rtabmap::exportPoses(const std::string & path, bool optimized, bool global, if(optimized) { cv::Mat covariance; - this->optimizeCurrentMap(_memory->getLastWorkingSignature()->id(), global, poses, covariance, &constraints); + this->optimizeCurrentMap(_memory->getLastWorkingSignature(!global)->id(), global, poses, covariance, &constraints); } else { - std::map ids = _memory->getNeighborsId(_memory->getLastWorkingSignature()->id(), 0, global?-1:0, true); + std::map ids = _memory->getNeighborsId(_memory->getLastWorkingSignature(!global)->id(), 0, global?-1:0, true); _memory->getMetricConstraints(uKeysSet(ids), poses, constraints, global); } @@ -1114,15 +1112,20 @@ void Rtabmap::resetMemory() _globalScanMap.clear(); _globalScanMapPoses.clear(); _nodesToRepublish.clear(); + _lastRejectedLoopClosureIds = std::make_pair(0,0); this->clearPath(0); if(_memory) { + if(_memory->isReadOnly()) + { + UWARN("Memory is reset but the database won't be cleared because read-only mode is enabled."); + } _memory->init(_databasePath, true, _parameters, true); - if(_memory->getLastWorkingSignature()) + if(_memory->getLastWorkingSignature(true)) { cv::Mat covariance; - optimizeCurrentMap(_memory->getLastWorkingSignature()->id(), false, _optimizedPoses, covariance, &_constraints); + optimizeCurrentMap(_memory->getLastWorkingSignature(true)->id(), false, _optimizedPoses, covariance, &_constraints); } if(_bayesFilter) { @@ -1430,9 +1433,9 @@ bool Rtabmap::process( else if(_memory->isIncremental()) // only in mapping mode { // Detect if the odometry is reset. If yes, trigger a new map. - if(_memory->getLastWorkingSignature()) + if(_memory->getLastWorkingSignature(false)) { - const Transform & lastPose = _memory->getLastWorkingSignature()->getPose(); // use raw odometry + const Transform & lastPose = _memory->getLastWorkingSignature(false)->getPose(); // use raw odometry // look for identity if(!lastPose.isIdentity() && odomPose.isIdentity()) @@ -1479,14 +1482,14 @@ bool Rtabmap::process( } } - signature = _memory->getLastWorkingSignature(); + signature = _memory->getLastWorkingSignature(false); _currentSessionHasGPS = _currentSessionHasGPS || signature->sensorData().gps().stamp() > 0.0; if(!signature) { UFATAL("Not supposed to be here...last signature is null?!?"); } - ULOGGER_INFO("Processing signature %d w=%d map=%d", signature->id(), signature->getWeight(), signature->mapId()); + ULOGGER_INFO("Processing signature %d (%f) w=%d map=%d", signature->id(), signature->getStamp(), signature->getWeight(), signature->mapId()); timeMemoryUpdate = timer.ticks(); ULOGGER_INFO("timeMemoryUpdate=%fs", timeMemoryUpdate); @@ -1502,8 +1505,8 @@ bool Rtabmap::process( float angleToClosestNodeInTheGraph = 0; if(_rgbdSlamMode) { - double linVar = odomCovariance.empty()?1.0f:uMax3(odomCovariance.at(0,0), odomCovariance.at(1,1)>=9999?0:odomCovariance.at(1,1), odomCovariance.at(2,2)>=9999?0:odomCovariance.at(2,2)); - double angVar = odomCovariance.empty()?1.0f:uMax3(odomCovariance.at(3,3)>=9999?0:odomCovariance.at(3,3), odomCovariance.at(4,4)>=9999?0:odomCovariance.at(4,4), odomCovariance.at(5,5)); + double linVar = odomCovariance.empty()?0.0f:uMax3(odomCovariance.at(0,0), odomCovariance.at(1,1)>=9999?0:odomCovariance.at(1,1), odomCovariance.at(2,2)>=9999?0:odomCovariance.at(2,2)); + double angVar = odomCovariance.empty()?0.0f:uMax3(odomCovariance.at(3,3)>=9999?0:odomCovariance.at(3,3), odomCovariance.at(4,4)>=9999?0:odomCovariance.at(4,4), odomCovariance.at(5,5)); statistics_.addStatistic(Statistics::kMemoryOdometry_variance_lin(), (float)linVar); statistics_.addStatistic(Statistics::kMemoryOdometry_variance_ang(), (float)angVar); @@ -1515,38 +1518,56 @@ bool Rtabmap::process( } else { + bool linkedToIntermediateNode = false; + Transform t; + if(_memory->isIncremental()) + { + // Check small motion if current node is not an intermediate node already + if(signature->getWeight() >= 0) + { + // It should contain only the query and its first (non-intermediate) neighbor (smaller id) + std::map neighbors = _memory->getNeighborsId(signature->id(), 2, 0, true, true, true, true); + if(neighbors.size() == 2) + { + int nid = neighbors.begin()->first; + const std::multimap & links = signature->getLinks(); + if(links.find(nid) != links.end()) + { + // direct neighbor + t = links.find(nid)->second.transform(); + } + else + { + // Use optimized poses to check how far it is from the latest non-intermediate node + std::map::iterator niter = _optimizedPoses.find(nid); + if(niter != _optimizedPoses.end()) + { + t = niter->second.inverse() * _mapCorrection * signature->getPose(); + } + // not direct link, it means there are intermediate nodes + linkedToIntermediateNode = true; + } + } + } + } + else if(!_odomCachePoses.empty()) + { + t = _odomCachePoses.rbegin()->second.inverse() * signature->getPose(); + } + if(_rgbdLinearUpdate > 0.0f || _rgbdAngularUpdate > 0.0f) { //============================================================ // Minimum displacement required to add to Memory //============================================================ - Transform t; - - if(_memory->isIncremental()) - { - const std::multimap & links = signature->getLinks(); - if(links.size() && links.begin()->second.type() == Link::kNeighbor) - { - const Signature * s = _memory->getSignature(links.begin()->second.to()); - UASSERT(s!=0); - // don't filter if the new node is not intermediate but previous one is - if(signature->getWeight() < 0 || s->getWeight() >= 0) - { - t = links.begin()->second.transform(); - } - } - } - else if(!_odomCachePoses.empty()) - { - t = _odomCachePoses.rbegin()->second.inverse() * signature->getPose(); - } if(!t.isNull()) { float x,y,z, roll,pitch,yaw; t.getTranslationAndEulerAngles(x,y,z, roll,pitch,yaw); - bool isMoving = fabs(x) > _rgbdLinearUpdate || - fabs(y) > _rgbdLinearUpdate || - fabs(z) > _rgbdLinearUpdate || + bool isMoving = (_rgbdLinearUpdate > 0.0f && ( + fabs(x) > _rgbdLinearUpdate || + fabs(y) > _rgbdLinearUpdate || + fabs(z) > _rgbdLinearUpdate)) || (_rgbdAngularUpdate>0.0f && ( fabs(roll) > _rgbdAngularUpdate || fabs(pitch) > _rgbdAngularUpdate || @@ -1560,7 +1581,7 @@ bool Rtabmap::process( } } } - if(odomVelocity.size() == 6) + if(odomVelocity.size() == 6 && signature->getWeight() != -1) { // This will disable global loop closure detection, only retrieval will be done. // The location will also be deleted at the end. @@ -1568,6 +1589,10 @@ bool Rtabmap::process( (_rgbdLinearSpeedUpdate>0.0f && uMax3(fabs(odomVelocity[0]), fabs(odomVelocity[1]), fabs(odomVelocity[2])) > _rgbdLinearSpeedUpdate) || (_rgbdAngularSpeedUpdate>0.0f && uMax3(fabs(odomVelocity[3]), fabs(odomVelocity[4]), fabs(odomVelocity[5])) > _rgbdAngularSpeedUpdate); } + if(linkedToIntermediateNode && (smallDisplacement || tooFastMovement)) + { + _memory->convertToIntermediate(signature->id()); + } } // Update optimizedPoses with the newly added node @@ -1607,6 +1632,7 @@ bool Rtabmap::process( Transform t = _memory->computeTransform(oldId, signature->id(), guess, &info); if(!t.isNull()) { + UASSERT(!info.covariance.empty() && info.covariance.at(0,0) > 0.0 && info.covariance.at(5,5) > 0.0); UINFO("Odometry refining: update neighbor link (%d->%d, variance:lin=%f, ang=%f) from %s to %s", oldId, signature->id(), @@ -1614,7 +1640,6 @@ bool Rtabmap::process( info.covariance.at(5,5), guess.prettyPrint().c_str(), t.prettyPrint().c_str()); - UASSERT(info.covariance.at(0,0) > 0.0 && info.covariance.at(5,5) > 0.0); _memory->updateLink(Link(oldId, signature->id(), signature->getLinks().begin()->second.type(), t, info.covariance.inv())); if(_optimizeFromGraphEnd) @@ -1788,7 +1813,7 @@ bool Rtabmap::process( } } _lastLocalizationPose = newPose; // keep in cache the latest corrected pose - if(!_memory->isIncremental() && signature->getWeight() >= 0) + if(signature->getWeight() >= 0) { UDEBUG("Update odometry localization cache (size=%d/%d)", (int)_odomCachePoses.size(), _maxOdomCacheSize); if(!_odomCachePoses.empty()) @@ -1894,7 +1919,7 @@ bool Rtabmap::process( *iter, transform.prettyPrint().c_str()); // Add a loop constraint - UASSERT(info.covariance.at(0,0) > 0.0 && info.covariance.at(5,5) > 0.0); + UASSERT(!info.covariance.empty() && info.covariance.at(0,0) > 0.0 && info.covariance.at(5,5) > 0.0); if(_memory->addLink(Link(signature->id(), *iter, Link::kLocalTimeClosure, transform, getInformation(info.covariance)))) { ++proximityDetectionsInTimeFound; @@ -2203,7 +2228,7 @@ bool Rtabmap::process( } } // if(_memory->getWorkingMemSize()) }// !isBadSignature - else if(!signature->isBadSignature() && (smallDisplacement || tooFastMovement)) + else if(!signature->isBadSignature() && signature->getWeight()>=0 && (smallDisplacement || tooFastMovement)) { _highestHypothesis = lastHighestHypothesis; UDEBUG("smallDisplacement=%d tooFastMovement=%d", smallDisplacement?1:0, tooFastMovement?1:0); @@ -2237,8 +2262,9 @@ bool Rtabmap::process( maxLocalLocationsImmunized = _localImmunizationRatio * float(_memory->getWorkingMem().size()); } // no need to do retrieval or immunization of locations if memory management - // is disabled and all nodes are in WM - if(!(_memory->allNodesInWM() && maxLocalLocationsImmunized == 0)) + // is disabled and all nodes are in WM. + // Also skip memory mangement on intermediate nodes + if(!(_memory->allNodesInWM() && maxLocalLocationsImmunized == 0) && signature->getWeight()>=0) { if(retrievalId > 0) { @@ -2378,11 +2404,13 @@ bool Rtabmap::process( // RETRIEVAL 2/3 : Update planned path and get next nodes to retrieve //============================================================ std::list retrievalLocalIds; - if(_rgbdSlamMode) + if(_rgbdSlamMode && signature->getWeight()>=0) { // Priority on locations on the planned path if(_path.size()) { + // Note: retrieval on path with intermediate nodes is not supported. Note that the planned path would + // eventually fail anyway because intermediate nodes are not in _optimizedPoses. updateGoalIndex(); float distanceSoFar = 0.0f; @@ -2395,24 +2423,22 @@ bool Rtabmap::process( distanceSoFar += _path[i-1].second.getDistance(_path[i].second); } - if(distanceSoFar <= _localRadius) + if(_memory->getSignature(_path[i].first) != 0) { - if(_memory->getSignature(_path[i].first) != 0) + if(immunizedLocations.insert(_path[i].first).second) { - if(immunizedLocations.insert(_path[i].first).second) - { - ++immunizedLocally; - } - UDEBUG("Path immunization: node %d (dist=%fm)", _path[i].first, distanceSoFar); - } - else if(retrievalLocalIds.size() < _maxLocalRetrieved) - { - UINFO("retrieval of node %d on path (dist=%fm)", _path[i].first, distanceSoFar); - retrievalLocalIds.push_back(_path[i].first); - // retrieved locations are automatically immunized + ++immunizedLocally; } + UDEBUG("Path immunization: node %d (dist=%fm)", _path[i].first, distanceSoFar); } - else + else if(retrievalLocalIds.size() < _maxLocalRetrieved) + { + UINFO("retrieval of node %d on path (dist=%fm)", _path[i].first, distanceSoFar); + retrievalLocalIds.push_back(_path[i].first); + // retrieved locations are automatically immunized + } + + if(distanceSoFar > _localRadius) { UDEBUG("Stop on node %d (dist=%fm > %fm)", _path[i].first, distanceSoFar, _localRadius); @@ -2507,11 +2533,13 @@ bool Rtabmap::process( { nearNodesByDist.insert(std::make_pair(iter->second, iter->first)); } - UINFO("near nodes=%d, max local immunized=%d, ratio=%f WM=%d", + UINFO("near nodes=%d, max local immunized=%d (immunized by path so far=%d), ratio=%f WM=%d", (int)nearNodesByDist.size(), maxLocalLocationsImmunized, + immunizedLocally, _localImmunizationRatio, (int)_memory->getWorkingMem().size()); + std::list retrievalLocalIdsIntermediate; for(std::multimap::iterator iter=nearNodesByDist.begin(); iter!=nearNodesByDist.end() && (retrievalLocalIds.size() < _maxLocalRetrieved || immunizedLocally < maxLocalLocationsImmunized); ++iter) @@ -2519,17 +2547,29 @@ bool Rtabmap::process( const Signature * s = _memory->getSignature(iter->second); if(s!=0) { - // If there is a change of direction, better to be retrieving - // ALL nearest signatures than only newest neighbors - const std::multimap & links = s->getLinks(); - for(std::multimap::const_reverse_iterator jter=links.rbegin(); - jter!=links.rend() && retrievalLocalIds.size() < _maxLocalRetrieved; - ++jter) + if(s->getWeight() != -1 && retrievalLocalIds.size() < _maxLocalRetrieved) { - if(_memory->getSignature(jter->first) == 0) + // If there is a change of direction, better to be retrieving + // all nearest signatures than only newest neighbors. + // Use getNeighborsId instead of direct links to support intermediate nodes. + std::map ids = _memory->getNeighborsId(s->id(), 2, _maxLocalRetrieved-retrievalLocalIds.size(), true, false, false); + for(std::map::const_reverse_iterator jter=ids.rbegin(); + jter!=ids.rend() && (retrievalLocalIds.size() < _maxLocalRetrieved || jter->second == 0); + ++jter) { - UINFO("retrieval of node %d on local map", jter->first); - retrievalLocalIds.push_back(jter->first); + if(_memory->getSignature(jter->first) == 0) + { + if(jter->second == 0) + { + UINFO("retrieval of intermediate node %d (margin=%d) on local map (from=%d)", jter->first, jter->second, s->id()); + retrievalLocalIdsIntermediate.push_back(jter->first); + } + else + { + UINFO("retrieval of node %d (margin=%d) on local map (from=%d)", jter->first, jter->second, s->id()); + retrievalLocalIds.push_back(jter->first); + } + } } } if(!_memory->isInSTM(s->id()) && immunizedLocally < maxLocalLocationsImmunized) @@ -2546,20 +2586,29 @@ bool Rtabmap::process( if(retrievalLocalIds.size() < _maxLocalRetrieved) { std::set retrievalLocalIdsSet(retrievalLocalIds.begin(), retrievalLocalIds.end()); + retrievalLocalIdsSet.insert(retrievalLocalIdsIntermediate.begin(), retrievalLocalIdsIntermediate.end()); for(std::list::iterator iter=retrievalLocalIds.begin(); iter!=retrievalLocalIds.end() && retrievalLocalIds.size() < _maxLocalRetrieved; ++iter) { - std::map ids = _memory->getNeighborsId(*iter, 2, _maxLocalRetrieved - (unsigned int)retrievalLocalIds.size() + 1, true, false); + std::map ids = _memory->getNeighborsId(*iter, 2, _maxLocalRetrieved - (unsigned int)retrievalLocalIds.size() + 1, true, false, false); for(std::map::reverse_iterator jter=ids.rbegin(); - jter!=ids.rend() && retrievalLocalIds.size() < _maxLocalRetrieved; + jter!=ids.rend() && (retrievalLocalIds.size() < _maxLocalRetrieved || jter->second == 0); ++jter) { if(_memory->getSignature(jter->first) == 0 && retrievalLocalIdsSet.find(jter->first) == retrievalLocalIdsSet.end()) { - UINFO("retrieval of node %d on local map", jter->first); - retrievalLocalIds.push_back(jter->first); + if(jter->second == 0) + { + UINFO("retrieval of intermediate node %d (margin=%d) on local map (from=%d)", jter->first, jter->second, *iter); + retrievalLocalIdsIntermediate.push_back(jter->first); + } + else + { + UINFO("retrieval of node %d (margin=%d) on local map (from=%d)", jter->first, jter->second, *iter); + retrievalLocalIds.push_back(jter->first); + } retrievalLocalIdsSet.insert(jter->first); } } @@ -2573,6 +2622,7 @@ bool Rtabmap::process( } // insert them first to make sure they are loaded. + reactivatedIds.insert(reactivatedIds.begin(), retrievalLocalIdsIntermediate.begin(), retrievalLocalIdsIntermediate.end()); reactivatedIds.insert(reactivatedIds.begin(), retrievalLocalIds.begin(), retrievalLocalIds.end()); } } @@ -2614,6 +2664,7 @@ bool Rtabmap::process( int loopClosureVisualInliers = 0; // for statistics float loopClosureVisualInliersRatio = 0.0f; int loopClosureVisualMatches = 0; + float loopClosureVisualVariance = 0.0f; float loopClosureLinearVariance = 0.0f; float loopClosureAngularVariance = 0.0f; float loopClosureVisualInliersMeanDist = 0; @@ -2667,7 +2718,9 @@ bool Rtabmap::process( std::map nearestIds = graph::findNearestNodes(signature->id(), _optimizedPoses, _localRadius); UDEBUG("nearestIds=%d/%d", (int)nearestIds.size(), (int)_optimizedPoses.size()); std::map nearestPoses; + std::map optimizedPosesWithOdomCache; std::multimap links; + std::map * refPoses = &_optimizedPoses; if(_memory->isIncremental() && _proximityMaxGraphDepth>0) { // get bidirectional links @@ -2679,6 +2732,25 @@ bool Rtabmap::process( links.insert(std::make_pair(iter->second.to(), iter->second.from())); // <-> } } + if(_odomCachePoses.size() > 1) + { + // Add odometry cache if it contains a loop closure + // That could happen when we just switched from localization mode to + // mapping mode while being localized on the previous session. + optimizedPosesWithOdomCache = _optimizedPoses; + optimizedPosesWithOdomCache.insert(_odomCachePoses.begin(), _odomCachePoses.end()); + refPoses = &optimizedPosesWithOdomCache; + for(std::multimap::iterator iter=_odomCacheConstraints.begin(); iter!=_odomCacheConstraints.end(); ++iter) + { + if(uContains(optimizedPosesWithOdomCache, iter->second.from()) && + uContains(optimizedPosesWithOdomCache, iter->second.to()) && + iter->second.from() != iter->second.to()) + { + links.insert(std::make_pair(iter->second.from(), iter->second.to())); + links.insert(std::make_pair(iter->second.to(), iter->second.from())); // <-> + } + } + } } for(std::map::iterator iter=nearestIds.lower_bound(1); iter!=nearestIds.end(); ++iter) { @@ -2686,7 +2758,7 @@ bool Rtabmap::process( { if(_memory->isIncremental() && _proximityMaxGraphDepth > 0) { - std::list > path = graph::computePath(_optimizedPoses, links, signature->id(), iter->first); + std::list > path = graph::computePath(*refPoses, links, signature->id(), iter->first); UDEBUG("Graph depth to %d = %ld", iter->first, path.size()); if(!path.empty() && (int)path.size() <= _proximityMaxGraphDepth) { @@ -2784,7 +2856,7 @@ bool Rtabmap::process( signature->id(), nearestId, transform.prettyPrint().c_str()); - UASSERT(info.covariance.at(0,0) > 0.0 && info.covariance.at(5,5) > 0.0); + UASSERT(!info.covariance.empty() && info.covariance.at(0,0) > 0.0 && info.covariance.at(5,5) > 0.0); //for statistics loopClosureVisualInliersMeanDist = info.inliersMeanDistance; @@ -2796,6 +2868,7 @@ bool Rtabmap::process( loopClosureVisualInliers = info.inliers; loopClosureVisualInliersRatio = info.inliersRatio; loopClosureVisualMatches = info.matches; + loopClosureVisualVariance = info.variance; cv::Mat information = getInformation(info.covariance); loopClosureLinearVariance = 1.0/information.at(0,0); @@ -2995,7 +3068,7 @@ bool Rtabmap::process( } // set Identify covariance for laser scan matching only - UASSERT(info.covariance.at(0,0) > 0.0 && info.covariance.at(5,5) > 0.0); + UASSERT(!info.covariance.empty() && info.covariance.at(0,0) > 0.0 && info.covariance.at(5,5) > 0.0); _memory->addLink(Link(signature->id(), nearestId, Link::kLocalSpaceClosure, transform, getInformation(info.covariance)/_proximityMergedScanCovFactor, scanMatchingIds)); loopClosureLinksAdded.push_back(std::make_pair(signature->id(), nearestId)); @@ -3063,6 +3136,7 @@ bool Rtabmap::process( loopClosureVisualInliers = info.inliers; loopClosureVisualInliersRatio = info.inliersRatio; loopClosureVisualMatches = info.matches; + loopClosureVisualVariance = info.variance; rejectedLoopClosure = transform.isNull(); if(rejectedLoopClosure) { @@ -3083,7 +3157,7 @@ bool Rtabmap::process( if(!rejectedLoopClosure) { // Make the new one the parent of the old one - UASSERT(info.covariance.at(0,0) > 0.0 && info.covariance.at(5,5) > 0.0); + UASSERT(!info.covariance.empty() && info.covariance.at(0,0) > 0.0 && info.covariance.at(5,5) > 0.0); loopClosureLinearVariance = uMax3(info.covariance.at(0,0), info.covariance.at(1,1)>=9999?0:info.covariance.at(1,1), info.covariance.at(2,2)>=9999?0:info.covariance.at(2,2)); loopClosureAngularVariance = uMax3(info.covariance.at(3,3)>=9999?0:info.covariance.at(3,3), info.covariance.at(4,4)>=9999?0:info.covariance.at(4,4), info.covariance.at(5,5)); @@ -3113,7 +3187,7 @@ bool Rtabmap::process( // Landmark //============================================================ std::map > landmarksDetected; // - if(!signature->getLandmarks().empty() && !_graphOptimizer->landmarksIgnored()) + if(!signature->getLandmarks().empty() && !_graphOptimizer->landmarksIgnored() && signature->getWeight()!=-1) { bool hasGlobalLoopClosuresInOdomCache = !graph::filterLinks(_odomCacheConstraints, Link::kGlobalClosure, true).empty() || _loopClosureHypothesis.first != 0; UDEBUG("hasGlobalLoopClosuresInOdomCache=%d", hasGlobalLoopClosuresInOdomCache?1:0); @@ -3158,27 +3232,18 @@ bool Rtabmap::process( UASSERT(uContains(_optimizedPoses, signature->id())); UASSERT_MSG(uContains(_optimizedPoses, _path[_pathCurrentIndex].first), uFormat("id=%d", _path[_pathCurrentIndex].first).c_str()); Transform virtualLoop = _optimizedPoses.at(signature->id()).inverse() * _optimizedPoses.at(_path[_pathCurrentIndex].first); - - if(_localRadius == 0.0f || virtualLoop.getNorm() < _localRadius) - { - _memory->addLink(Link(signature->id(), _path[_pathCurrentIndex].first, Link::kVirtualClosure, virtualLoop, cv::Mat::eye(6,6,CV_64FC1)*0.01)); // set high variance - } - else - { - UERROR("Virtual link larger than local radius (%fm > %fm). Aborting the plan!", - virtualLoop.getNorm(), _localRadius); - this->clearPath(-1); - } + _memory->addLink(Link(signature->id(), _path[_pathCurrentIndex].first, Link::kVirtualClosure, virtualLoop, cv::Mat::eye(6,6,CV_64FC1)*0.01)); // set high variance } } //============================================================ // Optimize map graph //============================================================ - float maxLinearError = 0.0f; - float maxLinearErrorRatio = 0.0f; - float maxAngularError = 0.0f; - float maxAngularErrorRatio = 0.0f; + graph::MaxGraphErrors maxGraphErrors; + std::pair maxGraphErrorsLinearIds(0,0); + std::pair maxGraphErrorsAngularIds(0,0); + std::pair maxGraphErrorsRemovedIds(0,0); + int maxGraphErrorsRemovedCount = 0; double optimizationError = 0.0; int optimizationIterations = 0; Transform previousMapCorrection; @@ -3198,6 +3263,8 @@ bool Rtabmap::process( UDEBUG("Not self ref links: %d", (int)graph::filterLinks(signature->getLinks(), Link::kSelfRefLink).size()); if(_rgbdSlamMode + && + signature->getWeight() != -1 // Ignore graph optimization on intermediate nodes && (_loopClosureHypothesis.first>0 || lastProximitySpaceClosureId>0 || // can be different map of the current one @@ -3269,6 +3336,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) { @@ -3276,6 +3344,7 @@ bool Rtabmap::process( if(iterPose != _optimizedPoses.end() && poses.find(iterPose->first) == poses.end()) { 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, 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); @@ -3285,11 +3354,14 @@ bool Rtabmap::process( std::map posesOut; std::multimap edgeConstraintsOut; + bool priorsIgnored = _graphOptimizer->priorsIgnored(); 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); + if(ULogger::level() == ULogger::kDebug) { for(std::map::iterator iter=posesOut.begin(); iter!=posesOut.end(); ++iter) @@ -3314,6 +3386,7 @@ bool Rtabmap::process( UDEBUG("Opt %d %s", iter->first, iter->second.prettyPrint().c_str()); } + std::list > removedLinks; if(optPoses.empty()) { UWARN("Optimization failed, rejecting localization!"); @@ -3322,76 +3395,105 @@ bool Rtabmap::process( else { UINFO("Compute max graph errors..."); - const Link * maxLinearLink = 0; - const Link * maxAngularLink = 0; - graph::computeMaxGraphErrors( + maxGraphErrors = graph::computeMaxGraphErrors( optPoses, edgeConstraintsOut, - maxLinearErrorRatio, - maxAngularErrorRatio, - maxLinearError, - maxAngularError, - &maxLinearLink, - &maxAngularLink, _graphOptimizer->isSlam2d()); - if(maxLinearLink == 0 && maxAngularLink==0) + if(!maxGraphErrors.linearLink.isValid() && !maxGraphErrors.angularLink.isValid()) { UWARN("Could not compute graph errors! Rejecting localization!"); rejectLocalization = true; } - if(maxLinearLink) + if(maxGraphErrors.linearLink.isValid()) { + maxGraphErrorsLinearIds = std::make_pair(maxGraphErrors.linearLink.from(), maxGraphErrors.linearLink.to()); UINFO("Max optimization linear error = %f m (link %d->%d, var=%f, ratio error/std=%f, thr=%f)", - maxLinearError, - maxLinearLink->from(), - maxLinearLink->to(), - maxLinearLink->transVariance(), - maxLinearError/sqrt(maxLinearLink->transVariance()), + maxGraphErrors.linear, + maxGraphErrors.linearLink.from(), + maxGraphErrors.linearLink.to(), + maxGraphErrors.linearLink.transVariance(), + maxGraphErrors.linear/sqrt(maxGraphErrors.linearLink.transVariance()), _optimizationMaxError); - if(_optimizationMaxError > 0.0f && maxLinearErrorRatio > _optimizationMaxError) + if(_optimizationMaxError > 0.0f && maxGraphErrors.linearRatio > _optimizationMaxError) { - UWARN("Rejecting localization (%d <-> %d) in this " - "iteration because a wrong loop closure has been " - "detected after graph optimization, resulting in " - "a maximum graph error ratio of %f (edge %d->%d, type=%d, abs error=%f m, stddev=%f). The " - "maximum error ratio parameter \"%s\" is %f of std deviation.", - localizationLinks.rbegin()->second.from(), - localizationLinks.rbegin()->second.to(), - maxLinearErrorRatio, - maxLinearLink->from(), - maxLinearLink->to(), - maxLinearLink->type(), - maxLinearError, - sqrt(maxLinearLink->transVariance()), - Parameters::kRGBDOptimizeMaxError().c_str(), - _optimizationMaxError); - rejectLocalization = true; + if( _optimizationMaxErrorRepairRadius > 0.0 && + maxGraphErrorsLinearIds == _lastRejectedLoopClosureIds && + graph::findLink(edgeConstraintsOut, maxGraphErrorsLinearIds.first, maxGraphErrorsLinearIds.second) != edgeConstraintsOut.end()) + { + UWARN("We detected 2 consecutive loop closure rejections because of the same loop closure link (%d->%d), trying optimization again without that link...", + maxGraphErrorsLinearIds.first, maxGraphErrorsLinearIds.second); + + UDEBUG("priorsIgnored was %s", priorsIgnored?"true":"false"); + _graphOptimizer->setPriorsIgnored(false); //temporary set false to use priors above to fix nodes of the map + removedLinks = repairGraph( + maxGraphErrors, + optPoses, + edgeConstraintsOut, + optimizationError, + optimizationIterations, + locOptCovariance); + _graphOptimizer->setPriorsIgnored(priorsIgnored); // set back + + if(removedLinks.empty()) + { + UWARN("Optimization failed when trying to repair the graph."); + rejectLocalization = true; + } + } + else { + rejectLocalization = true; + } + + if(rejectLocalization) + { + UWARN("Rejecting localization (%d <-> %d) in this " + "iteration because a wrong loop closure has been " + "detected after graph optimization, resulting in " + "a maximum graph error ratio of %f (edge %d->%d, type=%d, abs error=%f m, stddev=%f). The " + "maximum error ratio parameter \"%s\" is %f of std deviation.", + localizationLinks.rbegin()->second.from(), + localizationLinks.rbegin()->second.to(), + maxGraphErrors.linearRatio, + maxGraphErrors.linearLink.from(), + maxGraphErrors.linearLink.to(), + maxGraphErrors.linearLink.type(), + maxGraphErrors.linear, + sqrt(maxGraphErrors.linearLink.transVariance()), + Parameters::kRGBDOptimizeMaxError().c_str(), + _optimizationMaxError); + + if(maxGraphErrors.linearLink.type() != Link::kNeighbor) + { + _lastRejectedLoopClosureIds = maxGraphErrorsLinearIds; + } + } } - else if(_optimizationMaxError == 0.0f && maxLinearErrorRatio>100 && !_graphOptimizer->isRobust()) + else if(_optimizationMaxError == 0.0f && maxGraphErrors.linearRatio>100 && !_graphOptimizer->isRobust()) { 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()), + maxGraphErrors.linearRatio, + maxGraphErrors.linearLink.from(), + maxGraphErrors.linearLink.to(), + maxGraphErrors.linearLink.type(), + maxGraphErrors.linear, + sqrt(maxGraphErrors.linearLink.transVariance()), Parameters::kRGBDOptimizeMaxError().c_str()); } } - if(maxAngularLink) + if(maxGraphErrors.angularLink.isValid()) { + maxGraphErrorsAngularIds = std::make_pair(maxGraphErrors.angularLink.from(), maxGraphErrors.angularLink.to()); UINFO("Max optimization angular error = %f deg (link %d->%d, var=%f, ratio error/std=%f, thr=%f)", - maxAngularError*180.0f/CV_PI, - maxAngularLink->from(), - maxAngularLink->to(), - maxAngularLink->rotVariance(), - maxAngularError/sqrt(maxAngularLink->rotVariance()), + maxGraphErrors.angular*180.0f/CV_PI, + maxGraphErrors.angularLink.from(), + maxGraphErrors.angularLink.to(), + maxGraphErrors.angularLink.rotVariance(), + maxGraphErrors.angular/sqrt(maxGraphErrors.angularLink.rotVariance()), _optimizationMaxError); - if(_optimizationMaxError > 0.0f && maxAngularErrorRatio > _optimizationMaxError) + if(_optimizationMaxError > 0.0f && maxGraphErrors.angularRatio > _optimizationMaxError) { UWARN("Rejecting localization (%d <-> %d) in this " "iteration because a wrong loop closure has been " @@ -3400,27 +3502,27 @@ bool Rtabmap::process( "maximum error ratio parameter \"%s\" is %f of std deviation.", localizationLinks.rbegin()->second.from(), localizationLinks.rbegin()->second.to(), - maxAngularErrorRatio, - maxAngularLink->from(), - maxAngularLink->to(), - maxAngularLink->type(), - maxAngularError*180.0f/CV_PI, - sqrt(maxAngularLink->rotVariance()), + maxGraphErrors.angularRatio, + maxGraphErrors.angularLink.from(), + maxGraphErrors.angularLink.to(), + maxGraphErrors.angularLink.type(), + maxGraphErrors.angular*180.0f/CV_PI, + sqrt(maxGraphErrors.angularLink.rotVariance()), Parameters::kRGBDOptimizeMaxError().c_str(), _optimizationMaxError); rejectLocalization = true; } - else if(_optimizationMaxError == 0.0f && maxAngularErrorRatio>100 && !_graphOptimizer->isRobust()) + else if(_optimizationMaxError == 0.0f && maxGraphErrors.angularRatio>100 && !_graphOptimizer->isRobust()) { 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()), + maxGraphErrors.angularRatio, + maxGraphErrors.angularLink.from(), + maxGraphErrors.angularLink.to(), + maxGraphErrors.angularLink.type(), + maxGraphErrors.angular*180.0f/CV_PI, + sqrt(maxGraphErrors.angularLink.rotVariance()), Parameters::kRGBDOptimizeMaxError().c_str()); } } @@ -3443,7 +3545,6 @@ bool Rtabmap::process( { rejectLocalization = false; UWARN("Global and loop closures seem not tallying together, try again to optimize without local loop closures..."); - priorsIgnored = _graphOptimizer->priorsIgnored(); 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. @@ -3472,34 +3573,27 @@ bool Rtabmap::process( else { UINFO("Compute max graph errors..."); - const Link * maxLinearLink = 0; - const Link * maxAngularLink = 0; - graph::computeMaxGraphErrors( + maxGraphErrors = graph::computeMaxGraphErrors( optPoses, edgeConstraintsOut, - maxLinearErrorRatio, - maxAngularErrorRatio, - maxLinearError, - maxAngularError, - &maxLinearLink, - &maxAngularLink, _graphOptimizer->isSlam2d()); - if(maxLinearLink == 0 && maxAngularLink==0) + if(!maxGraphErrors.linearLink.isValid() && !maxGraphErrors.angularLink.isValid()) { UWARN("Could not compute graph errors! Rejecting localization!"); rejectLocalization = true; } - if(maxLinearLink) + if(maxGraphErrors.linearLink.isValid()) { + maxGraphErrorsLinearIds = std::make_pair(maxGraphErrors.linearLink.from(), maxGraphErrors.linearLink.to()); UINFO("Max optimization linear error = %f m (link %d->%d, var=%f, ratio error/std=%f, thr=%f)", - maxLinearError, - maxLinearLink->from(), - maxLinearLink->to(), - maxLinearLink->transVariance(), - maxLinearError/sqrt(maxLinearLink->transVariance()), + maxGraphErrors.linear, + maxGraphErrors.linearLink.from(), + maxGraphErrors.linearLink.to(), + maxGraphErrors.linearLink.transVariance(), + maxGraphErrors.linear/sqrt(maxGraphErrors.linearLink.transVariance()), _optimizationMaxError); - if(_optimizationMaxError > 0.0f && maxLinearErrorRatio > _optimizationMaxError) + if(_optimizationMaxError > 0.0f && maxGraphErrors.linearRatio > _optimizationMaxError) { UWARN("Rejecting localization (%d <-> %d) in this " "iteration because a wrong loop closure has been " @@ -3508,40 +3602,41 @@ bool Rtabmap::process( "maximum error ratio parameter \"%s\" is %f of std deviation.", localizationLinks.rbegin()->second.from(), localizationLinks.rbegin()->second.to(), - maxLinearErrorRatio, - maxLinearLink->from(), - maxLinearLink->to(), - maxLinearLink->type(), - maxLinearError, - sqrt(maxLinearLink->transVariance()), + maxGraphErrors.linearRatio, + maxGraphErrors.linearLink.from(), + maxGraphErrors.linearLink.to(), + maxGraphErrors.linearLink.type(), + maxGraphErrors.linear, + sqrt(maxGraphErrors.linearLink.transVariance()), Parameters::kRGBDOptimizeMaxError().c_str(), _optimizationMaxError); rejectLocalization = true; } - else if(_optimizationMaxError == 0.0f && maxLinearErrorRatio>100 && !_graphOptimizer->isRobust()) + else if(_optimizationMaxError == 0.0f && maxGraphErrors.linearRatio>100 && !_graphOptimizer->isRobust()) { 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()), + maxGraphErrors.linearRatio, + maxGraphErrors.linearLink.from(), + maxGraphErrors.linearLink.to(), + maxGraphErrors.linearLink.type(), + maxGraphErrors.linear, + sqrt(maxGraphErrors.linearLink.transVariance()), Parameters::kRGBDOptimizeMaxError().c_str()); } } - if(maxAngularLink) + if(maxGraphErrors.angularLink.isValid()) { + maxGraphErrorsAngularIds = std::make_pair(maxGraphErrors.angularLink.from(), maxGraphErrors.angularLink.to()); UINFO("Max optimization angular error = %f deg (link %d->%d, var=%f, ratio error/std=%f, thr=%f)", - maxAngularError*180.0f/CV_PI, - maxAngularLink->from(), - maxAngularLink->to(), - maxAngularLink->rotVariance(), - maxAngularError/sqrt(maxAngularLink->rotVariance()), + maxGraphErrors.angular*180.0f/CV_PI, + maxGraphErrors.angularLink.from(), + maxGraphErrors.angularLink.to(), + maxGraphErrors.angularLink.rotVariance(), + maxGraphErrors.angular/sqrt(maxGraphErrors.angularLink.rotVariance()), _optimizationMaxError); - if(_optimizationMaxError > 0.0f && maxAngularErrorRatio > _optimizationMaxError) + if(_optimizationMaxError > 0.0f && maxGraphErrors.angularRatio > _optimizationMaxError) { UWARN("Rejecting localization (%d <-> %d) in this " "iteration because a wrong loop closure has been " @@ -3550,27 +3645,27 @@ bool Rtabmap::process( "maximum error ratio parameter \"%s\" is %f of std deviation.", localizationLinks.rbegin()->second.from(), localizationLinks.rbegin()->second.to(), - maxAngularErrorRatio, - maxAngularLink->from(), - maxAngularLink->to(), - maxAngularLink->type(), - maxAngularError*180.0f/CV_PI, - sqrt(maxAngularLink->rotVariance()), + maxGraphErrors.angularRatio, + maxGraphErrors.angularLink.from(), + maxGraphErrors.angularLink.to(), + maxGraphErrors.angularLink.type(), + maxGraphErrors.angular*180.0f/CV_PI, + sqrt(maxGraphErrors.angularLink.rotVariance()), Parameters::kRGBDOptimizeMaxError().c_str(), _optimizationMaxError); rejectLocalization = true; } - else if(_optimizationMaxError == 0.0f && maxAngularErrorRatio>100 && !_graphOptimizer->isRobust()) + else if(_optimizationMaxError == 0.0f && maxGraphErrors.angularRatio>100 && !_graphOptimizer->isRobust()) { 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()), + maxGraphErrors.angularRatio, + maxGraphErrors.angularLink.from(), + maxGraphErrors.angularLink.to(), + maxGraphErrors.angularLink.type(), + maxGraphErrors.angular*180.0f/CV_PI, + sqrt(maxGraphErrors.angularLink.rotVariance()), Parameters::kRGBDOptimizeMaxError().c_str()); } } @@ -3597,6 +3692,21 @@ bool Rtabmap::process( } odomCacheProximityLinksCleared = before - _odomCacheConstraints.size(); } + else if(!removedLinks.empty()) + { + // If we removed links (repaired the graph) + for(auto link: removedLinks) + { + UWARN("Removing link %d->%d from odometry cache", link.first, link.second); + auto iter = graph::findLink(_odomCacheConstraints, link.first, link.second); + if(iter!=_odomCacheConstraints.end()) { + _odomCacheConstraints.erase(iter); + } + } + UWARN("Successfully repaired the graph."); + maxGraphErrorsRemovedIds = removedLinks.front(); + maxGraphErrorsRemovedCount = removedLinks.size(); + } // Count how many localization links are in the constraints bool hadAlreadyLocalizationLinks = false; @@ -3823,64 +3933,102 @@ bool Rtabmap::process( constraints.size()) { UINFO("Compute max graph errors..."); - const Link * maxLinearLink = 0; - const Link * maxAngularLink = 0; - graph::computeMaxGraphErrors( + maxGraphErrors = graph::computeMaxGraphErrors( poses, - constraints, - maxLinearErrorRatio, - maxAngularErrorRatio, - maxLinearError, - maxAngularError, - &maxLinearLink, - &maxAngularLink); - if(maxLinearLink == 0 && maxAngularLink==0) + constraints); + if(!maxGraphErrors.linearLink.isValid() && !maxGraphErrors.angularLink.isValid()) { UWARN("Could not compute graph errors! Wrong loop closures could be accepted!"); } bool reject = false; - if(maxLinearLink) + if(maxGraphErrors.linearLink.isValid()) { - 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(_optimizationMaxError > 0.0f && maxLinearErrorRatio > _optimizationMaxError) + maxGraphErrorsLinearIds = std::make_pair(maxGraphErrors.linearLink.from(), maxGraphErrors.linearLink.to()); + UINFO("Max optimization linear error = %f m (link %d->%d, var=%f, ratio error/std=%f)", maxGraphErrors.linear, maxGraphErrors.linearLink.from(), maxGraphErrors.linearLink.to(), maxGraphErrors.linearLink.transVariance(), maxGraphErrors.linear/sqrt(maxGraphErrors.linearLink.transVariance())); + if(_optimizationMaxError > 0.0f && maxGraphErrors.linearRatio > _optimizationMaxError) { - UWARN("Rejecting all added loop closures (%d, first is %d <-> %d) in this " - "iteration because a wrong loop closure has been " - "detected after graph optimization, resulting in " - "a maximum graph error ratio of %f (edge %d->%d, type=%d, abs error=%f m, stddev=%f). The " - "maximum error ratio parameter \"%s\" is %f of std deviation.", - (int)loopClosureLinksAdded.size(), - loopClosureLinksAdded.front().first, - loopClosureLinksAdded.front().second, - maxLinearErrorRatio, - maxLinearLink->from(), - maxLinearLink->to(), - maxLinearLink->type(), - maxLinearError, - sqrt(maxLinearLink->transVariance()), - Parameters::kRGBDOptimizeMaxError().c_str(), - _optimizationMaxError); - reject = true; + if( _optimizationMaxErrorRepairRadius > 0.0 && + maxGraphErrorsLinearIds == _lastRejectedLoopClosureIds && + graph::findLink(constraints, maxGraphErrorsLinearIds.first, maxGraphErrorsLinearIds.second) != constraints.end()) + { + UWARN("We detected 2 consecutive loop closure rejections because of the same loop closure link (%d->%d), trying optimization again without that link...", + maxGraphErrorsLinearIds.first, maxGraphErrorsLinearIds.second); + + std::list > removedLinks = repairGraph( + maxGraphErrors, + poses, + constraints, + optimizationError, + optimizationIterations, + covariance); + + if(removedLinks.empty()) + { + UWARN("Optimization failed when trying to repair the graph."); + reject = true; + } + else + { + for(auto link: removedLinks) + { + UWARN("Removing link %d->%d from memory", link.first, link.second); + _memory->removeLink(link.first, link.second); + } + UWARN("Successfully repaired the graph."); + maxGraphErrorsRemovedIds = removedLinks.front(); + maxGraphErrorsRemovedCount = removedLinks.size(); + } + } + else { + reject = true; + } + + if(reject) + { + UWARN("Rejecting all added loop closures (%d, first is %d <-> %d) in this " + "iteration because a wrong loop closure has been " + "detected after graph optimization, resulting in " + "a maximum graph error ratio of %f (edge %d->%d, type=%d, abs error=%f m, stddev=%f). The " + "maximum error ratio parameter \"%s\" is %f of std deviation.", + (int)loopClosureLinksAdded.size(), + loopClosureLinksAdded.front().first, + loopClosureLinksAdded.front().second, + maxGraphErrors.linearRatio, + maxGraphErrors.linearLink.from(), + maxGraphErrors.linearLink.to(), + maxGraphErrors.linearLink.type(), + maxGraphErrors.linear, + sqrt(maxGraphErrors.linearLink.transVariance()), + Parameters::kRGBDOptimizeMaxError().c_str(), + _optimizationMaxError); + + if(maxGraphErrors.linearLink.type() != Link::kNeighbor) + { + _lastRejectedLoopClosureIds = maxGraphErrorsLinearIds; + } + } } - else if(_optimizationMaxError == 0.0f && maxLinearErrorRatio>100 && !_graphOptimizer->isRobust()) + else if(_optimizationMaxError == 0.0f && maxGraphErrors.linearRatio>100 && !_graphOptimizer->isRobust()) { 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()), + maxGraphErrors.linearRatio, + maxGraphErrors.linearLink.from(), + maxGraphErrors.linearLink.to(), + maxGraphErrors.linearLink.type(), + maxGraphErrors.linear, + sqrt(maxGraphErrors.linearLink.transVariance()), Parameters::kRGBDOptimizeMaxError().c_str()); } } - if(maxAngularLink) + + if(maxGraphErrors.angularLink.isValid()) { - 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(_optimizationMaxError > 0.0f && maxAngularErrorRatio > _optimizationMaxError) + maxGraphErrorsAngularIds = std::make_pair(maxGraphErrors.angularLink.from(), maxGraphErrors.angularLink.to()); + UINFO("Max optimization angular error = %f deg (link %d->%d, var=%f, ratio error/std=%f)", maxGraphErrors.angular*180.0f/CV_PI, maxGraphErrors.angularLink.from(), maxGraphErrors.angularLink.to(), maxGraphErrors.angularLink.rotVariance(), maxGraphErrors.angular/sqrt(maxGraphErrors.angularLink.rotVariance())); + if(_optimizationMaxError > 0.0f && maxGraphErrors.angularRatio > _optimizationMaxError) { UWARN("Rejecting all added loop closures (%d, first is %d <-> %d) in this " "iteration because a wrong loop closure has been " @@ -3890,27 +4038,27 @@ bool Rtabmap::process( (int)loopClosureLinksAdded.size(), loopClosureLinksAdded.front().first, loopClosureLinksAdded.front().second, - maxAngularErrorRatio, - maxAngularLink->from(), - maxAngularLink->to(), - maxAngularLink->type(), - maxAngularError*180.0f/CV_PI, - sqrt(maxAngularLink->rotVariance()), + maxGraphErrors.angularRatio, + maxGraphErrors.angularLink.from(), + maxGraphErrors.angularLink.to(), + maxGraphErrors.angularLink.type(), + maxGraphErrors.angular*180.0f/CV_PI, + sqrt(maxGraphErrors.angularLink.rotVariance()), Parameters::kRGBDOptimizeMaxError().c_str(), _optimizationMaxError); reject = true; } - else if(_optimizationMaxError == 0.0f && maxAngularErrorRatio>100 && !_graphOptimizer->isRobust()) + else if(_optimizationMaxError == 0.0f && maxGraphErrors.angularRatio>100 && !_graphOptimizer->isRobust()) { 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()), + maxGraphErrors.angularRatio, + maxGraphErrors.angularLink.from(), + maxGraphErrors.angularLink.to(), + maxGraphErrors.angularLink.type(), + maxGraphErrors.angular*180.0f/CV_PI, + sqrt(maxGraphErrors.angularLink.rotVariance()), Parameters::kRGBDOptimizeMaxError().c_str()); } } @@ -4055,13 +4203,28 @@ bool Rtabmap::process( statistics_.addStatistic(Statistics::kLoopVisual_inliers(), loopClosureVisualInliers); statistics_.addStatistic(Statistics::kLoopVisual_inliers_ratio(), loopClosureVisualInliersRatio); statistics_.addStatistic(Statistics::kLoopVisual_matches(), loopClosureVisualMatches); + statistics_.addStatistic(Statistics::kLoopVisual_variance(), loopClosureVisualVariance); statistics_.addStatistic(Statistics::kLoopLinear_variance(), loopClosureLinearVariance); statistics_.addStatistic(Statistics::kLoopAngular_variance(), loopClosureAngularVariance); statistics_.addStatistic(Statistics::kLoopLast_id(), _memory->getLastGlobalLoopClosureId()); - statistics_.addStatistic(Statistics::kLoopOptimization_max_error(), maxLinearError); - statistics_.addStatistic(Statistics::kLoopOptimization_max_error_ratio(), maxLinearErrorRatio); - statistics_.addStatistic(Statistics::kLoopOptimization_max_ang_error(), maxAngularError*180.0f/M_PI); - statistics_.addStatistic(Statistics::kLoopOptimization_max_ang_error_ratio(), maxAngularErrorRatio); + if(maxGraphErrors.linear>=0 || maxGraphErrors.angular>=0) + { + statistics_.addStatistic(Statistics::kLoopOptimization_max_error(), maxGraphErrors.linear); + statistics_.addStatistic(Statistics::kLoopOptimization_max_error_ratio(), maxGraphErrors.linearRatio); + statistics_.addStatistic(Statistics::kLoopOptimization_max_ang_error(), maxGraphErrors.angular*180.0f/M_PI); + statistics_.addStatistic(Statistics::kLoopOptimization_max_ang_error_ratio(), maxGraphErrors.angularRatio); + statistics_.addStatistic(Statistics::kLoopOptimization_max_error_from_id(), maxGraphErrorsLinearIds.first); + statistics_.addStatistic(Statistics::kLoopOptimization_max_error_to_id(), maxGraphErrorsLinearIds.second); + statistics_.addStatistic(Statistics::kLoopOptimization_max_ang_error_from_id(), maxGraphErrorsAngularIds.first); + statistics_.addStatistic(Statistics::kLoopOptimization_max_ang_error_to_id(), maxGraphErrorsAngularIds.second); + + if(_optimizationMaxErrorRepairRadius > 0) + { + statistics_.addStatistic(Statistics::kLoopOptimization_max_error_removed_from_id(), maxGraphErrorsRemovedIds.first); + statistics_.addStatistic(Statistics::kLoopOptimization_max_error_removed_to_id(), maxGraphErrorsRemovedIds.second); + statistics_.addStatistic(Statistics::kLoopOptimization_max_error_removed_count(), maxGraphErrorsRemovedCount); + } + } statistics_.addStatistic(Statistics::kLoopOptimization_error(), optimizationError); statistics_.addStatistic(Statistics::kLoopOptimization_iterations(), optimizationIterations); statistics_.addStatistic(Statistics::kLoopLandmark_detected(), landmarksDetected.empty()?0:-landmarksDetected.begin()->first); @@ -4255,12 +4418,12 @@ bool Rtabmap::process( } if(!_publishLastSignatureData) { - lastSignatureData.sensorData().clearCompressedData(); + lastSignatureData.sensorData().clearCompressedData(true, true, true, false); lastSignatureData.sensorData().clearRawData(); } if(!_rawDataKept) { - _memory->removeRawData(signature->id(), true, !_neighborLinkRefining && !_proximityBySpace, true); + _memory->removeRawData(signature->id(), true, !_neighborLinkRefining && !_proximityBySpace, true, false); } // Localization mode and saving localization data: save odometry covariance in a prior link @@ -4269,6 +4432,7 @@ bool Rtabmap::process( { _memory->addLink(Link(signature->id(), signature->id(), Link::kPosePrior, odomPose, odomCovariance.inv())); } + bool lastSignatureWasIntermediateNode = signature->getWeight() == -1; // remove last signature if the memory is not incremental or is a bad signature (if bad signatures are ignored) int signatureRemoved = _memory->cleanup(); @@ -4303,7 +4467,8 @@ bool Rtabmap::process( signaturesRemoved.push_back(signature->id()); _memory->deleteLocation(signature->id()); } - else if((smallDisplacement || tooFastMovement) && + else if((!_memory->isIncremental() || signature->getWeight()>=0) && + (smallDisplacement || tooFastMovement) && _loopClosureHypothesis.first == 0 && lastProximitySpaceClosureId == 0 && (rejectedLoopClosure || landmarksDetected.empty()) && @@ -4315,6 +4480,20 @@ bool Rtabmap::process( // If there is a too small displacement, remove the node signaturesRemoved.push_back(signature->id()); _memory->deleteLocation(signature->id()); + + // Update odom cache (if we just switched from mapping mode to localization mode) + _odomCachePoses.erase(signature->id()); + for(std::multimap::iterator iter=_odomCacheConstraints.begin(); iter!=_odomCacheConstraints.end();) + { + if(iter->second.from() == signature->id() || iter->second.to() == signature->id()) + { + _odomCacheConstraints.erase(iter++); + } + else + { + ++iter; + } + } } else { @@ -4325,8 +4504,7 @@ bool Rtabmap::process( (smallDisplacement || tooFastMovement) && _loopClosureHypothesis.first == 0 && lastProximitySpaceClosureId == 0 && - !delayedLocalization && - (rejectedLoopClosure || landmarksDetected.empty())) + !delayedLocalization) { _odomCachePoses.erase(signatureRemoved); for(std::multimap::iterator iter=_odomCacheConstraints.begin(); iter!=_odomCacheConstraints.end();) @@ -4360,10 +4538,20 @@ bool Rtabmap::process( //============================================================ double totalTime = timerTotal.ticks(); ULOGGER_INFO("Total time processing = %fs...", totalTime); - if((_maxTimeAllowed != 0 && totalTime*1000>_maxTimeAllowed) || - (_maxMemoryAllowed != 0 && _memory->getWorkingMem().size() > _maxMemoryAllowed)) + if(!lastSignatureWasIntermediateNode && // skip memory management on intermediate nodes + ((_maxTimeAllowed != 0 && totalTime*1000>_maxTimeAllowed) || + (_maxMemoryAllowed != 0 && _memory->getWorkingMem().size() > _maxMemoryAllowed))) { - ULOGGER_INFO("Removing old signatures because time limit is reached %f>%f or memory is reached %d>%d...", totalTime*1000, _maxTimeAllowed, _memory->getWorkingMem().size(), _maxMemoryAllowed); + if(_maxTimeAllowed!=0 && totalTime*1000>_maxTimeAllowed) + { + ULOGGER_INFO("Removing old signatures because time limit is reached %f ms > %f ms...", + totalTime*1000, _maxTimeAllowed); + } + if(_maxMemoryAllowed != 0 && _memory->getWorkingMem().size() > _maxMemoryAllowed) + { + ULOGGER_INFO("Removing old signatures because memory limit is reached %d > %d...", + _memory->getWorkingMem().size(), _maxMemoryAllowed); + } immunizedLocations.insert(_lastLocalizationNodeId); // keep the latest localization in working memory std::list transferred = _memory->forget(immunizedLocations); signaturesRemoved.insert(signaturesRemoved.end(), transferred.begin(), transferred.end()); @@ -4411,9 +4599,9 @@ bool Rtabmap::process( } else if(_memory->isIncremental() && _optimizedPoses.size() && - _memory->getLastWorkingSignature()) + _memory->getLastWorkingSignature(true)) { - id = _memory->getLastWorkingSignature()->id(); + id = _memory->getLastWorkingSignature(true)->id(); UDEBUG("Refresh local map from %d", id); } UDEBUG("id=%d _optimizedPoses=%d", id, (int)_optimizedPoses.size()); @@ -4833,9 +5021,14 @@ void Rtabmap::setWorkingDirectory(std::string path) void Rtabmap::rejectLastLoopClosure() { - if(_memory && _memory->getStMem().find(getLastLocationId())!=_memory->getStMem().end()) + if(!_memory) { - std::multimap links = _memory->getLinks(getLastLocationId(), false); + return; + } + const Signature * lastS = _memory->getLastWorkingSignature(true); // last non-intermediate + if(lastS && _memory->getStMem().find(lastS->id())!=_memory->getStMem().end()) + { + std::multimap links = _memory->getLinks(lastS->id(), false); bool linksRemoved = false; for(std::multimap::iterator iter = links.begin(); iter!=links.end(); ++iter) { @@ -4871,7 +5064,7 @@ void Rtabmap::rejectLastLoopClosure() std::map poses = _optimizedPoses; std::multimap constraints; cv::Mat covariance; - optimizeCurrentMap(getLastLocationId(), false, poses, covariance, &constraints); + optimizeCurrentMap(lastS->id(), false, poses, covariance, &constraints); if(poses.empty()) { @@ -4882,7 +5075,7 @@ void Rtabmap::rejectLastLoopClosure() UINFO("Updated local map (old size=%d, new size=%d)", (int)_optimizedPoses.size(), (int)poses.size()); _optimizedPoses = poses; _constraints = constraints; - _mapCorrection = _optimizedPoses.at(_memory->getLastWorkingSignature()->id()) * _memory->getLastWorkingSignature()->getPose().inverse(); + _mapCorrection = _optimizedPoses.at(lastS->id()) * lastS->getPose().inverse(); } } } @@ -4894,6 +5087,13 @@ void Rtabmap::deleteLastLocation() if(_memory && _memory->getStMem().size()) { int lastId = *_memory->getStMem().rbegin(); + const Signature * s = _memory->getSignature(lastId); + UASSERT(s); + if(s->getWeight() == -1) + { + UERROR("Deleting last location with inermediate nodes is not supported. Aborting."); + return; + } _memory->deleteLocation(lastId); // we have to re-optimize the graph without the deleted location if(_memory->isIncremental() && _optimizedPoses.size()) @@ -4922,7 +5122,7 @@ void Rtabmap::deleteLastLocation() { std::multimap constraints; cv::Mat covariance; - optimizeCurrentMap(_memory->getLastWorkingSignature()->id(), false, poses, covariance, &constraints); + optimizeCurrentMap(_memory->getLastWorkingSignature(true)->id(), false, poses, covariance, &constraints); if(poses.empty()) { @@ -4932,7 +5132,7 @@ void Rtabmap::deleteLastLocation() { _optimizedPoses = poses; _constraints = constraints; - _mapCorrection = _optimizedPoses.at(_memory->getLastWorkingSignature()->id()) * _memory->getLastWorkingSignature()->getPose().inverse(); + _mapCorrection = _optimizedPoses.at(_memory->getLastWorkingSignature(true)->id()) * _memory->getLastWorkingSignature(true)->getPose().inverse(); } } } @@ -5174,11 +5374,43 @@ void Rtabmap::optimizeCurrentMap( UINFO("Optimize map: around location %d (lookInDatabase=%s)", id, lookInDatabase?"true":"false"); if(_memory && id > 0) { + if(!lookInDatabase && (!_memory->getSignature(id) || _memory->getSignature(id)->getWeight() == -1)) + { + UERROR("When doing a local optimization, the root id (%d) must exist and not be an intermediate node! Aborting...", id); + optimizedPoses.clear(); + if(constraints) + { + constraints->clear(); + } + return; + } + UTimer timer; std::map ids = _memory->getNeighborsId(id, 0, lookInDatabase?-1:0, true, false); if(!_optimizeFromGraphEnd && ids.size() > 1) { - id = ids.begin()->first; + if(lookInDatabase) + { + id = ids.begin()->first; + } + else + { + // Find first node that is not intermediate + for(auto pair: ids) + { + // Make sure fromId is not an intermediate node + const Signature * s = _memory->getSignature(pair.first); + if(s && s->getWeight() != -1) + { + id = pair.first; + break; + } + else if(!s) + { + UWARN("Not found node %d in memory?!", pair.first); + } + } + } } UINFO("get %d ids time %f s", (int)ids.size(), timer.ticks()); @@ -5300,6 +5532,141 @@ std::map Rtabmap::optimizeGraph( return optimizedPoses; } +// If repairing works, all input arguments are updated accordingly to new graph +// Returns IDs of the links removed from constraints +std::list > Rtabmap::repairGraph( + graph::MaxGraphErrors & maxGraphErrors, + std::map & poses, + std::multimap & constraints, + double & optimizationError, + int & optimizationIterations, + cv::Mat & optimizationCovariance) +{ + UASSERT(maxGraphErrors.linearLink.isValid()); + UASSERT(graph::findLink(constraints, maxGraphErrors.linearLink.from(), maxGraphErrors.linearLink.to()) != constraints.end()); + + int originalMaxErrorLinkFrom = maxGraphErrors.linearLink.from(); + int originalMaxErrorLinkTo = maxGraphErrors.linearLink.to(); + + graph::MaxGraphErrors subMaxGraphErrors = maxGraphErrors; + std::list > removedLinks; + std::map subPoses = poses; + std::multimap subConstraints = constraints; + while(subMaxGraphErrors.linearLink.isValid() && subMaxGraphErrors.linearRatio > _optimizationMaxError) + { + removedLinks.push_back(std::make_pair(subMaxGraphErrors.linearLink.from(), subMaxGraphErrors.linearLink.to())); + subConstraints.erase(graph::findLink(subConstraints, subMaxGraphErrors.linearLink.from(), subMaxGraphErrors.linearLink.to())); + subMaxGraphErrors.linearLink = Link(); + + // Get connected graph just to check if a node got disconnected (localization mode) + std::map posesOut; + std::multimap subConstraintsOut; + _graphOptimizer->getConnectedGraph(subPoses.rbegin()->first, subPoses, subConstraints, posesOut, subConstraintsOut); + if(subPoses.size() - posesOut.size() > 1) + { + UWARN("More than one pose filtered in one iteration when trying to repair graph. Aborting repairing."); + break; + } + else if(subPoses.size() != posesOut.size()) + { + for(std::map::iterator pter=subPoses.begin(); pter!=subPoses.end(); ++pter) + { + if(posesOut.find(pter->first) == posesOut.end()) + { + subPoses.erase(pter); + break; // should be only one if different, break now + } + } + } + cv::Mat subOptimizationCovariance; + double subOptimizationError = 0.0; + int subOptimizationIterations = 0; + + int fromId = subPoses.rbegin()->first; + if(!_optimizeFromGraphEnd) + { + // Find first node that is not intermediate + for(std::map::iterator iter = subPoses.lower_bound(1); iter!=subPoses.end(); ++iter) + { + // Make sure fromId is not an intermediate node + const Signature * s = _memory->getSignature(iter->first); + if(s && s->getWeight() != -1) + { + fromId = iter->first; + break; + } + else if(!s) + { + UWARN("Not found node %d in memory?!", iter->first); + } + } + } + + subPoses = _graphOptimizer->optimize( + fromId, + subPoses, + subConstraintsOut, + subOptimizationCovariance, + 0, + &subOptimizationError, + &subOptimizationIterations); + + if(subPoses.empty()) + { + UWARN("Optimization failed when trying to repair graph."); + } + else + { + subMaxGraphErrors = graph::computeMaxGraphErrors( + subPoses, + subConstraintsOut); + if(!subMaxGraphErrors.linearLink.isValid()) + { + UWARN("Could not compute graph errors! Wrong loop closures could be accepted!"); + } + else if(subMaxGraphErrors.linearRatio > _optimizationMaxError) + { + float distance = poses.at(originalMaxErrorLinkFrom).getDistance(poses.at(subMaxGraphErrors.linearLink.from())); + if(subMaxGraphErrors.linearLink.type() != Link::kNeighbor && distance < _optimizationMaxErrorRepairRadius) + { + UWARN("Optimization error is still high (%f, on link %d->%d type=%d) after removing the loop closure with the highest error. " + "As it is close (%f m < %s=%f m) to original loop closure with high error (%d->%d), we will reject again this one to see if it helps.", + subMaxGraphErrors.linearRatio, subMaxGraphErrors.linearLink.from(), subMaxGraphErrors.linearLink.to(), subMaxGraphErrors.linearLink.type(), + distance, Parameters::kRGBDOptimizeMaxErrorRepairRadius().c_str(), _optimizationMaxErrorRepairRadius, + originalMaxErrorLinkFrom, originalMaxErrorLinkTo); + } + else + { + UWARN("Optimization error is still high (%f, on link %d->%d) after removing loop closure with highest error.", + subMaxGraphErrors.linearRatio, subMaxGraphErrors.linearLink.from(), subMaxGraphErrors.linearLink.to()); + subMaxGraphErrors.linearLink = Link(); + } + } + else + { + UWARN("Optimization error is lower (%f, on link %d->%d) after removing loop " + "closure with highest error. We will remove the old link (%d->%d collaterals=%ld) and accept the new one.", + subMaxGraphErrors.linearRatio, subMaxGraphErrors.linearLink.from(), subMaxGraphErrors.linearLink.to(), + removedLinks.front().first, removedLinks.front().second, + removedLinks.size()-1); + + maxGraphErrors = subMaxGraphErrors; + + // Make sure the link pointers in maxGraphErrors point on same constraints + std::swap(poses, subPoses); + std::swap(constraints, subConstraintsOut); + + optimizationCovariance = subOptimizationCovariance; + optimizationError = subOptimizationError; + optimizationIterations = subOptimizationIterations; + + return removedLinks; + } + } + } + return std::list >(); +} + void Rtabmap::adjustLikelihood(std::map & likelihood) const { ULOGGER_DEBUG("likelihood.size()=%d", likelihood.size()); @@ -5532,7 +5899,7 @@ void Rtabmap::getGraph( bool withWords, bool withGlobalDescriptors) const { - if(_memory && _memory->getLastWorkingSignature()) + if(_memory && _memory->getLastWorkingSignature(!global)) { if(_rgbdSlamMode) { @@ -5540,7 +5907,7 @@ void Rtabmap::getGraph( { poses = _optimizedPoses; // guess cv::Mat covariance; - this->optimizeCurrentMap(_memory->getLastWorkingSignature()->id(), global, poses, covariance, &constraints); + this->optimizeCurrentMap(_memory->getLastWorkingSignature(!global)->id(), global, poses, covariance, &constraints); if(!global && !_optimizedPoses.empty()) { // We send directly the already optimized poses if they are set @@ -5550,14 +5917,14 @@ void Rtabmap::getGraph( } else { - std::map ids = _memory->getNeighborsId(_memory->getLastWorkingSignature()->id(), 0, global?-1:0, true); + std::map ids = _memory->getNeighborsId(_memory->getLastWorkingSignature(!global)->id(), 0, global?-1:0, true); _memory->getMetricConstraints(uKeysSet(ids), poses, constraints, global); } } else { // no optimization on appearance-only mode - std::map ids = _memory->getNeighborsId(_memory->getLastWorkingSignature()->id(), 0, global?-1:0, true); + std::map ids = _memory->getNeighborsId(_memory->getLastWorkingSignature(!global)->id(), 0, global?-1:0, true); _memory->getMetricConstraints(uKeysSet(ids), poses, constraints, global); } @@ -5642,8 +6009,10 @@ int Rtabmap::detectMoreLoopClosures( bool intraSession, bool interSession, const ProgressState * processState, - float clusterRadiusMin) + float clusterRadiusMin, + int toFromMapId) { + UDEBUG(""); UASSERT(iterations>0); if(_graphOptimizer->iterations() <= 0) @@ -5668,17 +6037,23 @@ int Rtabmap::detectMoreLoopClosures( std::map posesToCheckLoopClosures; std::map poses; std::multimap links; - std::map signatures; // some signatures may be in LTM, get them all - this->getGraph(poses, links, true, true, &signatures); + this->getGraph(poses, links, true, true); std::map mapIds; UDEBUG("remove all invalid or intermediate nodes, fill mapIds"); for(std::map::iterator iter=poses.upper_bound(0); iter!=poses.end();++iter) { - if(signatures.at(iter->first).getWeight() >= 0) + Transform odom, gt; + int mapId, weight; + std::string l; + double s; + std::vector v; + GPS gps; + EnvSensors srs; + if(_memory->getNodeInfo(iter->first, odom, mapId, weight, l, s, gt, v, gps, srs, true) && weight >= 0) { posesToCheckLoopClosures.insert(*iter); - mapIds.insert(std::make_pair(iter->first, signatures.at(iter->first).mapId())); + mapIds.insert(std::make_pair(iter->first, mapId)); } } @@ -5692,7 +6067,56 @@ int Rtabmap::detectMoreLoopClosures( clusterRadiusMax, clusterAngle); - UINFO("Looking for more loop closures, clustering poses... found %d clusters.", (int)clusters.size()); + UINFO("Looking for more loop closures: clustering poses... found %ld clusters.", clusters.size()); + + if(toFromMapId >=0) + { + size_t clustersBefore = clusters.size(); + for(std::multimap::iterator iter=clusters.begin(); iter!=clusters.end();) + { + int mapId = uValue(mapIds, iter->first, 0); + if(mapId != toFromMapId) + { + iter = clusters.erase(iter); + } + else { + ++iter; + } + } + UINFO("Looking for more loop closures: filtered %ld/%ld clusters for map session %d.", clustersBefore-clusters.size(), clustersBefore, toFromMapId); + if(clusters.empty()) + { + UERROR("No clusters belong to mapId %d, aborting.", toFromMapId); + break; + } + } + + if(_memory->getMaxStMemSize() > 1) + { + size_t clustersBefore = clusters.size(); + for(std::multimap::iterator iter=clusters.begin(); iter!=clusters.end();) + { + if(abs(iter->first - iter->second) < _memory->getMaxStMemSize()) + { + iter = clusters.erase(iter); + } + else + { + // compute path to know how far we are in terms of graph length + std::map ids = _memory->getNeighborsId(iter->first, _memory->getMaxStMemSize(), -1, true, true, true); + if(ids.find(iter->second) != ids.end()) + { + iter = clusters.erase(iter); + } + else + { + ++iter; + } + } + } + UINFO("Looking for more loop closures: filtered %ld/%ld clusters for too close nodes (below %s=%d).", + clustersBefore-clusters.size(), clustersBefore, Parameters::kMemSTMSize().c_str(), _memory->getMaxStMemSize()); + } int i=0; std::set addedLinks; @@ -5744,8 +6168,10 @@ int Rtabmap::detectMoreLoopClosures( { checkedLoopClosures.insert(std::make_pair(from, to)); - UASSERT(signatures.find(from) != signatures.end()); - UASSERT(signatures.find(to) != signatures.end()); + Signature fromS = getSignatureCopy(from, false, true, false, false, true, false); + Signature toS = getSignatureCopy(to, false, true, false, false, true, false); + UASSERT(fromS.getWeight()>=0); + UASSERT(toS.getWeight()>=0); Transform guess; if(_proximityBySpace && uContains(poses, from) && uContains(poses, to)) @@ -5755,7 +6181,7 @@ int Rtabmap::detectMoreLoopClosures( RegistrationInfo info; // use signatures instead of IDs because some signatures may not be in WM - Transform t = _memory->computeTransform(signatures.at(from), signatures.at(to), guess, &info); + Transform t = _memory->computeTransform(fromS, toS, guess, &info); if(!t.isNull()) { @@ -5764,11 +6190,11 @@ int Rtabmap::detectMoreLoopClosures( //optimize the graph to see if the new constraint is globally valid int fromId = from; - int mapId = signatures.at(from).mapId(); + int mapId = fromS.mapId(); // use first node of the map containing from - for(std::map::iterator ster=signatures.begin(); ster!=signatures.end(); ++ster) + for(std::map::iterator ster=posesToCheckLoopClosures.begin(); ster!=posesToCheckLoopClosures.end(); ++ster) { - if(ster->second.mapId() == mapId) + if(uValue(mapIds, ster->first, 0) == mapId) { fromId = ster->first; break; @@ -5776,96 +6202,85 @@ int Rtabmap::detectMoreLoopClosures( } 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; + graph::MaxGraphErrors maxGraphErrors; std::map optimizedPoses; - std::multimap links; + std::multimap linksOut; 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); + _graphOptimizer->getConnectedGraph(fromId, poses, linksIn, optimizedPoses, linksOut); 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); + UASSERT_MSG(optimizedPoses.find(from) != optimizedPoses.end(), uFormat("id=%d poses=%d links=%d", from, (int)optimizedPoses.size(), (int)linksOut.size()).c_str()); + UASSERT_MSG(optimizedPoses.find(to) != optimizedPoses.end(), uFormat("id=%d poses=%d links=%d", to, (int)optimizedPoses.size(), (int)linksOut.size()).c_str()); + UASSERT(graph::findLink(linksOut, from, to) != linksOut.end()); + optimizedPoses = _graphOptimizer->optimize(fromId, optimizedPoses, linksOut); std::string msg; if(optimizedPoses.size()) { - graph::computeMaxGraphErrors( + maxGraphErrors = graph::computeMaxGraphErrors( optimizedPoses, - links, - maxLinearErrorRatio, - maxAngularErrorRatio, - maxLinearError, - maxAngularError, - &maxLinearLink, - &maxAngularLink); - if(maxLinearLink) + linksOut); + if(maxGraphErrors.linearLink.isValid()) { - 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)", maxGraphErrors.linear, maxGraphErrors.linearLink.from(), maxGraphErrors.linearLink.to()); + if(_optimizationMaxError > 0.0f && maxGraphErrors.linearRatio > _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()), + maxGraphErrors.linear, + maxGraphErrors.linearLink.from(), + maxGraphErrors.linearLink.to(), + maxGraphErrors.linearRatio, + sqrt(maxGraphErrors.linearLink.transVariance()), Parameters::kRGBDOptimizeMaxError().c_str(), _optimizationMaxError); } - else if(_optimizationMaxError == 0.0f && maxLinearErrorRatio>100 && !_graphOptimizer->isRobust()) + else if(_optimizationMaxError == 0.0f && maxGraphErrors.linearRatio>100 && !_graphOptimizer->isRobust()) { 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()), + maxGraphErrors.linearRatio, + maxGraphErrors.linearLink.from(), + maxGraphErrors.linearLink.to(), + maxGraphErrors.linearLink.type(), + maxGraphErrors.linear, + sqrt(maxGraphErrors.linearLink.transVariance()), Parameters::kRGBDOptimizeMaxError().c_str()); } } - else if(maxAngularLink) + else if(maxGraphErrors.angularLink.isValid()) { - 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) + UINFO("Max optimization angular error = %f deg (link %d->%d)", maxGraphErrors.angular*180.0f/M_PI, maxGraphErrors.angularLink.from(), maxGraphErrors.angularLink.to()); + if(_optimizationMaxError > 0.0f && maxGraphErrors.angularRatio > _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()), + maxGraphErrors.angular*180.0f/M_PI, + maxGraphErrors.angularLink.from(), + maxGraphErrors.angularLink.to(), + maxGraphErrors.angularRatio, + sqrt(maxGraphErrors.angularLink.rotVariance()), Parameters::kRGBDOptimizeMaxError().c_str(), _optimizationMaxError); } - else if(_optimizationMaxError == 0.0f && maxAngularErrorRatio>100 && !_graphOptimizer->isRobust()) + else if(_optimizationMaxError == 0.0f && maxGraphErrors.angularRatio>100 && !_graphOptimizer->isRobust()) { 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()), + maxGraphErrors.angularRatio, + maxGraphErrors.angularLink.from(), + maxGraphErrors.angularLink.to(), + maxGraphErrors.angularLink.type(), + maxGraphErrors.angular*180.0f/CV_PI, + sqrt(maxGraphErrors.angularLink.rotVariance()), Parameters::kRGBDOptimizeMaxError().c_str()); } } @@ -6146,7 +6561,7 @@ bool Rtabmap::addLink(const Link & link) std::map poses = _optimizedPoses; std::multimap links; cv::Mat covariance; - optimizeCurrentMap(this->getLastLocationId(), false, poses, covariance, &links); + optimizeCurrentMap(_memory->getLastWorkingSignature(true)->id(), false, poses, covariance, &links); if(poses.find(link.from()) == poses.end()) { @@ -6168,83 +6583,72 @@ bool Rtabmap::addLink(const Link & link) } else { - float maxLinearError = 0.0f; - float maxLinearErrorRatio = 0.0f; - float maxAngularError = 0.0f; - float maxAngularErrorRatio = 0.0f; - const Link * maxLinearLink = 0; - const Link * maxAngularLink = 0; + graph::MaxGraphErrors maxGraphErrors; - graph::computeMaxGraphErrors( + maxGraphErrors = graph::computeMaxGraphErrors( poses, - links, - maxLinearErrorRatio, - maxAngularErrorRatio, - maxLinearError, - maxAngularError, - &maxLinearLink, - &maxAngularLink); - if(maxLinearLink) + links); + if(maxGraphErrors.linearLink.isValid()) { - 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)", maxGraphErrors.linear, maxGraphErrors.linearLink.from(), maxGraphErrors.linearLink.to()); + if(_optimizationMaxError > 0.0f && maxGraphErrors.linearRatio > _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.", link.from(), link.to(), - maxLinearError, - maxLinearLink->from(), - maxLinearLink->to(), - maxLinearErrorRatio, - sqrt(maxLinearLink->transVariance()), + maxGraphErrors.linear, + maxGraphErrors.linearLink.from(), + maxGraphErrors.linearLink.to(), + maxGraphErrors.linearRatio, + sqrt(maxGraphErrors.linearLink.transVariance()), Parameters::kRGBDOptimizeMaxError().c_str(), _optimizationMaxError); } - else if(_optimizationMaxError == 0.0f && maxLinearErrorRatio>100 && !_graphOptimizer->isRobust()) + else if(_optimizationMaxError == 0.0f && maxGraphErrors.linearRatio>100 && !_graphOptimizer->isRobust()) { 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()), + maxGraphErrors.linearRatio, + maxGraphErrors.linearLink.from(), + maxGraphErrors.linearLink.to(), + maxGraphErrors.linearLink.type(), + maxGraphErrors.linear, + sqrt(maxGraphErrors.linearLink.transVariance()), Parameters::kRGBDOptimizeMaxError().c_str()); } } - else if(maxAngularLink) + else if(maxGraphErrors.angularLink.isValid()) { - 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) + UINFO("Max optimization angular error = %f deg (link %d->%d)", maxGraphErrors.angular*180.0f/M_PI, maxGraphErrors.angularLink.from(), maxGraphErrors.angularLink.to()); + if(_optimizationMaxError > 0.0f && maxGraphErrors.angularRatio > _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.", link.from(), link.to(), - maxAngularError*180.0f/M_PI, - maxAngularLink->from(), - maxAngularLink->to(), - maxAngularErrorRatio, - sqrt(maxAngularLink->rotVariance()), + maxGraphErrors.angular*180.0f/M_PI, + maxGraphErrors.angularLink.from(), + maxGraphErrors.angularLink.to(), + maxGraphErrors.angularRatio, + sqrt(maxGraphErrors.angularLink.rotVariance()), Parameters::kRGBDOptimizeMaxError().c_str(), _optimizationMaxError); } - else if(_optimizationMaxError == 0.0f && maxAngularErrorRatio>100 && !_graphOptimizer->isRobust()) + else if(_optimizationMaxError == 0.0f && maxGraphErrors.angularRatio>100 && !_graphOptimizer->isRobust()) { 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()), + maxGraphErrors.angularRatio, + maxGraphErrors.angularLink.from(), + maxGraphErrors.angularLink.to(), + maxGraphErrors.angularLink.type(), + maxGraphErrors.angular*180.0f/CV_PI, + sqrt(maxGraphErrors.angularLink.rotVariance()), Parameters::kRGBDOptimizeMaxError().c_str()); } } @@ -6326,6 +6730,7 @@ bool Rtabmap::addLink(const Link & link) std::map poses = _odomCachePoses; std::multimap constraints = _odomCacheConstraints; constraints.insert(std::make_pair(link.from(), link)); + 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()); @@ -6333,7 +6738,7 @@ bool Rtabmap::addLink(const Link & link) { 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)*999999))); + constraints.insert(std::make_pair(iterPose->first, Link(iterPose->first, iterPose->first, Link::kPosePrior, iterPose->second, priorInfMat))); } } @@ -6354,31 +6759,19 @@ bool Rtabmap::addLink(const Link & link) else { UINFO("Compute max graph errors..."); - float maxLinearError = 0.0f; - float maxLinearErrorRatio = 0.0f; - float maxAngularError = 0.0f; - float maxAngularErrorRatio = 0.0f; - const Link * maxLinearLink = 0; - const Link * maxAngularLink = 0; - graph::computeMaxGraphErrors( + graph::MaxGraphErrors maxGraphErrors = graph::computeMaxGraphErrors( optPoses, edgeConstraintsOut, - maxLinearErrorRatio, - maxAngularErrorRatio, - maxLinearError, - maxAngularError, - &maxLinearLink, - &maxAngularLink, _graphOptimizer->isSlam2d()); - if(maxLinearLink == 0 && maxAngularLink==0) + if(!maxGraphErrors.linearLink.isValid() && !maxGraphErrors.angularLink.isValid()) { UWARN("Could not compute graph errors! Wrong loop closures could be accepted!"); } - if(maxLinearLink) + if(maxGraphErrors.linearLink.isValid()) { - 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(_optimizationMaxError > 0.0f && maxLinearErrorRatio > _optimizationMaxError) + UINFO("Max optimization linear error = %f m (link %d->%d, var=%f, ratio error/std=%f)", maxGraphErrors.linear, maxGraphErrors.linearLink.from(), maxGraphErrors.linearLink.to(), maxGraphErrors.linearLink.transVariance(), maxGraphErrors.linear/sqrt(maxGraphErrors.linearLink.transVariance())); + if(_optimizationMaxError > 0.0f && maxGraphErrors.linearRatio > _optimizationMaxError) { UWARN("Rejecting localization (%d <-> %d) in this " "iteration because a wrong loop closure has been " @@ -6387,34 +6780,34 @@ bool Rtabmap::addLink(const Link & link) "maximum error ratio parameter \"%s\" is %f of std deviation.", link.from(), link.to(), - maxLinearErrorRatio, - maxLinearLink->from(), - maxLinearLink->to(), - maxLinearLink->type(), - maxLinearError, - sqrt(maxLinearLink->transVariance()), + maxGraphErrors.linearRatio, + maxGraphErrors.linearLink.from(), + maxGraphErrors.linearLink.to(), + maxGraphErrors.linearLink.type(), + maxGraphErrors.linear, + sqrt(maxGraphErrors.linearLink.transVariance()), Parameters::kRGBDOptimizeMaxError().c_str(), _optimizationMaxError); rejectLocalization = true; } - else if(_optimizationMaxError == 0.0f && maxLinearErrorRatio>100 && !_graphOptimizer->isRobust()) + else if(_optimizationMaxError == 0.0f && maxGraphErrors.linearRatio>100 && !_graphOptimizer->isRobust()) { 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()), + maxGraphErrors.linearRatio, + maxGraphErrors.linearLink.from(), + maxGraphErrors.linearLink.to(), + maxGraphErrors.linearLink.type(), + maxGraphErrors.linear, + sqrt(maxGraphErrors.linearLink.transVariance()), Parameters::kRGBDOptimizeMaxError().c_str()); } } - if(maxAngularLink) + if(maxGraphErrors.angularLink.isValid()) { - 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(_optimizationMaxError > 0.0f && maxAngularErrorRatio > _optimizationMaxError) + UINFO("Max optimization angular error = %f deg (link %d->%d, var=%f, ratio error/std=%f)", maxGraphErrors.angular*180.0f/CV_PI, maxGraphErrors.angularLink.from(), maxGraphErrors.angularLink.to(), maxGraphErrors.angularLink.rotVariance(), maxGraphErrors.angular/sqrt(maxGraphErrors.angularLink.rotVariance())); + if(_optimizationMaxError > 0.0f && maxGraphErrors.angularRatio > _optimizationMaxError) { UWARN("Rejecting localization (%d <-> %d) in this " "iteration because a wrong loop closure has been " @@ -6423,27 +6816,27 @@ bool Rtabmap::addLink(const Link & link) "maximum error ratio parameter \"%s\" is %f of std deviation.", link.from(), link.to(), - maxAngularErrorRatio, - maxAngularLink->from(), - maxAngularLink->to(), - maxAngularLink->type(), - maxAngularError*180.0f/CV_PI, - sqrt(maxAngularLink->rotVariance()), + maxGraphErrors.angularRatio, + maxGraphErrors.angularLink.from(), + maxGraphErrors.angularLink.to(), + maxGraphErrors.angularLink.type(), + maxGraphErrors.angular*180.0f/CV_PI, + sqrt(maxGraphErrors.angularLink.rotVariance()), Parameters::kRGBDOptimizeMaxError().c_str(), _optimizationMaxError); rejectLocalization = true; } - else if(_optimizationMaxError == 0.0f && maxAngularErrorRatio>100 && !_graphOptimizer->isRobust()) + else if(_optimizationMaxError == 0.0f && maxGraphErrors.angularRatio>100 && !_graphOptimizer->isRobust()) { 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()), + maxGraphErrors.angularRatio, + maxGraphErrors.angularLink.from(), + maxGraphErrors.angularLink.to(), + maxGraphErrors.angularLink.type(), + maxGraphErrors.angular*180.0f/CV_PI, + sqrt(maxGraphErrors.angularLink.rotVariance()), Parameters::kRGBDOptimizeMaxError().c_str()); } } @@ -6567,12 +6960,12 @@ bool Rtabmap::computePath(int targetNode, bool global) int currentNode = 0; if(_memory->isIncremental()) { - if(!_memory->getLastWorkingSignature()) + if(!_memory->getLastWorkingSignature(true)) { UWARN("Working memory is empty... cannot compute a path"); return false; } - currentNode = _memory->getLastWorkingSignature()->id(); + currentNode = _memory->getLastWorkingSignature(true)->id(); } else { @@ -6724,12 +7117,12 @@ bool Rtabmap::computePath(const Transform & targetPose, float tolerance) int currentNode = 0; if(_memory->isIncremental()) { - if(!_memory->getLastWorkingSignature()) + if(!_memory->getLastWorkingSignature(true)) { UWARN("Working memory is empty... cannot compute a path"); return false; } - currentNode = _memory->getLastWorkingSignature()->id(); + currentNode = _memory->getLastWorkingSignature(true)->id(); } else { @@ -6883,6 +7276,7 @@ void Rtabmap::updateGoalIndex() if( _memory && _path.size()) { // remove all previous virtual links + bool hasIntermediateNodes = false; for(unsigned int i=0; i<_pathCurrentIndex && i<_path.size(); ++i) { const Signature * s = _memory->getSignature(_path[i].first); @@ -6890,6 +7284,10 @@ void Rtabmap::updateGoalIndex() { _memory->removeVirtualLinks(s->id()); } + if(s->getWeight() == -1) + { + hasIntermediateNodes = true; + } } // for the current index, only keep the newest virtual link @@ -6918,7 +7316,7 @@ void Rtabmap::updateGoalIndex() // Make sure the next signatures on the path are linked together float distanceSoFar = 0.0f; for(unsigned int i=_pathCurrentIndex+1; - i<_path.size(); + i<_path.size() && !hasIntermediateNodes; ++i) { if(i>0) @@ -6927,41 +7325,53 @@ void Rtabmap::updateGoalIndex() { distanceSoFar += _path[i-1].second.getDistance(_path[i].second); } - if(distanceSoFar <= _localRadius) + + if(_path[i].first != _path[i-1].first) { - if(_path[i].first != _path[i-1].first) + const Signature * s = _memory->getSignature(_path[i].first); + if(s) { - const Signature * s = _memory->getSignature(_path[i].first); - if(s) + if(s->getWeight() == -1) { - if(!s->hasLink(_path[i-1].first) && _memory->getSignature(_path[i-1].first) != 0) - { - Transform virtualLoop = _path[i].second.inverse() * _path[i-1].second; - _memory->addLink(Link(_path[i].first, _path[i-1].first, Link::kVirtualClosure, virtualLoop, cv::Mat::eye(6,6,CV_64FC1)*0.01)); // on the optimized path - UINFO("Added Virtual link between %d and %d", _path[i-1].first, _path[i].first); - } + hasIntermediateNodes = true; + break; + } + if(!s->hasLink(_path[i-1].first) && _memory->getSignature(_path[i-1].first) != 0) + { + Transform virtualLoop = _path[i].second.inverse() * _path[i-1].second; + _memory->addLink(Link(_path[i].first, _path[i-1].first, Link::kVirtualClosure, virtualLoop, cv::Mat::eye(6,6,CV_64FC1)*0.01)); // on the optimized path + UINFO("Added Virtual link between %d and %d", _path[i-1].first, _path[i].first); } } } - else + + if(distanceSoFar > _localRadius) { + UDEBUG("Farthest goal=%d : %f m", _path[i].first, distanceSoFar); break; } } } + if(hasIntermediateNodes) + { + UERROR("Cannot follow a path with a map containing intermediate nodes (not supported: don't use intermediate nodes if rtabmap's planner has to be used). Aborting current plan!"); + this->clearPath(-1); + return; + } + UDEBUG("current node = %d current goal = %d", _path[_pathCurrentIndex].first, _path[_pathGoalIndex].first); Transform currentPose; if(_memory->isIncremental()) { - if(_memory->getLastWorkingSignature() == 0 || - !uContains(_optimizedPoses, _memory->getLastWorkingSignature()->id())) + if(_memory->getLastWorkingSignature(true) == 0 || + !uContains(_optimizedPoses, _memory->getLastWorkingSignature(true)->id())) { UERROR("Last node is null in memory or not in optimized poses. Aborting the plan..."); this->clearPath(-1); return; } - currentPose = _optimizedPoses.at(_memory->getLastWorkingSignature()->id()); + currentPose = _optimizedPoses.at(_memory->getLastWorkingSignature(true)->id()); } else { @@ -7004,11 +7414,8 @@ void Rtabmap::updateGoalIndex() if((goalIndex == _pathCurrentIndex && i == _path.size()-1) || _pathUnreachableNodes.find(i) == _pathUnreachableNodes.end()) { - if(distanceFromCurrentNode <= _localRadius) - { - goalIndex = i; - } - else + goalIndex = i; + if(distanceFromCurrentNode > _localRadius) { break; } diff --git a/corelib/src/RtabmapThread.cpp b/corelib/src/RtabmapThread.cpp index d9f227b5..44522fab 100644 --- a/corelib/src/RtabmapThread.cpp +++ b/corelib/src/RtabmapThread.cpp @@ -47,8 +47,7 @@ RtabmapThread::RtabmapThread(Rtabmap * rtabmap) : _dataBufferMaxSize(Parameters::defaultRtabmapImageBufferSize()), _rate(Parameters::defaultRtabmapDetectionRate()), _createIntermediateNodes(Parameters::defaultRtabmapCreateIntermediateNodes()), - _frameRateTimer(new UTimer()), - _previousStamp(0.0), + _previousStamp(-1.0), _rtabmap(rtabmap), _paused(false), lastPose_(Transform::getIdentity()) @@ -62,8 +61,6 @@ RtabmapThread::~RtabmapThread() UEventsManager::removeHandler(this); close(true); - - delete _frameRateTimer; } void RtabmapThread::pushNewState(State newState, const RtabmapEventCmd & cmdEvent) @@ -88,7 +85,7 @@ void RtabmapThread::clearBufferedData() _newMapEvents.clear(); lastPose_.setIdentity(); covariance_ = cv::Mat(); - _previousStamp = 0; + _previousStamp = -1; } _dataMutex.unlock(); @@ -500,15 +497,17 @@ void RtabmapThread::addData(const OdometryEvent & odomEvent) bool ignoreFrame = false; if(_rate>0.0f) { - if((_previousStamp>=0.0 && odomEvent.data().stamp()>_previousStamp && odomEvent.data().stamp() - _previousStamp < 1.0f/_rate) || - ((_previousStamp<=0.0 || odomEvent.data().stamp()<=_previousStamp) && _frameRateTimer->getElapsedTime() < 1.0f/_rate)) + if((_previousStamp>=0.0 && odomEvent.data().stamp()>_previousStamp && odomEvent.data().stamp() - _previousStamp < 1.0f/_rate)) { + UDEBUG("Ignoring frame %f (previous stamp=%f, period=%f)", + odomEvent.data().stamp(), _previousStamp, 1.0/_rate); ignoreFrame = true; } } + UASSERT(!odomEvent.info().reg.covariance.empty()); if(!lastPose_.isIdentity() && - (odomEvent.pose().isIdentity() || - odomEvent.info().reg.covariance.at(0,0)>=9999)) + (odomEvent.pose().isIdentity() || + odomEvent.info().reg.covariance.at(0,0)>=9999)) { if(odomEvent.pose().isIdentity()) { @@ -539,7 +538,6 @@ void RtabmapThread::addData(const OdometryEvent & odomEvent) } else if(!ignoreFrame) { - _frameRateTimer->start(); _previousStamp = odomEvent.data().stamp(); } @@ -558,7 +556,6 @@ void RtabmapThread::addData(const OdometryEvent & odomEvent) // set negative id so rtabmap will detect it as an intermediate node SensorData tmp = odomEvent.data(); tmp.setId(-1); - tmp.setFeatures(std::vector(), std::vector(), cv::Mat());// remove features _dataBuffer.push_back(OdometryEvent(tmp, odomEvent.pose(), odomInfo)); } else diff --git a/corelib/src/SensorCaptureThread.cpp b/corelib/src/SensorCaptureThread.cpp index 8941c3d0..6414fb9f 100644 --- a/corelib/src/SensorCaptureThread.cpp +++ b/corelib/src/SensorCaptureThread.cpp @@ -960,7 +960,6 @@ void SensorCaptureThread::postUpdate(SensorData * dataPtr, SensorCaptureInfo * i { UASSERT(!data.imu().localTransform().isNull()); imu.convertToBaseFrame(); - } _imuFilter->update( imu.angularVelocity()[0], diff --git a/corelib/src/SensorData.cpp b/corelib/src/SensorData.cpp index f616df23..8954af0f 100644 --- a/corelib/src/SensorData.cpp +++ b/corelib/src/SensorData.cpp @@ -548,7 +548,7 @@ void SensorData::setOccupancyGrid( float cellSize, const cv::Point3f & viewPoint) { - UDEBUG("ground=%d obstacles=%d empty=%d", ground.cols, obstacles.cols, empty.cols); + //UDEBUG("ground=%d obstacles=%d empty=%d", ground.cols, obstacles.cols, empty.cols); if((!ground.empty() && (!_groundCellsCompressed.empty() || !_groundCellsRaw.empty())) || (!obstacles.empty() && (!_obstacleCellsCompressed.empty() || !_obstacleCellsRaw.empty())) || (!empty.empty() && (!_emptyCellsCompressed.empty() || !_emptyCellsRaw.empty()))) @@ -649,7 +649,7 @@ void SensorData::uncompressData( cv::Mat * emptyCellsRaw, cv::Mat * depthConfidenceRaw) { - UDEBUG("%d data(%d,%d,%d,%d,%d,%d,%d,%d)", + /*UDEBUG("%d data(%d,%d,%d,%d,%d,%d,%d,%d)", this->id(), imageRaw?1:0, depthRaw?1:0, @@ -658,7 +658,7 @@ void SensorData::uncompressData( groundCellsRaw?1:0, obstacleCellsRaw?1:0, emptyCellsRaw?1:0, - depthConfidenceRaw?1:0); + depthConfidenceRaw?1:0);*/ if(imageRaw == 0 && depthRaw == 0 && laserScanRaw == 0 && @@ -976,7 +976,7 @@ unsigned long SensorData::getMemoryUsed() const // Return memory usage in Bytes (_descriptors.empty()?0:_descriptors.total()*_descriptors.elemSize()); } -void SensorData::clearCompressedData(bool images, bool scan, bool userData) +void SensorData::clearCompressedData(bool images, bool scan, bool userData, bool occupancyGrid) { if(images) { @@ -992,14 +992,32 @@ void SensorData::clearCompressedData(bool images, bool scan, bool userData) { _userDataCompressed=cv::Mat(); } + if(occupancyGrid) + { + _groundCellsCompressed=cv::Mat(); + _emptyCellsCompressed=cv::Mat(); + _obstacleCellsCompressed=cv::Mat(); + + if( _groundCellsCompressed.empty() && _groundCellsRaw.empty() && + _obstacleCellsCompressed.empty() && _obstacleCellsRaw.empty() && + _emptyCellsCompressed.empty() && _emptyCellsRaw.empty()) + { + _cellSize = 0.0f; + _viewPoint = cv::Point3f(); + } + } } -void SensorData::clearRawData(bool images, bool scan, bool userData) +void SensorData::clearRawData(bool images, bool scan, bool userData, bool occupancyGrid) { if(images) { _imageRaw=cv::Mat(); _depthOrRightRaw=cv::Mat(); _depthConfidenceRaw=cv::Mat(); +#ifdef HAVE_OPENCV_CUDEV + _imageRawGpu = cv::cuda::GpuMat(); + _depthOrRightRawGpu = cv::cuda::GpuMat(); +#endif } if(scan) { @@ -1009,6 +1027,20 @@ void SensorData::clearRawData(bool images, bool scan, bool userData) { _userDataRaw=cv::Mat(); } + if(occupancyGrid) + { + _groundCellsRaw=cv::Mat(); + _emptyCellsRaw=cv::Mat(); + _obstacleCellsRaw=cv::Mat(); + + if( _groundCellsCompressed.empty() && _groundCellsRaw.empty() && + _obstacleCellsCompressed.empty() && _obstacleCellsRaw.empty() && + _emptyCellsCompressed.empty() && _emptyCellsRaw.empty()) + { + _cellSize = 0.0f; + _viewPoint = cv::Point3f(); + } + } } diff --git a/corelib/src/Signature.cpp b/corelib/src/Signature.cpp index 32fca300..1c1ea27a 100644 --- a/corelib/src/Signature.cpp +++ b/corelib/src/Signature.cpp @@ -118,7 +118,7 @@ void Signature::addLinks(const std::map & links) } void Signature::addLink(const Link & link) { - UDEBUG("Add link %d to %d (type=%d/%s var=%f,%f)", link.to(), this->id(), (int)link.type(), link.typeName().c_str(), link.transVariance(), link.rotVariance()); + //UDEBUG("Add link %d to %d (type=%d/%s var=%f,%f)", link.to(), this->id(), (int)link.type(), link.typeName().c_str(), link.transVariance(), link.rotVariance()); UASSERT_MSG(link.from() == this->id(), uFormat("%d->%d for signature %d (type=%d)", link.from(), link.to(), this->id(), link.type()).c_str()); UASSERT_MSG((link.to() != this->id()) || link.type()==Link::kPosePrior || link.type()==Link::kGravity, uFormat("%d->%d for signature %d (type=%d)", link.from(), link.to(), this->id(), link.type()).c_str()); UASSERT_MSG(link.to() == this->id() || _links.find(link.to()) == _links.end(), uFormat("Link %d (type=%d) already added to signature %d!", link.to(), link.type(), this->id()).c_str()); @@ -318,7 +318,7 @@ void Signature::setWords(const std::multimap & words, UASSERT_MSG(descriptors.empty() || descriptors.rows == (int)words.size(), uFormat("words=%d, descriptors=%d", (int)words.size(), descriptors.rows).c_str()); UASSERT_MSG(points.empty() || points.size() == words.size(), uFormat("words=%d, points=%d", (int)words.size(), (int)points.size()).c_str()); UASSERT_MSG(keypoints.empty() || keypoints.size() == words.size(), uFormat("words=%d, descriptors=%d", (int)words.size(), (int)keypoints.size()).c_str()); - UASSERT(words.empty() || !keypoints.empty() || !points.empty() || !descriptors.empty()); + //UASSERT(words.empty() || !keypoints.empty() || !points.empty() || !descriptors.empty()); _invalidWordsCount = 0; for(std::multimap::const_iterator iter=words.begin(); iter!=words.end(); ++iter) @@ -328,7 +328,7 @@ void Signature::setWords(const std::multimap & words, ++_invalidWordsCount; } // make sure indexes are all valid! - UASSERT_MSG(iter->second >=0 && iter->second < (int)words.size(), uFormat("iter->second=%d words.size()=%d", iter->second, (int)words.size()).c_str()); + UASSERT_MSG(iter->second<0 || iter->second < (int)words.size(), uFormat("iter->second=%d words.size()=%d", iter->second, (int)words.size()).c_str()); } _enabled = false; diff --git a/corelib/src/Stereo.cpp b/corelib/src/Stereo.cpp index 0269f2b5..a3554f6a 100644 --- a/corelib/src/Stereo.cpp +++ b/corelib/src/Stereo.cpp @@ -113,6 +113,9 @@ std::vector Stereo::computeCorrespondences( StereoOpticalFlow::StereoOpticalFlow(const ParametersMap & parameters) : Stereo(parameters), epsilon_(Parameters::defaultStereoEps()), + useMinEigenVals_(Parameters::defaultStereoUseMinEigenVals()), + minEigThreshold_(Parameters::defaultStereoMinEigThreshold()), + errorThreshold_(Parameters::defaultStereoErrorThreshold()), gpu_(Parameters::defaultStereoGpu()) { this->parseParameters(parameters); @@ -122,6 +125,9 @@ void StereoOpticalFlow::parseParameters(const ParametersMap & parameters) { Stereo::parseParameters(parameters); Parameters::parse(parameters, Parameters::kStereoEps(), epsilon_); + Parameters::parse(parameters, Parameters::kStereoUseMinEigenVals(), useMinEigenVals_); + Parameters::parse(parameters, Parameters::kStereoMinEigThreshold(), minEigThreshold_); + Parameters::parse(parameters, Parameters::kStereoErrorThreshold(), errorThreshold_); Parameters::parse(parameters, Parameters::kStereoGpu(), gpu_); #ifndef HAVE_OPENCV_CUDAOPTFLOW if(gpu_) @@ -171,11 +177,18 @@ std::vector StereoOpticalFlow::computeCorrespondences( err, this->winSize(), this->maxLevel(), - cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, this->iterations(), epsilon_), - cv::OPTFLOW_LK_GET_MIN_EIGENVALS, 1e-4); + cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, this->iterations(), this->epsilon()), + this->usingMinEigenVals() ? cv::OPTFLOW_LK_GET_MIN_EIGENVALS : 0, this->minEigThreshold()); UDEBUG("util2d::calcOpticalFlowPyrLKStereo() end"); } - updateStatus(leftCorners, rightCorners, status); + if(this->usingMinEigenVals()) + { + updateStatus(leftCorners, rightCorners, status); + } + else + { + updateStatus(leftCorners, rightCorners, status, err); + } return rightCorners; } @@ -227,14 +240,17 @@ std::vector StereoOpticalFlow::computeCorrespondences( void StereoOpticalFlow::updateStatus( const std::vector & leftCorners, const std::vector & rightCorners, - std::vector & status) const + std::vector & status, + std::vector err) const { - UASSERT(leftCorners.size() == rightCorners.size() && status.size() == leftCorners.size()); + UASSERT( + leftCorners.size() == rightCorners.size() && status.size() == leftCorners.size() && + (err.empty() || err.size() == leftCorners.size())); int countFlowRejected = 0; int countDisparityRejected = 0; for(unsigned int i=0; ierrorThreshold())) { float disparity = leftCorners[i].x - rightCorners[i].x; if(disparity <= this->minDisparity() || disparity > this->maxDisparity()) diff --git a/corelib/src/Transform.cpp b/corelib/src/Transform.cpp index 08c8d71a..3e511b7e 100644 --- a/corelib/src/Transform.cpp +++ b/corelib/src/Transform.cpp @@ -52,6 +52,26 @@ Transform::Transform( r11, r12, r13, o14, r21, r22, r23, o24, r31, r32, r33, o34); + + if( r11>0.0f || r12>0.0f || r13>0.0f || + r21>0.0f || r22>0.0f || r23>0.0f || + r31>0.0f || r32>0.0f || r33>0.0f) + { + Eigen::Matrix3f m; + m << r11, r12, r13, + r21, r22, r23, + r31, r32, r33; + float d = m.determinant(); + if(fabs(d-1.0f) > 0.0001) + { + UWARN("Created transform doesn't have normalized rotation. Any transformation with this transform can cause unexpected results!" + " Determinant([%f %f %f;%f %f %f;%f %f %f])=%f", + r11, r12, r13, + r21, r22, r23, + r31, r32, r33, + d); + } + } } Transform::Transform(const cv::Mat & transformationMatrix) @@ -509,6 +529,11 @@ Transform Transform::fromString(const std::string & string) numbers[4], numbers[5], numbers[6], numbers[7], numbers[8], numbers[9], numbers[10], numbers[11]); } + // Always normalize + if(!t.isNull()) + { + t.normalizeRotation(); + } return t; } diff --git a/corelib/src/VWDictionary.cpp b/corelib/src/VWDictionary.cpp index fc1bb172..952e012c 100644 --- a/corelib/src/VWDictionary.cpp +++ b/corelib/src/VWDictionary.cpp @@ -51,7 +51,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include -#define KDTREE_SIZE 4 #define KNN_CHECKS 32 namespace rtabmap @@ -69,9 +68,11 @@ VWDictionary::VWDictionary(const ParametersMap & parameters) : _nndrRatio(Parameters::defaultKpNndrRatio()), _newDictionaryPath(Parameters::defaultKpDictionaryPath()), _newWordsComparedTogether(Parameters::defaultKpNewWordsComparedTogether()), + _serializeWithChecksum(Parameters::defaultKpSerializeWithChecksum()), _lastWordId(0), useDistanceL1_(false), _flannIndex(new FlannIndex()), + _modified(true), _strategy(kNNBruteForce) { this->setNNStrategy((NNStrategy)Parameters::defaultKpNNStrategy()); @@ -89,6 +90,7 @@ void VWDictionary::parseParameters(const ParametersMap & parameters) ParametersMap::const_iterator iter; Parameters::parse(parameters, Parameters::kKpNndrRatio(), _nndrRatio); Parameters::parse(parameters, Parameters::kKpNewWordsComparedTogether(), _newWordsComparedTogether); + Parameters::parse(parameters, Parameters::kKpSerializeWithChecksum(), _serializeWithChecksum); Parameters::parse(parameters, Parameters::kKpIncrementalFlann(), _incrementalFlann); Parameters::parse(parameters, Parameters::kKpFlannRebalancingFactor(), _rebalancingFactor); bool byteToFloat = _byteToFloat; @@ -160,7 +162,7 @@ void VWDictionary::setFixedDictionary(const std::string & dictionaryPath) DBDriver * driver = DBDriver::create(); if(driver->openConnection(dictionaryPath, false)) { - driver->load(this, false); + driver->load(*this, false); for(std::map::iterator iter=_visualWords.begin(); iter!=_visualWords.end(); ++iter) { iter->second->setSaved(true); @@ -289,6 +291,11 @@ void VWDictionary::setFixedDictionary(const std::string & dictionaryPath) _newDictionaryPath = dictionaryPath; } +bool VWDictionary::isModified() const +{ + return _modified; +} + bool VWDictionary::setNNStrategy(NNStrategy strategy) { #if CV_MAJOR_VERSION < 3 @@ -484,7 +491,13 @@ void VWDictionary::update() if(_notIndexedWords.size() || _visualWords.size() == 0 || _removedIndexedWords.size()) { - if(_incrementalFlann && + _modified = true; + bool firstUpdate = _removedIndexedWords.empty() && _visualWords.size() == _notIndexedWords.size(); + UDEBUG("firstUpdate=%s (_removedIndexedWords=%ld, _visualWords=%ld, _notIndexedWords=%ld)", + firstUpdate?"true":"false", _removedIndexedWords.size(), _visualWords.size(), _notIndexedWords.size()); + + if(!firstUpdate && + _incrementalFlann && _strategy < kNNBruteForce && _visualWords.size()) { @@ -501,7 +514,9 @@ void VWDictionary::update() if(_notIndexedWords.size()) { - ULOGGER_DEBUG("Incremental FLANN: Inserting %d words...", (int)_notIndexedWords.size()); + UTimer timer; + timer.start(); + ULOGGER_DEBUG("Incremental FLANN: Inserting %d words...", (int)_notIndexedWords.size(), _byteToFloat?"true":"false"); for(std::set::iterator iter=_notIndexedWords.begin(); iter!=_notIndexedWords.end(); ++iter) { VisualWord* w = uValue(_visualWords, *iter, (VisualWord*)0); @@ -528,24 +543,13 @@ void VWDictionary::update() int index = 0; if(!_flannIndex->isBuilt()) { - UDEBUG("Building FLANN index..."); - switch(_strategy) - { - case kNNFlannNaive: - _flannIndex->buildLinearIndex(descriptor, useDistanceL1_, _rebalancingFactor); - break; - case kNNFlannKdTree: - UASSERT_MSG(descriptor.type() == CV_32F, "To use KdTree dictionary, float descriptors are required!"); - _flannIndex->buildKDTreeIndex(descriptor, KDTREE_SIZE, useDistanceL1_, _rebalancingFactor); - break; - case kNNFlannLSH: - UASSERT_MSG(descriptor.type() == CV_8U, "To use LSH dictionary, binary descriptors are required!"); - _flannIndex->buildLSHIndex(descriptor, 12, 20, 2, _rebalancingFactor); - break; - default: - UFATAL("Not supposed to be here!"); - break; - } + UDEBUG("Building FLANN index... (strategy=%s, byteToFloat=%s, useDistanceL1=%s, rebalancingFactor=%f)", + nnStrategyName(_strategy).c_str(), _byteToFloat?"true":"false", useDistanceL1_?"true":"false", _rebalancingFactor); + _flannIndex->buildIndex( + _strategy == kNNFlannNaive ? FlannIndex::FLANN_INDEX_LINEAR: + _strategy == kNNFlannLSH ? FlannIndex::FLANN_INDEX_LSH: + FlannIndex::FLANN_INDEX_KDTREE, // kNNFlannKdTree + descriptor, useDistanceL1_, _rebalancingFactor); UDEBUG("Building FLANN index... done!"); } else @@ -561,25 +565,42 @@ void VWDictionary::update() inserted = _mapIdIndex.insert(std::pair(w->id(), index)); UASSERT(inserted.second); } - ULOGGER_DEBUG("Incremental FLANN: Inserting %d words... done!", (int)_notIndexedWords.size()); + ULOGGER_DEBUG("Incremental FLANN: Inserting %d words... done! (in %f s)", (int)_notIndexedWords.size(), timer.ticks()); } } else if(_strategy >= kNNBruteForce && _notIndexedWords.size() && _removedIndexedWords.size() == 0 && - _visualWords.size() && - _dataTree.rows) + _visualWords.size()) { + const int IMGIDX_SHIFT = 18; + const int IMGIDX_ONE = (1 << IMGIDX_SHIFT); // a limit defined in https://github.com/opencv/opencv/blob/4.x/modules/features2d/src/matchers.cpp + if(_dataTree.rows >= IMGIDX_ONE) + { + UWARN("%s=%d is not a FLANN strategy and the number of words in the vocabulary (%d) is over %d (IMGIDX_ONE), so opencv may " + "assert on an IMGIDX_ONE check when adding new words. Use a FLANN strategy instead (%s<%d).", + Parameters::kKpNNStrategy().c_str(), _strategy, _dataTree.rows, IMGIDX_ONE, Parameters::kKpNNStrategy().c_str(), kNNBruteForce); + } + //just add not indexed words int i = _dataTree.rows; - _dataTree.reserve(_dataTree.rows + _notIndexedWords.size()); + if(!_dataTree.empty()) { + _dataTree.reserve(_dataTree.rows + _notIndexedWords.size()); + } for(std::set::iterator iter=_notIndexedWords.begin(); iter!=_notIndexedWords.end(); ++iter) { VisualWord* w = uValue(_visualWords, *iter, (VisualWord*)0); UASSERT(w); - UASSERT(w->getDescriptor().cols == _dataTree.cols); - UASSERT(w->getDescriptor().type() == _dataTree.type()); - _dataTree.push_back(w->getDescriptor()); + if(_dataTree.empty()) + { + _dataTree = w->getDescriptor().clone(); + } + else + { + UASSERT(w->getDescriptor().cols == _dataTree.cols); + UASSERT(w->getDescriptor().type() == _dataTree.type()); + _dataTree.push_back(w->getDescriptor()); + } _mapIndexId.insert(_mapIndexId.end(), std::pair(i, w->id())); std::pair::iterator, bool> inserted = _mapIdIndex.insert(std::pair(w->id(), i)); UASSERT(inserted.second); @@ -657,23 +678,13 @@ void VWDictionary::update() ULOGGER_DEBUG("_mapIndexId.size() = %d, words.size()=%d, _dim=%d",_mapIndexId.size(), _visualWords.size(), dim); ULOGGER_DEBUG("copying data = %f s", timer.ticks()); - switch(_strategy) - { - case kNNFlannNaive: - _flannIndex->buildLinearIndex(_dataTree, useDistanceL1_, _incrementalDictionary&&_incrementalFlann?_rebalancingFactor:1); - break; - case kNNFlannKdTree: - UASSERT_MSG(type == CV_32F, "To use KdTree dictionary, float descriptors are required!"); - _flannIndex->buildKDTreeIndex(_dataTree, KDTREE_SIZE, useDistanceL1_, _incrementalDictionary&&_incrementalFlann?_rebalancingFactor:1); - break; - case kNNFlannLSH: - UASSERT_MSG(type == CV_8U, "To use LSH dictionary, binary descriptors are required!"); - _flannIndex->buildLSHIndex(_dataTree, 12, 20, 2, _incrementalDictionary&&_incrementalFlann?_rebalancingFactor:1); - break; - default: - break; - } - + _flannIndex->buildIndex( + _strategy == kNNFlannNaive ? FlannIndex::FLANN_INDEX_LINEAR: + _strategy == kNNFlannLSH ? FlannIndex::FLANN_INDEX_LSH: + FlannIndex::FLANN_INDEX_KDTREE, // kNNFlannKdTree + _dataTree, + useDistanceL1_, + _incrementalDictionary&&_incrementalFlann?_rebalancingFactor:1); ULOGGER_DEBUG("Time to create kd tree = %f s", timer.ticks()); } } @@ -689,6 +700,146 @@ void VWDictionary::update() UDEBUG(""); } +std::vector VWDictionary::serializeIndex() const +{ + if(_strategy >= kNNBruteForce) { + UINFO("Not flann strategy, ignoring serialization..."); + return std::vector(); + } + if(!_flannIndex->isBuilt() || !_removedIndexedWords.empty() || !_notIndexedWords.empty() || _visualWords.empty()) { + UWARN("Flann index is not buit, or there are words not indexed, cannot do serialization."); + return std::vector(); + } + + return _flannIndex->serializeIndex(_serializeWithChecksum); +} + +void VWDictionary::deserializeIndex(const std::vector & data) +{ + deserializeIndex(data.data(), data.size()); +} + +void VWDictionary::deserializeIndex(const unsigned char * data, size_t size) +{ + if(data== NULL || size == 0) + { + UWARN("Trying to deserialize empty data, aborting."); + return; + } + UDEBUG("Loading flann index... (data size=%ld bytes)", size); + if(_strategy >= kNNBruteForce) { + //ignore + return; + } + + if(_flannIndex->isBuilt()) { + UERROR("Flann index is already built, cannot deserialize data!"); + return; + } + + if(_visualWords.empty()) { + UERROR("Descriptors should be added before deserializing flann index! See VWDictionary::addWord()"); + return; + } + + if(!(_removedIndexedWords.empty() && _visualWords.size() == _notIndexedWords.size())) { + UERROR("State of dictionary not as expected before deserializing. (removed words=%ld, words=%ld, not indexed=%ld)", + _removedIndexedWords.size(), _visualWords.size(), _notIndexedWords.size()); + return; + } + + std::map mapIndexId; + std::map mapIdIndex; + cv::Mat dataTree; + + UTimer timer; + timer.start(); + + int dim = _visualWords.begin()->second->getDescriptor().cols; + int type; + if(_visualWords.begin()->second->getDescriptor().type() == CV_8U) + { + useDistanceL1_ = true; + if(_strategy == kNNFlannKdTree) + { + type = CV_32F; + if(!_byteToFloat) + { + dim *= 8; + } + } + else + { + type = _visualWords.begin()->second->getDescriptor().type(); + } + } + else + { + type = _visualWords.begin()->second->getDescriptor().type(); + } + + UASSERT(type == CV_32F || type == CV_8U); + UASSERT(dim > 0); + + // Create the data matrix + dataTree = cv::Mat(_visualWords.size(), dim, type); // SURF descriptors are CV_32F + std::map::const_iterator iter = _visualWords.begin(); + for(unsigned int i=0; i < _visualWords.size(); ++i, ++iter) + { + cv::Mat descriptor; + if(iter->second->getDescriptor().type() == CV_8U) + { + if(_strategy == kNNFlannKdTree) + { + descriptor = convertBinTo32F(iter->second->getDescriptor(), _byteToFloat); + } + else + { + descriptor = iter->second->getDescriptor(); + } + } + else + { + descriptor = iter->second->getDescriptor(); + } + + UASSERT_MSG(descriptor.type() == type, uFormat("%d vs %d", descriptor.type(), type).c_str()); + UASSERT_MSG(descriptor.cols == dim, uFormat("%d vs %d", descriptor.cols, dim).c_str()); + + descriptor.copyTo(dataTree.row(i)); + mapIndexId.insert(mapIndexId.end(), std::pair(i, iter->second->id())); + mapIdIndex.insert(mapIdIndex.end(), std::pair(iter->second->id(), i)); + } + + ULOGGER_DEBUG("mapIndexId.size() = %d, words.size()=%d, dim=%d", mapIndexId.size(), _visualWords.size(), dim); + ULOGGER_DEBUG("copying data = %f s", timer.ticks()); + + std::string errorMsg; + if(_flannIndex->loadIndex( + data, + size, + _strategy == kNNFlannNaive ? FlannIndex::FLANN_INDEX_LINEAR: + _strategy == kNNFlannLSH ? FlannIndex::FLANN_INDEX_LSH: + FlannIndex::FLANN_INDEX_KDTREE, + dataTree, + useDistanceL1_, + _incrementalDictionary && _incrementalFlann ? _rebalancingFactor:1, + &errorMsg)) + { + _mapIndexId = mapIndexId; + _mapIdIndex = mapIdIndex; + _dataTree = dataTree; + _notIndexedWords.clear(); + _modified = false; + } + else { + UWARN("Failed deserializing flann index data (error: %s), the index will be rebuilt on next update.", errorMsg.c_str()); + _flannIndex->release(); // reset to initial state + } + + ULOGGER_DEBUG("Time to load flann index = %f s", timer.ticks()); +} + void VWDictionary::clear(bool printWarningsIfNotEmpty) { ULOGGER_DEBUG(""); @@ -718,6 +869,7 @@ void VWDictionary::clear(bool printWarningsIfNotEmpty) _unusedWords.clear(); _flannIndex->release(); useDistanceL1_ = false; + _modified = true; } int VWDictionary::getNextId() @@ -725,7 +877,7 @@ int VWDictionary::getNextId() return ++_lastWordId; } -void VWDictionary::addWordRef(int wordId, int signatureId) +bool VWDictionary::addWordRef(int wordId, int signatureId) { VisualWord * vw = 0; vw = uValue(_visualWords, wordId, vw); @@ -735,10 +887,12 @@ void VWDictionary::addWordRef(int wordId, int signatureId) _totalActiveReferences += 1; _unusedWords.erase(vw->id()); + return true; } else { - UERROR("Not found word %d (dict size=%d)", wordId, (int)_visualWords.size()); + UWARN("Not found word %d (dict size=%d)", wordId, (int)_visualWords.size()); + return false; } } @@ -784,14 +938,21 @@ std::list VWDictionary::addNewWords( type = _visualWords.begin()->second->getDescriptor().type(); UASSERT(type == CV_32F || type == CV_8U); } + static std::string moreInfo = uFormat( + "This could happen if the computer doesn't have access to same " + "feature detectors than when the database was created. This could " + "also happen if we enabled \"%s\" but the first frame received " + "was empty, thus features were re-extracted with a different detector " + "than the one used by the odometry.", + Parameters::kMemUseOdomFeatures().c_str()); if(dim && dim != descriptorsIn.cols) { - UERROR("Descriptors (size=%d) are not the same size as already added words in dictionary(size=%d)", descriptorsIn.cols, dim); + UERROR("Descriptors (size=%d) are not the same size as already added words in dictionary (size=%d). %s", descriptorsIn.cols, dim, moreInfo.c_str()); return wordIds; } if(type>=0 && type != descriptorsIn.type()) { - UERROR("Descriptors (type=%d) are not the same type as already added words in dictionary(type=%d)", descriptorsIn.type(), type); + UERROR("Descriptors (type=%d) are not the same type as already added words in dictionary (type=%d). %s", descriptorsIn.type(), type, moreInfo.c_str()); return wordIds; } @@ -854,7 +1015,7 @@ std::list VWDictionary::addNewWords( if(_flannIndex->isBuilt() || (!_dataTree.empty() && _dataTree.rows >= (int)k)) { //Find nearest neighbors - UDEBUG("newPts.total()=%d ", descriptors.rows); + UDEBUG("newPts.total()=%d _strategy=%d", descriptors.rows, _strategy); if(_strategy == kNNFlannNaive || _strategy == kNNFlannKdTree || _strategy == kNNFlannLSH) { @@ -1394,15 +1555,15 @@ void VWDictionary::addWord(VisualWord * vw) { if(vw) { - _visualWords.insert(std::pair(vw->id(), vw)); - _notIndexedWords.insert(vw->id()); + _visualWords.insert(_visualWords.end(), std::pair(vw->id(), vw)); + _notIndexedWords.insert(_notIndexedWords.end(), vw->id()); if(vw->getReferences().size()) { _totalActiveReferences += uSum(uValues(vw->getReferences())); } else { - _unusedWords.insert(std::pair(vw->id(), vw)); + _unusedWords.insert(_unusedWords.end(), std::pair(vw->id(), vw)); } if(_lastWordId < vw->id()) { diff --git a/corelib/src/VisualWord.cpp b/corelib/src/VisualWord.cpp index f1b1196c..70584430 100644 --- a/corelib/src/VisualWord.cpp +++ b/corelib/src/VisualWord.cpp @@ -57,7 +57,7 @@ void VisualWord::addRef(int signatureId) } else { - _references.insert(std::pair(signatureId, 1)); + _references.insert(_references.end(), std::pair(signatureId, 1)); } ++_totalReferences; } diff --git a/corelib/src/camera/CameraDepthAI.cpp b/corelib/src/camera/CameraDepthAI.cpp index 9eb01791..beaca874 100644 --- a/corelib/src/camera/CameraDepthAI.cpp +++ b/corelib/src/camera/CameraDepthAI.cpp @@ -370,9 +370,10 @@ bool CameraDepthAI::init(const std::string & calibrationFolder, const std::strin matrix[2][0], matrix[2][1], matrix[2][2]); std::vector coeffs = calibHandler.getDistortionCoefficients(cameraId); - if(calibHandler.getDistortionModel(cameraId) == 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(calibHandler.getDistortionModel(cameraId) == dai::CameraModel::Perspective) { + UASSERT(coeffs.size()>=14); + distCoeffs = (cv::Mat_(1,14) << coeffs[0], coeffs[1], coeffs[2], coeffs[3], coeffs[4], coeffs[5], coeffs[6], coeffs[7], coeffs[8], coeffs[9], coeffs[10], coeffs[11], coeffs[12], coeffs[13]); + } if(alphaScaling_>-1.0f) newCameraMatrix = cv::getOptimalNewCameraMatrix(cameraMatrix, distCoeffs, targetSize_, alphaScaling_); else diff --git a/corelib/src/camera/CameraImages.cpp b/corelib/src/camera/CameraImages.cpp index 0591181a..97674121 100644 --- a/corelib/src/camera/CameraImages.cpp +++ b/corelib/src/camera/CameraImages.cpp @@ -63,6 +63,7 @@ CameraImages::CameraImages() : _syncImageRateWithStamps(true), _odometryFormat(0), _groundTruthFormat(0), + _groundTruthLocalTransform(Transform::getIdentity()), _maxPoseTimeDiff(0.02), _captureDelay(0.0) {} @@ -93,6 +94,7 @@ CameraImages::CameraImages(const std::string & path, _syncImageRateWithStamps(true), _odometryFormat(0), _groundTruthFormat(0), + _groundTruthLocalTransform(Transform::getIdentity()), _maxPoseTimeDiff(0.02), _captureDelay(0.0) { @@ -478,27 +480,43 @@ bool CameraImages::init(const std::string & calibrationFolder, const std::string if(success && _odometryPath.size() && odometry_.empty()) { success = readPoses(odometry_, _stamps, _odometryPath, _odometryFormat, _maxPoseTimeDiff); + if(!success) + { + UERROR("Failed to read odometry poses."); + } + + if(success) + { + for(size_t i=0; i(3,3) *= 0.01; + covariance.at(4,4) *= 0.01; + covariance.at(5,5) *= 0.01; + } + covariances_.push_back(covariance); + } + } } if(success && _groundTruthPath.size()) { success = readPoses(groundTruth_, _stamps, _groundTruthPath, _groundTruthFormat, _maxPoseTimeDiff); - } - - if(!odometry_.empty()) - { - for(size_t i=0; i(3,3) *= 0.01; - covariance.at(4,4) *= 0.01; - covariance.at(5,5) *= 0.01; + pose = pose*gtInv; // pose of base_link, assuming ground truth frame and base frame are rigidly fixed } - covariances_.push_back(covariance); } } } @@ -523,19 +541,19 @@ bool CameraImages::readPoses( UERROR("Cannot read pose file \"%s\".", filePath.c_str()); return false; } - else if((format != 1 && format != 10 && format != 5 && format != 6 && format != 7 && format != 9) && poses.size() != this->imagesCount()) + else if((format != 1 && format != 10 && format != 12 && format != 5 && format != 6 && format != 7 && format != 9) && poses.size() != this->imagesCount()) { UERROR("The pose count is not the same as the images (%d vs %d)! Please remove " "the pose file path if you don't want to use it (current file path=%s).", (int)poses.size(), this->imagesCount(), filePath.c_str()); return false; } - else if((format == 1 || format == 10 || format == 5 || format == 6 || format == 7 || format == 9) && (inOutStamps.empty() && stamps.size()!=poses.size())) + else if((format == 1 || format == 10 || format == 12 || format == 5 || format == 6 || format == 7 || format == 9) && (inOutStamps.empty() && stamps.size()!=poses.size())) { UERROR("When using RGBD-SLAM, GPS, MALAGA, ST LUCIA and EuRoC MAV formats, images must have timestamps!"); return false; } - else if(format == 1 || format == 10 || format == 5 || format == 6 || format == 7 || format == 9) + else if(format == 1 || format == 10 || format == 12 || format == 5 || format == 6 || format == 7 || format == 9) { UDEBUG(""); //Match ground truth values with images @@ -607,7 +625,7 @@ bool CameraImages::readPoses( } if(validPoses != (int)inOutStamps.size()) { - UWARN("%d valid poses of %d stamps", validPoses, (int)inOutStamps.size()); + UWARN("%d/%ld valid poses of %ld stamps", validPoses, outputPoses.size(), inOutStamps.size()); } } else @@ -756,32 +774,19 @@ SensorData CameraImages::captureImage(SensorCaptureInfo * info) if(_stamps.size()) { - stamp = _stamps.front(); - _stamps.pop_front(); - if(_stamps.size()) - { - _captureDelay = _stamps.front() - stamp; - } + UERROR("stamps cannot be used when startAt < 0"); } if(odometry_.size()) { - odometryPose = odometry_.front(); - odometry_.pop_front(); - if(covariances_.size()) - { - covariance = covariances_.front(); - covariances_.pop_front(); - } + UERROR("odometry cannot be used when startAt < 0"); } if(groundTruth_.size()) { - groundTruthPose = groundTruth_.front(); - groundTruth_.pop_front(); + UERROR("groundTruth cannot be used when startAt < 0"); } if(_models.size() && !model.isValidForProjection()) { - model = _models.front(); - _models.pop_front(); + UERROR("models cannot be used when startAt < 0"); } } else @@ -792,6 +797,7 @@ SensorData CameraImages::captureImage(SensorCaptureInfo * info) { imageFilePath = _path + imageFileName; scanFilePath = _scanPath + scanFileName; + size_t stampsSize = _stamps.size(); if(_stamps.size()) { stamp = _stamps.front(); @@ -803,6 +809,8 @@ SensorData CameraImages::captureImage(SensorCaptureInfo * info) } if(odometry_.size()) { + UASSERT_MSG(stampsSize==0 || stampsSize == odometry_.size(), + uFormat("Stamps=%ld odometry=%ld", _stamps.size(), odometry_.size()).c_str()); odometryPose = odometry_.front(); odometry_.pop_front(); if(covariances_.size()) @@ -813,11 +821,15 @@ SensorData CameraImages::captureImage(SensorCaptureInfo * info) } if(groundTruth_.size()) { + UASSERT_MSG(stampsSize==0 || stampsSize == groundTruth_.size(), + uFormat("Stamps=%ld groundTruth=%ld", _stamps.size(), groundTruth_.size()).c_str()); groundTruthPose = groundTruth_.front(); groundTruth_.pop_front(); } if(_models.size() && !model.isValidForProjection()) { + UASSERT_MSG(stampsSize==0 || stampsSize == _models.size(), + uFormat("Stamps=%ld models=%ld", _stamps.size(), _models.size()).c_str()); model = _models.front(); _models.pop_front(); } @@ -834,6 +846,7 @@ SensorData CameraImages::captureImage(SensorCaptureInfo * info) imageFilePath = _path + imageFileName; scanFilePath = _scanPath + scanFileName; + size_t stampsSize = _stamps.size(); if(_stamps.size()) { stamp = _stamps.front(); @@ -845,6 +858,8 @@ SensorData CameraImages::captureImage(SensorCaptureInfo * info) } if(odometry_.size()) { + UASSERT_MSG(stampsSize==0 || stampsSize == odometry_.size(), + uFormat("Stamps=%ld odometry=%ld", stampsSize, odometry_.size()).c_str()); odometryPose = odometry_.front(); odometry_.pop_front(); if(covariances_.size()) @@ -855,11 +870,15 @@ SensorData CameraImages::captureImage(SensorCaptureInfo * info) } if(groundTruth_.size()) { + UASSERT_MSG(stampsSize==0 || stampsSize == groundTruth_.size(), + uFormat("Stamps=%ld groundTruth=%ld", _stamps.size(), groundTruth_.size()).c_str()); groundTruthPose = groundTruth_.front(); groundTruth_.pop_front(); } if(_models.size() && !model.isValidForProjection()) { + UASSERT_MSG(stampsSize==0 || stampsSize == _models.size(), + uFormat("Stamps=%ld models=%ld", _stamps.size(), _models.size()).c_str()); model = _models.front(); _models.pop_front(); } diff --git a/corelib/src/camera/CameraOrbbecSDK.cpp b/corelib/src/camera/CameraOrbbecSDK.cpp new file mode 100644 index 00000000..30d11277 --- /dev/null +++ b/corelib/src/camera/CameraOrbbecSDK.cpp @@ -0,0 +1,742 @@ +/* +Copyright (c) 2010-2025, 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 +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 +#include +#include +#include +#include + +#ifdef RTABMAP_ORBBEC_SDK +#include +#endif + +namespace rtabmap +{ +#ifdef RTABMAP_ORBBEC_SDK +Transform obToRtabmap(const OBExtrinsic & t) +{ + return Transform(t.rot[0], t.rot[1], t.rot[2], t.trans[0]/1000.0f, + t.rot[3], t.rot[4], t.rot[5], t.trans[1]/1000.0f, + t.rot[6], t.rot[7], t.rot[8], t.trans[2]/1000.0f); +} + + +cv::Mat obColorFrameToCv(const ob::VideoFrame & videoFrame) +{ + cv::Mat rgb; + switch(videoFrame.getFormat()) { + case OB_FORMAT_MJPG: { + cv::Mat rawMat(1, videoFrame.getDataSize(), CV_8UC1, videoFrame.getData()); + rgb = cv::imdecode(rawMat, 1); + } break; + case OB_FORMAT_NV21: { + cv::Mat rawMat(videoFrame.getHeight() * 3 / 2, videoFrame.getWidth(), CV_8UC1, videoFrame.getData()); + cv::cvtColor(rawMat, rgb, cv::COLOR_YUV2BGR_NV21); + } break; + case OB_FORMAT_YUYV: + case OB_FORMAT_YUY2: { + cv::Mat rawMat(videoFrame.getHeight(), videoFrame.getWidth(), CV_8UC2, videoFrame.getData()); + cv::cvtColor(rawMat, rgb, cv::COLOR_YUV2BGR_YUY2); + } break; + case OB_FORMAT_BGR: { + cv::Mat rawMat(videoFrame.getHeight(), videoFrame.getWidth(), CV_8UC3, videoFrame.getData()); + cv::cvtColor(rawMat, rgb, cv::COLOR_BGR2RGB); + } break; + case OB_FORMAT_RGB: { + cv::Mat rawMat(videoFrame.getHeight(), videoFrame.getWidth(), CV_8UC3, videoFrame.getData()); + cv::cvtColor(rawMat, rgb, cv::COLOR_RGB2BGR); + } break; + case OB_FORMAT_RGBA: { + cv::Mat rawMat(videoFrame.getHeight(), videoFrame.getWidth(), CV_8UC4, videoFrame.getData()); + cv::cvtColor(rawMat, rgb, cv::COLOR_RGBA2BGR); + } break; + case OB_FORMAT_BGRA: { + cv::Mat rawMat(videoFrame.getHeight(), videoFrame.getWidth(), CV_8UC4, videoFrame.getData()); + cv::cvtColor(rawMat, rgb, cv::COLOR_BGRA2RGB); + } break; + case OB_FORMAT_UYVY: { + cv::Mat rawMat(videoFrame.getHeight(), videoFrame.getWidth(), CV_8UC2, videoFrame.getData()); + cv::cvtColor(rawMat, rgb, cv::COLOR_YUV2BGR_UYVY); + } break; + case OB_FORMAT_I420: { + cv::Mat rawMat(videoFrame.getHeight() * 3 / 2, videoFrame.getWidth(), CV_8UC1, videoFrame.getData()); + cv::cvtColor(rawMat, rgb, cv::COLOR_YUV2BGR_I420); + } break; + case OB_FORMAT_Y8: { + rgb = cv::Mat(videoFrame.getHeight(), videoFrame.getWidth(), CV_8UC1, videoFrame.getData()).clone(); + } break; + case OB_FORMAT_Y16: { + cv::Mat rawMat(videoFrame.getHeight(), videoFrame.getWidth(), CV_16UC1, videoFrame.getData()); + rawMat.convertTo(rgb, CV_8UC1, 255.0 / 65535.0); + } break; + default: + break; + } + return rgb; +} + +cv::Mat obDepthFrameToCv(const ob::DepthFrame & depthFrame) +{ + cv::Mat depth; + if(depthFrame.getFormat() == OB_FORMAT_Y16 || depthFrame.getFormat() == OB_FORMAT_Z16 || depthFrame.getFormat() == OB_FORMAT_Y12C4) { + cv::Mat rawMat = cv::Mat(depthFrame.getHeight(), depthFrame.getWidth(), CV_16UC1, depthFrame.getData()); + float scale = depthFrame.getValueScale() / 1000.0f; + rawMat.convertTo(depth, CV_32F, scale); + } + return depth; +} + +cv::Mat obIntrinsicToK(const OBCameraIntrinsic & intrinsics) +{ + cv::Mat K = cv::Mat::eye(3,3,CV_64FC1); + K.at(0,0) = intrinsics.fx; + K.at(1,1) = intrinsics.fy; + K.at(0,2) = intrinsics.cx; + K.at(1,2) = intrinsics.cy; + return K; +} +cv::Mat obIntrinsicToP(const OBCameraIntrinsic & intrinsics) +{ + cv::Mat P = cv::Mat::eye(3,4,CV_64FC1); + obIntrinsicToK(intrinsics).copyTo(P.colRange(0,3)); + return P; +} +cv::Mat obDistortionToD(const OBCameraDistortion & distortion) +{ + cv::Mat D = cv::Mat(1,8,CV_64FC1); + D.at(0,0) = distortion.k1; + D.at(0,1) = distortion.k2; + D.at(0,2) = distortion.p1; + D.at(0,3) = distortion.p2; + D.at(0,4) = distortion.k3; + D.at(0,5) = distortion.k4; + D.at(0,6) = distortion.k5; + D.at(0,7) = distortion.k6; + if(distortion.k4 == 0 && distortion.k5 == 0 && distortion.k6 == 0) + { + D = D.colRange(0,5); + } + return D; +} +#endif + +bool CameraOrbbecSDK::available() +{ +#ifdef RTABMAP_ORBBEC_SDK + return true; +#else + return false; +#endif +} + +CameraOrbbecSDK::CameraOrbbecSDK( + std::string deviceId, + unsigned int colorWidth, + unsigned int colorHeight, + unsigned int depthWidth, + unsigned int depthHeight, + float imageRate, + const Transform & localTransform) : + Camera(imageRate, localTransform) +#ifdef RTABMAP_ORBBEC_SDK + , deviceId_(deviceId), + colorWidth_(colorWidth), + colorHeight_(colorHeight), + depthWidth_(depthWidth), + depthHeight_(depthHeight), + pipeline_(nullptr), + imuPipeline_(nullptr), + alignFilter_(nullptr), + imuLocalTransformInitialized_(false), + lastAccStamp_(0), + lastImageStamp_(0), + globalTimestampAvailable_(false), + rectifyColor_(false), + convertDepthToMM_(true), + imuPublished_(true) +#endif +{ +} + +CameraOrbbecSDK::~CameraOrbbecSDK() +{ +#ifdef RTABMAP_ORBBEC_SDK + this->close(); +#endif +} + +void CameraOrbbecSDK::close() +{ +#ifdef RTABMAP_ORBBEC_SDK + if(imuPipeline_) { + imuPipeline_->stop(); + delete imuPipeline_; + imuPipeline_=nullptr; + } + + if(pipeline_) { + pipeline_->stop(); + delete pipeline_; + pipeline_=nullptr; + } + delete alignFilter_; + alignFilter_ = nullptr; + imuLocalTransform_ = Transform(); + imuLocalTransformInitialized_ = false; + lastAccStamp_ = 0; + lastImageStamp_ = 0; + globalTimestampAvailable_ = false; + model_ = CameraModel(); + imuBuffer_.clear(); +#endif +} + +bool CameraOrbbecSDK::init(const std::string & calibrationFolder, const std::string & cameraName) +{ +#ifdef RTABMAP_ORBBEC_SDK + this->close(); + + std::shared_ptr device; + + ob::Context context; + auto devices = context.queryDeviceList(); + UINFO("%d device(s) found", devices->getCount()); + for(uint32_t i=0; igetCount(); ++i) + { + auto currentDevice = devices->getDevice(i); + auto info = currentDevice->getDeviceInfo(); + + if(deviceId_.find('-') != std::string::npos) + { + // UID + if(deviceId_.compare(info->getUid()) == 0) { + device = currentDevice; + } + } + else if(uSplitNumChar(deviceId_).size() > 1) + { + // Serial + if(deviceId_.compare(info->getSerialNumber()) == 0) { + device = currentDevice; + } + } + else if((deviceId_.empty() && i==0) || + (!deviceId_.empty() && uStr2Int(deviceId_) == (int)i)) { + // Index + device = currentDevice; + } + + std::string type = "Unknown"; + switch(info->getDeviceType()) + { + case OB_STRUCTURED_LIGHT_MONOCULAR_CAMERA: + type = "Structured Light Monocular Camera"; + break; + case OB_STRUCTURED_LIGHT_BINOCULAR_CAMERA: + type = "Structured Light Binocular Camera"; + break; + case OB_TOF_CAMERA: + type = "TOF Camera"; + break; + default: + break; + } + UINFO("Device %ld:", i); + UINFO(" Name: %s", info->getName()); + UINFO(" Type: %s", type.c_str()); + UINFO(" Serial: %s", info->getSerialNumber()); + UINFO(" UID: %s", info->getUid()); + UINFO(" Chip: %s", info->getAsicName()); + UINFO(" Hardware version: %s", info->getHardwareVersion()); + UINFO(" Firmware version: %s", info->getFirmwareVersion()); + } + + if(device.get() == nullptr) { + if(deviceId_.empty()) { + UERROR( "Could not find any orbbec compatible devices! Verify that the " + "camera is correctly connected and the udev rules are installed."); + } + else { + UERROR("Could not find an orbbec device with ID \"%s\"! Verify that the " + "camera is correctly connected and the udev rules are installed. " + "Unset the ID to choose the first camera found."); + } + + return false; + } + + bool hasGyro = false; + bool hasAccel = false; + auto sensors = device->getSensorList(); + + if(device->isGlobalTimestampSupported()) + { + UINFO("Global (host time sync) timestamp is supported."); + device->enableGlobalTimestamp(true); + globalTimestampAvailable_ = true; + } + else + { + UWARN("Global (host time sync) timestamp is not supported! We will use device timestamp, so the camera frames won't be synchronizable with other sensors."); + } + + uint32_t maxColorFps = 0; + uint32_t maxDepthFps = 0; + for(uint32_t i=0; igetCount(); ++i) + { + if(sensors->getSensorType(i) == OB_SENSOR_GYRO) + { + hasGyro = true; + } + if(sensors->getSensorType(i) == OB_SENSOR_ACCEL) + { + hasAccel = true; + } + if( sensors->getSensorType(i) == OB_SENSOR_DEPTH || + sensors->getSensorType(i) == OB_SENSOR_COLOR) + { + auto profiles = sensors->getSensor(i)->getStreamProfileList(); + UINFO("Supported %s profiles:", sensors->getSensorType(i) == OB_SENSOR_DEPTH?"depth":"color"); + for(uint32_t j=0; jgetCount(); ++j) + { + auto profile = profiles->getProfile(j)->as(); + UINFO("Resolution: %ldx%ld, FPS: %ld, Format: %d", + profile->getWidth(), profile->getHeight(), profile->getFps(), profile->getFormat(), j==0?" (default)":""); + + // Get maximum frame rate based on resolution selected + if(sensors->getSensorType(i) == OB_SENSOR_DEPTH) { + if( profile->getFps() > maxDepthFps && + depthWidth_ == profile->getWidth() && + depthHeight_ == profile->getHeight()) + { + maxDepthFps = profile->getFps(); + } + } + else + { + if( profile->getFps() > maxColorFps && + colorWidth_ == profile->getWidth() && + colorHeight_ == profile->getHeight()) + { + maxColorFps = profile->getFps(); + } + } + } + } + } + + // Note that for TOF camera, we want maximum frame rate to better + // sync rgb and depth. For stereo cameras, use the specified frame rate. + if(this->getImageRate()!=0.0f && device->getDeviceInfo()->getDeviceType() != OB_TOF_CAMERA) + { + maxColorFps = maxDepthFps = (unsigned int)this->getImageRate(); + this->setImageRate(0); + } + + std::shared_ptr imuConfig; + if(imuPublished_) + { + if(hasGyro && hasAccel) + { + imuPipeline_ = new ob::Pipeline(device); + imuConfig = std::make_shared(); + imuConfig->enableGyroStream(); + imuConfig->enableAccelStream(); + try { + UINFO("Starting imu pipeline"); + imuPipeline_->start(imuConfig, [&](std::shared_ptr frameSet) { + if(frameSet->getCount() != 2) + { + return; + } + + if(!imuLocalTransformInitialized_) + { + return; + } + + UASSERT(frameSet->getFrame(OB_FRAME_ACCEL) != nullptr && + frameSet->getFrame(OB_FRAME_GYRO) != nullptr); + + auto accel = frameSet->getFrame(OB_FRAME_ACCEL)->as(); + auto gyro = frameSet->getFrame(OB_FRAME_GYRO)->as(); + + uint64_t accelStampUs = globalTimestampAvailable_?accel->getGlobalTimeStampUs():accel->getTimeStampUs(); + uint64_t gyroStampUs = globalTimestampAvailable_?gyro->getGlobalTimeStampUs():gyro->getTimeStampUs(); + + if(accelStampUs != gyroStampUs) + { + UWARN("Received accel and gyro frames with different timestamps (%llu vs %llu), skipping.", + accelStampUs, gyroStampUs); + return; + } + + double accStamp = double(accelStampUs)/1e6; + + if(accelStampUs <= lastAccStamp_) { + return; + } + + lastAccStamp_ = accelStampUs; + + auto accelValue = accel->getValue(); + auto gyroValue = gyro->getValue(); + if(isInterIMUPublishing()) + { + IMU imu(cv::Vec3f(gyroValue.x, gyroValue.y, gyroValue.z), cv::Mat::eye(3,3,CV_64FC1), + cv::Vec3f(accelValue.x, accelValue.y, accelValue.z), cv::Mat::eye(3,3,CV_64FC1), + imuLocalTransform_); + this->postInterIMU(imu, accStamp); + } + else + { + UScopeMutex lock(imuMutex_); + imuBuffer_.emplace_hint(imuBuffer_.end(), accStamp, cv::Vec6f(gyroValue.x, gyroValue.y, gyroValue.z, accelValue.x, accelValue.y, accelValue.z)); + if(imuBuffer_.size()>1000) { + imuBuffer_.erase(imuBuffer_.begin()); + } + } + }); + } + catch(const ob::Error & e) { + UERROR("Unexpected error when configuring IMU stream: %s", e.what()); + } + } + else + { + UWARN("IMU option is enabled but the camera doesn't have an IMU, ignoring."); + } + } + + pipeline_ = new ob::Pipeline(device); + auto config = std::make_shared(); + + // Set highest frame rate possible to reduce color/depth sync diff + config->enableVideoStream(OB_STREAM_COLOR, colorWidth_, colorHeight_, maxColorFps, OB_FORMAT_RGB); + config->enableVideoStream(OB_STREAM_DEPTH, depthWidth_, depthHeight_, maxDepthFps, OB_FORMAT_Y16); + + UINFO("Using color profile: %dx%d", colorWidth_, colorHeight_); + UINFO("Using depth profile: %dx%d", depthWidth_, depthHeight_); + + config->setFrameAggregateOutputMode(OB_FRAME_AGGREGATE_OUTPUT_ALL_TYPE_FRAME_REQUIRE); + + config->setAlignMode(ALIGN_DISABLE); + config->setDepthScaleRequire(true); + + pipeline_->enableFrameSync(); + + try { + UINFO("Starting camera pipeline"); + pipeline_->start(config); + + auto enabledStreams = pipeline_->getConfig()->getEnabledStreamProfileList(); + if(imuPipeline_ != nullptr) { + for(uint32_t i=0; igetCount() && !imuLocalTransformInitialized_; ++i) + { + if(enabledStreams->getProfile(i)->getType() == OB_STREAM_COLOR) + { + auto enabledImuStreams = imuPipeline_->getConfig()->getEnabledStreamProfileList(); + for(uint32_t j=0; jgetCount(); ++j) + { + if(enabledImuStreams->getProfile(j)->getType() == OB_STREAM_ACCEL) + { + auto extrinsics = enabledStreams->getProfile(i)->as()->getExtrinsicTo(enabledImuStreams->getProfile(j)->as()); + // base -> color -> imu + imuLocalTransform_ = this->getLocalTransform() * obToRtabmap(extrinsics); + UINFO("IMU local transform: %s", imuLocalTransform_.prettyPrint().c_str()); + imuLocalTransformInitialized_ = true; + break; + } + } + } + } + } + + std::shared_ptr colorProfile; + std::shared_ptr depthProfile; + for(uint32_t i=0; igetCount(); ++i) + { + if(enabledStreams->getProfile(i)->getType() == OB_STREAM_COLOR) { + colorProfile = enabledStreams->getProfile(i); + } + else if(enabledStreams->getProfile(i)->getType() == OB_STREAM_DEPTH) { + depthProfile = enabledStreams->getProfile(i); + } + } + bool currentSelectionSupportsHwD2C = false; + auto hwD2CSupportedDepthStreamProfiles = pipeline_->getD2CDepthProfileList(colorProfile, ALIGN_D2C_HW_MODE); + if(hwD2CSupportedDepthStreamProfiles->count() == 0) { + UWARN("Current color profile selected doesn't support any hardware depth to color registration. Software registration is done instead."); + } + else + { + auto depthVsp = depthProfile->as(); + auto count = hwD2CSupportedDepthStreamProfiles->getCount(); + for(uint32_t i = 0; i < count; i++) { + auto vsp = hwD2CSupportedDepthStreamProfiles->getProfile(i)->as(); + UINFO("Supported depth to color format: Resolution: %ldx%ld, FPS: %ld, Format: %d", vsp->getWidth(), vsp->getHeight(), vsp->getFps(), vsp->getFormat(), i==0?" (default)":""); + if(vsp->getWidth() == depthVsp->getWidth() && vsp->getHeight() == depthVsp->getHeight() && vsp->getFormat() == depthVsp->getFormat() + && vsp->getFps() == depthVsp->getFps()) { + currentSelectionSupportsHwD2C = true; + } + } + } + if(!currentSelectionSupportsHwD2C) { + UWARN("Hardware depth to color registration cannot be done with the selected color and depth profiles. " + "Software registration is done instead, so more CPU will be needed on the host computer. " + "Set logger level to info to see comptible depth formats for the selected color profile."); + alignFilter_ = new ob::Align(OB_STREAM_COLOR); + alignFilter_->setMatchTargetResolution(false); + } + else { + UINFO("Enabling hardware depth to color registration!"); + config->setAlignMode(ALIGN_D2C_HW_MODE); + config->setDepthScaleRequire(false); + pipeline_->stop(); + pipeline_->start(config); + } + } + catch(const ob::Error & e) + { + UERROR("Configuration not supported! Exception: %s", e.what()); + UERROR("Supported formats:"); + for(uint32_t i=0; igetCount(); ++i) + { + if( sensors->getSensorType(i) == OB_SENSOR_DEPTH || + sensors->getSensorType(i) == OB_SENSOR_COLOR) + { + auto profiles = sensors->getSensor(i)->getStreamProfileList(); + for(uint32_t j=0; jgetCount(); ++j) + { + auto profile = profiles->getProfile(j)->as(); + UERROR("%sResolution: %ldx%ld, FPS: %ld, Format: %d", + sensors->getSensorType(i) == OB_SENSOR_DEPTH?"Depth":"Color", + profile->getWidth(), + profile->getHeight(), + profile->getFps(), + profile->getFormat(), + j==0?" (default)":""); + } + } + } + return false; + } + + return true; +#else + UERROR("CameraOrbbecSDK: RTAB-Map is not built with Orbbec SDK support!"); + return false; +#endif +} + +bool CameraOrbbecSDK::isCalibrated() const +{ + return true; +} + +std::string CameraOrbbecSDK::getSerial() const +{ +#ifdef RTABMAP_ORBBEC_SDK + if(pipeline_) { + return pipeline_->getDevice()->getDeviceInfo()->getSerialNumber(); + } +#endif + return ""; +} + +void CameraOrbbecSDK::enableColorRectification(bool enabled) +{ +#ifdef RTABMAP_ORBBEC_SDK + rectifyColor_ = enabled; +#endif +} + +void CameraOrbbecSDK::enableImu(bool enabled) +{ +#ifdef RTABMAP_ORBBEC_SDK + imuPublished_ = enabled; +#endif +} + +void CameraOrbbecSDK::enableDepthMM(bool enabled) +{ +#ifdef RTABMAP_ORBBEC_SDK + convertDepthToMM_ = enabled; +#endif +} + +SensorData CameraOrbbecSDK::captureImage(SensorCaptureInfo * info) +{ + SensorData data; + +#ifdef RTABMAP_ORBBEC_SDK + if(!pipeline_) { + UERROR("Camera is not initialized!"); + return data; + } + auto frameset = pipeline_->waitForFrameset(); + if(frameset == nullptr || frameset->getCount() == 0) { + UWARN("No frame received!"); + return data; + } + if(frameset->getCount() != 2) { + UWARN("Received %s frames, expecting 2!", frameset->getCount()); + return data; + } + + if(alignFilter_ != nullptr) { + // Software depth to color registration + frameset = alignFilter_->process(frameset)->as(); + UASSERT(frameset != nullptr); + } + + auto colorFrame = frameset->getFrame(OB_FRAME_COLOR); + UASSERT(colorFrame != nullptr); + + auto depthFrame = frameset->getFrame(OB_FRAME_DEPTH); + UASSERT(depthFrame != nullptr); + + auto colorVideoFrame = colorFrame->as(); + auto depthVideoFrame = depthFrame->as(); + + cv::Mat rgb = obColorFrameToCv(*colorVideoFrame); + cv::Mat depth = obDepthFrameToCv(*depthVideoFrame); + + if(rgb.empty()) { + UERROR("Could not convert the color frame! Type=%d Format=%d", colorFrame->getType(), colorVideoFrame->getFormat()); + } + else if(depth.empty()) { + UERROR("Could not convert the depth frame! Type=%d Format=%d", depthFrame->getType(), depthVideoFrame->getFormat()); + } + else if(!rgb.empty() && !depth.empty()) + { + if(!model_.isValidForProjection()) + { + auto streamProfile = colorFrame->getStreamProfile(); + auto videoStreamProfile = streamProfile->as(); + + auto intrinsics = videoStreamProfile->getIntrinsic(); + model_ = CameraModel( + getSerial(), + cv::Size(intrinsics.width, intrinsics.height), + obIntrinsicToK(intrinsics), + obDistortionToD(videoStreamProfile->getDistortion()), + cv::Mat::eye(3,3,CV_64FC1), + obIntrinsicToP(intrinsics), + this->getLocalTransform()); + if(rectifyColor_ && !model_.initRectificationMap()) { + UWARN("Could not initialize rectification map, color images won't be rectified."); + } + } + + if(rectifyColor_ && model_.isValidForRectification()) + { + rgb = model_.rectifyImage(rgb); + } + + if(convertDepthToMM_) + { + depth = util2d::cvtDepthFromFloat(depth); + } + + uint64_t colorStampUs = globalTimestampAvailable_?colorFrame->getGlobalTimeStampUs():colorFrame->getTimeStampUs(); + uint64_t depthStampUs = globalTimestampAvailable_?depthFrame->getGlobalTimeStampUs():depthFrame->getTimeStampUs(); + double colorStamp = double(colorStampUs) / 1e6; + double depthStamp = double(depthStampUs) / 1e6; + if(fabs(colorStamp - depthStamp) > 0.018) { + // The difference seems varying between 0 and 17 ms normally + UWARN("Large timestamp difference (%fs) between color (%f) and depth (%f) frames. " + "Depth registration would be wrong on fast motion.", + colorStamp - depthStamp, colorStamp, depthStamp); + } + + uint64_t stampUs = colorStampUs < depthStampUs ? colorStampUs : depthStampUs; + +#ifdef WIN32 + // On Windows, there is an issue that timestamps are not populated by default without following instructions from: + // https://github.com/orbbec/OrbbecSDK_v2/blob/main/scripts/env_setup/obsensor_metadata_win10.md + // Detect if the consecutive timestamps are identical, then send error! + if (stampUs <= lastImageStamp_) + { + UERROR("We detected non-consecutive timestamps, make sure you applied the fix from https://github.com/orbbec/OrbbecSDK_v2/blob/main/scripts/env_setup/obsensor_metadata_win10.md ."); + } + lastImageStamp_ = stampUs; +#endif + double stamp = double(stampUs)/1e6; + + data = SensorData(rgb, depth, model_, this->getNextSeqID(), stamp); + + if(imuPublished_ && !imuBuffer_.empty() && !this->isInterIMUPublishing()) + { + cv::Vec6f imuVec; + std::map::const_iterator iterA, iterB; + + imuMutex_.lock(); + int maximumTries = 10; + while(imuBuffer_.rbegin()->first < stamp && maximumTries-- > 0) + { + imuMutex_.unlock(); + uSleep(1); + imuMutex_.lock(); + } + + if(imuBuffer_.rbegin()->first < stamp) + { + UWARN("Could not get IMU data at request image stamp %f after waiting 10 ms, latest imu stamp is %f", stamp, imuBuffer_.rbegin()->first); + imuMutex_.unlock(); + } + else + { + // Interpolate imu data on image stamp + iterB = imuBuffer_.lower_bound(stamp); + iterA = iterB; + if(iterA != imuBuffer_.begin()) + iterA = --iterA; + if(iterA == iterB || stamp == iterB->first) + { + imuVec = iterB->second; + } + else if(stamp > iterA->first && stamp < iterB->first) + { + float t = (stamp-iterA->first) / (iterB->first-iterA->first); + imuVec = iterA->second + t*(iterB->second - iterA->second); + } + imuBuffer_.erase(imuBuffer_.begin(), iterB); + + imuMutex_.unlock(); + data.setIMU(IMU(cv::Vec3d(imuVec[0], imuVec[1], imuVec[2]), cv::Mat::eye(3, 3, CV_64FC1), cv::Vec3d(imuVec[3], imuVec[4], imuVec[5]), cv::Mat::eye(3, 3, CV_64FC1), imuLocalTransform_)); + } + } + } + +#else + UERROR("CameraOrbbecSDK: RTAB-Map is not built with Orbbec SDK support!"); +#endif + return data; +} + +} // namespace rtabmap diff --git a/corelib/src/camera/CameraRealSense2.cpp b/corelib/src/camera/CameraRealSense2.cpp index 4ddfc775..fb15b0c3 100644 --- a/corelib/src/camera/CameraRealSense2.cpp +++ b/corelib/src/camera/CameraRealSense2.cpp @@ -30,6 +30,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include +#include #include #ifdef RTABMAP_REALSENSE2 @@ -78,7 +79,8 @@ CameraRealSense2::CameraRealSense2( cameraDepthFps_(30), globalTimeSync_(true), dualMode_(false), - closing_(false) + closing_(false), + playback_(false) #endif { UDEBUG(""); @@ -130,6 +132,7 @@ void CameraRealSense2::close() } closing_ = false; + playback_ = false; } void CameraRealSense2::imu_callback(rs2::frame frame) @@ -492,64 +495,76 @@ bool CameraRealSense2::init(const std::string & calibrationFolder, const std::st clockSyncWarningShown_ = false; imuGlobalSyncWarningShown_ = false; - rs2::device_list list = ctx_.query_devices(); - if (0 == list.size()) + if(uStrContains(deviceId_, ".bag")) { - UERROR("No RealSense2 devices were found!"); - return false; + // playback (bag recorded by realsense-viewer) + dev_.resize(1); + dev_[0] = ctx_.load_device(uReplaceChar(deviceId_, '~', UDirectory::homeDir())); + playback_ = true; + UINFO("Device ID is a bag (\"%s\"), using playback mode", deviceId_.c_str()); } - - bool found=false; - try + else { - for (rs2::device dev : list) + rs2::device_list list = ctx_.query_devices(); + if (0 == list.size()) { - auto sn = dev.get_info(RS2_CAMERA_INFO_SERIAL_NUMBER); - auto pid_str = dev.get_info(RS2_CAMERA_INFO_PRODUCT_ID); - auto name = dev.get_info(RS2_CAMERA_INFO_NAME); + UERROR("No RealSense2 devices were found!"); + return false; + } - uint16_t pid; - std::stringstream ss; - ss << std::hex << pid_str; - ss >> pid; - UINFO("Device \"%s\" with serial number %s was found with product ID=%d.", name, sn, (int)pid); - if(dualMode_ && pid == 0x0B37) + bool found=false; + try + { + for (rs2::device dev : list) { - // Dual setup: device[0] = D400, device[1] = T265 - // T265 - dev_.resize(2); - dev_[1] = dev; - } - else if (!found && (deviceId_.empty() || deviceId_ == sn || uStrContains(name, uToUpperCase(deviceId_)))) - { - if(dev_.empty()) + auto sn = dev.get_info(RS2_CAMERA_INFO_SERIAL_NUMBER); + auto pid_str = dev.get_info(RS2_CAMERA_INFO_PRODUCT_ID); + auto name = dev.get_info(RS2_CAMERA_INFO_NAME); + + uint16_t pid; + std::stringstream ss; + ss << std::hex << pid_str; + ss >> pid; + UINFO("Device \"%s\" with serial number %s was found with product ID=%d.", name, sn, (int)pid); + if(dualMode_ && pid == 0x0B37) { - dev_.resize(1); + // Dual setup: device[0] = D400, device[1] = T265 + // T265 + dev_.resize(2); + dev_[1] = dev; + } + else if (!found && (deviceId_.empty() || deviceId_ == sn || uStrContains(name, uToUpperCase(deviceId_)))) + { + if(dev_.empty()) + { + dev_.resize(1); + } + dev_[0] = dev; + found=true; } - dev_[0] = dev; - found=true; } } - } - catch(const rs2::error & error) - { - UWARN("%s. Is the camera already used with another app?", error.what()); + catch(const rs2::error & error) + { + UWARN("%s. Is the camera already used with another app?", error.what()); + } + + if (!found) + { + if(dualMode_ && dev_.size()==2) + { + UERROR("Dual setup is enabled, but a D400 camera is not detected!"); + dev_.clear(); + } + else + { + UERROR("The requested device \"%s\" is NOT found!", deviceId_.c_str()); + } + return false; + } } - if (!found) - { - if(dualMode_ && dev_.size()==2) - { - UERROR("Dual setup is enabled, but a D400 camera is not detected!"); - dev_.clear(); - } - else - { - UERROR("The requested device \"%s\" is NOT found!", deviceId_.c_str()); - } - return false; - } - else if(dualMode_ && dev_.size()!=2) + if(dualMode_ && dev_.size()!=2) { UERROR("Dual setup is enabled, but a T265 camera is not detected!"); dev_.clear(); @@ -634,7 +649,14 @@ bool CameraRealSense2::init(const std::string & calibrationFolder, const std::st sensors[1] = elem; if(sensors[1].supports(rs2_option::RS2_OPTION_EMITTER_ENABLED)) { - sensors[1].set_option(rs2_option::RS2_OPTION_EMITTER_ENABLED, emitterEnabled_); + if(!sensors[1].is_option_read_only(rs2_option::RS2_OPTION_EMITTER_ENABLED)) + { + sensors[1].set_option(rs2_option::RS2_OPTION_EMITTER_ENABLED, emitterEnabled_); + } + else if(!emitterEnabled_) + { + UWARN("rs2_option::RS2_OPTION_EMITTER_ENABLED option is read-only, cannot disable IR emitter."); + } } } else if ("Coded-Light Depth Sensor" == module_name) @@ -712,14 +734,14 @@ bool CameraRealSense2::init(const std::string & calibrationFolder, const std::st for (auto& profile : profiles) { auto video_profile = profile.as(); - UINFO("%s %d %d %d %d %s type=%d", rs2_format_to_string( - video_profile.format()), - video_profile.width(), - video_profile.height(), - video_profile.fps(), - video_profile.stream_index(), - video_profile.stream_name().c_str(), - video_profile.stream_type()); + UINFO("%s %d %d %d %d %s type=%d", + rs2_format_to_string(profile.format()), + video_profile.get()?video_profile.width():-1, + video_profile.get()?video_profile.height():-1, + profile.fps(), + profile.stream_index(), + profile.stream_name().c_str(), + profile.stream_type()); } } int pi = 0; @@ -728,10 +750,12 @@ bool CameraRealSense2::init(const std::string & calibrationFolder, const std::st auto video_profile = profile.as(); if(!stereo) { - if( (video_profile.width() == cameraWidth_ && + if( (video_profile.get() && + video_profile.width() == cameraWidth_ && video_profile.height() == cameraHeight_ && video_profile.fps() == cameraFps_) || - (strcmp(sensors[i].get_info(RS2_CAMERA_INFO_NAME), "L500 Depth Sensor")==0 && + ((strcmp(sensors[i].get_info(RS2_CAMERA_INFO_NAME), "L500 Depth Sensor")==0 || + (playback_ && strcmp(sensors[i].get_info(RS2_CAMERA_INFO_NAME), "Stereo Module")==0)) && video_profile.width() == cameraDepthWidth_ && video_profile.height() == cameraDepthHeight_ && video_profile.fps() == cameraDepthFps_)) @@ -740,7 +764,7 @@ bool CameraRealSense2::init(const std::string & calibrationFolder, const std::st // rgb or ir left if((!ir_ && video_profile.format() == RS2_FORMAT_RGB8 && video_profile.stream_type() == RS2_STREAM_COLOR) || - (ir_ && video_profile.format() == RS2_FORMAT_Y8 && (video_profile.stream_index() == 1 || isL500))) + (ir_ && video_profile.format() == RS2_FORMAT_Y8 && (video_profile.stream_index() == 1 || isL500))) { if(!profilesPerSensor[i].empty()) { @@ -778,7 +802,7 @@ bool CameraRealSense2::init(const std::string & calibrationFolder, const std::st } } } - else if(video_profile.format() == RS2_FORMAT_MOTION_XYZ32F || video_profile.format() == RS2_FORMAT_6DOF) + else if(profile.format() == RS2_FORMAT_MOTION_XYZ32F || profile.format() == RS2_FORMAT_6DOF) { //D435i: //MOTION_XYZ32F 0 0 200 (gyro) @@ -817,6 +841,7 @@ bool CameraRealSense2::init(const std::string & calibrationFolder, const std::st { //T265: if(!dualMode_ && + video_profile.get() && video_profile.format() == RS2_FORMAT_Y8 && video_profile.width() == 848 && video_profile.height() == 800 && @@ -865,7 +890,7 @@ bool CameraRealSense2::init(const std::string & calibrationFolder, const std::st } added = true; } - else if(video_profile.format() == RS2_FORMAT_MOTION_XYZ32F || video_profile.format() == RS2_FORMAT_6DOF) + else if(profile.format() == RS2_FORMAT_MOTION_XYZ32F || profile.format() == RS2_FORMAT_6DOF) { //MOTION_XYZ32F 0 0 200 //MOTION_XYZ32F 0 0 62 @@ -878,20 +903,29 @@ bool CameraRealSense2::init(const std::string & calibrationFolder, const std::st } if (!added) { - UERROR("Given stream configuration is not supported by the device! " - "Stream Index: %d, Width: %d, Height: %d, FPS: %d", i, cameraWidth_, cameraHeight_, cameraFps_); + if(strcmp(sensors[i].get_info(RS2_CAMERA_INFO_NAME), "L500 Depth Sensor")==0 || + (playback_ && strcmp(sensors[i].get_info(RS2_CAMERA_INFO_NAME), "Stereo Module")==0)) + { + UERROR("Given stream configuration is not supported by the device! " + "Stream Index: %d, Width: %d, Height: %d, FPS: %d", i, cameraDepthWidth_, cameraDepthHeight_, cameraDepthFps_); + } + else + { + UERROR("Given stream configuration is not supported by the device! " + "Stream Index: %d, Width: %d, Height: %d, FPS: %d", i, cameraWidth_, cameraHeight_, cameraFps_); + } UERROR("Available configurations:"); for (auto& profile : profiles) { auto video_profile = profile.as(); - UERROR("%s %d %d %d %d %s type=%d", rs2_format_to_string( - video_profile.format()), - video_profile.width(), - video_profile.height(), - video_profile.fps(), - video_profile.stream_index(), - video_profile.stream_name().c_str(), - video_profile.stream_type()); + UERROR("%s %d %d %d %d %s type=%d", + rs2_format_to_string(profile.format()), + video_profile.get()?video_profile.width():-1, + video_profile.get()?video_profile.height():-1, + profile.fps(), + profile.stream_index(), + profile.stream_name().c_str(), + profile.stream_type()); } return false; } @@ -1075,19 +1109,26 @@ bool CameraRealSense2::init(const std::string & calibrationFolder, const std::st { auto video_profile = profilesPerSensor[i][j].as(); UINFO("Opening: %s %d %d %d %d %s type=%d", rs2_format_to_string( - video_profile.format()), - video_profile.width(), - video_profile.height(), - video_profile.fps(), - video_profile.stream_index(), - video_profile.stream_name().c_str(), - video_profile.stream_type()); + profilesPerSensor[i][j].format()), + video_profile.get()?video_profile.width():-1, + video_profile.get()?video_profile.height():-1, + profilesPerSensor[i][j].fps(), + profilesPerSensor[i][j].stream_index(), + profilesPerSensor[i][j].stream_name().c_str(), + profilesPerSensor[i][j].stream_type()); } if(globalTimeSync_ && sensors[i].supports(rs2_option::RS2_OPTION_GLOBAL_TIME_ENABLED)) { - float value = sensors[i].get_option(rs2_option::RS2_OPTION_GLOBAL_TIME_ENABLED); - UINFO("Set RS2_OPTION_GLOBAL_TIME_ENABLED=1 (was %f) for sensor %d", value, (int)i); - sensors[i].set_option(rs2_option::RS2_OPTION_GLOBAL_TIME_ENABLED, 1); + float value = sensors[i].get_option(rs2_option::RS2_OPTION_GLOBAL_TIME_ENABLED); + UINFO("Set RS2_OPTION_GLOBAL_TIME_ENABLED=1 (was %f) for sensor %d", value, (int)i); + if(!sensors[i].is_option_read_only(rs2_option::RS2_OPTION_GLOBAL_TIME_ENABLED)) + { + sensors[i].set_option(rs2_option::RS2_OPTION_GLOBAL_TIME_ENABLED, 1); + } + else if(value != 1) + { + UWARN("rs2_option::RS2_OPTION_GLOBAL_TIME_ENABLED option is read-only, cannot enable it."); + } } sensors[i].open(profilesPerSensor[i]); if(sensors[i].is()) @@ -1525,11 +1566,21 @@ SensorData CameraRealSense2::captureImage(SensorCaptureInfo * info) else { UERROR("Missing frames (received %d, needed=%d)", (int)frameset.size(), desiredFramesetSize); + if(frameset.size()>0) + { + for (auto it = frameset.begin(); it != frameset.end(); ++it) + { + UERROR("Received frame only from %s", (*it).get_profile().stream_name().c_str()); + } + } } } catch(const std::exception& ex) { - UERROR("An error has occurred during frame callback: %s", ex.what()); + if(!playback_) + { + UERROR("An error has occurred during frame callback: %s", ex.what()); + } } #else UERROR("CameraRealSense2: RTAB-Map is not built with RealSense2 support!"); diff --git a/corelib/src/global_map/OccupancyGrid.cpp b/corelib/src/global_map/OccupancyGrid.cpp index 7bc0db0f..0590bc45 100644 --- a/corelib/src/global_map/OccupancyGrid.cpp +++ b/corelib/src/global_map/OccupancyGrid.cpp @@ -92,7 +92,6 @@ void OccupancyGrid::clear() { map_ = cv::Mat(); mapInfo_ = cv::Mat(); - cellCount_.clear(); GlobalMap::clear(); } @@ -223,14 +222,14 @@ void OccupancyGrid::assemble(const std::list > & newPo if(!cache().empty()) { - UDEBUG("Updating from cache"); + UDEBUG("Updating %ld poses from cache", newPoses.size()); for(std::list >::const_iterator iter = newPoses.begin(); iter!=newPoses.end(); ++iter) { if(uContains(cache(), iter->first)) { const LocalGrid & localGrid = cache().at(iter->first); - UDEBUG("Adding grid %d: ground=%d obstacles=%d empty=%d", iter->first, localGrid.groundCells.cols, localGrid.obstacleCells.cols, localGrid.emptyCells.cols); + //UDEBUG("Adding grid %d: ground=%d obstacles=%d empty=%d", iter->first, localGrid.groundCells.cols, localGrid.obstacleCells.cols, localGrid.emptyCells.cols); //ground cv::Mat ground; @@ -433,11 +432,6 @@ void OccupancyGrid::assemble(const std::list > & newPo if(iter != emptyLocalMaps.end() || jter!=occupiedLocalMaps.end()) { addAssembledNode(kter->first, kter->second); - std::map >::iterator cter = cellCount_.find(kter->first); - if(cter == cellCount_.end() && kter->first > 0) - { - cter = cellCount_.insert(std::make_pair(kter->first, std::pair(0,0))).first; - } if(iter!=emptyLocalMaps.end()) { for(int i=0; isecond.cols; ++i) @@ -459,30 +453,12 @@ void OccupancyGrid::assemble(const std::list > & newPo // cannot rewrite on cells referred by more recent nodes continue; } - if(nodeId > 0) - { - std::map >::iterator eter = cellCount_.find(nodeId); - UASSERT_MSG(eter != cellCount_.end(), uFormat("current pose=%d nodeId=%d", kter->first, nodeId).c_str()); - if(value == 0) - { - eter->second.first -= 1; - } - else if(value == 100) - { - eter->second.second -= 1; - } - if(kter->first < 0) - { - eter->second.first += 1; - } - } } if(kter->first > 0) { info[0] = (float)kter->first; info[1] = ptf[0]; info[2] = ptf[1]; - cter->second.first+=1; } value = 0; // free space @@ -533,23 +509,6 @@ void OccupancyGrid::assemble(const std::list > & newPo // cannot rewrite on cells referred by more recent nodes continue; } - if(nodeId>0) - { - std::map >::iterator eter = cellCount_.find(nodeId); - UASSERT_MSG(eter != cellCount_.end(), uFormat("current pose=%d nodeId=%d", kter->first, nodeId).c_str()); - if(value == 0) - { - eter->second.first -= 1; - } - else if(value == 100) - { - eter->second.second -= 1; - } - if(kter->first < 0) - { - eter->second.first += 1; - } - } } if(kter->first > 0) { @@ -557,7 +516,6 @@ void OccupancyGrid::assemble(const std::list > & newPo info[1] = float(i) * cellSize_ + xMin; info[2] = float(j) * cellSize_ + yMin; info[3] = logOddsClampingMin_; - cter->second.first+=1; } value = -2; // free space (footprint) } @@ -585,30 +543,12 @@ void OccupancyGrid::assemble(const std::list > & newPo // cannot rewrite on cells referred by more recent nodes continue; } - if(nodeId>0) - { - std::map >::iterator eter = cellCount_.find(nodeId); - UASSERT_MSG(eter != cellCount_.end(), uFormat("current pose=%d nodeId=%d", kter->first, nodeId).c_str()); - if(value == 0) - { - eter->second.first -= 1; - } - else if(value == 100) - { - eter->second.second -= 1; - } - if(kter->first < 0) - { - eter->second.second += 1; - } - } } if(kter->first > 0) { info[0] = (float)kter->first; info[1] = ptf[0]; info[2] = ptf[1]; - cter->second.second+=1; } // update odds @@ -651,20 +591,6 @@ void OccupancyGrid::assemble(const std::list > & newPo mapInfo_ = mapInfo; minValues_[0] = xMin; minValues_[1] = yMin; - - // clean cellCount_ - for(std::map >::iterator iter= cellCount_.begin(); iter!=cellCount_.end();) - { - UASSERT(iter->second.first >= 0 && iter->second.second >= 0); - if(iter->second.first == 0 && iter->second.second == 0) - { - cellCount_.erase(iter++); - } - else - { - ++iter; - } - } } } @@ -677,7 +603,6 @@ unsigned long OccupancyGrid::getMemoryUsed() const memoryUsage += map_.total() * map_.elemSize(); memoryUsage += mapInfo_.total() * mapInfo_.elemSize(); - memoryUsage += cellCount_.size()*(sizeof(int)*3 + sizeof(std::pair) + sizeof(std::map >::iterator)) + sizeof(std::map >); return memoryUsage; } diff --git a/corelib/src/odometry/OdometryCuVSLAM.cpp b/corelib/src/odometry/OdometryCuVSLAM.cpp new file mode 100644 index 00000000..14b9384f --- /dev/null +++ b/corelib/src/odometry/OdometryCuVSLAM.cpp @@ -0,0 +1,1101 @@ +/* +Copyright (c) 2025 Felix Toft +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/odometry/OdometryCuVSLAM.h" +#include "rtabmap/core/OdometryInfo.h" +#include "rtabmap/utilite/ULogger.h" +#include "rtabmap/utilite/UTimer.h" +#include + +#ifdef RTABMAP_CUVSLAM +#include "rtabmap/core/CameraModel.h" +#include "rtabmap/core/StereoCameraModel.h" +#include "rtabmap/core/SensorData.h" +#include "rtabmap/core/Transform.h" +#include "rtabmap/core/util3d_transforms.h" +#include +#include +#include +#include +#include + +// ============================================================================ +// Coordinate System Transformation Constants +// Based on Isaac ROS implementation: +// Source: isaac_ros_visual_slam/include/isaac_ros_visual_slam/impl/cuvslam_ros_conversion.hpp +// ============================================================================ + +// Transformation converting from +// Canonical ROS Frame (x-forward, y-left, z-up) to +// cuVSLAM Frame (x-right, y-up, z-backward) +// x -> -z +// y -> -x +// z -> y +const rtabmap::Transform cuvslam_pose_canonical( + 0, -1, 0, 0, + 0, 0, 1, 0, + -1, 0, 0, 0 +); + +// Transformation converting from +// cuVSLAM Frame (x-right, y-up, z-backward) to +// Canonical ROS Frame (x-forward, y-left, z-up) +const rtabmap::Transform canonical_pose_cuvslam = cuvslam_pose_canonical.inverse(); + +// Transformation converting from +// Optical Frame (x-right, y-down, z-forward) to +// cuVSLAM Frame (x-right, y-up, z-backward) +// Optical -> cuVSLAM +// x -> x +// y -> -y +// z -> -z +const rtabmap::Transform cuvslam_pose_optical( + 1, 0, 0, 0, + 0, -1, 0, 0, + 0, 0, -1, 0 +); + +// Transformation converting from +// cuVSLAM Frame (x-right, y-up, z-backward) to +// Optical Frame (x-right, y-down, z-forward) +const rtabmap::Transform optical_pose_cuvslam = cuvslam_pose_optical.inverse(); + + +// ============================================================================ +// Forward Declarations +// ============================================================================ + +namespace rtabmap { + +bool initializeCuVSLAM(const SensorData & data, + CUVSLAM_TrackerHandle & cuvslam_handle, + CUVSLAM_GroundConstraintHandle & ground_constraint_handle, + bool planar_constraints, + int multicam_mode, + std::vector & gpu_left_image_data, + std::vector & gpu_right_image_data, + std::vector & gpu_left_image_sizes, + std::vector & gpu_right_image_sizes, + std::vector & cuvslam_cameras, + std::vector> & intrinsics, + cudaStream_t & cuda_stream); + +CUVSLAM_Configuration CreateConfiguration(const SensorData & data, int multicam_mode); + +bool prepareImages(const SensorData & data, + std::vector & cuvslam_images, + std::vector & gpu_left_image_data, + std::vector & gpu_right_image_data, + std::vector & gpu_left_image_sizes, + std::vector & gpu_right_image_sizes, + cudaStream_t & cuda_stream); + +cv::Mat convertCuVSLAMCovariance(const float * cuvslam_covariance, bool use_raw_covariance); + + +// ============================================================================ +// Transform Conversion Functions and Misc Helpers +// ============================================================================ + +// Helper function that converts RTAB-Map Transform into CUVSLAM_Pose +CUVSLAM_Pose TocuVSLAMPose(const Transform & rtabmap_transform) +{ + CUVSLAM_Pose cuvslamPose; + // RTAB-Map Transform is row major, but cuVSLAM is column major + // We need to transpose the rotation matrix when converting + const int32_t kRotationMatCol = 3; + const int32_t kRotationMatRow = 3; + int cuvslam_idx = 0; + for (int col_idx = 0; col_idx < kRotationMatCol; ++col_idx) { + for (int row_idx = 0; row_idx < kRotationMatRow; ++row_idx) { + // Access RTAB-Map Transform as (row, col) but store in column-major order for cuVSLAM + cuvslamPose.r[cuvslam_idx] = rtabmap_transform(row_idx, col_idx); + cuvslam_idx++; + } + } + + cuvslamPose.t[0] = rtabmap_transform.x(); + cuvslamPose.t[1] = rtabmap_transform.y(); + cuvslamPose.t[2] = rtabmap_transform.z(); + return cuvslamPose; +} + +// Helper function to convert cuVSLAM pose to RTAB-Map Transform +Transform FromcuVSLAMPose(const CUVSLAM_Pose & cuvslam_pose) +{ + const auto & r = cuvslam_pose.r; + const auto & t = cuvslam_pose.t; + // RTAB-Map Transform is row major and cuVSLAM rotation mat is column major. + Transform rtabmap_transform( + r[0], r[3], r[6], t[0], // r11, r12, r13, tx + r[1], r[4], r[7], t[1], // r21, r22, r23, ty + r[2], r[5], r[8], t[2] // r31, r32, r33, tz + ); + + return rtabmap_transform; +} + +} // namespace rtabmap + +#endif + +// ============================================================================ +// OdometryCuVSLAM Class Implementation +// ============================================================================ + +namespace rtabmap { + +OdometryCuVSLAM::OdometryCuVSLAM(const ParametersMap & parameters) : + Odometry(parameters) +#ifdef RTABMAP_CUVSLAM + , + cuvslam_handle_(nullptr), + ground_constraint_handle_(nullptr), + initialized_(false), + lost_(false), + tracking_(false), + planar_constraints_(false), + multicam_mode_(0), + previous_pose_(Transform::getIdentity()), + last_timestamp_(-1.0), + observations_(5000), + landmarks_(5000), + gpu_left_image_data_(), + gpu_right_image_data_(), + gpu_left_image_sizes_(), + gpu_right_image_sizes_(), + cuda_stream_(nullptr) +#endif +{ +#ifdef RTABMAP_CUVSLAM + Parameters::parse(parameters, Parameters::kRegForce3DoF(), planar_constraints_); + Parameters::parse(parameters, Parameters::kOdomCuVSLAMMulticamMode(), multicam_mode_); + UASSERT(multicam_mode_ >= 0 && multicam_mode_ <= 2); + UINFO("%s=%d", Parameters::kOdomCuVSLAMMulticamMode().c_str(), multicam_mode_); + // Warm up GPU and create CUDA context before tracker initialization + // Supposedly this will speed up the tracker initialization + CUVSLAM_WarmUpGPU(); +#endif +} + +OdometryCuVSLAM::~OdometryCuVSLAM() +{ +#ifdef RTABMAP_CUVSLAM + // Clean up cuVSLAM handles + if(cuvslam_handle_) + { + CUVSLAM_DestroyTracker(cuvslam_handle_); + } + if(ground_constraint_handle_){ + CUVSLAM_GroundConstraintDestroy(ground_constraint_handle_); + } + + // Clean up GPU memory + for(uint8_t * gpu_ptr : gpu_left_image_data_) { + if(gpu_ptr) { + cudaFree(gpu_ptr); + } + } + for(uint8_t * gpu_ptr : gpu_right_image_data_) { + if(gpu_ptr) { + cudaFree(gpu_ptr); + } + } + if(cuda_stream_) { + cudaStreamDestroy(cuda_stream_); + cuda_stream_ = nullptr; + } +#endif +} + +void OdometryCuVSLAM::reset(const Transform & initialPose) +{ + Odometry::reset(initialPose); + +#ifdef RTABMAP_CUVSLAM + this->cleanupCuVSLAMResources(); +#endif +} + +void OdometryCuVSLAM::cleanupCuVSLAMResources() +{ +#ifdef RTABMAP_CUVSLAM + // Clean up cuVSLAM handles + if(cuvslam_handle_) + { + CUVSLAM_DestroyTracker(cuvslam_handle_); + cuvslam_handle_ = nullptr; + } + if(ground_constraint_handle_){ + CUVSLAM_GroundConstraintDestroy(ground_constraint_handle_); + ground_constraint_handle_ = nullptr; + } + + // Clean up GPU memory + for(uint8_t * gpu_ptr : gpu_left_image_data_) { + if(gpu_ptr) { + cudaFree(gpu_ptr); + } + } + gpu_left_image_data_.clear(); + for(uint8_t * gpu_ptr : gpu_right_image_data_) { + if(gpu_ptr) { + cudaFree(gpu_ptr); + } + } + gpu_right_image_data_.clear(); + if(cuda_stream_) { + cudaStreamDestroy(cuda_stream_); + cuda_stream_ = nullptr; + } + + // Reset our internal state variables + gpu_left_image_sizes_.clear(); + gpu_right_image_sizes_.clear(); + cuvslam_cameras_.clear(); + intrinsics_.clear(); + initialized_ = false; + lost_ = false; + tracking_ = false; + previous_pose_ = Transform::getIdentity(); + last_timestamp_ = -1.0; +#endif +} + +Transform OdometryCuVSLAM::computeTransform( + SensorData & data, + const Transform & guess, + OdometryInfo * info) +{ +#ifdef RTABMAP_CUVSLAM + UTimer timer; + + UDEBUG("=== computeTransform ENTRY === lost_=%s, tracking_=%s, initialized_=%s", + lost_ ? "true" : "false", + tracking_ ? "true" : "false", + initialized_ ? "true" : "false"); + + // If we are lost after tracking has begun, return null transform + // We wait until a reset is triggered. + if(lost_ && tracking_) { + UDEBUG("EARLY EXIT: lost_ && tracking_ is true, returning null"); + if(info) { + info->reg.covariance = cv::Mat::eye(6, 6, CV_64FC1) * 9999.0; + info->timeEstimation = timer.ticks(); + } + return Transform(); + } + + // Check if we have valid image data + if(data.imageRaw().empty() || data.rightRaw().empty()) + { + UERROR("cuVSLAM odometry only works with stereo cameras! It requires both left and right images! Left: %s, Right: %s", + data.imageRaw().empty() ? "empty" : "ok", + data.rightRaw().empty() ? "empty" : "ok"); + return Transform(); + } + + // Check if we have valid stereo camera models + if(data.stereoCameraModels().size() == 0) + { + UERROR("cuVSLAM odometry requires stereo camera models!"); + return Transform(); + } + + // Initialize cuVSLAM tracker on first frame + if(!initialized_) + { + if(!initializeCuVSLAM( + data, + cuvslam_handle_, + ground_constraint_handle_, + planar_constraints_, + multicam_mode_, + gpu_left_image_data_, + gpu_right_image_data_, + gpu_left_image_sizes_, + gpu_right_image_sizes_, + cuvslam_cameras_, + intrinsics_, + cuda_stream_)) + { + UERROR("Failed to initialize cuVSLAM tracker"); + return Transform(); + } + } + + // Prepare images for cuVSLAM + std::vector cuvslam_image_objects; + if(!prepareImages( + data, + cuvslam_image_objects, + gpu_left_image_data_, + gpu_right_image_data_, + gpu_left_image_sizes_, + gpu_right_image_sizes_, + cuda_stream_)) + { + UERROR("Failed to prepare images for cuVSLAM"); + return Transform(); + } + + // Not using the IMU yet + if(!data.imu().empty()) + { + UWARN("IMU data available but processing not implemented yet"); + } + + // Validate images and tracker status + if(cuvslam_image_objects.empty()) { + UERROR("No images prepared for cuVSLAM tracking"); + return Transform(); + } + if(!cuvslam_handle_) { + UERROR("cuVSLAM tracker is null! initialized_: %s", initialized_ ? "true" : "false"); + return Transform(); + } + + // Process wheel odom pose if available + CUVSLAM_Pose * predicted_pose_ptr = nullptr; + CUVSLAM_Pose predicted_pose; + if(!guess.isNull()) { + Transform absolute_guess = previous_pose_ * guess; + absolute_guess = cuvslam_pose_canonical * absolute_guess * canonical_pose_cuvslam; + predicted_pose = TocuVSLAMPose(absolute_guess); + predicted_pose_ptr = &predicted_pose; + } + + CUVSLAM_PoseEstimate vo_pose_estimate; + const CUVSLAM_Status vo_status = CUVSLAM_TrackGpuMem( + cuvslam_handle_, + cuvslam_image_objects.data(), + cuvslam_image_objects.size(), + nullptr, // depth_image (not used in this mode) + predicted_pose_ptr, // can safely handle nullptr if no guess is provided + &vo_pose_estimate + ); + + if(vo_status != CUVSLAM_SUCCESS) + { + // Provide specific error message + const char * error_msg = "Unknown error"; + switch(vo_status) { + case CUVSLAM_TRACKING_LOST: error_msg = "CUVSLAM_TRACKING_LOST"; break; + case CUVSLAM_INVALID_ARG: error_msg = "CUVSLAM_INVALID_PARAMETER"; break; + case CUVSLAM_CAN_NOT_LOCALIZE: error_msg = "CUVSLAM_CAN_NOT_LOCALIZE"; break; + case CUVSLAM_GENERIC_ERROR: error_msg = "CUVSLAM_GENERIC_ERROR"; break; + case CUVSLAM_UNSUPPORTED_NUMBER_OF_CAMERAS: error_msg = "CUVSLAM_UNSUPPORTED_NUMBER_OF_CAMERAS"; break; + case CUVSLAM_SLAM_IS_NOT_INITIALIZED: error_msg = "CUVSLAM_SLAM_IS_NOT_INITIALIZED"; break; + default: error_msg = "Unknown cuVSLAM error"; break; + } + + // Update timing information even on failure + last_timestamp_ = data.stamp(); + + if(info) + { + // Report very high uncertainty to upstream consumers + info->reg.covariance = cv::Mat::eye(6, 6, CV_64FC1) * 9999.0; + info->timeEstimation = timer.ticks(); + } + + // The cuVSLAM tracking status never reports lost in my testing. + // Thus we use covariance to detect lost state. + if(vo_status == CUVSLAM_TRACKING_LOST) + { + UWARN("LOST: cuVSLAM reported CUVSLAM_TRACKING_LOST"); + lost_ = true; + } + else + { + UWARN("cuVSLAM tracking error: %d (%s)", vo_status, error_msg); + } + + return Transform(); + } + + // Check if we have invalid covariance values + bool valid_covariance = true; + for(int i = 0; i < 6; i++) + { + float & diag_val = vo_pose_estimate.covariance[i*6+i]; + + // conditions for immediate failure and tracking loss + if(!std::isfinite(diag_val) || diag_val < 0.0) + { + diag_val = 9999.0; + valid_covariance = false; + } + // Tracker returns identity covariance and 0.0 values after initialization before motion. + if(std::abs(diag_val) < 1e-7f) + { + diag_val = 0.0001; + } + if(diag_val > 0.1) { + valid_covariance = false; + + // If we don't have a guess, we can't use velocity difference to detect lost state. + // Thus at this point, we are lost. Warn the user that cuVSLAM probably needs a guess to work well. + if(guess.isNull()) { + UWARN("No guess provided, but covariance is invalid: %.8f", diag_val); + UWARN("We cannot use velocity difference to detect lost state without a guess!"); + UWARN("Without a guess cuVSLAM is prone to getting lost easily!"); + UWARN("It is highly recommended to provide a guess to cuVSLAM!"); + lost_ = true; + if(info) { + info->reg.covariance = cv::Mat::eye(6, 6, CV_64FC1) * 9999.0; + info->timeEstimation = timer.ticks(); + } + return Transform(); + } + } + } + + // Convert to RTABMAP covariance format and scale to meet RTABMAP expectations + cv::Mat covMat = convertCuVSLAMCovariance(vo_pose_estimate.covariance, use_raw_covariance_); + + // Apply ground constraint + if(planar_constraints_) { + if(CUVSLAM_GroundConstraintAddNextPose(ground_constraint_handle_, &vo_pose_estimate.pose) != CUVSLAM_SUCCESS) { + UERROR("Failed to add next pose to ground constraint"); + return Transform(); + } + if(CUVSLAM_GroundConstraintGetPoseOnGround(ground_constraint_handle_, &vo_pose_estimate.pose) != CUVSLAM_SUCCESS) { + UERROR("Failed to get pose on ground"); + return Transform(); + } + } + + // Convert cuVSLAM absolute pose to incremental RTAB-Map Transform + Transform current_pose = FromcuVSLAMPose(vo_pose_estimate.pose); + current_pose = canonical_pose_cuvslam * current_pose * cuvslam_pose_canonical; + UASSERT(!previous_pose_.isNull()); + Transform transform = previous_pose_.inverse() * current_pose; + + // Compute guess and estimated velocity and report lost if velocity ratio is high and covariance is invalid + double time_delta_s = 0.0; + double guess_velocity_ms = 0.0; + double estimated_velocity_ms = 0.0; + + if(!guess.isNull() && last_timestamp_ > 0.0 && !use_raw_covariance_ && !valid_covariance) { + time_delta_s = data.stamp() - last_timestamp_; + + guess_velocity_ms = guess.getNorm() / time_delta_s; + estimated_velocity_ms = transform.getNorm() / time_delta_s; + double velocity_ratio = estimated_velocity_ms / guess_velocity_ms; + double velocity_difference = std::abs(estimated_velocity_ms - guess_velocity_ms); + + // Check if the expected and predicted velocities are divergent. + // Also ensure estimated velocity is not zero. + // In rapid deceleration cases, estimated velocity zeros out faster then the guess but we aren't lost yet. So we need to check for this. + bool zero_estimated_velocity = estimated_velocity_ms < zero_estimated_velocity_threshold_; + bool invalid_velocity_ratio = velocity_ratio > velocity_ratio_threshold_high_ || velocity_ratio < velocity_ratio_threshold_low_; + bool invalid_velocity_difference = velocity_difference > velocity_difference_threshold_; + + if(invalid_velocity_ratio && invalid_velocity_difference && !zero_estimated_velocity) { + UWARN("Velocity ratio is high and covariance is invalid: %.4f, returning null transform", velocity_ratio); + lost_ = true; + if(info) { + info->reg.covariance = cv::Mat::eye(6, 6, CV_64FC1) * 9999.0; + info->timeEstimation = timer.ticks(); + } + return Transform(); + } else { + covMat = cv::Mat::eye(6, 6, CV_64FC1) * 0.0001; + } + } + + // At this point we have passed the covariance lost checks, so we are tracking. + tracking_ = true; + + if(info) + { + info->reg.covariance = covMat; + info->timeEstimation = timer.ticks(); + } + + // extract 3D VO landmarks for visualization + // This will be used to determine if we have enough features to start tracking. + CUVSLAM_LandmarkVector landmark_vector; + landmark_vector.max = landmarks_.size(); + landmark_vector.landmarks = landmarks_.data(); + CUVSLAM_Status landmark_status = CUVSLAM_GetLastLandmarks(cuvslam_handle_, &landmark_vector); + int landmarks_num = landmark_vector.num; + + // Fill info with visualization data + if(info) { + if(data.stereoCameraModels().size()==1) { + info->type = kTypeF2F; + + // extract 2D VO observations for visualization + CUVSLAM_ObservationVector observation_vector; + observation_vector.max = observations_.size(); + observation_vector.observations = observations_.data(); + CUVSLAM_Status observation_status = CUVSLAM_GetLastLeftObservations(cuvslam_handle_, &observation_vector); + if(observation_status == CUVSLAM_SUCCESS && observation_vector.num > 0) { + info->newCorners.reserve(observation_vector.num); + for(uint32_t i = 0; i < observation_vector.num; ++i) + { + const CUVSLAM_Observation & observation = observation_vector.observations[i]; + info->newCorners.emplace_back(observation.u, observation.v); + } + } + } + else { + info->type = kTypeF2M; + } + + std::vector local_transform_inv(data.stereoCameraModels().size()); + for(size_t i=0; i 0) { + Transform absolute_pose = this->getPose() * transform; + for(uint32_t i = 0; i < landmark_vector.num; ++i) + { + const CUVSLAM_Landmark & landmark = landmark_vector.landmarks[i]; + cv::Point3f pt = util3d::transformPoint(cv::Point3f(landmark.x, landmark.y, landmark.z), canonical_pose_cuvslam); + info->localMap.insert(std::make_pair(landmark.id, util3d::transformPoint(pt, absolute_pose))); + if(data.stereoCameraModels().size() > 1) { + for(size_t i=0; i 0) + { + data.stereoCameraModels()[i].left().reproject(pt_in_cam.x, pt_in_cam.y, pt_in_cam.z, u, v); + if(data.stereoCameraModels()[i].left().inFrame(u,v)) + { + info->words.insert(std::make_pair(landmark.id, cv::KeyPoint(u + i*image_width, v, 3))); + info->reg.inliersIDs.push_back(landmark.id); + break; + } + + // Update landmarks number based on which landmarks were successfully reprojected in the current frame + landmarks_num = info->words.size(); + } + } + } + } + } + + // If we are in a multi-camera setup and successfully reprojected landmarks into camera frames, + // use the number of successfully reprojected landmarks instead of the raw cuVSLAM landmark count. + if(data.stereoCameraModels().size() > 1) { + landmarks_num = (int)info->words.size(); + } + } + + // Check if we have enough features to start tracking. Otherwise we are lost. + if(landmarks_num < min_landmarks_threshold_ && !initialized_) { + if(info) { + info->reg.covariance = cv::Mat::eye(6, 6, CV_64FC1) * 9999.0; + info->timeEstimation = timer.ticks(); + } + // Free GPU resources and reset state. Prevent memory leaks on init loops. + cleanupCuVSLAMResources(); + lost_ = true; + tracking_ = false; + initialized_ = false; + return Transform(); + } else { + initialized_ = true; + } + + previous_pose_ = current_pose; + last_timestamp_ = data.stamp(); + return transform; +#else + UERROR("cuVSLAM support not compiled in RTAB-Map");\ + return Transform(); +#endif + +} + +#ifdef RTABMAP_CUVSLAM + +// ============================================================================ +// cuVSLAM Initialization and Configuration +// ============================================================================ + +bool initializeCuVSLAM(const SensorData & data, + CUVSLAM_TrackerHandle & cuvslam_handle, + CUVSLAM_GroundConstraintHandle & ground_constraint_handle, + bool planar_constraints, + int multicam_mode, + std::vector & gpu_left_image_data, + std::vector & gpu_right_image_data, + std::vector & gpu_left_image_sizes, + std::vector & gpu_right_image_sizes, + std::vector & cuvslam_cameras, + std::vector> & intrinsics, + cudaStream_t & cuda_stream) +{ + // cuVSLAM verbosity level (0=none, 1=errors, 2=warnings, 3=info) + CUVSLAM_SetVerbosity(0); + + // Initialize cuVSLAM cameras and intrinsic vectors + cuvslam_cameras.resize(data.stereoCameraModels().size()*2); + intrinsics.resize(data.stereoCameraModels().size()*2); + + // Handle stereo cameras + for(size_t i = 0; i < data.stereoCameraModels().size(); ++i) + { + const StereoCameraModel & stereoModel = data.stereoCameraModels()[i]; + if(!stereoModel.isValidForProjection()) + { + UERROR("Invalid stereo camera model %d for cuVSLAM initialization!", static_cast(i)); + return false; + } + const CameraModel & leftModel = stereoModel.left(); + const CameraModel & rightModel = stereoModel.right(); + + auto & cam_left = cuvslam_cameras[i*2]; + auto & cam_right = cuvslam_cameras[i*2+1]; + auto & intrinsics_left = intrinsics[i*2]; + auto & intrinsics_right = intrinsics[i*2+1]; + + // Left camera + cam_left.parameters = intrinsics_left.data(); + cam_left.width = leftModel.imageWidth(); + cam_left.height = leftModel.imageHeight(); + cam_left.distortion_model = "pinhole"; + cam_left.num_parameters = 4; + intrinsics_left[0] = leftModel.cx(); + intrinsics_left[1] = leftModel.cy(); + intrinsics_left[2] = leftModel.fx(); + intrinsics_left[3] = leftModel.fy(); + + // Transform sequence: + // cuvslam -> optical -> camera extrinsics in optical -> cuvslam + rtabmap::Transform extrinsics = cuvslam_pose_canonical * stereoModel.localTransform() * optical_pose_cuvslam; + cam_left.pose = TocuVSLAMPose(extrinsics); + cam_left.border_top = 0; + cam_left.border_bottom = 0; + cam_left.border_left = 0; + cam_left.border_right = 0; + + // Right camera + cam_right.parameters = intrinsics_right.data(); + cam_right.width = rightModel.imageWidth(); + cam_right.height = rightModel.imageHeight(); + cam_right.distortion_model = "pinhole"; + cam_right.num_parameters = 4; + intrinsics_right[0] = rightModel.cx(); + intrinsics_right[1] = rightModel.cy(); + intrinsics_right[2] = rightModel.fx(); + intrinsics_right[3] = rightModel.fy(); + Transform baseline_transform(1, 0, 0, stereoModel.baseline(), + 0, 1, 0, 0, + 0, 0, 1, 0); + // Transform sequence: + // cuvslam -> optical -> baseline offset in optical -> camera extrinsics in optical -> cuvslam + extrinsics = cuvslam_pose_canonical * stereoModel.localTransform() * baseline_transform * optical_pose_cuvslam; + cam_right.pose = TocuVSLAMPose(extrinsics); + cam_right.border_top = 0; + cam_right.border_bottom = 0; + cam_right.border_left = 0; + cam_right.border_right = 0; + } + + // Set up camera rig + CUVSLAM_CameraRig camera_rig; + camera_rig.cameras = cuvslam_cameras.data(); + camera_rig.num_cameras = cuvslam_cameras.size(); + + const CUVSLAM_Configuration configuration = CreateConfiguration(data, multicam_mode); + + // Create tracker + CUVSLAM_TrackerHandle tracker_handle; + UTimer create_timer; create_timer.start(); + + const CUVSLAM_Status status_tracker = CUVSLAM_CreateTracker(&tracker_handle, &camera_rig, &configuration); + + if (status_tracker != CUVSLAM_SUCCESS) { + UERROR("Failed to initialize CUVSLAM tracker: %d", status_tracker); + return false; + } + + cuvslam_handle = tracker_handle; + + // Initialize gpu image data vectors and sizes + size_t stereo_pairs_count = data.stereoCameraModels().size(); + gpu_left_image_data.resize(stereo_pairs_count, nullptr); + gpu_right_image_data.resize(stereo_pairs_count, nullptr); + gpu_left_image_sizes.resize(stereo_pairs_count, 0); + gpu_right_image_sizes.resize(stereo_pairs_count, 0); + + // initialize ground constraints + if (planar_constraints) + { + CUVSLAM_Pose identity_cuvslam = TocuVSLAMPose(Transform::getIdentity()); // same in both frames + + const CUVSLAM_Status status_ground = CUVSLAM_GroundConstraintCreate( + &ground_constraint_handle, + &identity_cuvslam, + &identity_cuvslam, + &identity_cuvslam + ); + if(status_ground != CUVSLAM_SUCCESS) { + UERROR("Failed to initialize CUVSLAM ground constraint: %d", status_ground); + return false; + } + } + + return true; +} + +/* +Implementation based on Isaac ROS VisualSlamNode::VisualSlamImpl::CreateConfiguration() +Source: isaac_ros_visual_slam/isaac_ros_visual_slam/src/impl/visual_slam_impl.cpp:379-422 +https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_visual_slam/blob/19be8c781a55dee9cfbe9f097adca3986638feb1/isaac_ros_visual_slam/src/impl/visual_slam_impl.cpp#L379-L422 +*/ +CUVSLAM_Configuration CreateConfiguration(const SensorData & data, int multicam_mode) +{ + CUVSLAM_Configuration configuration; + CUVSLAM_InitDefaultConfiguration(&configuration); + + + // Core Visual Odometry Settings + configuration.use_motion_model = 1; // Enable motion model for better tracking + configuration.use_denoising = 0; // Disable denoising by default + configuration.use_gpu = 1; // Use GPU acceleration + configuration.horizontal_stereo_camera = 1; // Stereo camera configuration + + configuration.enable_observations_export = 1; // Export observations for external reading + + // SLAM Enabled (required for observation buffer allocation) + configuration.enable_localization_n_mapping = 0; // NO SLAM + configuration.enable_landmarks_export = 0; // SLAM feature (optional) + configuration.enable_reading_slam_internals = 0; // SLAM feature (optional) + + // Odometry configuration (Vision-only, no IMU) + configuration.odometry_mode = CUVSLAM_OdometryMode::Multicamera; + configuration.multicam_mode = multicam_mode; // moderate (0), performance (1) or precision (2). + configuration.debug_imu_mode = 0; + + // SLAM parameters (disabled) + configuration.planar_constraints = 0; + configuration.slam_throttling_time_ms = 0; + configuration.slam_max_map_size = 0; + configuration.slam_sync_mode = 0; + + // Use for getting debug images and logs + // configuration.debug_dump_directory = "/home/...your desired directory..."; + + return configuration; +} + +// ============================================================================ +// GPU Memory Management +// ============================================================================ + +bool allocateGpuMemory(size_t size, uint8_t ** gpu_ptr, size_t * current_size) +{ + if(*current_size != size) { + // Reallocate GPU memory if size changed + if(*gpu_ptr != nullptr) { + cudaFree(*gpu_ptr); + } + *gpu_ptr = nullptr; + cudaError_t cuda_err = cudaMalloc(gpu_ptr, size); + if(cuda_err != cudaSuccess) { + UERROR("Failed to allocate GPU memory: %s", cudaGetErrorString(cuda_err)); + return false; + } + *current_size = size; + } + return true; +} + +bool copyToGpuAsync(const cv::Mat & cpu_image, uint8_t * gpu_ptr, size_t size, cudaStream_t & cuda_stream) +{ + // Initialize CUDA stream if not already done + if(cuda_stream == nullptr) { + cudaError_t stream_err = cudaStreamCreate(&cuda_stream); + if(stream_err != cudaSuccess) { + UERROR("Failed to create CUDA stream: %s", cudaGetErrorString(stream_err)); + return false; + } + } + + // Copy CPU data to GPU memory with async operation for better performance + cudaError_t cuda_err = cudaMemcpyAsync(gpu_ptr, cpu_image.data, size, + cudaMemcpyHostToDevice, cuda_stream); + if(cuda_err != cudaSuccess) { + UERROR("Failed to copy image to GPU: %s", cudaGetErrorString(cuda_err)); + return false; + } + + return true; +} + +bool synchronizeGpuOperations(cudaStream_t & cuda_stream) +{ + if(cuda_stream) { + cudaError_t cuda_err = cudaStreamSynchronize(cuda_stream); + if(cuda_err != cudaSuccess) { + UERROR("Failed to synchronize GPU operations: %s", cudaGetErrorString(cuda_err)); + return false; + } + } + return true; +} + +// ============================================================================ +// Image Processing and Preparation +// ============================================================================ + +bool prepareImages(const SensorData & data, + std::vector & cuvslam_images, + std::vector & gpu_left_image_data, + std::vector & gpu_right_image_data, + std::vector & gpu_left_image_sizes, + std::vector & gpu_right_image_sizes, + cudaStream_t & cuda_stream) +{ + // Convert timestamp to nanoseconds (cuVSLAM expects nanoseconds) + int64_t timestamp_ns = static_cast(data.stamp() * 1000000000.0); + + // Horizontally stitched images received by RTAB-Map + cv::Mat left_image = data.imageRaw(); + cv::Mat right_image = data.rightRaw(); + + // Validate basic image properties + if(left_image.empty() || right_image.empty()) { + UERROR("No left or right image available for stereo camera"); + return false; + } + if(left_image.channels() != 1 && left_image.channels() != 3) { + UERROR("Unsupported left image format: %d channels", left_image.channels()); + return false; + } + if(right_image.channels() != 1 && right_image.channels() != 3) { + UERROR("Unsupported right image format: %d channels", right_image.channels()); + return false; + } + + // Convert image format for cuVSLAM - mono8 or rgb8 + cv::Mat processed_left_image; + cv::Mat processed_right_image; + CUVSLAM_ImageEncoding left_encoding; + CUVSLAM_ImageEncoding right_encoding; + + // process left image - copies image if BGR to RGB conversion is needed + processed_left_image = left_image; + if(left_image.channels() == 1) { + left_encoding = CUVSLAM_ImageEncoding::MONO8; + } else if(left_image.channels() == 3) { + // convert from BGR to RGB + cv::cvtColor(left_image, processed_left_image, cv::COLOR_BGR2RGB); + left_encoding = CUVSLAM_ImageEncoding::RGB8; + } else { + UERROR("Unsupported left image format: %d channels", left_image.channels()); + return false; + } + + // process right image - copies image if BGR to RGB conversion is needed + processed_right_image = right_image; + if(right_image.channels() == 1) { + right_encoding = CUVSLAM_ImageEncoding::MONO8; + } else if(right_image.channels() == 3) { + // convert from BGR to RGB + cv::cvtColor(right_image, processed_right_image, cv::COLOR_BGR2RGB); + right_encoding = CUVSLAM_ImageEncoding::RGB8; + } else { + UERROR("Unsupported right image format: %d channels", right_image.channels()); + return false; + } + + int camera_index = 0; + int stereo_index = 0; + for(const StereoCameraModel & model : data.stereoCameraModels()) { + // slice out the image for the current stereo pair + // Assumes all images have the same width and height + int left_image_width = model.left().imageWidth(); + int right_image_width = model.right().imageWidth(); + int left_image_height = model.left().imageHeight(); + int right_image_height = model.right().imageHeight(); + + // makes a copy for the sliced images + cv::Mat left_image_slice = processed_left_image(cv::Rect(stereo_index * left_image_width, 0, left_image_width, left_image_height)).clone(); + cv::Mat right_image_slice = processed_right_image(cv::Rect(stereo_index * right_image_width, 0, right_image_width, right_image_height)).clone(); + + size_t left_image_size = left_image_slice.total() * left_image_slice.elemSize(); + size_t right_image_size = right_image_slice.total() * right_image_slice.elemSize(); + + // Allocate GPU memory for left camera + if(!allocateGpuMemory(left_image_size, &gpu_left_image_data[stereo_index], &gpu_left_image_sizes[stereo_index])) { + UERROR("PREPARE IMAGES: Failed to allocate GPU memory for left image"); + return false; + } + if(!copyToGpuAsync(left_image_slice, gpu_left_image_data[stereo_index], left_image_size, cuda_stream)) { + UERROR("PREPARE IMAGES: Failed to copy left image to GPU"); + return false; + } + + // Create CUVSLAM_Image for left camera with GPU memory + CUVSLAM_Image left_cuvslam_image; + left_cuvslam_image.width = left_image_width; + left_cuvslam_image.height = left_image_height; + left_cuvslam_image.pixels = gpu_left_image_data[stereo_index]; // GPU memory pointer + left_cuvslam_image.timestamp_ns = timestamp_ns; + left_cuvslam_image.camera_index = camera_index; + left_cuvslam_image.pitch = left_image_slice.step; + left_cuvslam_image.image_encoding = left_encoding; + // Mask fields (not used in this implementation) + left_cuvslam_image.input_mask = nullptr; + left_cuvslam_image.mask_width = 0; + left_cuvslam_image.mask_height = 0; + left_cuvslam_image.mask_pitch = 0; + + cuvslam_images.push_back(left_cuvslam_image); + + camera_index++; + + // Allocate GPU memory for right camera + if(!allocateGpuMemory(right_image_size, &gpu_right_image_data[stereo_index], &gpu_right_image_sizes[stereo_index])) { + UERROR("PREPARE IMAGES: Failed to allocate GPU memory for right image"); + return false; + } + + if(!copyToGpuAsync(right_image_slice, gpu_right_image_data[stereo_index], right_image_size, cuda_stream)) { + UERROR("PREPARE IMAGES: Failed to copy right image to GPU"); + return false; + } + + CUVSLAM_Image right_cuvslam_image; + right_cuvslam_image.width = right_image_width; + right_cuvslam_image.height = right_image_height; + right_cuvslam_image.pixels = gpu_right_image_data[stereo_index]; // GPU memory pointer + right_cuvslam_image.timestamp_ns = timestamp_ns; + right_cuvslam_image.camera_index = camera_index; + right_cuvslam_image.pitch = right_image_slice.step; + right_cuvslam_image.image_encoding = right_encoding; + // Mask fields (not used in this implementation) + right_cuvslam_image.input_mask = nullptr; + right_cuvslam_image.mask_width = 0; + right_cuvslam_image.mask_height = 0; + right_cuvslam_image.mask_pitch = 0; + + cuvslam_images.push_back(right_cuvslam_image); + + stereo_index++; + camera_index++; + } + + // Synchronize all async GPU operations before returning + if(!synchronizeGpuOperations(cuda_stream)) { + UERROR("PREPARE IMAGES: Failed to synchronize GPU operations"); + return false; + } + + return true; +} + + +/* +Convert cuVSLAM covariance to RTAB-Map format. +Based on Isaac ROS implementation: FromcuVSLAMCovariance() +Source: isaac_ros_visual_slam/src/impl/cuvslam_ros_conversion.cpp:275-299 +*/ +cv::Mat convertCuVSLAMCovariance(const float * cuvslam_covariance, bool use_raw_covariance) +{ + + // Scale cuvslam covariance to make it more realistic + const double scaling_factor = use_raw_covariance ? 1.0 : 10.0; + + // Handle null covariance pointer + if(cuvslam_covariance == nullptr) + { + UWARN("Covariance was recieved as a nullptr, proceeding with default infinite covariance"); + cv::Mat default_infinite_covariance = cv::Mat::eye(6, 6, CV_64FC1) * 9999.0; + return default_infinite_covariance; + } + + const float * covariance = cuvslam_covariance; + + // Create transformation matrix for coordinate system conversion + // cuVSLAM frame (x-right, y-up, z-backward) to RTAB-Map frame (x-forward, y-left, z-up) + // Get rotation matrix from canonical_pose_cuvslam transform + Eigen::Matrix canonical_pose_cuvslam_mat; + canonical_pose_cuvslam_mat << + canonical_pose_cuvslam.r11(), canonical_pose_cuvslam.r12(), canonical_pose_cuvslam.r13(), + canonical_pose_cuvslam.r21(), canonical_pose_cuvslam.r22(), canonical_pose_cuvslam.r23(), + canonical_pose_cuvslam.r31(), canonical_pose_cuvslam.r32(), canonical_pose_cuvslam.r33(); + + // Create 6x6 block diagonal transformation matrix + Eigen::Matrix block_canonical_pose_cuvslam = Eigen::Matrix::Zero(); + block_canonical_pose_cuvslam.block<3, 3>(0, 0) = canonical_pose_cuvslam_mat; + block_canonical_pose_cuvslam.block<3, 3>(3, 3) = canonical_pose_cuvslam_mat; + + // Map cuVSLAM covariance array to Eigen matrix and convert to double for numerical stability + Eigen::Matrix covariance_mat_float = + Eigen::Map>(const_cast(covariance)); + Eigen::Matrix covariance_mat = covariance_mat_float.cast(); + + // Reorder covariance matrix elements (in double precision) + // The covariance matrix from cuVSLAM arranges elements as follows: + // (rotation about X axis, rotation about Y axis, rotation about Z axis, x, y, z) + // However, in RTAB-Map, the order is: + // (x, y, z, rotation about X axis, rotation about Y axis, rotation about Z axis) + Eigen::Matrix rtabmap_covariance_mat = Eigen::Matrix::Zero(); + rtabmap_covariance_mat.block<3, 3>(0, 0) = covariance_mat.block<3, 3>(3, 3); // translation-translation + rtabmap_covariance_mat.block<3, 3>(0, 3) = covariance_mat.block<3, 3>(3, 0); // translation-rotation + rtabmap_covariance_mat.block<3, 3>(3, 0) = covariance_mat.block<3, 3>(0, 3); // rotation-translation + rtabmap_covariance_mat.block<3, 3>(3, 3) = covariance_mat.block<3, 3>(0, 0); // rotation-rotation + + // Convert transformation matrix to double for numerical stability in matrix operations + Eigen::Matrix block_canonical_pose_cuvslam_double = block_canonical_pose_cuvslam.cast(); + + // Apply coordinate system transformation (in double precision) + Eigen::Matrix covariance_mat_change_basis = + block_canonical_pose_cuvslam_double * rtabmap_covariance_mat * block_canonical_pose_cuvslam_double.transpose(); + + // Convert Eigen matrix to OpenCV Mat (already in double precision) + cv::Mat cv_covariance(6, 6, CV_64FC1); + for(int i = 0; i < 6; i++) + { + for(int j = 0; j < 6; j++) + { + cv_covariance.at(i, j) = covariance_mat_change_basis(i, j); + // for angular values, scale again to make it more realistic + if(i > 2 || j > 2) { + cv_covariance.at(i, j) *= scaling_factor; + } + } + } + + // Ensure diagonal elements are positive and finite (RTAB-Map requirement) + return cv_covariance; +} + +#endif // RTABMAP_CUVSLAM + +} // namespace rtabmap + diff --git a/corelib/src/odometry/OdometryF2F.cpp b/corelib/src/odometry/OdometryF2F.cpp index 0f335fe9..86a0a983 100644 --- a/corelib/src/odometry/OdometryF2F.cpp +++ b/corelib/src/odometry/OdometryF2F.cpp @@ -176,6 +176,8 @@ Transform OdometryF2F::computeTransform( if(info && this->isInfoDataFilled()) { std::list > > pairs; + UASSERT(tmpRefFrame.getWords().size() == tmpRefFrame.getWordsKpts().size()); + UASSERT(newFrame.getWords().size() == newFrame.getWordsKpts().size()); EpipolarGeometry::findPairsUnique(tmpRefFrame.getWords(), newFrame.getWords(), pairs); info->refCorners.resize(pairs.size()); info->newCorners.resize(pairs.size()); diff --git a/corelib/src/odometry/OdometryF2M.cpp b/corelib/src/odometry/OdometryF2M.cpp index ba47ac15..58d0e3aa 100644 --- a/corelib/src/odometry/OdometryF2M.cpp +++ b/corelib/src/odometry/OdometryF2M.cpp @@ -793,6 +793,7 @@ Transform OdometryF2M::computeTransform( if(!lastFrameModels.empty()) { + UASSERT(lastFrame_->getWordsKpts().size() == lastFrame_->getWords().size()); for(std::multimap::const_iterator iter = lastFrame_->getWords().begin(); iter!=lastFrame_->getWords().end(); ++iter) { const cv::Point3f & pt = lastFrame_->getWords3()[iter->second]; @@ -1559,6 +1560,10 @@ Transform OdometryF2M::computeTransform( { info->reg = regInfo.copyWithoutData(); } + if(output.isNull()) + { + info->reg.covariance = cv::Mat::eye(6,6,CV_64FC1)*9999.0; // Lost + } } UINFO("Odom update time = %fs lost=%s features=%d inliers=%d/%d variance:lin=%f, ang=%f local_map=%d local_scan_map=%d", diff --git a/corelib/src/odometry/OdometryLIOSAM.cpp b/corelib/src/odometry/OdometryLIOSAM.cpp new file mode 100644 index 00000000..abd38aa8 --- /dev/null +++ b/corelib/src/odometry/OdometryLIOSAM.cpp @@ -0,0 +1,478 @@ +/* +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/odometry/OdometryLIOSAM.h" +#include "rtabmap/core/OdometryInfo.h" +#include "rtabmap/core/util3d.h" +#include "rtabmap/utilite/ULogger.h" +#include "rtabmap/utilite/UTimer.h" +#include "rtabmap/utilite/UStl.h" +#include "rtabmap/utilite/UDirectory.h" +#include "rtabmap/utilite/UFile.h" + +#ifdef RTABMAP_LIOSAM +#include +#include +#endif + +namespace rtabmap { + +static ParametersMap disableDeskewing(ParametersMap params) { + // LIO-SAM performs its own internal deskewing via imageProjection. + // The base-class deskew must be disabled so that the original per-point + // timestamps reach LIO-SAM intact. + params[Parameters::kOdomDeskewing()] = "false"; + return params; +} + +OdometryLIOSAM::OdometryLIOSAM(const ParametersMap & parameters) : + Odometry(disableDeskewing(parameters)) +#ifdef RTABMAP_LIOSAM + ,lioSam_(0) + ,lastPose_(Transform::getIdentity()) + ,lost_(false) + ,linVar_(Parameters::defaultOdomLIOSAMLinVar()) + ,angVar_(Parameters::defaultOdomLIOSAMAngVar()) + ,parameters_(parameters) +#endif +{ +#ifdef RTABMAP_LIOSAM + Parameters::parse(parameters, Parameters::kOdomLIOSAMLinVar(), linVar_); + UASSERT(linVar_ > 0.0f); + Parameters::parse(parameters, Parameters::kOdomLIOSAMAngVar(), angVar_); + UASSERT(angVar_ > 0.0f); +#endif +} + +OdometryLIOSAM::~OdometryLIOSAM() +{ +#ifdef RTABMAP_LIOSAM + delete lioSam_; +#endif +} + +void OdometryLIOSAM::reset(const Transform & initialPose) +{ + Odometry::reset(initialPose); +#ifdef RTABMAP_LIOSAM + if(lioSam_) + { + lioSam_->reset(); + } + lastPose_ = Transform::getIdentity(); + lost_ = false; + imuLocalTransform_ = Transform(); + imuBuffer_.clear(); +#endif +} + +#ifdef RTABMAP_LIOSAM +bool OdometryLIOSAM::init(const Transform & imuLocalTransform, const Transform & lidarLocalTransform) +{ + ParamServer config; + + // Check if a config file path was provided + std::string configPath; + Parameters::parse(parameters_, Parameters::kOdomLIOSAMConfigPath(), configPath); + if(!configPath.empty()) + { + configPath = uReplaceChar(configPath, '~', UDirectory::homeDir()); + if(!UFile::exists(configPath)) + { + UERROR("LIO-SAM config file not found: %s", configPath.c_str()); + return false; + } + UINFO("Loading LIO-SAM parameters from config file: %s", configPath.c_str()); + config = loadParamsFromYaml(configPath); + } + else + { + UINFO("No LIO-SAM config file provided, using rtabmap parameters"); + + // Build ParamServer from individual rtabmap parameters + int sensorType = Parameters::defaultOdomLIOSAMSensor(); + Parameters::parse(parameters_, Parameters::kOdomLIOSAMSensor(), sensorType); + if(sensorType == 1) + config.sensor = SensorType::OUSTER; + else if(sensorType == 2) + config.sensor = SensorType::LIVOX; + else + config.sensor = SensorType::VELODYNE; + + config.N_SCAN = Parameters::defaultOdomLIOSAMNScan(); + Parameters::parse(parameters_, Parameters::kOdomLIOSAMNScan(), config.N_SCAN); + + config.Horizon_SCAN = Parameters::defaultOdomLIOSAMHorizonScan(); + Parameters::parse(parameters_, Parameters::kOdomLIOSAMHorizonScan(), config.Horizon_SCAN); + + config.imuAccNoise = Parameters::defaultOdomLIOSAMImuAccNoise(); + Parameters::parse(parameters_, Parameters::kOdomLIOSAMImuAccNoise(), config.imuAccNoise); + + config.imuGyrNoise = Parameters::defaultOdomLIOSAMImuGyrNoise(); + Parameters::parse(parameters_, Parameters::kOdomLIOSAMImuGyrNoise(), config.imuGyrNoise); + + config.imuAccBiasN = Parameters::defaultOdomLIOSAMImuAccBiasN(); + Parameters::parse(parameters_, Parameters::kOdomLIOSAMImuAccBiasN(), config.imuAccBiasN); + + config.imuGyrBiasN = Parameters::defaultOdomLIOSAMImuGyrBiasN(); + Parameters::parse(parameters_, Parameters::kOdomLIOSAMImuGyrBiasN(), config.imuGyrBiasN); + + config.imuGravity = Parameters::defaultOdomLIOSAMImuGravity(); + Parameters::parse(parameters_, Parameters::kOdomLIOSAMImuGravity(), config.imuGravity); + + config.edgeThreshold = Parameters::defaultOdomLIOSAMEdgeThreshold(); + Parameters::parse(parameters_, Parameters::kOdomLIOSAMEdgeThreshold(), config.edgeThreshold); + + config.surfThreshold = Parameters::defaultOdomLIOSAMSurfThreshold(); + Parameters::parse(parameters_, Parameters::kOdomLIOSAMSurfThreshold(), config.surfThreshold); + + // Set reasonable defaults for params not exposed via rtabmap + config.downsampleRate = 1; + config.lidarMinRange = 1.0f; + config.lidarMaxRange = 1000.0f; + config.imuRPYWeight = 0.01f; + config.odometrySurfLeafSize = 0.2f; + config.mappingCornerLeafSize = 0.2f; + config.mappingSurfLeafSize = 0.4f; + config.z_tollerance = FLT_MAX; + config.rotation_tollerance = FLT_MAX; + config.numberOfCores = 4; + config.mappingProcessInterval = 0.01; + config.surroundingkeyframeAddingDistThreshold = 1.0f; + config.surroundingkeyframeAddingAngleThreshold = 0.2f; + config.surroundingKeyframeDensity = 1.0f; + config.surroundingKeyframeSearchRadius = 50.0f; + config.loopClosureEnableFlag = false; // rtabmap handles loop closures + config.loopClosureFrequency = 1.0f; + config.surroundingKeyframeSize = 50; + config.historyKeyframeSearchRadius = 10.0f; + config.historyKeyframeSearchTimeDiff = 30.0f; + config.historyKeyframeSearchNum = 25; + config.historyKeyframeFitnessScore = 0.3f; + config.globalMapVisualizationSearchRadius = 1e3f; + config.globalMapVisualizationPoseDensity = 10.0f; + config.globalMapVisualizationLeafSize = 1.0f; + config.edgeFeatureMinValidNum = 10; + config.surfFeatureMinValidNum = 100; + config.savePCD = false; + config.useImuHeadingInitialization = false; + config.useGpsElevation = false; + config.gpsCovThreshold = 2.0f; + config.poseCovThreshold = 25.0f; + } + + // Always override extrinsics from sensor local transforms when available. + // This ensures the IMU-to-lidar transform matches the actual sensor setup + // regardless of what the config file says. + // imuLocalTransform = T_base_imu (base_link -> imu_link) + // lidarLocalTransform = T_base_lidar (base_link -> lidar_link) + // LIO-SAM's imuConverter() expects T_lidar_imu: + // T_lidar_imu = T_base_lidar^{-1} * T_base_imu + if(!imuLocalTransform.isNull() && !lidarLocalTransform.isNull()) + { + Transform T_lidar_imu = lidarLocalTransform.inverse() * imuLocalTransform; + Eigen::Matrix4d T = T_lidar_imu.toEigen4d(); + Eigen::Matrix3d rot = T.block<3,3>(0,0); + Eigen::Vector3d trans = T.block<3,1>(0,3); + config.extRotV = {rot(0,0), rot(0,1), rot(0,2), + rot(1,0), rot(1,1), rot(1,2), + rot(2,0), rot(2,1), rot(2,2)}; + config.extRPYV = config.extRotV; + config.extTransV = {trans(0), trans(1), trans(2)}; + UINFO("LIO-SAM extrinsics (T_lidar_imu) computed from sensor local transforms: %s", T_lidar_imu.prettyPrint().c_str()); + } + else if(config.extRotV.size() != 9 || config.extTransV.size() != 3) + { + // No valid extrinsics from sensor data or config file + UERROR("Cannot compute IMU-to-lidar extrinsics: IMU local transform %s, lidar local transform %s. " + "Both must be valid, or the config file must contain valid extrinsics.", + imuLocalTransform.isNull() ? "is null" : "is valid", + lidarLocalTransform.isNull() ? "is null" : "is valid"); + return false; + } + else + { + UINFO("Using extrinsics from config file (sensor local transforms not available)"); + } + + // Set the global extrinsics used by imuConverter + extRot = Eigen::Map >(config.extRotV.data()); + extRPY = Eigen::Map >(config.extRPYV.data()); + extTrans = Eigen::Map >(config.extTransV.data()); + extQRPY = Eigen::Quaterniond(extRPY).inverse(); + + lioSam_ = new lio_sam::LioSamCore(config); + + // Replay buffered IMU samples + UINFO("Replaying %d buffered IMU samples into LIO-SAM", (int)imuBuffer_.size()); + for(const ImuSample & s : imuBuffer_) + { + lioSam_->addImu(s.stamp, s.acc, s.gyro, s.orientation); + } + imuBuffer_.clear(); + + return true; +} +#endif + +Transform OdometryLIOSAM::computeTransform( + SensorData & data, + const Transform & guess, + OdometryInfo * info) +{ + Transform t; +#ifdef RTABMAP_LIOSAM + UTimer timer; + UTimer timerTotal; + + // Handle async IMU data (canProcessAsyncIMU() == true means + // the base class sends IMU-only data directly to computeTransform) + if(!data.imu().empty()) + { + Eigen::Quaterniond qd( + data.imu().orientation()[3], // w + data.imu().orientation()[0], // x + data.imu().orientation()[1], // y + data.imu().orientation()[2]); // z + Eigen::Vector3d acc( + data.imu().linearAcceleration()[0], + data.imu().linearAcceleration()[1], + data.imu().linearAcceleration()[2]); + Eigen::Vector3d gyro( + data.imu().angularVelocity()[0], + data.imu().angularVelocity()[1], + data.imu().angularVelocity()[2]); + + // Deferred initialization: need both IMU and lidar local transforms + // to compute T_lidar_imu extrinsics for LIO-SAM. + if(!lioSam_) + { + // Cache IMU local transform when first available + if(imuLocalTransform_.isNull() && !data.imu().localTransform().isNull()) + { + imuLocalTransform_ = data.imu().localTransform(); + } + + // Try to initialize if we have both transforms + if(!imuLocalTransform_.isNull() && !data.laserScanRaw().isEmpty() && + !data.laserScanRaw().localTransform().isNull()) + { + if(!init(imuLocalTransform_, data.laserScanRaw().localTransform())) + { + UERROR("Failed to initialize LIO-SAM"); + return t; + } + } + else + { + // Buffer IMU until we can initialize + ImuSample s; + s.stamp = data.stamp(); + s.acc = acc; + s.gyro = gyro; + s.orientation = qd; + imuBuffer_.push_back(s); + + if(data.laserScanRaw().isEmpty()) + { + return t; + } + } + } + + if(lioSam_) + { + lioSam_->addImu(data.stamp(), acc, gyro, qd); + } + + // IMU-only: no pose to return + if(data.laserScanRaw().isEmpty()) + { + return t; + } + } + + if(!lioSam_) + { + // A scan arrived without IMU in the same message. + // Try to init if the IMU local transform was already cached. + if(!imuLocalTransform_.isNull() && !data.laserScanRaw().isEmpty() && + !data.laserScanRaw().localTransform().isNull()) + { + if(!init(imuLocalTransform_, data.laserScanRaw().localTransform())) + { + UERROR("Failed to initialize LIO-SAM"); + return t; + } + } + else + { + UDEBUG("LIO-SAM not yet initialized, waiting for IMU (have=%s) and lidar (need scan) local transforms...", + imuLocalTransform_.isNull() ? "no" : "yes"); + return t; + } + } + + if(data.laserScanRaw().isEmpty()) + { + UERROR("LIO-SAM requires laser scans and the current input is empty. Aborting odometry update..."); + return t; + } + else if(data.laserScanRaw().is2d()) + { + UERROR("LIO-SAM requires 3D laser scans. Aborting odometry update..."); + return t; + } + + cv::Mat covariance = cv::Mat::eye(6, 6, CV_64FC1) * 9999; + if(!lost_) + { + const LaserScan & scan = data.laserScanRaw(); + if(scan.format() != LaserScan::kXYZIRT) + { + UERROR("LIO-SAM requires a scan in format %s (got %s). " + "Populate the scan via util3d::laserScanFromPointCloud() " + "so that per-point ring and time fields are available.", + LaserScan::formatName(LaserScan::kXYZIRT).c_str(), + scan.formatName().c_str()); + return t; + } + + // Split the kXYZIRT scan into the three parallel buffers LIO-SAM expects. + const int numPoints = scan.size(); + const int ringOffset = scan.getRingOffset(); + const int timeOffset = scan.getTimeOffset(); + pcl::PointCloud::Ptr laserCloudIn(new pcl::PointCloud); + laserCloudIn->reserve(numPoints); + std::vector rings; + std::vector times; + rings.reserve(numPoints); + times.reserve(numPoints); + for(int i=0; i(row, col); + pcl::PointXYZI pt; + pt.x = ptr[0]; + pt.y = ptr[1]; + pt.z = ptr[2]; + pt.intensity = ptr[3]; + laserCloudIn->push_back(pt); + rings.push_back(static_cast(ptr[ringOffset])); + times.push_back(ptr[timeOffset]); + } + UDEBUG("Scan split: %fs, points=%d", timer.ticks(), (int)laserCloudIn->size()); + + // Process scan. Retrieve the deskewed (motion-compensated) cloud + // produced by LIO-SAM's image projection stage so we can propagate + // it back into SensorData: otherwise downstream consumers such as + // loop closure registration would still see the raw pre-deskew scan. + Eigen::Affine3f poseOut; + Eigen::MatrixXd covOut; + pcl::PointCloud::Ptr deskewedCloud(new pcl::PointCloud); + bool ok = lioSam_->processScan(data.stamp(), laserCloudIn, rings, times, poseOut, covOut, deskewedCloud); + UDEBUG("LIO-SAM process: %fs", timer.ticks()); + + if(ok) + { + // Replace the raw scan on SensorData with LIO-SAM's deskewed + // cloud so downstream stages (loop closure registration in + // particular) use the motion-compensated points instead of + // the raw pre-deskew scan. The deskewed cloud is still in the + // lidar frame, so the existing localTransform/rangeMax apply. + if(deskewedCloud && !deskewedCloud->empty()) + { + const LaserScan & rawScan = data.laserScanRaw(); + LaserScan deskewedScan( + util3d::laserScanFromPointCloud(*deskewedCloud), + rawScan.maxPoints(), + rawScan.rangeMax(), + rawScan.localTransform()); + data.setLaserScan(deskewedScan); + UDEBUG("Replaced raw scan with deskewed cloud (%d -> %d points)", + (int)laserCloudIn->size(), (int)deskewedCloud->size()); + } + + Transform pose = Transform::fromEigen3f(poseOut); + + if(!pose.isNull()) + { + covariance = cv::Mat::eye(6, 6, CV_64FC1); + covariance(cv::Range(0, 3), cv::Range(0, 3)) *= linVar_; + covariance(cv::Range(3, 6), cv::Range(3, 6)) *= angVar_; + + t = lastPose_.inverse() * pose; // incremental + lastPose_ = pose; + + const Transform & localTransform = data.laserScanRaw().localTransform(); + if(!t.isNull() && !t.isIdentity() && !localTransform.isIdentity() && !localTransform.isNull()) + { + // from laser frame to base frame + t = localTransform * t * localTransform.inverse(); + } + + if(info) + { + info->type = (int)kTypeLIOSAM; + if(covariance.cols == 6 && covariance.rows == 6 && covariance.type() == CV_64FC1) + { + info->reg.covariance = covariance; + } + + if(this->isInfoDataFilled()) + { + pcl::PointCloud::Ptr localMap = lioSam_->getLocalMap(); + if(localMap && !localMap->empty()) + { + info->localScanMapSize = localMap->size(); + info->localScanMap = LaserScan(util3d::laserScanFromPointCloud(*localMap), 0, data.laserScanRaw().rangeMax(), data.laserScanRaw().localTransform()); + } + UDEBUG("Fill info data: %fs", timer.ticks()); + } + } + } + else + { + lost_ = true; + UWARN("LIO-SAM failed to register the latest scan, odometry should be reset."); + } + } + else + { + UDEBUG("LIO-SAM processScan returned false (may be initializing)"); + } + } + UINFO("LIO-SAM odom update time = %fs, lost=%s", timerTotal.elapsed(), lost_ ? "true" : "false"); + +#else + UERROR("RTAB-Map is not built with LIO-SAM support! Select another odometry approach."); +#endif + return t; +} + +} // namespace rtabmap diff --git a/corelib/src/odometry/OdometryMono.cpp b/corelib/src/odometry/OdometryMono.cpp index 7df312e7..b942aa88 100644 --- a/corelib/src/odometry/OdometryMono.cpp +++ b/corelib/src/odometry/OdometryMono.cpp @@ -239,7 +239,7 @@ Transform OdometryMono::computeTransform(SensorData & data, const Transform & gu { UDEBUG(""); bool newPtsAdded = false; - const Signature * newS = memory_->getLastWorkingSignature(); + const Signature * newS = memory_->getLastWorkingSignature(false); UDEBUG("newWords=%d", (int)newS->getWords().size()); nFeatures = (int)newS->getWords().size(); if((int)newS->getWords().size() > minInliers_) @@ -646,7 +646,7 @@ Transform OdometryMono::computeTransform(SensorData & data, const Transform & gu info->type = 1; } - const Signature * refS = memory_->getLastWorkingSignature(); + const Signature * refS = memory_->getLastWorkingSignature(false); std::vector refCorners(firstFrameGuessCorners_.size()); std::vector refCornersGuess(firstFrameGuessCorners_.size()); @@ -775,6 +775,7 @@ Transform OdometryMono::computeTransform(SensorData & data, const Transform & gu cameraTransform, fundMatrixReprojError_, fundMatrixConfidence_, + 4, refWords3Guess); // for scale estimation if(cameraTransform.getNorm() < minTranslation_*5) @@ -803,10 +804,10 @@ Transform OdometryMono::computeTransform(SensorData & data, const Transform & gu if(!refWords3.empty()) { UDEBUG("Added %d/%d valid 3D features", (int)refWords3.size(), (int)localMap_.size()); - keyFrameWords3D_.insert(std::make_pair(memory_->getLastWorkingSignature()->id(), refWords3)); + keyFrameWords3D_.insert(std::make_pair(memory_->getLastWorkingSignature(false)->id(), refWords3)); } - keyFramePoses_.insert(std::make_pair(memory_->getLastWorkingSignature()->id(), this->getPose())); - keyFrameModels_.insert(std::make_pair(memory_->getLastWorkingSignature()->id(), newModel)); + keyFramePoses_.insert(std::make_pair(memory_->getLastWorkingSignature(false)->id(), this->getPose())); + keyFrameModels_.insert(std::make_pair(memory_->getLastWorkingSignature(false)->id(), newModel)); } } else @@ -828,7 +829,7 @@ Transform OdometryMono::computeTransform(SensorData & data, const Transform & gu // generate kpts if(memory_->update(SensorData(data))) { - const Signature * s = memory_->getLastWorkingSignature(); + const Signature * s = memory_->getLastWorkingSignature(false); const std::multimap & words = s->getWords(); if((int)words.size() > minInliers_ && !s->getWordsKpts().empty()) { diff --git a/corelib/src/odometry/OdometryORBSLAM3.cpp b/corelib/src/odometry/OdometryORBSLAM3.cpp index 7cde70a5..f0e5d644 100644 --- a/corelib/src/odometry/OdometryORBSLAM3.cpp +++ b/corelib/src/odometry/OdometryORBSLAM3.cpp @@ -32,6 +32,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "rtabmap/utilite/UTimer.h" #include "rtabmap/utilite/UStl.h" #include "rtabmap/utilite/UDirectory.h" +#include "rtabmap/utilite/UFile.h" #include #include #include @@ -116,6 +117,13 @@ bool OdometryORBSLAM3::init(const rtabmap::CameraModel & model1, const rtabmap:: } //Load ORB Vocabulary vocabularyPath = uReplaceChar(vocabularyPath, '~', UDirectory::homeDir()); + if(!UFile::exists(vocabularyPath)) + { + UERROR("ORB_SLAM vocabulary path \"%s\" doesn't exist! (Parameter name=\"%s\")", + vocabularyPath.c_str(), + rtabmap::Parameters::kOdomORBSLAMVocPath().c_str()); + return false; + } UWARN("Loading ORB Vocabulary: \"%s\". This could take a while...", vocabularyPath.c_str()); // Create configuration file @@ -240,7 +248,7 @@ bool OdometryORBSLAM3::init(const rtabmap::CameraModel & model1, const rtabmap:: //# IMU Parameters TODO: hard-coded, not used //#-------------------------------------------------------------------------------------------- // Transformation from camera 0 to body-frame (imu) - rtabmap::Transform camImuT = model1.localTransform()*imuLocalTransform_; + rtabmap::Transform camImuT = imuLocalTransform_.inverse()*model1.localTransform(); ofs << "IMU.T_b_c1: !!opencv-matrix" << std::endl; ofs << " rows: 4" << std::endl; ofs << " cols: 4" << std::endl; @@ -340,14 +348,16 @@ bool OdometryORBSLAM3::init(const rtabmap::CameraModel & model1, const rtabmap:: ofs.close(); + ORB_SLAM3::System::eSensor sensor = + stereo?(withIMU?ORB_SLAM3::System::IMU_STEREO:ORB_SLAM3::System::STEREO): + (withIMU?ORB_SLAM3::System::IMU_RGBD:ORB_SLAM3::System::RGBD); + UINFO("Initializing ORB_SLAM3 system with sensor %d...", (int)sensor); 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, + sensor, false); + UINFO("Initializing ORB_SLAM3 system with sensor %d... done!", (int)sensor); return true; #else UERROR("RTAB-Map is not built with ORB_SLAM support! Select another visual odometry approach."); @@ -373,6 +383,7 @@ Transform OdometryORBSLAM3::computeTransform( { if(lastImuStamp_ == 0.0 || lastImuStamp_ < data.stamp()) { + UDEBUG("Adding IMU %f", data.stamp()); orbslamImus_.push_back(ORB_SLAM3::IMU::Point( data.imu().linearAcceleration().val[0], data.imu().linearAcceleration().val[1], @@ -419,7 +430,14 @@ Transform OdometryORBSLAM3::computeTransform( (data.stereoCameraModels().size() == 1 && data.stereoCameraModels()[0].isValidForProjection()))) { - UERROR("Invalid camera model!"); + if(data.cameraModels().size() > 1 || data.stereoCameraModels().size() > 1) + { + UERROR("Multi-camera not supported with ORB_SLAM integration!"); + } + else + { + UERROR("Invalid camera model!"); + } return t; } @@ -432,6 +450,7 @@ Transform OdometryORBSLAM3::computeTransform( if(lastImageStamp_ == 0.0) { lastImageStamp_ = data.stamp(); + UDEBUG("Waiting for another image to initialize..."); return t; } @@ -457,6 +476,7 @@ Transform OdometryORBSLAM3::computeTransform( rightMono = cv::Mat(); cv::cvtColor(data.imageRaw(), rightMono, CV_BGR2GRAY); } + UDEBUG("Adding Stereo Frame %f", data.stamp()); Tcw = orbslam_->TrackStereo(leftMono, rightMono, data.stamp(), orbslamImus_); orbslamImus_.clear(); } @@ -472,15 +492,22 @@ Transform OdometryORBSLAM3::computeTransform( { depth = util2d::cvtDepthToFloat(data.depthRaw()); } + UDEBUG("Adding RGBD Frame %f", data.stamp()); 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()) + std::vector trackedMapPoints = orbslam_->GetTrackedMapPoints(); + if(orbslam_->isLost() || trackedMapPoints.empty()) { covariance = cv::Mat::eye(6,6,CV_64FC1)*9999.0f; + if(!imuLocalTransform_.isNull()) { + UWARN("ORBSLAM lost tracking! If it is on initialization, try moving the sensor in a circle for a couple of seconds."); + } + else { + UWARN("ORBSLAM lost tracking!"); + } } else { @@ -490,14 +517,16 @@ Transform OdometryORBSLAM3::computeTransform( if(!p.isNull()) { - if(!localTransform.isNull()) + if(!imuLocalTransform_.isNull()) { - if(originLocalTransform_.isNull()) - { - originLocalTransform_ = localTransform; - } - // transform in base frame - p = originLocalTransform_ * p.inverse() * localTransform.inverse(); + // Transform p from optical-imu system (x->left, y->back and z->up) to ros system, then remove camera local transform + p = Transform(0,0,0,0,0,-M_PI/2) * p.inverse() * localTransform.inverse(); + } + else + { + UASSERT(!localTransform.isNull()); + // Transform p from optical system (x->right, y->down and z->forward) to ros system, then remove camera local transform + p = CameraModel::opticalRotation() * p.inverse() * localTransform.inverse(); } t = previousPoseInv*p; } @@ -534,12 +563,14 @@ Transform OdometryORBSLAM3::computeTransform( } } + size_t mapPointsSize = 0; if(info) { info->lost = t.isNull(); info->type = (int)kTypeORBSLAM; info->reg.covariance = covariance; - info->localMapSize = mapPoints.size(); + std::vector mapPoints = orbslam_->GetAllMapPoints(); + info->localMapSize = mapPointsSize = mapPoints.size(); info->localKeyFrames = 0; if(this->isInfoDataFilled()) @@ -549,20 +580,20 @@ Transform OdometryORBSLAM3::computeTransform( info->reg.inliersIDs.resize(kpts.size()); int oi = 0; - UASSERT(mapPoints.size() == kpts.size()); + UASSERT(trackedMapPoints.size() == kpts.size()); for (unsigned int i = 0; i < kpts.size(); ++i) { int wordId; - if(mapPoints[i] != 0) + if(trackedMapPoints[i] != 0) { - wordId = mapPoints[i]->mnId; + wordId = trackedMapPoints[i]->mnId; } else { wordId = -(i+1); } info->words.insert(std::make_pair(wordId, kpts[i])); - if(mapPoints[i] != 0) + if(trackedMapPoints[i] != 0) { info->reg.matchesIDs[oi] = wordId; info->reg.inliersIDs[oi] = wordId; @@ -574,7 +605,15 @@ Transform OdometryORBSLAM3::computeTransform( info->reg.inliers = oi; info->reg.matches = oi; - Eigen::Affine3f fixRot = (this->getPose()*previousPoseInv*originLocalTransform_).toEigen3f(); + Eigen::Affine3f fixRot; + if(!imuLocalTransform_.isNull()) + { + fixRot = (this->getPose()*previousPoseInv*Transform(0,0,0,0,0,-M_PI/2)).toEigen3f(); + } + else + { + fixRot = (this->getPose()*previousPoseInv*CameraModel::opticalRotation()).toEigen3f(); + } for (unsigned int i = 0; i < mapPoints.size(); ++i) { if(mapPoints[i]) @@ -587,7 +626,8 @@ Transform OdometryORBSLAM3::computeTransform( } } - UINFO("Odom update time = %fs, map points=%ld, lost=%s", timer.elapsed(), mapPoints.size(), t.isNull()?"true":"false"); + UINFO("Odom update time = %fs, tracked points=%ld, map points=%ld, lost=%s", + timer.elapsed(), trackedMapPoints.size(), mapPointsSize, t.isNull()?"true":"false"); #else UERROR("RTAB-Map is not built with ORB_SLAM support! Select another visual odometry approach."); diff --git a/corelib/src/odometry/OdometryOpenVINS.cpp b/corelib/src/odometry/OdometryOpenVINS.cpp index da81f7ee..e9539688 100644 --- a/corelib/src/odometry/OdometryOpenVINS.cpp +++ b/corelib/src/odometry/OdometryOpenVINS.cpp @@ -152,6 +152,85 @@ OdometryOpenVINS::OdometryOpenVINS(const ParametersMap & parameters) : 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; + + if(parameters.find(Parameters::kOdomOpenVINSConfigPath()) != parameters.end()) + { + // Load the config: will override all parameters above! + std::string configPath = parameters.at(Parameters::kOdomOpenVINSConfigPath()); + if(!configPath.empty()) + { + if(UFile::exists(configPath)) + { + UWARN("OpenVINS config file is provided (%s=\"%s\"), reading it. The parameters from the config file will overwrite OdomOpenVINS/*** parameters.", + Parameters::kOdomOpenVINSConfigPath().c_str(), configPath.c_str()); + auto parser = std::make_shared(configPath); + + // The sequence of loading is based on VioManagerOptions::print_and_load() + // We removed all parts about intrinsics/extrinsics, which will be loaded later + // when we receive the data (which should already include intrinsics and extrinsics). + + params_->state_options.print(parser); + + params_->init_options.print_and_load_initializer(parser); + params_->init_options.print_and_load_noise(parser); + parser->parse_config("gravity_mag", params_->init_options.gravity_mag); + parser->parse_config("max_cameras", params_->init_options.num_cameras); + parser->parse_config("use_stereo", params_->init_options.use_stereo); + parser->parse_config("downsample_cameras", params_->init_options.downsample_cameras); + + parser->parse_config("dt_slam_delay", params_->dt_slam_delay); + parser->parse_config("try_zupt", params_->try_zupt); + parser->parse_config("zupt_max_velocity",params_-> zupt_max_velocity); + parser->parse_config("zupt_noise_multiplier", params_->zupt_noise_multiplier); + parser->parse_config("zupt_max_disparity", params_->zupt_max_disparity); + parser->parse_config("zupt_only_at_beginning", params_->zupt_only_at_beginning); + parser->parse_config("record_timing_information", params_->record_timing_information); + parser->parse_config("record_timing_filepath", params_->record_timing_filepath); + + params_->print_and_load_trackers(parser); + params_->print_and_load_noise(parser); + + if(params_->state_options.num_cameras > 2) + { + UFATAL("OpenVINS integration in RTAB-Map doesn't support more than 2 cameras (num_cameras=%d).", params_->state_options.num_cameras); + } + + parser->parse_config("gravity_mag", params_->gravity_mag); + parser->parse_config("use_mask", params_->use_mask); + params_->masks.clear(); + if (params_->use_mask) { + for (int i = 0; i < params_->state_options.num_cameras; i++) { + std::string mask_path; + std::string mask_node = "mask" + std::to_string(i); + parser->parse_config(mask_node, mask_path); + std::string total_mask_path = parser->get_config_folder() + mask_path; + if (!boost::filesystem::exists(total_mask_path)) { + PRINT_ERROR(RED "VioManager(): invalid mask path:\n" RESET); + PRINT_ERROR(RED "\t- mask%d - %s\n" RESET, i, total_mask_path.c_str()); + std::exit(EXIT_FAILURE); + } + params_->masks.emplace(i, cv::imread(total_mask_path, cv::IMREAD_GRAYSCALE)); + } + } + + if (!parser->successful()) { + UWARN("Not all expected OpenVINS parameters were read successfully " + "from \"%s\". Values from RTAB-Map's OpenOpenVINS/* parameters " + "will be used instead for the missing ones.", + configPath.c_str()); + } + else { + UINFO("OpenVINS config file(%s=\"%s\") read.", + Parameters::kOdomOpenVINSConfigPath().c_str(), configPath.c_str()); + } + } + else + { + UERROR("OpenVINS config file is provided (%s=\"%s\") but it doesn't exist!", + Parameters::kOdomOpenVINSConfigPath().c_str(), configPath.c_str()); + } + } + } #endif } diff --git a/corelib/src/odometry/OdometryVINS.cpp b/corelib/src/odometry/OdometryVINS.cpp deleted file mode 100644 index aa18393c..00000000 --- a/corelib/src/odometry/OdometryVINS.cpp +++ /dev/null @@ -1,516 +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. - */ - -#include "rtabmap/core/odometry/OdometryVINS.h" -#include "rtabmap/core/OdometryInfo.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/UThread.h" -#include "rtabmap/utilite/UDirectory.h" -#include - -#ifdef RTABMAP_VINS -#include -#include -#include -#include -#include -#endif - -namespace rtabmap { - -#ifdef RTABMAP_VINS -class VinsEstimator: public Estimator -{ -public: - VinsEstimator( - const Transform & imuLocalTransform, - const StereoCameraModel & model, - bool rectified) : Estimator() - { - MULTIPLE_THREAD = 0; - setParameter(); - - //overwrite camera calibration only if received model is radtan, otherwise use config - UASSERT(NUM_OF_CAM >= 1 && NUM_OF_CAM <=2); - - if( (NUM_OF_CAM == 2 && model.left().D_raw().cols == 4 && model.right().D_raw().cols == 4) || - (NUM_OF_CAM == 1 && model.left().D_raw().cols == 4)) - { - UWARN("Overwriting VINS camera calibration config with received pinhole model... rectified=%d", rectified?1:0); - featureTracker.m_camera.clear(); - - camodocal::PinholeCameraPtr camera( new camodocal::PinholeCamera ); - camodocal::PinholeCamera::Parameters params( - model.name(), - model.left().imageWidth(), model.left().imageHeight(), - rectified?0:model.left().D_raw().at(0,0), - rectified?0:model.left().D_raw().at(0,1), - rectified?0:model.left().D_raw().at(0,2), - rectified?0:model.left().D_raw().at(0,3), - rectified?model.left().fx():model.left().K_raw().at(0,0), - rectified?model.left().fy():model.left().K_raw().at(1,1), - rectified?model.left().cx():model.left().K_raw().at(0,2), - rectified?model.left().cy():model.left().K_raw().at(1,2)); - camera->setParameters(params); - featureTracker.m_camera.push_back(camera); - - if(NUM_OF_CAM == 2) - { - camodocal::PinholeCameraPtr camera( new camodocal::PinholeCamera ); - camodocal::PinholeCamera::Parameters params( - model.name(), - model.right().imageWidth(), model.right().imageHeight(), - rectified?0:model.right().D_raw().at(0,0), - rectified?0:model.right().D_raw().at(0,1), - rectified?0:model.right().D_raw().at(0,2), - rectified?0:model.right().D_raw().at(0,3), - rectified?model.right().fx():model.right().K_raw().at(0,0), - rectified?model.right().fy():model.right().K_raw().at(1,1), - rectified?model.right().cx():model.right().K_raw().at(0,2), - rectified?model.right().cy():model.right().K_raw().at(1,2)); - camera->setParameters(params); - featureTracker.m_camera.push_back(camera); - } - } - else if(rectified) - { - UWARN("Images are rectified but received calibration cannot be " - "used, make sure calibration in config file doesn't have " - "distortion or send raw images to VINS odometry."); - if(!featureTracker.m_camera.empty()) - { - if(featureTracker.m_camera.front()->imageWidth() != model.left().imageWidth() || - featureTracker.m_camera.front()->imageHeight() != model.left().imageHeight()) - { - UERROR("Received images don't have same size (%dx%d) than in the config file (%dx%d)!", - model.left().imageWidth(), - model.left().imageHeight(), - featureTracker.m_camera.front()->imageWidth(), - featureTracker.m_camera.front()->imageHeight()); - } - } - } - - Transform imuCam0 = imuLocalTransform.inverse() * model.localTransform(); - - tic[0] = Vector3d(imuCam0.x(), imuCam0.y(), imuCam0.z()); - ric[0] = imuCam0.toEigen4d().block<3,3>(0,0); - - if(NUM_OF_CAM == 2) - { - Transform cam0cam1; - if(rectified) - { - cam0cam1 = Transform( - 1, 0, 0, model.baseline(), - 0, 1, 0, 0, - 0, 0, 1, 0); - } - else - { - cam0cam1 = model.stereoTransform().inverse(); - } - UASSERT(!cam0cam1.isNull()); - Transform imuCam1 = imuCam0 * cam0cam1; - - tic[1] = Vector3d(imuCam1.x(), imuCam1.y(), imuCam1.z()); - ric[1] = imuCam1.toEigen4d().block<3,3>(0,0); - } - - for (int i = 0; i < NUM_OF_CAM; i++) - { - cout << " exitrinsic cam " << i << endl << ric[i] << endl << tic[i].transpose() << endl; - } - f_manager.setRic(ric); - ProjectionTwoFrameOneCamFactor::sqrt_info = FOCAL_LENGTH / 1.5 * Matrix2d::Identity(); - ProjectionTwoFrameTwoCamFactor::sqrt_info = FOCAL_LENGTH / 1.5 * Matrix2d::Identity(); - ProjectionOneFrameTwoCamFactor::sqrt_info = FOCAL_LENGTH / 1.5 * Matrix2d::Identity(); - td = TD; - g = G; - cout << "set g " << g.transpose() << endl; - } - - // Copy of original inputImage() so that overridden processMeasurements() is used and threading is disabled. - void inputImage(double t, const cv::Mat &_img, const cv::Mat &_img1) - { - inputImageCnt++; - map>>> featureFrame; - TicToc featureTrackerTime; - if(_img1.empty()) - featureFrame = featureTracker.trackImage(t, _img); - else - featureFrame = featureTracker.trackImage(t, _img, _img1); - //printf("featureTracker time: %f\n", featureTrackerTime.toc()); - - //if(MULTIPLE_THREAD) - //{ - // if(inputImageCnt % 2 == 0) - // { - // mBuf.lock(); - // featureBuf.push(make_pair(t, featureFrame)); - // mBuf.unlock(); - // } - //} - //else - { - mBuf.lock(); - featureBuf.push(make_pair(t, featureFrame)); - mBuf.unlock(); - TicToc processTime; - processMeasurements(); - UDEBUG("VINS process time: %f", processTime.toc()); - } - - } - - // Copy of original inputIMU() but with publisher commented - void inputIMU(double t, const Vector3d &linearAcceleration, const Vector3d &angularVelocity) - { - mBuf.lock(); - accBuf.push(make_pair(t, linearAcceleration)); - gyrBuf.push(make_pair(t, angularVelocity)); - //printf("input imu with time %f \n", t); - mBuf.unlock(); - - fastPredictIMU(t, linearAcceleration, angularVelocity); - //if (solver_flag == NON_LINEAR) - // pubLatestOdometry(latest_P, latest_Q, latest_V, t); - } - - // Copy of original processMeasurements() but with publishers commented and threading disabled - void processMeasurements() - { - //while (1) - { - //printf("process measurments\n"); - pair > > > > feature; - vector> accVector, gyrVector; - if(!featureBuf.empty()) - { - feature = featureBuf.front(); - curTime = feature.first + td; - //while(1) - //{ - if (!((!USE_IMU || IMUAvailable(feature.first + td)))) - //if ((!USE_IMU || IMUAvailable(feature.first + td))) - // break; - //else - { - printf("wait for imu ... \n"); - //if (! MULTIPLE_THREAD) - return; - //std::chrono::milliseconds dura(5); - //std::this_thread::sleep_for(dura); - } - //} - mBuf.lock(); - if(USE_IMU) - getIMUInterval(prevTime, curTime, accVector, gyrVector); - - featureBuf.pop(); - mBuf.unlock(); - - if(USE_IMU) - { - if(!initFirstPoseFlag) - initFirstIMUPose(accVector); - UDEBUG("accVector.size() = %d", accVector.size()); - for(size_t i = 0; i < accVector.size(); i++) - { - double dt; - if(i == 0) - dt = accVector[i].first - prevTime; - else if (i == accVector.size() - 1) - dt = curTime - accVector[i - 1].first; - else - dt = accVector[i].first - accVector[i - 1].first; - processIMU(accVector[i].first, dt, accVector[i].second, gyrVector[i].second); - } - } - - processImage(feature.second, feature.first); - - prevTime = curTime; - - printStatistics(*this, 0); - - //std_msgs::Header header; - //header.frame_id = "world"; - //header.stamp = ros::Time(feature.first); - - //pubOdometry(*this, header); - //pubKeyPoses(*this, header); - //pubCameraPose(*this, header); - //pubPointCloud(*this, header); - //pubKeyframe(*this); - //pubTF(*this, header); - } - - //if (! MULTIPLE_THREAD) - // break; - - //std::chrono::milliseconds dura(2); - //std::this_thread::sleep_for(dura); - } - } -}; -#endif - -OdometryVINS::OdometryVINS(const ParametersMap & parameters) : - Odometry(parameters) -#ifdef RTABMAP_VINS - , - vinsEstimator_(0), - initGravity_(false), - previousPose_(Transform::getIdentity()) -#endif -{ -#ifdef RTABMAP_VINS - // intialize - std::string configFilename; - Parameters::parse(parameters, Parameters::kOdomVINSConfigPath(), configFilename); - if(configFilename.empty()) - { - UERROR("VINS config file is empty (%s=%s)!", - Parameters::kOdomVINSConfigPath().c_str(), - Parameters::kOdomVINSConfigPath().c_str()); - } - else - { - readParameters(uReplaceChar(configFilename, '~', UDirectory::homeDir())); - } -#endif -} - -OdometryVINS::~OdometryVINS() -{ -#ifdef RTABMAP_VINS - delete vinsEstimator_; -#endif -} - -void OdometryVINS::reset(const Transform & initialPose) -{ - Odometry::reset(initialPose); -#ifdef RTABMAP_VINS - if(!initGravity_) - { - delete vinsEstimator_; - vinsEstimator_ = 0; - previousPose_.setIdentity(); - lastImu_ = IMU(); - previousLocalTransform_.setNull(); - } - initGravity_ = false; -#endif -} - -// return not null transform if odometry is correctly computed -Transform OdometryVINS::computeTransform( - SensorData & data, - const Transform & guess, - OdometryInfo * info) -{ - Transform t; -#ifdef RTABMAP_VINS - UTimer timer; - - if(USE_IMU!=0 && !data.imu().empty()) - { - double t = data.stamp(); - double dx = data.imu().linearAcceleration().val[0]; - double dy = data.imu().linearAcceleration().val[1]; - double dz = data.imu().linearAcceleration().val[2]; - double rx = data.imu().angularVelocity().val[0]; - double ry = data.imu().angularVelocity().val[1]; - double rz = data.imu().angularVelocity().val[2]; - Vector3d acc(dx, dy, dz); - Vector3d gyr(rx, ry, rz); - - UDEBUG("IMU update stamp=%f", data.stamp()); - - if(vinsEstimator_ != 0) - { - vinsEstimator_->inputIMU(t, acc, gyr); - } - else - { - lastImu_ = data.imu(); - UWARN("Waiting an image for initialization..."); - } - } - - if(!data.imageRaw().empty() && !data.rightRaw().empty() && data.stereoCameraModels().size() == 1 && data.stereoCameraModels()[0].isValidForProjection()) - { - if(USE_IMU==1 && lastImu_.localTransform().isNull()) - { - UWARN("Waiting IMU for initialization..."); - return t; - } - if(vinsEstimator_ == 0) - { - // intialize - vinsEstimator_ = new VinsEstimator( - lastImu_.localTransform().isNull()?Transform::getIdentity():lastImu_.localTransform(), - data.stereoCameraModels()[0], - this->imagesAlreadyRectified()); - } - - UDEBUG("Image update stamp=%f", data.stamp()); - cv::Mat left; - cv::Mat right; - if(data.imageRaw().type() == CV_8UC3) - { - cv::cvtColor(data.imageRaw(), left, CV_BGR2GRAY); - } - 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!"); - } - - vinsEstimator_->inputImage(data.stamp(), left, right); - - if(vinsEstimator_->solver_flag == Estimator::NON_LINEAR) - { - Quaterniond tmp_Q; - tmp_Q = Quaterniond(vinsEstimator_->Rs[WINDOW_SIZE]); - Transform p( - vinsEstimator_->Ps[WINDOW_SIZE].x(), - vinsEstimator_->Ps[WINDOW_SIZE].y(), - vinsEstimator_->Ps[WINDOW_SIZE].z(), - tmp_Q.x(), - tmp_Q.y(), - tmp_Q.z(), - tmp_Q.w()); - - if(!p.isNull()) - { - if(!lastImu_.localTransform().isNull()) - { - p = p * lastImu_.localTransform().inverse(); - } - - if(this->getPose().rotation().isIdentity()) - { - initGravity_ = true; - this->reset(this->getPose()*p.rotation()); - } - - if(previousPose_.isIdentity()) - { - previousPose_ = p; - } - - // make it incremental - Transform previousPoseInv = previousPose_.inverse(); - t = previousPoseInv*p; - previousPose_ = p; - - if(info) - { - info->type = this->getType(); - info->reg.covariance = cv::Mat::eye(6,6, CV_64FC1); - info->reg.covariance *= this->framesProcessed() == 0?9999:0.0001; - - // feature map - Transform fixT = this->getPose()*previousPoseInv; - for (auto &it_per_id : vinsEstimator_->f_manager.feature) - { - int used_num; - used_num = it_per_id.feature_per_frame.size(); - if (!(used_num >= 2 && it_per_id.start_frame < WINDOW_SIZE - 2)) - continue; - if (it_per_id.start_frame > WINDOW_SIZE * 3.0 / 4.0 || it_per_id.solve_flag != 1) - continue; - int imu_i = it_per_id.start_frame; - Vector3d pts_i = it_per_id.feature_per_frame[it_per_id.feature_per_frame.size()-1].point * it_per_id.estimated_depth; - Vector3d w_pts_i = vinsEstimator_->Rs[imu_i] * (vinsEstimator_->ric[0] * pts_i + vinsEstimator_->tic[0]) + vinsEstimator_->Ps[imu_i]; - - cv::Point3f p; - p.x = w_pts_i(0); - p.y = w_pts_i(1); - p.z = w_pts_i(2); - p = util3d::transformPoint(p, fixT); - info->localMap.insert(std::make_pair(it_per_id.feature_id, p)); - - if(this->imagesAlreadyRectified()) - { - cv::Point2f pt; - data.stereoCameraModels()[0].left().reproject(pts_i(0), pts_i(1), pts_i(2), pt.x, pt.y); - info->reg.inliersIDs.push_back(info->newCorners.size()); - info->newCorners.push_back(pt); - } - } - info->features = info->newCorners.size(); - info->localMapSize = info->localMap.size(); - } - UINFO("Odom update time = %fs p=%s", timer.elapsed(), p.prettyPrint().c_str()); - } - } - else - { - UWARN("VINS not yet initialized... waiting to get enough IMU messages"); - } - } - else if(!data.imageRaw().empty() && !data.depthRaw().empty()) - { - UERROR("VINS-Fusion doesn't work with RGB-D data, stereo images are required!"); - } - else if(!data.imageRaw().empty() && data.depthOrRightRaw().empty()) - { - UERROR("VINS-Fusion requires stereo images!"); - } - else if(data.imu().empty()) - { - UERROR("VINS-Fusion requires stereo images (and only one stereo camera with valid calibration)!"); - } -#else - UERROR("RTAB-Map is not built with VINS support! Select another visual odometry approach."); -#endif - return t; -} - -} // namespace rtabmap diff --git a/corelib/src/odometry/OdometryVINSFusion.cpp b/corelib/src/odometry/OdometryVINSFusion.cpp new file mode 100644 index 00000000..9c633605 --- /dev/null +++ b/corelib/src/odometry/OdometryVINSFusion.cpp @@ -0,0 +1,581 @@ +/* +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/odometry/OdometryVINSFusion.h" +#include "rtabmap/core/OdometryInfo.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/UThread.h" +#include "rtabmap/utilite/UDirectory.h" +#include + +#ifdef RTABMAP_VINS_FUSION +#include +#include +#include +#include +#include +#endif + +namespace rtabmap { + +#ifdef RTABMAP_VINS_FUSION +class VinsFusionEstimator: public Estimator +{ +public: + VinsFusionEstimator() : Estimator() + {} + + bool init(const Transform & imuLocalTransform, + const StereoCameraModel & model, + bool rectified) + { + MULTIPLE_THREAD = 0; + setParameter(); + + ROW=model.left().imageHeight(); + COL=model.left().imageWidth(); + + //overwrite camera calibration only if received model is radtan, otherwise use config + UASSERT(NUM_OF_CAM >= 1 && NUM_OF_CAM <=2); + + if( (NUM_OF_CAM == 2 && (rectified || (model.left().D_raw().cols >= 4 && model.right().D_raw().cols >= 4))) || + (NUM_OF_CAM == 1 && (rectified || model.left().D_raw().cols >= 4))) + { + UINFO("Setting up VINS camera calibration config with received pinhole model... rectified=%d distortion coefficients=%d", + rectified?1:0, model.left().D_raw().cols); + featureTracker.m_camera.clear(); + + double fx = 0.0; + if(!rectified && model.left().D_raw().cols >= 8) + { + if(model.left().D_raw().cols > 8) + { + UWARN("Received %d distortion coefficients, but only the first 8 are supported, ignoring the last coefficents.", + model.left().D_raw().cols); + } + camodocal::PinholeFullCameraPtr camera( new camodocal::PinholeFullCamera ); + camodocal::PinholeFullCamera::Parameters params( + model.name(), + model.left().imageWidth(), model.left().imageHeight(), + model.left().D_raw().at(0,0), // k1 + model.left().D_raw().at(0,1), // k1 + model.left().D_raw().at(0,4), // k3 + model.left().D_raw().at(0,5), // k4 + model.left().D_raw().at(0,6), // k5 + model.left().D_raw().at(0,7), // k6 + model.left().D_raw().at(0,2), // p1 + model.left().D_raw().at(0,3), // p1 + model.left().K_raw().at(0,0), // fx + model.left().K_raw().at(1,1), // fy + model.left().K_raw().at(0,2), // cx + model.left().K_raw().at(1,2)); // cy + camera->setParameters(params); + featureTracker.m_camera.push_back(camera); + fx = params.fx(); + if(NUM_OF_CAM == 2) + { + UASSERT(model.left().D_raw().cols == model.right().D_raw().cols); + camodocal::PinholeFullCameraPtr camera2( new camodocal::PinholeFullCamera ); + camodocal::PinholeFullCamera::Parameters params2( + model.name(), + model.right().imageWidth(), model.right().imageHeight(), + model.right().D_raw().at(0,0), // k1 + model.right().D_raw().at(0,1), // k2 + model.right().D_raw().at(0,4), // k3 + model.right().D_raw().at(0,5), // k4 + model.right().D_raw().at(0,6), // k5 + model.right().D_raw().at(0,7), // k6 + model.right().D_raw().at(0,2), // p1 + model.right().D_raw().at(0,3), // p2 + model.right().K_raw().at(0,0), // fx + model.right().K_raw().at(1,1), // fy + model.right().K_raw().at(0,2), // cx + model.right().K_raw().at(1,2)); // cy + camera2->setParameters(params2); + featureTracker.m_camera.push_back(camera2); + } + } + else + { + if(!rectified) + { + if(model.left().D_raw().cols == 6) { + UERROR("Fisheye camera model support not implemented! Provide rectified images instead (see %s).", + Parameters::kRtabmapImagesAlreadyRectified().c_str()); + return false; + } + if(model.left().D_raw().cols > 4) + { + UWARN("Received %d distortion coefficients, but only 4 or 8 are supported, ignoring the last coefficents.", + model.left().D_raw().cols); + } + } + + camodocal::PinholeCameraPtr camera( new camodocal::PinholeCamera ); + camodocal::PinholeCamera::Parameters params( + model.name(), + model.left().imageWidth(), model.left().imageHeight(), + rectified?0:model.left().D_raw().at(0,0), // k1 + rectified?0:model.left().D_raw().at(0,1), // k2 + rectified?0:model.left().D_raw().at(0,2), // p1 + rectified?0:model.left().D_raw().at(0,3), // p2 + rectified?model.left().fx():model.left().K_raw().at(0,0), + rectified?model.left().fy():model.left().K_raw().at(1,1), + rectified?model.left().cx():model.left().K_raw().at(0,2), + rectified?model.left().cy():model.left().K_raw().at(1,2)); + camera->setParameters(params); + featureTracker.m_camera.push_back(camera); + fx = params.fx(); + if(NUM_OF_CAM == 2) + { + UASSERT(model.left().D_raw().cols == model.right().D_raw().cols); + camodocal::PinholeCameraPtr camera2( new camodocal::PinholeCamera ); + camodocal::PinholeCamera::Parameters params2( + model.name(), + model.right().imageWidth(), model.right().imageHeight(), + rectified?0:model.right().D_raw().at(0,0), // k1 + rectified?0:model.right().D_raw().at(0,1), // k2 + rectified?0:model.right().D_raw().at(0,2), // p1 + rectified?0:model.right().D_raw().at(0,3), // p2 + rectified?model.right().fx():model.right().K_raw().at(0,0), + rectified?model.right().fy():model.right().K_raw().at(1,1), + rectified?model.right().cx():model.right().K_raw().at(0,2), + rectified?model.right().cy():model.right().K_raw().at(1,2)); + camera2->setParameters(params2); + featureTracker.m_camera.push_back(camera2); + } + } + + double originalParalax = MIN_PARALLAX * FOCAL_LENGTH; + // If you have compiler error about FOCAL_LENGTH being const, make sure to use the following patch for ROS1: + // https://gist.github.com/matlabbe/795ab37067367dca58bbadd8201d986c#file-vins-fusion_pull136-patch + // Use this patch for ROS2: https://gist.github.com/matlabbe/ebbb343cd744da9d6d6d6ded2e1557fd + FOCAL_LENGTH = fx; + MIN_PARALLAX = originalParalax / FOCAL_LENGTH; + ProjectionTwoFrameOneCamFactor::sqrt_info = FOCAL_LENGTH / 1.5 * Matrix2d::Identity(); + ProjectionTwoFrameTwoCamFactor::sqrt_info = FOCAL_LENGTH / 1.5 * Matrix2d::Identity(); + ProjectionOneFrameTwoCamFactor::sqrt_info = FOCAL_LENGTH / 1.5 * Matrix2d::Identity(); + } + else + { + UERROR("Received stereo camera model is not compatible with VINS-Fusion."); + if(!rectified && model.left().D_raw().cols != 4) { + UERROR("When raw images are provided (%s=false), we expect 4 distortion coefficients (k1,k2,p1,p2), received %d", + Parameters::kRtabmapImagesAlreadyRectified().c_str(), + model.left().D_raw().cols); + } + return false; + } + + Transform imuCam0 = imuLocalTransform.inverse() * model.localTransform(); + + tic[0] = TIC[0] = Vector3d(imuCam0.x(), imuCam0.y(), imuCam0.z()); + ric[0] = RIC[0] = imuCam0.toEigen4d().block<3,3>(0,0); + + if(NUM_OF_CAM == 2) + { + Transform cam0cam1; + if(rectified) + { + cam0cam1 = Transform( + 1, 0, 0, model.baseline(), + 0, 1, 0, 0, + 0, 0, 1, 0); + } + else + { + cam0cam1 = model.stereoTransform().inverse(); + } + UASSERT(!cam0cam1.isNull()); + Transform imuCam1 = imuCam0 * cam0cam1; + + tic[1] = TIC[0] = Vector3d(imuCam1.x(), imuCam1.y(), imuCam1.z()); + ric[1] = RIC[0] = imuCam1.toEigen4d().block<3,3>(0,0); + } + + for (int i = 0; i < NUM_OF_CAM; i++) + { + cout << " new extrinsic cam " << i << endl << ric[i] << endl << tic[i].transpose() << endl; + } + for (int i = 0; i < NUM_OF_CAM; i++) + { + cout << " new intrinsic cam " << i << endl << featureTracker.m_camera[i]->parametersToString() << endl; + } + f_manager.setRic(ric); + return true; + } + + // Copy of original inputImage() so that overridden processMeasurements() is used and threading is disabled. + void inputImage(double t, const cv::Mat &_img, const cv::Mat &_img1) + { + TicToc processTime; + inputImageCnt++; + map>>> featureFrame; + if(_img1.empty()) { + featureFrame = featureTracker.trackImage(t, _img); + } + else { + featureFrame = featureTracker.trackImage(t, _img, _img1); + } + + mBuf.lock(); + featureBuf.push(make_pair(t, featureFrame)); + mBuf.unlock(); + processMeasurements(); + UDEBUG("VINS process time: %f", processTime.toc()); + } + + // Copy of original inputIMU() but with publisher commented + void inputIMU(double t, const Vector3d &linearAcceleration, const Vector3d &angularVelocity) + { + mBuf.lock(); + accBuf.push(make_pair(t, linearAcceleration)); + gyrBuf.push(make_pair(t, angularVelocity)); + //printf("input imu with time %f \n", t); + mBuf.unlock(); + + if (solver_flag == NON_LINEAR) + { + mPropagate.lock(); + fastPredictIMU(t, linearAcceleration, angularVelocity); + mPropagate.unlock(); + } + } + + // Copy of original processMeasurements() but with publishers commented and threading disabled + void processMeasurements() + { + pair > > > > feature; + vector> accVector, gyrVector; + if(!featureBuf.empty()) + { + feature = featureBuf.front(); + curTime = feature.first + td; + if (USE_IMU && !IMUAvailable(feature.first + td)) + { + printf("wait for imu ... \n"); + return; + } + mBuf.lock(); + if(USE_IMU) + getIMUInterval(prevTime, curTime, accVector, gyrVector); + + featureBuf.pop(); + mBuf.unlock(); + + if(USE_IMU) + { + if(!initFirstPoseFlag) + initFirstIMUPose(accVector); + + for(size_t i = 0; i < accVector.size(); i++) + { + double dt; + if(i == 0) + dt = accVector[i].first - prevTime; + else if (i == accVector.size() - 1) + dt = curTime - accVector[i - 1].first; + else + dt = accVector[i].first - accVector[i - 1].first; + processIMU(accVector[i].first, dt, accVector[i].second, gyrVector[i].second); + } + } + mProcess.lock(); + processImage(feature.second, feature.first); + prevTime = curTime; + mProcess.unlock(); + } + } +}; +#endif + +OdometryVINSFusion::OdometryVINSFusion(const ParametersMap & parameters) : + Odometry(parameters) +#ifdef RTABMAP_VINS_FUSION + , + vinsEstimator_(0), + initGravity_(false), + previousPose_(Transform::getIdentity()) +#endif +{ +#ifdef RTABMAP_VINS_FUSION + // intialize + std::string configFilename; + Parameters::parse(parameters, Parameters::kOdomVINSFusionConfigPath(), configFilename); + if(configFilename.empty()) + { + UERROR("VINS config file is empty (%s)!", + Parameters::kOdomVINSFusionConfigPath().c_str()); + } + else + { + UINFO("Using config file %s", configFilename.c_str()); + readParameters(uReplaceChar(configFilename, '~', UDirectory::homeDir())); + } +#endif +} + +OdometryVINSFusion::~OdometryVINSFusion() +{ +#ifdef RTABMAP_VINS_FUSION + delete vinsEstimator_; +#endif +} + +void OdometryVINSFusion::reset(const Transform & initialPose) +{ + Odometry::reset(initialPose); +#ifdef RTABMAP_VINS_FUSION + if(!initGravity_) + { + delete vinsEstimator_; + vinsEstimator_ = 0; + previousPose_.setIdentity(); + lastImu_ = IMU(); + previousLocalTransform_.setNull(); + } + initGravity_ = false; +#endif +} + +// return not null transform if odometry is correctly computed +Transform OdometryVINSFusion::computeTransform( + SensorData & data, + const Transform & guess, + OdometryInfo * info) +{ + Transform t; +#ifdef RTABMAP_VINS_FUSION + UTimer timer; + + bool hasImage = !data.imageRaw().empty() && !data.rightRaw().empty() && data.stereoCameraModels().size() == 1 && data.stereoCameraModels()[0].isValidForProjection(); + + if(USE_IMU!=0 && !data.imu().empty()) + { + double dx = data.imu().linearAcceleration().val[0]; + double dy = data.imu().linearAcceleration().val[1]; + double dz = data.imu().linearAcceleration().val[2]; + double rx = data.imu().angularVelocity().val[0]; + double ry = data.imu().angularVelocity().val[1]; + double rz = data.imu().angularVelocity().val[2]; + Vector3d acc(dx, dy, dz); + Vector3d gyr(rx, ry, rz); + + UDEBUG("IMU update stamp=%f", data.stamp()); + + if(vinsEstimator_ != 0) + { + vinsEstimator_->inputIMU(data.stamp(), acc, gyr); + } + else + { + lastImu_ = data.imu(); + lastImuStamp_ = data.stamp(); + if(!hasImage) { + UWARN("Waiting an image for initialization..."); + } + } + } + + if(hasImage) + { + if(USE_IMU==1 && lastImu_.localTransform().isNull()) + { + UWARN("Waiting IMU for initialization..."); + return t; + } + if(vinsEstimator_ == 0) + { + // intialize + UINFO("Initializing with image %f", data.stamp()); + vinsEstimator_ = new VinsFusionEstimator(); + if(!vinsEstimator_->init( + lastImu_.localTransform().isNull()?Transform::getIdentity():lastImu_.localTransform(), + data.stereoCameraModels()[0], + this->imagesAlreadyRectified())) + { + delete vinsEstimator_; + vinsEstimator_ = 0; + return Transform(); + } + + if(USE_IMU) { + double dx = lastImu_.linearAcceleration().val[0]; + double dy = lastImu_.linearAcceleration().val[1]; + double dz = lastImu_.linearAcceleration().val[2]; + double rx = lastImu_.angularVelocity().val[0]; + double ry = lastImu_.angularVelocity().val[1]; + double rz = lastImu_.angularVelocity().val[2]; + Vector3d acc(dx, dy, dz); + Vector3d gyr(rx, ry, rz); + vinsEstimator_->inputIMU(lastImuStamp_, acc, gyr); + } + } + + UDEBUG("Image update stamp=%f", data.stamp()); + cv::Mat left; + cv::Mat right; + if(data.imageRaw().type() == CV_8UC3) + { + cv::cvtColor(data.imageRaw(), left, CV_BGR2GRAY); + } + 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!"); + } + + vinsEstimator_->inputImage(data.stamp(), left, right); + + if(vinsEstimator_->solver_flag == Estimator::NON_LINEAR) + { + Quaterniond tmp_Q; + tmp_Q = Quaterniond(vinsEstimator_->Rs[WINDOW_SIZE]); + Transform p( + vinsEstimator_->Ps[WINDOW_SIZE].x(), + vinsEstimator_->Ps[WINDOW_SIZE].y(), + vinsEstimator_->Ps[WINDOW_SIZE].z(), + tmp_Q.x(), + tmp_Q.y(), + tmp_Q.z(), + tmp_Q.w()); + + if(!p.isNull()) + { + if(!lastImu_.localTransform().isNull()) + { + p = p * lastImu_.localTransform().inverse(); + } + + if(this->getPose().rotation().isIdentity()) + { + initGravity_ = true; + this->reset(this->getPose()*p.rotation()); + } + + if(previousPose_.isIdentity()) + { + previousPose_ = p; + } + + // make it incremental + Transform previousPoseInv = previousPose_.inverse(); + t = previousPoseInv*p; + previousPose_ = p; + + if(info) + { + info->type = this->getType(); + info->reg.covariance = cv::Mat::eye(6,6, CV_64FC1); + info->reg.covariance *= this->framesProcessed() == 0?9999:0.0001; + + // feature map: based on code from pubPointCloud() of vins's visualization.cpp + for (auto &it_per_id : vinsEstimator_->f_manager.feature) + { + if(it_per_id.feature_per_frame.size() < 2) { + // feature just added but not tracked, or old feature not tracked anymore + continue; + } + + int imu_i = it_per_id.start_frame; + Vector3d pts_i = it_per_id.feature_per_frame[0].point * it_per_id.estimated_depth; + Vector3d w_pts_i = vinsEstimator_->Rs[imu_i] * (vinsEstimator_->ric[0] * pts_i + vinsEstimator_->tic[0]) + vinsEstimator_->Ps[imu_i]; + + cv::Point3f p; + p.x = w_pts_i(0); + p.y = w_pts_i(1); + p.z = w_pts_i(2); + + int featureIndex = info->localMap.size(); + info->localMap.insert(std::make_pair(featureIndex, p)); + + FeaturePerFrame & refFrame = it_per_id.feature_per_frame[0]; // First frame it was seen + FeaturePerFrame & newFrame = it_per_id.feature_per_frame[it_per_id.feature_per_frame.size()-1]; // Last frame it was seen (not necessary in last frame) + cv::Point2f refUV(refFrame.uv[0], refFrame.uv[1]); + cv::Point2f newUV(newFrame.uv[0], newFrame.uv[1]); + info->refCorners.push_back(refUV); + info->newCorners.push_back(newUV); + + info->reg.matchesIDs.push_back(featureIndex); + if(it_per_id.solve_flag > 0) { + // Feature correctly tracked + info->words.insert(std::make_pair(featureIndex, cv::KeyPoint(newUV, 3.0f))); + info->cornerInliers.push_back(featureIndex); + info->reg.inliersIDs.push_back(featureIndex); + } + + ++featureIndex; + } + info->features = info->localMap.size(); + info->reg.inliers = info->reg.inliersIDs.size(); + info->localMapSize = info->localMap.size(); + } + UINFO("Odom update time = %fs p=%s", timer.elapsed(), p.prettyPrint().c_str()); + } + } + else + { + UWARN("VINS-Fusion not yet initialized... needing more data."); + } + } + else if(!data.imageRaw().empty() && !data.depthRaw().empty()) + { + UERROR("VINS-Fusion doesn't work with RGB-D data, stereo images are required!"); + } + else if(!data.imageRaw().empty() && data.depthOrRightRaw().empty()) + { + UERROR("VINS-Fusion requires stereo images!"); + } + else if(data.imu().empty()) + { + UERROR("VINS-Fusion requires stereo images (and only one stereo camera with valid calibration)!"); + } +#else + UERROR("RTAB-Map is not built with VINS support! Select another visual odometry approach."); +#endif + return t; +} + +} // namespace rtabmap diff --git a/corelib/src/optimizer/OptimizerG2O.cpp b/corelib/src/optimizer/OptimizerG2O.cpp index cdf5619c..0eafcc24 100644 --- a/corelib/src/optimizer/OptimizerG2O.cpp +++ b/corelib/src/optimizer/OptimizerG2O.cpp @@ -61,7 +61,6 @@ typedef Eigen::Matrix Matr #include "g2o/types/slam3d/types_slam3d.h" #include "g2o/edge_se3_xyzprior.h" // Include after types_slam3d.h to be ignored on newest g2o versions #include "g2o/edge_se3_gravity.h" -#include "g2o/edge_sbacam_gravity.h" #include "g2o/edge_xy_prior.h" // Include after types_slam2d.h to be ignored on newest g2o versions #include "g2o/edge_xyz_prior.h" // Include after types_slam3d.h to be ignored on newest g2o versions #ifdef G2O_HAVE_CSPARSE @@ -77,6 +76,19 @@ typedef Eigen::Matrix Matr #include "g2o/types/types_sba.h" #include "g2o/types/types_six_dof_expmap.h" #include "g2o/solvers/linear_solver_eigen.h" +#include "g2o/edge_se3_expmap.h" +#endif + +#if defined(RTABMAP_G2O) || defined(RTABMAP_ORB_SLAM) +namespace rtabmap { +#ifdef RTABMAP_ORB_SLAM +typedef g2o::VertexSE3Expmap VertexCam; +#else +typedef g2o::VertexCam VertexCam; +#endif +} +#include "g2o/edge_sbacam_gravity.h" +#include "g2o/edge_sbacam_prior.h" #endif typedef g2o::BlockSolver< g2o::BlockSolverTraits<-1, -1> > SlamBlockSolver; @@ -90,14 +102,10 @@ typedef g2o::LinearSolverCSparse SlamLinearCSpa typedef g2o::LinearSolverCholmod SlamLinearCholmodSolver; #endif -// We use G2O_SRC_DIR to know we are version after December 24 2020 +// We check if g2o/types/sba/sba_utils.h exists to know we use a version after December 24 2020 // where VertexSBAPointXYZ has been renamed to VertexPointXYZ // (g2o: 0fcccb302787e70ff19f65e70fb103a1295b33a2) -// -// VCPKG commented G2O_SRC_DIR from their port so we cannot use -// G2O_SRC_DIR on windows to deduce it, we then assume it is the -// latest version without VertexSBAPointXYZ -#if defined(G2O_SRC_DIR) or defined(WIN32) +#ifdef RTABMAP_G2O_WITH_SBA_UTILS namespace g2o { typedef VertexPointXYZ VertexSBAPointXYZ; } @@ -220,7 +228,7 @@ std::map OptimizerG2O::optimize( outputCovariance = cv::Mat::eye(6,6,CV_64FC1); std::map optimizedPoses; #ifdef RTABMAP_G2O - UDEBUG("Optimizing graph..."); + UDEBUG("Optimizing graph... (rootId=%d)", rootId); #ifndef RTABMAP_VERTIGO if(this->isRobust()) @@ -352,6 +360,9 @@ std::map OptimizerG2O::optimize( { if(!priorsIgnored() && iter->second.type() == Link::kPosePrior) { + if(rootId!=0) { + UDEBUG("Removed rootId=%d because there are priors."); + } rootId = 0; break; } @@ -594,7 +605,9 @@ std::map OptimizerG2O::optimize( g2o::EdgeSE2Prior * priorEdge = new g2o::EdgeSE2Prior(); g2o::VertexSE2* v1 = (g2o::VertexSE2*)optimizer.vertex(id1); priorEdge->setVertex(0, v1); - priorEdge->setMeasurement(g2o::SE2(iter->second.transform().x(), iter->second.transform().y(), iter->second.transform().theta())); + auto pose = g2o::SE2(iter->second.transform().x(), iter->second.transform().y(), iter->second.transform().theta()); + v1->setEstimate(pose); // This will help g2o to converge faster (https://github.com/introlab/rtabmap_ros/issues/1371) + priorEdge->setMeasurement(pose); priorEdge->setParameterId(0, PARAM_OFFSET); Eigen::Matrix information = Eigen::Matrix::Identity(); if(!isCovarianceIgnored()) @@ -675,6 +688,7 @@ std::map OptimizerG2O::optimize( Eigen::Isometry3d pose; pose = a.linear(); pose.translation() = a.translation(); + v1->setEstimate(pose); // This will help g2o to converge faster (https://github.com/introlab/rtabmap_ros/issues/1371) priorEdge->setMeasurement(pose); priorEdge->setParameterId(0, PARAM_OFFSET); Eigen::Matrix information = Eigen::Matrix::Identity(); @@ -1005,8 +1019,8 @@ std::map OptimizerG2O::optimize( g2o::EdgeSE3 * e = new g2o::EdgeSE3(); g2o::VertexSE3* v1 = (g2o::VertexSE3*)optimizer.vertex(id1); g2o::VertexSE3* v2 = (g2o::VertexSE3*)optimizer.vertex(id2); - UASSERT(v1 != 0); - UASSERT(v2 != 0); + UASSERT_MSG(v1 != 0, uFormat("v1=%d v2=%d", id1, id2).c_str()); + UASSERT_MSG(v2 != 0, uFormat("v1=%d v2=%d", id1, id2).c_str()); e->setVertex(0, v1); e->setVertex(1, v2); e->setMeasurement(constraint); @@ -1173,7 +1187,7 @@ std::map OptimizerG2O::optimize( if(i>0 && optimizer.activeRobustChi2() > 1000000000000.0) { - UERROR("g2o: Large optimimzation error detected (%f), aborting optimization!"); + UERROR("g2o: Large optimization error detected (%f), aborting optimization!"); return optimizedPoses; } @@ -1222,7 +1236,7 @@ std::map OptimizerG2O::optimize( if(optimizer.activeRobustChi2() > 1000000000000.0) { - UERROR("g2o: Large optimimzation error detected (%f), aborting optimization!"); + UERROR("g2o: Large optimization error detected (%f), aborting optimization!"); return optimizedPoses; } @@ -1412,81 +1426,6 @@ std::map OptimizerG2O::optimize( return optimizedPoses; } -#ifdef RTABMAP_ORB_SLAM -/** - * \brief 3D edge between two SBAcam - */ - class EdgeSE3Expmap : public g2o::BaseBinaryEdge<6, g2o::SE3Quat, g2o::VertexSE3Expmap, g2o::VertexSE3Expmap> -{ - public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW; - EdgeSE3Expmap(): BaseBinaryEdge<6, g2o::SE3Quat, g2o::VertexSE3Expmap, g2o::VertexSE3Expmap>(){} - bool read(std::istream& is) - { - return false; - } - - bool write(std::ostream& os) const - { - return false; - } - - void computeError() - { - const g2o::VertexSE3Expmap* v1 = dynamic_cast(_vertices[0]); - const g2o::VertexSE3Expmap* v2 = dynamic_cast(_vertices[1]); - g2o::SE3Quat delta = _inverseMeasurement * (v1->estimate().inverse()*v2->estimate()); - _error[0]=delta.translation().x(); - _error[1]=delta.translation().y(); - _error[2]=delta.translation().z(); - _error[3]=delta.rotation().x(); - _error[4]=delta.rotation().y(); - _error[5]=delta.rotation().z(); - } - - virtual void setMeasurement(const g2o::SE3Quat& meas){ - _measurement=meas; - _inverseMeasurement=meas.inverse(); - } - - virtual double initialEstimatePossible(const g2o::OptimizableGraph::VertexSet& , g2o::OptimizableGraph::Vertex* ) { return 1.;} - virtual void initialEstimate(const g2o::OptimizableGraph::VertexSet& from_, g2o::OptimizableGraph::Vertex* ){ - g2o::VertexSE3Expmap* from = static_cast(_vertices[0]); - g2o::VertexSE3Expmap* to = static_cast(_vertices[1]); - if (from_.count(from) > 0) - to->setEstimate((g2o::SE3Quat) from->estimate() * _measurement); - else - from->setEstimate((g2o::SE3Quat) to->estimate() * _inverseMeasurement); - } - - virtual bool setMeasurementData(const double* d){ - Eigen::Map v(d); - _measurement.fromVector(v); - _inverseMeasurement = _measurement.inverse(); - return true; - } - - virtual bool getMeasurementData(double* d) const{ - Eigen::Map v(d); - v = _measurement.toVector(); - return true; - } - - virtual int measurementDimension() const {return 7;} - - virtual bool setMeasurementFromState() { - const g2o::VertexSE3Expmap* v1 = dynamic_cast(_vertices[0]); - const g2o::VertexSE3Expmap* v2 = dynamic_cast(_vertices[1]); - _measurement = (v1->estimate().inverse()*v2->estimate()); - _inverseMeasurement = _measurement.inverse(); - return true; - } - - protected: - g2o::SE3Quat _inverseMeasurement; -}; -#endif - std::map OptimizerG2O::optimizeBA( int rootId, const std::map & poses, @@ -1557,7 +1496,13 @@ std::map OptimizerG2O::optimizeBA( #endif // RTABMAP_ORB_SLAM #ifndef RTABMAP_ORB_SLAM - if(optimizer_ == 1) + // ISSUE: It seems the fatal error + // "[SetJac] infinite jac" happens relatively + // easily with GaussNewton on SBA problem, + // ignore optimizer_ and always use Levenberg for SBA. + // TODO: Note that g2o/RobustKernelDelta parameter could be + // potentially tuned to avoid that error with GaussNewton. + if(0)//optimizer_ == 1) { #ifdef RTABMAP_G2O_CPP11 optimizer.setAlgorithm(new g2o::OptimizationAlgorithmGaussNewton( @@ -1577,8 +1522,23 @@ std::map OptimizerG2O::optimizeBA( #endif } + // detect if there are gravity constraints + bool hasGravityConstraints = false; + if(!isSlam2d() && gravitySigma() > 0) + { + for(std::multimap::const_iterator iter=links.begin(); iter!=links.end(); ++iter) + { + if( iter->second.from() == iter->second.to() && + iter->second.type() == Link::kGravity) + { + hasGravityConstraints = true; + break; + } + } + } - UDEBUG("fill poses to g2o..."); + + UDEBUG("fill %ld poses to g2o... (rootId=%d hasGravityConstraints=%d isSlam2d=%d)", poses.size(), rootId, hasGravityConstraints?1:0, isSlam2d()?1:0); for(std::map::const_iterator iter=poses.begin(); iter!=poses.end(); ++iter) { if(iter->first > 0) @@ -1594,11 +1554,8 @@ std::map OptimizerG2O::optimizeBA( // Add node's pose UASSERT(!camPose.isNull()); -#ifdef RTABMAP_ORB_SLAM - g2o::VertexSE3Expmap * vCam = new g2o::VertexSE3Expmap(); -#else - g2o::VertexCam * vCam = new g2o::VertexCam(); -#endif + + rtabmap::VertexCam * vCam = new rtabmap::VertexCam(); Eigen::Affine3d a = camPose.toEigen3d(); #ifdef RTABMAP_ORB_SLAM @@ -1617,7 +1574,65 @@ std::map OptimizerG2O::optimizeBA( vCam->setId(iter->first*MULTICAM_OFFSET + i); // negative root means that all other poses should be fixed instead of the root - vCam->setFixed((rootId >= 0 && iter->first == rootId) || (rootId < 0 && iter->first != -rootId)); + bool fixNode = (rootId >= 0 && iter->first == rootId) || (rootId < 0 && iter->first != -rootId); + + UASSERT_MSG(optimizer.addVertex(vCam), uFormat("cannot insert cam vertex %d (pose=%d)!?", vCam->id(), iter->first).c_str()); + + if(this->isSlam2d()) + { + if(fixNode) + { + UDEBUG("Set node %d fixed", iter->first); + vCam->setFixed(true); + } + else if(i==0) // Only set prior on the first camera + { + // add a singleton constraint that locks the position of the robot on the plane + EdgeSBACamPrior* planeConstraint = new EdgeSBACamPrior(); + Eigen::Matrix pinfo = Eigen::Matrix::Zero(); + pinfo(2, 2) = 1e9; + planeConstraint->setInformation(pinfo); + g2o::SE3Quat fixedZ = g2o::SE3Quat(); + fixedZ.setTranslation(Eigen::Vector3d(0,0,iter->second.z())); + planeConstraint->setMeasurement(fixedZ); + Eigen::Affine3d a = iterModel->second[i].localTransform().inverse().toEigen3d(); + planeConstraint->setCameraInvLocalTransform(g2o::SE3Quat(a.linear(), a.translation())); + planeConstraint->vertices()[0] = vCam; + optimizer.addEdge(planeConstraint); + } + } + else if(fixNode) + { + if(rootId < 0 || !hasGravityConstraints) + { + UDEBUG("Set node %d fixed", iter->first); + vCam->setFixed(true); + } + else if(hasGravityConstraints && i==0) // Only set prior on the first camera in case of multi-cam + { + // Setup root prior (fixed x,y,z,yaw) + EdgeSBACamPrior * e = new EdgeSBACamPrior(); + e->vertices()[0] = vCam; + Eigen::Affine3d a = iter->second.toEigen3d(); + e->setMeasurement(g2o::SE3Quat(a.linear(), a.translation())); + a = iterModel->second[i].localTransform().inverse().toEigen3d(); + e->setCameraInvLocalTransform(g2o::SE3Quat(a.linear(), a.translation())); + Eigen::Matrix information = Eigen::Matrix::Identity()*10e6; + // pitch and roll not fixed + information(3,3) = information(4,4) = 1; + e->setInformation(information); + if (!optimizer.addEdge(e)) + { + delete e; + UERROR("Map: Failed adding fixed constraint of node %d, set as fixed instead", iter->first); + vCam->setFixed(true); + } + else + { + UDEBUG("Set node %d fixed with prior (have gravity constraints)", iter->first); + } + } + } /*UDEBUG("camPose %d (camid=%d) (fixed=%d) fx=%f fy=%f cx=%f cy=%f Tx=%f baseline=%f t=%s", iter->first, @@ -1630,8 +1645,6 @@ std::map OptimizerG2O::optimizeBA( iterModel->second[i].Tx(), iterModel->second[i].Tx()<0.0?-iterModel->second[i].Tx()/iterModel->second[i].fx():baseline_, camPose.prettyPrint().c_str());*/ - - UASSERT_MSG(optimizer.addVertex(vCam), uFormat("cannot insert cam vertex %d (pose=%d)!?", vCam->id(), iter->first).c_str()); } } } @@ -1650,7 +1663,6 @@ std::map OptimizerG2O::optimizeBA( if(id1 == id2) { -#ifndef RTABMAP_ORB_SLAM g2o::HyperGraph::Edge * edge = 0; if(gravitySigma() > 0 && iter->second.type() == Link::kGravity && poses.find(iter->first) != poses.end()) { @@ -1664,7 +1676,7 @@ std::map OptimizerG2O::optimizeBA( Eigen::MatrixXd information = Eigen::MatrixXd::Identity(3, 3) * 1.0/(gravitySigma()*gravitySigma()); - g2o::VertexCam* v1 = (g2o::VertexCam*)optimizer.vertex(id1*MULTICAM_OFFSET); + rtabmap::VertexCam* v1 = (rtabmap::VertexCam*)optimizer.vertex(id1*MULTICAM_OFFSET); EdgeSBACamGravity* priorEdge(new EdgeSBACamGravity()); std::map >::const_iterator iterModel = models.find(iter->first); // Gravity constraint added only to first camera of a pose @@ -1681,7 +1693,6 @@ std::map OptimizerG2O::optimizeBA( UERROR("Map: Failed adding constraint between %d and %d, skipping", id1, id2); return optimizedPoses; } -#endif } else if(id1>0 && id2>0) // not supporting landmarks { @@ -1837,14 +1848,14 @@ std::map OptimizerG2O::optimizeBA( g2o::OptimizableGraph::Edge * e; double baseline = 0.0; + rtabmap::VertexCam* vcam = dynamic_cast(optimizer.vertex(camId)); #ifdef RTABMAP_ORB_SLAM - g2o::VertexSE3Expmap* vcam = dynamic_cast(optimizer.vertex(camId)); + std::map >::const_iterator iterModel = models.find(poseId); - UASSERT(iterModel != models.end() && camIndexsecond.size() && iterModel->second[camIndex].isValidForProjection()); + UASSERT(iterModel != models.end() && camIndex<(int)iterModel->second.size() && iterModel->second[camIndex].isValidForProjection()); baseline = iterModel->second[camIndex].Tx()<0.0?-iterModel->second[camIndex].Tx()/iterModel->second[camIndex].fx():baseline_; #else - g2o::VertexCam* vcam = dynamic_cast(optimizer.vertex(camId)); baseline = vcam->estimate().baseline; #endif double variance = pixelVariance_; @@ -1942,7 +1953,8 @@ std::map OptimizerG2O::optimizeBA( if(uIsNan(chi2)) { - UERROR("Optimization generated NANs, aborting optimization! Try another g2o's optimizer (current=%d).", optimizer_); + UERROR("Optimization generated NANs, aborting optimization! Try another g2o's optimizer (current %s=%d) or solver (current %s=%d).", + Parameters::kg2oOptimizer().c_str(), optimizer_, Parameters::kg2oSolver().c_str(), solver_); return optimizedPoses; } UDEBUG("iteration %d: %d nodes, %d edges, chi2: %f", i, (int)optimizer.vertices().size(), (int)optimizer.edges().size(), chi2); @@ -1976,15 +1988,18 @@ std::map OptimizerG2O::optimizeBA( //UDEBUG("Ignoring edge (%d<->%d) d=%f var=%f kernel=%f chi2=%f", (*iter)->vertex(0)->id()-stepVertexId, (*iter)->vertex(1)->id(), d, 1.0/((g2o::EdgeProjectP2SC*)(*iter))->information()(0,0), (*iter)->robustKernel()->delta(), (*iter)->chi2()); #endif - cv::Point3f pt3d; + int id=-1; if((*iter)->vertex(0)->id() > negVertexOffset) { - pt3d = points3DMap.at(negVertexOffset - (*iter)->vertex(0)->id()); + id = negVertexOffset - (*iter)->vertex(0)->id(); } else { - pt3d = points3DMap.at((*iter)->vertex(0)->id()-stepVertexId); + id = (*iter)->vertex(0)->id() - stepVertexId; } + UASSERT_MSG(points3DMap.find(id) != points3DMap.end(), uFormat("word id=%d points3DMap=%ld vertex id=%d (negVertexOffset=%d stepVertexId=%d)", + id, points3DMap.size(), (*iter)->vertex(0)->id(), negVertexOffset, stepVertexId).c_str()); + cv::Point3f pt3d = points3DMap.at(id); ((g2o::VertexSBAPointXYZ*)(*iter)->vertex(0))->setEstimate(Eigen::Vector3d(pt3d.x, pt3d.y, pt3d.z)); if(outliers) @@ -2041,12 +2056,26 @@ std::map OptimizerG2O::optimizeBA( return optimizedPoses; } - // FIXME: is there a way that we can add the 2D constraint directly in SBA? if(this->isSlam2d()) { - // get transform between old and new pose - t = iter->second.inverse() * t; - optimizedPoses.insert(std::pair(iter->first, iter->second * t.to3DoF())); + // The optimized poses should be already fixed to original height, + // but it may have varied a little (not exaclty the same number). + // Here we just put back the original z value. + if(fabs(t.z() - iter->second.z()) < 0.001) + { + t.z() = iter->second.z(); + optimizedPoses.insert(std::pair(iter->first, t)); + } + else + { + UWARN("Planar constraints didn't work!? original pose (%d), pose %s -> %s. Falling back to old approach.", + iter->first, + iter->second.prettyPrint().c_str(), + t.prettyPrint().c_str()); + // get transform between old and new pose + t = iter->second.inverse() * t; + optimizedPoses.insert(std::pair(iter->first, iter->second * t.to3DoF())); + } } else { @@ -2106,6 +2135,390 @@ std::map OptimizerG2O::optimizeBA( return optimizedPoses; } +bool OptimizerG2O::loadGraph( + const std::string & fileName, + std::map & poses, + std::multimap & edgeConstraints) +{ + FILE * file = 0; +#ifdef _MSC_VER + fopen_s(&file, fileName.c_str(), "r"); +#else + file = fopen(fileName.c_str(), "r"); +#endif + + if(!file) + { + UERROR("Cannot open file %s", fileName.c_str()); + return false; + } + + // saveGraph() writes landmarks (originally negative ids, remapped to + // landmarkOffset - id) first in DESCENDING file-id order, then regular + // poses in ASCENDING file-id order. We recover landmarkOffset from this + // order to restore the original negative landmark ids. + struct VertexEntry { + int fileId; + Transform transform; + bool definitelyLandmark; // VERTEX_XY / VERTEX_TRACKXYZ + }; + struct EdgeEntry { + int from; + int to; + Link::Type type; + Transform transform; + cv::Mat info; + bool isPrior; // from==to, prior on a single vertex + bool hasLandmarkEndpoint; // tag implies a landmark on one side + }; + std::vector verticesList; + std::vector edgesList; + + char line[2048]; + while(fgets(line, 2048, file) != NULL) + { + std::list tokenList = uSplit(uReplaceChar(uReplaceChar(line, '\n', ' '), '\r', ' '), ' '); + std::vector v; + v.reserve(tokenList.size()); + for(std::list::const_iterator iter = tokenList.begin(); iter != tokenList.end(); ++iter) + { + if(!iter->empty()) + { + v.push_back(*iter); + } + } + if(v.empty()) + { + continue; + } + const std::string & tag = v[0]; + + // Skip parameters, switch helpers and unrelated entries + if(tag == "PARAMS_SE2OFFSET" || tag == "PARAMS_SE3OFFSET" || + tag == "VERTEX_SWITCH" || tag == "EDGE_SWITCH_PRIOR") + { + continue; + } + + if(tag == "VERTEX_SE2" && v.size() == 5) + { + VertexEntry e; + e.fileId = atoi(v[1].c_str()); + e.transform = Transform(uStr2Float(v[2]), uStr2Float(v[3]), uStr2Float(v[4])); + e.definitelyLandmark = false; + verticesList.push_back(e); + } + else if(tag == "VERTEX_XY" && v.size() == 4) + { + VertexEntry e; + e.fileId = atoi(v[1].c_str()); + e.transform = Transform(uStr2Float(v[2]), uStr2Float(v[3]), 0); + e.definitelyLandmark = true; + verticesList.push_back(e); + } + else if(tag == "VERTEX_SE3:QUAT" && v.size() == 9) + { + VertexEntry e; + e.fileId = atoi(v[1].c_str()); + e.transform = Transform(uStr2Float(v[2]), uStr2Float(v[3]), uStr2Float(v[4]), + uStr2Float(v[5]), uStr2Float(v[6]), uStr2Float(v[7]), uStr2Float(v[8])); + e.definitelyLandmark = false; + verticesList.push_back(e); + } + else if(tag == "VERTEX_TRACKXYZ" && v.size() == 5) + { + VertexEntry e; + e.fileId = atoi(v[1].c_str()); + e.transform = Transform(uStr2Float(v[2]), uStr2Float(v[3]), uStr2Float(v[4]), 0, 0, 0); + e.definitelyLandmark = true; + verticesList.push_back(e); + } + else if(tag == "EDGE_SE2" && v.size() == 12) + { + EdgeEntry e; + e.from = atoi(v[1].c_str()); + e.to = atoi(v[2].c_str()); + e.transform = Transform(uStr2Float(v[3]), uStr2Float(v[4]), uStr2Float(v[5])); + e.info = cv::Mat::eye(6, 6, CV_64FC1); + e.info.at(0, 0) = uStr2Double(v[6]); + e.info.at(0, 1) = e.info.at(1, 0) = uStr2Double(v[7]); + e.info.at(0, 5) = e.info.at(5, 0) = uStr2Double(v[8]); + e.info.at(1, 1) = uStr2Double(v[9]); + e.info.at(1, 5) = e.info.at(5, 1) = uStr2Double(v[10]); + e.info.at(5, 5) = uStr2Double(v[11]); + e.type = Link::kUndef; // disambiguated after we know landmarkOffset + e.isPrior = false; + e.hasLandmarkEndpoint = false; + edgesList.push_back(e); + } + else if(tag == "EDGE_SE2_XY" && v.size() == 8) + { + EdgeEntry e; + e.from = atoi(v[1].c_str()); + e.to = atoi(v[2].c_str()); + e.transform = Transform(uStr2Float(v[3]), uStr2Float(v[4]), 0); + e.info = cv::Mat::eye(6, 6, CV_64FC1); + e.info.at(0, 0) = uStr2Double(v[5]); + e.info.at(0, 1) = e.info.at(1, 0) = uStr2Double(v[6]); + e.info.at(1, 1) = uStr2Double(v[7]); + e.type = Link::kLandmark; + e.isPrior = false; + e.hasLandmarkEndpoint = true; + edgesList.push_back(e); + } + else if((tag == "EDGE_SE3:QUAT" || tag == "EDGE_SE3") && v.size() == 31) + { + EdgeEntry e; + e.from = atoi(v[1].c_str()); + e.to = atoi(v[2].c_str()); + e.transform = Transform(uStr2Float(v[3]), uStr2Float(v[4]), uStr2Float(v[5]), + uStr2Float(v[6]), uStr2Float(v[7]), uStr2Float(v[8]), uStr2Float(v[9])); + e.info = cv::Mat::eye(6, 6, CV_64FC1); + int idx = 10; + for(int r = 0; r < 6; ++r) + { + for(int c = r; c < 6; ++c) + { + e.info.at(r, c) = uStr2Double(v[idx++]); + if(r != c) e.info.at(c, r) = e.info.at(r, c); + } + } + // EDGE_SE3 (no :QUAT) is the landmark variant emitted by saveGraph + bool landmarkTag = (tag == "EDGE_SE3"); + e.type = landmarkTag ? Link::kLandmark : Link::kUndef; + e.isPrior = false; + e.hasLandmarkEndpoint = landmarkTag; + edgesList.push_back(e); + } + else if(tag == "EDGE_SE3_TRACKXYZ" && v.size() == 13) + { + EdgeEntry e; + e.from = atoi(v[1].c_str()); + e.to = atoi(v[2].c_str()); + // v[3] = param_offset id, ignored + e.transform = Transform(uStr2Float(v[4]), uStr2Float(v[5]), uStr2Float(v[6]), 0, 0, 0); + e.info = cv::Mat::eye(6, 6, CV_64FC1); + e.info.at(0, 0) = uStr2Double(v[7]); + e.info.at(0, 1) = e.info.at(1, 0) = uStr2Double(v[8]); + e.info.at(0, 2) = e.info.at(2, 0) = uStr2Double(v[9]); + e.info.at(1, 1) = uStr2Double(v[10]); + e.info.at(1, 2) = e.info.at(2, 1) = uStr2Double(v[11]); + e.info.at(2, 2) = uStr2Double(v[12]); + e.type = Link::kLandmark; + e.isPrior = false; + e.hasLandmarkEndpoint = true; + edgesList.push_back(e); + } + else if(tag == "EDGE_PRIOR_SE2" && v.size() == 11) + { + EdgeEntry e; + e.from = atoi(v[1].c_str()); + e.to = e.from; + e.transform = Transform(uStr2Float(v[2]), uStr2Float(v[3]), uStr2Float(v[4])); + e.info = cv::Mat::eye(6, 6, CV_64FC1); + e.info.at(0, 0) = uStr2Double(v[5]); + e.info.at(0, 1) = e.info.at(1, 0) = uStr2Double(v[6]); + e.info.at(0, 5) = e.info.at(5, 0) = uStr2Double(v[7]); + e.info.at(1, 1) = uStr2Double(v[8]); + e.info.at(1, 5) = e.info.at(5, 1) = uStr2Double(v[9]); + e.info.at(5, 5) = uStr2Double(v[10]); + e.type = Link::kPosePrior; + e.isPrior = true; + e.hasLandmarkEndpoint = false; + edgesList.push_back(e); + } + else if(tag == "EDGE_PRIOR_SE2_XY" && v.size() == 7) + { + EdgeEntry e; + e.from = atoi(v[1].c_str()); + e.to = e.from; + e.transform = Transform(uStr2Float(v[2]), uStr2Float(v[3]), 0); + e.info = cv::Mat::eye(6, 6, CV_64FC1); + e.info.at(0, 0) = uStr2Double(v[4]); + e.info.at(0, 1) = e.info.at(1, 0) = uStr2Double(v[5]); + e.info.at(1, 1) = uStr2Double(v[6]); + // no orientation info on this prior + e.info.at(3, 3) = e.info.at(4, 4) = e.info.at(5, 5) = 1.0 / 9999.0; + e.type = Link::kPosePrior; + e.isPrior = true; + e.hasLandmarkEndpoint = false; + edgesList.push_back(e); + } + else if(tag == "EDGE_SE3_PRIOR" && v.size() == 31) + { + EdgeEntry e; + e.from = atoi(v[1].c_str()); + e.to = e.from; + // v[2] = param_offset id, ignored + e.transform = Transform(uStr2Float(v[3]), uStr2Float(v[4]), uStr2Float(v[5]), + uStr2Float(v[6]), uStr2Float(v[7]), uStr2Float(v[8]), uStr2Float(v[9])); + e.info = cv::Mat::eye(6, 6, CV_64FC1); + int idx = 10; + for(int r = 0; r < 6; ++r) + { + for(int c = r; c < 6; ++c) + { + e.info.at(r, c) = uStr2Double(v[idx++]); + if(r != c) e.info.at(c, r) = e.info.at(r, c); + } + } + e.type = Link::kPosePrior; + e.isPrior = true; + e.hasLandmarkEndpoint = false; + edgesList.push_back(e); + } + else if(tag == "EDGE_POINTXYZ_PRIOR" && v.size() == 11) + { + EdgeEntry e; + e.from = atoi(v[1].c_str()); + e.to = e.from; + e.transform = Transform(uStr2Float(v[2]), uStr2Float(v[3]), uStr2Float(v[4]), 0, 0, 0); + e.info = cv::Mat::eye(6, 6, CV_64FC1); + e.info.at(0, 0) = uStr2Double(v[5]); + e.info.at(0, 1) = e.info.at(1, 0) = uStr2Double(v[6]); + e.info.at(0, 2) = e.info.at(2, 0) = uStr2Double(v[7]); + e.info.at(1, 1) = uStr2Double(v[8]); + e.info.at(1, 2) = e.info.at(2, 1) = uStr2Double(v[9]); + e.info.at(2, 2) = uStr2Double(v[10]); + // no orientation info on this prior + e.info.at(3, 3) = e.info.at(4, 4) = e.info.at(5, 5) = 1.0 / 9999.0; + e.type = Link::kPosePrior; + e.isPrior = true; + e.hasLandmarkEndpoint = false; + edgesList.push_back(e); + } + else if(tag == "EDGE_SE2_SWITCHABLE" && v.size() == 13) + { + EdgeEntry e; + e.from = atoi(v[1].c_str()); + e.to = atoi(v[2].c_str()); + // v[3] = switch vertex id, ignored + e.transform = Transform(uStr2Float(v[4]), uStr2Float(v[5]), uStr2Float(v[6])); + e.info = cv::Mat::eye(6, 6, CV_64FC1); + e.info.at(0, 0) = uStr2Double(v[7]); + e.info.at(0, 1) = e.info.at(1, 0) = uStr2Double(v[8]); + e.info.at(0, 5) = e.info.at(5, 0) = uStr2Double(v[9]); + e.info.at(1, 1) = uStr2Double(v[10]); + e.info.at(1, 5) = e.info.at(5, 1) = uStr2Double(v[11]); + e.info.at(5, 5) = uStr2Double(v[12]); + e.type = Link::kUndef; + e.isPrior = false; + e.hasLandmarkEndpoint = false; + edgesList.push_back(e); + } + else if(tag == "EDGE_SE3_SWITCHABLE" && v.size() == 32) + { + EdgeEntry e; + e.from = atoi(v[1].c_str()); + e.to = atoi(v[2].c_str()); + // v[3] = switch vertex id, ignored + e.transform = Transform(uStr2Float(v[4]), uStr2Float(v[5]), uStr2Float(v[6]), + uStr2Float(v[7]), uStr2Float(v[8]), uStr2Float(v[9]), uStr2Float(v[10])); + e.info = cv::Mat::eye(6, 6, CV_64FC1); + int idx = 11; + for(int r = 0; r < 6; ++r) + { + for(int c = r; c < 6; ++c) + { + e.info.at(r, c) = uStr2Double(v[idx++]); + if(r != c) e.info.at(c, r) = e.info.at(r, c); + } + } + e.type = Link::kUndef; + e.isPrior = false; + e.hasLandmarkEndpoint = false; + edgesList.push_back(e); + } + else + { + UWARN("Unsupported or malformed g2o line: \"%s\" (tag=%s, tokens=%d)", line, tag.c_str(), (int)v.size()); + } + } + fclose(file); + + // Recover landmarkOffset from vertex order: + // file order = [landmarks with DESCENDING file_ids] + [regular poses with ASCENDING file_ids] + // Walk backwards from the end and take the longest ascending suffix as the regular poses. + // landmarkOffset = max regular pose id (= last fileId of that suffix). + int landmarkOffset = 0; + int firstRegularIdx = (int)verticesList.size(); + if(!verticesList.empty()) + { + firstRegularIdx = (int)verticesList.size() - 1; + while(firstRegularIdx > 0 && + verticesList[firstRegularIdx - 1].fileId < verticesList[firstRegularIdx].fileId) + { + --firstRegularIdx; + } + landmarkOffset = verticesList.back().fileId; + + // If the alleged regular suffix actually starts on a definite landmark + // (VERTEX_XY / VERTEX_TRACKXYZ), then there are no regular poses and + // saveGraph used landmarkOffset = 0; restore that case. + if(verticesList[firstRegularIdx].definitelyLandmark) + { + landmarkOffset = 0; + firstRegularIdx = (int)verticesList.size(); + } + } + + // Insert vertices into poses, remapping landmark file ids back to negative. + for(int i = 0; i < (int)verticesList.size(); ++i) + { + int originalId; + if(i < firstRegularIdx) + { + originalId = landmarkOffset - verticesList[i].fileId; // negative + } + else + { + originalId = verticesList[i].fileId; + } + if(poses.find(originalId) == poses.end()) + { + poses.insert(std::make_pair(originalId, verticesList[i].transform)); + } + else + { + UWARN("Vertex %d (file id %d) already exists, ignoring duplicate", originalId, verticesList[i].fileId); + } + } + + // Remap edge endpoints. Any file id > landmarkOffset (or, if landmarkOffset == 0 + // and there are any landmarks at all, any id present in the landmark prefix) + // is a landmark and gets the negative id back. + bool allLandmarks = (landmarkOffset == 0 && firstRegularIdx == (int)verticesList.size() && !verticesList.empty()); + auto remap = [&](int fileId) -> int { + if(landmarkOffset > 0 && fileId > landmarkOffset) + { + return landmarkOffset - fileId; // negative + } + if(allLandmarks) + { + return -fileId; + } + return fileId; + }; + + for(const EdgeEntry & e : edgesList) + { + int from = remap(e.from); + int to = e.isPrior ? from : remap(e.to); + Link::Type type = e.type; + // Promote ambiguous edges (EDGE_SE2) to kLandmark when an endpoint + // turns out to be a landmark after remapping. + if(type == Link::kUndef && (from < 0 || to < 0)) + { + type = Link::kLandmark; + } + edgeConstraints.insert(std::make_pair(from, Link(from, to, type, e.transform, e.info))); + } + + UINFO("Graph loaded from %s (%d poses, %d edges, landmarkOffset=%d)", + fileName.c_str(), (int)poses.size(), (int)edgeConstraints.size(), landmarkOffset); + return true; +} + bool OptimizerG2O::saveGraph( const std::string & fileName, const std::map & poses, diff --git a/corelib/src/optimizer/OptimizerGTSAM.cpp b/corelib/src/optimizer/OptimizerGTSAM.cpp index 53c87edb..6361f537 100644 --- a/corelib/src/optimizer/OptimizerGTSAM.cpp +++ b/corelib/src/optimizer/OptimizerGTSAM.cpp @@ -51,7 +51,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include -#include "gtsam/GravityFactor.h" +#include #include #include #include @@ -121,7 +121,7 @@ void OptimizerGTSAM::parseParameters(const ParametersMap & parameters) params.relinearizeThreshold = threshold; params.relinearizeSkip = skip; params.evaluateNonlinearError = true; - isam2_ = new ISAM2(params); + isam2_ = new gtsam::ISAM2(params); addedPoses_.clear(); lastAddedConstraints_.clear(); @@ -239,6 +239,7 @@ std::map OptimizerGTSAM::optimize( isam2_ = new gtsam::ISAM2(params); addedPoses_.clear(); lastAddedConstraints_.clear(); + isLandmarkWithRotation_.clear(); lastRootFactorIndex_.first = 0; lastSwitchId_ = 1000000000; } @@ -308,7 +309,13 @@ std::map OptimizerGTSAM::optimize( UDEBUG("fill poses to gtsam... rootId=%d (priorsIgnored=%d landmarksIgnored=%d)", rootId, priorsIgnored()?1:0, landmarksIgnored()?1:0); gtsam::Values initialEstimate; - std::map isLandmarkWithRotation; + // In batch (non-iSAM2) mode each optimize() call is independent. + // In iSAM2 mode the map persists so we can resolve landmarks added + // in a previous incremental call but referenced by a new edge. + if(!isam2_) + { + isLandmarkWithRotation_.clear(); + } for(std::map::const_iterator iter = newPoses.begin(); iter!=newPoses.end(); ++iter) { UASSERT(!iter->second.isNull()); @@ -328,12 +335,12 @@ std::map OptimizerGTSAM::optimize( if (1 / static_cast(jter->second.infMatrix().at(5,5)) >= 9999.0) { initialEstimate.insert(iter->first, gtsam::Point2(iter->second.x(), iter->second.y())); - isLandmarkWithRotation.insert(std::make_pair(iter->first, false)); + isLandmarkWithRotation_.insert(std::make_pair(iter->first, false)); } else { initialEstimate.insert(iter->first, gtsam::Pose2(iter->second.x(), iter->second.y(), iter->second.theta())); - isLandmarkWithRotation.insert(std::make_pair(iter->first, true)); + isLandmarkWithRotation_.insert(std::make_pair(iter->first, true)); } addedPoses_.insert(iter->first); } @@ -357,12 +364,12 @@ std::map OptimizerGTSAM::optimize( 1 / static_cast(jter->second.infMatrix().at(5,5)) >= 9999.0) { initialEstimate.insert(iter->first, gtsam::Point3(iter->second.x(), iter->second.y(), iter->second.z())); - isLandmarkWithRotation.insert(std::make_pair(iter->first, false)); + isLandmarkWithRotation_.insert(std::make_pair(iter->first, false)); } else { initialEstimate.insert(iter->first, gtsam::Pose3(iter->second.toEigen4d())); - isLandmarkWithRotation.insert(std::make_pair(iter->first, true)); + isLandmarkWithRotation_.insert(std::make_pair(iter->first, true)); } addedPoses_.insert(iter->first); } @@ -379,8 +386,8 @@ std::map OptimizerGTSAM::optimize( int id1 = iter->second.from(); int id2 = iter->second.to(); - UASSERT_MSG(poses.find(id1)!=poses.end(), uFormat("id1=%d", id1).c_str()); - UASSERT_MSG(poses.find(id2)!=poses.end(), uFormat("id2=%d", id2).c_str()); + UASSERT_MSG(poses.find(id1)!=poses.end(), uFormat("id1=%d for constraint %d->%d (type=%d)", id1, id1, id2, iter->second.type()).c_str()); + UASSERT_MSG(poses.find(id2)!=poses.end(), uFormat("id2=%d for constraint %d->%d (type=%d)", id2, id1, id2, iter->second.type()).c_str()); UASSERT(!iter->second.transform().isNull()); if(id1 == id2) @@ -390,9 +397,9 @@ std::map OptimizerGTSAM::optimize( { if(isSlam2d()) { - if(id1 < 0 && !isLandmarkWithRotation.at(id1)) + if(id1 < 0 && !isLandmarkWithRotation_.at(id1)) { - noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Variances(Vector2( + gtsam::noiseModel::Diagonal::shared_ptr model = gtsam::noiseModel::Diagonal::Variances(gtsam::Vector2( 1/iter->second.infMatrix().at(0,0), 1/iter->second.infMatrix().at(1,1))); graph.add(XYFactor(id1, gtsam::Point2(iter->second.transform().x(), iter->second.transform().y()), model)); @@ -400,7 +407,7 @@ std::map OptimizerGTSAM::optimize( } else if (1 / static_cast(iter->second.infMatrix().at(5,5)) >= 9999.0) { - noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Variances(Vector2( + gtsam::noiseModel::Diagonal::shared_ptr model = gtsam::noiseModel::Diagonal::Variances(gtsam::Vector2( 1/iter->second.infMatrix().at(0,0), 1/iter->second.infMatrix().at(1,1))); graph.add(XYFactor(id1, gtsam::Point2(iter->second.transform().x(), iter->second.transform().y()), model)); @@ -429,9 +436,9 @@ std::map OptimizerGTSAM::optimize( } else { - if(id1 < 0 && !isLandmarkWithRotation.at(id1)) + if(id1 < 0 && !isLandmarkWithRotation_.at(id1)) { - noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Precisions(Vector3( + gtsam::noiseModel::Diagonal::shared_ptr model = gtsam::noiseModel::Diagonal::Precisions(gtsam::Vector3( iter->second.infMatrix().at(0,0), iter->second.infMatrix().at(1,1), iter->second.infMatrix().at(2,2))); @@ -442,7 +449,7 @@ std::map OptimizerGTSAM::optimize( 1 / static_cast(iter->second.infMatrix().at(4,4)) >= 9999.0 || 1 / static_cast(iter->second.infMatrix().at(5,5)) >= 9999.0) { - noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Precisions(Vector3( + gtsam::noiseModel::Diagonal::shared_ptr model = gtsam::noiseModel::Diagonal::Precisions(gtsam::Vector3( iter->second.infMatrix().at(0,0), iter->second.infMatrix().at(1,1), iter->second.infMatrix().at(2,2))); @@ -471,10 +478,17 @@ std::map OptimizerGTSAM::optimize( } else if(!isSlam2d() && gravitySigma() > 0 && iter->second.type() == Link::kGravity && newPoses.find(iter->first) != newPoses.end()) { - Vector3 r = gtsam::Pose3(iter->second.transform().toEigen4d()).rotation().xyz(); - gtsam::Unit3 nG = gtsam::Rot3::RzRyRx(r.x(), r.y(), 0).rotate(gtsam::Unit3(0,0,-1)); - gtsam::SharedNoiseModel model = gtsam::noiseModel::Isotropic::Sigmas(gtsam::Vector2(gravitySigma(), gravitySigma())); - graph.add(Pose3GravityFactor(iter->first, nG, model, Unit3(0,0,1))); + gtsam::Rot3 nRbMeas = gtsam::Pose3(iter->second.transform().toEigen4d()).rotation(); + gtsam::Unit3 nZ(0,0,1); + gtsam::Unit3 bGMeas = nRbMeas.unrotate(nZ); + gtsam::SharedNoiseModel model = gtsam::noiseModel::Isotropic::Sigma(2, gravitySigma()); +#if GTSAM_VERSION_NUMERIC <= 40300 + // Note: till 40301 is officially released, version 40300 with "4.3a1" would fail here. + // Just replace "<=" above by "<" to use AttitudeFactor below. + graph.add(gtsam::Pose3AttitudeFactor(iter->first, nZ, model, bGMeas)); +#else + graph.add(gtsam::AttitudeFactor(iter->first, nZ, model, bGMeas)); +#endif lastAddedConstraints_.push_back(ConstraintToFactor(iter->first, iter->first, -1)); } } @@ -494,9 +508,9 @@ std::map OptimizerGTSAM::optimize( t = iter->second.transform().inverse(); std::swap(id1, id2); // should be node -> landmark } - + UASSERT(isLandmarkWithRotation_.find(id2) != isLandmarkWithRotation_.end()); #ifdef RTABMAP_VERTIGO - if(this->isRobust() && isLandmarkWithRotation.at(id2)) + if(this->isRobust() && isLandmarkWithRotation_.at(id2)) { // create new switch variable // Sunderhauf IROS 2012: @@ -514,7 +528,7 @@ std::map OptimizerGTSAM::optimize( gtsam::noiseModel::Diagonal::shared_ptr switchPriorModel = gtsam::noiseModel::Diagonal::Sigmas(gtsam::Vector1(1.0)); graph.add(gtsam::PriorFactor (gtsam::Symbol('s',lastSwitchId_), vertigo::SwitchVariableLinear(prior), switchPriorModel)); } - else if(this->isRobust() && !isLandmarkWithRotation.at(id2)) + else if(this->isRobust() && !isLandmarkWithRotation_.at(id2)) { UWARN("%s cannot be used for landmark constraints without orientation.", Parameters::kOptimizerRobust().c_str()); } @@ -522,7 +536,7 @@ std::map OptimizerGTSAM::optimize( if(isSlam2d()) { - if(isLandmarkWithRotation.at(id2)) + if(isLandmarkWithRotation_.at(id2)) { Eigen::Matrix information = Eigen::Matrix::Identity(); if(!isCovarianceIgnored()) @@ -585,7 +599,7 @@ std::map OptimizerGTSAM::optimize( } else { - if(isLandmarkWithRotation.at(id2)) + if(isLandmarkWithRotation_.at(id2)) { Eigen::Matrix information = Eigen::Matrix::Identity(); if(!isCovarianceIgnored()) @@ -783,7 +797,7 @@ std::map OptimizerGTSAM::optimize( { float x,y,z,roll,pitch,yaw; std::map tmpPoses; - const Values values = isam2_?isam2_->calculateEstimate():optimizer->values(); + const gtsam::Values values = isam2_?isam2_->calculateEstimate():optimizer->values(); #if GTSAM_VERSION_NUMERIC >= 40200 for(gtsam::Values::deref_iterator iter=values.begin(); iter!=values.end(); ++iter) #else @@ -800,9 +814,9 @@ std::map OptimizerGTSAM::optimize( gtsam::Pose2 p = iter->value.cast(); tmpPoses.insert(std::make_pair(key, Transform(p.x(), p.y(), p.theta()))); } - else if(!landmarksIgnored() && isLandmarkWithRotation.find(key)!=isLandmarkWithRotation.end()) + else if(!landmarksIgnored() && isLandmarkWithRotation_.find(key)!=isLandmarkWithRotation_.end()) { - if(isLandmarkWithRotation.at(key)) + if(isLandmarkWithRotation_.at(key)) { newPoses.at(key).getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw); gtsam::Pose2 p = iter->value.cast(); @@ -823,9 +837,9 @@ std::map OptimizerGTSAM::optimize( gtsam::Pose3 p = iter->value.cast(); tmpPoses.insert(std::make_pair(key, Transform::fromEigen4d(p.matrix()))); } - else if(!landmarksIgnored() && isLandmarkWithRotation.find(key)!=isLandmarkWithRotation.end()) + else if(!landmarksIgnored() && isLandmarkWithRotation_.find(key)!=isLandmarkWithRotation_.end()) { - if(isLandmarkWithRotation.at(key)) + if(isLandmarkWithRotation_.at(key)) { gtsam::Pose3 p = iter->value.cast(); tmpPoses.insert(std::make_pair(key, Transform::fromEigen4d(p.matrix()))); @@ -993,9 +1007,9 @@ std::map OptimizerGTSAM::optimize( gtsam::Pose2 p = iter->value.cast(); optimizedPoses.insert(std::make_pair(key, Transform(p.x(), p.y(), z, roll, pitch, p.theta()))); } - else if(!landmarksIgnored() && isLandmarkWithRotation.find(key)!=isLandmarkWithRotation.end()) + else if(!landmarksIgnored() && isLandmarkWithRotation_.find(key)!=isLandmarkWithRotation_.end()) { - if(isLandmarkWithRotation.at(key)) + if(isLandmarkWithRotation_.at(key)) { poses.at(key).getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw); gtsam::Pose2 p = iter->value.cast(); @@ -1016,9 +1030,9 @@ std::map OptimizerGTSAM::optimize( gtsam::Pose3 p = iter->value.cast(); optimizedPoses.insert(std::make_pair(key, Transform::fromEigen4d(p.matrix()))); } - else if(!landmarksIgnored() && isLandmarkWithRotation.find(key)!=isLandmarkWithRotation.end()) + else if(!landmarksIgnored() && isLandmarkWithRotation_.find(key)!=isLandmarkWithRotation_.end()) { - if(isLandmarkWithRotation.at(key)) + if(isLandmarkWithRotation_.at(key)) { gtsam::Pose3 p = iter->value.cast(); optimizedPoses.insert(std::make_pair(key, Transform::fromEigen4d(p.matrix()))); diff --git a/corelib/src/optimizer/OptimizerTORO.cpp b/corelib/src/optimizer/OptimizerTORO.cpp index 175f616d..48c6f075 100644 --- a/corelib/src/optimizer/OptimizerTORO.cpp +++ b/corelib/src/optimizer/OptimizerTORO.cpp @@ -380,7 +380,7 @@ bool OptimizerTORO::saveGraph( if(file) { - for (std::map::const_iterator iter = poses.begin(); iter != poses.end(); ++iter) + for (std::map::const_iterator iter = poses.lower_bound(0); iter != poses.end(); ++iter) { if (isSlam2d()) { @@ -409,7 +409,7 @@ bool OptimizerTORO::saveGraph( for(std::multimap::const_iterator iter = edgeConstraints.begin(); iter!=edgeConstraints.end(); ++iter) { - if (iter->second.type() != Link::kPosePrior && iter->second.type() != Link::kGravity) + if (iter->second.from() != iter->second.to() && iter->second.type() != Link::kLandmark) { if (isSlam2d()) { @@ -494,9 +494,31 @@ bool OptimizerTORO::loadGraph( while ( fgets (line , 400 , file) != NULL ) { std::vector strList = uListToVector(uSplit(uReplaceChar(line, '\n', ' '), ' ')); - if(strList.size() == 8) + if(strList.empty()) { - //VERTEX3 + continue; + } + const std::string & tag = strList[0]; + if(tag.compare("VERTEX2") == 0 && strList.size() == 5) + { + //VERTEX2 id x y theta + int id = atoi(strList[1].c_str()); + float x = uStr2Float(strList[2]); + float y = uStr2Float(strList[3]); + float theta = uStr2Float(strList[4]); + Transform pose(x, y, theta); + if(poses.find(id) == poses.end()) + { + poses.insert(std::make_pair(id, pose)); + } + else + { + UFATAL("Pose %d already added", id); + } + } + else if(tag.compare("VERTEX3") == 0 && strList.size() == 8) + { + //VERTEX3 id x y z roll pitch yaw int id = atoi(strList[1].c_str()); float x = uStr2Float(strList[2]); float y = uStr2Float(strList[3]); @@ -514,9 +536,44 @@ bool OptimizerTORO::loadGraph( UFATAL("Pose %d already added", id); } } - else if(strList.size() == 30) + else if(tag.compare("EDGE2") == 0 && strList.size() == 12) { - //EDGE3 + //EDGE2 observed_vertex_id observing_vertex_id x y theta inf_11 inf_12 inf_13 inf_22 inf_23 inf_33 + int idFrom = atoi(strList[1].c_str()); + int idTo = atoi(strList[2].c_str()); + float x = uStr2Float(strList[3]); + float y = uStr2Float(strList[4]); + float theta = uStr2Float(strList[5]); + cv::Mat informationMatrix = cv::Mat::eye(6,6,CV_64FC1); + informationMatrix.at(0,0) = uStr2Float(strList[6]); // x-x + informationMatrix.at(0,1) = uStr2Float(strList[7]); // x-y + informationMatrix.at(0,5) = uStr2Float(strList[8]); // x-theta + informationMatrix.at(1,1) = uStr2Float(strList[9]); // y-y + informationMatrix.at(1,5) = uStr2Float(strList[10]); // y-theta + informationMatrix.at(5,5) = uStr2Float(strList[11]); // theta-theta + // symmetric counterparts + informationMatrix.at(1,0) = informationMatrix.at(0,1); + informationMatrix.at(5,0) = informationMatrix.at(0,5); + informationMatrix.at(5,1) = informationMatrix.at(1,5); + informationMatrix.at(2,2) = 0.00010001; // 9999 cov + informationMatrix.at(3,3) = 0.00010001; // 9999 cov + informationMatrix.at(4,4) = 0.00010001; // 9999 cov + UASSERT_MSG(informationMatrix.at(0,0) > 0.0 && informationMatrix.at(1,1) > 0.0 && informationMatrix.at(5,5) > 0.0, uFormat("Information matrix should not be null! line=\"%s\"", line).c_str()); + Transform transform(x, y, theta); + if(poses.find(idFrom) != poses.end() && poses.find(idTo) != poses.end()) + { + //Link type is unknown + Link link(idFrom, idTo, Link::kUndef, transform, informationMatrix); + edgeConstraints.insert(std::pair(idFrom, link)); + } + else + { + UERROR("Referred poses from the link (%d->%d) don't exist! Link ignored!", idFrom, idTo); + } + } + else if(tag.compare("EDGE3") == 0 && strList.size() == 30) + { + //EDGE3 observed_vertex_id observing_vertex_id x y z roll pitch yaw inf_11 inf_12 .. inf_16 inf_22 .. inf_66 int idFrom = atoi(strList[1].c_str()); int idTo = atoi(strList[2].c_str()); float x = uStr2Float(strList[3]); @@ -525,15 +582,20 @@ bool OptimizerTORO::loadGraph( float roll = uStr2Float(strList[6]); float pitch = uStr2Float(strList[7]); float yaw = uStr2Float(strList[8]); + // upper triangle is stored row by row (same order as saveGraph) cv::Mat informationMatrix(6,6,CV_64FC1); - informationMatrix.at(3,3) = uStr2Float(strList[9]); - informationMatrix.at(4,4) = uStr2Float(strList[15]); - informationMatrix.at(5,5) = uStr2Float(strList[20]); - UASSERT_MSG(informationMatrix.at(3,3) > 0.0 && informationMatrix.at(4,4) > 0.0 && informationMatrix.at(5,5) > 0.0, uFormat("Information matrix should not be null! line=\"%s\"", line).c_str()); - informationMatrix.at(0,0) = uStr2Float(strList[24]); - informationMatrix.at(1,1) = uStr2Float(strList[27]); - informationMatrix.at(2,2) = uStr2Float(strList[29]); - UASSERT_MSG(informationMatrix.at(0,0) > 0.0 && informationMatrix.at(1,1) > 0.0 && informationMatrix.at(2,2) > 0.0, uFormat("Information matrix should not be null! line=\"%s\"", line).c_str()); + int index = 9; + for(int i=0; i<6; ++i) + { + for(int j=i; j<6; ++j) + { + double value = uStr2Float(strList[index++]); + informationMatrix.at(i,j) = value; + informationMatrix.at(j,i) = value; + } + } + UASSERT_MSG(informationMatrix.at(0,0) > 0.0 && informationMatrix.at(1,1) > 0.0 && informationMatrix.at(2,2) > 0.0 && + informationMatrix.at(3,3) > 0.0 && informationMatrix.at(4,4) > 0.0 && informationMatrix.at(5,5) > 0.0, uFormat("Information matrix should not be null! line=\"%s\"", line).c_str()); Transform transform(x, y, z, roll, pitch, yaw); if(poses.find(idFrom) != poses.end() && poses.find(idTo) != poses.end()) { @@ -543,10 +605,10 @@ bool OptimizerTORO::loadGraph( } else { - UFATAL("Referred poses from the link not exist!"); + UERROR("Referred poses from the link (%d->%d) don't exist! Link ignored!", idFrom, idTo); } } - else if(strList.size()) + else { UFATAL("Error parsing graph file %s on line \"%s\" (strList.size()=%d)", fileName.c_str(), line, (int)strList.size()); } diff --git a/corelib/src/optimizer/ceres/pose_graph_2d/angle_manifold.h b/corelib/src/optimizer/ceres/pose_graph_2d/angle_manifold.h index 2f551038..5a3d1e28 100644 --- a/corelib/src/optimizer/ceres/pose_graph_2d/angle_manifold.h +++ b/corelib/src/optimizer/ceres/pose_graph_2d/angle_manifold.h @@ -77,7 +77,7 @@ class AngleManifold { #else -class AngleManfold { +class AngleManifold { public: template @@ -90,7 +90,7 @@ class AngleManfold { } static ceres::LocalParameterization* Create() { - return (new ceres::AutoDiffLocalParameterization); + return (new ceres::AutoDiffLocalParameterization); } }; diff --git a/corelib/src/optimizer/g2o/edge_sbacam_gravity.h b/corelib/src/optimizer/g2o/edge_sbacam_gravity.h index d2fe24cd..9f146344 100644 --- a/corelib/src/optimizer/g2o/edge_sbacam_gravity.h +++ b/corelib/src/optimizer/g2o/edge_sbacam_gravity.h @@ -32,18 +32,23 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #ifndef RTAB_G2O_EDGE_SBACAM_GRAVITY_H_ #define RTAB_G2O_EDGE_SBACAM_GRAVITY_H_ +#ifdef RTABMAP_ORB_SLAM +#include "g2o/types/types_six_dof_expmap.h" +#else #include "g2o/types/sba/types_sba.h" +#endif #include "g2o/core/base_unary_edge.h" namespace rtabmap { /** * \brief EdgeSBACamGravity * \brief g2o edge with gravity constraint */ -class EdgeSBACamGravity : public g2o::BaseUnaryEdge<3, Eigen::Matrix, g2o::VertexCam> { +class EdgeSBACamGravity : public g2o::BaseUnaryEdge<3, Eigen::Matrix, VertexCam> { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW EdgeSBACamGravity(){ information().setIdentity(); + cameraInvLocalTransform_.setIdentity(); } virtual bool read(std::istream& is) {return false;} // not implemented virtual bool write(std::ostream& os) const {return false;} // not implemented @@ -55,30 +60,36 @@ class EdgeSBACamGravity : public g2o::BaseUnaryEdge<3, Eigen::Matrix(_vertices[0]); + const VertexCam* v = static_cast(_vertices[0]); - Eigen::Vector3d direction = _measurement.head<3>(); - Eigen::Vector3d measurement = _measurement.tail<3>(); + Eigen::Vector3d direction = _measurement.head<3>(); + Eigen::Vector3d measurement = _measurement.tail<3>(); - Eigen::Vector3d ea; + g2o::SE3Quat estimate; +#ifdef RTABMAP_ORB_SLAM + estimate = v->estimate().inverse(); +#else + estimate = v->estimate(); +#endif - // Transform pose from camera frame to world frame - Eigen::Matrix3d t = v1->estimate().rotation().toRotationMatrix() * cameraInvLocalTransform_; - ea[0] = atan2(t (2, 1), t (2, 2)); - ea[1] = asin(-t (2, 0)); - ea[2] = atan2(t (1, 0), t (0, 0)); + // Transform pose from camera frame to world frame + Eigen::Matrix3d t = estimate.rotation().toRotationMatrix() * cameraInvLocalTransform_; + Eigen::Vector3d ea; + ea[0] = atan2(t (2, 1), t (2, 2)); + ea[1] = asin(-t (2, 0)); + ea[2] = atan2(t (1, 0), t (0, 0)); - Eigen::Matrix3d rot = - (Eigen::AngleAxisd(ea[1], Eigen::Vector3d::UnitY()) * - Eigen::AngleAxisd(ea[0], Eigen::Vector3d::UnitX())).toRotationMatrix(); + Eigen::Matrix3d rot = + (Eigen::AngleAxisd(ea[1], Eigen::Vector3d::UnitY()) * + Eigen::AngleAxisd(ea[0], Eigen::Vector3d::UnitX())).toRotationMatrix(); - Eigen::Vector3d estimate = rot * -direction; - _error = estimate - measurement; + Eigen::Vector3d newEstimate = rot * -direction; + _error = newEstimate - measurement; - /*printf("%d : measured=%f %f %f est=%f %f %f error=%f %f %f\n", v1->id(), - measurement[0], measurement[1], measurement[2], - estimate[0], estimate[1], estimate[2], - _error[0], _error[1], _error[2]);*/ + /*printf("%d : measured=%f %f %f est=%f %f %f error=%f %f %f\n", v1->id(), + measurement[0], measurement[1], measurement[2], + estimate[0], estimate[1], estimate[2], + _error[0], _error[1], _error[2]);*/ } // 6 values: diff --git a/corelib/src/optimizer/g2o/edge_sbacam_prior.h b/corelib/src/optimizer/g2o/edge_sbacam_prior.h new file mode 100644 index 00000000..3976e2e8 --- /dev/null +++ b/corelib/src/optimizer/g2o/edge_sbacam_prior.h @@ -0,0 +1,135 @@ +/* +Copyright (c) 2010-2019, 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. +*/ + +/** + * Adapted from EdgeSE3Prior + */ + +#ifndef RTAB_G2O_EDGE_SBACAM_PRIOR_H_ +#define RTAB_G2O_EDGE_SBACAM_PRIOR_H_ + +#ifdef RTABMAP_ORB_SLAM +#include "g2o/types/types_six_dof_expmap.h" +#else +#include "g2o/types/sba/types_sba.h" +#endif +#include "g2o/core/base_unary_edge.h" +namespace rtabmap { + /** + * \brief EdgeSBACamPrior + * \brief g2o edge with gravity constraint + */ +class EdgeSBACamPrior : public g2o::BaseUnaryEdge<6, g2o::SE3Quat, VertexCam> { + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + EdgeSBACamPrior() { + setMeasurement(g2o::SE3Quat()); + information().setIdentity(); + } + + void setCameraInvLocalTransform(const g2o::SE3Quat & t) + { + _cameraInvLocalTransform = t; + } + + // return the error estimate as a 3-vector + void computeError() { + const VertexCam* v = static_cast(_vertices[0]); + g2o::SE3Quat estimate; +#ifdef RTABMAP_ORB_SLAM + estimate = v->estimate().inverse(); +#else + estimate = v->estimate(); +#endif + g2o::SE3Quat delta = _inverseMeasurement * estimate * _cameraInvLocalTransform; + _error[0]=delta.translation().x(); + _error[1]=delta.translation().y(); + _error[2]=delta.translation().z(); + _error[3]=delta.rotation().x(); + _error[4]=delta.rotation().y(); + _error[5]=delta.rotation().z(); + } + + // jacobian + virtual void linearizeOplus() { + _jacobianOplusXi = Eigen::Matrix::Identity(); + } + + virtual void setMeasurement(const g2o::SE3Quat& m){ + _measurement = m; + _inverseMeasurement = m.inverse(); + } + + virtual bool setMeasurementData(const double* d) override { + Eigen::Map > v(d); + // SE3Quat expects [x, y, z, qx, qy, qz, qw] + _measurement.fromVector(v); + _inverseMeasurement = _measurement.inverse(); + return true; + } + + virtual bool getMeasurementData(double* d) const override { + Eigen::Map > v(d); + // Returns [x, y, z, qx, qy, qz, qw] + v = _measurement.toVector(); + return true; + } + + virtual int measurementDimension() const {return 7;} + + virtual double initialEstimatePossible(const g2o::OptimizableGraph::VertexSet& /*from*/, + g2o::OptimizableGraph::Vertex* /*to*/) { + return 1.; + } + + virtual void initialEstimate(const g2o::OptimizableGraph::VertexSet& from, g2o::OptimizableGraph::Vertex* to) { + VertexCam *v = static_cast(_vertices[0]); + assert(v && "Vertex for the Prior edge is not set"); + +#ifdef RTABMAP_ORB_SLAM + g2o::SE3Quat newEstimate = _cameraInvLocalTransform * _inverseMeasurement; +#else + g2o::SE3Quat newEstimate = measurement()*_cameraInvLocalTransform.inverse(); +#endif + if (_information.block<3,3>(0,0).array().abs().sum() == 0){ // do not set translation, as that part of the information is all zero + newEstimate.setTranslation(v->estimate().translation()); + } + if (_information.block<3,3>(3,3).array().abs().sum() == 0){ // do not set rotation, as that part of the information is all zero + newEstimate.setRotation(v->estimate().rotation()); + } + v->setEstimate(newEstimate); + } + + virtual bool read(std::istream& is) override { return true; } + virtual bool write(std::ostream& os) const override { return true; } + protected: + g2o::SE3Quat _inverseMeasurement; + g2o::SE3Quat _cameraInvLocalTransform; +}; + +} +#endif diff --git a/corelib/src/optimizer/g2o/edge_se3_expmap.h b/corelib/src/optimizer/g2o/edge_se3_expmap.h new file mode 100644 index 00000000..5bad8278 --- /dev/null +++ b/corelib/src/optimizer/g2o/edge_se3_expmap.h @@ -0,0 +1,74 @@ +#include "g2o/types/types_six_dof_expmap.h" + +/** + * \brief 3D edge between two SBAcam + */ + class EdgeSE3Expmap : public g2o::BaseBinaryEdge<6, g2o::SE3Quat, g2o::VertexSE3Expmap, g2o::VertexSE3Expmap> +{ + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW; + EdgeSE3Expmap(): BaseBinaryEdge<6, g2o::SE3Quat, g2o::VertexSE3Expmap, g2o::VertexSE3Expmap>(){} + bool read(std::istream& is) + { + return false; + } + + bool write(std::ostream& os) const + { + return false; + } + + void computeError() + { + const g2o::VertexSE3Expmap* v1 = dynamic_cast(_vertices[0]); + const g2o::VertexSE3Expmap* v2 = dynamic_cast(_vertices[1]); + g2o::SE3Quat delta = _inverseMeasurement * (v1->estimate().inverse()*v2->estimate()); + _error[0]=delta.translation().x(); + _error[1]=delta.translation().y(); + _error[2]=delta.translation().z(); + _error[3]=delta.rotation().x(); + _error[4]=delta.rotation().y(); + _error[5]=delta.rotation().z(); + } + + virtual void setMeasurement(const g2o::SE3Quat& meas){ + _measurement=meas; + _inverseMeasurement=meas.inverse(); + } + + virtual double initialEstimatePossible(const g2o::OptimizableGraph::VertexSet& , g2o::OptimizableGraph::Vertex* ) { return 1.;} + virtual void initialEstimate(const g2o::OptimizableGraph::VertexSet& from_, g2o::OptimizableGraph::Vertex* ){ + g2o::VertexSE3Expmap* from = static_cast(_vertices[0]); + g2o::VertexSE3Expmap* to = static_cast(_vertices[1]); + if (from_.count(from) > 0) + to->setEstimate((g2o::SE3Quat) from->estimate() * _measurement); + else + from->setEstimate((g2o::SE3Quat) to->estimate() * _inverseMeasurement); + } + + virtual bool setMeasurementData(const double* d){ + Eigen::Map v(d); + _measurement.fromVector(v); + _inverseMeasurement = _measurement.inverse(); + return true; + } + + virtual bool getMeasurementData(double* d) const{ + Eigen::Map v(d); + v = _measurement.toVector(); + return true; + } + + virtual int measurementDimension() const {return 7;} + + virtual bool setMeasurementFromState() { + const g2o::VertexSE3Expmap* v1 = dynamic_cast(_vertices[0]); + const g2o::VertexSE3Expmap* v2 = dynamic_cast(_vertices[1]); + _measurement = (v1->estimate().inverse()*v2->estimate()); + _inverseMeasurement = _measurement.inverse(); + return true; + } + + protected: + g2o::SE3Quat _inverseMeasurement; +}; \ No newline at end of file diff --git a/corelib/src/optimizer/gtsam/GravityFactor.cpp b/corelib/src/optimizer/gtsam/GravityFactor.cpp deleted file mode 100644 index 81d0da9a..00000000 --- a/corelib/src/optimizer/gtsam/GravityFactor.cpp +++ /dev/null @@ -1,91 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/** - * Author: Mathieu Labbe - * This file is a copy of AttitudeFactor.cpp of gtsam library but - * with attitudeError() function overridden to ignore yaw errors. - * For the noise model, use Sigmas(Vector2(0.1, 10)) (with second sigma high!) - */ - -/** - * @file GravityFactor.cpp - * @author Frank Dellaert - * @brief Implementation file for Attitude factor - * @date January 28, 2014 - **/ - -#include "GravityFactor.h" - -using namespace std; - -namespace rtabmap { - -//*************************************************************************** -Vector GravityFactor::attitudeError(const Rot3& nRb, - OptionalJacobian<2, 3> H) const { - if (H) { - Matrix23 D_nRef_R; - Matrix22 D_e_nRef; - Vector3 r = nRb.xyz(); - Unit3 nRef = Rot3::RzRyRx(r.x(), r.y(), 0).rotate(bRef_, D_nRef_R); - Vector e = nZ_.error(nRef, D_e_nRef); - (*H) = D_e_nRef * D_nRef_R; - //printf("ref=%f %f %f grav=%f %f %f e= %f %f H=%f %f %f, %f %f %f\n", - // nRef.point3().x(), nRef.point3().y(), nRef.point3().z(), nZ_.point3().x(), nZ_.point3().y(), nZ_.point3().z(), e(0), e(1), - // (*H)(0,0), (*H)(0,1), (*H)(0,2), (*H)(1,0), (*H)(1,1), (*H)(1,2)); - return e; - } else { - Vector3 r = nRb.xyz(); - Unit3 nRef = Rot3::RzRyRx(r.x(), r.y(), 0) * bRef_; - Vector e = nZ_.error(nRef); - //printf("ref=%f %f %f grav=%f %f %f e= %f %f\n", nRef.point3().x(), nRef.point3().y(), nRef.point3().z(), nZ_.point3().x(), nZ_.point3().y(), nZ_.point3().z(), e(0), e(1)); - return e; - } -} - -//*************************************************************************** -void Rot3GravityFactor::print(const string& s, - const KeyFormatter& keyFormatter) const { - cout << s << "Rot3GravityFactor on " << keyFormatter(this->key()) << "\n"; - nZ_.print(" measured direction in nav frame: "); - bRef_.print(" reference direction in body frame: "); - this->noiseModel_->print(" noise model: "); -} - -//*************************************************************************** -bool Rot3GravityFactor::equals(const NonlinearFactor& expected, - double tol) const { - const This* e = dynamic_cast(&expected); - return e != NULL && Base::equals(*e, tol) && this->nZ_.equals(e->nZ_, tol) - && this->bRef_.equals(e->bRef_, tol); -} - -//*************************************************************************** -void Pose3GravityFactor::print(const string& s, - const KeyFormatter& keyFormatter) const { - cout << s << "Pose3GravityFactor on " << keyFormatter(this->key()) << "\n"; - nZ_.print(" measured direction in nav frame: "); - bRef_.print(" reference direction in body frame: "); - this->noiseModel_->print(" noise model: "); -} - -//*************************************************************************** -bool Pose3GravityFactor::equals(const NonlinearFactor& expected, - double tol) const { - const This* e = dynamic_cast(&expected); - return e != NULL && Base::equals(*e, tol) && this->nZ_.equals(e->nZ_, tol) - && this->bRef_.equals(e->bRef_, tol); -} - -//*************************************************************************** - -}/// namespace gtsam diff --git a/corelib/src/optimizer/gtsam/GravityFactor.h b/corelib/src/optimizer/gtsam/GravityFactor.h deleted file mode 100644 index fb2b3ee3..00000000 --- a/corelib/src/optimizer/gtsam/GravityFactor.h +++ /dev/null @@ -1,268 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/** - * Author: Mathieu Labbe - * This file is a copy of AttitudeFactor.h of gtsam library but - * with attitudeError() function overridden to ignore yaw errors. - * For the noise model, use Sigmas(Vector2(0.1, 10)) (with second sigma high!) - */ - -/** - * @file Pose3GravityFactor.h - * @author Frank Dellaert - * @brief Header file for Attitude factor - * @date January 28, 2014 - **/ -#pragma once - -#include -#include -#include - -using namespace gtsam; - -namespace rtabmap { - -/** - * Base class for prior on gravity - * Example: - * - measurement is direction of gravity in navigation frame nG - * - reference is direction of z axis in body frame bF - * This factor will give zero error if nG is opposite direction of bF - * @addtogroup Navigation - */ -class GravityFactor { - -protected: - - const Unit3 nZ_, bRef_; ///< Position measurement in - -public: - - /** default constructor - only use for serialization */ - GravityFactor() { - } - - /** - * @brief Constructor - * @param nZ measured direction in navigation frame - * @param bRef reference direction in body frame (default Z-axis in NED frame, i.e., [0; 0; 1]) - */ - GravityFactor(const Unit3& nZ, const Unit3& bRef = Unit3(0, 0, 1)) : - nZ_(nZ), bRef_(bRef) { - } - - /** vector of errors */ - Vector attitudeError(const Rot3& p, -#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 -}; - -/** - * Version of GravityFactor for Rot3 - * @addtogroup Navigation - */ -class Rot3GravityFactor: public NoiseModelFactor1, public GravityFactor { - - typedef NoiseModelFactor1 Base; - -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; - - /** default constructor - only use for serialization */ - Rot3GravityFactor() { - } - - virtual ~Rot3GravityFactor() { - } - - /** - * @brief Constructor - * @param key of the Rot3 variable that will be constrained - * @param nZ measured direction in navigation frame (remove yaw before rotating the gravity vector) - * @param model Gaussian noise model - * @param bRef reference direction in body frame (default Z-axis) - */ - Rot3GravityFactor(Key key, const Unit3& nZ, const SharedNoiseModel& model, - const Unit3& bRef = Unit3(0, 0, 1)) : - Base(model, key), GravityFactor(nZ, bRef) { - } - - /// @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))); - } - - /** print */ - virtual void print(const std::string& s, const KeyFormatter& keyFormatter = - DefaultKeyFormatter) const; - - /** equals */ - virtual bool equals(const NonlinearFactor& expected, double tol = 1e-9) const; - - /** vector of errors */ - virtual Vector evaluateError(const Rot3& nRb, // -#if GTSAM_VERSION_NUMERIC >= 40300 - OptionalMatrixType H = OptionalNone) const { -#else - boost::optional H = boost::none) const { -#endif - return attitudeError(nRb, H); - } - Unit3 nZ() const { - return nZ_; - } - Unit3 bRef() const { - return bRef_; - } - -private: -#if defined(GTSAM_ENABLE_BOOST_SERIALIZATION) || GTSAM_VERSION_NUMERIC < 40300 - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int /*version*/) { - /*ar & boost::serialization::make_nvp("NoiseModelFactor1", - boost::serialization::base_object(*this)); - ar & boost::serialization::make_nvp("GravityFactor", - boost::serialization::base_object(*this));*/ - } -#endif - -public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW -}; - - -/** - * Version of GravityFactor for Pose3 - * @addtogroup Navigation - */ -class Pose3GravityFactor: public NoiseModelFactor1, - public GravityFactor { - - typedef NoiseModelFactor1 Base; - -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; - - /** default constructor - only use for serialization */ - Pose3GravityFactor() { - } - - virtual ~Pose3GravityFactor() { - } - - /** - * @brief Constructor - * @param key of the Pose3 variable that will be constrained - * @param nZ measured direction in navigation frame (remove yaw before rotating the gravity vector) - * @param model Gaussian noise model - * @param bRef reference direction in body frame (default Z-axis) - */ - Pose3GravityFactor(Key key, const Unit3& nZ, const SharedNoiseModel& model, - const Unit3& bRef = Unit3(0, 0, 1)) : - Base(model, key), GravityFactor(nZ, bRef) { - } - - /// @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))); - } - - /** print */ - virtual void print(const std::string& s, const KeyFormatter& keyFormatter = - DefaultKeyFormatter) const; - - /** equals */ - virtual bool equals(const NonlinearFactor& expected, double tol = 1e-9) const; - - /** 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; - *H = Matrix::Zero(2,6); - H->block<2,3>(0,0) = H23; - } - return e; - } - Unit3 nZ() const { - return nZ_; - } - Unit3 bRef() const { - return bRef_; - } - -private: -#if defined(GTSAM_ENABLE_BOOST_SERIALIZATION) || GTSAM_VERSION_NUMERIC < 40300 - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int /*version*/) { - /*ar & boost::serialization::make_nvp("NoiseModelFactor1", - boost::serialization::base_object(*this)); - ar & boost::serialization::make_nvp("GravityFactor", - boost::serialization::base_object(*this));*/ - } -#endif -public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW -}; - -} /// namespace gtsam - diff --git a/corelib/src/optimizer/gtsam/XYFactor.h b/corelib/src/optimizer/gtsam/XYFactor.h index 8a7c6558..80192fda 100644 --- a/corelib/src/optimizer/gtsam/XYFactor.h +++ b/corelib/src/optimizer/gtsam/XYFactor.h @@ -43,7 +43,7 @@ public: // @param H the optional Jacobian matrix, which use boost optional and has default null pointer gtsam::Vector evaluateError(const VALUE& p, #if GTSAM_VERSION_NUMERIC >= 40300 - OptionalMatrixType H = OptionalNone) const { + gtsam::OptionalMatrixType H = OptionalNone) const { #else boost::optional H = boost::none) const { #endif @@ -59,5 +59,5 @@ public: }; -} // namespace gtsamexamples +} // namespace rtabmap diff --git a/corelib/src/optimizer/gtsam/XYZFactor.h b/corelib/src/optimizer/gtsam/XYZFactor.h index 42e55d0b..f1826c6f 100644 --- a/corelib/src/optimizer/gtsam/XYZFactor.h +++ b/corelib/src/optimizer/gtsam/XYZFactor.h @@ -43,7 +43,7 @@ public: // @param H the optional Jacobian matrix, which use boost optional and has default null pointer gtsam::Vector evaluateError(const gtsam::Pose3& p, #if GTSAM_VERSION_NUMERIC >= 40300 - OptionalMatrixType H = OptionalNone) const { + gtsam::OptionalMatrixType H = OptionalNone) const { #else boost::optional H = boost::none) const { #endif @@ -55,7 +55,7 @@ public: } gtsam::Vector evaluateError(const gtsam::Point3& p, #if GTSAM_VERSION_NUMERIC >= 40300 - OptionalMatrixType H = OptionalNone) const { + gtsam::OptionalMatrixType H = OptionalNone) const { #else boost::optional H = boost::none) const { #endif @@ -63,5 +63,5 @@ public: } }; -} // namespace gtsamexamples +} // namespace rtabmap diff --git a/corelib/src/optimizer/vertigo/gtsam/betweenFactorSwitchable.h b/corelib/src/optimizer/vertigo/gtsam/betweenFactorSwitchable.h index de1b4d73..71446457 100644 --- a/corelib/src/optimizer/vertigo/gtsam/betweenFactorSwitchable.h +++ b/corelib/src/optimizer/vertigo/gtsam/betweenFactorSwitchable.h @@ -31,9 +31,9 @@ namespace vertigo { 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 + gtsam::OptionalMatrixType H1 = OptionalNone, + gtsam::OptionalMatrixType H2 = OptionalNone, + gtsam::OptionalMatrixType H3 = OptionalNone) const #else boost::optional H1 = boost::none, boost::optional H2 = boost::none, @@ -71,9 +71,9 @@ namespace vertigo { 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 + gtsam::OptionalMatrixType H1 = OptionalNone, + gtsam::OptionalMatrixType H2 = OptionalNone, + gtsam::OptionalMatrixType H3 = OptionalNone) const #else boost::optional H1 = boost::none, boost::optional H2 = boost::none, diff --git a/corelib/src/optimizer/vertigo/gtsam/switchVariableLinear.h b/corelib/src/optimizer/vertigo/gtsam/switchVariableLinear.h index e95e0b56..708a7f53 100644 --- a/corelib/src/optimizer/vertigo/gtsam/switchVariableLinear.h +++ b/corelib/src/optimizer/vertigo/gtsam/switchVariableLinear.h @@ -13,6 +13,7 @@ // DerivedValue2.h removed from gtsam repo (Dec 2018): https://github.com/borglab/gtsam/commit/e550f4f2aec423cb3f2791b81cb5858b8826ebac #include "DerivedValue.h" #include +#include namespace vertigo { @@ -77,8 +78,8 @@ namespace vertigo { /** between operation */ inline SwitchVariableLinear between(const SwitchVariableLinear& l2, #if GTSAM_VERSION_NUMERIC >= 40300 - OptionalMatrixType H1=OptionalNone, - OptionalMatrixType H2=OptionalNone) const { + gtsam::OptionalMatrixType H1=OptionalNone, + gtsam::OptionalMatrixType H2=OptionalNone) const { #else boost::optional H1=boost::none, boost::optional H2=boost::none) const { diff --git a/corelib/src/optimizer/vertigo/gtsam/switchVariableSigmoid.h b/corelib/src/optimizer/vertigo/gtsam/switchVariableSigmoid.h index 79e1fca9..a949cc2b 100644 --- a/corelib/src/optimizer/vertigo/gtsam/switchVariableSigmoid.h +++ b/corelib/src/optimizer/vertigo/gtsam/switchVariableSigmoid.h @@ -13,6 +13,7 @@ // DerivedValue.h removed from gtsam repo (Dec 2018): https://github.com/borglab/gtsam/commit/e550f4f2aec423cb3f2791b81cb5858b8826ebac #include "DerivedValue.h" #include +#include namespace vertigo { @@ -77,8 +78,8 @@ namespace vertigo { /** between operation */ inline SwitchVariableSigmoid between(const SwitchVariableSigmoid& l2, #if GTSAM_VERSION_NUMERIC >= 40300 - OptionalMatrixType H1=OptionalNone, - OptionalMatrixType H2=OptionalNone) const { + gtsam::OptionalMatrixType H1=OptionalNone, + gtsam::OptionalMatrixType H2=OptionalNone) const { #else boost::optional H1=boost::none, boost::optional H2=boost::none) const { diff --git a/corelib/src/python/PyDetector.cpp b/corelib/src/python/PyDetector.cpp index 46f9f256..a22a2f07 100644 --- a/corelib/src/python/PyDetector.cpp +++ b/corelib/src/python/PyDetector.cpp @@ -1,230 +1,262 @@ -/** - * Python interface for SuperGlue: https://github.com/magicleap/SuperGluePretrainedNetwork - */ - -#include "PyDetector.h" -#include -#include -#include -#include -#include -#include - -#include - -#define NPY_NO_DEPRECATED_API NPY_API_VERSION -#include - -namespace rtabmap -{ - -PyDetector::PyDetector(const ParametersMap & parameters) : - pModule_(0), - pFunc_(0), - path_(Parameters::defaultPyDetectorPath()), - cuda_(Parameters::defaultPyDetectorCuda()) -{ - this->parseParameters(parameters); - - UDEBUG("path = %s", path_.c_str()); - if(!UFile::exists(path_) || UFile::getExtension(path_).compare("py") != 0) - { - UERROR("Cannot initialize Python detector, the path is not valid: \"%s\"=\"%s\"", - Parameters::kPyDetectorPath().c_str(), path_.c_str()); - return; - } - - pybind11::gil_scoped_acquire acquire; - - std::string matcherPythonDir = UDirectory::getDir(path_); - if(!matcherPythonDir.empty()) - { - PyRun_SimpleString("import sys"); - PyRun_SimpleString(uFormat("sys.path.append(\"%s\")", matcherPythonDir.c_str()).c_str()); - } - - _import_array(); - - std::string scriptName = uSplit(UFile::getName(path_), '.').front(); - PyObject * pName = PyUnicode_FromString(scriptName.c_str()); - UDEBUG("PyImport_Import() beg"); - pModule_ = PyImport_Import(pName); - UDEBUG("PyImport_Import() end"); - - Py_DECREF(pName); - - if(!pModule_) - { - UERROR("Module \"%s\" could not be imported! (File=\"%s\")", scriptName.c_str(), path_.c_str()); - UERROR("%s", getPythonTraceback().c_str()); - } -} - -PyDetector::~PyDetector() -{ - pybind11::gil_scoped_acquire acquire; - - if(pFunc_) - { - Py_DECREF(pFunc_); - } - if(pModule_) - { - Py_DECREF(pModule_); - } -} - -void PyDetector::parseParameters(const ParametersMap & parameters) -{ - Feature2D::parseParameters(parameters); - - Parameters::parse(parameters, Parameters::kPyDetectorPath(), path_); - Parameters::parse(parameters, Parameters::kPyDetectorCuda(), cuda_); - - path_ = uReplaceChar(path_, '~', UDirectory::homeDir()); -} - -std::vector PyDetector::generateKeypointsImpl(const cv::Mat & image, const cv::Rect & roi, const cv::Mat & mask) -{ - UDEBUG(""); - descriptors_ = cv::Mat(); - UASSERT(!image.empty() && image.channels() == 1 && image.depth() == CV_8U); - std::vector keypoints; - cv::Mat imgRoi(image, roi); - - UTimer timer; - - if(!pModule_) - { - UERROR("Python detector module not loaded!"); - return keypoints; - } - - pybind11::gil_scoped_acquire acquire; - - if(!pFunc_) - { - PyObject * pFunc = PyObject_GetAttrString(pModule_, "init"); - if(pFunc) - { - if(PyCallable_Check(pFunc)) - { - PyObject * result = PyObject_CallFunction(pFunc, "i", cuda_?1:0); - - if(result == NULL) - { - UERROR("Call to \"init(...)\" in \"%s\" failed!", path_.c_str()); - UERROR("%s", getPythonTraceback().c_str()); - return keypoints; - } - Py_DECREF(result); - - pFunc_ = PyObject_GetAttrString(pModule_, "detect"); - if(pFunc_ && PyCallable_Check(pFunc_)) - { - // we are ready! - } - else - { - UERROR("Cannot find method \"detect(...)\" in %s", path_.c_str()); - UERROR("%s", getPythonTraceback().c_str()); - if(pFunc_) - { - Py_DECREF(pFunc_); - pFunc_ = 0; - } - return keypoints; - } - } - else - { - UERROR("Cannot call method \"init(...)\" in %s", path_.c_str()); - UERROR("%s", getPythonTraceback().c_str()); - return keypoints; - } - Py_DECREF(pFunc); - } - else - { - UERROR("Cannot find method \"init(...)\""); - UERROR("%s", getPythonTraceback().c_str()); - return keypoints; - } - UDEBUG("init time = %fs", timer.ticks()); - } - - if(pFunc_) - { - npy_intp dims[2] = {imgRoi.rows, imgRoi.cols}; - PyObject* pImageBuffer = PyArray_SimpleNewFromData(2, dims, NPY_UBYTE, (void*)imgRoi.data); - UASSERT(pImageBuffer); - - UDEBUG("Preparing data time = %fs", timer.ticks()); - - PyObject *pReturn = PyObject_CallFunctionObjArgs(pFunc_, pImageBuffer, NULL); - if(pReturn == NULL) - { - UERROR("Failed to call match() function!"); - UERROR("%s", getPythonTraceback().c_str()); - } - else - { - UDEBUG("Python detector time = %fs", timer.ticks()); - - if (PyTuple_Check(pReturn) && PyTuple_GET_SIZE(pReturn) == 2) - { - PyObject *kptsPtr = PyTuple_GET_ITEM(pReturn, 0); - PyObject *descPtr = PyTuple_GET_ITEM(pReturn, 1); - if(PyArray_Check(kptsPtr) && PyArray_Check(descPtr)) - { - PyArrayObject *arrayPtr = reinterpret_cast(kptsPtr); - int nKpts = PyArray_SHAPE(arrayPtr)[0]; - int kptSize = PyArray_SHAPE(arrayPtr)[1]; - int type = PyArray_TYPE(arrayPtr); - UDEBUG("Kpts array %dx%d (type=%d)", nKpts, kptSize, type); - UASSERT(kptSize == 3); - UASSERT_MSG(type == NPY_FLOAT, uFormat("Returned matches should type FLOAT=11, received type=%d", type).c_str()); - - float* c_out = reinterpret_cast(PyArray_DATA(arrayPtr)); - keypoints.reserve(nKpts); - for (int i = 0; i < nKpts*kptSize; i+=kptSize) - { - cv::KeyPoint kpt(c_out[i], c_out[i+1], 8, -1, c_out[i+2]); - keypoints.push_back(kpt); - } - - arrayPtr = reinterpret_cast(descPtr); - int nDesc = PyArray_SHAPE(arrayPtr)[0]; - UASSERT(nDesc = nKpts); - int dim = PyArray_SHAPE(arrayPtr)[1]; - type = PyArray_TYPE(arrayPtr); - UDEBUG("Desc array %dx%d (type=%d)", nDesc, dim, type); - UASSERT_MSG(type == NPY_FLOAT, uFormat("Returned matches should type FLOAT=11, received type=%d", type).c_str()); - - c_out = reinterpret_cast(PyArray_DATA(arrayPtr)); - for (int i = 0; i < nDesc*dim; i+=dim) - { - cv::Mat descriptor = cv::Mat(1, dim, CV_32FC1, &c_out[i]).clone(); - descriptors_.push_back(descriptor); - } - } - } - else - { - UWARN("Expected tuple (Kpts 3 x N, Descriptors dim x N), returning empty features."); - } - Py_DECREF(pReturn); - } - Py_DECREF(pImageBuffer); - } - - return keypoints; -} - -cv::Mat PyDetector::generateDescriptorsImpl(const cv::Mat & image, std::vector & keypoints) const -{ - UASSERT((int)keypoints.size() == descriptors_.rows); - return descriptors_; -} - -} +/** + * Python interface for SuperGlue: https://github.com/magicleap/SuperGluePretrainedNetwork + */ + +#include "PyDetector.h" +#include +#include +#include +#include +#include +#include + +#include + +#define NPY_NO_DEPRECATED_API NPY_API_VERSION +#include + +namespace rtabmap +{ + +PyDetector::PyDetector(const ParametersMap & parameters) : + pModule_(0), + pFunc_(0), + path_(Parameters::defaultPyDetectorPath()), + cuda_(Parameters::defaultPyDetectorCuda()) +{ + this->parseParameters(parameters); + + UDEBUG("path = %s", path_.c_str()); + if(!UFile::exists(path_) || UFile::getExtension(path_).compare("py") != 0) + { + UERROR("Cannot initialize Python detector, the path is not valid: \"%s\"=\"%s\"", + Parameters::kPyDetectorPath().c_str(), path_.c_str()); + return; + } + + pybind11::gil_scoped_acquire acquire; + + std::string matcherPythonDir = UDirectory::getDir(path_); + if(!matcherPythonDir.empty()) + { + PyRun_SimpleString("import sys"); + PyRun_SimpleString(uFormat("sys.path.append(\"%s\")", matcherPythonDir.c_str()).c_str()); + } + + _import_array(); + + std::string scriptName = uSplit(UFile::getName(path_), '.').front(); + PyObject * pName = PyUnicode_FromString(scriptName.c_str()); + UDEBUG("PyImport_Import() beg"); + pModule_ = PyImport_Import(pName); + UDEBUG("PyImport_Import() end"); + + Py_DECREF(pName); + + if(!pModule_) + { + UERROR("Module \"%s\" could not be imported! (File=\"%s\")", scriptName.c_str(), path_.c_str()); + UERROR("%s", getPythonTraceback().c_str()); + } +} + +PyDetector::~PyDetector() +{ + pybind11::gil_scoped_acquire acquire; + + if(pFunc_) + { + Py_DECREF(pFunc_); + } + if(pModule_) + { + Py_DECREF(pModule_); + } +} + +void PyDetector::parseParameters(const ParametersMap & parameters) +{ + Feature2D::parseParameters(parameters); + + Parameters::parse(parameters, Parameters::kPyDetectorPath(), path_); + Parameters::parse(parameters, Parameters::kPyDetectorCuda(), cuda_); + + path_ = uReplaceChar(path_, '~', UDirectory::homeDir()); +} + +std::vector PyDetector::generateKeypointsImpl(const cv::Mat & image, const cv::Rect & roi, const cv::Mat & mask) +{ + UDEBUG(""); + descriptors_ = cv::Mat(); + UASSERT(!image.empty() && image.channels() == 1 && image.depth() == CV_8U); + std::vector keypoints; + cv::Mat imgRoi(image, roi); + + UTimer timer; + + if(!pModule_) + { + UERROR("Python detector module not loaded!"); + return keypoints; + } + + pybind11::gil_scoped_acquire acquire; + + if(!pFunc_) + { + PyObject * pFunc = PyObject_GetAttrString(pModule_, "init"); + if(pFunc) + { + if(PyCallable_Check(pFunc)) + { + PyObject * result = PyObject_CallFunction(pFunc, "i", cuda_?1:0); + + if(result == NULL) + { + UERROR("Call to \"init(...)\" in \"%s\" failed!", path_.c_str()); + UERROR("%s", getPythonTraceback().c_str()); + return keypoints; + } + Py_DECREF(result); + + pFunc_ = PyObject_GetAttrString(pModule_, "detect"); + if(pFunc_ && PyCallable_Check(pFunc_)) + { + // we are ready! + } + else + { + UERROR("Cannot find method \"detect(...)\" in %s", path_.c_str()); + UERROR("%s", getPythonTraceback().c_str()); + if(pFunc_) + { + Py_DECREF(pFunc_); + pFunc_ = 0; + } + return keypoints; + } + } + else + { + UERROR("Cannot call method \"init(...)\" in %s", path_.c_str()); + UERROR("%s", getPythonTraceback().c_str()); + return keypoints; + } + Py_DECREF(pFunc); + } + else + { + UERROR("Cannot find method \"init(...)\""); + UERROR("%s", getPythonTraceback().c_str()); + return keypoints; + } + UDEBUG("init time = %fs", timer.ticks()); + } + + if(pFunc_) + { + npy_intp dims[2] = {imgRoi.rows, imgRoi.cols}; + PyObject * pImageBuffer = PyArray_SimpleNewFromData(2, dims, NPY_UBYTE, (void*)imgRoi.data); + UASSERT(pImageBuffer); + + UDEBUG("Preparing data time = %fs", timer.ticks()); + + PyObject * pReturn = PyObject_CallFunctionObjArgs(pFunc_, pImageBuffer, NULL); + if(pReturn == NULL) + { + UERROR("Failed to call match() function!"); + UERROR("%s", getPythonTraceback().c_str()); + } + else + { + UDEBUG("Python detector time = %fs", timer.ticks()); + + if (PyTuple_Check(pReturn) && PyTuple_GET_SIZE(pReturn) == 2) + { + PyObject * kptsPtr = PyTuple_GET_ITEM(pReturn, 0); + PyObject * descPtr = PyTuple_GET_ITEM(pReturn, 1); + if(PyArray_Check(kptsPtr) && PyArray_Check(descPtr)) + { + PyArrayObject *arrayPtr = reinterpret_cast(kptsPtr); + int nKpts = PyArray_SHAPE(arrayPtr)[0]; + int kptSize = PyArray_SHAPE(arrayPtr)[1]; + int type = PyArray_TYPE(arrayPtr); + UDEBUG("Kpts array %dx%d (type=%d)", nKpts, kptSize, type); + UASSERT(kptSize == 3); + UASSERT_MSG(type == NPY_FLOAT, uFormat("Returned matches should type FLOAT=11, received type=%d", type).c_str()); + + float* c_out = reinterpret_cast(PyArray_DATA(arrayPtr)); + std::vector keep_kpt(nKpts); + keypoints.reserve(nKpts); + for (int i = 0, kpt_idx = 0; i < nKpts*kptSize; i+=kptSize, kpt_idx++) + { + // x,y in full image coordinates. Mask is in full image coordinates too. + int full_x = (int)(c_out[i] + roi.x); + int full_y = (int)(c_out[i+1] + roi.y); + keep_kpt[kpt_idx] = mask.empty() || (full_x >= 0 && full_x < mask.cols && full_y >= 0 && full_y < mask.rows && mask.at(full_y, full_x) != 0); + if(keep_kpt[kpt_idx]) { + cv::KeyPoint kpt(c_out[i], c_out[i+1], 8, -1, c_out[i+2]); + keypoints.push_back(kpt); + } + } + + arrayPtr = reinterpret_cast(descPtr); + int nDesc = PyArray_SHAPE(arrayPtr)[0]; + int dim = PyArray_SHAPE(arrayPtr)[1]; + type = PyArray_TYPE(arrayPtr); + UDEBUG("Desc array %dx%d (type=%d)", nDesc, dim, type); + + if(nDesc != nKpts || dim <= 0) + { + UWARN("Python detector returned mismatched arrays: " + "%d keypoints vs %d descriptors (dim=%d). " + "Returning empty features.", + nKpts, nDesc, dim); + keypoints.clear(); + descriptors_ = cv::Mat(); + } + else + { + UASSERT_MSG(type == NPY_FLOAT, uFormat("Returned matches should type FLOAT=11, received type=%d", type).c_str()); + + c_out = reinterpret_cast(PyArray_DATA(arrayPtr)); + for (int i = 0, kpt_idx = 0; i < nDesc*dim; i+=dim, kpt_idx++) + { + if(keep_kpt[kpt_idx]) { + cv::Mat descriptor = cv::Mat(1, dim, CV_32FC1, &c_out[i]).clone(); + descriptors_.push_back(descriptor); + } + } + } + } + } + else + { + UWARN("Expected tuple (Kpts 3 x N, Descriptors dim x N), returning empty features."); + } + Py_DECREF(pReturn); + } + Py_DECREF(pImageBuffer); + } + + // Apply limitKeypoints to enforce maxFeatures and SSC + this->limitKeypoints(keypoints, descriptors_, this->getMaxFeatures(), cv::Size(roi.width, roi.height), this->getSSC()); + + return keypoints; +} + +cv::Mat PyDetector::generateDescriptorsImpl(const cv::Mat & image, std::vector & keypoints) const +{ + if(!keypoints.empty() && (int)keypoints.size() != descriptors_.rows) + { + UERROR("The number of keypoints (%ld) doesn't match the number of buffered " + "descriptors (%d). PyDetector's descriptors extraction should " + "be called right after keypoints detection, with same keypoints " + "returned by the detection. Returning empty descriptors.", + keypoints.size(), descriptors_.rows); + return cv::Mat(); + } + return descriptors_; +} + +} diff --git a/corelib/src/python/PyMatcher.cpp b/corelib/src/python/PyMatcher.cpp index aac482ef..786f0a4f 100644 --- a/corelib/src/python/PyMatcher.cpp +++ b/corelib/src/python/PyMatcher.cpp @@ -228,16 +228,32 @@ std::vector PyMatcher::match( int len2 = PyArray_SHAPE(np_ret)[1]; int type = PyArray_TYPE(np_ret); UDEBUG("Matches array %dx%d (type=%d)", len1, len2, type); - UASSERT_MSG(type == NPY_LONG || type == NPY_INT, uFormat("Returned matches should type INT=5 or LONG=7, received type=%d", type).c_str()); - if(type == NPY_LONG) + UASSERT_MSG(type == NPY_INT32 || type == NPY_UINT32 || type == NPY_INT64 || type == NPY_UINT64, uFormat("Returned matches should type INT32=%d UINT32=%d, INT64=%d or UINT64=%d, received type=%d", NPY_INT, NPY_UINT32, NPY_INT64, NPY_UINT64, type).c_str()); + if(type == NPY_UINT64) { - long* c_out = reinterpret_cast(PyArray_DATA(np_ret)); + long long* c_out = reinterpret_cast(PyArray_DATA(np_ret)); for (int i = 0; i < len1*len2; i+=2) { matches.push_back(cv::DMatch(c_out[i], c_out[i+1], 0)); } } - else // INT + if(type == NPY_INT64) + { + unsigned long long* c_out = reinterpret_cast(PyArray_DATA(np_ret)); + for (int i = 0; i < len1*len2; i+=2) + { + matches.push_back(cv::DMatch(c_out[i], c_out[i+1], 0)); + } + } + else if(type == NPY_UINT32) + { + unsigned int* c_out = reinterpret_cast(PyArray_DATA(np_ret)); + for (int i = 0; i < len1*len2; i+=2) + { + matches.push_back(cv::DMatch(c_out[i], c_out[i+1], 0)); + } + } + else // NPY_INT { int* c_out = reinterpret_cast(PyArray_DATA(np_ret)); for (int i = 0; i < len1*len2; i+=2) diff --git a/corelib/src/python/PythonInterface.cpp b/corelib/src/python/PythonInterface.cpp index 0e2d9879..3b9abc94 100644 --- a/corelib/src/python/PythonInterface.cpp +++ b/corelib/src/python/PythonInterface.cpp @@ -9,6 +9,7 @@ #include #include #include +#include namespace rtabmap { @@ -16,6 +17,14 @@ PythonInterface::PythonInterface() { UINFO("Initialize python interpreter"); guard_ = new pybind11::scoped_interpreter(); + + // Tell Python to look in this directory for DLLs +#ifdef _WIN32 + std::string exe_dir = std::filesystem::current_path().string(); + pybind11::module_ os = pybind11::module_::import("os"); + os.attr("add_dll_directory")(exe_dir); +#endif + pybind11::module::import("threading"); release_ = new pybind11::gil_scoped_release(); } diff --git a/corelib/src/python/rtabmap_superglue.py b/corelib/src/python/rtabmap_superglue.py index d9758d72..c3dafe2e 100644 --- a/corelib/src/python/rtabmap_superglue.py +++ b/corelib/src/python/rtabmap_superglue.py @@ -38,7 +38,6 @@ def init(descriptorDim, matchThreshold, iterations, cuda, model): global superglue superglue = SuperGlue(config.get('superglue', {})).eval().to(device) - def match(kptsFrom, kptsTo, scoresFrom, scoresTo, descriptorsFrom, descriptorsTo, imageWidth, imageHeight): #print("SuperGlue python match()") global device @@ -77,6 +76,8 @@ def match(kptsFrom, kptsTo, scoresFrom, scoresTo, descriptorsFrom, descriptorsTo matchesArray = np.stack((matchesFrom, matchesTo), axis=1); + # rtabmap expects format: + # matches: array Nx2 (type=9 or uint64) return matchesArray diff --git a/corelib/src/python/rtabmap_superpoint.py b/corelib/src/python/rtabmap_superpoint.py index 51fce24d..79f4ce2d 100644 --- a/corelib/src/python/rtabmap_superpoint.py +++ b/corelib/src/python/rtabmap_superpoint.py @@ -8,6 +8,7 @@ import random import numpy as np import torch +import os #import sys #import os @@ -21,15 +22,19 @@ torch.set_grad_enabled(False) device = 'cpu' superpoint = [] +script_dir = os.path.dirname(os.path.abspath(__file__)) + def init(cuda): #print("SuperPoint python init()") global device device = 'cuda' if torch.cuda.is_available() and cuda else 'cpu' + weights_abs_path = os.path.join(script_dir, "superpoint_v1.pth") + # This class runs the SuperPoint network and processes its outputs. global superpoint - superpoint = SuperPointFrontend(weights_path="superpoint_v1.pth", + superpoint = SuperPointFrontend(weights_path=weights_abs_path, nms_dist=4, conf_thresh=0.015, nn_thresh=1, @@ -47,10 +52,14 @@ def detect(imageBuffer): # use copy to make sure memory is correctly re-ordered pts = np.float32(np.transpose(pts)).copy() desc = np.float32(np.transpose(desc)).copy() + + # rtabmap expects format: + # pts: array Nx3 (type=11 or float) + # descriptors: array NxDIM 35x256 (type=11 or float) return pts, desc if __name__ == '__main__': #test - init(True) + init(False) detect(np.random.rand(640,480)*255) diff --git a/corelib/src/python/rtabmap_superpoint_rpautrat.py b/corelib/src/python/rtabmap_superpoint_rpautrat.py new file mode 100644 index 00000000..ebcf18b4 --- /dev/null +++ b/corelib/src/python/rtabmap_superpoint_rpautrat.py @@ -0,0 +1,71 @@ +#! /usr/bin/env python3 +# +# Drop this file in the root folder of SuperPoint git: https://github.com/rpautrat/SuperPoint +# To use with rtabmap: +# --Vis/FeatureType 15 --Kp/DetectorStrategy 15 --PyDetector/Path "~/SuperPoint/rtabmap_superpoint_rpautrat.py" +# +import numpy as np +import os +import torch +from superpoint_pytorch import SuperPoint + +superpoint = [] +device = 'cpu' + +def init(cuda): + global superpoint, device + + superpoint = SuperPoint().eval() + + # set up device, gpu or cpu depending on the availability and the user's choice + device = 'cuda' if torch.cuda.is_available() and cuda else 'cpu' + + # Load weights directly to target device + # Get the directory where this script is located + script_dir = os.path.dirname(os.path.abspath(__file__)) + weights_path = os.path.join(script_dir, 'weights', 'superpoint_v6_from_tf.pth') + + # Load model weights with proper error handling + try: + state_dict = torch.load(weights_path, map_location=device, weights_only=True) + superpoint.load_state_dict(state_dict) + except Exception as e: + print(f"Error loading weights: {e}") + raise + + # Move the model to the target device + superpoint.to(device) + + # Ensure model is in eval mode for inference + superpoint.eval() + +def detect(imageBuffer): + global superpoint, device + + image = np.asarray(imageBuffer) + image = (image.astype('float32') / 255.) + + try: + image_with_dims = image[None, None] # Add batch and channel dims + image_tensor = torch.from_numpy(image_with_dims).float() + image_tensor = image_tensor.to(device) + except Exception as e: + print(f"Error creating tensor: {e}") + raise + # Result: (1, 1, H, W) - PyTorch tensor on correct device (CPU or GPU). + + with torch.no_grad(): + pred = superpoint({'image': image_tensor}) + + # Extract keypoints and descriptors + keypoints = pred['keypoints'][0].cpu().numpy() # Shape: (N, 2) + keypoints_response = pred['keypoint_scores'][0].cpu().numpy() + keypoints_with_response = np.column_stack([keypoints, keypoints_response]).astype(np.float32) + # Result: (N, 3) with [x, y, response] + + descriptors = pred['descriptors'][0].cpu().numpy() + # Result: (N, descriptor_dim) + + desc = np.float32(descriptors).copy() + pts = np.float32(keypoints_with_response).copy() + return pts, desc \ No newline at end of file diff --git a/corelib/src/python/rtabmap_trace_superpoint.py b/corelib/src/python/rtabmap_trace_superpoint.py new file mode 100644 index 00000000..b74db6c1 --- /dev/null +++ b/corelib/src/python/rtabmap_trace_superpoint.py @@ -0,0 +1,13 @@ +import os +import sys +from pathlib import Path + +import torch +import torchvision +from demo_superpoint import SuperPointNet +model = SuperPointNet() +model.load_state_dict(torch.load("superpoint_v1.pth")) +model.eval() +example = torch.rand(1, 1, 640, 480) +traced_script_module = torch.jit.trace(model, example, check_trace=False) +traced_script_module.save("superpoint_v1.pt") diff --git a/corelib/src/resources/DatabaseSchema.sql.in b/corelib/src/resources/DatabaseSchema.sql.in index 0b2a37d8..ea905de5 100644 --- a/corelib/src/resources/DatabaseSchema.sql.in +++ b/corelib/src/resources/DatabaseSchema.sql.in @@ -131,7 +131,9 @@ CREATE TABLE Admin ( opt_map BLOB, -- compressed CV_8SC1 occupancy grid opt_map_x_min FLOAT, opt_map_y_min FLOAT, - opt_map_resolution FLOAT, + opt_map_resolution FLOAT, + + dictionary_index BLOB, -- serialized dictionary index time_enter DATE ); diff --git a/corelib/src/resources/backward_compatibility/DatabaseSchema_0_22_0.sql b/corelib/src/resources/backward_compatibility/DatabaseSchema_0_22_0.sql new file mode 100644 index 00000000..75cd7d19 --- /dev/null +++ b/corelib/src/resources/backward_compatibility/DatabaseSchema_0_22_0.sql @@ -0,0 +1,183 @@ +-- ******************************************************************* +-- DatabaseSchema: Script for creating the database +-- Usage: +-- $ sqlite3 LTM.db < DatabaseSchema.sql +-- +-- ******************************************************************* + +-- ******************************************************************* +-- CLEAN +-- ******************************************************************* +/*DROP TABLE Node;*/ + +-- ******************************************************************* +-- CREATE +-- ******************************************************************* +CREATE TABLE Node ( + id INTEGER NOT NULL, + map_id INTEGER NOT NULL, + weight INTEGER, + stamp FLOAT, + pose BLOB, -- 3x4 float + ground_truth_pose BLOB, -- 3x4 float + velocity BLOB, -- 6 float (vx,vy,vz,vroll,vpitch,vyaw) m/s and rad/s + label TEXT, + gps BLOB, -- 1x6 double: stamp, longitude (DD), latitude (DD), altitude (m), accuracy (m), bearing (North 0->360 deg clockwise) + env_sensors BLOB, -- Variable 3xdouble: (sensorId1, value, stamp, sensorId2, value, stamp, ...) + time_enter DATE, + PRIMARY KEY (id) +); + +CREATE TABLE Data ( + id INTEGER NOT NULL, + image BLOB, -- compressed image (Grayscale or RGB) + depth BLOB, -- compressed image (Depth or Right image) + depth_confidence BLOB, -- compressed data (low=0 high=100) + calibration BLOB, -- fx, fy, cx, cy, [baseline,] width, height, local_transform + + scan BLOB, -- compressed data (Laser scan) + scan_info BLOB, -- scan_max_pts, scan_max_range, scan_format, local_transform + + ground_cells BLOB, -- compressed data (occupancy grid) + obstacle_cells BLOB, -- compressed data (occupancy grid) + empty_cells BLOB, -- compressed data (occupancy grid) + cell_size FLOAT, + view_point_x FLOAT, + view_point_y FLOAT, + view_point_z FLOAT, + + user_data BLOB, -- compressed data (User data) + time_enter DATE, + PRIMARY KEY (id) +); + +CREATE TABLE Link ( + from_id INTEGER NOT NULL, + to_id INTEGER NOT NULL, + type INTEGER NOT NULL, -- kNeighbor=0, kGlobalClosure=1, kLocalSpaceClosure=2, kLocalTimeClosure=3, kUserClosure=4, kVirtualClosure=5, kNeighborMerged=6, kPosePrior=7, kLandmark=8 + information_matrix BLOB NOT NULL, -- 6x6 double (inverse covariance) + transform BLOB, -- 3x4 float + user_data BLOB, -- compressed data (User data) + FOREIGN KEY (from_id) REFERENCES Node(id), + FOREIGN KEY (to_id) REFERENCES Node(id) +); + +-- +CREATE TABLE Word ( + id INTEGER NOT NULL, + descriptor_size INTEGER NOT NULL, + descriptor BLOB NOT NULL, + time_enter DATE, + PRIMARY KEY (id) +); + +CREATE TABLE Feature ( + node_id INTEGER NOT NULL, + word_id INTEGER NOT NULL, + pos_x FLOAT NOT NULL, + pos_y FLOAT NOT NULL, + size INTEGER NOT NULL, + dir FLOAT NOT NULL, + response FLOAT NOT NULL, + octave INTEGER NOT NULL, + depth_x FLOAT, + depth_y FLOAT, + depth_z FLOAT, + descriptor_size INTEGER, + descriptor BLOB, + FOREIGN KEY (node_id) REFERENCES Node(id) +); + +CREATE TABLE GlobalDescriptor ( + node_id INTEGER NOT NULL, + type INTEGER NOT NULL, + info BLOB, + data BLOB NOT NULL, + FOREIGN KEY (node_id) REFERENCES Node(id) +); + +-- + +CREATE TABLE Info ( + STM_size INTEGER, + last_sign_added INTEGER, + process_mem_used INTEGER, + database_mem_used INTEGER, + dictionary_size INTEGER, + parameters TEXT, + time_enter DATE +); + +CREATE TABLE Statistics ( + id INTEGER NOT NULL, + stamp FLOAT, + data BLOB, -- compressed string + wm_state BLOB, -- compressed data + FOREIGN KEY (id) REFERENCES Node(id) +); + +CREATE TABLE Admin ( + version TEXT, + preview_image BLOB, -- compressed image + + opt_cloud BLOB, -- compressed data + opt_ids BLOB, -- Node ids used to generate the optimized cloud/mesh + opt_poses BLOB, -- compressed N*3x4 float + opt_last_localization BLOB, -- 3x4 float + opt_polygons_size INTEGER, -- e.g., 3 + opt_polygons BLOB, -- compressed data [length_v0, i0,i1,i3, length_v1, i0,i1,i3] + opt_tex_coords BLOB, -- compressed data [length_v0, u0,v0,u1,v1,u2,v2, length_v1, u0,v0,u1,v1,u2,v2] + opt_tex_materials BLOB, -- compressed image + opt_map BLOB, -- compressed CV_8SC1 occupancy grid + opt_map_x_min FLOAT, + opt_map_y_min FLOAT, + opt_map_resolution FLOAT, + + time_enter DATE +); + +-- ******************************************************************* +-- TRIGGERS +-- ******************************************************************* +CREATE TRIGGER insert_Feature BEFORE INSERT ON Feature +WHEN NOT EXISTS (SELECT Node.id FROM Node WHERE Node.id = NEW.node_id) +BEGIN + SELECT RAISE(ABORT, 'Foreign key constraint failed in Feature table'); +END; + + -- Creating a trigger for time_enter +CREATE TRIGGER insert_Node_timeEnter AFTER INSERT ON Node +BEGIN + UPDATE Node SET time_enter = DATETIME('NOW') WHERE rowid = new.rowid; +END; + +CREATE TRIGGER insert_Data_timeEnter AFTER INSERT ON Data +BEGIN + UPDATE Node SET time_enter = DATETIME('NOW') WHERE rowid = new.rowid; +END; + +CREATE TRIGGER insert_Word_timeEnter AFTER INSERT ON Word +BEGIN + UPDATE Word SET time_enter = DATETIME('NOW') WHERE rowid = new.rowid; +END; + +CREATE TRIGGER insert_Info_timeEnter AFTER INSERT ON Info +BEGIN + UPDATE Info SET time_enter = DATETIME('NOW') WHERE rowid = new.rowid; +END; + +-- ******************************************************************* +-- INDEXES +-- ******************************************************************* +CREATE UNIQUE INDEX IDX_Node_id on Node (id); +CREATE INDEX IDX_Feature_node_id on Feature (node_id); +CREATE INDEX IDX_GlobalDescriptor_node_id on GlobalDescriptor (node_id); +CREATE INDEX IDX_Link_from_id on Link (from_id); +CREATE UNIQUE INDEX IDX_node_label on Node (label); +CREATE UNIQUE INDEX IDX_Statistics_id on Statistics (id); + +-- ******************************************************************* +-- VERSION +-- ******************************************************************* +INSERT INTO Admin(version) VALUES('0.22.0'); + diff --git a/corelib/src/rtflann/flann.hpp b/corelib/src/rtflann/flann.hpp index 6507cf8d..11477cdf 100644 --- a/corelib/src/rtflann/flann.hpp +++ b/corelib/src/rtflann/flann.hpp @@ -103,7 +103,6 @@ public: { flann_algorithm_t index_type = get_param(params,"algorithm"); loaded_ = false; - if (index_type == FLANN_INDEX_SAVED) { nnIndex_ = load_saved_index(features, get_param(params,"filename"), distance); loaded_ = true; @@ -180,10 +179,20 @@ public: if (fout == NULL) { throw FLANNException("Cannot open file"); } - nnIndex_->saveIndex(fout); + save(fout); fclose(fout); } + /** + * Save index to file stream. + * Caller has to open file stream with "wb" and close it afterwards. + * @param filename + */ + void save(FILE * stream) + { + nnIndex_->saveIndex(stream); + } + /** * \returns number of features in this index. */ @@ -377,6 +386,26 @@ public: return nnIndex_->radiusSearch(queries, indices, dists, radius, params); } + void load_saved_index(FILE* fin) + { + if(loaded_) { + throw FLANNException("Index already loaded!"); + } + if(nnIndex_->sizeAtBuild() != 0) { + throw FLANNException("Index must not be already built to load data."); + } + if (fin == NULL) { + throw FLANNException("File pointer must be valid!"); + } + IndexHeader header = load_header(fin); + if (header.h.data_type != flann_datatype_value::value) { + throw FLANNException("Datatype of saved index is different than of the one to be loaded."); + } + rewind(fin); + nnIndex_->loadIndex(fin); + loaded_ = true; + } + private: IndexType* load_saved_index(const Matrix& dataset, const std::string& filename, Distance distance) { diff --git a/corelib/src/superpoint_rpautrat/SuperpointRpautrat.cpp b/corelib/src/superpoint_rpautrat/SuperpointRpautrat.cpp new file mode 100644 index 00000000..baa37593 --- /dev/null +++ b/corelib/src/superpoint_rpautrat/SuperpointRpautrat.cpp @@ -0,0 +1,234 @@ +/** + * SuperPoint implementation based on the PyTorch version by Rémi Pautrat, Paul-Edouard Sarlin + * Adapted for RTAB-Map integration + */ + +#include "SuperpointRpautrat.h" +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include "superpoint_to_torchscript_py.h" + +namespace rtabmap +{ + +// Run the python script to export the SuperPoint model file with the desired parameters +static std::string exportSuperPointTorchScript( + const std::string & superpointWeightsPath, + const std::string & superpointModelPath, + const std::string & outputDir, + const int & width, + const int & height, + const float & threshold, + const int & nms_radius, + const bool & cuda) +{ + // Validate output directory is explicitly set and exists + if(outputDir.empty()) + { + UERROR("Output directory is not set."); + return std::string(""); + } + if(!UDirectory::exists(outputDir)) + { + UERROR("Output directory does not exist: %s", outputDir.c_str()); + return std::string(""); + } + + // Resolve paths (no dependency on source tree) + const std::string weightsPath = superpointWeightsPath; + const std::string modelPath = superpointModelPath; + const std::string output = std::string(outputDir + "/superpoint_v6_from_tf.pt"); + + // Sanity checks + if(!UFile::exists(weightsPath)) { + UERROR("Weights not found: %s", weightsPath.c_str()); + return ""; + } + if(!UFile::exists(modelPath)) { + UERROR("Model not found: %s", modelPath.c_str()); + return ""; + } + + // Execute the script inside the embedded Python interpreter + try + { + pybind11::gil_scoped_acquire acquire; + pybind11::dict scope; + scope["__builtins__"] = pybind11::module_::import("builtins"); + + // set sys.path to the location of the model definition so it can be imported + std::string model_dir = UDirectory::getDir(modelPath); + auto sys = pybind11::module_::import("sys"); + pybind11::list sys_path = sys.attr("path"); + sys_path.attr("insert")(0, model_dir); + + try { + // execute the script to generate the model + pybind11::exec(uHex2Str(SUPERPOINT_TO_TORCHSCRIPT_PY), scope, scope); + pybind11::function generate_model = scope["generate_model"].cast(); + pybind11::object result = generate_model(weightsPath, output, cuda, nms_radius, threshold, width, height); + sys_path.attr("remove")(model_dir); + } + catch(...) { + // Ensure sys.path cleanup on any exception + sys_path.attr("remove")(model_dir); + throw; + } + + } + // pybind11 throws std::exception for RuntimeError + catch (const std::exception &e) + { + UERROR("Python export failed: %s", e.what()); + return ""; + } + + return output; +} + +SPDetectorRpautrat::SPDetectorRpautrat(std::string superpointWeightsPath, std::string superpointModelPath, std::string outputDir, float threshold, bool nms, int minDistance, bool cuda, int maxFeatures, bool ssc) : + device_(torch::kCPU), + superpointWeightsPath_(superpointWeightsPath), + superpointModelPath_(superpointModelPath), + outputDir_(outputDir), + threshold_(threshold), + nms_(nms), + minDistance_(minDistance), + maxFeatures_(maxFeatures), + ssc_(ssc), + detected_(false) +{ + if(cuda && !torch::cuda::is_available()) + { + UWARN("Cuda option is enabled but torch doesn't have cuda support on this platform, using CPU instead."); + } + cuda_ = cuda && torch::cuda::is_available(); + + if(!UFile::exists(superpointWeightsPath_)) { + UERROR("Superpoint weights not found: %s", superpointWeightsPath_.c_str()); + } + + // Update device based on cuda availability + device_ = torch::Device(cuda_ ? torch::kCUDA : torch::kCPU); +} + +SPDetectorRpautrat::~SPDetectorRpautrat() +{ +} + +cv::Mat SPDetectorRpautrat::compute(const std::vector &keypoints) +{ + if(!detected_) + { + UERROR("SPDetectorRpautrat has been reset before extracting the descriptors! detect() should be called before compute()."); + return cv::Mat(); + } + if(keypoints.empty()) + { + return cv::Mat(); + } + + // These should have the same size + UASSERT(static_cast(desc_.rows) == keypoints.size()); + + return desc_; +} + +std::vector SPDetectorRpautrat::detect(const cv::Mat &img, const cv::Mat & mask) +{ + // On first frame, run a trace of the model with the desired parameters and load the model file + if(!detected_) + { + // effectively disable nms if it is not enabled by setting radius to 0 + int nms_radius = nms_ ? minDistance_ : 0; + + std::string modelPath = exportSuperPointTorchScript( + superpointWeightsPath_, + superpointModelPath_, + outputDir_, + img.cols, + img.rows, + threshold_, + nms_radius, + cuda_ + ); + + UDEBUG("Initializing SuperPoint Rpautrat detector with model: %s", modelPath.c_str()); + UDEBUG("modelPath=%s thr=%f nms=%d minDistance=%d cuda=%d", modelPath.c_str(), threshold_, nms_?1:0, minDistance_, cuda_?1:0); + if(modelPath.empty()) + { + UERROR("Model's path is empty! The model was not exported correctly."); + return std::vector(); + } + if(!UFile::exists(modelPath)) + { + UERROR("Model's path \"%s\" doesn't exist!", modelPath.c_str()); + return std::vector(); + } + + // Load TorchScript model + model_ = torch::jit::load(modelPath); + model_.eval(); // put in evaluation mode + model_.to(device_); + } + + // format the input tensor for the model + torch::NoGradGuard no_grad_guard; + auto x = torch::from_blob(img.data, {1, 1, img.rows, img.cols}, torch::kByte); + x = x.to(torch::kFloat) / 255; + x = x.set_requires_grad(false).to(device_); + + auto outputs = model_.forward({x}).toTuple(); + auto kpts_tensor = outputs->elements()[0].toTensor(); // [N, 2] keypoint coordinates + auto scores_tensor = outputs->elements()[1].toTensor(); // [N] keypoint scores + torch::Tensor desc_tensor = outputs->elements()[2].toTensor(); // [N, 256] descriptors + + // Convert to CPU for processing + auto keypoints_cpu = kpts_tensor.to(torch::kCPU); + auto scores_cpu = scores_tensor.to(torch::kCPU); + + std::vector filtered_keypoints; + std::vector keep_indices_vec; + + // Apply mask filtering + for(int i = 0; i < keypoints_cpu.size(0); i++) { + float score = scores_cpu[i].item(); + float x = keypoints_cpu[i][0].item(); // x coordinate + float y = keypoints_cpu[i][1].item(); // y coordinate + + // Check mask if provided + if(mask.empty() || mask.at((int)y, (int)x) != 0) { + keep_indices_vec.push_back(i); + filtered_keypoints.emplace_back(cv::KeyPoint(x, y, 8, -1, score)); + } + } + + // Filter descriptors based on mask + auto keep_indices = torch::from_blob(keep_indices_vec.data(), {(long int)keep_indices_vec.size()}, torch::kLong); + keep_indices = keep_indices.to(desc_tensor.device()); + auto filtered_descriptors = desc_tensor.index_select(0, keep_indices); + + // Convert descriptors to cv::Mat + auto filtered_descriptors_cpu = filtered_descriptors.to(torch::kCPU); + cv::Mat descriptors_mat(filtered_descriptors_cpu.size(0), filtered_descriptors_cpu.size(1), CV_32FC1, filtered_descriptors_cpu.data_ptr()); + cv::Mat descriptors_clone = descriptors_mat.clone(); // Clone to own the memory + + // Apply limitKeypoints to enforce maxFeatures and SSC + Feature2D::limitKeypoints(filtered_keypoints, descriptors_clone, maxFeatures_, cv::Size(img.cols, img.rows), ssc_); + + desc_ = descriptors_clone; + detected_ = true; + return filtered_keypoints; +} + +} // namespace rtabmap \ No newline at end of file diff --git a/corelib/src/superpoint_rpautrat/SuperpointRpautrat.h b/corelib/src/superpoint_rpautrat/SuperpointRpautrat.h new file mode 100644 index 00000000..0eefaa57 --- /dev/null +++ b/corelib/src/superpoint_rpautrat/SuperpointRpautrat.h @@ -0,0 +1,59 @@ +/** + * SuperPoint implementation based on the PyTorch version by Rémi Pautrat, Paul-Edouard Sarlin + * Adapted for RTAB-Map integration + */ + +#ifndef SUPERPOINT_RPAUTRAT_H +#define SUPERPOINT_RPAUTRAT_H + +#include +#include +#include +#include + +namespace rtabmap +{ + +class SPDetectorRpautrat { + public: + SPDetectorRpautrat( + std::string superpointWeightsPath, + std::string superpointModelPath, + std::string outputDir, + float threshold = 0.005f, + bool nms = true, + int nmsRadius = 4, + bool cuda = false, + int maxFeatures = 1000, + bool ssc = false + ); + virtual ~SPDetectorRpautrat(); + std::vector detect(const cv::Mat &img, const cv::Mat & mask = cv::Mat()); + cv::Mat compute(const std::vector &keypoints); + + // Setters for post-processing parameters that don't require model reinitialization + void setMaxFeatures(int maxFeatures) { maxFeatures_ = maxFeatures; } + void setSSC(bool ssc) { ssc_ = ssc; } + + private: + torch::jit::script::Module model_; + torch::Device device_; + cv::Mat desc_; + + std::string superpointWeightsPath_; + std::string superpointModelPath_; + std::string outputDir_; + float threshold_; + bool nms_; + int minDistance_; + bool cuda_; + int maxFeatures_; + bool ssc_; + + bool detected_; + }; +} + + + +#endif // SUPERPOINT_RPAUTRAT_H diff --git a/corelib/src/superpoint_rpautrat/superpoint_to_torchscript.py b/corelib/src/superpoint_rpautrat/superpoint_to_torchscript.py new file mode 100644 index 00000000..804ddc85 --- /dev/null +++ b/corelib/src/superpoint_rpautrat/superpoint_to_torchscript.py @@ -0,0 +1,107 @@ +#!/usr/bin/env python3 +""" +Convert PyTorch weights to TorchScript format for C++ usage. +""" + +import argparse +import os + +import torch +import torch.nn as nn +from superpoint_pytorch import SuperPoint + + +def wrap_model(model: nn.Module): + """ + Simple wrapper to fix SuperPoint input format for TorchScript. + Easier to call from C++ code since the input isn't a dictionary. + """ + class Wrapper(nn.Module): + def __init__(self, net: nn.Module): + super().__init__() + self.net = net + + def forward(self, x: torch.Tensor): + # SuperPoint expects {"image": tensor} but TorchScript doesn't like dict indexing + out = self.net.forward({"image": x}) + + # Return the format expected by C++ code: keypoints, scores, descriptors + # For single batch item, take the first (and only) element + keypoints = out["keypoints"][0] if out["keypoints"] else torch.empty(0, 2) + scores = out["keypoint_scores"][0] if out["keypoint_scores"] else torch.empty(0) + descriptors = out["descriptors"][0] if out["descriptors"] else torch.empty(0, 256) + + return (keypoints, scores, descriptors) + + return Wrapper(model) + +def generate_model( + weights_path: str, + output_path: str, + cuda: bool, + nms_radius: int, + threshold: float, + width: int, + height: int, +): + # Check if weights are already TorchScript + try: + scripted = torch.jit.load(weights_path, map_location="cpu") + scripted.eval() + torch.jit.save(scripted, output_path) + print(f"Converted TorchScript file: {output_path}") + return + except: + pass + + device = "cuda" if cuda else "cpu" + + # Load SuperPoint model and weights + model = SuperPoint( + nms_radius=nms_radius, + detection_threshold=threshold, + ).eval().to(device) + + # Load weights without forcing CPU location to allow CUDA usage + weights = torch.load(weights_path, map_location=None) + if isinstance(weights, dict) and "state_dict" in weights: + weights = weights["state_dict"] + + model.load_state_dict(weights, strict=False) + + wrapped = wrap_model(model) + dummy = torch.randn(1, 1, height, width, device=device) # Dummy input, grayscale, using cuda. + + # Convert to TorchScript using trace (SuperPoint has dynamic behavior that scripting can't handle) + print("Using torch.jit.trace (SuperPoint has dynamic behavior)...") + scripted = torch.jit.trace(wrapped, (dummy,), strict=False) + print("Successfully traced SuperPoint model") + + # Save output + os.makedirs(os.path.dirname(output_path), exist_ok=True) + torch.jit.save(scripted, output_path) + print(f"Converted SuperPoint weights to TorchScript: {output_path}") + + + +if __name__ == "__main__": + parser = argparse.ArgumentParser(description="Convert SuperPoint weights to TorchScript") + parser.add_argument("--weights", required=True, help="Path to weights file") + parser.add_argument("--output", required=True, help="Output TorchScript file") + parser.add_argument("--cuda", action="store_true", help="Use CUDA") + parser.add_argument("--width", type=int, default=1920, help="Width of the input image") + parser.add_argument("--height", type=int, default=288, help="Height of the input image") + parser.add_argument("--nms_radius", type=int, default=4, help="NMS radius") + parser.add_argument("--threshold", type=float, default=0.005, help="Confidence threshold") + args = parser.parse_args() + print(f"Generating model from weights: {args.weights} to output: {args.output}") + + generate_model( + weights_path=args.weights, + output_path=args.output, + cuda=args.cuda, + nms_radius=args.nms_radius, + threshold=args.threshold, + width=args.width, + height=args.height, + ) diff --git a/corelib/src/superpoint_torch/SuperPoint.cc b/corelib/src/superpoint_torch/SuperPoint.cc index 17dbd0cc..94979c1c 100644 --- a/corelib/src/superpoint_torch/SuperPoint.cc +++ b/corelib/src/superpoint_torch/SuperPoint.cc @@ -144,7 +144,12 @@ std::vector SPDetector::detect(const cv::Mat &img, const cv::Mat & UASSERT(img.type() == CV_8UC1); UASSERT(mask.empty() || (mask.type() == CV_8UC1 && img.cols == mask.cols && img.rows == mask.rows)); detected_ = false; - if(model_) + if(!model_) + { + UERROR("No model is loaded!"); + return std::vector(); + } + try { torch::NoGradGuard no_grad_guard; auto x = torch::from_blob(img.data, {1, 1, img.rows, img.cols}, torch::kByte); @@ -199,9 +204,9 @@ std::vector SPDetector::detect(const cv::Mat &img, const cv::Mat & detected_ = true; return keypoints; } - else + catch(const std::exception & e) { - UERROR("No model is loaded!"); + UERROR("SPDetector::detect() threw: %s", e.what()); return std::vector(); } } diff --git a/corelib/src/util2d.cpp b/corelib/src/util2d.cpp index 604eaa33..0251d0fe 100644 --- a/corelib/src/util2d.cpp +++ b/corelib/src/util2d.cpp @@ -2296,6 +2296,7 @@ std::vector SSC( const std::vector & keypoints, int maxKeypoints, float tolerance, int cols, int rows, const std::vector & indx) { bool useIndx = keypoints.size() == indx.size(); + maxKeypoints = maxKeypoints - round(maxKeypoints * tolerance); // Just the make sure the solution will always be <= input maxKeypoints // several temp expression variables to simplify solution equation int exp1 = rows + cols + 2*maxKeypoints; diff --git a/corelib/src/util3d.cpp b/corelib/src/util3d.cpp index 563e22a6..facb0d6c 100644 --- a/corelib/src/util3d.cpp +++ b/corelib/src/util3d.cpp @@ -1762,6 +1762,55 @@ LaserScan laserScanFromPointCloud(const pcl::PointCloud & cloud, return laserScanFromPointCloud(cloud, pcl::IndicesPtr(), transform, filterNaNs); } +LaserScan laserScanFromPointCloud(const pcl::PointCloud & cloud, const Transform & transform, bool filterNaNs) +{ + return laserScanFromPointCloud(cloud, pcl::IndicesPtr(), transform, filterNaNs); +} + +LaserScan laserScanFromPointCloud(const pcl::PointCloud & cloud, const pcl::IndicesPtr & indices, const Transform & transform, bool filterNaNs) +{ + // Layout: [x, y, z, intensity, ring, time] (ring cast to float, values up to + // ~16M are exactly representable so all realistic laser line counts fit). + cv::Mat laserScan; + bool nullTransform = transform.isNull() || transform.isIdentity(); + Eigen::Affine3f transform3f = transform.toEigen3f(); + int oi = 0; + const int total = indices.get() ? (int)indices->size() : (int)cloud.size(); + laserScan = cv::Mat(1, total, CV_32FC(6)); + for(int i=0; iat(i) : i; + const rtabmap::PointXYZIRT & src = cloud.at(index); + if(filterNaNs && !pcl::isFinite(src)) + { + continue; + } + float * ptr = laserScan.ptr(0, oi++); + if(!nullTransform) + { + pcl::PointXYZ pt(src.x, src.y, src.z); + pt = pcl::transformPoint(pt, transform3f); + ptr[0] = pt.x; + ptr[1] = pt.y; + ptr[2] = pt.z; + } + else + { + ptr[0] = src.x; + ptr[1] = src.y; + ptr[2] = src.z; + } + ptr[3] = src.intensity; + ptr[4] = static_cast(src.ring); + ptr[5] = src.time; + } + if(oi == 0) + { + return LaserScan(); + } + return LaserScan(laserScan(cv::Range::all(), cv::Range(0, oi)), 0, 0.0f, LaserScan::kXYZIRT); +} + LaserScan laserScanFromPointCloud(const pcl::PointCloud & cloud, const pcl::IndicesPtr & indices, const Transform & transform, bool filterNaNs) { cv::Mat laserScan; @@ -2343,7 +2392,7 @@ pcl::PCLPointCloud2::Ptr laserScanToPointCloud2(const LaserScan & laserScan, con { pcl::toPCLPointCloud2(*laserScanToPointCloud(laserScan, transform), *cloud); } - else if(laserScan.format() == LaserScan::kXYI || laserScan.format() == LaserScan::kXYZI || laserScan.format() == LaserScan::kXYZIT) + else if(laserScan.format() == LaserScan::kXYI || laserScan.format() == LaserScan::kXYZI || laserScan.format() == LaserScan::kXYZIT || laserScan.format() == LaserScan::kXYZIRT) { pcl::toPCLPointCloud2(*laserScanToPointCloudI(laserScan, transform), *cloud); } @@ -3807,9 +3856,11 @@ LaserScan deskew( return LaserScan(); } - if(input.format() != LaserScan::kXYZIT) + if(!input.hasTime()) { - UERROR("input scan doesn't have \"time\" channel! Only format \"%s\" supported yet.", LaserScan::formatName(LaserScan::kXYZIT).c_str()); + UERROR("input scan doesn't have a \"time\" channel! Supported formats: \"%s\", \"%s\".", + LaserScan::formatName(LaserScan::kXYZIT).c_str(), + LaserScan::formatName(LaserScan::kXYZIRT).c_str()); return LaserScan(); } @@ -3865,7 +3916,14 @@ LaserScan deskew( double stamp; UTimer processingTime; double scanTime = lastStamp - firstStamp; - cv::Mat output(1, input.size(), CV_32FC4); // XYZI - Dense + // Preserve ring when input carries it (kXYZIRT): the geometric channel is + // still meaningful after deskewing. Per-point time is zeroed because all + // points share the same pose after correction. + const bool preserveRing = input.hasRing(); + const int offsetRing = input.getRingOffset(); + const LaserScan::Format outputFormat = preserveRing ? LaserScan::kXYZIRT : LaserScan::kXYZI; + const int outputChannels = preserveRing ? 6 : 4; + cv::Mat output(1, input.size(), CV_32FC(outputChannels)); int offsetIntensity = input.getIntensityOffset(); bool isLocalTransformIdentity = input.localTransform().isIdentity(); Transform localTransformInv = input.localTransform().inverse(); @@ -3904,7 +3962,12 @@ LaserScan deskew( dataPtr[0] = pt.x; dataPtr[1] = pt.y; dataPtr[2] = pt.z; - dataPtr[3] = input.data().ptr(v, u)[offsetIntensity]; + dataPtr[3] = inputPtr[offsetIntensity]; + if(preserveRing) + { + dataPtr[4] = inputPtr[offsetRing]; + dataPtr[5] = 0.0f; + } } } } @@ -3941,14 +4004,19 @@ LaserScan deskew( dataPtr[0] = pt.x; dataPtr[1] = pt.y; dataPtr[2] = pt.z; - dataPtr[3] = input.data().ptr(v, u)[offsetIntensity]; + dataPtr[3] = inputPtr[offsetIntensity]; + if(preserveRing) + { + dataPtr[4] = inputPtr[offsetRing]; + dataPtr[5] = 0.0f; + } } } } } output = cv::Mat(output, cv::Range::all(), cv::Range(0, oi)); UDEBUG("Lidar deskewing time=%fs", processingTime.elapsed()); - return LaserScan(output, input.maxPoints(), input.rangeMax(), LaserScan::kXYZI, input.localTransform()); + return LaserScan(output, input.maxPoints(), input.rangeMax(), outputFormat, input.localTransform()); } diff --git a/corelib/src/util3d_features.cpp b/corelib/src/util3d_features.cpp index f6eb40f4..c2871be4 100644 --- a/corelib/src/util3d_features.cpp +++ b/corelib/src/util3d_features.cpp @@ -213,6 +213,7 @@ std::map generateWords3DMono( Transform & cameraTransform, float ransacReprojThreshold, float ransacConfidence, + int varianceMedianRatio, const std::map & refGuess3D, double * varianceOut, std::vector * matchesOut) @@ -345,7 +346,7 @@ std::map generateWords3DMono( errorSqrdDists[j] = uNormSquared(refPt.x-newPt.x, refPt.y-newPt.y, refPt.z-newPt.z); } std::sort(errorSqrdDists.begin(), errorSqrdDists.end()); - double median_error_sqr = (double)errorSqrdDists[errorSqrdDists.size () >> 2]; + double median_error_sqr = (double)errorSqrdDists[errorSqrdDists.size () >> varianceMedianRatio]; float var = 2.1981 * median_error_sqr; //UDEBUG("scale %d = %f variance = %f", (int)i, s, variance); @@ -369,7 +370,7 @@ std::map generateWords3DMono( errorSqrdDists[j] = uNormSquared(refPt.x-newPt.x, refPt.y-newPt.y, refPt.z-newPt.z); } std::sort(errorSqrdDists.begin(), errorSqrdDists.end()); - double median_error_sqr = (double)errorSqrdDists[errorSqrdDists.size () >> 2]; + double median_error_sqr = (double)errorSqrdDists[errorSqrdDists.size () >> varianceMedianRatio]; variance = 2.1981 * median_error_sqr; } } diff --git a/corelib/src/util3d_filtering.cpp b/corelib/src/util3d_filtering.cpp index 5fb1cad6..96e107d3 100644 --- a/corelib/src/util3d_filtering.cpp +++ b/corelib/src/util3d_filtering.cpp @@ -954,7 +954,8 @@ pcl::IndicesPtr cropBoxImpl( const Transform & transform, bool negative) { - UASSERT(min[0] < max[0] && min[1] < max[1] && min[2] < max[2]); + UASSERT_MSG(min[0] < max[0] && min[1] < max[1] && min[2] <= max[2], // z can be equal in 2D case + uFormat("x=%f->%f y=%f->%f z=%f->%f", min[0], max[0], min[1], max[1], min[2], max[2]).c_str()); pcl::IndicesPtr output(new std::vector); pcl::CropBox filter; @@ -973,7 +974,8 @@ pcl::IndicesPtr cropBoxImpl( pcl::IndicesPtr cropBox(const pcl::PCLPointCloud2::Ptr & cloud, const pcl::IndicesPtr & indices, const Eigen::Vector4f & min, const Eigen::Vector4f & max, const Transform & transform, bool negative) { - UASSERT(min[0] < max[0] && min[1] < max[1] && min[2] < max[2]); + UASSERT_MSG(min[0] < max[0] && min[1] < max[1] && min[2] <= max[2], // z can be equal in 2D case + uFormat("x=%f->%f y=%f->%f z=%f->%f", min[0], max[0], min[1], max[1], min[2], max[2]).c_str()); pcl::IndicesPtr output(new std::vector); pcl::CropBox filter; diff --git a/docker/noble/android/Dockerfile b/docker/noble/android/Dockerfile index 5f4404c5..2234b7c1 100644 --- a/docker/noble/android/Dockerfile +++ b/docker/noble/android/Dockerfile @@ -3,14 +3,17 @@ FROM ubuntu:24.04 # Install build dependencies -RUN apt-get update && apt-get install -y --no-install-recommends apt-utils +RUN apt-get update && apt-get install -y --no-install-recommends \ + apt-utils && apt-get clean && \ + rm -rf /var/lib/apt/lists/ RUN apt-get update && apt-get install -y \ git unzip wget ant cmake \ g++ lib32stdc++6 lib32z1 \ software-properties-common \ freeglut3-dev \ openjdk-8-jdk openjdk-8-jre \ - curl + curl && \ + apt-get clean && rm -rf /var/lib/apt/lists/ ENV ANDROID_HOME=/opt/android-sdk ENV PATH=$PATH:/opt/android-sdk/cmdline-tools/latest/bin:/opt/android-sdk/tools:/opt/android-sdk/platform-tools:/opt/android-sdk/ndk/21.4.7075529 @@ -53,6 +56,8 @@ RUN echo "Install boost..." && \ cd build && \ cmake -DCMAKE_TOOLCHAIN_FILE=$ANDROID_NDK/build/cmake/android.toolchain.cmake -DANDROID_ABI=arm64-v8a -DANDROID_NDK=$ANDROID_NDK -DANDROID_NATIVE_API_LEVEL=23 -DBUILD_SHARED_LIBS=OFF -DCMAKE_BUILD_TYPE=Release -DCMAKE_INSTALL_PREFIX=/opt/android/arm64-v8a -DCMAKE_FIND_ROOT_PATH="/opt/android/arm64-v8a/bin;/opt/android/arm64-v8a;/opt/android/arm64-v8a/share" .. && \ make -j4 && \ + find . -type f -name "*.a" -print0 \ + | xargs -0 $ANDROID_NDK/toolchains/llvm/prebuilt/linux-x86_64/bin/aarch64-linux-android-strip -g -S -d --strip-debug && \ make install && \ cd /root && \ rm -r boost_1_59_0.tar.gz boost_1_59_0 @@ -80,6 +85,8 @@ RUN echo "Install flann..." && \ cd build && \ cmake -DCMAKE_TOOLCHAIN_FILE=$ANDROID_NDK/build/cmake/android.toolchain.cmake -DANDROID_ABI=arm64-v8a -DANDROID_NDK=$ANDROID_NDK -DANDROID_NATIVE_API_LEVEL=23 -DBUILD_SHARED_LIBS=OFF -DBUILD_PYTHON_BINDINGS=OFF -DCMAKE_BUILD_TYPE=Release -DCMAKE_INSTALL_PREFIX=/opt/android/arm64-v8a -DCMAKE_FIND_ROOT_PATH="/opt/android/arm64-v8a/bin;/opt/android/arm64-v8a;/opt/android/arm64-v8a/share" .. && \ make -j4 && \ + find . -type f -name "*.a" -print0 \ + | xargs -0 $ANDROID_NDK/toolchains/llvm/prebuilt/linux-x86_64/bin/aarch64-linux-android-strip -g -S -d --strip-debug && \ make install && \ cd /root && \ rm -rf flann @@ -95,6 +102,8 @@ RUN echo "Install gtsam..." && \ cd build && \ cmake -DCMAKE_TOOLCHAIN_FILE=$ANDROID_NDK/build/cmake/android.toolchain.cmake -DANDROID_ABI=arm64-v8a -DANDROID_NDK=$ANDROID_NDK -DANDROID_NATIVE_API_LEVEL=23 -DBUILD_SHARED_LIBS=OFF -DCMAKE_BUILD_TYPE=Release -DCMAKE_INSTALL_PREFIX=/opt/android/arm64-v8a -DCMAKE_FIND_ROOT_PATH="/opt/android/arm64-v8a/bin;/opt/android/arm64-v8a;/opt/android/arm64-v8a/share" -DMETIS_SHARED=OFF -DGTSAM_BUILD_STATIC_LIBRARY=ON -DGTSAM_BUILD_TESTS=OFF -DGTSAM_BUILD_EXAMPLES_ALWAYS=OFF -DGTSAM_USE_SYSTEM_EIGEN=ON .. && \ make -j4 && \ + find . -type f -name "*.a" -print0 \ + | xargs -0 $ANDROID_NDK/toolchains/llvm/prebuilt/linux-x86_64/bin/aarch64-linux-android-strip -g -S -d --strip-debug && \ make install && \ cd /root && \ rm -rf gtsam @@ -108,6 +117,8 @@ RUN echo "Install g2o..." && \ cd build && \ cmake -DCMAKE_TOOLCHAIN_FILE=$ANDROID_NDK/build/cmake/android.toolchain.cmake -DANDROID_ABI=arm64-v8a -DANDROID_NDK=$ANDROID_NDK -DANDROID_NATIVE_API_LEVEL=23 -DBUILD_SHARED_LIBS=OFF -DCMAKE_BUILD_TYPE=Release -DCMAKE_INSTALL_PREFIX=/opt/android/arm64-v8a -DCMAKE_FIND_ROOT_PATH="/opt/android/arm64-v8a/bin;/opt/android/arm64-v8a;/opt/android/arm64-v8a/share" -DBUILD_LGPL_SHARED_LIBS=OFF -DG2O_BUILD_APPS=OFF -DG2O_BUILD_EXAMPLES=OFF -DG2O_USE_OPENGL=OFF .. && \ make -j4 && \ + find "../lib" -type f -name "*.a" -print0 \ + | xargs -0 $ANDROID_NDK/toolchains/llvm/prebuilt/linux-x86_64/bin/aarch64-linux-android-strip -g -S -d --strip-debug && \ make install && \ cd /root && \ rm -rf g2o @@ -117,12 +128,14 @@ RUN echo "Install VTK..." && \ git clone https://github.com/Kitware/VTK.git && \ cd VTK && \ git checkout tags/v8.2.0 && \ - wget https://gist.github.com/matlabbe/e217259fb8ece9ee6daf5a8f70e896a0/raw/2214b503a537d6431d764526b5b780f07d6f168d/vtk_8_2_0_android_r21_fix.patch && \ + wget https://gist.github.com/matlabbe/e217259fb8ece9ee6daf5a8f70e896a0/raw/36d879f0827289e75de969afb7b6515d9a1c71d7/vtk_8_2_0_android_r21_fix.patch && \ git apply vtk_8_2_0_android_r21_fix.patch && \ mkdir build && \ cd build && \ cmake -DBUILD_EXAMPLES=OFF -DBUILD_TESTING=OFF -DVTK_ANDROID_BUILD=ON -DBUILD_SHARED_LIBS=OFF -DCMAKE_BUILD_TYPE=Release -DCMAKE_INSTALL_PREFIX=/opt/android/arm64-v8a -DANDROID_ARCH_ABI=arm64-v8a -DCMAKE_FIND_ROOT_PATH="/opt/android/arm64-v8a/bin;/opt/android/arm64-v8a;/opt/android/arm64-v8a/share" .. && \ make -j4 && \ + find . -type f -name "*.a" -print0 \ + | xargs -0 $ANDROID_NDK/toolchains/llvm/prebuilt/linux-x86_64/bin/aarch64-linux-android-strip -g -S -d --strip-debug && \ cp -r CMakeExternals/Install/vtk-android/* /opt/android/arm64-v8a/. && \ cd /root && \ rm -rf VTK @@ -140,6 +153,8 @@ RUN echo "Install pcl..." && \ cmake -DCMAKE_TOOLCHAIN_FILE=$ANDROID_NDK/build/cmake/android.toolchain.cmake -DANDROID_ABI=arm64-v8a -DANDROID_NDK=$ANDROID_NDK -DANDROID_NATIVE_API_LEVEL=23 -DBUILD_SHARED_LIBS=OFF -DCMAKE_BUILD_TYPE=Release -DCMAKE_INSTALL_PREFIX=/opt/android/arm64-v8a -DCMAKE_FIND_ROOT_PATH="/opt/android/arm64-v8a/bin;/opt/android/arm64-v8a;/opt/android/arm64-v8a/share" -DBUILD_apps=OFF -DBUILD_examples=OFF -DBUILD_tools=OFF -DBUILD_visualization=OFF -DBUILD_tracking=OFF -DBUILD_people=OFF -DBUILD_tools=OFF -DBUILD_global_tests=OFF -DWITH_QT=OFF -DWITH_OPENGL=OFF -DWITH_VTK=ON -DPCL_SHARED_LIBS=OFF .. || true && \ cmake -DCMAKE_TOOLCHAIN_FILE=$ANDROID_NDK/build/cmake/android.toolchain.cmake -DANDROID_ABI=arm64-v8a -DANDROID_NDK=$ANDROID_NDK -DANDROID_NATIVE_API_LEVEL=23 -DBUILD_SHARED_LIBS=OFF -DCMAKE_BUILD_TYPE=Release -DCMAKE_INSTALL_PREFIX=/opt/android/arm64-v8a -DCMAKE_FIND_ROOT_PATH="/opt/android/arm64-v8a/bin;/opt/android/arm64-v8a;/opt/android/arm64-v8a/share" -DBUILD_apps=OFF -DBUILD_examples=OFF -DBUILD_tools=OFF -DBUILD_visualization=OFF -DBUILD_tracking=OFF -DBUILD_people=OFF -DBUILD_tools=OFF -DBUILD_global_tests=OFF -DWITH_QT=OFF -DWITH_OPENGL=OFF -DWITH_VTK=ON -DPCL_SHARED_LIBS=OFF .. && \ make -j4 && \ + find . -type f -name "*.a" -print0 \ + | xargs -0 $ANDROID_NDK/toolchains/llvm/prebuilt/linux-x86_64/bin/aarch64-linux-android-strip -g -S -d --strip-debug && \ make install && \ cd /root && \ rm -rf pcl @@ -161,14 +176,12 @@ RUN echo "Install OpenCV..." && \ cd build && \ cmake -DCMAKE_TOOLCHAIN_FILE=$ANDROID_NDK/build/cmake/android.toolchain.cmake -DANDROID_ABI=arm64-v8a -DANDROID_NDK=$ANDROID_NDK -DANDROID_NATIVE_API_LEVEL=23 -DBUILD_SHARED_LIBS=OFF -DCMAKE_BUILD_TYPE=Release -DCMAKE_INSTALL_PREFIX=/opt/android/arm64-v8a -DCMAKE_FIND_ROOT_PATH="/opt/android/arm64-v8a/bin;/opt/android/arm64-v8a;/opt/android/arm64-v8a/share" -DOPENCV_EXTRA_MODULES_PATH=/root/opencv_contrib/modules -DBUILD_TESTS=OFF -DBUILD_PERF_TESTS=OFF -DWITH_CUDA=OFF -DBUILD_opencv_structured_light=OFF -DBUILD_ANDROID_PROJECTS=OFF -DOPENCV_ENABLE_NONFREE=ON -DBUILD_ANDROID_EXAMPLES=OFF -DWITH_PROTOBUF=OFF -DBUILD_opencv_stereo=OFF -DBUILD_JAVA=OFF -DWITH_QUIRC=OFF -DBUILD_opencv_js_bindings_generator=OFF -DBUILD_opencv_objc_bindings_generator=OFF -DBUILD_opencv_objdetect=OFF -DBUILD_opencv_xobjdetect=OFF .. && \ make -j4 && \ + find . -type f -name "*.a" -print0 \ + | xargs -0 $ANDROID_NDK/toolchains/llvm/prebuilt/linux-x86_64/bin/aarch64-linux-android-strip -g -S -d --strip-debug && \ make install && \ cd /root && \ rm -rf opencv opencv_contrib -RUN echo "Strip libraries..." && \ - $ANDROID_NDK/toolchains/llvm/prebuilt/linux-x86_64/bin/aarch64-linux-android-strip -g -S -d --strip-debug --verbose /opt/android/arm64-v8a/lib/*.a && \ - $ANDROID_NDK/toolchains/llvm/prebuilt/linux-x86_64/bin/aarch64-linux-android-strip -g -S -d --strip-debug --verbose /opt/android/arm64-v8a/sdk/native/staticlibs/arm64-v8a/*.a - RUN mkdir /opt/android/lib # tango diff --git a/docker/noble/android/rtabmap_apiXX/rtabmap.bash b/docker/noble/android/rtabmap_apiXX/rtabmap.bash index 67154dfe..2d1bf32d 100755 --- a/docker/noble/android/rtabmap_apiXX/rtabmap.bash +++ b/docker/noble/android/rtabmap_apiXX/rtabmap.bash @@ -21,7 +21,7 @@ make # rtabmap mkdir arm64-v8a cd arm64-v8a -cmake -DCMAKE_TOOLCHAIN_FILE=$ANDROID_NDK/build/cmake/android.toolchain.cmake -DANDROID_ABI=arm64-v8a -DANDROID_NDK=$ANDROID_NDK -DANDROID_NATIVE_API_LEVEL=$api -DBUILD_SHARED_LIBS=OFF -DCMAKE_BUILD_TYPE=Release -DCMAKE_INSTALL_PREFIX=$prefix/arm64-v8a -DCMAKE_FIND_ROOT_PATH="$prefix/arm64-v8a/bin;$prefix/arm64-v8a;$prefix/arm64-v8a/share" -DBUILD_EXAMPLES=OFF -DBUILD_TOOLS=OFF -DOpenCV_DIR=$prefix/arm64-v8a/sdk/native/jni ../.. +cmake -DCMAKE_TOOLCHAIN_FILE=$ANDROID_NDK/build/cmake/android.toolchain.cmake -DANDROID_ABI=arm64-v8a -DANDROID_NDK=$ANDROID_NDK -DANDROID_NATIVE_API_LEVEL=$api -DBUILD_SHARED_LIBS=OFF -DCMAKE_BUILD_TYPE=Release -DCMAKE_INSTALL_PREFIX=$prefix/arm64-v8a -DCMAKE_FIND_ROOT_PATH="$prefix/arm64-v8a/bin;$prefix/arm64-v8a;$prefix/arm64-v8a/share" -DBUILD_EXAMPLES=OFF -DBUILD_TOOLS=OFF -DWITH_OPENGV=OFF -DOpenCV_DIR=$prefix/arm64-v8a/sdk/native/jni ../.. make make clean diff --git a/docker/resolute/Dockerfile b/docker/resolute/Dockerfile new file mode 100644 index 00000000..8e043f61 --- /dev/null +++ b/docker/resolute/Dockerfile @@ -0,0 +1,20 @@ +# Image: introlab3it/rtabmap:resolute + +FROM introlab3it/rtabmap:resolute-deps + +# 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 -j4 && \ + make install && \ + cd ../.. && \ + rm -rf rtabmap && \ + ldconfig + diff --git a/docker/resolute/deps/Dockerfile b/docker/resolute/deps/Dockerfile new file mode 100644 index 00000000..3e2986c2 --- /dev/null +++ b/docker/resolute/deps/Dockerfile @@ -0,0 +1,121 @@ + +# Image: introlab3it/rtabmap:resolute-deps + +FROM ubuntu:26.04 + +ARG TARGETPLATFORM +ENV TARGETPLATFORM=${TARGETPLATFORM:-linux/amd64} +RUN echo "I am building for $TARGETPLATFORM" + +ENV DEBIAN_FRONTEND=noninteractive + +# 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/ + +# Install build dependencies +RUN apt-get update && \ + apt upgrade -y && \ + apt-get install -y \ + git \ + wget \ + libtbb-dev \ + libproj-dev \ + libpcl-dev \ + liboctomap-dev \ + libfreenect-dev \ + libceres-dev \ + ros-lyrical-ros-base \ + ros-dev-tools \ + ros-lyrical-cv-bridge \ + ros-lyrical-image-geometry \ + ros-lyrical-laser-geometry \ + ros-lyrical-pcl-conversions \ + ros-lyrical-rviz-common \ + ros-lyrical-rviz-rendering \ + ros-lyrical-rviz-default-plugins \ + ros-lyrical-pcl-ros \ + ros-lyrical-imu-filter-madgwick \ + ros-lyrical-image-transport \ + ros-lyrical-octomap-msgs \ + ros-lyrical-libg2o \ + ros-lyrical-gtsam \ + ros-lyrical-qt-gui-cpp \ + ros-lyrical-diagnostic-updater && \ + apt-get clean && rm -rf /var/lib/apt/lists/ + +WORKDIR /root/ + +# 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 -DCMAKE_BUILD_TYPE=Release -DCMAKE_POLICY_VERSION_MINIMUM=3.5 .. && \ + make -j4 && \ + 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 -DCMAKE_BUILD_TYPE=Release -DCMAKE_POLICY_VERSION_MINIMUM=3.5 .. && \ + make -j4 && \ + make install && \ + cd && \ + rm -r zed-open-capture; fi + +# OpenCV with all modules (same version than distro version to avoid conflicts with cv_bridge ros package) +RUN git clone --branch 4.10.0 https://github.com/opencv/opencv.git && \ + git clone --branch 4.10.0 https://github.com/opencv/opencv_contrib.git && \ + cd opencv && \ + # FFmpeg 7/8 compatibility (Ubuntu 26.04): avcodec_close / av_stream_get_side_data removed + git -c user.email=docker@build -c user.name=docker cherry-pick -x 90c444abd387ffa70b2e72a34922903a2f0f4f5a 443d0ae63fad6dfd8c485d609203db16c8bd0ec3 && \ + mkdir build && \ + cd build && \ + cmake -DCMAKE_BUILD_TYPE=Release -DWITH_TBB=ON -DWITH_ADE=OFF -DWITH_OPENMP=ON -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 -j4 && \ + make install && \ + cd ../.. && \ + rm -rf opencv opencv_contrib + +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 -DBUILD_TESTS=OFF -DCMAKE_POLICY_VERSION_MINIMUM=3.5 .. && \ + make -j4 && \ + make install && \ + cd && \ + rm -r opengv + +RUN rm /bin/sh && ln -s /bin/bash /bin/sh + +RUN echo -e '#!/bin/bash\nset -e\n\n# setup ros2 environment\nsource "/opt/ros/lyrical/setup.bash" --\nexec "$@"' > /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/lyrical/lib/x86_64-linux-gnu:/opt/ros/lyrical/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/examples/RGBDMapping/main.cpp b/examples/RGBDMapping/main.cpp index 5f2b75fd..94545e1e 100644 --- a/examples/RGBDMapping/main.cpp +++ b/examples/RGBDMapping/main.cpp @@ -50,7 +50,24 @@ void showUsage() { printf("\nUsage:\n" "rtabmap-rgbd_mapping driver\n" - " driver Driver number to use: 0=OpenNI-PCL, 1=OpenNI2, 2=Freenect, 3=OpenNI-CV, 4=OpenNI-CV-ASUS, 5=Freenect2, 6=ZED SDK, 7=RealSense, 8=RealSense2 9=Kinect for Azure SDK 10=MYNT EYE S\n\n"); + " driver Driver number to use: 0=OpenNI-PCL (Kinect)\n" + " 1=OpenNI2 (Kinect and Xtion PRO Live)\n" + " 2=Freenect (Kinect)\n" + " 3=OpenNI-CV (Kinect)\n" + " 4=OpenNI-CV-ASUS (Xtion PRO Live)\n" + " 5=Freenect2 (Kinect v2)\n" + " 6=DC1394 (Bumblebee2)\n" + " 7=FlyCapture2 (Bumblebee2)\n" + " 8=ZED stereo\n" + " 9=RealSense\n" + " 10=Kinect for Windows 2 SDK\n" + " 11=RealSense2\n" + " 12=Kinect for Azure SDK\n" + " 13=MYNT EYE S\n" + " 14=ZED Open Capture\n" + " 15=depthai-core\n" + " 16=XVSDK (SeerSense)\n" + " 17=Orbbec SDK\n\n"); exit(1); } @@ -58,7 +75,7 @@ using namespace rtabmap; int main(int argc, char * argv[]) { ULogger::setType(ULogger::kTypeConsole); - ULogger::setLevel(ULogger::kWarning); + ULogger::setLevel(ULogger::kInfo); #ifdef RTABMAP_PYTHON PythonInterface python; // Make sure we initialize python in main thread @@ -72,92 +89,120 @@ int main(int argc, char * argv[]) else { driver = atoi(argv[argc-1]); - if(driver < 0 || driver > 10) + if(driver < 0 || driver > 17) { - UERROR("driver should be between 0 and 10."); + UERROR("driver should be between 0 and 17."); showUsage(); } } // Here is the pipeline that we will use: // CameraOpenni -> "SensorEvent" -> OdometryThread -> "OdometryEvent" -> RtabmapThread -> "RtabmapEvent" - - // Create the OpenNI camera, it will send a SensorEvent at the rate specified. - // Set transform to camera so z is up, y is left and x going forward Camera * camera = 0; - if(driver == 1) + if (driver == 0) { - if(!CameraOpenNI2::available()) + camera = new rtabmap::CameraOpenni(); + } + else if (driver == 1) + { + if (!rtabmap::CameraOpenNI2::available()) { UERROR("Not built with OpenNI2 support..."); exit(-1); } - camera = new CameraOpenNI2(); + camera = new rtabmap::CameraOpenNI2(); } - else if(driver == 2) + else if (driver == 2) { - if(!CameraFreenect::available()) + if (!rtabmap::CameraFreenect::available()) { UERROR("Not built with Freenect support..."); exit(-1); } - camera = new CameraFreenect(); + camera = new rtabmap::CameraFreenect(); } - else if(driver == 3) + else if (driver == 3) { - if(!CameraOpenNICV::available()) + if (!rtabmap::CameraOpenNICV::available()) { UERROR("Not built with OpenNI from OpenCV support..."); exit(-1); } - camera = new CameraOpenNICV(); + camera = new rtabmap::CameraOpenNICV(false); } - else if(driver == 4) + else if (driver == 4) { - if(!CameraOpenNICV::available()) + if (!rtabmap::CameraOpenNICV::available()) { UERROR("Not built with OpenNI from OpenCV support..."); exit(-1); } - camera = new CameraOpenNICV(true); + camera = new rtabmap::CameraOpenNICV(true); } else if (driver == 5) { - if (!CameraFreenect2::available()) + if (!rtabmap::CameraFreenect2::available()) { UERROR("Not built with Freenect2 support..."); exit(-1); } - camera = new CameraFreenect2(0, CameraFreenect2::kTypeColor2DepthSD); + camera = new rtabmap::CameraFreenect2(0, rtabmap::CameraFreenect2::kTypeColor2DepthSD); } else if (driver == 6) { - if (!CameraStereoZed::available()) + if (!rtabmap::CameraStereoDC1394::available()) { - UERROR("Not built with ZED SDK support..."); + UERROR("Not built with DC1394 support..."); exit(-1); } - camera = new CameraStereoZed(0, -1, 1, 1, 100, false); + camera = new rtabmap::CameraStereoDC1394(); } else if (driver == 7) { - if (!CameraRealSense::available()) + if (!rtabmap::CameraStereoFlyCapture2::available()) + { + UERROR("Not built with FlyCapture2/Triclops support..."); + exit(-1); + } + camera = new rtabmap::CameraStereoFlyCapture2(); + } + else if (driver == 8) + { + if (!rtabmap::CameraStereoZed::available()) + { + UERROR("Not built with ZED sdk support..."); + exit(-1); + } + camera = new rtabmap::CameraStereoZed(0); + } + else if (driver == 9) + { + if (!rtabmap::CameraRealSense::available()) { UERROR("Not built with RealSense support..."); exit(-1); } - camera = new CameraRealSense(); + camera = new rtabmap::CameraRealSense(); } - else if (driver == 8) + else if (driver == 10) { - if (!CameraRealSense2::available()) + if (!rtabmap::CameraK4W2::available()) { - UERROR("Not built with RealSense2 support..."); + UERROR("Not built with Kinect for Windows 2 SDK support..."); exit(-1); } - camera = new CameraRealSense2(); + camera = new rtabmap::CameraK4W2(); } - else if (driver == 9) + else if (driver == 11) + { + if (!rtabmap::CameraRealSense2::available()) + { + UERROR("Not built with RealSense2 SDK support..."); + exit(-1); + } + camera = new rtabmap::CameraRealSense2(); + } + else if (driver == 12) { if (!rtabmap::CameraK4A::available()) { @@ -166,7 +211,7 @@ int main(int argc, char * argv[]) } camera = new rtabmap::CameraK4A(1); } - else if (driver == 10) + else if (driver == 13) { if (!rtabmap::CameraMyntEye::available()) { @@ -175,9 +220,45 @@ int main(int argc, char * argv[]) } camera = new rtabmap::CameraMyntEye(); } + else if (driver == 14) + { + if (!rtabmap::CameraStereoZedOC::available()) + { + UERROR("Not built with Zed Open Capture support..."); + exit(-1); + } + camera = new rtabmap::CameraStereoZedOC(0); + } + else if (driver == 15) + { + if (!rtabmap::CameraDepthAI::available()) + { + UERROR("Not built with depthai-core support..."); + exit(-1); + } + camera = new rtabmap::CameraDepthAI(); + } + else if (driver == 16) + { + if (!rtabmap::CameraSeerSense::available()) + { + UERROR("Not built with XVisio SDK support..."); + exit(-1); + } + camera = new rtabmap::CameraSeerSense(); + } + else if (driver == 17) + { + if (!rtabmap::CameraOrbbecSDK::available()) + { + UERROR("Not built with Orbbec SDK support..."); + exit(-1); + } + camera = new rtabmap::CameraOrbbecSDK(); + } else { - camera = new rtabmap::CameraOpenni(); + UFATAL(""); } if(!camera->init()) diff --git a/examples/WifiMapping/WifiThread.h b/examples/WifiMapping/WifiThread.h index 7e3d14c6..27da516f 100644 --- a/examples/WifiMapping/WifiThread.h +++ b/examples/WifiMapping/WifiThread.h @@ -177,7 +177,8 @@ private: struct iwreq req; struct iw_statistics stats; - strncpy(req.ifr_name, interfaceName_.c_str(), IFNAMSIZ); + strncpy(req.ifr_name, interfaceName_.c_str(), IFNAMSIZ - 1); + req.ifr_name[IFNAMSIZ - 1] = '\0'; //make room for the iw_statistics object req.u.data.pointer = (caddr_t) &stats; diff --git a/guilib/include/rtabmap/gui/CameraViewer.h b/guilib/include/rtabmap/gui/CameraViewer.h index 60aa212a..9e6a0c5b 100644 --- a/guilib/include/rtabmap/gui/CameraViewer.h +++ b/guilib/include/rtabmap/gui/CameraViewer.h @@ -32,6 +32,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include +#include #include #include @@ -73,6 +74,10 @@ private: QCheckBox * showScanCheckbox_; QCheckBox * markerCheckbox_; MarkerDetector * markerDetector_; + QElapsedTimer fpsTimer_; + double lastCapturePeriod_; + double previousCaptureStamp_; + std::map _landmarksSize; }; } /* namespace rtabmap */ diff --git a/guilib/include/rtabmap/gui/CloudViewer.h b/guilib/include/rtabmap/gui/CloudViewer.h index eb16c4b1..996619d5 100644 --- a/guilib/include/rtabmap/gui/CloudViewer.h +++ b/guilib/include/rtabmap/gui/CloudViewer.h @@ -231,7 +231,8 @@ public: const Transform & to, const QColor & color, bool arrow = false, - bool foreground = false); + bool foreground = false, + double width = 1.0); void removeLine(const std::string & id); void removeAllLines(); const std::set & getAddedLines() const {return _lines;} diff --git a/guilib/include/rtabmap/gui/DatabaseViewer.h b/guilib/include/rtabmap/gui/DatabaseViewer.h index a11b4c93..07f94943 100644 --- a/guilib/include/rtabmap/gui/DatabaseViewer.h +++ b/guilib/include/rtabmap/gui/DatabaseViewer.h @@ -220,6 +220,7 @@ private: std::map mapIds_; std::map weights_; std::map > wmStates_; + std::map envSensors_; QMap idToIndex_; QList neighborLinks_; QList loopLinks_; diff --git a/guilib/include/rtabmap/gui/GraphViewer.h b/guilib/include/rtabmap/gui/GraphViewer.h index 8df08885..8cf9d68a 100644 --- a/guilib/include/rtabmap/gui/GraphViewer.h +++ b/guilib/include/rtabmap/gui/GraphViewer.h @@ -77,6 +77,7 @@ public: // Use updateNodeColorByValue() instead with valueName="Posterior". RTABMAP_DEPRECATED void updatePosterior(const std::map & posterior, float fixedMax = 0.0f, int zValueOffset = 0); void updateNodeColorByValue(const std::string & valueName, const std::map & values, float fixedMax = 0.0f, bool invertedColorScale = false, int zValueOffset = 0); + void updateNodeColorByValue(const std::string & valueName, const std::map & values, float fixedMin, float fixedMax, bool invertedColorScale = false, unsigned short hueMin=0, unsigned short hueMax=180, int zValueOffset = 0); void updateLocalPath(const std::vector & localPath); void setGlobalPath(const std::vector > & globalPath); void setCurrentGoalID(int id, const Transform & pose = Transform()); @@ -118,8 +119,9 @@ public: bool isReferentialVisible() const; bool isLocalRadiusVisible() const; float getLoopClosureOutlierThr() const {return _loopClosureOutlierThr;} - float getMaxLinkLength() const {return _maxLinkLength;} + float getMinLinkLength() const {return _minLinkLength;} bool isGraphVisible() const; + bool isNodeVisible() const; bool isGlobalPathVisible() const; bool isLocalPathVisible() const; bool isGtGraphVisible() const; @@ -158,7 +160,7 @@ public: void setReferentialVisible(bool visible); void setLocalRadiusVisible(bool visible); void setLoopClosureOutlierThr(float value); - void setMaxLinkLength(float value); + void setMinLinkLength(float value); void setGraphVisible(bool visible); void setGlobalPathVisible(bool visible); void setLocalPathVisible(bool visible); @@ -184,6 +186,9 @@ protected: virtual void mousePressEvent(QMouseEvent * event); virtual void contextMenuEvent(QContextMenuEvent * event); +private: + void setupGraphicsScene(); + private: QString _workingDirectory; QColor _nodeColor; @@ -237,7 +242,7 @@ private: QGraphicsEllipseItem * _localRadius; QGraphicsRectItem * _odomCacheOverlay; float _loopClosureOutlierThr; - float _maxLinkLength; + float _minLinkLength; bool _orientationENU; bool _mouseTracking; ViewPlane _viewPlane; diff --git a/guilib/include/rtabmap/gui/MainWindow.h b/guilib/include/rtabmap/gui/MainWindow.h index c0badb39..24db8941 100644 --- a/guilib/include/rtabmap/gui/MainWindow.h +++ b/guilib/include/rtabmap/gui/MainWindow.h @@ -178,6 +178,7 @@ protected Q_SLOTS: void selectFreenect2(); void selectK4W2(); void selectK4A(); + void selectOrbbecSDK(); void selectRealSense(); void selectRealSense2(); void selectRealSense2L515(); @@ -329,6 +330,7 @@ protected: int iterations, bool interSession, bool intraSession, + int minGraphDistance, // SBA params: bool sba, int sbaIterations, diff --git a/guilib/include/rtabmap/gui/MapVisibilityWidget.h b/guilib/include/rtabmap/gui/MapVisibilityWidget.h index e4675a35..55abc90f 100644 --- a/guilib/include/rtabmap/gui/MapVisibilityWidget.h +++ b/guilib/include/rtabmap/gui/MapVisibilityWidget.h @@ -44,6 +44,7 @@ public: void setMap(const std::map & poses, const std::map & mask); std::map getVisiblePoses() const; + bool isEmpty() const {return _poses.empty();} void clear(); diff --git a/guilib/include/rtabmap/gui/PostProcessingDialog.h b/guilib/include/rtabmap/gui/PostProcessingDialog.h index b0b2bfe8..c9310a82 100644 --- a/guilib/include/rtabmap/gui/PostProcessingDialog.h +++ b/guilib/include/rtabmap/gui/PostProcessingDialog.h @@ -59,6 +59,7 @@ public: int iterations() const; bool intraSession() const; bool interSession() const; + int minGraphDistance() const; bool isRefineNeighborLinks() const; bool isRefineLoopClosureLinks() const; bool isSBA() const; @@ -74,6 +75,7 @@ public: void setIterations(int iterations); void setIntraSession(bool enabled); void setInterSession(bool enabled); + void setMinGraphDistance(int value); void setRefineNeighborLinks(bool on); void setRefineLoopClosureLinks(bool on); void setSBA(bool on); diff --git a/guilib/include/rtabmap/gui/PreferencesDialog.h b/guilib/include/rtabmap/gui/PreferencesDialog.h index df34b25a..5fef4cba 100644 --- a/guilib/include/rtabmap/gui/PreferencesDialog.h +++ b/guilib/include/rtabmap/gui/PreferencesDialog.h @@ -98,6 +98,7 @@ public: kSrcRealSense2 = 9, kSrcK4A = 10, kSrcSeerSense = 11, + kSrcOrbbecSDK = 12, kSrcStereo = 100, kSrcDC1394 = 100, @@ -173,6 +174,7 @@ public: int getOdomRegistrationApproach() const; double getOdomF2MGravitySigma() const; bool isOdomDisabled() const; + bool isOdomAsGuessEnabled() const; bool isOdomSensorAsGt() const; bool isGroundTruthAligned() const; @@ -291,6 +293,7 @@ public: double getSourceScanForceGroundNormalsUp() const; Transform getSourceLocalTransform() const; //Openni group Transform getLaserLocalTransform() const; // directory images + Transform getGroundTruthLocalTransform() const; // directory images Transform getIMULocalTransform() const; // directory images QString getIMUPath() const; int getIMURate() const; @@ -361,17 +364,22 @@ private Q_SLOTS: void updateStereoDisparityVisibility(); void updateFeatureMatchingVisibility(); void updateGlobalDescriptorVisibility(); + void updateAvailableMarkerDictionaries(); void updateOdometryStackedIndex(int index); void useOdomFeatures(); void changeWorkingDirectory(); void changeDictionaryPath(); void changeOdometryORBSLAMVocabulary(); void changeOdometryOKVISConfigPath(); - void changeOdometryVINSConfigPath(); + void changeOdometryVINSFusionConfigPath(); + void changeOdometryOpenVINSConfigPath(); + void changeOdometryLIOSAMConfigPath(); void changeOdometryOpenVINSLeftMask(); void changeOdometryOpenVINSRightMask(); void changeIcpPMConfigPath(); void changeSuperPointModelPath(); + void changeSuperPointRpautratWeightsPath(); + void changeSuperPointRpautratModelPath(); void changePyMatcherPath(); void changePyMatcherModel(); void changePyDescriptorPath(); diff --git a/guilib/include/rtabmap/gui/StatsToolBox.h b/guilib/include/rtabmap/gui/StatsToolBox.h index b08b7cdf..c230d273 100644 --- a/guilib/include/rtabmap/gui/StatsToolBox.h +++ b/guilib/include/rtabmap/gui/StatsToolBox.h @@ -32,6 +32,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include +#include class QToolButton; class QLabel; @@ -59,6 +60,7 @@ public: public Q_SLOTS: void updateMenu(const QMenu * menu); + void updateLabel(); Q_SIGNALS: void valueAdded(qreal); @@ -117,6 +119,8 @@ Q_SIGNALS: private Q_SLOTS: void plot(const StatItem * stat, const QString & plotName = QString()); void figureDeleted(QObject * obj); + void requestLabelsUpdate(); + void updateLabels(); protected: virtual void contextMenuEvent(QContextMenuEvent * event); @@ -127,6 +131,7 @@ private: QString _workingDirectory; int _newFigureMaxItems; QMap _figures; + QTimer _updateLabelsTimer; }; } diff --git a/guilib/src/3rdParty/QMultiComboBox.cpp b/guilib/src/3rdParty/QMultiComboBox.cpp index 64e19c68..f3df7939 100644 --- a/guilib/src/3rdParty/QMultiComboBox.cpp +++ b/guilib/src/3rdParty/QMultiComboBox.cpp @@ -40,7 +40,7 @@ QMultiComboBox::QMultiComboBox(QWidget *widget ) : QMultiComboBox::~QMultiComboBox() { - disconnect(&vlist_,0,0,0); + vlist_.disconnect(SIGNAL(itemChanged(QListWidgetItem*))); } diff --git a/guilib/src/AboutDialog.cpp b/guilib/src/AboutDialog.cpp index 69a34000..5d130797 100644 --- a/guilib/src/AboutDialog.cpp +++ b/guilib/src/AboutDialog.cpp @@ -86,6 +86,13 @@ AboutDialog::AboutDialog(QWidget * parent) : _ui->label_sptorch->setText("No"); _ui->label_sptorch_license->setEnabled(false); #endif +#if defined(RTABMAP_TORCH) && defined(RTABMAP_PYTHON) + _ui->label_sprpautrat->setText("Yes"); + _ui->label_sprpautrat_license->setEnabled(true); +#else + _ui->label_sprpautrat->setText("No"); + _ui->label_sprpautrat_license->setEnabled(false); +#endif #ifdef RTABMAP_PYTHON _ui->label_pymatcher->setText("Yes"); _ui->label_pymatcher_license->setEnabled(true); @@ -100,6 +107,13 @@ AboutDialog::AboutDialog(QWidget * parent) : _ui->label_fastcv->setText("No"); _ui->label_fastcv_license->setEnabled(false); #endif +#ifdef RTABMAP_APRILTAG + _ui->label_apriltag->setText("Yes"); + _ui->label_apriltag_license->setEnabled(true); +#else + _ui->label_apriltag->setText("No"); + _ui->label_apriltag_license->setEnabled(false); +#endif #ifdef RTABMAP_PDAL _ui->label_pdal->setText("Yes"); _ui->label_pdal_license->setEnabled(true); @@ -114,6 +128,13 @@ AboutDialog::AboutDialog(QWidget * parent) : _ui->label_liblas->setText("No"); _ui->label_liblas_license->setEnabled(false); #endif +#ifdef RTABMAP_OPENGV + _ui->label_opengv->setText("Yes"); + _ui->label_opengv_license->setEnabled(true); +#else + _ui->label_opengv->setText("No"); + _ui->label_opengv_license->setEnabled(false); +#endif #ifdef RTABMAP_CUDASIFT _ui->label_cudasift->setText("Yes"); _ui->label_cudasift_license->setEnabled(true); @@ -179,6 +200,8 @@ AboutDialog::AboutDialog(QWidget * parent) : _ui->label_depthai->setText(CameraDepthAI::available() ? "Yes" : "No"); _ui->label_depthai_license->setEnabled(CameraDepthAI::available()); _ui->label_xvsdk->setText(CameraSeerSense::available() ? "Yes" : "No"); + _ui->label_orbbec_sdk->setText(CameraOrbbecSDK::available() ? "Yes" : "No"); + _ui->label_orbbec_sdk_license->setEnabled(CameraOrbbecSDK::available()); _ui->label_toro->setText(Optimizer::isAvailable(Optimizer::kTypeTORO)?"Yes":"No"); _ui->label_toro_license->setEnabled(Optimizer::isAvailable(Optimizer::kTypeTORO)?true:false); @@ -281,7 +304,7 @@ AboutDialog::AboutDialog(QWidget * parent) : _ui->label_msckf_license->setEnabled(false); #endif -#ifdef RTABMAP_VINS +#ifdef RTABMAP_VINS_FUSION _ui->label_vins_fusion->setText("Yes"); _ui->label_vins_fusion_license->setEnabled(true); #else @@ -297,6 +320,14 @@ AboutDialog::AboutDialog(QWidget * parent) : _ui->label_openvins_license->setEnabled(false); #endif +#ifdef RTABMAP_CUVSLAM + _ui->label_cuvslam->setText("Yes"); + _ui->label_cuvslam_license->setEnabled(true); +#else + _ui->label_cuvslam->setText("No"); + _ui->label_cuvslam_license->setEnabled(false); +#endif + } AboutDialog::~AboutDialog() diff --git a/guilib/src/CMakeLists.txt b/guilib/src/CMakeLists.txt index 8eb53433..c590077a 100644 --- a/guilib/src/CMakeLists.txt +++ b/guilib/src/CMakeLists.txt @@ -97,11 +97,7 @@ IF(MSVC) SET(SRC_FILES ${SRC_FILES} ${HEADERS}) ENDIF(MSVC) -SET(INCLUDE_DIRS - ${CMAKE_CURRENT_SOURCE_DIR}/../include - ${CMAKE_CURRENT_SOURCE_DIR} - ${CMAKE_CURRENT_BINARY_DIR} # for qt ui generated in binary dir -) +SET(INCLUDE_DIRS "") IF(QT4_FOUND) INCLUDE(${QT_USE_FILE}) @@ -172,9 +168,6 @@ IF(VTK_USE_QVTK) SET(LIBRARIES ${LIBRARIES} ${QVTK_LIBRARY}) ENDIF(VTK_USE_QVTK) -#include files -INCLUDE_DIRECTORIES(${INCLUDE_DIRS}) - add_definitions(${PCL_DEFINITIONS}) # Include presets @@ -205,8 +198,12 @@ generate_export_header(rtabmap_gui DEPRECATED_MACRO_NAME RTABMAP_DEPRECATED) target_include_directories(rtabmap_gui PUBLIC - "$" - "$") + "$" + "$") + +target_include_directories(rtabmap_gui SYSTEM PUBLIC + "$" + "$") TARGET_LINK_LIBRARIES(rtabmap_gui PUBLIC diff --git a/guilib/src/CalibrationDialog.cpp b/guilib/src/CalibrationDialog.cpp index dd4a0523..f6b27fe7 100644 --- a/guilib/src/CalibrationDialog.cpp +++ b/guilib/src/CalibrationDialog.cpp @@ -250,17 +250,18 @@ void CalibrationDialog::resetSettings() cv::Mat drawChessboard(int squareSize, int boardWidth, int boardHeight, int borderSize) { - int imageWidth = squareSize*boardWidth + borderSize; - int imageHeight = squareSize*boardHeight + borderSize; - cv::Mat chessboard(imageWidth, imageHeight, CV_8UC1, 255); - unsigned char color = 0; + int imageWidth = squareSize*(boardWidth+1) + 2*borderSize; + int imageHeight = squareSize*(boardHeight+1) + 2*borderSize; + cv::Mat chessboard(imageHeight, imageWidth, CV_8UC1, 255); + unsigned char rowColor = 0; for(int i=borderSize;i #include #include +#include #include #include #include @@ -54,7 +55,9 @@ CameraViewer::CameraViewer(QWidget * parent, const ParametersMap & parameters) : cloudView_(new CloudViewer(this)), processingImages_(false), parameters_(parameters), - markerDetector_(0) + markerDetector_(0), + lastCapturePeriod_(0.0), + previousCaptureStamp_(0.0) { qRegisterMetaType("rtabmap::SensorData"); @@ -82,7 +85,7 @@ CameraViewer::CameraViewer(QWidget * parent, const ParametersMap & parameters) : showScanCheckbox_->setChecked(true); markerCheckbox_ = new QCheckBox("Detect markers", this); -#ifdef HAVE_OPENCV_ARUCO +#if defined(HAVE_OPENCV_ARUCO) || defined(RTABMAP_APRILTAG) markerCheckbox_->setEnabled(true); markerDetector_ = new MarkerDetector(parameters); #else @@ -115,6 +118,7 @@ CameraViewer::CameraViewer(QWidget * parent, const ParametersMap & parameters) : vlayout->addLayout(layout2); this->setLayout(vlayout); + fpsTimer_.start(); } CameraViewer::~CameraViewer() @@ -148,6 +152,7 @@ void CameraViewer::showImage(const rtabmap::SensorData & data) imageView_->setVisible(!left.empty() || !left.empty()); std::map detections; + UTimer markerDetectionTime; if(!left.empty()) { std::vector models; @@ -162,12 +167,22 @@ void CameraViewer::showImage(const rtabmap::SensorData & data) } } } + else + { + _landmarksSize.clear(); + } if(!models.empty() && models[0].isValidForProjection()) { cv::Mat imageWithDetections; - detections = markerDetector_->detect(left, models, depthOrRight, std::map(), &imageWithDetections); + detections = markerDetector_->detect(left, models, depthOrRight, _landmarksSize, &imageWithDetections); imageView_->setImage(uCvMat2QImage(imageWithDetections)); + for(std::map::iterator iter=detections.begin(); iter!=detections.end(); ++iter) + { + if(iter->second.length() > 0.0f) { + _landmarksSize.insert(std::make_pair(iter->first, iter->second.length())); + } + } } else { @@ -180,6 +195,11 @@ void CameraViewer::showImage(const rtabmap::SensorData & data) imageView_->setImageDepth(depthOrRight); sizes.append(QString(" Depth=%1x%2").arg(depthOrRight.cols).arg(depthOrRight.rows)); } + sizes.append(QString(" FPS capture=%1 render=%2").arg(lastCapturePeriod_>0.0?(int)round(1.0/lastCapturePeriod_):0).arg((int)round(1.0/fpsTimer_.restart()*1000))); + if(markerCheckbox_->isEnabled() && markerCheckbox_->isChecked()) + { + sizes.append(QString(" Marker=%1ms").arg(int(markerDetectionTime.ticks()*1000))); + } imageSizeLabel_->setText(sizes); if(!depthOrRight.empty() && @@ -232,7 +252,7 @@ void CameraViewer::showImage(const rtabmap::SensorData & data) #if PCL_VERSION_COMPARE(>=, 1, 7, 2) cloudView_->addOrUpdateCoordinate(uFormat("landmark_%d", iter->first), iter->second.pose(), iter->second.length(), false); #endif - std::string num = uNumber2Str(iter->first); + std::string num = uFormat("%d (%.1f cm)", iter->first, iter->second.length()*100.0f); cloudView_->addOrUpdateText( std::string("landmark_str_") + num, num, @@ -308,6 +328,8 @@ bool CameraViewer::handleEvent(UEvent * event) { if(camEvent->data().isValid()) { + lastCapturePeriod_ = camEvent->data().stamp() - previousCaptureStamp_; + previousCaptureStamp_ = camEvent->data().stamp(); if(!processingImages_ && this->isVisible() && camEvent->data().isValid()) { processingImages_ = true; diff --git a/guilib/src/CloudViewer.cpp b/guilib/src/CloudViewer.cpp index fd042c79..6b6f04d5 100644 --- a/guilib/src/CloudViewer.cpp +++ b/guilib/src/CloudViewer.cpp @@ -1732,7 +1732,8 @@ void CloudViewer::addOrUpdateLine( const Transform & to, const QColor & color, bool arrow, - bool foreground) + bool foreground, + double width) { if(id.empty()) { @@ -1764,6 +1765,7 @@ void CloudViewer::addOrUpdateLine( _visualizer->addLine(pt2, pt1, c.redF(), c.greenF(), c.blueF(), id, foreground?3:2); } _visualizer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_OPACITY, c.alphaF(), id); + _visualizer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_LINE_WIDTH, width, id); } } @@ -2969,18 +2971,19 @@ void CloudViewer::updateCameraTargetPosition(const Transform & pose) Eigen::Vector3f zAxis(cameras.front().view[0], cameras.front().view[1], cameras.front().view[2]); Eigen::Vector3f yAxis = zAxis.cross(vPosToFocal); Eigen::Vector3f xAxis = yAxis.cross(zAxis); - Transform PR(xAxis[0], xAxis[1], xAxis[2],0, - yAxis[0], yAxis[1], yAxis[2],0, - zAxis[0], zAxis[1], zAxis[2],0); - PR.normalizeRotation(); + Eigen::Matrix3f m; + m << xAxis[0], xAxis[1], xAxis[2], + yAxis[0], yAxis[1], yAxis[2], + zAxis[0], zAxis[1], zAxis[2]; - Transform P(PR[0], PR[1], PR[2], cameras.front().pos[0], - PR[4], PR[5], PR[6], cameras.front().pos[1], - PR[8], PR[9], PR[10], cameras.front().pos[2]); - Transform F(PR[0], PR[1], PR[2], cameras.front().focal[0], - PR[4], PR[5], PR[6], cameras.front().focal[1], - PR[8], PR[9], PR[10], cameras.front().focal[2]); + // Make sure it is normalized + Eigen::Quaternionf q = Eigen::Quaternionf(m).normalized(); + + Transform P(cameras.front().pos[0], cameras.front().pos[1], cameras.front().pos[2], + q.x(), q.y(), q.z(), q.w()); + Transform F(cameras.front().focal[0], cameras.front().focal[1], cameras.front().focal[2], + q.x(), q.y(), q.z(), q.w()); Transform N = pose; Transform O = _lastPose; Transform O2N = O.inverse()*N; diff --git a/guilib/src/DataRecorder.cpp b/guilib/src/DataRecorder.cpp index 31baa28d..940f29f7 100644 --- a/guilib/src/DataRecorder.cpp +++ b/guilib/src/DataRecorder.cpp @@ -72,13 +72,15 @@ bool DataRecorder::init(const QString & path, bool recordInRAM) ParametersMap customParameters; customParameters.insert(ParametersPair(Parameters::kMemRehearsalSimilarity(), "1.0")); // deactivate rehearsal customParameters.insert(ParametersPair(Parameters::kKpMaxFeatures(), "-1")); // deactivate keypoints extraction - customParameters.insert(ParametersPair(Parameters::kMemBinDataKept(), "true")); // to keep images + customParameters.insert(ParametersPair(Parameters::kMemBinDataKept(), "true")); // make sure we keep images customParameters.insert(ParametersPair(Parameters::kMemMapLabelsAdded(), "false")); // don't create map labels + customParameters.insert(ParametersPair(Parameters::kRGBDCreateOccupancyGrid(), "false")); // make sure we don't create local grids customParameters.insert(ParametersPair(Parameters::kMemBadSignaturesIgnored(), "true")); // make sure memory cleanup is done - customParameters.insert(ParametersPair(Parameters::kMemIntermediateNodeDataKept(), "true")); - if(!recordInRAM) + customParameters.insert(ParametersPair(Parameters::kMemNotLinkedNodesKept(), "true")); // make sure we save every frame + + if(recordInRAM) { - customParameters.insert(ParametersPair(Parameters::kDbSqlite3InMemory(), "false")); + customParameters.insert(ParametersPair(Parameters::kDbSqlite3InMemory(), "true")); } memory_ = new Memory(); if(!memory_->init(path.toStdString(), true, customParameters)) @@ -138,7 +140,7 @@ void DataRecorder::addData(const rtabmap::SensorData & data, const Transform & p //save to database UTimer time; memory_->update(data, pose, covariance); - const Signature * s = memory_->getLastWorkingSignature(); + const Signature * s = memory_->getLastWorkingSignature(false); totalSizeKB_ += (int)s->sensorData().imageCompressed().total()/1000; totalSizeKB_ += (int)s->sensorData().depthOrRightCompressed().total()/1000; totalSizeKB_ += (int)s->sensorData().laserScanCompressed().data().total()/1000; diff --git a/guilib/src/DatabaseViewer.cpp b/guilib/src/DatabaseViewer.cpp index 586a386b..8951266a 100644 --- a/guilib/src/DatabaseViewer.cpp +++ b/guilib/src/DatabaseViewer.cpp @@ -239,6 +239,7 @@ DatabaseViewer::DatabaseViewer(const QString & ini, QWidget * parent) : parameters.insert(*Parameters::getDefaultParameters().find(Parameters::kRGBDLoopClosureReextractFeatures())); parameters.insert(*Parameters::getDefaultParameters().find(Parameters::kRGBDLoopCovLimited())); parameters.insert(*Parameters::getDefaultParameters().find(Parameters::kRGBDProximityPathFilteringRadius())); + parameters.insert(*Parameters::getDefaultParameters().find(Parameters::kMemSTMSize())); ui_->parameters_toolbox->setupUi(parameters); exportDialog_->setObjectName("ExportCloudsDialog"); restoreDefaultSettings(); @@ -394,11 +395,10 @@ DatabaseViewer::DatabaseViewer(const QString & ini, QWidget * parent) : connect(ui_->toolButton_constraint, SIGNAL(clicked(bool)), this, SLOT(editConstraint())); connect(ui_->checkBox_enableForAll, SIGNAL(stateChanged(int)), this, SLOT(updateConstraintButtons())); - ui_->horizontalSlider_iterations->setTracking(false); + ui_->horizontalSlider_iterations->setTracking(true); ui_->horizontalSlider_iterations->setEnabled(false); ui_->spinBox_optimizationsFrom->setEnabled(false); 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_->comboBox_optimizationFlavor, SIGNAL(activated(int)), this, SLOT(updateGraphView())); connect(ui_->checkBox_spanAllMaps, SIGNAL(stateChanged(int)), this, SLOT(updateGraphView())); @@ -454,6 +454,7 @@ DatabaseViewer::DatabaseViewer(const QString & ini, QWidget * parent) : connect(ui_->checkBox_alignScansCloudsWithGroundTruth, SIGNAL(stateChanged(int)), this, SLOT(updateGraphView())); connect(ui_->checkBox_ignoreIntermediateNodes, SIGNAL(stateChanged(int)), this, SLOT(configModified())); connect(ui_->checkBox_ignoreIntermediateNodes, SIGNAL(stateChanged(int)), this, SLOT(updateGraphView())); + connect(ui_->comboBox_env_sensor_graph_colormap, SIGNAL(currentIndexChanged(int)), this, SLOT(updateGraphView())); connect(ui_->checkBox_timeStats, SIGNAL(stateChanged(int)), this, SLOT(configModified())); connect(ui_->checkBox_timeStats, SIGNAL(stateChanged(int)), this, SLOT(updateStatistics())); // Graph view @@ -481,6 +482,7 @@ DatabaseViewer::DatabaseViewer(const QString & ini, QWidget * parent) : connect(ui_->spinBox_detectMore_iterations, SIGNAL(valueChanged(int)), this, SLOT(configModified())); connect(ui_->checkBox_detectMore_intraSession, SIGNAL(stateChanged(int)), this, SLOT(configModified())); connect(ui_->checkBox_detectMore_interSession, SIGNAL(stateChanged(int)), this, SLOT(configModified())); + connect(ui_->spinBox_minGraphDistance, SIGNAL(valueChanged(int)), this, SLOT(configModified())); connect(ui_->checkBox_opt_graph_as_guess, SIGNAL(stateChanged(int)), this, SLOT(configModified())); connect(ui_->lineEdit_obstacleColor, SIGNAL(textChanged(const QString &)), this, SLOT(configModified())); @@ -678,6 +680,7 @@ void DatabaseViewer::readSettings() ui_->checkBox_detectMore_intraSession->setChecked(settings.value("intra_session", ui_->checkBox_detectMore_intraSession->isChecked()).toBool()); ui_->checkBox_detectMore_interSession->setChecked(settings.value("inter_session", ui_->checkBox_detectMore_interSession->isChecked()).toBool()); ui_->checkBox_opt_graph_as_guess->setChecked(settings.value("opt_graph_as_guess", ui_->checkBox_opt_graph_as_guess->isChecked()).toBool()); + ui_->spinBox_minGraphDistance->setValue(settings.value("min_graph_distance", ui_->spinBox_minGraphDistance->value()).toInt()); settings.endGroup(); settings.endGroup(); @@ -775,6 +778,7 @@ void DatabaseViewer::writeSettings() settings.setValue("intra_session", ui_->checkBox_detectMore_intraSession->isChecked()); settings.setValue("inter_session", ui_->checkBox_detectMore_interSession->isChecked()); settings.setValue("opt_graph_as_guess", ui_->checkBox_opt_graph_as_guess->isChecked()); + settings.setValue("min_graph_distance", ui_->spinBox_minGraphDistance->value()); settings.endGroup(); settings.endGroup(); @@ -855,6 +859,8 @@ void DatabaseViewer::restoreDefaultSettings() ui_->checkBox_detectMore_intraSession->setChecked(true); ui_->checkBox_detectMore_interSession->setChecked(true); ui_->checkBox_opt_graph_as_guess->setChecked(true); + ui_->spinBox_fromToMapId->setValue(-1); + ui_->spinBox_minGraphDistance->setValue(10); } void DatabaseViewer::openDatabase() @@ -1129,6 +1135,7 @@ bool DatabaseViewer::closeDatabase() lastWmIds_.clear(); mapIds_.clear(); weights_.clear(); + envSensors_.clear(); wmStates_.clear(); links_.clear(); linksAdded_.clear(); @@ -1806,6 +1813,7 @@ void DatabaseViewer::updateIds() idToIndex_.clear(); mapIds_.clear(); weights_.clear(); + envSensors_.clear(); wmStates_.clear(); odomPoses_.clear(); groundTruthPoses_.clear(); @@ -1912,6 +1920,7 @@ void DatabaseViewer::updateIds() dbDriver_->getNodeInfo(ids_[i], p, mapId, w, l, s, g, v, gps, sensors); mapIds_.insert(std::make_pair(ids_[i], mapId)); weights_.insert(std::make_pair(ids_[i], w)); + envSensors_.insert(std::make_pair(ids_[i], sensors)); if(w>=0) { for(std::multimap::iterator iter=links.find(ids_[i]); iter!=links.end() && iter->first==ids_[i]; ++iter) @@ -4281,10 +4290,12 @@ void DatabaseViewer::detectMoreLoopClosures() const ParametersMap & parameters = ui_->parameters_toolbox->getParameters(); bool loopCovLimited = Parameters::defaultRGBDLoopCovLimited(); Parameters::parse(parameters, Parameters::kRGBDLoopCovLimited(), loopCovLimited); + std::multimap links = updateLinksWithModifications(links_); if(loopCovLimited) { - odomMaxInf_ = graph::getMaxOdomInf(updateLinksWithModifications(links_)); + odomMaxInf_ = graph::getMaxOdomInf(links); } + links = graph::filterLinks(links, Link::kNeighbor, true); // keep only neighbor links int iterations = ui_->spinBox_detectMore_iterations->value(); UASSERT(iterations > 0); @@ -4294,6 +4305,8 @@ void DatabaseViewer::detectMoreLoopClosures() bool intraSession = ui_->checkBox_detectMore_intraSession->isChecked(); bool interSession = ui_->checkBox_detectMore_interSession->isChecked(); bool useOptimizedGraphAsGuess = ui_->checkBox_opt_graph_as_guess->isChecked(); + int fromToMapId = ui_->spinBox_fromToMapId->value(); + int minimumGraphDistance = ui_->spinBox_minGraphDistance->value(); if(!interSession && !intraSession) { QMessageBox::warning(this, tr("Cannot detect more loop closures"), tr("Intra and inter session parameters are disabled! Enable one or both.")); @@ -4310,14 +4323,68 @@ void DatabaseViewer::detectMoreLoopClosures() ui_->doubleSpinBox_detectMore_radius->value(), ui_->doubleSpinBox_detectMore_angle->value()*CV_PI/180.0); - progressDialog->setMaximumSteps(progressDialog->maximumSteps()+(int)clusters.size()); - progressDialog->appendText(tr("Looking for more loop closures, %1 clusters found.").arg(clusters.size())); QApplication::processEvents(); if(progressDialog->isCanceled()) { break; } + progressDialog->appendText(tr("Looking for more loop closures: %1 clusters found.").arg(clusters.size())); + if(fromToMapId >=0) + { + int clusterBefore = clusters.size(); + for(std::multimap::iterator iter=clusters.begin(); iter!=clusters.end();) + { + int mapId = uValue(mapIds_, iter->first, 0); + if(mapId != fromToMapId) + { + iter = clusters.erase(iter); + } + else { + ++iter; + } + } + progressDialog->appendText(tr("Looking for more loop closures: filtered %1/%2 clusters for map session %3.") + .arg(clusterBefore-clusters.size()).arg(clusterBefore).arg(fromToMapId)); + if(clusters.empty()) + { + progressDialog->appendText(tr("No clusters belong to mapId %1, aborting!").arg(fromToMapId)); + QApplication::processEvents(); + break; + } + } + + if(minimumGraphDistance > 1) + { + int clusterBefore = clusters.size(); + for(std::multimap::iterator iter=clusters.begin(); iter!=clusters.end();) + { + if(abs(iter->first - iter->second) < minimumGraphDistance) + { + iter = clusters.erase(iter); + } + else + { + // compute path to know how far we are in terms of graph length + std::list path = graph::computePath(links, iter->first, iter->second); + if(!path.empty() && (int)path.size() <= minimumGraphDistance) + { + iter = clusters.erase(iter); + } + else + { + ++iter; + } + } + } + progressDialog->appendText(tr("Filtered %1/%2 clusters for too close nodes (below minimum graph distance=%3).") + .arg(clusterBefore-clusters.size()).arg(clusterBefore).arg(minimumGraphDistance)); + QApplication::processEvents(); + } + + progressDialog->setMaximumSteps(progressDialog->maximumSteps()+(int)clusters.size()); + QApplication::processEvents(); + std::set addedLinks; int i=0; for(std::multimap::iterator iter=clusters.begin(); iter!= clusters.end() && !progressDialog->isCanceled(); ++iter, ++i) @@ -4671,10 +4738,17 @@ void DatabaseViewer::graphNodeSelected(int id) void DatabaseViewer::graphLinkSelected(int from, int to) { - if(from>0 && idToIndex_.contains(from)) - ui_->horizontalSlider_A->setValue(idToIndex_.value(from)); - if(to>0 && idToIndex_.contains(to)) - ui_->horizontalSlider_B->setValue(idToIndex_.value(to)); + if(from < 0 || to < 0) + { + updateLoopClosuresSlider(from, to); + } + else + { + if(idToIndex_.contains(from)) + ui_->horizontalSlider_A->setValue(idToIndex_.value(from)); + if(idToIndex_.contains(to)) + ui_->horizontalSlider_B->setValue(idToIndex_.value(to)); + } } void DatabaseViewer::sliderAValueChanged(int value) @@ -6134,6 +6208,14 @@ void DatabaseViewer::updateWordsMatching(const std::vector & inliers) kptB->keypoint().pt.y, cB); } + else if(ids[i]<0) + { + ui_->graphicsView_A->setFeatureColor(ids[i], Qt::gray); + } + } + for(auto iter = wordsB.begin(); iter.key()<0 && iter!=wordsB.end(); ++iter) + { + ui_->graphicsView_B->setFeatureColor(iter.key(), Qt::gray); } ui_->graphicsView_A->update(); ui_->graphicsView_B->update(); @@ -7100,6 +7182,7 @@ void DatabaseViewer::updateConstraintButtons() void DatabaseViewer::sliderIterationsValueChanged(int value) { + UDEBUG("sender=%s value=%d currentValue = %d", sender()?sender()->objectName().toStdString().c_str():"NA", value, ui_->horizontalSlider_iterations->value()); if(dbDriver_ && value >=0 && value < (int)graphes_.size()) { std::map graph = uValueAt(graphes_, value); @@ -7242,7 +7325,47 @@ void DatabaseViewer::sliderIterationsValueChanged(int value) ui_->graphViewer->updateGTGraph(groundTruthPoses_); ui_->graphViewer->updateGPSGraph(gpsPoses_, gpsValues_); ui_->graphViewer->updateGraph(graph, graphLinks_, mapIds_, weights_); - if(ui_->checkBox_wmState->isEnabled() && + if(ui_->comboBox_env_sensor_graph_colormap->currentIndex() != 0) + { + std::map colors; + EnvSensor::Type curentType = (EnvSensor::Type)ui_->comboBox_env_sensor_graph_colormap->currentIndex(); + for(std::map::iterator iter=graph.begin(); iter!=graph.end(); ++iter) + { + auto jter = envSensors_.find(iter->first); + if(jter != envSensors_.end() && jter->second.find(curentType) != jter->second.end()) + { + colors.insert(std::make_pair(iter->first, jter->second.at(curentType).value())); + } + } + std::string legend; + bool invertedColor = false; + unsigned char hueMax = 240; // blue + switch(curentType) + { + case EnvSensor::kWifiSignalStrength: + legend = "Wifi Signal Strength (dBm)"; + invertedColor = true; + hueMax = 120; // green + break; + case EnvSensor::kAmbientTemperature: + legend = "Ambient Temperature (Celcius)"; + break; + case EnvSensor::kAmbientAirPressure: + legend = "Ambient Air Pressure (hPa)"; + break; + case EnvSensor::kAmbientLight: + legend = "Ambient Light / Illuminance (lx)"; + break; + case EnvSensor::kAmbientRelativeHumidity: + legend = "Ambient Relative Humidity (%)"; + break; + default: + break; + } + ui_->graphViewer->updateNodeColorByValue(legend, colors, 0.0f, 0.0f, invertedColor, 0, hueMax, 1); + UDEBUG("Updated node color based on env sensor %d", (int)curentType); + } + else if(ui_->checkBox_wmState->isEnabled() && ui_->checkBox_wmState->isChecked() && !lastWmIds_.empty()) { @@ -7263,6 +7386,7 @@ void DatabaseViewer::sliderIterationsValueChanged(int value) { ui_->graphViewer->updateNodeColorByValue("In WM", colors, 1, false, 1); } + UDEBUG("Updated node color based working memory state"); } QGraphicsRectItem * rectScaleItem = 0; ui_->graphViewer->clearMap(); @@ -7466,13 +7590,16 @@ void DatabaseViewer::sliderIterationsValueChanged(int value) } #endif } - ui_->graphViewer->fitInView(ui_->graphViewer->scene()->itemsBoundingRect(), Qt::KeepAspectRatio); + if(rectScaleItem != 0) { ui_->graphViewer->fitInView(rectScaleItem, Qt::KeepAspectRatio); ui_->graphViewer->scene()->removeItem(rectScaleItem); delete rectScaleItem; } + else { + ui_->graphViewer->fitInView(ui_->graphViewer->sceneRect(), Qt::KeepAspectRatio); + } ui_->graphViewer->update(); ui_->label_iterations->setNum(value); @@ -7803,8 +7930,7 @@ void DatabaseViewer::updateGraphView() // remove intermediate nodes? if(ui_->checkBox_ignoreIntermediateNodes->isVisible() && - ui_->checkBox_ignoreIntermediateNodes->isEnabled() && - ui_->checkBox_ignoreIntermediateNodes->isChecked()) + (ui_->checkBox_ignoreIntermediateNodes->isChecked() || ui_->comboBox_optimizationFlavor->currentIndex() == 2)) { for(std::multimap::iterator iter=links.begin(); iter!=links.end(); ++iter) { @@ -7969,6 +8095,17 @@ void DatabaseViewer::updateGraphView() ui_->label_timeOptimization->setNum(0); ui_->label_poses->setNum((int)optPoses.size()); graphes_.push_back(optPoses); + // Just get the links: + std::map posesOut; + UINFO("Get connected graph from %d (%d poses, %d links)", fromId, (int)poses.size(), (int)links.size()); + std::shared_ptr optimizer(Optimizer::create(parameters)); + optimizer->getConnectedGraph( + fromId, + optPoses, + links, + posesOut, + graphLinks_); + UINFO("Connected graph of %d poses and %d links", (int)posesOut.size(), (int)graphLinks_.size()); } ui_->horizontalSlider_rotation->setEnabled(false); ui_->pushButton_applyRotation->setEnabled(false); @@ -8361,7 +8498,7 @@ void DatabaseViewer::refineConstraint(int from, int to, Registration * reg, Regi } Transform toPoseInv = filteredScanPoses.at(currentLink.to()).inverse(); - dbDriver_->loadNodeData(fromS, !silent, true, !silent, !silent); + dbDriver_->loadNodeData(*fromS, !silent, true, !silent, !silent); fromS->sensorData().uncompressData(); LaserScan fromScan = fromS->sensorData().laserScanRaw(); int maxPoints = fromScan.size(); @@ -8507,8 +8644,8 @@ void DatabaseViewer::refineConstraint(int from, int to, Registration * reg, Regi reextractVisualFeatures || !silent) { - dbDriver_->loadNodeData(fromS, reextractVisualFeatures || !silent || (reg->isScanRequired() && ui_->checkBox_icp_from_depth->isChecked()), reg->isScanRequired() || !silent, reg->isUserDataRequired() || !silent, !silent); - dbDriver_->loadNodeData(toS, reextractVisualFeatures || !silent || (reg->isScanRequired() && ui_->checkBox_icp_from_depth->isChecked()), reg->isScanRequired() || !silent, reg->isUserDataRequired() || !silent, !silent); + dbDriver_->loadNodeData(*fromS, reextractVisualFeatures || !silent || (reg->isScanRequired() && ui_->checkBox_icp_from_depth->isChecked()), reg->isScanRequired() || !silent, reg->isUserDataRequired() || !silent, !silent); + dbDriver_->loadNodeData(*toS, reextractVisualFeatures || !silent || (reg->isScanRequired() && ui_->checkBox_icp_from_depth->isChecked()), reg->isScanRequired() || !silent, reg->isUserDataRequired() || !silent, !silent); if(!silent) { @@ -8603,6 +8740,7 @@ void DatabaseViewer::refineConstraint(int from, int to, Registration * reg, Regi if(!transform.isNull()) { + UASSERT(!info.covariance.empty()); if(!transform.isIdentity()) { if(info.covariance.at(0,0)<=0.0) @@ -8796,9 +8934,9 @@ bool DatabaseViewer::addConstraint(int from, int to, Registration * reg, bool si !silent) { // Add sensor data to generate features - dbDriver_->loadNodeData(fromS, reextractVisualFeatures || !silent || (reg->isScanRequired() && ui_->checkBox_icp_from_depth->isChecked()), reg->isScanRequired() || !silent, reg->isUserDataRequired() || !silent, !silent); + dbDriver_->loadNodeData(*fromS, reextractVisualFeatures || !silent || (reg->isScanRequired() && ui_->checkBox_icp_from_depth->isChecked()), reg->isScanRequired() || !silent, reg->isUserDataRequired() || !silent, !silent); fromS->sensorData().uncompressData(); - dbDriver_->loadNodeData(toS, reextractVisualFeatures || !silent || (reg->isScanRequired() && ui_->checkBox_icp_from_depth->isChecked()), reg->isScanRequired() || !silent, reg->isUserDataRequired() || !silent, !silent); + dbDriver_->loadNodeData(*toS, reextractVisualFeatures || !silent || (reg->isScanRequired() && ui_->checkBox_icp_from_depth->isChecked()), reg->isScanRequired() || !silent, reg->isUserDataRequired() || !silent, !silent); toS->sensorData().uncompressData(); if(reextractVisualFeatures) { @@ -8990,10 +9128,6 @@ bool DatabaseViewer::addConstraint(int from, int to, Registration * reg, bool si int fromId = newLink.from(); std::multimap linksIn = updateLinksWithModifications(links_); linksIn.insert(std::make_pair(newLink.from(), newLink)); - const Link * maxLinearLink = 0; - const Link * maxAngularLink = 0; - float maxLinearErrorRatio = 0.0f; - float maxAngularErrorRatio = 0.0f; Optimizer * optimizer = Optimizer::create(ui_->parameters_toolbox->getParameters()); std::map poses; std::multimap links; @@ -9026,51 +9160,43 @@ bool DatabaseViewer::addConstraint(int from, int to, Registration * reg, bool si std::string msg; if(poses.size()) { - float maxLinearError = 0.0f; - float maxAngularError = 0.0f; - graph::computeMaxGraphErrors( + graph::MaxGraphErrors maxGraphErrors = graph::computeMaxGraphErrors( poses, - links, - maxLinearErrorRatio, - maxAngularErrorRatio, - maxLinearError, - maxAngularError, - &maxLinearLink, - &maxAngularLink); - if(maxLinearLink) + links); + if(maxGraphErrors.linearLink.isValid()) { - 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 > maxOptimizationError) + UINFO("Max optimization linear error = %f m (link %d->%d, var=%f, ratio error/std=%f)", maxGraphErrors.linear, maxGraphErrors.linearLink.from(), maxGraphErrors.linearLink.to(), maxGraphErrors.linearLink.transVariance(), maxGraphErrors.linear/sqrt(maxGraphErrors.linearLink.transVariance())); + if(maxGraphErrors.linearRatio > maxOptimizationError) { msg = uFormat("Rejecting edge %d->%d because " "graph error is too large (abs=%f m) after optimization (ratio %f for edge %d->%d, stddev=%f m). " "\"%s\" is %f.", newLink.from(), newLink.to(), - maxLinearError, - maxLinearErrorRatio, - maxLinearLink->from(), - maxLinearLink->to(), - sqrt(maxLinearLink->transVariance()), + maxGraphErrors.linear, + maxGraphErrors.linearRatio, + maxGraphErrors.linearLink.from(), + maxGraphErrors.linearLink.to(), + sqrt(maxGraphErrors.linearLink.transVariance()), Parameters::kRGBDOptimizeMaxError().c_str(), maxOptimizationError); } } - if(maxAngularLink) + if(maxGraphErrors.angularLink.isValid()) { - 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 > maxOptimizationError) + UINFO("Max optimization angular error = %f deg (link %d->%d, var=%f, ratio error/std=%f)", maxGraphErrors.angular*180.0f/CV_PI, maxGraphErrors.angularLink.from(), maxGraphErrors.angularLink.to(), maxGraphErrors.angularLink.rotVariance(), maxGraphErrors.angular/sqrt(maxGraphErrors.angularLink.rotVariance())); + if(maxGraphErrors.angularRatio > maxOptimizationError) { msg = uFormat("Rejecting edge %d->%d because " "graph error is too large (abs=%f deg) after optimization (ratio %f for edge %d->%d, stddev=%f deg). " "\"%s\" is %f.", newLink.from(), newLink.to(), - maxAngularError*180.0f/CV_PI, - maxAngularErrorRatio, - maxAngularLink->from(), - maxAngularLink->to(), - sqrt(maxAngularLink->rotVariance()), + maxGraphErrors.angular*180.0f/CV_PI, + maxGraphErrors.angularRatio, + maxGraphErrors.angularLink.from(), + maxGraphErrors.angularLink.to(), + sqrt(maxGraphErrors.angularLink.rotVariance()), Parameters::kRGBDOptimizeMaxError().c_str(), maxOptimizationError); } @@ -9312,7 +9438,7 @@ std::multimap DatabaseViewer::updateLinksWithModifications( findIter = rtabmap::graph::findLink(linksRemoved_, iter->second.from(), iter->second.to()); if(findIter != linksRemoved_.end()) { - UDEBUG("Removed link (%d->%d, %d)", iter->second.from(), iter->second.to(), iter->second.type()); + //UDEBUG("Removed link (%d->%d, %d)", iter->second.from(), iter->second.to(), iter->second.type()); continue; // don't add this link } @@ -9329,7 +9455,7 @@ std::multimap DatabaseViewer::updateLinksWithModifications( { links.insert(*findIter); } - UDEBUG("Updated link (%d->%d, %d)", iter->second.from(), iter->second.to(), iter->second.type()); + //UDEBUG("Updated link (%d->%d, %d)", iter->second.from(), iter->second.to(), iter->second.type()); continue; } @@ -9345,14 +9471,18 @@ std::multimap DatabaseViewer::updateLinksWithModifications( if(findIter!=linksRefined_.end()) { links.insert(*findIter); // add the refined link - links.insert(std::make_pair(findIter->second.to(), findIter->second.inverse())); // return both ways - UDEBUG("Added refined link (%d->%d, %d)", findIter->second.from(), findIter->second.to(), findIter->second.type()); + if(findIter->second.from() != findIter->second.to()) { + links.insert(std::make_pair(findIter->second.to(), findIter->second.inverse())); // return both ways + } + //UDEBUG("Added refined link (%d->%d, %d)", findIter->second.from(), findIter->second.to(), findIter->second.type()); continue; } - UDEBUG("Added link (%d->%d, %d)", iter->second.from(), iter->second.to(), iter->second.type()); + //UDEBUG("Added link (%d->%d, %d)", iter->second.from(), iter->second.to(), iter->second.type()); links.insert(*iter); - links.insert(std::make_pair(iter->second.to(), iter->second.inverse())); // return both ways + if(iter->second.from() != iter->second.to()) { + links.insert(std::make_pair(iter->second.to(), iter->second.inverse())); // return both ways + } } return links; diff --git a/guilib/src/ExportCloudsDialog.cpp b/guilib/src/ExportCloudsDialog.cpp index 2c5ef930..a85c9e53 100644 --- a/guilib/src/ExportCloudsDialog.cpp +++ b/guilib/src/ExportCloudsDialog.cpp @@ -3703,7 +3703,7 @@ std::map::Ptr, pcl::Indic 0, 0, 0, - _ui->checkBox_fromDepth->isChecked()?&confidence:0); + _ui->checkBox_fromDepth->isChecked()&&_ui->spinBox_depthConfidence->value()>0?&confidence:0); } else if(_dbDriver) { @@ -3717,7 +3717,7 @@ std::map::Ptr, pcl::Indic 0, 0, 0, - _ui->checkBox_fromDepth->isChecked()?&confidence:0); + _ui->checkBox_fromDepth->isChecked()&&_ui->spinBox_depthConfidence->value()>0?&confidence:0); } if(_ui->checkBox_fromDepth->isChecked() && !data.imageRaw().empty() && !data.depthOrRightRaw().empty()) diff --git a/guilib/src/GraphViewer.cpp b/guilib/src/GraphViewer.cpp index 8f1f64b9..89f23a86 100644 --- a/guilib/src/GraphViewer.cpp +++ b/guilib/src/GraphViewer.cpp @@ -337,17 +337,37 @@ GraphViewer::GraphViewer(QWidget * parent) : _gridCellSize(0.0f), _localRadius(0), _loopClosureOutlierThr(0), - _maxLinkLength(0.02f), + _minLinkLength(0.02f), _orientationENU(false), _mouseTracking(false), _viewPlane(XY), _ensureFrameVisible(true) { - this->setScene(new QGraphicsScene(this)); this->setDragMode(QGraphicsView::ScrollHandDrag); _workingDirectory = QDir::homePath(); - this->scene()->clear(); + setupGraphicsScene(); + + // Match by default scan colors from DatabaseViewer + _highlightedNodes.push_back(QPair(Qt::yellow, nullptr)); + _highlightedNodes.push_back(QPair(Qt::magenta, nullptr)); + + this->restoreDefaults(); + + this->fitInView(this->sceneRect(), Qt::KeepAspectRatio); +} + +GraphViewer::~GraphViewer() +{ +} + +void GraphViewer::setupGraphicsScene() +{ + if(this->scene()) + { + delete this->scene(); + } + this->setScene(new QGraphicsScene(this)); _world = (QGraphicsItem *)this->scene()->addEllipse(QRectF(-0.0001,-0.0001,0.0001,0.0001)); _root = (QGraphicsItem *)this->scene()->addEllipse(QRectF(-0.0001,-0.0001,0.0001,0.0001)); _root->setParentItem(_world); @@ -485,17 +505,6 @@ GraphViewer::GraphViewer(QWidget * parent) : _odomCacheOverlay->setBrush(QBrush(QColor(255, 255, 255, 150))); _odomCacheOverlay->setPen(QPen(Qt::NoPen)); - // Match by default scan colors from DatabaseViewer - _highlightedNodes.push_back(QPair(Qt::yellow, nullptr)); - _highlightedNodes.push_back(QPair(Qt::magenta, nullptr)); - - this->restoreDefaults(); - - this->fitInView(this->sceneRect(), Qt::KeepAspectRatio); -} - -GraphViewer::~GraphViewer() -{ } void GraphViewer::setWorldMapRotation(const float & theta) @@ -510,18 +519,48 @@ void GraphViewer::updateGraph(const std::map & poses, const std::map & weights, const std::set & odomCacheIds) { - UTimer timer; - bool wasVisible = _graphRoot->isVisible(); - _graphRoot->show(); - - bool wasEmpty = _nodeItems.size() == 0 && _linkItems.size() == 0; - UDEBUG("poses=%d constraints=%d", (int)poses.size(), (int)constraints.size()); - //Hide nodes and links - for(QMap::iterator iter = _nodeItems.begin(); iter!=_nodeItems.end(); ++iter) + if(!_graphRoot->isVisible()) { + UDEBUG("Ignoring updating graph, the graph root is not visible."); + return; + } + + UTimer timer; + + bool wasEmpty = _nodeItems.size() == 0 && _linkItems.size() == 0 && _gridMap->pixmap().isNull(); + UDEBUG("poses=%ld constraints=%ld mapIds=%ld weights=%ld", poses.size(), constraints.size(), mapIds.size(), weights.size()); + for(QMultiMap::iterator iter = _linkItems.begin(); iter!=_linkItems.end(); ++iter) + { + iter.value()->hide(); + } + UDEBUG("hidden %d links", _linkItems.size()); + + int created = 0; + int reused = 0; + int removed = 0; + QMap::iterator nter = _nodeItems.begin(); + std::map::const_iterator iter=_nodeVisible?poses.begin():poses.end(); + while(nter!=_nodeItems.end() || iter!=poses.end()) + { + if(nter!=_nodeItems.end() && (iter==poses.end() || nter.key() < iter->first || iter->second.isNull())) + { + // NodeItem is not in poses anymore, increase only _nodeItems iterator + for(int i=0; i<_highlightedNodes.size(); ++i) + { + if(_highlightedNodes[i].second && _highlightedNodes[i].second == nter.value()) + { + _highlightedNodes[i].second = nullptr; + } + } + delete nter.value(); + nter = _nodeItems.erase(nter); + ++removed; + continue; + } + QColor color = _nodeColor; - bool isOdomCache = odomCacheIds.find(iter.key()) != odomCacheIds.end(); - if(iter.key()<0) + bool isOdomCache = odomCacheIds.find(iter->first) != odomCacheIds.end(); + if(iter->first<0) { color = QColor(255-color.red(), 255-color.green(), 255-color.blue()); } @@ -529,51 +568,50 @@ void GraphViewer::updateGraph(const std::map & poses, { color = _nodeOdomCacheColor; } - iter.value()->hide(); - iter.value()->setColor(color); // reset color - iter.value()->setToolTipInfo(QString()); - iter.value()->setZValue(iter.key()<0?21:20); - } - for(QMultiMap::iterator iter = _linkItems.begin(); iter!=_linkItems.end(); ++iter) - { - iter.value()->hide(); - } - for(std::map::const_iterator iter=poses.begin(); iter!=poses.end(); ++iter) - { - if(!iter->second.isNull()) + UASSERT(iter!=poses.end()); + + if(nter == _nodeItems.end() || nter.key() > iter->first) { - QMap::iterator itemIter = _nodeItems.find(iter->first); - if(itemIter != _nodeItems.end()) - { - itemIter.value()->setPose(iter->second, _viewPlane); - itemIter.value()->show(); - } - else - { - // create node item - QColor color = _nodeColor; - bool isOdomCache = odomCacheIds.find(iter->first) != odomCacheIds.end(); - if(iter->first<0) - { - color = QColor(255-color.red(), 255-color.green(), 255-color.blue()); - } - else if(isOdomCache) - { - color = _nodeOdomCacheColor; - } - const Transform & pose = iter->second; - NodeItem * item = new NodeItem(iter->first, uContains(mapIds, iter->first)?mapIds.at(iter->first):-1, pose, _nodeRadius, uContains(weights, iter->first)?weights.at(iter->first):-1, _viewPlane, _linkWidth); - this->scene()->addItem(item); - item->setZValue(iter->first<0?21:20); - item->setColor(color); - item->setParentItem(_graphRoot); - item->show(); - _nodeItems.insert(iter->first, item); - } + // NodeItem is not in poses, create a new one and increase poses iterator + const Transform & pose = iter->second; + NodeItem * item = new NodeItem( + iter->first, + uValue(mapIds, iter->first, -1), + pose, + _nodeRadius, + uValue(weights, iter->first, -1), + _viewPlane, + _linkWidth); + this->scene()->addItem(item); + item->setZValue(iter->first<0?21:20); + item->setColor(color); + item->setParentItem(_graphRoot); + item->show(); + _nodeItems.insert(iter->first, item); + ++iter; + ++created; + } + else + { + // NodeItem exists for the pose, copy data and increase both iterators + UASSERT(iter->first == nter.key()); + nter.value()->setColor(color); // reset color + nter.value()->setToolTipInfo(QString()); + nter.value()->setZValue(iter->first<0?21:20); + nter.value()->setPose(iter->second, _viewPlane); + nter.value()->show(); + ++nter; + ++iter; + ++reused; } } - + UDEBUG("Nodes created=%d, reused=%d removed=%d", created, reused, removed); + created = 0; + reused = 0; + removed = 0; + int removedSmallLinks = 0; + int ignoredSmallLinks = 0; for(std::multimap::const_iterator iter=constraints.begin(); iter!=constraints.end(); ++iter) { // make the first id the smallest one @@ -587,30 +625,28 @@ void GraphViewer::updateGraph(const std::map & poses, std::map::const_iterator jterA = poses.find(idFrom); std::map::const_iterator jterB = poses.find(idTo); LinkItem * linkItem = 0; - if(jterA != poses.end() && jterB != poses.end() && - _nodeItems.contains(idFrom) && _nodeItems.contains(idTo)) + if(jterA != poses.end() && jterB != poses.end()) { const Transform & poseA = jterA->second; const Transform & poseB = jterB->second; - QMultiMap::iterator itemIter = _linkItems.end(); - - if(_linkItems.contains(idFrom)) + QMultiMap::iterator itemIter = _linkItems.find(idFrom); + bool alreadyAdded = false; + while(itemIter != _linkItems.end() && itemIter.key() == idFrom) { - itemIter = _linkItems.find(idFrom); - bool alreadyAdded = false; - while(itemIter != _linkItems.end() && itemIter.key() == idFrom) + if(itemIter.value()->to() == idTo) { - if(itemIter.value()->to() == idTo && itemIter.value()->isVisible()) - { + if(itemIter.value()->isVisible()) { alreadyAdded = true; - break; + } else { + linkItem = itemIter.value(); } - ++itemIter; - } - if(alreadyAdded){ - continue; + break; } + ++itemIter; + } + if(alreadyAdded){ + continue; } bool interSessionClosure = false; @@ -625,11 +661,15 @@ void GraphViewer::updateGraph(const std::map & poses, if(isLinkedToOdomCachePoses) { - _nodeItems.value(idFrom)->setZValue(odomCacheIds.find(idFrom)!=odomCacheIds.end()?24:23); - _nodeItems.value(idTo)->setZValue(odomCacheIds.find(idTo)!=odomCacheIds.end()?24:23); + if(_nodeItems.contains(idFrom)) { + _nodeItems.value(idFrom)->setZValue(odomCacheIds.find(idFrom)!=odomCacheIds.end()?24:23); + } + if(_nodeItems.contains(idTo)) { + _nodeItems.value(idTo)->setZValue(odomCacheIds.find(idTo)!=odomCacheIds.end()?24:23); + } } - if(poseA.getDistance(poseB) > _maxLinkLength) + if(poseA.getDistance(poseB) > _minLinkLength) { if(linkItem == 0) { @@ -642,14 +682,24 @@ void GraphViewer::updateGraph(const std::map & poses, this->scene()->addItem(linkItem); linkItem->setParentItem(_graphRoot); _linkItems.insert(idFrom, linkItem); + ++created; + } + else { + linkItem->setPoses(poseA, poseB, _viewPlane); + linkItem->show(); + ++reused; } } - else if(linkItem && itemIter != _linkItems.end()) + else if(linkItem) { // erase small links _linkItems.erase(itemIter); delete linkItem; linkItem = 0; + ++removedSmallLinks; + } + else { + ++ignoredSmallLinks; } if(linkItem) @@ -730,71 +780,53 @@ void GraphViewer::updateGraph(const std::map & poses, } } } - + UDEBUG("Links created=%d, reused=%d, small links: removed=%d ignored=%d", created, reused, removedSmallLinks, ignoredSmallLinks); //remove not used nodes and links - for(QMap::iterator iter = _nodeItems.begin(); iter!=_nodeItems.end();) - { - if(!iter.value()->isVisible()) - { - for(int i=0; i<_highlightedNodes.size(); ++i) - { - if(_highlightedNodes[i].second && _highlightedNodes[i].second == iter.value()) - { - _highlightedNodes[i].second = nullptr; - } - } - - delete iter.value(); - iter = _nodeItems.erase(iter); - } - else - { - iter.value()->setVisible(_nodeVisible); - ++iter; - } - } + removed = 0; + int visible = 0; for(QMultiMap::iterator iter = _linkItems.begin(); iter!=_linkItems.end();) { if(!iter.value()->isVisible()) { delete iter.value(); iter = _linkItems.erase(iter); + ++removed; } else { ++iter; + ++visible; } } - + UDEBUG("Links removed=%d, visible=%d", removed, visible); if(_nodeItems.size()) { (--_nodeItems.end()).value()->setColor(_nodeOdomCacheColor); } - this->scene()->setSceneRect(this->scene()->itemsBoundingRect()); // Re-shrink the scene to it's bounding contents - + QRectF rect = this->scene()->sceneRect(); if(!odomCacheIds.empty()) - _odomCacheOverlay->setRect(this->scene()->itemsBoundingRect()); + _odomCacheOverlay->setRect(rect); else _odomCacheOverlay->setRect(0, 0, 0, 0); - + if(wasEmpty) { - QRectF rect = this->scene()->itemsBoundingRect(); this->fitInView(rect.adjusted(-rect.width()/2.0f, -rect.height()/2.0f, rect.width()/2.0f, rect.height()/2.0f), Qt::KeepAspectRatio); } - _graphRoot->setVisible(wasVisible); - UDEBUG("_nodeItems=%d, _linkItems=%d, timer=%fs", _nodeItems.size(), _linkItems.size(), timer.ticks()); } void GraphViewer::updateGTGraph(const std::map & poses) { + if(!_gtGraphRoot->isVisible()) + { + UDEBUG("Ignoring updating gt graph, the graph root is not visible."); + return; + } + UTimer timer; - bool wasVisible = _gtGraphRoot->isVisible(); - _gtGraphRoot->show(); - bool wasEmpty = _gtNodeItems.size() == 0 && _gtLinkItems.size() == 0; UDEBUG("poses=%d", (int)poses.size()); //Hide nodes and links for(QMap::iterator iter = _gtNodeItems.begin(); iter!=_gtNodeItems.end(); ++iter) @@ -811,23 +843,26 @@ void GraphViewer::updateGTGraph(const std::map & poses) { if(!iter->second.isNull()) { - QMap::iterator itemIter = _gtNodeItems.find(iter->first); - if(itemIter != _gtNodeItems.end()) + if(_nodeVisible) { - itemIter.value()->setPose(iter->second, _viewPlane); - itemIter.value()->show(); - } - else - { - // create node item - const Transform & pose = iter->second; - NodeItem * item = new NodeItem(iter->first, -1, pose, _nodeRadius, -1, _viewPlane, _linkWidth); - this->scene()->addItem(item); - item->setZValue(20); - item->setColor(_gtPathColor); - item->setParentItem(_gtGraphRoot); - item->setVisible(_nodeVisible); - _gtNodeItems.insert(iter->first, item); + QMap::iterator itemIter = _gtNodeItems.find(iter->first); + if(itemIter != _gtNodeItems.end()) + { + itemIter.value()->setPose(iter->second, _viewPlane); + itemIter.value()->show(); + } + else + { + // create node item + const Transform & pose = iter->second; + NodeItem * item = new NodeItem(iter->first, -1, pose, _nodeRadius, -1, _viewPlane, _linkWidth); + this->scene()->addItem(item); + item->setZValue(20); + item->setColor(_gtPathColor); + item->setParentItem(_gtGraphRoot); + item->setVisible(_nodeVisible); + _gtNodeItems.insert(iter->first, item); + } } if(iter!=poses.begin()) @@ -913,20 +948,6 @@ void GraphViewer::updateGTGraph(const std::map & poses) ++iter; } } - - if(_gtNodeItems.size() || _gtLinkItems.size()) - { - this->scene()->setSceneRect(this->scene()->itemsBoundingRect()); // Re-shrink the scene to it's bounding contents - - if(wasEmpty) - { - QRectF rect = this->scene()->itemsBoundingRect(); - this->fitInView(rect.adjusted(-rect.width()/2.0f, -rect.height()/2.0f, rect.width()/2.0f, rect.height()/2.0f), Qt::KeepAspectRatio); - } - } - - _gtGraphRoot->setVisible(wasVisible); - UDEBUG("_gtNodeItems=%d, _gtLinkItems=%d timer=%fs", _gtNodeItems.size(), _gtLinkItems.size(), timer.ticks()); } @@ -934,10 +955,11 @@ void GraphViewer::updateGPSGraph( const std::map & poses, const std::map & gpsValues) { + if(!_gpsGraphRoot->isVisible()) { + UDEBUG("Ignoring updating gps graph, the graph root is not visible."); + return; + } UTimer timer; - bool wasVisible = _gpsGraphRoot->isVisible(); - _gpsGraphRoot->show(); - bool wasEmpty = _gpsNodeItems.size() == 0 && _gpsNodeItems.size() == 0; UDEBUG("poses=%d", (int)poses.size()); //Hide nodes and links for(QMap::iterator iter = _gpsNodeItems.begin(); iter!=_gpsNodeItems.end(); ++iter) @@ -954,24 +976,27 @@ void GraphViewer::updateGPSGraph( { if(!iter->second.isNull()) { - QMap::iterator itemIter = _gpsNodeItems.find(iter->first); - if(itemIter != _gpsNodeItems.end()) + if(_nodeVisible) { - itemIter.value()->setPose(iter->second, _viewPlane); - itemIter.value()->show(); - } - else - { - // create node item - const Transform & pose = iter->second; - UASSERT(gpsValues.find(iter->first) != gpsValues.end()); - NodeItem * item = new NodeGPSItem(iter->first, -1, pose, _nodeRadius, gpsValues.at(iter->first), _viewPlane, _linkWidth); - this->scene()->addItem(item); - item->setZValue(20); - item->setColor(_gpsPathColor); - item->setParentItem(_gpsGraphRoot); - item->setVisible(_nodeVisible); - _gpsNodeItems.insert(iter->first, item); + QMap::iterator itemIter = _gpsNodeItems.find(iter->first); + if(itemIter != _gpsNodeItems.end()) + { + itemIter.value()->setPose(iter->second, _viewPlane); + itemIter.value()->show(); + } + else + { + // create node item + const Transform & pose = iter->second; + UASSERT(gpsValues.find(iter->first) != gpsValues.end()); + NodeItem * item = new NodeGPSItem(iter->first, -1, pose, _nodeRadius, gpsValues.at(iter->first), _viewPlane, _linkWidth); + this->scene()->addItem(item); + item->setZValue(20); + item->setColor(_gpsPathColor); + item->setParentItem(_gpsGraphRoot); + item->setVisible(_nodeVisible); + _gpsNodeItems.insert(iter->first, item); + } } if(iter!=poses.begin()) @@ -1043,20 +1068,6 @@ void GraphViewer::updateGPSGraph( ++iter; } } - - if(_gpsNodeItems.size() || _gpsLinkItems.size()) - { - this->scene()->setSceneRect(this->scene()->itemsBoundingRect()); // Re-shrink the scene to it's bounding contents - - if(wasEmpty) - { - QRectF rect = this->scene()->itemsBoundingRect(); - this->fitInView(rect.adjusted(-rect.width()/2.0f, -rect.height()/2.0f, rect.width()/2.0f, rect.height()/2.0f), Qt::KeepAspectRatio); - } - } - - _gpsGraphRoot->setVisible(wasVisible); - UDEBUG("_gpsNodeItems=%d, _gpsLinkItems=%d timer=%fs", _gpsNodeItems.size(), _gpsLinkItems.size(), timer.ticks()); } @@ -1085,6 +1096,7 @@ void GraphViewer::updateMap(const cv::Mat & map8U, float resolution, float xMin, UASSERT(map8U.empty() || (!map8U.empty() && resolution > 0.0f)); if(!map8U.empty()) { + bool wasEmpty = _nodeItems.size() <= 1 && _linkItems.size() == 0 && _gridMap->pixmap().isNull(); _gridCellSize = resolution; QImage image = uCvMat2QImage(map8U, false); _gridMap->resetTransform(); @@ -1092,8 +1104,11 @@ void GraphViewer::updateMap(const cv::Mat & map8U, float resolution, float xMin, _gridMap->setRotation(90); _gridMap->setPixmap(QPixmap::fromImage(image)); _gridMap->setPos(-yMin*100.0f, -xMin*100.0f); - // Re-shrink the scene to it's bounding contents - this->scene()->setSceneRect(this->scene()->itemsBoundingRect()); + + if(wasEmpty) + { + this->fitInView(this->scene()->sceneRect(), Qt::KeepAspectRatio); + } } else { @@ -1134,6 +1149,66 @@ void GraphViewer::updateNodeColorByValue(const std::string & valueName, const st } } +void GraphViewer::updateNodeColorByValue( + const std::string & valueName, + const std::map & values, + float min, + float max, + bool invertedColorScale, + unsigned short hueMin, + unsigned short hueMax, + int zValueOffset) +{ + hueMax = hueMax > 360 ? 360 : hueMax; + + //find min/max + if(min >= max) + { + bool firstValueSet = false; + for(std::map::const_iterator iter = values.begin(); iter!=values.end(); ++iter) + { + if(iter->first > 0) + { + if(!firstValueSet) { + min = max = iter->second; + firstValueSet = true; + } + else if(iter->second>max) + { + max = iter->second; + } + else if(iter->secondsecond; + } + } + } + } + if(min < max && hueMin < hueMax) + { + float range = max - min; + float hueRange = float(hueMax - hueMin); + for(QMap::iterator iter = _nodeItems.begin(); iter!=_nodeItems.end(); ++iter) + { + std::map::const_iterator jter = values.find(iter.key()); + if(jter != values.end()) + { + float v = jter->second; + v = std::min(v, max); + v = std::max(v, min); + iter.value()->setColor(QColor::fromHsvF(( invertedColorScale ? (v-min)/range : 1-(v-min)/range )*hueRange/360.0f + hueMin/360.0f, 1, 1, 1), valueName.c_str(), jter->second); + iter.value()->setZValue(iter.value()->zValue()+zValueOffset); + } + } + } + else if(min >= max) { + UWARN("min (%f) is not less than max (%f), cannot change color of the graph.", min, max); + } + else if(hueMin >= hueMax) { + UWARN("Hue min (%d) is not less than hue max (%d), cannot change color of the graph. The hue values should be set between 0 (red) and 360(pink).", (int)hueMin, (int)hueMax); + } +} + void GraphViewer::setGlobalPath(const std::vector > & globalPath) { UDEBUG("Set global path size=%d", (int)globalPath.size()); @@ -1200,6 +1275,11 @@ void GraphViewer::setNodeInfo(int id, const QString & info) void GraphViewer::setLocalRadius(float radius) { _localRadius->setRect(-radius*100, -radius*100, radius*200, radius*200); + if(_nodeItems.empty() && _linkItems.empty() && _gridMap->pixmap().isNull()) + { + QRectF rect = this->scene()->sceneRect(); + this->fitInView(rect.adjusted(-rect.width()/2.0f, -rect.height()/2.0f, rect.width()/2.0f, rect.height()/2.0f), Qt::KeepAspectRatio); + } } void GraphViewer::updateLocalPath(const std::vector & localPath) @@ -1324,14 +1404,17 @@ void GraphViewer::clearGraph() _worldMapRotation = 0.0f; _referential->resetTransform(); _localRadius->resetTransform(); - this->scene()->setSceneRect(this->scene()->itemsBoundingRect()); // Re-shrink the scene to it's bounding contents } void GraphViewer::clearMap() { - _gridMap->setPixmap(QPixmap()); _gridCellSize = 0.0f; - this->scene()->setSceneRect(this->scene()->itemsBoundingRect()); // Re-shrink the scene to it's bounding contents + if(_gridMap->pixmap().isNull()) + { + // there is no grid map, just return + return; + } + _gridMap->setPixmap(QPixmap()); } void GraphViewer::clearPosterior() @@ -1351,6 +1434,14 @@ void GraphViewer::clearAll() { clearMap(); clearGraph(); + + // The only way to re-shrink the dynamic scene rect is to re-create the QGraphicsScene. + QSettings tmp; + saveSettings(tmp); + QRectF localRadiusRectF = _localRadius->rect(); + setupGraphicsScene(); + loadSettings(tmp); // restore previous state + _localRadius->setRect(localRadiusRectF); } void GraphViewer::saveSettings(QSettings & settings, const QString & group) const @@ -1387,8 +1478,9 @@ void GraphViewer::saveSettings(QSettings & settings, const QString & group) cons settings.setValue("referential_visible", this->isReferentialVisible()); settings.setValue("local_radius_visible", this->isLocalRadiusVisible()); settings.setValue("loop_closure_outlier_thr", this->getLoopClosureOutlierThr()); - settings.setValue("max_link_length", this->getMaxLinkLength()); + settings.setValue("min_link_length", this->getMinLinkLength()); settings.setValue("graph_visible", this->isGraphVisible()); + settings.setValue("node_visible", this->isNodeVisible()); settings.setValue("global_path_visible", this->isGlobalPathVisible()); settings.setValue("local_path_visible", this->isLocalPathVisible()); settings.setValue("gt_graph_visible", this->isGtGraphVisible()); @@ -1437,8 +1529,9 @@ void GraphViewer::loadSettings(QSettings & settings, const QString & group) this->setLocalRadiusVisible(settings.value("local_radius_visible", this->isLocalRadiusVisible()).toBool()); this->setIntraInterSessionColorsEnabled(settings.value("intra_inter_session_colors_enabled", this->isIntraInterSessionColorsEnabled()).toBool()); this->setLoopClosureOutlierThr(settings.value("loop_closure_outlier_thr", this->getLoopClosureOutlierThr()).toDouble()); - this->setMaxLinkLength(settings.value("max_link_length", this->getMaxLinkLength()).toDouble()); + this->setMinLinkLength(settings.value("min_link_length", this->getMinLinkLength()).toDouble()); this->setGraphVisible(settings.value("graph_visible", this->isGraphVisible()).toBool()); + this->setNodeVisible(settings.value("node_visible", this->isNodeVisible()).toBool()); this->setGlobalPathVisible(settings.value("global_path_visible", this->isGlobalPathVisible()).toBool()); this->setLocalPathVisible(settings.value("local_path_visible", this->isLocalPathVisible()).toBool()); this->setGtGraphVisible(settings.value("gt_graph_visible", this->isGtGraphVisible()).toBool()); @@ -1473,6 +1566,10 @@ bool GraphViewer::isGraphVisible() const { return _graphRoot->isVisible(); } +bool GraphViewer::isNodeVisible() const +{ + return _nodeVisible; +} bool GraphViewer::isGlobalPathVisible() const { return _globalPathRoot->isVisible(); @@ -1795,9 +1892,9 @@ void GraphViewer::setLoopClosureOutlierThr(float value) { _loopClosureOutlierThr = value; } -void GraphViewer::setMaxLinkLength(float value) +void GraphViewer::setMinLinkLength(float value) { - _maxLinkLength = value; + _minLinkLength = value; } void GraphViewer::setGraphVisible(bool visible) { @@ -1838,10 +1935,6 @@ void GraphViewer::setOrientationENU(bool enabled) 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 - } } void GraphViewer::setViewPlane(ViewPlane plane) @@ -1868,11 +1961,6 @@ void GraphViewer::setViewPlane(ViewPlane plane) _referentialXY->setVisible(plane==XY); _referentialXZ->setVisible(plane==XZ); _referentialYZ->setVisible(plane==YZ); - - if(_nodeItems.size() || _linkItems.size()) - { - this->scene()->setSceneRect(this->scene()->itemsBoundingRect()); // Re-shrink the scene to it's bounding contents - } } void GraphViewer::setEnsureFrameVisible(bool visible) { @@ -2063,7 +2151,7 @@ void GraphViewer::contextMenuEvent(QContextMenuEvent * event) menu.addSeparator(); QAction * aSetNodeSize = menu.addAction(tr("Set node radius...")); QAction * aSetLinkSize = menu.addAction(tr("Set link width...")); - QAction * aChangeMaxLinkLength = menu.addAction(tr("Set maximum link length...")); + QAction * aChangeMinLinkLength = menu.addAction(tr("Set minimum link length...")); menu.addSeparator(); QAction * aEnsureFrameVisible; QAction * aShowHideGridMap; @@ -2181,12 +2269,9 @@ void GraphViewer::contextMenuEvent(QContextMenuEvent * event) aMouseTracking->setCheckable(true); aMouseTracking->setChecked(_mouseTracking); aMouseTracking->setEnabled(_viewPlane == XY); - aShowHideGraph->setEnabled(_nodeItems.size() && _viewPlane == XY); - aShowHideGraphNodes->setEnabled(_nodeItems.size() && _graphRoot->isVisible()); + aShowHideGraph->setEnabled(_viewPlane == XY); aShowHideGlobalPath->setEnabled(_globalPathLinkItems.size()); aShowHideLocalPath->setEnabled(_localPathLinkItems.size()); - aShowHideGtGraph->setEnabled(_gtNodeItems.size()); - aShowHideGPSGraph->setEnabled(_gpsNodeItems.size()); aShowHideOdomCacheOverlay->setEnabled(_odomCacheOverlay->rect().width()>0); QMenu * viewPlaneMenu = menu.addMenu("View Plane..."); @@ -2303,8 +2388,7 @@ void GraphViewer::contextMenuEvent(QContextMenuEvent * event) //reset scale _root->setScale(1.0f); - this->scene()->setSceneRect(this->scene()->itemsBoundingRect()); // Re-shrink the scene to it's bounding contents - + this->scene()->setSceneRect(QRectF()); QDesktopServices::openUrl(QUrl::fromLocalFile(filePath)); } @@ -2366,13 +2450,13 @@ void GraphViewer::contextMenuEvent(QContextMenuEvent * event) setLoopClosureOutlierThr(value); } } - else if(r == aChangeMaxLinkLength) + else if(r == aChangeMinLinkLength) { bool ok; - double value = QInputDialog::getDouble(this, tr("Maximum link length to be shown"), tr("Value (m)"), _maxLinkLength, 0.0, 1000.0, 3, &ok); + double value = QInputDialog::getDouble(this, tr("Minimum link length to be shown"), tr("Value (m)"), _minLinkLength, 0.0, 1000.0, 3, &ok); if(ok) { - setMaxLinkLength(value); + setMinLinkLength(value); } } else if(r == aChangeNodeColor || diff --git a/guilib/src/GuiLib.qrc b/guilib/src/GuiLib.qrc index 5880443f..710b690b 100644 --- a/guilib/src/GuiLib.qrc +++ b/guilib/src/GuiLib.qrc @@ -44,6 +44,7 @@ images/oakd.png images/oakd_lite.png images/astra.png + images/astra2.png images/oakdpro.png images/seer_sense_DS80.png diff --git a/guilib/src/ImageView.cpp b/guilib/src/ImageView.cpp index 73bc9413..84f03687 100644 --- a/guilib/src/ImageView.cpp +++ b/guilib/src/ImageView.cpp @@ -1141,14 +1141,14 @@ void ImageView::mouseMoveEvent(QMouseEvent * event) if(_mouseTracking->isChecked() && !_graphicsView->scene()->sceneRect().isNull() && !_image.isNull() && - !_imageDepthCv.empty() &&(_imageDepthCv.type() == CV_16UC1 || _imageDepthCv.type() == CV_32FC1)) + !_imageDepthCv.empty() && (_imageDepthCv.type() == CV_16UC1 || _imageDepthCv.type() == CV_32FC1)) { float scale, offsetX, offsetY; computeScaleOffsets(this->rect(), scale, offsetX, offsetY); float u = (event->pos().x() - offsetX) / scale; float v = (event->pos().y() - offsetY) / scale; float depthScale = 1; - if(_image.width() > _imageDepthCv.cols) + if(_image.width() != _imageDepthCv.cols) { depthScale = float(_imageDepthCv.cols) / float(_image.width()); } @@ -1255,11 +1255,11 @@ void ImageView::setFeatures(const std::multimap & refWords, c { if (xRatio > 0 && yRatio > 0) { - addFeature(iter->first, iter->second, util2d::getDepth(depth, iter->second.pt.x*xRatio, iter->second.pt.y*yRatio, false), color); + addFeature(iter->first, iter->second, util2d::getDepth(depth, iter->second.pt.x*xRatio, iter->second.pt.y*yRatio, false), iter->first<0?Qt::gray:color); } else { - addFeature(iter->first, iter->second, 0, color); + addFeature(iter->first, iter->second, 0, iter->first<0?Qt::gray:color); } } @@ -1382,6 +1382,16 @@ void ImageView::setImageDepth(const cv::Mat & imageDepth, const cv::Mat & imageD // convert the depth values in height values cv::Mat depthInBaseFrame = _imageDepthCv.clone(); int subImageWidth = _imageDepthCv.cols / _models.size(); + std::vector models; // scale model to size of depth image if needed + for(const auto & model: _models) { + UASSERT(subImageWidth <= model.imageWidth()); + if(subImageWidth < model.imageWidth()) { + models.push_back(model.scaled(float(subImageWidth)/float(model.imageWidth()))); + } + else { + models.push_back(model); + } + } if(depthInBaseFrame.type() == CV_16UC1) { for(int v=0; v(v); @@ -1390,9 +1400,9 @@ void ImageView::setImageDepth(const cv::Mat & imageDepth, const cv::Mat & imageD if(val > 0) { cv::Point3f pt; int cameraIndex = u/subImageWidth; - UASSERT(cameraIndex>=0 && cameraIndex < (int)_models.size() && subImageWidth == _models[cameraIndex].imageWidth()); - _models[cameraIndex].project(u,v,float(val)/1000.0f, pt.x, pt.y, pt.z); - pt = util3d::transformPoint(pt, _models[cameraIndex].localTransform()); + UASSERT(cameraIndex>=0 && cameraIndex < (int)models.size() && subImageWidth == models[cameraIndex].imageWidth()); + models[cameraIndex].project(u-(cameraIndex*subImageWidth),v,float(val)/1000.0f, pt.x, pt.y, pt.z); + pt = util3d::transformPoint(pt, models[cameraIndex].localTransform()); val = (unsigned short)(pt.z*1000.0f); } } @@ -1406,9 +1416,9 @@ void ImageView::setImageDepth(const cv::Mat & imageDepth, const cv::Mat & imageD if(val > 0) { cv::Point3f pt; int cameraIndex = u/subImageWidth; - UASSERT(cameraIndex>=0 && cameraIndex < (int)_models.size() && subImageWidth == _models[cameraIndex].imageWidth()); - _models[cameraIndex].project(u,v,val, pt.x, pt.y, pt.z); - pt = util3d::transformPoint(pt, _models[cameraIndex].localTransform()); + UASSERT(cameraIndex>=0 && cameraIndex < (int)models.size() && subImageWidth == models[cameraIndex].imageWidth()); + models[cameraIndex].project(u-(cameraIndex*subImageWidth),v,val, pt.x, pt.y, pt.z); + pt = util3d::transformPoint(pt, models[cameraIndex].localTransform()); val = pt.z; } } @@ -1438,8 +1448,8 @@ void ImageView::setImageDepth(const QImage & imageDepth, const QImage & imageDep UASSERT(_imageDepth.width() && _imageDepth.height()); if( _image.width() > 0 && - _image.width() > _imageDepth.width() && - _image.height() > _imageDepth.height()) + _image.width() != _imageDepth.width() && + _image.height() != _imageDepth.height()) { // scale depth to rgb _imageDepth = _imageDepth.scaled(_image.size()); diff --git a/guilib/src/MainWindow.cpp b/guilib/src/MainWindow.cpp index 86e69244..8daf26b2 100644 --- a/guilib/src/MainWindow.cpp +++ b/guilib/src/MainWindow.cpp @@ -470,6 +470,7 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent, bool sh connect(_ui->actionDepthAI_oakdlite, SIGNAL(triggered()), this, SLOT(selectDepthAIOAKDLite())); connect(_ui->actionDepthAI_oakdpro, SIGNAL(triggered()), this, SLOT(selectDepthAIOAKDPro())); connect(_ui->actionXvisio_SeerSense, SIGNAL(triggered()), this, SLOT(selectXvisioSeerSense())); + connect(_ui->actionOrbbecSDK_astra2, SIGNAL(triggered()), this, SLOT(selectOrbbecSDK())); connect(_ui->actionVelodyne_VLP_16, SIGNAL(triggered()), this, SLOT(selectVLP16())); _ui->actionFreenect->setEnabled(CameraFreenect::available()); _ui->actionOpenNI_PCL->setEnabled(CameraOpenni::available()); @@ -499,6 +500,7 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent, bool sh _ui->actionDepthAI_oakdlite->setEnabled(CameraDepthAI::available()); _ui->actionDepthAI_oakdpro->setEnabled(CameraDepthAI::available()); _ui->actionXvisio_SeerSense->setEnabled(CameraSeerSense::available()); + _ui->actionOrbbecSDK_astra2->setEnabled(CameraOrbbecSDK::available()); this->updateSelectSourceMenu(); connect(_ui->actionPreferences, SIGNAL(triggered()), this, SLOT(openPreferences())); @@ -583,6 +585,7 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent, bool sh // Apply state this->changeState(kIdle); this->applyPrefSettings(PreferencesDialog::kPanelAll); + applyPrefSettings(parameters, false); _ui->statsToolBox->setNewFigureMaxItems(50); _ui->statsToolBox->setWorkingDirectory(_preferencesDialog->getWorkingDirectory()); @@ -707,10 +710,6 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent, bool sh this->loadFigures(); connect(_ui->statsToolBox, SIGNAL(figuresSetupChanged()), this, SLOT(configGUIModified())); - // update loop closure viewer parameters - _loopClosureViewer->setDecimation(_preferencesDialog->getCloudDecimation(0)); - _loopClosureViewer->setMaxDepth(_preferencesDialog->getCloudMaxDepth(0)); - if (splash) { splash->close(); @@ -753,7 +752,7 @@ void MainWindow::setupMainLayout(bool vertical) std::map MainWindow::currentVisiblePosesMap() const { - return _ui->widget_mapVisibility->getVisiblePoses(); + return !_ui->widget_mapVisibility->isEmpty()?_ui->widget_mapVisibility->getVisiblePoses():_currentPosesMap; } void MainWindow::setCloudViewer(rtabmap::CloudViewer * cloudViewer) @@ -1586,63 +1585,61 @@ void MainWindow::processOdometry(const rtabmap::OdometryEvent & odom, bool dataI { _odometryReceived = true; // update camera position - if(data->cameraModels().size() && data->cameraModels()[0].isValidForProjection()) + if(_cloudViewer->isVisible()) { - _cloudViewer->updateCameraFrustums(_odometryCorrection*odom.pose(), data->cameraModels()); - } - else if(data->stereoCameraModels().size() && data->stereoCameraModels()[0].isValidForProjection()) - { - _cloudViewer->updateCameraFrustums(_odometryCorrection*odom.pose(), data->stereoCameraModels()); - } - else if(!data->laserScanRaw().isEmpty() || - !data->laserScanCompressed().isEmpty()) - { - Transform scanLocalTransform; - if(!data->laserScanRaw().isEmpty()) + if(data->cameraModels().size() && data->cameraModels()[0].isValidForProjection()) { - scanLocalTransform = data->laserScanRaw().localTransform(); + _cloudViewer->updateCameraFrustums(_odometryCorrection*odom.pose(), data->cameraModels()); + } + else if(data->stereoCameraModels().size() && data->stereoCameraModels()[0].isValidForProjection()) + { + _cloudViewer->updateCameraFrustums(_odometryCorrection*odom.pose(), data->stereoCameraModels()); + } + else if(!data->laserScanRaw().isEmpty() || + !data->laserScanCompressed().isEmpty()) + { + Transform scanLocalTransform; + if(!data->laserScanRaw().isEmpty()) + { + scanLocalTransform = data->laserScanRaw().localTransform(); + } + else + { + scanLocalTransform = data->laserScanCompressed().localTransform(); + } + //fake frustum + CameraModel model( + 2, + 2, + 2, + 1.5, + scanLocalTransform*CameraModel::opticalRotation(), + 0, + cv::Size(4,3)); + _cloudViewer->updateCameraFrustum(_odometryCorrection*odom.pose(), model); + + } +#if PCL_VERSION_COMPARE(>=, 1, 7, 2) + if(_preferencesDialog->isFramesShown()) + { + _cloudViewer->addOrUpdateLine("odom_to_base_link", _odometryCorrection, _odometryCorrection*odom.pose(), qRgb(255, 128, 0), true, false); } else { - scanLocalTransform = data->laserScanCompressed().localTransform(); + _cloudViewer->removeLine("odom_to_base_link"); } - //fake frustum - CameraModel model( - 2, - 2, - 2, - 1.5, - scanLocalTransform*CameraModel::opticalRotation(), - 0, - cv::Size(4,3)); - _cloudViewer->updateCameraFrustum(_odometryCorrection*odom.pose(), model); - - } -#if PCL_VERSION_COMPARE(>=, 1, 7, 2) - if(_preferencesDialog->isFramesShown()) - { - _cloudViewer->addOrUpdateLine("odom_to_base_link", _odometryCorrection, _odometryCorrection*odom.pose(), qRgb(255, 128, 0), true, false); - } - else - { - _cloudViewer->removeLine("odom_to_base_link"); - } #endif - _cloudViewer->updateCameraTargetPosition(_odometryCorrection*odom.pose()); - UDEBUG("Time Update Pose: %fs", time.ticks()); - } - - _cloudViewer->refreshView(); - - if(_ui->graphicsView_graphView->isVisible()) - { - if(!pose.isNull() && !odom.pose().isNull()) + _cloudViewer->updateCameraTargetPosition(_odometryCorrection*odom.pose()); + UDEBUG("Time Update Pose: %fs", time.ticks()); + _cloudViewer->refreshView(); + } + if(_ui->graphicsView_graphView->isVisible()) { _ui->graphicsView_graphView->updateReferentialPosition(_odometryCorrection*odom.pose()); _ui->graphicsView_graphView->update(); UDEBUG("Time Update graphview: %fs", time.ticks()); } - } + } if(_ui->dockWidget_odometry->isVisible() && !data->imageRaw().empty()) @@ -1675,7 +1672,7 @@ void MainWindow::processOdometry(const rtabmap::OdometryEvent & odom, bool dataI odom.info().type == (int)Odometry::kTypeViso2 || odom.info().type == (int)Odometry::kTypeFovis || odom.info().type == (int)Odometry::kTypeMSCKF || - odom.info().type == (int)Odometry::kTypeVINS || + odom.info().type == (int)Odometry::kTypeVINSFusion || odom.info().type == (int)Odometry::kTypeOpenVINS) { std::vector kpts; @@ -1725,7 +1722,6 @@ void MainWindow::processOdometry(const rtabmap::OdometryEvent & odom, bool dataI if( odom.info().type == (int)Odometry::kTypeF2M || odom.info().type == (int)Odometry::kTypeORBSLAM || odom.info().type == (int)Odometry::kTypeMSCKF || - odom.info().type == (int)Odometry::kTypeVINS || odom.info().type == (int)Odometry::kTypeOpenVINS) { if(_ui->imageView_odometry->isFeaturesShown() && !_preferencesDialog->isOdomOnlyInliersShown()) @@ -1742,6 +1738,7 @@ void MainWindow::processOdometry(const rtabmap::OdometryEvent & odom, bool dataI } if((odom.info().type == (int)Odometry::kTypeF2F || odom.info().type == (int)Odometry::kTypeViso2 || + odom.info().type == (int)Odometry::kTypeVINSFusion || odom.info().type == (int)Odometry::kTypeFovis) && odom.info().refCorners.size()) { if(_ui->imageView_odometry->isFeaturesShown() || _ui->imageView_odometry->isLinesShown()) @@ -1795,6 +1792,7 @@ void MainWindow::processOdometry(const rtabmap::OdometryEvent & odom, bool dataI //Process info if(_preferencesDialog->isCacheSavedInFigures() || _ui->statsToolBox->isVisible()) { + UASSERT(odom.info().reg.covariance.total() == 36 && odom.info().reg.covariance.type() == CV_64FC1); double linVar = uMax3(odom.info().reg.covariance.at(0,0), odom.info().reg.covariance.at(1,1)>=9999?0:odom.info().reg.covariance.at(1,1), odom.info().reg.covariance.at(2,2)>=9999?0:odom.info().reg.covariance.at(2,2)); double angVar = uMax3(odom.info().reg.covariance.at(3,3)>=9999?0:odom.info().reg.covariance.at(3,3), odom.info().reg.covariance.at(4,4)>=9999?0:odom.info().reg.covariance.at(4,4), odom.info().reg.covariance.at(5,5)); _ui->statsToolBox->updateStat("Odometry/Inliers/", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), (float)odom.info().reg.inliers, _preferencesDialog->isCacheSavedInFigures()); @@ -2028,13 +2026,14 @@ void MainWindow::processStats(const rtabmap::Statistics & stat) { // make sure data are uncompressed // We don't need to uncompress images if we don't show them - bool uncompressImages = !signature.sensorData().imageCompressed().empty() && ( - _ui->imageView_source->isVisible() || - (_loopClosureViewer->isVisible() && - !signature.sensorData().depthOrRightCompressed().empty()) || - (_cloudViewer->isVisible() && - _preferencesDialog->isCloudsShown(0) && - !signature.sensorData().depthOrRightCompressed().empty())); + bool uncompressImages = (!signature.sensorData().imageCompressed().empty() && + ((_ui->imageView_source->isVisible() && _ui->imageView_source->isImageShown()) || + _loopClosureViewer->isVisible())) + || + (!signature.sensorData().depthOrRightCompressed().empty() && + ((_ui->imageView_loopClosure->isVisible() && _ui->imageView_loopClosure->isImageShown()) || + (_cloudViewer->isVisible() && _preferencesDialog->isCloudsShown(0)))); + bool uncompressScan = !signature.sensorData().laserScanCompressed().isEmpty() && ( _loopClosureViewer->isVisible() || (_cloudViewer->isVisible() && _preferencesDialog->isScansShown(0))); @@ -2108,23 +2107,31 @@ void MainWindow::processStats(const rtabmap::Statistics & stat) } // For intermediate empty nodes, keep latest image shown + bool rehearsedSimilarity = (float)uValue(stat.data(), Statistics::kMemoryRehearsal_id(), 0.0f) != 0.0f; if(signature.getWeight() >= 0) { _ui->imageView_source->clear(); _ui->imageView_loopClosure->clear(); - if(signature.sensorData().imageRaw().empty() && signature.getWords().empty()) + // To see colors + QRect rect(0,0,640,480); // default + if(signature.sensorData().cameraModels().size() && signature.sensorData().cameraModels().at(0).imageSize()!=cv::Size()) { - // To see colors - _ui->imageView_source->setSceneRect(QRect(0,0,640,480)); + rect.setWidth(signature.sensorData().cameraModels().at(0).imageWidth()*signature.sensorData().cameraModels().size()); + rect.setHeight(signature.sensorData().cameraModels().at(0).imageHeight()); } + else if(signature.sensorData().stereoCameraModels().size() && signature.sensorData().stereoCameraModels().at(0).left().imageSize()!=cv::Size()) + { + rect.setWidth(signature.sensorData().stereoCameraModels().at(0).left().imageWidth()*signature.sensorData().stereoCameraModels().size()); + rect.setHeight(signature.sensorData().stereoCameraModels().at(0).left().imageHeight()); + } + _ui->imageView_source->setSceneRect(rect); _ui->imageView_source->setBackgroundColor(_ui->imageView_source->getDefaultBackgroundColor()); _ui->imageView_loopClosure->setBackgroundColor(_ui->imageView_loopClosure->getDefaultBackgroundColor()); _ui->label_matchId->clear(); - bool rehearsedSimilarity = (float)uValue(stat.data(), Statistics::kMemoryRehearsal_id(), 0.0f) != 0.0f; int proximityTimeDetections = (int)uValue(stat.data(), Statistics::kProximityTime_detections(), 0.0f); bool scanMatchingSuccess = (bool)uValue(stat.data(), Statistics::kNeighborLinkRefiningAccepted(), 0.0f); _ui->label_stats_imageNumber->setText(QString("%1 [%2]").arg(stat.refImageId()).arg(refMapId)); @@ -2258,22 +2265,27 @@ void MainWindow::processStats(const rtabmap::Statistics & stat) QMap::iterator iter = _cachedSignatures.find(shownLoopId); if(iter != _cachedSignatures.end()) { - // uncompress after copy to avoid keeping uncompressed data in memory loopSignature = iter.value(); - bool uncompressImages = !loopSignature.sensorData().imageCompressed().empty() && ( - _ui->imageView_source->isVisible() || - (_loopClosureViewer->isVisible() && - !loopSignature.sensorData().depthOrRightCompressed().empty())); - bool uncompressScan = _loopClosureViewer->isVisible() && - !loopSignature.sensorData().laserScanCompressed().isEmpty(); - if(uncompressImages || uncompressScan) + + if((_ui->imageView_loopClosure->isVisible() && (_ui->imageView_loopClosure->isImageShown() || _ui->imageView_loopClosure->isImageDepthShown())) || + _loopClosureViewer->isVisible()) { - cv::Mat tmpRGB, tmpDepth; - LaserScan tmpScan; - loopSignature.sensorData().uncompressData( - uncompressImages?&tmpRGB:0, - uncompressImages?&tmpDepth:0, - uncompressScan?&tmpScan:0); + // uncompress after copy to avoid keeping uncompressed data in memory + bool uncompressImages = !loopSignature.sensorData().imageCompressed().empty() && ( + (_ui->imageView_loopClosure->isVisible() && (_ui->imageView_loopClosure->isImageShown() || _ui->imageView_loopClosure->isImageDepthShown())) || + (_loopClosureViewer->isVisible() && + !loopSignature.sensorData().depthOrRightCompressed().empty())); + bool uncompressScan = _loopClosureViewer->isVisible() && + !loopSignature.sensorData().laserScanCompressed().isEmpty(); + if(uncompressImages || uncompressScan) + { + cv::Mat tmpRGB, tmpDepth; + LaserScan tmpScan; + loopSignature.sensorData().uncompressData( + uncompressImages?&tmpRGB:0, + uncompressImages?&tmpDepth:0, + uncompressScan?&tmpScan:0); + } } } } @@ -2291,14 +2303,14 @@ void MainWindow::processStats(const rtabmap::Statistics & stat) !loopSignature.sensorData().imageRaw().empty() || signature.getWords().size()) { - cv::Mat refImage = signature.sensorData().imageRaw(); - cv::Mat loopImage = loopSignature.sensorData().imageRaw(); + cv::Mat refImage = _ui->imageView_source->isImageShown()?signature.sensorData().imageRaw():cv::Mat(); + cv::Mat loopImage = _ui->imageView_loopClosure->isImageShown()?loopSignature.sensorData().imageRaw():cv::Mat(); if( _preferencesDialog->isMarkerDetection() && _preferencesDialog->isLandmarksShown()) { //draw markers - if(!signature.getLandmarks().empty()) + if(!signature.getLandmarks().empty() && !refImage.empty()) { if(refImage.channels() == 1) { @@ -2312,7 +2324,7 @@ void MainWindow::processStats(const rtabmap::Statistics & stat) } drawLandmarks(refImage, signature); } - if(!loopSignature.getLandmarks().empty()) + if(!loopSignature.getLandmarks().empty() && !loopImage.empty()) { if(loopImage.channels() == 1) { @@ -2342,42 +2354,37 @@ void MainWindow::processStats(const rtabmap::Statistics & stat) { _ui->imageView_source->setImage(img); } - if(!signature.sensorData().depthOrRightRaw().empty()) + if(!signature.sensorData().depthOrRightRaw().empty() && _ui->imageView_source->isImageDepthShown()) { _ui->imageView_source->setImageDepth(signature.sensorData().depthOrRightRaw(), signature.sensorData().depthConfidenceRaw()); } - if(img.isNull() && signature.sensorData().depthOrRightRaw().empty()) - { - QRect sceneRect; - if(signature.sensorData().cameraModels().size()) - { - for(unsigned int i=0; iimageView_source->setSceneRect(sceneRect); - } - } if(!lcImg.isNull()) { _ui->imageView_loopClosure->setImage(lcImg); } - if(!loopSignature.sensorData().depthOrRightRaw().empty()) + if(!loopSignature.sensorData().depthOrRightRaw().empty() && _ui->imageView_loopClosure->isImageDepthShown()) { _ui->imageView_loopClosure->setImageDepth(loopSignature.sensorData().depthOrRightRaw(), loopSignature.sensorData().depthConfidenceRaw()); } + + if(lcImg.isNull()) + { + QRect sceneRect; + if(loopSignature.sensorData().cameraModels().size() && loopSignature.sensorData().cameraModels().at(0).imageSize()!=cv::Size()) + { + rect.setWidth(loopSignature.sensorData().cameraModels().at(0).imageWidth()*loopSignature.sensorData().cameraModels().size()); + rect.setHeight(loopSignature.sensorData().cameraModels().at(0).imageHeight()); + } + else if(loopSignature.sensorData().stereoCameraModels().size() && loopSignature.sensorData().stereoCameraModels().at(0).left().imageSize()!=cv::Size()) + { + rect.setWidth(loopSignature.sensorData().stereoCameraModels().at(0).left().imageWidth()*loopSignature.sensorData().stereoCameraModels().size()); + rect.setHeight(loopSignature.sensorData().stereoCameraModels().at(0).left().imageHeight()); + } + if(sceneRect.isValid()) + { + _ui->imageView_loopClosure->setSceneRect(sceneRect); + } + } if(_ui->imageView_loopClosure->sceneRect().isNull()) { _ui->imageView_loopClosure->setSceneRect(_ui->imageView_source->sceneRect()); @@ -2391,18 +2398,37 @@ void MainWindow::processStats(const rtabmap::Statistics & stat) UDEBUG("time= %d ms (update detection imageviews)", time.restart()); - // do it after scaling - std::multimap wordsA; - std::multimap wordsB; - for(std::map::const_iterator iter=signature.getWords().begin(); iter!=signature.getWords().end(); ++iter) + if(_ui->imageView_source->isFeaturesShown() || _ui->imageView_loopClosure->isFeaturesShown() || + (_ui->imageView_source->isLinesShown() && _ui->imageView_loopClosure->isLinesShown())) { - wordsA.insert(wordsA.end(), std::make_pair(iter->first, signature.getWordsKpts()[iter->second])); + // do it after scaling + std::multimap wordsA; + std::multimap wordsB; + if(signature.getWords().size() == signature.getWordsKpts().size() && + (_ui->imageView_source->isFeaturesShown() || (_ui->imageView_source->isLinesShown() && _ui->imageView_loopClosure->isLinesShown()))) + { + for(std::map::const_iterator iter=signature.getWords().begin(); iter!=signature.getWords().end(); ++iter) + { + wordsA.insert(wordsA.end(), std::make_pair(iter->first, signature.getWordsKpts()[iter->second])); + } + } + if(loopSignature.getWords().size() == loopSignature.getWordsKpts().size() && + (_ui->imageView_loopClosure->isFeaturesShown() || (_ui->imageView_source->isLinesShown() && _ui->imageView_loopClosure->isLinesShown()))) + { + for(std::map::const_iterator iter=loopSignature.getWords().begin(); iter!=loopSignature.getWords().end(); ++iter) + { + wordsB.insert(wordsB.end(), std::make_pair(iter->first, loopSignature.getWordsKpts()[iter->second])); + } + } + this->drawKeypoints(wordsA, wordsB); } - for(std::map::const_iterator iter=loopSignature.getWords().begin(); iter!=loopSignature.getWords().end(); ++iter) - { - wordsB.insert(wordsB.end(), std::make_pair(iter->first, loopSignature.getWordsKpts()[iter->second])); + else { + _ui->imageView_source->clearFeatures(); + _ui->imageView_loopClosure->clearFeatures(); + _ui->imageView_source->clearLines(); + _ui->imageView_loopClosure->clearLines(); + _lastIds.clear(); } - this->drawKeypoints(wordsA, wordsB); UDEBUG("time= %d ms (draw keypoints)", time.restart()); @@ -2429,6 +2455,18 @@ void MainWindow::processStats(const rtabmap::Statistics & stat) UDEBUG("time= %d ms (update loop closure viewer)", time.restart()); } } + else if(rehearsedSimilarity) + { + _ui->imageView_source->setBackgroundColor(Qt::darkBlue); + } + else if(smallMovement) + { + _ui->imageView_source->setBackgroundColor(Qt::gray); + } + else if(fastMovement) + { + _ui->imageView_source->setBackgroundColor(Qt::magenta); + } // PDF AND LIKELIHOOD if(!stat.posterior().empty() && _ui->dockWidget_posterior->isVisible()) @@ -2511,43 +2549,45 @@ void MainWindow::processStats(const rtabmap::Statistics & stat) UDEBUG("%d %d %d", poses.size(), poses.size()?poses.rbegin()->first:0, stat.refImageId()); if(!_odometryReceived && poses.size() && poses.rbegin()->first == stat.refImageId()) { - if(poses.rbegin()->first == stat.getLastSignatureData().id()) + if(_cloudViewer->isVisible()) { - if(stat.getLastSignatureData().sensorData().cameraModels().size() && stat.getLastSignatureData().sensorData().cameraModels()[0].isValidForProjection()) + if(poses.rbegin()->first == stat.getLastSignatureData().id()) { - _cloudViewer->updateCameraFrustums(poses.rbegin()->second, stat.getLastSignatureData().sensorData().cameraModels()); - } - else if(stat.getLastSignatureData().sensorData().stereoCameraModels().size() && stat.getLastSignatureData().sensorData().stereoCameraModels()[0].isValidForProjection()) - { - _cloudViewer->updateCameraFrustums(poses.rbegin()->second, stat.getLastSignatureData().sensorData().stereoCameraModels()); - } - else if(!stat.getLastSignatureData().sensorData().laserScanRaw().isEmpty() || - !stat.getLastSignatureData().sensorData().laserScanCompressed().isEmpty()) - { - Transform scanLocalTransform; - if(!stat.getLastSignatureData().sensorData().laserScanRaw().isEmpty()) + if(stat.getLastSignatureData().sensorData().cameraModels().size() && stat.getLastSignatureData().sensorData().cameraModels()[0].isValidForProjection()) { - scanLocalTransform = stat.getLastSignatureData().sensorData().laserScanRaw().localTransform(); + _cloudViewer->updateCameraFrustums(poses.rbegin()->second, stat.getLastSignatureData().sensorData().cameraModels()); } - else + else if(stat.getLastSignatureData().sensorData().stereoCameraModels().size() && stat.getLastSignatureData().sensorData().stereoCameraModels()[0].isValidForProjection()) { - scanLocalTransform = stat.getLastSignatureData().sensorData().laserScanCompressed().localTransform(); + _cloudViewer->updateCameraFrustums(poses.rbegin()->second, stat.getLastSignatureData().sensorData().stereoCameraModels()); + } + else if(!stat.getLastSignatureData().sensorData().laserScanRaw().isEmpty() || + !stat.getLastSignatureData().sensorData().laserScanCompressed().isEmpty()) + { + Transform scanLocalTransform; + if(!stat.getLastSignatureData().sensorData().laserScanRaw().isEmpty()) + { + scanLocalTransform = stat.getLastSignatureData().sensorData().laserScanRaw().localTransform(); + } + else + { + scanLocalTransform = stat.getLastSignatureData().sensorData().laserScanCompressed().localTransform(); + } + //fake frustum + CameraModel model( + 2, + 2, + 2, + 1.5, + scanLocalTransform*CameraModel::opticalRotation(), + 0, + cv::Size(4,3)); + _cloudViewer->updateCameraFrustum(poses.rbegin()->second, model); } - //fake frustum - CameraModel model( - 2, - 2, - 2, - 1.5, - scanLocalTransform*CameraModel::opticalRotation(), - 0, - cv::Size(4,3)); - _cloudViewer->updateCameraFrustum(poses.rbegin()->second, model); } + _cloudViewer->updateCameraTargetPosition(poses.rbegin()->second); } - _cloudViewer->updateCameraTargetPosition(poses.rbegin()->second); - if(_ui->graphicsView_graphView->isVisible()) { _ui->graphicsView_graphView->updateReferentialPosition(poses.rbegin()->second); @@ -2684,7 +2724,6 @@ void MainWindow::processStats(const rtabmap::Statistics & stat) Signature & s = *_cachedSignatures.find(stat.refImageId()); _cachedMemoryUsage -= s.sensorData().getMemoryUsed(); s.sensorData().clearRawData(); - s.sensorData().clearOccupancyGridRaw(); _cachedMemoryUsage += s.sensorData().getMemoryUsed(); } @@ -2842,6 +2881,7 @@ void MainWindow::updateMapCloud( _progressDialog->appendText(tr("Map update: %1 nodes shown of %2 (cloud filtering is on)").arg(poses.size()).arg(nodePoses.size())); QApplication::processEvents(); } + UDEBUG("Filtered poses"); } else { @@ -2849,27 +2889,33 @@ void MainWindow::updateMapCloud( mapIds = mapIdsIn; } - std::map posesMask; - for(std::map::const_iterator iter = nodePoses.begin(); iter!=nodePoses.end(); ++iter) + if(_ui->widget_mapVisibility->isVisible()) { - posesMask.insert(posesMask.end(), std::make_pair(iter->first, poses.find(iter->first) != poses.end())); + std::map posesMask; + for(std::map::const_iterator iter = nodePoses.begin(); iter!=nodePoses.end(); ++iter) + { + posesMask.insert(posesMask.end(), std::make_pair(iter->first, poses.find(iter->first) != poses.end())); + } + _ui->widget_mapVisibility->setMap(nodePoses, posesMask); + UDEBUG("Updated map visibility with %ld poses", nodePoses.size()); + } + else { + _ui->widget_mapVisibility->clear(); } - _ui->widget_mapVisibility->setMap(nodePoses, posesMask); if(groundTruths.size() && _ui->actionAnchor_clouds_to_ground_truth->isChecked()) { + int anchored = 0; for(std::map::iterator iter = poses.begin(); iter!=poses.end(); ++iter) { std::map::const_iterator gtIter = groundTruths.find(iter->first); if(gtIter!=groundTruths.end()) { iter->second = gtIter->second; - } - else - { - UWARN("Not found ground truth pose for node %d", iter->first); + ++anchored; } } + UDEBUG("Anchored %d/%ld poses to ground truth", anchored, poses.size()); } else if(_currentGTPosesMap.size() == 0) { @@ -3041,7 +3087,9 @@ void MainWindow::updateMapCloud( cv::Mat obstacles; cv::Mat empty; + UTimer decompressionTime; jter->sensorData().uncompressDataConst(0, 0, 0, 0, &ground, &obstacles, &empty); + UDEBUG("Uncompressed local occupancy grid of node %d (%f s)", jter->id(), decompressionTime.ticks()); double resolution = jter->sensorData().gridCellSize(); if(_preferencesDialog->getGridUIResolution() > jter->sensorData().gridCellSize()) @@ -3210,21 +3258,27 @@ void MainWindow::updateMapCloud( if(_preferencesDialog->isGroundTruthAligned() && _currentGTPosesMap.size()) { mapToGt = alignPosesToGroundTruth(_currentPosesMap, _currentGTPosesMap).inverse(); + UDEBUG("Aligned poses to ground truth (%ld poses %ld gt poses)", _currentPosesMap.size(), _currentGTPosesMap.size()); } std::map posesWithOdomCache; - + std::set odomCachePosesIds; if(_ui->graphicsView_graphView->isVisible() || ((_preferencesDialog->isGraphsShown() || _preferencesDialog->isFrustumsShown(0)) && _currentPosesMap.size())) { posesWithOdomCache = posesIn; for(std::map::const_iterator iter=odomCachePoses.begin(); iter!=odomCachePoses.end(); ++iter) { - posesWithOdomCache.insert(std::make_pair(iter->first, _odometryCorrection*iter->second)); + if(posesWithOdomCache.insert(std::make_pair(iter->first, _odometryCorrection*iter->second)).second) + { + odomCachePosesIds.insert(iter->first); + } } } - if((_preferencesDialog->isGraphsShown() || _preferencesDialog->isFrustumsShown(0)) && _currentPosesMap.size()) + if( _cloudViewer->isVisible() && + (_preferencesDialog->isGraphsShown() || _preferencesDialog->isFrustumsShown(0)) && + _currentPosesMap.size()) { UTimer timerGraph; // Find all graphs @@ -3270,7 +3324,7 @@ void MainWindow::updateMapCloud( { std::string gtFrustumId = uFormat("f_gt_%d", iter->first); color = Qt::gray; - _cloudViewer->addOrUpdateFrustum(gtFrustumId, _currentGTPosesMap.at(iter->first), t, _cloudViewer->getFrustumScale(), color, model.fovX(), model.fovY()); + _cloudViewer->addOrUpdateFrustum(gtFrustumId, mapToGt*_currentGTPosesMap.at(iter->first), t, _cloudViewer->getFrustumScale(), color, model.fovX(), model.fovY()); } } } @@ -3327,7 +3381,7 @@ void MainWindow::updateMapCloud( } } - UDEBUG("timerGraph=%fs", timerGraph.ticks()); + UDEBUG("timerGraph (CloudViewer)=%fs", timerGraph.ticks()); } UDEBUG("labels.size()=%d", (int)labels.size()); @@ -3477,7 +3531,7 @@ void MainWindow::updateMapCloud( std::multimap constraintsWithOdomCache; constraintsWithOdomCache = constraints; constraintsWithOdomCache.insert(odomCacheConstraints.begin(), odomCacheConstraints.end()); - _ui->graphicsView_graphView->updateGraph(posesWithOdomCache, constraintsWithOdomCache, mapIdsIn, std::map(), uKeysSet(odomCachePoses)); + _ui->graphicsView_graphView->updateGraph(posesWithOdomCache, constraintsWithOdomCache, mapIdsIn, std::map(), odomCachePosesIds); if(_preferencesDialog->isGroundTruthAligned() && !mapToGt.isIdentity()) { std::map gtPoses = _currentGTPosesMap; @@ -4363,7 +4417,7 @@ void MainWindow::createAndAddFeaturesToMap(int nodeId, const Transform & pose, i UASSERT(iter->getWords().size() == iter->getWords3().size()); float maxDepth = _preferencesDialog->getCloudMaxDepth(0); UDEBUG("rgb.channels()=%d"); - if(!iter->getWords3().empty() && !iter->getWordsKpts().empty()) + if(!iter->getWords3().empty() && iter->getWords3().size() == iter->getWordsKpts().size()) { Transform invLocalTransform = Transform::getIdentity(); if(iter.value().sensorData().cameraModels().size() == 1 && @@ -5012,96 +5066,113 @@ void MainWindow::drawKeypoints(const std::multimap & refWords timer.start(); ULOGGER_DEBUG("refWords.size() = %d", refWords.size()); - if(refWords.size()) + _ui->imageView_source->clearFeatures(); + if(_ui->imageView_source->isFeaturesShown()) { - _ui->imageView_source->clearFeatures(); + for(std::multimap::const_iterator iter = refWords.begin(); iter != refWords.end(); ++iter ) + { + int id = iter->first; + QColor color; + if(id<0) + { + // GRAY = NOT QUANTIZED + color = Qt::gray; + } + else if(uContains(loopWords, id)) + { + // PINK = FOUND IN LOOP SIGNATURE + color = Qt::magenta; + } + else if(_lastIds.contains(id)) + { + // BLUE = FOUND IN LAST SIGNATURE + color = Qt::blue; + } + else if(id<=_lastId) + { + // RED = ALREADY EXISTS + color = Qt::red; + } + else if(refWords.count(id) > 1) + { + // YELLOW = NEW and multiple times + color = Qt::yellow; + } + else + { + // GREEN = NEW + color = Qt::green; + } + _ui->imageView_source->addFeature(iter->first, iter->second, 0, color); + } } - for(std::multimap::const_iterator iter = refWords.begin(); iter != refWords.end(); ++iter ) - { - int id = iter->first; - QColor color; - if(id<0) - { - // GRAY = NOT QUANTIZED - color = Qt::gray; - } - else if(uContains(loopWords, id)) - { - // PINK = FOUND IN LOOP SIGNATURE - color = Qt::magenta; - } - else if(_lastIds.contains(id)) - { - // BLUE = FOUND IN LAST SIGNATURE - color = Qt::blue; - } - else if(id<=_lastId) - { - // RED = ALREADY EXISTS - color = Qt::red; - } - else if(refWords.count(id) > 1) - { - // YELLOW = NEW and multiple times - color = Qt::yellow; - } - else - { - // GREEN = NEW - color = Qt::green; - } - _ui->imageView_source->addFeature(iter->first, iter->second, 0, color); - } - ULOGGER_DEBUG("source time = %f s", timer.ticks()); + ULOGGER_DEBUG("source time (shown=%d) = %f s", _ui->imageView_source->isFeaturesShown()?1:0, timer.ticks()); timer.start(); ULOGGER_DEBUG("loopWords.size() = %d", loopWords.size()); QList > uniqueCorrespondences; - if(loopWords.size()) + _ui->imageView_loopClosure->clearFeatures(); + if(_ui->imageView_loopClosure->isFeaturesShown()) { - _ui->imageView_loopClosure->clearFeatures(); - } - for(std::multimap::const_iterator iter = loopWords.begin(); iter != loopWords.end(); ++iter ) - { - int id = iter->first; - QColor color; - if(id<0) + for(std::multimap::const_iterator iter = loopWords.begin(); iter != loopWords.end(); ++iter ) { - // GRAY = NOT QUANTIZED - color = Qt::gray; - } - else if(uContains(refWords, id)) - { - // PINK = FOUND IN LOOP SIGNATURE - color = Qt::magenta; - //To draw lines... get only unique correspondences - if(uValues(refWords, id).size() == 1 && uValues(loopWords, id).size() == 1) + int id = iter->first; + QColor color; + if(id<0) { - const cv::KeyPoint & a = refWords.find(id)->second; - const cv::KeyPoint & b = iter->second; - uniqueCorrespondences.push_back(QPair(a.pt, b.pt)); + // GRAY = NOT QUANTIZED + color = Qt::gray; + } + else if(uContains(refWords, id)) + { + // PINK = FOUND IN LOOP SIGNATURE + color = Qt::magenta; + //To draw lines... get only unique correspondences + if(uValues(refWords, id).size() == 1 && uValues(loopWords, id).size() == 1) + { + const cv::KeyPoint & a = refWords.find(id)->second; + const cv::KeyPoint & b = iter->second; + uniqueCorrespondences.push_back(QPair(a.pt, b.pt)); + } + } + else if(id<=_lastId) + { + // RED = ALREADY EXISTS + color = Qt::red; + } + else if(refWords.count(id) > 1) + { + // YELLOW = NEW and multiple times + color = Qt::yellow; + } + else + { + // GREEN = NEW + color = Qt::green; + } + _ui->imageView_loopClosure->addFeature(iter->first, iter->second, 0, color); + } + } + else if(_ui->imageView_source->isLinesShown() && _ui->imageView_loopClosure->isLinesShown()) + { + for(std::multimap::const_iterator iter = loopWords.begin(); iter != loopWords.end(); ++iter ) + { + int id = iter->first; + if(id>=0 && uContains(refWords, id)) + { + //To draw lines... get only unique correspondences + if(uValues(refWords, id).size() == 1 && uValues(loopWords, id).size() == 1) + { + const cv::KeyPoint & a = refWords.find(id)->second; + const cv::KeyPoint & b = iter->second; + uniqueCorrespondences.push_back(QPair(a.pt, b.pt)); + } } } - else if(id<=_lastId) - { - // RED = ALREADY EXISTS - color = Qt::red; - } - else if(refWords.count(id) > 1) - { - // YELLOW = NEW and multiple times - color = Qt::yellow; - } - else - { - // GREEN = NEW - color = Qt::green; - } - _ui->imageView_loopClosure->addFeature(iter->first, iter->second, 0, color); } + ULOGGER_DEBUG("loop closure time (shown=%d) = %f s", _ui->imageView_loopClosure->isFeaturesShown()?1:0, timer.ticks()); - ULOGGER_DEBUG("loop closure time = %f s", timer.ticks()); - + _lastIds.clear(); if(refWords.size()>0) { if((*refWords.rbegin()).first > _lastId) @@ -5116,52 +5187,51 @@ void MainWindow::drawKeypoints(const std::multimap & refWords #endif } - // Draw lines between corresponding features... - float scaleSource = _ui->imageView_source->viewScale(); - float scaleLoop = _ui->imageView_loopClosure->viewScale(); - UDEBUG("scale source=%f loop=%f", scaleSource, scaleLoop); - // Delta in actual window pixels - float sourceMarginX = (_ui->imageView_source->width() - _ui->imageView_source->sceneRect().width()*scaleSource)/2.0f; - float sourceMarginY = (_ui->imageView_source->height() - _ui->imageView_source->sceneRect().height()*scaleSource)/2.0f; - float loopMarginX = (_ui->imageView_loopClosure->width() - _ui->imageView_loopClosure->sceneRect().width()*scaleLoop)/2.0f; - float loopMarginY = (_ui->imageView_loopClosure->height() - _ui->imageView_loopClosure->sceneRect().height()*scaleLoop)/2.0f; - - float deltaX = 0; - float deltaY = 0; - - if(_preferencesDialog->isVerticalLayoutUsed()) + _ui->imageView_source->clearLines(); + _ui->imageView_loopClosure->clearLines(); + if(_ui->imageView_source->isLinesShown() && _ui->imageView_loopClosure->isLinesShown()) { - deltaY = _ui->label_matchId->height() + _ui->imageView_source->height(); - } - else - { - deltaX = _ui->imageView_source->width(); - } + // Draw lines between corresponding features... + float scaleSource = _ui->imageView_source->viewScale(); + float scaleLoop = _ui->imageView_loopClosure->viewScale(); + UDEBUG("scale source=%f loop=%f", scaleSource, scaleLoop); + // Delta in actual window pixels + float sourceMarginX = (_ui->imageView_source->width() - _ui->imageView_source->sceneRect().width()*scaleSource)/2.0f; + float sourceMarginY = (_ui->imageView_source->height() - _ui->imageView_source->sceneRect().height()*scaleSource)/2.0f; + float loopMarginX = (_ui->imageView_loopClosure->width() - _ui->imageView_loopClosure->sceneRect().width()*scaleLoop)/2.0f; + float loopMarginY = (_ui->imageView_loopClosure->height() - _ui->imageView_loopClosure->sceneRect().height()*scaleLoop)/2.0f; - if(refWords.size() && loopWords.size()) - { - _ui->imageView_source->clearLines(); - _ui->imageView_loopClosure->clearLines(); - } + float deltaX = 0; + float deltaY = 0; - for(QList >::iterator iter = uniqueCorrespondences.begin(); - iter!=uniqueCorrespondences.end(); - ++iter) - { + if(_preferencesDialog->isVerticalLayoutUsed()) + { + deltaY = _ui->label_matchId->height() + _ui->imageView_source->height(); + } + else + { + deltaX = _ui->imageView_source->width(); + } - _ui->imageView_source->addLine( - iter->first.x, - iter->first.y, - (iter->second.x*scaleLoop+loopMarginX+deltaX-sourceMarginX)/scaleSource, - (iter->second.y*scaleLoop+loopMarginY+deltaY-sourceMarginY)/scaleSource, - _ui->imageView_source->getDefaultMatchingLineColor()); + for(QList >::iterator iter = uniqueCorrespondences.begin(); + iter!=uniqueCorrespondences.end(); + ++iter) + { - _ui->imageView_loopClosure->addLine( - (iter->first.x*scaleSource+sourceMarginX-deltaX-loopMarginX)/scaleLoop, - (iter->first.y*scaleSource+sourceMarginY-deltaY-loopMarginY)/scaleLoop, - iter->second.x, - iter->second.y, - _ui->imageView_loopClosure->getDefaultMatchingLineColor()); + _ui->imageView_source->addLine( + iter->first.x, + iter->first.y, + (iter->second.x*scaleLoop+loopMarginX+deltaX-sourceMarginX)/scaleSource, + (iter->second.y*scaleLoop+loopMarginY+deltaY-sourceMarginY)/scaleSource, + _ui->imageView_source->getDefaultMatchingLineColor()); + + _ui->imageView_loopClosure->addLine( + (iter->first.x*scaleSource+sourceMarginX-deltaX-loopMarginX)/scaleLoop, + (iter->first.y*scaleSource+sourceMarginY-deltaY-loopMarginY)/scaleLoop, + iter->second.x, + iter->second.y, + _ui->imageView_loopClosure->getDefaultMatchingLineColor()); + } } _ui->imageView_source->update(); _ui->imageView_loopClosure->update(); @@ -5169,6 +5239,7 @@ void MainWindow::drawKeypoints(const std::multimap & refWords void MainWindow::drawLandmarks(cv::Mat & image, const Signature & signature) { + UDEBUG("%ld landmarks", signature.getLandmarks().size()); for(std::map::const_iterator iter=signature.getLandmarks().begin(); iter!=signature.getLandmarks().end(); ++iter) { // Project in all cameras in which the landmark is visible @@ -5225,6 +5296,9 @@ void MainWindow::drawLandmarks(cv::Mat & image, const Signature & signature) { imagePoints[j].x += i*model.imageWidth(); } + // Make sure the frame origin is visible + valid = imagePoints[0].x >= i*model.imageWidth() && imagePoints[0].x < (i+1)*model.imageWidth() && + imagePoints[0].y >= 0 && imagePoints[0].y < image.rows; } } if(valid) @@ -5331,6 +5405,7 @@ void MainWindow::updateSelectSourceMenu() _ui->actionDepthAI_oakdlite->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcStereoDepthAI); _ui->actionDepthAI_oakdpro->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcStereoDepthAI); _ui->actionXvisio_SeerSense->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcSeerSense); + _ui->actionOrbbecSDK_astra2->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcOrbbecSDK); _ui->actionVelodyne_VLP_16->setChecked(_preferencesDialog->getLidarSourceDriver() == PreferencesDialog::kSrcLidarVLP16); } @@ -5989,7 +6064,7 @@ void MainWindow::startDetection() _imuThread = 0; } - if(!_sensorCapture->odomProvided() && !_preferencesDialog->isOdomDisabled()) + if((!_sensorCapture->odomProvided() || _preferencesDialog->isOdomAsGuessEnabled()) && !_preferencesDialog->isOdomDisabled()) { ParametersMap odomParameters = parameters; if(_preferencesDialog->getOdomRegistrationApproach() < 3) @@ -6047,7 +6122,7 @@ void MainWindow::startDetection() } } - if(_dataRecorder && _sensorCapture && _odomThread) + if(_dataRecorder && _sensorCapture) { UEventsManager::createPipe(_sensorCapture, _dataRecorder, "SensorEvent"); } @@ -6503,6 +6578,7 @@ void MainWindow::showPostProcessingDialog() _postProcessingDialog->iterations(), _postProcessingDialog->interSession(), _postProcessingDialog->intraSession(), + _postProcessingDialog->minGraphDistance(), _postProcessingDialog->isSBA(), _postProcessingDialog->sbaIterations(), _postProcessingDialog->sbaVariance(), @@ -6519,6 +6595,7 @@ void MainWindow::postProcessing( int iterations, bool interSession, bool intraSession, + int minGraphDistance, bool sba, int sbaIterations, double sbaVariance, @@ -6627,6 +6704,7 @@ void MainWindow::postProcessing( { odomMaxInf = graph::getMaxOdomInf(_currentLinksMap); } + std::multimap neigborLinks = graph::filterLinks(_currentLinksMap, Link::kNeighbor, true); std::shared_ptr registration(Registration::create(parameters)); @@ -6645,6 +6723,34 @@ void MainWindow::postProcessing( _progressDialog->appendText(tr("Looking for more loop closures, clustering poses... found %1 clusters.").arg(clusters.size())); QApplication::processEvents(); + if(minGraphDistance > 1) + { + int clustersBefore = clusters.size(); + for(std::multimap::iterator iter=clusters.begin(); iter!=clusters.end();) + { + if(abs(iter->first - iter->second) < minGraphDistance) + { + iter = clusters.erase(iter); + } + else + { + // compute path to know how far we are in terms of graph length + std::list path = graph::computePath(neigborLinks, iter->first, iter->second); + if(!path.empty() && (int)path.size() <= minGraphDistance) + { + iter = clusters.erase(iter); + } + else + { + ++iter; + } + } + } + _progressDialog->appendText(tr("Filtered %1/%2 clusters for too close nodes (below minimum graph distance=%3).") + .arg(clustersBefore-clusters.size()).arg(clustersBefore).arg(minGraphDistance)); + QApplication::processEvents(); + } + int i=0; std::set addedLinks; for(std::multimap::iterator iter=clusters.begin(); iter!= clusters.end() && !_progressCanceled; ++iter, ++i) @@ -6771,10 +6877,6 @@ void MainWindow::postProcessing( } std::multimap linksIn = _currentLinksMap; linksIn.insert(std::make_pair(from, Link(from, to, Link::kUserClosure, transform, information))); - const Link * maxLinearLink = 0; - const Link * maxAngularLink = 0; - float maxLinearError = 0.0f; - float maxAngularError = 0.0f; std::map poses; std::multimap links; UASSERT(_currentPosesMap.find(fromId) != _currentPosesMap.end()); @@ -6789,51 +6891,43 @@ void MainWindow::postProcessing( std::string msg; if(poses.size()) { - float maxLinearErrorRatio = 0.0f; - float maxAngularErrorRatio = 0.0f; - graph::computeMaxGraphErrors( + graph::MaxGraphErrors maxGraphErrors = graph::computeMaxGraphErrors( poses, - links, - maxLinearErrorRatio, - maxAngularErrorRatio, - maxLinearError, - maxAngularError, - &maxLinearLink, - &maxAngularLink); - if(maxLinearLink) + links); + if(maxGraphErrors.linearLink.isValid()) { - UINFO("Max optimization linear error = %f m (link %d->%d)", maxLinearError, maxLinearLink->from(), maxLinearLink->to()); - if(maxLinearErrorRatio > optimizeMaxError) + UINFO("Max optimization linear error = %f m (link %d->%d)", maxGraphErrors.linear, maxGraphErrors.linearLink.from(), maxGraphErrors.linearLink.to()); + if(maxGraphErrors.linearRatio > optimizeMaxError) { 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()), + maxGraphErrors.linear, + maxGraphErrors.linearLink.from(), + maxGraphErrors.linearLink.to(), + maxGraphErrors.linearRatio, + sqrt(maxGraphErrors.linearLink.transVariance()), Parameters::kRGBDOptimizeMaxError().c_str(), optimizeMaxError); } } - else if(maxAngularLink) + else if(maxGraphErrors.angularLink.isValid()) { - UINFO("Max optimization angular error = %f deg (link %d->%d)", maxAngularError*180.0f/M_PI, maxAngularLink->from(), maxAngularLink->to()); - if(maxAngularErrorRatio > optimizeMaxError) + UINFO("Max optimization angular error = %f deg (link %d->%d)", maxGraphErrors.angular*180.0f/M_PI, maxGraphErrors.angularLink.from(), maxGraphErrors.angularLink.to()); + if(maxGraphErrors.angularRatio > optimizeMaxError) { 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()), + maxGraphErrors.angular*180.0f/M_PI, + maxGraphErrors.angularLink.from(), + maxGraphErrors.angularLink.to(), + maxGraphErrors.angularRatio, + sqrt(maxGraphErrors.angularLink.rotVariance()), Parameters::kRGBDOptimizeMaxError().c_str(), optimizeMaxError); } @@ -7207,6 +7301,11 @@ void MainWindow::selectK4A() _preferencesDialog->selectSourceDriver(PreferencesDialog::kSrcK4A); } +void MainWindow::selectOrbbecSDK() +{ + _preferencesDialog->selectSourceDriver(PreferencesDialog::kSrcOrbbecSDK); +} + void MainWindow::selectRealSense() { _preferencesDialog->selectSourceDriver(PreferencesDialog::kSrcRealSense); @@ -7981,7 +8080,7 @@ void MainWindow::exportClouds() return; } - std::map poses = _ui->widget_mapVisibility->getVisiblePoses(); + std::map poses = !_ui->widget_mapVisibility->isEmpty()?_ui->widget_mapVisibility->getVisiblePoses():_currentPosesMap; // Use ground truth poses if current clouds are using them if(_currentGTPosesMap.size() && _ui->actionAnchor_clouds_to_ground_truth->isChecked()) @@ -8018,7 +8117,7 @@ void MainWindow::viewClouds() return; } - std::map poses = _ui->widget_mapVisibility->getVisiblePoses(); + std::map poses = !_ui->widget_mapVisibility->isEmpty()?_ui->widget_mapVisibility->getVisiblePoses():_currentPosesMap; // Use ground truth poses if current clouds are using them if(_currentGTPosesMap.size() && _ui->actionAnchor_clouds_to_ground_truth->isChecked()) @@ -8094,7 +8193,7 @@ void MainWindow::exportImages() QMessageBox::warning(this, tr("Export images..."), tr("Cannot export images, the cache is empty!")); return; } - std::map poses = _ui->widget_mapVisibility->getVisiblePoses(); + std::map poses = !_ui->widget_mapVisibility->isEmpty()?_ui->widget_mapVisibility->getVisiblePoses():_currentPosesMap; if(poses.empty()) { @@ -8311,7 +8410,7 @@ void MainWindow::exportBundlerFormat() return; } - std::map posesIn = _ui->widget_mapVisibility->getVisiblePoses(); + std::map posesIn = !_ui->widget_mapVisibility->isEmpty()?_ui->widget_mapVisibility->getVisiblePoses():_currentPosesMap; // Use ground truth poses if current clouds are using them if(_currentGTPosesMap.size() && _ui->actionAnchor_clouds_to_ground_truth->isChecked()) diff --git a/guilib/src/MultiSessionLocWidget.cpp b/guilib/src/MultiSessionLocWidget.cpp index 73549677..5fb254f8 100644 --- a/guilib/src/MultiSessionLocWidget.cpp +++ b/guilib/src/MultiSessionLocWidget.cpp @@ -149,11 +149,13 @@ void MultiSessionLocWidget::updateView( Link link = loopLinks.find(nodeId) != loopLinks.end()?loopLinks.find(nodeId)->second:Link(); std::multimap keypoints; - for(std::multimap::const_iterator jter=s.getWords().begin(); jter!=s.getWords().end(); ++jter) - { - if(jter->first>0 && lastSignature.getWords().find(jter->first) != lastSignature.getWords().end()) + if(s.getWords().size() == s.getWordsKpts().size()) { + for(std::multimap::const_iterator jter=s.getWords().begin(); jter!=s.getWords().end(); ++jter) { - keypoints.insert(std::make_pair(jter->first, s.getWordsKpts()[jter->second])); + if(jter->first>0 && lastSignature.getWords().find(jter->first) != lastSignature.getWords().end()) + { + keypoints.insert(std::make_pair(jter->first, s.getWordsKpts()[jter->second])); + } } } diff --git a/guilib/src/PostProcessingDialog.cpp b/guilib/src/PostProcessingDialog.cpp index 54c498bf..e4bf4409 100644 --- a/guilib/src/PostProcessingDialog.cpp +++ b/guilib/src/PostProcessingDialog.cpp @@ -82,6 +82,7 @@ PostProcessingDialog::PostProcessingDialog(QWidget * parent) : connect(_ui->iterations, SIGNAL(valueChanged(int)), this, SIGNAL(configChanged())); connect(_ui->intraSession, SIGNAL(stateChanged(int)), this, SIGNAL(configChanged())); connect(_ui->interSession, SIGNAL(stateChanged(int)), this, SIGNAL(configChanged())); + connect(_ui->minGraphDistance, SIGNAL(valueChanged(int)), this, SIGNAL(configChanged())); connect(_ui->refineNeighborLinks, SIGNAL(stateChanged(int)), this, SIGNAL(configChanged())); connect(_ui->refineLoopClosureLinks, SIGNAL(stateChanged(int)), this, SIGNAL(configChanged())); @@ -150,6 +151,7 @@ void PostProcessingDialog::saveSettings(QSettings & settings, const QString & gr settings.setValue("iterations", this->iterations()); settings.setValue("intra_session", this->intraSession()); settings.setValue("inter_session", this->interSession()); + settings.setValue("min_graph_distance", this->minGraphDistance()); settings.setValue("refine_neigbors", this->isRefineNeighborLinks()); settings.setValue("refine_lc", this->isRefineLoopClosureLinks()); settings.setValue("sba", this->isSBA()); @@ -175,6 +177,7 @@ void PostProcessingDialog::loadSettings(QSettings & settings, const QString & gr this->setIterations(settings.value("iterations", this->iterations()).toInt()); this->setIntraSession(settings.value("intra_session", this->intraSession()).toBool()); this->setInterSession(settings.value("inter_session", this->interSession()).toBool()); + this->setMinGraphDistance(settings.value("min_graph_distance", this->minGraphDistance()).toInt()); this->setRefineNeighborLinks(settings.value("refine_neigbors", this->isRefineNeighborLinks()).toBool()); this->setRefineLoopClosureLinks(settings.value("refine_lc", this->isRefineLoopClosureLinks()).toBool()); this->setSBA(settings.value("sba", this->isSBA()).toBool()); @@ -197,6 +200,7 @@ void PostProcessingDialog::restoreDefaults() setIterations(5); setIntraSession(true); setInterSession(true); + setMinGraphDistance(10); setRefineNeighborLinks(false); setRefineLoopClosureLinks(false); setSBA(false); @@ -254,6 +258,11 @@ bool PostProcessingDialog::interSession() const return _ui->interSession->isChecked(); } +int PostProcessingDialog::minGraphDistance() const +{ + return _ui->minGraphDistance->value(); +} + bool PostProcessingDialog::isRefineNeighborLinks() const { return _ui->refineNeighborLinks->isChecked(); @@ -311,6 +320,10 @@ void PostProcessingDialog::setInterSession(bool enabled) { _ui->interSession->setChecked(enabled); } +void PostProcessingDialog::setMinGraphDistance(int value) +{ + _ui->minGraphDistance->setValue(value); +} void PostProcessingDialog::setRefineNeighborLinks(bool on) { _ui->refineNeighborLinks->setChecked(on); diff --git a/guilib/src/PreferencesDialog.cpp b/guilib/src/PreferencesDialog.cpp index 9f08f66a..2ccdd09e 100644 --- a/guilib/src/PreferencesDialog.cpp +++ b/guilib/src/PreferencesDialog.cpp @@ -171,6 +171,8 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) : _ui->sift_label_gpu->setEnabled(false); _ui->sift_doubleSpinBox_gaussianDiffThreshold->setEnabled(false); _ui->sift_label_gaussianThreshold->setEnabled(false); + _ui->sift_doubleSpinBox_maxGaussianDiffThreshold->setEnabled(false); + _ui->sift_label_maxGaussianThreshold->setEnabled(false); _ui->sift_checkBox_upscale->setEnabled(false); _ui->sift_label_upscale->setEnabled(false); #endif @@ -226,7 +228,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) : #ifndef RTABMAP_MSCKF_VIO _ui->odom_strategy->setItemData(8, 0, Qt::UserRole - 1); #endif -#ifndef RTABMAP_VINS +#ifndef RTABMAP_VINS_FUSION _ui->odom_strategy->setItemData(9, 0, Qt::UserRole - 1); #endif #ifndef RTABMAP_OPENVINS @@ -238,6 +240,12 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) : #ifndef RTABMAP_OPEN3D _ui->odom_strategy->setItemData(12, 0, Qt::UserRole - 1); #endif +#ifndef RTABMAP_CUVSLAM + _ui->odom_strategy->setItemData(13, 0, Qt::UserRole - 1); +#endif +#ifndef RTABMAP_LIOSAM + _ui->odom_strategy->setItemData(14, 0, Qt::UserRole - 1); +#endif #if CV_MAJOR_VERSION < 3 _ui->stereosgbm_mode->setItemData(2, 0, Qt::UserRole - 1); @@ -291,6 +299,10 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) : _ui->comboBox_detector_strategy->setItemData(11, 0, Qt::UserRole - 1); _ui->vis_feature_detector->setItemData(11, 0, Qt::UserRole - 1); #endif +#if !defined(RTABMAP_TORCH) || !defined(RTABMAP_PYTHON) + _ui->comboBox_detector_strategy->setItemData(16, 0, Qt::UserRole - 1); + _ui->vis_feature_detector->setItemData(16, 0, Qt::UserRole - 1); +#endif #ifndef RTABMAP_PYTHON _ui->comboBox_detector_strategy->setItemData(15, 0, Qt::UserRole - 1); @@ -398,6 +410,10 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) : { _ui->comboBox_cameraRGBD->setItemData(kSrcK4A - kSrcRGBD, 0, Qt::UserRole - 1); } + if (!CameraOrbbecSDK::available()) + { + _ui->comboBox_cameraRGBD->setItemData(kSrcOrbbecSDK - kSrcRGBD, 0, Qt::UserRole - 1); + } if (!CameraRealSense::available()) { _ui->comboBox_cameraRGBD->setItemData(kSrcRealSense - kSrcRGBD, 0, Qt::UserRole - 1); @@ -459,15 +475,14 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) : _ui->checkBox_showOdomFrustums->setChecked(false); #endif - //if OpenCV < 3.4.2 -#if CV_MAJOR_VERSION < 3 || (CV_MAJOR_VERSION == 3 && (CV_MINOR_VERSION <4 || (CV_MINOR_VERSION ==4 && CV_SUBMINOR_VERSION<2))) - _ui->ArucoDictionary->setItemData(17, 0, Qt::UserRole - 1); - _ui->ArucoDictionary->setItemData(18, 0, Qt::UserRole - 1); - _ui->ArucoDictionary->setItemData(19, 0, Qt::UserRole - 1); - _ui->ArucoDictionary->setItemData(20, 0, Qt::UserRole - 1); +#if !defined(HAVE_OPENCV_ARUCO) && !defined(RTABMAP_APRILTAG) + _ui->label_markerDetection->setText(_ui->label_markerDetection->text()+" This option works only if OpenCV has been built with \"aruco\" module and/or RTAB-Map has been built with AprilTag library support."); #endif #ifndef HAVE_OPENCV_ARUCO - _ui->label_markerDetection->setText(_ui->label_markerDetection->text()+" This option works only if OpenCV has been built with \"aruco\" module."); + _ui->MarkerStrategy->setItemData(0, 0, Qt::UserRole - 1); +#endif +#ifndef RTABMAP_APRILTAG + _ui->MarkerStrategy->setItemData(1, 0, Qt::UserRole - 1); #endif #ifndef RTABMAP_MADGWICK @@ -744,10 +759,14 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) : 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_checkBox_ignoreIMU, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel())); + connect(_ui->source_checkBox_intermediateNodesAreNormalNodes, 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())); connect(_ui->source_lineEdit_databaseCameraIndex, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel())); + connect(_ui->source_checkBox_overrideLocalTransforms, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel())); + connect(_ui->source_lineEdit_databaseLocalTransformOffset, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel())); connect(_ui->source_checkBox_stereoToDepthDB, SIGNAL(toggled(bool)), _ui->checkbox_stereo_depthGenerated, SLOT(setChecked(bool))); connect(_ui->checkbox_stereo_depthGenerated, SIGNAL(toggled(bool)), _ui->source_checkBox_stereoToDepthDB, SLOT(setChecked(bool))); @@ -767,19 +786,39 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) : connect(_ui->openni2_hshift, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel())); connect(_ui->openni2_vshift, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel())); connect(_ui->openni2_depth_decimation, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel())); + connect(_ui->comboBox_freenect2Format, SIGNAL(currentIndexChanged(int)), this, SLOT(makeObsoleteSourcePanel())); connect(_ui->doubleSpinBox_freenect2MinDepth, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteSourcePanel())); connect(_ui->doubleSpinBox_freenect2MaxDepth, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteSourcePanel())); + connect(_ui->checkBox_freenect2BilateralFiltering, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel())); connect(_ui->checkBox_freenect2EdgeAwareFiltering, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel())); connect(_ui->checkBox_freenect2NoiseFiltering, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel())); connect(_ui->lineEdit_freenect2Pipeline, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel())); + connect(_ui->comboBox_k4w2Format, 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->lineEdit_k4a_mkv, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel())); + connect(_ui->source_checkBox_useMKVStamps, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel())); + + connect(_ui->spinBox_orbbec_sdk_color_width, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel())); + connect(_ui->spinBox_orbbec_sdk_color_height, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel())); + connect(_ui->spinBox_orbbec_sdk_depth_width, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel())); + connect(_ui->spinBox_orbbec_sdk_depth_height, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel())); + connect(_ui->checkBox_orbbec_sdk_color_rectification, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel())); + connect(_ui->checkBox_orbbec_sdk_imu, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel())); + connect(_ui->checkBox_orbbec_sdk_depth_mm, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel())); + connect(_ui->comboBox_realsensePresetRGB, SIGNAL(currentIndexChanged(int)), this, SLOT(makeObsoleteSourcePanel())); connect(_ui->comboBox_realsensePresetDepth, SIGNAL(currentIndexChanged(int)), this, SLOT(makeObsoleteSourcePanel())); connect(_ui->checkbox_realsenseOdom, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel())); connect(_ui->checkbox_realsenseDepthScaledToRGBSize, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel())); connect(_ui->comboBox_realsenseRGBSource, SIGNAL(currentIndexChanged(int)), this, SLOT(makeObsoleteSourcePanel())); + connect(_ui->checkbox_rs2_emitter, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel())); connect(_ui->checkbox_rs2_irMode, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel())); connect(_ui->checkbox_rs2_irDepth, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel())); @@ -816,6 +855,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) : connect(_ui->comboBox_cameraImages_odomFormat, SIGNAL(currentIndexChanged(int)), this, SLOT(makeObsoleteSourcePanel())); connect(_ui->lineEdit_cameraImages_gt, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel())); connect(_ui->comboBox_cameraImages_gtFormat, SIGNAL(currentIndexChanged(int)), this, SLOT(makeObsoleteSourcePanel())); + connect(_ui->lineEdit_cameraImages_gt_transform, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel())); connect(_ui->doubleSpinBox_maxPoseTimeDiff, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteSourcePanel())); connect(_ui->lineEdit_cameraImages_path_imu, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel())); connect(_ui->lineEdit_cameraImages_imu_transform, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel())); @@ -918,6 +958,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) : connect(_ui->doubleSpinBox_odom_sensor_scale_factor, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteSourcePanel())); connect(_ui->doubleSpinBox_odom_sensor_wait_time, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteSourcePanel())); connect(_ui->checkBox_odom_sensor_use_as_gt, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel())); + connect(_ui->checkbox_passthrough_source_odom, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel())); connect(_ui->comboBox_imuFilter_strategy, SIGNAL(currentIndexChanged(int)), this, SLOT(makeObsoleteSourcePanel())); connect(_ui->comboBox_imuFilter_strategy, SIGNAL(currentIndexChanged(int)), _ui->stackedWidget_imuFilter, SLOT(setCurrentIndex(int))); @@ -1005,6 +1046,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) : _ui->lineEdit_rgbCompressionFormat->setObjectName(Parameters::kMemImageCompressionFormat().c_str()); _ui->lineEdit_depthCompressionFormat->setObjectName(Parameters::kMemDepthCompressionFormat().c_str()); _ui->general_checkBox_keepDescriptors->setObjectName(Parameters::kMemRawDescriptorsKept().c_str()); + _ui->general_checkBox_loadVisualLocalFeaturesOnInit->setObjectName(Parameters::kMemLoadVisualLocalFeaturesOnInit().c_str()); _ui->general_checkBox_saveDepth16bits->setObjectName(Parameters::kMemSaveDepth16Format().c_str()); _ui->general_checkBox_compressionParallelized->setObjectName(Parameters::kMemCompressionParallelized().c_str()); _ui->general_checkBox_reduceGraph->setObjectName(Parameters::kMemReduceGraph().c_str()); @@ -1034,6 +1076,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) : // Database _ui->checkBox_dbInMemory->setObjectName(Parameters::kDbSqlite3InMemory().c_str()); + _ui->checkBox_dbReadOnly->setObjectName(Parameters::kMemLocalizationReadOnly().c_str()); _ui->spinBox_dbCacheSize->setObjectName(Parameters::kDbSqlite3CacheSize().c_str()); _ui->comboBox_dbJournalMode->setObjectName(Parameters::kDbSqlite3JournalMode().c_str()); _ui->comboBox_dbSynchronous->setObjectName(Parameters::kDbSqlite3Synchronous().c_str()); @@ -1079,6 +1122,8 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) : _ui->lineEdit_dictionaryPath->setObjectName(Parameters::kKpDictionaryPath().c_str()); connect(_ui->toolButton_dictionaryPath, SIGNAL(clicked()), this, SLOT(changeDictionaryPath())); _ui->checkBox_kp_newWordsComparedTogether->setObjectName(Parameters::kKpNewWordsComparedTogether().c_str()); + _ui->checkBox_kp_flannIndexSaved->setObjectName(Parameters::kKpFlannIndexSaved().c_str()); + _ui->checkBox_kp_serializeWithChecksum->setObjectName(Parameters::kKpSerializeWithChecksum().c_str()); _ui->subpix_winSize_kp->setObjectName(Parameters::kKpSubPixWinSize().c_str()); _ui->subpix_iterations_kp->setObjectName(Parameters::kKpSubPixIterations().c_str()); _ui->subpix_eps_kp->setObjectName(Parameters::kKpSubPixEps().c_str()); @@ -1105,6 +1150,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) : _ui->sift_checkBox_rootsift->setObjectName(Parameters::kSIFTRootSIFT().c_str()); _ui->sift_checkBox_gpu->setObjectName(Parameters::kSIFTGpu().c_str()); _ui->sift_doubleSpinBox_gaussianDiffThreshold->setObjectName(Parameters::kSIFTGaussianThreshold().c_str()); + _ui->sift_doubleSpinBox_maxGaussianDiffThreshold->setObjectName(Parameters::kSIFTMaxGaussianThreshold().c_str()); _ui->sift_checkBox_upscale->setObjectName(Parameters::kSIFTUpscale().c_str()); //BRIEF descriptor @@ -1166,6 +1212,16 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) : _ui->spinBox_sptorch_minDistance->setObjectName(Parameters::kSuperPointNMSRadius().c_str()); _ui->checkBox_sptorch_cuda->setObjectName(Parameters::kSuperPointCuda().c_str()); + // SuperPoint Rpautrat + _ui->lineEdit_sprpautrat_weights_path->setObjectName(Parameters::kSuperPointRpautratWeightsPath().c_str()); + connect(_ui->toolButton_sprpautrat_weights_path, SIGNAL(clicked()), this, SLOT(changeSuperPointRpautratWeightsPath())); + _ui->lineEdit_sprpautrat_model_path->setObjectName(Parameters::kSuperPointRpautratModelPath().c_str()); + connect(_ui->toolButton_sprpautrat_model_path, SIGNAL(clicked()), this, SLOT(changeSuperPointRpautratModelPath())); + _ui->doubleSpinBox_sprpautrat_threshold->setObjectName(Parameters::kSuperPointRpautratThreshold().c_str()); + _ui->checkBox_sprpautrat_nms->setObjectName(Parameters::kSuperPointRpautratNMS().c_str()); + _ui->spinBox_sprpautrat_minDistance->setObjectName(Parameters::kSuperPointRpautratNMSRadius().c_str()); + _ui->checkBox_sprpautrat_cuda->setObjectName(Parameters::kSuperPointRpautratCuda().c_str()); + // PyMatcher _ui->lineEdit_pymatcher_path->setObjectName(Parameters::kPyMatcherPath().c_str()); connect(_ui->toolButton_pymatcher_path, SIGNAL(clicked()), this, SLOT(changePyMatcherPath())); @@ -1216,6 +1272,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) : _ui->graphOptimization_covarianceIgnored->setObjectName(Parameters::kOptimizerVarianceIgnored().c_str()); _ui->graphOptimization_fromGraphEnd->setObjectName(Parameters::kRGBDOptimizeFromGraphEnd().c_str()); _ui->graphOptimization_maxError->setObjectName(Parameters::kRGBDOptimizeMaxError().c_str()); + _ui->graphOptimization_maxErrorRepairRadius->setObjectName(Parameters::kRGBDOptimizeMaxErrorRepairRadius().c_str()); _ui->graphOptimization_gravitySigma->setObjectName(Parameters::kOptimizerGravitySigma().c_str()); _ui->graphOptimization_stopEpsilon->setObjectName(Parameters::kOptimizerEpsilon().c_str()); _ui->graphOptimization_robust->setObjectName(Parameters::kOptimizerRobust().c_str()); @@ -1309,6 +1366,9 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) : _ui->odom_flow_iterations->setObjectName(Parameters::kVisCorFlowIterations().c_str()); _ui->odom_flow_eps->setObjectName(Parameters::kVisCorFlowEps().c_str()); _ui->odom_flow_gpu->setObjectName(Parameters::kVisCorFlowGpu().c_str()); + _ui->odom_flow_useMinEigenVals->setObjectName(Parameters::kVisCorFlowUseMinEigenVals().c_str()); + _ui->odom_flow_minEigThreshold->setObjectName(Parameters::kVisCorFlowMinEigThreshold().c_str()); + _ui->odom_flow_errorThreshold->setObjectName(Parameters::kVisCorFlowErrorThreshold().c_str()); _ui->loopClosure_bundle->setObjectName(Parameters::kVisBundleAdjustment().c_str()); //RegistrationIcp @@ -1553,10 +1613,12 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) : _ui->OdomMSCKFInitCovExTrans->setObjectName(Parameters::kOdomMSCKFInitCovExTrans().c_str()); // Odometry VINS - _ui->lineEdit_OdomVinsPath->setObjectName(Parameters::kOdomVINSConfigPath().c_str()); - connect(_ui->toolButton_OdomVinsPath, SIGNAL(clicked()), this, SLOT(changeOdometryVINSConfigPath())); + _ui->lineEdit_OdomVinsFusionPath->setObjectName(Parameters::kOdomVINSFusionConfigPath().c_str()); + connect(_ui->toolButton_OdomVinsFusionPath, SIGNAL(clicked()), this, SLOT(changeOdometryVINSFusionConfigPath())); // Odometry OpenVINS + _ui->lineEdit_openvinsConfigPath->setObjectName(Parameters::kOdomOpenVINSConfigPath().c_str()); + connect(_ui->toolButton_openvinsConfigPath, SIGNAL(clicked()), this, SLOT(changeOdometryOpenVINSConfigPath())); _ui->checkBox_OdomOpenVINSUseStereo->setObjectName(Parameters::kOdomOpenVINSUseStereo().c_str()); _ui->checkBox_OdomOpenVINSUseKLT->setObjectName(Parameters::kOdomOpenVINSUseKLT().c_str()); _ui->spinBox_OdomOpenVINSNumPts->setObjectName(Parameters::kOdomOpenVINSNumPts().c_str()); @@ -1629,14 +1691,35 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) : _ui->stereo_maxDisparity->setObjectName(Parameters::kStereoMaxDisparity().c_str()); _ui->stereo_ssd->setObjectName(Parameters::kStereoSSD().c_str()); _ui->stereo_flow_eps->setObjectName(Parameters::kStereoEps().c_str()); + _ui->stereo_flow_useMinEigenVals->setObjectName(Parameters::kStereoUseMinEigenVals().c_str()); + _ui->stereo_flow_minEigThreshold->setObjectName(Parameters::kStereoMinEigThreshold().c_str()); + _ui->stereo_flow_errorThreshold->setObjectName(Parameters::kStereoErrorThreshold().c_str()); _ui->stereo_opticalFlow->setObjectName(Parameters::kStereoOpticalFlow().c_str()); _ui->stereo_flow_gpu->setObjectName(Parameters::kStereoGpu().c_str()); - // Odometry Open3D _ui->odom_open3d_method->setObjectName(Parameters::kOdomOpen3DMethod().c_str()); _ui->odom_open3d_max_depth->setObjectName(Parameters::kOdomOpen3DMaxDepth().c_str()); + // Odometry CuVSLAM + _ui->odom_cuvslam_multicam_mode->setObjectName(Parameters::kOdomCuVSLAMMulticamMode().c_str()); + + // Odometry LIO-SAM + _ui->lineEdit_OdomLIOSAMPath->setObjectName(Parameters::kOdomLIOSAMConfigPath().c_str()); + connect(_ui->toolButton_OdomLIOSAMPath, SIGNAL(clicked()), this, SLOT(changeOdometryLIOSAMConfigPath())); + _ui->odom_liosam_sensor->setObjectName(Parameters::kOdomLIOSAMSensor().c_str()); + _ui->odom_liosam_nscan->setObjectName(Parameters::kOdomLIOSAMNScan().c_str()); + _ui->odom_liosam_horizon_scan->setObjectName(Parameters::kOdomLIOSAMHorizonScan().c_str()); + _ui->odom_liosam_imu_acc_noise->setObjectName(Parameters::kOdomLIOSAMImuAccNoise().c_str()); + _ui->odom_liosam_imu_gyr_noise->setObjectName(Parameters::kOdomLIOSAMImuGyrNoise().c_str()); + _ui->odom_liosam_imu_acc_bias_n->setObjectName(Parameters::kOdomLIOSAMImuAccBiasN().c_str()); + _ui->odom_liosam_imu_gyr_bias_n->setObjectName(Parameters::kOdomLIOSAMImuGyrBiasN().c_str()); + _ui->odom_liosam_imu_gravity->setObjectName(Parameters::kOdomLIOSAMImuGravity().c_str()); + _ui->odom_liosam_edge_threshold->setObjectName(Parameters::kOdomLIOSAMEdgeThreshold().c_str()); + _ui->odom_liosam_surf_threshold->setObjectName(Parameters::kOdomLIOSAMSurfThreshold().c_str()); + _ui->odom_liosam_linvar->setObjectName(Parameters::kOdomLIOSAMLinVar().c_str()); + _ui->odom_liosam_angvar->setObjectName(Parameters::kOdomLIOSAMAngVar().c_str()); + //StereoDense _ui->comboBox_stereoDense_strategy->setObjectName(Parameters::kStereoDenseStrategy().c_str()); connect(_ui->comboBox_stereoDense_strategy, SIGNAL(currentIndexChanged(int)), _ui->stackedWidget_stereoDense, SLOT(setCurrentIndex(int))); @@ -1669,18 +1752,30 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) : _ui->stereosgbm_mode->setObjectName(Parameters::kStereoSGBMMode().c_str()); // Aruco marker - _ui->ArucoDictionary->setObjectName(Parameters::kMarkerDictionary().c_str()); - _ui->ArucoMarkerLength->setObjectName(Parameters::kMarkerLength().c_str()); - _ui->ArucoMaxDepthError->setObjectName(Parameters::kMarkerMaxDepthError().c_str()); - _ui->ArucoVarianceLinear->setObjectName(Parameters::kMarkerVarianceLinear().c_str()); - _ui->ArucoVarianceAngular->setObjectName(Parameters::kMarkerVarianceAngular().c_str()); - _ui->ArucoVarianceOrientationIgnored->setObjectName(Parameters::kMarkerVarianceOrientationIgnored().c_str()); - _ui->ArucoMarkerRangeMin->setObjectName(Parameters::kMarkerMinRange().c_str()); - _ui->ArucoMarkerRangeMax->setObjectName(Parameters::kMarkerMaxRange().c_str()); - _ui->ArucoMarkerPriors->setObjectName(Parameters::kMarkerPriors().c_str()); - _ui->ArucoPriorsVarianceLinear->setObjectName(Parameters::kMarkerPriorsVarianceLinear().c_str()); - _ui->ArucoPriorsVarianceAngular->setObjectName(Parameters::kMarkerPriorsVarianceAngular().c_str()); - _ui->ArucoCornerRefinementMethod->setObjectName(Parameters::kMarkerCornerRefinementMethod().c_str()); + _ui->MarkerStrategy->setObjectName(Parameters::kMarkerStrategy().c_str()); + connect(_ui->MarkerStrategy, SIGNAL(currentIndexChanged(int)), _ui->stackedWidget_markerStrategy, SLOT(setCurrentIndex(int))); + connect(_ui->MarkerStrategy, SIGNAL(currentIndexChanged(int)), this, SLOT(updateAvailableMarkerDictionaries())); + _ui->MarkerStrategy->setCurrentIndex(Parameters::defaultMarkerStrategy()); + updateAvailableMarkerDictionaries(); + _ui->MarkerDictionary->setObjectName(Parameters::kMarkerDictionary().c_str()); + _ui->MarkerLength->setObjectName(Parameters::kMarkerLength().c_str()); + _ui->MarkerLengths->setObjectName(Parameters::kMarkerLengths().c_str()); + _ui->MarkerMaxDepthError->setObjectName(Parameters::kMarkerMaxDepthError().c_str()); + _ui->MarkerVarianceLinear->setObjectName(Parameters::kMarkerVarianceLinear().c_str()); + _ui->MarkerVarianceAngular->setObjectName(Parameters::kMarkerVarianceAngular().c_str()); + _ui->MarkerVarianceOrientationIgnored->setObjectName(Parameters::kMarkerVarianceOrientationIgnored().c_str()); + _ui->MarkerRangeMin->setObjectName(Parameters::kMarkerMinRange().c_str()); + _ui->MarkerRangeMax->setObjectName(Parameters::kMarkerMaxRange().c_str()); + _ui->MarkerPriors->setObjectName(Parameters::kMarkerPriors().c_str()); + _ui->MarkerPriorsVarianceLinear->setObjectName(Parameters::kMarkerPriorsVarianceLinear().c_str()); + _ui->MarkerPriorsVarianceAngular->setObjectName(Parameters::kMarkerPriorsVarianceAngular().c_str()); + _ui->OpenCVCornerRefinementMethod->setObjectName(Parameters::kMarkerOpenCVCornerRefinementMethod().c_str()); + _ui->apriltag_nthreads->setObjectName(Parameters::kMarkerAprilTagNThreads().c_str()); + _ui->apriltag_quad_decimate->setObjectName(Parameters::kMarkerAprilTagQuadDecimate().c_str()); + _ui->apriltag_quad_sigma->setObjectName(Parameters::kMarkerAprilTagQuadSigma().c_str()); + _ui->apriltag_refine_edges->setObjectName(Parameters::kMarkerAprilTagRefineEdges().c_str()); + _ui->apriltag_decode_sharpening->setObjectName(Parameters::kMarkerAprilTagDecodeSharpening().c_str()); + _ui->apriltag_debug->setObjectName(Parameters::kMarkerAprilTagDebug().c_str()); // IMU filter _ui->doubleSpinBox_imuFilterMadgwickGain->setObjectName(Parameters::kImuFilterMadgwickGain().c_str()); @@ -2147,10 +2242,14 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox) _ui->source_checkBox_ignoreLandmarks->setChecked(true); _ui->source_checkBox_ignoreFeatures->setChecked(true); _ui->source_checkBox_ignorePriors->setChecked(false); + _ui->source_checkBox_ignoreIMU->setChecked(false); + _ui->source_checkBox_intermediateNodesAreNormalNodes->setChecked(false); _ui->source_spinBox_databaseStartId->setValue(0); _ui->source_spinBox_databaseStopId->setValue(0); _ui->source_lineEdit_databaseCameraIndex->setText(""); _ui->source_checkBox_useDbStamps->setChecked(true); + _ui->source_checkBox_overrideLocalTransforms->setChecked(false); + _ui->source_lineEdit_databaseLocalTransformOffset->setText(""); #ifdef _WIN32 _ui->comboBox_cameraRGBD->setCurrentIndex(kSrcOpenNI2-kSrcRGBD); // openni2 @@ -2228,6 +2327,13 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox) _ui->comboBox_k4a_depth_resolution->setCurrentIndex(2); _ui->checkbox_k4a_irDepth->setChecked(false); _ui->lineEdit_k4a_mkv->clear(); + _ui->spinBox_orbbec_sdk_color_width->setValue(800); + _ui->spinBox_orbbec_sdk_color_height->setValue(600); + _ui->spinBox_orbbec_sdk_depth_width->setValue(800); + _ui->spinBox_orbbec_sdk_depth_height->setValue(600); + _ui->checkBox_orbbec_sdk_color_rectification->setChecked(false); + _ui->checkBox_orbbec_sdk_imu->setChecked(true); + _ui->checkBox_orbbec_sdk_depth_mm->setChecked(true); _ui->source_checkBox_useMKVStamps->setChecked(true); _ui->lineEdit_cameraRGBDImages_path_rgb->setText(""); _ui->lineEdit_cameraRGBDImages_path_depth->setText(""); @@ -2293,6 +2399,7 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox) _ui->comboBox_cameraImages_odomFormat->setCurrentIndex(0); _ui->lineEdit_cameraImages_gt->setText(""); _ui->comboBox_cameraImages_gtFormat->setCurrentIndex(0); + _ui->lineEdit_cameraImages_gt_transform->setText("0 0 0 0 0 0"); _ui->doubleSpinBox_maxPoseTimeDiff->setValue(0.02); _ui->lineEdit_cameraImages_path_imu->setText(""); _ui->lineEdit_cameraImages_imu_transform->setText("0 0 1 0 -1 0 1 0 0"); @@ -2306,6 +2413,7 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox) _ui->doubleSpinBox_odom_sensor_scale_factor->setValue(1); _ui->doubleSpinBox_odom_sensor_wait_time->setValue(100); _ui->checkBox_odom_sensor_use_as_gt->setChecked(false); + _ui->checkbox_passthrough_source_odom->setChecked(false); _ui->comboBox_imuFilter_strategy->setCurrentIndex(2); _ui->doubleSpinBox_imuFilterMadgwickGain->setValue(Parameters::defaultImuFilterMadgwickGain()); @@ -2713,6 +2821,16 @@ void PreferencesDialog::readCameraSettings(const QString & filePath) _ui->source_checkBox_useMKVStamps->setChecked(settings.value("useMkvStamps", _ui->source_checkBox_useMKVStamps->isChecked()).toBool()); settings.endGroup(); // K4A + settings.beginGroup("OrbbecSDK"); + _ui->spinBox_orbbec_sdk_color_width->setValue(settings.value("color_width", _ui->spinBox_orbbec_sdk_color_width->value()).toInt()); + _ui->spinBox_orbbec_sdk_color_height->setValue(settings.value("color_height", _ui->spinBox_orbbec_sdk_color_height->value()).toInt()); + _ui->spinBox_orbbec_sdk_depth_width->setValue(settings.value("depth_width", _ui->spinBox_orbbec_sdk_depth_width->value()).toInt()); + _ui->spinBox_orbbec_sdk_depth_height->setValue(settings.value("depth_height", _ui->spinBox_orbbec_sdk_depth_height->value()).toInt()); + _ui->checkBox_orbbec_sdk_color_rectification->setChecked(settings.value("rectify_color", _ui->checkBox_orbbec_sdk_color_rectification->isChecked()).toBool()); + _ui->checkBox_orbbec_sdk_imu->setChecked(settings.value("enable_imu", _ui->checkBox_orbbec_sdk_imu->isChecked()).toBool()); + _ui->checkBox_orbbec_sdk_depth_mm->setChecked(settings.value("depth_mm", _ui->checkBox_orbbec_sdk_depth_mm->isChecked()).toBool()); + settings.endGroup(); // Orbbec SDK + settings.beginGroup("RealSense"); _ui->comboBox_realsensePresetRGB->setCurrentIndex(settings.value("presetRGB", _ui->comboBox_realsensePresetRGB->currentIndex()).toInt()); _ui->comboBox_realsensePresetDepth->setCurrentIndex(settings.value("presetDepth", _ui->comboBox_realsensePresetDepth->currentIndex()).toInt()); @@ -2816,6 +2934,7 @@ void PreferencesDialog::readCameraSettings(const QString & filePath) _ui->comboBox_cameraImages_odomFormat->setCurrentIndex(settings.value("odom_format", _ui->comboBox_cameraImages_odomFormat->currentIndex()).toInt()); _ui->lineEdit_cameraImages_gt->setText(settings.value("gt_path", _ui->lineEdit_cameraImages_gt->text()).toString()); _ui->comboBox_cameraImages_gtFormat->setCurrentIndex(settings.value("gt_format", _ui->comboBox_cameraImages_gtFormat->currentIndex()).toInt()); + _ui->lineEdit_cameraImages_gt_transform->setText(settings.value("gt_transform", _ui->lineEdit_cameraImages_gt_transform->text()).toString()); _ui->doubleSpinBox_maxPoseTimeDiff->setValue(settings.value("max_pose_time_diff", _ui->doubleSpinBox_maxPoseTimeDiff->value()).toDouble()); _ui->lineEdit_cameraImages_path_imu->setText(settings.value("imu_path", _ui->lineEdit_cameraImages_path_imu->text()).toString()); @@ -2832,6 +2951,7 @@ void PreferencesDialog::readCameraSettings(const QString & filePath) _ui->doubleSpinBox_odom_sensor_scale_factor->setValue(settings.value("odom_sensor_scale_factor", _ui->doubleSpinBox_odom_sensor_scale_factor->value()).toDouble()); _ui->doubleSpinBox_odom_sensor_wait_time->setValue(settings.value("odom_sensor_wait_time", _ui->doubleSpinBox_odom_sensor_wait_time->value()).toDouble()); _ui->checkBox_odom_sensor_use_as_gt->setChecked(settings.value("odom_sensor_odom_as_gt", _ui->checkBox_odom_sensor_use_as_gt->isChecked()).toBool()); + _ui->checkbox_passthrough_source_odom->setChecked(settings.value("odom_sensor_as_guess", _ui->checkbox_passthrough_source_odom->isChecked()).toBool()); settings.endGroup(); // OdomSensor settings.beginGroup("UsbCam"); @@ -2885,10 +3005,15 @@ void PreferencesDialog::readCameraSettings(const QString & filePath) _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_checkBox_ignoreIMU->setChecked(settings.value("ignoreImu", _ui->source_checkBox_ignoreIMU->isChecked()).toBool()); + _ui->source_checkBox_intermediateNodesAreNormalNodes->setChecked(settings.value("intermediateNodesAreNormalNodes", _ui->source_checkBox_intermediateNodesAreNormalNodes->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_lineEdit_databaseCameraIndex->setText(settings.value("cameraIndices", _ui->source_lineEdit_databaseCameraIndex->text()).toString()); _ui->source_checkBox_useDbStamps->setChecked(settings.value("useDatabaseStamps", _ui->source_checkBox_useDbStamps->isChecked()).toBool()); + _ui->source_checkBox_overrideLocalTransforms->setChecked(settings.value("overrideLocalTransforms", _ui->source_checkBox_overrideLocalTransforms->isChecked()).toBool()); + _ui->source_lineEdit_databaseLocalTransformOffset->setText(settings.value("localTransformOffsets", _ui->source_lineEdit_databaseLocalTransformOffset->text()).toString()); settings.endGroup(); // Database settings.endGroup(); // Camera @@ -3319,6 +3444,16 @@ void PreferencesDialog::writeCameraSettings(const QString & filePath) const settings.setValue("useMkvStamps", _ui->source_checkBox_useMKVStamps->isChecked()); settings.endGroup(); // K4A + settings.beginGroup("OrbbecSDK"); + settings.setValue("color_width", _ui->spinBox_orbbec_sdk_color_width->value()); + settings.setValue("color_height", _ui->spinBox_orbbec_sdk_color_height->value()); + settings.setValue("depth_width", _ui->spinBox_orbbec_sdk_depth_width->value()); + settings.setValue("depth_height", _ui->spinBox_orbbec_sdk_depth_height->value()); + settings.setValue("rectify_color", _ui->checkBox_orbbec_sdk_color_rectification->isChecked()); + settings.setValue("enable_imu", _ui->checkBox_orbbec_sdk_imu->isChecked()); + settings.setValue("depth_mm", _ui->checkBox_orbbec_sdk_depth_mm->isChecked()); + settings.endGroup(); // Orbbec SDK + settings.beginGroup("RealSense"); settings.setValue("presetRGB", _ui->comboBox_realsensePresetRGB->currentIndex()); settings.setValue("presetDepth", _ui->comboBox_realsensePresetDepth->currentIndex()); @@ -3421,6 +3556,7 @@ void PreferencesDialog::writeCameraSettings(const QString & filePath) const settings.setValue("odom_format", _ui->comboBox_cameraImages_odomFormat->currentIndex()); settings.setValue("gt_path", _ui->lineEdit_cameraImages_gt->text()); settings.setValue("gt_format", _ui->comboBox_cameraImages_gtFormat->currentIndex()); + settings.setValue("gt_transform", _ui->lineEdit_cameraImages_gt_transform->text()); settings.setValue("max_pose_time_diff", _ui->doubleSpinBox_maxPoseTimeDiff->value()); settings.setValue("imu_path", _ui->lineEdit_cameraImages_path_imu->text()); settings.setValue("imu_local_transform", _ui->lineEdit_cameraImages_imu_transform->text()); @@ -3436,6 +3572,7 @@ void PreferencesDialog::writeCameraSettings(const QString & filePath) const settings.setValue("odom_sensor_scale_factor", _ui->doubleSpinBox_odom_sensor_scale_factor->value()); settings.setValue("odom_sensor_wait_time", _ui->doubleSpinBox_odom_sensor_wait_time->value()); settings.setValue("odom_sensor_odom_as_gt", _ui->checkBox_odom_sensor_use_as_gt->isChecked()); + settings.setValue("odom_sensor_as_guess", _ui->checkbox_passthrough_source_odom->isChecked()); settings.endGroup(); // OdomSensor settings.beginGroup("UsbCam"); @@ -3489,10 +3626,14 @@ void PreferencesDialog::writeCameraSettings(const QString & filePath) const 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("ignoreImu", _ui->source_checkBox_ignoreIMU->isChecked()); + settings.setValue("intermediateNodesAreNormalNodes", _ui->source_checkBox_intermediateNodesAreNormalNodes->isChecked()); settings.setValue("startId", _ui->source_spinBox_databaseStartId->value()); settings.setValue("stopId", _ui->source_spinBox_databaseStopId->value()); settings.setValue("cameraIndices", _ui->source_lineEdit_databaseCameraIndex->text()); settings.setValue("useDatabaseStamps", _ui->source_checkBox_useDbStamps->isChecked()); + settings.setValue("overrideLocalTransforms", _ui->source_checkBox_overrideLocalTransforms->isChecked()); + settings.setValue("localTransformOffsets", _ui->source_lineEdit_databaseLocalTransformOffset->text()); settings.endGroup(); // Database settings.endGroup(); // Camera @@ -3730,15 +3871,6 @@ bool PreferencesDialog::validateForm() _ui->odom_f2m_bundleStrategy->setCurrentIndex(0); } - // verify that Robust and Reject threshold are not set at the same time - if(_ui->graphOptimization_robust->isChecked() && _ui->graphOptimization_maxError->value()>0.0) - { - QMessageBox::warning(this, tr("Parameter warning"), - tr("Robust graph optimization and maximum optimization error threshold cannot be " - "both used at the same time. Disabling robust optimization.")); - _ui->graphOptimization_robust->setChecked(false); - } - //verify binary features and nearest neighbor // BOW dictionary type if(_ui->comboBox_dictionary_strategy->currentIndex() == VWDictionary::kNNFlannLSH && _ui->comboBox_detector_strategy->currentIndex() <= 1) @@ -3819,15 +3951,36 @@ bool PreferencesDialog::validateForm() _ui->checkbox_odomDisabled->setChecked(false); } -#if CV_MAJOR_VERSION < 3 || (CV_MAJOR_VERSION == 3 && (CV_MINOR_VERSION <4 || (CV_MINOR_VERSION ==4 && CV_SUBMINOR_VERSION<2))) - if(_ui->ArucoDictionary->currentIndex()>=17) - { - QMessageBox::warning(this, tr("Parameter warning"), - tr("ArUco dictionary: cannot select AprilTag dictionary, OpenCV version should be at least 3.4.2. Setting back to 0.")); - _ui->ArucoDictionary->setCurrentIndex(0); - } -#endif + if(_ui->MarkerStrategy->currentIndex() == 0) + { +#if CV_MAJOR_VERSION < 3 || (CV_MAJOR_VERSION == 3 && (CV_MINOR_VERSION <4 || (CV_MINOR_VERSION ==4 && CV_SUBMINOR_VERSION<2))) + if(_ui->MarkerDictionary->currentIndex()>=17) + { + QMessageBox::warning(this, tr("Parameter warning"), + tr("opencv-aruco: cannot use the selected dictionary (%1), OpenCV version should be at least 3.4.2. Setting back to 0.").arg(_ui->MarkerDictionary->currentIndex())); + _ui->MarkerDictionary->setCurrentIndex(0); + } +#elif CV_MAJOR_VERSION < 4 || (CV_MAJOR_VERSION == 4 && CV_MINOR_VERSION <8) + if(_ui->MarkerDictionary->currentIndex()>=21) + { + QMessageBox::warning(this, tr("Parameter warning"), + tr("Opencv Strategy: cannot use selected dictionary (%1), OpenCV version should be at least 4.8.0. Setting back to 0.").arg(_ui->MarkerDictionary->currentIndex())); + _ui->MarkerDictionary->setCurrentIndex(0); + } +#endif + } + else if(_ui->MarkerStrategy->currentIndex() == 1) + { +#ifndef RTABMAP_APRILTAG_WITH_ARUCO + if(_ui->MarkerDictionary->currentIndex() < 17 || _ui->MarkerDictionary->currentIndex() == 21) + { + QMessageBox::warning(this, tr("Parameter warning"), + tr("AprilTag Strategy: cannot use selected dictionary (%1), AprilTag should be built with aruco support. Setting back to 17.").arg(_ui->MarkerDictionary->currentIndex())); + _ui->MarkerDictionary->setCurrentIndex(17); + } +#endif + } return true; } @@ -5454,6 +5607,64 @@ void PreferencesDialog::updateGlobalDescriptorVisibility() _ui->groupBox_pydescriptor->setVisible(_ui->comboBox_globalDescriptorExtractor->currentIndex() == 1); } +void PreferencesDialog::updateAvailableMarkerDictionaries() +{ + Qt::ItemFlags enableFlags = Qt::ItemFlags(Qt::ItemIsEnabled) | Qt::ItemIsSelectable; + for(int i=0;i<_ui->MarkerDictionary->count();++i) { + _ui->MarkerDictionary->setItemData(i, QVariant(static_cast(enableFlags)), Qt::UserRole - 1); + } + + if(_ui->MarkerStrategy->currentIndex() == 1) // AprilTag Strategy is selected + { + // ARUCO_ORIGINAL not available with AprilTag lib + _ui->MarkerDictionary->setItemData(16, 0, Qt::UserRole - 1); +#ifndef RTABMAP_APRILTAG_WITH_ARUCO + // disable all aruco dictionaries + for(int i=0;i<17;++i) { + _ui->MarkerDictionary->setItemData(i, 0, Qt::UserRole - 1); + } + _ui->MarkerDictionary->setItemData(21, 0, Qt::UserRole - 1); + if(_ui->MarkerDictionary->currentIndex() < 17 || _ui->MarkerDictionary->currentIndex() > 20) + { + _ui->MarkerDictionary->setCurrentIndex(20); // 36h11 by default + } +#endif + } + else //if(_ui->MarkerStrategy->currentIndex() == 0) // OpenCV Strategy is selected + { + //if OpenCV < 3.4.2 +#if CV_MAJOR_VERSION < 3 || (CV_MAJOR_VERSION == 3 && (CV_MINOR_VERSION <4 || (CV_MINOR_VERSION ==4 && CV_SUBMINOR_VERSION<2))) + // disable all apriltag dictionaries + for(int i=17;i<21;++i) { + _ui->MarkerDictionary->setItemData(i, 0, Qt::UserRole - 1); + } + if(_ui->MarkerDictionary->currentIndex() >=17 && _ui->MarkerDictionary->currentIndex() <= 20) + { + _ui->MarkerDictionary->setCurrentIndex(Parameters::defaultMarkerDictionary()); + } +#else + if(_ui->MarkerDictionary->currentIndex() >=17 && _ui->MarkerDictionary->currentIndex() <= 20) + { + // If apriltag is selected, select apriltag refinement by default + _ui->OpenCVCornerRefinementMethod->setCurrentIndex(3); + } + else if(_ui->OpenCVCornerRefinementMethod->currentIndex() == 3) + { + // If not apriltag dictionary selected, reset refinement to default. + _ui->OpenCVCornerRefinementMethod->setCurrentIndex(Parameters::defaultMarkerOpenCVCornerRefinementMethod()); + } +#endif +#if CV_MAJOR_VERSION < 4 || (CV_MAJOR_VERSION == 4 && CV_MINOR_VERSION <8) + // disable aruco MPI dictionary + _ui->MarkerDictionary->setItemData(21, 0, Qt::UserRole - 1); + if(_ui->MarkerDictionary->currentIndex() == 21) + { + _ui->MarkerDictionary->setCurrentIndex(Parameters::defaultMarkerDictionary()); + } +#endif + } +} + void PreferencesDialog::updateOdometryStackedIndex(int index) { if(index == 11) // FLOAM -> LOAM @@ -5473,9 +5684,11 @@ void PreferencesDialog::updateOdometryStackedIndex(int index) _ui->groupBox_odomOKVIS->setVisible(index==6); _ui->groupBox_odomLOAM->setVisible(index==7); _ui->groupBox_odomMSCKF->setVisible(index==8); - _ui->groupBox_odomVINS->setVisible(index==9); + _ui->groupBox_odomVINSFusion->setVisible(index==9); _ui->groupBox_odomOpenVINS->setVisible(index==10); _ui->groupBox_odomOpen3D->setVisible(index==12); + _ui->groupBox_odomCuvslam->setVisible(index==13); + _ui->groupBox_odomLIOSAM->setVisible(index==14); } void PreferencesDialog::useOdomFeatures() @@ -5564,20 +5777,54 @@ void PreferencesDialog::changeOdometryOKVISConfigPath() } } -void PreferencesDialog::changeOdometryVINSConfigPath() +void PreferencesDialog::changeOdometryVINSFusionConfigPath() { QString path; - if(_ui->lineEdit_OdomVinsPath->text().isEmpty()) + if(_ui->lineEdit_OdomVinsFusionPath->text().isEmpty()) { path = QFileDialog::getOpenFileName(this, tr("VINS-Fusion Config"), this->getWorkingDirectory(), tr("VINS-Fusion config (*.yaml)")); } else { - path = QFileDialog::getOpenFileName(this, tr("VINS-Fusion Config"), _ui->lineEdit_OdomVinsPath->text(), tr("VINS-Fusion config (*.yaml)")); + path = QFileDialog::getOpenFileName(this, tr("VINS-Fusion Config"), _ui->lineEdit_OdomVinsFusionPath->text(), tr("VINS-Fusion config (*.yaml)")); } if(!path.isEmpty()) { - _ui->lineEdit_OdomVinsPath->setText(path); + _ui->lineEdit_OdomVinsFusionPath->setText(path); + } +} + +void PreferencesDialog::changeOdometryOpenVINSConfigPath() +{ + QString path; + if(_ui->lineEdit_openvinsConfigPath->text().isEmpty()) + { + path = QFileDialog::getOpenFileName(this, tr("OpenVINS Config"), this->getWorkingDirectory(), tr("OpenVINS config (*.yaml)")); + } + else + { + path = QFileDialog::getOpenFileName(this, tr("OpenVINS Config"), _ui->lineEdit_openvinsConfigPath->text(), tr("OpenVINS config (*.yaml)")); + } + if(!path.isEmpty()) + { + _ui->lineEdit_openvinsConfigPath->setText(path); + } +} + +void PreferencesDialog::changeOdometryLIOSAMConfigPath() +{ + QString path; + if(_ui->lineEdit_OdomLIOSAMPath->text().isEmpty()) + { + path = QFileDialog::getOpenFileName(this, tr("LIO-SAM Config"), this->getWorkingDirectory(), tr("LIO-SAM config (*.yaml)")); + } + else + { + path = QFileDialog::getOpenFileName(this, tr("LIO-SAM Config"), _ui->lineEdit_OdomLIOSAMPath->text(), tr("LIO-SAM config (*.yaml)")); + } + if(!path.isEmpty()) + { + _ui->lineEdit_OdomLIOSAMPath->setText(path); } } @@ -5649,6 +5896,41 @@ void PreferencesDialog::changeSuperPointModelPath() } } +void PreferencesDialog::changeSuperPointRpautratWeightsPath() +{ + QString path; + if(_ui->lineEdit_sprpautrat_weights_path->text().isEmpty()) + { + path = QFileDialog::getOpenFileName(this, tr("Select SuperPoint weights"), this->getWorkingDirectory(), tr("SuperPoint weights (*.pth)")); + } + else + { + path = QFileDialog::getOpenFileName(this, tr("Select SuperPoint weights"), _ui->lineEdit_sprpautrat_weights_path->text(), tr("SuperPoint weights (*.pth)")); + } + if(!path.isEmpty()) + { + _ui->lineEdit_sprpautrat_weights_path->setText(path); + } +} + +void PreferencesDialog::changeSuperPointRpautratModelPath() +{ + QString path; + if(_ui->lineEdit_sprpautrat_model_path->text().isEmpty()) + { + path = QFileDialog::getOpenFileName(this, tr("Select SuperPoint Python Model"), this->getWorkingDirectory(), tr("SuperPoint Python Model (*.py)")); + } + else + { + path = QFileDialog::getOpenFileName(this, tr("Select SuperPoint Python Model"), _ui->lineEdit_sprpautrat_model_path->text(), tr("SuperPoint Python Model (*.py)")); + } + if(!path.isEmpty()) + { + _ui->lineEdit_sprpautrat_model_path->setText(path); + } +} + + void PreferencesDialog::changePyMatcherPath() { QString path; @@ -5731,6 +6013,7 @@ void PreferencesDialog::updateSourceGrpVisibility() _ui->comboBox_cameraRGBD->currentIndex() == kSrcFreenect2-kSrcRGBD || _ui->comboBox_cameraRGBD->currentIndex() == kSrcK4W2 - kSrcRGBD || _ui->comboBox_cameraRGBD->currentIndex() == kSrcK4A - kSrcRGBD || + _ui->comboBox_cameraRGBD->currentIndex() == kSrcOrbbecSDK - kSrcRGBD || _ui->comboBox_cameraRGBD->currentIndex() == kSrcRealSense - kSrcRGBD || _ui->comboBox_cameraRGBD->currentIndex() == kSrcRGBDImages-kSrcRGBD || _ui->comboBox_cameraRGBD->currentIndex() == kSrcOpenNI_PCL-kSrcRGBD || @@ -5740,6 +6023,7 @@ void PreferencesDialog::updateSourceGrpVisibility() _ui->groupBox_freenect2->setVisible(_ui->comboBox_sourceType->currentIndex() == 0 && _ui->comboBox_cameraRGBD->currentIndex() == kSrcFreenect2-kSrcRGBD); _ui->groupBox_k4w2->setVisible(_ui->comboBox_sourceType->currentIndex() == 0 && _ui->comboBox_cameraRGBD->currentIndex() == kSrcK4W2 - kSrcRGBD); _ui->groupBox_k4a->setVisible(_ui->comboBox_sourceType->currentIndex() == 0 && _ui->comboBox_cameraRGBD->currentIndex() == kSrcK4A - kSrcRGBD); + _ui->groupBox_orbbec_sdk->setVisible(_ui->comboBox_sourceType->currentIndex() == 0 && _ui->comboBox_cameraRGBD->currentIndex() == kSrcOrbbecSDK - kSrcRGBD); _ui->groupBox_realsense->setVisible(_ui->comboBox_sourceType->currentIndex() == 0 && _ui->comboBox_cameraRGBD->currentIndex() == kSrcRealSense - kSrcRGBD); _ui->groupBox_realsense2->setVisible(_ui->comboBox_sourceType->currentIndex() == 0 && _ui->comboBox_cameraRGBD->currentIndex() == kSrcRealSense2 - kSrcRGBD); _ui->groupBox_cameraRGBDImages->setVisible(_ui->comboBox_sourceType->currentIndex() == 0 && _ui->comboBox_cameraRGBD->currentIndex() == kSrcRGBDImages-kSrcRGBD); @@ -5783,7 +6067,7 @@ void PreferencesDialog::updateSourceGrpVisibility() // Odom Sensor Group _ui->frame_visual_odometry_sensor->setVisible(getOdomSourceDriver() != kSrcUndef); // Not Lidar None - _ui->groupBox_odom_sensor->setVisible(_ui->comboBox_sourceType->currentIndex() != 3); // Don't show when database is selected + _ui->comboBox_odom_sensor->setEnabled(_ui->comboBox_sourceType->currentIndex() != 3); // Don't enable when database is selected // Lidar Sensor Group _ui->comboBox_lidar_src->setEnabled(_ui->comboBox_sourceType->currentIndex() != 3); // Disable if database input @@ -5809,6 +6093,7 @@ void PreferencesDialog::updateSourceGrpVisibility() (_ui->comboBox_sourceType->currentIndex() == 2 && _ui->source_comboBox_image_type->currentIndex() == kSrcImages-kSrcRGB) || (_ui->comboBox_sourceType->currentIndex() == 0 && _ui->comboBox_cameraRGBD->currentIndex() == kSrcFreenect - kSrcRGBD) || //Kinect360 (_ui->comboBox_sourceType->currentIndex() == 0 && _ui->comboBox_cameraRGBD->currentIndex() == kSrcK4A - kSrcRGBD) || //K4A + (_ui->comboBox_sourceType->currentIndex() == 0 && _ui->comboBox_cameraRGBD->currentIndex() == kSrcOrbbecSDK - kSrcRGBD) || //Orbbec SDK (_ui->comboBox_sourceType->currentIndex() == 0 && _ui->comboBox_cameraRGBD->currentIndex() == kSrcRealSense2 - kSrcRGBD) || //D435i (_ui->comboBox_sourceType->currentIndex() == 0 && _ui->comboBox_cameraRGBD->currentIndex() == kSrcSeerSense - kSrcRGBD) || (_ui->comboBox_sourceType->currentIndex() == 1 && _ui->comboBox_cameraStereo->currentIndex() == kSrcStereoRealSense2 - kSrcStereo) || //T265 @@ -5816,7 +6101,6 @@ void PreferencesDialog::updateSourceGrpVisibility() (_ui->comboBox_sourceType->currentIndex() == 1 && _ui->comboBox_cameraStereo->currentIndex() == kSrcStereoMyntEye - kSrcStereo) || // MYNT EYE S (_ui->comboBox_sourceType->currentIndex() == 1 && _ui->comboBox_cameraStereo->currentIndex() == kSrcStereoZedOC - kSrcStereo) || (_ui->comboBox_sourceType->currentIndex() == 1 && _ui->comboBox_cameraStereo->currentIndex() == kSrcStereoDepthAI - kSrcStereo)); - _ui->frame_imu_filtering->setVisible(getIMUFilteringStrategy() > 0); // Not None _ui->stackedWidget_imuFilter->setVisible(_ui->comboBox_imuFilter_strategy->currentIndex() > 0); _ui->groupBox_madgwickfilter->setVisible(_ui->comboBox_imuFilter_strategy->currentIndex() == 1); _ui->groupBox_complementaryfilter->setVisible(_ui->comboBox_imuFilter_strategy->currentIndex() == 2); @@ -5917,6 +6201,10 @@ bool PreferencesDialog::isOdomDisabled() const { return _ui->checkbox_odomDisabled->isChecked(); } +bool PreferencesDialog::isOdomAsGuessEnabled() const +{ + return _ui->checkbox_passthrough_source_odom->isChecked(); +} bool PreferencesDialog::isOdomSensorAsGt() const { return _ui->checkBox_odom_sensor_use_as_gt->isChecked(); @@ -6058,7 +6346,7 @@ bool PreferencesDialog::isMarkerDetection() const } double PreferencesDialog::getMarkerLength() const { - return _ui->ArucoMarkerLength->value(); + return _ui->MarkerLength->value(); } bool PreferencesDialog::isCloudMeshing() const { @@ -6395,6 +6683,15 @@ Transform PreferencesDialog::getLaserLocalTransform() const } return t; } +Transform PreferencesDialog::getGroundTruthLocalTransform() const +{ + Transform t = Transform::fromString(_ui->lineEdit_cameraImages_gt_transform->text().replace("PI_2", QString::number(3.141592/2.0)).toStdString()); + if(t.isNull()) + { + return Transform::getIdentity(); + } + return t; +} QString PreferencesDialog::getIMUPath() const { @@ -6637,6 +6934,26 @@ Camera * PreferencesDialog::createCamera( _ui->comboBox_k4a_framerate->currentIndex(), _ui->comboBox_k4a_depth_resolution->currentIndex()); } + else if (driver == kSrcOrbbecSDK) + { + camera = new CameraOrbbecSDK( + device.toStdString(), + _ui->spinBox_orbbec_sdk_color_width->value(), + _ui->spinBox_orbbec_sdk_color_height->value(), + _ui->spinBox_orbbec_sdk_depth_width->value(), + _ui->spinBox_orbbec_sdk_depth_height->value(), + this->getGeneralInputRate(), + this->getSourceLocalTransform()); + ((CameraOrbbecSDK*)camera)->enableColorRectification(_ui->checkBox_orbbec_sdk_color_rectification->isChecked()); + ((CameraOrbbecSDK*)camera)->enableImu(_ui->checkBox_orbbec_sdk_imu->isChecked()); + ((CameraOrbbecSDK*)camera)->enableDepthMM(_ui->checkBox_orbbec_sdk_depth_mm->isChecked()); + + camera->setInterIMUPublishing( + _ui->checkbox_publishInterIMU->isChecked(), + _ui->checkbox_publishInterIMU->isChecked() && getIMUFilteringStrategy()>0? + IMUFilter::create((IMUFilter::Type)(getIMUFilteringStrategy()-1), this->getAllParameters()):0, + getIMUFilteringBaseFrameConversion()); + } else if (driver == kSrcRealSense) { if(useRawImages && _ui->comboBox_realsenseRGBSource->currentIndex()!=2) @@ -6677,7 +6994,8 @@ Camera * PreferencesDialog::createCamera( camera->setInterIMUPublishing( _ui->checkbox_publishInterIMU->isChecked(), _ui->checkbox_publishInterIMU->isChecked() && getIMUFilteringStrategy()>0? - IMUFilter::create((IMUFilter::Type)(getIMUFilteringStrategy()-1), this->getAllParameters()):0); + IMUFilter::create((IMUFilter::Type)(getIMUFilteringStrategy()-1), this->getAllParameters()):0, + getIMUFilteringBaseFrameConversion()); if(driver == kSrcStereoRealSense2) { ((CameraRealSense2*)camera)->setImagesRectified((_ui->checkBox_stereo_rectify->isEnabled() && _ui->checkBox_stereo_rectify->isChecked()) && !useRawImages); @@ -6740,7 +7058,7 @@ Camera * PreferencesDialog::createCamera( ((CameraRGBDImages*)camera)->setMaxFrames(_ui->spinBox_cameraRGBDImages_maxFrames->value()); ((CameraRGBDImages*)camera)->setBayerMode(_ui->comboBox_cameraImages_bayerMode->currentIndex()-1); ((CameraRGBDImages*)camera)->setOdometryPath(_ui->lineEdit_cameraImages_odom->text().toStdString(), _ui->comboBox_cameraImages_odomFormat->currentIndex()); - ((CameraRGBDImages*)camera)->setGroundTruthPath(_ui->lineEdit_cameraImages_gt->text().toStdString(), _ui->comboBox_cameraImages_gtFormat->currentIndex()); + ((CameraRGBDImages*)camera)->setGroundTruthPath(_ui->lineEdit_cameraImages_gt->text().toStdString(), _ui->comboBox_cameraImages_gtFormat->currentIndex(), this->getGroundTruthLocalTransform()); ((CameraRGBDImages*)camera)->setMaxPoseTimeDiff(_ui->doubleSpinBox_maxPoseTimeDiff->value()); ((CameraRGBDImages*)camera)->setScanPath( _ui->lineEdit_cameraImages_path_scans->text().isEmpty()?"":_ui->lineEdit_cameraImages_path_scans->text().append(QDir::separator()).toStdString(), @@ -6786,7 +7104,7 @@ Camera * PreferencesDialog::createCamera( ((CameraStereoImages*)camera)->setMaxFrames(_ui->spinBox_cameraStereoImages_maxFrames->value()); ((CameraStereoImages*)camera)->setBayerMode(_ui->comboBox_cameraImages_bayerMode->currentIndex()-1); ((CameraStereoImages*)camera)->setOdometryPath(_ui->lineEdit_cameraImages_odom->text().toStdString(), _ui->comboBox_cameraImages_odomFormat->currentIndex()); - ((CameraStereoImages*)camera)->setGroundTruthPath(_ui->lineEdit_cameraImages_gt->text().toStdString(), _ui->comboBox_cameraImages_gtFormat->currentIndex()); + ((CameraStereoImages*)camera)->setGroundTruthPath(_ui->lineEdit_cameraImages_gt->text().toStdString(), _ui->comboBox_cameraImages_gtFormat->currentIndex(), this->getGroundTruthLocalTransform()); ((CameraStereoImages*)camera)->setMaxPoseTimeDiff(_ui->doubleSpinBox_maxPoseTimeDiff->value()); ((CameraStereoImages*)camera)->setScanPath( _ui->lineEdit_cameraImages_path_scans->text().isEmpty()?"":_ui->lineEdit_cameraImages_path_scans->text().append(QDir::separator()).toStdString(), @@ -6895,7 +7213,8 @@ Camera * PreferencesDialog::createCamera( camera->setInterIMUPublishing( _ui->checkbox_publishInterIMU->isChecked(), _ui->checkbox_publishInterIMU->isChecked() && getIMUFilteringStrategy()>0? - IMUFilter::create((IMUFilter::Type)(getIMUFilteringStrategy()-1), this->getAllParameters()):0); + IMUFilter::create((IMUFilter::Type)(getIMUFilteringStrategy()-1), this->getAllParameters()):0, + getIMUFilteringBaseFrameConversion()); ((CameraStereoZed*)camera)->setRightGrayScale(_ui->checkBox_stereo_rightGrayScale->isChecked()); } else if (driver == kSrcStereoZedOC) @@ -6944,7 +7263,8 @@ Camera * PreferencesDialog::createCamera( camera->setInterIMUPublishing( _ui->checkbox_publishInterIMU->isChecked(), _ui->checkbox_publishInterIMU->isChecked() && getIMUFilteringStrategy()>0? - IMUFilter::create((IMUFilter::Type)(getIMUFilteringStrategy()-1), this->getAllParameters()):0); + IMUFilter::create((IMUFilter::Type)(getIMUFilteringStrategy()-1), this->getAllParameters()):0, + getIMUFilteringBaseFrameConversion()); } else if(driver == kSrcUsbDevice) { @@ -6984,7 +7304,8 @@ Camera * PreferencesDialog::createCamera( _ui->comboBox_cameraImages_odomFormat->currentIndex()); ((CameraImages*)camera)->setGroundTruthPath( _ui->lineEdit_cameraImages_gt->text().toStdString(), - _ui->comboBox_cameraImages_gtFormat->currentIndex()); + _ui->comboBox_cameraImages_gtFormat->currentIndex(), + this->getGroundTruthLocalTransform()); ((CameraImages*)camera)->setMaxPoseTimeDiff(_ui->doubleSpinBox_maxPoseTimeDiff->value()); ((CameraImages*)camera)->setScanPath( _ui->lineEdit_cameraImages_path_scans->text().isEmpty()?"":_ui->lineEdit_cameraImages_path_scans->text().append(QDir::separator()).toStdString(), @@ -7038,6 +7359,54 @@ Camera * PreferencesDialog::createCamera( } } } + + std::vector localTransformOverrides; + if(_ui->source_checkBox_overrideLocalTransforms->isChecked()) + { + if(!_ui->lineEdit_sourceLocalTransform->text().isEmpty()) + { + std::list transforms = uSplit(_ui->lineEdit_sourceLocalTransform->text().replace("PI_2", QString::number(3.141592/2.0)).toStdString(), ';'); + for(auto t: transforms) + { + localTransformOverrides.push_back(Transform::fromString(t)); + } + + // offset(s)? + if(!_ui->source_lineEdit_databaseLocalTransformOffset->text().isEmpty()) + { + std::vector localTransformOffsetOverrides; + std::list offsetStr = uSplit(_ui->source_lineEdit_databaseLocalTransformOffset->text().toStdString(), ' '); + for(std::list::iterator iter=offsetStr.begin(); iter!=offsetStr.end(); ++iter) + { + localTransformOffsetOverrides.push_back(uStr2Float(*iter)); + UINFO("Camera offset = %f", localTransformOffsetOverrides.back()); + } + if(!localTransformOffsetOverrides.empty()) + { + if(!localTransformOverrides.empty() && localTransformOffsetOverrides.size() > 1 && localTransformOffsetOverrides.size() != localTransformOverrides.size()) + { + QMessageBox::warning(this, tr("DBReader"), + tr( "Camera lens offset vector size (%1) is not equal to local transform overrides (%2). " + "Camera lens offset vector should be one to affect all cameras or the same size than local transforms overrides.").arg(localTransformOffsetOverrides.size()).arg(localTransformOverrides.size()), QMessageBox::Ok); + return 0; + } + else { + for(size_t i=0; isource_lineEdit_databaseLocalTransformOffset->text().isEmpty()) + { + UWARN("Overriding camera offsets can only be used when camera local transforms are overriden. Ignoring offsets :\"%s\"", + _ui->source_lineEdit_databaseLocalTransformOffset->text().toStdString().c_str()); + } + } camera = new DBReader(_ui->source_database_lineEdit_path->text().toStdString(), _ui->source_checkBox_useDbStamps->isChecked()?-1:this->getGeneralInputRate(), @@ -7047,12 +7416,15 @@ Camera * PreferencesDialog::createCamera( _ui->source_spinBox_databaseStartId->value(), cameraIndices, _ui->source_spinBox_databaseStopId->value(), - !_ui->general_checkBox_createIntermediateNodes->isChecked(), + !_ui->source_checkBox_intermediateNodesAreNormalNodes->isChecked() && !_ui->general_checkBox_createIntermediateNodes->isChecked(), _ui->source_checkBox_ignoreLandmarks->isChecked(), _ui->source_checkBox_ignoreFeatures->isChecked(), 0, -1, - _ui->source_checkBox_ignorePriors->isChecked()); + _ui->source_checkBox_ignorePriors->isChecked(), + _ui->source_checkBox_ignoreIMU->isChecked(), + _ui->source_checkBox_intermediateNodesAreNormalNodes->isChecked(), + localTransformOverrides); } else { diff --git a/guilib/src/ProgressDialog.cpp b/guilib/src/ProgressDialog.cpp index ac2bdd0d..de921438 100644 --- a/guilib/src/ProgressDialog.cpp +++ b/guilib/src/ProgressDialog.cpp @@ -101,7 +101,7 @@ void ProgressDialog::setCancelButtonVisible(bool visible) void ProgressDialog::appendText(const QString & text, const QColor & color) { - UDEBUG(text.toStdString().c_str()); + //UDEBUG(text.toStdString().c_str()); _text->setText(text); QString html = tr("%1 %3").arg(QTime::currentTime().toString("HH:mm:ss")).arg(color.name()).arg(text); _detailedText->append(html); diff --git a/guilib/src/StatsToolBox.cpp b/guilib/src/StatsToolBox.cpp index 54933e94..cf6d0fbe 100644 --- a/guilib/src/StatsToolBox.cpp +++ b/guilib/src/StatsToolBox.cpp @@ -69,6 +69,7 @@ StatItem::StatItem(const QString & name, bool cacheOn, const std::vector _y = y; } _unit->setText(unit); + _value->setTextFormat(Qt::PlainText); this->updateMenu(menu); } @@ -90,7 +91,6 @@ void StatItem::addValue(qreal y) { _y.push_back(y); } - _value->setText(QString::number(y, 'g', 3)); Q_EMIT valueAdded(y); } @@ -107,7 +107,6 @@ void StatItem::addValue(qreal x, qreal y) _x.push_back(x); } - _value->setText(QString::number(y, 'g', 3)); Q_EMIT valueAdded(x,y); } @@ -117,18 +116,23 @@ void StatItem::setValues(const std::vector & x, const std::vector { _x = x; _y = y; - if(y.size()) - { - _value->setNum(y[y.size()-1]); - } - } - else - { - _value->setText("*"); } Q_EMIT valuesChanged(x,y); } +void StatItem::updateLabel() +{ + QString newText; + if(_y.size()) + { + newText = QString::number(_y.back(), 'g', 3); + } + if(newText != _value->text()) + { + _value->setText(newText); + } +} + QString StatItem::value() const { return _value->text(); @@ -227,6 +231,8 @@ StatsToolBox::StatsToolBox(QWidget * parent) : _plotMenu->addAction(tr("")); _workingDirectory = QDir::homePath(); _newFigureMaxItems = 0; + _updateLabelsTimer.setSingleShot(true); + connect(&_updateLabelsTimer, &QTimer::timeout, this, &StatsToolBox::updateLabels); } StatsToolBox::~StatsToolBox() @@ -263,6 +269,7 @@ void StatsToolBox::updateStat(const QString & statFullName, qreal y, bool cacheO std::vector vx,vy(1); vy[0] = y; updateStat(statFullName, vx, vy, cacheOn); + requestLabelsUpdate(); } void StatsToolBox::updateStat(const QString & statFullName, qreal x, qreal y, bool cacheOn) @@ -271,6 +278,7 @@ void StatsToolBox::updateStat(const QString & statFullName, qreal x, qreal y, bo vx[0] = x; vy[0] = y; updateStat(statFullName, vx, vy, cacheOn); + requestLabelsUpdate(); } void StatsToolBox::updateStat(const QString & statFullName, const std::vector & x, const std::vector & y, bool cacheOn) @@ -295,6 +303,7 @@ void StatsToolBox::updateStat(const QString & statFullName, const std::vectorsetValues(x, y); } + requestLabelsUpdate(); } else { @@ -378,6 +387,22 @@ void StatsToolBox::updateStat(const QString & statFullName, const std::vector items = _statBox->findChildren(); + for(int i=0; iupdateLabel(); + } +} + void StatsToolBox::plot(const StatItem * stat, const QString & plotName) { QWidget * fig = _figures.value(plotName, (QWidget*)0); diff --git a/guilib/src/images/IntRoLab.png b/guilib/src/images/IntRoLab.png index 12ed4e41..2d9ac03a 100644 Binary files a/guilib/src/images/IntRoLab.png and b/guilib/src/images/IntRoLab.png differ diff --git a/guilib/src/images/IntRoLabSmall.png b/guilib/src/images/IntRoLabSmall.png index e3a38fac..c5de684b 100644 Binary files a/guilib/src/images/IntRoLabSmall.png and b/guilib/src/images/IntRoLabSmall.png differ diff --git a/guilib/src/images/PauseNormal.png b/guilib/src/images/PauseNormal.png index b043b640..90d16d03 100644 Binary files a/guilib/src/images/PauseNormal.png and b/guilib/src/images/PauseNormal.png differ diff --git a/guilib/src/images/PauseNormalRed.png b/guilib/src/images/PauseNormalRed.png index 39a245b8..b332ab96 100644 Binary files a/guilib/src/images/PauseNormalRed.png and b/guilib/src/images/PauseNormalRed.png differ diff --git a/guilib/src/images/Play1Normal.png b/guilib/src/images/Play1Normal.png index bfa64fe2..245650bc 100644 Binary files a/guilib/src/images/Play1Normal.png and b/guilib/src/images/Play1Normal.png differ diff --git a/guilib/src/images/Plot16.png b/guilib/src/images/Plot16.png index 91432b13..57caaba0 100644 Binary files a/guilib/src/images/Plot16.png and b/guilib/src/images/Plot16.png differ diff --git a/guilib/src/images/Plot48.png b/guilib/src/images/Plot48.png index fa475cb6..e89d24b7 100644 Binary files a/guilib/src/images/Plot48.png and b/guilib/src/images/Plot48.png differ diff --git a/guilib/src/images/RTAB-Map.png b/guilib/src/images/RTAB-Map.png index a14115b7..ace8aed3 100644 Binary files a/guilib/src/images/RTAB-Map.png and b/guilib/src/images/RTAB-Map.png differ diff --git a/guilib/src/images/RTAB-Map100.png b/guilib/src/images/RTAB-Map100.png index 56c6628a..713198ef 100644 Binary files a/guilib/src/images/RTAB-Map100.png and b/guilib/src/images/RTAB-Map100.png differ diff --git a/guilib/src/images/RTAB-Map2.png b/guilib/src/images/RTAB-Map2.png index 26689e08..0ceae5ca 100644 Binary files a/guilib/src/images/RTAB-Map2.png and b/guilib/src/images/RTAB-Map2.png differ diff --git a/guilib/src/images/Stop1Normal.png b/guilib/src/images/Stop1Normal.png index a28d9f43..6449c1f1 100644 Binary files a/guilib/src/images/Stop1Normal.png and b/guilib/src/images/Stop1Normal.png differ diff --git a/guilib/src/images/Stop1NormalYellow.png b/guilib/src/images/Stop1NormalYellow.png index 743603d9..416b582e 100644 Binary files a/guilib/src/images/Stop1NormalYellow.png and b/guilib/src/images/Stop1NormalYellow.png differ diff --git a/guilib/src/images/astra.png b/guilib/src/images/astra.png index 82593017..c13edebc 100644 Binary files a/guilib/src/images/astra.png and b/guilib/src/images/astra.png differ diff --git a/guilib/src/images/astra2.png b/guilib/src/images/astra2.png new file mode 100644 index 00000000..45af0365 Binary files /dev/null and b/guilib/src/images/astra2.png differ diff --git a/guilib/src/images/bumblebee2.png b/guilib/src/images/bumblebee2.png index 15e9f94a..ad1282f7 100644 Binary files a/guilib/src/images/bumblebee2.png and b/guilib/src/images/bumblebee2.png differ diff --git a/guilib/src/images/d415.png b/guilib/src/images/d415.png index d536cfe3..dccd34ae 100644 Binary files a/guilib/src/images/d415.png and b/guilib/src/images/d415.png differ diff --git a/guilib/src/images/d435.png b/guilib/src/images/d435.png index 667f6f8f..2d23a623 100644 Binary files a/guilib/src/images/d435.png and b/guilib/src/images/d435.png differ diff --git a/guilib/src/images/document-new.png b/guilib/src/images/document-new.png index e6d64bb9..7dfd0d9e 100644 Binary files a/guilib/src/images/document-new.png and b/guilib/src/images/document-new.png differ diff --git a/guilib/src/images/document-open.png b/guilib/src/images/document-open.png index f35f2583..805c0df0 100644 Binary files a/guilib/src/images/document-open.png and b/guilib/src/images/document-open.png differ diff --git a/guilib/src/images/document-properties.png b/guilib/src/images/document-properties.png index fa697db4..3144963e 100644 Binary files a/guilib/src/images/document-properties.png and b/guilib/src/images/document-properties.png differ diff --git a/guilib/src/images/document-save.png b/guilib/src/images/document-save.png index db5c52b7..e6fd350b 100644 Binary files a/guilib/src/images/document-save.png and b/guilib/src/images/document-save.png differ diff --git a/guilib/src/images/k4a.png b/guilib/src/images/k4a.png index 644398dc..9071330b 100644 Binary files a/guilib/src/images/k4a.png and b/guilib/src/images/k4a.png differ diff --git a/guilib/src/images/kinect_xbox_360.png b/guilib/src/images/kinect_xbox_360.png index 9e610e81..26fd8fba 100644 Binary files a/guilib/src/images/kinect_xbox_360.png and b/guilib/src/images/kinect_xbox_360.png differ diff --git a/guilib/src/images/kinect_xbox_one.png b/guilib/src/images/kinect_xbox_one.png index 3df73aa6..11537c4e 100644 Binary files a/guilib/src/images/kinect_xbox_one.png and b/guilib/src/images/kinect_xbox_one.png differ diff --git a/guilib/src/images/l515.png b/guilib/src/images/l515.png index 6bf73ad5..25f293c6 100644 Binary files a/guilib/src/images/l515.png and b/guilib/src/images/l515.png differ diff --git a/guilib/src/images/mag_glass.png b/guilib/src/images/mag_glass.png index f9f573c9..c308c61c 100644 Binary files a/guilib/src/images/mag_glass.png and b/guilib/src/images/mag_glass.png differ diff --git a/guilib/src/images/mynteyes.png b/guilib/src/images/mynteyes.png index 0c1bcd02..62bdf31d 100644 Binary files a/guilib/src/images/mynteyes.png and b/guilib/src/images/mynteyes.png differ diff --git a/guilib/src/images/oakd.png b/guilib/src/images/oakd.png index b6c25cc8..12252aa1 100644 Binary files a/guilib/src/images/oakd.png and b/guilib/src/images/oakd.png differ diff --git a/guilib/src/images/oakd_lite.png b/guilib/src/images/oakd_lite.png index 6515d25e..4f186324 100644 Binary files a/guilib/src/images/oakd_lite.png and b/guilib/src/images/oakd_lite.png differ diff --git a/guilib/src/images/oakdpro.png b/guilib/src/images/oakdpro.png index 9308b34a..daef30cd 100644 Binary files a/guilib/src/images/oakdpro.png and b/guilib/src/images/oakdpro.png differ diff --git a/guilib/src/images/r200.png b/guilib/src/images/r200.png index 8531ac07..9a997ca7 100644 Binary files a/guilib/src/images/r200.png and b/guilib/src/images/r200.png differ diff --git a/guilib/src/images/seer_sense_DS80.png b/guilib/src/images/seer_sense_DS80.png index e2fa5fd3..0485310a 100644 Binary files a/guilib/src/images/seer_sense_DS80.png and b/guilib/src/images/seer_sense_DS80.png differ diff --git a/guilib/src/images/sense.png b/guilib/src/images/sense.png index 78775b51..2ac3b601 100644 Binary files a/guilib/src/images/sense.png and b/guilib/src/images/sense.png differ diff --git a/guilib/src/images/sr300.png b/guilib/src/images/sr300.png index 48a6b6c8..1593884f 100644 Binary files a/guilib/src/images/sr300.png and b/guilib/src/images/sr300.png differ diff --git a/guilib/src/images/system-log-out.png b/guilib/src/images/system-log-out.png index fddbc2bc..fe7e2191 100644 Binary files a/guilib/src/images/system-log-out.png and b/guilib/src/images/system-log-out.png differ diff --git a/guilib/src/images/t265.png b/guilib/src/images/t265.png index bb8d7e2f..c0738ad1 100644 Binary files a/guilib/src/images/t265.png and b/guilib/src/images/t265.png differ diff --git a/guilib/src/images/tara.png b/guilib/src/images/tara.png index 29d52b5f..5ced46c9 100644 Binary files a/guilib/src/images/tara.png and b/guilib/src/images/tara.png differ diff --git a/guilib/src/images/view-refresh.png b/guilib/src/images/view-refresh.png index 606ea9eb..8e59f043 100644 Binary files a/guilib/src/images/view-refresh.png and b/guilib/src/images/view-refresh.png differ diff --git a/guilib/src/images/webcam.png b/guilib/src/images/webcam.png index 07c88755..3fc1f234 100644 Binary files a/guilib/src/images/webcam.png and b/guilib/src/images/webcam.png differ diff --git a/guilib/src/images/xtion_pro_live.png b/guilib/src/images/xtion_pro_live.png index cf464a8b..7864d343 100644 Binary files a/guilib/src/images/xtion_pro_live.png and b/guilib/src/images/xtion_pro_live.png differ diff --git a/guilib/src/images/zed.png b/guilib/src/images/zed.png index 8bfc30f4..d2419587 100644 Binary files a/guilib/src/images/zed.png and b/guilib/src/images/zed.png differ diff --git a/guilib/src/images/zr300.png b/guilib/src/images/zr300.png index 6926d070..8bd47243 100644 Binary files a/guilib/src/images/zr300.png and b/guilib/src/images/zr300.png differ diff --git a/guilib/src/ui/DatabaseViewer.ui b/guilib/src/ui/DatabaseViewer.ui index 48717272..cc7918fb 100644 --- a/guilib/src/ui/DatabaseViewer.ui +++ b/guilib/src/ui/DatabaseViewer.ui @@ -17,7 +17,7 @@ Qt::LeftToRight - + 0 @@ -44,7 +44,7 @@ - + 0 @@ -61,7 +61,7 @@ 0 0 - 307 + 364 389 @@ -392,7 +392,7 @@ 0 0 - 306 + 364 389 @@ -776,71 +776,90 @@ - - - - 12 - - - 12 - - - 12 - - - 12 - - - - - - - Index : - + + + + 0 + + + 0 + + + 0 + + + 0 + + + 0 + + + + + 12 + + + 12 + + + 12 + + + 12 + + + + + + + Index : + + + + + + + Id : + + + + + + + + + + + false + + + + + + + idB + + + + + + + + + Qt::ClickFocus + + + Qt::Horizontal + + + QSlider::TicksAbove + - - - - - Id : - - - + - - - - - - - false - - - - - - - idB - - - - - - - - - Qt::ClickFocus - - - Qt::Horizontal - - - QSlider::TicksAbove - - - + - + @@ -1252,322 +1271,382 @@ - - + + + + 0 + + + 0 + + + 0 + + + 0 + + + 0 + - + - - - 0.0 deg - - - - - - - Qt::ClickFocus - - - -1799 - - - 1800 - - - 0 - - - Qt::Horizontal - - - QSlider::TicksAbove - - - 100 - - - - - - - <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> - - - Apply Rotation - - - - - - - - - - - # - - - - - - - Qt::ClickFocus - - - Qt::Horizontal - - - QSlider::TicksAbove - - - - - - - QComboBox::AdjustToContents - + + - Global Iterative + 0.0 deg + - - Global Full + + + Qt::ClickFocus + + -1799 + + + 1800 + + + 0 + + + Qt::Horizontal + + + QSlider::TicksAbove + + + 100 + + - - Local Optimized + + + <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> + + Apply Rotation + + - + - - - - - - - - Align poses with ground truth - - - true - - - - - - - - - - false - - - - - + + - + - Root + # - + - + + + Qt::ClickFocus + + + Qt::Horizontal + + + QSlider::TicksAbove + + + + + + + QComboBox::AdjustToContents + + + + Global Iterative + + + + + Global Full + + + + + Local Optimized + + + + + + + + + + - Span to all maps + Align poses with GPS + + + true + + + + + + + Time grid (s) + + + + + + + Time optimization (s) + + + + + + + + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + - true + true - + - - + + - WM + - + + true + + - + + + + + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + Align poses with ground truth + + + true + + + + + + + RMSE (m) + + + + + + + + + Root + + + + + + + Span to all maps + + + true + + + + + + + WM + + + + + + + + + + + + + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + Ignore intermediate nodes + + + true + + + + + + + + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + Poses + + + + + + + Path length (m) + + + + + + + Align scans/clouds with ground truth + + + true + + + + + + + <html><head/><body><p>N: Neighbor</p><p>NM: Neighbor Merged</p><p>G: Global</p><p>LS: Local by Space (Proximity)</p><p>LT: Local by Time (Proximity)</p><p>U: User</p><p>P: Prior</p><p>LM: Landmark</p><p>GR: Gravity</p></body></html> + + + Links (N, NM, G, LS, LT, U, P, LM, GR) + + + + + + + + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + + false + + + + + + + + + + true + + + + + + + Env Sensor Colormap + + + + + + + + Disabled + + + + + Wifi + + + + + Temperature + + + + + Air Pressure + + + + + Light + + + + + Relative Humidity + + + + + - - - - Time grid (s) - - - - - - - RMSE (m) - - - - - - - - - - true - - - - - - - - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - Align scans/clouds with ground truth - - - true - - - - - - - - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - <html><head/><body><p>N: Neighbor</p><p>NM: Neighbor Merged</p><p>G: Global</p><p>LS: Local by Space (Proximity)</p><p>LT: Local by Time (Proximity)</p><p>U: User</p><p>P: Prior</p><p>LM: Landmark</p><p>GR: Gravity</p></body></html> - - - Links (N, NM, G, LS, LT, U, P, LM, GR) - - - - - - - Path length (m) - - - - - - - Poses - - - - - - - - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - - - - - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - Ignore intermediate nodes - - - true - - - - - - - Time optimization (s) - - - - - - - - - - true - - - - - - - Align poses with GPS - - - true - - - - - - - - - - true - - - - + - + @@ -1643,14 +1722,14 @@ - 1 + 2 0 0 - 314 + 403 188 @@ -1805,7 +1884,7 @@ 0 - -316 + 0 518 1071 @@ -1815,7 +1894,7 @@ - + @@ -2525,15 +2604,133 @@ 0 - 0 - 296 - 272 + -175 + 403 + 319 Detect more loop closures + + + + Angle + + + + + + + Inter-session + + + + + + + + + + + + + + Radius Max + + + + + + + Intra-session + + + + + + + Qt::Vertical + + + + 20 + 40 + + + + + + + + degrees + + + 0 + + + 180.000000000000000 + + + 1.000000000000000 + + + 30.000000000000000 + + + + + + + Radius Min + + + + + + + 1 + + + 100 + + + 5 + + + + + + + From/to map ID only (-1 is all) + + + + + + + Iterations + + + + + + + + + + true + + + + + + + + + + @@ -2553,63 +2750,13 @@ - - + + - Radius Min + Use optimized graph as guess - - - - - - Intra-session - - - - - - - Qt::Vertical - - - - 20 - 40 - - - - - - - - Radius Max - - - - - - - Angle - - - - - - - degrees - - - 0 - - - 180.000000000000000 - - - 1.000000000000000 - - - 30.000000000000000 + + true @@ -2632,64 +2779,36 @@ - - + + + + -1 + + + 9999 + + + -1 + + + + + + + Minimum graph distance + + + + + 1 - 100 + 9999 - 5 - - - - - - - - - - - - - - Iterations - - - - - - - Inter-session - - - - - - - - - - - - - - - - - true - - - - - - - Use optimized graph as guess - - - true + 10 diff --git a/guilib/src/ui/aboutDialog.ui b/guilib/src/ui/aboutDialog.ui index d78e8c7b..532c9764 100644 --- a/guilib/src/ui/aboutDialog.ui +++ b/guilib/src/ui/aboutDialog.ui @@ -162,9 +162,9 @@ p, li { white-space: pre-wrap; } 0 - -43 + 0 596 - 1165 + 1280 @@ -185,697 +185,7 @@ p, li { white-space: pre-wrap; } - - - - - - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter - - - true - - - - - - - BSD - - - true - - - - - - - GPLv3 - - - true - - - - - - - MIT - - - true - - - - - - - With Open3D : - - - true - - - - - - - GPLv3 - - - true - - - - - - - BSD - - - true - - - - - - - PCL version : - - - true - - - - - - - With Python3 : - - - true - - - - - - - With FastCV : - - - true - - - - - - - MPL2 - - - true - - - - - - - - - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter - - - true - - - - - - - MIT - - - true - - - - - - - Apache-2 - - - true - - - - - - - BSD - - - true - - - - - - - With SuperPoint Torch : - - - true - - - - - - - Apache v2 and/or GPLv2 - - - true - - - - - - - - - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter - - - true - - - - - - - - - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter - - - true - - - - - - - With stereo FlyCapture2 : - - - true - - - - - - - - - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter - - - true - - - - - - - With Zed SDK : - - - true - - - - - - - Apache v2 - - - true - - - - - - - - - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter - - - true - - - - - - - With RealSense : - - - true - - - - - - - With OpenChisel : - - - true - - - - - - - With TORO : - - - true - - - - - - - - - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter - - - true - - - - - - - - - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter - - - true - - - - - - - PSF - - - true - - - - - - - With DepthAI : - - - true - - - - - - - With AliceVision : - - - true - - - - - - - - - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter - - - true - - - - - - - - - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter - - - true - - - - - - - With DVO : - - - true - - - - - - - - - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter - - - true - - - - - - - - - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter - - - true - - - - - - - LGPL - - - true - - - - - - - With libpointmatcher : - - - true - - - - - - - With MSCKF : - - - true - - - - - - - With CPU-TSDF : - - - true - - - - - - - - - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter - - - true - - - - - - - With MYNTEYE S : - - - true - - - - - - - - - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter - - - true - - - - - - - MIT - - - true - - - - - - - MIT - - - true - - - - - - - With g2o : - - - true - - - - - - BSD - - - true - - - - - - - BSD - - - true - - - - - - - - - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter - - - true - - - - - - - - - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter - - - true - - - - - - - GPLv2 - - - true - - - - - - - With OpenNI2 : - - - true - - - - - - - Apache-2 - - - true - - - - - - - Qt version : - - - true - - - - - - - - - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter - - - true - - - - - - - With stereo dc1394 : - - - true - - - - - - - - - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter - - - true - - - - - - - - - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter - - - true - - - - - - - BSD - - - true - - - - - - - With ORB SLAM 2 : - - - true - - - - - - - With Ceres : - - - true - - - - - - - - - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter - - - true - - - - Apache v2 @@ -885,231 +195,17 @@ p, li { white-space: pre-wrap; } - - - - With OKVIS : - - - true - - - - - - - With XVisio SDK : - - - true - - - - - - - VTK version : - - - true - - - - - - - Creative Commons [Attribution-NonCommercial-ShareAlike] - - - true - - - - - - - With K4A : - - - true - - - - - - - BSD - - - true - - - - - - - BSD - - - true - - - - - - - - - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter - - - true - - - - - - - - - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter - - - true - - - - - - - BSD - - - true - - - - - - - - - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter - - - true - - - - - - - With GridMap : - - - true - - - - - - - - - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter - - - true - - - - - - - GPLv3 - - - true - - - - + - With Viso2 : + With CPU-TSDF : true - - - - - - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter - - - true - - - - - - - - - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter - - - true - - - - - - - - - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter - - - true - - - - - - - With GTSAM : - - - true - - - - - - - - - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter - - - true - - - - + @@ -1122,142 +218,20 @@ p, li { white-space: pre-wrap; } - - + + - - - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + With CudaSift : true - - + + - MIT - - - true - - - - - - - With CCCoreLib : - - - true - - - - - - - Apache v2 and/or GPLv2 - - - true - - - - - - - - - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter - - - true - - - - - - - - - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter - - - true - - - - - - - With FOVIS : - - - true - - - - - - - BSD - - - true - - - - - - - BSD - - - true - - - - - - - With PDAL : - - - true - - - - - - - With Zed Open Capture : - - - true - - - - - - - With ORB Octree : - - - true - - - - - - - - - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + With Zed SDK : true @@ -1277,8 +251,98 @@ p, li { white-space: pre-wrap; } - - + + + + With PDAL : + + + true + + + + + + + BSD + + + true + + + + + + + With Zed Open Capture : + + + true + + + + + + + BSD + + + true + + + + + + + With ORB SLAM 2 : + + + true + + + + + + + With MSCKF : + + + true + + + + + + + GPLv3 + + + true + + + + + + + BSD + + + true + + + + + + + With K4W2 : + + + true + + + + + @@ -1290,60 +354,30 @@ p, li { white-space: pre-wrap; } - - + + - BSD + With OpenChisel : true - - + + - GPLv3 + MIT true - - + + - With CudaSift : - - - true - - - - - - - GPLv2 - - - true - - - - - - - BSD - - - true - - - - - - - <html><head/><body><p><span style=" font-weight:600;">License</span></p></body></html> + With RealSense : true @@ -1351,6 +385,19 @@ p, li { white-space: pre-wrap; } + + + + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + + + true + + + + @@ -1363,8 +410,8 @@ p, li { white-space: pre-wrap; } - - + + @@ -1376,6 +423,39 @@ p, li { white-space: pre-wrap; } + + + + + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + + + true + + + + + + + With Freenect2 : + + + true + + + + + + + <html><head/><body><p><span style=" font-weight:600;">License</span></p></body></html> + + + true + + + @@ -1389,21 +469,18 @@ p, li { white-space: pre-wrap; } - - + + - - - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + GPLv3 true - - + + BSD @@ -1412,68 +489,18 @@ p, li { white-space: pre-wrap; } - - + + - With cvsba : + With libpointmatcher : true - - - - Open Source or Commercial - - - true - - - - - - - Apache v2 - - - true - - - - - - - With Freenect : - - - true - - - - - - - With OpenVINS : - - - true - - - - - - - With OpenNI : - - - true - - - - - + + @@ -1485,8 +512,48 @@ p, li { white-space: pre-wrap; } - - + + + + BSD + + + true + + + + + + + With MYNTEYE S : + + + true + + + + + + + BSD + + + true + + + + + + + Creative Commons [Attribution-NonCommercial-ShareAlike] + + + true + + + + + @@ -1498,18 +565,18 @@ p, li { white-space: pre-wrap; } - - + + - With Octomap : + BSD true - - + + @@ -1521,42 +588,6 @@ p, li { white-space: pre-wrap; } - - - - - - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter - - - true - - - - - - - - - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter - - - true - - - - - - - GPLv2 - - - true - - - @@ -1567,63 +598,7 @@ p, li { white-space: pre-wrap; } - - - - - - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter - - - true - - - - - - - With Freenect2 : - - - true - - - - - - - - - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter - - - true - - - - - - - With VINS-Fusion : - - - true - - - - - - - With MRPT : - - - true - - - - + GPLv3 @@ -1633,16 +608,6 @@ p, li { white-space: pre-wrap; } - - - - With loam_velodyne : - - - true - - - @@ -1656,7 +621,56 @@ p, li { white-space: pre-wrap; } - + + + + + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + + + true + + + + + + + + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + + + true + + + + + + + BSD + + + true + + + + + + + + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + + + true + + + + With RealSense2 : @@ -1666,6 +680,758 @@ p, li { white-space: pre-wrap; } + + + + With g2o : + + + true + + + + + + + + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + + + true + + + + + + + MIT + + + true + + + + + + + + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + + + true + + + + + + + Penn Software License + + + true + + + + + + + + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + + + true + + + + + + + With Orbbec SDK : + + + true + + + + + + + + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + + + true + + + + + + + + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + + + true + + + + + + + With OpenGV : + + + true + + + + + + + + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + + + true + + + + + + + + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + + + true + + + + + + + + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + + + true + + + + + + + With XVisio SDK : + + + true + + + + + + + + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + + + true + + + + + + + With loam_velodyne : + + + true + + + + + + + With TORO : + + + true + + + + + + + With OpenNI : + + + true + + + + + + + Apache v2 + + + true + + + + + + + With cuVSLAM : + + + true + + + + + + + With Python3 : + + + true + + + + + + + With Viso2 : + + + true + + + + + + + + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + + + true + + + + + + + With FOVIS : + + + true + + + + + + + PCL version : + + + true + + + + + + + + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + + + true + + + + + + + With cvsba : + + + true + + + + + + + With SuperPoint Rpautrat : + + + + + + + MIT + + + + + + + With OpenNI2 : + + + true + + + + + + + BSD + + + true + + + + + + + + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + + + true + + + + + + + MPL2 + + + true + + + + + + + + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + + + true + + + + + + + BSD + + + true + + + + + + + With AliceVision : + + + true + + + + + + + BSD + + + true + + + + + + + + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + + + true + + + + + + + MIT + + + true + + + + + + + BSD + + + true + + + + + + + MIT + + + true + + + + + + + + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + + + true + + + + + + + + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + + + true + + + + + + + + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + + + true + + + + + + + With FastCV : + + + true + + + + + + + + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + + + true + + + + + + + With Open3D : + + + true + + + + + + + Apache-2 + + + true + + + + + + + + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + + + true + + + + + + + BSD + + + true + + + + + + + With VINS-Fusion : + + + true + + + + + + + + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + + + true + + + + + + + GPLv2 + + + true + + + + + + + + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + + + true + + + + + + + BSD + + + true + + + + + + + With OKVIS : + + + true + + + + + + + + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + + + true + + + + + + + + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + + + true + + + + + + + + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + + + true + + + + + + + + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + + + true + + + + + + + With stereo dc1394 : + + + true + + + + + + + GPLv3 + + + true + + + + + + + Apache v2 and/or GPLv2 + + + true + + + + + + + With OpenVINS : + + + true + + + + + + + Apache v2 and/or GPLv2 + + + true + + + + + + + + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + + + true + + + + + + + Qt version : + + + true + + + + + + + GPLv2 + + + true + + + @@ -1676,17 +1442,80 @@ p, li { white-space: pre-wrap; } - - + + - With K4W2 : + NVIDIA ISAAC ROS SOFTWARE LICENSE true - + + + + Apache-2 + + + true + + + + + + + GPLv3 + + + true + + + + + + + MIT + + + true + + + + + + + + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + + + true + + + + + + + BSD + + + true + + + + + + + BSD + + + true + + + + @@ -1699,27 +1528,142 @@ p, li { white-space: pre-wrap; } - - + + - Penn Software License + Open Source or Commercial true - - + + - GPLv3 + With Ceres : true - + + + + LGPL + + + true + + + + + + + + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + + + true + + + + + + + + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + + + true + + + + + + + GPLv2 + + + true + + + + + + + With K4A : + + + true + + + + + + + + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + + + true + + + + + + + + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + + + true + + + + + + + With DepthAI : + + + true + + + + + + + + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + + + true + + + + + + + With Freenect : + + + true + + + + @@ -1732,17 +1676,143 @@ p, li { white-space: pre-wrap; } - - + + - BSD + MIT true - + + + + + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + + + true + + + + + + + With MRPT : + + + true + + + + + + + With SuperPoint Torch : + + + true + + + + + + + PSF + + + true + + + + + + + With CCCoreLib : + + + true + + + + + + + + + + + + + + + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + + + true + + + + + + + With Octomap : + + + true + + + + + + + + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + + + true + + + + + + + With DVO : + + + true + + + + + + + Apache v2 + + + true + + + + + + + With GridMap : + + + true + + + + With libLAS : @@ -1752,8 +1822,91 @@ p, li { white-space: pre-wrap; } + + + + VTK version : + + + true + + + + + + + With GTSAM : + + + true + + + + + + + With ORB Octree : + + + true + + + + + + + + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + + + true + + + + + + + GPLv3 + + + true + + + + + + + With stereo FlyCapture2 : + + + true + + + + + + + BSD + + + true + + + + + + + With AprilTag : + + + true + + + - + @@ -1800,7 +1953,7 @@ p, li { white-space: pre-wrap; } - Copyright (C) 2010-2024 IntRoLab - Université de Sherbrooke + Copyright (C) 2010-2026 IntRoLab - Université de Sherbrooke Qt::AlignCenter diff --git a/guilib/src/ui/calibrationDialog.ui b/guilib/src/ui/calibrationDialog.ui index b56336d4..d4292528 100644 --- a/guilib/src/ui/calibrationDialog.ui +++ b/guilib/src/ui/calibrationDialog.ui @@ -360,7 +360,7 @@ false - 2 + 3 8 @@ -373,7 +373,7 @@ false - 2 + 3 6 diff --git a/guilib/src/ui/consoleWidget.ui b/guilib/src/ui/consoleWidget.ui index 112de6f7..d824ad19 100644 --- a/guilib/src/ui/consoleWidget.ui +++ b/guilib/src/ui/consoleWidget.ui @@ -35,7 +35,7 @@ 99999 - 100 + 500 diff --git a/guilib/src/ui/mainWindow.ui b/guilib/src/ui/mainWindow.ui index 666e809a..fc4519a0 100644 --- a/guilib/src/ui/mainWindow.ui +++ b/guilib/src/ui/mainWindow.ui @@ -18,7 +18,7 @@ :/images/RTAB-Map.ico:/images/RTAB-Map.ico - QMainWindow::AllowNestedDocks|QMainWindow::AllowTabbedDocks|QMainWindow::AnimatedDocks|QMainWindow::VerticalTabs + QMainWindow::DockOption::AllowNestedDocks|QMainWindow::DockOption::AllowTabbedDocks|QMainWindow::DockOption::AnimatedDocks|QMainWindow::DockOption::VerticalTabs @@ -27,7 +27,7 @@ 0 0 1012 - 22 + 21 @@ -239,9 +239,20 @@ + + + Orbbec Astra 2 + + + + :/images/astra2.png:/images/astra2.png + + + + @@ -483,7 +494,7 @@ - true + false Statistics @@ -511,7 +522,7 @@ - QLayout::SetMinimumSize + QLayout::SizeConstraint::SetMinimumSize 2 @@ -519,7 +530,7 @@ - Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + Qt::AlignmentFlag::AlignRight|Qt::AlignmentFlag::AlignTrailing|Qt::AlignmentFlag::AlignVCenter ms @@ -551,14 +562,14 @@ Unknown - Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + Qt::AlignmentFlag::AlignRight|Qt::AlignmentFlag::AlignTrailing|Qt::AlignmentFlag::AlignVCenter - Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + Qt::AlignmentFlag::AlignRight|Qt::AlignmentFlag::AlignTrailing|Qt::AlignmentFlag::AlignVCenter Hz @@ -593,7 +604,7 @@ Unknown - Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + Qt::AlignmentFlag::AlignRight|Qt::AlignmentFlag::AlignTrailing|Qt::AlignmentFlag::AlignVCenter @@ -610,7 +621,7 @@ Unknown - Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + Qt::AlignmentFlag::AlignRight|Qt::AlignmentFlag::AlignTrailing|Qt::AlignmentFlag::AlignVCenter @@ -627,7 +638,7 @@ 0 - Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + Qt::AlignmentFlag::AlignRight|Qt::AlignmentFlag::AlignTrailing|Qt::AlignmentFlag::AlignVCenter @@ -645,7 +656,7 @@ 0 - Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + Qt::AlignmentFlag::AlignRight|Qt::AlignmentFlag::AlignTrailing|Qt::AlignmentFlag::AlignVCenter @@ -662,7 +673,7 @@ 0 - Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + Qt::AlignmentFlag::AlignRight|Qt::AlignmentFlag::AlignTrailing|Qt::AlignmentFlag::AlignVCenter @@ -686,7 +697,7 @@ - Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + Qt::AlignmentFlag::AlignRight|Qt::AlignmentFlag::AlignTrailing|Qt::AlignmentFlag::AlignVCenter Hz @@ -720,7 +731,7 @@ - Qt::Vertical + Qt::Orientation::Vertical @@ -882,7 +893,7 @@ - true + false Map visibility @@ -945,7 +956,7 @@ - Qt::AllDockWidgetAreas + Qt::DockWidgetArea::AllDockWidgetAreas Loop closure detection @@ -987,7 +998,7 @@ - Qt::AlignCenter + Qt::AlignmentFlag::AlignCenter @@ -1013,7 +1024,7 @@ - Qt::AlignCenter + Qt::AlignmentFlag::AlignCenter @@ -1411,6 +1422,9 @@ + + true + :/images/document-save.png:/images/document-save.png @@ -1749,6 +1763,14 @@ Xvisio + + + true + + + Orbbec SDK + + diff --git a/guilib/src/ui/postProcessingDialog.ui b/guilib/src/ui/postProcessingDialog.ui index f6fbe23e..de0292fb 100644 --- a/guilib/src/ui/postProcessingDialog.ui +++ b/guilib/src/ui/postProcessingDialog.ui @@ -6,8 +6,8 @@ 0 0 - 552 - 633 + 553 + 662 @@ -67,10 +67,10 @@ - - + + - Cluster radius + Inter-session true @@ -87,10 +87,10 @@ - - + + - Iterations + Cluster radius true @@ -120,16 +120,6 @@ - - - - Inter-session - - - true - - - @@ -137,6 +127,16 @@ + + + + Iterations + + + true + + + @@ -144,6 +144,32 @@ + + + + Minimum graph distance + + + true + + + + + + + nodes + + + 1 + + + 9999 + + + 10 + + + diff --git a/guilib/src/ui/preferencesDialog.ui b/guilib/src/ui/preferencesDialog.ui index 5e3f6235..64eff092 100644 --- a/guilib/src/ui/preferencesDialog.ui +++ b/guilib/src/ui/preferencesDialog.ui @@ -6,7 +6,7 @@ 0 0 - 1009 + 980 925 @@ -63,9 +63,9 @@ 0 - 0 - 713 - 4705 + -648 + 684 + 5218 @@ -95,7 +95,7 @@ QFrame::Raised - 1 + 19 @@ -3165,22 +3165,16 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki 0 - - - - Hz + + + + Create a calibration file with known intrinsics (fx, fy, cx, cy). Useful if you already know the intrinsics of the source of images. - - 1 + + true - - 100.000000000000000 - - - 0.100000000000000 - - - 0.000000000000000 + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse @@ -3194,13 +3188,6 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki - - - - - - - @@ -3214,40 +3201,34 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki - - - - <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 - - + + + + + + ... + + + + + + + + 100 + 0 + + + + + + + + - - + + - 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 - - - - - - - - - - - - - - Mirroring mode (flip image horizontally). It has no effect on database source. + Equalizes the histogram of grayscale images. true @@ -3264,10 +3245,33 @@ 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. + + + + false + + + + + + + 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 @@ -3302,102 +3306,6 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki - - - - Equalizes the histogram of grayscale images. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - - - - false - - - - - - - Only RGB images (for RGB-D cameras) or left images (for stereo cameras) are published. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - - - - false - - - - - - - Detect features inside camera thread using parameters set in Visual Registration panel. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - - - ... - - - - - - - - 100 - 0 - - - - - - - - - - - - - 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 - - - @@ -3424,23 +3332,17 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki - - - - - 0 - 0 - - + + - Create Calibration + - - + + - Create a calibration file with known intrinsics (fx, fy, cx, cy). Useful if you already know the intrinsics of the source of images. + Mirroring mode (flip image horizontally). It has no effect on database source. true @@ -3463,13 +3365,111 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki + + + + + + + false + + + + + + + Only RGB images (for RGB-D cameras) or left images (for stereo cameras) are published. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + Hz + + + 1 + + + 100.000000000000000 + + + 0.100000000000000 + + + 0.000000000000000 + + + + + + + + + + + + + + <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 + + + + + + + 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 + + + + + + + + 0 + 0 + + + + Create Calibration + + + + + + + Local transform from /base_link to /camera_link (without optical transform). Mouse over the box to show formats. If odometry sensor is enabled, it is the transform to odometry sensor. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + - 1 + 3 @@ -3565,6 +3565,11 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki Xvisio SeerSense + + + Orbbec SDK + + @@ -3585,7 +3590,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki - 11 + 9 @@ -3631,6 +3636,19 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki + + + + Qt::Vertical + + + + 0 + 0 + + + + @@ -3642,8 +3660,8 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki - 20 - 696 + 0 + 0 @@ -3659,7 +3677,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki - 20 + 0 0 @@ -3676,7 +3694,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki - 20 + 0 0 @@ -3911,6 +3929,19 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki + + + + Qt::Vertical + + + + 0 + 0 + + + + @@ -4113,6 +4144,19 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki + + + + Qt::Vertical + + + + 0 + 0 + + + + @@ -4283,6 +4327,19 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki + + + + Qt::Vertical + + + + 0 + 0 + + + + @@ -4425,10 +4482,23 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki + + + + Qt::Vertical + + + + 0 + 0 + + + + - + @@ -4479,10 +4549,23 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki + + + + Qt::Vertical + + + + 0 + 0 + + + + - + @@ -4495,8 +4578,15 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki RealSense2 - - + + + + ... + + + + + @@ -4505,10 +4595,13 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki - - + + + + + - IR emitter enabled + Use depth image in IR mode (instead of right image). true @@ -4518,13 +4611,16 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki - - + + - + (L515 or playback) Depth stream rate. - - false + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse @@ -4541,45 +4637,15 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki - - + + - - - - false - - - - - - - Use depth image in IR mode (instead of right image). + <html><head/><body><p>D400 Series Visual Presets. See this <a href="https://github.com/IntelRealSense/librealsense/wiki/D400-Series-Visual-Presets"><span style=" text-decoration: underline; color:#0000ff;">page</span></a>.</p></body></html> true - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - pix - - - 9999 - - - - - - - RGB/IR stream width. Set 1280 for L515. - - + true @@ -4587,16 +4653,6 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki - - - - pix - - - 9999 - - - @@ -4633,8 +4689,67 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki - - + + + + RGB/IR stream width. Set 1280 for L515. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + pix + + + 9999 + + + + + + + IR emitter enabled + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + + false + + + + + + + (L515 or playback) Depth stream height. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + pix @@ -4646,7 +4761,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki - (L515) Depth stream width. + (L515 or playback) Depth stream width. true @@ -4656,54 +4771,8 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki - - - - pix - - - 9999 - - - - - - - (L515) Depth stream height. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - Hz - - - 999 - - - - - - - (L515) Depth stream rate. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - + + @@ -4725,39 +4794,66 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki - - - - ... + + + + pix + + + 9999 - - + + + + Hz + + + 999 + + - - + + + + pix + + + 9999 + + + + + - <html><head/><body><p>D400 Series Visual Presets. See this <a href="https://github.com/IntelRealSense/librealsense/wiki/D400-Series-Visual-Presets"><span style=" text-decoration: underline; color:#0000ff;">page</span></a>.</p></body></html> + - - true - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + false + + + + Qt::Vertical + + + + 0 + 0 + + + + - + @@ -4962,6 +5058,19 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki + + + + Qt::Vertical + + + + 0 + 0 + + + + @@ -4973,8 +5082,193 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki - 20 - 40 + 0 + 0 + + + + + + + + + + + + Orbbec SDK + + + + + + + 0 + 0 + + + + Resolution + + + + + + 9999 + + + + + + + 9999 + + + + + + + Depth + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + 9999 + + + + + + + Color + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + Width + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + Height + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + 9999 + + + + + + + + + + + + Enable IMU + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + Rectify color frames + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + + + + + + + + + + + + + Convert depth in mm (16 bits format) + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + + + + + + + + + + + Qt::Vertical + + + + 0 + 0 @@ -6886,33 +7180,10 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki false - - - - 0 - - - 99999 - - - - - - - 0 - - - 99999 - - - - - - - - + + - Ignore odometry saved in the database, so if RGB-D SLAM is activated, odometry will be recomputed. + Ignore IMU (i.e., gravity links). true @@ -6922,60 +7193,6 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki - - - - - - - - - - - Ignore goals saved in the database. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - Qt::Vertical - - - - 20 - 0 - - - - - - - - - - - - - - - Open database viewer - - - - - - - :/images/mag_glass.png:/images/mag_glass.png - - - @@ -6983,26 +7200,17 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki - - + + - Ignore goal delay. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + ... - - + + - Ignore features. - - - true + Start position (node ID). Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse @@ -7022,10 +7230,17 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki - - + + - Stop position (node ID) is the last node to process. If 0, all nodes after start position are published. + + + + + + + + Ignore priors. true @@ -7035,7 +7250,37 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki - + + + + + + + + + + + + + + Ignore features. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + + + Camera index. If the database contains multi-camera data, you can choose which camera to use. Leave empty to use all cameras. Can also be multiple indices split by spaces in a string like "0 2" to stream cameras 0 and 2 only. @@ -7048,27 +7293,6 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki - - - - - - - - - - - ... - - - - - - - - - - @@ -7076,6 +7300,89 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki + + + + Open database viewer + + + + + + + :/images/mag_glass.png:/images/mag_glass.png + + + + + + + Ignore goals saved in the database. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + Publish intermediate nodes as normal nodes. + + + true + + + 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. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + 0 + + + 99999 + + + + + + + Stop position (node ID) is the last node to process. If 0, all nodes after start position are published. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + + @@ -7089,46 +7396,69 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki - - + + + + + + + 0 + + + 99999 + + + + + - Start position (node ID). + Ignore goal delay. + + + true Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - + + + + Add an y-axis offset before optical rotation on the overriden local transform(s). For multi-cameras,explicitly enumerate offsets if they are different (e.g., "0.05 0.075" for two cameras setup), or set single number to apply to all camera transforms. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + Override camera local transform(s) with local transform(s) set above. For multi-cameras, use a ";" between each transform. + + + true + + + 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. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - Ignore priors. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -7139,12 +7469,52 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki - + + + + + Ignore odometry saved in the database, so if RGB-D SLAM is activated, odometry will be recomputed. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + + + + + + + + + + + + + Qt::Vertical + + + + 20 + 0 + + + + @@ -7186,119 +7556,6 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki - - - - Ground truth file. Select the correct format below. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - Ground truth format. See tool tip for more details on formats. Note that formats without stamps should have the same number of values than the source images. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - Bayer mode. For convenience, if the images are bayered. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - ... - - - - - - - - - - - - - - Path to directory containing optional laser scans (*.pcd, *.ply, *.bin [KITTI format]). The directory should have the same size has the images directory. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - Odometry file. Select the correct format below. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - Use file names as timestamps. Format is epoch time. Example: "1305031102.175304.png". - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - ... - - - - - - - - - - - - - - - - - @@ -7306,205 +7563,43 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki - - - - - - - - - - - Maximum laser scan points. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - Synchronize capture rate with timestamps. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - + + + s - - - - - - Timestamps file (*.txt). The file should contain one column. The number of rows should be the same than the number of images in the folder. Not used if "Use file names as timestamps" above is checked. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - <html><head/><body><p>KITTI: 130 000 points</p></body></html> + + 4 - 99999999 + 9.990000000000000 + + + 0.010000000000000 + + + 0.020000000000000 - - - - <html><head/><body><p>Raw Format (3 values): x y z<br/>Raw Format (6 values): x y z roll pitch yaw<br/>Raw Format (7 values): x y z qx qy qz qw<br/>Raw Format (9 values, 3x3 rotation): r11 r12 r13 r21 r22 r23 r31 r32 r33<br/>Raw Format (12 values, 3x4 transform): r11 r12 r13 tx r21 r22 r23 ty r31 r32 r33 tz</p><p>RGBD-SLAM (stamp tx ty tz qx qy qz qw)<br/>RGBD-SLAM + ID (stamp id tx ty tz qx qy qz qw)<br/>KITTI (stamp + 12 values transform)<br/>TORO<br/>g2o<br/>NewCollege (stamp x y)<br/>Malaga Urban (GPS)<br/>St Lucia Stereo (INS)<br/>EuRoC MAV (stamp,tx,ty,tz,qw,qx,qy,qz...)</p></body></html> + + + + - - QComboBox::AdjustToContents - - - - Raw - - - - - RGBD-SLAM (motion capture) - - - - - KITTI - - - - - TORO - - - - - g2o - - - - - NewCollege - - - - - Malaga Urban - - - - - St Lucia Stereo - - - - - Karlsruhe - - - - - EuRoC MAV - - - - - RGBD-SLAM - - - - - RGBD-SLAM + ID - - - - + + ... - - - - <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 /scan = -0.27 0 0.08 0 0 0<br/>KITTI: /base_footprint to /scan = -0.27 0 1.75 0 0 0</p></body></html> - + + - 0 0 0 0 0 0 - - - - - - - Local transform from /base_link to /laser_link. Mouse over the box to show formats. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - QComboBox::AdjustToContents - - - - Disabled - - - - - BG - - - - - GB - - - - - RG - - - - - GR - - - - - - - - Odometry format. See tool tip for more details on formats. Note that formats without stamps should have the same number of values than the source images. + Path to file containing optional IMU data (*.csv [EuRoC format]). Mouse over the box to show formats. true @@ -7582,98 +7677,14 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki RGBD-SLAM + ID + + + rgbd_bonn + + - - - - ... - - - - - - - Max time difference between data and corresponding pose for format with stamps. If delay is over this threshold, the pose won't be set on data loaded. This is used when odometry and/or ground truth files are set. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - s - - - 4 - - - 9.990000000000000 - - - 0.010000000000000 - - - 0.020000000000000 - - - - - - - Local transform from /base_link to /imu_link. Mouse over the box to show formats. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - Path to file containing optional IMU data (*.csv [EuRoC format]). - - - 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>EuRoC: /base_link to /imu = 0 0 1 0 -1 0 1 0 0</p></body></html> - - - 0 0 0 0 0 0 - - - - + IMU Rate. To synchronize capture rate with IMU timestamps, set to 0. This can be set a little over the actual IMU rate to keep up with camera capture rate if images are dropped by odometry. @@ -7686,7 +7697,129 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki - + + + + Synchronize capture rate with timestamps. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + Local transform from /base_link to /laser_link. Mouse over the box to show formats. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + Odometry file. Select the correct format below. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + <html><head/><body><p>cvs format (comma split): &quot;stamp_sec,gyro_x,gyro_y,gyro_z,acc_x,acc_y,acc_z&quot;</p><p>EuRoC format: &quot;stamp_nanosec,gyro_x,gyro_y,gyro_z,acc_x,acc_y,acc_z&quot;</p></body></html> + + + + + + + + + + Local transform from /base_link to /imu_link. Mouse over the box to show formats. + + + 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 /scan = -0.27 0 0.08 0 0 0<br/>KITTI: /base_footprint to /scan = -0.27 0 1.75 0 0 0</p></body></html> + + + 0 0 0 0 0 0 + + + + + + + Path to directory containing optional laser scans (*.pcd, *.ply, *.bin [KITTI format]). The directory should have the same size has the images directory. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + + + + + + Maximum laser scan points. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + <html><head/><body><p>KITTI: 130 000 points</p></body></html> + + + 99999999 + + + + EuRoC: 200 Hz -> 250 Hz @@ -7696,6 +7829,197 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki + + + + QComboBox::AdjustToContents + + + + Disabled + + + + + BG + + + + + GB + + + + + RG + + + + + GR + + + + + + + + Max time difference between data and corresponding pose for format with stamps. If delay is over this threshold, the pose won't be set on data loaded. This is used when odometry and/or ground truth files are set. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + ... + + + + + + + + + + + + + + Use file names as timestamps. Format is epoch time. Example: "1305031102.175304.png". + + + 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>EuRoC: /base_link to /imu = 0 0 1 0 -1 0 1 0 0</p></body></html> + + + 0 0 0 0 0 0 + + + + + + + Ground truth file. Select the correct format below. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + ... + + + + + + + + + + + + + + ... + + + + + + + <html><head/><body><p>Raw Format (3 values): x y z<br/>Raw Format (6 values): x y z roll pitch yaw<br/>Raw Format (7 values): x y z qx qy qz qw<br/>Raw Format (9 values, 3x3 rotation): r11 r12 r13 r21 r22 r23 r31 r32 r33<br/>Raw Format (12 values, 3x4 transform): r11 r12 r13 tx r21 r22 r23 ty r31 r32 r33 tz</p><p>RGBD-SLAM (stamp tx ty tz qx qy qz qw)<br/>RGBD-SLAM + ID (stamp id tx ty tz qx qy qz qw)<br/>KITTI (stamp + 12 values transform)<br/>TORO<br/>g2o<br/>NewCollege (stamp x y)<br/>Malaga Urban (GPS)<br/>St Lucia Stereo (INS)<br/>EuRoC MAV (stamp,tx,ty,tz,qw,qx,qy,qz...)</p></body></html> + + + QComboBox::AdjustToContents + + + + Raw + + + + + RGBD-SLAM (motion capture) + + + + + KITTI + + + + + TORO + + + + + g2o + + + + + NewCollege + + + + + Malaga Urban + + + + + St Lucia Stereo + + + + + Karlsruhe + + + + + EuRoC MAV + + + + + RGBD-SLAM + + + + + RGBD-SLAM + ID + + + + + rgbd_bonn + + + + @@ -7709,13 +8033,95 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki - - + + + + ... + + + + + + + Odometry format. See tool tip for more details on formats. Note that formats without stamps should have the same number of values than the source images. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + Ground truth format. See tool tip for more details on formats. Note that formats without stamps should have the same number of values than the source images. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + Timestamps file (*.txt). The file should contain one column. The number of rows should be the same than the number of images in the folder. Not used if "Use file names as timestamps" above is checked. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + Bayer mode. For convenience, if the images are bayered. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + Local transform from /base_link to /gt_link. Mouse over the box to show formats. By default, we assume the ground truth matches the base frame, if the ground truth refers to another frame, set this to convert the poses in base frame. + + + 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></body></html> + + + 0 0 0 0 0 0 + + + @@ -7979,7 +8385,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki - + Convert IMU in base frame before filtering. This can help to initialize correctly the yaw. @@ -8004,7 +8410,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki - Publish inter IMU messages from the camera. IMU received between images will be published as separate topic. Orientation overridden (if any) if IMU filtering is used. + Publish inter IMU messages from the camera. IMU received between images will be published as separate topic. Orientation overridden (if any) if IMU filtering is used. true @@ -8282,7 +8688,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki - Visual Odometry Sensor + Odometry Sensor @@ -8327,6 +8733,26 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki + + + + Pass input odometry as guess to internal odometry. Otherwise, internal odometry is skipped and input odometry is sent directly to SLAM's back-end. This applies to all sources that can provide odometry (e.g., odometry file, database, ...). + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + + @@ -8344,10 +8770,111 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki 0 - - + + - Calibration file path (*.yaml) for the visual odometry sensor. If empty, the GUID of the camera is used (for those having one). Only required to calibrate extrinsics. + 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 + + + + + + + Use as ground truth. The odometry poses will be saved in ground truth field of sensor data instead of being used as odometry. The actual odometry will be computed by rtabmap using the camera sensor. This can be useful to compare rtabmap odometry versus odometry sensor, and to estimate a scale factor between the sensors (using "rtabmap-report -- scale") that can be used above. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + Time offset between camera and visual odometry sensors. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + ms + + + 1 + + + -999.000000000000000 + + + 999.000000000000000 + + + + + + + + 0 + 0 + + + + Calibrate Extrinsics + + + + + + + + + + + + + + <html><head/><body><p>Extrinsics between pose frame and camera's left lens (without optical rotation). Default extrinsics match the 3D printed bracket <a href=" https://www.intelrealsense.com/depth-and-tracking-combined-get-started/"><span style=" text-decoration: underline; color:#0000ff;">here</span></a> for T265+D400 setup. (<a href="https://github.com/IntelRealSense/realsense-ros/blob/occupancy-mapping/realsense2_camera/meshes/mount_t265_d435.stl"><span style=" text-decoration: underline; color:#0000ff;">stl</span></a>). Not used if camera and visual odometry sensors are the same sensor.</p></body></html> + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + Calibrate extrinsics between visual odometry sensor and camera. Both sensors should be already calibrated. See Calibrate button above to calibrate them individually. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + Maximum wait time to get the pose for latest data captured. true @@ -8381,16 +8908,32 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki - - + + + + ms + + + 0 + + + 0.000000000000000 + + + 1000.000000000000000 + + + 100.000000000000000 + + + + + + + <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><br/></p></body></html> + - Calibrate extrinsics between visual odometry sensor and camera. Both sensors should be already calibrated. See Calibrate button above to calibrate them individually. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + 0.009 0.021 0.027 0.000 -0.018 0.005 @@ -8401,26 +8944,10 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki - - - - ms - - - 1 - - - -999.000000000000000 - - - 999.000000000000000 - - - - - + + - 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. + Scale factor between camera and visual odometry sensor. This factor is multiplied to translation components of each pose in the odometry trajectory. true @@ -8430,10 +8957,10 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki - - + + - Time offset between camera and visual odometry sensors. + Calibration file path (*.yaml) for the visual odometry sensor. If empty, the GUID of the camera is used (for those having one). Only required to calibrate extrinsics. true @@ -8465,107 +8992,6 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki - - - - Scale factor between camera and visual odometry sensor. This factor is multiplied to translation components of each pose in the odometry trajectory. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - Use as ground truth. The odometry poses will be saved in ground truth field of sensor data instead of being used as odometry. The actual odometry will be computed by rtabmap using the camera sensor. This can be useful to compare rtabmap odometry versus odometry sensor, and to estimate a scale factor between the sensors (using "rtabmap-report -- scale") that can be used above. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - <html><head/><body><p>Extrinsics between pose frame and camera's left lens (without optical rotation). Default extrinsics match the 3D printed bracket <a href=" https://www.intelrealsense.com/depth-and-tracking-combined-get-started/"><span style=" text-decoration: underline; color:#0000ff;">here</span></a> for T265+D400 setup. (<a href="https://github.com/IntelRealSense/realsense-ros/blob/occupancy-mapping/realsense2_camera/meshes/mount_t265_d435.stl"><span style=" text-decoration: underline; color:#0000ff;">stl</span></a>). Not used if camera and visual odometry sensors are the same sensor.</p></body></html> - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - - 0 - 0 - - - - Calibrate Extrinsics - - - - - - - - - - - - - - <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><br/></p></body></html> - - - 0.009 0.021 0.027 0.000 -0.018 0.005 - - - - - - - Maximum wait time to get the pose for latest data captured. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - ms - - - 0 - - - 0.000000000000000 - - - 1000.000000000000000 - - - 100.000000000000000 - - - @@ -10092,10 +10518,10 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - + + - Image pre decimation. Decimation of the RGB image before visual feature detection. If depth size is larger than decimated RGB size, depth is decimated to be always at most equal to RGB size. If Vocabulary->Depth As Mask is true and if depth is smaller than decimated RGB, depth may be interpolated to match RGB size for feature detection. + Save intermediate node data. true @@ -10105,27 +10531,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - - - - - - true - - - - - - - - - - false - - - - + Image post decimation. Decimation of the RGB image before saving it to database. If depth size is larger than decimated RGB size, depth is decimated to be always at most equal to RGB size. Decimation is done from the original image. If set to same value than Pre Decimation, data already decimated is saved (no need to re-decimate the image). @@ -10138,91 +10544,6 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - - - - - - false - - - - - - - 1 - - - 16 - - - - - - - - - - false - - - - - - - Raw descriptors kept in memory. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - Create map labels. The first node of a map will be labelled as "map#" where # is the map ID. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - - 50 - 0 - - - - 1.000000000000000 - - - 0.010000000000000 - - - 0.200000000000000 - - - - - - - - - - false - - - @@ -10236,8 +10557,8 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - + + @@ -10246,30 +10567,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - - - Reduce graph. Merge nodes when loop closures are added (ignoring those with user data set). - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - - - - false - - - - + Multi-threaded compression. @@ -10282,10 +10580,10 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - + + - T_recent : Ratio of locations after the last loop closure in WM that cannot be transferred. + Raw descriptors kept in memory. true @@ -10295,44 +10593,8 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - - - - - - true - - - - - - - Save intermediate node data. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - Keep raw sensor data. Only useful to save loop closure computation time when features re-extraction is enabled. Disable to save RAM memory. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - + + @@ -10341,13 +10603,26 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - - - 1 + + + + Create map labels. The first node of a map will be labelled as "map#" where # is the map ID. - - 16 + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + + false @@ -10364,32 +10639,6 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - - - 1 - - - 9999 - - - 15 - - - - - - - STM size : Short-term memory size. - - - false - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - @@ -10413,6 +10662,127 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag + + + + + + + false + + + + + + + + + + false + + + + + + + + + + false + + + + + + + Image pre decimation. Decimation of the RGB image before visual feature detection. If depth size is larger than decimated RGB size, depth is decimated to be always at most equal to RGB size. If Vocabulary->Depth As Mask is true and if depth is smaller than decimated RGB, depth may be interpolated to match RGB size for feature detection. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + + false + + + + + + + STM size : Short-term memory size. + + + false + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + 50 + 0 + + + + 1.000000000000000 + + + 0.010000000000000 + + + 0.200000000000000 + + + + + + + 1 + + + 9999 + + + 15 + + + + + + + + + + false + + + + + + + T_recent : Ratio of locations after the last loop closure in WM that cannot be transferred. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + @@ -10426,10 +10796,13 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - + + + + This can be useful in case the robots don't have all same camera orientation but are using the same map, so that not rotation-invariant visual features can still be used across the fleet. + - Save depth image into 16 bits format to reduce memory used. Warning: values over ~65 meters are ignored (maximum 65535 millimeters). + Rotate images so that upside is up if they are not already. true @@ -10439,6 +10812,49 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag + + + + + + + true + + + + + + + Reduce graph. Merge nodes when loop closures are added (ignoring those with user data). Note that this approach assumes that 100% of the loop closures accepted are good, so it is highly recommended to enable optimization error ratio at the same time. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + 1 + + + 16 + + + + + + + + + + false + + + @@ -10465,23 +10881,20 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - - - + + + + 1 - - false + + 16 - - - - This can be useful in case the robots don't have all same camera orientation but are using the same map, so that not rotation-invariant visual features can still be used across the fleet. - + + - Rotate images so that upside is up if they are not already. + Save depth image into 16 bits format to reduce memory used. Warning: values over ~65 meters are ignored (maximum 65535 millimeters). true @@ -10491,13 +10904,49 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - + + + + Keep raw sensor data. Only useful to save loop closure computation time when features re-extraction is enabled. Disable to save RAM memory. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + - false + true + + + + + + + Load all local visual features (keypoints, descriptors and 3D points) in RAM when loading an existing database. This can add significant time to initialize the memory but the features will be already loaded before computing loop closure transforms. If false, the features are loaded on-demand from the database when a loop closure transformation should be estimated. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + + true @@ -10673,7 +11122,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - On merging, update to new id. + On merging, update to new id. Keep this unchecked if intermediate nodes are created. true @@ -10885,6 +11334,11 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag PyDetector + + + Superpoint Rpautrat + + @@ -11225,7 +11679,7 @@ generate the number of words requested. - Filter floor from depth mask. 0 means disabled, negative means keeping pixels below the floor theshold instead. + Filter floor from depth mask. 0 means disabled. true @@ -11384,6 +11838,16 @@ generate the number of words requested. Features Quantization + + + + + + + true + + + @@ -11391,6 +11855,19 @@ generate the number of words requested. + + + + Factor used when rebuilding the incremental FLANN index. Set 1 to disable. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + @@ -11406,32 +11883,10 @@ Lower the ratio -> higher the precision. - - - - 2 - - - 0.010000000000000 - - - 1.000000000000000 - - - 0.100000000000000 - - - 0.700000000000000 - - - - - - - - + + - Path to a pre-computed dictionary (when "Use an incremental vocabulary" is not set). + When using a FLANN-based nearest neighbor strategy, add/remove points to its index without always rebuilding the index (the index is built only when the dictionary increases of the factor below in size). true @@ -11451,16 +11906,6 @@ Lower the ratio -> higher the precision. - - - - Nearest neighbor strategy. - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - @@ -11493,33 +11938,6 @@ Lower the ratio -> higher the precision. - - - - Use an incremental vocabulary. -When set to false, no new words are added to dictionary, so no more updates are required after each detection, which greatly increases time performance at the cost of lower adaptation to new environments. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - If the dictionary update and signature creation were parallelized. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - @@ -11530,16 +11948,6 @@ When set to false, no new words are added to dictionary, so no more updates are - - - - - - - true - - - @@ -11553,10 +11961,11 @@ When set to false, no new words are added to dictionary, so no more updates are - - + + - When using a FLANN-based nearest neighbor strategy, add/remove points to its index without always rebuilding the index (the index is built only when the dictionary increases of the factor below in size). + Use an incremental vocabulary. +When set to false, no new words are added to dictionary, so no more updates are required after each detection, which greatly increases time performance at the cost of lower adaptation to new environments. true @@ -11566,26 +11975,22 @@ When set to false, no new words are added to dictionary, so no more updates are - - - - + + + + 2 - - true + + 0.010000000000000 - - - - - - Factor used when rebuilding the incremental FLANN index. Set 1 to disable. + + 1.000000000000000 - - true + + 0.100000000000000 - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + 0.700000000000000 @@ -11608,6 +12013,16 @@ When set to false, no new words are added to dictionary, so no more updates are + + + + + + + true + + + @@ -11621,8 +12036,93 @@ When set to false, no new words are added to dictionary, so no more updates are - - + + + + + + + Path to a pre-computed dictionary (when "Use an incremental vocabulary" is not set). + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + + true + + + + + + + On serialization of the FLANN index, compute checksum of the data used by the FLANN index. This adds a slight overhead on serialization/deserialization to make sure that the dictionary data correspond to same data used when the index was built. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + If the dictionary update and signature creation were parallelized. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + Nearest neighbor strategy. + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + Save FLANN index during localization session (when Mem/IncrementalMemory=false). The FLANN index will be saved to database after the first time localization mode is used, then on next sessions, the index is reloaded from the database instead of being rebuilt again. This can save significant loading time when the visual word dictionary is big (>1M words). Note that if the dictionary is modified (parameters or data), the index will be rebuilt and saved again on the next session. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + + false + + + + + @@ -11660,6 +12160,47 @@ When set to false, no new words are added to dictionary, so no more updates are Database + + + + Sqlite3 journal mode, +see Sqlite3 doc 'PRAGMA journal_mode'. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + Sqlite3 cache size, +see Sqlite3 doc 'PRAGMA cache_size'. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + Target database version for backward compatibility purpose. Only Major and minor versions are used and should be set (e.g., "0.19" vs "0.20" or "1.0" vs "2.0"). Patch version is ignored (e.g., "0.20.1" and "0.20.3" will generate a "0.20" database). + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + @@ -11670,6 +12211,146 @@ When set to false, no new words are added to dictionary, so no more updates are + + + + Using database in the memory instead of a file on the hard disk (this greatly improve database access performance but it requires more RAM memory). + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + Sqlite3 synchronous, +see Sqlite3 doc 'PRAGMA synchronous'. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + + true + + + + + + + 10 + + + 999999 + + + 100 + + + 2000 + + + + + + + 2 + + + + OFF + + + + + NORMAL + + + + + FULL + + + + + + + + + DEFAULT + + + + + FILE + + + + + MEMORY + + + + + + + + Depth image compression format (should be ".png" or ".rvl"). + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + Keep not linked nodes in db (rehearsed nodes and deleted nodes are saved). + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + major.minor + + + + + + + RGB image compression format (should be ".jpg" or ".png"). + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + @@ -11683,14 +12364,7 @@ When set to false, no new words are added to dictionary, so no more updates are - - - - - - - - + @@ -11719,20 +12393,7 @@ When set to false, no new words are added to dictionary, so no more updates are - - - - Using database in the memory instead of a file on the hard disk (this greatly improve database access performance but it requires more RAM memory). - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - + Sqlite3 temp store, @@ -11746,132 +12407,23 @@ see Sqlite3 doc 'PRAGMA temp_store'. - - - - - DEFAULT - - - - - FILE - - - - - MEMORY - - - + + - - + + + + + - - true - - - - - - - 10 - - - 999999 - - - 100 - - - 2000 - - - - - - - Sqlite3 cache size, -see Sqlite3 doc 'PRAGMA cache_size'. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - Sqlite3 journal mode, -see Sqlite3 doc 'PRAGMA journal_mode'. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - Sqlite3 synchronous, -see Sqlite3 doc 'PRAGMA synchronous'. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - 2 - - - - OFF - - - - - NORMAL - - - - - FULL - - - - - - - - Keep not linked nodes in db (rehearsed nodes and deleted nodes are saved). - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - + - RGB image compression format (should be ".jpg" or ".png"). + In localization mode, open the database in read-only mode (ignored in SLAM mode). Currrenty incompatible with memory management (i.e., Time and/or Memory thresholds cannot be used) and if there are disjoint sessions in working memory. Last localization pose won't be saved back in the database at the end of the session, so the robot will always restart to original last localization pose, unless RGBD/StartAtOrigin is used or an external initial pose is provided on initialization. true @@ -11882,41 +12434,9 @@ see Sqlite3 doc 'PRAGMA synchronous'. - - - - + - Depth image compression format (should be ".png" or ".rvl"). - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - - - - Target database version for backward compatibility purpose. Only Major and minor versions are used and should be set (e.g., "0.19" vs "0.20" or "1.0" vs "2.0"). Patch version is ignored (e.g., "0.20.1" and "0.20.3" will generate a "0.20" database). - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - major.minor + @@ -12490,7 +13010,7 @@ 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. + Localization prior error. Used only in localization mode. The corresponding variance (error x error) set to priors of the map's poses during localization. Set to 0 to only fix the first map's pose linked in odometry cache (i.e., other map's poses will be allowed to move during the optimization). true @@ -12748,7 +13268,7 @@ 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. + Linear update: Minimum linear displacement to update the map. Note that Weight Update is done prior to this, so weights are still updated. To update the map when not moving, both linear and angular update parameters should be 0. true @@ -12838,7 +13358,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - Angular update: Minimum angular displacement to update the map. Note that Weight Update is done prior to this, so weights are still updated. + Angular update: Minimum angular displacement to update the map. Note that Weight Update is done prior to this, so weights are still updated. To update the map when not moving, both linear and angular update parameters should be 0. true @@ -12910,7 +13430,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - Maximum linear speed to update the map (0 means not limit). + Maximum linear speed to update the map (0 means not limit). true @@ -12928,6 +13448,9 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag 4 + + 0.000100000000000 + 1.000000000000000 @@ -13310,6 +13833,152 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag Graph Optimization + + + + + + + 2 + + + 0.000000000000000 + + + 100.000000000000000 + + + 1.000000000000000 + + + 1.000000000000000 + + + + + + + Qt::Horizontal + + + + + + + Ignore landmarks. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + Gravity sigma value (>=0, typically between 0.1 and 0.3). Optimization is done while preserving gravity orientation of the poses. This should be used only with visual/lidar inertial odometry approaches, for which we assume that all odometry poses are aligned with gravity. Set to 0 to disable gravity constraints. Currently supported only with g2o and GTSAM optimization strategies. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + + + + + + Optimize graph from the newest node. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + Reject loop closures if optimization error ratio is greater than this value (0=disabled). Ratio is computed as absolute error over standard deviation of each link. This will help to detect when a wrong loop closure is added to the graph. If used with robust graph optimization, the disabled loop closure links will be removed. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + + 4 + + + 0.000000000000000 + + + 10.000000000000000 + + + 0.100000000000000 + + + 0.000000000000000 + + + + + + + Graph optimization algorithm. See Optimizer panel. + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + -If true, there is no odometry correction computed. All previous poses in the map are corrected instead, not the last one (which corresponds to latest odometry value). So, the transform between frames /map to /odom will be always Identity even on loop closures. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + Ignore pose priors. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + @@ -13334,10 +14003,20 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - + + - Graph optimization algorithm. See Optimizer panel. + + + + + + + + Robust graph optimization using Vertigo (only work for g2o and GTSAM optimization strategies). + + + true Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse @@ -13351,95 +14030,14 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - - - Robust graph optimization using Vertigo (only for g2o and GTSAM optimization strategies). This approach can filter wrong loop closure detections. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - - - - 2 - - - 0.000000000000000 - - - 100.000000000000000 - - - 1.000000000000000 - - - 1.000000000000000 - - - - - - - Reject loop closures if optimization error ratio is greater than this value (0=disabled). Ratio is computed as absolute error over standard deviation of each link. This will help to detect when a wrong loop closure is added to the graph. It should not be used at the same time than Vertigo above. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - + - - - - Qt::Horizontal - - - - - - - Optimize graph from the newest node. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - -If true, there is no odometry correction computed. All previous poses in the map are corrected instead, not the last one (which corresponds to latest odometry value). So, the transform between frames /map to /odom will be always Identity even on loop closures. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - + -If false, the graph is optimized from the oldest node of the current graph. It can be useful to preserve the map referential from the oldest node. An odometry correction between frames /map to /odom is computed. Warning: when some nodes are transferred, the first referential of the local map may change, resulting in momentary changes in robot/map position (which are annoying in teleoperation). @@ -13452,50 +14050,10 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - - - Ignore pose priors. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - - - - - - - - - - - - - - - Ignore landmarks. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - + - Gravity sigma value (>=0, typically between 0.1 and 0.3). Optimization is done while preserving gravity orientation of the poses. This should be used only with visual/lidar inertial odometry approaches, for which we assume that all odometry poses are aligned with gravity. Set to 0 to disable gravity constraints. Currently supported only with g2o and GTSAM optimization strategies. + Repairing graph radius. If two consecutive loop closures are rejected by the optimization error ratio on the same old loop closure link, we will remove that old link, and other old links under that radius if necessary, until optimization is accepted. When optimization is accepted, the old loop closure links are removed from the graph. This feature is useful to reject bad loop closures that were accepted previously. Set to 0 to disable this feature. true @@ -13506,24 +14064,24 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - + - 4 + 1 0.000000000000000 - 10.000000000000000 + 999.000000000000000 - 0.100000000000000 + 1.000000000000000 - 0.000000000000000 + 1.000000000000000 @@ -14947,7 +15505,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - + @@ -14956,30 +15514,29 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - - - m - - - 2 - - - 0.000000000000000 - - - 999.000000000000000 - - - 1.000000000000000 - - - 0.000000000000000 + + + + + opencv-aruco + + + + + apriltag + + + + + + + + - - + + @@ -14989,9 +15546,6 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag 0.000001000000000 - - 9999.000000000000000 - 0.001000000000000 @@ -15000,8 +15554,8 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - + + m @@ -15019,23 +15573,10 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - + - World prior locations of the markers. The map will be transformed in marker's world frame when a tag is detected. Format is the marker's ID followed by its position (angles in rad), markers are separated by a vertical line ("id1 x y z roll pitch yaw|id2 x y z roll pitch yaw"). Example: "1 0 0 1 0 0 0|2 1 0 1 0 0 1.57" (marker 2 is 1 meter forward than marker 1 with 90 deg yaw rotation). - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - Minimum detection range (0=disabled). + World prior locations of the markers. The map will be transformed in marker's world frame when a tag is detected. Format is the marker's ID followed by its position (angles in rad), multiple markers are separated by a vertical line ("id1 x y z roll pitch yaw|id2 x y z roll pitch yaw"). Example: "1 0 0 1 0 0 0|2 1 0 1 0 0 1.57" (marker 2 is 1 meter forward than marker 1 with 90 deg yaw rotation). true @@ -15046,100 +15587,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - - - - - 6 - - - 0.000001000000000 - - - 9999.000000000000000 - - - 0.001000000000000 - - - 0.001000000000000 - - - - - - - Linear variance to set on marker priors. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - Angular variance to set on marker priors. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - - - - - - - - - - - 6 - - - 0.000001000000000 - - - 0.001000000000000 - - - 0.001000000000000 - - - - - - - m - - - 4 - - - 0.000100000000000 - - - 0.010000000000000 - - - 0.100000000000000 - - - - - + m @@ -15160,33 +15608,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - - - Maximum depth error between all corners of a marker when estimating the marker length (when marker length above is 0). The smaller it is, the more perpendicular the camera should be toward the marker to initialize the length. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - Linear variance to set on marker detections. If variance is adjusted to ignore orientation (see below) and Optimizer/Strategy=2 (GTSAM): it is the variance of the range factor, with 9999 to disable range factor and to do only bearing. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - + Angular variance to set on marker detections. If variance is adjusted to ignore orientation (see below), the angular variance is ignored with Optimizer/Strategy=1 (g2o) and it corresponds to bearing variance with Optimizer/Strategy=2 (GTSAM). @@ -15199,29 +15621,14 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - - - + + + + If 0, the length is estimated only on the first marker detected, then re-used for all next detections (i.e., this assumes that markers have all the same length). +With <0, the length is estimated once for each unique marker, then re-used for next detections with the same marker ID. - - 6 - - - 0.000001000000000 - - - 0.001000000000000 - - - 0.001000000000000 - - - - - - Maximum detection range (0=unlimited). + Marker length. The length (m) of the markers' side. Value &lt;=0 means automatic marker length estimation using the depth image (the camera should look at the marker perpendicularly for initialization). Mouse over this label to see the difference between negative and 0. true @@ -15231,17 +15638,159 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - - - + + + + Linear variance to set on marker priors. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - + + + + + + + 6 + + + 0.000001000000000 + + + 9999.000000000000000 + + + 0.001000000000000 + + + 0.001000000000000 + + + + + + + + ARUCO_4X4_50 + + + + + ARUCO_4X4_100 + + + + + ARUCO_4X4_250 + + + + + ARUCO_4X4_1000 + + + + + ARUCO_5X5_50 + + + + + ARUCO_5X5_100 + + + + + ARUCO_5X5_250 + + + + + ARUCO_5X5_1000 + + + + + ARUCO_6X6_50 + + + + + ARUCO_6X6_100 + + + + + ARUCO_6X6_250 + + + + + ARUCO_6X6_1000 + + + + + ARUCO_7X7_50 + + + + + ARUCO_7X7_100 + + + + + ARUCO_7X7_250 + + + + + ARUCO_7X7_1000 + + + + + ARUCO_ORIGINAL + + + + + APRILTAG_16h5 + + + + + APRILTAG_25h9 + + + + + APRILTAG_36h10 + + + + + APRILTAG_36h11 + + + + + ARUCO_MIP_36h12 + + + + + + - Marker length. The length (m) of the markers' side. 0 means automatic marker length estimation using the depth image (the camera should look at the marker perpendicularly for initialization). + Dictionary to use. true @@ -15264,7 +15813,40 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - + + + + Linear variance to set on marker detections. If variance is adjusted to ignore orientation (see below) and Optimizer/Strategy=2 (GTSAM): it is the variance of the range factor, with 9999 to disable range factor and to do only bearing. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + + + + + + Angular variance to set on marker priors. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + When this setting is false, the landmark's orientation is optimized during graph optimization. When this setting is true, only the position of the landmark is optimized. This can be useful when the landmark's orientation estimation is not reliable. Note that for Optimizer/Strategy=1 (g2o), only Marker/VarianceLinear needs be set if we ignore orientation. For Optimizer/Strategy=2 (GTSAM), instead of optimizing the landmark's position directly, a bearing/range factor is used, with Marker/VarianceLinear as the variance of the range factor (with 9999 to optimize the position with only a bearing factor) and Marker/VarianceAngular as the variance of the bearing factor (pitch/yaw). @@ -15280,181 +15862,409 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag + + + + m + + + 2 + + + 0.000000000000000 + + + 999.000000000000000 + + + 1.000000000000000 + + + 0.000000000000000 + + + + + + + Minimum detection range (0=disabled). + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + Maximum detection range (0=unlimited). + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + Maximum depth error between all corners of a marker when estimating the marker length (when marker length above is 0). The smaller it is, the more perpendicular the camera should be toward the marker to initialize the length. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + - + + + m + + + 4 + + + 0.000100000000000 + + + 0.010000000000000 + + + 0.100000000000000 + + + + + + + + + + + + 6 + + + 0.000001000000000 + + + 0.001000000000000 + + + 0.001000000000000 + + + + + + + Detector implementation. Note that both aruco and apriltag dictionaries may be used with either strategy. Scroll down to see specific parameters for the strategy used. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + + 6 + + + 0.000001000000000 + + + 9999.000000000000000 + + + 0.001000000000000 + + + 0.001000000000000 + + + + + + + + + + + + + + For example, to detect markers 12 and 14 with lengths of 8 and 15 cm respectively, and all markers between 30 and 40 with a length of 10 cm, set "12 0.08|14 0.15|30:40 0.1" + + + List of markers to detect. Format is the marker's ID followed by its length (in meters), markers are separated by a vertical line ("id1 length|id2 length"). We can also define a range of markers with "id1:id2 length" (id2 included). If empty, all markers of the chosen dictionary can be detected and their length is set/estimated based on Marker Length. Mouse over this label for example. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + - - - ArUco + + + 0 - - - - - - 4X4_50 + + + + + + OpenCV - - - - 4X4_100 + + + + + Corner refinement method. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + None + + + + + Subpixel + + + + + Contour + + + + + AprilTag 2 + + + + + + + + Qt::Vertical + + + + 0 + 0 + + + + + + + + + + + + + + + AprilTag - - - - 4X4_250 - - - - - 4X4_1000 - - - - - 5X5_50 - - - - - 5X5_100 - - - - - 5X5_250 - - - - - 5X5_1000 - - - - - 6X6_50 - - - - - 6X6_100 - - - - - 6X6_250 - - - - - 6X6_1000 - - - - - 7X7_50 - - - - - 7X7_100 - - - - - 7X7_250 - - - - - 7X7_1000 - - - - - ARUCO_ORIGINAL - - - - - APRILTAG_16h5 - - - - - APRILTAG_25h9 - - - - - APRILTAG_36h10 - - - - - APRILTAG_36h11 - - - - - - - - Dictionary to use. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - - None - - - - - Subpixel - - - - - Contour - - - - - AprilTag 2 - - - - - - - - Corner refinement method. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - + + + + + decode_sharpening: How much sharpening should be done to decoded images? This can help decode small tags but may or may not help in odd lighting conditions or low light conditions. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + nthreads: How many threads should be used? + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + 1 + + + 1 + + + + + + + quad_decimate: Detection of quads can be done on a lower-resolution image, improving speed at a cost of pose accuracy and a slight decrease in detection rate. Decoding the binary payload is still done at full resolution. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + refine_edges: When true, the edges of the each quad are adjusted to "snap to" strong gradients nearby. This is useful when decimation is employed, as it can increase the quality of the initial quad estimate substantially. Generally recommended to be on (true). Very computationally inexpensive. Option is ignored if quad_decimate = 1. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + quad_sigma: What Gaussian blur should be applied to the segmented image (used for quad detection?) Parameter is the standard deviation in pixels. Very noisy images benefit from non-zero values (e.g. 0.8). + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + debug: When true, write a variety of debugging images to the working directory where the app started (not RTAB-Map's working directory) at various stages through the detection process. (Somewhat slow). + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + + + + + + + + + + + + + + + + 2 + + + 0.000000000000000 + + + 0.050000000000000 + + + 0.250000000000000 + + + + + + + + + + 1 + + + 0.000000000000000 + + + 99.900000000000006 + + + 0.100000000000000 + + + 0.000000000000000 + + + + + + + 1 + + + 1.000000000000000 + + + 2.000000000000000 + + + + + + + + @@ -15629,12 +16439,22 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag Open3D + + + CuVSLAM + + + + + LIO-SAM + + - Automatically reset odometry after X consecutive images on which odometry cannot be computed (value=0 disables auto-reset). When reset, the odometry starts from the last pose computed. + Automatically reset odometry after X consecutive images where odometry cannot be computed (a value of 0 disables auto-reset). When a reset occurs, odometry resumes from the last successfully computed pose with large covariance to trigger a new map. If external odometry is used, it will also be reset based on the motion estimated relative to the last computed pose but no large covariance will be received, so that a new map won't be triggered. true @@ -15943,7 +16763,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - 0 + 10 @@ -16483,6 +17303,22 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag [Visual] Correspondences computation: Optical Flow + + + + 1 + + + 999999 + + + 1 + + + 21 + + + @@ -16496,19 +17332,6 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - - - Max level. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - @@ -16525,19 +17348,16 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - - - 1 + + + + Iterations. - - 999999 + + true - - 1 - - - 21 + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse @@ -16560,10 +17380,10 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - + + - Iterations. + Epsilon. true @@ -16589,23 +17409,10 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - + + - Epsilon. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - Use GPU. OpenCV built with CUDA required. + Max level. true @@ -16616,12 +17423,109 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag + + + + + + + + + + Use minimum eigen values as an error measure, otherwise L1 distance between patches is used as an error measure. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + 6 + + + 0.000000000000000 + + + 1.000000000000000 + + + 0.000100000000000 + + + 0.000100000000000 + + + + + + + [Eigen error measure] If the minimum eigenvalue of a feature's spatial gradient matrix is less than this threshold, then the feature is filtered out. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + 6 + + + 0.000000000000000 + + + 9999.000000000000000 + + + 1.000000000000000 + + + 50.000000000000000 + + + + + + + [L1 error measure] Filter out features with error greater than this threshold. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + Use GPU. OpenCV built with CUDA required. Note that the error measure parameter is not used with the GPU implementation. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + @@ -18174,23 +19078,39 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - Enable IMU. Only supported with ORB_SLAM3. + Enable IMU. true + + + + Only supported with ORB_SLAM3. Inter IMU publishing should be enabled (see Source panel under IMU Filtering section). + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + - 8 + 6 - 0.010000000000000 + 0.000001000000000 + 1.000000000000000 + + 0.010000000000000 @@ -18237,12 +19157,15 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - 8 + 6 - 0.100000000000000 + 0.000001000000000 + 1.000000000000000 + + 0.100000000000000 @@ -18276,10 +19199,10 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - 8 + 6 - 0.000000000000000 + 0.000001000000000 1.000000000000000 @@ -18295,10 +19218,10 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - 8 + 6 - 0.000000000000000 + 0.000001000000000 1.000000000000000 @@ -19392,7 +20315,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - + VINS-Fusion @@ -19416,7 +20339,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - + @@ -19432,7 +20355,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - + ... @@ -19465,7 +20388,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag OpenVINS - + @@ -19482,6 +20405,33 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag + + + + + + ... + + + + + + + + + + Configuration file (*.yaml). Same format used than OpenVINS library. Note that any parameter from that config file will overwrite the same parameter below. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + @@ -19498,7 +20448,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - If we should process two cameras are being stereo or binocular. If binocular, we do monocular feature tracking on each image. + Stereo mode. If we should process two cameras are being stereo or binocular. If binocular, we do monocular feature tracking on each image. Ignored if provided input data is not stereo. true @@ -19518,7 +20468,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - If we should use KLT tracking, or descriptor matcher + KLT tracking. Uncheck to use descriptor matcher. true @@ -19544,7 +20494,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - Number of points (per camera) we will extract and try to track + Number of points (per camera) we will extract and try to track. true @@ -19567,7 +20517,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - Will check after doing KLT track and remove any features closer than this + Minimum pixel distance. Will check after doing KLT track and remove any features closer than this. true @@ -19587,7 +20537,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - If we should perform 1d triangulation instead of 3d + If we should perform 1d triangulation instead of 3d. true @@ -19607,7 +20557,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - If we should perform Levenberg-Marquardt refinement + If we should perform Levenberg-Marquardt refinement. true @@ -19630,7 +20580,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - Max runs for Levenberg-Marquardt + Max runs for Levenberg-Marquardt. true @@ -19656,7 +20606,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - Max baseline ratio to accept triangulated features + Max baseline ratio to accept triangulated features. true @@ -19682,7 +20632,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - Max condition number of linear triangulation matrix accept triangulated features + Max condition number of linear triangulation matrix accept triangulated features. true @@ -19711,7 +20661,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - If first-estimate Jacobians should be used (enable for good consistency) + If first-estimate Jacobians should be used (enable for good consistency). true @@ -19749,7 +20699,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - Numerical integration methods + Numerical integration methods. true @@ -19769,7 +20719,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - If the transform between camera and IMU should be optimized (R_ItoC, p_CinI) + If the transform between camera and IMU should be optimized (R_ItoC, p_CinI). true @@ -19789,7 +20739,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - If camera intrinsics should be optimized (focal, center, distortion) + If camera intrinsics should be optimized (focal, center, distortion). true @@ -19809,7 +20759,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - If timeoffset between camera and IMU should be optimized + If timeoffset between camera and IMU should be optimized. true @@ -19829,7 +20779,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - If imu intrinsics should be calibrated (rotation and skew-scale matrix) + If imu intrinsics should be calibrated (rotation and skew-scale matrix). true @@ -19849,7 +20799,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - If gyroscope gravity sensitivity (Tg) should be calibrated + If gyroscope gravity sensitivity (Tg) should be calibrated. true @@ -19872,7 +20822,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - Max clone size of sliding window + Max clone size of sliding window. true @@ -19898,7 +20848,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - Max number of estimated SLAM features + Max number of estimated SLAM features. true @@ -20003,7 +20953,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - What representation our features are in (msckf features) + What representation our features are in (msckf features). true @@ -20019,7 +20969,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag 4 - QComboBox::AdjustToContents + QComboBox::AdjustToContentsOnFirstShow @@ -20056,7 +21006,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - What representation our features are in (slam features) + What representation our features are in (slam features). true @@ -20085,7 +21035,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - Delay before initializing (helps with stability from bad initialization...) + Delay before initializing (helps with stability from bad initialization...). true @@ -20111,7 +21061,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - Magnitude of gravity in this location + Magnitude of gravity in this location. true @@ -20138,7 +21088,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - Mask for left image + Mask for left image (stereo mode) or mono image (RGB-D mode). For RGB-D mode, to use depth as mask, enable that option under Visual Registration panel. true @@ -20165,7 +21115,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - Mask for right image + Mask for right image. true @@ -20203,7 +21153,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - Amount of time we will initialize over + Amount of time we will initialize over. true @@ -20232,7 +21182,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - Variance threshold on our acceleration to be classified as moving + Variance threshold on our acceleration to be classified as moving. true @@ -20258,7 +21208,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - Max disparity to consider the platform stationary (dependent on resolution) + Max disparity to consider the platform stationary (dependent on resolution). true @@ -20284,7 +21234,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - How many features to track during initialization (saves on computation) + How many features to track during initialization (saves on computation). true @@ -20304,7 +21254,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - If we should perform dynamic initialization + If we should perform dynamic initialization. true @@ -20324,7 +21274,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - If we should optimize and recover the calibration in our MLE + If we should optimize and recover the calibration in our MLE. true @@ -20347,7 +21297,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - Max number of MLE iterations for dynamic initialization + Max number of MLE iterations for dynamic initialization. true @@ -20376,7 +21326,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - Max time for MLE optimization + Max time for MLE optimization. true @@ -20399,7 +21349,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - Max number of MLE threads for dynamic initialization + Max number of MLE threads for dynamic initialization. true @@ -20422,7 +21372,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - Number of poses to use during initialization (max should be cam freq * window) + Number of poses to use during initialization (max should be cam freq * window). true @@ -20451,7 +21401,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - Minimum degrees we need to rotate before we try to init (sum of norm) + Minimum degrees we need to rotate before we try to init (sum of norm). true @@ -20477,7 +21427,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - Magnitude we will inflate initial covariance of orientation + Magnitude we will inflate initial covariance of orientation. true @@ -20503,7 +21453,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - Magnitude we will inflate initial covariance of velocity + Magnitude we will inflate initial covariance of velocity. true @@ -20529,7 +21479,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - Magnitude we will inflate initial covariance of gyroscope bias + Magnitude we will inflate initial covariance of gyroscope bias. true @@ -20555,7 +21505,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - Magnitude we will inflate initial covariance of accelerometer bias + Magnitude we will inflate initial covariance of accelerometer bias. true @@ -20584,7 +21534,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - Minimum reciprocal condition number acceptable for our covariance recovery + Minimum reciprocal condition number acceptable for our covariance recovery. true @@ -20613,7 +21563,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - If we should try to use zero velocity update + If we should try to use zero velocity update. true @@ -20642,7 +21592,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - Chi2 multiplier for zero velocity + Chi2 multiplier for zero velocity. true @@ -20671,7 +21621,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - Max velocity we will consider to try to do a zupt (i.e. if above this, don't do zupt) + Max velocity we will consider to try to do a zupt (i.e. if above this, don't do zupt). true @@ -20700,7 +21650,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - Multiplier of our zupt measurement IMU noise matrix (default should be 1.0) + Multiplier of our zupt measurement IMU noise matrix (default should be 1.0). true @@ -20729,7 +21679,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - Max disparity we will consider to try to do a zupt (i.e. if above this, don't do zupt) + Max disparity we will consider to try to do a zupt (i.e. if above this, don't do zupt). true @@ -20749,7 +21699,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - If we should only use the zupt at the very beginning static initialization phase + If we should only use the zupt at the very beginning static initialization phase. true @@ -20790,7 +21740,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - Accel "white noise" + Accel "white noise". true @@ -20822,7 +21772,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - Accel bias diffusion + Accel bias diffusion. true @@ -20854,7 +21804,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - Gyro "white noise" + Gyro "white noise". true @@ -20886,7 +21836,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - Gyro bias diffusion + Gyro bias diffusion. true @@ -20915,7 +21865,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - Pixel noise for MSCKF features + Pixel noise for MSCKF features. true @@ -20944,7 +21894,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - Chi2 multiplier for MSCKF features + Chi2 multiplier for MSCKF features. true @@ -20973,7 +21923,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - Pixel noise for SLAM features + Pixel noise for SLAM features. true @@ -21002,7 +21952,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - Chi2 multiplier for SLAM features + Chi2 multiplier for SLAM features. true @@ -21155,6 +22105,539 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag + + + + + + CuVSLAM + + + + + + <html><head/><body><p>CuVSLAM: <a href="https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_visual_slam"><span style=" text-decoration: underline; color:#0000ff;">https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_visual_slam</span></a></p><p>Working only with stereo cameras with current integration. If Reg/Force3DoF is enabled, planar constraints will be used by CuVSLAM.</p></body></html> + + + true + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + Multi-camera Mode + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + QComboBox::AdjustToContents + + + + Moderate + + + + + Performance + + + + + Precision + + + + + + + + + + + + + Qt::Vertical + + + + 20 + 2069 + + + + + + + + + + + + LIO-SAM + + + + + + <html><head/><body><p>LIO-SAM: <a href="https://github.com/TixiaoShan/LIO-SAM"><span style=" text-decoration: underline; color:#0000ff;">https://github.com/TixiaoShan/LIO-SAM</span></a></p><p>Tightly-coupled lidar-inertial odometry via smoothing and mapping. Requires 3D lidar with per-point ring and time fields (kXYZIRT scan format) and an IMU.</p><p>If a config file path is provided, the parameters below are ignored and read from the file instead.</p></body></html> + + + true + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + + + Optional: path to LIO-SAM config YAML + + + + + + + ... + + + + + + + + + Config file path (overrides parameters below). + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + QComboBox::AdjustToContents + + + + Velodyne + + + + + Ouster + + + + + Livox + + + + + + + + LiDAR sensor type. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + 1 + + + 256 + + + 16 + + + + + + + Number of LiDAR channels (16, 32, 64, 128). + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + 1 + + + 10000 + + + 1800 + + + + + + + Horizontal resolution (Velodyne:1800, Ouster:512/1024/2048). + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + 6 + + + 0.000001000000000 + + + 1.000000000000000 + + + 0.001000000000000 + + + 0.010000000000000 + + + + + + + IMU accelerometer white noise. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + 6 + + + 0.000001000000000 + + + 1.000000000000000 + + + 0.000100000000000 + + + 0.001000000000000 + + + + + + + IMU gyroscope white noise. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + 6 + + + 0.000001000000000 + + + 1.000000000000000 + + + 0.000100000000000 + + + 0.000200000000000 + + + + + + + IMU accelerometer bias noise. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + 6 + + + 0.000001000000000 + + + 1.000000000000000 + + + 0.000010000000000 + + + 0.000030000000000 + + + + + + + IMU gyroscope bias noise. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + 5 + + + 0.000000000000000 + + + 20.000000000000000 + + + 0.010000000000000 + + + 9.805110000000001 + + + + + + + Gravity magnitude. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + 4 + + + 0.000100000000000 + + + 100.000000000000000 + + + 0.100000000000000 + + + 1.000000000000000 + + + + + + + Edge feature curvature threshold. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + 4 + + + 0.000100000000000 + + + 100.000000000000000 + + + 0.010000000000000 + + + 0.100000000000000 + + + + + + + Surface feature curvature threshold. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + 4 + + + 0.000100000000000 + + + 1.000000000000000 + + + 0.001000000000000 + + + 0.010000000000000 + + + + + + + Linear output variance. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + 4 + + + 0.000100000000000 + + + 1.000000000000000 + + + 0.001000000000000 + + + 0.010000000000000 + + + + + + + Angular output variance. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + + + + Qt::Vertical + + + + 20 + 0 + + + + + + @@ -22438,6 +23921,35 @@ Lower the ratio -> higher the precision. Motion estimation: 3D to 3D + + + + Refine iterations of the resulting transformation computed by RANSAC. 0 means no refining. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + 0 + + + 10000 + + + 1 + + + 10 + + + @@ -22470,34 +23982,18 @@ Lower the ratio -> higher the precision. - - - - 0 + + + + Qt::Vertical - - 10000 + + + 20 + 40 + - - 1 - - - 10 - - - - - - - Refine iterations of the resulting transformation computed by RANSAC. 0 means no refining. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - + @@ -23093,6 +24589,11 @@ Lower the ratio -> higher the precision. PyDetector + + + SuperPoint Rpautrat + + @@ -23150,7 +24651,7 @@ Lower the ratio -> higher the precision. - Filter floor from depth mask. 0 means disabled, negative means keeping pixels below the floor theshold instead. + Filter floor from depth mask. 0 means disabled. true @@ -23979,7 +25480,7 @@ Lower the ratio -> higher the precision. - 2 + 0 @@ -24945,10 +26446,17 @@ Lower the ratio -> higher the precision. - - + + - [OpticalFlow = true] Do optical flow on GPU. RTAB-Map should be built with OpenCV CUDA. + + + + + + + + [OpticalFlow = true] Use minimum eigen values as an error measure, otherwise L1 distance between patches is used as an error measure. true @@ -24958,13 +26466,90 @@ Lower the ratio -> higher the precision. - + + + + 6 + + + 0.000000000000000 + + + 1.000000000000000 + + + 0.000100000000000 + + + 0.000100000000000 + + + + + + + [OpticalFlow = true / Eigen error measure] If the minimum eigenvalue of a feature's spatial gradient matrix is less than this threshold, then the feature is filtered out. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + 6 + + + 0.000000000000000 + + + 9999.000000000000000 + + + 1.000000000000000 + + + 50.000000000000000 + + + + + + + [OpticalFlow = true / L1 error measure] Filter out features with error greater than this threshold. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + [OpticalFlow = true] Do optical flow on GPU. RTAB-Map should be built with OpenCV CUDA. Note that the error measure parameter is not used with the GPU implementation. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + @@ -25939,39 +27524,6 @@ Lower the ratio -> higher the precision. SIFT - - - - - - - - - - - Edge threshold. The threshold used to filter out edge-like features. Note that the its meaning is different from the contrastThreshold, i.e. the larger the edgeThreshold, the less features are filtered out (more features are retained). - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - - - - Whether to enable precise upscaling in the scale pyramid. - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - @@ -25985,13 +27537,53 @@ Lower the ratio -> higher the precision. - - - - 0.100000000000000 + + + + - - 10.000000000000000 + + + + + + Whether to enable precise upscaling in the scale pyramid. + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + CudaSift: Threshold on difference of Gaussians for feature pruning. The higher the threshold, the less features with low response/hessian are produced by the detector. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + + + + + + Contrast threshold. The contrast threshold used to filter out weak features in semi-uniform (low-contrast) regions. The larger the threshold, the less features are produced by the detector. Not used by CudaSift. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse @@ -26002,6 +27594,42 @@ Lower the ratio -> higher the precision. + + + + + + + + + + + CudaSift: Use GPU version of SIFT. This option is enabled only RTAB-Map is built with CudaSift dependency and GPUs are detected. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + + Sigma. The sigma of the Gaussian applied to the input image at the octave #0. If your image is captured with a weak camera with soft lenses, you might want to reduce the number. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + @@ -26031,23 +27659,20 @@ Lower the ratio -> higher the precision. - - + + - CudaSift: Use GPU version of SIFT. This option is enabled only RTAB-Map is built with CudaSift dependency and GPUs are detected. - - - true + CudaSift: Whether to enable upscaling. Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - + + - Contrast threshold. The contrast threshold used to filter out weak features in semi-uniform (low-contrast) regions. The larger the threshold, the less features are produced by the detector. Not used by CudaSift. + Edge threshold. The threshold used to filter out edge-like features. Note that the its meaning is different from the contrastThreshold, i.e. the larger the edgeThreshold, the less features are filtered out (more features are retained). true @@ -26076,6 +27701,16 @@ Lower the ratio -> higher the precision. + + + + 0.100000000000000 + + + 10.000000000000000 + + + @@ -26086,43 +27721,13 @@ Lower the ratio -> higher the precision. - - - - CudaSift: Threshold on difference of Gaussians for feature pruning. The higher the threshold, the less features are produced by the detector. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - - - - - - - - Sigma. The sigma of the Gaussian applied to the input image at the octave #0. If your image is captured with a weak camera with soft lenses, you might want to reduce the number. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - + - CudaSift: Whether to enable upscaling. + CudaSift: Maximum threshold on difference of Gaussians for feature pruning (ignored if smaller or equal than gaussian threshold above). The lower the threshold, the less features with high response/hessian are produced by the detector. + + + true Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse @@ -26130,9 +27735,21 @@ Lower the ratio -> higher the precision. - - - + + + 2 + + + 0.000000000000000 + + + 100.000000000000000 + + + 0.100000000000000 + + + 4.500000000000000 @@ -27463,6 +29080,183 @@ Lower the ratio -> higher the precision. + + + + + + SuperPoint Rpautrat + + + + + + <!DOCTYPE HTML PUBLIC "-//W3C//DTD HTML 4.0//EN" "http://www.w3.org/TR/REC-html40/strict.dtd"> +<html><head><meta name="qrichtext" content="1" /><style type="text/css"> +p, li { white-space: pre-wrap; } +</style></head><body style=" font-family:'Ubuntu'; font-size:11pt; font-weight:400; font-style:normal;"> +<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;">SuperPoint C++ implementation from the <a href="https://github.com/rpautrat/SuperPoint"><span style=" text-decoration: underline; color:#0000ff;">rpautrat/SuperPoint</span></a> project.</p> +<p style=" margin-top:12px; margin-bottom:12px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;">A &quot;pt&quot; model file with same name than the &quot;pth&quot; file will be generated automatically in RTAB-Map's working directory so that the model can be called by c++ libtorch directly.</p> +<p style=" margin-top:12px; margin-bottom:12px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;">RTAB-Map must be built with libtorch and python to use this feature.<br /></p></body></html> + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + + + + + + + + + + + + + + [Required] SuperPoint weights file (*.pth). + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + Detector response threshold to accept keypoint. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + Use Cuda device for Torch, otherwise CPU device is used by default. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + ... + + + + + + + 4 + + + 0.000100000000000 + + + 1.000000000000000 + + + 0.001000000000000 + + + 0.001000000000000 + + + + + + + If true, non-maximum suppression is applied to detected keypoints. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + + + + + + Minimum distance (pixels) between keypoints (when non-maximum suppression is enabled). + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + [Required] SuperPoint python model file (superpoint_pytorch.py). + + + + + + + + + + ... + + + + + + + + + + + + Qt::Vertical + + + + 20 + 4561 + + + + + + diff --git a/package.xml b/package.xml index c394fa77..4b178ba1 100644 --- a/package.xml +++ b/package.xml @@ -1,7 +1,7 @@ rtabmap - 0.22.1 + 0.23.7 RTAB-Map's standalone library. RTAB-Map is a RGB-D SLAM approach with real-time constraints. Mathieu Labbe Mathieu Labbe diff --git a/patches/apply_patch.cmake b/patches/apply_patch.cmake new file mode 100644 index 00000000..d8356ff0 --- /dev/null +++ b/patches/apply_patch.cmake @@ -0,0 +1,18 @@ +# Try to apply the patch in reverse first to see if it's already there +execute_process( + COMMAND git apply --reverse --check "${PATCH_FILE}" + RESULT_VARIABLE PATCH_APPLIED + OUTPUT_QUIET + ERROR_QUIET +) + +if(NOT PATCH_APPLIED EQUAL 0) + # If not applied, apply it now with whitespace fixes for Windows + execute_process( + COMMAND git apply --ignore-whitespace --whitespace=fix "${PATCH_FILE}" + RESULT_VARIABLE RESULT + ) + if(NOT RESULT EQUAL 0) + message(FATAL_ERROR "Failed to apply patch: ${PATCH_FILE}") + endif() +endif() \ No newline at end of file diff --git a/patches/gtsam_4_3a0-ros.patch b/patches/gtsam_4_3a0-ros.patch new file mode 100644 index 00000000..e2b2ab54 --- /dev/null +++ b/patches/gtsam_4_3a0-ros.patch @@ -0,0 +1,13 @@ +diff --git a/cmake/Config.cmake.in b/cmake/Config.cmake.in +index 338ff8500..0b738ea3c 100644 +--- a/cmake/Config.cmake.in ++++ b/cmake/Config.cmake.in +@@ -28,7 +28,7 @@ if(@GTSAM_USE_TBB@) + endif() + + if(@GTSAM_USE_SYSTEM_EIGEN@) +-find_dependency(Eigen3 REQUIRED) ++find_dependency(Eigen3 REQUIRED CONFIG) + endif() + + # Load exports diff --git a/patches/libnabo_c925c47.patch b/patches/libnabo_c925c47.patch new file mode 100644 index 00000000..65f87e60 --- /dev/null +++ b/patches/libnabo_c925c47.patch @@ -0,0 +1,58 @@ +diff --git a/CMakeLists.txt b/CMakeLists.txt +index 2e9de0c..6a031c7 100644 +--- a/CMakeLists.txt ++++ b/CMakeLists.txt +@@ -103,16 +103,7 @@ endif () + include(GNUInstallDirs) + + # eigen 2 or 3 +-find_path(EIGEN_INCLUDE_DIR NAMES signature_of_eigen3_matrix_library +- HINTS ENV EIGEN3_INC_DIR +- ENV EIGEN3_DIR +- PATHS Eigen/Core +- /usr/local/include +- /usr/include +- /opt/local/include +- PATH_SUFFIXES include eigen3 eigen2 eigen +- DOC "Directory containing the Eigen3 header files" +-) ++find_package(Eigen3 REQUIRED) + + # optionally, opencl + # OpenCL disabled as its code is not up-to-date with API +@@ -165,10 +156,10 @@ endif () + set_target_properties(${LIB_NAME} PROPERTIES VERSION "${PROJECT_VERSION}" SOVERSION 1) + + target_include_directories(${LIB_NAME} PUBLIC +- ${EIGEN_INCLUDE_DIR} + $ + $ + ) ++target_link_libraries(${LIB_NAME} PUBLIC Eigen3::Eigen) + + # openmp + set(USE_OPEN_MP TRUE CACHE BOOL "Set to FALSE to not use OpenMP") +diff --git a/libnaboConfig.cmake.in b/libnaboConfig.cmake.in +index 0de01c3..8bb8194 100644 +--- a/libnaboConfig.cmake.in ++++ b/libnaboConfig.cmake.in +@@ -1,5 +1,7 @@ + # - Config file for the libnabo package + ++find_dependency(Eigen3 CONFIG) ++ + include(${CMAKE_CURRENT_LIST_DIR}/libnabo-targets.cmake) + + # This causes catkin_simple to link against these libraries +diff --git a/nabo/kdtree_cpu.cpp b/nabo/kdtree_cpu.cpp +index cb1f8d1..52a444f 100644 +--- a/nabo/kdtree_cpu.cpp ++++ b/nabo/kdtree_cpu.cpp +@@ -37,6 +37,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + #include + #include + #include ++#include + #ifdef HAVE_OPENMP + #include + #endif diff --git a/patches/opengv_91f4b19c.patch b/patches/opengv_91f4b19c.patch new file mode 100644 index 00000000..e15b212e --- /dev/null +++ b/patches/opengv_91f4b19c.patch @@ -0,0 +1,314 @@ +diff --git a/CMakeLists.txt b/CMakeLists.txt +index 9660f55..c4a076f 100644 +--- a/CMakeLists.txt ++++ b/CMakeLists.txt +@@ -18,6 +18,7 @@ set(LIBRARY_OUTPUT_PATH ${CMAKE_BINARY_DIR}/lib) + + OPTION(BUILD_TESTS "Build tests" ON) + OPTION(BUILD_PYTHON "Build Python extension" OFF) ++OPTION(BUILD_WITH_MARCHNATIVE "Build with -march=native" ON) + + IF(MSVC) + set(BUILD_SHARED_LIBS OFF) +@@ -35,7 +36,7 @@ ELSE() + ELSEIF (CMAKE_SYSTEM_PROCESSOR MATCHES + "(arm)|(ARM)|(armhf)|(ARMHF)|(armel)|(ARMEL)") + add_definitions (-march=armv7-a) +- ELSE () ++ ELSEIF (BUILD_WITH_MARCHNATIVE) + add_definitions (-march=native) #TODO use correct c++11 def once everybody has moved to gcc 4.7 # for now I even removed std=gnu++0x + ENDIF() + add_definitions ( +@@ -54,8 +55,9 @@ IF(BUILD_POSITION_INDEPENDENT_CODE) + ENDIF() + + set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${PROJECT_SOURCE_DIR}/modules/") +-find_package(Eigen REQUIRED) +-set(ADDITIONAL_INCLUDE_DIRS ${EIGEN_INCLUDE_DIRS} ${EIGEN_INCLUDE_DIR}/unsupported) ++find_package(Eigen3 REQUIRED) ++get_target_property(EIGEN3_INCLUDE_DIR Eigen3::Eigen INTERFACE_INCLUDE_DIRECTORIES) ++set(ADDITIONAL_INCLUDE_DIRS ${EIGEN3_INCLUDE_DIR}/unsupported) + + set( OPENGV_SOURCE_FILES + src/absolute_pose/modules/main.cpp +@@ -187,13 +189,10 @@ set_target_properties( opengv random_generators PROPERTIES + + target_include_directories( opengv PUBLIC + # only when building from the source tree +- $ ++ "$" + # only when using the lib from the install path +- $ +- ${ADDITIONAL_INCLUDE_DIRS} ) +- +-target_include_directories( random_generators PRIVATE ${ADDITIONAL_INCLUDE_DIRS} ) +- ++ "$" ) ++target_link_libraries(opengv PUBLIC Eigen3::Eigen) + target_link_libraries( random_generators opengv ) + + IF (BUILD_TESTS) +diff --git a/modules/Config.cmake.in b/modules/Config.cmake.in +index 9b4c9ee..6963356 100644 +--- a/modules/Config.cmake.in ++++ b/modules/Config.cmake.in +@@ -1,4 +1,4 @@ + @PACKAGE_INIT@ +- ++find_dependency(Eigen3 CONFIG) + include("${CMAKE_CURRENT_LIST_DIR}/@targets_export_name@.cmake") + check_required_components("@PROJECT_NAME@") +diff --git a/src/absolute_pose/CentralAbsoluteAdapter.cpp b/src/absolute_pose/CentralAbsoluteAdapter.cpp +index 684fa7e..ead54ac 100644 +--- a/src/absolute_pose/CentralAbsoluteAdapter.cpp ++++ b/src/absolute_pose/CentralAbsoluteAdapter.cpp +@@ -30,6 +30,7 @@ + + + #include ++#include + + + opengv::absolute_pose::CentralAbsoluteAdapter::CentralAbsoluteAdapter( +diff --git a/src/absolute_pose/MACentralAbsolute.cpp b/src/absolute_pose/MACentralAbsolute.cpp +index 6edbabc..1164b30 100644 +--- a/src/absolute_pose/MACentralAbsolute.cpp ++++ b/src/absolute_pose/MACentralAbsolute.cpp +@@ -30,6 +30,7 @@ + + + #include ++#include + + + opengv::absolute_pose::MACentralAbsolute::MACentralAbsolute( +diff --git a/src/absolute_pose/MANoncentralAbsolute.cpp b/src/absolute_pose/MANoncentralAbsolute.cpp +index d9b5b09..1d2041c 100644 +--- a/src/absolute_pose/MANoncentralAbsolute.cpp ++++ b/src/absolute_pose/MANoncentralAbsolute.cpp +@@ -30,6 +30,7 @@ + + + #include ++#include + + opengv::absolute_pose::MANoncentralAbsolute::MANoncentralAbsolute( + const double * points, +diff --git a/src/absolute_pose/NoncentralAbsoluteAdapter.cpp b/src/absolute_pose/NoncentralAbsoluteAdapter.cpp +index 30176aa..399699b 100644 +--- a/src/absolute_pose/NoncentralAbsoluteAdapter.cpp ++++ b/src/absolute_pose/NoncentralAbsoluteAdapter.cpp +@@ -30,6 +30,7 @@ + + + #include ++#include + + opengv::absolute_pose::NoncentralAbsoluteAdapter::NoncentralAbsoluteAdapter( + const bearingVectors_t & bearingVectors, +diff --git a/src/absolute_pose/NoncentralAbsoluteMultiAdapter.cpp b/src/absolute_pose/NoncentralAbsoluteMultiAdapter.cpp +index 88c237a..b88bbb2 100644 +--- a/src/absolute_pose/NoncentralAbsoluteMultiAdapter.cpp ++++ b/src/absolute_pose/NoncentralAbsoluteMultiAdapter.cpp +@@ -30,6 +30,7 @@ + + + #include ++#include + + opengv::absolute_pose::NoncentralAbsoluteMultiAdapter::NoncentralAbsoluteMultiAdapter( + std::vector > bearingVectors, +diff --git a/src/absolute_pose/methods.cpp b/src/absolute_pose/methods.cpp +index b1f0889..6d7a73c 100644 +--- a/src/absolute_pose/methods.cpp ++++ b/src/absolute_pose/methods.cpp +@@ -34,6 +34,7 @@ + + #include + #include ++#include + + #include + #include +diff --git a/src/absolute_pose/modules/main.cpp b/src/absolute_pose/modules/main.cpp +index ed0c271..011dbcc 100644 +--- a/src/absolute_pose/modules/main.cpp ++++ b/src/absolute_pose/modules/main.cpp +@@ -46,6 +46,8 @@ + #include + #include + ++#include ++ + void + opengv::absolute_pose::modules::p3p_kneip_main( + const bearingVectors_t & f, +diff --git a/src/math/arun.cpp b/src/math/arun.cpp +index a0d6296..bc729f6 100644 +--- a/src/math/arun.cpp ++++ b/src/math/arun.cpp +@@ -30,6 +30,7 @@ + + + #include ++#include + + opengv::rotation_t + opengv::math::arun( const Eigen::MatrixXd & Hcross ) +diff --git a/src/point_cloud/MAPointCloud.cpp b/src/point_cloud/MAPointCloud.cpp +index 81fd5dd..a216857 100644 +--- a/src/point_cloud/MAPointCloud.cpp ++++ b/src/point_cloud/MAPointCloud.cpp +@@ -30,6 +30,7 @@ + + + #include ++#include + + opengv::point_cloud::MAPointCloud::MAPointCloud( + const double * points1, +diff --git a/src/point_cloud/PointCloudAdapter.cpp b/src/point_cloud/PointCloudAdapter.cpp +index f9faaeb..1f8e951 100644 +--- a/src/point_cloud/PointCloudAdapter.cpp ++++ b/src/point_cloud/PointCloudAdapter.cpp +@@ -30,6 +30,7 @@ + + + #include ++#include + + opengv::point_cloud::PointCloudAdapter::PointCloudAdapter( + const points_t & points1, +diff --git a/src/point_cloud/methods.cpp b/src/point_cloud/methods.cpp +index 5409eeb..f098a1d 100644 +--- a/src/point_cloud/methods.cpp ++++ b/src/point_cloud/methods.cpp +@@ -39,6 +39,8 @@ + #include + #include + ++#include ++ + namespace opengv + { + namespace point_cloud +diff --git a/src/relative_pose/CentralRelativeAdapter.cpp b/src/relative_pose/CentralRelativeAdapter.cpp +index 38e9a62..b2e9d98 100644 +--- a/src/relative_pose/CentralRelativeAdapter.cpp ++++ b/src/relative_pose/CentralRelativeAdapter.cpp +@@ -30,6 +30,7 @@ + + + #include ++#include + + opengv::relative_pose::CentralRelativeAdapter::CentralRelativeAdapter( + const bearingVectors_t & bearingVectors1, +diff --git a/src/relative_pose/CentralRelativeMultiAdapter.cpp b/src/relative_pose/CentralRelativeMultiAdapter.cpp +index 2ab7476..e522205 100644 +--- a/src/relative_pose/CentralRelativeMultiAdapter.cpp ++++ b/src/relative_pose/CentralRelativeMultiAdapter.cpp +@@ -30,6 +30,7 @@ + + + #include ++#include + + opengv::relative_pose::CentralRelativeMultiAdapter::CentralRelativeMultiAdapter( + std::vector > bearingVectors1, +diff --git a/src/relative_pose/CentralRelativeWeightingAdapter.cpp b/src/relative_pose/CentralRelativeWeightingAdapter.cpp +index a6ab478..9401dc2 100644 +--- a/src/relative_pose/CentralRelativeWeightingAdapter.cpp ++++ b/src/relative_pose/CentralRelativeWeightingAdapter.cpp +@@ -30,6 +30,7 @@ + + + #include ++#include + + opengv::relative_pose::CentralRelativeWeightingAdapter::CentralRelativeWeightingAdapter( + const bearingVectors_t & bearingVectors1, +diff --git a/src/relative_pose/MACentralRelative.cpp b/src/relative_pose/MACentralRelative.cpp +index ec2959f..c1feb7c 100644 +--- a/src/relative_pose/MACentralRelative.cpp ++++ b/src/relative_pose/MACentralRelative.cpp +@@ -30,6 +30,7 @@ + + + #include ++#include + + opengv::relative_pose::MACentralRelative::MACentralRelative( + const double * bearingVectors1, +diff --git a/src/relative_pose/MANoncentralRelative.cpp b/src/relative_pose/MANoncentralRelative.cpp +index cea9c14..fc65c64 100644 +--- a/src/relative_pose/MANoncentralRelative.cpp ++++ b/src/relative_pose/MANoncentralRelative.cpp +@@ -30,6 +30,7 @@ + + + #include ++#include + + opengv::relative_pose::MANoncentralRelative::MANoncentralRelative( + const double * bearingVectors1, +diff --git a/src/relative_pose/MANoncentralRelativeMulti.cpp b/src/relative_pose/MANoncentralRelativeMulti.cpp +index 49f8ecf..1e58fde 100644 +--- a/src/relative_pose/MANoncentralRelativeMulti.cpp ++++ b/src/relative_pose/MANoncentralRelativeMulti.cpp +@@ -30,6 +30,7 @@ + + + #include ++#include + + opengv::relative_pose::MANoncentralRelativeMulti::MANoncentralRelativeMulti( + const std::vector & bearingVectors1, +diff --git a/src/relative_pose/NoncentralRelativeAdapter.cpp b/src/relative_pose/NoncentralRelativeAdapter.cpp +index 552f180..9edf294 100644 +--- a/src/relative_pose/NoncentralRelativeAdapter.cpp ++++ b/src/relative_pose/NoncentralRelativeAdapter.cpp +@@ -30,6 +30,7 @@ + + + #include ++#include + + opengv::relative_pose::NoncentralRelativeAdapter::NoncentralRelativeAdapter( + const bearingVectors_t & bearingVectors1, +diff --git a/src/relative_pose/NoncentralRelativeMultiAdapter.cpp b/src/relative_pose/NoncentralRelativeMultiAdapter.cpp +index f41edbe..c720a5a 100644 +--- a/src/relative_pose/NoncentralRelativeMultiAdapter.cpp ++++ b/src/relative_pose/NoncentralRelativeMultiAdapter.cpp +@@ -30,6 +30,7 @@ + + + #include ++#include + + opengv::relative_pose::NoncentralRelativeMultiAdapter::NoncentralRelativeMultiAdapter( + std::vector > bearingVectors1, +diff --git a/src/relative_pose/methods.cpp b/src/relative_pose/methods.cpp +index 0027dae..e2e26b1 100644 +--- a/src/relative_pose/methods.cpp ++++ b/src/relative_pose/methods.cpp +@@ -42,6 +42,7 @@ + #include + + #include ++#include + + opengv::translation_t + opengv::relative_pose::twopt( +diff --git a/src/relative_pose/modules/fivept_nister/modules.cpp b/src/relative_pose/modules/fivept_nister/modules.cpp +index 4b134c5..f24e3f1 100644 +--- a/src/relative_pose/modules/fivept_nister/modules.cpp ++++ b/src/relative_pose/modules/fivept_nister/modules.cpp +@@ -34,6 +34,7 @@ + #include + + #include ++#include + + void + opengv::relative_pose::modules::fivept_nister::composeA( diff --git a/patches/pointmatcher_7dc58e5.patch b/patches/pointmatcher_7dc58e5.patch new file mode 100644 index 00000000..1b0a911b --- /dev/null +++ b/patches/pointmatcher_7dc58e5.patch @@ -0,0 +1,589 @@ +diff --git a/.gitignore b/.gitignore +index 85152fb..da016a7 100644 +--- a/.gitignore ++++ b/.gitignore +@@ -5,3 +5,4 @@ + *.cur_trans + build + .ipynb_checkpoints/ ++pointmatcher/pm_export.h +\ No newline at end of file +diff --git a/CMakeLists.txt b/CMakeLists.txt +index 819a834..d72850a 100644 +--- a/CMakeLists.txt ++++ b/CMakeLists.txt +@@ -233,7 +233,7 @@ else() + #get_property(yaml-cpp-pm_INCLUDE TARGET yaml-cpp-pm PROPERTY INCLUDE_DIRECTORIES) + #include_directories(${yaml-cpp-pm_INCLUDE}) + +- list(APPEND EXTERNAL_LIBS $) ++ list(APPEND EXTERNAL_LIBS $) + list(APPEND EXTRA_DEPS yaml-cpp-pm) + set(yamlcpp_FOUND) + +@@ -291,6 +291,8 @@ else () + set (CMAKE_CXX_STANDARD 11) + endif () + ++option(BUILD_SHARED_LIBS "Set to ON to build shared libraries" OFF) ++ + # SOURCE + + # Pointmatcher lib and install +@@ -363,12 +365,18 @@ set(POINTMATCHER_SRC + + file(GLOB_RECURSE POINTMATCHER_HEADERS "pointmatcher/*.h") + +- + # In CMake >=3.4 we can easily build shared libraries in Mac and Windows. + # No need to distinguish between operating systems while building targets + + add_library(pointmatcher ${POINTMATCHER_SRC} ${POINTMATCHER_HEADERS} ) + ++include(GenerateExportHeader) ++generate_export_header(pointmatcher ++ BASE_NAME PM ++ EXPORT_FILE_NAME "${CMAKE_CURRENT_SOURCE_DIR}/pointmatcher/pm_export.h" ++ DEFINE_NO_DEPRECATED ++) ++ + target_include_directories(pointmatcher PUBLIC + $ + $ +@@ -417,6 +425,7 @@ install(FILES + pointmatcher/Timer.h + pointmatcher/Functions.h + pointmatcher/IO.h ++ pointmatcher/pm_export.h + DESTINATION ${INSTALL_INCLUDE_DIR}/pointmatcher + ) + +@@ -515,7 +524,7 @@ add_library(${PROJECT_NAME}::${PROJECT_NAME} ALIAS pointmatcher) + get_property(CONF_INCLUDE_DIRS DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR} PROPERTY INCLUDE_DIRECTORIES) + + # Create variable with the library location +-set(POINTMATCHER_LIB $) ++set(POINTMATCHER_LIB $) + + # Configure config file for local build tree + configure_file(libpointmatcherConfig.cmake.in +diff --git a/pointmatcher/Bibliography.h b/pointmatcher/Bibliography.h +index 17a353d..c86def5 100644 +--- a/pointmatcher/Bibliography.h ++++ b/pointmatcher/Bibliography.h +@@ -40,6 +40,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + #include + #include + ++#include "pm_export.h" ++ + namespace PointMatcherSupport + { + typedef std::vector StringVector; +@@ -48,7 +50,7 @@ namespace PointMatcherSupport + typedef StringMapMap Bibliography; + typedef std::map BibIndices; + +- struct CurrentBibliography ++ struct PM_EXPORT CurrentBibliography + { + enum Mode + { +@@ -68,7 +70,7 @@ namespace PointMatcherSupport + void dumpBibtex(std::ostream& os) const; + }; + +- std::string getAndReplaceBibEntries(const std::string&, CurrentBibliography& curBib); ++ PM_EXPORT std::string getAndReplaceBibEntries(const std::string&, CurrentBibliography& curBib); + + }; // PointMatcherSupport + +diff --git a/pointmatcher/DeprecationWarnings.h b/pointmatcher/DeprecationWarnings.h +index b1ca8ec..dc84582 100644 +--- a/pointmatcher/DeprecationWarnings.h ++++ b/pointmatcher/DeprecationWarnings.h +@@ -31,7 +31,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + #ifndef __POINTMATCHER_DEPRECATION_WARNINGS_H + #define __POINTMATCHER_DEPRECATION_WARNINGS_H + +- + #if __cplusplus >= 201402L + #define PM_DEPRECATED(msg) [[deprecated(msg)]] + #define PM_DEPRECATION_SUPPORTED +diff --git a/pointmatcher/IO.cpp b/pointmatcher/IO.cpp +index c4288c9..8722747 100644 +--- a/pointmatcher/IO.cpp ++++ b/pointmatcher/IO.cpp +@@ -51,7 +51,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + #include "boost/lexical_cast.hpp" + #include "boost/foreach.hpp" + +-#ifdef WIN32 ++#ifdef _WIN32 + #define strtok_r strtok_s + #endif // WIN32 + +@@ -359,7 +359,9 @@ void PointMatcherSupport::validateFile(const std::string& fileName) + ifstream ifs(fileName.c_str()); + if (!ifs.good() || !boost::filesystem::is_regular_file(fullPath)) + #if BOOST_FILESYSTEM_VERSION >= 3 +- #if BOOST_VERSION >= 105000 ++ #if BOOST_VERSION >= 109000 ++ throw runtime_error(string("Cannot open file ") + boost::filesystem::absolute(fullPath).generic_string()); ++ #elif BOOST_VERSION >= 105000 + throw runtime_error(string("Cannot open file ") + boost::filesystem::complete(fullPath).generic_string()); + #else + throw runtime_error(string("Cannot open file ") + boost::filesystem3::complete(fullPath).generic_string()); +@@ -375,7 +377,11 @@ template + typename PointMatcher::DataPoints PointMatcher::DataPoints::load(const std::string& fileName) + { + const boost::filesystem::path path(fileName); ++#if BOOST_VERSION >= 109000 ++ const string ext = path.extension().string(); ++#else + const string& ext(boost::filesystem::extension(path)); ++#endif + if (boost::iequals(ext, ".vtk")) + return PointMatcherIO::loadVTK(fileName); + else if (boost::iequals(ext, ".csv")) +@@ -809,7 +815,11 @@ template + void PointMatcher::DataPoints::save(const std::string& fileName, bool binary) const + { + const boost::filesystem::path path(fileName); ++#if BOOST_VERSION >= 109000 ++ const string ext = path.extension().string(); ++#else + const string& ext(boost::filesystem::extension(path)); ++#endif + if (boost::iequals(ext, ".vtk")) + return PointMatcherIO::saveVTK(*this, fileName, binary); + +diff --git a/pointmatcher/IO.h b/pointmatcher/IO.h +index 162dc21..8a0cefa 100644 +--- a/pointmatcher/IO.h ++++ b/pointmatcher/IO.h +@@ -58,7 +58,7 @@ struct PointMatcherIO + //! ex: nx, ny, nz are associated with (0,normals) (1,normals) (2,normals) respectively + typedef std::map SublabelAssociationMap; + +- static std::string getColLabel(const Label& label, const int row); //!< convert a descriptor label to an appropriate sub-label ++ PM_EXPORT static std::string getColLabel(const Label& label, const int row); //!< convert a descriptor label to an appropriate sub-label + + //! Type of information in a DataPoints. Each type is stored in its own dense matrix. + enum PMPropTypes +@@ -70,7 +70,7 @@ struct PointMatcherIO + }; + + //! Structure containing all information required to map external information to PointMatcher internal representation +- struct SupportedLabel ++ struct PM_EXPORT SupportedLabel + { + std::string internalName; //!< name used in PointMatcher + std::string externalName; //!< name used in external format +@@ -84,7 +84,7 @@ struct PointMatcherIO + typedef std::vector SupportedLabels; + + //! Helper structure designed to parse file headers +- struct GenericInputHeader ++ struct PM_EXPORT GenericInputHeader + { + std::string name; //!< name found in the file + unsigned int matrixRowId; //!< on which row the information will be loaded +@@ -159,7 +159,7 @@ struct PointMatcherIO + } + + //! Generate a vector of Labels by checking for collision is the same name is reused. +- class LabelGenerator ++ class PM_EXPORT LabelGenerator + { + Labels labels; //!< vector of labels used to cumulat information + +@@ -180,11 +180,11 @@ struct PointMatcherIO + //static PMPropTypes getPMType(const std::string& externalName); //! Return the type of information specific to a DataPoints based on a sulabel name + + // CSV +- static DataPoints loadCSV(const std::string& fileName); +- static DataPoints loadCSV(std::istream& is); ++ PM_EXPORT static DataPoints loadCSV(const std::string& fileName); ++ PM_EXPORT static DataPoints loadCSV(std::istream& is); + +- static void saveCSV(const DataPoints& data, const std::string& fileName); +- static void saveCSV(const DataPoints& data, std::ostream& os); ++ PM_EXPORT static void saveCSV(const DataPoints& data, const std::string& fileName); ++ PM_EXPORT static void saveCSV(const DataPoints& data, std::ostream& os); + + // VTK + //! Enumeration of legacy VTK data types that can be parsed +@@ -209,25 +209,25 @@ struct PointMatcherIO + + }; + +- static DataPoints loadVTK(const std::string& fileName); +- static DataPoints loadVTK(std::istream& is); ++ PM_EXPORT static DataPoints loadVTK(const std::string& fileName); ++ PM_EXPORT static DataPoints loadVTK(std::istream& is); + +- static void saveVTK(const DataPoints& data, const std::string& fileName, bool binary = false); ++ PM_EXPORT static void saveVTK(const DataPoints& data, const std::string& fileName, bool binary = false); + + // PLY +- static DataPoints loadPLY(const std::string& fileName); +- static DataPoints loadPLY(std::istream& is); ++ PM_EXPORT static DataPoints loadPLY(const std::string& fileName); ++ PM_EXPORT static DataPoints loadPLY(std::istream& is); + +- static void savePLY(const DataPoints& data, const std::string& fileName); //!< save datapoints to PLY point cloud format ++ PM_EXPORT static void savePLY(const DataPoints& data, const std::string& fileName); //!< save datapoints to PLY point cloud format + + // PCD +- static DataPoints loadPCD(const std::string& fileName); +- static DataPoints loadPCD(std::istream& is); ++ PM_EXPORT static DataPoints loadPCD(const std::string& fileName); ++ PM_EXPORT static DataPoints loadPCD(std::istream& is); + +- static void savePCD(const DataPoints& data, const std::string& fileName); //!< save datapoints to PCD point cloud format ++ PM_EXPORT static void savePCD(const DataPoints& data, const std::string& fileName); //!< save datapoints to PCD point cloud format + + //! Information to exploit a reading from a file using this library. Fields might be left blank if unused. +- struct FileInfo ++ struct PM_EXPORT FileInfo + { + typedef Eigen::Matrix Vector3; //!< alias + +@@ -242,7 +242,7 @@ struct PointMatcherIO + }; + + //! A vector of file info, to be used in batch processing +- struct FileInfoVector: public std::vector ++ struct PM_EXPORT FileInfoVector: public std::vector + { + FileInfoVector(); + FileInfoVector(const std::string& fileName, std::string dataPath = "", std::string configPath = ""); +@@ -264,7 +264,7 @@ struct PointMatcherIO + static bool plyPropTypeValid (const std::string& type); + + //! Interface for PLY property +- struct PLYProperty ++ struct PM_EXPORT PLYProperty + { + //PLY information: + std::string name; //!< name of PLY property +@@ -299,7 +299,7 @@ struct PointMatcherIO + typedef typename PLYProperties::iterator it_PLYProp; + + //! Interface for all PLY elements. +- class PLYElement ++ class PM_EXPORT PLYElement + { + public: + std::string name; //!< name identifying the PLY element +@@ -331,7 +331,7 @@ struct PointMatcherIO + + + //! Implementation of PLY vertex element +- class PLYVertex : public PLYElement ++ class PM_EXPORT PLYVertex : public PLYElement + { + public: + //! Constructor +@@ -346,7 +346,7 @@ struct PointMatcherIO + }; + + //! Factory for PLY elements +- class PLYElementF ++ class PM_EXPORT PLYElementF + { + enum ElementTypes + { +diff --git a/pointmatcher/Parametrizable.h b/pointmatcher/Parametrizable.h +index 318e7f1..edc1355 100644 +--- a/pointmatcher/Parametrizable.h ++++ b/pointmatcher/Parametrizable.h +@@ -46,6 +46,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + #define BOOST_ASSIGN_MAX_PARAMS 6 + #include + ++#include "pm_export.h" + + namespace PointMatcherSupport + { +@@ -95,7 +96,7 @@ namespace PointMatcherSupport + } + + //! The superclass of classes that are constructed using generic parameters. This class provides the parameter storage and fetching mechanism +- struct Parametrizable ++ struct PM_EXPORT Parametrizable + { + //! An exception thrown when one tries to fetch the value of an unexisting parameter + struct InvalidParameter: std::runtime_error +@@ -114,7 +115,7 @@ namespace PointMatcherSupport + } + + //! The documentation of a parameter +- struct ParameterDoc ++ struct PM_EXPORT ParameterDoc + { + std::string name; //!< name + std::string doc; //!< short documentation +@@ -173,7 +174,7 @@ namespace PointMatcherSupport + + friend std::ostream& operator<< (std::ostream& o, const Parametrizable& p); + }; +- std::ostream& operator<< (std::ostream& o, const Parametrizable::ParametersDoc& p); ++ PM_EXPORT std::ostream& operator<< (std::ostream& o, const Parametrizable::ParametersDoc& p); + } // namespace PointMatcherSupport + + #endif // __POINTMATCHER_PARAMETRIZABLE_H +diff --git a/pointmatcher/PointMatcher.h b/pointmatcher/PointMatcher.h +index f26aa2c..435a464 100644 +--- a/pointmatcher/PointMatcher.h ++++ b/pointmatcher/PointMatcher.h +@@ -101,7 +101,7 @@ namespace PointMatcherSupport + + + //! The logger interface, used to output warnings and informations +- struct Logger: public Parametrizable ++ struct PM_EXPORT Logger: public Parametrizable + { + Logger(); + Logger(const std::string& className, const ParametersDoc paramsDoc, const Parameters& params); +@@ -127,7 +127,7 @@ namespace PointMatcherSupport + + //! Functions and classes that are dependant on scalar type are defined in this templatized class + template +-struct PointMatcher ++struct PM_EXPORT PointMatcher + { + // --------------------------------- + // macros for constants +@@ -145,7 +145,7 @@ struct PointMatcher + //TODO: gather exceptions here and in Exceptions.cpp + + //! Point matcher did not converge +- struct ConvergenceError: std::runtime_error ++ struct PM_EXPORT ConvergenceError: std::runtime_error + { + ConvergenceError(const std::string& reason); + }; +@@ -204,7 +204,7 @@ struct PointMatcher + Moreover, the position of the points is in homogeneous coordinates because they need both translation and rotation, while the normals need only rotation. + All channels contain scalar values of type ScalarType. + */ +- struct DataPoints ++ struct PM_EXPORT DataPoints + { + //! A view on a feature or descriptor + typedef Eigen::Block View; +@@ -218,7 +218,7 @@ struct PointMatcher + typedef typename Matrix::Index Index; + + //! The name for a certain number of dim +- struct Label ++ struct PM_EXPORT Label + { + std::string text; //!< name of the label + size_t span; //!< number of data dimensions the label spans +@@ -226,7 +226,7 @@ struct PointMatcher + bool operator ==(const Label& that) const; + }; + //! A vector of Label +- struct Labels: std::vector
LinuxBuild Status
Build Status
Build Status -
WindowsBuild Status + + CMake Linux Build Status
+ CMake Windows Build Status
+ CMake MaCOS Build Status
+ CMake ROS Build Status
+ Docker Build Status
Build Status
ROS 2ROS 2 Humble Build Status
Jazzy Build Status
KiltedBuild Status
Rolling Build Status