From 9269a3b5faf75d07db81a72c7cdeeee00535c374 Mon Sep 17 00:00:00 2001 From: Victor Reijgwart Date: Thu, 21 Nov 2024 17:42:07 +0100 Subject: [PATCH] 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