Merge pull request #37 from ethz-asl/feature/optimize_chunked_integrator

Optimize integrators, Rviz plugin and add usage examples

.devcontainer/clion/Dockerfile

@@ -0,0 +1,39 @@
+ARG VERSION=latest
+
+FROM ghcr.io/ethz-asl/wavemap:${VERSION}
+
+# Install dependencies
+# hadolint ignore=DL3008
+RUN apt-get update && apt-get install -yq --no-install-recommends curl sudo && rm -rf /var/lib/apt/lists/*
+
+# Install CLion in the container
+ARG CLION_VERSION=2023.2
+SHELL ["/bin/bash", "-o", "pipefail", "-c"]
+# hadolint ignore=DL3003
+RUN mkdir -p /opt/clion && \
+    curl -L "https://download.jetbrains.com/cpp/CLion-${CLION_VERSION}.tar.gz" \
+    | tar -C /opt/clion --strip-components 1 -xzvf - && \
+    cd /opt/clion/bin && ./remote-dev-server.sh registerBackendLocationForGateway
+
+# Create a non-root user
+ARG USERNAME=ci
+ARG USER_UID=1000
+ARG USER_GID=$USER_UID
+RUN groupadd --gid $USER_GID $USERNAME && \
+    useradd --uid $USER_UID --gid $USER_GID $USERNAME && \
+    chown -R $USERNAME:$USERNAME /home/$USERNAME && \
+    echo $USERNAME ALL=\(root\) NOPASSWD:ALL > /etc/sudoers.d/$USERNAME && \
+    chmod 0440 /etc/sudoers.d/$USERNAME
+
+# Make CLion available to the current user
+USER $USERNAME
+# hadolint ignore=DL3003
+RUN cd /opt/clion/bin && ./remote-dev-server.sh registerBackendLocationForGateway
+
+# Prebuild the package
+RUN catkin build wavemap_all
+
+# Make root the default user
+# NOTE: This is currently required for CLion's local deployment procedure to work.
+# hadolint ignore=DL3002
+USER root

.devcontainer/clion/devcontainer.json

@@ -0,0 +1,10 @@
+{
+  "build": {
+    "dockerfile": "Dockerfile"
+  },
+  "forwardPorts": [],
+  "workspaceFolder": "/home/ci/catkin_ws/src",
+  "workspaceMount": "source=${localWorkspaceFolder},target=/home/ci/catkin_ws/src,type=bind,consistency=cached",
+  "runArgs": ["--env-file", "${localWorkspaceFolder}/.devcontainer/clion/ros_env.list"],
+  "remoteUser": "ci"
+}

.devcontainer/clion/ros_env.list

@@ -0,0 +1,13 @@
+# This is a workaround for CLion not allowing files to be sourced
+# (~/catkin_ws/devel/setup.bash) before launching the remote IDE handler.
+ROS_VERSION=1
+ROS_DISTRO=noetic
+ROS_PYTHON_VERSION=3
+ROS_ROOT=/opt/ros/noetic/share/ros
+ROS_MASTER_URI=http://localhost:11311
+ROS_PACKAGE_PATH=/home/ci/catkin_ws/src/dependencies/catkin_simple:/home/ci/catkin_ws/src/dependencies/eigen_catkin:/home/ci/catkin_ws/src/dependencies/gflags_catkin:/home/ci/catkin_ws/src/dependencies/glog_catkin:/home/ci/catkin_ws/src/dependencies/eigen_checks:/home/ci/catkin_ws/src/dependencies/minkindr/minkindr:/home/ci/catkin_ws/src/wavemap/libraries/wavemap:/home/ci/catkin_ws/src/wavemap/tooling/packages/wavemap_all:/home/ci/catkin_ws/src/wavemap/libraries/wavemap_io:/home/ci/catkin_ws/src/wavemap/ros/wavemap_msgs:/home/ci/catkin_ws/src/wavemap/ros/wavemap_ros_conversions:/home/ci/catkin_ws/src/wavemap/ros/wavemap_ros:/home/ci/catkin_ws/src/wavemap/ros/wavemap_rviz_plugin:/home/ci/catkin_ws/src/wavemap/tooling/packages/wavemap_utils:/opt/ros/noetic/share
+CMAKE_PREFIX_PATH=/home/ci/catkin_ws/devel:/opt/ros/noetic
+PKG_CONFIG_PATH=/home/ci/catkin_ws/devel/lib/pkgconfig:/opt/ros/noetic/lib/pkgconfig
+PYTHONPATH=/home/ci/catkin_ws/devel/lib/python3/dist-packages:/opt/ros/noetic/lib/python3/dist-packages
+LD_LIBRARY_PATH=/home/ci/catkin_ws/devel/lib:/opt/ros/noetic/lib
+PATH=/home/ci/catkin_ws/devel/bin:/opt/ros/noetic/bin:/usr/lib/ccache:/usr/lib/ccache:/home/ci/.local/bin:/usr/local/sbin:/usr/local/bin:/usr/sbin:/usr/bin:/sbin:/bin:/usr/games:/usr/local/games:/snap/bin:/home/ci/repos/linter/bin

.devcontainer/vscode/Dockerfile

@@ -0,0 +1,17 @@
+ARG VERSION=latest
+
+FROM ghcr.io/ethz-asl/wavemap:${VERSION}
+
+# Create a non-root user and make it the default user
+ARG USERNAME=ci
+ARG USER_UID=1000
+ARG USER_GID=$USER_UID
+RUN groupadd --gid $USER_GID $USERNAME && \
+    useradd --uid $USER_UID --gid $USER_GID $USERNAME && \
+    chown -R $USERNAME:$USERNAME /home/$USERNAME && \
+    echo $USERNAME ALL=\(root\) NOPASSWD:ALL > /etc/sudoers.d/$USERNAME && \
+    chmod 0440 /etc/sudoers.d/$USERNAME
+USER $USERNAME
+
+# Build the package
+RUN catkin build wavemap_all

.devcontainer/vscode/devcontainer.json

@@ -0,0 +1,10 @@
+{
+  "build": {
+    "dockerfile": "Dockerfile"
+  },
+  "forwardPorts": [],
+  "workspaceFolder": "/home/ci/catkin_ws/src/wavemap",
+  "workspaceMount": "source=${localWorkspaceFolder},target=/home/ci/catkin_ws/src/wavemap,type=bind,consistency=cached",
+  "containerUser": "ci",
+  "remoteUser": "ci"
+}

.github/workflows/cd.yml

@@ -187,7 +187,7 @@ jobs:
         run: |
           apt-get update
           apt-get install -q -y --no-install-recommends python3-pip doxygen
-          pip3 install exhale sphinx-rtd-theme
+          pip3 install exhale sphinx-rtd-theme sphinx-copybutton
 
       - name: Build documentation
         working-directory: ${{ env.CATKIN_WS_PATH }}/src/${{ env.REPOSITORY_NAME }}/docs

.github/workflows/ci.yml

@@ -206,7 +206,7 @@ jobs:
         run: |
           apt-get update
           apt-get install -q -y --no-install-recommends python3-pip doxygen
-          pip3 install exhale sphinx-rtd-theme
+          pip3 install exhale sphinx-rtd-theme sphinx-copybutton
 
       - name: Build documentation
         working-directory: ${{ env.CATKIN_WS_PATH }}/src/${{ env.REPOSITORY_NAME }}/docs
@@ -379,6 +379,7 @@ jobs:
         shell: bash
         run: |
           all_tests_passed=1
+          source devel/setup.bash
           for f in devel/lib/wavemap*/test_*
             do $f --gtest_color=yes || all_tests_passed=0
           done
