diff --git a/docs/python_api/index.rst b/docs/python_api/index.rst index 0066b42f1..a4785194c 100644 --- a/docs/python_api/index.rst +++ b/docs/python_api/index.rst @@ -44,6 +44,7 @@ Python API .. automethod:: pywavemap.convert.height_to_cell_width .. automethod:: pywavemap.convert.point_to_nearest_index .. automethod:: pywavemap.convert.point_to_node_index +.. automethod:: pywavemap.convert.node_index_to_center_point .. automodule:: pywavemap.logging .. automethod:: pywavemap.logging.set_level diff --git a/examples/cpp/CMakeLists.txt b/examples/cpp/CMakeLists.txt index 9cbadf540..a5ec4991e 100644 --- a/examples/cpp/CMakeLists.txt +++ b/examples/cpp/CMakeLists.txt @@ -31,6 +31,7 @@ else () endif () # Add each set of examples +add_subdirectory(edit) +add_subdirectory(conversions) add_subdirectory(io) add_subdirectory(queries) -add_subdirectory(planning) diff --git a/examples/cpp/planning/CMakeLists.txt b/examples/cpp/conversions/CMakeLists.txt similarity index 51% rename from examples/cpp/planning/CMakeLists.txt rename to examples/cpp/conversions/CMakeLists.txt index 13cd14524..019f73c9c 100644 --- a/examples/cpp/planning/CMakeLists.txt +++ b/examples/cpp/conversions/CMakeLists.txt @@ -3,3 +3,8 @@ add_executable(occupancy_to_esdf occupancy_to_esdf.cc) set_wavemap_target_properties(occupancy_to_esdf) target_link_libraries(occupancy_to_esdf PUBLIC wavemap::wavemap_core wavemap::wavemap_io) + +add_executable(occupancy_to_ply occupancy_to_ply.cc) +set_wavemap_target_properties(occupancy_to_ply) +target_link_libraries(occupancy_to_ply PUBLIC + wavemap::wavemap_core wavemap::wavemap_io) diff --git a/examples/cpp/planning/occupancy_to_esdf.cc b/examples/cpp/conversions/occupancy_to_esdf.cc similarity index 100% rename from examples/cpp/planning/occupancy_to_esdf.cc rename to examples/cpp/conversions/occupancy_to_esdf.cc diff --git a/examples/cpp/conversions/occupancy_to_ply.cc b/examples/cpp/conversions/occupancy_to_ply.cc new file mode 100644 index 000000000..f684eb678 --- /dev/null +++ b/examples/cpp/conversions/occupancy_to_ply.cc @@ -0,0 +1,94 @@ +#include +#include +#include + +#include +#include +#include +#include +#include + +using namespace wavemap; // NOLINT +int main(int argc, char** argv) { + // Initialize GLOG + google::InitGoogleLogging(argv[0]); + google::InstallFailureSignalHandler(); + FLAGS_alsologtostderr = true; + FLAGS_colorlogtostderr = true; + + // Read params + if (argc != 2) { + LOG(ERROR) + << "Please supply a path to an occupancy map as the first argument."; + return EXIT_FAILURE; + } + const std::filesystem::path map_file_path = argv[1]; + + // Load the occupancy map + MapBase::Ptr occupancy_map; + if (!io::fileToMap(map_file_path, occupancy_map)) { + LOG(WARNING) << "Could not open map for reading: " << map_file_path; + return EXIT_FAILURE; + } + + // Convert each occupied leaf node to high-res points and add to pointcloud + std::vector pointcloud; + constexpr FloatingPoint kOccupancyThreshold = 0.01f; + const FloatingPoint min_cell_width = occupancy_map->getMinCellWidth(); + occupancy_map->forEachLeaf([min_cell_width, &pointcloud]( + const OctreeIndex& node_index, + FloatingPoint node_log_odds) { + if (kOccupancyThreshold < node_log_odds) { + if (node_index.height == 0) { + const Point3D center = + convert::indexToCenterPoint(node_index.position, min_cell_width); + pointcloud.emplace_back(center); + } else { + const Index3D node_min_corner = + convert::nodeIndexToMinCornerIndex(node_index); + const Index3D node_max_corner = + convert::nodeIndexToMaxCornerIndex(node_index); + for (const Index3D& index : Grid(node_min_corner, node_max_corner)) { + const Point3D center = + convert::indexToCenterPoint(index, min_cell_width); + pointcloud.emplace_back(center); + } + } + } + }); + + // Create the PLY output file + const std::filesystem::path ply_file_path = + std::filesystem::path(map_file_path).replace_extension(".ply"); + LOG(INFO) << "Creating PLY file: " << ply_file_path; + std::ofstream ply_file(ply_file_path, + std::ofstream::out | std::ofstream::binary); + if (!ply_file.is_open()) { + LOG(WARNING) << "Could not open file for writing. Error: " + << strerror(errno); + return EXIT_FAILURE; + } + + // Write the PLY header + // clang-format off + ply_file << "ply\n" + "format ascii 1.0\n" + "comment The voxel size is " << min_cell_width << " meters.\n" + "element vertex " << pointcloud.size() << "\n" + "property float x\n" + "property float y\n" + "property float z\n" + "end_header" + << std::endl; + // clang-format on + + // Add the points + for (const Point3D& point : pointcloud) { + ply_file << point.x() << " " << point.y() << " " << point.z() << "\n"; + } + ply_file.flush(); + + // Close the file and communicate whether writing succeeded + ply_file.close(); + return static_cast(ply_file) ? EXIT_SUCCESS : EXIT_FAILURE; +} diff --git a/examples/cpp/edit/CMakeLists.txt b/examples/cpp/edit/CMakeLists.txt new file mode 100644 index 000000000..3e4369e8a --- /dev/null +++ b/examples/cpp/edit/CMakeLists.txt @@ -0,0 +1,5 @@ +# Binaries +add_executable(transform_map transform_map.cc) +set_wavemap_target_properties(transform_map) +target_link_libraries(transform_map PUBLIC + wavemap::wavemap_core wavemap::wavemap_io) diff --git a/examples/cpp/edit/transform_map.cc b/examples/cpp/edit/transform_map.cc new file mode 100644 index 000000000..68f730acb --- /dev/null +++ b/examples/cpp/edit/transform_map.cc @@ -0,0 +1,36 @@ +#include + +#include +#include +#include + +int main(int, char**) { + // Settings + std::filesystem::path input_map_path = + "/home/victor/data/wavemaps/newer_college_cloister_10cm.wvmp"; + std::filesystem::path output_map_path = + "/home/victor/data/wavemaps/newer_college_cloister_10cm_tranformed.wvmp"; + + // Create a smart pointer that will own the loaded map + wavemap::MapBase::Ptr map_base; + + // Load the map + bool success = wavemap::io::fileToMap(input_map_path, map_base); + CHECK(success); + + // Downcast it to a concrete map type + auto map = std::dynamic_pointer_cast(map_base); + CHECK_NOTNULL(map); + + // Define a transformation that flips the map upside down, for illustration + wavemap::Transformation3D T_AB; + T_AB.getRotation() = wavemap::Rotation3D{0.f, 1.f, 0.f, 0.f}; + + // Transform the map + map = wavemap::edit::transform(*map, T_AB, + std::make_shared()); + + // Save the map + success &= wavemap::io::mapToFile(*map, output_map_path); + CHECK(success); +} diff --git a/examples/python/queries/occupied_cell_lists.py b/examples/python/queries/occupied_cell_lists.py new file mode 100644 index 000000000..e928c0448 --- /dev/null +++ b/examples/python/queries/occupied_cell_lists.py @@ -0,0 +1,17 @@ +import _dummy_objects + +# Load a map +your_map = _dummy_objects.example_map() + +# Log odds threshold above which to consider a cell occupied +log_odds_occupancy_threshold = 1e-3 + +# Get the node indices of all occupied leaf nodes +occupied_nodes = your_map.get_occupied_node_indices( + log_odds_occupancy_threshold) +print(occupied_nodes) + +# Get the center points of all occupied cells at the highest resolution +occupied_points = your_map.get_occupied_pointcloud( + log_odds_occupancy_threshold) +print(occupied_points) diff --git a/interfaces/ros1/wavemap_ros/CMakeLists.txt b/interfaces/ros1/wavemap_ros/CMakeLists.txt index e90bc7449..8824e4da6 100644 --- a/interfaces/ros1/wavemap_ros/CMakeLists.txt +++ b/interfaces/ros1/wavemap_ros/CMakeLists.txt @@ -34,6 +34,7 @@ add_library(${PROJECT_NAME} src/inputs/ros_input_base.cc src/inputs/ros_input_factory.cc src/map_operations/crop_map_operation.cc + src/map_operations/decay_map_operation.cc src/map_operations/map_ros_operation_factory.cc src/map_operations/publish_map_operation.cc src/map_operations/publish_pointcloud_operation.cc diff --git a/interfaces/ros1/wavemap_ros/include/wavemap_ros/map_operations/crop_map_operation.h b/interfaces/ros1/wavemap_ros/include/wavemap_ros/map_operations/crop_map_operation.h index 9f133d3fc..6a8b5867f 100644 --- a/interfaces/ros1/wavemap_ros/include/wavemap_ros/map_operations/crop_map_operation.h +++ b/interfaces/ros1/wavemap_ros/include/wavemap_ros/map_operations/crop_map_operation.h @@ -3,10 +3,11 @@ #include #include -#include #include #include +#include +#include #include #include "wavemap_ros/utils/tf_transformer.h" @@ -15,17 +16,28 @@ namespace wavemap { /** * Config struct for map cropping operations. */ -struct CropMapOperationConfig : public ConfigBase { +struct CropMapOperationConfig : public ConfigBase { //! Time period controlling how often the map is cropped. Seconds once_every = 10.f; //! Name of the TF frame to treat as the center point. Usually the robot's - //! body frame. When the cropper runs, all blocks that are further than - //! remove_blocks_beyond_distance from this point are deleted. + //! body frame. When the cropper runs, all cells that are further than + //! `radius` from this point are deleted. std::string body_frame = "body"; - //! Distance beyond which blocks are deleted when the cropper is executed. - Meters remove_blocks_beyond_distance; + //! Time offset applied when retrieving the transform from body_frame to + //! world_frame. Set to -1 to use the most recent transform available in ROS + //! TF, ignoring timestamps (default). If set to a non-negative value, the + //! transform lookup uses a timestamp of `ros::Time::now() - tf_time_offset`. + Seconds tf_time_offset = -1.f; + + //! Distance beyond which to remove cells from the map. + Meters radius; + + //! Maximum resolution at which the crop is applied. Set to 0 to match the + //! map's maximum resolution (default). Setting a higher value reduces + //! computation but produces more jagged borders. + Meters max_update_resolution = 0.f; static MemberMap memberMap; @@ -36,6 +48,7 @@ class CropMapOperation : public MapOperationBase { public: CropMapOperation(const CropMapOperationConfig& config, MapBase::Ptr occupancy_map, + std::shared_ptr thread_pool, std::shared_ptr transformer, std::string world_frame); @@ -45,9 +58,18 @@ class CropMapOperation : public MapOperationBase { private: const CropMapOperationConfig config_; + const std::shared_ptr thread_pool_; const std::shared_ptr transformer_; const std::string world_frame_; ros::Time last_run_timestamp_; + Stopwatch timer_; + + const FloatingPoint min_cell_width_ = occupancy_map_->getMinCellWidth(); + const IndexElement termination_height_ = + min_cell_width_ < config_.max_update_resolution + ? static_cast(std::round( + std::log2(config_.max_update_resolution / min_cell_width_))) + : 0; }; } // namespace wavemap diff --git a/interfaces/ros1/wavemap_ros/include/wavemap_ros/map_operations/decay_map_operation.h b/interfaces/ros1/wavemap_ros/include/wavemap_ros/map_operations/decay_map_operation.h new file mode 100644 index 000000000..0662dd383 --- /dev/null +++ b/interfaces/ros1/wavemap_ros/include/wavemap_ros/map_operations/decay_map_operation.h @@ -0,0 +1,48 @@ +#ifndef WAVEMAP_ROS_MAP_OPERATIONS_DECAY_MAP_OPERATION_H_ +#define WAVEMAP_ROS_MAP_OPERATIONS_DECAY_MAP_OPERATION_H_ + +#include + +#include +#include +#include +#include +#include +#include + +namespace wavemap { +/** + * Config struct for map decaying operations. + */ +struct DecayMapOperationConfig : public ConfigBase { + //! Time period controlling how often the map is decayed. + Seconds once_every = 2.f; + + //! Decay rate in the range (0, 1), applied to each map value as + //! `map_value *= decay_rate` at each operation run. + FloatingPoint decay_rate = 0.9f; + + static MemberMap memberMap; + + bool isValid(bool verbose) const override; +}; + +class DecayMapOperation : public MapOperationBase { + public: + DecayMapOperation(const DecayMapOperationConfig& config, + MapBase::Ptr occupancy_map, + std::shared_ptr thread_pool); + + bool shouldRun(const ros::Time& current_time = ros::Time::now()); + + void run(bool force_run) override; + + private: + const DecayMapOperationConfig config_; + const std::shared_ptr thread_pool_; + ros::Time last_run_timestamp_; + Stopwatch timer_; +}; +} // namespace wavemap + +#endif // WAVEMAP_ROS_MAP_OPERATIONS_DECAY_MAP_OPERATION_H_ diff --git a/interfaces/ros1/wavemap_ros/include/wavemap_ros/map_operations/map_ros_operation_types.h b/interfaces/ros1/wavemap_ros/include/wavemap_ros/map_operations/map_ros_operation_types.h index f5f9147c9..c8224556e 100644 --- a/interfaces/ros1/wavemap_ros/include/wavemap_ros/map_operations/map_ros_operation_types.h +++ b/interfaces/ros1/wavemap_ros/include/wavemap_ros/map_operations/map_ros_operation_types.h @@ -7,10 +7,10 @@ namespace wavemap { struct MapRosOperationType : public TypeSelector { using TypeSelector::TypeSelector; - enum Id : TypeId { kPublishMap, kPublishPointcloud, kCropMap }; + enum Id : TypeId { kPublishMap, kPublishPointcloud, kCropMap, kDecayMap }; static constexpr std::array names = {"publish_map", "publish_pointcloud", - "crop_map"}; + "crop_map", "decay_map"}; }; } // namespace wavemap 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 e1bbebc5e..a8a769618 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 @@ -4,32 +4,39 @@ #include #include -#include #include #include +#include namespace wavemap { DECLARE_CONFIG_MEMBERS(CropMapOperationConfig, (once_every) (body_frame) - (remove_blocks_beyond_distance)); + (tf_time_offset) + (radius) + (max_update_resolution)); bool CropMapOperationConfig::isValid(bool verbose) const { bool all_valid = true; all_valid &= IS_PARAM_GT(once_every, 0.f, verbose); all_valid &= IS_PARAM_NE(body_frame, "", verbose); - all_valid &= IS_PARAM_GT(remove_blocks_beyond_distance, 0.f, verbose); + all_valid &= + IS_PARAM_TRUE(tf_time_offset == -1.f || 0.f <= tf_time_offset, verbose); + all_valid &= IS_PARAM_GT(radius, 0.f, verbose); + all_valid &= IS_PARAM_GE(max_update_resolution, 0.f, verbose); return all_valid; } CropMapOperation::CropMapOperation(const CropMapOperationConfig& config, MapBase::Ptr occupancy_map, + std::shared_ptr thread_pool, std::shared_ptr transformer, std::string world_frame) : MapOperationBase(std::move(occupancy_map)), config_(config.checkValid()), + thread_pool_(std::move(thread_pool)), transformer_(std::move(transformer)), world_frame_(std::move(world_frame)) {} @@ -49,42 +56,51 @@ void CropMapOperation::run(bool force_run) { return; } + // Get the center point of the cropping sphere (usually the robot's position) + const bool use_most_recent_transform = config_.tf_time_offset < 0.f; + const ros::Time timestamp = + use_most_recent_transform + ? ros::Time::UNINITIALIZED + : current_time - ros::Duration(config_.tf_time_offset); const auto T_W_B = transformer_->lookupTransform( - world_frame_, config_.body_frame, current_time); + world_frame_, config_.body_frame, timestamp); if (!T_W_B) { - ROS_WARN_STREAM( - "Could not look up center point for map cropping. TF lookup of " - "body_frame \"" - << config_.body_frame << "\" w.r.t. world_frame \"" << world_frame_ - << "\" at time " << current_time << " failed."); + if (use_most_recent_transform) { + ROS_WARN_STREAM( + "Could not look up center point for map cropping. No TF from " + "body_frame \"" + << config_.body_frame << "\" to world_frame \"" << world_frame_ + << "\"."); + } else { + ROS_WARN_STREAM( + "Could not look up center point for map cropping. TF lookup from " + "body_frame \"" + << config_.body_frame << "\" to world_frame \"" << world_frame_ + << "\" at time " << timestamp << " failed."); + } return; } - const IndexElement tree_height = occupancy_map_->getTreeHeight(); - const FloatingPoint min_cell_width = occupancy_map_->getMinCellWidth(); - 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*/) { - const auto block_node_index = OctreeIndex{tree_height, block_index}; - const auto block_aabb = - convert::nodeIndexToAABB(block_node_index, min_cell_width); - const FloatingPoint d_B_block = block_aabb.minDistanceTo(t_W_B); - return config.remove_blocks_beyond_distance < d_B_block; - }; - + // Crop the map + timer_.start(); if (auto* hashed_wavelet_octree = dynamic_cast(occupancy_map_.get()); hashed_wavelet_octree) { - hashed_wavelet_octree->eraseBlockIf(indicator_fn); + edit::crop_to_sphere(*hashed_wavelet_octree, T_W_B->getPosition(), + config_.radius, termination_height_, thread_pool_); } else if (auto* hashed_chunked_wavelet_octree = dynamic_cast( occupancy_map_.get()); hashed_chunked_wavelet_octree) { - hashed_chunked_wavelet_octree->eraseBlockIf(indicator_fn); + edit::crop_to_sphere(*hashed_chunked_wavelet_octree, T_W_B->getPosition(), + config_.radius, termination_height_, thread_pool_); } else { ROS_WARN( "Map cropping is only supported for hash-based map data structures."); } + timer_.stop(); + ROS_DEBUG_STREAM("Cropped map in " << timer_.getLastEpisodeDuration() + << "s. Total cropping time: " + << timer_.getTotalDuration() << "s."); } } // namespace wavemap diff --git a/interfaces/ros1/wavemap_ros/src/map_operations/decay_map_operation.cc b/interfaces/ros1/wavemap_ros/src/map_operations/decay_map_operation.cc new file mode 100644 index 000000000..a3c6696ce --- /dev/null +++ b/interfaces/ros1/wavemap_ros/src/map_operations/decay_map_operation.cc @@ -0,0 +1,69 @@ +#include "wavemap_ros/map_operations/decay_map_operation.h" + +#include +#include + +#include +#include + +#include "wavemap/core/utils/edit/multiply.h" + +namespace wavemap { +DECLARE_CONFIG_MEMBERS(DecayMapOperationConfig, + (once_every) + (decay_rate)); + +bool DecayMapOperationConfig::isValid(bool verbose) const { + bool all_valid = true; + + all_valid &= IS_PARAM_GT(once_every, 0.f, verbose); + all_valid &= IS_PARAM_GT(decay_rate, 0.f, verbose); + all_valid &= IS_PARAM_LT(decay_rate, 1.f, verbose); + + return all_valid; +} + +DecayMapOperation::DecayMapOperation(const DecayMapOperationConfig& config, + MapBase::Ptr occupancy_map, + std::shared_ptr thread_pool) + : MapOperationBase(std::move(occupancy_map)), + config_(config.checkValid()), + thread_pool_(std::move(thread_pool)) {} + +bool DecayMapOperation::shouldRun(const ros::Time& current_time) { + return config_.once_every < (current_time - last_run_timestamp_).toSec(); +} + +void DecayMapOperation::run(bool force_run) { + const ros::Time current_time = ros::Time::now(); + if (!force_run && !shouldRun(current_time)) { + return; + } + last_run_timestamp_ = current_time; + + // If the map is empty, there's no work to do + if (occupancy_map_->empty()) { + return; + } + + // Decay the map + timer_.start(); + if (auto* hashed_wavelet_octree = + dynamic_cast(occupancy_map_.get()); + hashed_wavelet_octree) { + edit::multiply(*hashed_wavelet_octree, config_.decay_rate, thread_pool_); + } else if (auto* hashed_chunked_wavelet_octree = + dynamic_cast( + occupancy_map_.get()); + hashed_chunked_wavelet_octree) { + edit::multiply(*hashed_chunked_wavelet_octree, config_.decay_rate, + thread_pool_); + } else { + ROS_WARN("Map decay is only supported for hash-based map data structures."); + } + timer_.stop(); + ROS_DEBUG_STREAM("Decayed map in " << timer_.getLastEpisodeDuration() + << "s. Total decaying time: " + << timer_.getTotalDuration() << "s."); +} +} // namespace wavemap diff --git a/interfaces/ros1/wavemap_ros/src/map_operations/map_ros_operation_factory.cc b/interfaces/ros1/wavemap_ros/src/map_operations/map_ros_operation_factory.cc index 59571508a..4e68d5cf2 100644 --- a/interfaces/ros1/wavemap_ros/src/map_operations/map_ros_operation_factory.cc +++ b/interfaces/ros1/wavemap_ros/src/map_operations/map_ros_operation_factory.cc @@ -5,6 +5,7 @@ #include #include "wavemap_ros/map_operations/crop_map_operation.h" +#include "wavemap_ros/map_operations/decay_map_operation.h" #include "wavemap_ros/map_operations/publish_map_operation.h" #include "wavemap_ros/map_operations/publish_pointcloud_operation.h" @@ -58,12 +59,20 @@ std::unique_ptr MapRosOperationFactory::create( case MapRosOperationType::kCropMap: if (const auto config = CropMapOperationConfig::from(params); config) { return std::make_unique( - config.value(), std::move(occupancy_map), std::move(transformer), - std::move(world_frame)); + config.value(), std::move(occupancy_map), std::move(thread_pool), + std::move(transformer), std::move(world_frame)); } else { ROS_ERROR("Crop map operation config could not be loaded."); return nullptr; } + case MapRosOperationType::kDecayMap: + if (const auto config = DecayMapOperationConfig::from(params); config) { + return std::make_unique( + config.value(), std::move(occupancy_map), std::move(thread_pool)); + } else { + ROS_ERROR("Decay map operation config could not be loaded."); + return nullptr; + } } LOG(ERROR) << "Factory does not (yet) support creation of map operation type " diff --git a/interfaces/ros1/wavemap_ros/src/utils/tf_transformer.cc b/interfaces/ros1/wavemap_ros/src/utils/tf_transformer.cc index 54cbf73e4..ca7bf0717 100644 --- a/interfaces/ros1/wavemap_ros/src/utils/tf_transformer.cc +++ b/interfaces/ros1/wavemap_ros/src/utils/tf_transformer.cc @@ -23,7 +23,8 @@ bool TfTransformer::waitForTransform(const std::string& to_frame_id, 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), {}); + sanitizeFrameId(from_frame_id), + ros::Time::UNINITIALIZED); } std::optional TfTransformer::lookupTransform( diff --git a/library/cpp/CMakeLists.txt b/library/cpp/CMakeLists.txt index 1a94b0a8d..602c8c048 100644 --- a/library/cpp/CMakeLists.txt +++ b/library/cpp/CMakeLists.txt @@ -12,6 +12,16 @@ option(USE_SYSTEM_EIGEN "Use system pre-installed Eigen" ON) option(USE_SYSTEM_GLOG "Use system pre-installed glog" ON) option(USE_SYSTEM_BOOST "Use system pre-installed Boost" ON) +# Build in Release mode by default +if (NOT CMAKE_BUILD_TYPE AND NOT CMAKE_CONFIGURATION_TYPES) + message(STATUS "Setting build type to Release as none was specified.") + set(CMAKE_BUILD_TYPE Release CACHE + STRING "Choose the type of build." FORCE) + # Set the possible values of build type for cmake-gui + set_property(CACHE CMAKE_BUILD_TYPE PROPERTY STRINGS + "Debug" "Release" "MinSizeRel" "RelWithDebInfo") +endif () + # CMake helpers and general wavemap tooling (e.g. to run clang-tidy CI) include(GNUInstallDirs) include(cmake/wavemap-extras.cmake) diff --git a/library/cpp/include/wavemap/core/map/hashed_blocks.h b/library/cpp/include/wavemap/core/map/hashed_blocks.h index 703badbb1..bf58a657b 100644 --- a/library/cpp/include/wavemap/core/map/hashed_blocks.h +++ b/library/cpp/include/wavemap/core/map/hashed_blocks.h @@ -42,7 +42,7 @@ class HashedBlocks : public MapBase, Index3D getMinBlockIndex() const { return block_map_.getMinBlockIndex(); } Index3D getMaxBlockIndex() const { return block_map_.getMaxBlockIndex(); } IndexElement getTreeHeight() const override { return 0; } - const MapBaseConfig& getConfig() { return config_; } + const MapBaseConfig& getConfig() const { return config_; } FloatingPoint getCellValue(const Index3D& index) const override; void setCellValue(const Index3D& index, FloatingPoint new_value) override; 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 3d4ee2a43..435f3009c 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 @@ -97,7 +97,7 @@ class HashedChunkedWaveletOctree : public MapBase { Index3D getBlockSize() const { return Index3D::Constant(cells_per_block_side_); } - const HashedChunkedWaveletOctreeConfig& getConfig() { return config_; } + const HashedChunkedWaveletOctreeConfig& getConfig() const { return config_; } FloatingPoint getCellValue(const Index3D& index) const override; FloatingPoint getCellValue(const OctreeIndex& index) const; diff --git a/library/cpp/include/wavemap/core/map/hashed_wavelet_octree.h b/library/cpp/include/wavemap/core/map/hashed_wavelet_octree.h index 986792a8f..be545d89d 100644 --- a/library/cpp/include/wavemap/core/map/hashed_wavelet_octree.h +++ b/library/cpp/include/wavemap/core/map/hashed_wavelet_octree.h @@ -91,7 +91,7 @@ class HashedWaveletOctree : public MapBase { Index3D getBlockSize() const { return Index3D::Constant(cells_per_block_side_); } - const HashedWaveletOctreeConfig& getConfig() { return config_; } + const HashedWaveletOctreeConfig& getConfig() const { return config_; } FloatingPoint getCellValue(const Index3D& index) const override; FloatingPoint getCellValue(const OctreeIndex& index) const; diff --git a/library/cpp/include/wavemap/core/map/wavelet_octree.h b/library/cpp/include/wavemap/core/map/wavelet_octree.h index 95e6beffc..a0227c16e 100644 --- a/library/cpp/include/wavemap/core/map/wavelet_octree.h +++ b/library/cpp/include/wavemap/core/map/wavelet_octree.h @@ -74,7 +74,7 @@ class WaveletOctree : public MapBase { Index3D getMinPossibleIndex() const; Index3D getMaxPossibleIndex() const; IndexElement getTreeHeight() const override { return config_.tree_height; } - const WaveletOctreeConfig& getConfig() { return config_; } + const WaveletOctreeConfig& getConfig() const { return config_; } FloatingPoint getCellValue(const Index3D& index) const override; FloatingPoint getCellValue(const OctreeIndex& index) const; diff --git a/library/cpp/include/wavemap/core/utils/edit/crop.h b/library/cpp/include/wavemap/core/utils/edit/crop.h new file mode 100644 index 000000000..faf2876dc --- /dev/null +++ b/library/cpp/include/wavemap/core/utils/edit/crop.h @@ -0,0 +1,154 @@ +#ifndef WAVEMAP_CORE_UTILS_EDIT_CROP_H_ +#define WAVEMAP_CORE_UTILS_EDIT_CROP_H_ + +#include + +#include "wavemap/core/common.h" +#include "wavemap/core/indexing/index_conversions.h" +#include "wavemap/core/indexing/ndtree_index.h" +#include "wavemap/core/utils/thread_pool.h" + +namespace wavemap::edit { +namespace detail { +template +void cropLeavesBatch(typename MapT::Block::OctreeType::NodeRefType node, + const OctreeIndex& node_index, FloatingPoint& node_value, + const Point3D& t_W_center, FloatingPoint radius, + FloatingPoint min_cell_width) { + // Decompress child values + using Transform = typename MapT::Block::Transform; + auto& node_details = node.data(); + auto child_values = Transform::backward({node_value, {node_details}}); + + // Set all children whose center is outside the cropping sphere to zero + for (NdtreeIndexRelativeChild child_idx = 0; + child_idx < OctreeIndex::kNumChildren; ++child_idx) { + const OctreeIndex child_index = node_index.computeChildIndex(child_idx); + const Point3D t_W_child = + convert::nodeIndexToCenterPoint(child_index, min_cell_width); + const FloatingPoint d_center_child = (t_W_child - t_W_center).norm(); + if (radius < d_center_child) { + child_values[child_idx] = 0; + if (0 < child_index.height) { + node.eraseChild(child_idx); + } + } + } + + // Compress + const auto [new_value, new_details] = + MapT::Block::Transform::forward(child_values); + node_details = new_details; + node_value = new_value; +} + +template +void cropNodeRecursive(typename MapT::Block::OctreeType::NodeRefType node, + const OctreeIndex& node_index, FloatingPoint& node_value, + const Point3D& t_W_center, FloatingPoint radius, + FloatingPoint min_cell_width, + IndexElement termination_height) { + using NodeRefType = decltype(node); + + // Decompress child values + using Transform = typename MapT::Block::Transform; + auto& node_details = node.data(); + auto child_values = Transform::backward({node_value, {node_details}}); + + // Handle each child + for (NdtreeIndexRelativeChild child_idx = 0; + child_idx < OctreeIndex::kNumChildren; ++child_idx) { + // If the node is fully inside the cropping sphere, do nothing + const auto child_aabb = + convert::nodeIndexToAABB(node_index, min_cell_width); + if (child_aabb.maxDistanceTo(t_W_center) < radius) { + continue; + } + + // If the node is fully outside the cropping sphere, set it to zero + auto& child_value = child_values[child_idx]; + if (radius < child_aabb.minDistanceTo(t_W_center)) { + child_value = 0; + node.eraseChild(child_idx); + continue; + } + + // Otherwise, continue at a higher resolution + NodeRefType child_node = node.getOrAllocateChild(child_idx); + const OctreeIndex child_index = node_index.computeChildIndex(child_idx); + if (child_index.height <= termination_height + 1) { + cropLeavesBatch(child_node, child_index, child_value, t_W_center, + radius, min_cell_width); + } else { + cropNodeRecursive(child_node, child_index, child_value, t_W_center, + radius, min_cell_width, termination_height); + } + } + + // Compress + const auto [new_value, new_details] = Transform::forward(child_values); + node_details = new_details; + node_value = new_value; +} +} // namespace detail + +template +void crop_to_sphere(MapT& map, const Point3D& t_W_center, FloatingPoint radius, + IndexElement termination_height, + const std::shared_ptr& thread_pool = nullptr) { + using NodePtrType = typename MapT::Block::OctreeType::NodePtrType; + const IndexElement tree_height = map.getTreeHeight(); + const FloatingPoint min_cell_width = map.getMinCellWidth(); + + // Check all blocks + for (auto it = map.getHashMap().begin(); it != map.getHashMap().end();) { + // Start by testing at the block level + const Index3D& block_index = it->first; + const OctreeIndex block_node_index{tree_height, block_index}; + const auto block_aabb = + convert::nodeIndexToAABB(block_node_index, min_cell_width); + // If the block is fully inside the cropping sphere, do nothing + if (block_aabb.maxDistanceTo(t_W_center) < radius) { + ++it; + continue; + } + // If the block is fully outside the cropping sphere, erase it entirely + if (radius < block_aabb.minDistanceTo(t_W_center)) { + it = map.getHashMap().erase(it); + continue; + } + + // Since the block overlaps with the sphere's boundary, we need to process + // it at a higher resolution by recursing over its cells + auto& block = it->second; + // Indicate that the block has changed + block.setLastUpdatedStamp(); + // Get pointers to the root value and node, which contain the wavelet + // scale and detail coefficients, respectively + FloatingPoint* root_value_ptr = &block.getRootScale(); + NodePtrType root_node_ptr = &block.getRootNode(); + // Recursively crop all nodes + if (thread_pool) { + thread_pool->add_task([root_node_ptr, block_node_index, root_value_ptr, + t_W_center, radius, min_cell_width, + termination_height]() { + detail::cropNodeRecursive(*root_node_ptr, block_node_index, + *root_value_ptr, t_W_center, radius, + min_cell_width, termination_height); + }); + } else { + detail::cropNodeRecursive(*root_node_ptr, block_node_index, + *root_value_ptr, t_W_center, radius, + min_cell_width, termination_height); + } + // Advance to the next block + ++it; + } + // Wait for all parallel jobs to finish + if (thread_pool) { + thread_pool->wait_all(); + } +} +} // namespace wavemap::edit + +#endif // WAVEMAP_CORE_UTILS_EDIT_CROP_H_ diff --git a/library/cpp/include/wavemap/core/utils/edit/multiply.h b/library/cpp/include/wavemap/core/utils/edit/multiply.h new file mode 100644 index 000000000..fe0174d1b --- /dev/null +++ b/library/cpp/include/wavemap/core/utils/edit/multiply.h @@ -0,0 +1,55 @@ +#ifndef WAVEMAP_CORE_UTILS_EDIT_MULTIPLY_H_ +#define WAVEMAP_CORE_UTILS_EDIT_MULTIPLY_H_ + +#include + +#include "wavemap/core/common.h" +#include "wavemap/core/utils/thread_pool.h" + +namespace wavemap::edit { +namespace detail { +template +void multiplyNodeRecursive(typename MapT::Block::OctreeType::NodeRefType node, + FloatingPoint multiplier) { + // Multiply + node.data() *= multiplier; + + // Recursively handle all children + for (int child_idx = 0; child_idx < OctreeIndex::kNumChildren; ++child_idx) { + if (auto child_node = node.getChild(child_idx); child_node) { + multiplyNodeRecursive(*child_node, multiplier); + } + } +} +} // namespace detail + +template +void multiply(MapT& map, FloatingPoint multiplier, + const std::shared_ptr& thread_pool = nullptr) { + using NodePtrType = typename MapT::Block::OctreeType::NodePtrType; + + // Process all blocks + for (auto& [block_index, block] : map.getHashMap()) { + // Indicate that the block has changed + block.setLastUpdatedStamp(); + // Multiply the block's average value (wavelet scale coefficient) + FloatingPoint& root_value = block.getRootScale(); + root_value *= multiplier; + // Recursively multiply all node values (wavelet detail coefficients) + NodePtrType root_node_ptr = &block.getRootNode(); + if (thread_pool) { + thread_pool->add_task([root_node_ptr, multiplier]() { + detail::multiplyNodeRecursive(*root_node_ptr, multiplier); + }); + } else { + detail::multiplyNodeRecursive(*root_node_ptr, multiplier); + } + } + // Wait for all parallel jobs to finish + if (thread_pool) { + thread_pool->wait_all(); + } +} +} // namespace wavemap::edit + +#endif // WAVEMAP_CORE_UTILS_EDIT_MULTIPLY_H_ diff --git a/library/cpp/include/wavemap/core/utils/edit/sample.h b/library/cpp/include/wavemap/core/utils/edit/sample.h new file mode 100644 index 000000000..0de5b9dc5 --- /dev/null +++ b/library/cpp/include/wavemap/core/utils/edit/sample.h @@ -0,0 +1,122 @@ +#ifndef WAVEMAP_CORE_UTILS_EDIT_SAMPLE_H_ +#define WAVEMAP_CORE_UTILS_EDIT_SAMPLE_H_ + +#include +#include + +#include "wavemap/core/common.h" +#include "wavemap/core/utils/thread_pool.h" + +namespace wavemap::edit { +namespace detail { +template +void sampleLeavesBatch(typename MapT::Block::OctreeType::NodeRefType node, + const OctreeIndex& node_index, FloatingPoint& node_value, + SamplingFn&& sampling_function, + FloatingPoint min_cell_width) { + // Decompress child values + using Transform = typename MapT::Block::Transform; + auto& node_details = node.data(); + auto child_values = Transform::backward({node_value, {node_details}}); + + // Sample all children + for (NdtreeIndexRelativeChild child_idx = 0; + child_idx < OctreeIndex::kNumChildren; ++child_idx) { + const OctreeIndex child_index = node_index.computeChildIndex(child_idx); + const Point3D t_W_child = + convert::nodeIndexToCenterPoint(child_index, min_cell_width); + child_values[child_idx] = sampling_function(t_W_child); + } + + // Compress + const auto [new_value, new_details] = + MapT::Block::Transform::forward(child_values); + node_details = new_details; + node_value = new_value; +} + +template +void sampleNodeRecursive(typename MapT::Block::OctreeType::NodeRefType node, + const OctreeIndex& node_index, + FloatingPoint& node_value, + SamplingFn&& sampling_function, + FloatingPoint min_cell_width, + IndexElement termination_height = 0) { + using NodeRefType = decltype(node); + + // Decompress child values + using Transform = typename MapT::Block::Transform; + auto& node_details = node.data(); + auto child_values = Transform::backward({node_value, {node_details}}); + + // Handle each child + for (NdtreeIndexRelativeChild child_idx = 0; + child_idx < OctreeIndex::kNumChildren; ++child_idx) { + NodeRefType child_node = node.getOrAllocateChild(child_idx); + const OctreeIndex child_index = node_index.computeChildIndex(child_idx); + auto& child_value = child_values[child_idx]; + if (child_index.height <= termination_height + 1) { + sampleLeavesBatch(child_node, child_index, child_value, + std::forward(sampling_function), + min_cell_width); + } else { + sampleNodeRecursive(child_node, child_index, child_value, + std::forward(sampling_function), + min_cell_width, termination_height); + } + } + + // Compress + const auto [new_value, new_details] = Transform::forward(child_values); + node_details = new_details; + node_value = new_value; +} +} // namespace detail + +template +void sample(MapT& map, SamplingFn sampling_function, + IndexElement termination_height = 0, + const std::shared_ptr& thread_pool = nullptr) { + using NodePtrType = typename MapT::Block::OctreeType::NodePtrType; + const IndexElement tree_height = map.getTreeHeight(); + const FloatingPoint min_cell_width = map.getMinCellWidth(); + // Evaluate the sampling function over the entire map + map.forEachBlock([&thread_pool, &sampling_function, tree_height, + min_cell_width, termination_height]( + const Index3D& block_index, auto& block) { + // Indicate that the block has changed + block.setLastUpdatedStamp(); + block.setNeedsPruning(); + + // Get pointers to the root value and node, which contain the wavelet + // scale and detail coefficients, respectively + FloatingPoint* root_value_ptr = &block.getRootScale(); + NodePtrType root_node_ptr = &block.getRootNode(); + const OctreeIndex root_node_index{tree_height, block_index}; + + // Recursively crop all nodes + if (thread_pool) { + thread_pool->add_task([root_node_ptr, root_node_index, root_value_ptr, + block_ptr = &block, sampling_function, + min_cell_width, termination_height]() { + detail::sampleNodeRecursive(*root_node_ptr, root_node_index, + *root_value_ptr, sampling_function, + min_cell_width, termination_height); + block_ptr->prune(); + }); + } else { + detail::sampleNodeRecursive(*root_node_ptr, root_node_index, + *root_value_ptr, sampling_function, + min_cell_width, termination_height); + block.prune(); + } + }); + + // Wait for all parallel jobs to finish + if (thread_pool) { + thread_pool->wait_all(); + } +} +} // namespace wavemap::edit + +#endif // WAVEMAP_CORE_UTILS_EDIT_SAMPLE_H_ diff --git a/library/cpp/include/wavemap/core/utils/edit/transform.h b/library/cpp/include/wavemap/core/utils/edit/transform.h new file mode 100644 index 000000000..32dc8ed2c --- /dev/null +++ b/library/cpp/include/wavemap/core/utils/edit/transform.h @@ -0,0 +1,103 @@ +#ifndef WAVEMAP_CORE_UTILS_EDIT_TRANSFORM_H_ +#define WAVEMAP_CORE_UTILS_EDIT_TRANSFORM_H_ + +#include +#include +#include + +#include "wavemap/core/common.h" +#include "wavemap/core/data_structure/aabb.h" +#include "wavemap/core/indexing/index_conversions.h" +#include "wavemap/core/map/hashed_wavelet_octree.h" +#include "wavemap/core/utils/edit/sample.h" +#include "wavemap/core/utils/iterate/grid_iterator.h" +#include "wavemap/core/utils/query/map_interpolator.h" +#include "wavemap/core/utils/thread_pool.h" + +namespace wavemap::edit { +template +std::unique_ptr transform( + const MapT& B_map, const Transformation3D& T_AB, + const std::shared_ptr& thread_pool = nullptr) { + using NodePtrType = typename MapT::Block::OctreeType::NodePtrType; + const IndexElement tree_height = B_map.getTreeHeight(); + const FloatingPoint min_cell_width = B_map.getMinCellWidth(); + const FloatingPoint block_width = + convert::heightToCellWidth(min_cell_width, tree_height); + const FloatingPoint block_width_inv = 1.f / block_width; + + // Allocate blocks in the result map + auto A_map = std::make_unique(B_map.getConfig()); + B_map.forEachBlock([&A_map, &T_AB, tree_height, min_cell_width, + block_width_inv](const Index3D& block_index, + const auto& /*block*/) { + AABB A_aabb{}; + const auto B_block_aabb = + convert::nodeIndexToAABB<3>({tree_height, block_index}, min_cell_width); + for (int idx = 0; idx < B_block_aabb.kNumCorners; ++idx) { + const Point3D& B_corner = B_block_aabb.corner_point(idx); + const Point3D A_corner = T_AB * B_corner; + A_aabb.includePoint(A_corner); + } + const Index3D A_block_index_min = + convert::pointToFloorIndex(A_aabb.min, block_width_inv); + const Index3D A_block_index_max = + convert::pointToCeilIndex(A_aabb.max, block_width_inv); + for (const auto& A_block_index : + Grid(A_block_index_min, A_block_index_max)) { + A_map->getOrAllocateBlock(A_block_index); + } + }); + + // Populate map A by interpolating map B + const Transformation3D T_BA = T_AB.inverse(); + QueryAccelerator B_accelerator{B_map}; + A_map->forEachBlock( + [&B_accelerator, &T_BA, &thread_pool, tree_height, min_cell_width]( + const Index3D& block_index, auto& block) { + // Indicate that the block has changed + block.setLastUpdatedStamp(); + block.setNeedsPruning(); + + // Get pointers to the root value and node, which contain the wavelet + // scale and detail coefficients, respectively + FloatingPoint* root_value_ptr = &block.getRootScale(); + NodePtrType root_node_ptr = &block.getRootNode(); + const OctreeIndex root_node_index{tree_height, block_index}; + + // Recursively crop all nodes + if (thread_pool) { + thread_pool->add_task([B_accelerator, &T_BA, root_node_ptr, + root_node_index, root_value_ptr, + block_ptr = &block, min_cell_width]() mutable { + detail::sampleNodeRecursive( + *root_node_ptr, root_node_index, *root_value_ptr, + [&B_accelerator, &T_BA](const Point3D& A_point) { + const auto B_point = T_BA * A_point; + return interpolate::trilinear(B_accelerator, B_point); + }, + min_cell_width); + block_ptr->prune(); + }); + } else { + detail::sampleNodeRecursive( + *root_node_ptr, root_node_index, *root_value_ptr, + [&B_accelerator, &T_BA](const Point3D& A_point) { + const auto B_point = T_BA * A_point; + return interpolate::trilinear(B_accelerator, B_point); + }, + min_cell_width); + block.prune(); + } + }); + + // Wait for all parallel jobs to finish + if (thread_pool) { + thread_pool->wait_all(); + } + + return A_map; +} +} // namespace wavemap::edit + +#endif // WAVEMAP_CORE_UTILS_EDIT_TRANSFORM_H_ diff --git a/library/cpp/src/core/config/value_with_unit.cc b/library/cpp/src/core/config/value_with_unit.cc index d8ab30558..ef84b3425 100644 --- a/library/cpp/src/core/config/value_with_unit.cc +++ b/library/cpp/src/core/config/value_with_unit.cc @@ -7,23 +7,33 @@ namespace wavemap::param { // clang-format off static const std::map> UnitToSi{ - {"meters", {1e0f, SiUnit::kMeters}}, - {"m", {1e0f, SiUnit::kMeters}}, - {"decimeters", {1e-1f, SiUnit::kMeters}}, - {"dm", {1e-1f, SiUnit::kMeters}}, - {"centimeters", {1e-2f, SiUnit::kMeters}}, - {"cm", {1e-2f, SiUnit::kMeters}}, - {"millimeters", {1e-3f, SiUnit::kMeters}}, - {"mm", {1e-3f, SiUnit::kMeters}}, - {"radians", {1.f, SiUnit::kRadians}}, - {"rad", {1.f, SiUnit::kRadians}}, - {"degrees", {kPi / 180.f, SiUnit::kRadians}}, - {"deg", {kPi / 180.f, SiUnit::kRadians}}, - {"pixels", {1.f, SiUnit::kPixels}}, - {"px", {1.f, SiUnit::kPixels}}, - {"seconds", {1.f, SiUnit::kSeconds}}, - {"sec", {1.f, SiUnit::kSeconds}}, - {"s", {1.f, SiUnit::kSeconds}}, + {"kilometers", {1e3f, SiUnit::kMeters}}, + {"km", {1e3f, SiUnit::kMeters}}, + {"meters", {1e0f, SiUnit::kMeters}}, + {"m", {1e0f, SiUnit::kMeters}}, + {"decimeters", {1e-1f, SiUnit::kMeters}}, + {"dm", {1e-1f, SiUnit::kMeters}}, + {"centimeters", {1e-2f, SiUnit::kMeters}}, + {"cm", {1e-2f, SiUnit::kMeters}}, + {"millimeters", {1e-3f, SiUnit::kMeters}}, + {"mm", {1e-3f, SiUnit::kMeters}}, + {"radians", {1.f, SiUnit::kRadians}}, + {"rad", {1.f, SiUnit::kRadians}}, + {"degrees", {kPi / 180.f, SiUnit::kRadians}}, + {"deg", {kPi / 180.f, SiUnit::kRadians}}, + {"pixels", {1.f, SiUnit::kPixels}}, + {"px", {1.f, SiUnit::kPixels}}, + {"hours", {3600.f, SiUnit::kSeconds}}, + {"h", {3600.f, SiUnit::kSeconds}}, + {"minutes", {60.f, SiUnit::kSeconds}}, + {"seconds", {1.f, SiUnit::kSeconds}}, + {"sec", {1.f, SiUnit::kSeconds}}, + {"s", {1.f, SiUnit::kSeconds}}, + {"milliseconds", {1e-3f, SiUnit::kSeconds}}, + {"ms", {1e-3f, SiUnit::kSeconds}}, + {"microseconds", {1e-6f, SiUnit::kSeconds}}, + {"nanoseconds", {1e-9f, SiUnit::kSeconds}}, + {"ns", {1e-9f, SiUnit::kSeconds}}, }; // clang-format on diff --git a/library/python/src/convert.cc b/library/python/src/convert.cc index 424aad96a..c0e719855 100644 --- a/library/python/src/convert.cc +++ b/library/python/src/convert.cc @@ -43,5 +43,12 @@ void add_convert_module(nb::module_& m_convert) { "(max map resolution).\n" " :param height: The desired height (resolution level) of " "the node index."); + + m_convert.def("node_index_to_center_point", + &convert::nodeIndexToCenterPoint<3>, "node_index"_a, + "min_cell_width"_a, + "Compute the center point of a node given its node index.\n\n" + " :param min_cell_width: The grid resolution at height 0 " + "(max map resolution)."); } } // namespace wavemap diff --git a/library/python/src/maps.cc b/library/python/src/maps.cc index 6aa29c3bd..1bead5cb0 100644 --- a/library/python/src/maps.cc +++ b/library/python/src/maps.cc @@ -1,6 +1,7 @@ #include "pywavemap/maps.h" #include +#include #include #include @@ -96,7 +97,7 @@ void add_map_bindings(nb::module_& m) { [](const std::filesystem::path& file_path) -> std::shared_ptr { std::shared_ptr map; - if (wavemap::io::fileToMap(file_path, map)) { + if (io::fileToMap(file_path, map)) { return map; } return nullptr; @@ -105,8 +106,104 @@ void add_map_bindings(nb::module_& m) { .def( "store", [](const MapBase& self, const std::filesystem::path& file_path) - -> bool { return wavemap::io::mapToFile(self, file_path); }, - "file_path"_a, "Store a wavemap map as a .wvmp file."); + -> bool { return io::mapToFile(self, file_path); }, + "file_path"_a, "Store a wavemap map as a .wvmp file.") + .def( + "get_occupied_node_indices", + [](const MapBase& self, FloatingPoint log_odds_occupancy_threshold) { + // Get the node indices + std::vector node_indices; + self.forEachLeaf( + [&node_indices, log_odds_occupancy_threshold]( + const OctreeIndex& node_index, FloatingPoint node_value) { + if (log_odds_occupancy_threshold < node_value) { + node_indices.emplace_back(node_index); + } + }); + + // Create the raw results array and wrap it in a Python capsule that + // deallocates it when all references to it expire + auto* results = new IndexElement[4 * node_indices.size()]; + nb::capsule owner(results, [](void* p) noexcept { + delete[] reinterpret_cast(p); + }); + + // Populate the results + size_t idx = 0; + for (const auto& node_index : node_indices) { + results[idx + 0] = node_index.height; + results[idx + 1] = node_index.position.x(); + results[idx + 2] = node_index.position.y(); + results[idx + 3] = node_index.position.z(); + idx += 4; + } + + // Return results as numpy array + return nb::ndarray{ + results, {node_indices.size(), 4u}, owner}; + }, + "threshold"_a = 1e-3f, + "Retrieve the indices of all occupied leaf nodes.\n\n" + " :param threshold: The log-odds threshold above which a node is " + "considered occupied.\n" + " :returns: An (N, 4) numpy array where each row contains the " + "height, x, y, and z indices (int32) of an occupied leaf node.") + .def( + "get_occupied_pointcloud", + [](const MapBase& self, FloatingPoint log_odds_occupancy_threshold) { + // Get the center points of all occupied high resolution cells + std::vector pointcloud; + const FloatingPoint min_cell_width = self.getMinCellWidth(); + self.forEachLeaf( + [&pointcloud, log_odds_occupancy_threshold, min_cell_width]( + const OctreeIndex& node_index, FloatingPoint node_value) { + if (log_odds_occupancy_threshold < node_value) { + if (node_index.height == 0) { + const Point3D center = convert::indexToCenterPoint( + node_index.position, min_cell_width); + pointcloud.emplace_back(center); + } else { + const Index3D node_min_corner = + convert::nodeIndexToMinCornerIndex(node_index); + const Index3D node_max_corner = + convert::nodeIndexToMaxCornerIndex(node_index); + for (const Index3D& index : + Grid(node_min_corner, node_max_corner)) { + const Point3D center = + convert::indexToCenterPoint(index, min_cell_width); + pointcloud.emplace_back(center); + } + } + } + }); + + // 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[3 * pointcloud.size()]; + nb::capsule owner(results, [](void* p) noexcept { + delete[] reinterpret_cast(p); + }); + + // Populate the results + size_t idx = 0; + for (const auto& point : pointcloud) { + results[idx + 0] = point.x(); + results[idx + 1] = point.y(); + results[idx + 2] = point.z(); + idx += 3; + } + + // Return results as numpy array + return nb::ndarray{ + results, {pointcloud.size(), 3u}, owner}; + }, + "threshold"_a = 1e-3f, + "Retrieve the center points of all occupied cells at the highest " + "resolution.\n\n" + " :param threshold: The log-odds threshold above which a cell is " + "considered occupied.\n" + " :returns: An (N, 3) numpy array where each row contains the " + "x, y, and z coordinates (float32) of an occupied cell center."); nb::class_( m, "HashedWaveletOctree", diff --git a/tooling/schemas/wavemap/map_operations/crop_map_operation.json b/tooling/schemas/wavemap/map_operations/crop_map_operation.json index d155cc872..a8983f0fb 100644 --- a/tooling/schemas/wavemap/map_operations/crop_map_operation.json +++ b/tooling/schemas/wavemap/map_operations/crop_map_operation.json @@ -12,14 +12,22 @@ "$ref": "../value_with_unit/convertible_to_seconds.json" }, "body_frame": { - "description": "Name of the TF frame to treat as the center point. Usually the robot's body frame. When the cropper runs, all blocks that are further than remove_blocks_beyond_distance from this point are deleted.", + "description": "Name of the TF frame to treat as the center point. Usually the robot's body frame. When the cropper runs, all cells that are further than `radius` from this point are deleted.", "type": "string", "examples": [ "body" ] }, - "remove_blocks_beyond_distance": { - "description": "Distance beyond which blocks are deleted when the cropper is executed.", + "tf_time_offset": { + "description": "Time offset applied when retrieving the transform from body_frame to world_frame. Set to -1 to use the most recent transform available in ROS TF, ignoring timestamps (default). If set to a non-negative value, the transform lookup uses a timestamp of `ros::Time::now() - tf_time_offset`.", + "$ref": "../value_with_unit/convertible_to_seconds.json" + }, + "radius": { + "description": "Distance beyond which to remove cells from the map.", + "$ref": "../value_with_unit/convertible_to_meters.json" + }, + "max_update_resolution": { + "description": "Maximum resolution at which the crop is applied. Set to 0 to match the map's maximum resolution (default). Setting a higher value reduces computation but produces more jagged borders.", "$ref": "../value_with_unit/convertible_to_meters.json" } } diff --git a/tooling/schemas/wavemap/map_operations/decay_map_operation.json b/tooling/schemas/wavemap/map_operations/decay_map_operation.json new file mode 100644 index 000000000..c46a23c10 --- /dev/null +++ b/tooling/schemas/wavemap/map_operations/decay_map_operation.json @@ -0,0 +1,19 @@ +{ + "$schema": "https://json-schema.org/draft-07/schema", + "description": "Properties of a single map decay operation.", + "type": "object", + "additionalProperties": false, + "properties": { + "type": { + "const": "decay_map" + }, + "once_every": { + "description": "Time period controlling how often the map is decayed.", + "$ref": "../value_with_unit/convertible_to_seconds.json" + }, + "decay_rate": { + "description": "Decay rate in the range (0, 1), applied to each map value as `map_value *= decay_rate` at each operation run.", + "type": "number" + } + } +} diff --git a/tooling/schemas/wavemap/map_operations/map_operation_base.json b/tooling/schemas/wavemap/map_operations/map_operation_base.json index 23a053d81..6aeaf05d1 100644 --- a/tooling/schemas/wavemap/map_operations/map_operation_base.json +++ b/tooling/schemas/wavemap/map_operations/map_operation_base.json @@ -14,7 +14,8 @@ "prune_map", "publish_map", "publish_pointcloud", - "crop_map" + "crop_map", + "decay_map" ] } }, @@ -33,6 +34,9 @@ }, { "$ref": "crop_map_operation.json" + }, + { + "$ref": "decay_map_operation.json" } ] } diff --git a/tooling/schemas/wavemap/value_with_unit/convertible_to_meters.json b/tooling/schemas/wavemap/value_with_unit/convertible_to_meters.json index 6328f534c..4f14495ef 100644 --- a/tooling/schemas/wavemap/value_with_unit/convertible_to_meters.json +++ b/tooling/schemas/wavemap/value_with_unit/convertible_to_meters.json @@ -5,6 +5,12 @@ "minProperties": 1, "maxProperties": 1, "properties": { + "kilometers": { + "type": "number" + }, + "km": { + "type": "number" + }, "meters": { "type": "number" }, diff --git a/tooling/schemas/wavemap/value_with_unit/convertible_to_seconds.json b/tooling/schemas/wavemap/value_with_unit/convertible_to_seconds.json index 69bb9bb3f..a4cfc291b 100644 --- a/tooling/schemas/wavemap/value_with_unit/convertible_to_seconds.json +++ b/tooling/schemas/wavemap/value_with_unit/convertible_to_seconds.json @@ -5,6 +5,15 @@ "minProperties": 1, "maxProperties": 1, "properties": { + "hours": { + "type": "number" + }, + "h": { + "type": "number" + }, + "minutes": { + "type": "number" + }, "seconds": { "type": "number" }, @@ -13,6 +22,21 @@ }, "s": { "type": "number" + }, + "milliseconds": { + "type": "number" + }, + "ms": { + "type": "number" + }, + "microseconds": { + "type": "number" + }, + "nanoseconds": { + "type": "number" + }, + "ns": { + "type": "number" } } }