From 4be1065bd106be020ffef266802cf4ee8a29d7fd Mon Sep 17 00:00:00 2001 From: Victor Reijgwart Date: Fri, 8 Nov 2024 14:49:21 +0100 Subject: [PATCH 01/22] Explicitly define struct ctors for compatibility with emplace_back --- library/cpp/include/wavemap/core/data_structure/aabb.h | 4 ++++ library/cpp/include/wavemap/core/indexing/ndtree_index.h | 4 ++++ .../core/utils/undistortion/stamped_pose_buffer.h | 9 +++++++-- library/cpp/test/src/core/data_structure/test_aabb.cc | 2 +- .../cpp/test/src/core/indexing/test_index_conversions.cc | 2 +- library/cpp/test/src/core/indexing/test_ndtree_index.cc | 8 ++++---- 6 files changed, 21 insertions(+), 8 deletions(-) diff --git a/library/cpp/include/wavemap/core/data_structure/aabb.h b/library/cpp/include/wavemap/core/data_structure/aabb.h index 2f60c2351..6182cdeda 100644 --- a/library/cpp/include/wavemap/core/data_structure/aabb.h +++ b/library/cpp/include/wavemap/core/data_structure/aabb.h @@ -1,6 +1,7 @@ #ifndef WAVEMAP_CORE_DATA_STRUCTURE_AABB_H_ #define WAVEMAP_CORE_DATA_STRUCTURE_AABB_H_ +#include #include #include @@ -23,6 +24,9 @@ struct AABB { PointType min = PointType::Constant(kInitialMin); PointType max = PointType::Constant(kInitialMax); + AABB() = default; + AABB(PointT min, PointT max) : min(min), max(max) {} + void includePoint(const PointType& point) { min = min.cwiseMin(point); max = max.cwiseMax(point); diff --git a/library/cpp/include/wavemap/core/indexing/ndtree_index.h b/library/cpp/include/wavemap/core/indexing/ndtree_index.h index 3e2d0ff71..825371a63 100644 --- a/library/cpp/include/wavemap/core/indexing/ndtree_index.h +++ b/library/cpp/include/wavemap/core/indexing/ndtree_index.h @@ -32,6 +32,10 @@ struct NdtreeIndex { //! by *height* Position position = Position::Zero(); + NdtreeIndex() = default; + NdtreeIndex(Element height, Position position) + : height(height), position(position) {} + bool operator==(const NdtreeIndex& other) const { return height == other.height && position == other.position; } diff --git a/library/cpp/include/wavemap/core/utils/undistortion/stamped_pose_buffer.h b/library/cpp/include/wavemap/core/utils/undistortion/stamped_pose_buffer.h index f2a155eab..c146daabb 100644 --- a/library/cpp/include/wavemap/core/utils/undistortion/stamped_pose_buffer.h +++ b/library/cpp/include/wavemap/core/utils/undistortion/stamped_pose_buffer.h @@ -7,9 +7,14 @@ namespace wavemap::undistortion { struct StampedPose { - TimeAbsolute stamp; - Transformation3D pose; + TimeAbsolute stamp = 0u; + Transformation3D pose{}; + + StampedPose() = default; + StampedPose(TimeAbsolute stamp, Transformation3D pose) + : stamp(stamp), pose(pose) {} }; + using StampedPoseBuffer = std::vector; } // namespace wavemap::undistortion diff --git a/library/cpp/test/src/core/data_structure/test_aabb.cc b/library/cpp/test/src/core/data_structure/test_aabb.cc index 140e53b7e..a0b17b099 100644 --- a/library/cpp/test/src/core/data_structure/test_aabb.cc +++ b/library/cpp/test/src/core/data_structure/test_aabb.cc @@ -54,7 +54,7 @@ TYPED_TEST(AabbTest, ClosestPointsAndDistances) { const TypeParam min_corner = TypeParam::Zero(); TypeParam max_corner = TypeParam::Ones(); max_corner[dim_idx] = 0.5f; - aabbs.template emplace_back(AABB{min_corner, max_corner}); + aabbs.emplace_back(min_corner, max_corner); } for (const auto& aabb : aabbs) { tests.emplace_back(aabb, TypeParam::Zero()); diff --git a/library/cpp/test/src/core/indexing/test_index_conversions.cc b/library/cpp/test/src/core/indexing/test_index_conversions.cc index f09e93c65..fbd499658 100644 --- a/library/cpp/test/src/core/indexing/test_index_conversions.cc +++ b/library/cpp/test/src/core/indexing/test_index_conversions.cc @@ -62,7 +62,7 @@ TYPED_TEST(IndexConversionsTest, NodeIndexConversions) { GeometryGenerator::getRandomNdtreeIndexVector>( TestFixture::kMinNdtreePositionIndex, TestFixture::kMaxNdtreePositionIndex, 1, TestFixture::kMaxHeight); - random_indices.emplace_back(NdtreeIndex{0, {0, 0}}); + random_indices.emplace_back(0, NdtreeIndex::Position::Zero()); for (IndexElement index_height = 0; index_height < TestFixture::kMaxHeight; ++index_height) { for (IndexElement index_x = -1; index_x <= 1; ++index_x) { diff --git a/library/cpp/test/src/core/indexing/test_ndtree_index.cc b/library/cpp/test/src/core/indexing/test_ndtree_index.cc index 8db3e13c6..443f8914f 100644 --- a/library/cpp/test/src/core/indexing/test_ndtree_index.cc +++ b/library/cpp/test/src/core/indexing/test_ndtree_index.cc @@ -40,7 +40,7 @@ TYPED_TEST(NdtreeIndexTest, ChildParentIndexing) { TestFixture::getRandomNdtreeIndexHeight(0, TestFixture::kMaxHeight); index = index.computeParentIndex(new_height); } - random_indices.emplace_back(TypeParam{0, TypeParam::Position::Zero()}); + random_indices.emplace_back(0, TypeParam::Position::Zero()); for (typename TypeParam::Element height_idx = 0; height_idx < TestFixture::kMaxHeight; ++height_idx) { for (int relative_child_idx = 0; @@ -49,7 +49,7 @@ TYPED_TEST(NdtreeIndexTest, ChildParentIndexing) { for (int dim_idx = 0; dim_idx < TypeParam::kDim; ++dim_idx) { position_index[dim_idx] = (relative_child_idx >> dim_idx) & 0b1; } - random_indices.emplace_back(TypeParam{height_idx, position_index}); + random_indices.emplace_back(height_idx, position_index); } } @@ -109,7 +109,7 @@ TYPED_TEST(NdtreeIndexTest, LastCommonAncestor) { TestFixture::getRandomNdtreeIndexHeight(index.height, 14); index = index.computeParentIndex(test_height); } - random_indices.emplace_back(TypeParam{0, TypeParam::Position::Zero()}); + random_indices.emplace_back(0, TypeParam::Position::Zero()); random_indices.emplace_back( TypeParam{kMaxTreeHeight, TypeParam::Position::Zero()}); @@ -176,7 +176,7 @@ TYPED_TEST(NdtreeIndexTest, LinearOffsets) { GeometryGenerator::getRandomNdtreeIndexVector( TestFixture::kMinNdtreePositionIndex, TestFixture::kMaxNdtreePositionIndex, 0, 0); - random_indices.emplace_back(TypeParam{0, TypeParam::Position::Zero()}); + random_indices.emplace_back(0, TypeParam::Position::Zero()); for (auto& index : random_indices) { const int test_height = TestFixture::getRandomNdtreeIndexHeight(index.height, 14); From 1f365fbd8eb6cf8f818587665ec512bff13d3640 Mon Sep 17 00:00:00 2001 From: Victor Reijgwart Date: Fri, 8 Nov 2024 14:55:36 +0100 Subject: [PATCH 02/22] Refactor TfTransformer to return by value, signal failure using optional --- .../wavemap_ros/utils/tf_transformer.h | 20 ++++++------- .../src/inputs/depth_image_topic_input.cc | 8 ++--- .../src/inputs/pointcloud_topic_input.cc | 10 +++---- .../src/map_operations/crop_map_operation.cc | 8 ++--- .../src/utils/pointcloud_undistorter.cc | 16 +++++----- .../wavemap_ros/src/utils/tf_transformer.cc | 29 ++++++++----------- 6 files changed, 42 insertions(+), 49 deletions(-) diff --git a/interfaces/ros1/wavemap_ros/include/wavemap_ros/utils/tf_transformer.h b/interfaces/ros1/wavemap_ros/include/wavemap_ros/utils/tf_transformer.h index 9905c1e30..4428f00e8 100644 --- a/interfaces/ros1/wavemap_ros/include/wavemap_ros/utils/tf_transformer.h +++ b/interfaces/ros1/wavemap_ros/include/wavemap_ros/utils/tf_transformer.h @@ -2,6 +2,7 @@ #define WAVEMAP_ROS_UTILS_TF_TRANSFORMER_H_ #include +#include #include #include @@ -26,13 +27,11 @@ class TfTransformer { const ros::Time& frame_timestamp); // Lookup transforms and convert them to Kindr - bool lookupTransform(const std::string& to_frame_id, - const std::string& from_frame_id, - const ros::Time& frame_timestamp, - Transformation3D& transform); - bool lookupLatestTransform(const std::string& to_frame_id, - const std::string& from_frame_id, - Transformation3D& transform); + std::optional lookupTransform( + const std::string& to_frame_id, const std::string& from_frame_id, + const ros::Time& frame_timestamp); + std::optional lookupLatestTransform( + const std::string& to_frame_id, const std::string& from_frame_id); // Strip leading slashes if needed to avoid TF errors static std::string sanitizeFrameId(const std::string& string); @@ -50,10 +49,9 @@ class TfTransformer { bool waitForTransformImpl(const std::string& to_frame_id, const std::string& from_frame_id, const ros::Time& frame_timestamp) const; - bool lookupTransformImpl(const std::string& to_frame_id, - const std::string& from_frame_id, - const ros::Time& frame_timestamp, - Transformation3D& transform); + std::optional lookupTransformImpl( + const std::string& to_frame_id, const std::string& from_frame_id, + const ros::Time& frame_timestamp); }; } // namespace wavemap diff --git a/interfaces/ros1/wavemap_ros/src/inputs/depth_image_topic_input.cc b/interfaces/ros1/wavemap_ros/src/inputs/depth_image_topic_input.cc index 8b168bf88..025694725 100644 --- a/interfaces/ros1/wavemap_ros/src/inputs/depth_image_topic_input.cc +++ b/interfaces/ros1/wavemap_ros/src/inputs/depth_image_topic_input.cc @@ -71,11 +71,11 @@ void DepthImageTopicInput::processQueue() { : config_.sensor_frame_id; // Get the sensor pose in world frame - Transformation3D T_W_C; const ros::Time stamp = oldest_msg.header.stamp + ros::Duration(config_.time_offset); - if (!transformer_->lookupTransform(world_frame_, sensor_frame_id, stamp, - T_W_C)) { + const auto T_W_C = + transformer_->lookupTransform(world_frame_, sensor_frame_id, stamp); + if (!T_W_C) { const auto newest_msg = depth_image_queue_.back(); if ((newest_msg.header.stamp - oldest_msg.header.stamp).toSec() < config_.max_wait_for_pose) { @@ -105,7 +105,7 @@ void DepthImageTopicInput::processQueue() { // Create the posed depth image input PosedImage<> posed_depth_image(cv_image->image.rows, cv_image->image.cols); cv::cv2eigen(cv_image->image, posed_depth_image.getData()); - posed_depth_image.setPose(T_W_C); + posed_depth_image.setPose(*T_W_C); // Integrate the depth image ROS_DEBUG_STREAM("Inserting depth image with " diff --git a/interfaces/ros1/wavemap_ros/src/inputs/pointcloud_topic_input.cc b/interfaces/ros1/wavemap_ros/src/inputs/pointcloud_topic_input.cc index 3044f5863..bbd78edc6 100644 --- a/interfaces/ros1/wavemap_ros/src/inputs/pointcloud_topic_input.cc +++ b/interfaces/ros1/wavemap_ros/src/inputs/pointcloud_topic_input.cc @@ -236,16 +236,16 @@ void PointcloudTopicInput::processQueue() { } } else { // Get the pointcloud's pose - Transformation3D T_W_C; - if (!transformer_->lookupTransform( - world_frame_, oldest_msg.getSensorFrame(), - convert::nanoSecondsToRosTime(oldest_msg.getTimeBase()), T_W_C)) { + const auto T_W_C = transformer_->lookupTransform( + world_frame_, oldest_msg.getSensorFrame(), + convert::nanoSecondsToRosTime(oldest_msg.getTimeBase())); + if (!T_W_C) { // Try to get this pointcloud's pose again at the next iteration return; } // Convert to a posed pointcloud - posed_pointcloud = PosedPointcloud<>(T_W_C); + posed_pointcloud = PosedPointcloud<>(*T_W_C); posed_pointcloud.resize(oldest_msg.getPoints().size()); for (unsigned point_idx = 0; point_idx < oldest_msg.getPoints().size(); ++point_idx) { diff --git a/interfaces/ros1/wavemap_ros/src/map_operations/crop_map_operation.cc b/interfaces/ros1/wavemap_ros/src/map_operations/crop_map_operation.cc index 2ab72c618..e1bbebc5e 100644 --- a/interfaces/ros1/wavemap_ros/src/map_operations/crop_map_operation.cc +++ b/interfaces/ros1/wavemap_ros/src/map_operations/crop_map_operation.cc @@ -49,9 +49,9 @@ void CropMapOperation::run(bool force_run) { return; } - Transformation3D T_W_B; - if (!transformer_->lookupTransform(world_frame_, config_.body_frame, - current_time, T_W_B)) { + const auto T_W_B = transformer_->lookupTransform( + world_frame_, config_.body_frame, current_time); + if (!T_W_B) { ROS_WARN_STREAM( "Could not look up center point for map cropping. TF lookup of " "body_frame \"" @@ -62,7 +62,7 @@ void CropMapOperation::run(bool force_run) { const IndexElement tree_height = occupancy_map_->getTreeHeight(); const FloatingPoint min_cell_width = occupancy_map_->getMinCellWidth(); - const Point3D t_W_B = T_W_B.getPosition(); + const Point3D t_W_B = T_W_B->getPosition(); auto indicator_fn = [tree_height, min_cell_width, &config = config_, &t_W_B]( const Index3D& block_index, const auto& /*block*/) { diff --git a/interfaces/ros1/wavemap_ros/src/utils/pointcloud_undistorter.cc b/interfaces/ros1/wavemap_ros/src/utils/pointcloud_undistorter.cc index d7e3106a2..16e5334a8 100644 --- a/interfaces/ros1/wavemap_ros/src/utils/pointcloud_undistorter.cc +++ b/interfaces/ros1/wavemap_ros/src/utils/pointcloud_undistorter.cc @@ -42,15 +42,15 @@ PointcloudUndistorter::Result PointcloudUndistorter::undistortPointcloud( undistortion::StampedPoseBuffer pose_buffer; pose_buffer.reserve(num_time_steps); for (int step_idx = 0; step_idx < num_time_steps; ++step_idx) { - auto& stamped_pose = pose_buffer.emplace_back(); - stamped_pose.stamp = start_time + step_idx * step_size; - if (!transformer_->lookupTransform( - fixed_frame, stamped_pointcloud.getSensorFrame(), - convert::nanoSecondsToRosTime(stamped_pose.stamp), - stamped_pose.pose)) { + const auto stamp = start_time + step_idx * step_size; + const auto pose = transformer_->lookupTransform( + fixed_frame, stamped_pointcloud.getSensorFrame(), + convert::nanoSecondsToRosTime(stamp)); + if (pose) { + pose_buffer.emplace_back(stamp, *pose); + } else { ROS_WARN_STREAM("Failed to buffer intermediate pose at time " - << convert::nanoSecondsToRosTime(stamped_pose.stamp) - << "."); + << convert::nanoSecondsToRosTime(stamp) << "."); return Result::kIntermediateTimeNotInTfBuffer; } } diff --git a/interfaces/ros1/wavemap_ros/src/utils/tf_transformer.cc b/interfaces/ros1/wavemap_ros/src/utils/tf_transformer.cc index 2b212927a..54cbf73e4 100644 --- a/interfaces/ros1/wavemap_ros/src/utils/tf_transformer.cc +++ b/interfaces/ros1/wavemap_ros/src/utils/tf_transformer.cc @@ -20,20 +20,17 @@ bool TfTransformer::waitForTransform(const std::string& to_frame_id, sanitizeFrameId(from_frame_id), frame_timestamp); } -bool TfTransformer::lookupLatestTransform(const std::string& to_frame_id, - const std::string& from_frame_id, - Transformation3D& transform) { +std::optional TfTransformer::lookupLatestTransform( + const std::string& to_frame_id, const std::string& from_frame_id) { return lookupTransformImpl(sanitizeFrameId(to_frame_id), - sanitizeFrameId(from_frame_id), {}, transform); + sanitizeFrameId(from_frame_id), {}); } -bool TfTransformer::lookupTransform(const std::string& to_frame_id, - const std::string& from_frame_id, - const ros::Time& frame_timestamp, - Transformation3D& transform) { +std::optional TfTransformer::lookupTransform( + const std::string& to_frame_id, const std::string& from_frame_id, + const ros::Time& frame_timestamp) { return lookupTransformImpl(sanitizeFrameId(to_frame_id), - sanitizeFrameId(from_frame_id), frame_timestamp, - transform); + sanitizeFrameId(from_frame_id), frame_timestamp); } std::string TfTransformer::sanitizeFrameId(const std::string& string) { @@ -64,16 +61,14 @@ bool TfTransformer::waitForTransformImpl( return false; } -bool TfTransformer::lookupTransformImpl(const std::string& to_frame_id, - const std::string& from_frame_id, - const ros::Time& frame_timestamp, - Transformation3D& transform) { +std::optional TfTransformer::lookupTransformImpl( + const std::string& to_frame_id, const std::string& from_frame_id, + const ros::Time& frame_timestamp) { if (!isTransformAvailable(to_frame_id, from_frame_id, frame_timestamp)) { - return false; + return std::nullopt; } geometry_msgs::TransformStamped transform_msg = tf_buffer_.lookupTransform(to_frame_id, from_frame_id, frame_timestamp); - transform = convert::transformMsgToTransformation3D(transform_msg.transform); - return true; + return convert::transformMsgToTransformation3D(transform_msg.transform); } } // namespace wavemap From ab59bd9c5ed397b0c8eb1408724f90b249b75075 Mon Sep 17 00:00:00 2001 From: Victor Reijgwart Date: Sun, 10 Nov 2024 00:58:05 +0100 Subject: [PATCH 03/22] Use ChunkedNdtreeNodeRef to simplify chunked map accessors --- .../chunked_ndtree_node_address.h | 2 +- .../impl/chunked_ndtree_node_address_inl.h | 3 +- .../hashed_chunked_wavelet_octree_block_inl.h | 42 ++---- .../hashed_chunked_wavelet_octree_block.cc | 127 ++++++------------ 4 files changed, 53 insertions(+), 121 deletions(-) diff --git a/library/cpp/include/wavemap/core/data_structure/chunked_ndtree/chunked_ndtree_node_address.h b/library/cpp/include/wavemap/core/data_structure/chunked_ndtree/chunked_ndtree_node_address.h index 3f05d8854..c059bf372 100644 --- a/library/cpp/include/wavemap/core/data_structure/chunked_ndtree/chunked_ndtree_node_address.h +++ b/library/cpp/include/wavemap/core/data_structure/chunked_ndtree/chunked_ndtree_node_address.h @@ -89,7 +89,7 @@ class ChunkedNdtreeNodeRef { NodeConstPtr getChild(NdtreeIndexRelativeChild child_index) const; template NodeRef getOrAllocateChild(NdtreeIndexRelativeChild child_index, - DefaultArgs&&... args) const; + DefaultArgs&&... args); private: ChunkType& chunk_; diff --git a/library/cpp/include/wavemap/core/data_structure/chunked_ndtree/impl/chunked_ndtree_node_address_inl.h b/library/cpp/include/wavemap/core/data_structure/chunked_ndtree/impl/chunked_ndtree_node_address_inl.h index f2d71c297..608056d07 100644 --- a/library/cpp/include/wavemap/core/data_structure/chunked_ndtree/impl/chunked_ndtree_node_address_inl.h +++ b/library/cpp/include/wavemap/core/data_structure/chunked_ndtree/impl/chunked_ndtree_node_address_inl.h @@ -161,7 +161,8 @@ template template ChunkedNdtreeNodeRef ChunkedNdtreeNodeRef::getOrAllocateChild( - NdtreeIndexRelativeChild child_index, DefaultArgs&&... args) const { + NdtreeIndexRelativeChild child_index, DefaultArgs&&... args) { + hasAtLeastOneChild() = true; const IndexElement child_depth = relative_node_depth_ + 1; const LinearIndex child_level_traversal_distance = computeChildLevelTraversalDistance(child_index); diff --git a/library/cpp/include/wavemap/core/map/impl/hashed_chunked_wavelet_octree_block_inl.h b/library/cpp/include/wavemap/core/map/impl/hashed_chunked_wavelet_octree_block_inl.h index 42fc84154..3eeb42931 100644 --- a/library/cpp/include/wavemap/core/map/impl/hashed_chunked_wavelet_octree_block_inl.h +++ b/library/cpp/include/wavemap/core/map/impl/hashed_chunked_wavelet_octree_block_inl.h @@ -20,44 +20,20 @@ inline FloatingPoint HashedChunkedWaveletOctreeBlock::getTimeSinceLastUpdated() inline FloatingPoint HashedChunkedWaveletOctreeBlock::getCellValue( const OctreeIndex& index) const { - // Descend the tree chunk by chunk const MortonIndex morton_code = convert::nodeIndexToMorton(index); - const ChunkedOctreeType::ChunkType* current_chunk = - &chunked_ndtree_.getRootChunk(); + ChunkedOctreeType::NodeConstPtrType node = &chunked_ndtree_.getRootNode(); FloatingPoint value = root_scale_coefficient_; - for (int chunk_top_height = tree_height_; index.height < chunk_top_height; - chunk_top_height -= kChunkHeight) { - // Decompress level by level - for (int parent_height = chunk_top_height; - chunk_top_height - kChunkHeight < parent_height; --parent_height) { - // Perform one decompression stage - const LinearIndex relative_node_index = - OctreeIndex::computeTreeTraversalDistance( - morton_code, chunk_top_height, parent_height); - const NdtreeIndexRelativeChild relative_child_index = - OctreeIndex::computeRelativeChildIndex(morton_code, parent_height); - value = Transform::backwardSingleChild( - {value, current_chunk->nodeData(relative_node_index)}, - relative_child_index); - // If we've reached the requested resolution or there are no remaining - // higher resolution details, return - if (parent_height == index.height + 1 || - !current_chunk->nodeHasAtLeastOneChild(relative_node_index)) { - return value; - } - } - - // Descend to the next chunk if it exists - const LinearIndex linear_child_index = - OctreeIndex::computeLevelTraversalDistance( - morton_code, chunk_top_height, chunk_top_height - kChunkHeight); - // If there are no remaining higher resolution details, return - if (!current_chunk->hasChild(linear_child_index)) { + for (int parent_height = tree_height_; index.height < parent_height; + --parent_height) { + const NdtreeIndexRelativeChild child_index = + OctreeIndex::computeRelativeChildIndex(morton_code, parent_height); + value = Transform::backwardSingleChild({value, node->data()}, child_index); + auto child = node->getChild(child_index); + if (!child) { break; } - current_chunk = current_chunk->getChild(linear_child_index); + node = child; } - return value; } } // namespace wavemap diff --git a/library/cpp/src/core/map/hashed_chunked_wavelet_octree_block.cc b/library/cpp/src/core/map/hashed_chunked_wavelet_octree_block.cc index d9aea9bb8..ff36d967c 100644 --- a/library/cpp/src/core/map/hashed_chunked_wavelet_octree_block.cc +++ b/library/cpp/src/core/map/hashed_chunked_wavelet_octree_block.cc @@ -2,6 +2,7 @@ #include #include +#include #include @@ -37,70 +38,37 @@ void HashedChunkedWaveletOctreeBlock::setCellValue(const OctreeIndex& index, setNeedsPruning(); setNeedsThresholding(); setLastUpdatedStamp(); - - // Descend the tree chunk by chunk while decompressing, and caching chunk ptrs const MortonIndex morton_code = convert::nodeIndexToMorton(index); - std::array chunk_ptrs{}; - chunk_ptrs[0] = &chunked_ndtree_.getRootChunk(); + std::vector ancestors; + const int height_difference = tree_height_ - index.height; + ancestors.reserve(height_difference); + ancestors.emplace_back(chunked_ndtree_.getRootNode()); FloatingPoint current_value = root_scale_coefficient_; - for (int chunk_top_height = tree_height_; index.height < chunk_top_height; - chunk_top_height -= kChunkHeight) { - // Get the current chunk - const int chunk_depth = (tree_height_ - chunk_top_height) / kChunkHeight; - ChunkedOctreeType::ChunkType* const current_chunk = chunk_ptrs[chunk_depth]; - // Decompress level by level - for (int parent_height = chunk_top_height; - chunk_top_height - kChunkHeight < parent_height; --parent_height) { - // Perform one decompression stage - const LinearIndex relative_node_index = - OctreeIndex::computeTreeTraversalDistance( - morton_code, chunk_top_height, parent_height); - const NdtreeIndexRelativeChild relative_child_index = - OctreeIndex::computeRelativeChildIndex(morton_code, parent_height); - current_value = Transform::backwardSingleChild( - {current_value, current_chunk->nodeData(relative_node_index)}, - relative_child_index); - // If we've reached the requested resolution, stop descending - if (parent_height == index.height + 1) { - break; - } - } - if (chunk_top_height - kChunkHeight <= index.height + 1) { - break; - } - - // Descend to the next chunk - const LinearIndex linear_child_index = - OctreeIndex::computeLevelTraversalDistance( - morton_code, chunk_top_height, chunk_top_height - kChunkHeight); - if (current_chunk->hasChild(linear_child_index)) { - chunk_ptrs[chunk_depth + 1] = current_chunk->getChild(linear_child_index); - } else { - chunk_ptrs[chunk_depth + 1] = - ¤t_chunk->getOrAllocateChild(linear_child_index); - } + for (int parent_height = tree_height_; index.height + 1 < parent_height; + --parent_height) { + const NdtreeIndexRelativeChild child_index = + OctreeIndex::computeRelativeChildIndex(morton_code, parent_height); + ChunkedOctreeType::NodeRefType current_parent = ancestors.back(); + current_value = Transform::backwardSingleChild( + {current_value, current_parent.data()}, child_index); + ChunkedOctreeType::NodeRefType child = + current_parent.getOrAllocateChild(child_index); + ancestors.emplace_back(child); } + DCHECK_EQ(ancestors.size(), height_difference); Coefficients::Parent coefficients{new_value - current_value, {}}; for (int parent_height = index.height + 1; parent_height <= tree_height_; ++parent_height) { - // Get the current chunk - const int chunk_depth = (tree_height_ - parent_height) / kChunkHeight; - ChunkedOctreeType::ChunkType* current_chunk = chunk_ptrs[chunk_depth]; - // Get the index of the data w.r.t. the chunk - const int chunk_top_height = tree_height_ - chunk_depth * kChunkHeight; - const LinearIndex relative_node_index = - OctreeIndex::computeTreeTraversalDistance(morton_code, chunk_top_height, - parent_height); - // Compute and apply the transformed update - const NdtreeIndexRelativeChild relative_child_index = + const NdtreeIndexRelativeChild child_index = OctreeIndex::computeRelativeChildIndex(morton_code, parent_height); + ChunkedOctreeType::NodeRefType current_node = ancestors.back(); + ancestors.pop_back(); coefficients = - Transform::forwardSingleChild(coefficients.scale, relative_child_index); - current_chunk->nodeData(relative_node_index) += coefficients.details; - // TODO(victorr): Flag should skip last level - current_chunk->nodeHasAtLeastOneChild(relative_node_index) = true; + Transform::forwardSingleChild(coefficients.scale, child_index); + current_node.data() += coefficients.details; } + root_scale_coefficient_ += coefficients.scale; } @@ -109,46 +77,33 @@ void HashedChunkedWaveletOctreeBlock::addToCellValue(const OctreeIndex& index, setNeedsPruning(); setNeedsThresholding(); setLastUpdatedStamp(); - const MortonIndex morton_code = convert::nodeIndexToMorton(index); - std::array chunk_ptrs{}; - chunk_ptrs[0] = &chunked_ndtree_.getRootChunk(); - const int last_chunk_depth = (tree_height_ - index.height - 1) / kChunkHeight; - for (int chunk_depth = 1; chunk_depth <= last_chunk_depth; ++chunk_depth) { - const int parent_chunk_top_height = - tree_height_ - (chunk_depth - 1) * kChunkHeight; - const int chunk_top_height = tree_height_ - chunk_depth * kChunkHeight; - const LinearIndex linear_child_index = - OctreeIndex::computeLevelTraversalDistance( - morton_code, parent_chunk_top_height, chunk_top_height); - ChunkedOctreeType::ChunkType* current_chunk = chunk_ptrs[chunk_depth - 1]; - if (current_chunk->hasChild(linear_child_index)) { - chunk_ptrs[chunk_depth] = current_chunk->getChild(linear_child_index); - } else { - chunk_ptrs[chunk_depth] = - ¤t_chunk->getOrAllocateChild(linear_child_index); - } + + std::vector ancestors; + const int height_difference = tree_height_ - index.height; + ancestors.reserve(height_difference); + ancestors.emplace_back(chunked_ndtree_.getRootNode()); + for (int parent_height = tree_height_; index.height + 1 < parent_height; + --parent_height) { + const NdtreeIndexRelativeChild child_index = + OctreeIndex::computeRelativeChildIndex(morton_code, parent_height); + ChunkedOctreeType::NodeRefType current_parent = ancestors.back(); + ChunkedOctreeType::NodeRefType child = + current_parent.getOrAllocateChild(child_index); + ancestors.emplace_back(child); } + DCHECK_EQ(ancestors.size(), height_difference); Coefficients::Parent coefficients{update, {}}; for (int parent_height = index.height + 1; parent_height <= tree_height_; ++parent_height) { - // Get the current chunk - const int chunk_depth = (tree_height_ - parent_height) / kChunkHeight; - ChunkedOctreeType::ChunkType* current_chunk = chunk_ptrs[chunk_depth]; - // Get the index of the data w.r.t. the chunk - const int chunk_top_height = tree_height_ - chunk_depth * kChunkHeight; - const LinearIndex relative_node_index = - OctreeIndex::computeTreeTraversalDistance(morton_code, chunk_top_height, - parent_height); - // Compute and apply the transformed update - const NdtreeIndexRelativeChild relative_child_index = + ChunkedOctreeType::NodeRefType current_node = ancestors.back(); + ancestors.pop_back(); + const NdtreeIndexRelativeChild child_index = OctreeIndex::computeRelativeChildIndex(morton_code, parent_height); coefficients = - Transform::forwardSingleChild(coefficients.scale, relative_child_index); - current_chunk->nodeData(relative_node_index) += coefficients.details; - // TODO(victorr): Flag should skip last level - current_chunk->nodeHasAtLeastOneChild(relative_node_index) = true; + Transform::forwardSingleChild(coefficients.scale, child_index); + current_node.data() += coefficients.details; } root_scale_coefficient_ += coefficients.scale; } From 0b1a86f450b8cb6f924f89f3cf236450c807c575 Mon Sep 17 00:00:00 2001 From: Victor Reijgwart Date: Sun, 10 Nov 2024 01:05:05 +0100 Subject: [PATCH 04/22] Minor code cleanup --- .../core/utils/iterate/subtree_iterator.h | 4 +-- .../core/map/hashed_wavelet_octree_block.cc | 32 +++++++++---------- 2 files changed, 18 insertions(+), 18 deletions(-) diff --git a/library/cpp/include/wavemap/core/utils/iterate/subtree_iterator.h b/library/cpp/include/wavemap/core/utils/iterate/subtree_iterator.h index 9050a0db0..acff52e3b 100644 --- a/library/cpp/include/wavemap/core/utils/iterate/subtree_iterator.h +++ b/library/cpp/include/wavemap/core/utils/iterate/subtree_iterator.h @@ -61,7 +61,7 @@ class SubtreeIterator public: explicit SubtreeIterator(NodeT* root_node) { if (root_node) { - upcoming_nodes_.template emplace_front(root_node); + upcoming_nodes_.emplace_front(root_node); } } SubtreeIterator& operator++() override; @@ -109,7 +109,7 @@ class SubtreeIterator public: explicit SubtreeIterator(NodeT* root_node) { if (root_node) { - upcoming_nodes_.template emplace_front(root_node); + upcoming_nodes_.emplace_front(root_node); } } SubtreeIterator& operator++() override; diff --git a/library/cpp/src/core/map/hashed_wavelet_octree_block.cc b/library/cpp/src/core/map/hashed_wavelet_octree_block.cc index 7ab3ec524..e37344ab8 100644 --- a/library/cpp/src/core/map/hashed_wavelet_octree_block.cc +++ b/library/cpp/src/core/map/hashed_wavelet_octree_block.cc @@ -37,30 +37,30 @@ void HashedWaveletOctreeBlock::setCellValue(const OctreeIndex& index, setNeedsThresholding(); setLastUpdatedStamp(); const MortonIndex morton_code = convert::nodeIndexToMorton(index); - std::vector node_ptrs; + std::vector ancestors; const int height_difference = tree_height_ - index.height; - node_ptrs.reserve(height_difference); - node_ptrs.emplace_back(&ndtree_.getRootNode()); + ancestors.reserve(height_difference); + ancestors.emplace_back(&ndtree_.getRootNode()); FloatingPoint current_value = root_scale_coefficient_; for (int parent_height = tree_height_; index.height + 1 < parent_height; --parent_height) { const NdtreeIndexRelativeChild child_index = OctreeIndex::computeRelativeChildIndex(morton_code, parent_height); - NodeType* current_parent = node_ptrs.back(); + NodeType* current_parent = ancestors.back(); current_value = Transform::backwardSingleChild( {current_value, current_parent->data()}, child_index); NodeType& child = current_parent->getOrAllocateChild(child_index); - node_ptrs.emplace_back(&child); + ancestors.emplace_back(&child); } - DCHECK_EQ(node_ptrs.size(), height_difference); + DCHECK_EQ(ancestors.size(), height_difference); Coefficients::Parent coefficients{new_value - current_value, {}}; for (int parent_height = index.height + 1; parent_height <= tree_height_; ++parent_height) { const NdtreeIndexRelativeChild child_index = OctreeIndex::computeRelativeChildIndex(morton_code, parent_height); - NodeType* current_node = node_ptrs.back(); - node_ptrs.pop_back(); + NodeType* current_node = ancestors.back(); + ancestors.pop_back(); coefficients = Transform::forwardSingleChild(coefficients.scale, child_index); current_node->data() += coefficients.details; @@ -76,25 +76,25 @@ void HashedWaveletOctreeBlock::addToCellValue(const OctreeIndex& index, setLastUpdatedStamp(); const MortonIndex morton_code = convert::nodeIndexToMorton(index); - std::vector node_ptrs; + std::vector ancestors; const int height_difference = tree_height_ - index.height; - node_ptrs.reserve(height_difference); - node_ptrs.emplace_back(&ndtree_.getRootNode()); + ancestors.reserve(height_difference); + ancestors.emplace_back(&ndtree_.getRootNode()); for (int parent_height = tree_height_; index.height + 1 < parent_height; --parent_height) { const NdtreeIndexRelativeChild child_index = OctreeIndex::computeRelativeChildIndex(morton_code, parent_height); - NodeType* current_parent = node_ptrs.back(); + NodeType* current_parent = ancestors.back(); NodeType& child = current_parent->getOrAllocateChild(child_index); - node_ptrs.emplace_back(&child); + ancestors.emplace_back(&child); } - DCHECK_EQ(node_ptrs.size(), height_difference); + DCHECK_EQ(ancestors.size(), height_difference); Coefficients::Parent coefficients{update, {}}; for (int parent_height = index.height + 1; parent_height <= tree_height_; ++parent_height) { - NodeType* current_node = node_ptrs.back(); - node_ptrs.pop_back(); + NodeType* current_node = ancestors.back(); + ancestors.pop_back(); const NdtreeIndexRelativeChild child_index = OctreeIndex::computeRelativeChildIndex(morton_code, parent_height); coefficients = From af7ecc471e751e15eb4b8d9fe456b7fce274cb08 Mon Sep 17 00:00:00 2001 From: Victor Reijgwart Date: Wed, 25 Sep 2024 17:02:24 +0200 Subject: [PATCH 05/22] Simplify integrator code by using new ChunkedNdtree API features --- .../hashed_chunked_wavelet_integrator.h | 23 ++++------ .../hashed_chunked_wavelet_integrator_inl.h | 2 +- .../hashed_chunked_wavelet_integrator.cc | 46 ++++--------------- 3 files changed, 21 insertions(+), 50 deletions(-) diff --git a/library/cpp/include/wavemap/core/integrator/projective/coarse_to_fine/hashed_chunked_wavelet_integrator.h b/library/cpp/include/wavemap/core/integrator/projective/coarse_to_fine/hashed_chunked_wavelet_integrator.h index ff8cfc916..f105a88b7 100644 --- a/library/cpp/include/wavemap/core/integrator/projective/coarse_to_fine/hashed_chunked_wavelet_integrator.h +++ b/library/cpp/include/wavemap/core/integrator/projective/coarse_to_fine/hashed_chunked_wavelet_integrator.h @@ -5,6 +5,7 @@ #include #include +#include "wavemap/core/data_structure/chunked_ndtree/chunked_ndtree_node_address.h" #include "wavemap/core/integrator/projective/coarse_to_fine/range_image_intersector.h" #include "wavemap/core/integrator/projective/projective_integrator.h" #include "wavemap/core/map/hashed_chunked_wavelet_octree.h" @@ -31,6 +32,7 @@ class HashedChunkedWaveletIntegrator : public ProjectiveIntegrator { private: using BlockList = std::vector; + using OctreeType = HashedChunkedWaveletOctreeBlock::ChunkedOctreeType; const HashedChunkedWaveletOctree::Ptr occupancy_map_; std::shared_ptr thread_pool_; @@ -48,7 +50,6 @@ class HashedChunkedWaveletIntegrator : public ProjectiveIntegrator { const FloatingPoint min_log_odds_shrunk_ = min_log_odds_ + kNoiseThreshold; const FloatingPoint max_log_odds_padded_ = max_log_odds_ + kNoiseThreshold; const IndexElement tree_height_ = occupancy_map_->getTreeHeight(); - const IndexElement chunk_height_ = occupancy_map_->getChunkHeight(); const IndexElement termination_height_ = min_cell_width_ < config_.max_update_resolution ? static_cast(std::round( @@ -64,18 +65,14 @@ class HashedChunkedWaveletIntegrator : public ProjectiveIntegrator { void updateBlock(HashedChunkedWaveletOctree::Block& block, const HashedChunkedWaveletOctree::BlockIndex& block_index); - void updateNodeRecursive( - HashedChunkedWaveletOctreeBlock::ChunkedOctreeType::ChunkType& - parent_chunk, - const OctreeIndex& parent_node_index, LinearIndex parent_in_chunk_index, - FloatingPoint& parent_value, - HashedChunkedWaveletOctreeBlock::ChunkedOctreeType::ChunkType::BitRef - parent_has_child, - bool& block_needs_thresholding); - void updateLeavesBatch( - const OctreeIndex& parent_index, FloatingPoint& parent_value, - HashedChunkedWaveletOctreeBlock::ChunkedOctreeType::ChunkType::DataType& - parent_details); + void updateNodeRecursive(OctreeType::NodeRefType parent_node_ref, + const OctreeIndex& parent_node_index, + FloatingPoint& parent_value, + OctreeType::ChunkType::BitRef parent_has_child, + bool& block_needs_thresholding); + void updateLeavesBatch(const OctreeIndex& parent_index, + FloatingPoint& parent_value, + OctreeType::NodeDataType& parent_details); }; } // namespace wavemap diff --git a/library/cpp/include/wavemap/core/integrator/projective/coarse_to_fine/impl/hashed_chunked_wavelet_integrator_inl.h b/library/cpp/include/wavemap/core/integrator/projective/coarse_to_fine/impl/hashed_chunked_wavelet_integrator_inl.h index 0d5e83c05..f6159ce93 100644 --- a/library/cpp/include/wavemap/core/integrator/projective/coarse_to_fine/impl/hashed_chunked_wavelet_integrator_inl.h +++ b/library/cpp/include/wavemap/core/integrator/projective/coarse_to_fine/impl/hashed_chunked_wavelet_integrator_inl.h @@ -39,7 +39,7 @@ inline void HashedChunkedWaveletIntegrator::recursiveTester( // NOLINT inline void HashedChunkedWaveletIntegrator::updateLeavesBatch( const OctreeIndex& parent_index, FloatingPoint& parent_value, - HaarCoefficients::Details& parent_details) { + HashedChunkedWaveletIntegrator::OctreeType::NodeDataType& parent_details) { // Decompress auto child_values = HashedChunkedWaveletOctreeBlock::Transform::backward( {parent_value, parent_details}); diff --git a/library/cpp/src/core/integrator/projective/coarse_to_fine/hashed_chunked_wavelet_integrator.cc b/library/cpp/src/core/integrator/projective/coarse_to_fine/hashed_chunked_wavelet_integrator.cc index 3352217c9..72edfcff9 100644 --- a/library/cpp/src/core/integrator/projective/coarse_to_fine/hashed_chunked_wavelet_integrator.cc +++ b/library/cpp/src/core/integrator/projective/coarse_to_fine/hashed_chunked_wavelet_integrator.cc @@ -77,7 +77,7 @@ void HashedChunkedWaveletIntegrator::updateBlock( bool block_needs_thresholding = block.getNeedsThresholding(); const OctreeIndex root_node_index{tree_height_, block_index}; - updateNodeRecursive(block.getRootChunk(), root_node_index, 0u, + updateNodeRecursive(block.getRootNode(), root_node_index, block.getRootScale(), block.getRootChunk().nodeHasAtLeastOneChild(0u), block_needs_thresholding); @@ -85,13 +85,12 @@ void HashedChunkedWaveletIntegrator::updateBlock( } void HashedChunkedWaveletIntegrator::updateNodeRecursive( // NOLINT - HashedChunkedWaveletOctreeBlock::ChunkedOctreeType::ChunkType& parent_chunk, - const OctreeIndex& parent_node_index, LinearIndex parent_in_chunk_index, - FloatingPoint& parent_value, - HashedChunkedWaveletOctreeBlock::ChunkedOctreeType::ChunkType::BitRef + HashedChunkedWaveletIntegrator::OctreeType::NodeRefType parent_node_ref, + const OctreeIndex& parent_node_index, FloatingPoint& parent_value, + HashedChunkedWaveletIntegrator::OctreeType::ChunkType::BitRef parent_has_child, bool& block_needs_thresholding) { - auto& parent_details = parent_chunk.nodeData(parent_in_chunk_index); + auto& parent_details = parent_node_ref.data(); auto child_values = HashedChunkedWaveletOctreeBlock::Transform::backward( {parent_value, parent_details}); @@ -145,34 +144,10 @@ void HashedChunkedWaveletIntegrator::updateNodeRecursive( // NOLINT } // Since the approximation error would still be too big, refine - 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_); - - HashedChunkedWaveletOctreeBlock::ChunkedOctreeType::ChunkType* - 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, child_height); - chunk_containing_child = parent_chunk.getChild(linear_child_index); - if (!chunk_containing_child) { - chunk_containing_child = - &parent_chunk.getOrAllocateChild(linear_child_index); - } - 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); + auto child_node_ref = + parent_node_ref.getOrAllocateChild(relative_child_idx); + auto& child_details = child_node_ref.data(); + auto child_has_children = child_node_ref.hasAtLeastOneChild(); // If we're at the leaf level, directly compute the update if (child_height <= termination_height_ + 1) { @@ -180,8 +155,7 @@ void HashedChunkedWaveletIntegrator::updateNodeRecursive( // NOLINT } else { // Otherwise, recurse DCHECK_GE(child_height, 0); - updateNodeRecursive(*chunk_containing_child, child_index, - child_node_in_chunk_index, child_value, + updateNodeRecursive(child_node_ref, child_index, child_value, child_has_children, block_needs_thresholding); } From 129f95b5f48535d9fc472a98b204df6ea1ef7638 Mon Sep 17 00:00:00 2001 From: Victor Reijgwart Date: Mon, 11 Nov 2024 14:49:13 +0100 Subject: [PATCH 06/22] Add QueryAccelerator for chunked wavelet octrees --- .../core/map/hashed_chunked_wavelet_octree.h | 6 +- .../map/hashed_chunked_wavelet_octree_block.h | 3 - .../utils/query/impl/query_accelerator_inl.h | 2 +- .../core/utils/query/query_accelerator.h | 58 ++++++++++++- .../src/core/utils/query/classified_map.cc | 2 +- .../src/core/utils/query/query_accelerator.cc | 87 ++++++++++++++++++- .../utils/query/test_query_accelerator.cc | 34 +++++--- 7 files changed, 167 insertions(+), 25 deletions(-) diff --git a/library/cpp/include/wavemap/core/map/hashed_chunked_wavelet_octree.h b/library/cpp/include/wavemap/core/map/hashed_chunked_wavelet_octree.h index c16bc40a8..3d4ee2a43 100644 --- a/library/cpp/include/wavemap/core/map/hashed_chunked_wavelet_octree.h +++ b/library/cpp/include/wavemap/core/map/hashed_chunked_wavelet_octree.h @@ -124,15 +124,15 @@ class HashedChunkedWaveletOctree : public MapBase { void forEachLeaf( typename MapBase::IndexedLeafVisitorFunction visitor_fn) const override; + BlockIndex indexToBlockIndex(const OctreeIndex& node_index) const; + CellIndex indexToCellIndex(OctreeIndex index) const; + private: const HashedChunkedWaveletOctreeConfig config_; const IndexElement cells_per_block_side_ = int_math::exp2(config_.tree_height); BlockHashMap block_map_; - - BlockIndex indexToBlockIndex(const OctreeIndex& node_index) const; - CellIndex indexToCellIndex(OctreeIndex index) const; }; } // namespace wavemap diff --git a/library/cpp/include/wavemap/core/map/hashed_chunked_wavelet_octree_block.h b/library/cpp/include/wavemap/core/map/hashed_chunked_wavelet_octree_block.h index 5f114afbc..1ee7ca16b 100644 --- a/library/cpp/include/wavemap/core/map/hashed_chunked_wavelet_octree_block.h +++ b/library/cpp/include/wavemap/core/map/hashed_chunked_wavelet_octree_block.h @@ -82,9 +82,6 @@ class HashedChunkedWaveletOctreeBlock { size_t getMemoryUsage() const { return chunked_ndtree_.getMemoryUsage(); } private: - static constexpr IndexElement kMaxChunkStackDepth = - kMaxSupportedTreeHeight / kChunkHeight; - const IndexElement tree_height_; const FloatingPoint min_log_odds_; const FloatingPoint max_log_odds_; diff --git a/library/cpp/include/wavemap/core/utils/query/impl/query_accelerator_inl.h b/library/cpp/include/wavemap/core/utils/query/impl/query_accelerator_inl.h index b04dc0c31..d65de1af1 100644 --- a/library/cpp/include/wavemap/core/utils/query/impl/query_accelerator_inl.h +++ b/library/cpp/include/wavemap/core/utils/query/impl/query_accelerator_inl.h @@ -71,7 +71,7 @@ void QueryAccelerator>::reset() { morton_code = std::numeric_limits::max(); block_ = nullptr; - node_stack = std::array>{}; + node_stack.fill({}); } template diff --git a/library/cpp/include/wavemap/core/utils/query/query_accelerator.h b/library/cpp/include/wavemap/core/utils/query/query_accelerator.h index 0e6c76569..50991aef1 100644 --- a/library/cpp/include/wavemap/core/utils/query/query_accelerator.h +++ b/library/cpp/include/wavemap/core/utils/query/query_accelerator.h @@ -7,6 +7,7 @@ #include "wavemap/core/data_structure/ndtree_block_hash.h" #include "wavemap/core/data_structure/spatial_hash.h" #include "wavemap/core/indexing/index_hashes.h" +#include "wavemap/core/map/hashed_chunked_wavelet_octree.h" #include "wavemap/core/map/hashed_wavelet_octree.h" namespace wavemap { @@ -126,7 +127,7 @@ class QueryAccelerator { //! Reset the cache //! @note This method must be called whenever the map changes, not only to //! guarantee correct values (after node value changes) but also to - //! avoid segmentation fault after map topology changes (e.g. after + //! avoid segmentation faults after map topology changes (e.g. after //! pruning). void reset(); @@ -152,7 +153,60 @@ class QueryAccelerator { std::array> value_stack_{}; BlockIndex block_index_ = - BlockIndex ::Constant(std::numeric_limits::max()); + BlockIndex::Constant(std::numeric_limits::max()); + MortonIndex morton_code_ = std::numeric_limits::max(); + IndexElement height_ = tree_height_; +}; + +/** + * A class that accelerates queries by caching block and parent node addresses + * to speed up data structure traversals, and intermediate wavelet decompression + * results to reduce redundant computation. + * @note This class is safe to use in a multi-threaded environment. However, + * concurrent calls to a single instance from multiple threads are not. + * Since the accelerator is lightweight and cheap to construct, we + * recommend using a separate instance per thread for the best performance + * and simplicity. + */ +template <> +class QueryAccelerator { + public: + static constexpr int kDim = HashedChunkedWaveletOctree::kDim; + + explicit QueryAccelerator(const HashedChunkedWaveletOctree& map) + : map_(map) {} + + //! Reset the cache + //! @note This method must be called whenever the map changes, not only to + //! guarantee correct values (after node value changes) but also to + //! avoid segmentation faults after map topology changes (e.g. after + //! pruning). + void reset(); + + //! Query the value of the map at a given index + FloatingPoint getCellValue(const Index3D& index) { + return getCellValue(OctreeIndex{0, index}); + } + + //! Query the value of the map at a given octree node index + FloatingPoint getCellValue(const OctreeIndex& index); + + //! Convenience function to get the map's minimum cell width + FloatingPoint getMinCellWidth() const { return map_.getMinCellWidth(); } + + private: + using BlockIndex = HashedChunkedWaveletOctree::BlockIndex; + using NodePtrType = + HashedChunkedWaveletOctree::Block::ChunkedOctreeType::NodeConstPtrType; + + const HashedChunkedWaveletOctree& map_; + const IndexElement tree_height_ = map_.getTreeHeight(); + + std::array> node_stack_{}; + std::array> value_stack_{}; + + BlockIndex block_index_ = + BlockIndex::Constant(std::numeric_limits::max()); MortonIndex morton_code_ = std::numeric_limits::max(); IndexElement height_ = tree_height_; }; diff --git a/library/cpp/src/core/utils/query/classified_map.cc b/library/cpp/src/core/utils/query/classified_map.cc index 7ebd9e4b3..3d06ba783 100644 --- a/library/cpp/src/core/utils/query/classified_map.cc +++ b/library/cpp/src/core/utils/query/classified_map.cc @@ -330,7 +330,7 @@ void ClassifiedMap::QueryCache::reset() { morton_code = std::numeric_limits::max(); block = nullptr; - node_stack = std::array>{}; + node_stack.fill({}); } void ClassifiedMap::recursiveClassifier( // NOLINT diff --git a/library/cpp/src/core/utils/query/query_accelerator.cc b/library/cpp/src/core/utils/query/query_accelerator.cc index f7e3bb674..c4a530f63 100644 --- a/library/cpp/src/core/utils/query/query_accelerator.cc +++ b/library/cpp/src/core/utils/query/query_accelerator.cc @@ -4,10 +4,10 @@ namespace wavemap { void QueryAccelerator::reset() { - node_stack_ = std::array>{}; - value_stack_ = std::array>{}; + node_stack_.fill({}); + value_stack_.fill({}); - block_index_ = BlockIndex::Constant(std::numeric_limits::max()); + block_index_.setConstant(std::numeric_limits::max()); morton_code_ = std::numeric_limits::max(); height_ = tree_height_; } @@ -85,4 +85,85 @@ FloatingPoint QueryAccelerator::getCellValue( return value_stack_[height_]; } + +void QueryAccelerator::reset() { + node_stack_.fill({}); + value_stack_.fill({}); + + block_index_.setConstant(std::numeric_limits::max()); + morton_code_ = std::numeric_limits::max(); + height_ = tree_height_; +} + +FloatingPoint QueryAccelerator::getCellValue( + const OctreeIndex& index) { + // Remember previous query indices and compute new ones + const BlockIndex previous_block_index = block_index_; + const MortonIndex previous_morton_code = morton_code_; + const IndexElement previous_height = height_; + block_index_ = map_.indexToBlockIndex(index); + morton_code_ = convert::nodeIndexToMorton(index); + + // Check whether we're in the same block as last time + if (block_index_ == previous_block_index) { + // If the block is the same, but it doesn't exist, return 'unknown' + if (!node_stack_[tree_height_]) { + return 0.f; + } + // Compute the last ancestor the current and previous query had in common + auto last_common_ancestor = OctreeIndex::computeLastCommonAncestorHeight( + morton_code_, index.height, previous_morton_code, previous_height); + height_ = last_common_ancestor; + DCHECK_LE(height_, tree_height_); + } else { + // Test if the queried block exists + const auto* current_block = map_.getBlock(block_index_); + if (current_block) { + // If yes, load it + node_stack_[tree_height_] = ¤t_block->getRootNode(); + value_stack_[tree_height_] = current_block->getRootScale(); + height_ = tree_height_; + } else { + // Otherwise remember that it doesn't exist and return 'unknown' + node_stack_[tree_height_].reset(); + value_stack_[tree_height_] = 0.f; + height_ = tree_height_; + return 0.f; + } + } + + // If the requested value was already decompressed in the last query, return + if (height_ == index.height) { + return value_stack_[height_]; + } + + // Load the node at height_ if it was not yet loaded last time + if (previous_height != tree_height_ && height_ == previous_height) { + NodePtrType parent_node = node_stack_[height_ + 1]; + const NdtreeIndexRelativeChild child_index = + OctreeIndex::computeRelativeChildIndex(morton_code_, height_ + 1); + if (!parent_node->hasChild(child_index)) { + return value_stack_[height_]; + } + node_stack_[height_] = parent_node->getChild(child_index); + } + + // Walk down the tree from height_ to index.height + while (true) { + NodePtrType parent_node = node_stack_[height_]; + const FloatingPoint parent_value = value_stack_[height_]; + const NdtreeIndexRelativeChild child_idx = + OctreeIndex::computeRelativeChildIndex(morton_code_, height_); + --height_; + value_stack_[height_] = + HashedWaveletOctree::Block::Transform::backwardSingleChild( + {parent_value, parent_node->data()}, child_idx); + if (height_ == index.height || !parent_node->hasChild(child_idx)) { + break; + } + node_stack_[height_] = parent_node->getChild(child_idx); + } + + return value_stack_[height_]; +} } // namespace wavemap diff --git a/library/cpp/test/src/core/utils/query/test_query_accelerator.cc b/library/cpp/test/src/core/utils/query/test_query_accelerator.cc index 67d91ef61..7225451d4 100644 --- a/library/cpp/test/src/core/utils/query/test_query_accelerator.cc +++ b/library/cpp/test/src/core/utils/query/test_query_accelerator.cc @@ -11,22 +11,30 @@ #include "wavemap/core/utils/query/query_accelerator.h" namespace wavemap { +template class QueryAcceleratorTest : public FixtureBase, public GeometryGenerator, - public ConfigGenerator {}; + public ConfigGenerator { + protected: + static constexpr FloatingPoint kNumericalNoise = 2 * kEpsilon; +}; -TEST_F(QueryAcceleratorTest, Equivalence) { - constexpr int kNumRepetitions = 3; +using MapTypes = + ::testing::Types; +TYPED_TEST_SUITE(QueryAcceleratorTest, MapTypes, ); + +TYPED_TEST(QueryAcceleratorTest, Equivalence) { + constexpr int kNumRepetitions = 10; for (int i = 0; i < kNumRepetitions; ++i) { // Create a random map const auto config = - ConfigGenerator::getRandomConfig(); - HashedWaveletOctree map(config); + ConfigGenerator::getRandomConfig(); + TypeParam map(config); const std::vector random_indices = GeometryGenerator::getRandomIndexVector<3>( - 10000u, 20000u, Index3D::Constant(-5000), Index3D::Constant(5000)); + 1000u, 2000u, Index3D::Constant(-5000), Index3D::Constant(5000)); for (const Index3D& index : random_indices) { - const FloatingPoint update = getRandomUpdate(); + const FloatingPoint update = TestFixture::getRandomUpdate(); map.addToCellValue(index, update); } map.prune(); @@ -37,22 +45,24 @@ TEST_F(QueryAcceleratorTest, Equivalence) { // Test all leaves map.forEachLeaf( [&query_accelerator](const OctreeIndex& index, FloatingPoint value) { - EXPECT_NEAR(query_accelerator.getCellValue(index), value, kEpsilon); + EXPECT_NEAR(query_accelerator.getCellValue(index), value, + TestFixture::kNumericalNoise); }); // Test random indices const IndexElement tree_height = map.getTreeHeight(); - auto random_offsets = getRandomIndexVector<3>(Index3D::Constant(-10), - Index3D::Constant(10), 2, 10); + auto random_offsets = TestFixture::template getRandomIndexVector<3>( + Index3D::Constant(-10), Index3D::Constant(10), 2, 10); random_offsets.emplace_back(Index3D::Zero()); OctreeIndex previous_index{}; for (const Index3D& index : random_indices) { for (const Index3D& offset : random_offsets) { - const IndexElement height = getRandomInteger(0, tree_height); + const IndexElement height = + TestFixture::getRandomInteger(0, tree_height); const auto node_index = OctreeIndex{0, index + offset}.computeParentIndex(height); EXPECT_NEAR(query_accelerator.getCellValue(node_index), - map.getCellValue(node_index), kEpsilon) + map.getCellValue(node_index), TestFixture::kNumericalNoise) << "For node_index " << node_index.toString() << " (in block" << print::eigen::oneLine(map.indexToBlockIndex(node_index)) << ")" << ", previous index " << previous_index.toString() << " (in block" From 4f85fd8b87e95d549a6f77fecae20a83d2a58044 Mon Sep 17 00:00:00 2001 From: Victor Reijgwart Date: Mon, 11 Nov 2024 15:46:41 +0100 Subject: [PATCH 07/22] Correctly handle constness in chunked octree node ptrs/refs The initial implementation did not properly distinguish pointers-to-const (i.e. `const T*`) from const pointers (i.e. `T* const`), resulting in complicated code and incorrect handling of some edge cases. The new version's code is cleaner and shorter, and more closely aligns with standard C++ syntax: * pointer-to-const: `ChunkedNdtreeNodePtr` * const pointer: `const ChunkedNdtreeNodePtr` * const pointer-to-const: `const ChunkedNdtreeNodePtr` --- .../chunked_ndtree_node_address.h | 26 ++++------- .../impl/chunked_ndtree_node_address_inl.h | 44 +++---------------- 2 files changed, 16 insertions(+), 54 deletions(-) diff --git a/library/cpp/include/wavemap/core/data_structure/chunked_ndtree/chunked_ndtree_node_address.h b/library/cpp/include/wavemap/core/data_structure/chunked_ndtree/chunked_ndtree_node_address.h index c059bf372..d5e45ea34 100644 --- a/library/cpp/include/wavemap/core/data_structure/chunked_ndtree/chunked_ndtree_node_address.h +++ b/library/cpp/include/wavemap/core/data_structure/chunked_ndtree/chunked_ndtree_node_address.h @@ -15,7 +15,6 @@ template class ChunkedNdtreeNodePtr { public: using NodeRef = ChunkedNdtreeNodeRef; - using NodeConstRef = ChunkedNdtreeNodeRef; // Constructors ChunkedNdtreeNodePtr() = default; @@ -33,10 +32,9 @@ class ChunkedNdtreeNodePtr { // Emulate pointer semantics void reset() { node_.reset(); } operator bool() const { return node_.has_value(); } // NOLINT - NodeRef operator*() { return node_.value(); } - NodeConstRef operator*() const { return node_.value(); } + NodeRef operator*() const { return node_.value(); } NodeRef* operator->() { return node_.operator->(); } - const NodeConstRef* operator->() const { return node_.operator->(); } + const NodeRef* operator->() const { return node_.operator->(); } private: std::optional node_; @@ -46,9 +44,7 @@ template class ChunkedNdtreeNodeRef { public: using NodeRef = ChunkedNdtreeNodeRef; - using NodeConstRef = ChunkedNdtreeNodeRef; using NodePtr = ChunkedNdtreeNodePtr; - using NodeConstPtr = ChunkedNdtreeNodePtr; static constexpr int kDim = ChunkType::kDim; static constexpr int kNumChildren = NdtreeIndex::kNumChildren; @@ -65,31 +61,27 @@ class ChunkedNdtreeNodeRef { ChunkedNdtreeNodeRef& operator=(const ChunkedNdtreeNodeRef&) = delete; ChunkedNdtreeNodeRef& operator=(ChunkedNdtreeNodeRef&&) = delete; - // Conversion to const ref - operator NodeConstRef(); + // Conversion of non-const ref to const ref + operator ChunkedNdtreeNodeRef() const; // NOLINT // Conversion to pointer - NodePtr operator&(); // NOLINT - NodeConstPtr operator&() const; // NOLINT + NodePtr operator&() const; // NOLINT // Regular node methods bool empty() const; bool hasNonzeroData() const; bool hasNonzeroData(FloatingPoint threshold) const; - auto& data(); - const auto& data() const; + auto& data() const; - typename ChunkType::BitRef hasAtLeastOneChild(); - bool hasAtLeastOneChild() const; + typename ChunkType::BitRef hasAtLeastOneChild() const; bool hasChild(NdtreeIndexRelativeChild child_index) const; - NodePtr getChild(NdtreeIndexRelativeChild child_index); - NodeConstPtr getChild(NdtreeIndexRelativeChild child_index) const; + NodePtr getChild(NdtreeIndexRelativeChild child_index) const; template NodeRef getOrAllocateChild(NdtreeIndexRelativeChild child_index, - DefaultArgs&&... args); + DefaultArgs&&... args) const; private: ChunkType& chunk_; diff --git a/library/cpp/include/wavemap/core/data_structure/chunked_ndtree/impl/chunked_ndtree_node_address_inl.h b/library/cpp/include/wavemap/core/data_structure/chunked_ndtree/impl/chunked_ndtree_node_address_inl.h index 608056d07..a8d2e2679 100644 --- a/library/cpp/include/wavemap/core/data_structure/chunked_ndtree/impl/chunked_ndtree_node_address_inl.h +++ b/library/cpp/include/wavemap/core/data_structure/chunked_ndtree/impl/chunked_ndtree_node_address_inl.h @@ -68,18 +68,13 @@ ChunkedNdtreeNodeRef::ChunkedNdtreeNodeRef( level_traversal_distance_(other.level_traversal_distance_) {} template -ChunkedNdtreeNodeRef::operator NodeConstRef() { +ChunkedNdtreeNodeRef::operator ChunkedNdtreeNodeRef< + const ChunkType>() const { return {chunk_, relative_node_depth_, level_traversal_distance_}; } template typename ChunkedNdtreeNodeRef::NodePtr -ChunkedNdtreeNodeRef::operator&() { // NOLINT - return {&chunk_, relative_node_depth_, level_traversal_distance_}; -} - -template -typename ChunkedNdtreeNodeRef::NodeConstPtr ChunkedNdtreeNodeRef::operator&() const { // NOLINT return {&chunk_, relative_node_depth_, level_traversal_distance_}; } @@ -101,23 +96,13 @@ bool ChunkedNdtreeNodeRef::hasNonzeroData( } template -auto& ChunkedNdtreeNodeRef::data() { +auto& ChunkedNdtreeNodeRef::data() const { return chunk_.nodeData(computeRelativeNodeIndex()); } template -const auto& ChunkedNdtreeNodeRef::data() const { - return chunk_.nodeData(computeRelativeNodeIndex()); -} - -template -typename ChunkType::BitRef -ChunkedNdtreeNodeRef::hasAtLeastOneChild() { - return chunk_.nodeHasAtLeastOneChild(computeRelativeNodeIndex()); -} - -template -bool ChunkedNdtreeNodeRef::hasAtLeastOneChild() const { +typename ChunkType::BitRef ChunkedNdtreeNodeRef::hasAtLeastOneChild() + const { return chunk_.nodeHasAtLeastOneChild(computeRelativeNodeIndex()); } @@ -129,28 +114,13 @@ bool ChunkedNdtreeNodeRef::hasChild( template typename ChunkedNdtreeNodeRef::NodePtr -ChunkedNdtreeNodeRef::getChild( - NdtreeIndexRelativeChild child_index) { - const IndexElement child_depth = relative_node_depth_ + 1; - const LinearIndex child_level_traversal_distance = - computeChildLevelTraversalDistance(child_index); - if (child_depth % ChunkType::kHeight == 0) { - auto* child_chunk = chunk_.getChild(child_level_traversal_distance); - return {child_chunk, 0, 0u}; - } else { - return {&chunk_, child_depth, child_level_traversal_distance}; - } -} - -template -typename ChunkedNdtreeNodeRef::NodeConstPtr ChunkedNdtreeNodeRef::getChild( NdtreeIndexRelativeChild child_index) const { const IndexElement child_depth = relative_node_depth_ + 1; const LinearIndex child_level_traversal_distance = computeChildLevelTraversalDistance(child_index); if (child_depth % ChunkType::kHeight == 0) { - const auto* child_chunk = chunk_.getChild(child_level_traversal_distance); + auto* child_chunk = chunk_.getChild(child_level_traversal_distance); return {child_chunk, 0, 0u}; } else { return {&chunk_, child_depth, child_level_traversal_distance}; @@ -161,7 +131,7 @@ template template ChunkedNdtreeNodeRef ChunkedNdtreeNodeRef::getOrAllocateChild( - NdtreeIndexRelativeChild child_index, DefaultArgs&&... args) { + NdtreeIndexRelativeChild child_index, DefaultArgs&&... args) const { hasAtLeastOneChild() = true; const IndexElement child_depth = relative_node_depth_ + 1; const LinearIndex child_level_traversal_distance = From bd9cfcfde509eaf6f7941004688413fe7cec3f81 Mon Sep 17 00:00:00 2001 From: Victor Reijgwart Date: Mon, 11 Nov 2024 15:47:41 +0100 Subject: [PATCH 08/22] Minor code cleanup --- .../utils/query/impl/query_accelerator_inl.h | 32 +++++++++---------- .../core/utils/query/query_accelerator.h | 2 +- 2 files changed, 17 insertions(+), 17 deletions(-) diff --git a/library/cpp/include/wavemap/core/utils/query/impl/query_accelerator_inl.h b/library/cpp/include/wavemap/core/utils/query/impl/query_accelerator_inl.h index d65de1af1..cc006fb84 100644 --- a/library/cpp/include/wavemap/core/utils/query/impl/query_accelerator_inl.h +++ b/library/cpp/include/wavemap/core/utils/query/impl/query_accelerator_inl.h @@ -71,7 +71,7 @@ void QueryAccelerator>::reset() { morton_code = std::numeric_limits::max(); block_ = nullptr; - node_stack.fill({}); + node_stack_.fill({}); } template @@ -83,7 +83,7 @@ QueryAccelerator>::getBlock( height = tree_height_; block_ = ndtree_block_hash_.getBlock(block_index); if (block_) { - node_stack[tree_height_] = &block_->getRootNode(); + node_stack_[tree_height_] = &block_->getRootNode(); } } return block_; @@ -99,7 +99,7 @@ QueryAccelerator>::getOrAllocateBlock( height = tree_height_; block_ = &ndtree_block_hash_.getOrAllocateBlock( block_index, std::forward(args)...); - node_stack[tree_height_] = &block_->getRootNode(); + node_stack_[tree_height_] = &block_->getRootNode(); } return *block_; } @@ -127,24 +127,24 @@ QueryAccelerator>::getNode( DCHECK_LE(height, tree_height_); if (height == index.height) { - DCHECK_NOTNULL(node_stack[height]); - return node_stack[height]; + DCHECK_NOTNULL(node_stack_[height]); + return node_stack_[height]; } // Walk down the tree from height to index.height for (; index.height < height;) { - DCHECK_NOTNULL(node_stack[height]); + DCHECK_NOTNULL(node_stack_[height]); const NdtreeIndexRelativeChild child_index = OctreeIndex::computeRelativeChildIndex(morton_code, height); // Check if the child is allocated - NodeType* child = node_stack[height]->getChild(child_index); + NodeType* child = node_stack_[height]->getChild(child_index); if (!child) { - return node_stack[height]; + return node_stack_[height]; } - node_stack[--height] = child; + node_stack_[--height] = child; } - return node_stack[height]; + return node_stack_[height]; } template @@ -169,22 +169,22 @@ QueryAccelerator>::getOrAllocateNode( DCHECK_LE(height, tree_height_); if (height == index.height) { - DCHECK_NOTNULL(node_stack[height]); - return *node_stack[height]; + DCHECK_NOTNULL(node_stack_[height]); + return *node_stack_[height]; } // Walk down the tree from height to index.height for (; index.height < height;) { - DCHECK_NOTNULL(node_stack[height]); + DCHECK_NOTNULL(node_stack_[height]); const NdtreeIndexRelativeChild child_index = OctreeIndex::computeRelativeChildIndex(morton_code, height); // Get or allocate the child - auto& child = node_stack[height]->getOrAllocateChild( + auto& child = node_stack_[height]->getOrAllocateChild( child_index, std::forward(args)...); - node_stack[--height] = &child; + node_stack_[--height] = &child; } - return *node_stack[height]; + return *node_stack_[height]; } } // namespace wavemap diff --git a/library/cpp/include/wavemap/core/utils/query/query_accelerator.h b/library/cpp/include/wavemap/core/utils/query/query_accelerator.h index 50991aef1..aa8423de1 100644 --- a/library/cpp/include/wavemap/core/utils/query/query_accelerator.h +++ b/library/cpp/include/wavemap/core/utils/query/query_accelerator.h @@ -104,7 +104,7 @@ class QueryAccelerator> { MortonIndex morton_code = std::numeric_limits::max(); BlockType* block_ = nullptr; - std::array> node_stack{}; + std::array> node_stack_{}; }; /** From 35ec8aee681f448fe8ea4b8a61e94a9ca607b2ac Mon Sep 17 00:00:00 2001 From: Victor Reijgwart Date: Mon, 11 Nov 2024 15:59:09 +0100 Subject: [PATCH 09/22] Add accelerated HashedChunkedWaveletOctree accessors to Python API --- docs/pages/tutorials/python.rst | 4 -- library/python/src/maps.cc | 112 +++++++++++++++++++++++++++++++- 2 files changed, 111 insertions(+), 5 deletions(-) diff --git a/docs/pages/tutorials/python.rst b/docs/pages/tutorials/python.rst index dde65c4e8..42bb83bce 100644 --- a/docs/pages/tutorials/python.rst +++ b/docs/pages/tutorials/python.rst @@ -104,10 +104,6 @@ If you need to look up multiple node values, we recommend using our batched quer .. literalinclude:: ../../../examples/python/queries/accelerated_queries.py :language: python -.. note:: - - So far batched queries are only implemented for HashedWaveletOctree maps. We will add support for HashedChunkedWaveletOctree maps in the near future. - .. _python-code-examples-interpolation: Real coordinates diff --git a/library/python/src/maps.cc b/library/python/src/maps.cc index d1589e75a..6aa29c3bd 100644 --- a/library/python/src/maps.cc +++ b/library/python/src/maps.cc @@ -252,6 +252,68 @@ void add_map_bindings(nb::module_& m) { &HashedChunkedWaveletOctree::getCellValue, nb::const_), "node_index"_a, "Query the value of the map at a given octree node index.") + .def( + "get_cell_values", + [](const HashedChunkedWaveletOctree& self, + const nb::ndarray, nb::device::cpu>& + indices) { + // Create a query accelerator + QueryAccelerator query_accelerator{ + self}; + // Create nb::ndarray view for efficient access to the query indices + const auto index_view = indices.view(); + const auto num_queries = index_view.shape(0); + // Create the raw results array and wrap it in a Python capsule that + // deallocates it when all references to it expire + auto* results = new float[num_queries]; + nb::capsule owner(results, [](void* p) noexcept { + delete[] reinterpret_cast(p); + }); + // Compute the interpolated values + for (size_t query_idx = 0; query_idx < num_queries; ++query_idx) { + results[query_idx] = query_accelerator.getCellValue( + {index_view(query_idx, 0), index_view(query_idx, 1), + index_view(query_idx, 2)}); + } + // Return results as numpy array + return nb::ndarray{ + results, {num_queries, 1u}, owner}; + }, + "index_list"_a, + "Query the map at the given indices, provided as a matrix with one " + "(x, y, z) index per row.") + .def( + "get_cell_values", + [](const HashedChunkedWaveletOctree& self, + const nb::ndarray, nb::device::cpu>& + indices) { + // Create a query accelerator + QueryAccelerator query_accelerator{ + self}; + // Create nb::ndarray view for efficient access to the query indices + auto index_view = indices.view(); + const auto num_queries = index_view.shape(0); + // Create the raw results array and wrap it in a Python capsule that + // deallocates it when all references to it expire + auto* results = new float[num_queries]; + nb::capsule owner(results, [](void* p) noexcept { + delete[] reinterpret_cast(p); + }); + // Compute the interpolated values + for (size_t query_idx = 0; query_idx < num_queries; ++query_idx) { + const OctreeIndex node_index{ + index_view(query_idx, 0), + {index_view(query_idx, 1), index_view(query_idx, 2), + index_view(query_idx, 3)}}; + results[query_idx] = query_accelerator.getCellValue(node_index); + } + // Return results as numpy array + return nb::ndarray{ + results, {num_queries, 1u}, owner}; + }, + "node_index_list"_a, + "Query the map at the given node indices, provided as a matrix with " + "one (height, x, y, z) node index per row.") .def( "interpolate", [](const MapBase& self, const Point3D& position, @@ -267,6 +329,54 @@ void add_map_bindings(nb::module_& m) { }, "position"_a, "mode"_a = InterpolationMode::kTrilinear, "Query the map's value at a point, using the specified interpolation " - "mode."); + "mode.") + .def( + "interpolate", + [](const HashedChunkedWaveletOctree& self, + const nb::ndarray, + nb::device::cpu>& positions, + InterpolationMode mode) { + // Create a query accelerator + QueryAccelerator query_accelerator{ + self}; + // Create nb::ndarray view for efficient access to the query points + const auto positions_view = positions.view(); + const auto num_queries = positions_view.shape(0); + // Create the raw results array and wrap it in a Python capsule that + // deallocates it when all references to it expire + auto* results = new float[num_queries]; + nb::capsule owner(results, [](void* p) noexcept { + delete[] reinterpret_cast(p); + }); + // Compute the interpolated values + switch (mode) { + case InterpolationMode::kNearest: + for (size_t query_idx = 0; query_idx < num_queries; + ++query_idx) { + results[query_idx] = interpolate::nearestNeighbor( + query_accelerator, {positions_view(query_idx, 0), + positions_view(query_idx, 1), + positions_view(query_idx, 2)}); + } + break; + case InterpolationMode::kTrilinear: + for (size_t query_idx = 0; query_idx < num_queries; + ++query_idx) { + results[query_idx] = interpolate::trilinear( + query_accelerator, {positions_view(query_idx, 0), + positions_view(query_idx, 1), + positions_view(query_idx, 2)}); + } + break; + default: + throw nb::type_error("Unknown interpolation mode."); + } + // Return results as numpy array + return nb::ndarray{ + results, {num_queries, 1u}, owner}; + }, + "position_list"_a, "mode"_a = InterpolationMode::kTrilinear, + "Query the map's value at the given points, using the specified " + "interpolation mode."); } } // namespace wavemap From 6eade892739159d615337b3d414c057b3379d40f Mon Sep 17 00:00:00 2001 From: Victor Reijgwart Date: Mon, 11 Nov 2024 17:15:44 +0100 Subject: [PATCH 10/22] Improve consistency between coding using regular and chunked octrees --- .../src/map_msg_conversions.cc | 4 +- .../chunked_ndtree/chunked_ndtree.h | 6 --- .../core/data_structure/ndtree/ndtree.h | 4 ++ .../hashed_chunked_wavelet_integrator.h | 2 +- .../map/hashed_chunked_wavelet_octree_block.h | 33 ++++++------- .../core/map/hashed_wavelet_octree_block.h | 11 +++-- .../hashed_chunked_wavelet_octree_block_inl.h | 4 +- .../impl/hashed_wavelet_octree_block_inl.h | 2 +- .../wavemap/core/utils/query/classified_map.h | 4 +- .../core/utils/query/query_accelerator.h | 6 +-- .../hashed_wavelet_integrator.cc | 10 ++-- .../hashed_chunked_wavelet_octree_block.cc | 47 +++++++++---------- .../core/map/hashed_wavelet_octree_block.cc | 33 +++++++------ .../src/core/utils/query/classified_map.cc | 4 +- .../src/core/utils/query/query_accelerator.cc | 6 +-- library/cpp/src/io/stream_conversions.cc | 4 +- 16 files changed, 88 insertions(+), 92 deletions(-) diff --git a/interfaces/ros1/wavemap_ros_conversions/src/map_msg_conversions.cc b/interfaces/ros1/wavemap_ros_conversions/src/map_msg_conversions.cc index 441aec583..23982aabb 100644 --- a/interfaces/ros1/wavemap_ros_conversions/src/map_msg_conversions.cc +++ b/interfaces/ros1/wavemap_ros_conversions/src/map_msg_conversions.cc @@ -335,7 +335,7 @@ void blockToRosMsg(const HashedWaveletOctree::BlockIndex& block_index, // Convenience type for elements on the stack used to iterate over the map struct StackElement { const FloatingPoint scale; - const HashedWaveletOctreeBlock::NodeType& node; + HashedWaveletOctreeBlock::OctreeType::NodeConstRefType node; }; // Serialize the block's metadata @@ -534,7 +534,7 @@ void blockToRosMsg(const HashedChunkedWaveletOctree::BlockIndex& block_index, // Define convenience types and constants struct StackElement { const FloatingPoint scale; - HashedChunkedWaveletOctreeBlock::ChunkedOctreeType::NodeConstRefType node; + HashedChunkedWaveletOctreeBlock::OctreeType::NodeConstRefType node; }; // Serialize the block's metadata diff --git a/library/cpp/include/wavemap/core/data_structure/chunked_ndtree/chunked_ndtree.h b/library/cpp/include/wavemap/core/data_structure/chunked_ndtree/chunked_ndtree.h index 0b530aaa7..6f0050f9b 100644 --- a/library/cpp/include/wavemap/core/data_structure/chunked_ndtree/chunked_ndtree.h +++ b/library/cpp/include/wavemap/core/data_structure/chunked_ndtree/chunked_ndtree.h @@ -36,12 +36,6 @@ class ChunkedNdtree { HeightType getMaxHeight() const { return max_height_; } size_t getMemoryUsage() const; - // TODO(victorr): Add methods to directly query and operate on chunks, - // once a proper index type for chunks has been defined. - // The ChunkIndex type would be similar to NdtreeIndex, but has - // to account for the chunks having a branching factor that - // differs from 2 (probably 2^(dim * chunk_height)). - bool hasNode(const IndexType& index) const { return getNode(index); } NodePtrType getNode(const IndexType& index); NodeConstPtrType getNode(const IndexType& index) const; diff --git a/library/cpp/include/wavemap/core/data_structure/ndtree/ndtree.h b/library/cpp/include/wavemap/core/data_structure/ndtree/ndtree.h index 3d6ddc7c9..a0bc77df8 100644 --- a/library/cpp/include/wavemap/core/data_structure/ndtree/ndtree.h +++ b/library/cpp/include/wavemap/core/data_structure/ndtree/ndtree.h @@ -17,6 +17,10 @@ class Ndtree { using IndexType = NdtreeIndex; using HeightType = IndexElement; using NodeType = NdtreeNode; + using NodeRefType = NodeType&; + using NodeConstRefType = const NodeType&; + using NodePtrType = NodeType*; + using NodeConstPtrType = const NodeType*; using NodeDataType = NodeDataT; static constexpr HeightType kChunkHeight = 1; diff --git a/library/cpp/include/wavemap/core/integrator/projective/coarse_to_fine/hashed_chunked_wavelet_integrator.h b/library/cpp/include/wavemap/core/integrator/projective/coarse_to_fine/hashed_chunked_wavelet_integrator.h index f105a88b7..2678ec6a3 100644 --- a/library/cpp/include/wavemap/core/integrator/projective/coarse_to_fine/hashed_chunked_wavelet_integrator.h +++ b/library/cpp/include/wavemap/core/integrator/projective/coarse_to_fine/hashed_chunked_wavelet_integrator.h @@ -32,7 +32,7 @@ class HashedChunkedWaveletIntegrator : public ProjectiveIntegrator { private: using BlockList = std::vector; - using OctreeType = HashedChunkedWaveletOctreeBlock::ChunkedOctreeType; + using OctreeType = HashedChunkedWaveletOctreeBlock::OctreeType; const HashedChunkedWaveletOctree::Ptr occupancy_map_; std::shared_ptr thread_pool_; diff --git a/library/cpp/include/wavemap/core/map/hashed_chunked_wavelet_octree_block.h b/library/cpp/include/wavemap/core/map/hashed_chunked_wavelet_octree_block.h index 1ee7ca16b..f7e8c5883 100644 --- a/library/cpp/include/wavemap/core/map/hashed_chunked_wavelet_octree_block.h +++ b/library/cpp/include/wavemap/core/map/hashed_chunked_wavelet_octree_block.h @@ -17,7 +17,7 @@ class HashedChunkedWaveletOctreeBlock { using BlockIndex = Index3D; using Coefficients = HaarCoefficients; using Transform = HaarTransform; - using ChunkedOctreeType = ChunkedOctree; + using OctreeType = ChunkedOctree; explicit HashedChunkedWaveletOctreeBlock(IndexElement tree_height, FloatingPoint min_log_odds, @@ -27,7 +27,7 @@ class HashedChunkedWaveletOctreeBlock { max_log_odds_(max_log_odds) {} bool empty() const; - size_t size() const { return chunked_ndtree_.size(); } + size_t size() const { return ndtree_.size(); } void threshold(); void prune(); void clear(); @@ -44,17 +44,13 @@ class HashedChunkedWaveletOctreeBlock { const Coefficients::Scale& getRootScale() const { return root_scale_coefficient_; } - ChunkedOctreeType::NodeRefType getRootNode() { - return chunked_ndtree_.getRootNode(); + OctreeType::NodeRefType getRootNode() { return ndtree_.getRootNode(); } + OctreeType::NodeConstRefType getRootNode() const { + return ndtree_.getRootNode(); } - ChunkedOctreeType::NodeConstRefType getRootNode() const { - return chunked_ndtree_.getRootNode(); - } - ChunkedOctreeType::ChunkType& getRootChunk() { - return chunked_ndtree_.getRootChunk(); - } - const ChunkedOctreeType::ChunkType& getRootChunk() const { - return chunked_ndtree_.getRootChunk(); + OctreeType::ChunkType& getRootChunk() { return ndtree_.getRootChunk(); } + const OctreeType::ChunkType& getRootChunk() const { + return ndtree_.getRootChunk(); } void setNeedsPruning(bool value = true) { needs_pruning_ = value; } @@ -72,21 +68,21 @@ class HashedChunkedWaveletOctreeBlock { template auto getChunkIterator() { - return chunked_ndtree_.getChunkIterator(); + return ndtree_.getChunkIterator(); } template auto getChunkIterator() const { - return chunked_ndtree_.getChunkIterator(); + return ndtree_.getChunkIterator(); } - size_t getMemoryUsage() const { return chunked_ndtree_.getMemoryUsage(); } + size_t getMemoryUsage() const { return ndtree_.getMemoryUsage(); } private: const IndexElement tree_height_; const FloatingPoint min_log_odds_; const FloatingPoint max_log_odds_; - ChunkedOctreeType chunked_ndtree_{tree_height_ - 1}; + OctreeType ndtree_{tree_height_ - 1}; Coefficients::Scale root_scale_coefficient_{}; bool needs_thresholding_ = false; @@ -98,9 +94,8 @@ class HashedChunkedWaveletOctreeBlock { bool is_nonzero_child; }; RecursiveThresholdReturnValue recursiveThreshold( - ChunkedOctreeType::ChunkType& chunk, - Coefficients::Scale scale_coefficient); - void recursivePrune(ChunkedOctreeType::ChunkType& chunk); + OctreeType::ChunkType& chunk, Coefficients::Scale scale_coefficient); + void recursivePrune(OctreeType::ChunkType& chunk); }; } // namespace wavemap diff --git a/library/cpp/include/wavemap/core/map/hashed_wavelet_octree_block.h b/library/cpp/include/wavemap/core/map/hashed_wavelet_octree_block.h index 3be70a41e..b27467fa2 100644 --- a/library/cpp/include/wavemap/core/map/hashed_wavelet_octree_block.h +++ b/library/cpp/include/wavemap/core/map/hashed_wavelet_octree_block.h @@ -16,7 +16,6 @@ class HashedWaveletOctreeBlock { using Coefficients = HaarCoefficients; using Transform = HaarTransform; using OctreeType = Octree; - using NodeType = OctreeType::NodeType; explicit HashedWaveletOctreeBlock(IndexElement tree_height, FloatingPoint min_log_odds, @@ -43,8 +42,10 @@ class HashedWaveletOctreeBlock { const Coefficients::Scale& getRootScale() const { return root_scale_coefficient_; } - NodeType& getRootNode() { return ndtree_.getRootNode(); } - const NodeType& getRootNode() const { return ndtree_.getRootNode(); } + OctreeType::NodeRefType getRootNode() { return ndtree_.getRootNode(); } + OctreeType::NodeConstRefType getRootNode() const { + return ndtree_.getRootNode(); + } void setNeedsPruning(bool value = true) { needs_pruning_ = value; } bool getNeedsPruning() const { return needs_pruning_; } @@ -79,9 +80,9 @@ class HashedWaveletOctreeBlock { bool needs_pruning_ = false; Timestamp last_updated_stamp_ = Time::now(); - Coefficients::Scale recursiveThreshold(NodeType& node, + Coefficients::Scale recursiveThreshold(OctreeType::NodeRefType node, Coefficients::Scale scale_coefficient); - void recursivePrune(NodeType& node); + void recursivePrune(OctreeType::NodeRefType node); }; } // namespace wavemap diff --git a/library/cpp/include/wavemap/core/map/impl/hashed_chunked_wavelet_octree_block_inl.h b/library/cpp/include/wavemap/core/map/impl/hashed_chunked_wavelet_octree_block_inl.h index 3eeb42931..9c55969c5 100644 --- a/library/cpp/include/wavemap/core/map/impl/hashed_chunked_wavelet_octree_block_inl.h +++ b/library/cpp/include/wavemap/core/map/impl/hashed_chunked_wavelet_octree_block_inl.h @@ -9,7 +9,7 @@ inline bool HashedChunkedWaveletOctreeBlock::empty() const { // NOTE: Aside from checking whether the block contains no detail // coefficients, we also need to check whether its scale coefficient // (average value over the whole block) is zero. - return chunked_ndtree_.empty() && + return ndtree_.empty() && OccupancyClassifier::isUnobserved(root_scale_coefficient_); } @@ -21,7 +21,7 @@ inline FloatingPoint HashedChunkedWaveletOctreeBlock::getTimeSinceLastUpdated() inline FloatingPoint HashedChunkedWaveletOctreeBlock::getCellValue( const OctreeIndex& index) const { const MortonIndex morton_code = convert::nodeIndexToMorton(index); - ChunkedOctreeType::NodeConstPtrType node = &chunked_ndtree_.getRootNode(); + OctreeType::NodeConstPtrType node = &ndtree_.getRootNode(); FloatingPoint value = root_scale_coefficient_; for (int parent_height = tree_height_; index.height < parent_height; --parent_height) { diff --git a/library/cpp/include/wavemap/core/map/impl/hashed_wavelet_octree_block_inl.h b/library/cpp/include/wavemap/core/map/impl/hashed_wavelet_octree_block_inl.h index 2319ec385..37d8389d6 100644 --- a/library/cpp/include/wavemap/core/map/impl/hashed_wavelet_octree_block_inl.h +++ b/library/cpp/include/wavemap/core/map/impl/hashed_wavelet_octree_block_inl.h @@ -20,7 +20,7 @@ inline FloatingPoint HashedWaveletOctreeBlock::getTimeSinceLastUpdated() const { inline FloatingPoint HashedWaveletOctreeBlock::getCellValue( const OctreeIndex& index) const { const MortonIndex morton_code = convert::nodeIndexToMorton(index); - const NodeType* node = &ndtree_.getRootNode(); + OctreeType::NodeConstPtrType node = &ndtree_.getRootNode(); FloatingPoint value = root_scale_coefficient_; for (int parent_height = tree_height_; index.height < parent_height; --parent_height) { diff --git a/library/cpp/include/wavemap/core/utils/query/classified_map.h b/library/cpp/include/wavemap/core/utils/query/classified_map.h index 28fd7c079..a5d5153b7 100644 --- a/library/cpp/include/wavemap/core/utils/query/classified_map.h +++ b/library/cpp/include/wavemap/core/utils/query/classified_map.h @@ -148,12 +148,12 @@ class ClassifiedMap { mutable QueryCache query_cache_{tree_height_}; void recursiveClassifier( - const HashedWaveletOctreeBlock::NodeType& occupancy_node, + HashedWaveletOctreeBlock::OctreeType::NodeConstRefType occupancy_node, FloatingPoint average_occupancy, Node& classified_node); void recursiveClassifier( const OctreeIndex& node_index, - const HashedWaveletOctreeBlock::NodeType* occupancy_node, + HashedWaveletOctreeBlock::OctreeType::NodeConstPtrType occupancy_node, FloatingPoint occupancy_average, QueryAccelerator& esdf_map, FloatingPoint robot_radius, Node& classified_node); diff --git a/library/cpp/include/wavemap/core/utils/query/query_accelerator.h b/library/cpp/include/wavemap/core/utils/query/query_accelerator.h index aa8423de1..238d47bcc 100644 --- a/library/cpp/include/wavemap/core/utils/query/query_accelerator.h +++ b/library/cpp/include/wavemap/core/utils/query/query_accelerator.h @@ -144,12 +144,12 @@ class QueryAccelerator { private: using BlockIndex = HashedWaveletOctree::BlockIndex; - using NodeType = HashedWaveletOctree::Block::NodeType; + using NodePtrType = HashedWaveletOctree::Block::OctreeType::NodeConstPtrType; const HashedWaveletOctree& map_; const IndexElement tree_height_ = map_.getTreeHeight(); - std::array> node_stack_{}; + std::array> node_stack_{}; std::array> value_stack_{}; BlockIndex block_index_ = @@ -197,7 +197,7 @@ class QueryAccelerator { private: using BlockIndex = HashedChunkedWaveletOctree::BlockIndex; using NodePtrType = - HashedChunkedWaveletOctree::Block::ChunkedOctreeType::NodeConstPtrType; + HashedChunkedWaveletOctree::Block::OctreeType::NodeConstPtrType; const HashedChunkedWaveletOctree& map_; const IndexElement tree_height_ = map_.getTreeHeight(); diff --git a/library/cpp/src/core/integrator/projective/coarse_to_fine/hashed_wavelet_integrator.cc b/library/cpp/src/core/integrator/projective/coarse_to_fine/hashed_wavelet_integrator.cc index 9168b35cd..2297e33a2 100644 --- a/library/cpp/src/core/integrator/projective/coarse_to_fine/hashed_wavelet_integrator.cc +++ b/library/cpp/src/core/integrator/projective/coarse_to_fine/hashed_wavelet_integrator.cc @@ -72,14 +72,15 @@ HashedWaveletIntegrator::getFovMinMaxIndices( void HashedWaveletIntegrator::updateBlock(HashedWaveletOctree::Block& block, const OctreeIndex& block_index) { ProfilerZoneScoped; - HashedWaveletOctreeBlock::NodeType& root_node = block.getRootNode(); + HashedWaveletOctreeBlock::OctreeType::NodeRefType root_node = + block.getRootNode(); HashedWaveletOctreeBlock::Coefficients::Scale& root_node_scale = block.getRootScale(); block.setNeedsPruning(); block.setLastUpdatedStamp(); struct StackElement { - HashedWaveletOctreeBlock::NodeType& parent_node; + HashedWaveletOctreeBlock::OctreeType::NodeRefType parent_node; const OctreeIndex parent_node_index; NdtreeIndexRelativeChild next_child_idx; HashedWaveletOctreeBlock::Coefficients::CoefficientsArray @@ -116,7 +117,8 @@ void HashedWaveletIntegrator::updateBlock(HashedWaveletOctree::Block& block, DCHECK_GE(current_child_idx, 0); DCHECK_LT(current_child_idx, OctreeIndex::kNumChildren); - HashedWaveletOctreeBlock::NodeType& parent_node = stack.top().parent_node; + HashedWaveletOctreeBlock::OctreeType::NodeRefType parent_node = + stack.top().parent_node; FloatingPoint& node_value = stack.top().child_scale_coefficients[current_child_idx]; const OctreeIndex node_index = @@ -169,7 +171,7 @@ void HashedWaveletIntegrator::updateBlock(HashedWaveletOctree::Block& block, projection_model_->cartesianToSensorZ(C_node_center); const FloatingPoint bounding_sphere_radius = kUnitCubeHalfDiagonal * node_width; - HashedWaveletOctreeBlock::NodeType* node = + HashedWaveletOctreeBlock::OctreeType::NodePtrType node = parent_node.getChild(node_index.computeRelativeChildIndex()); if (measurement_model_->computeWorstCaseApproximationError( update_type, d_C_cell, bounding_sphere_radius) < diff --git a/library/cpp/src/core/map/hashed_chunked_wavelet_octree_block.cc b/library/cpp/src/core/map/hashed_chunked_wavelet_octree_block.cc index ff36d967c..b531c49b1 100644 --- a/library/cpp/src/core/map/hashed_chunked_wavelet_octree_block.cc +++ b/library/cpp/src/core/map/hashed_chunked_wavelet_octree_block.cc @@ -10,9 +10,9 @@ namespace wavemap { void HashedChunkedWaveletOctreeBlock::threshold() { ProfilerZoneScoped; if (getNeedsThresholding()) { - root_scale_coefficient_ = recursiveThreshold(chunked_ndtree_.getRootChunk(), - root_scale_coefficient_) - .scale; + root_scale_coefficient_ = + recursiveThreshold(ndtree_.getRootChunk(), root_scale_coefficient_) + .scale; setNeedsThresholding(false); } } @@ -21,7 +21,7 @@ void HashedChunkedWaveletOctreeBlock::prune() { ProfilerZoneScoped; if (getNeedsPruning()) { threshold(); - recursivePrune(chunked_ndtree_.getRootChunk()); + recursivePrune(ndtree_.getRootChunk()); setNeedsPruning(false); } } @@ -29,7 +29,7 @@ void HashedChunkedWaveletOctreeBlock::prune() { void HashedChunkedWaveletOctreeBlock::clear() { ProfilerZoneScoped; root_scale_coefficient_ = Coefficients::Scale{}; - chunked_ndtree_.clear(); + ndtree_.clear(); setLastUpdatedStamp(); } @@ -39,19 +39,19 @@ void HashedChunkedWaveletOctreeBlock::setCellValue(const OctreeIndex& index, setNeedsThresholding(); setLastUpdatedStamp(); const MortonIndex morton_code = convert::nodeIndexToMorton(index); - std::vector ancestors; + std::vector ancestors; const int height_difference = tree_height_ - index.height; ancestors.reserve(height_difference); - ancestors.emplace_back(chunked_ndtree_.getRootNode()); + ancestors.emplace_back(ndtree_.getRootNode()); FloatingPoint current_value = root_scale_coefficient_; for (int parent_height = tree_height_; index.height + 1 < parent_height; --parent_height) { const NdtreeIndexRelativeChild child_index = OctreeIndex::computeRelativeChildIndex(morton_code, parent_height); - ChunkedOctreeType::NodeRefType current_parent = ancestors.back(); + OctreeType::NodeRefType current_parent = ancestors.back(); current_value = Transform::backwardSingleChild( {current_value, current_parent.data()}, child_index); - ChunkedOctreeType::NodeRefType child = + OctreeType::NodeRefType child = current_parent.getOrAllocateChild(child_index); ancestors.emplace_back(child); } @@ -62,7 +62,7 @@ void HashedChunkedWaveletOctreeBlock::setCellValue(const OctreeIndex& index, ++parent_height) { const NdtreeIndexRelativeChild child_index = OctreeIndex::computeRelativeChildIndex(morton_code, parent_height); - ChunkedOctreeType::NodeRefType current_node = ancestors.back(); + OctreeType::NodeRefType current_node = ancestors.back(); ancestors.pop_back(); coefficients = Transform::forwardSingleChild(coefficients.scale, child_index); @@ -79,16 +79,16 @@ void HashedChunkedWaveletOctreeBlock::addToCellValue(const OctreeIndex& index, setLastUpdatedStamp(); const MortonIndex morton_code = convert::nodeIndexToMorton(index); - std::vector ancestors; + std::vector ancestors; const int height_difference = tree_height_ - index.height; ancestors.reserve(height_difference); - ancestors.emplace_back(chunked_ndtree_.getRootNode()); + ancestors.emplace_back(ndtree_.getRootNode()); for (int parent_height = tree_height_; index.height + 1 < parent_height; --parent_height) { const NdtreeIndexRelativeChild child_index = OctreeIndex::computeRelativeChildIndex(morton_code, parent_height); - ChunkedOctreeType::NodeRefType current_parent = ancestors.back(); - ChunkedOctreeType::NodeRefType child = + OctreeType::NodeRefType current_parent = ancestors.back(); + OctreeType::NodeRefType child = current_parent.getOrAllocateChild(child_index); ancestors.emplace_back(child); } @@ -97,7 +97,7 @@ void HashedChunkedWaveletOctreeBlock::addToCellValue(const OctreeIndex& index, Coefficients::Parent coefficients{update, {}}; for (int parent_height = index.height + 1; parent_height <= tree_height_; ++parent_height) { - ChunkedOctreeType::NodeRefType current_node = ancestors.back(); + OctreeType::NodeRefType current_node = ancestors.back(); ancestors.pop_back(); const NdtreeIndexRelativeChild child_index = OctreeIndex::computeRelativeChildIndex(morton_code, parent_height); @@ -119,12 +119,12 @@ void HashedChunkedWaveletOctreeBlock::forEachLeaf( struct StackElement { const OctreeIndex node_index; - ChunkedOctreeType::NodeConstRefType node; + OctreeType::NodeConstRefType node; const Coefficients::Scale scale_coefficient{}; }; std::stack stack; stack.emplace(StackElement{{tree_height_, block_index}, - chunked_ndtree_.getRootNode(), + ndtree_.getRootNode(), root_scale_coefficient_}); while (!stack.empty()) { const OctreeIndex index = stack.top().node_index; @@ -152,7 +152,7 @@ void HashedChunkedWaveletOctreeBlock::forEachLeaf( HashedChunkedWaveletOctreeBlock::RecursiveThresholdReturnValue HashedChunkedWaveletOctreeBlock::recursiveThreshold( // NOLINT - HashedChunkedWaveletOctreeBlock::ChunkedOctreeType::ChunkType& chunk, + HashedChunkedWaveletOctreeBlock::OctreeType::ChunkType& chunk, FloatingPoint scale_coefficient) { constexpr auto tree_size = [](auto tree_height) { return static_cast( @@ -187,10 +187,10 @@ HashedChunkedWaveletOctreeBlock::recursiveThreshold( // NOLINT // Threshold const int first_leaf_idx = tree_size(kChunkHeight); for (LinearIndex child_idx = 0; - child_idx < ChunkedOctreeType::ChunkType::kNumChildren; ++child_idx) { + child_idx < OctreeType::ChunkType::kNumChildren; ++child_idx) { const LinearIndex array_idx = first_leaf_idx + child_idx; if (chunk.hasChild(child_idx)) { - ChunkedOctreeType::ChunkType& child_chunk = *chunk.getChild(child_idx); + OctreeType::ChunkType& child_chunk = *chunk.getChild(child_idx); const auto rv = recursiveThreshold(child_chunk, chunk_scale_coefficients[array_idx]); chunk_scale_coefficients[array_idx] = rv.scale; @@ -233,15 +233,14 @@ HashedChunkedWaveletOctreeBlock::recursiveThreshold( // NOLINT } void HashedChunkedWaveletOctreeBlock::recursivePrune( // NOLINT - HashedChunkedWaveletOctreeBlock::ChunkedOctreeType::ChunkType& chunk) { + HashedChunkedWaveletOctreeBlock::OctreeType::ChunkType& chunk) { constexpr FloatingPoint kNonzeroCoefficientThreshold = 1e-3f; bool has_at_least_one_child = false; for (LinearIndex linear_child_idx = 0; - linear_child_idx < ChunkedOctreeType::ChunkType::kNumChildren; + linear_child_idx < OctreeType::ChunkType::kNumChildren; ++linear_child_idx) { if (chunk.hasChild(linear_child_idx)) { - ChunkedOctreeType::ChunkType& child_chunk = - *chunk.getChild(linear_child_idx); + OctreeType::ChunkType& child_chunk = *chunk.getChild(linear_child_idx); recursivePrune(child_chunk); if (!child_chunk.hasChildrenArray() && !child_chunk.hasNonzeroData(kNonzeroCoefficientThreshold)) { diff --git a/library/cpp/src/core/map/hashed_wavelet_octree_block.cc b/library/cpp/src/core/map/hashed_wavelet_octree_block.cc index e37344ab8..4d2f6c395 100644 --- a/library/cpp/src/core/map/hashed_wavelet_octree_block.cc +++ b/library/cpp/src/core/map/hashed_wavelet_octree_block.cc @@ -37,7 +37,7 @@ void HashedWaveletOctreeBlock::setCellValue(const OctreeIndex& index, setNeedsThresholding(); setLastUpdatedStamp(); const MortonIndex morton_code = convert::nodeIndexToMorton(index); - std::vector ancestors; + std::vector ancestors; const int height_difference = tree_height_ - index.height; ancestors.reserve(height_difference); ancestors.emplace_back(&ndtree_.getRootNode()); @@ -46,10 +46,11 @@ void HashedWaveletOctreeBlock::setCellValue(const OctreeIndex& index, --parent_height) { const NdtreeIndexRelativeChild child_index = OctreeIndex::computeRelativeChildIndex(morton_code, parent_height); - NodeType* current_parent = ancestors.back(); + OctreeType::NodePtrType current_parent = ancestors.back(); current_value = Transform::backwardSingleChild( {current_value, current_parent->data()}, child_index); - NodeType& child = current_parent->getOrAllocateChild(child_index); + OctreeType::NodeRefType child = + current_parent->getOrAllocateChild(child_index); ancestors.emplace_back(&child); } DCHECK_EQ(ancestors.size(), height_difference); @@ -59,7 +60,7 @@ void HashedWaveletOctreeBlock::setCellValue(const OctreeIndex& index, ++parent_height) { const NdtreeIndexRelativeChild child_index = OctreeIndex::computeRelativeChildIndex(morton_code, parent_height); - NodeType* current_node = ancestors.back(); + OctreeType::NodePtrType current_node = ancestors.back(); ancestors.pop_back(); coefficients = Transform::forwardSingleChild(coefficients.scale, child_index); @@ -76,7 +77,7 @@ void HashedWaveletOctreeBlock::addToCellValue(const OctreeIndex& index, setLastUpdatedStamp(); const MortonIndex morton_code = convert::nodeIndexToMorton(index); - std::vector ancestors; + std::vector ancestors; const int height_difference = tree_height_ - index.height; ancestors.reserve(height_difference); ancestors.emplace_back(&ndtree_.getRootNode()); @@ -84,8 +85,9 @@ void HashedWaveletOctreeBlock::addToCellValue(const OctreeIndex& index, --parent_height) { const NdtreeIndexRelativeChild child_index = OctreeIndex::computeRelativeChildIndex(morton_code, parent_height); - NodeType* current_parent = ancestors.back(); - NodeType& child = current_parent->getOrAllocateChild(child_index); + OctreeType::NodePtrType current_parent = ancestors.back(); + OctreeType::NodeRefType child = + current_parent->getOrAllocateChild(child_index); ancestors.emplace_back(&child); } DCHECK_EQ(ancestors.size(), height_difference); @@ -93,7 +95,7 @@ void HashedWaveletOctreeBlock::addToCellValue(const OctreeIndex& index, Coefficients::Parent coefficients{update, {}}; for (int parent_height = index.height + 1; parent_height <= tree_height_; ++parent_height) { - NodeType* current_node = ancestors.back(); + OctreeType::NodePtrType current_node = ancestors.back(); ancestors.pop_back(); const NdtreeIndexRelativeChild child_index = OctreeIndex::computeRelativeChildIndex(morton_code, parent_height); @@ -115,7 +117,7 @@ void HashedWaveletOctreeBlock::forEachLeaf( struct StackElement { const OctreeIndex node_index; - const NodeType& node; + OctreeType::NodeConstRefType node; const Coefficients::Scale scale_coefficient{}; }; std::stack stack; @@ -123,7 +125,7 @@ void HashedWaveletOctreeBlock::forEachLeaf( ndtree_.getRootNode(), root_scale_coefficient_}); while (!stack.empty()) { const OctreeIndex node_index = stack.top().node_index; - const NodeType& node = stack.top().node; + OctreeType::NodeConstRefType node = stack.top().node; const FloatingPoint node_scale_coefficient = stack.top().scale_coefficient; stack.pop(); @@ -137,7 +139,7 @@ void HashedWaveletOctreeBlock::forEachLeaf( child_scale_coefficients[child_idx]; if (node.hasChild(child_idx) && termination_height < child_node_index.height) { - const NodeType& child_node = *node.getChild(child_idx); + OctreeType::NodeConstRefType child_node = *node.getChild(child_idx); stack.emplace(StackElement{child_node_index, child_node, child_scale_coefficient}); } else { @@ -149,14 +151,15 @@ void HashedWaveletOctreeBlock::forEachLeaf( HashedWaveletOctreeBlock::Coefficients::Scale HashedWaveletOctreeBlock::recursiveThreshold( // NOLINT - HashedWaveletOctreeBlock::NodeType& node, FloatingPoint scale_coefficient) { + HashedWaveletOctreeBlock::OctreeType::NodeRefType node, + FloatingPoint scale_coefficient) { Coefficients::CoefficientsArray child_scale_coefficients = Transform::backward({scale_coefficient, node.data()}); for (NdtreeIndexRelativeChild child_idx = 0; child_idx < OctreeIndex::kNumChildren; ++child_idx) { if (node.hasChild(child_idx)) { - NodeType& child_node = *node.getChild(child_idx); + OctreeType::NodeRefType child_node = *node.getChild(child_idx); child_scale_coefficients[child_idx] = recursiveThreshold(child_node, child_scale_coefficients[child_idx]); } else { @@ -173,12 +176,12 @@ HashedWaveletOctreeBlock::recursiveThreshold( // NOLINT } void HashedWaveletOctreeBlock::recursivePrune( // NOLINT - HashedWaveletOctreeBlock::NodeType& node) { + HashedWaveletOctreeBlock::OctreeType::NodeRefType node) { bool has_at_least_one_child = false; for (NdtreeIndexRelativeChild child_idx = 0; child_idx < OctreeIndex::kNumChildren; ++child_idx) { if (node.hasChild(child_idx)) { - NodeType& child_node = *node.getChild(child_idx); + OctreeType::NodeRefType child_node = *node.getChild(child_idx); recursivePrune(child_node); if (!child_node.hasChildrenArray() && !child_node.hasNonzeroData(1e-3f)) { node.eraseChild(child_idx); diff --git a/library/cpp/src/core/utils/query/classified_map.cc b/library/cpp/src/core/utils/query/classified_map.cc index 3d06ba783..75f6f2d3c 100644 --- a/library/cpp/src/core/utils/query/classified_map.cc +++ b/library/cpp/src/core/utils/query/classified_map.cc @@ -334,7 +334,7 @@ void ClassifiedMap::QueryCache::reset() { } void ClassifiedMap::recursiveClassifier( // NOLINT - const HashedWaveletOctreeBlock::NodeType& occupancy_node, + HashedWaveletOctreeBlock::OctreeType::NodeConstRefType occupancy_node, FloatingPoint average_occupancy, ClassifiedMap::Node& classified_node) { const auto child_occupancies = HaarTransform::backward({average_occupancy, occupancy_node.data()}); @@ -374,7 +374,7 @@ void ClassifiedMap::recursiveClassifier( // NOLINT void ClassifiedMap::recursiveClassifier( // NOLINT const OctreeIndex& node_index, - const HashedWaveletOctreeBlock::NodeType* occupancy_node, + HashedWaveletOctreeBlock::OctreeType::NodeConstPtrType occupancy_node, FloatingPoint occupancy_average, QueryAccelerator& esdf_map, FloatingPoint robot_radius, ClassifiedMap::Node& classified_node) { diff --git a/library/cpp/src/core/utils/query/query_accelerator.cc b/library/cpp/src/core/utils/query/query_accelerator.cc index c4a530f63..c5de07565 100644 --- a/library/cpp/src/core/utils/query/query_accelerator.cc +++ b/library/cpp/src/core/utils/query/query_accelerator.cc @@ -56,8 +56,7 @@ FloatingPoint QueryAccelerator::getCellValue( // Load the node at height_ if it was not yet loaded last time if (previous_height != tree_height_ && height_ == previous_height) { - const HashedWaveletOctree::Block::NodeType* parent_node = - node_stack_[height_ + 1]; + NodePtrType parent_node = node_stack_[height_ + 1]; const NdtreeIndexRelativeChild child_index = OctreeIndex::computeRelativeChildIndex(morton_code_, height_ + 1); if (!parent_node->hasChild(child_index)) { @@ -68,8 +67,7 @@ FloatingPoint QueryAccelerator::getCellValue( // Walk down the tree from height_ to index.height while (true) { - const HashedWaveletOctree::Block::NodeType* parent_node = - node_stack_[height_]; + NodePtrType parent_node = node_stack_[height_]; const FloatingPoint parent_value = value_stack_[height_]; const NdtreeIndexRelativeChild child_idx = OctreeIndex::computeRelativeChildIndex(morton_code_, height_); diff --git a/library/cpp/src/io/stream_conversions.cc b/library/cpp/src/io/stream_conversions.cc index d196902b0..364260678 100644 --- a/library/cpp/src/io/stream_conversions.cc +++ b/library/cpp/src/io/stream_conversions.cc @@ -275,7 +275,7 @@ bool mapToStream(const HashedWaveletOctree& map, std::ostream& ostream) { // Define convenience types and constants struct StackElement { const FloatingPoint scale; - const HashedWaveletOctreeBlock::NodeType& node; + HashedWaveletOctreeBlock::OctreeType::NodeConstRefType node; }; constexpr FloatingPoint kNumericalNoise = 1e-3f; const auto min_log_odds = map.getMinLogOdds() + kNumericalNoise; @@ -432,7 +432,7 @@ bool mapToStream(const HashedChunkedWaveletOctree& map, std::ostream& ostream) { // Define convenience types and constants struct StackElement { const FloatingPoint scale; - HashedChunkedWaveletOctreeBlock::ChunkedOctreeType::NodeConstRefType node; + HashedChunkedWaveletOctreeBlock::OctreeType::NodeConstRefType node; }; constexpr FloatingPoint kNumericalNoise = 1e-3f; const auto min_log_odds = map.getMinLogOdds() + kNumericalNoise; From d6e7f74cb2efd8e381e1dd3a245be5633887731e Mon Sep 17 00:00:00 2001 From: Victor Reijgwart Date: Tue, 12 Nov 2024 12:44:44 +0100 Subject: [PATCH 11/22] Simplify and improve clarity of old code --- .../core/data_structure/dense_block_hash.h | 2 +- .../ndtree/impl/ndtree_node_inl.h | 2 +- .../core/data_structure/ndtree_block_hash.h | 2 +- .../hashed_chunked_wavelet_integrator.h | 8 ++-- .../core/map/hashed_wavelet_octree_block.h | 4 +- .../map_operations/map_operation_base.h | 2 +- .../hashed_chunked_wavelet_integrator.cc | 40 +++++++++---------- .../hashed_chunked_wavelet_octree_block.cc | 3 ++ .../core/map/hashed_wavelet_octree_block.cc | 28 ++++++------- 9 files changed, 47 insertions(+), 44 deletions(-) diff --git a/library/cpp/include/wavemap/core/data_structure/dense_block_hash.h b/library/cpp/include/wavemap/core/data_structure/dense_block_hash.h index bc1454e4f..a187313ab 100644 --- a/library/cpp/include/wavemap/core/data_structure/dense_block_hash.h +++ b/library/cpp/include/wavemap/core/data_structure/dense_block_hash.h @@ -18,7 +18,7 @@ class DenseBlockHash { using BlockHashMap = SpatialHash; using Cell = CellDataT; - explicit DenseBlockHash(CellDataT default_value = CellDataT{}) + explicit DenseBlockHash(CellDataT default_value = {}) : default_value_(default_value) {} bool empty() const { return block_map_.empty(); } diff --git a/library/cpp/include/wavemap/core/data_structure/ndtree/impl/ndtree_node_inl.h b/library/cpp/include/wavemap/core/data_structure/ndtree/impl/ndtree_node_inl.h index ef34bdba0..7bd4c0b64 100644 --- a/library/cpp/include/wavemap/core/data_structure/ndtree/impl/ndtree_node_inl.h +++ b/library/cpp/include/wavemap/core/data_structure/ndtree/impl/ndtree_node_inl.h @@ -15,7 +15,7 @@ bool NdtreeNode::empty() const { template void NdtreeNode::clear() { deleteChildrenArray(); - data() = DataT{}; + data() = {}; } template diff --git a/library/cpp/include/wavemap/core/data_structure/ndtree_block_hash.h b/library/cpp/include/wavemap/core/data_structure/ndtree_block_hash.h index 910c5a5e2..5b9e1ad69 100644 --- a/library/cpp/include/wavemap/core/data_structure/ndtree_block_hash.h +++ b/library/cpp/include/wavemap/core/data_structure/ndtree_block_hash.h @@ -22,7 +22,7 @@ class NdtreeBlockHash { static constexpr HeightType kChunkHeight = 1; explicit NdtreeBlockHash(HeightType max_tree_height, - CellDataT default_value = CellDataT{}) + CellDataT default_value = {}) : max_height_(max_tree_height), default_value_(default_value) {} bool empty() const { return block_map_.empty(); } diff --git a/library/cpp/include/wavemap/core/integrator/projective/coarse_to_fine/hashed_chunked_wavelet_integrator.h b/library/cpp/include/wavemap/core/integrator/projective/coarse_to_fine/hashed_chunked_wavelet_integrator.h index 2678ec6a3..4c642cb27 100644 --- a/library/cpp/include/wavemap/core/integrator/projective/coarse_to_fine/hashed_chunked_wavelet_integrator.h +++ b/library/cpp/include/wavemap/core/integrator/projective/coarse_to_fine/hashed_chunked_wavelet_integrator.h @@ -65,10 +65,10 @@ class HashedChunkedWaveletIntegrator : public ProjectiveIntegrator { void updateBlock(HashedChunkedWaveletOctree::Block& block, const HashedChunkedWaveletOctree::BlockIndex& block_index); - void updateNodeRecursive(OctreeType::NodeRefType parent_node_ref, - const OctreeIndex& parent_node_index, - FloatingPoint& parent_value, - OctreeType::ChunkType::BitRef parent_has_child, + void updateNodeRecursive(OctreeType::NodeRefType node, + const OctreeIndex& node_index, + FloatingPoint& node_value, + OctreeType::ChunkType::BitRef node_has_child, bool& block_needs_thresholding); void updateLeavesBatch(const OctreeIndex& parent_index, FloatingPoint& parent_value, diff --git a/library/cpp/include/wavemap/core/map/hashed_wavelet_octree_block.h b/library/cpp/include/wavemap/core/map/hashed_wavelet_octree_block.h index b27467fa2..b622b9372 100644 --- a/library/cpp/include/wavemap/core/map/hashed_wavelet_octree_block.h +++ b/library/cpp/include/wavemap/core/map/hashed_wavelet_octree_block.h @@ -80,8 +80,8 @@ class HashedWaveletOctreeBlock { bool needs_pruning_ = false; Timestamp last_updated_stamp_ = Time::now(); - Coefficients::Scale recursiveThreshold(OctreeType::NodeRefType node, - Coefficients::Scale scale_coefficient); + void recursiveThreshold(OctreeType::NodeRefType node, + Coefficients::Scale& node_scale_coefficient); void recursivePrune(OctreeType::NodeRefType node); }; } // namespace wavemap diff --git a/library/cpp/include/wavemap/pipeline/map_operations/map_operation_base.h b/library/cpp/include/wavemap/pipeline/map_operations/map_operation_base.h index cec8ffb24..b8e653489 100644 --- a/library/cpp/include/wavemap/pipeline/map_operations/map_operation_base.h +++ b/library/cpp/include/wavemap/pipeline/map_operations/map_operation_base.h @@ -17,7 +17,7 @@ struct MapOperationType : public TypeSelector { class MapOperationBase { public: explicit MapOperationBase(MapBase::Ptr occupancy_map) - : occupancy_map_(std::move(occupancy_map)) {} + : occupancy_map_(std::move(CHECK_NOTNULL(occupancy_map))) {} virtual ~MapOperationBase() = default; diff --git a/library/cpp/src/core/integrator/projective/coarse_to_fine/hashed_chunked_wavelet_integrator.cc b/library/cpp/src/core/integrator/projective/coarse_to_fine/hashed_chunked_wavelet_integrator.cc index 72edfcff9..6452d9906 100644 --- a/library/cpp/src/core/integrator/projective/coarse_to_fine/hashed_chunked_wavelet_integrator.cc +++ b/library/cpp/src/core/integrator/projective/coarse_to_fine/hashed_chunked_wavelet_integrator.cc @@ -85,21 +85,21 @@ void HashedChunkedWaveletIntegrator::updateBlock( } void HashedChunkedWaveletIntegrator::updateNodeRecursive( // NOLINT - HashedChunkedWaveletIntegrator::OctreeType::NodeRefType parent_node_ref, - const OctreeIndex& parent_node_index, FloatingPoint& parent_value, + HashedChunkedWaveletIntegrator::OctreeType::NodeRefType node, + const OctreeIndex& node_index, FloatingPoint& node_value, HashedChunkedWaveletIntegrator::OctreeType::ChunkType::BitRef - parent_has_child, + node_has_child, bool& block_needs_thresholding) { - auto& parent_details = parent_node_ref.data(); + // Decompress child values + auto& node_details = node.data(); auto child_values = HashedChunkedWaveletOctreeBlock::Transform::backward( - {parent_value, parent_details}); + {node_value, node_details}); - // Handle all the children + // Handle each child 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; + node_index.computeChildIndex(relative_child_idx); FloatingPoint& child_value = child_values[relative_child_idx]; // Test whether it is fully occupied; free or unknown; or fully unknown @@ -144,29 +144,29 @@ void HashedChunkedWaveletIntegrator::updateNodeRecursive( // NOLINT } // Since the approximation error would still be too big, refine - auto child_node_ref = - parent_node_ref.getOrAllocateChild(relative_child_idx); - auto& child_details = child_node_ref.data(); - auto child_has_children = child_node_ref.hasAtLeastOneChild(); + auto child_node = node.getOrAllocateChild(relative_child_idx); + auto& child_details = child_node.data(); + auto child_has_child = child_node.hasAtLeastOneChild(); // If we're at the leaf level, directly compute the update - if (child_height <= termination_height_ + 1) { + if (child_index.height <= termination_height_ + 1) { updateLeavesBatch(child_index, child_value, child_details); } else { // Otherwise, recurse - DCHECK_GE(child_height, 0); - updateNodeRecursive(child_node_ref, child_index, child_value, - child_has_children, block_needs_thresholding); + DCHECK_GE(child_index.height, 0); + updateNodeRecursive(child_node, child_index, child_value, child_has_child, + block_needs_thresholding); } - if (child_has_children || data::is_nonzero(child_details)) { - parent_has_child = true; + if (child_has_child || data::is_nonzero(child_details)) { + node_has_child = true; } } + // Compress const auto [new_value, new_details] = HashedChunkedWaveletOctreeBlock::Transform::forward(child_values); - parent_details = new_details; - parent_value = new_value; + node_details = new_details; + node_value = new_value; } } // namespace wavemap diff --git a/library/cpp/src/core/map/hashed_chunked_wavelet_octree_block.cc b/library/cpp/src/core/map/hashed_chunked_wavelet_octree_block.cc index b531c49b1..57fd2c9a9 100644 --- a/library/cpp/src/core/map/hashed_chunked_wavelet_octree_block.cc +++ b/library/cpp/src/core/map/hashed_chunked_wavelet_octree_block.cc @@ -150,6 +150,9 @@ void HashedChunkedWaveletOctreeBlock::forEachLeaf( } } +// TODO(victorr): Once the automatic benchmarking system is ready, see if this +// method can be simplified using OctreeType::NodeRefType without +// sacrificing too much performance HashedChunkedWaveletOctreeBlock::RecursiveThresholdReturnValue HashedChunkedWaveletOctreeBlock::recursiveThreshold( // NOLINT HashedChunkedWaveletOctreeBlock::OctreeType::ChunkType& chunk, diff --git a/library/cpp/src/core/map/hashed_wavelet_octree_block.cc b/library/cpp/src/core/map/hashed_wavelet_octree_block.cc index 4d2f6c395..0afb6603c 100644 --- a/library/cpp/src/core/map/hashed_wavelet_octree_block.cc +++ b/library/cpp/src/core/map/hashed_wavelet_octree_block.cc @@ -9,8 +9,7 @@ namespace wavemap { void HashedWaveletOctreeBlock::threshold() { ProfilerZoneScoped; if (getNeedsThresholding()) { - root_scale_coefficient_ -= - recursiveThreshold(ndtree_.getRootNode(), root_scale_coefficient_); + recursiveThreshold(ndtree_.getRootNode(), root_scale_coefficient_); setNeedsThresholding(false); } } @@ -149,30 +148,31 @@ void HashedWaveletOctreeBlock::forEachLeaf( } } -HashedWaveletOctreeBlock::Coefficients::Scale -HashedWaveletOctreeBlock::recursiveThreshold( // NOLINT +void HashedWaveletOctreeBlock::recursiveThreshold( // NOLINT HashedWaveletOctreeBlock::OctreeType::NodeRefType node, - FloatingPoint scale_coefficient) { + FloatingPoint& node_scale_coefficient) { + // Decompress child values + auto& node_detail_coefficients = node.data(); Coefficients::CoefficientsArray child_scale_coefficients = - Transform::backward({scale_coefficient, node.data()}); + Transform::backward({node_scale_coefficient, node_detail_coefficients}); + // Handle each child for (NdtreeIndexRelativeChild child_idx = 0; child_idx < OctreeIndex::kNumChildren; ++child_idx) { + Coefficients::Scale& child_scale = child_scale_coefficients[child_idx]; if (node.hasChild(child_idx)) { OctreeType::NodeRefType child_node = *node.getChild(child_idx); - child_scale_coefficients[child_idx] = - recursiveThreshold(child_node, child_scale_coefficients[child_idx]); + recursiveThreshold(child_node, child_scale); } else { - child_scale_coefficients[child_idx] -= std::clamp( - child_scale_coefficients[child_idx], min_log_odds_, max_log_odds_); + child_scale = std::clamp(child_scale, min_log_odds_, max_log_odds_); } } - const auto [scale_update, detail_updates] = + // Compress + const auto [new_scale, new_details] = Transform::forward(child_scale_coefficients); - node.data() -= detail_updates; - - return scale_update; + node_detail_coefficients = new_details; + node_scale_coefficient = new_scale; } void HashedWaveletOctreeBlock::recursivePrune( // NOLINT From 26fba562873d522ea960513311fce5dfbba5e774 Mon Sep 17 00:00:00 2001 From: Victor Reijgwart Date: Tue, 12 Nov 2024 17:17:51 +0100 Subject: [PATCH 12/22] Add method to erase individual nodes from chunked octrees --- .../chunked_ndtree/chunked_ndtree.h | 2 +- .../chunked_ndtree_node_address.h | 9 ++++- .../impl/chunked_ndtree_chunk_inl.h | 3 +- .../impl/chunked_ndtree_node_address_inl.h | 37 +++++++++++++++++-- .../src/core/data_structure/test_ndtree.cc | 9 ++--- 5 files changed, 48 insertions(+), 12 deletions(-) diff --git a/library/cpp/include/wavemap/core/data_structure/chunked_ndtree/chunked_ndtree.h b/library/cpp/include/wavemap/core/data_structure/chunked_ndtree/chunked_ndtree.h index 6f0050f9b..50fd748ad 100644 --- a/library/cpp/include/wavemap/core/data_structure/chunked_ndtree/chunked_ndtree.h +++ b/library/cpp/include/wavemap/core/data_structure/chunked_ndtree/chunked_ndtree.h @@ -28,7 +28,7 @@ class ChunkedNdtree { explicit ChunkedNdtree(HeightType max_height); ~ChunkedNdtree() = default; - bool empty() const { return root_chunk_.empty(); } + bool empty() const { return getRootNode().empty(); } size_t size() const; void clear() { root_chunk_.clear(); } void prune(); diff --git a/library/cpp/include/wavemap/core/data_structure/chunked_ndtree/chunked_ndtree_node_address.h b/library/cpp/include/wavemap/core/data_structure/chunked_ndtree/chunked_ndtree_node_address.h index d5e45ea34..9e7be7096 100644 --- a/library/cpp/include/wavemap/core/data_structure/chunked_ndtree/chunked_ndtree_node_address.h +++ b/library/cpp/include/wavemap/core/data_structure/chunked_ndtree/chunked_ndtree_node_address.h @@ -37,6 +37,8 @@ class ChunkedNdtreeNodePtr { const NodeRef* operator->() const { return node_.operator->(); } private: + // TODO(victorr): Benchmark version that uses chunk_=nullptr instead of + // std::optional to mark invalid states std::optional node_; }; @@ -74,9 +76,12 @@ class ChunkedNdtreeNodeRef { bool hasNonzeroData(FloatingPoint threshold) const; auto& data() const; - typename ChunkType::BitRef hasAtLeastOneChild() const; + auto hasAtLeastOneChild() const; // Returns a BitRef or bool, depending on + // whether the ChunkType is const-qualified bool hasChild(NdtreeIndexRelativeChild child_index) const; + // TODO(victorr): Add tests for this method + void eraseChild(NdtreeIndexRelativeChild child_index) const; NodePtr getChild(NdtreeIndexRelativeChild child_index) const; template @@ -85,7 +90,9 @@ class ChunkedNdtreeNodeRef { private: ChunkType& chunk_; + // TODO(victorr): Benchmark with int8_t const IndexElement relative_node_depth_ = 0; + // TODO(victorr): Benchmark with uint32_t const LinearIndex level_traversal_distance_ = 0u; LinearIndex computeRelativeNodeIndex() const; diff --git a/library/cpp/include/wavemap/core/data_structure/chunked_ndtree/impl/chunked_ndtree_chunk_inl.h b/library/cpp/include/wavemap/core/data_structure/chunked_ndtree/impl/chunked_ndtree_chunk_inl.h index 470986b23..ca0e4d625 100644 --- a/library/cpp/include/wavemap/core/data_structure/chunked_ndtree/impl/chunked_ndtree_chunk_inl.h +++ b/library/cpp/include/wavemap/core/data_structure/chunked_ndtree/impl/chunked_ndtree_chunk_inl.h @@ -15,7 +15,8 @@ bool ChunkedNdtreeChunk::empty() const { template void ChunkedNdtreeChunk::clear() { deleteChildrenArray(); - std::fill(node_data_.begin(), node_data_.end(), DataT{}); + node_data_.fill({}); + node_has_at_least_one_child_.reset(); } template diff --git a/library/cpp/include/wavemap/core/data_structure/chunked_ndtree/impl/chunked_ndtree_node_address_inl.h b/library/cpp/include/wavemap/core/data_structure/chunked_ndtree/impl/chunked_ndtree_node_address_inl.h index a8d2e2679..c345a175c 100644 --- a/library/cpp/include/wavemap/core/data_structure/chunked_ndtree/impl/chunked_ndtree_node_address_inl.h +++ b/library/cpp/include/wavemap/core/data_structure/chunked_ndtree/impl/chunked_ndtree_node_address_inl.h @@ -101,8 +101,7 @@ auto& ChunkedNdtreeNodeRef::data() const { } template -typename ChunkType::BitRef ChunkedNdtreeNodeRef::hasAtLeastOneChild() - const { +auto ChunkedNdtreeNodeRef::hasAtLeastOneChild() const { return chunk_.nodeHasAtLeastOneChild(computeRelativeNodeIndex()); } @@ -112,14 +111,44 @@ bool ChunkedNdtreeNodeRef::hasChild( return getChild(child_index); } +template +void ChunkedNdtreeNodeRef::eraseChild( + NdtreeIndexRelativeChild child_index) const { + if (!hasAtLeastOneChild()) { + return; + } + LinearIndex child_start_idx = computeChildLevelTraversalDistance(child_index); + LinearIndex child_end_idx = child_start_idx + 1; + for (int child_depth = relative_node_depth_ + 1; + child_depth <= ChunkType::kHeight; ++child_depth) { + for (LinearIndex level_child_idx = child_start_idx; + level_child_idx < child_end_idx; ++level_child_idx) { + if (child_depth == ChunkType::kHeight) { + chunk_.eraseChild(level_child_idx); + } else { + const LinearIndex level_offset = + tree_math::perfect_tree::num_total_nodes_fast(child_depth); + const LinearIndex chunk_child_idx = level_offset + level_child_idx; + chunk_.nodeData(chunk_child_idx) = {}; + chunk_.nodeHasAtLeastOneChild(chunk_child_idx) = false; + } + } + child_start_idx <<= kDim; + child_end_idx <<= kDim; + } +} + template typename ChunkedNdtreeNodeRef::NodePtr ChunkedNdtreeNodeRef::getChild( NdtreeIndexRelativeChild child_index) const { + if (!hasAtLeastOneChild()) { + return {nullptr, 0, 0u}; + } const IndexElement child_depth = relative_node_depth_ + 1; const LinearIndex child_level_traversal_distance = computeChildLevelTraversalDistance(child_index); - if (child_depth % ChunkType::kHeight == 0) { + if (child_depth == ChunkType::kHeight) { auto* child_chunk = chunk_.getChild(child_level_traversal_distance); return {child_chunk, 0, 0u}; } else { @@ -136,7 +165,7 @@ ChunkedNdtreeNodeRef::getOrAllocateChild( const IndexElement child_depth = relative_node_depth_ + 1; const LinearIndex child_level_traversal_distance = computeChildLevelTraversalDistance(child_index); - if (child_depth % ChunkType::kHeight == 0) { + if (child_depth == ChunkType::kHeight) { auto& child_chunk = chunk_.getOrAllocateChild( child_level_traversal_distance, std::forward(args)...); return {child_chunk, 0, 0}; diff --git a/library/cpp/test/src/core/data_structure/test_ndtree.cc b/library/cpp/test/src/core/data_structure/test_ndtree.cc index 9ea81deb0..58dc715cb 100644 --- a/library/cpp/test/src/core/data_structure/test_ndtree.cc +++ b/library/cpp/test/src/core/data_structure/test_ndtree.cc @@ -41,18 +41,17 @@ TYPED_TEST(NdtreeTest, AllocatingAndClearing) { GeometryGenerator::getRandomNdtreeIndex(min_child_pos, max_child_pos, 0, 0) .computeParentIndex(test_height); - const bool index_is_inside_root_chunk_node = - (tree_height - random_index.height) < TypeParam::kChunkHeight; + const bool index_is_root = tree_height == random_index.height; - EXPECT_EQ(ndtree.hasNode(random_index), index_is_inside_root_chunk_node) + EXPECT_EQ(ndtree.hasNode(random_index), index_is_root) << random_index.toString() << " and tree height " << tree_height; ndtree.getOrAllocateNode(random_index); EXPECT_TRUE(ndtree.hasNode(random_index)) << random_index.toString() << " and tree height " << tree_height; - EXPECT_EQ(ndtree.empty(), index_is_inside_root_chunk_node); + EXPECT_EQ(ndtree.empty(), index_is_root); ndtree.clear(); - EXPECT_EQ(ndtree.hasNode(random_index), index_is_inside_root_chunk_node) + EXPECT_EQ(ndtree.hasNode(random_index), index_is_root) << random_index.toString() << " and tree height " << tree_height; EXPECT_TRUE(ndtree.empty()); } From beb4c5c1b295242d9df5ee075f668a433f6115c0 Mon Sep 17 00:00:00 2001 From: Victor Reijgwart Date: Mon, 18 Nov 2024 17:29:39 +0100 Subject: [PATCH 13/22] Simplify thresholding and pruning code --- .../map/hashed_chunked_wavelet_octree_block.h | 11 +- .../hashed_chunked_wavelet_octree_block.cc | 124 +++++------------- .../core/map/hashed_wavelet_octree_block.cc | 5 +- 3 files changed, 36 insertions(+), 104 deletions(-) diff --git a/library/cpp/include/wavemap/core/map/hashed_chunked_wavelet_octree_block.h b/library/cpp/include/wavemap/core/map/hashed_chunked_wavelet_octree_block.h index f7e8c5883..48cd6df0b 100644 --- a/library/cpp/include/wavemap/core/map/hashed_chunked_wavelet_octree_block.h +++ b/library/cpp/include/wavemap/core/map/hashed_chunked_wavelet_octree_block.h @@ -89,13 +89,10 @@ class HashedChunkedWaveletOctreeBlock { bool needs_pruning_ = false; Timestamp last_updated_stamp_ = Time::now(); - struct RecursiveThresholdReturnValue { - Coefficients::Scale scale; - bool is_nonzero_child; - }; - RecursiveThresholdReturnValue recursiveThreshold( - OctreeType::ChunkType& chunk, Coefficients::Scale scale_coefficient); - void recursivePrune(OctreeType::ChunkType& chunk); + void recursiveThreshold(OctreeType::NodeRefType node, + Coefficients::Scale& node_scale_coefficient); + void recursivePrune( + HashedChunkedWaveletOctreeBlock::OctreeType::NodeRefType chunk); }; } // namespace wavemap diff --git a/library/cpp/src/core/map/hashed_chunked_wavelet_octree_block.cc b/library/cpp/src/core/map/hashed_chunked_wavelet_octree_block.cc index 57fd2c9a9..1cf109f9d 100644 --- a/library/cpp/src/core/map/hashed_chunked_wavelet_octree_block.cc +++ b/library/cpp/src/core/map/hashed_chunked_wavelet_octree_block.cc @@ -10,9 +10,7 @@ namespace wavemap { void HashedChunkedWaveletOctreeBlock::threshold() { ProfilerZoneScoped; if (getNeedsThresholding()) { - root_scale_coefficient_ = - recursiveThreshold(ndtree_.getRootChunk(), root_scale_coefficient_) - .scale; + recursiveThreshold(ndtree_.getRootNode(), root_scale_coefficient_); setNeedsThresholding(false); } } @@ -21,7 +19,7 @@ void HashedChunkedWaveletOctreeBlock::prune() { ProfilerZoneScoped; if (getNeedsPruning()) { threshold(); - recursivePrune(ndtree_.getRootChunk()); + recursivePrune(ndtree_.getRootNode()); setNeedsPruning(false); } } @@ -150,111 +148,49 @@ void HashedChunkedWaveletOctreeBlock::forEachLeaf( } } -// TODO(victorr): Once the automatic benchmarking system is ready, see if this -// method can be simplified using OctreeType::NodeRefType without -// sacrificing too much performance -HashedChunkedWaveletOctreeBlock::RecursiveThresholdReturnValue -HashedChunkedWaveletOctreeBlock::recursiveThreshold( // NOLINT - HashedChunkedWaveletOctreeBlock::OctreeType::ChunkType& chunk, - FloatingPoint scale_coefficient) { - constexpr auto tree_size = [](auto tree_height) { - return static_cast( - tree_math::perfect_tree::num_total_nodes_fast(tree_height)); - }; - constexpr auto level_size = [](auto level_height) { - return static_cast( - tree_math::perfect_tree::num_leaf_nodes(level_height)); - }; - - // Decompress - std::array - chunk_scale_coefficients{}; - std::bitset is_nonzero_child{}; - chunk_scale_coefficients[0] = scale_coefficient; - for (int level_idx = 0; level_idx < kChunkHeight; ++level_idx) { - const int first_idx = tree_size(level_idx); - const int last_idx = tree_size(level_idx + 1); - for (int relative_idx = 0; relative_idx < level_size(level_idx + 1); - ++relative_idx) { - const int src_idx = first_idx + relative_idx; - const Coefficients::CoefficientsArray child_scale_coefficients = - Transform::backward( - {chunk_scale_coefficients[src_idx], chunk.nodeData(src_idx)}); - const int first_dest_idx = last_idx + 8 * relative_idx; - std::move(child_scale_coefficients.begin(), - child_scale_coefficients.end(), - chunk_scale_coefficients.begin() + first_dest_idx); - } - } - - // Threshold - const int first_leaf_idx = tree_size(kChunkHeight); - for (LinearIndex child_idx = 0; - child_idx < OctreeType::ChunkType::kNumChildren; ++child_idx) { - const LinearIndex array_idx = first_leaf_idx + child_idx; - if (chunk.hasChild(child_idx)) { - OctreeType::ChunkType& child_chunk = *chunk.getChild(child_idx); - const auto rv = - recursiveThreshold(child_chunk, chunk_scale_coefficients[array_idx]); - chunk_scale_coefficients[array_idx] = rv.scale; - is_nonzero_child[array_idx] = rv.is_nonzero_child; +void HashedChunkedWaveletOctreeBlock::recursiveThreshold( // NOLINT + OctreeType::NodeRefType node, Coefficients::Scale& node_scale_coefficient) { + // Decompress child values + auto& node_detail_coefficients = node.data(); + Coefficients::CoefficientsArray child_scale_coefficients = + Transform::backward({node_scale_coefficient, node_detail_coefficients}); + + // Handle each child + for (NdtreeIndexRelativeChild child_idx = 0; + child_idx < OctreeIndex::kNumChildren; ++child_idx) { + Coefficients::Scale& child_scale = child_scale_coefficients[child_idx]; + if (auto child_node = node.getChild(child_idx); child_node) { + recursiveThreshold(*child_node, child_scale); } else { - chunk_scale_coefficients[array_idx] = std::clamp( - chunk_scale_coefficients[array_idx], min_log_odds_, max_log_odds_); + child_scale = std::clamp(child_scale, min_log_odds_, max_log_odds_); } } // Compress - for (int level_idx = kChunkHeight - 1; 0 <= level_idx; --level_idx) { - const int first_idx = tree_size(level_idx); - const int last_idx = tree_size(level_idx + 1); - for (int relative_idx = level_size(level_idx + 1) - 1; 0 <= relative_idx; - --relative_idx) { - const int first_src_idx = last_idx + 8 * relative_idx; - Coefficients::CoefficientsArray scale_coefficients_subset{}; - std::move(chunk_scale_coefficients.begin() + first_src_idx, - chunk_scale_coefficients.begin() + first_src_idx + 8, - scale_coefficients_subset.begin()); - bool has_nonzero_child = false; - for (int src_idx = first_src_idx; src_idx < first_src_idx + 8; - ++src_idx) { - has_nonzero_child |= is_nonzero_child[src_idx]; - } - - const int dst_idx = first_idx + relative_idx; - auto [new_scale, new_details] = - Transform::forward(scale_coefficients_subset); - chunk_scale_coefficients[dst_idx] = new_scale; - chunk.nodeData(dst_idx) = new_details; - chunk.nodeHasAtLeastOneChild(dst_idx) = has_nonzero_child; - is_nonzero_child[dst_idx] = - has_nonzero_child || data::is_nonzero(new_details); - } - } - - return {chunk_scale_coefficients[0], is_nonzero_child[0]}; + const auto [new_scale, new_details] = + Transform::forward(child_scale_coefficients); + node_detail_coefficients = new_details; + node_scale_coefficient = new_scale; } void HashedChunkedWaveletOctreeBlock::recursivePrune( // NOLINT - HashedChunkedWaveletOctreeBlock::OctreeType::ChunkType& chunk) { - constexpr FloatingPoint kNonzeroCoefficientThreshold = 1e-3f; + HashedChunkedWaveletOctreeBlock::OctreeType::NodeRefType node) { bool has_at_least_one_child = false; - for (LinearIndex linear_child_idx = 0; - linear_child_idx < OctreeType::ChunkType::kNumChildren; - ++linear_child_idx) { - if (chunk.hasChild(linear_child_idx)) { - OctreeType::ChunkType& child_chunk = *chunk.getChild(linear_child_idx); - recursivePrune(child_chunk); - if (!child_chunk.hasChildrenArray() && - !child_chunk.hasNonzeroData(kNonzeroCoefficientThreshold)) { - chunk.eraseChild(linear_child_idx); + for (NdtreeIndexRelativeChild child_idx = 0; + child_idx < OctreeIndex::kNumChildren; ++child_idx) { + if (node.hasChild(child_idx)) { + OctreeType::NodeRefType child_node = *node.getChild(child_idx); + recursivePrune(child_node); + if (!child_node.hasAtLeastOneChild() && + !child_node.hasNonzeroData(1e-3f)) { + node.eraseChild(child_idx); } else { has_at_least_one_child = true; } } } if (!has_at_least_one_child) { - chunk.deleteChildrenArray(); + node.hasAtLeastOneChild() = false; } } } // namespace wavemap diff --git a/library/cpp/src/core/map/hashed_wavelet_octree_block.cc b/library/cpp/src/core/map/hashed_wavelet_octree_block.cc index 0afb6603c..fbd23d5ca 100644 --- a/library/cpp/src/core/map/hashed_wavelet_octree_block.cc +++ b/library/cpp/src/core/map/hashed_wavelet_octree_block.cc @@ -160,9 +160,8 @@ void HashedWaveletOctreeBlock::recursiveThreshold( // NOLINT for (NdtreeIndexRelativeChild child_idx = 0; child_idx < OctreeIndex::kNumChildren; ++child_idx) { Coefficients::Scale& child_scale = child_scale_coefficients[child_idx]; - if (node.hasChild(child_idx)) { - OctreeType::NodeRefType child_node = *node.getChild(child_idx); - recursiveThreshold(child_node, child_scale); + if (auto child_node = node.getChild(child_idx); child_node) { + recursiveThreshold(*child_node, child_scale); } else { child_scale = std::clamp(child_scale, min_log_odds_, max_log_odds_); } From 642680f1ca4efb8a8587567d44e8fc4e0a90d743 Mon Sep 17 00:00:00 2001 From: Victor Reijgwart Date: Mon, 18 Nov 2024 18:07:55 +0100 Subject: [PATCH 14/22] Optimize child node access by eliminating redundant lookup Instead of first calling hasChild and then retrieving a reference with *getChild, this change directly uses the pointer returned by getChild, avoiding a double lookup and improving efficiency. With C++17's if condition init-statements, the code remains clean and easy to read. --- .../impl/ndtree_block_hash_inl.h | 5 ++--- .../map/hashed_chunked_wavelet_octree_block.cc | 10 +++++----- .../core/map/hashed_wavelet_octree_block.cc | 16 ++++++++-------- library/cpp/src/core/map/volumetric_octree.cc | 6 +++--- library/cpp/src/core/map/wavelet_octree.cc | 18 ++++++++---------- .../cpp/src/core/utils/query/classified_map.cc | 11 +++++------ 6 files changed, 31 insertions(+), 35 deletions(-) diff --git a/library/cpp/include/wavemap/core/data_structure/impl/ndtree_block_hash_inl.h b/library/cpp/include/wavemap/core/data_structure/impl/ndtree_block_hash_inl.h index 15b5d847a..018dcbb15 100644 --- a/library/cpp/include/wavemap/core/data_structure/impl/ndtree_block_hash_inl.h +++ b/library/cpp/include/wavemap/core/data_structure/impl/ndtree_block_hash_inl.h @@ -210,9 +210,8 @@ void NdtreeBlockHash::forEachLeaf( child_idx < OctreeIndex::kNumChildren; ++child_idx) { const OctreeIndex child_node_index = node_index.computeChildIndex(child_idx); - if (node.hasChild(child_idx)) { - Node& child_node = *node.getChild(child_idx); - stack.emplace(StackElement{child_node_index, child_node}); + if (const Node* child_node = node.getChild(child_idx); child_node) { + stack.emplace(StackElement{child_node_index, *child_node}); } else { visitor_fn(child_node_index, node.data()); } diff --git a/library/cpp/src/core/map/hashed_chunked_wavelet_octree_block.cc b/library/cpp/src/core/map/hashed_chunked_wavelet_octree_block.cc index 1cf109f9d..29d87c8f0 100644 --- a/library/cpp/src/core/map/hashed_chunked_wavelet_octree_block.cc +++ b/library/cpp/src/core/map/hashed_chunked_wavelet_octree_block.cc @@ -178,11 +178,11 @@ void HashedChunkedWaveletOctreeBlock::recursivePrune( // NOLINT bool has_at_least_one_child = false; for (NdtreeIndexRelativeChild child_idx = 0; child_idx < OctreeIndex::kNumChildren; ++child_idx) { - if (node.hasChild(child_idx)) { - OctreeType::NodeRefType child_node = *node.getChild(child_idx); - recursivePrune(child_node); - if (!child_node.hasAtLeastOneChild() && - !child_node.hasNonzeroData(1e-3f)) { + if (OctreeType::NodePtrType child_node = node.getChild(child_idx); + child_node) { + recursivePrune(*child_node); + if (!child_node->hasAtLeastOneChild() && + !child_node->hasNonzeroData(1e-3f)) { node.eraseChild(child_idx); } else { has_at_least_one_child = true; diff --git a/library/cpp/src/core/map/hashed_wavelet_octree_block.cc b/library/cpp/src/core/map/hashed_wavelet_octree_block.cc index fbd23d5ca..a0633ab36 100644 --- a/library/cpp/src/core/map/hashed_wavelet_octree_block.cc +++ b/library/cpp/src/core/map/hashed_wavelet_octree_block.cc @@ -136,10 +136,9 @@ void HashedWaveletOctreeBlock::forEachLeaf( node_index.computeChildIndex(child_idx); const FloatingPoint child_scale_coefficient = child_scale_coefficients[child_idx]; - if (node.hasChild(child_idx) && - termination_height < child_node_index.height) { - OctreeType::NodeConstRefType child_node = *node.getChild(child_idx); - stack.emplace(StackElement{child_node_index, child_node, + OctreeType::NodeConstPtrType child_node = node.getChild(child_idx); + if (child_node && termination_height < child_node_index.height) { + stack.emplace(StackElement{child_node_index, *child_node, child_scale_coefficient}); } else { visitor_fn(child_node_index, child_scale_coefficient); @@ -179,10 +178,11 @@ void HashedWaveletOctreeBlock::recursivePrune( // NOLINT bool has_at_least_one_child = false; for (NdtreeIndexRelativeChild child_idx = 0; child_idx < OctreeIndex::kNumChildren; ++child_idx) { - if (node.hasChild(child_idx)) { - OctreeType::NodeRefType child_node = *node.getChild(child_idx); - recursivePrune(child_node); - if (!child_node.hasChildrenArray() && !child_node.hasNonzeroData(1e-3f)) { + if (OctreeType::NodePtrType child_node = node.getChild(child_idx); + child_node) { + recursivePrune(*child_node); + if (!child_node->hasChildrenArray() && + !child_node->hasNonzeroData(1e-3f)) { node.eraseChild(child_idx); } else { has_at_least_one_child = true; diff --git a/library/cpp/src/core/map/volumetric_octree.cc b/library/cpp/src/core/map/volumetric_octree.cc index 62cd82802..88d1b953a 100644 --- a/library/cpp/src/core/map/volumetric_octree.cc +++ b/library/cpp/src/core/map/volumetric_octree.cc @@ -134,9 +134,9 @@ void VolumetricOctree::forEachLeaf( child_idx < OctreeIndex::kNumChildren; ++child_idx) { const OctreeIndex child_node_index = node_index.computeChildIndex(child_idx); - if (node.hasChild(child_idx)) { - const NodeType& child_node = *node.getChild(child_idx); - stack.emplace(StackElement{child_node_index, child_node, node_value}); + if (const NodeType* child_node = node.getChild(child_idx); child_node) { + stack.emplace( + StackElement{child_node_index, *child_node, node_value}); } else { const OctreeIndex external_node_index = toExternalNodeIndex(child_node_index); diff --git a/library/cpp/src/core/map/wavelet_octree.cc b/library/cpp/src/core/map/wavelet_octree.cc index 33346c556..e4c72fb27 100644 --- a/library/cpp/src/core/map/wavelet_octree.cc +++ b/library/cpp/src/core/map/wavelet_octree.cc @@ -90,9 +90,8 @@ void WaveletOctree::forEachLeaf( node_index.computeChildIndex(child_idx); const FloatingPoint child_scale_coefficient = child_scale_coefficients[child_idx]; - if (node.hasChild(child_idx)) { - const NodeType& child_node = *node.getChild(child_idx); - stack.emplace(StackElement{child_node_index, child_node, + if (const NodeType* child_node = node.getChild(child_idx); child_node) { + stack.emplace(StackElement{child_node_index, *child_node, child_scale_coefficient}); } else { const OctreeIndex external_node_index = @@ -110,10 +109,9 @@ WaveletOctree::Coefficients::Scale WaveletOctree::recursiveThreshold( // NOLINT for (NdtreeIndexRelativeChild child_idx = 0; child_idx < OctreeIndex::kNumChildren; ++child_idx) { - if (node.hasChild(child_idx)) { - NodeType& child_node = *node.getChild(child_idx); + if (NodeType* child_node = node.getChild(child_idx); child_node) { child_scale_coefficients[child_idx] = - recursiveThreshold(child_node, child_scale_coefficients[child_idx]); + recursiveThreshold(*child_node, child_scale_coefficients[child_idx]); } else { child_scale_coefficients[child_idx] -= clamp(child_scale_coefficients[child_idx]); @@ -135,11 +133,11 @@ WaveletOctree::Coefficients::Scale WaveletOctree::recursivePrune( // NOLINT bool has_at_least_one_child = false; for (NdtreeIndexRelativeChild child_idx = 0; child_idx < OctreeIndex::kNumChildren; ++child_idx) { - if (node.hasChild(child_idx)) { - NodeType& child_node = *node.getChild(child_idx); + if (NodeType* child_node = node.getChild(child_idx); child_node) { child_scale_coefficients[child_idx] = - recursivePrune(child_node, child_scale_coefficients[child_idx]); - if (!child_node.hasChildrenArray() && !child_node.hasNonzeroData(1e-3f)) { + recursivePrune(*child_node, child_scale_coefficients[child_idx]); + if (!child_node->hasChildrenArray() && + !child_node->hasNonzeroData(1e-3f)) { node.eraseChild(child_idx); } else { has_at_least_one_child = true; diff --git a/library/cpp/src/core/utils/query/classified_map.cc b/library/cpp/src/core/utils/query/classified_map.cc index 75f6f2d3c..ac401fcfd 100644 --- a/library/cpp/src/core/utils/query/classified_map.cc +++ b/library/cpp/src/core/utils/query/classified_map.cc @@ -131,10 +131,9 @@ void ClassifiedMap::forEachLeaf(IndexedLeafVisitorFunction visitor_fn, child_idx < OctreeIndex::kNumChildren; ++child_idx) { const OctreeIndex child_node_index = node_index.computeChildIndex(child_idx); - if (node.hasChild(child_idx) && - termination_height < child_node_index.height) { - const Node& child_node = *node.getChild(child_idx); - stack.emplace(StackElement{child_node_index, child_node}); + const Node* child_node = node.getChild(child_idx); + if (child_node && termination_height < child_node_index.height) { + stack.emplace(StackElement{child_node_index, *child_node}); } else { const Occupancy::Mask child_occupancy = node.data().childOccupancyMask(child_idx); @@ -341,8 +340,8 @@ void ClassifiedMap::recursiveClassifier( // NOLINT for (int child_idx = 0; child_idx < OctreeIndex::kNumChildren; ++child_idx) { const FloatingPoint child_occupancy = child_occupancies[child_idx]; // If the node has children, recurse - if (occupancy_node.hasChild(child_idx)) { - const auto* occupancy_child_node = occupancy_node.getChild(child_idx); + if (const auto* occupancy_child_node = occupancy_node.getChild(child_idx); + occupancy_child_node) { auto& classified_child_node = classified_node.getOrAllocateChild(child_idx); recursiveClassifier(*occupancy_child_node, child_occupancy, From 9269a3b5faf75d07db81a72c7cdeeee00535c374 Mon Sep 17 00:00:00 2001 From: Victor Reijgwart Date: Thu, 21 Nov 2024 17:42:07 +0100 Subject: [PATCH 15/22] Code cleanup and consistency improvements to prepare PR#84 --- .../wavemap/core/data_structure/aabb.h | 4 ++- .../chunked_ndtree_node_address.h | 3 +- .../chunked_ndtree/impl/chunked_ndtree_inl.h | 18 +++------- .../data_structure/ndtree/impl/ndtree_inl.h | 9 ++--- .../wavemap/core/indexing/ndtree_index.h | 5 ++- .../hashed_chunked_wavelet_integrator.h | 3 +- .../hashed_wavelet_integrator.h | 8 +++-- .../impl/hashed_wavelet_integrator_inl.h | 4 +-- .../map/hashed_chunked_wavelet_octree_block.h | 2 +- .../hashed_chunked_wavelet_octree_block_inl.h | 8 ++--- .../impl/hashed_wavelet_octree_block_inl.h | 5 +-- .../utils/undistortion/stamped_pose_buffer.h | 2 +- .../hashed_chunked_wavelet_integrator.cc | 13 ++----- .../hashed_wavelet_integrator.cc | 35 ++++++++++--------- .../hashed_chunked_wavelet_octree_block.cc | 4 +-- 15 files changed, 48 insertions(+), 75 deletions(-) diff --git a/library/cpp/include/wavemap/core/data_structure/aabb.h b/library/cpp/include/wavemap/core/data_structure/aabb.h index 6182cdeda..b0864aad9 100644 --- a/library/cpp/include/wavemap/core/data_structure/aabb.h +++ b/library/cpp/include/wavemap/core/data_structure/aabb.h @@ -4,6 +4,7 @@ #include #include #include +#include #include "wavemap/core/common.h" #include "wavemap/core/utils/math/int_math.h" @@ -25,7 +26,8 @@ struct AABB { PointType max = PointType::Constant(kInitialMax); AABB() = default; - AABB(PointT min, PointT max) : min(min), max(max) {} + AABB(const PointT& min, const PointT& max) : min(min), max(max) {} + AABB(PointT&& min, PointT&& max) : min(std::move(min)), max(std::move(max)) {} void includePoint(const PointType& point) { min = min.cwiseMin(point); diff --git a/library/cpp/include/wavemap/core/data_structure/chunked_ndtree/chunked_ndtree_node_address.h b/library/cpp/include/wavemap/core/data_structure/chunked_ndtree/chunked_ndtree_node_address.h index 9e7be7096..3f19474b6 100644 --- a/library/cpp/include/wavemap/core/data_structure/chunked_ndtree/chunked_ndtree_node_address.h +++ b/library/cpp/include/wavemap/core/data_structure/chunked_ndtree/chunked_ndtree_node_address.h @@ -37,8 +37,6 @@ class ChunkedNdtreeNodePtr { const NodeRef* operator->() const { return node_.operator->(); } private: - // TODO(victorr): Benchmark version that uses chunk_=nullptr instead of - // std::optional to mark invalid states std::optional node_; }; @@ -52,6 +50,7 @@ class ChunkedNdtreeNodeRef { static constexpr int kNumChildren = NdtreeIndex::kNumChildren; using NodeDataType = typename ChunkType::DataType; + ChunkedNdtreeNodeRef() = delete; ChunkedNdtreeNodeRef(ChunkType& chunk, IndexElement relative_node_depth, LinearIndex level_traversal_distance); diff --git a/library/cpp/include/wavemap/core/data_structure/chunked_ndtree/impl/chunked_ndtree_inl.h b/library/cpp/include/wavemap/core/data_structure/chunked_ndtree/impl/chunked_ndtree_inl.h index 85764f822..56691599d 100644 --- a/library/cpp/include/wavemap/core/data_structure/chunked_ndtree/impl/chunked_ndtree_inl.h +++ b/library/cpp/include/wavemap/core/data_structure/chunked_ndtree/impl/chunked_ndtree_inl.h @@ -81,16 +81,11 @@ ChunkedNdtree::getNode( const ChunkedNdtree::IndexType& index) { NodePtrType node = &getRootNode(); const MortonIndex morton_code = convert::nodeIndexToMorton(index); - for (int node_height = max_height_; index.height < node_height; + for (int node_height = max_height_; node && index.height < node_height; --node_height) { const NdtreeIndexRelativeChild child_index = NdtreeIndex::computeRelativeChildIndex(morton_code, node_height); - // Check if the child is allocated - NodePtrType child = node->getChild(child_index); - if (!child) { - return {}; - } - node = child; + node = node->getChild(child_index); } return node; } @@ -101,16 +96,11 @@ ChunkedNdtree::getNode( const ChunkedNdtree::IndexType& index) const { NodeConstPtrType node = &getRootNode(); const MortonIndex morton_code = convert::nodeIndexToMorton(index); - for (int node_height = max_height_; index.height < node_height; + for (int node_height = max_height_; node && index.height < node_height; --node_height) { const NdtreeIndexRelativeChild child_index = NdtreeIndex::computeRelativeChildIndex(morton_code, node_height); - // Check if the child is allocated - NodeConstPtrType child = node->getChild(child_index); - if (!child) { - return {}; - } - node = child; + node = node->getChild(child_index); } return node; } diff --git a/library/cpp/include/wavemap/core/data_structure/ndtree/impl/ndtree_inl.h b/library/cpp/include/wavemap/core/data_structure/ndtree/impl/ndtree_inl.h index ec56158ee..18b7a059d 100644 --- a/library/cpp/include/wavemap/core/data_structure/ndtree/impl/ndtree_inl.h +++ b/library/cpp/include/wavemap/core/data_structure/ndtree/impl/ndtree_inl.h @@ -77,16 +77,11 @@ const typename Ndtree::NodeType* Ndtree::getNode(const IndexType& index) const { const NodeType* node = &root_node_; const MortonIndex morton_code = convert::nodeIndexToMorton(index); - for (int node_height = max_height_; index.height < node_height; + for (int node_height = max_height_; node && index.height < node_height; --node_height) { const NdtreeIndexRelativeChild child_index = NdtreeIndex::computeRelativeChildIndex(morton_code, node_height); - // Check if the child is allocated - const NodeType* child = node->getChild(child_index); - if (!child) { - return nullptr; - } - node = child; + node = node->getChild(child_index); } return node; } diff --git a/library/cpp/include/wavemap/core/indexing/ndtree_index.h b/library/cpp/include/wavemap/core/indexing/ndtree_index.h index 825371a63..8dfacf060 100644 --- a/library/cpp/include/wavemap/core/indexing/ndtree_index.h +++ b/library/cpp/include/wavemap/core/indexing/ndtree_index.h @@ -3,6 +3,7 @@ #include #include +#include #include #include "wavemap/core/common.h" @@ -33,8 +34,10 @@ struct NdtreeIndex { Position position = Position::Zero(); NdtreeIndex() = default; - NdtreeIndex(Element height, Position position) + NdtreeIndex(Element height, const Position& position) : height(height), position(position) {} + NdtreeIndex(Element height, Position&& position) + : height(height), position(std::move(position)) {} bool operator==(const NdtreeIndex& other) const { return height == other.height && position == other.position; diff --git a/library/cpp/include/wavemap/core/integrator/projective/coarse_to_fine/hashed_chunked_wavelet_integrator.h b/library/cpp/include/wavemap/core/integrator/projective/coarse_to_fine/hashed_chunked_wavelet_integrator.h index 4c642cb27..a8ebabc56 100644 --- a/library/cpp/include/wavemap/core/integrator/projective/coarse_to_fine/hashed_chunked_wavelet_integrator.h +++ b/library/cpp/include/wavemap/core/integrator/projective/coarse_to_fine/hashed_chunked_wavelet_integrator.h @@ -35,7 +35,7 @@ class HashedChunkedWaveletIntegrator : public ProjectiveIntegrator { using OctreeType = HashedChunkedWaveletOctreeBlock::OctreeType; const HashedChunkedWaveletOctree::Ptr occupancy_map_; - std::shared_ptr thread_pool_; + const std::shared_ptr thread_pool_; std::shared_ptr range_image_intersector_; // Cache/pre-compute commonly used values @@ -68,7 +68,6 @@ class HashedChunkedWaveletIntegrator : public ProjectiveIntegrator { void updateNodeRecursive(OctreeType::NodeRefType node, const OctreeIndex& node_index, FloatingPoint& node_value, - OctreeType::ChunkType::BitRef node_has_child, bool& block_needs_thresholding); void updateLeavesBatch(const OctreeIndex& parent_index, FloatingPoint& parent_value, diff --git a/library/cpp/include/wavemap/core/integrator/projective/coarse_to_fine/hashed_wavelet_integrator.h b/library/cpp/include/wavemap/core/integrator/projective/coarse_to_fine/hashed_wavelet_integrator.h index 44d3b78fa..aac70459a 100644 --- a/library/cpp/include/wavemap/core/integrator/projective/coarse_to_fine/hashed_wavelet_integrator.h +++ b/library/cpp/include/wavemap/core/integrator/projective/coarse_to_fine/hashed_wavelet_integrator.h @@ -29,8 +29,11 @@ class HashedWaveletIntegrator : public ProjectiveIntegrator { : std::make_shared()) {} private: + using BlockList = std::vector; + using OctreeType = HashedWaveletOctreeBlock::OctreeType; + const HashedWaveletOctree::Ptr occupancy_map_; - std::shared_ptr thread_pool_; + const std::shared_ptr thread_pool_; std::shared_ptr range_image_intersector_; // Cache/pre-compute commonly used values @@ -51,13 +54,12 @@ class HashedWaveletIntegrator : public ProjectiveIntegrator { std::pair getFovMinMaxIndices( const Point3D& sensor_origin) const; - using BlockList = std::vector; void recursiveTester(const OctreeIndex& node_index, BlockList& update_job_list); void updateMap() override; void updateBlock(HashedWaveletOctree::Block& block, - const OctreeIndex& block_index); + const HashedWaveletOctree::BlockIndex& block_index); }; } // namespace wavemap diff --git a/library/cpp/include/wavemap/core/integrator/projective/coarse_to_fine/impl/hashed_wavelet_integrator_inl.h b/library/cpp/include/wavemap/core/integrator/projective/coarse_to_fine/impl/hashed_wavelet_integrator_inl.h index 47ac96f5d..279c90099 100644 --- a/library/cpp/include/wavemap/core/integrator/projective/coarse_to_fine/impl/hashed_wavelet_integrator_inl.h +++ b/library/cpp/include/wavemap/core/integrator/projective/coarse_to_fine/impl/hashed_wavelet_integrator_inl.h @@ -19,14 +19,14 @@ inline void HashedWaveletIntegrator::recursiveTester( // NOLINT if (node_index.height == tree_height_) { // Get the block if (update_type == UpdateType::kPossiblyOccupied) { - update_job_list.emplace_back(node_index); + update_job_list.emplace_back(node_index.position); return; } if (const auto* block = occupancy_map_->getBlock(node_index.position); block) { if (min_log_odds_ + kNoiseThreshold / 10.f <= block->getRootScale()) { // Add the block to the job list - update_job_list.emplace_back(node_index); + update_job_list.emplace_back(node_index.position); } } return; diff --git a/library/cpp/include/wavemap/core/map/hashed_chunked_wavelet_octree_block.h b/library/cpp/include/wavemap/core/map/hashed_chunked_wavelet_octree_block.h index 48cd6df0b..5c457414e 100644 --- a/library/cpp/include/wavemap/core/map/hashed_chunked_wavelet_octree_block.h +++ b/library/cpp/include/wavemap/core/map/hashed_chunked_wavelet_octree_block.h @@ -92,7 +92,7 @@ class HashedChunkedWaveletOctreeBlock { void recursiveThreshold(OctreeType::NodeRefType node, Coefficients::Scale& node_scale_coefficient); void recursivePrune( - HashedChunkedWaveletOctreeBlock::OctreeType::NodeRefType chunk); + HashedChunkedWaveletOctreeBlock::OctreeType::NodeRefType node); }; } // namespace wavemap diff --git a/library/cpp/include/wavemap/core/map/impl/hashed_chunked_wavelet_octree_block_inl.h b/library/cpp/include/wavemap/core/map/impl/hashed_chunked_wavelet_octree_block_inl.h index 9c55969c5..0cd6ae505 100644 --- a/library/cpp/include/wavemap/core/map/impl/hashed_chunked_wavelet_octree_block_inl.h +++ b/library/cpp/include/wavemap/core/map/impl/hashed_chunked_wavelet_octree_block_inl.h @@ -23,16 +23,12 @@ inline FloatingPoint HashedChunkedWaveletOctreeBlock::getCellValue( const MortonIndex morton_code = convert::nodeIndexToMorton(index); OctreeType::NodeConstPtrType node = &ndtree_.getRootNode(); FloatingPoint value = root_scale_coefficient_; - for (int parent_height = tree_height_; index.height < parent_height; + for (int parent_height = tree_height_; node && index.height < parent_height; --parent_height) { const NdtreeIndexRelativeChild child_index = OctreeIndex::computeRelativeChildIndex(morton_code, parent_height); value = Transform::backwardSingleChild({value, node->data()}, child_index); - auto child = node->getChild(child_index); - if (!child) { - break; - } - node = child; + node = node->getChild(child_index); } return value; } diff --git a/library/cpp/include/wavemap/core/map/impl/hashed_wavelet_octree_block_inl.h b/library/cpp/include/wavemap/core/map/impl/hashed_wavelet_octree_block_inl.h index 37d8389d6..cfbfeda34 100644 --- a/library/cpp/include/wavemap/core/map/impl/hashed_wavelet_octree_block_inl.h +++ b/library/cpp/include/wavemap/core/map/impl/hashed_wavelet_octree_block_inl.h @@ -22,14 +22,11 @@ inline FloatingPoint HashedWaveletOctreeBlock::getCellValue( const MortonIndex morton_code = convert::nodeIndexToMorton(index); OctreeType::NodeConstPtrType node = &ndtree_.getRootNode(); FloatingPoint value = root_scale_coefficient_; - for (int parent_height = tree_height_; index.height < parent_height; + for (int parent_height = tree_height_; node && index.height < parent_height; --parent_height) { const NdtreeIndexRelativeChild child_index = OctreeIndex::computeRelativeChildIndex(morton_code, parent_height); value = Transform::backwardSingleChild({value, node->data()}, child_index); - if (!node->hasChild(child_index)) { - break; - } node = node->getChild(child_index); } return value; diff --git a/library/cpp/include/wavemap/core/utils/undistortion/stamped_pose_buffer.h b/library/cpp/include/wavemap/core/utils/undistortion/stamped_pose_buffer.h index c146daabb..8cb13b966 100644 --- a/library/cpp/include/wavemap/core/utils/undistortion/stamped_pose_buffer.h +++ b/library/cpp/include/wavemap/core/utils/undistortion/stamped_pose_buffer.h @@ -11,7 +11,7 @@ struct StampedPose { Transformation3D pose{}; StampedPose() = default; - StampedPose(TimeAbsolute stamp, Transformation3D pose) + StampedPose(TimeAbsolute stamp, const Transformation3D& pose) : stamp(stamp), pose(pose) {} }; diff --git a/library/cpp/src/core/integrator/projective/coarse_to_fine/hashed_chunked_wavelet_integrator.cc b/library/cpp/src/core/integrator/projective/coarse_to_fine/hashed_chunked_wavelet_integrator.cc index 6452d9906..cc9709acc 100644 --- a/library/cpp/src/core/integrator/projective/coarse_to_fine/hashed_chunked_wavelet_integrator.cc +++ b/library/cpp/src/core/integrator/projective/coarse_to_fine/hashed_chunked_wavelet_integrator.cc @@ -78,17 +78,13 @@ void HashedChunkedWaveletIntegrator::updateBlock( bool block_needs_thresholding = block.getNeedsThresholding(); const OctreeIndex root_node_index{tree_height_, block_index}; updateNodeRecursive(block.getRootNode(), root_node_index, - block.getRootScale(), - block.getRootChunk().nodeHasAtLeastOneChild(0u), - block_needs_thresholding); + block.getRootScale(), block_needs_thresholding); block.setNeedsThresholding(block_needs_thresholding); } void HashedChunkedWaveletIntegrator::updateNodeRecursive( // NOLINT HashedChunkedWaveletIntegrator::OctreeType::NodeRefType node, const OctreeIndex& node_index, FloatingPoint& node_value, - HashedChunkedWaveletIntegrator::OctreeType::ChunkType::BitRef - node_has_child, bool& block_needs_thresholding) { // Decompress child values auto& node_details = node.data(); @@ -146,7 +142,6 @@ void HashedChunkedWaveletIntegrator::updateNodeRecursive( // NOLINT // Since the approximation error would still be too big, refine auto child_node = node.getOrAllocateChild(relative_child_idx); auto& child_details = child_node.data(); - auto child_has_child = child_node.hasAtLeastOneChild(); // If we're at the leaf level, directly compute the update if (child_index.height <= termination_height_ + 1) { @@ -154,13 +149,9 @@ void HashedChunkedWaveletIntegrator::updateNodeRecursive( // NOLINT } else { // Otherwise, recurse DCHECK_GE(child_index.height, 0); - updateNodeRecursive(child_node, child_index, child_value, child_has_child, + updateNodeRecursive(child_node, child_index, child_value, block_needs_thresholding); } - - if (child_has_child || data::is_nonzero(child_details)) { - node_has_child = true; - } } // Compress diff --git a/library/cpp/src/core/integrator/projective/coarse_to_fine/hashed_wavelet_integrator.cc b/library/cpp/src/core/integrator/projective/coarse_to_fine/hashed_wavelet_integrator.cc index 2297e33a2..7751b8ede 100644 --- a/library/cpp/src/core/integrator/projective/coarse_to_fine/hashed_wavelet_integrator.cc +++ b/library/cpp/src/core/integrator/projective/coarse_to_fine/hashed_wavelet_integrator.cc @@ -33,13 +33,13 @@ void HashedWaveletIntegrator::updateMap() { // Make sure the to-be-updated blocks are allocated for (const auto& block_index : blocks_to_update) { - occupancy_map_->getOrAllocateBlock(block_index.position); + occupancy_map_->getOrAllocateBlock(block_index); } // Update it with the threadpool for (const auto& block_index : blocks_to_update) { thread_pool_->add_task([this, block_index]() { - if (auto* block = occupancy_map_->getBlock(block_index.position)) { + if (auto* block = occupancy_map_->getBlock(block_index); block) { updateBlock(*block, block_index); } }); @@ -50,10 +50,9 @@ void HashedWaveletIntegrator::updateMap() { std::pair HashedWaveletIntegrator::getFovMinMaxIndices( const Point3D& sensor_origin) const { - const IndexElement height = - 1 + std::max(static_cast(std::ceil( - std::log2(config_.max_range / min_cell_width_))), - tree_height_); + const int height = 1 + std::max(static_cast(std::ceil(std::log2( + config_.max_range / min_cell_width_))), + tree_height_); const OctreeIndex fov_min_idx = convert::indexAndHeightToNodeIndex<3>( convert::pointToFloorIndex<3>( sensor_origin - Vector3D::Constant(config_.max_range), @@ -69,25 +68,28 @@ HashedWaveletIntegrator::getFovMinMaxIndices( return {fov_min_idx, fov_max_idx}; } -void HashedWaveletIntegrator::updateBlock(HashedWaveletOctree::Block& block, - const OctreeIndex& block_index) { +void HashedWaveletIntegrator::updateBlock( + HashedWaveletOctree::Block& block, + const HashedWaveletOctree::BlockIndex& block_index) { ProfilerZoneScoped; - HashedWaveletOctreeBlock::OctreeType::NodeRefType root_node = - block.getRootNode(); - HashedWaveletOctreeBlock::Coefficients::Scale& root_node_scale = - block.getRootScale(); block.setNeedsPruning(); block.setLastUpdatedStamp(); struct StackElement { - HashedWaveletOctreeBlock::OctreeType::NodeRefType parent_node; + OctreeType::NodeRefType parent_node; const OctreeIndex parent_node_index; NdtreeIndexRelativeChild next_child_idx; HashedWaveletOctreeBlock::Coefficients::CoefficientsArray child_scale_coefficients; }; std::stack stack; - stack.emplace(StackElement{root_node, block_index, 0, + + OctreeType::NodeRefType root_node = block.getRootNode(); + HashedWaveletOctreeBlock::Coefficients::Scale& root_node_scale = + block.getRootScale(); + stack.emplace(StackElement{root_node, + {tree_height_, block_index}, + 0, HashedWaveletOctreeBlock::Transform::backward( {root_node_scale, root_node.data()})}); @@ -117,8 +119,7 @@ void HashedWaveletIntegrator::updateBlock(HashedWaveletOctree::Block& block, DCHECK_GE(current_child_idx, 0); DCHECK_LT(current_child_idx, OctreeIndex::kNumChildren); - HashedWaveletOctreeBlock::OctreeType::NodeRefType parent_node = - stack.top().parent_node; + OctreeType::NodeRefType parent_node = stack.top().parent_node; FloatingPoint& node_value = stack.top().child_scale_coefficients[current_child_idx]; const OctreeIndex node_index = @@ -171,7 +172,7 @@ void HashedWaveletIntegrator::updateBlock(HashedWaveletOctree::Block& block, projection_model_->cartesianToSensorZ(C_node_center); const FloatingPoint bounding_sphere_radius = kUnitCubeHalfDiagonal * node_width; - HashedWaveletOctreeBlock::OctreeType::NodePtrType node = + OctreeType::NodePtrType node = parent_node.getChild(node_index.computeRelativeChildIndex()); if (measurement_model_->computeWorstCaseApproximationError( update_type, d_C_cell, bounding_sphere_radius) < diff --git a/library/cpp/src/core/map/hashed_chunked_wavelet_octree_block.cc b/library/cpp/src/core/map/hashed_chunked_wavelet_octree_block.cc index 29d87c8f0..b5600653a 100644 --- a/library/cpp/src/core/map/hashed_chunked_wavelet_octree_block.cc +++ b/library/cpp/src/core/map/hashed_chunked_wavelet_octree_block.cc @@ -189,8 +189,6 @@ void HashedChunkedWaveletOctreeBlock::recursivePrune( // NOLINT } } } - if (!has_at_least_one_child) { - node.hasAtLeastOneChild() = false; - } + node.hasAtLeastOneChild() = has_at_least_one_child; } } // namespace wavemap From 620f1619cceb77afca3cc8e43c3a0c48ab327a9d Mon Sep 17 00:00:00 2001 From: Victor Reijgwart Date: Thu, 21 Nov 2024 18:21:47 +0100 Subject: [PATCH 16/22] Tidy up constructors and moves vs copies --- .../cpp/include/wavemap/core/config/string_list.h | 13 +++++++++---- .../cpp/include/wavemap/core/data_structure/image.h | 5 ++++- .../wavemap/core/data_structure/pointcloud.h | 3 ++- .../wavemap/core/data_structure/posed_object.h | 2 +- .../include/wavemap/core/utils/print/container.h | 3 +-- 5 files changed, 17 insertions(+), 9 deletions(-) diff --git a/library/cpp/include/wavemap/core/config/string_list.h b/library/cpp/include/wavemap/core/config/string_list.h index 10e783193..e55a37a1a 100644 --- a/library/cpp/include/wavemap/core/config/string_list.h +++ b/library/cpp/include/wavemap/core/config/string_list.h @@ -15,10 +15,15 @@ struct StringList { // Constructors StringList() = default; - StringList(ValueType value) : value(std::move(value)) {} // NOLINT + StringList(const ValueType& value) : value(value) {} // NOLINT + StringList(ValueType&& value) : value(std::move(value)) {} // NOLINT // Assignment operator - StringList& operator=(ValueType rhs) { + StringList& operator=(const ValueType& rhs) { + value = rhs; + return *this; + } + StringList& operator=(ValueType&& rhs) { value = std::move(rhs); return *this; } @@ -29,8 +34,8 @@ struct StringList { bool operator!=(const StringList& rhs) const { return value != rhs.value; } // Allow implicit conversions to the underlying type - operator ValueType&() { return value; } - operator const ValueType&() const { return value; } + operator ValueType&() { return value; } // NOLINT + operator const ValueType&() const { return value; } // NOLINT // Method to load from configs static std::optional from(const param::Value& param); diff --git a/library/cpp/include/wavemap/core/data_structure/image.h b/library/cpp/include/wavemap/core/data_structure/image.h index 1573b9379..e4dc0c82d 100644 --- a/library/cpp/include/wavemap/core/data_structure/image.h +++ b/library/cpp/include/wavemap/core/data_structure/image.h @@ -25,7 +25,10 @@ class Image { PixelT initial_value = data::fill::zero()) : initial_value_(initial_value), data_(Data::Constant(num_rows, num_columns, initial_value)) {} - explicit Image(Data data, PixelT initial_value = data::fill::zero()) + explicit Image(const Data& data, + PixelT initial_value = data::fill::zero()) + : initial_value_(initial_value), data_(data) {} + explicit Image(Data&& data, PixelT initial_value = data::fill::zero()) : initial_value_(initial_value), data_(std::move(data)) {} bool empty() const { return !size(); } diff --git a/library/cpp/include/wavemap/core/data_structure/pointcloud.h b/library/cpp/include/wavemap/core/data_structure/pointcloud.h index cb08090cd..fcf731555 100644 --- a/library/cpp/include/wavemap/core/data_structure/pointcloud.h +++ b/library/cpp/include/wavemap/core/data_structure/pointcloud.h @@ -20,7 +20,8 @@ class Pointcloud { using Data = Eigen::Matrix; Pointcloud() = default; - explicit Pointcloud(Data pointcloud) : data_(std::move(pointcloud)) {} + explicit Pointcloud(const Data& pointcloud) : data_(pointcloud) {} + explicit Pointcloud(Data&& pointcloud) : data_(std::move(pointcloud)) {} template explicit Pointcloud(const PointContainer& point_container) { diff --git a/library/cpp/include/wavemap/core/data_structure/posed_object.h b/library/cpp/include/wavemap/core/data_structure/posed_object.h index d16ae7891..826c0de32 100644 --- a/library/cpp/include/wavemap/core/data_structure/posed_object.h +++ b/library/cpp/include/wavemap/core/data_structure/posed_object.h @@ -16,7 +16,7 @@ class PosedObject : public ObjectT { using ObjectT::ObjectT; template - explicit PosedObject(Transformation3D T_W_C, Args... args) + explicit PosedObject(const Transformation3D& T_W_C, Args... args) : ObjectT(std::forward(args)...) { setPose(T_W_C); } diff --git a/library/cpp/include/wavemap/core/utils/print/container.h b/library/cpp/include/wavemap/core/utils/print/container.h index b98f1f390..606d069d3 100644 --- a/library/cpp/include/wavemap/core/utils/print/container.h +++ b/library/cpp/include/wavemap/core/utils/print/container.h @@ -24,8 +24,7 @@ inline std::string sequence(const SequenceT& sequence, return std::accumulate(std::next(sequence.cbegin()), sequence.cend(), print::element(sequence[0]), [separator](auto str, const auto& el) -> std::string { - return std::move(str) + separator + - print::element(el); + return str + separator + print::element(el); }); } } // namespace wavemap::print From 3de032394f45fb841d78b8036aaab2c52c9a6238 Mon Sep 17 00:00:00 2001 From: Victor Reijgwart Date: Thu, 21 Nov 2024 18:39:09 +0100 Subject: [PATCH 17/22] Attempt to address 'maybe uninitialized' warnings from GCC >= 11 --- .../chunked_ndtree_node_address.h | 7 +++++-- .../src/core/utils/query/query_accelerator.cc | 20 ++++--------------- 2 files changed, 9 insertions(+), 18 deletions(-) diff --git a/library/cpp/include/wavemap/core/data_structure/chunked_ndtree/chunked_ndtree_node_address.h b/library/cpp/include/wavemap/core/data_structure/chunked_ndtree/chunked_ndtree_node_address.h index 3f19474b6..eeb2e335f 100644 --- a/library/cpp/include/wavemap/core/data_structure/chunked_ndtree/chunked_ndtree_node_address.h +++ b/library/cpp/include/wavemap/core/data_structure/chunked_ndtree/chunked_ndtree_node_address.h @@ -31,13 +31,16 @@ class ChunkedNdtreeNodePtr { // Emulate pointer semantics void reset() { node_.reset(); } - operator bool() const { return node_.has_value(); } // NOLINT NodeRef operator*() const { return node_.value(); } NodeRef* operator->() { return node_.operator->(); } const NodeRef* operator->() const { return node_.operator->(); } + // Emulate null check semantics + operator bool() const { return node_.has_value(); } // NOLINT + bool operator==(nullptr_t) noexcept { return !node_.has_value(); } + private: - std::optional node_; + std::optional node_{}; }; template diff --git a/library/cpp/src/core/utils/query/query_accelerator.cc b/library/cpp/src/core/utils/query/query_accelerator.cc index c5de07565..35fb17c0b 100644 --- a/library/cpp/src/core/utils/query/query_accelerator.cc +++ b/library/cpp/src/core/utils/query/query_accelerator.cc @@ -56,17 +56,14 @@ FloatingPoint QueryAccelerator::getCellValue( // Load the node at height_ if it was not yet loaded last time if (previous_height != tree_height_ && height_ == previous_height) { - NodePtrType parent_node = node_stack_[height_ + 1]; + NodePtrType parent_node = CHECK_NOTNULL(node_stack_[height_ + 1]); const NdtreeIndexRelativeChild child_index = OctreeIndex::computeRelativeChildIndex(morton_code_, height_ + 1); - if (!parent_node->hasChild(child_index)) { - return value_stack_[height_]; - } node_stack_[height_] = parent_node->getChild(child_index); } // Walk down the tree from height_ to index.height - while (true) { + while (node_stack_[height_] && height_ != index.height) { NodePtrType parent_node = node_stack_[height_]; const FloatingPoint parent_value = value_stack_[height_]; const NdtreeIndexRelativeChild child_idx = @@ -75,9 +72,6 @@ FloatingPoint QueryAccelerator::getCellValue( value_stack_[height_] = HashedWaveletOctree::Block::Transform::backwardSingleChild( {parent_value, parent_node->data()}, child_idx); - if (height_ == index.height || !parent_node->hasChild(child_idx)) { - break; - } node_stack_[height_] = parent_node->getChild(child_idx); } @@ -137,17 +131,14 @@ FloatingPoint QueryAccelerator::getCellValue( // Load the node at height_ if it was not yet loaded last time if (previous_height != tree_height_ && height_ == previous_height) { - NodePtrType parent_node = node_stack_[height_ + 1]; + NodePtrType parent_node = CHECK_NOTNULL(node_stack_[height_ + 1]); const NdtreeIndexRelativeChild child_index = OctreeIndex::computeRelativeChildIndex(morton_code_, height_ + 1); - if (!parent_node->hasChild(child_index)) { - return value_stack_[height_]; - } node_stack_[height_] = parent_node->getChild(child_index); } // Walk down the tree from height_ to index.height - while (true) { + while (node_stack_[height_] && height_ != index.height) { NodePtrType parent_node = node_stack_[height_]; const FloatingPoint parent_value = value_stack_[height_]; const NdtreeIndexRelativeChild child_idx = @@ -156,9 +147,6 @@ FloatingPoint QueryAccelerator::getCellValue( value_stack_[height_] = HashedWaveletOctree::Block::Transform::backwardSingleChild( {parent_value, parent_node->data()}, child_idx); - if (height_ == index.height || !parent_node->hasChild(child_idx)) { - break; - } node_stack_[height_] = parent_node->getChild(child_idx); } From d7a20b3160bcfbd0a50d68b590cfbf20ec884fa7 Mon Sep 17 00:00:00 2001 From: Victor Reijgwart Date: Thu, 21 Nov 2024 19:25:12 +0100 Subject: [PATCH 18/22] Switch from `nullptr_t` to `std::nullptr_t` type for portability --- .../data_structure/chunked_ndtree/chunked_ndtree_node_address.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/library/cpp/include/wavemap/core/data_structure/chunked_ndtree/chunked_ndtree_node_address.h b/library/cpp/include/wavemap/core/data_structure/chunked_ndtree/chunked_ndtree_node_address.h index eeb2e335f..716573a79 100644 --- a/library/cpp/include/wavemap/core/data_structure/chunked_ndtree/chunked_ndtree_node_address.h +++ b/library/cpp/include/wavemap/core/data_structure/chunked_ndtree/chunked_ndtree_node_address.h @@ -37,7 +37,7 @@ class ChunkedNdtreeNodePtr { // Emulate null check semantics operator bool() const { return node_.has_value(); } // NOLINT - bool operator==(nullptr_t) noexcept { return !node_.has_value(); } + bool operator==(std::nullptr_t) noexcept { return !node_.has_value(); } private: std::optional node_{}; From 75f4edf1a8b46a4a0354cce80872d0a406860453 Mon Sep 17 00:00:00 2001 From: Victor Reijgwart Date: Fri, 22 Nov 2024 17:02:26 +0100 Subject: [PATCH 19/22] Reintroduce the option to profile with Tracy --- interfaces/ros1/wavemap/CMakeLists.txt | 4 ++-- library/cpp/CMakeLists.txt | 1 + library/cpp/cmake/find-wavemap-deps.cmake | 12 +++++++++++- library/cpp/cmake/wavemap-extras.cmake | 5 +++++ library/cpp/src/core/CMakeLists.txt | 4 ++-- 5 files changed, 21 insertions(+), 5 deletions(-) diff --git a/interfaces/ros1/wavemap/CMakeLists.txt b/interfaces/ros1/wavemap/CMakeLists.txt index 6c157c5a2..996b3fdb3 100644 --- a/interfaces/ros1/wavemap/CMakeLists.txt +++ b/interfaces/ros1/wavemap/CMakeLists.txt @@ -27,8 +27,8 @@ add_subdirectory(${CMAKE_CURRENT_SOURCE_DIR}/../../../library/cpp # Include the library's tests in catkin's run_tests target include(cmake/append_to_catkin_tests.cmake) -append_to_catkin_tests(test_wavemap_io) append_to_catkin_tests(test_wavemap_core) +append_to_catkin_tests(test_wavemap_io) # Support installs # Mark targets for installation @@ -38,6 +38,6 @@ install(TARGETS wavemap_core wavemap_io wavemap_pipeline RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) # Mark headers for installation install(DIRECTORY - ${CMAKE_CURRENT_SOURCE_DIR}/../../../library/cpp/include/wavemap/ + "${CMAKE_CURRENT_SOURCE_DIR}/../../../library/cpp/include/wavemap/" DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} FILES_MATCHING PATTERN "*.h") diff --git a/library/cpp/CMakeLists.txt b/library/cpp/CMakeLists.txt index 39c81ec8d..400cf4c41 100644 --- a/library/cpp/CMakeLists.txt +++ b/library/cpp/CMakeLists.txt @@ -11,6 +11,7 @@ option(BUILD_SHARED_LIBS option(USE_SYSTEM_EIGEN "Use system pre-installed Eigen" ON) option(USE_SYSTEM_GLOG "Use system pre-installed glog" ON) option(USE_SYSTEM_BOOST "Use system pre-installed Boost" ON) +option(TRACY_ENABLE "Enable profiling with Tracy" OFF) # CMake helpers and general wavemap tooling (e.g. to run clang-tidy CI) include(GNUInstallDirs) diff --git a/library/cpp/cmake/find-wavemap-deps.cmake b/library/cpp/cmake/find-wavemap-deps.cmake index 695f0c98e..195945ec3 100644 --- a/library/cpp/cmake/find-wavemap-deps.cmake +++ b/library/cpp/cmake/find-wavemap-deps.cmake @@ -55,4 +55,14 @@ else () endif () # Optional dependencies -find_package(tracy QUIET) +if (TRACY_ENABLE) + find_package(Tracy QUIET) + if (NOT Tracy_FOUND) + message(FATAL_ERROR + "Profiling is enabled (TRACY_ENABLE=ON), but Tracy is not available. " + "Please switch it to OFF or install Tracy as a system package with: " + "cmake -S . -B build -DTRACY_ENABLE=ON -DBUILD_SHARED_LIBS=true " + "-DCMAKE_BUILD_TYPE=Release && cmake --build build --parallel && " + "sudo make install") + endif () +endif () diff --git a/library/cpp/cmake/wavemap-extras.cmake b/library/cpp/cmake/wavemap-extras.cmake index 33aa2e3cb..2cdae7cdf 100644 --- a/library/cpp/cmake/wavemap-extras.cmake +++ b/library/cpp/cmake/wavemap-extras.cmake @@ -38,6 +38,11 @@ function(set_wavemap_target_properties target) # General C++ defines target_compile_definitions(${target} PUBLIC EIGEN_INITIALIZE_MATRICES_BY_NAN) + # Profiling related C++ defines + if (TRACY_ENABLE) + target_compile_definitions(${target} PUBLIC TRACY_ENABLE) + endif () + # Conditional compilation options if (DCHECK_ALWAYS_ON) target_compile_definitions(${target} PUBLIC DCHECK_ALWAYS_ON) diff --git a/library/cpp/src/core/CMakeLists.txt b/library/cpp/src/core/CMakeLists.txt index a48abab05..00f39baa6 100644 --- a/library/cpp/src/core/CMakeLists.txt +++ b/library/cpp/src/core/CMakeLists.txt @@ -7,8 +7,8 @@ set_wavemap_target_properties(wavemap_core) add_wavemap_include_directories(wavemap_core) target_link_libraries(wavemap_core PUBLIC Eigen3::Eigen glog Boost::preprocessor) -if (tracy_FOUND) - target_link_libraries(wavemap_core PUBLIC TracyClient) +if (Tracy_FOUND) + target_link_libraries(wavemap_core PUBLIC TracyClient dl) endif () # Set sources From 57ad3bc550418f35f8d528d73e3783bee16e2371 Mon Sep 17 00:00:00 2001 From: Victor Reijgwart Date: Mon, 25 Nov 2024 11:07:07 +0100 Subject: [PATCH 20/22] Revert "Reintroduce the option to profile with Tracy" This reverts commit 75f4edf1a8b46a4a0354cce80872d0a406860453, as we believe its changes are not yet ready for release. Tracy is configured through CMake options, part of which are converted to C++ compile definitions. Since they affect both Tracy's binary and headers, the same options/definitions must be set when compiling Tracy's system library, wavemap C++ library (CMake), and its ROS1 interface (catkin). A complicating factor is that catkin ignores standard CMake features like inheriting a linked library's PUBLIC definitions. We will reintroduce Tracy-support once we find a cleaner, less error-prone way to integrate it into the project. --- interfaces/ros1/wavemap/CMakeLists.txt | 4 ++-- library/cpp/CMakeLists.txt | 1 - library/cpp/cmake/find-wavemap-deps.cmake | 12 +----------- library/cpp/cmake/wavemap-extras.cmake | 5 ----- library/cpp/src/core/CMakeLists.txt | 4 ++-- 5 files changed, 5 insertions(+), 21 deletions(-) diff --git a/interfaces/ros1/wavemap/CMakeLists.txt b/interfaces/ros1/wavemap/CMakeLists.txt index 996b3fdb3..6c157c5a2 100644 --- a/interfaces/ros1/wavemap/CMakeLists.txt +++ b/interfaces/ros1/wavemap/CMakeLists.txt @@ -27,8 +27,8 @@ add_subdirectory(${CMAKE_CURRENT_SOURCE_DIR}/../../../library/cpp # Include the library's tests in catkin's run_tests target include(cmake/append_to_catkin_tests.cmake) -append_to_catkin_tests(test_wavemap_core) append_to_catkin_tests(test_wavemap_io) +append_to_catkin_tests(test_wavemap_core) # Support installs # Mark targets for installation @@ -38,6 +38,6 @@ install(TARGETS wavemap_core wavemap_io wavemap_pipeline RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) # Mark headers for installation install(DIRECTORY - "${CMAKE_CURRENT_SOURCE_DIR}/../../../library/cpp/include/wavemap/" + ${CMAKE_CURRENT_SOURCE_DIR}/../../../library/cpp/include/wavemap/ DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} FILES_MATCHING PATTERN "*.h") diff --git a/library/cpp/CMakeLists.txt b/library/cpp/CMakeLists.txt index 400cf4c41..39c81ec8d 100644 --- a/library/cpp/CMakeLists.txt +++ b/library/cpp/CMakeLists.txt @@ -11,7 +11,6 @@ option(BUILD_SHARED_LIBS option(USE_SYSTEM_EIGEN "Use system pre-installed Eigen" ON) option(USE_SYSTEM_GLOG "Use system pre-installed glog" ON) option(USE_SYSTEM_BOOST "Use system pre-installed Boost" ON) -option(TRACY_ENABLE "Enable profiling with Tracy" OFF) # CMake helpers and general wavemap tooling (e.g. to run clang-tidy CI) include(GNUInstallDirs) diff --git a/library/cpp/cmake/find-wavemap-deps.cmake b/library/cpp/cmake/find-wavemap-deps.cmake index 195945ec3..695f0c98e 100644 --- a/library/cpp/cmake/find-wavemap-deps.cmake +++ b/library/cpp/cmake/find-wavemap-deps.cmake @@ -55,14 +55,4 @@ else () endif () # Optional dependencies -if (TRACY_ENABLE) - find_package(Tracy QUIET) - if (NOT Tracy_FOUND) - message(FATAL_ERROR - "Profiling is enabled (TRACY_ENABLE=ON), but Tracy is not available. " - "Please switch it to OFF or install Tracy as a system package with: " - "cmake -S . -B build -DTRACY_ENABLE=ON -DBUILD_SHARED_LIBS=true " - "-DCMAKE_BUILD_TYPE=Release && cmake --build build --parallel && " - "sudo make install") - endif () -endif () +find_package(tracy QUIET) diff --git a/library/cpp/cmake/wavemap-extras.cmake b/library/cpp/cmake/wavemap-extras.cmake index 2cdae7cdf..33aa2e3cb 100644 --- a/library/cpp/cmake/wavemap-extras.cmake +++ b/library/cpp/cmake/wavemap-extras.cmake @@ -38,11 +38,6 @@ function(set_wavemap_target_properties target) # General C++ defines target_compile_definitions(${target} PUBLIC EIGEN_INITIALIZE_MATRICES_BY_NAN) - # Profiling related C++ defines - if (TRACY_ENABLE) - target_compile_definitions(${target} PUBLIC TRACY_ENABLE) - endif () - # Conditional compilation options if (DCHECK_ALWAYS_ON) target_compile_definitions(${target} PUBLIC DCHECK_ALWAYS_ON) diff --git a/library/cpp/src/core/CMakeLists.txt b/library/cpp/src/core/CMakeLists.txt index 00f39baa6..a48abab05 100644 --- a/library/cpp/src/core/CMakeLists.txt +++ b/library/cpp/src/core/CMakeLists.txt @@ -7,8 +7,8 @@ set_wavemap_target_properties(wavemap_core) add_wavemap_include_directories(wavemap_core) target_link_libraries(wavemap_core PUBLIC Eigen3::Eigen glog Boost::preprocessor) -if (Tracy_FOUND) - target_link_libraries(wavemap_core PUBLIC TracyClient dl) +if (tracy_FOUND) + target_link_libraries(wavemap_core PUBLIC TracyClient) endif () # Set sources From 9c9554de29dd7be243467150fd6d80cbb35797a7 Mon Sep 17 00:00:00 2001 From: Victor Reijgwart Date: Mon, 25 Nov 2024 22:01:55 +0100 Subject: [PATCH 21/22] Minor release management script improvements --- tooling/scripts/prepare_release.py | 398 +++++++++++++++++------------ 1 file changed, 229 insertions(+), 169 deletions(-) diff --git a/tooling/scripts/prepare_release.py b/tooling/scripts/prepare_release.py index d34b8c63f..6abf79382 100755 --- a/tooling/scripts/prepare_release.py +++ b/tooling/scripts/prepare_release.py @@ -1,145 +1,145 @@ #!/usr/bin/env python3 - +from typing import Optional, List, AnyStr, Iterable from dataclasses import dataclass from enum import Enum -from typing import List, AnyStr import os import re import argparse -import xml.etree.ElementTree as et +import subprocess from datetime import datetime +import xml.etree.ElementTree as et import git +# Types of packages we support +class PkgType(Enum): + + def __str__(self): + return { + PkgType.CPP: 'C++', + PkgType.PYTHON_BINDINGS: 'Python', + PkgType.ROS1: 'ROS1', + PkgType.PYTHON: 'Python' + }[self] + + CPP = 1 + PYTHON_BINDINGS = 2 + ROS1 = 3 + PYTHON = 4 + + +# Class used to specify a package in our repository +@dataclass +class Pkg: + name: AnyStr + type: PkgType + current_path: AnyStr + old_paths: List[AnyStr] + + +# Parameters +pkgs = { + 'libraries': [ + Pkg('wavemap', PkgType.CPP, 'library/cpp', []), + Pkg('pywavemap', PkgType.PYTHON_BINDINGS, 'library/python', []) + ], + 'interfaces': [ + Pkg('wavemap', PkgType.ROS1, 'interfaces/ros1/wavemap', []), + Pkg('wavemap_msgs', PkgType.ROS1, 'interfaces/ros1/wavemap_msgs', []), + Pkg('wavemap_ros_conversions', PkgType.ROS1, + 'interfaces/ros1/wavemap_ros_conversions', []), + Pkg('wavemap_ros', PkgType.ROS1, 'interfaces/ros1/wavemap_ros', []), + Pkg('wavemap_rviz_plugin', PkgType.ROS1, + 'interfaces/ros1/wavemap_rviz_plugin', []), + Pkg('wavemap_all', PkgType.ROS1, 'interfaces/ros1/wavemap_all', []) + ], + 'helpers': [ + Pkg('catkin_setup', PkgType.ROS1, 'tooling/packages/catkin_setup', []), + ], + 'examples': [ + Pkg('wavemap_examples_cpp', PkgType.CPP, 'examples/cpp', []), + Pkg('wavemap_examples_ros1', PkgType.ROS1, 'examples/ros1', []), + Pkg('wavemap_examples_python', PkgType.PYTHON, 'examples/python', []) + ] +} + + # Filter and convert tags to version numbers -def extract_version(tag_name): +def extract_version_numbers(version: str) -> List[int]: # Strip 'v' prefix if present - version_str = tag_name.lstrip('v') + version = version.lstrip('v') # Match the major, minor and patch number - match = re.match(r'^(\d+)\.(\d+)\.(\d+)$', version_str) + match = re.match(r'^(\d+)\.(\d+)\.(\d+)$', version) if match is None: raise ValueError(f'Invalid version string, ' - f'must be int.int.int: "{version_str}"') + f'must be int.int.int: "{version}"') # Cast all numbers to integers version = [int(x) for x in match.groups()] return version +# Increment a version number by a given bump level +def bump_version(version: str, level='patch') -> str: + # Extract version number + bumped_version = extract_version_numbers(version) + # Increment the appropriate number + bump_idx = {'major': 0, 'minor': 1, 'patch': 2}[level] + bumped_version[bump_idx] += 1 + # Reset the trailing numbers + bumped_version = [ + x if idx <= bump_idx else 0 for idx, x in enumerate(bumped_version) + ] + return '.'.join([str(x) for x in bumped_version]) + + # Find the tag with the highest version number in a list of git tags -def extract_highest_version(tag_list): +def find_highest_version(tag_list) -> Optional[List[int]]: # Filter out tags that don't have valid versions versioned_tags = [] for tag in tag_list: try: - version = extract_version(tag.name) - versioned_tags.append((tag, version)) + version = extract_version_numbers(tag.name) + versioned_tags.append(version) except ValueError: pass # Sort tags by their version numbers - versioned_tags.sort(key=lambda x: x[1], reverse=True) + versioned_tags.sort(reverse=True) # Return the highest tag or None - return versioned_tags[0][0] if versioned_tags else None - - -# Increment a version number by a given bump level -def bump_version(version, level='patch'): - # Extract version number - bumped_version = extract_version(version) - # Increment appropriate part - bump_idx = {'major': 0, 'minor': 1, 'patch': 2}[level] - bumped_version[bump_idx] += 1 - # Reset trailing parts - bumped_version = [ - x if idx <= bump_idx else 0 for idx, x in enumerate(bumped_version) - ] - return bumped_version + return versioned_tags[0] if versioned_tags else None -# Types of packages we support -class PkgType(Enum): +def version_from_cmake_project(project_root_path: str) -> Optional[str]: + cmakelists_path = os.path.join(project_root_path, 'CMakeLists.txt') + if os.path.exists(cmakelists_path): + # Read the existing content of the CMakeLists.txt file + with open(cmakelists_path, 'r', encoding='utf-8') as file: + cmake_content = file.read() - def __str__(self): - return { - PkgType.CPP: "C++", - PkgType.PYTHON_BINDINGS: "Python", - PkgType.ROS1: "ROS1", - PkgType.PYTHON: "Python" - }[self] - - CPP = 1 - PYTHON_BINDINGS = 2 - ROS1 = 3 - PYTHON = 4 + # Replace the old version number with the new version number + pattern = re.compile( + r'(project\(wavemap\s+VERSION)(\s+\d+\.\d+\.\d+\s+)') + matches = pattern.findall(cmake_content) + # Make the replacement was successful and unique + if len(matches) == 1 and len(matches[0]) == 2: + return matches[0][1].strip() -# Class used to specify a package in our repository -@dataclass -class Pkg: - name: AnyStr - type: PkgType - current_path: AnyStr - old_paths: List[AnyStr] + print(f'Failed to find version number in {cmakelists_path}') + else: + print(f'Failed to find {cmakelists_path}') + return None -def draft_release_notes(): - out = "# Summary\n" - out += "...\n" - out += "\n" - out += "### Detailed description\n" - out += "...\n" - out += "\n" - out += "# Package changelogs\n" - out += "\n" - - out += "### Libraries\n" - for pkg in pkgs["libraries"]: - out += f"* [{pkg.type}](https://github.com/ethz-asl/wavemap/blob/" - out += f"v{new_version_str}/{pkg.current_path}/CHANGELOG.rst)\n" - out += "\n" - - out += "### Interfaces\n" - out += "* ROS1\n" - for pkg in pkgs["interfaces"]: - out += f" * [{pkg.name}](https://github.com/ethz-asl/wavemap/blob/" - out += f"v{new_version_str}/{pkg.current_path}/CHANGELOG.rst)\n" - out += "\n" - - out += "### Examples\n" - for pkg in pkgs["examples"]: - out += f"* [{pkg.type}](https://github.com/ethz-asl/wavemap/blob/" - out += f"v{new_version_str}/{pkg.current_path}/CHANGELOG.rst)\n" - out += "\n" - - out += "# Upgrade notes\n" - out += "Upgrade instructions for\n" - out += "* C++ Library\n" - out += " * To use wavemap as a standalone CMake project, please see " - out += "[these instructions]" - out += "(https://ethz-asl.github.io/wavemap/pages/installation/cpp)\n" - out += "* Python Library\n" - out += " * To install wavemap's Python API, please see " - out += "[these instructions]" - out += "(https://ethz-asl.github.io/wavemap/pages/installation/python)\n" - out += "* ROS1\n" - out += " * Catkin\n" - out += " * Go to your catkin workspace src directory: " - out += "`cd ~/catkin_ws/src`\n" - out += " * Pull the newest wavemap code:" - out += "`cd wavemap && git checkout main && git pull`\n" - out += " * Rebuild wavemap: `catkin build wavemap_all`\n" - out += " * Docker\n" - out += " * `docker build --tag=wavemap_ros1 " - out += f"--build-arg=\"VERSION=v{new_version_str}\" -<<< $(curl -s https://" - out += f"raw.githubusercontent.com/ethz-asl/wavemap/v{new_version_str}" - out += "/tooling/docker/ros1/incremental.Dockerfile)`\n\n" - out += "For more info, see our guides on [installing wavemap](https://" - out += "ethz-asl.github.io/wavemap/pages/installation)." - out += "\n" - print(out) +def version_from_git_tags( + tag_list: Iterable[git.TagReference]) -> Optional[str]: + most_recent_version = find_highest_version(tag_list) + if most_recent_version and isinstance(most_recent_version, list): + return '.'.join(map(str, most_recent_version)) + return None -def prepare_release_files(): +def update_release_files(previous_version: str, new_version: str) -> None: # Generate the changelogs for each package in our repo packages = [pk for key, value in pkgs.items() for pk in value] for pkg in packages: @@ -148,10 +148,10 @@ def prepare_release_files(): pkg_all_paths = pkg.old_paths + [pkg.current_path] print(f'Processing {pkg_debug_name}') - pkg_changelog_path = os.path.join(pkg.current_path, "CHANGELOG.rst") + pkg_changelog_path = os.path.join(pkg.current_path, 'CHANGELOG.rst') if os.path.exists(pkg_changelog_path): # Read the package's changelog - with open(pkg_changelog_path, "r") as f: + with open(pkg_changelog_path, 'r') as f: changelog = f.readlines() # Check the title to ensure we're editing the right file @@ -164,12 +164,12 @@ def prepare_release_files(): raise SystemExit # Generate the title for the new section - date_str = datetime.now().strftime("%Y-%m-%d") - section_title = f'{new_version_str} ({date_str})' - section_title_underline = "-" * len(section_title) + date_str = datetime.now().strftime('%Y-%m-%d') + section_title = f'{new_version} ({date_str})' + section_title_underline = '-' * len(section_title) # Generate an overview of the current changes and contributors - pkg_commits = repo.iter_commits(rev=f'{most_recent_release}..HEAD', + pkg_commits = repo.iter_commits(rev=f'v{previous_version}..HEAD', paths=pkg_all_paths) commit_msgs = [] authors = set() @@ -191,8 +191,8 @@ def prepare_release_files(): changelog.insert(7, section_contributors + 2 * os.linesep) # Write the updated content back to the file - with open(pkg_changelog_path, "w") as f: - changelog = "".join(changelog) + with open(pkg_changelog_path, 'w') as f: + changelog = ''.join(changelog) f.write(changelog) else: @@ -200,7 +200,7 @@ def prepare_release_files(): raise SystemExit if pkg.type in (PkgType.CPP, PkgType.PYTHON_BINDINGS): - pkg_cmake_path = os.path.join(pkg.current_path, "CMakeLists.txt") + pkg_cmake_path = os.path.join(pkg.current_path, 'CMakeLists.txt') if os.path.exists(pkg_cmake_path): # Read the existing content of the CMakeLists.txt file with open(pkg_cmake_path, 'r', encoding='utf-8') as file: @@ -209,13 +209,13 @@ def prepare_release_files(): # Replace the old version number with the new version number pattern = re.compile(r'(project\(' + pkg.name + r'\s+VERSION)(\s+\d+\.\d+\.\d+\s+)') - substitution = r'\1 ' + new_version_str + r' ' + substitution = r'\1 ' + new_version + r' ' new_content, count = pattern.subn(substitution, cmake_content) # Make the replacement was successful and unique if count == 0 or 1 < count: - print("Failed to update version number in " - f"{pkg_cmake_path}") + print('Failed to update version number in ' + f'{pkg_cmake_path}') raise SystemExit # Write the updated content back to the file @@ -228,7 +228,7 @@ def prepare_release_files(): if pkg.type == PkgType.PYTHON_BINDINGS: pyproject_toml_path = os.path.join(pkg.current_path, - "pyproject.toml") + 'pyproject.toml') if os.path.exists(pyproject_toml_path): # Read the existing content of the CMakeLists.txt file with open(pyproject_toml_path, 'r', encoding='utf-8') as file: @@ -237,13 +237,13 @@ def prepare_release_files(): # Replace the old version number with the new version number pattern = re.compile( r'(version\s+=\s+)(\"\d+\.\d+\.\d+\")(.*)') - substitution = r'\1"' + new_version_str + r'"\3' + substitution = r'\1"' + new_version + r'"\3' new_content, count = pattern.subn(substitution, cmake_content) # Make the replacement was successful and unique if count == 0 or 1 < count: - print("Failed to update version number in " - f"{pyproject_toml_path}") + print('Failed to update version number in ' + f'{pyproject_toml_path}') raise SystemExit # Write the updated content back to the file @@ -255,7 +255,7 @@ def prepare_release_files(): raise SystemExit if pkg.type == PkgType.ROS1: - pkg_xml_path = os.path.join(pkg.current_path, "package.xml") + pkg_xml_path = os.path.join(pkg.current_path, 'package.xml') if os.path.exists(pkg_xml_path): # Parse the XML file tree = et.parse(pkg_xml_path) @@ -272,7 +272,7 @@ def prepare_release_files(): version_tag = version_tags[0] # Update the version tag and save the changes - version_tag.text = new_version_str + version_tag.text = new_version tree.write(pkg_xml_path, encoding='utf-8', xml_declaration=True) @@ -280,69 +280,129 @@ def prepare_release_files(): print(f'Could NOT find package.xml for {pkg_debug_name}') raise SystemExit + print("\nRunning pre-commit to ensure consistent formatting:") + subprocess.run(['pre-commit', 'run', '--all-files']) + + +def create_release_tag(version: str) -> None: + tag = f'v{version}' + message = f'Release v{version}' + repo.create_tag(tag, message=message) + + +def draft_release_notes(version: str) -> None: + out = '# Summary\n' + out += '...\n' + out += '\n' + out += '### Detailed description\n' + out += '...\n' + out += '\n' + out += '# Package changelogs\n' + out += '\n' + + out += '### Libraries\n' + for pkg in pkgs['libraries']: + out += f'* [{pkg.type}](https://github.com/ethz-asl/wavemap/blob/' + out += f'v{version}/{pkg.current_path}/CHANGELOG.rst)\n' + out += '\n' + + out += '### Interfaces\n' + out += '* ROS1\n' + for pkg in pkgs['interfaces']: + out += f' * [{pkg.name}](https://github.com/ethz-asl/wavemap/blob/' + out += f'v{version}/{pkg.current_path}/CHANGELOG.rst)\n' + out += '\n' + + out += '### Examples\n' + for pkg in pkgs['examples']: + out += f'* [{pkg.type}](https://github.com/ethz-asl/wavemap/blob/' + out += f'v{version}/{pkg.current_path}/CHANGELOG.rst)\n' + out += '\n' + + out += '# Upgrade notes\n' + out += 'Upgrade instructions for\n' + out += '* C++ Library\n' + out += ' * To use wavemap as a standalone CMake project, please see ' + out += '[these instructions]' + out += '(https://ethz-asl.github.io/wavemap/pages/installation/cpp)\n' + out += '* Python Library\n' + out += ' * To install wavemap\'s Python API, please see ' + out += '[these instructions]' + out += '(https://ethz-asl.github.io/wavemap/pages/installation/python)\n' + out += '* ROS1\n' + out += ' * Catkin\n' + out += ' * Go to your catkin workspace src directory: ' + out += '`cd ~/catkin_ws/src`\n' + out += ' * Pull the newest wavemap code:' + out += '`cd wavemap && git checkout main && git pull`\n' + out += ' * Rebuild wavemap: `catkin build wavemap_all`\n' + out += ' * Docker\n' + out += ' * `docker build --tag=wavemap_ros1 ' + out += f'--build-arg=\'VERSION=v{version}\' -<<< $(curl -s https://' + out += f'raw.githubusercontent.com/ethz-asl/wavemap/v{version}' + out += '/tooling/docker/ros1/incremental.Dockerfile)`\n\n' + out += 'For more info, see our guides on [installing wavemap](https://' + out += 'ethz-asl.github.io/wavemap/pages/installation).' + out += '\n' + + print(out) -# Parameters -pkgs = { - "libraries": [ - Pkg('wavemap', PkgType.CPP, 'library/cpp', []), - Pkg('pywavemap', PkgType.PYTHON_BINDINGS, 'library/python', []) - ], - "interfaces": [ - Pkg('wavemap', PkgType.ROS1, 'interfaces/ros1/wavemap', []), - Pkg('wavemap_msgs', PkgType.ROS1, 'interfaces/ros1/wavemap_msgs', []), - Pkg('wavemap_ros_conversions', PkgType.ROS1, - 'interfaces/ros1/wavemap_ros_conversions', []), - Pkg('wavemap_ros', PkgType.ROS1, 'interfaces/ros1/wavemap_ros', []), - Pkg('wavemap_rviz_plugin', PkgType.ROS1, - 'interfaces/ros1/wavemap_rviz_plugin', []), - Pkg('wavemap_all', PkgType.ROS1, 'interfaces/ros1/wavemap_all', []) - ], - "helpers": [ - Pkg('catkin_setup', PkgType.ROS1, 'tooling/packages/catkin_setup', []), - ], - "examples": [ - Pkg('wavemap_examples_cpp', PkgType.CPP, 'examples/cpp', []), - Pkg('wavemap_examples_ros1', PkgType.ROS1, 'examples/ros1', []), - Pkg('wavemap_examples_python', PkgType.PYTHON, 'examples/python', []) - ] -} if __name__ == '__main__': parser = argparse.ArgumentParser( prog='PrepareRelease', description='Prepares wavemap\'s release files.') - parser.add_argument('--bump_level', required=False) - parser.add_argument('--new_version', required=False) - parser.add_argument('--draft_release_notes', + parser.add_argument('--update-release-files', + action='store_true', + default=False) + parser.add_argument('--create-release-tag', + action='store_true', + default=False) + parser.add_argument('--draft-release-notes', action='store_true', default=False) + parser.add_argument('--level', required=False) + parser.add_argument('--version', required=False) args = parser.parse_args() - if (args.bump_level == '') != (args.new_version == ''): - print("Specify either the bump level or new version number, not both.") - raise SystemExit - # Load our git repo repo = git.Repo(os.path.abspath(__file__), search_parent_directories=True) # Run the rest of the script from the repo's root - os.chdir(repo.git.rev_parse("--show-toplevel")) + os.chdir(repo.git.rev_parse('--show-toplevel')) - # Find the most recent release - tags = repo.tags - most_recent_release = extract_highest_version(tags) - if most_recent_release is None: - raise SystemExit - most_recent_release = most_recent_release.name + # Get the current versions + wavemap_cpp_root = os.path.join('library', 'cpp') + last_released_version = version_from_git_tags(repo.tags) + current_file_version = version_from_cmake_project(wavemap_cpp_root) # Determine the new release number - if args.new_version: - new_version_str = args.new_version - else: - new_version = bump_version(most_recent_release, args.bump_level) - new_version_str = '.'.join([str(x) for x in new_version]) - - if args.draft_release_notes: - draft_release_notes() + next_version = None + if args.version and args.level: + print('Specify either the bump level or the new version, not both.') + raise SystemExit + if args.version: + next_version = args.version + elif args.level: + next_version = bump_version(last_released_version, args.level) + + if args.update_release_files: + if next_version is None: + print('Specify either a --level=(major|minor|patch),' + ' or a specific --version=X.Y.Z.') + raise SystemExit + update_release_files(last_released_version, next_version) + elif args.create_release_tag: + # print(current_file_version) + if current_file_version == last_released_version: + print(f'Current project file version ({current_file_version}) ' + 'already has a corresponding git release tag ' + f'(v{last_released_version}).') + raise SystemExit + create_release_tag(current_file_version) + elif args.draft_release_notes: + draft_release_notes(current_file_version) else: - prepare_release_files() + print('No release action specified. Please choose ' + '--update-release-files, --create-release-tag, ' + 'or --draft-release-notes.') From 85320a0cddeb1a1b4c86cde02f42c5f412b89962 Mon Sep 17 00:00:00 2001 From: Victor Reijgwart Date: Mon, 25 Nov 2024 22:39:32 +0100 Subject: [PATCH 22/22] Update changelogs --- examples/cpp/CHANGELOG.rst | 3 ++ examples/cpp/CMakeLists.txt | 2 +- examples/python/CHANGELOG.rst | 3 ++ examples/ros1/CHANGELOG.rst | 3 ++ examples/ros1/package.xml | 2 +- interfaces/ros1/wavemap/CHANGELOG.rst | 3 ++ interfaces/ros1/wavemap/package.xml | 2 +- interfaces/ros1/wavemap_all/CHANGELOG.rst | 3 ++ interfaces/ros1/wavemap_all/package.xml | 2 +- interfaces/ros1/wavemap_msgs/CHANGELOG.rst | 3 ++ interfaces/ros1/wavemap_msgs/package.xml | 2 +- interfaces/ros1/wavemap_ros/CHANGELOG.rst | 5 ++++ interfaces/ros1/wavemap_ros/package.xml | 2 +- .../wavemap_ros_conversions/CHANGELOG.rst | 5 ++++ .../ros1/wavemap_ros_conversions/package.xml | 2 +- .../ros1/wavemap_rviz_plugin/CHANGELOG.rst | 3 ++ .../ros1/wavemap_rviz_plugin/package.xml | 2 +- library/cpp/CHANGELOG.rst | 28 +++++++++++++++++++ library/cpp/CMakeLists.txt | 2 +- library/python/CHANGELOG.rst | 5 ++++ library/python/CMakeLists.txt | 2 +- library/python/pyproject.toml | 2 +- tooling/packages/catkin_setup/CHANGELOG.rst | 3 ++ tooling/packages/catkin_setup/package.xml | 2 +- 24 files changed, 79 insertions(+), 12 deletions(-) diff --git a/examples/cpp/CHANGELOG.rst b/examples/cpp/CHANGELOG.rst index 980f322fe..62226e3f1 100644 --- a/examples/cpp/CHANGELOG.rst +++ b/examples/cpp/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package wavemap_examples_cpp ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.2.0 (2024-11-25) +------------------ + 2.1.2 (2024-11-20) ------------------ diff --git a/examples/cpp/CMakeLists.txt b/examples/cpp/CMakeLists.txt index 914e98fe0..428126512 100644 --- a/examples/cpp/CMakeLists.txt +++ b/examples/cpp/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.10) -project(wavemap_examples_cpp VERSION 2.1.2 LANGUAGES CXX) +project(wavemap_examples_cpp VERSION 2.2.0 LANGUAGES CXX) # Load the wavemap library # First, try to load it from sources diff --git a/examples/python/CHANGELOG.rst b/examples/python/CHANGELOG.rst index 092fa7365..a14c2b098 100644 --- a/examples/python/CHANGELOG.rst +++ b/examples/python/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package wavemap_examples_python ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.2.0 (2024-11-25) +------------------ + 2.1.2 (2024-11-20) ------------------ diff --git a/examples/ros1/CHANGELOG.rst b/examples/ros1/CHANGELOG.rst index 7cd810a50..79166bb60 100644 --- a/examples/ros1/CHANGELOG.rst +++ b/examples/ros1/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package wavemap_examples_ros1 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.2.0 (2024-11-25) +------------------ + 2.1.2 (2024-11-20) ------------------ diff --git a/examples/ros1/package.xml b/examples/ros1/package.xml index 21da126fd..551c77a06 100644 --- a/examples/ros1/package.xml +++ b/examples/ros1/package.xml @@ -1,7 +1,7 @@ wavemap_examples_ros1 - 2.1.2 + 2.2.0 Usages examples for wavemap's ROS1 interface. Victor Reijgwart diff --git a/interfaces/ros1/wavemap/CHANGELOG.rst b/interfaces/ros1/wavemap/CHANGELOG.rst index 93efbefd8..ddc7e296d 100644 --- a/interfaces/ros1/wavemap/CHANGELOG.rst +++ b/interfaces/ros1/wavemap/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package wavemap ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.2.0 (2024-11-25) +------------------ + 2.1.2 (2024-11-20) ------------------ diff --git a/interfaces/ros1/wavemap/package.xml b/interfaces/ros1/wavemap/package.xml index 25212d864..7521458ce 100644 --- a/interfaces/ros1/wavemap/package.xml +++ b/interfaces/ros1/wavemap/package.xml @@ -1,7 +1,7 @@ wavemap - 2.1.2 + 2.2.0 Base library for wavemap. Victor Reijgwart diff --git a/interfaces/ros1/wavemap_all/CHANGELOG.rst b/interfaces/ros1/wavemap_all/CHANGELOG.rst index fb1abe52c..e00f08961 100644 --- a/interfaces/ros1/wavemap_all/CHANGELOG.rst +++ b/interfaces/ros1/wavemap_all/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package wavemap_all ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.2.0 (2024-11-25) +------------------ + 2.1.2 (2024-11-20) ------------------ diff --git a/interfaces/ros1/wavemap_all/package.xml b/interfaces/ros1/wavemap_all/package.xml index dbf7e1380..4c659a368 100644 --- a/interfaces/ros1/wavemap_all/package.xml +++ b/interfaces/ros1/wavemap_all/package.xml @@ -1,7 +1,7 @@ wavemap_all - 2.1.2 + 2.2.0 Metapackage that builds all wavemap packages. Victor Reijgwart diff --git a/interfaces/ros1/wavemap_msgs/CHANGELOG.rst b/interfaces/ros1/wavemap_msgs/CHANGELOG.rst index a58fdfba4..e366f6eff 100644 --- a/interfaces/ros1/wavemap_msgs/CHANGELOG.rst +++ b/interfaces/ros1/wavemap_msgs/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package wavemap_msgs ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.2.0 (2024-11-25) +------------------ + 2.1.2 (2024-11-20) ------------------ diff --git a/interfaces/ros1/wavemap_msgs/package.xml b/interfaces/ros1/wavemap_msgs/package.xml index 105e124bb..42f1a1875 100644 --- a/interfaces/ros1/wavemap_msgs/package.xml +++ b/interfaces/ros1/wavemap_msgs/package.xml @@ -1,7 +1,7 @@ wavemap_msgs - 2.1.2 + 2.2.0 Message definitions for wavemap's ROS interfaces. Victor Reijgwart diff --git a/interfaces/ros1/wavemap_ros/CHANGELOG.rst b/interfaces/ros1/wavemap_ros/CHANGELOG.rst index f5f4c64bf..66e86d30c 100644 --- a/interfaces/ros1/wavemap_ros/CHANGELOG.rst +++ b/interfaces/ros1/wavemap_ros/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package wavemap_ros ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.2.0 (2024-11-25) +------------------ +* Refactor TfTransformer to return by value, using std::nullopt to signal failure +* Contributors: Victor Reijgwart + 2.1.2 (2024-11-20) ------------------ * Report CPU, wall time and RAM usage when rosbag_processor completes diff --git a/interfaces/ros1/wavemap_ros/package.xml b/interfaces/ros1/wavemap_ros/package.xml index 2fb3dc6be..c221df73c 100644 --- a/interfaces/ros1/wavemap_ros/package.xml +++ b/interfaces/ros1/wavemap_ros/package.xml @@ -1,7 +1,7 @@ wavemap_ros - 2.1.2 + 2.2.0 ROS interface for wavemap. Victor Reijgwart diff --git a/interfaces/ros1/wavemap_ros_conversions/CHANGELOG.rst b/interfaces/ros1/wavemap_ros_conversions/CHANGELOG.rst index b8f49bfce..cc0236092 100644 --- a/interfaces/ros1/wavemap_ros_conversions/CHANGELOG.rst +++ b/interfaces/ros1/wavemap_ros_conversions/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package wavemap_ros_conversions ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.2.0 (2024-11-25) +------------------ +* Improve consistency between code operating on standard and chunked octrees +* Contributors: Victor Reijgwart + 2.1.2 (2024-11-20) ------------------ * Update include path for profiler_interface.h diff --git a/interfaces/ros1/wavemap_ros_conversions/package.xml b/interfaces/ros1/wavemap_ros_conversions/package.xml index abc5ec026..caa8b2b7e 100644 --- a/interfaces/ros1/wavemap_ros_conversions/package.xml +++ b/interfaces/ros1/wavemap_ros_conversions/package.xml @@ -1,7 +1,7 @@ wavemap_ros_conversions - 2.1.2 + 2.2.0 Conversions between wavemap and ROS types. Victor Reijgwart diff --git a/interfaces/ros1/wavemap_rviz_plugin/CHANGELOG.rst b/interfaces/ros1/wavemap_rviz_plugin/CHANGELOG.rst index 25fe911ce..e6475197e 100644 --- a/interfaces/ros1/wavemap_rviz_plugin/CHANGELOG.rst +++ b/interfaces/ros1/wavemap_rviz_plugin/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package wavemap_rviz_plugin ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.2.0 (2024-11-25) +------------------ + 2.1.2 (2024-11-20) ------------------ * Report CPU, wall time and RAM usage when rosbag_processor completes diff --git a/interfaces/ros1/wavemap_rviz_plugin/package.xml b/interfaces/ros1/wavemap_rviz_plugin/package.xml index 043783900..bdd898c86 100644 --- a/interfaces/ros1/wavemap_rviz_plugin/package.xml +++ b/interfaces/ros1/wavemap_rviz_plugin/package.xml @@ -1,7 +1,7 @@ wavemap_rviz_plugin - 2.1.2 + 2.2.0 Plugin to interactively visualize maps published in wavemap's native format. diff --git a/library/cpp/CHANGELOG.rst b/library/cpp/CHANGELOG.rst index 9b4fc340a..274465b40 100644 --- a/library/cpp/CHANGELOG.rst +++ b/library/cpp/CHANGELOG.rst @@ -2,6 +2,34 @@ Changelog for package wavemap ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.2.0 (2024-11-25) +------------------ +* New features + + * Add QueryAccelerator for HashedChunkedWaveletOctrees + * Add method to erase individual nodes from chunked octrees + +* Improvements + + * Improve ChunkedNdtree NodePtr and NodeRef implementation + + * Correctly emulate the distinction between const pointers and pointers-to-const + + * Use the improved ChunkedNdtree NodePtrs and NodeRefs to + + * Improve consistency between code operating on standard and chunked octrees + * Simplify HashedChunkedWaveletOctree measurement integrator code + * Simplify HashedChunkedWaveletOctree map thresholding and pruning code + + * General refactoring and code cleanup + + * Tidy up constructors, in particular in terms of argument moves vs copies + * Define constructors for commonly used structs explicitly, for compatibility with emplace_back + * Optimize child node accesses in tree data structures by avoiding redundant lookups + * Simplify and improve clarity of old code + +* Contributors: Victor Reijgwart + 2.1.2 (2024-11-20) ------------------ * Adds a ResourceMonitor class for measuring CPU time, wall time, and RAM usage during macro-benchmarking diff --git a/library/cpp/CMakeLists.txt b/library/cpp/CMakeLists.txt index 39c81ec8d..eee92bf3b 100644 --- a/library/cpp/CMakeLists.txt +++ b/library/cpp/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.10) -project(wavemap VERSION 2.1.2 LANGUAGES CXX) +project(wavemap VERSION 2.2.0 LANGUAGES CXX) # General options cmake_policy(SET CMP0077 NEW) diff --git a/library/python/CHANGELOG.rst b/library/python/CHANGELOG.rst index 58898d3ec..f584307fa 100644 --- a/library/python/CHANGELOG.rst +++ b/library/python/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package pywavemap ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.2.0 (2024-11-25) +------------------ +* Add accelerated HashedChunkedWaveletOctree accessors to Python API +* Contributors: Victor Reijgwart + 2.1.2 (2024-11-20) ------------------ diff --git a/library/python/CMakeLists.txt b/library/python/CMakeLists.txt index fae701f9c..88023b7a0 100644 --- a/library/python/CMakeLists.txt +++ b/library/python/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.18) -project(pywavemap VERSION 2.1.2 LANGUAGES CXX) +project(pywavemap VERSION 2.2.0 LANGUAGES CXX) # Warn if the user invokes CMake directly if (NOT SKBUILD AND NOT $ENV{CLION_IDE}) diff --git a/library/python/pyproject.toml b/library/python/pyproject.toml index c4fe49030..4efc9d123 100644 --- a/library/python/pyproject.toml +++ b/library/python/pyproject.toml @@ -4,7 +4,7 @@ build-backend = "scikit_build_core.build" [project] name = "pywavemap" -version = "2.1.2" +version = "2.2.0" description = "A fast, efficient and accurate multi-resolution, multi-sensor 3D occupancy mapping framework." readme = "../../README.md" requires-python = ">=3.8" diff --git a/tooling/packages/catkin_setup/CHANGELOG.rst b/tooling/packages/catkin_setup/CHANGELOG.rst index a8ff8a2e1..b71ee48c3 100644 --- a/tooling/packages/catkin_setup/CHANGELOG.rst +++ b/tooling/packages/catkin_setup/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package catkin_setup ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.2.0 (2024-11-25) +------------------ + 2.1.2 (2024-11-20) ------------------ diff --git a/tooling/packages/catkin_setup/package.xml b/tooling/packages/catkin_setup/package.xml index 8e75b5deb..df6ba8934 100644 --- a/tooling/packages/catkin_setup/package.xml +++ b/tooling/packages/catkin_setup/package.xml @@ -1,7 +1,7 @@ catkin_setup - 2.1.2 + 2.2.0 Dummy package to make it easy to setup the workspace and generate the setup.[sh|bash|zsh] scripts in CI. Victor Reijgwart