@@ -587,6 +588,7 @@ jobs:
         run: |
           echo "::add-matcher::./.github/problem-matchers/valgrind.json"
           all_tests_passed=1
+          source devel/setup.bash
           for f in devel/lib/wavemap*/test_*
             do valgrind --tool=memcheck --leak-check=full --leak-resolution=high --num-callers=20 --track-origins=yes --show-possibly-lost=no --errors-for-leak-kinds=definite,indirect --error-exitcode=1 --xml=yes --xml-file=valgrind-log.xml $f --gtest_color=yes || all_tests_passed=0
             grep -Poz '(?<=<error>)(.*\n)*.*(?=</error>)' valgrind-log.xml || true

.github/workflows/release_management.yml

@@ -112,10 +112,8 @@ jobs:
 
           if dpkg --compare-versions $version_main "lt" $version_local; then
             echo "Local version ($version_local) is ahead of origin main ($version_main)"
-            exit 0
           else
-            echo "Local version ($version_local) lags origin main ($version_main)"
-            exit 1
+            echo "::warning title=Not ready to merge::Local version ($version_local) lags origin main ($version_main)"
           fi
 
   tag-release:

CMakeLists.txt

@@ -18,3 +18,6 @@ add_subdirectory(ros/wavemap_msgs)
 add_subdirectory(ros/wavemap_ros_conversions)
 add_subdirectory(ros/wavemap_ros)
 add_subdirectory(ros/wavemap_rviz_plugin)
