diff --git a/.devcontainer/clion/Dockerfile b/.devcontainer/clion/Dockerfile new file mode 100644 index 000000000..7f9f8bacb --- /dev/null +++ b/.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 diff --git a/.devcontainer/clion/devcontainer.json b/.devcontainer/clion/devcontainer.json new file mode 100644 index 000000000..62f548285 --- /dev/null +++ b/.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" +} diff --git a/.devcontainer/clion/ros_env.list b/.devcontainer/clion/ros_env.list new file mode 100644 index 000000000..5fcaa8597 --- /dev/null +++ b/.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 diff --git a/.devcontainer/vscode/Dockerfile b/.devcontainer/vscode/Dockerfile new file mode 100644 index 000000000..b32a0b35c --- /dev/null +++ b/.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 diff --git a/.devcontainer/vscode/devcontainer.json b/.devcontainer/vscode/devcontainer.json new file mode 100644 index 000000000..4b63a34bb --- /dev/null +++ b/.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" +} diff --git a/.github/workflows/cd.yml b/.github/workflows/cd.yml index fb3640ba4..65a09844c 100644 --- a/.github/workflows/cd.yml +++ b/.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 diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index b914ac3ac..640a6762d 100644 --- a/.github/workflows/ci.yml +++ b/.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 '(?<=)(.*\n)*.*(?=)' valgrind-log.xml || true diff --git a/.github/workflows/release_management.yml b/.github/workflows/release_management.yml index c35f0a4b9..7d9224f5e 100644 --- a/.github/workflows/release_management.yml +++ b/.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: diff --git a/CMakeLists.txt b/CMakeLists.txt index b5d316472..bfba9f57e 100644 --- a/CMakeLists.txt +++ b/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) diff --git a/docs/conf.py b/docs/conf.py index 6db777a84..cd36c5018 100644 --- a/docs/conf.py +++ b/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`) diff --git a/docs/index.rst b/docs/index.rst index 747f16128..afdad5010 100644 --- a/docs/index.rst +++ b/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 `__, which is hosted on GitHub. Paper ***** -A technical introduction to the theory behind the wavemap is provided in our open-access `RSS paper `_. +A technical introduction to the theory behind the wavemap is provided in our open-access `RSS paper `__. 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 `_. +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 `__. diff --git a/docs/pages/demos.rst b/docs/pages/demos.rst index 342548448..902837230 100644 --- a/docs/pages/demos.rst +++ b/docs/pages/demos.rst @@ -1,17 +1,24 @@ Demos ##### .. highlight:: bash +.. include:: .. 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 `__ and directly view them in Rviz. To do so, + +* Choose and download one of the maps provided `here `__ +* 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 `__. 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 `_. Alternatively, we provide rosbags with pre-recorded odometry -for the Multi-Cam Cloister, Park, Math-easy and Mine-easy -sequences `here `__. +The Newer College dataset is available `here `__. 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 `_. Alternatively, we provide rosbags with pre-recorded odometry for the Multi-Cam Cloister, Park, Math-easy and Mine-easy sequences `here `__. To run wavemap on the Cloister sequence used in the paper, run:: diff --git a/docs/pages/faq.rst b/docs/pages/faq.rst index 03faa37ca..ff4105478 100644 --- a/docs/pages/faq.rst +++ b/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 `_ or contact us over email. -If you have a question, please do not hesitate to open a `GitHub Issue `_. +How do I query if a point in the map is occupied? +================================================= +Please see the :doc:`usage examples ` on :ref:`interpolation ` and :ref:`classification `. + +Does wavemap support (Euclidean) Signed Distance Fields? +======================================================== +Not yet, but we will add this feature in the near future. diff --git a/docs/pages/usage_examples.rst b/docs/pages/usage_examples.rst index 5d76882c7..766d174da 100644 --- a/docs/pages/usage_examples.rst +++ b/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++ diff --git a/examples/CHANGELOG.rst b/examples/CHANGELOG.rst new file mode 100644 index 000000000..8f6150972 --- /dev/null +++ b/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 diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt new file mode 100644 index 000000000..3e2c83a98 --- /dev/null +++ b/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) diff --git a/examples/README.md b/examples/README.md new file mode 100644 index 000000000..c33614f99 --- /dev/null +++ b/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. diff --git a/examples/include/CPPLINT.cfg b/examples/include/CPPLINT.cfg new file mode 100644 index 000000000..2fce9d52b --- /dev/null +++ b/examples/include/CPPLINT.cfg @@ -0,0 +1 @@ +root=. diff --git a/examples/include/wavemap_examples/common.h b/examples/include/wavemap_examples/common.h new file mode 100644 index 000000000..59e6351ee --- /dev/null +++ b/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 +void doSomething([[maybe_unused]] T... t) {} +} // namespace wavemap::examples + +#endif // WAVEMAP_EXAMPLES_COMMON_H_ diff --git a/examples/package.xml b/examples/package.xml new file mode 100644 index 000000000..6285db6d1 --- /dev/null +++ b/examples/package.xml @@ -0,0 +1,20 @@ + + + wavemap_examples + 1.6.0 + Usages examples for wavemap. + + Victor Reijgwart + BSD + https://github.com/ethz-asl/wavemap + + Victor Reijgwart + + catkin + catkin_simple + + wavemap + wavemap_io + wavemap_msgs + wavemap_ros_conversions + diff --git a/examples/src/CPPLINT.cfg b/examples/src/CPPLINT.cfg new file mode 100644 index 000000000..03c80b6f9 --- /dev/null +++ b/examples/src/CPPLINT.cfg @@ -0,0 +1 @@ +filter=-build/namespaces diff --git a/examples/src/io/load_map_from_file.cc b/examples/src/io/load_map_from_file.cc new file mode 100644 index 000000000..dfb9389b7 --- /dev/null +++ b/examples/src/io/load_map_from_file.cc @@ -0,0 +1,9 @@ +#include + +int main(int, char**) { + // Create a smart pointer that will own the loaded map + wavemap::VolumetricDataStructureBase::Ptr loaded_map; + + // Load the map + wavemap::io::fileToMap("/some/path/to/your/map.wvmp", loaded_map); +} diff --git a/examples/src/io/receive_map_over_ros.cc b/examples/src/io/receive_map_over_ros.cc new file mode 100644 index 000000000..a5ea7729e --- /dev/null +++ b/examples/src/io/receive_map_over_ros.cc @@ -0,0 +1,24 @@ +#include +#include + +// Create a smart pointer that will own the received map +wavemap::VolumetricDataStructureBase::Ptr loaded_map; + +// Define the map callback +void mapCallback(const wavemap_msgs::Map& msg) { + // Load the received map + wavemap::convert::rosMsgToMap(msg, loaded_map); +} + +// NOTE: We only define loaded_map and mapCallback globally for illustration +// purposes. In practice, we recommend defining them as class members. + +int main(int argc, char** argv) { + // Register your node with ROS + ros::init(argc, argv, "your_node"); + ros::NodeHandle nh; + ros::NodeHandle nh_private("~"); + + // Subscribe to the ROS topic + ros::Subscriber map_sub = nh.subscribe("map", 1, &mapCallback); +} diff --git a/examples/src/io/save_map_to_file.cc b/examples/src/io/save_map_to_file.cc new file mode 100644 index 000000000..6276fd729 --- /dev/null +++ b/examples/src/io/save_map_to_file.cc @@ -0,0 +1,10 @@ +#include + +int main(int, char**) { + // Create an empty map for illustration purposes + wavemap::HashedWaveletOctreeConfig config; + wavemap::HashedWaveletOctree map(config); + + // Save the map + wavemap::io::mapToFile(map, "/some/path/to/your/map.wvmp"); +} diff --git a/examples/src/io/send_map_over_ros.cc b/examples/src/io/send_map_over_ros.cc new file mode 100644 index 000000000..258273424 --- /dev/null +++ b/examples/src/io/send_map_over_ros.cc @@ -0,0 +1,28 @@ +#include +#include + +int main(int argc, char** argv) { + // Register your node with ROS + ros::init(argc, argv, "your_node"); + ros::NodeHandle nh; + ros::NodeHandle nh_private("~"); + + // Create an empty map for illustration + wavemap::HashedWaveletOctreeConfig config; + wavemap::HashedWaveletOctree map(config); + + // Advertise the ROS topic + const std::string ros_topic = "map"; + const int queue_size = 1; + ros::Publisher map_pub = + nh_private.advertise(ros_topic, queue_size); + + // Convert the map to a ROS msg + wavemap_msgs::Map map_msg; + const std::string map_frame = "odom"; + const ros::Time stamp = ros::Time::now(); + wavemap::convert::mapToRosMsg(map, map_frame, stamp, map_msg); + + // Publish the ROS message + map_pub.publish(map_msg); +} diff --git a/examples/src/queries/accelerated_queries.cc b/examples/src/queries/accelerated_queries.cc new file mode 100644 index 000000000..e43621a54 --- /dev/null +++ b/examples/src/queries/accelerated_queries.cc @@ -0,0 +1,24 @@ +#include +#include +#include + +#include "wavemap_examples/common.h" + +using namespace wavemap; +int main(int, char**) { + // Declare a map pointer for illustration purposes + // NOTE: See the other tutorials on how to load maps from files or ROS topics, + // such as the map topic published by the wavemap ROS server. + HashedWaveletOctree::Ptr map; + + // Create the query accelerator + QueryAccelerator query_accelerator(*map); + + // Query all points within a grid + for (const auto& query_index : + Grid<3>(Index3D::Constant(-10), Index3D::Constant(10))) { + const FloatingPoint occupancy_log_odds = + query_accelerator.getCellValue(query_index); + examples::doSomething(occupancy_log_odds); + } +} diff --git a/examples/src/queries/classification.cc b/examples/src/queries/classification.cc new file mode 100644 index 000000000..62e931d41 --- /dev/null +++ b/examples/src/queries/classification.cc @@ -0,0 +1,52 @@ +#include + +#include "wavemap_examples/common.h" + +using namespace wavemap; +int main(int, char**) { + // Declare a floating point value representing the occupancy posterior in log + // odds as queried from the map in one of the previous examples + const FloatingPoint occupancy_log_odds{}; + + // A point is considered unobserved if its occupancy posterior is equal to the + // prior. Wavemap assumes that an unobserved point is equally likely to be + // free or occupied. In other words, the prior occupancy probability is 0.5, + // which corresponds to a log odds value of 0.0. Accounting for numerical + // noise, checking whether a point is unobserved can be done as follows: + constexpr FloatingPoint kUnobservedThreshold = 1e-3; + const bool is_unobserved = + std::abs(occupancy_log_odds) < kUnobservedThreshold; + examples::doSomething(is_unobserved); + + // In case you would like to convert log odds into probabilities, we provide + // the following convenience function: + const FloatingPoint occupancy_probability = + convert::logOddsToProbability(occupancy_log_odds); + examples::doSomething(occupancy_probability); + + // To classify whether a point is estimated to be occupied or free, you need + // to choose a discrimination threshold. A reasonable default threshold is 0.5 + // (probability), which corresponds to 0.0 log odds. + constexpr FloatingPoint kOccupancyThresholdProb = 0.5; + constexpr FloatingPoint kOccupancyThresholdLogOdds = 0.0; + + // NOTE: To tailor the threshold, we recommend running wavemap on a dataset + // that is representative of your application and analyzing the Receiver + // Operating Characteristic curve. + + // Once a threshold has been chosen, you can either classify in log space + { + const bool is_occupied = kOccupancyThresholdLogOdds < occupancy_log_odds; + const bool is_free = occupancy_log_odds < kOccupancyThresholdLogOdds; + examples::doSomething(is_occupied); + examples::doSomething(is_free); + } + + // Or in probability space + { + const bool is_occupied = kOccupancyThresholdProb < occupancy_probability; + const bool is_free = occupancy_probability < kOccupancyThresholdProb; + examples::doSomething(is_occupied); + examples::doSomething(is_free); + } +} diff --git a/examples/src/queries/fixed_resolution.cc b/examples/src/queries/fixed_resolution.cc new file mode 100644 index 000000000..6d6a21c4f --- /dev/null +++ b/examples/src/queries/fixed_resolution.cc @@ -0,0 +1,20 @@ +#include + +#include "wavemap_examples/common.h" + +using namespace wavemap; +int main(int, char**) { + // Declare a map pointer for illustration purposes + // NOTE: See the other tutorials on how to load maps from files or ROS topics, + // such as the map topic published by the wavemap ROS server. + VolumetricDataStructureBase::Ptr map; + + // Declare the index to query + // NOTE: See wavemap/indexing/index_conversions.h for helper methods + // to compute and convert indices. + const Index3D query_index = Index3D::Zero(); + + // Query the map + const FloatingPoint occupancy_log_odds = map->getCellValue(query_index); + examples::doSomething(occupancy_log_odds); +} diff --git a/examples/src/queries/multi_resolution.cc b/examples/src/queries/multi_resolution.cc new file mode 100644 index 000000000..f741b57f0 --- /dev/null +++ b/examples/src/queries/multi_resolution.cc @@ -0,0 +1,27 @@ +#include +#include + +#include "wavemap_examples/common.h" + +using namespace wavemap; +int main(int, char**) { + // Declare a map pointer for illustration purposes + // NOTE: See the other tutorials on how to load maps from files or ROS topics, + // such as the map topic published by the wavemap ROS server. + HashedWaveletOctree::Ptr map; + + // Define the center point and the minimum width of the octree cell to query + const Point3D query_point = Point3D::Zero(); + const FloatingPoint query_min_cell_width = 0.5f; // in meters + + // Convert it to an octree node index + const FloatingPoint map_min_cell_width = map->getMinCellWidth(); + const IndexElement query_height = convert::cellWidthToHeight( + query_min_cell_width, 1.f / map_min_cell_width); + const OctreeIndex query_index = + convert::pointToNodeIndex(query_point, map_min_cell_width, query_height); + + // Query the map + const FloatingPoint occupancy_log_odds = map->getCellValue(query_index); + examples::doSomething(occupancy_log_odds); +} diff --git a/examples/src/queries/nearest_neighbor_interpolation.cc b/examples/src/queries/nearest_neighbor_interpolation.cc new file mode 100644 index 000000000..de7015946 --- /dev/null +++ b/examples/src/queries/nearest_neighbor_interpolation.cc @@ -0,0 +1,25 @@ +#include +#include + +#include "wavemap_examples/common.h" + +using namespace wavemap; +int main(int, char**) { + // Create an empty map for illustration purposes + // NOTE: See the other tutorials on how to load maps from files or ROS topics, + // such as the map topic published by the wavemap ROS server. + VolumetricDataStructureBase::Ptr map; + + // Declare the point to query [in map frame] + const Point3D query_point = Point3D::Zero(); + + // Compute the index that's nearest to the query point + const FloatingPoint min_cell_width_inv = 1.f / map->getMinCellWidth(); + const Index3D nearest_neighbor_index = + convert::pointToNearestIndex(query_point, min_cell_width_inv); + + // Query the map + const FloatingPoint occupancy_log_odds = + map->getCellValue(nearest_neighbor_index); + examples::doSomething(occupancy_log_odds); +} diff --git a/examples/src/queries/trilinear_interpolation.cc b/examples/src/queries/trilinear_interpolation.cc new file mode 100644 index 000000000..2fbfb4be9 --- /dev/null +++ b/examples/src/queries/trilinear_interpolation.cc @@ -0,0 +1,20 @@ +#include +#include + +#include "wavemap_examples/common.h" + +using namespace wavemap; +int main(int, char**) { + // Create an empty map for illustration purposes + // NOTE: See the other tutorials on how to load maps from files or ROS topics, + // such as the map topic published by the wavemap ROS server. + VolumetricDataStructureBase::Ptr map; + + // Declare the point to query [in map frame] + const Point3D query_point = Point3D::Zero(); + + // Query the map and compute the interpolated value + const FloatingPoint occupancy_log_odds = + interpolate::trilinear(*map, query_point); + examples::doSomething(occupancy_log_odds); +} diff --git a/libraries/wavemap/CHANGELOG.rst b/libraries/wavemap/CHANGELOG.rst index b6afb7ad8..7da3bde72 100644 --- a/libraries/wavemap/CHANGELOG.rst +++ b/libraries/wavemap/CHANGELOG.rst @@ -2,6 +2,30 @@ Changelog for package wavemap ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.6.0 (2023-10-17) +------------------ +* New features + + * Map query accelerator + * Trilinear interpolator + +* Improvements + + * Optimize measurement integration + + * Replace stack with recursion (faster and easier to read) + * Vectorize batched leaf updater + * Reduce memory move and copy overheads + * Simplify measurement model math + * Postpone image offset error norm root computation + * Share a single thread pool among all integrators + + * Refactor wavemap utils + * Add tests for nearest index and offset methods + * Add initial usage examples + +* Contributors: Victor Reijgwart + 1.5.3 (2023-09-28) ------------------ * Address failing DCHECKs for Morton conversions of negative indices diff --git a/libraries/wavemap/CMakeLists.txt b/libraries/wavemap/CMakeLists.txt index c6d86dc81..420d19e1b 100644 --- a/libraries/wavemap/CMakeLists.txt +++ b/libraries/wavemap/CMakeLists.txt @@ -66,7 +66,6 @@ add_library(${PROJECT_NAME} src/integrator/ray_tracing/ray_tracing_integrator.cc src/integrator/integrator_base.cc src/integrator/integrator_factory.cc - src/utils/eigen_format.cc src/utils/stopwatch.cc src/utils/thread_pool.cc) target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES}) @@ -114,8 +113,11 @@ if (CATKIN_ENABLE_TESTING) test/src/utils/test_data_utils.cc test/src/utils/test_fill_utils.cc test/src/utils/test_int_math.cc - test/src/utils/test_tree_math.cc - test/src/utils/test_thread_pool.cc) + test/src/utils/test_log_odds_converter.cc + test/src/utils/test_map_interpolator.cpp + test/src/utils/test_query_accelerator.cc + test/src/utils/test_thread_pool.cc + test/src/utils/test_tree_math.cc) target_include_directories(test_${PROJECT_NAME} PRIVATE test/include) target_link_libraries(test_${PROJECT_NAME} ${PROJECT_NAME} gtest_main) endif () diff --git a/libraries/wavemap/include/wavemap/common.h b/libraries/wavemap/include/wavemap/common.h index b5619dfb9..fc9d9218b 100644 --- a/libraries/wavemap/include/wavemap/common.h +++ b/libraries/wavemap/include/wavemap/common.h @@ -8,7 +8,7 @@ #include #include -#include "wavemap/constants.h" +#include "wavemap/utils/data/constants.h" namespace wavemap { using FloatingPoint = float; @@ -69,6 +69,12 @@ inline constexpr int dim_v = 2; template <> inline constexpr int dim_v = 3; +using ImageCoordinates = Vector2D; +struct SensorCoordinates { + ImageCoordinates image; + FloatingPoint depth; +}; + constexpr auto kEpsilon = constants::kEpsilon; constexpr auto kNaN = std::numeric_limits::quiet_NaN(); diff --git a/libraries/wavemap/include/wavemap/config/config_base.h b/libraries/wavemap/include/wavemap/config/config_base.h index ddf285483..ea0a88f68 100644 --- a/libraries/wavemap/include/wavemap/config/config_base.h +++ b/libraries/wavemap/include/wavemap/config/config_base.h @@ -6,7 +6,7 @@ #include "wavemap/config/param.h" #include "wavemap/config/param_checks.h" #include "wavemap/config/value_with_unit.h" -#include "wavemap/utils/type_utils.h" +#include "wavemap/utils/meta/type_utils.h" namespace wavemap { template , Radians, Pixels, Seconds>::Append; using MemberPointer = - inject_type_list_as_member_ptrs_t; + meta::inject_type_list_as_member_ptrs_t; struct MemberMetadata { param::Name name; MemberPointer ptr; diff --git a/libraries/wavemap/include/wavemap/config/impl/config_base_inl.h b/libraries/wavemap/include/wavemap/config/impl/config_base_inl.h index 0a04a245a..dc37a71cf 100644 --- a/libraries/wavemap/include/wavemap/config/impl/config_base_inl.h +++ b/libraries/wavemap/include/wavemap/config/impl/config_base_inl.h @@ -61,7 +61,7 @@ namespace wavemap { namespace detail { // Loader for PrimitiveValueTypes template >, + typename ConfigValueT = meta::member_type_t>, std::enable_if_t, bool> = true> void loadParam(const param::Name& param_name, const param::Value& param_value, @@ -92,7 +92,7 @@ void loadParam(const param::Name& param_name, const param::Value& param_value, // ConfigBase, type IDs derived from TypeSelector, and config values derived // from ValueWithUnits template >, + typename ConfigValueT = meta::member_type_t>, decltype(ConfigValueT::from(std::declval()), bool()) = true> void loadParam(const param::Name& param_name, const param::Value& param_value, @@ -103,7 +103,7 @@ void loadParam(const param::Name& param_name, const param::Value& param_value, } else { // Report the error, and print the fallback (default) value that will be // used instead if possible - if constexpr (has_to_str_member_fn_v) { + if constexpr (meta::has_to_str_member_fn_v) { LOG(WARNING) << "Param " << param_name << " could not be loaded. Keeping default value: " << config_value.toStr() << "."; diff --git a/libraries/wavemap/include/wavemap/config/param.h b/libraries/wavemap/include/wavemap/config/param.h index cf802ed12..e95910f4b 100644 --- a/libraries/wavemap/include/wavemap/config/param.h +++ b/libraries/wavemap/include/wavemap/config/param.h @@ -7,7 +7,7 @@ #include #include "wavemap/common.h" -#include "wavemap/utils/type_utils.h" +#include "wavemap/utils/meta/type_utils.h" namespace wavemap::param { using Name = std::string; @@ -57,8 +57,9 @@ class ValueT { std::variant data_; }; -using PrimitiveValueTypes = TypeList; -using Value = inject_type_list_t; +using PrimitiveValueTypes = + meta::TypeList; +using Value = meta::inject_type_list_t; using Array = Value::Array; using Map = Value::Map; } // namespace wavemap::param diff --git a/libraries/wavemap/include/wavemap/config/param_checks.h b/libraries/wavemap/include/wavemap/config/param_checks.h index 09ec91c53..fe01a66fa 100644 --- a/libraries/wavemap/include/wavemap/config/param_checks.h +++ b/libraries/wavemap/include/wavemap/config/param_checks.h @@ -4,27 +4,32 @@ #include #include -#include "wavemap/utils/nameof.h" +#include "wavemap/utils/meta/nameof.h" -namespace wavemap { -#define IS_PARAM_EQ(value, threshold, verbose) \ - is_param>(value, threshold, verbose, NAMEOF(value), "==") +#define IS_PARAM_EQ(value, threshold, verbose) \ + wavemap::is_param>(value, threshold, verbose, NAMEOF(value), \ + "==") -#define IS_PARAM_NE(value, threshold, verbose) \ - is_param>(value, threshold, verbose, NAMEOF(value), "!=") +#define IS_PARAM_NE(value, threshold, verbose) \ + wavemap::is_param>(value, threshold, verbose, \ + NAMEOF(value), "!=") #define IS_PARAM_LT(value, threshold, verbose) \ - is_param>(value, threshold, verbose, NAMEOF(value), "<") + wavemap::is_param>(value, threshold, verbose, NAMEOF(value), "<") -#define IS_PARAM_LE(value, threshold, verbose) \ - is_param>(value, threshold, verbose, NAMEOF(value), "<=") +#define IS_PARAM_LE(value, threshold, verbose) \ + wavemap::is_param>(value, threshold, verbose, \ + NAMEOF(value), "<=") -#define IS_PARAM_GT(value, threshold, verbose) \ - is_param>(value, threshold, verbose, NAMEOF(value), ">") +#define IS_PARAM_GT(value, threshold, verbose) \ + wavemap::is_param>(value, threshold, verbose, NAMEOF(value), \ + ">") -#define IS_PARAM_GE(value, threshold, verbose) \ - is_param>(value, threshold, verbose, NAMEOF(value), ">=") +#define IS_PARAM_GE(value, threshold, verbose) \ + wavemap::is_param>(value, threshold, verbose, \ + NAMEOF(value), ">=") +namespace wavemap { template bool is_param(A value, B threshold) { return ComparisonOp{}(value, threshold); diff --git a/libraries/wavemap/include/wavemap/data_structure/aabb.h b/libraries/wavemap/include/wavemap/data_structure/aabb.h index 4e6a98fb6..c0ddf89a4 100644 --- a/libraries/wavemap/include/wavemap/data_structure/aabb.h +++ b/libraries/wavemap/include/wavemap/data_structure/aabb.h @@ -5,8 +5,8 @@ #include #include "wavemap/common.h" -#include "wavemap/utils/eigen_format.h" -#include "wavemap/utils/int_math.h" +#include "wavemap/utils/math/int_math.h" +#include "wavemap/utils/print/eigen.h" namespace wavemap { template @@ -89,8 +89,8 @@ struct AABB { std::string toString() const { std::stringstream ss; - ss << "[min =" << EigenFormat::oneLine(min) - << ", max =" << EigenFormat::oneLine(max) << "]"; + ss << "[min =" << print::eigen::oneLine(min) + << ", max =" << print::eigen::oneLine(max) << "]"; return ss.str(); } }; diff --git a/libraries/wavemap/include/wavemap/data_structure/chunked_ndtree/chunked_ndtree.h b/libraries/wavemap/include/wavemap/data_structure/chunked_ndtree/chunked_ndtree.h index bb046d085..f3bbc6097 100644 --- a/libraries/wavemap/include/wavemap/data_structure/chunked_ndtree/chunked_ndtree.h +++ b/libraries/wavemap/include/wavemap/data_structure/chunked_ndtree/chunked_ndtree.h @@ -8,7 +8,7 @@ #include "wavemap/common.h" #include "wavemap/data_structure/chunked_ndtree/ndtree_node_chunk.h" #include "wavemap/indexing/ndtree_index.h" -#include "wavemap/iterator/subtree_iterator.h" +#include "wavemap/utils/iterate/subtree_iterator.h" namespace wavemap { template diff --git a/libraries/wavemap/include/wavemap/data_structure/chunked_ndtree/impl/chunked_ndtree_inl.h b/libraries/wavemap/include/wavemap/data_structure/chunked_ndtree/impl/chunked_ndtree_inl.h index 72b6cfb1f..29b7231b9 100644 --- a/libraries/wavemap/include/wavemap/data_structure/chunked_ndtree/impl/chunked_ndtree_inl.h +++ b/libraries/wavemap/include/wavemap/data_structure/chunked_ndtree/impl/chunked_ndtree_inl.h @@ -7,7 +7,6 @@ #include "wavemap/data_structure/pointcloud.h" #include "wavemap/indexing/index_conversions.h" -#include "wavemap/utils/eigen_format.h" namespace wavemap { template diff --git a/libraries/wavemap/include/wavemap/data_structure/chunked_ndtree/impl/ndtree_node_chunk_inl.h b/libraries/wavemap/include/wavemap/data_structure/chunked_ndtree/impl/ndtree_node_chunk_inl.h index 8c8d725cb..62476c621 100644 --- a/libraries/wavemap/include/wavemap/data_structure/chunked_ndtree/impl/ndtree_node_chunk_inl.h +++ b/libraries/wavemap/include/wavemap/data_structure/chunked_ndtree/impl/ndtree_node_chunk_inl.h @@ -4,7 +4,7 @@ #include #include -#include "wavemap/utils/data_utils.h" +#include "wavemap/utils/data/comparisons.h" namespace wavemap { template @@ -22,7 +22,7 @@ template bool NdtreeNodeChunk::hasNonzeroData() const { return std::any_of( node_data_.cbegin(), node_data_.cend(), - [](const auto& node_data) { return data_utils::is_nonzero(node_data); }); + [](const auto& node_data) { return data::is_nonzero(node_data); }); } template @@ -30,7 +30,7 @@ bool NdtreeNodeChunk::hasNonzeroData( FloatingPoint threshold) const { return std::any_of(node_data_.cbegin(), node_data_.cend(), [threshold](const auto& node_data) { - return data_utils::is_nonzero(node_data, threshold); + return data::is_nonzero(node_data, threshold); }); } @@ -127,14 +127,14 @@ template bool NdtreeNodeChunk::nodeHasNonzeroData( LinearIndex relative_node_index) const { DCHECK_LT(relative_node_index, kNumInnerNodes); - return data_utils::is_nonzero(node_data_[relative_node_index]); + return data::is_nonzero(node_data_[relative_node_index]); } template bool NdtreeNodeChunk::nodeHasNonzeroData( LinearIndex relative_node_index, FloatingPoint threshold) const { DCHECK_LT(relative_node_index, kNumInnerNodes); - return data_utils::is_nonzero(node_data_[relative_node_index], threshold); + return data::is_nonzero(node_data_[relative_node_index], threshold); } template diff --git a/libraries/wavemap/include/wavemap/data_structure/chunked_ndtree/ndtree_node_chunk.h b/libraries/wavemap/include/wavemap/data_structure/chunked_ndtree/ndtree_node_chunk.h index 82106a424..c604042b1 100644 --- a/libraries/wavemap/include/wavemap/data_structure/chunked_ndtree/ndtree_node_chunk.h +++ b/libraries/wavemap/include/wavemap/data_structure/chunked_ndtree/ndtree_node_chunk.h @@ -8,7 +8,7 @@ #include "wavemap/common.h" #include "wavemap/indexing/ndtree_index.h" -#include "wavemap/utils/tree_math.h" +#include "wavemap/utils/math/tree_math.h" namespace wavemap { template diff --git a/libraries/wavemap/include/wavemap/data_structure/image.h b/libraries/wavemap/include/wavemap/data_structure/image.h index 670e6de43..d5364902b 100644 --- a/libraries/wavemap/include/wavemap/data_structure/image.h +++ b/libraries/wavemap/include/wavemap/data_structure/image.h @@ -6,7 +6,7 @@ #include "wavemap/common.h" #include "wavemap/data_structure/posed_object.h" -#include "wavemap/utils/fill_utils.h" +#include "wavemap/utils/data/fill.h" namespace wavemap { template @@ -17,13 +17,14 @@ class Image { using Data = Eigen::Matrix; explicit Image(const Index2D& dimensions, - PixelT initial_value = fill::zero()) + PixelT initial_value = data::fill::zero()) : Image(dimensions.x(), dimensions.y(), initial_value) {} Image(IndexElement num_rows, IndexElement num_columns, - PixelT initial_value = fill::zero()) + PixelT initial_value = data::fill::zero()) : initial_value_(initial_value), data_(Data::Constant(num_rows, num_columns, initial_value)) {} - explicit Image(const Data& data, PixelT initial_value = fill::zero()) + explicit Image(const Data& data, + PixelT initial_value = data::fill::zero()) : initial_value_(initial_value), data_(data) {} bool empty() const { return !size(); } @@ -51,13 +52,13 @@ class Image { .all(); } - PixelT& at(Index2D index) { + PixelT& at(const Index2D& index) { DCHECK((0 <= index.array()).all()); DCHECK_LT(index.x(), data_.rows()); DCHECK_LT(index.y(), data_.cols()); return data_(index.x(), index.y()); } - const PixelT& at(Index2D index) const { + const PixelT& at(const Index2D& index) const { DCHECK((0 <= index.array()).all()); DCHECK_LT(index.x(), data_.rows()); DCHECK_LT(index.y(), data_.cols()); diff --git a/libraries/wavemap/include/wavemap/data_structure/ndtree/impl/ndtree_inl.h b/libraries/wavemap/include/wavemap/data_structure/ndtree/impl/ndtree_inl.h index 573fb1faf..c5f078bfc 100644 --- a/libraries/wavemap/include/wavemap/data_structure/ndtree/impl/ndtree_inl.h +++ b/libraries/wavemap/include/wavemap/data_structure/ndtree/impl/ndtree_inl.h @@ -6,7 +6,6 @@ #include "wavemap/data_structure/pointcloud.h" #include "wavemap/indexing/index_conversions.h" -#include "wavemap/utils/eigen_format.h" namespace wavemap { template diff --git a/libraries/wavemap/include/wavemap/data_structure/ndtree/impl/ndtree_node_inl.h b/libraries/wavemap/include/wavemap/data_structure/ndtree/impl/ndtree_node_inl.h index 6782dc792..3e8e11b08 100644 --- a/libraries/wavemap/include/wavemap/data_structure/ndtree/impl/ndtree_node_inl.h +++ b/libraries/wavemap/include/wavemap/data_structure/ndtree/impl/ndtree_node_inl.h @@ -4,7 +4,7 @@ #include #include -#include "wavemap/utils/data_utils.h" +#include "wavemap/utils/data/comparisons.h" namespace wavemap { template @@ -20,12 +20,12 @@ void NdtreeNode::clear() { template bool NdtreeNode::hasNonzeroData() const { - return data_utils::is_nonzero(data_); + return data::is_nonzero(data_); } template bool NdtreeNode::hasNonzeroData(FloatingPoint threshold) const { - return data_utils::is_nonzero(data_, threshold); + return data::is_nonzero(data_, threshold); } template diff --git a/libraries/wavemap/include/wavemap/data_structure/ndtree/ndtree.h b/libraries/wavemap/include/wavemap/data_structure/ndtree/ndtree.h index df750aaff..bc2e05413 100644 --- a/libraries/wavemap/include/wavemap/data_structure/ndtree/ndtree.h +++ b/libraries/wavemap/include/wavemap/data_structure/ndtree/ndtree.h @@ -8,7 +8,7 @@ #include "wavemap/common.h" #include "wavemap/data_structure/ndtree/ndtree_node.h" #include "wavemap/indexing/ndtree_index.h" -#include "wavemap/iterator/subtree_iterator.h" +#include "wavemap/utils/iterate/subtree_iterator.h" namespace wavemap { template diff --git a/libraries/wavemap/include/wavemap/data_structure/pointcloud.h b/libraries/wavemap/include/wavemap/data_structure/pointcloud.h index 25c71c939..cb2b5b61d 100644 --- a/libraries/wavemap/include/wavemap/data_structure/pointcloud.h +++ b/libraries/wavemap/include/wavemap/data_structure/pointcloud.h @@ -6,7 +6,7 @@ #include "wavemap/common.h" #include "wavemap/data_structure/posed_object.h" -#include "wavemap/iterator/pointcloud_iterator.h" +#include "wavemap/utils/iterate/pointcloud_iterator.h" namespace wavemap { template diff --git a/libraries/wavemap/include/wavemap/data_structure/volumetric/cell_types/haar_coefficients.h b/libraries/wavemap/include/wavemap/data_structure/volumetric/cell_types/haar_coefficients.h index 5a695462c..102d0cd64 100644 --- a/libraries/wavemap/include/wavemap/data_structure/volumetric/cell_types/haar_coefficients.h +++ b/libraries/wavemap/include/wavemap/data_structure/volumetric/cell_types/haar_coefficients.h @@ -7,6 +7,7 @@ #include "wavemap/common.h" #include "wavemap/indexing/ndtree_index.h" +#include "wavemap/utils/math/int_math.h" namespace wavemap { template diff --git a/libraries/wavemap/include/wavemap/data_structure/volumetric/cell_types/haar_transform.h b/libraries/wavemap/include/wavemap/data_structure/volumetric/cell_types/haar_transform.h index 1a24c4ecd..f90ca5311 100644 --- a/libraries/wavemap/include/wavemap/data_structure/volumetric/cell_types/haar_transform.h +++ b/libraries/wavemap/include/wavemap/data_structure/volumetric/cell_types/haar_transform.h @@ -6,7 +6,6 @@ #include "wavemap/common.h" #include "wavemap/data_structure/volumetric/cell_types/haar_coefficients.h" #include "wavemap/indexing/ndtree_index.h" -#include "wavemap/utils/bit_manipulation.h" namespace wavemap { // Transform 2^d scale space coefficients into 1 coarse scale and 2^d - 1 diff --git a/libraries/wavemap/include/wavemap/data_structure/volumetric/cell_types/impl/haar_transform_inl.h b/libraries/wavemap/include/wavemap/data_structure/volumetric/cell_types/impl/haar_transform_inl.h index 87a9c209a..e2fb260bd 100644 --- a/libraries/wavemap/include/wavemap/data_structure/volumetric/cell_types/impl/haar_transform_inl.h +++ b/libraries/wavemap/include/wavemap/data_structure/volumetric/cell_types/impl/haar_transform_inl.h @@ -1,6 +1,9 @@ #ifndef WAVEMAP_DATA_STRUCTURE_VOLUMETRIC_CELL_TYPES_IMPL_HAAR_TRANSFORM_INL_H_ #define WAVEMAP_DATA_STRUCTURE_VOLUMETRIC_CELL_TYPES_IMPL_HAAR_TRANSFORM_INL_H_ +#include "wavemap/utils/bits/bit_operations.h" +#include "wavemap/utils/math/int_math.h" + namespace wavemap { template typename HaarCoefficients::Parent ForwardLifted( @@ -13,9 +16,9 @@ typename HaarCoefficients::Parent ForwardLifted( // Perform the transform along axis 0 while moving the data into the array for (NdtreeIndexElement beam_idx = 0; beam_idx < kNumBeams; ++beam_idx) { const NdtreeIndexElement scale_idx = - bit_manip::squeeze_in(beam_idx, false, 0); + bit_ops::squeeze_in(beam_idx, false, 0); const NdtreeIndexElement detail_idx = - bit_manip::squeeze_in(beam_idx, true, 0); + bit_ops::squeeze_in(beam_idx, true, 0); parent_coefficients[detail_idx] = child_scales[detail_idx] - child_scales[scale_idx]; parent_coefficients[scale_idx] = @@ -26,9 +29,9 @@ typename HaarCoefficients::Parent ForwardLifted( for (NdtreeIndexElement dim_idx = 1; dim_idx < dim; ++dim_idx) { for (NdtreeIndexElement beam_idx = 0; beam_idx < kNumBeams; ++beam_idx) { const NdtreeIndexElement scale_idx = - bit_manip::squeeze_in(beam_idx, false, dim_idx); + bit_ops::squeeze_in(beam_idx, false, dim_idx); const NdtreeIndexElement detail_idx = - bit_manip::squeeze_in(beam_idx, true, dim_idx); + bit_ops::squeeze_in(beam_idx, true, dim_idx); parent_coefficients[detail_idx] -= parent_coefficients[scale_idx]; parent_coefficients[scale_idx] += static_cast(0.5) * parent_coefficients[detail_idx]; @@ -54,11 +57,11 @@ typename HaarCoefficients::Parent ForwardParallel( for (NdtreeIndexElement child_idx = 0; child_idx < kNumCoefficients; ++child_idx) { parent_coefficients[parent_idx] += - bit_manip::parity(parent_idx & ~child_idx) ? -child_scales[child_idx] - : +child_scales[child_idx]; + bit_ops::parity(parent_idx & ~child_idx) ? -child_scales[child_idx] + : +child_scales[child_idx]; } parent_coefficients[parent_idx] /= static_cast( - int_math::exp2(dim - bit_manip::popcount(parent_idx))); + int_math::exp2(dim - bit_ops::popcount(parent_idx))); } return parent_coefficients; @@ -74,11 +77,10 @@ typename HaarCoefficients::Parent ForwardSingleChild( HaarCoefficients::kNumCoefficients; for (NdtreeIndexElement parent_idx = 0; parent_idx < kNumCoefficients; ++parent_idx) { - parent_coefficients[parent_idx] = bit_manip::parity(parent_idx & ~child_idx) - ? -child_scale - : +child_scale; + parent_coefficients[parent_idx] = + bit_ops::parity(parent_idx & ~child_idx) ? -child_scale : +child_scale; parent_coefficients[parent_idx] /= static_cast( - int_math::exp2(dim - bit_manip::popcount(parent_idx))); + int_math::exp2(dim - bit_ops::popcount(parent_idx))); } return parent_coefficients; @@ -94,9 +96,9 @@ typename HaarCoefficients::CoefficientsArray BackwardLifted( // Perform the transform along axis 0 while moving the data into the array for (NdtreeIndexElement beam_idx = 0; beam_idx < kNumBeams; ++beam_idx) { const NdtreeIndexElement scale_idx = - bit_manip::squeeze_in(beam_idx, false, 0); + bit_ops::squeeze_in(beam_idx, false, 0); const NdtreeIndexElement detail_idx = - bit_manip::squeeze_in(beam_idx, true, 0); + bit_ops::squeeze_in(beam_idx, true, 0); child_scales[scale_idx] = parent[scale_idx] - static_cast(0.5) * parent[detail_idx]; child_scales[detail_idx] = parent[detail_idx] + child_scales[scale_idx]; @@ -105,9 +107,9 @@ typename HaarCoefficients::CoefficientsArray BackwardLifted( for (NdtreeIndexElement dim_idx = 1; dim_idx < dim; ++dim_idx) { for (NdtreeIndexElement beam_idx = 0; beam_idx < kNumBeams; ++beam_idx) { const NdtreeIndexElement scale_idx = - bit_manip::squeeze_in(beam_idx, false, dim_idx); + bit_ops::squeeze_in(beam_idx, false, dim_idx); const NdtreeIndexElement detail_idx = - bit_manip::squeeze_in(beam_idx, true, dim_idx); + bit_ops::squeeze_in(beam_idx, true, dim_idx); child_scales[scale_idx] -= static_cast(0.5) * child_scales[detail_idx]; child_scales[detail_idx] += child_scales[scale_idx]; @@ -131,8 +133,8 @@ typename HaarCoefficients::CoefficientsArray BackwardParallel( ++parent_idx) { const ValueT contribution = parent.details[parent_idx - 1] / - static_cast(int_math::exp2(bit_manip::popcount(parent_idx))); - child_scales[child_idx] += bit_manip::parity(~child_idx & parent_idx) + static_cast(int_math::exp2(bit_ops::popcount(parent_idx))); + child_scales[child_idx] += bit_ops::parity(~child_idx & parent_idx) ? -contribution : contribution; } @@ -152,9 +154,9 @@ typename HaarCoefficients::Scale BackwardSingleChild( ++parent_idx) { const ValueT contribution = parent.details[parent_idx - 1] / - static_cast(int_math::exp2(bit_manip::popcount(parent_idx))); - scale += bit_manip::parity(~child_idx & parent_idx) ? -contribution - : contribution; + static_cast(int_math::exp2(bit_ops::popcount(parent_idx))); + scale += + bit_ops::parity(~child_idx & parent_idx) ? -contribution : contribution; } return scale; diff --git a/libraries/wavemap/include/wavemap/data_structure/volumetric/hashed_blocks.h b/libraries/wavemap/include/wavemap/data_structure/volumetric/hashed_blocks.h index 386005593..d87200903 100644 --- a/libraries/wavemap/include/wavemap/data_structure/volumetric/hashed_blocks.h +++ b/libraries/wavemap/include/wavemap/data_structure/volumetric/hashed_blocks.h @@ -9,7 +9,7 @@ #include "wavemap/config/config_base.h" #include "wavemap/data_structure/volumetric/volumetric_data_structure_base.h" #include "wavemap/indexing/index_hashes.h" -#include "wavemap/utils/int_math.h" +#include "wavemap/utils/math/int_math.h" namespace wavemap { class HashedBlocks : public VolumetricDataStructureBase { diff --git a/libraries/wavemap/include/wavemap/data_structure/volumetric/hashed_chunked_wavelet_octree.h b/libraries/wavemap/include/wavemap/data_structure/volumetric/hashed_chunked_wavelet_octree.h index d7d683d34..b0e309087 100644 --- a/libraries/wavemap/include/wavemap/data_structure/volumetric/hashed_chunked_wavelet_octree.h +++ b/libraries/wavemap/include/wavemap/data_structure/volumetric/hashed_chunked_wavelet_octree.h @@ -9,7 +9,7 @@ #include "wavemap/data_structure/volumetric/hashed_chunked_wavelet_octree_block.h" #include "wavemap/data_structure/volumetric/volumetric_data_structure_base.h" #include "wavemap/indexing/index_hashes.h" -#include "wavemap/utils/int_math.h" +#include "wavemap/utils/math/int_math.h" namespace wavemap { /** @@ -78,7 +78,7 @@ class HashedChunkedWaveletOctree : public VolumetricDataStructureBase { size_t size() const override; void threshold() override; void prune() override; - void pruneDistant() override; + void pruneSmart() override; void clear() override { blocks_.clear(); } size_t getMemoryUsage() const override; diff --git a/libraries/wavemap/include/wavemap/data_structure/volumetric/hashed_chunked_wavelet_octree_block.h b/libraries/wavemap/include/wavemap/data_structure/volumetric/hashed_chunked_wavelet_octree_block.h index bc240ff8d..c1d14d41d 100644 --- a/libraries/wavemap/include/wavemap/data_structure/volumetric/hashed_chunked_wavelet_octree_block.h +++ b/libraries/wavemap/include/wavemap/data_structure/volumetric/hashed_chunked_wavelet_octree_block.h @@ -6,7 +6,7 @@ #include "wavemap/data_structure/volumetric/cell_types/haar_coefficients.h" #include "wavemap/data_structure/volumetric/cell_types/haar_transform.h" #include "wavemap/data_structure/volumetric/volumetric_data_structure_base.h" -#include "wavemap/utils/time.h" +#include "wavemap/utils/time/time.h" namespace wavemap { class HashedChunkedWaveletOctreeBlock { @@ -52,8 +52,11 @@ class HashedChunkedWaveletOctreeBlock { void setNeedsPruning(bool value = true) { needs_pruning_ = value; } bool getNeedsPruning() const { return needs_pruning_; } + bool& getNeedsPruning() { return needs_pruning_; } + void setNeedsThresholding(bool value = true) { needs_thresholding_ = value; } bool getNeedsThresholding() const { return needs_thresholding_; } + void setLastUpdatedStamp(Timestamp stamp = Time::now()) { last_updated_stamp_ = stamp; } diff --git a/libraries/wavemap/include/wavemap/data_structure/volumetric/hashed_wavelet_octree.h b/libraries/wavemap/include/wavemap/data_structure/volumetric/hashed_wavelet_octree.h index ac2216443..4a09985cd 100644 --- a/libraries/wavemap/include/wavemap/data_structure/volumetric/hashed_wavelet_octree.h +++ b/libraries/wavemap/include/wavemap/data_structure/volumetric/hashed_wavelet_octree.h @@ -9,7 +9,7 @@ #include "wavemap/data_structure/volumetric/hashed_wavelet_octree_block.h" #include "wavemap/data_structure/volumetric/volumetric_data_structure_base.h" #include "wavemap/indexing/index_hashes.h" -#include "wavemap/utils/int_math.h" +#include "wavemap/utils/math/int_math.h" namespace wavemap { /** @@ -73,7 +73,7 @@ class HashedWaveletOctree : public VolumetricDataStructureBase { size_t size() const override; void threshold() override; void prune() override; - void pruneDistant() override; + void pruneSmart() override; void clear() override { blocks_.clear(); } size_t getMemoryUsage() const override; diff --git a/libraries/wavemap/include/wavemap/data_structure/volumetric/hashed_wavelet_octree_block.h b/libraries/wavemap/include/wavemap/data_structure/volumetric/hashed_wavelet_octree_block.h index 421c6a40d..c891703c7 100644 --- a/libraries/wavemap/include/wavemap/data_structure/volumetric/hashed_wavelet_octree_block.h +++ b/libraries/wavemap/include/wavemap/data_structure/volumetric/hashed_wavelet_octree_block.h @@ -6,7 +6,7 @@ #include "wavemap/data_structure/volumetric/cell_types/haar_coefficients.h" #include "wavemap/data_structure/volumetric/cell_types/haar_transform.h" #include "wavemap/data_structure/volumetric/volumetric_data_structure_base.h" -#include "wavemap/utils/time.h" +#include "wavemap/utils/time/time.h" namespace wavemap { class HashedWaveletOctreeBlock { diff --git a/libraries/wavemap/include/wavemap/data_structure/volumetric/impl/hashed_blocks_inl.h b/libraries/wavemap/include/wavemap/data_structure/volumetric/impl/hashed_blocks_inl.h index fb301f9b3..179569230 100644 --- a/libraries/wavemap/include/wavemap/data_structure/volumetric/impl/hashed_blocks_inl.h +++ b/libraries/wavemap/include/wavemap/data_structure/volumetric/impl/hashed_blocks_inl.h @@ -6,7 +6,7 @@ #include #include "wavemap/indexing/index_conversions.h" -#include "wavemap/iterator/grid_iterator.h" +#include "wavemap/utils/iterate/grid_iterator.h" namespace wavemap { inline FloatingPoint HashedBlocks::getCellValue(const Index3D& index) const { diff --git a/libraries/wavemap/include/wavemap/data_structure/volumetric/impl/hashed_chunked_wavelet_octree_block_inl.h b/libraries/wavemap/include/wavemap/data_structure/volumetric/impl/hashed_chunked_wavelet_octree_block_inl.h index 5be3714ed..b194cfa8f 100644 --- a/libraries/wavemap/include/wavemap/data_structure/volumetric/impl/hashed_chunked_wavelet_octree_block_inl.h +++ b/libraries/wavemap/include/wavemap/data_structure/volumetric/impl/hashed_chunked_wavelet_octree_block_inl.h @@ -15,7 +15,7 @@ inline bool HashedChunkedWaveletOctreeBlock::empty() const { inline FloatingPoint HashedChunkedWaveletOctreeBlock::getTimeSinceLastUpdated() const { - return to_seconds(Time::now() - last_updated_stamp_); + return time::to_seconds(Time::now() - last_updated_stamp_); } inline FloatingPoint HashedChunkedWaveletOctreeBlock::getCellValue( diff --git a/libraries/wavemap/include/wavemap/data_structure/volumetric/impl/hashed_wavelet_octree_block_inl.h b/libraries/wavemap/include/wavemap/data_structure/volumetric/impl/hashed_wavelet_octree_block_inl.h index 06a588411..03fe0a012 100644 --- a/libraries/wavemap/include/wavemap/data_structure/volumetric/impl/hashed_wavelet_octree_block_inl.h +++ b/libraries/wavemap/include/wavemap/data_structure/volumetric/impl/hashed_wavelet_octree_block_inl.h @@ -14,7 +14,7 @@ inline bool HashedWaveletOctreeBlock::empty() const { } inline FloatingPoint HashedWaveletOctreeBlock::getTimeSinceLastUpdated() const { - return to_seconds(Time::now() - last_updated_stamp_); + return time::to_seconds(Time::now() - last_updated_stamp_); } inline FloatingPoint HashedWaveletOctreeBlock::getCellValue( diff --git a/libraries/wavemap/include/wavemap/data_structure/volumetric/volumetric_data_structure_base.h b/libraries/wavemap/include/wavemap/data_structure/volumetric/volumetric_data_structure_base.h index 9be7bf247..69a247a7a 100644 --- a/libraries/wavemap/include/wavemap/data_structure/volumetric/volumetric_data_structure_base.h +++ b/libraries/wavemap/include/wavemap/data_structure/volumetric/volumetric_data_structure_base.h @@ -70,7 +70,7 @@ class VolumetricDataStructureBase { virtual size_t size() const = 0; virtual void threshold() = 0; virtual void prune() = 0; - virtual void pruneDistant() { + virtual void pruneSmart() { // NOTE: This method can be overriden by derived classes to provide more // efficient selective pruning strategies. Otherwise, just prune all. prune(); diff --git a/libraries/wavemap/include/wavemap/indexing/impl/ndtree_index_inl.h b/libraries/wavemap/include/wavemap/indexing/impl/ndtree_index_inl.h index 682eccfbe..66c62f332 100644 --- a/libraries/wavemap/include/wavemap/indexing/impl/ndtree_index_inl.h +++ b/libraries/wavemap/include/wavemap/indexing/impl/ndtree_index_inl.h @@ -1,11 +1,13 @@ #ifndef WAVEMAP_INDEXING_IMPL_NDTREE_INDEX_INL_H_ #define WAVEMAP_INDEXING_IMPL_NDTREE_INDEX_INL_H_ +#include +#include #include #include -#include "wavemap/utils/bit_manipulation.h" -#include "wavemap/utils/tree_math.h" +#include "wavemap/utils/bits/bit_operations.h" +#include "wavemap/utils/math/tree_math.h" namespace wavemap { template @@ -68,6 +70,28 @@ NdtreeIndexRelativeChild NdtreeIndex::computeRelativeChildIndex( return (morton >> (child_height * dim)) & kRelativeChildIndexMask; } +template +IndexElement NdtreeIndex::computeLastCommonAncestorHeight( + MortonIndex first_morton, Element first_height, MortonIndex second_morton, + Element second_height) { + // When the morton indices are identical, the last common ancestor + // corresponds to the most senior of the two children + const Element max_height = std::max(first_height, second_height); + if (first_morton == second_morton) { + return max_height; + } + + // Find the first height where the indices start to differ + // NOTE: We count the bit index from right to left s.t. it corresponds to the + // height in the tree, instead of the depth. + const MortonIndex morton_diff = first_morton ^ second_morton; + const Element first_diff_bit = + static_cast(std::numeric_limits::digits) - + static_cast(bit_ops::clz(morton_diff)); + const Element first_diff_height = int_math::div_round_up(first_diff_bit, dim); + return std::max(max_height, first_diff_height); +} + template LinearIndex NdtreeIndex::computeLevelTraversalDistance( MortonIndex morton, NdtreeIndex::Element parent_height, diff --git a/libraries/wavemap/include/wavemap/indexing/index_conversions.h b/libraries/wavemap/include/wavemap/indexing/index_conversions.h index 081da02ab..a19e31216 100644 --- a/libraries/wavemap/include/wavemap/indexing/index_conversions.h +++ b/libraries/wavemap/include/wavemap/indexing/index_conversions.h @@ -8,12 +8,10 @@ #include "wavemap/common.h" #include "wavemap/data_structure/aabb.h" #include "wavemap/indexing/ndtree_index.h" -#include "wavemap/utils/int_math.h" -#include "wavemap/utils/morton_encoding.h" +#include "wavemap/utils/bits/morton_encoding.h" +#include "wavemap/utils/math/int_math.h" namespace wavemap::convert { -// TODO(victorr): Check styleguide on whether these classless methods names -// should start with a capital template inline Index scaledPointToNearestIndex(const Point& point) { return (point - Vector::Constant(0.5f)) @@ -82,6 +80,11 @@ inline FloatingPoint heightToCellWidth(FloatingPoint min_cell_width, return min_cell_width * static_cast(int_math::exp2(height)); } +inline NdtreeIndexElement cellWidthToHeight(FloatingPoint cell_width, + FloatingPoint min_cell_width_inv) { + return std::ceil(std::log2(cell_width * min_cell_width_inv)); +} + template inline LinearIndex indexToLinearIndex(const Index& index) { DCHECK((0 <= index.array() && index.array() < cells_per_side).all()); diff --git a/libraries/wavemap/include/wavemap/indexing/ndtree_index.h b/libraries/wavemap/include/wavemap/indexing/ndtree_index.h index 2d910b65c..7f619af27 100644 --- a/libraries/wavemap/include/wavemap/indexing/ndtree_index.h +++ b/libraries/wavemap/include/wavemap/indexing/ndtree_index.h @@ -6,7 +6,7 @@ #include #include "wavemap/common.h" -#include "wavemap/utils/int_math.h" +#include "wavemap/utils/math/int_math.h" namespace wavemap { using NdtreeIndexElement = int; @@ -46,6 +46,11 @@ struct NdtreeIndex { static RelativeChild computeRelativeChildIndex(MortonIndex morton, Element parent_height); + static IndexElement computeLastCommonAncestorHeight(MortonIndex first_morton, + Element first_height, + MortonIndex second_morton, + Element second_height); + static LinearIndex computeLevelTraversalDistance(MortonIndex morton, Element parent_height, Element child_height); diff --git a/libraries/wavemap/include/wavemap/integrator/integrator_factory.h b/libraries/wavemap/include/wavemap/integrator/integrator_factory.h index 9be0f08a5..245961829 100644 --- a/libraries/wavemap/include/wavemap/integrator/integrator_factory.h +++ b/libraries/wavemap/include/wavemap/integrator/integrator_factory.h @@ -1,10 +1,12 @@ #ifndef WAVEMAP_INTEGRATOR_INTEGRATOR_FACTORY_H_ #define WAVEMAP_INTEGRATOR_INTEGRATOR_FACTORY_H_ +#include #include #include "wavemap/data_structure/volumetric/volumetric_data_structure_base.h" #include "wavemap/integrator/integrator_base.h" +#include "wavemap/utils/thread_pool.h" namespace wavemap { class IntegratorFactory { @@ -12,11 +14,13 @@ class IntegratorFactory { static IntegratorBase::Ptr create( const param::Value& params, VolumetricDataStructureBase::Ptr occupancy_map, + std::shared_ptr thread_pool = nullptr, std::optional default_integrator_type = std::nullopt); static IntegratorBase::Ptr create( IntegratorType integrator_type, const param::Value& params, - VolumetricDataStructureBase::Ptr occupancy_map); + VolumetricDataStructureBase::Ptr occupancy_map, + std::shared_ptr thread_pool = nullptr); }; } // namespace wavemap diff --git a/libraries/wavemap/include/wavemap/integrator/measurement_model/continuous_beam.h b/libraries/wavemap/include/wavemap/integrator/measurement_model/continuous_beam.h index 722fd8c26..fffa1abd8 100644 --- a/libraries/wavemap/include/wavemap/integrator/measurement_model/continuous_beam.h +++ b/libraries/wavemap/include/wavemap/integrator/measurement_model/continuous_beam.h @@ -10,7 +10,7 @@ #include "wavemap/indexing/index_conversions.h" #include "wavemap/integrator/measurement_model/measurement_model_base.h" #include "wavemap/integrator/projective/update_type.h" -#include "wavemap/utils/eigen_format.h" +#include "wavemap/utils/print/eigen.h" namespace wavemap { /** @@ -59,7 +59,10 @@ class ContinuousBeam : public MeasurementModelBase { : config_(config.checkValid()), projection_model_(std::move(projection_model)), range_image_(std::move(range_image)), - beam_offset_image_(std::move(beam_offset_image)) {} + beam_offset_image_(std::move(beam_offset_image)) { + CHECK_EQ(range_image_->getDimensions(), + beam_offset_image_->getDimensions()); + } const ContinuousBeamConfig& getConfig() const { return config_; } FloatingPoint getPaddingAngle() const override { return angle_threshold_; } @@ -75,7 +78,7 @@ class ContinuousBeam : public MeasurementModelBase { FloatingPoint cell_bounding_radius) const override; FloatingPoint computeUpdate( - const Vector3D& sensor_coordinates) const override; + const SensorCoordinates& sensor_coordinates) const override; private: const ContinuousBeamConfig config_; @@ -85,6 +88,8 @@ class ContinuousBeam : public MeasurementModelBase { const Image::ConstPtr beam_offset_image_; const FloatingPoint angle_threshold_ = 6.f * config_.angle_sigma; + const FloatingPoint angle_threshold_squared = + angle_threshold_ * angle_threshold_; const FloatingPoint range_threshold_front = 3.f * config_.range_sigma; const FloatingPoint range_threshold_back_ = 6.f * config_.range_sigma; // NOTE: The angle and upper range thresholds have a width of 6 sigmas because @@ -92,10 +97,21 @@ class ContinuousBeam : public MeasurementModelBase { // angular/range uncertainty extends the non-zero regions with another 3 // sigma. + // Compute the measurement update for a neighborhood in the range image + FloatingPoint computeBeamUpdateNearestNeighbor( + const Image<>& range_image, const Image& beam_offset_image, + const ProjectorBase& projection_model, + const SensorCoordinates& sensor_coordinates) const; + FloatingPoint computeBeamUpdateAllNeighbors( + const Image<>& range_image, const Image& beam_offset_image, + const ProjectorBase& projection_model, + const SensorCoordinates& sensor_coordinates) const; + // Compute the measurement update for a single beam - FloatingPoint computeBeamUpdate(FloatingPoint cell_to_sensor_distance, - FloatingPoint cell_to_beam_image_error_norm, - FloatingPoint measured_distance) const; + FloatingPoint computeBeamUpdate( + FloatingPoint cell_to_sensor_distance, + FloatingPoint cell_to_beam_image_error_norm_squared, + FloatingPoint measured_distance) const; }; } // namespace wavemap diff --git a/libraries/wavemap/include/wavemap/integrator/measurement_model/continuous_ray.h b/libraries/wavemap/include/wavemap/integrator/measurement_model/continuous_ray.h index 5e91cb760..faef614fb 100644 --- a/libraries/wavemap/include/wavemap/integrator/measurement_model/continuous_ray.h +++ b/libraries/wavemap/include/wavemap/integrator/measurement_model/continuous_ray.h @@ -64,7 +64,7 @@ class ContinuousRay : public MeasurementModelBase { FloatingPoint cell_bounding_radius) const override; FloatingPoint computeUpdate( - const Vector3D& sensor_coordinates) const override; + const SensorCoordinates& sensor_coordinates) const override; private: const ContinuousRayConfig config_; @@ -77,8 +77,7 @@ class ContinuousRay : public MeasurementModelBase { // NOTE: The upper range thresholds has a width of 6 sigmas because the // assumed 'ground truth' surface thickness is 3 sigma, and the range // uncertainty extends the non-zero regions with another 3 sigma. - - FloatingPoint computeBeamUpdate(const Vector3D& sensor_coordinates, + FloatingPoint computeBeamUpdate(const SensorCoordinates& sensor_coordinates, const Index2D& image_index) const; // Compute the full measurement update, i.e. valid anywhere diff --git a/libraries/wavemap/include/wavemap/integrator/measurement_model/impl/continuous_beam_inl.h b/libraries/wavemap/include/wavemap/integrator/measurement_model/impl/continuous_beam_inl.h index daeabcd6a..b3f1a677f 100644 --- a/libraries/wavemap/include/wavemap/integrator/measurement_model/impl/continuous_beam_inl.h +++ b/libraries/wavemap/include/wavemap/integrator/measurement_model/impl/continuous_beam_inl.h @@ -31,84 +31,107 @@ inline FloatingPoint ContinuousBeam::computeWorstCaseApproximationError( } inline FloatingPoint ContinuousBeam::computeUpdate( - const Vector3D& sensor_coordinates) const { - const FloatingPoint cell_to_sensor_distance = sensor_coordinates.z(); - + const SensorCoordinates& sensor_coordinates) const { switch (config_.beam_selector_type.toTypeId()) { - case BeamSelectorType::kNearestNeighbor: { - // Get the measured distance and cell to beam offset - const auto [image_index, cell_offset] = - projection_model_->imageToNearestIndexAndOffset( - sensor_coordinates.head<2>()); - if (!range_image_->isIndexWithinBounds(image_index)) { - return 0.f; - } - const FloatingPoint measured_distance = range_image_->at(image_index); - const Vector2D cell_to_beam_offset = - beam_offset_image_->at(image_index) - cell_offset; - - // Compute the image error norm - const FloatingPoint cell_to_beam_image_error_norm = - projection_model_->imageOffsetToErrorNorm( - sensor_coordinates.head<2>(), cell_to_beam_offset); - - // Compute the update - return computeBeamUpdate(cell_to_sensor_distance, - cell_to_beam_image_error_norm, - measured_distance); - } + case BeamSelectorType::kNearestNeighbor: + return computeBeamUpdateNearestNeighbor( + *range_image_, *beam_offset_image_, *projection_model_, + sensor_coordinates); + case BeamSelectorType::kAllNeighbors: + return computeBeamUpdateAllNeighbors(*range_image_, *beam_offset_image_, + *projection_model_, + sensor_coordinates); + default: + return 0.f; + } +} - case BeamSelectorType::kAllNeighbors: { - // Get the measured distances and cell to beam offsets - std::array measured_distances{}; - ProjectorBase::CellToBeamOffsetArray cell_to_beam_offsets{}; - const auto [image_indices, cell_offsets] = - projection_model_->imageToNearestIndicesAndOffsets( - sensor_coordinates.head<2>()); - for (int neighbor_idx = 0; neighbor_idx < 4; ++neighbor_idx) { - const Index2D& image_index = image_indices[neighbor_idx]; - const Vector2D& cell_offset = cell_offsets[neighbor_idx]; - // Get the measured distance and cell to beam offset - if (!range_image_->isIndexWithinBounds(image_index)) { - continue; - } - measured_distances[neighbor_idx] = range_image_->at(image_index); - cell_to_beam_offsets[neighbor_idx] = - beam_offset_image_->at(image_index) - cell_offset; - } - - // Compute the image error norms - const auto cell_to_beam_image_error_norms = - projection_model_->imageOffsetsToErrorNorms( - sensor_coordinates.head<2>(), cell_to_beam_offsets); - - // Compute the update - FloatingPoint update = 0.f; - for (int neighbor_idx = 0; neighbor_idx < 4; ++neighbor_idx) { - update += - computeBeamUpdate(cell_to_sensor_distance, - cell_to_beam_image_error_norms[neighbor_idx], - measured_distances[neighbor_idx]); - } - return update; +inline FloatingPoint ContinuousBeam::computeBeamUpdateNearestNeighbor( + const Image<>& range_image, const Image& beam_offset_image, + const ProjectorBase& projection_model, + const SensorCoordinates& sensor_coordinates) const { + // Get the measured distance and cell to beam offset + const auto [image_index, cell_offset] = + projection_model.imageToNearestIndexAndOffset(sensor_coordinates.image); + if (!range_image.isIndexWithinBounds(image_index)) { + return 0.f; + } + const FloatingPoint measured_distance = range_image.at(image_index); + const Vector2D cell_to_beam_offset = + beam_offset_image.at(image_index) - cell_offset; + + // Compute the image error norm + const FloatingPoint cell_to_beam_image_error_norm_squared = + projection_model.imageOffsetToErrorSquaredNorm(sensor_coordinates.image, + cell_to_beam_offset); + + // Compute the update + const FloatingPoint cell_to_sensor_distance = sensor_coordinates.depth; + return computeBeamUpdate(cell_to_sensor_distance, + cell_to_beam_image_error_norm_squared, + measured_distance); +} + +inline FloatingPoint ContinuousBeam::computeBeamUpdateAllNeighbors( + const Image<>& range_image, const Image& beam_offset_image, + const ProjectorBase& projection_model, + const SensorCoordinates& sensor_coordinates) const { + // Get the measured distances and cell to beam offsets + std::array measured_distances{}; + auto [image_indices, cell_to_beam_offsets] = + projection_model.imageToNearestIndicesAndOffsets( + sensor_coordinates.image); + constexpr bool row_major = Image<>::Data::IsRowMajor; + const int stride = static_cast(range_image.getData().outerStride()); + for (int neighbor_idx = 0; neighbor_idx < 4; ++neighbor_idx) { + const auto& image_index = image_indices.col(neighbor_idx); + // Get the measured distance and cell to beam offset + if (range_image.isIndexWithinBounds(image_index)) { + const int linear_index = + row_major ? stride * image_index[0] + image_index[1] // NOLINT + : stride * image_index[1] + image_index[0]; + measured_distances[neighbor_idx] = + range_image.getData().coeff(linear_index); + cell_to_beam_offsets.col(neighbor_idx) -= + beam_offset_image.getData().coeffRef(linear_index); } + } - default: - return 0.f; + // Compute the image error norms + const auto cell_to_beam_image_error_norms_sq = + projection_model.imageOffsetsToErrorSquaredNorms(sensor_coordinates.image, + cell_to_beam_offsets); + + // Compute the update + FloatingPoint update = 0.f; + const FloatingPoint cell_to_sensor_distance = sensor_coordinates.depth; + for (int neighbor_idx = 0; neighbor_idx < 4; ++neighbor_idx) { + update += computeBeamUpdate(cell_to_sensor_distance, + cell_to_beam_image_error_norms_sq[neighbor_idx], + measured_distances[neighbor_idx]); } + return update; } inline FloatingPoint ContinuousBeam::computeBeamUpdate( FloatingPoint cell_to_sensor_distance, - FloatingPoint cell_to_beam_image_error_norm, + FloatingPoint cell_to_beam_image_error_norm_squared, FloatingPoint measured_distance) const { const bool fully_in_unknown_space = - angle_threshold_ < cell_to_beam_image_error_norm || + angle_threshold_squared < cell_to_beam_image_error_norm_squared || measured_distance + range_threshold_back_ < cell_to_sensor_distance; if (fully_in_unknown_space) { return 0.f; } + const FloatingPoint g = + std::sqrt(cell_to_beam_image_error_norm_squared) / config_.angle_sigma; + // NOTE: As derived in our paper, angle_contrib = C(g + 3.f) - C(g - 3.f) + // where C is ApproximateGaussianDistribution::cumulative. Since 0 <= g, + // C is always 1 and angle_contrib simplifies to 1 - C(g - 3.f). + const FloatingPoint angle_contrib = + 1.f - ApproximateGaussianDistribution::cumulative(g - 3.f); + const bool fully_in_free_space = cell_to_sensor_distance < measured_distance - range_threshold_front; constexpr FloatingPoint kFreeSpaceRangeContrib = -0.5f; @@ -121,11 +144,6 @@ inline FloatingPoint ContinuousBeam::computeBeamUpdate( 0.5f * ApproximateGaussianDistribution::cumulative(f - 3.f) - 0.5f; } - const FloatingPoint g = cell_to_beam_image_error_norm / config_.angle_sigma; - const FloatingPoint angle_contrib = - ApproximateGaussianDistribution::cumulative(g + 3.f) - - ApproximateGaussianDistribution::cumulative(g - 3.f); - const FloatingPoint contribs = range_contrib * angle_contrib; const FloatingPoint scaled_contribs = (fully_in_free_space || contribs < 0.f) diff --git a/libraries/wavemap/include/wavemap/integrator/measurement_model/impl/continuous_ray_inl.h b/libraries/wavemap/include/wavemap/integrator/measurement_model/impl/continuous_ray_inl.h index e3f764c47..06467b69e 100644 --- a/libraries/wavemap/include/wavemap/integrator/measurement_model/impl/continuous_ray_inl.h +++ b/libraries/wavemap/include/wavemap/integrator/measurement_model/impl/continuous_ray_inl.h @@ -25,19 +25,19 @@ inline FloatingPoint ContinuousRay::computeWorstCaseApproximationError( } inline FloatingPoint ContinuousRay::computeUpdate( - const Vector3D& sensor_coordinates) const { + const SensorCoordinates& sensor_coordinates) const { switch (config_.beam_selector_type.toTypeId()) { case BeamSelectorType::kNearestNeighbor: { const auto image_index = - projection_model_->imageToNearestIndex(sensor_coordinates.head<2>()); + projection_model_->imageToNearestIndex(sensor_coordinates.image); return computeBeamUpdate(sensor_coordinates, image_index); } case BeamSelectorType::kAllNeighbors: { FloatingPoint update = 0.f; - const auto image_indices = projection_model_->imageToNearestIndices( - sensor_coordinates.head<2>()); + const auto image_indices = + projection_model_->imageToNearestIndices(sensor_coordinates.image); for (int neighbor_idx = 0; neighbor_idx < 4; ++neighbor_idx) { - const Index2D& image_index = image_indices[neighbor_idx]; + const Index2D& image_index = image_indices.col(neighbor_idx); update += computeBeamUpdate(sensor_coordinates, image_index); } return update; @@ -48,21 +48,22 @@ inline FloatingPoint ContinuousRay::computeUpdate( } inline FloatingPoint ContinuousRay::computeBeamUpdate( - const Vector3D& sensor_coordinates, const Index2D& image_index) const { + const SensorCoordinates& sensor_coordinates, + const Index2D& image_index) const { if (!range_image_->isIndexWithinBounds(image_index)) { return 0.f; } const FloatingPoint measured_distance = range_image_->at(image_index); - if (range_threshold_back_ < sensor_coordinates.z() - measured_distance) { + if (range_threshold_back_ < sensor_coordinates.depth - measured_distance) { return 0.f; } - if (sensor_coordinates.z() < measured_distance - range_threshold_front_) { + if (sensor_coordinates.depth < measured_distance - range_threshold_front_) { return computeFreeSpaceBeamUpdate(); } else { - return computeFullBeamUpdate(sensor_coordinates.z(), measured_distance); + return computeFullBeamUpdate(sensor_coordinates.depth, measured_distance); } } diff --git a/libraries/wavemap/include/wavemap/integrator/measurement_model/measurement_model_base.h b/libraries/wavemap/include/wavemap/integrator/measurement_model/measurement_model_base.h index 007fb0cde..6df7fd50b 100644 --- a/libraries/wavemap/include/wavemap/integrator/measurement_model/measurement_model_base.h +++ b/libraries/wavemap/include/wavemap/integrator/measurement_model/measurement_model_base.h @@ -43,7 +43,7 @@ class MeasurementModelBase { FloatingPoint cell_bounding_radius) const = 0; virtual FloatingPoint computeUpdate( - const Vector3D& sensor_coordinates) const = 0; + const SensorCoordinates& sensor_coordinates) const = 0; }; } // namespace wavemap diff --git a/libraries/wavemap/include/wavemap/integrator/projection_model/impl/ouster_projector_inl.h b/libraries/wavemap/include/wavemap/integrator/projection_model/impl/ouster_projector_inl.h index 82e25168e..d6124bbe4 100644 --- a/libraries/wavemap/include/wavemap/integrator/projection_model/impl/ouster_projector_inl.h +++ b/libraries/wavemap/include/wavemap/integrator/projection_model/impl/ouster_projector_inl.h @@ -1,8 +1,10 @@ #ifndef WAVEMAP_INTEGRATOR_PROJECTION_MODEL_IMPL_OUSTER_PROJECTOR_INL_H_ #define WAVEMAP_INTEGRATOR_PROJECTION_MODEL_IMPL_OUSTER_PROJECTOR_INL_H_ +#include "wavemap/utils/math/approximate_trigonometry.h" + namespace wavemap { -inline Vector3D OusterProjector::cartesianToSensor( +inline SensorCoordinates OusterProjector::cartesianToSensor( const Point3D& C_point) const { // Project the beam's endpoint into the 2D plane B whose origin lies at the // beam's start point, X-axis is parallel to the projection of the beam onto @@ -10,19 +12,19 @@ inline Vector3D OusterProjector::cartesianToSensor( const Point2D B_point{ C_point.head<2>().norm() - config_.lidar_origin_to_beam_origin, C_point.z() - config_.lidar_origin_to_sensor_origin_z_offset}; - const FloatingPoint elevation_angle = - approximate::atan2()(B_point.y(), B_point.x()); const FloatingPoint azimuth_angle = - approximate::atan2()(C_point.y(), C_point.x()); + approximate::atan2()(C_point[1], C_point[0]); + const FloatingPoint elevation_angle = + approximate::atan2()(B_point[1], B_point[0]); const FloatingPoint range = B_point.norm(); - return {elevation_angle, azimuth_angle, range}; + return {{elevation_angle, azimuth_angle}, range}; } inline Point3D OusterProjector::sensorToCartesian( - const Vector3D& coordinates) const { - const FloatingPoint elevation_angle = coordinates[0]; - const FloatingPoint azimuth_angle = coordinates[1]; - const FloatingPoint range = coordinates[2]; + const SensorCoordinates& coordinates) const { + const FloatingPoint elevation_angle = coordinates.image[0]; + const FloatingPoint azimuth_angle = coordinates.image[1]; + const FloatingPoint range = coordinates.depth; const Point2D B_point = range * Vector2D(std::cos(elevation_angle), std::sin(elevation_angle)) + Vector2D(config_.lidar_origin_to_beam_origin, @@ -32,41 +34,47 @@ inline Point3D OusterProjector::sensorToCartesian( return C_point; } -inline FloatingPoint OusterProjector::imageOffsetToErrorNorm( - const Vector2D& linearization_point, Vector2D offset) const { +inline FloatingPoint OusterProjector::imageOffsetToErrorSquaredNorm( + const ImageCoordinates& linearization_point, const Vector2D& offset) const { // Scale the azimuth offset by the cosine of the elevation angle to account // for the change in density along the azimuth axis in function of elevation const FloatingPoint cos_elevation_angle = std::cos(linearization_point[0]); - offset[1] *= cos_elevation_angle; - return offset.norm(); + const FloatingPoint cos_elevation_angle_sq = + cos_elevation_angle * cos_elevation_angle; + const FloatingPoint offset_x = offset[0]; + const FloatingPoint offset_y = offset[1]; + return (offset_x * offset_x) + cos_elevation_angle_sq * (offset_y * offset_y); } -inline std::array OusterProjector::imageOffsetsToErrorNorms( - const Vector2D& linearization_point, - ProjectorBase::CellToBeamOffsetArray offsets) const { +inline std::array +OusterProjector::imageOffsetsToErrorSquaredNorms( + const ImageCoordinates& linearization_point, + const CellToBeamOffsetArray& offsets) const { const FloatingPoint cos_elevation_angle = std::cos(linearization_point[0]); - for (int offset_idx = 0; offset_idx < 4; ++offset_idx) { - offsets[offset_idx][1] *= cos_elevation_angle; - } + const FloatingPoint cos_elevation_angle_sq = + cos_elevation_angle * cos_elevation_angle; std::array error_norms{}; for (int offset_idx = 0; offset_idx < 4; ++offset_idx) { - error_norms[offset_idx] = offsets[offset_idx].norm(); + const FloatingPoint offset_x = offsets(0, offset_idx); + const FloatingPoint offset_y = offsets(1, offset_idx); + error_norms[offset_idx] = + (offset_x * offset_x) + cos_elevation_angle_sq * (offset_y * offset_y); } return error_norms; } -inline Vector2D OusterProjector::cartesianToImage( +inline ImageCoordinates OusterProjector::cartesianToImage( const Point3D& C_point) const { // Project the beam's endpoint into the 2D plane B whose origin lies at the // beam's start point, X-axis is parallel to the projection of the beam onto // frame C's XY-plane and Y-axis is parallel to frame C's Z-axis - const Vector2D B_point{ + const std::array B_point{ C_point.head<2>().norm() - config_.lidar_origin_to_beam_origin, C_point.z() - config_.lidar_origin_to_sensor_origin_z_offset}; - const FloatingPoint elevation_angle = - approximate::atan2()(B_point.y(), B_point.x()); const FloatingPoint azimuth_angle = - approximate::atan2()(C_point.y(), C_point.x()); + approximate::atan2()(C_point[1], C_point[0]); + const FloatingPoint elevation_angle = + approximate::atan2()(B_point[1], B_point[0]); return {elevation_angle, azimuth_angle}; } @@ -77,18 +85,6 @@ inline FloatingPoint OusterProjector::cartesianToSensorZ( C_point.z() - config_.lidar_origin_to_sensor_origin_z_offset}; return B_point.norm(); } - -inline Vector2D OusterProjector::cartesianToImageApprox( - const Point3D& C_point) const { - const Vector2D B_point{ - C_point.head<2>().norm() - config_.lidar_origin_to_beam_origin, - C_point.z() - config_.lidar_origin_to_sensor_origin_z_offset}; - const FloatingPoint elevation_angle = - approximate::atan2()(B_point.y(), B_point.x()); - const FloatingPoint azimuth_angle = - approximate::atan2()(C_point.y(), C_point.x()); - return {elevation_angle, azimuth_angle}; -} } // namespace wavemap #endif // WAVEMAP_INTEGRATOR_PROJECTION_MODEL_IMPL_OUSTER_PROJECTOR_INL_H_ diff --git a/libraries/wavemap/include/wavemap/integrator/projection_model/impl/pinhole_camera_projector_inl.h b/libraries/wavemap/include/wavemap/integrator/projection_model/impl/pinhole_camera_projector_inl.h index b416232ba..cb436719a 100644 --- a/libraries/wavemap/include/wavemap/integrator/projection_model/impl/pinhole_camera_projector_inl.h +++ b/libraries/wavemap/include/wavemap/integrator/projection_model/impl/pinhole_camera_projector_inl.h @@ -4,42 +4,42 @@ #include namespace wavemap { -inline Vector3D PinholeCameraProjector::cartesianToSensor( +inline SensorCoordinates PinholeCameraProjector::cartesianToSensor( const Point3D& C_point) const { const FloatingPoint w_clamped = std::max(C_point.z(), kEpsilon); const FloatingPoint uw = config_.fx * C_point.x() / w_clamped + config_.cx; const FloatingPoint vw = config_.fy * C_point.y() / w_clamped + config_.cy; - return {uw, vw, C_point.z()}; + return {{uw, vw}, C_point.z()}; } inline Point3D PinholeCameraProjector::sensorToCartesian( - const Vector3D& coordinates) const { - const FloatingPoint u_scaled = coordinates[0]; - const FloatingPoint v_scaled = coordinates[1]; - const FloatingPoint w = coordinates[2]; + const SensorCoordinates& coordinates) const { + const FloatingPoint u_scaled = coordinates.image[0]; + const FloatingPoint v_scaled = coordinates.image[1]; + const FloatingPoint w = coordinates.depth; Point3D C_point = w * fxfy_inv_ * Point3D{config_.fy * u_scaled - cxfy_, config_.fx * v_scaled - cyfx_, fxfy_}; return C_point; } -inline Point3D PinholeCameraProjector::sensorToCartesian( - const Vector2D& image_coordinates, FloatingPoint depth) const { - return sensorToCartesian( - {image_coordinates.x(), image_coordinates.y(), depth}); -} - -inline FloatingPoint PinholeCameraProjector::imageOffsetToErrorNorm( - const Vector2D&, Vector2D offset) const { - return offset.norm(); +inline FloatingPoint PinholeCameraProjector::imageOffsetToErrorSquaredNorm( + const ImageCoordinates& /*linearization_point*/, + const Vector2D& offset) const { + const FloatingPoint offset_x = offset[0]; + const FloatingPoint offset_y = offset[1]; + return (offset_x * offset_x) + (offset_y * offset_y); } inline std::array -PinholeCameraProjector::imageOffsetsToErrorNorms( - const Vector2D&, ProjectorBase::CellToBeamOffsetArray offsets) const { +PinholeCameraProjector::imageOffsetsToErrorSquaredNorms( + const ImageCoordinates& /*linearization_point*/, + const CellToBeamOffsetArray& offsets) const { std::array error_norms{}; for (int offset_idx = 0; offset_idx < 4; ++offset_idx) { - error_norms[offset_idx] = offsets[offset_idx].norm(); + const FloatingPoint offset_x = offsets(0, offset_idx); + const FloatingPoint offset_y = offsets(1, offset_idx); + error_norms[offset_idx] = (offset_x * offset_x) + (offset_y * offset_y); } return error_norms; } diff --git a/libraries/wavemap/include/wavemap/integrator/projection_model/impl/projector_base_inl.h b/libraries/wavemap/include/wavemap/integrator/projection_model/impl/projector_base_inl.h index 83f365747..41c50162a 100644 --- a/libraries/wavemap/include/wavemap/integrator/projection_model/impl/projector_base_inl.h +++ b/libraries/wavemap/include/wavemap/integrator/projection_model/impl/projector_base_inl.h @@ -5,7 +5,7 @@ namespace wavemap { inline Index2D ProjectorBase::imageToNearestIndex( - const Vector2D& image_coordinates) const { + const ImageCoordinates& image_coordinates) const { return imageToIndexReal(image_coordinates) .array() .round() @@ -13,7 +13,7 @@ inline Index2D ProjectorBase::imageToNearestIndex( } inline Index2D ProjectorBase::imageToFloorIndex( - const Vector2D& image_coordinates) const { + const ImageCoordinates& image_coordinates) const { return imageToIndexReal(image_coordinates) .array() .floor() @@ -21,7 +21,7 @@ inline Index2D ProjectorBase::imageToFloorIndex( } inline Index2D ProjectorBase::imageToCeilIndex( - const Vector2D& image_coordinates) const { + const ImageCoordinates& image_coordinates) const { return imageToIndexReal(image_coordinates) .array() .ceil() @@ -29,60 +29,91 @@ inline Index2D ProjectorBase::imageToCeilIndex( } inline std::pair ProjectorBase::imageToNearestIndexAndOffset( - const Vector2D& image_coordinates) const { - const Vector2D index = imageToIndexReal(image_coordinates); - const Vector2D index_rounded = index.array().round(); - const Vector2D image_coordinate_offset = - (index - index_rounded).cwiseProduct(index_to_image_scale_factor_); - return {index_rounded.cast(), image_coordinate_offset}; + const ImageCoordinates& image_coordinates) const { + const Vector2D index_real = imageToIndexReal(image_coordinates); + const Vector2D index_rounded = index_real.array().round(); + Vector2D image_coordinate_offset = + (index_rounded - index_real).cwiseProduct(index_to_image_scale_factor_); + return {index_rounded.cast(), + std::move(image_coordinate_offset)}; } -inline std::array ProjectorBase::imageToNearestIndices( - const Vector2D& image_coordinates) const { - const Vector2D index = imageToIndexReal(image_coordinates); - const Vector2D index_lower = index.array().floor(); - const Vector2D index_upper = index.array().ceil(); - - std::array indices{}; - for (int neighbox_idx = 0; neighbox_idx < 4; ++neighbox_idx) { - const Vector2D index_rounded{ - neighbox_idx & 0b01 ? index_upper[0] : index_lower[0], - neighbox_idx & 0b10 ? index_upper[1] : index_lower[1]}; - indices[neighbox_idx] = index_rounded.cast(); - } +inline ProjectorBase::NearestIndexArray ProjectorBase::imageToNearestIndices( + const ImageCoordinates& image_coordinates) const { + NearestIndexArray indices; + + // Populate the indices for the min and max corners, where + // min_corner = [floor_x, floor_y] and max_corner = [ceil_x, ceil_y] + const Vector2D index_real = imageToIndexReal(image_coordinates); + indices.col(0) = index_real.array().floor().cast(); + indices.col(3) = index_real.array().ceil().cast(); + + // Fill in the intermediate corners, where + // corner_1 = [ceil_x, floor_y] and corner_2 = [floor_x, ceil_y] + indices(0, 1) = indices(0, 3); + indices(1, 1) = indices(1, 0); + indices(0, 2) = indices(0, 0); + indices(1, 2) = indices(1, 3); return indices; } -inline std::pair, std::array> +inline std::pair ProjectorBase::imageToNearestIndicesAndOffsets( - const Vector2D& image_coordinates) const { - const Vector2D index = imageToIndexReal(image_coordinates); - const Vector2D index_lower = index.array().floor(); - const Vector2D index_upper = index.array().ceil(); - - std::array indices{}; - std::array offsets{}; - for (int neighbox_idx = 0; neighbox_idx < 4; ++neighbox_idx) { - const Vector2D index_rounded{ - neighbox_idx & 0b01 ? index_upper[0] : index_lower[0], - neighbox_idx & 0b10 ? index_upper[1] : index_lower[1]}; - indices[neighbox_idx] = index_rounded.cast(); - offsets[neighbox_idx] = - (index - index_rounded).cwiseProduct(index_to_image_scale_factor_); - } + const ImageCoordinates& image_coordinates) const { + std::pair result; + auto& indices = result.first; + auto& offsets = result.second; + + // Write the real-valued indices for the min and max corners into the offsets + // array, where min_corner = [floor_x, floor_y], max_corner = [ceil_x, ceil_y] + const Vector2D index_real = imageToIndexReal(image_coordinates); + offsets.col(0) = index_real.array().floor(); + offsets.col(3) = index_real.array().ceil(); + // Also fill in the intermediate corners, where + // corner_1 = [ceil_x, floor_y] and corner_2 = [floor_x, ceil_y] + offsets(0, 1) = offsets(0, 3); + offsets(1, 1) = offsets(1, 0); + offsets(0, 2) = offsets(0, 0); + offsets(1, 2) = offsets(1, 3); + + // Obtain the indices by casting the real-values into integers + indices = offsets.cast(); - return {indices, offsets}; + // Compute the offsets by taking the difference between the real valued and + // rounded indices and rescaling the result back into image coordinates + offsets = index_to_image_scale_factor_.asDiagonal() * + (offsets.colwise() - index_real); + + return result; } -inline Vector2D ProjectorBase::indexToImage(const Index2D& index) const { +inline ImageCoordinates ProjectorBase::indexToImage( + const Index2D& index) const { return index.cast().cwiseProduct( index_to_image_scale_factor_) + image_offset_; } +inline FloatingPoint ProjectorBase::imageOffsetToErrorNorm( + const ImageCoordinates& linearization_point, const Vector2D& offset) const { + return std::sqrt(imageOffsetToErrorSquaredNorm(linearization_point, offset)); +} + +inline std::array ProjectorBase::imageOffsetsToErrorNorms( + const ImageCoordinates& linearization_point, + const ProjectorBase::CellToBeamOffsetArray& offsets) const { + auto error_norms = + imageOffsetsToErrorSquaredNorms(linearization_point, offsets); + for (auto& error_norm : error_norms) { + error_norm = std::sqrt(error_norm); + } + return error_norms; +} + inline Vector2D ProjectorBase::imageToIndexReal( - const Vector2D& image_coordinates) const { + const ImageCoordinates& image_coordinates) const { return (image_coordinates - image_offset_) .cwiseProduct(image_to_index_scale_factor_); } diff --git a/libraries/wavemap/include/wavemap/integrator/projection_model/impl/spherical_projector_inl.h b/libraries/wavemap/include/wavemap/integrator/projection_model/impl/spherical_projector_inl.h index 64aedfc6f..74d67bb44 100644 --- a/libraries/wavemap/include/wavemap/integrator/projection_model/impl/spherical_projector_inl.h +++ b/libraries/wavemap/include/wavemap/integrator/projection_model/impl/spherical_projector_inl.h @@ -1,50 +1,60 @@ #ifndef WAVEMAP_INTEGRATOR_PROJECTION_MODEL_IMPL_SPHERICAL_PROJECTOR_INL_H_ #define WAVEMAP_INTEGRATOR_PROJECTION_MODEL_IMPL_SPHERICAL_PROJECTOR_INL_H_ +#include + +#include "wavemap/utils/math/approximate_trigonometry.h" + namespace wavemap { -inline Vector3D SphericalProjector::cartesianToSensor( +inline SensorCoordinates SphericalProjector::cartesianToSensor( const Point3D& C_point) const { - const Vector2D image_coordinates = cartesianToImage(C_point); + ImageCoordinates image_coordinates = cartesianToImage(C_point); const FloatingPoint range = C_point.norm(); - return {image_coordinates.x(), image_coordinates.y(), range}; + return {std::move(image_coordinates), range}; } inline Point3D SphericalProjector::sensorToCartesian( - const Vector3D& coordinates) const { - const FloatingPoint elevation_angle = coordinates[0]; - const FloatingPoint azimuth_angle = coordinates[1]; - const FloatingPoint range = coordinates[2]; - const Vector3D bearing{std::cos(elevation_angle) * std::cos(azimuth_angle), - std::cos(elevation_angle) * std::sin(azimuth_angle), + const SensorCoordinates& coordinates) const { + const FloatingPoint elevation_angle = coordinates.image[0]; + const FloatingPoint azimuth_angle = coordinates.image[1]; + const FloatingPoint range = coordinates.depth; + const FloatingPoint cos_elevation_angle = std::cos(elevation_angle); + const Vector3D bearing{cos_elevation_angle * std::cos(azimuth_angle), + cos_elevation_angle * std::sin(azimuth_angle), std::sin(elevation_angle)}; return range * bearing; } -inline FloatingPoint SphericalProjector::imageOffsetToErrorNorm( - const Vector2D& linearization_point, Vector2D offset) const { +inline FloatingPoint SphericalProjector::imageOffsetToErrorSquaredNorm( + const ImageCoordinates& linearization_point, const Vector2D& offset) const { // Scale the azimuth offset by the cosine of the elevation angle to account // for the change in density along the azimuth axis in function of elevation const FloatingPoint cos_elevation_angle = std::cos(linearization_point[0]); - offset[1] *= cos_elevation_angle; - return offset.norm(); + const FloatingPoint cos_elevation_angle_sq = + cos_elevation_angle * cos_elevation_angle; + const FloatingPoint offset_x = offset[0]; + const FloatingPoint offset_y = offset[1]; + return (offset_x * offset_x) + cos_elevation_angle_sq * (offset_y * offset_y); } inline std::array -SphericalProjector::imageOffsetsToErrorNorms( - const Vector2D& linearization_point, - ProjectorBase::CellToBeamOffsetArray offsets) const { +SphericalProjector::imageOffsetsToErrorSquaredNorms( + const ImageCoordinates& linearization_point, + const CellToBeamOffsetArray& offsets) const { const FloatingPoint cos_elevation_angle = std::cos(linearization_point[0]); - for (int offset_idx = 0; offset_idx < 4; ++offset_idx) { - offsets[offset_idx][1] *= cos_elevation_angle; - } + const FloatingPoint cos_elevation_angle_sq = + cos_elevation_angle * cos_elevation_angle; std::array error_norms{}; for (int offset_idx = 0; offset_idx < 4; ++offset_idx) { - error_norms[offset_idx] = offsets[offset_idx].norm(); + const FloatingPoint offset_x = offsets(0, offset_idx); + const FloatingPoint offset_y = offsets(1, offset_idx); + error_norms[offset_idx] = + (offset_x * offset_x) + cos_elevation_angle_sq * (offset_y * offset_y); } return error_norms; } -inline Vector2D SphericalProjector::cartesianToImage( +inline ImageCoordinates SphericalProjector::cartesianToImage( const Point3D& C_point) const { const FloatingPoint elevation_angle = approximate::atan2()(C_point.z(), C_point.head<2>().norm()); @@ -57,15 +67,6 @@ inline FloatingPoint SphericalProjector::cartesianToSensorZ( const Point3D& C_point) const { return C_point.norm(); } - -inline Vector2D SphericalProjector::cartesianToImageApprox( - const Point3D& C_point) const { - const FloatingPoint elevation_angle = - approximate::atan2()(C_point.z(), C_point.head<2>().norm()); - const FloatingPoint azimuth_angle = - approximate::atan2()(C_point.y(), C_point.x()); - return {elevation_angle, azimuth_angle}; -} } // namespace wavemap #endif // WAVEMAP_INTEGRATOR_PROJECTION_MODEL_IMPL_SPHERICAL_PROJECTOR_INL_H_ diff --git a/libraries/wavemap/include/wavemap/integrator/projection_model/ouster_projector.h b/libraries/wavemap/include/wavemap/integrator/projection_model/ouster_projector.h index 0a10df089..f800f6031 100644 --- a/libraries/wavemap/include/wavemap/integrator/projection_model/ouster_projector.h +++ b/libraries/wavemap/include/wavemap/integrator/projection_model/ouster_projector.h @@ -7,7 +7,6 @@ #include "wavemap/config/config_base.h" #include "wavemap/integrator/projection_model/circular_projector.h" #include "wavemap/integrator/projection_model/projector_base.h" -#include "wavemap/utils/approximate_trigonometry.h" namespace wavemap { /** @@ -51,14 +50,6 @@ class OusterProjector : public ProjectorBase { explicit OusterProjector(const Config& config); - IndexElement getNumRows() const final { return config_.elevation.num_cells; } - IndexElement getNumColumns() const final { return config_.azimuth.num_cells; } - Vector2D getMinImageCoordinates() const final { - return {config_.elevation.min_angle, config_.azimuth.min_angle}; - } - Vector2D getMaxImageCoordinates() const final { - return {config_.elevation.max_angle, config_.azimuth.max_angle}; - } Eigen::Matrix sensorAxisIsPeriodic() const final; Eigen::Matrix sensorAxisCouldBePeriodic() const final { return {true, true, false}; @@ -66,21 +57,17 @@ class OusterProjector : public ProjectorBase { SiUnit getImageCoordinatesUnit() const final { return SiUnit::kRadians; } // Coordinate transforms between Cartesian and sensor space - Vector3D cartesianToSensor(const Point3D& C_point) const final; - Point3D sensorToCartesian(const Vector3D& coordinates) const final; - Point3D sensorToCartesian(const Vector2D& image_coordinates, - FloatingPoint range) const final { - return sensorToCartesian( - {image_coordinates.x(), image_coordinates.y(), range}); - } - FloatingPoint imageOffsetToErrorNorm(const Vector2D& linearization_point, - Vector2D offset) const final; - std::array imageOffsetsToErrorNorms( - const Vector2D& linearization_point, - CellToBeamOffsetArray offsets) const final; + SensorCoordinates cartesianToSensor(const Point3D& C_point) const final; + Point3D sensorToCartesian(const SensorCoordinates& coordinates) const final; + FloatingPoint imageOffsetToErrorSquaredNorm( + const ImageCoordinates& linearization_point, + const Vector2D& offset) const final; + std::array imageOffsetsToErrorSquaredNorms( + const ImageCoordinates& linearization_point, + const CellToBeamOffsetArray& offsets) const final; // Projection from Cartesian space onto the sensor's image surface - Vector2D cartesianToImage(const Point3D& C_point) const final; + ImageCoordinates cartesianToImage(const Point3D& C_point) const final; FloatingPoint cartesianToSensorZ(const Point3D& C_point) const final; // NOTE: When the AABB is right behind the sensor, the angle range will wrap @@ -92,8 +79,6 @@ class OusterProjector : public ProjectorBase { private: const OusterProjectorConfig config_; - - Vector2D cartesianToImageApprox(const Point3D& C_point) const; }; } // namespace wavemap diff --git a/libraries/wavemap/include/wavemap/integrator/projection_model/pinhole_camera_projector.h b/libraries/wavemap/include/wavemap/integrator/projection_model/pinhole_camera_projector.h index 93e3c910f..c99a603dc 100644 --- a/libraries/wavemap/include/wavemap/integrator/projection_model/pinhole_camera_projector.h +++ b/libraries/wavemap/include/wavemap/integrator/projection_model/pinhole_camera_projector.h @@ -45,18 +45,8 @@ class PinholeCameraProjector : public ProjectorBase { public: using Config = PinholeCameraProjectorConfig; - explicit PinholeCameraProjector(const Config& config) - : ProjectorBase(Vector2D::Ones(), Vector2D::Zero()), - config_(config.checkValid()) {} - - IndexElement getNumRows() const final { return config_.width; } - IndexElement getNumColumns() const final { return config_.height; } - Vector2D getMinImageCoordinates() const final { - return indexToImage(Index2D::Zero()); - } - Vector2D getMaxImageCoordinates() const final { - return {indexToImage({config_.width - 1, config_.height - 1})}; - } + explicit PinholeCameraProjector(const Config& config); + Eigen::Matrix sensorAxisIsPeriodic() const final { return {false, false, false}; } @@ -66,19 +56,18 @@ class PinholeCameraProjector : public ProjectorBase { SiUnit getImageCoordinatesUnit() const final { return SiUnit::kPixels; } // Coordinate transforms between Cartesian and sensor space - Vector3D cartesianToSensor(const Point3D& C_point) const final; - Point3D sensorToCartesian(const Vector3D& coordinates) const final; - Point3D sensorToCartesian(const Vector2D& image_coordinates, - FloatingPoint depth) const final; - FloatingPoint imageOffsetToErrorNorm(const Vector2D& /*linearization_point*/, - Vector2D offset) const final; - std::array imageOffsetsToErrorNorms( - const Vector2D& /*linearization_point*/, - CellToBeamOffsetArray offsets) const final; + SensorCoordinates cartesianToSensor(const Point3D& C_point) const final; + Point3D sensorToCartesian(const SensorCoordinates& coordinates) const final; + FloatingPoint imageOffsetToErrorSquaredNorm( + const ImageCoordinates& /*linearization_point*/, + const Vector2D& offset) const final; + std::array imageOffsetsToErrorSquaredNorms( + const ImageCoordinates& /*linearization_point*/, + const CellToBeamOffsetArray& offsets) const final; // Projection from Cartesian space onto the sensor's image surface - Vector2D cartesianToImage(const Point3D& C_point) const final { - return cartesianToSensor(C_point).head<2>(); + ImageCoordinates cartesianToImage(const Point3D& C_point) const final { + return cartesianToSensor(C_point).image; } FloatingPoint cartesianToSensorZ(const Point3D& C_point) const final { return C_point.z(); diff --git a/libraries/wavemap/include/wavemap/integrator/projection_model/projector_base.h b/libraries/wavemap/include/wavemap/integrator/projection_model/projector_base.h index 88a6ae907..69a0379d8 100644 --- a/libraries/wavemap/include/wavemap/integrator/projection_model/projector_base.h +++ b/libraries/wavemap/include/wavemap/integrator/projection_model/projector_base.h @@ -28,56 +28,78 @@ class ProjectorBase { using Ptr = std::shared_ptr; using ConstPtr = std::shared_ptr; - ProjectorBase(Vector2D index_to_image_scale_factor, Vector2D image_offset) - : index_to_image_scale_factor_(std::move(index_to_image_scale_factor)), - image_offset_(std::move(image_offset)) {} + ProjectorBase(Index2D dimensions, Vector2D index_to_image_scale_factor, + ImageCoordinates image_offset, + ImageCoordinates min_image_coordinates, + ImageCoordinates max_image_coordinates) + : dimensions_(std::move(dimensions)), + index_to_image_scale_factor_(std::move(index_to_image_scale_factor)), + image_offset_(std::move(image_offset)), + min_image_coordinates_(std::move(min_image_coordinates)), + max_image_coordinates_(std::move(max_image_coordinates)) {} virtual ~ProjectorBase() = default; - virtual IndexElement getNumRows() const = 0; - virtual IndexElement getNumColumns() const = 0; - Index2D getDimensions() const { return {getNumRows(), getNumColumns()}; } - virtual Vector2D getMinImageCoordinates() const = 0; - virtual Vector2D getMaxImageCoordinates() const = 0; + IndexElement getNumRows() const { return dimensions_.x(); } + IndexElement getNumColumns() const { return dimensions_.y(); } + Index2D getDimensions() const { return dimensions_; } + ImageCoordinates getMinImageCoordinates() const { + return min_image_coordinates_; + } + ImageCoordinates getMaxImageCoordinates() const { + return max_image_coordinates_; + } virtual Eigen::Matrix sensorAxisIsPeriodic() const = 0; virtual Eigen::Matrix sensorAxisCouldBePeriodic() const = 0; virtual SiUnit getImageCoordinatesUnit() const = 0; // Coordinate transforms between Cartesian and sensor space - virtual Vector3D cartesianToSensor(const Point3D& C_point) const = 0; - virtual Point3D sensorToCartesian(const Vector3D& coordinates) const = 0; - virtual Point3D sensorToCartesian(const Vector2D& image_coordinates, - FloatingPoint range) const = 0; + virtual SensorCoordinates cartesianToSensor(const Point3D& C_point) const = 0; + virtual Point3D sensorToCartesian( + const SensorCoordinates& coordinates) const = 0; + Point3D sensorToCartesian(const ImageCoordinates& image_coordinates, + FloatingPoint normal) const { + return sensorToCartesian({image_coordinates, normal}); + } // Projection from Cartesian space onto the sensor's image surface - virtual Vector2D cartesianToImage(const Point3D& C_point) const = 0; + virtual ImageCoordinates cartesianToImage(const Point3D& C_point) const = 0; virtual FloatingPoint cartesianToSensorZ(const Point3D& C_point) const = 0; // Conversions between real (unscaled) coordinates on the sensor's image // surface and indices corresponding to sensor pixels/rays - Index2D imageToNearestIndex(const Vector2D& image_coordinates) const; - Index2D imageToFloorIndex(const Vector2D& image_coordinates) const; - Index2D imageToCeilIndex(const Vector2D& image_coordinates) const; - std::array imageToNearestIndices( - const Vector2D& image_coordinates) const; + Index2D imageToNearestIndex(const ImageCoordinates& image_coordinates) const; + Index2D imageToFloorIndex(const ImageCoordinates& image_coordinates) const; + Index2D imageToCeilIndex(const ImageCoordinates& image_coordinates) const; + using NearestIndexArray = Eigen::Matrix; + NearestIndexArray imageToNearestIndices( + const ImageCoordinates& image_coordinates) const; std::pair imageToNearestIndexAndOffset( - const Vector2D& image_coordinates) const; - std::pair, std::array> - imageToNearestIndicesAndOffsets(const Vector2D& image_coordinates) const; + const ImageCoordinates& image_coordinates) const; + using CellToBeamOffsetArray = Eigen::Matrix; + std::pair + imageToNearestIndicesAndOffsets( + const ImageCoordinates& image_coordinates) const; - Vector2D indexToImage(const Index2D& index) const; + ImageCoordinates indexToImage(const Index2D& index) const; // Compute the error norm in the image plane based on an offset vector (in // image plane) and a linearization point // NOTE: For spherical models, this error norm corresponds to the relative // angle between the two rays whose offset is given. For camera models, // it corresponds to the reprojection error in pixels. - virtual FloatingPoint imageOffsetToErrorNorm( - const Vector2D& linearization_point, Vector2D offset) const = 0; - using CellToBeamOffsetArray = std::array; - virtual std::array imageOffsetsToErrorNorms( - const Vector2D& linearization_point, - CellToBeamOffsetArray offsets) const = 0; + FloatingPoint imageOffsetToErrorNorm( + const ImageCoordinates& linearization_point, + const Vector2D& offset) const; + virtual FloatingPoint imageOffsetToErrorSquaredNorm( + const ImageCoordinates& linearization_point, + const Vector2D& offset) const = 0; + std::array imageOffsetsToErrorNorms( + const ImageCoordinates& linearization_point, + const CellToBeamOffsetArray& offsets) const; + virtual std::array imageOffsetsToErrorSquaredNorms( + const ImageCoordinates& linearization_point, + const CellToBeamOffsetArray& offsets) const = 0; // Convenience functions combining multiple of the above methods Index2D cartesianToNearestIndex(const Point3D& C_point) const { @@ -90,12 +112,21 @@ class ProjectorBase { const Point3D& t_W_C) const = 0; protected: + const Index2D dimensions_; const Vector2D index_to_image_scale_factor_; const Vector2D image_to_index_scale_factor_ = Vector2D::Ones().cwiseQuotient(index_to_image_scale_factor_); - const Vector2D image_offset_; + const ImageCoordinates image_offset_; + + const ImageCoordinates min_image_coordinates_; + const ImageCoordinates max_image_coordinates_; + + Vector2D imageToIndexReal(const ImageCoordinates& image_coordinates) const; - Vector2D imageToIndexReal(const Vector2D& image_coordinates) const; + // Give test 'imageToNearestIndicesAndOffsets' of gtest test suite + // 'Image2DProjectorTypedTest' access to private methods and members + template + friend class Image2DProjectorTypedTest_imageToNearestIndicesAndOffsets_Test; }; } // namespace wavemap diff --git a/libraries/wavemap/include/wavemap/integrator/projection_model/spherical_projector.h b/libraries/wavemap/include/wavemap/integrator/projection_model/spherical_projector.h index 5cb5a1470..30099662c 100644 --- a/libraries/wavemap/include/wavemap/integrator/projection_model/spherical_projector.h +++ b/libraries/wavemap/include/wavemap/integrator/projection_model/spherical_projector.h @@ -7,7 +7,6 @@ #include "wavemap/config/config_base.h" #include "wavemap/integrator/projection_model/circular_projector.h" #include "wavemap/integrator/projection_model/projector_base.h" -#include "wavemap/utils/approximate_trigonometry.h" namespace wavemap { /** @@ -37,14 +36,6 @@ class SphericalProjector : public ProjectorBase { explicit SphericalProjector(const Config& config); - IndexElement getNumRows() const final { return config_.elevation.num_cells; } - IndexElement getNumColumns() const final { return config_.azimuth.num_cells; } - Vector2D getMinImageCoordinates() const final { - return {config_.elevation.min_angle, config_.azimuth.min_angle}; - } - Vector2D getMaxImageCoordinates() const final { - return {config_.elevation.max_angle, config_.azimuth.max_angle}; - } Eigen::Matrix sensorAxisIsPeriodic() const final; Eigen::Matrix sensorAxisCouldBePeriodic() const final { return {true, true, false}; @@ -52,21 +43,17 @@ class SphericalProjector : public ProjectorBase { SiUnit getImageCoordinatesUnit() const final { return SiUnit::kRadians; } // Coordinate transforms between Cartesian and sensor space - Vector3D cartesianToSensor(const Point3D& C_point) const final; - Point3D sensorToCartesian(const Vector3D& coordinates) const final; - Point3D sensorToCartesian(const Vector2D& image_coordinates, - FloatingPoint range) const final { - return sensorToCartesian( - {image_coordinates.x(), image_coordinates.y(), range}); - } - FloatingPoint imageOffsetToErrorNorm(const Vector2D& linearization_point, - Vector2D offset) const final; - std::array imageOffsetsToErrorNorms( - const Vector2D& linearization_point, - CellToBeamOffsetArray offsets) const final; + SensorCoordinates cartesianToSensor(const Point3D& C_point) const final; + Point3D sensorToCartesian(const SensorCoordinates& coordinates) const final; + FloatingPoint imageOffsetToErrorSquaredNorm( + const ImageCoordinates& linearization_point, + const Vector2D& offset) const final; + std::array imageOffsetsToErrorSquaredNorms( + const ImageCoordinates& linearization_point, + const CellToBeamOffsetArray& offsets) const final; // Projection from Cartesian space onto the sensor's image surface - Vector2D cartesianToImage(const Point3D& C_point) const final; + ImageCoordinates cartesianToImage(const Point3D& C_point) const final; FloatingPoint cartesianToSensorZ(const Point3D& C_point) const final; // NOTE: When the AABB is right behind the sensor, the angle range will wrap @@ -78,8 +65,6 @@ class SphericalProjector : public ProjectorBase { private: const Config config_; - - Vector2D cartesianToImageApprox(const Point3D& C_point) const; }; } // namespace wavemap diff --git a/libraries/wavemap/include/wavemap/integrator/projective/coarse_to_fine/hashed_chunked_wavelet_integrator.h b/libraries/wavemap/include/wavemap/integrator/projective/coarse_to_fine/hashed_chunked_wavelet_integrator.h index e53ee900a..ed5e92044 100644 --- a/libraries/wavemap/include/wavemap/integrator/projective/coarse_to_fine/hashed_chunked_wavelet_integrator.h +++ b/libraries/wavemap/include/wavemap/integrator/projective/coarse_to_fine/hashed_chunked_wavelet_integrator.h @@ -20,17 +20,20 @@ class HashedChunkedWaveletIntegrator : public ProjectiveIntegrator { PosedImage<>::Ptr posed_range_image, Image::Ptr beam_offset_image, MeasurementModelBase::ConstPtr measurement_model, - HashedChunkedWaveletOctree::Ptr occupancy_map) + HashedChunkedWaveletOctree::Ptr occupancy_map, + std::shared_ptr thread_pool = nullptr) : ProjectiveIntegrator( config, std::move(projection_model), std::move(posed_range_image), std::move(beam_offset_image), std::move(measurement_model)), - occupancy_map_(std::move(CHECK_NOTNULL(occupancy_map))) {} + occupancy_map_(std::move(CHECK_NOTNULL(occupancy_map))), + thread_pool_(thread_pool ? std::move(thread_pool) + : std::make_shared()) {} private: using BlockList = std::vector; const HashedChunkedWaveletOctree::Ptr occupancy_map_; - ThreadPool thread_pool_; + std::shared_ptr thread_pool_; std::shared_ptr range_image_intersector_; // Cache/pre-computed commonly used values @@ -55,6 +58,16 @@ class HashedChunkedWaveletIntegrator : public ProjectiveIntegrator { void updateMap() override; void updateBlock(HashedChunkedWaveletOctree::Block& block, const HashedChunkedWaveletOctree::BlockIndex& block_index); + + void updateNodeRecursive( + HashedChunkedWaveletOctreeBlock::NodeChunkType& parent_chunk, + const OctreeIndex& parent_node_index, LinearIndex parent_in_chunk_index, + FloatingPoint& parent_value, + HashedChunkedWaveletOctreeBlock::NodeChunkType::BitRef parent_has_child, + bool& block_needs_thresholding); + void updateLeavesBatch( + const OctreeIndex& parent_index, FloatingPoint& parent_value, + HashedChunkedWaveletOctreeBlock::NodeChunkType::DataType& parent_details); }; } // namespace wavemap diff --git a/libraries/wavemap/include/wavemap/integrator/projective/coarse_to_fine/hashed_wavelet_integrator.h b/libraries/wavemap/include/wavemap/integrator/projective/coarse_to_fine/hashed_wavelet_integrator.h index 4c28cc7bc..597e8c091 100644 --- a/libraries/wavemap/include/wavemap/integrator/projective/coarse_to_fine/hashed_wavelet_integrator.h +++ b/libraries/wavemap/include/wavemap/integrator/projective/coarse_to_fine/hashed_wavelet_integrator.h @@ -19,17 +19,20 @@ class HashedWaveletIntegrator : public ProjectiveIntegrator { PosedImage<>::Ptr posed_range_image, Image::Ptr beam_offset_image, MeasurementModelBase::ConstPtr measurement_model, - HashedWaveletOctree::Ptr occupancy_map) + HashedWaveletOctree::Ptr occupancy_map, + std::shared_ptr thread_pool = nullptr) : ProjectiveIntegrator( config, std::move(projection_model), std::move(posed_range_image), std::move(beam_offset_image), std::move(measurement_model)), - occupancy_map_(std::move(CHECK_NOTNULL(occupancy_map))) {} + occupancy_map_(std::move(CHECK_NOTNULL(occupancy_map))), + thread_pool_(thread_pool ? std::move(thread_pool) + : std::make_shared()) {} private: const HashedWaveletOctree::Ptr occupancy_map_; const FloatingPoint min_cell_width_ = occupancy_map_->getMinCellWidth(); const FloatingPoint min_cell_width_inv_ = 1.f / min_cell_width_; - ThreadPool thread_pool_; + std::shared_ptr thread_pool_; static constexpr FloatingPoint kNoiseThreshold = 1e-4f; const FloatingPoint min_log_odds_ = occupancy_map_->getMinLogOdds(); diff --git a/libraries/wavemap/include/wavemap/integrator/projective/coarse_to_fine/hierarchical_range_sets.h b/libraries/wavemap/include/wavemap/integrator/projective/coarse_to_fine/hierarchical_range_sets.h deleted file mode 100644 index 52b271143..000000000 --- a/libraries/wavemap/include/wavemap/integrator/projective/coarse_to_fine/hierarchical_range_sets.h +++ /dev/null @@ -1,69 +0,0 @@ -#ifndef WAVEMAP_INTEGRATOR_PROJECTIVE_COARSE_TO_FINE_HIERARCHICAL_RANGE_SETS_H_ -#define WAVEMAP_INTEGRATOR_PROJECTIVE_COARSE_TO_FINE_HIERARCHICAL_RANGE_SETS_H_ - -#include -#include -#include -#include - -#include "wavemap/data_structure/image.h" -#include "wavemap/integrator/projective/update_type.h" - -namespace wavemap { -template -class HierarchicalRangeSets { - public: - explicit HierarchicalRangeSets(std::shared_ptr range_image) - : range_image_(std::move(range_image)), - levels_(computeReducedLevels(*range_image_)), - max_height_(static_cast(levels_.size())) { - DCHECK_EQ(levels_.size(), max_height_); - } - - NdtreeIndexElement getMaxHeight() const { return max_height_; } - static FloatingPoint getRangeMin() { return kMinRepresentableRange; } - - UpdateType getUpdateType(const Index2D& bottom_left_image_idx, - const Index2D& top_right_image_idx, - FloatingPoint range_min, - FloatingPoint range_max) const; - - private: - using RangeCellIdx = uint8_t; - using RangeCellSet = std::vector; - using RangeCellSetImage = - Eigen::Matrix; - - const std::shared_ptr range_image_; - - std::vector levels_; - const NdtreeIndexElement max_height_; - static constexpr std::tuple scale_ = {2, 1}; - - static constexpr FloatingPoint kMinRepresentableRange = 0.2f; - static constexpr FloatingPoint kRangeResolutionAt1m = 0.03f; - static constexpr RangeCellIdx rangeToRangeCellIdx(FloatingPoint range); - static constexpr RangeCellIdx rangeToRangeCellIdxClamped(FloatingPoint range); - static constexpr FloatingPoint rangeCellIdxToRange( - RangeCellIdx range_cell_idx); - static constexpr FloatingPoint kMaxRepresentableRange = 100.f; - static FloatingPoint valueOrInit(FloatingPoint value, FloatingPoint init) { - if (value < kMinRepresentableRange) { - return init; - } - return value; - } - - static std::vector computeReducedLevels( - const RangeImage2D& range_image); - - UpdateType getUpdateType( - RangeCellIdx range_cell_min_idx, RangeCellIdx range_cell_max_idx, - std::initializer_list> - range_cell_sets) const; -}; -} // namespace wavemap - -#include "wavemap/integrator/projective/coarse_to_fine/impl/hierarchical_range_sets_inl.h" - -#endif // WAVEMAP_INTEGRATOR_PROJECTIVE_COARSE_TO_FINE_HIERARCHICAL_RANGE_SETS_H_ diff --git a/libraries/wavemap/include/wavemap/integrator/projective/coarse_to_fine/impl/hashed_chunked_wavelet_integrator_inl.h b/libraries/wavemap/include/wavemap/integrator/projective/coarse_to_fine/impl/hashed_chunked_wavelet_integrator_inl.h index 1f3c283d7..439528ed4 100644 --- a/libraries/wavemap/include/wavemap/integrator/projective/coarse_to_fine/impl/hashed_chunked_wavelet_integrator_inl.h +++ b/libraries/wavemap/include/wavemap/integrator/projective/coarse_to_fine/impl/hashed_chunked_wavelet_integrator_inl.h @@ -36,6 +36,50 @@ inline void HashedChunkedWaveletIntegrator::recursiveTester( // NOLINT recursiveTester(child_index, update_job_list); } } + +inline void HashedChunkedWaveletIntegrator::updateLeavesBatch( + const OctreeIndex& parent_index, FloatingPoint& parent_value, + HaarCoefficients::Details& parent_details) { + // Decompress + auto child_values = HashedChunkedWaveletOctreeBlock::Transform::backward( + {parent_value, parent_details}); + + // Get child center points in world frame W + Eigen::Matrix child_centers; + for (int child_idx = 0; child_idx < OctreeIndex::kNumChildren; ++child_idx) { + const auto child_index = parent_index.computeChildIndex(child_idx); + child_centers.col(child_idx) = + convert::nodeIndexToCenterPoint(child_index, min_cell_width_); + } + + // Transform into sensor frame C + const auto& T_C_W = posed_range_image_->getPoseInverse(); + for (int child_idx = 0; child_idx < OctreeIndex::kNumChildren; ++child_idx) { + child_centers.col(child_idx) = T_C_W * child_centers.col(child_idx); + } + + // Compute updated values + for (int child_idx = 0; child_idx < OctreeIndex::kNumChildren; ++child_idx) { + const FloatingPoint sample = computeUpdate(child_centers.col(child_idx)); + FloatingPoint& child_value = child_values[child_idx]; + child_value = sample + child_value; + } + + // Threshold + for (int child_idx = 0; child_idx < OctreeIndex::kNumChildren; ++child_idx) { + FloatingPoint& child_value = child_values[child_idx]; + child_value = (child_value < min_log_odds_padded_) ? min_log_odds_padded_ + : child_value; + child_value = (max_log_odds_padded_ < child_value) ? max_log_odds_padded_ + : child_value; + } + + // Compress + const auto [new_value, new_details] = + HashedChunkedWaveletOctreeBlock::Transform::forward(child_values); + parent_details = new_details; + parent_value = new_value; +} } // namespace wavemap #endif // WAVEMAP_INTEGRATOR_PROJECTIVE_COARSE_TO_FINE_IMPL_HASHED_CHUNKED_WAVELET_INTEGRATOR_INL_H_ diff --git a/libraries/wavemap/include/wavemap/integrator/projective/coarse_to_fine/impl/hierarchical_range_bounds_inl.h b/libraries/wavemap/include/wavemap/integrator/projective/coarse_to_fine/impl/hierarchical_range_bounds_inl.h index 222dc51d0..bb2c31f6a 100644 --- a/libraries/wavemap/include/wavemap/integrator/projective/coarse_to_fine/impl/hierarchical_range_bounds_inl.h +++ b/libraries/wavemap/include/wavemap/integrator/projective/coarse_to_fine/impl/hierarchical_range_bounds_inl.h @@ -4,8 +4,8 @@ #include #include -#include "wavemap/iterator/grid_iterator.h" -#include "wavemap/utils/bit_manipulation.h" +#include "wavemap/utils/bits/bit_operations.h" +#include "wavemap/utils/iterate/grid_iterator.h" namespace wavemap { inline Bounds HierarchicalRangeBounds::getBounds( @@ -293,7 +293,7 @@ inline Index2D HierarchicalRangeBounds::computeImageToPyramidScaleFactor( template std::vector> HierarchicalRangeBounds::computeReducedPyramid( const Image& range_image, BinaryFunctor reduction_functor, T init) { - CHECK(!azimuth_wraps_pi_ || bit_manip::popcount(range_image.getNumColumns())) + CHECK(!azimuth_wraps_pi_ || bit_ops::popcount(range_image.getNumColumns())) << "For LiDAR range images that wrap around horizontally (FoV of " "360deg), only column numbers that are exact powers of 2 are " "currently supported."; diff --git a/libraries/wavemap/include/wavemap/integrator/projective/coarse_to_fine/impl/hierarchical_range_sets_inl.h b/libraries/wavemap/include/wavemap/integrator/projective/coarse_to_fine/impl/hierarchical_range_sets_inl.h deleted file mode 100644 index 73f4086aa..000000000 --- a/libraries/wavemap/include/wavemap/integrator/projective/coarse_to_fine/impl/hierarchical_range_sets_inl.h +++ /dev/null @@ -1,324 +0,0 @@ -#ifndef WAVEMAP_INTEGRATOR_PROJECTIVE_COARSE_TO_FINE_IMPL_HIERARCHICAL_RANGE_SETS_INL_H_ -#define WAVEMAP_INTEGRATOR_PROJECTIVE_COARSE_TO_FINE_IMPL_HIERARCHICAL_RANGE_SETS_INL_H_ - -#include -#include -#include - -#include "wavemap/iterator/grid_iterator.h" -#include "wavemap/utils/bit_manipulation.h" - -namespace wavemap { -template -UpdateType HierarchicalRangeSets::getUpdateType( - const Index2D& bottom_left_image_idx, const Index2D& top_right_image_idx, - FloatingPoint range_min, FloatingPoint range_max) const { - if (bottom_left_image_idx == top_right_image_idx) { - const auto range = range_image_->getRange(bottom_left_image_idx); - if (valueOrInit(range, 0.f) < range_min) { - return UpdateType::kFullyUnknown; - } else if (range_max < valueOrInit(range, 1e3f)) { - return UpdateType::kFreeOrUnknown; - } else { - return UpdateType::kPossiblyOccupied; - } - } - - DCHECK_LE(range_min, range_max); - if (kMaxRepresentableRange < range_min) { - return UpdateType::kFullyUnknown; - } - - Index2D top_right_image_idx_unwrapped = top_right_image_idx; - if (azimuth_wraps_pi && top_right_image_idx.y() < bottom_left_image_idx.y()) { - top_right_image_idx_unwrapped.y() += range_image_->getNumColumns(); - } - DCHECK( - (bottom_left_image_idx.array() <= top_right_image_idx_unwrapped.array()) - .all()); - - const Index2D bottom_left_image_idx_scaled = { - std::get<0>(scale_) * bottom_left_image_idx.x(), - std::get<1>(scale_) * bottom_left_image_idx.y()}; - const Index2D top_right_image_idx_scaled = { - std::get<0>(scale_) * top_right_image_idx.x(), - std::get<1>(scale_) * top_right_image_idx.y()}; - const Index2D top_right_image_idx_unwrapped_scaled = { - std::get<0>(scale_) * top_right_image_idx_unwrapped.x(), - std::get<1>(scale_) * top_right_image_idx_unwrapped.y()}; - - const IndexElement max_idx_diff = - (top_right_image_idx_unwrapped_scaled - bottom_left_image_idx_scaled) - .maxCoeff(); - const IndexElement min_level_up = - max_idx_diff == 0 ? 0 : int_math::log2_floor(max_idx_diff); - - // Compute the node indices at the minimum level we have to go up to fully - // cover the interval with 4 nodes or less - const Index2D bottom_left_child_idx = - int_math::div_exp2_floor(bottom_left_image_idx_scaled, min_level_up); - const Index2D top_right_child_idx = - int_math::div_exp2_floor(top_right_image_idx_scaled, min_level_up); - const Index2D top_right_child_idx_unwrapped = int_math::div_exp2_floor( - top_right_image_idx_unwrapped_scaled, min_level_up); - const RangeCellIdx range_min_child_idx = int_math::div_exp2_floor( - rangeToRangeCellIdxClamped(range_min), min_level_up); - const RangeCellIdx range_max_child_idx = int_math::div_exp2_floor( - rangeToRangeCellIdxClamped(range_max), min_level_up); - const IndexElement child_height_idx = min_level_up - 1; - - // Compute the node indices at the maximum level we have to go up to fully - // cover the interval with 4 nodes or less - const Index2D& bottom_left_parent_idx = - int_math::div_exp2_floor(bottom_left_child_idx, 1); - const Index2D top_right_parent_idx = - int_math::div_exp2_floor(top_right_child_idx, 1); - const Index2D top_right_parent_idx_unwrapped = - int_math::div_exp2_floor(top_right_child_idx_unwrapped, 1); - const RangeCellIdx range_min_parent_idx = - int_math::div_exp2_floor(range_min_child_idx, 1); - const RangeCellIdx range_max_parent_idx = - int_math::div_exp2_floor(range_max_child_idx, 1); - const IndexElement parent_height_idx = min_level_up; - DCHECK_LT(parent_height_idx, max_height_); - - // Check if the nodes at min_level_up are direct neighbors - if (bottom_left_child_idx + Index2D::Ones() == - top_right_child_idx_unwrapped) { - // Check if they even share the same parent (node at min_level_up + 1) - if (bottom_left_parent_idx == top_right_parent_idx_unwrapped) { - // Since they do, checking the nodes at min_level_up is equivalent to - // checking their common parent, so we do that instead as its cheaper - return getUpdateType( - range_min_parent_idx, range_max_parent_idx, - {levels_[parent_height_idx](bottom_left_parent_idx.x(), - bottom_left_parent_idx.y())}); - } else if (bottom_left_parent_idx.x() == - top_right_parent_idx_unwrapped.x() || - bottom_left_parent_idx.y() == - top_right_parent_idx_unwrapped.y()) { - return getUpdateType( - range_min_parent_idx, range_max_parent_idx, - {levels_[parent_height_idx](bottom_left_parent_idx.x(), - bottom_left_parent_idx.y()), - levels_[parent_height_idx](top_right_parent_idx.x(), - top_right_parent_idx.y())}); - } else { - // Check all four nodes at min_level_up - if (min_level_up == 0) { - const auto image_values = { - range_image_->getRange(bottom_left_image_idx), - range_image_->getRange( - {bottom_left_image_idx.x(), top_right_image_idx.y()}), - range_image_->getRange( - {top_right_image_idx.x(), bottom_left_image_idx.y()}), - range_image_->getRange(top_right_image_idx)}; - if (std::all_of(image_values.begin(), image_values.end(), - [range_min](auto range) { - return valueOrInit(range, 0.f) < range_min; - })) { - return UpdateType::kFullyUnknown; - } else if (std::all_of(image_values.begin(), image_values.end(), - [range_max](auto range) { - return range_max < valueOrInit(range, 1e3f); - })) { - return UpdateType::kFreeOrUnknown; - } else { - return UpdateType::kPossiblyOccupied; - } - } else { - return getUpdateType( - range_min_child_idx, range_max_child_idx, - {levels_[child_height_idx](bottom_left_child_idx.x(), - bottom_left_child_idx.y()), - levels_[child_height_idx](bottom_left_child_idx.x(), - top_right_child_idx.y()), - levels_[child_height_idx](top_right_child_idx.x(), - bottom_left_child_idx.y()), - levels_[child_height_idx](top_right_child_idx.x(), - top_right_child_idx.y())}); - } - } - } else { - // Since the nodes at min_level_up are not direct neighbors we need to go - // one level up and check all four parents there - return getUpdateType( - range_min_parent_idx, range_max_parent_idx, - {levels_[parent_height_idx](bottom_left_parent_idx.x(), - bottom_left_parent_idx.y()), - levels_[parent_height_idx](bottom_left_parent_idx.x(), - top_right_parent_idx.y()), - levels_[parent_height_idx](top_right_parent_idx.x(), - bottom_left_parent_idx.y()), - levels_[parent_height_idx](top_right_parent_idx.x(), - top_right_parent_idx.y())}); - } -} - -template -std::vector::RangeCellSetImage> -HierarchicalRangeSets::computeReducedLevels( - const RangeImage2D& range_image) { - CHECK(!azimuth_wraps_pi || bit_manip::popcount(range_image.getNumColumns())) - << "For LiDAR range images that wrap around horizontally (FoV of " - "360deg), only column numbers that are exact powers of 2 are " - "currently supported."; - - const Index2D range_image_dims = range_image.getDimensions(); - const Index2D range_image_dims_scaled = { - std::get<0>(scale_) * range_image_dims.x(), - std::get<1>(scale_) * range_image_dims.y()}; - const int max_num_halvings = - int_math::log2_ceil(range_image_dims_scaled.maxCoeff()); - - // Reduce the range image max_num_halvings times - std::vector levels; - levels.reserve(max_num_halvings); - for (int level_idx = 0; level_idx < max_num_halvings; ++level_idx) { - // Initialize the current level - const Index2D level_dims = - int_math::div_exp2_ceil(range_image_dims_scaled, level_idx + 1); - RangeCellSetImage& current_level = - levels.template emplace_back(level_dims.x(), level_dims.y()); - const Index2D previous_level_dims = - level_idx == 0 - ? range_image_dims_scaled - : int_math::div_exp2_ceil(range_image_dims_scaled, level_idx); - - // Iterate over all azimuth and elevation bins - std::vector reduced_child_range_indices; - for (const Index2D& idx : - Grid<2>(Index2D::Zero(), level_dims - Index2D::Ones())) { - reduced_child_range_indices.clear(); - - const Index2D min_child_idx = 2 * idx; - // Iterate over all children of the current bin - for (int relative_child_idx = 0; relative_child_idx < 4; - ++relative_child_idx) { - Index2D child_idx = { - min_child_idx.x() + (relative_child_idx & 0b1), - min_child_idx.y() + (relative_child_idx >> 1 & 0b1)}; - // Skip child indices if they're out-of-bounds - if (previous_level_dims.x() <= child_idx.x() || - previous_level_dims.y() < child_idx.y()) { - continue; - } else if (child_idx.y() == previous_level_dims.y()) { - if (azimuth_wraps_pi) { - child_idx.y() = 0; - } else { - continue; - } - } - - if (level_idx == 0) { - // At the first level, we gather the range indices from the range - // img - const Index2D child_idx_rescaled = { - child_idx.x() / std::get<0>(scale_), - child_idx.y() / std::get<1>(scale_)}; - const FloatingPoint child_range = - range_image.getRange(child_idx_rescaled); - if (kMinRepresentableRange < child_range && - child_range < kMaxRepresentableRange) { - const RangeCellIdx reduced_child_range_idx = - rangeToRangeCellIdxClamped(child_range) >> 1; - reduced_child_range_indices.template emplace_back( - reduced_child_range_idx); - } - } else { - // Otherwise we gather them from the previous level - const RangeCellSetImage& previous_level = levels[level_idx - 1]; - const RangeCellSet& child_range_indices = - previous_level(child_idx.x(), child_idx.y()); - // Copy and reduce the child range indices - std::transform(child_range_indices.cbegin(), - child_range_indices.cend(), - std::back_inserter(reduced_child_range_indices), - [](auto range_idx) { return range_idx >> 1; }); - } - } - - // Insert the reduced child range cell indices into the current cell, - // in sorted order and without duplicates - RangeCellSet& current_cell = current_level(idx.x(), idx.y()); - std::sort(reduced_child_range_indices.begin(), - reduced_child_range_indices.end()); - std::unique_copy(reduced_child_range_indices.cbegin(), - reduced_child_range_indices.cend(), - std::back_inserter(current_cell)); - current_cell.shrink_to_fit(); - } - } - - return levels; -} - -template -constexpr typename HierarchicalRangeSets::RangeCellIdx -HierarchicalRangeSets::rangeToRangeCellIdx( - FloatingPoint range) { - constexpr FloatingPoint kRangeResAt1mInv = 1.f / kRangeResolutionAt1m; - constexpr FloatingPoint kRangeMinInv = 1.f / kMinRepresentableRange; - return static_cast(kRangeResAt1mInv * - std::log(kRangeMinInv * range)); -} - -template -constexpr typename HierarchicalRangeSets::RangeCellIdx -HierarchicalRangeSets::rangeToRangeCellIdxClamped( - FloatingPoint range) { - if (range <= kMinRepresentableRange) { - return 0.f; - } else if (kMaxRepresentableRange <= range) { - return std::numeric_limits::max(); - } - return static_cast(rangeToRangeCellIdx(range)); -} - -template -constexpr FloatingPoint -HierarchicalRangeSets::rangeCellIdxToRange( - HierarchicalRangeSets::RangeCellIdx range_cell_idx) { - return kMinRepresentableRange * - std::exp(kRangeResolutionAt1m * - static_cast(range_cell_idx)); -} - -template -UpdateType HierarchicalRangeSets::getUpdateType( - HierarchicalRangeSets::RangeCellIdx range_cell_min_idx, - HierarchicalRangeSets::RangeCellIdx range_cell_max_idx, - std::initializer_list> - range_cell_sets) const { - if (std::all_of(range_cell_sets.begin(), range_cell_sets.end(), - [range_cell_min_idx](const RangeCellSet& range_cell_set) { - return range_cell_set.empty() || - range_cell_set.back() < range_cell_min_idx; - })) { - return UpdateType::kFullyUnknown; - } - - if (std::all_of(range_cell_sets.begin(), range_cell_sets.end(), - [range_cell_max_idx](const RangeCellSet& range_cell_set) { - return range_cell_set.empty() || - range_cell_max_idx < range_cell_set.front(); - })) { - return UpdateType::kFreeOrUnknown; - } - - for (const RangeCellSet& range_cell_set : range_cell_sets) { - for (auto range_cell_idx : range_cell_set) { - if (range_cell_min_idx <= range_cell_idx && - range_cell_idx <= range_cell_max_idx) { - return UpdateType::kPossiblyOccupied; - } else if (range_cell_max_idx < range_cell_idx) { - break; - } - } - } - - return UpdateType::kFreeOrUnknown; -} -} // namespace wavemap - -#endif // WAVEMAP_INTEGRATOR_PROJECTIVE_COARSE_TO_FINE_IMPL_HIERARCHICAL_RANGE_SETS_INL_H_ diff --git a/libraries/wavemap/include/wavemap/integrator/projective/coarse_to_fine/impl/range_image_intersector_inl.h b/libraries/wavemap/include/wavemap/integrator/projective/coarse_to_fine/impl/range_image_intersector_inl.h index 43c4ff14d..2bc330922 100644 --- a/libraries/wavemap/include/wavemap/integrator/projective/coarse_to_fine/impl/range_image_intersector_inl.h +++ b/libraries/wavemap/include/wavemap/integrator/projective/coarse_to_fine/impl/range_image_intersector_inl.h @@ -6,8 +6,6 @@ #include #include "wavemap/integrator/measurement_model/continuous_beam.h" -#include "wavemap/utils/angle_utils.h" -#include "wavemap/utils/approximate_trigonometry.h" namespace wavemap { inline UpdateType RangeImageIntersector::determineUpdateType( diff --git a/libraries/wavemap/include/wavemap/integrator/projective/impl/projective_integrator_inl.h b/libraries/wavemap/include/wavemap/integrator/projective/impl/projective_integrator_inl.h index 802a673c3..d366bf9d4 100644 --- a/libraries/wavemap/include/wavemap/integrator/projective/impl/projective_integrator_inl.h +++ b/libraries/wavemap/include/wavemap/integrator/projective/impl/projective_integrator_inl.h @@ -4,15 +4,15 @@ namespace wavemap { inline FloatingPoint ProjectiveIntegrator::computeUpdate( const Point3D& C_cell_center) const { - const Vector3D sensor_coordinates = + const auto sensor_coordinates = projection_model_->cartesianToSensor(C_cell_center); // Check if we're outside the min/max range // NOTE: For spherical (e.g. LiDAR) projection models, sensor_coordinates.z() // corresponds to the range, whereas for camera models it corresponds to // the depth. - if (sensor_coordinates.z() < config_.min_range || - config_.max_range < sensor_coordinates.z()) { + if (sensor_coordinates.depth < config_.min_range || + config_.max_range < sensor_coordinates.depth) { return 0.f; } diff --git a/libraries/wavemap/include/wavemap/integrator/projective/update_type.h b/libraries/wavemap/include/wavemap/integrator/projective/update_type.h index 66fafec87..945012ca5 100644 --- a/libraries/wavemap/include/wavemap/integrator/projective/update_type.h +++ b/libraries/wavemap/include/wavemap/integrator/projective/update_type.h @@ -4,22 +4,23 @@ #include #include "wavemap/common.h" -#include "wavemap/utils/type_utils.h" +#include "wavemap/config/type_selector.h" namespace wavemap { -enum class UpdateType : int { - kFullyUnobserved, - kFullyFree, - kFreeOrUnobserved, - kPossiblyOccupied -}; +struct UpdateType : TypeSelector { + using TypeSelector::TypeSelector; + + enum Id : TypeId { + kFullyUnobserved, + kFullyFree, + kFreeOrUnobserved, + kPossiblyOccupied + }; -inline std::string getUpdateTypeStr(UpdateType update_type) { - static constexpr std::array kUpdateTypeStrs{"fully_unobserved", "fully_free", - "free_or_unobserved", - "possibly_occupied"}; - return kUpdateTypeStrs[to_underlying(update_type)]; -} + static constexpr std::array names = {"fully_unobserved", "fully_free", + "free_or_unobserved", + "possibly_occupied"}; +}; } // namespace wavemap #endif // WAVEMAP_INTEGRATOR_PROJECTIVE_UPDATE_TYPE_H_ diff --git a/libraries/wavemap/include/wavemap/integrator/ray_tracing/ray_tracing_integrator.h b/libraries/wavemap/include/wavemap/integrator/ray_tracing/ray_tracing_integrator.h index 894010dfd..19a768cd4 100644 --- a/libraries/wavemap/include/wavemap/integrator/ray_tracing/ray_tracing_integrator.h +++ b/libraries/wavemap/include/wavemap/integrator/ray_tracing/ray_tracing_integrator.h @@ -6,7 +6,7 @@ #include "wavemap/data_structure/volumetric/volumetric_data_structure_base.h" #include "wavemap/integrator/integrator_base.h" #include "wavemap/integrator/measurement_model/constant_ray.h" -#include "wavemap/iterator/ray_iterator.h" +#include "wavemap/utils/iterate/ray_iterator.h" namespace wavemap { /** diff --git a/libraries/wavemap/include/wavemap/utils/approximate_trigonometry.h b/libraries/wavemap/include/wavemap/utils/approximate_trigonometry.h deleted file mode 100644 index e247c03d2..000000000 --- a/libraries/wavemap/include/wavemap/utils/approximate_trigonometry.h +++ /dev/null @@ -1,84 +0,0 @@ -#ifndef WAVEMAP_UTILS_APPROXIMATE_TRIGONOMETRY_H_ -#define WAVEMAP_UTILS_APPROXIMATE_TRIGONOMETRY_H_ - -#include -#include - -#include "wavemap/common.h" - -namespace wavemap::approximate { -namespace internal { -inline FloatingPoint atan_fma_approximation(FloatingPoint x) { - // Copyright (c) 2021 Francesco Mazzoli - // - // Permission to use, copy, modify, and distribute this software for any - // purpose with or without fee is hereby granted, provided that the above - // copyright notice and this permission notice appear in all copies. - // - // THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES - // WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF - // MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR - // ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES - // WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN - // ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF - // OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. - constexpr FloatingPoint a1 = 0.99997726f; - constexpr FloatingPoint a3 = -0.33262347f; - constexpr FloatingPoint a5 = 0.19354346f; - constexpr FloatingPoint a7 = -0.11643287f; - constexpr FloatingPoint a9 = 0.05265332f; - constexpr FloatingPoint a11 = -0.01172120f; - - // Compute approximation using Horner's method - const FloatingPoint x_sq = x * x; - return x * - std::fma( - x_sq, - std::fma(x_sq, - std::fma(x_sq, - std::fma(x_sq, std::fma(x_sq, a11, a9), a7), a5), - a3), - a1); -} -} // namespace internal - -struct atan2 - : public std::binary_function { - static constexpr FloatingPoint kWorstCaseError = 1.908e-6f; - - FloatingPoint operator()(FloatingPoint y, FloatingPoint x) const { - // Copyright (c) 2021 Francesco Mazzoli - // - // Permission to use, copy, modify, and distribute this software for any - // purpose with or without fee is hereby granted, provided that the above - // copyright notice and this permission notice appear in all copies. - // - // THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES - // WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF - // MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR - // ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES - // WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN - // ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF - // OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. - - // Ensure input is in [-1, +1] - const bool swap = std::abs(x) < std::abs(y); - const FloatingPoint atan_input = (swap ? x : y) / (swap ? y : x); - - // Approximate atan - FloatingPoint res = internal::atan_fma_approximation(atan_input); - - // If swapped, adjust atan output - res = swap ? std::copysign(kHalfPi, atan_input) - res : res; - // Adjust the result depending on the input quadrant - if (x < 0.0f) { - res = std::copysign(kPi, y) + res; - } - - // Return the result - return res; - } -}; -} // namespace wavemap::approximate - -#endif // WAVEMAP_UTILS_APPROXIMATE_TRIGONOMETRY_H_ diff --git a/libraries/wavemap/include/wavemap/utils/bit_manipulation.h b/libraries/wavemap/include/wavemap/utils/bits/bit_operations.h similarity index 91% rename from libraries/wavemap/include/wavemap/utils/bit_manipulation.h rename to libraries/wavemap/include/wavemap/utils/bits/bit_operations.h index 6c648eca3..5912b5b73 100644 --- a/libraries/wavemap/include/wavemap/utils/bit_manipulation.h +++ b/libraries/wavemap/include/wavemap/utils/bits/bit_operations.h @@ -1,5 +1,5 @@ -#ifndef WAVEMAP_UTILS_BIT_MANIPULATION_H_ -#define WAVEMAP_UTILS_BIT_MANIPULATION_H_ +#ifndef WAVEMAP_UTILS_BITS_BIT_OPERATIONS_H_ +#define WAVEMAP_UTILS_BITS_BIT_OPERATIONS_H_ #include "wavemap/common.h" @@ -9,7 +9,7 @@ #define BIT_COMPRESS_AVAILABLE #endif -namespace wavemap::bit_manip { +namespace wavemap::bit_ops { namespace detail { inline constexpr uint32_t popcount(uint32_t bitstring) { return __builtin_popcount(bitstring); @@ -44,11 +44,11 @@ inline uint64_t expand(uint64_t source, uint64_t selector) { template T expand(T source, T selector) { static_assert( - "Method bit_manip::expand is not available for the current hardware, " + "Method bit_ops::expand is not available for the current hardware, " "either because it has yet to be implemented or because general-purpose " "bit expansion cannot efficiently be performed/emulated on this " "hardware. For portability, it is recommended to implement case-specific " - "fallbacks for bit_manip::expand wherever it is used. These fallbacks " + "fallbacks for bit_ops::expand wherever it is used. These fallbacks " "can be toggled by checking whether the preprocessor variable " "BIT_EXPAND_AVAILABLE is set."); } @@ -65,11 +65,11 @@ inline uint64_t compress(uint64_t source, uint64_t selector) { template T compress(T source, T selector) { static_assert( - "Method bit_manip::compress is not available for the current hardware, " + "Method bit_ops::compress is not available for the current hardware, " "either because it has yet to be implemented or because general-purpose " "bit compression cannot efficiently be performed/emulated on this " "hardware. For portability, it is recommended to implement case-specific " - "fallbacks for bit_manip::compress wherever it is used. These fallbacks " + "fallbacks for bit_ops::compress wherever it is used. These fallbacks " "can be toggled by checking whether the preprocessor variable " "BIT_COMPRESS_AVAILABLE is set."); } @@ -169,6 +169,6 @@ T compress(T source, T selector) { return detail::compress(bit_cast_unsigned(source), bit_cast_unsigned(selector)); } -} // namespace wavemap::bit_manip +} // namespace wavemap::bit_ops -#endif // WAVEMAP_UTILS_BIT_MANIPULATION_H_ +#endif // WAVEMAP_UTILS_BITS_BIT_OPERATIONS_H_ diff --git a/libraries/wavemap/include/wavemap/utils/morton_encoding.h b/libraries/wavemap/include/wavemap/utils/bits/morton_encoding.h similarity index 89% rename from libraries/wavemap/include/wavemap/utils/morton_encoding.h rename to libraries/wavemap/include/wavemap/utils/bits/morton_encoding.h index 92e63eb6a..ff92b6906 100644 --- a/libraries/wavemap/include/wavemap/utils/morton_encoding.h +++ b/libraries/wavemap/include/wavemap/utils/bits/morton_encoding.h @@ -1,13 +1,14 @@ -#ifndef WAVEMAP_UTILS_MORTON_ENCODING_H_ -#define WAVEMAP_UTILS_MORTON_ENCODING_H_ +#ifndef WAVEMAP_UTILS_BITS_MORTON_ENCODING_H_ +#define WAVEMAP_UTILS_BITS_MORTON_ENCODING_H_ #include -#include "wavemap/utils/bit_manipulation.h" +#include "wavemap/utils/bits/bit_operations.h" +#include "wavemap/utils/math/int_math.h" namespace wavemap::morton { template -constexpr int kMaxTreeHeight = 8 * sizeof(MortonIndex) / dim; +constexpr int kMaxTreeHeight = 8 * sizeof(MortonIndex) / dim - 1; template constexpr IndexElement kMaxSingleCoordinate = @@ -77,9 +78,9 @@ MortonIndex encode(const Index& index) { // target CPU architecture and LUTs otherwise uint64_t morton = 0u; #ifdef BIT_EXPAND_AVAILABLE - constexpr auto pattern = bit_manip::repeat_block(dim, 0b1); + constexpr auto pattern = bit_ops::repeat_block(dim, 0b1); for (int dim_idx = 0; dim_idx < dim; ++dim_idx) { - morton |= bit_manip::expand(index[dim_idx], pattern << dim_idx); + morton |= bit_ops::expand(index[dim_idx], pattern << dim_idx); } #else constexpr std::make_unsigned_t kByteMask = (1 << 8) - 1; @@ -126,9 +127,9 @@ Index decode(MortonIndex morton) { // target CPU architecture and LUTs otherwise Index index; #ifdef BIT_COMPRESS_AVAILABLE - constexpr auto pattern = bit_manip::repeat_block(dim, 0b1); + constexpr auto pattern = bit_ops::repeat_block(dim, 0b1); for (int dim_idx = 0; dim_idx < dim; ++dim_idx) { - index[dim_idx] = bit_manip::compress(morton, pattern << dim_idx); + index[dim_idx] = bit_ops::compress(morton, pattern << dim_idx); } #else for (int dim_idx = 0; dim_idx < dim; ++dim_idx) { @@ -139,4 +140,4 @@ Index decode(MortonIndex morton) { } } // namespace wavemap::morton -#endif // WAVEMAP_UTILS_MORTON_ENCODING_H_ +#endif // WAVEMAP_UTILS_BITS_MORTON_ENCODING_H_ diff --git a/libraries/wavemap/include/wavemap/utils/data/comparisons.h b/libraries/wavemap/include/wavemap/utils/data/comparisons.h new file mode 100644 index 000000000..ffad33312 --- /dev/null +++ b/libraries/wavemap/include/wavemap/utils/data/comparisons.h @@ -0,0 +1,59 @@ +#ifndef WAVEMAP_UTILS_DATA_COMPARISONS_H_ +#define WAVEMAP_UTILS_DATA_COMPARISONS_H_ + +#include +#include +#include + +#include "wavemap/common.h" + +namespace wavemap::data { +// Test strict equality to zero +template +bool is_nonzero(const T& data) { + return data != T{}; +} + +// Test equality to zero with a tolerance (for POD types) +template , bool> = true> +bool is_nonzero(const DataT& data, ThresholdT threshold) { + return threshold < std::abs(data); +} + +// Test element-wise equality to zero with a tolerance (for array types) +template +bool is_nonzero(const DataT& data, ThresholdT threshold) { + return std::any_of(data.cbegin(), data.cend(), [threshold](auto value) { + return threshold < std::abs(value); + }); +} + +template +bool EigenCwise(const FirstType& matrix_a, const SecondType& matrix_b) { + static_assert(static_cast(FirstType::RowsAtCompileTime) == + static_cast(SecondType::RowsAtCompileTime)); + static_assert(static_cast(FirstType::ColsAtCompileTime) == + static_cast(SecondType::ColsAtCompileTime)); + if (ComparisonOp{}(matrix_a.array(), matrix_b.array()).all()) { + return true; + } else { + return false; + } +} + +template +bool EigenCwiseNear(const FirstType& matrix_a, const SecondType& matrix_b, + FloatingPoint precision = kEpsilon) { + static_assert(FirstType::RowsAtCompileTime == SecondType::RowsAtCompileTime); + static_assert(FirstType::ColsAtCompileTime == SecondType::ColsAtCompileTime); + if (((matrix_a - matrix_b).array().abs() < precision).all()) { + return true; + } else { + return false; + } +} +} // namespace wavemap::data + +#endif // WAVEMAP_UTILS_DATA_COMPARISONS_H_ diff --git a/libraries/wavemap/include/wavemap/constants.h b/libraries/wavemap/include/wavemap/utils/data/constants.h similarity index 90% rename from libraries/wavemap/include/wavemap/constants.h rename to libraries/wavemap/include/wavemap/utils/data/constants.h index 1304a59ac..43cf24242 100644 --- a/libraries/wavemap/include/wavemap/constants.h +++ b/libraries/wavemap/include/wavemap/utils/data/constants.h @@ -1,5 +1,5 @@ -#ifndef WAVEMAP_CONSTANTS_H_ -#define WAVEMAP_CONSTANTS_H_ +#ifndef WAVEMAP_UTILS_DATA_CONSTANTS_H_ +#define WAVEMAP_UTILS_DATA_CONSTANTS_H_ namespace wavemap { // NOTE: We define commonly used constants here instead of directly using the @@ -24,4 +24,4 @@ struct constants { }; } // namespace wavemap -#endif // WAVEMAP_CONSTANTS_H_ +#endif // WAVEMAP_UTILS_DATA_CONSTANTS_H_ diff --git a/libraries/wavemap/include/wavemap/utils/data/eigen_checks.h b/libraries/wavemap/include/wavemap/utils/data/eigen_checks.h new file mode 100644 index 000000000..fb0d09149 --- /dev/null +++ b/libraries/wavemap/include/wavemap/utils/data/eigen_checks.h @@ -0,0 +1,94 @@ +#ifndef WAVEMAP_UTILS_DATA_EIGEN_CHECKS_H_ +#define WAVEMAP_UTILS_DATA_EIGEN_CHECKS_H_ + +#include + +#include + +#include "wavemap/common.h" +#include "wavemap/utils/data/comparisons.h" +#include "wavemap/utils/print/eigen.h" + +// Define regular checks +#define CHECK_EIGEN_EQ(MatrixA, MatrixB) \ + CHECK(wavemap::data::EigenCwise>(MatrixA, MatrixB)) \ + << "For matrix A" << print::eigen::oneLine(MatrixA) << " and matrix B" \ + << print::eigen::oneLine(MatrixB) + +#define CHECK_EIGEN_NE(MatrixA, MatrixB) \ + CHECK(wavemap::data::EigenCwise>(MatrixA, MatrixB)) \ + << "For matrix A" << print::eigen::oneLine(MatrixA) << " and matrix B" \ + << print::eigen::oneLine(MatrixB) + +#define CHECK_EIGEN_LE(MatrixA, MatrixB) \ + CHECK(wavemap::data::EigenCwise>(MatrixA, MatrixB)) \ + << "For matrix A" << print::eigen::oneLine(MatrixA) << " and matrix B" \ + << print::eigen::oneLine(MatrixB) + +#define CHECK_EIGEN_LT(MatrixA, MatrixB) \ + CHECK(wavemap::data::EigenCwise>(MatrixA, MatrixB)) \ + << "For matrix A" << print::eigen::oneLine(MatrixA) << " and matrix B" \ + << print::eigen::oneLine(MatrixB) + +#define CHECK_EIGEN_GE(MatrixA, MatrixB) \ + CHECK(wavemap::data::EigenCwise>(MatrixA, MatrixB)) \ + << "For matrix A" << print::eigen::oneLine(MatrixA) << " and matrix B" \ + << print::eigen::oneLine(MatrixB) + +#define CHECK_EIGEN_GT(MatrixA, MatrixB) \ + CHECK(wavemap::data::EigenCwise>(MatrixA, MatrixB)) \ + << "For matrix A" << print::eigen::oneLine(MatrixA) << " and matrix B" \ + << print::eigen::oneLine(MatrixB) + +#define CHECK_EIGEN_NEAR(MatrixA, MatrixB, Precision) \ + CHECK(wavemap::data::EigenCwiseNear(MatrixA, MatrixB, Precision)) \ + << "For matrix A" << print::eigen::oneLine(MatrixA) << " and matrix B" \ + << print::eigen::oneLine(MatrixB) << " with precision " << Precision + +// Define debug checks +#if DCHECK_IS_ON() + +#define DCHECK_EIGEN_EQ(MatrixA, MatrixB) CHECK_EIGEN_EQ(MatrixA, MatrixB) +#define DCHECK_EIGEN_NE(MatrixA, MatrixB) CHECK_EIGEN_NE(MatrixA, MatrixB) +#define DCHECK_EIGEN_LE(MatrixA, MatrixB) CHECK_EIGEN_LE(MatrixA, MatrixB) +#define DCHECK_EIGEN_LT(MatrixA, MatrixB) CHECK_EIGEN_LT(MatrixA, MatrixB) +#define DCHECK_EIGEN_GE(MatrixA, MatrixB) CHECK_EIGEN_GE(MatrixA, MatrixB) +#define DCHECK_EIGEN_GT(MatrixA, MatrixB) CHECK_EIGEN_GT(MatrixA, MatrixB) +#define DCHECK_EIGEN_NEAR(MatrixA, MatrixB, Precision) \ + CHECK_EIGEN_NEAR(MatrixA, MatrixB, Precision) + +// When debug checking is disabled, replace the DCHECKs with empty macros +#else + +#define DCHECK_EIGEN_EQ(MatrixA, MatrixB) \ + GLOG_MSVC_PUSH_DISABLE_WARNING(4127) \ + while (false) GLOG_MSVC_POP_WARNING() CHECK_EIGEN_EQ(MatrixA, MatrixB) + +#define DCHECK_EIGEN_NE(MatrixA, MatrixB) \ + GLOG_MSVC_PUSH_DISABLE_WARNING(4127) \ + while (false) GLOG_MSVC_POP_WARNING() CHECK_EIGEN_NE(MatrixA, MatrixB) + +#define DCHECK_EIGEN_LE(MatrixA, MatrixB) \ + GLOG_MSVC_PUSH_DISABLE_WARNING(4127) \ + while (false) GLOG_MSVC_POP_WARNING() CHECK_EIGEN_LE(MatrixA, MatrixB) + +#define DCHECK_EIGEN_LT(MatrixA, MatrixB) \ + GLOG_MSVC_PUSH_DISABLE_WARNING(4127) \ + while (false) GLOG_MSVC_POP_WARNING() CHECK_EIGEN_LT(MatrixA, MatrixB) + +#define DCHECK_EIGEN_GE(MatrixA, MatrixB) \ + GLOG_MSVC_PUSH_DISABLE_WARNING(4127) \ + while (false) GLOG_MSVC_POP_WARNING() CHECK_EIGEN_GE(MatrixA, MatrixB) + +#define DCHECK_EIGEN_GT(MatrixA, MatrixB) \ + GLOG_MSVC_PUSH_DISABLE_WARNING(4127) \ + while (false) GLOG_MSVC_POP_WARNING() CHECK_EIGEN_GT(MatrixA, MatrixB) + +#define DCHECK_EIGEN_NEAR(MatrixA, MatrixB, Precision) \ + GLOG_MSVC_PUSH_DISABLE_WARNING(4127) \ + while (false) \ + GLOG_MSVC_POP_WARNING() CHECK_EIGEN_NEAR(MatrixA, MatrixB, Precision) + +#endif + +#endif // WAVEMAP_UTILS_DATA_EIGEN_CHECKS_H_ diff --git a/libraries/wavemap/include/wavemap/utils/fill_utils.h b/libraries/wavemap/include/wavemap/utils/data/fill.h similarity index 73% rename from libraries/wavemap/include/wavemap/utils/fill_utils.h rename to libraries/wavemap/include/wavemap/utils/data/fill.h index bf7a3eb1a..b05a60e90 100644 --- a/libraries/wavemap/include/wavemap/utils/fill_utils.h +++ b/libraries/wavemap/include/wavemap/utils/data/fill.h @@ -1,9 +1,9 @@ -#ifndef WAVEMAP_UTILS_FILL_UTILS_H_ -#define WAVEMAP_UTILS_FILL_UTILS_H_ +#ifndef WAVEMAP_UTILS_DATA_FILL_H_ +#define WAVEMAP_UTILS_DATA_FILL_H_ #include -namespace wavemap::fill { +namespace wavemap::data::fill { // Fill POD types with a constant value template auto constant(V value) -> std::enable_if_t, T> { @@ -21,6 +21,6 @@ template T zero() { return constant(0); } -} // namespace wavemap::fill +} // namespace wavemap::data::fill -#endif // WAVEMAP_UTILS_FILL_UTILS_H_ +#endif // WAVEMAP_UTILS_DATA_FILL_H_ diff --git a/libraries/wavemap/include/wavemap/utils/data_utils.h b/libraries/wavemap/include/wavemap/utils/data_utils.h deleted file mode 100644 index 8386c5ff0..000000000 --- a/libraries/wavemap/include/wavemap/utils/data_utils.h +++ /dev/null @@ -1,28 +0,0 @@ -#ifndef WAVEMAP_UTILS_DATA_UTILS_H_ -#define WAVEMAP_UTILS_DATA_UTILS_H_ - -namespace wavemap::data_utils { -// Test strict equality to zero -template -bool is_nonzero(const T& data) { - return data != T{}; -} - -// Test equality to zero with a tolerance (for POD types) -template , bool> = true> -bool is_nonzero(const DataT& data, ThresholdT threshold) { - return threshold < std::abs(data); -} - -// Test element-wise equality to zero with a tolerance (for array types) -template -bool is_nonzero(const DataT& data, ThresholdT threshold) { - return std::any_of(data.cbegin(), data.cend(), [threshold](auto value) { - return threshold < std::abs(value); - }); -} -} // namespace wavemap::data_utils - -#endif // WAVEMAP_UTILS_DATA_UTILS_H_ diff --git a/libraries/wavemap/include/wavemap/utils/eigen_format.h b/libraries/wavemap/include/wavemap/utils/eigen_format.h deleted file mode 100644 index 3b5cd4638..000000000 --- a/libraries/wavemap/include/wavemap/utils/eigen_format.h +++ /dev/null @@ -1,17 +0,0 @@ -#ifndef WAVEMAP_UTILS_EIGEN_FORMAT_H_ -#define WAVEMAP_UTILS_EIGEN_FORMAT_H_ - -#include - -namespace wavemap { -struct EigenFormat { - static const Eigen::IOFormat kOneLine; - - template - static Eigen::WithFormat oneLine(const Derived& matrix) { - return Eigen::WithFormat(matrix, kOneLine); - } -}; -} // namespace wavemap - -#endif // WAVEMAP_UTILS_EIGEN_FORMAT_H_ diff --git a/libraries/wavemap/include/wavemap/iterator/grid_iterator.h b/libraries/wavemap/include/wavemap/utils/iterate/grid_iterator.h similarity index 93% rename from libraries/wavemap/include/wavemap/iterator/grid_iterator.h rename to libraries/wavemap/include/wavemap/utils/iterate/grid_iterator.h index 32cae4798..f9244369e 100644 --- a/libraries/wavemap/include/wavemap/iterator/grid_iterator.h +++ b/libraries/wavemap/include/wavemap/utils/iterate/grid_iterator.h @@ -1,5 +1,5 @@ -#ifndef WAVEMAP_ITERATOR_GRID_ITERATOR_H_ -#define WAVEMAP_ITERATOR_GRID_ITERATOR_H_ +#ifndef WAVEMAP_UTILS_ITERATE_GRID_ITERATOR_H_ +#define WAVEMAP_UTILS_ITERATE_GRID_ITERATOR_H_ #include @@ -66,4 +66,4 @@ class Grid { }; } // namespace wavemap -#endif // WAVEMAP_ITERATOR_GRID_ITERATOR_H_ +#endif // WAVEMAP_UTILS_ITERATE_GRID_ITERATOR_H_ diff --git a/libraries/wavemap/include/wavemap/iterator/impl/subtree_iterator_inl.h b/libraries/wavemap/include/wavemap/utils/iterate/impl/subtree_iterator_inl.h similarity index 94% rename from libraries/wavemap/include/wavemap/iterator/impl/subtree_iterator_inl.h rename to libraries/wavemap/include/wavemap/utils/iterate/impl/subtree_iterator_inl.h index 1fd07d0a2..35114cfd2 100644 --- a/libraries/wavemap/include/wavemap/iterator/impl/subtree_iterator_inl.h +++ b/libraries/wavemap/include/wavemap/utils/iterate/impl/subtree_iterator_inl.h @@ -1,5 +1,5 @@ -#ifndef WAVEMAP_ITERATOR_IMPL_SUBTREE_ITERATOR_INL_H_ -#define WAVEMAP_ITERATOR_IMPL_SUBTREE_ITERATOR_INL_H_ +#ifndef WAVEMAP_UTILS_ITERATE_IMPL_SUBTREE_ITERATOR_INL_H_ +#define WAVEMAP_UTILS_ITERATE_IMPL_SUBTREE_ITERATOR_INL_H_ namespace wavemap { template @@ -98,4 +98,4 @@ void SubtreeIterator::enqueueNodeChildren( } } // namespace wavemap -#endif // WAVEMAP_ITERATOR_IMPL_SUBTREE_ITERATOR_INL_H_ +#endif // WAVEMAP_UTILS_ITERATE_IMPL_SUBTREE_ITERATOR_INL_H_ diff --git a/libraries/wavemap/include/wavemap/iterator/pointcloud_iterator.h b/libraries/wavemap/include/wavemap/utils/iterate/pointcloud_iterator.h similarity index 91% rename from libraries/wavemap/include/wavemap/iterator/pointcloud_iterator.h rename to libraries/wavemap/include/wavemap/utils/iterate/pointcloud_iterator.h index 249abb389..21d1e3ab8 100644 --- a/libraries/wavemap/include/wavemap/iterator/pointcloud_iterator.h +++ b/libraries/wavemap/include/wavemap/utils/iterate/pointcloud_iterator.h @@ -1,5 +1,5 @@ -#ifndef WAVEMAP_ITERATOR_POINTCLOUD_ITERATOR_H_ -#define WAVEMAP_ITERATOR_POINTCLOUD_ITERATOR_H_ +#ifndef WAVEMAP_UTILS_ITERATE_POINTCLOUD_ITERATOR_H_ +#define WAVEMAP_UTILS_ITERATE_POINTCLOUD_ITERATOR_H_ #include @@ -52,4 +52,4 @@ class PointcloudIterator { }; } // namespace wavemap -#endif // WAVEMAP_ITERATOR_POINTCLOUD_ITERATOR_H_ +#endif // WAVEMAP_UTILS_ITERATE_POINTCLOUD_ITERATOR_H_ diff --git a/libraries/wavemap/include/wavemap/iterator/ray_iterator.h b/libraries/wavemap/include/wavemap/utils/iterate/ray_iterator.h similarity index 96% rename from libraries/wavemap/include/wavemap/iterator/ray_iterator.h rename to libraries/wavemap/include/wavemap/utils/iterate/ray_iterator.h index 931787b56..f9373cad9 100644 --- a/libraries/wavemap/include/wavemap/iterator/ray_iterator.h +++ b/libraries/wavemap/include/wavemap/utils/iterate/ray_iterator.h @@ -1,5 +1,5 @@ -#ifndef WAVEMAP_ITERATOR_RAY_ITERATOR_H_ -#define WAVEMAP_ITERATOR_RAY_ITERATOR_H_ +#ifndef WAVEMAP_UTILS_ITERATE_RAY_ITERATOR_H_ +#define WAVEMAP_UTILS_ITERATE_RAY_ITERATOR_H_ #include "wavemap/common.h" #include "wavemap/indexing/index_conversions.h" @@ -104,4 +104,4 @@ class Ray { }; } // namespace wavemap -#endif // WAVEMAP_ITERATOR_RAY_ITERATOR_H_ +#endif // WAVEMAP_UTILS_ITERATE_RAY_ITERATOR_H_ diff --git a/libraries/wavemap/include/wavemap/iterator/subtree_iterator.h b/libraries/wavemap/include/wavemap/utils/iterate/subtree_iterator.h similarity index 95% rename from libraries/wavemap/include/wavemap/iterator/subtree_iterator.h rename to libraries/wavemap/include/wavemap/utils/iterate/subtree_iterator.h index 63f8896bb..6cb0ddca1 100644 --- a/libraries/wavemap/include/wavemap/iterator/subtree_iterator.h +++ b/libraries/wavemap/include/wavemap/utils/iterate/subtree_iterator.h @@ -1,5 +1,5 @@ -#ifndef WAVEMAP_ITERATOR_SUBTREE_ITERATOR_H_ -#define WAVEMAP_ITERATOR_SUBTREE_ITERATOR_H_ +#ifndef WAVEMAP_UTILS_ITERATE_SUBTREE_ITERATOR_H_ +#define WAVEMAP_UTILS_ITERATE_SUBTREE_ITERATOR_H_ #include @@ -134,6 +134,6 @@ class Subtree { }; } // namespace wavemap -#include "wavemap/iterator/impl/subtree_iterator_inl.h" +#include "wavemap/utils/iterate/impl/subtree_iterator_inl.h" -#endif // WAVEMAP_ITERATOR_SUBTREE_ITERATOR_H_ +#endif // WAVEMAP_UTILS_ITERATE_SUBTREE_ITERATOR_H_ diff --git a/libraries/wavemap/include/wavemap/utils/angle_utils.h b/libraries/wavemap/include/wavemap/utils/math/angle_normalization.h similarity index 70% rename from libraries/wavemap/include/wavemap/utils/angle_utils.h rename to libraries/wavemap/include/wavemap/utils/math/angle_normalization.h index 446df8bc6..78a6736ed 100644 --- a/libraries/wavemap/include/wavemap/utils/angle_utils.h +++ b/libraries/wavemap/include/wavemap/utils/math/angle_normalization.h @@ -1,5 +1,5 @@ -#ifndef WAVEMAP_UTILS_ANGLE_UTILS_H_ -#define WAVEMAP_UTILS_ANGLE_UTILS_H_ +#ifndef WAVEMAP_UTILS_MATH_ANGLE_NORMALIZATION_H_ +#define WAVEMAP_UTILS_MATH_ANGLE_NORMALIZATION_H_ #include "wavemap/common.h" @@ -18,4 +18,4 @@ inline FloatingPoint normalize_near(FloatingPoint angle) { } } // namespace wavemap::angle_math -#endif // WAVEMAP_UTILS_ANGLE_UTILS_H_ +#endif // WAVEMAP_UTILS_MATH_ANGLE_NORMALIZATION_H_ diff --git a/libraries/wavemap/include/wavemap/utils/math/approximate_trigonometry.h b/libraries/wavemap/include/wavemap/utils/math/approximate_trigonometry.h new file mode 100644 index 000000000..4c47c5ff5 --- /dev/null +++ b/libraries/wavemap/include/wavemap/utils/math/approximate_trigonometry.h @@ -0,0 +1,82 @@ +#ifndef WAVEMAP_UTILS_MATH_APPROXIMATE_TRIGONOMETRY_H_ +#define WAVEMAP_UTILS_MATH_APPROXIMATE_TRIGONOMETRY_H_ + +#include +#include + +#include "wavemap/common.h" + +namespace wavemap::approximate { +struct atan : public std::unary_function { + FloatingPoint operator()(FloatingPoint x) const { + // Copyright (c) 2021 Francesco Mazzoli + // + // Permission to use, copy, modify, and distribute this software for any + // purpose with or without fee is hereby granted, provided that the above + // copyright notice and this permission notice appear in all copies. + // + // THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES + // WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF + // MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR + // ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES + // WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN + // ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF + // OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + constexpr FloatingPoint a1 = 0.99997726f; + constexpr FloatingPoint a3 = -0.33262347f; + constexpr FloatingPoint a5 = 0.19354346f; + constexpr FloatingPoint a7 = -0.11643287f; + constexpr FloatingPoint a9 = 0.05265332f; + constexpr FloatingPoint a11 = -0.01172120f; + + // Compute approximation using Horner's method + const FloatingPoint x_sq = x * x; + FloatingPoint u = std::fma(x_sq, a11, a9); + u = std::fma(x_sq, u, a7); + u = std::fma(x_sq, u, a5); + u = std::fma(x_sq, u, a3); + u = std::fma(x_sq, u, a1); + return x * u; + } +}; + +struct atan2 + : public std::binary_function { + static constexpr FloatingPoint kWorstCaseError = 1.908e-6f; + + FloatingPoint operator()(FloatingPoint y, FloatingPoint x) const { + // Copyright (c) 2021 Francesco Mazzoli + // + // Permission to use, copy, modify, and distribute this software for any + // purpose with or without fee is hereby granted, provided that the above + // copyright notice and this permission notice appear in all copies. + // + // THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES + // WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF + // MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR + // ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES + // WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN + // ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF + // OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + + // Ensure input is in [-1, +1] + const bool swap = std::abs(x) < std::abs(y); + const FloatingPoint atan_input = (swap ? x : y) / (swap ? y : x); + + // Approximate atan + FloatingPoint res = atan()(atan_input); + + // If swapped, adjust atan output + res = swap ? std::copysign(kHalfPi, atan_input) - res : res; + // Adjust the result depending on the input quadrant + if (x < 0.0f) { + res = std::copysign(kPi, y) + res; + } + + // Return the result + return res; + } +}; +} // namespace wavemap::approximate + +#endif // WAVEMAP_UTILS_MATH_APPROXIMATE_TRIGONOMETRY_H_ diff --git a/libraries/wavemap/include/wavemap/utils/int_math.h b/libraries/wavemap/include/wavemap/utils/math/int_math.h similarity index 87% rename from libraries/wavemap/include/wavemap/utils/int_math.h rename to libraries/wavemap/include/wavemap/utils/math/int_math.h index e437cb5a2..4e982aec9 100644 --- a/libraries/wavemap/include/wavemap/utils/int_math.h +++ b/libraries/wavemap/include/wavemap/utils/math/int_math.h @@ -1,10 +1,10 @@ -#ifndef WAVEMAP_UTILS_INT_MATH_H_ -#define WAVEMAP_UTILS_INT_MATH_H_ +#ifndef WAVEMAP_UTILS_MATH_INT_MATH_H_ +#define WAVEMAP_UTILS_MATH_INT_MATH_H_ #include #include "wavemap/common.h" -#include "wavemap/utils/bit_manipulation.h" +#include "wavemap/utils/bits/bit_operations.h" namespace wavemap::int_math { template @@ -17,14 +17,14 @@ template constexpr T log2_floor(T value) { static_assert(std::is_integral_v); DCHECK(value != static_cast(0)); - return std::numeric_limits::digits - bit_manip::clz(value); + return std::numeric_limits::digits - bit_ops::clz(value); } template constexpr T log2_ceil(T value) { static_assert(std::is_integral_v); const T log2_floored = log2_floor(value); - if (bit_manip::popcount(value) == static_cast(1)) { + if (bit_ops::popcount(value) == static_cast(1)) { return log2_floored; } else { return log2_floored + static_cast(1); @@ -79,4 +79,4 @@ constexpr T div_round_up(T numerator, T denominator) { } } // namespace wavemap::int_math -#endif // WAVEMAP_UTILS_INT_MATH_H_ +#endif // WAVEMAP_UTILS_MATH_INT_MATH_H_ diff --git a/libraries/wavemap/include/wavemap/utils/tree_math.h b/libraries/wavemap/include/wavemap/utils/math/tree_math.h similarity index 84% rename from libraries/wavemap/include/wavemap/utils/tree_math.h rename to libraries/wavemap/include/wavemap/utils/math/tree_math.h index 68f2944dc..0f78819fb 100644 --- a/libraries/wavemap/include/wavemap/utils/tree_math.h +++ b/libraries/wavemap/include/wavemap/utils/math/tree_math.h @@ -1,7 +1,7 @@ -#ifndef WAVEMAP_UTILS_TREE_MATH_H_ -#define WAVEMAP_UTILS_TREE_MATH_H_ +#ifndef WAVEMAP_UTILS_MATH_TREE_MATH_H_ +#define WAVEMAP_UTILS_MATH_TREE_MATH_H_ -#include "wavemap/utils/int_math.h" +#include "wavemap/utils/math/int_math.h" namespace wavemap::tree_math::perfect_tree { template @@ -31,4 +31,4 @@ constexpr size_t num_leaf_nodes(size_t tree_height) { } } // namespace wavemap::tree_math::perfect_tree -#endif // WAVEMAP_UTILS_TREE_MATH_H_ +#endif // WAVEMAP_UTILS_MATH_TREE_MATH_H_ diff --git a/libraries/wavemap/include/wavemap/utils/nameof.h b/libraries/wavemap/include/wavemap/utils/meta/nameof.h similarity index 85% rename from libraries/wavemap/include/wavemap/utils/nameof.h rename to libraries/wavemap/include/wavemap/utils/meta/nameof.h index 80fc0efd8..07ecc043c 100644 --- a/libraries/wavemap/include/wavemap/utils/nameof.h +++ b/libraries/wavemap/include/wavemap/utils/meta/nameof.h @@ -27,18 +27,18 @@ ** ****************************************************************************/ -#ifndef WAVEMAP_UTILS_NAMEOF_H_ -#define WAVEMAP_UTILS_NAMEOF_H_ +#ifndef WAVEMAP_UTILS_META_NAMEOF_H_ +#define WAVEMAP_UTILS_META_NAMEOF_H_ -#define NAMEOF(x) wavemap::_nameof<0>(#x, sizeof(x)) // NOLINT +#define NAMEOF(x) wavemap::meta::nameof<0>(#x, sizeof(x)) // NOLINT #include // NOLINT #include #include -namespace wavemap { +namespace wavemap::meta { template -std::string _nameof(const std::string& x, std::size_t) { +std::string nameof(const std::string& x, std::size_t) { std::regex regex(R"(^&?([_a-zA-Z]\w*(->|\.|::))*([_a-zA-Z]\w*)$)"); std::smatch match; if (std::regex_match(x, match, regex) && match.size() == 4) { @@ -47,6 +47,6 @@ std::string _nameof(const std::string& x, std::size_t) { throw std::logic_error( "A bad expression x in nameof(x). The expression is \"" + x + "\"."); } -} // namespace wavemap +} // namespace wavemap::meta -#endif // WAVEMAP_UTILS_NAMEOF_H_ +#endif // WAVEMAP_UTILS_META_NAMEOF_H_ diff --git a/libraries/wavemap/include/wavemap/utils/type_utils.h b/libraries/wavemap/include/wavemap/utils/meta/type_utils.h similarity index 92% rename from libraries/wavemap/include/wavemap/utils/type_utils.h rename to libraries/wavemap/include/wavemap/utils/meta/type_utils.h index de901f3aa..854252d83 100644 --- a/libraries/wavemap/include/wavemap/utils/type_utils.h +++ b/libraries/wavemap/include/wavemap/utils/meta/type_utils.h @@ -1,9 +1,9 @@ -#ifndef WAVEMAP_UTILS_TYPE_UTILS_H_ -#define WAVEMAP_UTILS_TYPE_UTILS_H_ +#ifndef WAVEMAP_UTILS_META_TYPE_UTILS_H_ +#define WAVEMAP_UTILS_META_TYPE_UTILS_H_ #include -namespace wavemap { +namespace wavemap::meta { // Helpers for enum classes template constexpr auto to_underlying(T value) noexcept { @@ -74,6 +74,6 @@ struct has_to_str_member_fn().toStr())>> template constexpr bool has_to_str_member_fn_v = has_to_str_member_fn::value; -} // namespace wavemap +} // namespace wavemap::meta -#endif // WAVEMAP_UTILS_TYPE_UTILS_H_ +#endif // WAVEMAP_UTILS_META_TYPE_UTILS_H_ diff --git a/libraries/wavemap/include/wavemap/utils/container_print_utils.h b/libraries/wavemap/include/wavemap/utils/print/container.h similarity index 61% rename from libraries/wavemap/include/wavemap/utils/container_print_utils.h rename to libraries/wavemap/include/wavemap/utils/print/container.h index 1901e75e2..7e248107e 100644 --- a/libraries/wavemap/include/wavemap/utils/container_print_utils.h +++ b/libraries/wavemap/include/wavemap/utils/print/container.h @@ -1,19 +1,19 @@ -#ifndef WAVEMAP_UTILS_CONTAINER_PRINT_UTILS_H_ -#define WAVEMAP_UTILS_CONTAINER_PRINT_UTILS_H_ +#ifndef WAVEMAP_UTILS_PRINT_CONTAINER_H_ +#define WAVEMAP_UTILS_PRINT_CONTAINER_H_ #include #include #include -namespace wavemap { +namespace wavemap::print { template -inline std::string ToString(const SequenceContainerT& container) { +inline std::string container(const SequenceContainerT& container) { return std::accumulate(std::next(container.cbegin()), container.cend(), std::to_string(container[0]), [](auto str, const auto& el) -> std::string { return std::move(str) + ", " + std::to_string(el); }); } -} // namespace wavemap +} // namespace wavemap::print -#endif // WAVEMAP_UTILS_CONTAINER_PRINT_UTILS_H_ +#endif // WAVEMAP_UTILS_PRINT_CONTAINER_H_ diff --git a/libraries/wavemap/include/wavemap/utils/print/eigen.h b/libraries/wavemap/include/wavemap/utils/print/eigen.h new file mode 100644 index 000000000..d53cf4e19 --- /dev/null +++ b/libraries/wavemap/include/wavemap/utils/print/eigen.h @@ -0,0 +1,15 @@ +#ifndef WAVEMAP_UTILS_PRINT_EIGEN_H_ +#define WAVEMAP_UTILS_PRINT_EIGEN_H_ + +#include + +namespace wavemap::print::eigen { +template +static Eigen::WithFormat oneLine(const Derived& matrix) { + static const Eigen::IOFormat kOneLine = + Eigen::IOFormat(4, Eigen::DontAlignCols, ", ", ", ", "", "", " [", "]"); + return Eigen::WithFormat(matrix, kOneLine); +} +} // namespace wavemap::print::eigen + +#endif // WAVEMAP_UTILS_PRINT_EIGEN_H_ diff --git a/libraries/wavemap/include/wavemap/utils/query/map_interpolator.h b/libraries/wavemap/include/wavemap/utils/query/map_interpolator.h new file mode 100644 index 000000000..638d6dae2 --- /dev/null +++ b/libraries/wavemap/include/wavemap/utils/query/map_interpolator.h @@ -0,0 +1,59 @@ +#ifndef WAVEMAP_UTILS_QUERY_MAP_INTERPOLATOR_H_ +#define WAVEMAP_UTILS_QUERY_MAP_INTERPOLATOR_H_ + +#include "wavemap/common.h" +#include "wavemap/data_structure/volumetric/volumetric_data_structure_base.h" +#include "wavemap/indexing/index_conversions.h" +#include "wavemap/utils/data/eigen_checks.h" + +namespace wavemap::interpolate { +FloatingPoint trilinear(const wavemap::VolumetricDataStructureBase& map, + const wavemap::Point3D& position) { + const FloatingPoint cell_width = map.getMinCellWidth(); + const FloatingPoint cell_width_inv = 1.f / cell_width; + const auto min_corner_index = + wavemap::convert::pointToFloorIndex(position, cell_width_inv); + + // Gather the values of the 8 neighbors + std::array cube_corners{}; + for (int neighbor_idx = 0; neighbor_idx < 8; ++neighbor_idx) { + const Index3D offset{(neighbor_idx >> 2) & 1, (neighbor_idx >> 1) & 1, + neighbor_idx & 1}; + cube_corners[neighbor_idx] = map.getCellValue(min_corner_index + offset); + } + + // Compute the offset between the query point, the min corner and max corner + const Point3D position_min_corner = + wavemap::convert::indexToCenterPoint(min_corner_index, cell_width); + // Offset to min corner + const Vector3D a = (position - position_min_corner) * cell_width_inv; + DCHECK_EIGEN_GE(a, Vector3D::Constant(0.f - kEpsilon)); + DCHECK_EIGEN_LE(a, Vector3D::Constant(1.f + kEpsilon)); + // Offset to max corner + const Vector3D a_comp = 1.f - a.array(); + DCHECK_EIGEN_GE(a_comp, Vector3D::Constant(0.f - kEpsilon)); + DCHECK_EIGEN_LE(a_comp, Vector3D::Constant(1.f + kEpsilon)); + + // Interpolate out the first dimension, + // reducing the cube into a square that contains the query point + std::array plane_corners{}; + for (int corner_idx = 0; corner_idx < 4; ++corner_idx) { + plane_corners[corner_idx] = a_comp[0] * cube_corners[corner_idx] + + a[0] * cube_corners[corner_idx + 0b100]; + } + + // Interpolate out the second dimension, + // reducing the square into a line segment that contains the query point + std::array line_corners{}; + for (int side_idx = 0; side_idx < 2; ++side_idx) { + line_corners[side_idx] = a_comp[1] * plane_corners[side_idx] + + a[1] * plane_corners[side_idx + 0b10]; + } + + // Interpolate out the third dimension, + // reducing the line segment into a single value + return a_comp[2] * line_corners[0] + a[2] * line_corners[1]; +} +} // namespace wavemap::interpolate + +#endif // WAVEMAP_UTILS_QUERY_MAP_INTERPOLATOR_H_ diff --git a/libraries/wavemap/include/wavemap/utils/query/probability_conversions.h b/libraries/wavemap/include/wavemap/utils/query/probability_conversions.h new file mode 100644 index 000000000..01ff75a0b --- /dev/null +++ b/libraries/wavemap/include/wavemap/utils/query/probability_conversions.h @@ -0,0 +1,26 @@ +#ifndef WAVEMAP_UTILS_QUERY_PROBABILITY_CONVERSIONS_H_ +#define WAVEMAP_UTILS_QUERY_PROBABILITY_CONVERSIONS_H_ + +#include + +#include "wavemap/common.h" + +namespace wavemap::convert { +constexpr FloatingPoint kLogOddsNumericalMin = + std::numeric_limits::lowest(); +const FloatingPoint kLogOddsNumericalMax = + std::nextafter(std::log(std::numeric_limits::max()), 0.f); + +constexpr FloatingPoint logOddsToProbability(FloatingPoint log_odds) { + const FloatingPoint odds = std::exp(log_odds); + const FloatingPoint prob = odds / (1.f + odds); + return prob; +} + +constexpr FloatingPoint probabilityToLogOdds(FloatingPoint probability) { + const FloatingPoint odds = probability / (1.f - probability); + return std::log(odds); +} +} // namespace wavemap::convert + +#endif // WAVEMAP_UTILS_QUERY_PROBABILITY_CONVERSIONS_H_ diff --git a/libraries/wavemap/include/wavemap/utils/query/query_accelerator.h b/libraries/wavemap/include/wavemap/utils/query/query_accelerator.h new file mode 100644 index 000000000..74c7b1b32 --- /dev/null +++ b/libraries/wavemap/include/wavemap/utils/query/query_accelerator.h @@ -0,0 +1,117 @@ +#ifndef WAVEMAP_UTILS_QUERY_QUERY_ACCELERATOR_H_ +#define WAVEMAP_UTILS_QUERY_QUERY_ACCELERATOR_H_ + +#include +#include + +#include "wavemap/data_structure/volumetric/hashed_wavelet_octree.h" +#include "wavemap/indexing/index_hashes.h" + +namespace wavemap { +class QueryAccelerator { + public: + static constexpr int kDim = 3; + + explicit QueryAccelerator(const HashedWaveletOctree& map) + : block_map_(map.getBlocks()), tree_height_(map.getTreeHeight()) {} + + FloatingPoint getCellValue(const Index3D& index) { + return getCellValue(OctreeIndex{0, index}); + } + + FloatingPoint getCellValue(const OctreeIndex& index) { + // Remember previous query indices and compute new ones + const BlockIndex previous_block_index = block_index_; + const MortonIndex previous_morton_code = morton_code_; + const IndexElement previous_height = height_; + block_index_ = computeBlockIndexFromIndex(index); + morton_code_ = convert::nodeIndexToMorton(index); + + // Check whether we're in the same block as last time + if (block_index_ == previous_block_index) { + // Compute the last ancestor the current and previous query had in common + auto last_common_ancestor = OctreeIndex::computeLastCommonAncestorHeight( + morton_code_, index.height, previous_morton_code, previous_height); + height_ = last_common_ancestor; + DCHECK_LE(height_, tree_height_); + } else { + // Test if the queried block exists + if (block_map_.count(block_index_)) { + // If yes, load it + const auto& current_block = block_map_.at(block_index_); + node_stack_[tree_height_] = ¤t_block.getRootNode(); + value_stack_[tree_height_] = current_block.getRootScale(); + height_ = tree_height_; + } else { + // Otherwise return ignore this query and return 'unknown' + block_index_ = previous_block_index; + morton_code_ = previous_morton_code; + return 0.f; + } + } + + // If the requested value was already decompressed in the last query, return + if (height_ == index.height) { + return value_stack_[height_]; + } + + // Load the node at height_ if it was not yet loaded last time + if (previous_height != tree_height_ && height_ == previous_height) { + const HashedWaveletOctree::Block::NodeType* parent_node = + node_stack_[height_ + 1]; + const NdtreeIndexRelativeChild child_index = + OctreeIndex::computeRelativeChildIndex(morton_code_, height_ + 1); + if (!parent_node->hasChild(child_index)) { + return value_stack_[height_]; + } + node_stack_[height_] = parent_node->getChild(child_index); + } + + // Walk down the tree from height_ to index.height + while (true) { + const HashedWaveletOctree::Block::NodeType* parent_node = + node_stack_[height_]; + const FloatingPoint parent_value = value_stack_[height_]; + const NdtreeIndexRelativeChild child_idx = + OctreeIndex::computeRelativeChildIndex(morton_code_, height_); + --height_; + value_stack_[height_] = Transform::backwardSingleChild( + {parent_value, parent_node->data()}, child_idx); + if (height_ == index.height || !parent_node->hasChild(child_idx)) { + break; + } + node_stack_[height_] = parent_node->getChild(child_idx); + } + + return value_stack_[height_]; + } + + private: + using Coefficients = HaarCoefficients; + using Transform = HaarTransform; + using BlockIndex = Index3D; + using BlockMap = + std::unordered_map>; + using NodeType = NdtreeNode; + + const BlockMap& block_map_; + const IndexElement tree_height_; + + std::array> node_stack_{}; + std::array> value_stack_{}; + + Index3D block_index_ = + Index3D::Constant(std::numeric_limits::max()); + MortonIndex morton_code_ = std::numeric_limits::max(); + IndexElement height_ = tree_height_; + + BlockIndex computeBlockIndexFromIndex(const OctreeIndex& node_index) const { + const Index3D index = convert::nodeIndexToMinCornerIndex(node_index); + return int_math::div_exp2_floor(index, tree_height_); + } + + friend class QueryAcceleratorTest_Equivalence_Test; +}; +} // namespace wavemap + +#endif // WAVEMAP_UTILS_QUERY_QUERY_ACCELERATOR_H_ diff --git a/libraries/wavemap/include/wavemap/utils/stopwatch.h b/libraries/wavemap/include/wavemap/utils/time/stopwatch.h similarity index 54% rename from libraries/wavemap/include/wavemap/utils/stopwatch.h rename to libraries/wavemap/include/wavemap/utils/time/stopwatch.h index 6b552e4dc..f6bbcbc89 100644 --- a/libraries/wavemap/include/wavemap/utils/stopwatch.h +++ b/libraries/wavemap/include/wavemap/utils/time/stopwatch.h @@ -1,7 +1,7 @@ -#ifndef WAVEMAP_UTILS_STOPWATCH_H_ -#define WAVEMAP_UTILS_STOPWATCH_H_ +#ifndef WAVEMAP_UTILS_TIME_STOPWATCH_H_ +#define WAVEMAP_UTILS_TIME_STOPWATCH_H_ -#include "wavemap/utils/time.h" +#include "wavemap/utils/time/time.h" namespace wavemap { class Stopwatch { @@ -10,10 +10,10 @@ class Stopwatch { void stop(); double getLastEpisodeDuration() const { - return to_seconds(last_episode_duration_); + return time::to_seconds(last_episode_duration_); } double getTotalDuration() const { - return to_seconds(total_duration_); + return time::to_seconds(total_duration_); } private: @@ -25,4 +25,4 @@ class Stopwatch { }; } // namespace wavemap -#endif // WAVEMAP_UTILS_STOPWATCH_H_ +#endif // WAVEMAP_UTILS_TIME_STOPWATCH_H_ diff --git a/libraries/wavemap/include/wavemap/utils/time.h b/libraries/wavemap/include/wavemap/utils/time/time.h similarity index 69% rename from libraries/wavemap/include/wavemap/utils/time.h rename to libraries/wavemap/include/wavemap/utils/time/time.h index eada02045..418c1a08b 100644 --- a/libraries/wavemap/include/wavemap/utils/time.h +++ b/libraries/wavemap/include/wavemap/utils/time/time.h @@ -1,5 +1,5 @@ -#ifndef WAVEMAP_UTILS_TIME_H_ -#define WAVEMAP_UTILS_TIME_H_ +#ifndef WAVEMAP_UTILS_TIME_TIME_H_ +#define WAVEMAP_UTILS_TIME_TIME_H_ #include @@ -8,10 +8,12 @@ using Time = std::chrono::steady_clock; using Timestamp = std::chrono::time_point