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..172ac007d --- /dev/null +++ b/examples/cpp/edit/CMakeLists.txt @@ -0,0 +1,20 @@ +# Binaries +add_executable(crop_map crop_map.cc) +set_wavemap_target_properties(crop_map) +target_link_libraries(crop_map PUBLIC + wavemap::wavemap_core wavemap::wavemap_io) + +add_executable(multiply_map multiply_map.cc) +set_wavemap_target_properties(multiply_map) +target_link_libraries(multiply_map PUBLIC + wavemap::wavemap_core wavemap::wavemap_io) + +add_executable(sum_map sum_map.cc) +set_wavemap_target_properties(sum_map) +target_link_libraries(sum_map PUBLIC + wavemap::wavemap_core wavemap::wavemap_io) + +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/crop_map.cc b/examples/cpp/edit/crop_map.cc new file mode 100644 index 000000000..06907211d --- /dev/null +++ b/examples/cpp/edit/crop_map.cc @@ -0,0 +1,32 @@ +#include + +#include +#include +#include + +using namespace wavemap; // NOLINT +int main(int, char**) { + // Load the map + const std::filesystem::path home_dir = CHECK_NOTNULL(getenv("HOME")); + const std::filesystem::path input_map_path = home_dir / "your_map.wvmp"; + MapBase::Ptr map_base; + bool success = 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); + + // Crop the map + const Point3D t_W_center{-2.2, -1.4, 0.0}; + const FloatingPoint radius = 3.0; + const Sphere cropping_sphere{t_W_center, radius}; + auto thread_pool = std::make_shared(); // Optional + edit::crop(*map, cropping_sphere, 0, thread_pool); + + // Save the map + const std::filesystem::path output_map_path = + home_dir / "your_map_cropped.wvmp"; + success &= io::mapToFile(*map, output_map_path); + CHECK(success); +} diff --git a/examples/cpp/edit/multiply_map.cc b/examples/cpp/edit/multiply_map.cc new file mode 100644 index 000000000..7946f8bc9 --- /dev/null +++ b/examples/cpp/edit/multiply_map.cc @@ -0,0 +1,30 @@ +#include + +#include +#include +#include + +using namespace wavemap; // NOLINT +int main(int, char**) { + // Load the map + const std::filesystem::path home_dir = CHECK_NOTNULL(getenv("HOME")); + const std::filesystem::path input_map_path = home_dir / "your_map.wvmp"; + MapBase::Ptr map_base; + bool success = 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); + + // Use the multiply method to implement exponential forgetting + const FloatingPoint decay_factor = 0.9; + auto thread_pool = std::make_shared(); // Optional + edit::multiply(*map, decay_factor, thread_pool); + + // Save the map + const std::filesystem::path output_map_path = + home_dir / "your_map_decayed.wvmp"; + success &= io::mapToFile(*map, output_map_path); + CHECK(success); +} diff --git a/examples/cpp/edit/sum_map.cc b/examples/cpp/edit/sum_map.cc new file mode 100644 index 000000000..508b29cf5 --- /dev/null +++ b/examples/cpp/edit/sum_map.cc @@ -0,0 +1,52 @@ +#include + +#include +#include +#include +#include +#include +#include +#include + +using namespace wavemap; // NOLINT +int main(int, char**) { + // Load the map + const std::filesystem::path home_dir = CHECK_NOTNULL(getenv("HOME")); + const std::filesystem::path input_map_path = home_dir / "your_map.wvmp"; + MapBase::Ptr map_base; + bool success = 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); + + // Crop the map + const Point3D t_W_center{-2.2, -1.4, 0.0}; + const FloatingPoint radius = 3.0; + const Sphere cropping_sphere{t_W_center, radius}; + auto thread_pool = std::make_shared(); // Optional + edit::crop(*map, cropping_sphere, 0, thread_pool); + + // Create a translated copy + Transformation3D T_AB; + T_AB.getPosition() = {5.0, 5.0, 0.0}; + auto map_translated = edit::transform(*map, T_AB, thread_pool); + + // Merge them together + edit::sum(*map, *map_translated); + + // Set a box in the map to free + AABB box{{6.f, 6.f, -2.f}, {10.f, 10.f, 2.f}}; + edit::sum(*map, box, -1.f, thread_pool); + + // Set a sphere in the map to occupied + Sphere sphere{{8.f, 8.f, 0.f}, 1.5f}; + edit::sum(*map, sphere, 2.f, thread_pool); + + // Save the map + const std::filesystem::path output_map_path = + home_dir / "your_map_merged.wvmp"; + success &= io::mapToFile(*map, output_map_path); + CHECK(success); +} diff --git a/examples/cpp/edit/transform_map.cc b/examples/cpp/edit/transform_map.cc new file mode 100644 index 000000000..6b5502ac4 --- /dev/null +++ b/examples/cpp/edit/transform_map.cc @@ -0,0 +1,33 @@ +#include + +#include +#include +#include + +using namespace wavemap; // NOLINT +int main(int, char**) { + // Load the map + const std::filesystem::path home_dir = CHECK_NOTNULL(getenv("HOME")); + const std::filesystem::path input_map_path = home_dir / "your_map.wvmp"; + MapBase::Ptr map_base; + bool success = 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 + Transformation3D T_AB; + T_AB.getRotation() = Rotation3D{0.f, 1.f, 0.f, 0.f}; + + // Transform the map + auto thread_pool = std::make_shared(); // Optional + map = edit::transform(*map, T_AB, thread_pool); + + // Save the map + const std::filesystem::path output_map_path = + home_dir / "your_map_transformed.wvmp"; + success &= io::mapToFile(*map, output_map_path); + CHECK(success); +} diff --git a/examples/cpp/io/load_map_from_file.cc b/examples/cpp/io/load_map_from_file.cc index 5862575ae..52cbf4939 100644 --- a/examples/cpp/io/load_map_from_file.cc +++ b/examples/cpp/io/load_map_from_file.cc @@ -1,10 +1,11 @@ #include +using namespace wavemap; // NOLINT int main(int, char**) { // Create a smart pointer that will own the loaded map - wavemap::MapBase::Ptr loaded_map; + MapBase::Ptr loaded_map; // Load the map - const bool success = - wavemap::io::fileToMap("/path/to/your/map.wvmp", loaded_map); + const bool success = io::fileToMap("/path/to/your/map.wvmp", loaded_map); + CHECK(success); } diff --git a/examples/cpp/io/load_map_from_stream.cc b/examples/cpp/io/load_map_from_stream.cc index 1f855230b..22a70d043 100644 --- a/examples/cpp/io/load_map_from_stream.cc +++ b/examples/cpp/io/load_map_from_stream.cc @@ -2,13 +2,15 @@ #include +using namespace wavemap; // NOLINT int main(int, char**) { // Create a smart pointer that will own the loaded map - wavemap::MapBase::Ptr loaded_map; + MapBase::Ptr loaded_map; // Create an input stream for illustration purposes std::istrstream input_stream{""}; // Load the map - const bool success = wavemap::io::streamToMap(input_stream, loaded_map); + const bool success = io::streamToMap(input_stream, loaded_map); + CHECK(success); } diff --git a/examples/cpp/io/save_map_to_file.cc b/examples/cpp/io/save_map_to_file.cc index fa7c862b5..df865c1e6 100644 --- a/examples/cpp/io/save_map_to_file.cc +++ b/examples/cpp/io/save_map_to_file.cc @@ -1,10 +1,12 @@ #include +using namespace wavemap; // NOLINT int main(int, char**) { // Create an empty map for illustration purposes - wavemap::HashedWaveletOctreeConfig config; - wavemap::HashedWaveletOctree map(config); + HashedWaveletOctreeConfig config; + HashedWaveletOctree map(config); // Save the map - const bool success = wavemap::io::mapToFile(map, "/path/to/your/map.wvmp"); + const bool success = io::mapToFile(map, "/path/to/your/map.wvmp"); + CHECK(success); } diff --git a/examples/cpp/io/save_map_to_stream.cc b/examples/cpp/io/save_map_to_stream.cc index 251beec78..b3c67e525 100644 --- a/examples/cpp/io/save_map_to_stream.cc +++ b/examples/cpp/io/save_map_to_stream.cc @@ -2,16 +2,18 @@ #include +using namespace wavemap; // NOLINT int main(int, char**) { // Create an empty map for illustration purposes - wavemap::HashedWaveletOctreeConfig config; - wavemap::HashedWaveletOctree map(config); + HashedWaveletOctreeConfig config; + HashedWaveletOctree map(config); // Create an output stream for illustration purposes std::ostrstream output_stream; // Save the map - bool success = wavemap::io::mapToStream(map, output_stream); + bool success = io::mapToStream(map, output_stream); output_stream.flush(); success &= output_stream.good(); + CHECK(success); } diff --git a/examples/python/edit/crop_map.py b/examples/python/edit/crop_map.py new file mode 100644 index 000000000..61998aaa6 --- /dev/null +++ b/examples/python/edit/crop_map.py @@ -0,0 +1,16 @@ +import os +import numpy as np +import pywavemap as wave + +# Load the map +user_home = os.path.expanduser('~') +input_map_path = os.path.join(user_home, "your_map.wvmp") +your_map = wave.Map.load(input_map_path) + +# Crop the map +cropping_sphere = wave.Sphere(center=np.array([-2.2, -1.4, 0.0]), radius=3.0) +wave.edit.crop(your_map, cropping_sphere) + +# Save the map +output_map_path = os.path.join(user_home, "your_map_cropped.wvmp") +your_map.store(output_map_path) diff --git a/examples/python/edit/generate_random_map.py b/examples/python/edit/generate_random_map.py new file mode 100644 index 000000000..54d3f77d1 --- /dev/null +++ b/examples/python/edit/generate_random_map.py @@ -0,0 +1,43 @@ +import os +import numpy as np +import pywavemap as wave + +# Create an empty map +user_home = os.path.expanduser('~') +your_map = wave.Map.create({ + "type": "hashed_wavelet_octree", + "min_cell_width": { + "meters": 0.1 + } +}) + +# Create a large box of free space +map_aabb = wave.AABB(min=np.array([-50.0, -50.0, -50.0]), + max=np.array([50.0, 50.0, 50.0])) +wave.edit.sum(your_map, map_aabb, -0.05) + +# Add random obstacles and cutouts +num_obstacles = 500 +for idx in range(num_obstacles): + random_center = 2.0 * (np.random.rand(3) - 0.5) + random_center *= np.array([50.0, 50.0, 50.0]) + random_width = 10.0 * np.random.rand(3) + update = float(np.random.rand(1)[0] * 0.2) + if np.random.rand(1)[0] < 0.7: + random_radius = float(random_width[0]) + sphere = wave.Sphere(center=random_center, radius=random_radius) + wave.edit.sum(your_map, sphere, update) + else: + box = wave.AABB(min=random_center - random_width, + max=random_center + random_width) + wave.edit.sum(your_map, box, update) + if idx % 20: + your_map.threshold() + +# Trim off any excess parts +wave.edit.crop(your_map, map_aabb) +your_map.prune() + +# Save the map +output_map_path = os.path.join(user_home, "random_map.wvmp") +your_map.store(output_map_path) diff --git a/examples/python/edit/multiply_map.py b/examples/python/edit/multiply_map.py new file mode 100644 index 000000000..11e53964b --- /dev/null +++ b/examples/python/edit/multiply_map.py @@ -0,0 +1,15 @@ +import os +import pywavemap as wave + +# Load the map +user_home = os.path.expanduser('~') +input_map_path = os.path.join(user_home, "your_map.wvmp") +your_map = wave.Map.load(input_map_path) + +# Use the multiply method to implement exponential forgetting +decay_factor = 0.9 +wave.edit.multiply(your_map, decay_factor) + +# Save the map +output_map_path = os.path.join(user_home, "your_map_decayed.wvmp") +your_map.store(output_map_path) diff --git a/examples/python/edit/sum_map.py b/examples/python/edit/sum_map.py new file mode 100644 index 000000000..6d64e0d8b --- /dev/null +++ b/examples/python/edit/sum_map.py @@ -0,0 +1,33 @@ +import os +import numpy as np +import pywavemap as wave + +# Load the map +user_home = os.path.expanduser('~') +input_map_path = os.path.join(user_home, "your_map.wvmp") +your_map = wave.Map.load(input_map_path) + +# Crop the map +cropping_sphere = wave.Sphere(center=np.array([-2.2, -1.4, 0.0]), radius=3.0) +wave.edit.crop(your_map, cropping_sphere) + +# Create a translated copy +transformation = wave.Pose(rotation=wave.Rotation(w=1.0, x=0.0, y=0.0, z=0.0), + translation=np.array([5.0, 5.0, 0.0])) +your_map_translated = wave.edit.transform(your_map, transformation) + +# Merge them together +wave.edit.sum(your_map, your_map_translated) + +# Set a box in the map to free +box = wave.AABB(min=np.array([6.0, 6.0, -2.0]), + max=np.array([10.0, 10.0, 2.0])) +wave.edit.sum(your_map, box, -1.0) + +# Set a sphere in the map to occupied +sphere = wave.Sphere(center=np.array([8.0, 8.0, 0.0]), radius=1.5) +wave.edit.sum(your_map, sphere, 2.0) + +# Save the map +output_map_path = os.path.join(user_home, "your_map_merged.wvmp") +your_map.store(output_map_path) diff --git a/examples/python/edit/transform_map.py b/examples/python/edit/transform_map.py new file mode 100644 index 000000000..bf8afa4ad --- /dev/null +++ b/examples/python/edit/transform_map.py @@ -0,0 +1,19 @@ +import os +import numpy as np +import pywavemap as wave + +# Load the map +user_home = os.path.expanduser('~') +input_map_path = os.path.join(user_home, "your_map.wvmp") +your_map = wave.Map.load(input_map_path) + +# Define a transformation that flips the map upside down, for illustration +transformation = wave.Pose(rotation=wave.Rotation(w=0.0, x=1.0, y=0.0, z=0.0), + translation=np.array([0.0, 0.0, 0.0])) + +# Transform the map +your_map = wave.edit.transform(your_map, transformation) + +# Save the map +output_map_path = os.path.join(user_home, "your_map_transformed.wvmp") +your_map.store(output_map_path) 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..c31ab58db 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,52 @@ 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(); + const Sphere cropping_sphere{T_W_B->getPosition(), config_.radius}; if (auto* hashed_wavelet_octree = dynamic_cast(occupancy_map_.get()); hashed_wavelet_octree) { - hashed_wavelet_octree->eraseBlockIf(indicator_fn); + edit::crop(*hashed_wavelet_octree, cropping_sphere, 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(*hashed_chunked_wavelet_octree, cropping_sphere, + 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/interfaces/ros1/wavemap_rviz_plugin/include/wavemap_rviz_plugin/visuals/cell_selector.h b/interfaces/ros1/wavemap_rviz_plugin/include/wavemap_rviz_plugin/visuals/cell_selector.h index 41ca0d023..4dc337ebb 100644 --- a/interfaces/ros1/wavemap_rviz_plugin/include/wavemap_rviz_plugin/visuals/cell_selector.h +++ b/interfaces/ros1/wavemap_rviz_plugin/include/wavemap_rviz_plugin/visuals/cell_selector.h @@ -14,9 +14,9 @@ namespace wavemap::rviz_plugin { struct CellSelectionMode : public TypeSelector { using TypeSelector::TypeSelector; - enum Id : TypeId { kSurface, kBand }; + enum Id : TypeId { kSurface, kBoundary, kBand }; - static constexpr std::array names = {"Surface", "Band"}; + static constexpr std::array names = {"Surface", "Boundary", "Band"}; }; class CellSelector : public QObject { @@ -35,6 +35,7 @@ class CellSelector : public QObject { return std::abs(log_odds) < unknown_occupancy_threshold_; } bool hasFreeNeighbor(const OctreeIndex& cell_index) const; + bool hasFreeOrUnknownNeighbor(const OctreeIndex& cell_index) const; private Q_SLOTS: // NOLINT // These Qt slots get connected to signals indicating changes in the diff --git a/interfaces/ros1/wavemap_rviz_plugin/src/visuals/cell_selector.cc b/interfaces/ros1/wavemap_rviz_plugin/src/visuals/cell_selector.cc index 9188a7e8e..37c506c65 100644 --- a/interfaces/ros1/wavemap_rviz_plugin/src/visuals/cell_selector.cc +++ b/interfaces/ros1/wavemap_rviz_plugin/src/visuals/cell_selector.cc @@ -71,17 +71,29 @@ void CellSelector::setMap(const MapBase::ConstPtr& map) { bool CellSelector::shouldBeDrawn(const OctreeIndex& cell_index, FloatingPoint cell_log_odds) const { switch (cell_selection_mode_) { + default: case CellSelectionMode::kSurface: // Skip free cells if (cell_log_odds < surface_occupancy_threshold_ || isUnknown(cell_log_odds)) { return false; } - // Skip cells that are occluded by neighbors on all sides + // Skip cells that would not be visible in real life if (!hasFreeNeighbor(cell_index)) { return false; } break; + case CellSelectionMode::kBoundary: + // Skip free cells + if (cell_log_odds < surface_occupancy_threshold_ || + isUnknown(cell_log_odds)) { + return false; + } + // Skip cells that are occluded by neighbors on all sides in Rviz + if (!hasFreeOrUnknownNeighbor(cell_index)) { + return false; + } + break; case CellSelectionMode::kBand: // Skip cells that don't meet the occupancy threshold if (cell_log_odds < band_min_occupancy_threshold_ || @@ -109,6 +121,21 @@ bool CellSelector::hasFreeNeighbor(const OctreeIndex& cell_index) const { return false; } +bool CellSelector::hasFreeOrUnknownNeighbor( + const OctreeIndex& cell_index) const { + for (const auto& offset : kNeighborOffsets) { // NOLINT + const OctreeIndex neighbor_index = {cell_index.height, + cell_index.position + offset}; + const FloatingPoint neighbor_log_odds = + query_accelerator_->getCellValue(neighbor_index); + // Check if the neighbor is free and observed + if (neighbor_log_odds < surface_occupancy_threshold_) { + return true; + } + } + return false; +} + void CellSelector::cellSelectionModeUpdateCallback() { // Update the cached cell selection mode value const CellSelectionMode old_cell_selection_mode = cell_selection_mode_; diff --git a/interfaces/ros1/wavemap_rviz_plugin/src/visuals/slice_visual.cc b/interfaces/ros1/wavemap_rviz_plugin/src/visuals/slice_visual.cc index b21154f13..fdb2fb4db 100644 --- a/interfaces/ros1/wavemap_rviz_plugin/src/visuals/slice_visual.cc +++ b/interfaces/ros1/wavemap_rviz_plugin/src/visuals/slice_visual.cc @@ -144,17 +144,17 @@ void SliceVisual::update() { // Allocate the pointcloud representing this grid level if needed if (static_cast(grid_levels_.size()) <= height) { const Ogre::String name = "multi_res_slice_" + std::to_string(height); - const FloatingPoint cell_width = - convert::heightToCellWidth(min_cell_width, height); auto& grid_level = grid_levels_.emplace_back( std::make_unique(slice_cell_material_)); grid_level->setName(name); - grid_level->setCellDimensions(cell_width, cell_width, 0.0); grid_level->setAlpha(alpha); frame_node_->attachObject(grid_level.get()); } // Update the points auto& grid_level = grid_levels_[height]; + const FloatingPoint cell_width = + convert::heightToCellWidth(min_cell_width, height); + grid_level->setCellDimensions(cell_width, cell_width, 0.0); grid_level->clear(); const auto& cells_at_level = cells_per_level[height]; grid_level->setCells(cells_at_level); diff --git a/interfaces/ros1/wavemap_rviz_plugin/src/visuals/voxel_visual.cc b/interfaces/ros1/wavemap_rviz_plugin/src/visuals/voxel_visual.cc index 1b7fe412c..d093f0d7d 100644 --- a/interfaces/ros1/wavemap_rviz_plugin/src/visuals/voxel_visual.cc +++ b/interfaces/ros1/wavemap_rviz_plugin/src/visuals/voxel_visual.cc @@ -383,18 +383,18 @@ void VoxelVisual::drawMultiResolutionVoxels(IndexElement tree_height, // Allocate the pointcloud representing this voxel grid level if needed if (voxel_layer_visuals.size() <= depth) { const Ogre::String name = prefix + std::to_string(depth); - const IndexElement height = tree_height - static_cast(depth); - const FloatingPoint cell_width = - convert::heightToCellWidth(min_cell_width, height); auto& voxel_layer = voxel_layer_visuals.emplace_back( std::make_unique(voxel_material_)); voxel_layer->setName(name); - voxel_layer->setCellDimensions(cell_width, cell_width, cell_width); voxel_layer->setAlpha(alpha); frame_node_->attachObject(voxel_layer.get()); } // Update the cells auto& voxel_layer = voxel_layer_visuals[depth]; + const IndexElement height = tree_height - static_cast(depth); + const FloatingPoint cell_width = + convert::heightToCellWidth(min_cell_width, height); + voxel_layer->setCellDimensions(cell_width, cell_width, cell_width); const auto& cells_at_level = voxels_per_level[depth]; voxel_layer->setCells(cells_at_level); } diff --git a/interfaces/ros1/wavemap_rviz_plugin/src/wavemap_map_display.cc b/interfaces/ros1/wavemap_rviz_plugin/src/wavemap_map_display.cc index a9452c92e..872c28253 100644 --- a/interfaces/ros1/wavemap_rviz_plugin/src/wavemap_map_display.cc +++ b/interfaces/ros1/wavemap_rviz_plugin/src/wavemap_map_display.cc @@ -275,7 +275,12 @@ void WavemapMapDisplay::requestWholeMapCallback() { void WavemapMapDisplay::loadMapFromDiskCallback() { ProfilerZoneScoped; // Open file selection dialog - const auto filepath_qt = QFileDialog::getOpenFileName(); + const bool has_last_path = + std::filesystem::exists(load_map_from_disk_property_.getAtRestValue()); + const std::string last_path = + has_last_path ? load_map_from_disk_property_.getAtRestValue() : ""; + const auto filepath_qt = QFileDialog::getOpenFileName( + nullptr, "Choose a wavemap map file", last_path.c_str()); // Check if the chosen filepath is not empty if (filepath_qt.isEmpty()) { 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/indexing/index_conversions.h b/library/cpp/include/wavemap/core/indexing/index_conversions.h index 77fb78aa3..6c8a5cacb 100644 --- a/library/cpp/include/wavemap/core/indexing/index_conversions.h +++ b/library/cpp/include/wavemap/core/indexing/index_conversions.h @@ -6,11 +6,11 @@ #include #include "wavemap/core/common.h" -#include "wavemap/core/data_structure/aabb.h" #include "wavemap/core/indexing/ndtree_index.h" #include "wavemap/core/utils/bits/morton_encoding.h" #include "wavemap/core/utils/data/eigen_checks.h" #include "wavemap/core/utils/math/int_math.h" +#include "wavemap/core/utils/shape/aabb.h" namespace wavemap::convert { template diff --git a/library/cpp/include/wavemap/core/integrator/projection_model/projector_base.h b/library/cpp/include/wavemap/core/integrator/projection_model/projector_base.h index a55ddbec8..2af6b41df 100644 --- a/library/cpp/include/wavemap/core/integrator/projection_model/projector_base.h +++ b/library/cpp/include/wavemap/core/integrator/projection_model/projector_base.h @@ -7,7 +7,7 @@ #include "wavemap/core/common.h" #include "wavemap/core/config/type_selector.h" #include "wavemap/core/config/value_with_unit.h" -#include "wavemap/core/data_structure/aabb.h" +#include "wavemap/core/utils/shape/aabb.h" namespace wavemap { struct ProjectorType : TypeSelector { diff --git a/library/cpp/include/wavemap/core/integrator/projective/coarse_to_fine/impl/range_image_intersector_inl.h b/library/cpp/include/wavemap/core/integrator/projective/coarse_to_fine/impl/range_image_intersector_inl.h index a014d4255..53a42919a 100644 --- a/library/cpp/include/wavemap/core/integrator/projective/coarse_to_fine/impl/range_image_intersector_inl.h +++ b/library/cpp/include/wavemap/core/integrator/projective/coarse_to_fine/impl/range_image_intersector_inl.h @@ -11,7 +11,7 @@ namespace wavemap { inline UpdateType RangeImageIntersector::determineUpdateType( const AABB& W_cell_aabb, const Transformation3D::RotationMatrix& R_C_W, const Point3D& t_W_C) const { - if (W_cell_aabb.containsPoint(t_W_C)) { + if (W_cell_aabb.contains(t_W_C)) { return UpdateType::kPossiblyOccupied; } diff --git a/library/cpp/include/wavemap/core/integrator/projective/coarse_to_fine/range_image_intersector.h b/library/cpp/include/wavemap/core/integrator/projective/coarse_to_fine/range_image_intersector.h index d4f893c0f..d6363eb3d 100644 --- a/library/cpp/include/wavemap/core/integrator/projective/coarse_to_fine/range_image_intersector.h +++ b/library/cpp/include/wavemap/core/integrator/projective/coarse_to_fine/range_image_intersector.h @@ -6,12 +6,12 @@ #include #include "wavemap/core/common.h" -#include "wavemap/core/data_structure/aabb.h" #include "wavemap/core/data_structure/image.h" #include "wavemap/core/integrator/measurement_model/measurement_model_base.h" #include "wavemap/core/integrator/projection_model/projector_base.h" #include "wavemap/core/integrator/projective/coarse_to_fine/hierarchical_range_bounds.h" #include "wavemap/core/integrator/projective/update_type.h" +#include "wavemap/core/utils/shape/aabb.h" namespace wavemap { class RangeImageIntersector { diff --git a/library/cpp/include/wavemap/core/integrator/projective/fixed_resolution/fixed_resolution_integrator.h b/library/cpp/include/wavemap/core/integrator/projective/fixed_resolution/fixed_resolution_integrator.h index 7e11aebc7..b7a845d31 100644 --- a/library/cpp/include/wavemap/core/integrator/projective/fixed_resolution/fixed_resolution_integrator.h +++ b/library/cpp/include/wavemap/core/integrator/projective/fixed_resolution/fixed_resolution_integrator.h @@ -4,9 +4,9 @@ #include #include -#include "wavemap/core/data_structure/aabb.h" #include "wavemap/core/data_structure/image.h" #include "wavemap/core/integrator/projective/projective_integrator.h" +#include "wavemap/core/utils/shape/aabb.h" namespace wavemap { class FixedResolutionIntegrator : public ProjectiveIntegrator { 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..31d601560 --- /dev/null +++ b/library/cpp/include/wavemap/core/utils/edit/crop.h @@ -0,0 +1,31 @@ +#ifndef WAVEMAP_CORE_UTILS_EDIT_CROP_H_ +#define WAVEMAP_CORE_UTILS_EDIT_CROP_H_ + +#include + +#include "wavemap/core/common.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, + ShapeT&& mask, FloatingPoint min_cell_width); + +template +void cropNodeRecursive(typename MapT::Block::OctreeType::NodeRefType node, + const OctreeIndex& node_index, FloatingPoint& node_value, + ShapeT&& mask, FloatingPoint min_cell_width, + IndexElement termination_height = 0); +} // namespace detail + +template +void crop(MapT& map, ShapeT mask, IndexElement termination_height = 0, + const std::shared_ptr& thread_pool = nullptr); +} // namespace wavemap::edit + +#include "wavemap/core/utils/edit/impl/crop_inl.h" + +#endif // WAVEMAP_CORE_UTILS_EDIT_CROP_H_ diff --git a/library/cpp/include/wavemap/core/utils/edit/impl/crop_inl.h b/library/cpp/include/wavemap/core/utils/edit/impl/crop_inl.h new file mode 100644 index 000000000..8b952afdc --- /dev/null +++ b/library/cpp/include/wavemap/core/utils/edit/impl/crop_inl.h @@ -0,0 +1,148 @@ +#ifndef WAVEMAP_CORE_UTILS_EDIT_IMPL_CROP_INL_H_ +#define WAVEMAP_CORE_UTILS_EDIT_IMPL_CROP_INL_H_ + +#include + +#include "wavemap/core/indexing/index_conversions.h" +#include "wavemap/core/utils/shape/intersection_tests.h" + +namespace wavemap::edit { +namespace detail { +template +void cropLeavesBatch(typename MapT::Block::OctreeType::NodeRefType node, + const OctreeIndex& node_index, FloatingPoint& node_value, + ShapeT&& mask, 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 shape 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); + if (!shape::is_inside(t_W_child, mask)) { + child_values[child_idx] = 0.f; + 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, + ShapeT&& mask, 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 shape, do nothing + const OctreeIndex child_index = node_index.computeChildIndex(child_idx); + const AABB child_aabb = + convert::nodeIndexToAABB(child_index, min_cell_width); + if (shape::is_inside(child_aabb, mask)) { + continue; + } + + // If the node is fully outside the cropping shape, set it to zero + auto& child_value = child_values[child_idx]; + if (!shape::overlaps(child_aabb, mask)) { + child_value = 0.f; + node.eraseChild(child_idx); + continue; + } + + // Otherwise, continue at a higher resolution + NodeRefType child_node = node.getOrAllocateChild(child_idx); + if (child_index.height <= termination_height + 1) { + cropLeavesBatch(child_node, child_index, child_value, mask, + min_cell_width); + } else { + cropNodeRecursive(child_node, child_index, child_value, mask, + 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(MapT& map, ShapeT mask, IndexElement termination_height, + const std::shared_ptr& thread_pool) { + 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 shape, do nothing + if (shape::is_inside(block_aabb, mask)) { + ++it; + continue; + } + // If the block is fully outside the cropping shape, erase it entirely + if (!shape::overlaps(block_aabb, mask)) { + it = map.getHashMap().erase(it); + continue; + } + + // Since the block overlaps with the shape'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([&mask, root_node_ptr, block_node_index, + root_value_ptr, min_cell_width, + termination_height]() { + detail::cropNodeRecursive(*root_node_ptr, block_node_index, + *root_value_ptr, mask, min_cell_width, + termination_height); + }); + } else { + detail::cropNodeRecursive(*root_node_ptr, block_node_index, + *root_value_ptr, mask, 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_IMPL_CROP_INL_H_ diff --git a/library/cpp/include/wavemap/core/utils/edit/impl/multiply_inl.h b/library/cpp/include/wavemap/core/utils/edit/impl/multiply_inl.h new file mode 100644 index 000000000..45235859a --- /dev/null +++ b/library/cpp/include/wavemap/core/utils/edit/impl/multiply_inl.h @@ -0,0 +1,58 @@ +#ifndef WAVEMAP_CORE_UTILS_EDIT_IMPL_MULTIPLY_INL_H_ +#define WAVEMAP_CORE_UTILS_EDIT_IMPL_MULTIPLY_INL_H_ + +#include + +#include "wavemap/core/indexing/ndtree_index.h" + +namespace wavemap::edit { +namespace detail { +template +void multiplyNodeRecursive(typename MapT::Block::OctreeType::NodeRefType node, + FloatingPoint multiplier) { + // Multiply + node.data() *= multiplier; + + // Recursively handle all child nodes + 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) { + using NodePtrType = typename MapT::Block::OctreeType::NodePtrType; + + // Process all blocks + map.forEachBlock( + [&thread_pool, multiplier](const Index3D& /*block_index*/, auto& block) { + // 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_IMPL_MULTIPLY_INL_H_ diff --git a/library/cpp/include/wavemap/core/utils/edit/impl/sum_inl.h b/library/cpp/include/wavemap/core/utils/edit/impl/sum_inl.h new file mode 100644 index 000000000..4f725054e --- /dev/null +++ b/library/cpp/include/wavemap/core/utils/edit/impl/sum_inl.h @@ -0,0 +1,330 @@ +#ifndef WAVEMAP_CORE_UTILS_EDIT_IMPL_SUM_INL_H_ +#define WAVEMAP_CORE_UTILS_EDIT_IMPL_SUM_INL_H_ + +#include +#include +#include + +#include "wavemap/core/indexing/index_conversions.h" +#include "wavemap/core/utils/iterate/grid_iterator.h" +#include "wavemap/core/utils/shape/aabb.h" +#include "wavemap/core/utils/shape/intersection_tests.h" + +namespace wavemap::edit { +namespace detail { +template +void sumNodeRecursive( + typename MapT::Block::OctreeType::NodeRefType node_A, + typename MapT::Block::OctreeType::NodeConstRefType node_B) { + using NodeRefType = decltype(node_A); + using NodeConstPtrType = typename MapT::Block::OctreeType::NodeConstPtrType; + + // Sum + node_A.data() += node_B.data(); + + // Recursively handle all child nodes + for (NdtreeIndexRelativeChild child_idx = 0; + child_idx < OctreeIndex::kNumChildren; ++child_idx) { + NodeConstPtrType child_node_B = node_B.getChild(child_idx); + if (!child_node_B) { + continue; + } + NodeRefType child_node_A = node_A.getOrAllocateChild(child_idx); + sumNodeRecursive(child_node_A, *child_node_B); + } +} + +template +void sumLeavesBatch(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}}); + + // Sum 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 sumNodeRecursive(typename MapT::Block::OctreeType::NodeRefType node, + const OctreeIndex& node_index, FloatingPoint& node_value, + SamplingFn&& sampling_function, + 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) { + 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) { + sumLeavesBatch(child_node, child_index, child_value, + std::forward(sampling_function), + min_cell_width); + } else { + sumNodeRecursive(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; +} + +template +void sumLeavesBatch(typename MapT::Block::OctreeType::NodeRefType node, + const OctreeIndex& node_index, FloatingPoint& node_value, + ShapeT&& mask, FloatingPoint summand, + 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}}); + + // Sum 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); + if (shape::is_inside(t_W_child, mask)) { + child_values[child_idx] += summand; + } + } + + // Compress + const auto [new_value, new_details] = + MapT::Block::Transform::forward(child_values); + node_details = new_details; + node_value = new_value; +} + +template +void sumNodeRecursive(typename MapT::Block::OctreeType::NodeRefType node, + const OctreeIndex& node_index, FloatingPoint& node_value, + ShapeT&& mask, FloatingPoint summand, + 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 outside the shape, skip it + const OctreeIndex child_index = node_index.computeChildIndex(child_idx); + const AABB child_aabb = + convert::nodeIndexToAABB(child_index, min_cell_width); + if (!shape::overlaps(child_aabb, mask)) { + continue; + } + + // If the node is fully inside the shape, sum at the current resolution + auto& child_value = child_values[child_idx]; + if (shape::is_inside(child_aabb, mask)) { + child_value += summand; + continue; + } + + // Otherwise, continue at a higher resolution + NodeRefType child_node = node.getOrAllocateChild(child_idx); + if (child_index.height <= termination_height + 1) { + sumLeavesBatch(child_node, child_index, child_value, + std::forward(mask), summand, min_cell_width); + } else { + sumNodeRecursive(child_node, child_index, child_value, + std::forward(mask), summand, + 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 sum(MapT& map_A, const MapT& map_B, + const std::shared_ptr& thread_pool) { + CHECK_EQ(map_A.getTreeHeight(), map_B.getTreeHeight()); + CHECK_EQ(map_A.getMinCellWidth(), map_B.getMinCellWidth()); + using NodePtrType = typename MapT::Block::OctreeType::NodePtrType; + using NodeConstPtrType = typename MapT::Block::OctreeType::NodeConstPtrType; + + // Process all blocks + map_B.forEachBlock( + [&map_A, &thread_pool](const Index3D& block_index, const auto& block_B) { + auto& block_A = map_A.getOrAllocateBlock(block_index); + + // Indicate that the block has changed + block_A.setLastUpdatedStamp(); + block_A.setNeedsPruning(); + + // Sum the blocks' average values (wavelet scale coefficient) + block_A.getRootScale() += block_B.getRootScale(); + + // Recursively sum all node values (wavelet detail coefficients) + NodePtrType root_node_ptr_A = &block_A.getRootNode(); + NodeConstPtrType root_node_ptr_B = &block_B.getRootNode(); + if (thread_pool) { + thread_pool->add_task([root_node_ptr_A, root_node_ptr_B, + block_ptr_A = &block_A]() { + detail::sumNodeRecursive(*root_node_ptr_A, *root_node_ptr_B); + block_ptr_A->prune(); + }); + } else { + detail::sumNodeRecursive(*root_node_ptr_A, *root_node_ptr_B); + block_A.prune(); + } + }); + + // Wait for all parallel jobs to finish + if (thread_pool) { + thread_pool->wait_all(); + } +} + +template +void sum(MapT& map, SamplingFn sampling_function, + const std::unordered_set>& block_indices, + IndexElement termination_height, + const std::shared_ptr& thread_pool) { + // Make sure all requested blocks have been allocated + for (const Index3D& block_index : block_indices) { + map.getOrAllocateBlock(block_index); + } + + // Evaluate the sampling function for each requested block + using NodePtrType = typename MapT::Block::OctreeType::NodePtrType; + const IndexElement tree_height = map.getTreeHeight(); + const FloatingPoint min_cell_width = map.getMinCellWidth(); + for (const Index3D& block_index : block_indices) { + // Indicate that the block has changed + auto& block = *CHECK_NOTNULL(map.getBlock(block_index)); + 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 sum all nodes + if (thread_pool) { + thread_pool->add_task([root_node_ptr, root_node_index, root_value_ptr, + block_ptr = &block, + sampling_fn_copy = sampling_function, + min_cell_width, termination_height]() mutable { + detail::sumNodeRecursive(*root_node_ptr, root_node_index, + *root_value_ptr, sampling_fn_copy, + min_cell_width, termination_height); + block_ptr->prune(); + }); + } else { + detail::sumNodeRecursive(*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(); + } +} + +template +void sum(MapT& map, ShapeT mask, FloatingPoint summand, + const std::shared_ptr& thread_pool) { + // Find the blocks that overlap with the shape + const FloatingPoint block_width = + convert::heightToCellWidth(map.getMinCellWidth(), map.getTreeHeight()); + const FloatingPoint block_width_inv = 1.f / block_width; + const auto aabb = static_cast>(mask); + const Index3D block_index_min = + convert::pointToFloorIndex(aabb.min, block_width_inv); + const Index3D block_index_max = + convert::pointToCeilIndex(aabb.max, block_width_inv); + std::unordered_set> block_indices; + for (const Index3D& block_index : Grid(block_index_min, block_index_max)) { + block_indices.emplace(block_index); + } + + // Make sure all overlapping blocks have been allocated + for (const Index3D& block_index : block_indices) { + map.getOrAllocateBlock(block_index); + } + + // Apply the sum to each overlapping block + using NodePtrType = typename MapT::Block::OctreeType::NodePtrType; + const IndexElement tree_height = map.getTreeHeight(); + const FloatingPoint min_cell_width = map.getMinCellWidth(); + for (const Index3D& block_index : block_indices) { + // Indicate that the block has changed + auto& block = *CHECK_NOTNULL(map.getBlock(block_index)); + 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 sum all nodes + if (thread_pool) { + thread_pool->add_task([root_node_ptr, root_node_index, root_value_ptr, + block_ptr = &block, &mask, summand, + min_cell_width]() mutable { + detail::sumNodeRecursive(*root_node_ptr, root_node_index, + *root_value_ptr, mask, summand, + min_cell_width, 0); + }); + } else { + detail::sumNodeRecursive(*root_node_ptr, root_node_index, + *root_value_ptr, mask, summand, + min_cell_width, 0); + } + } + + // Wait for all parallel jobs to finish + if (thread_pool) { + thread_pool->wait_all(); + } +} +} // namespace wavemap::edit + +#endif // WAVEMAP_CORE_UTILS_EDIT_IMPL_SUM_INL_H_ diff --git a/library/cpp/include/wavemap/core/utils/edit/impl/transform_inl.h b/library/cpp/include/wavemap/core/utils/edit/impl/transform_inl.h new file mode 100644 index 000000000..59be0ce52 --- /dev/null +++ b/library/cpp/include/wavemap/core/utils/edit/impl/transform_inl.h @@ -0,0 +1,64 @@ +#ifndef WAVEMAP_CORE_UTILS_EDIT_IMPL_TRANSFORM_INL_H_ +#define WAVEMAP_CORE_UTILS_EDIT_IMPL_TRANSFORM_INL_H_ + +#include +#include +#include +#include + +#include "wavemap/core/indexing/index_conversions.h" +#include "wavemap/core/indexing/index_hashes.h" +#include "wavemap/core/utils/edit/sum.h" +#include "wavemap/core/utils/iterate/grid_iterator.h" +#include "wavemap/core/utils/query/map_interpolator.h" +#include "wavemap/core/utils/shape/aabb.h" + +namespace wavemap::edit { +template +std::unique_ptr transform( + const MapT& map_B, const Transformation3D& T_AB, + const std::shared_ptr& thread_pool) { + const IndexElement tree_height = map_B.getTreeHeight(); + const FloatingPoint min_cell_width = map_B.getMinCellWidth(); + const FloatingPoint block_width = + convert::heightToCellWidth(min_cell_width, tree_height); + const FloatingPoint block_width_inv = 1.f / block_width; + + // Find all blocks that need to be updated + std::unordered_set> block_indices_A; + map_B.forEachBlock([&block_indices_A, &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.insert(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)) { + block_indices_A.emplace(A_block_index); + } + }); + + // Populate map A by interpolating map B + auto map_A = std::make_unique(map_B.getConfig()); + const Transformation3D T_BA = T_AB.inverse(); + QueryAccelerator accelerator_B{map_B}; + auto interpolator_B = [&T_BA, accelerator_B](const Point3D& A_point) mutable { + const auto B_point = T_BA * A_point; + return interpolate::trilinear(accelerator_B, B_point); + }; + sum(*map_A, interpolator_B, block_indices_A, 0, thread_pool); + + return map_A; +} +} // namespace wavemap::edit + +#endif // WAVEMAP_CORE_UTILS_EDIT_IMPL_TRANSFORM_INL_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..1183fb4fa --- /dev/null +++ b/library/cpp/include/wavemap/core/utils/edit/multiply.h @@ -0,0 +1,23 @@ +#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); +} // namespace detail + +template +void multiply(MapT& map, FloatingPoint multiplier, + const std::shared_ptr& thread_pool = nullptr); +} // namespace wavemap::edit + +#include "wavemap/core/utils/edit/impl/multiply_inl.h" + +#endif // WAVEMAP_CORE_UTILS_EDIT_MULTIPLY_H_ diff --git a/library/cpp/include/wavemap/core/utils/edit/sum.h b/library/cpp/include/wavemap/core/utils/edit/sum.h new file mode 100644 index 000000000..506ed264f --- /dev/null +++ b/library/cpp/include/wavemap/core/utils/edit/sum.h @@ -0,0 +1,67 @@ +#ifndef WAVEMAP_CORE_UTILS_EDIT_SUM_H_ +#define WAVEMAP_CORE_UTILS_EDIT_SUM_H_ + +#include +#include + +#include "wavemap/core/common.h" +#include "wavemap/core/indexing/index_hashes.h" +#include "wavemap/core/indexing/ndtree_index.h" +#include "wavemap/core/utils/thread_pool.h" + +namespace wavemap::edit { +namespace detail { +// Recursively sum two maps together +template +void sumNodeRecursive( + typename MapT::Block::OctreeType::NodeRefType node_A, + typename MapT::Block::OctreeType::NodeConstRefType node_B); + +// Recursively add a sampled value +template +void sumLeavesBatch(typename MapT::Block::OctreeType::NodeRefType node, + const OctreeIndex& node_index, FloatingPoint& node_value, + SamplingFn&& sampling_function, + FloatingPoint min_cell_width); +template +void sumNodeRecursive(typename MapT::Block::OctreeType::NodeRefType node, + const OctreeIndex& node_index, FloatingPoint& node_value, + SamplingFn&& sampling_function, + FloatingPoint min_cell_width, + IndexElement termination_height = 0); + +// Recursively add a scalar value to all cells within a given shape +template +void sumLeavesBatch(typename MapT::Block::OctreeType::NodeRefType node, + const OctreeIndex& node_index, FloatingPoint& node_value, + ShapeT&& mask, FloatingPoint summand, + FloatingPoint min_cell_width); +template +void sumNodeRecursive(typename MapT::Block::OctreeType::NodeRefType node, + const OctreeIndex& node_index, FloatingPoint& node_value, + ShapeT&& mask, FloatingPoint summand, + FloatingPoint min_cell_width, + IndexElement termination_height = 0); +} // namespace detail + +// Sum two maps together +template +void sum(MapT& map_A, const MapT& map_B, + const std::shared_ptr& thread_pool = nullptr); + +// Add a sampled value to all cells within a given list of blocks +template +void sum(MapT& map, SamplingFn sampling_function, + const std::unordered_set>& block_indices, + IndexElement termination_height = 0, + const std::shared_ptr& thread_pool = nullptr); + +// Add a scalar value to all cells within a given shape +template +void sum(MapT& map, ShapeT mask, FloatingPoint summand, + const std::shared_ptr& thread_pool = nullptr); +} // namespace wavemap::edit + +#include "wavemap/core/utils/edit/impl/sum_inl.h" + +#endif // WAVEMAP_CORE_UTILS_EDIT_SUM_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..f1a8cb89f --- /dev/null +++ b/library/cpp/include/wavemap/core/utils/edit/transform.h @@ -0,0 +1,18 @@ +#ifndef WAVEMAP_CORE_UTILS_EDIT_TRANSFORM_H_ +#define WAVEMAP_CORE_UTILS_EDIT_TRANSFORM_H_ + +#include + +#include "wavemap/core/common.h" +#include "wavemap/core/utils/thread_pool.h" + +namespace wavemap::edit { +template +std::unique_ptr transform( + const MapT& map_B, const Transformation3D& T_AB, + const std::shared_ptr& thread_pool = nullptr); +} // namespace wavemap::edit + +#include "wavemap/core/utils/edit/impl/transform_inl.h" + +#endif // WAVEMAP_CORE_UTILS_EDIT_TRANSFORM_H_ diff --git a/library/cpp/include/wavemap/core/utils/query/point_sampler.h b/library/cpp/include/wavemap/core/utils/query/point_sampler.h index e02da0ed0..a39b36bb8 100644 --- a/library/cpp/include/wavemap/core/utils/query/point_sampler.h +++ b/library/cpp/include/wavemap/core/utils/query/point_sampler.h @@ -5,10 +5,10 @@ #include #include "wavemap/core/common.h" -#include "wavemap/core/data_structure/aabb.h" #include "wavemap/core/utils/query/classified_map.h" #include "wavemap/core/utils/query/occupancy.h" #include "wavemap/core/utils/random_number_generator.h" +#include "wavemap/core/utils/shape/aabb.h" namespace wavemap { class PointSampler { 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 238d47bcc..7cdae7bcc 100644 --- a/library/cpp/include/wavemap/core/utils/query/query_accelerator.h +++ b/library/cpp/include/wavemap/core/utils/query/query_accelerator.h @@ -30,6 +30,10 @@ class QueryAccelerator> { explicit QueryAccelerator(SpatialHash& spatial_hash) : spatial_hash_(spatial_hash) {} + //! Copy and move constructors + QueryAccelerator(const QueryAccelerator& other) = default; + QueryAccelerator(QueryAccelerator&& other) = default; + void reset(); BlockDataT* getBlock(const Index& block_index); @@ -59,6 +63,10 @@ class QueryAccelerator> { const DenseBlockHash& dense_block_hash) : dense_block_hash_(dense_block_hash) {} + //! Copy and move constructors + QueryAccelerator(const QueryAccelerator& other) = default; + QueryAccelerator(QueryAccelerator&& other) = default; + void reset(); const BlockType* getBlock(const Index& block_index); @@ -83,6 +91,10 @@ class QueryAccelerator> { explicit QueryAccelerator(NdtreeBlockHash& ndtree_block_hash) : ndtree_block_hash_(ndtree_block_hash) {} + //! Copy and move constructors + QueryAccelerator(const QueryAccelerator& other) = default; + QueryAccelerator(QueryAccelerator&& other) = default; + void reset(); BlockType* getBlock(const Index& block_index); @@ -124,6 +136,10 @@ class QueryAccelerator { explicit QueryAccelerator(const HashedWaveletOctree& map) : map_(map) {} + //! Copy and move constructors + QueryAccelerator(const QueryAccelerator& other) = default; + QueryAccelerator(QueryAccelerator&& other) = default; + //! 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 @@ -176,6 +192,10 @@ class QueryAccelerator { explicit QueryAccelerator(const HashedChunkedWaveletOctree& map) : map_(map) {} + //! Copy and move constructors + QueryAccelerator(const QueryAccelerator& other) = default; + QueryAccelerator(QueryAccelerator&& other) = default; + //! 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 diff --git a/library/cpp/include/wavemap/core/data_structure/aabb.h b/library/cpp/include/wavemap/core/utils/shape/aabb.h similarity index 71% rename from library/cpp/include/wavemap/core/data_structure/aabb.h rename to library/cpp/include/wavemap/core/utils/shape/aabb.h index b0864aad9..d22079ed5 100644 --- a/library/cpp/include/wavemap/core/data_structure/aabb.h +++ b/library/cpp/include/wavemap/core/utils/shape/aabb.h @@ -1,5 +1,5 @@ -#ifndef WAVEMAP_CORE_DATA_STRUCTURE_AABB_H_ -#define WAVEMAP_CORE_DATA_STRUCTURE_AABB_H_ +#ifndef WAVEMAP_CORE_UTILS_SHAPE_AABB_H_ +#define WAVEMAP_CORE_UTILS_SHAPE_AABB_H_ #include #include @@ -16,56 +16,56 @@ struct AABB { static constexpr int kDim = dim_v; static constexpr int kNumCorners = int_math::exp2(kDim); using PointType = PointT; - using ScalarType = typename PointType::Scalar; + using ScalarType = typename PointT::Scalar; using Corners = Eigen::Matrix; static constexpr auto kInitialMin = std::numeric_limits::max(); static constexpr auto kInitialMax = std::numeric_limits::lowest(); - PointType min = PointType::Constant(kInitialMin); - PointType max = PointType::Constant(kInitialMax); + PointT min = PointT::Constant(kInitialMin); + PointT max = PointT::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) { + void insert(const PointT& point) { min = min.cwiseMin(point); max = max.cwiseMax(point); } - bool containsPoint(const PointType& point) const { + bool contains(const PointT& point) const { return (min.array() <= point.array() && point.array() <= max.array()).all(); } - PointType closestPointTo(const PointType& point) const { - PointType closest_point = point.cwiseMax(min).cwiseMin(max); + PointT closestPointTo(const PointT& point) const { + PointT closest_point = point.cwiseMax(min).cwiseMin(max); return closest_point; } - PointType furthestPointFrom(const PointType& point) const { - const PointType aabb_center = (min + max) / static_cast(2); - PointType furthest_point = + PointT furthestPointFrom(const PointT& point) const { + const PointT aabb_center = (min + max) / static_cast(2); + PointT furthest_point = (aabb_center.array() < point.array()).select(min, max); return furthest_point; } - PointType minOffsetTo(const PointType& point) const { + PointT minOffsetTo(const PointT& point) const { return point - closestPointTo(point); } - PointType maxOffsetTo(const PointType& point) const { + PointT maxOffsetTo(const PointT& point) const { return point - furthestPointFrom(point); } // TODO(victorr): Check correctness with unit tests - PointType minOffsetTo(const AABB& aabb) const { - const PointType greatest_min = min.cwiseMax(aabb.min); - const PointType smallest_max = max.cwiseMin(aabb.max); + PointT minOffsetTo(const AABB& other) const { + const PointT greatest_min = min.cwiseMax(other.min); + const PointT smallest_max = max.cwiseMin(other.max); return (greatest_min - smallest_max).cwiseMax(0); } // TODO(victorr): Check correctness with unit tests. Pay particular // attention to whether the offset signs are correct. - PointType maxOffsetTo(const AABB& aabb) const { - const PointType diff_1 = min - aabb.max; - const PointType diff_2 = max - aabb.min; - PointType offset = + PointT maxOffsetTo(const AABB& other) const { + const PointT diff_1 = min - other.max; + const PointT diff_2 = max - other.min; + PointT offset = (diff_2.array().abs() < diff_1.array().abs()).select(diff_1, diff_2); return offset; } @@ -92,7 +92,7 @@ struct AABB { ScalarType width() const { return max[dim] - min[dim]; } - PointType widths() const { return max - min; } + PointT widths() const { return max - min; } Corners corner_matrix() const { Eigen::Matrix corners; @@ -104,8 +104,8 @@ struct AABB { return corners; } - PointType corner_point(int corner_idx) const { - PointType corner; + PointT corner_point(int corner_idx) const { + PointT corner; for (int dim_idx = 0; dim_idx < kDim; ++dim_idx) { corner[dim_idx] = corner_coordinate(dim_idx, corner_idx); } @@ -129,4 +129,4 @@ struct AABB { }; } // namespace wavemap -#endif // WAVEMAP_CORE_DATA_STRUCTURE_AABB_H_ +#endif // WAVEMAP_CORE_UTILS_SHAPE_AABB_H_ diff --git a/library/cpp/include/wavemap/core/utils/shape/intersection_tests.h b/library/cpp/include/wavemap/core/utils/shape/intersection_tests.h new file mode 100644 index 000000000..dde6944c9 --- /dev/null +++ b/library/cpp/include/wavemap/core/utils/shape/intersection_tests.h @@ -0,0 +1,57 @@ +#ifndef WAVEMAP_CORE_UTILS_SHAPE_INTERSECTION_TESTS_H_ +#define WAVEMAP_CORE_UTILS_SHAPE_INTERSECTION_TESTS_H_ + +#include "wavemap/core/utils/shape/aabb.h" +#include "wavemap/core/utils/shape/sphere.h" + +namespace wavemap::shape { +// Point inside Shape +template +bool is_inside(const Point& inner, const ShapeT& outer) { + return outer.contains(inner); +} + +// AABB inside AABB +template +bool is_inside(const AABB& inner, const AABB& outer) { + return (outer.min.array() <= inner.min.array() && + inner.max.array() <= outer.max.array()) + .all(); +} + +// AABB inside Sphere +template +bool is_inside(const AABB& inner, const Sphere& outer) { + const PointT furthest_point_in_aabb = inner.furthestPointFrom(outer.center); + return outer.contains(furthest_point_in_aabb); +} + +// Sphere inside AABB +template +bool is_inside(const Sphere& inner, const AABB& outer) { + return (outer.min.array() <= inner.center.array() - inner.radius && + inner.center.array() + inner.radius <= outer.max.array()) + .all(); +} + +// AABB <-> AABB overlap +template +bool overlaps(const AABB& aabb_A, const AABB& aabb_B) { + const auto axis_separated = aabb_A.max.array() < aabb_B.min.array() || + aabb_B.max.array() < aabb_A.min.array(); + return !axis_separated.any(); +} + +// AABB <-> Sphere overlap +template +bool overlaps(const AABB& aabb, const Sphere& sphere) { + const PointT closest_point_in_aabb = aabb.closestPointTo(sphere.center); + return sphere.contains(closest_point_in_aabb); +} +template +bool overlaps(const Sphere& sphere, const AABB& aabb) { + return overlaps(aabb, sphere); +} +} // namespace wavemap::shape + +#endif // WAVEMAP_CORE_UTILS_SHAPE_INTERSECTION_TESTS_H_ diff --git a/library/cpp/include/wavemap/core/utils/shape/sphere.h b/library/cpp/include/wavemap/core/utils/shape/sphere.h new file mode 100644 index 000000000..e2e89baba --- /dev/null +++ b/library/cpp/include/wavemap/core/utils/shape/sphere.h @@ -0,0 +1,48 @@ +#ifndef WAVEMAP_CORE_UTILS_SHAPE_SPHERE_H_ +#define WAVEMAP_CORE_UTILS_SHAPE_SPHERE_H_ + +#include +#include +#include + +#include "wavemap/core/common.h" +#include "wavemap/core/utils/print/eigen.h" + +namespace wavemap { +template +struct Sphere { + static constexpr int kDim = dim_v; + using PointType = PointT; + using ScalarType = typename PointT::Scalar; + + PointT center = PointT::Constant(kNaN); + ScalarType radius = static_cast(0); + + Sphere() = default; + Sphere(const PointT& center, ScalarType radius) + : center(center), radius(radius) {} + Sphere(PointT&& center, ScalarType radius) + : center(std::move(center)), radius(radius) {} + + operator AABB() const { + if (std::isnan(center[0])) { + return {}; + } + return {center.array() - radius, center.array() + radius}; + } + + // TODO(victorr): Add tests, incl. behavior after default construction + bool contains(const PointT& point) const { + return (point - center).squaredNorm() <= radius * radius; + } + + std::string toString() const { + std::stringstream ss; + ss << "[center =" << print::eigen::oneLine(center) + << ", radius = " << radius << "]"; + return ss.str(); + } +}; +} // namespace wavemap + +#endif // WAVEMAP_CORE_UTILS_SHAPE_SPHERE_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/cpp/src/core/integrator/projective/fixed_resolution/fixed_resolution_integrator.cc b/library/cpp/src/core/integrator/projective/fixed_resolution/fixed_resolution_integrator.cc index e68f6bb4c..9d8dbc371 100644 --- a/library/cpp/src/core/integrator/projective/fixed_resolution/fixed_resolution_integrator.cc +++ b/library/cpp/src/core/integrator/projective/fixed_resolution/fixed_resolution_integrator.cc @@ -13,7 +13,7 @@ void FixedResolutionIntegrator::importPointcloud( aabb_ = AABB{}; // Import all the points while updating the AABB - aabb_.includePoint(pointcloud.getOrigin()); // sensor origin + aabb_.insert(pointcloud.getOrigin()); // sensor origin for (const auto& C_point : pointcloud.getPointsLocal()) { // Filter out noisy points and compute point's range if (!isMeasurementValid(C_point)) { @@ -45,7 +45,7 @@ void FixedResolutionIntegrator::importPointcloud( Point3D C_point_truncated = getEndPointOrMaxRange(Point3D::Zero(), C_point, range, config_.max_range); const Point3D W_point_truncated = pointcloud.getPose() * C_point_truncated; - aabb_.includePoint(W_point_truncated); + aabb_.insert(W_point_truncated); } // Pad the aabb to account for the beam uncertainties @@ -63,7 +63,7 @@ void FixedResolutionIntegrator::importRangeImage( // Update the AABB to contain the camera frustum aabb_ = AABB{}; - aabb_.includePoint(posed_range_image_->getOrigin()); // sensor + aabb_.insert(posed_range_image_->getOrigin()); // sensor for (int corner_idx = 0; corner_idx < 4; ++corner_idx) { const Index2D frustum_corner_image_index = posed_range_image_->getDimensions().cwiseProduct( @@ -74,7 +74,7 @@ void FixedResolutionIntegrator::importRangeImage( frustum_corner_coordinate, config_.max_range); const Point3D W_frustum_corner = posed_range_image_->getPose() * C_frustum_corner; - aabb_.includePoint(W_frustum_corner); + aabb_.insert(W_frustum_corner); } // Pad the aabb to account for the beam uncertainties 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 a0b17b099..2139dc5f2 100644 --- a/library/cpp/test/src/core/data_structure/test_aabb.cc +++ b/library/cpp/test/src/core/data_structure/test_aabb.cc @@ -5,8 +5,8 @@ #include #include "wavemap/core/common.h" -#include "wavemap/core/data_structure/aabb.h" #include "wavemap/core/utils/print/eigen.h" +#include "wavemap/core/utils/shape/aabb.h" #include "wavemap/test/fixture_base.h" #include "wavemap/test/geometry_generator.h" @@ -22,10 +22,10 @@ TYPED_TEST(AabbTest, InitializationAndInclusion) { for (const auto& random_point : GeometryGenerator::getRandomPointVector>()) { AABB aabb; - EXPECT_FALSE(aabb.containsPoint(random_point)) + EXPECT_FALSE(aabb.contains(random_point)) << "The uninitialized AABB should be empty."; - aabb.includePoint(random_point); - EXPECT_TRUE(aabb.containsPoint(random_point)) + aabb.insert(random_point); + EXPECT_TRUE(aabb.contains(random_point)) << "The AABB should contain points after they have been included."; } } @@ -113,7 +113,7 @@ TYPED_TEST(AabbTest, ClosestPointsAndDistances) { // Check closest and furthest point queries and distances if (min_distance <= 0) { - EXPECT_TRUE(test.aabb.containsPoint(test.query_point)) + EXPECT_TRUE(test.aabb.contains(test.query_point)) << test.getDescription(); } diff --git a/library/cpp/test/src/core/integrator/projection_model/test_image_projectors.cc b/library/cpp/test/src/core/integrator/projection_model/test_image_projectors.cc index 942d9fb78..3e7993dbc 100644 --- a/library/cpp/test/src/core/integrator/projection_model/test_image_projectors.cc +++ b/library/cpp/test/src/core/integrator/projection_model/test_image_projectors.cc @@ -247,7 +247,7 @@ TYPED_TEST(Image2DProjectorTypedTest, SensorCoordinateAABBs) { for (const auto& test : tests) { // The Projector::cartesianToSensorAABB(...) method is only valid if the // AABB does not contain the sensor's origin - if (test.W_aabb.containsPoint(test.T_W_C.getPosition())) { + if (test.W_aabb.contains(test.T_W_C.getPosition())) { continue; } diff --git a/library/python/CMakeLists.txt b/library/python/CMakeLists.txt index cfb1e5f66..94f2b9055 100644 --- a/library/python/CMakeLists.txt +++ b/library/python/CMakeLists.txt @@ -58,6 +58,8 @@ find_package(nanobind CONFIG REQUIRED) nanobind_add_module(_pywavemap_bindings STABLE_ABI src/pywavemap.cc src/convert.cc + src/edit.cc + src/geometry.cc src/indices.cc src/logging.cc src/maps.cc @@ -81,10 +83,6 @@ nanobind_add_stub(pywavemap_stub INSTALL_TIME OUTPUT "pywavemap/__init__.pyi" MARKER_FILE "pywavemap/py.typed" PYTHON_PATH "pywavemap") -nanobind_add_stub(pywavemap_convert_stub INSTALL_TIME - MODULE _pywavemap_bindings.convert - OUTPUT "pywavemap/convert.pyi" - PYTHON_PATH "pywavemap") nanobind_add_stub(pywavemap_logging_stub INSTALL_TIME MODULE _pywavemap_bindings.logging OUTPUT "pywavemap/logging.pyi" @@ -93,3 +91,11 @@ nanobind_add_stub(pywavemap_param_stub INSTALL_TIME MODULE _pywavemap_bindings.param OUTPUT "pywavemap/param.pyi" PYTHON_PATH "pywavemap") +nanobind_add_stub(pywavemap_convert_stub INSTALL_TIME + MODULE _pywavemap_bindings.convert + OUTPUT "pywavemap/convert.pyi" + PYTHON_PATH "pywavemap") +nanobind_add_stub(pywavemap_edit_stub INSTALL_TIME + MODULE _pywavemap_bindings.edit + OUTPUT "pywavemap/edit.pyi" + PYTHON_PATH "pywavemap") diff --git a/library/python/include/pywavemap/edit.h b/library/python/include/pywavemap/edit.h new file mode 100644 index 000000000..e160cff47 --- /dev/null +++ b/library/python/include/pywavemap/edit.h @@ -0,0 +1,12 @@ +#ifndef PYWAVEMAP_EDIT_H_ +#define PYWAVEMAP_EDIT_H_ + +#include + +namespace nb = nanobind; + +namespace wavemap { +void add_edit_module(nb::module_& m_edit); +} // namespace wavemap + +#endif // PYWAVEMAP_EDIT_H_ diff --git a/library/python/include/pywavemap/geometry.h b/library/python/include/pywavemap/geometry.h new file mode 100644 index 000000000..b374eca89 --- /dev/null +++ b/library/python/include/pywavemap/geometry.h @@ -0,0 +1,12 @@ +#ifndef PYWAVEMAP_GEOMETRY_H_ +#define PYWAVEMAP_GEOMETRY_H_ + +#include + +namespace nb = nanobind; + +namespace wavemap { +void add_geometry_bindings(nb::module_& m); +} + +#endif // PYWAVEMAP_GEOMETRY_H_ 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/edit.cc b/library/python/src/edit.cc new file mode 100644 index 000000000..28b8af69c --- /dev/null +++ b/library/python/src/edit.cc @@ -0,0 +1,132 @@ +#include "pywavemap/edit.h" + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace nb::literals; // NOLINT + +namespace wavemap { +void add_edit_module(nb::module_& m_edit) { + // Multiply a map with a scalar + // NOTE: Among others, this can be used to implement exponential forgetting, + // by multiplying the map with a scalar between 0 and 1. + m_edit.def( + "multiply", + [](HashedWaveletOctree& map, FloatingPoint multiplier) { + edit::multiply(map, multiplier, std::make_shared()); + }, + "map"_a, "multiplier"_a); + m_edit.def( + "multiply", + [](HashedChunkedWaveletOctree& map, FloatingPoint multiplier) { + edit::multiply(map, multiplier, std::make_shared()); + }, + "map"_a, "multiplier"_a); + + // Sum two maps together + m_edit.def( + "sum", + [](HashedWaveletOctree& map_A, const HashedWaveletOctree& map_B) { + edit::sum(map_A, map_B, std::make_shared()); + }, + "map_A"_a, "map_B"_a); + m_edit.def( + "sum", + [](HashedChunkedWaveletOctree& map_A, + const HashedChunkedWaveletOctree& map_B) { + edit::sum(map_A, map_B, std::make_shared()); + }, + "map_A"_a, "map_B"_a); + + // Add a scalar value to all cells within an axis aligned bounding box + m_edit.def( + "sum", + [](HashedWaveletOctree& map, const AABB& aabb, + FloatingPoint update) { + edit::sum(map, aabb, update, std::make_shared()); + }, + "map"_a, "aabb"_a, "update"_a); + m_edit.def( + "sum", + [](HashedChunkedWaveletOctree& map, const AABB& aabb, + FloatingPoint update) { + edit::sum(map, aabb, update, std::make_shared()); + }, + "map"_a, "aabb"_a, "update"_a); + + // Add a scalar value to all cells within a sphere + m_edit.def( + "sum", + [](HashedWaveletOctree& map, const Sphere& sphere, + FloatingPoint update) { + edit::sum(map, sphere, update, std::make_shared()); + }, + "map"_a, "sphere"_a, "update"_a); + m_edit.def( + "sum", + [](HashedChunkedWaveletOctree& map, const Sphere& sphere, + FloatingPoint update) { + edit::sum(map, sphere, update, std::make_shared()); + }, + "map"_a, "sphere"_a, "update"_a); + + // Transform a map into a different coordinate frame + m_edit.def( + "transform", + [](HashedWaveletOctree& B_map, const Transformation3D& T_AB) { + return edit::transform(B_map, T_AB, std::make_shared()); + }, + "map"_a, "transformation"_a); + m_edit.def( + "transform", + [](HashedChunkedWaveletOctree& B_map, const Transformation3D& T_AB) { + return edit::transform(B_map, T_AB, std::make_shared()); + }, + "map"_a, "transformation"_a); + + // Crop a map to a box + m_edit.def( + "crop", + [](HashedWaveletOctree& map, const AABB& aabb, + IndexElement termination_height) { + edit::crop(map, aabb, termination_height, + std::make_shared()); + }, + "map"_a, "aabb"_a, "termination_height"_a = 0); + m_edit.def( + "crop", + [](HashedChunkedWaveletOctree& map, const AABB& aabb, + IndexElement termination_height) { + edit::crop(map, aabb, termination_height, + std::make_shared()); + }, + "map"_a, "aabb"_a, "termination_height"_a = 0); + + // Crop a map to a sphere + m_edit.def( + "crop", + [](HashedWaveletOctree& map, const Sphere& sphere, + IndexElement termination_height) { + edit::crop(map, sphere, termination_height, + std::make_shared()); + }, + "map"_a, "sphere"_a, "termination_height"_a = 0); + m_edit.def( + "crop", + [](HashedChunkedWaveletOctree& map, const Sphere& sphere, + IndexElement termination_height) { + edit::crop(map, sphere, termination_height, + std::make_shared()); + }, + "map"_a, "sphere"_a, "termination_height"_a = 0); +} +} // namespace wavemap diff --git a/library/python/src/geometry.cc b/library/python/src/geometry.cc new file mode 100644 index 000000000..c4ccb5cd8 --- /dev/null +++ b/library/python/src/geometry.cc @@ -0,0 +1,34 @@ +#include "pywavemap/geometry.h" + +#include +#include +#include +#include + +using namespace nb::literals; // NOLINT + +namespace wavemap { +void add_geometry_bindings(nb::module_& m) { + // Axis-Aligned Bounding Box + nb::class_>( + m, "AABB", "A class representing an Axis-Aligned Bounding Box.") + .def(nb::init()) + .def(nb::init(), "min"_a, "max"_a) + .def_rw("min", &AABB::min) + .def_rw("max", &AABB::max) + .def("insert", &AABB::insert, + "Expand the AABB to tightly fit the new point " + "and its previous self.") + .def("contains", &AABB::contains, + "Test whether the AABB contains the given point."); + + // Axis-Aligned Bounding Box + nb::class_>(m, "Sphere", "A class representing a sphere.") + .def(nb::init()) + .def(nb::init(), "center"_a, "radius"_a) + .def_rw("center", &Sphere::center) + .def_rw("radius", &Sphere::radius) + .def("contains", &Sphere::contains, + "Test whether the sphere contains the given point."); +} +} // namespace wavemap diff --git a/library/python/src/indices.cc b/library/python/src/indices.cc index 849f9f92e..b7bd451ff 100644 --- a/library/python/src/indices.cc +++ b/library/python/src/indices.cc @@ -9,7 +9,7 @@ namespace wavemap { void add_index_bindings(nb::module_& m) { nb::class_(m, "OctreeIndex", "A class representing indices of octree nodes.") - .def(nb::init<>()) + .def(nb::init()) .def(nb::init(), "height"_a, "position"_a) .def_rw("height", &OctreeIndex::height, "height"_a = 0, diff --git a/library/python/src/maps.cc b/library/python/src/maps.cc index 6aa29c3bd..e083d7bf4 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 @@ -9,6 +10,7 @@ #include #include #include +#include #include #include #include @@ -96,7 +98,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 +107,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/library/python/src/measurements.cc b/library/python/src/measurements.cc index 1c51f7052..cf8a8c58e 100644 --- a/library/python/src/measurements.cc +++ b/library/python/src/measurements.cc @@ -13,6 +13,9 @@ void add_measurement_bindings(nb::module_& m) { nb::class_(m, "Rotation", "A class representing rotations in 3D space.") .def(nb::init(), "rotation_matrix"_a) + .def(nb::init(), + "w"_a, "x"_a, "y"_a, "z"_a) .def("inverse", &Rotation3D::inverse, "Compute the rotation's inverse."); nb::class_(m, "Pose", "A class representing poses in 3D space.") @@ -24,7 +27,8 @@ void add_measurement_bindings(nb::module_& m) { // Pointclouds nb::class_>(m, "Pointcloud", "A class to store pointclouds.") - .def(nb::init::Data>(), "point_matrix"_a); + .def(nb::init::Data>(), "point_matrix"_a) + .def_prop_ro("size", &Pointcloud<>::size); nb::class_>( m, "PosedPointcloud", "A class to store pointclouds with an associated pose.") @@ -33,7 +37,9 @@ void add_measurement_bindings(nb::module_& m) { // Images nb::class_>(m, "Image", "A class to store depth images.") - .def(nb::init::Data>(), "pixel_matrix"_a); + .def(nb::init::Data>(), "pixel_matrix"_a) + .def_prop_ro("size", &Image<>::size) + .def_prop_ro("dimensions", &Image<>::getDimensions); nb::class_>( m, "PosedImage", "A class to store depth images with an associated pose.") .def(nb::init>(), "pose"_a, "image"_a); diff --git a/library/python/src/pywavemap.cc b/library/python/src/pywavemap.cc index 2cc7ba0ae..3daf38674 100644 --- a/library/python/src/pywavemap.cc +++ b/library/python/src/pywavemap.cc @@ -1,6 +1,8 @@ #include #include "pywavemap/convert.h" +#include "pywavemap/edit.h" +#include "pywavemap/geometry.h" #include "pywavemap/indices.h" #include "pywavemap/logging.h" #include "pywavemap/maps.h" @@ -42,12 +44,23 @@ NB_MODULE(_pywavemap_bindings, m) { "Submodule with common conversion functions for wavemap index types."); add_convert_module(m_convert); + // Bindings for map editing tools + nb::module_ m_edit = + m.def_submodule("edit", + "edit\n" + "=======\n" + "Submodule with tools to edit wavemap maps."); + add_edit_module(m_edit); + // Bindings for index types add_index_bindings(m); // Bindings for measurement types add_measurement_bindings(m); + // Bindings for geometric types + add_geometry_bindings(m); + // Bindings for map types add_map_bindings(m); diff --git a/library/python/src/pywavemap/__init__.py b/library/python/src/pywavemap/__init__.py index f1db669c3..32601c5d2 100644 --- a/library/python/src/pywavemap/__init__.py +++ b/library/python/src/pywavemap/__init__.py @@ -5,10 +5,11 @@ from ._pywavemap_bindings import OctreeIndex from ._pywavemap_bindings import (Rotation, Pose, Pointcloud, PosedPointcloud, Image, PosedImage) +from ._pywavemap_bindings import AABB, Sphere from ._pywavemap_bindings import (Map, HashedWaveletOctree, HashedChunkedWaveletOctree, InterpolationMode) from ._pywavemap_bindings import Pipeline # Binding submodules -from ._pywavemap_bindings import logging, param, convert +from ._pywavemap_bindings import logging, param, convert, edit 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" } } } diff --git a/tooling/scripts/build_and_test_all.sh b/tooling/scripts/build_and_test_all.sh index 0318b80ae..84c4707f0 100755 --- a/tooling/scripts/build_and_test_all.sh +++ b/tooling/scripts/build_and_test_all.sh @@ -7,11 +7,16 @@ if [ "$CI" ]; then flags="--no-status --force-color" fi +# Enter wavemap's home directory +pushd "$(dirname "$0")/../../" >> /dev/null || exit 1 + +# Build all ROS1 packages, including the ROS examples # shellcheck disable=SC2086 catkin build wavemap_all $flags # shellcheck disable=SC2086 catkin build wavemap_all $flags --no-deps --catkin-make-args tests +# Run all ROS1 tests, which indirectly also runs the C++ API's tests pushd "$(catkin locate --devel wavemap)/../.." >> /dev/null || exit 1 all_tests_passed=1 for f in lib/wavemap*/test_*; do @@ -23,3 +28,12 @@ if [ $all_tests_passed -ne 1 ]; then echo "Not all tests passed!" exit 1 fi + +# Build and test the Python API +python3 -m pip install -v ./library/python/ +pytest -rAv ./library/python/ + +# Build the C++ examples +pushd examples/cpp >> /dev/null || exit 1 +cmake -S . -B build -DCMAKE_BUILD_TYPE=Release +cmake --build build --parallel