+
+# Usage examples
+add_subdirectory(examples)

docs/conf.py

@@ -43,7 +43,8 @@
 # extensions coming with Sphinx (named 'sphinx.ext.*') or your custom
 # ones.
 extensions = [
-    'sphinx.ext.mathjax', 'sphinx.ext.githubpages', 'breathe', 'exhale'
+    'sphinx.ext.mathjax', 'sphinx.ext.githubpages', 'breathe', 'exhale',
+    'sphinx_copybutton'
 ]
 
 # Add any paths that contain templates here, relative to this directory.
@@ -213,7 +214,7 @@
 extensions.append("sphinx.ext.extlinks")
 extlinks = {
     'repo_file':
-    ('https://github.com/ethz-asl/wavemap/tree/main/%s', 'source file %s')
+    ('https://github.com/ethz-asl/wavemap/tree/main/%s', 'source: %s')
 }
 
 # Configure the link checker (invoked with `make linkcheck`)

docs/index.rst

@@ -23,11 +23,14 @@ The framework is very flexible and supports several data structures, measurement
    api/library_root
    pages/faq
 
+Code
+****
+This documentation website accompanies wavemap's `open-source implementation <https://github.com/ethz-asl/wavemap>`__, which is hosted on GitHub.
 
 Paper
 *****
 
-A technical introduction to the theory behind the wavemap is provided in our open-access `RSS paper <https://www.roboticsproceedings.org/rss19/p065.pdf>`_.
+A technical introduction to the theory behind the wavemap is provided in our open-access `RSS paper <https://www.roboticsproceedings.org/rss19/p065.pdf>`__.
 
 Abstract:
     Volumetric maps are widely used in robotics due to their desirable properties in applications such as path planning, exploration, and manipulation. Constant advances in mapping technologies are needed to keep up with the improvements in sensor technology, generating increasingly vast amounts of precise measurements. Handling this data in a computationally and memory-efficient manner is paramount to representing the environment at the desired scales and resolutions. In this work, we express the desirable properties of a volumetric mapping framework through the lens of multi-resolution analysis. This shows that wavelets are a natural foundation for hierarchical and multi-resolution volumetric mapping. Based on this insight we design an efficient mapping system that uses wavelet decomposition. The efficiency of the system enables the use of uncertainty-aware sensor models, improving the quality of the maps. Experiments on both synthetic and real-world data provide mapping accuracy and runtime performance comparisons with state-of-the-art methods on both RGB-D and 3D LiDAR data. The framework is open-sourced to allow the robotics community at large to explore this approach.
@@ -46,4 +49,4 @@ BibTeX::
     }
 
 
-Note that the code has significantly improved since the paper was written. Wavemap is now up to 10x faster, thanks to new multi-threaded measurement integrators, and uses up to 50% less RAM, by virtue of new memory efficient data structures inspired by `OpenVDB <https://github.com/AcademySoftwareFoundation/openvdb>`_.
+Note that the code has significantly improved since the paper was written. Wavemap is now up to 10x faster, thanks to new multi-threaded measurement integrators, and uses up to 50% less RAM, by virtue of new memory efficient data structures inspired by `OpenVDB <https://github.com/AcademySoftwareFoundation/openvdb>`__.

docs/pages/demos.rst

@@ -1,17 +1,24 @@
 Demos
 #####
 .. highlight:: bash
+.. include:: <isonum.txt>
 .. rstcheck: ignore-roles=repo_file
 
