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/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/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/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/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 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_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/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/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/aabb.h b/library/cpp/include/wavemap/core/data_structure/aabb.h index 2f60c2351..b0864aad9 100644 --- a/library/cpp/include/wavemap/core/data_structure/aabb.h +++ b/library/cpp/include/wavemap/core/data_structure/aabb.h @@ -1,8 +1,10 @@ #ifndef WAVEMAP_CORE_DATA_STRUCTURE_AABB_H_ #define WAVEMAP_CORE_DATA_STRUCTURE_AABB_H_ +#include #include #include +#include #include "wavemap/core/common.h" #include "wavemap/core/utils/math/int_math.h" @@ -23,6 +25,10 @@ struct AABB { PointType min = PointType::Constant(kInitialMin); PointType max = PointType::Constant(kInitialMax); + AABB() = default; + 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); max = max.cwiseMax(point); 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..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(); @@ -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/chunked_ndtree/chunked_ndtree_node_address.h b/library/cpp/include/wavemap/core/data_structure/chunked_ndtree/chunked_ndtree_node_address.h index 3f05d8854..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 @@ -15,7 +15,6 @@ template class ChunkedNdtreeNodePtr { public: using NodeRef = ChunkedNdtreeNodeRef; - using NodeConstRef = ChunkedNdtreeNodeRef; // Constructors ChunkedNdtreeNodePtr() = default; @@ -32,28 +31,29 @@ 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->(); } + + // Emulate null check semantics + operator bool() const { return node_.has_value(); } // NOLINT + bool operator==(std::nullptr_t) noexcept { return !node_.has_value(); } private: - std::optional node_; + std::optional node_{}; }; 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; using NodeDataType = typename ChunkType::DataType; + ChunkedNdtreeNodeRef() = delete; ChunkedNdtreeNodeRef(ChunkType& chunk, IndexElement relative_node_depth, LinearIndex level_traversal_distance); @@ -65,35 +65,36 @@ 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; + 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); - NodeConstPtr getChild(NdtreeIndexRelativeChild child_index) const; + NodePtr getChild(NdtreeIndexRelativeChild child_index) const; template NodeRef getOrAllocateChild(NdtreeIndexRelativeChild child_index, DefaultArgs&&... args) const; 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_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/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..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 @@ -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,12 @@ 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 { +auto ChunkedNdtreeNodeRef::hasAtLeastOneChild() const { return chunk_.nodeHasAtLeastOneChild(computeRelativeNodeIndex()); } @@ -128,29 +112,44 @@ 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}; +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::NodeConstPtr +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) { - const auto* child_chunk = chunk_.getChild(child_level_traversal_distance); + if (child_depth == ChunkType::kHeight) { + auto* child_chunk = chunk_.getChild(child_level_traversal_distance); return {child_chunk, 0, 0u}; } else { return {&chunk_, child_depth, child_level_traversal_distance}; @@ -162,10 +161,11 @@ template ChunkedNdtreeNodeRef ChunkedNdtreeNodeRef::getOrAllocateChild( NdtreeIndexRelativeChild child_index, DefaultArgs&&... args) const { + hasAtLeastOneChild() = true; 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/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/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/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/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/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/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/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/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/indexing/ndtree_index.h b/library/cpp/include/wavemap/core/indexing/ndtree_index.h index 3e2d0ff71..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" @@ -32,6 +33,12 @@ struct NdtreeIndex { //! by *height* Position position = Position::Zero(); + NdtreeIndex() = default; + 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 ff8cfc916..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 @@ -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,9 +32,10 @@ class HashedChunkedWaveletIntegrator : public ProjectiveIntegrator { private: using BlockList = std::vector; + 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 @@ -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,13 @@ 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 node, + const OctreeIndex& node_index, + FloatingPoint& node_value, + 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/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_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/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.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..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 @@ -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,38 +68,31 @@ 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: - static constexpr IndexElement kMaxChunkStackDepth = - kMaxSupportedTreeHeight / kChunkHeight; - 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; bool needs_pruning_ = false; Timestamp last_updated_stamp_ = Time::now(); - struct RecursiveThresholdReturnValue { - Coefficients::Scale scale; - bool is_nonzero_child; - }; - RecursiveThresholdReturnValue recursiveThreshold( - ChunkedOctreeType::ChunkType& chunk, - Coefficients::Scale scale_coefficient); - void recursivePrune(ChunkedOctreeType::ChunkType& chunk); + void recursiveThreshold(OctreeType::NodeRefType node, + Coefficients::Scale& node_scale_coefficient); + void recursivePrune( + HashedChunkedWaveletOctreeBlock::OctreeType::NodeRefType node); }; } // 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..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 @@ -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 scale_coefficient); - void recursivePrune(NodeType& node); + void recursiveThreshold(OctreeType::NodeRefType node, + Coefficients::Scale& node_scale_coefficient); + 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 42fc84154..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 @@ -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_); } @@ -20,44 +20,16 @@ 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(); + OctreeType::NodeConstPtrType node = &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)) { - break; - } - current_chunk = current_chunk->getChild(linear_child_index); + 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); + node = node->getChild(child_index); } - return value; } } // namespace wavemap 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..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 @@ -20,16 +20,13 @@ 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; + 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/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/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 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/impl/query_accelerator_inl.h b/library/cpp/include/wavemap/core/utils/query/impl/query_accelerator_inl.h index b04dc0c31..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 = std::array>{}; + 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 0e6c76569..238d47bcc 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 { @@ -103,7 +104,7 @@ class QueryAccelerator> { MortonIndex morton_code = std::numeric_limits::max(); BlockType* block_ = nullptr; - std::array> node_stack{}; + std::array> node_stack_{}; }; /** @@ -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(); @@ -143,16 +144,69 @@ 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_ = - 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::OctreeType::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/include/wavemap/core/utils/undistortion/stamped_pose_buffer.h b/library/cpp/include/wavemap/core/utils/undistortion/stamped_pose_buffer.h index f2a155eab..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 @@ -7,9 +7,14 @@ namespace wavemap::undistortion { struct StampedPose { - TimeAbsolute stamp; - Transformation3D pose; + TimeAbsolute stamp = 0u; + Transformation3D pose{}; + + StampedPose() = default; + StampedPose(TimeAbsolute stamp, const Transformation3D& pose) + : stamp(stamp), pose(pose) {} }; + using StampedPoseBuffer = std::vector; } // namespace wavemap::undistortion 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 3352217c9..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 @@ -77,30 +77,25 @@ 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, - block.getRootScale(), - block.getRootChunk().nodeHasAtLeastOneChild(0u), - block_needs_thresholding); + updateNodeRecursive(block.getRootNode(), root_node_index, + block.getRootScale(), block_needs_thresholding); block.setNeedsThresholding(block_needs_thresholding); } 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 - parent_has_child, + HashedChunkedWaveletIntegrator::OctreeType::NodeRefType node, + const OctreeIndex& node_index, FloatingPoint& node_value, bool& block_needs_thresholding) { - auto& parent_details = parent_chunk.nodeData(parent_in_chunk_index); + // 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 @@ -145,54 +140,24 @@ 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 = node.getOrAllocateChild(relative_child_idx); + auto& child_details = child_node.data(); // 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(*chunk_containing_child, child_index, - child_node_in_chunk_index, child_value, - child_has_children, block_needs_thresholding); - } - - if (child_has_children || data::is_nonzero(child_details)) { - parent_has_child = true; + DCHECK_GE(child_index.height, 0); + updateNodeRecursive(child_node, child_index, child_value, + block_needs_thresholding); } } + // 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/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..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,24 +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::NodeType& root_node = block.getRootNode(); - HashedWaveletOctreeBlock::Coefficients::Scale& root_node_scale = - block.getRootScale(); block.setNeedsPruning(); block.setLastUpdatedStamp(); struct StackElement { - HashedWaveletOctreeBlock::NodeType& 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()})}); @@ -116,7 +119,7 @@ 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; + 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 +172,7 @@ void HashedWaveletIntegrator::updateBlock(HashedWaveletOctree::Block& block, projection_model_->cartesianToSensorZ(C_node_center); const FloatingPoint bounding_sphere_radius = kUnitCubeHalfDiagonal * node_width; - HashedWaveletOctreeBlock::NodeType* 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 d9aea9bb8..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 @@ -2,6 +2,7 @@ #include #include +#include #include @@ -9,9 +10,7 @@ namespace wavemap { void HashedChunkedWaveletOctreeBlock::threshold() { ProfilerZoneScoped; if (getNeedsThresholding()) { - root_scale_coefficient_ = recursiveThreshold(chunked_ndtree_.getRootChunk(), - root_scale_coefficient_) - .scale; + recursiveThreshold(ndtree_.getRootNode(), root_scale_coefficient_); setNeedsThresholding(false); } } @@ -20,7 +19,7 @@ void HashedChunkedWaveletOctreeBlock::prune() { ProfilerZoneScoped; if (getNeedsPruning()) { threshold(); - recursivePrune(chunked_ndtree_.getRootChunk()); + recursivePrune(ndtree_.getRootNode()); setNeedsPruning(false); } } @@ -28,7 +27,7 @@ void HashedChunkedWaveletOctreeBlock::prune() { void HashedChunkedWaveletOctreeBlock::clear() { ProfilerZoneScoped; root_scale_coefficient_ = Coefficients::Scale{}; - chunked_ndtree_.clear(); + ndtree_.clear(); setLastUpdatedStamp(); } @@ -37,70 +36,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(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); + OctreeType::NodeRefType current_parent = ancestors.back(); + current_value = Transform::backwardSingleChild( + {current_value, current_parent.data()}, child_index); + OctreeType::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); + OctreeType::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 +75,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(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); + OctreeType::NodeRefType current_parent = ancestors.back(); + OctreeType::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 = + OctreeType::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; } @@ -164,12 +117,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; @@ -195,109 +148,47 @@ void HashedChunkedWaveletOctreeBlock::forEachLeaf( } } -HashedChunkedWaveletOctreeBlock::RecursiveThresholdReturnValue -HashedChunkedWaveletOctreeBlock::recursiveThreshold( // NOLINT - HashedChunkedWaveletOctreeBlock::ChunkedOctreeType::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 < ChunkedOctreeType::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); - 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::ChunkedOctreeType::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 < ChunkedOctreeType::ChunkType::kNumChildren; - ++linear_child_idx) { - if (chunk.hasChild(linear_child_idx)) { - ChunkedOctreeType::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 (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; } } } - if (!has_at_least_one_child) { - chunk.deleteChildrenArray(); - } + node.hasAtLeastOneChild() = has_at_least_one_child; } } // 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 7ab3ec524..a0633ab36 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); } } @@ -37,30 +36,31 @@ 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(); + OctreeType::NodePtrType 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); + OctreeType::NodeRefType child = + current_parent->getOrAllocateChild(child_index); + 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(); + OctreeType::NodePtrType current_node = ancestors.back(); + ancestors.pop_back(); coefficients = Transform::forwardSingleChild(coefficients.scale, child_index); current_node->data() += coefficients.details; @@ -76,25 +76,26 @@ 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& child = current_parent->getOrAllocateChild(child_index); - node_ptrs.emplace_back(&child); + OctreeType::NodePtrType current_parent = ancestors.back(); + OctreeType::NodeRefType child = + current_parent->getOrAllocateChild(child_index); + 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(); + OctreeType::NodePtrType current_node = ancestors.back(); + ancestors.pop_back(); const NdtreeIndexRelativeChild child_index = OctreeIndex::computeRelativeChildIndex(morton_code, parent_height); coefficients = @@ -115,7 +116,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 +124,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(); @@ -135,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) { - const NodeType& 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); @@ -147,40 +147,42 @@ void HashedWaveletOctreeBlock::forEachLeaf( } } -HashedWaveletOctreeBlock::Coefficients::Scale -HashedWaveletOctreeBlock::recursiveThreshold( // NOLINT - HashedWaveletOctreeBlock::NodeType& node, FloatingPoint scale_coefficient) { +void HashedWaveletOctreeBlock::recursiveThreshold( // NOLINT + HashedWaveletOctreeBlock::OctreeType::NodeRefType node, + 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) { - if (node.hasChild(child_idx)) { - NodeType& child_node = *node.getChild(child_idx); - child_scale_coefficients[child_idx] = - recursiveThreshold(child_node, child_scale_coefficients[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 { - 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 - 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); - 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 7ebd9e4b3..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); @@ -330,19 +329,19 @@ void ClassifiedMap::QueryCache::reset() { morton_code = std::numeric_limits::max(); block = nullptr; - node_stack = std::array>{}; + node_stack.fill({}); } 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()}); 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, @@ -374,7 +373,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 f7e3bb674..35fb17c0b 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_; } @@ -56,20 +56,15 @@ 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 = 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) { - const HashedWaveletOctree::Block::NodeType* parent_node = - node_stack_[height_]; + while (node_stack_[height_] && height_ != index.height) { + NodePtrType parent_node = node_stack_[height_]; const FloatingPoint parent_value = value_stack_[height_]; const NdtreeIndexRelativeChild child_idx = OctreeIndex::computeRelativeChildIndex(morton_code_, height_); @@ -77,9 +72,81 @@ 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); + } + + 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 = CHECK_NOTNULL(node_stack_[height_ + 1]); + const NdtreeIndexRelativeChild child_index = + OctreeIndex::computeRelativeChildIndex(morton_code_, height_ + 1); + node_stack_[height_] = parent_node->getChild(child_index); + } + + // Walk down the tree from height_ to index.height + while (node_stack_[height_] && height_ != index.height) { + 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); node_stack_[height_] = parent_node->getChild(child_idx); } 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; 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/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()); } 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); 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" 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/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 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 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.')