From 80ccdca769be8e739aeef84837168e9398ecc906 Mon Sep 17 00:00:00 2001 From: Victor Reijgwart Date: Fri, 18 Aug 2023 20:22:32 +0200 Subject: [PATCH 01/70] Add option to work in devcontainer (incl. CLion support) --- .devcontainer/Dockerfile | 10 ++++++++++ .devcontainer/devcontainer.json | 9 +++++++++ .devcontainer/ros_env.list | 14 ++++++++++++++ 3 files changed, 33 insertions(+) create mode 100644 .devcontainer/Dockerfile create mode 100644 .devcontainer/devcontainer.json create mode 100644 .devcontainer/ros_env.list diff --git a/.devcontainer/Dockerfile b/.devcontainer/Dockerfile new file mode 100644 index 000000000..09a6c4c77 --- /dev/null +++ b/.devcontainer/Dockerfile @@ -0,0 +1,10 @@ +ARG VERSION=latest + +FROM ghcr.io/ethz-asl/wavemap:${VERSION} + +# Install curl [needed by CLion] +# hadolint ignore=DL3008 +RUN apt-get update && apt-get install -yq --no-install-recommends curl && rm -rf /var/lib/apt/lists/* + +# Build the package +RUN catkin build wavemap_all diff --git a/.devcontainer/devcontainer.json b/.devcontainer/devcontainer.json new file mode 100644 index 000000000..27295667c --- /dev/null +++ b/.devcontainer/devcontainer.json @@ -0,0 +1,9 @@ +{ + "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/ros_env.list"] +} diff --git a/.devcontainer/ros_env.list b/.devcontainer/ros_env.list new file mode 100644 index 000000000..5a9d001f5 --- /dev/null +++ b/.devcontainer/ros_env.list @@ -0,0 +1,14 @@ +# This is a workaround for CLion not allowing files to be sourced +# (~/catkin_ws/devel/setup.bash) before launching the remote IDE handler. +USER_HOME=/home/ci +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=${USER_HOME}/catkin_ws/src/catkin_simple:${USER_HOME}/catkin_ws/src/eigen_catkin:${USER_HOME}/catkin_ws/src/gflags_catkin:${USER_HOME}/catkin_ws/src/glog_catkin:${USER_HOME}/catkin_ws/src/eigen_checks:${USER_HOME}/catkin_ws/src/minkindr/minkindr:${USER_HOME}/catkin_ws/src/tracy_catkin:${USER_HOME}/catkin_ws/src/wavemap/libraries/wavemap:${USER_HOME}/catkin_ws/src/wavemap/tooling/packages/wavemap_all:${USER_HOME}/catkin_ws/src/wavemap/libraries/wavemap_io:${USER_HOME}/catkin_ws/src/wavemap/ros/wavemap_msgs:${USER_HOME}/catkin_ws/src/wavemap/ros/wavemap_ros_conversions:${USER_HOME}/catkin_ws/src/wavemap/ros/wavemap_ros:${USER_HOME}/catkin_ws/src/wavemap/ros/wavemap_rviz_plugin:${USER_HOME}/catkin_ws/src/wavemap/tooling/packages/wavemap_utils:/opt/ros/noetic/share +CMAKE_PREFIX_PATH=${USER_HOME}/catkin_ws/devel:/opt/ros/noetic +PKG_CONFIG_PATH=${USER_HOME}/catkin_ws/devel/lib/pkgconfig:/opt/ros/noetic/lib/pkgconfig +PYTHONPATH=${USER_HOME}/catkin_ws/devel/lib/python3/dist-packages:/opt/ros/noetic/lib/python3/dist-packages +LD_LIBRARY_PATH=${USER_HOME}/catkin_ws/devel/lib:/opt/ros/noetic/lib +PATH=${USER_HOME}/catkin_ws/devel/bin:/opt/ros/noetic/bin:/usr/lib/ccache:/usr/lib/ccache:${USER_HOME}/.local/bin:/usr/local/sbin:/usr/local/bin:/usr/sbin:/usr/bin:/sbin:/bin:/usr/games:/usr/local/games:/snap/bin:${USER_HOME}/repos/linter/bin From dbe2b8f89c882266dc308206217193394695dab7 Mon Sep 17 00:00:00 2001 From: Victor Reijgwart Date: Tue, 22 Aug 2023 11:20:05 +0200 Subject: [PATCH 02/70] Separate vscode and CLion devcontainer definitions --- .devcontainer/Dockerfile | 10 ------ .devcontainer/clion/Dockerfile | 39 +++++++++++++++++++++ .devcontainer/{ => clion}/devcontainer.json | 3 +- .devcontainer/clion/ros_env.list | 13 +++++++ .devcontainer/ros_env.list | 14 -------- .devcontainer/vscode/Dockerfile | 17 +++++++++ .devcontainer/vscode/devcontainer.json | 10 ++++++ 7 files changed, 81 insertions(+), 25 deletions(-) delete mode 100644 .devcontainer/Dockerfile create mode 100644 .devcontainer/clion/Dockerfile rename .devcontainer/{ => clion}/devcontainer.json (86%) create mode 100644 .devcontainer/clion/ros_env.list delete mode 100644 .devcontainer/ros_env.list create mode 100644 .devcontainer/vscode/Dockerfile create mode 100644 .devcontainer/vscode/devcontainer.json diff --git a/.devcontainer/Dockerfile b/.devcontainer/Dockerfile deleted file mode 100644 index 09a6c4c77..000000000 --- a/.devcontainer/Dockerfile +++ /dev/null @@ -1,10 +0,0 @@ -ARG VERSION=latest - -FROM ghcr.io/ethz-asl/wavemap:${VERSION} - -# Install curl [needed by CLion] -# hadolint ignore=DL3008 -RUN apt-get update && apt-get install -yq --no-install-recommends curl && rm -rf /var/lib/apt/lists/* - -# Build the package -RUN catkin build wavemap_all 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/devcontainer.json b/.devcontainer/clion/devcontainer.json similarity index 86% rename from .devcontainer/devcontainer.json rename to .devcontainer/clion/devcontainer.json index 27295667c..62f548285 100644 --- a/.devcontainer/devcontainer.json +++ b/.devcontainer/clion/devcontainer.json @@ -5,5 +5,6 @@ "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/ros_env.list"] + "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/ros_env.list b/.devcontainer/ros_env.list deleted file mode 100644 index 5a9d001f5..000000000 --- a/.devcontainer/ros_env.list +++ /dev/null @@ -1,14 +0,0 @@ -# This is a workaround for CLion not allowing files to be sourced -# (~/catkin_ws/devel/setup.bash) before launching the remote IDE handler. -USER_HOME=/home/ci -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=${USER_HOME}/catkin_ws/src/catkin_simple:${USER_HOME}/catkin_ws/src/eigen_catkin:${USER_HOME}/catkin_ws/src/gflags_catkin:${USER_HOME}/catkin_ws/src/glog_catkin:${USER_HOME}/catkin_ws/src/eigen_checks:${USER_HOME}/catkin_ws/src/minkindr/minkindr:${USER_HOME}/catkin_ws/src/tracy_catkin:${USER_HOME}/catkin_ws/src/wavemap/libraries/wavemap:${USER_HOME}/catkin_ws/src/wavemap/tooling/packages/wavemap_all:${USER_HOME}/catkin_ws/src/wavemap/libraries/wavemap_io:${USER_HOME}/catkin_ws/src/wavemap/ros/wavemap_msgs:${USER_HOME}/catkin_ws/src/wavemap/ros/wavemap_ros_conversions:${USER_HOME}/catkin_ws/src/wavemap/ros/wavemap_ros:${USER_HOME}/catkin_ws/src/wavemap/ros/wavemap_rviz_plugin:${USER_HOME}/catkin_ws/src/wavemap/tooling/packages/wavemap_utils:/opt/ros/noetic/share -CMAKE_PREFIX_PATH=${USER_HOME}/catkin_ws/devel:/opt/ros/noetic -PKG_CONFIG_PATH=${USER_HOME}/catkin_ws/devel/lib/pkgconfig:/opt/ros/noetic/lib/pkgconfig -PYTHONPATH=${USER_HOME}/catkin_ws/devel/lib/python3/dist-packages:/opt/ros/noetic/lib/python3/dist-packages -LD_LIBRARY_PATH=${USER_HOME}/catkin_ws/devel/lib:/opt/ros/noetic/lib -PATH=${USER_HOME}/catkin_ws/devel/bin:/opt/ros/noetic/bin:/usr/lib/ccache:/usr/lib/ccache:${USER_HOME}/.local/bin:/usr/local/sbin:/usr/local/bin:/usr/sbin:/usr/bin:/sbin:/bin:/usr/games:/usr/local/games:/snap/bin:${USER_HOME}/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" +} From d2a9c432c2eaafe9ca25852c46ba7c63187ccd8e Mon Sep 17 00:00:00 2001 From: Victor Reijgwart Date: Thu, 28 Sep 2023 15:02:14 +0200 Subject: [PATCH 03/70] Replace stack with recursion (faster and easier to read) --- .../hashed_chunked_wavelet_integrator.h | 9 + .../hashed_chunked_wavelet_integrator_inl.h | 23 ++ .../hashed_chunked_wavelet_integrator.cc | 199 +++++++----------- 3 files changed, 108 insertions(+), 123 deletions(-) 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..6c136c940 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 @@ -55,6 +55,15 @@ 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); + 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/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..494d60f0c 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,29 @@ 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) { + auto child_values = HashedChunkedWaveletOctreeBlock::Transform::backward( + {parent_value, parent_details}); + for (NdtreeIndexRelativeChild relative_child_idx = 0; + relative_child_idx < OctreeIndex::kNumChildren; ++relative_child_idx) { + const auto child_index = parent_index.computeChildIndex(relative_child_idx); + FloatingPoint& child_value = child_values[relative_child_idx]; + const Point3D W_child_center = + convert::nodeIndexToCenterPoint(child_index, min_cell_width_); + const Point3D C_child_center = + posed_range_image_->getPoseInverse() * W_child_center; + const FloatingPoint sample = computeUpdate(C_child_center); + child_value = std::clamp(sample + child_value, min_log_odds_padded_, + max_log_odds_padded_); + } + 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/src/integrator/projective/coarse_to_fine/hashed_chunked_wavelet_integrator.cc b/libraries/wavemap/src/integrator/projective/coarse_to_fine/hashed_chunked_wavelet_integrator.cc index bfae287ec..1e38f71b7 100644 --- a/libraries/wavemap/src/integrator/projective/coarse_to_fine/hashed_chunked_wavelet_integrator.cc +++ b/libraries/wavemap/src/integrator/projective/coarse_to_fine/hashed_chunked_wavelet_integrator.cc @@ -68,99 +68,39 @@ void HashedChunkedWaveletIntegrator::updateBlock( HashedChunkedWaveletOctree::Block& block, const HashedChunkedWaveletOctree::BlockIndex& block_index) { ZoneScoped; + block.setNeedsThresholding(); block.setNeedsPruning(); block.setLastUpdatedStamp(); - struct StackElement { - HashedChunkedWaveletOctreeBlock::NodeChunkType& parent_chunk; - const OctreeIndex parent_node_index; - NdtreeIndexRelativeChild next_child_idx; - HashedChunkedWaveletOctreeBlock::Coefficients::CoefficientsArray - child_scale_coefficients; - bool has_nonzero_child; - }; - std::stack> stack; - stack.emplace(StackElement{ - block.getRootChunk(), OctreeIndex{tree_height_, block_index}, 0, - HashedChunkedWaveletOctreeBlock::Transform::backward( - {block.getRootScale(), block.getRootChunk().nodeData(0u)}), - block.getRootChunk().nodeHasAtLeastOneChild(0u)}); - - while (!stack.empty()) { - // If the current stack element has fully been processed, propagate upward - if (OctreeIndex::kNumChildren <= stack.top().next_child_idx) { - const auto [new_scale, new_details] = - HashedChunkedWaveletOctreeBlock::Transform::forward( - stack.top().child_scale_coefficients); - const MortonIndex morton_code = - convert::nodeIndexToMorton(stack.top().parent_node_index); - const int parent_height = stack.top().parent_node_index.height; - const int chunk_top_height = - chunk_height_ * int_math::div_round_up(parent_height, chunk_height_); - const LinearIndex relative_node_index = - OctreeIndex::computeTreeTraversalDistance( - morton_code, chunk_top_height, parent_height); - stack.top().parent_chunk.nodeData(relative_node_index) = new_details; - stack.top().parent_chunk.nodeHasAtLeastOneChild(relative_node_index) = - stack.top().has_nonzero_child; - const bool is_nonzero_child = - stack.top().has_nonzero_child || data_utils::is_nonzero(new_details); - stack.pop(); - if (stack.empty()) { - block.getRootScale() = new_scale; - return; - } else { - const NdtreeIndexRelativeChild current_child_idx = - stack.top().next_child_idx - 1; - stack.top().child_scale_coefficients[current_child_idx] = new_scale; - stack.top().has_nonzero_child |= is_nonzero_child; - continue; - } - } - - // If we're at the leaf level, directly compute the update for all children - if (stack.top().parent_node_index.height == - config_.termination_height + 1) { - DCHECK_EQ(stack.top().next_child_idx, 0); - for (NdtreeIndexRelativeChild relative_child_idx = 0; - relative_child_idx < OctreeIndex::kNumChildren; - ++relative_child_idx) { - const OctreeIndex node_index = - stack.top().parent_node_index.computeChildIndex(relative_child_idx); - FloatingPoint& node_value = - stack.top().child_scale_coefficients[relative_child_idx]; - const Point3D W_node_center = - convert::nodeIndexToCenterPoint(node_index, min_cell_width_); - const Point3D C_node_center = - posed_range_image_->getPoseInverse() * W_node_center; - const FloatingPoint sample = computeUpdate(C_node_center); - node_value = std::clamp(sample + node_value, min_log_odds_padded_, - max_log_odds_padded_); - } - stack.top().next_child_idx = OctreeIndex::kNumChildren; - continue; - } - - // Otherwise, handle the stack element's children one by one - // Get the active child - const NdtreeIndexRelativeChild current_child_idx = - stack.top().next_child_idx; - ++stack.top().next_child_idx; - DCHECK_GE(current_child_idx, 0); - DCHECK_LT(current_child_idx, OctreeIndex::kNumChildren); + const OctreeIndex root_node_index{tree_height_, block_index}; + updateNodeRecursive(block.getRootChunk(), root_node_index, 0u, + block.getRootScale(), + block.getRootChunk().nodeHasAtLeastOneChild(0u)); +} - const OctreeIndex node_index = - stack.top().parent_node_index.computeChildIndex(current_child_idx); - FloatingPoint& node_value = - stack.top().child_scale_coefficients[current_child_idx]; - DCHECK_GE(node_index.height, 0); +void HashedChunkedWaveletIntegrator::updateNodeRecursive( // NOLINT + HashedChunkedWaveletOctreeBlock::NodeChunkType& parent_chunk, + const OctreeIndex& parent_node_index, LinearIndex parent_in_chunk_index, + FloatingPoint& parent_value, + HashedChunkedWaveletOctreeBlock::NodeChunkType::BitRef parent_has_child) { + auto& parent_details = parent_chunk.nodeData(parent_in_chunk_index); + auto child_values = HashedChunkedWaveletOctreeBlock::Transform::backward( + {parent_value, parent_details}); + + // Handle all the children + for (NdtreeIndexRelativeChild relative_child_idx = 0; + relative_child_idx < OctreeIndex::kNumChildren; ++relative_child_idx) { + const OctreeIndex child_index = + parent_node_index.computeChildIndex(relative_child_idx); + const int child_height = child_index.height; + FloatingPoint& child_value = child_values[relative_child_idx]; // Test whether it is fully occupied; free or unknown; or fully unknown - const AABB W_cell_aabb = - convert::nodeIndexToAABB(node_index, min_cell_width_); + const AABB W_child_aabb = + convert::nodeIndexToAABB(child_index, min_cell_width_); const UpdateType update_type = range_image_intersector_->determineUpdateType( - W_cell_aabb, posed_range_image_->getRotationMatrixInverse(), + W_child_aabb, posed_range_image_->getRotationMatrixInverse(), posed_range_image_->getOrigin()); // If we're fully in unknown space, @@ -172,63 +112,76 @@ void HashedChunkedWaveletIntegrator::updateBlock( // We can also stop here if the cell will result in a free space update // (or zero) and the map is already saturated free if (update_type != UpdateType::kPossiblyOccupied && - node_value < min_log_odds_shrunk_) { + child_value < min_log_odds_shrunk_) { continue; } // Test if the worst-case error for the intersection type at the current // resolution falls within the acceptable approximation error - const FloatingPoint node_width = W_cell_aabb.width<0>(); - const Point3D W_node_center = - W_cell_aabb.min + Vector3D::Constant(node_width / 2.f); - const Point3D C_node_center = - posed_range_image_->getPoseInverse() * W_node_center; - const FloatingPoint d_C_cell = - projection_model_->cartesianToSensorZ(C_node_center); + const FloatingPoint child_width = W_child_aabb.width<0>(); + const Point3D W_child_center = + W_child_aabb.min + Vector3D::Constant(child_width / 2.f); + const Point3D C_child_center = + posed_range_image_->getPoseInverse() * W_child_center; + const FloatingPoint d_C_child = + projection_model_->cartesianToSensorZ(C_child_center); const FloatingPoint bounding_sphere_radius = - kUnitCubeHalfDiagonal * node_width; + kUnitCubeHalfDiagonal * child_width; if (measurement_model_->computeWorstCaseApproximationError( - update_type, d_C_cell, bounding_sphere_radius) < + update_type, d_C_child, bounding_sphere_radius) < config_.termination_update_error) { - const FloatingPoint sample = computeUpdate(C_node_center); - node_value += sample; - // TODO(victorr) Threshold directly if child has no children - block.setNeedsThresholding(); + const FloatingPoint sample = computeUpdate(C_child_center); + child_value += sample; continue; } // Since the approximation error would still be too big, refine - const MortonIndex morton_code = convert::nodeIndexToMorton(node_index); - const int node_height = node_index.height; - auto& parent_chunk = stack.top().parent_chunk; - const int parent_height = node_height + 1; + const MortonIndex morton_code = convert::nodeIndexToMorton(child_index); + const int parent_height = child_height + 1; const int parent_chunk_top_height = chunk_height_ * int_math::div_round_up(parent_height, chunk_height_); - if (node_height % chunk_height_ != 0) { - const LinearIndex relative_node_index = - OctreeIndex::computeTreeTraversalDistance( - morton_code, parent_chunk_top_height, node_height); - stack.emplace(StackElement{ - parent_chunk, node_index, 0, - HashedChunkedWaveletOctreeBlock::Transform::backward( - {node_value, parent_chunk.nodeData(relative_node_index)}), - parent_chunk.nodeHasAtLeastOneChild(relative_node_index)}); + + HashedChunkedWaveletOctreeBlock::NodeChunkType* chunk_containing_child; + LinearIndex child_node_in_chunk_index; + if (child_height % chunk_height_ != 0) { + chunk_containing_child = &parent_chunk; + child_node_in_chunk_index = OctreeIndex::computeTreeTraversalDistance( + morton_code, parent_chunk_top_height, child_height); } else { const LinearIndex linear_child_index = OctreeIndex::computeLevelTraversalDistance( - morton_code, parent_chunk_top_height, node_height); - HashedChunkedWaveletOctreeBlock::NodeChunkType* child_chunk = - parent_chunk.getChild(linear_child_index); - if (!child_chunk) { - child_chunk = parent_chunk.allocateChild(linear_child_index); + morton_code, parent_chunk_top_height, child_height); + chunk_containing_child = parent_chunk.getChild(linear_child_index); + if (!chunk_containing_child) { + chunk_containing_child = parent_chunk.allocateChild(linear_child_index); } - constexpr LinearIndex kRelativeNodeIndex = 0u; - stack.emplace(StackElement{ - *child_chunk, node_index, 0, - HashedChunkedWaveletOctreeBlock::Transform::backward( - {node_value, child_chunk->nodeData(kRelativeNodeIndex)}), - child_chunk->nodeHasAtLeastOneChild(kRelativeNodeIndex)}); + child_node_in_chunk_index = 0u; + } + + auto& child_details = + chunk_containing_child->nodeData(child_node_in_chunk_index); + auto child_has_children = chunk_containing_child->nodeHasAtLeastOneChild( + child_node_in_chunk_index); + + // If we're at the leaf level, directly compute the update + if (child_height == config_.termination_height + 1) { + updateLeavesBatch(child_index, child_value, child_details); + } else { + // Otherwise, recurse + DCHECK_GE(child_height, 0); + updateNodeRecursive(*chunk_containing_child, child_index, + child_node_in_chunk_index, child_value, + child_has_children); + } + + if (child_has_children || data_utils::is_nonzero(child_details)) { + parent_has_child = true; } } + + const auto [new_value, new_details] = + HashedChunkedWaveletOctreeBlock::Transform::forward(child_values); + parent_details = new_details; + parent_value = new_value; } } // namespace wavemap From cf2c97993ed0771d800834a90d730f3e2207a837 Mon Sep 17 00:00:00 2001 From: Victor Reijgwart Date: Thu, 28 Sep 2023 15:50:47 +0200 Subject: [PATCH 04/70] Add tests for map to ROS msg conversions --- ros/wavemap_ros_conversions/CMakeLists.txt | 8 + .../test/src/test_map_msg_conversions.cc | 148 ++++++++++++++++++ 2 files changed, 156 insertions(+) create mode 100644 ros/wavemap_ros_conversions/test/src/test_map_msg_conversions.cc diff --git a/ros/wavemap_ros_conversions/CMakeLists.txt b/ros/wavemap_ros_conversions/CMakeLists.txt index 05f7fcfd2..16b42ef4d 100644 --- a/ros/wavemap_ros_conversions/CMakeLists.txt +++ b/ros/wavemap_ros_conversions/CMakeLists.txt @@ -13,6 +13,14 @@ cs_add_library(${PROJECT_NAME} src/map_msg_conversions.cc src/time_conversions.cc) +# Tests +if (CATKIN_ENABLE_TESTING) + catkin_add_gtest( + test_${PROJECT_NAME} + test/src/test_map_msg_conversions.cc) + target_link_libraries(test_${PROJECT_NAME} ${PROJECT_NAME} gtest_main) +endif () + # Export cs_install() cs_export() diff --git a/ros/wavemap_ros_conversions/test/src/test_map_msg_conversions.cc b/ros/wavemap_ros_conversions/test/src/test_map_msg_conversions.cc new file mode 100644 index 000000000..b677f63cd --- /dev/null +++ b/ros/wavemap_ros_conversions/test/src/test_map_msg_conversions.cc @@ -0,0 +1,148 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "wavemap_ros_conversions/map_msg_conversions.h" + +namespace wavemap { +template +class FileConversionsTest : public FixtureBase, + public GeometryGenerator, + public ConfigGenerator { + protected: + void SetUp() override { + ros::Time::init(); + stamp = ros::Time::now(); + } + + const std::string frame_id = "odom"; + ros::Time stamp{}; + static constexpr FloatingPoint kAcceptableReconstructionError = 5e-2f; +}; + +using VolumetricDataStructureTypes = + ::testing::Types; +TYPED_TEST_SUITE(FileConversionsTest, VolumetricDataStructureTypes, ); + +TYPED_TEST(FileConversionsTest, MetadataPreservation) { + const auto config = + ConfigGenerator::getRandomConfig(); + + // Create the original map and make sure it matches the config + typename TypeParam::ConstPtr map = std::make_shared(config); + ASSERT_EQ(map->getMinCellWidth(), config.min_cell_width); + ASSERT_EQ(map->getMinLogOdds(), config.min_log_odds); + ASSERT_EQ(map->getMaxLogOdds(), config.max_log_odds); + ASSERT_EQ(map->getTreeHeight(), config.tree_height); + + // Convert to base pointer + VolumetricDataStructureBase::ConstPtr map_base = map; + ASSERT_EQ(map_base->getMinCellWidth(), config.min_cell_width); + ASSERT_EQ(map_base->getMinLogOdds(), config.min_log_odds); + ASSERT_EQ(map_base->getMaxLogOdds(), config.max_log_odds); + + // Serialize and deserialize + wavemap_msgs::Map map_msg; + ASSERT_TRUE(convert::mapToRosMsg(*map_base, TestFixture::frame_id, + TestFixture::stamp, map_msg)); + VolumetricDataStructureBase::Ptr map_base_round_trip; + ASSERT_TRUE(convert::rosMsgToMap(map_msg, map_base_round_trip)); + ASSERT_TRUE(map_base_round_trip); + + // Check the header + EXPECT_EQ(map_msg.header.frame_id, TestFixture::frame_id); + EXPECT_EQ(map_msg.header.stamp, TestFixture::stamp); + + // TODO(victorr): Add option to deserialize into hashed chunked wavelet + // octrees, instead of implicitly converting them to regular + // hashed wavelet octrees. + if (std::is_same_v) { + HashedWaveletOctree::ConstPtr map_round_trip = + std::dynamic_pointer_cast(map_base_round_trip); + ASSERT_TRUE(map_round_trip); + + // Check that the metadata still matches the original config + EXPECT_EQ(map_round_trip->getMinCellWidth(), config.min_cell_width); + EXPECT_EQ(map_round_trip->getMinLogOdds(), config.min_log_odds); + EXPECT_EQ(map_round_trip->getMaxLogOdds(), config.max_log_odds); + EXPECT_EQ(map_round_trip->getTreeHeight(), config.tree_height); + } else { + typename TypeParam::ConstPtr map_round_trip = + std::dynamic_pointer_cast(map_base_round_trip); + ASSERT_TRUE(map_round_trip); + + // Check that the metadata still matches the original config + EXPECT_EQ(map_round_trip->getMinCellWidth(), config.min_cell_width); + EXPECT_EQ(map_round_trip->getMinLogOdds(), config.min_log_odds); + EXPECT_EQ(map_round_trip->getMaxLogOdds(), config.max_log_odds); + EXPECT_EQ(map_round_trip->getTreeHeight(), config.tree_height); + } +} + +TYPED_TEST(FileConversionsTest, InsertionAndLeafVisitor) { + constexpr int kNumRepetitions = 3; + for (int i = 0; i < kNumRepetitions; ++i) { + // Create a random map + const auto config = + ConfigGenerator::getRandomConfig(); + TypeParam map_original(config); + const std::vector random_indices = + GeometryGenerator::getRandomIndexVector<3>( + 1000u, 2000u, Index3D::Constant(-5000), Index3D::Constant(5000)); + for (const Index3D& index : random_indices) { + const FloatingPoint update = TestFixture::getRandomUpdate(); + map_original.addToCellValue(index, update); + } + map_original.prune(); + + // Serialize and deserialize + wavemap_msgs::Map map_msg; + ASSERT_TRUE(convert::mapToRosMsg(map_original, TestFixture::frame_id, + TestFixture::stamp, map_msg)); + VolumetricDataStructureBase::Ptr map_base_round_trip; + ASSERT_TRUE(convert::rosMsgToMap(map_msg, map_base_round_trip)); + ASSERT_TRUE(map_base_round_trip); + + // Check that both maps contain the same leaves + map_base_round_trip->forEachLeaf( + [&map_original](const OctreeIndex& node_index, + FloatingPoint round_trip_value) { + EXPECT_NEAR(round_trip_value, map_original.getCellValue(node_index), + TestFixture::kAcceptableReconstructionError); + }); + + // TODO(victorr): Remove this special case once deserializing directly + // into HashedChunkedWaveletOctrees is supported + if (std::is_same_v) { + HashedWaveletOctree::ConstPtr map_round_trip = + std::dynamic_pointer_cast(map_base_round_trip); + ASSERT_TRUE(map_round_trip); + + map_original.forEachLeaf([&map_round_trip](const OctreeIndex& node_index, + FloatingPoint original_value) { + EXPECT_NEAR(original_value, map_round_trip->getCellValue(node_index), + TestFixture::kAcceptableReconstructionError); + }); + } else { + typename TypeParam::ConstPtr map_round_trip = + std::dynamic_pointer_cast(map_base_round_trip); + ASSERT_TRUE(map_round_trip); + + map_original.forEachLeaf([&map_round_trip](const OctreeIndex& node_index, + FloatingPoint original_value) { + EXPECT_NEAR(original_value, map_round_trip->getCellValue(node_index), + TestFixture::kAcceptableReconstructionError); + }); + } + } +} +} // namespace wavemap From 5e7c5b78bfff77a44a7d71aa12e972d88ade3555 Mon Sep 17 00:00:00 2001 From: Victor Reijgwart Date: Thu, 28 Sep 2023 20:57:47 +0200 Subject: [PATCH 05/70] Use a single thread pool for all integrators --- .../wavemap/integrator/integrator_factory.h | 6 +++++- .../hashed_chunked_wavelet_integrator.h | 8 +++++--- .../coarse_to_fine/hashed_wavelet_integrator.h | 8 +++++--- .../src/integrator/integrator_factory.cc | 15 +++++++++------ .../hashed_chunked_wavelet_integrator.cc | 4 ++-- .../hashed_wavelet_integrator.cc | 4 ++-- .../input_handler/depth_image_input_handler.h | 1 + .../wavemap_ros/input_handler/input_handler.h | 2 ++ .../input_handler/input_handler_factory.h | 7 +++++-- .../input_handler/pointcloud_input_handler.h | 1 + .../include/wavemap_ros/wavemap_server.h | 9 ++++++++- .../input_handler/depth_image_input_handler.cc | 7 ++++--- .../src/input_handler/input_handler.cc | 3 ++- .../src/input_handler/input_handler_factory.cc | 18 ++++++++++++------ .../input_handler/pointcloud_input_handler.cc | 6 ++++-- ros/wavemap_ros/src/wavemap_server.cc | 12 ++++++++---- 16 files changed, 75 insertions(+), 36 deletions(-) diff --git a/libraries/wavemap/include/wavemap/integrator/integrator_factory.h b/libraries/wavemap/include/wavemap/integrator/integrator_factory.h index 9be0f08a5..0758727b1 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, 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); }; } // 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 6c136c940..67943714d 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,19 @@ 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) : 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_(std::move(thread_pool)) {} 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 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..6cb406509 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,19 @@ 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) : 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_(std::move(thread_pool)) {} 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/src/integrator/integrator_factory.cc b/libraries/wavemap/src/integrator/integrator_factory.cc index e29e24236..b2d21702b 100644 --- a/libraries/wavemap/src/integrator/integrator_factory.cc +++ b/libraries/wavemap/src/integrator/integrator_factory.cc @@ -13,17 +13,19 @@ namespace wavemap { IntegratorBase::Ptr IntegratorFactory::create( const param::Value& params, VolumetricDataStructureBase::Ptr occupancy_map, + std::shared_ptr thread_pool, std::optional default_integrator_type) { if (const auto type = IntegratorType::from(params, "integration_method"); type) { - return create(type.value(), params, std::move(occupancy_map)); + return create(type.value(), params, std::move(occupancy_map), + std::move(thread_pool)); } if (default_integrator_type.has_value()) { LOG(WARNING) << "Default type \"" << default_integrator_type.value().toStr() << "\" will be created instead."; return create(default_integrator_type.value(), params, - std::move(occupancy_map)); + std::move(occupancy_map), std::move(thread_pool)); } LOG(ERROR) << "No default was set. Returning nullptr."; @@ -32,7 +34,8 @@ IntegratorBase::Ptr IntegratorFactory::create( IntegratorBase::Ptr IntegratorFactory::create( IntegratorType integrator_type, const param::Value& params, - VolumetricDataStructureBase::Ptr occupancy_map) { + VolumetricDataStructureBase::Ptr occupancy_map, + std::shared_ptr thread_pool) { // If we're using a ray tracing based integrator, we can build it directly if (integrator_type == IntegratorType::kRayTracingIntegrator) { if (const auto config = @@ -123,8 +126,8 @@ IntegratorBase::Ptr IntegratorFactory::create( if (hashed_wavelet_map) { return std::make_shared( integrator_config.value(), projection_model, posed_range_image, - beam_offset_image, measurement_model, - std::move(hashed_wavelet_map)); + beam_offset_image, measurement_model, std::move(hashed_wavelet_map), + std::move(thread_pool)); } else { LOG(ERROR) << "Integrator of type " << integrator_type.toStr() << " only supports data structures of type " @@ -141,7 +144,7 @@ IntegratorBase::Ptr IntegratorFactory::create( return std::make_shared( integrator_config.value(), projection_model, posed_range_image, beam_offset_image, measurement_model, - std::move(hashed_chunked_wavelet_map)); + std::move(hashed_chunked_wavelet_map), std::move(thread_pool)); } else { LOG(ERROR) << "Integrator of type " << integrator_type.toStr() diff --git a/libraries/wavemap/src/integrator/projective/coarse_to_fine/hashed_chunked_wavelet_integrator.cc b/libraries/wavemap/src/integrator/projective/coarse_to_fine/hashed_chunked_wavelet_integrator.cc index 1e38f71b7..598a003bd 100644 --- a/libraries/wavemap/src/integrator/projective/coarse_to_fine/hashed_chunked_wavelet_integrator.cc +++ b/libraries/wavemap/src/integrator/projective/coarse_to_fine/hashed_chunked_wavelet_integrator.cc @@ -35,12 +35,12 @@ void HashedChunkedWaveletIntegrator::updateMap() { // Update it with the threadpool for (const auto& block_index : blocks_to_update) { - thread_pool_.add_task([this, block_index]() { + thread_pool_->add_task([this, block_index]() { auto& block = occupancy_map_->getBlock(block_index); updateBlock(block, block_index); }); } - thread_pool_.wait_all(); + thread_pool_->wait_all(); } std::pair diff --git a/libraries/wavemap/src/integrator/projective/coarse_to_fine/hashed_wavelet_integrator.cc b/libraries/wavemap/src/integrator/projective/coarse_to_fine/hashed_wavelet_integrator.cc index 96f1e025d..cfc560448 100644 --- a/libraries/wavemap/src/integrator/projective/coarse_to_fine/hashed_wavelet_integrator.cc +++ b/libraries/wavemap/src/integrator/projective/coarse_to_fine/hashed_wavelet_integrator.cc @@ -35,12 +35,12 @@ void HashedWaveletIntegrator::updateMap() { // Update it with the threadpool for (const auto& block_index : blocks_to_update) { - thread_pool_.add_task([this, block_index]() { + thread_pool_->add_task([this, block_index]() { auto& block = occupancy_map_->getBlock(block_index.position); updateBlock(block, block_index); }); } - thread_pool_.wait_all(); + thread_pool_->wait_all(); } std::pair diff --git a/ros/wavemap_ros/include/wavemap_ros/input_handler/depth_image_input_handler.h b/ros/wavemap_ros/include/wavemap_ros/input_handler/depth_image_input_handler.h index f51730ac4..696e8bdb0 100644 --- a/ros/wavemap_ros/include/wavemap_ros/input_handler/depth_image_input_handler.h +++ b/ros/wavemap_ros/include/wavemap_ros/input_handler/depth_image_input_handler.h @@ -72,6 +72,7 @@ class DepthImageInputHandler : public InputHandler { const param::Value& params, std::string world_frame, VolumetricDataStructureBase::Ptr occupancy_map, std::shared_ptr transformer, + std::shared_ptr thread_pool, ros::NodeHandle nh, ros::NodeHandle nh_private); InputHandlerType getType() const override { diff --git a/ros/wavemap_ros/include/wavemap_ros/input_handler/input_handler.h b/ros/wavemap_ros/include/wavemap_ros/input_handler/input_handler.h index 77e245ab8..4b637a1c6 100644 --- a/ros/wavemap_ros/include/wavemap_ros/input_handler/input_handler.h +++ b/ros/wavemap_ros/include/wavemap_ros/input_handler/input_handler.h @@ -13,6 +13,7 @@ #include #include #include +#include #include "wavemap_ros/tf_transformer.h" @@ -59,6 +60,7 @@ class InputHandler { std::string world_frame, VolumetricDataStructureBase::Ptr occupancy_map, std::shared_ptr transformer, + std::shared_ptr thread_pool, const ros::NodeHandle& nh, ros::NodeHandle nh_private); virtual ~InputHandler() = default; diff --git a/ros/wavemap_ros/include/wavemap_ros/input_handler/input_handler_factory.h b/ros/wavemap_ros/include/wavemap_ros/input_handler/input_handler_factory.h index 23e8bd2ad..9089c1f0c 100644 --- a/ros/wavemap_ros/include/wavemap_ros/input_handler/input_handler_factory.h +++ b/ros/wavemap_ros/include/wavemap_ros/input_handler/input_handler_factory.h @@ -4,6 +4,7 @@ #include #include +#include "wavemap/utils/thread_pool.h" #include "wavemap_ros/input_handler/input_handler.h" namespace wavemap { @@ -12,7 +13,8 @@ class InputHandlerFactory { static std::unique_ptr create( const param::Value& params, std::string world_frame, VolumetricDataStructureBase::Ptr occupancy_map, - std::shared_ptr transformer, ros::NodeHandle nh, + std::shared_ptr transformer, + std::shared_ptr thread_pool, ros::NodeHandle nh, ros::NodeHandle nh_private, std::optional default_input_handler_type = std::nullopt); @@ -20,7 +22,8 @@ class InputHandlerFactory { static std::unique_ptr create( InputHandlerType input_handler_type, const param::Value& params, std::string world_frame, VolumetricDataStructureBase::Ptr occupancy_map, - std::shared_ptr transformer, ros::NodeHandle nh, + std::shared_ptr transformer, + std::shared_ptr thread_pool, ros::NodeHandle nh, ros::NodeHandle nh_private); }; } // namespace wavemap diff --git a/ros/wavemap_ros/include/wavemap_ros/input_handler/pointcloud_input_handler.h b/ros/wavemap_ros/include/wavemap_ros/input_handler/pointcloud_input_handler.h index 43d917f48..a8009b9ce 100644 --- a/ros/wavemap_ros/include/wavemap_ros/input_handler/pointcloud_input_handler.h +++ b/ros/wavemap_ros/include/wavemap_ros/input_handler/pointcloud_input_handler.h @@ -86,6 +86,7 @@ class PointcloudInputHandler : public InputHandler { const param::Value& params, std::string world_frame, VolumetricDataStructureBase::Ptr occupancy_map, std::shared_ptr transformer, + std::shared_ptr thread_pool, ros::NodeHandle nh, ros::NodeHandle nh_private); InputHandlerType getType() const override { diff --git a/ros/wavemap_ros/include/wavemap_ros/wavemap_server.h b/ros/wavemap_ros/include/wavemap_ros/wavemap_server.h index ab996a00b..7ffe44661 100644 --- a/ros/wavemap_ros/include/wavemap_ros/wavemap_server.h +++ b/ros/wavemap_ros/include/wavemap_ros/wavemap_server.h @@ -1,6 +1,7 @@ #ifndef WAVEMAP_ROS_WAVEMAP_SERVER_H_ #define WAVEMAP_ROS_WAVEMAP_SERVER_H_ +#include #include #include #include @@ -15,6 +16,7 @@ #include #include #include +#include #include #include "wavemap_ros/input_handler/input_handler.h" @@ -24,7 +26,7 @@ namespace wavemap { /** * Config struct for wavemap's ROS server. */ -struct WavemapServerConfig : ConfigBase { +struct WavemapServerConfig : ConfigBase { //! Name of the coordinate frame in which to store the map. //! Will be used as the frame_id for ROS TF lookups. std::string world_frame = "odom"; @@ -41,6 +43,10 @@ struct WavemapServerConfig : ConfigBase { //! Used to control the maximum message size. Only works in combination with //! hash-based map data structures. int max_num_blocks_per_msg = 1000; + //! Maximum number of threads to use. + //! Defaults to the number of threads supported by the CPU. + int num_threads = + std::max(1, static_cast(std::thread::hardware_concurrency())); static MemberMap memberMap; @@ -71,6 +77,7 @@ class WavemapServer { VolumetricDataStructureBase::Ptr occupancy_map_; std::shared_ptr transformer_; + std::shared_ptr thread_pool_; std::vector> input_handlers_; void subscribeToTimers(const ros::NodeHandle& nh); diff --git a/ros/wavemap_ros/src/input_handler/depth_image_input_handler.cc b/ros/wavemap_ros/src/input_handler/depth_image_input_handler.cc index 134c20eef..9a0d8d663 100644 --- a/ros/wavemap_ros/src/input_handler/depth_image_input_handler.cc +++ b/ros/wavemap_ros/src/input_handler/depth_image_input_handler.cc @@ -33,11 +33,12 @@ bool DepthImageInputHandlerConfig::isValid(bool verbose) const { DepthImageInputHandler::DepthImageInputHandler( const DepthImageInputHandlerConfig& config, const param::Value& params, std::string world_frame, VolumetricDataStructureBase::Ptr occupancy_map, - std::shared_ptr transformer, ros::NodeHandle nh, + std::shared_ptr transformer, + std::shared_ptr thread_pool, ros::NodeHandle nh, ros::NodeHandle nh_private) : InputHandler(config, params, std::move(world_frame), - std::move(occupancy_map), std::move(transformer), nh, - nh_private), + std::move(occupancy_map), std::move(transformer), + std::move(thread_pool), nh, nh_private), config_(config.checkValid()) { // Get pointers to the underlying scanwise integrators for (const auto& integrator : integrators_) { diff --git a/ros/wavemap_ros/src/input_handler/input_handler.cc b/ros/wavemap_ros/src/input_handler/input_handler.cc index 8a5c23deb..6fc961dba 100644 --- a/ros/wavemap_ros/src/input_handler/input_handler.cc +++ b/ros/wavemap_ros/src/input_handler/input_handler.cc @@ -31,6 +31,7 @@ InputHandler::InputHandler(const InputHandlerConfig& config, const param::Value& params, std::string world_frame, VolumetricDataStructureBase::Ptr occupancy_map, std::shared_ptr transformer, + std::shared_ptr thread_pool, const ros::NodeHandle& nh, ros::NodeHandle nh_private) : config_(config.checkValid()), @@ -51,7 +52,7 @@ InputHandler::InputHandler(const InputHandlerConfig& config, } for (const auto& integrator_params : integrators_array.value()) { auto integrator = - IntegratorFactory::create(integrator_params, occupancy_map, + IntegratorFactory::create(integrator_params, occupancy_map, thread_pool, IntegratorType::kRayTracingIntegrator); CHECK_NOTNULL(integrator); integrators_.emplace_back(std::move(integrator)); diff --git a/ros/wavemap_ros/src/input_handler/input_handler_factory.cc b/ros/wavemap_ros/src/input_handler/input_handler_factory.cc index 2e48b326c..95e3d593d 100644 --- a/ros/wavemap_ros/src/input_handler/input_handler_factory.cc +++ b/ros/wavemap_ros/src/input_handler/input_handler_factory.cc @@ -7,12 +7,14 @@ namespace wavemap { std::unique_ptr InputHandlerFactory::create( const param::Value& params, std::string world_frame, VolumetricDataStructureBase::Ptr occupancy_map, - std::shared_ptr transformer, ros::NodeHandle nh, + std::shared_ptr transformer, + std::shared_ptr thread_pool, ros::NodeHandle nh, ros::NodeHandle nh_private, std::optional default_input_handler_type) { if (const auto type = InputHandlerType::from(params); type) { return create(type.value(), params, world_frame, occupancy_map, - std::move(transformer), nh, nh_private); + std::move(transformer), std::move(thread_pool), nh, + nh_private); } if (default_input_handler_type.has_value()) { @@ -21,7 +23,8 @@ std::unique_ptr InputHandlerFactory::create( << "\" will be created instead."; return create(default_input_handler_type.value(), params, std::move(world_frame), std::move(occupancy_map), - std::move(transformer), nh, nh_private); + std::move(transformer), std::move(thread_pool), nh, + nh_private); } LOG(ERROR) << "No default was set. Returning nullptr."; @@ -31,7 +34,8 @@ std::unique_ptr InputHandlerFactory::create( std::unique_ptr InputHandlerFactory::create( InputHandlerType input_handler_type, const param::Value& params, std::string world_frame, VolumetricDataStructureBase::Ptr occupancy_map, - std::shared_ptr transformer, ros::NodeHandle nh, + std::shared_ptr transformer, + std::shared_ptr thread_pool, ros::NodeHandle nh, ros::NodeHandle nh_private) { // Create the input handler switch (input_handler_type.toTypeId()) { @@ -41,7 +45,8 @@ std::unique_ptr InputHandlerFactory::create( config) { return std::make_unique( config.value(), params, std::move(world_frame), - std::move(occupancy_map), std::move(transformer), nh, nh_private); + std::move(occupancy_map), std::move(transformer), + std::move(thread_pool), nh, nh_private); } else { LOG(ERROR) << "Pointcloud input handler config could not be loaded."; return nullptr; @@ -53,7 +58,8 @@ std::unique_ptr InputHandlerFactory::create( config) { return std::make_unique( config.value(), params, std::move(world_frame), - std::move(occupancy_map), std::move(transformer), nh, nh_private); + std::move(occupancy_map), std::move(transformer), + std::move(thread_pool), nh, nh_private); } else { LOG(ERROR) << "Depth image input handler config could not be loaded."; return nullptr; diff --git a/ros/wavemap_ros/src/input_handler/pointcloud_input_handler.cc b/ros/wavemap_ros/src/input_handler/pointcloud_input_handler.cc index 3a12e7254..33b9762fa 100644 --- a/ros/wavemap_ros/src/input_handler/pointcloud_input_handler.cc +++ b/ros/wavemap_ros/src/input_handler/pointcloud_input_handler.cc @@ -33,10 +33,12 @@ bool PointcloudInputHandlerConfig::isValid(bool verbose) const { PointcloudInputHandler::PointcloudInputHandler( const PointcloudInputHandlerConfig& config, const param::Value& params, std::string world_frame, VolumetricDataStructureBase::Ptr occupancy_map, - std::shared_ptr transformer, ros::NodeHandle nh, + std::shared_ptr transformer, + std::shared_ptr thread_pool, ros::NodeHandle nh, ros::NodeHandle nh_private) : InputHandler(config, params, std::move(world_frame), - std::move(occupancy_map), transformer, nh, nh_private), + std::move(occupancy_map), transformer, + std::move(thread_pool), nh, nh_private), config_(config.checkValid()), pointcloud_undistorter_( transformer, diff --git a/ros/wavemap_ros/src/wavemap_server.cc b/ros/wavemap_ros/src/wavemap_server.cc index 37adb3868..df30b3c5c 100644 --- a/ros/wavemap_ros/src/wavemap_server.cc +++ b/ros/wavemap_ros/src/wavemap_server.cc @@ -18,13 +18,15 @@ DECLARE_CONFIG_MEMBERS(WavemapServerConfig, (thresholding_period) (pruning_period) (publication_period) - (max_num_blocks_per_msg)); + (max_num_blocks_per_msg) + (num_threads)); bool WavemapServerConfig::isValid(bool verbose) const { bool all_valid = true; all_valid &= IS_PARAM_NE(world_frame, std::string(""), verbose); all_valid &= IS_PARAM_GT(max_num_blocks_per_msg, 0, verbose); + all_valid &= IS_PARAM_GT(num_threads, 0, verbose); return all_valid; } @@ -47,6 +49,8 @@ WavemapServer::WavemapServer(ros::NodeHandle nh, ros::NodeHandle nh_private, occupancy_map_ = VolumetricDataStructureFactory::create( data_structure_params, VolumetricDataStructureType::kHashedBlocks); CHECK_NOTNULL(occupancy_map_); + thread_pool_ = std::make_shared(config_.num_threads); + CHECK_NOTNULL(thread_pool_); // Setup input handlers const param::Array integrator_params_array = @@ -102,9 +106,9 @@ bool WavemapServer::loadMap(const std::filesystem::path& file_path) { InputHandler* WavemapServer::addInput(const param::Value& integrator_params, const ros::NodeHandle& nh, ros::NodeHandle nh_private) { - auto input_handler = - InputHandlerFactory::create(integrator_params, config_.world_frame, - occupancy_map_, transformer_, nh, nh_private); + auto input_handler = InputHandlerFactory::create( + integrator_params, config_.world_frame, occupancy_map_, transformer_, + thread_pool_, nh, nh_private); if (input_handler) { return input_handlers_.emplace_back(std::move(input_handler)).get(); } From a3fcee22eded6e2cc7205e6ea48daa51cbd110f6 Mon Sep 17 00:00:00 2001 From: Victor Reijgwart Date: Thu, 28 Sep 2023 21:05:58 +0200 Subject: [PATCH 06/70] Multi-thread block to ROS msg serialization --- .../wavemap_ros/impl/wavemap_server_inl.h | 2 +- .../map_msg_conversions.h | 21 +- .../src/map_msg_conversions.cc | 286 +++++++++++------- 3 files changed, 201 insertions(+), 108 deletions(-) diff --git a/ros/wavemap_ros/include/wavemap_ros/impl/wavemap_server_inl.h b/ros/wavemap_ros/include/wavemap_ros/impl/wavemap_server_inl.h index fa58e1d1b..161096309 100644 --- a/ros/wavemap_ros/include/wavemap_ros/impl/wavemap_server_inl.h +++ b/ros/wavemap_ros/include/wavemap_ros/impl/wavemap_server_inl.h @@ -66,7 +66,7 @@ void WavemapServer::publishHashedMap(HashedMapT* hashed_map, map_msg.header.stamp = ros::Time::now(); convert::mapToRosMsg(*hashed_map, map_msg.hashed_wavelet_octree.emplace_back(), - blocks_to_publish); + blocks_to_publish, thread_pool_); { ZoneScopedN("publishMapRosMsg"); map_pub_.publish(map_msg); diff --git a/ros/wavemap_ros_conversions/include/wavemap_ros_conversions/map_msg_conversions.h b/ros/wavemap_ros_conversions/include/wavemap_ros_conversions/map_msg_conversions.h index 6dcf8574d..5b338a63a 100644 --- a/ros/wavemap_ros_conversions/include/wavemap_ros_conversions/map_msg_conversions.h +++ b/ros/wavemap_ros_conversions/include/wavemap_ros_conversions/map_msg_conversions.h @@ -2,6 +2,7 @@ #define WAVEMAP_ROS_CONVERSIONS_MAP_MSG_CONVERSIONS_H_ #include +#include #include #include #include @@ -12,6 +13,7 @@ #include #include #include +#include #include namespace wavemap::convert { @@ -27,15 +29,26 @@ void rosMsgToMap(const wavemap_msgs::WaveletOctree& msg, void mapToRosMsg(const HashedWaveletOctree& map, wavemap_msgs::HashedWaveletOctree& msg, - const std::optional>& - include_blocks = std::nullopt); + std::optional> + include_blocks = std::nullopt, + std::shared_ptr thread_pool = nullptr); +void blockToRosMsg(const HashedWaveletOctree::BlockIndex& block_index, + const HashedWaveletOctree::Block& block, + FloatingPoint min_log_odds, FloatingPoint max_log_odds, + wavemap_msgs::HashedWaveletOctreeBlock& msg); void rosMsgToMap(const wavemap_msgs::HashedWaveletOctree& msg, HashedWaveletOctree::Ptr& map); void mapToRosMsg(const HashedChunkedWaveletOctree& map, wavemap_msgs::HashedWaveletOctree& msg, - const std::optional>& - include_blocks = std::nullopt); + std::optional> + include_blocks = std::nullopt, + std::shared_ptr thread_pool = nullptr); +void blockToRosMsg(const HashedChunkedWaveletOctree::BlockIndex& block_index, + const HashedChunkedWaveletOctree::Block& block, + FloatingPoint min_log_odds, FloatingPoint max_log_odds, + IndexElement tree_height, + wavemap_msgs::HashedWaveletOctreeBlock& msg); } // namespace wavemap::convert #endif // WAVEMAP_ROS_CONVERSIONS_MAP_MSG_CONVERSIONS_H_ diff --git a/ros/wavemap_ros_conversions/src/map_msg_conversions.cc b/ros/wavemap_ros_conversions/src/map_msg_conversions.cc index 7ee798ba5..47fb676a5 100644 --- a/ros/wavemap_ros_conversions/src/map_msg_conversions.cc +++ b/ros/wavemap_ros_conversions/src/map_msg_conversions.cc @@ -128,16 +128,11 @@ void rosMsgToMap(const wavemap_msgs::WaveletOctree& msg, } } -void mapToRosMsg(const HashedWaveletOctree& map, - wavemap_msgs::HashedWaveletOctree& msg, - const std::optional>& - include_blocks) { +void mapToRosMsg( + const HashedWaveletOctree& map, wavemap_msgs::HashedWaveletOctree& msg, + std::optional> include_blocks, + std::shared_ptr thread_pool) { ZoneScoped; - struct StackElement { - const FloatingPoint scale; - const HashedWaveletOctreeBlock::NodeType& node; - }; - constexpr FloatingPoint kNumericalNoise = 1e-3f; const auto min_log_odds = map.getMinLogOdds() + kNumericalNoise; const auto max_log_odds = map.getMaxLogOdds() - kNumericalNoise; @@ -147,43 +142,86 @@ void mapToRosMsg(const HashedWaveletOctree& map, msg.max_log_odds = map.getMaxLogOdds(); msg.tree_height = map.getTreeHeight(); - for (const auto& [block_index, block] : map.getBlocks()) { - if (include_blocks.has_value() && !include_blocks->count(block_index)) { - continue; + // If blocks to include were specified, check that they exist + // and remove the ones that don't + if (include_blocks) { + for (auto include_block_it = include_blocks->begin(); + include_block_it != include_blocks->end();) { + if (map.hasBlock(*include_block_it)) { + ++include_block_it; + } else { + include_block_it = include_blocks->erase(include_block_it); + } } + } else { // Otherwise, include all blocks + include_blocks.emplace(); + for (const auto& [block_index, block] : map.getBlocks()) { + include_blocks->emplace(block_index); + } + } - auto& block_msg = msg.blocks.emplace_back(); - block_msg.root_node_offset = {block_index.x(), block_index.y(), - block_index.z()}; - block_msg.root_node_scale_coefficient = block.getRootScale(); + // Serialize the specified blocks + int block_idx = 0; + msg.blocks.resize(include_blocks->size()); + for (const auto& block_index : include_blocks.value()) { + const auto& block = map.getBlock(block_index); + auto& block_msg = msg.blocks[block_idx++]; + // If a thread pool was provided, use it + if (thread_pool) { + thread_pool->add_task([&]() { + blockToRosMsg(block_index, block, min_log_odds, max_log_odds, + block_msg); + }); + } else { // Otherwise, use the current thread + blockToRosMsg(block_index, block, min_log_odds, max_log_odds, block_msg); + } + } - std::stack stack; - stack.emplace(StackElement{block.getRootScale(), block.getRootNode()}); + // If a thread pool was used, wait for all jobs to finish + if (thread_pool) { + thread_pool->wait_all(); + } +} - while (!stack.empty()) { - const FloatingPoint scale = stack.top().scale; - const auto& node = stack.top().node; - stack.pop(); +void blockToRosMsg(const HashedWaveletOctree::BlockIndex& block_index, + const HashedWaveletOctree::Block& block, + FloatingPoint min_log_odds, FloatingPoint max_log_odds, + wavemap_msgs::HashedWaveletOctreeBlock& msg) { + ZoneScoped; + struct StackElement { + const FloatingPoint scale; + const HashedWaveletOctreeBlock::NodeType& node; + }; - auto& node_msg = block_msg.nodes.emplace_back(); - std::copy(node.data().cbegin(), node.data().cend(), - node_msg.detail_coefficients.begin()); - node_msg.allocated_children_bitset = 0; + msg.root_node_offset = {block_index.x(), block_index.y(), block_index.z()}; + msg.root_node_scale_coefficient = block.getRootScale(); - const auto child_scales = - HashedWaveletOctreeBlock::Transform::backward({scale, node.data()}); - for (int relative_child_idx = OctreeIndex::kNumChildren - 1; - 0 <= relative_child_idx; --relative_child_idx) { - const auto child_scale = child_scales[relative_child_idx]; - if (child_scale < min_log_odds || max_log_odds < child_scale) { - continue; - } + std::stack stack; + stack.emplace(StackElement{block.getRootScale(), block.getRootNode()}); - const auto* child = node.getChild(relative_child_idx); - if (child) { - stack.emplace(StackElement{child_scale, *child}); - node_msg.allocated_children_bitset += (1 << relative_child_idx); - } + while (!stack.empty()) { + const FloatingPoint scale = stack.top().scale; + const auto& node = stack.top().node; + stack.pop(); + + auto& node_msg = msg.nodes.emplace_back(); + std::copy(node.data().cbegin(), node.data().cend(), + node_msg.detail_coefficients.begin()); + node_msg.allocated_children_bitset = 0; + + const auto child_scales = + HashedWaveletOctreeBlock::Transform::backward({scale, node.data()}); + for (int relative_child_idx = OctreeIndex::kNumChildren - 1; + 0 <= relative_child_idx; --relative_child_idx) { + const auto child_scale = child_scales[relative_child_idx]; + if (child_scale < min_log_odds || max_log_odds < child_scale) { + continue; + } + + const auto* child = node.getChild(relative_child_idx); + if (child) { + stack.emplace(StackElement{child_scale, *child}); + node_msg.allocated_children_bitset += (1 << relative_child_idx); } } } @@ -237,91 +275,133 @@ void rosMsgToMap(const wavemap_msgs::HashedWaveletOctree& msg, } } -void mapToRosMsg(const HashedChunkedWaveletOctree& map, - wavemap_msgs::HashedWaveletOctree& msg, - const std::optional>& - include_blocks) { +void mapToRosMsg( + const HashedChunkedWaveletOctree& map, + wavemap_msgs::HashedWaveletOctree& msg, + std::optional> include_blocks, + std::shared_ptr thread_pool) { ZoneScoped; - struct StackElement { - const OctreeIndex node_index; - const HashedChunkedWaveletOctreeBlock::NodeChunkType& chunk; - const FloatingPoint scale_coefficient; - }; - constexpr FloatingPoint kNumericalNoise = 1e-3f; const auto min_log_odds = map.getMinLogOdds() + kNumericalNoise; const auto max_log_odds = map.getMaxLogOdds() - kNumericalNoise; const auto tree_height = map.getTreeHeight(); - const auto chunk_height = map.getChunkHeight(); msg.min_cell_width = map.getMinCellWidth(); msg.min_log_odds = map.getMinLogOdds(); msg.max_log_odds = map.getMaxLogOdds(); msg.tree_height = map.getTreeHeight(); - for (const auto& [block_index, block] : map.getBlocks()) { - if (include_blocks.has_value() && !include_blocks->count(block_index)) { - continue; + // If blocks to include were specified, check that they exist + // and remove the ones that don't + if (include_blocks) { + for (auto include_block_it = include_blocks->begin(); + include_block_it != include_blocks->end();) { + if (map.hasBlock(*include_block_it)) { + ++include_block_it; + } else { + include_block_it = include_blocks->erase(include_block_it); + } + } + } else { // Otherwise, include all blocks + include_blocks.emplace(); + for (const auto& [block_index, block] : map.getBlocks()) { + include_blocks->emplace(block_index); } + } - auto& block_msg = msg.blocks.emplace_back(); - block_msg.root_node_offset = {block_index.x(), block_index.y(), - block_index.z()}; - block_msg.root_node_scale_coefficient = block.getRootScale(); - - std::stack stack; - stack.emplace(StackElement{{tree_height, block_index}, - block.getRootChunk(), - block.getRootScale()}); - while (!stack.empty()) { - const OctreeIndex index = stack.top().node_index; - const FloatingPoint scale = stack.top().scale_coefficient; - const auto& chunk = stack.top().chunk; - stack.pop(); + // Serialize the specified blocks + int block_idx = 0; + msg.blocks.resize(include_blocks->size()); + for (const auto& block_index : include_blocks.value()) { + const auto& block = map.getBlock(block_index); + auto& block_msg = msg.blocks[block_idx++]; + // If a thread pool was provided, use it + if (thread_pool) { + thread_pool->add_task([&]() { + blockToRosMsg(block_index, block, min_log_odds, max_log_odds, + tree_height, block_msg); + }); + } else { // Otherwise, use the current thread + blockToRosMsg(block_index, block, min_log_odds, max_log_odds, tree_height, + block_msg); + } + } - const MortonIndex morton_code = convert::nodeIndexToMorton(index); - const int chunk_top_height = - chunk_height * int_math::div_round_up(index.height, chunk_height); - const LinearIndex relative_node_index = - OctreeIndex::computeTreeTraversalDistance( - morton_code, chunk_top_height, index.height); + // If a thread pool was used, wait for all jobs to finish + if (thread_pool) { + thread_pool->wait_all(); + } +} - auto& node_msg = block_msg.nodes.emplace_back(); - const auto& node_data = chunk.nodeData(relative_node_index); - std::copy(node_data.cbegin(), node_data.cend(), - node_msg.detail_coefficients.begin()); - node_msg.allocated_children_bitset = 0; +void blockToRosMsg(const HashedChunkedWaveletOctree::BlockIndex& block_index, + const HashedChunkedWaveletOctree::Block& block, + FloatingPoint min_log_odds, FloatingPoint max_log_odds, + IndexElement tree_height, + wavemap_msgs::HashedWaveletOctreeBlock& msg) { + ZoneScoped; + struct StackElement { + const OctreeIndex node_index; + const HashedChunkedWaveletOctreeBlock::NodeChunkType& chunk; + const FloatingPoint scale_coefficient; + }; + + constexpr IndexElement chunk_height = + HashedChunkedWaveletOctreeBlock::kChunkHeight; + + msg.root_node_offset = {block_index.x(), block_index.y(), block_index.z()}; + msg.root_node_scale_coefficient = block.getRootScale(); + + std::stack stack; + stack.emplace(StackElement{ + {tree_height, block_index}, block.getRootChunk(), block.getRootScale()}); + while (!stack.empty()) { + const OctreeIndex index = stack.top().node_index; + const FloatingPoint scale = stack.top().scale_coefficient; + const auto& chunk = stack.top().chunk; + stack.pop(); + + const MortonIndex morton_code = convert::nodeIndexToMorton(index); + const int chunk_top_height = + chunk_height * int_math::div_round_up(index.height, chunk_height); + const LinearIndex relative_node_index = + OctreeIndex::computeTreeTraversalDistance(morton_code, chunk_top_height, + index.height); + + auto& node_msg = msg.nodes.emplace_back(); + const auto& node_data = chunk.nodeData(relative_node_index); + std::copy(node_data.cbegin(), node_data.cend(), + node_msg.detail_coefficients.begin()); + node_msg.allocated_children_bitset = 0; - if (!chunk.nodeHasAtLeastOneChild(relative_node_index)) { + if (!chunk.nodeHasAtLeastOneChild(relative_node_index)) { + continue; + } + + const auto child_scales = + HashedWaveletOctreeBlock::Transform::backward({scale, node_data}); + for (int relative_child_idx = OctreeIndex::kNumChildren - 1; + 0 <= relative_child_idx; --relative_child_idx) { + const FloatingPoint child_scale = child_scales[relative_child_idx]; + if (child_scale < min_log_odds || max_log_odds < child_scale) { continue; } - const auto child_scales = - HashedWaveletOctreeBlock::Transform::backward({scale, node_data}); - for (int relative_child_idx = OctreeIndex::kNumChildren - 1; - 0 <= relative_child_idx; --relative_child_idx) { - const FloatingPoint child_scale = child_scales[relative_child_idx]; - if (child_scale < min_log_odds || max_log_odds < child_scale) { - continue; - } - - const OctreeIndex child_index = - index.computeChildIndex(relative_child_idx); - if (child_index.height % chunk_height == 0) { - const MortonIndex child_morton = - convert::nodeIndexToMorton(child_index); - const LinearIndex linear_child_index = - OctreeIndex::computeLevelTraversalDistance( - child_morton, chunk_top_height, child_index.height); - if (chunk.hasChild(linear_child_index)) { - const auto& child_chunk = *chunk.getChild(linear_child_index); - stack.emplace(StackElement{child_index, child_chunk, child_scale}); - node_msg.allocated_children_bitset += (1 << relative_child_idx); - } - } else { - stack.emplace(StackElement{child_index, chunk, child_scale}); + const OctreeIndex child_index = + index.computeChildIndex(relative_child_idx); + if (child_index.height % chunk_height == 0) { + const MortonIndex child_morton = + convert::nodeIndexToMorton(child_index); + const LinearIndex linear_child_index = + OctreeIndex::computeLevelTraversalDistance( + child_morton, chunk_top_height, child_index.height); + if (chunk.hasChild(linear_child_index)) { + const auto& child_chunk = *chunk.getChild(linear_child_index); + stack.emplace(StackElement{child_index, child_chunk, child_scale}); node_msg.allocated_children_bitset += (1 << relative_child_idx); } + } else { + stack.emplace(StackElement{child_index, chunk, child_scale}); + node_msg.allocated_children_bitset += (1 << relative_child_idx); } } } From 135107f8fb2c740e08175ab896b72b89fc4addc9 Mon Sep 17 00:00:00 2001 From: Victor Reijgwart Date: Fri, 29 Sep 2023 09:30:49 +0200 Subject: [PATCH 07/70] Reintroduce that blocks are only thresholded when needed --- .../volumetric/hashed_chunked_wavelet_octree_block.h | 4 ++++ .../coarse_to_fine/hashed_chunked_wavelet_integrator.h | 3 ++- .../hashed_chunked_wavelet_integrator.cc | 10 ++++++---- 3 files changed, 12 insertions(+), 5 deletions(-) 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..94c154d68 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 @@ -52,8 +52,12 @@ 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_; } + bool& getNeedsThresholding() { return needs_thresholding_; } + void setLastUpdatedStamp(Timestamp stamp = Time::now()) { last_updated_stamp_ = stamp; } 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 67943714d..729f6deef 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 @@ -62,7 +62,8 @@ class HashedChunkedWaveletIntegrator : public ProjectiveIntegrator { HashedChunkedWaveletOctreeBlock::NodeChunkType& parent_chunk, const OctreeIndex& parent_node_index, LinearIndex parent_in_chunk_index, FloatingPoint& parent_value, - HashedChunkedWaveletOctreeBlock::NodeChunkType::BitRef parent_has_child); + HashedChunkedWaveletOctreeBlock::NodeChunkType::BitRef parent_has_child, + bool& block_needs_thresholding); void updateLeavesBatch( const OctreeIndex& parent_index, FloatingPoint& parent_value, HashedChunkedWaveletOctreeBlock::NodeChunkType::DataType& parent_details); diff --git a/libraries/wavemap/src/integrator/projective/coarse_to_fine/hashed_chunked_wavelet_integrator.cc b/libraries/wavemap/src/integrator/projective/coarse_to_fine/hashed_chunked_wavelet_integrator.cc index 598a003bd..f19dd9c87 100644 --- a/libraries/wavemap/src/integrator/projective/coarse_to_fine/hashed_chunked_wavelet_integrator.cc +++ b/libraries/wavemap/src/integrator/projective/coarse_to_fine/hashed_chunked_wavelet_integrator.cc @@ -68,21 +68,22 @@ void HashedChunkedWaveletIntegrator::updateBlock( HashedChunkedWaveletOctree::Block& block, const HashedChunkedWaveletOctree::BlockIndex& block_index) { ZoneScoped; - block.setNeedsThresholding(); block.setNeedsPruning(); block.setLastUpdatedStamp(); const OctreeIndex root_node_index{tree_height_, block_index}; updateNodeRecursive(block.getRootChunk(), root_node_index, 0u, block.getRootScale(), - block.getRootChunk().nodeHasAtLeastOneChild(0u)); + block.getRootChunk().nodeHasAtLeastOneChild(0u), + block.getNeedsThresholding()); } void HashedChunkedWaveletIntegrator::updateNodeRecursive( // NOLINT HashedChunkedWaveletOctreeBlock::NodeChunkType& parent_chunk, const OctreeIndex& parent_node_index, LinearIndex parent_in_chunk_index, FloatingPoint& parent_value, - HashedChunkedWaveletOctreeBlock::NodeChunkType::BitRef parent_has_child) { + HashedChunkedWaveletOctreeBlock::NodeChunkType::BitRef parent_has_child, + bool& block_needs_thresholding) { auto& parent_details = parent_chunk.nodeData(parent_in_chunk_index); auto child_values = HashedChunkedWaveletOctreeBlock::Transform::backward( {parent_value, parent_details}); @@ -132,6 +133,7 @@ void HashedChunkedWaveletIntegrator::updateNodeRecursive( // NOLINT config_.termination_update_error) { const FloatingPoint sample = computeUpdate(C_child_center); child_value += sample; + block_needs_thresholding = true; continue; } @@ -171,7 +173,7 @@ void HashedChunkedWaveletIntegrator::updateNodeRecursive( // NOLINT DCHECK_GE(child_height, 0); updateNodeRecursive(*chunk_containing_child, child_index, child_node_in_chunk_index, child_value, - child_has_children); + child_has_children, block_needs_thresholding); } if (child_has_children || data_utils::is_nonzero(child_details)) { From 9d7eed2a45c746d55934ed87b2337b719168c840 Mon Sep 17 00:00:00 2001 From: Victor Reijgwart Date: Fri, 29 Sep 2023 09:39:27 +0200 Subject: [PATCH 08/70] Rename selective pruning method --- .../data_structure/volumetric/hashed_chunked_wavelet_octree.h | 2 +- .../wavemap/data_structure/volumetric/hashed_wavelet_octree.h | 2 +- .../data_structure/volumetric/volumetric_data_structure_base.h | 2 +- .../data_structure/volumetric/hashed_chunked_wavelet_octree.cc | 2 +- .../src/data_structure/volumetric/hashed_wavelet_octree.cc | 2 +- ros/wavemap_ros/src/wavemap_server.cc | 2 +- 6 files changed, 6 insertions(+), 6 deletions(-) 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..d9fb2f79d 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 @@ -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_wavelet_octree.h b/libraries/wavemap/include/wavemap/data_structure/volumetric/hashed_wavelet_octree.h index ac2216443..9b1d3b75e 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 @@ -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/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/src/data_structure/volumetric/hashed_chunked_wavelet_octree.cc b/libraries/wavemap/src/data_structure/volumetric/hashed_chunked_wavelet_octree.cc index 2a8204ebe..a612a3fb8 100644 --- a/libraries/wavemap/src/data_structure/volumetric/hashed_chunked_wavelet_octree.cc +++ b/libraries/wavemap/src/data_structure/volumetric/hashed_chunked_wavelet_octree.cc @@ -44,7 +44,7 @@ void HashedChunkedWaveletOctree::prune() { } } -void HashedChunkedWaveletOctree::pruneDistant() { +void HashedChunkedWaveletOctree::pruneSmart() { ZoneScoped; std::unordered_set> blocks_to_remove; for (auto& [block_index, block] : blocks_) { diff --git a/libraries/wavemap/src/data_structure/volumetric/hashed_wavelet_octree.cc b/libraries/wavemap/src/data_structure/volumetric/hashed_wavelet_octree.cc index 9167368cd..e605a4c36 100644 --- a/libraries/wavemap/src/data_structure/volumetric/hashed_wavelet_octree.cc +++ b/libraries/wavemap/src/data_structure/volumetric/hashed_wavelet_octree.cc @@ -43,7 +43,7 @@ void HashedWaveletOctree::prune() { } } -void HashedWaveletOctree::pruneDistant() { +void HashedWaveletOctree::pruneSmart() { ZoneScoped; std::unordered_set> blocks_to_remove; for (auto& [block_index, block] : blocks_) { diff --git a/ros/wavemap_ros/src/wavemap_server.cc b/ros/wavemap_ros/src/wavemap_server.cc index df30b3c5c..d5a1fa3da 100644 --- a/ros/wavemap_ros/src/wavemap_server.cc +++ b/ros/wavemap_ros/src/wavemap_server.cc @@ -129,7 +129,7 @@ void WavemapServer::subscribeToTimers(const ros::NodeHandle& nh) { << config_.pruning_period << "s"); map_pruning_timer_ = nh.createTimer( ros::Duration(config_.pruning_period), - [this](const auto& /*event*/) { occupancy_map_->pruneDistant(); }); + [this](const auto& /*event*/) { occupancy_map_->pruneSmart(); }); } if (0.f < config_.publication_period) { From b478b60c904aae5abcf07b5d7b064585dfbebec6 Mon Sep 17 00:00:00 2001 From: Victor Reijgwart Date: Fri, 29 Sep 2023 09:50:00 +0200 Subject: [PATCH 09/70] Update incremental transmission and Rviz to remove deleted blocks --- ros/wavemap_msgs/msg/HashedWaveletOctree.msg | 2 + .../msg/HashedWaveletOctreeBlock.msg | 2 +- ros/wavemap_msgs/msg/Index3D.msg | 3 ++ .../wavemap_ros/impl/wavemap_server_inl.h | 4 +- .../src/map_msg_conversions.cc | 45 ++++++++++++++++--- .../src/visuals/grid_visual.cc | 29 ++++++++++-- 6 files changed, 73 insertions(+), 12 deletions(-) create mode 100644 ros/wavemap_msgs/msg/Index3D.msg diff --git a/ros/wavemap_msgs/msg/HashedWaveletOctree.msg b/ros/wavemap_msgs/msg/HashedWaveletOctree.msg index db2afa94e..96b1cf74d 100644 --- a/ros/wavemap_msgs/msg/HashedWaveletOctree.msg +++ b/ros/wavemap_msgs/msg/HashedWaveletOctree.msg @@ -4,4 +4,6 @@ float32 max_log_odds int32 tree_height +Index3D[] allocated_block_indices + HashedWaveletOctreeBlock[] blocks diff --git a/ros/wavemap_msgs/msg/HashedWaveletOctreeBlock.msg b/ros/wavemap_msgs/msg/HashedWaveletOctreeBlock.msg index f82bb3712..0cb34913f 100644 --- a/ros/wavemap_msgs/msg/HashedWaveletOctreeBlock.msg +++ b/ros/wavemap_msgs/msg/HashedWaveletOctreeBlock.msg @@ -1,3 +1,3 @@ -int32[3] root_node_offset +Index3D root_node_offset float32 root_node_scale_coefficient WaveletOctreeNode[] nodes diff --git a/ros/wavemap_msgs/msg/Index3D.msg b/ros/wavemap_msgs/msg/Index3D.msg new file mode 100644 index 000000000..5c84798a9 --- /dev/null +++ b/ros/wavemap_msgs/msg/Index3D.msg @@ -0,0 +1,3 @@ +int32 x +int32 y +int32 z diff --git a/ros/wavemap_ros/include/wavemap_ros/impl/wavemap_server_inl.h b/ros/wavemap_ros/include/wavemap_ros/impl/wavemap_server_inl.h index 161096309..f4017c758 100644 --- a/ros/wavemap_ros/include/wavemap_ros/impl/wavemap_server_inl.h +++ b/ros/wavemap_ros/include/wavemap_ros/impl/wavemap_server_inl.h @@ -36,9 +36,7 @@ void WavemapServer::publishHashedMap(HashedMapT* hashed_map, changed_blocks_sorted[last_modified_time] = block_idx; ++block_it; } else { - LOG(WARNING) << "Removing block " << block_idx.transpose() - << " as it no longer exists."; - block_publishing_queue_.erase(block_it++); + block_it = block_publishing_queue_.erase(block_it); } } diff --git a/ros/wavemap_ros_conversions/src/map_msg_conversions.cc b/ros/wavemap_ros_conversions/src/map_msg_conversions.cc index 47fb676a5..9f81b27f5 100644 --- a/ros/wavemap_ros_conversions/src/map_msg_conversions.cc +++ b/ros/wavemap_ros_conversions/src/map_msg_conversions.cc @@ -142,6 +142,14 @@ void mapToRosMsg( msg.max_log_odds = map.getMaxLogOdds(); msg.tree_height = map.getTreeHeight(); + msg.allocated_block_indices.reserve(map.getBlocks().size()); + for (const auto& [block_index, _] : map.getBlocks()) { + auto& block_index_msg = msg.allocated_block_indices.emplace_back(); + block_index_msg.x = block_index.x(); + block_index_msg.y = block_index.y(); + block_index_msg.z = block_index.z(); + } + // If blocks to include were specified, check that they exist // and remove the ones that don't if (include_blocks) { @@ -193,7 +201,9 @@ void blockToRosMsg(const HashedWaveletOctree::BlockIndex& block_index, const HashedWaveletOctreeBlock::NodeType& node; }; - msg.root_node_offset = {block_index.x(), block_index.y(), block_index.z()}; + msg.root_node_offset.x = block_index.x(); + msg.root_node_offset.y = block_index.y(); + msg.root_node_offset.z = block_index.z(); msg.root_node_scale_coefficient = block.getRootScale(); std::stack stack; @@ -239,12 +249,27 @@ void rosMsgToMap(const wavemap_msgs::HashedWaveletOctree& msg, if (!map || map->getConfig() != config) { // Otherwise create a new one map = std::make_shared(config); + } else { + // Load allocated block list into a hash table for quick membership lookups + std::unordered_set allocated_blocks; + for (const auto& block_index : msg.allocated_block_indices) { + allocated_blocks.emplace(block_index.x, block_index.y, block_index.z); + } + // Remove local blocks that should no longer exist according to the map msg + for (auto it = map->getBlocks().begin(); it != map->getBlocks().end();) { + const auto block_index = it->first; + if (!allocated_blocks.count(block_index)) { + it = map->getBlocks().erase(it); + } else { + ++it; + } + } } for (const auto& block_msg : msg.blocks) { - const Index3D block_index{block_msg.root_node_offset[0], - block_msg.root_node_offset[1], - block_msg.root_node_offset[2]}; + const Index3D block_index{block_msg.root_node_offset.x, + block_msg.root_node_offset.y, + block_msg.root_node_offset.z}; const bool block_existed = map->hasBlock(block_index); auto& block = map->getOrAllocateBlock(block_index); @@ -291,6 +316,14 @@ void mapToRosMsg( msg.max_log_odds = map.getMaxLogOdds(); msg.tree_height = map.getTreeHeight(); + msg.allocated_block_indices.reserve(map.getBlocks().size()); + for (const auto& [block_index, _] : map.getBlocks()) { + auto& block_index_msg = msg.allocated_block_indices.emplace_back(); + block_index_msg.x = block_index.x(); + block_index_msg.y = block_index.y(); + block_index_msg.z = block_index.z(); + } + // If blocks to include were specified, check that they exist // and remove the ones that don't if (include_blocks) { @@ -348,7 +381,9 @@ void blockToRosMsg(const HashedChunkedWaveletOctree::BlockIndex& block_index, constexpr IndexElement chunk_height = HashedChunkedWaveletOctreeBlock::kChunkHeight; - msg.root_node_offset = {block_index.x(), block_index.y(), block_index.z()}; + msg.root_node_offset.x = block_index.x(); + msg.root_node_offset.y = block_index.y(); + msg.root_node_offset.z = block_index.z(); msg.root_node_scale_coefficient = block.getRootScale(); std::stack stack; diff --git a/ros/wavemap_rviz_plugin/src/visuals/grid_visual.cc b/ros/wavemap_rviz_plugin/src/visuals/grid_visual.cc index d6a37696c..65120f6ee 100644 --- a/ros/wavemap_rviz_plugin/src/visuals/grid_visual.cc +++ b/ros/wavemap_rviz_plugin/src/visuals/grid_visual.cc @@ -124,11 +124,34 @@ void GridVisual::updateMap(bool redraw_all) { if (const auto* hashed_map = dynamic_cast(map.get()); hashed_map) { + // Remove blocks that no longer exist in the map + { + // From the visuals (blocks that were already drawn) + for (auto it = block_grids_.begin(); it != block_grids_.end();) { + const auto block_idx = it->first; + if (!hashed_map->getBlocks().count(block_idx)) { + it = block_grids_.erase(it); + } else { + ++it; + } + } + // From the queue (blocks that were about to be drawn) + for (auto it = block_update_queue_.begin(); + it != block_update_queue_.end();) { + const auto block_idx = it->first; + if (!hashed_map->getBlocks().count(block_idx)) { + it = block_update_queue_.erase(it); + } else { + ++it; + } + } + } + + // Add all blocks that changed since the last publication time + // to the drawing queue const auto min_termination_height = termination_height_property_.getInt(); for (const auto& [block_idx, block] : hashed_map->getBlocks()) { - // Add all blocks that changed since the last publication time to - // the queue. Since the queue is stored as a set, there are no - // duplicates. + // NOTE: Since the queue is stored as a set, there are no duplicates. if (redraw_all || last_update_time_ < block.getLastUpdatedStamp()) { block_update_queue_[block_idx] = min_termination_height; // Force the LODs to be updated, s.t. the new blocks directly get From b5541120c092a8dd3acf15c1aab53bee7b4081ad Mon Sep 17 00:00:00 2001 From: Victor Reijgwart Date: Fri, 29 Sep 2023 11:05:53 +0200 Subject: [PATCH 10/70] Fix bug causing blocks with identical timestamps to not be published --- .../wavemap_ros/impl/wavemap_server_inl.h | 26 ++++++++++++------- 1 file changed, 17 insertions(+), 9 deletions(-) diff --git a/ros/wavemap_ros/include/wavemap_ros/impl/wavemap_server_inl.h b/ros/wavemap_ros/include/wavemap_ros/impl/wavemap_server_inl.h index f4017c758..cef26b9cb 100644 --- a/ros/wavemap_ros/include/wavemap_ros/impl/wavemap_server_inl.h +++ b/ros/wavemap_ros/include/wavemap_ros/impl/wavemap_server_inl.h @@ -2,8 +2,9 @@ #define WAVEMAP_ROS_IMPL_WAVEMAP_SERVER_INL_H_ #include -#include #include +#include +#include #include #include @@ -26,31 +27,38 @@ void WavemapServer::publishHashedMap(HashedMapT* hashed_map, // Make sure that all the blocks in the queue still exist and // sort them by their modification time - std::map> changed_blocks_sorted; + std::vector> changed_blocks_stamped; for (auto block_it = block_publishing_queue_.cbegin(); block_it != block_publishing_queue_.cend();) { const Index3D block_idx = *block_it; if (hashed_map->hasBlock(block_idx)) { const Timestamp& last_modified_time = hashed_map->getBlock(block_idx).getLastUpdatedStamp(); - changed_blocks_sorted[last_modified_time] = block_idx; + changed_blocks_stamped.emplace_back(last_modified_time, block_idx); ++block_it; } else { block_it = block_publishing_queue_.erase(block_it); } } - // Select the blocks to publish in the current cycle + // If the number of changed blocks exceeds 'max_num_blocks_per_msg', + // select the 'max_num_blocks_per_msg' oldest blocks and drop the rest + if (static_cast(config_.max_num_blocks_per_msg) <= + changed_blocks_stamped.size()) { + auto nth = changed_blocks_stamped.begin() + config_.max_num_blocks_per_msg; + std::nth_element( + changed_blocks_stamped.begin(), nth, changed_blocks_stamped.end(), + [](const auto& lhs, const auto& rhs) { return lhs.first < rhs.first; }); + changed_blocks_stamped.resize(config_.max_num_blocks_per_msg); + } + + // Prepare the blocks to publish in the current cycle // NOTE: We take the config_.max_num_blocks_per_msg most recently modified // blocks. std::unordered_set blocks_to_publish; - for (const auto& [_, block_idx] : changed_blocks_sorted) { + for (const auto& [_, block_idx] : changed_blocks_stamped) { hashed_map->getBlock(block_idx).threshold(); blocks_to_publish.insert(block_idx); - if (static_cast(config_.max_num_blocks_per_msg) <= - blocks_to_publish.size()) { - break; - } } // If there are no blocks to publish, we're done From 90ed5ed7ad63a124f7cb090013077cda80e7d45d Mon Sep 17 00:00:00 2001 From: Victor Reijgwart Date: Fri, 29 Sep 2023 11:42:34 +0200 Subject: [PATCH 11/70] Simplify control flow --- .../measurement_model/impl/continuous_beam_inl.h | 9 ++++----- 1 file changed, 4 insertions(+), 5 deletions(-) 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..bc17c9478 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 @@ -69,12 +69,11 @@ inline FloatingPoint ContinuousBeam::computeUpdate( 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; + if (range_image_->isIndexWithinBounds(image_index)) { + measured_distances[neighbor_idx] = range_image_->at(image_index); + cell_to_beam_offsets[neighbor_idx] = + beam_offset_image_->at(image_index) - cell_offset; } - 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 From b9182a137f0286b6893e5d7481c2a60106b04c13 Mon Sep 17 00:00:00 2001 From: Victor Reijgwart Date: Fri, 29 Sep 2023 12:03:13 +0200 Subject: [PATCH 12/70] Make specifying a thread pool when constructing integrators optional --- .../wavemap/include/wavemap/integrator/integrator_factory.h | 4 ++-- .../coarse_to_fine/hashed_chunked_wavelet_integrator.h | 5 +++-- .../projective/coarse_to_fine/hashed_wavelet_integrator.h | 5 +++-- 3 files changed, 8 insertions(+), 6 deletions(-) diff --git a/libraries/wavemap/include/wavemap/integrator/integrator_factory.h b/libraries/wavemap/include/wavemap/integrator/integrator_factory.h index 0758727b1..245961829 100644 --- a/libraries/wavemap/include/wavemap/integrator/integrator_factory.h +++ b/libraries/wavemap/include/wavemap/integrator/integrator_factory.h @@ -14,13 +14,13 @@ class IntegratorFactory { static IntegratorBase::Ptr create( const param::Value& params, VolumetricDataStructureBase::Ptr occupancy_map, - std::shared_ptr thread_pool, + 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, - std::shared_ptr thread_pool); + std::shared_ptr thread_pool = nullptr); }; } // 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 729f6deef..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 @@ -21,12 +21,13 @@ class HashedChunkedWaveletIntegrator : public ProjectiveIntegrator { Image::Ptr beam_offset_image, MeasurementModelBase::ConstPtr measurement_model, HashedChunkedWaveletOctree::Ptr occupancy_map, - std::shared_ptr thread_pool) + 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))), - thread_pool_(std::move(thread_pool)) {} + thread_pool_(thread_pool ? std::move(thread_pool) + : std::make_shared()) {} private: using BlockList = std::vector; 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 6cb406509..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 @@ -20,12 +20,13 @@ class HashedWaveletIntegrator : public ProjectiveIntegrator { Image::Ptr beam_offset_image, MeasurementModelBase::ConstPtr measurement_model, HashedWaveletOctree::Ptr occupancy_map, - std::shared_ptr thread_pool) + 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))), - thread_pool_(std::move(thread_pool)) {} + thread_pool_(thread_pool ? std::move(thread_pool) + : std::make_shared()) {} private: const HashedWaveletOctree::Ptr occupancy_map_; From 4e0a172dc541fbade82edc474d4ab730f9e689f9 Mon Sep 17 00:00:00 2001 From: Victor Reijgwart Date: Fri, 29 Sep 2023 14:24:44 +0200 Subject: [PATCH 13/70] Avoid unnecessary copies --- .../impl/ouster_projector_inl.h | 17 +++++++++-------- .../impl/pinhole_camera_projector_inl.h | 9 ++++++--- .../impl/spherical_projector_inl.h | 17 +++++++++-------- .../projection_model/ouster_projector.h | 4 ++-- .../projection_model/pinhole_camera_projector.h | 4 ++-- .../projection_model/projector_base.h | 4 ++-- .../projection_model/spherical_projector.h | 4 ++-- 7 files changed, 32 insertions(+), 27 deletions(-) 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..9df5c987a 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 @@ -33,24 +33,25 @@ inline Point3D OusterProjector::sensorToCartesian( } inline FloatingPoint OusterProjector::imageOffsetToErrorNorm( - const Vector2D& linearization_point, Vector2D offset) const { + const Vector2D& 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(); + return std::sqrt(offset[0] * offset[0] + + (cos_elevation_angle * cos_elevation_angle) * + (offset[1] * offset[1])); } inline std::array OusterProjector::imageOffsetsToErrorNorms( const Vector2D& linearization_point, - ProjectorBase::CellToBeamOffsetArray offsets) const { + const ProjectorBase::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; - } std::array error_norms{}; for (int offset_idx = 0; offset_idx < 4; ++offset_idx) { - error_norms[offset_idx] = offsets[offset_idx].norm(); + error_norms[offset_idx] = + std::sqrt((offsets[offset_idx][0] * offsets[offset_idx][0]) + + (cos_elevation_angle * cos_elevation_angle) * + (offsets[offset_idx][1] * offsets[offset_idx][1])); } return error_norms; } 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..44a2a8082 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 @@ -30,16 +30,19 @@ inline Point3D PinholeCameraProjector::sensorToCartesian( } inline FloatingPoint PinholeCameraProjector::imageOffsetToErrorNorm( - const Vector2D&, Vector2D offset) const { + const Vector2D&, const Vector2D& offset) const { return offset.norm(); } inline std::array PinholeCameraProjector::imageOffsetsToErrorNorms( - const Vector2D&, ProjectorBase::CellToBeamOffsetArray offsets) const { + const Vector2D&, + const ProjectorBase::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(); + error_norms[offset_idx] = + std::sqrt(offsets[offset_idx][0] * offsets[offset_idx][0] + + offsets[offset_idx][1] * offsets[offset_idx][1]); } return error_norms; } 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..d0c1651ba 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 @@ -21,25 +21,26 @@ inline Point3D SphericalProjector::sensorToCartesian( } inline FloatingPoint SphericalProjector::imageOffsetToErrorNorm( - const Vector2D& linearization_point, Vector2D offset) const { + const Vector2D& 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(); + return std::sqrt(offset[0] * offset[0] + + (cos_elevation_angle * cos_elevation_angle) * + (offset[1] * offset[1])); } inline std::array SphericalProjector::imageOffsetsToErrorNorms( const Vector2D& linearization_point, - ProjectorBase::CellToBeamOffsetArray offsets) const { + const ProjectorBase::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; - } std::array error_norms{}; for (int offset_idx = 0; offset_idx < 4; ++offset_idx) { - error_norms[offset_idx] = offsets[offset_idx].norm(); + error_norms[offset_idx] = + std::sqrt((offsets[offset_idx][0] * offsets[offset_idx][0]) + + (cos_elevation_angle * cos_elevation_angle) * + (offsets[offset_idx][1] * offsets[offset_idx][1])); } return error_norms; } 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..3748883f4 100644 --- a/libraries/wavemap/include/wavemap/integrator/projection_model/ouster_projector.h +++ b/libraries/wavemap/include/wavemap/integrator/projection_model/ouster_projector.h @@ -74,10 +74,10 @@ class OusterProjector : public ProjectorBase { {image_coordinates.x(), image_coordinates.y(), range}); } FloatingPoint imageOffsetToErrorNorm(const Vector2D& linearization_point, - Vector2D offset) const final; + const Vector2D& offset) const final; std::array imageOffsetsToErrorNorms( const Vector2D& linearization_point, - CellToBeamOffsetArray offsets) const final; + const CellToBeamOffsetArray& offsets) const final; // Projection from Cartesian space onto the sensor's image surface Vector2D cartesianToImage(const Point3D& C_point) const final; 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..80219917d 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 @@ -71,10 +71,10 @@ class PinholeCameraProjector : public ProjectorBase { Point3D sensorToCartesian(const Vector2D& image_coordinates, FloatingPoint depth) const final; FloatingPoint imageOffsetToErrorNorm(const Vector2D& /*linearization_point*/, - Vector2D offset) const final; + const Vector2D& offset) const final; std::array imageOffsetsToErrorNorms( const Vector2D& /*linearization_point*/, - CellToBeamOffsetArray offsets) const final; + const CellToBeamOffsetArray& offsets) const final; // Projection from Cartesian space onto the sensor's image surface Vector2D cartesianToImage(const Point3D& C_point) const final { 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..99dfcc9ec 100644 --- a/libraries/wavemap/include/wavemap/integrator/projection_model/projector_base.h +++ b/libraries/wavemap/include/wavemap/integrator/projection_model/projector_base.h @@ -73,11 +73,11 @@ class ProjectorBase { // 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; + const Vector2D& linearization_point, const Vector2D& offset) const = 0; using CellToBeamOffsetArray = std::array; virtual std::array imageOffsetsToErrorNorms( const Vector2D& linearization_point, - CellToBeamOffsetArray offsets) const = 0; + const CellToBeamOffsetArray& offsets) const = 0; // Convenience functions combining multiple of the above methods Index2D cartesianToNearestIndex(const Point3D& C_point) const { 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..5247fac85 100644 --- a/libraries/wavemap/include/wavemap/integrator/projection_model/spherical_projector.h +++ b/libraries/wavemap/include/wavemap/integrator/projection_model/spherical_projector.h @@ -60,10 +60,10 @@ class SphericalProjector : public ProjectorBase { {image_coordinates.x(), image_coordinates.y(), range}); } FloatingPoint imageOffsetToErrorNorm(const Vector2D& linearization_point, - Vector2D offset) const final; + const Vector2D& offset) const final; std::array imageOffsetsToErrorNorms( const Vector2D& linearization_point, - CellToBeamOffsetArray offsets) const final; + const CellToBeamOffsetArray& offsets) const final; // Projection from Cartesian space onto the sensor's image surface Vector2D cartesianToImage(const Point3D& C_point) const final; From 464fd9c35a486012a3bf39c301588a36334869eb Mon Sep 17 00:00:00 2001 From: Victor Reijgwart Date: Fri, 29 Sep 2023 15:44:37 +0200 Subject: [PATCH 14/70] Various minor optimizations --- .../projection_model/impl/ouster_projector_inl.h | 4 +++- .../projection_model/impl/projector_base_inl.h | 13 ++++++++----- .../projection_model/impl/spherical_projector_inl.h | 4 +++- .../integrator/projection_model/ouster_projector.h | 2 -- .../projection_model/pinhole_camera_projector.h | 5 ++--- .../integrator/projection_model/projector_base.h | 13 ++++++++----- .../projection_model/spherical_projector.h | 2 -- .../integrator/projection_model/ouster_projector.cc | 3 ++- .../projection_model/spherical_projector.cc | 3 ++- 9 files changed, 28 insertions(+), 21 deletions(-) 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 9df5c987a..733c7fb91 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 @@ -46,11 +46,13 @@ inline std::array OusterProjector::imageOffsetsToErrorNorms( const Vector2D& linearization_point, const ProjectorBase::CellToBeamOffsetArray& offsets) const { const FloatingPoint cos_elevation_angle = std::cos(linearization_point[0]); + 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] = std::sqrt((offsets[offset_idx][0] * offsets[offset_idx][0]) + - (cos_elevation_angle * cos_elevation_angle) * + cos_elevation_angle_sq * (offsets[offset_idx][1] * offsets[offset_idx][1])); } 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..28bf91b35 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 @@ -32,9 +32,10 @@ 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 = + Vector2D image_coordinate_offset = (index - index_rounded).cwiseProduct(index_to_image_scale_factor_); - return {index_rounded.cast(), image_coordinate_offset}; + return {index_rounded.cast(), + std::move(image_coordinate_offset)}; } inline std::array ProjectorBase::imageToNearestIndices( @@ -68,11 +69,13 @@ ProjectorBase::imageToNearestIndicesAndOffsets( 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_); + offsets[neighbox_idx][0] = + index_to_image_scale_factor_[0] * (index[0] - index_rounded[0]); + offsets[neighbox_idx][1] = + index_to_image_scale_factor_[1] * (index[1] - index_rounded[1]); } - return {indices, offsets}; + return {std::move(indices), std::move(offsets)}; } inline Vector2D ProjectorBase::indexToImage(const Index2D& index) const { 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 d0c1651ba..7bdb02f5a 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 @@ -35,11 +35,13 @@ SphericalProjector::imageOffsetsToErrorNorms( const Vector2D& linearization_point, const ProjectorBase::CellToBeamOffsetArray& offsets) const { const FloatingPoint cos_elevation_angle = std::cos(linearization_point[0]); + 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] = std::sqrt((offsets[offset_idx][0] * offsets[offset_idx][0]) + - (cos_elevation_angle * cos_elevation_angle) * + cos_elevation_angle_sq * (offsets[offset_idx][1] * offsets[offset_idx][1])); } return error_norms; 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 3748883f4..3f270671e 100644 --- a/libraries/wavemap/include/wavemap/integrator/projection_model/ouster_projector.h +++ b/libraries/wavemap/include/wavemap/integrator/projection_model/ouster_projector.h @@ -51,8 +51,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}; } 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 80219917d..57360b9e2 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 @@ -46,11 +46,10 @@ class PinholeCameraProjector : public ProjectorBase { using Config = PinholeCameraProjectorConfig; explicit PinholeCameraProjector(const Config& config) - : ProjectorBase(Vector2D::Ones(), Vector2D::Zero()), + : ProjectorBase({config.width, config.height}, 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()); } 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 99dfcc9ec..0230eab88 100644 --- a/libraries/wavemap/include/wavemap/integrator/projection_model/projector_base.h +++ b/libraries/wavemap/include/wavemap/integrator/projection_model/projector_base.h @@ -28,14 +28,16 @@ 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)), + ProjectorBase(Index2D dimensions, Vector2D index_to_image_scale_factor, + Vector2D image_offset) + : dimensions_(std::move(dimensions)), + index_to_image_scale_factor_(std::move(index_to_image_scale_factor)), image_offset_(std::move(image_offset)) {} virtual ~ProjectorBase() = default; - virtual IndexElement getNumRows() const = 0; - virtual IndexElement getNumColumns() const = 0; - Index2D getDimensions() const { return {getNumRows(), getNumColumns()}; } + IndexElement getNumRows() const { return dimensions_.x(); } + IndexElement getNumColumns() const { return dimensions_.y(); } + Index2D getDimensions() const { return dimensions_; } virtual Vector2D getMinImageCoordinates() const = 0; virtual Vector2D getMaxImageCoordinates() const = 0; virtual Eigen::Matrix sensorAxisIsPeriodic() const = 0; @@ -90,6 +92,7 @@ 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_); 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 5247fac85..555e63a83 100644 --- a/libraries/wavemap/include/wavemap/integrator/projection_model/spherical_projector.h +++ b/libraries/wavemap/include/wavemap/integrator/projection_model/spherical_projector.h @@ -37,8 +37,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}; } diff --git a/libraries/wavemap/src/integrator/projection_model/ouster_projector.cc b/libraries/wavemap/src/integrator/projection_model/ouster_projector.cc index bd108c29f..b4e6ade7c 100644 --- a/libraries/wavemap/src/integrator/projection_model/ouster_projector.cc +++ b/libraries/wavemap/src/integrator/projection_model/ouster_projector.cc @@ -5,12 +5,13 @@ namespace wavemap { OusterProjector::OusterProjector(const OusterProjector::Config& config) : ProjectorBase( + {config.elevation.num_cells, config.azimuth.num_cells}, Vector2D(config.elevation.max_angle - config.elevation.min_angle, config.azimuth.max_angle - config.azimuth.min_angle) .cwiseQuotient(Index2D(config.elevation.num_cells - 1, config.azimuth.num_cells - 1) .cast()), - Vector2D(config.elevation.min_angle, config.azimuth.min_angle)), + {config.elevation.min_angle, config.azimuth.min_angle}), config_(config.checkValid()) {} Eigen::Matrix OusterProjector::sensorAxisIsPeriodic() const { diff --git a/libraries/wavemap/src/integrator/projection_model/spherical_projector.cc b/libraries/wavemap/src/integrator/projection_model/spherical_projector.cc index e68ee12dd..dcc28d5cc 100644 --- a/libraries/wavemap/src/integrator/projection_model/spherical_projector.cc +++ b/libraries/wavemap/src/integrator/projection_model/spherical_projector.cc @@ -5,12 +5,13 @@ namespace wavemap { SphericalProjector::SphericalProjector(const SphericalProjector::Config& config) : ProjectorBase( + {config.elevation.num_cells, config.azimuth.num_cells}, Vector2D(config.elevation.max_angle - config.elevation.min_angle, config.azimuth.max_angle - config.azimuth.min_angle) .cwiseQuotient(Index2D(config.elevation.num_cells - 1, config.azimuth.num_cells - 1) .cast()), - Vector2D(config.elevation.min_angle, config.azimuth.min_angle)), + {config.elevation.min_angle, config.azimuth.min_angle}), config_(config.checkValid()) {} Eigen::Matrix SphericalProjector::sensorAxisIsPeriodic() const { From 8496ee7053820f5f5c0ae2f6d699af7c103e511d Mon Sep 17 00:00:00 2001 From: Victor Reijgwart Date: Fri, 29 Sep 2023 16:53:50 +0200 Subject: [PATCH 15/70] Name thread worker pool threads --- libraries/wavemap/src/utils/thread_pool.cc | 11 ++++++++++- 1 file changed, 10 insertions(+), 1 deletion(-) diff --git a/libraries/wavemap/src/utils/thread_pool.cc b/libraries/wavemap/src/utils/thread_pool.cc index 169ea45a7..6f59c61f4 100644 --- a/libraries/wavemap/src/utils/thread_pool.cc +++ b/libraries/wavemap/src/utils/thread_pool.cc @@ -1,12 +1,21 @@ #include "wavemap/utils/thread_pool.h" +#include + namespace wavemap { ThreadPool::ThreadPool(size_t thread_count) : task_count_(0), terminate_(false) { // Create the worker threads + static int pool_id = 0; for (size_t i = 0; i < thread_count; ++i) { - workers_.emplace_back([this] { worker_loop(); }); + const std::string thread_name = + "pool_" + std::to_string(pool_id) + "_worker_" + std::to_string(i); + workers_.emplace_back([this, thread_name] { + tracy::SetThreadName(thread_name.c_str()); + worker_loop(); + }); } + ++pool_id; } ThreadPool::~ThreadPool() { From e21d0965f8e9bf1375384361ffb761784d4a887c Mon Sep 17 00:00:00 2001 From: Victor Reijgwart Date: Fri, 29 Sep 2023 17:39:46 +0200 Subject: [PATCH 16/70] Make ROS logging level configurable through ROS params --- .../include/wavemap_ros/logging_level.h | 22 +++++++++++++++++++ .../include/wavemap_ros/wavemap_server.h | 5 ++++- .../depth_image_input_handler.cc | 16 +++++++------- .../input_handler/pointcloud_input_handler.cc | 16 +++++++------- ros/wavemap_ros/src/wavemap_server.cc | 10 ++++++++- tooling/schemas/wavemap/wavemap_server.json | 16 ++++++++++++++ 6 files changed, 67 insertions(+), 18 deletions(-) create mode 100644 ros/wavemap_ros/include/wavemap_ros/logging_level.h diff --git a/ros/wavemap_ros/include/wavemap_ros/logging_level.h b/ros/wavemap_ros/include/wavemap_ros/logging_level.h new file mode 100644 index 000000000..6cafd8650 --- /dev/null +++ b/ros/wavemap_ros/include/wavemap_ros/logging_level.h @@ -0,0 +1,22 @@ +#ifndef WAVEMAP_ROS_LOGGING_LEVEL_H_ +#define WAVEMAP_ROS_LOGGING_LEVEL_H_ + +#include +#include + +namespace wavemap { +struct LoggingLevel : public TypeSelector { + using TypeSelector::TypeSelector; + + enum Id : TypeId { kDebug, kInfo, kWarning, kError, kFatal }; + + static constexpr std::array names = {"debug", "info", "warning", "error", + "fatal"}; + static constexpr std::array ros_levels = { + ros::console::levels::Debug, ros::console::levels::Info, + ros::console::levels::Warn, ros::console::levels::Error, + ros::console::levels::Fatal}; +}; +} // namespace wavemap + +#endif // WAVEMAP_ROS_LOGGING_LEVEL_H_ diff --git a/ros/wavemap_ros/include/wavemap_ros/wavemap_server.h b/ros/wavemap_ros/include/wavemap_ros/wavemap_server.h index 7ffe44661..11bee15ab 100644 --- a/ros/wavemap_ros/include/wavemap_ros/wavemap_server.h +++ b/ros/wavemap_ros/include/wavemap_ros/wavemap_server.h @@ -18,6 +18,7 @@ #include #include #include +#include #include "wavemap_ros/input_handler/input_handler.h" #include "wavemap_ros/tf_transformer.h" @@ -26,7 +27,7 @@ namespace wavemap { /** * Config struct for wavemap's ROS server. */ -struct WavemapServerConfig : ConfigBase { +struct WavemapServerConfig : ConfigBase { //! Name of the coordinate frame in which to store the map. //! Will be used as the frame_id for ROS TF lookups. std::string world_frame = "odom"; @@ -47,6 +48,8 @@ struct WavemapServerConfig : ConfigBase { //! Defaults to the number of threads supported by the CPU. int num_threads = std::max(1, static_cast(std::thread::hardware_concurrency())); + //! Minimum severity level for ROS logging messages to be logged. + LoggingLevel logging_level = LoggingLevel::kInfo; static MemberMap memberMap; diff --git a/ros/wavemap_ros/src/input_handler/depth_image_input_handler.cc b/ros/wavemap_ros/src/input_handler/depth_image_input_handler.cc index 9a0d8d663..37998b371 100644 --- a/ros/wavemap_ros/src/input_handler/depth_image_input_handler.cc +++ b/ros/wavemap_ros/src/input_handler/depth_image_input_handler.cc @@ -104,19 +104,19 @@ void DepthImageInputHandler::processQueue() { posed_range_image.setPose(T_W_C); // Integrate the depth image - ROS_INFO_STREAM("Inserting depth image with " - << EigenFormat::oneLine(posed_range_image.getDimensions()) - << " points. Remaining pointclouds in queue: " - << depth_image_queue_.size() - 1 << "."); + ROS_DEBUG_STREAM("Inserting depth image with " + << EigenFormat::oneLine(posed_range_image.getDimensions()) + << " points. Remaining pointclouds in queue: " + << depth_image_queue_.size() - 1 << "."); integration_timer_.start(); for (const auto& integrator : scanwise_integrators_) { integrator->integrateRangeImage(posed_range_image); } integration_timer_.stop(); - ROS_INFO_STREAM("Integrated new depth image in " - << integration_timer_.getLastEpisodeDuration() - << "s. Total integration time: " - << integration_timer_.getTotalDuration() << "s."); + ROS_DEBUG_STREAM("Integrated new depth image in " + << integration_timer_.getLastEpisodeDuration() + << "s. Total integration time: " + << integration_timer_.getTotalDuration() << "s."); // Publish debugging visualizations if (shouldPublishReprojectedPointcloud()) { diff --git a/ros/wavemap_ros/src/input_handler/pointcloud_input_handler.cc b/ros/wavemap_ros/src/input_handler/pointcloud_input_handler.cc index 33b9762fa..def282c92 100644 --- a/ros/wavemap_ros/src/input_handler/pointcloud_input_handler.cc +++ b/ros/wavemap_ros/src/input_handler/pointcloud_input_handler.cc @@ -229,19 +229,19 @@ void PointcloudInputHandler::processQueue() { } // Integrate the pointcloud - ROS_INFO_STREAM("Inserting pointcloud with " - << posed_pointcloud.size() - << " points. Remaining pointclouds in queue: " - << pointcloud_queue_.size() - 1 << "."); + ROS_DEBUG_STREAM("Inserting pointcloud with " + << posed_pointcloud.size() + << " points. Remaining pointclouds in queue: " + << pointcloud_queue_.size() - 1 << "."); integration_timer_.start(); for (const auto& integrator : integrators_) { integrator->integratePointcloud(posed_pointcloud); } integration_timer_.stop(); - ROS_INFO_STREAM("Integrated new pointcloud in " - << integration_timer_.getLastEpisodeDuration() - << "s. Total integration time: " - << integration_timer_.getTotalDuration() << "s."); + ROS_DEBUG_STREAM("Integrated new pointcloud in " + << integration_timer_.getLastEpisodeDuration() + << "s. Total integration time: " + << integration_timer_.getTotalDuration() << "s."); // Publish debugging visualizations if (shouldPublishReprojectedPointcloud()) { diff --git a/ros/wavemap_ros/src/wavemap_server.cc b/ros/wavemap_ros/src/wavemap_server.cc index d5a1fa3da..a99130748 100644 --- a/ros/wavemap_ros/src/wavemap_server.cc +++ b/ros/wavemap_ros/src/wavemap_server.cc @@ -19,7 +19,8 @@ DECLARE_CONFIG_MEMBERS(WavemapServerConfig, (pruning_period) (publication_period) (max_num_blocks_per_msg) - (num_threads)); + (num_threads) + (logging_level)); bool WavemapServerConfig::isValid(bool verbose) const { bool all_valid = true; @@ -43,6 +44,13 @@ WavemapServer::WavemapServer(ros::NodeHandle nh, ros::NodeHandle nh_private, const WavemapServerConfig& config) : config_(config.checkValid()), transformer_(std::make_shared()) { + // Set the ROS logging level + if (ros::console::set_logger_level( + ROSCONSOLE_DEFAULT_NAME, + LoggingLevel::ros_levels[config_.logging_level.toTypeId()])) { + ros::console::notifyLoggerLevelsChanged(); + } + // Setup data structure const auto data_structure_params = param::convert::toParamValue(nh_private, "map/data_structure"); diff --git a/tooling/schemas/wavemap/wavemap_server.json b/tooling/schemas/wavemap/wavemap_server.json index 8c9275ecf..d15fb3d0e 100644 --- a/tooling/schemas/wavemap/wavemap_server.json +++ b/tooling/schemas/wavemap/wavemap_server.json @@ -28,6 +28,22 @@ "description": "Maximum number of blocks to transmit per wavemap map message. Used to control the maximum message size. Only works in combination with hash-based map data structures.", "type": "integer", "exclusiveMinimum": 0 + }, + "num_threads": { + "description": "Maximum number of threads to use. Defaults to the number of threads supported by the CPU.", + "type": "integer", + "exclusiveMinimum": 0 + }, + "logging_level": { + "description": "Minimum severity level for ROS logging messages to be logged. Defaults to \"info\".", + "type": "string", + "enum": [ + "debug", + "info", + "warning", + "error", + "fatal" + ] } } } From 9a4aa61f1b55061a03ba37bd422cdafb69c54ee2 Mon Sep 17 00:00:00 2001 From: Victor Reijgwart Date: Fri, 29 Sep 2023 17:58:33 +0200 Subject: [PATCH 17/70] Consistently use ROS logging in ROS packages --- .../impl/pointcloud_input_handler_impl.h | 11 ++++++----- .../src/input_handler/input_handler.cc | 10 ++++++---- .../src/input_handler/input_handler_factory.cc | 12 ++++++------ ros/wavemap_ros/src/rosbag_processor.cc | 2 +- ros/wavemap_ros/src/wavemap_server.cc | 6 +++++- .../src/config_conversions.cc | 16 ++++++++-------- .../src/map_msg_conversions.cc | 17 ++++++++++------- .../src/visuals/grid_layer.cpp | 5 +++-- 8 files changed, 45 insertions(+), 34 deletions(-) diff --git a/ros/wavemap_ros/include/wavemap_ros/input_handler/impl/pointcloud_input_handler_impl.h b/ros/wavemap_ros/include/wavemap_ros/input_handler/impl/pointcloud_input_handler_impl.h index 877a3195d..589475bb9 100644 --- a/ros/wavemap_ros/include/wavemap_ros/input_handler/impl/pointcloud_input_handler_impl.h +++ b/ros/wavemap_ros/include/wavemap_ros/input_handler/impl/pointcloud_input_handler_impl.h @@ -29,14 +29,15 @@ bool PointcloudInputHandler::registerCallback(PointcloudTopicType type, // clang-format on return true; #else - LOG(ERROR) << "Livox support is currently not available. Please install " - "livox_ros_driver2 and rebuild wavemap."; + ROS_ERROR( + "Livox support is currently not available. Please install " + "livox_ros_driver2 and rebuild wavemap."); return false; #endif default: - LOG(ERROR) << "Requested callback registration for unknown " - "PointcloudTopicType \"" - << type.toStr() << "\""; + ROS_ERROR_STREAM( + "Requested callback registration for unknown PointcloudTopicType \"" + << type.toStr() << "\""); return false; } } diff --git a/ros/wavemap_ros/src/input_handler/input_handler.cc b/ros/wavemap_ros/src/input_handler/input_handler.cc index 6fc961dba..53f1f9d8b 100644 --- a/ros/wavemap_ros/src/input_handler/input_handler.cc +++ b/ros/wavemap_ros/src/input_handler/input_handler.cc @@ -40,14 +40,16 @@ InputHandler::InputHandler(const InputHandlerConfig& config, // Create the integrators const auto integrators_param = params.getChild("integrators"); if (!integrators_param) { - LOG(WARNING) << "Could not find key named \"integrators\" in input handler " - "params. Input handler will be disabled."; + ROS_WARN( + "Could not find key named \"integrators\" in input handler " + "params. Input handler will be disabled."); return; } const auto integrators_array = integrators_param->get(); if (!integrators_array) { - LOG(WARNING) << "Key named \"integrators\" in input handler params is not " - "of type Array (list). Input handler will be disabled."; + ROS_WARN( + "Key named \"integrators\" in input handler params is not " + "of type Array (list). Input handler will be disabled."); return; } for (const auto& integrator_params : integrators_array.value()) { diff --git a/ros/wavemap_ros/src/input_handler/input_handler_factory.cc b/ros/wavemap_ros/src/input_handler/input_handler_factory.cc index 95e3d593d..934167607 100644 --- a/ros/wavemap_ros/src/input_handler/input_handler_factory.cc +++ b/ros/wavemap_ros/src/input_handler/input_handler_factory.cc @@ -18,16 +18,16 @@ std::unique_ptr InputHandlerFactory::create( } if (default_input_handler_type.has_value()) { - LOG(WARNING) << "Default type \"" - << default_input_handler_type.value().toStr() - << "\" will be created instead."; + ROS_WARN_STREAM("Default type \"" + << default_input_handler_type.value().toStr() + << "\" will be created instead."); return create(default_input_handler_type.value(), params, std::move(world_frame), std::move(occupancy_map), std::move(transformer), std::move(thread_pool), nh, nh_private); } - LOG(ERROR) << "No default was set. Returning nullptr."; + ROS_ERROR("No default was set. Returning nullptr."); return nullptr; } @@ -48,7 +48,7 @@ std::unique_ptr InputHandlerFactory::create( std::move(occupancy_map), std::move(transformer), std::move(thread_pool), nh, nh_private); } else { - LOG(ERROR) << "Pointcloud input handler config could not be loaded."; + ROS_ERROR("Pointcloud input handler config could not be loaded."); return nullptr; } } @@ -61,7 +61,7 @@ std::unique_ptr InputHandlerFactory::create( std::move(occupancy_map), std::move(transformer), std::move(thread_pool), nh, nh_private); } else { - LOG(ERROR) << "Depth image input handler config could not be loaded."; + ROS_ERROR("Depth image input handler config could not be loaded."); return nullptr; } } diff --git a/ros/wavemap_ros/src/rosbag_processor.cc b/ros/wavemap_ros/src/rosbag_processor.cc index 6b75b39ff..35b8a7f4c 100644 --- a/ros/wavemap_ros/src/rosbag_processor.cc +++ b/ros/wavemap_ros/src/rosbag_processor.cc @@ -13,7 +13,7 @@ RosbagProcessor::~RosbagProcessor() { void RosbagProcessor::addRosbag(const std::string& rosbag_path) { opened_rosbags_.emplace_back(rosbag_path); bag_view_.addQuery(opened_rosbags_.back()); - LOG(INFO) << "Loaded rosbag " << rosbag_path; + ROS_INFO_STREAM("Loaded rosbag " << rosbag_path); } bool RosbagProcessor::addRosbags(std::istringstream& rosbag_paths) { diff --git a/ros/wavemap_ros/src/wavemap_server.cc b/ros/wavemap_ros/src/wavemap_server.cc index a99130748..c65cd5b54 100644 --- a/ros/wavemap_ros/src/wavemap_server.cc +++ b/ros/wavemap_ros/src/wavemap_server.cc @@ -57,6 +57,10 @@ WavemapServer::WavemapServer(ros::NodeHandle nh, ros::NodeHandle nh_private, occupancy_map_ = VolumetricDataStructureFactory::create( data_structure_params, VolumetricDataStructureType::kHashedBlocks); CHECK_NOTNULL(occupancy_map_); + + // Setup thread pool + ROS_INFO_STREAM("Creating thread pool with " << config_.num_threads + << " threads."); thread_pool_ = std::make_shared(config_.num_threads); CHECK_NOTNULL(thread_pool_); @@ -102,7 +106,7 @@ bool WavemapServer::saveMap(const std::filesystem::path& file_path) const { occupancy_map_->threshold(); return io::mapToFile(*occupancy_map_, file_path); } else { - LOG(ERROR) << "Could not save map because it has not yet been allocated."; + ROS_ERROR("Could not save map because it has not yet been allocated."); } return false; } diff --git a/ros/wavemap_ros_conversions/src/config_conversions.cc b/ros/wavemap_ros_conversions/src/config_conversions.cc index 615888dc4..ef9926361 100644 --- a/ros/wavemap_ros_conversions/src/config_conversions.cc +++ b/ros/wavemap_ros_conversions/src/config_conversions.cc @@ -7,8 +7,8 @@ param::Map toParamMap(const ros::NodeHandle& nh, const std::string& ns) { return toParamMap(xml_rpc_value); } - LOG(WARNING) << "Could not load ROS params under namespace " - << nh.resolveName(ns); + ROS_WARN_STREAM("Could not load ROS params under namespace " + << nh.resolveName(ns)); return {}; } @@ -18,8 +18,8 @@ param::Array toParamArray(const ros::NodeHandle& nh, const std::string& ns) { return toParamArray(xml_rpc_value); } - LOG(WARNING) << "Could not load ROS params under namespace " - << nh.resolveName(ns); + ROS_WARN_STREAM("Could not load ROS params under namespace " + << nh.resolveName(ns)); return {}; } @@ -29,15 +29,15 @@ param::Value toParamValue(const ros::NodeHandle& nh, const std::string& ns) { return toParamValue(xml_rpc_value); } - LOG(WARNING) << "Could not load ROS params under namespace " - << nh.resolveName(ns); + ROS_WARN_STREAM("Could not load ROS params under namespace " + << nh.resolveName(ns)); return param::Value{param::Map{}}; // Return an empty map } param::Map toParamMap( // NOLINT const XmlRpc::XmlRpcValue& xml_rpc_value) { if (xml_rpc_value.getType() != XmlRpc::XmlRpcValue::TypeStruct) { - LOG(WARNING) << "Expected param map."; + ROS_WARN("Expected param map."); return {}; } @@ -51,7 +51,7 @@ param::Map toParamMap( // NOLINT param::Array toParamArray( // NOLINT const XmlRpc::XmlRpcValue& xml_rpc_value) { if (xml_rpc_value.getType() != XmlRpc::XmlRpcValue::TypeArray) { - LOG(WARNING) << "Expected param array."; + ROS_WARN("Expected param array."); return {}; } diff --git a/ros/wavemap_ros_conversions/src/map_msg_conversions.cc b/ros/wavemap_ros_conversions/src/map_msg_conversions.cc index 9f81b27f5..e67082040 100644 --- a/ros/wavemap_ros_conversions/src/map_msg_conversions.cc +++ b/ros/wavemap_ros_conversions/src/map_msg_conversions.cc @@ -1,5 +1,6 @@ #include "wavemap_ros_conversions/map_msg_conversions.h" +#include #include namespace wavemap::convert { @@ -31,8 +32,9 @@ bool mapToRosMsg(const VolumetricDataStructureBase& map, return true; } - LOG(WARNING) << "Could not serialize requested map to ROS msg. " - "Map type not yet supported."; + ROS_WARN( + "Could not serialize requested map to ROS msg. " + "Map type not yet supported."); return false; } @@ -42,9 +44,9 @@ bool rosMsgToMap(const wavemap_msgs::Map& msg, // Check validity if ((msg.wavelet_octree.size() == 1) != (msg.hashed_wavelet_octree.size() != 1)) { - LOG(WARNING) - << "Maps must be serialized either as wavelet octrees or hashed " - "wavelet octrees. Encountered message contains both. Ignoring."; + ROS_WARN( + "Maps must be serialized either as wavelet octrees or hashed " + "wavelet octrees. Encountered message contains both. Ignoring."); map = nullptr; return false; } @@ -64,8 +66,9 @@ bool rosMsgToMap(const wavemap_msgs::Map& msg, return true; } - LOG(WARNING) << "Conversion of the requested map ROS msg to a wavemap map is " - "not yet supported."; + ROS_WARN( + "Conversion of the requested map ROS msg to a wavemap map is " + "not yet supported."); map = nullptr; return false; } diff --git a/ros/wavemap_rviz_plugin/src/visuals/grid_layer.cpp b/ros/wavemap_rviz_plugin/src/visuals/grid_layer.cpp index cf908dc3c..feca86ce8 100644 --- a/ros/wavemap_rviz_plugin/src/visuals/grid_layer.cpp +++ b/ros/wavemap_rviz_plugin/src/visuals/grid_layer.cpp @@ -9,6 +9,7 @@ #include #include #include +#include #include #include @@ -51,8 +52,8 @@ GridLayer::GridLayer(const Ogre::MaterialPtr& cell_material) current_mode_supports_geometry_shader_ = true; } } else { - LOG(ERROR) << "No techniques available for material [%s]", - cell_material_->getName().c_str(); + ROS_ERROR_STREAM("No techniques available for material " + << cell_material_->getName()); } } From 98b25fe22a0232cd563f97345983cdd79bb36390 Mon Sep 17 00:00:00 2001 From: Victor Reijgwart Date: Sun, 1 Oct 2023 14:18:17 +0200 Subject: [PATCH 18/70] Switch to dedicated struct for sensor coordinates --- libraries/wavemap/include/wavemap/common.h | 6 ++ .../measurement_model/continuous_beam.h | 2 +- .../measurement_model/continuous_ray.h | 4 +- .../impl/continuous_beam_inl.h | 16 +++--- .../impl/continuous_ray_inl.h | 17 +++--- .../measurement_model_base.h | 2 +- .../impl/ouster_projector_inl.h | 30 +++------- .../impl/pinhole_camera_projector_inl.h | 23 +++----- .../impl/projector_base_inl.h | 17 +++--- .../impl/spherical_projector_inl.h | 31 ++++------ .../projection_model/ouster_projector.h | 26 +++------ .../pinhole_camera_projector.h | 28 +++------- .../projection_model/projector_base.h | 56 ++++++++++++------- .../projection_model/spherical_projector.h | 26 +++------ .../impl/projective_integrator_inl.h | 6 +- .../projection_model/ouster_projector.cc | 6 +- .../pinhole_camera_projector.cc | 26 +++++++-- .../projection_model/spherical_projector.cc | 6 +- .../fixed_resolution_integrator.cc | 6 +- .../projective/projective_integrator.cc | 6 +- .../projection_model/test_image_projectors.cc | 20 ++++--- .../test_spherical_projector.cc | 15 ++--- .../integrator/test_pointcloud_integrators.cc | 2 +- 23 files changed, 182 insertions(+), 195 deletions(-) diff --git a/libraries/wavemap/include/wavemap/common.h b/libraries/wavemap/include/wavemap/common.h index b5619dfb9..6a659e694 100644 --- a/libraries/wavemap/include/wavemap/common.h +++ b/libraries/wavemap/include/wavemap/common.h @@ -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 normal; +}; + constexpr auto kEpsilon = constants::kEpsilon; constexpr auto kNaN = std::numeric_limits::quiet_NaN(); 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..424493d0d 100644 --- a/libraries/wavemap/include/wavemap/integrator/measurement_model/continuous_beam.h +++ b/libraries/wavemap/include/wavemap/integrator/measurement_model/continuous_beam.h @@ -75,7 +75,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_; 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..6bc9575a9 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_; @@ -78,7 +78,7 @@ class ContinuousRay : public MeasurementModelBase { // 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 bc17c9478..0425d6376 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,15 +31,15 @@ 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 { + const FloatingPoint cell_to_sensor_distance = sensor_coordinates.normal; 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>()); + sensor_coordinates.image); if (!range_image_->isIndexWithinBounds(image_index)) { return 0.f; } @@ -49,8 +49,8 @@ inline FloatingPoint ContinuousBeam::computeUpdate( // Compute the image error norm const FloatingPoint cell_to_beam_image_error_norm = - projection_model_->imageOffsetToErrorNorm( - sensor_coordinates.head<2>(), cell_to_beam_offset); + projection_model_->imageOffsetToErrorNorm(sensor_coordinates.image, + cell_to_beam_offset); // Compute the update return computeBeamUpdate(cell_to_sensor_distance, @@ -64,7 +64,7 @@ inline FloatingPoint ContinuousBeam::computeUpdate( ProjectorBase::CellToBeamOffsetArray cell_to_beam_offsets{}; const auto [image_indices, cell_offsets] = projection_model_->imageToNearestIndicesAndOffsets( - sensor_coordinates.head<2>()); + sensor_coordinates.image); 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]; @@ -78,8 +78,8 @@ inline FloatingPoint ContinuousBeam::computeUpdate( // Compute the image error norms const auto cell_to_beam_image_error_norms = - projection_model_->imageOffsetsToErrorNorms( - sensor_coordinates.head<2>(), cell_to_beam_offsets); + projection_model_->imageOffsetsToErrorNorms(sensor_coordinates.image, + cell_to_beam_offsets); // Compute the update FloatingPoint update = 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..bed446eec 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,17 +25,17 @@ 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]; update += computeBeamUpdate(sensor_coordinates, image_index); @@ -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.normal - measured_distance) { return 0.f; } - if (sensor_coordinates.z() < measured_distance - range_threshold_front_) { + if (sensor_coordinates.normal < measured_distance - range_threshold_front_) { return computeFreeSpaceBeamUpdate(); } else { - return computeFullBeamUpdate(sensor_coordinates.z(), measured_distance); + return computeFullBeamUpdate(sensor_coordinates.normal, 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 733c7fb91..6061b83b6 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 @@ -2,7 +2,7 @@ #define WAVEMAP_INTEGRATOR_PROJECTION_MODEL_IMPL_OUSTER_PROJECTOR_INL_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 @@ -15,14 +15,14 @@ inline Vector3D OusterProjector::cartesianToSensor( const FloatingPoint azimuth_angle = approximate::atan2()(C_point.y(), C_point.x()); 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.normal; const Point2D B_point = range * Vector2D(std::cos(elevation_angle), std::sin(elevation_angle)) + Vector2D(config_.lidar_origin_to_beam_origin, @@ -33,7 +33,7 @@ inline Point3D OusterProjector::sensorToCartesian( } inline FloatingPoint OusterProjector::imageOffsetToErrorNorm( - const Vector2D& linearization_point, const Vector2D& offset) const { + 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]); @@ -43,7 +43,7 @@ inline FloatingPoint OusterProjector::imageOffsetToErrorNorm( } inline std::array OusterProjector::imageOffsetsToErrorNorms( - const Vector2D& linearization_point, + const ImageCoordinates& linearization_point, const ProjectorBase::CellToBeamOffsetArray& offsets) const { const FloatingPoint cos_elevation_angle = std::cos(linearization_point[0]); const FloatingPoint cos_elevation_angle_sq = @@ -58,7 +58,7 @@ inline std::array OusterProjector::imageOffsetsToErrorNorms( 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 @@ -80,18 +80,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 44a2a8082..1ec42ad39 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,39 +4,34 @@ #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.normal; 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&, const Vector2D& offset) const { + const ImageCoordinates& /*linearization_point*/, + const Vector2D& offset) const { return offset.norm(); } inline std::array PinholeCameraProjector::imageOffsetsToErrorNorms( - const Vector2D&, + const ImageCoordinates& /*linearization_point*/, const ProjectorBase::CellToBeamOffsetArray& offsets) const { std::array error_norms{}; for (int offset_idx = 0; offset_idx < 4; ++offset_idx) { 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 28bf91b35..78a2c7120 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,7 +29,7 @@ inline Index2D ProjectorBase::imageToCeilIndex( } inline std::pair ProjectorBase::imageToNearestIndexAndOffset( - const Vector2D& image_coordinates) const { + const ImageCoordinates& image_coordinates) const { const Vector2D index = imageToIndexReal(image_coordinates); const Vector2D index_rounded = index.array().round(); Vector2D image_coordinate_offset = @@ -39,7 +39,7 @@ inline std::pair ProjectorBase::imageToNearestIndexAndOffset( } inline std::array ProjectorBase::imageToNearestIndices( - const Vector2D& image_coordinates) const { + const ImageCoordinates& image_coordinates) const { const Vector2D index = imageToIndexReal(image_coordinates); const Vector2D index_lower = index.array().floor(); const Vector2D index_upper = index.array().ceil(); @@ -57,7 +57,7 @@ inline std::array ProjectorBase::imageToNearestIndices( inline std::pair, std::array> ProjectorBase::imageToNearestIndicesAndOffsets( - const Vector2D& image_coordinates) const { + const ImageCoordinates& image_coordinates) const { const Vector2D index = imageToIndexReal(image_coordinates); const Vector2D index_lower = index.array().floor(); const Vector2D index_upper = index.array().ceil(); @@ -78,14 +78,15 @@ ProjectorBase::imageToNearestIndicesAndOffsets( return {std::move(indices), std::move(offsets)}; } -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 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 7bdb02f5a..6db8435c1 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,19 +1,21 @@ #ifndef WAVEMAP_INTEGRATOR_PROJECTION_MODEL_IMPL_SPHERICAL_PROJECTOR_INL_H_ #define WAVEMAP_INTEGRATOR_PROJECTION_MODEL_IMPL_SPHERICAL_PROJECTOR_INL_H_ +#include + 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 SensorCoordinates& coordinates) const { + const FloatingPoint elevation_angle = coordinates.image[0]; + const FloatingPoint azimuth_angle = coordinates.image[1]; + const FloatingPoint range = coordinates.normal; const Vector3D bearing{std::cos(elevation_angle) * std::cos(azimuth_angle), std::cos(elevation_angle) * std::sin(azimuth_angle), std::sin(elevation_angle)}; @@ -21,7 +23,7 @@ inline Point3D SphericalProjector::sensorToCartesian( } inline FloatingPoint SphericalProjector::imageOffsetToErrorNorm( - const Vector2D& linearization_point, const Vector2D& offset) const { + 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]); @@ -32,7 +34,7 @@ inline FloatingPoint SphericalProjector::imageOffsetToErrorNorm( inline std::array SphericalProjector::imageOffsetsToErrorNorms( - const Vector2D& linearization_point, + const ImageCoordinates& linearization_point, const ProjectorBase::CellToBeamOffsetArray& offsets) const { const FloatingPoint cos_elevation_angle = std::cos(linearization_point[0]); const FloatingPoint cos_elevation_angle_sq = @@ -47,7 +49,7 @@ SphericalProjector::imageOffsetsToErrorNorms( 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()); @@ -60,15 +62,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 3f270671e..f1d7a174a 100644 --- a/libraries/wavemap/include/wavemap/integrator/projection_model/ouster_projector.h +++ b/libraries/wavemap/include/wavemap/integrator/projection_model/ouster_projector.h @@ -51,12 +51,6 @@ class OusterProjector : public ProjectorBase { explicit OusterProjector(const Config& config); - 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}; @@ -64,21 +58,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, - const Vector2D& offset) const final; + SensorCoordinates cartesianToSensor(const Point3D& C_point) const final; + Point3D sensorToCartesian(const SensorCoordinates& coordinates) const final; + FloatingPoint imageOffsetToErrorNorm( + const ImageCoordinates& linearization_point, + const Vector2D& offset) const final; std::array imageOffsetsToErrorNorms( - const Vector2D& linearization_point, + 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 @@ -90,8 +80,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 57360b9e2..a2dc2138b 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,17 +45,8 @@ class PinholeCameraProjector : public ProjectorBase { public: using Config = PinholeCameraProjectorConfig; - explicit PinholeCameraProjector(const Config& config) - : ProjectorBase({config.width, config.height}, Vector2D::Ones(), - Vector2D::Zero()), - config_(config.checkValid()) {} + explicit PinholeCameraProjector(const Config& config); - Vector2D getMinImageCoordinates() const final { - return indexToImage(Index2D::Zero()); - } - Vector2D getMaxImageCoordinates() const final { - return {indexToImage({config_.width - 1, config_.height - 1})}; - } Eigen::Matrix sensorAxisIsPeriodic() const final { return {false, false, false}; } @@ -65,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*/, - const Vector2D& offset) const final; + SensorCoordinates cartesianToSensor(const Point3D& C_point) const final; + Point3D sensorToCartesian(const SensorCoordinates& coordinates) const final; + FloatingPoint imageOffsetToErrorNorm( + const ImageCoordinates& /*linearization_point*/, + const Vector2D& offset) const final; std::array imageOffsetsToErrorNorms( - const Vector2D& /*linearization_point*/, + 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 0230eab88..0e5aa045a 100644 --- a/libraries/wavemap/include/wavemap/integrator/projection_model/projector_base.h +++ b/libraries/wavemap/include/wavemap/integrator/projection_model/projector_base.h @@ -29,45 +29,57 @@ class ProjectorBase { using ConstPtr = std::shared_ptr; ProjectorBase(Index2D dimensions, Vector2D index_to_image_scale_factor, - Vector2D image_offset) + 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)) {} + 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; IndexElement getNumRows() const { return dimensions_.x(); } IndexElement getNumColumns() const { return dimensions_.y(); } Index2D getDimensions() const { return dimensions_; } - virtual Vector2D getMinImageCoordinates() const = 0; - virtual Vector2D getMaxImageCoordinates() const = 0; + 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; + Index2D imageToNearestIndex(const ImageCoordinates& image_coordinates) const; + Index2D imageToFloorIndex(const ImageCoordinates& image_coordinates) const; + Index2D imageToCeilIndex(const ImageCoordinates& image_coordinates) const; std::array imageToNearestIndices( - const Vector2D& image_coordinates) const; + const ImageCoordinates& image_coordinates) const; std::pair imageToNearestIndexAndOffset( - const Vector2D& image_coordinates) const; + const ImageCoordinates& image_coordinates) const; std::pair, std::array> - imageToNearestIndicesAndOffsets(const Vector2D& image_coordinates) const; + 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 @@ -75,10 +87,11 @@ class ProjectorBase { // 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, const Vector2D& offset) const = 0; + const ImageCoordinates& linearization_point, + const Vector2D& offset) const = 0; using CellToBeamOffsetArray = std::array; virtual std::array imageOffsetsToErrorNorms( - const Vector2D& linearization_point, + const ImageCoordinates& linearization_point, const CellToBeamOffsetArray& offsets) const = 0; // Convenience functions combining multiple of the above methods @@ -96,9 +109,12 @@ class ProjectorBase { 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 Vector2D& image_coordinates) const; + Vector2D imageToIndexReal(const ImageCoordinates& image_coordinates) const; }; } // 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 555e63a83..3df8d62ab 100644 --- a/libraries/wavemap/include/wavemap/integrator/projection_model/spherical_projector.h +++ b/libraries/wavemap/include/wavemap/integrator/projection_model/spherical_projector.h @@ -37,12 +37,6 @@ class SphericalProjector : public ProjectorBase { explicit SphericalProjector(const Config& config); - 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}; @@ -50,21 +44,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, - const Vector2D& offset) const final; + SensorCoordinates cartesianToSensor(const Point3D& C_point) const final; + Point3D sensorToCartesian(const SensorCoordinates& coordinates) const final; + FloatingPoint imageOffsetToErrorNorm( + const ImageCoordinates& linearization_point, + const Vector2D& offset) const final; std::array imageOffsetsToErrorNorms( - const Vector2D& linearization_point, + 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 @@ -76,8 +66,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/impl/projective_integrator_inl.h b/libraries/wavemap/include/wavemap/integrator/projective/impl/projective_integrator_inl.h index 802a673c3..3dfc1305a 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.normal < config_.min_range || + config_.max_range < sensor_coordinates.normal) { return 0.f; } diff --git a/libraries/wavemap/src/integrator/projection_model/ouster_projector.cc b/libraries/wavemap/src/integrator/projection_model/ouster_projector.cc index b4e6ade7c..d12a08bc6 100644 --- a/libraries/wavemap/src/integrator/projection_model/ouster_projector.cc +++ b/libraries/wavemap/src/integrator/projection_model/ouster_projector.cc @@ -11,7 +11,9 @@ OusterProjector::OusterProjector(const OusterProjector::Config& config) .cwiseQuotient(Index2D(config.elevation.num_cells - 1, config.azimuth.num_cells - 1) .cast()), - {config.elevation.min_angle, config.azimuth.min_angle}), + {config.elevation.min_angle, config.azimuth.min_angle}, + {config.elevation.min_angle, config.azimuth.min_angle}, + {config.elevation.max_angle, config.azimuth.max_angle}), config_(config.checkValid()) {} Eigen::Matrix OusterProjector::sensorAxisIsPeriodic() const { @@ -58,7 +60,7 @@ AABB OusterProjector::cartesianToSensorAABB( for (int corner_idx = 0; corner_idx < AABB::kNumCorners; ++corner_idx) { corner_sensor_coordinates[corner_idx] = - cartesianToImageApprox(C_aabb_corners[corner_idx]); + cartesianToImage(C_aabb_corners[corner_idx]); } for (const int axis : {0, 1}) { diff --git a/libraries/wavemap/src/integrator/projection_model/pinhole_camera_projector.cc b/libraries/wavemap/src/integrator/projection_model/pinhole_camera_projector.cc index 25a029325..2cf9133fd 100644 --- a/libraries/wavemap/src/integrator/projection_model/pinhole_camera_projector.cc +++ b/libraries/wavemap/src/integrator/projection_model/pinhole_camera_projector.cc @@ -1,6 +1,13 @@ #include "wavemap/integrator/projection_model/pinhole_camera_projector.h" namespace wavemap { +PinholeCameraProjector::PinholeCameraProjector( + const PinholeCameraProjector::Config& config) + : ProjectorBase({config.width, config.height}, Vector2D::Ones(), + Vector2D::Zero(), indexToImage(Index2D::Zero()), + indexToImage({config.width - 1, config.height - 1})), + config_(config.checkValid()) {} + AABB PinholeCameraProjector::cartesianToSensorAABB( const AABB& W_aabb, const kindr::minimal::QuatTransformationTemplate< @@ -21,7 +28,8 @@ AABB PinholeCameraProjector::cartesianToSensorAABB( } } - std::array::kNumCorners> corner_sensor_coordinates; + std::array::kNumCorners> + corner_sensor_coordinates; for (int corner_idx = 0; corner_idx < AABB::kNumCorners; ++corner_idx) { corner_sensor_coordinates[corner_idx] = @@ -29,16 +37,22 @@ AABB PinholeCameraProjector::cartesianToSensorAABB( } AABB sensor_coordinate_aabb; - for (int dim_idx = 0; dim_idx < 3; ++dim_idx) { - for (int corner_idx = 0; corner_idx < AABB::kNumCorners; - ++corner_idx) { + for (int corner_idx = 0; corner_idx < AABB::kNumCorners; + ++corner_idx) { + for (int dim_idx = 0; dim_idx < 2; ++dim_idx) { sensor_coordinate_aabb.min[dim_idx] = std::min(sensor_coordinate_aabb.min[dim_idx], - corner_sensor_coordinates[corner_idx][dim_idx]); + corner_sensor_coordinates[corner_idx].image[dim_idx]); sensor_coordinate_aabb.max[dim_idx] = std::max(sensor_coordinate_aabb.max[dim_idx], - corner_sensor_coordinates[corner_idx][dim_idx]); + corner_sensor_coordinates[corner_idx].image[dim_idx]); } + sensor_coordinate_aabb.min[2] = + std::min(sensor_coordinate_aabb.min[2], + corner_sensor_coordinates[corner_idx].normal); + sensor_coordinate_aabb.max[2] = + std::max(sensor_coordinate_aabb.max[2], + corner_sensor_coordinates[corner_idx].normal); } return sensor_coordinate_aabb; } diff --git a/libraries/wavemap/src/integrator/projection_model/spherical_projector.cc b/libraries/wavemap/src/integrator/projection_model/spherical_projector.cc index dcc28d5cc..5c0a7168e 100644 --- a/libraries/wavemap/src/integrator/projection_model/spherical_projector.cc +++ b/libraries/wavemap/src/integrator/projection_model/spherical_projector.cc @@ -11,7 +11,9 @@ SphericalProjector::SphericalProjector(const SphericalProjector::Config& config) .cwiseQuotient(Index2D(config.elevation.num_cells - 1, config.azimuth.num_cells - 1) .cast()), - {config.elevation.min_angle, config.azimuth.min_angle}), + {config.elevation.min_angle, config.azimuth.min_angle}, + {config.elevation.min_angle, config.azimuth.min_angle}, + {config.elevation.max_angle, config.azimuth.max_angle}), config_(config.checkValid()) {} Eigen::Matrix SphericalProjector::sensorAxisIsPeriodic() const { @@ -58,7 +60,7 @@ AABB SphericalProjector::cartesianToSensorAABB( for (int corner_idx = 0; corner_idx < AABB::kNumCorners; ++corner_idx) { corner_sensor_coordinates[corner_idx] = - cartesianToImageApprox(C_aabb_corners[corner_idx]); + cartesianToImage(C_aabb_corners[corner_idx]); } for (const int axis : {0, 1}) { diff --git a/libraries/wavemap/src/integrator/projective/fixed_resolution/fixed_resolution_integrator.cc b/libraries/wavemap/src/integrator/projective/fixed_resolution/fixed_resolution_integrator.cc index fe5d4ef34..54437fa09 100644 --- a/libraries/wavemap/src/integrator/projective/fixed_resolution/fixed_resolution_integrator.cc +++ b/libraries/wavemap/src/integrator/projective/fixed_resolution/fixed_resolution_integrator.cc @@ -21,11 +21,11 @@ void FixedResolutionIntegrator::importPointcloud( } // Add the point to the range image - const Vector3D sensor_coordinates = + const auto sensor_coordinates = projection_model_->cartesianToSensor(C_point); const auto [range_image_index, beam_to_pixel_offset] = projection_model_->imageToNearestIndexAndOffset( - sensor_coordinates.head<2>()); + sensor_coordinates.image); if (!posed_range_image_->isIndexWithinBounds(range_image_index)) { // Prevent out-of-bounds access continue; @@ -33,7 +33,7 @@ void FixedResolutionIntegrator::importPointcloud( // Add the point to the range image // If multiple points hit the same image pixel, keep the closest point - const FloatingPoint range = sensor_coordinates[2]; + const FloatingPoint range = sensor_coordinates.normal; const FloatingPoint old_range_value = posed_range_image_->at(range_image_index); if (old_range_value < config_.min_range || range < old_range_value) { diff --git a/libraries/wavemap/src/integrator/projective/projective_integrator.cc b/libraries/wavemap/src/integrator/projective/projective_integrator.cc index 646825620..f18b160c8 100644 --- a/libraries/wavemap/src/integrator/projective/projective_integrator.cc +++ b/libraries/wavemap/src/integrator/projective/projective_integrator.cc @@ -54,12 +54,12 @@ void ProjectiveIntegrator::importPointcloud( } // Calculate the range image index - const Vector3D sensor_coordinates = + const auto sensor_coordinates = projection_model_->cartesianToSensor(C_point); const auto [range_image_index, beam_to_pixel_offset] = projection_model_->imageToNearestIndexAndOffset( - sensor_coordinates.head<2>()); + sensor_coordinates.image); if (!posed_range_image_->isIndexWithinBounds(range_image_index)) { // Prevent out-of-bounds access continue; @@ -67,7 +67,7 @@ void ProjectiveIntegrator::importPointcloud( // Add the point to the range image, if multiple points hit the same image // pixel, keep the closest point - const FloatingPoint range = sensor_coordinates[2]; + const FloatingPoint range = sensor_coordinates.normal; const FloatingPoint old_range_value = posed_range_image_->at(range_image_index); if (old_range_value < config_.min_range || range < old_range_value) { diff --git a/libraries/wavemap/test/src/integrator/projection_model/test_image_projectors.cc b/libraries/wavemap/test/src/integrator/projection_model/test_image_projectors.cc index 70a2f6d11..39978981a 100644 --- a/libraries/wavemap/test/src/integrator/projection_model/test_image_projectors.cc +++ b/libraries/wavemap/test/src/integrator/projection_model/test_image_projectors.cc @@ -156,9 +156,9 @@ TYPED_TEST(Image2DProjectorTypedTest, Conversions) { // Get a random point in Cartesian space and ensure it's in the FoV const Point3D C_point = GeometryGenerator::getRandomPoint<3>(min_range, max_range); - const Vector3D sensor_coordinates = projector.cartesianToSensor(C_point); - const Vector2D image_coordinates = sensor_coordinates.head<2>(); - const FloatingPoint range_or_depth = sensor_coordinates[2]; + const auto sensor_coordinates = projector.cartesianToSensor(C_point); + const Vector2D image_coordinates = sensor_coordinates.image; + const FloatingPoint range_or_depth = sensor_coordinates.normal; if (range_or_depth < 1e-1f) { --repetition; continue; @@ -183,7 +183,8 @@ TYPED_TEST(Image2DProjectorTypedTest, Conversions) { << " with norm " << range << ", but after round trip it became " << EigenFormat::oneLine(C_point_roundtrip) << ". Intermediate sensor coordinates were " - << EigenFormat::oneLine(sensor_coordinates) << "."; + << EigenFormat::oneLine(sensor_coordinates.image) << ", " + << sensor_coordinates.normal << "."; } // Test sensor -> Cartesian -> sensor round trips @@ -192,7 +193,8 @@ TYPED_TEST(Image2DProjectorTypedTest, Conversions) { const Index2D image_index = GeometryGenerator::getRandomIndex<2>( Index2D::Zero(), projector.getDimensions()); const Vector2D image_coordinates = projector.indexToImage(image_index); - const Point3D C_point = projector.sensorToCartesian(image_coordinates, 1.f); + const Point3D C_point = + projector.sensorToCartesian({image_coordinates, 1.f}); const Index2D image_index_roundtrip = projector.cartesianToNearestIndex(C_point); @@ -252,11 +254,11 @@ TYPED_TEST(Image2DProjectorTypedTest, SensorCoordinateAABBs) { ++corner_idx) { const Point3D C_t_C_corner = test.T_W_C.inverse() * test.W_aabb.corner_point(corner_idx); - const Vector3D corner_sensor_coordinates = + const auto corner_sensor_coordinates = projector.cartesianToSensor(C_t_C_corner); - corners_x[corner_idx] = corner_sensor_coordinates.x(); - corners_y[corner_idx] = corner_sensor_coordinates.y(); - corners_z[corner_idx] = corner_sensor_coordinates.z(); + corners_x[corner_idx] = corner_sensor_coordinates.image.x(); + corners_y[corner_idx] = corner_sensor_coordinates.image.y(); + corners_z[corner_idx] = corner_sensor_coordinates.normal; } // Find the min/max corner coordinates for (auto [axis, coordinates] : diff --git a/libraries/wavemap/test/src/integrator/projection_model/test_spherical_projector.cc b/libraries/wavemap/test/src/integrator/projection_model/test_spherical_projector.cc index 963e09488..0fbba612e 100644 --- a/libraries/wavemap/test/src/integrator/projection_model/test_spherical_projector.cc +++ b/libraries/wavemap/test/src/integrator/projection_model/test_spherical_projector.cc @@ -54,10 +54,10 @@ TEST_F(SphericalProjectorTest, CellToBeamAngles) { const auto projector = getRandomProjectionModel(); for (int repetition = 0; repetition < 1000; ++repetition) { const Point3D C_point = getRandomPoint<3>(); - const Vector3D sensor_coordinates = projector.cartesianToSensor(C_point); + const auto sensor_coordinates = projector.cartesianToSensor(C_point); const auto [index, offset] = - projector.imageToNearestIndexAndOffset(sensor_coordinates.head<2>()); - if (sensor_coordinates[2] < 1e-1f || (index.array() < 0).any() || + projector.imageToNearestIndexAndOffset(sensor_coordinates.image); + if (sensor_coordinates.normal < 1e-1f || (index.array() < 0).any() || (projector.getDimensions().array() <= index.array()).any()) { --repetition; continue; @@ -66,11 +66,11 @@ TEST_F(SphericalProjectorTest, CellToBeamAngles) { const Point3D C_point_round_trip = projector.sensorToCartesian(sensor_coordinates); ASSERT_LE((C_point - C_point_round_trip).norm(), - kNoiseTolerance * (1.f + sensor_coordinates[2])); + kNoiseTolerance * (1.f + sensor_coordinates.normal)); // Compute "ground truth" using double precision const Point3D C_from_closest_pixel = projector.sensorToCartesian( - projector.indexToImage(index), sensor_coordinates[2]); + {projector.indexToImage(index), sensor_coordinates.normal}); const double projected_double = C_point.cast().dot(C_from_closest_pixel.cast()) / (C_point.cast().norm() * @@ -82,13 +82,14 @@ TEST_F(SphericalProjectorTest, CellToBeamAngles) { // Compute based on the offsets const FloatingPoint angle_from_offset = - projector.imageOffsetToErrorNorm(sensor_coordinates.head<2>(), offset); + projector.imageOffsetToErrorNorm(sensor_coordinates.image, offset); EXPECT_NEAR(angle_from_offset, angle, kMaxAcceptableAngleError) << "For C_point " << EigenFormat::oneLine(C_point) << ", C_from_closest_pixel " << EigenFormat::oneLine(C_from_closest_pixel) << " and intermediate sensor coordinates " - << EigenFormat::oneLine(sensor_coordinates); + << EigenFormat::oneLine(sensor_coordinates.image) << ", " + << sensor_coordinates.normal; } } diff --git a/libraries/wavemap/test/src/integrator/test_pointcloud_integrators.cc b/libraries/wavemap/test/src/integrator/test_pointcloud_integrators.cc index 28145dbcb..7b780282a 100644 --- a/libraries/wavemap/test/src/integrator/test_pointcloud_integrators.cc +++ b/libraries/wavemap/test/src/integrator/test_pointcloud_integrators.cc @@ -48,7 +48,7 @@ class PointcloudIntegratorTest : public FixtureBase, pointcloud_index / projection_model.getNumRows()}; pointcloud[pointcloud_index] = range * projection_model.sensorToCartesian( - projection_model.indexToImage(image_index), 1.f); + {projection_model.indexToImage(image_index), 1.f}); } return PosedPointcloud<>(getRandomTransformation<3>(), pointcloud); From 70cd901d454e4be3385dd869dd67b030e0343c69 Mon Sep 17 00:00:00 2001 From: Victor Reijgwart Date: Sun, 1 Oct 2023 19:22:43 +0200 Subject: [PATCH 19/70] Postpone image offset error norm root computation --- .../measurement_model/continuous_beam.h | 9 +++++--- .../measurement_model/continuous_ray.h | 1 - .../impl/continuous_beam_inl.h | 23 ++++++++++--------- .../impl/ouster_projector_inl.h | 18 +++++++-------- .../impl/pinhole_camera_projector_inl.h | 13 +++++------ .../impl/projector_base_inl.h | 16 +++++++++++++ .../impl/spherical_projector_inl.h | 17 +++++++------- .../projection_model/ouster_projector.h | 4 ++-- .../pinhole_camera_projector.h | 4 ++-- .../projection_model/projector_base.h | 10 ++++++-- .../projection_model/spherical_projector.h | 4 ++-- 11 files changed, 71 insertions(+), 48 deletions(-) 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 424493d0d..c7ce2d4c2 100644 --- a/libraries/wavemap/include/wavemap/integrator/measurement_model/continuous_beam.h +++ b/libraries/wavemap/include/wavemap/integrator/measurement_model/continuous_beam.h @@ -85,6 +85,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 @@ -93,9 +95,10 @@ class ContinuousBeam : public MeasurementModelBase { // sigma. // 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 6bc9575a9..faef614fb 100644 --- a/libraries/wavemap/include/wavemap/integrator/measurement_model/continuous_ray.h +++ b/libraries/wavemap/include/wavemap/integrator/measurement_model/continuous_ray.h @@ -77,7 +77,6 @@ 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 SensorCoordinates& sensor_coordinates, const Index2D& image_index) const; 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 0425d6376..7c3bb26ca 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 @@ -48,13 +48,13 @@ inline FloatingPoint ContinuousBeam::computeUpdate( 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.image, - cell_to_beam_offset); + const FloatingPoint cell_to_beam_image_error_norm_squared = + projection_model_->imageOffsetToErrorSquaredNorm( + sensor_coordinates.image, cell_to_beam_offset); // Compute the update return computeBeamUpdate(cell_to_sensor_distance, - cell_to_beam_image_error_norm, + cell_to_beam_image_error_norm_squared, measured_distance); } @@ -77,16 +77,16 @@ inline FloatingPoint ContinuousBeam::computeUpdate( } // Compute the image error norms - const auto cell_to_beam_image_error_norms = - projection_model_->imageOffsetsToErrorNorms(sensor_coordinates.image, - cell_to_beam_offsets); + 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; for (int neighbor_idx = 0; neighbor_idx < 4; ++neighbor_idx) { update += computeBeamUpdate(cell_to_sensor_distance, - cell_to_beam_image_error_norms[neighbor_idx], + cell_to_beam_image_error_norms_sq[neighbor_idx], measured_distances[neighbor_idx]); } return update; @@ -99,10 +99,10 @@ inline FloatingPoint ContinuousBeam::computeUpdate( 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; @@ -120,7 +120,8 @@ 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 g = + std::sqrt(cell_to_beam_image_error_norm_squared) / config_.angle_sigma; const FloatingPoint angle_contrib = ApproximateGaussianDistribution::cumulative(g + 3.f) - ApproximateGaussianDistribution::cumulative(g - 3.f); 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 6061b83b6..848426abd 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 @@ -32,28 +32,28 @@ inline Point3D OusterProjector::sensorToCartesian( return C_point; } -inline FloatingPoint OusterProjector::imageOffsetToErrorNorm( +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]); - return std::sqrt(offset[0] * offset[0] + - (cos_elevation_angle * cos_elevation_angle) * - (offset[1] * offset[1])); + return offset[0] * offset[0] + + (cos_elevation_angle * cos_elevation_angle) * (offset[1] * offset[1]); } -inline std::array OusterProjector::imageOffsetsToErrorNorms( +inline std::array +OusterProjector::imageOffsetsToErrorSquaredNorms( const ImageCoordinates& linearization_point, - const ProjectorBase::CellToBeamOffsetArray& offsets) const { + const CellToBeamOffsetArray& offsets) const { const FloatingPoint cos_elevation_angle = std::cos(linearization_point[0]); 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] = - std::sqrt((offsets[offset_idx][0] * offsets[offset_idx][0]) + - cos_elevation_angle_sq * - (offsets[offset_idx][1] * offsets[offset_idx][1])); + (offsets[offset_idx][0] * offsets[offset_idx][0]) + + cos_elevation_angle_sq * + (offsets[offset_idx][1] * offsets[offset_idx][1]); } return error_norms; } 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 1ec42ad39..34f6439a2 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 @@ -23,21 +23,20 @@ inline Point3D PinholeCameraProjector::sensorToCartesian( return C_point; } -inline FloatingPoint PinholeCameraProjector::imageOffsetToErrorNorm( +inline FloatingPoint PinholeCameraProjector::imageOffsetToErrorSquaredNorm( const ImageCoordinates& /*linearization_point*/, const Vector2D& offset) const { - return offset.norm(); + return offset.squaredNorm(); } inline std::array -PinholeCameraProjector::imageOffsetsToErrorNorms( +PinholeCameraProjector::imageOffsetsToErrorSquaredNorms( const ImageCoordinates& /*linearization_point*/, - const ProjectorBase::CellToBeamOffsetArray& offsets) const { + const CellToBeamOffsetArray& offsets) const { std::array error_norms{}; for (int offset_idx = 0; offset_idx < 4; ++offset_idx) { - error_norms[offset_idx] = - std::sqrt(offsets[offset_idx][0] * offsets[offset_idx][0] + - offsets[offset_idx][1] * offsets[offset_idx][1]); + error_norms[offset_idx] = offsets[offset_idx][0] * offsets[offset_idx][0] + + offsets[offset_idx][1] * offsets[offset_idx][1]; } 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 78a2c7120..9d875f95c 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 @@ -85,6 +85,22 @@ inline ImageCoordinates ProjectorBase::indexToImage( 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 ImageCoordinates& image_coordinates) const { return (image_coordinates - image_offset_) 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 6db8435c1..368080110 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 @@ -22,29 +22,28 @@ inline Point3D SphericalProjector::sensorToCartesian( return range * bearing; } -inline FloatingPoint SphericalProjector::imageOffsetToErrorNorm( +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]); - return std::sqrt(offset[0] * offset[0] + - (cos_elevation_angle * cos_elevation_angle) * - (offset[1] * offset[1])); + return offset[0] * offset[0] + + (cos_elevation_angle * cos_elevation_angle) * (offset[1] * offset[1]); } inline std::array -SphericalProjector::imageOffsetsToErrorNorms( +SphericalProjector::imageOffsetsToErrorSquaredNorms( const ImageCoordinates& linearization_point, - const ProjectorBase::CellToBeamOffsetArray& offsets) const { + const CellToBeamOffsetArray& offsets) const { const FloatingPoint cos_elevation_angle = std::cos(linearization_point[0]); 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] = - std::sqrt((offsets[offset_idx][0] * offsets[offset_idx][0]) + - cos_elevation_angle_sq * - (offsets[offset_idx][1] * offsets[offset_idx][1])); + (offsets[offset_idx][0] * offsets[offset_idx][0]) + + cos_elevation_angle_sq * + (offsets[offset_idx][1] * offsets[offset_idx][1]); } return error_norms; } 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 f1d7a174a..e80927d11 100644 --- a/libraries/wavemap/include/wavemap/integrator/projection_model/ouster_projector.h +++ b/libraries/wavemap/include/wavemap/integrator/projection_model/ouster_projector.h @@ -60,10 +60,10 @@ class OusterProjector : public ProjectorBase { // Coordinate transforms between Cartesian and sensor space SensorCoordinates cartesianToSensor(const Point3D& C_point) const final; Point3D sensorToCartesian(const SensorCoordinates& coordinates) const final; - FloatingPoint imageOffsetToErrorNorm( + FloatingPoint imageOffsetToErrorSquaredNorm( const ImageCoordinates& linearization_point, const Vector2D& offset) const final; - std::array imageOffsetsToErrorNorms( + std::array imageOffsetsToErrorSquaredNorms( const ImageCoordinates& linearization_point, const CellToBeamOffsetArray& offsets) const final; 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 a2dc2138b..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 @@ -58,10 +58,10 @@ class PinholeCameraProjector : public ProjectorBase { // Coordinate transforms between Cartesian and sensor space SensorCoordinates cartesianToSensor(const Point3D& C_point) const final; Point3D sensorToCartesian(const SensorCoordinates& coordinates) const final; - FloatingPoint imageOffsetToErrorNorm( + FloatingPoint imageOffsetToErrorSquaredNorm( const ImageCoordinates& /*linearization_point*/, const Vector2D& offset) const final; - std::array imageOffsetsToErrorNorms( + std::array imageOffsetsToErrorSquaredNorms( const ImageCoordinates& /*linearization_point*/, const CellToBeamOffsetArray& offsets) const final; 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 0e5aa045a..df7cde4f0 100644 --- a/libraries/wavemap/include/wavemap/integrator/projection_model/projector_base.h +++ b/libraries/wavemap/include/wavemap/integrator/projection_model/projector_base.h @@ -86,11 +86,17 @@ class ProjectorBase { // 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( + FloatingPoint imageOffsetToErrorNorm( + const ImageCoordinates& linearization_point, + const Vector2D& offset) const; + virtual FloatingPoint imageOffsetToErrorSquaredNorm( const ImageCoordinates& linearization_point, const Vector2D& offset) const = 0; using CellToBeamOffsetArray = std::array; - virtual std::array imageOffsetsToErrorNorms( + std::array imageOffsetsToErrorNorms( + const ImageCoordinates& linearization_point, + const CellToBeamOffsetArray& offsets) const; + virtual std::array imageOffsetsToErrorSquaredNorms( const ImageCoordinates& linearization_point, const CellToBeamOffsetArray& offsets) const = 0; 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 3df8d62ab..b1f7d1be7 100644 --- a/libraries/wavemap/include/wavemap/integrator/projection_model/spherical_projector.h +++ b/libraries/wavemap/include/wavemap/integrator/projection_model/spherical_projector.h @@ -46,10 +46,10 @@ class SphericalProjector : public ProjectorBase { // Coordinate transforms between Cartesian and sensor space SensorCoordinates cartesianToSensor(const Point3D& C_point) const final; Point3D sensorToCartesian(const SensorCoordinates& coordinates) const final; - FloatingPoint imageOffsetToErrorNorm( + FloatingPoint imageOffsetToErrorSquaredNorm( const ImageCoordinates& linearization_point, const Vector2D& offset) const final; - std::array imageOffsetsToErrorNorms( + std::array imageOffsetsToErrorSquaredNorms( const ImageCoordinates& linearization_point, const CellToBeamOffsetArray& offsets) const final; From f538711124fa0bf87b8fc2075937496bb8d4957f Mon Sep 17 00:00:00 2001 From: Victor Reijgwart Date: Sun, 1 Oct 2023 19:52:20 +0200 Subject: [PATCH 20/70] Handle predictable branch with prediction not cmov --- .../wavemap/include/wavemap/utils/approximate_trigonometry.h | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/libraries/wavemap/include/wavemap/utils/approximate_trigonometry.h b/libraries/wavemap/include/wavemap/utils/approximate_trigonometry.h index e247c03d2..e1b0aeae4 100644 --- a/libraries/wavemap/include/wavemap/utils/approximate_trigonometry.h +++ b/libraries/wavemap/include/wavemap/utils/approximate_trigonometry.h @@ -69,7 +69,9 @@ struct atan2 FloatingPoint res = internal::atan_fma_approximation(atan_input); // If swapped, adjust atan output - res = swap ? std::copysign(kHalfPi, atan_input) - res : res; + if (swap) { + res = std::copysign(kHalfPi, atan_input) - res; + } // Adjust the result depending on the input quadrant if (x < 0.0f) { res = std::copysign(kPi, y) + res; From 2223752f326dcc65275b5fa3a0406c385abfb1fa Mon Sep 17 00:00:00 2001 From: Victor Reijgwart Date: Sun, 1 Oct 2023 22:24:51 +0200 Subject: [PATCH 21/70] Vectorize batched leaf updater --- .../hashed_chunked_wavelet_integrator_inl.h | 41 ++++++++++++++----- 1 file changed, 31 insertions(+), 10 deletions(-) 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 494d60f0c..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 @@ -40,20 +40,41 @@ inline void HashedChunkedWaveletIntegrator::recursiveTester( // NOLINT 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}); - for (NdtreeIndexRelativeChild relative_child_idx = 0; - relative_child_idx < OctreeIndex::kNumChildren; ++relative_child_idx) { - const auto child_index = parent_index.computeChildIndex(relative_child_idx); - FloatingPoint& child_value = child_values[relative_child_idx]; - const Point3D W_child_center = + + // 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_); - const Point3D C_child_center = - posed_range_image_->getPoseInverse() * W_child_center; - const FloatingPoint sample = computeUpdate(C_child_center); - child_value = std::clamp(sample + child_value, min_log_odds_padded_, - max_log_odds_padded_); } + + // 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; From 9b56dfd0de041216d006af638f9fc2656cc4478a Mon Sep 17 00:00:00 2001 From: Victor Reijgwart Date: Sun, 1 Oct 2023 23:25:16 +0200 Subject: [PATCH 22/70] Various small improvements --- .../include/wavemap/data_structure/image.h | 4 ++-- .../impl/continuous_beam_inl.h | 24 +++++++++++-------- .../impl/ouster_projector_inl.h | 14 +++++------ .../projection_model/ouster_projector.cc | 8 +++---- 4 files changed, 27 insertions(+), 23 deletions(-) diff --git a/libraries/wavemap/include/wavemap/data_structure/image.h b/libraries/wavemap/include/wavemap/data_structure/image.h index 670e6de43..bd5fcd877 100644 --- a/libraries/wavemap/include/wavemap/data_structure/image.h +++ b/libraries/wavemap/include/wavemap/data_structure/image.h @@ -51,13 +51,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/integrator/measurement_model/impl/continuous_beam_inl.h b/libraries/wavemap/include/wavemap/integrator/measurement_model/impl/continuous_beam_inl.h index 7c3bb26ca..c098b4211 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 @@ -32,24 +32,27 @@ inline FloatingPoint ContinuousBeam::computeWorstCaseApproximationError( inline FloatingPoint ContinuousBeam::computeUpdate( const SensorCoordinates& sensor_coordinates) const { + const auto& range_image = *range_image_; + const auto& beam_offset_image = *beam_offset_image_; + const auto& projection_model = *projection_model_; const FloatingPoint cell_to_sensor_distance = sensor_coordinates.normal; 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( + projection_model.imageToNearestIndexAndOffset( sensor_coordinates.image); - if (!range_image_->isIndexWithinBounds(image_index)) { + if (!range_image.isIndexWithinBounds(image_index)) { return 0.f; } - const FloatingPoint measured_distance = range_image_->at(image_index); + const FloatingPoint measured_distance = range_image.at(image_index); const Vector2D cell_to_beam_offset = - beam_offset_image_->at(image_index) - cell_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( + projection_model.imageOffsetToErrorSquaredNorm( sensor_coordinates.image, cell_to_beam_offset); // Compute the update @@ -63,22 +66,23 @@ inline FloatingPoint ContinuousBeam::computeUpdate( std::array measured_distances{}; ProjectorBase::CellToBeamOffsetArray cell_to_beam_offsets{}; const auto [image_indices, cell_offsets] = - projection_model_->imageToNearestIndicesAndOffsets( + projection_model.imageToNearestIndicesAndOffsets( sensor_coordinates.image); 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)) { - measured_distances[neighbor_idx] = range_image_->at(image_index); + if (range_image.isIndexWithinBounds(image_index)) { + measured_distances[neighbor_idx] = range_image.at(image_index); cell_to_beam_offsets[neighbor_idx] = - beam_offset_image_->at(image_index) - cell_offset; + beam_offset_image.at(image_index); + cell_to_beam_offsets[neighbor_idx] -= cell_offset; } } // Compute the image error norms const auto cell_to_beam_image_error_norms_sq = - projection_model_->imageOffsetsToErrorSquaredNorms( + projection_model.imageOffsetsToErrorSquaredNorms( sensor_coordinates.image, cell_to_beam_offsets); // Compute the update 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 848426abd..6b226f1cc 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 @@ -10,10 +10,10 @@ inline SensorCoordinates 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}; } @@ -63,13 +63,13 @@ inline ImageCoordinates OusterProjector::cartesianToImage( // 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}; } diff --git a/libraries/wavemap/src/integrator/projection_model/ouster_projector.cc b/libraries/wavemap/src/integrator/projection_model/ouster_projector.cc index d12a08bc6..3b990ce02 100644 --- a/libraries/wavemap/src/integrator/projection_model/ouster_projector.cc +++ b/libraries/wavemap/src/integrator/projection_model/ouster_projector.cc @@ -68,10 +68,10 @@ AABB OusterProjector::cartesianToSensorAABB( FloatingPoint& max_coordinate = sensor_coordinate_aabb.max[axis]; for (int corner_idx = 0; corner_idx < AABB::kNumCorners; ++corner_idx) { - min_coordinate = - std::min(min_coordinate, corner_sensor_coordinates[corner_idx][axis]); - max_coordinate = - std::max(max_coordinate, corner_sensor_coordinates[corner_idx][axis]); + const FloatingPoint coordinate = + corner_sensor_coordinates[corner_idx][axis]; + min_coordinate = std::min(min_coordinate, coordinate); + max_coordinate = std::max(max_coordinate, coordinate); } const bool angle_interval_wraps_around = From b110f0acf0f808c46e0f60faba4471378e3bf994 Mon Sep 17 00:00:00 2001 From: Victor Reijgwart Date: Mon, 2 Oct 2023 10:54:13 +0200 Subject: [PATCH 23/70] Reduce memory move overhead --- .../measurement_model/continuous_beam.h | 5 ++- .../impl/continuous_beam_inl.h | 31 ++++++++------- .../impl/ouster_projector_inl.h | 4 +- .../impl/pinhole_camera_projector_inl.h | 4 +- .../impl/projector_base_inl.h | 38 ++++++++++--------- .../impl/spherical_projector_inl.h | 4 +- .../projection_model/projector_base.h | 4 +- .../projection_model/test_image_projectors.cc | 7 ++-- 8 files changed, 52 insertions(+), 45 deletions(-) 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 c7ce2d4c2..17e12a3cc 100644 --- a/libraries/wavemap/include/wavemap/integrator/measurement_model/continuous_beam.h +++ b/libraries/wavemap/include/wavemap/integrator/measurement_model/continuous_beam.h @@ -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_; } 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 c098b4211..8f893b80f 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 @@ -64,19 +64,22 @@ inline FloatingPoint ContinuousBeam::computeUpdate( 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] = + 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 Index2D& image_index = image_indices[neighbor_idx]; - const Vector2D& cell_offset = cell_offsets[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)) { - measured_distances[neighbor_idx] = range_image.at(image_index); - cell_to_beam_offsets[neighbor_idx] = - beam_offset_image.at(image_index); - cell_to_beam_offsets[neighbor_idx] -= cell_offset; + 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); } } @@ -112,6 +115,12 @@ inline FloatingPoint ContinuousBeam::computeBeamUpdate( return 0.f; } + const FloatingPoint g = + std::sqrt(cell_to_beam_image_error_norm_squared) / config_.angle_sigma; + const FloatingPoint angle_contrib = + ApproximateGaussianDistribution::cumulative(g + 3.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; @@ -124,12 +133,6 @@ inline FloatingPoint ContinuousBeam::computeBeamUpdate( 0.5f * ApproximateGaussianDistribution::cumulative(f - 3.f) - 0.5f; } - const FloatingPoint g = - std::sqrt(cell_to_beam_image_error_norm_squared) / 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/projection_model/impl/ouster_projector_inl.h b/libraries/wavemap/include/wavemap/integrator/projection_model/impl/ouster_projector_inl.h index 6b226f1cc..b8a53e2d7 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 @@ -51,9 +51,9 @@ OusterProjector::imageOffsetsToErrorSquaredNorms( std::array error_norms{}; for (int offset_idx = 0; offset_idx < 4; ++offset_idx) { error_norms[offset_idx] = - (offsets[offset_idx][0] * offsets[offset_idx][0]) + + (offsets(0, offset_idx) * offsets(0, offset_idx)) + cos_elevation_angle_sq * - (offsets[offset_idx][1] * offsets[offset_idx][1]); + (offsets(1, offset_idx) * offsets(1, offset_idx)); } return error_norms; } 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 34f6439a2..ebb355a83 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 @@ -35,8 +35,8 @@ PinholeCameraProjector::imageOffsetsToErrorSquaredNorms( 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][0] * offsets[offset_idx][0] + - offsets[offset_idx][1] * offsets[offset_idx][1]; + error_norms[offset_idx] = offsets(0, offset_idx) * offsets(0, offset_idx) + + offsets(1, offset_idx) * offsets(1, offset_idx); } 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 9d875f95c..f610508fc 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 @@ -55,27 +55,29 @@ inline std::array ProjectorBase::imageToNearestIndices( return indices; } -inline std::pair, std::array> +inline std::pair, + Eigen::Matrix> ProjectorBase::imageToNearestIndicesAndOffsets( const ImageCoordinates& 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][0] = - index_to_image_scale_factor_[0] * (index[0] - index_rounded[0]); - offsets[neighbox_idx][1] = - index_to_image_scale_factor_[1] * (index[1] - index_rounded[1]); - } + std::pair, + Eigen::Matrix> + result; + auto& indices = result.first; + auto& offsets = result.second; - return {std::move(indices), std::move(offsets)}; + const Vector2D index = imageToIndexReal(image_coordinates); + offsets.col(0) = index.array().floor(); + offsets.col(3) = index.array().ceil(); + offsets(0, 1) = offsets(0, 0); + offsets(1, 1) = offsets(1, 3); + offsets(0, 2) = offsets(0, 3); + offsets(1, 2) = offsets(1, 0); + + indices = offsets.cast(); + offsets = + index_to_image_scale_factor_.asDiagonal() * (offsets.colwise() - index); + + return result; } inline ImageCoordinates ProjectorBase::indexToImage( 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 368080110..be53dc934 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 @@ -41,9 +41,9 @@ SphericalProjector::imageOffsetsToErrorSquaredNorms( std::array error_norms{}; for (int offset_idx = 0; offset_idx < 4; ++offset_idx) { error_norms[offset_idx] = - (offsets[offset_idx][0] * offsets[offset_idx][0]) + + (offsets(0, offset_idx) * offsets(0, offset_idx)) + cos_elevation_angle_sq * - (offsets[offset_idx][1] * offsets[offset_idx][1]); + (offsets(1, offset_idx) * offsets(1, offset_idx)); } return error_norms; } 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 df7cde4f0..69de5c5db 100644 --- a/libraries/wavemap/include/wavemap/integrator/projection_model/projector_base.h +++ b/libraries/wavemap/include/wavemap/integrator/projection_model/projector_base.h @@ -75,7 +75,8 @@ class ProjectorBase { std::pair imageToNearestIndexAndOffset( const ImageCoordinates& image_coordinates) const; - std::pair, std::array> + using CellToBeamOffsetArray = Eigen::Matrix; + std::pair, CellToBeamOffsetArray> imageToNearestIndicesAndOffsets( const ImageCoordinates& image_coordinates) const; @@ -92,7 +93,6 @@ class ProjectorBase { virtual FloatingPoint imageOffsetToErrorSquaredNorm( const ImageCoordinates& linearization_point, const Vector2D& offset) const = 0; - using CellToBeamOffsetArray = std::array; std::array imageOffsetsToErrorNorms( const ImageCoordinates& linearization_point, const CellToBeamOffsetArray& offsets) const; diff --git a/libraries/wavemap/test/src/integrator/projection_model/test_image_projectors.cc b/libraries/wavemap/test/src/integrator/projection_model/test_image_projectors.cc index 39978981a..715039a8b 100644 --- a/libraries/wavemap/test/src/integrator/projection_model/test_image_projectors.cc +++ b/libraries/wavemap/test/src/integrator/projection_model/test_image_projectors.cc @@ -394,14 +394,13 @@ TYPED_TEST(Image2DProjectorTypedTest, ImageOffsetErrorNorms) { // Test single and batched computation equivalence const Vector2D linearization_point = Vector2D::Random(); - ProjectorBase::CellToBeamOffsetArray offsets{}; - std::generate(offsets.begin(), offsets.end(), - []() { return Vector2D::Random(); }); + const ProjectorBase::CellToBeamOffsetArray offsets = + ProjectorBase::CellToBeamOffsetArray::Random(); const auto error_norms = projector.imageOffsetsToErrorNorms(linearization_point, offsets); for (int offset_idx = 0; offset_idx < 4; ++offset_idx) { const FloatingPoint error_norm = projector.imageOffsetToErrorNorm( - linearization_point, offsets[offset_idx]); + linearization_point, offsets.col(offset_idx)); EXPECT_NEAR(error_norms[offset_idx], error_norm, kEpsilon); } } From b69645c8813715624e0fb0fcc612b62af922bc58 Mon Sep 17 00:00:00 2001 From: Victor Reijgwart Date: Mon, 2 Oct 2023 11:35:04 +0200 Subject: [PATCH 24/70] Simplify measurement model math --- .../integrator/measurement_model/impl/continuous_beam_inl.h | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) 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 8f893b80f..28e5ae976 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 @@ -117,9 +117,11 @@ inline FloatingPoint ContinuousBeam::computeBeamUpdate( 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 = - ApproximateGaussianDistribution::cumulative(g + 3.f) - - ApproximateGaussianDistribution::cumulative(g - 3.f); + 1.f - ApproximateGaussianDistribution::cumulative(g - 3.f); const bool fully_in_free_space = cell_to_sensor_distance < measured_distance - range_threshold_front; From 72010a7e539378f5a10cc51e083ef7b18bf27df1 Mon Sep 17 00:00:00 2001 From: Victor Reijgwart Date: Mon, 2 Oct 2023 12:54:39 +0200 Subject: [PATCH 25/70] Add tests for nearest index and offset methods --- .../impl/continuous_ray_inl.h | 2 +- .../impl/projector_base_inl.h | 54 +++++++++---------- .../projection_model/projector_base.h | 12 +++-- .../projection_model/test_image_projectors.cc | 41 ++++++++++++++ 4 files changed, 75 insertions(+), 34 deletions(-) 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 bed446eec..72ef1759f 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 @@ -37,7 +37,7 @@ inline FloatingPoint ContinuousRay::computeUpdate( 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; 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 f610508fc..8864a0fd3 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 @@ -30,52 +30,48 @@ inline Index2D ProjectorBase::imageToCeilIndex( inline std::pair ProjectorBase::imageToNearestIndexAndOffset( const ImageCoordinates& image_coordinates) const { - const Vector2D index = imageToIndexReal(image_coordinates); - const Vector2D index_rounded = index.array().round(); + const Vector2D index_real = imageToIndexReal(image_coordinates); + const Vector2D index_rounded = index_real.array().round(); Vector2D image_coordinate_offset = - (index - index_rounded).cwiseProduct(index_to_image_scale_factor_); + (index_rounded - index_real).cwiseProduct(index_to_image_scale_factor_); return {index_rounded.cast(), std::move(image_coordinate_offset)}; } -inline std::array ProjectorBase::imageToNearestIndices( +inline ProjectorBase::NearestIndexArray ProjectorBase::imageToNearestIndices( const ImageCoordinates& 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(); - } + NearestIndexArray indices; + + const Vector2D index_real = imageToIndexReal(image_coordinates); + indices.col(0) = index_real.array().floor().cast(); + indices.col(3) = index_real.array().ceil().cast(); + 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, - Eigen::Matrix> +inline std::pair ProjectorBase::imageToNearestIndicesAndOffsets( const ImageCoordinates& image_coordinates) const { - std::pair, - Eigen::Matrix> - result; + std::pair result; auto& indices = result.first; auto& offsets = result.second; - const Vector2D index = imageToIndexReal(image_coordinates); - offsets.col(0) = index.array().floor(); - offsets.col(3) = index.array().ceil(); - offsets(0, 1) = offsets(0, 0); - offsets(1, 1) = offsets(1, 3); - offsets(0, 2) = offsets(0, 3); - offsets(1, 2) = offsets(1, 0); + const Vector2D index_real = imageToIndexReal(image_coordinates); + offsets.col(0) = index_real.array().floor(); + offsets.col(3) = index_real.array().ceil(); + offsets(0, 1) = offsets(0, 3); + offsets(1, 1) = offsets(1, 0); + offsets(0, 2) = offsets(0, 0); + offsets(1, 2) = offsets(1, 3); indices = offsets.cast(); - offsets = - index_to_image_scale_factor_.asDiagonal() * (offsets.colwise() - index); + offsets = index_to_image_scale_factor_.asDiagonal() * + (offsets.colwise() - index_real); return result; } 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 69de5c5db..4467021f5 100644 --- a/libraries/wavemap/include/wavemap/integrator/projection_model/projector_base.h +++ b/libraries/wavemap/include/wavemap/integrator/projection_model/projector_base.h @@ -70,13 +70,14 @@ class ProjectorBase { Index2D imageToNearestIndex(const ImageCoordinates& image_coordinates) const; Index2D imageToFloorIndex(const ImageCoordinates& image_coordinates) const; Index2D imageToCeilIndex(const ImageCoordinates& image_coordinates) const; - std::array imageToNearestIndices( + using NearestIndexArray = Eigen::Matrix; + NearestIndexArray imageToNearestIndices( const ImageCoordinates& image_coordinates) const; std::pair imageToNearestIndexAndOffset( const ImageCoordinates& image_coordinates) const; using CellToBeamOffsetArray = Eigen::Matrix; - std::pair, CellToBeamOffsetArray> + std::pair imageToNearestIndicesAndOffsets( const ImageCoordinates& image_coordinates) const; @@ -110,6 +111,11 @@ class ProjectorBase { const Transformation3D::RotationMatrix& R_C_W, const Point3D& t_W_C) const = 0; + Vector2D imageToIndexReal(const ImageCoordinates& image_coordinates) const; + const Vector2D& getIndexToImageScaleFactor() const { + return index_to_image_scale_factor_; + } + protected: const Index2D dimensions_; const Vector2D index_to_image_scale_factor_; @@ -119,8 +125,6 @@ class ProjectorBase { const ImageCoordinates min_image_coordinates_; const ImageCoordinates max_image_coordinates_; - - Vector2D imageToIndexReal(const ImageCoordinates& image_coordinates) const; }; } // namespace wavemap diff --git a/libraries/wavemap/test/src/integrator/projection_model/test_image_projectors.cc b/libraries/wavemap/test/src/integrator/projection_model/test_image_projectors.cc index 715039a8b..39b9fd158 100644 --- a/libraries/wavemap/test/src/integrator/projection_model/test_image_projectors.cc +++ b/libraries/wavemap/test/src/integrator/projection_model/test_image_projectors.cc @@ -383,6 +383,47 @@ TYPED_TEST(Image2DProjectorTypedTest, SensorCoordinateAABBs) { } } +TYPED_TEST(Image2DProjectorTypedTest, imageToNearestIndicesAndOffsets) { + constexpr int kNumRandomProjectorConfigs = 10; + for (int config_idx = 0; config_idx < kNumRandomProjectorConfigs; + ++config_idx) { + // Create a projector with random params + typename TypeParam::Config projector_config; + Image2DProjectorTest::getRandomProjectorConfig(projector_config); + const TypeParam projector(projector_config); + + // Test single and batched computation equivalence + const Vector2D image_coordinates = Vector2D::Random(); + const auto indices = projector.imageToNearestIndices(image_coordinates); + const auto indices_and_offsets = + projector.imageToNearestIndicesAndOffsets(image_coordinates); + + const Vector2D index_real = projector.imageToIndexReal(image_coordinates); + const Vector2D index_lower = index_real.array().floor(); + const Vector2D index_upper = index_real.array().ceil(); + + 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]}; + const Index2D index_expected = index_rounded.cast(); + const Vector2D offset_expected = + projector.getIndexToImageScaleFactor().cwiseProduct(index_rounded - + index_real); + + EXPECT_EQ(indices(0, neighbox_idx), index_expected[0]); + EXPECT_EQ(indices(1, neighbox_idx), index_expected[1]); + + EXPECT_EQ(indices_and_offsets.first(0, neighbox_idx), index_expected[0]); + EXPECT_EQ(indices_and_offsets.first(1, neighbox_idx), index_expected[1]); + EXPECT_NEAR(indices_and_offsets.second(0, neighbox_idx), + offset_expected[0], kEpsilon); + EXPECT_NEAR(indices_and_offsets.second(1, neighbox_idx), + offset_expected[1], kEpsilon); + } + } +} + TYPED_TEST(Image2DProjectorTypedTest, ImageOffsetErrorNorms) { constexpr int kNumRandomProjectorConfigs = 10; for (int config_idx = 0; config_idx < kNumRandomProjectorConfigs; From 71bae4243884ccebbebeee40d74cbe9f90e623bd Mon Sep 17 00:00:00 2001 From: Victor Reijgwart Date: Mon, 2 Oct 2023 13:16:48 +0200 Subject: [PATCH 26/70] Restore method privacy --- .../integrator/projection_model/projector_base.h | 12 +++++++----- .../projection_model/test_image_projectors.cc | 2 +- 2 files changed, 8 insertions(+), 6 deletions(-) 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 4467021f5..69a0379d8 100644 --- a/libraries/wavemap/include/wavemap/integrator/projection_model/projector_base.h +++ b/libraries/wavemap/include/wavemap/integrator/projection_model/projector_base.h @@ -111,11 +111,6 @@ class ProjectorBase { const Transformation3D::RotationMatrix& R_C_W, const Point3D& t_W_C) const = 0; - Vector2D imageToIndexReal(const ImageCoordinates& image_coordinates) const; - const Vector2D& getIndexToImageScaleFactor() const { - return index_to_image_scale_factor_; - } - protected: const Index2D dimensions_; const Vector2D index_to_image_scale_factor_; @@ -125,6 +120,13 @@ class ProjectorBase { const ImageCoordinates min_image_coordinates_; const ImageCoordinates max_image_coordinates_; + + Vector2D imageToIndexReal(const ImageCoordinates& 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/test/src/integrator/projection_model/test_image_projectors.cc b/libraries/wavemap/test/src/integrator/projection_model/test_image_projectors.cc index 39b9fd158..794ccd2ca 100644 --- a/libraries/wavemap/test/src/integrator/projection_model/test_image_projectors.cc +++ b/libraries/wavemap/test/src/integrator/projection_model/test_image_projectors.cc @@ -408,7 +408,7 @@ TYPED_TEST(Image2DProjectorTypedTest, imageToNearestIndicesAndOffsets) { neighbox_idx & 0b10 ? index_upper[1] : index_lower[1]}; const Index2D index_expected = index_rounded.cast(); const Vector2D offset_expected = - projector.getIndexToImageScaleFactor().cwiseProduct(index_rounded - + projector.index_to_image_scale_factor_.cwiseProduct(index_rounded - index_real); EXPECT_EQ(indices(0, neighbox_idx), index_expected[0]); From a91a6e1c6645e0c136f21349dab821cf608ee2b4 Mon Sep 17 00:00:00 2001 From: Victor Reijgwart Date: Mon, 2 Oct 2023 14:18:10 +0200 Subject: [PATCH 27/70] Attempt to fix issue loading ROS libraries in CI --- .github/workflows/ci.yml | 1 + 1 file changed, 1 insertion(+) diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index b914ac3ac..7acf1840a 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -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 From 0fbc41c724d604dca63bc133c1df9aaf25967e48 Mon Sep 17 00:00:00 2001 From: Victor Reijgwart Date: Sun, 1 Oct 2023 19:52:20 +0200 Subject: [PATCH 28/70] Revert "Handle predictable branch with prediction not cmov" This reverts commit f538711124fa0bf87b8fc2075937496bb8d4957f. --- .../wavemap/include/wavemap/utils/approximate_trigonometry.h | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/libraries/wavemap/include/wavemap/utils/approximate_trigonometry.h b/libraries/wavemap/include/wavemap/utils/approximate_trigonometry.h index e1b0aeae4..e247c03d2 100644 --- a/libraries/wavemap/include/wavemap/utils/approximate_trigonometry.h +++ b/libraries/wavemap/include/wavemap/utils/approximate_trigonometry.h @@ -69,9 +69,7 @@ struct atan2 FloatingPoint res = internal::atan_fma_approximation(atan_input); // If swapped, adjust atan output - if (swap) { - res = std::copysign(kHalfPi, atan_input) - res; - } + 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; From 437a00474e5278e9633b99659382839c40792d21 Mon Sep 17 00:00:00 2001 From: Victor Reijgwart Date: Mon, 2 Oct 2023 14:49:02 +0200 Subject: [PATCH 29/70] Also make sure ROS is sourced when running valgrind --- .github/workflows/ci.yml | 1 + 1 file changed, 1 insertion(+) diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 7acf1840a..d509a612b 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -588,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 From 506f35a60c2d30052a5f2b49e48f6141fa180d19 Mon Sep 17 00:00:00 2001 From: Victor Reijgwart Date: Tue, 3 Oct 2023 11:52:54 +0200 Subject: [PATCH 30/70] Improve example configs --- ros/wavemap_ros/config/wavemap_azure_kinect.yaml | 10 ++++++---- ros/wavemap_ros/config/wavemap_livox_mid360.yaml | 10 ++++++---- ros/wavemap_ros/config/wavemap_ouster_os0.yaml | 1 + ros/wavemap_ros/config/wavemap_ouster_os1.yaml | 4 +++- .../config/wavemap_panoptic_mapping_rgbd.yaml | 1 + ros/wavemap_ros/config/wavemap_pico_flexx.yaml | 1 + ros/wavemap_ros/config/wavemap_pico_monstar.yaml | 1 + 7 files changed, 19 insertions(+), 9 deletions(-) diff --git a/ros/wavemap_ros/config/wavemap_azure_kinect.yaml b/ros/wavemap_ros/config/wavemap_azure_kinect.yaml index 60e8b1a2a..2852a76db 100644 --- a/ros/wavemap_ros/config/wavemap_azure_kinect.yaml +++ b/ros/wavemap_ros/config/wavemap_azure_kinect.yaml @@ -4,6 +4,7 @@ map: thresholding_period: { seconds: 2.0 } pruning_period: { seconds: 10.0 } publication_period: { seconds: 2.0 } + logging_level: debug data_structure: type: hashed_chunked_wavelet_octree min_cell_width: { meters: 0.01 } @@ -61,6 +62,7 @@ inputs: general: topic_name: "/livox/lidar" topic_type: livox + undistort_motion: true # reprojected_topic_name: "/wavemap/livox_input" topic_queue_length: 1 max_wait_for_pose: { seconds: 1.0 } @@ -74,18 +76,18 @@ inputs: projection_model: type: spherical_projector elevation: - num_cells: 64 + num_cells: 32 min_angle: { degrees: -7.0 } max_angle: { degrees: 52.0 } azimuth: - num_cells: 1024 + num_cells: 512 min_angle: { degrees: -180.0 } max_angle: { degrees: 180.0 } measurement_model: type: continuous_beam - angle_sigma: { degrees: 0.035 } + angle_sigma: { degrees: 0.05 } # NOTE: For best results, we recommend setting range_sigma to # max(min_cell_width / 2, ouster_noise) where ouster_noise = 0.05 - range_sigma: { meters: 0.05 } + range_sigma: { meters: 0.08 } scaling_free: 0.2 scaling_occupied: 0.4 diff --git a/ros/wavemap_ros/config/wavemap_livox_mid360.yaml b/ros/wavemap_ros/config/wavemap_livox_mid360.yaml index 39534f74e..607cbe0e3 100644 --- a/ros/wavemap_ros/config/wavemap_livox_mid360.yaml +++ b/ros/wavemap_ros/config/wavemap_livox_mid360.yaml @@ -4,6 +4,7 @@ map: thresholding_period: { seconds: 2.0 } pruning_period: { seconds: 10.0 } publication_period: { seconds: 3.0 } + logging_level: debug data_structure: type: hashed_chunked_wavelet_octree min_cell_width: { meters: 0.1 } @@ -13,6 +14,7 @@ inputs: general: topic_name: "/livox/lidar" topic_type: livox + undistort_motion: true reprojected_pointcloud_topic_name: "/wavemap/input" topic_queue_length: 10 max_wait_for_pose: { seconds: 1.0 } @@ -25,18 +27,18 @@ inputs: projection_model: type: spherical_projector elevation: - num_cells: 64 + num_cells: 32 min_angle: { degrees: -7.0 } max_angle: { degrees: 52.0 } azimuth: - num_cells: 1024 + num_cells: 512 min_angle: { degrees: -180.0 } max_angle: { degrees: 180.0 } measurement_model: type: continuous_beam - angle_sigma: { degrees: 0.035 } + angle_sigma: { degrees: 0.05 } # NOTE: For best results, we recommend setting range_sigma to # max(min_cell_width / 2, ouster_noise) where ouster_noise = 0.05 - range_sigma: { meters: 0.05 } + range_sigma: { meters: 0.08 } scaling_free: 0.2 scaling_occupied: 0.4 diff --git a/ros/wavemap_ros/config/wavemap_ouster_os0.yaml b/ros/wavemap_ros/config/wavemap_ouster_os0.yaml index ad415ba27..d62c2061b 100644 --- a/ros/wavemap_ros/config/wavemap_ouster_os0.yaml +++ b/ros/wavemap_ros/config/wavemap_ouster_os0.yaml @@ -4,6 +4,7 @@ map: thresholding_period: { seconds: 2.0 } pruning_period: { seconds: 10.0 } publication_period: { seconds: 2.0 } + logging_level: debug data_structure: type: hashed_chunked_wavelet_octree min_cell_width: { meters: 0.1 } diff --git a/ros/wavemap_ros/config/wavemap_ouster_os1.yaml b/ros/wavemap_ros/config/wavemap_ouster_os1.yaml index 0b426b7bb..b1b61550b 100644 --- a/ros/wavemap_ros/config/wavemap_ouster_os1.yaml +++ b/ros/wavemap_ros/config/wavemap_ouster_os1.yaml @@ -3,6 +3,7 @@ map: world_frame: "odom" pruning_period: { seconds: 10.0 } publication_period: { seconds: 10.0 } + logging_level: debug data_structure: type: wavelet_octree min_cell_width: { meters: 0.2 } @@ -11,7 +12,8 @@ inputs: - type: pointcloud general: topic_name: "/os1_cloud_node/points" - topic_type: PointCloud2 + topic_type: ouster + undistort_motion: true topic_queue_length: 10 max_wait_for_pose: { seconds: 1.0 } integrators: diff --git a/ros/wavemap_ros/config/wavemap_panoptic_mapping_rgbd.yaml b/ros/wavemap_ros/config/wavemap_panoptic_mapping_rgbd.yaml index 9593565d1..ee5fcd5f6 100644 --- a/ros/wavemap_ros/config/wavemap_panoptic_mapping_rgbd.yaml +++ b/ros/wavemap_ros/config/wavemap_panoptic_mapping_rgbd.yaml @@ -3,6 +3,7 @@ map: world_frame: "odom" pruning_period: { seconds: 10.0 } publication_period: { seconds: 2.0 } + logging_level: debug data_structure: type: hashed_chunked_wavelet_octree min_cell_width: { meters: 0.02 } diff --git a/ros/wavemap_ros/config/wavemap_pico_flexx.yaml b/ros/wavemap_ros/config/wavemap_pico_flexx.yaml index f36adbac0..3fa2a5c7c 100644 --- a/ros/wavemap_ros/config/wavemap_pico_flexx.yaml +++ b/ros/wavemap_ros/config/wavemap_pico_flexx.yaml @@ -4,6 +4,7 @@ map: thresholding_period: { seconds: 2.0 } pruning_period: { seconds: 10.0 } publication_period: { seconds: 2.0 } + logging_level: debug data_structure: type: hashed_chunked_wavelet_octree min_cell_width: { meters: 0.01 } diff --git a/ros/wavemap_ros/config/wavemap_pico_monstar.yaml b/ros/wavemap_ros/config/wavemap_pico_monstar.yaml index ec479cc75..d1b31e5a6 100644 --- a/ros/wavemap_ros/config/wavemap_pico_monstar.yaml +++ b/ros/wavemap_ros/config/wavemap_pico_monstar.yaml @@ -4,6 +4,7 @@ map: thresholding_period: { seconds: 2.0 } pruning_period: { seconds: 10.0 } publication_period: { seconds: 2.0 } + logging_level: debug data_structure: type: hashed_chunked_wavelet_octree min_cell_width: { meters: 0.01 } From 93ae040a6711ce9e608c90d05d2ab3e1f92d3772 Mon Sep 17 00:00:00 2001 From: Victor Reijgwart Date: Tue, 3 Oct 2023 18:28:21 +0200 Subject: [PATCH 31/70] Minor approximate atan2 code cleanup --- .../wavemap/utils/approximate_trigonometry.h | 14 ++++++-------- 1 file changed, 6 insertions(+), 8 deletions(-) diff --git a/libraries/wavemap/include/wavemap/utils/approximate_trigonometry.h b/libraries/wavemap/include/wavemap/utils/approximate_trigonometry.h index e247c03d2..a8aaa23b9 100644 --- a/libraries/wavemap/include/wavemap/utils/approximate_trigonometry.h +++ b/libraries/wavemap/include/wavemap/utils/approximate_trigonometry.h @@ -31,14 +31,12 @@ inline FloatingPoint atan_fma_approximation(FloatingPoint x) { // 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); + 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; } } // namespace internal From 76281ac3e4ab14e62819d21efa39d6d2fdc0311c Mon Sep 17 00:00:00 2001 From: Victor Reijgwart Date: Fri, 6 Oct 2023 10:14:00 +0200 Subject: [PATCH 32/70] Hide hidden cells in Rviz --- .../wavemap_rviz_plugin/visuals/grid_visual.h | 13 +++++++ .../src/visuals/grid_visual.cc | 37 +++++++++++++++---- 2 files changed, 43 insertions(+), 7 deletions(-) diff --git a/ros/wavemap_rviz_plugin/include/wavemap_rviz_plugin/visuals/grid_visual.h b/ros/wavemap_rviz_plugin/include/wavemap_rviz_plugin/visuals/grid_visual.h index 7f1becd20..dc9aac24e 100644 --- a/ros/wavemap_rviz_plugin/include/wavemap_rviz_plugin/visuals/grid_visual.h +++ b/ros/wavemap_rviz_plugin/include/wavemap_rviz_plugin/visuals/grid_visual.h @@ -18,6 +18,7 @@ #include #include #include +#include #include #include #include @@ -113,6 +114,18 @@ class GridVisual : public QObject { FloatingPoint distance_to_cam, FloatingPoint min_cell_width, NdtreeIndexElement min_height, NdtreeIndexElement max_height); + // Hidden cell pruning methods + const HashedWaveletOctree* hashed_map_; + static bool isOccupied(FloatingPoint min_occupancy_log_odds, + FloatingPoint max_occupancy_log_odds, + FloatingPoint cell_log_odds) { + return min_occupancy_log_odds < cell_log_odds && + cell_log_odds < max_occupancy_log_odds; + } + bool hasFreeNeighbor(FloatingPoint min_occupancy_log_odds, + FloatingPoint max_occupancy_log_odds, + const OctreeIndex& cell_index); + // Drawing related methods using GridLayerList = std::vector>; void getLeafCentersAndColors(int tree_height, FloatingPoint min_cell_width, diff --git a/ros/wavemap_rviz_plugin/src/visuals/grid_visual.cc b/ros/wavemap_rviz_plugin/src/visuals/grid_visual.cc index 65120f6ee..b2269ae46 100644 --- a/ros/wavemap_rviz_plugin/src/visuals/grid_visual.cc +++ b/ros/wavemap_rviz_plugin/src/visuals/grid_visual.cc @@ -292,6 +292,24 @@ void GridVisual::flatColorUpdateCallback() { } } +bool GridVisual::hasFreeNeighbor(FloatingPoint min_occupancy_log_odds, + FloatingPoint max_occupancy_log_odds, + const OctreeIndex& cell_index) { + for (int dim_idx = 0; dim_idx < 3; ++dim_idx) { + for (const int offset : {-1, 1}) { + auto neighbor_index = cell_index; + neighbor_index.position[dim_idx] += offset; + const FloatingPoint neighbor_log_odds = + hashed_map_->getCellValue(neighbor_index); + if (!isOccupied(min_occupancy_log_odds, max_occupancy_log_odds, + neighbor_log_odds)) { + return true; + } + } + } + return false; +} + void GridVisual::getLeafCentersAndColors(int tree_height, FloatingPoint min_cell_width, FloatingPoint min_occupancy_log_odds, @@ -300,8 +318,14 @@ void GridVisual::getLeafCentersAndColors(int tree_height, FloatingPoint cell_log_odds, GridLayerList& cells_per_level) { // Skip cells that don't meet the occupancy threshold - if (cell_log_odds < min_occupancy_log_odds || - max_occupancy_log_odds < cell_log_odds) { + if (!isOccupied(min_occupancy_log_odds, max_occupancy_log_odds, + cell_log_odds)) { + return; + } + + // Skip cells that are occluded by neighbors on all sides + if (!hasFreeNeighbor(min_occupancy_log_odds, max_occupancy_log_odds, + cell_index)) { return; } @@ -383,9 +407,8 @@ void GridVisual::processBlockUpdateQueue() { return; } - if (const auto* hashed_map = - dynamic_cast(map.get()); - hashed_map) { + if (hashed_map_ = dynamic_cast(map.get()); + hashed_map_) { // Constants const FloatingPoint min_cell_width = map->getMinCellWidth(); const FloatingPoint min_log_odds = @@ -398,7 +421,7 @@ void GridVisual::processBlockUpdateQueue() { std::map changed_blocks_sorted; for (const auto& [block_idx, term_height] : block_update_queue_) { const Timestamp& last_modified_time = - hashed_map->getBlock(block_idx).getLastUpdatedStamp(); + hashed_map_->getBlock(block_idx).getLastUpdatedStamp(); changed_blocks_sorted[last_modified_time] = block_idx; } @@ -409,7 +432,7 @@ void GridVisual::processBlockUpdateQueue() { std::chrono::milliseconds(max_ms_per_frame_property_.getInt()); const auto max_end_time = start_time + max_time_per_frame; for (const auto& [_, block_idx] : changed_blocks_sorted) { - const auto& block = hashed_map->getBlock(block_idx); + const auto& block = hashed_map_->getBlock(block_idx); const IndexElement tree_height = map->getTreeHeight(); const IndexElement term_height = block_update_queue_[block_idx]; const int num_levels = tree_height + 1 - term_height; From 0915b7a704599dc964b41689b26b70754366ee45 Mon Sep 17 00:00:00 2001 From: Victor Reijgwart Date: Fri, 6 Oct 2023 17:17:57 +0200 Subject: [PATCH 33/70] Improve surface cell selector and reorganize menu --- ros/wavemap_rviz_plugin/CMakeLists.txt | 6 +- .../visuals/cell_selector.h | 62 +++++++ .../wavemap_rviz_plugin/visuals/grid_visual.h | 34 ++-- .../src/visuals/cell_selector.cc | 154 ++++++++++++++++++ .../visuals/{grid_layer.cpp => grid_layer.cc} | 0 .../src/visuals/grid_visual.cc | 84 +++------- 6 files changed, 254 insertions(+), 86 deletions(-) create mode 100644 ros/wavemap_rviz_plugin/include/wavemap_rviz_plugin/visuals/cell_selector.h create mode 100644 ros/wavemap_rviz_plugin/src/visuals/cell_selector.cc rename ros/wavemap_rviz_plugin/src/visuals/{grid_layer.cpp => grid_layer.cc} (100%) diff --git a/ros/wavemap_rviz_plugin/CMakeLists.txt b/ros/wavemap_rviz_plugin/CMakeLists.txt index 6bdf636cf..f7dd2b0ff 100644 --- a/ros/wavemap_rviz_plugin/CMakeLists.txt +++ b/ros/wavemap_rviz_plugin/CMakeLists.txt @@ -28,7 +28,8 @@ set(CMAKE_AUTOMOC ON) set(HEADERS_TO_MOC include/wavemap_rviz_plugin/wavemap_map_display.h include/wavemap_rviz_plugin/visuals/grid_visual.h - include/wavemap_rviz_plugin/visuals/slice_visual.h) + include/wavemap_rviz_plugin/visuals/slice_visual.h + include/wavemap_rviz_plugin/visuals/cell_selector.h) if (rviz_QT_VERSION VERSION_LESS "5") message(STATUS "Using Qt4 based on the rviz_QT_VERSION: ${rviz_QT_VERSION}") find_package(Qt4 ${rviz_QT_VERSION} EXACT REQUIRED QtCore QtGui) @@ -56,7 +57,8 @@ add_library(${PROJECT_NAME} src/utils/listeners.cc src/visuals/grid_visual.cc src/visuals/slice_visual.cc - src/visuals/grid_layer.cpp + src/visuals/grid_layer.cc + src/visuals/cell_selector.cc src/wavemap_map_display.cc ${QT_MOC}) target_link_libraries(${PROJECT_NAME} ${QT_LIBRARIES} ${catkin_LIBRARIES}) diff --git a/ros/wavemap_rviz_plugin/include/wavemap_rviz_plugin/visuals/cell_selector.h b/ros/wavemap_rviz_plugin/include/wavemap_rviz_plugin/visuals/cell_selector.h new file mode 100644 index 000000000..968f04c25 --- /dev/null +++ b/ros/wavemap_rviz_plugin/include/wavemap_rviz_plugin/visuals/cell_selector.h @@ -0,0 +1,62 @@ +#ifndef WAVEMAP_RVIZ_PLUGIN_VISUALS_CELL_SELECTOR_H_ +#define WAVEMAP_RVIZ_PLUGIN_VISUALS_CELL_SELECTOR_H_ + +#include +#include +#include +#include +#include + +namespace wavemap::rviz_plugin { +struct CellSelectionMode : public TypeSelector { + using TypeSelector::TypeSelector; + + enum Id : TypeId { kSurface, kBand }; + + static constexpr std::array names = {"Surface", "Band"}; +}; + +class CellSelector : public QObject { + Q_OBJECT + public: // NOLINT + CellSelector(rviz::Property* submenu_root_property, + std::function redraw_map); + + void initializePropertyMenu(); + + void setMap(const VolumetricDataStructureBase::ConstPtr& hashed_map); + + bool shouldBeDrawn(const OctreeIndex& cell_index, + FloatingPoint cell_log_odds) const; + bool isUnknown(FloatingPoint log_odds) const { + return std::abs(log_odds) < unknown_occupancy_threshold_; + } + bool hasFreeNeighbor(const OctreeIndex& cell_index) const; + + private Q_SLOTS: // NOLINT + // These Qt slots get connected to signals indicating changes in the + // user-editable properties + void cellSelectionModeUpdateCallback(); + void thresholdUpdateCallback(); + + private: + std::function redraw_map_; + HashedWaveletOctree::ConstPtr hashed_map_; + + // Selection mode and thresholds + CellSelectionMode cell_selection_mode_ = CellSelectionMode::kSurface; + FloatingPoint surface_occupancy_threshold_ = 1e-3f; + FloatingPoint band_min_occupancy_threshold_ = 1e-3f; + FloatingPoint band_max_occupancy_threshold_ = 1e6f; + FloatingPoint unknown_occupancy_threshold_ = 1e-4f; + + // User-editable property variables, contained in the visual's submenu + rviz::EnumProperty cell_selection_mode_property_; + rviz::FloatProperty surface_occupancy_threshold_property_; + rviz::FloatProperty band_min_occupancy_threshold_property_; + rviz::FloatProperty band_max_occupancy_threshold_property_; + rviz::FloatProperty unknown_occupancy_threshold_property_; +}; +} // namespace wavemap::rviz_plugin + +#endif // WAVEMAP_RVIZ_PLUGIN_VISUALS_CELL_SELECTOR_H_ diff --git a/ros/wavemap_rviz_plugin/include/wavemap_rviz_plugin/visuals/grid_visual.h b/ros/wavemap_rviz_plugin/include/wavemap_rviz_plugin/visuals/grid_visual.h index dc9aac24e..e040bbcfd 100644 --- a/ros/wavemap_rviz_plugin/include/wavemap_rviz_plugin/visuals/grid_visual.h +++ b/ros/wavemap_rviz_plugin/include/wavemap_rviz_plugin/visuals/grid_visual.h @@ -18,7 +18,6 @@ #include #include #include -#include #include #include #include @@ -26,6 +25,7 @@ #include "wavemap_rviz_plugin/common.h" #include "wavemap_rviz_plugin/utils/color_conversions.h" #include "wavemap_rviz_plugin/utils/listeners.h" +#include "wavemap_rviz_plugin/visuals/cell_selector.h" #include "wavemap_rviz_plugin/visuals/grid_layer.h" #endif @@ -64,9 +64,8 @@ class GridVisual : public QObject { private Q_SLOTS: // NOLINT // These Qt slots get connected to signals indicating changes in the // user-editable properties - void thresholdUpdateCallback() { updateMap(true); } - void terminationHeightUpdateCallback() { force_lod_update_ = true; } void visibilityUpdateCallback(); + void terminationHeightUpdateCallback() { force_lod_update_ = true; } void opacityUpdateCallback(); void colorModeUpdateCallback(); void flatColorUpdateCallback(); @@ -87,13 +86,16 @@ class GridVisual : public QObject { Ogre::SceneNode* frame_node_; // User-editable property variables, contained in the visual's submenu + // Grid visibility rviz::BoolProperty visibility_property_; - rviz::FloatProperty min_occupancy_threshold_property_; - rviz::FloatProperty max_occupancy_threshold_property_; + // Cell selection + CellSelector cell_selector_; rviz::IntProperty termination_height_property_; + // Colors rviz::FloatProperty opacity_property_; rviz::EnumProperty color_mode_property_; rviz::ColorProperty flat_color_property_; + // Frame-rate stats rviz::Property frame_rate_properties_; rviz::IntProperty num_queued_blocks_indicator_; rviz::IntProperty max_ms_per_frame_property_; @@ -114,26 +116,12 @@ class GridVisual : public QObject { FloatingPoint distance_to_cam, FloatingPoint min_cell_width, NdtreeIndexElement min_height, NdtreeIndexElement max_height); - // Hidden cell pruning methods - const HashedWaveletOctree* hashed_map_; - static bool isOccupied(FloatingPoint min_occupancy_log_odds, - FloatingPoint max_occupancy_log_odds, - FloatingPoint cell_log_odds) { - return min_occupancy_log_odds < cell_log_odds && - cell_log_odds < max_occupancy_log_odds; - } - bool hasFreeNeighbor(FloatingPoint min_occupancy_log_odds, - FloatingPoint max_occupancy_log_odds, - const OctreeIndex& cell_index); - // Drawing related methods using GridLayerList = std::vector>; - void getLeafCentersAndColors(int tree_height, FloatingPoint min_cell_width, - FloatingPoint min_occupancy_log_odds, - FloatingPoint max_occupancy_log_odds, - const OctreeIndex& cell_index, - FloatingPoint cell_log_odds, - GridLayerList& cells_per_level); + void appendLeafCenterAndColor(int tree_height, FloatingPoint min_cell_width, + const OctreeIndex& cell_index, + FloatingPoint cell_log_odds, + GridLayerList& cells_per_level); void drawMultiResolutionGrid(IndexElement tree_height, FloatingPoint min_cell_width, const Index3D& block_index, FloatingPoint alpha, diff --git a/ros/wavemap_rviz_plugin/src/visuals/cell_selector.cc b/ros/wavemap_rviz_plugin/src/visuals/cell_selector.cc new file mode 100644 index 000000000..44b8a366f --- /dev/null +++ b/ros/wavemap_rviz_plugin/src/visuals/cell_selector.cc @@ -0,0 +1,154 @@ +#include "wavemap_rviz_plugin/visuals/cell_selector.h" + +#include + +namespace wavemap::rviz_plugin { +std::array generateNeighborIndexOffsets() { + std::array neighbor_offsets{}; + size_t array_idx = 0u; + for (const Index3D& index : Grid<3>(-Index3D::Ones(), Index3D::Ones())) { + if (index != Index3D::Zero()) { + neighbor_offsets[array_idx] = index; + ++array_idx; + } + } + std::sort( + neighbor_offsets.begin(), neighbor_offsets.end(), + [](const auto& lhs, const auto& rhs) { return lhs.norm() < rhs.norm(); }); + return neighbor_offsets; +} +static const auto kNeighborOffsets = generateNeighborIndexOffsets(); + +CellSelector::CellSelector(rviz::Property* submenu_root_property, + std::function redraw_map) + : redraw_map_(std::move(redraw_map)), + cell_selection_mode_property_( + "Cell selector", "", "Mode determining what cells to draw.", + submenu_root_property, SLOT(cellSelectionModeUpdateCallback()), this), + surface_occupancy_threshold_property_( + "Log odds threshold", surface_occupancy_threshold_, + "Classification threshold above which a cell is considered occupied. " + "Ranges from -Inf to Inf.", + submenu_root_property, SLOT(thresholdUpdateCallback()), this), + band_min_occupancy_threshold_property_( + "Min log odds", band_min_occupancy_threshold_, + "Only show cells whose occupancy exceeds this threshold. " + "Ranges from -Inf to Inf.", + submenu_root_property, SLOT(thresholdUpdateCallback()), this), + band_max_occupancy_threshold_property_( + "Max log odds", band_max_occupancy_threshold_, + "Only show cells whose occupancy falls below this threshold. " + "Ranges from -Inf to Inf.", + submenu_root_property, SLOT(thresholdUpdateCallback()), this), + unknown_occupancy_threshold_property_( + "Unknown log odds", unknown_occupancy_threshold_, + "Hide cells whose absolute occupancy falls below this threshold, " + "considering them as unknown. Ranges from -Inf to Inf.", + submenu_root_property, SLOT(thresholdUpdateCallback()), this) { + // Initialize the property menu + initializePropertyMenu(); +} + +void CellSelector::initializePropertyMenu() { + cell_selection_mode_property_.clearOptions(); + for (const auto& name : CellSelectionMode::names) { + cell_selection_mode_property_.addOption(name); + } + cell_selection_mode_property_.setStringStd(cell_selection_mode_.toStr()); + surface_occupancy_threshold_property_.setHidden(cell_selection_mode_ != + CellSelectionMode::kSurface); + band_min_occupancy_threshold_property_.setHidden(cell_selection_mode_ != + CellSelectionMode::kBand); + band_max_occupancy_threshold_property_.setHidden(cell_selection_mode_ != + CellSelectionMode::kBand); +} + +void CellSelector::setMap( + const VolumetricDataStructureBase::ConstPtr& hashed_map) { + hashed_map_ = + std::dynamic_pointer_cast(hashed_map); + if (!hashed_map_ && cell_selection_mode_ == CellSelectionMode::kSurface) { + ROS_WARN( + "Cell selection mode 'Surface' only supports HashedWaveletOctree maps. " + "Falling back to mode 'Band'."); + cell_selection_mode_ = CellSelectionMode::kBand; + initializePropertyMenu(); + } +} + +bool CellSelector::shouldBeDrawn(const OctreeIndex& cell_index, + FloatingPoint cell_log_odds) const { + switch (cell_selection_mode_.toTypeId()) { + case CellSelectionMode::kSurface: + // Skip free cells + if (cell_log_odds < surface_occupancy_threshold_ || + isUnknown(cell_log_odds)) { + return false; + } + // Skip cells that are occluded by neighbors on all sides + if (!hasFreeNeighbor(cell_index)) { + return false; + } + break; + case CellSelectionMode::kBand: + // Skip cells that don't meet the occupancy threshold + if (cell_log_odds < band_min_occupancy_threshold_ || + band_max_occupancy_threshold_ < cell_log_odds || + isUnknown(cell_log_odds)) { + return false; + } + break; + } + return true; +} + +bool CellSelector::hasFreeNeighbor(const OctreeIndex& cell_index) const { + for (const auto& offset : kNeighborOffsets) { // NOLINT + const OctreeIndex neighbor_index = {cell_index.height, + cell_index.position + offset}; + const FloatingPoint neighbor_log_odds = + hashed_map_->getCellValue(neighbor_index); + // Check if the neighbor is free and observed + if (neighbor_log_odds < surface_occupancy_threshold_ && + !isUnknown(neighbor_log_odds)) { + return true; + } + } + return false; +} + +void CellSelector::cellSelectionModeUpdateCallback() { + // Update the cached cell selection mode value + const CellSelectionMode old_cell_selection_mode = cell_selection_mode_; + cell_selection_mode_ = + CellSelectionMode(cell_selection_mode_property_.getStdString()); + + // Show/hide properties that only affect certain modes + surface_occupancy_threshold_property_.setHidden(cell_selection_mode_ != + CellSelectionMode::kSurface); + band_min_occupancy_threshold_property_.setHidden(cell_selection_mode_ != + CellSelectionMode::kBand); + band_max_occupancy_threshold_property_.setHidden(cell_selection_mode_ != + CellSelectionMode::kBand); + + // Redraw the map if the color mode changed + if (cell_selection_mode_ != old_cell_selection_mode) { + std::invoke(redraw_map_); + } +} + +void CellSelector::thresholdUpdateCallback() { + // Update the thresholds + surface_occupancy_threshold_ = + surface_occupancy_threshold_property_.getFloat(); + band_min_occupancy_threshold_ = + band_min_occupancy_threshold_property_.getFloat(); + band_max_occupancy_threshold_ = + band_max_occupancy_threshold_property_.getFloat(); + unknown_occupancy_threshold_ = + unknown_occupancy_threshold_property_.getFloat(); + + // Redraw the map + std::invoke(redraw_map_); +} +} // namespace wavemap::rviz_plugin diff --git a/ros/wavemap_rviz_plugin/src/visuals/grid_layer.cpp b/ros/wavemap_rviz_plugin/src/visuals/grid_layer.cc similarity index 100% rename from ros/wavemap_rviz_plugin/src/visuals/grid_layer.cpp rename to ros/wavemap_rviz_plugin/src/visuals/grid_layer.cc diff --git a/ros/wavemap_rviz_plugin/src/visuals/grid_visual.cc b/ros/wavemap_rviz_plugin/src/visuals/grid_visual.cc index b2269ae46..5b0e4e0db 100644 --- a/ros/wavemap_rviz_plugin/src/visuals/grid_visual.cc +++ b/ros/wavemap_rviz_plugin/src/visuals/grid_visual.cc @@ -20,12 +20,7 @@ GridVisual::GridVisual(Ogre::SceneManager* scene_manager, "Whether to show the octree as a multi-resolution grid.", CHECK_NOTNULL(submenu_root_property), SLOT(visibilityUpdateCallback()), this), - min_occupancy_threshold_property_( - "Min log odds", 1e-3, "Ranges from -Inf to Inf.", - submenu_root_property, SLOT(thresholdUpdateCallback()), this), - max_occupancy_threshold_property_( - "Max log odds", 1e6, "Ranges from -Inf to Inf.", - submenu_root_property, SLOT(thresholdUpdateCallback()), this), + cell_selector_(submenu_root_property, [this]() { updateMap(true); }), termination_height_property_( "Termination height", 0, "Controls the resolution at which the map is drawn. Set to 0 to draw " @@ -54,15 +49,17 @@ GridVisual::GridVisual(Ogre::SceneManager* scene_manager, "a reasonable frame rate when maps are large.", &frame_rate_properties_) { // Initialize the property menu + // General + termination_height_property_.setMin(0); + num_queued_blocks_indicator_.setReadOnly(true); + max_ms_per_frame_property_.setMin(0); + // Color mode color_mode_property_.clearOptions(); for (const auto& name : ColorMode::names) { color_mode_property_.addOption(name); } color_mode_property_.setStringStd(grid_color_mode_.toStr()); flat_color_property_.setHidden(grid_color_mode_ != ColorMode::kFlat); - termination_height_property_.setMin(0); - num_queued_blocks_indicator_.setReadOnly(true); - max_ms_per_frame_property_.setMin(0); // Initialize the camera tracker used to update the LOD levels for each block prerender_listener_ = std::make_unique( @@ -103,14 +100,11 @@ void GridVisual::updateMap(bool redraw_all) { if (!map) { return; } + cell_selector_.setMap(map); // Constants const IndexElement tree_height = map->getTreeHeight(); const FloatingPoint min_cell_width = map->getMinCellWidth(); - const FloatingPoint min_log_odds = - min_occupancy_threshold_property_.getFloat(); - const FloatingPoint max_log_odds = - max_occupancy_threshold_property_.getFloat(); const FloatingPoint alpha = opacity_property_.getFloat(); // Limit the max selectable termination height to the height of the tree @@ -163,9 +157,8 @@ void GridVisual::updateMap(bool redraw_all) { const IndexElement num_levels = tree_height + 1; GridLayerList cells_per_level(num_levels); map->forEachLeaf([&](const auto& cell_index, auto cell_log_odds) { - getLeafCentersAndColors(tree_height, min_cell_width, min_log_odds, - max_log_odds, cell_index, cell_log_odds, - cells_per_level); + appendLeafCenterAndColor(tree_height, min_cell_width, cell_index, + cell_log_odds, cells_per_level); }); const Index3D root_idx = Index3D::Zero(); drawMultiResolutionGrid(tree_height, min_cell_width, root_idx, alpha, @@ -292,40 +285,13 @@ void GridVisual::flatColorUpdateCallback() { } } -bool GridVisual::hasFreeNeighbor(FloatingPoint min_occupancy_log_odds, - FloatingPoint max_occupancy_log_odds, - const OctreeIndex& cell_index) { - for (int dim_idx = 0; dim_idx < 3; ++dim_idx) { - for (const int offset : {-1, 1}) { - auto neighbor_index = cell_index; - neighbor_index.position[dim_idx] += offset; - const FloatingPoint neighbor_log_odds = - hashed_map_->getCellValue(neighbor_index); - if (!isOccupied(min_occupancy_log_odds, max_occupancy_log_odds, - neighbor_log_odds)) { - return true; - } - } - } - return false; -} - -void GridVisual::getLeafCentersAndColors(int tree_height, - FloatingPoint min_cell_width, - FloatingPoint min_occupancy_log_odds, - FloatingPoint max_occupancy_log_odds, - const OctreeIndex& cell_index, - FloatingPoint cell_log_odds, - GridLayerList& cells_per_level) { - // Skip cells that don't meet the occupancy threshold - if (!isOccupied(min_occupancy_log_odds, max_occupancy_log_odds, - cell_log_odds)) { - return; - } - - // Skip cells that are occluded by neighbors on all sides - if (!hasFreeNeighbor(min_occupancy_log_odds, max_occupancy_log_odds, - cell_index)) { +void GridVisual::appendLeafCenterAndColor(int tree_height, + FloatingPoint min_cell_width, + const OctreeIndex& cell_index, + FloatingPoint cell_log_odds, + GridLayerList& cells_per_level) { + // Check if the cell should be drawn + if (!cell_selector_.shouldBeDrawn(cell_index, cell_log_odds)) { return; } @@ -407,21 +373,18 @@ void GridVisual::processBlockUpdateQueue() { return; } - if (hashed_map_ = dynamic_cast(map.get()); - hashed_map_) { + if (const auto* hashed_map = + dynamic_cast(map.get()); + hashed_map) { // Constants const FloatingPoint min_cell_width = map->getMinCellWidth(); - const FloatingPoint min_log_odds = - min_occupancy_threshold_property_.getFloat(); - const FloatingPoint max_log_odds = - max_occupancy_threshold_property_.getFloat(); const FloatingPoint alpha = opacity_property_.getFloat(); // Sort the blocks in the queue by their modification time std::map changed_blocks_sorted; for (const auto& [block_idx, term_height] : block_update_queue_) { const Timestamp& last_modified_time = - hashed_map_->getBlock(block_idx).getLastUpdatedStamp(); + hashed_map->getBlock(block_idx).getLastUpdatedStamp(); changed_blocks_sorted[last_modified_time] = block_idx; } @@ -432,7 +395,7 @@ void GridVisual::processBlockUpdateQueue() { std::chrono::milliseconds(max_ms_per_frame_property_.getInt()); const auto max_end_time = start_time + max_time_per_frame; for (const auto& [_, block_idx] : changed_blocks_sorted) { - const auto& block = hashed_map_->getBlock(block_idx); + const auto& block = hashed_map->getBlock(block_idx); const IndexElement tree_height = map->getTreeHeight(); const IndexElement term_height = block_update_queue_[block_idx]; const int num_levels = tree_height + 1 - term_height; @@ -440,9 +403,8 @@ void GridVisual::processBlockUpdateQueue() { block.forEachLeaf( block_idx, [&](const auto& cell_index, auto cell_log_odds) { - getLeafCentersAndColors(tree_height, min_cell_width, min_log_odds, - max_log_odds, cell_index, cell_log_odds, - cells_per_level); + appendLeafCenterAndColor(tree_height, min_cell_width, cell_index, + cell_log_odds, cells_per_level); }, term_height); drawMultiResolutionGrid(tree_height, min_cell_width, block_idx, alpha, From 58b17af5617fcfb5c8b5313a3de18411c80df1de Mon Sep 17 00:00:00 2001 From: Victor Reijgwart Date: Mon, 9 Oct 2023 11:10:05 +0200 Subject: [PATCH 34/70] Major cleanup of utils --- libraries/wavemap/CMakeLists.txt | 1 - libraries/wavemap/include/wavemap/common.h | 2 +- .../include/wavemap/config/config_base.h | 6 +- .../wavemap/config/impl/config_base_inl.h | 6 +- .../wavemap/include/wavemap/config/param.h | 7 +- .../include/wavemap/config/param_checks.h | 2 +- .../include/wavemap/data_structure/aabb.h | 8 +- .../chunked_ndtree/impl/chunked_ndtree_inl.h | 1 - .../impl/ndtree_node_chunk_inl.h | 10 +- .../chunked_ndtree/ndtree_node_chunk.h | 2 +- .../include/wavemap/data_structure/image.h | 9 +- .../data_structure/ndtree/impl/ndtree_inl.h | 1 - .../ndtree/impl/ndtree_node_inl.h | 6 +- .../volumetric/cell_types/haar_coefficients.h | 1 + .../volumetric/cell_types/haar_transform.h | 1 - .../cell_types/impl/haar_transform_inl.h | 42 +-- .../data_structure/volumetric/hashed_blocks.h | 2 +- .../hashed_chunked_wavelet_octree.h | 2 +- .../hashed_chunked_wavelet_octree_block.h | 2 +- .../volumetric/hashed_wavelet_octree.h | 2 +- .../volumetric/hashed_wavelet_octree_block.h | 2 +- .../hashed_chunked_wavelet_octree_block_inl.h | 2 +- .../impl/hashed_wavelet_octree_block_inl.h | 2 +- .../wavemap/indexing/impl/ndtree_index_inl.h | 4 +- .../wavemap/indexing/index_conversions.h | 6 +- .../include/wavemap/indexing/ndtree_index.h | 2 +- .../measurement_model/continuous_beam.h | 2 +- .../impl/ouster_projector_inl.h | 2 + .../impl/spherical_projector_inl.h | 2 + .../projection_model/ouster_projector.h | 1 - .../projection_model/spherical_projector.h | 1 - .../coarse_to_fine/hierarchical_range_sets.h | 69 ---- .../impl/hierarchical_range_bounds_inl.h | 4 +- .../impl/hierarchical_range_sets_inl.h | 324 ------------------ .../impl/range_image_intersector_inl.h | 2 - .../integrator/projective/update_type.h | 27 +- .../wavemap/utils/approximate_trigonometry.h | 82 ----- .../bit_operations.h} | 18 +- .../utils/{ => bits}/morton_encoding.h | 17 +- .../{data_utils.h => data/comparisons.h} | 14 +- .../wavemap/{ => utils/data}/constants.h | 6 +- .../utils/{fill_utils.h => data/fill.h} | 10 +- .../include/wavemap/utils/eigen_format.h | 17 - .../angle_normalization.h} | 6 +- .../utils/math/approximate_trigonometry.h | 82 +++++ .../wavemap/utils/{ => math}/int_math.h | 12 +- .../wavemap/utils/{ => math}/tree_math.h | 8 +- .../include/wavemap/utils/{ => meta}/nameof.h | 14 +- .../wavemap/utils/{ => meta}/type_utils.h | 10 +- .../container.h} | 12 +- .../include/wavemap/utils/print/eigen.h | 15 + .../wavemap/utils/{ => time}/stopwatch.h | 12 +- .../include/wavemap/utils/{ => time}/time.h | 8 +- .../hashed_chunked_wavelet_octree_block.cc | 2 +- .../volumetric_data_structure_base.cc | 2 +- .../projection_model/ouster_projector.cc | 2 +- .../projection_model/spherical_projector.cc | 2 +- .../hashed_chunked_wavelet_integrator.cc | 2 +- libraries/wavemap/src/utils/eigen_format.cc | 6 - libraries/wavemap/src/utils/stopwatch.cc | 2 +- .../test/include/wavemap/test/eigen_utils.h | 10 +- .../test/src/data_structure/test_aabb.cc | 4 +- .../test/src/data_structure/test_haar_cell.cc | 15 +- .../src/data_structure/test_pointcloud.cc | 6 +- .../test_volumetric_data_structure.cc | 3 +- .../src/indexing/test_index_conversions.cc | 29 +- .../test/src/indexing/test_ndtree_index.cc | 2 +- .../projection_model/test_image_projectors.cc | 42 +-- .../test_spherical_projector.cc | 8 +- .../integrator/test_pointcloud_integrators.cc | 6 +- .../test_range_image_intersector.cc | 10 +- .../test/src/iterator/test_grid_iterator.cc | 28 +- .../test/src/iterator/test_ray_iterator.cc | 10 +- .../utils/test_approximate_trigonometry.cc | 2 +- .../test/src/utils/test_bit_manipulation.cc | 166 ++++----- .../wavemap/test/src/utils/test_data_utils.cc | 58 ++-- .../wavemap/test/src/utils/test_fill_utils.cc | 32 +- .../wavemap/test/src/utils/test_int_math.cc | 2 +- .../wavemap/test/src/utils/test_tree_math.cc | 2 +- .../test/src/test_file_conversions.cc | 1 - .../wavemap_ros/input_handler/input_handler.h | 2 +- .../include/wavemap_ros/utils/color.h | 19 - .../include/wavemap_ros/wavemap_server.h | 4 +- .../depth_image_input_handler.cc | 4 +- ros/wavemap_ros/src/wavemap_server.cc | 1 - .../test/src/test_map_msg_conversions.cc | 1 - .../wavemap_rviz_plugin/visuals/grid_visual.h | 2 +- .../src/visuals/grid_visual.cc | 1 - 88 files changed, 489 insertions(+), 907 deletions(-) delete mode 100644 libraries/wavemap/include/wavemap/integrator/projective/coarse_to_fine/hierarchical_range_sets.h delete mode 100644 libraries/wavemap/include/wavemap/integrator/projective/coarse_to_fine/impl/hierarchical_range_sets_inl.h delete mode 100644 libraries/wavemap/include/wavemap/utils/approximate_trigonometry.h rename libraries/wavemap/include/wavemap/utils/{bit_manipulation.h => bits/bit_operations.h} (91%) rename libraries/wavemap/include/wavemap/utils/{ => bits}/morton_encoding.h (90%) rename libraries/wavemap/include/wavemap/utils/{data_utils.h => data/comparisons.h} (75%) rename libraries/wavemap/include/wavemap/{ => utils/data}/constants.h (90%) rename libraries/wavemap/include/wavemap/utils/{fill_utils.h => data/fill.h} (73%) delete mode 100644 libraries/wavemap/include/wavemap/utils/eigen_format.h rename libraries/wavemap/include/wavemap/utils/{angle_utils.h => math/angle_normalization.h} (70%) create mode 100644 libraries/wavemap/include/wavemap/utils/math/approximate_trigonometry.h rename libraries/wavemap/include/wavemap/utils/{ => math}/int_math.h (87%) rename libraries/wavemap/include/wavemap/utils/{ => math}/tree_math.h (84%) rename libraries/wavemap/include/wavemap/utils/{ => meta}/nameof.h (85%) rename libraries/wavemap/include/wavemap/utils/{ => meta}/type_utils.h (92%) rename libraries/wavemap/include/wavemap/utils/{container_print_utils.h => print/container.h} (61%) create mode 100644 libraries/wavemap/include/wavemap/utils/print/eigen.h rename libraries/wavemap/include/wavemap/utils/{ => time}/stopwatch.h (54%) rename libraries/wavemap/include/wavemap/utils/{ => time}/time.h (69%) delete mode 100644 libraries/wavemap/src/utils/eigen_format.cc delete mode 100644 ros/wavemap_ros/include/wavemap_ros/utils/color.h diff --git a/libraries/wavemap/CMakeLists.txt b/libraries/wavemap/CMakeLists.txt index c6d86dc81..45ea9536e 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}) diff --git a/libraries/wavemap/include/wavemap/common.h b/libraries/wavemap/include/wavemap/common.h index 6a659e694..405cc7b80 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; 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..11ef01a9f 100644 --- a/libraries/wavemap/include/wavemap/config/param_checks.h +++ b/libraries/wavemap/include/wavemap/config/param_checks.h @@ -4,7 +4,7 @@ #include #include -#include "wavemap/utils/nameof.h" +#include "wavemap/utils/meta/nameof.h" namespace wavemap { #define IS_PARAM_EQ(value, threshold, verbose) \ 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/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 bd5fcd877..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(); } 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/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 d9fb2f79d..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 { /** 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 94c154d68..dacda5bd8 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 { 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 9b1d3b75e..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 { /** 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_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/indexing/impl/ndtree_index_inl.h b/libraries/wavemap/include/wavemap/indexing/impl/ndtree_index_inl.h index 682eccfbe..1b88efa86 100644 --- a/libraries/wavemap/include/wavemap/indexing/impl/ndtree_index_inl.h +++ b/libraries/wavemap/include/wavemap/indexing/impl/ndtree_index_inl.h @@ -4,8 +4,8 @@ #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 diff --git a/libraries/wavemap/include/wavemap/indexing/index_conversions.h b/libraries/wavemap/include/wavemap/indexing/index_conversions.h index 081da02ab..73544b335 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)) diff --git a/libraries/wavemap/include/wavemap/indexing/ndtree_index.h b/libraries/wavemap/include/wavemap/indexing/ndtree_index.h index 2d910b65c..e8df22ad9 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; 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 17e12a3cc..68fdc4d66 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 { /** 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 b8a53e2d7..947a589ce 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,6 +1,8 @@ #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 SensorCoordinates OusterProjector::cartesianToSensor( const Point3D& C_point) const { 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 be53dc934..e25fc4472 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 @@ -3,6 +3,8 @@ #include +#include "wavemap/utils/math/approximate_trigonometry.h" + namespace wavemap { inline SensorCoordinates SphericalProjector::cartesianToSensor( const Point3D& C_point) const { 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 e80927d11..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 { /** 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 b1f7d1be7..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 { /** 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/hierarchical_range_bounds_inl.h b/libraries/wavemap/include/wavemap/integrator/projective/coarse_to_fine/impl/hierarchical_range_bounds_inl.h index 222dc51d0..5a847d68f 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 @@ -5,7 +5,7 @@ #include #include "wavemap/iterator/grid_iterator.h" -#include "wavemap/utils/bit_manipulation.h" +#include "wavemap/utils/bits/bit_operations.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/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/utils/approximate_trigonometry.h b/libraries/wavemap/include/wavemap/utils/approximate_trigonometry.h deleted file mode 100644 index a8aaa23b9..000000000 --- a/libraries/wavemap/include/wavemap/utils/approximate_trigonometry.h +++ /dev/null @@ -1,82 +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; - 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; -} -} // 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 90% rename from libraries/wavemap/include/wavemap/utils/morton_encoding.h rename to libraries/wavemap/include/wavemap/utils/bits/morton_encoding.h index 92e63eb6a..483fcf9e2 100644 --- a/libraries/wavemap/include/wavemap/utils/morton_encoding.h +++ b/libraries/wavemap/include/wavemap/utils/bits/morton_encoding.h @@ -1,9 +1,10 @@ -#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 @@ -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_utils.h b/libraries/wavemap/include/wavemap/utils/data/comparisons.h similarity index 75% rename from libraries/wavemap/include/wavemap/utils/data_utils.h rename to libraries/wavemap/include/wavemap/utils/data/comparisons.h index 8386c5ff0..938fcfbea 100644 --- a/libraries/wavemap/include/wavemap/utils/data_utils.h +++ b/libraries/wavemap/include/wavemap/utils/data/comparisons.h @@ -1,7 +1,11 @@ -#ifndef WAVEMAP_UTILS_DATA_UTILS_H_ -#define WAVEMAP_UTILS_DATA_UTILS_H_ +#ifndef WAVEMAP_UTILS_DATA_COMPARISONS_H_ +#define WAVEMAP_UTILS_DATA_COMPARISONS_H_ -namespace wavemap::data_utils { +#include +#include +#include + +namespace wavemap::data { // Test strict equality to zero template bool is_nonzero(const T& data) { @@ -23,6 +27,6 @@ bool is_nonzero(const DataT& data, ThresholdT threshold) { return threshold < std::abs(value); }); } -} // namespace wavemap::data_utils +} // namespace wavemap::data -#endif // WAVEMAP_UTILS_DATA_UTILS_H_ +#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/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/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/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/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