+Quick start
+***********
+To get an impression of the maps wavemap can generate, you can download the maps of our `RSS paper's <https://www.roboticsproceedings.org/rss19/p065.pdf>`__ and directly view them in Rviz. To do so,
+
+* Choose and download one of the maps provided `here <https://drive.google.com/drive/folders/1sTmDBUt97wwE220gVFwCq88JT5IOQlk5>`__
+* Open Rviz, for example by running :code:`roscore & rviz`
+* Load wavemap's rviz plugin by clicking: `Add` |rarr| `By display type` |rarr| `wavemap_rviz_plugin` |rarr| `WavemapMap`
+* In the plugin settings, under `Source` select `File`
+* Load the map you just downloaded by clicking: `Loaded map` |rarr| `Choose file`
+* [Optional] Tweak the visualization options under `Render voxels` and `Render slice`
+
+
 Newer College dataset
 *********************
-The Newer College dataset is available `here <https://ori-drs.github.io/newer-college-dataset/download/>`__. To get the
-sharpest maps, we recommend supplying wavemap with a high-rate odometry estimate and turning on its built-in pointcloud
-motion undistortion. In our experiments, we got these estimates by modifying FastLIO2 to publish its forward-integrated
-IMU poses. If you would like to run FastLIO2 yourself, our public fork
-is `available here <https://github.com/ethz-asl/fast_lio>`_. Alternatively, we provide rosbags with pre-recorded odometry
-for the Multi-Cam Cloister, Park, Math-easy and Mine-easy
-sequences `here <https://drive.google.com/drive/folders/1sTmDBUt97wwE220gVFwCq88JT5IOQlk5>`__.
+The Newer College dataset is available `here <https://ori-drs.github.io/newer-college-dataset/download/>`__. To get the sharpest maps, we recommend supplying wavemap with a high-rate odometry estimate and turning on wavemap's built-in pointcloud motion undistortion. In our experiments, we got these estimates by modifying FastLIO2 to publish its forward-integrated IMU poses. If you would like to run FastLIO2 yourself, our public fork is `available here <https://github.com/ethz-asl/fast_lio>`_. Alternatively, we provide rosbags with pre-recorded odometry for the Multi-Cam Cloister, Park, Math-easy and Mine-easy sequences `here <https://drive.google.com/drive/folders/1sTmDBUt97wwE220gVFwCq88JT5IOQlk5>`__.
 
 To run wavemap on the Cloister sequence used in the paper, run::
 

docs/pages/faq.rst

@@ -1,6 +1,12 @@
 FAQ
 ###
 
-We do not yet have FAQs.
+If you have a question that is not yet answered below, feel free to open a `GitHub Issue <https://github.com/ethz-asl/wavemap/issues>`_ or contact us over email.
 
-If you have a question, please do not hesitate to open a `GitHub Issue <https://github.com/ethz-asl/wavemap/issues>`_.
+How do I query if a point in the map is occupied?
+=================================================
+Please see the :doc:`usage examples <usage_examples>` on :ref:`interpolation <examples-interpolation>` and :ref:`classification <examples-classification>`.
+
+Does wavemap support (Euclidean) Signed Distance Fields?
+========================================================
+Not yet, but we will add this feature in the near future.

docs/pages/usage_examples.rst

@@ -1,4 +1,68 @@
-Code examples
-#############
+Usage examples
+##############
 
-Examples of how to use wavemap's API to save, load and query maps are coming soon. We also intend to add examples of how to use wavemap for common tasks including collision checking and path planning.
+Serialization
+*************
+
+Files
+=====
+Saving maps to files:
+
+.. literalinclude:: ../../examples/src/io/save_map_to_file.cc
+    :language: c++
+
+Loading maps from files:
+
+.. literalinclude:: ../../examples/src/io/load_map_from_file.cc
+    :language: c++
+
+ROS msgs
+========
+Receiving maps over ROS topics:
+
+.. literalinclude:: ../../examples/src/io/receive_map_over_ros.cc
+    :language: c++
+
+Sending maps over ROS topics:
+
+.. literalinclude:: ../../examples/src/io/send_map_over_ros.cc
+    :language: c++
+
+Queries
+*******
+
+Fixed resolution
+================
+.. literalinclude:: ../../examples/src/queries/fixed_resolution.cc
+    :language: c++
+
+Multi-res averages
+==================
+.. literalinclude:: ../../examples/src/queries/multi_resolution.cc
+    :language: c++
+
+Accelerators
+============
+.. literalinclude:: ../../examples/src/queries/accelerated_queries.cc
+    :language: c++
+
+Interpolation
+=============
+.. _examples-interpolation:
+
+Nearest neighbor interpolation:
+
+.. literalinclude:: ../../examples/src/queries/nearest_neighbor_interpolation.cc
+    :language: c++
+
+Trilinear interpolation:
+
+.. literalinclude:: ../../examples/src/queries/trilinear_interpolation.cc
+    :language: c++
+
+Classification
+==============
+.. _examples-classification:
+
+.. literalinclude:: ../../examples/src/queries/classification.cc
+    :language: c++

examples/CHANGELOG.rst

@@ -0,0 +1,9 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package wavemap_examples
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+1.6.0 (2023-10-17)
+------------------
+* Add initial usage examples
+* Address clang-tidy unused variable warnings
+* Contributors: Victor Reijgwart

examples/CMakeLists.txt

@@ -0,0 +1,37 @@
+cmake_minimum_required(VERSION 3.0.2)
+project(wavemap_examples)
+
+find_package(catkin_simple REQUIRED)
+catkin_simple(ALL_DEPS_REQUIRED)
+
+# Compiler definitions and options
+add_wavemap_compile_definitions_and_options()
+add_compile_options(-Wno-suggest-attribute=const)
+
+# For all targets
+include_directories(include)
+
+# Binaries
+cs_add_executable(save_map_to_file
+        src/io/save_map_to_file.cc)
+cs_add_executable(load_map_from_file
+        src/io/load_map_from_file.cc)
+cs_add_executable(receive_map_over_ros
+        src/io/receive_map_over_ros.cc)
+cs_add_executable(send_map_over_ros
+        src/io/send_map_over_ros.cc)
+
+cs_add_executable(fixed_resolution
+        src/queries/fixed_resolution.cc)
+cs_add_executable(multi_resolution
+        src/queries/multi_resolution.cc)
+cs_add_executable(accelerated_queries
+        src/queries/accelerated_queries.cc)
+
+cs_add_executable(nearest_neighbor_interpolation
+        src/queries/nearest_neighbor_interpolation.cc)
+cs_add_executable(trilinear_interpolation
+        src/queries/trilinear_interpolation.cc)
+
+cs_add_executable(classification
+        src/queries/classification.cc)

examples/README.md

@@ -0,0 +1,5 @@
+# Examples
+
+Welcome! We're glad you're interested in using wavemap.
+
+To get started, we recommend taking a look at the [Usage examples](https://ethz-asl.github.io/wavemap/pages/usage_examples.html) documentation page.

examples/include/CPPLINT.cfg

@@ -0,0 +1 @@
+root=.

examples/include/wavemap_examples/common.h

@@ -0,0 +1,14 @@
+#ifndef WAVEMAP_EXAMPLES_COMMON_H_
+#define WAVEMAP_EXAMPLES_COMMON_H_
+
+namespace wavemap::examples {
+/**
+ * A placeholder method used to illustrate where the user would use a value.
+ * Concretely, this method is also used to suppress 'unused variable' warnings
+ * issued by GCC when compiling the usage examples.
+ */
+template <typename... T>
+void doSomething([[maybe_unused]] T... t) {}
+}  // namespace wavemap::examples
+
+#endif  // WAVEMAP_EXAMPLES_COMMON_H_

examples/package.xml

@@ -0,0 +1,20 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>wavemap_examples</name>
+  <version>1.6.0</version>
+  <description>Usages examples for wavemap.</description>
+
+  <maintainer email="[email protected]">Victor Reijgwart</maintainer>
+  <license>BSD</license>
+  <url>https://github.com/ethz-asl/wavemap</url>
+
+  <author email="[email protected]">Victor Reijgwart</author>
+
+  <buildtool_depend>catkin</buildtool_depend>
+  <buildtool_depend>catkin_simple</buildtool_depend>
+
+  <depend>wavemap</depend>
+  <depend>wavemap_io</depend>
+  <depend>wavemap_msgs</depend>
+  <depend>wavemap_ros_conversions</depend>
+</package>