From 1cd6d46c4395e043841b2fb4132251d83a256bc9 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Guilherne=20Caba=C3=A7o?= Date: Mon, 18 May 2026 13:33:01 +0000 Subject: [PATCH 01/14] Add digital twin wavemap experiments & Update wavemap ROS server --- examples/cpp/CMakeLists.txt | 8 ++ examples/cpp/digital_twin/CMakeLists.txt | 24 ++++ .../cpp/digital_twin/blocks_experiment.cc | 124 ++++++++++++++++++ examples/cpp/digital_twin/experiment.cc | 58 ++++++++ .../digital_twin/partial_save_experiment.cc | 111 ++++++++++++++++ .../cpp/digital_twin/publish_experiment.cc | 84 ++++++++++++ .../include/wavemap_ros/ros_server.h | 3 + interfaces/ros1/wavemap_ros/src/ros_server.cc | 54 ++++++++ 8 files changed, 466 insertions(+) create mode 100644 examples/cpp/digital_twin/CMakeLists.txt create mode 100644 examples/cpp/digital_twin/blocks_experiment.cc create mode 100644 examples/cpp/digital_twin/experiment.cc create mode 100644 examples/cpp/digital_twin/partial_save_experiment.cc create mode 100644 examples/cpp/digital_twin/publish_experiment.cc diff --git a/examples/cpp/CMakeLists.txt b/examples/cpp/CMakeLists.txt index 9cbadf540..d02f2a69c 100644 --- a/examples/cpp/CMakeLists.txt +++ b/examples/cpp/CMakeLists.txt @@ -34,3 +34,11 @@ endif () add_subdirectory(io) add_subdirectory(queries) add_subdirectory(planning) + +find_package(catkin QUIET + COMPONENTS roscpp wavemap wavemap_msgs wavemap_ros_conversions) +if (catkin_FOUND) + add_subdirectory(digital_twin) +else () + message(STATUS "Skipping digital_twin examples because ROS1/catkin dependencies were not found.") +endif () diff --git a/examples/cpp/digital_twin/CMakeLists.txt b/examples/cpp/digital_twin/CMakeLists.txt new file mode 100644 index 000000000..5c7ea23c6 --- /dev/null +++ b/examples/cpp/digital_twin/CMakeLists.txt @@ -0,0 +1,24 @@ +# Binaries +add_executable(experiment experiment.cc) +set_wavemap_target_properties(experiment) +target_include_directories(experiment PUBLIC ${catkin_INCLUDE_DIRS}) +target_link_libraries(experiment PUBLIC + wavemap::wavemap_core wavemap::wavemap_io ${catkin_LIBRARIES}) + +add_executable(publish_experiment publish_experiment.cc) +set_wavemap_target_properties(publish_experiment) +target_include_directories(publish_experiment PUBLIC ${catkin_INCLUDE_DIRS}) +target_link_libraries(publish_experiment PUBLIC + wavemap::wavemap_core wavemap::wavemap_io ${catkin_LIBRARIES}) + +add_executable(blocks_experiment blocks_experiment.cc) +set_wavemap_target_properties(blocks_experiment) +target_include_directories(blocks_experiment PUBLIC ${catkin_INCLUDE_DIRS}) +target_link_libraries(blocks_experiment PUBLIC + wavemap::wavemap_core wavemap::wavemap_io ${catkin_LIBRARIES}) + +add_executable(partial_save_experiment partial_save_experiment.cc) +set_wavemap_target_properties(partial_save_experiment) +target_include_directories(partial_save_experiment PUBLIC ${catkin_INCLUDE_DIRS}) +target_link_libraries(partial_save_experiment PUBLIC + wavemap::wavemap_core wavemap::wavemap_io ${catkin_LIBRARIES}) diff --git a/examples/cpp/digital_twin/blocks_experiment.cc b/examples/cpp/digital_twin/blocks_experiment.cc new file mode 100644 index 000000000..149ff2ea0 --- /dev/null +++ b/examples/cpp/digital_twin/blocks_experiment.cc @@ -0,0 +1,124 @@ +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +namespace { +template +bool publishMap(const HashedMapT& map, ros::Publisher& map_pub, + const std::string& map_frame, const std::string& label) { + wavemap_msgs::Map map_msg; + if (!wavemap::convert::mapToRosMsg(map, map_frame, ros::Time::now(), + map_msg)) { + std::cerr << "Failed to convert map to ROS msg for: " << label << "\n"; + return false; + } + + map_pub.publish(map_msg); + ros::spinOnce(); + std::cout << "Published " << label << " on " << map_pub.getTopic() << "\n"; + return true; +} + +template +bool eraseBlocksAndPublish(HashedMapT& map, const std::string& map_type_name, + ros::Publisher& map_pub, + const std::string& map_frame) { + std::cout << "Loaded map type: " << map_type_name << "\n"; + std::cout << "Initial block count: " << map.getHashMap().size() << "\n"; + + if (!publishMap(map, map_pub, map_frame, "original map")) { + return false; + } + + const std::vector blocks_to_erase{ + {0, 0, 0}, {1, 0, 0}, {1, 1, 0}, {0, 1, 0}, + {-1, 1, 0}, {-1, 0, 0}, {-1, -1, 0}, {0, -1, 0}, + {1, -1, 0}, {2, -1, 0}, {2, 0, 0}, {2, 1, 0}, + {2, 2, 0}, {1, 2, 0}, {0, 2, 0}, {-1, 2, 0}, + {-2, 2, 0}, {-2, 1, 0}, {-2, 0, 0}, {-2, -1, 0}, + {-2, -2, 0}, {-1, -2, 0}, {0, -2, 0}, {1, -2, 0}, + {2, -2, 0}, + }; + + for (const wavemap::Index3D& block_to_erase : blocks_to_erase) { + ros::Duration(1.0).sleep(); + + std::cout << "Erasing block " << block_to_erase.transpose() + << " | existed before erase: " << std::boolalpha + << map.hasBlock(block_to_erase) << "\n"; + const bool erased = map.eraseBlock(block_to_erase); + std::cout << " erased: " << erased + << " | remaining block count: " << map.getHashMap().size() + << "\n"; + + const std::string label = + "map after erasing block " + std::to_string(block_to_erase.x()) + " " + + std::to_string(block_to_erase.y()) + " " + + std::to_string(block_to_erase.z()); + if (!publishMap(map, map_pub, map_frame, label)) { + return false; + } + } + + return true; +} +} // namespace + +int main(int argc, char** argv) { + ros::init(argc, argv, "wavemap_blocks_experiment"); + ros::NodeHandle nh; + + const std::filesystem::path input_path = "/home/ci/data/maps/wavemap1.wvmp"; + const std::string map_topic = "/wavemap/map"; + const std::string map_frame = "map"; + + const int queue_size = 1; + const bool latch = true; + ros::Publisher map_pub = + nh.advertise(map_topic, queue_size, latch); + ros::Duration(0.5).sleep(); + + wavemap::MapBase::Ptr map; + if (!wavemap::io::fileToMap(input_path, map)) { + std::cerr << "Failed to load map from: " << input_path << "\n"; + return 1; + } + + if (!map) { + std::cerr << "Loaded map pointer is null.\n"; + return 1; + } + + if (auto hashed_chunked_map = + std::dynamic_pointer_cast( + map)) { + return eraseBlocksAndPublish(*hashed_chunked_map, + "hashed_chunked_wavelet_octree", map_pub, + map_frame) + ? 0 + : 1; + } + + if (auto hashed_map = + std::dynamic_pointer_cast(map)) { + return eraseBlocksAndPublish(*hashed_map, "hashed_wavelet_octree", map_pub, + map_frame) + ? 0 + : 1; + } + + std::cerr << "Loaded map does not expose hashed block APIs.\n"; + std::cerr << "Use a hashed_blocks, hashed_wavelet_octree, or " + "hashed_chunked_wavelet_octree map.\n"; + return 1; +} diff --git a/examples/cpp/digital_twin/experiment.cc b/examples/cpp/digital_twin/experiment.cc new file mode 100644 index 000000000..a596cf20d --- /dev/null +++ b/examples/cpp/digital_twin/experiment.cc @@ -0,0 +1,58 @@ +#include +#include + +#include +#include +#include +#include + +int main() { + const std::filesystem::path input_path = "/home/ci/data/maps/wavemap1.wvmp"; + const std::filesystem::path output_path = "/home/ci/data/maps/map_copy.wvmp"; + + wavemap::MapBase::Ptr map; + + if (!wavemap::io::fileToMap(input_path, map)) { + std::cerr << "Failed to load map from: " << input_path << "\n"; + return 1; + } + + if (!map) { + std::cerr << "Loaded map pointer is null.\n"; + return 1; + } + std::cout << "Loaded map from: " << input_path << "\n"; + + map->threshold(); + + size_t occupied_like = 0; + size_t free_like = 0; + + map->forEachLeaf([&](const wavemap::OctreeIndex&, float value) { + if (value > 0.f) { + ++occupied_like; + } else if (value < 0.f) { + ++free_like; + } + }); + + std::cout << "Occupied-like leaves: " << occupied_like << "\n"; + std::cout << "Free-like leaves: " << free_like << "\n"; + + if (std::dynamic_pointer_cast(map)) { + std::cout << "Loaded as hashed_wavelet_octree\n"; + } + + if (std::dynamic_pointer_cast(map)) { + std::cout << "Loaded as hashed_chunked_wavelet_octree\n"; + } + + if (!wavemap::io::mapToFile(*map, output_path)) { + std::cerr << "Failed to save map to: " << output_path << "\n"; + return 1; + } + + std::cout << "Saved map to: " << output_path << "\n"; + + return 0; +} diff --git a/examples/cpp/digital_twin/partial_save_experiment.cc b/examples/cpp/digital_twin/partial_save_experiment.cc new file mode 100644 index 000000000..79b1f9839 --- /dev/null +++ b/examples/cpp/digital_twin/partial_save_experiment.cc @@ -0,0 +1,111 @@ +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +namespace { +bool keepBlock(const wavemap::Index3D& block_index) { + return block_index.x() <= 1 && block_index.x() >= -1 && block_index.y() <= 1 && block_index.y() >= -1 && block_index.z() <= 1 && block_index.z() >= -1; +} + +template +bool publishMap(const MapT& map, ros::Publisher& map_pub, const std::string& map_frame, const std::string& label) { + wavemap_msgs::Map map_msg; + if (!wavemap::convert::mapToRosMsg(map, map_frame, ros::Time::now(), map_msg)) { + std::cerr << "Failed to convert " << label << " to ROS msg.\n"; + return false; + } + + map_pub.publish(map_msg); + ros::spinOnce(); + std::cout << "Published " << label << " on " << map_pub.getTopic() << "\n"; + return true; +} + +template +bool savePartialMap(HashedMapT& map, const std::filesystem::path& output_path) { + const size_t initial_block_count = map.getHashMap().size(); + + map.eraseBlockIf([](const wavemap::Index3D& block_index, const auto& /*block*/) { return !keepBlock(block_index); }); + + const size_t final_block_count = map.getHashMap().size(); + std::cout << "Kept " << final_block_count << " of " << initial_block_count << " blocks.\n"; + + if (!wavemap::io::mapToFile(map, output_path)) { + std::cerr << "Failed to save partial map to: " << output_path << "\n"; + return false; + } + + std::cout << "Saved partial map to: " << output_path << "\n"; + return true; +} + +template +int runExperiment(HashedMapT& map, ros::Publisher& map_pub, const std::string& map_frame, const std::filesystem::path& output_path, const std::string& map_type_name) { + std::cout << "Loaded map type: " << map_type_name << "\n"; + std::cout << "Original block count: " << map.getHashMap().size() << "\n"; + + if (!publishMap(map, map_pub, map_frame, "original map")) { + return 1; + } + + std::cout << "Showing original map...\n"; + ros::Duration(5.0).sleep(); + + if (!savePartialMap(map, output_path)) { + return 1; + } + + if (!publishMap(map, map_pub, map_frame, "partial map")) { + return 1; + } + + ros::spin(); + return 0; +} +} // namespace + +int main(int argc, char** argv) { + ros::init(argc, argv, "wavemap_partial_save_experiment"); + ros::NodeHandle nh; + + const std::filesystem::path input_path = "/home/ci/data/maps/wavemap1.wvmp"; + const std::filesystem::path output_path = "/home/ci/data/maps/wavemap_blocks.wvmp"; + const std::string map_topic = "/wavemap/map"; + const std::string map_frame = "map"; + + const int queue_size = 1; + const bool latch = true; + ros::Publisher map_pub = nh.advertise(map_topic, queue_size, latch); + ros::Duration(0.5).sleep(); + + wavemap::MapBase::Ptr map; + if (!wavemap::io::fileToMap(input_path, map)) { + std::cerr << "Failed to load map from: " << input_path << "\n"; + return 1; + } + + if (!map) { + std::cerr << "Loaded map pointer is null.\n"; + return 1; + } + + if (auto hashed_chunked_map = std::dynamic_pointer_cast(map)) { + return runExperiment(*hashed_chunked_map, map_pub, map_frame, output_path, "hashed_chunked_wavelet_octree"); + } + + if (auto hashed_map = std::dynamic_pointer_cast(map)) { + return runExperiment(*hashed_map, map_pub, map_frame, output_path, "hashed_wavelet_octree"); + } + + std::cerr << "Loaded map does not expose hashed block APIs.\n"; + std::cerr << "Use a hashed_wavelet_octree or hashed_chunked_wavelet_octree map.\n"; + return 1; +} diff --git a/examples/cpp/digital_twin/publish_experiment.cc b/examples/cpp/digital_twin/publish_experiment.cc new file mode 100644 index 000000000..ae177b5be --- /dev/null +++ b/examples/cpp/digital_twin/publish_experiment.cc @@ -0,0 +1,84 @@ +#include +#include +#include + +#include +#include +#include +#include +#include + +int main(int argc, char** argv) { + ros::init(argc, argv, "wavemap_publish_experiment"); + ros::NodeHandle nh; + + const std::filesystem::path input_path = "/home/ci/data/maps/wavemap1.wvmp"; + const std::string map_topic = "/wavemap/map"; + const std::string map_frame = "map"; + const std::filesystem::path output_path = + "/home/ci/data/maps/wavemap1_updated.wvmp"; + + wavemap::MapBase::Ptr map; + if (!wavemap::io::fileToMap(input_path, map)) { + std::cerr << "Failed to load map from: " << input_path << "\n"; + return 1; + } + + if (!map) { + std::cerr << "Loaded map pointer is null.\n"; + return 1; + } + + wavemap_msgs::Map map_msg; + if (!wavemap::convert::mapToRosMsg(*map, map_frame, ros::Time::now(), + map_msg)) { + std::cerr << "Failed to convert loaded map to wavemap_msgs::Map.\n"; + return 1; + } + + const int queue_size = 1; + const bool latch = true; + ros::Publisher map_pub = + nh.advertise(map_topic, queue_size, latch); + + ros::Duration(0.5).sleep(); + map_pub.publish(map_msg); + ros::spinOnce(); + + std::cout << "Before changes " << input_path << "\n"; + + ros::Duration(5.0).sleep(); + for (int x = 0; x <= 10; ++x) { + for (int y = 0; y <= 10; ++y) { + for (int z = 0; z <= 10; ++z) { + wavemap::Index3D index(x, y, z); + map->setCellValue(index, 5.0f); + } + } + } + + wavemap_msgs::Map updated_msg; + if (!wavemap::convert::mapToRosMsg(*map, map_frame, ros::Time::now(), + updated_msg)) { + std::cerr << "Failed to convert updated map to wavemap_msgs::Map.\n"; + return 1; + } + map_pub.publish(updated_msg); + + std::cout << "After changes " << input_path << "\n"; + + std::cout << "Loaded map from: " << input_path << "\n"; + std::cout << "Published map on: " << map_topic << "\n"; + std::cout << "RViz fixed frame should be: " << map_frame << "\n"; + + if (!wavemap::io::mapToFile(*map, output_path)) { + std::cerr << "Failed to save map to: " << output_path << "\n"; + return 1; + } + + std::cout << "Saved modified map to: " << output_path << "\n"; + + ros::spin(); + + return 0; +} diff --git a/interfaces/ros1/wavemap_ros/include/wavemap_ros/ros_server.h b/interfaces/ros1/wavemap_ros/include/wavemap_ros/ros_server.h index 801f53138..30c9db3dd 100644 --- a/interfaces/ros1/wavemap_ros/include/wavemap_ros/ros_server.h +++ b/interfaces/ros1/wavemap_ros/include/wavemap_ros/ros_server.h @@ -82,6 +82,9 @@ class RosServer { // Map management pipeline std::shared_ptr pipeline_; + param::Array map_operation_param_array_;// dt_architecture_changes + param::Map measurement_integrator_param_map_;// dt_architecture_changes + param::Array input_param_array_;// dt_architecture_changes // Measurement and pose inputs std::vector> inputs_; diff --git a/interfaces/ros1/wavemap_ros/src/ros_server.cc b/interfaces/ros1/wavemap_ros/src/ros_server.cc index 3d94171a4..d53cfec79 100644 --- a/interfaces/ros1/wavemap_ros/src/ros_server.cc +++ b/interfaces/ros1/wavemap_ros/src/ros_server.cc @@ -64,25 +64,46 @@ RosServer::RosServer(ros::NodeHandle nh, ros::NodeHandle nh_private, CHECK_NOTNULL(pipeline_); // Add map operations to pipeline + /* dt_architecture_changes const param::Array map_operation_param_array = param::convert::toParamArray(nh_private, "map_operations"); for (const auto& operation_params : map_operation_param_array) { addOperation(operation_params, nh_private); + }*/ + // dt_architecture_changes + map_operation_param_array_ = + param::convert::toParamArray(nh_private, "map_operations"); + for (const auto& operation_params : map_operation_param_array_) { + addOperation(operation_params, nh_private); } // Add measurement integrators to pipeline + /* dt_architecture_changes const param::Map measurement_integrator_param_map = param::convert::toParamMap(nh_private, "measurement_integrators"); for (const auto& [integrator_name, integrator_params] : measurement_integrator_param_map) { pipeline_->addIntegrator(integrator_name, integrator_params); + }*/ + measurement_integrator_param_map_ = + param::convert::toParamMap(nh_private, "measurement_integrators"); + for (const auto& [integrator_name, integrator_params] : + measurement_integrator_param_map_) { + pipeline_->addIntegrator(integrator_name, integrator_params); } + // Setup measurement inputs + /* dt_architecture_changes const param::Array input_param_array = param::convert::toParamArray(nh_private, "inputs"); for (const auto& integrator_params : input_param_array) { addInput(integrator_params, nh, nh_private); + }*/ + input_param_array_ = + param::convert::toParamArray(nh_private, "inputs"); + for (const auto& integrator_params : input_param_array_) { + addInput(integrator_params, nh, nh_private); } // Connect to ROS @@ -164,9 +185,42 @@ bool RosServer::saveMap(const std::filesystem::path& file_path) const { return false; } +/*dt_architecture_changes bool RosServer::loadMap(const std::filesystem::path& file_path) { return io::fileToMap(file_path, occupancy_map_); } +*/ +//dt_architecture_changes +bool RosServer::loadMap(const std::filesystem::path& file_path) { + MapBase::Ptr loaded_map; + if (!io::fileToMap(file_path, loaded_map)) { + return false; + } + + occupancy_map_ = loaded_map; + + clearInputs(); + pipeline_.reset(); + pipeline_ = std::make_shared(occupancy_map_, thread_pool_); + + ros::NodeHandle nh; + ros::NodeHandle nh_private("~"); + + for (const auto& operation_params : map_operation_param_array_) { + addOperation(operation_params, nh_private); + } + + for (const auto& [integrator_name, integrator_params] : + measurement_integrator_param_map_) { + pipeline_->addIntegrator(integrator_name, integrator_params); + } + + for (const auto& input_params : input_param_array_) { + addInput(input_params, nh, nh_private); + } + + return true; +} void RosServer::advertiseServices(ros::NodeHandle& nh_private) { reset_map_srv_ = nh_private.advertiseService Date: Wed, 3 Jun 2026 11:33:58 +0000 Subject: [PATCH 02/14] Add voxel data type example --- examples/cpp/CMakeLists.txt | 8 +- examples/cpp/digital_twin/CMakeLists.txt | 51 ++++--- examples/cpp/digital_twin/new_voxel.cc | 40 ++++++ .../wavemap/core/map/cell_types/voxel_data.h | 127 ++++++++++++++++++ 4 files changed, 199 insertions(+), 27 deletions(-) create mode 100644 examples/cpp/digital_twin/new_voxel.cc create mode 100644 library/cpp/include/wavemap/core/map/cell_types/voxel_data.h diff --git a/examples/cpp/CMakeLists.txt b/examples/cpp/CMakeLists.txt index d02f2a69c..bbb5fbc9c 100644 --- a/examples/cpp/CMakeLists.txt +++ b/examples/cpp/CMakeLists.txt @@ -35,10 +35,4 @@ add_subdirectory(io) add_subdirectory(queries) add_subdirectory(planning) -find_package(catkin QUIET - COMPONENTS roscpp wavemap wavemap_msgs wavemap_ros_conversions) -if (catkin_FOUND) - add_subdirectory(digital_twin) -else () - message(STATUS "Skipping digital_twin examples because ROS1/catkin dependencies were not found.") -endif () +add_subdirectory(digital_twin) diff --git a/examples/cpp/digital_twin/CMakeLists.txt b/examples/cpp/digital_twin/CMakeLists.txt index 5c7ea23c6..3be13619f 100644 --- a/examples/cpp/digital_twin/CMakeLists.txt +++ b/examples/cpp/digital_twin/CMakeLists.txt @@ -1,24 +1,35 @@ # Binaries -add_executable(experiment experiment.cc) -set_wavemap_target_properties(experiment) -target_include_directories(experiment PUBLIC ${catkin_INCLUDE_DIRS}) -target_link_libraries(experiment PUBLIC - wavemap::wavemap_core wavemap::wavemap_io ${catkin_LIBRARIES}) +add_executable(new_voxel new_voxel.cc) +set_wavemap_target_properties(new_voxel) +target_link_libraries(new_voxel PUBLIC wavemap::wavemap_core) -add_executable(publish_experiment publish_experiment.cc) -set_wavemap_target_properties(publish_experiment) -target_include_directories(publish_experiment PUBLIC ${catkin_INCLUDE_DIRS}) -target_link_libraries(publish_experiment PUBLIC - wavemap::wavemap_core wavemap::wavemap_io ${catkin_LIBRARIES}) +if ("$ENV{ROS_VERSION}" STREQUAL "1") + find_package(catkin REQUIRED + COMPONENTS roscpp wavemap wavemap_msgs wavemap_ros_conversions) -add_executable(blocks_experiment blocks_experiment.cc) -set_wavemap_target_properties(blocks_experiment) -target_include_directories(blocks_experiment PUBLIC ${catkin_INCLUDE_DIRS}) -target_link_libraries(blocks_experiment PUBLIC - wavemap::wavemap_core wavemap::wavemap_io ${catkin_LIBRARIES}) + add_executable(experiment experiment.cc) + set_wavemap_target_properties(experiment) + target_include_directories(experiment PUBLIC ${catkin_INCLUDE_DIRS}) + target_link_libraries(experiment PUBLIC + wavemap::wavemap_core wavemap::wavemap_io ${catkin_LIBRARIES}) -add_executable(partial_save_experiment partial_save_experiment.cc) -set_wavemap_target_properties(partial_save_experiment) -target_include_directories(partial_save_experiment PUBLIC ${catkin_INCLUDE_DIRS}) -target_link_libraries(partial_save_experiment PUBLIC - wavemap::wavemap_core wavemap::wavemap_io ${catkin_LIBRARIES}) + add_executable(publish_experiment publish_experiment.cc) + set_wavemap_target_properties(publish_experiment) + target_include_directories(publish_experiment PUBLIC ${catkin_INCLUDE_DIRS}) + target_link_libraries(publish_experiment PUBLIC + wavemap::wavemap_core wavemap::wavemap_io ${catkin_LIBRARIES}) + + add_executable(blocks_experiment blocks_experiment.cc) + set_wavemap_target_properties(blocks_experiment) + target_include_directories(blocks_experiment PUBLIC ${catkin_INCLUDE_DIRS}) + target_link_libraries(blocks_experiment PUBLIC + wavemap::wavemap_core wavemap::wavemap_io ${catkin_LIBRARIES}) + + add_executable(partial_save_experiment partial_save_experiment.cc) + set_wavemap_target_properties(partial_save_experiment) + target_include_directories(partial_save_experiment PUBLIC ${catkin_INCLUDE_DIRS}) + target_link_libraries(partial_save_experiment PUBLIC + wavemap::wavemap_core wavemap::wavemap_io ${catkin_LIBRARIES}) +else () + message(STATUS "Skipping ROS digital_twin examples because ROS1 is not active.") +endif () diff --git a/examples/cpp/digital_twin/new_voxel.cc b/examples/cpp/digital_twin/new_voxel.cc new file mode 100644 index 000000000..8dee71904 --- /dev/null +++ b/examples/cpp/digital_twin/new_voxel.cc @@ -0,0 +1,40 @@ +#include + +#include + +namespace { +struct DigitalTwinData { + float r = 0.f; + float g = 0.f; + float b = 0.f; + float traversability = 0.f; + + bool operator==(const DigitalTwinData& other) const { + return r == other.r && g == other.g && b == other.b && traversability == other.traversability; + } +}; +} // namespace + +int main() { + wavemap::VoxelData<> occupancy_only; + occupancy_only = 1.0f; + occupancy_only += 0.5f; + + std::cout << "Occupancy only: " << occupancy_only.occupancy << "\n"; + + wavemap::VoxelData digital_twin_voxel( + 0.8f, DigitalTwinData{1.f, 0.f, 0.f, 0.75f}); + digital_twin_voxel += 0.2f; + + std::cout << "Digital twin occupancy: " << digital_twin_voxel.occupancy + << "\n"; + std::cout << "Color: " << digital_twin_voxel.data.r << " " + << digital_twin_voxel.data.g << " " + << digital_twin_voxel.data.b << "\n"; + std::cout << "Traversability: " << digital_twin_voxel.data.traversability << "\n"; + + wavemap::FloatingPoint occupancy_value = digital_twin_voxel; + std::cout << "Implicit occupancy value: " << occupancy_value << "\n"; + + return 0; +} diff --git a/library/cpp/include/wavemap/core/map/cell_types/voxel_data.h b/library/cpp/include/wavemap/core/map/cell_types/voxel_data.h new file mode 100644 index 000000000..a3283839e --- /dev/null +++ b/library/cpp/include/wavemap/core/map/cell_types/voxel_data.h @@ -0,0 +1,127 @@ +#ifndef WAVEMAP_CORE_DATA_STRUCTURE_VOXEL_DATA_H_ +#define WAVEMAP_CORE_DATA_STRUCTURE_VOXEL_DATA_H_ + +#include "wavemap/core/common.h" + +namespace wavemap { +/** + * @brief A template structure for storing voxel data with mandatory occupancy + * and arbitrary additional data. + * + * This template allows users to store custom data alongside occupancy + * information in each voxel without compromising efficiency. + * + * @tparam AdditionalDataT The type of additional data to store in each voxel. + * Use void (default) for occupancy-only storage. + * + * Example usage: + * @code + * // Voxel with occupancy only (same as FloatingPoint) + * using BasicVoxel = VoxelData; + * + * // Voxel with occupancy + color + * struct ColorData { + * uint8_t r, g, b; + * }; + * using ColorVoxel = VoxelData; + * + * // Voxel with occupancy + semantic label + * using SemanticVoxel = VoxelData; + * @endcode + */ +template +struct VoxelData { + FloatingPoint occupancy = 0.f; + AdditionalDataT data{}; + + // Default constructor + VoxelData() = default; + + // Constructor from occupancy only + explicit VoxelData(FloatingPoint occupancy_value) + : occupancy(occupancy_value) {} + + // Constructor from occupancy and additional data + VoxelData(FloatingPoint occupancy_value, const AdditionalDataT& data_value) + : occupancy(occupancy_value), data(data_value) {} + + // Implicit conversion to FloatingPoint for backward compatibility + operator FloatingPoint() const { return occupancy; } + + // Assignment from FloatingPoint + VoxelData& operator=(FloatingPoint value) { + occupancy = value; + return *this; + } + + // Equality operators + bool operator==(const VoxelData& other) const { + return occupancy == other.occupancy && data == other.data; + } + + bool operator!=(const VoxelData& other) const { return !(*this == other); } + + // Allow use with += operator for occupancy updates + VoxelData& operator+=(FloatingPoint value) { + occupancy += value; + return *this; + } + + // Addition operator + VoxelData operator+(FloatingPoint value) const { + VoxelData result = *this; + result.occupancy += value; + return result; + } +}; + +/** + * @brief Specialization for void additional data (occupancy-only). + * + * This specialization provides the same interface as the general template + * but stores only the occupancy value, making it fully compatible with + * FloatingPoint while still allowing future extension. + */ +template <> +struct VoxelData { + FloatingPoint occupancy = 0.f; + + // Default constructor + VoxelData() = default; + + // Constructor from occupancy + explicit VoxelData(FloatingPoint occupancy_value) + : occupancy(occupancy_value) {} + + // Implicit conversion to FloatingPoint + operator FloatingPoint() const { return occupancy; } + + // Assignment from FloatingPoint + VoxelData& operator=(FloatingPoint value) { + occupancy = value; + return *this; + } + + // Equality operators + bool operator==(const VoxelData& other) const { + return occupancy == other.occupancy; + } + + bool operator!=(const VoxelData& other) const { return !(*this == other); } + + // Allow use with += operator + VoxelData& operator+=(FloatingPoint value) { + occupancy += value; + return *this; + } + + // Addition operator + VoxelData operator+(FloatingPoint value) const { + VoxelData result = *this; + result.occupancy += value; + return result; + } +}; +} // namespace wavemap + +#endif // WAVEMAP_CORE_DATA_STRUCTURE_VOXEL_DATA_H_ \ No newline at end of file From a662e3a2428cd64b47cb4dce93d5aa7bcebfd5f2 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Guilherne=20Caba=C3=A7o?= Date: Tue, 9 Jun 2026 10:19:13 +0000 Subject: [PATCH 03/14] Add templated voxel data support to hashed wavelet octree --- examples/cpp/digital_twin/CMakeLists.txt | 18 ++++ ...legacy_hashed_wavelet_octree_experiment.cc | 38 ++++++++ examples/cpp/digital_twin/new_voxel.cc | 49 ++++++++--- .../digital_twin/voxel_block_experiment.cc | 86 +++++++++++++++++++ .../cpp/digital_twin/voxel_haar_transform.cc | 77 +++++++++++++++++ .../cpp/digital_twin/voxel_map_experiment.cc | 85 ++++++++++++++++++ 6 files changed, 343 insertions(+), 10 deletions(-) create mode 100644 examples/cpp/digital_twin/legacy_hashed_wavelet_octree_experiment.cc create mode 100644 examples/cpp/digital_twin/voxel_block_experiment.cc create mode 100644 examples/cpp/digital_twin/voxel_haar_transform.cc create mode 100644 examples/cpp/digital_twin/voxel_map_experiment.cc diff --git a/examples/cpp/digital_twin/CMakeLists.txt b/examples/cpp/digital_twin/CMakeLists.txt index 3be13619f..fc3631006 100644 --- a/examples/cpp/digital_twin/CMakeLists.txt +++ b/examples/cpp/digital_twin/CMakeLists.txt @@ -3,6 +3,24 @@ add_executable(new_voxel new_voxel.cc) set_wavemap_target_properties(new_voxel) target_link_libraries(new_voxel PUBLIC wavemap::wavemap_core) +add_executable(voxel_haar_transform voxel_haar_transform.cc) +set_wavemap_target_properties(voxel_haar_transform) +target_link_libraries(voxel_haar_transform PUBLIC wavemap::wavemap_core) + +add_executable(voxel_block_experiment voxel_block_experiment.cc) +set_wavemap_target_properties(voxel_block_experiment) +target_link_libraries(voxel_block_experiment PUBLIC wavemap::wavemap_core) + +add_executable(voxel_map_experiment voxel_map_experiment.cc) +set_wavemap_target_properties(voxel_map_experiment) +target_link_libraries(voxel_map_experiment PUBLIC wavemap::wavemap_core) + +add_executable(legacy_hashed_wavelet_octree_experiment + legacy_hashed_wavelet_octree_experiment.cc) +set_wavemap_target_properties(legacy_hashed_wavelet_octree_experiment) +target_link_libraries(legacy_hashed_wavelet_octree_experiment + PUBLIC wavemap::wavemap_core) + if ("$ENV{ROS_VERSION}" STREQUAL "1") find_package(catkin REQUIRED COMPONENTS roscpp wavemap wavemap_msgs wavemap_ros_conversions) diff --git a/examples/cpp/digital_twin/legacy_hashed_wavelet_octree_experiment.cc b/examples/cpp/digital_twin/legacy_hashed_wavelet_octree_experiment.cc new file mode 100644 index 000000000..3e8509561 --- /dev/null +++ b/examples/cpp/digital_twin/legacy_hashed_wavelet_octree_experiment.cc @@ -0,0 +1,38 @@ +#include + +#include + +int main() { + wavemap::HashedWaveletOctreeConfig config; + config.min_cell_width = 0.1f; + config.min_log_odds = -2.f; + config.max_log_odds = 4.f; + config.tree_height = 3; + + wavemap::HashedWaveletOctree map(config); + + const wavemap::Index3D voxel_index(9, 2, 3); + + map.setCellValue(voxel_index, 0.8f); + std::cout << "After setCellValue: " << map.getCellValue(voxel_index) + << "\n"; + + map.addToCellValue(voxel_index, 0.2f); + std::cout << "After addToCellValue: " << map.getCellValue(voxel_index) + << "\n"; + + size_t visited_leaves = 0u; + map.forEachLeaf( + [&visited_leaves, &voxel_index](const wavemap::OctreeIndex& leaf_index, + wavemap::FloatingPoint occupancy) { + ++visited_leaves; + if (leaf_index.height == 0 && leaf_index.position == voxel_index) { + std::cout << "Visited target leaf occupancy: " << occupancy << "\n"; + } + }); + + std::cout << "Visited leaves: " << visited_leaves << "\n"; + std::cout << "Block count: " << map.getHashMap().size() << "\n"; + + return 0; +} diff --git a/examples/cpp/digital_twin/new_voxel.cc b/examples/cpp/digital_twin/new_voxel.cc index 8dee71904..5dff14c17 100644 --- a/examples/cpp/digital_twin/new_voxel.cc +++ b/examples/cpp/digital_twin/new_voxel.cc @@ -10,9 +10,35 @@ struct DigitalTwinData { float traversability = 0.f; bool operator==(const DigitalTwinData& other) const { - return r == other.r && g == other.g && b == other.b && traversability == other.traversability; + return r == other.r && g == other.g && b == other.b && + traversability == other.traversability; } }; + +struct DigitalTwinDataPolicy { + static DigitalTwinData add(const DigitalTwinData& lhs, const DigitalTwinData& rhs) { + return {lhs.r + rhs.r, lhs.g + rhs.g, lhs.b + rhs.b, + lhs.traversability + rhs.traversability}; + } + + static DigitalTwinData subtract(const DigitalTwinData& lhs, + const DigitalTwinData& rhs) { + return {lhs.r - rhs.r, lhs.g - rhs.g, lhs.b - rhs.b, + lhs.traversability - rhs.traversability}; + } + + static DigitalTwinData scale(const DigitalTwinData& data, + wavemap::FloatingPoint factor) { + return {factor * data.r, factor * data.g, factor * data.b, + factor * data.traversability}; + } +}; + +void printVoxel(const char* label, const wavemap::VoxelData& voxel) { + std::cout << label << " occupancy: " << voxel.occupancy << "\n"; + std::cout << label << " color: " << voxel.data.r << " " << voxel.data.g << " " << voxel.data.b << "\n"; + std::cout << label << " traversability: " << voxel.data.traversability << "\n"; +} } // namespace int main() { @@ -22,19 +48,22 @@ int main() { std::cout << "Occupancy only: " << occupancy_only.occupancy << "\n"; - wavemap::VoxelData digital_twin_voxel( - 0.8f, DigitalTwinData{1.f, 0.f, 0.f, 0.75f}); - digital_twin_voxel += 0.2f; + using DigitalTwinVoxel = wavemap::VoxelData; - std::cout << "Digital twin occupancy: " << digital_twin_voxel.occupancy - << "\n"; - std::cout << "Color: " << digital_twin_voxel.data.r << " " - << digital_twin_voxel.data.g << " " - << digital_twin_voxel.data.b << "\n"; - std::cout << "Traversability: " << digital_twin_voxel.data.traversability << "\n"; + DigitalTwinVoxel digital_twin_voxel(0.8f, DigitalTwinData{1.f, 0.f, 0.f, 0.75f}); + digital_twin_voxel += 0.2f; + printVoxel("Digital twin", digital_twin_voxel); wavemap::FloatingPoint occupancy_value = digital_twin_voxel; std::cout << "Implicit occupancy value: " << occupancy_value << "\n"; + const DigitalTwinVoxel voxel_a(0.4f, DigitalTwinData{1.f, 0.2f, 0.f, 0.3f}); + const DigitalTwinVoxel voxel_b(0.6f, DigitalTwinData{0.f, 0.3f, 1.f, 0.5f}); + + printVoxel("Sum", voxel_a + voxel_b); + printVoxel("Difference", voxel_b - voxel_a); + printVoxel("Scaled", 0.5f * voxel_b); + printVoxel("Divided", voxel_b / 2.f); + return 0; } diff --git a/examples/cpp/digital_twin/voxel_block_experiment.cc b/examples/cpp/digital_twin/voxel_block_experiment.cc new file mode 100644 index 000000000..a1fde770c --- /dev/null +++ b/examples/cpp/digital_twin/voxel_block_experiment.cc @@ -0,0 +1,86 @@ +#include + +#include +#include + +namespace { +struct DigitalTwinData { + float r = 0.f; + float g = 0.f; + float b = 0.f; + float traversability = 0.f; + + bool operator==(const DigitalTwinData& other) const { + return r == other.r && g == other.g && b == other.b && + traversability == other.traversability; + } +}; + +struct DigitalTwinDataPolicy { + static DigitalTwinData add(const DigitalTwinData& lhs, + const DigitalTwinData& rhs) { + return {lhs.r + rhs.r, lhs.g + rhs.g, lhs.b + rhs.b, + lhs.traversability + rhs.traversability}; + } + + static DigitalTwinData subtract(const DigitalTwinData& lhs, + const DigitalTwinData& rhs) { + return {lhs.r - rhs.r, lhs.g - rhs.g, lhs.b - rhs.b, + lhs.traversability - rhs.traversability}; + } + + static DigitalTwinData scale(const DigitalTwinData& data, + wavemap::FloatingPoint factor) { + return {factor * data.r, factor * data.g, factor * data.b, + factor * data.traversability}; + } +}; + +using DigitalTwinVoxel = wavemap::VoxelData; +using DigitalTwinBlock = wavemap::HashedWaveletOctreeBlockT; + +void printVoxel(const char* label, const DigitalTwinVoxel& voxel) { + std::cout << label << " occupancy: " << voxel.occupancy << "\n"; + std::cout << label << " color: " << voxel.data.r << " " << voxel.data.g + << " " << voxel.data.b << "\n"; + std::cout << label << " traversability: " + << voxel.data.traversability << "\n"; +} +} // namespace + +int main() { + constexpr wavemap::IndexElement tree_height = 3; + constexpr wavemap::FloatingPoint min_log_odds = -2.f; + constexpr wavemap::FloatingPoint max_log_odds = 4.f; + + DigitalTwinBlock block(tree_height, min_log_odds, max_log_odds); + + const wavemap::OctreeIndex voxel_index{0, wavemap::Index3D{1, 2, 3}}; + const DigitalTwinVoxel initial_voxel(0.8f, DigitalTwinData{1.f, 0.f, 0.f, 0.75f}); + + block.setVoxelValue(voxel_index, initial_voxel); + printVoxel("Stored voxel", block.getVoxelValue(voxel_index)); + + block.addToCellValue(voxel_index, 0.2f); + printVoxel("After occupancy update", block.getVoxelValue(voxel_index)); + + const DigitalTwinVoxel voxel_update(0.1f, DigitalTwinData{0.f, 0.5f, 0.f, -0.25f}); + block.addToVoxelValue(voxel_index, voxel_update); + printVoxel("After full voxel update", block.getVoxelValue(voxel_index)); + + size_t visited_leaves = 0u; + block.forEachVoxelLeaf( + wavemap::Index3D::Zero(), + [&visited_leaves](const wavemap::OctreeIndex& leaf_index, + const DigitalTwinVoxel& voxel) { + ++visited_leaves; + if (leaf_index.height == 0 && leaf_index.position == wavemap::Index3D{1, 2, 3}) { + printVoxel("Visited target leaf", voxel); + } + }); + + std::cout << "Visited leaves: " << visited_leaves << "\n"; + std::cout << "Occupancy view: " << block.getCellValue(voxel_index) << "\n"; + + return 0; +} diff --git a/examples/cpp/digital_twin/voxel_haar_transform.cc b/examples/cpp/digital_twin/voxel_haar_transform.cc new file mode 100644 index 000000000..e09e007e3 --- /dev/null +++ b/examples/cpp/digital_twin/voxel_haar_transform.cc @@ -0,0 +1,77 @@ +#include +#include + +#include +#include +#include + +namespace { +struct DigitalTwinData { + float r = 0.f; + float g = 0.f; + float b = 0.f; + float traversability = 0.f; + + bool operator==(const DigitalTwinData& other) const { + return r == other.r && g == other.g && b == other.b && + traversability == other.traversability; + } +}; + +struct DigitalTwinDataPolicy { + static DigitalTwinData add(const DigitalTwinData& lhs, + const DigitalTwinData& rhs) { + return {lhs.r + rhs.r, lhs.g + rhs.g, lhs.b + rhs.b, + lhs.traversability + rhs.traversability}; + } + + static DigitalTwinData subtract(const DigitalTwinData& lhs, + const DigitalTwinData& rhs) { + return {lhs.r - rhs.r, lhs.g - rhs.g, lhs.b - rhs.b, + lhs.traversability - rhs.traversability}; + } + + static DigitalTwinData scale(const DigitalTwinData& data, + wavemap::FloatingPoint factor) { + return {factor * data.r, factor * data.g, factor * data.b, + factor * data.traversability}; + } +}; + +using DigitalTwinVoxel = + wavemap::VoxelData; +using Coefficients = wavemap::HaarCoefficients; +using Transform = wavemap::HaarTransform; + +void printVoxel(const char* label, const DigitalTwinVoxel& voxel) { + std::cout << label << " occupancy: " << voxel.occupancy << "\n"; + std::cout << label << " color: " << voxel.data.r << " " << voxel.data.g + << " " << voxel.data.b << "\n"; + std::cout << label << " traversability: " + << voxel.data.traversability << "\n"; +} +} // namespace + +int main() { + Coefficients::CoefficientsArray child_voxels{ + DigitalTwinVoxel{0.1f, DigitalTwinData{1.f, 0.f, 0.f, 0.1f}}, + DigitalTwinVoxel{0.2f, DigitalTwinData{0.f, 1.f, 0.f, 0.2f}}, + DigitalTwinVoxel{0.3f, DigitalTwinData{0.f, 0.f, 1.f, 0.3f}}, + DigitalTwinVoxel{0.4f, DigitalTwinData{1.f, 1.f, 0.f, 0.4f}}, + DigitalTwinVoxel{0.5f, DigitalTwinData{1.f, 0.f, 1.f, 0.5f}}, + DigitalTwinVoxel{0.6f, DigitalTwinData{0.f, 1.f, 1.f, 0.6f}}, + DigitalTwinVoxel{0.7f, DigitalTwinData{0.5f, 0.5f, 0.5f, 0.7f}}, + DigitalTwinVoxel{0.8f, DigitalTwinData{1.f, 1.f, 1.f, 0.8f}}, + }; + + const Coefficients::Parent parent_coefficients = + Transform::forward(child_voxels); + const Coefficients::CoefficientsArray reconstructed_children = + Transform::backward(parent_coefficients); + + printVoxel("Parent scale", parent_coefficients.scale); + printVoxel("First reconstructed child", reconstructed_children[0]); + printVoxel("Last reconstructed child", reconstructed_children[7]); + + return 0; +} diff --git a/examples/cpp/digital_twin/voxel_map_experiment.cc b/examples/cpp/digital_twin/voxel_map_experiment.cc new file mode 100644 index 000000000..28f5948e8 --- /dev/null +++ b/examples/cpp/digital_twin/voxel_map_experiment.cc @@ -0,0 +1,85 @@ +#include + +#include +#include + +namespace { +struct DigitalTwinData { + float r = 0.f; + float g = 0.f; + float b = 0.f; + float traversability = 0.f; + + bool operator==(const DigitalTwinData& other) const { + return r == other.r && g == other.g && b == other.b && + traversability == other.traversability; + } +}; + +struct DigitalTwinDataPolicy { + static DigitalTwinData add(const DigitalTwinData& lhs, const DigitalTwinData& rhs) { + return {lhs.r + rhs.r, lhs.g + rhs.g, lhs.b + rhs.b, + lhs.traversability + rhs.traversability}; + } + + static DigitalTwinData subtract(const DigitalTwinData& lhs, + const DigitalTwinData& rhs) { + return {lhs.r - rhs.r, lhs.g - rhs.g, lhs.b - rhs.b, + lhs.traversability - rhs.traversability}; + } + + static DigitalTwinData scale(const DigitalTwinData& data, + wavemap::FloatingPoint factor) { + return {factor * data.r, factor * data.g, factor * data.b, + factor * data.traversability}; + } +}; + +using DigitalTwinVoxel = wavemap::VoxelData; +using DigitalTwinMap = wavemap::HashedWaveletOctreeT; + +void printVoxel(const char* label, const DigitalTwinVoxel& voxel) { + std::cout << label << " occupancy: " << voxel.occupancy << "\n"; + std::cout << label << " color: " << voxel.data.r << " " << voxel.data.g + << " " << voxel.data.b << "\n"; + std::cout << label << " traversability: " + << voxel.data.traversability << "\n"; +} +} // namespace + +int main() { + wavemap::HashedWaveletOctreeConfig config; + config.min_cell_width = 0.1f; + config.min_log_odds = -2.f; + config.max_log_odds = 4.f; + config.tree_height = 3; + + DigitalTwinMap map(config); + + const wavemap::Index3D voxel_index(9, 2, 3); + const DigitalTwinVoxel initial_voxel(0.8f, DigitalTwinData{1.f, 0.f, 0.f, 0.75f}); + + map.setVoxelValue(voxel_index, initial_voxel); + printVoxel("Stored voxel", map.getVoxelValue(voxel_index)); + + map.addToCellValue(voxel_index, 0.2f); + printVoxel("After occupancy update", map.getVoxelValue(voxel_index)); + + const DigitalTwinVoxel voxel_update(0.1f, DigitalTwinData{0.f, 0.5f, 0.f, -0.25f}); + map.addToVoxelValue(voxel_index, voxel_update); + printVoxel("After full voxel update", map.getVoxelValue(voxel_index)); + + size_t visited_leaves = 0u; + map.forEachVoxelLeaf([&visited_leaves, &voxel_index](const wavemap::OctreeIndex& leaf_index, const DigitalTwinVoxel& voxel) { + ++visited_leaves; + if (leaf_index.height == 0 && leaf_index.position == voxel_index) { + printVoxel("Visited target leaf", voxel); + } + }); + + std::cout << "Visited leaves: " << visited_leaves << "\n"; + std::cout << "Occupancy view: " << map.getCellValue(voxel_index) << "\n"; + std::cout << "Block count: " << map.getHashMap().size() << "\n"; + + return 0; +} From ea0ee61db4a4b0cac4100c8b0250b5500b9c0c18 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Guilherne=20Caba=C3=A7o?= Date: Tue, 9 Jun 2026 15:28:48 +0000 Subject: [PATCH 04/14] Add layered voxel prototype with type map IO --- examples/cpp/digital_twin/CMakeLists.txt | 9 + .../digital_twin/layered_map_io_experiment.cc | 81 ++++++ .../cpp/digital_twin/layered_voxel_config.h | 68 +++++ .../digital_twin/legacy_file_io_experiment.cc | 51 ++++ examples/cpp/digital_twin/new_voxel.cc | 53 +--- .../digital_twin/voxel_block_experiment.cc | 61 +--- .../cpp/digital_twin/voxel_haar_transform.cc | 66 +---- .../cpp/digital_twin/voxel_map_experiment.cc | 59 +--- .../wavemap/core/map/cell_types/voxel_data.h | 242 ++++++++++++---- .../include/wavemap/core/map/hashed_blocks.h | 2 + .../core/map/hashed_chunked_wavelet_octree.h | 4 + .../wavemap/core/map/hashed_wavelet_octree.h | 32 ++- .../core/map/hashed_wavelet_octree_block.h | 51 ++-- .../impl/hashed_wavelet_octree_block_inl.h | 269 +++++++++++++++++- .../core/map/impl/hashed_wavelet_octree_inl.h | 183 ++++++++++-- .../cpp/include/wavemap/core/map/map_base.h | 7 +- .../wavemap/core/map/volumetric_octree.h | 2 + .../include/wavemap/core/map/wavelet_octree.h | 2 + .../cpp/include/wavemap/io/file_conversions.h | 7 + .../wavemap/io/impl/file_conversions_inl.h | 55 ++++ .../wavemap/io/impl/stream_conversions_inl.h | 153 ++++++++++ .../include/wavemap/io/stream_conversions.h | 32 +++ .../cpp/include/wavemap/io/streamable_types.h | 10 +- .../cpp/src/core/map/hashed_wavelet_octree.cc | 62 ---- .../core/map/hashed_wavelet_octree_block.cc | 195 ------------- library/cpp/src/io/stream_conversions.cc | 43 ++- 26 files changed, 1204 insertions(+), 595 deletions(-) create mode 100644 examples/cpp/digital_twin/layered_map_io_experiment.cc create mode 100644 examples/cpp/digital_twin/layered_voxel_config.h create mode 100644 examples/cpp/digital_twin/legacy_file_io_experiment.cc create mode 100644 library/cpp/include/wavemap/io/impl/file_conversions_inl.h create mode 100644 library/cpp/include/wavemap/io/impl/stream_conversions_inl.h diff --git a/examples/cpp/digital_twin/CMakeLists.txt b/examples/cpp/digital_twin/CMakeLists.txt index fc3631006..f2c584372 100644 --- a/examples/cpp/digital_twin/CMakeLists.txt +++ b/examples/cpp/digital_twin/CMakeLists.txt @@ -21,6 +21,15 @@ set_wavemap_target_properties(legacy_hashed_wavelet_octree_experiment) target_link_libraries(legacy_hashed_wavelet_octree_experiment PUBLIC wavemap::wavemap_core) +add_executable(legacy_file_io_experiment legacy_file_io_experiment.cc) +set_wavemap_target_properties(legacy_file_io_experiment) +target_link_libraries(legacy_file_io_experiment + PUBLIC wavemap::wavemap_core wavemap::wavemap_io) + +add_executable(layered_map_io_experiment layered_map_io_experiment.cc) +set_wavemap_target_properties(layered_map_io_experiment) +target_link_libraries(layered_map_io_experiment PUBLIC wavemap::wavemap_core) + if ("$ENV{ROS_VERSION}" STREQUAL "1") find_package(catkin REQUIRED COMPONENTS roscpp wavemap wavemap_msgs wavemap_ros_conversions) diff --git a/examples/cpp/digital_twin/layered_map_io_experiment.cc b/examples/cpp/digital_twin/layered_map_io_experiment.cc new file mode 100644 index 000000000..47b2badd4 --- /dev/null +++ b/examples/cpp/digital_twin/layered_map_io_experiment.cc @@ -0,0 +1,81 @@ +#include +#include +#include + +#include + +#include "layered_voxel_config.h" + +int main() { + wavemap::HashedWaveletOctreeConfig config; + config.min_cell_width = 0.1f; + config.min_log_odds = -2.f; + config.max_log_odds = 4.f; + config.tree_height = 3; + + LayeredMap map(config); + + const wavemap::Index3D cube_origin(8, 2, 2); + constexpr int cube_size_x = 2; + constexpr int cube_size_y = 2; + constexpr int cube_size_z = 4; + size_t populated_voxels = 0u; + std::vector populated_indices; + populated_indices.reserve(cube_size_x * cube_size_y * cube_size_z); + + // Populate a small cube of voxels with varying occupancy, color, and traversability values. + for (int dx = 0; dx < cube_size_x; ++dx) { + for (int dy = 0; dy < cube_size_y; ++dy) { + for (int dz = 0; dz < cube_size_z; ++dz) { + const wavemap::Index3D voxel_index = cube_origin + wavemap::Index3D(dx, dy, dz); + const wavemap::FloatingPoint layer_value = static_cast(populated_voxels); + const LayeredVoxel original_voxel(0.5f + 0.02f * layer_value, LayeredData{0.1f * dx, 0.1f * dy, 0.05f * dz, 0.9f - 0.03f * layer_value}); + const LayeredVoxel voxel_update(0.1f, LayeredData{0.01f, 0.02f, 0.03f, -0.01f}); + + map.setVoxelValue(voxel_index, original_voxel); + map.addToVoxelValue(voxel_index, voxel_update); + populated_indices.emplace_back(voxel_index); + ++populated_voxels; + } + } + } + + const wavemap::Index3D sample_voxel_index = cube_origin + wavemap::Index3D(1, 1, 2); + + const std::filesystem::path output_path = "/home/ci/data/maps/layered_map.wvmp"; + if (!wavemap::io::mapToFile(map, output_path)) { + std::cerr << "Failed to save layered map to: " << output_path << "\n"; + return 1; + } + + LayeredMap::Ptr loaded_map; + if (!wavemap::io::fileToMap(output_path, loaded_map)) { + std::cerr << "Failed to load layered map from: " << output_path << "\n"; + return 1; + } + + if (!loaded_map) { + std::cerr << "Loaded map pointer is null.\n"; + return 1; + } + + size_t verified_voxels = 0u; + for (const wavemap::Index3D& voxel_index : populated_indices) { + const LayeredVoxel original_voxel = map.getVoxelValue(voxel_index); + const LayeredVoxel loaded_voxel = loaded_map->getVoxelValue(voxel_index); + if (original_voxel == loaded_voxel) { + ++verified_voxels; + } else { + std::cerr << "Layered voxel mismatch at index " << voxel_index.transpose() << "\n"; + printVoxel("Original", original_voxel); + printVoxel("Loaded", loaded_voxel); + } + } + + printVoxel("Original sample voxel", map.getVoxelValue(sample_voxel_index)); + printVoxel("Loaded sample voxel", loaded_map->getVoxelValue(sample_voxel_index)); + std::cout << "Verified layered voxels: " << verified_voxels << " / " << populated_indices.size() << "\n"; + std::cout << "Saved layered map to: " << output_path << "\n"; + + return verified_voxels == populated_indices.size() ? 0 : 1; +} diff --git a/examples/cpp/digital_twin/layered_voxel_config.h b/examples/cpp/digital_twin/layered_voxel_config.h new file mode 100644 index 000000000..2f309f09c --- /dev/null +++ b/examples/cpp/digital_twin/layered_voxel_config.h @@ -0,0 +1,68 @@ +#ifndef WAVEMAP_EXAMPLES_CPP_DIGITAL_TWIN_LAYERED_VOXEL_CONFIG_H_ +#define WAVEMAP_EXAMPLES_CPP_DIGITAL_TWIN_LAYERED_VOXEL_CONFIG_H_ + +#include +#include +#include + +#include +#include +#include +#include + +struct LayeredData { + float r = 0.f; + float g = 0.f; + float b = 0.f; + float traversability = 0.f; + + bool operator==(const LayeredData& other) const { + return r == other.r && g == other.g && b == other.b && traversability == other.traversability; + } +}; + +struct LayeredDataPolicy { + static LayeredData add(const LayeredData& lhs, const LayeredData& rhs) { + return {lhs.r + rhs.r, lhs.g + rhs.g, lhs.b + rhs.b, lhs.traversability + rhs.traversability}; + } + + static LayeredData subtract(const LayeredData& lhs, const LayeredData& rhs) { + return {lhs.r - rhs.r, lhs.g - rhs.g, lhs.b - rhs.b, lhs.traversability - rhs.traversability}; + } + + static LayeredData scale(const LayeredData& data, wavemap::FloatingPoint factor) { + return {factor * data.r, factor * data.g, factor * data.b, factor * data.traversability}; + } +}; + +using LayeredVoxel = wavemap::VoxelData; +using LayeredBlock = wavemap::HashedWaveletOctreeBlockT; +using LayeredMap = wavemap::HashedWaveletOctreeT; + +struct LayeredVoxelSerializer { + static void write(std::ostream& ostream, const LayeredVoxel& voxel) { + wavemap::io::StreamableCellData::write(ostream, voxel.occupancy); + wavemap::io::StreamableCellData::write(ostream, voxel.data.r); + wavemap::io::StreamableCellData::write(ostream, voxel.data.g); + wavemap::io::StreamableCellData::write(ostream, voxel.data.b); + wavemap::io::StreamableCellData::write(ostream, voxel.data.traversability); + } + + static LayeredVoxel read(std::istream& istream) { + LayeredVoxel voxel; + voxel.occupancy = wavemap::io::StreamableCellData::read(istream); + voxel.data.r = wavemap::io::StreamableCellData::read(istream); + voxel.data.g = wavemap::io::StreamableCellData::read(istream); + voxel.data.b = wavemap::io::StreamableCellData::read(istream); + voxel.data.traversability = wavemap::io::StreamableCellData::read(istream); + return voxel; + } +}; + +inline void printVoxel(const char* label, const LayeredVoxel& voxel) { + std::cout << label << " occupancy: " << voxel.occupancy << "\n"; + std::cout << label << " color: " << voxel.data.r << " " << voxel.data.g << " " << voxel.data.b << "\n"; + std::cout << label << " traversability: " << voxel.data.traversability << "\n"; +} + +#endif // WAVEMAP_EXAMPLES_CPP_DIGITAL_TWIN_LAYERED_VOXEL_CONFIG_H_ diff --git a/examples/cpp/digital_twin/legacy_file_io_experiment.cc b/examples/cpp/digital_twin/legacy_file_io_experiment.cc new file mode 100644 index 000000000..846ebe962 --- /dev/null +++ b/examples/cpp/digital_twin/legacy_file_io_experiment.cc @@ -0,0 +1,51 @@ +#include +#include +#include + +#include +#include +#include + +int main() { + wavemap::HashedWaveletOctreeConfig config; + config.min_cell_width = 0.1f; + config.min_log_odds = -2.f; + config.max_log_odds = 4.f; + config.tree_height = 3; + + wavemap::HashedWaveletOctree map(config); + + const wavemap::Index3D voxel_index(9, 2, 3); + map.setCellValue(voxel_index, 0.8f); + map.addToCellValue(voxel_index, 0.2f); + + const std::filesystem::path output_path = + "/home/ci/data/maps/legacy_hashed_wavelet_octree.wvmp"; + if (!wavemap::io::mapToFile(map, output_path)) { + std::cerr << "Failed to save legacy map to: " << output_path << "\n"; + return 1; + } + + wavemap::MapBase::Ptr loaded_map; + if (!wavemap::io::fileToMap(output_path, loaded_map)) { + std::cerr << "Failed to load legacy map from: " << output_path << "\n"; + return 1; + } + + auto loaded_hashed_map = + std::dynamic_pointer_cast(loaded_map); + if (!loaded_hashed_map) { + std::cerr << "Loaded map is not a HashedWaveletOctree.\n"; + return 1; + } + + const wavemap::FloatingPoint original_value = map.getCellValue(voxel_index); + const wavemap::FloatingPoint loaded_value = + loaded_hashed_map->getCellValue(voxel_index); + + std::cout << "Original occupancy: " << original_value << "\n"; + std::cout << "Loaded occupancy: " << loaded_value << "\n"; + std::cout << "Saved legacy map to: " << output_path << "\n"; + + return original_value == loaded_value ? 0 : 1; +} diff --git a/examples/cpp/digital_twin/new_voxel.cc b/examples/cpp/digital_twin/new_voxel.cc index 5dff14c17..0c0dd563e 100644 --- a/examples/cpp/digital_twin/new_voxel.cc +++ b/examples/cpp/digital_twin/new_voxel.cc @@ -2,44 +2,7 @@ #include -namespace { -struct DigitalTwinData { - float r = 0.f; - float g = 0.f; - float b = 0.f; - float traversability = 0.f; - - bool operator==(const DigitalTwinData& other) const { - return r == other.r && g == other.g && b == other.b && - traversability == other.traversability; - } -}; - -struct DigitalTwinDataPolicy { - static DigitalTwinData add(const DigitalTwinData& lhs, const DigitalTwinData& rhs) { - return {lhs.r + rhs.r, lhs.g + rhs.g, lhs.b + rhs.b, - lhs.traversability + rhs.traversability}; - } - - static DigitalTwinData subtract(const DigitalTwinData& lhs, - const DigitalTwinData& rhs) { - return {lhs.r - rhs.r, lhs.g - rhs.g, lhs.b - rhs.b, - lhs.traversability - rhs.traversability}; - } - - static DigitalTwinData scale(const DigitalTwinData& data, - wavemap::FloatingPoint factor) { - return {factor * data.r, factor * data.g, factor * data.b, - factor * data.traversability}; - } -}; - -void printVoxel(const char* label, const wavemap::VoxelData& voxel) { - std::cout << label << " occupancy: " << voxel.occupancy << "\n"; - std::cout << label << " color: " << voxel.data.r << " " << voxel.data.g << " " << voxel.data.b << "\n"; - std::cout << label << " traversability: " << voxel.data.traversability << "\n"; -} -} // namespace +#include "layered_voxel_config.h" int main() { wavemap::VoxelData<> occupancy_only; @@ -48,17 +11,15 @@ int main() { std::cout << "Occupancy only: " << occupancy_only.occupancy << "\n"; - using DigitalTwinVoxel = wavemap::VoxelData; - - DigitalTwinVoxel digital_twin_voxel(0.8f, DigitalTwinData{1.f, 0.f, 0.f, 0.75f}); - digital_twin_voxel += 0.2f; - printVoxel("Digital twin", digital_twin_voxel); + LayeredVoxel layered_voxel(0.8f, LayeredData{1.f, 0.f, 0.f, 0.75f}); + layered_voxel += 0.2f; + printVoxel("Layered voxel", layered_voxel); - wavemap::FloatingPoint occupancy_value = digital_twin_voxel; + wavemap::FloatingPoint occupancy_value = layered_voxel; std::cout << "Implicit occupancy value: " << occupancy_value << "\n"; - const DigitalTwinVoxel voxel_a(0.4f, DigitalTwinData{1.f, 0.2f, 0.f, 0.3f}); - const DigitalTwinVoxel voxel_b(0.6f, DigitalTwinData{0.f, 0.3f, 1.f, 0.5f}); + const LayeredVoxel voxel_a(0.4f, LayeredData{1.f, 0.2f, 0.f, 0.3f}); + const LayeredVoxel voxel_b(0.6f, LayeredData{0.f, 0.3f, 1.f, 0.5f}); printVoxel("Sum", voxel_a + voxel_b); printVoxel("Difference", voxel_b - voxel_a); diff --git a/examples/cpp/digital_twin/voxel_block_experiment.cc b/examples/cpp/digital_twin/voxel_block_experiment.cc index a1fde770c..bea945681 100644 --- a/examples/cpp/digital_twin/voxel_block_experiment.cc +++ b/examples/cpp/digital_twin/voxel_block_experiment.cc @@ -1,62 +1,17 @@ #include -#include -#include - -namespace { -struct DigitalTwinData { - float r = 0.f; - float g = 0.f; - float b = 0.f; - float traversability = 0.f; - - bool operator==(const DigitalTwinData& other) const { - return r == other.r && g == other.g && b == other.b && - traversability == other.traversability; - } -}; - -struct DigitalTwinDataPolicy { - static DigitalTwinData add(const DigitalTwinData& lhs, - const DigitalTwinData& rhs) { - return {lhs.r + rhs.r, lhs.g + rhs.g, lhs.b + rhs.b, - lhs.traversability + rhs.traversability}; - } - - static DigitalTwinData subtract(const DigitalTwinData& lhs, - const DigitalTwinData& rhs) { - return {lhs.r - rhs.r, lhs.g - rhs.g, lhs.b - rhs.b, - lhs.traversability - rhs.traversability}; - } - - static DigitalTwinData scale(const DigitalTwinData& data, - wavemap::FloatingPoint factor) { - return {factor * data.r, factor * data.g, factor * data.b, - factor * data.traversability}; - } -}; - -using DigitalTwinVoxel = wavemap::VoxelData; -using DigitalTwinBlock = wavemap::HashedWaveletOctreeBlockT; - -void printVoxel(const char* label, const DigitalTwinVoxel& voxel) { - std::cout << label << " occupancy: " << voxel.occupancy << "\n"; - std::cout << label << " color: " << voxel.data.r << " " << voxel.data.g - << " " << voxel.data.b << "\n"; - std::cout << label << " traversability: " - << voxel.data.traversability << "\n"; -} -} // namespace +#include "layered_voxel_config.h" int main() { constexpr wavemap::IndexElement tree_height = 3; constexpr wavemap::FloatingPoint min_log_odds = -2.f; constexpr wavemap::FloatingPoint max_log_odds = 4.f; - DigitalTwinBlock block(tree_height, min_log_odds, max_log_odds); + LayeredBlock block(tree_height, min_log_odds, max_log_odds); const wavemap::OctreeIndex voxel_index{0, wavemap::Index3D{1, 2, 3}}; - const DigitalTwinVoxel initial_voxel(0.8f, DigitalTwinData{1.f, 0.f, 0.f, 0.75f}); + const LayeredVoxel initial_voxel( + 0.8f, LayeredData{1.f, 0.f, 0.f, 0.75f}); block.setVoxelValue(voxel_index, initial_voxel); printVoxel("Stored voxel", block.getVoxelValue(voxel_index)); @@ -64,7 +19,8 @@ int main() { block.addToCellValue(voxel_index, 0.2f); printVoxel("After occupancy update", block.getVoxelValue(voxel_index)); - const DigitalTwinVoxel voxel_update(0.1f, DigitalTwinData{0.f, 0.5f, 0.f, -0.25f}); + const LayeredVoxel voxel_update( + 0.1f, LayeredData{0.f, 0.5f, 0.f, -0.25f}); block.addToVoxelValue(voxel_index, voxel_update); printVoxel("After full voxel update", block.getVoxelValue(voxel_index)); @@ -72,9 +28,10 @@ int main() { block.forEachVoxelLeaf( wavemap::Index3D::Zero(), [&visited_leaves](const wavemap::OctreeIndex& leaf_index, - const DigitalTwinVoxel& voxel) { + const LayeredVoxel& voxel) { ++visited_leaves; - if (leaf_index.height == 0 && leaf_index.position == wavemap::Index3D{1, 2, 3}) { + if (leaf_index.height == 0 && + leaf_index.position == wavemap::Index3D{1, 2, 3}) { printVoxel("Visited target leaf", voxel); } }); diff --git a/examples/cpp/digital_twin/voxel_haar_transform.cc b/examples/cpp/digital_twin/voxel_haar_transform.cc index e09e007e3..7cd38a546 100644 --- a/examples/cpp/digital_twin/voxel_haar_transform.cc +++ b/examples/cpp/digital_twin/voxel_haar_transform.cc @@ -1,67 +1,25 @@ #include -#include #include #include -#include -namespace { -struct DigitalTwinData { - float r = 0.f; - float g = 0.f; - float b = 0.f; - float traversability = 0.f; - - bool operator==(const DigitalTwinData& other) const { - return r == other.r && g == other.g && b == other.b && - traversability == other.traversability; - } -}; - -struct DigitalTwinDataPolicy { - static DigitalTwinData add(const DigitalTwinData& lhs, - const DigitalTwinData& rhs) { - return {lhs.r + rhs.r, lhs.g + rhs.g, lhs.b + rhs.b, - lhs.traversability + rhs.traversability}; - } - - static DigitalTwinData subtract(const DigitalTwinData& lhs, - const DigitalTwinData& rhs) { - return {lhs.r - rhs.r, lhs.g - rhs.g, lhs.b - rhs.b, - lhs.traversability - rhs.traversability}; - } +#include "layered_voxel_config.h" - static DigitalTwinData scale(const DigitalTwinData& data, - wavemap::FloatingPoint factor) { - return {factor * data.r, factor * data.g, factor * data.b, - factor * data.traversability}; - } -}; - -using DigitalTwinVoxel = - wavemap::VoxelData; -using Coefficients = wavemap::HaarCoefficients; -using Transform = wavemap::HaarTransform; - -void printVoxel(const char* label, const DigitalTwinVoxel& voxel) { - std::cout << label << " occupancy: " << voxel.occupancy << "\n"; - std::cout << label << " color: " << voxel.data.r << " " << voxel.data.g - << " " << voxel.data.b << "\n"; - std::cout << label << " traversability: " - << voxel.data.traversability << "\n"; -} +namespace { +using Coefficients = wavemap::HaarCoefficients; +using Transform = wavemap::HaarTransform; } // namespace int main() { Coefficients::CoefficientsArray child_voxels{ - DigitalTwinVoxel{0.1f, DigitalTwinData{1.f, 0.f, 0.f, 0.1f}}, - DigitalTwinVoxel{0.2f, DigitalTwinData{0.f, 1.f, 0.f, 0.2f}}, - DigitalTwinVoxel{0.3f, DigitalTwinData{0.f, 0.f, 1.f, 0.3f}}, - DigitalTwinVoxel{0.4f, DigitalTwinData{1.f, 1.f, 0.f, 0.4f}}, - DigitalTwinVoxel{0.5f, DigitalTwinData{1.f, 0.f, 1.f, 0.5f}}, - DigitalTwinVoxel{0.6f, DigitalTwinData{0.f, 1.f, 1.f, 0.6f}}, - DigitalTwinVoxel{0.7f, DigitalTwinData{0.5f, 0.5f, 0.5f, 0.7f}}, - DigitalTwinVoxel{0.8f, DigitalTwinData{1.f, 1.f, 1.f, 0.8f}}, + LayeredVoxel{0.1f, LayeredData{1.f, 0.f, 0.f, 0.1f}}, + LayeredVoxel{0.2f, LayeredData{0.f, 1.f, 0.f, 0.2f}}, + LayeredVoxel{0.3f, LayeredData{0.f, 0.f, 1.f, 0.3f}}, + LayeredVoxel{0.4f, LayeredData{1.f, 1.f, 0.f, 0.4f}}, + LayeredVoxel{0.5f, LayeredData{1.f, 0.f, 1.f, 0.5f}}, + LayeredVoxel{0.6f, LayeredData{0.f, 1.f, 1.f, 0.6f}}, + LayeredVoxel{0.7f, LayeredData{0.5f, 0.5f, 0.5f, 0.7f}}, + LayeredVoxel{0.8f, LayeredData{1.f, 1.f, 1.f, 0.8f}}, }; const Coefficients::Parent parent_coefficients = diff --git a/examples/cpp/digital_twin/voxel_map_experiment.cc b/examples/cpp/digital_twin/voxel_map_experiment.cc index 28f5948e8..e886aa3f4 100644 --- a/examples/cpp/digital_twin/voxel_map_experiment.cc +++ b/examples/cpp/digital_twin/voxel_map_experiment.cc @@ -1,51 +1,6 @@ #include -#include -#include - -namespace { -struct DigitalTwinData { - float r = 0.f; - float g = 0.f; - float b = 0.f; - float traversability = 0.f; - - bool operator==(const DigitalTwinData& other) const { - return r == other.r && g == other.g && b == other.b && - traversability == other.traversability; - } -}; - -struct DigitalTwinDataPolicy { - static DigitalTwinData add(const DigitalTwinData& lhs, const DigitalTwinData& rhs) { - return {lhs.r + rhs.r, lhs.g + rhs.g, lhs.b + rhs.b, - lhs.traversability + rhs.traversability}; - } - - static DigitalTwinData subtract(const DigitalTwinData& lhs, - const DigitalTwinData& rhs) { - return {lhs.r - rhs.r, lhs.g - rhs.g, lhs.b - rhs.b, - lhs.traversability - rhs.traversability}; - } - - static DigitalTwinData scale(const DigitalTwinData& data, - wavemap::FloatingPoint factor) { - return {factor * data.r, factor * data.g, factor * data.b, - factor * data.traversability}; - } -}; - -using DigitalTwinVoxel = wavemap::VoxelData; -using DigitalTwinMap = wavemap::HashedWaveletOctreeT; - -void printVoxel(const char* label, const DigitalTwinVoxel& voxel) { - std::cout << label << " occupancy: " << voxel.occupancy << "\n"; - std::cout << label << " color: " << voxel.data.r << " " << voxel.data.g - << " " << voxel.data.b << "\n"; - std::cout << label << " traversability: " - << voxel.data.traversability << "\n"; -} -} // namespace +#include "layered_voxel_config.h" int main() { wavemap::HashedWaveletOctreeConfig config; @@ -54,10 +9,11 @@ int main() { config.max_log_odds = 4.f; config.tree_height = 3; - DigitalTwinMap map(config); + LayeredMap map(config); const wavemap::Index3D voxel_index(9, 2, 3); - const DigitalTwinVoxel initial_voxel(0.8f, DigitalTwinData{1.f, 0.f, 0.f, 0.75f}); + const LayeredVoxel initial_voxel( + 0.8f, LayeredData{1.f, 0.f, 0.f, 0.75f}); map.setVoxelValue(voxel_index, initial_voxel); printVoxel("Stored voxel", map.getVoxelValue(voxel_index)); @@ -65,12 +21,15 @@ int main() { map.addToCellValue(voxel_index, 0.2f); printVoxel("After occupancy update", map.getVoxelValue(voxel_index)); - const DigitalTwinVoxel voxel_update(0.1f, DigitalTwinData{0.f, 0.5f, 0.f, -0.25f}); + const LayeredVoxel voxel_update( + 0.1f, LayeredData{0.f, 0.5f, 0.f, -0.25f}); map.addToVoxelValue(voxel_index, voxel_update); printVoxel("After full voxel update", map.getVoxelValue(voxel_index)); size_t visited_leaves = 0u; - map.forEachVoxelLeaf([&visited_leaves, &voxel_index](const wavemap::OctreeIndex& leaf_index, const DigitalTwinVoxel& voxel) { + map.forEachVoxelLeaf( + [&visited_leaves, &voxel_index](const wavemap::OctreeIndex& leaf_index, + const LayeredVoxel& voxel) { ++visited_leaves; if (leaf_index.height == 0 && leaf_index.position == voxel_index) { printVoxel("Visited target leaf", voxel); diff --git a/library/cpp/include/wavemap/core/map/cell_types/voxel_data.h b/library/cpp/include/wavemap/core/map/cell_types/voxel_data.h index a3283839e..5aacbe662 100644 --- a/library/cpp/include/wavemap/core/map/cell_types/voxel_data.h +++ b/library/cpp/include/wavemap/core/map/cell_types/voxel_data.h @@ -1,127 +1,259 @@ -#ifndef WAVEMAP_CORE_DATA_STRUCTURE_VOXEL_DATA_H_ -#define WAVEMAP_CORE_DATA_STRUCTURE_VOXEL_DATA_H_ +#ifndef WAVEMAP_CORE_MAP_CELL_TYPES_VOXEL_DATA_H_ +#define WAVEMAP_CORE_MAP_CELL_TYPES_VOXEL_DATA_H_ #include "wavemap/core/common.h" namespace wavemap { +template +struct DefaultVoxelDataPolicy { + static AdditionalDataT add(const AdditionalDataT& lhs, + const AdditionalDataT& rhs) { + return lhs + rhs; + } + + static AdditionalDataT subtract(const AdditionalDataT& lhs, + const AdditionalDataT& rhs) { + return lhs - rhs; + } + + static AdditionalDataT scale(const AdditionalDataT& data, + FloatingPoint factor) { + return data * factor; + } +}; + +template <> +struct DefaultVoxelDataPolicy {}; + /** - * @brief A template structure for storing voxel data with mandatory occupancy - * and arbitrary additional data. - * - * This template allows users to store custom data alongside occupancy - * information in each voxel without compromising efficiency. - * - * @tparam AdditionalDataT The type of additional data to store in each voxel. - * Use void (default) for occupancy-only storage. - * - * Example usage: - * @code - * // Voxel with occupancy only (same as FloatingPoint) - * using BasicVoxel = VoxelData; + * @brief Voxel data with mandatory occupancy and optional user data. * - * // Voxel with occupancy + color - * struct ColorData { - * uint8_t r, g, b; - * }; - * using ColorVoxel = VoxelData; - * - * // Voxel with occupancy + semantic label - * using SemanticVoxel = VoxelData; - * @endcode + * AdditionalDataPolicyT defines the arithmetic of the additional data when this + * type is used in numerical algorithms such as wavelet transforms. */ -template +template > struct VoxelData { + using AdditionalData = AdditionalDataT; + using AdditionalDataPolicy = AdditionalDataPolicyT; + FloatingPoint occupancy = 0.f; AdditionalDataT data{}; - // Default constructor VoxelData() = default; - - // Constructor from occupancy only explicit VoxelData(FloatingPoint occupancy_value) : occupancy(occupancy_value) {} - - // Constructor from occupancy and additional data VoxelData(FloatingPoint occupancy_value, const AdditionalDataT& data_value) : occupancy(occupancy_value), data(data_value) {} - // Implicit conversion to FloatingPoint for backward compatibility - operator FloatingPoint() const { return occupancy; } + operator FloatingPoint() const { return occupancy; } // NOLINT - // Assignment from FloatingPoint VoxelData& operator=(FloatingPoint value) { occupancy = value; return *this; } - // Equality operators bool operator==(const VoxelData& other) const { return occupancy == other.occupancy && data == other.data; } - bool operator!=(const VoxelData& other) const { return !(*this == other); } - // Allow use with += operator for occupancy updates VoxelData& operator+=(FloatingPoint value) { occupancy += value; return *this; } - // Addition operator + VoxelData& operator+=(const VoxelData& other) { + occupancy += other.occupancy; + data = AdditionalDataPolicyT::add(data, other.data); + return *this; + } + + VoxelData& operator-=(const VoxelData& other) { + occupancy -= other.occupancy; + data = AdditionalDataPolicyT::subtract(data, other.data); + return *this; + } + + VoxelData& operator*=(FloatingPoint factor) { + occupancy *= factor; + data = AdditionalDataPolicyT::scale(data, factor); + return *this; + } + + VoxelData& operator/=(FloatingPoint factor) { + occupancy /= factor; + data = AdditionalDataPolicyT::scale(data, 1.f / factor); + return *this; + } + + VoxelData& operator*=(const VoxelData& scalar) { + return *this *= scalar.occupancy; + } + + VoxelData& operator/=(const VoxelData& scalar) { + return *this /= scalar.occupancy; + } + VoxelData operator+(FloatingPoint value) const { VoxelData result = *this; result.occupancy += value; return result; } + + friend VoxelData operator+(VoxelData lhs, const VoxelData& rhs) { + lhs += rhs; + return lhs; + } + + friend VoxelData operator-(VoxelData lhs, const VoxelData& rhs) { + lhs -= rhs; + return lhs; + } + + friend VoxelData operator+(VoxelData value) { return value; } + + friend VoxelData operator-(VoxelData value) { + value.occupancy = -value.occupancy; + value.data = AdditionalDataPolicyT::scale(value.data, -1.f); + return value; + } + + friend VoxelData operator*(FloatingPoint factor, VoxelData value) { + value *= factor; + return value; + } + + friend VoxelData operator*(VoxelData value, FloatingPoint factor) { + value *= factor; + return value; + } + + // Supports generic numeric code that represents scalars as ValueT objects, + // e.g. static_cast(0.5f) * voxel in Haar transforms. + friend VoxelData operator*(const VoxelData& scalar, VoxelData value) { + value *= scalar.occupancy; + return value; + } + + friend VoxelData operator/(VoxelData value, FloatingPoint factor) { + value /= factor; + return value; + } + + friend VoxelData operator/(VoxelData value, const VoxelData& scalar) { + value /= scalar.occupancy; + return value; + } }; /** - * @brief Specialization for void additional data (occupancy-only). - * - * This specialization provides the same interface as the general template - * but stores only the occupancy value, making it fully compatible with - * FloatingPoint while still allowing future extension. + * @brief Occupancy-only specialization. */ -template <> -struct VoxelData { +template +struct VoxelData { FloatingPoint occupancy = 0.f; - // Default constructor VoxelData() = default; - - // Constructor from occupancy explicit VoxelData(FloatingPoint occupancy_value) : occupancy(occupancy_value) {} - // Implicit conversion to FloatingPoint - operator FloatingPoint() const { return occupancy; } + operator FloatingPoint() const { return occupancy; } // NOLINT - // Assignment from FloatingPoint VoxelData& operator=(FloatingPoint value) { occupancy = value; return *this; } - // Equality operators bool operator==(const VoxelData& other) const { return occupancy == other.occupancy; } - bool operator!=(const VoxelData& other) const { return !(*this == other); } - // Allow use with += operator VoxelData& operator+=(FloatingPoint value) { occupancy += value; return *this; } - // Addition operator + VoxelData& operator+=(const VoxelData& other) { + occupancy += other.occupancy; + return *this; + } + + VoxelData& operator-=(const VoxelData& other) { + occupancy -= other.occupancy; + return *this; + } + + VoxelData& operator*=(FloatingPoint factor) { + occupancy *= factor; + return *this; + } + + VoxelData& operator/=(FloatingPoint factor) { + occupancy /= factor; + return *this; + } + + VoxelData& operator*=(const VoxelData& scalar) { + occupancy *= scalar.occupancy; + return *this; + } + + VoxelData& operator/=(const VoxelData& scalar) { + occupancy /= scalar.occupancy; + return *this; + } + VoxelData operator+(FloatingPoint value) const { VoxelData result = *this; result.occupancy += value; return result; } + + friend VoxelData operator+(VoxelData lhs, const VoxelData& rhs) { + lhs += rhs; + return lhs; + } + + friend VoxelData operator-(VoxelData lhs, const VoxelData& rhs) { + lhs -= rhs; + return lhs; + } + + friend VoxelData operator+(VoxelData value) { return value; } + + friend VoxelData operator-(VoxelData value) { + value.occupancy = -value.occupancy; + return value; + } + + friend VoxelData operator*(FloatingPoint factor, VoxelData value) { + value *= factor; + return value; + } + + friend VoxelData operator*(VoxelData value, FloatingPoint factor) { + value *= factor; + return value; + } + + friend VoxelData operator*(const VoxelData& lhs, VoxelData rhs) { + rhs *= lhs.occupancy; + return rhs; + } + + friend VoxelData operator/(VoxelData value, FloatingPoint factor) { + value /= factor; + return value; + } + + friend VoxelData operator/(VoxelData value, const VoxelData& scalar) { + value /= scalar.occupancy; + return value; + } }; } // namespace wavemap -#endif // WAVEMAP_CORE_DATA_STRUCTURE_VOXEL_DATA_H_ \ No newline at end of file +#endif // WAVEMAP_CORE_MAP_CELL_TYPES_VOXEL_DATA_H_ diff --git a/library/cpp/include/wavemap/core/map/hashed_blocks.h b/library/cpp/include/wavemap/core/map/hashed_blocks.h index 703badbb1..3c92339f2 100644 --- a/library/cpp/include/wavemap/core/map/hashed_blocks.h +++ b/library/cpp/include/wavemap/core/map/hashed_blocks.h @@ -25,6 +25,8 @@ class HashedBlocks : public MapBase, FloatingPoint default_value = 0.f) : MapBase(config.checkValid()), DenseBlockHash(default_value) {} + MapType getMapType() const override { return MapType::kHashedBlocks; } + bool empty() const override { return DenseBlockHash::empty(); } size_t size() const override { return DenseBlockHash::size(); } void threshold() 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..a733fc0f7 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 @@ -79,6 +79,10 @@ class HashedChunkedWaveletOctree : public MapBase { // Copy construction is not supported HashedChunkedWaveletOctree(const HashedChunkedWaveletOctree&) = delete; + MapType getMapType() const override { + return MapType::kHashedChunkedWaveletOctree; + } + bool empty() const override { return block_map_.empty(); } size_t size() const override; void threshold() override; 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..099ecc692 100644 --- a/library/cpp/include/wavemap/core/map/hashed_wavelet_octree.h +++ b/library/cpp/include/wavemap/core/map/hashed_wavelet_octree.h @@ -2,6 +2,7 @@ #define WAVEMAP_CORE_MAP_HASHED_WAVELET_OCTREE_H_ #include +#include #include #include "wavemap/core/common.h" @@ -56,23 +57,33 @@ struct HashedWaveletOctreeConfig : ConfigBase { bool isValid(bool verbose) const override; }; -class HashedWaveletOctree : public MapBase { +template +class HashedWaveletOctreeT : public MapBase { public: - using Ptr = std::shared_ptr; - using ConstPtr = std::shared_ptr; + using Ptr = std::shared_ptr>; + using ConstPtr = std::shared_ptr>; using Config = HashedWaveletOctreeConfig; + using CellDataType = CellDataT; static constexpr bool kRequiresExplicitThresholding = true; using BlockIndex = Index3D; using CellIndex = OctreeIndex; - using Block = HashedWaveletOctreeBlock; + using Block = HashedWaveletOctreeBlockT; using BlockHashMap = SpatialHash; - explicit HashedWaveletOctree(const HashedWaveletOctreeConfig& config) + explicit HashedWaveletOctreeT(const HashedWaveletOctreeConfig& config) : MapBase(config), config_(config.checkValid()) {} // Copy construction is not supported - HashedWaveletOctree(const HashedWaveletOctree&) = delete; + HashedWaveletOctreeT(const HashedWaveletOctreeT&) = delete; + + MapType getMapType() const override { + if constexpr (std::is_same_v) { + return MapType::kHashedWaveletOctree; + } else { + return MapType::kLayeredHashedWaveletOctree; + } + } bool empty() const override { return block_map_.empty(); } size_t size() const override; @@ -95,8 +106,12 @@ class HashedWaveletOctree : public MapBase { FloatingPoint getCellValue(const Index3D& index) const override; FloatingPoint getCellValue(const OctreeIndex& index) const; + CellDataT getVoxelValue(const Index3D& index) const; + CellDataT getVoxelValue(const OctreeIndex& index) const; void setCellValue(const Index3D& index, FloatingPoint new_value) override; + void setVoxelValue(const Index3D& index, const CellDataT& new_value); void addToCellValue(const Index3D& index, FloatingPoint update) override; + void addToVoxelValue(const Index3D& index, const CellDataT& update); bool hasBlock(const Index3D& block_index) const; bool eraseBlock(const BlockIndex& block_index); @@ -118,6 +133,9 @@ class HashedWaveletOctree : public MapBase { void forEachLeaf( typename MapBase::IndexedLeafVisitorFunction visitor_fn) const override; + template + void forEachVoxelLeaf(IndexedVoxelLeafVisitorFunction visitor_fn) const; + BlockIndex indexToBlockIndex(const OctreeIndex& node_index) const; CellIndex indexToCellIndex(OctreeIndex index) const; @@ -128,6 +146,8 @@ class HashedWaveletOctree : public MapBase { BlockHashMap block_map_; }; + +using HashedWaveletOctree = HashedWaveletOctreeT; } // namespace wavemap #include "wavemap/core/map/impl/hashed_wavelet_octree_inl.h" diff --git a/library/cpp/include/wavemap/core/map/hashed_wavelet_octree_block.h b/library/cpp/include/wavemap/core/map/hashed_wavelet_octree_block.h index b622b9372..f413b22dc 100644 --- a/library/cpp/include/wavemap/core/map/hashed_wavelet_octree_block.h +++ b/library/cpp/include/wavemap/core/map/hashed_wavelet_octree_block.h @@ -5,21 +5,24 @@ #include "wavemap/core/data_structure/ndtree/ndtree.h" #include "wavemap/core/map/cell_types/haar_coefficients.h" #include "wavemap/core/map/cell_types/haar_transform.h" +#include "wavemap/core/map/cell_types/voxel_data.h" #include "wavemap/core/map/map_base.h" #include "wavemap/core/utils/time/time.h" namespace wavemap { -class HashedWaveletOctreeBlock { +template +class HashedWaveletOctreeBlockT { public: static constexpr int kDim = 3; + using CellDataType = CellDataT; using BlockIndex = Index3D; - using Coefficients = HaarCoefficients; - using Transform = HaarTransform; - using OctreeType = Octree; + using Coefficients = HaarCoefficients; + using Transform = HaarTransform; + using OctreeType = Octree; - explicit HashedWaveletOctreeBlock(IndexElement tree_height, - FloatingPoint min_log_odds, - FloatingPoint max_log_odds) + explicit HashedWaveletOctreeBlockT(IndexElement tree_height, + FloatingPoint min_log_odds, + FloatingPoint max_log_odds) : tree_height_(tree_height), min_log_odds_(min_log_odds), max_log_odds_(max_log_odds) {} @@ -31,19 +34,31 @@ class HashedWaveletOctreeBlock { void clear(); FloatingPoint getCellValue(const OctreeIndex& index) const; + CellDataT getVoxelValue(const OctreeIndex& index) const; void setCellValue(const OctreeIndex& index, FloatingPoint new_value); + void setVoxelValue(const OctreeIndex& index, const CellDataT& new_value); void addToCellValue(const OctreeIndex& index, FloatingPoint update); + void addToVoxelValue(const OctreeIndex& index, const CellDataT& update); void forEachLeaf(const BlockIndex& block_index, typename MapBase::IndexedLeafVisitorFunction visitor_fn, IndexElement termination_height = 0) const; - Coefficients::Scale& getRootScale() { return root_scale_coefficient_; } - const Coefficients::Scale& getRootScale() const { + template + void forEachVoxelLeaf(const BlockIndex& block_index, + IndexedVoxelLeafVisitorFunction visitor_fn, + IndexElement termination_height = 0) const; + + typename Coefficients::Scale& getRootScale() { + return root_scale_coefficient_; + } + const typename Coefficients::Scale& getRootScale() const { return root_scale_coefficient_; } - OctreeType::NodeRefType getRootNode() { return ndtree_.getRootNode(); } - OctreeType::NodeConstRefType getRootNode() const { + typename OctreeType::NodeRefType getRootNode() { + return ndtree_.getRootNode(); + } + typename OctreeType::NodeConstRefType getRootNode() const { return ndtree_.getRootNode(); } @@ -59,11 +74,11 @@ class HashedWaveletOctreeBlock { template auto getNodeIterator() { - return ndtree_.getIterator(); + return ndtree_.template getIterator(); } template auto getNodeIterator() const { - return ndtree_.getIterator(); + return ndtree_.template getIterator(); } size_t getMemoryUsage() const { return ndtree_.getMemoryUsage(); } @@ -74,16 +89,18 @@ class HashedWaveletOctreeBlock { const FloatingPoint max_log_odds_; OctreeType ndtree_{tree_height_ - 1}; - Coefficients::Scale root_scale_coefficient_{}; + typename Coefficients::Scale root_scale_coefficient_{}; bool needs_thresholding_ = false; bool needs_pruning_ = false; Timestamp last_updated_stamp_ = Time::now(); - void recursiveThreshold(OctreeType::NodeRefType node, - Coefficients::Scale& node_scale_coefficient); - void recursivePrune(OctreeType::NodeRefType node); + void recursiveThreshold(typename OctreeType::NodeRefType node, + typename Coefficients::Scale& node_scale_coefficient); + void recursivePrune(typename OctreeType::NodeRefType node); }; + +using HashedWaveletOctreeBlock = HashedWaveletOctreeBlockT; } // namespace wavemap #include "wavemap/core/map/impl/hashed_wavelet_octree_block_inl.h" diff --git a/library/cpp/include/wavemap/core/map/impl/hashed_wavelet_octree_block_inl.h b/library/cpp/include/wavemap/core/map/impl/hashed_wavelet_octree_block_inl.h index cfbfeda34..22390219d 100644 --- a/library/cpp/include/wavemap/core/map/impl/hashed_wavelet_octree_block_inl.h +++ b/library/cpp/include/wavemap/core/map/impl/hashed_wavelet_octree_block_inl.h @@ -1,27 +1,50 @@ #ifndef WAVEMAP_CORE_MAP_IMPL_HASHED_WAVELET_OCTREE_BLOCK_INL_H_ #define WAVEMAP_CORE_MAP_IMPL_HASHED_WAVELET_OCTREE_BLOCK_INL_H_ +#include +#include +#include + +#include "wavemap/core/utils/profile/profiler_interface.h" #include "wavemap/core/utils/query/occupancy_classifier.h" namespace wavemap { -inline bool HashedWaveletOctreeBlock::empty() const { - // Check if all cells in the block are equal to zero - // NOTE: Aside from checking whether the block contains no detail - // coefficients, we also need to check whether its scale coefficient - // (average value over the whole block) is zero. - return ndtree_.empty() && - OccupancyClassifier::isUnobserved(root_scale_coefficient_); +namespace detail { +template +FloatingPoint getOccupancy(const CellDataT& value) { + return static_cast(value); +} + +template +void setOccupancy(CellDataT& value, FloatingPoint occupancy) { + value = occupancy; +} +} // namespace detail + +template +bool HashedWaveletOctreeBlockT::empty() const { + return ndtree_.empty() && OccupancyClassifier::isUnobserved( + detail::getOccupancy(root_scale_coefficient_)); } -inline FloatingPoint HashedWaveletOctreeBlock::getTimeSinceLastUpdated() const { +template +FloatingPoint HashedWaveletOctreeBlockT::getTimeSinceLastUpdated() + const { return time::to_seconds(Time::now() - last_updated_stamp_); } -inline FloatingPoint HashedWaveletOctreeBlock::getCellValue( +template +FloatingPoint HashedWaveletOctreeBlockT::getCellValue( + const OctreeIndex& index) const { + return detail::getOccupancy(getVoxelValue(index)); +} + +template +CellDataT HashedWaveletOctreeBlockT::getVoxelValue( const OctreeIndex& index) const { const MortonIndex morton_code = convert::nodeIndexToMorton(index); - OctreeType::NodeConstPtrType node = &ndtree_.getRootNode(); - FloatingPoint value = root_scale_coefficient_; + typename OctreeType::NodeConstPtrType node = &ndtree_.getRootNode(); + CellDataT value = root_scale_coefficient_; for (int parent_height = tree_height_; node && index.height < parent_height; --parent_height) { const NdtreeIndexRelativeChild child_index = @@ -31,6 +54,230 @@ inline FloatingPoint HashedWaveletOctreeBlock::getCellValue( } return value; } + +template +void HashedWaveletOctreeBlockT::threshold() { + ProfilerZoneScoped; + if (getNeedsThresholding()) { + recursiveThreshold(ndtree_.getRootNode(), root_scale_coefficient_); + setNeedsThresholding(false); + } +} + +template +void HashedWaveletOctreeBlockT::prune() { + ProfilerZoneScoped; + if (getNeedsPruning()) { + threshold(); + recursivePrune(ndtree_.getRootNode()); + setNeedsPruning(false); + } +} + +template +void HashedWaveletOctreeBlockT::clear() { + ProfilerZoneScoped; + root_scale_coefficient_ = typename Coefficients::Scale{}; + ndtree_.clear(); + setLastUpdatedStamp(); +} + +template +void HashedWaveletOctreeBlockT::setCellValue( + const OctreeIndex& index, FloatingPoint new_value) { + CellDataT voxel = getVoxelValue(index); + detail::setOccupancy(voxel, new_value); + setVoxelValue(index, voxel); +} + +template +void HashedWaveletOctreeBlockT::setVoxelValue( + const OctreeIndex& index, const CellDataT& new_value) { + setNeedsPruning(); + setNeedsThresholding(); + setLastUpdatedStamp(); + const MortonIndex morton_code = convert::nodeIndexToMorton(index); + std::vector ancestors; + const int height_difference = tree_height_ - index.height; + ancestors.reserve(height_difference); + ancestors.emplace_back(&ndtree_.getRootNode()); + CellDataT current_value = root_scale_coefficient_; + for (int parent_height = tree_height_; index.height + 1 < parent_height; + --parent_height) { + const NdtreeIndexRelativeChild child_index = + OctreeIndex::computeRelativeChildIndex(morton_code, parent_height); + typename OctreeType::NodePtrType current_parent = ancestors.back(); + current_value = Transform::backwardSingleChild( + {current_value, current_parent->data()}, child_index); + typename OctreeType::NodeRefType child = + current_parent->getOrAllocateChild(child_index); + ancestors.emplace_back(&child); + } + DCHECK_EQ(ancestors.size(), height_difference); + + typename Coefficients::Parent coefficients{new_value - current_value, {}}; + for (int parent_height = index.height + 1; parent_height <= tree_height_; + ++parent_height) { + const NdtreeIndexRelativeChild child_index = + OctreeIndex::computeRelativeChildIndex(morton_code, parent_height); + typename OctreeType::NodePtrType current_node = ancestors.back(); + ancestors.pop_back(); + coefficients = + Transform::forwardSingleChild(coefficients.scale, child_index); + current_node->data() += coefficients.details; + } + + root_scale_coefficient_ += coefficients.scale; +} + +template +void HashedWaveletOctreeBlockT::addToCellValue( + const OctreeIndex& index, FloatingPoint update) { + addToVoxelValue(index, CellDataT{update}); +} + +template +void HashedWaveletOctreeBlockT::addToVoxelValue( + const OctreeIndex& index, const CellDataT& update) { + setNeedsPruning(); + setNeedsThresholding(); + setLastUpdatedStamp(); + const MortonIndex morton_code = convert::nodeIndexToMorton(index); + + std::vector ancestors; + const int height_difference = tree_height_ - index.height; + ancestors.reserve(height_difference); + ancestors.emplace_back(&ndtree_.getRootNode()); + for (int parent_height = tree_height_; index.height + 1 < parent_height; + --parent_height) { + const NdtreeIndexRelativeChild child_index = + OctreeIndex::computeRelativeChildIndex(morton_code, parent_height); + typename OctreeType::NodePtrType current_parent = ancestors.back(); + typename OctreeType::NodeRefType child = + current_parent->getOrAllocateChild(child_index); + ancestors.emplace_back(&child); + } + DCHECK_EQ(ancestors.size(), height_difference); + + typename Coefficients::Parent coefficients{update, {}}; + for (int parent_height = index.height + 1; parent_height <= tree_height_; + ++parent_height) { + typename OctreeType::NodePtrType current_node = ancestors.back(); + ancestors.pop_back(); + const NdtreeIndexRelativeChild child_index = + OctreeIndex::computeRelativeChildIndex(morton_code, parent_height); + coefficients = + Transform::forwardSingleChild(coefficients.scale, child_index); + current_node->data() += coefficients.details; + } + root_scale_coefficient_ += coefficients.scale; +} + +template +void HashedWaveletOctreeBlockT::forEachLeaf( + const BlockIndex& block_index, + MapBase::IndexedLeafVisitorFunction visitor_fn, + IndexElement termination_height) const { + forEachVoxelLeaf( + block_index, + [&visitor_fn](const OctreeIndex& node_index, + const CellDataT& voxel_value) { + visitor_fn(node_index, detail::getOccupancy(voxel_value)); + }, + termination_height); +} + +template +template +void HashedWaveletOctreeBlockT::forEachVoxelLeaf( + const BlockIndex& block_index, IndexedVoxelLeafVisitorFunction visitor_fn, + IndexElement termination_height) const { + ProfilerZoneScoped; + if (empty()) { + return; + } + + struct StackElement { + const OctreeIndex node_index; + typename OctreeType::NodeConstRefType node; + const typename Coefficients::Scale scale_coefficient{}; + }; + std::stack stack; + stack.emplace(StackElement{OctreeIndex{tree_height_, block_index}, + ndtree_.getRootNode(), root_scale_coefficient_}); + while (!stack.empty()) { + const OctreeIndex node_index = stack.top().node_index; + typename OctreeType::NodeConstRefType node = stack.top().node; + const typename Coefficients::Scale node_scale_coefficient = + stack.top().scale_coefficient; + stack.pop(); + + const typename Coefficients::CoefficientsArray child_scale_coefficients = + Transform::backward({node_scale_coefficient, {node.data()}}); + for (NdtreeIndexRelativeChild child_idx = 0; + child_idx < OctreeIndex::kNumChildren; ++child_idx) { + const OctreeIndex child_node_index = + node_index.computeChildIndex(child_idx); + const typename Coefficients::Scale child_scale_coefficient = + child_scale_coefficients[child_idx]; + typename OctreeType::NodeConstPtrType child_node = node.getChild(child_idx); + if (child_node && termination_height < child_node_index.height) { + stack.emplace(StackElement{child_node_index, *child_node, + child_scale_coefficient}); + } else { + visitor_fn(child_node_index, child_scale_coefficient); + } + } + } +} + +template +void HashedWaveletOctreeBlockT::recursiveThreshold( + typename OctreeType::NodeRefType node, + typename Coefficients::Scale& node_scale_coefficient) { + auto& node_detail_coefficients = node.data(); + typename Coefficients::CoefficientsArray child_scale_coefficients = + Transform::backward({node_scale_coefficient, node_detail_coefficients}); + + for (NdtreeIndexRelativeChild child_idx = 0; + child_idx < OctreeIndex::kNumChildren; ++child_idx) { + typename Coefficients::Scale& child_scale = child_scale_coefficients[child_idx]; + if (auto child_node = node.getChild(child_idx); child_node) { + recursiveThreshold(*child_node, child_scale); + } else { + detail::setOccupancy( + child_scale, std::clamp(detail::getOccupancy(child_scale), + min_log_odds_, max_log_odds_)); + } + } + + const auto [new_scale, new_details] = + Transform::forward(child_scale_coefficients); + node_detail_coefficients = new_details; + node_scale_coefficient = new_scale; +} + +template +void HashedWaveletOctreeBlockT::recursivePrune( + typename OctreeType::NodeRefType node) { + bool has_at_least_one_child = false; + for (NdtreeIndexRelativeChild child_idx = 0; + child_idx < OctreeIndex::kNumChildren; ++child_idx) { + if (typename OctreeType::NodePtrType child_node = node.getChild(child_idx); + child_node) { + recursivePrune(*child_node); + if (!child_node->hasChildrenArray() && + !child_node->hasNonzeroData(1e-3f)) { + node.eraseChild(child_idx); + } else { + has_at_least_one_child = true; + } + } + } + if (!has_at_least_one_child) { + node.deleteChildrenArray(); + } +} } // namespace wavemap #endif // WAVEMAP_CORE_MAP_IMPL_HASHED_WAVELET_OCTREE_BLOCK_INL_H_ diff --git a/library/cpp/include/wavemap/core/map/impl/hashed_wavelet_octree_inl.h b/library/cpp/include/wavemap/core/map/impl/hashed_wavelet_octree_inl.h index 3866b97dd..7564a6322 100644 --- a/library/cpp/include/wavemap/core/map/impl/hashed_wavelet_octree_inl.h +++ b/library/cpp/include/wavemap/core/map/impl/hashed_wavelet_octree_inl.h @@ -2,9 +2,11 @@ #define WAVEMAP_CORE_MAP_IMPL_HASHED_WAVELET_OCTREE_INL_H_ #include "wavemap/core/indexing/index_conversions.h" +#include "wavemap/core/utils/profile/profiler_interface.h" namespace wavemap { -inline size_t HashedWaveletOctree::size() const { +template +size_t HashedWaveletOctreeT::size() const { size_t size = 0u; forEachBlock([&size](const BlockIndex& /*block_index*/, const Block& block) { size += block.size(); @@ -12,7 +14,63 @@ inline size_t HashedWaveletOctree::size() const { return size; } -inline FloatingPoint HashedWaveletOctree::getCellValue( +template +void HashedWaveletOctreeT::threshold() { + ProfilerZoneScoped; + forEachBlock([](const BlockIndex& /*block_index*/, Block& block) { + block.threshold(); + }); +} + +template +void HashedWaveletOctreeT::prune() { + ProfilerZoneScoped; + block_map_.eraseBlockIf([](const BlockIndex& /*block_index*/, Block& block) { + block.prune(); + return block.empty(); + }); +} + +template +void HashedWaveletOctreeT::pruneSmart() { + ProfilerZoneScoped; + block_map_.eraseBlockIf( + [&config = config_](const BlockIndex& /*block_index*/, Block& block) { + if (config.only_prune_blocks_if_unused_for < + block.getTimeSinceLastUpdated()) { + block.prune(); + } + return block.empty(); + }); +} + +template +size_t HashedWaveletOctreeT::getMemoryUsage() const { + ProfilerZoneScoped; + // TODO(victorr): Also include the memory usage of the unordered map itself + size_t memory_usage = 0u; + forEachBlock( + [&memory_usage](const BlockIndex& /*block_index*/, const Block& block) { + memory_usage += block.getMemoryUsage(); + }); + return memory_usage; +} + +template +Index3D HashedWaveletOctreeT::getMinIndex() const { + return cells_per_block_side_ * getMinBlockIndex(); +} + +template +Index3D HashedWaveletOctreeT::getMaxIndex() const { + if (empty()) { + return Index3D::Zero(); + } + return cells_per_block_side_ * (getMaxBlockIndex().array() + 1) - 1; +} + +template +FloatingPoint HashedWaveletOctreeT::getCellValue( const Index3D& index) const { const BlockIndex block_index = convert::indexToBlockIndex(index, config_.tree_height); @@ -24,7 +82,8 @@ inline FloatingPoint HashedWaveletOctree::getCellValue( return block->getCellValue(cell_index); } -inline FloatingPoint HashedWaveletOctree::getCellValue( +template +FloatingPoint HashedWaveletOctreeT::getCellValue( const OctreeIndex& index) const { const BlockIndex block_index = indexToBlockIndex(index); const Block* block = getBlock(block_index); @@ -35,8 +94,34 @@ inline FloatingPoint HashedWaveletOctree::getCellValue( return block->getCellValue(cell_index); } -inline void HashedWaveletOctree::setCellValue(const Index3D& index, - FloatingPoint new_value) { +template +CellDataT HashedWaveletOctreeT::getVoxelValue( + const Index3D& index) const { + const BlockIndex block_index = + convert::indexToBlockIndex(index, config_.tree_height); + const Block* block = getBlock(block_index); + if (!block) { + return {}; + } + const CellIndex cell_index = indexToCellIndex({0, index}); + return block->getVoxelValue(cell_index); +} + +template +CellDataT HashedWaveletOctreeT::getVoxelValue( + const OctreeIndex& index) const { + const BlockIndex block_index = indexToBlockIndex(index); + const Block* block = getBlock(block_index); + if (!block) { + return {}; + } + const CellIndex cell_index = indexToCellIndex(index); + return block->getVoxelValue(cell_index); +} + +template +void HashedWaveletOctreeT::setCellValue(const Index3D& index, + FloatingPoint new_value) { const BlockIndex block_index = convert::indexToBlockIndex(index, config_.tree_height); auto& block = getOrAllocateBlock(block_index); @@ -44,8 +129,19 @@ inline void HashedWaveletOctree::setCellValue(const Index3D& index, block.setCellValue(cell_index, new_value); } -inline void HashedWaveletOctree::addToCellValue(const Index3D& index, - FloatingPoint update) { +template +void HashedWaveletOctreeT::setVoxelValue( + const Index3D& index, const CellDataT& new_value) { + const BlockIndex block_index = + convert::indexToBlockIndex(index, config_.tree_height); + auto& block = getOrAllocateBlock(block_index); + const CellIndex cell_index = indexToCellIndex({0, index}); + block.setVoxelValue(cell_index, new_value); +} + +template +void HashedWaveletOctreeT::addToCellValue(const Index3D& index, + FloatingPoint update) { const BlockIndex block_index = convert::indexToBlockIndex(index, config_.tree_height); auto& block = getOrAllocateBlock(block_index); @@ -53,55 +149,100 @@ inline void HashedWaveletOctree::addToCellValue(const Index3D& index, block.addToCellValue(cell_index, update); } -inline bool HashedWaveletOctree::hasBlock(const Index3D& block_index) const { +template +void HashedWaveletOctreeT::addToVoxelValue( + const Index3D& index, const CellDataT& update) { + const BlockIndex block_index = + convert::indexToBlockIndex(index, config_.tree_height); + auto& block = getOrAllocateBlock(block_index); + const CellIndex cell_index = indexToCellIndex({0, index}); + block.addToVoxelValue(cell_index, update); +} + +template +bool HashedWaveletOctreeT::hasBlock( + const Index3D& block_index) const { return block_map_.hasBlock(block_index); } -inline bool HashedWaveletOctree::eraseBlock( - const HashedWaveletOctree::BlockIndex& block_index) { +template +bool HashedWaveletOctreeT::eraseBlock( + const typename HashedWaveletOctreeT::BlockIndex& block_index) { return block_map_.eraseBlock(block_index); } +template template -void HashedWaveletOctree::eraseBlockIf(IndexedBlockVisitor indicator_fn) { +void HashedWaveletOctreeT::eraseBlockIf( + IndexedBlockVisitor indicator_fn) { block_map_.eraseBlockIf(indicator_fn); } -inline HashedWaveletOctree::Block* HashedWaveletOctree::getBlock( - const Index3D& block_index) { +template +typename HashedWaveletOctreeT::Block* +HashedWaveletOctreeT::getBlock(const Index3D& block_index) { return block_map_.getBlock(block_index); } -inline const HashedWaveletOctree::Block* HashedWaveletOctree::getBlock( - const Index3D& block_index) const { +template +const typename HashedWaveletOctreeT::Block* +HashedWaveletOctreeT::getBlock(const Index3D& block_index) const { return block_map_.getBlock(block_index); } -inline HashedWaveletOctree::Block& HashedWaveletOctree::getOrAllocateBlock( +template +typename HashedWaveletOctreeT::Block& +HashedWaveletOctreeT::getOrAllocateBlock( const Index3D& block_index) { return block_map_.getOrAllocateBlock(block_index, config_.tree_height, config_.min_log_odds, config_.max_log_odds); } +template template -void HashedWaveletOctree::forEachBlock(IndexedBlockVisitor visitor_fn) { +void HashedWaveletOctreeT::forEachBlock( + IndexedBlockVisitor visitor_fn) { block_map_.forEachBlock(visitor_fn); } +template template -void HashedWaveletOctree::forEachBlock(IndexedBlockVisitor visitor_fn) const { +void HashedWaveletOctreeT::forEachBlock( + IndexedBlockVisitor visitor_fn) const { block_map_.forEachBlock(visitor_fn); } -inline HashedWaveletOctree::BlockIndex HashedWaveletOctree::indexToBlockIndex( +template +void HashedWaveletOctreeT::forEachLeaf( + MapBase::IndexedLeafVisitorFunction visitor_fn) const { + forEachBlock( + [&visitor_fn](const BlockIndex& block_index, const Block& block) { + block.forEachLeaf(block_index, visitor_fn); + }); +} + +template +template +void HashedWaveletOctreeT::forEachVoxelLeaf( + IndexedVoxelLeafVisitorFunction visitor_fn) const { + forEachBlock( + [&visitor_fn](const BlockIndex& block_index, const Block& block) { + block.forEachVoxelLeaf(block_index, visitor_fn); + }); +} + +template +typename HashedWaveletOctreeT::BlockIndex +HashedWaveletOctreeT::indexToBlockIndex( const OctreeIndex& node_index) const { const Index3D index = convert::nodeIndexToMinCornerIndex(node_index); return convert::indexToBlockIndex(index, config_.tree_height); } -inline HashedWaveletOctree::CellIndex HashedWaveletOctree::indexToCellIndex( - OctreeIndex index) const { +template +typename HashedWaveletOctreeT::CellIndex +HashedWaveletOctreeT::indexToCellIndex(OctreeIndex index) const { DCHECK_LE(index.height, config_.tree_height); const IndexElement height_difference = config_.tree_height - index.height; index.position = diff --git a/library/cpp/include/wavemap/core/map/map_base.h b/library/cpp/include/wavemap/core/map/map_base.h index f618e86d6..15295d60d 100644 --- a/library/cpp/include/wavemap/core/map/map_base.h +++ b/library/cpp/include/wavemap/core/map/map_base.h @@ -19,12 +19,13 @@ struct MapType : TypeSelector { kOctree, kWaveletOctree, kHashedWaveletOctree, - kHashedChunkedWaveletOctree + kHashedChunkedWaveletOctree, + kLayeredHashedWaveletOctree }; static constexpr std::array names = { "hashed_blocks", "octree", "wavelet_octree", "hashed_wavelet_octree", - "hashed_chunked_wavelet_octree"}; + "hashed_chunked_wavelet_octree", "layered_hashed_wavelet_octree"}; }; /** @@ -66,6 +67,8 @@ class MapBase { : config_(config.checkValid()) {} virtual ~MapBase() = default; + virtual MapType getMapType() const = 0; + //! Whether the map is empty virtual bool empty() const = 0; //! The number of cells or nodes in the map diff --git a/library/cpp/include/wavemap/core/map/volumetric_octree.h b/library/cpp/include/wavemap/core/map/volumetric_octree.h index b763f5a11..ce173bf7c 100644 --- a/library/cpp/include/wavemap/core/map/volumetric_octree.h +++ b/library/cpp/include/wavemap/core/map/volumetric_octree.h @@ -55,6 +55,8 @@ class VolumetricOctree : public MapBase { explicit VolumetricOctree(const VolumetricOctreeConfig& config) : MapBase(config), config_(config.checkValid()) {} + MapType getMapType() const override { return MapType::kOctree; } + bool empty() const override { return ndtree_.empty(); } size_t size() const override { return ndtree_.size(); } void threshold() override; diff --git a/library/cpp/include/wavemap/core/map/wavelet_octree.h b/library/cpp/include/wavemap/core/map/wavelet_octree.h index 95e6beffc..03c0a52ca 100644 --- a/library/cpp/include/wavemap/core/map/wavelet_octree.h +++ b/library/cpp/include/wavemap/core/map/wavelet_octree.h @@ -61,6 +61,8 @@ class WaveletOctree : public MapBase { explicit WaveletOctree(const WaveletOctreeConfig& config) : MapBase(config), config_(config.checkValid()) {} + MapType getMapType() const override { return MapType::kWaveletOctree; } + bool empty() const override; size_t size() const override { return ndtree_.size(); } void threshold() override; diff --git a/library/cpp/include/wavemap/io/file_conversions.h b/library/cpp/include/wavemap/io/file_conversions.h index 6e0cb70f2..9bfb8521c 100644 --- a/library/cpp/include/wavemap/io/file_conversions.h +++ b/library/cpp/include/wavemap/io/file_conversions.h @@ -9,6 +9,13 @@ namespace wavemap::io { bool mapToFile(const MapBase& map, const std::filesystem::path& file_path); bool fileToMap(const std::filesystem::path& file_path, MapBase::Ptr& map); +template +bool mapToFile(const HashedWaveletOctreeT& map, const std::filesystem::path& file_path); + +template +bool fileToMap(const std::filesystem::path& file_path, typename HashedWaveletOctreeT::Ptr& map); } // namespace wavemap::io +#include "wavemap/io/impl/file_conversions_inl.h" + #endif // WAVEMAP_IO_FILE_CONVERSIONS_H_ diff --git a/library/cpp/include/wavemap/io/impl/file_conversions_inl.h b/library/cpp/include/wavemap/io/impl/file_conversions_inl.h new file mode 100644 index 000000000..b9ec5c72a --- /dev/null +++ b/library/cpp/include/wavemap/io/impl/file_conversions_inl.h @@ -0,0 +1,55 @@ +#ifndef WAVEMAP_IO_IMPL_FILE_CONVERSIONS_INL_H_ +#define WAVEMAP_IO_IMPL_FILE_CONVERSIONS_INL_H_ + +#include +#include +#include + +namespace wavemap::io { +template +bool mapToFile(const HashedWaveletOctreeT& map, const std::filesystem::path& file_path) { + if (file_path.empty()) { + LOG(WARNING) << "Could open file for writing. Specified file path is empty."; + return false; + } + + std::ofstream file_ostream(file_path, + std::ofstream::out | std::ofstream::binary); + if (!file_ostream.is_open()) { + LOG(WARNING) << "Could not open file " << file_path + << " for writing. Error: " << strerror(errno); + return false; + } + + if (!mapToStream(map, file_ostream)) { + return false; + } + + file_ostream.close(); + return static_cast(file_ostream); +} + +template +bool fileToMap(const std::filesystem::path& file_path, typename HashedWaveletOctreeT::Ptr& map) { + if (file_path.empty()) { + LOG(WARNING) << "Could not open file for reading. Specified file path is empty."; + return false; + } + + std::ifstream file_istream(file_path, + std::ifstream::in | std::ifstream::binary); + if (!file_istream.is_open()) { + LOG(WARNING) << "Could not open file " << file_path << " for reading. Error: " << strerror(errno); + return false; + } + + if (!streamToMap(file_istream, map)) { + LOG(WARNING) << "Failed to parse map from file " << file_path << "."; + return false; + } + + return true; +} +} // namespace wavemap::io + +#endif // WAVEMAP_IO_IMPL_FILE_CONVERSIONS_INL_H_ diff --git a/library/cpp/include/wavemap/io/impl/stream_conversions_inl.h b/library/cpp/include/wavemap/io/impl/stream_conversions_inl.h new file mode 100644 index 000000000..718c11e31 --- /dev/null +++ b/library/cpp/include/wavemap/io/impl/stream_conversions_inl.h @@ -0,0 +1,153 @@ +#ifndef WAVEMAP_IO_IMPL_STREAM_CONVERSIONS_INL_H_ +#define WAVEMAP_IO_IMPL_STREAM_CONVERSIONS_INL_H_ + +#include + +#include "wavemap/core/utils/bits/bit_operations.h" + +namespace wavemap::io { +template +bool mapToStream(const HashedWaveletOctreeT& map, + std::ostream& ostream) { + if (!ostream.good()) { + return false; + } + + using Block = typename HashedWaveletOctreeT::Block; + using NodeConstRefType = typename Block::OctreeType::NodeConstRefType; + + struct StackElement { + const CellDataT scale; + NodeConstRefType node; + }; + + constexpr FloatingPoint kNumericalNoise = 1e-3f; + const auto min_log_odds = map.getMinLogOdds() + kNumericalNoise; + const auto max_log_odds = map.getMaxLogOdds() - kNumericalNoise; + + streamable::StorageFormat storage_format = + streamable::StorageFormat::kLayeredHashedWaveletOctree; + storage_format.write(ostream); + + streamable::HashedWaveletOctreeHeader header; + header.min_cell_width = map.getMinCellWidth(); + header.min_log_odds = map.getMinLogOdds(); + header.max_log_odds = map.getMaxLogOdds(); + header.tree_height = map.getTreeHeight(); + header.num_blocks = map.getHashMap().size(); + header.write(ostream); + + map.forEachBlock([&ostream, min_log_odds, max_log_odds]( + const Index3D& block_index, const auto& block) { + if (!ostream.good()) { + return; + } + + streamable::Index3D streamable_block_index{block_index.x(), + block_index.y(), + block_index.z()}; + streamable_block_index.write(ostream); + CellDataSerializerT::write(ostream, block.getRootScale()); + + std::stack stack; + stack.emplace(StackElement{block.getRootScale(), block.getRootNode()}); + while (!stack.empty()) { + const CellDataT scale = stack.top().scale; + const auto& node = stack.top().node; + stack.pop(); + + streamable::UInt8 allocated_children_bitset{}; + for (const CellDataT& coefficient : node.data()) { + CellDataSerializerT::write(ostream, coefficient); + } + + const auto child_scales = Block::Transform::backward({scale, node.data()}); + for (int relative_child_idx = OctreeIndex::kNumChildren - 1; + 0 <= relative_child_idx; --relative_child_idx) { + const auto child_scale = child_scales[relative_child_idx]; + const FloatingPoint child_occupancy = + static_cast(child_scale); + if (child_occupancy < min_log_odds || max_log_odds < child_occupancy) { + continue; + } + + const auto* child = node.getChild(relative_child_idx); + if (child) { + stack.emplace(StackElement{child_scale, *child}); + allocated_children_bitset += (1 << relative_child_idx); + } + } + + ostream.write(reinterpret_cast(&allocated_children_bitset), + sizeof(allocated_children_bitset)); + } + }); + + return ostream.good(); +} + +template +bool streamToMap(std::istream& istream, + typename HashedWaveletOctreeT::Ptr& map) { + if (!istream.good()) { + return false; + } + + if (streamable::StorageFormat::read(istream) != + streamable::StorageFormat::kLayeredHashedWaveletOctree) { + return false; + } + + const auto header = streamable::HashedWaveletOctreeHeader::read(istream); + HashedWaveletOctreeConfig config; + config.min_cell_width = header.min_cell_width; + config.min_log_odds = header.min_log_odds; + config.max_log_odds = header.max_log_odds; + config.tree_height = header.tree_height; + map = std::make_shared>(config); + + using Block = typename HashedWaveletOctreeT::Block; + using NodeType = typename Block::OctreeType::NodeType; + + for (size_t block_idx = 0; block_idx < header.num_blocks; ++block_idx) { + if (!istream.good()) { + return false; + } + + const auto streamable_block_index = streamable::Index3D::read(istream); + const Index3D block_index{streamable_block_index.x, + streamable_block_index.y, + streamable_block_index.z}; + auto& block = map->getOrAllocateBlock(block_index); + block.getRootScale() = CellDataSerializerT::read(istream); + + std::stack stack; + stack.emplace(&block.getRootNode()); + while (!stack.empty()) { + NodeType* node = stack.top(); + stack.pop(); + + for (CellDataT& coefficient : node->data()) { + coefficient = CellDataSerializerT::read(istream); + } + + streamable::UInt8 allocated_children_bitset{}; + istream.read(reinterpret_cast(&allocated_children_bitset), + sizeof(allocated_children_bitset)); + + for (int relative_child_idx = OctreeIndex::kNumChildren - 1; + 0 <= relative_child_idx; --relative_child_idx) { + const bool child_exists = bit_ops::is_bit_set( + allocated_children_bitset, relative_child_idx); + if (child_exists) { + stack.emplace(&node->getOrAllocateChild(relative_child_idx)); + } + } + } + } + + return istream.good(); +} +} // namespace wavemap::io + +#endif // WAVEMAP_IO_IMPL_STREAM_CONVERSIONS_INL_H_ diff --git a/library/cpp/include/wavemap/io/stream_conversions.h b/library/cpp/include/wavemap/io/stream_conversions.h index f3341999b..75789ab38 100644 --- a/library/cpp/include/wavemap/io/stream_conversions.h +++ b/library/cpp/include/wavemap/io/stream_conversions.h @@ -3,6 +3,7 @@ #include #include +#include #include "wavemap/core/common.h" #include "wavemap/core/map/cell_types/haar_coefficients.h" @@ -16,6 +17,35 @@ namespace wavemap::io { bool mapToStream(const MapBase& map, std::ostream& ostream); bool streamToMap(std::istream& istream, MapBase::Ptr& map); +struct StreamableFloatingPoint { + static void write(std::ostream& ostream, FloatingPoint value) { + streamable::Float serialized_value = value; + ostream.write(reinterpret_cast(&serialized_value), + sizeof(serialized_value)); + } + + static FloatingPoint read(std::istream& istream) { + streamable::Float serialized_value{}; + istream.read(reinterpret_cast(&serialized_value), + sizeof(serialized_value)); + return serialized_value; + } +}; + +template +struct StreamableCellData; + +template <> +struct StreamableCellData : StreamableFloatingPoint {}; + +template +bool mapToStream(const HashedWaveletOctreeT& map, + std::ostream& ostream); + +template +bool streamToMap(std::istream& istream, + typename HashedWaveletOctreeT::Ptr& map); + bool mapToStream(const HashedBlocks& map, std::ostream& ostream); bool streamToMap(std::istream& istream, HashedBlocks::Ptr& map); @@ -28,4 +58,6 @@ bool streamToMap(std::istream& istream, HashedWaveletOctree::Ptr& map); bool mapToStream(const HashedChunkedWaveletOctree& map, std::ostream& ostream); } // namespace wavemap::io +#include "wavemap/io/impl/stream_conversions_inl.h" + #endif // WAVEMAP_IO_STREAM_CONVERSIONS_H_ diff --git a/library/cpp/include/wavemap/io/streamable_types.h b/library/cpp/include/wavemap/io/streamable_types.h index 3a82d09c5..7112f159d 100644 --- a/library/cpp/include/wavemap/io/streamable_types.h +++ b/library/cpp/include/wavemap/io/streamable_types.h @@ -88,10 +88,16 @@ struct HashedWaveletOctreeHeader { struct StorageFormat : TypeSelector { using TypeSelector::TypeSelector; - enum Id : TypeId { kWaveletOctree, kHashedWaveletOctree, kHashedBlocks }; + enum Id : TypeId { + kWaveletOctree, + kHashedWaveletOctree, + kHashedBlocks, + kLayeredHashedWaveletOctree + }; static constexpr std::array names = { - "wavelet_octree", "hashed_wavelet_octree", "hashed_blocks"}; + "wavelet_octree", "hashed_wavelet_octree", "hashed_blocks", + "layered_hashed_wavelet_octree"}; inline void write(std::ostream& ostream) const; inline static StorageFormat read(std::istream& istream); diff --git a/library/cpp/src/core/map/hashed_wavelet_octree.cc b/library/cpp/src/core/map/hashed_wavelet_octree.cc index 91c468527..92139d8d0 100644 --- a/library/cpp/src/core/map/hashed_wavelet_octree.cc +++ b/library/cpp/src/core/map/hashed_wavelet_octree.cc @@ -1,9 +1,5 @@ #include "wavemap/core/map/hashed_wavelet_octree.h" -#include - -#include - namespace wavemap { DECLARE_CONFIG_MEMBERS(HashedWaveletOctreeConfig, (min_cell_width) @@ -21,62 +17,4 @@ bool HashedWaveletOctreeConfig::isValid(bool verbose) const { return is_valid; } - -void HashedWaveletOctree::threshold() { - ProfilerZoneScoped; - forEachBlock([](const BlockIndex& /*block_index*/, Block& block) { - block.threshold(); - }); -} - -void HashedWaveletOctree::prune() { - ProfilerZoneScoped; - block_map_.eraseBlockIf([](const BlockIndex& /*block_index*/, Block& block) { - block.prune(); - return block.empty(); - }); -} - -void HashedWaveletOctree::pruneSmart() { - ProfilerZoneScoped; - block_map_.eraseBlockIf( - [&config = config_](const BlockIndex& /*block_index*/, Block& block) { - if (config.only_prune_blocks_if_unused_for < - block.getTimeSinceLastUpdated()) { - block.prune(); - } - return block.empty(); - }); -} - -size_t HashedWaveletOctree::getMemoryUsage() const { - ProfilerZoneScoped; - // TODO(victorr): Also include the memory usage of the unordered map itself - size_t memory_usage = 0u; - forEachBlock( - [&memory_usage](const BlockIndex& /*block_index*/, const Block& block) { - memory_usage += block.getMemoryUsage(); - }); - return memory_usage; -} - -Index3D HashedWaveletOctree::getMinIndex() const { - return cells_per_block_side_ * getMinBlockIndex(); -} - -Index3D HashedWaveletOctree::getMaxIndex() const { - if (empty()) { - return Index3D::Zero(); - } - return cells_per_block_side_ * (getMaxBlockIndex().array() + 1) - 1; -} - -void HashedWaveletOctree::forEachLeaf( - MapBase::IndexedLeafVisitorFunction visitor_fn) const { - forEachBlock( - [&visitor_fn](const BlockIndex& block_index, const Block& block) { - block.forEachLeaf(block_index, visitor_fn); - }); -} - } // namespace wavemap diff --git a/library/cpp/src/core/map/hashed_wavelet_octree_block.cc b/library/cpp/src/core/map/hashed_wavelet_octree_block.cc index a0633ab36..0d827cad4 100644 --- a/library/cpp/src/core/map/hashed_wavelet_octree_block.cc +++ b/library/cpp/src/core/map/hashed_wavelet_octree_block.cc @@ -1,196 +1 @@ #include "wavemap/core/map/hashed_wavelet_octree_block.h" - -#include -#include - -#include - -namespace wavemap { -void HashedWaveletOctreeBlock::threshold() { - ProfilerZoneScoped; - if (getNeedsThresholding()) { - recursiveThreshold(ndtree_.getRootNode(), root_scale_coefficient_); - setNeedsThresholding(false); - } -} - -void HashedWaveletOctreeBlock::prune() { - ProfilerZoneScoped; - if (getNeedsPruning()) { - threshold(); - recursivePrune(ndtree_.getRootNode()); - setNeedsPruning(false); - } -} - -void HashedWaveletOctreeBlock::clear() { - ProfilerZoneScoped; - root_scale_coefficient_ = Coefficients::Scale{}; - ndtree_.clear(); - setLastUpdatedStamp(); -} - -void HashedWaveletOctreeBlock::setCellValue(const OctreeIndex& index, - FloatingPoint new_value) { - setNeedsPruning(); - setNeedsThresholding(); - setLastUpdatedStamp(); - const MortonIndex morton_code = convert::nodeIndexToMorton(index); - std::vector ancestors; - const int height_difference = tree_height_ - index.height; - ancestors.reserve(height_difference); - ancestors.emplace_back(&ndtree_.getRootNode()); - FloatingPoint current_value = root_scale_coefficient_; - for (int parent_height = tree_height_; index.height + 1 < parent_height; - --parent_height) { - const NdtreeIndexRelativeChild child_index = - OctreeIndex::computeRelativeChildIndex(morton_code, parent_height); - OctreeType::NodePtrType current_parent = ancestors.back(); - current_value = Transform::backwardSingleChild( - {current_value, current_parent->data()}, child_index); - OctreeType::NodeRefType child = - current_parent->getOrAllocateChild(child_index); - ancestors.emplace_back(&child); - } - DCHECK_EQ(ancestors.size(), height_difference); - - Coefficients::Parent coefficients{new_value - current_value, {}}; - for (int parent_height = index.height + 1; parent_height <= tree_height_; - ++parent_height) { - const NdtreeIndexRelativeChild child_index = - OctreeIndex::computeRelativeChildIndex(morton_code, parent_height); - OctreeType::NodePtrType current_node = ancestors.back(); - ancestors.pop_back(); - coefficients = - Transform::forwardSingleChild(coefficients.scale, child_index); - current_node->data() += coefficients.details; - } - - root_scale_coefficient_ += coefficients.scale; -} - -void HashedWaveletOctreeBlock::addToCellValue(const OctreeIndex& index, - FloatingPoint update) { - setNeedsPruning(); - setNeedsThresholding(); - setLastUpdatedStamp(); - const MortonIndex morton_code = convert::nodeIndexToMorton(index); - - std::vector ancestors; - const int height_difference = tree_height_ - index.height; - ancestors.reserve(height_difference); - ancestors.emplace_back(&ndtree_.getRootNode()); - for (int parent_height = tree_height_; index.height + 1 < parent_height; - --parent_height) { - const NdtreeIndexRelativeChild child_index = - OctreeIndex::computeRelativeChildIndex(morton_code, parent_height); - OctreeType::NodePtrType current_parent = ancestors.back(); - OctreeType::NodeRefType child = - current_parent->getOrAllocateChild(child_index); - ancestors.emplace_back(&child); - } - DCHECK_EQ(ancestors.size(), height_difference); - - Coefficients::Parent coefficients{update, {}}; - for (int parent_height = index.height + 1; parent_height <= tree_height_; - ++parent_height) { - OctreeType::NodePtrType current_node = ancestors.back(); - ancestors.pop_back(); - const NdtreeIndexRelativeChild child_index = - OctreeIndex::computeRelativeChildIndex(morton_code, parent_height); - coefficients = - Transform::forwardSingleChild(coefficients.scale, child_index); - current_node->data() += coefficients.details; - } - root_scale_coefficient_ += coefficients.scale; -} - -void HashedWaveletOctreeBlock::forEachLeaf( - const BlockIndex& block_index, - MapBase::IndexedLeafVisitorFunction visitor_fn, - IndexElement termination_height) const { - ProfilerZoneScoped; - if (empty()) { - return; - } - - struct StackElement { - const OctreeIndex node_index; - OctreeType::NodeConstRefType node; - const Coefficients::Scale scale_coefficient{}; - }; - std::stack stack; - stack.emplace(StackElement{OctreeIndex{tree_height_, block_index}, - ndtree_.getRootNode(), root_scale_coefficient_}); - while (!stack.empty()) { - const OctreeIndex node_index = stack.top().node_index; - OctreeType::NodeConstRefType node = stack.top().node; - const FloatingPoint node_scale_coefficient = stack.top().scale_coefficient; - stack.pop(); - - const Coefficients::CoefficientsArray child_scale_coefficients = - Transform::backward({node_scale_coefficient, {node.data()}}); - for (NdtreeIndexRelativeChild child_idx = 0; - child_idx < OctreeIndex::kNumChildren; ++child_idx) { - const OctreeIndex child_node_index = - node_index.computeChildIndex(child_idx); - const FloatingPoint child_scale_coefficient = - child_scale_coefficients[child_idx]; - OctreeType::NodeConstPtrType child_node = node.getChild(child_idx); - if (child_node && termination_height < child_node_index.height) { - stack.emplace(StackElement{child_node_index, *child_node, - child_scale_coefficient}); - } else { - visitor_fn(child_node_index, child_scale_coefficient); - } - } - } -} - -void HashedWaveletOctreeBlock::recursiveThreshold( // NOLINT - HashedWaveletOctreeBlock::OctreeType::NodeRefType node, - FloatingPoint& node_scale_coefficient) { - // Decompress child values - auto& node_detail_coefficients = node.data(); - Coefficients::CoefficientsArray child_scale_coefficients = - Transform::backward({node_scale_coefficient, node_detail_coefficients}); - - // Handle each child - for (NdtreeIndexRelativeChild child_idx = 0; - child_idx < OctreeIndex::kNumChildren; ++child_idx) { - Coefficients::Scale& child_scale = child_scale_coefficients[child_idx]; - if (auto child_node = node.getChild(child_idx); child_node) { - recursiveThreshold(*child_node, child_scale); - } else { - child_scale = std::clamp(child_scale, min_log_odds_, max_log_odds_); - } - } - - // Compress - const auto [new_scale, new_details] = - Transform::forward(child_scale_coefficients); - node_detail_coefficients = new_details; - node_scale_coefficient = new_scale; -} - -void HashedWaveletOctreeBlock::recursivePrune( // NOLINT - HashedWaveletOctreeBlock::OctreeType::NodeRefType node) { - bool has_at_least_one_child = false; - for (NdtreeIndexRelativeChild child_idx = 0; - child_idx < OctreeIndex::kNumChildren; ++child_idx) { - if (OctreeType::NodePtrType child_node = node.getChild(child_idx); - child_node) { - recursivePrune(*child_node); - if (!child_node->hasChildrenArray() && - !child_node->hasNonzeroData(1e-3f)) { - node.eraseChild(child_idx); - } else { - has_at_least_one_child = true; - } - } - } - if (!has_at_least_one_child) { - node.deleteChildrenArray(); - } -} -} // namespace wavemap diff --git a/library/cpp/src/io/stream_conversions.cc b/library/cpp/src/io/stream_conversions.cc index 364260678..0abaf5233 100644 --- a/library/cpp/src/io/stream_conversions.cc +++ b/library/cpp/src/io/stream_conversions.cc @@ -6,29 +6,28 @@ namespace wavemap::io { bool mapToStream(const MapBase& map, std::ostream& ostream) { - // Call the appropriate mapToStream converter based on the map's derived type - if (const auto* hashed_blocks = dynamic_cast(&map); - hashed_blocks) { - return io::mapToStream(*hashed_blocks, ostream); - } - if (const auto* wavelet_octree = dynamic_cast(&map); - wavelet_octree) { - return io::mapToStream(*wavelet_octree, ostream); - } - if (const auto* hashed_wavelet_octree = - dynamic_cast(&map); - hashed_wavelet_octree) { - return io::mapToStream(*hashed_wavelet_octree, ostream); - } - if (const auto* hashed_chunked_wavelet_octree = - dynamic_cast(&map); - hashed_chunked_wavelet_octree) { - return io::mapToStream(*hashed_chunked_wavelet_octree, ostream); + // Call the appropriate mapToStream converter based on the map's type. + switch (map.getMapType()) { + case MapType::kHashedBlocks: + return io::mapToStream(static_cast(map), ostream); + case MapType::kWaveletOctree: + return io::mapToStream(static_cast(map), ostream); + case MapType::kHashedWaveletOctree: + return io::mapToStream(static_cast(map), + ostream); + case MapType::kHashedChunkedWaveletOctree: + return io::mapToStream( + static_cast(map), ostream); + case MapType::kLayeredHashedWaveletOctree: + LOG(WARNING) << "Layered hashed wavelet octree maps require typed IO " + "with an explicit cell data serializer."; + return false; + case MapType::kOctree: + default: + LOG(WARNING) << "Could not serialize requested map to stream. " + "Map type not yet supported."; + return false; } - - LOG(WARNING) << "Could not serialize requested map to stream. " - "Map type not yet supported."; - return false; } bool streamToMap(std::istream& istream, MapBase::Ptr& map) { From 29e97451d0933e5959548118f39a727aee3a74e7 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Guilherne=20Caba=C3=A7o?= Date: Thu, 11 Jun 2026 18:17:50 +0000 Subject: [PATCH 05/14] Add layered wavemap ROS message support --- examples/cpp/digital_twin/CMakeLists.txt | 40 ++- .../common/layered_ros_converter.h | 79 ++++++ .../{ => common}/layered_voxel_config.h | 0 .../{ => io}/layered_map_io_experiment.cc | 2 +- .../{ => io}/legacy_file_io_experiment.cc | 0 ...legacy_hashed_wavelet_octree_experiment.cc | 0 .../{ => map_api}/voxel_block_experiment.cc | 2 +- .../{ => map_api}/voxel_map_experiment.cc | 2 +- .../blocks_experiment.cc | 0 .../{ => old_experiments}/experiment.cc | 0 .../partial_save_experiment.cc | 0 .../publish_experiment.cc | 0 .../ros/layered_ros_msg_experiment.cc | 71 +++++ .../ros/publish_layered_map_experiment.cc | 50 ++++ .../ros/subscribe_layered_map_experiment.cc | 50 ++++ .../{ => voxel_data}/new_voxel.cc | 2 +- .../{ => voxel_data}/voxel_haar_transform.cc | 2 +- interfaces/ros1/wavemap_msgs/CMakeLists.txt | 4 + interfaces/ros1/wavemap_msgs/msg/Layer.msg | 6 + .../msg/LayeredHashedWaveletOctree.msg | 12 + .../msg/LayeredHashedWaveletOctreeBlock.msg | 6 + .../msg/LayeredWaveletOctreeNode.msg | 3 + interfaces/ros1/wavemap_msgs/msg/Map.msg | 1 + .../impl/layered_map_msg_conversions_inl.h | 248 ++++++++++++++++++ .../map_msg_conversions.h | 20 ++ 25 files changed, 584 insertions(+), 16 deletions(-) create mode 100644 examples/cpp/digital_twin/common/layered_ros_converter.h rename examples/cpp/digital_twin/{ => common}/layered_voxel_config.h (100%) rename examples/cpp/digital_twin/{ => io}/layered_map_io_experiment.cc (98%) rename examples/cpp/digital_twin/{ => io}/legacy_file_io_experiment.cc (100%) rename examples/cpp/digital_twin/{ => io}/legacy_hashed_wavelet_octree_experiment.cc (100%) rename examples/cpp/digital_twin/{ => map_api}/voxel_block_experiment.cc (97%) rename examples/cpp/digital_twin/{ => map_api}/voxel_map_experiment.cc (96%) rename examples/cpp/digital_twin/{ => old_experiments}/blocks_experiment.cc (100%) rename examples/cpp/digital_twin/{ => old_experiments}/experiment.cc (100%) rename examples/cpp/digital_twin/{ => old_experiments}/partial_save_experiment.cc (100%) rename examples/cpp/digital_twin/{ => old_experiments}/publish_experiment.cc (100%) create mode 100644 examples/cpp/digital_twin/ros/layered_ros_msg_experiment.cc create mode 100644 examples/cpp/digital_twin/ros/publish_layered_map_experiment.cc create mode 100644 examples/cpp/digital_twin/ros/subscribe_layered_map_experiment.cc rename examples/cpp/digital_twin/{ => voxel_data}/new_voxel.cc (95%) rename examples/cpp/digital_twin/{ => voxel_data}/voxel_haar_transform.cc (96%) create mode 100644 interfaces/ros1/wavemap_msgs/msg/Layer.msg create mode 100644 interfaces/ros1/wavemap_msgs/msg/LayeredHashedWaveletOctree.msg create mode 100644 interfaces/ros1/wavemap_msgs/msg/LayeredHashedWaveletOctreeBlock.msg create mode 100644 interfaces/ros1/wavemap_msgs/msg/LayeredWaveletOctreeNode.msg create mode 100644 interfaces/ros1/wavemap_ros_conversions/include/wavemap_ros_conversions/impl/layered_map_msg_conversions_inl.h diff --git a/examples/cpp/digital_twin/CMakeLists.txt b/examples/cpp/digital_twin/CMakeLists.txt index f2c584372..f3054db5a 100644 --- a/examples/cpp/digital_twin/CMakeLists.txt +++ b/examples/cpp/digital_twin/CMakeLists.txt @@ -1,32 +1,32 @@ # Binaries -add_executable(new_voxel new_voxel.cc) +add_executable(new_voxel voxel_data/new_voxel.cc) set_wavemap_target_properties(new_voxel) target_link_libraries(new_voxel PUBLIC wavemap::wavemap_core) -add_executable(voxel_haar_transform voxel_haar_transform.cc) +add_executable(voxel_haar_transform voxel_data/voxel_haar_transform.cc) set_wavemap_target_properties(voxel_haar_transform) target_link_libraries(voxel_haar_transform PUBLIC wavemap::wavemap_core) -add_executable(voxel_block_experiment voxel_block_experiment.cc) +add_executable(voxel_block_experiment map_api/voxel_block_experiment.cc) set_wavemap_target_properties(voxel_block_experiment) target_link_libraries(voxel_block_experiment PUBLIC wavemap::wavemap_core) -add_executable(voxel_map_experiment voxel_map_experiment.cc) +add_executable(voxel_map_experiment map_api/voxel_map_experiment.cc) set_wavemap_target_properties(voxel_map_experiment) target_link_libraries(voxel_map_experiment PUBLIC wavemap::wavemap_core) add_executable(legacy_hashed_wavelet_octree_experiment - legacy_hashed_wavelet_octree_experiment.cc) + io/legacy_hashed_wavelet_octree_experiment.cc) set_wavemap_target_properties(legacy_hashed_wavelet_octree_experiment) target_link_libraries(legacy_hashed_wavelet_octree_experiment PUBLIC wavemap::wavemap_core) -add_executable(legacy_file_io_experiment legacy_file_io_experiment.cc) +add_executable(legacy_file_io_experiment io/legacy_file_io_experiment.cc) set_wavemap_target_properties(legacy_file_io_experiment) target_link_libraries(legacy_file_io_experiment PUBLIC wavemap::wavemap_core wavemap::wavemap_io) -add_executable(layered_map_io_experiment layered_map_io_experiment.cc) +add_executable(layered_map_io_experiment io/layered_map_io_experiment.cc) set_wavemap_target_properties(layered_map_io_experiment) target_link_libraries(layered_map_io_experiment PUBLIC wavemap::wavemap_core) @@ -34,29 +34,47 @@ if ("$ENV{ROS_VERSION}" STREQUAL "1") find_package(catkin REQUIRED COMPONENTS roscpp wavemap wavemap_msgs wavemap_ros_conversions) - add_executable(experiment experiment.cc) + add_executable(experiment old_experiments/experiment.cc) set_wavemap_target_properties(experiment) target_include_directories(experiment PUBLIC ${catkin_INCLUDE_DIRS}) target_link_libraries(experiment PUBLIC wavemap::wavemap_core wavemap::wavemap_io ${catkin_LIBRARIES}) - add_executable(publish_experiment publish_experiment.cc) + add_executable(publish_experiment old_experiments/publish_experiment.cc) set_wavemap_target_properties(publish_experiment) target_include_directories(publish_experiment PUBLIC ${catkin_INCLUDE_DIRS}) target_link_libraries(publish_experiment PUBLIC wavemap::wavemap_core wavemap::wavemap_io ${catkin_LIBRARIES}) - add_executable(blocks_experiment blocks_experiment.cc) + add_executable(blocks_experiment old_experiments/blocks_experiment.cc) set_wavemap_target_properties(blocks_experiment) target_include_directories(blocks_experiment PUBLIC ${catkin_INCLUDE_DIRS}) target_link_libraries(blocks_experiment PUBLIC wavemap::wavemap_core wavemap::wavemap_io ${catkin_LIBRARIES}) - add_executable(partial_save_experiment partial_save_experiment.cc) + add_executable(partial_save_experiment old_experiments/partial_save_experiment.cc) set_wavemap_target_properties(partial_save_experiment) target_include_directories(partial_save_experiment PUBLIC ${catkin_INCLUDE_DIRS}) target_link_libraries(partial_save_experiment PUBLIC wavemap::wavemap_core wavemap::wavemap_io ${catkin_LIBRARIES}) + + add_executable(layered_ros_msg_experiment ros/layered_ros_msg_experiment.cc) + set_wavemap_target_properties(layered_ros_msg_experiment) + target_include_directories(layered_ros_msg_experiment PUBLIC ${catkin_INCLUDE_DIRS}) + target_link_libraries(layered_ros_msg_experiment PUBLIC + wavemap::wavemap_core ${catkin_LIBRARIES}) + + add_executable(publish_layered_map_experiment ros/publish_layered_map_experiment.cc) + set_wavemap_target_properties(publish_layered_map_experiment) + target_include_directories(publish_layered_map_experiment PUBLIC ${catkin_INCLUDE_DIRS}) + target_link_libraries(publish_layered_map_experiment PUBLIC + wavemap::wavemap_core ${catkin_LIBRARIES}) + + add_executable(subscribe_layered_map_experiment ros/subscribe_layered_map_experiment.cc) + set_wavemap_target_properties(subscribe_layered_map_experiment) + target_include_directories(subscribe_layered_map_experiment PUBLIC ${catkin_INCLUDE_DIRS}) + target_link_libraries(subscribe_layered_map_experiment PUBLIC + wavemap::wavemap_core ${catkin_LIBRARIES}) else () message(STATUS "Skipping ROS digital_twin examples because ROS1 is not active.") endif () diff --git a/examples/cpp/digital_twin/common/layered_ros_converter.h b/examples/cpp/digital_twin/common/layered_ros_converter.h new file mode 100644 index 000000000..ce525268e --- /dev/null +++ b/examples/cpp/digital_twin/common/layered_ros_converter.h @@ -0,0 +1,79 @@ +#ifndef WAVEMAP_EXAMPLES_CPP_DIGITAL_TWIN_LAYERED_ROS_CONVERTER_H_ +#define WAVEMAP_EXAMPLES_CPP_DIGITAL_TWIN_LAYERED_ROS_CONVERTER_H_ + +#include +#include + +#include + +#include "layered_voxel_config.h" + +struct LayeredVoxelRosConverter { + static std::vector layerNames() { + // These names describe the custom data carried inside each LayeredVoxel. + return {"color", "traversability"}; + } + + static std::vector layerTypes() { + // The type strings tell consumers how to interpret each layer's arrays. + return {"float32_rgb", "float32"}; + } + + static std::vector makeLayers() { + // The message stores custom values layer-major: all color values go into the color layer, and all traversability values go into the traversability layer. + wavemap_msgs::Layer color_layer; + color_layer.name = "color"; + color_layer.type = "float32_rgb"; + + wavemap_msgs::Layer traversability_layer; + traversability_layer.name = "traversability"; + traversability_layer.type = "float32"; + + return {color_layer, traversability_layer}; + } + + static void appendLayerValues(const LayeredVoxel& voxel, std::vector& layers) { + // Color uses three float values per voxel/coefficient: r, g, and b. + layers[0].float32_values.emplace_back(voxel.data.r); + layers[0].float32_values.emplace_back(voxel.data.g); + layers[0].float32_values.emplace_back(voxel.data.b); + + // Traversability uses one float value per voxel/coefficient. + layers[1].float32_values.emplace_back(voxel.data.traversability); + } + + static LayeredVoxel readCellData(wavemap::FloatingPoint occupancy, const std::vector& layers, size_t value_index) { + // The value_index selects which voxel/coefficient is being reconstructed inside each layer-major array. + const size_t color_index = 3u * value_index; + return LayeredVoxel(occupancy, LayeredData{layers[0].float32_values[color_index], layers[0].float32_values[color_index + 1u], layers[0].float32_values[color_index + 2u], layers[1].float32_values[value_index]}); + } +}; + +inline void populateLayeredCube(LayeredMap& map, std::vector& populated_indices) { + const wavemap::Index3D cube_origin(8, 2, 2); + constexpr int cube_size_x = 2; + constexpr int cube_size_y = 2; + constexpr int cube_size_z = 4; + + size_t populated_voxels = 0u; + populated_indices.reserve(cube_size_x * cube_size_y * cube_size_z); + + // Populate a small cube so conversions and publishing touch several wavelet coefficients instead of a single trivial voxel. + for (int dx = 0; dx < cube_size_x; ++dx) { + for (int dy = 0; dy < cube_size_y; ++dy) { + for (int dz = 0; dz < cube_size_z; ++dz) { + const wavemap::Index3D voxel_index = cube_origin + wavemap::Index3D(dx, dy, dz); + const wavemap::FloatingPoint layer_value = static_cast(populated_voxels); + const LayeredVoxel original_voxel(0.5f + 0.02f * layer_value, LayeredData{0.1f * dx, 0.1f * dy, 0.05f * dz, 0.9f - 0.03f * layer_value}); + const LayeredVoxel voxel_update(0.1f, LayeredData{0.01f, 0.02f, 0.03f, -0.01f}); + + map.setVoxelValue(voxel_index, original_voxel); + map.addToVoxelValue(voxel_index, voxel_update); + populated_indices.emplace_back(voxel_index); + ++populated_voxels; + } + } + } +} + +#endif // WAVEMAP_EXAMPLES_CPP_DIGITAL_TWIN_LAYERED_ROS_CONVERTER_H_ diff --git a/examples/cpp/digital_twin/layered_voxel_config.h b/examples/cpp/digital_twin/common/layered_voxel_config.h similarity index 100% rename from examples/cpp/digital_twin/layered_voxel_config.h rename to examples/cpp/digital_twin/common/layered_voxel_config.h diff --git a/examples/cpp/digital_twin/layered_map_io_experiment.cc b/examples/cpp/digital_twin/io/layered_map_io_experiment.cc similarity index 98% rename from examples/cpp/digital_twin/layered_map_io_experiment.cc rename to examples/cpp/digital_twin/io/layered_map_io_experiment.cc index 47b2badd4..2b37b97c3 100644 --- a/examples/cpp/digital_twin/layered_map_io_experiment.cc +++ b/examples/cpp/digital_twin/io/layered_map_io_experiment.cc @@ -4,7 +4,7 @@ #include -#include "layered_voxel_config.h" +#include "../common/layered_voxel_config.h" int main() { wavemap::HashedWaveletOctreeConfig config; diff --git a/examples/cpp/digital_twin/legacy_file_io_experiment.cc b/examples/cpp/digital_twin/io/legacy_file_io_experiment.cc similarity index 100% rename from examples/cpp/digital_twin/legacy_file_io_experiment.cc rename to examples/cpp/digital_twin/io/legacy_file_io_experiment.cc diff --git a/examples/cpp/digital_twin/legacy_hashed_wavelet_octree_experiment.cc b/examples/cpp/digital_twin/io/legacy_hashed_wavelet_octree_experiment.cc similarity index 100% rename from examples/cpp/digital_twin/legacy_hashed_wavelet_octree_experiment.cc rename to examples/cpp/digital_twin/io/legacy_hashed_wavelet_octree_experiment.cc diff --git a/examples/cpp/digital_twin/voxel_block_experiment.cc b/examples/cpp/digital_twin/map_api/voxel_block_experiment.cc similarity index 97% rename from examples/cpp/digital_twin/voxel_block_experiment.cc rename to examples/cpp/digital_twin/map_api/voxel_block_experiment.cc index bea945681..60489cd76 100644 --- a/examples/cpp/digital_twin/voxel_block_experiment.cc +++ b/examples/cpp/digital_twin/map_api/voxel_block_experiment.cc @@ -1,6 +1,6 @@ #include -#include "layered_voxel_config.h" +#include "../common/layered_voxel_config.h" int main() { constexpr wavemap::IndexElement tree_height = 3; diff --git a/examples/cpp/digital_twin/voxel_map_experiment.cc b/examples/cpp/digital_twin/map_api/voxel_map_experiment.cc similarity index 96% rename from examples/cpp/digital_twin/voxel_map_experiment.cc rename to examples/cpp/digital_twin/map_api/voxel_map_experiment.cc index e886aa3f4..4d2738d11 100644 --- a/examples/cpp/digital_twin/voxel_map_experiment.cc +++ b/examples/cpp/digital_twin/map_api/voxel_map_experiment.cc @@ -1,6 +1,6 @@ #include -#include "layered_voxel_config.h" +#include "../common/layered_voxel_config.h" int main() { wavemap::HashedWaveletOctreeConfig config; diff --git a/examples/cpp/digital_twin/blocks_experiment.cc b/examples/cpp/digital_twin/old_experiments/blocks_experiment.cc similarity index 100% rename from examples/cpp/digital_twin/blocks_experiment.cc rename to examples/cpp/digital_twin/old_experiments/blocks_experiment.cc diff --git a/examples/cpp/digital_twin/experiment.cc b/examples/cpp/digital_twin/old_experiments/experiment.cc similarity index 100% rename from examples/cpp/digital_twin/experiment.cc rename to examples/cpp/digital_twin/old_experiments/experiment.cc diff --git a/examples/cpp/digital_twin/partial_save_experiment.cc b/examples/cpp/digital_twin/old_experiments/partial_save_experiment.cc similarity index 100% rename from examples/cpp/digital_twin/partial_save_experiment.cc rename to examples/cpp/digital_twin/old_experiments/partial_save_experiment.cc diff --git a/examples/cpp/digital_twin/publish_experiment.cc b/examples/cpp/digital_twin/old_experiments/publish_experiment.cc similarity index 100% rename from examples/cpp/digital_twin/publish_experiment.cc rename to examples/cpp/digital_twin/old_experiments/publish_experiment.cc diff --git a/examples/cpp/digital_twin/ros/layered_ros_msg_experiment.cc b/examples/cpp/digital_twin/ros/layered_ros_msg_experiment.cc new file mode 100644 index 000000000..28752b0ea --- /dev/null +++ b/examples/cpp/digital_twin/ros/layered_ros_msg_experiment.cc @@ -0,0 +1,71 @@ +#include +#include +#include + +#include + +#include "../common/layered_ros_converter.h" + +namespace { +bool almostEqual(float lhs, float rhs) { + return std::abs(lhs - rhs) < 1e-5f; +} + +bool almostEqual(const LayeredVoxel& lhs, const LayeredVoxel& rhs) { + return almostEqual(lhs.occupancy, rhs.occupancy) && almostEqual(lhs.data.r, rhs.data.r) && almostEqual(lhs.data.g, rhs.data.g) && almostEqual(lhs.data.b, rhs.data.b) && almostEqual(lhs.data.traversability, rhs.data.traversability); +} +} // namespace + +int main() { + wavemap::HashedWaveletOctreeConfig config; + config.min_cell_width = 0.1f; + config.min_log_odds = -2.f; + config.max_log_odds = 4.f; + config.tree_height = 3; + + LayeredMap map(config); + std::vector populated_indices; + populateLayeredCube(map, populated_indices); + + // Convert the layered C++ map into the standard wavemap_msgs::Map wrapper, matching the normal wavemap publishing path. + wavemap_msgs::Map msg; + if (!wavemap::convert::mapToRosMsg(map, "map", ros::Time(1, 0), msg)) { + std::cerr << "Failed to convert layered map to wavemap_msgs::Map.\n"; + return 1; + } + + // Convert the wrapped ROS message back into a fresh C++ map to validate the full wrapper round-trip. + LayeredMap::Ptr loaded_map; + if (!wavemap::convert::rosMsgToMap(msg, loaded_map)) { + std::cerr << "Failed to convert wavemap_msgs::Map back to a layered map.\n"; + return 1; + } + + if (!loaded_map) { + std::cerr << "Loaded map pointer is null.\n"; + return 1; + } + + size_t verified_voxels = 0u; + for (const wavemap::Index3D& voxel_index : populated_indices) { + const LayeredVoxel original_voxel = map.getVoxelValue(voxel_index); + const LayeredVoxel loaded_voxel = loaded_map->getVoxelValue(voxel_index); + if (almostEqual(original_voxel, loaded_voxel)) { + ++verified_voxels; + } else { + std::cerr << "Layered ROS voxel mismatch at index " << voxel_index.transpose() << "\n"; + printVoxel("Original", original_voxel); + printVoxel("Loaded", loaded_voxel); + } + } + + const wavemap::Index3D sample_voxel_index = wavemap::Index3D(8, 2, 2) + wavemap::Index3D(1, 1, 2); + printVoxel("Original sample voxel", map.getVoxelValue(sample_voxel_index)); + printVoxel("Loaded sample voxel", loaded_map->getVoxelValue(sample_voxel_index)); + std::cout << "ROS wrapper frame: " << msg.header.frame_id << "\n"; + std::cout << "ROS message layers: " << msg.layered_hashed_wavelet_octree.front().layer_names.size() << "\n"; + std::cout << "ROS message blocks: " << msg.layered_hashed_wavelet_octree.front().blocks.size() << "\n"; + std::cout << "Verified layered ROS voxels: " << verified_voxels << " / " << populated_indices.size() << "\n"; + + return verified_voxels == populated_indices.size() ? 0 : 1; +} diff --git a/examples/cpp/digital_twin/ros/publish_layered_map_experiment.cc b/examples/cpp/digital_twin/ros/publish_layered_map_experiment.cc new file mode 100644 index 000000000..c64d24d1c --- /dev/null +++ b/examples/cpp/digital_twin/ros/publish_layered_map_experiment.cc @@ -0,0 +1,50 @@ +#include +#include + +#include +#include + +#include "../common/layered_ros_converter.h" + +int main(int argc, char** argv) { + ros::init(argc, argv, "wavemap_publish_layered_map_experiment"); + ros::NodeHandle nh; + + wavemap::HashedWaveletOctreeConfig config; + config.min_cell_width = 0.1f; + config.min_log_odds = -2.f; + config.max_log_odds = 4.f; + config.tree_height = 3; + + LayeredMap map(config); + std::vector populated_indices; + populateLayeredCube(map, populated_indices); + + const std::string map_topic = "/wavemap/map"; + const std::string map_frame = "map"; + const int queue_size = 1; + const bool latch = true; + + // Publish through the same wrapper type used by the existing wavemap ROS path. + ros::Publisher map_pub = nh.advertise(map_topic, queue_size, latch); + ros::Duration(0.5).sleep(); + + wavemap_msgs::Map map_msg; + if (!wavemap::convert::mapToRosMsg(map, map_frame, ros::Time::now(), map_msg)) { + std::cerr << "Failed to convert layered map to wavemap_msgs::Map.\n"; + return 1; + } + + // The message carries the normal ROS header plus the layered map payload in layered_hashed_wavelet_octree. + map_pub.publish(map_msg); + ros::spinOnce(); + + std::cout << "Published layered map on: " << map_topic << "\n"; + std::cout << "Frame: " << map_frame << "\n"; + std::cout << "Populated voxels: " << populated_indices.size() << "\n"; + std::cout << "Layer count: " << map_msg.layered_hashed_wavelet_octree.front().layer_names.size() << "\n"; + std::cout << "Block count: " << map_msg.layered_hashed_wavelet_octree.front().blocks.size() << "\n"; + + ros::spin(); + return 0; +} diff --git a/examples/cpp/digital_twin/ros/subscribe_layered_map_experiment.cc b/examples/cpp/digital_twin/ros/subscribe_layered_map_experiment.cc new file mode 100644 index 000000000..374d46cad --- /dev/null +++ b/examples/cpp/digital_twin/ros/subscribe_layered_map_experiment.cc @@ -0,0 +1,50 @@ +#include + +#include +#include + +#include "../common/layered_ros_converter.h" + +namespace { +void layeredMapCallback(const wavemap_msgs::Map::ConstPtr& msg) { + // Convert the standard wavemap ROS wrapper back into the custom layered C++ map type. + LayeredMap::Ptr map; + if (!wavemap::convert::rosMsgToMap(*msg, map)) { + std::cerr << "Failed to convert wavemap_msgs::Map back to a layered map.\n"; + return; + } + + if (!map) { + std::cerr << "Converted layered map pointer is null.\n"; + return; + } + + // This index is one of the voxels populated by publish_layered_map_experiment. + const wavemap::Index3D sample_voxel_index = wavemap::Index3D(8, 2, 2) + wavemap::Index3D(1, 1, 2); + const LayeredVoxel sample_voxel = map->getVoxelValue(sample_voxel_index); + + std::cout << "Received layered map frame: " << msg->header.frame_id << "\n"; + std::cout << "Received layered map stamp: " << msg->header.stamp << "\n"; + std::cout << "Block count: " << map->getHashMap().size() << "\n"; + std::cout << "Layer count: " << msg->layered_hashed_wavelet_octree.front().layer_names.size() << "\n"; + printVoxel("Received sample voxel", sample_voxel); + + // The example only needs one message to prove the subscriber path works. + ros::shutdown(); +} +} // namespace + +int main(int argc, char** argv) { + ros::init(argc, argv, "wavemap_subscribe_layered_map_experiment"); + ros::NodeHandle nh; + + const std::string map_topic = "/wavemap/map"; + const int queue_size = 1; + + // Subscribe to the same topic used by the existing wavemap map publisher path. + ros::Subscriber map_sub = nh.subscribe(map_topic, queue_size, layeredMapCallback); + + std::cout << "Waiting for layered map on: " << map_topic << "\n"; + ros::spin(); + return 0; +} diff --git a/examples/cpp/digital_twin/new_voxel.cc b/examples/cpp/digital_twin/voxel_data/new_voxel.cc similarity index 95% rename from examples/cpp/digital_twin/new_voxel.cc rename to examples/cpp/digital_twin/voxel_data/new_voxel.cc index 0c0dd563e..b3a4fd593 100644 --- a/examples/cpp/digital_twin/new_voxel.cc +++ b/examples/cpp/digital_twin/voxel_data/new_voxel.cc @@ -2,7 +2,7 @@ #include -#include "layered_voxel_config.h" +#include "../common/layered_voxel_config.h" int main() { wavemap::VoxelData<> occupancy_only; diff --git a/examples/cpp/digital_twin/voxel_haar_transform.cc b/examples/cpp/digital_twin/voxel_data/voxel_haar_transform.cc similarity index 96% rename from examples/cpp/digital_twin/voxel_haar_transform.cc rename to examples/cpp/digital_twin/voxel_data/voxel_haar_transform.cc index 7cd38a546..5ca8db04a 100644 --- a/examples/cpp/digital_twin/voxel_haar_transform.cc +++ b/examples/cpp/digital_twin/voxel_data/voxel_haar_transform.cc @@ -3,7 +3,7 @@ #include #include -#include "layered_voxel_config.h" +#include "../common/layered_voxel_config.h" namespace { using Coefficients = wavemap::HaarCoefficients; diff --git a/interfaces/ros1/wavemap_msgs/CMakeLists.txt b/interfaces/ros1/wavemap_msgs/CMakeLists.txt index 58dba387c..cb43f9ccf 100644 --- a/interfaces/ros1/wavemap_msgs/CMakeLists.txt +++ b/interfaces/ros1/wavemap_msgs/CMakeLists.txt @@ -11,6 +11,10 @@ add_message_files(FILES HashedWaveletOctree.msg HashedWaveletOctreeBlock.msg Index3D.msg + LayeredHashedWaveletOctree.msg + LayeredHashedWaveletOctreeBlock.msg + LayeredWaveletOctreeNode.msg + Layer.msg Map.msg OctreeNode.msg WaveletOctree.msg diff --git a/interfaces/ros1/wavemap_msgs/msg/Layer.msg b/interfaces/ros1/wavemap_msgs/msg/Layer.msg new file mode 100644 index 000000000..8e5f5d41e --- /dev/null +++ b/interfaces/ros1/wavemap_msgs/msg/Layer.msg @@ -0,0 +1,6 @@ +string name +string type + +float32[] float32_values +int32[] int32_values +uint8[] uint8_values diff --git a/interfaces/ros1/wavemap_msgs/msg/LayeredHashedWaveletOctree.msg b/interfaces/ros1/wavemap_msgs/msg/LayeredHashedWaveletOctree.msg new file mode 100644 index 000000000..fc11851b1 --- /dev/null +++ b/interfaces/ros1/wavemap_msgs/msg/LayeredHashedWaveletOctree.msg @@ -0,0 +1,12 @@ +float32 min_cell_width +float32 min_log_odds +float32 max_log_odds + +int32 tree_height + +string[] layer_names +string[] layer_types + +Index3D[] allocated_block_indices + +LayeredHashedWaveletOctreeBlock[] blocks diff --git a/interfaces/ros1/wavemap_msgs/msg/LayeredHashedWaveletOctreeBlock.msg b/interfaces/ros1/wavemap_msgs/msg/LayeredHashedWaveletOctreeBlock.msg new file mode 100644 index 000000000..b552eee9b --- /dev/null +++ b/interfaces/ros1/wavemap_msgs/msg/LayeredHashedWaveletOctreeBlock.msg @@ -0,0 +1,6 @@ +Index3D root_node_offset + +float32 root_node_occupancy_scale_coefficient +Layer[] root_node_layers + +LayeredWaveletOctreeNode[] nodes diff --git a/interfaces/ros1/wavemap_msgs/msg/LayeredWaveletOctreeNode.msg b/interfaces/ros1/wavemap_msgs/msg/LayeredWaveletOctreeNode.msg new file mode 100644 index 000000000..cc91939c7 --- /dev/null +++ b/interfaces/ros1/wavemap_msgs/msg/LayeredWaveletOctreeNode.msg @@ -0,0 +1,3 @@ +float32[8] occupancy_detail_coefficients +Layer[] detail_layers +uint8 allocated_children_bitset diff --git a/interfaces/ros1/wavemap_msgs/msg/Map.msg b/interfaces/ros1/wavemap_msgs/msg/Map.msg index 41db3abe1..67cea39cf 100644 --- a/interfaces/ros1/wavemap_msgs/msg/Map.msg +++ b/interfaces/ros1/wavemap_msgs/msg/Map.msg @@ -3,3 +3,4 @@ Header header HashedBlocks[] hashed_blocks WaveletOctree[] wavelet_octree HashedWaveletOctree[] hashed_wavelet_octree +LayeredHashedWaveletOctree[] layered_hashed_wavelet_octree diff --git a/interfaces/ros1/wavemap_ros_conversions/include/wavemap_ros_conversions/impl/layered_map_msg_conversions_inl.h b/interfaces/ros1/wavemap_ros_conversions/include/wavemap_ros_conversions/impl/layered_map_msg_conversions_inl.h new file mode 100644 index 000000000..d06474f69 --- /dev/null +++ b/interfaces/ros1/wavemap_ros_conversions/include/wavemap_ros_conversions/impl/layered_map_msg_conversions_inl.h @@ -0,0 +1,248 @@ +#ifndef WAVEMAP_ROS_CONVERSIONS_IMPL_LAYERED_MAP_MSG_CONVERSIONS_INL_H_ +#define WAVEMAP_ROS_CONVERSIONS_IMPL_LAYERED_MAP_MSG_CONVERSIONS_INL_H_ + +#include + +#include +#include +#include + +namespace wavemap::convert { +template +bool mapToRosMsg(const HashedWaveletOctreeT& map, const std::string& frame_id, const ros::Time& stamp, wavemap_msgs::Map& msg, std::optional> include_blocks, std::shared_ptr thread_pool) { + ProfilerZoneScoped; + // Layered equivalent of the existing MapBase wrapper + // Fills the common ROS header and stores the typed map inside wavemap_msgs::Map. + msg.header.stamp = stamp; + msg.header.frame_id = frame_id; + + // The custom voxel type needs the user-provided converter + auto& layered_map_msg = msg.layered_hashed_wavelet_octree.emplace_back(); + convert::mapToRosMsg(map, layered_map_msg, std::move(include_blocks), std::move(thread_pool)); + return true; +} + +template +void mapToRosMsg(const HashedWaveletOctreeT& map, wavemap_msgs::LayeredHashedWaveletOctree& msg, std::optional> include_blocks, std::shared_ptr thread_pool) { + ProfilerZoneScoped; + // Use a small margin when deciding whether child nodes are saturated + constexpr FloatingPoint kNumericalNoise = 1e-3f; + const auto min_log_odds = map.getMinLogOdds() + kNumericalNoise; + const auto max_log_odds = map.getMaxLogOdds() - kNumericalNoise; + + // Copy the map metadata first + msg.min_cell_width = map.getMinCellWidth(); + msg.min_log_odds = map.getMinLogOdds(); + msg.max_log_odds = map.getMaxLogOdds(); + msg.tree_height = map.getTreeHeight(); + msg.layer_names = CellDataRosConverterT::layerNames(); + msg.layer_types = CellDataRosConverterT::layerTypes(); + + // Always publish the full list of allocated block indices + msg.allocated_block_indices.reserve(map.getHashMap().size()); + map.forEachBlock([&msg](const Index3D& block_index, const auto& /*block*/) { + auto& block_index_msg = msg.allocated_block_indices.emplace_back(); + block_index_msg.x = block_index.x(); + block_index_msg.y = block_index.y(); + block_index_msg.z = block_index.z(); + }); + + // If the caller requested a subset of blocks, discard indices that are not currently allocated + if (include_blocks) { + for (auto include_block_it = include_blocks->begin(); + include_block_it != include_blocks->end();) { + if (map.hasBlock(*include_block_it)) { + ++include_block_it; + } else { + include_block_it = include_blocks->erase(include_block_it); + } + } + } else { + include_blocks.emplace(); + map.forEachBlock( + [&include_blocks](const Index3D& block_index, const auto& /*block*/) { + include_blocks->emplace(block_index); + }); + } + + int block_idx = 0; + msg.blocks.resize(include_blocks->size()); + for (const auto& block_index : include_blocks.value()) { + if (const auto* block = map.getBlock(block_index); block) { + auto& block_msg = msg.blocks[block_idx++]; + // Block conversion is independent per block, + // so it can safely run in the optional thread pool when the caller has one available. + if (thread_pool) { + thread_pool->add_task([block_index, block, min_log_odds, max_log_odds, + &block_msg]() { + blockToRosMsg( + block_index, *block, min_log_odds, max_log_odds, block_msg); + }); + } else { + blockToRosMsg( + block_index, *block, min_log_odds, max_log_odds, block_msg); + } + } else { + ROS_ERROR("Block index not found. This should never happen."); + } + } + if (thread_pool) { + thread_pool->wait_all(); + } +} + +template +void blockToRosMsg( + const typename HashedWaveletOctreeT::BlockIndex& block_index, + const typename HashedWaveletOctreeT::Block& block, + FloatingPoint min_log_odds, FloatingPoint max_log_odds, + wavemap_msgs::LayeredHashedWaveletOctreeBlock& msg) { + using Block = typename HashedWaveletOctreeT::Block; + using NodeConstRefType = typename Block::OctreeType::NodeConstRefType; + + struct StackElement { + const CellDataT scale; + NodeConstRefType node; + }; + + // Store the block position and root scale separately from the internal wavelet nodes + // Occupancy is kept in the explicit occupancy field, while extra layers are encoded through the user-provided converter + msg.root_node_offset.x = block_index.x(); + msg.root_node_offset.y = block_index.y(); + msg.root_node_offset.z = block_index.z(); + msg.root_node_occupancy_scale_coefficient = static_cast(block.getRootScale()); + msg.root_node_layers = CellDataRosConverterT::makeLayers(); + CellDataRosConverterT::appendLayerValues(block.getRootScale(), msg.root_node_layers); + + // Traverse the block's wavelet tree without recursion + // Each stack entry carries the node and its reconstructed scale coefficient, to decide whether the children are saturated and should be serialized + std::stack stack; + stack.emplace(StackElement{block.getRootScale(), block.getRootNode()}); + while (!stack.empty()) { + const CellDataT scale = stack.top().scale; + const auto& node = stack.top().node; + stack.pop(); + + auto& node_msg = msg.nodes.emplace_back(); + node_msg.detail_layers = CellDataRosConverterT::makeLayers(); + size_t coefficient_idx = 0u; + // Each wavelet node stores eight detail coefficients. + // Occupancy is copied into the fixed array; all custom layer values are appended layer-major into detail_layers by CellDataRosConverterT + for (const CellDataT& coefficient : node.data()) { + node_msg.occupancy_detail_coefficients[coefficient_idx++] = + static_cast(coefficient); + CellDataRosConverterT::appendLayerValues(coefficient, + node_msg.detail_layers); + } + + // Reconstruct child scales from the parent scale and detail coefficients + // Only children whose occupancy is not saturated need to be sent further + const auto child_scales = Block::Transform::backward({scale, node.data()}); + for (int relative_child_idx = OctreeIndex::kNumChildren - 1; + 0 <= relative_child_idx; --relative_child_idx) { + const auto child_scale = child_scales[relative_child_idx]; + const FloatingPoint child_occupancy = + static_cast(child_scale); + if (child_occupancy < min_log_odds || max_log_odds < child_occupancy) { + continue; + } + + if (const auto* child = node.getChild(relative_child_idx); child) { + stack.emplace(StackElement{child_scale, *child}); + // The bitset records which children have serialized node data. + node_msg.allocated_children_bitset += (1 << relative_child_idx); + } + } + } +} + +template +void rosMsgToMap(const wavemap_msgs::LayeredHashedWaveletOctree& msg, + typename HashedWaveletOctreeT::Ptr& map) { + ProfilerZoneScoped; + // Rebuild the wavemap config from the message metadata + // If the destination map is null or incompatible, allocate a fresh layered map instance. + HashedWaveletOctreeConfig config; + config.min_cell_width = msg.min_cell_width; + config.min_log_odds = msg.min_log_odds; + config.max_log_odds = msg.max_log_odds; + config.tree_height = msg.tree_height; + + if (!map || map->getConfig() != config) { + map = std::make_shared>(config); + } + + // Synchronize allocated blocks with the message + // Blocks not listed in allocated_block_indices are removed from the destination map. + std::unordered_set allocated_blocks; + for (const auto& block_index : msg.allocated_block_indices) { + allocated_blocks.emplace(block_index.x, block_index.y, block_index.z); + } + map->eraseBlockIf( + [&allocated_blocks](const Index3D& block_index, const auto& /*block*/) { + return !allocated_blocks.count(block_index); + }); + + using Block = typename HashedWaveletOctreeT::Block; + using NodeType = typename Block::OctreeType::NodeType; + + for (const auto& block_msg : msg.blocks) { + // Allocate or fetch the target block and reconstruct its root voxel data from occupancy plus the serialized custom layers + const Index3D block_index{block_msg.root_node_offset.x, + block_msg.root_node_offset.y, + block_msg.root_node_offset.z}; + auto& block = map->getOrAllocateBlock(block_index); + block.getRootScale() = CellDataRosConverterT::readCellData( + block_msg.root_node_occupancy_scale_coefficient, + block_msg.root_node_layers, 0u); + + std::stack stack; + stack.emplace(&block.getRootNode()); + for (const auto& node_msg : block_msg.nodes) { + DCHECK(!stack.empty()); + NodeType* node = stack.top(); + stack.pop(); + + // Reconstruct each detail coefficient + // The converter receives the fixed occupancy coefficient and the layer-major custom data for the same coefficient index. + for (size_t coefficient_idx = 0u; coefficient_idx < node->data().size(); + ++coefficient_idx) { + node->data()[coefficient_idx] = CellDataRosConverterT::readCellData( + node_msg.occupancy_detail_coefficients[coefficient_idx], + node_msg.detail_layers, coefficient_idx); + } + + // Recreate only the children marked in the transmitted bitset + // Their node contents will be filled by subsequent node messages in stack traversal order. + for (int relative_child_idx = OctreeIndex::kNumChildren - 1; + 0 <= relative_child_idx; --relative_child_idx) { + const bool child_exists = bit_ops::is_bit_set( + node_msg.allocated_children_bitset, relative_child_idx); + if (child_exists) { + stack.emplace(&node->getOrAllocateChild(relative_child_idx)); + } + } + } + } +} + +template +bool rosMsgToMap(const wavemap_msgs::Map& msg, typename HashedWaveletOctreeT::Ptr& map) { + ProfilerZoneScoped; + // The typed layered wrapper accepts exactly one layered map and no legacy map payloads. This keeps the old generic MapBase path unchanged while avoiding ambiguous custom conversions. + const bool has_one_layered_map = msg.layered_hashed_wavelet_octree.size() == 1u; + const bool has_legacy_payload = !msg.hashed_blocks.empty() || !msg.wavelet_octree.empty() || !msg.hashed_wavelet_octree.empty(); + if (!has_one_layered_map || has_legacy_payload) { + ROS_WARN( + "Layered map ROS msg must contain exactly one layered hashed wavelet " + "octree and no legacy map payloads. Ignoring."); + map = nullptr; + return false; + } + + convert::rosMsgToMap(msg.layered_hashed_wavelet_octree.front(), map); + return static_cast(map); +} +} // namespace wavemap::convert + +#endif // WAVEMAP_ROS_CONVERSIONS_IMPL_LAYERED_MAP_MSG_CONVERSIONS_INL_H_ diff --git a/interfaces/ros1/wavemap_ros_conversions/include/wavemap_ros_conversions/map_msg_conversions.h b/interfaces/ros1/wavemap_ros_conversions/include/wavemap_ros_conversions/map_msg_conversions.h index 8d230c14f..8af2aff6b 100644 --- a/interfaces/ros1/wavemap_ros_conversions/include/wavemap_ros_conversions/map_msg_conversions.h +++ b/interfaces/ros1/wavemap_ros_conversions/include/wavemap_ros_conversions/map_msg_conversions.h @@ -15,6 +15,9 @@ #include #include #include +#include +#include +#include #include namespace wavemap::convert { @@ -50,6 +53,23 @@ void blockToRosMsg(const HashedChunkedWaveletOctree::BlockIndex& block_index, const HashedChunkedWaveletOctree::Block& block, FloatingPoint min_log_odds, FloatingPoint max_log_odds, wavemap_msgs::HashedWaveletOctreeBlock& msg); + +template +bool mapToRosMsg(const HashedWaveletOctreeT& map, const std::string& frame_id, const ros::Time& stamp, wavemap_msgs::Map& msg, std::optional> include_blocks = std::nullopt, std::shared_ptr thread_pool = nullptr); + +template +void mapToRosMsg(const HashedWaveletOctreeT& map, wavemap_msgs::LayeredHashedWaveletOctree& msg, std::optional> include_blocks = std::nullopt, std::shared_ptr thread_pool = nullptr); + +template +void blockToRosMsg(const typename HashedWaveletOctreeT::BlockIndex& block_index, const typename HashedWaveletOctreeT::Block& block, FloatingPoint min_log_odds, FloatingPoint max_log_odds, wavemap_msgs::LayeredHashedWaveletOctreeBlock& msg); + +template +void rosMsgToMap(const wavemap_msgs::LayeredHashedWaveletOctree& msg, typename HashedWaveletOctreeT::Ptr& map); + +template +bool rosMsgToMap(const wavemap_msgs::Map& msg, typename HashedWaveletOctreeT::Ptr& map); } // namespace wavemap::convert +#include "wavemap_ros_conversions/impl/layered_map_msg_conversions_inl.h" + #endif // WAVEMAP_ROS_CONVERSIONS_MAP_MSG_CONVERSIONS_H_ From f7682f10b57cf9af52205c9818d70d4edbe9654d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Guilherne=20Caba=C3=A7o?= Date: Wed, 17 Jun 2026 15:06:38 +0000 Subject: [PATCH 06/14] Add layered map visualization support to RViz plugin --- .../common/layered_ros_converter.h | 2 +- .../ros1/wavemap_rviz_plugin/CMakeLists.txt | 5 +- .../include/wavemap_rviz_plugin/common.h | 9 + .../wavemap_rviz_plugin/layered_map_factory.h | 59 ++++++ .../layered_map_interface.h | 124 ++++++++++++ .../wavemap_rviz_plugin/occupancy_query.h | 38 ++++ .../visuals/cell_selector.h | 8 +- .../visuals/voxel_visual.h | 16 ++ .../wavemap_rviz_plugin/wavemap_map_display.h | 13 ++ .../src/visuals/cell_selector.cc | 33 ++- .../src/visuals/voxel_visual.cc | 190 ++++++++++++++++++ .../src/wavemap_map_display.cc | 132 +++++++++++- 12 files changed, 614 insertions(+), 15 deletions(-) create mode 100644 interfaces/ros1/wavemap_rviz_plugin/include/wavemap_rviz_plugin/layered_map_factory.h create mode 100644 interfaces/ros1/wavemap_rviz_plugin/include/wavemap_rviz_plugin/layered_map_interface.h create mode 100644 interfaces/ros1/wavemap_rviz_plugin/include/wavemap_rviz_plugin/occupancy_query.h diff --git a/examples/cpp/digital_twin/common/layered_ros_converter.h b/examples/cpp/digital_twin/common/layered_ros_converter.h index ce525268e..0e3a3de4f 100644 --- a/examples/cpp/digital_twin/common/layered_ros_converter.h +++ b/examples/cpp/digital_twin/common/layered_ros_converter.h @@ -65,7 +65,7 @@ inline void populateLayeredCube(LayeredMap& map, std::vector& const wavemap::Index3D voxel_index = cube_origin + wavemap::Index3D(dx, dy, dz); const wavemap::FloatingPoint layer_value = static_cast(populated_voxels); const LayeredVoxel original_voxel(0.5f + 0.02f * layer_value, LayeredData{0.1f * dx, 0.1f * dy, 0.05f * dz, 0.9f - 0.03f * layer_value}); - const LayeredVoxel voxel_update(0.1f, LayeredData{0.01f, 0.02f, 0.03f, -0.01f}); + const LayeredVoxel voxel_update(0.1f, LayeredData{-0.01f, -0.01f, -0.01f, -0.01f}); map.setVoxelValue(voxel_index, original_voxel); map.addToVoxelValue(voxel_index, voxel_update); diff --git a/interfaces/ros1/wavemap_rviz_plugin/CMakeLists.txt b/interfaces/ros1/wavemap_rviz_plugin/CMakeLists.txt index d316cf978..b4dead116 100644 --- a/interfaces/ros1/wavemap_rviz_plugin/CMakeLists.txt +++ b/interfaces/ros1/wavemap_rviz_plugin/CMakeLists.txt @@ -14,7 +14,10 @@ catkin_package(CATKIN_DEPENDS wavemap wavemap_msgs wavemap_ros_conversions) # For all targets -include_directories(include SYSTEM ${catkin_INCLUDE_DIRS}) +include_directories( + include + ${CMAKE_CURRENT_SOURCE_DIR}/../../../examples/cpp/digital_twin/common + SYSTEM ${catkin_INCLUDE_DIRS}) # This setting causes Qt's "MOC" generation to happen automatically set(CMAKE_AUTOMOC ON) diff --git a/interfaces/ros1/wavemap_rviz_plugin/include/wavemap_rviz_plugin/common.h b/interfaces/ros1/wavemap_rviz_plugin/include/wavemap_rviz_plugin/common.h index 2a9093a3b..c7b2df946 100644 --- a/interfaces/ros1/wavemap_rviz_plugin/include/wavemap_rviz_plugin/common.h +++ b/interfaces/ros1/wavemap_rviz_plugin/include/wavemap_rviz_plugin/common.h @@ -2,12 +2,21 @@ #define WAVEMAP_RVIZ_PLUGIN_COMMON_H_ #include +#include +#include +#include #include +#include + +#include "wavemap_rviz_plugin/layered_map_interface.h" namespace wavemap::rviz_plugin { struct MapAndMutex { MapBase::Ptr map; + std::shared_ptr layered_map; + std::optional layered_map_msg; + std::string selected_layer_name = "occupancy"; std::mutex mutex; }; } // namespace wavemap::rviz_plugin diff --git a/interfaces/ros1/wavemap_rviz_plugin/include/wavemap_rviz_plugin/layered_map_factory.h b/interfaces/ros1/wavemap_rviz_plugin/include/wavemap_rviz_plugin/layered_map_factory.h new file mode 100644 index 000000000..78b874bf4 --- /dev/null +++ b/interfaces/ros1/wavemap_rviz_plugin/include/wavemap_rviz_plugin/layered_map_factory.h @@ -0,0 +1,59 @@ +#ifndef WAVEMAP_RVIZ_PLUGIN_LAYERED_MAP_FACTORY_H_ +#define WAVEMAP_RVIZ_PLUGIN_LAYERED_MAP_FACTORY_H_ + +#include +#include +#include + +#include +#include +#include + +#include "wavemap_rviz_plugin/layered_map_interface.h" + +namespace wavemap::rviz_plugin { +inline std::vector extractLayerMetadata( + const wavemap_msgs::LayeredHashedWaveletOctree& msg) { + std::vector layers; + const size_t num_layers = + std::min(msg.layer_names.size(), msg.layer_types.size()); + layers.reserve(num_layers); + for (size_t layer_idx = 0u; layer_idx < num_layers; ++layer_idx) { + layers.push_back({msg.layer_names[layer_idx], msg.layer_types[layer_idx]}); + } + return layers; +} + +// Runtime factory interface. The display owns factories through this base class, +// while each concrete factory knows how to rebuild one specific user voxel type. +class LayeredMapFactory { + public: + virtual ~LayeredMapFactory() = default; + + virtual std::shared_ptr tryCreate( + const wavemap_msgs::LayeredHashedWaveletOctree& msg) const = 0; +}; + +template +class TypedLayeredMapFactory : public LayeredMapFactory { + public: + std::shared_ptr tryCreate( + const wavemap_msgs::LayeredHashedWaveletOctree& msg) const override { + typename LayeredMapT::Ptr map; + convert::rosMsgToMap(msg, map); + if (!map) { + ROS_WARN("Failed to create typed layered map for RViz display."); + return nullptr; + } + + return std::make_shared< + HashedWaveletOctreeLayeredMapAdapter>( + map, extractLayerMetadata(msg)); + } +}; + +} // namespace wavemap::rviz_plugin + +#endif // WAVEMAP_RVIZ_PLUGIN_LAYERED_MAP_FACTORY_H_ diff --git a/interfaces/ros1/wavemap_rviz_plugin/include/wavemap_rviz_plugin/layered_map_interface.h b/interfaces/ros1/wavemap_rviz_plugin/include/wavemap_rviz_plugin/layered_map_interface.h new file mode 100644 index 000000000..e53b245fc --- /dev/null +++ b/interfaces/ros1/wavemap_rviz_plugin/include/wavemap_rviz_plugin/layered_map_interface.h @@ -0,0 +1,124 @@ +#ifndef WAVEMAP_RVIZ_PLUGIN_LAYERED_MAP_INTERFACE_H_ +#define WAVEMAP_RVIZ_PLUGIN_LAYERED_MAP_INTERFACE_H_ + +#include +#include +#include +#include +#include + +#include +#include +#include + +#include "wavemap_rviz_plugin/occupancy_query.h" + +namespace wavemap::rviz_plugin { +struct LayerMetadata { + std::string name; + std::string type; +}; + +// Runtime interface used by the RViz plugin for layered maps. Concrete user +// voxel types remain hidden behind this API, keeping the display code modular. +class LayeredMapInterface : public OccupancyQuery { + public: + using IndexedOccupancyVisitor = + std::function; + + ~LayeredMapInterface() override = default; + + virtual IndexElement getTreeHeight() const = 0; + virtual FloatingPoint getMinCellWidth() const = 0; + virtual const std::vector& getLayers() const = 0; + + virtual std::vector getBlockIndices() const = 0; + virtual bool hasBlock(const Index3D& block_index) const = 0; + virtual void forEachLeaf(const IndexedOccupancyVisitor& visitor_fn, + IndexElement termination_height = 0) const = 0; + virtual void forEachBlockLeaf(const Index3D& block_index, + const IndexedOccupancyVisitor& visitor_fn, + IndexElement termination_height = 0) const = 0; + virtual bool getLayerColor(const std::string& layer_name, + const OctreeIndex& index, + FloatingPoint occupancy, + Ogre::ColourValue& color) const = 0; +}; + +struct NoLayerColorProvider { + template + static bool getLayerColor(const std::string& /*layer_name*/, + const CellDataT& /*voxel*/, + FloatingPoint /*occupancy*/, + Ogre::ColourValue& /*color*/) { + return false; + } +}; + +template +class HashedWaveletOctreeLayeredMapAdapter : public LayeredMapInterface { + public: + HashedWaveletOctreeLayeredMapAdapter( + std::shared_ptr map, + std::vector layers) + : map_(std::move(map)), layers_(std::move(layers)) {} + + IndexElement getTreeHeight() const override { return map_->getTreeHeight(); } + + FloatingPoint getMinCellWidth() const override { + return map_->getMinCellWidth(); + } + + const std::vector& getLayers() const override { + return layers_; + } + + FloatingPoint getCellValue(const OctreeIndex& index) override { + return map_->getCellValue(index); + } + + std::vector getBlockIndices() const override { + std::vector block_indices; + block_indices.reserve(map_->getHashMap().size()); + map_->forEachBlock([&block_indices](const Index3D& block_index, + const auto& /*block*/) { + block_indices.push_back(block_index); + }); + return block_indices; + } + + bool hasBlock(const Index3D& block_index) const override { + return map_->hasBlock(block_index); + } + + void forEachLeaf(const IndexedOccupancyVisitor& visitor_fn, + IndexElement termination_height = 0) const override { + map_->forEachBlock([&visitor_fn, termination_height]( + const Index3D& block_index, const auto& block) { + block.forEachLeaf(block_index, visitor_fn, termination_height); + }); + } + + void forEachBlockLeaf(const Index3D& block_index, + const IndexedOccupancyVisitor& visitor_fn, + IndexElement termination_height = 0) const override { + if (const auto* block = map_->getBlock(block_index); block) { + block->forEachLeaf(block_index, visitor_fn, termination_height); + } + } + + bool getLayerColor(const std::string& layer_name, const OctreeIndex& index, + FloatingPoint occupancy, + Ogre::ColourValue& color) const override { + const auto voxel = map_->getVoxelValue(index); + return LayerColorProviderT::getLayerColor(layer_name, voxel, occupancy, + color); + } + + private: + std::shared_ptr map_; + std::vector layers_; +}; +} // namespace wavemap::rviz_plugin + +#endif // WAVEMAP_RVIZ_PLUGIN_LAYERED_MAP_INTERFACE_H_ diff --git a/interfaces/ros1/wavemap_rviz_plugin/include/wavemap_rviz_plugin/occupancy_query.h b/interfaces/ros1/wavemap_rviz_plugin/include/wavemap_rviz_plugin/occupancy_query.h new file mode 100644 index 000000000..bbe7c78b0 --- /dev/null +++ b/interfaces/ros1/wavemap_rviz_plugin/include/wavemap_rviz_plugin/occupancy_query.h @@ -0,0 +1,38 @@ +#ifndef WAVEMAP_RVIZ_PLUGIN_OCCUPANCY_QUERY_H_ +#define WAVEMAP_RVIZ_PLUGIN_OCCUPANCY_QUERY_H_ + +#include + +#include +#include +#include +#include + +namespace wavemap::rviz_plugin { +// Small runtime interface for map occupancy queries. The cell selector only +// needs this API, so it does not need to know whether the backing map is a +// legacy wavemap map or a future layered map adapter. +class OccupancyQuery { + public: + virtual ~OccupancyQuery() = default; + + virtual FloatingPoint getCellValue(const OctreeIndex& index) = 0; +}; + +class HashedWaveletOctreeOccupancyQuery : public OccupancyQuery { + public: + explicit HashedWaveletOctreeOccupancyQuery( + std::shared_ptr map) + : map_(std::move(map)), query_accelerator_(*map_) {} + + FloatingPoint getCellValue(const OctreeIndex& index) override { + return query_accelerator_.getCellValue(index); + } + + private: + std::shared_ptr map_; + QueryAccelerator query_accelerator_; +}; +} // namespace wavemap::rviz_plugin + +#endif // WAVEMAP_RVIZ_PLUGIN_OCCUPANCY_QUERY_H_ 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..02a5771c8 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 @@ -7,7 +7,8 @@ #include #include #include -#include + +#include "wavemap_rviz_plugin/occupancy_query.h" #endif namespace wavemap::rviz_plugin { @@ -28,6 +29,8 @@ class CellSelector : public QObject { void initializePropertyMenu(); void setMap(const MapBase::ConstPtr& map); + void setOccupancyQuery(std::shared_ptr occupancy_query); + void clearOccupancyQuery(); bool shouldBeDrawn(const OctreeIndex& cell_index, FloatingPoint cell_log_odds) const; @@ -44,8 +47,7 @@ class CellSelector : public QObject { private: std::function redraw_map_; - mutable std::optional> - query_accelerator_; + std::shared_ptr occupancy_query_; // Selection mode and thresholds CellSelectionMode cell_selection_mode_ = CellSelectionMode::kSurface; diff --git a/interfaces/ros1/wavemap_rviz_plugin/include/wavemap_rviz_plugin/visuals/voxel_visual.h b/interfaces/ros1/wavemap_rviz_plugin/include/wavemap_rviz_plugin/visuals/voxel_visual.h index 863b9fa57..8d4870ffe 100644 --- a/interfaces/ros1/wavemap_rviz_plugin/include/wavemap_rviz_plugin/visuals/voxel_visual.h +++ b/interfaces/ros1/wavemap_rviz_plugin/include/wavemap_rviz_plugin/visuals/voxel_visual.h @@ -5,6 +5,8 @@ #include #include #include +#include +#include #include #include @@ -128,12 +130,26 @@ class VoxelVisual : public QObject { const OctreeIndex& cell_index, FloatingPoint cell_log_odds, VoxelsPerLevel& voxels_per_level); + void appendLayeredLeafCenterAndColor(const LayeredMapInterface& layered_map, + const std::string& selected_layer_name, + int tree_height, + FloatingPoint min_cell_width, + const OctreeIndex& cell_index, + FloatingPoint cell_log_odds, + VoxelsPerLevel& voxels_per_level); void drawMultiResolutionVoxels(IndexElement tree_height, FloatingPoint min_cell_width, const Index3D& block_index, FloatingPoint alpha, VoxelsPerLevel& voxels_per_level, VoxelLayers& voxel_layer_visuals); + void drawLayeredMapOccupancy(const LayeredMapInterface& layered_map); + void drawLayeredMapOccupancy( + const wavemap_msgs::LayeredHashedWaveletOctree& layered_map_msg); + void appendLayeredBlockOccupancy( + const wavemap_msgs::LayeredHashedWaveletOctree& layered_map_msg, + const wavemap_msgs::LayeredHashedWaveletOctreeBlock& block_msg, + IndexElement termination_height, VoxelsPerLevel& voxels_per_level); // Block update queue // NOTE: Instead of performing all the block updates at once whenever the map diff --git a/interfaces/ros1/wavemap_rviz_plugin/include/wavemap_rviz_plugin/wavemap_map_display.h b/interfaces/ros1/wavemap_rviz_plugin/include/wavemap_rviz_plugin/wavemap_map_display.h index 43da57d65..c2c250fe3 100644 --- a/interfaces/ros1/wavemap_rviz_plugin/include/wavemap_rviz_plugin/wavemap_map_display.h +++ b/interfaces/ros1/wavemap_rviz_plugin/include/wavemap_rviz_plugin/wavemap_map_display.h @@ -5,12 +5,14 @@ #include #include #include +#include #include #include #include #include "wavemap_rviz_plugin/common.h" +#include "wavemap_rviz_plugin/layered_map_factory.h" #include "wavemap_rviz_plugin/utils/button_property.h" #include "wavemap_rviz_plugin/visuals/slice_visual.h" #include "wavemap_rviz_plugin/visuals/voxel_visual.h" @@ -51,9 +53,13 @@ class WavemapMapDisplay : public rviz::MessageFilterDisplay { void requestWavemapServerResetCallback(); void requestWholeMapCallback(); void loadMapFromDiskCallback(); + void updateLayerSelectionCallback(); private: SourceMode source_mode_ = SourceMode::kFromTopic; + std::vector available_layers_; + std::string selected_layer_name_ = "occupancy"; + std::vector> layered_map_factories_; bool hasMap(); void clearMap(); @@ -67,6 +73,9 @@ class WavemapMapDisplay : public rviz::MessageFilterDisplay { const std::shared_ptr map_and_mutex_ = std::make_shared(); void updateMapFromRosMsg(const wavemap_msgs::Map& map_msg); + std::shared_ptr createLayeredMapFromRosMsg( + const wavemap_msgs::LayeredHashedWaveletOctree& layered_map_msg) const; + void updateLayerMetadataFromRosMsg(const wavemap_msgs::Map& map_msg); // Submenus for each visual's properties rviz::EnumProperty source_mode_property_{"Source", "", @@ -90,6 +99,10 @@ class WavemapMapDisplay : public rviz::MessageFilterDisplay { rviz::Property voxel_visual_properties_{ "Render voxels", QVariant(), "Properties for the voxel-based visualization.", this}; + rviz::EnumProperty layer_property_{ + "Layer", "occupancy", + "Layer to use for voxel coloring when a layered map is received.", + &voxel_visual_properties_, SLOT(updateLayerSelectionCallback()), this}; rviz::Property slice_visual_properties_{ "Render slice", QVariant(), "Properties for the slice visualization.", this}; 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..cb7b8bbc3 100644 --- a/interfaces/ros1/wavemap_rviz_plugin/src/visuals/cell_selector.cc +++ b/interfaces/ros1/wavemap_rviz_plugin/src/visuals/cell_selector.cc @@ -56,15 +56,26 @@ void CellSelector::setMap(const MapBase::ConstPtr& map) { const auto hashed_map = std::dynamic_pointer_cast(map); if (hashed_map) { - query_accelerator_.emplace(*hashed_map); + setOccupancyQuery( + std::make_shared(hashed_map)); } else { - if (cell_selection_mode_ == CellSelectionMode::kSurface) { - ROS_WARN( - "Cell selection mode 'Surface' only supports HashedWaveletOctree " - "maps. Falling back to mode 'Band'."); - cell_selection_mode_ = CellSelectionMode::kBand; - initializePropertyMenu(); - } + clearOccupancyQuery(); + } +} + +void CellSelector::setOccupancyQuery( + std::shared_ptr occupancy_query) { + occupancy_query_ = std::move(occupancy_query); +} + +void CellSelector::clearOccupancyQuery() { + occupancy_query_.reset(); + if (cell_selection_mode_ == CellSelectionMode::kSurface) { + ROS_WARN( + "Cell selection mode 'Surface' requires an occupancy query. Falling " + "back to mode 'Band'."); + cell_selection_mode_ = CellSelectionMode::kBand; + initializePropertyMenu(); } } @@ -95,11 +106,15 @@ bool CellSelector::shouldBeDrawn(const OctreeIndex& cell_index, } bool CellSelector::hasFreeNeighbor(const OctreeIndex& cell_index) const { + if (!occupancy_query_) { + return true; + } + 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); + occupancy_query_->getCellValue(neighbor_index); // Check if the neighbor is free and observed if (neighbor_log_odds < surface_occupancy_threshold_ && !isUnknown(neighbor_log_odds)) { 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..f241d2cbc 100644 --- a/interfaces/ros1/wavemap_rviz_plugin/src/visuals/voxel_visual.cc +++ b/interfaces/ros1/wavemap_rviz_plugin/src/visuals/voxel_visual.cc @@ -2,14 +2,17 @@ #include #include +#include #include #include +#include #include #include #include #include #include +#include #include namespace wavemap::rviz_plugin { @@ -109,6 +112,12 @@ void VoxelVisual::updateMap(bool redraw_all) { std::scoped_lock lock(map_and_mutex_->mutex); MapBase::ConstPtr map = map_and_mutex_->map; if (!map) { + if (map_and_mutex_->layered_map) { + cell_selector_.setOccupancyQuery(map_and_mutex_->layered_map); + drawLayeredMapOccupancy(*map_and_mutex_->layered_map); + } else if (map_and_mutex_->layered_map_msg) { + drawLayeredMapOccupancy(map_and_mutex_->layered_map_msg.value()); + } return; } cell_selector_.setMap(map); @@ -369,6 +378,42 @@ void VoxelVisual::appendLeafCenterAndColor(int tree_height, } } +void VoxelVisual::appendLayeredLeafCenterAndColor( + const LayeredMapInterface& layered_map, + const std::string& selected_layer_name, int tree_height, + FloatingPoint min_cell_width, const OctreeIndex& cell_index, + FloatingPoint cell_log_odds, VoxelsPerLevel& voxels_per_level) { + if (selected_layer_name == "occupancy") { + appendLeafCenterAndColor(tree_height, min_cell_width, cell_index, + cell_log_odds, voxels_per_level); + return; + } + + if (!cell_selector_.shouldBeDrawn(cell_index, cell_log_odds)) { + return; + } + + Ogre::ColourValue layer_color; + if (!layered_map.getLayerColor(selected_layer_name, cell_index, cell_log_odds, + layer_color)) { + appendLeafCenterAndColor(tree_height, min_cell_width, cell_index, + cell_log_odds, voxels_per_level); + return; + } + + const IndexElement depth = tree_height - cell_index.height; + CHECK_GE(depth, 0); + CHECK_LT(depth, voxels_per_level.size()); + const Point3D cell_center = + convert::nodeIndexToCenterPoint(cell_index, min_cell_width); + + auto& point = voxels_per_level[depth].emplace_back(); + point.center.x = cell_center[0]; + point.center.y = cell_center[1]; + point.center.z = cell_center[2]; + point.color = layer_color; +} + void VoxelVisual::drawMultiResolutionVoxels(IndexElement tree_height, FloatingPoint min_cell_width, const Index3D& block_index, @@ -406,6 +451,151 @@ void VoxelVisual::drawMultiResolutionVoxels(IndexElement tree_height, } } + +void VoxelVisual::drawLayeredMapOccupancy(const LayeredMapInterface& layered_map) { + ProfilerZoneScoped; + const IndexElement tree_height = layered_map.getTreeHeight(); + const FloatingPoint min_cell_width = layered_map.getMinCellWidth(); + const FloatingPoint alpha = opacity_property_.getFloat(); + const std::string selected_layer_name = map_and_mutex_->selected_layer_name; + const IndexElement termination_height = + std::min(termination_height_property_.getInt(), tree_height); + + termination_height_property_.setMax(tree_height); + block_update_queue_.clear(); + + // Remove visuals for blocks that no longer exist in the layered map. + const std::vector block_indices = layered_map.getBlockIndices(); + std::unordered_set allocated_blocks( + block_indices.begin(), block_indices.end()); + for (auto it = block_voxel_layers_map_.begin(); + it != block_voxel_layers_map_.end();) { + if (!allocated_blocks.count(it->first)) { + it = block_voxel_layers_map_.erase(it); + } else { + ++it; + } + } + + // Draw occupancy through the runtime layered map interface. Layer-specific + // color mapping will be added on top of this path. + for (const auto& block_index : block_indices) { + const int num_levels = tree_height + 1 - termination_height; + VoxelsPerLevel voxels_per_level(num_levels); + layered_map.forEachBlockLeaf( + block_index, + [this, &layered_map, &selected_layer_name, tree_height, min_cell_width, + &voxels_per_level](const OctreeIndex& cell_index, + FloatingPoint cell_log_odds) { + appendLayeredLeafCenterAndColor(layered_map, selected_layer_name, + tree_height, min_cell_width, + cell_index, cell_log_odds, + voxels_per_level); + }, + termination_height); + drawMultiResolutionVoxels(tree_height, min_cell_width, block_index, alpha, + voxels_per_level, + block_voxel_layers_map_[block_index]); + } + + num_queued_blocks_indicator_.setInt(0); +} + + +void VoxelVisual::drawLayeredMapOccupancy(const wavemap_msgs::LayeredHashedWaveletOctree& layered_map_msg) { + ProfilerZoneScoped; + const IndexElement tree_height = layered_map_msg.tree_height; + const FloatingPoint min_cell_width = layered_map_msg.min_cell_width; + const FloatingPoint alpha = opacity_property_.getFloat(); + const IndexElement termination_height = termination_height_property_.getInt(); + + termination_height_property_.setMax(tree_height); + block_update_queue_.clear(); + + // Remove visuals for blocks that no longer exist in the layered map message. + std::unordered_set allocated_blocks; + for (const auto& block_index_msg : layered_map_msg.allocated_block_indices) { + allocated_blocks.emplace(block_index_msg.x, block_index_msg.y, block_index_msg.z); + } + for (auto it = block_voxel_layers_map_.begin(); it != block_voxel_layers_map_.end();) { + if (!allocated_blocks.count(it->first)) { + it = block_voxel_layers_map_.erase(it); + } else { + ++it; + } + } + + // Draw the layered map using only the occupancy coefficients. Layer-specific color mapping is added in the next phase. + for (const auto& block_msg : layered_map_msg.blocks) { + const Index3D block_index{block_msg.root_node_offset.x, block_msg.root_node_offset.y, block_msg.root_node_offset.z}; + const int num_levels = tree_height + 1 - termination_height; + VoxelsPerLevel voxels_per_level(num_levels); + appendLayeredBlockOccupancy(layered_map_msg, block_msg, termination_height, voxels_per_level); + drawMultiResolutionVoxels(tree_height, min_cell_width, block_index, alpha, voxels_per_level, block_voxel_layers_map_[block_index]); + } + + num_queued_blocks_indicator_.setInt(0); +} + +void VoxelVisual::appendLayeredBlockOccupancy(const wavemap_msgs::LayeredHashedWaveletOctree& layered_map_msg, const wavemap_msgs::LayeredHashedWaveletOctreeBlock& block_msg, IndexElement termination_height, VoxelsPerLevel& voxels_per_level) { + ProfilerZoneScoped; + if (block_msg.nodes.empty()) { + return; + } + + using Coefficients = HashedWaveletOctreeBlock::Coefficients; + using Transform = HashedWaveletOctreeBlock::Transform; + + struct StackElement { + OctreeIndex node_index; + FloatingPoint scale; + size_t node_msg_index; + }; + + size_t next_node_msg_index = 0u; + std::stack stack; + stack.emplace(StackElement{OctreeIndex{layered_map_msg.tree_height, Index3D{block_msg.root_node_offset.x, block_msg.root_node_offset.y, block_msg.root_node_offset.z}}, block_msg.root_node_occupancy_scale_coefficient, next_node_msg_index++}); + + while (!stack.empty()) { + const StackElement stack_element = stack.top(); + stack.pop(); + + if (block_msg.nodes.size() <= stack_element.node_msg_index) { + ROS_WARN("Layered map block ended before all queued nodes were read."); + return; + } + + const auto& node_msg = block_msg.nodes[stack_element.node_msg_index]; + Coefficients::Details details; + std::copy_n(node_msg.occupancy_detail_coefficients.begin(), details.size(), details.begin()); + const auto child_scales = Transform::backward({stack_element.scale, details}); + + struct ChildToVisit { + OctreeIndex node_index; + FloatingPoint scale; + size_t node_msg_index; + }; + std::vector children_to_visit; + + for (NdtreeIndexRelativeChild child_idx = 0; child_idx < OctreeIndex::kNumChildren; ++child_idx) { + const OctreeIndex child_node_index = stack_element.node_index.computeChildIndex(child_idx); + const FloatingPoint child_scale = child_scales[child_idx]; + const bool child_exists = bit_ops::is_bit_set(node_msg.allocated_children_bitset, child_idx); + + if (child_exists && termination_height < child_node_index.height) { + children_to_visit.push_back({child_node_index, child_scale, next_node_msg_index++}); + } else { + appendLeafCenterAndColor(layered_map_msg.tree_height, layered_map_msg.min_cell_width, child_node_index, child_scale, voxels_per_level); + } + } + + // The serialized node array follows the same DFS order as the stack traversal. Push in reverse so the next serialized node is popped first. + for (auto child_it = children_to_visit.rbegin(); child_it != children_to_visit.rend(); ++child_it) { + stack.emplace(StackElement{child_it->node_index, child_it->scale, child_it->node_msg_index}); + } + } +} + void VoxelVisual::processBlockUpdateQueue(const Point3D& camera_position) { ProfilerZoneScoped; if (!visibility_property_.getBool()) { 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..d632b40c6 100644 --- a/interfaces/ros1/wavemap_rviz_plugin/src/wavemap_map_display.cc +++ b/interfaces/ros1/wavemap_rviz_plugin/src/wavemap_map_display.cc @@ -1,10 +1,12 @@ #include "wavemap_rviz_plugin/wavemap_map_display.h" +#include #include #include #include #include +#include #include #include #include @@ -13,8 +15,37 @@ #include #include +#include "layered_ros_converter.h" +#include "layered_voxel_config.h" #include "wavemap_rviz_plugin/utils/alert_dialog.h" + +namespace { +struct LayeredVoxelColorProvider { + static bool getLayerColor(const std::string& layer_name, + const LayeredVoxel& voxel, + wavemap::FloatingPoint /*occupancy*/, + Ogre::ColourValue& color) { + if (layer_name == "color") { + color = Ogre::ColourValue(voxel.data.r, voxel.data.g, voxel.data.b, 1.f); + return true; + } + + if (layer_name == "traversability") { + const float value = std::clamp(voxel.data.traversability, 0.f, 1.f); + if (value < 0.5f) { + color = Ogre::ColourValue(1.f, 2.f * value, 0.f, 1.f); + } else { + color = Ogre::ColourValue(2.f * (1.f - value), 1.f, 0.f, 1.f); + } + return true; + } + + return false; + } +}; +} // namespace + namespace wavemap::rviz_plugin { WavemapMapDisplay::WavemapMapDisplay() { // Initialize the property menu @@ -23,6 +54,18 @@ WavemapMapDisplay::WavemapMapDisplay() { source_mode_property_.addOption(name); } source_mode_property_.setStringStd(source_mode_.toStr()); + + // Temporary prototype wiring: this RViz plugin knows the current layered voxel type. + // Later this can move back to pluginlib-based factory registration. + layered_map_factories_.push_back( + std::make_shared< + TypedLayeredMapFactory>()); + + // The layered map selector is only useful after receiving a layered map message. + layer_property_.clearOptions(); + layer_property_.addOption(QString::fromStdString(selected_layer_name_)); + layer_property_.setStringStd(selected_layer_name_); + layer_property_.setHidden(true); } // After the top-level rviz::Display::initialize() does its own setup, @@ -51,7 +94,7 @@ void WavemapMapDisplay::reset() { bool WavemapMapDisplay::hasMap() { ProfilerZoneScoped; std::scoped_lock lock(map_and_mutex_->mutex); - return static_cast(map_and_mutex_->map); + return static_cast(map_and_mutex_->map) || static_cast(map_and_mutex_->layered_map) || map_and_mutex_->layered_map_msg.has_value(); } void WavemapMapDisplay::clearMap() { @@ -60,11 +103,15 @@ void WavemapMapDisplay::clearMap() { if (map_and_mutex_->map) { map_and_mutex_->map->clear(); } + map_and_mutex_->layered_map.reset(); + map_and_mutex_->layered_map_msg.reset(); } bool WavemapMapDisplay::loadMapFromDisk(const std::filesystem::path& filepath) { ProfilerZoneScoped; std::scoped_lock lock(map_and_mutex_->mutex); + map_and_mutex_->layered_map.reset(); + map_and_mutex_->layered_map_msg.reset(); return io::fileToMap(filepath, map_and_mutex_->map); } @@ -117,12 +164,95 @@ void WavemapMapDisplay::processMessage( void WavemapMapDisplay::updateMapFromRosMsg(const wavemap_msgs::Map& map_msg) { ProfilerZoneScoped; + updateLayerMetadataFromRosMsg(map_msg); + + const bool has_layered_map = map_msg.layered_hashed_wavelet_octree.size() == 1u; + const bool has_legacy_map = !map_msg.hashed_blocks.empty() || !map_msg.wavelet_octree.empty() || !map_msg.hashed_wavelet_octree.empty(); + std::scoped_lock lock(map_and_mutex_->mutex); + map_and_mutex_->selected_layer_name = selected_layer_name_; + if (has_layered_map && !has_legacy_map) { + const auto& layered_map_msg = map_msg.layered_hashed_wavelet_octree.front(); + map_and_mutex_->map.reset(); + map_and_mutex_->layered_map = createLayeredMapFromRosMsg(layered_map_msg); + if (map_and_mutex_->layered_map) { + map_and_mutex_->layered_map_msg.reset(); + } else { + // Keep the message fallback until users can register typed factories for their custom voxel data. + map_and_mutex_->layered_map_msg = layered_map_msg; + } + return; + } + + map_and_mutex_->layered_map.reset(); + map_and_mutex_->layered_map_msg.reset(); if (!convert::rosMsgToMap(map_msg, map_and_mutex_->map)) { ROS_WARN("Failed to parse map message."); } } +std::shared_ptr WavemapMapDisplay::createLayeredMapFromRosMsg( + const wavemap_msgs::LayeredHashedWaveletOctree& layered_map_msg) const { + ProfilerZoneScoped; + for (const auto& factory : layered_map_factories_) { + if (!factory) { + continue; + } + + if (auto layered_map = factory->tryCreate(layered_map_msg)) { + return layered_map; + } + } + + return nullptr; +} + +void WavemapMapDisplay::updateLayerMetadataFromRosMsg(const wavemap_msgs::Map& map_msg) { + ProfilerZoneScoped; + available_layers_.clear(); + layer_property_.clearOptions(); + layer_property_.addOption("occupancy"); + + if (map_msg.layered_hashed_wavelet_octree.empty()) { + selected_layer_name_ = "occupancy"; + layer_property_.setStringStd(selected_layer_name_); + layer_property_.setHidden(true); + return; + } + + const auto& layered_map_msg = map_msg.layered_hashed_wavelet_octree.front(); + const size_t num_layers = std::min(layered_map_msg.layer_names.size(), layered_map_msg.layer_types.size()); + for (size_t layer_idx = 0u; layer_idx < num_layers; ++layer_idx) { + available_layers_.push_back({layered_map_msg.layer_names[layer_idx], layered_map_msg.layer_types[layer_idx]}); + layer_property_.addOption(QString::fromStdString(layered_map_msg.layer_names[layer_idx])); + } + + const bool selected_layer_still_exists = + selected_layer_name_ == "occupancy" || + std::any_of(available_layers_.begin(), available_layers_.end(), + [this](const LayerMetadata& layer) { + return layer.name == selected_layer_name_; + }); + if (!selected_layer_still_exists) { + selected_layer_name_ = "occupancy"; + } + + layer_property_.setStringStd(selected_layer_name_); + layer_property_.setHidden(false); + + ROS_DEBUG_STREAM("Received layered map metadata with " << available_layers_.size() << " custom layers."); +} + +void WavemapMapDisplay::updateLayerSelectionCallback() { + ProfilerZoneScoped; + selected_layer_name_ = layer_property_.getStdString(); + { + std::scoped_lock lock(map_and_mutex_->mutex); + map_and_mutex_->selected_layer_name = selected_layer_name_; + } + updateVisuals(true); +} + void WavemapMapDisplay::updateSourceModeCallback() { ProfilerZoneScoped; // Update the cached source mode value From 0e9401900e35a3e846665216ad11b8dd040cfcff Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Guilherne=20Caba=C3=A7o?= Date: Wed, 5 Aug 2026 18:08:42 +0000 Subject: [PATCH 07/14] Add layered map support --- examples/cpp/digital_twin/CMakeLists.txt | 195 +++++++- .../common/custom_layered_map_config.h | 162 +++++++ .../common/example_layered_map_config.h | 43 ++ .../common/example_layered_map_ros_config.h | 68 +++ .../common/layered_ros_converter.h | 48 +- .../common/layered_voxel_config.h | 186 +++++++- .../common/layered_voxel_updater.h | 31 ++ .../custom_layered_map_save_load_example.cc | 87 ++++ .../io/layered_map_io_experiment.cc | 8 +- .../io/layered_map_save_load_experiment.cc | 115 +++++ ...ayered_map_save_load_failure_experiment.cc | 143 ++++++ .../map_api/baseline/float_map_experiment.cc | 54 +++ .../map_api/baseline/int_map_experiment.cc | 54 +++ .../{ => baseline}/voxel_block_experiment.cc | 6 +- .../{ => baseline}/voxel_map_experiment.cc | 8 +- .../continuous_voxel_many_voxels_benchmark.cc | 98 ++++ ...at_cast_discrete_many_voxels_experiment.cc | 168 +++++++ .../layered_pruning_policy_experiment.cc | 373 +++++++++++++++ .../occupancy_many_voxels_benchmark.cc | 79 ++++ .../raw_discrete_many_voxels_experiment.cc | 108 +++++ .../raw_inside_map_many_voxels_experiment.cc | 158 +++++++ .../continuous_voxel_map_experiment.cc | 65 +++ .../heterogeneous_voxel_map_experiment.cc | 135 ++++++ .../layered_full_update_pruning_experiment.cc | 243 ++++++++++ ...ed_hashed_wavelet_integrator_experiment.cc | 117 +++++ ...ayered_occupancy_update_path_experiment.cc | 94 ++++ .../layered_per_voxel_update_experiment.cc | 143 ++++++ .../continuous/layered_pruning_config.md | 60 +++ .../layered_pruning_config_example.cc | 88 ++++ .../continuous/mixed_voxel_map_experiment.cc | 97 ++++ .../discrete_layer_compression_experiment.cc | 138 ++++++ ...screte_layer_update_behavior_experiment.cc | 112 +++++ .../float_cast_discrete_experiment.cc | 147 ++++++ .../layered_label_storage_experiment.cc | 76 +++ .../discrete/layered_map_experiment.cc | 123 +++++ .../layered_map_update_behavior_experiment.cc | 82 ++++ .../raw_discrete_voxel_storage_experiment.cc | 90 ++++ .../discrete/raw_inside_map_experiment.cc | 143 ++++++ ...semantic_majority_exceptions_experiment.cc | 242 ++++++++++ .../user_facing/custom_layered_map_example.cc | 119 +++++ ...yered_map_structured_ros_msg_experiment.cc | 105 +++++ .../ros/layered_ros_msg_experiment.cc | 6 +- .../ros/publish_layered_map_experiment.cc | 2 +- ...blish_structured_layered_map_experiment.cc | 99 ++++ .../ros/subscribe_layered_map_experiment.cc | 2 +- ...cribe_structured_layered_map_experiment.cc | 109 +++++ .../cpp/digital_twin/voxel_data/new_voxel.cc | 6 +- .../voxel_data/voxel_haar_transform.cc | 16 +- interfaces/ros1/wavemap_msgs/CMakeLists.txt | 3 + .../ros1/wavemap_msgs/msg/DiscreteLayer.msg | 10 + .../wavemap_msgs/msg/DiscreteLayerCell.msg | 21 + .../ros1/wavemap_msgs/msg/LayeredMap.msg | 7 + .../wavemap_ros_conversions/CMakeLists.txt | 4 +- .../discrete_layer_marker_conversions.h | 160 +++++++ .../layered_map_msg_conversions.h | 309 +++++++++++++ .../ros1/wavemap_ros_conversions/package.xml | 1 + .../src/wavemap_map_display.cc | 2 +- library/cpp/CMakeLists.txt | 4 + .../hashed_wavelet_integrator.h | 48 +- .../impl/hashed_wavelet_integrator_inl.h | 204 ++++++++- .../core/map/cell_types/cell_data_traits.h | 134 ++++++ .../wavemap/core/map/hashed_wavelet_octree.h | 17 +- .../core/map/hashed_wavelet_octree_block.h | 20 +- .../impl/hashed_wavelet_octree_block_inl.h | 54 ++- .../core/map/impl/hashed_wavelet_octree_inl.h | 6 +- .../wavemap/core/utils/data/comparisons.h | 17 +- .../include/wavemap/layered/discrete_layer.h | 250 ++++++++++ .../cpp/include/wavemap/layered/layered_map.h | 76 +++ .../include/wavemap/layered/layered_map_io.h | 433 ++++++++++++++++++ .../hashed_wavelet_integrator.cc | 202 -------- 70 files changed, 6525 insertions(+), 308 deletions(-) create mode 100644 examples/cpp/digital_twin/common/custom_layered_map_config.h create mode 100644 examples/cpp/digital_twin/common/example_layered_map_config.h create mode 100644 examples/cpp/digital_twin/common/example_layered_map_ros_config.h create mode 100644 examples/cpp/digital_twin/common/layered_voxel_updater.h create mode 100644 examples/cpp/digital_twin/io/custom_layered_map_save_load_example.cc create mode 100644 examples/cpp/digital_twin/io/layered_map_save_load_experiment.cc create mode 100644 examples/cpp/digital_twin/io/layered_map_save_load_failure_experiment.cc create mode 100644 examples/cpp/digital_twin/map_api/baseline/float_map_experiment.cc create mode 100644 examples/cpp/digital_twin/map_api/baseline/int_map_experiment.cc rename examples/cpp/digital_twin/map_api/{ => baseline}/voxel_block_experiment.cc (88%) rename examples/cpp/digital_twin/map_api/{ => baseline}/voxel_map_experiment.cc (86%) create mode 100644 examples/cpp/digital_twin/map_api/benchmarks/continuous_voxel_many_voxels_benchmark.cc create mode 100644 examples/cpp/digital_twin/map_api/benchmarks/float_cast_discrete_many_voxels_experiment.cc create mode 100644 examples/cpp/digital_twin/map_api/benchmarks/layered_pruning_policy_experiment.cc create mode 100644 examples/cpp/digital_twin/map_api/benchmarks/occupancy_many_voxels_benchmark.cc create mode 100644 examples/cpp/digital_twin/map_api/benchmarks/raw_discrete_many_voxels_experiment.cc create mode 100644 examples/cpp/digital_twin/map_api/benchmarks/raw_inside_map_many_voxels_experiment.cc create mode 100644 examples/cpp/digital_twin/map_api/continuous/continuous_voxel_map_experiment.cc create mode 100644 examples/cpp/digital_twin/map_api/continuous/heterogeneous_voxel_map_experiment.cc create mode 100644 examples/cpp/digital_twin/map_api/continuous/layered_full_update_pruning_experiment.cc create mode 100644 examples/cpp/digital_twin/map_api/continuous/layered_hashed_wavelet_integrator_experiment.cc create mode 100644 examples/cpp/digital_twin/map_api/continuous/layered_occupancy_update_path_experiment.cc create mode 100644 examples/cpp/digital_twin/map_api/continuous/layered_per_voxel_update_experiment.cc create mode 100644 examples/cpp/digital_twin/map_api/continuous/layered_pruning_config.md create mode 100644 examples/cpp/digital_twin/map_api/continuous/layered_pruning_config_example.cc create mode 100644 examples/cpp/digital_twin/map_api/continuous/mixed_voxel_map_experiment.cc create mode 100644 examples/cpp/digital_twin/map_api/discrete/discrete_layer_compression_experiment.cc create mode 100644 examples/cpp/digital_twin/map_api/discrete/discrete_layer_update_behavior_experiment.cc create mode 100644 examples/cpp/digital_twin/map_api/discrete/float_cast_discrete_experiment.cc create mode 100644 examples/cpp/digital_twin/map_api/discrete/layered_label_storage_experiment.cc create mode 100644 examples/cpp/digital_twin/map_api/discrete/layered_map_experiment.cc create mode 100644 examples/cpp/digital_twin/map_api/discrete/layered_map_update_behavior_experiment.cc create mode 100644 examples/cpp/digital_twin/map_api/discrete/raw_discrete_voxel_storage_experiment.cc create mode 100644 examples/cpp/digital_twin/map_api/discrete/raw_inside_map_experiment.cc create mode 100644 examples/cpp/digital_twin/map_api/discrete/semantic_majority_exceptions_experiment.cc create mode 100644 examples/cpp/digital_twin/map_api/user_facing/custom_layered_map_example.cc create mode 100644 examples/cpp/digital_twin/ros/layered_map_structured_ros_msg_experiment.cc create mode 100644 examples/cpp/digital_twin/ros/publish_structured_layered_map_experiment.cc create mode 100644 examples/cpp/digital_twin/ros/subscribe_structured_layered_map_experiment.cc create mode 100644 interfaces/ros1/wavemap_msgs/msg/DiscreteLayer.msg create mode 100644 interfaces/ros1/wavemap_msgs/msg/DiscreteLayerCell.msg create mode 100644 interfaces/ros1/wavemap_msgs/msg/LayeredMap.msg create mode 100644 interfaces/ros1/wavemap_ros_conversions/include/wavemap_ros_conversions/discrete_layer_marker_conversions.h create mode 100644 interfaces/ros1/wavemap_ros_conversions/include/wavemap_ros_conversions/layered_map_msg_conversions.h create mode 100644 library/cpp/include/wavemap/core/map/cell_types/cell_data_traits.h create mode 100644 library/cpp/include/wavemap/layered/discrete_layer.h create mode 100644 library/cpp/include/wavemap/layered/layered_map.h create mode 100644 library/cpp/include/wavemap/layered/layered_map_io.h diff --git a/examples/cpp/digital_twin/CMakeLists.txt b/examples/cpp/digital_twin/CMakeLists.txt index f3054db5a..9398a0f19 100644 --- a/examples/cpp/digital_twin/CMakeLists.txt +++ b/examples/cpp/digital_twin/CMakeLists.txt @@ -7,14 +7,166 @@ add_executable(voxel_haar_transform voxel_data/voxel_haar_transform.cc) set_wavemap_target_properties(voxel_haar_transform) target_link_libraries(voxel_haar_transform PUBLIC wavemap::wavemap_core) -add_executable(voxel_block_experiment map_api/voxel_block_experiment.cc) +add_executable(voxel_block_experiment map_api/baseline/voxel_block_experiment.cc) set_wavemap_target_properties(voxel_block_experiment) target_link_libraries(voxel_block_experiment PUBLIC wavemap::wavemap_core) -add_executable(voxel_map_experiment map_api/voxel_map_experiment.cc) +add_executable(voxel_map_experiment map_api/baseline/voxel_map_experiment.cc) set_wavemap_target_properties(voxel_map_experiment) target_link_libraries(voxel_map_experiment PUBLIC wavemap::wavemap_core) +add_executable(int_map_experiment map_api/baseline/int_map_experiment.cc) +set_wavemap_target_properties(int_map_experiment) +target_link_libraries(int_map_experiment PUBLIC wavemap::wavemap_core) + +add_executable(float_map_experiment map_api/baseline/float_map_experiment.cc) +set_wavemap_target_properties(float_map_experiment) +target_link_libraries(float_map_experiment PUBLIC wavemap::wavemap_core) + +add_executable(continuous_voxel_map_experiment + map_api/continuous/continuous_voxel_map_experiment.cc) +set_wavemap_target_properties(continuous_voxel_map_experiment) +target_link_libraries(continuous_voxel_map_experiment + PUBLIC wavemap::wavemap_core) + +add_executable(layered_occupancy_update_path_experiment + map_api/continuous/layered_occupancy_update_path_experiment.cc) +set_wavemap_target_properties(layered_occupancy_update_path_experiment) +target_link_libraries(layered_occupancy_update_path_experiment + PUBLIC wavemap::wavemap_core) + +add_executable(layered_hashed_wavelet_integrator_experiment + map_api/continuous/layered_hashed_wavelet_integrator_experiment.cc) +set_wavemap_target_properties(layered_hashed_wavelet_integrator_experiment) +target_link_libraries(layered_hashed_wavelet_integrator_experiment + PUBLIC wavemap::wavemap_core) + +add_executable(layered_per_voxel_update_experiment + map_api/continuous/layered_per_voxel_update_experiment.cc) +set_wavemap_target_properties(layered_per_voxel_update_experiment) +target_link_libraries(layered_per_voxel_update_experiment + PUBLIC wavemap::wavemap_core) + +add_executable(layered_full_update_pruning_experiment + map_api/continuous/layered_full_update_pruning_experiment.cc) +set_wavemap_target_properties(layered_full_update_pruning_experiment) +target_link_libraries(layered_full_update_pruning_experiment + PUBLIC wavemap::wavemap_core) + +add_executable(layered_pruning_config_example + map_api/continuous/layered_pruning_config_example.cc) +set_wavemap_target_properties(layered_pruning_config_example) +target_link_libraries(layered_pruning_config_example + PUBLIC wavemap::wavemap_core) + +add_executable(mixed_voxel_map_experiment + map_api/continuous/mixed_voxel_map_experiment.cc) +set_wavemap_target_properties(mixed_voxel_map_experiment) +target_link_libraries(mixed_voxel_map_experiment PUBLIC wavemap::wavemap_core) + +add_executable(layered_label_storage_experiment + map_api/discrete/layered_label_storage_experiment.cc) +set_wavemap_target_properties(layered_label_storage_experiment) +target_link_libraries(layered_label_storage_experiment + PUBLIC wavemap::wavemap_core) + +add_executable(layered_map_experiment + map_api/discrete/layered_map_experiment.cc) +set_wavemap_target_properties(layered_map_experiment) +target_link_libraries(layered_map_experiment + PUBLIC wavemap::wavemap_core) + +add_executable(custom_layered_map_example + map_api/user_facing/custom_layered_map_example.cc) +set_wavemap_target_properties(custom_layered_map_example) +target_link_libraries(custom_layered_map_example + PUBLIC wavemap::wavemap_core) + +add_executable(discrete_layer_compression_experiment + map_api/discrete/discrete_layer_compression_experiment.cc) +set_wavemap_target_properties(discrete_layer_compression_experiment) +target_link_libraries(discrete_layer_compression_experiment + PUBLIC wavemap::wavemap_core) + +add_executable(discrete_layer_update_behavior_experiment + map_api/discrete/discrete_layer_update_behavior_experiment.cc) +set_wavemap_target_properties(discrete_layer_update_behavior_experiment) +target_link_libraries(discrete_layer_update_behavior_experiment + PUBLIC wavemap::wavemap_core) + +add_executable(layered_map_update_behavior_experiment + map_api/discrete/layered_map_update_behavior_experiment.cc) +set_wavemap_target_properties(layered_map_update_behavior_experiment) +target_link_libraries(layered_map_update_behavior_experiment + PUBLIC wavemap::wavemap_core) + +add_executable(heterogeneous_voxel_map_experiment + map_api/continuous/heterogeneous_voxel_map_experiment.cc) +set_wavemap_target_properties(heterogeneous_voxel_map_experiment) +target_link_libraries(heterogeneous_voxel_map_experiment + PUBLIC wavemap::wavemap_core) + +add_executable(raw_discrete_voxel_storage_experiment + map_api/discrete/raw_discrete_voxel_storage_experiment.cc) +set_wavemap_target_properties(raw_discrete_voxel_storage_experiment) +target_link_libraries(raw_discrete_voxel_storage_experiment + PUBLIC wavemap::wavemap_core) + +add_executable(raw_inside_map_experiment + map_api/discrete/raw_inside_map_experiment.cc) +set_wavemap_target_properties(raw_inside_map_experiment) +target_link_libraries(raw_inside_map_experiment + PUBLIC wavemap::wavemap_core) + +add_executable(float_cast_discrete_experiment + map_api/discrete/float_cast_discrete_experiment.cc) +set_wavemap_target_properties(float_cast_discrete_experiment) +target_link_libraries(float_cast_discrete_experiment + PUBLIC wavemap::wavemap_core) + +add_executable(float_cast_discrete_many_voxels_experiment + map_api/benchmarks/float_cast_discrete_many_voxels_experiment.cc) +set_wavemap_target_properties(float_cast_discrete_many_voxels_experiment) +target_link_libraries(float_cast_discrete_many_voxels_experiment + PUBLIC wavemap::wavemap_core) + +add_executable(raw_discrete_many_voxels_experiment + map_api/benchmarks/raw_discrete_many_voxels_experiment.cc) +set_wavemap_target_properties(raw_discrete_many_voxels_experiment) +target_link_libraries(raw_discrete_many_voxels_experiment + PUBLIC wavemap::wavemap_core) + +add_executable(raw_inside_map_many_voxels_experiment + map_api/benchmarks/raw_inside_map_many_voxels_experiment.cc) +set_wavemap_target_properties(raw_inside_map_many_voxels_experiment) +target_link_libraries(raw_inside_map_many_voxels_experiment + PUBLIC wavemap::wavemap_core) + +add_executable(occupancy_many_voxels_benchmark + map_api/benchmarks/occupancy_many_voxels_benchmark.cc) +set_wavemap_target_properties(occupancy_many_voxels_benchmark) +target_link_libraries(occupancy_many_voxels_benchmark + PUBLIC wavemap::wavemap_core) + +add_executable(continuous_voxel_many_voxels_benchmark + map_api/benchmarks/continuous_voxel_many_voxels_benchmark.cc) +set_wavemap_target_properties(continuous_voxel_many_voxels_benchmark) +target_link_libraries(continuous_voxel_many_voxels_benchmark + PUBLIC wavemap::wavemap_core) + +add_executable(layered_pruning_policy_experiment + map_api/benchmarks/layered_pruning_policy_experiment.cc) +set_wavemap_target_properties(layered_pruning_policy_experiment) +target_link_libraries(layered_pruning_policy_experiment + PUBLIC wavemap::wavemap_core) + + +add_executable(semantic_majority_exceptions_experiment + map_api/discrete/semantic_majority_exceptions_experiment.cc) +set_wavemap_target_properties(semantic_majority_exceptions_experiment) +target_link_libraries(semantic_majority_exceptions_experiment + PUBLIC wavemap::wavemap_core) + add_executable(legacy_hashed_wavelet_octree_experiment io/legacy_hashed_wavelet_octree_experiment.cc) set_wavemap_target_properties(legacy_hashed_wavelet_octree_experiment) @@ -30,9 +182,25 @@ add_executable(layered_map_io_experiment io/layered_map_io_experiment.cc) set_wavemap_target_properties(layered_map_io_experiment) target_link_libraries(layered_map_io_experiment PUBLIC wavemap::wavemap_core) +add_executable(layered_map_save_load_experiment io/layered_map_save_load_experiment.cc) +set_wavemap_target_properties(layered_map_save_load_experiment) +target_link_libraries(layered_map_save_load_experiment PUBLIC wavemap::wavemap_core) + +add_executable(layered_map_save_load_failure_experiment + io/layered_map_save_load_failure_experiment.cc) +set_wavemap_target_properties(layered_map_save_load_failure_experiment) +target_link_libraries(layered_map_save_load_failure_experiment + PUBLIC wavemap::wavemap_core) + +add_executable(custom_layered_map_save_load_example + io/custom_layered_map_save_load_example.cc) +set_wavemap_target_properties(custom_layered_map_save_load_example) +target_link_libraries(custom_layered_map_save_load_example + PUBLIC wavemap::wavemap_core) + if ("$ENV{ROS_VERSION}" STREQUAL "1") find_package(catkin REQUIRED - COMPONENTS roscpp wavemap wavemap_msgs wavemap_ros_conversions) + COMPONENTS roscpp visualization_msgs wavemap wavemap_msgs wavemap_ros_conversions) add_executable(experiment old_experiments/experiment.cc) set_wavemap_target_properties(experiment) @@ -64,6 +232,27 @@ if ("$ENV{ROS_VERSION}" STREQUAL "1") target_link_libraries(layered_ros_msg_experiment PUBLIC wavemap::wavemap_core ${catkin_LIBRARIES}) + add_executable(layered_map_structured_ros_msg_experiment + ros/layered_map_structured_ros_msg_experiment.cc) + set_wavemap_target_properties(layered_map_structured_ros_msg_experiment) + target_include_directories(layered_map_structured_ros_msg_experiment PUBLIC ${catkin_INCLUDE_DIRS}) + target_link_libraries(layered_map_structured_ros_msg_experiment PUBLIC + wavemap::wavemap_core ${catkin_LIBRARIES}) + + add_executable(publish_structured_layered_map_experiment + ros/publish_structured_layered_map_experiment.cc) + set_wavemap_target_properties(publish_structured_layered_map_experiment) + target_include_directories(publish_structured_layered_map_experiment PUBLIC ${catkin_INCLUDE_DIRS}) + target_link_libraries(publish_structured_layered_map_experiment PUBLIC + wavemap::wavemap_core ${catkin_LIBRARIES}) + + add_executable(subscribe_structured_layered_map_experiment + ros/subscribe_structured_layered_map_experiment.cc) + set_wavemap_target_properties(subscribe_structured_layered_map_experiment) + target_include_directories(subscribe_structured_layered_map_experiment PUBLIC ${catkin_INCLUDE_DIRS}) + target_link_libraries(subscribe_structured_layered_map_experiment PUBLIC + wavemap::wavemap_core ${catkin_LIBRARIES}) + add_executable(publish_layered_map_experiment ros/publish_layered_map_experiment.cc) set_wavemap_target_properties(publish_layered_map_experiment) target_include_directories(publish_layered_map_experiment PUBLIC ${catkin_INCLUDE_DIRS}) diff --git a/examples/cpp/digital_twin/common/custom_layered_map_config.h b/examples/cpp/digital_twin/common/custom_layered_map_config.h new file mode 100644 index 000000000..babee14fc --- /dev/null +++ b/examples/cpp/digital_twin/common/custom_layered_map_config.h @@ -0,0 +1,162 @@ +#ifndef WAVEMAP_EXAMPLES_CPP_DIGITAL_TWIN_CUSTOM_LAYERED_MAP_CONFIG_H_ +#define WAVEMAP_EXAMPLES_CPP_DIGITAL_TWIN_CUSTOM_LAYERED_MAP_CONFIG_H_ + +#include +#include +#include +#include + +#include +#include + +#include +#include + +using wavemap::layered::DiscreteCompressionConfig; +using wavemap::layered::DiscreteLayer; +using wavemap::layered::LayeredMap; + +struct MyContinuousLayers { + float temperature = 0.f; + float intensity = 0.f; + + bool operator==(const MyContinuousLayers& other) const { + return temperature == other.temperature && intensity == other.intensity; + } +}; + +struct MyContinuousPolicy { + static MyContinuousLayers add(const MyContinuousLayers& lhs, + const MyContinuousLayers& rhs) { + return {lhs.temperature + rhs.temperature, lhs.intensity + rhs.intensity}; + } + + static MyContinuousLayers subtract(const MyContinuousLayers& lhs, + const MyContinuousLayers& rhs) { + return {lhs.temperature - rhs.temperature, lhs.intensity - rhs.intensity}; + } + + static MyContinuousLayers scale(const MyContinuousLayers& data, + wavemap::FloatingPoint factor) { + return {factor * data.temperature, factor * data.intensity}; + } +}; + +namespace wavemap { +template <> +struct CellDataTraits<::MyContinuousLayers> { + struct ThresholdConfig { + FloatingPoint min_temperature = -50.f; + FloatingPoint max_temperature = 80.f; + FloatingPoint min_intensity = 0.f; + FloatingPoint max_intensity = 1.f; + }; + + struct PruningConfig { + FieldPruningConfig temperature{}; + FieldPruningConfig intensity{}; + FloatingPoint combined_threshold = 1.f; + }; + + static void threshold(MyContinuousLayers& layers, + const ThresholdConfig& config) { + layers.temperature = std::clamp(layers.temperature, + config.min_temperature, + config.max_temperature); + layers.intensity = std::clamp(layers.intensity, config.min_intensity, + config.max_intensity); + } + + static FloatingPoint pruningScore(const MyContinuousLayers& layers, + const PruningConfig& config) { + return weightedPruningScore(std::abs(layers.temperature), + config.temperature) + + weightedPruningScore(std::abs(layers.intensity), config.intensity); + } + + static bool isNonzero(const MyContinuousLayers& layers, + const PruningConfig& config) { + return config.combined_threshold < pruningScore(layers, config); + } +}; +} // namespace wavemap + +struct MyDiscreteLayers { + explicit MyDiscreteLayers( + DiscreteCompressionConfig config = DiscreteCompressionConfig()) + : semantic_label(config), manually_changed(config) {} + + DiscreteLayer semantic_label; + DiscreteLayer manually_changed; +}; + +using MyLayeredMap = + LayeredMap; +using MyLayeredMapConfig = MyLayeredMap::Config; +using MyVoxel = MyLayeredMap::ContinuousVoxel; + +struct MyVoxelSerializer { + static void write(std::ostream& ostream, const MyVoxel& voxel) { + wavemap::io::StreamableCellData::write( + ostream, voxel.occupancy); + wavemap::io::StreamableCellData::write( + ostream, voxel.data.temperature); + wavemap::io::StreamableCellData::write( + ostream, voxel.data.intensity); + } + + static MyVoxel read(std::istream& istream) { + MyVoxel voxel; + voxel.occupancy = + wavemap::io::StreamableCellData::read(istream); + voxel.data.temperature = + wavemap::io::StreamableCellData::read(istream); + voxel.data.intensity = + wavemap::io::StreamableCellData::read(istream); + return voxel; + } +}; + +namespace wavemap::layered::io { +template <> +struct DiscreteLayerBundleTraits<::MyDiscreteLayers> { + static auto layers(MyDiscreteLayers& layers) { + return std::make_tuple( + namedDiscreteLayer("semantic_label", layers.semantic_label), + namedDiscreteLayer("manually_changed", layers.manually_changed)); + } + + static auto layers(const MyDiscreteLayers& layers) { + return std::make_tuple( + namedDiscreteLayer("semantic_label", layers.semantic_label), + namedDiscreteLayer("manually_changed", layers.manually_changed)); + } +}; +} // namespace wavemap::layered::io + +using MyLayeredMapIo = wavemap::layered::io::LayeredMapIo; + +inline MyLayeredMapConfig makeMyLayeredMapConfig() { + MyLayeredMapConfig config; + config.continuous_map.min_cell_width = 0.1f; + config.continuous_map.min_log_odds = -100.f; + config.continuous_map.max_log_odds = 100.f; + config.continuous_map.tree_height = 3; + config.continuous_threshold.data.min_temperature = -20.f; + config.continuous_threshold.data.max_temperature = 60.f; + config.continuous_threshold.data.min_intensity = 0.f; + config.continuous_threshold.data.max_intensity = 1.f; + config.continuous_pruning.occupancy.weight = 1.f; + config.continuous_pruning.occupancy.scale = 1e-3f; + config.continuous_pruning.data.temperature.weight = 1.f; + config.continuous_pruning.data.temperature.scale = 1e-3f; + config.continuous_pruning.data.intensity.weight = 1.f; + config.continuous_pruning.data.intensity.scale = 1e-3f; + config.continuous_pruning.combined_threshold = 1.f; + config.continuous_pruning.data.combined_threshold = 1.f; + config.discrete_compression.block_height = 1; + return config; +} + +#endif // WAVEMAP_EXAMPLES_CPP_DIGITAL_TWIN_CUSTOM_LAYERED_MAP_CONFIG_H_ diff --git a/examples/cpp/digital_twin/common/example_layered_map_config.h b/examples/cpp/digital_twin/common/example_layered_map_config.h new file mode 100644 index 000000000..a0451301f --- /dev/null +++ b/examples/cpp/digital_twin/common/example_layered_map_config.h @@ -0,0 +1,43 @@ +#ifndef WAVEMAP_EXAMPLES_CPP_DIGITAL_TWIN_EXAMPLE_LAYERED_MAP_CONFIG_H_ +#define WAVEMAP_EXAMPLES_CPP_DIGITAL_TWIN_EXAMPLE_LAYERED_MAP_CONFIG_H_ + +#include +#include +#include "layered_voxel_config.h" + +using wavemap::layered::DiscreteCompressionConfig; +using wavemap::layered::DiscreteLayer; +using wavemap::layered::LayeredMap; + +struct ExampleDiscreteLayers { + explicit ExampleDiscreteLayers( + DiscreteCompressionConfig config = DiscreteCompressionConfig()) + : semantic(config), changed(config) {} + + DiscreteLayer semantic; + DiscreteLayer changed; +}; + +using ExampleLayeredMap = + LayeredMap; +using ExampleLayeredMapConfig = ExampleLayeredMap::Config; +using ExampleLayeredMapIo = + wavemap::layered::io::LayeredMapIo; + +namespace wavemap::layered::io { +template <> +struct DiscreteLayerBundleTraits<::ExampleDiscreteLayers> { + static auto layers(ExampleDiscreteLayers& layers) { + return std::make_tuple(namedDiscreteLayer("semantic", layers.semantic), + namedDiscreteLayer("changed", layers.changed)); + } + + static auto layers(const ExampleDiscreteLayers& layers) { + return std::make_tuple(namedDiscreteLayer("semantic", layers.semantic), + namedDiscreteLayer("changed", layers.changed)); + } +}; +} // namespace wavemap::layered::io + +#endif // WAVEMAP_EXAMPLES_CPP_DIGITAL_TWIN_EXAMPLE_LAYERED_MAP_CONFIG_H_ diff --git a/examples/cpp/digital_twin/common/example_layered_map_ros_config.h b/examples/cpp/digital_twin/common/example_layered_map_ros_config.h new file mode 100644 index 000000000..f5868b80d --- /dev/null +++ b/examples/cpp/digital_twin/common/example_layered_map_ros_config.h @@ -0,0 +1,68 @@ +#ifndef WAVEMAP_EXAMPLES_CPP_DIGITAL_TWIN_EXAMPLE_LAYERED_MAP_ROS_CONFIG_H_ +#define WAVEMAP_EXAMPLES_CPP_DIGITAL_TWIN_EXAMPLE_LAYERED_MAP_ROS_CONFIG_H_ + +#include + +#include + +#include +#include "example_layered_map_config.h" + +namespace wavemap::convert { +template <> +struct DiscreteLayerBundleRosTraits<::ExampleDiscreteLayers> { + static auto layers(ExampleDiscreteLayers& layers) { + return std::make_tuple( + namedDiscreteLayerForRos("semantic", layers.semantic), + namedDiscreteLayerForRos("changed", layers.changed)); + } + + static auto layers(const ExampleDiscreteLayers& layers) { + return std::make_tuple( + namedDiscreteLayerForRos("semantic", layers.semantic), + namedDiscreteLayerForRos("changed", layers.changed)); + } +}; +} // namespace wavemap::convert + + +namespace layered_map_viz { +struct ExampleSemanticColorPolicy { + bool operator()(int label, std_msgs::ColorRGBA& color_msg) const { + switch (label) { + case 1: + color_msg = color(0.1f, 0.6f, 1.f, 0.85f); + return true; + case 2: + color_msg = color(1.f, 0.5f, 0.1f, 0.85f); + return true; + default: + color_msg = color(0.7f, 0.7f, 0.7f, 0.6f); + return true; + } + } +}; + +struct ExampleChangedColorPolicy { + bool operator()(bool changed, std_msgs::ColorRGBA& color_msg) const { + if (!changed) { + return false; + } + color_msg = color(1.f, 0.05f, 0.05f, 1.f); + return true; + } +}; + +template <> +struct DiscreteLayerMarkerTraits<::ExampleDiscreteLayers> { + static auto layers(const ExampleDiscreteLayers& layers) { + return std::make_tuple( + namedDiscreteMarkerLayer("semantic", layers.semantic, + ExampleSemanticColorPolicy{}), + namedDiscreteMarkerLayer("changed", layers.changed, + ExampleChangedColorPolicy{})); + } +}; +} // namespace layered_map_viz + +#endif // WAVEMAP_EXAMPLES_CPP_DIGITAL_TWIN_EXAMPLE_LAYERED_MAP_ROS_CONFIG_H_ diff --git a/examples/cpp/digital_twin/common/layered_ros_converter.h b/examples/cpp/digital_twin/common/layered_ros_converter.h index 0e3a3de4f..5c5868b48 100644 --- a/examples/cpp/digital_twin/common/layered_ros_converter.h +++ b/examples/cpp/digital_twin/common/layered_ros_converter.h @@ -20,7 +20,9 @@ struct LayeredVoxelRosConverter { } static std::vector makeLayers() { - // The message stores custom values layer-major: all color values go into the color layer, and all traversability values go into the traversability layer. + // The message stores custom values layer-major: all color values go into + // the color layer, and all traversability values go into the traversability + // layer. wavemap_msgs::Layer color_layer; color_layer.name = "color"; color_layer.type = "float32_rgb"; @@ -32,24 +34,34 @@ struct LayeredVoxelRosConverter { return {color_layer, traversability_layer}; } - static void appendLayerValues(const LayeredVoxel& voxel, std::vector& layers) { + static void appendLayerValues(const LayeredVoxel& voxel, + std::vector& layers) { // Color uses three float values per voxel/coefficient: r, g, and b. - layers[0].float32_values.emplace_back(voxel.data.r); - layers[0].float32_values.emplace_back(voxel.data.g); - layers[0].float32_values.emplace_back(voxel.data.b); + layers[0].float32_values.emplace_back(voxel.data.rgb.r); + layers[0].float32_values.emplace_back(voxel.data.rgb.g); + layers[0].float32_values.emplace_back(voxel.data.rgb.b); // Traversability uses one float value per voxel/coefficient. layers[1].float32_values.emplace_back(voxel.data.traversability); } - static LayeredVoxel readCellData(wavemap::FloatingPoint occupancy, const std::vector& layers, size_t value_index) { - // The value_index selects which voxel/coefficient is being reconstructed inside each layer-major array. + static LayeredVoxel readCellData( + wavemap::FloatingPoint occupancy, + const std::vector& layers, size_t value_index) { + // The value_index selects which voxel/coefficient is being reconstructed + // inside each layer-major array. const size_t color_index = 3u * value_index; - return LayeredVoxel(occupancy, LayeredData{layers[0].float32_values[color_index], layers[0].float32_values[color_index + 1u], layers[0].float32_values[color_index + 2u], layers[1].float32_values[value_index]}); + const Rgb color = rgb(layers[0].float32_values[color_index], + layers[0].float32_values[color_index + 1u], + layers[0].float32_values[color_index + 2u]); + return LayeredVoxel( + occupancy, + ContinuousLayers{color, layers[1].float32_values[value_index]}); } }; -inline void populateLayeredCube(LayeredMap& map, std::vector& populated_indices) { +inline void populateLayeredCube( + ContinuousWaveletMap& map, std::vector& populated_indices) { const wavemap::Index3D cube_origin(8, 2, 2); constexpr int cube_size_x = 2; constexpr int cube_size_y = 2; @@ -58,14 +70,22 @@ inline void populateLayeredCube(LayeredMap& map, std::vector& size_t populated_voxels = 0u; populated_indices.reserve(cube_size_x * cube_size_y * cube_size_z); - // Populate a small cube so conversions and publishing touch several wavelet coefficients instead of a single trivial voxel. + // Populate a small cube so conversions and publishing touch several wavelet + // coefficients instead of a single trivial voxel. for (int dx = 0; dx < cube_size_x; ++dx) { for (int dy = 0; dy < cube_size_y; ++dy) { for (int dz = 0; dz < cube_size_z; ++dz) { - const wavemap::Index3D voxel_index = cube_origin + wavemap::Index3D(dx, dy, dz); - const wavemap::FloatingPoint layer_value = static_cast(populated_voxels); - const LayeredVoxel original_voxel(0.5f + 0.02f * layer_value, LayeredData{0.1f * dx, 0.1f * dy, 0.05f * dz, 0.9f - 0.03f * layer_value}); - const LayeredVoxel voxel_update(0.1f, LayeredData{-0.01f, -0.01f, -0.01f, -0.01f}); + const wavemap::Index3D voxel_index = + cube_origin + wavemap::Index3D(dx, dy, dz); + const wavemap::FloatingPoint layer_value = + static_cast(populated_voxels); + const LayeredVoxel original_voxel( + 0.5f + 0.02f * layer_value, + ContinuousLayers{rgb(0.1f * dx, 0.1f * dy, 0.05f * dz), + 0.9f - 0.03f * layer_value}); + const LayeredVoxel voxel_update( + 0.1f, + ContinuousLayers{rgb(-0.01f, -0.01f, -0.01f), -0.01f}); map.setVoxelValue(voxel_index, original_voxel); map.addToVoxelValue(voxel_index, voxel_update); diff --git a/examples/cpp/digital_twin/common/layered_voxel_config.h b/examples/cpp/digital_twin/common/layered_voxel_config.h index 2f309f09c..dfb550c29 100644 --- a/examples/cpp/digital_twin/common/layered_voxel_config.h +++ b/examples/cpp/digital_twin/common/layered_voxel_config.h @@ -1,68 +1,210 @@ #ifndef WAVEMAP_EXAMPLES_CPP_DIGITAL_TWIN_LAYERED_VOXEL_CONFIG_H_ #define WAVEMAP_EXAMPLES_CPP_DIGITAL_TWIN_LAYERED_VOXEL_CONFIG_H_ +#include +#include #include #include #include +#include +#include #include #include #include #include -struct LayeredData { +struct Rgb { float r = 0.f; float g = 0.f; float b = 0.f; + + bool operator==(const Rgb& other) const { + return r == other.r && g == other.g && b == other.b; + } +}; + +inline Rgb operator+(const Rgb& lhs, const Rgb& rhs) { + return {lhs.r + rhs.r, lhs.g + rhs.g, lhs.b + rhs.b}; +} + +inline Rgb operator-(const Rgb& lhs, const Rgb& rhs) { + return {lhs.r - rhs.r, lhs.g - rhs.g, lhs.b - rhs.b}; +} + +inline Rgb operator*(float factor, const Rgb& color) { + return {factor * color.r, factor * color.g, factor * color.b}; +} + +inline Rgb operator*(const Rgb& color, float factor) { + return factor * color; +} + +inline Rgb clampRgb(const Rgb& color, const Rgb& min, const Rgb& max) { + return {std::clamp(color.r, min.r, max.r), + std::clamp(color.g, min.g, max.g), + std::clamp(color.b, min.b, max.b)}; +} + +inline float maxAbs(const Rgb& color) { + return std::max({std::abs(color.r), std::abs(color.g), std::abs(color.b)}); +} + +inline float sumAbsDiff(const Rgb& lhs, const Rgb& rhs) { + return std::abs(lhs.r - rhs.r) + std::abs(lhs.g - rhs.g) + + std::abs(lhs.b - rhs.b); +} + +struct ContinuousLayers { + Rgb rgb{}; float traversability = 0.f; - bool operator==(const LayeredData& other) const { - return r == other.r && g == other.g && b == other.b && traversability == other.traversability; + bool operator==(const ContinuousLayers& other) const { + return rgb == other.rgb && traversability == other.traversability; } }; -struct LayeredDataPolicy { - static LayeredData add(const LayeredData& lhs, const LayeredData& rhs) { - return {lhs.r + rhs.r, lhs.g + rhs.g, lhs.b + rhs.b, lhs.traversability + rhs.traversability}; +struct ContinuousLayersPolicy { + static ContinuousLayers add(const ContinuousLayers& lhs, + const ContinuousLayers& rhs) { + return ContinuousLayers{lhs.rgb + rhs.rgb, + lhs.traversability + rhs.traversability}; } - static LayeredData subtract(const LayeredData& lhs, const LayeredData& rhs) { - return {lhs.r - rhs.r, lhs.g - rhs.g, lhs.b - rhs.b, lhs.traversability - rhs.traversability}; + static ContinuousLayers subtract(const ContinuousLayers& lhs, + const ContinuousLayers& rhs) { + return ContinuousLayers{lhs.rgb - rhs.rgb, + lhs.traversability - rhs.traversability}; } - static LayeredData scale(const LayeredData& data, wavemap::FloatingPoint factor) { - return {factor * data.r, factor * data.g, factor * data.b, factor * data.traversability}; + static ContinuousLayers scale(const ContinuousLayers& data, + wavemap::FloatingPoint factor) { + return ContinuousLayers{factor * data.rgb, factor * data.traversability}; } }; -using LayeredVoxel = wavemap::VoxelData; +namespace wavemap { +template <> +struct CellDataTraits<::ContinuousLayers> { + struct ThresholdConfig { + Rgb rgb_min{}; + Rgb rgb_max{1.f, 1.f, 1.f}; + FloatingPoint traversability_min = 0.f; + FloatingPoint traversability_max = 1.f; + }; + + struct PruningConfig { + FieldPruningConfig rgb{}; + FieldPruningConfig traversability{}; + FloatingPoint combined_threshold = 1.f; + }; + + static void threshold(ContinuousLayers& layers, + const ThresholdConfig& config) { + layers.rgb = clampRgb(layers.rgb, config.rgb_min, config.rgb_max); + layers.traversability = std::clamp(layers.traversability, + config.traversability_min, + config.traversability_max); + } + + static FloatingPoint pruningScore(const ContinuousLayers& layers, + const PruningConfig& config) { + return weightedPruningScore(maxAbs(layers.rgb), config.rgb) + + weightedPruningScore(std::abs(layers.traversability), + config.traversability); + } + + static bool isNonzero(const ContinuousLayers& layers, + const PruningConfig& config) { + return config.combined_threshold < pruningScore(layers, config); + } +}; +} // namespace wavemap + +using LayeredVoxel = wavemap::VoxelData; using LayeredBlock = wavemap::HashedWaveletOctreeBlockT; -using LayeredMap = wavemap::HashedWaveletOctreeT; +using ContinuousWaveletMap = wavemap::HashedWaveletOctreeT; +using LayeredPruningConfig = ContinuousWaveletMap::PruningConfig; + +// Pruning uses a normalized weighted score over all wavelet detail +// coefficients: score = sum(weight * abs(detail) / scale). A node is kept +// when score > combined_threshold. Set a field weight to 0 to ignore it. +inline LayeredPruningConfig makeLayeredPruningConfig( + float occupancy_weight, float rgb_weight, float traversability_weight, + float scale = 1e-3f, float combined_threshold = 1.f) { + LayeredPruningConfig config; + config.occupancy.scale = scale; + config.occupancy.weight = occupancy_weight; + config.data.rgb.scale = scale; + config.data.rgb.weight = rgb_weight; + config.data.traversability.scale = scale; + config.data.traversability.weight = traversability_weight; + config.combined_threshold = combined_threshold; + config.data.combined_threshold = combined_threshold; + return config; +} + +inline LayeredPruningConfig makeLayeredPruningConfigWithScales( + float occupancy_weight, float rgb_weight, float traversability_weight, + float occupancy_scale, float rgb_scale, float traversability_scale, + float combined_threshold = 1.f) { + LayeredPruningConfig config; + config.occupancy.scale = occupancy_scale; + config.occupancy.weight = occupancy_weight; + config.data.rgb.scale = rgb_scale; + config.data.rgb.weight = rgb_weight; + config.data.traversability.scale = traversability_scale; + config.data.traversability.weight = traversability_weight; + config.combined_threshold = combined_threshold; + config.data.combined_threshold = combined_threshold; + return config; +} + +inline LayeredPruningConfig makeOccupancyOnlyPruningConfig( + float scale = 1e-3f, float combined_threshold = 1.f) { + return makeLayeredPruningConfig(1.f, 0.f, 0.f, scale, combined_threshold); +} + +inline LayeredPruningConfig makeEqualLayerPruningConfig( + float scale = 1e-3f, float combined_threshold = 1.f) { + return makeLayeredPruningConfig(1.f, 1.f, 1.f, scale, combined_threshold); +} struct LayeredVoxelSerializer { static void write(std::ostream& ostream, const LayeredVoxel& voxel) { wavemap::io::StreamableCellData::write(ostream, voxel.occupancy); - wavemap::io::StreamableCellData::write(ostream, voxel.data.r); - wavemap::io::StreamableCellData::write(ostream, voxel.data.g); - wavemap::io::StreamableCellData::write(ostream, voxel.data.b); + wavemap::io::StreamableCellData::write(ostream, voxel.data.rgb.r); + wavemap::io::StreamableCellData::write(ostream, voxel.data.rgb.g); + wavemap::io::StreamableCellData::write(ostream, voxel.data.rgb.b); wavemap::io::StreamableCellData::write(ostream, voxel.data.traversability); } static LayeredVoxel read(std::istream& istream) { LayeredVoxel voxel; voxel.occupancy = wavemap::io::StreamableCellData::read(istream); - voxel.data.r = wavemap::io::StreamableCellData::read(istream); - voxel.data.g = wavemap::io::StreamableCellData::read(istream); - voxel.data.b = wavemap::io::StreamableCellData::read(istream); + voxel.data.rgb.r = wavemap::io::StreamableCellData::read(istream); + voxel.data.rgb.g = wavemap::io::StreamableCellData::read(istream); + voxel.data.rgb.b = wavemap::io::StreamableCellData::read(istream); voxel.data.traversability = wavemap::io::StreamableCellData::read(istream); return voxel; } }; -inline void printVoxel(const char* label, const LayeredVoxel& voxel) { - std::cout << label << " occupancy: " << voxel.occupancy << "\n"; - std::cout << label << " color: " << voxel.data.r << " " << voxel.data.g << " " << voxel.data.b << "\n"; - std::cout << label << " traversability: " << voxel.data.traversability << "\n"; +inline Rgb rgb(float r, float g, float b) { + return Rgb{r, g, b}; +} + +inline LayeredVoxel makeLayeredVoxel(wavemap::FloatingPoint occupancy, + const Rgb& rgb, + float traversability) { + return LayeredVoxel(occupancy, ContinuousLayers{rgb, traversability}); +} + +inline void printVoxel(const std::string& label, const LayeredVoxel& voxel) { + std::cout << label << ": occ=" << voxel.occupancy + << " trav=" << voxel.data.traversability << " rgb=(" + << voxel.data.rgb.r << ", " << voxel.data.rgb.g << ", " + << voxel.data.rgb.b << ")\n"; } #endif // WAVEMAP_EXAMPLES_CPP_DIGITAL_TWIN_LAYERED_VOXEL_CONFIG_H_ diff --git a/examples/cpp/digital_twin/common/layered_voxel_updater.h b/examples/cpp/digital_twin/common/layered_voxel_updater.h new file mode 100644 index 000000000..54d0cb132 --- /dev/null +++ b/examples/cpp/digital_twin/common/layered_voxel_updater.h @@ -0,0 +1,31 @@ +#ifndef WAVEMAP_EXAMPLES_CPP_DIGITAL_TWIN_LAYERED_VOXEL_UPDATER_H_ +#define WAVEMAP_EXAMPLES_CPP_DIGITAL_TWIN_LAYERED_VOXEL_UPDATER_H_ + +#include + +#include + +#include "layered_voxel_config.h" + +struct ContinuousLayerObservation { + wavemap::Index3D index; + ContinuousLayers data{}; +}; + +inline void setContinuousLayersAtVoxel( + ContinuousWaveletMap& map, const wavemap::Index3D& index, + const ContinuousLayers& observed_data) { + const LayeredVoxel current_voxel = map.getVoxelValue(index); + const ContinuousLayers data_update = + ContinuousLayersPolicy::subtract(observed_data, current_voxel.data); + map.addToVoxelValue(index, LayeredVoxel(0.f, data_update)); +} + +inline void setContinuousLayers( + ContinuousWaveletMap& map, const std::vector& observations) { + for (const ContinuousLayerObservation& observation : observations) { + setContinuousLayersAtVoxel(map, observation.index, observation.data); + } +} + +#endif // WAVEMAP_EXAMPLES_CPP_DIGITAL_TWIN_LAYERED_VOXEL_UPDATER_H_ diff --git a/examples/cpp/digital_twin/io/custom_layered_map_save_load_example.cc b/examples/cpp/digital_twin/io/custom_layered_map_save_load_example.cc new file mode 100644 index 000000000..e0b0b74d1 --- /dev/null +++ b/examples/cpp/digital_twin/io/custom_layered_map_save_load_example.cc @@ -0,0 +1,87 @@ +#include +#include +#include +#include + +#include "../common/custom_layered_map_config.h" + +namespace { +template +bool sameValue(const std::optional& value, const ValueT& expected) { + return value && *value == expected; +} + +bool sameVoxel(const MyVoxel& lhs, const MyVoxel& rhs) { + return lhs.occupancy == rhs.occupancy && lhs.data == rhs.data; +} +} // namespace + +int main() { + MyLayeredMapConfig config = makeMyLayeredMapConfig(); + MyLayeredMap map(config); + const std::vector indices{ + wavemap::Index3D(1, 2, 3), wavemap::Index3D(2, 2, 3)}; + + map.continuousMap().setVoxelValue( + indices[0], MyVoxel(12.f, MyContinuousLayers{22.5f, 0.75f})); + map.continuousMap().setVoxelValue( + indices[1], MyVoxel(14.f, MyContinuousLayers{24.0f, 0.50f})); + map.discreteLayers().semantic_label.setValue(indices[0], 4); + map.discreteLayers().semantic_label.setValue(indices[1], 7); + map.discreteLayers().manually_changed.setValue(indices[0], true); + map.discreteLayers().manually_changed.setValue(indices[1], false); + + const std::filesystem::path output_dir = "/home/ci/data/maps"; + std::filesystem::create_directories(output_dir); + const std::filesystem::path output_path = + output_dir / "custom_layered_map_full.lwvmp"; + + if (!MyLayeredMapIo::save(output_path, map)) { + std::cerr << "Failed to save custom LayeredMap.\n"; + return 1; + } + + MyLayeredMap loaded_map(config); + if (!MyLayeredMapIo::load(output_path, loaded_map)) { + std::cerr << "Failed to load custom LayeredMap.\n"; + return 1; + } + + size_t matching_continuous = 0u; + for (const wavemap::Index3D& index : indices) { + if (sameVoxel(map.continuousMap().getVoxelValue(index), + loaded_map.continuousMap().getVoxelValue(index))) { + ++matching_continuous; + } + } + + const bool semantic_ok = + sameValue(loaded_map.discreteLayers().semantic_label.getValue(indices[0]), + 4) && + sameValue(loaded_map.discreteLayers().semantic_label.getValue(indices[1]), + 7); + const bool changed_ok = + sameValue(loaded_map.discreteLayers().manually_changed.getValue(indices[0]), + true) && + sameValue(loaded_map.discreteLayers().manually_changed.getValue(indices[1]), + false); + const bool unknown_ok = + !loaded_map.discreteLayers().semantic_label.getValue( + wavemap::Index3D(9, 9, 9)); + + std::cout << "Custom LayeredMap save/load example\n"; + std::cout << "saved file: " << output_path << "\n"; + std::cout << "continuous voxels matched: " << matching_continuous << "/" + << indices.size() << "\n"; + std::cout << "semantic labels preserved: " << (semantic_ok ? "yes" : "no") + << "\n"; + std::cout << "manual changed flags preserved: " + << (changed_ok ? "yes" : "no") << "\n"; + std::cout << "unknown custom discrete voxel remains unknown: " + << (unknown_ok ? "yes" : "no") << "\n"; + + return matching_continuous == indices.size() && semantic_ok && changed_ok && + unknown_ok + ? 0 + : 1; +} diff --git a/examples/cpp/digital_twin/io/layered_map_io_experiment.cc b/examples/cpp/digital_twin/io/layered_map_io_experiment.cc index 2b37b97c3..3b8c77090 100644 --- a/examples/cpp/digital_twin/io/layered_map_io_experiment.cc +++ b/examples/cpp/digital_twin/io/layered_map_io_experiment.cc @@ -13,7 +13,7 @@ int main() { config.max_log_odds = 4.f; config.tree_height = 3; - LayeredMap map(config); + ContinuousWaveletMap map(config); const wavemap::Index3D cube_origin(8, 2, 2); constexpr int cube_size_x = 2; @@ -29,8 +29,8 @@ int main() { for (int dz = 0; dz < cube_size_z; ++dz) { const wavemap::Index3D voxel_index = cube_origin + wavemap::Index3D(dx, dy, dz); const wavemap::FloatingPoint layer_value = static_cast(populated_voxels); - const LayeredVoxel original_voxel(0.5f + 0.02f * layer_value, LayeredData{0.1f * dx, 0.1f * dy, 0.05f * dz, 0.9f - 0.03f * layer_value}); - const LayeredVoxel voxel_update(0.1f, LayeredData{0.01f, 0.02f, 0.03f, -0.01f}); + const LayeredVoxel original_voxel(0.5f + 0.02f * layer_value, ContinuousLayers{rgb(0.1f * dx, 0.1f * dy, 0.05f * dz), 0.9f - 0.03f * layer_value}); + const LayeredVoxel voxel_update(0.1f, ContinuousLayers{rgb(0.01f, 0.02f, 0.03f), -0.01f}); map.setVoxelValue(voxel_index, original_voxel); map.addToVoxelValue(voxel_index, voxel_update); @@ -48,7 +48,7 @@ int main() { return 1; } - LayeredMap::Ptr loaded_map; + ContinuousWaveletMap::Ptr loaded_map; if (!wavemap::io::fileToMap(output_path, loaded_map)) { std::cerr << "Failed to load layered map from: " << output_path << "\n"; return 1; diff --git a/examples/cpp/digital_twin/io/layered_map_save_load_experiment.cc b/examples/cpp/digital_twin/io/layered_map_save_load_experiment.cc new file mode 100644 index 000000000..21d022e6b --- /dev/null +++ b/examples/cpp/digital_twin/io/layered_map_save_load_experiment.cc @@ -0,0 +1,115 @@ +#include +#include +#include +#include + +#include "../common/example_layered_map_config.h" +#include + +namespace { +template +bool sameValue(const std::optional& value, const ValueT& expected) { + return value && *value == expected; +} + +bool sameVoxel(const LayeredVoxel& lhs, const LayeredVoxel& rhs) { + return lhs == rhs; +} +} // namespace + +int main() { + ExampleLayeredMapConfig config; + config.continuous_map.min_cell_width = 0.1f; + config.continuous_map.min_log_odds = -100.f; + config.continuous_map.max_log_odds = 100.f; + config.continuous_map.tree_height = 3; + config.discrete_compression.block_height = 1; + + ExampleLayeredMap map(config); + + const std::vector indices{ + wavemap::Index3D(0, 0, 0), wavemap::Index3D(1, 0, 0), + wavemap::Index3D(1, 1, 1), wavemap::Index3D(4, 0, 0)}; + + map.continuousMap().setVoxelValue( + indices[0], makeLayeredVoxel(10.f, rgb(1.f, 0.f, 0.f), 0.1f)); + map.continuousMap().setVoxelValue( + indices[1], makeLayeredVoxel(11.f, rgb(0.f, 1.f, 0.f), 0.2f)); + map.continuousMap().setVoxelValue( + indices[2], makeLayeredVoxel(12.f, rgb(0.f, 0.f, 1.f), 0.3f)); + map.continuousMap().setVoxelValue( + indices[3], makeLayeredVoxel(13.f, rgb(0.5f, 0.5f, 0.5f), 0.4f)); + + map.discreteLayers().semantic.setValue(indices[0], 1); + map.discreteLayers().semantic.setValue(indices[1], 1); + map.discreteLayers().semantic.setValue(indices[2], 2); + map.discreteLayers().semantic.setValue(indices[3], 4); + + map.discreteLayers().changed.setValue(indices[0], false); + map.discreteLayers().changed.setValue(indices[1], false); + map.discreteLayers().changed.setValue(indices[2], true); + map.discreteLayers().changed.setValue(indices[3], true); + + const std::filesystem::path output_dir = "/home/ci/data/maps"; + std::filesystem::create_directories(output_dir); + const std::filesystem::path output_path = output_dir / "layered_map_full.lwvmp"; + + if (!ExampleLayeredMapIo::save(output_path, map)) { + std::cerr << "Failed to save LayeredMap to " << output_path << "\n"; + return 1; + } + + ExampleLayeredMap loaded_map(config); + if (!ExampleLayeredMapIo::load(output_path, loaded_map)) { + std::cerr << "Failed to load LayeredMap from " << output_path << "\n"; + return 1; + } + + size_t matching_continuous = 0u; + for (const wavemap::Index3D& index : indices) { + if (sameVoxel(map.continuousMap().getVoxelValue(index), + loaded_map.continuousMap().getVoxelValue(index))) { + ++matching_continuous; + } + } + + const bool semantic_ok = + sameValue(loaded_map.discreteLayers().semantic.getValue(indices[0]), 1) && + sameValue(loaded_map.discreteLayers().semantic.getValue(indices[1]), 1) && + sameValue(loaded_map.discreteLayers().semantic.getValue(indices[2]), 2) && + sameValue(loaded_map.discreteLayers().semantic.getValue(indices[3]), 4); + const bool changed_ok = + sameValue(loaded_map.discreteLayers().changed.getValue(indices[0]), false) && + sameValue(loaded_map.discreteLayers().changed.getValue(indices[1]), false) && + sameValue(loaded_map.discreteLayers().changed.getValue(indices[2]), true) && + sameValue(loaded_map.discreteLayers().changed.getValue(indices[3]), true); + const bool unknown_stays_unknown = + !loaded_map.discreteLayers().semantic.getValue(wavemap::Index3D(2, 2, 2)); + const bool stats_ok = + loaded_map.discreteLayers().semantic.parentCount() == + map.discreteLayers().semantic.parentCount() && + loaded_map.discreteLayers().semantic.exceptionCount() == + map.discreteLayers().semantic.exceptionCount() && + loaded_map.discreteLayers().changed.parentCount() == + map.discreteLayers().changed.parentCount() && + loaded_map.discreteLayers().changed.exceptionCount() == + map.discreteLayers().changed.exceptionCount(); + + std::cout << "LayeredMap save/load experiment\n"; + std::cout << "saved file: " << output_path << "\n"; + std::cout << "continuous voxels matched: " << matching_continuous << "/" + << indices.size() << "\n"; + std::cout << "semantic values preserved: " << (semantic_ok ? "yes" : "no") + << "\n"; + std::cout << "changed values preserved: " << (changed_ok ? "yes" : "no") + << "\n"; + std::cout << "unknown discrete voxel remains unknown: " + << (unknown_stays_unknown ? "yes" : "no") << "\n"; + std::cout << "discrete compression stats preserved: " + << (stats_ok ? "yes" : "no") << "\n"; + + return matching_continuous == indices.size() && semantic_ok && changed_ok && + unknown_stays_unknown && stats_ok + ? 0 + : 1; +} diff --git a/examples/cpp/digital_twin/io/layered_map_save_load_failure_experiment.cc b/examples/cpp/digital_twin/io/layered_map_save_load_failure_experiment.cc new file mode 100644 index 000000000..e063b47c7 --- /dev/null +++ b/examples/cpp/digital_twin/io/layered_map_save_load_failure_experiment.cc @@ -0,0 +1,143 @@ +#include +#include +#include +#include +#include + +#include "../common/example_layered_map_config.h" +#include + +namespace { +bool expectFailure(const std::string& name, bool load_result) { + const bool failed = !load_result; + std::cout << " " << name << ": " << (failed ? "pass" : "FAIL") << "\n"; + return failed; +} + +bool replaceBytes(const std::filesystem::path& path, const std::string& from, + const std::string& to) { + if (from.size() != to.size()) { + return false; + } + + std::ifstream input(path, std::ios::binary); + if (!input.is_open()) { + return false; + } + std::string data((std::istreambuf_iterator(input)), + std::istreambuf_iterator()); + const size_t pos = data.find(from); + if (pos == std::string::npos) { + return false; + } + data.replace(pos, from.size(), to); + + std::ofstream output(path, std::ios::binary | std::ios::trunc); + if (!output.is_open()) { + return false; + } + output.write(data.data(), static_cast(data.size())); + return static_cast(output); +} + +bool writeHeaderOnlyFile(const std::filesystem::path& path, + const std::string& magic, uint32_t version) { + std::ofstream output(path, std::ios::binary | std::ios::trunc); + if (!output.is_open()) { + return false; + } + return wavemap::layered::io::detail::writeString(output, magic) && + wavemap::layered::io::detail::writePod(output, version); +} + +void fillExampleMap(ExampleLayeredMap& map) { + const wavemap::Index3D a(0, 0, 0); + const wavemap::Index3D b(1, 0, 0); + map.continuousMap().setVoxelValue( + a, makeLayeredVoxel(10.f, rgb(1.f, 0.f, 0.f), 0.1f)); + map.continuousMap().setVoxelValue( + b, makeLayeredVoxel(11.f, rgb(0.f, 1.f, 0.f), 0.2f)); + map.discreteLayers().semantic.setValue(a, 1); + map.discreteLayers().semantic.setValue(b, 2); + map.discreteLayers().changed.setValue(a, false); + map.discreteLayers().changed.setValue(b, true); +} +} // namespace + +int main() { + ExampleLayeredMapConfig config; + config.continuous_map.min_cell_width = 0.1f; + config.continuous_map.min_log_odds = -100.f; + config.continuous_map.max_log_odds = 100.f; + config.continuous_map.tree_height = 3; + config.discrete_compression.block_height = 1; + + const std::filesystem::path output_dir = "/home/ci/data/maps"; + std::filesystem::create_directories(output_dir); + + bool ok = true; + std::cout << "LayeredMap save/load failure experiment\n"; + + { + ExampleLayeredMap loaded(config); + ok &= expectFailure( + "missing file fails", + ExampleLayeredMapIo::load(output_dir / "missing_file.lwvmp", loaded)); + } + + { + const auto path = output_dir / "bad_magic.lwvmp"; + if (!writeHeaderOnlyFile(path, "BADMP", wavemap::layered::io::detail::kVersion)) { + std::cerr << "Failed to write bad magic test file.\n"; + return 1; + } + ExampleLayeredMap loaded(config); + ok &= expectFailure("wrong magic fails", + ExampleLayeredMapIo::load(path, loaded)); + } + + { + const auto path = output_dir / "bad_version.lwvmp"; + if (!writeHeaderOnlyFile(path, wavemap::layered::io::detail::kMagic, + wavemap::layered::io::detail::kVersion + 1u)) { + std::cerr << "Failed to write bad version test file.\n"; + return 1; + } + ExampleLayeredMap loaded(config); + ok &= expectFailure("unsupported version fails", + ExampleLayeredMapIo::load(path, loaded)); + } + + ExampleLayeredMap map(config); + fillExampleMap(map); + + { + const auto path = output_dir / "bad_layer_name.lwvmp"; + if (!ExampleLayeredMapIo::save(path, map) || + !replaceBytes(path, "semantic", "semantix")) { + std::cerr << "Failed to create bad layer name test file.\n"; + return 1; + } + ExampleLayeredMap loaded(config); + ok &= expectFailure("wrong discrete layer name fails", + ExampleLayeredMapIo::load(path, loaded)); + } + + { + const auto path = output_dir / "bad_layer_type.lwvmp"; + if (!ExampleLayeredMapIo::save(path, map) || + !replaceBytes(path, "bool", "b00l")) { + std::cerr << "Failed to create bad layer type test file.\n"; + return 1; + } + ExampleLayeredMap loaded(config); + ok &= expectFailure("wrong discrete layer type fails", + ExampleLayeredMapIo::load(path, loaded)); + } + + if (!ok) { + std::cout << "LayeredMap failure checks did not all fail as expected.\n"; + return 1; + } + return 0; +} diff --git a/examples/cpp/digital_twin/map_api/baseline/float_map_experiment.cc b/examples/cpp/digital_twin/map_api/baseline/float_map_experiment.cc new file mode 100644 index 000000000..20d81f4e4 --- /dev/null +++ b/examples/cpp/digital_twin/map_api/baseline/float_map_experiment.cc @@ -0,0 +1,54 @@ +#include + +#include "wavemap/core/indexing/ndtree_index.h" +#include "wavemap/core/map/hashed_wavelet_octree.h" + +int main() { + wavemap::HashedWaveletOctreeConfig config; + config.min_cell_width = 0.1f; + config.min_log_odds = -100.f; + config.max_log_odds = 100.f; + config.tree_height = 3; + + wavemap::HashedWaveletOctree map(config); + + const wavemap::Index3D a(1, 2, 3); + const wavemap::Index3D b(2, 2, 3); + const wavemap::Index3D c(3, 2, 3); + + map.setCellValue(a, 10.f); + map.setCellValue(b, 20.f); + map.setCellValue(c, 30.f); + + std::cout << "Before threshold/prune\n"; + std::cout << "a getCellValue: " << map.getCellValue(a) << "\n"; + std::cout << "b getCellValue: " << map.getCellValue(b) << "\n"; + std::cout << "c getCellValue: " << map.getCellValue(c) << "\n"; + std::cout << "block count: " << map.getHashMap().size() << "\n"; + std::cout << "node count: " << map.size() << "\n"; + + size_t visited_leaves = 0u; + map.forEachVoxelLeaf( + [&visited_leaves](const wavemap::OctreeIndex& index, + wavemap::FloatingPoint value) { + ++visited_leaves; + if (value != 0) { + std::cout << "leaf h=" << index.height << " pos=" + << index.position.transpose() << " value=" << value + << "\n"; + } + }); + std::cout << "visited leaves: " << visited_leaves << "\n"; + + map.threshold(); + map.prune(); + + std::cout << "After threshold/prune\n"; + std::cout << "a getCellValue: " << map.getCellValue(a) << "\n"; + std::cout << "b getCellValue: " << map.getCellValue(b) << "\n"; + std::cout << "c getCellValue: " << map.getCellValue(c) << "\n"; + std::cout << "block count: " << map.getHashMap().size() << "\n"; + std::cout << "node count: " << map.size() << "\n"; + + return 0; +} diff --git a/examples/cpp/digital_twin/map_api/baseline/int_map_experiment.cc b/examples/cpp/digital_twin/map_api/baseline/int_map_experiment.cc new file mode 100644 index 000000000..30670c500 --- /dev/null +++ b/examples/cpp/digital_twin/map_api/baseline/int_map_experiment.cc @@ -0,0 +1,54 @@ +#include + +#include "wavemap/core/indexing/ndtree_index.h" +#include "wavemap/core/map/hashed_wavelet_octree.h" + +int main() { + wavemap::HashedWaveletOctreeConfig config; + config.min_cell_width = 0.1f; + config.min_log_odds = -100.f; + config.max_log_odds = 100.f; + config.tree_height = 3; + + wavemap::HashedWaveletOctreeT map(config); + + const wavemap::Index3D a(1, 2, 3); + const wavemap::Index3D b(2, 2, 3); + const wavemap::Index3D c(3, 2, 3); + + map.setVoxelValue(a, 10); + map.setVoxelValue(b, 20); + map.setVoxelValue(c, 30); + + std::cout << "Before threshold/prune\n"; + std::cout << "a getVoxelValue: " << map.getVoxelValue(a) << "\n"; + std::cout << "b getVoxelValue: " << map.getVoxelValue(b) << "\n"; + std::cout << "c getVoxelValue: " << map.getVoxelValue(c) << "\n"; + std::cout << "a getCellValue: " << map.getCellValue(a) << "\n"; + std::cout << "block count: " << map.getHashMap().size() << "\n"; + std::cout << "node count: " << map.size() << "\n"; + + size_t visited_leaves = 0u; + map.forEachVoxelLeaf( + [&visited_leaves](const wavemap::OctreeIndex& index, int value) { + ++visited_leaves; + if (value != 0) { + std::cout << "leaf h=" << index.height << " pos=" + << index.position.transpose() << " value=" << value + << "\n"; + } + }); + std::cout << "visited leaves: " << visited_leaves << "\n"; + + map.threshold(); + map.prune(); + + std::cout << "After threshold/prune\n"; + std::cout << "a getVoxelValue: " << map.getVoxelValue(a) << "\n"; + std::cout << "b getVoxelValue: " << map.getVoxelValue(b) << "\n"; + std::cout << "c getVoxelValue: " << map.getVoxelValue(c) << "\n"; + std::cout << "block count: " << map.getHashMap().size() << "\n"; + std::cout << "node count: " << map.size() << "\n"; + + return 0; +} diff --git a/examples/cpp/digital_twin/map_api/voxel_block_experiment.cc b/examples/cpp/digital_twin/map_api/baseline/voxel_block_experiment.cc similarity index 88% rename from examples/cpp/digital_twin/map_api/voxel_block_experiment.cc rename to examples/cpp/digital_twin/map_api/baseline/voxel_block_experiment.cc index 60489cd76..d9b0b82ef 100644 --- a/examples/cpp/digital_twin/map_api/voxel_block_experiment.cc +++ b/examples/cpp/digital_twin/map_api/baseline/voxel_block_experiment.cc @@ -1,6 +1,6 @@ #include -#include "../common/layered_voxel_config.h" +#include "../../common/layered_voxel_config.h" int main() { constexpr wavemap::IndexElement tree_height = 3; @@ -11,7 +11,7 @@ int main() { const wavemap::OctreeIndex voxel_index{0, wavemap::Index3D{1, 2, 3}}; const LayeredVoxel initial_voxel( - 0.8f, LayeredData{1.f, 0.f, 0.f, 0.75f}); + 0.8f, ContinuousLayers{rgb(1.f, 0.f, 0.f), 0.75f}); block.setVoxelValue(voxel_index, initial_voxel); printVoxel("Stored voxel", block.getVoxelValue(voxel_index)); @@ -20,7 +20,7 @@ int main() { printVoxel("After occupancy update", block.getVoxelValue(voxel_index)); const LayeredVoxel voxel_update( - 0.1f, LayeredData{0.f, 0.5f, 0.f, -0.25f}); + 0.1f, ContinuousLayers{rgb(0.f, 0.5f, 0.f), -0.25f}); block.addToVoxelValue(voxel_index, voxel_update); printVoxel("After full voxel update", block.getVoxelValue(voxel_index)); diff --git a/examples/cpp/digital_twin/map_api/voxel_map_experiment.cc b/examples/cpp/digital_twin/map_api/baseline/voxel_map_experiment.cc similarity index 86% rename from examples/cpp/digital_twin/map_api/voxel_map_experiment.cc rename to examples/cpp/digital_twin/map_api/baseline/voxel_map_experiment.cc index 4d2738d11..d91994a76 100644 --- a/examples/cpp/digital_twin/map_api/voxel_map_experiment.cc +++ b/examples/cpp/digital_twin/map_api/baseline/voxel_map_experiment.cc @@ -1,6 +1,6 @@ #include -#include "../common/layered_voxel_config.h" +#include "../../common/layered_voxel_config.h" int main() { wavemap::HashedWaveletOctreeConfig config; @@ -9,11 +9,11 @@ int main() { config.max_log_odds = 4.f; config.tree_height = 3; - LayeredMap map(config); + ContinuousWaveletMap map(config); const wavemap::Index3D voxel_index(9, 2, 3); const LayeredVoxel initial_voxel( - 0.8f, LayeredData{1.f, 0.f, 0.f, 0.75f}); + 0.8f, ContinuousLayers{rgb(1.f, 0.f, 0.f), 0.75f}); map.setVoxelValue(voxel_index, initial_voxel); printVoxel("Stored voxel", map.getVoxelValue(voxel_index)); @@ -22,7 +22,7 @@ int main() { printVoxel("After occupancy update", map.getVoxelValue(voxel_index)); const LayeredVoxel voxel_update( - 0.1f, LayeredData{0.f, 0.5f, 0.f, -0.25f}); + 0.1f, ContinuousLayers{rgb(0.f, 0.5f, 0.f), -0.25f}); map.addToVoxelValue(voxel_index, voxel_update); printVoxel("After full voxel update", map.getVoxelValue(voxel_index)); diff --git a/examples/cpp/digital_twin/map_api/benchmarks/continuous_voxel_many_voxels_benchmark.cc b/examples/cpp/digital_twin/map_api/benchmarks/continuous_voxel_many_voxels_benchmark.cc new file mode 100644 index 000000000..b6c21db70 --- /dev/null +++ b/examples/cpp/digital_twin/map_api/benchmarks/continuous_voxel_many_voxels_benchmark.cc @@ -0,0 +1,98 @@ +#include +#include +#include +#include +#include + +#include "../../common/layered_voxel_config.h" + +LayeredVoxel makeVoxel(int x, int y, int z) { + const auto occupancy = + static_cast(1 + ((3 * x + 5 * y + 7 * z) % 11)); + const float traversability = static_cast(x + y + z) / 18.f; + const float r = static_cast(x) / 7.f; + const float g = static_cast(y) / 7.f; + const float b = static_cast(z) / 3.f; + return makeLayeredVoxel(occupancy, rgb(r, g, b), traversability); +} + +float absError(const LayeredVoxel& read, const LayeredVoxel& expected) { + return std::abs(read.occupancy - expected.occupancy) + + std::abs(read.data.traversability - expected.data.traversability) + + std::abs(read.data.rgb.r - expected.data.rgb.r) + + std::abs(read.data.rgb.g - expected.data.rgb.g) + + std::abs(read.data.rgb.b - expected.data.rgb.b); +} + +int main() { + wavemap::HashedWaveletOctreeConfig config; + config.min_cell_width = 0.1f; + config.min_log_odds = -100.f; + config.max_log_odds = 100.f; + config.tree_height = 4; + + std::vector> samples; + samples.reserve(8 * 8 * 4); + for (int x = 0; x < 8; ++x) { + for (int y = 0; y < 8; ++y) { + for (int z = 0; z < 4; ++z) { + samples.emplace_back(wavemap::Index3D(x, y, z), makeVoxel(x, y, z)); + } + } + } + + ContinuousWaveletMap map(config); + + const auto write_start = std::chrono::steady_clock::now(); + for (const auto& [index, voxel] : samples) { + map.setVoxelValue(index, voxel); + } + const auto write_end = std::chrono::steady_clock::now(); + + float abs_error_sum = 0.f; + float max_abs_error = 0.f; + + const auto read_start = std::chrono::steady_clock::now(); + for (const auto& [index, expected] : samples) { + const LayeredVoxel read = map.getVoxelValue(index); + const float error = absError(read, expected); + abs_error_sum += error; + max_abs_error = std::max(max_abs_error, error); + } + const auto read_end = std::chrono::steady_clock::now(); + + const auto write_us = + std::chrono::duration_cast(write_end - + write_start) + .count(); + const auto read_us = + std::chrono::duration_cast(read_end - + read_start) + .count(); + + size_t non_zero_leaves = 0u; + map.forEachVoxelLeaf( + [&non_zero_leaves](const wavemap::OctreeIndex& /*index*/, + const LayeredVoxel& voxel) { + const bool non_zero = std::abs(voxel.occupancy) > 1e-5f || + std::abs(voxel.data.traversability) > 1e-5f || + std::abs(voxel.data.rgb.r) > 1e-5f || + std::abs(voxel.data.rgb.g) > 1e-5f || + std::abs(voxel.data.rgb.b) > 1e-5f; + if (non_zero) { + ++non_zero_leaves; + } + }); + + std::cout << "samples: " << samples.size() << "\n"; + std::cout << "combined abs error sum: " << abs_error_sum << "\n"; + std::cout << "max combined abs error: " << max_abs_error << "\n"; + std::cout << "non-zero leaves: " << non_zero_leaves << "\n"; + std::cout << "block count: " << map.getHashMap().size() << "\n"; + std::cout << "node count: " << map.size() << "\n"; + std::cout << "memory usage bytes: " << map.getMemoryUsage() << "\n"; + std::cout << "write time [us]: " << write_us << "\n"; + std::cout << "read time [us]: " << read_us << "\n"; + + return 0; +} diff --git a/examples/cpp/digital_twin/map_api/benchmarks/float_cast_discrete_many_voxels_experiment.cc b/examples/cpp/digital_twin/map_api/benchmarks/float_cast_discrete_many_voxels_experiment.cc new file mode 100644 index 000000000..1f966e80e --- /dev/null +++ b/examples/cpp/digital_twin/map_api/benchmarks/float_cast_discrete_many_voxels_experiment.cc @@ -0,0 +1,168 @@ +#include +#include +#include +#include +#include + +#include "wavemap/core/indexing/ndtree_index.h" +#include "wavemap/core/map/cell_types/voxel_data.h" +#include "wavemap/core/map/hashed_wavelet_octree.h" + +struct CastDiscreteData { + float label = 0.f; + float dynamic = 0.f; + float code = 0.f; + + bool operator==(const CastDiscreteData& other) const { + return label == other.label && dynamic == other.dynamic && + code == other.code; + } +}; + +struct CastDiscreteDataPolicy { + static CastDiscreteData add(const CastDiscreteData& lhs, + const CastDiscreteData& rhs) { + return {lhs.label + rhs.label, lhs.dynamic + rhs.dynamic, + lhs.code + rhs.code}; + } + + static CastDiscreteData subtract(const CastDiscreteData& lhs, + const CastDiscreteData& rhs) { + return {lhs.label - rhs.label, lhs.dynamic - rhs.dynamic, + lhs.code - rhs.code}; + } + + static CastDiscreteData scale(const CastDiscreteData& data, + wavemap::FloatingPoint factor) { + return {factor * data.label, factor * data.dynamic, factor * data.code}; + } +}; + +struct RawDiscreteData { + int label = 0; + bool dynamic = false; + char code = '\0'; + + bool operator==(const RawDiscreteData& other) const { + return label == other.label && dynamic == other.dynamic && + code == other.code; + } +}; + +using CastVoxel = wavemap::VoxelData; +using CastMap = wavemap::HashedWaveletOctreeT; + +CastVoxel toVoxel(wavemap::FloatingPoint occupancy, + const RawDiscreteData& data) { + return CastVoxel( + occupancy, + {static_cast(data.label), data.dynamic ? 1.f : 0.f, + static_cast(static_cast(data.code))}); +} + +RawDiscreteData fromVoxelRounded(const CastVoxel& voxel) { + return {static_cast(std::lround(voxel.data.label)), + std::lround(voxel.data.dynamic) != 0, + static_cast(std::lround(voxel.data.code))}; +} + +RawDiscreteData makeData(int x, int y, int z) { + return {1 + ((3 * x + 5 * y + 7 * z) % 11), + ((x + y + z) % 2) == 0, + static_cast('A' + ((x + 2 * y + 3 * z) % 26))}; +} + +void printMismatch(const wavemap::Index3D& index, + const RawDiscreteData& expected, + const CastVoxel& read_voxel, + const RawDiscreteData& rounded) { + std::cout << "mismatch at " << index.transpose() << " expected=(label=" + << expected.label << ", dynamic=" << std::boolalpha + << expected.dynamic << ", code=" << expected.code + << ") read_f=(label=" << read_voxel.data.label + << ", dynamic=" << read_voxel.data.dynamic + << ", code=" << read_voxel.data.code << ") rounded=(label=" + << rounded.label << ", dynamic=" << rounded.dynamic + << ", code=" << rounded.code << ")\n"; +} + +int main() { + wavemap::HashedWaveletOctreeConfig config; + config.min_cell_width = 0.1f; + config.min_log_odds = -100.f; + config.max_log_odds = 100.f; + config.tree_height = 4; + + CastMap map(config); + + std::vector> samples; + samples.reserve(8 * 8 * 4); + for (int x = 0; x < 8; ++x) { + for (int y = 0; y < 8; ++y) { + for (int z = 0; z < 4; ++z) { + samples.emplace_back(wavemap::Index3D(x, y, z), makeData(x, y, z)); + } + } + } + + for (const auto& [index, data] : samples) { + const wavemap::FloatingPoint occupancy = + static_cast(data.label); + map.setVoxelValue(index, toVoxel(occupancy, data)); + } + + size_t preserved = 0u; + size_t label_errors = 0u; + size_t dynamic_errors = 0u; + size_t code_errors = 0u; + constexpr size_t kMaxPrintedMismatches = 12u; + size_t printed_mismatches = 0u; + + for (const auto& [index, expected] : samples) { + const CastVoxel read_voxel = map.getVoxelValue(index); + const RawDiscreteData rounded = fromVoxelRounded(read_voxel); + if (rounded == expected) { + ++preserved; + continue; + } + + if (rounded.label != expected.label) { + ++label_errors; + } + if (rounded.dynamic != expected.dynamic) { + ++dynamic_errors; + } + if (rounded.code != expected.code) { + ++code_errors; + } + if (printed_mismatches < kMaxPrintedMismatches) { + printMismatch(index, expected, read_voxel, rounded); + ++printed_mismatches; + } + } + + size_t interesting_leaves = 0u; + map.forEachVoxelLeaf( + [&interesting_leaves](const wavemap::OctreeIndex& /*index*/, + const CastVoxel& voxel) { + const bool interesting = std::abs(voxel.occupancy) > 1e-5f || + std::abs(voxel.data.label) > 1e-5f || + std::abs(voxel.data.dynamic) > 1e-5f || + std::abs(voxel.data.code) > 1e-5f; + if (interesting) { + ++interesting_leaves; + } + }); + + std::cout << "samples: " << samples.size() << "\n"; + std::cout << "preserved after rounding: " << preserved << "/" + << samples.size() << "\n"; + std::cout << "label errors: " << label_errors << "\n"; + std::cout << "dynamic errors: " << dynamic_errors << "\n"; + std::cout << "code errors: " << code_errors << "\n"; + std::cout << "interesting leaves: " << interesting_leaves << "\n"; + std::cout << "block count: " << map.getHashMap().size() << "\n"; + std::cout << "node count: " << map.size() << "\n"; + + return preserved == samples.size() ? 0 : 1; +} diff --git a/examples/cpp/digital_twin/map_api/benchmarks/layered_pruning_policy_experiment.cc b/examples/cpp/digital_twin/map_api/benchmarks/layered_pruning_policy_experiment.cc new file mode 100644 index 000000000..c341e3a03 --- /dev/null +++ b/examples/cpp/digital_twin/map_api/benchmarks/layered_pruning_policy_experiment.cc @@ -0,0 +1,373 @@ +#include +#include +#include +#include +#include +#include + +#include "../../common/layered_voxel_config.h" + +struct Sample { + wavemap::Index3D index; + LayeredVoxel expected; +}; + +struct ErrorStats { + float occupancy_sum = 0.f; + float rgb_sum = 0.f; + float traversability_sum = 0.f; + float max_occupancy = 0.f; + float max_rgb = 0.f; + float max_traversability = 0.f; +}; + +struct PruningRunStats { + std::string label; + std::string dataset; + size_t nodes_before = 0u; + size_t nodes_after = 0u; + size_t memory_before = 0u; + size_t memory_after = 0u; + long long write_us = 0; + long long read_before_us = 0; + long long threshold_prune_us = 0; + ErrorStats before_expected; + ErrorStats after_expected; + ErrorStats prune_delta; +}; + +LayeredVoxel makeLargeExpectedVoxel(int x, int y, int z) { + const wavemap::FloatingPoint occupancy = 1.f; + const Rgb color = rgb(static_cast(x) / 49.f, + static_cast(y) / 49.f, + static_cast(z) / 39.f); + const float traversability = + static_cast((x + 2 * y + 3 * z) % 97) / 96.f; + return makeLayeredVoxel(occupancy, color, traversability); +} + +std::vector makeLargeSamples() { + constexpr int kSizeX = 50; + constexpr int kSizeY = 50; + constexpr int kSizeZ = 40; + + std::vector samples; + samples.reserve(kSizeX * kSizeY * kSizeZ); + for (int x = 0; x < kSizeX; ++x) { + for (int y = 0; y < kSizeY; ++y) { + for (int z = 0; z < kSizeZ; ++z) { + const wavemap::Index3D index(x, y, z); + samples.push_back({index, makeLargeExpectedVoxel(x, y, z)}); + } + } + } + return samples; +} + +LayeredVoxel makeLayerBoundaryExpectedVoxel(int x, int y, int z) { + const wavemap::FloatingPoint occupancy = 0.5f; + + Rgb color = rgb(0.2f, 0.2f, 0.2f); + if (16 <= x) { + color = rgb(0.9f, 0.1f, 0.1f); + } + if (16 <= y) { + color = rgb(0.1f, 0.1f, 0.9f); + } + + float traversability = 0.25f; + if (8 <= z && z < 24) { + traversability = 0.85f; + } + + return makeLayeredVoxel(occupancy, color, traversability); +} + +std::vector makeLayerBoundarySamples() { + constexpr int kSize = 32; + + std::vector samples; + samples.reserve(kSize * kSize * kSize); + for (int x = 0; x < kSize; ++x) { + for (int y = 0; y < kSize; ++y) { + for (int z = 0; z < kSize; ++z) { + const wavemap::Index3D index(x, y, z); + samples.push_back({index, makeLayerBoundaryExpectedVoxel(x, y, z)}); + } + } + } + return samples; +} + +float rgbError(const LayeredVoxel& read, const LayeredVoxel& expected) { + return sumAbsDiff(read.data.rgb, expected.data.rgb); +} + +float traversabilityError(const LayeredVoxel& read, + const LayeredVoxel& expected) { + return std::abs(read.data.traversability - expected.data.traversability); +} + +float occupancyError(const LayeredVoxel& read, const LayeredVoxel& expected) { + return std::abs(read.occupancy - expected.occupancy); +} + +ErrorStats computeErrorsAgainstExpected(const ContinuousWaveletMap& map, + const std::vector& samples) { + ErrorStats stats; + for (const auto& sample : samples) { + const LayeredVoxel read = map.getVoxelValue(sample.index); + const float occ_error = occupancyError(read, sample.expected); + const float color_error = rgbError(read, sample.expected); + const float trav_error = traversabilityError(read, sample.expected); + stats.occupancy_sum += occ_error; + stats.rgb_sum += color_error; + stats.traversability_sum += trav_error; + stats.max_occupancy = std::max(stats.max_occupancy, occ_error); + stats.max_rgb = std::max(stats.max_rgb, color_error); + stats.max_traversability = std::max(stats.max_traversability, trav_error); + } + return stats; +} + +ErrorStats computeErrorsAgainstBaseline( + const ContinuousWaveletMap& map, const std::vector& samples, + const std::vector& baseline) { + ErrorStats stats; + for (size_t sample_idx = 0; sample_idx < samples.size(); ++sample_idx) { + const LayeredVoxel read = map.getVoxelValue(samples[sample_idx].index); + const LayeredVoxel& expected = baseline[sample_idx]; + const float occ_error = occupancyError(read, expected); + const float color_error = rgbError(read, expected); + const float trav_error = traversabilityError(read, expected); + stats.occupancy_sum += occ_error; + stats.rgb_sum += color_error; + stats.traversability_sum += trav_error; + stats.max_occupancy = std::max(stats.max_occupancy, occ_error); + stats.max_rgb = std::max(stats.max_rgb, color_error); + stats.max_traversability = std::max(stats.max_traversability, trav_error); + } + return stats; +} + +std::vector readBackAll(const ContinuousWaveletMap& map, + const std::vector& samples) { + std::vector values; + values.reserve(samples.size()); + for (const auto& sample : samples) { + values.push_back(map.getVoxelValue(sample.index)); + } + return values; +} + +bool detailsNonzero(const LayeredBlock::Coefficients::Details& details, + const ContinuousWaveletMap::PruningConfig& config) { + return std::any_of(details.cbegin(), details.cend(), + [&config](const LayeredVoxel& voxel) { + return wavemap::CellDataTraits::isNonzero( + voxel, config); + }); +} + +float detailScore(const LayeredVoxel& voxel, + const ContinuousWaveletMap::PruningConfig& config) { + return wavemap::CellDataTraits::pruningScore(voxel, config); +} + +void printCoefficientPolicyCheck() { + LayeredBlock::Coefficients::Details zero_details{}; + LayeredBlock::Coefficients::Details occupancy_details{}; + LayeredBlock::Coefficients::Details color_details{}; + LayeredBlock::Coefficients::Details combined_small_details{}; + + occupancy_details[0] = makeLayeredVoxel(0.25f, rgb(0.f, 0.f, 0.f), 0.f); + color_details[0] = makeLayeredVoxel(0.f, rgb(1.f, 0.f, 0.f), 0.5f); + combined_small_details[0] = + makeLayeredVoxel(0.0004f, rgb(0.0004f, 0.f, 0.f), 0.0004f); + + const auto occupancy_only = makeLayeredPruningConfig(1.f, 0.f, 0.f); + const auto equal_layers = makeLayeredPruningConfig(1.f, 1.f, 1.f); + const auto traversability_heavy = makeLayeredPruningConfig(1.f, 1.f, 2.f); + + const auto print_case = [](const char* label, + const LayeredBlock::Coefficients::Details& details, + const ContinuousWaveletMap::PruningConfig& config) { + std::cout << " " << label << ": nonzero=" + << detailsNonzero(details, config) + << " score[0]=" << detailScore(details[0], config) << "\n"; + }; + + std::cout << "Direct coefficient pruning check\n"; + std::cout << " occupancy-only config\n"; + print_case("zero", zero_details, occupancy_only); + print_case("occupancy detail", occupancy_details, occupancy_only); + print_case("color/traversability detail", color_details, occupancy_only); + print_case("combined small details", combined_small_details, + occupancy_only); + + std::cout << " equal-layer config\n"; + print_case("zero", zero_details, equal_layers); + print_case("occupancy detail", occupancy_details, equal_layers); + print_case("color/traversability detail", color_details, equal_layers); + print_case("combined small details", combined_small_details, equal_layers); + + std::cout << " traversability-heavy config\n"; + print_case("zero", zero_details, traversability_heavy); + print_case("occupancy detail", occupancy_details, traversability_heavy); + print_case("color/traversability detail", color_details, + traversability_heavy); + print_case("combined small details", combined_small_details, + traversability_heavy); +} + +void printStats(const char* label, const ErrorStats& stats, + size_t sample_count) { + std::cout << label << "\n"; + std::cout << " occupancy error sum: " << stats.occupancy_sum << "\n"; + std::cout << " occupancy mean abs error: " + << stats.occupancy_sum / static_cast(sample_count) << "\n"; + std::cout << " max occupancy error: " << stats.max_occupancy << "\n"; + std::cout << " rgb error sum: " << stats.rgb_sum << "\n"; + std::cout << " rgb mean abs error: " + << stats.rgb_sum / static_cast(sample_count) << "\n"; + std::cout << " max rgb error: " << stats.max_rgb << "\n"; + std::cout << " traversability error sum: " << stats.traversability_sum + << "\n"; + std::cout << " traversability mean abs error: " + << stats.traversability_sum / static_cast(sample_count) + << "\n"; + std::cout << " max traversability error: " << stats.max_traversability + << "\n"; +} + +PruningRunStats runPruningExperiment( + const std::string& dataset, const std::string& label, + const wavemap::HashedWaveletOctreeConfig& config, + const ContinuousWaveletMap::PruningConfig& pruning_config, + const std::vector& samples) { + PruningRunStats stats; + stats.dataset = dataset; + stats.label = label; + + ContinuousWaveletMap map(config, ContinuousWaveletMap::ThresholdConfig{}, pruning_config); + + const auto write_start = std::chrono::steady_clock::now(); + for (const auto& sample : samples) { + map.setVoxelValue(sample.index, sample.expected); + } + const auto write_end = std::chrono::steady_clock::now(); + + const auto read_before_start = std::chrono::steady_clock::now(); + const std::vector before_prune_values = + readBackAll(map, samples); + const auto read_before_end = std::chrono::steady_clock::now(); + + stats.before_expected = computeErrorsAgainstExpected(map, samples); + stats.nodes_before = map.size(); + stats.memory_before = map.getMemoryUsage(); + + const auto prune_start = std::chrono::steady_clock::now(); + map.threshold(); + map.prune(); + const auto prune_end = std::chrono::steady_clock::now(); + + stats.after_expected = computeErrorsAgainstExpected(map, samples); + stats.prune_delta = + computeErrorsAgainstBaseline(map, samples, before_prune_values); + stats.nodes_after = map.size(); + stats.memory_after = map.getMemoryUsage(); + + stats.write_us = std::chrono::duration_cast( + write_end - write_start) + .count(); + stats.read_before_us = + std::chrono::duration_cast(read_before_end - + read_before_start) + .count(); + stats.threshold_prune_us = + std::chrono::duration_cast(prune_end - + prune_start) + .count(); + return stats; +} + +void printRunStats(const PruningRunStats& stats, size_t sample_count) { + std::cout << "Pruning loss test [" << stats.dataset << "] (" << stats.label + << ")\n"; + std::cout << " samples: " << sample_count << "\n"; + std::cout << " nodes before prune: " << stats.nodes_before << "\n"; + std::cout << " nodes after prune: " << stats.nodes_after << "\n"; + std::cout << " memory before prune bytes: " << stats.memory_before << "\n"; + std::cout << " memory after prune bytes: " << stats.memory_after << "\n"; + std::cout << " write time [us]: " << stats.write_us << "\n"; + std::cout << " read-before-threshold time [us]: " << stats.read_before_us + << "\n"; + std::cout << " threshold+prune time [us]: " << stats.threshold_prune_us + << "\n"; + + printStats("Error vs original values before prune", stats.before_expected, + sample_count); + printStats("Error vs original values after prune", stats.after_expected, + sample_count); + printStats("Extra error introduced by threshold+prune", stats.prune_delta, + sample_count); +} + +int main() { + printCoefficientPolicyCheck(); + std::cout << "\n"; + + wavemap::HashedWaveletOctreeConfig config; + config.min_cell_width = 0.1f; + config.min_log_odds = -100.f; + config.max_log_odds = 100.f; + config.tree_height = 4; + + const std::vector samples = makeLargeSamples(); + const std::vector layer_boundary_samples = + makeLayerBoundarySamples(); + const std::vector> configs{ + {"occupancy-only", makeLayeredPruningConfig(1.f, 0.f, 0.f)}, + {"equal weighted layers", makeLayeredPruningConfig(1.f, 1.f, 1.f)}, + {"traversability-heavy", makeLayeredPruningConfig(1.f, 1.f, 2.f)}}; + const std::vector> + coarse_occupancy_configs{ + {"coarse occupancy-only", + makeLayeredPruningConfigWithScales(1.f, 0.f, 0.f, 1.f, 0.1f, 0.1f)}, + {"coarse occupancy + equal layers", + makeLayeredPruningConfigWithScales(1.f, 1.f, 1.f, 1.f, 0.1f, 0.1f)}, + {"coarse occupancy + traversability-heavy", + makeLayeredPruningConfigWithScales(4.f, 1.f, 1.f, 1.f, 0.1f, 0.1f)}}; + + for (const auto& [label, pruning_config] : configs) { + const PruningRunStats stats = + runPruningExperiment("smooth-gradient", label, config, pruning_config, + samples); + printRunStats(stats, samples.size()); + std::cout << "\n"; + } + + for (const auto& [label, pruning_config] : configs) { + const PruningRunStats stats = runPruningExperiment( + "layer-boundary/default-scale", label, config, pruning_config, + layer_boundary_samples); + printRunStats(stats, layer_boundary_samples.size()); + std::cout << "\n"; + } + + for (const auto& [label, pruning_config] : coarse_occupancy_configs) { + const PruningRunStats stats = runPruningExperiment( + "layer-boundary/coarse-occupancy", label, config, pruning_config, + layer_boundary_samples); + printRunStats(stats, layer_boundary_samples.size()); + std::cout << "\n"; + } + + std::cout << "Interpretation: score = sum(weight * magnitude / scale). " + "A node is kept when the combined score is above the combined " + "threshold. Setting a layer weight to 0 ignores that layer; " + "increasing a weight makes that layer contribute more to the " + "combined pruning decision.\n"; + + return 0; +} diff --git a/examples/cpp/digital_twin/map_api/benchmarks/occupancy_many_voxels_benchmark.cc b/examples/cpp/digital_twin/map_api/benchmarks/occupancy_many_voxels_benchmark.cc new file mode 100644 index 000000000..beb404414 --- /dev/null +++ b/examples/cpp/digital_twin/map_api/benchmarks/occupancy_many_voxels_benchmark.cc @@ -0,0 +1,79 @@ +#include +#include +#include +#include + +#include "wavemap/core/indexing/ndtree_index.h" +#include "wavemap/core/map/hashed_wavelet_octree.h" + +wavemap::FloatingPoint makeOccupancy(int x, int y, int z) { + return static_cast(1 + ((3 * x + 5 * y + 7 * z) % 11)); +} + +int main() { + wavemap::HashedWaveletOctreeConfig config; + config.min_cell_width = 0.1f; + config.min_log_odds = -100.f; + config.max_log_odds = 100.f; + config.tree_height = 4; + + std::vector> samples; + samples.reserve(8 * 8 * 4); + for (int x = 0; x < 8; ++x) { + for (int y = 0; y < 8; ++y) { + for (int z = 0; z < 4; ++z) { + samples.emplace_back(wavemap::Index3D(x, y, z), makeOccupancy(x, y, z)); + } + } + } + + wavemap::HashedWaveletOctree map(config); + + const auto write_start = std::chrono::steady_clock::now(); + for (const auto& [index, occupancy] : samples) { + map.setCellValue(index, occupancy); + } + const auto write_end = std::chrono::steady_clock::now(); + + wavemap::FloatingPoint abs_error_sum = 0.f; + wavemap::FloatingPoint max_abs_error = 0.f; + + const auto read_start = std::chrono::steady_clock::now(); + for (const auto& [index, expected] : samples) { + const wavemap::FloatingPoint read = map.getCellValue(index); + const wavemap::FloatingPoint abs_error = std::abs(read - expected); + abs_error_sum += abs_error; + max_abs_error = std::max(max_abs_error, abs_error); + } + const auto read_end = std::chrono::steady_clock::now(); + + const auto write_us = + std::chrono::duration_cast(write_end - + write_start) + .count(); + const auto read_us = + std::chrono::duration_cast(read_end - + read_start) + .count(); + + size_t non_zero_leaves = 0u; + map.forEachVoxelLeaf( + [&non_zero_leaves](const wavemap::OctreeIndex& /*index*/, + wavemap::FloatingPoint value) { + if (std::abs(value) > 1e-5f) { + ++non_zero_leaves; + } + }); + + std::cout << "samples: " << samples.size() << "\n"; + std::cout << "abs error sum: " << abs_error_sum << "\n"; + std::cout << "max abs error: " << max_abs_error << "\n"; + std::cout << "non-zero leaves: " << non_zero_leaves << "\n"; + std::cout << "block count: " << map.getHashMap().size() << "\n"; + std::cout << "node count: " << map.size() << "\n"; + std::cout << "memory usage bytes: " << map.getMemoryUsage() << "\n"; + std::cout << "write time [us]: " << write_us << "\n"; + std::cout << "read time [us]: " << read_us << "\n"; + + return 0; +} diff --git a/examples/cpp/digital_twin/map_api/benchmarks/raw_discrete_many_voxels_experiment.cc b/examples/cpp/digital_twin/map_api/benchmarks/raw_discrete_many_voxels_experiment.cc new file mode 100644 index 000000000..c0d852be8 --- /dev/null +++ b/examples/cpp/digital_twin/map_api/benchmarks/raw_discrete_many_voxels_experiment.cc @@ -0,0 +1,108 @@ +#include +#include +#include +#include +#include + +#include "wavemap/core/indexing/ndtree_index.h" + +struct RawDiscreteData { + int label = 0; + bool dynamic = false; + char code = '\0'; + + bool operator==(const RawDiscreteData& other) const { + return label == other.label && dynamic == other.dynamic && + code == other.code; + } +}; + +struct IndexKey { + int x = 0; + int y = 0; + int z = 0; + + explicit IndexKey(const wavemap::Index3D& index) + : x(index.x()), y(index.y()), z(index.z()) {} + + bool operator<(const IndexKey& other) const { + return std::tie(x, y, z) < std::tie(other.x, other.y, other.z); + } +}; + +using RawStorage = std::map; + +RawDiscreteData makeData(int x, int y, int z) { + return {1 + ((3 * x + 5 * y + 7 * z) % 11), + ((x + y + z) % 2) == 0, + static_cast('A' + ((x + 2 * y + 3 * z) % 26))}; +} + +void printMismatch(const wavemap::Index3D& index, + const RawDiscreteData& expected, + const RawDiscreteData& read) { + std::cout << "mismatch at " << index.transpose() << " expected=(label=" + << expected.label << ", dynamic=" << std::boolalpha + << expected.dynamic << ", code=" << expected.code + << ") read=(label=" << read.label << ", dynamic=" << read.dynamic + << ", code=" << read.code << ")\n"; +} + +int main() { + std::vector> samples; + samples.reserve(8 * 8 * 4); + for (int x = 0; x < 8; ++x) { + for (int y = 0; y < 8; ++y) { + for (int z = 0; z < 4; ++z) { + samples.emplace_back(wavemap::Index3D(x, y, z), makeData(x, y, z)); + } + } + } + + RawStorage storage; + + const auto write_start = std::chrono::steady_clock::now(); + for (const auto& [index, data] : samples) { + storage[IndexKey(index)] = data; + } + const auto write_end = std::chrono::steady_clock::now(); + + size_t preserved = 0u; + size_t printed_mismatches = 0u; + constexpr size_t kMaxPrintedMismatches = 12u; + + const auto read_start = std::chrono::steady_clock::now(); + for (const auto& [index, expected] : samples) { + const RawDiscreteData read = storage.at(IndexKey(index)); + if (read == expected) { + ++preserved; + } else if (printed_mismatches < kMaxPrintedMismatches) { + printMismatch(index, expected, read); + ++printed_mismatches; + } + } + const auto read_end = std::chrono::steady_clock::now(); + + const auto write_us = + std::chrono::duration_cast(write_end - + write_start) + .count(); + const auto read_us = + std::chrono::duration_cast(read_end - + read_start) + .count(); + + const size_t approximate_payload_bytes = + storage.size() * (sizeof(IndexKey) + sizeof(RawDiscreteData)); + + std::cout << "samples: " << samples.size() << "\n"; + std::cout << "entries: " << storage.size() << "\n"; + std::cout << "preserved values: " << preserved << "/" << samples.size() + << "\n"; + std::cout << "approx payload bytes without map overhead: " + << approximate_payload_bytes << "\n"; + std::cout << "write time [us]: " << write_us << "\n"; + std::cout << "read time [us]: " << read_us << "\n"; + + return preserved == samples.size() ? 0 : 1; +} diff --git a/examples/cpp/digital_twin/map_api/benchmarks/raw_inside_map_many_voxels_experiment.cc b/examples/cpp/digital_twin/map_api/benchmarks/raw_inside_map_many_voxels_experiment.cc new file mode 100644 index 000000000..dadce57c8 --- /dev/null +++ b/examples/cpp/digital_twin/map_api/benchmarks/raw_inside_map_many_voxels_experiment.cc @@ -0,0 +1,158 @@ +#include +#include +#include +#include +#include +#include + +#include "wavemap/core/indexing/ndtree_index.h" +#include "wavemap/core/map/hashed_wavelet_octree.h" + +struct RawDiscreteData { + int label = 0; + bool dynamic = false; + char code = '\0'; + + bool operator==(const RawDiscreteData& other) const { + return label == other.label && dynamic == other.dynamic && + code == other.code; + } +}; + +struct IndexKey { + int x = 0; + int y = 0; + int z = 0; + + explicit IndexKey(const wavemap::Index3D& index) + : x(index.x()), y(index.y()), z(index.z()) {} + + bool operator<(const IndexKey& other) const { + return std::tie(x, y, z) < std::tie(other.x, other.y, other.z); + } +}; + +class RawDiscreteLayeredMap { + public: + explicit RawDiscreteLayeredMap( + const wavemap::HashedWaveletOctreeConfig& config) + : occupancy_map_(config) {} + + void setOccupancy(const wavemap::Index3D& index, + wavemap::FloatingPoint occupancy) { + occupancy_map_.setCellValue(index, occupancy); + } + + wavemap::FloatingPoint getOccupancy(const wavemap::Index3D& index) const { + return occupancy_map_.getCellValue(index); + } + + void setRawData(const wavemap::Index3D& index, const RawDiscreteData& data) { + raw_data_[IndexKey(index)] = data; + } + + std::optional getRawData( + const wavemap::Index3D& index) const { + const auto it = raw_data_.find(IndexKey(index)); + if (it == raw_data_.end()) { + return std::nullopt; + } + return it->second; + } + + size_t rawDataSize() const { return raw_data_.size(); } + size_t occupancyBlockCount() const { return occupancy_map_.getHashMap().size(); } + size_t occupancyNodeCount() const { return occupancy_map_.size(); } + + size_t approximateRawBytesWithoutMapOverhead() const { + return raw_data_.size() * (sizeof(IndexKey) + sizeof(RawDiscreteData)); + } + + private: + wavemap::HashedWaveletOctree occupancy_map_; + std::map raw_data_; +}; + +RawDiscreteData makeData(int x, int y, int z) { + return {1 + ((3 * x + 5 * y + 7 * z) % 11), + ((x + y + z) % 2) == 0, + static_cast('A' + ((x + 2 * y + 3 * z) % 26))}; +} + +void printMismatch(const wavemap::Index3D& index, + const RawDiscreteData& expected, + const RawDiscreteData& read, + wavemap::FloatingPoint occupancy) { + std::cout << "mismatch at " << index.transpose() << " occ=" << occupancy + << " expected=(label=" << expected.label << ", dynamic=" + << std::boolalpha << expected.dynamic << ", code=" + << expected.code << ") read=(label=" << read.label + << ", dynamic=" << read.dynamic << ", code=" << read.code + << ")\n"; +} + +int main() { + wavemap::HashedWaveletOctreeConfig config; + config.min_cell_width = 0.1f; + config.min_log_odds = -100.f; + config.max_log_odds = 100.f; + config.tree_height = 4; + + std::vector> samples; + samples.reserve(8 * 8 * 4); + for (int x = 0; x < 8; ++x) { + for (int y = 0; y < 8; ++y) { + for (int z = 0; z < 4; ++z) { + samples.emplace_back(wavemap::Index3D(x, y, z), makeData(x, y, z)); + } + } + } + + RawDiscreteLayeredMap map(config); + + const auto write_start = std::chrono::steady_clock::now(); + for (const auto& [index, data] : samples) { + map.setOccupancy(index, static_cast(data.label)); + map.setRawData(index, data); + } + const auto write_end = std::chrono::steady_clock::now(); + + size_t preserved_raw = 0u; + size_t printed_mismatches = 0u; + constexpr size_t kMaxPrintedMismatches = 12u; + + const auto read_start = std::chrono::steady_clock::now(); + for (const auto& [index, expected] : samples) { + const wavemap::FloatingPoint occupancy = map.getOccupancy(index); + const auto read = map.getRawData(index); + if (read && *read == expected) { + ++preserved_raw; + } else if (read && printed_mismatches < kMaxPrintedMismatches) { + printMismatch(index, expected, *read, occupancy); + ++printed_mismatches; + } + } + const auto read_end = std::chrono::steady_clock::now(); + + const auto write_us = + std::chrono::duration_cast(write_end - + write_start) + .count(); + const auto read_us = + std::chrono::duration_cast(read_end - + read_start) + .count(); + + std::cout << "samples: " << samples.size() << "\n"; + std::cout << "raw entries: " << map.rawDataSize() << "\n"; + std::cout << "preserved raw values: " << preserved_raw << "/" + << samples.size() << "\n"; + std::cout << "occupancy blocks: " << map.occupancyBlockCount() << "\n"; + std::cout << "occupancy nodes: " << map.occupancyNodeCount() << "\n"; + std::cout << "approx raw bytes without map overhead: " + << map.approximateRawBytesWithoutMapOverhead() << "\n"; + std::cout << "write time [us]: " << write_us << "\n"; + std::cout << "read time [us]: " << read_us << "\n"; + + return preserved_raw == samples.size() ? 0 : 1; +} diff --git a/examples/cpp/digital_twin/map_api/continuous/continuous_voxel_map_experiment.cc b/examples/cpp/digital_twin/map_api/continuous/continuous_voxel_map_experiment.cc new file mode 100644 index 000000000..ae779e911 --- /dev/null +++ b/examples/cpp/digital_twin/map_api/continuous/continuous_voxel_map_experiment.cc @@ -0,0 +1,65 @@ +#include +#include + +#include "../../common/layered_voxel_config.h" + +int main() { + wavemap::HashedWaveletOctreeConfig config; + config.min_cell_width = 0.1f; + config.min_log_odds = -100.f; + config.max_log_odds = 100.f; + config.tree_height = 3; + + ContinuousWaveletMap map(config); + + const wavemap::Index3D a(1, 2, 3); + const wavemap::Index3D b(2, 2, 3); + const wavemap::Index3D c(3, 2, 3); + + map.setVoxelValue(a, + makeLayeredVoxel(10.f, rgb(1.f, 0.f, 0.f), + 0.1f)); + map.setVoxelValue(b, + makeLayeredVoxel(20.f, rgb(0.f, 1.f, 0.f), + 0.5f)); + map.setVoxelValue(c, + makeLayeredVoxel(30.f, rgb(0.f, 1.f, 1.f), + 0.9f)); + + std::cout << "Before threshold/prune\n"; + printVoxel("a", map.getVoxelValue(a)); + printVoxel("b", map.getVoxelValue(b)); + printVoxel("c", map.getVoxelValue(c)); + std::cout << "block count: " << map.getHashMap().size() << "\n"; + std::cout << "node count: " << map.size() << "\n"; + + size_t non_zero_leaves = 0u; + map.forEachVoxelLeaf( + [&non_zero_leaves](const wavemap::OctreeIndex& index, + const LayeredVoxel& voxel) { + const bool has_data = std::abs(voxel.occupancy) > 1e-5f || + std::abs(voxel.data.traversability) > 1e-5f || + std::abs(voxel.data.rgb.r) > 1e-5f || + std::abs(voxel.data.rgb.g) > 1e-5f || + std::abs(voxel.data.rgb.b) > 1e-5f; + if (has_data) { + ++non_zero_leaves; + std::cout << "leaf h=" << index.height << " pos=" + << index.position.transpose(); + printVoxel(" value", voxel); + } + }); + std::cout << "non-zero leaves: " << non_zero_leaves << "\n"; + + map.threshold(); + map.prune(); + + std::cout << "After threshold/prune\n"; + printVoxel("a", map.getVoxelValue(a)); + printVoxel("b", map.getVoxelValue(b)); + printVoxel("c", map.getVoxelValue(c)); + std::cout << "block count: " << map.getHashMap().size() << "\n"; + std::cout << "node count: " << map.size() << "\n"; + + return 0; +} diff --git a/examples/cpp/digital_twin/map_api/continuous/heterogeneous_voxel_map_experiment.cc b/examples/cpp/digital_twin/map_api/continuous/heterogeneous_voxel_map_experiment.cc new file mode 100644 index 000000000..d9365659f --- /dev/null +++ b/examples/cpp/digital_twin/map_api/continuous/heterogeneous_voxel_map_experiment.cc @@ -0,0 +1,135 @@ +#include +#include +#include + +#include "wavemap/core/indexing/ndtree_index.h" +#include "wavemap/core/map/cell_types/voxel_data.h" +#include "wavemap/core/map/hashed_wavelet_octree.h" + +struct HeterogeneousData { + float temperature = 0.f; + int label = 0; + bool dynamic = false; + std::string name; + + bool operator==(const HeterogeneousData& other) const { + return temperature == other.temperature && label == other.label && + dynamic == other.dynamic && name == other.name; + } +}; + +struct HeterogeneousDataPolicy { + static HeterogeneousData add(const HeterogeneousData& lhs, + const HeterogeneousData& rhs) { + return {lhs.temperature + rhs.temperature, lhs.label + rhs.label, + lhs.dynamic || rhs.dynamic, mergeName(lhs.name, rhs.name)}; + } + + static HeterogeneousData subtract(const HeterogeneousData& lhs, + const HeterogeneousData& rhs) { + return {lhs.temperature - rhs.temperature, lhs.label - rhs.label, + lhs.dynamic != rhs.dynamic, keepName(lhs.name, rhs.name)}; + } + + static HeterogeneousData scale(const HeterogeneousData& data, + wavemap::FloatingPoint factor) { + return {factor * data.temperature, static_cast(factor * data.label), + data.dynamic && std::abs(factor) > 1e-6f, data.name}; + } + + private: + static std::string mergeName(const std::string& lhs, + const std::string& rhs) { + if (!lhs.empty()) { + return lhs; + } + return rhs; + } + + static std::string keepName(const std::string& lhs, + const std::string& rhs) { + if (!lhs.empty()) { + return lhs; + } + return rhs; + } +}; + +using HeterogeneousVoxel = + wavemap::VoxelData; +using HeterogeneousMap = wavemap::HashedWaveletOctreeT; + +void printVoxel(const std::string& name, const HeterogeneousVoxel& voxel) { + std::cout << name << ": occ=" << voxel.occupancy + << " temperature=" << voxel.data.temperature + << " label=" << voxel.data.label + << " dynamic=" << std::boolalpha << voxel.data.dynamic + << " name=\"" << voxel.data.name << "\"\n"; +} + +int main() { + wavemap::HashedWaveletOctreeConfig config; + config.min_cell_width = 0.1f; + config.min_log_odds = -100.f; + config.max_log_odds = 100.f; + config.tree_height = 3; + + HeterogeneousMap map(config); + + const wavemap::Index3D a(1, 2, 3); + const wavemap::Index3D b(2, 2, 3); + const wavemap::Index3D c(3, 2, 3); + + const HeterogeneousVoxel voxel_a( + 10.f, HeterogeneousData{22.5f, 1, false, "wall"}); + const HeterogeneousVoxel voxel_b( + 20.f, HeterogeneousData{24.0f, 5, true, "chair"}); + const HeterogeneousVoxel voxel_c( + 30.f, HeterogeneousData{25.5f, 9, false, "table"}); + + map.setVoxelValue(a, voxel_a); + map.setVoxelValue(b, voxel_b); + map.setVoxelValue(c, voxel_c); + + std::cout << "Expected values\n"; + printVoxel("a expected", voxel_a); + printVoxel("b expected", voxel_b); + printVoxel("c expected", voxel_c); + + std::cout << "Read through wavemap/Haar\n"; + printVoxel("a read", map.getVoxelValue(a)); + printVoxel("b read", map.getVoxelValue(b)); + printVoxel("c read", map.getVoxelValue(c)); + std::cout << "block count: " << map.getHashMap().size() << "\n"; + std::cout << "node count: " << map.size() << "\n"; + + size_t interesting_leaves = 0u; + map.forEachVoxelLeaf( + [&interesting_leaves](const wavemap::OctreeIndex& index, + const HeterogeneousVoxel& voxel) { + const bool interesting = + std::abs(voxel.occupancy) > 1e-5f || + std::abs(voxel.data.temperature) > 1e-5f || + voxel.data.label != 0 || voxel.data.dynamic || + !voxel.data.name.empty(); + if (interesting) { + ++interesting_leaves; + std::cout << "leaf h=" << index.height << " pos=" + << index.position.transpose(); + printVoxel(" value", voxel); + } + }); + std::cout << "interesting leaves: " << interesting_leaves << "\n"; + + map.threshold(); + map.prune(); + + std::cout << "After threshold/prune\n"; + printVoxel("a read", map.getVoxelValue(a)); + printVoxel("b read", map.getVoxelValue(b)); + printVoxel("c read", map.getVoxelValue(c)); + std::cout << "block count: " << map.getHashMap().size() << "\n"; + std::cout << "node count: " << map.size() << "\n"; + + return 0; +} diff --git a/examples/cpp/digital_twin/map_api/continuous/layered_full_update_pruning_experiment.cc b/examples/cpp/digital_twin/map_api/continuous/layered_full_update_pruning_experiment.cc new file mode 100644 index 000000000..920e7034e --- /dev/null +++ b/examples/cpp/digital_twin/map_api/continuous/layered_full_update_pruning_experiment.cc @@ -0,0 +1,243 @@ +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include "../../common/layered_voxel_config.h" +#include "../../common/layered_voxel_updater.h" + +namespace { +struct RunConfig { + std::string name; + LayeredPruningConfig pruning_config; +}; + +struct RunResult { + std::string name; + size_t nodes_before_prune = 0u; + size_t nodes_after_prune = 0u; + size_t memory_before_prune = 0u; + size_t memory_after_prune = 0u; + float mean_occupancy_delta_after_prune = 0.f; + float mean_rgb_delta_after_prune = 0.f; + float mean_traversability_delta_after_prune = 0.f; + float max_occupancy_delta_after_prune = 0.f; + float max_rgb_delta_after_prune = 0.f; + float max_traversability_delta_after_prune = 0.f; +}; + +ContinuousLayers makeLayerObservation(int x, int y, int z) { + Rgb color = rgb(0.2f, 0.2f, 0.2f); + if (13 <= x) { + color = rgb(0.9f, 0.1f, 0.1f); + } + if (21 <= y) { + color = rgb(0.1f, 0.1f, 0.9f); + } + + const float traversability = (7 <= z && z < 23) ? 0.85f : 0.25f; + return ContinuousLayers{color, traversability}; +} + +std::vector makeLayerObservations() { + constexpr int kSize = 32; + + std::vector observations; + observations.reserve(kSize * kSize * kSize); + for (int x = 0; x < kSize; ++x) { + for (int y = 0; y < kSize; ++y) { + for (int z = 0; z < kSize; ++z) { + observations.push_back( + {wavemap::Index3D(x, y, z), makeLayerObservation(x, y, z)}); + } + } + } + return observations; +} + +std::shared_ptr makeMapAfterRayAndLayerUpdates( + const wavemap::HashedWaveletOctreeConfig& map_config, + const LayeredPruningConfig& pruning_config, + const std::vector& observations) { + auto map = std::make_shared( + map_config, ContinuousWaveletMap::ThresholdConfig{}, pruning_config); + + auto projection_model = std::make_shared( + wavemap::SphericalProjectorConfig( + wavemap::CircularProjectorConfig(-1.2f, 1.2f, 32), + wavemap::CircularProjectorConfig(-1.2f, 1.2f, 32))); + auto posed_range_image = + std::make_shared>(projection_model->getDimensions()); + auto beam_offset_image = + std::make_shared>( + projection_model->getDimensions()); + + wavemap::ContinuousBeamConfig measurement_config; + measurement_config.angle_sigma = 0.01f; + measurement_config.range_sigma = 0.02f; + measurement_config.scaling_free = 0.2f; + measurement_config.scaling_occupied = 0.4f; + auto measurement_model = std::make_shared( + measurement_config, projection_model, posed_range_image, + beam_offset_image); + + wavemap::ProjectiveIntegratorConfig integrator_config; + integrator_config.min_range = 0.05f; + integrator_config.max_range = 4.f; + integrator_config.max_update_resolution = 0.f; + integrator_config.termination_update_error = 0.05f; + + wavemap::HashedWaveletIntegratorT integrator( + integrator_config, projection_model, posed_range_image, + beam_offset_image, measurement_model, map); + + wavemap::Pointcloud<> pointcloud; + pointcloud.resize(3u); + pointcloud[0] = wavemap::Point3D(1.0f, 0.0f, 0.0f); + pointcloud[1] = wavemap::Point3D(1.5f, 0.2f, 0.0f); + pointcloud[2] = wavemap::Point3D(2.0f, -0.2f, 0.1f); + const wavemap::Transformation3D T_W_C; + const wavemap::PosedPointcloud<> posed_pointcloud(T_W_C, pointcloud); + + integrator.integrate(posed_pointcloud); + map->threshold(); + + setContinuousLayers(*map, observations); + map->threshold(); + return map; +} + +std::vector readVoxels( + const ContinuousWaveletMap& map, + const std::vector& observations) { + std::vector values; + values.reserve(observations.size()); + for (const ContinuousLayerObservation& observation : observations) { + values.push_back(map.getVoxelValue(observation.index)); + } + return values; +} + +RunResult runFullPipelinePruning( + const wavemap::HashedWaveletOctreeConfig& map_config, + const RunConfig& run_config, + const std::vector& observations) { + auto map = makeMapAfterRayAndLayerUpdates( + map_config, run_config.pruning_config, observations); + + RunResult result; + result.name = run_config.name; + result.nodes_before_prune = map->size(); + result.memory_before_prune = map->getMemoryUsage(); + const std::vector before_prune_values = + readVoxels(*map, observations); + + map->prune(); + + result.nodes_after_prune = map->size(); + result.memory_after_prune = map->getMemoryUsage(); + + for (size_t i = 0; i < observations.size(); ++i) { + const LayeredVoxel after = map->getVoxelValue(observations[i].index); + const LayeredVoxel& before = before_prune_values[i]; + + const float occupancy_delta = std::abs(after.occupancy - before.occupancy); + const float rgb_delta = sumAbsDiff(after.data.rgb, before.data.rgb); + const float traversability_delta = + std::abs(after.data.traversability - before.data.traversability); + + result.mean_occupancy_delta_after_prune += occupancy_delta; + result.mean_rgb_delta_after_prune += rgb_delta; + result.mean_traversability_delta_after_prune += traversability_delta; + result.max_occupancy_delta_after_prune = + std::max(result.max_occupancy_delta_after_prune, occupancy_delta); + result.max_rgb_delta_after_prune = + std::max(result.max_rgb_delta_after_prune, rgb_delta); + result.max_traversability_delta_after_prune = + std::max(result.max_traversability_delta_after_prune, + traversability_delta); + } + + const float sample_count = static_cast(observations.size()); + result.mean_occupancy_delta_after_prune /= sample_count; + result.mean_rgb_delta_after_prune /= sample_count; + result.mean_traversability_delta_after_prune /= sample_count; + return result; +} + +void printRunResult(const RunResult& result) { + std::cout << result.name << "\n"; + std::cout << " nodes before prune: " << result.nodes_before_prune << "\n"; + std::cout << " nodes after prune: " << result.nodes_after_prune << "\n"; + std::cout << " memory before prune bytes: " << result.memory_before_prune + << "\n"; + std::cout << " memory after prune bytes: " << result.memory_after_prune + << "\n"; + std::cout << " mean occupancy delta after prune: " + << result.mean_occupancy_delta_after_prune << "\n"; + std::cout << " max occupancy delta after prune: " + << result.max_occupancy_delta_after_prune << "\n"; + std::cout << " mean rgb delta after prune: " + << result.mean_rgb_delta_after_prune << "\n"; + std::cout << " max rgb delta after prune: " + << result.max_rgb_delta_after_prune << "\n"; + std::cout << " mean traversability delta after prune: " + << result.mean_traversability_delta_after_prune << "\n"; + std::cout << " max traversability delta after prune: " + << result.max_traversability_delta_after_prune << "\n"; +} +} // namespace + +int main() { + wavemap::HashedWaveletOctreeConfig map_config; + map_config.min_cell_width = 0.1f; + map_config.min_log_odds = -2.f; + map_config.max_log_odds = 4.f; + map_config.tree_height = 4; + + const std::vector observations = + makeLayerObservations(); + + const std::vector run_configs{ + {"Full pipeline pruning: occupancy-only", + makeLayeredPruningConfigWithScales(1.f, 0.f, 0.f, 1.f, 1.f, 1.f)}, + {"Full pipeline pruning: equal layers", + makeLayeredPruningConfigWithScales(1.f, 1.f, 1.f, 1.f, 0.1f, 0.1f)}, + {"Full pipeline pruning: occupancy-heavy", + makeLayeredPruningConfigWithScales(4.f, 1.f, 1.f, 1.f, 0.75f, 0.75f)}}; + + std::cout << "Layered full update + pruning experiment\n"; + std::cout << "Occupancy is integrated with the original ray-based Wavemap " + "pipeline. Continuous layers are then set from per-voxel " + "observations and the map is pruned with different weighted " + "policies.\n"; + std::cout << "Per-voxel layer observations: " << observations.size() + << "\n\n"; + + bool saw_compression = false; + bool saw_layer_preservation = false; + for (const RunConfig& run_config : run_configs) { + const RunResult result = + runFullPipelinePruning(map_config, run_config, observations); + printRunResult(result); + std::cout << "\n"; + + saw_compression |= result.nodes_after_prune < result.nodes_before_prune; + saw_layer_preservation |= result.mean_rgb_delta_after_prune < 1e-4f && + result.mean_traversability_delta_after_prune < + 1e-4f; + } + + if (!saw_compression || !saw_layer_preservation) { + return 1; + } + return 0; +} diff --git a/examples/cpp/digital_twin/map_api/continuous/layered_hashed_wavelet_integrator_experiment.cc b/examples/cpp/digital_twin/map_api/continuous/layered_hashed_wavelet_integrator_experiment.cc new file mode 100644 index 000000000..f260a0cdf --- /dev/null +++ b/examples/cpp/digital_twin/map_api/continuous/layered_hashed_wavelet_integrator_experiment.cc @@ -0,0 +1,117 @@ +#include +#include +#include +#include + +#include +#include +#include +#include + +#include "../../common/layered_voxel_config.h" + +namespace { +bool approximatelyEqual(float lhs, float rhs, float tolerance = 1e-5f) { + return std::abs(lhs - rhs) <= tolerance; +} + +bool approximatelyEqual(const Rgb& lhs, const Rgb& rhs, + float tolerance = 1e-5f) { + return sumAbsDiff(lhs, rhs) <= tolerance; +} +} // namespace + +int main() { + wavemap::HashedWaveletOctreeConfig map_config; + map_config.min_cell_width = 0.1f; + map_config.min_log_odds = -2.f; + map_config.max_log_odds = 4.f; + map_config.tree_height = 4; + + auto map = std::make_shared(map_config); + + const wavemap::Index3D seeded_index(10, 0, 0); + const LayeredVoxel seeded_voxel = + makeLayeredVoxel(0.f, rgb(0.25f, 0.5f, 0.75f), 0.8f); + map->setVoxelValue(seeded_index, seeded_voxel); + + auto projection_model = std::make_shared( + wavemap::SphericalProjectorConfig( + wavemap::CircularProjectorConfig(-1.2f, 1.2f, 32), + wavemap::CircularProjectorConfig(-1.2f, 1.2f, 32))); + auto posed_range_image = + std::make_shared>(projection_model->getDimensions()); + auto beam_offset_image = + std::make_shared>( + projection_model->getDimensions()); + + wavemap::ContinuousBeamConfig measurement_config; + measurement_config.angle_sigma = 0.01f; + measurement_config.range_sigma = 0.02f; + measurement_config.scaling_free = 0.2f; + measurement_config.scaling_occupied = 0.4f; + auto measurement_model = std::make_shared( + measurement_config, projection_model, posed_range_image, + beam_offset_image); + + wavemap::ProjectiveIntegratorConfig integrator_config; + integrator_config.min_range = 0.05f; + integrator_config.max_range = 2.f; + integrator_config.max_update_resolution = 0.f; + integrator_config.termination_update_error = 0.05f; + + wavemap::HashedWaveletIntegratorT integrator( + integrator_config, projection_model, posed_range_image, + beam_offset_image, measurement_model, map); + + wavemap::Pointcloud<> pointcloud; + pointcloud.resize(1u); + pointcloud[0] = wavemap::Point3D(1.f, 0.f, 0.f); + const wavemap::Transformation3D T_W_C; + const wavemap::PosedPointcloud<> posed_pointcloud(T_W_C, pointcloud); + + const LayeredVoxel before = map->getVoxelValue(seeded_index); + const size_t size_before = map->size(); + + integrator.integrate(posed_pointcloud); + map->threshold(); + + const LayeredVoxel after = map->getVoxelValue(seeded_index); + const size_t size_after = map->size(); + + float max_abs_occupancy = 0.f; + map->forEachVoxelLeaf([&max_abs_occupancy](const wavemap::OctreeIndex& /*index*/, + const LayeredVoxel& voxel) { + max_abs_occupancy = std::max(max_abs_occupancy, std::abs(voxel.occupancy)); + }); + + std::cout << "Layered hashed wavelet integrator experiment\n"; + printVoxel("Layered voxel before integration", before); + printVoxel("Same layered voxel after integration", after); + + const bool occupancy_changed_somewhere = 1e-5f < max_abs_occupancy; + const bool map_allocated_integration_nodes = size_before < size_after; + const bool rgb_preserved = approximatelyEqual(before.data.rgb, after.data.rgb); + const bool traversability_preserved = + approximatelyEqual(before.data.traversability, + after.data.traversability); + + std::cout << "Map size before integration: " << size_before << "\n"; + std::cout << "Map size after integration: " << size_after << "\n"; + std::cout << "Max abs occupancy after integration: " + << max_abs_occupancy << "\n"; + std::cout << "Occupancy changed through templated hashed wavelet integrator: " + << (occupancy_changed_somewhere ? "yes" : "no") << "\n"; + std::cout << "Integrator allocated/updated nodes: " + << (map_allocated_integration_nodes ? "yes" : "no") << "\n"; + std::cout << "RGB preserved by occupancy-only update: " + << (rgb_preserved ? "yes" : "no") << "\n"; + std::cout << "Traversability preserved by occupancy-only update: " + << (traversability_preserved ? "yes" : "no") << "\n"; + + if (!occupancy_changed_somewhere || !rgb_preserved || + !traversability_preserved) { + return 1; + } + return 0; +} diff --git a/examples/cpp/digital_twin/map_api/continuous/layered_occupancy_update_path_experiment.cc b/examples/cpp/digital_twin/map_api/continuous/layered_occupancy_update_path_experiment.cc new file mode 100644 index 000000000..51d044a9d --- /dev/null +++ b/examples/cpp/digital_twin/map_api/continuous/layered_occupancy_update_path_experiment.cc @@ -0,0 +1,94 @@ +#include +#include +#include +#include + +#include +#include +#include + +#include "../../common/layered_voxel_config.h" + +namespace { +bool approximatelyEqual(float lhs, float rhs, float tolerance = 1e-5f) { + return std::abs(lhs - rhs) <= tolerance; +} + +bool approximatelyEqual(const Rgb& lhs, const Rgb& rhs, + float tolerance = 1e-5f) { + return sumAbsDiff(lhs, rhs) <= tolerance; +} +} // namespace + +int main() { + wavemap::HashedWaveletOctreeConfig map_config; + map_config.min_cell_width = 0.1f; + map_config.min_log_odds = -2.f; + map_config.max_log_odds = 4.f; + map_config.tree_height = 4; + + auto map = std::make_shared(map_config); + + const wavemap::Index3D endpoint_index(10, 0, 0); + const LayeredVoxel original_endpoint = + makeLayeredVoxel(0.f, rgb(0.25f, 0.5f, 0.75f), 0.8f); + map->setVoxelValue(endpoint_index, original_endpoint); + + wavemap::Pointcloud<> pointcloud; + pointcloud.resize(1u); + pointcloud[0] = wavemap::Point3D(1.f, 0.f, 0.f); + const wavemap::Transformation3D T_W_C; + const wavemap::PosedPointcloud<> posed_pointcloud(T_W_C, pointcloud); + + wavemap::RayTracingIntegratorConfig integrator_config; + integrator_config.min_range = 0.05f; + integrator_config.max_range = 2.f; + + std::shared_ptr map_base = map; + wavemap::RayTracingIntegrator integrator(integrator_config, map_base); + + const LayeredVoxel before = map->getVoxelValue(endpoint_index); + const size_t size_before = map->size(); + + integrator.integrate(posed_pointcloud); + map->threshold(); + + const LayeredVoxel after = map->getVoxelValue(endpoint_index); + const size_t size_after = map->size(); + + float max_abs_occupancy = 0.f; + map->forEachVoxelLeaf([&max_abs_occupancy](const wavemap::OctreeIndex& /*index*/, + const LayeredVoxel& voxel) { + max_abs_occupancy = std::max(max_abs_occupancy, std::abs(voxel.occupancy)); + }); + + std::cout << "Occupancy-first layered map integration experiment\n"; + printVoxel("Layered voxel before integration", before); + printVoxel("Same layered voxel after integration", after); + + const bool occupancy_changed_somewhere = 1e-5f < max_abs_occupancy; + const bool map_allocated_integration_nodes = size_before < size_after; + const bool rgb_preserved = approximatelyEqual(before.data.rgb, after.data.rgb); + const bool traversability_preserved = + approximatelyEqual(before.data.traversability, + after.data.traversability); + + std::cout << "Map size before integration: " << size_before << "\n"; + std::cout << "Map size after integration: " << size_after << "\n"; + std::cout << "Max abs occupancy after integration: " + << max_abs_occupancy << "\n"; + std::cout << "Occupancy changed through real integrator: " + << (occupancy_changed_somewhere ? "yes" : "no") << "\n"; + std::cout << "Integrator allocated/updated nodes: " + << (map_allocated_integration_nodes ? "yes" : "no") << "\n"; + std::cout << "RGB preserved by occupancy-only update: " + << (rgb_preserved ? "yes" : "no") << "\n"; + std::cout << "Traversability preserved by occupancy-only update: " + << (traversability_preserved ? "yes" : "no") << "\n"; + + if (!occupancy_changed_somewhere || !rgb_preserved || + !traversability_preserved) { + return 1; + } + return 0; +} diff --git a/examples/cpp/digital_twin/map_api/continuous/layered_per_voxel_update_experiment.cc b/examples/cpp/digital_twin/map_api/continuous/layered_per_voxel_update_experiment.cc new file mode 100644 index 000000000..37489967b --- /dev/null +++ b/examples/cpp/digital_twin/map_api/continuous/layered_per_voxel_update_experiment.cc @@ -0,0 +1,143 @@ +#include +#include +#include +#include + +#include +#include +#include +#include + +#include "../../common/layered_voxel_config.h" +#include "../../common/layered_voxel_updater.h" + +namespace { +bool approximatelyEqual(float lhs, float rhs, float tolerance = 1e-4f) { + return std::abs(lhs - rhs) <= tolerance; +} + +bool approximatelyEqual(const Rgb& lhs, const Rgb& rhs, + float tolerance = 1e-4f) { + return sumAbsDiff(lhs, rhs) <= tolerance; +} + +void printObservationResult(const ContinuousLayerObservation& observation, + const LayeredVoxel& before, + const LayeredVoxel& after) { + std::cout << "Voxel " << observation.index.transpose() << "\n"; + std::cout << " occupancy before layer update: " << before.occupancy << "\n"; + std::cout << " occupancy after layer update: " << after.occupancy << "\n"; + std::cout << " requested layer data: trav=" + << observation.data.traversability << " rgb=(" + << observation.data.rgb.r << ", " << observation.data.rgb.g + << ", " << observation.data.rgb.b << ")\n"; + printVoxel(" stored voxel", after); +} +} // namespace + +int main() { + wavemap::HashedWaveletOctreeConfig map_config; + map_config.min_cell_width = 0.1f; + map_config.min_log_odds = -2.f; + map_config.max_log_odds = 4.f; + map_config.tree_height = 4; + + auto map = std::make_shared(map_config); + + auto projection_model = std::make_shared( + wavemap::SphericalProjectorConfig( + wavemap::CircularProjectorConfig(-1.2f, 1.2f, 32), + wavemap::CircularProjectorConfig(-1.2f, 1.2f, 32))); + auto posed_range_image = + std::make_shared>(projection_model->getDimensions()); + auto beam_offset_image = + std::make_shared>( + projection_model->getDimensions()); + + wavemap::ContinuousBeamConfig measurement_config; + measurement_config.angle_sigma = 0.01f; + measurement_config.range_sigma = 0.02f; + measurement_config.scaling_free = 0.2f; + measurement_config.scaling_occupied = 0.4f; + auto measurement_model = std::make_shared( + measurement_config, projection_model, posed_range_image, + beam_offset_image); + + wavemap::ProjectiveIntegratorConfig integrator_config; + integrator_config.min_range = 0.05f; + integrator_config.max_range = 2.f; + integrator_config.max_update_resolution = 0.f; + integrator_config.termination_update_error = 0.05f; + + wavemap::HashedWaveletIntegratorT integrator( + integrator_config, projection_model, posed_range_image, + beam_offset_image, measurement_model, map); + + wavemap::Pointcloud<> pointcloud; + pointcloud.resize(1u); + pointcloud[0] = wavemap::Point3D(1.f, 0.f, 0.f); + const wavemap::Transformation3D T_W_C; + const wavemap::PosedPointcloud<> posed_pointcloud(T_W_C, pointcloud); + + integrator.integrate(posed_pointcloud); + map->threshold(); + + std::vector updated_indices; + map->forEachVoxelLeaf( + [&updated_indices](const wavemap::OctreeIndex& node_index, + const LayeredVoxel& voxel) { + if (updated_indices.size() < 3u && 1e-5f < std::abs(voxel.occupancy)) { + updated_indices.push_back( + wavemap::convert::nodeIndexToMinCornerIndex(node_index)); + } + }); + + if (updated_indices.size() < 3u) { + std::cout << "Not enough occupancy-updated voxels found.\n"; + return 1; + } + + const std::vector observations{ + {updated_indices[0], ContinuousLayers{rgb(0.9f, 0.1f, 0.1f), 0.8f}}, + {updated_indices[1], ContinuousLayers{rgb(0.1f, 0.8f, 0.2f), 0.4f}}, + {updated_indices[2], ContinuousLayers{rgb(0.2f, 0.2f, 0.9f), 0.6f}}}; + + std::vector before_layer_update; + before_layer_update.reserve(observations.size()); + for (const ContinuousLayerObservation& observation : observations) { + before_layer_update.push_back(map->getVoxelValue(observation.index)); + } + + setContinuousLayers(*map, observations); + map->threshold(); + + bool all_occupancy_preserved = true; + bool all_layers_updated = true; + + std::cout << "Layered per-voxel update experiment\n"; + std::cout << "Occupancy was integrated through the original ray-based " + "Wavemap pipeline. Continuous layers are then set from " + "per-voxel observations using a replacement policy.\n"; + + for (size_t i = 0; i < observations.size(); ++i) { + const LayeredVoxel after = map->getVoxelValue(observations[i].index); + printObservationResult(observations[i], before_layer_update[i], after); + + all_occupancy_preserved &= approximatelyEqual( + before_layer_update[i].occupancy, after.occupancy); + all_layers_updated &= approximatelyEqual(observations[i].data.rgb, + after.data.rgb, 1e-3f); + all_layers_updated &= approximatelyEqual( + observations[i].data.traversability, after.data.traversability, 1e-3f); + } + + std::cout << "Occupancy preserved by per-voxel layer update: " + << (all_occupancy_preserved ? "yes" : "no") << "\n"; + std::cout << "Continuous layers updated from per-voxel observations: " + << (all_layers_updated ? "yes" : "no") << "\n"; + + if (!all_occupancy_preserved || !all_layers_updated) { + return 1; + } + return 0; +} diff --git a/examples/cpp/digital_twin/map_api/continuous/layered_pruning_config.md b/examples/cpp/digital_twin/map_api/continuous/layered_pruning_config.md new file mode 100644 index 000000000..e2f1ffe4b --- /dev/null +++ b/examples/cpp/digital_twin/map_api/continuous/layered_pruning_config.md @@ -0,0 +1,60 @@ +# Layered pruning config + +This example uses `LayeredVoxel`, which stores occupancy plus continuous layers: + +```cpp +struct ContinuousLayers { + Rgb rgb; + float traversability; +}; +``` + +The map still stores wavelet coefficients. Pruning decides whether a node's detail coefficients are relevant enough to keep. + +The layered pruning score is: + +```cpp +score = sum(weight * abs(wavelet_detail) / scale) +``` + +A node is kept when: + +```cpp +score > combined_threshold +``` + +## Parameters + +`scale` is the amount of detail that counts as one unit of relevance for a field. Larger scale means that field is more tolerant and will allow more pruning. + +`weight` is the relative importance of the field. Use `0` to ignore a field during pruning. + +`combined_threshold` is the final pruning decision threshold. The default is `1`. + +## Examples + +Occupancy-only pruning: + +```cpp +auto pruning = makeOccupancyOnlyPruningConfig(); +``` + +Equal weighting for occupancy, RGB, and traversability: + +```cpp +auto pruning = makeEqualLayerPruningConfig(); +``` + +Custom field scales and weights: + +```cpp +auto pruning = makeLayeredPruningConfigWithScales( + /*occupancy_weight=*/1.f, + /*rgb_weight=*/1.f, + /*traversability_weight=*/2.f, + /*occupancy_scale=*/1.f, + /*rgb_scale=*/0.1f, + /*traversability_scale=*/0.1f); +``` + +The trade-off is expected: ignoring extra layers usually gives stronger compression, but can remove RGB/traversability details. Including extra layers preserves them, but may keep more nodes. diff --git a/examples/cpp/digital_twin/map_api/continuous/layered_pruning_config_example.cc b/examples/cpp/digital_twin/map_api/continuous/layered_pruning_config_example.cc new file mode 100644 index 000000000..1d328d5b4 --- /dev/null +++ b/examples/cpp/digital_twin/map_api/continuous/layered_pruning_config_example.cc @@ -0,0 +1,88 @@ +#include +#include +#include +#include + +#include "../../common/layered_voxel_config.h" + +namespace { +struct ExampleRun { + std::string name; + LayeredPruningConfig pruning_config; +}; + +void fillMapWithLayerBoundary(ContinuousWaveletMap& map) { + for (int x = 0; x < 32; ++x) { + for (int y = 0; y < 32; ++y) { + for (int z = 0; z < 32; ++z) { + const wavemap::FloatingPoint occupancy = 0.5f; + + Rgb color = rgb(0.2f, 0.2f, 0.2f); + if (16 <= x) { + color = rgb(0.9f, 0.1f, 0.1f); + } + if (16 <= y) { + color = rgb(0.1f, 0.1f, 0.9f); + } + + const float traversability = (8 <= z && z < 24) ? 0.85f : 0.25f; + map.setVoxelValue(wavemap::Index3D{x, y, z}, + makeLayeredVoxel(occupancy, color, traversability)); + } + } + } +} + +void runExample(const wavemap::HashedWaveletOctreeConfig& map_config, + const ExampleRun& run) { + ContinuousWaveletMap map(map_config, ContinuousWaveletMap::ThresholdConfig{}, + run.pruning_config); + fillMapWithLayerBoundary(map); + + const size_t nodes_before = map.size(); + const size_t memory_before = map.getMemoryUsage(); + + map.threshold(); + map.prune(); + + std::cout << run.name << "\n"; + std::cout << " nodes before prune: " << nodes_before << "\n"; + std::cout << " nodes after prune: " << map.size() << "\n"; + std::cout << " memory before prune bytes: " << memory_before << "\n"; + std::cout << " memory after prune bytes: " << map.getMemoryUsage() << "\n"; + printVoxel(" sample voxel", map.getVoxelValue(wavemap::Index3D{20, 20, 12})); + std::cout << "\n"; +} +} // namespace + +int main() { + wavemap::HashedWaveletOctreeConfig map_config; + map_config.min_cell_width = 0.1f; + map_config.min_log_odds = -100.f; + map_config.max_log_odds = 100.f; + map_config.tree_height = 4; + + // Pruning config quick guide: + // scale: detail magnitude that counts as one unit of relevance. + // weight: relative importance of this field in the combined score. + // combined_threshold: prune if the total score is not above this value. + // Formula: + // score = sum(weight * abs(wavelet_detail) / scale) + // A field with weight 0 is ignored by pruning. + const std::vector runs{ + {"Occupancy-only pruning", + makeOccupancyOnlyPruningConfig(/*scale=*/1.f)}, + {"Equal occupancy/RGB/traversability pruning", + makeEqualLayerPruningConfig(/*scale=*/0.1f)}, + {"Custom pruning: occupancy-heavy, layers still count", + makeLayeredPruningConfigWithScales( + /*occupancy_weight=*/4.f, /*rgb_weight=*/1.f, + /*traversability_weight=*/1.f, /*occupancy_scale=*/1.f, + /*rgb_scale=*/1.f, /*traversability_scale=*/1.f)}}; + + for (const ExampleRun& run : runs) { + runExample(map_config, run); + } + + return 0; +} diff --git a/examples/cpp/digital_twin/map_api/continuous/mixed_voxel_map_experiment.cc b/examples/cpp/digital_twin/map_api/continuous/mixed_voxel_map_experiment.cc new file mode 100644 index 000000000..bf0cdedc9 --- /dev/null +++ b/examples/cpp/digital_twin/map_api/continuous/mixed_voxel_map_experiment.cc @@ -0,0 +1,97 @@ +#include +#include +#include + +#include "wavemap/core/indexing/ndtree_index.h" +#include "wavemap/core/map/cell_types/voxel_data.h" +#include "wavemap/core/map/hashed_wavelet_octree.h" + +struct MixedData { + float traversability = 0.f; + float r = 0.f; + float g = 0.f; + float b = 0.f; + int label = 0; + + bool operator==(const MixedData& other) const { + return traversability == other.traversability && r == other.r && + g == other.g && b == other.b && label == other.label; + } +}; + +struct MixedDataPolicy { + static MixedData add(const MixedData& lhs, const MixedData& rhs) { + return {lhs.traversability + rhs.traversability, lhs.r + rhs.r, + lhs.g + rhs.g, lhs.b + rhs.b, lhs.label + rhs.label}; + } + + static MixedData subtract(const MixedData& lhs, const MixedData& rhs) { + return {lhs.traversability - rhs.traversability, lhs.r - rhs.r, + lhs.g - rhs.g, lhs.b - rhs.b, lhs.label - rhs.label}; + } + + static MixedData scale(const MixedData& data, wavemap::FloatingPoint factor) { + return {factor * data.traversability, factor * data.r, factor * data.g, + factor * data.b, static_cast(factor * data.label)}; + } +}; + +using MixedVoxel = wavemap::VoxelData; +using MixedMap = wavemap::HashedWaveletOctreeT; + +void printVoxel(const std::string& name, const MixedVoxel& voxel) { + std::cout << name << ": occ=" << voxel.occupancy + << " trav=" << voxel.data.traversability << " rgb=(" + << voxel.data.r << ", " << voxel.data.g << ", " << voxel.data.b + << ") label=" << voxel.data.label << "\n"; +} + +int main() { + wavemap::HashedWaveletOctreeConfig config; + config.min_cell_width = 0.1f; + config.min_log_odds = -100.f; + config.max_log_odds = 100.f; + config.tree_height = 3; + + MixedMap map(config); + + const wavemap::Index3D a(1, 2, 3); + const wavemap::Index3D b(2, 2, 3); + const wavemap::Index3D c(3, 2, 3); + + map.setVoxelValue(a, MixedVoxel(10.f, {0.1f, 1.f, 0.f, 0.f, 1})); + map.setVoxelValue(b, MixedVoxel(20.f, {0.5f, 0.f, 1.f, 0.f, 5})); + map.setVoxelValue(c, MixedVoxel(30.f, {0.9f, 0.f, 0.f, 1.f, 9})); + + std::cout << "Before threshold/prune\n"; + printVoxel("a", map.getVoxelValue(a)); + printVoxel("b", map.getVoxelValue(b)); + printVoxel("c", map.getVoxelValue(c)); + std::cout << "block count: " << map.getHashMap().size() << "\n"; + std::cout << "node count: " << map.size() << "\n"; + + size_t labeled_leaves = 0u; + map.forEachVoxelLeaf( + [&labeled_leaves](const wavemap::OctreeIndex& index, + const MixedVoxel& voxel) { + if (voxel.data.label != 0) { + ++labeled_leaves; + std::cout << "leaf h=" << index.height << " pos=" + << index.position.transpose(); + printVoxel(" value", voxel); + } + }); + std::cout << "labeled leaves: " << labeled_leaves << "\n"; + + map.threshold(); + map.prune(); + + std::cout << "After threshold/prune\n"; + printVoxel("a", map.getVoxelValue(a)); + printVoxel("b", map.getVoxelValue(b)); + printVoxel("c", map.getVoxelValue(c)); + std::cout << "block count: " << map.getHashMap().size() << "\n"; + std::cout << "node count: " << map.size() << "\n"; + + return 0; +} diff --git a/examples/cpp/digital_twin/map_api/discrete/discrete_layer_compression_experiment.cc b/examples/cpp/digital_twin/map_api/discrete/discrete_layer_compression_experiment.cc new file mode 100644 index 000000000..4b2694ff7 --- /dev/null +++ b/examples/cpp/digital_twin/map_api/discrete/discrete_layer_compression_experiment.cc @@ -0,0 +1,138 @@ +#include +#include +#include + +#include + +using wavemap::layered::DiscreteCompressionConfig; +using wavemap::layered::DiscreteLayer; +using wavemap::layered::RawDiscreteLayer; + +namespace { +template +void fillBlock(LayerT& layer, const wavemap::Index3D& origin, int side_length, + const ValueT& value) { + for (int dx = 0; dx < side_length; ++dx) { + for (int dy = 0; dy < side_length; ++dy) { + for (int dz = 0; dz < side_length; ++dz) { + layer.setValue(origin + wavemap::Index3D(dx, dy, dz), value); + } + } + } +} + +template +bool sameValue(const std::optional& read, const ValueT& expected) { + return read && *read == expected; +} + +template +size_t countPreservedValues(const RawDiscreteLayer& raw, + const DiscreteLayer& compressed) { + size_t preserved = 0u; + for (const auto& [key, expected] : raw.values()) { + if (sameValue(compressed.getValue(indexFromKey(key)), expected)) { + ++preserved; + } + } + return preserved; +} + +template +void printCompressionStats(const std::string& name, + const RawDiscreteLayer& raw, + const DiscreteLayer& compressed) { + std::cout << name << "\n"; + std::cout << " raw entries: " << raw.size() << "\n"; + std::cout << " compressed parents: " << compressed.parentCount() << "\n"; + std::cout << " exceptions: " << compressed.exceptionCount() << "\n"; + std::cout << " preserved values: " + << countPreservedValues(raw, compressed) << "/" << raw.size() + << "\n"; +} +} // namespace + +int main() { + RawDiscreteLayer semantic_layer; + RawDiscreteLayer changed_layer; + + const wavemap::Index3D uniform_block(0, 0, 0); + fillBlock(semantic_layer, uniform_block, 2, 1); + fillBlock(changed_layer, uniform_block, 2, false); + + const wavemap::Index3D one_exception_block(4, 0, 0); + fillBlock(semantic_layer, one_exception_block, 2, 2); + fillBlock(changed_layer, one_exception_block, 2, false); + semantic_layer.setValue(one_exception_block + wavemap::Index3D(1, 1, 1), 5); + changed_layer.setValue(one_exception_block + wavemap::Index3D(1, 1, 1), true); + + const wavemap::Index3D mixed_block(8, 0, 0); + fillBlock(semantic_layer, mixed_block, 2, 3); + fillBlock(changed_layer, mixed_block, 2, false); + semantic_layer.setValue(mixed_block + wavemap::Index3D(0, 1, 0), 4); + semantic_layer.setValue(mixed_block + wavemap::Index3D(1, 0, 1), 4); + semantic_layer.setValue(mixed_block + wavemap::Index3D(1, 1, 0), 6); + changed_layer.setValue(mixed_block + wavemap::Index3D(0, 1, 0), true); + changed_layer.setValue(mixed_block + wavemap::Index3D(1, 0, 1), true); + changed_layer.setValue(mixed_block + wavemap::Index3D(1, 1, 0), true); + + const DiscreteCompressionConfig one_octree_level{1}; + const auto compressed_semantic = + DiscreteLayer::fromRaw(semantic_layer, one_octree_level); + const auto compressed_changed = + DiscreteLayer::fromRaw(changed_layer, one_octree_level); + + const DiscreteCompressionConfig two_octree_levels{2}; + DiscreteLayer directly_updated_semantic(two_octree_levels); + fillBlock(directly_updated_semantic, wavemap::Index3D(0, 8, 0), + two_octree_levels.blockSideLength(), 7); + directly_updated_semantic.setValue(wavemap::Index3D(3, 11, 3), 9); + + std::cout << "Discrete layer compression experiment\n"; + std::cout << "Compression stores one dominant value per configured parent " + "region and explicit child exceptions. Reconstruction is " + "lossless for stored voxels.\n"; + printCompressionStats("semantic layer", semantic_layer, + compressed_semantic); + printCompressionStats("binary changed layer", changed_layer, + compressed_changed); + std::cout << "directly updated semantic layer\n"; + std::cout << " block height: " + << directly_updated_semantic.config().block_height << "\n"; + std::cout << " block side length: " + << directly_updated_semantic.config().blockSideLength() << "\n"; + std::cout << " compressed parents: " + << directly_updated_semantic.parentCount() << "\n"; + std::cout << " exceptions: " + << directly_updated_semantic.exceptionCount() << "\n"; + std::cout << " updated exception preserved: " + << (sameValue(directly_updated_semantic.getValue( + wavemap::Index3D(3, 11, 3)), + 9) + ? "yes" + : "no") + << "\n"; + std::cout << " sparse unknown remains empty: " + << (!directly_updated_semantic.getValue(wavemap::Index3D(8, 8, 8)) + ? "yes" + : "no") + << "\n"; + + const bool semantic_lossless = + countPreservedValues(semantic_layer, compressed_semantic) == + semantic_layer.size(); + const bool changed_lossless = + countPreservedValues(changed_layer, compressed_changed) == + changed_layer.size(); + + const bool direct_update_ok = + sameValue(directly_updated_semantic.getValue(wavemap::Index3D(3, 11, 3)), + 9) && + !directly_updated_semantic.getValue(wavemap::Index3D(8, 8, 8)); + + if (!semantic_lossless || !changed_lossless || !direct_update_ok) { + std::cout << "Compressed discrete layer did not reconstruct all values.\n"; + return 1; + } + return 0; +} diff --git a/examples/cpp/digital_twin/map_api/discrete/discrete_layer_update_behavior_experiment.cc b/examples/cpp/digital_twin/map_api/discrete/discrete_layer_update_behavior_experiment.cc new file mode 100644 index 000000000..ee0643a7f --- /dev/null +++ b/examples/cpp/digital_twin/map_api/discrete/discrete_layer_update_behavior_experiment.cc @@ -0,0 +1,112 @@ +#include +#include +#include + +#include + +using wavemap::layered::DiscreteCompressionConfig; +using wavemap::layered::DiscreteLayer; +using wavemap::layered::RawDiscreteLayer; + +namespace { +template +bool sameValue(const std::optional& value, const ValueT& expected) { + return value && *value == expected; +} + +void fillBlock(DiscreteLayer& layer, const wavemap::Index3D& origin, + int side_length, int value) { + for (int dx = 0; dx < side_length; ++dx) { + for (int dy = 0; dy < side_length; ++dy) { + for (int dz = 0; dz < side_length; ++dz) { + layer.setValue(origin + wavemap::Index3D(dx, dy, dz), value); + } + } + } +} + +bool expect(const std::string& name, bool condition) { + std::cout << " " << name << ": " << (condition ? "pass" : "FAIL") + << "\n"; + return condition; +} +} // namespace + +int main() { + bool ok = true; + + std::cout << "Discrete layer update behavior experiment\n"; + + { + DiscreteLayer layer(DiscreteCompressionConfig{1}); + fillBlock(layer, wavemap::Index3D(0, 0, 0), 2, 1); + ok &= expect("uniform block has one parent", layer.parentCount() == 1u); + ok &= expect("uniform block has zero exceptions", + layer.exceptionCount() == 0u); + ok &= expect("uniform block observed all children", + layer.observedValueCount() == 8u); + ok &= expect("uniform block reconstructs dominant value", + sameValue(layer.getValue(wavemap::Index3D(1, 1, 1)), 1)); + } + + { + DiscreteLayer layer(DiscreteCompressionConfig{1}); + fillBlock(layer, wavemap::Index3D(0, 0, 0), 2, 1); + layer.setValue(wavemap::Index3D(1, 1, 1), 2); + ok &= expect("one different child becomes one exception", + layer.exceptionCount() == 1u); + ok &= expect("exception value reconstructs exactly", + sameValue(layer.getValue(wavemap::Index3D(1, 1, 1)), 2)); + + layer.setValue(wavemap::Index3D(1, 1, 1), 1); + ok &= expect("setting exception back removes exception", + layer.exceptionCount() == 0u); + ok &= expect("restored child reconstructs dominant value", + sameValue(layer.getValue(wavemap::Index3D(1, 1, 1)), 1)); + } + + { + DiscreteLayer layer(DiscreteCompressionConfig{1}); + fillBlock(layer, wavemap::Index3D(0, 0, 0), 2, 1); + layer.setValue(wavemap::Index3D(0, 0, 0), 2); + layer.setValue(wavemap::Index3D(1, 0, 0), 2); + layer.setValue(wavemap::Index3D(0, 1, 0), 2); + layer.setValue(wavemap::Index3D(1, 1, 0), 2); + layer.setValue(wavemap::Index3D(0, 0, 1), 2); + ok &= expect("majority update changes dominant value", + sameValue(layer.dominantValue(wavemap::Index3D(0, 0, 0)), 2)); + ok &= expect("old minority values become exceptions", + layer.exceptionCount() == 3u); + ok &= expect("new majority child reconstructs exactly", + sameValue(layer.getValue(wavemap::Index3D(0, 0, 0)), 2)); + ok &= expect("old minority child reconstructs exactly", + sameValue(layer.getValue(wavemap::Index3D(1, 1, 1)), 1)); + } + + { + DiscreteLayer layer(DiscreteCompressionConfig{1}); + layer.setValue(wavemap::Index3D(0, 0, 0), 7); + ok &= expect("unknown child in existing parent remains unknown", + !layer.getValue(wavemap::Index3D(1, 1, 1))); + ok &= expect("observed child still reconstructs", + sameValue(layer.getValue(wavemap::Index3D(0, 0, 0)), 7)); + } + + { + DiscreteLayer layer(DiscreteCompressionConfig{2}); + fillBlock(layer, wavemap::Index3D(0, 0, 0), 4, 3); + layer.setValue(wavemap::Index3D(3, 3, 3), 4); + ok &= expect("block_height 2 uses one 4x4x4 parent", + layer.parentCount() == 1u); + ok &= expect("block_height 2 stores one exception", + layer.exceptionCount() == 1u); + ok &= expect("block_height 2 reconstructs exception", + sameValue(layer.getValue(wavemap::Index3D(3, 3, 3)), 4)); + } + + if (!ok) { + std::cout << "Discrete layer update behavior failed.\n"; + return 1; + } + return 0; +} diff --git a/examples/cpp/digital_twin/map_api/discrete/float_cast_discrete_experiment.cc b/examples/cpp/digital_twin/map_api/discrete/float_cast_discrete_experiment.cc new file mode 100644 index 000000000..2a8ed5ec1 --- /dev/null +++ b/examples/cpp/digital_twin/map_api/discrete/float_cast_discrete_experiment.cc @@ -0,0 +1,147 @@ +#include +#include +#include +#include +#include + +#include "wavemap/core/indexing/ndtree_index.h" +#include "wavemap/core/map/cell_types/voxel_data.h" +#include "wavemap/core/map/hashed_wavelet_octree.h" + +struct CastDiscreteData { + float label = 0.f; + float dynamic = 0.f; + float code = 0.f; + + bool operator==(const CastDiscreteData& other) const { + return label == other.label && dynamic == other.dynamic && + code == other.code; + } +}; + +struct CastDiscreteDataPolicy { + static CastDiscreteData add(const CastDiscreteData& lhs, + const CastDiscreteData& rhs) { + return {lhs.label + rhs.label, lhs.dynamic + rhs.dynamic, + lhs.code + rhs.code}; + } + + static CastDiscreteData subtract(const CastDiscreteData& lhs, + const CastDiscreteData& rhs) { + return {lhs.label - rhs.label, lhs.dynamic - rhs.dynamic, + lhs.code - rhs.code}; + } + + static CastDiscreteData scale(const CastDiscreteData& data, + wavemap::FloatingPoint factor) { + return {factor * data.label, factor * data.dynamic, factor * data.code}; + } +}; + +struct RawDiscreteData { + int label = 0; + bool dynamic = false; + char code = '\0'; + + bool operator==(const RawDiscreteData& other) const { + return label == other.label && dynamic == other.dynamic && + code == other.code; + } +}; + +using CastVoxel = wavemap::VoxelData; +using CastMap = wavemap::HashedWaveletOctreeT; + +CastVoxel toVoxel(wavemap::FloatingPoint occupancy, + const RawDiscreteData& data) { + return CastVoxel( + occupancy, + {static_cast(data.label), data.dynamic ? 1.f : 0.f, + static_cast(static_cast(data.code))}); +} + +RawDiscreteData fromVoxelRounded(const CastVoxel& voxel) { + return {static_cast(std::lround(voxel.data.label)), + std::lround(voxel.data.dynamic) != 0, + static_cast(std::lround(voxel.data.code))}; +} + +void printRaw(const std::string& name, const RawDiscreteData& data) { + std::cout << name << ": label=" << data.label + << " dynamic=" << std::boolalpha << data.dynamic + << " code=" << data.code << "\n"; +} + +void printVoxel(const std::string& name, const CastVoxel& voxel) { + std::cout << name << ": occ=" << voxel.occupancy + << " label_f=" << voxel.data.label + << " dynamic_f=" << voxel.data.dynamic + << " code_f=" << voxel.data.code << " rounded=(" + << fromVoxelRounded(voxel).label << ", " << std::boolalpha + << fromVoxelRounded(voxel).dynamic << ", " + << fromVoxelRounded(voxel).code << ")\n"; +} + +int main() { + wavemap::HashedWaveletOctreeConfig config; + config.min_cell_width = 0.1f; + config.min_log_odds = -100.f; + config.max_log_odds = 100.f; + config.tree_height = 3; + + CastMap map(config); + + const std::vector> samples = { + {wavemap::Index3D(1, 2, 3), 10.f, {1, false, 'A'}}, + {wavemap::Index3D(2, 2, 3), 20.f, {5, true, 'B'}}, + {wavemap::Index3D(3, 2, 3), 30.f, {9, false, 'C'}}, + }; + + for (const auto& [index, occupancy, data] : samples) { + map.setVoxelValue(index, toVoxel(occupancy, data)); + } + + std::cout << "Expected raw values\n"; + for (const auto& [index, occupancy, data] : samples) { + (void)index; + (void)occupancy; + printRaw("expected", data); + } + + size_t preserved_after_rounding = 0u; + std::cout << "Read through float cast + Haar\n"; + for (const auto& [index, occupancy, expected] : samples) { + (void)occupancy; + const CastVoxel read_voxel = map.getVoxelValue(index); + const RawDiscreteData rounded = fromVoxelRounded(read_voxel); + printVoxel("read", read_voxel); + if (rounded == expected) { + ++preserved_after_rounding; + } + } + + size_t interesting_leaves = 0u; + map.forEachVoxelLeaf( + [&interesting_leaves](const wavemap::OctreeIndex& index, + const CastVoxel& voxel) { + const bool interesting = std::abs(voxel.occupancy) > 1e-5f || + std::abs(voxel.data.label) > 1e-5f || + std::abs(voxel.data.dynamic) > 1e-5f || + std::abs(voxel.data.code) > 1e-5f; + if (interesting) { + ++interesting_leaves; + std::cout << "leaf h=" << index.height << " pos=" + << index.position.transpose(); + printVoxel(" value", voxel); + } + }); + + std::cout << "preserved after rounding: " << preserved_after_rounding << "/" + << samples.size() << "\n"; + std::cout << "interesting leaves: " << interesting_leaves << "\n"; + std::cout << "block count: " << map.getHashMap().size() << "\n"; + std::cout << "node count: " << map.size() << "\n"; + + return preserved_after_rounding == samples.size() ? 0 : 1; +} diff --git a/examples/cpp/digital_twin/map_api/discrete/layered_label_storage_experiment.cc b/examples/cpp/digital_twin/map_api/discrete/layered_label_storage_experiment.cc new file mode 100644 index 000000000..036967f07 --- /dev/null +++ b/examples/cpp/digital_twin/map_api/discrete/layered_label_storage_experiment.cc @@ -0,0 +1,76 @@ +#include +#include +#include +#include + +#include "../../common/layered_voxel_config.h" + +struct IndexKey { + int x = 0; + int y = 0; + int z = 0; + + explicit IndexKey(const wavemap::Index3D& index) + : x(index.x()), y(index.y()), z(index.z()) {} + + bool operator<(const IndexKey& other) const { + return std::tie(x, y, z) < std::tie(other.x, other.y, other.z); + } +}; + +void printVoxelWithLabel(const std::string& name, const LayeredVoxel& voxel, + int label) { + std::cout << name << ": occ=" << voxel.occupancy + << " trav=" << voxel.data.traversability << " rgb=(" + << voxel.data.rgb.r << ", " << voxel.data.rgb.g << ", " + << voxel.data.rgb.b << ") label=" << label << "\n"; +} + +int main() { + wavemap::HashedWaveletOctreeConfig config; + config.min_cell_width = 0.1f; + config.min_log_odds = -100.f; + config.max_log_odds = 100.f; + config.tree_height = 3; + + ContinuousWaveletMap map(config); + std::map labels; + + const wavemap::Index3D a(1, 2, 3); + const wavemap::Index3D b(2, 2, 3); + const wavemap::Index3D c(3, 2, 3); + + map.setVoxelValue(a, + makeLayeredVoxel(10.f, rgb(1.f, 0.f, 0.f), + 0.1f)); + map.setVoxelValue(b, + makeLayeredVoxel(20.f, rgb(0.f, 1.f, 0.f), + 0.5f)); + map.setVoxelValue(c, + makeLayeredVoxel(30.f, rgb(0.f, 0.f, 1.f), + 0.9f)); + labels[IndexKey(a)] = 1; + labels[IndexKey(b)] = 5; + labels[IndexKey(c)] = 9; + + std::cout << "Before threshold/prune\n"; + printVoxelWithLabel("a", map.getVoxelValue(a), labels[IndexKey(a)]); + printVoxelWithLabel("b", map.getVoxelValue(b), labels[IndexKey(b)]); + printVoxelWithLabel("c", map.getVoxelValue(c), labels[IndexKey(c)]); + std::cout << "label entries: " << labels.size() << "\n"; + std::cout << "block count: " << map.getHashMap().size() << "\n"; + std::cout << "node count: " << map.size() << "\n"; + + map.threshold(); + map.prune(); + + std::cout << "After threshold/prune\n"; + printVoxelWithLabel("a", map.getVoxelValue(a), labels[IndexKey(a)]); + printVoxelWithLabel("b", map.getVoxelValue(b), labels[IndexKey(b)]); + printVoxelWithLabel("c", map.getVoxelValue(c), labels[IndexKey(c)]); + std::cout << "label entries: " << labels.size() << "\n"; + std::cout << "block count: " << map.getHashMap().size() << "\n"; + std::cout << "node count: " << map.size() << "\n"; + + return 0; +} diff --git a/examples/cpp/digital_twin/map_api/discrete/layered_map_experiment.cc b/examples/cpp/digital_twin/map_api/discrete/layered_map_experiment.cc new file mode 100644 index 000000000..22d67e1cb --- /dev/null +++ b/examples/cpp/digital_twin/map_api/discrete/layered_map_experiment.cc @@ -0,0 +1,123 @@ +#include +#include +#include + +#include "../../common/example_layered_map_config.h" + +namespace { +struct ExampleVoxelView { + LayeredVoxel continuous; + std::optional semantic_label; + std::optional changed; +}; + +ExampleVoxelView getExampleVoxel(const ExampleLayeredMap& map, + const wavemap::Index3D& index) { + return {map.continuousMap().getVoxelValue(index), + map.discreteLayers().semantic.getValue(index), + map.discreteLayers().changed.getValue(index)}; +} + +void printOptionalBool(const std::optional& value) { + if (value) { + std::cout << (*value ? "true" : "false"); + } else { + std::cout << "none"; + } +} + +void printLayeredMapVoxel(const std::string& name, + const ExampleVoxelView& voxel) { + std::cout << name << ": occ=" << voxel.continuous.occupancy + << " trav=" << voxel.continuous.data.traversability << " rgb=(" + << voxel.continuous.data.rgb.r << ", " + << voxel.continuous.data.rgb.g << ", " + << voxel.continuous.data.rgb.b << ") semantic="; + if (voxel.semantic_label) { + std::cout << *voxel.semantic_label; + } else { + std::cout << "none"; + } + std::cout << " changed="; + printOptionalBool(voxel.changed); + std::cout << "\n"; +} +} // namespace + +int main() { + ExampleLayeredMapConfig config; + config.continuous_map.min_cell_width = 0.1f; + config.continuous_map.min_log_odds = -100.f; + config.continuous_map.max_log_odds = 100.f; + config.continuous_map.tree_height = 3; + config.discrete_compression.block_height = 1; + + ExampleLayeredMap map(config); + + const wavemap::Index3D a(1, 2, 3); + const wavemap::Index3D b(2, 2, 3); + const wavemap::Index3D c(3, 2, 3); + + map.continuousMap().setVoxelValue( + a, makeLayeredVoxel(10.f, rgb(1.f, 0.f, 0.f), 0.1f)); + map.continuousMap().setVoxelValue( + b, makeLayeredVoxel(20.f, rgb(0.f, 1.f, 0.f), 0.5f)); + map.continuousMap().setVoxelValue( + c, makeLayeredVoxel(30.f, rgb(0.f, 0.f, 1.f), 0.9f)); + + map.discreteLayers().semantic.setValue(a, 1); + map.discreteLayers().semantic.setValue(b, 5); + map.discreteLayers().semantic.setValue(c, 9); + + map.discreteLayers().changed.setValue(a, false); + map.discreteLayers().changed.setValue(b, true); + map.discreteLayers().changed.setValue(c, false); + + std::cout << "LayeredMap experiment\n"; + std::cout << "Continuous fields are stored in the wavelet octree. " + "Discrete semantic and binary layers are stored directly in " + "compressed parent + exception layers, outside Haar arithmetic.\n"; + + std::cout << "Before threshold/prune\n"; + printLayeredMapVoxel("a", getExampleVoxel(map, a)); + printLayeredMapVoxel("b", getExampleVoxel(map, b)); + printLayeredMapVoxel("c", getExampleVoxel(map, c)); + std::cout << "continuous nodes: " << map.continuousMap().size() << "\n"; + std::cout << "semantic compressed parents: " << map.discreteLayers().semantic.parentCount() + << " exceptions: " << map.discreteLayers().semantic.exceptionCount() + << " observed values: " << map.discreteLayers().semantic.observedValueCount() + << "\n"; + std::cout << "changed compressed parents: " << map.discreteLayers().changed.parentCount() + << " exceptions: " << map.discreteLayers().changed.exceptionCount() + << " observed values: " << map.discreteLayers().changed.observedValueCount() + << "\n"; + + map.continuousMap().threshold(); + map.continuousMap().prune(); + + std::cout << "After threshold/prune\n"; + printLayeredMapVoxel("a", getExampleVoxel(map, a)); + printLayeredMapVoxel("b", getExampleVoxel(map, b)); + printLayeredMapVoxel("c", getExampleVoxel(map, c)); + std::cout << "continuous nodes: " << map.continuousMap().size() << "\n"; + std::cout << "semantic compressed parents: " << map.discreteLayers().semantic.parentCount() + << " exceptions: " << map.discreteLayers().semantic.exceptionCount() + << " observed values: " << map.discreteLayers().semantic.observedValueCount() + << "\n"; + std::cout << "changed compressed parents: " << map.discreteLayers().changed.parentCount() + << " exceptions: " << map.discreteLayers().changed.exceptionCount() + << " observed values: " << map.discreteLayers().changed.observedValueCount() + << "\n"; + + const bool semantic_labels_preserved = getExampleVoxel(map, a).semantic_label == 1 && + getExampleVoxel(map, b).semantic_label == 5 && + getExampleVoxel(map, c).semantic_label == 9; + const bool changed_flags_preserved = getExampleVoxel(map, a).changed == false && + getExampleVoxel(map, b).changed == true && + getExampleVoxel(map, c).changed == false; + if (!semantic_labels_preserved || !changed_flags_preserved) { + std::cout << "Discrete layers were not preserved.\n"; + return 1; + } + return 0; +} diff --git a/examples/cpp/digital_twin/map_api/discrete/layered_map_update_behavior_experiment.cc b/examples/cpp/digital_twin/map_api/discrete/layered_map_update_behavior_experiment.cc new file mode 100644 index 000000000..1dad3d844 --- /dev/null +++ b/examples/cpp/digital_twin/map_api/discrete/layered_map_update_behavior_experiment.cc @@ -0,0 +1,82 @@ +#include +#include + +#include "../../common/example_layered_map_config.h" + +namespace { +template +bool sameValue(const std::optional& value, const ValueT& expected) { + return value && *value == expected; +} + +bool expect(const std::string& name, bool condition) { + std::cout << " " << name << ": " << (condition ? "pass" : "FAIL") + << "\n"; + return condition; +} +} // namespace + +int main() { + ExampleLayeredMapConfig config; + config.continuous_map.min_cell_width = 0.1f; + config.continuous_map.min_log_odds = -100.f; + config.continuous_map.max_log_odds = 100.f; + config.continuous_map.tree_height = 3; + config.discrete_compression.block_height = 1; + + ExampleLayeredMap map(config); + const wavemap::Index3D a(0, 0, 0); + const wavemap::Index3D b(1, 0, 0); + const wavemap::Index3D c(1, 1, 1); + + map.continuousMap().setVoxelValue( + a, makeLayeredVoxel(8.f, rgb(1.2f, -0.1f, 0.5f), 1.2f)); + map.continuousMap().setVoxelValue( + b, makeLayeredVoxel(9.f, rgb(0.2f, 0.3f, 0.4f), 0.6f)); + map.continuousMap().setVoxelValue( + c, makeLayeredVoxel(7.f, rgb(0.2f, 0.3f, 0.4f), 0.6f)); + + map.discreteLayers().semantic.setValue(a, 1); + map.discreteLayers().semantic.setValue(b, 1); + map.discreteLayers().semantic.setValue(c, 2); + map.discreteLayers().changed.setValue(a, false); + map.discreteLayers().changed.setValue(b, false); + map.discreteLayers().changed.setValue(c, true); + + map.continuousMap().threshold(); + map.continuousMap().prune(); + + bool ok = true; + std::cout << "LayeredMap continuous + discrete update behavior experiment\n"; + + const LayeredVoxel voxel_a = map.continuousMap().getVoxelValue(a); + ok &= expect("continuous threshold clamps rgb lower bound", + 0.f <= voxel_a.data.rgb.g); + ok &= expect("continuous threshold clamps rgb upper bound", + voxel_a.data.rgb.r <= 1.f); + ok &= expect("continuous threshold clamps traversability", + voxel_a.data.traversability <= 1.f); + + ok &= expect("semantic value a preserved", + sameValue(map.discreteLayers().semantic.getValue(a), 1)); + ok &= expect("semantic value c preserved", + sameValue(map.discreteLayers().semantic.getValue(c), 2)); + ok &= expect("binary value a preserved", + sameValue(map.discreteLayers().changed.getValue(a), false)); + ok &= expect("binary value c preserved", + sameValue(map.discreteLayers().changed.getValue(c), true)); + ok &= expect("unknown discrete voxel remains unknown", + !map.discreteLayers().semantic.getValue(wavemap::Index3D(2, 2, 2))); + ok &= expect("semantic layer compressed parent count is stable", + map.discreteLayers().semantic.parentCount() == 1u); + ok &= expect("semantic layer has one exception", + map.discreteLayers().semantic.exceptionCount() == 1u); + ok &= expect("changed layer has one exception", + map.discreteLayers().changed.exceptionCount() == 1u); + + if (!ok) { + std::cout << "LayeredMap integrated update behavior failed.\n"; + return 1; + } + return 0; +} diff --git a/examples/cpp/digital_twin/map_api/discrete/raw_discrete_voxel_storage_experiment.cc b/examples/cpp/digital_twin/map_api/discrete/raw_discrete_voxel_storage_experiment.cc new file mode 100644 index 000000000..76f03475a --- /dev/null +++ b/examples/cpp/digital_twin/map_api/discrete/raw_discrete_voxel_storage_experiment.cc @@ -0,0 +1,90 @@ +#include +#include +#include +#include +#include +#include + +#include "wavemap/core/indexing/index_hashes.h" +#include "wavemap/core/indexing/ndtree_index.h" + +struct RawDiscreteData { + int label = 0; + bool dynamic = false; + char code = '\0'; + + bool operator==(const RawDiscreteData& other) const { + return label == other.label && dynamic == other.dynamic && + code == other.code; + } +}; + +struct IndexKey { + int x = 0; + int y = 0; + int z = 0; + + explicit IndexKey(const wavemap::Index3D& index) + : x(index.x()), y(index.y()), z(index.z()) {} + + bool operator<(const IndexKey& other) const { + return std::tie(x, y, z) < std::tie(other.x, other.y, other.z); + } +}; + +using RawStorage = std::map; + +void printData(const std::string& name, const RawDiscreteData& data) { + std::cout << name << ": label=" << data.label + << " dynamic=" << std::boolalpha << data.dynamic + << " code=" << data.code << "\n"; +} + +int main() { + const std::vector> samples = { + {wavemap::Index3D(1, 2, 3), {1, false, 'A'}}, + {wavemap::Index3D(2, 2, 3), {5, true, 'B'}}, + {wavemap::Index3D(3, 2, 3), {9, false, 'C'}}, + }; + + RawStorage storage; + + const auto write_start = std::chrono::steady_clock::now(); + for (const auto& [index, data] : samples) { + storage[IndexKey(index)] = data; + } + const auto write_end = std::chrono::steady_clock::now(); + + size_t preserved_values = 0u; + const auto read_start = std::chrono::steady_clock::now(); + for (const auto& [index, expected] : samples) { + const RawDiscreteData read = storage.at(IndexKey(index)); + printData("read", read); + if (read == expected) { + ++preserved_values; + } + } + const auto read_end = std::chrono::steady_clock::now(); + + const auto write_us = + std::chrono::duration_cast(write_end - + write_start) + .count(); + const auto read_us = + std::chrono::duration_cast(read_end - + read_start) + .count(); + + const size_t approximate_payload_bytes = storage.size() * + (sizeof(IndexKey) + sizeof(RawDiscreteData)); + + std::cout << "entries: " << storage.size() << "\n"; + std::cout << "preserved values: " << preserved_values << "/" + << samples.size() << "\n"; + std::cout << "approx payload bytes without map overhead: " + << approximate_payload_bytes << "\n"; + std::cout << "write time [us]: " << write_us << "\n"; + std::cout << "read time [us]: " << read_us << "\n"; + + return preserved_values == samples.size() ? 0 : 1; +} diff --git a/examples/cpp/digital_twin/map_api/discrete/raw_inside_map_experiment.cc b/examples/cpp/digital_twin/map_api/discrete/raw_inside_map_experiment.cc new file mode 100644 index 000000000..462b4a0d7 --- /dev/null +++ b/examples/cpp/digital_twin/map_api/discrete/raw_inside_map_experiment.cc @@ -0,0 +1,143 @@ +#include +#include +#include +#include +#include +#include +#include + +#include "wavemap/core/indexing/ndtree_index.h" +#include "wavemap/core/map/hashed_wavelet_octree.h" + +struct RawDiscreteData { + int label = 0; + bool dynamic = false; + char code = '\0'; + + bool operator==(const RawDiscreteData& other) const { + return label == other.label && dynamic == other.dynamic && + code == other.code; + } +}; + +struct IndexKey { + int x = 0; + int y = 0; + int z = 0; + + explicit IndexKey(const wavemap::Index3D& index) + : x(index.x()), y(index.y()), z(index.z()) {} + + bool operator<(const IndexKey& other) const { + return std::tie(x, y, z) < std::tie(other.x, other.y, other.z); + } +}; + +class RawDiscreteLayeredMap { + public: + explicit RawDiscreteLayeredMap( + const wavemap::HashedWaveletOctreeConfig& config) + : occupancy_map_(config) {} + + void setOccupancy(const wavemap::Index3D& index, + wavemap::FloatingPoint occupancy) { + occupancy_map_.setCellValue(index, occupancy); + } + + wavemap::FloatingPoint getOccupancy(const wavemap::Index3D& index) const { + return occupancy_map_.getCellValue(index); + } + + void setRawData(const wavemap::Index3D& index, const RawDiscreteData& data) { + raw_data_[IndexKey(index)] = data; + } + + std::optional getRawData( + const wavemap::Index3D& index) const { + const auto it = raw_data_.find(IndexKey(index)); + if (it == raw_data_.end()) { + return std::nullopt; + } + return it->second; + } + + size_t rawDataSize() const { return raw_data_.size(); } + size_t occupancyBlockCount() const { return occupancy_map_.getHashMap().size(); } + size_t occupancyNodeCount() const { return occupancy_map_.size(); } + + size_t approximateRawBytesWithoutMapOverhead() const { + return raw_data_.size() * (sizeof(IndexKey) + sizeof(RawDiscreteData)); + } + + private: + wavemap::HashedWaveletOctree occupancy_map_; + std::map raw_data_; +}; + +void printData(const std::string& name, wavemap::FloatingPoint occupancy, + const RawDiscreteData& data) { + std::cout << name << ": occ=" << occupancy << " label=" << data.label + << " dynamic=" << std::boolalpha << data.dynamic + << " code=" << data.code << "\n"; +} + +int main() { + wavemap::HashedWaveletOctreeConfig config; + config.min_cell_width = 0.1f; + config.min_log_odds = -100.f; + config.max_log_odds = 100.f; + config.tree_height = 3; + + RawDiscreteLayeredMap map(config); + + const std::vector> samples = { + {wavemap::Index3D(1, 2, 3), 10.f, {1, false, 'A'}}, + {wavemap::Index3D(2, 2, 3), 20.f, {5, true, 'B'}}, + {wavemap::Index3D(3, 2, 3), 30.f, {9, false, 'C'}}, + }; + + const auto write_start = std::chrono::steady_clock::now(); + for (const auto& [index, occupancy, data] : samples) { + map.setOccupancy(index, occupancy); + map.setRawData(index, data); + } + const auto write_end = std::chrono::steady_clock::now(); + + size_t preserved_raw_values = 0u; + const auto read_start = std::chrono::steady_clock::now(); + for (const auto& [index, expected_occupancy, expected_data] : samples) { + const wavemap::FloatingPoint occupancy = map.getOccupancy(index); + const auto data = map.getRawData(index); + if (!data) { + std::cout << "missing raw data\n"; + continue; + } + printData("read", occupancy, *data); + if (*data == expected_data) { + ++preserved_raw_values; + } + } + const auto read_end = std::chrono::steady_clock::now(); + + const auto write_us = + std::chrono::duration_cast(write_end - + write_start) + .count(); + const auto read_us = + std::chrono::duration_cast(read_end - + read_start) + .count(); + + std::cout << "raw entries: " << map.rawDataSize() << "\n"; + std::cout << "preserved raw values: " << preserved_raw_values << "/" + << samples.size() << "\n"; + std::cout << "occupancy blocks: " << map.occupancyBlockCount() << "\n"; + std::cout << "occupancy nodes: " << map.occupancyNodeCount() << "\n"; + std::cout << "approx raw bytes without map overhead: " + << map.approximateRawBytesWithoutMapOverhead() << "\n"; + std::cout << "write time [us]: " << write_us << "\n"; + std::cout << "read time [us]: " << read_us << "\n"; + + return preserved_raw_values == samples.size() ? 0 : 1; +} diff --git a/examples/cpp/digital_twin/map_api/discrete/semantic_majority_exceptions_experiment.cc b/examples/cpp/digital_twin/map_api/discrete/semantic_majority_exceptions_experiment.cc new file mode 100644 index 000000000..ac04963fc --- /dev/null +++ b/examples/cpp/digital_twin/map_api/discrete/semantic_majority_exceptions_experiment.cc @@ -0,0 +1,242 @@ +#include +#include +#include +#include +#include +#include +#include + +#include "wavemap/core/indexing/ndtree_index.h" + +struct IndexKey { + int x = 0; + int y = 0; + int z = 0; + + explicit IndexKey(const wavemap::Index3D& index) + : x(index.x()), y(index.y()), z(index.z()) {} + + bool operator<(const IndexKey& other) const { + return std::tie(x, y, z) < std::tie(other.x, other.y, other.z); + } +}; + +struct BlockKey { + int x = 0; + int y = 0; + int z = 0; + + bool operator<(const BlockKey& other) const { + return std::tie(x, y, z) < std::tie(other.x, other.y, other.z); + } +}; + +struct LocalKey { + int x = 0; + int y = 0; + int z = 0; + + bool operator<(const LocalKey& other) const { + return std::tie(x, y, z) < std::tie(other.x, other.y, other.z); + } +}; + +struct ExceptionEntry { + LocalKey local_index; + int label = 0; +}; + +struct CompressedSemanticBlock { + int majority_label = 0; + std::vector exceptions; +}; + +class MajorityExceptionSemanticLayer { + public: + explicit MajorityExceptionSemanticLayer(int block_side) + : block_side_(block_side) {} + + void build(const std::map& raw_labels) { + std::map>> pending_blocks; + for (const auto& [index_key, label] : raw_labels) { + const wavemap::Index3D index(index_key.x, index_key.y, index_key.z); + pending_blocks[toBlockKey(index)].push_back({toLocalKey(index), label}); + } + + blocks_.clear(); + for (const auto& [block_key, entries] : pending_blocks) { + std::unordered_map counts; + for (const auto& [local_key, label] : entries) { + (void)local_key; + ++counts[label]; + } + + const auto majority_it = std::max_element( + counts.begin(), counts.end(), [](const auto& lhs, const auto& rhs) { + return lhs.second < rhs.second; + }); + + CompressedSemanticBlock block; + block.majority_label = majority_it->first; + for (const auto& [local_key, label] : entries) { + if (label != block.majority_label) { + block.exceptions.push_back({local_key, label}); + } + } + blocks_[block_key] = std::move(block); + } + } + + int getLabel(const wavemap::Index3D& index) const { + const BlockKey block_key = toBlockKey(index); + const auto block_it = blocks_.find(block_key); + if (block_it == blocks_.end()) { + return 0; + } + + const LocalKey local_key = toLocalKey(index); + const auto& block = block_it->second; + for (const ExceptionEntry& exception : block.exceptions) { + if (exception.local_index.x == local_key.x && + exception.local_index.y == local_key.y && + exception.local_index.z == local_key.z) { + return exception.label; + } + } + return block.majority_label; + } + + size_t numBlocks() const { return blocks_.size(); } + + size_t numExceptions() const { + size_t total = 0u; + for (const auto& [block_key, block] : blocks_) { + (void)block_key; + total += block.exceptions.size(); + } + return total; + } + + size_t approximateBytes() const { + return blocks_.size() * (sizeof(BlockKey) + sizeof(int)) + + numExceptions() * sizeof(ExceptionEntry); + } + + private: + const int block_side_; + std::map blocks_; + + BlockKey toBlockKey(const wavemap::Index3D& index) const { + return {floorDiv(index.x(), block_side_), floorDiv(index.y(), block_side_), + floorDiv(index.z(), block_side_)}; + } + + LocalKey toLocalKey(const wavemap::Index3D& index) const { + return {floorMod(index.x(), block_side_), floorMod(index.y(), block_side_), + floorMod(index.z(), block_side_)}; + } + + static int floorDiv(int value, int divisor) { + int quotient = value / divisor; + const int remainder = value % divisor; + if (remainder != 0 && ((remainder < 0) != (divisor < 0))) { + --quotient; + } + return quotient; + } + + static int floorMod(int value, int divisor) { + const int remainder = value % divisor; + return remainder < 0 ? remainder + divisor : remainder; + } +}; + +int makeMostlyHomogeneousLabel(int x, int y, int z) { + const int region_label = 1 + (x / 4) + 2 * (y / 4) + 4 * (z / 4); + const bool is_exception = ((3 * x + 5 * y + 7 * z) % 17) == 0; + if (!is_exception) { + return region_label; + } + return 20 + ((x + y + z) % 5); +} + +int makeNoisyLabel(int x, int y, int z) { + return 1 + ((3 * x + 5 * y + 7 * z) % 6); +} + +std::map makeRawLabels(bool noisy) { + std::map labels; + for (int x = 0; x < 8; ++x) { + for (int y = 0; y < 8; ++y) { + for (int z = 0; z < 4; ++z) { + const wavemap::Index3D index(x, y, z); + labels[IndexKey(index)] = noisy ? makeNoisyLabel(x, y, z) + : makeMostlyHomogeneousLabel(x, y, z); + } + } + } + return labels; +} + +void runScenario(const char* name, bool noisy) { + const std::map raw_labels = makeRawLabels(noisy); + MajorityExceptionSemanticLayer layer(/*block_side=*/4); + + const auto build_start = std::chrono::steady_clock::now(); + layer.build(raw_labels); + const auto build_end = std::chrono::steady_clock::now(); + + size_t preserved = 0u; + size_t printed_mismatches = 0u; + constexpr size_t kMaxPrintedMismatches = 5u; + + const auto read_start = std::chrono::steady_clock::now(); + for (const auto& [index_key, expected_label] : raw_labels) { + const wavemap::Index3D index(index_key.x, index_key.y, index_key.z); + const int read_label = layer.getLabel(index); + if (read_label == expected_label) { + ++preserved; + } else if (printed_mismatches < kMaxPrintedMismatches) { + std::cout << "mismatch at " << index.transpose() + << " expected=" << expected_label << " read=" << read_label + << "\n"; + ++printed_mismatches; + } + } + const auto read_end = std::chrono::steady_clock::now(); + + const auto build_us = + std::chrono::duration_cast(build_end - + build_start) + .count(); + const auto read_us = + std::chrono::duration_cast(read_end - + read_start) + .count(); + + const size_t raw_bytes = raw_labels.size() * (sizeof(IndexKey) + sizeof(int)); + const size_t compressed_bytes = layer.approximateBytes(); + + std::cout << "Scenario: " << name << "\n"; + std::cout << "samples: " << raw_labels.size() << "\n"; + std::cout << "preserved labels: " << preserved << "/" << raw_labels.size() + << "\n"; + std::cout << "blocks: " << layer.numBlocks() << "\n"; + std::cout << "exceptions: " << layer.numExceptions() << "\n"; + std::cout << "raw approx bytes: " << raw_bytes << "\n"; + std::cout << "majority+exceptions approx bytes: " << compressed_bytes + << "\n"; + std::cout << "approx compression ratio vs raw: " + << static_cast(compressed_bytes) / + static_cast(raw_bytes) + << "\n"; + std::cout << "build time [us]: " << build_us << "\n"; + std::cout << "read time [us]: " << read_us << "\n"; +} + +int main() { + runScenario("mostly homogeneous labels", false); + std::cout << "---\n"; + runScenario("noisy labels", true); + return 0; +} diff --git a/examples/cpp/digital_twin/map_api/user_facing/custom_layered_map_example.cc b/examples/cpp/digital_twin/map_api/user_facing/custom_layered_map_example.cc new file mode 100644 index 000000000..5a923c1d8 --- /dev/null +++ b/examples/cpp/digital_twin/map_api/user_facing/custom_layered_map_example.cc @@ -0,0 +1,119 @@ +#include +#include +#include +#include +#include + +#include "../../common/custom_layered_map_config.h" + +namespace { +bool almostEqual(float lhs, float rhs) { return std::abs(lhs - rhs) < 1e-5f; } + +template +bool sameValue(const std::optional& value, const ValueT& expected) { + return value && *value == expected; +} + +void printOptionalBool(const std::optional& value) { + if (value) { + std::cout << (*value ? "true" : "false"); + } else { + std::cout << "none"; + } +} +} // namespace + +int main() { + // 1. User code starts from its own config type. + MyLayeredMapConfig config = makeMyLayeredMapConfig(); + MyLayeredMap map(config); + + // 2. Continuous data lives inside the wavelet map together with occupancy. + const wavemap::Index3D tree_index(1, 2, 3); + const wavemap::Index3D road_index(2, 2, 3); + map.continuousMap().setVoxelValue( + tree_index, MyVoxel(/*occupancy=*/12.f, + MyContinuousLayers{/*temperature=*/22.5f, + /*intensity=*/0.75f})); + map.continuousMap().setVoxelValue( + road_index, MyVoxel(/*occupancy=*/8.f, + MyContinuousLayers{/*temperature=*/19.0f, + /*intensity=*/0.25f})); + + // 3. Discrete data is updated per voxel in the side discrete layer bundle. + map.discreteLayers().semantic_label.setValue(tree_index, 4); + map.discreteLayers().semantic_label.setValue(road_index, 7); + map.discreteLayers().manually_changed.setValue(tree_index, true); + map.discreteLayers().manually_changed.setValue(road_index, false); + + // 4. Continuous data still uses the Wavemap wavelet threshold/prune path. + map.continuousMap().threshold(); + map.continuousMap().prune(); + + const MyVoxel tree_voxel = map.continuousMap().getVoxelValue(tree_index); + const std::optional tree_label = + map.discreteLayers().semantic_label.getValue(tree_index); + const std::optional tree_changed = + map.discreteLayers().manually_changed.getValue(tree_index); + + std::cout << "User-facing LayeredMap example\n"; + std::cout << "continuous tree voxel: occupancy=" << tree_voxel.occupancy + << " temperature=" << tree_voxel.data.temperature + << " intensity=" << tree_voxel.data.intensity << "\n"; + std::cout << "discrete tree voxel: semantic_label=" + << (tree_label ? std::to_string(*tree_label) : "none") + << " manually_changed="; + printOptionalBool(tree_changed); + std::cout << "\n"; + + // 5. Full LayeredMap persistence stores continuous + discrete layers. + const std::filesystem::path output_dir = "/home/ci/data/maps"; + std::filesystem::create_directories(output_dir); + const std::filesystem::path output_file = + output_dir / "user_facing_layered_map_example.lwvmp"; + if (!MyLayeredMapIo::save(output_file, map)) { + std::cerr << "Failed to save LayeredMap to " << output_file << "\n"; + return 1; + } + + MyLayeredMap loaded_map(config); + if (!MyLayeredMapIo::load(output_file, loaded_map)) { + std::cerr << "Failed to load LayeredMap from " << output_file << "\n"; + return 1; + } + + const MyVoxel loaded_tree_voxel = + loaded_map.continuousMap().getVoxelValue(tree_index); + const bool continuous_ok = + almostEqual(tree_voxel.occupancy, loaded_tree_voxel.occupancy) && + almostEqual(tree_voxel.data.temperature, + loaded_tree_voxel.data.temperature) && + almostEqual(tree_voxel.data.intensity, loaded_tree_voxel.data.intensity); + const bool discrete_ok = + sameValue(loaded_map.discreteLayers().semantic_label.getValue(tree_index), + 4) && + sameValue(loaded_map.discreteLayers().semantic_label.getValue(road_index), + 7) && + sameValue( + loaded_map.discreteLayers().manually_changed.getValue(tree_index), + true) && + sameValue( + loaded_map.discreteLayers().manually_changed.getValue(road_index), + false); + const bool unknown_ok = + !loaded_map.discreteLayers().semantic_label.getValue( + wavemap::Index3D(100, 100, 100)); + + std::cout << "saved file: " << output_file << "\n"; + std::cout << "continuous values preserved after load: " + << (continuous_ok ? "yes" : "no") << "\n"; + std::cout << "discrete values preserved after load: " + << (discrete_ok ? "yes" : "no") << "\n"; + std::cout << "unknown discrete voxel remains unknown: " + << (unknown_ok ? "yes" : "no") << "\n"; + std::cout << "semantic parent cells: " + << loaded_map.discreteLayers().semantic_label.parentCount() + << "\n"; + + return continuous_ok && discrete_ok && unknown_ok ? 0 : 1; +} diff --git a/examples/cpp/digital_twin/ros/layered_map_structured_ros_msg_experiment.cc b/examples/cpp/digital_twin/ros/layered_map_structured_ros_msg_experiment.cc new file mode 100644 index 000000000..b277a3e01 --- /dev/null +++ b/examples/cpp/digital_twin/ros/layered_map_structured_ros_msg_experiment.cc @@ -0,0 +1,105 @@ +#include +#include +#include +#include + +#include "../common/example_layered_map_ros_config.h" +#include "../common/layered_ros_converter.h" + +namespace { +bool almostEqual(float lhs, float rhs) { + return std::abs(lhs - rhs) < 1e-5f; +} + +bool almostEqual(const LayeredVoxel& lhs, const LayeredVoxel& rhs) { + return almostEqual(lhs.occupancy, rhs.occupancy) && + almostEqual(lhs.data.rgb.r, rhs.data.rgb.r) && + almostEqual(lhs.data.rgb.g, rhs.data.rgb.g) && + almostEqual(lhs.data.rgb.b, rhs.data.rgb.b) && + almostEqual(lhs.data.traversability, rhs.data.traversability); +} + +template +bool sameValue(const std::optional& value, const ValueT& expected) { + return value && *value == expected; +} +} // namespace + +int main() { + ExampleLayeredMapConfig config; + config.continuous_map.min_cell_width = 0.1f; + config.continuous_map.min_log_odds = -2.f; + config.continuous_map.max_log_odds = 4.f; + config.continuous_map.tree_height = 3; + config.discrete_compression.block_height = 1; + + ExampleLayeredMap map(config); + std::vector populated_indices; + populateLayeredCube(map.continuousMap(), populated_indices); + + const wavemap::Index3D first_index = populated_indices.front(); + const wavemap::Index3D middle_index = populated_indices[populated_indices.size() / 2u]; + const wavemap::Index3D last_index = populated_indices.back(); + + map.discreteLayers().semantic.setValue(first_index, 1); + map.discreteLayers().semantic.setValue(middle_index, 2); + map.discreteLayers().semantic.setValue(last_index, 1); + map.discreteLayers().changed.setValue(first_index, false); + map.discreteLayers().changed.setValue(middle_index, true); + map.discreteLayers().changed.setValue(last_index, false); + + wavemap_msgs::LayeredMap msg; + if (!wavemap::convert::layeredMapToRosMsg( + map, "map", ros::Time(1, 0), msg)) { + std::cerr << "Failed to convert LayeredMap to ROS message.\n"; + return 1; + } + + ExampleLayeredMap loaded_map(config); + if (!wavemap::convert::rosMsgToLayeredMap( + msg, loaded_map)) { + std::cerr << "Failed to convert ROS message back to LayeredMap.\n"; + return 1; + } + + size_t verified_continuous = 0u; + for (const wavemap::Index3D& voxel_index : populated_indices) { + if (almostEqual(map.continuousMap().getVoxelValue(voxel_index), + loaded_map.continuousMap().getVoxelValue(voxel_index))) { + ++verified_continuous; + } + } + + const bool semantic_ok = + sameValue(loaded_map.discreteLayers().semantic.getValue(first_index), 1) && + sameValue(loaded_map.discreteLayers().semantic.getValue(middle_index), 2) && + sameValue(loaded_map.discreteLayers().semantic.getValue(last_index), 1); + const bool changed_ok = + sameValue(loaded_map.discreteLayers().changed.getValue(first_index), false) && + sameValue(loaded_map.discreteLayers().changed.getValue(middle_index), true) && + sameValue(loaded_map.discreteLayers().changed.getValue(last_index), false); + const bool unknown_ok = + !loaded_map.discreteLayers().semantic.getValue(wavemap::Index3D(0, 0, 0)); + + std::cout << "LayeredMap ROS message experiment\n"; + std::cout << "frame: " << msg.header.frame_id << "\n"; + std::cout << "continuous ROS map blocks: " + << msg.continuous_map.layered_hashed_wavelet_octree.front().blocks.size() + << "\n"; + std::cout << "discrete ROS layers: " << msg.discrete_layers.size() << "\n"; + std::cout << "continuous voxels verified: " << verified_continuous << "/" + << populated_indices.size() << "\n"; + std::cout << "semantic labels preserved: " << (semantic_ok ? "yes" : "no") + << "\n"; + std::cout << "changed flags preserved: " << (changed_ok ? "yes" : "no") + << "\n"; + std::cout << "unknown discrete voxel remains unknown: " + << (unknown_ok ? "yes" : "no") << "\n"; + + return verified_continuous == populated_indices.size() && semantic_ok && + changed_ok && unknown_ok + ? 0 + : 1; +} diff --git a/examples/cpp/digital_twin/ros/layered_ros_msg_experiment.cc b/examples/cpp/digital_twin/ros/layered_ros_msg_experiment.cc index 28752b0ea..3599d31bf 100644 --- a/examples/cpp/digital_twin/ros/layered_ros_msg_experiment.cc +++ b/examples/cpp/digital_twin/ros/layered_ros_msg_experiment.cc @@ -12,7 +12,7 @@ bool almostEqual(float lhs, float rhs) { } bool almostEqual(const LayeredVoxel& lhs, const LayeredVoxel& rhs) { - return almostEqual(lhs.occupancy, rhs.occupancy) && almostEqual(lhs.data.r, rhs.data.r) && almostEqual(lhs.data.g, rhs.data.g) && almostEqual(lhs.data.b, rhs.data.b) && almostEqual(lhs.data.traversability, rhs.data.traversability); + return almostEqual(lhs.occupancy, rhs.occupancy) && almostEqual(lhs.data.rgb.r, rhs.data.rgb.r) && almostEqual(lhs.data.rgb.g, rhs.data.rgb.g) && almostEqual(lhs.data.rgb.b, rhs.data.rgb.b) && almostEqual(lhs.data.traversability, rhs.data.traversability); } } // namespace @@ -23,7 +23,7 @@ int main() { config.max_log_odds = 4.f; config.tree_height = 3; - LayeredMap map(config); + ContinuousWaveletMap map(config); std::vector populated_indices; populateLayeredCube(map, populated_indices); @@ -35,7 +35,7 @@ int main() { } // Convert the wrapped ROS message back into a fresh C++ map to validate the full wrapper round-trip. - LayeredMap::Ptr loaded_map; + ContinuousWaveletMap::Ptr loaded_map; if (!wavemap::convert::rosMsgToMap(msg, loaded_map)) { std::cerr << "Failed to convert wavemap_msgs::Map back to a layered map.\n"; return 1; diff --git a/examples/cpp/digital_twin/ros/publish_layered_map_experiment.cc b/examples/cpp/digital_twin/ros/publish_layered_map_experiment.cc index c64d24d1c..e221ce6d0 100644 --- a/examples/cpp/digital_twin/ros/publish_layered_map_experiment.cc +++ b/examples/cpp/digital_twin/ros/publish_layered_map_experiment.cc @@ -16,7 +16,7 @@ int main(int argc, char** argv) { config.max_log_odds = 4.f; config.tree_height = 3; - LayeredMap map(config); + ContinuousWaveletMap map(config); std::vector populated_indices; populateLayeredCube(map, populated_indices); diff --git a/examples/cpp/digital_twin/ros/publish_structured_layered_map_experiment.cc b/examples/cpp/digital_twin/ros/publish_structured_layered_map_experiment.cc new file mode 100644 index 000000000..2220c3cb8 --- /dev/null +++ b/examples/cpp/digital_twin/ros/publish_structured_layered_map_experiment.cc @@ -0,0 +1,99 @@ +#include +#include +#include + +#include +#include + +#include "../common/example_layered_map_ros_config.h" +#include "../common/layered_ros_converter.h" + +namespace { +void populateDiscreteLayers(ExampleLayeredMap& map, + const std::vector& indices) { + const wavemap::Index3D first_index = indices.front(); + const wavemap::Index3D middle_index = indices[indices.size() / 2u]; + const wavemap::Index3D last_index = indices.back(); + + map.discreteLayers().semantic.setValue(first_index, 1); + map.discreteLayers().semantic.setValue(middle_index, 2); + map.discreteLayers().semantic.setValue(last_index, 1); + map.discreteLayers().changed.setValue(first_index, false); + map.discreteLayers().changed.setValue(middle_index, true); + map.discreteLayers().changed.setValue(last_index, false); +} +} // namespace + +int main(int argc, char** argv) { + ros::init(argc, argv, "wavemap_publish_structured_layered_map_experiment"); + ros::NodeHandle nh; + + ExampleLayeredMapConfig config; + config.continuous_map.min_cell_width = 0.1f; + config.continuous_map.min_log_odds = -2.f; + config.continuous_map.max_log_odds = 4.f; + config.continuous_map.tree_height = 3; + config.discrete_compression.block_height = 1; + + ExampleLayeredMap map(config); + std::vector populated_indices; + populateLayeredCube(map.continuousMap(), populated_indices); + populateDiscreteLayers(map, populated_indices); + + wavemap_msgs::LayeredMap msg; + if (!wavemap::convert::layeredMapToRosMsg( + map, "map", ros::Time::now(), msg)) { + std::cerr << "Failed to convert LayeredMap to ROS message.\n"; + return 1; + } + + const std::string layered_map_topic = "/wavemap/layered_map"; + const std::string continuous_map_topic = "/wavemap/map"; + const int queue_size = 1; + const bool latch = true; + ros::Publisher layered_map_publisher = + nh.advertise(layered_map_topic, queue_size, latch); + ros::Publisher continuous_map_publisher = + nh.advertise(continuous_map_topic, queue_size, latch); + std::map marker_publishers; + auto marker_publisher_factory = [&](const std::string& topic) -> ros::Publisher& { + auto [publisher_it, inserted] = marker_publishers.emplace(topic, ros::Publisher{}); + if (inserted) { + publisher_it->second = + nh.advertise(topic, queue_size, latch); + } + return publisher_it->second; + }; + ros::Duration(0.5).sleep(); + + layered_map_publisher.publish(msg); + continuous_map_publisher.publish(msg.continuous_map); + if (!layered_map_viz::publishDiscreteLayerMarkers( + map.discreteLayers(), msg.header.frame_id, msg.header.stamp, + marker_publisher_factory)) { + std::cerr << "Failed to publish discrete layer markers.\n"; + return 1; + } + ros::spinOnce(); + + std::cout << "Published LayeredMap on: " << layered_map_topic << "\n"; + std::cout << "Published continuous map on: " << continuous_map_topic << "\n"; + for (const auto& [topic, publisher] : marker_publishers) { + (void)publisher; + std::cout << "Published discrete markers on: " << topic << "\n"; + } + std::cout << "Frame: " << msg.header.frame_id << "\n"; + std::cout << "Continuous ROS map blocks: " + << msg.continuous_map.layered_hashed_wavelet_octree.front().blocks.size() + << "\n"; + std::cout << "Discrete ROS layers: " << msg.discrete_layers.size() << "\n"; + std::cout << "Populated continuous voxels: " << populated_indices.size() << "\n"; + std::cout << "Semantic parent cells: " + << map.discreteLayers().semantic.parentCount() << "\n"; + std::cout << "Changed parent cells: " + << map.discreteLayers().changed.parentCount() << "\n"; + + ros::spin(); + return 0; +} diff --git a/examples/cpp/digital_twin/ros/subscribe_layered_map_experiment.cc b/examples/cpp/digital_twin/ros/subscribe_layered_map_experiment.cc index 374d46cad..987119b83 100644 --- a/examples/cpp/digital_twin/ros/subscribe_layered_map_experiment.cc +++ b/examples/cpp/digital_twin/ros/subscribe_layered_map_experiment.cc @@ -8,7 +8,7 @@ namespace { void layeredMapCallback(const wavemap_msgs::Map::ConstPtr& msg) { // Convert the standard wavemap ROS wrapper back into the custom layered C++ map type. - LayeredMap::Ptr map; + ContinuousWaveletMap::Ptr map; if (!wavemap::convert::rosMsgToMap(*msg, map)) { std::cerr << "Failed to convert wavemap_msgs::Map back to a layered map.\n"; return; diff --git a/examples/cpp/digital_twin/ros/subscribe_structured_layered_map_experiment.cc b/examples/cpp/digital_twin/ros/subscribe_structured_layered_map_experiment.cc new file mode 100644 index 000000000..78ccc8a98 --- /dev/null +++ b/examples/cpp/digital_twin/ros/subscribe_structured_layered_map_experiment.cc @@ -0,0 +1,109 @@ +#include +#include +#include +#include + +#include + +#include "../common/example_layered_map_ros_config.h" +#include "../common/layered_ros_converter.h" + +namespace { +ExampleLayeredMapConfig makeConfig() { + ExampleLayeredMapConfig config; + config.continuous_map.min_cell_width = 0.1f; + config.continuous_map.min_log_odds = -2.f; + config.continuous_map.max_log_odds = 4.f; + config.continuous_map.tree_height = 3; + config.discrete_compression.block_height = 1; + return config; +} + +bool almostEqual(float lhs, float rhs) { return std::abs(lhs - rhs) < 1e-5f; } + +bool almostEqual(const LayeredVoxel& lhs, const LayeredVoxel& rhs) { + return almostEqual(lhs.occupancy, rhs.occupancy) && + almostEqual(lhs.data.rgb.r, rhs.data.rgb.r) && + almostEqual(lhs.data.rgb.g, rhs.data.rgb.g) && + almostEqual(lhs.data.rgb.b, rhs.data.rgb.b) && + almostEqual(lhs.data.traversability, rhs.data.traversability); +} + +template +bool sameValue(const std::optional& value, const ValueT& expected) { + return value && *value == expected; +} + +void structuredLayeredMapCallback(const wavemap_msgs::LayeredMap::ConstPtr& msg) { + ExampleLayeredMapConfig config = makeConfig(); + + ExampleLayeredMap received_map(config); + if (!wavemap::convert::rosMsgToLayeredMap( + *msg, received_map)) { + std::cerr << "Failed to convert ROS message to LayeredMap.\n"; + ros::shutdown(); + return; + } + + ExampleLayeredMap expected_map(config); + std::vector populated_indices; + populateLayeredCube(expected_map.continuousMap(), populated_indices); + + const wavemap::Index3D first_index = populated_indices.front(); + const wavemap::Index3D middle_index = populated_indices[populated_indices.size() / 2u]; + const wavemap::Index3D last_index = populated_indices.back(); + + size_t verified_continuous = 0u; + for (const wavemap::Index3D& voxel_index : populated_indices) { + if (almostEqual(expected_map.continuousMap().getVoxelValue(voxel_index), + received_map.continuousMap().getVoxelValue(voxel_index))) { + ++verified_continuous; + } + } + + const bool semantic_ok = + sameValue(received_map.discreteLayers().semantic.getValue(first_index), 1) && + sameValue(received_map.discreteLayers().semantic.getValue(middle_index), 2) && + sameValue(received_map.discreteLayers().semantic.getValue(last_index), 1); + const bool changed_ok = + sameValue(received_map.discreteLayers().changed.getValue(first_index), false) && + sameValue(received_map.discreteLayers().changed.getValue(middle_index), true) && + sameValue(received_map.discreteLayers().changed.getValue(last_index), false); + const bool unknown_ok = + !received_map.discreteLayers().semantic.getValue(wavemap::Index3D(0, 0, 0)); + + std::cout << "Received LayeredMap frame: " << msg->header.frame_id + << "\n"; + std::cout << "Received LayeredMap stamp: " << msg->header.stamp + << "\n"; + std::cout << "Continuous ROS map blocks: " + << msg->continuous_map.layered_hashed_wavelet_octree.front().blocks.size() + << "\n"; + std::cout << "Discrete ROS layers: " << msg->discrete_layers.size() << "\n"; + std::cout << "continuous voxels verified: " << verified_continuous << "/" + << populated_indices.size() << "\n"; + std::cout << "semantic labels preserved: " << (semantic_ok ? "yes" : "no") + << "\n"; + std::cout << "changed flags preserved: " << (changed_ok ? "yes" : "no") + << "\n"; + std::cout << "unknown discrete voxel remains unknown: " + << (unknown_ok ? "yes" : "no") << "\n"; + + ros::shutdown(); +} +} // namespace + +int main(int argc, char** argv) { + ros::init(argc, argv, "wavemap_subscribe_structured_layered_map_experiment"); + ros::NodeHandle nh; + + const std::string topic = "/wavemap/layered_map"; + const int queue_size = 1; + ros::Subscriber subscriber = + nh.subscribe(topic, queue_size, structuredLayeredMapCallback); + + std::cout << "Waiting for LayeredMap on: " << topic << "\n"; + ros::spin(); + return 0; +} diff --git a/examples/cpp/digital_twin/voxel_data/new_voxel.cc b/examples/cpp/digital_twin/voxel_data/new_voxel.cc index b3a4fd593..c4ed2469f 100644 --- a/examples/cpp/digital_twin/voxel_data/new_voxel.cc +++ b/examples/cpp/digital_twin/voxel_data/new_voxel.cc @@ -11,15 +11,15 @@ int main() { std::cout << "Occupancy only: " << occupancy_only.occupancy << "\n"; - LayeredVoxel layered_voxel(0.8f, LayeredData{1.f, 0.f, 0.f, 0.75f}); + LayeredVoxel layered_voxel(0.8f, ContinuousLayers{rgb(1.f, 0.f, 0.f), 0.75f}); layered_voxel += 0.2f; printVoxel("Layered voxel", layered_voxel); wavemap::FloatingPoint occupancy_value = layered_voxel; std::cout << "Implicit occupancy value: " << occupancy_value << "\n"; - const LayeredVoxel voxel_a(0.4f, LayeredData{1.f, 0.2f, 0.f, 0.3f}); - const LayeredVoxel voxel_b(0.6f, LayeredData{0.f, 0.3f, 1.f, 0.5f}); + const LayeredVoxel voxel_a(0.4f, ContinuousLayers{rgb(1.f, 0.2f, 0.f), 0.3f}); + const LayeredVoxel voxel_b(0.6f, ContinuousLayers{rgb(0.f, 0.3f, 1.f), 0.5f}); printVoxel("Sum", voxel_a + voxel_b); printVoxel("Difference", voxel_b - voxel_a); diff --git a/examples/cpp/digital_twin/voxel_data/voxel_haar_transform.cc b/examples/cpp/digital_twin/voxel_data/voxel_haar_transform.cc index 5ca8db04a..4da2d17dc 100644 --- a/examples/cpp/digital_twin/voxel_data/voxel_haar_transform.cc +++ b/examples/cpp/digital_twin/voxel_data/voxel_haar_transform.cc @@ -12,14 +12,14 @@ using Transform = wavemap::HaarTransform; int main() { Coefficients::CoefficientsArray child_voxels{ - LayeredVoxel{0.1f, LayeredData{1.f, 0.f, 0.f, 0.1f}}, - LayeredVoxel{0.2f, LayeredData{0.f, 1.f, 0.f, 0.2f}}, - LayeredVoxel{0.3f, LayeredData{0.f, 0.f, 1.f, 0.3f}}, - LayeredVoxel{0.4f, LayeredData{1.f, 1.f, 0.f, 0.4f}}, - LayeredVoxel{0.5f, LayeredData{1.f, 0.f, 1.f, 0.5f}}, - LayeredVoxel{0.6f, LayeredData{0.f, 1.f, 1.f, 0.6f}}, - LayeredVoxel{0.7f, LayeredData{0.5f, 0.5f, 0.5f, 0.7f}}, - LayeredVoxel{0.8f, LayeredData{1.f, 1.f, 1.f, 0.8f}}, + LayeredVoxel{0.1f, ContinuousLayers{rgb(1.f, 0.f, 0.f), 0.1f}}, + LayeredVoxel{0.2f, ContinuousLayers{rgb(0.f, 1.f, 0.f), 0.2f}}, + LayeredVoxel{0.3f, ContinuousLayers{rgb(0.f, 0.f, 1.f), 0.3f}}, + LayeredVoxel{0.4f, ContinuousLayers{rgb(1.f, 1.f, 0.f), 0.4f}}, + LayeredVoxel{0.5f, ContinuousLayers{rgb(1.f, 0.f, 1.f), 0.5f}}, + LayeredVoxel{0.6f, ContinuousLayers{rgb(0.f, 1.f, 1.f), 0.6f}}, + LayeredVoxel{0.7f, ContinuousLayers{rgb(0.5f, 0.5f, 0.5f), 0.7f}}, + LayeredVoxel{0.8f, ContinuousLayers{rgb(1.f, 1.f, 1.f), 0.8f}}, }; const Coefficients::Parent parent_coefficients = diff --git a/interfaces/ros1/wavemap_msgs/CMakeLists.txt b/interfaces/ros1/wavemap_msgs/CMakeLists.txt index cb43f9ccf..37b852266 100644 --- a/interfaces/ros1/wavemap_msgs/CMakeLists.txt +++ b/interfaces/ros1/wavemap_msgs/CMakeLists.txt @@ -6,6 +6,8 @@ find_package(catkin REQUIRED COMPONENTS message_generation std_msgs) # Declare message files to be built add_message_files(FILES + DiscreteLayer.msg + DiscreteLayerCell.msg HashedBlock.msg HashedBlocks.msg HashedWaveletOctree.msg @@ -13,6 +15,7 @@ add_message_files(FILES Index3D.msg LayeredHashedWaveletOctree.msg LayeredHashedWaveletOctreeBlock.msg + LayeredMap.msg LayeredWaveletOctreeNode.msg Layer.msg Map.msg diff --git a/interfaces/ros1/wavemap_msgs/msg/DiscreteLayer.msg b/interfaces/ros1/wavemap_msgs/msg/DiscreteLayer.msg new file mode 100644 index 000000000..05cd17cc8 --- /dev/null +++ b/interfaces/ros1/wavemap_msgs/msg/DiscreteLayer.msg @@ -0,0 +1,10 @@ +# A compressed discrete voxel layer stored as parent cells with dominant values +# and explicit exceptions. +string name +string value_type + +# Same meaning as DiscreteCompressionConfig.block_height: +# 1 -> 2x2x2, 2 -> 4x4x4, etc. +int32 block_height + +DiscreteLayerCell[] cells diff --git a/interfaces/ros1/wavemap_msgs/msg/DiscreteLayerCell.msg b/interfaces/ros1/wavemap_msgs/msg/DiscreteLayerCell.msg new file mode 100644 index 000000000..0bd1cc0ef --- /dev/null +++ b/interfaces/ros1/wavemap_msgs/msg/DiscreteLayerCell.msg @@ -0,0 +1,21 @@ +# One compressed parent cell in a discrete layer. +# The parent_index identifies the compressed parent block. +Index3D parent_index + +# Default value for all observed children unless an exception overrides it. +# Exactly one value array is used, according to DiscreteLayer.value_type. +int32 dominant_int32 +uint8 dominant_uint8 + +# Child offsets observed inside this parent cell. +# This keeps sparse unknown children unknown instead of implicitly assigning +# them the dominant value. +int32[] observed_offsets + +# Child offsets whose value differs from the dominant value. +int32[] exception_offsets + +# Exception values aligned with exception_offsets. Exactly one array is used, +# according to DiscreteLayer.value_type. +int32[] exception_int32_values +uint8[] exception_uint8_values diff --git a/interfaces/ros1/wavemap_msgs/msg/LayeredMap.msg b/interfaces/ros1/wavemap_msgs/msg/LayeredMap.msg new file mode 100644 index 000000000..65364ce77 --- /dev/null +++ b/interfaces/ros1/wavemap_msgs/msg/LayeredMap.msg @@ -0,0 +1,7 @@ +Header header + +# Existing Wavemap structured message for the continuous wavelet map. +Map continuous_map + +# Additional non-wavelet discrete layers aligned with the same voxel index space. +DiscreteLayer[] discrete_layers diff --git a/interfaces/ros1/wavemap_ros_conversions/CMakeLists.txt b/interfaces/ros1/wavemap_ros_conversions/CMakeLists.txt index 2c505017e..97a1972f0 100644 --- a/interfaces/ros1/wavemap_ros_conversions/CMakeLists.txt +++ b/interfaces/ros1/wavemap_ros_conversions/CMakeLists.txt @@ -3,13 +3,13 @@ project(wavemap_ros_conversions) # Find dependencies find_package(catkin REQUIRED - COMPONENTS roscpp eigen_conversions wavemap wavemap_msgs) + COMPONENTS roscpp eigen_conversions visualization_msgs wavemap wavemap_msgs) # Register catkin package catkin_package( INCLUDE_DIRS include LIBRARIES ${PROJECT_NAME} - CATKIN_DEPENDS roscpp eigen_conversions wavemap wavemap_msgs) + CATKIN_DEPENDS roscpp eigen_conversions visualization_msgs wavemap wavemap_msgs) # Libraries add_library(${PROJECT_NAME} diff --git a/interfaces/ros1/wavemap_ros_conversions/include/wavemap_ros_conversions/discrete_layer_marker_conversions.h b/interfaces/ros1/wavemap_ros_conversions/include/wavemap_ros_conversions/discrete_layer_marker_conversions.h new file mode 100644 index 000000000..f8439a083 --- /dev/null +++ b/interfaces/ros1/wavemap_ros_conversions/include/wavemap_ros_conversions/discrete_layer_marker_conversions.h @@ -0,0 +1,160 @@ +#ifndef WAVEMAP_ROS_CONVERSIONS_DISCRETE_LAYER_MARKER_CONVERSIONS_H_ +#define WAVEMAP_ROS_CONVERSIONS_DISCRETE_LAYER_MARKER_CONVERSIONS_H_ + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include + +namespace layered_map_viz { +namespace detail { +template +inline constexpr bool kAlwaysFalse = false; + +template +bool forEachTupleElementImpl(TupleT&& tuple, FuncT&& func, + std::index_sequence) { + bool ok = true; + ((ok = ok && func(std::get(tuple))), ...); + return ok; +} + +template +bool forEachTupleElement(TupleT&& tuple, FuncT&& func) { + constexpr size_t tuple_size = + std::tuple_size_v>; + return forEachTupleElementImpl(std::forward(tuple), + std::forward(func), + std::make_index_sequence{}); +} +} // namespace detail + +inline std_msgs::ColorRGBA color(float r, float g, float b, float a = 1.f) { + std_msgs::ColorRGBA color; + color.r = r; + color.g = g; + color.b = b; + color.a = a; + return color; +} + +struct LayerMarkerConfig { + std::string topic; + std::string ns; + float marker_scale = 0.1f; +}; + +template +struct NamedDiscreteMarkerLayer { + std::string name; + const LayerT& layer; + ColorPolicyT color_policy; + LayerMarkerConfig marker_config; +}; + +template +NamedDiscreteMarkerLayer namedDiscreteMarkerLayer( + std::string name, const LayerT& layer, ColorPolicyT color_policy, + LayerMarkerConfig marker_config = {}) { + if (marker_config.topic.empty()) { + marker_config.topic = "/wavemap/discrete/" + name; + } + if (marker_config.ns.empty()) { + marker_config.ns = name; + } + return {std::move(name), layer, std::move(color_policy), + std::move(marker_config)}; +} + +template +struct DiscreteLayerMarkerTraits { + static auto layers(const DiscreteLayersT&) { + static_assert(detail::kAlwaysFalse, + "DiscreteLayerMarkerTraits must be specialized for this " + "discrete layer bundle type."); + } +}; + +template +bool discreteLayerToMarkerArray(const std::string& frame_id, + const ros::Time& stamp, + const wavemap::layered::DiscreteLayer& layer, + const ColorPolicyT& color_policy, + const LayerMarkerConfig& config, + visualization_msgs::MarkerArray& msg) { + msg.markers.clear(); + + visualization_msgs::Marker marker; + marker.header.frame_id = frame_id; + marker.header.stamp = stamp; + marker.ns = config.ns; + marker.id = 0; + marker.type = visualization_msgs::Marker::CUBE_LIST; + marker.action = visualization_msgs::Marker::ADD; + marker.pose.orientation.w = 1.0; + marker.scale.x = config.marker_scale; + marker.scale.y = config.marker_scale; + marker.scale.z = config.marker_scale; + + for (const auto& [parent_key, cell] : layer.cells()) { + for (const int offset : cell.observed_offsets) { + const auto exception_it = cell.exceptions.find(offset); + const ValueT value = exception_it != cell.exceptions.end() + ? exception_it->second + : cell.dominant_value; + + std_msgs::ColorRGBA voxel_color; + if (!color_policy(value, voxel_color)) { + continue; + } + + const int side_length = layer.config().blockSideLength(); + const int local_x = offset % side_length; + const int local_y = (offset / side_length) % side_length; + const int local_z = offset / (side_length * side_length); + + geometry_msgs::Point point; + point.x = (parent_key.x * side_length + local_x) * config.marker_scale; + point.y = (parent_key.y * side_length + local_y) * config.marker_scale; + point.z = (parent_key.z * side_length + local_z) * config.marker_scale; + marker.points.emplace_back(point); + marker.colors.emplace_back(voxel_color); + } + } + + if (!marker.points.empty()) { + msg.markers.emplace_back(std::move(marker)); + } + return true; +} + +template +bool publishDiscreteLayerMarkers(const DiscreteLayersT& layers, + const std::string& frame_id, + const ros::Time& stamp, + PublisherFactoryT&& publisher_factory) { + const auto named_layers = DiscreteLayerMarkerTraits::layers(layers); + return detail::forEachTupleElement(named_layers, [&](const auto& named_layer) { + visualization_msgs::MarkerArray marker_msg; + if (!discreteLayerToMarkerArray(frame_id, stamp, named_layer.layer, + named_layer.color_policy, + named_layer.marker_config, marker_msg)) { + return false; + } + publisher_factory(named_layer.marker_config.topic).publish(marker_msg); + return true; + }); +} +} // namespace layered_map_viz + +#endif // WAVEMAP_ROS_CONVERSIONS_DISCRETE_LAYER_MARKER_CONVERSIONS_H_ diff --git a/interfaces/ros1/wavemap_ros_conversions/include/wavemap_ros_conversions/layered_map_msg_conversions.h b/interfaces/ros1/wavemap_ros_conversions/include/wavemap_ros_conversions/layered_map_msg_conversions.h new file mode 100644 index 000000000..98ec6603d --- /dev/null +++ b/interfaces/ros1/wavemap_ros_conversions/include/wavemap_ros_conversions/layered_map_msg_conversions.h @@ -0,0 +1,309 @@ +#ifndef WAVEMAP_ROS_CONVERSIONS_LAYERED_MAP_MSG_CONVERSIONS_H_ +#define WAVEMAP_ROS_CONVERSIONS_LAYERED_MAP_MSG_CONVERSIONS_H_ + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +namespace wavemap::convert { +namespace detail { +template +inline constexpr bool kLayeredMapRosAlwaysFalse = false; + +template +inline wavemap_msgs::Index3D indexKeyToRosMsg(const IndexKeyT& key) { + wavemap_msgs::Index3D msg; + msg.x = key.x; + msg.y = key.y; + msg.z = key.z; + return msg; +} + +template +inline IndexKeyT rosMsgToIndexKey(const wavemap_msgs::Index3D& msg) { + return IndexKeyT(wavemap::Index3D(msg.x, msg.y, msg.z)); +} +} // namespace detail + +template +struct NamedDiscreteLayerForRos { + std::string name; + LayerT& layer; +}; + +template +NamedDiscreteLayerForRos namedDiscreteLayerForRos( + std::string name, LayerT& layer) { + return {std::move(name), layer}; +} + +template +struct DiscreteLayerBundleRosTraits { + static auto layers(DiscreteLayersT&) { + static_assert(detail::kLayeredMapRosAlwaysFalse, + "DiscreteLayerBundleRosTraits must be specialized for " + "this discrete layer bundle type."); + } +}; + +template +struct DiscreteRosValueConverter { + static const char* typeName() { + static_assert(detail::kLayeredMapRosAlwaysFalse, + "DiscreteRosValueConverter must be specialized for this " + "discrete layer value type."); + return "unsupported"; + } +}; + +template <> +struct DiscreteRosValueConverter { + static const char* typeName() { return "int"; } + + static void setDominantValue(wavemap_msgs::DiscreteLayerCell& msg, + int value) { + msg.dominant_int32 = value; + } + + static bool readDominantValue(const wavemap_msgs::DiscreteLayerCell& msg, + int& value) { + value = msg.dominant_int32; + return true; + } + + static void appendExceptionValue(wavemap_msgs::DiscreteLayerCell& msg, + int value) { + msg.exception_int32_values.emplace_back(value); + } + + static bool readExceptionValue(const wavemap_msgs::DiscreteLayerCell& msg, + size_t index, int& value) { + if (msg.exception_int32_values.size() <= index) { + return false; + } + value = msg.exception_int32_values[index]; + return true; + } + + static bool hasExpectedExceptionValueCount( + const wavemap_msgs::DiscreteLayerCell& msg) { + return msg.exception_int32_values.size() == msg.exception_offsets.size(); + } +}; + +template <> +struct DiscreteRosValueConverter { + static const char* typeName() { return "bool"; } + + static void setDominantValue(wavemap_msgs::DiscreteLayerCell& msg, + bool value) { + msg.dominant_uint8 = value ? 1u : 0u; + } + + static bool readDominantValue(const wavemap_msgs::DiscreteLayerCell& msg, + bool& value) { + value = msg.dominant_uint8 != 0u; + return true; + } + + static void appendExceptionValue(wavemap_msgs::DiscreteLayerCell& msg, + bool value) { + msg.exception_uint8_values.emplace_back(value ? 1u : 0u); + } + + static bool readExceptionValue(const wavemap_msgs::DiscreteLayerCell& msg, + size_t index, bool& value) { + if (msg.exception_uint8_values.size() <= index) { + return false; + } + value = msg.exception_uint8_values[index] != 0u; + return true; + } + + static bool hasExpectedExceptionValueCount( + const wavemap_msgs::DiscreteLayerCell& msg) { + return msg.exception_uint8_values.size() == msg.exception_offsets.size(); + } +}; + +template +void discreteLayerToRosMsg(const std::string& name, + const DiscreteLayerT& layer, + wavemap_msgs::DiscreteLayer& msg) { + using ValueT = typename DiscreteLayerT::Value; + + msg.name = name; + msg.value_type = DiscreteRosValueConverter::typeName(); + msg.block_height = layer.config().block_height; + msg.cells.clear(); + msg.cells.reserve(layer.cells().size()); + + for (const auto& [parent_key, cell] : layer.cells()) { + wavemap_msgs::DiscreteLayerCell cell_msg; + cell_msg.parent_index = detail::indexKeyToRosMsg(parent_key); + DiscreteRosValueConverter::setDominantValue( + cell_msg, cell.dominant_value); + + cell_msg.observed_offsets.reserve(cell.observed_offsets.size()); + for (const int offset : cell.observed_offsets) { + cell_msg.observed_offsets.emplace_back(offset); + } + + cell_msg.exception_offsets.reserve(cell.exceptions.size()); + for (const auto& [offset, value] : cell.exceptions) { + cell_msg.exception_offsets.emplace_back(offset); + DiscreteRosValueConverter::appendExceptionValue(cell_msg, value); + } + + msg.cells.emplace_back(std::move(cell_msg)); + } +} + +template +bool rosMsgToDiscreteLayer(const wavemap_msgs::DiscreteLayer& msg, + const std::string& expected_name, + DiscreteLayerT& layer) { + using ValueT = typename DiscreteLayerT::Value; + using CellsT = std::remove_cv_t>; + using IndexKeyT = typename CellsT::key_type; + using ConfigT = std::remove_cv_t>; + + if (msg.name != expected_name || + msg.value_type != DiscreteRosValueConverter::typeName()) { + return false; + } + + std::map cells; + for (const wavemap_msgs::DiscreteLayerCell& cell_msg : msg.cells) { + if (!DiscreteRosValueConverter::hasExpectedExceptionValueCount( + cell_msg)) { + return false; + } + + typename DiscreteLayerT::Cell cell; + if (!DiscreteRosValueConverter::readDominantValue( + cell_msg, cell.dominant_value)) { + return false; + } + + for (const int offset : cell_msg.observed_offsets) { + cell.observed_offsets.insert(offset); + } + + for (size_t exception_i = 0u; + exception_i < cell_msg.exception_offsets.size(); ++exception_i) { + ValueT value{}; + if (!DiscreteRosValueConverter::readExceptionValue( + cell_msg, exception_i, value)) { + return false; + } + cell.exceptions[cell_msg.exception_offsets[exception_i]] = value; + } + + cells[detail::rosMsgToIndexKey(cell_msg.parent_index)] = cell; + } + + layer.replaceCellsForLoad(ConfigT{msg.block_height}, std::move(cells)); + return true; +} + +template +bool forEachRosTupleElementImpl(TupleT&& tuple, FuncT&& func, + std::index_sequence) { + bool ok = true; + ((ok = ok && func(std::get(tuple))), ...); + return ok; +} + +template +bool forEachRosTupleElement(TupleT&& tuple, FuncT&& func) { + constexpr size_t tuple_size = + std::tuple_size_v>; + return forEachRosTupleElementImpl(std::forward(tuple), + std::forward(func), + std::make_index_sequence{}); +} + +template +bool discreteLayerBundleToRosMsg(const DiscreteLayersT& layers, + std::vector& msg) { + const auto named_layers = + DiscreteLayerBundleRosTraits::layers(layers); + msg.clear(); + msg.reserve(std::tuple_size_v>); + + return forEachRosTupleElement(named_layers, [&](const auto& named_layer) { + wavemap_msgs::DiscreteLayer layer_msg; + discreteLayerToRosMsg(named_layer.name, named_layer.layer, layer_msg); + msg.emplace_back(std::move(layer_msg)); + return true; + }); +} + +template +bool rosMsgToDiscreteLayerBundle( + const std::vector& msg, + DiscreteLayersT& layers) { + const auto named_layers = + DiscreteLayerBundleRosTraits::layers(layers); + constexpr size_t expected_layer_count = + std::tuple_size_v>; + if (msg.size() != expected_layer_count) { + return false; + } + + size_t layer_index = 0u; + return forEachRosTupleElement(named_layers, [&](auto named_layer) { + return rosMsgToDiscreteLayer(msg[layer_index++], named_layer.name, + named_layer.layer); + }); +} + +template +bool layeredMapToRosMsg(const LayeredMapT& map, const std::string& frame_id, + const ros::Time& stamp, + wavemap_msgs::LayeredMap& msg) { + msg.header.frame_id = frame_id; + msg.header.stamp = stamp; + if (!mapToRosMsg(map.continuousMap(), frame_id, + stamp, msg.continuous_map)) { + return false; + } + return discreteLayerBundleToRosMsg(map.discreteLayers(), + msg.discrete_layers); +} + +template +bool rosMsgToLayeredMap(const wavemap_msgs::LayeredMap& msg, + LayeredMapT& map) { + typename LayeredMapT::ContinuousMap::Ptr continuous_map; + if (!rosMsgToMap(msg.continuous_map, + continuous_map) || + !continuous_map) { + return false; + } + + typename LayeredMapT::DiscreteLayers discrete_layers; + if (!rosMsgToDiscreteLayerBundle(msg.discrete_layers, discrete_layers)) { + return false; + } + + map = LayeredMapT(std::move(continuous_map), std::move(discrete_layers)); + return true; +} +} // namespace wavemap::convert + +#endif // WAVEMAP_ROS_CONVERSIONS_LAYERED_MAP_MSG_CONVERSIONS_H_ diff --git a/interfaces/ros1/wavemap_ros_conversions/package.xml b/interfaces/ros1/wavemap_ros_conversions/package.xml index 9336c2bea..55c6b219c 100644 --- a/interfaces/ros1/wavemap_ros_conversions/package.xml +++ b/interfaces/ros1/wavemap_ros_conversions/package.xml @@ -17,4 +17,5 @@ roscpp eigen_conversions + visualization_msgs 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 d632b40c6..7730cd0e2 100644 --- a/interfaces/ros1/wavemap_rviz_plugin/src/wavemap_map_display.cc +++ b/interfaces/ros1/wavemap_rviz_plugin/src/wavemap_map_display.cc @@ -27,7 +27,7 @@ struct LayeredVoxelColorProvider { wavemap::FloatingPoint /*occupancy*/, Ogre::ColourValue& color) { if (layer_name == "color") { - color = Ogre::ColourValue(voxel.data.r, voxel.data.g, voxel.data.b, 1.f); + color = Ogre::ColourValue(voxel.data.rgb.r, voxel.data.rgb.g, voxel.data.rgb.b, 1.f); return true; } diff --git a/library/cpp/CMakeLists.txt b/library/cpp/CMakeLists.txt index 1a94b0a8d..d5528d554 100644 --- a/library/cpp/CMakeLists.txt +++ b/library/cpp/CMakeLists.txt @@ -54,6 +54,10 @@ if (GENERATE_WAVEMAP_INSTALL_RULES) install(DIRECTORY ${PROJECT_SOURCE_DIR}/include/wavemap/3rd_party/ DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/wavemap/3rd_party FILES_MATCHING PATTERN "*.h") + # Mark layered map headers for installation + install(DIRECTORY ${PROJECT_SOURCE_DIR}/include/wavemap/layered/ + DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/wavemap/layered + FILES_MATCHING PATTERN "*.h") # Install the CMake exports file install(EXPORT wavemap-targets FILE wavemap-targets.cmake diff --git a/library/cpp/include/wavemap/core/integrator/projective/coarse_to_fine/hashed_wavelet_integrator.h b/library/cpp/include/wavemap/core/integrator/projective/coarse_to_fine/hashed_wavelet_integrator.h index aac70459a..2d216f2c4 100644 --- a/library/cpp/include/wavemap/core/integrator/projective/coarse_to_fine/hashed_wavelet_integrator.h +++ b/library/cpp/include/wavemap/core/integrator/projective/coarse_to_fine/hashed_wavelet_integrator.h @@ -1,6 +1,7 @@ #ifndef WAVEMAP_CORE_INTEGRATOR_PROJECTIVE_COARSE_TO_FINE_HASHED_WAVELET_INTEGRATOR_H_ #define WAVEMAP_CORE_INTEGRATOR_PROJECTIVE_COARSE_TO_FINE_HASHED_WAVELET_INTEGRATOR_H_ +#include #include #include #include @@ -12,15 +13,20 @@ #include "wavemap/core/utils/thread_pool.h" namespace wavemap { -class HashedWaveletIntegrator : public ProjectiveIntegrator { +template +class HashedWaveletIntegratorT : public ProjectiveIntegrator { public: - HashedWaveletIntegrator(const ProjectiveIntegratorConfig& config, - ProjectorBase::ConstPtr projection_model, - PosedImage<>::Ptr posed_range_image, - Image::Ptr beam_offset_image, - MeasurementModelBase::ConstPtr measurement_model, - HashedWaveletOctree::Ptr occupancy_map, - std::shared_ptr thread_pool = nullptr) + using MapType = HashedWaveletOctreeT; + using MapPtr = typename MapType::Ptr; + + HashedWaveletIntegratorT( + const ProjectiveIntegratorConfig& config, + ProjectorBase::ConstPtr projection_model, + PosedImage<>::Ptr posed_range_image, + Image::Ptr beam_offset_image, + MeasurementModelBase::ConstPtr measurement_model, + MapPtr occupancy_map, + std::shared_ptr thread_pool = nullptr) : ProjectiveIntegrator( config, std::move(projection_model), std::move(posed_range_image), std::move(beam_offset_image), std::move(measurement_model)), @@ -29,10 +35,13 @@ class HashedWaveletIntegrator : public ProjectiveIntegrator { : std::make_shared()) {} private: - using BlockList = std::vector; - using OctreeType = HashedWaveletOctreeBlock::OctreeType; + using BlockList = std::vector; + using Block = typename MapType::Block; + using BlockIndex = typename MapType::BlockIndex; + using OctreeType = typename Block::OctreeType; + using Coefficients = typename Block::Coefficients; - const HashedWaveletOctree::Ptr occupancy_map_; + const MapPtr occupancy_map_; const std::shared_ptr thread_pool_; std::shared_ptr range_image_intersector_; @@ -51,6 +60,18 @@ class HashedWaveletIntegrator : public ProjectiveIntegrator { static constexpr auto kUnitCubeHalfDiagonal = constants::kSqrt3 / 2.f; + static FloatingPoint occupancyOf(const CellDataT& value) { + return static_cast(value); + } + + static void addClampedOccupancyUpdate(CellDataT& value, + FloatingPoint update, + FloatingPoint min_log_odds, + FloatingPoint max_log_odds) { + value += update; + value = std::clamp(occupancyOf(value), min_log_odds, max_log_odds); + } + std::pair getFovMinMaxIndices( const Point3D& sensor_origin) const; @@ -58,9 +79,10 @@ class HashedWaveletIntegrator : public ProjectiveIntegrator { BlockList& update_job_list); void updateMap() override; - void updateBlock(HashedWaveletOctree::Block& block, - const HashedWaveletOctree::BlockIndex& block_index); + void updateBlock(Block& block, const BlockIndex& block_index); }; + +using HashedWaveletIntegrator = HashedWaveletIntegratorT; } // namespace wavemap #include "wavemap/core/integrator/projective/coarse_to_fine/impl/hashed_wavelet_integrator_inl.h" diff --git a/library/cpp/include/wavemap/core/integrator/projective/coarse_to_fine/impl/hashed_wavelet_integrator_inl.h b/library/cpp/include/wavemap/core/integrator/projective/coarse_to_fine/impl/hashed_wavelet_integrator_inl.h index 279c90099..e95abb00e 100644 --- a/library/cpp/include/wavemap/core/integrator/projective/coarse_to_fine/impl/hashed_wavelet_integrator_inl.h +++ b/library/cpp/include/wavemap/core/integrator/projective/coarse_to_fine/impl/hashed_wavelet_integrator_inl.h @@ -1,12 +1,79 @@ #ifndef WAVEMAP_CORE_INTEGRATOR_PROJECTIVE_COARSE_TO_FINE_IMPL_HASHED_WAVELET_INTEGRATOR_INL_H_ #define WAVEMAP_CORE_INTEGRATOR_PROJECTIVE_COARSE_TO_FINE_IMPL_HASHED_WAVELET_INTEGRATOR_INL_H_ +#include +#include #include +#include + namespace wavemap { -inline void HashedWaveletIntegrator::recursiveTester( // NOLINT +template +void HashedWaveletIntegratorT::updateMap() { + ProfilerZoneScoped; + // Update the range image intersector + { + ProfilerZoneScopedN("updateRangeImageIntersector"); + range_image_intersector_ = std::make_shared( + posed_range_image_, projection_model_, *measurement_model_, + config_.min_range, config_.max_range); + } + + // Find all the indices of blocks that need updating + BlockList blocks_to_update; + { + ProfilerZoneScopedN("selectBlocksToUpdate"); + const auto [fov_min_idx, fov_max_idx] = + getFovMinMaxIndices(posed_range_image_->getOrigin()); + for (const auto& block_index : + Grid(fov_min_idx.position, fov_max_idx.position)) { + recursiveTester(OctreeIndex{fov_min_idx.height, block_index}, + blocks_to_update); + } + } + + // Make sure the to-be-updated blocks are allocated + for (const auto& block_index : blocks_to_update) { + occupancy_map_->getOrAllocateBlock(block_index); + } + + // Update it with the threadpool + for (const auto& block_index : blocks_to_update) { + thread_pool_->add_task([this, block_index]() { + if (auto* block = occupancy_map_->getBlock(block_index); block) { + updateBlock(*block, block_index); + } + }); + } + thread_pool_->wait_all(); +} + +template +std::pair +HashedWaveletIntegratorT::getFovMinMaxIndices( + const Point3D& sensor_origin) const { + const int height = 1 + std::max(static_cast(std::ceil(std::log2( + config_.max_range / min_cell_width_))), + tree_height_); + const OctreeIndex fov_min_idx = convert::indexAndHeightToNodeIndex<3>( + convert::pointToFloorIndex<3>( + sensor_origin - Vector3D::Constant(config_.max_range), + min_cell_width_inv_) - + occupancy_map_->getBlockSize(), + height); + const OctreeIndex fov_max_idx = convert::indexAndHeightToNodeIndex<3>( + convert::pointToCeilIndex<3>( + sensor_origin + Vector3D::Constant(config_.max_range), + min_cell_width_inv_) + + occupancy_map_->getBlockSize(), + height); + return {fov_min_idx, fov_max_idx}; +} + +template +void HashedWaveletIntegratorT::recursiveTester( // NOLINT const OctreeIndex& node_index, - HashedWaveletIntegrator::BlockList& update_job_list) { + HashedWaveletIntegratorT::BlockList& update_job_list) { const AABB block_aabb = convert::nodeIndexToAABB(node_index, min_cell_width_); const UpdateType update_type = range_image_intersector_->determineUpdateType( @@ -24,7 +91,8 @@ inline void HashedWaveletIntegrator::recursiveTester( // NOLINT } if (const auto* block = occupancy_map_->getBlock(node_index.position); block) { - if (min_log_odds_ + kNoiseThreshold / 10.f <= block->getRootScale()) { + if (min_log_odds_ + kNoiseThreshold / 10.f <= + occupancyOf(block->getRootScale())) { // Add the block to the job list update_job_list.emplace_back(node_index.position); } @@ -36,6 +104,136 @@ inline void HashedWaveletIntegrator::recursiveTester( // NOLINT recursiveTester(child_index, update_job_list); } } + +template +void HashedWaveletIntegratorT::updateBlock( + Block& block, const BlockIndex& block_index) { + ProfilerZoneScoped; + block.setNeedsPruning(); + block.setLastUpdatedStamp(); + + struct StackElement { + typename OctreeType::NodeRefType parent_node; + const OctreeIndex parent_node_index; + NdtreeIndexRelativeChild next_child_idx; + typename Coefficients::CoefficientsArray child_scale_coefficients; + }; + std::stack stack; + + typename OctreeType::NodeRefType root_node = block.getRootNode(); + typename Coefficients::Scale& root_node_scale = block.getRootScale(); + stack.emplace(StackElement{root_node, + {tree_height_, block_index}, + 0, + Block::Transform::backward( + {root_node_scale, root_node.data()})}); + + while (!stack.empty()) { + // If the current stack element has fully been processed, propagate upward + if (OctreeIndex::kNumChildren <= stack.top().next_child_idx) { + const auto [scale, details] = + Block::Transform::forward(stack.top().child_scale_coefficients); + stack.top().parent_node.data() = details; + stack.pop(); + if (stack.empty()) { + root_node_scale = scale; + return; + } else { + const NdtreeIndexRelativeChild current_child_idx = + stack.top().next_child_idx - 1; + stack.top().child_scale_coefficients[current_child_idx] = scale; + continue; + } + } + + // Evaluate stack element's active child + const NdtreeIndexRelativeChild current_child_idx = + stack.top().next_child_idx; + ++stack.top().next_child_idx; + DCHECK_GE(current_child_idx, 0); + DCHECK_LT(current_child_idx, OctreeIndex::kNumChildren); + + typename OctreeType::NodeRefType parent_node = stack.top().parent_node; + CellDataT& node_value = + stack.top().child_scale_coefficients[current_child_idx]; + const OctreeIndex node_index = + stack.top().parent_node_index.computeChildIndex(current_child_idx); + DCHECK_GE(node_index.height, 0); + + // If we're at the leaf level, directly update the node + if (node_index.height <= termination_height_) { + const Point3D W_node_center = + convert::nodeIndexToCenterPoint(node_index, min_cell_width_); + const Point3D C_node_center = + posed_range_image_->getPoseInverse() * W_node_center; + const FloatingPoint sample = computeUpdate(C_node_center); + addClampedOccupancyUpdate(node_value, sample, + min_log_odds_ - kNoiseThreshold, + max_log_odds_ + kNoiseThreshold); + continue; + } + + // Otherwise, test whether the current node is fully occupied; + // free or unknown; or fully unknown + const AABB W_cell_aabb = + convert::nodeIndexToAABB(node_index, min_cell_width_); + const UpdateType update_type = + range_image_intersector_->determineUpdateType( + W_cell_aabb, posed_range_image_->getRotationMatrixInverse(), + posed_range_image_->getOrigin()); + + // If we're fully in unknown space, + // there's no need to evaluate this node or its children + if (update_type == UpdateType::kFullyUnobserved) { + continue; + } + + // We can also stop here if the cell will result in a free space update + // (or zero) and the map is already saturated free + if (update_type != UpdateType::kPossiblyOccupied && + occupancyOf(node_value) < min_log_odds_ + kNoiseThreshold / 10.f) { + continue; + } + + // Test if the worst-case error for the intersection type at the current + // resolution falls within the acceptable approximation error + const FloatingPoint node_width = W_cell_aabb.width<0>(); + const Point3D W_node_center = + W_cell_aabb.min + Vector3D::Constant(node_width / 2.f); + const Point3D C_node_center = + posed_range_image_->getPoseInverse() * W_node_center; + const FloatingPoint d_C_cell = + projection_model_->cartesianToSensorZ(C_node_center); + const FloatingPoint bounding_sphere_radius = + kUnitCubeHalfDiagonal * node_width; + typename OctreeType::NodePtrType node = + parent_node.getChild(node_index.computeRelativeChildIndex()); + if (measurement_model_->computeWorstCaseApproximationError( + update_type, d_C_cell, bounding_sphere_radius) < + config_.termination_update_error) { + const FloatingPoint sample = computeUpdate(C_node_center); + if (!node || !node->hasAtLeastOneChild()) { + addClampedOccupancyUpdate(node_value, sample, + min_log_odds_ - kNoiseThreshold, + max_log_odds_ + kNoiseThreshold); + } else { + node_value += sample; + block.setNeedsThresholding(); + } + continue; + } + + // Since the approximation error would still be too big, refine + if (!node) { + // Allocate the current node if it has not yet been allocated + node = &parent_node.getOrAllocateChild( + node_index.computeRelativeChildIndex()); + } + stack.emplace(StackElement{*node, node_index, 0, + Block::Transform::backward( + {node_value, node->data()})}); + } +} } // namespace wavemap #endif // WAVEMAP_CORE_INTEGRATOR_PROJECTIVE_COARSE_TO_FINE_IMPL_HASHED_WAVELET_INTEGRATOR_INL_H_ diff --git a/library/cpp/include/wavemap/core/map/cell_types/cell_data_traits.h b/library/cpp/include/wavemap/core/map/cell_types/cell_data_traits.h new file mode 100644 index 000000000..bf0a5149e --- /dev/null +++ b/library/cpp/include/wavemap/core/map/cell_types/cell_data_traits.h @@ -0,0 +1,134 @@ +#ifndef WAVEMAP_CORE_MAP_CELL_TYPES_CELL_DATA_TRAITS_H_ +#define WAVEMAP_CORE_MAP_CELL_TYPES_CELL_DATA_TRAITS_H_ + +#include +#include +#include + +#include "wavemap/core/common.h" +#include "wavemap/core/map/cell_types/voxel_data.h" + +namespace wavemap { +namespace detail { +template +inline constexpr bool kAlwaysFalse = false; +} // namespace detail + +struct EmptyCellDataConfig {}; + +struct FieldPruningConfig { + FloatingPoint scale = 1e-3f; + FloatingPoint weight = 1.f; +}; + +inline FloatingPoint weightedPruningScore( + FloatingPoint magnitude, const FieldPruningConfig& config) { + if (config.weight <= 0.f || magnitude <= 0.f) { + return 0.f; + } + if (config.scale <= 0.f) { + return std::numeric_limits::infinity(); + } + return config.weight * magnitude / config.scale; +} + +template +struct CellDataTraits { + using ThresholdConfig = EmptyCellDataConfig; + using PruningConfig = EmptyCellDataConfig; + + static void threshold(CellDataT&, const ThresholdConfig&) { + static_assert(detail::kAlwaysFalse, + "CellDataTraits::threshold must be specialized for this " + "custom cell data type."); + } + + static FloatingPoint pruningScore(const CellDataT&, const PruningConfig&) { + static_assert(detail::kAlwaysFalse, + "CellDataTraits::pruningScore must be specialized for " + "this custom cell data type."); + return 0.f; + } + + static bool isNonzero(const CellDataT& value, const PruningConfig& config) { + return 1.f < pruningScore(value, config); + } +}; + +template <> +struct CellDataTraits { + using ThresholdConfig = EmptyCellDataConfig; + using PruningConfig = FieldPruningConfig; + + static void threshold(FloatingPoint&, const ThresholdConfig&) {} + + static FloatingPoint pruningScore(FloatingPoint value, + const PruningConfig& config) { + return weightedPruningScore(std::abs(value), config); + } + + static bool isNonzero(FloatingPoint value, const PruningConfig& config) { + return 1.f < pruningScore(value, config); + } +}; + +template +struct CellDataTraits> { + using AdditionalTraits = CellDataTraits; + + struct ThresholdConfig { + typename AdditionalTraits::ThresholdConfig data{}; + }; + + struct PruningConfig { + FieldPruningConfig occupancy{}; + typename AdditionalTraits::PruningConfig data{}; + FloatingPoint combined_threshold = 1.f; + }; + + static void threshold( + VoxelData& voxel, + const ThresholdConfig& config) { + AdditionalTraits::threshold(voxel.data, config.data); + } + + static FloatingPoint pruningScore( + const VoxelData& voxel, + const PruningConfig& config) { + return weightedPruningScore(std::abs(voxel.occupancy), config.occupancy) + + AdditionalTraits::pruningScore(voxel.data, config.data); + } + + static bool isNonzero( + const VoxelData& voxel, + const PruningConfig& config) { + return config.combined_threshold < pruningScore(voxel, config); + } +}; + +template +struct CellDataTraits> { + using ThresholdConfig = EmptyCellDataConfig; + + struct PruningConfig { + FieldPruningConfig occupancy{}; + FloatingPoint combined_threshold = 1.f; + }; + + static void threshold(VoxelData&, + const ThresholdConfig&) {} + + static FloatingPoint pruningScore( + const VoxelData& voxel, + const PruningConfig& config) { + return weightedPruningScore(std::abs(voxel.occupancy), config.occupancy); + } + + static bool isNonzero(const VoxelData& voxel, + const PruningConfig& config) { + return config.combined_threshold < pruningScore(voxel, config); + } +}; +} // namespace wavemap + +#endif // WAVEMAP_CORE_MAP_CELL_TYPES_CELL_DATA_TRAITS_H_ 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 099ecc692..4992ed1f6 100644 --- a/library/cpp/include/wavemap/core/map/hashed_wavelet_octree.h +++ b/library/cpp/include/wavemap/core/map/hashed_wavelet_octree.h @@ -70,9 +70,18 @@ class HashedWaveletOctreeT : public MapBase { using CellIndex = OctreeIndex; using Block = HashedWaveletOctreeBlockT; using BlockHashMap = SpatialHash; - - explicit HashedWaveletOctreeT(const HashedWaveletOctreeConfig& config) - : MapBase(config), config_(config.checkValid()) {} + using Traits = CellDataTraits; + using ThresholdConfig = typename Traits::ThresholdConfig; + using PruningConfig = typename Traits::PruningConfig; + + explicit HashedWaveletOctreeT( + const HashedWaveletOctreeConfig& config, + ThresholdConfig threshold_config = ThresholdConfig{}, + PruningConfig pruning_config = PruningConfig{}) + : MapBase(config), + config_(config.checkValid()), + threshold_config_(threshold_config), + pruning_config_(pruning_config) {} // Copy construction is not supported HashedWaveletOctreeT(const HashedWaveletOctreeT&) = delete; @@ -143,6 +152,8 @@ class HashedWaveletOctreeT : public MapBase { const HashedWaveletOctreeConfig config_; const IndexElement cells_per_block_side_ = int_math::exp2(config_.tree_height); + const ThresholdConfig threshold_config_; + const PruningConfig pruning_config_; BlockHashMap block_map_; }; diff --git a/library/cpp/include/wavemap/core/map/hashed_wavelet_octree_block.h b/library/cpp/include/wavemap/core/map/hashed_wavelet_octree_block.h index f413b22dc..fc5a77c50 100644 --- a/library/cpp/include/wavemap/core/map/hashed_wavelet_octree_block.h +++ b/library/cpp/include/wavemap/core/map/hashed_wavelet_octree_block.h @@ -3,6 +3,7 @@ #include "wavemap/core/common.h" #include "wavemap/core/data_structure/ndtree/ndtree.h" +#include "wavemap/core/map/cell_types/cell_data_traits.h" #include "wavemap/core/map/cell_types/haar_coefficients.h" #include "wavemap/core/map/cell_types/haar_transform.h" #include "wavemap/core/map/cell_types/voxel_data.h" @@ -19,13 +20,20 @@ class HashedWaveletOctreeBlockT { using Coefficients = HaarCoefficients; using Transform = HaarTransform; using OctreeType = Octree; - - explicit HashedWaveletOctreeBlockT(IndexElement tree_height, - FloatingPoint min_log_odds, - FloatingPoint max_log_odds) + using Traits = CellDataTraits; + using ThresholdConfig = typename Traits::ThresholdConfig; + using PruningConfig = typename Traits::PruningConfig; + + explicit HashedWaveletOctreeBlockT( + IndexElement tree_height, FloatingPoint min_log_odds, + FloatingPoint max_log_odds, + ThresholdConfig threshold_config = ThresholdConfig{}, + PruningConfig pruning_config = PruningConfig{}) : tree_height_(tree_height), min_log_odds_(min_log_odds), - max_log_odds_(max_log_odds) {} + max_log_odds_(max_log_odds), + threshold_config_(threshold_config), + pruning_config_(pruning_config) {} bool empty() const; size_t size() const { return ndtree_.size(); } @@ -87,6 +95,8 @@ class HashedWaveletOctreeBlockT { const IndexElement tree_height_; const FloatingPoint min_log_odds_; const FloatingPoint max_log_odds_; + const ThresholdConfig threshold_config_; + const PruningConfig pruning_config_; OctreeType ndtree_{tree_height_ - 1}; typename Coefficients::Scale root_scale_coefficient_{}; diff --git a/library/cpp/include/wavemap/core/map/impl/hashed_wavelet_octree_block_inl.h b/library/cpp/include/wavemap/core/map/impl/hashed_wavelet_octree_block_inl.h index 22390219d..fa05876db 100644 --- a/library/cpp/include/wavemap/core/map/impl/hashed_wavelet_octree_block_inl.h +++ b/library/cpp/include/wavemap/core/map/impl/hashed_wavelet_octree_block_inl.h @@ -3,6 +3,8 @@ #include #include +#include +#include #include #include "wavemap/core/utils/profile/profiler_interface.h" @@ -19,6 +21,50 @@ template void setOccupancy(CellDataT& value, FloatingPoint occupancy) { value = occupancy; } + +template +struct HasVoxelDataFields : std::false_type {}; + +template +struct HasVoxelDataFields< + T, std::void_t().occupancy), + decltype(std::declval().data)>> : std::true_type {}; + +template +struct HasOccupancyField : std::false_type {}; + +template +struct HasOccupancyField().occupancy)>> + : std::true_type {}; + +template +void thresholdCellData(CellDataT& value, FloatingPoint min_log_odds, + FloatingPoint max_log_odds, + const typename CellDataTraits::ThresholdConfig& + threshold_config) { + if constexpr (std::is_arithmetic_v) { + value = std::clamp(value, min_log_odds, max_log_odds); + } else { + if constexpr (HasVoxelDataFields::value) { + value.occupancy = std::clamp(value.occupancy, min_log_odds, max_log_odds); + } else if constexpr (HasOccupancyField::value) { + value.occupancy = std::clamp(value.occupancy, min_log_odds, max_log_odds); + } + CellDataTraits::threshold(value, threshold_config); + } +} + +template +bool detailsHaveNonzeroData( + const typename HaarCoefficients::Details& details, + const typename CellDataTraits::PruningConfig& pruning_config) { + return std::any_of(details.cbegin(), details.cend(), + [&pruning_config](const CellDataT& value) { + return CellDataTraits::isNonzero( + value, pruning_config); + }); +} + } // namespace detail template @@ -245,9 +291,8 @@ void HashedWaveletOctreeBlockT::recursiveThreshold( if (auto child_node = node.getChild(child_idx); child_node) { recursiveThreshold(*child_node, child_scale); } else { - detail::setOccupancy( - child_scale, std::clamp(detail::getOccupancy(child_scale), - min_log_odds_, max_log_odds_)); + detail::thresholdCellData(child_scale, min_log_odds_, max_log_odds_, + threshold_config_); } } @@ -267,7 +312,8 @@ void HashedWaveletOctreeBlockT::recursivePrune( child_node) { recursivePrune(*child_node); if (!child_node->hasChildrenArray() && - !child_node->hasNonzeroData(1e-3f)) { + !detail::detailsHaveNonzeroData(child_node->data(), + pruning_config_)) { node.eraseChild(child_idx); } else { has_at_least_one_child = true; diff --git a/library/cpp/include/wavemap/core/map/impl/hashed_wavelet_octree_inl.h b/library/cpp/include/wavemap/core/map/impl/hashed_wavelet_octree_inl.h index 7564a6322..0526b0fa2 100644 --- a/library/cpp/include/wavemap/core/map/impl/hashed_wavelet_octree_inl.h +++ b/library/cpp/include/wavemap/core/map/impl/hashed_wavelet_octree_inl.h @@ -194,9 +194,9 @@ template typename HashedWaveletOctreeT::Block& HashedWaveletOctreeT::getOrAllocateBlock( const Index3D& block_index) { - return block_map_.getOrAllocateBlock(block_index, config_.tree_height, - config_.min_log_odds, - config_.max_log_odds); + return block_map_.getOrAllocateBlock( + block_index, config_.tree_height, config_.min_log_odds, + config_.max_log_odds, threshold_config_, pruning_config_); } template diff --git a/library/cpp/include/wavemap/core/utils/data/comparisons.h b/library/cpp/include/wavemap/core/utils/data/comparisons.h index 791aa8ebc..9e5c87a72 100644 --- a/library/cpp/include/wavemap/core/utils/data/comparisons.h +++ b/library/cpp/include/wavemap/core/utils/data/comparisons.h @@ -6,27 +6,32 @@ #include #include "wavemap/core/common.h" +#include "wavemap/core/map/cell_types/cell_data_traits.h" namespace wavemap::data { -// Test strict equality to zero +// Test strict equality to zero. template bool is_nonzero(const T& data) { return data != T{}; } -// Test equality to zero with a tolerance (for POD types) +// Test equality to zero with a tolerance for scalar arithmetic types. template , bool> = true> bool is_nonzero(const DataT& data, ThresholdT threshold) { return threshold < std::abs(data); } -// Test element-wise equality to zero with a tolerance (for array types) +// Test element-wise equality to zero with the element type's default pruning +// config. Code paths that need custom thresholds should call CellDataTraits +// directly with an explicit PruningConfig. template -bool is_nonzero(const DataT& data, ThresholdT threshold) { - return std::any_of(data.cbegin(), data.cend(), [threshold](auto value) { - return threshold < std::abs(value); +bool is_nonzero(const DataT& data, ThresholdT /*threshold*/) { + using ElementT = std::decay_t; + const typename CellDataTraits::PruningConfig pruning_config{}; + return std::any_of(data.cbegin(), data.cend(), [&pruning_config](const auto& value) { + return CellDataTraits::isNonzero(value, pruning_config); }); } diff --git a/library/cpp/include/wavemap/layered/discrete_layer.h b/library/cpp/include/wavemap/layered/discrete_layer.h new file mode 100644 index 000000000..e33456110 --- /dev/null +++ b/library/cpp/include/wavemap/layered/discrete_layer.h @@ -0,0 +1,250 @@ +#ifndef WAVEMAP_LAYERED_DISCRETE_LAYER_H_ +#define WAVEMAP_LAYERED_DISCRETE_LAYER_H_ + +#include +#include +#include +#include +#include + +#include + +namespace wavemap::layered { + +struct IndexKey { + int x = 0; + int y = 0; + int z = 0; + + explicit IndexKey(const wavemap::Index3D& index) + : x(index.x()), y(index.y()), z(index.z()) {} + + bool operator<(const IndexKey& other) const { + return std::tie(x, y, z) < std::tie(other.x, other.y, other.z); + } +}; + +inline wavemap::Index3D indexFromKey(const IndexKey& key) { + return wavemap::Index3D(key.x, key.y, key.z); +} + +struct DiscreteCompressionConfig { + // Matches one octree level by default: one parent represents 2x2x2 children. + int block_height = 1; + + int blockSideLength() const { return 1 << block_height; } + + int blockVoxelCount() const { + const int side_length = blockSideLength(); + return side_length * side_length * side_length; + } +}; + +inline int floorDiv(int value, int divisor) { + int quotient = value / divisor; + const int remainder = value % divisor; + if (remainder != 0 && ((remainder < 0) != (divisor < 0))) { + --quotient; + } + return quotient; +} + +inline int positiveModulo(int value, int divisor) { + const int remainder = value % divisor; + return remainder < 0 ? remainder + divisor : remainder; +} + +inline IndexKey parentKey(const wavemap::Index3D& index, + const DiscreteCompressionConfig& config) { + const int side_length = config.blockSideLength(); + return IndexKey(wavemap::Index3D(floorDiv(index.x(), side_length), + floorDiv(index.y(), side_length), + floorDiv(index.z(), side_length))); +} + +inline int childOffset(const wavemap::Index3D& index, + const DiscreteCompressionConfig& config) { + const int side_length = config.blockSideLength(); + const int x = positiveModulo(index.x(), side_length); + const int y = positiveModulo(index.y(), side_length); + const int z = positiveModulo(index.z(), side_length); + return x + side_length * (y + side_length * z); +} + +template +class RawDiscreteLayer { + public: + using Value = ValueT; + + void setValue(const wavemap::Index3D& index, const ValueT& value) { + values_[IndexKey(index)] = value; + } + + std::optional getValue(const wavemap::Index3D& index) const { + const auto it = values_.find(IndexKey(index)); + if (it == values_.end()) { + return std::nullopt; + } + return it->second; + } + + bool hasValue(const wavemap::Index3D& index) const { + return values_.find(IndexKey(index)) != values_.end(); + } + + size_t size() const { return values_.size(); } + + void clear() { values_.clear(); } + + const std::map& values() const { return values_; } + + private: + std::map values_; +}; + +template +struct CompressedDiscreteCell { + ValueT dominant_value{}; + std::set observed_offsets; + std::map exceptions; +}; + +template +class DiscreteLayer { + public: + using Value = ValueT; + using Cell = CompressedDiscreteCell; + + explicit DiscreteLayer( + DiscreteCompressionConfig config = DiscreteCompressionConfig()) + : config_(config) {} + + static DiscreteLayer fromRaw( + const RawDiscreteLayer& layer, + DiscreteCompressionConfig config = DiscreteCompressionConfig()) { + std::map> grouped_values; + for (const auto& [key, value] : layer.values()) { + const wavemap::Index3D index = indexFromKey(key); + grouped_values[parentKey(index, config)][childOffset(index, config)] = + value; + } + + DiscreteLayer compressed(config); + for (const auto& [parent_key, child_values] : grouped_values) { + Cell cell; + compressed.rebuildCell(child_values, cell); + compressed.cells_[parent_key] = cell; + } + return compressed; + } + + static DiscreteLayer fromRaw2x2x2( + const RawDiscreteLayer& layer) { + return fromRaw(layer, DiscreteCompressionConfig{1}); + } + + void setValue(const wavemap::Index3D& index, const ValueT& value) { + Cell& cell = cells_[parentKey(index, config_)]; + std::map values = expandCell(cell); + values[childOffset(index, config_)] = value; + rebuildCell(values, cell); + } + + std::optional getValue(const wavemap::Index3D& index) const { + const auto cell_it = cells_.find(parentKey(index, config_)); + if (cell_it == cells_.end()) { + return std::nullopt; + } + const Cell& cell = cell_it->second; + const int offset = childOffset(index, config_); + if (cell.observed_offsets.find(offset) == cell.observed_offsets.end()) { + return std::nullopt; + } + const auto exception_it = cell.exceptions.find(offset); + if (exception_it != cell.exceptions.end()) { + return exception_it->second; + } + return cell.dominant_value; + } + + size_t parentCount() const { return cells_.size(); } + + size_t exceptionCount() const { + size_t count = 0u; + for (const auto& [parent_key, cell] : cells_) { + (void)parent_key; + count += cell.exceptions.size(); + } + return count; + } + + size_t observedValueCount() const { + size_t count = 0u; + for (const auto& [parent_key, cell] : cells_) { + (void)parent_key; + count += cell.observed_offsets.size(); + } + return count; + } + + std::optional dominantValue(const wavemap::Index3D& index) const { + const auto cell_it = cells_.find(parentKey(index, config_)); + if (cell_it == cells_.end()) { + return std::nullopt; + } + return cell_it->second.dominant_value; + } + + const DiscreteCompressionConfig& config() const { return config_; } + + const std::map& cells() const { return cells_; } + + void replaceCellsForLoad(DiscreteCompressionConfig config, + std::map cells) { + config_ = config; + cells_ = std::move(cells); + } + + private: + std::map expandCell(const Cell& cell) const { + std::map values; + for (const int offset : cell.observed_offsets) { + const auto exception_it = cell.exceptions.find(offset); + values[offset] = exception_it != cell.exceptions.end() + ? exception_it->second + : cell.dominant_value; + } + return values; + } + + void rebuildCell(const std::map& values, Cell& cell) const { + std::map counts; + ValueT dominant_value{}; + size_t dominant_count = 0u; + for (const auto& [offset, value] : values) { + (void)offset; + const size_t count = ++counts[value]; + if (dominant_count < count) { + dominant_value = value; + dominant_count = count; + } + } + + cell.dominant_value = dominant_value; + cell.observed_offsets.clear(); + cell.exceptions.clear(); + for (const auto& [offset, value] : values) { + cell.observed_offsets.insert(offset); + if (value != dominant_value) { + cell.exceptions[offset] = value; + } + } + } + + DiscreteCompressionConfig config_; + std::map cells_; +}; + +} // namespace wavemap::layered + +#endif // WAVEMAP_LAYERED_DISCRETE_LAYER_H_ diff --git a/library/cpp/include/wavemap/layered/layered_map.h b/library/cpp/include/wavemap/layered/layered_map.h new file mode 100644 index 000000000..595e3e263 --- /dev/null +++ b/library/cpp/include/wavemap/layered/layered_map.h @@ -0,0 +1,76 @@ +#ifndef WAVEMAP_LAYERED_LAYERED_MAP_H_ +#define WAVEMAP_LAYERED_LAYERED_MAP_H_ + +#include +#include + +#include +#include + +#include + +namespace wavemap::layered { + +template +using LayeredMapVoxel = + wavemap::VoxelData; + +template +using ContinuousWaveletMapT = + wavemap::HashedWaveletOctreeT< + LayeredMapVoxel>; + +template +struct LayeredMapConfig { + wavemap::HashedWaveletOctreeConfig continuous_map; + typename ContinuousMapT::ThresholdConfig continuous_threshold; + typename ContinuousMapT::PruningConfig continuous_pruning; + DiscreteCompressionConfig discrete_compression; +}; + +template +class LayeredMap { + public: + using ContinuousVoxel = LayeredMapVoxel; + using ContinuousMap = ContinuousWaveletMapT; + using Config = LayeredMapConfig; + using DiscreteLayers = DiscreteLayersT; + + explicit LayeredMap(const Config& config) + : continuous_map_(std::make_shared( + config.continuous_map, config.continuous_threshold, + config.continuous_pruning)), + discrete_layers_(config.discrete_compression) {} + + explicit LayeredMap(const wavemap::HashedWaveletOctreeConfig& config) + : continuous_map_(std::make_shared(config)), + discrete_layers_() {} + + LayeredMap(const wavemap::HashedWaveletOctreeConfig& config, + const typename ContinuousMap::ThresholdConfig& threshold_config, + const typename ContinuousMap::PruningConfig& pruning_config) + : continuous_map_(std::make_shared( + config, threshold_config, pruning_config)), + discrete_layers_() {} + + LayeredMap(typename ContinuousMap::Ptr continuous_map, + DiscreteLayersT discrete_layers) + : continuous_map_(std::move(continuous_map)), + discrete_layers_(std::move(discrete_layers)) {} + + ContinuousMap& continuousMap() { return *continuous_map_; } + const ContinuousMap& continuousMap() const { return *continuous_map_; } + + DiscreteLayersT& discreteLayers() { return discrete_layers_; } + const DiscreteLayersT& discreteLayers() const { return discrete_layers_; } + + private: + typename ContinuousMap::Ptr continuous_map_; + DiscreteLayersT discrete_layers_; +}; + +} // namespace wavemap::layered + +#endif // WAVEMAP_LAYERED_LAYERED_MAP_H_ diff --git a/library/cpp/include/wavemap/layered/layered_map_io.h b/library/cpp/include/wavemap/layered/layered_map_io.h new file mode 100644 index 000000000..76dcd85f7 --- /dev/null +++ b/library/cpp/include/wavemap/layered/layered_map_io.h @@ -0,0 +1,433 @@ +#ifndef WAVEMAP_LAYERED_LAYERED_MAP_IO_H_ +#define WAVEMAP_LAYERED_LAYERED_MAP_IO_H_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include +#include + +namespace wavemap::layered::io { +namespace detail { +template +inline constexpr bool kAlwaysFalse = false; + +constexpr char kMagic[] = "LWVMP"; +constexpr uint32_t kVersion = 1u; +constexpr char kContinuousMapSection[] = "continuous_map"; +constexpr char kDiscreteLayersSection[] = "discrete_layers"; + +inline bool writeBytes(std::ostream& ostream, const char* data, size_t size) { + ostream.write(data, static_cast(size)); + return static_cast(ostream); +} + +inline bool readBytes(std::istream& istream, char* data, size_t size) { + istream.read(data, static_cast(size)); + return static_cast(istream); +} + +template +bool writePod(std::ostream& ostream, const T& value) { + static_assert(std::is_trivially_copyable_v); + return writeBytes(ostream, reinterpret_cast(&value), sizeof(T)); +} + +template +bool readPod(std::istream& istream, T& value) { + static_assert(std::is_trivially_copyable_v); + return readBytes(istream, reinterpret_cast(&value), sizeof(T)); +} + +inline bool writeString(std::ostream& ostream, const std::string& value) { + const uint64_t size = value.size(); + return writePod(ostream, size) && writeBytes(ostream, value.data(), value.size()); +} + +inline bool readString(std::istream& istream, std::string& value) { + uint64_t size = 0u; + if (!readPod(istream, size)) { + return false; + } + value.resize(size); + return readBytes(istream, value.data(), value.size()); +} + +inline bool writeSectionMarker(std::ostream& ostream, + const std::string& section_name) { + return writeString(ostream, section_name); +} + +inline bool readSectionMarker(std::istream& istream, + const std::string& expected_section_name) { + std::string section_name; + return readString(istream, section_name) && + section_name == expected_section_name; +} + +inline bool writeFileHeader(std::ostream& ostream) { + return writeString(ostream, kMagic) && writePod(ostream, kVersion); +} + +inline bool readFileHeader(std::istream& istream) { + std::string magic; + uint32_t version = 0u; + return readString(istream, magic) && magic == kMagic && + readPod(istream, version) && version == kVersion; +} +} // namespace detail + +template +struct DiscreteValueSerializer { + static constexpr const char* typeName() { + static_assert(detail::kAlwaysFalse, + "DiscreteValueSerializer must be specialized before " + "DiscreteLayer can be saved or loaded. Add a serializer " + "for this value type, for example by following the int/bool " + "specializations in layered_map_io.h."); + return "unsupported"; + } + + static bool write(std::ostream&, const ValueT&) { + static_assert(detail::kAlwaysFalse, + "DiscreteValueSerializer::write is not specialized for " + "this discrete value type."); + return false; + } + + static bool read(std::istream&, ValueT&) { + static_assert(detail::kAlwaysFalse, + "DiscreteValueSerializer::read is not specialized for " + "this discrete value type."); + return false; + } +}; + +template <> +struct DiscreteValueSerializer { + static constexpr const char* typeName() { return "int"; } + + static bool write(std::ostream& ostream, int value) { + const int32_t serialized = value; + return detail::writePod(ostream, serialized); + } + + static bool read(std::istream& istream, int& value) { + int32_t serialized = 0; + if (!detail::readPod(istream, serialized)) { + return false; + } + value = serialized; + return true; + } +}; + +template <> +struct DiscreteValueSerializer { + static constexpr const char* typeName() { return "bool"; } + + static bool write(std::ostream& ostream, bool value) { + const uint8_t serialized = value ? 1u : 0u; + return detail::writePod(ostream, serialized); + } + + static bool read(std::istream& istream, bool& value) { + uint8_t serialized = 0u; + if (!detail::readPod(istream, serialized)) { + return false; + } + value = serialized != 0u; + return true; + } +}; + +template +struct NamedDiscreteLayer { + std::string name; + DiscreteLayer& layer; +}; + +template +struct ConstNamedDiscreteLayer { + std::string name; + const DiscreteLayer& layer; +}; + +template +NamedDiscreteLayer namedDiscreteLayer(const std::string& name, + DiscreteLayer& layer) { + return {name, layer}; +} + +template +ConstNamedDiscreteLayer namedDiscreteLayer( + const std::string& name, const DiscreteLayer& layer) { + return {name, layer}; +} + +template +struct DiscreteLayerBundleTraits { + static auto layers(DiscreteLayersT&) { + static_assert(detail::kAlwaysFalse, + "DiscreteLayerBundleTraits must be specialized for your " + "discrete layer bundle. Return a tuple of " + "namedDiscreteLayer(name, layers.layer_field) entries."); + return std::tuple<>(); + } + + static auto layers(const DiscreteLayersT&) { + static_assert(detail::kAlwaysFalse, + "DiscreteLayerBundleTraits must be specialized for your " + "discrete layer bundle. Return a tuple of " + "namedDiscreteLayer(name, layers.layer_field) entries."); + return std::tuple<>(); + } +}; + +template +struct DiscreteLayerSerializer { + static bool write(std::ostream& ostream, const DiscreteLayer& layer) { + const int32_t block_height = layer.config().block_height; + if (!detail::writePod(ostream, block_height)) { + return false; + } + + const uint64_t parent_count = layer.cells().size(); + if (!detail::writePod(ostream, parent_count)) { + return false; + } + + for (const auto& [parent_key, cell] : layer.cells()) { + const int32_t x = parent_key.x; + const int32_t y = parent_key.y; + const int32_t z = parent_key.z; + if (!detail::writePod(ostream, x) || !detail::writePod(ostream, y) || + !detail::writePod(ostream, z) || + !DiscreteValueSerializer::write(ostream, cell.dominant_value)) { + return false; + } + + const uint64_t observed_count = cell.observed_offsets.size(); + if (!detail::writePod(ostream, observed_count)) { + return false; + } + for (const int offset : cell.observed_offsets) { + const int32_t serialized_offset = offset; + if (!detail::writePod(ostream, serialized_offset)) { + return false; + } + } + + const uint64_t exception_count = cell.exceptions.size(); + if (!detail::writePod(ostream, exception_count)) { + return false; + } + for (const auto& [offset, value] : cell.exceptions) { + const int32_t serialized_offset = offset; + if (!detail::writePod(ostream, serialized_offset) || + !DiscreteValueSerializer::write(ostream, value)) { + return false; + } + } + } + return static_cast(ostream); + } + + static bool read(std::istream& istream, DiscreteLayer& layer) { + int32_t block_height = 0; + uint64_t parent_count = 0u; + if (!detail::readPod(istream, block_height) || + !detail::readPod(istream, parent_count)) { + return false; + } + + std::map::Cell> cells; + for (uint64_t parent_i = 0u; parent_i < parent_count; ++parent_i) { + int32_t x = 0; + int32_t y = 0; + int32_t z = 0; + typename DiscreteLayer::Cell cell; + if (!detail::readPod(istream, x) || !detail::readPod(istream, y) || + !detail::readPod(istream, z) || + !DiscreteValueSerializer::read(istream, cell.dominant_value)) { + return false; + } + + uint64_t observed_count = 0u; + if (!detail::readPod(istream, observed_count)) { + return false; + } + for (uint64_t observed_i = 0u; observed_i < observed_count; ++observed_i) { + int32_t offset = 0; + if (!detail::readPod(istream, offset)) { + return false; + } + cell.observed_offsets.insert(offset); + } + + uint64_t exception_count = 0u; + if (!detail::readPod(istream, exception_count)) { + return false; + } + for (uint64_t exception_i = 0u; exception_i < exception_count; + ++exception_i) { + int32_t offset = 0; + ValueT value{}; + if (!detail::readPod(istream, offset) || + !DiscreteValueSerializer::read(istream, value)) { + return false; + } + cell.exceptions[offset] = value; + } + + cells[IndexKey(wavemap::Index3D(x, y, z))] = cell; + } + + layer.replaceCellsForLoad(DiscreteCompressionConfig{block_height}, + std::move(cells)); + return true; + } +}; + +template +bool forEachTupleElementImpl(TupleT&& tuple, FuncT&& func, + std::index_sequence) { + bool ok = true; + ((ok = ok && func(std::get(tuple))), ...); + return ok; +} + +template +bool forEachTupleElement(TupleT&& tuple, FuncT&& func) { + constexpr size_t tuple_size = + std::tuple_size_v>; + return forEachTupleElementImpl(std::forward(tuple), + std::forward(func), + std::make_index_sequence{}); +} + +template +bool writeDiscreteLayerBundle(std::ostream& ostream, + const DiscreteLayersT& layers) { + const auto named_layers = DiscreteLayerBundleTraits::layers(layers); + constexpr uint64_t layer_count = + std::tuple_size_v>; + if (!detail::writePod(ostream, layer_count)) { + return false; + } + return forEachTupleElement(named_layers, [&](const auto& named_layer) { + using ValueT = typename std::remove_reference_t::Value; + return detail::writeString(ostream, named_layer.name) && + detail::writeString(ostream, DiscreteValueSerializer::typeName()) && + DiscreteLayerSerializer::write(ostream, named_layer.layer); + }); +} + +template +bool readDiscreteLayerBundle(std::istream& istream, DiscreteLayersT& layers) { + const auto named_layers = DiscreteLayerBundleTraits::layers(layers); + constexpr uint64_t expected_layer_count = + std::tuple_size_v>; + uint64_t layer_count = 0u; + if (!detail::readPod(istream, layer_count) || + layer_count != expected_layer_count) { + return false; + } + + return forEachTupleElement(named_layers, [&](auto named_layer) { + using ValueT = typename std::remove_reference_t::Value; + std::string name; + std::string type_name; + if (!detail::readString(istream, name) || + !detail::readString(istream, type_name)) { + return false; + } + if (name != named_layer.name || + type_name != DiscreteValueSerializer::typeName()) { + return false; + } + return DiscreteLayerSerializer::read(istream, named_layer.layer); + }); +} + +template +bool saveLayeredMap(const std::filesystem::path& file_path, + const LayeredMapT& map) { + std::ofstream ostream(file_path, std::ios::binary); + if (!ostream.is_open()) { + return false; + } + + if (!detail::writeFileHeader(ostream) || + !detail::writeSectionMarker(ostream, detail::kContinuousMapSection)) { + return false; + } + if (!wavemap::io::mapToStream( + map.continuousMap(), ostream)) { + return false; + } + if (!detail::writeSectionMarker(ostream, detail::kDiscreteLayersSection)) { + return false; + } + return writeDiscreteLayerBundle(ostream, map.discreteLayers()); +} + +template +bool loadLayeredMap(const std::filesystem::path& file_path, LayeredMapT& map) { + std::ifstream istream(file_path, std::ios::binary); + if (!istream.is_open()) { + return false; + } + + if (!detail::readFileHeader(istream) || + !detail::readSectionMarker(istream, detail::kContinuousMapSection)) { + return false; + } + + typename LayeredMapT::ContinuousMap::Ptr continuous_map; + if (!wavemap::io::streamToMap(istream, + continuous_map) || + !continuous_map) { + return false; + } + + typename LayeredMapT::DiscreteLayers discrete_layers; + if (!detail::readSectionMarker(istream, detail::kDiscreteLayersSection) || + !readDiscreteLayerBundle(istream, discrete_layers)) { + return false; + } + + map = LayeredMapT(std::move(continuous_map), std::move(discrete_layers)); + return true; +} + + +template +struct LayeredMapIo { + static bool save(const std::filesystem::path& file_path, + const LayeredMapT& map) { + return saveLayeredMap(file_path, + map); + } + + static bool load(const std::filesystem::path& file_path, LayeredMapT& map) { + return loadLayeredMap(file_path, + map); + } +}; + +} // namespace wavemap::layered::io + +#endif // WAVEMAP_LAYERED_LAYERED_MAP_IO_H_ diff --git a/library/cpp/src/core/integrator/projective/coarse_to_fine/hashed_wavelet_integrator.cc b/library/cpp/src/core/integrator/projective/coarse_to_fine/hashed_wavelet_integrator.cc index 7751b8ede..ff2c6bcb9 100644 --- a/library/cpp/src/core/integrator/projective/coarse_to_fine/hashed_wavelet_integrator.cc +++ b/library/cpp/src/core/integrator/projective/coarse_to_fine/hashed_wavelet_integrator.cc @@ -1,203 +1 @@ #include "wavemap/core/integrator/projective/coarse_to_fine/hashed_wavelet_integrator.h" - -#include -#include -#include -#include - -#include - -namespace wavemap { -void HashedWaveletIntegrator::updateMap() { - ProfilerZoneScoped; - // Update the range image intersector - { - ProfilerZoneScopedN("updateRangeImageIntersector"); - range_image_intersector_ = std::make_shared( - posed_range_image_, projection_model_, *measurement_model_, - config_.min_range, config_.max_range); - } - - // Find all the indices of blocks that need updating - BlockList blocks_to_update; - { - ProfilerZoneScopedN("selectBlocksToUpdate"); - const auto [fov_min_idx, fov_max_idx] = - getFovMinMaxIndices(posed_range_image_->getOrigin()); - for (const auto& block_index : - Grid(fov_min_idx.position, fov_max_idx.position)) { - recursiveTester(OctreeIndex{fov_min_idx.height, block_index}, - blocks_to_update); - } - } - - // Make sure the to-be-updated blocks are allocated - for (const auto& block_index : blocks_to_update) { - occupancy_map_->getOrAllocateBlock(block_index); - } - - // Update it with the threadpool - for (const auto& block_index : blocks_to_update) { - thread_pool_->add_task([this, block_index]() { - if (auto* block = occupancy_map_->getBlock(block_index); block) { - updateBlock(*block, block_index); - } - }); - } - thread_pool_->wait_all(); -} - -std::pair -HashedWaveletIntegrator::getFovMinMaxIndices( - const Point3D& sensor_origin) const { - const int height = 1 + std::max(static_cast(std::ceil(std::log2( - config_.max_range / min_cell_width_))), - tree_height_); - const OctreeIndex fov_min_idx = convert::indexAndHeightToNodeIndex<3>( - convert::pointToFloorIndex<3>( - sensor_origin - Vector3D::Constant(config_.max_range), - min_cell_width_inv_) - - occupancy_map_->getBlockSize(), - height); - const OctreeIndex fov_max_idx = convert::indexAndHeightToNodeIndex<3>( - convert::pointToCeilIndex<3>( - sensor_origin + Vector3D::Constant(config_.max_range), - min_cell_width_inv_) + - occupancy_map_->getBlockSize(), - height); - return {fov_min_idx, fov_max_idx}; -} - -void HashedWaveletIntegrator::updateBlock( - HashedWaveletOctree::Block& block, - const HashedWaveletOctree::BlockIndex& block_index) { - ProfilerZoneScoped; - block.setNeedsPruning(); - block.setLastUpdatedStamp(); - - struct StackElement { - OctreeType::NodeRefType parent_node; - const OctreeIndex parent_node_index; - NdtreeIndexRelativeChild next_child_idx; - HashedWaveletOctreeBlock::Coefficients::CoefficientsArray - child_scale_coefficients; - }; - std::stack stack; - - OctreeType::NodeRefType root_node = block.getRootNode(); - HashedWaveletOctreeBlock::Coefficients::Scale& root_node_scale = - block.getRootScale(); - stack.emplace(StackElement{root_node, - {tree_height_, block_index}, - 0, - HashedWaveletOctreeBlock::Transform::backward( - {root_node_scale, root_node.data()})}); - - while (!stack.empty()) { - // If the current stack element has fully been processed, propagate upward - if (OctreeIndex::kNumChildren <= stack.top().next_child_idx) { - const auto [scale, details] = - HashedWaveletOctreeBlock::Transform::forward( - stack.top().child_scale_coefficients); - stack.top().parent_node.data() = details; - stack.pop(); - if (stack.empty()) { - root_node_scale = scale; - return; - } else { - const NdtreeIndexRelativeChild current_child_idx = - stack.top().next_child_idx - 1; - stack.top().child_scale_coefficients[current_child_idx] = scale; - continue; - } - } - - // Evaluate stack element's active child - const NdtreeIndexRelativeChild current_child_idx = - stack.top().next_child_idx; - ++stack.top().next_child_idx; - DCHECK_GE(current_child_idx, 0); - DCHECK_LT(current_child_idx, OctreeIndex::kNumChildren); - - OctreeType::NodeRefType parent_node = stack.top().parent_node; - FloatingPoint& node_value = - stack.top().child_scale_coefficients[current_child_idx]; - const OctreeIndex node_index = - stack.top().parent_node_index.computeChildIndex(current_child_idx); - DCHECK_GE(node_index.height, 0); - - // If we're at the leaf level, directly update the node - if (node_index.height <= termination_height_) { - const Point3D W_node_center = - convert::nodeIndexToCenterPoint(node_index, min_cell_width_); - const Point3D C_node_center = - posed_range_image_->getPoseInverse() * W_node_center; - const FloatingPoint sample = computeUpdate(C_node_center); - node_value = - std::clamp(sample + node_value, min_log_odds_ - kNoiseThreshold, - max_log_odds_ + kNoiseThreshold); - continue; - } - - // Otherwise, test whether the current node is fully occupied; - // free or unknown; or fully unknown - const AABB W_cell_aabb = - convert::nodeIndexToAABB(node_index, min_cell_width_); - const UpdateType update_type = - range_image_intersector_->determineUpdateType( - W_cell_aabb, posed_range_image_->getRotationMatrixInverse(), - posed_range_image_->getOrigin()); - - // If we're fully in unknown space, - // there's no need to evaluate this node or its children - if (update_type == UpdateType::kFullyUnobserved) { - continue; - } - - // We can also stop here if the cell will result in a free space update - // (or zero) and the map is already saturated free - if (update_type != UpdateType::kPossiblyOccupied && - node_value < min_log_odds_ + kNoiseThreshold / 10.f) { - continue; - } - - // Test if the worst-case error for the intersection type at the current - // resolution falls within the acceptable approximation error - const FloatingPoint node_width = W_cell_aabb.width<0>(); - const Point3D W_node_center = - W_cell_aabb.min + Vector3D::Constant(node_width / 2.f); - const Point3D C_node_center = - posed_range_image_->getPoseInverse() * W_node_center; - const FloatingPoint d_C_cell = - projection_model_->cartesianToSensorZ(C_node_center); - const FloatingPoint bounding_sphere_radius = - kUnitCubeHalfDiagonal * node_width; - OctreeType::NodePtrType node = - parent_node.getChild(node_index.computeRelativeChildIndex()); - if (measurement_model_->computeWorstCaseApproximationError( - update_type, d_C_cell, bounding_sphere_radius) < - config_.termination_update_error) { - const FloatingPoint sample = computeUpdate(C_node_center); - if (!node || !node->hasAtLeastOneChild()) { - node_value = - std::clamp(sample + node_value, min_log_odds_ - kNoiseThreshold, - max_log_odds_ + kNoiseThreshold); - } else { - node_value += sample; - block.setNeedsThresholding(); - } - continue; - } - - // Since the approximation error would still be too big, refine - if (!node) { - // Allocate the current node if it has not yet been allocated - node = &parent_node.getOrAllocateChild( - node_index.computeRelativeChildIndex()); - } - stack.emplace(StackElement{*node, node_index, 0, - HashedWaveletOctreeBlock::Transform::backward( - {node_value, node->data()})}); - } -} -} // namespace wavemap From b8b5f058b72ec4448483be502f90bf094de5f36e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Guilherne=20Caba=C3=A7o?= Date: Fri, 7 Aug 2026 00:32:25 +0000 Subject: [PATCH 08/14] Add layered map ROS and RViz pipeline --- examples/cpp/digital_twin/CMakeLists.txt | 12 + .../common/example_layered_map_config.h | 16 +- .../common/example_layered_map_ros_config.h | 116 ++- .../common/layered_ros_converter.h | 42 +- .../common/layered_voxel_config.h | 10 + .../io/create_large_layered_map_experiment.cc | 120 ++++ ...ayered_map_save_load_failure_experiment.cc | 47 +- .../io/load_existing_maps_experiment.cc | 139 ++++ ...blish_structured_layered_map_experiment.cc | 17 +- interfaces/ros1/wavemap_ros/CMakeLists.txt | 3 +- .../wavemap_ros/config/rviz/layered_map.rviz | 152 ++++ .../layered_ros_server_extension.h | 122 ++++ .../include/wavemap_ros/ros_server.h | 2 + interfaces/ros1/wavemap_ros/src/ros_server.cc | 16 + .../discrete_layer_marker_conversions.h | 2 +- .../ros1/wavemap_rviz_plugin/CMakeLists.txt | 2 + .../include/wavemap_rviz_plugin/common.h | 3 + .../wavemap_rviz_plugin/layered_map_display.h | 116 +++ .../visuals/voxel_visual.h | 23 +- .../wavemap_rviz_plugin/wavemap_map_display.h | 4 - .../plugin_description.xml | 8 + .../src/layered_map_display.cc | 672 ++++++++++++++++++ .../src/visuals/voxel_visual.cc | 404 ++++++++++- .../src/wavemap_map_display.cc | 59 +- .../cpp/include/wavemap/layered/layered_map.h | 2 + .../include/wavemap/layered/layered_map_io.h | 151 +++- .../wavemap/layered/layered_map_schema.h | 111 +++ 27 files changed, 2241 insertions(+), 130 deletions(-) create mode 100644 examples/cpp/digital_twin/io/create_large_layered_map_experiment.cc create mode 100644 examples/cpp/digital_twin/io/load_existing_maps_experiment.cc create mode 100644 interfaces/ros1/wavemap_ros/config/rviz/layered_map.rviz create mode 100644 interfaces/ros1/wavemap_ros/include/wavemap_ros/layered_ros_server_extension.h create mode 100644 interfaces/ros1/wavemap_rviz_plugin/include/wavemap_rviz_plugin/layered_map_display.h create mode 100644 interfaces/ros1/wavemap_rviz_plugin/src/layered_map_display.cc create mode 100644 library/cpp/include/wavemap/layered/layered_map_schema.h diff --git a/examples/cpp/digital_twin/CMakeLists.txt b/examples/cpp/digital_twin/CMakeLists.txt index 9398a0f19..1783d83f4 100644 --- a/examples/cpp/digital_twin/CMakeLists.txt +++ b/examples/cpp/digital_twin/CMakeLists.txt @@ -198,6 +198,18 @@ set_wavemap_target_properties(custom_layered_map_save_load_example) target_link_libraries(custom_layered_map_save_load_example PUBLIC wavemap::wavemap_core) +add_executable(load_existing_maps_experiment + io/load_existing_maps_experiment.cc) +set_wavemap_target_properties(load_existing_maps_experiment) +target_link_libraries(load_existing_maps_experiment + PUBLIC wavemap::wavemap_core wavemap::wavemap_io) + +add_executable(create_large_layered_map_experiment + io/create_large_layered_map_experiment.cc) +set_wavemap_target_properties(create_large_layered_map_experiment) +target_link_libraries(create_large_layered_map_experiment + PUBLIC wavemap::wavemap_core) + if ("$ENV{ROS_VERSION}" STREQUAL "1") find_package(catkin REQUIRED COMPONENTS roscpp visualization_msgs wavemap wavemap_msgs wavemap_ros_conversions) diff --git a/examples/cpp/digital_twin/common/example_layered_map_config.h b/examples/cpp/digital_twin/common/example_layered_map_config.h index a0451301f..d2e8753b3 100644 --- a/examples/cpp/digital_twin/common/example_layered_map_config.h +++ b/examples/cpp/digital_twin/common/example_layered_map_config.h @@ -9,6 +9,9 @@ using wavemap::layered::DiscreteCompressionConfig; using wavemap::layered::DiscreteLayer; using wavemap::layered::LayeredMap; +inline constexpr char kSemanticLayerName[] = "semantic"; +inline constexpr char kChangedLayerName[] = "changed"; + struct ExampleDiscreteLayers { explicit ExampleDiscreteLayers( DiscreteCompressionConfig config = DiscreteCompressionConfig()) @@ -25,17 +28,22 @@ using ExampleLayeredMapConfig = ExampleLayeredMap::Config; using ExampleLayeredMapIo = wavemap::layered::io::LayeredMapIo; +inline wavemap::layered::LayeredMapSchema exampleLayeredMapSchema( + const ExampleLayeredMap& map) { + return wavemap::layered::io::layeredMapSchema(map); +} + namespace wavemap::layered::io { template <> struct DiscreteLayerBundleTraits<::ExampleDiscreteLayers> { static auto layers(ExampleDiscreteLayers& layers) { - return std::make_tuple(namedDiscreteLayer("semantic", layers.semantic), - namedDiscreteLayer("changed", layers.changed)); + return std::make_tuple(namedDiscreteLayer(kSemanticLayerName, layers.semantic), + namedDiscreteLayer(kChangedLayerName, layers.changed)); } static auto layers(const ExampleDiscreteLayers& layers) { - return std::make_tuple(namedDiscreteLayer("semantic", layers.semantic), - namedDiscreteLayer("changed", layers.changed)); + return std::make_tuple(namedDiscreteLayer(kSemanticLayerName, layers.semantic), + namedDiscreteLayer(kChangedLayerName, layers.changed)); } }; } // namespace wavemap::layered::io diff --git a/examples/cpp/digital_twin/common/example_layered_map_ros_config.h b/examples/cpp/digital_twin/common/example_layered_map_ros_config.h index f5868b80d..943ff0cb7 100644 --- a/examples/cpp/digital_twin/common/example_layered_map_ros_config.h +++ b/examples/cpp/digital_twin/common/example_layered_map_ros_config.h @@ -1,26 +1,130 @@ #ifndef WAVEMAP_EXAMPLES_CPP_DIGITAL_TWIN_EXAMPLE_LAYERED_MAP_ROS_CONFIG_H_ #define WAVEMAP_EXAMPLES_CPP_DIGITAL_TWIN_EXAMPLE_LAYERED_MAP_ROS_CONFIG_H_ +#include #include +#include +#include +#include #include #include #include "example_layered_map_config.h" +namespace layered_map_config { +namespace detail { +inline bool readRgbParam(const ros::NodeHandle& nh, const std::string& key, + Rgb& value) { + std::vector values; + if (!nh.getParam(key, values)) { + return false; + } + if (values.size() != 3u) { + ROS_WARN_STREAM("Ignoring layered map RGB param " << key + << " because it must contain exactly 3 values."); + return false; + } + value = rgb(static_cast(values[0]), static_cast(values[1]), + static_cast(values[2])); + return true; +} + +template +void readParam(const ros::NodeHandle& nh, const std::string& key, + ValueT& value) { + nh.param(key, value, value); +} +} // namespace detail + +inline ExampleLayeredMapConfig makeDefaultExampleLayeredMapConfig() { + ExampleLayeredMapConfig config; + config.continuous_map.min_cell_width = 0.1f; + config.continuous_map.min_log_odds = -2.f; + config.continuous_map.max_log_odds = 4.f; + config.continuous_map.tree_height = 3; + config.continuous_map.only_prune_blocks_if_unused_for = 5.f; + + config.continuous_threshold.data.rgb_min = rgb(0.f, 0.f, 0.f); + config.continuous_threshold.data.rgb_max = rgb(1.f, 1.f, 1.f); + config.continuous_threshold.data.traversability_min = 0.f; + config.continuous_threshold.data.traversability_max = 1.f; + + config.continuous_pruning.occupancy.scale = 1e-3f; + config.continuous_pruning.occupancy.weight = 1.f; + config.continuous_pruning.data.rgb.scale = 1e-3f; + config.continuous_pruning.data.rgb.weight = 1.f; + config.continuous_pruning.data.traversability.scale = 1e-3f; + config.continuous_pruning.data.traversability.weight = 1.f; + config.continuous_pruning.combined_threshold = 1.f; + config.continuous_pruning.data.combined_threshold = 1.f; + + config.discrete_compression.block_height = 1; + return config; +} + +inline ExampleLayeredMapConfig loadExampleLayeredMapConfigFromRosParams( + const ros::NodeHandle& nh_private, + const std::string& ns = "layered_map") { + ExampleLayeredMapConfig config = makeDefaultExampleLayeredMapConfig(); + + detail::readParam(nh_private, ns + "/continuous_map/min_cell_width/meters", + config.continuous_map.min_cell_width); + detail::readParam(nh_private, ns + "/continuous_map/min_log_odds", + config.continuous_map.min_log_odds); + detail::readParam(nh_private, ns + "/continuous_map/max_log_odds", + config.continuous_map.max_log_odds); + detail::readParam(nh_private, ns + "/continuous_map/tree_height", + config.continuous_map.tree_height); + detail::readParam( + nh_private, ns + "/continuous_map/only_prune_blocks_if_unused_for/seconds", + config.continuous_map.only_prune_blocks_if_unused_for); + + detail::readRgbParam(nh_private, ns + "/continuous_threshold/color/min", + config.continuous_threshold.data.rgb_min); + detail::readRgbParam(nh_private, ns + "/continuous_threshold/color/max", + config.continuous_threshold.data.rgb_max); + detail::readParam(nh_private, ns + "/continuous_threshold/traversability/min", + config.continuous_threshold.data.traversability_min); + detail::readParam(nh_private, ns + "/continuous_threshold/traversability/max", + config.continuous_threshold.data.traversability_max); + + detail::readParam(nh_private, ns + "/continuous_pruning/combined_threshold", + config.continuous_pruning.combined_threshold); + config.continuous_pruning.data.combined_threshold = + config.continuous_pruning.combined_threshold; + detail::readParam(nh_private, ns + "/continuous_pruning/occupancy/scale", + config.continuous_pruning.occupancy.scale); + detail::readParam(nh_private, ns + "/continuous_pruning/occupancy/weight", + config.continuous_pruning.occupancy.weight); + detail::readParam(nh_private, ns + "/continuous_pruning/color/scale", + config.continuous_pruning.data.rgb.scale); + detail::readParam(nh_private, ns + "/continuous_pruning/color/weight", + config.continuous_pruning.data.rgb.weight); + detail::readParam(nh_private, ns + "/continuous_pruning/traversability/scale", + config.continuous_pruning.data.traversability.scale); + detail::readParam(nh_private, ns + "/continuous_pruning/traversability/weight", + config.continuous_pruning.data.traversability.weight); + + detail::readParam(nh_private, ns + "/discrete_compression/block_height", + config.discrete_compression.block_height); + return config; +} +} // namespace layered_map_config + namespace wavemap::convert { template <> struct DiscreteLayerBundleRosTraits<::ExampleDiscreteLayers> { static auto layers(ExampleDiscreteLayers& layers) { return std::make_tuple( - namedDiscreteLayerForRos("semantic", layers.semantic), - namedDiscreteLayerForRos("changed", layers.changed)); + namedDiscreteLayerForRos(kSemanticLayerName, layers.semantic), + namedDiscreteLayerForRos(kChangedLayerName, layers.changed)); } static auto layers(const ExampleDiscreteLayers& layers) { return std::make_tuple( - namedDiscreteLayerForRos("semantic", layers.semantic), - namedDiscreteLayerForRos("changed", layers.changed)); + namedDiscreteLayerForRos(kSemanticLayerName, layers.semantic), + namedDiscreteLayerForRos(kChangedLayerName, layers.changed)); } }; } // namespace wavemap::convert @@ -57,9 +161,9 @@ template <> struct DiscreteLayerMarkerTraits<::ExampleDiscreteLayers> { static auto layers(const ExampleDiscreteLayers& layers) { return std::make_tuple( - namedDiscreteMarkerLayer("semantic", layers.semantic, + namedDiscreteMarkerLayer(kSemanticLayerName, layers.semantic, ExampleSemanticColorPolicy{}), - namedDiscreteMarkerLayer("changed", layers.changed, + namedDiscreteMarkerLayer(kChangedLayerName, layers.changed, ExampleChangedColorPolicy{})); } }; diff --git a/examples/cpp/digital_twin/common/layered_ros_converter.h b/examples/cpp/digital_twin/common/layered_ros_converter.h index 5c5868b48..879e421d8 100644 --- a/examples/cpp/digital_twin/common/layered_ros_converter.h +++ b/examples/cpp/digital_twin/common/layered_ros_converter.h @@ -10,28 +10,35 @@ struct LayeredVoxelRosConverter { static std::vector layerNames() { - // These names describe the custom data carried inside each LayeredVoxel. - return {"color", "traversability"}; + std::vector names; + for (const auto& layer : + wavemap::layered::ContinuousLayerSchemaTraits::layers()) { + names.emplace_back(layer.name); + } + return names; } static std::vector layerTypes() { - // The type strings tell consumers how to interpret each layer's arrays. - return {"float32_rgb", "float32"}; + std::vector types; + for (const auto& layer : + wavemap::layered::ContinuousLayerSchemaTraits::layers()) { + types.emplace_back(layer.type); + } + return types; } static std::vector makeLayers() { // The message stores custom values layer-major: all color values go into // the color layer, and all traversability values go into the traversability // layer. - wavemap_msgs::Layer color_layer; - color_layer.name = "color"; - color_layer.type = "float32_rgb"; - - wavemap_msgs::Layer traversability_layer; - traversability_layer.name = "traversability"; - traversability_layer.type = "float32"; - - return {color_layer, traversability_layer}; + std::vector layers; + for (const auto& layer_schema : + wavemap::layered::ContinuousLayerSchemaTraits::layers()) { + auto& layer = layers.emplace_back(); + layer.name = layer_schema.name; + layer.type = layer_schema.type; + } + return layers; } static void appendLayerValues(const LayeredVoxel& voxel, @@ -79,13 +86,16 @@ inline void populateLayeredCube( cube_origin + wavemap::Index3D(dx, dy, dz); const wavemap::FloatingPoint layer_value = static_cast(populated_voxels); + const Rgb visible_color = rgb( + dx == 0 ? 0.15f : 1.0f, + dy == 0 ? 0.15f : 1.0f, + 0.25f + 0.25f * static_cast(dz)); const LayeredVoxel original_voxel( 0.5f + 0.02f * layer_value, - ContinuousLayers{rgb(0.1f * dx, 0.1f * dy, 0.05f * dz), - 0.9f - 0.03f * layer_value}); + ContinuousLayers{visible_color, 0.9f - 0.03f * layer_value}); const LayeredVoxel voxel_update( 0.1f, - ContinuousLayers{rgb(-0.01f, -0.01f, -0.01f), -0.01f}); + ContinuousLayers{rgb(0.f, 0.f, 0.f), -0.01f}); map.setVoxelValue(voxel_index, original_voxel); map.addToVoxelValue(voxel_index, voxel_update); diff --git a/examples/cpp/digital_twin/common/layered_voxel_config.h b/examples/cpp/digital_twin/common/layered_voxel_config.h index dfb550c29..308133f22 100644 --- a/examples/cpp/digital_twin/common/layered_voxel_config.h +++ b/examples/cpp/digital_twin/common/layered_voxel_config.h @@ -13,6 +13,7 @@ #include #include #include +#include struct Rgb { float r = 0.f; @@ -83,6 +84,15 @@ struct ContinuousLayersPolicy { } }; +namespace wavemap::layered { +template <> +struct ContinuousLayerSchemaTraits<::ContinuousLayers> { + static std::vector layers() { + return {{"color", "float32_rgb"}, {"traversability", "float32"}}; + } +}; +} // namespace wavemap::layered + namespace wavemap { template <> struct CellDataTraits<::ContinuousLayers> { diff --git a/examples/cpp/digital_twin/io/create_large_layered_map_experiment.cc b/examples/cpp/digital_twin/io/create_large_layered_map_experiment.cc new file mode 100644 index 000000000..58868a204 --- /dev/null +++ b/examples/cpp/digital_twin/io/create_large_layered_map_experiment.cc @@ -0,0 +1,120 @@ +#include +#include +#include + +#include "../common/example_layered_map_config.h" + +namespace { +float normalized(float value, float max_value) { + return max_value <= 0.f ? 0.f : value / max_value; +} + +LayeredVoxel makeVoxel(int x, int y, int z, int size_x, int size_y, int size_z) { + const float nx = normalized(static_cast(x), static_cast(size_x - 1)); + const float ny = normalized(static_cast(y), static_cast(size_y - 1)); + const float nz = normalized(static_cast(z), static_cast(size_z - 1)); + + const float distance_from_center = + std::sqrt((nx - 0.5f) * (nx - 0.5f) + (ny - 0.5f) * (ny - 0.5f) + + (nz - 0.5f) * (nz - 0.5f)); + const float occupancy = 3.f - 12.f * distance_from_center; + const Rgb color = rgb(nx, ny, 1.f - nz); + const float traversability = std::clamp(1.f - 1.6f * distance_from_center, 0.f, 1.f); + + return makeLayeredVoxel(occupancy, color, traversability); +} + +int semanticLabelFor(int x, int y, int z, int size_x, int size_y) { + if (z > 20 && x > size_x / 4 && x < 3 * size_x / 4 && + y > size_y / 4 && y < 3 * size_y / 4) { + return 2; + } + if ((x / 16 + y / 16) % 2 == 0) { + return 1; + } + return 3; +} +} // namespace + +int main(int argc, char** argv) { + const std::filesystem::path output_path = + argc > 1 ? std::filesystem::path(argv[1]) + : std::filesystem::path("/home/ci/data/maps/layered_map_large.lwvmp"); + + ExampleLayeredMapConfig config; + config.continuous_map.min_cell_width = 0.25f; + config.continuous_map.min_log_odds = -2.f; + config.continuous_map.max_log_odds = 4.f; + config.continuous_map.tree_height = 6; + config.continuous_map.only_prune_blocks_if_unused_for = 5.f; + config.continuous_pruning = makeLayeredPruningConfigWithScales( + 4.f, 1.f, 1.f, 1e-3f, 1e-3f, 1e-3f, 10.f); + config.continuous_threshold.data.rgb_min = rgb(0.f, 0.f, 0.f); + config.continuous_threshold.data.rgb_max = rgb(1.f, 1.f, 1.f); + config.continuous_threshold.data.traversability_min = 0.f; + config.continuous_threshold.data.traversability_max = 1.f; + config.discrete_compression.block_height = 2; + + ExampleLayeredMap map(config); + + constexpr int kSizeX = 96; + constexpr int kSizeY = 80; + constexpr int kSizeZ = 40; + size_t populated_continuous_voxels = 0u; + size_t populated_discrete_voxels = 0u; + + for (int x = 0; x < kSizeX; ++x) { + for (int y = 0; y < kSizeY; ++y) { + for (int z = 0; z < kSizeZ; ++z) { + const wavemap::Index3D index{x, y, z}; + const LayeredVoxel voxel = makeVoxel(x, y, z, kSizeX, kSizeY, kSizeZ); + map.continuousMap().setVoxelValue(index, voxel); + ++populated_continuous_voxels; + + // Keep discrete attributes spatially aligned with the occupied part of the + // continuous map. The continuous map still stores free-space values around + // it, but labels only exist where a semantic value would make sense. + if (1e-3f <= voxel.occupancy) { + map.discreteLayers().semantic.setValue( + index, semanticLabelFor(x, y, z, kSizeX, kSizeY)); + map.discreteLayers().changed.setValue( + index, x == y || (z % 11 == 0 && x % 9 == 0)); + ++populated_discrete_voxels; + } + } + } + } + + const size_t continuous_nodes_before = map.continuousMap().size(); + const size_t continuous_blocks_before = map.continuousMap().getHashMap().size(); + map.continuousMap().threshold(); + map.continuousMap().prune(); + const size_t continuous_nodes_after = map.continuousMap().size(); + const size_t continuous_blocks_after = map.continuousMap().getHashMap().size(); + + std::filesystem::create_directories(output_path.parent_path()); + if (!ExampleLayeredMapIo::save(output_path, map)) { + std::cerr << "Failed to save LayeredMap to " << output_path << "\n"; + return 1; + } + + std::cout << "Created large layered map\n"; + std::cout << "output: " << output_path << "\n"; + std::cout << "populated continuous voxels: " << populated_continuous_voxels << "\n"; + std::cout << "populated discrete voxels: " << populated_discrete_voxels << "\n"; + std::cout << "continuous blocks before prune: " << continuous_blocks_before << "\n"; + std::cout << "continuous blocks after prune: " << continuous_blocks_after << "\n"; + std::cout << "continuous nodes before prune: " << continuous_nodes_before << "\n"; + std::cout << "continuous nodes after prune: " << continuous_nodes_after << "\n"; + std::cout << "semantic parents: " << map.discreteLayers().semantic.parentCount() + << " exceptions: " << map.discreteLayers().semantic.exceptionCount() + << " observed: " << map.discreteLayers().semantic.observedValueCount() + << "\n"; + std::cout << "changed parents: " << map.discreteLayers().changed.parentCount() + << " exceptions: " << map.discreteLayers().changed.exceptionCount() + << " observed: " << map.discreteLayers().changed.observedValueCount() + << "\n"; + std::cout << "file size bytes: " << std::filesystem::file_size(output_path) << "\n"; + + return 0; +} diff --git a/examples/cpp/digital_twin/io/layered_map_save_load_failure_experiment.cc b/examples/cpp/digital_twin/io/layered_map_save_load_failure_experiment.cc index e063b47c7..36554c1d1 100644 --- a/examples/cpp/digital_twin/io/layered_map_save_load_failure_experiment.cc +++ b/examples/cpp/digital_twin/io/layered_map_save_load_failure_experiment.cc @@ -8,12 +8,23 @@ #include namespace { -bool expectFailure(const std::string& name, bool load_result) { +bool expectFailure(const std::string& name, bool load_result, + const std::string& error_message) { const bool failed = !load_result; - std::cout << " " << name << ": " << (failed ? "pass" : "FAIL") << "\n"; + std::cout << " " << name << ": " << (failed ? "pass" : "FAIL"); + if (failed && !error_message.empty()) { + std::cout << " (" << error_message << ")"; + } + std::cout << "\n"; return failed; } +bool loadWithError(const std::filesystem::path& path, ExampleLayeredMap& map, + std::string& error_message) { + error_message.clear(); + return ExampleLayeredMapIo::load(path, map, &error_message); +} + bool replaceBytes(const std::filesystem::path& path, const std::string& from, const std::string& to) { if (from.size() != to.size()) { @@ -26,11 +37,17 @@ bool replaceBytes(const std::filesystem::path& path, const std::string& from, } std::string data((std::istreambuf_iterator(input)), std::istreambuf_iterator()); - const size_t pos = data.find(from); - if (pos == std::string::npos) { + + size_t replacements = 0u; + size_t pos = data.find(from); + while (pos != std::string::npos) { + data.replace(pos, from.size(), to); + ++replacements; + pos = data.find(from, pos + to.size()); + } + if (replacements == 0u) { return false; } - data.replace(pos, from.size(), to); std::ofstream output(path, std::ios::binary | std::ios::trunc); if (!output.is_open()) { @@ -80,9 +97,11 @@ int main() { { ExampleLayeredMap loaded(config); + std::string error_message; ok &= expectFailure( "missing file fails", - ExampleLayeredMapIo::load(output_dir / "missing_file.lwvmp", loaded)); + loadWithError(output_dir / "missing_file.lwvmp", loaded, error_message), + error_message); } { @@ -92,8 +111,10 @@ int main() { return 1; } ExampleLayeredMap loaded(config); + std::string error_message; ok &= expectFailure("wrong magic fails", - ExampleLayeredMapIo::load(path, loaded)); + loadWithError(path, loaded, error_message), + error_message); } { @@ -104,8 +125,10 @@ int main() { return 1; } ExampleLayeredMap loaded(config); + std::string error_message; ok &= expectFailure("unsupported version fails", - ExampleLayeredMapIo::load(path, loaded)); + loadWithError(path, loaded, error_message), + error_message); } ExampleLayeredMap map(config); @@ -119,8 +142,10 @@ int main() { return 1; } ExampleLayeredMap loaded(config); + std::string error_message; ok &= expectFailure("wrong discrete layer name fails", - ExampleLayeredMapIo::load(path, loaded)); + loadWithError(path, loaded, error_message), + error_message); } { @@ -131,8 +156,10 @@ int main() { return 1; } ExampleLayeredMap loaded(config); + std::string error_message; ok &= expectFailure("wrong discrete layer type fails", - ExampleLayeredMapIo::load(path, loaded)); + loadWithError(path, loaded, error_message), + error_message); } if (!ok) { diff --git a/examples/cpp/digital_twin/io/load_existing_maps_experiment.cc b/examples/cpp/digital_twin/io/load_existing_maps_experiment.cc new file mode 100644 index 000000000..549c105f9 --- /dev/null +++ b/examples/cpp/digital_twin/io/load_existing_maps_experiment.cc @@ -0,0 +1,139 @@ +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include "../common/example_layered_map_config.h" + +namespace { +void printUsage(const char* executable_name) { + std::cerr << "Usage: " << executable_name + << " \n"; +} + +void printLayeredMapSchema(const wavemap::layered::LayeredMapSchema& schema) { + std::cout << " schema continuous layers:"; + if (schema.continuous_layers.empty()) { + std::cout << " none"; + } + std::cout << "\n"; + for (const auto& layer : schema.continuous_layers) { + std::cout << " - " << layer.name << " (" << layer.type << ")\n"; + } + + std::cout << " schema discrete layers:"; + if (schema.discrete_layers.empty()) { + std::cout << " none"; + } + std::cout << "\n"; + for (const auto& layer : schema.discrete_layers) { + std::cout << " - " << layer.name << " (" << layer.type << ")\n"; + } +} + +bool inspectWavemapFile(const std::filesystem::path& path) { + wavemap::MapBase::Ptr map; + if (!wavemap::io::fileToMap(path, map) || !map) { + std::cerr << "Failed to load Wavemap file: " << path << "\n"; + return false; + } + + std::cout << "Loaded Wavemap file: " << path << "\n"; + std::cout << " min cell width: " << map->getMinCellWidth() << "\n"; + std::cout << " tree height: " << map->getTreeHeight() << "\n"; + std::cout << " log odds range: [" << map->getMinLogOdds() << ", " + << map->getMaxLogOdds() << "]\n"; + + if (const auto hashed_wavelet = + std::dynamic_pointer_cast(map)) { + std::cout << " type: HashedWaveletOctree\n"; + std::cout << " allocated blocks: " << hashed_wavelet->getHashMap().size() + << "\n"; + } else if (std::dynamic_pointer_cast(map)) { + std::cout << " type: WaveletOctree\n"; + } else if (const auto hashed_blocks = + std::dynamic_pointer_cast(map)) { + std::cout << " type: HashedBlocks\n"; + std::cout << " allocated blocks: " << hashed_blocks->getHashMap().size() + << "\n"; + } else if (std::dynamic_pointer_cast(map)) { + std::cout << " type: VolumetricOctree\n"; + } else { + std::cout << " type: unknown MapBase subclass\n"; + } + return true; +} + +bool inspectLayeredMapFile(const std::filesystem::path& path) { + ExampleLayeredMapConfig config; + ExampleLayeredMap map(config); + const wavemap::layered::LayeredMapSchema expected_schema = + exampleLayeredMapSchema(map); + + wavemap::layered::LayeredMapSchema file_schema; + if (wavemap::layered::io::readLayeredMapSchema(path, file_schema)) { + std::cout << "Read LayeredMap schema before full load: " << path << "\n"; + printLayeredMapSchema(file_schema); + + std::string compatibility_error; + if (wavemap::layered::checkLayeredMapSchemaCompatibility( + file_schema, expected_schema, &compatibility_error)) { + std::cout << " schema compatibility: pass\n"; + } else { + std::cout << " schema compatibility: FAIL\n"; + std::cout << " " << compatibility_error << "\n"; + } + } else { + std::cout << "Could not read LayeredMap schema before full load: " << path + << "\n"; + std::cout << " Note: this can happen for invalid/non-layered files, or " + ".lwvmp files written before schema metadata was added.\n"; + } + + std::string load_error; + if (!ExampleLayeredMapIo::load(path, map, &load_error)) { + std::cerr << "Failed to load LayeredMap file: " << path << "\n"; + if (!load_error.empty()) { + std::cerr << " " << load_error << "\n"; + } + std::cerr << " Note: .lwvmp files are typed. This inspector expects the " + "ExampleLayeredMap schema.\n"; + return false; + } + + std::cout << "Loaded LayeredMap file: " << path << "\n"; + std::cout << " continuous min cell width: " + << map.continuousMap().getMinCellWidth() << "\n"; + std::cout << " continuous tree height: " << map.continuousMap().getTreeHeight() + << "\n"; + std::cout << " continuous allocated blocks: " + << map.continuousMap().getHashMap().size() << "\n"; + std::cout << " semantic parent cells: " + << map.discreteLayers().semantic.parentCount() << "\n"; + std::cout << " semantic exceptions: " + << map.discreteLayers().semantic.exceptionCount() << "\n"; + std::cout << " changed parent cells: " + << map.discreteLayers().changed.parentCount() << "\n"; + std::cout << " changed exceptions: " + << map.discreteLayers().changed.exceptionCount() << "\n"; + return true; +} +} // namespace + +int main(int argc, char** argv) { + if (argc != 3) { + printUsage(argv[0]); + return 1; + } + + const bool wavemap_ok = inspectWavemapFile(argv[1]); + const bool layered_ok = inspectLayeredMapFile(argv[2]); + return wavemap_ok && layered_ok ? 0 : 1; +} diff --git a/examples/cpp/digital_twin/ros/publish_structured_layered_map_experiment.cc b/examples/cpp/digital_twin/ros/publish_structured_layered_map_experiment.cc index 2220c3cb8..40b9dd59b 100644 --- a/examples/cpp/digital_twin/ros/publish_structured_layered_map_experiment.cc +++ b/examples/cpp/digital_twin/ros/publish_structured_layered_map_experiment.cc @@ -27,13 +27,10 @@ void populateDiscreteLayers(ExampleLayeredMap& map, int main(int argc, char** argv) { ros::init(argc, argv, "wavemap_publish_structured_layered_map_experiment"); ros::NodeHandle nh; + ros::NodeHandle nh_private("~"); - ExampleLayeredMapConfig config; - config.continuous_map.min_cell_width = 0.1f; - config.continuous_map.min_log_odds = -2.f; - config.continuous_map.max_log_odds = 4.f; - config.continuous_map.tree_height = 3; - config.discrete_compression.block_height = 1; + ExampleLayeredMapConfig config = + layered_map_config::loadExampleLayeredMapConfigFromRosParams(nh_private); ExampleLayeredMap map(config); std::vector populated_indices; @@ -49,13 +46,16 @@ int main(int argc, char** argv) { } const std::string layered_map_topic = "/wavemap/layered_map"; - const std::string continuous_map_topic = "/wavemap/map"; + const std::string continuous_map_topic = "/wavemap/layered_map/continuous"; + const std::string continuous_map_compat_topic = "/wavemap/map"; const int queue_size = 1; const bool latch = true; ros::Publisher layered_map_publisher = nh.advertise(layered_map_topic, queue_size, latch); ros::Publisher continuous_map_publisher = nh.advertise(continuous_map_topic, queue_size, latch); + ros::Publisher continuous_map_compat_publisher = nh.advertise( + continuous_map_compat_topic, queue_size, latch); std::map marker_publishers; auto marker_publisher_factory = [&](const std::string& topic) -> ros::Publisher& { auto [publisher_it, inserted] = marker_publishers.emplace(topic, ros::Publisher{}); @@ -69,6 +69,7 @@ int main(int argc, char** argv) { layered_map_publisher.publish(msg); continuous_map_publisher.publish(msg.continuous_map); + continuous_map_compat_publisher.publish(msg.continuous_map); if (!layered_map_viz::publishDiscreteLayerMarkers( map.discreteLayers(), msg.header.frame_id, msg.header.stamp, marker_publisher_factory)) { @@ -79,6 +80,8 @@ int main(int argc, char** argv) { std::cout << "Published LayeredMap on: " << layered_map_topic << "\n"; std::cout << "Published continuous map on: " << continuous_map_topic << "\n"; + std::cout << "Published continuous compatibility map on: " + << continuous_map_compat_topic << "\n"; for (const auto& [topic, publisher] : marker_publishers) { (void)publisher; std::cout << "Published discrete markers on: " << topic << "\n"; diff --git a/interfaces/ros1/wavemap_ros/CMakeLists.txt b/interfaces/ros1/wavemap_ros/CMakeLists.txt index e90bc7449..59aa15a1b 100644 --- a/interfaces/ros1/wavemap_ros/CMakeLists.txt +++ b/interfaces/ros1/wavemap_ros/CMakeLists.txt @@ -44,7 +44,8 @@ add_library(${PROJECT_NAME} src/ros_server.cc) set_wavemap_target_properties(${PROJECT_NAME}) target_include_directories(${PROJECT_NAME} - PUBLIC include ${catkin_INCLUDE_DIRS}) + PUBLIC include ${catkin_INCLUDE_DIRS} + PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/../../../examples/cpp/digital_twin/common) target_link_libraries(${PROJECT_NAME} PUBLIC ${catkin_LIBRARIES} ${OpenCV_LIBRARIES}) diff --git a/interfaces/ros1/wavemap_ros/config/rviz/layered_map.rviz b/interfaces/ros1/wavemap_ros/config/rviz/layered_map.rviz new file mode 100644 index 000000000..fd8d904cb --- /dev/null +++ b/interfaces/ros1/wavemap_ros/config/rviz/layered_map.rviz @@ -0,0 +1,152 @@ +Panels: + - Class: rviz/Displays + Help Height: 138 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /LayeredMap1 + - /LayeredMap1/Render voxels1 + - /LayeredMap1/Render voxels1/Layer color1 + - /LayeredMap1/Render voxels1/Frame rate1 + Splitter Ratio: 0.5 + Tree Height: 1540 + - Class: rviz/Selection + Name: Selection + - Class: rviz/Tool Properties + Expanded: + - /2D Pose Estimate1 + - /2D Nav Goal1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz/Time + Name: Time + SyncMode: 0 + SyncSource: "" +Preferences: + PromptSaveOnExit: true +Toolbars: + toolButtonStyle: 2 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Class: wavemap_rviz_plugin/LayeredMap + Enabled: true + Name: LayeredMap + Queue Size: 10 + Source: Topic + Render voxels: + Alpha: 1 + Cell selector: Surface + Color mode: Height + Enable: true + Flat color: 0; 0; 255 + Frame rate: + Max update time: 20 + Layer: occupancy + Layer color: + Bool true color: 255; 12; 12 + Scalar high color: 255; 255; 0 + Scalar low color: 0; 0; 255 + Scalar max: 1 + Scalar min: 0 + Show bool false: false + Log odds threshold: 0.0010000000474974513 + Max log odds: 1e+06 + Min log odds: 0.0010000000474974513 + Termination height: 0 + Unknown log odds: 9.999999747378752e-05 + Topic: /wavemap/layered_map + Unreliable: false + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Default Light: true + Fixed Frame: map + Frame Rate: 30 + Name: root + Tools: + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/MoveCamera + - Class: rviz/Select + - Class: rviz/FocusCamera + - Class: rviz/Measure + - Class: rviz/SetInitialPose + Theta std deviation: 0.2617993950843811 + Topic: /initialpose + X std deviation: 0.5 + Y std deviation: 0.5 + - Class: rviz/SetGoal + Topic: /move_base_simple/goal + - Class: rviz/PublishPoint + Single click: true + Topic: /clicked_point + Value: true + Views: + Current: + Class: rviz/Orbit + Distance: 3.1961989402770996 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Field of View: 0.7853981852531433 + Focal Point: + X: 0 + Y: 0 + Z: 0 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.5203978419303894 + Target Frame: + Yaw: 0.670401394367218 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 2022 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd000000040000000000000331000006f2fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b000000ab00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000abfb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000069000006f20000017800fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000014f000006f2fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730100000069000006f20000012300fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e1000001970000000300000e7c00000056fc0100000002fb0000000800540069006d0065010000000000000e7c0000058200fffffffb0000000800540069006d00650100000000000004500000000000000000000009e4000006f200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 3708 + X: 1860 + Y: 64 diff --git a/interfaces/ros1/wavemap_ros/include/wavemap_ros/layered_ros_server_extension.h b/interfaces/ros1/wavemap_ros/include/wavemap_ros/layered_ros_server_extension.h new file mode 100644 index 000000000..5519af79d --- /dev/null +++ b/interfaces/ros1/wavemap_ros/include/wavemap_ros/layered_ros_server_extension.h @@ -0,0 +1,122 @@ +#ifndef WAVEMAP_ROS_LAYERED_ROS_SERVER_EXTENSION_H_ +#define WAVEMAP_ROS_LAYERED_ROS_SERVER_EXTENSION_H_ + +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +namespace wavemap { +class LayeredRosServerExtensionBase { + public: + virtual ~LayeredRosServerExtensionBase() = default; + + virtual bool saveLayeredMap(const std::filesystem::path& file_path) const = 0; + virtual bool loadLayeredMap(const std::filesystem::path& file_path, + std::string* error_message = nullptr) = 0; + virtual bool publishLayeredMap(const ros::Time& stamp = ros::Time::now()) = 0; +}; + +template +class LayeredRosServerExtension : public LayeredRosServerExtensionBase { + public: + LayeredRosServerExtension(ros::NodeHandle nh_private, std::string world_frame, + LayeredMapT layered_map, + std::string layered_map_topic = "layered_map") + : nh_private_(std::move(nh_private)), + world_frame_(std::move(world_frame)), + layered_map_(std::move(layered_map)) { + layered_map_pub_ = nh_private_.advertise( + layered_map_topic, kQueueSize, kLatchPublisher); + advertiseServices(); + } + + LayeredMapT& getLayeredMap() { return layered_map_; } + const LayeredMapT& getLayeredMap() const { return layered_map_; } + + bool saveLayeredMap(const std::filesystem::path& file_path) const override { + std::scoped_lock lock(mutex_); + return LayeredMapIoT::save(file_path, layered_map_); + } + + bool loadLayeredMap(const std::filesystem::path& file_path, + std::string* error_message = nullptr) override { + LayeredMapT loaded_map = layered_map_; + if (!LayeredMapIoT::load(file_path, loaded_map, error_message)) { + return false; + } + + { + std::scoped_lock lock(mutex_); + layered_map_ = std::move(loaded_map); + } + return publishLayeredMap(); + } + + bool publishLayeredMap(const ros::Time& stamp = ros::Time::now()) override { + wavemap_msgs::LayeredMap msg; + { + std::scoped_lock lock(mutex_); + if (!convert::layeredMapToRosMsg( + layered_map_, world_frame_, stamp, msg)) { + return false; + } + } + + layered_map_pub_.publish(msg); + return true; + } + + private: + void advertiseServices() { + save_layered_map_srv_ = + nh_private_.advertiseService( + "save_layered_map", [this](auto& request, auto& response) { + response.success = saveLayeredMap(request.file_path); + return true; + }); + + load_layered_map_srv_ = + nh_private_.advertiseService( + "load_layered_map", [this](auto& request, auto& response) { + std::string error_message; + response.success = loadLayeredMap(request.file_path, &error_message); + if (!response.success && !error_message.empty()) { + ROS_ERROR_STREAM(error_message); + } + return true; + }); + + request_full_layered_map_srv_ = + nh_private_.advertiseService( + "layered_map_request_full", [this](auto&, auto&) { + return publishLayeredMap(); + }); + } + + static constexpr int kQueueSize = 1; + static constexpr bool kLatchPublisher = true; + + ros::NodeHandle nh_private_; + std::string world_frame_; + + mutable std::mutex mutex_; + LayeredMapT layered_map_; + + ros::Publisher layered_map_pub_; + ros::ServiceServer load_layered_map_srv_; + ros::ServiceServer save_layered_map_srv_; + ros::ServiceServer request_full_layered_map_srv_; +}; +} // namespace wavemap + +#endif // WAVEMAP_ROS_LAYERED_ROS_SERVER_EXTENSION_H_ diff --git a/interfaces/ros1/wavemap_ros/include/wavemap_ros/ros_server.h b/interfaces/ros1/wavemap_ros/include/wavemap_ros/ros_server.h index 30c9db3dd..43bc245c2 100644 --- a/interfaces/ros1/wavemap_ros/include/wavemap_ros/ros_server.h +++ b/interfaces/ros1/wavemap_ros/include/wavemap_ros/ros_server.h @@ -18,6 +18,7 @@ #include #include "wavemap_ros/inputs/ros_input_base.h" +#include "wavemap_ros/layered_ros_server_extension.h" #include "wavemap_ros/utils/ros_logging_level.h" #include "wavemap_ros/utils/tf_transformer.h" @@ -76,6 +77,7 @@ class RosServer { // Map data structure MapBase::Ptr occupancy_map_; + std::unique_ptr layered_extension_; // Threadpool shared among all input handlers and operations std::shared_ptr thread_pool_; diff --git a/interfaces/ros1/wavemap_ros/src/ros_server.cc b/interfaces/ros1/wavemap_ros/src/ros_server.cc index d53cfec79..00183fb01 100644 --- a/interfaces/ros1/wavemap_ros/src/ros_server.cc +++ b/interfaces/ros1/wavemap_ros/src/ros_server.cc @@ -10,6 +10,8 @@ #include #include +#include "example_layered_map_ros_config.h" +#include "layered_ros_converter.h" #include "wavemap_ros/inputs/ros_input_factory.h" #include "wavemap_ros/map_operations/map_ros_operation_factory.h" @@ -108,6 +110,20 @@ RosServer::RosServer(ros::NodeHandle nh, ros::NodeHandle nh_private, // Connect to ROS advertiseServices(nh_private); + + bool enable_layered_map = false; + nh_private.param("layered_map/enabled", enable_layered_map, false); + if (enable_layered_map) { + ExampleLayeredMapConfig layered_config = + layered_map_config::loadExampleLayeredMapConfigFromRosParams(nh_private); + layered_extension_ = std::make_unique< + LayeredRosServerExtension>( + nh_private, config_.world_frame, ExampleLayeredMap(layered_config)); + ROS_INFO( + "Layered map extension enabled. Advertising layered map topic and " + "services."); + } } void RosServer::clear() { diff --git a/interfaces/ros1/wavemap_ros_conversions/include/wavemap_ros_conversions/discrete_layer_marker_conversions.h b/interfaces/ros1/wavemap_ros_conversions/include/wavemap_ros_conversions/discrete_layer_marker_conversions.h index f8439a083..efa9feae1 100644 --- a/interfaces/ros1/wavemap_ros_conversions/include/wavemap_ros_conversions/discrete_layer_marker_conversions.h +++ b/interfaces/ros1/wavemap_ros_conversions/include/wavemap_ros_conversions/discrete_layer_marker_conversions.h @@ -67,7 +67,7 @@ NamedDiscreteMarkerLayer namedDiscreteMarkerLayer( std::string name, const LayerT& layer, ColorPolicyT color_policy, LayerMarkerConfig marker_config = {}) { if (marker_config.topic.empty()) { - marker_config.topic = "/wavemap/discrete/" + name; + marker_config.topic = "/wavemap/layered_map/discrete/" + name; } if (marker_config.ns.empty()) { marker_config.ns = name; diff --git a/interfaces/ros1/wavemap_rviz_plugin/CMakeLists.txt b/interfaces/ros1/wavemap_rviz_plugin/CMakeLists.txt index b4dead116..3b97727d8 100644 --- a/interfaces/ros1/wavemap_rviz_plugin/CMakeLists.txt +++ b/interfaces/ros1/wavemap_rviz_plugin/CMakeLists.txt @@ -25,6 +25,7 @@ set(CMAKE_AUTOMOC ON) # This plugin includes Qt widgets, so we must include Qt. # We'll use the version that rviz used so they are compatible. set(HEADERS_TO_MOC + include/wavemap_rviz_plugin/layered_map_display.h include/wavemap_rviz_plugin/wavemap_map_display.h include/wavemap_rviz_plugin/visuals/voxel_visual.h include/wavemap_rviz_plugin/visuals/slice_visual.h @@ -55,6 +56,7 @@ add_library(${PROJECT_NAME} src/visuals/voxel_visual.cc src/visuals/cell_layer.cc src/visuals/cell_selector.cc + src/layered_map_display.cc src/wavemap_map_display.cc ${QT_MOC}) set_wavemap_target_properties(${PROJECT_NAME}) diff --git a/interfaces/ros1/wavemap_rviz_plugin/include/wavemap_rviz_plugin/common.h b/interfaces/ros1/wavemap_rviz_plugin/include/wavemap_rviz_plugin/common.h index c7b2df946..3840906da 100644 --- a/interfaces/ros1/wavemap_rviz_plugin/include/wavemap_rviz_plugin/common.h +++ b/interfaces/ros1/wavemap_rviz_plugin/include/wavemap_rviz_plugin/common.h @@ -7,6 +7,7 @@ #include #include +#include #include #include "wavemap_rviz_plugin/layered_map_interface.h" @@ -16,6 +17,8 @@ struct MapAndMutex { MapBase::Ptr map; std::shared_ptr layered_map; std::optional layered_map_msg; + std::optional discrete_layer_msg; + FloatingPoint discrete_layer_min_cell_width = 0.1f; std::string selected_layer_name = "occupancy"; std::mutex mutex; }; diff --git a/interfaces/ros1/wavemap_rviz_plugin/include/wavemap_rviz_plugin/layered_map_display.h b/interfaces/ros1/wavemap_rviz_plugin/include/wavemap_rviz_plugin/layered_map_display.h new file mode 100644 index 000000000..7fe94fd0a --- /dev/null +++ b/interfaces/ros1/wavemap_rviz_plugin/include/wavemap_rviz_plugin/layered_map_display.h @@ -0,0 +1,116 @@ +#ifndef WAVEMAP_RVIZ_PLUGIN_LAYERED_MAP_DISPLAY_H_ +#define WAVEMAP_RVIZ_PLUGIN_LAYERED_MAP_DISPLAY_H_ + +#ifndef Q_MOC_RUN +#include +#include +#include +#include + +#include + +#include +#include +#include +#include + +#include "wavemap_rviz_plugin/common.h" +#include "wavemap_rviz_plugin/utils/button_property.h" +#include "wavemap_rviz_plugin/visuals/voxel_visual.h" +#endif + +namespace Ogre { +class OverlayElement; +class TextAreaOverlayElement; +} + +namespace wavemap::rviz_plugin { +struct LayeredMapSourceMode : public TypeSelector { + using TypeSelector::TypeSelector; + + enum Id : TypeId { kFromTopic, kFromFile }; + + static constexpr std::array names = {"Topic", "File"}; +}; + +class LayeredMapDisplay + : public rviz::MessageFilterDisplay { + Q_OBJECT + public: // NOLINT + LayeredMapDisplay(); + ~LayeredMapDisplay() override; + + protected: + void onInitialize() override; + void reset() override; + + private Q_SLOTS: // NOLINT + void updateSourceModeCallback(); + void loadMapFromDiskCallback(); + void updateLayerSelectionCallback(); + + private: + struct DisplayLayer { + enum class Source { kContinuous, kDiscrete }; + + std::string name; + std::string type; + Source source = Source::kContinuous; + }; + + std::vector available_layers_; + LayeredMapSourceMode source_mode_ = LayeredMapSourceMode::kFromTopic; + std::string selected_layer_name_ = "occupancy"; + std::optional latest_msg_; + + const std::shared_ptr map_and_mutex_ = + std::make_shared(); + std::unique_ptr voxel_visual_; + + rviz::EnumProperty source_mode_property_{"Source", "", + "Where to load the layered map from.", + this, + SLOT(updateSourceModeCallback())}; + ButtonProperty load_map_from_disk_property_{ + "Loaded map", + "Choose file", + "Open a file dialog to choose and load a layered map from disk.", + this, + SLOT(loadMapFromDiskCallback()), + this}; + rviz::Property voxel_visual_properties_{"Render voxels", QVariant(), + "Properties for voxel rendering.", + this}; + rviz::EnumProperty layer_property_{ + "Layer", "occupancy", + "Layer to display. Populated from the received LayeredMap message.", + &voxel_visual_properties_, SLOT(updateLayerSelectionCallback()), this}; + void processMessage(const wavemap_msgs::LayeredMap::ConstPtr& msg) override; + bool loadMapFromDisk(const std::filesystem::path& filepath, + std::string* error_message = nullptr); + void displayMessage(const wavemap_msgs::LayeredMap& msg); + void clearStoredMap(); + void updateAvailableLayers(const wavemap_msgs::LayeredMap& msg); + void updateStoredMapForSelectedLayer(const wavemap_msgs::LayeredMap& msg); + void updateLegendOverlay(const wavemap_msgs::LayeredMap* msg = nullptr); + void initializeLegendOverlay(); + void destroyLegendOverlay(); + void clearLegendOverlayElements(); + std::string makeLegendMaterial(const Ogre::ColourValue& color, + const std::string& name_hint); + void addLegendText(const std::string& text, float x, float y, + float char_height, const Ogre::ColourValue& color); + void addLegendRow(const Ogre::ColourValue& color, const std::string& text, + float y); + const DisplayLayer* selectedLayer() const; + + Ogre::Overlay* legend_overlay_ = nullptr; + Ogre::OverlayContainer* legend_panel_ = nullptr; + std::vector legend_elements_; + std::vector legend_material_names_; + std::vector legend_texture_names_; + size_t legend_element_counter_ = 0u; +}; +} // namespace wavemap::rviz_plugin + +#endif // WAVEMAP_RVIZ_PLUGIN_LAYERED_MAP_DISPLAY_H_ diff --git a/interfaces/ros1/wavemap_rviz_plugin/include/wavemap_rviz_plugin/visuals/voxel_visual.h b/interfaces/ros1/wavemap_rviz_plugin/include/wavemap_rviz_plugin/visuals/voxel_visual.h index 8d4870ffe..6dab6a784 100644 --- a/interfaces/ros1/wavemap_rviz_plugin/include/wavemap_rviz_plugin/visuals/voxel_visual.h +++ b/interfaces/ros1/wavemap_rviz_plugin/include/wavemap_rviz_plugin/visuals/voxel_visual.h @@ -57,7 +57,7 @@ class VoxelVisual : public QObject { void updateMap(bool redraw_all = false); - void clear() { block_voxel_layers_map_.clear(); } + void clear(); // Set the pose of the coordinate frame the message refers to void setFramePosition(const Ogre::Vector3& position); @@ -71,10 +71,14 @@ class VoxelVisual : public QObject { void opacityUpdateCallback(); void colorModeUpdateCallback(); void flatColorUpdateCallback(); + void layerColorUpdateCallback(); private: VoxelColorMode voxel_color_mode_ = VoxelColorMode::kHeight; Ogre::ColourValue voxel_flat_color_ = Ogre::ColourValue::Blue; + Ogre::ColourValue scalar_low_color_ = Ogre::ColourValue(0.f, 0.f, 1.f); + Ogre::ColourValue scalar_high_color_ = Ogre::ColourValue(1.f, 1.f, 0.f); + Ogre::ColourValue bool_true_color_ = Ogre::ColourValue(1.f, 0.05f, 0.05f); // Shared pointer to the map, owned by WavemapMapDisplay const std::shared_ptr map_and_mutex_; @@ -97,6 +101,13 @@ class VoxelVisual : public QObject { rviz::FloatProperty opacity_property_; rviz::EnumProperty color_mode_property_; rviz::ColorProperty flat_color_property_; + rviz::Property layer_color_properties_; + rviz::FloatProperty scalar_min_property_; + rviz::FloatProperty scalar_max_property_; + rviz::ColorProperty scalar_low_color_property_; + rviz::ColorProperty scalar_high_color_property_; + rviz::ColorProperty bool_true_color_property_; + rviz::BoolProperty show_bool_false_property_; // Frame-rate stats rviz::Property frame_rate_properties_; rviz::IntProperty num_queued_blocks_indicator_; @@ -146,6 +157,16 @@ class VoxelVisual : public QObject { void drawLayeredMapOccupancy(const LayeredMapInterface& layered_map); void drawLayeredMapOccupancy( const wavemap_msgs::LayeredHashedWaveletOctree& layered_map_msg); + bool getGenericContinuousLayerColor( + const wavemap_msgs::LayeredHashedWaveletOctree& layered_map_msg, + const std::vector>& layer_values, + Ogre::ColourValue& color) const; + void drawDiscreteLayer(const wavemap_msgs::DiscreteLayer& discrete_layer_msg, + FloatingPoint min_cell_width); + bool getDiscreteCellColor(const wavemap_msgs::DiscreteLayer& layer_msg, + const wavemap_msgs::DiscreteLayerCell& cell_msg, + int exception_index, bool is_exception, + Ogre::ColourValue& color) const; void appendLayeredBlockOccupancy( const wavemap_msgs::LayeredHashedWaveletOctree& layered_map_msg, const wavemap_msgs::LayeredHashedWaveletOctreeBlock& block_msg, diff --git a/interfaces/ros1/wavemap_rviz_plugin/include/wavemap_rviz_plugin/wavemap_map_display.h b/interfaces/ros1/wavemap_rviz_plugin/include/wavemap_rviz_plugin/wavemap_map_display.h index c2c250fe3..4eb826d04 100644 --- a/interfaces/ros1/wavemap_rviz_plugin/include/wavemap_rviz_plugin/wavemap_map_display.h +++ b/interfaces/ros1/wavemap_rviz_plugin/include/wavemap_rviz_plugin/wavemap_map_display.h @@ -12,7 +12,6 @@ #include #include "wavemap_rviz_plugin/common.h" -#include "wavemap_rviz_plugin/layered_map_factory.h" #include "wavemap_rviz_plugin/utils/button_property.h" #include "wavemap_rviz_plugin/visuals/slice_visual.h" #include "wavemap_rviz_plugin/visuals/voxel_visual.h" @@ -59,7 +58,6 @@ class WavemapMapDisplay : public rviz::MessageFilterDisplay { SourceMode source_mode_ = SourceMode::kFromTopic; std::vector available_layers_; std::string selected_layer_name_ = "occupancy"; - std::vector> layered_map_factories_; bool hasMap(); void clearMap(); @@ -73,8 +71,6 @@ class WavemapMapDisplay : public rviz::MessageFilterDisplay { const std::shared_ptr map_and_mutex_ = std::make_shared(); void updateMapFromRosMsg(const wavemap_msgs::Map& map_msg); - std::shared_ptr createLayeredMapFromRosMsg( - const wavemap_msgs::LayeredHashedWaveletOctree& layered_map_msg) const; void updateLayerMetadataFromRosMsg(const wavemap_msgs::Map& map_msg); // Submenus for each visual's properties diff --git a/interfaces/ros1/wavemap_rviz_plugin/plugin_description.xml b/interfaces/ros1/wavemap_rviz_plugin/plugin_description.xml index b4df01c54..10becc5cf 100644 --- a/interfaces/ros1/wavemap_rviz_plugin/plugin_description.xml +++ b/interfaces/ros1/wavemap_rviz_plugin/plugin_description.xml @@ -7,4 +7,12 @@ wavemap_msgs/Map + + + Displays layered maps with continuous and discrete voxel layers. + + wavemap_msgs/LayeredMap + diff --git a/interfaces/ros1/wavemap_rviz_plugin/src/layered_map_display.cc b/interfaces/ros1/wavemap_rviz_plugin/src/layered_map_display.cc new file mode 100644 index 000000000..be88d79b8 --- /dev/null +++ b/interfaces/ros1/wavemap_rviz_plugin/src/layered_map_display.cc @@ -0,0 +1,672 @@ +#include "wavemap_rviz_plugin/layered_map_display.h" + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "example_layered_map_ros_config.h" +#include "layered_ros_converter.h" +#include "wavemap_rviz_plugin/layered_map_factory.h" +#include "wavemap_rviz_plugin/utils/alert_dialog.h" + +namespace wavemap::rviz_plugin { +namespace { +struct ExampleContinuousLayerColorProvider { + static bool getLayerColor(const std::string& layer_name, + const LayeredVoxel& voxel, + FloatingPoint /*occupancy*/, + Ogre::ColourValue& color) { + if (layer_name == "color") { + color = Ogre::ColourValue(std::clamp(voxel.data.rgb.r, 0.f, 1.f), + std::clamp(voxel.data.rgb.g, 0.f, 1.f), + std::clamp(voxel.data.rgb.b, 0.f, 1.f), 1.f); + return true; + } + + if (layer_name == "traversability") { + const FloatingPoint t = + std::clamp(voxel.data.traversability, 0.f, 1.f); + color = Ogre::ColourValue(1.f - t, t, 0.15f, 1.f); + return true; + } + + return false; + } +}; + +using ExampleContinuousMapFactory = + TypedLayeredMapFactory; + +Ogre::ColourValue stableIntColor(int value) { + const uint32_t hash = static_cast(value) * 2654435761u; + const float r = 0.25f + + 0.75f * static_cast((hash >> 16u) & 0xffu) / 255.f; + const float g = 0.25f + + 0.75f * static_cast((hash >> 8u) & 0xffu) / 255.f; + const float b = 0.25f + 0.75f * static_cast(hash & 0xffu) / 255.f; + return Ogre::ColourValue(r, g, b, 1.f); +} + +std::set collectIntValues(const wavemap_msgs::DiscreteLayer& layer) { + std::set values; + for (const auto& cell : layer.cells) { + values.insert(cell.dominant_int32); + values.insert(cell.exception_int32_values.begin(), + cell.exception_int32_values.end()); + } + return values; +} + +std::set collectBoolValues(const wavemap_msgs::DiscreteLayer& layer) { + std::set values; + for (const auto& cell : layer.cells) { + values.insert(cell.dominant_uint8 != 0u); + for (const uint8_t exception_value : cell.exception_uint8_values) { + values.insert(exception_value != 0u); + } + } + return values; +} +} // namespace + +LayeredMapDisplay::LayeredMapDisplay() { + source_mode_property_.clearOptions(); + for (const auto& name : LayeredMapSourceMode::names) { + source_mode_property_.addOption(name); + } + source_mode_property_.setStringStd(source_mode_.toStr()); + load_map_from_disk_property_.setHidden(true); + + layer_property_.clearOptions(); + layer_property_.addOption(QString::fromStdString(selected_layer_name_)); + layer_property_.setStringStd(selected_layer_name_); +} + +LayeredMapDisplay::~LayeredMapDisplay() { + // Destroy Ogre visuals before Qt-owned RViz properties start tearing down. + voxel_visual_.reset(); + destroyLegendOverlay(); +} + +void LayeredMapDisplay::onInitialize() { + ProfilerZoneScoped; + MFDClass::onInitialize(); + + const int source_row = source_mode_property_.rowNumberInParent(); + const int topic_row = topic_property_->rowNumberInParent(); + if (0 <= topic_row && topic_row < source_row) { + moveChild(source_row, topic_row); + } + + if (topic_property_->getStdString().empty()) { + topic_property_->setString("/wavemap/layered_map"); + } + voxel_visual_ = std::make_unique( + scene_manager_, context_->getViewManager(), scene_node_, + &voxel_visual_properties_, map_and_mutex_); + initializeLegendOverlay(); +} + +void LayeredMapDisplay::reset() { + ProfilerZoneScoped; + MFDClass::reset(); + latest_msg_.reset(); + clearStoredMap(); + if (voxel_visual_) { + voxel_visual_->clear(); + } + updateLegendOverlay(nullptr); +} + +void LayeredMapDisplay::processMessage( + const wavemap_msgs::LayeredMap::ConstPtr& msg) { + ProfilerZoneScoped; + if (!msg) { + ROS_WARN("Ignoring request to process non-existent layered map msg."); + return; + } + + displayMessage(*msg); +} + +void LayeredMapDisplay::displayMessage(const wavemap_msgs::LayeredMap& msg) { + latest_msg_ = msg; + updateAvailableLayers(msg); + updateLegendOverlay(&msg); + updateStoredMapForSelectedLayer(msg); + + if (!voxel_visual_) { + ROS_WARN("Voxel visual not initialized yet, skipping layered map message."); + return; + } + + Ogre::Vector3 position; + Ogre::Quaternion orientation; + if (!context_->getFrameManager()->getTransform(msg.header.frame_id, + msg.header.stamp, + position, orientation)) { + ROS_WARN("Error transforming from frame '%s' to frame '%s'", + msg.header.frame_id.c_str(), qPrintable(fixed_frame_)); + return; + } + voxel_visual_->setFramePosition(position); + voxel_visual_->setFrameOrientation(orientation); + voxel_visual_->updateMap(true); +} + +bool LayeredMapDisplay::loadMapFromDisk( + const std::filesystem::path& filepath, std::string* error_message) { + ExampleLayeredMap map(ExampleLayeredMapConfig{}); + if (!ExampleLayeredMapIo::load(filepath, map, error_message)) { + return false; + } + + wavemap_msgs::LayeredMap msg; + const std::string frame_id = fixed_frame_.isEmpty() ? "map" : fixed_frame_.toStdString(); + if (!convert::layeredMapToRosMsg( + map, frame_id, ros::Time(0), msg)) { + return false; + } + + displayMessage(msg); + return true; +} + +void LayeredMapDisplay::clearStoredMap() { + updateLegendOverlay(nullptr); + std::scoped_lock lock(map_and_mutex_->mutex); + map_and_mutex_->map.reset(); + map_and_mutex_->layered_map.reset(); + map_and_mutex_->layered_map_msg.reset(); + map_and_mutex_->discrete_layer_msg.reset(); +} + +void LayeredMapDisplay::updateAvailableLayers( + const wavemap_msgs::LayeredMap& msg) { + available_layers_.clear(); + available_layers_.push_back( + {"occupancy", "float32", DisplayLayer::Source::kContinuous}); + + if (msg.continuous_map.layered_hashed_wavelet_octree.size() == 1u) { + const auto& continuous_msg = + msg.continuous_map.layered_hashed_wavelet_octree.front(); + const size_t num_layers = std::min(continuous_msg.layer_names.size(), + continuous_msg.layer_types.size()); + for (size_t layer_i = 0u; layer_i < num_layers; ++layer_i) { + available_layers_.push_back({continuous_msg.layer_names[layer_i], + continuous_msg.layer_types[layer_i], + DisplayLayer::Source::kContinuous}); + } + } + + for (const auto& discrete_layer : msg.discrete_layers) { + available_layers_.push_back({discrete_layer.name, discrete_layer.value_type, + DisplayLayer::Source::kDiscrete}); + } + + const bool selected_still_exists = + std::any_of(available_layers_.begin(), available_layers_.end(), + [this](const DisplayLayer& layer) { + return layer.name == selected_layer_name_; + }); + if (!selected_still_exists) { + selected_layer_name_ = available_layers_.empty() ? "occupancy" + : available_layers_.front().name; + } + + layer_property_.clearOptions(); + for (const DisplayLayer& layer : available_layers_) { + layer_property_.addOption(QString::fromStdString(layer.name)); + } + layer_property_.setStringStd(selected_layer_name_); +} + +const LayeredMapDisplay::DisplayLayer* LayeredMapDisplay::selectedLayer() + const { + const auto layer_it = std::find_if( + available_layers_.begin(), available_layers_.end(), + [this](const DisplayLayer& layer) { return layer.name == selected_layer_name_; }); + return layer_it == available_layers_.end() ? nullptr : &*layer_it; +} + +void LayeredMapDisplay::updateStoredMapForSelectedLayer( + const wavemap_msgs::LayeredMap& msg) { + const DisplayLayer* layer = selectedLayer(); + const bool selected_is_discrete = + layer && layer->source == DisplayLayer::Source::kDiscrete; + + std::scoped_lock lock(map_and_mutex_->mutex); + map_and_mutex_->map.reset(); + map_and_mutex_->layered_map.reset(); + map_and_mutex_->layered_map_msg.reset(); + map_and_mutex_->discrete_layer_msg.reset(); + map_and_mutex_->selected_layer_name = selected_layer_name_; + + if (selected_is_discrete) { + const auto discrete_it = std::find_if( + msg.discrete_layers.begin(), msg.discrete_layers.end(), + [this](const wavemap_msgs::DiscreteLayer& discrete_layer) { + return discrete_layer.name == selected_layer_name_; + }); + if (discrete_it != msg.discrete_layers.end()) { + map_and_mutex_->discrete_layer_msg = *discrete_it; + if (msg.continuous_map.layered_hashed_wavelet_octree.size() == 1u) { + map_and_mutex_->discrete_layer_min_cell_width = + msg.continuous_map.layered_hashed_wavelet_octree.front().min_cell_width; + } + } + return; + } + + if (msg.continuous_map.layered_hashed_wavelet_octree.size() == 1u) { + const auto& continuous_msg = + msg.continuous_map.layered_hashed_wavelet_octree.front(); + if (auto typed_map = ExampleContinuousMapFactory{}.tryCreate(continuous_msg)) { + map_and_mutex_->layered_map = std::move(typed_map); + return; + } + + map_and_mutex_->layered_map_msg = continuous_msg; + return; + } + + MapBase::Ptr map; + if (!convert::rosMsgToMap(msg.continuous_map, map)) { + ROS_WARN("Failed to parse continuous map inside LayeredMap message."); + return; + } + map_and_mutex_->map = std::move(map); +} + +void LayeredMapDisplay::initializeLegendOverlay() { + if (legend_overlay_) { + return; + } + + auto& overlay_manager = Ogre::OverlayManager::getSingleton(); + const std::string name_suffix = std::to_string(reinterpret_cast(this)); + legend_overlay_ = overlay_manager.create("LayeredMapLegendOverlay_" + name_suffix); + legend_overlay_->setZOrder(650); + + legend_panel_ = static_cast( + overlay_manager.createOverlayElement( + "Panel", "LayeredMapLegendPanel_" + name_suffix)); + legend_panel_->setMetricsMode(Ogre::GMM_PIXELS); + legend_panel_->setHorizontalAlignment(Ogre::GHA_RIGHT); + legend_panel_->setVerticalAlignment(Ogre::GVA_TOP); + legend_panel_->setPosition(-430.f, 18.f); + legend_panel_->setDimensions(410.f, 150.f); + legend_panel_->setMaterialName(makeLegendMaterial( + Ogre::ColourValue(0.92f, 0.94f, 0.96f, 0.88f), "background")); + + legend_overlay_->add2D(legend_panel_); + legend_overlay_->show(); + updateLegendOverlay(nullptr); +} + +void LayeredMapDisplay::clearLegendOverlayElements() { + if (!legend_panel_) { + return; + } + + auto& overlay_manager = Ogre::OverlayManager::getSingleton(); + for (Ogre::OverlayElement* element : legend_elements_) { + if (!element) { + continue; + } + legend_panel_->removeChild(element->getName()); + overlay_manager.destroyOverlayElement(element); + } + legend_elements_.clear(); +} + +void LayeredMapDisplay::destroyLegendOverlay() { + auto& overlay_manager = Ogre::OverlayManager::getSingleton(); + clearLegendOverlayElements(); + + if (legend_overlay_ && legend_panel_) { + legend_overlay_->remove2D(legend_panel_); + overlay_manager.destroyOverlayElement(legend_panel_); + legend_panel_ = nullptr; + } + + if (legend_overlay_) { + overlay_manager.destroy(legend_overlay_); + legend_overlay_ = nullptr; + } + + auto& material_manager = Ogre::MaterialManager::getSingleton(); + for (const std::string& material_name : legend_material_names_) { + if (material_manager.resourceExists(material_name)) { + material_manager.remove(material_name); + } + } + legend_material_names_.clear(); + + auto& texture_manager = Ogre::TextureManager::getSingleton(); + for (const std::string& texture_name : legend_texture_names_) { + if (texture_manager.resourceExists(texture_name)) { + texture_manager.remove(texture_name); + } + } + legend_texture_names_.clear(); +} + +std::string LayeredMapDisplay::makeLegendMaterial( + const Ogre::ColourValue& color, const std::string& name_hint) { + std::ostringstream material_name; + material_name << "LayeredMapLegend_" << name_hint << "_" + << reinterpret_cast(this) << "_" + << legend_element_counter_++; + + Ogre::TexturePtr texture; + std::string texture_name; + if (name_hint != "background") { + std::ostringstream texture_name_stream; + texture_name_stream << "LayeredMapLegendTexture_" << name_hint << "_" + << reinterpret_cast(this) << "_" + << legend_element_counter_++; + texture_name = texture_name_stream.str(); + texture = Ogre::TextureManager::getSingleton().createManual( + texture_name, Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME, + Ogre::TEX_TYPE_2D, 1u, 1u, 0u, Ogre::PF_A8R8G8B8, + Ogre::TU_DYNAMIC_WRITE_ONLY); + + const auto to_byte = [](float value) { + return static_cast( + std::clamp(static_cast(std::round(value * 255.f)), 0, 255)); + }; + std::array pixel = {to_byte(color.b), to_byte(color.g), + to_byte(color.r), to_byte(color.a)}; + Ogre::PixelBox pixel_box(1u, 1u, 1u, Ogre::PF_A8R8G8B8, pixel.data()); + texture->getBuffer()->blitFromMemory(pixel_box); + legend_texture_names_.push_back(texture_name); + } + + Ogre::MaterialPtr material = Ogre::MaterialManager::getSingleton().create( + material_name.str(), Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME); + material->setLightingEnabled(false); + material->setDepthCheckEnabled(false); + material->setDepthWriteEnabled(false); + material->setSceneBlending(Ogre::SBT_TRANSPARENT_ALPHA); + Ogre::Pass* pass = material->getTechnique(0)->getPass(0); + pass->setAmbient(color); + pass->setDiffuse(color); + pass->setSelfIllumination(color); + if (!texture.isNull()) { + pass->createTextureUnitState(texture_name); + } + + legend_material_names_.push_back(material_name.str()); + return material_name.str(); +} + +void LayeredMapDisplay::addLegendText(const std::string& text, float x, float y, + float char_height, + const Ogre::ColourValue& color) { + if (!legend_panel_) { + return; + } + + auto& overlay_manager = Ogre::OverlayManager::getSingleton(); + std::ostringstream element_name; + element_name << "LayeredMapLegendText_" << reinterpret_cast(this) + << "_" << legend_element_counter_++; + + auto* text_element = static_cast( + overlay_manager.createOverlayElement("TextArea", element_name.str())); + text_element->setMetricsMode(Ogre::GMM_PIXELS); + text_element->setPosition(x, y); + text_element->setDimensions(380.f - x, 38.f); + text_element->setCharHeight(char_height); + text_element->setFontName("Liberation Sans"); + text_element->setColour(color); + text_element->setCaption(text); + + legend_panel_->addChild(text_element); + legend_elements_.push_back(text_element); +} + +void LayeredMapDisplay::addLegendRow(const Ogre::ColourValue& color, + const std::string& text, float y) { + if (!legend_panel_) { + return; + } + + auto& overlay_manager = Ogre::OverlayManager::getSingleton(); + std::ostringstream swatch_name; + swatch_name << "LayeredMapLegendSwatch_" << reinterpret_cast(this) + << "_" << legend_element_counter_++; + + auto* swatch = static_cast( + overlay_manager.createOverlayElement("Panel", swatch_name.str())); + swatch->setMetricsMode(Ogre::GMM_PIXELS); + constexpr float kRowHeight = 24.f; + constexpr float kSwatchSize = 24.f; + swatch->setPosition(24.f, y + 0.5f * (kRowHeight - kSwatchSize)); + swatch->setDimensions(kSwatchSize, kSwatchSize); + swatch->setMaterialName(makeLegendMaterial(color, "swatch")); + + legend_panel_->addChild(swatch); + legend_elements_.push_back(swatch); + + addLegendText(text, 62.f, y + 2.f, 24.f, + Ogre::ColourValue(0.05f, 0.06f, 0.07f, 1.f)); +} + +void LayeredMapDisplay::updateLegendOverlay(const wavemap_msgs::LayeredMap* msg) { + if (!legend_panel_ || !legend_overlay_) { + return; + } + + clearLegendOverlayElements(); + + struct LegendRow { + Ogre::ColourValue color; + std::string text; + }; + + const DisplayLayer* layer = selectedLayer(); + if (!layer) { + legend_panel_->setDimensions(410.f, 128.f); + addLegendText("Layered map", 22.f, 18.f, 28.f, + Ogre::ColourValue(0.02f, 0.025f, 0.03f, 1.f)); + addLegendText("No layer selected", 22.f, 68.f, 22.f, + Ogre::ColourValue(0.18f, 0.20f, 0.24f, 1.f)); + legend_overlay_->show(); + return; + } + + std::vector rows; + std::string subtitle; + + if (layer->source == DisplayLayer::Source::kContinuous) { + if (layer->name == "occupancy") { + subtitle = "occupancy / log-odds"; + rows.push_back({Ogre::ColourValue(0.12f, 0.32f, 0.95f, 1.f), + "lower probability"}); + rows.push_back({Ogre::ColourValue(0.95f, 0.18f, 0.10f, 1.f), + "higher probability"}); + } else if (layer->type == "float32_rgb" || layer->type == "float32_vec3") { + subtitle = "direct RGB color"; + rows.push_back({Ogre::ColourValue(1.f, 0.f, 0.f, 1.f), "red channel"}); + rows.push_back({Ogre::ColourValue(0.f, 1.f, 0.f, 1.f), "green channel"}); + rows.push_back({Ogre::ColourValue(0.f, 0.f, 1.f, 1.f), "blue channel"}); + } else if (layer->type == "float32") { + subtitle = "scalar value"; + rows.push_back({Ogre::ColourValue(1.f, 0.f, 0.15f, 1.f), "0.0"}); + rows.push_back({Ogre::ColourValue(0.f, 1.f, 0.15f, 1.f), "1.0"}); + } else { + subtitle = "No legend rule for type " + layer->type; + } + } else if (!msg) { + subtitle = "No discrete data loaded"; + } else { + const auto layer_it = std::find_if( + msg->discrete_layers.begin(), msg->discrete_layers.end(), + [this](const wavemap_msgs::DiscreteLayer& discrete_layer) { + return discrete_layer.name == selected_layer_name_; + }); + if (layer_it == msg->discrete_layers.end()) { + subtitle = "Layer missing in map"; + } else if (layer_it->value_type == "bool") { + subtitle = "boolean values"; + const std::set values = collectBoolValues(*layer_it); + if (values.count(true)) { + rows.push_back({Ogre::ColourValue(1.f, 0.05f, 0.05f, 1.f), "true"}); + } + if (values.count(false)) { + rows.push_back({Ogre::ColourValue(0.45f, 0.45f, 0.45f, 1.f), + "false hidden by default"}); + } + if (values.empty()) { + subtitle = "No observed values"; + } + } else if (layer_it->value_type == "int") { + subtitle = "integer values"; + const std::set values = collectIntValues(*layer_it); + constexpr size_t kMaxLegendEntries = 10u; + size_t entry_count = 0u; + for (const int value : values) { + if (kMaxLegendEntries <= entry_count) { + rows.push_back({Ogre::ColourValue(0.70f, 0.70f, 0.70f, 1.f), + "+" + std::to_string(values.size() - entry_count) + + " more"}); + break; + } + rows.push_back({stableIntColor(value), std::to_string(value)}); + ++entry_count; + } + if (values.empty()) { + subtitle = "No observed values"; + } + } else { + subtitle = "No legend rule for type " + layer_it->value_type; + } + } + + constexpr size_t kMaxVisibleRows = 10u; + const size_t num_visible_rows = std::min(rows.size(), kMaxVisibleRows); + constexpr float kLegendRowHeight = 40.f; + const float panel_height = std::max(136.f + kLegendRowHeight * + static_cast(num_visible_rows), + 170.f); + legend_panel_->setDimensions(410.f, panel_height); + + addLegendText("Layer: " + layer->name, 22.f, 20.f, 28.f, + Ogre::ColourValue(0.02f, 0.025f, 0.03f, 1.f)); + addLegendText(subtitle, 22.f, 70.f, 21.f, + Ogre::ColourValue(0.22f, 0.24f, 0.28f, 1.f)); + + float row_y = 108.f; + size_t visible_row_count = 0u; + for (const LegendRow& row : rows) { + if (kMaxVisibleRows <= visible_row_count) { + break; + } + addLegendRow(row.color, row.text, row_y); + row_y += kLegendRowHeight; + ++visible_row_count; + } + + legend_overlay_->show(); +} + +void LayeredMapDisplay::updateSourceModeCallback() { + ProfilerZoneScoped; + const LayeredMapSourceMode old_source_mode = source_mode_; + source_mode_ = LayeredMapSourceMode(source_mode_property_.getStdString()); + + unreliable_property_->setHidden(source_mode_ != LayeredMapSourceMode::kFromTopic); + queue_size_property_->setHidden(source_mode_ != LayeredMapSourceMode::kFromTopic); + topic_property_->setHidden(source_mode_ != LayeredMapSourceMode::kFromTopic); + + load_map_from_disk_property_.setHidden(source_mode_ != LayeredMapSourceMode::kFromFile); + load_map_from_disk_property_.resetAllValues(); + + if (source_mode_ != old_source_mode) { + if (source_mode_ == LayeredMapSourceMode::kFromTopic) { + updateTopic(); + } else { + unsubscribe(); + } + latest_msg_.reset(); + clearStoredMap(); + if (voxel_visual_) { + voxel_visual_->clear(); + } + } +} + +void LayeredMapDisplay::loadMapFromDiskCallback() { + ProfilerZoneScoped; + const auto filepath_qt = QFileDialog::getOpenFileName( + nullptr, "Open layered wavemap", QString(), + "Layered wavemap files (*.lwvmp *.lwmp);;All files (*)"); + + if (filepath_qt.isEmpty()) { + load_map_from_disk_property_.resetAllValues(); + return; + } + + const std::filesystem::path filepath{filepath_qt.toStdString()}; + std::string load_error; + if (!loadMapFromDisk(filepath, &load_error)) { + if (load_error.empty()) { + load_error = "This display currently supports layered map files that " + "match the registered ExampleLayeredMap schema."; + } + AlertDialog alert{ + "Could not load layered map", + std::string("Failed to load:\n\"") + filepath.string() + + "\"\n\n" + load_error}; + alert.exec(); + load_map_from_disk_property_.resetAllValues(); + return; + } + + load_map_from_disk_property_.setAtRestValue(filepath.filename().string()); +} + +void LayeredMapDisplay::updateLayerSelectionCallback() { + ProfilerZoneScoped; + selected_layer_name_ = layer_property_.getStdString(); + if (latest_msg_) { + updateLegendOverlay(&latest_msg_.value()); + updateStoredMapForSelectedLayer(latest_msg_.value()); + } else { + updateLegendOverlay(nullptr); + } + if (voxel_visual_) { + voxel_visual_->clear(); + voxel_visual_->updateMap(true); + } +} +} // namespace wavemap::rviz_plugin + +PLUGINLIB_EXPORT_CLASS(wavemap::rviz_plugin::LayeredMapDisplay, rviz::Display) 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 f241d2cbc..1b900bab1 100644 --- a/interfaces/ros1/wavemap_rviz_plugin/src/visuals/voxel_visual.cc +++ b/interfaces/ros1/wavemap_rviz_plugin/src/visuals/voxel_visual.cc @@ -1,13 +1,17 @@ #include "wavemap_rviz_plugin/visuals/voxel_visual.h" #include +#include #include #include #include #include +#include #include #include +#include + #include #include #include @@ -46,6 +50,33 @@ VoxelVisual::VoxelVisual(Ogre::SceneManager* scene_manager, "Flat color", rviz::ogreToQt(voxel_flat_color_), R"(Solid color to use when "Color Mode" is set to "Flat")", submenu_root_property, SLOT(flatColorUpdateCallback()), this), + layer_color_properties_("Layer color", QVariant(), + "Generic color settings for layered map fields.", + submenu_root_property), + scalar_min_property_("Scalar min", 0.0, + "Lower value used when coloring scalar continuous layers.", + &layer_color_properties_, + SLOT(layerColorUpdateCallback()), this), + scalar_max_property_("Scalar max", 1.0, + "Upper value used when coloring scalar continuous layers.", + &layer_color_properties_, + SLOT(layerColorUpdateCallback()), this), + scalar_low_color_property_( + "Scalar low color", rviz::ogreToQt(scalar_low_color_), + "Color used for the scalar minimum.", &layer_color_properties_, + SLOT(layerColorUpdateCallback()), this), + scalar_high_color_property_( + "Scalar high color", rviz::ogreToQt(scalar_high_color_), + "Color used for the scalar maximum.", &layer_color_properties_, + SLOT(layerColorUpdateCallback()), this), + bool_true_color_property_( + "Bool true color", rviz::ogreToQt(bool_true_color_), + "Color used for true values in boolean discrete layers.", + &layer_color_properties_, SLOT(layerColorUpdateCallback()), this), + show_bool_false_property_( + "Show bool false", false, + "Whether to draw false values in boolean discrete layers.", + &layer_color_properties_, SLOT(layerColorUpdateCallback()), this), frame_rate_properties_("Frame rate", QVariant(), "Properties to control the frame rate.", submenu_root_property), @@ -97,10 +128,28 @@ VoxelVisual::~VoxelVisual() { // NOTE: This must be done before any of the objects that are used in the // prerender callback (incl. members of VoxelVisual) are destroyed. prerender_listener_.reset(); + clear(); // Destroy the frame node scene_manager_->destroySceneNode(frame_node_); } +void VoxelVisual::clear() { + block_update_queue_.clear(); + for (auto& [block_index, voxel_layers] : block_voxel_layers_map_) { + (void)block_index; + for (auto& voxel_layer : voxel_layers) { + if (!voxel_layer) { + continue; + } + voxel_layer->clear(); + if (voxel_layer->getParentSceneNode()) { + voxel_layer->getParentSceneNode()->detachObject(voxel_layer.get()); + } + } + } + block_voxel_layers_map_.clear(); +} + void VoxelVisual::updateMap(bool redraw_all) { ProfilerZoneScoped; if (!visibility_property_.getBool()) { @@ -112,7 +161,10 @@ void VoxelVisual::updateMap(bool redraw_all) { std::scoped_lock lock(map_and_mutex_->mutex); MapBase::ConstPtr map = map_and_mutex_->map; if (!map) { - if (map_and_mutex_->layered_map) { + if (map_and_mutex_->discrete_layer_msg) { + drawDiscreteLayer(map_and_mutex_->discrete_layer_msg.value(), + map_and_mutex_->discrete_layer_min_cell_width); + } else if (map_and_mutex_->layered_map) { cell_selector_.setOccupancyQuery(map_and_mutex_->layered_map); drawLayeredMapOccupancy(*map_and_mutex_->layered_map); } else if (map_and_mutex_->layered_map_msg) { @@ -340,6 +392,14 @@ void VoxelVisual::flatColorUpdateCallback() { } } +void VoxelVisual::layerColorUpdateCallback() { + ProfilerZoneScoped; + scalar_low_color_ = scalar_low_color_property_.getOgreColor(); + scalar_high_color_ = scalar_high_color_property_.getOgreColor(); + bool_true_color_ = bool_true_color_property_.getOgreColor(); + updateMap(true); +} + void VoxelVisual::appendLeafCenterAndColor(int tree_height, FloatingPoint min_cell_width, const OctreeIndex& cell_index, @@ -507,7 +567,8 @@ void VoxelVisual::drawLayeredMapOccupancy(const wavemap_msgs::LayeredHashedWavel const IndexElement tree_height = layered_map_msg.tree_height; const FloatingPoint min_cell_width = layered_map_msg.min_cell_width; const FloatingPoint alpha = opacity_property_.getFloat(); - const IndexElement termination_height = termination_height_property_.getInt(); + const IndexElement termination_height = std::min( + termination_height_property_.getInt(), tree_height); termination_height_property_.setMax(tree_height); block_update_queue_.clear(); @@ -525,7 +586,6 @@ void VoxelVisual::drawLayeredMapOccupancy(const wavemap_msgs::LayeredHashedWavel } } - // Draw the layered map using only the occupancy coefficients. Layer-specific color mapping is added in the next phase. for (const auto& block_msg : layered_map_msg.blocks) { const Index3D block_index{block_msg.root_node_offset.x, block_msg.root_node_offset.y, block_msg.root_node_offset.z}; const int num_levels = tree_height + 1 - termination_height; @@ -537,6 +597,229 @@ void VoxelVisual::drawLayeredMapOccupancy(const wavemap_msgs::LayeredHashedWavel num_queued_blocks_indicator_.setInt(0); } +namespace { +Ogre::ColourValue stableIntColor(int value) { + uint32_t hash = static_cast(value) * 2654435761u; + const float r = 0.25f + 0.75f * static_cast((hash >> 16u) & 0xffu) / 255.f; + const float g = 0.25f + 0.75f * static_cast((hash >> 8u) & 0xffu) / 255.f; + const float b = 0.25f + 0.75f * static_cast(hash & 0xffu) / 255.f; + return Ogre::ColourValue(r, g, b, 1.f); +} + +int continuousLayerComponentCount(const std::string& type) { + if (type == "float32_rgb" || type == "float32_vec3") { + return 3; + } + if (type == "float32") { + return 1; + } + return 0; +} + +FloatingPoint readFloatLayerValue(const wavemap_msgs::Layer& layer, + size_t value_index, + int component_count, int component_index) { + const size_t array_index = value_index * static_cast(component_count) + + static_cast(component_index); + if (array_index < layer.float32_values.size()) { + return layer.float32_values[array_index]; + } + return 0.f; +} + +Ogre::ColourValue interpolateColor(const Ogre::ColourValue& low, + const Ogre::ColourValue& high, + FloatingPoint t) { + t = std::clamp(t, 0.f, 1.f); + return Ogre::ColourValue(low.r + t * (high.r - low.r), + low.g + t * (high.g - low.g), + low.b + t * (high.b - low.b), 1.f); +} +} // namespace + +bool VoxelVisual::getGenericContinuousLayerColor( + const wavemap_msgs::LayeredHashedWaveletOctree& layered_map_msg, + const std::vector>& layer_values, + Ogre::ColourValue& color) const { + const std::string selected_layer_name = map_and_mutex_->selected_layer_name; + const auto layer_name_it = std::find(layered_map_msg.layer_names.begin(), + layered_map_msg.layer_names.end(), + selected_layer_name); + if (layer_name_it == layered_map_msg.layer_names.end()) { + return false; + } + + const size_t layer_index = static_cast( + std::distance(layered_map_msg.layer_names.begin(), layer_name_it)); + if (layer_values.size() <= layer_index || + layered_map_msg.layer_types.size() <= layer_index) { + return false; + } + + const std::string& layer_type = layered_map_msg.layer_types[layer_index]; + const auto& values = layer_values[layer_index]; + if ((layer_type == "float32_rgb" || layer_type == "float32_vec3") && + values.size() >= 3u) { + color = Ogre::ColourValue(std::clamp(values[0], 0.f, 1.f), + std::clamp(values[1], 0.f, 1.f), + std::clamp(values[2], 0.f, 1.f), 1.f); + return true; + } + + if (layer_type == "float32" && !values.empty()) { + const FloatingPoint min_value = scalar_min_property_.getFloat(); + const FloatingPoint max_value = scalar_max_property_.getFloat(); + const FloatingPoint range = max_value - min_value; + const FloatingPoint t = std::abs(range) < kEpsilon + ? 0.f + : (values.front() - min_value) / range; + color = interpolateColor(scalar_low_color_, scalar_high_color_, t); + return true; + } + + return false; +} + +void VoxelVisual::drawDiscreteLayer( + const wavemap_msgs::DiscreteLayer& discrete_layer_msg, + FloatingPoint min_cell_width) { + ProfilerZoneScoped; + const FloatingPoint alpha = opacity_property_.getFloat(); + const int side_length = 1 << discrete_layer_msg.block_height; + const IndexElement termination_height = std::min( + termination_height_property_.getInt(), discrete_layer_msg.block_height); + const bool draw_parent_cells = + discrete_layer_msg.block_height <= termination_height; + + termination_height_property_.setMax(discrete_layer_msg.block_height); + block_update_queue_.clear(); + + std::unordered_map, Index3DHash> cells_by_parent; + std::unordered_set allocated_parents; + for (const auto& cell_msg : discrete_layer_msg.cells) { + const Index3D parent_index{cell_msg.parent_index.x, cell_msg.parent_index.y, + cell_msg.parent_index.z}; + allocated_parents.insert(parent_index); + auto& cells = cells_by_parent[parent_index]; + + if (draw_parent_cells) { + Ogre::ColourValue color; + if (!getDiscreteCellColor(discrete_layer_msg, cell_msg, + /*exception_index=*/-1, + /*is_exception=*/false, color)) { + continue; + } + + auto& cell = cells.emplace_back(); + const FloatingPoint parent_width = + static_cast(side_length) * min_cell_width; + cell.center = Ogre::Vector3( + (static_cast(parent_index.x()) + 0.5f) * parent_width, + (static_cast(parent_index.y()) + 0.5f) * parent_width, + (static_cast(parent_index.z()) + 0.5f) * parent_width); + cell.color = color; + continue; + } + + std::unordered_map exception_indices; + for (size_t exception_i = 0u; + exception_i < cell_msg.exception_offsets.size(); ++exception_i) { + exception_indices[cell_msg.exception_offsets[exception_i]] = + static_cast(exception_i); + } + + for (const int offset : cell_msg.observed_offsets) { + const auto exception_it = exception_indices.find(offset); + const bool is_exception = exception_it != exception_indices.end(); + const int exception_index = is_exception ? exception_it->second : -1; + + Ogre::ColourValue color; + if (!getDiscreteCellColor(discrete_layer_msg, cell_msg, exception_index, + is_exception, color)) { + continue; + } + + const int local_x = offset % side_length; + const int local_y = (offset / side_length) % side_length; + const int local_z = offset / (side_length * side_length); + const FloatingPoint x = + (cell_msg.parent_index.x * side_length + local_x + 0.5f) * + min_cell_width; + const FloatingPoint y = + (cell_msg.parent_index.y * side_length + local_y + 0.5f) * + min_cell_width; + const FloatingPoint z = + (cell_msg.parent_index.z * side_length + local_z + 0.5f) * + min_cell_width; + + auto& cell = cells.emplace_back(); + cell.center = Ogre::Vector3(x, y, z); + cell.color = color; + } + } + + for (auto it = block_voxel_layers_map_.begin(); + it != block_voxel_layers_map_.end();) { + if (!allocated_parents.count(it->first)) { + it = block_voxel_layers_map_.erase(it); + } else { + ++it; + } + } + + const FloatingPoint cell_width = + draw_parent_cells ? static_cast(side_length) * min_cell_width + : min_cell_width; + for (auto& [parent_index, cells] : cells_by_parent) { + if (cells.empty()) { + block_voxel_layers_map_.erase(parent_index); + continue; + } + + auto& voxel_layers = block_voxel_layers_map_[parent_index]; + if (voxel_layers.empty()) { + auto& voxel_layer = voxel_layers.emplace_back( + std::make_unique(voxel_material_)); + voxel_layer->setName("discrete_layer_" + discrete_layer_msg.name + "_" + + std::to_string(Index3DHash()(parent_index))); + frame_node_->attachObject(voxel_layer.get()); + } + + auto& voxel_layer = voxel_layers.front(); + voxel_layer->setCellDimensions(cell_width, cell_width, cell_width); + voxel_layer->setAlpha(alpha); + voxel_layer->setCells(cells); + } + + num_queued_blocks_indicator_.setInt(0); +} + +bool VoxelVisual::getDiscreteCellColor( + const wavemap_msgs::DiscreteLayer& layer_msg, + const wavemap_msgs::DiscreteLayerCell& cell_msg, int exception_index, + bool is_exception, Ogre::ColourValue& color) const { + if (layer_msg.value_type == "bool") { + const bool value = is_exception + ? cell_msg.exception_uint8_values[exception_index] != 0u + : cell_msg.dominant_uint8 != 0u; + if (!value && !show_bool_false_property_.getBool()) { + return false; + } + color = value ? bool_true_color_ : Ogre::ColourValue(0.35f, 0.35f, 0.35f, 1.f); + return true; + } + + if (layer_msg.value_type == "int") { + const int value = is_exception ? cell_msg.exception_int32_values[exception_index] + : cell_msg.dominant_int32; + color = stableIntColor(value); + return true; + } + + color = Ogre::ColourValue(0.7f, 0.7f, 0.7f, 1.f); + return true; +} + void VoxelVisual::appendLayeredBlockOccupancy(const wavemap_msgs::LayeredHashedWaveletOctree& layered_map_msg, const wavemap_msgs::LayeredHashedWaveletOctreeBlock& block_msg, IndexElement termination_height, VoxelsPerLevel& voxels_per_level) { ProfilerZoneScoped; if (block_msg.nodes.empty()) { @@ -545,19 +828,72 @@ void VoxelVisual::appendLayeredBlockOccupancy(const wavemap_msgs::LayeredHashedW using Coefficients = HashedWaveletOctreeBlock::Coefficients; using Transform = HashedWaveletOctreeBlock::Transform; + using LayerValues = std::vector>; struct StackElement { OctreeIndex node_index; - FloatingPoint scale; + FloatingPoint occupancy_scale; + LayerValues layer_scales; size_t node_msg_index; }; + LayerValues root_layer_scales; + root_layer_scales.reserve(layered_map_msg.layer_types.size()); + for (size_t layer_i = 0u; layer_i < layered_map_msg.layer_types.size(); ++layer_i) { + const int component_count = continuousLayerComponentCount(layered_map_msg.layer_types[layer_i]); + std::vector values; + values.reserve(component_count); + const wavemap_msgs::Layer* root_layer = + layer_i < block_msg.root_node_layers.size() ? &block_msg.root_node_layers[layer_i] : nullptr; + for (int component_i = 0; component_i < component_count; ++component_i) { + values.push_back(root_layer ? readFloatLayerValue(*root_layer, 0u, component_count, component_i) : 0.f); + } + root_layer_scales.push_back(std::move(values)); + } + + auto append_leaf = [this, &layered_map_msg, &voxels_per_level]( + const OctreeIndex& node_index, + FloatingPoint occupancy_scale, + const LayerValues& layer_scales) { + if (map_and_mutex_->selected_layer_name == "occupancy") { + appendLeafCenterAndColor(layered_map_msg.tree_height, + layered_map_msg.min_cell_width, node_index, + occupancy_scale, voxels_per_level); + return; + } + + if (!cell_selector_.shouldBeDrawn(node_index, occupancy_scale)) { + return; + } + + Ogre::ColourValue layer_color; + if (!getGenericContinuousLayerColor(layered_map_msg, layer_scales, + layer_color)) { + appendLeafCenterAndColor(layered_map_msg.tree_height, + layered_map_msg.min_cell_width, node_index, + occupancy_scale, voxels_per_level); + return; + } + + const IndexElement depth = layered_map_msg.tree_height - node_index.height; + CHECK_GE(depth, 0); + CHECK_LT(depth, voxels_per_level.size()); + const Point3D cell_center = convert::nodeIndexToCenterPoint( + node_index, layered_map_msg.min_cell_width); + + auto& point = voxels_per_level[depth].emplace_back(); + point.center.x = cell_center[0]; + point.center.y = cell_center[1]; + point.center.z = cell_center[2]; + point.color = layer_color; + }; + size_t next_node_msg_index = 0u; std::stack stack; - stack.emplace(StackElement{OctreeIndex{layered_map_msg.tree_height, Index3D{block_msg.root_node_offset.x, block_msg.root_node_offset.y, block_msg.root_node_offset.z}}, block_msg.root_node_occupancy_scale_coefficient, next_node_msg_index++}); + stack.emplace(StackElement{OctreeIndex{layered_map_msg.tree_height, Index3D{block_msg.root_node_offset.x, block_msg.root_node_offset.y, block_msg.root_node_offset.z}}, block_msg.root_node_occupancy_scale_coefficient, std::move(root_layer_scales), next_node_msg_index++}); while (!stack.empty()) { - const StackElement stack_element = stack.top(); + StackElement stack_element = stack.top(); stack.pop(); if (block_msg.nodes.size() <= stack_element.node_msg_index) { @@ -566,32 +902,70 @@ void VoxelVisual::appendLayeredBlockOccupancy(const wavemap_msgs::LayeredHashedW } const auto& node_msg = block_msg.nodes[stack_element.node_msg_index]; - Coefficients::Details details; - std::copy_n(node_msg.occupancy_detail_coefficients.begin(), details.size(), details.begin()); - const auto child_scales = Transform::backward({stack_element.scale, details}); + Coefficients::Details occupancy_details; + std::copy_n(node_msg.occupancy_detail_coefficients.begin(), + occupancy_details.size(), occupancy_details.begin()); + const auto child_occupancy_scales = Transform::backward( + {stack_element.occupancy_scale, occupancy_details}); + + std::array child_layer_scales; + for (size_t layer_i = 0u; layer_i < layered_map_msg.layer_types.size(); ++layer_i) { + const int component_count = continuousLayerComponentCount(layered_map_msg.layer_types[layer_i]); + if (component_count == 0) { + continue; + } + const wavemap_msgs::Layer* detail_layer = + layer_i < node_msg.detail_layers.size() ? &node_msg.detail_layers[layer_i] : nullptr; + for (int component_i = 0; component_i < component_count; ++component_i) { + Coefficients::Details detail_values; + for (size_t coefficient_i = 0u; coefficient_i < detail_values.size(); ++coefficient_i) { + detail_values[coefficient_i] = detail_layer ? readFloatLayerValue(*detail_layer, coefficient_i, component_count, component_i) : 0.f; + } + const FloatingPoint parent_value = + stack_element.layer_scales.size() > layer_i && + stack_element.layer_scales[layer_i].size() > static_cast(component_i) + ? stack_element.layer_scales[layer_i][component_i] + : 0.f; + const auto child_values = Transform::backward({parent_value, detail_values}); + for (NdtreeIndexRelativeChild child_idx = 0; child_idx < OctreeIndex::kNumChildren; ++child_idx) { + if (child_layer_scales[child_idx].size() <= layer_i) { + child_layer_scales[child_idx].resize(layer_i + 1u); + } + child_layer_scales[child_idx][layer_i].push_back(child_values[child_idx]); + } + } + } struct ChildToVisit { OctreeIndex node_index; - FloatingPoint scale; + FloatingPoint occupancy_scale; + LayerValues layer_scales; size_t node_msg_index; }; std::vector children_to_visit; for (NdtreeIndexRelativeChild child_idx = 0; child_idx < OctreeIndex::kNumChildren; ++child_idx) { const OctreeIndex child_node_index = stack_element.node_index.computeChildIndex(child_idx); - const FloatingPoint child_scale = child_scales[child_idx]; + const FloatingPoint child_occupancy_scale = child_occupancy_scales[child_idx]; const bool child_exists = bit_ops::is_bit_set(node_msg.allocated_children_bitset, child_idx); if (child_exists && termination_height < child_node_index.height) { - children_to_visit.push_back({child_node_index, child_scale, next_node_msg_index++}); + children_to_visit.push_back({child_node_index, child_occupancy_scale, + std::move(child_layer_scales[child_idx]), + next_node_msg_index++}); } else { - appendLeafCenterAndColor(layered_map_msg.tree_height, layered_map_msg.min_cell_width, child_node_index, child_scale, voxels_per_level); + append_leaf(child_node_index, child_occupancy_scale, + child_layer_scales[child_idx]); } } - // The serialized node array follows the same DFS order as the stack traversal. Push in reverse so the next serialized node is popped first. + // The serialized node array follows the same DFS order as the stack traversal. + // Push in reverse so the next serialized node is popped first. for (auto child_it = children_to_visit.rbegin(); child_it != children_to_visit.rend(); ++child_it) { - stack.emplace(StackElement{child_it->node_index, child_it->scale, child_it->node_msg_index}); + stack.emplace(StackElement{child_it->node_index, + child_it->occupancy_scale, + std::move(child_it->layer_scales), + child_it->node_msg_index}); } } } 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 7730cd0e2..33c67589d 100644 --- a/interfaces/ros1/wavemap_rviz_plugin/src/wavemap_map_display.cc +++ b/interfaces/ros1/wavemap_rviz_plugin/src/wavemap_map_display.cc @@ -15,37 +15,9 @@ #include #include -#include "layered_ros_converter.h" -#include "layered_voxel_config.h" #include "wavemap_rviz_plugin/utils/alert_dialog.h" -namespace { -struct LayeredVoxelColorProvider { - static bool getLayerColor(const std::string& layer_name, - const LayeredVoxel& voxel, - wavemap::FloatingPoint /*occupancy*/, - Ogre::ColourValue& color) { - if (layer_name == "color") { - color = Ogre::ColourValue(voxel.data.rgb.r, voxel.data.rgb.g, voxel.data.rgb.b, 1.f); - return true; - } - - if (layer_name == "traversability") { - const float value = std::clamp(voxel.data.traversability, 0.f, 1.f); - if (value < 0.5f) { - color = Ogre::ColourValue(1.f, 2.f * value, 0.f, 1.f); - } else { - color = Ogre::ColourValue(2.f * (1.f - value), 1.f, 0.f, 1.f); - } - return true; - } - - return false; - } -}; -} // namespace - namespace wavemap::rviz_plugin { WavemapMapDisplay::WavemapMapDisplay() { // Initialize the property menu @@ -55,12 +27,6 @@ WavemapMapDisplay::WavemapMapDisplay() { } source_mode_property_.setStringStd(source_mode_.toStr()); - // Temporary prototype wiring: this RViz plugin knows the current layered voxel type. - // Later this can move back to pluginlib-based factory registration. - layered_map_factories_.push_back( - std::make_shared< - TypedLayeredMapFactory>()); - // The layered map selector is only useful after receiving a layered map message. layer_property_.clearOptions(); layer_property_.addOption(QString::fromStdString(selected_layer_name_)); @@ -172,15 +138,9 @@ void WavemapMapDisplay::updateMapFromRosMsg(const wavemap_msgs::Map& map_msg) { std::scoped_lock lock(map_and_mutex_->mutex); map_and_mutex_->selected_layer_name = selected_layer_name_; if (has_layered_map && !has_legacy_map) { - const auto& layered_map_msg = map_msg.layered_hashed_wavelet_octree.front(); map_and_mutex_->map.reset(); - map_and_mutex_->layered_map = createLayeredMapFromRosMsg(layered_map_msg); - if (map_and_mutex_->layered_map) { - map_and_mutex_->layered_map_msg.reset(); - } else { - // Keep the message fallback until users can register typed factories for their custom voxel data. - map_and_mutex_->layered_map_msg = layered_map_msg; - } + map_and_mutex_->layered_map.reset(); + map_and_mutex_->layered_map_msg = map_msg.layered_hashed_wavelet_octree.front(); return; } @@ -191,21 +151,6 @@ void WavemapMapDisplay::updateMapFromRosMsg(const wavemap_msgs::Map& map_msg) { } } -std::shared_ptr WavemapMapDisplay::createLayeredMapFromRosMsg( - const wavemap_msgs::LayeredHashedWaveletOctree& layered_map_msg) const { - ProfilerZoneScoped; - for (const auto& factory : layered_map_factories_) { - if (!factory) { - continue; - } - - if (auto layered_map = factory->tryCreate(layered_map_msg)) { - return layered_map; - } - } - - return nullptr; -} void WavemapMapDisplay::updateLayerMetadataFromRosMsg(const wavemap_msgs::Map& map_msg) { ProfilerZoneScoped; diff --git a/library/cpp/include/wavemap/layered/layered_map.h b/library/cpp/include/wavemap/layered/layered_map.h index 595e3e263..70f74aeeb 100644 --- a/library/cpp/include/wavemap/layered/layered_map.h +++ b/library/cpp/include/wavemap/layered/layered_map.h @@ -32,6 +32,8 @@ template class LayeredMap { public: + using ContinuousLayers = ContinuousLayersT; + using ContinuousPolicy = ContinuousPolicyT; using ContinuousVoxel = LayeredMapVoxel; using ContinuousMap = ContinuousWaveletMapT; diff --git a/library/cpp/include/wavemap/layered/layered_map_io.h b/library/cpp/include/wavemap/layered/layered_map_io.h index 76dcd85f7..185c8ac6a 100644 --- a/library/cpp/include/wavemap/layered/layered_map_io.h +++ b/library/cpp/include/wavemap/layered/layered_map_io.h @@ -16,6 +16,7 @@ #include #include +#include namespace wavemap::layered::io { namespace detail { @@ -24,6 +25,7 @@ inline constexpr bool kAlwaysFalse = false; constexpr char kMagic[] = "LWVMP"; constexpr uint32_t kVersion = 1u; +constexpr char kSchemaSection[] = "schema"; constexpr char kContinuousMapSection[] = "continuous_map"; constexpr char kDiscreteLayersSection[] = "discrete_layers"; @@ -79,14 +81,75 @@ inline bool writeFileHeader(std::ostream& ostream) { return writeString(ostream, kMagic) && writePod(ostream, kVersion); } -inline bool readFileHeader(std::istream& istream) { +inline bool readFileHeader(std::istream& istream, uint32_t& version) { std::string magic; - uint32_t version = 0u; + version = 0u; return readString(istream, magic) && magic == kMagic && readPod(istream, version) && version == kVersion; } } // namespace detail +inline bool writeLayerSchemaEntries( + std::ostream& ostream, const std::vector& entries) { + const uint64_t entry_count = entries.size(); + if (!detail::writePod(ostream, entry_count)) { + return false; + } + for (const auto& entry : entries) { + if (!detail::writeString(ostream, entry.name) || + !detail::writeString(ostream, entry.type)) { + return false; + } + } + return static_cast(ostream); +} + +inline bool readLayerSchemaEntries( + std::istream& istream, std::vector& entries) { + uint64_t entry_count = 0u; + if (!detail::readPod(istream, entry_count)) { + return false; + } + entries.clear(); + entries.reserve(entry_count); + for (uint64_t entry_i = 0u; entry_i < entry_count; ++entry_i) { + auto& entry = entries.emplace_back(); + if (!detail::readString(istream, entry.name) || + !detail::readString(istream, entry.type)) { + return false; + } + } + return true; +} + +inline bool writeLayeredMapSchema(std::ostream& ostream, + const LayeredMapSchema& schema) { + return writeLayerSchemaEntries(ostream, schema.continuous_layers) && + writeLayerSchemaEntries(ostream, schema.discrete_layers); +} + +inline bool readLayeredMapSchema(std::istream& istream, + LayeredMapSchema& schema) { + return readLayerSchemaEntries(istream, schema.continuous_layers) && + readLayerSchemaEntries(istream, schema.discrete_layers); +} + +inline bool readLayeredMapSchema(const std::filesystem::path& file_path, + LayeredMapSchema& schema) { + std::ifstream istream(file_path, std::ios::binary); + if (!istream.is_open()) { + return false; + } + + uint32_t version = 0u; + if (!detail::readFileHeader(istream, version)) { + return false; + } + + return detail::readSectionMarker(istream, detail::kSchemaSection) && + readLayeredMapSchema(istream, schema); +} + template struct DiscreteValueSerializer { static constexpr const char* typeName() { @@ -316,6 +379,39 @@ bool forEachTupleElement(TupleT&& tuple, FuncT&& func) { std::make_index_sequence{}); } +template +LayerSchemaEntry discreteLayerSchemaEntry( + const ConstNamedDiscreteLayer& named_layer) { + return {named_layer.name, DiscreteValueSerializer::typeName()}; +} + +template +LayerSchemaEntry discreteLayerSchemaEntry( + const NamedDiscreteLayer& named_layer) { + return {named_layer.name, DiscreteValueSerializer::typeName()}; +} + +template +std::vector discreteLayerBundleSchema( + const DiscreteLayersT& layers) { + std::vector schema; + const auto named_layers = + DiscreteLayerBundleTraits::layers(layers); + schema.reserve( + std::tuple_size_v>); + forEachTupleElement(named_layers, [&](const auto& named_layer) { + schema.emplace_back(discreteLayerSchemaEntry(named_layer)); + return true; + }); + return schema; +} + +template +LayeredMapSchema layeredMapSchema(const LayeredMapT& map) { + return {continuousLayerSchema(), + discreteLayerBundleSchema(map.discreteLayers())}; +} + template bool writeDiscreteLayerBundle(std::ostream& ostream, const DiscreteLayersT& layers) { @@ -368,7 +464,10 @@ bool saveLayeredMap(const std::filesystem::path& file_path, return false; } + const LayeredMapSchema schema = layeredMapSchema(map); if (!detail::writeFileHeader(ostream) || + !detail::writeSectionMarker(ostream, detail::kSchemaSection) || + !writeLayeredMapSchema(ostream, schema) || !detail::writeSectionMarker(ostream, detail::kContinuousMapSection)) { return false; } @@ -384,14 +483,43 @@ bool saveLayeredMap(const std::filesystem::path& file_path, } template -bool loadLayeredMap(const std::filesystem::path& file_path, LayeredMapT& map) { +bool loadLayeredMap(const std::filesystem::path& file_path, LayeredMapT& map, + std::string* error_message = nullptr) { std::ifstream istream(file_path, std::ios::binary); if (!istream.is_open()) { + if (error_message) { + *error_message = "Could not open LayeredMap file: " + file_path.string(); + } return false; } - if (!detail::readFileHeader(istream) || - !detail::readSectionMarker(istream, detail::kContinuousMapSection)) { + uint32_t version = 0u; + if (!detail::readFileHeader(istream, version)) { + if (error_message) { + *error_message = "Invalid LayeredMap file header or unsupported version."; + } + return false; + } + + LayeredMapSchema schema; + if (!detail::readSectionMarker(istream, detail::kSchemaSection) || + !readLayeredMapSchema(istream, schema)) { + if (error_message) { + *error_message = "Failed to read LayeredMap schema section."; + } + return false; + } + + const LayeredMapSchema expected_schema = layeredMapSchema(map); + if (!checkLayeredMapSchemaCompatibility(schema, expected_schema, + error_message)) { + return false; + } + + if (!detail::readSectionMarker(istream, detail::kContinuousMapSection)) { + if (error_message) { + *error_message = "Failed to find LayeredMap continuous map section."; + } return false; } @@ -400,12 +528,18 @@ bool loadLayeredMap(const std::filesystem::path& file_path, LayeredMapT& map) { ContinuousCellSerializerT>(istream, continuous_map) || !continuous_map) { + if (error_message) { + *error_message = "Failed to deserialize LayeredMap continuous map data."; + } return false; } typename LayeredMapT::DiscreteLayers discrete_layers; if (!detail::readSectionMarker(istream, detail::kDiscreteLayersSection) || !readDiscreteLayerBundle(istream, discrete_layers)) { + if (error_message) { + *error_message = "Failed to deserialize LayeredMap discrete layer data."; + } return false; } @@ -422,9 +556,10 @@ struct LayeredMapIo { map); } - static bool load(const std::filesystem::path& file_path, LayeredMapT& map) { - return loadLayeredMap(file_path, - map); + static bool load(const std::filesystem::path& file_path, LayeredMapT& map, + std::string* error_message = nullptr) { + return loadLayeredMap( + file_path, map, error_message); } }; diff --git a/library/cpp/include/wavemap/layered/layered_map_schema.h b/library/cpp/include/wavemap/layered/layered_map_schema.h new file mode 100644 index 000000000..4b28d50e5 --- /dev/null +++ b/library/cpp/include/wavemap/layered/layered_map_schema.h @@ -0,0 +1,111 @@ +#ifndef WAVEMAP_LAYERED_LAYERED_MAP_SCHEMA_H_ +#define WAVEMAP_LAYERED_LAYERED_MAP_SCHEMA_H_ + +#include +#include +#include + +namespace wavemap::layered { +namespace detail { +template +inline constexpr bool kLayeredMapSchemaAlwaysFalse = false; +} + +struct LayerSchemaEntry { + std::string name; + std::string type; + + bool operator==(const LayerSchemaEntry& other) const { + return name == other.name && type == other.type; + } + + bool operator!=(const LayerSchemaEntry& other) const { + return !(*this == other); + } +}; + +struct LayeredMapSchema { + std::vector continuous_layers; + std::vector discrete_layers; + + bool operator==(const LayeredMapSchema& other) const { + return continuous_layers == other.continuous_layers && + discrete_layers == other.discrete_layers; + } + + bool operator!=(const LayeredMapSchema& other) const { + return !(*this == other); + } +}; + +inline std::string layerSchemaEntriesToString( + const std::vector& entries) { + std::ostringstream stream; + if (entries.empty()) { + stream << "none"; + return stream.str(); + } + + for (size_t entry_i = 0u; entry_i < entries.size(); ++entry_i) { + if (entry_i != 0u) { + stream << ", "; + } + stream << entries[entry_i].name << " (" << entries[entry_i].type << ")"; + } + return stream.str(); +} + +inline std::string layeredMapSchemaToString(const LayeredMapSchema& schema) { + std::ostringstream stream; + stream << "continuous: " + << layerSchemaEntriesToString(schema.continuous_layers) + << "; discrete: " + << layerSchemaEntriesToString(schema.discrete_layers); + return stream.str(); +} + +inline bool checkLayeredMapSchemaCompatibility( + const LayeredMapSchema& file_schema, const LayeredMapSchema& expected_schema, + std::string* error_message = nullptr) { + if (file_schema == expected_schema) { + return true; + } + + if (error_message) { + std::ostringstream stream; + stream << "Layered map schema mismatch. File has [" + << layeredMapSchemaToString(file_schema) << "]; expected [" + << layeredMapSchemaToString(expected_schema) << "]."; + *error_message = stream.str(); + } + return false; +} + +template +struct ContinuousLayerSchemaTraits { + static std::vector layers() { + static_assert(detail::kLayeredMapSchemaAlwaysFalse, + "ContinuousLayerSchemaTraits must be specialized for " + "your continuous layer bundle. Return the custom " + "continuous layers, excluding occupancy, because " + "occupancy is part of every LayeredMap."); + return {}; + } +}; + +inline std::vector occupancySchemaEntry() { + return {{"occupancy", "float32"}}; +} + +template +std::vector continuousLayerSchema() { + auto layers = occupancySchemaEntry(); + const auto custom_layers = + ContinuousLayerSchemaTraits::layers(); + layers.insert(layers.end(), custom_layers.begin(), custom_layers.end()); + return layers; +} + +} // namespace wavemap::layered + +#endif // WAVEMAP_LAYERED_LAYERED_MAP_SCHEMA_H_ From 09767e60b77fc49d11f2debe3dfd1d29ad5dcdc1 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Guilherne=20Caba=C3=A7o?= Date: Mon, 24 Aug 2026 10:25:29 +0000 Subject: [PATCH 09/14] Checkpoint layered Wavemap implementation --- CODEX_CONTEXT.md | 412 +++++++++++++++++ LAYERED_LAYER_POLICY_DESIGN_NOTES.txt | 196 ++++++++ LAYERED_WAVEMAP_IMPLEMENTATION_REVIEW.txt | 406 +++++++++++++++++ examples/cpp/digital_twin/CMakeLists.txt | 60 +++ examples/cpp/digital_twin/CMakeLists.txt.orig | 341 ++++++++++++++ .../common/custom_layered_map_config.h | 9 + .../common/field_layer_policies.h | 37 ++ .../digital_twin/common/field_layer_types.h | 14 + .../common/field_layered_map_config.h | 61 +++ .../common/field_layered_map_ros_config.h | 117 +++++ .../common/layered_ros_converter.h | 23 + .../common/layered_voxel_updater.h | 6 +- .../io/inspect_field_layered_map.cc | 58 +++ .../io/inspect_ouster_benchmark_maps.cc | 200 ++++++++ .../io/inspect_ouster_benchmark_maps.cc.orig | 211 +++++++++ .../io/inspect_reflectivity_layered_map.cc | 98 ++++ .../inspect_reflectivity_layered_map.cc.orig | 101 +++++ .../field_endpoint_integration_experiment.cc | 68 +++ .../field_layered_map_experiment.cc | 107 +++++ .../field_layered_pipeline_experiment.cc | 80 ++++ .../ouster_mapping/occupancy_map_layers.h | 29 ++ .../ouster_mapping/occupancy_sensor_inputs.cc | 42 ++ .../ouster_mapping/ouster_sensor_inputs.cc | 59 +++ .../ouster_mapping/reflectivity_map_layers.h | 63 +++ .../reflectivity_only_map_layers.h | 30 ++ .../reflectivity_only_sensor_inputs.cc | 45 ++ .../ros/field_layer_update_ros_experiment.cc | 114 +++++ .../ros/field_layered_map_ros_experiment.cc | 54 +++ ...native_sensor_layer_adapters_experiment.cc | 156 +++++++ interfaces/ros1/wavemap_msgs/CMakeLists.txt | 9 +- .../ros1/wavemap_msgs/msg/DiscreteLayer.msg | 8 + .../msg/DiscreteLayerCategory.msg | 7 + .../wavemap_msgs/msg/LayerVisualization.msg | 10 + .../msg/LayeredHashedWaveletOctree.msg | 3 + .../ros1/wavemap_msgs/msg/LayeredMap.msg | 4 + .../wavemap_msgs/msg/LayeredMapUpdate.msg | 6 + interfaces/ros1/wavemap_msgs/package.xml | 1 + interfaces/ros1/wavemap_ros/CMakeLists.txt | 54 ++- ...computed_point_field_binding_experiment.cc | 204 +++++++++ .../app/field_layer_sensor_bridge.cc | 198 ++++++++ .../layered_incremental_publish_experiment.cc | 145 ++++++ .../publish_field_layer_updates_experiment.cc | 47 ++ .../wavemap_ros/config/rviz/layered_map.rviz | 2 +- .../config/wavemap_ouster_occupancy_only.yaml | 57 +++ .../wavemap_ouster_reflectivity_layered.yaml | 51 +++ .../inputs/pointcloud_endpoint_adapter.h | 347 ++++++++++++++ .../inputs/pointcloud_topic_input.h | 19 +- .../inputs/sensor_frame_transformer.h | 80 ++++ ...ayered_hashed_wavelet_integrator_factory.h | 67 +++ .../layered_ros_server_extension.h | 224 ++++++++- .../wavemap_ros/layered_server_builder.h | 72 +++ .../layered_publish_map_operation.h | 212 +++++++++ .../include/wavemap_ros/ros_server.h | 156 +++++++ .../external_ouster_layered_smoke.launch | 33 ++ ...xternal_ouster_occupancy_validation.launch | 29 ++ .../wavemap_ros/launch/wavemap_server.launch | 4 +- interfaces/ros1/wavemap_ros/package.xml | 1 + .../src/inputs/pointcloud_topic_input.cc | 76 +++- interfaces/ros1/wavemap_ros/src/ros_server.cc | 89 +++- .../wavemap_ros_conversions/CMakeLists.txt | 4 +- ...descriptor_layer_observation_conversions.h | 169 +++++++ .../descriptor_layered_map_conversions.h | 308 +++++++++++++ .../impl/layered_map_msg_conversions_inl.h | 6 + .../layer_observation_msg_conversions.h | 24 + .../layered_map_msg_conversions.h | 154 ++++++- ...pointcloud_layer_observation_conversions.h | 218 +++++++++ .../rgbd_layer_observation_conversions.h | 124 +++++ .../ros1/wavemap_ros_conversions/package.xml | 1 + .../ros1/wavemap_rviz_plugin/CMakeLists.txt | 6 +- .../wavemap_rviz_plugin/layered_map_display.h | 15 + .../wavemap_rviz_plugin/layered_map_factory.h | 51 ++- .../layered_map_interface.h | 29 +- .../visuals/voxel_visual.h | 65 ++- .../ros1/wavemap_rviz_plugin/package.xml | 2 + .../plugin_description.xml | 25 + .../src/layered_map_display.cc | 427 +++++++++++++++--- .../src/layered_map_factories.cc | 195 ++++++++ .../src/layered_map_factories.cc.orig | 169 +++++++ .../src/visuals/voxel_visual.cc | 371 +++++++++------ .../src/wavemap_map_display.cc | 4 +- .../wavemap/core/data_structure/pointcloud.h | 4 + .../wavemap/core/map/hashed_wavelet_octree.h | 5 +- .../utils/undistortion/stamped_pointcloud.h | 7 +- .../layered/continuous_layer_updater.h | 38 ++ .../layered/descriptor_layered_map_io.h | 113 +++++ .../include/wavemap/layered/discrete_layer.h | 85 +++- .../layered/endpoint_attribute_integrator.h | 92 ++++ .../layered/endpoint_attribute_pointcloud.h | 94 ++++ .../include/wavemap/layered/endpoint_range.h | 30 ++ .../wavemap/layered/layer_bundle_traits.h | 40 ++ .../wavemap/layered/layer_descriptor.h | 101 +++++ .../wavemap/layered/layer_integrator.h | 146 ++++++ .../wavemap/layered/layer_observation.h | 34 ++ .../wavemap/layered/layer_observation_batch.h | 97 ++++ .../include/wavemap/layered/layer_schema.h | 114 +++++ .../wavemap/layered/layer_stream_codec.h | 86 ++++ .../include/wavemap/layered/layer_traits.h | 173 +++++++ .../wavemap/layered/layer_update_policy.h | 170 +++++++ .../cpp/include/wavemap/layered/layered_map.h | 4 + .../layered/layered_map_config_builder.h | 133 ++++++ .../wavemap/layered/layered_map_definition.h | 75 +++ .../include/wavemap/layered/layered_map_io.h | 20 +- .../wavemap/layered/layered_pipeline.h | 56 +++ .../layered/local_elevation_classifier.h | 141 ++++++ .../pointcloud_layer_integration_mode.h | 37 ++ .../layered/schema_continuous_traits.h | 310 +++++++++++++ .../wavemap/layered/schema_layer_storage.h | 166 +++++++ .../cpp/include/wavemap/layered/types/rgb.h | 47 ++ .../layered/types/weighted_mean_state.h | 26 ++ library/cpp/test/src/core/CMakeLists.txt | 2 + .../core/map/test_layered_map_definition.cc | 346 ++++++++++++++ .../query/test_local_elevation_classifier.cc | 109 +++++ 112 files changed, 10390 insertions(+), 290 deletions(-) create mode 100644 CODEX_CONTEXT.md create mode 100644 LAYERED_LAYER_POLICY_DESIGN_NOTES.txt create mode 100644 LAYERED_WAVEMAP_IMPLEMENTATION_REVIEW.txt create mode 100644 examples/cpp/digital_twin/CMakeLists.txt.orig create mode 100644 examples/cpp/digital_twin/common/field_layer_policies.h create mode 100644 examples/cpp/digital_twin/common/field_layer_types.h create mode 100644 examples/cpp/digital_twin/common/field_layered_map_config.h create mode 100644 examples/cpp/digital_twin/common/field_layered_map_ros_config.h create mode 100644 examples/cpp/digital_twin/io/inspect_field_layered_map.cc create mode 100644 examples/cpp/digital_twin/io/inspect_ouster_benchmark_maps.cc create mode 100644 examples/cpp/digital_twin/io/inspect_ouster_benchmark_maps.cc.orig create mode 100644 examples/cpp/digital_twin/io/inspect_reflectivity_layered_map.cc create mode 100644 examples/cpp/digital_twin/io/inspect_reflectivity_layered_map.cc.orig create mode 100644 examples/cpp/digital_twin/map_api/user_facing/field_endpoint_integration_experiment.cc create mode 100644 examples/cpp/digital_twin/map_api/user_facing/field_layered_map_experiment.cc create mode 100644 examples/cpp/digital_twin/map_api/user_facing/field_layered_pipeline_experiment.cc create mode 100644 examples/cpp/digital_twin/ouster_mapping/occupancy_map_layers.h create mode 100644 examples/cpp/digital_twin/ouster_mapping/occupancy_sensor_inputs.cc create mode 100644 examples/cpp/digital_twin/ouster_mapping/ouster_sensor_inputs.cc create mode 100644 examples/cpp/digital_twin/ouster_mapping/reflectivity_map_layers.h create mode 100644 examples/cpp/digital_twin/ouster_mapping/reflectivity_only_map_layers.h create mode 100644 examples/cpp/digital_twin/ouster_mapping/reflectivity_only_sensor_inputs.cc create mode 100644 examples/cpp/digital_twin/ros/field_layer_update_ros_experiment.cc create mode 100644 examples/cpp/digital_twin/ros/field_layered_map_ros_experiment.cc create mode 100644 examples/cpp/digital_twin/ros/native_sensor_layer_adapters_experiment.cc create mode 100644 interfaces/ros1/wavemap_msgs/msg/DiscreteLayerCategory.msg create mode 100644 interfaces/ros1/wavemap_msgs/msg/LayerVisualization.msg create mode 100644 interfaces/ros1/wavemap_msgs/msg/LayeredMapUpdate.msg create mode 100644 interfaces/ros1/wavemap_ros/app/computed_point_field_binding_experiment.cc create mode 100644 interfaces/ros1/wavemap_ros/app/field_layer_sensor_bridge.cc create mode 100644 interfaces/ros1/wavemap_ros/app/layered_incremental_publish_experiment.cc create mode 100644 interfaces/ros1/wavemap_ros/app/publish_field_layer_updates_experiment.cc create mode 100644 interfaces/ros1/wavemap_ros/config/wavemap_ouster_occupancy_only.yaml create mode 100644 interfaces/ros1/wavemap_ros/config/wavemap_ouster_reflectivity_layered.yaml create mode 100644 interfaces/ros1/wavemap_ros/include/wavemap_ros/inputs/pointcloud_endpoint_adapter.h create mode 100644 interfaces/ros1/wavemap_ros/include/wavemap_ros/inputs/sensor_frame_transformer.h create mode 100644 interfaces/ros1/wavemap_ros/include/wavemap_ros/layered_hashed_wavelet_integrator_factory.h create mode 100644 interfaces/ros1/wavemap_ros/include/wavemap_ros/layered_server_builder.h create mode 100644 interfaces/ros1/wavemap_ros/include/wavemap_ros/map_operations/layered_publish_map_operation.h create mode 100644 interfaces/ros1/wavemap_ros/launch/datasets/external_ouster_layered_smoke.launch create mode 100644 interfaces/ros1/wavemap_ros/launch/datasets/external_ouster_occupancy_validation.launch create mode 100644 interfaces/ros1/wavemap_ros_conversions/include/wavemap_ros_conversions/descriptor_layer_observation_conversions.h create mode 100644 interfaces/ros1/wavemap_ros_conversions/include/wavemap_ros_conversions/descriptor_layered_map_conversions.h create mode 100644 interfaces/ros1/wavemap_ros_conversions/include/wavemap_ros_conversions/layer_observation_msg_conversions.h create mode 100644 interfaces/ros1/wavemap_ros_conversions/include/wavemap_ros_conversions/pointcloud_layer_observation_conversions.h create mode 100644 interfaces/ros1/wavemap_ros_conversions/include/wavemap_ros_conversions/rgbd_layer_observation_conversions.h create mode 100644 interfaces/ros1/wavemap_rviz_plugin/src/layered_map_factories.cc create mode 100644 interfaces/ros1/wavemap_rviz_plugin/src/layered_map_factories.cc.orig create mode 100644 library/cpp/include/wavemap/layered/continuous_layer_updater.h create mode 100644 library/cpp/include/wavemap/layered/descriptor_layered_map_io.h create mode 100644 library/cpp/include/wavemap/layered/endpoint_attribute_integrator.h create mode 100644 library/cpp/include/wavemap/layered/endpoint_attribute_pointcloud.h create mode 100644 library/cpp/include/wavemap/layered/endpoint_range.h create mode 100644 library/cpp/include/wavemap/layered/layer_bundle_traits.h create mode 100644 library/cpp/include/wavemap/layered/layer_descriptor.h create mode 100644 library/cpp/include/wavemap/layered/layer_integrator.h create mode 100644 library/cpp/include/wavemap/layered/layer_observation.h create mode 100644 library/cpp/include/wavemap/layered/layer_observation_batch.h create mode 100644 library/cpp/include/wavemap/layered/layer_schema.h create mode 100644 library/cpp/include/wavemap/layered/layer_stream_codec.h create mode 100644 library/cpp/include/wavemap/layered/layer_traits.h create mode 100644 library/cpp/include/wavemap/layered/layer_update_policy.h create mode 100644 library/cpp/include/wavemap/layered/layered_map_config_builder.h create mode 100644 library/cpp/include/wavemap/layered/layered_map_definition.h create mode 100644 library/cpp/include/wavemap/layered/layered_pipeline.h create mode 100644 library/cpp/include/wavemap/layered/local_elevation_classifier.h create mode 100644 library/cpp/include/wavemap/layered/pointcloud_layer_integration_mode.h create mode 100644 library/cpp/include/wavemap/layered/schema_continuous_traits.h create mode 100644 library/cpp/include/wavemap/layered/schema_layer_storage.h create mode 100644 library/cpp/include/wavemap/layered/types/rgb.h create mode 100644 library/cpp/include/wavemap/layered/types/weighted_mean_state.h create mode 100644 library/cpp/test/src/core/map/test_layered_map_definition.cc create mode 100644 library/cpp/test/src/core/utils/query/test_local_elevation_classifier.cc diff --git a/CODEX_CONTEXT.md b/CODEX_CONTEXT.md new file mode 100644 index 000000000..1fd8472f9 --- /dev/null +++ b/CODEX_CONTEXT.md @@ -0,0 +1,412 @@ +# Codex Context: Layered Wavemap Work + +## Repository State + +Workspace: `/home/ci/catkin_ws/src/wavemap` + +The current work extends Wavemap with a layered map architecture for digital-twin style voxel data: + +- continuous data stored in a templated wavelet octree; +- discrete/categorical data stored in compressed discrete layers; +- `.lwvmp` persistence for full layered maps; +- ROS1 server support for loading/saving/publishing layered maps; +- RViz support for visualizing layered maps from topic or file. + +Original Wavemap behavior should remain available and should not be broken. The layered functionality is additive. + +## Main Design Decisions + +### Continuous Data + +Continuous layers stay inside a wavelet map. The current example type is based on: + +- occupancy; +- RGB/color; +- traversability. + +Continuous fields must support the arithmetic needed by the Haar/wavelet transform: + +- addition; +- subtraction; +- scalar multiplication; +- threshold/clamp/pruning traits. + +This is why continuous float-like data makes sense, while categorical labels do not. + +### Discrete Data + +Discrete data is stored separately from the continuous wavelet octree. + +Current example discrete layers: + +- `semantic`: integer label; +- `changed`: boolean flag. + +The goal is not to hardcode these forever. They are example layers. The intended user-facing pattern is: + +```cpp +struct MyDiscreteLayers { + DiscreteLayer semantic; + DiscreteLayer changed; +}; +``` + +Discrete data uses a compressed parent/exceptions representation. It does not use Haar arithmetic. + +### Full Layered Map + +The intended top-level structure is: + +```text +LayeredMap + continuous_map // wavelet octree with occupancy + continuous fields + discrete_layers // templated discrete layer struct +``` + +Naming decisions: + +- `ContinuousWaveletMap`: continuous wavelet map alias; +- `LayeredMap`: full map aggregation; +- `LayeredMapConfig`: full config; +- `ExampleLayeredMap`: current example config/type. + +## Implemented Components + +### Core / Library + +Implemented or modified: + +- `library/cpp/include/wavemap/layered/layered_map.h` +- `library/cpp/include/wavemap/layered/layered_map_io.h` +- `library/cpp/include/wavemap/layered/layered_map_schema.h` + +Current functionality: + +- full `LayeredMap` abstraction; +- continuous + discrete access; +- `.lwvmp` save/load; +- schema metadata stored in `.lwvmp`; +- schema compatibility checks. + +### Example Config + +Implemented or modified: + +- `examples/cpp/digital_twin/common/layered_voxel_config.h` +- `examples/cpp/digital_twin/common/example_layered_map_config.h` +- `examples/cpp/digital_twin/common/example_layered_map_ros_config.h` +- `examples/cpp/digital_twin/common/layered_ros_converter.h` + +Current example layers: + +```text +continuous: + occupancy + color / RGB + traversability + +discrete: + semantic + changed +``` + +### ROS Server + +Implemented or modified: + +- `interfaces/ros1/wavemap_ros/include/wavemap_ros/layered_ros_server_extension.h` +- `interfaces/ros1/wavemap_ros/include/wavemap_ros/ros_server.h` +- `interfaces/ros1/wavemap_ros/src/ros_server.cc` +- `interfaces/ros1/wavemap_ros/CMakeLists.txt` + +The layered extension is attached to the existing `RosServer`. + +It provides: + +```bash +/wavemap/layered_map +/wavemap/load_layered_map +/wavemap/save_layered_map +/wavemap/layered_map_request_full +``` + +Confirmed runtime log: + +```text +Layered map extension enabled. Advertising layered map topic and services. +``` + +The original Wavemap services/topics remain separate: + +```bash +/wavemap/map +/wavemap/load_map +/wavemap/save_map +/wavemap/map_request_full +``` + +Important caveat: + +- the original occupancy map and the layered map are currently separate internal maps; +- `/wavemap/load_map` loads `.wvmp`; +- `/wavemap/load_layered_map` loads `.lwvmp`. + +### ROS / RViz Conversions + +Implemented or modified: + +- `interfaces/ros1/wavemap_ros_conversions/include/wavemap_ros_conversions/discrete_layer_marker_conversions.h` +- layered ROS conversion helpers in example/common config. + +### RViz Plugin + +Implemented or modified: + +- `interfaces/ros1/wavemap_rviz_plugin/include/wavemap_rviz_plugin/layered_map_display.h` +- `interfaces/ros1/wavemap_rviz_plugin/src/layered_map_display.cc` +- `interfaces/ros1/wavemap_rviz_plugin/include/wavemap_rviz_plugin/common.h` +- `interfaces/ros1/wavemap_rviz_plugin/include/wavemap_rviz_plugin/visuals/voxel_visual.h` +- `interfaces/ros1/wavemap_rviz_plugin/src/visuals/voxel_visual.cc` +- `interfaces/ros1/wavemap_rviz_plugin/include/wavemap_rviz_plugin/wavemap_map_display.h` +- `interfaces/ros1/wavemap_rviz_plugin/src/wavemap_map_display.cc` +- `interfaces/ros1/wavemap_rviz_plugin/CMakeLists.txt` +- `interfaces/ros1/wavemap_rviz_plugin/plugin_description.xml` + +Current RViz functionality: + +- new `LayeredMap` display; +- source can be topic or file; +- topic default: `/wavemap/layered_map`; +- file loading supports `.lwvmp`; +- dynamic layer dropdown, not hardcoded to only semantic/changed; +- continuous visualization uses typed reconstruction/factory path; +- discrete visualization supports integer and bool layers; +- viewport legend/card overlay works; +- legend uses generated 1x1 textures for color swatches. + +Known RViz notes: + +- fixed frame should usually be `map`; +- if RViz says there is no TF data, set global fixed frame correctly; +- if launching inside container gives `qt.qpa.xcb: could not connect to display`, this is a display/X issue, not necessarily plugin failure. + +### Example / Experiment Files + +Implemented or modified: + +- `examples/cpp/digital_twin/CMakeLists.txt` +- `examples/cpp/digital_twin/io/create_large_layered_map_experiment.cc` +- `examples/cpp/digital_twin/io/load_existing_maps_experiment.cc` +- `examples/cpp/digital_twin/io/layered_map_save_load_failure_experiment.cc` +- `examples/cpp/digital_twin/ros/publish_structured_layered_map_experiment.cc` + +Useful generated test map: + +```text +/home/ci/data/maps/layered_map_large.lwvmp +``` + +Regenerate it with: + +```bash +cd ~/catkin_ws/src/wavemap +source /opt/ros/noetic/setup.bash +cmake --build examples/cpp/build --target create_large_layered_map_experiment +./examples/cpp/build/digital_twin/create_large_layered_map_experiment +``` + +Recent output from the large map example: + +```text +populated continuous voxels: 307200 +populated discrete voxels: 19128 +continuous blocks before prune: 4 +continuous blocks after prune: 4 +continuous nodes before prune: 43912 +continuous nodes after prune: 43912 +semantic parents: 456 exceptions: 1474 observed: 19128 +changed parents: 456 exceptions: 616 observed: 19128 +file size bytes: 5850724 +``` + +## Useful Commands + +### Build RViz Plugin + +```bash +cd ~/catkin_ws +source /opt/ros/noetic/setup.bash +source devel/setup.bash +catkin build wavemap_rviz_plugin --no-status +``` + +Last known result: build succeeded with no warnings. + +### Launch Server + +The correct dt architecture config file is: + +```text +/home/ci/catkin_ws/src/dt_architecture/load_config_file.yaml +``` + +Launch: + +```bash +cd ~/catkin_ws +source /opt/ros/noetic/setup.bash +source devel/setup.bash +roslaunch wavemap_ros wavemap_server.launch param_file:=/home/ci/catkin_ws/src/dt_architecture/load_config_file.yaml +``` + +If the launch also starts RViz and RViz crashes, the server may still be fine. Check for: + +```text +Layered map extension enabled. Advertising layered map topic and services. +``` + +### Check Layered Topics/Services + +In another terminal: + +```bash +cd ~/catkin_ws +source /opt/ros/noetic/setup.bash +source devel/setup.bash +rosservice list | grep layered +rostopic list | grep layered +``` + +Expected: + +```bash +/wavemap/load_layered_map +/wavemap/save_layered_map +/wavemap/layered_map_request_full +/wavemap/layered_map +``` + +### Load Layered Map Through Server + +```bash +rosservice call /wavemap/load_layered_map "file_path: '/home/ci/data/maps/layered_map_large.lwvmp'" +rosservice call /wavemap/layered_map_request_full "{}" +rostopic echo -n 1 /wavemap/layered_map +``` + +### Open RViz Manually + +```bash +cd ~/catkin_ws +source /opt/ros/noetic/setup.bash +source devel/setup.bash +rviz +``` + +Then add the `LayeredMap` display and select: + +```text +Topic: /wavemap/layered_map +``` + +Or use file mode and select: + +```text +/home/ci/data/maps/layered_map_large.lwvmp +``` + +## Current Git Commit Recommendation + +If not already committed, commit all current layered-map pipeline changes as one checkpoint: + +```bash +cd ~/catkin_ws/src/wavemap +git add . +git commit -m "Add layered map ROS and RViz pipeline" +``` + +A previous stable commit exists: + +```text +0e940190 Add layered map support +``` + +## What Is Still Missing + +The main missing piece is the live update pipeline for extra layers. + +Currently implemented: + +```text +create/load LayeredMap +save/load .lwvmp +publish /wavemap/layered_map +visualize in RViz +``` + +Still missing: + +```text +incoming live layer observations -> update LayeredMap -> publish updated /wavemap/layered_map +``` + +Since extra data comes per voxel, the planned path is: + +1. Add ROS message(s) for layered voxel updates. +2. Add converter/helper from update message to `LayeredMap` update. +3. Add subscriber in `LayeredRosServerExtension`. +4. Update continuous layers by replacement for now. +5. Update discrete layers with `DiscreteLayer::set(...)` or equivalent. +6. Publish `/wavemap/layered_map` after update. +7. Add publisher experiment for live update testing. +8. Test in RViz. + +Suggested first update message, example-specific for now: + +```text +LayeredVoxelUpdate + geometry_msgs/Point position + float32 r + float32 g + float32 b + float32 traversability + int32 semantic + bool changed + +LayeredVoxelUpdateArray + std_msgs/Header header + LayeredVoxelUpdate[] updates +``` + +Important design choice: + +- first version should require update positions to already be in `world_frame`; +- TF transformation can be added later; +- original occupancy integration should remain untouched. + +Live behavior target: + +```text +original Wavemap ray integration -> occupancy map +layered voxel update topic -> continuous/discrete extra layers +server publishes -> /wavemap/layered_map +``` + +Potential future work: + +- sync occupancy from original occupancy map into `LayeredMap` continuous map; +- generic schema-driven update messages instead of example-specific messages; +- richer RViz color policies configured by user; +- validation with real bag data. + +## Bag / External Drive Context + +The user has a bag outside the environment on an external drive: + +```text +/media/guilhermecabaco/CABACO_02/rosbag +``` + +It may require mounting/bind-mounting into the container/environment. The user does not want to copy the bag because it is too large. + +Previous attempts to bind mount to `/home/ci/data/external_bag` or `/tmp/external_bag` failed with permission denied. If the environment is restarted with the bind mount configured, this Codex session may be lost. This file exists to make the work resumable. diff --git a/LAYERED_LAYER_POLICY_DESIGN_NOTES.txt b/LAYERED_LAYER_POLICY_DESIGN_NOTES.txt new file mode 100644 index 000000000..e06d8a62f --- /dev/null +++ b/LAYERED_LAYER_POLICY_DESIGN_NOTES.txt @@ -0,0 +1,196 @@ +LAYERED LAYER POLICY AND STORAGE DESIGN NOTES +============================================= + +Status +------ +These are design decisions and deferred ideas. They are not requirements for +the current occupancy + reflectivity + class example and are not implemented +unless explicitly stated otherwise. + + +1. Layer semantics and storage are separate concepts +---------------------------------------------------- + +The public meaning of a layer must not determine its storage automatically. +A future layer definition should independently select: + + * Value type: float, integer/category, bool, RGB, vector, structured state. + * Update policy: replace, logical operation, voting, hysteresis, etc. + * Storage policy: wavelet, majority + exceptions, sparse voxels, bit-packed + booleans, direct structured storage, etc. + * Pruning/compression policy. + * Serialization/transport codec when the generic codec is insufficient. + +The intended shape is conceptually: + + Layer + +Compile-time templates are preferred so integration does not gain virtual-call +overhead. + + +2. Continuous and discrete are not a closed list of storage backends +------------------------------------------------------------------ + +The current continuous implementation is specifically suitable for numeric +state that supports meaningful addition, subtraction, scaling, wavelet +reconstruction, thresholding, and pruning. + +The current discrete implementation uses spatial majority + exceptions. + +Structured numeric information can require another backend even when it is +mathematically continuous. Examples include: + + * Unit surface normals. + * Covariance matrices and NDT statistics. + * Probability distributions or mixtures. + * Multiple surfaces/hypotheses in one voxel. + +Prefer storing additive sufficient statistics where possible. For example, +NDT data can store count, position sum, and outer-product sum, then derive mean +and covariance when queried. Coupled pruning is required so independent +thresholding does not create inconsistent or invalid reconstructed state. + +Variable-length histories, graphs, connectivity, and per-frame metadata may +belong alongside the voxel map instead of becoming voxel layers. + + +3. Current meaning of majority + exceptions +-------------------------------------------- + +Majority + exceptions is a spatial storage/compression method. A compressed +parent stores: + + * Its dominant child value. + * Which child offsets are observed. + * Explicit values for children that differ from the dominant value. + +It is not an observation update policy and is not restricted to replacement. +For example, the logical output of replace, logical OR, hysteresis, or temporal +voting can all be spatially stored with majority + exceptions. + +There are two different meanings of "majority" that must remain separate: + + * Temporal majority: observations over time vote for one voxel's value. + * Spatial majority: nearby child values are compressed under one parent. + + +4. The changed boolean layer +---------------------------- + +Majority + exceptions is valid for a spatial changed/not-changed boolean, +particularly when changed voxels are rare among mostly false voxels. + +A sparse or bit-packed boolean backend may eventually be more compact, but it +is an alternative storage policy, not a different semantic layer type. + +If "changed" represents an event or history rather than persistent state, a +boolean alone may be insufficient. It may need a last-changed timestamp or a +separate event/history system. + + +5. Per-layer discrete storage policies (deferred) +------------------------------------------------- + +The desired future map keeps heterogeneous side layers: + + class -> MajorityWithExceptionsStorage + changed -> MajorityWithExceptionsStorage or BitPackedBoolStorage + object_id -> SparseVoxelStorage + +Majority + exceptions should remain the default for backwards compatibility: + + DiscreteLayer> + +Required refactoring: + + 1. Extract majority + exceptions into a standalone storage policy. + 2. Make discrete configuration, including block height, per layer. + 3. Define a storage contract for get, set, erase, batch updates, iteration, + dirty-region tracking, and clearing only published dirty regions. + 4. Implement SparseVoxelStorage as a second backend to prove modularity. + 5. Decouple ROS and RViz from majority + exceptions internals. + 6. Provide a generic logical voxel patch and automatic visualization path. + 7. Allow optional optimized codecs for backends that should preserve their + compression over ROS or on disk. + 8. Store backend identifier and format version in persistent files. + +Users should not need custom ROS or RViz code for a normal custom storage +policy. Specialized codecs should only be necessary to preserve backend- +specific compression or hidden state. + + +6. Temporal majority with majority + exceptions (deferred) +---------------------------------------------------------- + +A true temporal majority requires per-voxel hidden integration state such as +vote counts. Keeping only the current winning integer is insufficient because +the history cannot be reconstructed from the winner. + +The processing sequence would be: + + observations for one voxel + -> update temporal vote state + -> calculate current winner + -> if winner changed, update logical class value + -> spatially recompress that value using majority + exceptions + -> mark/publish only the changed spatial parent + +Vote-state changes should not dirty the visible class layer when its winner +does not change. + +The architecture therefore needs a distinction between: + + * Public Value: the winning class ID. + * Stored integration State: vote counts or another stability state. + * Spatial StoragePolicy: how public class values are compressed. + +If mapping must resume exactly after loading, hidden vote state must also be +saved. Saving only winners is sufficient for visualization and queries, but +not for resuming exact temporal voting. + + +7. Efficiency decision for temporal class stability +---------------------------------------------------- + +Full vote histograms are not the preferred default for the current geometric +forest classifier. They require multiple counters per voxel and additional +update work. + +Lower-cost alternatives, in increasing complexity, are: + + * Replace/latest value. + * Class plus one contradictory-observation hysteresis counter. + * Candidate class plus confidence counter (streaming approximate majority). + * Two candidates and two counters. + * Decaying class confidence. + * Full per-class temporal vote histogram. + +For this project, if replacement later proves too noisy, the preferred next +experiment is a compact state: + + StableClassState { + int class_id; + uint8_t confidence; + } + +This has constant memory independent of the number of classes, cheap updates, +and should reduce class changes and discrete patch traffic. Full temporal +majority remains an optional advanced policy, not the default. + + +8. Suggested future implementation order +----------------------------------------- + + 1. Formalize custom stateless update-policy requirements. + 2. Add explicit public Value versus stored State support. + 3. Extract the existing majority + exceptions backend. + 4. Add per-layer storage configuration. + 5. Implement and test one second discrete backend. + 6. Introduce storage-independent logical patches for generic ROS/RViz use. + 7. Add optional optimized transport and file codecs. + 8. Experiment with compact class hysteresis only if replacement is too + unstable in real mapping tests. + diff --git a/LAYERED_WAVEMAP_IMPLEMENTATION_REVIEW.txt b/LAYERED_WAVEMAP_IMPLEMENTATION_REVIEW.txt new file mode 100644 index 000000000..4142b7f0c --- /dev/null +++ b/LAYERED_WAVEMAP_IMPLEMENTATION_REVIEW.txt @@ -0,0 +1,406 @@ +LAYERED WAVEMAP IMPLEMENTATION REVIEW +===================================== + +Date: 2026-08-07 +Scope: Current layered-map core, persistence, ROS server/conversions, RViz, + examples, and the proposed live voxel-update path. + + +1. CURRENT ARCHITECTURE +----------------------- + +The current RosServer owns two distinct map instances: + +1. occupancy_map_ + - Created by MapFactory from the existing "map" parameters. + - Passed to Pipeline. + - Receives the existing sensor/ray integration. + - Published and saved through the original Wavemap interfaces. + +2. layered_extension_.layered_map_ + - Constructed independently as ExampleLayeredMap. + - Its continuousMap() is a typed HashedWaveletOctree whose voxel contains: + occupancy + RGB + traversability + - Its discrete bundle contains: + semantic + changed + - Published and saved through the layered interfaces. + +Within the layered continuous map, occupancy, RGB, and traversability really do +share one wavelet octree. However, this layered octree is not the same object as +RosServer::occupancy_map_. There is currently no pointer/reference connection or +automatic synchronization between the two instances. + + +2. DOES LAYERED OCCUPANCY WORK LIKE ORIGINAL OCCUPANCY? +------------------------------------------------------- + +Mechanically, yes: + +- LayeredVoxel is VoxelData. +- VoxelData stores occupancy as a FloatingPoint and supports the arithmetic + required by the existing integrators and Haar transform. +- HashedWaveletOctreeT derives from MapBase. +- Existing experiments exercise ray-tracing and hashed-wavelet integration on + the typed map. +- Occupancy-only updates preserve the additional continuous data. +- Continuous-layer replacement can preserve occupancy by applying a voxel + update whose occupancy delta is zero. + +Equivalent implementation does not automatically mean identical runtime map. +For two independent maps to produce matching occupancy, they must receive the +same measurements in the same order with equivalent: + +- coordinate frame and poses; +- min/max range and measurement model; +- voxel resolution and tree height; +- min/max log-odds thresholds; +- pruning configuration and timing; +- update scheduling and concurrency behavior. + +The active dt_architecture config aligns both grids at 0.25 m. The layered map +explicitly uses min_log_odds=-2, max_log_odds=4, and tree_height=6. The original +map relies partly on the normal map defaults because only type and resolution +are explicitly configured. These effective configs should be compared or made +explicit before claiming exact equivalence. + +Most importantly, the current original sensor pipeline only receives +occupancy_map_. Therefore the layered occupancy field remains independent (and +usually unobserved/default) unless a layered file is loaded or it is populated +through another path. + + +3. HIGH-PRIORITY CORRECTNESS IMPROVEMENTS +----------------------------------------- + +3.1 Decide and document the occupancy relationship + +Choose one supported model explicitly: + +A. Compatibility-first dual maps + Keep original Wavemap untouched and keep layered occupancy independent. + Clearly document that /wavemap/map and /wavemap/layered_map may disagree. + +B. Dual integration + Feed the same observations to the original and typed layered octrees. + This duplicates computation and memory, and exact equivalence requires + matched configs and deterministic update ordering. + +C. Synchronize occupancy into the layered map + Copy/reconstruct occupancy periodically or before layered publication. + This is potentially expensive and requires a well-defined incremental sync. + +D. One typed continuous map with a compatibility view + Use the typed map as the integrated source and expose occupancy through the + legacy MapBase/topic/service interfaces. This is architecturally cleaner but + has the greatest risk of changing original behavior and needs extensive + compatibility tests. + +The current code behaves as A, but that behavior is implicit. + +3.2 Add the missing live update path + +Add example-specific ROS messages for the first implementation: + +LayeredVoxelUpdate.msg: + geometry_msgs/Point position + float32 r + float32 g + float32 b + float32 traversability + int32 semantic + bool changed + +LayeredVoxelUpdateArray.msg: + std_msgs/Header header + LayeredVoxelUpdate[] updates + +Add geometry_msgs to wavemap_msgs message-generation and package dependencies. + +3.3 Reuse the existing continuous replacement helper + +Use setContinuousLayersAtVoxel() from layered_voxel_updater.h. It computes the +difference between observed and stored continuous data, then applies: + + LayeredVoxel(0.f, data_update) + +This preserves layered occupancy while replacing RGB and traversability. Avoid +adding a second replacement implementation with subtly different semantics. + +3.4 Use the existing Wavemap position-to-index convention + +Convert world positions with pointToNearestIndex(position, +1 / min_cell_width). Do not use a custom floor(position / resolution), because +Wavemap defines indices through its voxel-center convention and handles negative +coordinates accordingly. + +3.5 Lock all access to the layered map + +The live callback must update continuous and discrete layers while holding the +same mutex used by load/save/publish. Do not mutate through getLayeredMap(), +because that method returns an unprotected mutable reference. + +Prefer a member function such as applyVoxelUpdates() that owns locking and +returns the publication stamp/result. Consider removing the mutable public +getter or replacing it with explicitly locked visitor/access functions. + +3.6 Fix load concurrency + +loadLayeredMap() currently copies layered_map_ before taking mutex_. Since the +LayeredMap copy contains a shared_ptr to the continuous map, concurrent update, +save, or publish operations can race with this initial copy/load workflow. +Protect the snapshot/replacement correctly, or load into a fresh map created +from a safely copied config without sharing mutable map storage. + +3.7 Make reset behavior consistent + +RosServer::clear() clears inputs, pipeline, and occupancy_map_, but does not +clear the layered map. Decide whether reset_map should also clear layered data. +If compatibility requires separate behavior, add a dedicated reset_layered_map +service and document the distinction. + + +4. LIVE UPDATE MESSAGE SEMANTICS TO DEFINE +------------------------------------------ + +Before implementing the subscriber, define these behaviors explicitly: + +- Version 1 requires header.frame_id == configured world_frame. +- Reject an empty or mismatched frame; do not silently interpret it. +- No TF conversion in version 1. Add TF later as a separate feature. +- Validate positions and float fields with std::isfinite. +- Define RGB range as [0, 1]. Decide whether invalid values are rejected or + clamped; logging and stored behavior must agree. +- Define traversability range as [0, 1]. +- Define valid semantic label values, including whether a sentinel means + unknown/unobserved. +- Decide whether changed is an input observation or a value derived by comparing + old and new data. It can remain explicit for the first pipeline experiment. +- All fields are currently mandatory and use replacement semantics. +- If several updates map to one voxel, apply message order and let the last valid + update win, or deduplicate explicitly. Document the choice. +- Define whether one invalid update rejects the whole batch or only that entry. + Skipping invalid entries with throttled warnings is practical for live input. +- Define maximum accepted batch size to prevent a malformed message from causing + excessive latency or memory use. +- Publish once per accepted batch, never once per voxel. +- Preserve the incoming timestamp when publishing the resulting state where + useful, rather than always replacing it with ros::Time::now(). + +Future messages may need per-field validity masks so a producer can update only +semantic, only RGB, etc. The first fixed message has no way to distinguish +"replace with zero/false" from "field not observed". + + +5. DISCRETE-LAYER IMPROVEMENTS +------------------------------ + +- The actual update method is setValue(), not set(). Use the existing API. +- Add an erase/set-unobserved operation. Currently a value can be set but there + is no public way to make one voxel unknown again. +- Remove empty parent cells after erase support is added. +- setValue() expands and rebuilds the entire compressed parent cell on every + voxel update. This is acceptable for 2x2x2 parents and initial validation, + but batch updates should group observations by parent to avoid repeated work. +- Validate DiscreteCompressionConfig.block_height. Negative values and large + shifts can cause invalid or overflowing side lengths/counts. +- Validate deserialized observed and exception offsets against the configured + parent voxel count. +- Validate that every exception offset is also observed. +- Reject duplicate parent cells in ROS/file input instead of silently allowing + later values to replace earlier values. +- Standardize serialized type names (currently examples use strings such as + "int" and "bool") and consider stable versioned identifiers. +- Add equality/comparison helpers to make round-trip and update tests clearer. + + +6. CONTINUOUS MAP, THRESHOLD, AND PRUNING IMPROVEMENTS +----------------------------------------------------- + +- Document that continuous replacement creates/updates wavelet coefficients and + should be followed by thresholding according to a defined schedule. +- Decide when prune() runs for live updates. Pruning every message may be too + expensive; never pruning can grow memory indefinitely. +- Verify that combined pruning weights cannot discard meaningful occupancy due + to interactions with RGB/traversability scores. +- Add tests for updates at negative indices, block boundaries, and exact voxel + boundaries. +- Add tests proving occupancy is unchanged after repeated continuous replacement, + threshold, prune, save/load, and ROS round trips. +- Add tests for non-finite and out-of-range continuous values. +- Ensure both original and layered effective occupancy thresholds are explicit + when equivalence is expected. + + +7. ROS PUBLICATION AND PERFORMANCE IMPROVEMENTS +----------------------------------------------- + +- publishLayeredMap() serializes the complete continuous octree and all discrete + cells. Publishing a full map after every high-rate update batch will become a + CPU, allocation, bandwidth, and RViz bottleneck. +- For the first small experiment, one full publish per batch is acceptable. +- For real data, coalesce updates and publish at a configurable maximum rate. +- Add incremental/delta layered messages later, including block allocation and + deletion semantics. +- Keep the existing full latched topic for new subscribers and explicit full-map + requests. +- Avoid holding the map mutex longer than necessary, but do not serialize from + mutable map storage without a safe snapshot strategy. +- Add counters/timing for received, accepted, skipped, and deduplicated updates; + update latency; serialization time; message bytes; and publication rate. +- Make subscriber queue size and publish rate configurable. +- Add backpressure/drop behavior instead of allowing stale update batches to + accumulate. +- Consider an AsyncSpinner only after map locking and callback ordering semantics + are explicitly safe. + + +8. ROS CONVERSION ROBUSTNESS +---------------------------- + +- LayeredVoxelRosConverter assumes fixed layer positions and directly indexes + arrays without validating names, types, layer counts, or value lengths. + Malformed messages can cause out-of-range access. +- Validate continuous layer schema before reading cell data. +- Validate expected RGB value count (3 per coefficient) and traversability count + (1 per coefficient). +- Return structured conversion errors rather than only bool where practical. +- Validate wavelet node count/stack consistency and child bitsets during map + reconstruction. +- Validate discrete layer ordering or resolve layers by name. The current bundle + conversion requires exactly the expected count and positional order. +- Add message/schema versioning for forward compatibility. +- Add fuzz or malformed-message tests for every conversion entry point. + + +9. SERVER AND PACKAGE STRUCTURE IMPROVEMENTS +-------------------------------------------- + +- wavemap_ros and the RViz plugin directly include configuration headers from + examples/cpp/digital_twin/common through private include paths. Installed + packages may not contain those example headers, making the feature fragile + outside this source workspace. +- Move the supported example layered types/converters into an installable + library/package, or create a dedicated wavemap_digital_twin package. +- Keep generic LayeredMap, I/O, and conversion mechanisms in library/interface + packages; keep example schema policy in the example/digital-twin package. +- Avoid hardwiring ExampleLayeredMap construction inside RosServer long term. + Use a factory/extension registration mechanism or an explicitly optional + digital-twin server component. +- Validate layered_map configuration with the same rigor as existing ConfigBase + configurations, including compression and threshold constraints. +- Improve startup logging to report effective layered resolution, tree height, + thresholds, schema, update topic, and occupancy relationship mode. + + +10. RVIZ IMPROVEMENTS +--------------------- + +- The RViz LayeredMap display is also compiled against example configuration + headers. Separate generic display mechanics from example continuous-layer + color policies. +- Validate incoming schemas and array lengths before reconstruction. +- Avoid rebuilding all geometry unnecessarily when only a small layer delta is + received (after delta transport exists). +- Add limits/decimation for very large maps to prevent CPU/GPU memory exhaustion. +- Expose voxel count, message size, reconstruction time, and rejected-message + errors in display status. +- Preserve the dynamic layer dropdown, but handle removed/renamed layers without + retaining an invalid selection. +- Test topic and file modes with empty maps, huge maps, malformed maps, frame + changes, and repeated load/unload cycles. + + +11. PERSISTENCE AND SCHEMA IMPROVEMENTS +-------------------------------------- + +- Add formal unit tests for .lwvmp round trips, incompatible schemas, truncated + files, corrupt counts, invalid compression offsets, and unsupported versions. +- Apply bounds before allocating from file-provided counts to avoid excessive + memory allocation on corrupt files. +- Include an explicit format version and migration policy if not already fully + enforced by the current header/schema implementation. +- Consider checksums for large files or sections so corruption is detected early. +- Ensure save is atomic: write a temporary file, flush/close successfully, then + rename into place so failure cannot destroy the previous valid map. +- Document endianness and numeric representation if files must be portable. + + +12. TESTING IMPROVEMENTS +------------------------ + +Many current checks are standalone experiment executables. Preserve them, but +promote the critical behavior into automated unit/integration tests: + +- original occupancy integration remains unchanged when layered support is off; +- enabling the extension does not alter original topics/services/results; +- typed layered occupancy matches scalar occupancy under identical config and + identical observation sequences; +- continuous replacement preserves occupancy; +- occupancy integration preserves RGB/traversability; +- discrete values remain sparse/unknown correctly; +- negative-coordinate compression and indexing; +- continuous and discrete save/load round trips; +- ROS message round trips and malformed-input rejection; +- frame mismatch and invalid-value behavior; +- concurrent update/save/load/publish/reset behavior; +- live publisher -> server -> LayeredMap topic -> RViz/converter test; +- save-after-live-update and reload verification; +- bounded publication rate and queue behavior under load; +- compatibility build with layered support disabled. + + +13. RECOMMENDED IMPLEMENTATION ORDER +------------------------------------ + +Phase 1: Safe proof of live updates + +1. Define and document the occupancy relationship (retain dual-map behavior for + compatibility during this phase). +2. Add the two example-specific update messages and dependencies. +3. Add a validated ROS-to-observation converter using pointToNearestIndex(). +4. Reuse setContinuousLayersAtVoxel() and DiscreteLayer::setValue(). +5. Add a mutex-protected update method/subscriber inside the extension. +6. Publish one full map per small test batch. +7. Add the publisher experiment. +8. Add automated occupancy-preservation and end-to-end tests. +9. Test live RViz and save-after-update. + +Phase 2: Correctness and operational hardening + +1. Fix load/reset/accessor concurrency and semantics. +2. Add strict message and deserialization validation. +3. Add publish throttling/coalescing and diagnostics. +4. Make original/layered effective configs explicit and comparable. +5. Move example headers out of private cross-package include paths. + +Phase 3: Occupancy unification decision + +1. Benchmark dual integration, incremental synchronization, and compatibility + view approaches. +2. Select one based on CPU, memory, exactness, and backward-compatibility needs. +3. Implement it behind an explicit configuration mode. +4. Add equivalence and regression tests before changing defaults. + +Phase 4: Generalization + +1. Add optional/per-field updates and validity masks. +2. Add TF support. +3. Add schema-driven generic updates only after real producer requirements are + known. +4. Add incremental layered publication and RViz application of deltas. + + +14. IMMEDIATE CONCLUSION +------------------------ + +The typed layered octree can perform occupancy integration similarly to the +original occupancy octree. The current server simply does not route the original +live integration into it. Keeping the original octree untouched is a valid and +useful compatibility strategy, but the relationship between the two occupancy +representations must be explicit to users and tests. + +The safest next implementation is an example-specific, world-frame-only voxel +update subscriber that changes RGB, traversability, semantic, and changed while +preserving the layered occupancy field. Occupancy synchronization/unification +should be treated as a separate architectural decision, not accidentally mixed +into the first live-layer update patch. diff --git a/examples/cpp/digital_twin/CMakeLists.txt b/examples/cpp/digital_twin/CMakeLists.txt index 1783d83f4..d803b4616 100644 --- a/examples/cpp/digital_twin/CMakeLists.txt +++ b/examples/cpp/digital_twin/CMakeLists.txt @@ -82,6 +82,42 @@ set_wavemap_target_properties(custom_layered_map_example) target_link_libraries(custom_layered_map_example PUBLIC wavemap::wavemap_core) +add_executable(field_layered_map_experiment + map_api/user_facing/field_layered_map_experiment.cc) +set_wavemap_target_properties(field_layered_map_experiment) +target_link_libraries(field_layered_map_experiment + PUBLIC wavemap::wavemap_core) + +add_executable(field_layered_pipeline_experiment + map_api/user_facing/field_layered_pipeline_experiment.cc) +set_wavemap_target_properties(field_layered_pipeline_experiment) +target_link_libraries(field_layered_pipeline_experiment + PUBLIC wavemap::wavemap_core) + +add_executable(field_endpoint_integration_experiment + map_api/user_facing/field_endpoint_integration_experiment.cc) +set_wavemap_target_properties(field_endpoint_integration_experiment) +target_link_libraries(field_endpoint_integration_experiment + PUBLIC wavemap::wavemap_core) + +add_executable(inspect_field_layered_map + io/inspect_field_layered_map.cc) +set_wavemap_target_properties(inspect_field_layered_map) +target_link_libraries(inspect_field_layered_map + PUBLIC wavemap::wavemap_core) + +add_executable(inspect_reflectivity_layered_map + io/inspect_reflectivity_layered_map.cc) +set_wavemap_target_properties(inspect_reflectivity_layered_map) +target_link_libraries(inspect_reflectivity_layered_map + PUBLIC wavemap::wavemap_core) + +add_executable(inspect_ouster_benchmark_maps + io/inspect_ouster_benchmark_maps.cc) +set_wavemap_target_properties(inspect_ouster_benchmark_maps) +target_link_libraries(inspect_ouster_benchmark_maps + PUBLIC wavemap::wavemap_core wavemap::wavemap_io) + add_executable(discrete_layer_compression_experiment map_api/discrete/discrete_layer_compression_experiment.cc) set_wavemap_target_properties(discrete_layer_compression_experiment) @@ -276,6 +312,30 @@ if ("$ENV{ROS_VERSION}" STREQUAL "1") target_include_directories(subscribe_layered_map_experiment PUBLIC ${catkin_INCLUDE_DIRS}) target_link_libraries(subscribe_layered_map_experiment PUBLIC wavemap::wavemap_core ${catkin_LIBRARIES}) + + add_executable(field_layered_map_ros_experiment + ros/field_layered_map_ros_experiment.cc) + set_wavemap_target_properties(field_layered_map_ros_experiment) + target_include_directories(field_layered_map_ros_experiment + PUBLIC ${catkin_INCLUDE_DIRS}) + target_link_libraries(field_layered_map_ros_experiment PUBLIC + wavemap::wavemap_core ${catkin_LIBRARIES}) + + add_executable(field_layer_update_ros_experiment + ros/field_layer_update_ros_experiment.cc) + set_wavemap_target_properties(field_layer_update_ros_experiment) + target_include_directories(field_layer_update_ros_experiment + PUBLIC ${catkin_INCLUDE_DIRS}) + target_link_libraries(field_layer_update_ros_experiment PUBLIC + wavemap::wavemap_core ${catkin_LIBRARIES}) + + add_executable(native_sensor_layer_adapters_experiment + ros/native_sensor_layer_adapters_experiment.cc) + set_wavemap_target_properties(native_sensor_layer_adapters_experiment) + target_include_directories(native_sensor_layer_adapters_experiment + PUBLIC ${catkin_INCLUDE_DIRS}) + target_link_libraries(native_sensor_layer_adapters_experiment PUBLIC + wavemap::wavemap_core ${catkin_LIBRARIES}) else () message(STATUS "Skipping ROS digital_twin examples because ROS1 is not active.") endif () diff --git a/examples/cpp/digital_twin/CMakeLists.txt.orig b/examples/cpp/digital_twin/CMakeLists.txt.orig new file mode 100644 index 000000000..04e789d86 --- /dev/null +++ b/examples/cpp/digital_twin/CMakeLists.txt.orig @@ -0,0 +1,341 @@ +# Binaries +add_executable(new_voxel voxel_data/new_voxel.cc) +set_wavemap_target_properties(new_voxel) +target_link_libraries(new_voxel PUBLIC wavemap::wavemap_core) + +add_executable(voxel_haar_transform voxel_data/voxel_haar_transform.cc) +set_wavemap_target_properties(voxel_haar_transform) +target_link_libraries(voxel_haar_transform PUBLIC wavemap::wavemap_core) + +add_executable(voxel_block_experiment map_api/baseline/voxel_block_experiment.cc) +set_wavemap_target_properties(voxel_block_experiment) +target_link_libraries(voxel_block_experiment PUBLIC wavemap::wavemap_core) + +add_executable(voxel_map_experiment map_api/baseline/voxel_map_experiment.cc) +set_wavemap_target_properties(voxel_map_experiment) +target_link_libraries(voxel_map_experiment PUBLIC wavemap::wavemap_core) + +add_executable(int_map_experiment map_api/baseline/int_map_experiment.cc) +set_wavemap_target_properties(int_map_experiment) +target_link_libraries(int_map_experiment PUBLIC wavemap::wavemap_core) + +add_executable(float_map_experiment map_api/baseline/float_map_experiment.cc) +set_wavemap_target_properties(float_map_experiment) +target_link_libraries(float_map_experiment PUBLIC wavemap::wavemap_core) + +add_executable(continuous_voxel_map_experiment + map_api/continuous/continuous_voxel_map_experiment.cc) +set_wavemap_target_properties(continuous_voxel_map_experiment) +target_link_libraries(continuous_voxel_map_experiment + PUBLIC wavemap::wavemap_core) + +add_executable(layered_occupancy_update_path_experiment + map_api/continuous/layered_occupancy_update_path_experiment.cc) +set_wavemap_target_properties(layered_occupancy_update_path_experiment) +target_link_libraries(layered_occupancy_update_path_experiment + PUBLIC wavemap::wavemap_core) + +add_executable(layered_hashed_wavelet_integrator_experiment + map_api/continuous/layered_hashed_wavelet_integrator_experiment.cc) +set_wavemap_target_properties(layered_hashed_wavelet_integrator_experiment) +target_link_libraries(layered_hashed_wavelet_integrator_experiment + PUBLIC wavemap::wavemap_core) + +add_executable(layered_per_voxel_update_experiment + map_api/continuous/layered_per_voxel_update_experiment.cc) +set_wavemap_target_properties(layered_per_voxel_update_experiment) +target_link_libraries(layered_per_voxel_update_experiment + PUBLIC wavemap::wavemap_core) + +add_executable(layered_full_update_pruning_experiment + map_api/continuous/layered_full_update_pruning_experiment.cc) +set_wavemap_target_properties(layered_full_update_pruning_experiment) +target_link_libraries(layered_full_update_pruning_experiment + PUBLIC wavemap::wavemap_core) + +add_executable(layered_pruning_config_example + map_api/continuous/layered_pruning_config_example.cc) +set_wavemap_target_properties(layered_pruning_config_example) +target_link_libraries(layered_pruning_config_example + PUBLIC wavemap::wavemap_core) + +add_executable(mixed_voxel_map_experiment + map_api/continuous/mixed_voxel_map_experiment.cc) +set_wavemap_target_properties(mixed_voxel_map_experiment) +target_link_libraries(mixed_voxel_map_experiment PUBLIC wavemap::wavemap_core) + +add_executable(layered_label_storage_experiment + map_api/discrete/layered_label_storage_experiment.cc) +set_wavemap_target_properties(layered_label_storage_experiment) +target_link_libraries(layered_label_storage_experiment + PUBLIC wavemap::wavemap_core) + +add_executable(layered_map_experiment + map_api/discrete/layered_map_experiment.cc) +set_wavemap_target_properties(layered_map_experiment) +target_link_libraries(layered_map_experiment + PUBLIC wavemap::wavemap_core) + +add_executable(custom_layered_map_example + map_api/user_facing/custom_layered_map_example.cc) +set_wavemap_target_properties(custom_layered_map_example) +target_link_libraries(custom_layered_map_example + PUBLIC wavemap::wavemap_core) + +add_executable(field_layered_map_experiment + map_api/user_facing/field_layered_map_experiment.cc) +set_wavemap_target_properties(field_layered_map_experiment) +target_link_libraries(field_layered_map_experiment + PUBLIC wavemap::wavemap_core) + +add_executable(field_layered_pipeline_experiment + map_api/user_facing/field_layered_pipeline_experiment.cc) +set_wavemap_target_properties(field_layered_pipeline_experiment) +target_link_libraries(field_layered_pipeline_experiment + PUBLIC wavemap::wavemap_core) + +add_executable(field_endpoint_integration_experiment + map_api/user_facing/field_endpoint_integration_experiment.cc) +set_wavemap_target_properties(field_endpoint_integration_experiment) +target_link_libraries(field_endpoint_integration_experiment + PUBLIC wavemap::wavemap_core) + +add_executable(inspect_field_layered_map + io/inspect_field_layered_map.cc) +set_wavemap_target_properties(inspect_field_layered_map) +target_link_libraries(inspect_field_layered_map + PUBLIC wavemap::wavemap_core) + +add_executable(inspect_reflectivity_layered_map + io/inspect_reflectivity_layered_map.cc) +set_wavemap_target_properties(inspect_reflectivity_layered_map) +target_link_libraries(inspect_reflectivity_layered_map + PUBLIC wavemap::wavemap_core) + +add_executable(inspect_ouster_benchmark_maps + io/inspect_ouster_benchmark_maps.cc) +set_wavemap_target_properties(inspect_ouster_benchmark_maps) +target_link_libraries(inspect_ouster_benchmark_maps + PUBLIC wavemap::wavemap_core) + +add_executable(discrete_layer_compression_experiment + map_api/discrete/discrete_layer_compression_experiment.cc) +set_wavemap_target_properties(discrete_layer_compression_experiment) +target_link_libraries(discrete_layer_compression_experiment + PUBLIC wavemap::wavemap_core) + +add_executable(discrete_layer_update_behavior_experiment + map_api/discrete/discrete_layer_update_behavior_experiment.cc) +set_wavemap_target_properties(discrete_layer_update_behavior_experiment) +target_link_libraries(discrete_layer_update_behavior_experiment + PUBLIC wavemap::wavemap_core) + +add_executable(layered_map_update_behavior_experiment + map_api/discrete/layered_map_update_behavior_experiment.cc) +set_wavemap_target_properties(layered_map_update_behavior_experiment) +target_link_libraries(layered_map_update_behavior_experiment + PUBLIC wavemap::wavemap_core) + +add_executable(heterogeneous_voxel_map_experiment + map_api/continuous/heterogeneous_voxel_map_experiment.cc) +set_wavemap_target_properties(heterogeneous_voxel_map_experiment) +target_link_libraries(heterogeneous_voxel_map_experiment + PUBLIC wavemap::wavemap_core) + +add_executable(raw_discrete_voxel_storage_experiment + map_api/discrete/raw_discrete_voxel_storage_experiment.cc) +set_wavemap_target_properties(raw_discrete_voxel_storage_experiment) +target_link_libraries(raw_discrete_voxel_storage_experiment + PUBLIC wavemap::wavemap_core) + +add_executable(raw_inside_map_experiment + map_api/discrete/raw_inside_map_experiment.cc) +set_wavemap_target_properties(raw_inside_map_experiment) +target_link_libraries(raw_inside_map_experiment + PUBLIC wavemap::wavemap_core) + +add_executable(float_cast_discrete_experiment + map_api/discrete/float_cast_discrete_experiment.cc) +set_wavemap_target_properties(float_cast_discrete_experiment) +target_link_libraries(float_cast_discrete_experiment + PUBLIC wavemap::wavemap_core) + +add_executable(float_cast_discrete_many_voxels_experiment + map_api/benchmarks/float_cast_discrete_many_voxels_experiment.cc) +set_wavemap_target_properties(float_cast_discrete_many_voxels_experiment) +target_link_libraries(float_cast_discrete_many_voxels_experiment + PUBLIC wavemap::wavemap_core) + +add_executable(raw_discrete_many_voxels_experiment + map_api/benchmarks/raw_discrete_many_voxels_experiment.cc) +set_wavemap_target_properties(raw_discrete_many_voxels_experiment) +target_link_libraries(raw_discrete_many_voxels_experiment + PUBLIC wavemap::wavemap_core) + +add_executable(raw_inside_map_many_voxels_experiment + map_api/benchmarks/raw_inside_map_many_voxels_experiment.cc) +set_wavemap_target_properties(raw_inside_map_many_voxels_experiment) +target_link_libraries(raw_inside_map_many_voxels_experiment + PUBLIC wavemap::wavemap_core) + +add_executable(occupancy_many_voxels_benchmark + map_api/benchmarks/occupancy_many_voxels_benchmark.cc) +set_wavemap_target_properties(occupancy_many_voxels_benchmark) +target_link_libraries(occupancy_many_voxels_benchmark + PUBLIC wavemap::wavemap_core) + +add_executable(continuous_voxel_many_voxels_benchmark + map_api/benchmarks/continuous_voxel_many_voxels_benchmark.cc) +set_wavemap_target_properties(continuous_voxel_many_voxels_benchmark) +target_link_libraries(continuous_voxel_many_voxels_benchmark + PUBLIC wavemap::wavemap_core) + +add_executable(layered_pruning_policy_experiment + map_api/benchmarks/layered_pruning_policy_experiment.cc) +set_wavemap_target_properties(layered_pruning_policy_experiment) +target_link_libraries(layered_pruning_policy_experiment + PUBLIC wavemap::wavemap_core) + + +add_executable(semantic_majority_exceptions_experiment + map_api/discrete/semantic_majority_exceptions_experiment.cc) +set_wavemap_target_properties(semantic_majority_exceptions_experiment) +target_link_libraries(semantic_majority_exceptions_experiment + PUBLIC wavemap::wavemap_core) + +add_executable(legacy_hashed_wavelet_octree_experiment + io/legacy_hashed_wavelet_octree_experiment.cc) +set_wavemap_target_properties(legacy_hashed_wavelet_octree_experiment) +target_link_libraries(legacy_hashed_wavelet_octree_experiment + PUBLIC wavemap::wavemap_core) + +add_executable(legacy_file_io_experiment io/legacy_file_io_experiment.cc) +set_wavemap_target_properties(legacy_file_io_experiment) +target_link_libraries(legacy_file_io_experiment + PUBLIC wavemap::wavemap_core wavemap::wavemap_io) + +add_executable(layered_map_io_experiment io/layered_map_io_experiment.cc) +set_wavemap_target_properties(layered_map_io_experiment) +target_link_libraries(layered_map_io_experiment PUBLIC wavemap::wavemap_core) + +add_executable(layered_map_save_load_experiment io/layered_map_save_load_experiment.cc) +set_wavemap_target_properties(layered_map_save_load_experiment) +target_link_libraries(layered_map_save_load_experiment PUBLIC wavemap::wavemap_core) + +add_executable(layered_map_save_load_failure_experiment + io/layered_map_save_load_failure_experiment.cc) +set_wavemap_target_properties(layered_map_save_load_failure_experiment) +target_link_libraries(layered_map_save_load_failure_experiment + PUBLIC wavemap::wavemap_core) + +add_executable(custom_layered_map_save_load_example + io/custom_layered_map_save_load_example.cc) +set_wavemap_target_properties(custom_layered_map_save_load_example) +target_link_libraries(custom_layered_map_save_load_example + PUBLIC wavemap::wavemap_core) + +add_executable(load_existing_maps_experiment + io/load_existing_maps_experiment.cc) +set_wavemap_target_properties(load_existing_maps_experiment) +target_link_libraries(load_existing_maps_experiment + PUBLIC wavemap::wavemap_core wavemap::wavemap_io) + +add_executable(create_large_layered_map_experiment + io/create_large_layered_map_experiment.cc) +set_wavemap_target_properties(create_large_layered_map_experiment) +target_link_libraries(create_large_layered_map_experiment + PUBLIC wavemap::wavemap_core) + +if ("$ENV{ROS_VERSION}" STREQUAL "1") + find_package(catkin REQUIRED + COMPONENTS roscpp visualization_msgs wavemap wavemap_msgs wavemap_ros_conversions) + + add_executable(experiment old_experiments/experiment.cc) + set_wavemap_target_properties(experiment) + target_include_directories(experiment PUBLIC ${catkin_INCLUDE_DIRS}) + target_link_libraries(experiment PUBLIC + wavemap::wavemap_core wavemap::wavemap_io ${catkin_LIBRARIES}) + + add_executable(publish_experiment old_experiments/publish_experiment.cc) + set_wavemap_target_properties(publish_experiment) + target_include_directories(publish_experiment PUBLIC ${catkin_INCLUDE_DIRS}) + target_link_libraries(publish_experiment PUBLIC + wavemap::wavemap_core wavemap::wavemap_io ${catkin_LIBRARIES}) + + add_executable(blocks_experiment old_experiments/blocks_experiment.cc) + set_wavemap_target_properties(blocks_experiment) + target_include_directories(blocks_experiment PUBLIC ${catkin_INCLUDE_DIRS}) + target_link_libraries(blocks_experiment PUBLIC + wavemap::wavemap_core wavemap::wavemap_io ${catkin_LIBRARIES}) + + add_executable(partial_save_experiment old_experiments/partial_save_experiment.cc) + set_wavemap_target_properties(partial_save_experiment) + target_include_directories(partial_save_experiment PUBLIC ${catkin_INCLUDE_DIRS}) + target_link_libraries(partial_save_experiment PUBLIC + wavemap::wavemap_core wavemap::wavemap_io ${catkin_LIBRARIES}) + + add_executable(layered_ros_msg_experiment ros/layered_ros_msg_experiment.cc) + set_wavemap_target_properties(layered_ros_msg_experiment) + target_include_directories(layered_ros_msg_experiment PUBLIC ${catkin_INCLUDE_DIRS}) + target_link_libraries(layered_ros_msg_experiment PUBLIC + wavemap::wavemap_core ${catkin_LIBRARIES}) + + add_executable(layered_map_structured_ros_msg_experiment + ros/layered_map_structured_ros_msg_experiment.cc) + set_wavemap_target_properties(layered_map_structured_ros_msg_experiment) + target_include_directories(layered_map_structured_ros_msg_experiment PUBLIC ${catkin_INCLUDE_DIRS}) + target_link_libraries(layered_map_structured_ros_msg_experiment PUBLIC + wavemap::wavemap_core ${catkin_LIBRARIES}) + + add_executable(publish_structured_layered_map_experiment + ros/publish_structured_layered_map_experiment.cc) + set_wavemap_target_properties(publish_structured_layered_map_experiment) + target_include_directories(publish_structured_layered_map_experiment PUBLIC ${catkin_INCLUDE_DIRS}) + target_link_libraries(publish_structured_layered_map_experiment PUBLIC + wavemap::wavemap_core ${catkin_LIBRARIES}) + + add_executable(subscribe_structured_layered_map_experiment + ros/subscribe_structured_layered_map_experiment.cc) + set_wavemap_target_properties(subscribe_structured_layered_map_experiment) + target_include_directories(subscribe_structured_layered_map_experiment PUBLIC ${catkin_INCLUDE_DIRS}) + target_link_libraries(subscribe_structured_layered_map_experiment PUBLIC + wavemap::wavemap_core ${catkin_LIBRARIES}) + + add_executable(publish_layered_map_experiment ros/publish_layered_map_experiment.cc) + set_wavemap_target_properties(publish_layered_map_experiment) + target_include_directories(publish_layered_map_experiment PUBLIC ${catkin_INCLUDE_DIRS}) + target_link_libraries(publish_layered_map_experiment PUBLIC + wavemap::wavemap_core ${catkin_LIBRARIES}) + + add_executable(subscribe_layered_map_experiment ros/subscribe_layered_map_experiment.cc) + set_wavemap_target_properties(subscribe_layered_map_experiment) + target_include_directories(subscribe_layered_map_experiment PUBLIC ${catkin_INCLUDE_DIRS}) + target_link_libraries(subscribe_layered_map_experiment PUBLIC + wavemap::wavemap_core ${catkin_LIBRARIES}) + + add_executable(field_layered_map_ros_experiment + ros/field_layered_map_ros_experiment.cc) + set_wavemap_target_properties(field_layered_map_ros_experiment) + target_include_directories(field_layered_map_ros_experiment + PUBLIC ${catkin_INCLUDE_DIRS}) + target_link_libraries(field_layered_map_ros_experiment PUBLIC + wavemap::wavemap_core ${catkin_LIBRARIES}) + + add_executable(field_layer_update_ros_experiment + ros/field_layer_update_ros_experiment.cc) + set_wavemap_target_properties(field_layer_update_ros_experiment) + target_include_directories(field_layer_update_ros_experiment + PUBLIC ${catkin_INCLUDE_DIRS}) + target_link_libraries(field_layer_update_ros_experiment PUBLIC + wavemap::wavemap_core ${catkin_LIBRARIES}) + + add_executable(native_sensor_layer_adapters_experiment + ros/native_sensor_layer_adapters_experiment.cc) + set_wavemap_target_properties(native_sensor_layer_adapters_experiment) + target_include_directories(native_sensor_layer_adapters_experiment + PUBLIC ${catkin_INCLUDE_DIRS}) + target_link_libraries(native_sensor_layer_adapters_experiment PUBLIC + wavemap::wavemap_core ${catkin_LIBRARIES}) +else () + message(STATUS "Skipping ROS digital_twin examples because ROS1 is not active.") +endif () diff --git a/examples/cpp/digital_twin/common/custom_layered_map_config.h b/examples/cpp/digital_twin/common/custom_layered_map_config.h index babee14fc..075ac6150 100644 --- a/examples/cpp/digital_twin/common/custom_layered_map_config.h +++ b/examples/cpp/digital_twin/common/custom_layered_map_config.h @@ -117,6 +117,15 @@ struct MyVoxelSerializer { } }; +namespace wavemap::layered { +template <> +struct ContinuousLayerSchemaTraits<::MyContinuousLayers> { + static std::vector layers() { + return {{"temperature", "float32"}, {"intensity", "float32"}}; + } +}; +} // namespace wavemap::layered + namespace wavemap::layered::io { template <> struct DiscreteLayerBundleTraits<::MyDiscreteLayers> { diff --git a/examples/cpp/digital_twin/common/field_layer_policies.h b/examples/cpp/digital_twin/common/field_layer_policies.h new file mode 100644 index 000000000..3bc50be5d --- /dev/null +++ b/examples/cpp/digital_twin/common/field_layer_policies.h @@ -0,0 +1,37 @@ +#ifndef WAVEMAP_EXAMPLES_CPP_DIGITAL_TWIN_FIELD_LAYER_POLICIES_H_ +#define WAVEMAP_EXAMPLES_CPP_DIGITAL_TWIN_FIELD_LAYER_POLICIES_H_ + +#include + +#include +#include + +#include "field_layer_types.h" + +namespace wavemap::examples::field_map { + +using ReflectivityUpdatePolicy = layered::ReplaceLayerUpdatePolicy; +using ClassUpdatePolicy = layered::ReplaceLayerUpdatePolicy; + +struct ReflectivityLayer + : layered::schema::ContinuousLayer { + static constexpr std::string_view name = "reflectivity"; +}; + +struct ClassLayer + : layered::schema::DiscreteLayer { + static constexpr std::string_view name = "class"; +}; + +using Schema = + layered::schema::LayerSchema; + +static_assert(Schema::size == 2u); +static_assert(Schema::continuousLayerCount == 1u); +static_assert(Schema::discreteLayerCount == 1u); +static_assert(Schema::containsContinuous); +static_assert(Schema::containsDiscrete); + +} // namespace wavemap::examples::field_map + +#endif // WAVEMAP_EXAMPLES_CPP_DIGITAL_TWIN_FIELD_LAYER_POLICIES_H_ diff --git a/examples/cpp/digital_twin/common/field_layer_types.h b/examples/cpp/digital_twin/common/field_layer_types.h new file mode 100644 index 000000000..0bfce5c70 --- /dev/null +++ b/examples/cpp/digital_twin/common/field_layer_types.h @@ -0,0 +1,14 @@ +#ifndef WAVEMAP_EXAMPLES_CPP_DIGITAL_TWIN_FIELD_LAYER_TYPES_H_ +#define WAVEMAP_EXAMPLES_CPP_DIGITAL_TWIN_FIELD_LAYER_TYPES_H_ + +namespace wavemap::examples::field_map { + +enum class ClassLabel : int { + kUnknown = 0, + kGround = 1, + kObstacle = 2, +}; + +} // namespace wavemap::examples::field_map + +#endif // WAVEMAP_EXAMPLES_CPP_DIGITAL_TWIN_FIELD_LAYER_TYPES_H_ diff --git a/examples/cpp/digital_twin/common/field_layered_map_config.h b/examples/cpp/digital_twin/common/field_layered_map_config.h new file mode 100644 index 000000000..ac7d49b0c --- /dev/null +++ b/examples/cpp/digital_twin/common/field_layered_map_config.h @@ -0,0 +1,61 @@ +#ifndef WAVEMAP_EXAMPLES_CPP_DIGITAL_TWIN_FIELD_LAYERED_MAP_CONFIG_H_ +#define WAVEMAP_EXAMPLES_CPP_DIGITAL_TWIN_FIELD_LAYERED_MAP_CONFIG_H_ + +#include +#include +#include +#include + +#include "field_layer_policies.h" + +namespace wavemap::examples::field_map { + +using ContinuousLayers = layered::schema::ContinuousLayerBundle; +using ContinuousArithmetic = + layered::schema::ContinuousBundleArithmetic; +using DiscreteLayers = layered::schema::DiscreteLayerBundle; + +using Map = + layered::LayeredMap; +using Config = Map::Config; +using Voxel = Map::ContinuousVoxel; +using ContinuousMap = Map::ContinuousMap; + +using VoxelSerializer = layered::DescriptorVoxelSerializer; +using MapIo = layered::io::LayeredMapIo; + +inline ContinuousLayers makeContinuousLayers(float reflectivity) { + ContinuousLayers layers; + layers.get() = reflectivity; + return layers; +} + +inline Config makeDefaultConfig() { + Config config; + config.continuous_map.min_cell_width = 0.25f; + config.continuous_map.min_log_odds = -2.f; + config.continuous_map.max_log_odds = 4.f; + config.continuous_map.tree_height = 6; + config.continuous_map.only_prune_blocks_if_unused_for = 5.f; + + auto& reflectivity_threshold = + config.continuous_threshold.data.get(); + reflectivity_threshold.min = 0.f; + reflectivity_threshold.max = 0.4f; + + config.continuous_pruning.occupancy.scale = 1e-3f; + config.continuous_pruning.occupancy.weight = 1.f; + auto& reflectivity_pruning = + config.continuous_pruning.data.get(); + reflectivity_pruning.scale = 1e-3f; + reflectivity_pruning.weight = 1.f; + config.continuous_pruning.combined_threshold = 1.f; + config.continuous_pruning.data.combined_threshold = 1.f; + + config.discrete_compression.block_height = 1; + return config; +} + +} // namespace wavemap::examples::field_map + +#endif // WAVEMAP_EXAMPLES_CPP_DIGITAL_TWIN_FIELD_LAYERED_MAP_CONFIG_H_ diff --git a/examples/cpp/digital_twin/common/field_layered_map_ros_config.h b/examples/cpp/digital_twin/common/field_layered_map_ros_config.h new file mode 100644 index 000000000..99a8d1107 --- /dev/null +++ b/examples/cpp/digital_twin/common/field_layered_map_ros_config.h @@ -0,0 +1,117 @@ +#ifndef WAVEMAP_EXAMPLES_CPP_DIGITAL_TWIN_FIELD_LAYERED_MAP_ROS_CONFIG_H_ +#define WAVEMAP_EXAMPLES_CPP_DIGITAL_TWIN_FIELD_LAYERED_MAP_ROS_CONFIG_H_ + +#include +#include + +#include +#include +#include +#include + + +#include "field_layered_map_config.h" + +namespace wavemap::examples::field_map::ros_config { +namespace detail { + +template +void readParam(const ros::NodeHandle& nh, const std::string& key, + ValueT& value) { + nh.param(key, value, value); +} + +} // namespace detail + +inline Config loadConfig(const ros::NodeHandle& nh_private, + const std::string& ns = "layered_map") { + Config config = makeDefaultConfig(); + + detail::readParam(nh_private, ns + "/continuous_map/min_cell_width/meters", + config.continuous_map.min_cell_width); + detail::readParam(nh_private, ns + "/continuous_map/min_log_odds", + config.continuous_map.min_log_odds); + detail::readParam(nh_private, ns + "/continuous_map/max_log_odds", + config.continuous_map.max_log_odds); + detail::readParam(nh_private, ns + "/continuous_map/tree_height", + config.continuous_map.tree_height); + detail::readParam( + nh_private, ns + "/continuous_map/only_prune_blocks_if_unused_for/seconds", + config.continuous_map.only_prune_blocks_if_unused_for); + + detail::readParam(nh_private, ns + "/continuous_pruning/combined_threshold", + config.continuous_pruning.combined_threshold); + config.continuous_pruning.data.combined_threshold = + config.continuous_pruning.combined_threshold; + + detail::readParam(nh_private, ns + "/continuous_pruning/occupancy/scale", + config.continuous_pruning.occupancy.scale); + detail::readParam(nh_private, ns + "/continuous_pruning/occupancy/weight", + config.continuous_pruning.occupancy.weight); + auto& reflectivity_pruning = + config.continuous_pruning.data.get(); + detail::readParam(nh_private, ns + "/continuous_pruning/reflectivity/scale", + reflectivity_pruning.scale); + detail::readParam(nh_private, ns + "/continuous_pruning/reflectivity/weight", + reflectivity_pruning.weight); + + detail::readParam(nh_private, ns + "/discrete_compression/block_height", + config.discrete_compression.block_height); + return config; +} + +using VoxelRosConverter = + convert::DescriptorContinuousRosConverter; + +} // namespace wavemap::examples::field_map::ros_config + +namespace wavemap::convert { + +template <> +struct DiscreteLayerBundleRosTraits< + examples::field_map::DiscreteLayers> { + static auto layers(examples::field_map::DiscreteLayers& layers) { + return descriptorNamedDiscreteLayersForRos(layers); + } + + static auto layers(const examples::field_map::DiscreteLayers& layers) { + return descriptorNamedDiscreteLayersForRos(layers); + } +}; + +} // namespace wavemap::convert + +namespace layered_map_viz { + +struct FieldClassColorPolicy { + bool operator()(int value, std_msgs::ColorRGBA& color_msg) const { + using ClassLabel = wavemap::examples::field_map::ClassLabel; + switch (static_cast(value)) { + case ClassLabel::kGround: + color_msg = color(0.55f, 0.32f, 0.12f, 0.9f); + return true; + case ClassLabel::kObstacle: + color_msg = color(0.85f, 0.15f, 0.1f, 0.9f); + return true; + case ClassLabel::kUnknown: + default: + color_msg = color(0.5f, 0.5f, 0.5f, 0.5f); + return true; + } + } +}; + +template <> +struct DiscreteLayerMarkerTraits< + wavemap::examples::field_map::DiscreteLayers> { + static auto layers( + const wavemap::examples::field_map::DiscreteLayers& layers) { + return std::make_tuple(namedDiscreteMarkerLayer( + std::string(wavemap::layered::LayerTraits::name), layers.get(), + FieldClassColorPolicy{})); + } +}; + +} // namespace layered_map_viz + +#endif // WAVEMAP_EXAMPLES_CPP_DIGITAL_TWIN_FIELD_LAYERED_MAP_ROS_CONFIG_H_ diff --git a/examples/cpp/digital_twin/common/layered_ros_converter.h b/examples/cpp/digital_twin/common/layered_ros_converter.h index 879e421d8..645c4e239 100644 --- a/examples/cpp/digital_twin/common/layered_ros_converter.h +++ b/examples/cpp/digital_twin/common/layered_ros_converter.h @@ -5,6 +5,7 @@ #include #include +#include #include "layered_voxel_config.h" @@ -27,6 +28,10 @@ struct LayeredVoxelRosConverter { return types; } + static std::vector layerVisualizations() { + return {}; + } + static std::vector makeLayers() { // The message stores custom values layer-major: all color values go into // the color layer, and all traversability values go into the traversability @@ -41,6 +46,24 @@ struct LayeredVoxelRosConverter { return layers; } + static std::vector makeLayerMinimums( + const ContinuousWaveletMap::ThresholdConfig& config) { + auto layers = makeLayers(); + layers[0].float32_values = {config.data.rgb_min.r, config.data.rgb_min.g, + config.data.rgb_min.b}; + layers[1].float32_values = {config.data.traversability_min}; + return layers; + } + + static std::vector makeLayerMaximums( + const ContinuousWaveletMap::ThresholdConfig& config) { + auto layers = makeLayers(); + layers[0].float32_values = {config.data.rgb_max.r, config.data.rgb_max.g, + config.data.rgb_max.b}; + layers[1].float32_values = {config.data.traversability_max}; + return layers; + } + static void appendLayerValues(const LayeredVoxel& voxel, std::vector& layers) { // Color uses three float values per voxel/coefficient: r, g, and b. diff --git a/examples/cpp/digital_twin/common/layered_voxel_updater.h b/examples/cpp/digital_twin/common/layered_voxel_updater.h index 54d0cb132..1e01c118b 100644 --- a/examples/cpp/digital_twin/common/layered_voxel_updater.h +++ b/examples/cpp/digital_twin/common/layered_voxel_updater.h @@ -4,6 +4,7 @@ #include #include +#include #include "layered_voxel_config.h" @@ -15,10 +16,7 @@ struct ContinuousLayerObservation { inline void setContinuousLayersAtVoxel( ContinuousWaveletMap& map, const wavemap::Index3D& index, const ContinuousLayers& observed_data) { - const LayeredVoxel current_voxel = map.getVoxelValue(index); - const ContinuousLayers data_update = - ContinuousLayersPolicy::subtract(observed_data, current_voxel.data); - map.addToVoxelValue(index, LayeredVoxel(0.f, data_update)); + wavemap::layered::setContinuousLayersAtVoxel(map, index, observed_data); } inline void setContinuousLayers( diff --git a/examples/cpp/digital_twin/io/inspect_field_layered_map.cc b/examples/cpp/digital_twin/io/inspect_field_layered_map.cc new file mode 100644 index 000000000..f3eb1c079 --- /dev/null +++ b/examples/cpp/digital_twin/io/inspect_field_layered_map.cc @@ -0,0 +1,58 @@ +#include +#include +#include +#include +#include + +#include "../common/field_layered_map_config.h" + +namespace field_map = wavemap::examples::field_map; + +int main(int argc, char** argv) { + if (argc != 2) { + std::cerr << "Usage: " << argv[0] << " \n"; + return 1; + } + + field_map::Map map(field_map::makeDefaultConfig()); + std::string error; + if (!field_map::MapIo::load(std::filesystem::path(argv[1]), map, &error)) { + std::cerr << "Could not load field layered map: " << error << "\n"; + return 1; + } + + size_t leaves = 0u; + size_t observed_occupancy = 0u; + size_t observed_reflectivity = 0u; + float min_reflectivity = std::numeric_limits::max(); + float max_reflectivity = std::numeric_limits::lowest(); + map.continuousMap().forEachVoxelLeaf( + [&](const wavemap::OctreeIndex&, const field_map::Voxel& voxel) { + ++leaves; + if (1e-5f < std::abs(voxel.occupancy)) { + ++observed_occupancy; + } + const float reflectivity = + voxel.data.get(); + if (1e-5f < std::abs(reflectivity)) { + ++observed_reflectivity; + min_reflectivity = std::min(min_reflectivity, reflectivity); + max_reflectivity = std::max(max_reflectivity, reflectivity); + } + }); + + std::cout << "Field layered map inspection\n" + << " allocated blocks: " + << map.continuousMap().getHashMap().size() << "\n" + << " leaf nodes: " << leaves << "\n" + << " leaves with occupancy evidence: " << observed_occupancy + << "\n" + << " leaves with reflectivity: " << observed_reflectivity << "\n"; + if (observed_reflectivity) { + std::cout << " nonzero reflectivity range: [" << min_reflectivity << ", " + << max_reflectivity << "]\n"; + } + + return observed_occupancy && observed_reflectivity ? 0 : 2; +} + diff --git a/examples/cpp/digital_twin/io/inspect_ouster_benchmark_maps.cc b/examples/cpp/digital_twin/io/inspect_ouster_benchmark_maps.cc new file mode 100644 index 000000000..9e8a3e7df --- /dev/null +++ b/examples/cpp/digital_twin/io/inspect_ouster_benchmark_maps.cc @@ -0,0 +1,200 @@ +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include "../ouster_mapping/occupancy_map_layers.h" +#include "../ouster_mapping/reflectivity_map_layers.h" +#include "../ouster_mapping/reflectivity_only_map_layers.h" + +namespace occupancy_map = wavemap::examples::occupancy_map; +namespace reflectivity_only_map = wavemap::examples::reflectivity_only_map; +namespace reflectivity_map = wavemap::examples::reflectivity_map; + +namespace { +struct OccupancyStats { + size_t blocks = 0u; + size_t leaves = 0u; + size_t observed = 0u; + float min = std::numeric_limits::max(); + float max = std::numeric_limits::lowest(); +}; + +template +OccupancyStats occupancyStats(const MapT& map) { + OccupancyStats stats; + stats.blocks = map.getHashMap().size(); + map.forEachLeaf([&](const wavemap::OctreeIndex&, float occupancy) { + ++stats.leaves; + stats.observed += 1e-5f < std::abs(occupancy); + stats.min = std::min(stats.min, occupancy); + stats.max = std::max(stats.max, occupancy); + }); + return stats; +} + +struct Comparison { + size_t samples = 0u; + size_t above_tolerance = 0u; + double absolute_error_sum = 0.; + float max_absolute_error = 0.f; +}; + +template +Comparison compareDirection(const LhsMapT& lhs, const RhsMapT& rhs, + float tolerance = 1e-4f) { + Comparison comparison; + lhs.forEachLeaf([&](const wavemap::OctreeIndex& index, float lhs_value) { + const float error = std::abs(lhs_value - rhs.getCellValue(index)); + ++comparison.samples; + comparison.above_tolerance += tolerance < error; + comparison.absolute_error_sum += error; + comparison.max_absolute_error = + std::max(comparison.max_absolute_error, error); + }); + return comparison; +} + +template +void printComparison(const std::string& name, const LhsMapT& lhs, + const RhsMapT& rhs) { + const Comparison forward = compareDirection(lhs, rhs); + const Comparison reverse = compareDirection(rhs, lhs); + const auto print = [](const char* direction, const Comparison& comparison) { + std::cout << " " << direction << ": samples=" << comparison.samples + << " above_1e-4=" << comparison.above_tolerance + << " mean_abs=" + << (comparison.samples + ? comparison.absolute_error_sum / comparison.samples + : 0.) + << " max_abs=" << comparison.max_absolute_error << "\n"; + }; + std::cout << " " << name << "\n"; + print("forward", forward); + print("reverse", reverse); +} + +template +void printReflectivityStats(const std::string& name, + const typename DefinitionT::Map& map) { + size_t nonzero = 0u; + size_t out_of_bounds = 0u; + float min_value = std::numeric_limits::max(); + float max_value = std::numeric_limits::lowest(); + map.continuousMap().forEachVoxelLeaf( + [&](const wavemap::OctreeIndex&, + const typename DefinitionT::Voxel& voxel) { + const float value = voxel.data.template get(); + nonzero += 1e-6f < std::abs(value); + out_of_bounds += value < 0.f || 1.f < value; + min_value = std::min(min_value, value); + max_value = std::max(max_value, value); + }); + std::cout << " " << name << ": nonzero=" << nonzero + << " out_of_[0,1]=" << out_of_bounds + << " value_range=[" << min_value << ", " << max_value << "]\n"; +} + +void printOccupancyStats(const std::string& name, + const OccupancyStats& stats) { + std::cout << " " << name << ": blocks=" << stats.blocks + << " leaves=" << stats.leaves << " observed=" << stats.observed + << " range=[" << stats.min << ", " << stats.max << "]\n"; +} +} // namespace + +int main(int argc, char** argv) { + if (argc != 5) { + std::cerr << "Usage: " << argv[0] + << " \n"; + return 1; + } + + wavemap::MapBase::Ptr original_base; + if (!wavemap::io::fileToMap(argv[1], original_base)) { + std::cerr << "Could not load A\n"; + return 2; + } + auto original = + std::dynamic_pointer_cast(original_base); + if (!original) { + std::cerr << "A is not a HashedWaveletOctree\n"; + return 2; + } + + occupancy_map::Definition::Map occupancy(occupancy_map::makeMapConfig()); + reflectivity_only_map::Definition::Map reflectivity( + reflectivity_only_map::makeMapConfig()); + reflectivity_map::Definition::Map reflectivity_class( + reflectivity_map::makeMapConfig()); + std::string error; + if (!occupancy_map::Definition::MapIo::load(argv[2], occupancy, &error)) { + std::cerr << "Could not load B: " << error << "\n"; + return 2; + } + if (!reflectivity_only_map::Definition::MapIo::load(argv[3], reflectivity, + &error)) { + std::cerr << "Could not load C: " << error << "\n"; + return 2; + } + if (!reflectivity_map::Definition::MapIo::load(argv[4], reflectivity_class, + &error)) { + std::cerr << "Could not load D: " << error << "\n"; + return 2; + } + + std::cout << "Occupancy statistics\n"; + printOccupancyStats("A original", occupancyStats(*original)); + printOccupancyStats("B layered occupancy", + occupancyStats(occupancy.continuousMap())); + printOccupancyStats("C reflectivity", + occupancyStats(reflectivity.continuousMap())); + printOccupancyStats("D reflectivity+class", + occupancyStats(reflectivity_class.continuousMap())); + + std::cout << "Occupancy comparisons\n"; + printComparison("A vs B", *original, occupancy.continuousMap()); + printComparison("A vs C", *original, reflectivity.continuousMap()); + printComparison("A vs D", *original, reflectivity_class.continuousMap()); + printComparison("B vs C", occupancy.continuousMap(), + reflectivity.continuousMap()); + printComparison("C vs D", reflectivity.continuousMap(), + reflectivity_class.continuousMap()); + + std::cout << "Reflectivity statistics\n"; + printReflectivityStats( + "C reflectivity", reflectivity); + printReflectivityStats( + "D reflectivity", reflectivity_class); + + const auto& class_layer = + reflectivity_class.discreteLayers().get(); + size_t ground = 0u; + size_t obstacle = 0u; + for (const auto& [key, cell] : class_layer.cells()) { + (void)key; + for (const int offset : cell.observed_offsets) { + const auto exception = cell.exceptions.find(offset); + const int value = exception == cell.exceptions.end() + ? cell.dominant_value + : exception->second; + ground += value == static_cast( + wavemap::layered::GeometricClass::kGround); + obstacle += value == static_cast( + wavemap::layered::GeometricClass::kObstacle); + } + } + std::cout << "Class statistics\n" + << " observed=" << class_layer.observedValueCount() + << " ground=" << ground << " obstacle=" << obstacle << "\n"; + return 0; +} diff --git a/examples/cpp/digital_twin/io/inspect_ouster_benchmark_maps.cc.orig b/examples/cpp/digital_twin/io/inspect_ouster_benchmark_maps.cc.orig new file mode 100644 index 000000000..ea516a6ab --- /dev/null +++ b/examples/cpp/digital_twin/io/inspect_ouster_benchmark_maps.cc.orig @@ -0,0 +1,211 @@ +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include "../ouster_mapping/occupancy_map_layers.h" +#include "../ouster_mapping/reflectivity_map_layers.h" +#include "../ouster_mapping/reflectivity_only_map_layers.h" + +namespace occupancy_map = wavemap::examples::occupancy_map; +namespace reflectivity_only_map = wavemap::examples::reflectivity_only_map; +namespace reflectivity_map = wavemap::examples::reflectivity_map; + +namespace { +struct OccupancyStats { + size_t blocks = 0u; + size_t leaves = 0u; + size_t observed = 0u; + float min = std::numeric_limits::max(); + float max = std::numeric_limits::lowest(); +}; + +template +OccupancyStats occupancyStats(const MapT& map) { + OccupancyStats stats; + stats.blocks = map.getHashMap().size(); + map.forEachLeaf([&](const wavemap::OctreeIndex&, float occupancy) { + ++stats.leaves; + stats.observed += 1e-5f < std::abs(occupancy); + stats.min = std::min(stats.min, occupancy); + stats.max = std::max(stats.max, occupancy); + }); + return stats; +} + +struct Comparison { + size_t samples = 0u; + size_t above_tolerance = 0u; + double absolute_error_sum = 0.; + float max_absolute_error = 0.f; +}; + +template +Comparison compareDirection(const LhsMapT& lhs, const RhsMapT& rhs, + float tolerance = 1e-4f) { + Comparison comparison; + lhs.forEachLeaf([&](const wavemap::OctreeIndex& index, float lhs_value) { + const float error = std::abs(lhs_value - rhs.getCellValue(index)); + ++comparison.samples; + comparison.above_tolerance += tolerance < error; + comparison.absolute_error_sum += error; + comparison.max_absolute_error = + std::max(comparison.max_absolute_error, error); + }); + return comparison; +} + +template +void printComparison(const std::string& name, const LhsMapT& lhs, + const RhsMapT& rhs) { + const Comparison forward = compareDirection(lhs, rhs); + const Comparison reverse = compareDirection(rhs, lhs); + const auto print = [](const char* direction, const Comparison& comparison) { + std::cout << " " << direction << ": samples=" << comparison.samples + << " above_1e-4=" << comparison.above_tolerance + << " mean_abs=" + << (comparison.samples + ? comparison.absolute_error_sum / comparison.samples + : 0.) + << " max_abs=" << comparison.max_absolute_error << "\n"; + }; + std::cout << " " << name << "\n"; + print("forward", forward); + print("reverse", reverse); +} + +template +void printReflectivityStats(const std::string& name, + const typename DefinitionT::Map& map) { + size_t observed = 0u; + size_t out_of_bounds = 0u; + size_t very_small_weight = 0u; + float min_value = std::numeric_limits::max(); + float max_value = std::numeric_limits::lowest(); + float min_weight = std::numeric_limits::max(); + float max_weight = 0.f; + map.continuousMap().forEachVoxelLeaf( + [&](const wavemap::OctreeIndex&, + const typename DefinitionT::Voxel& voxel) { + const auto& state = voxel.data.template get(); + if (0.f < state.total_weight) { + const float value = state.valueOr(); + ++observed; + out_of_bounds += value < 0.f || 1.f < value; + very_small_weight += state.total_weight < 1e-3f; + min_value = std::min(min_value, value); + max_value = std::max(max_value, value); + min_weight = std::min(min_weight, state.total_weight); + max_weight = std::max(max_weight, state.total_weight); + } + }); + std::cout << " " << name << ": observed=" << observed + << " out_of_[0,1]=" << out_of_bounds + << " weight_below_1e-3=" << very_small_weight + << " value_range=[" << min_value << ", " << max_value << "]" + << " weight_range=[" << min_weight << ", " << max_weight << "]\n"; +} + +void printOccupancyStats(const std::string& name, + const OccupancyStats& stats) { + std::cout << " " << name << ": blocks=" << stats.blocks + << " leaves=" << stats.leaves << " observed=" << stats.observed + << " range=[" << stats.min << ", " << stats.max << "]\n"; +} +} // namespace + +int main(int argc, char** argv) { + if (argc != 5) { + std::cerr << "Usage: " << argv[0] + << " \n"; + return 1; + } + + wavemap::MapBase::Ptr original_base; + if (!wavemap::io::fileToMap(argv[1], original_base)) { + std::cerr << "Could not load A\n"; + return 2; + } + auto original = + std::dynamic_pointer_cast(original_base); + if (!original) { + std::cerr << "A is not a HashedWaveletOctree\n"; + return 2; + } + + occupancy_map::Definition::Map occupancy(occupancy_map::makeMapConfig()); + reflectivity_only_map::Definition::Map reflectivity( + reflectivity_only_map::makeMapConfig()); + reflectivity_map::Definition::Map reflectivity_class( + reflectivity_map::makeMapConfig()); + std::string error; + if (!occupancy_map::Definition::MapIo::load(argv[2], occupancy, &error)) { + std::cerr << "Could not load B: " << error << "\n"; + return 2; + } + if (!reflectivity_only_map::Definition::MapIo::load(argv[3], reflectivity, + &error)) { + std::cerr << "Could not load C: " << error << "\n"; + return 2; + } + if (!reflectivity_map::Definition::MapIo::load(argv[4], reflectivity_class, + &error)) { + std::cerr << "Could not load D: " << error << "\n"; + return 2; + } + + std::cout << "Occupancy statistics\n"; + printOccupancyStats("A original", occupancyStats(*original)); + printOccupancyStats("B layered occupancy", + occupancyStats(occupancy.continuousMap())); + printOccupancyStats("C reflectivity", + occupancyStats(reflectivity.continuousMap())); + printOccupancyStats("D reflectivity+class", + occupancyStats(reflectivity_class.continuousMap())); + + std::cout << "Occupancy comparisons\n"; + printComparison("A vs B", *original, occupancy.continuousMap()); + printComparison("A vs C", *original, reflectivity.continuousMap()); + printComparison("A vs D", *original, reflectivity_class.continuousMap()); + printComparison("B vs C", occupancy.continuousMap(), + reflectivity.continuousMap()); + printComparison("C vs D", reflectivity.continuousMap(), + reflectivity_class.continuousMap()); + + std::cout << "Reflectivity statistics\n"; + printReflectivityStats( + "C reflectivity", reflectivity); + printReflectivityStats( + "D reflectivity", reflectivity_class); + + const auto& class_layer = + reflectivity_class.discreteLayers().get(); + size_t ground = 0u; + size_t obstacle = 0u; + for (const auto& [key, cell] : class_layer.cells()) { + (void)key; + for (const int offset : cell.observed_offsets) { + const auto exception = cell.exceptions.find(offset); + const int value = exception == cell.exceptions.end() + ? cell.dominant_value + : exception->second; + ground += value == static_cast( + wavemap::layered::GeometricClass::kGround); + obstacle += value == static_cast( + wavemap::layered::GeometricClass::kObstacle); + } + } + std::cout << "Class statistics\n" + << " observed=" << class_layer.observedValueCount() + << " ground=" << ground << " obstacle=" << obstacle << "\n"; + return 0; +} diff --git a/examples/cpp/digital_twin/io/inspect_reflectivity_layered_map.cc b/examples/cpp/digital_twin/io/inspect_reflectivity_layered_map.cc new file mode 100644 index 000000000..7553f2ad8 --- /dev/null +++ b/examples/cpp/digital_twin/io/inspect_reflectivity_layered_map.cc @@ -0,0 +1,98 @@ +#include +#include +#include +#include +#include + +#include + +#include "../ouster_mapping/reflectivity_map_layers.h" + +namespace reflectivity_map = wavemap::examples::reflectivity_map; + +int main(int argc, char** argv) { + if (argc != 2) { + std::cerr << "Usage: " << argv[0] << " \n"; + return 1; + } + + reflectivity_map::Definition::Map map(reflectivity_map::makeMapConfig()); + std::string error; + if (!reflectivity_map::Definition::MapIo::load( + std::filesystem::path(argv[1]), map, &error)) { + std::cerr << "Could not load reflectivity layered map: " << error << "\n"; + return 1; + } + + size_t leaves = 0u; + size_t observed_occupancy = 0u; + size_t observed_reflectivity = 0u; + float min_reflectivity = std::numeric_limits::max(); + float max_reflectivity = std::numeric_limits::lowest(); + size_t out_of_bounds_reflectivity = 0u; + size_t ground_classes = 0u; + size_t obstacle_classes = 0u; + const auto& storage_bounds = + map.continuousMap() + .getThresholdConfig() + .data.get(); + map.continuousMap().forEachVoxelLeaf( + [&](const wavemap::OctreeIndex&, + const reflectivity_map::Definition::Voxel& voxel) { + ++leaves; + if (1e-5f < std::abs(voxel.occupancy)) { + ++observed_occupancy; + } + const float reflectivity = + voxel.data.get(); + if (1e-6f < std::abs(reflectivity)) { + ++observed_reflectivity; + } + out_of_bounds_reflectivity += + reflectivity < storage_bounds.min || + storage_bounds.max < reflectivity; + min_reflectivity = std::min(min_reflectivity, reflectivity); + max_reflectivity = std::max(max_reflectivity, reflectivity); + }); + + const auto& class_layer = + map.discreteLayers().get(); + for (const auto& [parent_key, cell] : class_layer.cells()) { + (void)parent_key; + for (const int offset : cell.observed_offsets) { + const auto exception = cell.exceptions.find(offset); + const int value = exception == cell.exceptions.end() + ? cell.dominant_value + : exception->second; + ground_classes += value == static_cast( + wavemap::layered::GeometricClass::kGround); + obstacle_classes += + value == static_cast( + wavemap::layered::GeometricClass::kObstacle); + } + } + + std::cout << "Reflectivity layered map inspection\n" + << " allocated blocks: " + << map.continuousMap().getHashMap().size() << "\n" + << " leaf nodes: " << leaves << "\n" + << " leaves with occupancy evidence: " << observed_occupancy + << "\n" + << " leaves with reflectivity: " << observed_reflectivity << "\n" + << " class values: " << class_layer.observedValueCount() << "\n" + << " ground: " << ground_classes << "\n" + << " obstacle: " << obstacle_classes << "\n"; + std::cout << " configured reflectivity storage range: [" + << storage_bounds.min << ", " << storage_bounds.max << "]\n"; + if (leaves) { + std::cout << " reconstructed reflectivity range: [" << min_reflectivity + << ", " << max_reflectivity << "]\n" + << " reflectivity values outside storage bounds: " + << out_of_bounds_reflectivity << "\n"; + } + + return observed_occupancy && observed_reflectivity && + class_layer.observedValueCount() + ? 0 + : 2; +} diff --git a/examples/cpp/digital_twin/io/inspect_reflectivity_layered_map.cc.orig b/examples/cpp/digital_twin/io/inspect_reflectivity_layered_map.cc.orig new file mode 100644 index 000000000..60a9bf65e --- /dev/null +++ b/examples/cpp/digital_twin/io/inspect_reflectivity_layered_map.cc.orig @@ -0,0 +1,101 @@ +#include +#include +#include +#include +#include + +#include + +#include "../ouster_mapping/reflectivity_map_layers.h" + +namespace reflectivity_map = wavemap::examples::reflectivity_map; + +int main(int argc, char** argv) { + if (argc != 2) { + std::cerr << "Usage: " << argv[0] << " \n"; + return 1; + } + + reflectivity_map::Definition::Map map(reflectivity_map::makeMapConfig()); + std::string error; + if (!reflectivity_map::Definition::MapIo::load( + std::filesystem::path(argv[1]), map, &error)) { + std::cerr << "Could not load reflectivity layered map: " << error << "\n"; + return 1; + } + + size_t leaves = 0u; + size_t observed_occupancy = 0u; + size_t observed_reflectivity = 0u; + float min_reflectivity = std::numeric_limits::max(); + float max_reflectivity = std::numeric_limits::lowest(); + float max_reflectivity_weight = 0.f; + size_t ground_classes = 0u; + size_t obstacle_classes = 0u; + const auto& storage_bounds = + map.continuousMap() + .getThresholdConfig() + .data.get(); + map.continuousMap().forEachVoxelLeaf( + [&](const wavemap::OctreeIndex&, + const reflectivity_map::Definition::Voxel& voxel) { + ++leaves; + if (1e-5f < std::abs(voxel.occupancy)) { + ++observed_occupancy; + } + const auto& reflectivity_state = + voxel.data.get(); + if (0.f < reflectivity_state.total_weight) { + const float reflectivity = + wavemap::layered::LayerStateConversion< + reflectivity_map::ReflectivityLayer>::value( + reflectivity_state); + ++observed_reflectivity; + min_reflectivity = std::min(min_reflectivity, reflectivity); + max_reflectivity = std::max(max_reflectivity, reflectivity); + max_reflectivity_weight = std::max( + max_reflectivity_weight, reflectivity_state.total_weight); + } + }); + + const auto& class_layer = + map.discreteLayers().get(); + for (const auto& [parent_key, cell] : class_layer.cells()) { + (void)parent_key; + for (const int offset : cell.observed_offsets) { + const auto exception = cell.exceptions.find(offset); + const int value = exception == cell.exceptions.end() + ? cell.dominant_value + : exception->second; + ground_classes += value == static_cast( + wavemap::layered::GeometricClass::kGround); + obstacle_classes += + value == static_cast( + wavemap::layered::GeometricClass::kObstacle); + } + } + + std::cout << "Reflectivity layered map inspection\n" + << " allocated blocks: " + << map.continuousMap().getHashMap().size() << "\n" + << " leaf nodes: " << leaves << "\n" + << " leaves with occupancy evidence: " << observed_occupancy + << "\n" + << " leaves with reflectivity: " << observed_reflectivity << "\n" + << " class values: " << class_layer.observedValueCount() << "\n" + << " ground: " << ground_classes << "\n" + << " obstacle: " << obstacle_classes << "\n"; + std::cout << " configured reflectivity storage range: [" + << storage_bounds.min << ", " << storage_bounds.max << "]\n"; + if (observed_reflectivity) { + std::cout << " nonzero reflectivity range: [" << min_reflectivity << ", " + << max_reflectivity << "]\n" + << " maximum reflectivity weight: " + << max_reflectivity_weight << "\n"; + } + + return observed_occupancy && observed_reflectivity && + class_layer.observedValueCount() + ? 0 + : 2; +} diff --git a/examples/cpp/digital_twin/map_api/user_facing/field_endpoint_integration_experiment.cc b/examples/cpp/digital_twin/map_api/user_facing/field_endpoint_integration_experiment.cc new file mode 100644 index 000000000..1066c15c2 --- /dev/null +++ b/examples/cpp/digital_twin/map_api/user_facing/field_endpoint_integration_experiment.cc @@ -0,0 +1,68 @@ +#include +#include +#include + +#include +#include +#include +#include + +#include "../../common/field_layered_map_config.h" + +namespace field_map = wavemap::examples::field_map; + +int main() { + const auto map_config = field_map::makeDefaultConfig(); + field_map::Map layered_map(map_config); + auto baseline_map = std::make_shared( + map_config.continuous_map); + wavemap::RayTracingIntegratorConfig config; + config.min_range = 0.1f; + config.max_range = 20.f; + wavemap::RayTracingIntegrator occupancy_integrator( + config, layered_map.continuousMapPtr()); + wavemap::RayTracingIntegrator baseline_integrator(config, baseline_map); + + using Measurement = wavemap::layered::PosedEndpointAttributePointcloud< + field_map::ReflectivityLayer>; + Measurement measurement{wavemap::Transformation3D()}; + measurement.resize(2u); + measurement.point(0u) = wavemap::Point3D(1.1f, 0.1f, 0.1f); + measurement.point(1u) = wavemap::Point3D(1.1f, 1.1f, 0.1f); + measurement.attribute(0u) = 0.25f; + measurement.attribute(1u) = 0.75f; + + wavemap::PosedPointcloud<> baseline_measurement( + measurement.getPose(), measurement.points().data()); + baseline_integrator.integrate(baseline_measurement); + wavemap::layered::EndpointAttributeIntegrator< + field_map::Map, field_map::ReflectivityLayer> + integrator(layered_map, occupancy_integrator); + const auto result = integrator.integrate(measurement); + + const float cell_width_inv = + 1.f / layered_map.continuousMap().getMinCellWidth(); + bool occupancy_matches = true; + bool reflectivity_matches = true; + for (size_t i = 0u; i < measurement.size(); ++i) { + const wavemap::Point3D endpoint = measurement.point(i); + const auto index = + wavemap::convert::pointToNearestIndex(endpoint, cell_width_inv); + occupancy_matches &= + std::abs(layered_map.continuousMap().getCellValue(index) - + baseline_map->getCellValue(index)) < 1e-6f; + const auto voxel = layered_map.continuousMap().getVoxelValue(index); + reflectivity_matches &= + std::abs(voxel.data.template get() - + measurement.attribute(i)) < + 1e-6f; + } + const bool pass = result.integrated == 2u && occupancy_matches && + reflectivity_matches; + std::cout << "Field endpoint integration Phase 9 experiment\n" + << " original occupancy behavior preserved: " + << (occupancy_matches ? "pass" : "FAIL") << '\n' + << " reflectivity updated at matching endpoints: " + << (reflectivity_matches ? "pass" : "FAIL") << '\n'; + return pass ? 0 : 1; +} diff --git a/examples/cpp/digital_twin/map_api/user_facing/field_layered_map_experiment.cc b/examples/cpp/digital_twin/map_api/user_facing/field_layered_map_experiment.cc new file mode 100644 index 000000000..ad8862845 --- /dev/null +++ b/examples/cpp/digital_twin/map_api/user_facing/field_layered_map_experiment.cc @@ -0,0 +1,107 @@ +#include +#include +#include +#include + +#include + +#include "../../common/field_layered_map_config.h" + +namespace field_map = wavemap::examples::field_map; + +namespace { +bool approximatelyEqual(float lhs, float rhs, float tolerance = 1e-5f) { + return std::abs(lhs - rhs) <= tolerance; +} + +bool expect(const std::string& label, bool condition) { + std::cout << " " << label << ": " << (condition ? "pass" : "FAIL") + << "\n"; + return condition; +} +} // namespace + +int main() { + field_map::Map map(field_map::makeDefaultConfig()); + const wavemap::Index3D index(2, -1, 3); + map.continuousMap().setVoxelValue( + index, field_map::Voxel( + 0.7f, field_map::makeContinuousLayers(0.2f))); + + const field_map::Voxel before = map.continuousMap().getVoxelValue(index); + const wavemap::layered::LayerObservation observation( + wavemap::Point3D::Zero(), 1.2f); + const field_map::ReflectivityUpdatePolicy policy; + wavemap::layered::updateContinuousLayersAtVoxel( + map.continuousMap(), index, [&](field_map::ContinuousLayers& layers) { + auto& reflectivity = layers.get(); + reflectivity = policy(reflectivity, observation); + }); + + map.discreteLayers().get().setValue( + index, static_cast(field_map::ClassLabel::kObstacle)); + map.continuousMap().threshold(); + + const field_map::Voxel after = map.continuousMap().getVoxelValue(index); + const auto& configured_reflectivity_bounds = + map.continuousMap() + .getThresholdConfig() + .data.get(); + const auto class_value = + map.discreteLayers().get().getValue(index); + const auto schema = wavemap::layered::io::layeredMapSchema(map); + + bool ok = true; + std::cout << "Field LayeredMap generated-schema experiment\n"; + ok &= expect("occupancy preserved", + approximatelyEqual(before.occupancy, after.occupancy)); + ok &= expect("reflectivity thresholded to configured range", + approximatelyEqual( + after.data.get(), + configured_reflectivity_bounds.max)); + ok &= expect("class stored outside wavelet map", + class_value && + *class_value == + static_cast(field_map::ClassLabel::kObstacle)); + ok &= expect("schema has occupancy and reflectivity", + schema.continuous_layers.size() == 2u && + schema.continuous_layers[1].name == "reflectivity"); + ok &= expect("schema has one class layer", + schema.discrete_layers.size() == 1u && + schema.discrete_layers.front().name == "class"); + + const std::filesystem::path file_path = + "/tmp/field_layered_map_generated_schema.lwvmp"; + field_map::Map loaded(field_map::makeDefaultConfig()); + const auto* const continuous_map_before_load = + loaded.continuousMapPtr().get(); + const bool saved_and_loaded = + field_map::MapIo::save(file_path, map) && + field_map::MapIo::load(file_path, loaded); + const auto loaded_voxel = loaded.continuousMap().getVoxelValue(index); + const auto loaded_class = + loaded.discreteLayers().get().getValue(index); + ok &= expect( + "persistence round trip", + saved_and_loaded && + approximatelyEqual( + loaded_voxel.data.get(), + configured_reflectivity_bounds.max) && + loaded_class && + *loaded_class == + static_cast(field_map::ClassLabel::kObstacle)); + ok &= expect("load preserves continuous map allocation", + loaded.continuousMapPtr().get() == + continuous_map_before_load); + const auto& loaded_reflectivity_bounds = + loaded.continuousMap() + .getThresholdConfig() + .data.get(); + ok &= expect("load preserves runtime layer thresholds", + approximatelyEqual(loaded_reflectivity_bounds.min, + configured_reflectivity_bounds.min) && + approximatelyEqual(loaded_reflectivity_bounds.max, + configured_reflectivity_bounds.max)); + + return ok ? 0 : 1; +} diff --git a/examples/cpp/digital_twin/map_api/user_facing/field_layered_pipeline_experiment.cc b/examples/cpp/digital_twin/map_api/user_facing/field_layered_pipeline_experiment.cc new file mode 100644 index 000000000..5caf171b3 --- /dev/null +++ b/examples/cpp/digital_twin/map_api/user_facing/field_layered_pipeline_experiment.cc @@ -0,0 +1,80 @@ +#include +#include +#include +#include +#include + +#include +#include + +#include "../../common/field_layered_map_config.h" + +namespace field_map = wavemap::examples::field_map; + +namespace { +bool approximatelyEqual(float lhs, float rhs, float tolerance = 1e-5f) { + return std::abs(lhs - rhs) <= tolerance; +} + +bool expect(const std::string& label, bool condition) { + std::cout << " " << label << ": " << (condition ? "pass" : "FAIL") + << "\n"; + return condition; +} +} // namespace + +int main() { + field_map::Map map(field_map::makeDefaultConfig()); + wavemap::layered::LayeredPipeline pipeline(map); + + const wavemap::Point3D point(0.7f, -0.1f, 0.8f); + const wavemap::Index3D index = wavemap::convert::pointToNearestIndex( + point, 1.f / map.continuousMap().getMinCellWidth()); + map.continuousMap().setVoxelValue( + index, field_map::Voxel( + 0.6f, field_map::makeContinuousLayers(0.2f))); + + const std::vector> + reflectivity_observations{ + {point, 0.4f}, + {point, 0.65f}, + {wavemap::Point3D( + std::numeric_limits::quiet_NaN(), 0.f, 0.f), + 0.9f}}; + const auto reflectivity_result = + pipeline.integrate( + reflectivity_observations); + + const std::vector> + class_observations{ + {point, + static_cast(field_map::ClassLabel::kGround)}, + {point, + static_cast(field_map::ClassLabel::kObstacle)}}; + const auto class_result = + pipeline.integrate(class_observations); + + const auto voxel = map.continuousMap().getVoxelValue(index); + const auto class_value = + map.discreteLayers().get().getValue(index); + + bool ok = true; + std::cout << "Field LayeredMap Phase 3 pipeline experiment\n"; + ok &= expect("world position converted to voxel index", + reflectivity_result.updated_voxels == 1u); + ok &= expect("continuous observation integrated", + approximatelyEqual(voxel.data.get(), 0.65f)); + ok &= expect("occupancy preserved", + approximatelyEqual(voxel.occupancy, 0.6f)); + ok &= expect("invalid position rejected", + reflectivity_result.received == 3u && + reflectivity_result.integrated == 2u && + reflectivity_result.rejected == 1u); + ok &= expect( + "discrete policy and storage updated", + class_result.integrated == 2u && class_value && + *class_value == + static_cast(field_map::ClassLabel::kObstacle)); + + return ok ? 0 : 1; +} diff --git a/examples/cpp/digital_twin/ouster_mapping/occupancy_map_layers.h b/examples/cpp/digital_twin/ouster_mapping/occupancy_map_layers.h new file mode 100644 index 000000000..771e3afe1 --- /dev/null +++ b/examples/cpp/digital_twin/ouster_mapping/occupancy_map_layers.h @@ -0,0 +1,29 @@ +#ifndef WAVEMAP_EXAMPLES_DIGITAL_TWIN_OUSTER_OCCUPANCY_MAP_LAYERS_H_ +#define WAVEMAP_EXAMPLES_DIGITAL_TWIN_OUSTER_OCCUPANCY_MAP_LAYERS_H_ + +#include +#include +#include + +namespace wavemap::examples::occupancy_map { + +// Occupancy is built into every layered map, so this schema intentionally has +// no user-defined layers. It is benchmark variant B. +using Schema = layered::schema::LayerSchema<>; +using Definition = layered::LayeredMapDefinition; + +inline Definition::Config makeMapConfig() { + layered::LayeredMapConfigBuilder config; + config.map() + .minCellWidth(0.25f) + .occupancyLogOdds(-2.f, 4.f) + .treeHeight(7) + .onlyPruneBlocksIfUnusedFor(5.f); + config.occupancy().pruningScale(1e-3f).pruningWeight(1.f); + config.combinedPruningThreshold(1.f); + return config.build(); +} + +} // namespace wavemap::examples::occupancy_map + +#endif // WAVEMAP_EXAMPLES_DIGITAL_TWIN_OUSTER_OCCUPANCY_MAP_LAYERS_H_ diff --git a/examples/cpp/digital_twin/ouster_mapping/occupancy_sensor_inputs.cc b/examples/cpp/digital_twin/ouster_mapping/occupancy_sensor_inputs.cc new file mode 100644 index 000000000..e2a127bb3 --- /dev/null +++ b/examples/cpp/digital_twin/ouster_mapping/occupancy_sensor_inputs.cc @@ -0,0 +1,42 @@ +#include "occupancy_map_layers.h" + +#include + +#include +#include + +namespace wavemap::examples::occupancy_map { + +using RosConverter = + convert::DescriptorContinuousRosConverter; +using ServerBuilder = LayeredRosServerBuilder; + +void registerSensorInputs(RosServer& /*server*/, + ServerBuilder::Extension& /*extension*/) { + // Occupancy continues to use Wavemap's original point-cloud integration. +} + +} // namespace wavemap::examples::occupancy_map + +int main(int argc, char** argv) { + ros::init(argc, argv, "wavemap_ros_server"); + google::InitGoogleLogging(argv[0]); + google::InstallFailureSignalHandler(); + FLAGS_alsologtostderr = true; + FLAGS_colorlogtostderr = true; + + ros::NodeHandle nh; + ros::NodeHandle nh_private("~"); + wavemap::RosServer server( + nh, nh_private, + [](wavemap::RosServer& server, ros::NodeHandle private_nh) { + namespace map = wavemap::examples::occupancy_map; + map::ServerBuilder::install(server, private_nh, server.worldFrame(), + map::makeMapConfig(), + map::registerSensorInputs); + ROS_INFO("Installed layered occupancy-only benchmark application (B)."); + }); + + ros::spin(); + return 0; +} diff --git a/examples/cpp/digital_twin/ouster_mapping/ouster_sensor_inputs.cc b/examples/cpp/digital_twin/ouster_mapping/ouster_sensor_inputs.cc new file mode 100644 index 000000000..94ce7e737 --- /dev/null +++ b/examples/cpp/digital_twin/ouster_mapping/ouster_sensor_inputs.cc @@ -0,0 +1,59 @@ +#include "reflectivity_map_layers.h" + +#include + +#include +#include + +namespace wavemap::examples::reflectivity_map { // Ouster mapping example + +using RosConverter = + convert::DescriptorContinuousRosConverter; +using ServerBuilder = LayeredRosServerBuilder; + +void registerSensorInputs(RosServer& server, + ServerBuilder::Extension& extension) { + // The Ouster PointCloud2 stores reflectivity as a raw numeric field in + // [0, 255]. Decode it during the existing XYZ traversal, normalize it into + // the map's [0, 1] storage domain, and update occupied endpoints only. + server.bindNormalizedPointField( + extension, "reflectivity", 0.f, 255.f, + layered::EndpointRange{0.5f, 25.f}); + + // This bag has no class field. Derive a small example taxonomy directly + // from each posed cloud: 1 = local ground and 2 = obstacle. + layered::LocalElevationClassifierConfig class_config; + class_config.grid_cell_width = 0.5f; + class_config.ground_tolerance = 0.2f; + class_config.obstacle_height = 0.35f; + class_config.min_points_per_cell = 3u; + server.bindGeometricClass( + extension, class_config, layered::EndpointRange{0.5f, 25.f}); +} + +} // namespace wavemap::examples::reflectivity_map + +int main(int argc, char** argv) { + ros::init(argc, argv, "wavemap_ros_server"); + google::InitGoogleLogging(argv[0]); + google::InstallFailureSignalHandler(); + FLAGS_alsologtostderr = true; + FLAGS_colorlogtostderr = true; + + ros::NodeHandle nh; + ros::NodeHandle nh_private("~"); + wavemap::RosServer server( + nh, nh_private, + [](wavemap::RosServer& server, ros::NodeHandle private_nh) { + namespace map = wavemap::examples::reflectivity_map; + map::ServerBuilder::install(server, private_nh, server.worldFrame(), + map::makeMapConfig(), + map::registerSensorInputs); + ROS_INFO( + "Installed occupancy+reflectivity+class benchmark application " + "(D)."); + }); + + ros::spin(); + return 0; +} diff --git a/examples/cpp/digital_twin/ouster_mapping/reflectivity_map_layers.h b/examples/cpp/digital_twin/ouster_mapping/reflectivity_map_layers.h new file mode 100644 index 000000000..20669fa0d --- /dev/null +++ b/examples/cpp/digital_twin/ouster_mapping/reflectivity_map_layers.h @@ -0,0 +1,63 @@ +#ifndef WAVEMAP_EXAMPLES_DIGITAL_TWIN_OUSTER_REFLECTIVITY_MAP_LAYERS_H_ +#define WAVEMAP_EXAMPLES_DIGITAL_TWIN_OUSTER_REFLECTIVITY_MAP_LAYERS_H_ + +#include +#include + +#include +#include +#include +#include + +namespace wavemap::examples::reflectivity_map { + +using ReflectivityReplacePolicy = layered::ReplaceLayerUpdatePolicy; +using ReflectivityExponentialPolicy = + layered::ExponentialScalarLayerUpdatePolicy<1, 5>; + +template +struct ReflectivityLayerTemplate + : layered::schema::ContinuousLayer { + static constexpr layered::ScalarLayerVisualization visualization{ + {0.05f, 0.05f, 0.05f}, {1.f, 1.f, 1.f}}; + static constexpr std::string_view name = "reflectivity"; +}; + +// Both policies are user-selectable. This example uses alpha=0.2 exponential +// smoothing; changing this alias to ReflectivityReplacePolicy selects latest +// observation replacement without changing the adapter or pipeline. +using ReflectivityLayer = + ReflectivityLayerTemplate; + +struct ClassLayer : layered::schema::DiscreteLayer> { + static constexpr std::string_view name = "class"; + static constexpr std::array categories{ + layered::IntegerCategoryLabel{1, "Ground", {0.2f, 0.8f, 0.2f}}, + layered::IntegerCategoryLabel{2, "Obstacle", {0.9f, 0.2f, 0.15f}}}; +}; + +// Occupancy is built into the shared continuous voxel and is not repeated in +// the user schema. +using Schema = layered::schema::LayerSchema; +using Definition = layered::LayeredMapDefinition; + +inline Definition::Config makeMapConfig() { + layered::LayeredMapConfigBuilder config; + config.map() + .minCellWidth(0.25f) + .occupancyLogOdds(-2.f, 4.f) + .treeHeight(7) + .onlyPruneBlocksIfUnusedFor(5.f); + config.occupancy().pruningScale(1e-3f).pruningWeight(1.f); + config.layer() + .storageBounds(0.f, 1.f) + .pruningScale(1e-3f) + .pruningWeight(1.f); + config.discreteBlockHeight(0); + config.combinedPruningThreshold(1.f); + return config.build(); +} + +} // namespace wavemap::examples::reflectivity_map + +#endif // WAVEMAP_EXAMPLES_DIGITAL_TWIN_OUSTER_REFLECTIVITY_MAP_LAYERS_H_ diff --git a/examples/cpp/digital_twin/ouster_mapping/reflectivity_only_map_layers.h b/examples/cpp/digital_twin/ouster_mapping/reflectivity_only_map_layers.h new file mode 100644 index 000000000..f230e2f20 --- /dev/null +++ b/examples/cpp/digital_twin/ouster_mapping/reflectivity_only_map_layers.h @@ -0,0 +1,30 @@ +#ifndef WAVEMAP_EXAMPLES_DIGITAL_TWIN_OUSTER_REFLECTIVITY_ONLY_MAP_LAYERS_H_ +#define WAVEMAP_EXAMPLES_DIGITAL_TWIN_OUSTER_REFLECTIVITY_ONLY_MAP_LAYERS_H_ + +#include "reflectivity_map_layers.h" + +namespace wavemap::examples::reflectivity_only_map { + +using ReflectivityLayer = reflectivity_map::ReflectivityLayer; +using Schema = layered::schema::LayerSchema; +using Definition = layered::LayeredMapDefinition; + +inline Definition::Config makeMapConfig() { + layered::LayeredMapConfigBuilder config; + config.map() + .minCellWidth(0.25f) + .occupancyLogOdds(-2.f, 4.f) + .treeHeight(7) + .onlyPruneBlocksIfUnusedFor(5.f); + config.occupancy().pruningScale(1e-3f).pruningWeight(1.f); + config.layer() + .storageBounds(0.f, 1.f) + .pruningScale(1e-3f) + .pruningWeight(1.f); + config.combinedPruningThreshold(1.f); + return config.build(); +} + +} // namespace wavemap::examples::reflectivity_only_map + +#endif // WAVEMAP_EXAMPLES_DIGITAL_TWIN_OUSTER_REFLECTIVITY_ONLY_MAP_LAYERS_H_ diff --git a/examples/cpp/digital_twin/ouster_mapping/reflectivity_only_sensor_inputs.cc b/examples/cpp/digital_twin/ouster_mapping/reflectivity_only_sensor_inputs.cc new file mode 100644 index 000000000..20d739d37 --- /dev/null +++ b/examples/cpp/digital_twin/ouster_mapping/reflectivity_only_sensor_inputs.cc @@ -0,0 +1,45 @@ +#include "reflectivity_only_map_layers.h" + +#include + +#include +#include + +namespace wavemap::examples::reflectivity_only_map { + +using RosConverter = + convert::DescriptorContinuousRosConverter; +using ServerBuilder = LayeredRosServerBuilder; + +void registerSensorInputs(RosServer& server, + ServerBuilder::Extension& extension) { + server.bindNormalizedPointField( + extension, "reflectivity", 0.f, 255.f, + layered::EndpointRange{0.5f, 25.f}); +} + +} // namespace wavemap::examples::reflectivity_only_map + +int main(int argc, char** argv) { + ros::init(argc, argv, "wavemap_ros_server"); + google::InitGoogleLogging(argv[0]); + google::InstallFailureSignalHandler(); + FLAGS_alsologtostderr = true; + FLAGS_colorlogtostderr = true; + + ros::NodeHandle nh; + ros::NodeHandle nh_private("~"); + wavemap::RosServer server( + nh, nh_private, + [](wavemap::RosServer& server, ros::NodeHandle private_nh) { + namespace map = wavemap::examples::reflectivity_only_map; + map::ServerBuilder::install(server, private_nh, server.worldFrame(), + map::makeMapConfig(), + map::registerSensorInputs); + ROS_INFO( + "Installed occupancy+reflectivity benchmark application (C)."); + }); + + ros::spin(); + return 0; +} diff --git a/examples/cpp/digital_twin/ros/field_layer_update_ros_experiment.cc b/examples/cpp/digital_twin/ros/field_layer_update_ros_experiment.cc new file mode 100644 index 000000000..a6621c3ba --- /dev/null +++ b/examples/cpp/digital_twin/ros/field_layer_update_ros_experiment.cc @@ -0,0 +1,114 @@ +#include +#include +#include + +#include +#include +#include +#include + +#include "../common/field_layered_map_ros_config.h" + +namespace field_map = wavemap::examples::field_map; + +namespace { +bool approximatelyEqual(float lhs, float rhs, float tolerance = 1e-5f) { + return std::abs(lhs - rhs) <= tolerance; +} + +bool expect(const std::string& label, bool condition) { + std::cout << " " << label << ": " << (condition ? "pass" : "FAIL") + << "\n"; + return condition; +} + +geometry_msgs::Point32 point(float x, float y, float z) { + geometry_msgs::Point32 result; + result.x = x; + result.y = y; + result.z = z; + return result; +} +} // namespace + +int main() { + field_map::Map map(field_map::makeDefaultConfig()); + wavemap::layered::LayeredPipeline pipeline(map); + + wavemap_msgs::LayeredMapUpdate update; + update.header.frame_id = "map"; + update.positions = {point(0.1f, 0.1f, 0.1f), + point(-0.4f, 0.2f, 0.6f)}; + + wavemap_msgs::Layer reflectivity; + reflectivity.name = "reflectivity"; + reflectivity.type = "float32"; + reflectivity.float32_values = {0.25f, 0.8f}; + update.layers.emplace_back(reflectivity); + + wavemap_msgs::Layer class_layer; + class_layer.name = "class"; + class_layer.type = "int32"; + class_layer.int32_values = { + static_cast(field_map::ClassLabel::kGround), + static_cast(field_map::ClassLabel::kObstacle)}; + update.layers.emplace_back(class_layer); + + std::string error; + const bool converted = + wavemap::convert::DescriptorLayerObservationRosConverter< + field_map::Map>::integrate(update, pipeline, &error); + + const float cell_width_inv = + 1.f / map.continuousMap().getMinCellWidth(); + const auto first_index = wavemap::convert::pointToNearestIndex( + wavemap::Point3D(0.1f, 0.1f, 0.1f), cell_width_inv); + const auto second_index = wavemap::convert::pointToNearestIndex( + wavemap::Point3D(-0.4f, 0.2f, 0.6f), cell_width_inv); + + bool ok = true; + std::cout << "Field layered ROS update Phase 4 experiment\n"; + ok &= expect("valid subset converted", converted && error.empty()); + ok &= expect( + "continuous layer routed through pipeline", + approximatelyEqual( + map.continuousMap().getVoxelValue(first_index).data.get(), 0.25f) && + approximatelyEqual( + map.continuousMap().getVoxelValue(second_index).data.get(), + 0.8f)); + const auto first_class = + map.discreteLayers().get().getValue(first_index); + const auto second_class = + map.discreteLayers().get().getValue(second_index); + ok &= expect( + "discrete layer routed through pipeline", + first_class && second_class && + *first_class == + static_cast(field_map::ClassLabel::kGround) && + *second_class == + static_cast(field_map::ClassLabel::kObstacle)); + + wavemap_msgs::LayeredMapUpdate malformed = update; + malformed.layers.front().float32_values = {0.9f}; + error.clear(); + const bool malformed_accepted = + wavemap::convert::DescriptorLayerObservationRosConverter< + field_map::Map>::integrate(malformed, pipeline, &error); + ok &= expect( + "malformed batch rejected without partial update", + !malformed_accepted && !error.empty() && + approximatelyEqual( + map.continuousMap().getVoxelValue(first_index).data.get(), + 0.25f)); + + wavemap_msgs::LayeredMapUpdate unknown = update; + unknown.layers.front().name = "not_in_schema"; + error.clear(); + const bool unknown_accepted = + wavemap::convert::DescriptorLayerObservationRosConverter< + field_map::Map>::integrate(unknown, pipeline, &error); + ok &= expect("unknown layer rejected", + !unknown_accepted && !error.empty()); + + return ok ? 0 : 1; +} diff --git a/examples/cpp/digital_twin/ros/field_layered_map_ros_experiment.cc b/examples/cpp/digital_twin/ros/field_layered_map_ros_experiment.cc new file mode 100644 index 000000000..600a1009f --- /dev/null +++ b/examples/cpp/digital_twin/ros/field_layered_map_ros_experiment.cc @@ -0,0 +1,54 @@ +#include +#include + +#include +#include + +#include "../common/field_layered_map_ros_config.h" + +namespace field_map = wavemap::examples::field_map; +namespace field_map_ros = wavemap::examples::field_map::ros_config; + +namespace { +bool approximatelyEqual(float lhs, float rhs, float tolerance = 1e-5f) { + return std::abs(lhs - rhs) <= tolerance; +} +} // namespace + +int main() { + field_map::Map original(field_map::makeDefaultConfig()); + const wavemap::Index3D index(1, 2, 3); + original.continuousMap().setVoxelValue( + index, field_map::Voxel( + 0.8f, field_map::makeContinuousLayers(0.7f))); + original.discreteLayers().get().setValue( + index, static_cast(field_map::ClassLabel::kObstacle)); + + wavemap_msgs::LayeredMap message; + if (!wavemap::convert::layeredMapToRosMsg< + field_map::Map, field_map_ros::VoxelRosConverter>( + original, "map", ros::Time(10), message)) { + return 1; + } + + field_map::Map reconstructed(field_map::makeDefaultConfig()); + if (!wavemap::convert::rosMsgToLayeredMap< + field_map::Map, field_map_ros::VoxelRosConverter>( + message, reconstructed)) { + return 2; + } + + const auto voxel = reconstructed.continuousMap().getVoxelValue(index); + const auto class_value = + reconstructed.discreteLayers().get().getValue(index); + const bool ok = + message.header.frame_id == "map" && + message.continuous_map.layered_hashed_wavelet_octree.size() == 1u && + approximatelyEqual(voxel.data.get(), 0.7f) && + class_value && + *class_value == static_cast(field_map::ClassLabel::kObstacle); + + std::cout << "Field LayeredMap ROS round trip: " + << (ok ? "pass" : "FAIL") << "\n"; + return ok ? 0 : 3; +} diff --git a/examples/cpp/digital_twin/ros/native_sensor_layer_adapters_experiment.cc b/examples/cpp/digital_twin/ros/native_sensor_layer_adapters_experiment.cc new file mode 100644 index 000000000..af326959f --- /dev/null +++ b/examples/cpp/digital_twin/ros/native_sensor_layer_adapters_experiment.cc @@ -0,0 +1,156 @@ +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "../common/field_layer_policies.h" + +namespace { +bool approximatelyEqual(float lhs, float rhs, float tolerance = 1e-5f) { + return std::abs(lhs - rhs) <= tolerance; +} +bool expect(const std::string& label, bool condition) { + std::cout << " " << label << ": " << (condition ? "pass" : "FAIL") << "\n"; + return condition; +} +sensor_msgs::PointField field(const std::string& name, uint32_t offset, + uint8_t datatype) { + sensor_msgs::PointField result; + result.name = name; + result.offset = offset; + result.datatype = datatype; + result.count = 1u; + return result; +} +template +void write(std::vector& data, size_t offset, T value) { + std::memcpy(data.data() + offset, &value, sizeof(value)); +} +} // namespace + +int main() { + sensor_msgs::PointCloud2 cloud; + cloud.header.frame_id = "os_sensor"; + cloud.height = 1u; + cloud.width = 3u; + cloud.point_step = 16u; + cloud.row_step = cloud.width * cloud.point_step; + cloud.fields = { + field("x", 0u, sensor_msgs::PointField::FLOAT32), + field("y", 4u, sensor_msgs::PointField::FLOAT32), + field("z", 8u, sensor_msgs::PointField::FLOAT32), + field("reflectivity", 12u, sensor_msgs::PointField::UINT16)}; + cloud.data.resize(cloud.row_step); + write(cloud.data, 0u, 1.f); + write(cloud.data, 4u, 2.f); + write(cloud.data, 8u, 3.f); + write(cloud.data, 12u, 128u); + write(cloud.data, 16u, -1.f); + write(cloud.data, 20u, -2.f); + write(cloud.data, 24u, -3.f); + write(cloud.data, 28u, 255u); + write(cloud.data, 32u, std::numeric_limits::quiet_NaN()); + write(cloud.data, 36u, 0.f); + write(cloud.data, 40u, 0.f); + write(cloud.data, 44u, 64u); + + const auto reflectivity = + wavemap::convert::pointcloudToReflectivityObservations(cloud); + const auto endpoint_cloud = wavemap::convert:: + pointcloudToReflectivityEndpointPointcloud< + wavemap::examples::field_map::ReflectivityLayer>(cloud); + + sensor_msgs::Image color; + color.header.frame_id = "camera_color_optical_frame"; + color.width = 2u; + color.height = 1u; + color.encoding = sensor_msgs::image_encodings::RGB8; + color.step = 6u; + color.data = {255u, 0u, 0u, 0u, 128u, 255u}; + + sensor_msgs::Image depth; + depth.header.frame_id = color.header.frame_id; + depth.width = color.width; + depth.height = color.height; + depth.encoding = sensor_msgs::image_encodings::TYPE_16UC1; + depth.step = 4u; + depth.data.resize(4u); + write(depth.data, 0u, 1000u); + write(depth.data, 2u, 2000u); + + sensor_msgs::CameraInfo camera_info; + camera_info.header.frame_id = color.header.frame_id; + camera_info.width = color.width; + camera_info.height = color.height; + camera_info.K[0] = 1.0; + camera_info.K[4] = 1.0; + camera_info.K[8] = 1.0; + + const auto colors = wavemap::convert::rgbdToColorObservations( + color, depth, camera_info); + + wavemap::undistortion::StampedPointcloud timestamped_cloud(0u, "sensor", 2u); + timestamped_cloud.emplace(1.f, 0.f, 0.f, 20u, 0u); + timestamped_cloud.emplace(2.f, 0.f, 0.f, 10u, 1u); + timestamped_cloud.sort(); + + bool ok = true; + std::cout << "Native sensor layer adapters Phase 5 experiment\n"; + ok &= expect( + "Ouster reflectivity decoded and normalized", + reflectivity && reflectivity.header.frame_id == "os_sensor" && + reflectivity.observations.size() == 2u && + approximatelyEqual(reflectivity.observations[0].value, + 128.f / 255.f) && + approximatelyEqual(reflectivity.observations[1].value, 1.f)); + ok &= expect( + "single-pass endpoint attributes remain index-aligned", + endpoint_cloud && endpoint_cloud.rejected == 1u && + endpoint_cloud.pointcloud.size() == 2u && + endpoint_cloud.pointcloud.hasConsistentSizes() && + endpoint_cloud.pointcloud.point(0u) == + wavemap::Point3D(1.f, 2.f, 3.f) && + approximatelyEqual( + endpoint_cloud.pointcloud.attribute< + wavemap::examples::field_map::ReflectivityLayer>(0u), + 128.f / 255.f) && + endpoint_cloud.pointcloud.point(1u) == + wavemap::Point3D(-1.f, -2.f, -3.f) && + approximatelyEqual( + endpoint_cloud.pointcloud.attribute< + wavemap::examples::field_map::ReflectivityLayer>(1u), + 1.f)); + ok &= expect( + "motion-undistortion sorting preserves the attribute permutation", + timestamped_cloud[0u].source_index == 1u && + timestamped_cloud[1u].source_index == 0u); + ok &= expect( + "LiDAR positions remain in sensor frame", + reflectivity.observations[0].position == + wavemap::Point3D(1.f, 2.f, 3.f)); + ok &= expect( + "RGB-D color decoded and normalized", + colors && colors.observations.size() == 2u && + colors.observations[0].value == + wavemap::layered::Rgb{1.f, 0.f, 0.f} && + approximatelyEqual(colors.observations[1].value.g, 128.f / 255.f) && + approximatelyEqual(colors.observations[1].value.b, 1.f)); + ok &= expect( + "aligned depth back-projected with CameraInfo", + colors.observations[0].position == wavemap::Point3D(0.f, 0.f, 1.f) && + colors.observations[1].position == wavemap::Point3D(2.f, 0.f, 2.f)); + ok &= expect("RGB-D positions remain in camera frame", + colors.header.frame_id == "camera_color_optical_frame"); + return ok ? 0 : 1; +} diff --git a/interfaces/ros1/wavemap_msgs/CMakeLists.txt b/interfaces/ros1/wavemap_msgs/CMakeLists.txt index 37b852266..4a0505b05 100644 --- a/interfaces/ros1/wavemap_msgs/CMakeLists.txt +++ b/interfaces/ros1/wavemap_msgs/CMakeLists.txt @@ -2,20 +2,23 @@ cmake_minimum_required(VERSION 3.0.2) project(wavemap_msgs) # Load catkin and required dependencies to generate msgs -find_package(catkin REQUIRED COMPONENTS message_generation std_msgs) +find_package(catkin REQUIRED COMPONENTS geometry_msgs message_generation std_msgs) # Declare message files to be built add_message_files(FILES DiscreteLayer.msg + DiscreteLayerCategory.msg DiscreteLayerCell.msg HashedBlock.msg HashedBlocks.msg HashedWaveletOctree.msg HashedWaveletOctreeBlock.msg Index3D.msg + LayerVisualization.msg LayeredHashedWaveletOctree.msg LayeredHashedWaveletOctreeBlock.msg LayeredMap.msg + LayeredMapUpdate.msg LayeredWaveletOctreeNode.msg Layer.msg Map.msg @@ -28,7 +31,7 @@ add_service_files(FILES FilePath.srv) # Generate the language-specific message and service files -generate_messages(DEPENDENCIES std_msgs) +generate_messages(DEPENDENCIES geometry_msgs std_msgs) # Finish registering wavemap_msgs as a catkin pacakge -catkin_package(CATKIN_DEPENDS message_runtime std_msgs) +catkin_package(CATKIN_DEPENDS geometry_msgs message_runtime std_msgs) diff --git a/interfaces/ros1/wavemap_msgs/msg/DiscreteLayer.msg b/interfaces/ros1/wavemap_msgs/msg/DiscreteLayer.msg index 05cd17cc8..5a997be87 100644 --- a/interfaces/ros1/wavemap_msgs/msg/DiscreteLayer.msg +++ b/interfaces/ros1/wavemap_msgs/msg/DiscreteLayer.msg @@ -7,4 +7,12 @@ string value_type # 1 -> 2x2x2, 2 -> 4x4x4, etc. int32 block_height +# Optional labels for integer category values. Values without an entry remain +# valid and are displayed using their numeric value. +DiscreteLayerCategory[] categories + DiscreteLayerCell[] cells + +# Parent cells removed since the preceding update. Only used by incremental +# LayeredMap messages; full snapshots implicitly remove all omitted cells. +Index3D[] deleted_parent_indices diff --git a/interfaces/ros1/wavemap_msgs/msg/DiscreteLayerCategory.msg b/interfaces/ros1/wavemap_msgs/msg/DiscreteLayerCategory.msg new file mode 100644 index 000000000..722744c46 --- /dev/null +++ b/interfaces/ros1/wavemap_msgs/msg/DiscreteLayerCategory.msg @@ -0,0 +1,7 @@ +# Optional semantic label and schema-declared visualization color. +int32 value +string label +bool has_color +float32 r +float32 g +float32 b diff --git a/interfaces/ros1/wavemap_msgs/msg/LayerVisualization.msg b/interfaces/ros1/wavemap_msgs/msg/LayerVisualization.msg new file mode 100644 index 000000000..0f836bb3c --- /dev/null +++ b/interfaces/ros1/wavemap_msgs/msg/LayerVisualization.msg @@ -0,0 +1,10 @@ +# Optional visualization metadata declared by the layer schema. +string name +bool has_low_color +float32 low_r +float32 low_g +float32 low_b +bool has_high_color +float32 high_r +float32 high_g +float32 high_b diff --git a/interfaces/ros1/wavemap_msgs/msg/LayeredHashedWaveletOctree.msg b/interfaces/ros1/wavemap_msgs/msg/LayeredHashedWaveletOctree.msg index fc11851b1..70d154e06 100644 --- a/interfaces/ros1/wavemap_msgs/msg/LayeredHashedWaveletOctree.msg +++ b/interfaces/ros1/wavemap_msgs/msg/LayeredHashedWaveletOctree.msg @@ -6,6 +6,9 @@ int32 tree_height string[] layer_names string[] layer_types +Layer[] layer_min_values +Layer[] layer_max_values +LayerVisualization[] layer_visualizations Index3D[] allocated_block_indices diff --git a/interfaces/ros1/wavemap_msgs/msg/LayeredMap.msg b/interfaces/ros1/wavemap_msgs/msg/LayeredMap.msg index 65364ce77..01bd9de3d 100644 --- a/interfaces/ros1/wavemap_msgs/msg/LayeredMap.msg +++ b/interfaces/ros1/wavemap_msgs/msg/LayeredMap.msg @@ -1,5 +1,9 @@ Header header +# True for a complete snapshot. False for an incremental patch whose +# continuous blocks and discrete parent cells replace only the included data. +bool is_full_update + # Existing Wavemap structured message for the continuous wavelet map. Map continuous_map diff --git a/interfaces/ros1/wavemap_msgs/msg/LayeredMapUpdate.msg b/interfaces/ros1/wavemap_msgs/msg/LayeredMapUpdate.msg new file mode 100644 index 000000000..1a003d8e6 --- /dev/null +++ b/interfaces/ros1/wavemap_msgs/msg/LayeredMapUpdate.msg @@ -0,0 +1,6 @@ +# A sensor-independent batch of spatial layer observations. +# Every layer contains one value per position, in the same order. +# Producers may include any subset of the configured map layers. +std_msgs/Header header +geometry_msgs/Point32[] positions +Layer[] layers diff --git a/interfaces/ros1/wavemap_msgs/package.xml b/interfaces/ros1/wavemap_msgs/package.xml index 72be093e5..48db555cb 100644 --- a/interfaces/ros1/wavemap_msgs/package.xml +++ b/interfaces/ros1/wavemap_msgs/package.xml @@ -12,6 +12,7 @@ catkin + geometry_msgs message_generation message_runtime std_msgs diff --git a/interfaces/ros1/wavemap_ros/CMakeLists.txt b/interfaces/ros1/wavemap_ros/CMakeLists.txt index 59aa15a1b..9e6693e9c 100644 --- a/interfaces/ros1/wavemap_ros/CMakeLists.txt +++ b/interfaces/ros1/wavemap_ros/CMakeLists.txt @@ -7,7 +7,7 @@ project(wavemap_ros) find_package(OpenCV REQUIRED) find_package(catkin REQUIRED COMPONENTS wavemap wavemap_msgs wavemap_ros_conversions - roscpp rosbag cv_bridge image_transport tf2_ros + roscpp rosbag message_filters cv_bridge image_transport tf2_ros std_srvs sensor_msgs visualization_msgs) # Register catkin package @@ -16,7 +16,7 @@ catkin_package( LIBRARIES ${PROJECT_NAME} CATKIN_DEPENDS wavemap wavemap_msgs wavemap_ros_conversions - roscpp rosbag cv_bridge image_transport tf2_ros + roscpp rosbag message_filters cv_bridge image_transport tf2_ros std_srvs sensor_msgs visualization_msgs) @@ -45,7 +45,9 @@ add_library(${PROJECT_NAME} set_wavemap_target_properties(${PROJECT_NAME}) target_include_directories(${PROJECT_NAME} PUBLIC include ${catkin_INCLUDE_DIRS} - PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/../../../examples/cpp/digital_twin/common) + PRIVATE + ${CMAKE_CURRENT_SOURCE_DIR}/../../../examples/cpp/digital_twin/common + ${CMAKE_CURRENT_SOURCE_DIR}/../../../examples/cpp/digital_twin/ouster_mapping) target_link_libraries(${PROJECT_NAME} PUBLIC ${catkin_LIBRARIES} ${OpenCV_LIBRARIES}) @@ -54,10 +56,56 @@ add_executable(ros_server app/ros_server.cc) set_wavemap_target_properties(ros_server) target_link_libraries(ros_server ${PROJECT_NAME}) +add_executable(occupancy_layered_server + ../../../examples/cpp/digital_twin/ouster_mapping/occupancy_sensor_inputs.cc) +set_wavemap_target_properties(occupancy_layered_server) +target_include_directories(occupancy_layered_server PRIVATE + ${CMAKE_CURRENT_SOURCE_DIR}/../../../examples/cpp/digital_twin/common + ${CMAKE_CURRENT_SOURCE_DIR}/../../../examples/cpp/digital_twin/ouster_mapping) +target_link_libraries(occupancy_layered_server ${PROJECT_NAME}) + +add_executable(reflectivity_layered_server + ../../../examples/cpp/digital_twin/ouster_mapping/reflectivity_only_sensor_inputs.cc) +set_wavemap_target_properties(reflectivity_layered_server) +target_include_directories(reflectivity_layered_server PRIVATE + ${CMAKE_CURRENT_SOURCE_DIR}/../../../examples/cpp/digital_twin/common + ${CMAKE_CURRENT_SOURCE_DIR}/../../../examples/cpp/digital_twin/ouster_mapping) +target_link_libraries(reflectivity_layered_server ${PROJECT_NAME}) + +add_executable(reflectivity_class_layered_server + ../../../examples/cpp/digital_twin/ouster_mapping/ouster_sensor_inputs.cc) +set_wavemap_target_properties(reflectivity_class_layered_server) +target_include_directories(reflectivity_class_layered_server PRIVATE + ${CMAKE_CURRENT_SOURCE_DIR}/../../../examples/cpp/digital_twin/common + ${CMAKE_CURRENT_SOURCE_DIR}/../../../examples/cpp/digital_twin/ouster_mapping) +target_link_libraries(reflectivity_class_layered_server ${PROJECT_NAME}) + add_executable(rosbag_processor app/rosbag_processor.cc) set_wavemap_target_properties(rosbag_processor) target_link_libraries(rosbag_processor ${PROJECT_NAME}) +add_executable(publish_field_layer_updates_experiment + app/publish_field_layer_updates_experiment.cc) +set_wavemap_target_properties(publish_field_layer_updates_experiment) +target_link_libraries(publish_field_layer_updates_experiment ${PROJECT_NAME}) + +add_executable(field_layer_sensor_bridge + app/field_layer_sensor_bridge.cc) +set_wavemap_target_properties(field_layer_sensor_bridge) +target_link_libraries(field_layer_sensor_bridge ${PROJECT_NAME}) + +add_executable(layered_incremental_publish_experiment + app/layered_incremental_publish_experiment.cc) +set_wavemap_target_properties(layered_incremental_publish_experiment) +target_include_directories(layered_incremental_publish_experiment PRIVATE + ${CMAKE_CURRENT_SOURCE_DIR}/../../../examples/cpp/digital_twin/common) +target_link_libraries(layered_incremental_publish_experiment ${PROJECT_NAME}) + +add_executable(computed_point_field_binding_experiment + app/computed_point_field_binding_experiment.cc) +set_wavemap_target_properties(computed_point_field_binding_experiment) +target_link_libraries(computed_point_field_binding_experiment ${PROJECT_NAME}) + # Support installs # Mark targets for installation install(TARGETS ${PROJECT_NAME} diff --git a/interfaces/ros1/wavemap_ros/app/computed_point_field_binding_experiment.cc b/interfaces/ros1/wavemap_ros/app/computed_point_field_binding_experiment.cc new file mode 100644 index 000000000..27bc01a71 --- /dev/null +++ b/interfaces/ros1/wavemap_ros/app/computed_point_field_binding_experiment.cc @@ -0,0 +1,204 @@ +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include "wavemap_ros/inputs/pointcloud_endpoint_adapter.h" +#include "wavemap_ros/layered_ros_server_extension.h" + +namespace { +struct AveragedLayer + : wavemap::layered::schema::StatefulContinuousLayer< + float, wavemap::layered::WeightedMeanState, + wavemap::layered::WeightedMeanLayerUpdatePolicy> { + static constexpr std::string_view name = "averaged"; +}; + +struct ClassLayer + : wavemap::layered::schema::DiscreteLayer< + int, wavemap::layered::ReplaceLayerUpdatePolicy> { + static constexpr std::string_view name = "class"; +}; + +using AveragedDefinition = wavemap::layered::LayeredMapDefinition< + wavemap::layered::schema::LayerSchema>; +using AveragedRosConverter = wavemap::convert::DescriptorContinuousRosConverter< + AveragedDefinition::Voxel>; +using TestExtension = wavemap::LayeredRosServerExtension< + AveragedDefinition::Map, AveragedDefinition::MapIo, + AveragedRosConverter>; + +[[maybe_unused]] bool instantiateDiscreteEndpointIntegration( + TestExtension& extension, + const wavemap::PosedPointcloud<>& pointcloud, + const std::vector& classes) { + return extension.integrateEndpointLayer(pointcloud, classes); +} + +sensor_msgs::PointField makeField(const std::string& name, uint32_t offset, + uint8_t datatype) { + sensor_msgs::PointField field; + field.name = name; + field.offset = offset; + field.datatype = datatype; + field.count = 1u; + return field; +} + +template +void write(std::vector& data, size_t offset, ValueT value) { + std::memcpy(data.data() + offset, &value, sizeof(value)); +} + +bool approximatelyEqual(float lhs, float rhs, float tolerance = 1e-5f) { + return std::abs(lhs - rhs) <= tolerance; +} +} // namespace + +int main() { + sensor_msgs::PointCloud2 cloud; + cloud.height = 1u; + cloud.width = 3u; + cloud.point_step = 12u; + cloud.row_step = cloud.width * cloud.point_step; + cloud.fields = { + makeField("raw", 0u, sensor_msgs::PointField::UINT16), + makeField("gain", 4u, sensor_msgs::PointField::FLOAT32), + makeField("class", 8u, sensor_msgs::PointField::INT32)}; + cloud.data.resize(cloud.row_step); + + write(cloud.data, 0u, 100u); + write(cloud.data, 4u, 0.5f); + write(cloud.data, 8u, 4); + write(cloud.data, 12u, 200u); + write(cloud.data, 16u, + std::numeric_limits::quiet_NaN()); + write(cloud.data, 20u, 2); + write(cloud.data, 24u, 50u); + write(cloud.data, 28u, 2.f); + write(cloud.data, 32u, 7); + + std::vector> integrated_values; + wavemap::ComputedNumericPointcloudEndpointAdapter adapter( + {"raw", "gain"}, + [](const wavemap::NumericPointFieldValues& fields) + -> std::optional { + const float result = fields[0u] * 0.01f * fields[1u]; + return result <= 1.f ? std::optional(result) : std::nullopt; + }, + [&integrated_values]( + const wavemap::PosedPointcloud<>&, + const std::vector>& values) { + integrated_values = values; + }); + std::vector integrated_classes; + wavemap::NumericPointcloudEndpointAdapter class_adapter( + "class", 1.f, 0.f, 0, 255, + [&integrated_classes](const wavemap::PosedPointcloud<>&, + const std::vector& values) { + integrated_classes = values; + }); + + auto decoder = adapter.prepare(cloud, cloud.width); + if (!decoder) { + std::cerr << "Could not prepare computed point-field decoder.\n"; + return 1; + } + for (size_t index = 0u; index < cloud.width; ++index) { + if (!decoder->decodePoint(cloud, index * cloud.point_step)) { + return 1; + } + } + + auto channel = decoder->finish(); + auto class_decoder = class_adapter.prepare(cloud, cloud.width); + if (!class_decoder) { + std::cerr << "Could not prepare discrete point-field decoder.\n"; + return 1; + } + for (size_t index = 0u; index < cloud.width; ++index) { + if (!class_decoder->decodePoint(cloud, index * cloud.point_step)) { + return 1; + } + } + auto class_channel = class_decoder->finish(); + wavemap::PosedPointcloud<> posed_cloud; + posed_cloud.resize(3u); + std::vector sorted_points; + sorted_points.emplace_back(0.f, 0.f, 0.f, 0u, 2u); + sorted_points.emplace_back(0.f, 0.f, 0.f, 1u, 0u); + sorted_points.emplace_back(0.f, 0.f, 0.f, 2u, 1u); + + const bool integrated = channel->integrate(posed_cloud, sorted_points); + const bool class_integrated = + class_channel->integrate(posed_cloud, sorted_points); + AveragedDefinition::Voxel averaged_voxel; + averaged_voxel.data.get() = {2.6f, 4.f}; + auto averaged_layers = AveragedRosConverter::makeLayers(); + AveragedRosConverter::appendLayerValues(averaged_voxel, averaged_layers); + const auto restored_voxel = + AveragedRosConverter::readCellData(0.f, averaged_layers, 0u); + const auto restored_state = restored_voxel.data.get(); + std::vector endpoint_indices; + wavemap::layered::forEachPointcloudIntegrationIndex< + wavemap::layered::PointcloudLayerIntegrationMode::kEndpointOnly>( + wavemap::Point3D::Zero(), wavemap::Point3D(2.f, 0.f, 0.f), 1.f, + [&endpoint_indices](const wavemap::Index3D& index) { + endpoint_indices.emplace_back(index); + }); + std::vector ray_indices; + wavemap::layered::forEachPointcloudIntegrationIndex< + wavemap::layered::PointcloudLayerIntegrationMode::kAlongRay>( + wavemap::Point3D::Zero(), wavemap::Point3D(2.f, 0.f, 0.f), 1.f, + [&ray_indices](const wavemap::Index3D& index) { + ray_indices.emplace_back(index); + }); + const wavemap::Point3D origin = wavemap::Point3D::Zero(); + const wavemap::Index3D expected_origin = + wavemap::convert::pointToNearestIndex(origin, 1.f); + const wavemap::Index3D expected_endpoint = wavemap::convert::pointToNearestIndex( + wavemap::Point3D(2.f, 0.f, 0.f), 1.f); + + const bool passed = + integrated && class_integrated && integrated_values.size() == 3u && + integrated_values[0u] && + approximatelyEqual(*integrated_values[0u], 1.f) && + integrated_values[1u] && + approximatelyEqual(*integrated_values[1u], 0.5f) && + !integrated_values[2u] && integrated_classes == std::vector{7, 4, 2} && + endpoint_indices.size() == 1u && + endpoint_indices.front() == expected_endpoint && + ray_indices.size() > endpoint_indices.size() && + ray_indices.front() == expected_origin && + ray_indices.back() == expected_endpoint && averaged_layers.size() == 1u && + averaged_layers[0].type == "float32" && + averaged_layers[0].float32_values.size() == 1u && + approximatelyEqual(averaged_layers[0].float32_values[0], 0.65f) && + approximatelyEqual(restored_state.weighted_sum, 0.65f) && + approximatelyEqual(restored_state.total_weight, 1.f); + + std::cout << "Computed PointCloud2 field binding experiment: " + << (passed ? "pass" : "FAIL") << "\n"; + if (!passed) { + std::cout << "endpoint indices: " << endpoint_indices.size(); + for (const auto& index : endpoint_indices) { + std::cout << " [" << index.transpose() << "]"; + } + std::cout << "\nray indices: " << ray_indices.size(); + for (const auto& index : ray_indices) { + std::cout << " [" << index.transpose() << "]"; + } + std::cout << "\n"; + } + return passed ? 0 : 1; +} diff --git a/interfaces/ros1/wavemap_ros/app/field_layer_sensor_bridge.cc b/interfaces/ros1/wavemap_ros/app/field_layer_sensor_bridge.cc new file mode 100644 index 000000000..7814d5fc9 --- /dev/null +++ b/interfaces/ros1/wavemap_ros/app/field_layer_sensor_bridge.cc @@ -0,0 +1,198 @@ +#include +#include +#include +#include +#include + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace wavemap { +class FieldLayerSensorBridge { + public: + FieldLayerSensorBridge() + : nh_private_("~"), + transformer_(std::make_shared()) { + nh_private_.param("world_frame", world_frame_, std::string("odom")); + nh_private_.param("reflectivity_topic", reflectivity_topic_, + std::string("/ouster/points")); + nh_private_.param("color_topic", color_topic_, + std::string("/camera/color/image_raw")); + nh_private_.param("aligned_depth_topic", aligned_depth_topic_, + std::string( + "/camera/aligned_depth_to_color/image_raw")); + nh_private_.param("camera_info_topic", camera_info_topic_, + std::string("/camera/color/camera_info")); + nh_private_.param("reflectivity_enabled", reflectivity_enabled_, true); + nh_private_.param("color_enabled", color_enabled_, false); + + int point_stride = 1; + int pixel_stride = 4; + int queue_size = 10; + double sync_tolerance = 0.05; + nh_private_.param("reflectivity_point_stride", point_stride, point_stride); + nh_private_.param("color_pixel_stride", pixel_stride, pixel_stride); + nh_private_.param("sync_queue_size", queue_size, queue_size); + nh_private_.param("sync_tolerance", sync_tolerance, sync_tolerance); + reflectivity_config_.point_stride = + static_cast(std::max(1, point_stride)); + color_config_.pixel_stride = + static_cast(std::max(1, pixel_stride)); + + update_pub_ = nh_.advertise( + "/wavemap/layered_map_updates", 1); + + if (reflectivity_enabled_) { + reflectivity_sub_ = nh_.subscribe( + reflectivity_topic_, queue_size, + &FieldLayerSensorBridge::reflectivityCallback, this); + } + if (color_enabled_) { + color_sub_.subscribe(nh_, color_topic_, queue_size); + depth_sub_.subscribe(nh_, aligned_depth_topic_, queue_size); + camera_info_sub_.subscribe(nh_, camera_info_topic_, queue_size); + SyncPolicy policy(queue_size); + policy.setMaxIntervalDuration(ros::Duration(sync_tolerance)); + rgbd_sync_ = std::make_unique( + static_cast(policy), color_sub_, depth_sub_, + camera_info_sub_); + rgbd_sync_->registerCallback(boost::bind( + &FieldLayerSensorBridge::rgbdCallback, this, + boost::placeholders::_1, boost::placeholders::_2, + boost::placeholders::_3)); + } + } + + private: + using SyncPolicy = message_filters::sync_policies::ApproximateTime< + sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo>; + using Synchronizer = message_filters::Synchronizer; + + template + bool transformObservations( + std::vector>& observations, + const std::string& sensor_frame, const ros::Time& stamp) { + const auto T_W_S = + transformer_->lookupTransform(world_frame_, sensor_frame, stamp); + if (!T_W_S) { + ROS_WARN_STREAM_THROTTLE( + 2.0, "No transform from '" << sensor_frame << "' to '" + << world_frame_ + << "'; skipping layer observations."); + return false; + } + for (auto& observation : observations) { + observation.position = *T_W_S * observation.position; + } + return true; + } + + void reflectivityCallback(const sensor_msgs::PointCloud2& cloud) { + auto batch = convert::pointcloudToReflectivityObservations( + cloud, reflectivity_config_); + if (!batch) { + ROS_WARN_STREAM_THROTTLE( + 2.0, "Reflectivity conversion failed: " << batch.error); + return; + } + if (!transformObservations( + batch.observations, batch.header.frame_id, batch.header.stamp)) { + return; + } + + wavemap_msgs::LayeredMapUpdate update; + update.header = batch.header; + update.header.frame_id = world_frame_; + update.positions.reserve(batch.observations.size()); + wavemap_msgs::Layer layer; + layer.name = "reflectivity"; + layer.type = "float32"; + layer.float32_values.reserve(batch.observations.size()); + for (const auto& observation : batch.observations) { + geometry_msgs::Point32 position; + position.x = observation.position.x(); + position.y = observation.position.y(); + position.z = observation.position.z(); + update.positions.emplace_back(position); + layer.float32_values.emplace_back(observation.value); + } + update.layers.emplace_back(std::move(layer)); + update_pub_.publish(update); + } + + void rgbdCallback( + const sensor_msgs::ImageConstPtr& color, + const sensor_msgs::ImageConstPtr& depth, + const sensor_msgs::CameraInfoConstPtr& camera_info) { + auto batch = convert::rgbdToColorObservations( + *color, *depth, *camera_info, color_config_); + if (!batch) { + ROS_WARN_STREAM_THROTTLE(2.0, "RGB-D conversion failed: " << batch.error); + return; + } + if (!transformObservations( + batch.observations, batch.header.frame_id, batch.header.stamp)) { + return; + } + + wavemap_msgs::LayeredMapUpdate update; + update.header = batch.header; + update.header.frame_id = world_frame_; + update.positions.reserve(batch.observations.size()); + wavemap_msgs::Layer layer; + layer.name = "color"; + layer.type = "float32_rgb"; + layer.float32_values.reserve(3u * batch.observations.size()); + for (const auto& observation : batch.observations) { + geometry_msgs::Point32 position; + position.x = observation.position.x(); + position.y = observation.position.y(); + position.z = observation.position.z(); + update.positions.emplace_back(position); + layer.float32_values.emplace_back(observation.value.r); + layer.float32_values.emplace_back(observation.value.g); + layer.float32_values.emplace_back(observation.value.b); + } + update.layers.emplace_back(std::move(layer)); + update_pub_.publish(update); + } + + ros::NodeHandle nh_; + ros::NodeHandle nh_private_; + std::shared_ptr transformer_; + std::string world_frame_; + std::string reflectivity_topic_; + std::string color_topic_; + std::string aligned_depth_topic_; + std::string camera_info_topic_; + bool reflectivity_enabled_ = true; + bool color_enabled_ = false; + convert::ReflectivityObservationConfig reflectivity_config_; + convert::RgbdColorObservationConfig color_config_; + + ros::Publisher update_pub_; + ros::Subscriber reflectivity_sub_; + message_filters::Subscriber color_sub_; + message_filters::Subscriber depth_sub_; + message_filters::Subscriber camera_info_sub_; + std::unique_ptr rgbd_sync_; +}; +} // namespace wavemap + +int main(int argc, char** argv) { + ros::init(argc, argv, "field_layer_sensor_bridge"); + wavemap::FieldLayerSensorBridge bridge; + ros::spin(); + return 0; +} diff --git a/interfaces/ros1/wavemap_ros/app/layered_incremental_publish_experiment.cc b/interfaces/ros1/wavemap_ros/app/layered_incremental_publish_experiment.cc new file mode 100644 index 000000000..33ff92c1c --- /dev/null +++ b/interfaces/ros1/wavemap_ros/app/layered_incremental_publish_experiment.cc @@ -0,0 +1,145 @@ +#include +#include +#include +#include +#include +#include + +#include +#include + +#include "field_layered_map_ros_config.h" +#include +#include "wavemap_ros/map_operations/layered_publish_map_operation.h" + +namespace field_map = wavemap::examples::field_map; + +int main(int argc, char** argv) { + ros::init(argc, argv, "layered_incremental_publish_experiment"); + ros::NodeHandle nh; + + // Exercise the same type-erased threshold operation used by the original + // Pipeline. Virtual dispatch must retain the typed layer configuration. + bool threshold_valid = false; + { + field_map::Map threshold_map(field_map::makeDefaultConfig()); + const wavemap::Index3D threshold_index(2, 3, 4); + threshold_map.continuousMap().setVoxelValue( + threshold_index, + field_map::Voxel( + 8.f, field_map::makeContinuousLayers(0.65f))); + wavemap::ThresholdMapOperationConfig threshold_config; + wavemap::ThresholdMapOperation threshold_operation( + threshold_config, threshold_map.continuousMapPtr()); + threshold_operation.run(true); + const auto thresholded = + threshold_map.continuousMap().getVoxelValue(threshold_index); + threshold_valid = + std::abs(thresholded.occupancy - 4.f) < 1e-5f && + std::abs(thresholded.data.get() - + 0.4f) < 1e-5f; + } + + std::mutex mutex; + std::vector messages; + auto subscriber = nh.subscribe( + "/layered_incremental_test", 10, + [&mutex, &messages](const wavemap_msgs::LayeredMap::ConstPtr& msg) { + std::scoped_lock lock(mutex); + messages.emplace_back(*msg); + }); + + const auto map_config = field_map::makeDefaultConfig(); + field_map::Map map(map_config); + wavemap::PublishMapOperationConfig config; + config.once_every = 0.01f; + config.max_num_blocks_per_msg = 1; + config.topic = "/layered_incremental_test"; + using Operation = wavemap::UnifiedLayeredPublishMapOperation< + field_map::Map, field_map::ros_config::VoxelRosConverter>; + Operation operation(config, &map, std::make_shared(1), + "map", nh); + + ros::AsyncSpinner spinner(1); + spinner.start(); + ros::WallDuration(0.1).sleep(); + + map.continuousMap().setVoxelValue( + wavemap::Index3D(0, 0, 0), + field_map::Voxel(0.5f, field_map::makeContinuousLayers(0.25f))); + auto& class_layer = + map.discreteLayers().get(); + class_layer.setValue(wavemap::Index3D(0, 0, 0), 1); + class_layer.setValue(wavemap::Index3D(1, 0, 0), 1); + class_layer.setValue(wavemap::Index3D(0, 1, 0), 2); + // Rewriting the same compressed parent must still produce one patch cell. + class_layer.setValue(wavemap::Index3D(0, 1, 0), 3); + operation.run(true); + ros::WallDuration(0.1).sleep(); + + // Tree height 6 means an offset of 64 lies in a different hashed block. + map.continuousMap().setVoxelValue( + wavemap::Index3D(64, 0, 0), + field_map::Voxel(0.7f, field_map::makeContinuousLayers(0.75f))); + class_layer.eraseValue(wavemap::Index3D(0, 0, 0)); + class_layer.eraseValue(wavemap::Index3D(1, 0, 0)); + class_layer.eraseValue(wavemap::Index3D(0, 1, 0)); + operation.run(true); + ros::WallDuration(0.1).sleep(); + + // A discrete-only edit must publish without touching a continuous block. + class_layer.setValue(wavemap::Index3D(4, 0, 0), 2); + operation.run(true); + ros::WallDuration(0.1).sleep(); + spinner.stop(); + + std::scoped_lock lock(mutex); + bool valid = threshold_valid && messages.size() == 3u; + if (valid) { + const auto& first = + messages[0].continuous_map.layered_hashed_wavelet_octree; + const auto& second = + messages[1].continuous_map.layered_hashed_wavelet_octree; + const auto& third = + messages[2].continuous_map.layered_hashed_wavelet_octree; + valid = first.size() == 1u && second.size() == 1u && third.size() == 1u && + !messages[0].is_full_update && !messages[1].is_full_update && + !messages[2].is_full_update && + first[0].blocks.size() == 1u && + first[0].allocated_block_indices.size() == 1u && + second[0].blocks.size() == 1u && + second[0].allocated_block_indices.size() == 2u && + second[0].layer_names == std::vector{"reflectivity"} && + second[0].layer_types == std::vector{"float32"} && + second[0].layer_min_values.size() == 1u && + second[0].layer_min_values[0].float32_values == + std::vector{0.f} && + second[0].layer_max_values.size() == 1u && + second[0].layer_max_values[0].float32_values == + std::vector{0.4f} && + messages[0].discrete_layers.size() == 1u && + messages[0].discrete_layers[0].name == "class" && + messages[0].discrete_layers[0].cells.size() == 1u && + messages[0].discrete_layers[0].cells[0].dominant_int32 == 1 && + messages[0].discrete_layers[0].cells[0].exception_offsets.size() == + 1u && + messages[0].discrete_layers[0] + .cells[0] + .exception_int32_values == std::vector{3} && + messages[1].discrete_layers.size() == 1u && + messages[1].discrete_layers[0].cells.empty() && + messages[1].discrete_layers[0].deleted_parent_indices.size() == 1u && + third[0].blocks.empty() && + third[0].allocated_block_indices.size() == 2u && + messages[2].discrete_layers.size() == 1u && + messages[2].discrete_layers[0].cells.size() == 1u; + } + + std::cout << "Layered incremental publish experiment\n" + << " messages received: " << messages.size() << "\n" + << " bounded continuous blocks and discrete patches: " + << (valid ? "pass" : "FAIL") << "\n" + << " typed occupancy/reflectivity thresholding: " + << (threshold_valid ? "pass" : "FAIL") << "\n"; + return valid ? 0 : 1; +} diff --git a/interfaces/ros1/wavemap_ros/app/publish_field_layer_updates_experiment.cc b/interfaces/ros1/wavemap_ros/app/publish_field_layer_updates_experiment.cc new file mode 100644 index 000000000..0461e7838 --- /dev/null +++ b/interfaces/ros1/wavemap_ros/app/publish_field_layer_updates_experiment.cc @@ -0,0 +1,47 @@ +#include +#include + +namespace { +geometry_msgs::Point32 point(float x, float y, float z) { + geometry_msgs::Point32 result; + result.x = x; + result.y = y; + result.z = z; + return result; +} +} // namespace + +int main(int argc, char** argv) { + ros::init(argc, argv, "publish_field_layer_updates_experiment"); + ros::NodeHandle nh; + ros::NodeHandle nh_private("~"); + + std::string world_frame = "map"; + nh_private.param("world_frame", world_frame, world_frame); + + ros::Publisher publisher = + nh.advertise( + "/wavemap/layered_map_updates", 1, true); + + wavemap_msgs::LayeredMapUpdate update; + update.header.frame_id = world_frame; + update.header.stamp = ros::Time::now(); + update.positions = { + point(0.05f, 0.05f, 0.05f), + point(0.30f, 0.05f, 0.05f), + point(0.05f, 0.30f, 0.05f), + point(1.05f, 0.05f, 0.05f)}; + + wavemap_msgs::Layer class_layer; + class_layer.name = "class"; + class_layer.type = "int32"; + class_layer.int32_values = {1, 1, 2, 3}; + update.layers.emplace_back(class_layer); + + publisher.publish(update); + ROS_INFO_STREAM("Published " << update.positions.size() + << " class-only observations on " + << publisher.getTopic() << "."); + ros::spin(); + return 0; +} diff --git a/interfaces/ros1/wavemap_ros/config/rviz/layered_map.rviz b/interfaces/ros1/wavemap_ros/config/rviz/layered_map.rviz index fd8d904cb..1b46ef6a5 100644 --- a/interfaces/ros1/wavemap_ros/config/rviz/layered_map.rviz +++ b/interfaces/ros1/wavemap_ros/config/rviz/layered_map.rviz @@ -73,7 +73,7 @@ Visualization Manager: Bool true color: 255; 12; 12 Scalar high color: 255; 255; 0 Scalar low color: 0; 0; 255 - Scalar max: 1 + Scalar max: 0.2 Scalar min: 0 Show bool false: false Log odds threshold: 0.0010000000474974513 diff --git a/interfaces/ros1/wavemap_ros/config/wavemap_ouster_occupancy_only.yaml b/interfaces/ros1/wavemap_ros/config/wavemap_ouster_occupancy_only.yaml new file mode 100644 index 000000000..6e9791cfb --- /dev/null +++ b/interfaces/ros1/wavemap_ros/config/wavemap_ouster_occupancy_only.yaml @@ -0,0 +1,57 @@ +general: + world_frame: "map" + logging_level: debug + allow_reset_map_service: true + +# Original occupancy-only Wavemap data structure. No layered map is enabled. +map: + type: hashed_wavelet_octree + min_cell_width: { meters: 0.25 } + min_log_odds: -2.0 + max_log_odds: 4.0 + tree_height: 7 + only_prune_blocks_if_unused_for: { seconds: 5.0 } + +map_operations: + - type: threshold_map + once_every: { seconds: 2.0 } + - type: prune_map + once_every: { seconds: 10.0 } + - type: publish_map + once_every: { seconds: 0.5 } + max_num_blocks_per_msg: 50 + topic: "map" + +measurement_integrators: + ouster_os1: + projection_model: + type: ouster_projector + lidar_origin_to_beam_origin: { millimeters: 16.721 } + lidar_origin_to_sensor_origin_z_offset: { millimeters: 38.195 } + elevation: + num_cells: 128 + min_angle: { degrees: -21.39 } + max_angle: { degrees: 21.25 } + azimuth: + num_cells: 1024 + min_angle: { degrees: -180.0 } + max_angle: { degrees: 180.0 } + measurement_model: + type: continuous_beam + angle_sigma: { degrees: 0.1 } + range_sigma: { meters: 0.05 } + scaling_free: 0.2 + scaling_occupied: 0.4 + integration_method: + type: hashed_wavelet_integrator + min_range: { meters: 0.5 } + max_range: { meters: 25.0 } + +inputs: + - type: pointcloud_topic + topic_name: "/ouster/points" + topic_type: ouster + measurement_integrator_names: ouster_os1 + undistort_motion: false + topic_queue_length: 10 + max_wait_for_pose: { seconds: 1.0 } diff --git a/interfaces/ros1/wavemap_ros/config/wavemap_ouster_reflectivity_layered.yaml b/interfaces/ros1/wavemap_ros/config/wavemap_ouster_reflectivity_layered.yaml new file mode 100644 index 000000000..cdbd78015 --- /dev/null +++ b/interfaces/ros1/wavemap_ros/config/wavemap_ouster_reflectivity_layered.yaml @@ -0,0 +1,51 @@ +general: + world_frame: "map" + logging_level: debug + allow_reset_map_service: true + +map_operations: + - type: threshold_map + once_every: { seconds: 2.0 } + - type: prune_map + once_every: { seconds: 10.0 } + - type: publish_map + once_every: { seconds: 0.5 } + max_num_blocks_per_msg: 50 + topic: "layered_map" + +measurement_integrators: + ouster_os1: + projection_model: + type: ouster_projector + lidar_origin_to_beam_origin: { millimeters: 16.721 } + lidar_origin_to_sensor_origin_z_offset: { millimeters: 38.195 } + elevation: + num_cells: 128 + min_angle: { degrees: -21.39 } + max_angle: { degrees: 21.25 } + azimuth: + num_cells: 1024 + min_angle: { degrees: -180.0 } + max_angle: { degrees: 180.0 } + measurement_model: + type: continuous_beam + angle_sigma: { degrees: 0.1 } + range_sigma: { meters: 0.05 } + scaling_free: 0.2 + scaling_occupied: 0.4 + integration_method: + type: hashed_wavelet_integrator + min_range: { meters: 0.5 } + max_range: { meters: 25.0 } + +inputs: + - type: pointcloud_topic + topic_name: "/ouster/points" + topic_type: ouster + measurement_integrator_names: ouster_os1 + # IG-LIO publishes one map-to-body pose per processed scan. This bag's + # Ouster t field is not usable as Wavemap-relative per-point time, so asking + # Wavemap for intermediate poses drops the cloud. + undistort_motion: false + topic_queue_length: 10 + max_wait_for_pose: { seconds: 1.0 } diff --git a/interfaces/ros1/wavemap_ros/include/wavemap_ros/inputs/pointcloud_endpoint_adapter.h b/interfaces/ros1/wavemap_ros/include/wavemap_ros/inputs/pointcloud_endpoint_adapter.h new file mode 100644 index 000000000..f4a1aa556 --- /dev/null +++ b/interfaces/ros1/wavemap_ros/include/wavemap_ros/inputs/pointcloud_endpoint_adapter.h @@ -0,0 +1,347 @@ +#ifndef WAVEMAP_ROS_INPUTS_POINTCLOUD_ENDPOINT_ADAPTER_H_ +#define WAVEMAP_ROS_INPUTS_POINTCLOUD_ENDPOINT_ADAPTER_H_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +namespace wavemap { + +template +struct PointFieldBinding { + std::string field_name; + FloatingPoint scale = 1.f; + FloatingPoint offset = 0.f; + ValueT min_value = std::numeric_limits::lowest(); + ValueT max_value = std::numeric_limits::max(); + + explicit PointFieldBinding( + std::string field_name = {}, FloatingPoint scale = 1.f, + FloatingPoint offset = 0.f, + ValueT min_value = std::numeric_limits::lowest(), + ValueT max_value = std::numeric_limits::max()) + : field_name(std::move(field_name)), + scale(scale), + offset(offset), + min_value(min_value), + max_value(max_value) {} + + static PointFieldBinding normalized(std::string field_name, + FloatingPoint raw_min, + FloatingPoint raw_max) { + CHECK_LT(raw_min, raw_max); + const FloatingPoint scale = 1.f / (raw_max - raw_min); + return PointFieldBinding(std::move(field_name), scale, + -raw_min * scale, ValueT{0}, ValueT{1}); + } +}; + +class DecodedPointcloudEndpointChannel { + public: + virtual ~DecodedPointcloudEndpointChannel() = default; + virtual bool integrate( + const PosedPointcloud<>& pointcloud, + const std::vector& sorted_points) const = 0; +}; + +// A message-local decoder. PointcloudTopicInput owns the outer point loop and +// calls every prepared decoder once per point, so adding endpoint layers does +// not add another traversal of the PointCloud2 message. +class PointcloudEndpointDecoder { + public: + virtual ~PointcloudEndpointDecoder() = default; + virtual bool decodePoint(const sensor_msgs::PointCloud2& msg, + size_t point_offset) = 0; + virtual std::unique_ptr finish() = 0; +}; + +class PointcloudEndpointAdapter { + public: + virtual ~PointcloudEndpointAdapter() = default; + virtual std::unique_ptr prepare( + const sensor_msgs::PointCloud2& msg, size_t num_points) const = 0; +}; + +template +class DecodedScalarEndpointChannel final + : public DecodedPointcloudEndpointChannel { + public: + using Callback = + std::function&, const std::vector&)>; + + DecodedScalarEndpointChannel(std::vector values, Callback callback) + : values_(std::move(values)), callback_(std::move(callback)) {} + + bool integrate( + const PosedPointcloud<>& pointcloud, + const std::vector& sorted_points) + const override { + if (values_.size() != pointcloud.size() || + sorted_points.size() != pointcloud.size()) { + return false; + } + std::vector ordered_values(values_.size()); + for (size_t index = 0u; index < sorted_points.size(); ++index) { + if (sorted_points[index].source_index >= values_.size()) { + return false; + } + ordered_values[index] = values_[sorted_points[index].source_index]; + } + callback_(pointcloud, ordered_values); + return true; + } + + private: + std::vector values_; + Callback callback_; +}; + +template +class NumericPointcloudEndpointDecoder final + : public PointcloudEndpointDecoder { + public: + using Callback = typename DecodedScalarEndpointChannel::Callback; + + NumericPointcloudEndpointDecoder(const sensor_msgs::PointField& field, + size_t num_points, FloatingPoint scale, + FloatingPoint offset, ValueT min_value, + ValueT max_value, Callback callback) + : field_(field), + scale_(scale), + offset_(offset), + min_value_(min_value), + max_value_(max_value), + callback_(std::move(callback)) { + values_.reserve(num_points); + } + + bool decodePoint(const sensor_msgs::PointCloud2& msg, + size_t point_offset) override { + FloatingPoint raw = 0.f; + if (!convert::detail::readNumericPointField(msg, field_, point_offset, + raw) || + !std::isfinite(raw)) { + return false; + } + const FloatingPoint converted = offset_ + scale_ * raw; + if (!std::isfinite(converted)) { + return false; + } + if constexpr (std::is_integral_v) { + if (converted != std::trunc(converted) || + converted < static_cast( + std::numeric_limits::lowest()) || + static_cast(std::numeric_limits::max()) < + converted) { + return false; + } + } + values_.emplace_back( + std::clamp(static_cast(converted), min_value_, max_value_)); + return true; + } + + std::unique_ptr finish() override { + return std::make_unique>( + std::move(values_), std::move(callback_)); + } + + private: + sensor_msgs::PointField field_; + FloatingPoint scale_; + FloatingPoint offset_; + ValueT min_value_; + ValueT max_value_; + Callback callback_; + std::vector values_; +}; + +template +class NumericPointcloudEndpointAdapter final + : public PointcloudEndpointAdapter { + public: + using Callback = typename DecodedScalarEndpointChannel::Callback; + + NumericPointcloudEndpointAdapter(std::string field_name, FloatingPoint scale, + FloatingPoint offset, ValueT min_value, + ValueT max_value, Callback callback) + : field_name_(std::move(field_name)), + scale_(scale), + offset_(offset), + min_value_(min_value), + max_value_(max_value), + callback_(std::move(callback)) {} + + std::unique_ptr prepare( + const sensor_msgs::PointCloud2& msg, size_t num_points) const override { + const auto* field = convert::detail::findField(msg, field_name_); + if (!field || msg.point_step == 0u || msg.width == 0u || + msg.is_bigendian) { + return nullptr; + } + return std::make_unique>( + *field, num_points, scale_, offset_, min_value_, max_value_, callback_); + } + + private: + std::string field_name_; + FloatingPoint scale_; + FloatingPoint offset_; + ValueT min_value_; + ValueT max_value_; + Callback callback_; +}; + +// Lightweight view passed to a user conversion function. Field names and ROS +// datatypes are resolved once in prepare(); the per-point callback uses only +// numeric indices and performs no allocation or string lookup. +class NumericPointFieldValues { + public: + explicit NumericPointFieldValues(const std::vector& values) + : values_(values) {} + + size_t size() const { return values_.size(); } + FloatingPoint operator[](size_t index) const { return values_[index]; } + + private: + const std::vector& values_; +}; + +template +class DecodedOptionalScalarEndpointChannel final + : public DecodedPointcloudEndpointChannel { + public: + using OptionalValue = std::optional; + using Callback = std::function&, + const std::vector&)>; + + DecodedOptionalScalarEndpointChannel(std::vector values, + Callback callback) + : values_(std::move(values)), callback_(std::move(callback)) {} + + bool integrate( + const PosedPointcloud<>& pointcloud, + const std::vector& sorted_points) + const override { + if (values_.size() != pointcloud.size() || + sorted_points.size() != pointcloud.size()) { + return false; + } + std::vector ordered_values(values_.size()); + for (size_t index = 0u; index < sorted_points.size(); ++index) { + if (sorted_points[index].source_index >= values_.size()) { + return false; + } + ordered_values[index] = values_[sorted_points[index].source_index]; + } + callback_(pointcloud, ordered_values); + return true; + } + + private: + std::vector values_; + Callback callback_; +}; + +template +class ComputedNumericPointcloudEndpointDecoder final + : public PointcloudEndpointDecoder { + public: + using OptionalValue = std::optional; + using Converter = + std::function; + using Callback = + typename DecodedOptionalScalarEndpointChannel::Callback; + + ComputedNumericPointcloudEndpointDecoder( + std::vector fields, size_t num_points, + Converter converter, Callback callback) + : fields_(std::move(fields)), + field_values_(fields_.size()), + converter_(std::move(converter)), + callback_(std::move(callback)) { + values_.reserve(num_points); + } + + bool decodePoint(const sensor_msgs::PointCloud2& msg, + size_t point_offset) override { + for (size_t field_index = 0u; field_index < fields_.size(); ++field_index) { + auto& value = field_values_[field_index]; + if (!convert::detail::readNumericPointField( + msg, fields_[field_index], point_offset, value) || + !std::isfinite(value)) { + values_.emplace_back(std::nullopt); + return true; + } + } + values_.emplace_back(converter_(NumericPointFieldValues(field_values_))); + return true; + } + + std::unique_ptr finish() override { + return std::make_unique>( + std::move(values_), std::move(callback_)); + } + + private: + std::vector fields_; + std::vector field_values_; + Converter converter_; + Callback callback_; + std::vector values_; +}; + +template +class ComputedNumericPointcloudEndpointAdapter final + : public PointcloudEndpointAdapter { + public: + using Decoder = ComputedNumericPointcloudEndpointDecoder; + using Converter = typename Decoder::Converter; + using Callback = typename Decoder::Callback; + + ComputedNumericPointcloudEndpointAdapter( + std::vector field_names, Converter converter, + Callback callback) + : field_names_(std::move(field_names)), + converter_(std::move(converter)), + callback_(std::move(callback)) {} + + std::unique_ptr prepare( + const sensor_msgs::PointCloud2& msg, size_t num_points) const override { + if (field_names_.empty() || msg.point_step == 0u || msg.width == 0u || + msg.is_bigendian) { + return nullptr; + } + std::vector fields; + fields.reserve(field_names_.size()); + for (const auto& field_name : field_names_) { + const auto* field = convert::detail::findField(msg, field_name); + if (!field) { + return nullptr; + } + fields.emplace_back(*field); + } + return std::make_unique( + std::move(fields), num_points, converter_, callback_); + } + + private: + std::vector field_names_; + Converter converter_; + Callback callback_; +}; + +} // namespace wavemap + +#endif // WAVEMAP_ROS_INPUTS_POINTCLOUD_ENDPOINT_ADAPTER_H_ diff --git a/interfaces/ros1/wavemap_ros/include/wavemap_ros/inputs/pointcloud_topic_input.h b/interfaces/ros1/wavemap_ros/include/wavemap_ros/inputs/pointcloud_topic_input.h index f9148eecd..c1b1c1c95 100644 --- a/interfaces/ros1/wavemap_ros/include/wavemap_ros/inputs/pointcloud_topic_input.h +++ b/interfaces/ros1/wavemap_ros/include/wavemap_ros/inputs/pointcloud_topic_input.h @@ -2,6 +2,7 @@ #define WAVEMAP_ROS_INPUTS_POINTCLOUD_TOPIC_INPUT_H_ #include +#include #include #include #include @@ -12,6 +13,7 @@ #include #include "wavemap_ros/inputs/ros_input_base.h" +#include "wavemap_ros/inputs/pointcloud_endpoint_adapter.h" #include "wavemap_ros/utils/pointcloud_undistorter.h" #ifdef LIVOX_AVAILABLE @@ -100,6 +102,14 @@ class PointcloudTopicInput : public RosInputBase { return RosInputType::kPointcloudTopic; } PointcloudTopicType getTopicType() const { return config_.topic_type; } + void addEndpointAdapter(std::shared_ptr adapter) { + endpoint_adapters_.emplace_back(std::move(adapter)); + } + using PosedCloudCallback = + std::function& pointcloud)>; + void addPosedCloudCallback(PosedCloudCallback callback) { + posed_cloud_callbacks_.emplace_back(std::move(callback)); + } void callback(const sensor_msgs::PointCloud2& pointcloud_msg); #ifdef LIVOX_AVAILABLE @@ -117,7 +127,14 @@ class PointcloudTopicInput : public RosInputBase { PointcloudUndistorter pointcloud_undistorter_; ros::Subscriber pointcloud_sub_; - std::queue pointcloud_queue_; + struct QueuedPointcloud { + undistortion::StampedPointcloud pointcloud; + std::vector> + endpoint_channels; + }; + std::queue pointcloud_queue_; + std::vector> endpoint_adapters_; + std::vector posed_cloud_callbacks_; void processQueue() override; static bool hasField(const sensor_msgs::PointCloud2& msg, diff --git a/interfaces/ros1/wavemap_ros/include/wavemap_ros/inputs/sensor_frame_transformer.h b/interfaces/ros1/wavemap_ros/include/wavemap_ros/inputs/sensor_frame_transformer.h new file mode 100644 index 000000000..01617c257 --- /dev/null +++ b/interfaces/ros1/wavemap_ros/include/wavemap_ros/inputs/sensor_frame_transformer.h @@ -0,0 +1,80 @@ +#ifndef WAVEMAP_ROS_INPUTS_SENSOR_FRAME_TRANSFORMER_H_ +#define WAVEMAP_ROS_INPUTS_SENSOR_FRAME_TRANSFORMER_H_ + +#include +#include +#include +#include + +#include +#include + +#include "wavemap_ros/utils/tf_transformer.h" + +namespace wavemap { + +// Reuses the RosServer's TF buffer to convert custom sensor observations into +// the map's world frame. This helper owns neither a subscriber nor a TF +// listener and can therefore be stored directly by a user sensor-input class. +class SensorFrameTransformer { + public: + SensorFrameTransformer(std::shared_ptr transformer, + std::string world_frame) + : transformer_(std::move(transformer)), + world_frame_(std::move(world_frame)) {} + + const std::string& worldFrame() const { return world_frame_; } + + std::optional lookup( + const std_msgs::Header& header) const { + if (header.frame_id.empty()) { + ROS_WARN_THROTTLE(2.0, + "Cannot transform layered observations with an empty " + "source frame."); + return std::nullopt; + } + if (TfTransformer::sanitizeFrameId(header.frame_id) == + TfTransformer::sanitizeFrameId(world_frame_)) { + return Transformation3D(); + } + const auto transform = transformer_->lookupTransform( + world_frame_, header.frame_id, header.stamp); + if (!transform) { + ROS_WARN_STREAM_THROTTLE( + 2.0, "No transform from '" << header.frame_id << "' to '" + << world_frame_ << "' at " + << header.stamp << "; skipping layered " + "sensor observations."); + } + return transform; + } + + std::optional transformPosition( + const Point3D& sensor_position, + const std_msgs::Header& header) const { + const auto transform = lookup(header); + if (!transform) { + return std::nullopt; + } + return *transform * sensor_position; + } + + template + bool transformBatch(ObservationBatchT& batch, + const std_msgs::Header& header) const { + const auto transform = lookup(header); + if (!transform) { + return false; + } + batch.transformPositions(*transform); + return true; + } + + private: + std::shared_ptr transformer_; + std::string world_frame_; +}; + +} // namespace wavemap + +#endif // WAVEMAP_ROS_INPUTS_SENSOR_FRAME_TRANSFORMER_H_ diff --git a/interfaces/ros1/wavemap_ros/include/wavemap_ros/layered_hashed_wavelet_integrator_factory.h b/interfaces/ros1/wavemap_ros/include/wavemap_ros/layered_hashed_wavelet_integrator_factory.h new file mode 100644 index 000000000..afc766e8b --- /dev/null +++ b/interfaces/ros1/wavemap_ros/include/wavemap_ros/layered_hashed_wavelet_integrator_factory.h @@ -0,0 +1,67 @@ +#ifndef WAVEMAP_ROS_LAYERED_HASHED_WAVELET_INTEGRATOR_FACTORY_H_ +#define WAVEMAP_ROS_LAYERED_HASHED_WAVELET_INTEGRATOR_FACTORY_H_ + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace wavemap { + +// Typed counterpart of the original runtime IntegratorFactory boundary. The +// original factory remains unchanged for occupancy-only maps; layered servers +// use this path because their voxel type is known at compile time. +template +std::unique_ptr createLayeredHashedWaveletIntegrator( + const param::Value& params, + typename HashedWaveletOctreeT::Ptr occupancy_map, + std::shared_ptr thread_pool) { + const auto type = IntegratorType::from(params, "integration_method"); + if (!type || type.value() != IntegratorType::kHashedWaveletIntegrator) { + LOG(ERROR) << "A layered hashed wavelet map requires integration_method " + "type hashed_wavelet_integrator."; + return nullptr; + } + + const auto integrator_config = + ProjectiveIntegratorConfig::from(params, "integration_method"); + if (!integrator_config) { + LOG(ERROR) << "Layered integrator config could not be loaded."; + return nullptr; + } + + std::shared_ptr projection_model = + ProjectorFactory::create(params); + if (!projection_model) { + LOG(ERROR) << "Layered integrator projection model could not be created."; + return nullptr; + } + + auto posed_range_image = + std::make_shared>(projection_model->getDimensions()); + auto beam_offset_image = + std::make_shared>(projection_model->getDimensions()); + auto measurement_model = MeasurementModelFactory::create( + params, projection_model, posed_range_image, beam_offset_image); + if (!measurement_model) { + LOG(ERROR) << "Layered integrator measurement model could not be created."; + return nullptr; + } + + return std::make_unique>( + integrator_config.value(), std::move(projection_model), + std::move(posed_range_image), std::move(beam_offset_image), + std::move(measurement_model), std::move(occupancy_map), + std::move(thread_pool)); +} + +} // namespace wavemap + +#endif // WAVEMAP_ROS_LAYERED_HASHED_WAVELET_INTEGRATOR_FACTORY_H_ diff --git a/interfaces/ros1/wavemap_ros/include/wavemap_ros/layered_ros_server_extension.h b/interfaces/ros1/wavemap_ros/include/wavemap_ros/layered_ros_server_extension.h index 5519af79d..c294f905d 100644 --- a/interfaces/ros1/wavemap_ros/include/wavemap_ros/layered_ros_server_extension.h +++ b/interfaces/ros1/wavemap_ros/include/wavemap_ros/layered_ros_server_extension.h @@ -3,13 +3,23 @@ #include #include +#include #include #include #include #include +#include +#include +#include +#include +#include +#include +#include #include #include +#include +#include #include namespace wavemap { @@ -17,44 +27,138 @@ class LayeredRosServerExtensionBase { public: virtual ~LayeredRosServerExtensionBase() = default; - virtual bool saveLayeredMap(const std::filesystem::path& file_path) const = 0; + virtual bool saveLayeredMap(const std::filesystem::path& file_path) = 0; virtual bool loadLayeredMap(const std::filesystem::path& file_path, std::string* error_message = nullptr) = 0; virtual bool publishLayeredMap(const ros::Time& stamp = ros::Time::now()) = 0; + virtual MapBase::Ptr getContinuousMap() = 0; + virtual void clearLayeredMap() = 0; }; -template +template < + typename LayeredMapT, typename LayeredMapIoT, typename RosConverterT, + typename UpdateConverterT = + convert::DescriptorLayerObservationRosConverter> class LayeredRosServerExtension : public LayeredRosServerExtensionBase { public: LayeredRosServerExtension(ros::NodeHandle nh_private, std::string world_frame, LayeredMapT layered_map, - std::string layered_map_topic = "layered_map") + std::string layered_map_topic = "layered_map", + std::string update_topic = "layered_map_updates") : nh_private_(std::move(nh_private)), world_frame_(std::move(world_frame)), - layered_map_(std::move(layered_map)) { + layered_map_(std::move(layered_map)), + layered_pipeline_(layered_map_) { layered_map_pub_ = nh_private_.advertise( layered_map_topic, kQueueSize, kLatchPublisher); + layered_update_sub_ = nh_private_.subscribe( + update_topic, kQueueSize, + &LayeredRosServerExtension::layeredUpdateCallback, this); advertiseServices(); } LayeredMapT& getLayeredMap() { return layered_map_; } const LayeredMapT& getLayeredMap() const { return layered_map_; } + MapBase::Ptr getContinuousMap() override { + return layered_map_.continuousMapPtr(); + } + void clearLayeredMap() override { + std::scoped_lock lock(mutex_); + layered_map_.continuousMap().clear(); + layered::forEachLayerDescriptor( + layered_map_.discreteLayers(), + [](const auto& /*descriptor*/, auto& layer) { layer.clear(); }); + } + + template + bool integrateEndpointLayer(const PosedPointcloud<>& pointcloud, + const std::vector< + layered::LayerValueT>& values, + const layered::EndpointRange& endpoint_range = {}) { + return integratePointcloudLayer< + LayerTagT, + layered::PointcloudLayerIntegrationMode::kEndpointOnly>( + pointcloud, values, endpoint_range); + } + + template + bool integratePointcloudLayer( + const PosedPointcloud<>& pointcloud, + const std::vector>& values, + const layered::EndpointRange& endpoint_range = {}) { + return integratePointcloudLayerImpl( + pointcloud, values.size(), + [&values](size_t index) + -> std::optional> { + return values[index]; + }, + endpoint_range); + } + + template + bool integrateOptionalEndpointLayer( + const PosedPointcloud<>& pointcloud, + const std::vector>>& values, + const layered::EndpointRange& endpoint_range = {}) { + return integrateOptionalPointcloudLayer< + LayerTagT, + layered::PointcloudLayerIntegrationMode::kEndpointOnly>( + pointcloud, values, endpoint_range); + } + + template + bool integrateOptionalPointcloudLayer( + const PosedPointcloud<>& pointcloud, + const std::vector< + std::optional>>& values, + const layered::EndpointRange& endpoint_range = {}) { + return integratePointcloudLayerImpl( + pointcloud, values.size(), + [&values](size_t index) { return values[index]; }, endpoint_range); + } - bool saveLayeredMap(const std::filesystem::path& file_path) const override { + template + layered::LayerIntegrationResult integrateObservations( + const std::vector>>& observations) { std::scoped_lock lock(mutex_); + return layered_pipeline_.template integrate(observations); + } + + template + layered::LayerIntegrationResult integrateEndpointObservations( + const std::vector>>& observations) { + return integrateObservations(observations); + } + + template + layered::LayerIntegrationResult integrateObservationBatch( + const ObservationBatchT& batch) { + std::scoped_lock lock(mutex_); + return layered_pipeline_.integrateBatch(batch); + } + + bool saveLayeredMap(const std::filesystem::path& file_path) override { + std::scoped_lock lock(mutex_); + // Match the original save_map behavior: persist a fully thresholded + // snapshot, including every typed continuous layer. + layered_map_.continuousMap().threshold(); return LayeredMapIoT::save(file_path, layered_map_); } bool loadLayeredMap(const std::filesystem::path& file_path, std::string* error_message = nullptr) override { - LayeredMapT loaded_map = layered_map_; - if (!LayeredMapIoT::load(file_path, loaded_map, error_message)) { - return false; - } - { std::scoped_lock lock(mutex_); - layered_map_ = std::move(loaded_map); + // Load into the existing object so the core Pipeline keeps owning the + // exact same continuous octree. + if (!LayeredMapIoT::load(file_path, layered_map_, error_message)) { + return false; + } } return publishLayeredMap(); } @@ -67,6 +171,8 @@ class LayeredRosServerExtension : public LayeredRosServerExtensionBase { layered_map_, world_frame_, stamp, msg)) { return false; } + convert::clearDiscreteLayerBundleDirtyState( + layered_map_.discreteLayers()); } layered_map_pub_.publish(msg); @@ -74,6 +180,100 @@ class LayeredRosServerExtension : public LayeredRosServerExtensionBase { } private: + template + bool integratePointcloudLayerImpl( + const PosedPointcloud<>& pointcloud, size_t value_count, + ValueAccessorT&& value_at, + const layered::EndpointRange& endpoint_range) { + if (value_count != pointcloud.size()) { + return false; + } + if constexpr (!layered::kIsWaveletCompatibleLayer) { + static_assert( + IntegrationMode == + layered::PointcloudLayerIntegrationMode::kEndpointOnly, + "Discrete point fields can only describe occupied endpoints. Use " + "positioned observations for other integration semantics."); + using Value = layered::LayerValueT; + std::vector> observations; + observations.reserve(pointcloud.size()); + for (size_t point_index = 0u; point_index < pointcloud.size(); + ++point_index) { + const auto value = value_at(point_index); + if (!value) { + continue; + } + const Point3D C_endpoint = pointcloud[point_index]; + if (!endpoint_range.contains(C_endpoint)) { + continue; + } + observations.emplace_back(pointcloud.getPose() * C_endpoint, *value); + } + std::scoped_lock lock(mutex_); + const auto result = + layered_pipeline_.template integrate(observations); + return result.rejected == 0u; + } else { + constexpr auto descriptor = layered::detail::descriptorForLayer< + LayerTagT, typename LayeredMapT::ContinuousLayers>(); + const FloatingPoint cell_width = + layered_map_.continuousMap().getMinCellWidth(); + const Point3D& W_origin = pointcloud.getOrigin(); + std::scoped_lock lock(mutex_); + for (size_t point_index = 0u; point_index < pointcloud.size(); + ++point_index) { + const auto value = value_at(point_index); + if (!value) { + continue; + } + const Point3D C_endpoint = pointcloud[point_index]; + if (!endpoint_range.contains(C_endpoint)) { + continue; + } + const Point3D W_endpoint = pointcloud.getPose() * C_endpoint; + const layered::LayerObservation> + observation(W_endpoint, *value); + layered::forEachPointcloudIntegrationIndex( + W_origin, W_endpoint, cell_width, + [&](const Index3D& voxel_index) { + layered::updateContinuousLayersAtVoxel( + layered_map_.continuousMap(), voxel_index, + [&](auto& layers) { + auto& current = descriptor.value(layers); + current = layered::LayerUpdatePolicyT{}( + current, observation); + }); + }); + } + return true; + } + } + + void layeredUpdateCallback( + const wavemap_msgs::LayeredMapUpdate::ConstPtr& msg) { + if (msg->header.frame_id != world_frame_) { + ROS_WARN_STREAM("Ignoring layered map update in frame '" + << msg->header.frame_id << "'; expected '" + << world_frame_ << "'."); + return; + } + + std::string error_message; + { + std::scoped_lock lock(mutex_); + if (!UpdateConverterT::integrate( + *msg, layered_pipeline_, &error_message)) { + ROS_WARN_STREAM("Ignoring invalid layered map update: " + << error_message); + return; + } + } + // The periodic layered publish operation consumes the continuous block + // timestamps and discrete dirty-parent state in one coherent update. + } + void advertiseServices() { save_layered_map_srv_ = nh_private_.advertiseService layered_pipeline_; ros::Publisher layered_map_pub_; + ros::Subscriber layered_update_sub_; ros::ServiceServer load_layered_map_srv_; ros::ServiceServer save_layered_map_srv_; ros::ServiceServer request_full_layered_map_srv_; diff --git a/interfaces/ros1/wavemap_ros/include/wavemap_ros/layered_server_builder.h b/interfaces/ros1/wavemap_ros/include/wavemap_ros/layered_server_builder.h new file mode 100644 index 000000000..f351aa571 --- /dev/null +++ b/interfaces/ros1/wavemap_ros/include/wavemap_ros/layered_server_builder.h @@ -0,0 +1,72 @@ +#ifndef WAVEMAP_ROS_LAYERED_SERVER_BUILDER_H_ +#define WAVEMAP_ROS_LAYERED_SERVER_BUILDER_H_ + +#include +#include +#include + +#include + +#include "wavemap_ros/layered_hashed_wavelet_integrator_factory.h" +#include "wavemap_ros/layered_ros_server_extension.h" +#include "wavemap_ros/map_operations/layered_publish_map_operation.h" +#include "wavemap_ros/ros_server.h" + +namespace wavemap { + +// Installs one compile-time LayeredMap definition into the existing ROS server +// lifecycle. The original occupancy-only construction remains untouched. +// SensorBindingsT receives (RosServer&, TypedExtension&) and only registers +// how sensor data populates the schema's layers. +template +class LayeredRosServerBuilder { + public: + using Definition = MapDefinitionT; + using Map = typename Definition::Map; + using Config = typename Definition::Config; + using MapIo = typename Definition::MapIo; + using Voxel = typename Definition::Voxel; + using ObservationBatch = + layered::LayerObservationBatch; + using Extension = LayeredRosServerExtension; + + template + static Extension& install(RosServer& server, ros::NodeHandle nh_private, + std::string world_frame, Config config, + SensorBindingsT&& sensor_bindings) { + auto extension = std::make_unique( + nh_private, world_frame, Map(config)); + auto* const extension_ptr = extension.get(); + + std::forward(sensor_bindings)(server, *extension_ptr); + + server.layered_extension_ = std::move(extension); + server.occupancy_map_ = server.layered_extension_->getContinuousMap(); + + auto continuous_map = extension_ptr->getLayeredMap().continuousMapPtr(); + server.layered_integrator_factory_ = + [continuous_map](const param::Value& params, + std::shared_ptr thread_pool) { + return createLayeredHashedWaveletIntegrator( + params, continuous_map, std::move(thread_pool)); + }; + + server.layered_publish_operation_factory_ = + [&server, extension_ptr, world_frame = std::move(world_frame)]( + const PublishMapOperationConfig& publish_config, + ros::NodeHandle operation_nh_private) { + using Operation = + UnifiedLayeredPublishMapOperation; + return std::make_unique( + publish_config, &extension_ptr->getLayeredMap(), + server.thread_pool_, world_frame, + std::move(operation_nh_private)); + }; + + return *extension_ptr; + } +}; + +} // namespace wavemap + +#endif // WAVEMAP_ROS_LAYERED_SERVER_BUILDER_H_ diff --git a/interfaces/ros1/wavemap_ros/include/wavemap_ros/map_operations/layered_publish_map_operation.h b/interfaces/ros1/wavemap_ros/include/wavemap_ros/map_operations/layered_publish_map_operation.h new file mode 100644 index 000000000..507864523 --- /dev/null +++ b/interfaces/ros1/wavemap_ros/include/wavemap_ros/map_operations/layered_publish_map_operation.h @@ -0,0 +1,212 @@ +#ifndef WAVEMAP_ROS_MAP_OPERATIONS_LAYERED_PUBLISH_MAP_OPERATION_H_ +#define WAVEMAP_ROS_MAP_OPERATIONS_LAYERED_PUBLISH_MAP_OPERATION_H_ + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "wavemap_ros/map_operations/publish_map_operation.h" + +namespace wavemap { + +// Typed equivalent of PublishMapOperation for HashedWaveletOctreeT. +// It uses the same topic, batching, timestamps, full-map request service, and +// deletion semantics as the original occupancy-only publisher. +template +class LayeredPublishMapOperation final : public MapOperationBase { + public: + using Map = HashedWaveletOctreeT; + + LayeredPublishMapOperation( + const PublishMapOperationConfig& config, typename Map::Ptr map, + std::shared_ptr thread_pool, std::string world_frame, + ros::NodeHandle nh_private) + : MapOperationBase(map), + config_(config.checkValid()), + map_(std::move(map)), + thread_pool_(std::move(thread_pool)), + world_frame_(std::move(world_frame)) { + map_pub_ = nh_private.advertise(config_.topic, 10); + republish_whole_map_srv_ = + nh_private.advertiseService( + config_.topic + "_request_full", [this](auto&, auto&) { + publishMap(ros::Time::now(), true); + return true; + }); + } + + void run(bool force_run) override { + const ros::Time current_time = ros::Time::now(); + if (force_run || + config_.once_every < (current_time - last_run_timestamp_).toSec()) { + publishMap(current_time, false); + last_run_timestamp_ = current_time; + } + } + + private: + void publishMap(const ros::Time& current_time, bool republish_whole_map) { + ProfilerZoneScoped; + if (map_->empty()) { + return; + } + + const Timestamp selection_start = Time::now(); + std::unordered_set changed_blocks; + map_->forEachBlock( + [&changed_blocks, republish_whole_map, + last_published = last_run_timestamp_internal_]( + const Index3D& block_index, const auto& block) { + if (republish_whole_map || + last_published < block.getLastUpdatedStamp()) { + changed_blocks.emplace(block_index); + } + }); + last_run_timestamp_internal_ = selection_start; + + while (!changed_blocks.empty()) { + std::unordered_set blocks_to_publish; + for (const auto& block_index : changed_blocks) { + if (auto* block = map_->getBlock(block_index); block) { + block->threshold(); + blocks_to_publish.emplace(block_index); + if (config_.max_num_blocks_per_msg <= + static_cast(blocks_to_publish.size())) { + break; + } + } + } + + wavemap_msgs::Map map_msg; + convert::mapToRosMsg( + *map_, world_frame_, current_time, map_msg, blocks_to_publish, + thread_pool_); + map_pub_.publish(map_msg); + + for (const auto& block_index : blocks_to_publish) { + changed_blocks.erase(block_index); + } + } + } + + const PublishMapOperationConfig config_; + const typename Map::Ptr map_; + const std::shared_ptr thread_pool_; + const std::string world_frame_; + ros::Time last_run_timestamp_; + Timestamp last_run_timestamp_internal_; + ros::Publisher map_pub_; + ros::ServiceServer republish_whole_map_srv_; +}; + +// Unified publisher for a complete LayeredMap. Occupancy and other continuous +// fields use Wavemap's block timestamps, while discrete layers use dirty +// compressed-parent tracking. Continuous-only schemas pay only one empty tuple +// traversal per publication cycle. +template +class UnifiedLayeredPublishMapOperation final : public MapOperationBase { + public: + using ContinuousMap = typename LayeredMapT::ContinuousMap; + + UnifiedLayeredPublishMapOperation( + const PublishMapOperationConfig& config, + LayeredMapT* layered_map, + std::shared_ptr thread_pool, std::string world_frame, + ros::NodeHandle nh_private) + : MapOperationBase(CHECK_NOTNULL(layered_map)->continuousMapPtr()), + config_(config.checkValid()), + layered_map_(std::move(layered_map)), + thread_pool_(std::move(thread_pool)), + world_frame_(std::move(world_frame)) { + map_pub_ = nh_private.advertise(config_.topic, 10); + } + + void run(bool force_run) override { + const ros::Time current_time = ros::Time::now(); + if (force_run || + config_.once_every < (current_time - last_run_timestamp_).toSec()) { + publishIncrement(current_time); + last_run_timestamp_ = current_time; + } + } + + private: + void publishIncrement(const ros::Time& stamp) { + const Timestamp selection_start = Time::now(); + std::unordered_set changed_blocks; + layered_map_->continuousMap().forEachBlock( + [&changed_blocks, last_published = last_run_timestamp_internal_]( + const Index3D& block_index, const auto& block) { + if (last_published < block.getLastUpdatedStamp()) { + changed_blocks.emplace(block_index); + } + }); + const bool has_discrete_changes = + convert::discreteLayerBundleHasDirtyState( + layered_map_->discreteLayers()); + last_run_timestamp_internal_ = selection_start; + if (changed_blocks.empty() && !has_discrete_changes) { + return; + } + + bool discrete_patch_pending = has_discrete_changes; + do { + std::unordered_set blocks_to_publish; + for (const auto& block_index : changed_blocks) { + if (auto* block = + layered_map_->continuousMap().getBlock(block_index); + block) { + block->threshold(); + blocks_to_publish.emplace(block_index); + if (config_.max_num_blocks_per_msg <= + static_cast(blocks_to_publish.size())) { + break; + } + } + } + + wavemap_msgs::LayeredMap msg; + convert::layeredMapPatchToRosMsg( + *layered_map_, world_frame_, stamp, msg, blocks_to_publish, + thread_pool_); + if (!discrete_patch_pending) { + msg.discrete_layers.clear(); + } + map_pub_.publish(msg); + discrete_patch_pending = false; + for (const auto& block_index : blocks_to_publish) { + changed_blocks.erase(block_index); + } + } while (!changed_blocks.empty()); + + convert::clearDiscreteLayerBundleDirtyState( + layered_map_->discreteLayers()); + } + + const PublishMapOperationConfig config_; + LayeredMapT* const layered_map_; + const std::shared_ptr thread_pool_; + const std::string world_frame_; + ros::Time last_run_timestamp_; + Timestamp last_run_timestamp_internal_; + ros::Publisher map_pub_; +}; + +} // namespace wavemap + +#endif // WAVEMAP_ROS_MAP_OPERATIONS_LAYERED_PUBLISH_MAP_OPERATION_H_ diff --git a/interfaces/ros1/wavemap_ros/include/wavemap_ros/ros_server.h b/interfaces/ros1/wavemap_ros/include/wavemap_ros/ros_server.h index 43bc245c2..580cabff0 100644 --- a/interfaces/ros1/wavemap_ros/include/wavemap_ros/ros_server.h +++ b/interfaces/ros1/wavemap_ros/include/wavemap_ros/ros_server.h @@ -3,7 +3,9 @@ #include #include +#include #include +#include #include #include @@ -15,14 +17,21 @@ #include #include #include +#include #include #include "wavemap_ros/inputs/ros_input_base.h" +#include "wavemap_ros/inputs/pointcloud_topic_input.h" +#include "wavemap_ros/inputs/sensor_frame_transformer.h" #include "wavemap_ros/layered_ros_server_extension.h" +#include "wavemap_ros/map_operations/publish_map_operation.h" #include "wavemap_ros/utils/ros_logging_level.h" #include "wavemap_ros/utils/tf_transformer.h" namespace wavemap { +template +class LayeredRosServerBuilder; + /** * Config struct for wavemap's ROS server. */ @@ -46,9 +55,16 @@ struct RosServerConfig : ConfigBase { class RosServer { public: + using MapInstaller = + std::function; + RosServer(ros::NodeHandle nh, ros::NodeHandle nh_private); + RosServer(ros::NodeHandle nh, ros::NodeHandle nh_private, + MapInstaller map_installer); RosServer(ros::NodeHandle nh, ros::NodeHandle nh_private, const RosServerConfig& config); + RosServer(ros::NodeHandle nh, ros::NodeHandle nh_private, + const RosServerConfig& config, MapInstaller map_installer); void clear(); @@ -57,6 +73,11 @@ class RosServer { Pipeline& getPipeline() { return *pipeline_; } const Pipeline& getPipeline() const { return *pipeline_; } + const std::string& worldFrame() const { return config_.world_frame; } + + SensorFrameTransformer makeSensorFrameTransformer() const { + return SensorFrameTransformer(transformer_, config_.world_frame); + } MapOperationBase* addOperation(const param::Value& operation_params, ros::NodeHandle nh_private); @@ -64,6 +85,129 @@ class RosServer { RosInputBase* addInput(const param::Value& integrator_params, const ros::NodeHandle& nh, ros::NodeHandle nh_private); RosInputBase* addInput(std::unique_ptr input); + using PointcloudEndpointAdapterRegistrar = + std::function; + void addPointcloudEndpointAdapterRegistrar( + PointcloudEndpointAdapterRegistrar registrar); + template + void bindPointField( + LayeredExtensionT& extension, + PointFieldBinding> binding = {}, + layered::EndpointRange endpoint_range = {}) { + using Value = layered::LayerValueT; + static_assert(std::is_arithmetic_v, + "bindPointField supports numeric scalar layers. Use a " + "custom PointcloudEndpointAdapter for structured values."); + if (binding.field_name.empty()) { + binding.field_name = + std::string(layered::LayerTraits::name); + } + addPointcloudEndpointAdapterRegistrar( + [&extension, binding = std::move(binding), endpoint_range]( + PointcloudTopicInput& input) { + using Adapter = NumericPointcloudEndpointAdapter; + input.addEndpointAdapter(std::make_shared( + binding.field_name, binding.scale, binding.offset, + binding.min_value, binding.max_value, + [&extension, endpoint_range](const PosedPointcloud<>& pointcloud, + const std::vector& values) { + extension.template integratePointcloudLayer( + pointcloud, values, endpoint_range); + })); + }); + } + template + void bindNormalizedPointField( + LayeredExtensionT& extension, std::string field_name, + FloatingPoint raw_min, FloatingPoint raw_max, + layered::EndpointRange endpoint_range = {}) { + using Value = layered::LayerValueT; + static_assert(std::is_arithmetic_v, + "Normalized point fields require a numeric scalar layer."); + bindPointField( + extension, + PointFieldBinding::normalized(std::move(field_name), raw_min, + raw_max), + endpoint_range); + } + template + void bindDirectPointField( + LayeredExtensionT& extension, std::string field_name = {}, + layered::EndpointRange endpoint_range = {}) { + using Value = layered::LayerValueT; + static_assert(std::is_arithmetic_v, + "Direct point fields require a numeric scalar layer."); + bindPointField( + extension, PointFieldBinding(std::move(field_name)), + endpoint_range); + } + template + void bindComputedPointField( + LayeredExtensionT& extension, + std::vector source_field_names, ConverterT converter, + layered::EndpointRange endpoint_range = {}) { + using Value = layered::LayerValueT; + using FieldValues = NumericPointFieldValues; + using OptionalValue = std::optional; + static_assert( + std::is_invocable_r_v, + "A computed point-field converter must return optional " + "and accept const NumericPointFieldValues&."); + addPointcloudEndpointAdapterRegistrar( + [&extension, source_field_names = std::move(source_field_names), + converter = std::move(converter), endpoint_range]( + PointcloudTopicInput& input) { + using Adapter = ComputedNumericPointcloudEndpointAdapter; + input.addEndpointAdapter(std::make_shared( + source_field_names, converter, + [&extension, endpoint_range]( + const PosedPointcloud<>& pointcloud, + const std::vector& values) { + extension.template integrateOptionalPointcloudLayer< + LayerTagT, IntegrationMode>( + pointcloud, values, endpoint_range); + })); + }); + } + template + void bindGeometricClass( + LayeredExtensionT& extension, + layered::LocalElevationClassifierConfig classifier_config = {}, + layered::EndpointRange endpoint_range = {}) { + using Value = layered::LayerValueT; + static_assert(std::is_integral_v, + "Geometric class layers require an integral value type."); + addPointcloudEndpointAdapterRegistrar( + [&extension, classifier = layered::LocalElevationClassifier( + classifier_config), endpoint_range]( + PointcloudTopicInput& input) { + input.addPosedCloudCallback( + [&extension, classifier, endpoint_range]( + const PosedPointcloud<>& pointcloud) { + const auto observations = + layered::makeGeometricClassObservations( + pointcloud, classifier, endpoint_range); + const auto result = + extension.template integrateObservations< + LayerTagT>(observations); + return result.rejected == 0u; + }); + }); + } const std::vector>& getInputs() { return inputs_; } @@ -73,11 +217,23 @@ class RosServer { bool loadMap(const std::filesystem::path& file_path); private: + template + friend class LayeredRosServerBuilder; + const RosServerConfig config_; // Map data structure MapBase::Ptr occupancy_map_; std::unique_ptr layered_extension_; + using LayeredIntegratorFactory = std::function( + const param::Value&, std::shared_ptr)>; + LayeredIntegratorFactory layered_integrator_factory_; + using LayeredPublishOperationFactory = + std::function( + const PublishMapOperationConfig&, ros::NodeHandle)>; + LayeredPublishOperationFactory layered_publish_operation_factory_; + std::vector + pointcloud_endpoint_adapter_registrars_; // Threadpool shared among all input handlers and operations std::shared_ptr thread_pool_; diff --git a/interfaces/ros1/wavemap_ros/launch/datasets/external_ouster_layered_smoke.launch b/interfaces/ros1/wavemap_ros/launch/datasets/external_ouster_layered_smoke.launch new file mode 100644 index 000000000..40d17c4dc --- /dev/null +++ b/interfaces/ros1/wavemap_ros/launch/datasets/external_ouster_layered_smoke.launch @@ -0,0 +1,33 @@ + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/interfaces/ros1/wavemap_ros/launch/datasets/external_ouster_occupancy_validation.launch b/interfaces/ros1/wavemap_ros/launch/datasets/external_ouster_occupancy_validation.launch new file mode 100644 index 000000000..7d6acf0b8 --- /dev/null +++ b/interfaces/ros1/wavemap_ros/launch/datasets/external_ouster_occupancy_validation.launch @@ -0,0 +1,29 @@ + + + + + + + + + + + + + + + + + + + + + + diff --git a/interfaces/ros1/wavemap_ros/launch/wavemap_server.launch b/interfaces/ros1/wavemap_ros/launch/wavemap_server.launch index 41f24cff7..89daec007 100644 --- a/interfaces/ros1/wavemap_ros/launch/wavemap_server.launch +++ b/interfaces/ros1/wavemap_ros/launch/wavemap_server.launch @@ -11,6 +11,8 @@ doc="Automatically launch Rviz."/> + diff --git a/interfaces/ros1/wavemap_ros/package.xml b/interfaces/ros1/wavemap_ros/package.xml index 14fcf6d93..38d1cc545 100644 --- a/interfaces/ros1/wavemap_ros/package.xml +++ b/interfaces/ros1/wavemap_ros/package.xml @@ -20,6 +20,7 @@ rosbag cv_bridge image_transport + message_filters tf2_ros std_srvs diff --git a/interfaces/ros1/wavemap_ros/src/inputs/pointcloud_topic_input.cc b/interfaces/ros1/wavemap_ros/src/inputs/pointcloud_topic_input.cc index bbd78edc6..45b66c8b3 100644 --- a/interfaces/ros1/wavemap_ros/src/inputs/pointcloud_topic_input.cc +++ b/interfaces/ros1/wavemap_ros/src/inputs/pointcloud_topic_input.cc @@ -14,6 +14,7 @@ #include #include #include +#include namespace wavemap { DECLARE_CONFIG_MEMBERS(PointcloudTopicInputConfig, @@ -110,6 +111,37 @@ void PointcloudTopicInput::callback( : config_.sensor_frame_id; undistortion::StampedPointcloud stamped_pointcloud{ stamp_nsec, std::move(sensor_frame_id), num_points}; + std::vector> endpoint_decoders; + endpoint_decoders.reserve(endpoint_adapters_.size()); + for (const auto& adapter : endpoint_adapters_) { + if (auto decoder = adapter->prepare(pointcloud_msg, num_points)) { + endpoint_decoders.emplace_back(std::move(decoder)); + } + } + + // Decode all configured endpoint fields in one shared traversal. The XYZ + // conversion below remains unchanged to preserve Wavemap's input behavior. + bool endpoint_decoding_valid = true; + for (size_t row = 0u; row < pointcloud_msg.height; ++row) { + for (size_t column = 0u; column < pointcloud_msg.width; ++column) { + const size_t point_offset = + row * pointcloud_msg.row_step + column * pointcloud_msg.point_step; + for (auto& decoder : endpoint_decoders) { + endpoint_decoding_valid &= + decoder->decodePoint(pointcloud_msg, point_offset); + } + } + } + std::vector> + endpoint_channels; + if (endpoint_decoding_valid) { + endpoint_channels.reserve(endpoint_decoders.size()); + for (auto& decoder : endpoint_decoders) { + endpoint_channels.emplace_back(decoder->finish()); + } + } else { + ROS_WARN("Skipping malformed pointcloud endpoint channels."); + } // Load the points with time information if undistortion is enabled bool loaded = false; @@ -121,8 +153,10 @@ void PointcloudTopicInput::callback( if (hasField(pointcloud_msg, "t")) { sensor_msgs::PointCloud2ConstIterator t_it(pointcloud_msg, "t"); - for (; pos_it != pos_it.end(); ++pos_it, ++t_it) { - stamped_pointcloud.emplace(pos_it[0], pos_it[1], pos_it[2], *t_it); + size_t source_index = 0u; + for (; pos_it != pos_it.end(); ++pos_it, ++t_it, ++source_index) { + stamped_pointcloud.emplace(pos_it[0], pos_it[1], pos_it[2], *t_it, + source_index); } loaded = true; } else { @@ -142,13 +176,16 @@ void PointcloudTopicInput::callback( // If undistortion is disabled or loading failed, only load positions if (!loaded) { - for (; pos_it != pos_it.end(); ++pos_it) { - stamped_pointcloud.emplace(pos_it[0], pos_it[1], pos_it[2], 0); + size_t source_index = 0u; + for (; pos_it != pos_it.end(); ++pos_it, ++source_index) { + stamped_pointcloud.emplace(pos_it[0], pos_it[1], pos_it[2], 0, + source_index); } } // Add it to the integration queue - pointcloud_queue_.emplace(std::move(stamped_pointcloud)); + pointcloud_queue_.push( + {std::move(stamped_pointcloud), std::move(endpoint_channels)}); } #ifdef LIVOX_AVAILABLE @@ -176,18 +213,20 @@ void PointcloudTopicInput::callback( } // Add it to the integration queue - pointcloud_queue_.emplace(std::move(stamped_pointcloud)); + pointcloud_queue_.push({std::move(stamped_pointcloud), {}}); } #endif void PointcloudTopicInput::processQueue() { ProfilerZoneScoped; while (!pointcloud_queue_.empty()) { - auto& oldest_msg = pointcloud_queue_.front(); + auto& queued_cloud = pointcloud_queue_.front(); + auto& oldest_msg = queued_cloud.pointcloud; // Drop messages if they're older than max_wait_for_pose if (config_.max_wait_for_pose < - convert::nanoSecondsToSeconds(pointcloud_queue_.back().getEndTime() - + convert::nanoSecondsToSeconds( + pointcloud_queue_.back().pointcloud.getEndTime() - oldest_msg.getStartTime())) { ROS_WARN_STREAM( "Max waiting time of " @@ -196,7 +235,8 @@ void PointcloudTopicInput::processQueue() { << oldest_msg.getSensorFrame() << "\" and time interval [" << oldest_msg.getStartTime() << ", " << oldest_msg.getEndTime() << "] vs newest cloud end time " - << pointcloud_queue_.back().getEndTime() << ". Dropping cloud."); + << pointcloud_queue_.back().pointcloud.getEndTime() + << ". Dropping cloud."); pointcloud_queue_.pop(); continue; } @@ -260,8 +300,22 @@ void PointcloudTopicInput::processQueue() { << " points. Remaining pointclouds in queue: " << pointcloud_queue_.size() - 1 << "."); integration_timer_.start(); - pipeline_->runPipeline(config_.measurement_integrator_names, - posed_pointcloud); + pipeline_->runIntegrators(config_.measurement_integrator_names, + posed_pointcloud); + for (const auto& endpoint_channel : queued_cloud.endpoint_channels) { + if (!endpoint_channel->integrate(posed_pointcloud, + oldest_msg.getPoints())) { + ROS_WARN("Skipping malformed pointcloud endpoint channel."); + } + } + for (const auto& callback : posed_cloud_callbacks_) { + if (!callback(posed_pointcloud)) { + ROS_WARN("A derived endpoint layer rejected the posed pointcloud."); + } + } + // Publish/prune only after occupancy and endpoint fields from this + // measurement have both been committed. + pipeline_->runOperations(); integration_timer_.stop(); ROS_DEBUG_STREAM("Integrated new pointcloud in " << integration_timer_.getLastEpisodeDuration() diff --git a/interfaces/ros1/wavemap_ros/src/ros_server.cc b/interfaces/ros1/wavemap_ros/src/ros_server.cc index 00183fb01..e8f475205 100644 --- a/interfaces/ros1/wavemap_ros/src/ros_server.cc +++ b/interfaces/ros1/wavemap_ros/src/ros_server.cc @@ -10,9 +10,9 @@ #include #include -#include "example_layered_map_ros_config.h" -#include "layered_ros_converter.h" #include "wavemap_ros/inputs/ros_input_factory.h" +#include "wavemap_ros/inputs/pointcloud_topic_input.h" +#include "wavemap_ros/layered_server_builder.h" #include "wavemap_ros/map_operations/map_ros_operation_factory.h" namespace wavemap { @@ -38,21 +38,41 @@ RosServer::RosServer(ros::NodeHandle nh, ros::NodeHandle nh_private) : RosServer(nh, nh_private, RosServerConfig::from( param::convert::toParamValue(nh_private, "general")) - .value()) {} + .value(), + {}) {} + +RosServer::RosServer(ros::NodeHandle nh, ros::NodeHandle nh_private, + MapInstaller map_installer) + : RosServer(nh, nh_private, + RosServerConfig::from( + param::convert::toParamValue(nh_private, "general")) + .value(), + std::move(map_installer)) {} RosServer::RosServer(ros::NodeHandle nh, ros::NodeHandle nh_private, const RosServerConfig& config) + : RosServer(nh, nh_private, config, {}) {} + +RosServer::RosServer(ros::NodeHandle nh, ros::NodeHandle nh_private, + const RosServerConfig& config, + MapInstaller map_installer) : config_(config.checkValid()), transformer_(std::make_shared()) { // Set the logging level for wavemap's C++ library (uses glog) and ROS config_.logging_level.applyToGlog(); config_.logging_level.applyToRosConsole(); - // Setup data structure - const auto data_structure_params = - param::convert::toParamValue(nh_private, "map"); - occupancy_map_ = - MapFactory::create(data_structure_params, MapType::kHashedBlocks); + // A typed application may install one layered map before the original + // pipeline is created. Without an installer this is the unchanged original + // occupancy-only server construction path. + if (map_installer) { + map_installer(*this, nh_private); + } else { + const auto data_structure_params = + param::convert::toParamValue(nh_private, "map"); + occupancy_map_ = + MapFactory::create(data_structure_params, MapType::kHashedBlocks); + } CHECK_NOTNULL(occupancy_map_); // Setup thread pool @@ -91,7 +111,13 @@ RosServer::RosServer(ros::NodeHandle nh, ros::NodeHandle nh_private, param::convert::toParamMap(nh_private, "measurement_integrators"); for (const auto& [integrator_name, integrator_params] : measurement_integrator_param_map_) { - pipeline_->addIntegrator(integrator_name, integrator_params); + if (layered_integrator_factory_) { + pipeline_->addIntegrator( + integrator_name, + layered_integrator_factory_(integrator_params, thread_pool_)); + } else { + pipeline_->addIntegrator(integrator_name, integrator_params); + } } @@ -111,19 +137,6 @@ RosServer::RosServer(ros::NodeHandle nh, ros::NodeHandle nh_private, // Connect to ROS advertiseServices(nh_private); - bool enable_layered_map = false; - nh_private.param("layered_map/enabled", enable_layered_map, false); - if (enable_layered_map) { - ExampleLayeredMapConfig layered_config = - layered_map_config::loadExampleLayeredMapConfigFromRosParams(nh_private); - layered_extension_ = std::make_unique< - LayeredRosServerExtension>( - nh_private, config_.world_frame, ExampleLayeredMap(layered_config)); - ROS_INFO( - "Layered map extension enabled. Advertising layered map topic and " - "services."); - } } void RosServer::clear() { @@ -142,9 +155,26 @@ RosInputBase* RosServer::addInput(const param::Value& integrator_params, auto input = RosInputFactory::create(integrator_params, pipeline_, transformer_, config_.world_frame, nh, nh_private); + if (auto* pointcloud_input = + dynamic_cast(input.get())) { + for (const auto& registrar : pointcloud_endpoint_adapter_registrars_) { + registrar(*pointcloud_input); + } + } return addInput(std::move(input)); } +void RosServer::addPointcloudEndpointAdapterRegistrar( + PointcloudEndpointAdapterRegistrar registrar) { + for (const auto& input : inputs_) { + if (auto* pointcloud_input = + dynamic_cast(input.get())) { + registrar(*pointcloud_input); + } + } + pointcloud_endpoint_adapter_registrars_.emplace_back(std::move(registrar)); +} + RosInputBase* RosServer::addInput(std::unique_ptr input) { if (input) { return inputs_.emplace_back(std::move(input)).get(); @@ -170,6 +200,17 @@ MapOperationBase* RosServer::addOperation(const param::Value& operation_params, if (const auto type = MapRosOperationType{type_name.value()}; type.isValid()) { + if (type == MapRosOperationType::kPublishMap && + layered_publish_operation_factory_) { + const auto publish_config = + PublishMapOperationConfig::from(operation_params); + if (!publish_config) { + ROS_ERROR("Could not load layered publish_map configuration."); + return nullptr; + } + return pipeline_->addOperation(layered_publish_operation_factory_( + publish_config.value(), std::move(nh_private))); + } auto operation = MapRosOperationFactory::create( type, operation_params, occupancy_map_, thread_pool_, transformer_, config_.world_frame, nh_private); @@ -244,7 +285,9 @@ void RosServer::advertiseServices(ros::NodeHandle& nh_private) { "reset_map", [this](auto& /*request*/, auto& response) { response.success = false; if (config_.allow_reset_map_service) { - if (occupancy_map_) { + if (layered_extension_) { + layered_extension_->clearLayeredMap(); + } else if (occupancy_map_) { occupancy_map_->clear(); } ROS_INFO("Map reset request was successfully executed."); diff --git a/interfaces/ros1/wavemap_ros_conversions/CMakeLists.txt b/interfaces/ros1/wavemap_ros_conversions/CMakeLists.txt index 97a1972f0..b986f3bdf 100644 --- a/interfaces/ros1/wavemap_ros_conversions/CMakeLists.txt +++ b/interfaces/ros1/wavemap_ros_conversions/CMakeLists.txt @@ -3,13 +3,13 @@ project(wavemap_ros_conversions) # Find dependencies find_package(catkin REQUIRED - COMPONENTS roscpp eigen_conversions visualization_msgs wavemap wavemap_msgs) + COMPONENTS roscpp eigen_conversions sensor_msgs visualization_msgs wavemap wavemap_msgs) # Register catkin package catkin_package( INCLUDE_DIRS include LIBRARIES ${PROJECT_NAME} - CATKIN_DEPENDS roscpp eigen_conversions visualization_msgs wavemap wavemap_msgs) + CATKIN_DEPENDS roscpp eigen_conversions sensor_msgs visualization_msgs wavemap wavemap_msgs) # Libraries add_library(${PROJECT_NAME} diff --git a/interfaces/ros1/wavemap_ros_conversions/include/wavemap_ros_conversions/descriptor_layer_observation_conversions.h b/interfaces/ros1/wavemap_ros_conversions/include/wavemap_ros_conversions/descriptor_layer_observation_conversions.h new file mode 100644 index 000000000..085adeaba --- /dev/null +++ b/interfaces/ros1/wavemap_ros_conversions/include/wavemap_ros_conversions/descriptor_layer_observation_conversions.h @@ -0,0 +1,169 @@ +#ifndef WAVEMAP_ROS_CONVERSIONS_DESCRIPTOR_LAYER_OBSERVATION_CONVERSIONS_H_ +#define WAVEMAP_ROS_CONVERSIONS_DESCRIPTOR_LAYER_OBSERVATION_CONVERSIONS_H_ + +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +namespace wavemap::convert { +namespace detail { + +template +bool layerPayloadIsValid(const wavemap_msgs::Layer& layer, + size_t observation_count) { + return layer.type == LayerRosCodec::typeName() && + LayerRosCodec::payloadIsValid(layer, observation_count); +} + +template +bool integrateLayer( + const wavemap_msgs::Layer& layer, + const std::vector& positions, PipelineT& pipeline, + bool apply, std::string* error_message) { + using Value = layered::LayerValueT; + if (!layerPayloadIsValid(layer, positions.size())) { + if (error_message) { + *error_message = "Layer '" + layer.name + + "' has the wrong type or value count."; + } + return false; + } + + if (!apply) { + return true; + } + + std::vector> observations; + observations.reserve(positions.size()); + for (size_t index = 0u; index < positions.size(); ++index) { + observations.emplace_back( + positions[index], LayerRosCodec::read(layer, index)); + } + const auto result = pipeline.template integrate(observations); + if (result.rejected != 0u) { + if (error_message) { + *error_message = "Layer '" + layer.name + + "' contains an invalid spatial observation."; + } + return false; + } + return true; +} + +template +bool integrateBundle( + const wavemap_msgs::LayeredMapUpdate& msg, + const std::vector& positions, PipelineT& pipeline, + std::vector& consumed, bool apply, std::string* error_message) { + bool success = true; + const auto descriptors = layered::layerDescriptors(); + if constexpr (std::tuple_size_v == 0u) { + (void)msg; + (void)positions; + (void)pipeline; + (void)consumed; + (void)apply; + (void)error_message; + } else { + std::apply( + [&](const auto&... descriptor) { + auto process_descriptor = [&](const auto& current_descriptor) { + using Descriptor = std::decay_t; + using LayerTag = typename Descriptor::LayerTag; + size_t match_count = 0u; + size_t match_index = 0u; + for (size_t index = 0u; index < msg.layers.size(); ++index) { + if (msg.layers[index].name == current_descriptor.name()) { + ++match_count; + match_index = index; + } + } + if (match_count > 1u) { + success = false; + if (error_message) { + *error_message = "Layer '" + + std::string(current_descriptor.name()) + + "' occurs more than once."; + } + return; + } + if (match_count == 1u) { + consumed[match_index] = true; + success = success && + integrateLayer( + msg.layers[match_index], positions, pipeline, + apply, error_message); + } + }; + (process_descriptor(descriptor), ...); + }, + descriptors); + } + return success; +} + +} // namespace detail + +template +struct DescriptorLayerObservationRosConverter { + using Pipeline = layered::LayeredPipeline; + + static bool integrate(const wavemap_msgs::LayeredMapUpdate& msg, + Pipeline& pipeline, + std::string* error_message = nullptr) { + if (msg.positions.empty() || msg.layers.empty()) { + if (error_message) { + *error_message = + "A layered map update requires positions and at least one layer."; + } + return false; + } + + std::vector positions; + positions.reserve(msg.positions.size()); + for (const auto& position_msg : msg.positions) { + const Point3D position = point32MsgToPoint3D(position_msg); + if (!position.array().isFinite().all()) { + if (error_message) { + *error_message = "A layered map update position is not finite."; + } + return false; + } + positions.emplace_back(position); + } + + std::vector consumed(msg.layers.size(), false); + if (!detail::integrateBundle( + msg, positions, pipeline, consumed, false, error_message) || + !detail::integrateBundle( + msg, positions, pipeline, consumed, false, error_message)) { + return false; + } + + const auto unknown = std::find(consumed.cbegin(), consumed.cend(), false); + if (unknown != consumed.cend()) { + const size_t index = std::distance(consumed.cbegin(), unknown); + if (error_message) { + *error_message = "Unknown layer '" + msg.layers[index].name + "'."; + } + return false; + } + + std::fill(consumed.begin(), consumed.end(), false); + return detail::integrateBundle( + msg, positions, pipeline, consumed, true, error_message) && + detail::integrateBundle( + msg, positions, pipeline, consumed, true, error_message); + } +}; + +} // namespace wavemap::convert + +#endif // WAVEMAP_ROS_CONVERSIONS_DESCRIPTOR_LAYER_OBSERVATION_CONVERSIONS_H_ diff --git a/interfaces/ros1/wavemap_ros_conversions/include/wavemap_ros_conversions/descriptor_layered_map_conversions.h b/interfaces/ros1/wavemap_ros_conversions/include/wavemap_ros_conversions/descriptor_layered_map_conversions.h new file mode 100644 index 000000000..9ae4c6422 --- /dev/null +++ b/interfaces/ros1/wavemap_ros_conversions/include/wavemap_ros_conversions/descriptor_layered_map_conversions.h @@ -0,0 +1,308 @@ +#ifndef WAVEMAP_ROS_CONVERSIONS_DESCRIPTOR_LAYERED_MAP_CONVERSIONS_H_ +#define WAVEMAP_ROS_CONVERSIONS_DESCRIPTOR_LAYERED_MAP_CONVERSIONS_H_ + +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +namespace wavemap::convert { + +template +struct LayerRosCodec; + +namespace detail { +template +struct HasLayerRosCodec : std::false_type {}; + +template +struct HasLayerRosCodec< + ValueT, + std::void_t::typeName()), + decltype(LayerRosCodec::append( + std::declval(), + std::declval())), + decltype(LayerRosCodec::read( + std::declval(), size_t{})), + decltype(LayerRosCodec::payloadIsValid( + std::declval(), size_t{}))>> + : std::true_type {}; +} // namespace detail + +template +inline constexpr bool kHasLayerRosCodec = + detail::HasLayerRosCodec::value; + +template <> +struct LayerRosCodec { + static constexpr const char* typeName() { return "float32"; } + + static void append(const FloatingPoint& value, wavemap_msgs::Layer& layer) { + layer.float32_values.emplace_back(value); + } + + static FloatingPoint read(const wavemap_msgs::Layer& layer, + size_t value_index) { + return layer.float32_values[value_index]; + } + + static bool payloadIsValid(const wavemap_msgs::Layer& layer, + size_t value_count) { + return layer.float32_values.size() == value_count && + layer.int32_values.empty() && layer.uint8_values.empty(); + } +}; + +template <> +struct LayerRosCodec { + static constexpr const char* typeName() { return "float32_rgb"; } + + static void append(const layered::Rgb& value, + wavemap_msgs::Layer& layer) { + layer.float32_values.emplace_back(value.r); + layer.float32_values.emplace_back(value.g); + layer.float32_values.emplace_back(value.b); + } + + static layered::Rgb read(const wavemap_msgs::Layer& layer, + size_t value_index) { + const size_t first_component = 3u * value_index; + return {layer.float32_values[first_component], + layer.float32_values[first_component + 1u], + layer.float32_values[first_component + 2u]}; + } + + static bool payloadIsValid(const wavemap_msgs::Layer& layer, + size_t value_count) { + return layer.float32_values.size() == 3u * value_count && + layer.int32_values.empty() && layer.uint8_values.empty(); + } +}; + +template <> +struct LayerRosCodec { + static constexpr const char* typeName() { return "int32"; } + + static void append(int value, wavemap_msgs::Layer& layer) { + layer.int32_values.emplace_back(value); + } + + static int read(const wavemap_msgs::Layer& layer, size_t value_index) { + return layer.int32_values[value_index]; + } + + static bool payloadIsValid(const wavemap_msgs::Layer& layer, + size_t value_count) { + return layer.int32_values.size() == value_count && + layer.float32_values.empty() && layer.uint8_values.empty(); + } +}; + +template +struct DescriptorContinuousRosConverter { + using ContinuousLayers = typename VoxelT::AdditionalData; + + static_assert( + []() constexpr { + bool supported = true; + const auto descriptors = layered::layerDescriptors(); + std::apply( + [&](const auto&... descriptor) { + ((supported = supported && + kHasLayerRosCodec::LayerTag>>), + ...); + }, + descriptors); + return supported; + }(), + "Every continuous layer value published through ROS needs a " + "LayerRosCodec specialization."); + + static std::vector layerNames() { + std::vector names; + const auto descriptors = layered::layerDescriptors(); + names.reserve(std::tuple_size_v); + std::apply( + [&](const auto&... descriptor) { + (names.emplace_back(descriptor.name()), ...); + }, + descriptors); + return names; + } + + static std::vector layerTypes() { + std::vector types; + const auto descriptors = layered::layerDescriptors(); + types.reserve(std::tuple_size_v); + std::apply( + [&](const auto&... descriptor) { + (types.emplace_back( + LayerRosCodec::LayerTag>>:: + typeName()), + ...); + }, + descriptors); + return types; + } + + static std::vector layerVisualizations() { + std::vector visualizations; + const auto descriptors = layered::layerDescriptors(); + visualizations.reserve(std::tuple_size_v); + std::apply( + [&](const auto&... descriptor) { + ([&]() { + using LayerTag = + typename std::decay_t::LayerTag; + auto& msg = visualizations.emplace_back(); + msg.name = descriptor.name(); + if (const auto visualization = + layered::scalarLayerVisualization()) { + msg.has_low_color = true; + msg.low_r = visualization->low.r; + msg.low_g = visualization->low.g; + msg.low_b = visualization->low.b; + msg.has_high_color = true; + msg.high_r = visualization->high.r; + msg.high_g = visualization->high.g; + msg.high_b = visualization->high.b; + } + }(), ...); + }, descriptors); + return visualizations; + + } + static std::vector makeLayers() { + std::vector layers; + const auto names = layerNames(); + const auto types = layerTypes(); + layers.resize(names.size()); + for (size_t layer_index = 0u; layer_index < layers.size(); ++layer_index) { + layers[layer_index].name = names[layer_index]; + layers[layer_index].type = types[layer_index]; + } + return layers; + } + + static std::vector makeLayerMinimums( + const typename CellDataTraits::ThresholdConfig& config) { + return makeLayerBounds(config, true); + } + + static std::vector makeLayerMaximums( + const typename CellDataTraits::ThresholdConfig& config) { + return makeLayerBounds(config, false); + } + + static void appendLayerValues(const VoxelT& voxel, + std::vector& layers) { + size_t layer_index = 0u; + layered::forEachLayerDescriptor( + voxel.data, [&](const auto& descriptor, const auto& state) { + using LayerTag = typename std::decay_t::LayerTag; + using Value = layered::LayerValueT; + LayerRosCodec::append( + layered::LayerStateConversion::value(state), + layers[layer_index++]); + }); + } + + static VoxelT readCellData( + FloatingPoint occupancy, const std::vector& layers, + size_t value_index) { + VoxelT voxel; + voxel.occupancy = occupancy; + size_t layer_index = 0u; + layered::forEachLayerDescriptor( + voxel.data, [&](const auto& descriptor, auto& state) { + using LayerTag = typename std::decay_t::LayerTag; + using Value = layered::LayerValueT; + state = layered::LayerStateConversion::initialState( + LayerRosCodec::read(layers[layer_index++], value_index)); + }); + return voxel; + } + + private: + static std::vector makeLayerBounds( + const typename CellDataTraits::ThresholdConfig& config, + bool minimum) { + auto layers = makeLayers(); + size_t layer_index = 0u; + const auto descriptors = layered::layerDescriptors(); + std::apply( + [&](const auto&... descriptor) { + (appendLayerBound(descriptor, config, minimum, + layers[layer_index++]), + ...); + }, + descriptors); + return layers; + } + + template + static void appendLayerBound( + const DescriptorT& /*descriptor*/, + const typename CellDataTraits::ThresholdConfig& config, + bool minimum, wavemap_msgs::Layer& layer) { + using LayerTag = typename DescriptorT::LayerTag; + const auto& bounds = config.data.template get(); + using Value = layered::LayerValueT; + LayerRosCodec::append(minimum ? bounds.min : bounds.max, layer); + } +}; + +template +auto descriptorNamedDiscreteLayersForRos(DiscreteLayersT& layers) { + return layered::transformLayerDescriptors( + layered::layerDescriptors(), + [&](const auto& descriptor) { + using LayerTag = typename std::decay_t::LayerTag; + return namedDiscreteLayerForRos(std::string(descriptor.name()), + descriptor.value(layers), + layered::integerCategoryMetadata()); + }); +} + +template +auto descriptorNamedDiscreteLayersForRos(const DiscreteLayersT& layers) { + return layered::transformLayerDescriptors( + layered::layerDescriptors(), + [&](const auto& descriptor) { + using LayerTag = typename std::decay_t::LayerTag; + return namedDiscreteLayerForRos(std::string(descriptor.name()), + descriptor.value(layers), + layered::integerCategoryMetadata()); + }); +} + +// Schema-generated discrete bundles already carry their authoritative names +// and value types in their descriptors. They therefore need no user-written +// ROS serialization registration. +template +struct DiscreteLayerBundleRosTraits>> { + using Bundle = layered::schema::DiscreteLayerBundle< + layered::schema::LayerSchema>; + + static auto layers(Bundle& layers) { + return descriptorNamedDiscreteLayersForRos(layers); + } + + static auto layers(const Bundle& layers) { + return descriptorNamedDiscreteLayersForRos(layers); + } +}; + +} // namespace wavemap::convert + +#endif // WAVEMAP_ROS_CONVERSIONS_DESCRIPTOR_LAYERED_MAP_CONVERSIONS_H_ diff --git a/interfaces/ros1/wavemap_ros_conversions/include/wavemap_ros_conversions/impl/layered_map_msg_conversions_inl.h b/interfaces/ros1/wavemap_ros_conversions/include/wavemap_ros_conversions/impl/layered_map_msg_conversions_inl.h index d06474f69..a35d205c1 100644 --- a/interfaces/ros1/wavemap_ros_conversions/include/wavemap_ros_conversions/impl/layered_map_msg_conversions_inl.h +++ b/interfaces/ros1/wavemap_ros_conversions/include/wavemap_ros_conversions/impl/layered_map_msg_conversions_inl.h @@ -37,6 +37,12 @@ void mapToRosMsg(const HashedWaveletOctreeT& map, wavemap_msgs::Layer msg.tree_height = map.getTreeHeight(); msg.layer_names = CellDataRosConverterT::layerNames(); msg.layer_types = CellDataRosConverterT::layerTypes(); + msg.layer_min_values = + CellDataRosConverterT::makeLayerMinimums(map.getThresholdConfig()); + msg.layer_max_values = + CellDataRosConverterT::makeLayerMaximums(map.getThresholdConfig()); + msg.layer_visualizations = + CellDataRosConverterT::layerVisualizations(); // Always publish the full list of allocated block indices msg.allocated_block_indices.reserve(map.getHashMap().size()); diff --git a/interfaces/ros1/wavemap_ros_conversions/include/wavemap_ros_conversions/layer_observation_msg_conversions.h b/interfaces/ros1/wavemap_ros_conversions/include/wavemap_ros_conversions/layer_observation_msg_conversions.h new file mode 100644 index 000000000..c61d62edd --- /dev/null +++ b/interfaces/ros1/wavemap_ros_conversions/include/wavemap_ros_conversions/layer_observation_msg_conversions.h @@ -0,0 +1,24 @@ +#ifndef WAVEMAP_ROS_CONVERSIONS_LAYER_OBSERVATION_MSG_CONVERSIONS_H_ +#define WAVEMAP_ROS_CONVERSIONS_LAYER_OBSERVATION_MSG_CONVERSIONS_H_ + +#include +#include + +#include +#include + +namespace wavemap::convert { + +template +struct RosLayerObservationBatch { + std_msgs::Header header; + std::vector> observations; + size_t rejected = 0u; + std::string error; + + explicit operator bool() const { return error.empty(); } +}; + +} // namespace wavemap::convert + +#endif // WAVEMAP_ROS_CONVERSIONS_LAYER_OBSERVATION_MSG_CONVERSIONS_H_ diff --git a/interfaces/ros1/wavemap_ros_conversions/include/wavemap_ros_conversions/layered_map_msg_conversions.h b/interfaces/ros1/wavemap_ros_conversions/include/wavemap_ros_conversions/layered_map_msg_conversions.h index 98ec6603d..eb0151e63 100644 --- a/interfaces/ros1/wavemap_ros_conversions/include/wavemap_ros_conversions/layered_map_msg_conversions.h +++ b/interfaces/ros1/wavemap_ros_conversions/include/wavemap_ros_conversions/layered_map_msg_conversions.h @@ -9,9 +9,11 @@ #include #include +#include #include #include #include +#include #include #include #include @@ -40,12 +42,32 @@ template struct NamedDiscreteLayerForRos { std::string name; LayerT& layer; + std::vector categories; }; template NamedDiscreteLayerForRos namedDiscreteLayerForRos( - std::string name, LayerT& layer) { - return {std::move(name), layer}; + std::string name, LayerT& layer, + std::vector categories = {}) { + return {std::move(name), layer, std::move(categories)}; +} + +inline void setDiscreteCategoryLabels( + const std::vector& categories, + wavemap_msgs::DiscreteLayer& msg) { + msg.categories.clear(); + msg.categories.reserve(categories.size()); + for (const auto& metadata : categories) { + auto& category = msg.categories.emplace_back(); + category.value = metadata.value; + category.label = metadata.label; + category.has_color = metadata.color.has_value(); + if (metadata.color) { + category.r = metadata.color->r; + category.g = metadata.color->g; + category.b = metadata.color->b; + } + } } template @@ -137,36 +159,70 @@ struct DiscreteRosValueConverter { } }; +template +void discreteCellToRosMsg( + const typename DiscreteLayerT::Cell& cell, + const IndexKeyT& parent_key, + wavemap_msgs::DiscreteLayerCell& cell_msg) { + using ValueT = typename DiscreteLayerT::Value; + cell_msg.parent_index = detail::indexKeyToRosMsg(parent_key); + DiscreteRosValueConverter::setDominantValue( + cell_msg, cell.dominant_value); + + cell_msg.observed_offsets.reserve(cell.observed_offsets.size()); + for (const int offset : cell.observed_offsets) { + cell_msg.observed_offsets.emplace_back(offset); + } + cell_msg.exception_offsets.reserve(cell.exceptions.size()); + for (const auto& [offset, value] : cell.exceptions) { + cell_msg.exception_offsets.emplace_back(offset); + DiscreteRosValueConverter::appendExceptionValue(cell_msg, value); + } +} + template void discreteLayerToRosMsg(const std::string& name, const DiscreteLayerT& layer, - wavemap_msgs::DiscreteLayer& msg) { + wavemap_msgs::DiscreteLayer& msg, + const std::vector& + categories = {}) { using ValueT = typename DiscreteLayerT::Value; msg.name = name; msg.value_type = DiscreteRosValueConverter::typeName(); msg.block_height = layer.config().block_height; + setDiscreteCategoryLabels(categories, msg); msg.cells.clear(); msg.cells.reserve(layer.cells().size()); for (const auto& [parent_key, cell] : layer.cells()) { wavemap_msgs::DiscreteLayerCell cell_msg; - cell_msg.parent_index = detail::indexKeyToRosMsg(parent_key); - DiscreteRosValueConverter::setDominantValue( - cell_msg, cell.dominant_value); - - cell_msg.observed_offsets.reserve(cell.observed_offsets.size()); - for (const int offset : cell.observed_offsets) { - cell_msg.observed_offsets.emplace_back(offset); - } + discreteCellToRosMsg(cell, parent_key, cell_msg); + msg.cells.emplace_back(std::move(cell_msg)); + } +} - cell_msg.exception_offsets.reserve(cell.exceptions.size()); - for (const auto& [offset, value] : cell.exceptions) { - cell_msg.exception_offsets.emplace_back(offset); - DiscreteRosValueConverter::appendExceptionValue(cell_msg, value); +template +void discreteLayerPatchToRosMsg(const std::string& name, + const DiscreteLayerT& layer, + wavemap_msgs::DiscreteLayer& msg, + const std::vector& + categories = {}) { + using ValueT = typename DiscreteLayerT::Value; + msg.name = name; + msg.value_type = DiscreteRosValueConverter::typeName(); + msg.block_height = layer.config().block_height; + setDiscreteCategoryLabels(categories, msg); + for (const auto& parent_key : layer.dirtyParentKeys()) { + const auto cell_it = layer.cells().find(parent_key); + if (cell_it == layer.cells().end()) { + msg.deleted_parent_indices.emplace_back( + detail::indexKeyToRosMsg(parent_key)); + } else { + auto& cell_msg = msg.cells.emplace_back(); + discreteCellToRosMsg(cell_it->second, parent_key, + cell_msg); } - - msg.cells.emplace_back(std::move(cell_msg)); } } @@ -245,12 +301,54 @@ bool discreteLayerBundleToRosMsg(const DiscreteLayersT& layers, return forEachRosTupleElement(named_layers, [&](const auto& named_layer) { wavemap_msgs::DiscreteLayer layer_msg; - discreteLayerToRosMsg(named_layer.name, named_layer.layer, layer_msg); + discreteLayerToRosMsg(named_layer.name, named_layer.layer, layer_msg, + named_layer.categories); msg.emplace_back(std::move(layer_msg)); return true; }); } +template +bool discreteLayerBundlePatchToRosMsg( + const DiscreteLayersT& layers, + std::vector& msg) { + const auto named_layers = + DiscreteLayerBundleRosTraits::layers(layers); + msg.clear(); + return forEachRosTupleElement(named_layers, [&](const auto& named_layer) { + if (named_layer.layer.dirtyParentKeys().empty()) { + return true; + } + auto& layer_msg = msg.emplace_back(); + discreteLayerPatchToRosMsg(named_layer.name, named_layer.layer, layer_msg, + named_layer.categories); + return true; + }); +} + +template +void clearDiscreteLayerBundleDirtyState(DiscreteLayersT& layers) { + const auto named_layers = + DiscreteLayerBundleRosTraits::layers(layers); + forEachRosTupleElement(named_layers, [](auto named_layer) { + named_layer.layer.clearDirtyParentKeys(); + return true; + }); +} + +template +bool discreteLayerBundleHasDirtyState(const DiscreteLayersT& layers) { + const auto named_layers = + DiscreteLayerBundleRosTraits::layers(layers); + bool has_dirty_state = false; + forEachRosTupleElement(named_layers, [&](const auto& named_layer) { + has_dirty_state = has_dirty_state || + !named_layer.layer.dirtyParentKeys().empty(); + return true; + }); + return has_dirty_state; +} + template bool rosMsgToDiscreteLayerBundle( const std::vector& msg, @@ -276,6 +374,7 @@ bool layeredMapToRosMsg(const LayeredMapT& map, const std::string& frame_id, wavemap_msgs::LayeredMap& msg) { msg.header.frame_id = frame_id; msg.header.stamp = stamp; + msg.is_full_update = true; if (!mapToRosMsg(map.continuousMap(), frame_id, stamp, msg.continuous_map)) { @@ -285,6 +384,25 @@ bool layeredMapToRosMsg(const LayeredMapT& map, const std::string& frame_id, msg.discrete_layers); } +template +bool layeredMapPatchToRosMsg( + const LayeredMapT& map, const std::string& frame_id, + const ros::Time& stamp, wavemap_msgs::LayeredMap& msg, + std::optional> include_blocks, + std::shared_ptr thread_pool = nullptr) { + msg.header.frame_id = frame_id; + msg.header.stamp = stamp; + msg.is_full_update = false; + if (!mapToRosMsg( + map.continuousMap(), frame_id, stamp, msg.continuous_map, + std::move(include_blocks), std::move(thread_pool))) { + return false; + } + return discreteLayerBundlePatchToRosMsg(map.discreteLayers(), + msg.discrete_layers); +} + template bool rosMsgToLayeredMap(const wavemap_msgs::LayeredMap& msg, LayeredMapT& map) { diff --git a/interfaces/ros1/wavemap_ros_conversions/include/wavemap_ros_conversions/pointcloud_layer_observation_conversions.h b/interfaces/ros1/wavemap_ros_conversions/include/wavemap_ros_conversions/pointcloud_layer_observation_conversions.h new file mode 100644 index 000000000..391d35df5 --- /dev/null +++ b/interfaces/ros1/wavemap_ros_conversions/include/wavemap_ros_conversions/pointcloud_layer_observation_conversions.h @@ -0,0 +1,218 @@ +#ifndef WAVEMAP_ROS_CONVERSIONS_POINTCLOUD_LAYER_OBSERVATION_CONVERSIONS_H_ +#define WAVEMAP_ROS_CONVERSIONS_POINTCLOUD_LAYER_OBSERVATION_CONVERSIONS_H_ + +#include +#include +#include +#include +#include +#include + +#include +#include + +#include +#include + +namespace wavemap::convert { + +struct ReflectivityObservationConfig { + std::string field_name = "reflectivity"; + FloatingPoint scale = 1.f / 255.f; + FloatingPoint offset = 0.f; + FloatingPoint min_value = 0.f; + FloatingPoint max_value = 1.f; + size_t point_stride = 1u; +}; +namespace detail { + +inline const sensor_msgs::PointField* findField( + const sensor_msgs::PointCloud2& msg, const std::string& name) { + const auto it = std::find_if( + msg.fields.cbegin(), msg.fields.cend(), + [&](const auto& field) { return field.name == name; }); + return it == msg.fields.cend() ? nullptr : &*it; +} + +template +bool readPointField(const sensor_msgs::PointCloud2& msg, + const sensor_msgs::PointField& field, + size_t point_offset, FloatingPoint& value) { + if (point_offset + field.offset + sizeof(T) > msg.data.size()) { + return false; + } + T stored{}; + std::memcpy(&stored, msg.data.data() + point_offset + field.offset, + sizeof(T)); + value = static_cast(stored); + return true; +} + +inline bool readNumericPointField( + const sensor_msgs::PointCloud2& msg, + const sensor_msgs::PointField& field, size_t point_offset, + FloatingPoint& value) { + if (field.count != 1u) { + return false; + } + switch (field.datatype) { + case sensor_msgs::PointField::INT8: + return readPointField(msg, field, point_offset, value); + case sensor_msgs::PointField::UINT8: + return readPointField(msg, field, point_offset, value); + case sensor_msgs::PointField::INT16: + return readPointField(msg, field, point_offset, value); + case sensor_msgs::PointField::UINT16: + return readPointField(msg, field, point_offset, value); + case sensor_msgs::PointField::INT32: + return readPointField(msg, field, point_offset, value); + case sensor_msgs::PointField::UINT32: + return readPointField(msg, field, point_offset, value); + case sensor_msgs::PointField::FLOAT32: + return readPointField(msg, field, point_offset, value); + case sensor_msgs::PointField::FLOAT64: + return readPointField(msg, field, point_offset, value); + default: + return false; + } +} + +} // namespace detail + +template +struct RosReflectivityEndpointPointcloud { + std_msgs::Header header; + layered::EndpointAttributePointcloud pointcloud; + size_t rejected = 0u; + std::string error; + + explicit operator bool() const { return error.empty(); } +}; + +template +RosReflectivityEndpointPointcloud +pointcloudToReflectivityEndpointPointcloud( + const sensor_msgs::PointCloud2& msg, + const ReflectivityObservationConfig& config = {}) { + static_assert( + std::is_same_v, + FloatingPoint>, + "The reflectivity endpoint layer must store FloatingPoint values."); + RosReflectivityEndpointPointcloud result; + result.header = msg.header; + if (msg.is_bigendian) { + result.error = "Big-endian PointCloud2 messages are not supported."; + return result; + } + if (config.point_stride == 0u || msg.point_step == 0u || msg.width == 0u) { + result.error = + "Point stride, PointCloud2 point_step, and width must be nonzero."; + return result; + } + + const auto* x = detail::findField(msg, "x"); + const auto* y = detail::findField(msg, "y"); + const auto* z = detail::findField(msg, "z"); + const auto* reflectivity = detail::findField(msg, config.field_name); + if (!x || !y || !z || !reflectivity) { + result.error = "PointCloud2 is missing x, y, z, or reflectivity."; + return result; + } + + const size_t point_count = + static_cast(msg.width) * static_cast(msg.height); + const size_t capacity = + (point_count + config.point_stride - 1u) / config.point_stride; + result.pointcloud.resize(capacity); + size_t accepted = 0u; + for (size_t index = 0u; index < point_count; + index += config.point_stride) { + const size_t row = index / msg.width; + const size_t column = index % msg.width; + const size_t point_offset = + row * msg.row_step + column * msg.point_step; + FloatingPoint px = 0.f; + FloatingPoint py = 0.f; + FloatingPoint pz = 0.f; + FloatingPoint raw_reflectivity = 0.f; + if (!detail::readNumericPointField(msg, *x, point_offset, px) || + !detail::readNumericPointField(msg, *y, point_offset, py) || + !detail::readNumericPointField(msg, *z, point_offset, pz) || + !detail::readNumericPointField( + msg, *reflectivity, point_offset, raw_reflectivity) || + !std::isfinite(px) || !std::isfinite(py) || !std::isfinite(pz) || + !std::isfinite(raw_reflectivity)) { + ++result.rejected; + continue; + } + + result.pointcloud.point(accepted) = Point3D(px, py, pz); + result.pointcloud.template attribute(accepted) = + std::clamp(config.offset + config.scale * raw_reflectivity, + config.min_value, config.max_value); + ++accepted; + } + result.pointcloud.truncate(accepted); + return result; +} + +inline RosLayerObservationBatch +pointcloudToReflectivityObservations( + const sensor_msgs::PointCloud2& msg, + const ReflectivityObservationConfig& config = {}) { + RosLayerObservationBatch result; + result.header = msg.header; + if (msg.is_bigendian) { + result.error = "Big-endian PointCloud2 messages are not supported."; + return result; + } + if (config.point_stride == 0u || msg.point_step == 0u) { + result.error = "Point stride and PointCloud2 point_step must be nonzero."; + return result; + } + + const auto* x = detail::findField(msg, "x"); + const auto* y = detail::findField(msg, "y"); + const auto* z = detail::findField(msg, "z"); + const auto* reflectivity = detail::findField(msg, config.field_name); + if (!x || !y || !z || !reflectivity) { + result.error = "PointCloud2 is missing x, y, z, or reflectivity."; + return result; + } + + const size_t point_count = + static_cast(msg.width) * static_cast(msg.height); + result.observations.reserve( + (point_count + config.point_stride - 1u) / config.point_stride); + for (size_t index = 0u; index < point_count; + index += config.point_stride) { + const size_t row = index / msg.width; + const size_t column = index % msg.width; + const size_t point_offset = + row * msg.row_step + column * msg.point_step; + FloatingPoint px = 0.f; + FloatingPoint py = 0.f; + FloatingPoint pz = 0.f; + FloatingPoint raw_reflectivity = 0.f; + if (!detail::readNumericPointField(msg, *x, point_offset, px) || + !detail::readNumericPointField(msg, *y, point_offset, py) || + !detail::readNumericPointField(msg, *z, point_offset, pz) || + !detail::readNumericPointField( + msg, *reflectivity, point_offset, raw_reflectivity) || + !std::isfinite(px) || !std::isfinite(py) || !std::isfinite(pz) || + !std::isfinite(raw_reflectivity)) { + ++result.rejected; + continue; + } + + const FloatingPoint value = std::clamp( + config.offset + config.scale * raw_reflectivity, + config.min_value, config.max_value); + result.observations.emplace_back(Point3D(px, py, pz), value); + } + return result; +} + +} // namespace wavemap::convert + +#endif // WAVEMAP_ROS_CONVERSIONS_POINTCLOUD_LAYER_OBSERVATION_CONVERSIONS_H_ diff --git a/interfaces/ros1/wavemap_ros_conversions/include/wavemap_ros_conversions/rgbd_layer_observation_conversions.h b/interfaces/ros1/wavemap_ros_conversions/include/wavemap_ros_conversions/rgbd_layer_observation_conversions.h new file mode 100644 index 000000000..b0b9d155c --- /dev/null +++ b/interfaces/ros1/wavemap_ros_conversions/include/wavemap_ros_conversions/rgbd_layer_observation_conversions.h @@ -0,0 +1,124 @@ +#ifndef WAVEMAP_ROS_CONVERSIONS_RGBD_LAYER_OBSERVATION_CONVERSIONS_H_ +#define WAVEMAP_ROS_CONVERSIONS_RGBD_LAYER_OBSERVATION_CONVERSIONS_H_ + +#include +#include +#include +#include + +#include +#include +#include +#include + +#include + +namespace wavemap::convert { + +struct RgbdColorObservationConfig { + FloatingPoint depth_scale_16uc1 = 1e-3f; + FloatingPoint min_depth = 0.1f; + FloatingPoint max_depth = 20.f; + size_t pixel_stride = 1u; +}; + +inline RosLayerObservationBatch +rgbdToColorObservations( + const sensor_msgs::Image& color, const sensor_msgs::Image& aligned_depth, + const sensor_msgs::CameraInfo& camera_info, + const RgbdColorObservationConfig& config = {}) { + RosLayerObservationBatch result; + result.header = color.header; + + const bool rgb = color.encoding == sensor_msgs::image_encodings::RGB8; + const bool bgr = color.encoding == sensor_msgs::image_encodings::BGR8; + const bool depth_u16 = + aligned_depth.encoding == sensor_msgs::image_encodings::TYPE_16UC1; + const bool depth_f32 = + aligned_depth.encoding == sensor_msgs::image_encodings::TYPE_32FC1; + if ((!rgb && !bgr) || (!depth_u16 && !depth_f32)) { + result.error = + "Expected rgb8/bgr8 color and 16UC1/32FC1 aligned depth."; + return result; + } + if (color.is_bigendian || aligned_depth.is_bigendian) { + result.error = "Big-endian RGB-D images are not supported."; + return result; + } + if (config.pixel_stride == 0u || color.width != aligned_depth.width || + color.height != aligned_depth.height || color.width == 0u || + color.height == 0u) { + result.error = "RGB and aligned depth dimensions must match and be nonzero."; + return result; + } + if ((!aligned_depth.header.frame_id.empty() && + aligned_depth.header.frame_id != color.header.frame_id) || + (!camera_info.header.frame_id.empty() && + camera_info.header.frame_id != color.header.frame_id)) { + result.error = "RGB, aligned depth, and CameraInfo frames must match."; + return result; + } + if (color.step < 3u * color.width || + aligned_depth.step < + (depth_u16 ? 2u : 4u) * aligned_depth.width || + color.data.size() < + static_cast(color.step) * color.height || + aligned_depth.data.size() < + static_cast(aligned_depth.step) * aligned_depth.height) { + result.error = "RGB-D image payload is smaller than its declared layout."; + return result; + } + + const FloatingPoint fx = camera_info.K[0]; + const FloatingPoint fy = camera_info.K[4]; + const FloatingPoint cx = camera_info.K[2]; + const FloatingPoint cy = camera_info.K[5]; + if (!(0.f < fx) || !(0.f < fy)) { + result.error = "CameraInfo contains invalid focal lengths."; + return result; + } + + const size_t sampled_width = + (color.width + config.pixel_stride - 1u) / config.pixel_stride; + const size_t sampled_height = + (color.height + config.pixel_stride - 1u) / config.pixel_stride; + result.observations.reserve(sampled_width * sampled_height); + + for (size_t v = 0u; v < color.height; v += config.pixel_stride) { + for (size_t u = 0u; u < color.width; u += config.pixel_stride) { + const size_t depth_offset = v * aligned_depth.step + + u * (depth_u16 ? 2u : 4u); + FloatingPoint depth = 0.f; + if (depth_u16) { + uint16_t raw_depth = 0u; + std::memcpy(&raw_depth, aligned_depth.data.data() + depth_offset, + sizeof(raw_depth)); + depth = config.depth_scale_16uc1 * raw_depth; + } else { + std::memcpy(&depth, aligned_depth.data.data() + depth_offset, + sizeof(depth)); + } + if (!std::isfinite(depth) || depth < config.min_depth || + config.max_depth < depth) { + ++result.rejected; + continue; + } + + const size_t color_offset = v * color.step + 3u * u; + const auto* channels = color.data.data() + color_offset; + const FloatingPoint red = (rgb ? channels[0] : channels[2]) / 255.f; + const FloatingPoint green = channels[1] / 255.f; + const FloatingPoint blue = (rgb ? channels[2] : channels[0]) / 255.f; + const Point3D position((static_cast(u) - cx) * depth / fx, + (static_cast(v) - cy) * depth / fy, + depth); + result.observations.emplace_back( + position, layered::Rgb{red, green, blue}); + } + } + return result; +} + +} // namespace wavemap::convert + +#endif // WAVEMAP_ROS_CONVERSIONS_RGBD_LAYER_OBSERVATION_CONVERSIONS_H_ diff --git a/interfaces/ros1/wavemap_ros_conversions/package.xml b/interfaces/ros1/wavemap_ros_conversions/package.xml index 55c6b219c..a7e8ea250 100644 --- a/interfaces/ros1/wavemap_ros_conversions/package.xml +++ b/interfaces/ros1/wavemap_ros_conversions/package.xml @@ -16,6 +16,7 @@ wavemap_msgs roscpp + sensor_msgs eigen_conversions visualization_msgs diff --git a/interfaces/ros1/wavemap_rviz_plugin/CMakeLists.txt b/interfaces/ros1/wavemap_rviz_plugin/CMakeLists.txt index 3b97727d8..335306b9b 100644 --- a/interfaces/ros1/wavemap_rviz_plugin/CMakeLists.txt +++ b/interfaces/ros1/wavemap_rviz_plugin/CMakeLists.txt @@ -7,16 +7,17 @@ project(wavemap_rviz_plugin) # Dependencies find_package(catkin REQUIRED - COMPONENTS rviz wavemap wavemap_msgs wavemap_ros_conversions) + COMPONENTS pluginlib rviz wavemap wavemap_msgs wavemap_ros_conversions) # Setup catkin package catkin_package(CATKIN_DEPENDS - wavemap wavemap_msgs wavemap_ros_conversions) + pluginlib wavemap wavemap_msgs wavemap_ros_conversions) # For all targets include_directories( include ${CMAKE_CURRENT_SOURCE_DIR}/../../../examples/cpp/digital_twin/common + ${CMAKE_CURRENT_SOURCE_DIR}/../../../examples/cpp/digital_twin/ouster_mapping SYSTEM ${catkin_INCLUDE_DIRS}) # This setting causes Qt's "MOC" generation to happen automatically @@ -57,6 +58,7 @@ add_library(${PROJECT_NAME} src/visuals/cell_layer.cc src/visuals/cell_selector.cc src/layered_map_display.cc + src/layered_map_factories.cc src/wavemap_map_display.cc ${QT_MOC}) set_wavemap_target_properties(${PROJECT_NAME}) diff --git a/interfaces/ros1/wavemap_rviz_plugin/include/wavemap_rviz_plugin/layered_map_display.h b/interfaces/ros1/wavemap_rviz_plugin/include/wavemap_rviz_plugin/layered_map_display.h index 7fe94fd0a..24622f893 100644 --- a/interfaces/ros1/wavemap_rviz_plugin/include/wavemap_rviz_plugin/layered_map_display.h +++ b/interfaces/ros1/wavemap_rviz_plugin/include/wavemap_rviz_plugin/layered_map_display.h @@ -9,12 +9,17 @@ #include +#include + #include +#include #include #include #include +#include #include "wavemap_rviz_plugin/common.h" +#include "wavemap_rviz_plugin/layered_map_factory.h" #include "wavemap_rviz_plugin/utils/button_property.h" #include "wavemap_rviz_plugin/visuals/voxel_visual.h" #endif @@ -56,12 +61,19 @@ class LayeredMapDisplay std::string name; std::string type; Source source = Source::kContinuous; + std::vector min_values; + std::vector max_values; }; std::vector available_layers_; LayeredMapSourceMode source_mode_ = LayeredMapSourceMode::kFromTopic; std::string selected_layer_name_ = "occupancy"; + int continuous_termination_height_ = 0; + int discrete_termination_height_ = 1; std::optional latest_msg_; + std::unique_ptr> + layered_map_factory_loader_; + std::vector> layered_map_factories_; const std::shared_ptr map_and_mutex_ = std::make_shared(); @@ -92,6 +104,9 @@ class LayeredMapDisplay void clearStoredMap(); void updateAvailableLayers(const wavemap_msgs::LayeredMap& msg); void updateStoredMapForSelectedLayer(const wavemap_msgs::LayeredMap& msg); + void configureSelectedLayerAppearance( + const wavemap_msgs::LayeredMap& msg); + void loadLayeredMapFactories(); void updateLegendOverlay(const wavemap_msgs::LayeredMap* msg = nullptr); void initializeLegendOverlay(); void destroyLegendOverlay(); diff --git a/interfaces/ros1/wavemap_rviz_plugin/include/wavemap_rviz_plugin/layered_map_factory.h b/interfaces/ros1/wavemap_rviz_plugin/include/wavemap_rviz_plugin/layered_map_factory.h index 78b874bf4..64ec1517d 100644 --- a/interfaces/ros1/wavemap_rviz_plugin/include/wavemap_rviz_plugin/layered_map_factory.h +++ b/interfaces/ros1/wavemap_rviz_plugin/include/wavemap_rviz_plugin/layered_map_factory.h @@ -2,12 +2,16 @@ #define WAVEMAP_RVIZ_PLUGIN_LAYERED_MAP_FACTORY_H_ #include +#include #include +#include #include #include #include #include +#include +#include #include "wavemap_rviz_plugin/layered_map_interface.h" @@ -19,7 +23,15 @@ inline std::vector extractLayerMetadata( std::min(msg.layer_names.size(), msg.layer_types.size()); layers.reserve(num_layers); for (size_t layer_idx = 0u; layer_idx < num_layers; ++layer_idx) { - layers.push_back({msg.layer_names[layer_idx], msg.layer_types[layer_idx]}); + LayerMetadata metadata{msg.layer_names[layer_idx], + msg.layer_types[layer_idx], {}, {}}; + if (layer_idx < msg.layer_min_values.size()) { + metadata.min_values = msg.layer_min_values[layer_idx].float32_values; + } + if (layer_idx < msg.layer_max_values.size()) { + metadata.max_values = msg.layer_max_values[layer_idx].float32_values; + } + layers.emplace_back(std::move(metadata)); } return layers; } @@ -32,6 +44,13 @@ class LayeredMapFactory { virtual std::shared_ptr tryCreate( const wavemap_msgs::LayeredHashedWaveletOctree& msg) const = 0; + + virtual bool tryLoad(const std::filesystem::path& /*filepath*/, + const std::string& /*frame_id*/, + wavemap_msgs::LayeredMap& /*msg*/, + std::string* /*error_message*/) const { + return false; + } }; template tryCreate( const wavemap_msgs::LayeredHashedWaveletOctree& msg) const override { + // Never invoke a typed converter on a different schema. Besides producing + // invalid values, converters commonly index their expected layer arrays + // directly, so a mismatch can otherwise become an out-of-bounds access. + if (msg.layer_names != CellDataRosConverterT::layerNames() || + msg.layer_types != CellDataRosConverterT::layerTypes()) { + return nullptr; + } typename LayeredMapT::Ptr map; convert::rosMsgToMap(msg, map); @@ -54,6 +80,29 @@ class TypedLayeredMapFactory : public LayeredMapFactory { } }; +// Adds file loading to a typed topic factory. A user registering a new schema +// only supplies their map, existing IO codec, and config factory; ROS message +// construction remains shared infrastructure. +template +class TypedLayeredMapFileFactory + : public TypedLayeredMapFactory { + public: + bool tryLoad(const std::filesystem::path& filepath, + const std::string& frame_id, wavemap_msgs::LayeredMap& msg, + std::string* error_message) const override { + FullLayeredMapT map(MakeConfigT()); + if (!LayeredMapIoT::load(filepath, map, error_message)) { + return false; + } + return convert::layeredMapToRosMsg( + map, frame_id, ros::Time(0), msg); + } +}; + } // namespace wavemap::rviz_plugin #endif // WAVEMAP_RVIZ_PLUGIN_LAYERED_MAP_FACTORY_H_ diff --git a/interfaces/ros1/wavemap_rviz_plugin/include/wavemap_rviz_plugin/layered_map_interface.h b/interfaces/ros1/wavemap_rviz_plugin/include/wavemap_rviz_plugin/layered_map_interface.h index e53b245fc..29f930cd3 100644 --- a/interfaces/ros1/wavemap_rviz_plugin/include/wavemap_rviz_plugin/layered_map_interface.h +++ b/interfaces/ros1/wavemap_rviz_plugin/include/wavemap_rviz_plugin/layered_map_interface.h @@ -1,6 +1,7 @@ #ifndef WAVEMAP_RVIZ_PLUGIN_LAYERED_MAP_INTERFACE_H_ #define WAVEMAP_RVIZ_PLUGIN_LAYERED_MAP_INTERFACE_H_ +#include #include #include #include @@ -17,6 +18,8 @@ namespace wavemap::rviz_plugin { struct LayerMetadata { std::string name; std::string type; + std::vector min_values; + std::vector max_values; }; // Runtime interface used by the RViz plugin for layered maps. Concrete user @@ -42,6 +45,10 @@ class LayeredMapInterface : public OccupancyQuery { virtual bool getLayerColor(const std::string& layer_name, const OctreeIndex& index, FloatingPoint occupancy, + FloatingPoint scalar_display_min, + FloatingPoint scalar_display_max, + const Ogre::ColourValue& scalar_low_color, + const Ogre::ColourValue& scalar_high_color, Ogre::ColourValue& color) const = 0; }; @@ -50,6 +57,11 @@ struct NoLayerColorProvider { static bool getLayerColor(const std::string& /*layer_name*/, const CellDataT& /*voxel*/, FloatingPoint /*occupancy*/, + FloatingPoint /*scalar_display_min*/, + FloatingPoint /*scalar_display_max*/, + const Ogre::ColourValue& /*scalar_low_color*/, + const Ogre::ColourValue& /*scalar_high_color*/, + const LayerMetadata& /*metadata*/, Ogre::ColourValue& /*color*/) { return false; } @@ -109,10 +121,23 @@ class HashedWaveletOctreeLayeredMapAdapter : public LayeredMapInterface { bool getLayerColor(const std::string& layer_name, const OctreeIndex& index, FloatingPoint occupancy, + FloatingPoint scalar_display_min, + FloatingPoint scalar_display_max, + const Ogre::ColourValue& scalar_low_color, + const Ogre::ColourValue& scalar_high_color, Ogre::ColourValue& color) const override { const auto voxel = map_->getVoxelValue(index); - return LayerColorProviderT::getLayerColor(layer_name, voxel, occupancy, - color); + const auto metadata_it = + std::find_if(layers_.begin(), layers_.end(), + [&](const LayerMetadata& layer) { + return layer.name == layer_name; + }); + if (metadata_it == layers_.end()) { + return false; + } + return LayerColorProviderT::getLayerColor( + layer_name, voxel, occupancy, scalar_display_min, scalar_display_max, + scalar_low_color, scalar_high_color, *metadata_it, color); } private: diff --git a/interfaces/ros1/wavemap_rviz_plugin/include/wavemap_rviz_plugin/visuals/voxel_visual.h b/interfaces/ros1/wavemap_rviz_plugin/include/wavemap_rviz_plugin/visuals/voxel_visual.h index 6dab6a784..476bd9705 100644 --- a/interfaces/ros1/wavemap_rviz_plugin/include/wavemap_rviz_plugin/visuals/voxel_visual.h +++ b/interfaces/ros1/wavemap_rviz_plugin/include/wavemap_rviz_plugin/visuals/voxel_visual.h @@ -2,9 +2,16 @@ #define WAVEMAP_RVIZ_PLUGIN_VISUALS_VOXEL_VISUAL_H_ #ifndef Q_MOC_RUN +#include +#include +#include #include +#include #include +#include #include +#include +#include #include #include @@ -13,6 +20,7 @@ #include #include #include +#include #include #include #include @@ -50,15 +58,46 @@ class VoxelVisual : public QObject { VoxelVisual(Ogre::SceneManager* scene_manager, rviz::ViewManager* view_manager, Ogre::SceneNode* parent_node, rviz::Property* submenu_root_property, - std::shared_ptr map_and_mutex); + std::shared_ptr map_and_mutex, + std::function layer_color_changed_callback = {}); // Destructor. Removes the visual elements from the scene. ~VoxelVisual() override; + void applyDiscreteLayerPatch( + const wavemap_msgs::DiscreteLayer& patch, + FloatingPoint min_cell_width); + void updateMap(bool redraw_all = false); void clear(); + int terminationHeight() const { + return termination_height_property_.getInt(); + } + void setTerminationHeight(int max_height, int height) { + termination_height_property_.setMax(max_height); + termination_height_property_.setInt(std::clamp(height, 0, max_height)); + } + FloatingPoint scalarDisplayMin() const { + return scalar_min_property_.getFloat(); + } + FloatingPoint scalarDisplayMax() const { + return scalar_max_property_.getFloat(); + } + Ogre::ColourValue scalarLowColor() const { return scalar_low_color_; } + Ogre::ColourValue scalarHighColor() const { return scalar_high_color_; } + void configureLayerAppearance( + const std::string& layer_name, const std::string& layer_type, + bool is_discrete, FloatingPoint scalar_min, FloatingPoint scalar_max, + bool has_low_color, const Ogre::ColourValue& low_color, + bool has_high_color, const Ogre::ColourValue& high_color, + const std::vector& categories = {}); + Ogre::ColourValue categoryColor(int value) const; + bool categoryVisible(int value) const; + Ogre::ColourValue boolColor(bool value) const; + bool boolVisible(bool value) const; + // Set the pose of the coordinate frame the message refers to void setFramePosition(const Ogre::Vector3& position); void setFrameOrientation(const Ogre::Quaternion& orientation); @@ -79,9 +118,16 @@ class VoxelVisual : public QObject { Ogre::ColourValue scalar_low_color_ = Ogre::ColourValue(0.f, 0.f, 1.f); Ogre::ColourValue scalar_high_color_ = Ogre::ColourValue(1.f, 1.f, 0.f); Ogre::ColourValue bool_true_color_ = Ogre::ColourValue(1.f, 0.05f, 0.05f); + Ogre::ColourValue bool_false_color_ = + Ogre::ColourValue(0.35f, 0.35f, 0.35f); + + std::string selected_color_layer_; + std::unordered_map category_colors_; + static Ogre::ColourValue defaultCategoryColor(int value); // Shared pointer to the map, owned by WavemapMapDisplay const std::shared_ptr map_and_mutex_; + const std::function layer_color_changed_callback_; // The SceneManager, kept here only so the destructor can ask it to // destroy the `frame_node_`. @@ -101,13 +147,8 @@ class VoxelVisual : public QObject { rviz::FloatProperty opacity_property_; rviz::EnumProperty color_mode_property_; rviz::ColorProperty flat_color_property_; - rviz::Property layer_color_properties_; rviz::FloatProperty scalar_min_property_; rviz::FloatProperty scalar_max_property_; - rviz::ColorProperty scalar_low_color_property_; - rviz::ColorProperty scalar_high_color_property_; - rviz::ColorProperty bool_true_color_property_; - rviz::BoolProperty show_bool_false_property_; // Frame-rate stats rviz::Property frame_rate_properties_; rviz::IntProperty num_queued_blocks_indicator_; @@ -116,6 +157,7 @@ class VoxelVisual : public QObject { // The objects implementing the voxel visuals using VoxelLayers = std::vector>; std::unordered_map block_voxel_layers_map_; + void detachVoxelLayers(VoxelLayers& voxel_layers); // Material handling Ogre::MaterialPtr voxel_material_; @@ -163,6 +205,17 @@ class VoxelVisual : public QObject { Ogre::ColourValue& color) const; void drawDiscreteLayer(const wavemap_msgs::DiscreteLayer& discrete_layer_msg, FloatingPoint min_cell_width); + void rebuildDiscreteBatches( + const std::unordered_set& batch_indices, + FloatingPoint min_cell_width); + static Index3D discreteRenderBatchIndex(const Index3D& parent_index); + + std::optional discrete_layer_metadata_; + std::unordered_map + discrete_parent_cells_; + std::unordered_map, Index3DHash> + discrete_batch_parents_; bool getDiscreteCellColor(const wavemap_msgs::DiscreteLayer& layer_msg, const wavemap_msgs::DiscreteLayerCell& cell_msg, int exception_index, bool is_exception, diff --git a/interfaces/ros1/wavemap_rviz_plugin/package.xml b/interfaces/ros1/wavemap_rviz_plugin/package.xml index 6504135cf..8063ef7fb 100644 --- a/interfaces/ros1/wavemap_rviz_plugin/package.xml +++ b/interfaces/ros1/wavemap_rviz_plugin/package.xml @@ -17,11 +17,13 @@ wavemap wavemap_msgs wavemap_ros_conversions + pluginlib rviz qtbase5-dev + diff --git a/interfaces/ros1/wavemap_rviz_plugin/plugin_description.xml b/interfaces/ros1/wavemap_rviz_plugin/plugin_description.xml index 10becc5cf..6f1ef582c 100644 --- a/interfaces/ros1/wavemap_rviz_plugin/plugin_description.xml +++ b/interfaces/ros1/wavemap_rviz_plugin/plugin_description.xml @@ -15,4 +15,29 @@ wavemap_msgs/LayeredMap + + Schema handler for the legacy color/traversability example. + + + Schema handler for the reflectivity field map. + + + Schema handler for layered occupancy-only maps. + + + Schema handler for occupancy and reflectivity maps. + + + Schema handler for occupancy, reflectivity, and class maps. + diff --git a/interfaces/ros1/wavemap_rviz_plugin/src/layered_map_display.cc b/interfaces/ros1/wavemap_rviz_plugin/src/layered_map_display.cc index be88d79b8..b104ab6ed 100644 --- a/interfaces/ros1/wavemap_rviz_plugin/src/layered_map_display.cc +++ b/interfaces/ros1/wavemap_rviz_plugin/src/layered_map_display.cc @@ -4,6 +4,7 @@ #include #include #include +#include #include #include #include @@ -21,6 +22,7 @@ #include #include #include +#include #include #include #include @@ -30,37 +32,121 @@ #include "example_layered_map_ros_config.h" #include "layered_ros_converter.h" -#include "wavemap_rviz_plugin/layered_map_factory.h" #include "wavemap_rviz_plugin/utils/alert_dialog.h" namespace wavemap::rviz_plugin { namespace { -struct ExampleContinuousLayerColorProvider { - static bool getLayerColor(const std::string& layer_name, - const LayeredVoxel& voxel, - FloatingPoint /*occupancy*/, - Ogre::ColourValue& color) { - if (layer_name == "color") { - color = Ogre::ColourValue(std::clamp(voxel.data.rgb.r, 0.f, 1.f), - std::clamp(voxel.data.rgb.g, 0.f, 1.f), - std::clamp(voxel.data.rgb.b, 0.f, 1.f), 1.f); - return true; - } +using IndexTuple = std::tuple; - if (layer_name == "traversability") { - const FloatingPoint t = - std::clamp(voxel.data.traversability, 0.f, 1.f); - color = Ogre::ColourValue(1.f - t, t, 0.15f, 1.f); - return true; +template +IndexTuple indexTuple(const IndexMsgT& index) { + return {index.x, index.y, index.z}; +} + +void mergeContinuousPatch(const wavemap_msgs::Map& patch, + wavemap_msgs::Map& stored) { + if (patch.layered_hashed_wavelet_octree.size() != 1u) { + return; + } + if (stored.layered_hashed_wavelet_octree.size() != 1u) { + stored = patch; + return; + } + const auto& patch_map = patch.layered_hashed_wavelet_octree.front(); + auto& stored_map = stored.layered_hashed_wavelet_octree.front(); + std::set allocated; + for (const auto& block_index : patch_map.allocated_block_indices) { + allocated.emplace(indexTuple(block_index)); + } + stored_map.allocated_block_indices = patch_map.allocated_block_indices; + stored_map.min_cell_width = patch_map.min_cell_width; + stored_map.min_log_odds = patch_map.min_log_odds; + stored_map.max_log_odds = patch_map.max_log_odds; + stored_map.tree_height = patch_map.tree_height; + stored_map.layer_names = patch_map.layer_names; + stored_map.layer_types = patch_map.layer_types; + stored_map.layer_min_values = patch_map.layer_min_values; + stored_map.layer_max_values = patch_map.layer_max_values; + stored_map.layer_visualizations = patch_map.layer_visualizations; + + stored_map.blocks.erase( + std::remove_if(stored_map.blocks.begin(), stored_map.blocks.end(), + [&](const auto& block) { + return allocated.find(indexTuple(block.root_node_offset)) == + allocated.end(); + }), + stored_map.blocks.end()); + for (const auto& patch_block : patch_map.blocks) { + const auto patch_index = indexTuple(patch_block.root_node_offset); + const auto stored_it = std::find_if( + stored_map.blocks.begin(), stored_map.blocks.end(), + [&](const auto& block) { + return indexTuple(block.root_node_offset) == patch_index; + }); + if (stored_it == stored_map.blocks.end()) { + stored_map.blocks.emplace_back(patch_block); + } else { + *stored_it = patch_block; } + } + stored.header = patch.header; +} - return false; +void mergeDiscretePatch(const wavemap_msgs::DiscreteLayer& patch, + wavemap_msgs::DiscreteLayer& stored) { + stored.name = patch.name; + stored.value_type = patch.value_type; + stored.block_height = patch.block_height; + stored.categories = patch.categories; + + std::set deleted_keys; + for (const auto& deleted_index : patch.deleted_parent_indices) { + deleted_keys.insert(indexTuple(deleted_index)); + } + if (!deleted_keys.empty()) { + stored.cells.erase( + std::remove_if(stored.cells.begin(), stored.cells.end(), + [&](const auto& cell) { + return deleted_keys.count( + indexTuple(cell.parent_index)) != 0u; + }), + stored.cells.end()); + } + + std::map stored_positions; + for (size_t index = 0u; index < stored.cells.size(); ++index) { + stored_positions[indexTuple(stored.cells[index].parent_index)] = index; + } + for (const auto& patch_cell : patch.cells) { + const auto key = indexTuple(patch_cell.parent_index); + const auto position_it = stored_positions.find(key); + if (position_it == stored_positions.end()) { + stored_positions[key] = stored.cells.size(); + stored.cells.emplace_back(patch_cell); + } else { + stored.cells[position_it->second] = patch_cell; + } } -}; + stored.deleted_parent_indices.clear(); +} -using ExampleContinuousMapFactory = - TypedLayeredMapFactory; +void mergeLayeredPatch(const wavemap_msgs::LayeredMap& patch, + wavemap_msgs::LayeredMap& stored) { + stored.header = patch.header; + mergeContinuousPatch(patch.continuous_map, stored.continuous_map); + for (const auto& layer_patch : patch.discrete_layers) { + const auto stored_it = std::find_if( + stored.discrete_layers.begin(), stored.discrete_layers.end(), + [&](const auto& layer) { return layer.name == layer_patch.name; }); + if (stored_it == stored.discrete_layers.end()) { + stored.discrete_layers.emplace_back(layer_patch); + stored.discrete_layers.back().deleted_parent_indices.clear(); + } else { + mergeDiscretePatch(layer_patch, *stored_it); + } + } + stored.is_full_update = true; +} Ogre::ColourValue stableIntColor(int value) { const uint32_t hash = static_cast(value) * 2654435761u; @@ -128,10 +214,35 @@ void LayeredMapDisplay::onInitialize() { } voxel_visual_ = std::make_unique( scene_manager_, context_->getViewManager(), scene_node_, - &voxel_visual_properties_, map_and_mutex_); + &voxel_visual_properties_, map_and_mutex_, [this]() { + updateLegendOverlay(latest_msg_ ? &latest_msg_.value() : nullptr); + }); + loadLayeredMapFactories(); initializeLegendOverlay(); } +void LayeredMapDisplay::loadLayeredMapFactories() { + layered_map_factories_.clear(); + try { + layered_map_factory_loader_ = std::make_unique< + pluginlib::ClassLoader>( + "wavemap_rviz_plugin", "wavemap::rviz_plugin::LayeredMapFactory"); + for (const std::string& class_name : + layered_map_factory_loader_->getDeclaredClasses()) { + try { + layered_map_factories_.emplace_back( + layered_map_factory_loader_->createInstance(class_name)); + } catch (const pluginlib::PluginlibException& exception) { + ROS_WARN_STREAM("Could not load layered map schema handler '" + << class_name << "': " << exception.what()); + } + } + } catch (const pluginlib::PluginlibException& exception) { + ROS_ERROR_STREAM("Could not initialize layered map schema handlers: " + << exception.what()); + } +} + void LayeredMapDisplay::reset() { ProfilerZoneScoped; MFDClass::reset(); @@ -143,6 +254,7 @@ void LayeredMapDisplay::reset() { updateLegendOverlay(nullptr); } + void LayeredMapDisplay::processMessage( const wavemap_msgs::LayeredMap::ConstPtr& msg) { ProfilerZoneScoped; @@ -154,11 +266,25 @@ void LayeredMapDisplay::processMessage( displayMessage(*msg); } -void LayeredMapDisplay::displayMessage(const wavemap_msgs::LayeredMap& msg) { - latest_msg_ = msg; +void LayeredMapDisplay::displayMessage( + const wavemap_msgs::LayeredMap& incoming_msg) { + const bool replace_all = incoming_msg.is_full_update || !latest_msg_; + if (replace_all) { + latest_msg_ = incoming_msg; + } else { + mergeLayeredPatch(incoming_msg, *latest_msg_); + } + const wavemap_msgs::LayeredMap& msg = *latest_msg_; updateAvailableLayers(msg); + configureSelectedLayerAppearance(msg); updateLegendOverlay(&msg); - updateStoredMapForSelectedLayer(msg); + const DisplayLayer* selected_layer = selectedLayer(); + const bool selected_is_discrete = + selected_layer && + selected_layer->source == DisplayLayer::Source::kDiscrete; + if (!selected_is_discrete || replace_all) { + updateStoredMapForSelectedLayer(msg); + } if (!voxel_visual_) { ROS_WARN("Voxel visual not initialized yet, skipping layered map message."); @@ -176,25 +302,114 @@ void LayeredMapDisplay::displayMessage(const wavemap_msgs::LayeredMap& msg) { } voxel_visual_->setFramePosition(position); voxel_visual_->setFrameOrientation(orientation); + + if (selected_is_discrete && !replace_all) { + const auto patch_it = std::find_if( + incoming_msg.discrete_layers.begin(), + incoming_msg.discrete_layers.end(), + [this](const wavemap_msgs::DiscreteLayer& layer) { + return layer.name == selected_layer_name_; + }); + if (patch_it != incoming_msg.discrete_layers.end()) { + FloatingPoint min_cell_width = 0.1f; + if (msg.continuous_map.layered_hashed_wavelet_octree.size() == 1u) { + min_cell_width = msg.continuous_map.layered_hashed_wavelet_octree + .front() + .min_cell_width; + } + voxel_visual_->applyDiscreteLayerPatch(*patch_it, min_cell_width); + } + return; + } voxel_visual_->updateMap(true); } -bool LayeredMapDisplay::loadMapFromDisk( - const std::filesystem::path& filepath, std::string* error_message) { - ExampleLayeredMap map(ExampleLayeredMapConfig{}); - if (!ExampleLayeredMapIo::load(filepath, map, error_message)) { - return false; +void LayeredMapDisplay::configureSelectedLayerAppearance( + const wavemap_msgs::LayeredMap& msg) { + if (!voxel_visual_) { + return; + } + const DisplayLayer* layer = selectedLayer(); + if (!layer) { + return; } - wavemap_msgs::LayeredMap msg; - const std::string frame_id = fixed_frame_.isEmpty() ? "map" : fixed_frame_.toStdString(); - if (!convert::layeredMapToRosMsg( - map, frame_id, ros::Time(0), msg)) { - return false; + bool has_low_color = false; + bool has_high_color = false; + Ogre::ColourValue low_color; + FloatingPoint scalar_min = voxel_visual_->scalarDisplayMin(); + FloatingPoint scalar_max = voxel_visual_->scalarDisplayMax(); + Ogre::ColourValue high_color; + std::vector categories; + + if (layer->source == DisplayLayer::Source::kContinuous && + !msg.continuous_map.layered_hashed_wavelet_octree.empty()) { + const auto& continuous = + msg.continuous_map.layered_hashed_wavelet_octree.front(); + const auto layer_name_it = + std::find(continuous.layer_names.begin(), continuous.layer_names.end(), + selected_layer_name_); + if (layer_name_it != continuous.layer_names.end()) { + const size_t layer_index = static_cast( + std::distance(continuous.layer_names.begin(), layer_name_it)); + if (layer_index < continuous.layer_min_values.size() && + layer_index < continuous.layer_max_values.size() && + !continuous.layer_min_values[layer_index].float32_values.empty() && + !continuous.layer_max_values[layer_index].float32_values.empty()) { + scalar_min = continuous.layer_min_values[layer_index].float32_values[0]; + scalar_max = continuous.layer_max_values[layer_index].float32_values[0]; + } + } + const auto visualization_it = std::find_if( + continuous.layer_visualizations.begin(), + continuous.layer_visualizations.end(), [&](const auto& visualization) { + return visualization.name == selected_layer_name_; + }); + if (visualization_it != continuous.layer_visualizations.end()) { + has_low_color = visualization_it->has_low_color; + has_high_color = visualization_it->has_high_color; + low_color = Ogre::ColourValue(visualization_it->low_r, + visualization_it->low_g, + visualization_it->low_b, 1.f); + high_color = Ogre::ColourValue(visualization_it->high_r, + visualization_it->high_g, + visualization_it->high_b, 1.f); + } + } else if (layer->source == DisplayLayer::Source::kDiscrete) { + const auto layer_it = std::find_if( + msg.discrete_layers.begin(), msg.discrete_layers.end(), + [this](const auto& discrete_layer) { + return discrete_layer.name == selected_layer_name_; + }); + if (layer_it != msg.discrete_layers.end()) { + categories = layer_it->categories; + } } - displayMessage(msg); - return true; + voxel_visual_->configureLayerAppearance( + layer->name, layer->type, + layer->source == DisplayLayer::Source::kDiscrete, + scalar_min, scalar_max, has_low_color, low_color, + has_high_color, high_color, categories); +} + + +bool LayeredMapDisplay::loadMapFromDisk( + const std::filesystem::path& filepath, std::string* error_message) { + const std::string frame_id = + fixed_frame_.isEmpty() ? "map" : fixed_frame_.toStdString(); + for (const auto& factory : layered_map_factories_) { + wavemap_msgs::LayeredMap msg; + std::string factory_error; + if (factory->tryLoad(filepath, frame_id, msg, &factory_error)) { + displayMessage(msg); + return true; + } + if (error_message && !factory_error.empty()) { + *error_message = std::move(factory_error); + } + } + return false; } void LayeredMapDisplay::clearStoredMap() { @@ -208,9 +423,9 @@ void LayeredMapDisplay::clearStoredMap() { void LayeredMapDisplay::updateAvailableLayers( const wavemap_msgs::LayeredMap& msg) { - available_layers_.clear(); - available_layers_.push_back( - {"occupancy", "float32", DisplayLayer::Source::kContinuous}); + std::vector new_layers; + new_layers.push_back( + {"occupancy", "float32", DisplayLayer::Source::kContinuous, {}, {}}); if (msg.continuous_map.layered_hashed_wavelet_octree.size() == 1u) { const auto& continuous_msg = @@ -218,27 +433,53 @@ void LayeredMapDisplay::updateAvailableLayers( const size_t num_layers = std::min(continuous_msg.layer_names.size(), continuous_msg.layer_types.size()); for (size_t layer_i = 0u; layer_i < num_layers; ++layer_i) { - available_layers_.push_back({continuous_msg.layer_names[layer_i], - continuous_msg.layer_types[layer_i], - DisplayLayer::Source::kContinuous}); + DisplayLayer layer{continuous_msg.layer_names[layer_i], + continuous_msg.layer_types[layer_i], + DisplayLayer::Source::kContinuous, {}, {}}; + if (layer_i < continuous_msg.layer_min_values.size()) { + layer.min_values = + continuous_msg.layer_min_values[layer_i].float32_values; + } + if (layer_i < continuous_msg.layer_max_values.size()) { + layer.max_values = + continuous_msg.layer_max_values[layer_i].float32_values; + } + new_layers.emplace_back(std::move(layer)); } } for (const auto& discrete_layer : msg.discrete_layers) { - available_layers_.push_back({discrete_layer.name, discrete_layer.value_type, - DisplayLayer::Source::kDiscrete}); + new_layers.push_back({discrete_layer.name, discrete_layer.value_type, + DisplayLayer::Source::kDiscrete, {}, {}}); } const bool selected_still_exists = - std::any_of(available_layers_.begin(), available_layers_.end(), + std::any_of(new_layers.begin(), new_layers.end(), [this](const DisplayLayer& layer) { return layer.name == selected_layer_name_; }); if (!selected_still_exists) { - selected_layer_name_ = available_layers_.empty() ? "occupancy" - : available_layers_.front().name; + selected_layer_name_ = + new_layers.empty() ? "occupancy" : new_layers.front().name; } + const auto sameLayer = [](const DisplayLayer& lhs, const DisplayLayer& rhs) { + return lhs.name == rhs.name && lhs.type == rhs.type && + lhs.source == rhs.source && lhs.min_values == rhs.min_values && + lhs.max_values == rhs.max_values; + }; + const bool schema_unchanged = + available_layers_.size() == new_layers.size() && + std::equal(available_layers_.begin(), available_layers_.end(), + new_layers.begin(), sameLayer); + available_layers_ = std::move(new_layers); + if (schema_unchanged) { + return; + } + + // Rebuilding an EnumProperty emits selection-change signals. Block them + // here because displayMessage() performs exactly one map update below. + const QSignalBlocker signal_blocker(&layer_property_); layer_property_.clearOptions(); for (const DisplayLayer& layer : available_layers_) { layer_property_.addOption(QString::fromStdString(layer.name)); @@ -286,9 +527,11 @@ void LayeredMapDisplay::updateStoredMapForSelectedLayer( if (msg.continuous_map.layered_hashed_wavelet_octree.size() == 1u) { const auto& continuous_msg = msg.continuous_map.layered_hashed_wavelet_octree.front(); - if (auto typed_map = ExampleContinuousMapFactory{}.tryCreate(continuous_msg)) { - map_and_mutex_->layered_map = std::move(typed_map); - return; + for (const auto& factory : layered_map_factories_) { + if (auto typed_map = factory->tryCreate(continuous_msg)) { + map_and_mutex_->layered_map = std::move(typed_map); + return; + } } map_and_mutex_->layered_map_msg = continuous_msg; @@ -518,9 +761,23 @@ void LayeredMapDisplay::updateLegendOverlay(const wavemap_msgs::LayeredMap* msg) rows.push_back({Ogre::ColourValue(0.f, 1.f, 0.f, 1.f), "green channel"}); rows.push_back({Ogre::ColourValue(0.f, 0.f, 1.f, 1.f), "blue channel"}); } else if (layer->type == "float32") { - subtitle = "scalar value"; - rows.push_back({Ogre::ColourValue(1.f, 0.f, 0.15f, 1.f), "0.0"}); - rows.push_back({Ogre::ColourValue(0.f, 1.f, 0.15f, 1.f), "1.0"}); + subtitle = "scalar value / RViz display range"; + const FloatingPoint min_value = + voxel_visual_ ? voxel_visual_->scalarDisplayMin() : 0.f; + const FloatingPoint max_value = + voxel_visual_ ? voxel_visual_->scalarDisplayMax() : 1.f; + std::ostringstream min_label; + std::ostringstream max_label; + min_label << min_value; + max_label << max_value; + rows.push_back( + {voxel_visual_ ? voxel_visual_->scalarLowColor() + : Ogre::ColourValue(0.f, 0.f, 1.f, 1.f), + min_label.str()}); + rows.push_back( + {voxel_visual_ ? voxel_visual_->scalarHighColor() + : Ogre::ColourValue(1.f, 1.f, 0.f, 1.f), + max_label.str()}); } else { subtitle = "No legend rule for type " + layer->type; } @@ -538,11 +795,14 @@ void LayeredMapDisplay::updateLegendOverlay(const wavemap_msgs::LayeredMap* msg) subtitle = "boolean values"; const std::set values = collectBoolValues(*layer_it); if (values.count(true)) { - rows.push_back({Ogre::ColourValue(1.f, 0.05f, 0.05f, 1.f), "true"}); + rows.push_back({voxel_visual_ ? voxel_visual_->boolColor(true) + : Ogre::ColourValue(1.f, 0.05f, 0.05f), + "true"}); } if (values.count(false)) { - rows.push_back({Ogre::ColourValue(0.45f, 0.45f, 0.45f, 1.f), - "false hidden by default"}); + rows.push_back({voxel_visual_ ? voxel_visual_->boolColor(false) + : Ogre::ColourValue(0.35f, 0.35f, 0.35f), + "false"}); } if (values.empty()) { subtitle = "No observed values"; @@ -550,16 +810,31 @@ void LayeredMapDisplay::updateLegendOverlay(const wavemap_msgs::LayeredMap* msg) } else if (layer_it->value_type == "int") { subtitle = "integer values"; const std::set values = collectIntValues(*layer_it); + std::map labels; + for (const auto& category : layer_it->categories) { + labels[category.value] = category.label; + } constexpr size_t kMaxLegendEntries = 10u; + const size_t category_limit = values.size() > kMaxLegendEntries + ? kMaxLegendEntries - 1u + : kMaxLegendEntries; size_t entry_count = 0u; for (const int value : values) { - if (kMaxLegendEntries <= entry_count) { + if (category_limit <= entry_count) { rows.push_back({Ogre::ColourValue(0.70f, 0.70f, 0.70f, 1.f), "+" + std::to_string(values.size() - entry_count) + " more"}); break; } - rows.push_back({stableIntColor(value), std::to_string(value)}); + const auto label_it = labels.find(value); + const std::string label = + label_it == labels.end() || label_it->second.empty() + ? "Value " + std::to_string(value) + : label_it->second + " (" + std::to_string(value) + ")"; + rows.push_back( + {voxel_visual_ ? voxel_visual_->categoryColor(value) + : stableIntColor(value), + label}); ++entry_count; } if (values.empty()) { @@ -655,14 +930,46 @@ void LayeredMapDisplay::loadMapFromDiskCallback() { void LayeredMapDisplay::updateLayerSelectionCallback() { ProfilerZoneScoped; + const DisplayLayer* previous_layer = selectedLayer(); + if (voxel_visual_ && previous_layer) { + if (previous_layer->source == DisplayLayer::Source::kDiscrete) { + discrete_termination_height_ = voxel_visual_->terminationHeight(); + } else { + continuous_termination_height_ = voxel_visual_->terminationHeight(); + } + } selected_layer_name_ = layer_property_.getStdString(); if (latest_msg_) { + configureSelectedLayerAppearance(latest_msg_.value()); updateLegendOverlay(&latest_msg_.value()); updateStoredMapForSelectedLayer(latest_msg_.value()); } else { updateLegendOverlay(nullptr); } if (voxel_visual_) { + const DisplayLayer* current_layer = selectedLayer(); + if (current_layer && latest_msg_) { + if (current_layer->source == DisplayLayer::Source::kDiscrete) { + const auto layer_it = std::find_if( + latest_msg_->discrete_layers.begin(), + latest_msg_->discrete_layers.end(), + [this](const auto& layer) { + return layer.name == selected_layer_name_; + }); + const int max_height = + layer_it == latest_msg_->discrete_layers.end() + ? 1 + : layer_it->block_height; + voxel_visual_->setTerminationHeight(max_height, + discrete_termination_height_); + } else if (latest_msg_->continuous_map.layered_hashed_wavelet_octree.size() == + 1u) { + voxel_visual_->setTerminationHeight( + latest_msg_->continuous_map.layered_hashed_wavelet_octree.front() + .tree_height, + continuous_termination_height_); + } + } voxel_visual_->clear(); voxel_visual_->updateMap(true); } diff --git a/interfaces/ros1/wavemap_rviz_plugin/src/layered_map_factories.cc b/interfaces/ros1/wavemap_rviz_plugin/src/layered_map_factories.cc new file mode 100644 index 000000000..94c7fdeb1 --- /dev/null +++ b/interfaces/ros1/wavemap_rviz_plugin/src/layered_map_factories.cc @@ -0,0 +1,195 @@ +#include +#include + +#include +#include + +#include "example_layered_map_ros_config.h" +#include "field_layered_map_ros_config.h" +#include "layered_ros_converter.h" +#include "occupancy_map_layers.h" +#include "reflectivity_map_layers.h" +#include "reflectivity_only_map_layers.h" +#include "wavemap_rviz_plugin/layered_map_factory.h" + +namespace wavemap::rviz_plugin { +namespace field_map = wavemap::examples::field_map; +namespace occupancy_map = wavemap::examples::occupancy_map; +namespace reflectivity_map = wavemap::examples::reflectivity_map; +namespace reflectivity_only_map = wavemap::examples::reflectivity_only_map; + +FloatingPoint normalizeScalar(FloatingPoint value, + const LayerMetadata& metadata) { + if (metadata.min_values.size() != 1u || + metadata.max_values.size() != 1u || + metadata.max_values.front() <= metadata.min_values.front()) { + return std::clamp(value, 0.f, 1.f); + } + return std::clamp((value - metadata.min_values.front()) / + (metadata.max_values.front() - + metadata.min_values.front()), + 0.f, 1.f); +} + +FloatingPoint normalizeComponent(FloatingPoint value, + const LayerMetadata& metadata, + size_t component) { + if (component >= metadata.min_values.size() || + component >= metadata.max_values.size() || + metadata.max_values[component] <= metadata.min_values[component]) { + return std::clamp(value, 0.f, 1.f); + } + return std::clamp((value - metadata.min_values[component]) / + (metadata.max_values[component] - + metadata.min_values[component]), + 0.f, 1.f); +} + +Ogre::ColourValue scalarGray(FloatingPoint normalized_value) { + const FloatingPoint visible_value = + 0.15f + 0.85f * std::clamp(normalized_value, 0.f, 1.f); + return Ogre::ColourValue(visible_value, visible_value, visible_value, 1.f); +} + +Ogre::ColourValue scalarGradient(FloatingPoint normalized_value, + const Ogre::ColourValue& low, + const Ogre::ColourValue& high) { + const FloatingPoint value = std::clamp(normalized_value, 0.f, 1.f); + return Ogre::ColourValue(low.r + value * (high.r - low.r), + low.g + value * (high.g - low.g), + low.b + value * (high.b - low.b), 1.f); +} + +struct ExampleContinuousLayerColorProvider { + static bool getLayerColor(const std::string& layer_name, + const LayeredVoxel& voxel, + FloatingPoint /*occupancy*/, + FloatingPoint /*scalar_display_min*/, + FloatingPoint /*scalar_display_max*/, + const Ogre::ColourValue& /*scalar_low_color*/, + const Ogre::ColourValue& /*scalar_high_color*/, + const LayerMetadata& metadata, + Ogre::ColourValue& color) { + if (layer_name == "color") { + color = Ogre::ColourValue( + normalizeComponent(voxel.data.rgb.r, metadata, 0u), + normalizeComponent(voxel.data.rgb.g, metadata, 1u), + normalizeComponent(voxel.data.rgb.b, metadata, 2u), 1.f); + return true; + } + if (layer_name == "traversability") { + const FloatingPoint value = + normalizeScalar(voxel.data.traversability, metadata); + color = scalarGray(value); + return true; + } + return false; + } +}; + +struct FieldContinuousLayerColorProvider { + template + static bool getLayerColor(const std::string& layer_name, + const VoxelT& voxel, + FloatingPoint /*occupancy*/, + FloatingPoint scalar_display_min, + FloatingPoint scalar_display_max, + const Ogre::ColourValue& scalar_low_color, + const Ogre::ColourValue& scalar_high_color, + const LayerMetadata& /*metadata*/, + Ogre::ColourValue& color) { + if (layer_name != "reflectivity") { + return false; + } + LayerMetadata display_metadata; + display_metadata.min_values = {scalar_display_min}; + display_metadata.max_values = {scalar_display_max}; + const FloatingPoint value = normalizeScalar( + voxel.data.template get(), + display_metadata); + color = scalarGradient(value, scalar_low_color, scalar_high_color); + return true; + } +}; + +struct ReflectivityContinuousLayerColorProvider { + template + static bool getLayerColor(const std::string& layer_name, + const VoxelT& voxel, + FloatingPoint /*occupancy*/, + FloatingPoint scalar_display_min, + FloatingPoint scalar_display_max, + const Ogre::ColourValue& scalar_low_color, + const Ogre::ColourValue& scalar_high_color, + const LayerMetadata& /*metadata*/, + Ogre::ColourValue& color) { + if (layer_name != "reflectivity") { + return false; + } + LayerMetadata display_metadata; + display_metadata.min_values = {scalar_display_min}; + display_metadata.max_values = {scalar_display_max}; + const auto& state = + voxel.data.template get(); + const FloatingPoint value = normalizeScalar( + layered::LayerStateConversion< + reflectivity_map::ReflectivityLayer>::value(state), + display_metadata); + color = scalarGradient(value, scalar_low_color, scalar_high_color); + return true; + } +}; + +class ExampleLayeredMapFactory final + : public TypedLayeredMapFileFactory< + ContinuousWaveletMap, LayeredVoxelRosConverter, + ExampleContinuousLayerColorProvider, ExampleLayeredMap, + ExampleLayeredMapIo, + layered_map_config::makeDefaultExampleLayeredMapConfig> {}; + +class FieldLayeredMapFactory final + : public TypedLayeredMapFileFactory< + field_map::ContinuousMap, + field_map::ros_config::VoxelRosConverter, + FieldContinuousLayerColorProvider, field_map::Map, field_map::MapIo, + field_map::makeDefaultConfig> {}; + +class OccupancyLayeredMapFactory final + : public TypedLayeredMapFileFactory< + occupancy_map::Definition::ContinuousMap, + wavemap::convert::DescriptorContinuousRosConverter< + occupancy_map::Definition::Voxel>, + NoLayerColorProvider, occupancy_map::Definition::Map, + occupancy_map::Definition::MapIo, occupancy_map::makeMapConfig> {}; + +class ReflectivityLayeredMapFactory final + : public TypedLayeredMapFileFactory< + reflectivity_only_map::Definition::ContinuousMap, + wavemap::convert::DescriptorContinuousRosConverter< + reflectivity_only_map::Definition::Voxel>, + ReflectivityContinuousLayerColorProvider, + reflectivity_only_map::Definition::Map, + reflectivity_only_map::Definition::MapIo, + reflectivity_only_map::makeMapConfig> {}; + +class ReflectivityClassLayeredMapFactory final + : public TypedLayeredMapFileFactory< + reflectivity_map::Definition::ContinuousMap, + wavemap::convert::DescriptorContinuousRosConverter< + reflectivity_map::Definition::Voxel>, + ReflectivityContinuousLayerColorProvider, + reflectivity_map::Definition::Map, + reflectivity_map::Definition::MapIo, + reflectivity_map::makeMapConfig> {}; +} // namespace wavemap::rviz_plugin + +PLUGINLIB_EXPORT_CLASS(wavemap::rviz_plugin::ExampleLayeredMapFactory, + wavemap::rviz_plugin::LayeredMapFactory) +PLUGINLIB_EXPORT_CLASS(wavemap::rviz_plugin::FieldLayeredMapFactory, + wavemap::rviz_plugin::LayeredMapFactory) +PLUGINLIB_EXPORT_CLASS(wavemap::rviz_plugin::OccupancyLayeredMapFactory, + wavemap::rviz_plugin::LayeredMapFactory) +PLUGINLIB_EXPORT_CLASS(wavemap::rviz_plugin::ReflectivityLayeredMapFactory, + wavemap::rviz_plugin::LayeredMapFactory) +PLUGINLIB_EXPORT_CLASS(wavemap::rviz_plugin::ReflectivityClassLayeredMapFactory, + wavemap::rviz_plugin::LayeredMapFactory) diff --git a/interfaces/ros1/wavemap_rviz_plugin/src/layered_map_factories.cc.orig b/interfaces/ros1/wavemap_rviz_plugin/src/layered_map_factories.cc.orig new file mode 100644 index 000000000..ef36e1d3f --- /dev/null +++ b/interfaces/ros1/wavemap_rviz_plugin/src/layered_map_factories.cc.orig @@ -0,0 +1,169 @@ +#include +#include + +#include +#include + +#include "example_layered_map_ros_config.h" +#include "field_layered_map_ros_config.h" +#include "layered_ros_converter.h" +#include "reflectivity_map_layers.h" +#include "wavemap_rviz_plugin/layered_map_factory.h" + +namespace wavemap::rviz_plugin { +namespace field_map = wavemap::examples::field_map; +namespace reflectivity_map = wavemap::examples::reflectivity_map; + +FloatingPoint normalizeScalar(FloatingPoint value, + const LayerMetadata& metadata) { + if (metadata.min_values.size() != 1u || + metadata.max_values.size() != 1u || + metadata.max_values.front() <= metadata.min_values.front()) { + return std::clamp(value, 0.f, 1.f); + } + return std::clamp((value - metadata.min_values.front()) / + (metadata.max_values.front() - + metadata.min_values.front()), + 0.f, 1.f); +} + +FloatingPoint normalizeComponent(FloatingPoint value, + const LayerMetadata& metadata, + size_t component) { + if (component >= metadata.min_values.size() || + component >= metadata.max_values.size() || + metadata.max_values[component] <= metadata.min_values[component]) { + return std::clamp(value, 0.f, 1.f); + } + return std::clamp((value - metadata.min_values[component]) / + (metadata.max_values[component] - + metadata.min_values[component]), + 0.f, 1.f); +} + +Ogre::ColourValue scalarGray(FloatingPoint normalized_value) { + const FloatingPoint visible_value = + 0.15f + 0.85f * std::clamp(normalized_value, 0.f, 1.f); + return Ogre::ColourValue(visible_value, visible_value, visible_value, 1.f); +} + +Ogre::ColourValue scalarGradient(FloatingPoint normalized_value, + const Ogre::ColourValue& low, + const Ogre::ColourValue& high) { + const FloatingPoint value = std::clamp(normalized_value, 0.f, 1.f); + return Ogre::ColourValue(low.r + value * (high.r - low.r), + low.g + value * (high.g - low.g), + low.b + value * (high.b - low.b), 1.f); +} + +struct ExampleContinuousLayerColorProvider { + static bool getLayerColor(const std::string& layer_name, + const LayeredVoxel& voxel, + FloatingPoint /*occupancy*/, + FloatingPoint /*scalar_display_min*/, + FloatingPoint /*scalar_display_max*/, + const Ogre::ColourValue& /*scalar_low_color*/, + const Ogre::ColourValue& /*scalar_high_color*/, + const LayerMetadata& metadata, + Ogre::ColourValue& color) { + if (layer_name == "color") { + color = Ogre::ColourValue( + normalizeComponent(voxel.data.rgb.r, metadata, 0u), + normalizeComponent(voxel.data.rgb.g, metadata, 1u), + normalizeComponent(voxel.data.rgb.b, metadata, 2u), 1.f); + return true; + } + if (layer_name == "traversability") { + const FloatingPoint value = + normalizeScalar(voxel.data.traversability, metadata); + color = scalarGray(value); + return true; + } + return false; + } +}; + +struct FieldContinuousLayerColorProvider { + template + static bool getLayerColor(const std::string& layer_name, + const VoxelT& voxel, + FloatingPoint /*occupancy*/, + FloatingPoint scalar_display_min, + FloatingPoint scalar_display_max, + const Ogre::ColourValue& scalar_low_color, + const Ogre::ColourValue& scalar_high_color, + const LayerMetadata& /*metadata*/, + Ogre::ColourValue& color) { + if (layer_name != "reflectivity") { + return false; + } + LayerMetadata display_metadata; + display_metadata.min_values = {scalar_display_min}; + display_metadata.max_values = {scalar_display_max}; + const FloatingPoint value = normalizeScalar( + voxel.data.template get(), + display_metadata); + color = scalarGradient(value, scalar_low_color, scalar_high_color); + return true; + } +}; + +struct ReflectivityContinuousLayerColorProvider { + template + static bool getLayerColor(const std::string& layer_name, + const VoxelT& voxel, + FloatingPoint /*occupancy*/, + FloatingPoint scalar_display_min, + FloatingPoint scalar_display_max, + const Ogre::ColourValue& scalar_low_color, + const Ogre::ColourValue& scalar_high_color, + const LayerMetadata& /*metadata*/, + Ogre::ColourValue& color) { + if (layer_name != "reflectivity") { + return false; + } + LayerMetadata display_metadata; + display_metadata.min_values = {scalar_display_min}; + display_metadata.max_values = {scalar_display_max}; + const auto& state = + voxel.data.template get(); + const FloatingPoint value = normalizeScalar( + layered::LayerStateConversion< + reflectivity_map::ReflectivityLayer>::value(state), + display_metadata); + color = scalarGradient(value, scalar_low_color, scalar_high_color); + return true; + } +}; + +class ExampleLayeredMapFactory final + : public TypedLayeredMapFileFactory< + ContinuousWaveletMap, LayeredVoxelRosConverter, + ExampleContinuousLayerColorProvider, ExampleLayeredMap, + ExampleLayeredMapIo, + layered_map_config::makeDefaultExampleLayeredMapConfig> {}; + +class FieldLayeredMapFactory final + : public TypedLayeredMapFileFactory< + field_map::ContinuousMap, + field_map::ros_config::VoxelRosConverter, + FieldContinuousLayerColorProvider, field_map::Map, field_map::MapIo, + field_map::makeDefaultConfig> {}; + +class ReflectivityLayeredMapFactory final + : public TypedLayeredMapFileFactory< + reflectivity_map::Definition::ContinuousMap, + wavemap::convert::DescriptorContinuousRosConverter< + reflectivity_map::Definition::Voxel>, + ReflectivityContinuousLayerColorProvider, + reflectivity_map::Definition::Map, + reflectivity_map::Definition::MapIo, + reflectivity_map::makeMapConfig> {}; +} // namespace wavemap::rviz_plugin + +PLUGINLIB_EXPORT_CLASS(wavemap::rviz_plugin::ExampleLayeredMapFactory, + wavemap::rviz_plugin::LayeredMapFactory) +PLUGINLIB_EXPORT_CLASS(wavemap::rviz_plugin::FieldLayeredMapFactory, + wavemap::rviz_plugin::LayeredMapFactory) +PLUGINLIB_EXPORT_CLASS(wavemap::rviz_plugin::ReflectivityLayeredMapFactory, + wavemap::rviz_plugin::LayeredMapFactory) 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 1b900bab1..66632bc19 100644 --- a/interfaces/ros1/wavemap_rviz_plugin/src/visuals/voxel_visual.cc +++ b/interfaces/ros1/wavemap_rviz_plugin/src/visuals/voxel_visual.cc @@ -18,14 +18,18 @@ #include #include #include +#include namespace wavemap::rviz_plugin { VoxelVisual::VoxelVisual(Ogre::SceneManager* scene_manager, rviz::ViewManager* view_manager, Ogre::SceneNode* parent_node, rviz::Property* submenu_root_property, - std::shared_ptr map_and_mutex) + std::shared_ptr map_and_mutex, + std::function layer_color_changed_callback) : map_and_mutex_(std::move(map_and_mutex)), + layer_color_changed_callback_( + std::move(layer_color_changed_callback)), scene_manager_(CHECK_NOTNULL(scene_manager)), frame_node_(CHECK_NOTNULL(parent_node)->createChildSceneNode()), visibility_property_( @@ -50,33 +54,14 @@ VoxelVisual::VoxelVisual(Ogre::SceneManager* scene_manager, "Flat color", rviz::ogreToQt(voxel_flat_color_), R"(Solid color to use when "Color Mode" is set to "Flat")", submenu_root_property, SLOT(flatColorUpdateCallback()), this), - layer_color_properties_("Layer color", QVariant(), - "Generic color settings for layered map fields.", - submenu_root_property), scalar_min_property_("Scalar min", 0.0, "Lower value used when coloring scalar continuous layers.", - &layer_color_properties_, + submenu_root_property, SLOT(layerColorUpdateCallback()), this), scalar_max_property_("Scalar max", 1.0, "Upper value used when coloring scalar continuous layers.", - &layer_color_properties_, + submenu_root_property, SLOT(layerColorUpdateCallback()), this), - scalar_low_color_property_( - "Scalar low color", rviz::ogreToQt(scalar_low_color_), - "Color used for the scalar minimum.", &layer_color_properties_, - SLOT(layerColorUpdateCallback()), this), - scalar_high_color_property_( - "Scalar high color", rviz::ogreToQt(scalar_high_color_), - "Color used for the scalar maximum.", &layer_color_properties_, - SLOT(layerColorUpdateCallback()), this), - bool_true_color_property_( - "Bool true color", rviz::ogreToQt(bool_true_color_), - "Color used for true values in boolean discrete layers.", - &layer_color_properties_, SLOT(layerColorUpdateCallback()), this), - show_bool_false_property_( - "Show bool false", false, - "Whether to draw false values in boolean discrete layers.", - &layer_color_properties_, SLOT(layerColorUpdateCallback()), this), frame_rate_properties_("Frame rate", QVariant(), "Properties to control the frame rate.", submenu_root_property), @@ -137,17 +122,24 @@ void VoxelVisual::clear() { block_update_queue_.clear(); for (auto& [block_index, voxel_layers] : block_voxel_layers_map_) { (void)block_index; - for (auto& voxel_layer : voxel_layers) { - if (!voxel_layer) { - continue; - } - voxel_layer->clear(); - if (voxel_layer->getParentSceneNode()) { - voxel_layer->getParentSceneNode()->detachObject(voxel_layer.get()); - } - } + detachVoxelLayers(voxel_layers); } block_voxel_layers_map_.clear(); + discrete_layer_metadata_.reset(); + discrete_parent_cells_.clear(); + discrete_batch_parents_.clear(); +} + +void VoxelVisual::detachVoxelLayers(VoxelLayers& voxel_layers) { + for (auto& voxel_layer : voxel_layers) { + if (!voxel_layer) { + continue; + } + voxel_layer->clear(); + if (voxel_layer->getParentSceneNode()) { + voxel_layer->getParentSceneNode()->detachObject(voxel_layer.get()); + } + } } void VoxelVisual::updateMap(bool redraw_all) { @@ -197,6 +189,7 @@ void VoxelVisual::updateMap(bool redraw_all) { it != block_voxel_layers_map_.end();) { const auto block_idx = it->first; if (!hashed_map->hasBlock(block_idx)) { + detachVoxelLayers(it->second); it = block_voxel_layers_map_.erase(it); } else { ++it; @@ -394,12 +387,81 @@ void VoxelVisual::flatColorUpdateCallback() { void VoxelVisual::layerColorUpdateCallback() { ProfilerZoneScoped; - scalar_low_color_ = scalar_low_color_property_.getOgreColor(); - scalar_high_color_ = scalar_high_color_property_.getOgreColor(); - bool_true_color_ = bool_true_color_property_.getOgreColor(); updateMap(true); + if (layer_color_changed_callback_) { + layer_color_changed_callback_(); + } +} + +Ogre::ColourValue VoxelVisual::defaultCategoryColor(int value) { + const uint32_t hash = static_cast(value) * 2654435761u; + const float r = 0.25f + + 0.75f * static_cast((hash >> 16u) & 0xffu) / 255.f; + const float g = 0.25f + + 0.75f * static_cast((hash >> 8u) & 0xffu) / 255.f; + const float b = + 0.25f + 0.75f * static_cast(hash & 0xffu) / 255.f; + return Ogre::ColourValue(r, g, b, 1.f); +} + +void VoxelVisual::configureLayerAppearance( + const std::string& layer_name, const std::string& layer_type, + bool is_discrete, FloatingPoint scalar_min, FloatingPoint scalar_max, + bool has_low_color, const Ogre::ColourValue& low_color, + bool has_high_color, const Ogre::ColourValue& high_color, + const std::vector& categories) { + const bool layer_changed = selected_color_layer_ != layer_name; + selected_color_layer_ = layer_name; + const bool is_occupancy = layer_name == "occupancy"; + const bool is_scalar = + !is_discrete && !is_occupancy && layer_type == "float32"; + + color_mode_property_.setHidden(!is_occupancy); + flat_color_property_.setHidden(!is_occupancy || + voxel_color_mode_ != VoxelColorMode::kFlat); + scalar_min_property_.setHidden(!is_scalar); + scalar_max_property_.setHidden(!is_scalar); + + if (is_scalar && layer_changed) { + const QSignalBlocker min_blocker(&scalar_min_property_); + const QSignalBlocker max_blocker(&scalar_max_property_); + scalar_min_property_.setFloat(scalar_min); + scalar_max_property_.setFloat(scalar_max); + const int layer_hash = + static_cast(std::hash{}(layer_name)); + scalar_low_color_ = + has_low_color ? low_color : defaultCategoryColor(layer_hash); + scalar_high_color_ = has_high_color + ? high_color + : defaultCategoryColor(layer_hash ^ 0x5bd1e995); + } + + bool_false_color_ = defaultCategoryColor(0); + bool_true_color_ = defaultCategoryColor(1); + category_colors_.clear(); + for (const auto& category : categories) { + category_colors_[category.value] = + category.has_color + ? Ogre::ColourValue(category.r, category.g, category.b, 1.f) + : defaultCategoryColor(category.value); + } } +Ogre::ColourValue VoxelVisual::categoryColor(int value) const { + const auto it = category_colors_.find(value); + return it == category_colors_.end() ? defaultCategoryColor(value) + : it->second; +} + +bool VoxelVisual::categoryVisible(int /*value*/) const { return true; } + +Ogre::ColourValue VoxelVisual::boolColor(bool value) const { + return value ? bool_true_color_ : bool_false_color_; +} + +bool VoxelVisual::boolVisible(bool /*value*/) const { return true; } + + void VoxelVisual::appendLeafCenterAndColor(int tree_height, FloatingPoint min_cell_width, const OctreeIndex& cell_index, @@ -454,8 +516,11 @@ void VoxelVisual::appendLayeredLeafCenterAndColor( } Ogre::ColourValue layer_color; - if (!layered_map.getLayerColor(selected_layer_name, cell_index, cell_log_odds, - layer_color)) { + if (!layered_map.getLayerColor( + selected_layer_name, cell_index, cell_log_odds, + scalar_min_property_.getFloat(), scalar_max_property_.getFloat(), + scalar_low_color_, scalar_high_color_, + layer_color)) { appendLeafCenterAndColor(tree_height, min_cell_width, cell_index, cell_log_odds, voxels_per_level); return; @@ -531,6 +596,7 @@ void VoxelVisual::drawLayeredMapOccupancy(const LayeredMapInterface& layered_map for (auto it = block_voxel_layers_map_.begin(); it != block_voxel_layers_map_.end();) { if (!allocated_blocks.count(it->first)) { + detachVoxelLayers(it->second); it = block_voxel_layers_map_.erase(it); } else { ++it; @@ -580,6 +646,7 @@ void VoxelVisual::drawLayeredMapOccupancy(const wavemap_msgs::LayeredHashedWavel } for (auto it = block_voxel_layers_map_.begin(); it != block_voxel_layers_map_.end();) { if (!allocated_blocks.count(it->first)) { + detachVoxelLayers(it->second); it = block_voxel_layers_map_.erase(it); } else { ++it; @@ -598,14 +665,6 @@ void VoxelVisual::drawLayeredMapOccupancy(const wavemap_msgs::LayeredHashedWavel } namespace { -Ogre::ColourValue stableIntColor(int value) { - uint32_t hash = static_cast(value) * 2654435761u; - const float r = 0.25f + 0.75f * static_cast((hash >> 16u) & 0xffu) / 255.f; - const float g = 0.25f + 0.75f * static_cast((hash >> 8u) & 0xffu) / 255.f; - const float b = 0.25f + 0.75f * static_cast(hash & 0xffu) / 255.f; - return Ogre::ColourValue(r, g, b, 1.f); -} - int continuousLayerComponentCount(const std::string& type) { if (type == "float32_rgb" || type == "float32_vec3") { return 3; @@ -680,117 +739,162 @@ bool VoxelVisual::getGenericContinuousLayerColor( return false; } +Index3D VoxelVisual::discreteRenderBatchIndex(const Index3D& parent_index) { + constexpr int kParentsPerRenderBatchSide = 16; + const auto floor_divide = [](int value, int divisor) { + const int quotient = value / divisor; + const int remainder = value % divisor; + return quotient - (remainder < 0 ? 1 : 0); + }; + return {floor_divide(parent_index.x(), kParentsPerRenderBatchSide), + floor_divide(parent_index.y(), kParentsPerRenderBatchSide), + floor_divide(parent_index.z(), kParentsPerRenderBatchSide)}; +} + void VoxelVisual::drawDiscreteLayer( const wavemap_msgs::DiscreteLayer& discrete_layer_msg, FloatingPoint min_cell_width) { ProfilerZoneScoped; - const FloatingPoint alpha = opacity_property_.getFloat(); - const int side_length = 1 << discrete_layer_msg.block_height; - const IndexElement termination_height = std::min( - termination_height_property_.getInt(), discrete_layer_msg.block_height); - const bool draw_parent_cells = - discrete_layer_msg.block_height <= termination_height; - - termination_height_property_.setMax(discrete_layer_msg.block_height); + termination_height_property_.setMax(0); block_update_queue_.clear(); - std::unordered_map, Index3DHash> cells_by_parent; - std::unordered_set allocated_parents; - for (const auto& cell_msg : discrete_layer_msg.cells) { - const Index3D parent_index{cell_msg.parent_index.x, cell_msg.parent_index.y, - cell_msg.parent_index.z}; - allocated_parents.insert(parent_index); - auto& cells = cells_by_parent[parent_index]; - - if (draw_parent_cells) { - Ogre::ColourValue color; - if (!getDiscreteCellColor(discrete_layer_msg, cell_msg, - /*exception_index=*/-1, - /*is_exception=*/false, color)) { - continue; - } - - auto& cell = cells.emplace_back(); - const FloatingPoint parent_width = - static_cast(side_length) * min_cell_width; - cell.center = Ogre::Vector3( - (static_cast(parent_index.x()) + 0.5f) * parent_width, - (static_cast(parent_index.y()) + 0.5f) * parent_width, - (static_cast(parent_index.z()) + 0.5f) * parent_width); - cell.color = color; - continue; - } - - std::unordered_map exception_indices; - for (size_t exception_i = 0u; - exception_i < cell_msg.exception_offsets.size(); ++exception_i) { - exception_indices[cell_msg.exception_offsets[exception_i]] = - static_cast(exception_i); - } - - for (const int offset : cell_msg.observed_offsets) { - const auto exception_it = exception_indices.find(offset); - const bool is_exception = exception_it != exception_indices.end(); - const int exception_index = is_exception ? exception_it->second : -1; + for (auto& [batch_index, voxel_layers] : block_voxel_layers_map_) { + (void)batch_index; + detachVoxelLayers(voxel_layers); + } + block_voxel_layers_map_.clear(); + discrete_parent_cells_.clear(); + discrete_batch_parents_.clear(); + discrete_layer_metadata_ = discrete_layer_msg; + discrete_layer_metadata_->cells.clear(); + discrete_layer_metadata_->deleted_parent_indices.clear(); + + std::unordered_set affected_batches; + for (const auto& cell : discrete_layer_msg.cells) { + const Index3D parent_index{cell.parent_index.x, cell.parent_index.y, + cell.parent_index.z}; + const Index3D batch_index = discreteRenderBatchIndex(parent_index); + discrete_parent_cells_[parent_index] = cell; + discrete_batch_parents_[batch_index].insert(parent_index); + affected_batches.insert(batch_index); + } + rebuildDiscreteBatches(affected_batches, min_cell_width); +} - Ogre::ColourValue color; - if (!getDiscreteCellColor(discrete_layer_msg, cell_msg, exception_index, - is_exception, color)) { - continue; +void VoxelVisual::applyDiscreteLayerPatch( + const wavemap_msgs::DiscreteLayer& patch, FloatingPoint min_cell_width) { + ProfilerZoneScoped; + if (!discrete_layer_metadata_ || + discrete_layer_metadata_->name != patch.name) { + return; + } + discrete_layer_metadata_->value_type = patch.value_type; + discrete_layer_metadata_->block_height = patch.block_height; + discrete_layer_metadata_->categories = patch.categories; + + std::unordered_set affected_batches; + const auto erase_parent = [&](const Index3D& parent_index) { + const Index3D batch_index = discreteRenderBatchIndex(parent_index); + affected_batches.insert(batch_index); + discrete_parent_cells_.erase(parent_index); + const auto batch_it = discrete_batch_parents_.find(batch_index); + if (batch_it != discrete_batch_parents_.end()) { + batch_it->second.erase(parent_index); + if (batch_it->second.empty()) { + discrete_batch_parents_.erase(batch_it); } - - const int local_x = offset % side_length; - const int local_y = (offset / side_length) % side_length; - const int local_z = offset / (side_length * side_length); - const FloatingPoint x = - (cell_msg.parent_index.x * side_length + local_x + 0.5f) * - min_cell_width; - const FloatingPoint y = - (cell_msg.parent_index.y * side_length + local_y + 0.5f) * - min_cell_width; - const FloatingPoint z = - (cell_msg.parent_index.z * side_length + local_z + 0.5f) * - min_cell_width; - - auto& cell = cells.emplace_back(); - cell.center = Ogre::Vector3(x, y, z); - cell.color = color; } + }; + + for (const auto& deleted : patch.deleted_parent_indices) { + erase_parent(Index3D{deleted.x, deleted.y, deleted.z}); + } + for (const auto& cell : patch.cells) { + const Index3D parent_index{cell.parent_index.x, cell.parent_index.y, + cell.parent_index.z}; + const Index3D batch_index = discreteRenderBatchIndex(parent_index); + affected_batches.insert(batch_index); + discrete_parent_cells_[parent_index] = cell; + discrete_batch_parents_[batch_index].insert(parent_index); } + rebuildDiscreteBatches(affected_batches, min_cell_width); +} - for (auto it = block_voxel_layers_map_.begin(); - it != block_voxel_layers_map_.end();) { - if (!allocated_parents.count(it->first)) { - it = block_voxel_layers_map_.erase(it); - } else { - ++it; - } +void VoxelVisual::rebuildDiscreteBatches( + const std::unordered_set& batch_indices, + FloatingPoint min_cell_width) { + if (!discrete_layer_metadata_) { + return; } + const auto& layer_msg = *discrete_layer_metadata_; + const FloatingPoint alpha = opacity_property_.getFloat(); + const int side_length = 1 << layer_msg.block_height; + + for (const Index3D& batch_index : batch_indices) { + std::vector cells; + const auto batch_it = discrete_batch_parents_.find(batch_index); + if (batch_it != discrete_batch_parents_.end()) { + for (const Index3D& parent_index : batch_it->second) { + const auto parent_it = discrete_parent_cells_.find(parent_index); + if (parent_it == discrete_parent_cells_.end()) { + continue; + } + const auto& cell_msg = parent_it->second; + std::unordered_map exception_indices; + for (size_t exception_i = 0u; + exception_i < cell_msg.exception_offsets.size(); ++exception_i) { + exception_indices[cell_msg.exception_offsets[exception_i]] = + static_cast(exception_i); + } + for (const int offset : cell_msg.observed_offsets) { + const auto exception_it = exception_indices.find(offset); + const bool is_exception = exception_it != exception_indices.end(); + const int exception_index = + is_exception ? exception_it->second : -1; + Ogre::ColourValue color; + if (!getDiscreteCellColor(layer_msg, cell_msg, exception_index, + is_exception, color)) { + continue; + } + const int local_x = offset % side_length; + const int local_y = (offset / side_length) % side_length; + const int local_z = offset / (side_length * side_length); + auto& cell = cells.emplace_back(); + cell.center = Ogre::Vector3( + (parent_index.x() * side_length + local_x + 0.5f) * + min_cell_width, + (parent_index.y() * side_length + local_y + 0.5f) * + min_cell_width, + (parent_index.z() * side_length + local_z + 0.5f) * + min_cell_width); + cell.color = color; + } + } + } - const FloatingPoint cell_width = - draw_parent_cells ? static_cast(side_length) * min_cell_width - : min_cell_width; - for (auto& [parent_index, cells] : cells_by_parent) { if (cells.empty()) { - block_voxel_layers_map_.erase(parent_index); + const auto visual_it = block_voxel_layers_map_.find(batch_index); + if (visual_it != block_voxel_layers_map_.end()) { + detachVoxelLayers(visual_it->second); + block_voxel_layers_map_.erase(visual_it); + } continue; } - auto& voxel_layers = block_voxel_layers_map_[parent_index]; + auto& voxel_layers = block_voxel_layers_map_[batch_index]; if (voxel_layers.empty()) { auto& voxel_layer = voxel_layers.emplace_back( std::make_unique(voxel_material_)); - voxel_layer->setName("discrete_layer_" + discrete_layer_msg.name + "_" + - std::to_string(Index3DHash()(parent_index))); + voxel_layer->setName("discrete_layer_" + layer_msg.name + "_" + + std::to_string(Index3DHash()(batch_index))); frame_node_->attachObject(voxel_layer.get()); } - auto& voxel_layer = voxel_layers.front(); - voxel_layer->setCellDimensions(cell_width, cell_width, cell_width); + voxel_layer->setCellDimensions(min_cell_width, min_cell_width, + min_cell_width); voxel_layer->setAlpha(alpha); voxel_layer->setCells(cells); } - num_queued_blocks_indicator_.setInt(0); } @@ -802,17 +906,20 @@ bool VoxelVisual::getDiscreteCellColor( const bool value = is_exception ? cell_msg.exception_uint8_values[exception_index] != 0u : cell_msg.dominant_uint8 != 0u; - if (!value && !show_bool_false_property_.getBool()) { + if (!boolVisible(value)) { return false; } - color = value ? bool_true_color_ : Ogre::ColourValue(0.35f, 0.35f, 0.35f, 1.f); + color = boolColor(value); return true; } if (layer_msg.value_type == "int") { const int value = is_exception ? cell_msg.exception_int32_values[exception_index] : cell_msg.dominant_int32; - color = stableIntColor(value); + if (!categoryVisible(value)) { + return false; + } + color = categoryColor(value); return true; } 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 33c67589d..21e76b8c6 100644 --- a/interfaces/ros1/wavemap_rviz_plugin/src/wavemap_map_display.cc +++ b/interfaces/ros1/wavemap_rviz_plugin/src/wavemap_map_display.cc @@ -168,7 +168,9 @@ void WavemapMapDisplay::updateLayerMetadataFromRosMsg(const wavemap_msgs::Map& m const auto& layered_map_msg = map_msg.layered_hashed_wavelet_octree.front(); const size_t num_layers = std::min(layered_map_msg.layer_names.size(), layered_map_msg.layer_types.size()); for (size_t layer_idx = 0u; layer_idx < num_layers; ++layer_idx) { - available_layers_.push_back({layered_map_msg.layer_names[layer_idx], layered_map_msg.layer_types[layer_idx]}); + available_layers_.push_back({layered_map_msg.layer_names[layer_idx], + layered_map_msg.layer_types[layer_idx], + {}, {}}); layer_property_.addOption(QString::fromStdString(layered_map_msg.layer_names[layer_idx])); } diff --git a/library/cpp/include/wavemap/core/data_structure/pointcloud.h b/library/cpp/include/wavemap/core/data_structure/pointcloud.h index fcf731555..79d8c464a 100644 --- a/library/cpp/include/wavemap/core/data_structure/pointcloud.h +++ b/library/cpp/include/wavemap/core/data_structure/pointcloud.h @@ -38,6 +38,10 @@ class Pointcloud { CHECK_LT(n_points, std::numeric_limits::max()); data_.resize(kDim, n_points); } + void conservativeResize(size_t n_points) { + CHECK_LT(n_points, std::numeric_limits::max()); + data_.conservativeResize(Eigen::NoChange, n_points); + } void clear() { data_.resize(kDim, 0); } typename Data::ColXpr operator[](Eigen::Index point_index) { 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 4992ed1f6..f8c667ac1 100644 --- a/library/cpp/include/wavemap/core/map/hashed_wavelet_octree.h +++ b/library/cpp/include/wavemap/core/map/hashed_wavelet_octree.h @@ -111,7 +111,10 @@ class HashedWaveletOctreeT : public MapBase { Index3D getBlockSize() const { return Index3D::Constant(cells_per_block_side_); } - const HashedWaveletOctreeConfig& getConfig() { return config_; } + const HashedWaveletOctreeConfig& getConfig() const { return config_; } + const ThresholdConfig& getThresholdConfig() const { + return threshold_config_; + } FloatingPoint getCellValue(const Index3D& index) const override; FloatingPoint getCellValue(const OctreeIndex& index) const; diff --git a/library/cpp/include/wavemap/core/utils/undistortion/stamped_pointcloud.h b/library/cpp/include/wavemap/core/utils/undistortion/stamped_pointcloud.h index aff568c0c..e19c8abaf 100644 --- a/library/cpp/include/wavemap/core/utils/undistortion/stamped_pointcloud.h +++ b/library/cpp/include/wavemap/core/utils/undistortion/stamped_pointcloud.h @@ -13,10 +13,13 @@ namespace wavemap::undistortion { struct StampedPoint { Point3D position; TimeOffset time_offset; + size_t source_index; StampedPoint(FloatingPoint x, FloatingPoint y, FloatingPoint z, - TimeOffset time_offset) - : position(x, y, z), time_offset(time_offset) {} + TimeOffset time_offset, size_t source_index = 0u) + : position(x, y, z), + time_offset(time_offset), + source_index(source_index) {} std::string toStr() const; }; diff --git a/library/cpp/include/wavemap/layered/continuous_layer_updater.h b/library/cpp/include/wavemap/layered/continuous_layer_updater.h new file mode 100644 index 000000000..fc50a0525 --- /dev/null +++ b/library/cpp/include/wavemap/layered/continuous_layer_updater.h @@ -0,0 +1,38 @@ +#ifndef WAVEMAP_LAYERED_CONTINUOUS_LAYER_UPDATER_H_ +#define WAVEMAP_LAYERED_CONTINUOUS_LAYER_UPDATER_H_ + +#include + +#include + +namespace wavemap::layered { + +// Mutates only the additional continuous data and applies the resulting delta +// with zero occupancy. This preserves occupancy and every field the mutator +// leaves unchanged while still updating the wavelet representation correctly. +template +void updateContinuousLayersAtVoxel( + ContinuousMapT& map, const wavemap::Index3D& index, MutatorT&& mutator) { + using Voxel = typename ContinuousMapT::CellDataType; + using ContinuousPolicy = typename Voxel::AdditionalDataPolicy; + + const Voxel current_voxel = map.getVoxelValue(index); + auto updated_data = current_voxel.data; + std::forward(mutator)(updated_data); + + const auto data_update = + ContinuousPolicy::subtract(updated_data, current_voxel.data); + map.addToVoxelValue(index, Voxel(0.f, data_update)); +} + +template +void setContinuousLayersAtVoxel( + ContinuousMapT& map, const wavemap::Index3D& index, + const typename ContinuousMapT::CellDataType::AdditionalData& data) { + updateContinuousLayersAtVoxel( + map, index, [&data](auto& current_data) { current_data = data; }); +} + +} // namespace wavemap::layered + +#endif // WAVEMAP_LAYERED_CONTINUOUS_LAYER_UPDATER_H_ diff --git a/library/cpp/include/wavemap/layered/descriptor_layered_map_io.h b/library/cpp/include/wavemap/layered/descriptor_layered_map_io.h new file mode 100644 index 000000000..12ab431d8 --- /dev/null +++ b/library/cpp/include/wavemap/layered/descriptor_layered_map_io.h @@ -0,0 +1,113 @@ +#ifndef WAVEMAP_LAYERED_DESCRIPTOR_LAYERED_MAP_IO_H_ +#define WAVEMAP_LAYERED_DESCRIPTOR_LAYERED_MAP_IO_H_ + +#include +#include + +#include +#include +#include +#include +#include +#include + +namespace wavemap::layered { + +template +std::vector descriptorContinuousLayerSchema() { + std::vector schema; + const auto descriptors = layerDescriptors(); + schema.reserve(std::tuple_size_v); + std::apply( + [&](const auto&... descriptor) { + (schema.emplace_back( + LayerSchemaEntry{ + std::string(descriptor.name()), + LayerStreamCodec< + std::remove_cv_t()))>>>:: + typeName()}), + ...); + }, + descriptors); + return schema; +} + +template +struct DescriptorVoxelSerializer { + static void write(std::ostream& ostream, const VoxelT& voxel) { + wavemap::io::StreamableCellData::write(ostream, voxel.occupancy); + forEachLayerDescriptor( + voxel.data, [&](const auto& /*descriptor*/, const auto& value) { + using Value = + std::remove_cv_t>; + LayerStreamCodec::write(ostream, value); + }); + } + + static VoxelT read(std::istream& istream) { + VoxelT voxel; + voxel.occupancy = + wavemap::io::StreamableCellData::read(istream); + forEachLayerDescriptor( + voxel.data, [&](const auto& /*descriptor*/, auto& value) { + using Value = + std::remove_cv_t>; + value = LayerStreamCodec::read(istream); + }); + return voxel; + } +}; + +template +auto descriptorNamedDiscreteLayers(DiscreteLayersT& layers) { + return transformLayerDescriptors( + layerDescriptors(), [&](const auto& descriptor) { + return io::namedDiscreteLayer(std::string(descriptor.name()), + descriptor.value(layers)); + }); +} + +template +auto descriptorNamedDiscreteLayers(const DiscreteLayersT& layers) { + return transformLayerDescriptors( + layerDescriptors(), [&](const auto& descriptor) { + return io::namedDiscreteLayer(std::string(descriptor.name()), + descriptor.value(layers)); + }); +} + +template +struct ContinuousLayerSchemaTraits< + schema::ContinuousLayerBundle>> { + using Bundle = + schema::ContinuousLayerBundle>; + + static std::vector layers() { + return descriptorContinuousLayerSchema(); + } +}; + +} // namespace wavemap::layered + +namespace wavemap::layered::io { + +template +struct DiscreteLayerBundleTraits< + schema::DiscreteLayerBundle>> { + using Bundle = + schema::DiscreteLayerBundle>; + + static auto layers(Bundle& bundle) { + return descriptorNamedDiscreteLayers(bundle); + } + + static auto layers(const Bundle& bundle) { + return descriptorNamedDiscreteLayers(bundle); + } +}; + +} // namespace wavemap::layered::io + +#endif // WAVEMAP_LAYERED_DESCRIPTOR_LAYERED_MAP_IO_H_ diff --git a/library/cpp/include/wavemap/layered/discrete_layer.h b/library/cpp/include/wavemap/layered/discrete_layer.h index e33456110..f9e26aaaf 100644 --- a/library/cpp/include/wavemap/layered/discrete_layer.h +++ b/library/cpp/include/wavemap/layered/discrete_layer.h @@ -6,6 +6,7 @@ #include #include #include +#include #include @@ -144,10 +145,70 @@ class DiscreteLayer { } void setValue(const wavemap::Index3D& index, const ValueT& value) { - Cell& cell = cells_[parentKey(index, config_)]; - std::map values = expandCell(cell); - values[childOffset(index, config_)] = value; - rebuildCell(values, cell); + const IndexKey parent_key = parentKey(index, config_); + const int offset = childOffset(index, config_); + const auto cell_it = cells_.find(parent_key); + std::map values = + cell_it == cells_.end() ? std::map{} + : expandCell(cell_it->second); + const auto value_it = values.find(offset); + if (value_it != values.end() && value_it->second == value) { + return; + } + values[offset] = value; + rebuildCell(values, cells_[parent_key]); + dirty_parent_keys_.insert(parent_key); + } + + void setValues( + const std::vector>& updates) { + std::map> grouped_updates; + for (const auto& [index, value] : updates) { + grouped_updates[parentKey(index, config_)][childOffset(index, config_)] = + value; + } + + for (const auto& [parent_key, parent_updates] : grouped_updates) { + const auto cell_it = cells_.find(parent_key); + std::map values = + cell_it == cells_.end() ? std::map{} + : expandCell(cell_it->second); + bool changed = false; + for (const auto& [offset, value] : parent_updates) { + const auto value_it = values.find(offset); + if (value_it != values.end() && value_it->second == value) { + continue; + } + values[offset] = value; + changed = true; + } + if (!changed) { + continue; + } + rebuildCell(values, cells_[parent_key]); + dirty_parent_keys_.insert(parent_key); + } + } + + bool eraseValue(const wavemap::Index3D& index) { + const IndexKey parent_key = parentKey(index, config_); + const auto cell_it = cells_.find(parent_key); + if (cell_it == cells_.end()) { + return false; + } + + std::map values = expandCell(cell_it->second); + if (values.erase(childOffset(index, config_)) == 0u) { + return false; + } + + if (values.empty()) { + cells_.erase(cell_it); + } else { + rebuildCell(values, cell_it->second); + } + dirty_parent_keys_.insert(parent_key); + return true; } std::optional getValue(const wavemap::Index3D& index) const { @@ -187,6 +248,14 @@ class DiscreteLayer { return count; } + void clear() { + for (const auto& [parent_key, cell] : cells_) { + (void)cell; + dirty_parent_keys_.insert(parent_key); + } + cells_.clear(); + } + std::optional dominantValue(const wavemap::Index3D& index) const { const auto cell_it = cells_.find(parentKey(index, config_)); if (cell_it == cells_.end()) { @@ -199,10 +268,17 @@ class DiscreteLayer { const std::map& cells() const { return cells_; } + const std::set& dirtyParentKeys() const { + return dirty_parent_keys_; + } + + void clearDirtyParentKeys() { dirty_parent_keys_.clear(); } + void replaceCellsForLoad(DiscreteCompressionConfig config, std::map cells) { config_ = config; cells_ = std::move(cells); + dirty_parent_keys_.clear(); } private: @@ -243,6 +319,7 @@ class DiscreteLayer { DiscreteCompressionConfig config_; std::map cells_; + std::set dirty_parent_keys_; }; } // namespace wavemap::layered diff --git a/library/cpp/include/wavemap/layered/endpoint_attribute_integrator.h b/library/cpp/include/wavemap/layered/endpoint_attribute_integrator.h new file mode 100644 index 000000000..c7b423170 --- /dev/null +++ b/library/cpp/include/wavemap/layered/endpoint_attribute_integrator.h @@ -0,0 +1,92 @@ +#ifndef WAVEMAP_LAYERED_ENDPOINT_ATTRIBUTE_INTEGRATOR_H_ +#define WAVEMAP_LAYERED_ENDPOINT_ATTRIBUTE_INTEGRATOR_H_ + +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +namespace wavemap::layered { + +struct EndpointAttributeIntegrationResult { + size_t received = 0u; + size_t integrated = 0u; + size_t rejected = 0u; +}; + +// Adds endpoint-only continuous fields around an existing occupancy integrator. +// Occupancy remains entirely owned by the wrapped Wavemap integrator. +template +class EndpointAttributeIntegrator { + public: + using Measurement = + PosedEndpointAttributePointcloud; + + EndpointAttributeIntegrator(LayeredMapT& map, + IntegratorBase& occupancy_integrator) + : map_(map), occupancy_integrator_(occupancy_integrator) { + static_assert((kIsWaveletCompatibleLayer && ...), + "Endpoint sensor fields must be continuous layers."); + } + + EndpointAttributeIntegrationResult integrate(const Measurement& measurement) { + EndpointAttributeIntegrationResult result; + result.received = measurement.size(); + if (!measurement.hasConsistentSizes()) { + result.rejected = result.received; + return result; + } + + // Preserve the original occupancy path and its measurement model exactly. + const PosedPointcloud<> occupancy_measurement(measurement.getPose(), + measurement.points().data()); + occupancy_integrator_.integrate(occupancy_measurement); + + const FloatingPoint cell_width_inv = + 1.f / map_.continuousMap().getMinCellWidth(); + for (size_t point_idx = 0u; point_idx < measurement.size(); ++point_idx) { + const Point3D C_endpoint = measurement.point(point_idx); + if (!C_endpoint.array().isFinite().all()) { + ++result.rejected; + continue; + } + const Point3D W_endpoint = measurement.getPose() * C_endpoint; + const Index3D endpoint_index = + convert::pointToNearestIndex(W_endpoint, cell_width_inv); + updateContinuousLayersAtVoxel( + map_.continuousMap(), endpoint_index, [&](auto& layers) { + (updateLayer(layers, measurement, point_idx, + W_endpoint), + ...); + }); + ++result.integrated; + } + return result; + } + + private: + template + static void updateLayer(ContinuousLayersT& layers, + const Measurement& measurement, size_t point_idx, + const Point3D& W_endpoint) { + constexpr auto descriptor = + detail::descriptorForLayer(); + auto& current_value = descriptor.value(layers); + const LayerObservation> observation( + W_endpoint, measurement.template attribute(point_idx)); + current_value = LayerUpdatePolicyT{}(current_value, observation); + } + + LayeredMapT& map_; + IntegratorBase& occupancy_integrator_; +}; + +} // namespace wavemap::layered + +#endif // WAVEMAP_LAYERED_ENDPOINT_ATTRIBUTE_INTEGRATOR_H_ diff --git a/library/cpp/include/wavemap/layered/endpoint_attribute_pointcloud.h b/library/cpp/include/wavemap/layered/endpoint_attribute_pointcloud.h new file mode 100644 index 000000000..0cef3af84 --- /dev/null +++ b/library/cpp/include/wavemap/layered/endpoint_attribute_pointcloud.h @@ -0,0 +1,94 @@ +#ifndef WAVEMAP_LAYERED_ENDPOINT_ATTRIBUTE_POINTCLOUD_H_ +#define WAVEMAP_LAYERED_ENDPOINT_ATTRIBUTE_POINTCLOUD_H_ + +#include +#include +#include + +#include +#include + +namespace wavemap::layered { +namespace detail { +template +struct EndpointAttributeSlot { + std::vector> values; +}; +} // namespace detail + +// Structure-of-arrays point cloud whose endpoint attributes remain aligned by +// point index. Positions use the existing Pointcloud format and every +// configured attribute uses one contiguous typed array. +template +class EndpointAttributePointcloud + : private detail::EndpointAttributeSlot... { + public: + EndpointAttributePointcloud() = default; + explicit EndpointAttributePointcloud(size_t size) { resize(size); } + + bool empty() const { return points_.empty(); } + size_t size() const { return points_.size(); } + + void resize(size_t size) { + points_.resize(size); + (attributeValues().resize(size), ...); + } + + void clear() { + points_.clear(); + (attributeValues().clear(), ...); + } + + void truncate(size_t size) { + CHECK_LE(size, this->size()); + points_.conservativeResize(size); + (attributeValues().resize(size), ...); + } + + Pointcloud<>& points() { return points_; } + const Pointcloud<>& points() const { return points_; } + + auto point(size_t index) { return points_[index]; } + auto point(size_t index) const { return points_[index]; } + + template + std::vector>& attributeValues() { + static_assert((std::is_same_v || ...), + "Endpoint attribute is not present in this point cloud."); + return static_cast&>(*this) + .values; + } + + template + const std::vector>& attributeValues() const { + static_assert((std::is_same_v || ...), + "Endpoint attribute is not present in this point cloud."); + return static_cast&>(*this) + .values; + } + + template + LayerValueT& attribute(size_t index) { + return attributeValues()[index]; + } + + template + const LayerValueT& attribute(size_t index) const { + return attributeValues()[index]; + } + + bool hasConsistentSizes() const { + return ((attributeValues().size() == size()) && ...); + } + + private: + Pointcloud<> points_; +}; + +template +using PosedEndpointAttributePointcloud = + PosedObject>; + +} // namespace wavemap::layered + +#endif // WAVEMAP_LAYERED_ENDPOINT_ATTRIBUTE_POINTCLOUD_H_ diff --git a/library/cpp/include/wavemap/layered/endpoint_range.h b/library/cpp/include/wavemap/layered/endpoint_range.h new file mode 100644 index 000000000..0ac98832c --- /dev/null +++ b/library/cpp/include/wavemap/layered/endpoint_range.h @@ -0,0 +1,30 @@ +#ifndef WAVEMAP_LAYERED_ENDPOINT_RANGE_H_ +#define WAVEMAP_LAYERED_ENDPOINT_RANGE_H_ + +#include +#include + +#include + +namespace wavemap::layered { + +struct EndpointRange { + FloatingPoint min = 0.f; + FloatingPoint max = std::numeric_limits::infinity(); + + bool isValid() const { + return 0.f <= min && min <= max && !std::isnan(max); + } + + bool contains(const Point3D& sensor_point) const { + if (!sensor_point.array().isFinite().all()) { + return false; + } + const FloatingPoint squared_range = sensor_point.squaredNorm(); + return min * min <= squared_range && squared_range <= max * max; + } +}; + +} // namespace wavemap::layered + +#endif // WAVEMAP_LAYERED_ENDPOINT_RANGE_H_ diff --git a/library/cpp/include/wavemap/layered/layer_bundle_traits.h b/library/cpp/include/wavemap/layered/layer_bundle_traits.h new file mode 100644 index 000000000..ed89a4c95 --- /dev/null +++ b/library/cpp/include/wavemap/layered/layer_bundle_traits.h @@ -0,0 +1,40 @@ +#ifndef WAVEMAP_LAYERED_LAYER_BUNDLE_TRAITS_H_ +#define WAVEMAP_LAYERED_LAYER_BUNDLE_TRAITS_H_ + +#include +#include +#include + +namespace wavemap::layered { +namespace detail { +template +inline constexpr bool kLayerBundleTraitsAlwaysFalse = false; + +template +constexpr void forEachLayerTagImpl(FunctionT&& function, + std::index_sequence) { + (function(std::tuple_element_t{}), ...); +} +} // namespace detail + +// Users specialize this trait and expose LayerTags as a tuple of layer tags. +// The metadata is storage-independent so it can later drive integration, +// schema validation, ROS conversion, and visualization. +template +struct LayerBundleTraits { + static_assert( + detail::kLayerBundleTraitsAlwaysFalse, + "LayerBundleTraits must be specialized for each bundle."); +}; + +template +constexpr void forEachLayerTag(FunctionT&& function) { + using LayerTags = typename LayerBundleTraits::LayerTags; + detail::forEachLayerTagImpl( + std::forward(function), + std::make_index_sequence>{}); +} + +} // namespace wavemap::layered + +#endif // WAVEMAP_LAYERED_LAYER_BUNDLE_TRAITS_H_ diff --git a/library/cpp/include/wavemap/layered/layer_descriptor.h b/library/cpp/include/wavemap/layered/layer_descriptor.h new file mode 100644 index 000000000..02136d52d --- /dev/null +++ b/library/cpp/include/wavemap/layered/layer_descriptor.h @@ -0,0 +1,101 @@ +#ifndef WAVEMAP_LAYERED_LAYER_DESCRIPTOR_H_ +#define WAVEMAP_LAYERED_LAYER_DESCRIPTOR_H_ + +#include +#include +#include + +#include + +namespace wavemap::layered { +namespace detail { +template +inline constexpr bool kLayerDescriptorsAlwaysFalse = false; + +template +auto transformTupleImpl(TupleT&& tuple, FunctionT&& function, + std::index_sequence) { + return std::make_tuple( + function(std::get(std::forward(tuple)))...); +} +} // namespace detail + +template +struct LayerDescriptor { + using LayerTag = LayerTagT; + using Bundle = BundleT; + using Member = MemberT; + + MemberT BundleT::*member; + + static constexpr std::string_view name() { + return LayerTraits::name; + } + + MemberT& value(BundleT& bundle) const { return bundle.*member; } + const MemberT& value(const BundleT& bundle) const { return bundle.*member; } +}; + +template +constexpr auto layerDescriptor(MemberT BundleT::*member) { + return LayerDescriptor{member}; +} + +template +struct AccessorLayerDescriptor { + using LayerTag = LayerTagT; + using Bundle = BundleT; + using Member = LayerStateT; + + static constexpr std::string_view name() { + return LayerTraits::name; + } + + decltype(auto) value(BundleT& bundle) const { + return bundle.template get(); + } + decltype(auto) value(const BundleT& bundle) const { + return bundle.template get(); + } +}; + +template +constexpr auto accessorLayerDescriptor() { + return AccessorLayerDescriptor{}; +} + +// One specialization per bundle is the authoritative ordered layer list. +template +struct LayerDescriptors { + static_assert(detail::kLayerDescriptorsAlwaysFalse, + "LayerDescriptors must be specialized."); +}; + +template +constexpr auto layerDescriptors() { + return LayerDescriptors::descriptors(); +} + +template +auto transformLayerDescriptors(TupleT&& tuple, FunctionT&& function) { + constexpr size_t kTupleSize = + std::tuple_size_v>; + return detail::transformTupleImpl( + std::forward(tuple), std::forward(function), + std::make_index_sequence{}); +} + +template +void forEachLayerDescriptor(BundleT&& bundle, FunctionT&& function) { + const auto descriptors = + layerDescriptors>>(); + std::apply( + [&](const auto&... descriptor) { + (function(descriptor, descriptor.value(bundle)), ...); + }, + descriptors); +} + +} // namespace wavemap::layered + +#endif // WAVEMAP_LAYERED_LAYER_DESCRIPTOR_H_ diff --git a/library/cpp/include/wavemap/layered/layer_integrator.h b/library/cpp/include/wavemap/layered/layer_integrator.h new file mode 100644 index 000000000..33fb036a9 --- /dev/null +++ b/library/cpp/include/wavemap/layered/layer_integrator.h @@ -0,0 +1,146 @@ +#ifndef WAVEMAP_LAYERED_LAYER_INTEGRATOR_H_ +#define WAVEMAP_LAYERED_LAYER_INTEGRATOR_H_ + +#include +#include +#include +#include +#include + +#include + +#include +#include +#include +#include +#include + +namespace wavemap::layered { + +struct LayerIntegrationResult { + size_t received = 0u; + size_t integrated = 0u; + size_t rejected = 0u; + size_t updated_voxels = 0u; +}; + +namespace detail { + +inline bool isFinite(const Point3D& position) { + return position.array().isFinite().all(); +} + +template +inline constexpr bool kDescriptorMatchesLayer = + std::is_same_v; + +template +constexpr size_t descriptorMatchCount(std::index_sequence) { + return (size_t{0u} + ... + + (kDescriptorMatchesLayer< + LayerTagT, std::tuple_element_t> + ? size_t{1u} + : size_t{0u})); +} + +template +constexpr auto descriptorForLayer(const TupleT& descriptors) { + static_assert(Index < std::tuple_size_v, + "The requested layer is not present in this bundle."); + using Descriptor = std::tuple_element_t; + if constexpr (kDescriptorMatchesLayer) { + return std::get(descriptors); + } else { + return descriptorForLayer(descriptors); + } +} + +template +constexpr auto descriptorForLayer() { + constexpr auto descriptors = layerDescriptors(); + using DescriptorTuple = decltype(descriptors); + constexpr size_t kDescriptorCount = std::tuple_size_v; + static_assert( + descriptorMatchCount( + std::make_index_sequence{}) == 1u, + "Each layer tag must occur exactly once in its storage bundle."); + return descriptorForLayer(descriptors); +} + +template +ValueT applyDiscretePolicy(const PolicyT& policy, + const std::optional& current_value, + const LayerObservation& observation) { + if constexpr (std::is_invocable_r_v< + ValueT, PolicyT, const std::optional&, + const LayerObservation&>) { + return policy(current_value, observation); + } else { + return policy(current_value.value_or(ValueT{}), observation); + } +} + +} // namespace detail + +template +LayerIntegrationResult integrateLayerObservations( + LayeredMapT& map, + const std::vector>>& observations, + const LayerUpdatePolicyT& policy = + LayerUpdatePolicyT{}) { + using Value = LayerValueT; + LayerIntegrationResult result; + result.received = observations.size(); + + const FloatingPoint cell_width_inv = + 1.f / map.continuousMap().getMinCellWidth(); + std::map*>> grouped; + for (const auto& observation : observations) { + if (!detail::isFinite(observation.position)) { + ++result.rejected; + continue; + } + const Index3D index = + convert::pointToNearestIndex(observation.position, cell_width_inv); + grouped[IndexKey(index)].push_back(&observation); + ++result.integrated; + } + result.updated_voxels = grouped.size(); + + if constexpr (kIsWaveletCompatibleLayer) { + constexpr auto descriptor = + detail::descriptorForLayer< + LayerTagT, typename LayeredMapT::ContinuousLayers>(); + for (const auto& [index_key, voxel_observations] : grouped) { + updateContinuousLayersAtVoxel( + map.continuousMap(), indexFromKey(index_key), [&](auto& layers) { + auto& state = descriptor.value(layers); + for (const auto* observation : voxel_observations) { + state = policy(state, *observation); + } + }); + } + } else { + constexpr auto descriptor = + detail::descriptorForLayer< + LayerTagT, typename LayeredMapT::DiscreteLayers>(); + auto& layer = descriptor.value(map.discreteLayers()); + std::vector> updates; + updates.reserve(grouped.size()); + for (const auto& [index_key, voxel_observations] : grouped) { + const Index3D index = indexFromKey(index_key); + std::optional value = layer.getValue(index); + for (const auto* observation : voxel_observations) { + value = detail::applyDiscretePolicy(policy, value, *observation); + } + updates.emplace_back(index, *value); + } + layer.setValues(updates); + } + + return result; +} + +} // namespace wavemap::layered + +#endif // WAVEMAP_LAYERED_LAYER_INTEGRATOR_H_ diff --git a/library/cpp/include/wavemap/layered/layer_observation.h b/library/cpp/include/wavemap/layered/layer_observation.h new file mode 100644 index 000000000..0b57bd922 --- /dev/null +++ b/library/cpp/include/wavemap/layered/layer_observation.h @@ -0,0 +1,34 @@ +#ifndef WAVEMAP_LAYERED_LAYER_OBSERVATION_H_ +#define WAVEMAP_LAYERED_LAYER_OBSERVATION_H_ + +#include +#include +#include + +#include +#include + +namespace wavemap::layered { + +// A sensor-independent observation of one layer at a spatial position. +// Observation generation stays outside the map. The update policy decides how +// this value is fused. +template +struct LayerObservation { + using Value = ValueT; + + Point3D position = Point3D::Zero(); + ValueT value{}; + std::optional confidence; + std::optional timestamp; + std::string source; + + LayerObservation() = default; + LayerObservation(Point3D observed_position, ValueT observed_value) + : position(std::move(observed_position)), + value(std::move(observed_value)) {} +}; + +} // namespace wavemap::layered + +#endif // WAVEMAP_LAYERED_LAYER_OBSERVATION_H_ diff --git a/library/cpp/include/wavemap/layered/layer_observation_batch.h b/library/cpp/include/wavemap/layered/layer_observation_batch.h new file mode 100644 index 000000000..f3b80cc73 --- /dev/null +++ b/library/cpp/include/wavemap/layered/layer_observation_batch.h @@ -0,0 +1,97 @@ +#ifndef WAVEMAP_LAYERED_LAYER_OBSERVATION_BATCH_H_ +#define WAVEMAP_LAYERED_LAYER_OBSERVATION_BATCH_H_ + +#include +#include +#include +#include + +#include +#include + +namespace wavemap::layered { +namespace detail { + +template +struct LayerObservationSlot { + std::vector>> observations; +}; + +template +struct LayerTagIdentity { + using Type = LayerTagT; +}; + +} // namespace detail + +// A heterogeneous, sensor-independent batch generated directly from a layer +// schema. One sensor callback can populate any subset of the configured layers +// without defining a new adapter or storage type. +template +class LayerObservationBatch; + +template +class LayerObservationBatch> + : private detail::LayerObservationSlot... { + public: + using Schema = schema::LayerSchema; + + template + void add(LayerObservation> observation) { + static_assert(Schema::template contains, + "The observation layer is not present in this map schema."); + observations().emplace_back(std::move(observation)); + } + + template + void add(Point3D position, LayerValueT value) { + add({std::move(position), std::move(value)}); + } + + template + auto& observations() { + static_assert(Schema::template contains, + "The requested layer is not present in this map schema."); + return static_cast&>(*this) + .observations; + } + + template + const auto& observations() const { + static_assert(Schema::template contains, + "The requested layer is not present in this map schema."); + return static_cast&>(*this) + .observations; + } + + bool empty() const { return (observations().empty() && ...); } + size_t size() const { + return (size_t{0u} + ... + observations().size()); + } + + template + void forEachLayer(FunctionT&& function) { + (function(detail::LayerTagIdentity{}, + observations()), + ...); + } + + template + void forEachLayer(FunctionT&& function) const { + (function(detail::LayerTagIdentity{}, + observations()), + ...); + } + + void transformPositions(const Transformation3D& transform) { + forEachLayer([&transform](auto /*layer_tag*/, auto& layer_observations) { + for (auto& observation : layer_observations) { + observation.position = transform * observation.position; + } + }); + } +}; + +} // namespace wavemap::layered + +#endif // WAVEMAP_LAYERED_LAYER_OBSERVATION_BATCH_H_ diff --git a/library/cpp/include/wavemap/layered/layer_schema.h b/library/cpp/include/wavemap/layered/layer_schema.h new file mode 100644 index 000000000..24cb463ec --- /dev/null +++ b/library/cpp/include/wavemap/layered/layer_schema.h @@ -0,0 +1,114 @@ +#ifndef WAVEMAP_LAYERED_LAYER_SCHEMA_H_ +#define WAVEMAP_LAYERED_LAYER_SCHEMA_H_ + +#include +#include +#include +#include +#include + +#include +#include + +namespace wavemap::layered::schema { + +template +struct ContinuousLayer { + static_assert(kIsContinuousLayerUpdatePolicy, + "A continuous layer policy must return Value and accept " + "(const Value&, const LayerObservation&)."); + using Value = ValueT; + using State = ValueT; + using UpdatePolicy = UpdatePolicyT; + static constexpr LayerStorageKind storage_kind = + LayerStorageKind::kWaveletCompatible; +}; + +template +struct StatefulContinuousLayer { + static_assert( + kIsStatefulContinuousLayerUpdatePolicy, + "A stateful continuous policy must return State and accept " + "(const State&, const LayerObservation&)."); + using Value = ValueT; + using State = StateT; + using UpdatePolicy = UpdatePolicyT; + static constexpr LayerStorageKind storage_kind = + LayerStorageKind::kWaveletCompatible; +}; + +template +struct DiscreteLayer { + static_assert(kIsDiscreteLayerUpdatePolicy, + "A discrete layer policy must return Value and accept either " + "Value or optional plus LayerObservation."); + using Value = ValueT; + using State = ValueT; + using UpdatePolicy = UpdatePolicyT; + static constexpr LayerStorageKind storage_kind = + LayerStorageKind::kNonWavelet; +}; + +namespace detail { +template +struct IsLayerTag : std::false_type {}; + +template +struct IsLayerTag< + LayerTagT, + std::void_t::Value, + typename LayerTraits::UpdatePolicy, + decltype(LayerTraits::name), + decltype(LayerTraits::storage_kind)>> + : std::true_type {}; + +template +constexpr bool namesAreUnique() { + constexpr std::array names = { + LayerTraits::name...}; + for (size_t lhs = 0u; lhs < sizeof...(LayerTags); ++lhs) { + if (names[lhs].empty()) { + return false; + } + for (size_t rhs = lhs + 1u; rhs < sizeof...(LayerTags); ++rhs) { + if (names[lhs] == names[rhs]) { + return false; + } + } + } + return true; +} +} // namespace detail + +template +struct LayerSchema { + static_assert((detail::IsLayerTag::value && ...), + "Every schema entry must be a valid layer tag."); + static_assert(detail::namesAreUnique(), + "Layer names must be non-empty and unique within a schema."); + + using Layers = std::tuple; + static constexpr size_t size = sizeof...(LayerTags); + + template + static constexpr bool contains = + (std::is_same_v || ...); + + template + static constexpr bool containsContinuous = + contains && kIsWaveletCompatibleLayer; + + template + static constexpr bool containsDiscrete = + contains && !kIsWaveletCompatibleLayer; + + static constexpr size_t continuousLayerCount = + (size_t{0u} + ... + + (kIsWaveletCompatibleLayer ? size_t{1u} : size_t{0u})); + static constexpr size_t discreteLayerCount = + size - continuousLayerCount; +}; + +} // namespace wavemap::layered::schema + +#endif // WAVEMAP_LAYERED_LAYER_SCHEMA_H_ diff --git a/library/cpp/include/wavemap/layered/layer_stream_codec.h b/library/cpp/include/wavemap/layered/layer_stream_codec.h new file mode 100644 index 000000000..7a462afca --- /dev/null +++ b/library/cpp/include/wavemap/layered/layer_stream_codec.h @@ -0,0 +1,86 @@ +#ifndef WAVEMAP_LAYERED_LAYER_STREAM_CODEC_H_ +#define WAVEMAP_LAYERED_LAYER_STREAM_CODEC_H_ + +#include +#include +#include +#include + +#include +#include +#include + +namespace wavemap::layered { + +template +struct LayerStreamCodec; + +namespace detail { +template +struct HasLayerStreamCodec : std::false_type {}; + +template +struct HasLayerStreamCodec< + ValueT, + std::void_t::typeName()), + decltype(LayerStreamCodec::write( + std::declval(), + std::declval())), + decltype(LayerStreamCodec::read( + std::declval()))>> : std::true_type {}; +} // namespace detail + +template +inline constexpr bool kHasLayerStreamCodec = + detail::HasLayerStreamCodec::value; + +template <> +struct LayerStreamCodec { + static constexpr const char* typeName() { return "float32"; } + + static void write(std::ostream& ostream, FloatingPoint value) { + wavemap::io::StreamableCellData::write(ostream, value); + } + + static FloatingPoint read(std::istream& istream) { + return wavemap::io::StreamableCellData::read(istream); + } +}; + +template <> +struct LayerStreamCodec { + static constexpr const char* typeName() { return "float32_rgb"; } + + static void write(std::ostream& ostream, const Rgb& value) { + LayerStreamCodec::write(ostream, value.r); + LayerStreamCodec::write(ostream, value.g); + LayerStreamCodec::write(ostream, value.b); + } + + static Rgb read(std::istream& istream) { + return {LayerStreamCodec::read(istream), + LayerStreamCodec::read(istream), + LayerStreamCodec::read(istream)}; + } +}; + +template <> +struct LayerStreamCodec { + static constexpr const char* typeName() { + return "float32_weighted_mean_state"; + } + + static void write(std::ostream& ostream, const WeightedMeanState& state) { + LayerStreamCodec::write(ostream, state.weighted_sum); + LayerStreamCodec::write(ostream, state.total_weight); + } + + static WeightedMeanState read(std::istream& istream) { + return {LayerStreamCodec::read(istream), + LayerStreamCodec::read(istream)}; + } +}; + +} // namespace wavemap::layered + +#endif // WAVEMAP_LAYERED_LAYER_STREAM_CODEC_H_ diff --git a/library/cpp/include/wavemap/layered/layer_traits.h b/library/cpp/include/wavemap/layered/layer_traits.h new file mode 100644 index 000000000..f7a27c499 --- /dev/null +++ b/library/cpp/include/wavemap/layered/layer_traits.h @@ -0,0 +1,173 @@ +#ifndef WAVEMAP_LAYERED_LAYER_TRAITS_H_ +#define WAVEMAP_LAYERED_LAYER_TRAITS_H_ + +#include +#include +#include +#include +#include +#include + +#include + +namespace wavemap::layered { + +// Storage follows mathematical semantics, not the value C++ numeric type. +enum class LayerStorageKind { + kWaveletCompatible, + kNonWavelet, +}; + +struct VisualizationColor { + float r; + float g; + float b; +}; + +struct ScalarLayerVisualization { + VisualizationColor low; + VisualizationColor high; +}; + +struct IntegerCategoryLabel { + int value; + std::string_view label; + std::optional color = std::nullopt; + + constexpr IntegerCategoryLabel(int value_in, std::string_view label_in) + : value(value_in), label(label_in) {} + constexpr IntegerCategoryLabel(int value_in, std::string_view label_in, + VisualizationColor color_in) + : value(value_in), label(label_in), color(color_in) {} +}; + +struct IntegerCategoryMetadata { + int value; + std::string label; + std::optional color; +}; + +namespace detail { +template +inline constexpr bool kLayerTraitsAlwaysFalse = false; + +template +struct HasIntegerCategoryLabels : std::false_type {}; + +template +struct HasIntegerCategoryLabels< + LayerTagT, std::void_t> + : std::true_type {}; + +template +struct HasVisualization : std::false_type {}; + +template +struct HasVisualization> + : std::true_type {}; +} // namespace detail + +template +std::vector integerCategoryMetadata() { + std::vector result; + if constexpr (detail::HasIntegerCategoryLabels::value) { + result.reserve(LayerTagT::categories.size()); + for (const IntegerCategoryLabel& category : LayerTagT::categories) { + result.push_back( + {category.value, std::string(category.label), category.color}); + } + } + return result; +} + +template +std::optional scalarLayerVisualization() { + if constexpr (detail::HasVisualization::value) { + return LayerTagT::visualization; + } + return std::nullopt; +} + +// Layer tags can inherit from schema::ContinuousLayer or +// schema::DiscreteLayer, in which case this trait is populated automatically. +// Explicit specializations remain supported for backwards compatibility. +template +struct LayerTraits { + static_assert(detail::kLayerTraitsAlwaysFalse, + "LayerTraits must be specialized for each layer."); +}; + +template +struct LayerTraits< + LayerTagT, + std::void_t> { + using Value = typename LayerTagT::Value; + using UpdatePolicy = typename LayerTagT::UpdatePolicy; + static constexpr std::string_view name = LayerTagT::name; + static constexpr LayerStorageKind storage_kind = LayerTagT::storage_kind; +}; + +template +using LayerValueT = typename LayerTraits::Value; + +namespace detail { +template +struct LayerState { + using Type = LayerValueT; +}; + +template +struct LayerState> { + using Type = typename LayerTagT::State; +}; +} // namespace detail + +// Value is the public observation/query type. State is the representation +// actually stored in the map. They are identical for ordinary layers. +template +using LayerStateT = typename detail::LayerState::Type; + +template +using LayerUpdatePolicyT = typename LayerTraits::UpdatePolicy; + +template +struct LayerStateConversion { + static LayerValueT value(const LayerStateT& state) { + if constexpr (std::is_same_v, + WeightedMeanState>) { + static_assert(std::is_same_v, FloatingPoint>); + return state.valueOr(); + } else { + static_assert(std::is_same_v, + LayerStateT>, + "A stateful layer must specialize LayerStateConversion."); + return state; + } + } + + static LayerStateT initialState( + const LayerValueT& value) { + if constexpr (std::is_same_v, + WeightedMeanState>) { + static_assert(std::is_same_v, FloatingPoint>); + return {value, 1.f}; + } else { + static_assert(std::is_same_v, + LayerStateT>, + "A stateful layer must specialize LayerStateConversion."); + return value; + } + } +}; + +template +inline constexpr bool kIsWaveletCompatibleLayer = + LayerTraits::storage_kind == + LayerStorageKind::kWaveletCompatible; + +} // namespace wavemap::layered + +#endif // WAVEMAP_LAYERED_LAYER_TRAITS_H_ diff --git a/library/cpp/include/wavemap/layered/layer_update_policy.h b/library/cpp/include/wavemap/layered/layer_update_policy.h new file mode 100644 index 000000000..ca51b1f50 --- /dev/null +++ b/library/cpp/include/wavemap/layered/layer_update_policy.h @@ -0,0 +1,170 @@ +#ifndef WAVEMAP_LAYERED_LAYER_UPDATE_POLICY_H_ +#define WAVEMAP_LAYERED_LAYER_UPDATE_POLICY_H_ + +#include +#include +#include +#include + +#include + +#include +#include + +namespace wavemap::layered { + +template +inline constexpr bool kIsContinuousLayerUpdatePolicy = + std::is_invocable_r_v&>; + +template +inline constexpr bool kIsStatefulContinuousLayerUpdatePolicy = + std::is_invocable_r_v&>; + +template +inline constexpr bool kIsDiscreteLayerUpdatePolicy = + kIsContinuousLayerUpdatePolicy || + std::is_invocable_r_v&, + const LayerObservation&>; + +template +struct ReplaceLayerUpdatePolicy { + ValueT operator()(const ValueT& /*current_value*/, + const LayerObservation& observation) const { + return observation.value; + } +}; + +template +struct MinimumLayerUpdatePolicy { + ValueT operator()(const ValueT& current_value, + const LayerObservation& observation) const { + return std::min(current_value, observation.value); + } +}; + +template +struct MaximumLayerUpdatePolicy { + ValueT operator()(const ValueT& current_value, + const LayerObservation& observation) const { + return std::max(current_value, observation.value); + } +}; + +template > +struct AccumulateLayerUpdatePolicy { + ValueT operator()(const ValueT& current_value, + const LayerObservation& observation) const { + return AddT{}(current_value, observation.value); + } +}; + +struct LogicalOrLayerUpdatePolicy { + bool operator()(bool current_value, + const LayerObservation& observation) const { + return current_value || observation.value; + } +}; + +struct LogicalAndLayerUpdatePolicy { + bool operator()(bool current_value, + const LayerObservation& observation) const { + return current_value && observation.value; + } +}; + +// Schema-friendly exponential average for scalar floating-point layers. +// The compile-time ratio keeps the policy default-constructible, as required +// by the layer integrator, while allowing each layer to select its own gain. +// This is a convex update, so bounded current values and observations remain +// bounded. The zero-initialized voxel acts as the initial prior. +template +struct ExponentialScalarLayerUpdatePolicy { + static_assert(0 < ObservationWeightDenominator, + "The exponential update denominator must be positive."); + static_assert(0 <= ObservationWeightNumerator, + "The exponential observation weight cannot be negative."); + static_assert(ObservationWeightNumerator <= ObservationWeightDenominator, + "The exponential observation weight cannot exceed one."); + + static constexpr FloatingPoint observationWeight() { + return static_cast(ObservationWeightNumerator) / + static_cast(ObservationWeightDenominator); + } + + FloatingPoint operator()( + FloatingPoint current_value, + const LayerObservation& observation) const { + const FloatingPoint weight = observationWeight(); + return current_value + weight * (observation.value - current_value); + } +}; + +// Runtime form for direct/custom use when a policy instance is explicitly +// owned by the caller rather than default-constructed from a layer schema. +template +class ExponentialLayerUpdatePolicy { + public: + explicit ExponentialLayerUpdatePolicy(FloatingPoint observation_weight) + : observation_weight_( + std::clamp(observation_weight, FloatingPoint{0.f}, + FloatingPoint{1.f})) {} + + ValueT operator()(const ValueT& current_value, + const LayerObservation& observation) const { + return ArithmeticPolicyT::add( + ArithmeticPolicyT::scale(current_value, 1.f - observation_weight_), + ArithmeticPolicyT::scale(observation.value, observation_weight_)); + } + + private: + FloatingPoint observation_weight_; +}; + +// Uses observation confidence as the blend factor when present. This is an +// exponential update, not a running weighted mean: no historical weight is +// hidden outside the voxel value. +template +class ConfidenceBlendLayerUpdatePolicy { + public: + explicit ConfidenceBlendLayerUpdatePolicy( + FloatingPoint default_observation_weight = 1.f) + : default_observation_weight_(std::clamp( + default_observation_weight, FloatingPoint{0.f}, + FloatingPoint{1.f})) {} + + ValueT operator()(const ValueT& current_value, + const LayerObservation& observation) const { + const FloatingPoint weight = std::clamp( + observation.confidence.value_or(default_observation_weight_), + FloatingPoint{0.f}, FloatingPoint{1.f}); + return ArithmeticPolicyT::add( + ArithmeticPolicyT::scale(current_value, 1.f - weight), + ArithmeticPolicyT::scale(observation.value, weight)); + } + + private: + FloatingPoint default_observation_weight_; +}; + +struct WeightedMeanLayerUpdatePolicy { + WeightedMeanState operator()( + const WeightedMeanState& current, + const LayerObservation& observation) const { + const FloatingPoint weight = + std::max(0.f, observation.confidence.value_or(1.f)); + if (weight == 0.f) { + return current; + } + return {current.weighted_sum + weight * observation.value, + current.total_weight + weight}; + } +}; + +} // namespace wavemap::layered + +#endif // WAVEMAP_LAYERED_LAYER_UPDATE_POLICY_H_ diff --git a/library/cpp/include/wavemap/layered/layered_map.h b/library/cpp/include/wavemap/layered/layered_map.h index 70f74aeeb..a89b40162 100644 --- a/library/cpp/include/wavemap/layered/layered_map.h +++ b/library/cpp/include/wavemap/layered/layered_map.h @@ -64,6 +64,10 @@ class LayeredMap { ContinuousMap& continuousMap() { return *continuous_map_; } const ContinuousMap& continuousMap() const { return *continuous_map_; } + typename ContinuousMap::Ptr continuousMapPtr() { return continuous_map_; } + typename ContinuousMap::ConstPtr continuousMapPtr() const { + return continuous_map_; + } DiscreteLayersT& discreteLayers() { return discrete_layers_; } const DiscreteLayersT& discreteLayers() const { return discrete_layers_; } diff --git a/library/cpp/include/wavemap/layered/layered_map_config_builder.h b/library/cpp/include/wavemap/layered/layered_map_config_builder.h new file mode 100644 index 000000000..7381d5217 --- /dev/null +++ b/library/cpp/include/wavemap/layered/layered_map_config_builder.h @@ -0,0 +1,133 @@ +#ifndef WAVEMAP_LAYERED_LAYERED_MAP_CONFIG_BUILDER_H_ +#define WAVEMAP_LAYERED_LAYERED_MAP_CONFIG_BUILDER_H_ + +#include + +#include +#include + +namespace wavemap::layered { + +// User-facing facade over LayeredMap's internal threshold and pruning bundles. +// It intentionally owns no behavior: build() returns the existing Config type +// consumed by LayeredMap. +template +class LayeredMapConfigBuilder { + public: + using Definition = MapDefinitionT; + using Schema = typename Definition::Schema; + using Config = typename Definition::Config; + + class MapSettings { + public: + explicit MapSettings(Config& config) : config_(config) {} + + MapSettings& minCellWidth(FloatingPoint meters) { + config_.continuous_map.min_cell_width = meters; + return *this; + } + MapSettings& occupancyLogOdds(FloatingPoint min_log_odds, + FloatingPoint max_log_odds) { + config_.continuous_map.min_log_odds = min_log_odds; + config_.continuous_map.max_log_odds = max_log_odds; + return *this; + } + MapSettings& treeHeight(IndexElement tree_height) { + config_.continuous_map.tree_height = tree_height; + return *this; + } + MapSettings& onlyPruneBlocksIfUnusedFor(FloatingPoint seconds) { + config_.continuous_map.only_prune_blocks_if_unused_for = seconds; + return *this; + } + + private: + Config& config_; + }; + + class OccupancySettings { + public: + explicit OccupancySettings(Config& config) : config_(config) {} + + OccupancySettings& pruningScale(FloatingPoint scale) { + config_.continuous_pruning.occupancy.scale = scale; + return *this; + } + OccupancySettings& pruningWeight(FloatingPoint weight) { + config_.continuous_pruning.occupancy.weight = weight; + return *this; + } + + private: + Config& config_; + }; + + template + class ContinuousLayerSettings { + public: + using Value = LayerValueT; + + explicit ContinuousLayerSettings(Config& config) : config_(config) { + static_assert( + Schema::template containsContinuous, + "layer() requires a continuous layer in this schema."); + } + + ContinuousLayerSettings& storageBounds(const Value& min_value, + const Value& max_value) { + auto& bounds = + config_.continuous_threshold.data.template get(); + bounds.min = min_value; + bounds.max = max_value; + return *this; + } + ContinuousLayerSettings& pruningScale(FloatingPoint scale) { + config_.continuous_pruning.data.template get().scale = scale; + return *this; + } + ContinuousLayerSettings& pruningWeight(FloatingPoint weight) { + config_.continuous_pruning.data.template get().weight = + weight; + return *this; + } + + private: + Config& config_; + }; + + LayeredMapConfigBuilder() = default; + explicit LayeredMapConfigBuilder(Config config) + : config_(std::move(config)) {} + + MapSettings map() { return MapSettings(config_); } + OccupancySettings occupancy() { return OccupancySettings(config_); } + + template + ContinuousLayerSettings layer() { + return ContinuousLayerSettings(config_); + } + + LayeredMapConfigBuilder& combinedPruningThreshold(FloatingPoint threshold) { + config_.continuous_pruning.combined_threshold = threshold; + // The additional-data bundle uses the same cutoff while scoring its own + // fields. Keep this internal value synchronized for the user. + config_.continuous_pruning.data.combined_threshold = threshold; + return *this; + } + + LayeredMapConfigBuilder& discreteBlockHeight(int block_height) { + config_.discrete_compression.block_height = block_height; + return *this; + } + + const Config& config() const { return config_; } + Config build() const { return config_; } + Config&& moveConfig() { return std::move(config_); } + + private: + Config config_{}; +}; + +} // namespace wavemap::layered + +#endif // WAVEMAP_LAYERED_LAYERED_MAP_CONFIG_BUILDER_H_ diff --git a/library/cpp/include/wavemap/layered/layered_map_definition.h b/library/cpp/include/wavemap/layered/layered_map_definition.h new file mode 100644 index 000000000..3a26fa9fa --- /dev/null +++ b/library/cpp/include/wavemap/layered/layered_map_definition.h @@ -0,0 +1,75 @@ +#ifndef WAVEMAP_LAYERED_LAYERED_MAP_DEFINITION_H_ +#define WAVEMAP_LAYERED_LAYERED_MAP_DEFINITION_H_ + +#include +#include +#include +#include + +namespace wavemap::layered { +namespace detail { + +template +inline constexpr bool kContinuousLayerHasValueTraits = + !kIsWaveletCompatibleLayer || + schema::kHasContinuousValueTraits>; + +template +inline constexpr bool kContinuousLayerHasStreamCodec = + !kIsWaveletCompatibleLayer || + kHasLayerStreamCodec>; + +template +constexpr bool continuousLayersHaveValueTraits( + std::index_sequence) { + return (kContinuousLayerHasValueTraits< + std::tuple_element_t> && + ...); +} + +template +constexpr bool continuousLayersHaveStreamCodecs( + std::index_sequence) { + return (kContinuousLayerHasStreamCodec< + std::tuple_element_t> && + ...); +} + +} // namespace detail + +// Derives the complete core map representation from a user layer schema. +// Occupancy remains built into LayeredMapVoxel and is therefore intentionally +// not listed in SchemaT. +template +struct LayeredMapDefinition { + using Schema = SchemaT; + using LayerTags = typename Schema::Layers; + + static_assert( + detail::continuousLayersHaveValueTraits( + std::make_index_sequence>{}), + "Every continuous layer value needs ContinuousValueTraits providing " + "add, subtract, scale, threshold, and magnitude operations."); + static_assert( + detail::continuousLayersHaveStreamCodecs( + std::make_index_sequence>{}), + "Every continuous layer value needs a LayerStreamCodec for map " + "persistence."); + + using ContinuousLayers = schema::ContinuousLayerBundle; + using ContinuousArithmetic = schema::ContinuousBundleArithmetic; + using DiscreteLayers = schema::DiscreteLayerBundle; + + using Map = + LayeredMap; + using Config = typename Map::Config; + using Voxel = typename Map::ContinuousVoxel; + using ContinuousMap = typename Map::ContinuousMap; + + using VoxelSerializer = DescriptorVoxelSerializer; + using MapIo = io::LayeredMapIo; +}; + +} // namespace wavemap::layered + +#endif // WAVEMAP_LAYERED_LAYERED_MAP_DEFINITION_H_ diff --git a/library/cpp/include/wavemap/layered/layered_map_io.h b/library/cpp/include/wavemap/layered/layered_map_io.h index 185c8ac6a..acd733dca 100644 --- a/library/cpp/include/wavemap/layered/layered_map_io.h +++ b/library/cpp/include/wavemap/layered/layered_map_io.h @@ -543,7 +543,25 @@ bool loadLayeredMap(const std::filesystem::path& file_path, LayeredMapT& map, return false; } - map = LayeredMapT(std::move(continuous_map), std::move(discrete_layers)); + // Keep the destination map object and its continuous-map allocation alive. + // The ROS server's occupancy pipeline and layered integrators may retain a + // shared pointer to this exact octree. Replacing LayeredMap here would leave + // those components writing into the old allocation and would also discard + // the destination's runtime threshold and pruning policies, which are not + // part of the serialized map payload. + auto& destination_continuous_map = map.continuousMap(); + if (continuous_map->getConfig() != destination_continuous_map.getConfig()) { + if (error_message) { + *error_message = + "Loaded LayeredMap geometry is incompatible with the configured " + "destination map."; + } + return false; + } + + destination_continuous_map.getHashMap() = + std::move(continuous_map->getHashMap()); + map.discreteLayers() = std::move(discrete_layers); return true; } diff --git a/library/cpp/include/wavemap/layered/layered_pipeline.h b/library/cpp/include/wavemap/layered/layered_pipeline.h new file mode 100644 index 000000000..5770bdbad --- /dev/null +++ b/library/cpp/include/wavemap/layered/layered_pipeline.h @@ -0,0 +1,56 @@ +#ifndef WAVEMAP_LAYERED_LAYERED_PIPELINE_H_ +#define WAVEMAP_LAYERED_LAYERED_PIPELINE_H_ + +#include + +#include +#include + +namespace wavemap::layered { + +// A typed, sensor-independent input path for additional map layers. It is +// intentionally independent of wavemap::Pipeline so the original occupancy +// pipeline remains unchanged. +template +class LayeredPipeline { + public: + explicit LayeredPipeline(LayeredMapT& map) : map_(map) {} + + template + LayerIntegrationResult integrate( + const std::vector>>& + observations) { + return integrateLayerObservations(map_, observations); + } + + template + LayerIntegrationResult integrate( + const std::vector>>& observations, + const LayerUpdatePolicyT& policy) { + return integrateLayerObservations(map_, observations, policy); + } + + template + LayerIntegrationResult integrateBatch(const ObservationBatchT& batch) { + LayerIntegrationResult total; + batch.forEachLayer([&](auto layer_tag, const auto& observations) { + using LayerTag = typename decltype(layer_tag)::Type; + if (observations.empty()) { + return; + } + const auto result = integrate(observations); + total.received += result.received; + total.integrated += result.integrated; + total.rejected += result.rejected; + total.updated_voxels += result.updated_voxels; + }); + return total; + } + + private: + LayeredMapT& map_; +}; + +} // namespace wavemap::layered + +#endif // WAVEMAP_LAYERED_LAYERED_PIPELINE_H_ diff --git a/library/cpp/include/wavemap/layered/local_elevation_classifier.h b/library/cpp/include/wavemap/layered/local_elevation_classifier.h new file mode 100644 index 000000000..4c94aa974 --- /dev/null +++ b/library/cpp/include/wavemap/layered/local_elevation_classifier.h @@ -0,0 +1,141 @@ +#ifndef WAVEMAP_LAYERED_LOCAL_ELEVATION_CLASSIFIER_H_ +#define WAVEMAP_LAYERED_LOCAL_ELEVATION_CLASSIFIER_H_ + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +namespace wavemap::layered { + +enum class GeometricClass : int { + kUnknown = 0, + kGround = 1, + kObstacle = 2, +}; + +struct LocalElevationClassifierConfig { + FloatingPoint grid_cell_width = 0.5f; + FloatingPoint ground_tolerance = 0.2f; + FloatingPoint obstacle_height = 0.35f; + size_t min_points_per_cell = 3u; + + bool isValid() const { + return 0.f < grid_cell_width && 0.f <= ground_tolerance && + ground_tolerance < obstacle_height && 0u < min_points_per_cell; + } +}; + +// Lightweight frame-local classifier. It estimates the ground elevation as +// the lowest endpoint in each horizontal grid cell. Cells without enough +// support and points in the transition band remain unknown. +class LocalElevationClassifier { + public: + explicit LocalElevationClassifier( + LocalElevationClassifierConfig config = {}) + : config_(config) { + CHECK(config_.isValid()); + } + + std::vector classify( + const PosedPointcloud<>& pointcloud) const { + struct CellStats { + FloatingPoint min_height = std::numeric_limits::max(); + size_t point_count = 0u; + }; + + const FloatingPoint cell_width_inv = 1.f / config_.grid_cell_width; + std::map cells; + std::vector world_points(pointcloud.size()); + std::vector valid(pointcloud.size(), false); + + for (size_t index = 0u; index < pointcloud.size(); ++index) { + const Point3D sensor_point = pointcloud[index]; + if (!sensor_point.array().isFinite().all()) { + continue; + } + const Point3D world_point = pointcloud.getPose() * sensor_point; + if (!world_point.array().isFinite().all()) { + continue; + } + world_points[index] = world_point; + valid[index] = true; + CellStats& stats = cells[toCellKey(world_point, cell_width_inv)]; + stats.min_height = std::min(stats.min_height, world_point.z()); + ++stats.point_count; + } + + std::vector classes(pointcloud.size(), + GeometricClass::kUnknown); + for (size_t index = 0u; index < world_points.size(); ++index) { + if (!valid[index]) { + continue; + } + const Point3D& point = world_points[index]; + const CellStats& stats = cells.at(toCellKey(point, cell_width_inv)); + if (stats.point_count < config_.min_points_per_cell) { + continue; + } + const FloatingPoint height_above_ground = point.z() - stats.min_height; + if (height_above_ground <= config_.ground_tolerance) { + classes[index] = GeometricClass::kGround; + } else if (config_.obstacle_height <= height_above_ground) { + classes[index] = GeometricClass::kObstacle; + } + } + return classes; + } + + const LocalElevationClassifierConfig& config() const { return config_; } + + private: + struct GridCellKey { + int x; + int y; + + bool operator<(const GridCellKey& other) const { + return std::tie(x, y) < std::tie(other.x, other.y); + } + }; + + static GridCellKey toCellKey(const Point3D& point, + FloatingPoint cell_width_inv) { + return GridCellKey{ + static_cast(std::floor(point.x() * cell_width_inv)), + static_cast(std::floor(point.y() * cell_width_inv))}; + } + + LocalElevationClassifierConfig config_; +}; + +template +std::vector> makeGeometricClassObservations( + const PosedPointcloud<>& pointcloud, + const LocalElevationClassifier& classifier, + const EndpointRange& endpoint_range = {}) { + static_assert(std::is_integral_v); + const auto classes = classifier.classify(pointcloud); + std::vector> observations; + observations.reserve(classes.size()); + for (size_t index = 0u; index < classes.size(); ++index) { + if (classes[index] == GeometricClass::kUnknown || + !endpoint_range.contains(pointcloud[index])) { + continue; + } + observations.emplace_back(pointcloud.getPose() * pointcloud[index], + static_cast(classes[index])); + } + return observations; +} + +} // namespace wavemap::layered + +#endif // WAVEMAP_LAYERED_LOCAL_ELEVATION_CLASSIFIER_H_ diff --git a/library/cpp/include/wavemap/layered/pointcloud_layer_integration_mode.h b/library/cpp/include/wavemap/layered/pointcloud_layer_integration_mode.h new file mode 100644 index 000000000..f438caed0 --- /dev/null +++ b/library/cpp/include/wavemap/layered/pointcloud_layer_integration_mode.h @@ -0,0 +1,37 @@ +#ifndef WAVEMAP_LAYERED_POINTCLOUD_LAYER_INTEGRATION_MODE_H_ +#define WAVEMAP_LAYERED_POINTCLOUD_LAYER_INTEGRATION_MODE_H_ + +#include + +#include +#include + +namespace wavemap::layered { + +// Where a value attached to a PointCloud2 sample is written. EndpointOnly is +// the appropriate default for surface measurements such as reflectivity, +// color, roughness, and class. AlongRay is intentionally opt-in because it is +// both more expensive and only meaningful for values that describe traversed +// free space. +enum class PointcloudLayerIntegrationMode { kEndpointOnly, kAlongRay }; + +template +void forEachPointcloudIntegrationIndex(const Point3D& sensor_origin, + const Point3D& endpoint, + FloatingPoint cell_width, + VisitorT&& visitor) { + static_assert(Mode == PointcloudLayerIntegrationMode::kEndpointOnly || + Mode == PointcloudLayerIntegrationMode::kAlongRay, + "Unsupported pointcloud layer integration mode."); + if constexpr (Mode == PointcloudLayerIntegrationMode::kEndpointOnly) { + visitor(convert::pointToNearestIndex(endpoint, 1.f / cell_width)); + } else { + for (const Index3D& index : Ray<3>(sensor_origin, endpoint, cell_width)) { + visitor(index); + } + } +} + +} // namespace wavemap::layered + +#endif // WAVEMAP_LAYERED_POINTCLOUD_LAYER_INTEGRATION_MODE_H_ diff --git a/library/cpp/include/wavemap/layered/schema_continuous_traits.h b/library/cpp/include/wavemap/layered/schema_continuous_traits.h new file mode 100644 index 000000000..8e1178420 --- /dev/null +++ b/library/cpp/include/wavemap/layered/schema_continuous_traits.h @@ -0,0 +1,310 @@ +#ifndef WAVEMAP_LAYERED_SCHEMA_CONTINUOUS_TRAITS_H_ +#define WAVEMAP_LAYERED_SCHEMA_CONTINUOUS_TRAITS_H_ + +#include +#include +#include + +#include +#include +#include + +namespace wavemap::layered::schema { + +template +struct ContinuousValueTraits; + +namespace detail { +template +struct HasContinuousValueTraits : std::false_type {}; + +template +struct HasContinuousValueTraits< + ValueT, + std::void_t< + typename ContinuousValueTraits::ThresholdConfig, + decltype(ContinuousValueTraits::add( + std::declval(), std::declval())), + decltype(ContinuousValueTraits::subtract( + std::declval(), std::declval())), + decltype(ContinuousValueTraits::scale( + std::declval(), std::declval())), + decltype(ContinuousValueTraits::threshold( + std::declval(), + std::declval::ThresholdConfig&>())), + decltype(ContinuousValueTraits::magnitude( + std::declval()))>> : std::true_type {}; +} // namespace detail + +template +inline constexpr bool kHasContinuousValueTraits = + detail::HasContinuousValueTraits::value; + +template <> +struct ContinuousValueTraits { + struct ThresholdConfig { + FloatingPoint min = std::numeric_limits::lowest(); + FloatingPoint max = std::numeric_limits::max(); + }; + + static FloatingPoint add(FloatingPoint lhs, FloatingPoint rhs) { + return lhs + rhs; + } + static FloatingPoint subtract(FloatingPoint lhs, FloatingPoint rhs) { + return lhs - rhs; + } + static FloatingPoint scale(FloatingPoint value, FloatingPoint factor) { + return factor * value; + } + static void threshold(FloatingPoint& value, const ThresholdConfig& config) { + value = std::clamp(value, config.min, config.max); + } + static FloatingPoint magnitude(FloatingPoint value) { + return std::abs(value); + } +}; + +template <> +struct ContinuousValueTraits { + struct ThresholdConfig { + Rgb min{std::numeric_limits::lowest(), + std::numeric_limits::lowest(), + std::numeric_limits::lowest()}; + Rgb max{std::numeric_limits::max(), + std::numeric_limits::max(), + std::numeric_limits::max()}; + }; + + static Rgb add(const Rgb& lhs, const Rgb& rhs) { + return RgbArithmetic::add(lhs, rhs); + } + static Rgb subtract(const Rgb& lhs, const Rgb& rhs) { + return RgbArithmetic::subtract(lhs, rhs); + } + static Rgb scale(const Rgb& value, FloatingPoint factor) { + return RgbArithmetic::scale(value, factor); + } + static void threshold(Rgb& value, const ThresholdConfig& config) { + value = clampRgb(value, config.min, config.max); + } + static FloatingPoint magnitude(const Rgb& value) { return maxAbs(value); } +}; + +template <> +struct ContinuousValueTraits { + struct ThresholdConfig { + FloatingPoint min = std::numeric_limits::lowest(); + FloatingPoint max = std::numeric_limits::max(); + FloatingPoint max_total_weight = + std::numeric_limits::max(); + }; + + static WeightedMeanState add(const WeightedMeanState& lhs, + const WeightedMeanState& rhs) { + return {lhs.weighted_sum + rhs.weighted_sum, + lhs.total_weight + rhs.total_weight}; + } + static WeightedMeanState subtract(const WeightedMeanState& lhs, + const WeightedMeanState& rhs) { + return {lhs.weighted_sum - rhs.weighted_sum, + lhs.total_weight - rhs.total_weight}; + } + static WeightedMeanState scale(const WeightedMeanState& value, + FloatingPoint factor) { + return {factor * value.weighted_sum, factor * value.total_weight}; + } + static void threshold(WeightedMeanState& state, + const ThresholdConfig& config) { + if (state.total_weight <= 0.f) { + state = {}; + return; + } + if (config.max_total_weight < state.total_weight) { + const FloatingPoint factor = + config.max_total_weight / state.total_weight; + state = scale(state, factor); + } + const FloatingPoint bounded_value = + std::clamp(state.valueOr(), config.min, config.max); + state.weighted_sum = bounded_value * state.total_weight; + } + static FloatingPoint magnitude(const WeightedMeanState& state) { + return std::max(std::abs(state.weighted_sum), + std::abs(state.total_weight)); + } +}; + +namespace detail { +template +using ThresholdConfigFor = + typename ContinuousValueTraits>::ThresholdConfig; + +template +using PruningConfigFor = wavemap::FieldPruningConfig; + +template class ConfigT, bool Enabled> +struct ContinuousConfigSlot {}; + +template class ConfigT> +struct ContinuousConfigSlot { + ConfigT value{}; +}; +} // namespace detail + +template class ConfigT> +class ContinuousConfigBundle; + +template class ConfigT> +class ContinuousConfigBundle, ConfigT> + : private detail::ContinuousConfigSlot< + LayerTags, ConfigT, kIsWaveletCompatibleLayer>... { + public: + using Schema = LayerSchema; + + template + ConfigT& get() { + static_assert(Schema::template containsContinuous); + return static_cast< + detail::ContinuousConfigSlot&>(*this) + .value; + } + + template + const ConfigT& get() const { + static_assert(Schema::template containsContinuous); + return static_cast&>(*this) + .value; + } +}; + +template +using ContinuousThresholdConfig = + ContinuousConfigBundle; + +template +struct ContinuousPruningConfig + : ContinuousConfigBundle { + FloatingPoint combined_threshold = 1.f; +}; + +template +struct ContinuousBundleArithmetic; + +template +struct ContinuousBundleArithmetic> { + using Schema = LayerSchema; + using Bundle = ContinuousLayerBundle; + + static Bundle add(const Bundle& lhs, const Bundle& rhs) { + Bundle result; + (applyAdd(result, lhs, rhs), ...); + return result; + } + + static Bundle subtract(const Bundle& lhs, const Bundle& rhs) { + Bundle result; + (applySubtract(result, lhs, rhs), ...); + return result; + } + + static Bundle scale(const Bundle& value, FloatingPoint factor) { + Bundle result; + if constexpr (sizeof...(LayerTags) == 0u) { + (void)value; + (void)factor; + } + (applyScale(result, value, factor), ...); + return result; + } + + private: + template + static void applyAdd(Bundle& result, const Bundle& lhs, const Bundle& rhs) { + if constexpr (kIsWaveletCompatibleLayer) { + result.template get() = + ContinuousValueTraits>::add( + lhs.template get(), rhs.template get()); + } + } + + template + static void applySubtract(Bundle& result, const Bundle& lhs, + const Bundle& rhs) { + if constexpr (kIsWaveletCompatibleLayer) { + result.template get() = + ContinuousValueTraits>::subtract( + lhs.template get(), rhs.template get()); + } + } + + template + static void applyScale(Bundle& result, const Bundle& value, + FloatingPoint factor) { + if constexpr (kIsWaveletCompatibleLayer) { + result.template get() = + ContinuousValueTraits>::scale( + value.template get(), factor); + } else { + (void)result; + (void)value; + (void)factor; + } + } +}; + +} // namespace wavemap::layered::schema + +namespace wavemap { + +template +struct CellDataTraits>> { + using Schema = layered::schema::LayerSchema; + using Bundle = layered::schema::ContinuousLayerBundle; + using ThresholdConfig = layered::schema::ContinuousThresholdConfig; + using PruningConfig = layered::schema::ContinuousPruningConfig; + + static void threshold(Bundle& bundle, const ThresholdConfig& config) { + (thresholdOne(bundle, config), ...); + } + + static FloatingPoint pruningScore(const Bundle& bundle, + const PruningConfig& config) { + return (FloatingPoint{0.f} + ... + scoreOne(bundle, config)); + } + + static bool isNonzero(const Bundle& bundle, const PruningConfig& config) { + return config.combined_threshold < pruningScore(bundle, config); + } + + private: + template + static void thresholdOne(Bundle& bundle, const ThresholdConfig& config) { + if constexpr (layered::kIsWaveletCompatibleLayer) { + layered::schema::ContinuousValueTraits< + layered::LayerStateT>::threshold( + bundle.template get(), + config.template get()); + } + } + + template + static FloatingPoint scoreOne(const Bundle& bundle, + const PruningConfig& config) { + if constexpr (layered::kIsWaveletCompatibleLayer) { + return weightedPruningScore( + layered::schema::ContinuousValueTraits< + layered::LayerStateT>::magnitude( + bundle.template get()), + config.template get()); + } else { + return 0.f; + } + } +}; + +} // namespace wavemap + +#endif // WAVEMAP_LAYERED_SCHEMA_CONTINUOUS_TRAITS_H_ diff --git a/library/cpp/include/wavemap/layered/schema_layer_storage.h b/library/cpp/include/wavemap/layered/schema_layer_storage.h new file mode 100644 index 000000000..74b7a8d2b --- /dev/null +++ b/library/cpp/include/wavemap/layered/schema_layer_storage.h @@ -0,0 +1,166 @@ +#ifndef WAVEMAP_LAYERED_SCHEMA_LAYER_STORAGE_H_ +#define WAVEMAP_LAYERED_SCHEMA_LAYER_STORAGE_H_ + +#include +#include +#include + +#include +#include +#include + +namespace wavemap::layered::schema { +namespace detail { + +template +struct ContinuousSlot { + bool operator==(const ContinuousSlot&) const { return true; } +}; + +template +struct ContinuousSlot { + LayerStateT value{}; + + bool operator==(const ContinuousSlot& other) const { + return value == other.value; + } +}; + +template +struct DiscreteSlot { + explicit DiscreteSlot(const DiscreteCompressionConfig&) {} +}; + +template +struct DiscreteSlot { + explicit DiscreteSlot(const DiscreteCompressionConfig& config) + : value(config) {} + + layered::DiscreteLayer> value; +}; + +template +constexpr auto continuousDescriptorTuple() { + if constexpr (kIsWaveletCompatibleLayer) { + return std::make_tuple( + accessorLayerDescriptor()); + } else { + return std::tuple<>(); + } +} + +template +constexpr auto discreteDescriptorTuple() { + if constexpr (kIsWaveletCompatibleLayer) { + return std::tuple<>(); + } else { + return std::make_tuple( + accessorLayerDescriptor()); + } +} + +} // namespace detail + +template +class ContinuousLayerBundle; + +template +class ContinuousLayerBundle> + : private detail::ContinuousSlot< + LayerTags, kIsWaveletCompatibleLayer>... { + public: + using Schema = LayerSchema; + + template + LayerStateT& get() { + static_assert(Schema::template containsContinuous, + "Requested layer is not a continuous layer in this schema."); + return static_cast&>(*this).value; + } + + template + const LayerStateT& get() const { + static_assert(Schema::template containsContinuous, + "Requested layer is not a continuous layer in this schema."); + return static_cast&>(*this) + .value; + } + + bool operator==(const ContinuousLayerBundle& other) const { + return ((! + kIsWaveletCompatibleLayer || + getIfContinuous() == + other.template getIfContinuous()) && + ...); + } + + private: + template + const auto& getIfContinuous() const { + return static_cast>&>(*this); + } +}; + +template +class DiscreteLayerBundle; + +template +class DiscreteLayerBundle> + : private detail::DiscreteSlot< + LayerTags, !kIsWaveletCompatibleLayer>... { + public: + using Schema = LayerSchema; + + explicit DiscreteLayerBundle( + const DiscreteCompressionConfig& config = DiscreteCompressionConfig()) + : detail::DiscreteSlot< + LayerTags, !kIsWaveletCompatibleLayer>(config)... {} + + template + layered::DiscreteLayer>& get() { + static_assert(Schema::template containsDiscrete, + "Requested layer is not a discrete layer in this schema."); + return static_cast&>(*this).value; + } + + template + const layered::DiscreteLayer>& get() const { + static_assert(Schema::template containsDiscrete, + "Requested layer is not a discrete layer in this schema."); + return static_cast&>(*this) + .value; + } +}; + +} // namespace wavemap::layered::schema + +namespace wavemap::layered { + +template +struct LayerDescriptors< + schema::ContinuousLayerBundle>> { + using Bundle = + schema::ContinuousLayerBundle>; + + static constexpr auto descriptors() { + return std::tuple_cat( + schema::detail::continuousDescriptorTuple()...); + } +}; + +template +struct LayerDescriptors< + schema::DiscreteLayerBundle>> { + using Bundle = + schema::DiscreteLayerBundle>; + + static constexpr auto descriptors() { + return std::tuple_cat( + schema::detail::discreteDescriptorTuple()...); + } +}; + +} // namespace wavemap::layered + +#endif // WAVEMAP_LAYERED_SCHEMA_LAYER_STORAGE_H_ diff --git a/library/cpp/include/wavemap/layered/types/rgb.h b/library/cpp/include/wavemap/layered/types/rgb.h new file mode 100644 index 000000000..12813f7ff --- /dev/null +++ b/library/cpp/include/wavemap/layered/types/rgb.h @@ -0,0 +1,47 @@ +#ifndef WAVEMAP_LAYERED_TYPES_RGB_H_ +#define WAVEMAP_LAYERED_TYPES_RGB_H_ + +#include +#include + +#include + +namespace wavemap::layered { + +struct Rgb { + float r = 0.f; + float g = 0.f; + float b = 0.f; + + bool operator==(const Rgb& other) const { + return r == other.r && g == other.g && b == other.b; + } +}; + +struct RgbArithmetic { + static Rgb add(const Rgb& lhs, const Rgb& rhs) { + return {lhs.r + rhs.r, lhs.g + rhs.g, lhs.b + rhs.b}; + } + + static Rgb subtract(const Rgb& lhs, const Rgb& rhs) { + return {lhs.r - rhs.r, lhs.g - rhs.g, lhs.b - rhs.b}; + } + + static Rgb scale(const Rgb& color, FloatingPoint factor) { + return {factor * color.r, factor * color.g, factor * color.b}; + } +}; + +inline Rgb clampRgb(const Rgb& color, const Rgb& min, const Rgb& max) { + return {std::clamp(color.r, min.r, max.r), + std::clamp(color.g, min.g, max.g), + std::clamp(color.b, min.b, max.b)}; +} + +inline FloatingPoint maxAbs(const Rgb& color) { + return std::max({std::abs(color.r), std::abs(color.g), std::abs(color.b)}); +} + +} // namespace wavemap::layered + +#endif // WAVEMAP_LAYERED_TYPES_RGB_H_ diff --git a/library/cpp/include/wavemap/layered/types/weighted_mean_state.h b/library/cpp/include/wavemap/layered/types/weighted_mean_state.h new file mode 100644 index 000000000..95d5db8ea --- /dev/null +++ b/library/cpp/include/wavemap/layered/types/weighted_mean_state.h @@ -0,0 +1,26 @@ +#ifndef WAVEMAP_LAYERED_TYPES_WEIGHTED_MEAN_STATE_H_ +#define WAVEMAP_LAYERED_TYPES_WEIGHTED_MEAN_STATE_H_ + +#include + +namespace wavemap::layered { + +// Sufficient statistics for a true scalar running weighted mean. Zero weight +// explicitly represents an unobserved voxel and is not a measurement of zero. +struct WeightedMeanState { + FloatingPoint weighted_sum = 0.f; + FloatingPoint total_weight = 0.f; + + FloatingPoint valueOr(FloatingPoint fallback = 0.f) const { + return 0.f < total_weight ? weighted_sum / total_weight : fallback; + } + + bool operator==(const WeightedMeanState& other) const { + return weighted_sum == other.weighted_sum && + total_weight == other.total_weight; + } +}; + +} // namespace wavemap::layered + +#endif // WAVEMAP_LAYERED_TYPES_WEIGHTED_MEAN_STATE_H_ diff --git a/library/cpp/test/src/core/CMakeLists.txt b/library/cpp/test/src/core/CMakeLists.txt index cb246e0ec..2deb21e01 100644 --- a/library/cpp/test/src/core/CMakeLists.txt +++ b/library/cpp/test/src/core/CMakeLists.txt @@ -19,6 +19,7 @@ target_sources(test_wavemap_core PRIVATE integrator/test_range_image_intersector.cc map/test_haar_cell.cc map/test_hashed_blocks.cc + map/test_layered_map_definition.cc map/test_map.cc map/test_volumetric_octree.cc utils/bits/test_bit_operations.cc @@ -36,6 +37,7 @@ target_sources(test_wavemap_core PRIVATE utils/neighbors/test_ndtree_adjacency.cc utils/profile/test_resource_monitor.cc utils/query/test_classified_map.cc + utils/query/test_local_elevation_classifier.cc utils/query/test_map_interpolator.cpp utils/query/test_occupancy_classifier.cc utils/query/test_probability_conversions.cc diff --git a/library/cpp/test/src/core/map/test_layered_map_definition.cc b/library/cpp/test/src/core/map/test_layered_map_definition.cc new file mode 100644 index 000000000..56393c48c --- /dev/null +++ b/library/cpp/test/src/core/map/test_layered_map_definition.cc @@ -0,0 +1,346 @@ +#include +#include +#include + +#include + +#include +#include +#include +#include +#include +#include + +namespace wavemap::layered { +namespace { + +struct ReflectivityLayer + : schema::ContinuousLayer> { + static constexpr std::string_view name = "reflectivity"; +}; + +struct TemperatureLayer + : schema::ContinuousLayer> { + static constexpr std::string_view name = "temperature"; +}; + +struct ClassLayer + : schema::DiscreteLayer> { + static constexpr std::string_view name = "class"; +}; + +struct AveragedLayer + : schema::StatefulContinuousLayer { + static constexpr std::string_view name = "averaged"; +}; + +struct CustomDampedPolicy { + float operator()(float current, + const LayerObservation& observation) const { + return 0.75f * current + 0.25f * observation.value; + } +}; + +struct UnsupportedContinuousValue { + float value = 0.f; +}; + +static_assert(kIsContinuousLayerUpdatePolicy); +static_assert(kIsDiscreteLayerUpdatePolicy, int>); +static_assert(schema::kHasContinuousValueTraits); +static_assert(schema::kHasContinuousValueTraits); +static_assert(kHasLayerStreamCodec); +static_assert(kHasLayerStreamCodec); +static_assert(!schema::kHasContinuousValueTraits); +static_assert(!kHasLayerStreamCodec); + +using ContinuousDefinition = + LayeredMapDefinition>; +using MixedDefinition = + LayeredMapDefinition>; +using DiscreteDefinition = + LayeredMapDefinition>; +using EmptyDefinition = LayeredMapDefinition>; +using WeightedDefinition = + LayeredMapDefinition>; + +static_assert(std::is_same_v); +static_assert(std::is_same_v); +static_assert(ContinuousDefinition::Schema::continuousLayerCount == 2u); +static_assert(MixedDefinition::Schema::continuousLayerCount == 1u); +static_assert(MixedDefinition::Schema::discreteLayerCount == 1u); +static_assert(DiscreteDefinition::Schema::continuousLayerCount == 0u); +static_assert(EmptyDefinition::Schema::size == 0u); +static_assert( + std::is_same_v, FloatingPoint>); +static_assert( + std::is_same_v, WeightedMeanState>); +static_assert(schema::kHasContinuousValueTraits); +static_assert(kHasLayerStreamCodec); + +template +typename DefinitionT::Config makeConfig() { + typename DefinitionT::Config config; + config.continuous_map.min_cell_width = 0.25f; + config.continuous_map.min_log_odds = -2.f; + config.continuous_map.max_log_odds = 4.f; + config.continuous_map.tree_height = 4; + return config; +} + +TEST(LayeredMapDefinition, GeneratesContinuousStorage) { + ContinuousDefinition::Map map(makeConfig()); + const Index3D index{1, 2, 3}; + + ContinuousDefinition::Voxel voxel; + voxel.occupancy = 0.7f; + voxel.data.get() = 0.25f; + voxel.data.get() = 18.f; + map.continuousMap().setVoxelValue(index, voxel); + + const auto stored = map.continuousMap().getVoxelValue(index); + EXPECT_FLOAT_EQ(stored.occupancy, 0.7f); + EXPECT_FLOAT_EQ(stored.data.get(), 0.25f); + EXPECT_FLOAT_EQ(stored.data.get(), 18.f); +} + +TEST(LayeredMapDefinition, GeneratesMixedContinuousAndDiscreteStorage) { + MixedDefinition::Map map(makeConfig()); + const Index3D index{-2, 1, 4}; + + MixedDefinition::Voxel voxel; + voxel.occupancy = 0.4f; + voxel.data.get() = 0.8f; + map.continuousMap().setVoxelValue(index, voxel); + map.discreteLayers().get().setValue(index, 3); + + EXPECT_FLOAT_EQ(map.continuousMap() + .getVoxelValue(index) + .data.get(), + 0.8f); + ASSERT_TRUE(map.discreteLayers().get().getValue(index)); + EXPECT_EQ(*map.discreteLayers().get().getValue(index), 3); +} + +TEST(LayeredMapDefinition, IntegratesHeterogeneousObservationBatch) { + MixedDefinition::Map map(makeConfig()); + LayeredPipeline pipeline(map); + LayerObservationBatch batch; + const Point3D position(0.5f, 0.25f, -0.25f); + + batch.add(position, 0.65f); + batch.add(position, 4); + EXPECT_EQ(batch.size(), 2u); + + const auto result = pipeline.integrateBatch(batch); + EXPECT_EQ(result.received, 2u); + EXPECT_EQ(result.integrated, 2u); + EXPECT_EQ(result.rejected, 0u); + + const Index3D index = convert::pointToNearestIndex( + position, 1.f / map.continuousMap().getMinCellWidth()); + EXPECT_FLOAT_EQ(map.continuousMap() + .getVoxelValue(index) + .data.get(), + 0.65f); + ASSERT_TRUE(map.discreteLayers().get().getValue(index)); + EXPECT_EQ(*map.discreteLayers().get().getValue(index), 4); +} + +TEST(LayeredMapDefinition, TransformsAllObservationBatchPositions) { + LayerObservationBatch batch; + batch.add(Point3D(1.f, 2.f, 3.f), 0.5f); + batch.add(Point3D(-1.f, 0.f, 1.f), 2); + + const Transformation3D transform(Rotation3D(), Point3D(2.f, -1.f, 0.5f)); + batch.transformPositions(transform); + + EXPECT_TRUE(batch.observations()[0].position.isApprox( + Point3D(3.f, 1.f, 3.5f))); + EXPECT_TRUE(batch.observations()[0].position.isApprox( + Point3D(1.f, -1.f, 1.5f))); +} + +TEST(LayeredMapDefinition, StoresTrueWeightedMeanFusionState) { + WeightedDefinition::Map map(makeConfig()); + LayeredPipeline pipeline(map); + const Point3D position(0.5f, 0.25f, -0.25f); + + LayerObservation first(position, 0.2f); + first.confidence = 1.f; + LayerObservation second(position, 0.8f); + second.confidence = 3.f; + const std::vector> observations{first, second}; + const auto result = pipeline.integrate(observations); + + EXPECT_EQ(result.integrated, 2u); + EXPECT_EQ(result.updated_voxels, 1u); + const Index3D index = convert::pointToNearestIndex( + position, 1.f / map.continuousMap().getMinCellWidth()); + const auto state = + map.continuousMap().getVoxelValue(index).data.get(); + EXPECT_FLOAT_EQ(state.weighted_sum, 2.6f); + EXPECT_FLOAT_EQ(state.total_weight, 4.f); + EXPECT_FLOAT_EQ(LayerStateConversion::value(state), 0.65f); +} + +TEST(LayeredMapDefinition, WeightedMeanHasExplicitUnobservedState) { + const WeightedMeanState unobserved; + EXPECT_FLOAT_EQ(unobserved.total_weight, 0.f); + EXPECT_FLOAT_EQ(unobserved.valueOr(-1.f), -1.f); + + LayerObservation observation(Point3D::Zero(), 0.8f); + const auto initialized = + WeightedMeanLayerUpdatePolicy{}(unobserved, observation); + EXPECT_FLOAT_EQ(initialized.weighted_sum, 0.8f); + EXPECT_FLOAT_EQ(initialized.total_weight, 1.f); + EXPECT_FLOAT_EQ(initialized.valueOr(), 0.8f); +} + +TEST(LayeredMapDefinition, WeightedMeanFusionStateStreamRoundTrip) { + const WeightedMeanState expected{2.6f, 4.f}; + std::stringstream stream; + LayerStreamCodec::write(stream, expected); + const auto restored = LayerStreamCodec::read(stream); + EXPECT_FLOAT_EQ(restored.weighted_sum, expected.weighted_sum); + EXPECT_FLOAT_EQ(restored.total_weight, expected.total_weight); + EXPECT_FLOAT_EQ(restored.valueOr(), 0.65f); +} + +TEST(LayeredMapDefinition, SupportsDiscreteOnlyAndEmptySchemas) { + DiscreteDefinition::Map discrete_map(makeConfig()); + EmptyDefinition::Map empty_map(makeConfig()); + const Index3D index{0, 0, 0}; + + discrete_map.discreteLayers().get().setValue(index, 7); + discrete_map.continuousMap().setCellValue(index, 0.5f); + empty_map.continuousMap().setCellValue(index, 0.6f); + + EXPECT_EQ(*discrete_map.discreteLayers().get().getValue(index), + 7); + EXPECT_FLOAT_EQ(discrete_map.continuousMap().getCellValue(index), 0.5f); + EXPECT_FLOAT_EQ(empty_map.continuousMap().getCellValue(index), 0.6f); +} + +TEST(LayeredMapDefinition, UnchangedDiscreteAssignmentDoesNotBecomeDirty) { + DiscreteLayer layer(DiscreteCompressionConfig{1}); + const Index3D index{2, 3, 4}; + + layer.setValue(index, 7); + ASSERT_EQ(layer.dirtyParentKeys().size(), 1u); + layer.clearDirtyParentKeys(); + + layer.setValue(index, 7); + EXPECT_TRUE(layer.dirtyParentKeys().empty()); + + layer.setValue(index, 8); + EXPECT_EQ(layer.dirtyParentKeys().size(), 1u); + EXPECT_EQ(layer.getValue(index), 8); +} + +TEST(LayeredMapDefinition, BatchedDiscreteAssignmentMarksOnlyChangedParents) { + DiscreteLayer layer(DiscreteCompressionConfig{1}); + const Index3D unchanged{0, 0, 0}; + const Index3D changed{8, 0, 0}; + + layer.setValues({{unchanged, 1}, {changed, 2}}); + layer.clearDirtyParentKeys(); + layer.setValues({{unchanged, 1}, {changed, 3}}); + + ASSERT_EQ(layer.dirtyParentKeys().size(), 1u); + EXPECT_EQ(layer.getValue(unchanged), 1); + EXPECT_EQ(layer.getValue(changed), 3); + + layer.clearDirtyParentKeys(); + layer.setValues({{unchanged, 1}, {changed, 3}}); + EXPECT_TRUE(layer.dirtyParentKeys().empty()); +} + +TEST(LayeredMapDefinition, ConfigBuilderPopulatesExistingConfig) { + LayeredMapConfigBuilder builder; + builder.map() + .minCellWidth(0.2f) + .occupancyLogOdds(-3.f, 5.f) + .treeHeight(7) + .onlyPruneBlocksIfUnusedFor(8.f); + builder.occupancy().pruningScale(0.002f).pruningWeight(2.f); + builder.layer() + .storageBounds(0.f, 1.f) + .pruningScale(0.01f) + .pruningWeight(0.5f); + builder.combinedPruningThreshold(1.5f).discreteBlockHeight(2); + + const auto config = builder.build(); + EXPECT_FLOAT_EQ(config.continuous_map.min_cell_width, 0.2f); + EXPECT_FLOAT_EQ(config.continuous_map.min_log_odds, -3.f); + EXPECT_FLOAT_EQ(config.continuous_map.max_log_odds, 5.f); + EXPECT_EQ(config.continuous_map.tree_height, 7); + EXPECT_FLOAT_EQ(config.continuous_map.only_prune_blocks_if_unused_for, 8.f); + EXPECT_FLOAT_EQ(config.continuous_pruning.occupancy.scale, 0.002f); + EXPECT_FLOAT_EQ(config.continuous_pruning.occupancy.weight, 2.f); + + const auto& reflectivity_bounds = + config.continuous_threshold.data.get(); + const auto& reflectivity_pruning = + config.continuous_pruning.data.get(); + EXPECT_FLOAT_EQ(reflectivity_bounds.min, 0.f); + EXPECT_FLOAT_EQ(reflectivity_bounds.max, 1.f); + EXPECT_FLOAT_EQ(reflectivity_pruning.scale, 0.01f); + EXPECT_FLOAT_EQ(reflectivity_pruning.weight, 0.5f); + EXPECT_FLOAT_EQ(config.continuous_pruning.combined_threshold, 1.5f); + EXPECT_FLOAT_EQ(config.continuous_pruning.data.combined_threshold, 1.5f); + EXPECT_EQ(config.discrete_compression.block_height, 2); +} + +TEST(LayeredUpdatePolicy, ProvidesStandardAndCustomPolicies) { + const LayerObservation low(Point3D::Zero(), 0.2f); + const LayerObservation high(Point3D::Zero(), 0.8f); + + EXPECT_FLOAT_EQ(ReplaceLayerUpdatePolicy{}(0.5f, high), 0.8f); + EXPECT_FLOAT_EQ(MinimumLayerUpdatePolicy{}(0.5f, low), 0.2f); + EXPECT_FLOAT_EQ(MaximumLayerUpdatePolicy{}(0.5f, high), 0.8f); + EXPECT_FLOAT_EQ(AccumulateLayerUpdatePolicy{}(0.5f, low), 0.7f); + EXPECT_FLOAT_EQ(CustomDampedPolicy{}(0.4f, high), 0.5f); + + using ExponentialPolicy = ExponentialScalarLayerUpdatePolicy<1, 5>; + static_assert(kIsContinuousLayerUpdatePolicy); + EXPECT_FLOAT_EQ(ExponentialPolicy::observationWeight(), 0.2f); + EXPECT_FLOAT_EQ(ExponentialPolicy{}(0.5f, high), 0.56f); + + float bounded_value = 0.f; + for (int update_idx = 0; update_idx < 100; ++update_idx) { + const auto& observation = update_idx % 2 ? low : high; + bounded_value = ExponentialPolicy{}(bounded_value, observation); + EXPECT_GE(bounded_value, 0.f); + EXPECT_LE(bounded_value, 1.f); + } + + EXPECT_TRUE(LogicalOrLayerUpdatePolicy{}( + false, LayerObservation(Point3D::Zero(), true))); + EXPECT_FALSE(LogicalAndLayerUpdatePolicy{}( + true, LayerObservation(Point3D::Zero(), false))); +} + +TEST(LayeredUpdatePolicy, ConfidenceBlendUsesObservationConfidence) { + struct FloatArithmetic { + static float add(float lhs, float rhs) { return lhs + rhs; } + static float scale(float value, float factor) { return value * factor; } + }; + + LayerObservation observation(Point3D::Zero(), 1.f); + observation.confidence = 0.25f; + const ConfidenceBlendLayerUpdatePolicy policy; + EXPECT_FLOAT_EQ(policy(0.f, observation), 0.25f); + + observation.confidence.reset(); + const ConfidenceBlendLayerUpdatePolicy + default_policy(0.4f); + EXPECT_FLOAT_EQ(default_policy(0.f, observation), 0.4f); +} + +} // namespace +} // namespace wavemap::layered diff --git a/library/cpp/test/src/core/utils/query/test_local_elevation_classifier.cc b/library/cpp/test/src/core/utils/query/test_local_elevation_classifier.cc new file mode 100644 index 000000000..70124607c --- /dev/null +++ b/library/cpp/test/src/core/utils/query/test_local_elevation_classifier.cc @@ -0,0 +1,109 @@ +#include + +#include + +namespace wavemap::layered { +namespace { + +PosedPointcloud<> makePointcloud(std::vector points, + const Transformation3D& pose = {}) { + return PosedPointcloud<>(pose, Pointcloud<>(points)); +} + +TEST(LocalElevationClassifierTest, ValidatesDefaultConfig) { + EXPECT_TRUE(LocalElevationClassifierConfig{}.isValid()); + + LocalElevationClassifierConfig config; + config.obstacle_height = config.ground_tolerance; + EXPECT_FALSE(config.isValid()); +} + +TEST(LocalElevationClassifierTest, ClassifiesSupportedCellByRelativeHeight) { + LocalElevationClassifierConfig config; + config.min_points_per_cell = 3u; + const LocalElevationClassifier classifier(config); + const auto cloud = makePointcloud({Point3D(0.1f, 0.1f, 1.f), + Point3D(0.2f, 0.1f, 1.1f), + Point3D(0.3f, 0.1f, 1.3f), + Point3D(0.4f, 0.1f, 1.5f)}); + + const auto classes = classifier.classify(cloud); + + ASSERT_EQ(classes.size(), 4u); + EXPECT_EQ(classes[0], GeometricClass::kGround); + EXPECT_EQ(classes[1], GeometricClass::kGround); + EXPECT_EQ(classes[2], GeometricClass::kUnknown); + EXPECT_EQ(classes[3], GeometricClass::kObstacle); +} + +TEST(LocalElevationClassifierTest, LeavesSparseCellsUnknown) { + LocalElevationClassifierConfig config; + config.min_points_per_cell = 3u; + const LocalElevationClassifier classifier(config); + const auto cloud = makePointcloud( + {Point3D(-0.1f, -0.1f, 0.f), Point3D(-0.2f, -0.1f, 1.f)}); + + const auto classes = classifier.classify(cloud); + + ASSERT_EQ(classes.size(), 2u); + EXPECT_EQ(classes[0], GeometricClass::kUnknown); + EXPECT_EQ(classes[1], GeometricClass::kUnknown); +} + +TEST(LocalElevationClassifierTest, UsesWorldCoordinates) { + LocalElevationClassifierConfig config; + config.min_points_per_cell = 2u; + const LocalElevationClassifier classifier(config); + Transformation3D pose; + pose.getPosition() = Point3D(2.f, 3.f, 4.f); + const auto cloud = makePointcloud( + {Point3D(0.1f, 0.1f, 0.f), Point3D(0.2f, 0.1f, 0.5f)}, pose); + + const auto classes = classifier.classify(cloud); + + ASSERT_EQ(classes.size(), 2u); + EXPECT_EQ(classes[0], GeometricClass::kGround); + EXPECT_EQ(classes[1], GeometricClass::kObstacle); +} + +TEST(LocalElevationClassifierTest, CreatesOnlyObservedWorldFrameUpdates) { + LocalElevationClassifierConfig config; + config.min_points_per_cell = 3u; + const LocalElevationClassifier classifier(config); + Transformation3D pose; + pose.getPosition() = Point3D(2.f, 3.f, 4.f); + const auto cloud = makePointcloud({Point3D(0.1f, 0.1f, 0.f), + Point3D(0.2f, 0.1f, 0.3f), + Point3D(0.3f, 0.1f, 0.5f)}, + pose); + + const auto observations = + makeGeometricClassObservations(cloud, classifier); + + ASSERT_EQ(observations.size(), 2u); + EXPECT_EQ(observations[0].value, + static_cast(GeometricClass::kGround)); + EXPECT_EQ(observations[1].value, + static_cast(GeometricClass::kObstacle)); + EXPECT_TRUE(observations[0].position.isApprox(Point3D(2.1f, 3.1f, 4.f))); + EXPECT_TRUE(observations[1].position.isApprox(Point3D(2.3f, 3.1f, 4.5f))); +} + +TEST(LocalElevationClassifierTest, FiltersObservationsBySensorRange) { + LocalElevationClassifierConfig config; + config.grid_cell_width = 10.f; + config.min_points_per_cell = 3u; + const LocalElevationClassifier classifier(config); + const auto cloud = makePointcloud({Point3D(0.1f, 0.f, 0.f), + Point3D(1.f, 0.f, 0.f), + Point3D(2.f, 0.f, 0.5f)}); + + const auto observations = makeGeometricClassObservations( + cloud, classifier, EndpointRange{0.5f, 1.5f}); + + ASSERT_EQ(observations.size(), 1u); + EXPECT_TRUE(observations.front().position.isApprox(Point3D(1.f, 0.f, 0.f))); +} + +} // namespace +} // namespace wavemap::layered From a9a2c57c73b56afe8fa02e1270d191d6a9ad5819 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Guilherne=20Caba=C3=A7o?= Date: Thu, 10 Sep 2026 19:27:17 +0000 Subject: [PATCH 10/14] Complete layered Wavemap integration Organize the typed layer APIs, support hashed and chunked continuous backends, and add discrete side-layer storage. Integrate layered maps with ROS inputs, persistence, incremental transport, RViz, and Ouster/RGB applications while removing obsolete prototypes and backup files. --- .gitignore | 10 + CODEX_CONTEXT.md | 412 ------------------ LAYERED_WAVEMAP_IMPLEMENTATION_REVIEW.txt | 406 ----------------- examples/cpp/CMakeLists.txt | 2 - examples/cpp/digital_twin/CMakeLists.txt | 341 --------------- examples/cpp/digital_twin/CMakeLists.txt.orig | 341 --------------- .../common/custom_layered_map_config.h | 171 -------- .../common/example_layered_map_config.h | 51 --- .../common/example_layered_map_ros_config.h | 172 -------- .../common/field_layer_policies.h | 37 -- .../digital_twin/common/field_layer_types.h | 14 - .../common/field_layered_map_config.h | 61 --- .../common/field_layered_map_ros_config.h | 117 ----- .../common/layered_ros_converter.h | 132 ------ .../common/layered_voxel_config.h | 220 ---------- .../common/layered_voxel_updater.h | 29 -- .../io/create_large_layered_map_experiment.cc | 120 ----- .../custom_layered_map_save_load_example.cc | 87 ---- .../io/inspect_field_layered_map.cc | 58 --- .../io/inspect_ouster_benchmark_maps.cc.orig | 211 --------- .../io/inspect_reflectivity_layered_map.cc | 98 ----- .../inspect_reflectivity_layered_map.cc.orig | 101 ----- .../io/layered_map_io_experiment.cc | 81 ---- .../io/layered_map_save_load_experiment.cc | 115 ----- ...ayered_map_save_load_failure_experiment.cc | 170 -------- .../io/legacy_file_io_experiment.cc | 51 --- ...legacy_hashed_wavelet_octree_experiment.cc | 38 -- .../io/load_existing_maps_experiment.cc | 139 ------ .../map_api/baseline/float_map_experiment.cc | 54 --- .../map_api/baseline/int_map_experiment.cc | 54 --- .../baseline/voxel_block_experiment.cc | 43 -- .../map_api/baseline/voxel_map_experiment.cc | 44 -- .../continuous_voxel_many_voxels_benchmark.cc | 98 ----- ...at_cast_discrete_many_voxels_experiment.cc | 168 ------- .../layered_pruning_policy_experiment.cc | 373 ---------------- .../occupancy_many_voxels_benchmark.cc | 79 ---- .../raw_discrete_many_voxels_experiment.cc | 108 ----- .../raw_inside_map_many_voxels_experiment.cc | 158 ------- .../continuous_voxel_map_experiment.cc | 65 --- .../heterogeneous_voxel_map_experiment.cc | 135 ------ .../layered_full_update_pruning_experiment.cc | 243 ----------- ...ed_hashed_wavelet_integrator_experiment.cc | 117 ----- ...ayered_occupancy_update_path_experiment.cc | 94 ---- .../layered_per_voxel_update_experiment.cc | 143 ------ .../continuous/layered_pruning_config.md | 60 --- .../layered_pruning_config_example.cc | 88 ---- .../continuous/mixed_voxel_map_experiment.cc | 97 ----- .../discrete_layer_compression_experiment.cc | 138 ------ ...screte_layer_update_behavior_experiment.cc | 112 ----- .../float_cast_discrete_experiment.cc | 147 ------- .../layered_label_storage_experiment.cc | 76 ---- .../discrete/layered_map_experiment.cc | 123 ------ .../layered_map_update_behavior_experiment.cc | 82 ---- .../raw_discrete_voxel_storage_experiment.cc | 90 ---- .../discrete/raw_inside_map_experiment.cc | 143 ------ ...semantic_majority_exceptions_experiment.cc | 242 ---------- .../user_facing/custom_layered_map_example.cc | 119 ----- .../field_endpoint_integration_experiment.cc | 68 --- .../field_layered_map_experiment.cc | 107 ----- .../field_layered_pipeline_experiment.cc | 80 ---- .../old_experiments/blocks_experiment.cc | 124 ------ .../old_experiments/experiment.cc | 58 --- .../partial_save_experiment.cc | 111 ----- .../old_experiments/publish_experiment.cc | 84 ---- .../ros/field_layer_update_ros_experiment.cc | 114 ----- .../ros/field_layered_map_ros_experiment.cc | 54 --- ...yered_map_structured_ros_msg_experiment.cc | 105 ----- .../ros/layered_ros_msg_experiment.cc | 71 --- ...native_sensor_layer_adapters_experiment.cc | 156 ------- .../ros/publish_layered_map_experiment.cc | 50 --- ...blish_structured_layered_map_experiment.cc | 102 ----- .../ros/subscribe_layered_map_experiment.cc | 50 --- ...cribe_structured_layered_map_experiment.cc | 109 ----- .../cpp/digital_twin/voxel_data/new_voxel.cc | 30 -- .../voxel_data/voxel_haar_transform.cc | 35 -- interfaces/ros1/wavemap_ros/CMakeLists.txt | 105 +++-- ...computed_point_field_binding_experiment.cc | 204 --------- .../app/field_layer_sensor_bridge.cc | 198 --------- .../layered_incremental_publish_experiment.cc | 145 ------ .../inspect_ouster_benchmark_maps.cc | 35 +- .../occupancy_chunked_sensor_inputs.cc | 44 ++ .../ouster_layered}/occupancy_map_layers.h | 18 +- .../occupancy_sensor_inputs.cc | 0 .../ouster_layered}/ouster_sensor_inputs.cc | 6 +- .../reflectivity_chunked_sensor_inputs.cc | 51 +++ ...eflectivity_class_chunked_sensor_inputs.cc | 58 +++ .../ouster_layered}/reflectivity_map_layers.h | 25 +- .../reflectivity_only_map_layers.h | 14 +- .../reflectivity_only_sensor_inputs.cc | 0 .../app/ouster_layered/rgb_map_layers.h | 43 ++ .../signal_class_sensor_inputs.cc | 61 +++ .../app/ouster_layered/signal_map_layers.h | 86 ++++ .../signal_near_ir_class_sensor_inputs.cc | 64 +++ .../signal_near_ir_sensor_inputs.cc | 50 +++ .../ouster_layered/signal_sensor_inputs.cc | 50 +++ .../publish_field_layer_updates_experiment.cc | 47 -- .../app/synthetic_rgb_layered_map_demo.cc | 200 +++++++++ .../benchmark_ouster_chunked_layered.yaml | 43 ++ ...mark_ouster_chunked_occupancy_layered.yaml | 42 ++ ...ark_ouster_chunked_occupancy_original.yaml | 50 +++ .../benchmark_ouster_hashed_layered.yaml | 43 ++ ...mark_ouster_hashed_occupancy_original.yaml | 50 +++ .../config/benchmark_ouster_variants.csv | 19 + .../rviz/synthetic_rgb_layered_map.rviz | 106 +++++ .../wavemap_ouster_all_layers_demo.yaml | 51 +++ ...p_ouster_reflectivity_chunked_layered.yaml | 48 ++ .../inputs/pointcloud_topic_input.h | 26 +- ...ayered_hashed_wavelet_integrator_factory.h | 45 +- .../layered_ros_server_extension.h | 12 +- .../wavemap_ros/layered_server_builder.h | 2 +- .../include/wavemap_ros/ros_server.h | 26 +- .../external_ouster_layered_smoke.launch | 33 -- ...xternal_ouster_occupancy_validation.launch | 29 -- .../launch/synthetic_rgb_layered_map.launch | 27 ++ .../src/inputs/pointcloud_topic_input.cc | 108 ++++- ...descriptor_layer_observation_conversions.h | 2 +- .../descriptor_layered_map_conversions.h | 4 +- .../discrete_layer_marker_conversions.h | 2 +- .../impl/layered_map_msg_conversions_inl.h | 158 ++++++- .../layer_observation_msg_conversions.h | 2 +- .../layered_map_msg_conversions.h | 2 +- .../map_msg_conversions.h | 9 + ...pointcloud_layer_observation_conversions.h | 2 +- .../test/src/test_map_msg_conversions.cc | 60 +++ .../ros1/wavemap_rviz_plugin/CMakeLists.txt | 3 +- .../wavemap_rviz_plugin/layered_map_display.h | 5 + .../visuals/voxel_visual.h | 2 + .../plugin_description.xml | 20 +- .../src/layered_map_display.cc | 40 +- .../src/layered_map_factories.cc | 194 +++++---- .../src/layered_map_factories.cc.orig | 169 ------- .../src/visuals/voxel_visual.cc | 51 ++- .../hashed_chunked_wavelet_integrator.h | 45 +- .../hashed_chunked_wavelet_integrator_inl.h | 192 +++++++- .../core/map/hashed_chunked_wavelet_octree.h | 67 ++- .../map/hashed_chunked_wavelet_octree_block.h | 73 ++-- .../hashed_chunked_wavelet_octree_block_inl.h | 314 ++++++++++++- .../impl/hashed_chunked_wavelet_octree_inl.h | 189 ++++++-- .../wavemap/io/impl/stream_conversions_inl.h | 100 +++++ .../include/wavemap/io/stream_conversions.h | 9 + .../local_elevation_classifier.h | 4 +- .../continuous_layer_updater.h | 0 .../endpoint_attribute_integrator.h | 10 +- .../endpoint_attribute_pointcloud.h | 2 +- .../{ => integration}/endpoint_range.h | 0 .../{ => integration}/layer_integrator.h | 10 +- .../{ => integration}/layer_observation.h | 0 .../layer_observation_batch.h | 4 +- .../{ => integration}/layer_update_policy.h | 2 +- .../{ => integration}/layered_pipeline.h | 4 +- .../pointcloud_layer_integration_mode.h | 0 .../{ => io}/descriptor_layered_map_io.h | 10 +- .../layered/{ => io}/layer_stream_codec.h | 0 .../wavemap/layered/{ => io}/layered_map_io.h | 6 +- .../layered/{ => io}/layered_map_schema.h | 0 .../layered/map/continuous_map_backend.h | 21 + .../layered/{ => map}/discrete_layer.h | 0 .../wavemap/layered/{ => map}/layered_map.h | 24 +- .../{ => map}/layered_map_config_builder.h | 2 +- .../{ => map}/layered_map_definition.h | 16 +- .../{ => schema}/layer_bundle_traits.h | 0 .../layered/{ => schema}/layer_descriptor.h | 2 +- .../layered/{ => schema}/layer_schema.h | 4 +- .../layered/{ => schema}/layer_traits.h | 0 .../{ => schema}/schema_continuous_traits.h | 2 +- .../{ => schema}/schema_layer_storage.h | 6 +- .../hashed_chunked_wavelet_integrator.cc | 162 ------- .../core/map/hashed_chunked_wavelet_octree.cc | 57 --- .../hashed_chunked_wavelet_octree_block.cc | 193 -------- .../integrator/test_pointcloud_integrators.cc | 7 +- .../core/map/test_layered_map_definition.cc | 68 ++- library/cpp/test/src/core/map/test_map.cc | 27 ++ .../query/test_local_elevation_classifier.cc | 2 +- 173 files changed, 3006 insertions(+), 10403 deletions(-) delete mode 100644 CODEX_CONTEXT.md delete mode 100644 LAYERED_WAVEMAP_IMPLEMENTATION_REVIEW.txt delete mode 100644 examples/cpp/digital_twin/CMakeLists.txt delete mode 100644 examples/cpp/digital_twin/CMakeLists.txt.orig delete mode 100644 examples/cpp/digital_twin/common/custom_layered_map_config.h delete mode 100644 examples/cpp/digital_twin/common/example_layered_map_config.h delete mode 100644 examples/cpp/digital_twin/common/example_layered_map_ros_config.h delete mode 100644 examples/cpp/digital_twin/common/field_layer_policies.h delete mode 100644 examples/cpp/digital_twin/common/field_layer_types.h delete mode 100644 examples/cpp/digital_twin/common/field_layered_map_config.h delete mode 100644 examples/cpp/digital_twin/common/field_layered_map_ros_config.h delete mode 100644 examples/cpp/digital_twin/common/layered_ros_converter.h delete mode 100644 examples/cpp/digital_twin/common/layered_voxel_config.h delete mode 100644 examples/cpp/digital_twin/common/layered_voxel_updater.h delete mode 100644 examples/cpp/digital_twin/io/create_large_layered_map_experiment.cc delete mode 100644 examples/cpp/digital_twin/io/custom_layered_map_save_load_example.cc delete mode 100644 examples/cpp/digital_twin/io/inspect_field_layered_map.cc delete mode 100644 examples/cpp/digital_twin/io/inspect_ouster_benchmark_maps.cc.orig delete mode 100644 examples/cpp/digital_twin/io/inspect_reflectivity_layered_map.cc delete mode 100644 examples/cpp/digital_twin/io/inspect_reflectivity_layered_map.cc.orig delete mode 100644 examples/cpp/digital_twin/io/layered_map_io_experiment.cc delete mode 100644 examples/cpp/digital_twin/io/layered_map_save_load_experiment.cc delete mode 100644 examples/cpp/digital_twin/io/layered_map_save_load_failure_experiment.cc delete mode 100644 examples/cpp/digital_twin/io/legacy_file_io_experiment.cc delete mode 100644 examples/cpp/digital_twin/io/legacy_hashed_wavelet_octree_experiment.cc delete mode 100644 examples/cpp/digital_twin/io/load_existing_maps_experiment.cc delete mode 100644 examples/cpp/digital_twin/map_api/baseline/float_map_experiment.cc delete mode 100644 examples/cpp/digital_twin/map_api/baseline/int_map_experiment.cc delete mode 100644 examples/cpp/digital_twin/map_api/baseline/voxel_block_experiment.cc delete mode 100644 examples/cpp/digital_twin/map_api/baseline/voxel_map_experiment.cc delete mode 100644 examples/cpp/digital_twin/map_api/benchmarks/continuous_voxel_many_voxels_benchmark.cc delete mode 100644 examples/cpp/digital_twin/map_api/benchmarks/float_cast_discrete_many_voxels_experiment.cc delete mode 100644 examples/cpp/digital_twin/map_api/benchmarks/layered_pruning_policy_experiment.cc delete mode 100644 examples/cpp/digital_twin/map_api/benchmarks/occupancy_many_voxels_benchmark.cc delete mode 100644 examples/cpp/digital_twin/map_api/benchmarks/raw_discrete_many_voxels_experiment.cc delete mode 100644 examples/cpp/digital_twin/map_api/benchmarks/raw_inside_map_many_voxels_experiment.cc delete mode 100644 examples/cpp/digital_twin/map_api/continuous/continuous_voxel_map_experiment.cc delete mode 100644 examples/cpp/digital_twin/map_api/continuous/heterogeneous_voxel_map_experiment.cc delete mode 100644 examples/cpp/digital_twin/map_api/continuous/layered_full_update_pruning_experiment.cc delete mode 100644 examples/cpp/digital_twin/map_api/continuous/layered_hashed_wavelet_integrator_experiment.cc delete mode 100644 examples/cpp/digital_twin/map_api/continuous/layered_occupancy_update_path_experiment.cc delete mode 100644 examples/cpp/digital_twin/map_api/continuous/layered_per_voxel_update_experiment.cc delete mode 100644 examples/cpp/digital_twin/map_api/continuous/layered_pruning_config.md delete mode 100644 examples/cpp/digital_twin/map_api/continuous/layered_pruning_config_example.cc delete mode 100644 examples/cpp/digital_twin/map_api/continuous/mixed_voxel_map_experiment.cc delete mode 100644 examples/cpp/digital_twin/map_api/discrete/discrete_layer_compression_experiment.cc delete mode 100644 examples/cpp/digital_twin/map_api/discrete/discrete_layer_update_behavior_experiment.cc delete mode 100644 examples/cpp/digital_twin/map_api/discrete/float_cast_discrete_experiment.cc delete mode 100644 examples/cpp/digital_twin/map_api/discrete/layered_label_storage_experiment.cc delete mode 100644 examples/cpp/digital_twin/map_api/discrete/layered_map_experiment.cc delete mode 100644 examples/cpp/digital_twin/map_api/discrete/layered_map_update_behavior_experiment.cc delete mode 100644 examples/cpp/digital_twin/map_api/discrete/raw_discrete_voxel_storage_experiment.cc delete mode 100644 examples/cpp/digital_twin/map_api/discrete/raw_inside_map_experiment.cc delete mode 100644 examples/cpp/digital_twin/map_api/discrete/semantic_majority_exceptions_experiment.cc delete mode 100644 examples/cpp/digital_twin/map_api/user_facing/custom_layered_map_example.cc delete mode 100644 examples/cpp/digital_twin/map_api/user_facing/field_endpoint_integration_experiment.cc delete mode 100644 examples/cpp/digital_twin/map_api/user_facing/field_layered_map_experiment.cc delete mode 100644 examples/cpp/digital_twin/map_api/user_facing/field_layered_pipeline_experiment.cc delete mode 100644 examples/cpp/digital_twin/old_experiments/blocks_experiment.cc delete mode 100644 examples/cpp/digital_twin/old_experiments/experiment.cc delete mode 100644 examples/cpp/digital_twin/old_experiments/partial_save_experiment.cc delete mode 100644 examples/cpp/digital_twin/old_experiments/publish_experiment.cc delete mode 100644 examples/cpp/digital_twin/ros/field_layer_update_ros_experiment.cc delete mode 100644 examples/cpp/digital_twin/ros/field_layered_map_ros_experiment.cc delete mode 100644 examples/cpp/digital_twin/ros/layered_map_structured_ros_msg_experiment.cc delete mode 100644 examples/cpp/digital_twin/ros/layered_ros_msg_experiment.cc delete mode 100644 examples/cpp/digital_twin/ros/native_sensor_layer_adapters_experiment.cc delete mode 100644 examples/cpp/digital_twin/ros/publish_layered_map_experiment.cc delete mode 100644 examples/cpp/digital_twin/ros/publish_structured_layered_map_experiment.cc delete mode 100644 examples/cpp/digital_twin/ros/subscribe_layered_map_experiment.cc delete mode 100644 examples/cpp/digital_twin/ros/subscribe_structured_layered_map_experiment.cc delete mode 100644 examples/cpp/digital_twin/voxel_data/new_voxel.cc delete mode 100644 examples/cpp/digital_twin/voxel_data/voxel_haar_transform.cc delete mode 100644 interfaces/ros1/wavemap_ros/app/computed_point_field_binding_experiment.cc delete mode 100644 interfaces/ros1/wavemap_ros/app/field_layer_sensor_bridge.cc delete mode 100644 interfaces/ros1/wavemap_ros/app/layered_incremental_publish_experiment.cc rename {examples/cpp/digital_twin/io => interfaces/ros1/wavemap_ros/app/ouster_layered}/inspect_ouster_benchmark_maps.cc (81%) create mode 100644 interfaces/ros1/wavemap_ros/app/ouster_layered/occupancy_chunked_sensor_inputs.cc rename {examples/cpp/digital_twin/ouster_mapping => interfaces/ros1/wavemap_ros/app/ouster_layered}/occupancy_map_layers.h (55%) rename {examples/cpp/digital_twin/ouster_mapping => interfaces/ros1/wavemap_ros/app/ouster_layered}/occupancy_sensor_inputs.cc (100%) rename {examples/cpp/digital_twin/ouster_mapping => interfaces/ros1/wavemap_ros/app/ouster_layered}/ouster_sensor_inputs.cc (88%) create mode 100644 interfaces/ros1/wavemap_ros/app/ouster_layered/reflectivity_chunked_sensor_inputs.cc create mode 100644 interfaces/ros1/wavemap_ros/app/ouster_layered/reflectivity_class_chunked_sensor_inputs.cc rename {examples/cpp/digital_twin/ouster_mapping => interfaces/ros1/wavemap_ros/app/ouster_layered}/reflectivity_map_layers.h (71%) rename {examples/cpp/digital_twin/ouster_mapping => interfaces/ros1/wavemap_ros/app/ouster_layered}/reflectivity_only_map_layers.h (63%) rename {examples/cpp/digital_twin/ouster_mapping => interfaces/ros1/wavemap_ros/app/ouster_layered}/reflectivity_only_sensor_inputs.cc (100%) create mode 100644 interfaces/ros1/wavemap_ros/app/ouster_layered/rgb_map_layers.h create mode 100644 interfaces/ros1/wavemap_ros/app/ouster_layered/signal_class_sensor_inputs.cc create mode 100644 interfaces/ros1/wavemap_ros/app/ouster_layered/signal_map_layers.h create mode 100644 interfaces/ros1/wavemap_ros/app/ouster_layered/signal_near_ir_class_sensor_inputs.cc create mode 100644 interfaces/ros1/wavemap_ros/app/ouster_layered/signal_near_ir_sensor_inputs.cc create mode 100644 interfaces/ros1/wavemap_ros/app/ouster_layered/signal_sensor_inputs.cc delete mode 100644 interfaces/ros1/wavemap_ros/app/publish_field_layer_updates_experiment.cc create mode 100644 interfaces/ros1/wavemap_ros/app/synthetic_rgb_layered_map_demo.cc create mode 100644 interfaces/ros1/wavemap_ros/config/benchmark_ouster_chunked_layered.yaml create mode 100644 interfaces/ros1/wavemap_ros/config/benchmark_ouster_chunked_occupancy_layered.yaml create mode 100644 interfaces/ros1/wavemap_ros/config/benchmark_ouster_chunked_occupancy_original.yaml create mode 100644 interfaces/ros1/wavemap_ros/config/benchmark_ouster_hashed_layered.yaml create mode 100644 interfaces/ros1/wavemap_ros/config/benchmark_ouster_hashed_occupancy_original.yaml create mode 100644 interfaces/ros1/wavemap_ros/config/benchmark_ouster_variants.csv create mode 100644 interfaces/ros1/wavemap_ros/config/rviz/synthetic_rgb_layered_map.rviz create mode 100644 interfaces/ros1/wavemap_ros/config/wavemap_ouster_all_layers_demo.yaml create mode 100644 interfaces/ros1/wavemap_ros/config/wavemap_ouster_reflectivity_chunked_layered.yaml delete mode 100644 interfaces/ros1/wavemap_ros/launch/datasets/external_ouster_layered_smoke.launch delete mode 100644 interfaces/ros1/wavemap_ros/launch/datasets/external_ouster_occupancy_validation.launch create mode 100644 interfaces/ros1/wavemap_ros/launch/synthetic_rgb_layered_map.launch delete mode 100644 interfaces/ros1/wavemap_rviz_plugin/src/layered_map_factories.cc.orig rename library/cpp/include/wavemap/layered/{ => classification}/local_elevation_classifier.h (97%) rename library/cpp/include/wavemap/layered/{ => integration}/continuous_layer_updater.h (100%) rename library/cpp/include/wavemap/layered/{ => integration}/endpoint_attribute_integrator.h (91%) rename library/cpp/include/wavemap/layered/{ => integration}/endpoint_attribute_pointcloud.h (98%) rename library/cpp/include/wavemap/layered/{ => integration}/endpoint_range.h (100%) rename library/cpp/include/wavemap/layered/{ => integration}/layer_integrator.h (94%) rename library/cpp/include/wavemap/layered/{ => integration}/layer_observation.h (100%) rename library/cpp/include/wavemap/layered/{ => integration}/layer_observation_batch.h (96%) rename library/cpp/include/wavemap/layered/{ => integration}/layer_update_policy.h (99%) rename library/cpp/include/wavemap/layered/{ => integration}/layered_pipeline.h (93%) rename library/cpp/include/wavemap/layered/{ => integration}/pointcloud_layer_integration_mode.h (100%) rename library/cpp/include/wavemap/layered/{ => io}/descriptor_layered_map_io.h (93%) rename library/cpp/include/wavemap/layered/{ => io}/layer_stream_codec.h (100%) rename library/cpp/include/wavemap/layered/{ => io}/layered_map_io.h (99%) rename library/cpp/include/wavemap/layered/{ => io}/layered_map_schema.h (100%) create mode 100644 library/cpp/include/wavemap/layered/map/continuous_map_backend.h rename library/cpp/include/wavemap/layered/{ => map}/discrete_layer.h (100%) rename library/cpp/include/wavemap/layered/{ => map}/layered_map.h (79%) rename library/cpp/include/wavemap/layered/{ => map}/layered_map_config_builder.h (98%) rename library/cpp/include/wavemap/layered/{ => map}/layered_map_definition.h (82%) rename library/cpp/include/wavemap/layered/{ => schema}/layer_bundle_traits.h (100%) rename library/cpp/include/wavemap/layered/{ => schema}/layer_descriptor.h (98%) rename library/cpp/include/wavemap/layered/{ => schema}/layer_schema.h (97%) rename library/cpp/include/wavemap/layered/{ => schema}/layer_traits.h (100%) rename library/cpp/include/wavemap/layered/{ => schema}/schema_continuous_traits.h (99%) rename library/cpp/include/wavemap/layered/{ => schema}/schema_layer_storage.h (97%) diff --git a/.gitignore b/.gitignore index 7874f4da8..23073ec0b 100644 --- a/.gitignore +++ b/.gitignore @@ -61,3 +61,13 @@ docs/cpp_api docs/_doxygen_cpp docs/_doxygen_ros1 docs/_build + +# Local benchmark launch and monitoring utilities +/interfaces/ros1/wavemap_ros/launch/benchmark_*.launch +/interfaces/ros1/wavemap_ros/launch/benchmarks/ +/interfaces/ros1/wavemap_ros/scripts/monitor_wavemap_benchmark.py +/interfaces/ros1/wavemap_ros/scripts/run_ouster_benchmark.py +/interfaces/ros1/wavemap_ros/scripts/summarize_wavemap_benchmark.py + +# Local backup files +*.orig diff --git a/CODEX_CONTEXT.md b/CODEX_CONTEXT.md deleted file mode 100644 index 1fd8472f9..000000000 --- a/CODEX_CONTEXT.md +++ /dev/null @@ -1,412 +0,0 @@ -# Codex Context: Layered Wavemap Work - -## Repository State - -Workspace: `/home/ci/catkin_ws/src/wavemap` - -The current work extends Wavemap with a layered map architecture for digital-twin style voxel data: - -- continuous data stored in a templated wavelet octree; -- discrete/categorical data stored in compressed discrete layers; -- `.lwvmp` persistence for full layered maps; -- ROS1 server support for loading/saving/publishing layered maps; -- RViz support for visualizing layered maps from topic or file. - -Original Wavemap behavior should remain available and should not be broken. The layered functionality is additive. - -## Main Design Decisions - -### Continuous Data - -Continuous layers stay inside a wavelet map. The current example type is based on: - -- occupancy; -- RGB/color; -- traversability. - -Continuous fields must support the arithmetic needed by the Haar/wavelet transform: - -- addition; -- subtraction; -- scalar multiplication; -- threshold/clamp/pruning traits. - -This is why continuous float-like data makes sense, while categorical labels do not. - -### Discrete Data - -Discrete data is stored separately from the continuous wavelet octree. - -Current example discrete layers: - -- `semantic`: integer label; -- `changed`: boolean flag. - -The goal is not to hardcode these forever. They are example layers. The intended user-facing pattern is: - -```cpp -struct MyDiscreteLayers { - DiscreteLayer semantic; - DiscreteLayer changed; -}; -``` - -Discrete data uses a compressed parent/exceptions representation. It does not use Haar arithmetic. - -### Full Layered Map - -The intended top-level structure is: - -```text -LayeredMap - continuous_map // wavelet octree with occupancy + continuous fields - discrete_layers // templated discrete layer struct -``` - -Naming decisions: - -- `ContinuousWaveletMap`: continuous wavelet map alias; -- `LayeredMap`: full map aggregation; -- `LayeredMapConfig`: full config; -- `ExampleLayeredMap`: current example config/type. - -## Implemented Components - -### Core / Library - -Implemented or modified: - -- `library/cpp/include/wavemap/layered/layered_map.h` -- `library/cpp/include/wavemap/layered/layered_map_io.h` -- `library/cpp/include/wavemap/layered/layered_map_schema.h` - -Current functionality: - -- full `LayeredMap` abstraction; -- continuous + discrete access; -- `.lwvmp` save/load; -- schema metadata stored in `.lwvmp`; -- schema compatibility checks. - -### Example Config - -Implemented or modified: - -- `examples/cpp/digital_twin/common/layered_voxel_config.h` -- `examples/cpp/digital_twin/common/example_layered_map_config.h` -- `examples/cpp/digital_twin/common/example_layered_map_ros_config.h` -- `examples/cpp/digital_twin/common/layered_ros_converter.h` - -Current example layers: - -```text -continuous: - occupancy - color / RGB - traversability - -discrete: - semantic - changed -``` - -### ROS Server - -Implemented or modified: - -- `interfaces/ros1/wavemap_ros/include/wavemap_ros/layered_ros_server_extension.h` -- `interfaces/ros1/wavemap_ros/include/wavemap_ros/ros_server.h` -- `interfaces/ros1/wavemap_ros/src/ros_server.cc` -- `interfaces/ros1/wavemap_ros/CMakeLists.txt` - -The layered extension is attached to the existing `RosServer`. - -It provides: - -```bash -/wavemap/layered_map -/wavemap/load_layered_map -/wavemap/save_layered_map -/wavemap/layered_map_request_full -``` - -Confirmed runtime log: - -```text -Layered map extension enabled. Advertising layered map topic and services. -``` - -The original Wavemap services/topics remain separate: - -```bash -/wavemap/map -/wavemap/load_map -/wavemap/save_map -/wavemap/map_request_full -``` - -Important caveat: - -- the original occupancy map and the layered map are currently separate internal maps; -- `/wavemap/load_map` loads `.wvmp`; -- `/wavemap/load_layered_map` loads `.lwvmp`. - -### ROS / RViz Conversions - -Implemented or modified: - -- `interfaces/ros1/wavemap_ros_conversions/include/wavemap_ros_conversions/discrete_layer_marker_conversions.h` -- layered ROS conversion helpers in example/common config. - -### RViz Plugin - -Implemented or modified: - -- `interfaces/ros1/wavemap_rviz_plugin/include/wavemap_rviz_plugin/layered_map_display.h` -- `interfaces/ros1/wavemap_rviz_plugin/src/layered_map_display.cc` -- `interfaces/ros1/wavemap_rviz_plugin/include/wavemap_rviz_plugin/common.h` -- `interfaces/ros1/wavemap_rviz_plugin/include/wavemap_rviz_plugin/visuals/voxel_visual.h` -- `interfaces/ros1/wavemap_rviz_plugin/src/visuals/voxel_visual.cc` -- `interfaces/ros1/wavemap_rviz_plugin/include/wavemap_rviz_plugin/wavemap_map_display.h` -- `interfaces/ros1/wavemap_rviz_plugin/src/wavemap_map_display.cc` -- `interfaces/ros1/wavemap_rviz_plugin/CMakeLists.txt` -- `interfaces/ros1/wavemap_rviz_plugin/plugin_description.xml` - -Current RViz functionality: - -- new `LayeredMap` display; -- source can be topic or file; -- topic default: `/wavemap/layered_map`; -- file loading supports `.lwvmp`; -- dynamic layer dropdown, not hardcoded to only semantic/changed; -- continuous visualization uses typed reconstruction/factory path; -- discrete visualization supports integer and bool layers; -- viewport legend/card overlay works; -- legend uses generated 1x1 textures for color swatches. - -Known RViz notes: - -- fixed frame should usually be `map`; -- if RViz says there is no TF data, set global fixed frame correctly; -- if launching inside container gives `qt.qpa.xcb: could not connect to display`, this is a display/X issue, not necessarily plugin failure. - -### Example / Experiment Files - -Implemented or modified: - -- `examples/cpp/digital_twin/CMakeLists.txt` -- `examples/cpp/digital_twin/io/create_large_layered_map_experiment.cc` -- `examples/cpp/digital_twin/io/load_existing_maps_experiment.cc` -- `examples/cpp/digital_twin/io/layered_map_save_load_failure_experiment.cc` -- `examples/cpp/digital_twin/ros/publish_structured_layered_map_experiment.cc` - -Useful generated test map: - -```text -/home/ci/data/maps/layered_map_large.lwvmp -``` - -Regenerate it with: - -```bash -cd ~/catkin_ws/src/wavemap -source /opt/ros/noetic/setup.bash -cmake --build examples/cpp/build --target create_large_layered_map_experiment -./examples/cpp/build/digital_twin/create_large_layered_map_experiment -``` - -Recent output from the large map example: - -```text -populated continuous voxels: 307200 -populated discrete voxels: 19128 -continuous blocks before prune: 4 -continuous blocks after prune: 4 -continuous nodes before prune: 43912 -continuous nodes after prune: 43912 -semantic parents: 456 exceptions: 1474 observed: 19128 -changed parents: 456 exceptions: 616 observed: 19128 -file size bytes: 5850724 -``` - -## Useful Commands - -### Build RViz Plugin - -```bash -cd ~/catkin_ws -source /opt/ros/noetic/setup.bash -source devel/setup.bash -catkin build wavemap_rviz_plugin --no-status -``` - -Last known result: build succeeded with no warnings. - -### Launch Server - -The correct dt architecture config file is: - -```text -/home/ci/catkin_ws/src/dt_architecture/load_config_file.yaml -``` - -Launch: - -```bash -cd ~/catkin_ws -source /opt/ros/noetic/setup.bash -source devel/setup.bash -roslaunch wavemap_ros wavemap_server.launch param_file:=/home/ci/catkin_ws/src/dt_architecture/load_config_file.yaml -``` - -If the launch also starts RViz and RViz crashes, the server may still be fine. Check for: - -```text -Layered map extension enabled. Advertising layered map topic and services. -``` - -### Check Layered Topics/Services - -In another terminal: - -```bash -cd ~/catkin_ws -source /opt/ros/noetic/setup.bash -source devel/setup.bash -rosservice list | grep layered -rostopic list | grep layered -``` - -Expected: - -```bash -/wavemap/load_layered_map -/wavemap/save_layered_map -/wavemap/layered_map_request_full -/wavemap/layered_map -``` - -### Load Layered Map Through Server - -```bash -rosservice call /wavemap/load_layered_map "file_path: '/home/ci/data/maps/layered_map_large.lwvmp'" -rosservice call /wavemap/layered_map_request_full "{}" -rostopic echo -n 1 /wavemap/layered_map -``` - -### Open RViz Manually - -```bash -cd ~/catkin_ws -source /opt/ros/noetic/setup.bash -source devel/setup.bash -rviz -``` - -Then add the `LayeredMap` display and select: - -```text -Topic: /wavemap/layered_map -``` - -Or use file mode and select: - -```text -/home/ci/data/maps/layered_map_large.lwvmp -``` - -## Current Git Commit Recommendation - -If not already committed, commit all current layered-map pipeline changes as one checkpoint: - -```bash -cd ~/catkin_ws/src/wavemap -git add . -git commit -m "Add layered map ROS and RViz pipeline" -``` - -A previous stable commit exists: - -```text -0e940190 Add layered map support -``` - -## What Is Still Missing - -The main missing piece is the live update pipeline for extra layers. - -Currently implemented: - -```text -create/load LayeredMap -save/load .lwvmp -publish /wavemap/layered_map -visualize in RViz -``` - -Still missing: - -```text -incoming live layer observations -> update LayeredMap -> publish updated /wavemap/layered_map -``` - -Since extra data comes per voxel, the planned path is: - -1. Add ROS message(s) for layered voxel updates. -2. Add converter/helper from update message to `LayeredMap` update. -3. Add subscriber in `LayeredRosServerExtension`. -4. Update continuous layers by replacement for now. -5. Update discrete layers with `DiscreteLayer::set(...)` or equivalent. -6. Publish `/wavemap/layered_map` after update. -7. Add publisher experiment for live update testing. -8. Test in RViz. - -Suggested first update message, example-specific for now: - -```text -LayeredVoxelUpdate - geometry_msgs/Point position - float32 r - float32 g - float32 b - float32 traversability - int32 semantic - bool changed - -LayeredVoxelUpdateArray - std_msgs/Header header - LayeredVoxelUpdate[] updates -``` - -Important design choice: - -- first version should require update positions to already be in `world_frame`; -- TF transformation can be added later; -- original occupancy integration should remain untouched. - -Live behavior target: - -```text -original Wavemap ray integration -> occupancy map -layered voxel update topic -> continuous/discrete extra layers -server publishes -> /wavemap/layered_map -``` - -Potential future work: - -- sync occupancy from original occupancy map into `LayeredMap` continuous map; -- generic schema-driven update messages instead of example-specific messages; -- richer RViz color policies configured by user; -- validation with real bag data. - -## Bag / External Drive Context - -The user has a bag outside the environment on an external drive: - -```text -/media/guilhermecabaco/CABACO_02/rosbag -``` - -It may require mounting/bind-mounting into the container/environment. The user does not want to copy the bag because it is too large. - -Previous attempts to bind mount to `/home/ci/data/external_bag` or `/tmp/external_bag` failed with permission denied. If the environment is restarted with the bind mount configured, this Codex session may be lost. This file exists to make the work resumable. diff --git a/LAYERED_WAVEMAP_IMPLEMENTATION_REVIEW.txt b/LAYERED_WAVEMAP_IMPLEMENTATION_REVIEW.txt deleted file mode 100644 index 4142b7f0c..000000000 --- a/LAYERED_WAVEMAP_IMPLEMENTATION_REVIEW.txt +++ /dev/null @@ -1,406 +0,0 @@ -LAYERED WAVEMAP IMPLEMENTATION REVIEW -===================================== - -Date: 2026-08-07 -Scope: Current layered-map core, persistence, ROS server/conversions, RViz, - examples, and the proposed live voxel-update path. - - -1. CURRENT ARCHITECTURE ------------------------ - -The current RosServer owns two distinct map instances: - -1. occupancy_map_ - - Created by MapFactory from the existing "map" parameters. - - Passed to Pipeline. - - Receives the existing sensor/ray integration. - - Published and saved through the original Wavemap interfaces. - -2. layered_extension_.layered_map_ - - Constructed independently as ExampleLayeredMap. - - Its continuousMap() is a typed HashedWaveletOctree whose voxel contains: - occupancy + RGB + traversability - - Its discrete bundle contains: - semantic + changed - - Published and saved through the layered interfaces. - -Within the layered continuous map, occupancy, RGB, and traversability really do -share one wavelet octree. However, this layered octree is not the same object as -RosServer::occupancy_map_. There is currently no pointer/reference connection or -automatic synchronization between the two instances. - - -2. DOES LAYERED OCCUPANCY WORK LIKE ORIGINAL OCCUPANCY? -------------------------------------------------------- - -Mechanically, yes: - -- LayeredVoxel is VoxelData. -- VoxelData stores occupancy as a FloatingPoint and supports the arithmetic - required by the existing integrators and Haar transform. -- HashedWaveletOctreeT derives from MapBase. -- Existing experiments exercise ray-tracing and hashed-wavelet integration on - the typed map. -- Occupancy-only updates preserve the additional continuous data. -- Continuous-layer replacement can preserve occupancy by applying a voxel - update whose occupancy delta is zero. - -Equivalent implementation does not automatically mean identical runtime map. -For two independent maps to produce matching occupancy, they must receive the -same measurements in the same order with equivalent: - -- coordinate frame and poses; -- min/max range and measurement model; -- voxel resolution and tree height; -- min/max log-odds thresholds; -- pruning configuration and timing; -- update scheduling and concurrency behavior. - -The active dt_architecture config aligns both grids at 0.25 m. The layered map -explicitly uses min_log_odds=-2, max_log_odds=4, and tree_height=6. The original -map relies partly on the normal map defaults because only type and resolution -are explicitly configured. These effective configs should be compared or made -explicit before claiming exact equivalence. - -Most importantly, the current original sensor pipeline only receives -occupancy_map_. Therefore the layered occupancy field remains independent (and -usually unobserved/default) unless a layered file is loaded or it is populated -through another path. - - -3. HIGH-PRIORITY CORRECTNESS IMPROVEMENTS ------------------------------------------ - -3.1 Decide and document the occupancy relationship - -Choose one supported model explicitly: - -A. Compatibility-first dual maps - Keep original Wavemap untouched and keep layered occupancy independent. - Clearly document that /wavemap/map and /wavemap/layered_map may disagree. - -B. Dual integration - Feed the same observations to the original and typed layered octrees. - This duplicates computation and memory, and exact equivalence requires - matched configs and deterministic update ordering. - -C. Synchronize occupancy into the layered map - Copy/reconstruct occupancy periodically or before layered publication. - This is potentially expensive and requires a well-defined incremental sync. - -D. One typed continuous map with a compatibility view - Use the typed map as the integrated source and expose occupancy through the - legacy MapBase/topic/service interfaces. This is architecturally cleaner but - has the greatest risk of changing original behavior and needs extensive - compatibility tests. - -The current code behaves as A, but that behavior is implicit. - -3.2 Add the missing live update path - -Add example-specific ROS messages for the first implementation: - -LayeredVoxelUpdate.msg: - geometry_msgs/Point position - float32 r - float32 g - float32 b - float32 traversability - int32 semantic - bool changed - -LayeredVoxelUpdateArray.msg: - std_msgs/Header header - LayeredVoxelUpdate[] updates - -Add geometry_msgs to wavemap_msgs message-generation and package dependencies. - -3.3 Reuse the existing continuous replacement helper - -Use setContinuousLayersAtVoxel() from layered_voxel_updater.h. It computes the -difference between observed and stored continuous data, then applies: - - LayeredVoxel(0.f, data_update) - -This preserves layered occupancy while replacing RGB and traversability. Avoid -adding a second replacement implementation with subtly different semantics. - -3.4 Use the existing Wavemap position-to-index convention - -Convert world positions with pointToNearestIndex(position, -1 / min_cell_width). Do not use a custom floor(position / resolution), because -Wavemap defines indices through its voxel-center convention and handles negative -coordinates accordingly. - -3.5 Lock all access to the layered map - -The live callback must update continuous and discrete layers while holding the -same mutex used by load/save/publish. Do not mutate through getLayeredMap(), -because that method returns an unprotected mutable reference. - -Prefer a member function such as applyVoxelUpdates() that owns locking and -returns the publication stamp/result. Consider removing the mutable public -getter or replacing it with explicitly locked visitor/access functions. - -3.6 Fix load concurrency - -loadLayeredMap() currently copies layered_map_ before taking mutex_. Since the -LayeredMap copy contains a shared_ptr to the continuous map, concurrent update, -save, or publish operations can race with this initial copy/load workflow. -Protect the snapshot/replacement correctly, or load into a fresh map created -from a safely copied config without sharing mutable map storage. - -3.7 Make reset behavior consistent - -RosServer::clear() clears inputs, pipeline, and occupancy_map_, but does not -clear the layered map. Decide whether reset_map should also clear layered data. -If compatibility requires separate behavior, add a dedicated reset_layered_map -service and document the distinction. - - -4. LIVE UPDATE MESSAGE SEMANTICS TO DEFINE ------------------------------------------- - -Before implementing the subscriber, define these behaviors explicitly: - -- Version 1 requires header.frame_id == configured world_frame. -- Reject an empty or mismatched frame; do not silently interpret it. -- No TF conversion in version 1. Add TF later as a separate feature. -- Validate positions and float fields with std::isfinite. -- Define RGB range as [0, 1]. Decide whether invalid values are rejected or - clamped; logging and stored behavior must agree. -- Define traversability range as [0, 1]. -- Define valid semantic label values, including whether a sentinel means - unknown/unobserved. -- Decide whether changed is an input observation or a value derived by comparing - old and new data. It can remain explicit for the first pipeline experiment. -- All fields are currently mandatory and use replacement semantics. -- If several updates map to one voxel, apply message order and let the last valid - update win, or deduplicate explicitly. Document the choice. -- Define whether one invalid update rejects the whole batch or only that entry. - Skipping invalid entries with throttled warnings is practical for live input. -- Define maximum accepted batch size to prevent a malformed message from causing - excessive latency or memory use. -- Publish once per accepted batch, never once per voxel. -- Preserve the incoming timestamp when publishing the resulting state where - useful, rather than always replacing it with ros::Time::now(). - -Future messages may need per-field validity masks so a producer can update only -semantic, only RGB, etc. The first fixed message has no way to distinguish -"replace with zero/false" from "field not observed". - - -5. DISCRETE-LAYER IMPROVEMENTS ------------------------------- - -- The actual update method is setValue(), not set(). Use the existing API. -- Add an erase/set-unobserved operation. Currently a value can be set but there - is no public way to make one voxel unknown again. -- Remove empty parent cells after erase support is added. -- setValue() expands and rebuilds the entire compressed parent cell on every - voxel update. This is acceptable for 2x2x2 parents and initial validation, - but batch updates should group observations by parent to avoid repeated work. -- Validate DiscreteCompressionConfig.block_height. Negative values and large - shifts can cause invalid or overflowing side lengths/counts. -- Validate deserialized observed and exception offsets against the configured - parent voxel count. -- Validate that every exception offset is also observed. -- Reject duplicate parent cells in ROS/file input instead of silently allowing - later values to replace earlier values. -- Standardize serialized type names (currently examples use strings such as - "int" and "bool") and consider stable versioned identifiers. -- Add equality/comparison helpers to make round-trip and update tests clearer. - - -6. CONTINUOUS MAP, THRESHOLD, AND PRUNING IMPROVEMENTS ------------------------------------------------------ - -- Document that continuous replacement creates/updates wavelet coefficients and - should be followed by thresholding according to a defined schedule. -- Decide when prune() runs for live updates. Pruning every message may be too - expensive; never pruning can grow memory indefinitely. -- Verify that combined pruning weights cannot discard meaningful occupancy due - to interactions with RGB/traversability scores. -- Add tests for updates at negative indices, block boundaries, and exact voxel - boundaries. -- Add tests proving occupancy is unchanged after repeated continuous replacement, - threshold, prune, save/load, and ROS round trips. -- Add tests for non-finite and out-of-range continuous values. -- Ensure both original and layered effective occupancy thresholds are explicit - when equivalence is expected. - - -7. ROS PUBLICATION AND PERFORMANCE IMPROVEMENTS ------------------------------------------------ - -- publishLayeredMap() serializes the complete continuous octree and all discrete - cells. Publishing a full map after every high-rate update batch will become a - CPU, allocation, bandwidth, and RViz bottleneck. -- For the first small experiment, one full publish per batch is acceptable. -- For real data, coalesce updates and publish at a configurable maximum rate. -- Add incremental/delta layered messages later, including block allocation and - deletion semantics. -- Keep the existing full latched topic for new subscribers and explicit full-map - requests. -- Avoid holding the map mutex longer than necessary, but do not serialize from - mutable map storage without a safe snapshot strategy. -- Add counters/timing for received, accepted, skipped, and deduplicated updates; - update latency; serialization time; message bytes; and publication rate. -- Make subscriber queue size and publish rate configurable. -- Add backpressure/drop behavior instead of allowing stale update batches to - accumulate. -- Consider an AsyncSpinner only after map locking and callback ordering semantics - are explicitly safe. - - -8. ROS CONVERSION ROBUSTNESS ----------------------------- - -- LayeredVoxelRosConverter assumes fixed layer positions and directly indexes - arrays without validating names, types, layer counts, or value lengths. - Malformed messages can cause out-of-range access. -- Validate continuous layer schema before reading cell data. -- Validate expected RGB value count (3 per coefficient) and traversability count - (1 per coefficient). -- Return structured conversion errors rather than only bool where practical. -- Validate wavelet node count/stack consistency and child bitsets during map - reconstruction. -- Validate discrete layer ordering or resolve layers by name. The current bundle - conversion requires exactly the expected count and positional order. -- Add message/schema versioning for forward compatibility. -- Add fuzz or malformed-message tests for every conversion entry point. - - -9. SERVER AND PACKAGE STRUCTURE IMPROVEMENTS --------------------------------------------- - -- wavemap_ros and the RViz plugin directly include configuration headers from - examples/cpp/digital_twin/common through private include paths. Installed - packages may not contain those example headers, making the feature fragile - outside this source workspace. -- Move the supported example layered types/converters into an installable - library/package, or create a dedicated wavemap_digital_twin package. -- Keep generic LayeredMap, I/O, and conversion mechanisms in library/interface - packages; keep example schema policy in the example/digital-twin package. -- Avoid hardwiring ExampleLayeredMap construction inside RosServer long term. - Use a factory/extension registration mechanism or an explicitly optional - digital-twin server component. -- Validate layered_map configuration with the same rigor as existing ConfigBase - configurations, including compression and threshold constraints. -- Improve startup logging to report effective layered resolution, tree height, - thresholds, schema, update topic, and occupancy relationship mode. - - -10. RVIZ IMPROVEMENTS ---------------------- - -- The RViz LayeredMap display is also compiled against example configuration - headers. Separate generic display mechanics from example continuous-layer - color policies. -- Validate incoming schemas and array lengths before reconstruction. -- Avoid rebuilding all geometry unnecessarily when only a small layer delta is - received (after delta transport exists). -- Add limits/decimation for very large maps to prevent CPU/GPU memory exhaustion. -- Expose voxel count, message size, reconstruction time, and rejected-message - errors in display status. -- Preserve the dynamic layer dropdown, but handle removed/renamed layers without - retaining an invalid selection. -- Test topic and file modes with empty maps, huge maps, malformed maps, frame - changes, and repeated load/unload cycles. - - -11. PERSISTENCE AND SCHEMA IMPROVEMENTS --------------------------------------- - -- Add formal unit tests for .lwvmp round trips, incompatible schemas, truncated - files, corrupt counts, invalid compression offsets, and unsupported versions. -- Apply bounds before allocating from file-provided counts to avoid excessive - memory allocation on corrupt files. -- Include an explicit format version and migration policy if not already fully - enforced by the current header/schema implementation. -- Consider checksums for large files or sections so corruption is detected early. -- Ensure save is atomic: write a temporary file, flush/close successfully, then - rename into place so failure cannot destroy the previous valid map. -- Document endianness and numeric representation if files must be portable. - - -12. TESTING IMPROVEMENTS ------------------------- - -Many current checks are standalone experiment executables. Preserve them, but -promote the critical behavior into automated unit/integration tests: - -- original occupancy integration remains unchanged when layered support is off; -- enabling the extension does not alter original topics/services/results; -- typed layered occupancy matches scalar occupancy under identical config and - identical observation sequences; -- continuous replacement preserves occupancy; -- occupancy integration preserves RGB/traversability; -- discrete values remain sparse/unknown correctly; -- negative-coordinate compression and indexing; -- continuous and discrete save/load round trips; -- ROS message round trips and malformed-input rejection; -- frame mismatch and invalid-value behavior; -- concurrent update/save/load/publish/reset behavior; -- live publisher -> server -> LayeredMap topic -> RViz/converter test; -- save-after-live-update and reload verification; -- bounded publication rate and queue behavior under load; -- compatibility build with layered support disabled. - - -13. RECOMMENDED IMPLEMENTATION ORDER ------------------------------------- - -Phase 1: Safe proof of live updates - -1. Define and document the occupancy relationship (retain dual-map behavior for - compatibility during this phase). -2. Add the two example-specific update messages and dependencies. -3. Add a validated ROS-to-observation converter using pointToNearestIndex(). -4. Reuse setContinuousLayersAtVoxel() and DiscreteLayer::setValue(). -5. Add a mutex-protected update method/subscriber inside the extension. -6. Publish one full map per small test batch. -7. Add the publisher experiment. -8. Add automated occupancy-preservation and end-to-end tests. -9. Test live RViz and save-after-update. - -Phase 2: Correctness and operational hardening - -1. Fix load/reset/accessor concurrency and semantics. -2. Add strict message and deserialization validation. -3. Add publish throttling/coalescing and diagnostics. -4. Make original/layered effective configs explicit and comparable. -5. Move example headers out of private cross-package include paths. - -Phase 3: Occupancy unification decision - -1. Benchmark dual integration, incremental synchronization, and compatibility - view approaches. -2. Select one based on CPU, memory, exactness, and backward-compatibility needs. -3. Implement it behind an explicit configuration mode. -4. Add equivalence and regression tests before changing defaults. - -Phase 4: Generalization - -1. Add optional/per-field updates and validity masks. -2. Add TF support. -3. Add schema-driven generic updates only after real producer requirements are - known. -4. Add incremental layered publication and RViz application of deltas. - - -14. IMMEDIATE CONCLUSION ------------------------- - -The typed layered octree can perform occupancy integration similarly to the -original occupancy octree. The current server simply does not route the original -live integration into it. Keeping the original octree untouched is a valid and -useful compatibility strategy, but the relationship between the two occupancy -representations must be explicit to users and tests. - -The safest next implementation is an example-specific, world-frame-only voxel -update subscriber that changes RGB, traversability, semantic, and changed while -preserving the layered occupancy field. Occupancy synchronization/unification -should be treated as a separate architectural decision, not accidentally mixed -into the first live-layer update patch. diff --git a/examples/cpp/CMakeLists.txt b/examples/cpp/CMakeLists.txt index bbb5fbc9c..9cbadf540 100644 --- a/examples/cpp/CMakeLists.txt +++ b/examples/cpp/CMakeLists.txt @@ -34,5 +34,3 @@ endif () add_subdirectory(io) add_subdirectory(queries) add_subdirectory(planning) - -add_subdirectory(digital_twin) diff --git a/examples/cpp/digital_twin/CMakeLists.txt b/examples/cpp/digital_twin/CMakeLists.txt deleted file mode 100644 index d803b4616..000000000 --- a/examples/cpp/digital_twin/CMakeLists.txt +++ /dev/null @@ -1,341 +0,0 @@ -# Binaries -add_executable(new_voxel voxel_data/new_voxel.cc) -set_wavemap_target_properties(new_voxel) -target_link_libraries(new_voxel PUBLIC wavemap::wavemap_core) - -add_executable(voxel_haar_transform voxel_data/voxel_haar_transform.cc) -set_wavemap_target_properties(voxel_haar_transform) -target_link_libraries(voxel_haar_transform PUBLIC wavemap::wavemap_core) - -add_executable(voxel_block_experiment map_api/baseline/voxel_block_experiment.cc) -set_wavemap_target_properties(voxel_block_experiment) -target_link_libraries(voxel_block_experiment PUBLIC wavemap::wavemap_core) - -add_executable(voxel_map_experiment map_api/baseline/voxel_map_experiment.cc) -set_wavemap_target_properties(voxel_map_experiment) -target_link_libraries(voxel_map_experiment PUBLIC wavemap::wavemap_core) - -add_executable(int_map_experiment map_api/baseline/int_map_experiment.cc) -set_wavemap_target_properties(int_map_experiment) -target_link_libraries(int_map_experiment PUBLIC wavemap::wavemap_core) - -add_executable(float_map_experiment map_api/baseline/float_map_experiment.cc) -set_wavemap_target_properties(float_map_experiment) -target_link_libraries(float_map_experiment PUBLIC wavemap::wavemap_core) - -add_executable(continuous_voxel_map_experiment - map_api/continuous/continuous_voxel_map_experiment.cc) -set_wavemap_target_properties(continuous_voxel_map_experiment) -target_link_libraries(continuous_voxel_map_experiment - PUBLIC wavemap::wavemap_core) - -add_executable(layered_occupancy_update_path_experiment - map_api/continuous/layered_occupancy_update_path_experiment.cc) -set_wavemap_target_properties(layered_occupancy_update_path_experiment) -target_link_libraries(layered_occupancy_update_path_experiment - PUBLIC wavemap::wavemap_core) - -add_executable(layered_hashed_wavelet_integrator_experiment - map_api/continuous/layered_hashed_wavelet_integrator_experiment.cc) -set_wavemap_target_properties(layered_hashed_wavelet_integrator_experiment) -target_link_libraries(layered_hashed_wavelet_integrator_experiment - PUBLIC wavemap::wavemap_core) - -add_executable(layered_per_voxel_update_experiment - map_api/continuous/layered_per_voxel_update_experiment.cc) -set_wavemap_target_properties(layered_per_voxel_update_experiment) -target_link_libraries(layered_per_voxel_update_experiment - PUBLIC wavemap::wavemap_core) - -add_executable(layered_full_update_pruning_experiment - map_api/continuous/layered_full_update_pruning_experiment.cc) -set_wavemap_target_properties(layered_full_update_pruning_experiment) -target_link_libraries(layered_full_update_pruning_experiment - PUBLIC wavemap::wavemap_core) - -add_executable(layered_pruning_config_example - map_api/continuous/layered_pruning_config_example.cc) -set_wavemap_target_properties(layered_pruning_config_example) -target_link_libraries(layered_pruning_config_example - PUBLIC wavemap::wavemap_core) - -add_executable(mixed_voxel_map_experiment - map_api/continuous/mixed_voxel_map_experiment.cc) -set_wavemap_target_properties(mixed_voxel_map_experiment) -target_link_libraries(mixed_voxel_map_experiment PUBLIC wavemap::wavemap_core) - -add_executable(layered_label_storage_experiment - map_api/discrete/layered_label_storage_experiment.cc) -set_wavemap_target_properties(layered_label_storage_experiment) -target_link_libraries(layered_label_storage_experiment - PUBLIC wavemap::wavemap_core) - -add_executable(layered_map_experiment - map_api/discrete/layered_map_experiment.cc) -set_wavemap_target_properties(layered_map_experiment) -target_link_libraries(layered_map_experiment - PUBLIC wavemap::wavemap_core) - -add_executable(custom_layered_map_example - map_api/user_facing/custom_layered_map_example.cc) -set_wavemap_target_properties(custom_layered_map_example) -target_link_libraries(custom_layered_map_example - PUBLIC wavemap::wavemap_core) - -add_executable(field_layered_map_experiment - map_api/user_facing/field_layered_map_experiment.cc) -set_wavemap_target_properties(field_layered_map_experiment) -target_link_libraries(field_layered_map_experiment - PUBLIC wavemap::wavemap_core) - -add_executable(field_layered_pipeline_experiment - map_api/user_facing/field_layered_pipeline_experiment.cc) -set_wavemap_target_properties(field_layered_pipeline_experiment) -target_link_libraries(field_layered_pipeline_experiment - PUBLIC wavemap::wavemap_core) - -add_executable(field_endpoint_integration_experiment - map_api/user_facing/field_endpoint_integration_experiment.cc) -set_wavemap_target_properties(field_endpoint_integration_experiment) -target_link_libraries(field_endpoint_integration_experiment - PUBLIC wavemap::wavemap_core) - -add_executable(inspect_field_layered_map - io/inspect_field_layered_map.cc) -set_wavemap_target_properties(inspect_field_layered_map) -target_link_libraries(inspect_field_layered_map - PUBLIC wavemap::wavemap_core) - -add_executable(inspect_reflectivity_layered_map - io/inspect_reflectivity_layered_map.cc) -set_wavemap_target_properties(inspect_reflectivity_layered_map) -target_link_libraries(inspect_reflectivity_layered_map - PUBLIC wavemap::wavemap_core) - -add_executable(inspect_ouster_benchmark_maps - io/inspect_ouster_benchmark_maps.cc) -set_wavemap_target_properties(inspect_ouster_benchmark_maps) -target_link_libraries(inspect_ouster_benchmark_maps - PUBLIC wavemap::wavemap_core wavemap::wavemap_io) - -add_executable(discrete_layer_compression_experiment - map_api/discrete/discrete_layer_compression_experiment.cc) -set_wavemap_target_properties(discrete_layer_compression_experiment) -target_link_libraries(discrete_layer_compression_experiment - PUBLIC wavemap::wavemap_core) - -add_executable(discrete_layer_update_behavior_experiment - map_api/discrete/discrete_layer_update_behavior_experiment.cc) -set_wavemap_target_properties(discrete_layer_update_behavior_experiment) -target_link_libraries(discrete_layer_update_behavior_experiment - PUBLIC wavemap::wavemap_core) - -add_executable(layered_map_update_behavior_experiment - map_api/discrete/layered_map_update_behavior_experiment.cc) -set_wavemap_target_properties(layered_map_update_behavior_experiment) -target_link_libraries(layered_map_update_behavior_experiment - PUBLIC wavemap::wavemap_core) - -add_executable(heterogeneous_voxel_map_experiment - map_api/continuous/heterogeneous_voxel_map_experiment.cc) -set_wavemap_target_properties(heterogeneous_voxel_map_experiment) -target_link_libraries(heterogeneous_voxel_map_experiment - PUBLIC wavemap::wavemap_core) - -add_executable(raw_discrete_voxel_storage_experiment - map_api/discrete/raw_discrete_voxel_storage_experiment.cc) -set_wavemap_target_properties(raw_discrete_voxel_storage_experiment) -target_link_libraries(raw_discrete_voxel_storage_experiment - PUBLIC wavemap::wavemap_core) - -add_executable(raw_inside_map_experiment - map_api/discrete/raw_inside_map_experiment.cc) -set_wavemap_target_properties(raw_inside_map_experiment) -target_link_libraries(raw_inside_map_experiment - PUBLIC wavemap::wavemap_core) - -add_executable(float_cast_discrete_experiment - map_api/discrete/float_cast_discrete_experiment.cc) -set_wavemap_target_properties(float_cast_discrete_experiment) -target_link_libraries(float_cast_discrete_experiment - PUBLIC wavemap::wavemap_core) - -add_executable(float_cast_discrete_many_voxels_experiment - map_api/benchmarks/float_cast_discrete_many_voxels_experiment.cc) -set_wavemap_target_properties(float_cast_discrete_many_voxels_experiment) -target_link_libraries(float_cast_discrete_many_voxels_experiment - PUBLIC wavemap::wavemap_core) - -add_executable(raw_discrete_many_voxels_experiment - map_api/benchmarks/raw_discrete_many_voxels_experiment.cc) -set_wavemap_target_properties(raw_discrete_many_voxels_experiment) -target_link_libraries(raw_discrete_many_voxels_experiment - PUBLIC wavemap::wavemap_core) - -add_executable(raw_inside_map_many_voxels_experiment - map_api/benchmarks/raw_inside_map_many_voxels_experiment.cc) -set_wavemap_target_properties(raw_inside_map_many_voxels_experiment) -target_link_libraries(raw_inside_map_many_voxels_experiment - PUBLIC wavemap::wavemap_core) - -add_executable(occupancy_many_voxels_benchmark - map_api/benchmarks/occupancy_many_voxels_benchmark.cc) -set_wavemap_target_properties(occupancy_many_voxels_benchmark) -target_link_libraries(occupancy_many_voxels_benchmark - PUBLIC wavemap::wavemap_core) - -add_executable(continuous_voxel_many_voxels_benchmark - map_api/benchmarks/continuous_voxel_many_voxels_benchmark.cc) -set_wavemap_target_properties(continuous_voxel_many_voxels_benchmark) -target_link_libraries(continuous_voxel_many_voxels_benchmark - PUBLIC wavemap::wavemap_core) - -add_executable(layered_pruning_policy_experiment - map_api/benchmarks/layered_pruning_policy_experiment.cc) -set_wavemap_target_properties(layered_pruning_policy_experiment) -target_link_libraries(layered_pruning_policy_experiment - PUBLIC wavemap::wavemap_core) - - -add_executable(semantic_majority_exceptions_experiment - map_api/discrete/semantic_majority_exceptions_experiment.cc) -set_wavemap_target_properties(semantic_majority_exceptions_experiment) -target_link_libraries(semantic_majority_exceptions_experiment - PUBLIC wavemap::wavemap_core) - -add_executable(legacy_hashed_wavelet_octree_experiment - io/legacy_hashed_wavelet_octree_experiment.cc) -set_wavemap_target_properties(legacy_hashed_wavelet_octree_experiment) -target_link_libraries(legacy_hashed_wavelet_octree_experiment - PUBLIC wavemap::wavemap_core) - -add_executable(legacy_file_io_experiment io/legacy_file_io_experiment.cc) -set_wavemap_target_properties(legacy_file_io_experiment) -target_link_libraries(legacy_file_io_experiment - PUBLIC wavemap::wavemap_core wavemap::wavemap_io) - -add_executable(layered_map_io_experiment io/layered_map_io_experiment.cc) -set_wavemap_target_properties(layered_map_io_experiment) -target_link_libraries(layered_map_io_experiment PUBLIC wavemap::wavemap_core) - -add_executable(layered_map_save_load_experiment io/layered_map_save_load_experiment.cc) -set_wavemap_target_properties(layered_map_save_load_experiment) -target_link_libraries(layered_map_save_load_experiment PUBLIC wavemap::wavemap_core) - -add_executable(layered_map_save_load_failure_experiment - io/layered_map_save_load_failure_experiment.cc) -set_wavemap_target_properties(layered_map_save_load_failure_experiment) -target_link_libraries(layered_map_save_load_failure_experiment - PUBLIC wavemap::wavemap_core) - -add_executable(custom_layered_map_save_load_example - io/custom_layered_map_save_load_example.cc) -set_wavemap_target_properties(custom_layered_map_save_load_example) -target_link_libraries(custom_layered_map_save_load_example - PUBLIC wavemap::wavemap_core) - -add_executable(load_existing_maps_experiment - io/load_existing_maps_experiment.cc) -set_wavemap_target_properties(load_existing_maps_experiment) -target_link_libraries(load_existing_maps_experiment - PUBLIC wavemap::wavemap_core wavemap::wavemap_io) - -add_executable(create_large_layered_map_experiment - io/create_large_layered_map_experiment.cc) -set_wavemap_target_properties(create_large_layered_map_experiment) -target_link_libraries(create_large_layered_map_experiment - PUBLIC wavemap::wavemap_core) - -if ("$ENV{ROS_VERSION}" STREQUAL "1") - find_package(catkin REQUIRED - COMPONENTS roscpp visualization_msgs wavemap wavemap_msgs wavemap_ros_conversions) - - add_executable(experiment old_experiments/experiment.cc) - set_wavemap_target_properties(experiment) - target_include_directories(experiment PUBLIC ${catkin_INCLUDE_DIRS}) - target_link_libraries(experiment PUBLIC - wavemap::wavemap_core wavemap::wavemap_io ${catkin_LIBRARIES}) - - add_executable(publish_experiment old_experiments/publish_experiment.cc) - set_wavemap_target_properties(publish_experiment) - target_include_directories(publish_experiment PUBLIC ${catkin_INCLUDE_DIRS}) - target_link_libraries(publish_experiment PUBLIC - wavemap::wavemap_core wavemap::wavemap_io ${catkin_LIBRARIES}) - - add_executable(blocks_experiment old_experiments/blocks_experiment.cc) - set_wavemap_target_properties(blocks_experiment) - target_include_directories(blocks_experiment PUBLIC ${catkin_INCLUDE_DIRS}) - target_link_libraries(blocks_experiment PUBLIC - wavemap::wavemap_core wavemap::wavemap_io ${catkin_LIBRARIES}) - - add_executable(partial_save_experiment old_experiments/partial_save_experiment.cc) - set_wavemap_target_properties(partial_save_experiment) - target_include_directories(partial_save_experiment PUBLIC ${catkin_INCLUDE_DIRS}) - target_link_libraries(partial_save_experiment PUBLIC - wavemap::wavemap_core wavemap::wavemap_io ${catkin_LIBRARIES}) - - add_executable(layered_ros_msg_experiment ros/layered_ros_msg_experiment.cc) - set_wavemap_target_properties(layered_ros_msg_experiment) - target_include_directories(layered_ros_msg_experiment PUBLIC ${catkin_INCLUDE_DIRS}) - target_link_libraries(layered_ros_msg_experiment PUBLIC - wavemap::wavemap_core ${catkin_LIBRARIES}) - - add_executable(layered_map_structured_ros_msg_experiment - ros/layered_map_structured_ros_msg_experiment.cc) - set_wavemap_target_properties(layered_map_structured_ros_msg_experiment) - target_include_directories(layered_map_structured_ros_msg_experiment PUBLIC ${catkin_INCLUDE_DIRS}) - target_link_libraries(layered_map_structured_ros_msg_experiment PUBLIC - wavemap::wavemap_core ${catkin_LIBRARIES}) - - add_executable(publish_structured_layered_map_experiment - ros/publish_structured_layered_map_experiment.cc) - set_wavemap_target_properties(publish_structured_layered_map_experiment) - target_include_directories(publish_structured_layered_map_experiment PUBLIC ${catkin_INCLUDE_DIRS}) - target_link_libraries(publish_structured_layered_map_experiment PUBLIC - wavemap::wavemap_core ${catkin_LIBRARIES}) - - add_executable(subscribe_structured_layered_map_experiment - ros/subscribe_structured_layered_map_experiment.cc) - set_wavemap_target_properties(subscribe_structured_layered_map_experiment) - target_include_directories(subscribe_structured_layered_map_experiment PUBLIC ${catkin_INCLUDE_DIRS}) - target_link_libraries(subscribe_structured_layered_map_experiment PUBLIC - wavemap::wavemap_core ${catkin_LIBRARIES}) - - add_executable(publish_layered_map_experiment ros/publish_layered_map_experiment.cc) - set_wavemap_target_properties(publish_layered_map_experiment) - target_include_directories(publish_layered_map_experiment PUBLIC ${catkin_INCLUDE_DIRS}) - target_link_libraries(publish_layered_map_experiment PUBLIC - wavemap::wavemap_core ${catkin_LIBRARIES}) - - add_executable(subscribe_layered_map_experiment ros/subscribe_layered_map_experiment.cc) - set_wavemap_target_properties(subscribe_layered_map_experiment) - target_include_directories(subscribe_layered_map_experiment PUBLIC ${catkin_INCLUDE_DIRS}) - target_link_libraries(subscribe_layered_map_experiment PUBLIC - wavemap::wavemap_core ${catkin_LIBRARIES}) - - add_executable(field_layered_map_ros_experiment - ros/field_layered_map_ros_experiment.cc) - set_wavemap_target_properties(field_layered_map_ros_experiment) - target_include_directories(field_layered_map_ros_experiment - PUBLIC ${catkin_INCLUDE_DIRS}) - target_link_libraries(field_layered_map_ros_experiment PUBLIC - wavemap::wavemap_core ${catkin_LIBRARIES}) - - add_executable(field_layer_update_ros_experiment - ros/field_layer_update_ros_experiment.cc) - set_wavemap_target_properties(field_layer_update_ros_experiment) - target_include_directories(field_layer_update_ros_experiment - PUBLIC ${catkin_INCLUDE_DIRS}) - target_link_libraries(field_layer_update_ros_experiment PUBLIC - wavemap::wavemap_core ${catkin_LIBRARIES}) - - add_executable(native_sensor_layer_adapters_experiment - ros/native_sensor_layer_adapters_experiment.cc) - set_wavemap_target_properties(native_sensor_layer_adapters_experiment) - target_include_directories(native_sensor_layer_adapters_experiment - PUBLIC ${catkin_INCLUDE_DIRS}) - target_link_libraries(native_sensor_layer_adapters_experiment PUBLIC - wavemap::wavemap_core ${catkin_LIBRARIES}) -else () - message(STATUS "Skipping ROS digital_twin examples because ROS1 is not active.") -endif () diff --git a/examples/cpp/digital_twin/CMakeLists.txt.orig b/examples/cpp/digital_twin/CMakeLists.txt.orig deleted file mode 100644 index 04e789d86..000000000 --- a/examples/cpp/digital_twin/CMakeLists.txt.orig +++ /dev/null @@ -1,341 +0,0 @@ -# Binaries -add_executable(new_voxel voxel_data/new_voxel.cc) -set_wavemap_target_properties(new_voxel) -target_link_libraries(new_voxel PUBLIC wavemap::wavemap_core) - -add_executable(voxel_haar_transform voxel_data/voxel_haar_transform.cc) -set_wavemap_target_properties(voxel_haar_transform) -target_link_libraries(voxel_haar_transform PUBLIC wavemap::wavemap_core) - -add_executable(voxel_block_experiment map_api/baseline/voxel_block_experiment.cc) -set_wavemap_target_properties(voxel_block_experiment) -target_link_libraries(voxel_block_experiment PUBLIC wavemap::wavemap_core) - -add_executable(voxel_map_experiment map_api/baseline/voxel_map_experiment.cc) -set_wavemap_target_properties(voxel_map_experiment) -target_link_libraries(voxel_map_experiment PUBLIC wavemap::wavemap_core) - -add_executable(int_map_experiment map_api/baseline/int_map_experiment.cc) -set_wavemap_target_properties(int_map_experiment) -target_link_libraries(int_map_experiment PUBLIC wavemap::wavemap_core) - -add_executable(float_map_experiment map_api/baseline/float_map_experiment.cc) -set_wavemap_target_properties(float_map_experiment) -target_link_libraries(float_map_experiment PUBLIC wavemap::wavemap_core) - -add_executable(continuous_voxel_map_experiment - map_api/continuous/continuous_voxel_map_experiment.cc) -set_wavemap_target_properties(continuous_voxel_map_experiment) -target_link_libraries(continuous_voxel_map_experiment - PUBLIC wavemap::wavemap_core) - -add_executable(layered_occupancy_update_path_experiment - map_api/continuous/layered_occupancy_update_path_experiment.cc) -set_wavemap_target_properties(layered_occupancy_update_path_experiment) -target_link_libraries(layered_occupancy_update_path_experiment - PUBLIC wavemap::wavemap_core) - -add_executable(layered_hashed_wavelet_integrator_experiment - map_api/continuous/layered_hashed_wavelet_integrator_experiment.cc) -set_wavemap_target_properties(layered_hashed_wavelet_integrator_experiment) -target_link_libraries(layered_hashed_wavelet_integrator_experiment - PUBLIC wavemap::wavemap_core) - -add_executable(layered_per_voxel_update_experiment - map_api/continuous/layered_per_voxel_update_experiment.cc) -set_wavemap_target_properties(layered_per_voxel_update_experiment) -target_link_libraries(layered_per_voxel_update_experiment - PUBLIC wavemap::wavemap_core) - -add_executable(layered_full_update_pruning_experiment - map_api/continuous/layered_full_update_pruning_experiment.cc) -set_wavemap_target_properties(layered_full_update_pruning_experiment) -target_link_libraries(layered_full_update_pruning_experiment - PUBLIC wavemap::wavemap_core) - -add_executable(layered_pruning_config_example - map_api/continuous/layered_pruning_config_example.cc) -set_wavemap_target_properties(layered_pruning_config_example) -target_link_libraries(layered_pruning_config_example - PUBLIC wavemap::wavemap_core) - -add_executable(mixed_voxel_map_experiment - map_api/continuous/mixed_voxel_map_experiment.cc) -set_wavemap_target_properties(mixed_voxel_map_experiment) -target_link_libraries(mixed_voxel_map_experiment PUBLIC wavemap::wavemap_core) - -add_executable(layered_label_storage_experiment - map_api/discrete/layered_label_storage_experiment.cc) -set_wavemap_target_properties(layered_label_storage_experiment) -target_link_libraries(layered_label_storage_experiment - PUBLIC wavemap::wavemap_core) - -add_executable(layered_map_experiment - map_api/discrete/layered_map_experiment.cc) -set_wavemap_target_properties(layered_map_experiment) -target_link_libraries(layered_map_experiment - PUBLIC wavemap::wavemap_core) - -add_executable(custom_layered_map_example - map_api/user_facing/custom_layered_map_example.cc) -set_wavemap_target_properties(custom_layered_map_example) -target_link_libraries(custom_layered_map_example - PUBLIC wavemap::wavemap_core) - -add_executable(field_layered_map_experiment - map_api/user_facing/field_layered_map_experiment.cc) -set_wavemap_target_properties(field_layered_map_experiment) -target_link_libraries(field_layered_map_experiment - PUBLIC wavemap::wavemap_core) - -add_executable(field_layered_pipeline_experiment - map_api/user_facing/field_layered_pipeline_experiment.cc) -set_wavemap_target_properties(field_layered_pipeline_experiment) -target_link_libraries(field_layered_pipeline_experiment - PUBLIC wavemap::wavemap_core) - -add_executable(field_endpoint_integration_experiment - map_api/user_facing/field_endpoint_integration_experiment.cc) -set_wavemap_target_properties(field_endpoint_integration_experiment) -target_link_libraries(field_endpoint_integration_experiment - PUBLIC wavemap::wavemap_core) - -add_executable(inspect_field_layered_map - io/inspect_field_layered_map.cc) -set_wavemap_target_properties(inspect_field_layered_map) -target_link_libraries(inspect_field_layered_map - PUBLIC wavemap::wavemap_core) - -add_executable(inspect_reflectivity_layered_map - io/inspect_reflectivity_layered_map.cc) -set_wavemap_target_properties(inspect_reflectivity_layered_map) -target_link_libraries(inspect_reflectivity_layered_map - PUBLIC wavemap::wavemap_core) - -add_executable(inspect_ouster_benchmark_maps - io/inspect_ouster_benchmark_maps.cc) -set_wavemap_target_properties(inspect_ouster_benchmark_maps) -target_link_libraries(inspect_ouster_benchmark_maps - PUBLIC wavemap::wavemap_core) - -add_executable(discrete_layer_compression_experiment - map_api/discrete/discrete_layer_compression_experiment.cc) -set_wavemap_target_properties(discrete_layer_compression_experiment) -target_link_libraries(discrete_layer_compression_experiment - PUBLIC wavemap::wavemap_core) - -add_executable(discrete_layer_update_behavior_experiment - map_api/discrete/discrete_layer_update_behavior_experiment.cc) -set_wavemap_target_properties(discrete_layer_update_behavior_experiment) -target_link_libraries(discrete_layer_update_behavior_experiment - PUBLIC wavemap::wavemap_core) - -add_executable(layered_map_update_behavior_experiment - map_api/discrete/layered_map_update_behavior_experiment.cc) -set_wavemap_target_properties(layered_map_update_behavior_experiment) -target_link_libraries(layered_map_update_behavior_experiment - PUBLIC wavemap::wavemap_core) - -add_executable(heterogeneous_voxel_map_experiment - map_api/continuous/heterogeneous_voxel_map_experiment.cc) -set_wavemap_target_properties(heterogeneous_voxel_map_experiment) -target_link_libraries(heterogeneous_voxel_map_experiment - PUBLIC wavemap::wavemap_core) - -add_executable(raw_discrete_voxel_storage_experiment - map_api/discrete/raw_discrete_voxel_storage_experiment.cc) -set_wavemap_target_properties(raw_discrete_voxel_storage_experiment) -target_link_libraries(raw_discrete_voxel_storage_experiment - PUBLIC wavemap::wavemap_core) - -add_executable(raw_inside_map_experiment - map_api/discrete/raw_inside_map_experiment.cc) -set_wavemap_target_properties(raw_inside_map_experiment) -target_link_libraries(raw_inside_map_experiment - PUBLIC wavemap::wavemap_core) - -add_executable(float_cast_discrete_experiment - map_api/discrete/float_cast_discrete_experiment.cc) -set_wavemap_target_properties(float_cast_discrete_experiment) -target_link_libraries(float_cast_discrete_experiment - PUBLIC wavemap::wavemap_core) - -add_executable(float_cast_discrete_many_voxels_experiment - map_api/benchmarks/float_cast_discrete_many_voxels_experiment.cc) -set_wavemap_target_properties(float_cast_discrete_many_voxels_experiment) -target_link_libraries(float_cast_discrete_many_voxels_experiment - PUBLIC wavemap::wavemap_core) - -add_executable(raw_discrete_many_voxels_experiment - map_api/benchmarks/raw_discrete_many_voxels_experiment.cc) -set_wavemap_target_properties(raw_discrete_many_voxels_experiment) -target_link_libraries(raw_discrete_many_voxels_experiment - PUBLIC wavemap::wavemap_core) - -add_executable(raw_inside_map_many_voxels_experiment - map_api/benchmarks/raw_inside_map_many_voxels_experiment.cc) -set_wavemap_target_properties(raw_inside_map_many_voxels_experiment) -target_link_libraries(raw_inside_map_many_voxels_experiment - PUBLIC wavemap::wavemap_core) - -add_executable(occupancy_many_voxels_benchmark - map_api/benchmarks/occupancy_many_voxels_benchmark.cc) -set_wavemap_target_properties(occupancy_many_voxels_benchmark) -target_link_libraries(occupancy_many_voxels_benchmark - PUBLIC wavemap::wavemap_core) - -add_executable(continuous_voxel_many_voxels_benchmark - map_api/benchmarks/continuous_voxel_many_voxels_benchmark.cc) -set_wavemap_target_properties(continuous_voxel_many_voxels_benchmark) -target_link_libraries(continuous_voxel_many_voxels_benchmark - PUBLIC wavemap::wavemap_core) - -add_executable(layered_pruning_policy_experiment - map_api/benchmarks/layered_pruning_policy_experiment.cc) -set_wavemap_target_properties(layered_pruning_policy_experiment) -target_link_libraries(layered_pruning_policy_experiment - PUBLIC wavemap::wavemap_core) - - -add_executable(semantic_majority_exceptions_experiment - map_api/discrete/semantic_majority_exceptions_experiment.cc) -set_wavemap_target_properties(semantic_majority_exceptions_experiment) -target_link_libraries(semantic_majority_exceptions_experiment - PUBLIC wavemap::wavemap_core) - -add_executable(legacy_hashed_wavelet_octree_experiment - io/legacy_hashed_wavelet_octree_experiment.cc) -set_wavemap_target_properties(legacy_hashed_wavelet_octree_experiment) -target_link_libraries(legacy_hashed_wavelet_octree_experiment - PUBLIC wavemap::wavemap_core) - -add_executable(legacy_file_io_experiment io/legacy_file_io_experiment.cc) -set_wavemap_target_properties(legacy_file_io_experiment) -target_link_libraries(legacy_file_io_experiment - PUBLIC wavemap::wavemap_core wavemap::wavemap_io) - -add_executable(layered_map_io_experiment io/layered_map_io_experiment.cc) -set_wavemap_target_properties(layered_map_io_experiment) -target_link_libraries(layered_map_io_experiment PUBLIC wavemap::wavemap_core) - -add_executable(layered_map_save_load_experiment io/layered_map_save_load_experiment.cc) -set_wavemap_target_properties(layered_map_save_load_experiment) -target_link_libraries(layered_map_save_load_experiment PUBLIC wavemap::wavemap_core) - -add_executable(layered_map_save_load_failure_experiment - io/layered_map_save_load_failure_experiment.cc) -set_wavemap_target_properties(layered_map_save_load_failure_experiment) -target_link_libraries(layered_map_save_load_failure_experiment - PUBLIC wavemap::wavemap_core) - -add_executable(custom_layered_map_save_load_example - io/custom_layered_map_save_load_example.cc) -set_wavemap_target_properties(custom_layered_map_save_load_example) -target_link_libraries(custom_layered_map_save_load_example - PUBLIC wavemap::wavemap_core) - -add_executable(load_existing_maps_experiment - io/load_existing_maps_experiment.cc) -set_wavemap_target_properties(load_existing_maps_experiment) -target_link_libraries(load_existing_maps_experiment - PUBLIC wavemap::wavemap_core wavemap::wavemap_io) - -add_executable(create_large_layered_map_experiment - io/create_large_layered_map_experiment.cc) -set_wavemap_target_properties(create_large_layered_map_experiment) -target_link_libraries(create_large_layered_map_experiment - PUBLIC wavemap::wavemap_core) - -if ("$ENV{ROS_VERSION}" STREQUAL "1") - find_package(catkin REQUIRED - COMPONENTS roscpp visualization_msgs wavemap wavemap_msgs wavemap_ros_conversions) - - add_executable(experiment old_experiments/experiment.cc) - set_wavemap_target_properties(experiment) - target_include_directories(experiment PUBLIC ${catkin_INCLUDE_DIRS}) - target_link_libraries(experiment PUBLIC - wavemap::wavemap_core wavemap::wavemap_io ${catkin_LIBRARIES}) - - add_executable(publish_experiment old_experiments/publish_experiment.cc) - set_wavemap_target_properties(publish_experiment) - target_include_directories(publish_experiment PUBLIC ${catkin_INCLUDE_DIRS}) - target_link_libraries(publish_experiment PUBLIC - wavemap::wavemap_core wavemap::wavemap_io ${catkin_LIBRARIES}) - - add_executable(blocks_experiment old_experiments/blocks_experiment.cc) - set_wavemap_target_properties(blocks_experiment) - target_include_directories(blocks_experiment PUBLIC ${catkin_INCLUDE_DIRS}) - target_link_libraries(blocks_experiment PUBLIC - wavemap::wavemap_core wavemap::wavemap_io ${catkin_LIBRARIES}) - - add_executable(partial_save_experiment old_experiments/partial_save_experiment.cc) - set_wavemap_target_properties(partial_save_experiment) - target_include_directories(partial_save_experiment PUBLIC ${catkin_INCLUDE_DIRS}) - target_link_libraries(partial_save_experiment PUBLIC - wavemap::wavemap_core wavemap::wavemap_io ${catkin_LIBRARIES}) - - add_executable(layered_ros_msg_experiment ros/layered_ros_msg_experiment.cc) - set_wavemap_target_properties(layered_ros_msg_experiment) - target_include_directories(layered_ros_msg_experiment PUBLIC ${catkin_INCLUDE_DIRS}) - target_link_libraries(layered_ros_msg_experiment PUBLIC - wavemap::wavemap_core ${catkin_LIBRARIES}) - - add_executable(layered_map_structured_ros_msg_experiment - ros/layered_map_structured_ros_msg_experiment.cc) - set_wavemap_target_properties(layered_map_structured_ros_msg_experiment) - target_include_directories(layered_map_structured_ros_msg_experiment PUBLIC ${catkin_INCLUDE_DIRS}) - target_link_libraries(layered_map_structured_ros_msg_experiment PUBLIC - wavemap::wavemap_core ${catkin_LIBRARIES}) - - add_executable(publish_structured_layered_map_experiment - ros/publish_structured_layered_map_experiment.cc) - set_wavemap_target_properties(publish_structured_layered_map_experiment) - target_include_directories(publish_structured_layered_map_experiment PUBLIC ${catkin_INCLUDE_DIRS}) - target_link_libraries(publish_structured_layered_map_experiment PUBLIC - wavemap::wavemap_core ${catkin_LIBRARIES}) - - add_executable(subscribe_structured_layered_map_experiment - ros/subscribe_structured_layered_map_experiment.cc) - set_wavemap_target_properties(subscribe_structured_layered_map_experiment) - target_include_directories(subscribe_structured_layered_map_experiment PUBLIC ${catkin_INCLUDE_DIRS}) - target_link_libraries(subscribe_structured_layered_map_experiment PUBLIC - wavemap::wavemap_core ${catkin_LIBRARIES}) - - add_executable(publish_layered_map_experiment ros/publish_layered_map_experiment.cc) - set_wavemap_target_properties(publish_layered_map_experiment) - target_include_directories(publish_layered_map_experiment PUBLIC ${catkin_INCLUDE_DIRS}) - target_link_libraries(publish_layered_map_experiment PUBLIC - wavemap::wavemap_core ${catkin_LIBRARIES}) - - add_executable(subscribe_layered_map_experiment ros/subscribe_layered_map_experiment.cc) - set_wavemap_target_properties(subscribe_layered_map_experiment) - target_include_directories(subscribe_layered_map_experiment PUBLIC ${catkin_INCLUDE_DIRS}) - target_link_libraries(subscribe_layered_map_experiment PUBLIC - wavemap::wavemap_core ${catkin_LIBRARIES}) - - add_executable(field_layered_map_ros_experiment - ros/field_layered_map_ros_experiment.cc) - set_wavemap_target_properties(field_layered_map_ros_experiment) - target_include_directories(field_layered_map_ros_experiment - PUBLIC ${catkin_INCLUDE_DIRS}) - target_link_libraries(field_layered_map_ros_experiment PUBLIC - wavemap::wavemap_core ${catkin_LIBRARIES}) - - add_executable(field_layer_update_ros_experiment - ros/field_layer_update_ros_experiment.cc) - set_wavemap_target_properties(field_layer_update_ros_experiment) - target_include_directories(field_layer_update_ros_experiment - PUBLIC ${catkin_INCLUDE_DIRS}) - target_link_libraries(field_layer_update_ros_experiment PUBLIC - wavemap::wavemap_core ${catkin_LIBRARIES}) - - add_executable(native_sensor_layer_adapters_experiment - ros/native_sensor_layer_adapters_experiment.cc) - set_wavemap_target_properties(native_sensor_layer_adapters_experiment) - target_include_directories(native_sensor_layer_adapters_experiment - PUBLIC ${catkin_INCLUDE_DIRS}) - target_link_libraries(native_sensor_layer_adapters_experiment PUBLIC - wavemap::wavemap_core ${catkin_LIBRARIES}) -else () - message(STATUS "Skipping ROS digital_twin examples because ROS1 is not active.") -endif () diff --git a/examples/cpp/digital_twin/common/custom_layered_map_config.h b/examples/cpp/digital_twin/common/custom_layered_map_config.h deleted file mode 100644 index 075ac6150..000000000 --- a/examples/cpp/digital_twin/common/custom_layered_map_config.h +++ /dev/null @@ -1,171 +0,0 @@ -#ifndef WAVEMAP_EXAMPLES_CPP_DIGITAL_TWIN_CUSTOM_LAYERED_MAP_CONFIG_H_ -#define WAVEMAP_EXAMPLES_CPP_DIGITAL_TWIN_CUSTOM_LAYERED_MAP_CONFIG_H_ - -#include -#include -#include -#include - -#include -#include - -#include -#include - -using wavemap::layered::DiscreteCompressionConfig; -using wavemap::layered::DiscreteLayer; -using wavemap::layered::LayeredMap; - -struct MyContinuousLayers { - float temperature = 0.f; - float intensity = 0.f; - - bool operator==(const MyContinuousLayers& other) const { - return temperature == other.temperature && intensity == other.intensity; - } -}; - -struct MyContinuousPolicy { - static MyContinuousLayers add(const MyContinuousLayers& lhs, - const MyContinuousLayers& rhs) { - return {lhs.temperature + rhs.temperature, lhs.intensity + rhs.intensity}; - } - - static MyContinuousLayers subtract(const MyContinuousLayers& lhs, - const MyContinuousLayers& rhs) { - return {lhs.temperature - rhs.temperature, lhs.intensity - rhs.intensity}; - } - - static MyContinuousLayers scale(const MyContinuousLayers& data, - wavemap::FloatingPoint factor) { - return {factor * data.temperature, factor * data.intensity}; - } -}; - -namespace wavemap { -template <> -struct CellDataTraits<::MyContinuousLayers> { - struct ThresholdConfig { - FloatingPoint min_temperature = -50.f; - FloatingPoint max_temperature = 80.f; - FloatingPoint min_intensity = 0.f; - FloatingPoint max_intensity = 1.f; - }; - - struct PruningConfig { - FieldPruningConfig temperature{}; - FieldPruningConfig intensity{}; - FloatingPoint combined_threshold = 1.f; - }; - - static void threshold(MyContinuousLayers& layers, - const ThresholdConfig& config) { - layers.temperature = std::clamp(layers.temperature, - config.min_temperature, - config.max_temperature); - layers.intensity = std::clamp(layers.intensity, config.min_intensity, - config.max_intensity); - } - - static FloatingPoint pruningScore(const MyContinuousLayers& layers, - const PruningConfig& config) { - return weightedPruningScore(std::abs(layers.temperature), - config.temperature) + - weightedPruningScore(std::abs(layers.intensity), config.intensity); - } - - static bool isNonzero(const MyContinuousLayers& layers, - const PruningConfig& config) { - return config.combined_threshold < pruningScore(layers, config); - } -}; -} // namespace wavemap - -struct MyDiscreteLayers { - explicit MyDiscreteLayers( - DiscreteCompressionConfig config = DiscreteCompressionConfig()) - : semantic_label(config), manually_changed(config) {} - - DiscreteLayer semantic_label; - DiscreteLayer manually_changed; -}; - -using MyLayeredMap = - LayeredMap; -using MyLayeredMapConfig = MyLayeredMap::Config; -using MyVoxel = MyLayeredMap::ContinuousVoxel; - -struct MyVoxelSerializer { - static void write(std::ostream& ostream, const MyVoxel& voxel) { - wavemap::io::StreamableCellData::write( - ostream, voxel.occupancy); - wavemap::io::StreamableCellData::write( - ostream, voxel.data.temperature); - wavemap::io::StreamableCellData::write( - ostream, voxel.data.intensity); - } - - static MyVoxel read(std::istream& istream) { - MyVoxel voxel; - voxel.occupancy = - wavemap::io::StreamableCellData::read(istream); - voxel.data.temperature = - wavemap::io::StreamableCellData::read(istream); - voxel.data.intensity = - wavemap::io::StreamableCellData::read(istream); - return voxel; - } -}; - -namespace wavemap::layered { -template <> -struct ContinuousLayerSchemaTraits<::MyContinuousLayers> { - static std::vector layers() { - return {{"temperature", "float32"}, {"intensity", "float32"}}; - } -}; -} // namespace wavemap::layered - -namespace wavemap::layered::io { -template <> -struct DiscreteLayerBundleTraits<::MyDiscreteLayers> { - static auto layers(MyDiscreteLayers& layers) { - return std::make_tuple( - namedDiscreteLayer("semantic_label", layers.semantic_label), - namedDiscreteLayer("manually_changed", layers.manually_changed)); - } - - static auto layers(const MyDiscreteLayers& layers) { - return std::make_tuple( - namedDiscreteLayer("semantic_label", layers.semantic_label), - namedDiscreteLayer("manually_changed", layers.manually_changed)); - } -}; -} // namespace wavemap::layered::io - -using MyLayeredMapIo = wavemap::layered::io::LayeredMapIo; - -inline MyLayeredMapConfig makeMyLayeredMapConfig() { - MyLayeredMapConfig config; - config.continuous_map.min_cell_width = 0.1f; - config.continuous_map.min_log_odds = -100.f; - config.continuous_map.max_log_odds = 100.f; - config.continuous_map.tree_height = 3; - config.continuous_threshold.data.min_temperature = -20.f; - config.continuous_threshold.data.max_temperature = 60.f; - config.continuous_threshold.data.min_intensity = 0.f; - config.continuous_threshold.data.max_intensity = 1.f; - config.continuous_pruning.occupancy.weight = 1.f; - config.continuous_pruning.occupancy.scale = 1e-3f; - config.continuous_pruning.data.temperature.weight = 1.f; - config.continuous_pruning.data.temperature.scale = 1e-3f; - config.continuous_pruning.data.intensity.weight = 1.f; - config.continuous_pruning.data.intensity.scale = 1e-3f; - config.continuous_pruning.combined_threshold = 1.f; - config.continuous_pruning.data.combined_threshold = 1.f; - config.discrete_compression.block_height = 1; - return config; -} - -#endif // WAVEMAP_EXAMPLES_CPP_DIGITAL_TWIN_CUSTOM_LAYERED_MAP_CONFIG_H_ diff --git a/examples/cpp/digital_twin/common/example_layered_map_config.h b/examples/cpp/digital_twin/common/example_layered_map_config.h deleted file mode 100644 index d2e8753b3..000000000 --- a/examples/cpp/digital_twin/common/example_layered_map_config.h +++ /dev/null @@ -1,51 +0,0 @@ -#ifndef WAVEMAP_EXAMPLES_CPP_DIGITAL_TWIN_EXAMPLE_LAYERED_MAP_CONFIG_H_ -#define WAVEMAP_EXAMPLES_CPP_DIGITAL_TWIN_EXAMPLE_LAYERED_MAP_CONFIG_H_ - -#include -#include -#include "layered_voxel_config.h" - -using wavemap::layered::DiscreteCompressionConfig; -using wavemap::layered::DiscreteLayer; -using wavemap::layered::LayeredMap; - -inline constexpr char kSemanticLayerName[] = "semantic"; -inline constexpr char kChangedLayerName[] = "changed"; - -struct ExampleDiscreteLayers { - explicit ExampleDiscreteLayers( - DiscreteCompressionConfig config = DiscreteCompressionConfig()) - : semantic(config), changed(config) {} - - DiscreteLayer semantic; - DiscreteLayer changed; -}; - -using ExampleLayeredMap = - LayeredMap; -using ExampleLayeredMapConfig = ExampleLayeredMap::Config; -using ExampleLayeredMapIo = - wavemap::layered::io::LayeredMapIo; - -inline wavemap::layered::LayeredMapSchema exampleLayeredMapSchema( - const ExampleLayeredMap& map) { - return wavemap::layered::io::layeredMapSchema(map); -} - -namespace wavemap::layered::io { -template <> -struct DiscreteLayerBundleTraits<::ExampleDiscreteLayers> { - static auto layers(ExampleDiscreteLayers& layers) { - return std::make_tuple(namedDiscreteLayer(kSemanticLayerName, layers.semantic), - namedDiscreteLayer(kChangedLayerName, layers.changed)); - } - - static auto layers(const ExampleDiscreteLayers& layers) { - return std::make_tuple(namedDiscreteLayer(kSemanticLayerName, layers.semantic), - namedDiscreteLayer(kChangedLayerName, layers.changed)); - } -}; -} // namespace wavemap::layered::io - -#endif // WAVEMAP_EXAMPLES_CPP_DIGITAL_TWIN_EXAMPLE_LAYERED_MAP_CONFIG_H_ diff --git a/examples/cpp/digital_twin/common/example_layered_map_ros_config.h b/examples/cpp/digital_twin/common/example_layered_map_ros_config.h deleted file mode 100644 index 943ff0cb7..000000000 --- a/examples/cpp/digital_twin/common/example_layered_map_ros_config.h +++ /dev/null @@ -1,172 +0,0 @@ -#ifndef WAVEMAP_EXAMPLES_CPP_DIGITAL_TWIN_EXAMPLE_LAYERED_MAP_ROS_CONFIG_H_ -#define WAVEMAP_EXAMPLES_CPP_DIGITAL_TWIN_EXAMPLE_LAYERED_MAP_ROS_CONFIG_H_ - -#include -#include -#include - -#include -#include -#include - -#include -#include "example_layered_map_config.h" - -namespace layered_map_config { -namespace detail { -inline bool readRgbParam(const ros::NodeHandle& nh, const std::string& key, - Rgb& value) { - std::vector values; - if (!nh.getParam(key, values)) { - return false; - } - if (values.size() != 3u) { - ROS_WARN_STREAM("Ignoring layered map RGB param " << key - << " because it must contain exactly 3 values."); - return false; - } - value = rgb(static_cast(values[0]), static_cast(values[1]), - static_cast(values[2])); - return true; -} - -template -void readParam(const ros::NodeHandle& nh, const std::string& key, - ValueT& value) { - nh.param(key, value, value); -} -} // namespace detail - -inline ExampleLayeredMapConfig makeDefaultExampleLayeredMapConfig() { - ExampleLayeredMapConfig config; - config.continuous_map.min_cell_width = 0.1f; - config.continuous_map.min_log_odds = -2.f; - config.continuous_map.max_log_odds = 4.f; - config.continuous_map.tree_height = 3; - config.continuous_map.only_prune_blocks_if_unused_for = 5.f; - - config.continuous_threshold.data.rgb_min = rgb(0.f, 0.f, 0.f); - config.continuous_threshold.data.rgb_max = rgb(1.f, 1.f, 1.f); - config.continuous_threshold.data.traversability_min = 0.f; - config.continuous_threshold.data.traversability_max = 1.f; - - config.continuous_pruning.occupancy.scale = 1e-3f; - config.continuous_pruning.occupancy.weight = 1.f; - config.continuous_pruning.data.rgb.scale = 1e-3f; - config.continuous_pruning.data.rgb.weight = 1.f; - config.continuous_pruning.data.traversability.scale = 1e-3f; - config.continuous_pruning.data.traversability.weight = 1.f; - config.continuous_pruning.combined_threshold = 1.f; - config.continuous_pruning.data.combined_threshold = 1.f; - - config.discrete_compression.block_height = 1; - return config; -} - -inline ExampleLayeredMapConfig loadExampleLayeredMapConfigFromRosParams( - const ros::NodeHandle& nh_private, - const std::string& ns = "layered_map") { - ExampleLayeredMapConfig config = makeDefaultExampleLayeredMapConfig(); - - detail::readParam(nh_private, ns + "/continuous_map/min_cell_width/meters", - config.continuous_map.min_cell_width); - detail::readParam(nh_private, ns + "/continuous_map/min_log_odds", - config.continuous_map.min_log_odds); - detail::readParam(nh_private, ns + "/continuous_map/max_log_odds", - config.continuous_map.max_log_odds); - detail::readParam(nh_private, ns + "/continuous_map/tree_height", - config.continuous_map.tree_height); - detail::readParam( - nh_private, ns + "/continuous_map/only_prune_blocks_if_unused_for/seconds", - config.continuous_map.only_prune_blocks_if_unused_for); - - detail::readRgbParam(nh_private, ns + "/continuous_threshold/color/min", - config.continuous_threshold.data.rgb_min); - detail::readRgbParam(nh_private, ns + "/continuous_threshold/color/max", - config.continuous_threshold.data.rgb_max); - detail::readParam(nh_private, ns + "/continuous_threshold/traversability/min", - config.continuous_threshold.data.traversability_min); - detail::readParam(nh_private, ns + "/continuous_threshold/traversability/max", - config.continuous_threshold.data.traversability_max); - - detail::readParam(nh_private, ns + "/continuous_pruning/combined_threshold", - config.continuous_pruning.combined_threshold); - config.continuous_pruning.data.combined_threshold = - config.continuous_pruning.combined_threshold; - detail::readParam(nh_private, ns + "/continuous_pruning/occupancy/scale", - config.continuous_pruning.occupancy.scale); - detail::readParam(nh_private, ns + "/continuous_pruning/occupancy/weight", - config.continuous_pruning.occupancy.weight); - detail::readParam(nh_private, ns + "/continuous_pruning/color/scale", - config.continuous_pruning.data.rgb.scale); - detail::readParam(nh_private, ns + "/continuous_pruning/color/weight", - config.continuous_pruning.data.rgb.weight); - detail::readParam(nh_private, ns + "/continuous_pruning/traversability/scale", - config.continuous_pruning.data.traversability.scale); - detail::readParam(nh_private, ns + "/continuous_pruning/traversability/weight", - config.continuous_pruning.data.traversability.weight); - - detail::readParam(nh_private, ns + "/discrete_compression/block_height", - config.discrete_compression.block_height); - return config; -} -} // namespace layered_map_config - -namespace wavemap::convert { -template <> -struct DiscreteLayerBundleRosTraits<::ExampleDiscreteLayers> { - static auto layers(ExampleDiscreteLayers& layers) { - return std::make_tuple( - namedDiscreteLayerForRos(kSemanticLayerName, layers.semantic), - namedDiscreteLayerForRos(kChangedLayerName, layers.changed)); - } - - static auto layers(const ExampleDiscreteLayers& layers) { - return std::make_tuple( - namedDiscreteLayerForRos(kSemanticLayerName, layers.semantic), - namedDiscreteLayerForRos(kChangedLayerName, layers.changed)); - } -}; -} // namespace wavemap::convert - - -namespace layered_map_viz { -struct ExampleSemanticColorPolicy { - bool operator()(int label, std_msgs::ColorRGBA& color_msg) const { - switch (label) { - case 1: - color_msg = color(0.1f, 0.6f, 1.f, 0.85f); - return true; - case 2: - color_msg = color(1.f, 0.5f, 0.1f, 0.85f); - return true; - default: - color_msg = color(0.7f, 0.7f, 0.7f, 0.6f); - return true; - } - } -}; - -struct ExampleChangedColorPolicy { - bool operator()(bool changed, std_msgs::ColorRGBA& color_msg) const { - if (!changed) { - return false; - } - color_msg = color(1.f, 0.05f, 0.05f, 1.f); - return true; - } -}; - -template <> -struct DiscreteLayerMarkerTraits<::ExampleDiscreteLayers> { - static auto layers(const ExampleDiscreteLayers& layers) { - return std::make_tuple( - namedDiscreteMarkerLayer(kSemanticLayerName, layers.semantic, - ExampleSemanticColorPolicy{}), - namedDiscreteMarkerLayer(kChangedLayerName, layers.changed, - ExampleChangedColorPolicy{})); - } -}; -} // namespace layered_map_viz - -#endif // WAVEMAP_EXAMPLES_CPP_DIGITAL_TWIN_EXAMPLE_LAYERED_MAP_ROS_CONFIG_H_ diff --git a/examples/cpp/digital_twin/common/field_layer_policies.h b/examples/cpp/digital_twin/common/field_layer_policies.h deleted file mode 100644 index 3bc50be5d..000000000 --- a/examples/cpp/digital_twin/common/field_layer_policies.h +++ /dev/null @@ -1,37 +0,0 @@ -#ifndef WAVEMAP_EXAMPLES_CPP_DIGITAL_TWIN_FIELD_LAYER_POLICIES_H_ -#define WAVEMAP_EXAMPLES_CPP_DIGITAL_TWIN_FIELD_LAYER_POLICIES_H_ - -#include - -#include -#include - -#include "field_layer_types.h" - -namespace wavemap::examples::field_map { - -using ReflectivityUpdatePolicy = layered::ReplaceLayerUpdatePolicy; -using ClassUpdatePolicy = layered::ReplaceLayerUpdatePolicy; - -struct ReflectivityLayer - : layered::schema::ContinuousLayer { - static constexpr std::string_view name = "reflectivity"; -}; - -struct ClassLayer - : layered::schema::DiscreteLayer { - static constexpr std::string_view name = "class"; -}; - -using Schema = - layered::schema::LayerSchema; - -static_assert(Schema::size == 2u); -static_assert(Schema::continuousLayerCount == 1u); -static_assert(Schema::discreteLayerCount == 1u); -static_assert(Schema::containsContinuous); -static_assert(Schema::containsDiscrete); - -} // namespace wavemap::examples::field_map - -#endif // WAVEMAP_EXAMPLES_CPP_DIGITAL_TWIN_FIELD_LAYER_POLICIES_H_ diff --git a/examples/cpp/digital_twin/common/field_layer_types.h b/examples/cpp/digital_twin/common/field_layer_types.h deleted file mode 100644 index 0bfce5c70..000000000 --- a/examples/cpp/digital_twin/common/field_layer_types.h +++ /dev/null @@ -1,14 +0,0 @@ -#ifndef WAVEMAP_EXAMPLES_CPP_DIGITAL_TWIN_FIELD_LAYER_TYPES_H_ -#define WAVEMAP_EXAMPLES_CPP_DIGITAL_TWIN_FIELD_LAYER_TYPES_H_ - -namespace wavemap::examples::field_map { - -enum class ClassLabel : int { - kUnknown = 0, - kGround = 1, - kObstacle = 2, -}; - -} // namespace wavemap::examples::field_map - -#endif // WAVEMAP_EXAMPLES_CPP_DIGITAL_TWIN_FIELD_LAYER_TYPES_H_ diff --git a/examples/cpp/digital_twin/common/field_layered_map_config.h b/examples/cpp/digital_twin/common/field_layered_map_config.h deleted file mode 100644 index ac7d49b0c..000000000 --- a/examples/cpp/digital_twin/common/field_layered_map_config.h +++ /dev/null @@ -1,61 +0,0 @@ -#ifndef WAVEMAP_EXAMPLES_CPP_DIGITAL_TWIN_FIELD_LAYERED_MAP_CONFIG_H_ -#define WAVEMAP_EXAMPLES_CPP_DIGITAL_TWIN_FIELD_LAYERED_MAP_CONFIG_H_ - -#include -#include -#include -#include - -#include "field_layer_policies.h" - -namespace wavemap::examples::field_map { - -using ContinuousLayers = layered::schema::ContinuousLayerBundle; -using ContinuousArithmetic = - layered::schema::ContinuousBundleArithmetic; -using DiscreteLayers = layered::schema::DiscreteLayerBundle; - -using Map = - layered::LayeredMap; -using Config = Map::Config; -using Voxel = Map::ContinuousVoxel; -using ContinuousMap = Map::ContinuousMap; - -using VoxelSerializer = layered::DescriptorVoxelSerializer; -using MapIo = layered::io::LayeredMapIo; - -inline ContinuousLayers makeContinuousLayers(float reflectivity) { - ContinuousLayers layers; - layers.get() = reflectivity; - return layers; -} - -inline Config makeDefaultConfig() { - Config config; - config.continuous_map.min_cell_width = 0.25f; - config.continuous_map.min_log_odds = -2.f; - config.continuous_map.max_log_odds = 4.f; - config.continuous_map.tree_height = 6; - config.continuous_map.only_prune_blocks_if_unused_for = 5.f; - - auto& reflectivity_threshold = - config.continuous_threshold.data.get(); - reflectivity_threshold.min = 0.f; - reflectivity_threshold.max = 0.4f; - - config.continuous_pruning.occupancy.scale = 1e-3f; - config.continuous_pruning.occupancy.weight = 1.f; - auto& reflectivity_pruning = - config.continuous_pruning.data.get(); - reflectivity_pruning.scale = 1e-3f; - reflectivity_pruning.weight = 1.f; - config.continuous_pruning.combined_threshold = 1.f; - config.continuous_pruning.data.combined_threshold = 1.f; - - config.discrete_compression.block_height = 1; - return config; -} - -} // namespace wavemap::examples::field_map - -#endif // WAVEMAP_EXAMPLES_CPP_DIGITAL_TWIN_FIELD_LAYERED_MAP_CONFIG_H_ diff --git a/examples/cpp/digital_twin/common/field_layered_map_ros_config.h b/examples/cpp/digital_twin/common/field_layered_map_ros_config.h deleted file mode 100644 index 99a8d1107..000000000 --- a/examples/cpp/digital_twin/common/field_layered_map_ros_config.h +++ /dev/null @@ -1,117 +0,0 @@ -#ifndef WAVEMAP_EXAMPLES_CPP_DIGITAL_TWIN_FIELD_LAYERED_MAP_ROS_CONFIG_H_ -#define WAVEMAP_EXAMPLES_CPP_DIGITAL_TWIN_FIELD_LAYERED_MAP_ROS_CONFIG_H_ - -#include -#include - -#include -#include -#include -#include - - -#include "field_layered_map_config.h" - -namespace wavemap::examples::field_map::ros_config { -namespace detail { - -template -void readParam(const ros::NodeHandle& nh, const std::string& key, - ValueT& value) { - nh.param(key, value, value); -} - -} // namespace detail - -inline Config loadConfig(const ros::NodeHandle& nh_private, - const std::string& ns = "layered_map") { - Config config = makeDefaultConfig(); - - detail::readParam(nh_private, ns + "/continuous_map/min_cell_width/meters", - config.continuous_map.min_cell_width); - detail::readParam(nh_private, ns + "/continuous_map/min_log_odds", - config.continuous_map.min_log_odds); - detail::readParam(nh_private, ns + "/continuous_map/max_log_odds", - config.continuous_map.max_log_odds); - detail::readParam(nh_private, ns + "/continuous_map/tree_height", - config.continuous_map.tree_height); - detail::readParam( - nh_private, ns + "/continuous_map/only_prune_blocks_if_unused_for/seconds", - config.continuous_map.only_prune_blocks_if_unused_for); - - detail::readParam(nh_private, ns + "/continuous_pruning/combined_threshold", - config.continuous_pruning.combined_threshold); - config.continuous_pruning.data.combined_threshold = - config.continuous_pruning.combined_threshold; - - detail::readParam(nh_private, ns + "/continuous_pruning/occupancy/scale", - config.continuous_pruning.occupancy.scale); - detail::readParam(nh_private, ns + "/continuous_pruning/occupancy/weight", - config.continuous_pruning.occupancy.weight); - auto& reflectivity_pruning = - config.continuous_pruning.data.get(); - detail::readParam(nh_private, ns + "/continuous_pruning/reflectivity/scale", - reflectivity_pruning.scale); - detail::readParam(nh_private, ns + "/continuous_pruning/reflectivity/weight", - reflectivity_pruning.weight); - - detail::readParam(nh_private, ns + "/discrete_compression/block_height", - config.discrete_compression.block_height); - return config; -} - -using VoxelRosConverter = - convert::DescriptorContinuousRosConverter; - -} // namespace wavemap::examples::field_map::ros_config - -namespace wavemap::convert { - -template <> -struct DiscreteLayerBundleRosTraits< - examples::field_map::DiscreteLayers> { - static auto layers(examples::field_map::DiscreteLayers& layers) { - return descriptorNamedDiscreteLayersForRos(layers); - } - - static auto layers(const examples::field_map::DiscreteLayers& layers) { - return descriptorNamedDiscreteLayersForRos(layers); - } -}; - -} // namespace wavemap::convert - -namespace layered_map_viz { - -struct FieldClassColorPolicy { - bool operator()(int value, std_msgs::ColorRGBA& color_msg) const { - using ClassLabel = wavemap::examples::field_map::ClassLabel; - switch (static_cast(value)) { - case ClassLabel::kGround: - color_msg = color(0.55f, 0.32f, 0.12f, 0.9f); - return true; - case ClassLabel::kObstacle: - color_msg = color(0.85f, 0.15f, 0.1f, 0.9f); - return true; - case ClassLabel::kUnknown: - default: - color_msg = color(0.5f, 0.5f, 0.5f, 0.5f); - return true; - } - } -}; - -template <> -struct DiscreteLayerMarkerTraits< - wavemap::examples::field_map::DiscreteLayers> { - static auto layers( - const wavemap::examples::field_map::DiscreteLayers& layers) { - return std::make_tuple(namedDiscreteMarkerLayer( - std::string(wavemap::layered::LayerTraits::name), layers.get(), - FieldClassColorPolicy{})); - } -}; - -} // namespace layered_map_viz - -#endif // WAVEMAP_EXAMPLES_CPP_DIGITAL_TWIN_FIELD_LAYERED_MAP_ROS_CONFIG_H_ diff --git a/examples/cpp/digital_twin/common/layered_ros_converter.h b/examples/cpp/digital_twin/common/layered_ros_converter.h deleted file mode 100644 index 645c4e239..000000000 --- a/examples/cpp/digital_twin/common/layered_ros_converter.h +++ /dev/null @@ -1,132 +0,0 @@ -#ifndef WAVEMAP_EXAMPLES_CPP_DIGITAL_TWIN_LAYERED_ROS_CONVERTER_H_ -#define WAVEMAP_EXAMPLES_CPP_DIGITAL_TWIN_LAYERED_ROS_CONVERTER_H_ - -#include -#include - -#include -#include - -#include "layered_voxel_config.h" - -struct LayeredVoxelRosConverter { - static std::vector layerNames() { - std::vector names; - for (const auto& layer : - wavemap::layered::ContinuousLayerSchemaTraits::layers()) { - names.emplace_back(layer.name); - } - return names; - } - - static std::vector layerTypes() { - std::vector types; - for (const auto& layer : - wavemap::layered::ContinuousLayerSchemaTraits::layers()) { - types.emplace_back(layer.type); - } - return types; - } - - static std::vector layerVisualizations() { - return {}; - } - - static std::vector makeLayers() { - // The message stores custom values layer-major: all color values go into - // the color layer, and all traversability values go into the traversability - // layer. - std::vector layers; - for (const auto& layer_schema : - wavemap::layered::ContinuousLayerSchemaTraits::layers()) { - auto& layer = layers.emplace_back(); - layer.name = layer_schema.name; - layer.type = layer_schema.type; - } - return layers; - } - - static std::vector makeLayerMinimums( - const ContinuousWaveletMap::ThresholdConfig& config) { - auto layers = makeLayers(); - layers[0].float32_values = {config.data.rgb_min.r, config.data.rgb_min.g, - config.data.rgb_min.b}; - layers[1].float32_values = {config.data.traversability_min}; - return layers; - } - - static std::vector makeLayerMaximums( - const ContinuousWaveletMap::ThresholdConfig& config) { - auto layers = makeLayers(); - layers[0].float32_values = {config.data.rgb_max.r, config.data.rgb_max.g, - config.data.rgb_max.b}; - layers[1].float32_values = {config.data.traversability_max}; - return layers; - } - - static void appendLayerValues(const LayeredVoxel& voxel, - std::vector& layers) { - // Color uses three float values per voxel/coefficient: r, g, and b. - layers[0].float32_values.emplace_back(voxel.data.rgb.r); - layers[0].float32_values.emplace_back(voxel.data.rgb.g); - layers[0].float32_values.emplace_back(voxel.data.rgb.b); - - // Traversability uses one float value per voxel/coefficient. - layers[1].float32_values.emplace_back(voxel.data.traversability); - } - - static LayeredVoxel readCellData( - wavemap::FloatingPoint occupancy, - const std::vector& layers, size_t value_index) { - // The value_index selects which voxel/coefficient is being reconstructed - // inside each layer-major array. - const size_t color_index = 3u * value_index; - const Rgb color = rgb(layers[0].float32_values[color_index], - layers[0].float32_values[color_index + 1u], - layers[0].float32_values[color_index + 2u]); - return LayeredVoxel( - occupancy, - ContinuousLayers{color, layers[1].float32_values[value_index]}); - } -}; - -inline void populateLayeredCube( - ContinuousWaveletMap& map, std::vector& populated_indices) { - const wavemap::Index3D cube_origin(8, 2, 2); - constexpr int cube_size_x = 2; - constexpr int cube_size_y = 2; - constexpr int cube_size_z = 4; - - size_t populated_voxels = 0u; - populated_indices.reserve(cube_size_x * cube_size_y * cube_size_z); - - // Populate a small cube so conversions and publishing touch several wavelet - // coefficients instead of a single trivial voxel. - for (int dx = 0; dx < cube_size_x; ++dx) { - for (int dy = 0; dy < cube_size_y; ++dy) { - for (int dz = 0; dz < cube_size_z; ++dz) { - const wavemap::Index3D voxel_index = - cube_origin + wavemap::Index3D(dx, dy, dz); - const wavemap::FloatingPoint layer_value = - static_cast(populated_voxels); - const Rgb visible_color = rgb( - dx == 0 ? 0.15f : 1.0f, - dy == 0 ? 0.15f : 1.0f, - 0.25f + 0.25f * static_cast(dz)); - const LayeredVoxel original_voxel( - 0.5f + 0.02f * layer_value, - ContinuousLayers{visible_color, 0.9f - 0.03f * layer_value}); - const LayeredVoxel voxel_update( - 0.1f, - ContinuousLayers{rgb(0.f, 0.f, 0.f), -0.01f}); - - map.setVoxelValue(voxel_index, original_voxel); - map.addToVoxelValue(voxel_index, voxel_update); - populated_indices.emplace_back(voxel_index); - ++populated_voxels; - } - } - } -} - -#endif // WAVEMAP_EXAMPLES_CPP_DIGITAL_TWIN_LAYERED_ROS_CONVERTER_H_ diff --git a/examples/cpp/digital_twin/common/layered_voxel_config.h b/examples/cpp/digital_twin/common/layered_voxel_config.h deleted file mode 100644 index 308133f22..000000000 --- a/examples/cpp/digital_twin/common/layered_voxel_config.h +++ /dev/null @@ -1,220 +0,0 @@ -#ifndef WAVEMAP_EXAMPLES_CPP_DIGITAL_TWIN_LAYERED_VOXEL_CONFIG_H_ -#define WAVEMAP_EXAMPLES_CPP_DIGITAL_TWIN_LAYERED_VOXEL_CONFIG_H_ - -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include - -struct Rgb { - float r = 0.f; - float g = 0.f; - float b = 0.f; - - bool operator==(const Rgb& other) const { - return r == other.r && g == other.g && b == other.b; - } -}; - -inline Rgb operator+(const Rgb& lhs, const Rgb& rhs) { - return {lhs.r + rhs.r, lhs.g + rhs.g, lhs.b + rhs.b}; -} - -inline Rgb operator-(const Rgb& lhs, const Rgb& rhs) { - return {lhs.r - rhs.r, lhs.g - rhs.g, lhs.b - rhs.b}; -} - -inline Rgb operator*(float factor, const Rgb& color) { - return {factor * color.r, factor * color.g, factor * color.b}; -} - -inline Rgb operator*(const Rgb& color, float factor) { - return factor * color; -} - -inline Rgb clampRgb(const Rgb& color, const Rgb& min, const Rgb& max) { - return {std::clamp(color.r, min.r, max.r), - std::clamp(color.g, min.g, max.g), - std::clamp(color.b, min.b, max.b)}; -} - -inline float maxAbs(const Rgb& color) { - return std::max({std::abs(color.r), std::abs(color.g), std::abs(color.b)}); -} - -inline float sumAbsDiff(const Rgb& lhs, const Rgb& rhs) { - return std::abs(lhs.r - rhs.r) + std::abs(lhs.g - rhs.g) + - std::abs(lhs.b - rhs.b); -} - -struct ContinuousLayers { - Rgb rgb{}; - float traversability = 0.f; - - bool operator==(const ContinuousLayers& other) const { - return rgb == other.rgb && traversability == other.traversability; - } -}; - -struct ContinuousLayersPolicy { - static ContinuousLayers add(const ContinuousLayers& lhs, - const ContinuousLayers& rhs) { - return ContinuousLayers{lhs.rgb + rhs.rgb, - lhs.traversability + rhs.traversability}; - } - - static ContinuousLayers subtract(const ContinuousLayers& lhs, - const ContinuousLayers& rhs) { - return ContinuousLayers{lhs.rgb - rhs.rgb, - lhs.traversability - rhs.traversability}; - } - - static ContinuousLayers scale(const ContinuousLayers& data, - wavemap::FloatingPoint factor) { - return ContinuousLayers{factor * data.rgb, factor * data.traversability}; - } -}; - -namespace wavemap::layered { -template <> -struct ContinuousLayerSchemaTraits<::ContinuousLayers> { - static std::vector layers() { - return {{"color", "float32_rgb"}, {"traversability", "float32"}}; - } -}; -} // namespace wavemap::layered - -namespace wavemap { -template <> -struct CellDataTraits<::ContinuousLayers> { - struct ThresholdConfig { - Rgb rgb_min{}; - Rgb rgb_max{1.f, 1.f, 1.f}; - FloatingPoint traversability_min = 0.f; - FloatingPoint traversability_max = 1.f; - }; - - struct PruningConfig { - FieldPruningConfig rgb{}; - FieldPruningConfig traversability{}; - FloatingPoint combined_threshold = 1.f; - }; - - static void threshold(ContinuousLayers& layers, - const ThresholdConfig& config) { - layers.rgb = clampRgb(layers.rgb, config.rgb_min, config.rgb_max); - layers.traversability = std::clamp(layers.traversability, - config.traversability_min, - config.traversability_max); - } - - static FloatingPoint pruningScore(const ContinuousLayers& layers, - const PruningConfig& config) { - return weightedPruningScore(maxAbs(layers.rgb), config.rgb) + - weightedPruningScore(std::abs(layers.traversability), - config.traversability); - } - - static bool isNonzero(const ContinuousLayers& layers, - const PruningConfig& config) { - return config.combined_threshold < pruningScore(layers, config); - } -}; -} // namespace wavemap - -using LayeredVoxel = wavemap::VoxelData; -using LayeredBlock = wavemap::HashedWaveletOctreeBlockT; -using ContinuousWaveletMap = wavemap::HashedWaveletOctreeT; -using LayeredPruningConfig = ContinuousWaveletMap::PruningConfig; - -// Pruning uses a normalized weighted score over all wavelet detail -// coefficients: score = sum(weight * abs(detail) / scale). A node is kept -// when score > combined_threshold. Set a field weight to 0 to ignore it. -inline LayeredPruningConfig makeLayeredPruningConfig( - float occupancy_weight, float rgb_weight, float traversability_weight, - float scale = 1e-3f, float combined_threshold = 1.f) { - LayeredPruningConfig config; - config.occupancy.scale = scale; - config.occupancy.weight = occupancy_weight; - config.data.rgb.scale = scale; - config.data.rgb.weight = rgb_weight; - config.data.traversability.scale = scale; - config.data.traversability.weight = traversability_weight; - config.combined_threshold = combined_threshold; - config.data.combined_threshold = combined_threshold; - return config; -} - -inline LayeredPruningConfig makeLayeredPruningConfigWithScales( - float occupancy_weight, float rgb_weight, float traversability_weight, - float occupancy_scale, float rgb_scale, float traversability_scale, - float combined_threshold = 1.f) { - LayeredPruningConfig config; - config.occupancy.scale = occupancy_scale; - config.occupancy.weight = occupancy_weight; - config.data.rgb.scale = rgb_scale; - config.data.rgb.weight = rgb_weight; - config.data.traversability.scale = traversability_scale; - config.data.traversability.weight = traversability_weight; - config.combined_threshold = combined_threshold; - config.data.combined_threshold = combined_threshold; - return config; -} - -inline LayeredPruningConfig makeOccupancyOnlyPruningConfig( - float scale = 1e-3f, float combined_threshold = 1.f) { - return makeLayeredPruningConfig(1.f, 0.f, 0.f, scale, combined_threshold); -} - -inline LayeredPruningConfig makeEqualLayerPruningConfig( - float scale = 1e-3f, float combined_threshold = 1.f) { - return makeLayeredPruningConfig(1.f, 1.f, 1.f, scale, combined_threshold); -} - -struct LayeredVoxelSerializer { - static void write(std::ostream& ostream, const LayeredVoxel& voxel) { - wavemap::io::StreamableCellData::write(ostream, voxel.occupancy); - wavemap::io::StreamableCellData::write(ostream, voxel.data.rgb.r); - wavemap::io::StreamableCellData::write(ostream, voxel.data.rgb.g); - wavemap::io::StreamableCellData::write(ostream, voxel.data.rgb.b); - wavemap::io::StreamableCellData::write(ostream, voxel.data.traversability); - } - - static LayeredVoxel read(std::istream& istream) { - LayeredVoxel voxel; - voxel.occupancy = wavemap::io::StreamableCellData::read(istream); - voxel.data.rgb.r = wavemap::io::StreamableCellData::read(istream); - voxel.data.rgb.g = wavemap::io::StreamableCellData::read(istream); - voxel.data.rgb.b = wavemap::io::StreamableCellData::read(istream); - voxel.data.traversability = wavemap::io::StreamableCellData::read(istream); - return voxel; - } -}; - -inline Rgb rgb(float r, float g, float b) { - return Rgb{r, g, b}; -} - -inline LayeredVoxel makeLayeredVoxel(wavemap::FloatingPoint occupancy, - const Rgb& rgb, - float traversability) { - return LayeredVoxel(occupancy, ContinuousLayers{rgb, traversability}); -} - -inline void printVoxel(const std::string& label, const LayeredVoxel& voxel) { - std::cout << label << ": occ=" << voxel.occupancy - << " trav=" << voxel.data.traversability << " rgb=(" - << voxel.data.rgb.r << ", " << voxel.data.rgb.g << ", " - << voxel.data.rgb.b << ")\n"; -} - -#endif // WAVEMAP_EXAMPLES_CPP_DIGITAL_TWIN_LAYERED_VOXEL_CONFIG_H_ diff --git a/examples/cpp/digital_twin/common/layered_voxel_updater.h b/examples/cpp/digital_twin/common/layered_voxel_updater.h deleted file mode 100644 index 1e01c118b..000000000 --- a/examples/cpp/digital_twin/common/layered_voxel_updater.h +++ /dev/null @@ -1,29 +0,0 @@ -#ifndef WAVEMAP_EXAMPLES_CPP_DIGITAL_TWIN_LAYERED_VOXEL_UPDATER_H_ -#define WAVEMAP_EXAMPLES_CPP_DIGITAL_TWIN_LAYERED_VOXEL_UPDATER_H_ - -#include - -#include -#include - -#include "layered_voxel_config.h" - -struct ContinuousLayerObservation { - wavemap::Index3D index; - ContinuousLayers data{}; -}; - -inline void setContinuousLayersAtVoxel( - ContinuousWaveletMap& map, const wavemap::Index3D& index, - const ContinuousLayers& observed_data) { - wavemap::layered::setContinuousLayersAtVoxel(map, index, observed_data); -} - -inline void setContinuousLayers( - ContinuousWaveletMap& map, const std::vector& observations) { - for (const ContinuousLayerObservation& observation : observations) { - setContinuousLayersAtVoxel(map, observation.index, observation.data); - } -} - -#endif // WAVEMAP_EXAMPLES_CPP_DIGITAL_TWIN_LAYERED_VOXEL_UPDATER_H_ diff --git a/examples/cpp/digital_twin/io/create_large_layered_map_experiment.cc b/examples/cpp/digital_twin/io/create_large_layered_map_experiment.cc deleted file mode 100644 index 58868a204..000000000 --- a/examples/cpp/digital_twin/io/create_large_layered_map_experiment.cc +++ /dev/null @@ -1,120 +0,0 @@ -#include -#include -#include - -#include "../common/example_layered_map_config.h" - -namespace { -float normalized(float value, float max_value) { - return max_value <= 0.f ? 0.f : value / max_value; -} - -LayeredVoxel makeVoxel(int x, int y, int z, int size_x, int size_y, int size_z) { - const float nx = normalized(static_cast(x), static_cast(size_x - 1)); - const float ny = normalized(static_cast(y), static_cast(size_y - 1)); - const float nz = normalized(static_cast(z), static_cast(size_z - 1)); - - const float distance_from_center = - std::sqrt((nx - 0.5f) * (nx - 0.5f) + (ny - 0.5f) * (ny - 0.5f) + - (nz - 0.5f) * (nz - 0.5f)); - const float occupancy = 3.f - 12.f * distance_from_center; - const Rgb color = rgb(nx, ny, 1.f - nz); - const float traversability = std::clamp(1.f - 1.6f * distance_from_center, 0.f, 1.f); - - return makeLayeredVoxel(occupancy, color, traversability); -} - -int semanticLabelFor(int x, int y, int z, int size_x, int size_y) { - if (z > 20 && x > size_x / 4 && x < 3 * size_x / 4 && - y > size_y / 4 && y < 3 * size_y / 4) { - return 2; - } - if ((x / 16 + y / 16) % 2 == 0) { - return 1; - } - return 3; -} -} // namespace - -int main(int argc, char** argv) { - const std::filesystem::path output_path = - argc > 1 ? std::filesystem::path(argv[1]) - : std::filesystem::path("/home/ci/data/maps/layered_map_large.lwvmp"); - - ExampleLayeredMapConfig config; - config.continuous_map.min_cell_width = 0.25f; - config.continuous_map.min_log_odds = -2.f; - config.continuous_map.max_log_odds = 4.f; - config.continuous_map.tree_height = 6; - config.continuous_map.only_prune_blocks_if_unused_for = 5.f; - config.continuous_pruning = makeLayeredPruningConfigWithScales( - 4.f, 1.f, 1.f, 1e-3f, 1e-3f, 1e-3f, 10.f); - config.continuous_threshold.data.rgb_min = rgb(0.f, 0.f, 0.f); - config.continuous_threshold.data.rgb_max = rgb(1.f, 1.f, 1.f); - config.continuous_threshold.data.traversability_min = 0.f; - config.continuous_threshold.data.traversability_max = 1.f; - config.discrete_compression.block_height = 2; - - ExampleLayeredMap map(config); - - constexpr int kSizeX = 96; - constexpr int kSizeY = 80; - constexpr int kSizeZ = 40; - size_t populated_continuous_voxels = 0u; - size_t populated_discrete_voxels = 0u; - - for (int x = 0; x < kSizeX; ++x) { - for (int y = 0; y < kSizeY; ++y) { - for (int z = 0; z < kSizeZ; ++z) { - const wavemap::Index3D index{x, y, z}; - const LayeredVoxel voxel = makeVoxel(x, y, z, kSizeX, kSizeY, kSizeZ); - map.continuousMap().setVoxelValue(index, voxel); - ++populated_continuous_voxels; - - // Keep discrete attributes spatially aligned with the occupied part of the - // continuous map. The continuous map still stores free-space values around - // it, but labels only exist where a semantic value would make sense. - if (1e-3f <= voxel.occupancy) { - map.discreteLayers().semantic.setValue( - index, semanticLabelFor(x, y, z, kSizeX, kSizeY)); - map.discreteLayers().changed.setValue( - index, x == y || (z % 11 == 0 && x % 9 == 0)); - ++populated_discrete_voxels; - } - } - } - } - - const size_t continuous_nodes_before = map.continuousMap().size(); - const size_t continuous_blocks_before = map.continuousMap().getHashMap().size(); - map.continuousMap().threshold(); - map.continuousMap().prune(); - const size_t continuous_nodes_after = map.continuousMap().size(); - const size_t continuous_blocks_after = map.continuousMap().getHashMap().size(); - - std::filesystem::create_directories(output_path.parent_path()); - if (!ExampleLayeredMapIo::save(output_path, map)) { - std::cerr << "Failed to save LayeredMap to " << output_path << "\n"; - return 1; - } - - std::cout << "Created large layered map\n"; - std::cout << "output: " << output_path << "\n"; - std::cout << "populated continuous voxels: " << populated_continuous_voxels << "\n"; - std::cout << "populated discrete voxels: " << populated_discrete_voxels << "\n"; - std::cout << "continuous blocks before prune: " << continuous_blocks_before << "\n"; - std::cout << "continuous blocks after prune: " << continuous_blocks_after << "\n"; - std::cout << "continuous nodes before prune: " << continuous_nodes_before << "\n"; - std::cout << "continuous nodes after prune: " << continuous_nodes_after << "\n"; - std::cout << "semantic parents: " << map.discreteLayers().semantic.parentCount() - << " exceptions: " << map.discreteLayers().semantic.exceptionCount() - << " observed: " << map.discreteLayers().semantic.observedValueCount() - << "\n"; - std::cout << "changed parents: " << map.discreteLayers().changed.parentCount() - << " exceptions: " << map.discreteLayers().changed.exceptionCount() - << " observed: " << map.discreteLayers().changed.observedValueCount() - << "\n"; - std::cout << "file size bytes: " << std::filesystem::file_size(output_path) << "\n"; - - return 0; -} diff --git a/examples/cpp/digital_twin/io/custom_layered_map_save_load_example.cc b/examples/cpp/digital_twin/io/custom_layered_map_save_load_example.cc deleted file mode 100644 index e0b0b74d1..000000000 --- a/examples/cpp/digital_twin/io/custom_layered_map_save_load_example.cc +++ /dev/null @@ -1,87 +0,0 @@ -#include -#include -#include -#include - -#include "../common/custom_layered_map_config.h" - -namespace { -template -bool sameValue(const std::optional& value, const ValueT& expected) { - return value && *value == expected; -} - -bool sameVoxel(const MyVoxel& lhs, const MyVoxel& rhs) { - return lhs.occupancy == rhs.occupancy && lhs.data == rhs.data; -} -} // namespace - -int main() { - MyLayeredMapConfig config = makeMyLayeredMapConfig(); - MyLayeredMap map(config); - const std::vector indices{ - wavemap::Index3D(1, 2, 3), wavemap::Index3D(2, 2, 3)}; - - map.continuousMap().setVoxelValue( - indices[0], MyVoxel(12.f, MyContinuousLayers{22.5f, 0.75f})); - map.continuousMap().setVoxelValue( - indices[1], MyVoxel(14.f, MyContinuousLayers{24.0f, 0.50f})); - map.discreteLayers().semantic_label.setValue(indices[0], 4); - map.discreteLayers().semantic_label.setValue(indices[1], 7); - map.discreteLayers().manually_changed.setValue(indices[0], true); - map.discreteLayers().manually_changed.setValue(indices[1], false); - - const std::filesystem::path output_dir = "/home/ci/data/maps"; - std::filesystem::create_directories(output_dir); - const std::filesystem::path output_path = - output_dir / "custom_layered_map_full.lwvmp"; - - if (!MyLayeredMapIo::save(output_path, map)) { - std::cerr << "Failed to save custom LayeredMap.\n"; - return 1; - } - - MyLayeredMap loaded_map(config); - if (!MyLayeredMapIo::load(output_path, loaded_map)) { - std::cerr << "Failed to load custom LayeredMap.\n"; - return 1; - } - - size_t matching_continuous = 0u; - for (const wavemap::Index3D& index : indices) { - if (sameVoxel(map.continuousMap().getVoxelValue(index), - loaded_map.continuousMap().getVoxelValue(index))) { - ++matching_continuous; - } - } - - const bool semantic_ok = - sameValue(loaded_map.discreteLayers().semantic_label.getValue(indices[0]), - 4) && - sameValue(loaded_map.discreteLayers().semantic_label.getValue(indices[1]), - 7); - const bool changed_ok = - sameValue(loaded_map.discreteLayers().manually_changed.getValue(indices[0]), - true) && - sameValue(loaded_map.discreteLayers().manually_changed.getValue(indices[1]), - false); - const bool unknown_ok = - !loaded_map.discreteLayers().semantic_label.getValue( - wavemap::Index3D(9, 9, 9)); - - std::cout << "Custom LayeredMap save/load example\n"; - std::cout << "saved file: " << output_path << "\n"; - std::cout << "continuous voxels matched: " << matching_continuous << "/" - << indices.size() << "\n"; - std::cout << "semantic labels preserved: " << (semantic_ok ? "yes" : "no") - << "\n"; - std::cout << "manual changed flags preserved: " - << (changed_ok ? "yes" : "no") << "\n"; - std::cout << "unknown custom discrete voxel remains unknown: " - << (unknown_ok ? "yes" : "no") << "\n"; - - return matching_continuous == indices.size() && semantic_ok && changed_ok && - unknown_ok - ? 0 - : 1; -} diff --git a/examples/cpp/digital_twin/io/inspect_field_layered_map.cc b/examples/cpp/digital_twin/io/inspect_field_layered_map.cc deleted file mode 100644 index f3eb1c079..000000000 --- a/examples/cpp/digital_twin/io/inspect_field_layered_map.cc +++ /dev/null @@ -1,58 +0,0 @@ -#include -#include -#include -#include -#include - -#include "../common/field_layered_map_config.h" - -namespace field_map = wavemap::examples::field_map; - -int main(int argc, char** argv) { - if (argc != 2) { - std::cerr << "Usage: " << argv[0] << " \n"; - return 1; - } - - field_map::Map map(field_map::makeDefaultConfig()); - std::string error; - if (!field_map::MapIo::load(std::filesystem::path(argv[1]), map, &error)) { - std::cerr << "Could not load field layered map: " << error << "\n"; - return 1; - } - - size_t leaves = 0u; - size_t observed_occupancy = 0u; - size_t observed_reflectivity = 0u; - float min_reflectivity = std::numeric_limits::max(); - float max_reflectivity = std::numeric_limits::lowest(); - map.continuousMap().forEachVoxelLeaf( - [&](const wavemap::OctreeIndex&, const field_map::Voxel& voxel) { - ++leaves; - if (1e-5f < std::abs(voxel.occupancy)) { - ++observed_occupancy; - } - const float reflectivity = - voxel.data.get(); - if (1e-5f < std::abs(reflectivity)) { - ++observed_reflectivity; - min_reflectivity = std::min(min_reflectivity, reflectivity); - max_reflectivity = std::max(max_reflectivity, reflectivity); - } - }); - - std::cout << "Field layered map inspection\n" - << " allocated blocks: " - << map.continuousMap().getHashMap().size() << "\n" - << " leaf nodes: " << leaves << "\n" - << " leaves with occupancy evidence: " << observed_occupancy - << "\n" - << " leaves with reflectivity: " << observed_reflectivity << "\n"; - if (observed_reflectivity) { - std::cout << " nonzero reflectivity range: [" << min_reflectivity << ", " - << max_reflectivity << "]\n"; - } - - return observed_occupancy && observed_reflectivity ? 0 : 2; -} - diff --git a/examples/cpp/digital_twin/io/inspect_ouster_benchmark_maps.cc.orig b/examples/cpp/digital_twin/io/inspect_ouster_benchmark_maps.cc.orig deleted file mode 100644 index ea516a6ab..000000000 --- a/examples/cpp/digital_twin/io/inspect_ouster_benchmark_maps.cc.orig +++ /dev/null @@ -1,211 +0,0 @@ -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include - -#include "../ouster_mapping/occupancy_map_layers.h" -#include "../ouster_mapping/reflectivity_map_layers.h" -#include "../ouster_mapping/reflectivity_only_map_layers.h" - -namespace occupancy_map = wavemap::examples::occupancy_map; -namespace reflectivity_only_map = wavemap::examples::reflectivity_only_map; -namespace reflectivity_map = wavemap::examples::reflectivity_map; - -namespace { -struct OccupancyStats { - size_t blocks = 0u; - size_t leaves = 0u; - size_t observed = 0u; - float min = std::numeric_limits::max(); - float max = std::numeric_limits::lowest(); -}; - -template -OccupancyStats occupancyStats(const MapT& map) { - OccupancyStats stats; - stats.blocks = map.getHashMap().size(); - map.forEachLeaf([&](const wavemap::OctreeIndex&, float occupancy) { - ++stats.leaves; - stats.observed += 1e-5f < std::abs(occupancy); - stats.min = std::min(stats.min, occupancy); - stats.max = std::max(stats.max, occupancy); - }); - return stats; -} - -struct Comparison { - size_t samples = 0u; - size_t above_tolerance = 0u; - double absolute_error_sum = 0.; - float max_absolute_error = 0.f; -}; - -template -Comparison compareDirection(const LhsMapT& lhs, const RhsMapT& rhs, - float tolerance = 1e-4f) { - Comparison comparison; - lhs.forEachLeaf([&](const wavemap::OctreeIndex& index, float lhs_value) { - const float error = std::abs(lhs_value - rhs.getCellValue(index)); - ++comparison.samples; - comparison.above_tolerance += tolerance < error; - comparison.absolute_error_sum += error; - comparison.max_absolute_error = - std::max(comparison.max_absolute_error, error); - }); - return comparison; -} - -template -void printComparison(const std::string& name, const LhsMapT& lhs, - const RhsMapT& rhs) { - const Comparison forward = compareDirection(lhs, rhs); - const Comparison reverse = compareDirection(rhs, lhs); - const auto print = [](const char* direction, const Comparison& comparison) { - std::cout << " " << direction << ": samples=" << comparison.samples - << " above_1e-4=" << comparison.above_tolerance - << " mean_abs=" - << (comparison.samples - ? comparison.absolute_error_sum / comparison.samples - : 0.) - << " max_abs=" << comparison.max_absolute_error << "\n"; - }; - std::cout << " " << name << "\n"; - print("forward", forward); - print("reverse", reverse); -} - -template -void printReflectivityStats(const std::string& name, - const typename DefinitionT::Map& map) { - size_t observed = 0u; - size_t out_of_bounds = 0u; - size_t very_small_weight = 0u; - float min_value = std::numeric_limits::max(); - float max_value = std::numeric_limits::lowest(); - float min_weight = std::numeric_limits::max(); - float max_weight = 0.f; - map.continuousMap().forEachVoxelLeaf( - [&](const wavemap::OctreeIndex&, - const typename DefinitionT::Voxel& voxel) { - const auto& state = voxel.data.template get(); - if (0.f < state.total_weight) { - const float value = state.valueOr(); - ++observed; - out_of_bounds += value < 0.f || 1.f < value; - very_small_weight += state.total_weight < 1e-3f; - min_value = std::min(min_value, value); - max_value = std::max(max_value, value); - min_weight = std::min(min_weight, state.total_weight); - max_weight = std::max(max_weight, state.total_weight); - } - }); - std::cout << " " << name << ": observed=" << observed - << " out_of_[0,1]=" << out_of_bounds - << " weight_below_1e-3=" << very_small_weight - << " value_range=[" << min_value << ", " << max_value << "]" - << " weight_range=[" << min_weight << ", " << max_weight << "]\n"; -} - -void printOccupancyStats(const std::string& name, - const OccupancyStats& stats) { - std::cout << " " << name << ": blocks=" << stats.blocks - << " leaves=" << stats.leaves << " observed=" << stats.observed - << " range=[" << stats.min << ", " << stats.max << "]\n"; -} -} // namespace - -int main(int argc, char** argv) { - if (argc != 5) { - std::cerr << "Usage: " << argv[0] - << " \n"; - return 1; - } - - wavemap::MapBase::Ptr original_base; - if (!wavemap::io::fileToMap(argv[1], original_base)) { - std::cerr << "Could not load A\n"; - return 2; - } - auto original = - std::dynamic_pointer_cast(original_base); - if (!original) { - std::cerr << "A is not a HashedWaveletOctree\n"; - return 2; - } - - occupancy_map::Definition::Map occupancy(occupancy_map::makeMapConfig()); - reflectivity_only_map::Definition::Map reflectivity( - reflectivity_only_map::makeMapConfig()); - reflectivity_map::Definition::Map reflectivity_class( - reflectivity_map::makeMapConfig()); - std::string error; - if (!occupancy_map::Definition::MapIo::load(argv[2], occupancy, &error)) { - std::cerr << "Could not load B: " << error << "\n"; - return 2; - } - if (!reflectivity_only_map::Definition::MapIo::load(argv[3], reflectivity, - &error)) { - std::cerr << "Could not load C: " << error << "\n"; - return 2; - } - if (!reflectivity_map::Definition::MapIo::load(argv[4], reflectivity_class, - &error)) { - std::cerr << "Could not load D: " << error << "\n"; - return 2; - } - - std::cout << "Occupancy statistics\n"; - printOccupancyStats("A original", occupancyStats(*original)); - printOccupancyStats("B layered occupancy", - occupancyStats(occupancy.continuousMap())); - printOccupancyStats("C reflectivity", - occupancyStats(reflectivity.continuousMap())); - printOccupancyStats("D reflectivity+class", - occupancyStats(reflectivity_class.continuousMap())); - - std::cout << "Occupancy comparisons\n"; - printComparison("A vs B", *original, occupancy.continuousMap()); - printComparison("A vs C", *original, reflectivity.continuousMap()); - printComparison("A vs D", *original, reflectivity_class.continuousMap()); - printComparison("B vs C", occupancy.continuousMap(), - reflectivity.continuousMap()); - printComparison("C vs D", reflectivity.continuousMap(), - reflectivity_class.continuousMap()); - - std::cout << "Reflectivity statistics\n"; - printReflectivityStats( - "C reflectivity", reflectivity); - printReflectivityStats( - "D reflectivity", reflectivity_class); - - const auto& class_layer = - reflectivity_class.discreteLayers().get(); - size_t ground = 0u; - size_t obstacle = 0u; - for (const auto& [key, cell] : class_layer.cells()) { - (void)key; - for (const int offset : cell.observed_offsets) { - const auto exception = cell.exceptions.find(offset); - const int value = exception == cell.exceptions.end() - ? cell.dominant_value - : exception->second; - ground += value == static_cast( - wavemap::layered::GeometricClass::kGround); - obstacle += value == static_cast( - wavemap::layered::GeometricClass::kObstacle); - } - } - std::cout << "Class statistics\n" - << " observed=" << class_layer.observedValueCount() - << " ground=" << ground << " obstacle=" << obstacle << "\n"; - return 0; -} diff --git a/examples/cpp/digital_twin/io/inspect_reflectivity_layered_map.cc b/examples/cpp/digital_twin/io/inspect_reflectivity_layered_map.cc deleted file mode 100644 index 7553f2ad8..000000000 --- a/examples/cpp/digital_twin/io/inspect_reflectivity_layered_map.cc +++ /dev/null @@ -1,98 +0,0 @@ -#include -#include -#include -#include -#include - -#include - -#include "../ouster_mapping/reflectivity_map_layers.h" - -namespace reflectivity_map = wavemap::examples::reflectivity_map; - -int main(int argc, char** argv) { - if (argc != 2) { - std::cerr << "Usage: " << argv[0] << " \n"; - return 1; - } - - reflectivity_map::Definition::Map map(reflectivity_map::makeMapConfig()); - std::string error; - if (!reflectivity_map::Definition::MapIo::load( - std::filesystem::path(argv[1]), map, &error)) { - std::cerr << "Could not load reflectivity layered map: " << error << "\n"; - return 1; - } - - size_t leaves = 0u; - size_t observed_occupancy = 0u; - size_t observed_reflectivity = 0u; - float min_reflectivity = std::numeric_limits::max(); - float max_reflectivity = std::numeric_limits::lowest(); - size_t out_of_bounds_reflectivity = 0u; - size_t ground_classes = 0u; - size_t obstacle_classes = 0u; - const auto& storage_bounds = - map.continuousMap() - .getThresholdConfig() - .data.get(); - map.continuousMap().forEachVoxelLeaf( - [&](const wavemap::OctreeIndex&, - const reflectivity_map::Definition::Voxel& voxel) { - ++leaves; - if (1e-5f < std::abs(voxel.occupancy)) { - ++observed_occupancy; - } - const float reflectivity = - voxel.data.get(); - if (1e-6f < std::abs(reflectivity)) { - ++observed_reflectivity; - } - out_of_bounds_reflectivity += - reflectivity < storage_bounds.min || - storage_bounds.max < reflectivity; - min_reflectivity = std::min(min_reflectivity, reflectivity); - max_reflectivity = std::max(max_reflectivity, reflectivity); - }); - - const auto& class_layer = - map.discreteLayers().get(); - for (const auto& [parent_key, cell] : class_layer.cells()) { - (void)parent_key; - for (const int offset : cell.observed_offsets) { - const auto exception = cell.exceptions.find(offset); - const int value = exception == cell.exceptions.end() - ? cell.dominant_value - : exception->second; - ground_classes += value == static_cast( - wavemap::layered::GeometricClass::kGround); - obstacle_classes += - value == static_cast( - wavemap::layered::GeometricClass::kObstacle); - } - } - - std::cout << "Reflectivity layered map inspection\n" - << " allocated blocks: " - << map.continuousMap().getHashMap().size() << "\n" - << " leaf nodes: " << leaves << "\n" - << " leaves with occupancy evidence: " << observed_occupancy - << "\n" - << " leaves with reflectivity: " << observed_reflectivity << "\n" - << " class values: " << class_layer.observedValueCount() << "\n" - << " ground: " << ground_classes << "\n" - << " obstacle: " << obstacle_classes << "\n"; - std::cout << " configured reflectivity storage range: [" - << storage_bounds.min << ", " << storage_bounds.max << "]\n"; - if (leaves) { - std::cout << " reconstructed reflectivity range: [" << min_reflectivity - << ", " << max_reflectivity << "]\n" - << " reflectivity values outside storage bounds: " - << out_of_bounds_reflectivity << "\n"; - } - - return observed_occupancy && observed_reflectivity && - class_layer.observedValueCount() - ? 0 - : 2; -} diff --git a/examples/cpp/digital_twin/io/inspect_reflectivity_layered_map.cc.orig b/examples/cpp/digital_twin/io/inspect_reflectivity_layered_map.cc.orig deleted file mode 100644 index 60a9bf65e..000000000 --- a/examples/cpp/digital_twin/io/inspect_reflectivity_layered_map.cc.orig +++ /dev/null @@ -1,101 +0,0 @@ -#include -#include -#include -#include -#include - -#include - -#include "../ouster_mapping/reflectivity_map_layers.h" - -namespace reflectivity_map = wavemap::examples::reflectivity_map; - -int main(int argc, char** argv) { - if (argc != 2) { - std::cerr << "Usage: " << argv[0] << " \n"; - return 1; - } - - reflectivity_map::Definition::Map map(reflectivity_map::makeMapConfig()); - std::string error; - if (!reflectivity_map::Definition::MapIo::load( - std::filesystem::path(argv[1]), map, &error)) { - std::cerr << "Could not load reflectivity layered map: " << error << "\n"; - return 1; - } - - size_t leaves = 0u; - size_t observed_occupancy = 0u; - size_t observed_reflectivity = 0u; - float min_reflectivity = std::numeric_limits::max(); - float max_reflectivity = std::numeric_limits::lowest(); - float max_reflectivity_weight = 0.f; - size_t ground_classes = 0u; - size_t obstacle_classes = 0u; - const auto& storage_bounds = - map.continuousMap() - .getThresholdConfig() - .data.get(); - map.continuousMap().forEachVoxelLeaf( - [&](const wavemap::OctreeIndex&, - const reflectivity_map::Definition::Voxel& voxel) { - ++leaves; - if (1e-5f < std::abs(voxel.occupancy)) { - ++observed_occupancy; - } - const auto& reflectivity_state = - voxel.data.get(); - if (0.f < reflectivity_state.total_weight) { - const float reflectivity = - wavemap::layered::LayerStateConversion< - reflectivity_map::ReflectivityLayer>::value( - reflectivity_state); - ++observed_reflectivity; - min_reflectivity = std::min(min_reflectivity, reflectivity); - max_reflectivity = std::max(max_reflectivity, reflectivity); - max_reflectivity_weight = std::max( - max_reflectivity_weight, reflectivity_state.total_weight); - } - }); - - const auto& class_layer = - map.discreteLayers().get(); - for (const auto& [parent_key, cell] : class_layer.cells()) { - (void)parent_key; - for (const int offset : cell.observed_offsets) { - const auto exception = cell.exceptions.find(offset); - const int value = exception == cell.exceptions.end() - ? cell.dominant_value - : exception->second; - ground_classes += value == static_cast( - wavemap::layered::GeometricClass::kGround); - obstacle_classes += - value == static_cast( - wavemap::layered::GeometricClass::kObstacle); - } - } - - std::cout << "Reflectivity layered map inspection\n" - << " allocated blocks: " - << map.continuousMap().getHashMap().size() << "\n" - << " leaf nodes: " << leaves << "\n" - << " leaves with occupancy evidence: " << observed_occupancy - << "\n" - << " leaves with reflectivity: " << observed_reflectivity << "\n" - << " class values: " << class_layer.observedValueCount() << "\n" - << " ground: " << ground_classes << "\n" - << " obstacle: " << obstacle_classes << "\n"; - std::cout << " configured reflectivity storage range: [" - << storage_bounds.min << ", " << storage_bounds.max << "]\n"; - if (observed_reflectivity) { - std::cout << " nonzero reflectivity range: [" << min_reflectivity << ", " - << max_reflectivity << "]\n" - << " maximum reflectivity weight: " - << max_reflectivity_weight << "\n"; - } - - return observed_occupancy && observed_reflectivity && - class_layer.observedValueCount() - ? 0 - : 2; -} diff --git a/examples/cpp/digital_twin/io/layered_map_io_experiment.cc b/examples/cpp/digital_twin/io/layered_map_io_experiment.cc deleted file mode 100644 index 3b8c77090..000000000 --- a/examples/cpp/digital_twin/io/layered_map_io_experiment.cc +++ /dev/null @@ -1,81 +0,0 @@ -#include -#include -#include - -#include - -#include "../common/layered_voxel_config.h" - -int main() { - wavemap::HashedWaveletOctreeConfig config; - config.min_cell_width = 0.1f; - config.min_log_odds = -2.f; - config.max_log_odds = 4.f; - config.tree_height = 3; - - ContinuousWaveletMap map(config); - - const wavemap::Index3D cube_origin(8, 2, 2); - constexpr int cube_size_x = 2; - constexpr int cube_size_y = 2; - constexpr int cube_size_z = 4; - size_t populated_voxels = 0u; - std::vector populated_indices; - populated_indices.reserve(cube_size_x * cube_size_y * cube_size_z); - - // Populate a small cube of voxels with varying occupancy, color, and traversability values. - for (int dx = 0; dx < cube_size_x; ++dx) { - for (int dy = 0; dy < cube_size_y; ++dy) { - for (int dz = 0; dz < cube_size_z; ++dz) { - const wavemap::Index3D voxel_index = cube_origin + wavemap::Index3D(dx, dy, dz); - const wavemap::FloatingPoint layer_value = static_cast(populated_voxels); - const LayeredVoxel original_voxel(0.5f + 0.02f * layer_value, ContinuousLayers{rgb(0.1f * dx, 0.1f * dy, 0.05f * dz), 0.9f - 0.03f * layer_value}); - const LayeredVoxel voxel_update(0.1f, ContinuousLayers{rgb(0.01f, 0.02f, 0.03f), -0.01f}); - - map.setVoxelValue(voxel_index, original_voxel); - map.addToVoxelValue(voxel_index, voxel_update); - populated_indices.emplace_back(voxel_index); - ++populated_voxels; - } - } - } - - const wavemap::Index3D sample_voxel_index = cube_origin + wavemap::Index3D(1, 1, 2); - - const std::filesystem::path output_path = "/home/ci/data/maps/layered_map.wvmp"; - if (!wavemap::io::mapToFile(map, output_path)) { - std::cerr << "Failed to save layered map to: " << output_path << "\n"; - return 1; - } - - ContinuousWaveletMap::Ptr loaded_map; - if (!wavemap::io::fileToMap(output_path, loaded_map)) { - std::cerr << "Failed to load layered map from: " << output_path << "\n"; - return 1; - } - - if (!loaded_map) { - std::cerr << "Loaded map pointer is null.\n"; - return 1; - } - - size_t verified_voxels = 0u; - for (const wavemap::Index3D& voxel_index : populated_indices) { - const LayeredVoxel original_voxel = map.getVoxelValue(voxel_index); - const LayeredVoxel loaded_voxel = loaded_map->getVoxelValue(voxel_index); - if (original_voxel == loaded_voxel) { - ++verified_voxels; - } else { - std::cerr << "Layered voxel mismatch at index " << voxel_index.transpose() << "\n"; - printVoxel("Original", original_voxel); - printVoxel("Loaded", loaded_voxel); - } - } - - printVoxel("Original sample voxel", map.getVoxelValue(sample_voxel_index)); - printVoxel("Loaded sample voxel", loaded_map->getVoxelValue(sample_voxel_index)); - std::cout << "Verified layered voxels: " << verified_voxels << " / " << populated_indices.size() << "\n"; - std::cout << "Saved layered map to: " << output_path << "\n"; - - return verified_voxels == populated_indices.size() ? 0 : 1; -} diff --git a/examples/cpp/digital_twin/io/layered_map_save_load_experiment.cc b/examples/cpp/digital_twin/io/layered_map_save_load_experiment.cc deleted file mode 100644 index 21d022e6b..000000000 --- a/examples/cpp/digital_twin/io/layered_map_save_load_experiment.cc +++ /dev/null @@ -1,115 +0,0 @@ -#include -#include -#include -#include - -#include "../common/example_layered_map_config.h" -#include - -namespace { -template -bool sameValue(const std::optional& value, const ValueT& expected) { - return value && *value == expected; -} - -bool sameVoxel(const LayeredVoxel& lhs, const LayeredVoxel& rhs) { - return lhs == rhs; -} -} // namespace - -int main() { - ExampleLayeredMapConfig config; - config.continuous_map.min_cell_width = 0.1f; - config.continuous_map.min_log_odds = -100.f; - config.continuous_map.max_log_odds = 100.f; - config.continuous_map.tree_height = 3; - config.discrete_compression.block_height = 1; - - ExampleLayeredMap map(config); - - const std::vector indices{ - wavemap::Index3D(0, 0, 0), wavemap::Index3D(1, 0, 0), - wavemap::Index3D(1, 1, 1), wavemap::Index3D(4, 0, 0)}; - - map.continuousMap().setVoxelValue( - indices[0], makeLayeredVoxel(10.f, rgb(1.f, 0.f, 0.f), 0.1f)); - map.continuousMap().setVoxelValue( - indices[1], makeLayeredVoxel(11.f, rgb(0.f, 1.f, 0.f), 0.2f)); - map.continuousMap().setVoxelValue( - indices[2], makeLayeredVoxel(12.f, rgb(0.f, 0.f, 1.f), 0.3f)); - map.continuousMap().setVoxelValue( - indices[3], makeLayeredVoxel(13.f, rgb(0.5f, 0.5f, 0.5f), 0.4f)); - - map.discreteLayers().semantic.setValue(indices[0], 1); - map.discreteLayers().semantic.setValue(indices[1], 1); - map.discreteLayers().semantic.setValue(indices[2], 2); - map.discreteLayers().semantic.setValue(indices[3], 4); - - map.discreteLayers().changed.setValue(indices[0], false); - map.discreteLayers().changed.setValue(indices[1], false); - map.discreteLayers().changed.setValue(indices[2], true); - map.discreteLayers().changed.setValue(indices[3], true); - - const std::filesystem::path output_dir = "/home/ci/data/maps"; - std::filesystem::create_directories(output_dir); - const std::filesystem::path output_path = output_dir / "layered_map_full.lwvmp"; - - if (!ExampleLayeredMapIo::save(output_path, map)) { - std::cerr << "Failed to save LayeredMap to " << output_path << "\n"; - return 1; - } - - ExampleLayeredMap loaded_map(config); - if (!ExampleLayeredMapIo::load(output_path, loaded_map)) { - std::cerr << "Failed to load LayeredMap from " << output_path << "\n"; - return 1; - } - - size_t matching_continuous = 0u; - for (const wavemap::Index3D& index : indices) { - if (sameVoxel(map.continuousMap().getVoxelValue(index), - loaded_map.continuousMap().getVoxelValue(index))) { - ++matching_continuous; - } - } - - const bool semantic_ok = - sameValue(loaded_map.discreteLayers().semantic.getValue(indices[0]), 1) && - sameValue(loaded_map.discreteLayers().semantic.getValue(indices[1]), 1) && - sameValue(loaded_map.discreteLayers().semantic.getValue(indices[2]), 2) && - sameValue(loaded_map.discreteLayers().semantic.getValue(indices[3]), 4); - const bool changed_ok = - sameValue(loaded_map.discreteLayers().changed.getValue(indices[0]), false) && - sameValue(loaded_map.discreteLayers().changed.getValue(indices[1]), false) && - sameValue(loaded_map.discreteLayers().changed.getValue(indices[2]), true) && - sameValue(loaded_map.discreteLayers().changed.getValue(indices[3]), true); - const bool unknown_stays_unknown = - !loaded_map.discreteLayers().semantic.getValue(wavemap::Index3D(2, 2, 2)); - const bool stats_ok = - loaded_map.discreteLayers().semantic.parentCount() == - map.discreteLayers().semantic.parentCount() && - loaded_map.discreteLayers().semantic.exceptionCount() == - map.discreteLayers().semantic.exceptionCount() && - loaded_map.discreteLayers().changed.parentCount() == - map.discreteLayers().changed.parentCount() && - loaded_map.discreteLayers().changed.exceptionCount() == - map.discreteLayers().changed.exceptionCount(); - - std::cout << "LayeredMap save/load experiment\n"; - std::cout << "saved file: " << output_path << "\n"; - std::cout << "continuous voxels matched: " << matching_continuous << "/" - << indices.size() << "\n"; - std::cout << "semantic values preserved: " << (semantic_ok ? "yes" : "no") - << "\n"; - std::cout << "changed values preserved: " << (changed_ok ? "yes" : "no") - << "\n"; - std::cout << "unknown discrete voxel remains unknown: " - << (unknown_stays_unknown ? "yes" : "no") << "\n"; - std::cout << "discrete compression stats preserved: " - << (stats_ok ? "yes" : "no") << "\n"; - - return matching_continuous == indices.size() && semantic_ok && changed_ok && - unknown_stays_unknown && stats_ok - ? 0 - : 1; -} diff --git a/examples/cpp/digital_twin/io/layered_map_save_load_failure_experiment.cc b/examples/cpp/digital_twin/io/layered_map_save_load_failure_experiment.cc deleted file mode 100644 index 36554c1d1..000000000 --- a/examples/cpp/digital_twin/io/layered_map_save_load_failure_experiment.cc +++ /dev/null @@ -1,170 +0,0 @@ -#include -#include -#include -#include -#include - -#include "../common/example_layered_map_config.h" -#include - -namespace { -bool expectFailure(const std::string& name, bool load_result, - const std::string& error_message) { - const bool failed = !load_result; - std::cout << " " << name << ": " << (failed ? "pass" : "FAIL"); - if (failed && !error_message.empty()) { - std::cout << " (" << error_message << ")"; - } - std::cout << "\n"; - return failed; -} - -bool loadWithError(const std::filesystem::path& path, ExampleLayeredMap& map, - std::string& error_message) { - error_message.clear(); - return ExampleLayeredMapIo::load(path, map, &error_message); -} - -bool replaceBytes(const std::filesystem::path& path, const std::string& from, - const std::string& to) { - if (from.size() != to.size()) { - return false; - } - - std::ifstream input(path, std::ios::binary); - if (!input.is_open()) { - return false; - } - std::string data((std::istreambuf_iterator(input)), - std::istreambuf_iterator()); - - size_t replacements = 0u; - size_t pos = data.find(from); - while (pos != std::string::npos) { - data.replace(pos, from.size(), to); - ++replacements; - pos = data.find(from, pos + to.size()); - } - if (replacements == 0u) { - return false; - } - - std::ofstream output(path, std::ios::binary | std::ios::trunc); - if (!output.is_open()) { - return false; - } - output.write(data.data(), static_cast(data.size())); - return static_cast(output); -} - -bool writeHeaderOnlyFile(const std::filesystem::path& path, - const std::string& magic, uint32_t version) { - std::ofstream output(path, std::ios::binary | std::ios::trunc); - if (!output.is_open()) { - return false; - } - return wavemap::layered::io::detail::writeString(output, magic) && - wavemap::layered::io::detail::writePod(output, version); -} - -void fillExampleMap(ExampleLayeredMap& map) { - const wavemap::Index3D a(0, 0, 0); - const wavemap::Index3D b(1, 0, 0); - map.continuousMap().setVoxelValue( - a, makeLayeredVoxel(10.f, rgb(1.f, 0.f, 0.f), 0.1f)); - map.continuousMap().setVoxelValue( - b, makeLayeredVoxel(11.f, rgb(0.f, 1.f, 0.f), 0.2f)); - map.discreteLayers().semantic.setValue(a, 1); - map.discreteLayers().semantic.setValue(b, 2); - map.discreteLayers().changed.setValue(a, false); - map.discreteLayers().changed.setValue(b, true); -} -} // namespace - -int main() { - ExampleLayeredMapConfig config; - config.continuous_map.min_cell_width = 0.1f; - config.continuous_map.min_log_odds = -100.f; - config.continuous_map.max_log_odds = 100.f; - config.continuous_map.tree_height = 3; - config.discrete_compression.block_height = 1; - - const std::filesystem::path output_dir = "/home/ci/data/maps"; - std::filesystem::create_directories(output_dir); - - bool ok = true; - std::cout << "LayeredMap save/load failure experiment\n"; - - { - ExampleLayeredMap loaded(config); - std::string error_message; - ok &= expectFailure( - "missing file fails", - loadWithError(output_dir / "missing_file.lwvmp", loaded, error_message), - error_message); - } - - { - const auto path = output_dir / "bad_magic.lwvmp"; - if (!writeHeaderOnlyFile(path, "BADMP", wavemap::layered::io::detail::kVersion)) { - std::cerr << "Failed to write bad magic test file.\n"; - return 1; - } - ExampleLayeredMap loaded(config); - std::string error_message; - ok &= expectFailure("wrong magic fails", - loadWithError(path, loaded, error_message), - error_message); - } - - { - const auto path = output_dir / "bad_version.lwvmp"; - if (!writeHeaderOnlyFile(path, wavemap::layered::io::detail::kMagic, - wavemap::layered::io::detail::kVersion + 1u)) { - std::cerr << "Failed to write bad version test file.\n"; - return 1; - } - ExampleLayeredMap loaded(config); - std::string error_message; - ok &= expectFailure("unsupported version fails", - loadWithError(path, loaded, error_message), - error_message); - } - - ExampleLayeredMap map(config); - fillExampleMap(map); - - { - const auto path = output_dir / "bad_layer_name.lwvmp"; - if (!ExampleLayeredMapIo::save(path, map) || - !replaceBytes(path, "semantic", "semantix")) { - std::cerr << "Failed to create bad layer name test file.\n"; - return 1; - } - ExampleLayeredMap loaded(config); - std::string error_message; - ok &= expectFailure("wrong discrete layer name fails", - loadWithError(path, loaded, error_message), - error_message); - } - - { - const auto path = output_dir / "bad_layer_type.lwvmp"; - if (!ExampleLayeredMapIo::save(path, map) || - !replaceBytes(path, "bool", "b00l")) { - std::cerr << "Failed to create bad layer type test file.\n"; - return 1; - } - ExampleLayeredMap loaded(config); - std::string error_message; - ok &= expectFailure("wrong discrete layer type fails", - loadWithError(path, loaded, error_message), - error_message); - } - - if (!ok) { - std::cout << "LayeredMap failure checks did not all fail as expected.\n"; - return 1; - } - return 0; -} diff --git a/examples/cpp/digital_twin/io/legacy_file_io_experiment.cc b/examples/cpp/digital_twin/io/legacy_file_io_experiment.cc deleted file mode 100644 index 846ebe962..000000000 --- a/examples/cpp/digital_twin/io/legacy_file_io_experiment.cc +++ /dev/null @@ -1,51 +0,0 @@ -#include -#include -#include - -#include -#include -#include - -int main() { - wavemap::HashedWaveletOctreeConfig config; - config.min_cell_width = 0.1f; - config.min_log_odds = -2.f; - config.max_log_odds = 4.f; - config.tree_height = 3; - - wavemap::HashedWaveletOctree map(config); - - const wavemap::Index3D voxel_index(9, 2, 3); - map.setCellValue(voxel_index, 0.8f); - map.addToCellValue(voxel_index, 0.2f); - - const std::filesystem::path output_path = - "/home/ci/data/maps/legacy_hashed_wavelet_octree.wvmp"; - if (!wavemap::io::mapToFile(map, output_path)) { - std::cerr << "Failed to save legacy map to: " << output_path << "\n"; - return 1; - } - - wavemap::MapBase::Ptr loaded_map; - if (!wavemap::io::fileToMap(output_path, loaded_map)) { - std::cerr << "Failed to load legacy map from: " << output_path << "\n"; - return 1; - } - - auto loaded_hashed_map = - std::dynamic_pointer_cast(loaded_map); - if (!loaded_hashed_map) { - std::cerr << "Loaded map is not a HashedWaveletOctree.\n"; - return 1; - } - - const wavemap::FloatingPoint original_value = map.getCellValue(voxel_index); - const wavemap::FloatingPoint loaded_value = - loaded_hashed_map->getCellValue(voxel_index); - - std::cout << "Original occupancy: " << original_value << "\n"; - std::cout << "Loaded occupancy: " << loaded_value << "\n"; - std::cout << "Saved legacy map to: " << output_path << "\n"; - - return original_value == loaded_value ? 0 : 1; -} diff --git a/examples/cpp/digital_twin/io/legacy_hashed_wavelet_octree_experiment.cc b/examples/cpp/digital_twin/io/legacy_hashed_wavelet_octree_experiment.cc deleted file mode 100644 index 3e8509561..000000000 --- a/examples/cpp/digital_twin/io/legacy_hashed_wavelet_octree_experiment.cc +++ /dev/null @@ -1,38 +0,0 @@ -#include - -#include - -int main() { - wavemap::HashedWaveletOctreeConfig config; - config.min_cell_width = 0.1f; - config.min_log_odds = -2.f; - config.max_log_odds = 4.f; - config.tree_height = 3; - - wavemap::HashedWaveletOctree map(config); - - const wavemap::Index3D voxel_index(9, 2, 3); - - map.setCellValue(voxel_index, 0.8f); - std::cout << "After setCellValue: " << map.getCellValue(voxel_index) - << "\n"; - - map.addToCellValue(voxel_index, 0.2f); - std::cout << "After addToCellValue: " << map.getCellValue(voxel_index) - << "\n"; - - size_t visited_leaves = 0u; - map.forEachLeaf( - [&visited_leaves, &voxel_index](const wavemap::OctreeIndex& leaf_index, - wavemap::FloatingPoint occupancy) { - ++visited_leaves; - if (leaf_index.height == 0 && leaf_index.position == voxel_index) { - std::cout << "Visited target leaf occupancy: " << occupancy << "\n"; - } - }); - - std::cout << "Visited leaves: " << visited_leaves << "\n"; - std::cout << "Block count: " << map.getHashMap().size() << "\n"; - - return 0; -} diff --git a/examples/cpp/digital_twin/io/load_existing_maps_experiment.cc b/examples/cpp/digital_twin/io/load_existing_maps_experiment.cc deleted file mode 100644 index 549c105f9..000000000 --- a/examples/cpp/digital_twin/io/load_existing_maps_experiment.cc +++ /dev/null @@ -1,139 +0,0 @@ -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include - -#include "../common/example_layered_map_config.h" - -namespace { -void printUsage(const char* executable_name) { - std::cerr << "Usage: " << executable_name - << " \n"; -} - -void printLayeredMapSchema(const wavemap::layered::LayeredMapSchema& schema) { - std::cout << " schema continuous layers:"; - if (schema.continuous_layers.empty()) { - std::cout << " none"; - } - std::cout << "\n"; - for (const auto& layer : schema.continuous_layers) { - std::cout << " - " << layer.name << " (" << layer.type << ")\n"; - } - - std::cout << " schema discrete layers:"; - if (schema.discrete_layers.empty()) { - std::cout << " none"; - } - std::cout << "\n"; - for (const auto& layer : schema.discrete_layers) { - std::cout << " - " << layer.name << " (" << layer.type << ")\n"; - } -} - -bool inspectWavemapFile(const std::filesystem::path& path) { - wavemap::MapBase::Ptr map; - if (!wavemap::io::fileToMap(path, map) || !map) { - std::cerr << "Failed to load Wavemap file: " << path << "\n"; - return false; - } - - std::cout << "Loaded Wavemap file: " << path << "\n"; - std::cout << " min cell width: " << map->getMinCellWidth() << "\n"; - std::cout << " tree height: " << map->getTreeHeight() << "\n"; - std::cout << " log odds range: [" << map->getMinLogOdds() << ", " - << map->getMaxLogOdds() << "]\n"; - - if (const auto hashed_wavelet = - std::dynamic_pointer_cast(map)) { - std::cout << " type: HashedWaveletOctree\n"; - std::cout << " allocated blocks: " << hashed_wavelet->getHashMap().size() - << "\n"; - } else if (std::dynamic_pointer_cast(map)) { - std::cout << " type: WaveletOctree\n"; - } else if (const auto hashed_blocks = - std::dynamic_pointer_cast(map)) { - std::cout << " type: HashedBlocks\n"; - std::cout << " allocated blocks: " << hashed_blocks->getHashMap().size() - << "\n"; - } else if (std::dynamic_pointer_cast(map)) { - std::cout << " type: VolumetricOctree\n"; - } else { - std::cout << " type: unknown MapBase subclass\n"; - } - return true; -} - -bool inspectLayeredMapFile(const std::filesystem::path& path) { - ExampleLayeredMapConfig config; - ExampleLayeredMap map(config); - const wavemap::layered::LayeredMapSchema expected_schema = - exampleLayeredMapSchema(map); - - wavemap::layered::LayeredMapSchema file_schema; - if (wavemap::layered::io::readLayeredMapSchema(path, file_schema)) { - std::cout << "Read LayeredMap schema before full load: " << path << "\n"; - printLayeredMapSchema(file_schema); - - std::string compatibility_error; - if (wavemap::layered::checkLayeredMapSchemaCompatibility( - file_schema, expected_schema, &compatibility_error)) { - std::cout << " schema compatibility: pass\n"; - } else { - std::cout << " schema compatibility: FAIL\n"; - std::cout << " " << compatibility_error << "\n"; - } - } else { - std::cout << "Could not read LayeredMap schema before full load: " << path - << "\n"; - std::cout << " Note: this can happen for invalid/non-layered files, or " - ".lwvmp files written before schema metadata was added.\n"; - } - - std::string load_error; - if (!ExampleLayeredMapIo::load(path, map, &load_error)) { - std::cerr << "Failed to load LayeredMap file: " << path << "\n"; - if (!load_error.empty()) { - std::cerr << " " << load_error << "\n"; - } - std::cerr << " Note: .lwvmp files are typed. This inspector expects the " - "ExampleLayeredMap schema.\n"; - return false; - } - - std::cout << "Loaded LayeredMap file: " << path << "\n"; - std::cout << " continuous min cell width: " - << map.continuousMap().getMinCellWidth() << "\n"; - std::cout << " continuous tree height: " << map.continuousMap().getTreeHeight() - << "\n"; - std::cout << " continuous allocated blocks: " - << map.continuousMap().getHashMap().size() << "\n"; - std::cout << " semantic parent cells: " - << map.discreteLayers().semantic.parentCount() << "\n"; - std::cout << " semantic exceptions: " - << map.discreteLayers().semantic.exceptionCount() << "\n"; - std::cout << " changed parent cells: " - << map.discreteLayers().changed.parentCount() << "\n"; - std::cout << " changed exceptions: " - << map.discreteLayers().changed.exceptionCount() << "\n"; - return true; -} -} // namespace - -int main(int argc, char** argv) { - if (argc != 3) { - printUsage(argv[0]); - return 1; - } - - const bool wavemap_ok = inspectWavemapFile(argv[1]); - const bool layered_ok = inspectLayeredMapFile(argv[2]); - return wavemap_ok && layered_ok ? 0 : 1; -} diff --git a/examples/cpp/digital_twin/map_api/baseline/float_map_experiment.cc b/examples/cpp/digital_twin/map_api/baseline/float_map_experiment.cc deleted file mode 100644 index 20d81f4e4..000000000 --- a/examples/cpp/digital_twin/map_api/baseline/float_map_experiment.cc +++ /dev/null @@ -1,54 +0,0 @@ -#include - -#include "wavemap/core/indexing/ndtree_index.h" -#include "wavemap/core/map/hashed_wavelet_octree.h" - -int main() { - wavemap::HashedWaveletOctreeConfig config; - config.min_cell_width = 0.1f; - config.min_log_odds = -100.f; - config.max_log_odds = 100.f; - config.tree_height = 3; - - wavemap::HashedWaveletOctree map(config); - - const wavemap::Index3D a(1, 2, 3); - const wavemap::Index3D b(2, 2, 3); - const wavemap::Index3D c(3, 2, 3); - - map.setCellValue(a, 10.f); - map.setCellValue(b, 20.f); - map.setCellValue(c, 30.f); - - std::cout << "Before threshold/prune\n"; - std::cout << "a getCellValue: " << map.getCellValue(a) << "\n"; - std::cout << "b getCellValue: " << map.getCellValue(b) << "\n"; - std::cout << "c getCellValue: " << map.getCellValue(c) << "\n"; - std::cout << "block count: " << map.getHashMap().size() << "\n"; - std::cout << "node count: " << map.size() << "\n"; - - size_t visited_leaves = 0u; - map.forEachVoxelLeaf( - [&visited_leaves](const wavemap::OctreeIndex& index, - wavemap::FloatingPoint value) { - ++visited_leaves; - if (value != 0) { - std::cout << "leaf h=" << index.height << " pos=" - << index.position.transpose() << " value=" << value - << "\n"; - } - }); - std::cout << "visited leaves: " << visited_leaves << "\n"; - - map.threshold(); - map.prune(); - - std::cout << "After threshold/prune\n"; - std::cout << "a getCellValue: " << map.getCellValue(a) << "\n"; - std::cout << "b getCellValue: " << map.getCellValue(b) << "\n"; - std::cout << "c getCellValue: " << map.getCellValue(c) << "\n"; - std::cout << "block count: " << map.getHashMap().size() << "\n"; - std::cout << "node count: " << map.size() << "\n"; - - return 0; -} diff --git a/examples/cpp/digital_twin/map_api/baseline/int_map_experiment.cc b/examples/cpp/digital_twin/map_api/baseline/int_map_experiment.cc deleted file mode 100644 index 30670c500..000000000 --- a/examples/cpp/digital_twin/map_api/baseline/int_map_experiment.cc +++ /dev/null @@ -1,54 +0,0 @@ -#include - -#include "wavemap/core/indexing/ndtree_index.h" -#include "wavemap/core/map/hashed_wavelet_octree.h" - -int main() { - wavemap::HashedWaveletOctreeConfig config; - config.min_cell_width = 0.1f; - config.min_log_odds = -100.f; - config.max_log_odds = 100.f; - config.tree_height = 3; - - wavemap::HashedWaveletOctreeT map(config); - - const wavemap::Index3D a(1, 2, 3); - const wavemap::Index3D b(2, 2, 3); - const wavemap::Index3D c(3, 2, 3); - - map.setVoxelValue(a, 10); - map.setVoxelValue(b, 20); - map.setVoxelValue(c, 30); - - std::cout << "Before threshold/prune\n"; - std::cout << "a getVoxelValue: " << map.getVoxelValue(a) << "\n"; - std::cout << "b getVoxelValue: " << map.getVoxelValue(b) << "\n"; - std::cout << "c getVoxelValue: " << map.getVoxelValue(c) << "\n"; - std::cout << "a getCellValue: " << map.getCellValue(a) << "\n"; - std::cout << "block count: " << map.getHashMap().size() << "\n"; - std::cout << "node count: " << map.size() << "\n"; - - size_t visited_leaves = 0u; - map.forEachVoxelLeaf( - [&visited_leaves](const wavemap::OctreeIndex& index, int value) { - ++visited_leaves; - if (value != 0) { - std::cout << "leaf h=" << index.height << " pos=" - << index.position.transpose() << " value=" << value - << "\n"; - } - }); - std::cout << "visited leaves: " << visited_leaves << "\n"; - - map.threshold(); - map.prune(); - - std::cout << "After threshold/prune\n"; - std::cout << "a getVoxelValue: " << map.getVoxelValue(a) << "\n"; - std::cout << "b getVoxelValue: " << map.getVoxelValue(b) << "\n"; - std::cout << "c getVoxelValue: " << map.getVoxelValue(c) << "\n"; - std::cout << "block count: " << map.getHashMap().size() << "\n"; - std::cout << "node count: " << map.size() << "\n"; - - return 0; -} diff --git a/examples/cpp/digital_twin/map_api/baseline/voxel_block_experiment.cc b/examples/cpp/digital_twin/map_api/baseline/voxel_block_experiment.cc deleted file mode 100644 index d9b0b82ef..000000000 --- a/examples/cpp/digital_twin/map_api/baseline/voxel_block_experiment.cc +++ /dev/null @@ -1,43 +0,0 @@ -#include - -#include "../../common/layered_voxel_config.h" - -int main() { - constexpr wavemap::IndexElement tree_height = 3; - constexpr wavemap::FloatingPoint min_log_odds = -2.f; - constexpr wavemap::FloatingPoint max_log_odds = 4.f; - - LayeredBlock block(tree_height, min_log_odds, max_log_odds); - - const wavemap::OctreeIndex voxel_index{0, wavemap::Index3D{1, 2, 3}}; - const LayeredVoxel initial_voxel( - 0.8f, ContinuousLayers{rgb(1.f, 0.f, 0.f), 0.75f}); - - block.setVoxelValue(voxel_index, initial_voxel); - printVoxel("Stored voxel", block.getVoxelValue(voxel_index)); - - block.addToCellValue(voxel_index, 0.2f); - printVoxel("After occupancy update", block.getVoxelValue(voxel_index)); - - const LayeredVoxel voxel_update( - 0.1f, ContinuousLayers{rgb(0.f, 0.5f, 0.f), -0.25f}); - block.addToVoxelValue(voxel_index, voxel_update); - printVoxel("After full voxel update", block.getVoxelValue(voxel_index)); - - size_t visited_leaves = 0u; - block.forEachVoxelLeaf( - wavemap::Index3D::Zero(), - [&visited_leaves](const wavemap::OctreeIndex& leaf_index, - const LayeredVoxel& voxel) { - ++visited_leaves; - if (leaf_index.height == 0 && - leaf_index.position == wavemap::Index3D{1, 2, 3}) { - printVoxel("Visited target leaf", voxel); - } - }); - - std::cout << "Visited leaves: " << visited_leaves << "\n"; - std::cout << "Occupancy view: " << block.getCellValue(voxel_index) << "\n"; - - return 0; -} diff --git a/examples/cpp/digital_twin/map_api/baseline/voxel_map_experiment.cc b/examples/cpp/digital_twin/map_api/baseline/voxel_map_experiment.cc deleted file mode 100644 index d91994a76..000000000 --- a/examples/cpp/digital_twin/map_api/baseline/voxel_map_experiment.cc +++ /dev/null @@ -1,44 +0,0 @@ -#include - -#include "../../common/layered_voxel_config.h" - -int main() { - wavemap::HashedWaveletOctreeConfig config; - config.min_cell_width = 0.1f; - config.min_log_odds = -2.f; - config.max_log_odds = 4.f; - config.tree_height = 3; - - ContinuousWaveletMap map(config); - - const wavemap::Index3D voxel_index(9, 2, 3); - const LayeredVoxel initial_voxel( - 0.8f, ContinuousLayers{rgb(1.f, 0.f, 0.f), 0.75f}); - - map.setVoxelValue(voxel_index, initial_voxel); - printVoxel("Stored voxel", map.getVoxelValue(voxel_index)); - - map.addToCellValue(voxel_index, 0.2f); - printVoxel("After occupancy update", map.getVoxelValue(voxel_index)); - - const LayeredVoxel voxel_update( - 0.1f, ContinuousLayers{rgb(0.f, 0.5f, 0.f), -0.25f}); - map.addToVoxelValue(voxel_index, voxel_update); - printVoxel("After full voxel update", map.getVoxelValue(voxel_index)); - - size_t visited_leaves = 0u; - map.forEachVoxelLeaf( - [&visited_leaves, &voxel_index](const wavemap::OctreeIndex& leaf_index, - const LayeredVoxel& voxel) { - ++visited_leaves; - if (leaf_index.height == 0 && leaf_index.position == voxel_index) { - printVoxel("Visited target leaf", voxel); - } - }); - - std::cout << "Visited leaves: " << visited_leaves << "\n"; - std::cout << "Occupancy view: " << map.getCellValue(voxel_index) << "\n"; - std::cout << "Block count: " << map.getHashMap().size() << "\n"; - - return 0; -} diff --git a/examples/cpp/digital_twin/map_api/benchmarks/continuous_voxel_many_voxels_benchmark.cc b/examples/cpp/digital_twin/map_api/benchmarks/continuous_voxel_many_voxels_benchmark.cc deleted file mode 100644 index b6c21db70..000000000 --- a/examples/cpp/digital_twin/map_api/benchmarks/continuous_voxel_many_voxels_benchmark.cc +++ /dev/null @@ -1,98 +0,0 @@ -#include -#include -#include -#include -#include - -#include "../../common/layered_voxel_config.h" - -LayeredVoxel makeVoxel(int x, int y, int z) { - const auto occupancy = - static_cast(1 + ((3 * x + 5 * y + 7 * z) % 11)); - const float traversability = static_cast(x + y + z) / 18.f; - const float r = static_cast(x) / 7.f; - const float g = static_cast(y) / 7.f; - const float b = static_cast(z) / 3.f; - return makeLayeredVoxel(occupancy, rgb(r, g, b), traversability); -} - -float absError(const LayeredVoxel& read, const LayeredVoxel& expected) { - return std::abs(read.occupancy - expected.occupancy) + - std::abs(read.data.traversability - expected.data.traversability) + - std::abs(read.data.rgb.r - expected.data.rgb.r) + - std::abs(read.data.rgb.g - expected.data.rgb.g) + - std::abs(read.data.rgb.b - expected.data.rgb.b); -} - -int main() { - wavemap::HashedWaveletOctreeConfig config; - config.min_cell_width = 0.1f; - config.min_log_odds = -100.f; - config.max_log_odds = 100.f; - config.tree_height = 4; - - std::vector> samples; - samples.reserve(8 * 8 * 4); - for (int x = 0; x < 8; ++x) { - for (int y = 0; y < 8; ++y) { - for (int z = 0; z < 4; ++z) { - samples.emplace_back(wavemap::Index3D(x, y, z), makeVoxel(x, y, z)); - } - } - } - - ContinuousWaveletMap map(config); - - const auto write_start = std::chrono::steady_clock::now(); - for (const auto& [index, voxel] : samples) { - map.setVoxelValue(index, voxel); - } - const auto write_end = std::chrono::steady_clock::now(); - - float abs_error_sum = 0.f; - float max_abs_error = 0.f; - - const auto read_start = std::chrono::steady_clock::now(); - for (const auto& [index, expected] : samples) { - const LayeredVoxel read = map.getVoxelValue(index); - const float error = absError(read, expected); - abs_error_sum += error; - max_abs_error = std::max(max_abs_error, error); - } - const auto read_end = std::chrono::steady_clock::now(); - - const auto write_us = - std::chrono::duration_cast(write_end - - write_start) - .count(); - const auto read_us = - std::chrono::duration_cast(read_end - - read_start) - .count(); - - size_t non_zero_leaves = 0u; - map.forEachVoxelLeaf( - [&non_zero_leaves](const wavemap::OctreeIndex& /*index*/, - const LayeredVoxel& voxel) { - const bool non_zero = std::abs(voxel.occupancy) > 1e-5f || - std::abs(voxel.data.traversability) > 1e-5f || - std::abs(voxel.data.rgb.r) > 1e-5f || - std::abs(voxel.data.rgb.g) > 1e-5f || - std::abs(voxel.data.rgb.b) > 1e-5f; - if (non_zero) { - ++non_zero_leaves; - } - }); - - std::cout << "samples: " << samples.size() << "\n"; - std::cout << "combined abs error sum: " << abs_error_sum << "\n"; - std::cout << "max combined abs error: " << max_abs_error << "\n"; - std::cout << "non-zero leaves: " << non_zero_leaves << "\n"; - std::cout << "block count: " << map.getHashMap().size() << "\n"; - std::cout << "node count: " << map.size() << "\n"; - std::cout << "memory usage bytes: " << map.getMemoryUsage() << "\n"; - std::cout << "write time [us]: " << write_us << "\n"; - std::cout << "read time [us]: " << read_us << "\n"; - - return 0; -} diff --git a/examples/cpp/digital_twin/map_api/benchmarks/float_cast_discrete_many_voxels_experiment.cc b/examples/cpp/digital_twin/map_api/benchmarks/float_cast_discrete_many_voxels_experiment.cc deleted file mode 100644 index 1f966e80e..000000000 --- a/examples/cpp/digital_twin/map_api/benchmarks/float_cast_discrete_many_voxels_experiment.cc +++ /dev/null @@ -1,168 +0,0 @@ -#include -#include -#include -#include -#include - -#include "wavemap/core/indexing/ndtree_index.h" -#include "wavemap/core/map/cell_types/voxel_data.h" -#include "wavemap/core/map/hashed_wavelet_octree.h" - -struct CastDiscreteData { - float label = 0.f; - float dynamic = 0.f; - float code = 0.f; - - bool operator==(const CastDiscreteData& other) const { - return label == other.label && dynamic == other.dynamic && - code == other.code; - } -}; - -struct CastDiscreteDataPolicy { - static CastDiscreteData add(const CastDiscreteData& lhs, - const CastDiscreteData& rhs) { - return {lhs.label + rhs.label, lhs.dynamic + rhs.dynamic, - lhs.code + rhs.code}; - } - - static CastDiscreteData subtract(const CastDiscreteData& lhs, - const CastDiscreteData& rhs) { - return {lhs.label - rhs.label, lhs.dynamic - rhs.dynamic, - lhs.code - rhs.code}; - } - - static CastDiscreteData scale(const CastDiscreteData& data, - wavemap::FloatingPoint factor) { - return {factor * data.label, factor * data.dynamic, factor * data.code}; - } -}; - -struct RawDiscreteData { - int label = 0; - bool dynamic = false; - char code = '\0'; - - bool operator==(const RawDiscreteData& other) const { - return label == other.label && dynamic == other.dynamic && - code == other.code; - } -}; - -using CastVoxel = wavemap::VoxelData; -using CastMap = wavemap::HashedWaveletOctreeT; - -CastVoxel toVoxel(wavemap::FloatingPoint occupancy, - const RawDiscreteData& data) { - return CastVoxel( - occupancy, - {static_cast(data.label), data.dynamic ? 1.f : 0.f, - static_cast(static_cast(data.code))}); -} - -RawDiscreteData fromVoxelRounded(const CastVoxel& voxel) { - return {static_cast(std::lround(voxel.data.label)), - std::lround(voxel.data.dynamic) != 0, - static_cast(std::lround(voxel.data.code))}; -} - -RawDiscreteData makeData(int x, int y, int z) { - return {1 + ((3 * x + 5 * y + 7 * z) % 11), - ((x + y + z) % 2) == 0, - static_cast('A' + ((x + 2 * y + 3 * z) % 26))}; -} - -void printMismatch(const wavemap::Index3D& index, - const RawDiscreteData& expected, - const CastVoxel& read_voxel, - const RawDiscreteData& rounded) { - std::cout << "mismatch at " << index.transpose() << " expected=(label=" - << expected.label << ", dynamic=" << std::boolalpha - << expected.dynamic << ", code=" << expected.code - << ") read_f=(label=" << read_voxel.data.label - << ", dynamic=" << read_voxel.data.dynamic - << ", code=" << read_voxel.data.code << ") rounded=(label=" - << rounded.label << ", dynamic=" << rounded.dynamic - << ", code=" << rounded.code << ")\n"; -} - -int main() { - wavemap::HashedWaveletOctreeConfig config; - config.min_cell_width = 0.1f; - config.min_log_odds = -100.f; - config.max_log_odds = 100.f; - config.tree_height = 4; - - CastMap map(config); - - std::vector> samples; - samples.reserve(8 * 8 * 4); - for (int x = 0; x < 8; ++x) { - for (int y = 0; y < 8; ++y) { - for (int z = 0; z < 4; ++z) { - samples.emplace_back(wavemap::Index3D(x, y, z), makeData(x, y, z)); - } - } - } - - for (const auto& [index, data] : samples) { - const wavemap::FloatingPoint occupancy = - static_cast(data.label); - map.setVoxelValue(index, toVoxel(occupancy, data)); - } - - size_t preserved = 0u; - size_t label_errors = 0u; - size_t dynamic_errors = 0u; - size_t code_errors = 0u; - constexpr size_t kMaxPrintedMismatches = 12u; - size_t printed_mismatches = 0u; - - for (const auto& [index, expected] : samples) { - const CastVoxel read_voxel = map.getVoxelValue(index); - const RawDiscreteData rounded = fromVoxelRounded(read_voxel); - if (rounded == expected) { - ++preserved; - continue; - } - - if (rounded.label != expected.label) { - ++label_errors; - } - if (rounded.dynamic != expected.dynamic) { - ++dynamic_errors; - } - if (rounded.code != expected.code) { - ++code_errors; - } - if (printed_mismatches < kMaxPrintedMismatches) { - printMismatch(index, expected, read_voxel, rounded); - ++printed_mismatches; - } - } - - size_t interesting_leaves = 0u; - map.forEachVoxelLeaf( - [&interesting_leaves](const wavemap::OctreeIndex& /*index*/, - const CastVoxel& voxel) { - const bool interesting = std::abs(voxel.occupancy) > 1e-5f || - std::abs(voxel.data.label) > 1e-5f || - std::abs(voxel.data.dynamic) > 1e-5f || - std::abs(voxel.data.code) > 1e-5f; - if (interesting) { - ++interesting_leaves; - } - }); - - std::cout << "samples: " << samples.size() << "\n"; - std::cout << "preserved after rounding: " << preserved << "/" - << samples.size() << "\n"; - std::cout << "label errors: " << label_errors << "\n"; - std::cout << "dynamic errors: " << dynamic_errors << "\n"; - std::cout << "code errors: " << code_errors << "\n"; - std::cout << "interesting leaves: " << interesting_leaves << "\n"; - std::cout << "block count: " << map.getHashMap().size() << "\n"; - std::cout << "node count: " << map.size() << "\n"; - - return preserved == samples.size() ? 0 : 1; -} diff --git a/examples/cpp/digital_twin/map_api/benchmarks/layered_pruning_policy_experiment.cc b/examples/cpp/digital_twin/map_api/benchmarks/layered_pruning_policy_experiment.cc deleted file mode 100644 index c341e3a03..000000000 --- a/examples/cpp/digital_twin/map_api/benchmarks/layered_pruning_policy_experiment.cc +++ /dev/null @@ -1,373 +0,0 @@ -#include -#include -#include -#include -#include -#include - -#include "../../common/layered_voxel_config.h" - -struct Sample { - wavemap::Index3D index; - LayeredVoxel expected; -}; - -struct ErrorStats { - float occupancy_sum = 0.f; - float rgb_sum = 0.f; - float traversability_sum = 0.f; - float max_occupancy = 0.f; - float max_rgb = 0.f; - float max_traversability = 0.f; -}; - -struct PruningRunStats { - std::string label; - std::string dataset; - size_t nodes_before = 0u; - size_t nodes_after = 0u; - size_t memory_before = 0u; - size_t memory_after = 0u; - long long write_us = 0; - long long read_before_us = 0; - long long threshold_prune_us = 0; - ErrorStats before_expected; - ErrorStats after_expected; - ErrorStats prune_delta; -}; - -LayeredVoxel makeLargeExpectedVoxel(int x, int y, int z) { - const wavemap::FloatingPoint occupancy = 1.f; - const Rgb color = rgb(static_cast(x) / 49.f, - static_cast(y) / 49.f, - static_cast(z) / 39.f); - const float traversability = - static_cast((x + 2 * y + 3 * z) % 97) / 96.f; - return makeLayeredVoxel(occupancy, color, traversability); -} - -std::vector makeLargeSamples() { - constexpr int kSizeX = 50; - constexpr int kSizeY = 50; - constexpr int kSizeZ = 40; - - std::vector samples; - samples.reserve(kSizeX * kSizeY * kSizeZ); - for (int x = 0; x < kSizeX; ++x) { - for (int y = 0; y < kSizeY; ++y) { - for (int z = 0; z < kSizeZ; ++z) { - const wavemap::Index3D index(x, y, z); - samples.push_back({index, makeLargeExpectedVoxel(x, y, z)}); - } - } - } - return samples; -} - -LayeredVoxel makeLayerBoundaryExpectedVoxel(int x, int y, int z) { - const wavemap::FloatingPoint occupancy = 0.5f; - - Rgb color = rgb(0.2f, 0.2f, 0.2f); - if (16 <= x) { - color = rgb(0.9f, 0.1f, 0.1f); - } - if (16 <= y) { - color = rgb(0.1f, 0.1f, 0.9f); - } - - float traversability = 0.25f; - if (8 <= z && z < 24) { - traversability = 0.85f; - } - - return makeLayeredVoxel(occupancy, color, traversability); -} - -std::vector makeLayerBoundarySamples() { - constexpr int kSize = 32; - - std::vector samples; - samples.reserve(kSize * kSize * kSize); - for (int x = 0; x < kSize; ++x) { - for (int y = 0; y < kSize; ++y) { - for (int z = 0; z < kSize; ++z) { - const wavemap::Index3D index(x, y, z); - samples.push_back({index, makeLayerBoundaryExpectedVoxel(x, y, z)}); - } - } - } - return samples; -} - -float rgbError(const LayeredVoxel& read, const LayeredVoxel& expected) { - return sumAbsDiff(read.data.rgb, expected.data.rgb); -} - -float traversabilityError(const LayeredVoxel& read, - const LayeredVoxel& expected) { - return std::abs(read.data.traversability - expected.data.traversability); -} - -float occupancyError(const LayeredVoxel& read, const LayeredVoxel& expected) { - return std::abs(read.occupancy - expected.occupancy); -} - -ErrorStats computeErrorsAgainstExpected(const ContinuousWaveletMap& map, - const std::vector& samples) { - ErrorStats stats; - for (const auto& sample : samples) { - const LayeredVoxel read = map.getVoxelValue(sample.index); - const float occ_error = occupancyError(read, sample.expected); - const float color_error = rgbError(read, sample.expected); - const float trav_error = traversabilityError(read, sample.expected); - stats.occupancy_sum += occ_error; - stats.rgb_sum += color_error; - stats.traversability_sum += trav_error; - stats.max_occupancy = std::max(stats.max_occupancy, occ_error); - stats.max_rgb = std::max(stats.max_rgb, color_error); - stats.max_traversability = std::max(stats.max_traversability, trav_error); - } - return stats; -} - -ErrorStats computeErrorsAgainstBaseline( - const ContinuousWaveletMap& map, const std::vector& samples, - const std::vector& baseline) { - ErrorStats stats; - for (size_t sample_idx = 0; sample_idx < samples.size(); ++sample_idx) { - const LayeredVoxel read = map.getVoxelValue(samples[sample_idx].index); - const LayeredVoxel& expected = baseline[sample_idx]; - const float occ_error = occupancyError(read, expected); - const float color_error = rgbError(read, expected); - const float trav_error = traversabilityError(read, expected); - stats.occupancy_sum += occ_error; - stats.rgb_sum += color_error; - stats.traversability_sum += trav_error; - stats.max_occupancy = std::max(stats.max_occupancy, occ_error); - stats.max_rgb = std::max(stats.max_rgb, color_error); - stats.max_traversability = std::max(stats.max_traversability, trav_error); - } - return stats; -} - -std::vector readBackAll(const ContinuousWaveletMap& map, - const std::vector& samples) { - std::vector values; - values.reserve(samples.size()); - for (const auto& sample : samples) { - values.push_back(map.getVoxelValue(sample.index)); - } - return values; -} - -bool detailsNonzero(const LayeredBlock::Coefficients::Details& details, - const ContinuousWaveletMap::PruningConfig& config) { - return std::any_of(details.cbegin(), details.cend(), - [&config](const LayeredVoxel& voxel) { - return wavemap::CellDataTraits::isNonzero( - voxel, config); - }); -} - -float detailScore(const LayeredVoxel& voxel, - const ContinuousWaveletMap::PruningConfig& config) { - return wavemap::CellDataTraits::pruningScore(voxel, config); -} - -void printCoefficientPolicyCheck() { - LayeredBlock::Coefficients::Details zero_details{}; - LayeredBlock::Coefficients::Details occupancy_details{}; - LayeredBlock::Coefficients::Details color_details{}; - LayeredBlock::Coefficients::Details combined_small_details{}; - - occupancy_details[0] = makeLayeredVoxel(0.25f, rgb(0.f, 0.f, 0.f), 0.f); - color_details[0] = makeLayeredVoxel(0.f, rgb(1.f, 0.f, 0.f), 0.5f); - combined_small_details[0] = - makeLayeredVoxel(0.0004f, rgb(0.0004f, 0.f, 0.f), 0.0004f); - - const auto occupancy_only = makeLayeredPruningConfig(1.f, 0.f, 0.f); - const auto equal_layers = makeLayeredPruningConfig(1.f, 1.f, 1.f); - const auto traversability_heavy = makeLayeredPruningConfig(1.f, 1.f, 2.f); - - const auto print_case = [](const char* label, - const LayeredBlock::Coefficients::Details& details, - const ContinuousWaveletMap::PruningConfig& config) { - std::cout << " " << label << ": nonzero=" - << detailsNonzero(details, config) - << " score[0]=" << detailScore(details[0], config) << "\n"; - }; - - std::cout << "Direct coefficient pruning check\n"; - std::cout << " occupancy-only config\n"; - print_case("zero", zero_details, occupancy_only); - print_case("occupancy detail", occupancy_details, occupancy_only); - print_case("color/traversability detail", color_details, occupancy_only); - print_case("combined small details", combined_small_details, - occupancy_only); - - std::cout << " equal-layer config\n"; - print_case("zero", zero_details, equal_layers); - print_case("occupancy detail", occupancy_details, equal_layers); - print_case("color/traversability detail", color_details, equal_layers); - print_case("combined small details", combined_small_details, equal_layers); - - std::cout << " traversability-heavy config\n"; - print_case("zero", zero_details, traversability_heavy); - print_case("occupancy detail", occupancy_details, traversability_heavy); - print_case("color/traversability detail", color_details, - traversability_heavy); - print_case("combined small details", combined_small_details, - traversability_heavy); -} - -void printStats(const char* label, const ErrorStats& stats, - size_t sample_count) { - std::cout << label << "\n"; - std::cout << " occupancy error sum: " << stats.occupancy_sum << "\n"; - std::cout << " occupancy mean abs error: " - << stats.occupancy_sum / static_cast(sample_count) << "\n"; - std::cout << " max occupancy error: " << stats.max_occupancy << "\n"; - std::cout << " rgb error sum: " << stats.rgb_sum << "\n"; - std::cout << " rgb mean abs error: " - << stats.rgb_sum / static_cast(sample_count) << "\n"; - std::cout << " max rgb error: " << stats.max_rgb << "\n"; - std::cout << " traversability error sum: " << stats.traversability_sum - << "\n"; - std::cout << " traversability mean abs error: " - << stats.traversability_sum / static_cast(sample_count) - << "\n"; - std::cout << " max traversability error: " << stats.max_traversability - << "\n"; -} - -PruningRunStats runPruningExperiment( - const std::string& dataset, const std::string& label, - const wavemap::HashedWaveletOctreeConfig& config, - const ContinuousWaveletMap::PruningConfig& pruning_config, - const std::vector& samples) { - PruningRunStats stats; - stats.dataset = dataset; - stats.label = label; - - ContinuousWaveletMap map(config, ContinuousWaveletMap::ThresholdConfig{}, pruning_config); - - const auto write_start = std::chrono::steady_clock::now(); - for (const auto& sample : samples) { - map.setVoxelValue(sample.index, sample.expected); - } - const auto write_end = std::chrono::steady_clock::now(); - - const auto read_before_start = std::chrono::steady_clock::now(); - const std::vector before_prune_values = - readBackAll(map, samples); - const auto read_before_end = std::chrono::steady_clock::now(); - - stats.before_expected = computeErrorsAgainstExpected(map, samples); - stats.nodes_before = map.size(); - stats.memory_before = map.getMemoryUsage(); - - const auto prune_start = std::chrono::steady_clock::now(); - map.threshold(); - map.prune(); - const auto prune_end = std::chrono::steady_clock::now(); - - stats.after_expected = computeErrorsAgainstExpected(map, samples); - stats.prune_delta = - computeErrorsAgainstBaseline(map, samples, before_prune_values); - stats.nodes_after = map.size(); - stats.memory_after = map.getMemoryUsage(); - - stats.write_us = std::chrono::duration_cast( - write_end - write_start) - .count(); - stats.read_before_us = - std::chrono::duration_cast(read_before_end - - read_before_start) - .count(); - stats.threshold_prune_us = - std::chrono::duration_cast(prune_end - - prune_start) - .count(); - return stats; -} - -void printRunStats(const PruningRunStats& stats, size_t sample_count) { - std::cout << "Pruning loss test [" << stats.dataset << "] (" << stats.label - << ")\n"; - std::cout << " samples: " << sample_count << "\n"; - std::cout << " nodes before prune: " << stats.nodes_before << "\n"; - std::cout << " nodes after prune: " << stats.nodes_after << "\n"; - std::cout << " memory before prune bytes: " << stats.memory_before << "\n"; - std::cout << " memory after prune bytes: " << stats.memory_after << "\n"; - std::cout << " write time [us]: " << stats.write_us << "\n"; - std::cout << " read-before-threshold time [us]: " << stats.read_before_us - << "\n"; - std::cout << " threshold+prune time [us]: " << stats.threshold_prune_us - << "\n"; - - printStats("Error vs original values before prune", stats.before_expected, - sample_count); - printStats("Error vs original values after prune", stats.after_expected, - sample_count); - printStats("Extra error introduced by threshold+prune", stats.prune_delta, - sample_count); -} - -int main() { - printCoefficientPolicyCheck(); - std::cout << "\n"; - - wavemap::HashedWaveletOctreeConfig config; - config.min_cell_width = 0.1f; - config.min_log_odds = -100.f; - config.max_log_odds = 100.f; - config.tree_height = 4; - - const std::vector samples = makeLargeSamples(); - const std::vector layer_boundary_samples = - makeLayerBoundarySamples(); - const std::vector> configs{ - {"occupancy-only", makeLayeredPruningConfig(1.f, 0.f, 0.f)}, - {"equal weighted layers", makeLayeredPruningConfig(1.f, 1.f, 1.f)}, - {"traversability-heavy", makeLayeredPruningConfig(1.f, 1.f, 2.f)}}; - const std::vector> - coarse_occupancy_configs{ - {"coarse occupancy-only", - makeLayeredPruningConfigWithScales(1.f, 0.f, 0.f, 1.f, 0.1f, 0.1f)}, - {"coarse occupancy + equal layers", - makeLayeredPruningConfigWithScales(1.f, 1.f, 1.f, 1.f, 0.1f, 0.1f)}, - {"coarse occupancy + traversability-heavy", - makeLayeredPruningConfigWithScales(4.f, 1.f, 1.f, 1.f, 0.1f, 0.1f)}}; - - for (const auto& [label, pruning_config] : configs) { - const PruningRunStats stats = - runPruningExperiment("smooth-gradient", label, config, pruning_config, - samples); - printRunStats(stats, samples.size()); - std::cout << "\n"; - } - - for (const auto& [label, pruning_config] : configs) { - const PruningRunStats stats = runPruningExperiment( - "layer-boundary/default-scale", label, config, pruning_config, - layer_boundary_samples); - printRunStats(stats, layer_boundary_samples.size()); - std::cout << "\n"; - } - - for (const auto& [label, pruning_config] : coarse_occupancy_configs) { - const PruningRunStats stats = runPruningExperiment( - "layer-boundary/coarse-occupancy", label, config, pruning_config, - layer_boundary_samples); - printRunStats(stats, layer_boundary_samples.size()); - std::cout << "\n"; - } - - std::cout << "Interpretation: score = sum(weight * magnitude / scale). " - "A node is kept when the combined score is above the combined " - "threshold. Setting a layer weight to 0 ignores that layer; " - "increasing a weight makes that layer contribute more to the " - "combined pruning decision.\n"; - - return 0; -} diff --git a/examples/cpp/digital_twin/map_api/benchmarks/occupancy_many_voxels_benchmark.cc b/examples/cpp/digital_twin/map_api/benchmarks/occupancy_many_voxels_benchmark.cc deleted file mode 100644 index beb404414..000000000 --- a/examples/cpp/digital_twin/map_api/benchmarks/occupancy_many_voxels_benchmark.cc +++ /dev/null @@ -1,79 +0,0 @@ -#include -#include -#include -#include - -#include "wavemap/core/indexing/ndtree_index.h" -#include "wavemap/core/map/hashed_wavelet_octree.h" - -wavemap::FloatingPoint makeOccupancy(int x, int y, int z) { - return static_cast(1 + ((3 * x + 5 * y + 7 * z) % 11)); -} - -int main() { - wavemap::HashedWaveletOctreeConfig config; - config.min_cell_width = 0.1f; - config.min_log_odds = -100.f; - config.max_log_odds = 100.f; - config.tree_height = 4; - - std::vector> samples; - samples.reserve(8 * 8 * 4); - for (int x = 0; x < 8; ++x) { - for (int y = 0; y < 8; ++y) { - for (int z = 0; z < 4; ++z) { - samples.emplace_back(wavemap::Index3D(x, y, z), makeOccupancy(x, y, z)); - } - } - } - - wavemap::HashedWaveletOctree map(config); - - const auto write_start = std::chrono::steady_clock::now(); - for (const auto& [index, occupancy] : samples) { - map.setCellValue(index, occupancy); - } - const auto write_end = std::chrono::steady_clock::now(); - - wavemap::FloatingPoint abs_error_sum = 0.f; - wavemap::FloatingPoint max_abs_error = 0.f; - - const auto read_start = std::chrono::steady_clock::now(); - for (const auto& [index, expected] : samples) { - const wavemap::FloatingPoint read = map.getCellValue(index); - const wavemap::FloatingPoint abs_error = std::abs(read - expected); - abs_error_sum += abs_error; - max_abs_error = std::max(max_abs_error, abs_error); - } - const auto read_end = std::chrono::steady_clock::now(); - - const auto write_us = - std::chrono::duration_cast(write_end - - write_start) - .count(); - const auto read_us = - std::chrono::duration_cast(read_end - - read_start) - .count(); - - size_t non_zero_leaves = 0u; - map.forEachVoxelLeaf( - [&non_zero_leaves](const wavemap::OctreeIndex& /*index*/, - wavemap::FloatingPoint value) { - if (std::abs(value) > 1e-5f) { - ++non_zero_leaves; - } - }); - - std::cout << "samples: " << samples.size() << "\n"; - std::cout << "abs error sum: " << abs_error_sum << "\n"; - std::cout << "max abs error: " << max_abs_error << "\n"; - std::cout << "non-zero leaves: " << non_zero_leaves << "\n"; - std::cout << "block count: " << map.getHashMap().size() << "\n"; - std::cout << "node count: " << map.size() << "\n"; - std::cout << "memory usage bytes: " << map.getMemoryUsage() << "\n"; - std::cout << "write time [us]: " << write_us << "\n"; - std::cout << "read time [us]: " << read_us << "\n"; - - return 0; -} diff --git a/examples/cpp/digital_twin/map_api/benchmarks/raw_discrete_many_voxels_experiment.cc b/examples/cpp/digital_twin/map_api/benchmarks/raw_discrete_many_voxels_experiment.cc deleted file mode 100644 index c0d852be8..000000000 --- a/examples/cpp/digital_twin/map_api/benchmarks/raw_discrete_many_voxels_experiment.cc +++ /dev/null @@ -1,108 +0,0 @@ -#include -#include -#include -#include -#include - -#include "wavemap/core/indexing/ndtree_index.h" - -struct RawDiscreteData { - int label = 0; - bool dynamic = false; - char code = '\0'; - - bool operator==(const RawDiscreteData& other) const { - return label == other.label && dynamic == other.dynamic && - code == other.code; - } -}; - -struct IndexKey { - int x = 0; - int y = 0; - int z = 0; - - explicit IndexKey(const wavemap::Index3D& index) - : x(index.x()), y(index.y()), z(index.z()) {} - - bool operator<(const IndexKey& other) const { - return std::tie(x, y, z) < std::tie(other.x, other.y, other.z); - } -}; - -using RawStorage = std::map; - -RawDiscreteData makeData(int x, int y, int z) { - return {1 + ((3 * x + 5 * y + 7 * z) % 11), - ((x + y + z) % 2) == 0, - static_cast('A' + ((x + 2 * y + 3 * z) % 26))}; -} - -void printMismatch(const wavemap::Index3D& index, - const RawDiscreteData& expected, - const RawDiscreteData& read) { - std::cout << "mismatch at " << index.transpose() << " expected=(label=" - << expected.label << ", dynamic=" << std::boolalpha - << expected.dynamic << ", code=" << expected.code - << ") read=(label=" << read.label << ", dynamic=" << read.dynamic - << ", code=" << read.code << ")\n"; -} - -int main() { - std::vector> samples; - samples.reserve(8 * 8 * 4); - for (int x = 0; x < 8; ++x) { - for (int y = 0; y < 8; ++y) { - for (int z = 0; z < 4; ++z) { - samples.emplace_back(wavemap::Index3D(x, y, z), makeData(x, y, z)); - } - } - } - - RawStorage storage; - - const auto write_start = std::chrono::steady_clock::now(); - for (const auto& [index, data] : samples) { - storage[IndexKey(index)] = data; - } - const auto write_end = std::chrono::steady_clock::now(); - - size_t preserved = 0u; - size_t printed_mismatches = 0u; - constexpr size_t kMaxPrintedMismatches = 12u; - - const auto read_start = std::chrono::steady_clock::now(); - for (const auto& [index, expected] : samples) { - const RawDiscreteData read = storage.at(IndexKey(index)); - if (read == expected) { - ++preserved; - } else if (printed_mismatches < kMaxPrintedMismatches) { - printMismatch(index, expected, read); - ++printed_mismatches; - } - } - const auto read_end = std::chrono::steady_clock::now(); - - const auto write_us = - std::chrono::duration_cast(write_end - - write_start) - .count(); - const auto read_us = - std::chrono::duration_cast(read_end - - read_start) - .count(); - - const size_t approximate_payload_bytes = - storage.size() * (sizeof(IndexKey) + sizeof(RawDiscreteData)); - - std::cout << "samples: " << samples.size() << "\n"; - std::cout << "entries: " << storage.size() << "\n"; - std::cout << "preserved values: " << preserved << "/" << samples.size() - << "\n"; - std::cout << "approx payload bytes without map overhead: " - << approximate_payload_bytes << "\n"; - std::cout << "write time [us]: " << write_us << "\n"; - std::cout << "read time [us]: " << read_us << "\n"; - - return preserved == samples.size() ? 0 : 1; -} diff --git a/examples/cpp/digital_twin/map_api/benchmarks/raw_inside_map_many_voxels_experiment.cc b/examples/cpp/digital_twin/map_api/benchmarks/raw_inside_map_many_voxels_experiment.cc deleted file mode 100644 index dadce57c8..000000000 --- a/examples/cpp/digital_twin/map_api/benchmarks/raw_inside_map_many_voxels_experiment.cc +++ /dev/null @@ -1,158 +0,0 @@ -#include -#include -#include -#include -#include -#include - -#include "wavemap/core/indexing/ndtree_index.h" -#include "wavemap/core/map/hashed_wavelet_octree.h" - -struct RawDiscreteData { - int label = 0; - bool dynamic = false; - char code = '\0'; - - bool operator==(const RawDiscreteData& other) const { - return label == other.label && dynamic == other.dynamic && - code == other.code; - } -}; - -struct IndexKey { - int x = 0; - int y = 0; - int z = 0; - - explicit IndexKey(const wavemap::Index3D& index) - : x(index.x()), y(index.y()), z(index.z()) {} - - bool operator<(const IndexKey& other) const { - return std::tie(x, y, z) < std::tie(other.x, other.y, other.z); - } -}; - -class RawDiscreteLayeredMap { - public: - explicit RawDiscreteLayeredMap( - const wavemap::HashedWaveletOctreeConfig& config) - : occupancy_map_(config) {} - - void setOccupancy(const wavemap::Index3D& index, - wavemap::FloatingPoint occupancy) { - occupancy_map_.setCellValue(index, occupancy); - } - - wavemap::FloatingPoint getOccupancy(const wavemap::Index3D& index) const { - return occupancy_map_.getCellValue(index); - } - - void setRawData(const wavemap::Index3D& index, const RawDiscreteData& data) { - raw_data_[IndexKey(index)] = data; - } - - std::optional getRawData( - const wavemap::Index3D& index) const { - const auto it = raw_data_.find(IndexKey(index)); - if (it == raw_data_.end()) { - return std::nullopt; - } - return it->second; - } - - size_t rawDataSize() const { return raw_data_.size(); } - size_t occupancyBlockCount() const { return occupancy_map_.getHashMap().size(); } - size_t occupancyNodeCount() const { return occupancy_map_.size(); } - - size_t approximateRawBytesWithoutMapOverhead() const { - return raw_data_.size() * (sizeof(IndexKey) + sizeof(RawDiscreteData)); - } - - private: - wavemap::HashedWaveletOctree occupancy_map_; - std::map raw_data_; -}; - -RawDiscreteData makeData(int x, int y, int z) { - return {1 + ((3 * x + 5 * y + 7 * z) % 11), - ((x + y + z) % 2) == 0, - static_cast('A' + ((x + 2 * y + 3 * z) % 26))}; -} - -void printMismatch(const wavemap::Index3D& index, - const RawDiscreteData& expected, - const RawDiscreteData& read, - wavemap::FloatingPoint occupancy) { - std::cout << "mismatch at " << index.transpose() << " occ=" << occupancy - << " expected=(label=" << expected.label << ", dynamic=" - << std::boolalpha << expected.dynamic << ", code=" - << expected.code << ") read=(label=" << read.label - << ", dynamic=" << read.dynamic << ", code=" << read.code - << ")\n"; -} - -int main() { - wavemap::HashedWaveletOctreeConfig config; - config.min_cell_width = 0.1f; - config.min_log_odds = -100.f; - config.max_log_odds = 100.f; - config.tree_height = 4; - - std::vector> samples; - samples.reserve(8 * 8 * 4); - for (int x = 0; x < 8; ++x) { - for (int y = 0; y < 8; ++y) { - for (int z = 0; z < 4; ++z) { - samples.emplace_back(wavemap::Index3D(x, y, z), makeData(x, y, z)); - } - } - } - - RawDiscreteLayeredMap map(config); - - const auto write_start = std::chrono::steady_clock::now(); - for (const auto& [index, data] : samples) { - map.setOccupancy(index, static_cast(data.label)); - map.setRawData(index, data); - } - const auto write_end = std::chrono::steady_clock::now(); - - size_t preserved_raw = 0u; - size_t printed_mismatches = 0u; - constexpr size_t kMaxPrintedMismatches = 12u; - - const auto read_start = std::chrono::steady_clock::now(); - for (const auto& [index, expected] : samples) { - const wavemap::FloatingPoint occupancy = map.getOccupancy(index); - const auto read = map.getRawData(index); - if (read && *read == expected) { - ++preserved_raw; - } else if (read && printed_mismatches < kMaxPrintedMismatches) { - printMismatch(index, expected, *read, occupancy); - ++printed_mismatches; - } - } - const auto read_end = std::chrono::steady_clock::now(); - - const auto write_us = - std::chrono::duration_cast(write_end - - write_start) - .count(); - const auto read_us = - std::chrono::duration_cast(read_end - - read_start) - .count(); - - std::cout << "samples: " << samples.size() << "\n"; - std::cout << "raw entries: " << map.rawDataSize() << "\n"; - std::cout << "preserved raw values: " << preserved_raw << "/" - << samples.size() << "\n"; - std::cout << "occupancy blocks: " << map.occupancyBlockCount() << "\n"; - std::cout << "occupancy nodes: " << map.occupancyNodeCount() << "\n"; - std::cout << "approx raw bytes without map overhead: " - << map.approximateRawBytesWithoutMapOverhead() << "\n"; - std::cout << "write time [us]: " << write_us << "\n"; - std::cout << "read time [us]: " << read_us << "\n"; - - return preserved_raw == samples.size() ? 0 : 1; -} diff --git a/examples/cpp/digital_twin/map_api/continuous/continuous_voxel_map_experiment.cc b/examples/cpp/digital_twin/map_api/continuous/continuous_voxel_map_experiment.cc deleted file mode 100644 index ae779e911..000000000 --- a/examples/cpp/digital_twin/map_api/continuous/continuous_voxel_map_experiment.cc +++ /dev/null @@ -1,65 +0,0 @@ -#include -#include - -#include "../../common/layered_voxel_config.h" - -int main() { - wavemap::HashedWaveletOctreeConfig config; - config.min_cell_width = 0.1f; - config.min_log_odds = -100.f; - config.max_log_odds = 100.f; - config.tree_height = 3; - - ContinuousWaveletMap map(config); - - const wavemap::Index3D a(1, 2, 3); - const wavemap::Index3D b(2, 2, 3); - const wavemap::Index3D c(3, 2, 3); - - map.setVoxelValue(a, - makeLayeredVoxel(10.f, rgb(1.f, 0.f, 0.f), - 0.1f)); - map.setVoxelValue(b, - makeLayeredVoxel(20.f, rgb(0.f, 1.f, 0.f), - 0.5f)); - map.setVoxelValue(c, - makeLayeredVoxel(30.f, rgb(0.f, 1.f, 1.f), - 0.9f)); - - std::cout << "Before threshold/prune\n"; - printVoxel("a", map.getVoxelValue(a)); - printVoxel("b", map.getVoxelValue(b)); - printVoxel("c", map.getVoxelValue(c)); - std::cout << "block count: " << map.getHashMap().size() << "\n"; - std::cout << "node count: " << map.size() << "\n"; - - size_t non_zero_leaves = 0u; - map.forEachVoxelLeaf( - [&non_zero_leaves](const wavemap::OctreeIndex& index, - const LayeredVoxel& voxel) { - const bool has_data = std::abs(voxel.occupancy) > 1e-5f || - std::abs(voxel.data.traversability) > 1e-5f || - std::abs(voxel.data.rgb.r) > 1e-5f || - std::abs(voxel.data.rgb.g) > 1e-5f || - std::abs(voxel.data.rgb.b) > 1e-5f; - if (has_data) { - ++non_zero_leaves; - std::cout << "leaf h=" << index.height << " pos=" - << index.position.transpose(); - printVoxel(" value", voxel); - } - }); - std::cout << "non-zero leaves: " << non_zero_leaves << "\n"; - - map.threshold(); - map.prune(); - - std::cout << "After threshold/prune\n"; - printVoxel("a", map.getVoxelValue(a)); - printVoxel("b", map.getVoxelValue(b)); - printVoxel("c", map.getVoxelValue(c)); - std::cout << "block count: " << map.getHashMap().size() << "\n"; - std::cout << "node count: " << map.size() << "\n"; - - return 0; -} diff --git a/examples/cpp/digital_twin/map_api/continuous/heterogeneous_voxel_map_experiment.cc b/examples/cpp/digital_twin/map_api/continuous/heterogeneous_voxel_map_experiment.cc deleted file mode 100644 index d9365659f..000000000 --- a/examples/cpp/digital_twin/map_api/continuous/heterogeneous_voxel_map_experiment.cc +++ /dev/null @@ -1,135 +0,0 @@ -#include -#include -#include - -#include "wavemap/core/indexing/ndtree_index.h" -#include "wavemap/core/map/cell_types/voxel_data.h" -#include "wavemap/core/map/hashed_wavelet_octree.h" - -struct HeterogeneousData { - float temperature = 0.f; - int label = 0; - bool dynamic = false; - std::string name; - - bool operator==(const HeterogeneousData& other) const { - return temperature == other.temperature && label == other.label && - dynamic == other.dynamic && name == other.name; - } -}; - -struct HeterogeneousDataPolicy { - static HeterogeneousData add(const HeterogeneousData& lhs, - const HeterogeneousData& rhs) { - return {lhs.temperature + rhs.temperature, lhs.label + rhs.label, - lhs.dynamic || rhs.dynamic, mergeName(lhs.name, rhs.name)}; - } - - static HeterogeneousData subtract(const HeterogeneousData& lhs, - const HeterogeneousData& rhs) { - return {lhs.temperature - rhs.temperature, lhs.label - rhs.label, - lhs.dynamic != rhs.dynamic, keepName(lhs.name, rhs.name)}; - } - - static HeterogeneousData scale(const HeterogeneousData& data, - wavemap::FloatingPoint factor) { - return {factor * data.temperature, static_cast(factor * data.label), - data.dynamic && std::abs(factor) > 1e-6f, data.name}; - } - - private: - static std::string mergeName(const std::string& lhs, - const std::string& rhs) { - if (!lhs.empty()) { - return lhs; - } - return rhs; - } - - static std::string keepName(const std::string& lhs, - const std::string& rhs) { - if (!lhs.empty()) { - return lhs; - } - return rhs; - } -}; - -using HeterogeneousVoxel = - wavemap::VoxelData; -using HeterogeneousMap = wavemap::HashedWaveletOctreeT; - -void printVoxel(const std::string& name, const HeterogeneousVoxel& voxel) { - std::cout << name << ": occ=" << voxel.occupancy - << " temperature=" << voxel.data.temperature - << " label=" << voxel.data.label - << " dynamic=" << std::boolalpha << voxel.data.dynamic - << " name=\"" << voxel.data.name << "\"\n"; -} - -int main() { - wavemap::HashedWaveletOctreeConfig config; - config.min_cell_width = 0.1f; - config.min_log_odds = -100.f; - config.max_log_odds = 100.f; - config.tree_height = 3; - - HeterogeneousMap map(config); - - const wavemap::Index3D a(1, 2, 3); - const wavemap::Index3D b(2, 2, 3); - const wavemap::Index3D c(3, 2, 3); - - const HeterogeneousVoxel voxel_a( - 10.f, HeterogeneousData{22.5f, 1, false, "wall"}); - const HeterogeneousVoxel voxel_b( - 20.f, HeterogeneousData{24.0f, 5, true, "chair"}); - const HeterogeneousVoxel voxel_c( - 30.f, HeterogeneousData{25.5f, 9, false, "table"}); - - map.setVoxelValue(a, voxel_a); - map.setVoxelValue(b, voxel_b); - map.setVoxelValue(c, voxel_c); - - std::cout << "Expected values\n"; - printVoxel("a expected", voxel_a); - printVoxel("b expected", voxel_b); - printVoxel("c expected", voxel_c); - - std::cout << "Read through wavemap/Haar\n"; - printVoxel("a read", map.getVoxelValue(a)); - printVoxel("b read", map.getVoxelValue(b)); - printVoxel("c read", map.getVoxelValue(c)); - std::cout << "block count: " << map.getHashMap().size() << "\n"; - std::cout << "node count: " << map.size() << "\n"; - - size_t interesting_leaves = 0u; - map.forEachVoxelLeaf( - [&interesting_leaves](const wavemap::OctreeIndex& index, - const HeterogeneousVoxel& voxel) { - const bool interesting = - std::abs(voxel.occupancy) > 1e-5f || - std::abs(voxel.data.temperature) > 1e-5f || - voxel.data.label != 0 || voxel.data.dynamic || - !voxel.data.name.empty(); - if (interesting) { - ++interesting_leaves; - std::cout << "leaf h=" << index.height << " pos=" - << index.position.transpose(); - printVoxel(" value", voxel); - } - }); - std::cout << "interesting leaves: " << interesting_leaves << "\n"; - - map.threshold(); - map.prune(); - - std::cout << "After threshold/prune\n"; - printVoxel("a read", map.getVoxelValue(a)); - printVoxel("b read", map.getVoxelValue(b)); - printVoxel("c read", map.getVoxelValue(c)); - std::cout << "block count: " << map.getHashMap().size() << "\n"; - std::cout << "node count: " << map.size() << "\n"; - - return 0; -} diff --git a/examples/cpp/digital_twin/map_api/continuous/layered_full_update_pruning_experiment.cc b/examples/cpp/digital_twin/map_api/continuous/layered_full_update_pruning_experiment.cc deleted file mode 100644 index 920e7034e..000000000 --- a/examples/cpp/digital_twin/map_api/continuous/layered_full_update_pruning_experiment.cc +++ /dev/null @@ -1,243 +0,0 @@ -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include - -#include "../../common/layered_voxel_config.h" -#include "../../common/layered_voxel_updater.h" - -namespace { -struct RunConfig { - std::string name; - LayeredPruningConfig pruning_config; -}; - -struct RunResult { - std::string name; - size_t nodes_before_prune = 0u; - size_t nodes_after_prune = 0u; - size_t memory_before_prune = 0u; - size_t memory_after_prune = 0u; - float mean_occupancy_delta_after_prune = 0.f; - float mean_rgb_delta_after_prune = 0.f; - float mean_traversability_delta_after_prune = 0.f; - float max_occupancy_delta_after_prune = 0.f; - float max_rgb_delta_after_prune = 0.f; - float max_traversability_delta_after_prune = 0.f; -}; - -ContinuousLayers makeLayerObservation(int x, int y, int z) { - Rgb color = rgb(0.2f, 0.2f, 0.2f); - if (13 <= x) { - color = rgb(0.9f, 0.1f, 0.1f); - } - if (21 <= y) { - color = rgb(0.1f, 0.1f, 0.9f); - } - - const float traversability = (7 <= z && z < 23) ? 0.85f : 0.25f; - return ContinuousLayers{color, traversability}; -} - -std::vector makeLayerObservations() { - constexpr int kSize = 32; - - std::vector observations; - observations.reserve(kSize * kSize * kSize); - for (int x = 0; x < kSize; ++x) { - for (int y = 0; y < kSize; ++y) { - for (int z = 0; z < kSize; ++z) { - observations.push_back( - {wavemap::Index3D(x, y, z), makeLayerObservation(x, y, z)}); - } - } - } - return observations; -} - -std::shared_ptr makeMapAfterRayAndLayerUpdates( - const wavemap::HashedWaveletOctreeConfig& map_config, - const LayeredPruningConfig& pruning_config, - const std::vector& observations) { - auto map = std::make_shared( - map_config, ContinuousWaveletMap::ThresholdConfig{}, pruning_config); - - auto projection_model = std::make_shared( - wavemap::SphericalProjectorConfig( - wavemap::CircularProjectorConfig(-1.2f, 1.2f, 32), - wavemap::CircularProjectorConfig(-1.2f, 1.2f, 32))); - auto posed_range_image = - std::make_shared>(projection_model->getDimensions()); - auto beam_offset_image = - std::make_shared>( - projection_model->getDimensions()); - - wavemap::ContinuousBeamConfig measurement_config; - measurement_config.angle_sigma = 0.01f; - measurement_config.range_sigma = 0.02f; - measurement_config.scaling_free = 0.2f; - measurement_config.scaling_occupied = 0.4f; - auto measurement_model = std::make_shared( - measurement_config, projection_model, posed_range_image, - beam_offset_image); - - wavemap::ProjectiveIntegratorConfig integrator_config; - integrator_config.min_range = 0.05f; - integrator_config.max_range = 4.f; - integrator_config.max_update_resolution = 0.f; - integrator_config.termination_update_error = 0.05f; - - wavemap::HashedWaveletIntegratorT integrator( - integrator_config, projection_model, posed_range_image, - beam_offset_image, measurement_model, map); - - wavemap::Pointcloud<> pointcloud; - pointcloud.resize(3u); - pointcloud[0] = wavemap::Point3D(1.0f, 0.0f, 0.0f); - pointcloud[1] = wavemap::Point3D(1.5f, 0.2f, 0.0f); - pointcloud[2] = wavemap::Point3D(2.0f, -0.2f, 0.1f); - const wavemap::Transformation3D T_W_C; - const wavemap::PosedPointcloud<> posed_pointcloud(T_W_C, pointcloud); - - integrator.integrate(posed_pointcloud); - map->threshold(); - - setContinuousLayers(*map, observations); - map->threshold(); - return map; -} - -std::vector readVoxels( - const ContinuousWaveletMap& map, - const std::vector& observations) { - std::vector values; - values.reserve(observations.size()); - for (const ContinuousLayerObservation& observation : observations) { - values.push_back(map.getVoxelValue(observation.index)); - } - return values; -} - -RunResult runFullPipelinePruning( - const wavemap::HashedWaveletOctreeConfig& map_config, - const RunConfig& run_config, - const std::vector& observations) { - auto map = makeMapAfterRayAndLayerUpdates( - map_config, run_config.pruning_config, observations); - - RunResult result; - result.name = run_config.name; - result.nodes_before_prune = map->size(); - result.memory_before_prune = map->getMemoryUsage(); - const std::vector before_prune_values = - readVoxels(*map, observations); - - map->prune(); - - result.nodes_after_prune = map->size(); - result.memory_after_prune = map->getMemoryUsage(); - - for (size_t i = 0; i < observations.size(); ++i) { - const LayeredVoxel after = map->getVoxelValue(observations[i].index); - const LayeredVoxel& before = before_prune_values[i]; - - const float occupancy_delta = std::abs(after.occupancy - before.occupancy); - const float rgb_delta = sumAbsDiff(after.data.rgb, before.data.rgb); - const float traversability_delta = - std::abs(after.data.traversability - before.data.traversability); - - result.mean_occupancy_delta_after_prune += occupancy_delta; - result.mean_rgb_delta_after_prune += rgb_delta; - result.mean_traversability_delta_after_prune += traversability_delta; - result.max_occupancy_delta_after_prune = - std::max(result.max_occupancy_delta_after_prune, occupancy_delta); - result.max_rgb_delta_after_prune = - std::max(result.max_rgb_delta_after_prune, rgb_delta); - result.max_traversability_delta_after_prune = - std::max(result.max_traversability_delta_after_prune, - traversability_delta); - } - - const float sample_count = static_cast(observations.size()); - result.mean_occupancy_delta_after_prune /= sample_count; - result.mean_rgb_delta_after_prune /= sample_count; - result.mean_traversability_delta_after_prune /= sample_count; - return result; -} - -void printRunResult(const RunResult& result) { - std::cout << result.name << "\n"; - std::cout << " nodes before prune: " << result.nodes_before_prune << "\n"; - std::cout << " nodes after prune: " << result.nodes_after_prune << "\n"; - std::cout << " memory before prune bytes: " << result.memory_before_prune - << "\n"; - std::cout << " memory after prune bytes: " << result.memory_after_prune - << "\n"; - std::cout << " mean occupancy delta after prune: " - << result.mean_occupancy_delta_after_prune << "\n"; - std::cout << " max occupancy delta after prune: " - << result.max_occupancy_delta_after_prune << "\n"; - std::cout << " mean rgb delta after prune: " - << result.mean_rgb_delta_after_prune << "\n"; - std::cout << " max rgb delta after prune: " - << result.max_rgb_delta_after_prune << "\n"; - std::cout << " mean traversability delta after prune: " - << result.mean_traversability_delta_after_prune << "\n"; - std::cout << " max traversability delta after prune: " - << result.max_traversability_delta_after_prune << "\n"; -} -} // namespace - -int main() { - wavemap::HashedWaveletOctreeConfig map_config; - map_config.min_cell_width = 0.1f; - map_config.min_log_odds = -2.f; - map_config.max_log_odds = 4.f; - map_config.tree_height = 4; - - const std::vector observations = - makeLayerObservations(); - - const std::vector run_configs{ - {"Full pipeline pruning: occupancy-only", - makeLayeredPruningConfigWithScales(1.f, 0.f, 0.f, 1.f, 1.f, 1.f)}, - {"Full pipeline pruning: equal layers", - makeLayeredPruningConfigWithScales(1.f, 1.f, 1.f, 1.f, 0.1f, 0.1f)}, - {"Full pipeline pruning: occupancy-heavy", - makeLayeredPruningConfigWithScales(4.f, 1.f, 1.f, 1.f, 0.75f, 0.75f)}}; - - std::cout << "Layered full update + pruning experiment\n"; - std::cout << "Occupancy is integrated with the original ray-based Wavemap " - "pipeline. Continuous layers are then set from per-voxel " - "observations and the map is pruned with different weighted " - "policies.\n"; - std::cout << "Per-voxel layer observations: " << observations.size() - << "\n\n"; - - bool saw_compression = false; - bool saw_layer_preservation = false; - for (const RunConfig& run_config : run_configs) { - const RunResult result = - runFullPipelinePruning(map_config, run_config, observations); - printRunResult(result); - std::cout << "\n"; - - saw_compression |= result.nodes_after_prune < result.nodes_before_prune; - saw_layer_preservation |= result.mean_rgb_delta_after_prune < 1e-4f && - result.mean_traversability_delta_after_prune < - 1e-4f; - } - - if (!saw_compression || !saw_layer_preservation) { - return 1; - } - return 0; -} diff --git a/examples/cpp/digital_twin/map_api/continuous/layered_hashed_wavelet_integrator_experiment.cc b/examples/cpp/digital_twin/map_api/continuous/layered_hashed_wavelet_integrator_experiment.cc deleted file mode 100644 index f260a0cdf..000000000 --- a/examples/cpp/digital_twin/map_api/continuous/layered_hashed_wavelet_integrator_experiment.cc +++ /dev/null @@ -1,117 +0,0 @@ -#include -#include -#include -#include - -#include -#include -#include -#include - -#include "../../common/layered_voxel_config.h" - -namespace { -bool approximatelyEqual(float lhs, float rhs, float tolerance = 1e-5f) { - return std::abs(lhs - rhs) <= tolerance; -} - -bool approximatelyEqual(const Rgb& lhs, const Rgb& rhs, - float tolerance = 1e-5f) { - return sumAbsDiff(lhs, rhs) <= tolerance; -} -} // namespace - -int main() { - wavemap::HashedWaveletOctreeConfig map_config; - map_config.min_cell_width = 0.1f; - map_config.min_log_odds = -2.f; - map_config.max_log_odds = 4.f; - map_config.tree_height = 4; - - auto map = std::make_shared(map_config); - - const wavemap::Index3D seeded_index(10, 0, 0); - const LayeredVoxel seeded_voxel = - makeLayeredVoxel(0.f, rgb(0.25f, 0.5f, 0.75f), 0.8f); - map->setVoxelValue(seeded_index, seeded_voxel); - - auto projection_model = std::make_shared( - wavemap::SphericalProjectorConfig( - wavemap::CircularProjectorConfig(-1.2f, 1.2f, 32), - wavemap::CircularProjectorConfig(-1.2f, 1.2f, 32))); - auto posed_range_image = - std::make_shared>(projection_model->getDimensions()); - auto beam_offset_image = - std::make_shared>( - projection_model->getDimensions()); - - wavemap::ContinuousBeamConfig measurement_config; - measurement_config.angle_sigma = 0.01f; - measurement_config.range_sigma = 0.02f; - measurement_config.scaling_free = 0.2f; - measurement_config.scaling_occupied = 0.4f; - auto measurement_model = std::make_shared( - measurement_config, projection_model, posed_range_image, - beam_offset_image); - - wavemap::ProjectiveIntegratorConfig integrator_config; - integrator_config.min_range = 0.05f; - integrator_config.max_range = 2.f; - integrator_config.max_update_resolution = 0.f; - integrator_config.termination_update_error = 0.05f; - - wavemap::HashedWaveletIntegratorT integrator( - integrator_config, projection_model, posed_range_image, - beam_offset_image, measurement_model, map); - - wavemap::Pointcloud<> pointcloud; - pointcloud.resize(1u); - pointcloud[0] = wavemap::Point3D(1.f, 0.f, 0.f); - const wavemap::Transformation3D T_W_C; - const wavemap::PosedPointcloud<> posed_pointcloud(T_W_C, pointcloud); - - const LayeredVoxel before = map->getVoxelValue(seeded_index); - const size_t size_before = map->size(); - - integrator.integrate(posed_pointcloud); - map->threshold(); - - const LayeredVoxel after = map->getVoxelValue(seeded_index); - const size_t size_after = map->size(); - - float max_abs_occupancy = 0.f; - map->forEachVoxelLeaf([&max_abs_occupancy](const wavemap::OctreeIndex& /*index*/, - const LayeredVoxel& voxel) { - max_abs_occupancy = std::max(max_abs_occupancy, std::abs(voxel.occupancy)); - }); - - std::cout << "Layered hashed wavelet integrator experiment\n"; - printVoxel("Layered voxel before integration", before); - printVoxel("Same layered voxel after integration", after); - - const bool occupancy_changed_somewhere = 1e-5f < max_abs_occupancy; - const bool map_allocated_integration_nodes = size_before < size_after; - const bool rgb_preserved = approximatelyEqual(before.data.rgb, after.data.rgb); - const bool traversability_preserved = - approximatelyEqual(before.data.traversability, - after.data.traversability); - - std::cout << "Map size before integration: " << size_before << "\n"; - std::cout << "Map size after integration: " << size_after << "\n"; - std::cout << "Max abs occupancy after integration: " - << max_abs_occupancy << "\n"; - std::cout << "Occupancy changed through templated hashed wavelet integrator: " - << (occupancy_changed_somewhere ? "yes" : "no") << "\n"; - std::cout << "Integrator allocated/updated nodes: " - << (map_allocated_integration_nodes ? "yes" : "no") << "\n"; - std::cout << "RGB preserved by occupancy-only update: " - << (rgb_preserved ? "yes" : "no") << "\n"; - std::cout << "Traversability preserved by occupancy-only update: " - << (traversability_preserved ? "yes" : "no") << "\n"; - - if (!occupancy_changed_somewhere || !rgb_preserved || - !traversability_preserved) { - return 1; - } - return 0; -} diff --git a/examples/cpp/digital_twin/map_api/continuous/layered_occupancy_update_path_experiment.cc b/examples/cpp/digital_twin/map_api/continuous/layered_occupancy_update_path_experiment.cc deleted file mode 100644 index 51d044a9d..000000000 --- a/examples/cpp/digital_twin/map_api/continuous/layered_occupancy_update_path_experiment.cc +++ /dev/null @@ -1,94 +0,0 @@ -#include -#include -#include -#include - -#include -#include -#include - -#include "../../common/layered_voxel_config.h" - -namespace { -bool approximatelyEqual(float lhs, float rhs, float tolerance = 1e-5f) { - return std::abs(lhs - rhs) <= tolerance; -} - -bool approximatelyEqual(const Rgb& lhs, const Rgb& rhs, - float tolerance = 1e-5f) { - return sumAbsDiff(lhs, rhs) <= tolerance; -} -} // namespace - -int main() { - wavemap::HashedWaveletOctreeConfig map_config; - map_config.min_cell_width = 0.1f; - map_config.min_log_odds = -2.f; - map_config.max_log_odds = 4.f; - map_config.tree_height = 4; - - auto map = std::make_shared(map_config); - - const wavemap::Index3D endpoint_index(10, 0, 0); - const LayeredVoxel original_endpoint = - makeLayeredVoxel(0.f, rgb(0.25f, 0.5f, 0.75f), 0.8f); - map->setVoxelValue(endpoint_index, original_endpoint); - - wavemap::Pointcloud<> pointcloud; - pointcloud.resize(1u); - pointcloud[0] = wavemap::Point3D(1.f, 0.f, 0.f); - const wavemap::Transformation3D T_W_C; - const wavemap::PosedPointcloud<> posed_pointcloud(T_W_C, pointcloud); - - wavemap::RayTracingIntegratorConfig integrator_config; - integrator_config.min_range = 0.05f; - integrator_config.max_range = 2.f; - - std::shared_ptr map_base = map; - wavemap::RayTracingIntegrator integrator(integrator_config, map_base); - - const LayeredVoxel before = map->getVoxelValue(endpoint_index); - const size_t size_before = map->size(); - - integrator.integrate(posed_pointcloud); - map->threshold(); - - const LayeredVoxel after = map->getVoxelValue(endpoint_index); - const size_t size_after = map->size(); - - float max_abs_occupancy = 0.f; - map->forEachVoxelLeaf([&max_abs_occupancy](const wavemap::OctreeIndex& /*index*/, - const LayeredVoxel& voxel) { - max_abs_occupancy = std::max(max_abs_occupancy, std::abs(voxel.occupancy)); - }); - - std::cout << "Occupancy-first layered map integration experiment\n"; - printVoxel("Layered voxel before integration", before); - printVoxel("Same layered voxel after integration", after); - - const bool occupancy_changed_somewhere = 1e-5f < max_abs_occupancy; - const bool map_allocated_integration_nodes = size_before < size_after; - const bool rgb_preserved = approximatelyEqual(before.data.rgb, after.data.rgb); - const bool traversability_preserved = - approximatelyEqual(before.data.traversability, - after.data.traversability); - - std::cout << "Map size before integration: " << size_before << "\n"; - std::cout << "Map size after integration: " << size_after << "\n"; - std::cout << "Max abs occupancy after integration: " - << max_abs_occupancy << "\n"; - std::cout << "Occupancy changed through real integrator: " - << (occupancy_changed_somewhere ? "yes" : "no") << "\n"; - std::cout << "Integrator allocated/updated nodes: " - << (map_allocated_integration_nodes ? "yes" : "no") << "\n"; - std::cout << "RGB preserved by occupancy-only update: " - << (rgb_preserved ? "yes" : "no") << "\n"; - std::cout << "Traversability preserved by occupancy-only update: " - << (traversability_preserved ? "yes" : "no") << "\n"; - - if (!occupancy_changed_somewhere || !rgb_preserved || - !traversability_preserved) { - return 1; - } - return 0; -} diff --git a/examples/cpp/digital_twin/map_api/continuous/layered_per_voxel_update_experiment.cc b/examples/cpp/digital_twin/map_api/continuous/layered_per_voxel_update_experiment.cc deleted file mode 100644 index 37489967b..000000000 --- a/examples/cpp/digital_twin/map_api/continuous/layered_per_voxel_update_experiment.cc +++ /dev/null @@ -1,143 +0,0 @@ -#include -#include -#include -#include - -#include -#include -#include -#include - -#include "../../common/layered_voxel_config.h" -#include "../../common/layered_voxel_updater.h" - -namespace { -bool approximatelyEqual(float lhs, float rhs, float tolerance = 1e-4f) { - return std::abs(lhs - rhs) <= tolerance; -} - -bool approximatelyEqual(const Rgb& lhs, const Rgb& rhs, - float tolerance = 1e-4f) { - return sumAbsDiff(lhs, rhs) <= tolerance; -} - -void printObservationResult(const ContinuousLayerObservation& observation, - const LayeredVoxel& before, - const LayeredVoxel& after) { - std::cout << "Voxel " << observation.index.transpose() << "\n"; - std::cout << " occupancy before layer update: " << before.occupancy << "\n"; - std::cout << " occupancy after layer update: " << after.occupancy << "\n"; - std::cout << " requested layer data: trav=" - << observation.data.traversability << " rgb=(" - << observation.data.rgb.r << ", " << observation.data.rgb.g - << ", " << observation.data.rgb.b << ")\n"; - printVoxel(" stored voxel", after); -} -} // namespace - -int main() { - wavemap::HashedWaveletOctreeConfig map_config; - map_config.min_cell_width = 0.1f; - map_config.min_log_odds = -2.f; - map_config.max_log_odds = 4.f; - map_config.tree_height = 4; - - auto map = std::make_shared(map_config); - - auto projection_model = std::make_shared( - wavemap::SphericalProjectorConfig( - wavemap::CircularProjectorConfig(-1.2f, 1.2f, 32), - wavemap::CircularProjectorConfig(-1.2f, 1.2f, 32))); - auto posed_range_image = - std::make_shared>(projection_model->getDimensions()); - auto beam_offset_image = - std::make_shared>( - projection_model->getDimensions()); - - wavemap::ContinuousBeamConfig measurement_config; - measurement_config.angle_sigma = 0.01f; - measurement_config.range_sigma = 0.02f; - measurement_config.scaling_free = 0.2f; - measurement_config.scaling_occupied = 0.4f; - auto measurement_model = std::make_shared( - measurement_config, projection_model, posed_range_image, - beam_offset_image); - - wavemap::ProjectiveIntegratorConfig integrator_config; - integrator_config.min_range = 0.05f; - integrator_config.max_range = 2.f; - integrator_config.max_update_resolution = 0.f; - integrator_config.termination_update_error = 0.05f; - - wavemap::HashedWaveletIntegratorT integrator( - integrator_config, projection_model, posed_range_image, - beam_offset_image, measurement_model, map); - - wavemap::Pointcloud<> pointcloud; - pointcloud.resize(1u); - pointcloud[0] = wavemap::Point3D(1.f, 0.f, 0.f); - const wavemap::Transformation3D T_W_C; - const wavemap::PosedPointcloud<> posed_pointcloud(T_W_C, pointcloud); - - integrator.integrate(posed_pointcloud); - map->threshold(); - - std::vector updated_indices; - map->forEachVoxelLeaf( - [&updated_indices](const wavemap::OctreeIndex& node_index, - const LayeredVoxel& voxel) { - if (updated_indices.size() < 3u && 1e-5f < std::abs(voxel.occupancy)) { - updated_indices.push_back( - wavemap::convert::nodeIndexToMinCornerIndex(node_index)); - } - }); - - if (updated_indices.size() < 3u) { - std::cout << "Not enough occupancy-updated voxels found.\n"; - return 1; - } - - const std::vector observations{ - {updated_indices[0], ContinuousLayers{rgb(0.9f, 0.1f, 0.1f), 0.8f}}, - {updated_indices[1], ContinuousLayers{rgb(0.1f, 0.8f, 0.2f), 0.4f}}, - {updated_indices[2], ContinuousLayers{rgb(0.2f, 0.2f, 0.9f), 0.6f}}}; - - std::vector before_layer_update; - before_layer_update.reserve(observations.size()); - for (const ContinuousLayerObservation& observation : observations) { - before_layer_update.push_back(map->getVoxelValue(observation.index)); - } - - setContinuousLayers(*map, observations); - map->threshold(); - - bool all_occupancy_preserved = true; - bool all_layers_updated = true; - - std::cout << "Layered per-voxel update experiment\n"; - std::cout << "Occupancy was integrated through the original ray-based " - "Wavemap pipeline. Continuous layers are then set from " - "per-voxel observations using a replacement policy.\n"; - - for (size_t i = 0; i < observations.size(); ++i) { - const LayeredVoxel after = map->getVoxelValue(observations[i].index); - printObservationResult(observations[i], before_layer_update[i], after); - - all_occupancy_preserved &= approximatelyEqual( - before_layer_update[i].occupancy, after.occupancy); - all_layers_updated &= approximatelyEqual(observations[i].data.rgb, - after.data.rgb, 1e-3f); - all_layers_updated &= approximatelyEqual( - observations[i].data.traversability, after.data.traversability, 1e-3f); - } - - std::cout << "Occupancy preserved by per-voxel layer update: " - << (all_occupancy_preserved ? "yes" : "no") << "\n"; - std::cout << "Continuous layers updated from per-voxel observations: " - << (all_layers_updated ? "yes" : "no") << "\n"; - - if (!all_occupancy_preserved || !all_layers_updated) { - return 1; - } - return 0; -} diff --git a/examples/cpp/digital_twin/map_api/continuous/layered_pruning_config.md b/examples/cpp/digital_twin/map_api/continuous/layered_pruning_config.md deleted file mode 100644 index e2f1ffe4b..000000000 --- a/examples/cpp/digital_twin/map_api/continuous/layered_pruning_config.md +++ /dev/null @@ -1,60 +0,0 @@ -# Layered pruning config - -This example uses `LayeredVoxel`, which stores occupancy plus continuous layers: - -```cpp -struct ContinuousLayers { - Rgb rgb; - float traversability; -}; -``` - -The map still stores wavelet coefficients. Pruning decides whether a node's detail coefficients are relevant enough to keep. - -The layered pruning score is: - -```cpp -score = sum(weight * abs(wavelet_detail) / scale) -``` - -A node is kept when: - -```cpp -score > combined_threshold -``` - -## Parameters - -`scale` is the amount of detail that counts as one unit of relevance for a field. Larger scale means that field is more tolerant and will allow more pruning. - -`weight` is the relative importance of the field. Use `0` to ignore a field during pruning. - -`combined_threshold` is the final pruning decision threshold. The default is `1`. - -## Examples - -Occupancy-only pruning: - -```cpp -auto pruning = makeOccupancyOnlyPruningConfig(); -``` - -Equal weighting for occupancy, RGB, and traversability: - -```cpp -auto pruning = makeEqualLayerPruningConfig(); -``` - -Custom field scales and weights: - -```cpp -auto pruning = makeLayeredPruningConfigWithScales( - /*occupancy_weight=*/1.f, - /*rgb_weight=*/1.f, - /*traversability_weight=*/2.f, - /*occupancy_scale=*/1.f, - /*rgb_scale=*/0.1f, - /*traversability_scale=*/0.1f); -``` - -The trade-off is expected: ignoring extra layers usually gives stronger compression, but can remove RGB/traversability details. Including extra layers preserves them, but may keep more nodes. diff --git a/examples/cpp/digital_twin/map_api/continuous/layered_pruning_config_example.cc b/examples/cpp/digital_twin/map_api/continuous/layered_pruning_config_example.cc deleted file mode 100644 index 1d328d5b4..000000000 --- a/examples/cpp/digital_twin/map_api/continuous/layered_pruning_config_example.cc +++ /dev/null @@ -1,88 +0,0 @@ -#include -#include -#include -#include - -#include "../../common/layered_voxel_config.h" - -namespace { -struct ExampleRun { - std::string name; - LayeredPruningConfig pruning_config; -}; - -void fillMapWithLayerBoundary(ContinuousWaveletMap& map) { - for (int x = 0; x < 32; ++x) { - for (int y = 0; y < 32; ++y) { - for (int z = 0; z < 32; ++z) { - const wavemap::FloatingPoint occupancy = 0.5f; - - Rgb color = rgb(0.2f, 0.2f, 0.2f); - if (16 <= x) { - color = rgb(0.9f, 0.1f, 0.1f); - } - if (16 <= y) { - color = rgb(0.1f, 0.1f, 0.9f); - } - - const float traversability = (8 <= z && z < 24) ? 0.85f : 0.25f; - map.setVoxelValue(wavemap::Index3D{x, y, z}, - makeLayeredVoxel(occupancy, color, traversability)); - } - } - } -} - -void runExample(const wavemap::HashedWaveletOctreeConfig& map_config, - const ExampleRun& run) { - ContinuousWaveletMap map(map_config, ContinuousWaveletMap::ThresholdConfig{}, - run.pruning_config); - fillMapWithLayerBoundary(map); - - const size_t nodes_before = map.size(); - const size_t memory_before = map.getMemoryUsage(); - - map.threshold(); - map.prune(); - - std::cout << run.name << "\n"; - std::cout << " nodes before prune: " << nodes_before << "\n"; - std::cout << " nodes after prune: " << map.size() << "\n"; - std::cout << " memory before prune bytes: " << memory_before << "\n"; - std::cout << " memory after prune bytes: " << map.getMemoryUsage() << "\n"; - printVoxel(" sample voxel", map.getVoxelValue(wavemap::Index3D{20, 20, 12})); - std::cout << "\n"; -} -} // namespace - -int main() { - wavemap::HashedWaveletOctreeConfig map_config; - map_config.min_cell_width = 0.1f; - map_config.min_log_odds = -100.f; - map_config.max_log_odds = 100.f; - map_config.tree_height = 4; - - // Pruning config quick guide: - // scale: detail magnitude that counts as one unit of relevance. - // weight: relative importance of this field in the combined score. - // combined_threshold: prune if the total score is not above this value. - // Formula: - // score = sum(weight * abs(wavelet_detail) / scale) - // A field with weight 0 is ignored by pruning. - const std::vector runs{ - {"Occupancy-only pruning", - makeOccupancyOnlyPruningConfig(/*scale=*/1.f)}, - {"Equal occupancy/RGB/traversability pruning", - makeEqualLayerPruningConfig(/*scale=*/0.1f)}, - {"Custom pruning: occupancy-heavy, layers still count", - makeLayeredPruningConfigWithScales( - /*occupancy_weight=*/4.f, /*rgb_weight=*/1.f, - /*traversability_weight=*/1.f, /*occupancy_scale=*/1.f, - /*rgb_scale=*/1.f, /*traversability_scale=*/1.f)}}; - - for (const ExampleRun& run : runs) { - runExample(map_config, run); - } - - return 0; -} diff --git a/examples/cpp/digital_twin/map_api/continuous/mixed_voxel_map_experiment.cc b/examples/cpp/digital_twin/map_api/continuous/mixed_voxel_map_experiment.cc deleted file mode 100644 index bf0cdedc9..000000000 --- a/examples/cpp/digital_twin/map_api/continuous/mixed_voxel_map_experiment.cc +++ /dev/null @@ -1,97 +0,0 @@ -#include -#include -#include - -#include "wavemap/core/indexing/ndtree_index.h" -#include "wavemap/core/map/cell_types/voxel_data.h" -#include "wavemap/core/map/hashed_wavelet_octree.h" - -struct MixedData { - float traversability = 0.f; - float r = 0.f; - float g = 0.f; - float b = 0.f; - int label = 0; - - bool operator==(const MixedData& other) const { - return traversability == other.traversability && r == other.r && - g == other.g && b == other.b && label == other.label; - } -}; - -struct MixedDataPolicy { - static MixedData add(const MixedData& lhs, const MixedData& rhs) { - return {lhs.traversability + rhs.traversability, lhs.r + rhs.r, - lhs.g + rhs.g, lhs.b + rhs.b, lhs.label + rhs.label}; - } - - static MixedData subtract(const MixedData& lhs, const MixedData& rhs) { - return {lhs.traversability - rhs.traversability, lhs.r - rhs.r, - lhs.g - rhs.g, lhs.b - rhs.b, lhs.label - rhs.label}; - } - - static MixedData scale(const MixedData& data, wavemap::FloatingPoint factor) { - return {factor * data.traversability, factor * data.r, factor * data.g, - factor * data.b, static_cast(factor * data.label)}; - } -}; - -using MixedVoxel = wavemap::VoxelData; -using MixedMap = wavemap::HashedWaveletOctreeT; - -void printVoxel(const std::string& name, const MixedVoxel& voxel) { - std::cout << name << ": occ=" << voxel.occupancy - << " trav=" << voxel.data.traversability << " rgb=(" - << voxel.data.r << ", " << voxel.data.g << ", " << voxel.data.b - << ") label=" << voxel.data.label << "\n"; -} - -int main() { - wavemap::HashedWaveletOctreeConfig config; - config.min_cell_width = 0.1f; - config.min_log_odds = -100.f; - config.max_log_odds = 100.f; - config.tree_height = 3; - - MixedMap map(config); - - const wavemap::Index3D a(1, 2, 3); - const wavemap::Index3D b(2, 2, 3); - const wavemap::Index3D c(3, 2, 3); - - map.setVoxelValue(a, MixedVoxel(10.f, {0.1f, 1.f, 0.f, 0.f, 1})); - map.setVoxelValue(b, MixedVoxel(20.f, {0.5f, 0.f, 1.f, 0.f, 5})); - map.setVoxelValue(c, MixedVoxel(30.f, {0.9f, 0.f, 0.f, 1.f, 9})); - - std::cout << "Before threshold/prune\n"; - printVoxel("a", map.getVoxelValue(a)); - printVoxel("b", map.getVoxelValue(b)); - printVoxel("c", map.getVoxelValue(c)); - std::cout << "block count: " << map.getHashMap().size() << "\n"; - std::cout << "node count: " << map.size() << "\n"; - - size_t labeled_leaves = 0u; - map.forEachVoxelLeaf( - [&labeled_leaves](const wavemap::OctreeIndex& index, - const MixedVoxel& voxel) { - if (voxel.data.label != 0) { - ++labeled_leaves; - std::cout << "leaf h=" << index.height << " pos=" - << index.position.transpose(); - printVoxel(" value", voxel); - } - }); - std::cout << "labeled leaves: " << labeled_leaves << "\n"; - - map.threshold(); - map.prune(); - - std::cout << "After threshold/prune\n"; - printVoxel("a", map.getVoxelValue(a)); - printVoxel("b", map.getVoxelValue(b)); - printVoxel("c", map.getVoxelValue(c)); - std::cout << "block count: " << map.getHashMap().size() << "\n"; - std::cout << "node count: " << map.size() << "\n"; - - return 0; -} diff --git a/examples/cpp/digital_twin/map_api/discrete/discrete_layer_compression_experiment.cc b/examples/cpp/digital_twin/map_api/discrete/discrete_layer_compression_experiment.cc deleted file mode 100644 index 4b2694ff7..000000000 --- a/examples/cpp/digital_twin/map_api/discrete/discrete_layer_compression_experiment.cc +++ /dev/null @@ -1,138 +0,0 @@ -#include -#include -#include - -#include - -using wavemap::layered::DiscreteCompressionConfig; -using wavemap::layered::DiscreteLayer; -using wavemap::layered::RawDiscreteLayer; - -namespace { -template -void fillBlock(LayerT& layer, const wavemap::Index3D& origin, int side_length, - const ValueT& value) { - for (int dx = 0; dx < side_length; ++dx) { - for (int dy = 0; dy < side_length; ++dy) { - for (int dz = 0; dz < side_length; ++dz) { - layer.setValue(origin + wavemap::Index3D(dx, dy, dz), value); - } - } - } -} - -template -bool sameValue(const std::optional& read, const ValueT& expected) { - return read && *read == expected; -} - -template -size_t countPreservedValues(const RawDiscreteLayer& raw, - const DiscreteLayer& compressed) { - size_t preserved = 0u; - for (const auto& [key, expected] : raw.values()) { - if (sameValue(compressed.getValue(indexFromKey(key)), expected)) { - ++preserved; - } - } - return preserved; -} - -template -void printCompressionStats(const std::string& name, - const RawDiscreteLayer& raw, - const DiscreteLayer& compressed) { - std::cout << name << "\n"; - std::cout << " raw entries: " << raw.size() << "\n"; - std::cout << " compressed parents: " << compressed.parentCount() << "\n"; - std::cout << " exceptions: " << compressed.exceptionCount() << "\n"; - std::cout << " preserved values: " - << countPreservedValues(raw, compressed) << "/" << raw.size() - << "\n"; -} -} // namespace - -int main() { - RawDiscreteLayer semantic_layer; - RawDiscreteLayer changed_layer; - - const wavemap::Index3D uniform_block(0, 0, 0); - fillBlock(semantic_layer, uniform_block, 2, 1); - fillBlock(changed_layer, uniform_block, 2, false); - - const wavemap::Index3D one_exception_block(4, 0, 0); - fillBlock(semantic_layer, one_exception_block, 2, 2); - fillBlock(changed_layer, one_exception_block, 2, false); - semantic_layer.setValue(one_exception_block + wavemap::Index3D(1, 1, 1), 5); - changed_layer.setValue(one_exception_block + wavemap::Index3D(1, 1, 1), true); - - const wavemap::Index3D mixed_block(8, 0, 0); - fillBlock(semantic_layer, mixed_block, 2, 3); - fillBlock(changed_layer, mixed_block, 2, false); - semantic_layer.setValue(mixed_block + wavemap::Index3D(0, 1, 0), 4); - semantic_layer.setValue(mixed_block + wavemap::Index3D(1, 0, 1), 4); - semantic_layer.setValue(mixed_block + wavemap::Index3D(1, 1, 0), 6); - changed_layer.setValue(mixed_block + wavemap::Index3D(0, 1, 0), true); - changed_layer.setValue(mixed_block + wavemap::Index3D(1, 0, 1), true); - changed_layer.setValue(mixed_block + wavemap::Index3D(1, 1, 0), true); - - const DiscreteCompressionConfig one_octree_level{1}; - const auto compressed_semantic = - DiscreteLayer::fromRaw(semantic_layer, one_octree_level); - const auto compressed_changed = - DiscreteLayer::fromRaw(changed_layer, one_octree_level); - - const DiscreteCompressionConfig two_octree_levels{2}; - DiscreteLayer directly_updated_semantic(two_octree_levels); - fillBlock(directly_updated_semantic, wavemap::Index3D(0, 8, 0), - two_octree_levels.blockSideLength(), 7); - directly_updated_semantic.setValue(wavemap::Index3D(3, 11, 3), 9); - - std::cout << "Discrete layer compression experiment\n"; - std::cout << "Compression stores one dominant value per configured parent " - "region and explicit child exceptions. Reconstruction is " - "lossless for stored voxels.\n"; - printCompressionStats("semantic layer", semantic_layer, - compressed_semantic); - printCompressionStats("binary changed layer", changed_layer, - compressed_changed); - std::cout << "directly updated semantic layer\n"; - std::cout << " block height: " - << directly_updated_semantic.config().block_height << "\n"; - std::cout << " block side length: " - << directly_updated_semantic.config().blockSideLength() << "\n"; - std::cout << " compressed parents: " - << directly_updated_semantic.parentCount() << "\n"; - std::cout << " exceptions: " - << directly_updated_semantic.exceptionCount() << "\n"; - std::cout << " updated exception preserved: " - << (sameValue(directly_updated_semantic.getValue( - wavemap::Index3D(3, 11, 3)), - 9) - ? "yes" - : "no") - << "\n"; - std::cout << " sparse unknown remains empty: " - << (!directly_updated_semantic.getValue(wavemap::Index3D(8, 8, 8)) - ? "yes" - : "no") - << "\n"; - - const bool semantic_lossless = - countPreservedValues(semantic_layer, compressed_semantic) == - semantic_layer.size(); - const bool changed_lossless = - countPreservedValues(changed_layer, compressed_changed) == - changed_layer.size(); - - const bool direct_update_ok = - sameValue(directly_updated_semantic.getValue(wavemap::Index3D(3, 11, 3)), - 9) && - !directly_updated_semantic.getValue(wavemap::Index3D(8, 8, 8)); - - if (!semantic_lossless || !changed_lossless || !direct_update_ok) { - std::cout << "Compressed discrete layer did not reconstruct all values.\n"; - return 1; - } - return 0; -} diff --git a/examples/cpp/digital_twin/map_api/discrete/discrete_layer_update_behavior_experiment.cc b/examples/cpp/digital_twin/map_api/discrete/discrete_layer_update_behavior_experiment.cc deleted file mode 100644 index ee0643a7f..000000000 --- a/examples/cpp/digital_twin/map_api/discrete/discrete_layer_update_behavior_experiment.cc +++ /dev/null @@ -1,112 +0,0 @@ -#include -#include -#include - -#include - -using wavemap::layered::DiscreteCompressionConfig; -using wavemap::layered::DiscreteLayer; -using wavemap::layered::RawDiscreteLayer; - -namespace { -template -bool sameValue(const std::optional& value, const ValueT& expected) { - return value && *value == expected; -} - -void fillBlock(DiscreteLayer& layer, const wavemap::Index3D& origin, - int side_length, int value) { - for (int dx = 0; dx < side_length; ++dx) { - for (int dy = 0; dy < side_length; ++dy) { - for (int dz = 0; dz < side_length; ++dz) { - layer.setValue(origin + wavemap::Index3D(dx, dy, dz), value); - } - } - } -} - -bool expect(const std::string& name, bool condition) { - std::cout << " " << name << ": " << (condition ? "pass" : "FAIL") - << "\n"; - return condition; -} -} // namespace - -int main() { - bool ok = true; - - std::cout << "Discrete layer update behavior experiment\n"; - - { - DiscreteLayer layer(DiscreteCompressionConfig{1}); - fillBlock(layer, wavemap::Index3D(0, 0, 0), 2, 1); - ok &= expect("uniform block has one parent", layer.parentCount() == 1u); - ok &= expect("uniform block has zero exceptions", - layer.exceptionCount() == 0u); - ok &= expect("uniform block observed all children", - layer.observedValueCount() == 8u); - ok &= expect("uniform block reconstructs dominant value", - sameValue(layer.getValue(wavemap::Index3D(1, 1, 1)), 1)); - } - - { - DiscreteLayer layer(DiscreteCompressionConfig{1}); - fillBlock(layer, wavemap::Index3D(0, 0, 0), 2, 1); - layer.setValue(wavemap::Index3D(1, 1, 1), 2); - ok &= expect("one different child becomes one exception", - layer.exceptionCount() == 1u); - ok &= expect("exception value reconstructs exactly", - sameValue(layer.getValue(wavemap::Index3D(1, 1, 1)), 2)); - - layer.setValue(wavemap::Index3D(1, 1, 1), 1); - ok &= expect("setting exception back removes exception", - layer.exceptionCount() == 0u); - ok &= expect("restored child reconstructs dominant value", - sameValue(layer.getValue(wavemap::Index3D(1, 1, 1)), 1)); - } - - { - DiscreteLayer layer(DiscreteCompressionConfig{1}); - fillBlock(layer, wavemap::Index3D(0, 0, 0), 2, 1); - layer.setValue(wavemap::Index3D(0, 0, 0), 2); - layer.setValue(wavemap::Index3D(1, 0, 0), 2); - layer.setValue(wavemap::Index3D(0, 1, 0), 2); - layer.setValue(wavemap::Index3D(1, 1, 0), 2); - layer.setValue(wavemap::Index3D(0, 0, 1), 2); - ok &= expect("majority update changes dominant value", - sameValue(layer.dominantValue(wavemap::Index3D(0, 0, 0)), 2)); - ok &= expect("old minority values become exceptions", - layer.exceptionCount() == 3u); - ok &= expect("new majority child reconstructs exactly", - sameValue(layer.getValue(wavemap::Index3D(0, 0, 0)), 2)); - ok &= expect("old minority child reconstructs exactly", - sameValue(layer.getValue(wavemap::Index3D(1, 1, 1)), 1)); - } - - { - DiscreteLayer layer(DiscreteCompressionConfig{1}); - layer.setValue(wavemap::Index3D(0, 0, 0), 7); - ok &= expect("unknown child in existing parent remains unknown", - !layer.getValue(wavemap::Index3D(1, 1, 1))); - ok &= expect("observed child still reconstructs", - sameValue(layer.getValue(wavemap::Index3D(0, 0, 0)), 7)); - } - - { - DiscreteLayer layer(DiscreteCompressionConfig{2}); - fillBlock(layer, wavemap::Index3D(0, 0, 0), 4, 3); - layer.setValue(wavemap::Index3D(3, 3, 3), 4); - ok &= expect("block_height 2 uses one 4x4x4 parent", - layer.parentCount() == 1u); - ok &= expect("block_height 2 stores one exception", - layer.exceptionCount() == 1u); - ok &= expect("block_height 2 reconstructs exception", - sameValue(layer.getValue(wavemap::Index3D(3, 3, 3)), 4)); - } - - if (!ok) { - std::cout << "Discrete layer update behavior failed.\n"; - return 1; - } - return 0; -} diff --git a/examples/cpp/digital_twin/map_api/discrete/float_cast_discrete_experiment.cc b/examples/cpp/digital_twin/map_api/discrete/float_cast_discrete_experiment.cc deleted file mode 100644 index 2a8ed5ec1..000000000 --- a/examples/cpp/digital_twin/map_api/discrete/float_cast_discrete_experiment.cc +++ /dev/null @@ -1,147 +0,0 @@ -#include -#include -#include -#include -#include - -#include "wavemap/core/indexing/ndtree_index.h" -#include "wavemap/core/map/cell_types/voxel_data.h" -#include "wavemap/core/map/hashed_wavelet_octree.h" - -struct CastDiscreteData { - float label = 0.f; - float dynamic = 0.f; - float code = 0.f; - - bool operator==(const CastDiscreteData& other) const { - return label == other.label && dynamic == other.dynamic && - code == other.code; - } -}; - -struct CastDiscreteDataPolicy { - static CastDiscreteData add(const CastDiscreteData& lhs, - const CastDiscreteData& rhs) { - return {lhs.label + rhs.label, lhs.dynamic + rhs.dynamic, - lhs.code + rhs.code}; - } - - static CastDiscreteData subtract(const CastDiscreteData& lhs, - const CastDiscreteData& rhs) { - return {lhs.label - rhs.label, lhs.dynamic - rhs.dynamic, - lhs.code - rhs.code}; - } - - static CastDiscreteData scale(const CastDiscreteData& data, - wavemap::FloatingPoint factor) { - return {factor * data.label, factor * data.dynamic, factor * data.code}; - } -}; - -struct RawDiscreteData { - int label = 0; - bool dynamic = false; - char code = '\0'; - - bool operator==(const RawDiscreteData& other) const { - return label == other.label && dynamic == other.dynamic && - code == other.code; - } -}; - -using CastVoxel = wavemap::VoxelData; -using CastMap = wavemap::HashedWaveletOctreeT; - -CastVoxel toVoxel(wavemap::FloatingPoint occupancy, - const RawDiscreteData& data) { - return CastVoxel( - occupancy, - {static_cast(data.label), data.dynamic ? 1.f : 0.f, - static_cast(static_cast(data.code))}); -} - -RawDiscreteData fromVoxelRounded(const CastVoxel& voxel) { - return {static_cast(std::lround(voxel.data.label)), - std::lround(voxel.data.dynamic) != 0, - static_cast(std::lround(voxel.data.code))}; -} - -void printRaw(const std::string& name, const RawDiscreteData& data) { - std::cout << name << ": label=" << data.label - << " dynamic=" << std::boolalpha << data.dynamic - << " code=" << data.code << "\n"; -} - -void printVoxel(const std::string& name, const CastVoxel& voxel) { - std::cout << name << ": occ=" << voxel.occupancy - << " label_f=" << voxel.data.label - << " dynamic_f=" << voxel.data.dynamic - << " code_f=" << voxel.data.code << " rounded=(" - << fromVoxelRounded(voxel).label << ", " << std::boolalpha - << fromVoxelRounded(voxel).dynamic << ", " - << fromVoxelRounded(voxel).code << ")\n"; -} - -int main() { - wavemap::HashedWaveletOctreeConfig config; - config.min_cell_width = 0.1f; - config.min_log_odds = -100.f; - config.max_log_odds = 100.f; - config.tree_height = 3; - - CastMap map(config); - - const std::vector> samples = { - {wavemap::Index3D(1, 2, 3), 10.f, {1, false, 'A'}}, - {wavemap::Index3D(2, 2, 3), 20.f, {5, true, 'B'}}, - {wavemap::Index3D(3, 2, 3), 30.f, {9, false, 'C'}}, - }; - - for (const auto& [index, occupancy, data] : samples) { - map.setVoxelValue(index, toVoxel(occupancy, data)); - } - - std::cout << "Expected raw values\n"; - for (const auto& [index, occupancy, data] : samples) { - (void)index; - (void)occupancy; - printRaw("expected", data); - } - - size_t preserved_after_rounding = 0u; - std::cout << "Read through float cast + Haar\n"; - for (const auto& [index, occupancy, expected] : samples) { - (void)occupancy; - const CastVoxel read_voxel = map.getVoxelValue(index); - const RawDiscreteData rounded = fromVoxelRounded(read_voxel); - printVoxel("read", read_voxel); - if (rounded == expected) { - ++preserved_after_rounding; - } - } - - size_t interesting_leaves = 0u; - map.forEachVoxelLeaf( - [&interesting_leaves](const wavemap::OctreeIndex& index, - const CastVoxel& voxel) { - const bool interesting = std::abs(voxel.occupancy) > 1e-5f || - std::abs(voxel.data.label) > 1e-5f || - std::abs(voxel.data.dynamic) > 1e-5f || - std::abs(voxel.data.code) > 1e-5f; - if (interesting) { - ++interesting_leaves; - std::cout << "leaf h=" << index.height << " pos=" - << index.position.transpose(); - printVoxel(" value", voxel); - } - }); - - std::cout << "preserved after rounding: " << preserved_after_rounding << "/" - << samples.size() << "\n"; - std::cout << "interesting leaves: " << interesting_leaves << "\n"; - std::cout << "block count: " << map.getHashMap().size() << "\n"; - std::cout << "node count: " << map.size() << "\n"; - - return preserved_after_rounding == samples.size() ? 0 : 1; -} diff --git a/examples/cpp/digital_twin/map_api/discrete/layered_label_storage_experiment.cc b/examples/cpp/digital_twin/map_api/discrete/layered_label_storage_experiment.cc deleted file mode 100644 index 036967f07..000000000 --- a/examples/cpp/digital_twin/map_api/discrete/layered_label_storage_experiment.cc +++ /dev/null @@ -1,76 +0,0 @@ -#include -#include -#include -#include - -#include "../../common/layered_voxel_config.h" - -struct IndexKey { - int x = 0; - int y = 0; - int z = 0; - - explicit IndexKey(const wavemap::Index3D& index) - : x(index.x()), y(index.y()), z(index.z()) {} - - bool operator<(const IndexKey& other) const { - return std::tie(x, y, z) < std::tie(other.x, other.y, other.z); - } -}; - -void printVoxelWithLabel(const std::string& name, const LayeredVoxel& voxel, - int label) { - std::cout << name << ": occ=" << voxel.occupancy - << " trav=" << voxel.data.traversability << " rgb=(" - << voxel.data.rgb.r << ", " << voxel.data.rgb.g << ", " - << voxel.data.rgb.b << ") label=" << label << "\n"; -} - -int main() { - wavemap::HashedWaveletOctreeConfig config; - config.min_cell_width = 0.1f; - config.min_log_odds = -100.f; - config.max_log_odds = 100.f; - config.tree_height = 3; - - ContinuousWaveletMap map(config); - std::map labels; - - const wavemap::Index3D a(1, 2, 3); - const wavemap::Index3D b(2, 2, 3); - const wavemap::Index3D c(3, 2, 3); - - map.setVoxelValue(a, - makeLayeredVoxel(10.f, rgb(1.f, 0.f, 0.f), - 0.1f)); - map.setVoxelValue(b, - makeLayeredVoxel(20.f, rgb(0.f, 1.f, 0.f), - 0.5f)); - map.setVoxelValue(c, - makeLayeredVoxel(30.f, rgb(0.f, 0.f, 1.f), - 0.9f)); - labels[IndexKey(a)] = 1; - labels[IndexKey(b)] = 5; - labels[IndexKey(c)] = 9; - - std::cout << "Before threshold/prune\n"; - printVoxelWithLabel("a", map.getVoxelValue(a), labels[IndexKey(a)]); - printVoxelWithLabel("b", map.getVoxelValue(b), labels[IndexKey(b)]); - printVoxelWithLabel("c", map.getVoxelValue(c), labels[IndexKey(c)]); - std::cout << "label entries: " << labels.size() << "\n"; - std::cout << "block count: " << map.getHashMap().size() << "\n"; - std::cout << "node count: " << map.size() << "\n"; - - map.threshold(); - map.prune(); - - std::cout << "After threshold/prune\n"; - printVoxelWithLabel("a", map.getVoxelValue(a), labels[IndexKey(a)]); - printVoxelWithLabel("b", map.getVoxelValue(b), labels[IndexKey(b)]); - printVoxelWithLabel("c", map.getVoxelValue(c), labels[IndexKey(c)]); - std::cout << "label entries: " << labels.size() << "\n"; - std::cout << "block count: " << map.getHashMap().size() << "\n"; - std::cout << "node count: " << map.size() << "\n"; - - return 0; -} diff --git a/examples/cpp/digital_twin/map_api/discrete/layered_map_experiment.cc b/examples/cpp/digital_twin/map_api/discrete/layered_map_experiment.cc deleted file mode 100644 index 22d67e1cb..000000000 --- a/examples/cpp/digital_twin/map_api/discrete/layered_map_experiment.cc +++ /dev/null @@ -1,123 +0,0 @@ -#include -#include -#include - -#include "../../common/example_layered_map_config.h" - -namespace { -struct ExampleVoxelView { - LayeredVoxel continuous; - std::optional semantic_label; - std::optional changed; -}; - -ExampleVoxelView getExampleVoxel(const ExampleLayeredMap& map, - const wavemap::Index3D& index) { - return {map.continuousMap().getVoxelValue(index), - map.discreteLayers().semantic.getValue(index), - map.discreteLayers().changed.getValue(index)}; -} - -void printOptionalBool(const std::optional& value) { - if (value) { - std::cout << (*value ? "true" : "false"); - } else { - std::cout << "none"; - } -} - -void printLayeredMapVoxel(const std::string& name, - const ExampleVoxelView& voxel) { - std::cout << name << ": occ=" << voxel.continuous.occupancy - << " trav=" << voxel.continuous.data.traversability << " rgb=(" - << voxel.continuous.data.rgb.r << ", " - << voxel.continuous.data.rgb.g << ", " - << voxel.continuous.data.rgb.b << ") semantic="; - if (voxel.semantic_label) { - std::cout << *voxel.semantic_label; - } else { - std::cout << "none"; - } - std::cout << " changed="; - printOptionalBool(voxel.changed); - std::cout << "\n"; -} -} // namespace - -int main() { - ExampleLayeredMapConfig config; - config.continuous_map.min_cell_width = 0.1f; - config.continuous_map.min_log_odds = -100.f; - config.continuous_map.max_log_odds = 100.f; - config.continuous_map.tree_height = 3; - config.discrete_compression.block_height = 1; - - ExampleLayeredMap map(config); - - const wavemap::Index3D a(1, 2, 3); - const wavemap::Index3D b(2, 2, 3); - const wavemap::Index3D c(3, 2, 3); - - map.continuousMap().setVoxelValue( - a, makeLayeredVoxel(10.f, rgb(1.f, 0.f, 0.f), 0.1f)); - map.continuousMap().setVoxelValue( - b, makeLayeredVoxel(20.f, rgb(0.f, 1.f, 0.f), 0.5f)); - map.continuousMap().setVoxelValue( - c, makeLayeredVoxel(30.f, rgb(0.f, 0.f, 1.f), 0.9f)); - - map.discreteLayers().semantic.setValue(a, 1); - map.discreteLayers().semantic.setValue(b, 5); - map.discreteLayers().semantic.setValue(c, 9); - - map.discreteLayers().changed.setValue(a, false); - map.discreteLayers().changed.setValue(b, true); - map.discreteLayers().changed.setValue(c, false); - - std::cout << "LayeredMap experiment\n"; - std::cout << "Continuous fields are stored in the wavelet octree. " - "Discrete semantic and binary layers are stored directly in " - "compressed parent + exception layers, outside Haar arithmetic.\n"; - - std::cout << "Before threshold/prune\n"; - printLayeredMapVoxel("a", getExampleVoxel(map, a)); - printLayeredMapVoxel("b", getExampleVoxel(map, b)); - printLayeredMapVoxel("c", getExampleVoxel(map, c)); - std::cout << "continuous nodes: " << map.continuousMap().size() << "\n"; - std::cout << "semantic compressed parents: " << map.discreteLayers().semantic.parentCount() - << " exceptions: " << map.discreteLayers().semantic.exceptionCount() - << " observed values: " << map.discreteLayers().semantic.observedValueCount() - << "\n"; - std::cout << "changed compressed parents: " << map.discreteLayers().changed.parentCount() - << " exceptions: " << map.discreteLayers().changed.exceptionCount() - << " observed values: " << map.discreteLayers().changed.observedValueCount() - << "\n"; - - map.continuousMap().threshold(); - map.continuousMap().prune(); - - std::cout << "After threshold/prune\n"; - printLayeredMapVoxel("a", getExampleVoxel(map, a)); - printLayeredMapVoxel("b", getExampleVoxel(map, b)); - printLayeredMapVoxel("c", getExampleVoxel(map, c)); - std::cout << "continuous nodes: " << map.continuousMap().size() << "\n"; - std::cout << "semantic compressed parents: " << map.discreteLayers().semantic.parentCount() - << " exceptions: " << map.discreteLayers().semantic.exceptionCount() - << " observed values: " << map.discreteLayers().semantic.observedValueCount() - << "\n"; - std::cout << "changed compressed parents: " << map.discreteLayers().changed.parentCount() - << " exceptions: " << map.discreteLayers().changed.exceptionCount() - << " observed values: " << map.discreteLayers().changed.observedValueCount() - << "\n"; - - const bool semantic_labels_preserved = getExampleVoxel(map, a).semantic_label == 1 && - getExampleVoxel(map, b).semantic_label == 5 && - getExampleVoxel(map, c).semantic_label == 9; - const bool changed_flags_preserved = getExampleVoxel(map, a).changed == false && - getExampleVoxel(map, b).changed == true && - getExampleVoxel(map, c).changed == false; - if (!semantic_labels_preserved || !changed_flags_preserved) { - std::cout << "Discrete layers were not preserved.\n"; - return 1; - } - return 0; -} diff --git a/examples/cpp/digital_twin/map_api/discrete/layered_map_update_behavior_experiment.cc b/examples/cpp/digital_twin/map_api/discrete/layered_map_update_behavior_experiment.cc deleted file mode 100644 index 1dad3d844..000000000 --- a/examples/cpp/digital_twin/map_api/discrete/layered_map_update_behavior_experiment.cc +++ /dev/null @@ -1,82 +0,0 @@ -#include -#include - -#include "../../common/example_layered_map_config.h" - -namespace { -template -bool sameValue(const std::optional& value, const ValueT& expected) { - return value && *value == expected; -} - -bool expect(const std::string& name, bool condition) { - std::cout << " " << name << ": " << (condition ? "pass" : "FAIL") - << "\n"; - return condition; -} -} // namespace - -int main() { - ExampleLayeredMapConfig config; - config.continuous_map.min_cell_width = 0.1f; - config.continuous_map.min_log_odds = -100.f; - config.continuous_map.max_log_odds = 100.f; - config.continuous_map.tree_height = 3; - config.discrete_compression.block_height = 1; - - ExampleLayeredMap map(config); - const wavemap::Index3D a(0, 0, 0); - const wavemap::Index3D b(1, 0, 0); - const wavemap::Index3D c(1, 1, 1); - - map.continuousMap().setVoxelValue( - a, makeLayeredVoxel(8.f, rgb(1.2f, -0.1f, 0.5f), 1.2f)); - map.continuousMap().setVoxelValue( - b, makeLayeredVoxel(9.f, rgb(0.2f, 0.3f, 0.4f), 0.6f)); - map.continuousMap().setVoxelValue( - c, makeLayeredVoxel(7.f, rgb(0.2f, 0.3f, 0.4f), 0.6f)); - - map.discreteLayers().semantic.setValue(a, 1); - map.discreteLayers().semantic.setValue(b, 1); - map.discreteLayers().semantic.setValue(c, 2); - map.discreteLayers().changed.setValue(a, false); - map.discreteLayers().changed.setValue(b, false); - map.discreteLayers().changed.setValue(c, true); - - map.continuousMap().threshold(); - map.continuousMap().prune(); - - bool ok = true; - std::cout << "LayeredMap continuous + discrete update behavior experiment\n"; - - const LayeredVoxel voxel_a = map.continuousMap().getVoxelValue(a); - ok &= expect("continuous threshold clamps rgb lower bound", - 0.f <= voxel_a.data.rgb.g); - ok &= expect("continuous threshold clamps rgb upper bound", - voxel_a.data.rgb.r <= 1.f); - ok &= expect("continuous threshold clamps traversability", - voxel_a.data.traversability <= 1.f); - - ok &= expect("semantic value a preserved", - sameValue(map.discreteLayers().semantic.getValue(a), 1)); - ok &= expect("semantic value c preserved", - sameValue(map.discreteLayers().semantic.getValue(c), 2)); - ok &= expect("binary value a preserved", - sameValue(map.discreteLayers().changed.getValue(a), false)); - ok &= expect("binary value c preserved", - sameValue(map.discreteLayers().changed.getValue(c), true)); - ok &= expect("unknown discrete voxel remains unknown", - !map.discreteLayers().semantic.getValue(wavemap::Index3D(2, 2, 2))); - ok &= expect("semantic layer compressed parent count is stable", - map.discreteLayers().semantic.parentCount() == 1u); - ok &= expect("semantic layer has one exception", - map.discreteLayers().semantic.exceptionCount() == 1u); - ok &= expect("changed layer has one exception", - map.discreteLayers().changed.exceptionCount() == 1u); - - if (!ok) { - std::cout << "LayeredMap integrated update behavior failed.\n"; - return 1; - } - return 0; -} diff --git a/examples/cpp/digital_twin/map_api/discrete/raw_discrete_voxel_storage_experiment.cc b/examples/cpp/digital_twin/map_api/discrete/raw_discrete_voxel_storage_experiment.cc deleted file mode 100644 index 76f03475a..000000000 --- a/examples/cpp/digital_twin/map_api/discrete/raw_discrete_voxel_storage_experiment.cc +++ /dev/null @@ -1,90 +0,0 @@ -#include -#include -#include -#include -#include -#include - -#include "wavemap/core/indexing/index_hashes.h" -#include "wavemap/core/indexing/ndtree_index.h" - -struct RawDiscreteData { - int label = 0; - bool dynamic = false; - char code = '\0'; - - bool operator==(const RawDiscreteData& other) const { - return label == other.label && dynamic == other.dynamic && - code == other.code; - } -}; - -struct IndexKey { - int x = 0; - int y = 0; - int z = 0; - - explicit IndexKey(const wavemap::Index3D& index) - : x(index.x()), y(index.y()), z(index.z()) {} - - bool operator<(const IndexKey& other) const { - return std::tie(x, y, z) < std::tie(other.x, other.y, other.z); - } -}; - -using RawStorage = std::map; - -void printData(const std::string& name, const RawDiscreteData& data) { - std::cout << name << ": label=" << data.label - << " dynamic=" << std::boolalpha << data.dynamic - << " code=" << data.code << "\n"; -} - -int main() { - const std::vector> samples = { - {wavemap::Index3D(1, 2, 3), {1, false, 'A'}}, - {wavemap::Index3D(2, 2, 3), {5, true, 'B'}}, - {wavemap::Index3D(3, 2, 3), {9, false, 'C'}}, - }; - - RawStorage storage; - - const auto write_start = std::chrono::steady_clock::now(); - for (const auto& [index, data] : samples) { - storage[IndexKey(index)] = data; - } - const auto write_end = std::chrono::steady_clock::now(); - - size_t preserved_values = 0u; - const auto read_start = std::chrono::steady_clock::now(); - for (const auto& [index, expected] : samples) { - const RawDiscreteData read = storage.at(IndexKey(index)); - printData("read", read); - if (read == expected) { - ++preserved_values; - } - } - const auto read_end = std::chrono::steady_clock::now(); - - const auto write_us = - std::chrono::duration_cast(write_end - - write_start) - .count(); - const auto read_us = - std::chrono::duration_cast(read_end - - read_start) - .count(); - - const size_t approximate_payload_bytes = storage.size() * - (sizeof(IndexKey) + sizeof(RawDiscreteData)); - - std::cout << "entries: " << storage.size() << "\n"; - std::cout << "preserved values: " << preserved_values << "/" - << samples.size() << "\n"; - std::cout << "approx payload bytes without map overhead: " - << approximate_payload_bytes << "\n"; - std::cout << "write time [us]: " << write_us << "\n"; - std::cout << "read time [us]: " << read_us << "\n"; - - return preserved_values == samples.size() ? 0 : 1; -} diff --git a/examples/cpp/digital_twin/map_api/discrete/raw_inside_map_experiment.cc b/examples/cpp/digital_twin/map_api/discrete/raw_inside_map_experiment.cc deleted file mode 100644 index 462b4a0d7..000000000 --- a/examples/cpp/digital_twin/map_api/discrete/raw_inside_map_experiment.cc +++ /dev/null @@ -1,143 +0,0 @@ -#include -#include -#include -#include -#include -#include -#include - -#include "wavemap/core/indexing/ndtree_index.h" -#include "wavemap/core/map/hashed_wavelet_octree.h" - -struct RawDiscreteData { - int label = 0; - bool dynamic = false; - char code = '\0'; - - bool operator==(const RawDiscreteData& other) const { - return label == other.label && dynamic == other.dynamic && - code == other.code; - } -}; - -struct IndexKey { - int x = 0; - int y = 0; - int z = 0; - - explicit IndexKey(const wavemap::Index3D& index) - : x(index.x()), y(index.y()), z(index.z()) {} - - bool operator<(const IndexKey& other) const { - return std::tie(x, y, z) < std::tie(other.x, other.y, other.z); - } -}; - -class RawDiscreteLayeredMap { - public: - explicit RawDiscreteLayeredMap( - const wavemap::HashedWaveletOctreeConfig& config) - : occupancy_map_(config) {} - - void setOccupancy(const wavemap::Index3D& index, - wavemap::FloatingPoint occupancy) { - occupancy_map_.setCellValue(index, occupancy); - } - - wavemap::FloatingPoint getOccupancy(const wavemap::Index3D& index) const { - return occupancy_map_.getCellValue(index); - } - - void setRawData(const wavemap::Index3D& index, const RawDiscreteData& data) { - raw_data_[IndexKey(index)] = data; - } - - std::optional getRawData( - const wavemap::Index3D& index) const { - const auto it = raw_data_.find(IndexKey(index)); - if (it == raw_data_.end()) { - return std::nullopt; - } - return it->second; - } - - size_t rawDataSize() const { return raw_data_.size(); } - size_t occupancyBlockCount() const { return occupancy_map_.getHashMap().size(); } - size_t occupancyNodeCount() const { return occupancy_map_.size(); } - - size_t approximateRawBytesWithoutMapOverhead() const { - return raw_data_.size() * (sizeof(IndexKey) + sizeof(RawDiscreteData)); - } - - private: - wavemap::HashedWaveletOctree occupancy_map_; - std::map raw_data_; -}; - -void printData(const std::string& name, wavemap::FloatingPoint occupancy, - const RawDiscreteData& data) { - std::cout << name << ": occ=" << occupancy << " label=" << data.label - << " dynamic=" << std::boolalpha << data.dynamic - << " code=" << data.code << "\n"; -} - -int main() { - wavemap::HashedWaveletOctreeConfig config; - config.min_cell_width = 0.1f; - config.min_log_odds = -100.f; - config.max_log_odds = 100.f; - config.tree_height = 3; - - RawDiscreteLayeredMap map(config); - - const std::vector> samples = { - {wavemap::Index3D(1, 2, 3), 10.f, {1, false, 'A'}}, - {wavemap::Index3D(2, 2, 3), 20.f, {5, true, 'B'}}, - {wavemap::Index3D(3, 2, 3), 30.f, {9, false, 'C'}}, - }; - - const auto write_start = std::chrono::steady_clock::now(); - for (const auto& [index, occupancy, data] : samples) { - map.setOccupancy(index, occupancy); - map.setRawData(index, data); - } - const auto write_end = std::chrono::steady_clock::now(); - - size_t preserved_raw_values = 0u; - const auto read_start = std::chrono::steady_clock::now(); - for (const auto& [index, expected_occupancy, expected_data] : samples) { - const wavemap::FloatingPoint occupancy = map.getOccupancy(index); - const auto data = map.getRawData(index); - if (!data) { - std::cout << "missing raw data\n"; - continue; - } - printData("read", occupancy, *data); - if (*data == expected_data) { - ++preserved_raw_values; - } - } - const auto read_end = std::chrono::steady_clock::now(); - - const auto write_us = - std::chrono::duration_cast(write_end - - write_start) - .count(); - const auto read_us = - std::chrono::duration_cast(read_end - - read_start) - .count(); - - std::cout << "raw entries: " << map.rawDataSize() << "\n"; - std::cout << "preserved raw values: " << preserved_raw_values << "/" - << samples.size() << "\n"; - std::cout << "occupancy blocks: " << map.occupancyBlockCount() << "\n"; - std::cout << "occupancy nodes: " << map.occupancyNodeCount() << "\n"; - std::cout << "approx raw bytes without map overhead: " - << map.approximateRawBytesWithoutMapOverhead() << "\n"; - std::cout << "write time [us]: " << write_us << "\n"; - std::cout << "read time [us]: " << read_us << "\n"; - - return preserved_raw_values == samples.size() ? 0 : 1; -} diff --git a/examples/cpp/digital_twin/map_api/discrete/semantic_majority_exceptions_experiment.cc b/examples/cpp/digital_twin/map_api/discrete/semantic_majority_exceptions_experiment.cc deleted file mode 100644 index ac04963fc..000000000 --- a/examples/cpp/digital_twin/map_api/discrete/semantic_majority_exceptions_experiment.cc +++ /dev/null @@ -1,242 +0,0 @@ -#include -#include -#include -#include -#include -#include -#include - -#include "wavemap/core/indexing/ndtree_index.h" - -struct IndexKey { - int x = 0; - int y = 0; - int z = 0; - - explicit IndexKey(const wavemap::Index3D& index) - : x(index.x()), y(index.y()), z(index.z()) {} - - bool operator<(const IndexKey& other) const { - return std::tie(x, y, z) < std::tie(other.x, other.y, other.z); - } -}; - -struct BlockKey { - int x = 0; - int y = 0; - int z = 0; - - bool operator<(const BlockKey& other) const { - return std::tie(x, y, z) < std::tie(other.x, other.y, other.z); - } -}; - -struct LocalKey { - int x = 0; - int y = 0; - int z = 0; - - bool operator<(const LocalKey& other) const { - return std::tie(x, y, z) < std::tie(other.x, other.y, other.z); - } -}; - -struct ExceptionEntry { - LocalKey local_index; - int label = 0; -}; - -struct CompressedSemanticBlock { - int majority_label = 0; - std::vector exceptions; -}; - -class MajorityExceptionSemanticLayer { - public: - explicit MajorityExceptionSemanticLayer(int block_side) - : block_side_(block_side) {} - - void build(const std::map& raw_labels) { - std::map>> pending_blocks; - for (const auto& [index_key, label] : raw_labels) { - const wavemap::Index3D index(index_key.x, index_key.y, index_key.z); - pending_blocks[toBlockKey(index)].push_back({toLocalKey(index), label}); - } - - blocks_.clear(); - for (const auto& [block_key, entries] : pending_blocks) { - std::unordered_map counts; - for (const auto& [local_key, label] : entries) { - (void)local_key; - ++counts[label]; - } - - const auto majority_it = std::max_element( - counts.begin(), counts.end(), [](const auto& lhs, const auto& rhs) { - return lhs.second < rhs.second; - }); - - CompressedSemanticBlock block; - block.majority_label = majority_it->first; - for (const auto& [local_key, label] : entries) { - if (label != block.majority_label) { - block.exceptions.push_back({local_key, label}); - } - } - blocks_[block_key] = std::move(block); - } - } - - int getLabel(const wavemap::Index3D& index) const { - const BlockKey block_key = toBlockKey(index); - const auto block_it = blocks_.find(block_key); - if (block_it == blocks_.end()) { - return 0; - } - - const LocalKey local_key = toLocalKey(index); - const auto& block = block_it->second; - for (const ExceptionEntry& exception : block.exceptions) { - if (exception.local_index.x == local_key.x && - exception.local_index.y == local_key.y && - exception.local_index.z == local_key.z) { - return exception.label; - } - } - return block.majority_label; - } - - size_t numBlocks() const { return blocks_.size(); } - - size_t numExceptions() const { - size_t total = 0u; - for (const auto& [block_key, block] : blocks_) { - (void)block_key; - total += block.exceptions.size(); - } - return total; - } - - size_t approximateBytes() const { - return blocks_.size() * (sizeof(BlockKey) + sizeof(int)) + - numExceptions() * sizeof(ExceptionEntry); - } - - private: - const int block_side_; - std::map blocks_; - - BlockKey toBlockKey(const wavemap::Index3D& index) const { - return {floorDiv(index.x(), block_side_), floorDiv(index.y(), block_side_), - floorDiv(index.z(), block_side_)}; - } - - LocalKey toLocalKey(const wavemap::Index3D& index) const { - return {floorMod(index.x(), block_side_), floorMod(index.y(), block_side_), - floorMod(index.z(), block_side_)}; - } - - static int floorDiv(int value, int divisor) { - int quotient = value / divisor; - const int remainder = value % divisor; - if (remainder != 0 && ((remainder < 0) != (divisor < 0))) { - --quotient; - } - return quotient; - } - - static int floorMod(int value, int divisor) { - const int remainder = value % divisor; - return remainder < 0 ? remainder + divisor : remainder; - } -}; - -int makeMostlyHomogeneousLabel(int x, int y, int z) { - const int region_label = 1 + (x / 4) + 2 * (y / 4) + 4 * (z / 4); - const bool is_exception = ((3 * x + 5 * y + 7 * z) % 17) == 0; - if (!is_exception) { - return region_label; - } - return 20 + ((x + y + z) % 5); -} - -int makeNoisyLabel(int x, int y, int z) { - return 1 + ((3 * x + 5 * y + 7 * z) % 6); -} - -std::map makeRawLabels(bool noisy) { - std::map labels; - for (int x = 0; x < 8; ++x) { - for (int y = 0; y < 8; ++y) { - for (int z = 0; z < 4; ++z) { - const wavemap::Index3D index(x, y, z); - labels[IndexKey(index)] = noisy ? makeNoisyLabel(x, y, z) - : makeMostlyHomogeneousLabel(x, y, z); - } - } - } - return labels; -} - -void runScenario(const char* name, bool noisy) { - const std::map raw_labels = makeRawLabels(noisy); - MajorityExceptionSemanticLayer layer(/*block_side=*/4); - - const auto build_start = std::chrono::steady_clock::now(); - layer.build(raw_labels); - const auto build_end = std::chrono::steady_clock::now(); - - size_t preserved = 0u; - size_t printed_mismatches = 0u; - constexpr size_t kMaxPrintedMismatches = 5u; - - const auto read_start = std::chrono::steady_clock::now(); - for (const auto& [index_key, expected_label] : raw_labels) { - const wavemap::Index3D index(index_key.x, index_key.y, index_key.z); - const int read_label = layer.getLabel(index); - if (read_label == expected_label) { - ++preserved; - } else if (printed_mismatches < kMaxPrintedMismatches) { - std::cout << "mismatch at " << index.transpose() - << " expected=" << expected_label << " read=" << read_label - << "\n"; - ++printed_mismatches; - } - } - const auto read_end = std::chrono::steady_clock::now(); - - const auto build_us = - std::chrono::duration_cast(build_end - - build_start) - .count(); - const auto read_us = - std::chrono::duration_cast(read_end - - read_start) - .count(); - - const size_t raw_bytes = raw_labels.size() * (sizeof(IndexKey) + sizeof(int)); - const size_t compressed_bytes = layer.approximateBytes(); - - std::cout << "Scenario: " << name << "\n"; - std::cout << "samples: " << raw_labels.size() << "\n"; - std::cout << "preserved labels: " << preserved << "/" << raw_labels.size() - << "\n"; - std::cout << "blocks: " << layer.numBlocks() << "\n"; - std::cout << "exceptions: " << layer.numExceptions() << "\n"; - std::cout << "raw approx bytes: " << raw_bytes << "\n"; - std::cout << "majority+exceptions approx bytes: " << compressed_bytes - << "\n"; - std::cout << "approx compression ratio vs raw: " - << static_cast(compressed_bytes) / - static_cast(raw_bytes) - << "\n"; - std::cout << "build time [us]: " << build_us << "\n"; - std::cout << "read time [us]: " << read_us << "\n"; -} - -int main() { - runScenario("mostly homogeneous labels", false); - std::cout << "---\n"; - runScenario("noisy labels", true); - return 0; -} diff --git a/examples/cpp/digital_twin/map_api/user_facing/custom_layered_map_example.cc b/examples/cpp/digital_twin/map_api/user_facing/custom_layered_map_example.cc deleted file mode 100644 index 5a923c1d8..000000000 --- a/examples/cpp/digital_twin/map_api/user_facing/custom_layered_map_example.cc +++ /dev/null @@ -1,119 +0,0 @@ -#include -#include -#include -#include -#include - -#include "../../common/custom_layered_map_config.h" - -namespace { -bool almostEqual(float lhs, float rhs) { return std::abs(lhs - rhs) < 1e-5f; } - -template -bool sameValue(const std::optional& value, const ValueT& expected) { - return value && *value == expected; -} - -void printOptionalBool(const std::optional& value) { - if (value) { - std::cout << (*value ? "true" : "false"); - } else { - std::cout << "none"; - } -} -} // namespace - -int main() { - // 1. User code starts from its own config type. - MyLayeredMapConfig config = makeMyLayeredMapConfig(); - MyLayeredMap map(config); - - // 2. Continuous data lives inside the wavelet map together with occupancy. - const wavemap::Index3D tree_index(1, 2, 3); - const wavemap::Index3D road_index(2, 2, 3); - map.continuousMap().setVoxelValue( - tree_index, MyVoxel(/*occupancy=*/12.f, - MyContinuousLayers{/*temperature=*/22.5f, - /*intensity=*/0.75f})); - map.continuousMap().setVoxelValue( - road_index, MyVoxel(/*occupancy=*/8.f, - MyContinuousLayers{/*temperature=*/19.0f, - /*intensity=*/0.25f})); - - // 3. Discrete data is updated per voxel in the side discrete layer bundle. - map.discreteLayers().semantic_label.setValue(tree_index, 4); - map.discreteLayers().semantic_label.setValue(road_index, 7); - map.discreteLayers().manually_changed.setValue(tree_index, true); - map.discreteLayers().manually_changed.setValue(road_index, false); - - // 4. Continuous data still uses the Wavemap wavelet threshold/prune path. - map.continuousMap().threshold(); - map.continuousMap().prune(); - - const MyVoxel tree_voxel = map.continuousMap().getVoxelValue(tree_index); - const std::optional tree_label = - map.discreteLayers().semantic_label.getValue(tree_index); - const std::optional tree_changed = - map.discreteLayers().manually_changed.getValue(tree_index); - - std::cout << "User-facing LayeredMap example\n"; - std::cout << "continuous tree voxel: occupancy=" << tree_voxel.occupancy - << " temperature=" << tree_voxel.data.temperature - << " intensity=" << tree_voxel.data.intensity << "\n"; - std::cout << "discrete tree voxel: semantic_label=" - << (tree_label ? std::to_string(*tree_label) : "none") - << " manually_changed="; - printOptionalBool(tree_changed); - std::cout << "\n"; - - // 5. Full LayeredMap persistence stores continuous + discrete layers. - const std::filesystem::path output_dir = "/home/ci/data/maps"; - std::filesystem::create_directories(output_dir); - const std::filesystem::path output_file = - output_dir / "user_facing_layered_map_example.lwvmp"; - if (!MyLayeredMapIo::save(output_file, map)) { - std::cerr << "Failed to save LayeredMap to " << output_file << "\n"; - return 1; - } - - MyLayeredMap loaded_map(config); - if (!MyLayeredMapIo::load(output_file, loaded_map)) { - std::cerr << "Failed to load LayeredMap from " << output_file << "\n"; - return 1; - } - - const MyVoxel loaded_tree_voxel = - loaded_map.continuousMap().getVoxelValue(tree_index); - const bool continuous_ok = - almostEqual(tree_voxel.occupancy, loaded_tree_voxel.occupancy) && - almostEqual(tree_voxel.data.temperature, - loaded_tree_voxel.data.temperature) && - almostEqual(tree_voxel.data.intensity, loaded_tree_voxel.data.intensity); - const bool discrete_ok = - sameValue(loaded_map.discreteLayers().semantic_label.getValue(tree_index), - 4) && - sameValue(loaded_map.discreteLayers().semantic_label.getValue(road_index), - 7) && - sameValue( - loaded_map.discreteLayers().manually_changed.getValue(tree_index), - true) && - sameValue( - loaded_map.discreteLayers().manually_changed.getValue(road_index), - false); - const bool unknown_ok = - !loaded_map.discreteLayers().semantic_label.getValue( - wavemap::Index3D(100, 100, 100)); - - std::cout << "saved file: " << output_file << "\n"; - std::cout << "continuous values preserved after load: " - << (continuous_ok ? "yes" : "no") << "\n"; - std::cout << "discrete values preserved after load: " - << (discrete_ok ? "yes" : "no") << "\n"; - std::cout << "unknown discrete voxel remains unknown: " - << (unknown_ok ? "yes" : "no") << "\n"; - std::cout << "semantic parent cells: " - << loaded_map.discreteLayers().semantic_label.parentCount() - << "\n"; - - return continuous_ok && discrete_ok && unknown_ok ? 0 : 1; -} diff --git a/examples/cpp/digital_twin/map_api/user_facing/field_endpoint_integration_experiment.cc b/examples/cpp/digital_twin/map_api/user_facing/field_endpoint_integration_experiment.cc deleted file mode 100644 index 1066c15c2..000000000 --- a/examples/cpp/digital_twin/map_api/user_facing/field_endpoint_integration_experiment.cc +++ /dev/null @@ -1,68 +0,0 @@ -#include -#include -#include - -#include -#include -#include -#include - -#include "../../common/field_layered_map_config.h" - -namespace field_map = wavemap::examples::field_map; - -int main() { - const auto map_config = field_map::makeDefaultConfig(); - field_map::Map layered_map(map_config); - auto baseline_map = std::make_shared( - map_config.continuous_map); - wavemap::RayTracingIntegratorConfig config; - config.min_range = 0.1f; - config.max_range = 20.f; - wavemap::RayTracingIntegrator occupancy_integrator( - config, layered_map.continuousMapPtr()); - wavemap::RayTracingIntegrator baseline_integrator(config, baseline_map); - - using Measurement = wavemap::layered::PosedEndpointAttributePointcloud< - field_map::ReflectivityLayer>; - Measurement measurement{wavemap::Transformation3D()}; - measurement.resize(2u); - measurement.point(0u) = wavemap::Point3D(1.1f, 0.1f, 0.1f); - measurement.point(1u) = wavemap::Point3D(1.1f, 1.1f, 0.1f); - measurement.attribute(0u) = 0.25f; - measurement.attribute(1u) = 0.75f; - - wavemap::PosedPointcloud<> baseline_measurement( - measurement.getPose(), measurement.points().data()); - baseline_integrator.integrate(baseline_measurement); - wavemap::layered::EndpointAttributeIntegrator< - field_map::Map, field_map::ReflectivityLayer> - integrator(layered_map, occupancy_integrator); - const auto result = integrator.integrate(measurement); - - const float cell_width_inv = - 1.f / layered_map.continuousMap().getMinCellWidth(); - bool occupancy_matches = true; - bool reflectivity_matches = true; - for (size_t i = 0u; i < measurement.size(); ++i) { - const wavemap::Point3D endpoint = measurement.point(i); - const auto index = - wavemap::convert::pointToNearestIndex(endpoint, cell_width_inv); - occupancy_matches &= - std::abs(layered_map.continuousMap().getCellValue(index) - - baseline_map->getCellValue(index)) < 1e-6f; - const auto voxel = layered_map.continuousMap().getVoxelValue(index); - reflectivity_matches &= - std::abs(voxel.data.template get() - - measurement.attribute(i)) < - 1e-6f; - } - const bool pass = result.integrated == 2u && occupancy_matches && - reflectivity_matches; - std::cout << "Field endpoint integration Phase 9 experiment\n" - << " original occupancy behavior preserved: " - << (occupancy_matches ? "pass" : "FAIL") << '\n' - << " reflectivity updated at matching endpoints: " - << (reflectivity_matches ? "pass" : "FAIL") << '\n'; - return pass ? 0 : 1; -} diff --git a/examples/cpp/digital_twin/map_api/user_facing/field_layered_map_experiment.cc b/examples/cpp/digital_twin/map_api/user_facing/field_layered_map_experiment.cc deleted file mode 100644 index ad8862845..000000000 --- a/examples/cpp/digital_twin/map_api/user_facing/field_layered_map_experiment.cc +++ /dev/null @@ -1,107 +0,0 @@ -#include -#include -#include -#include - -#include - -#include "../../common/field_layered_map_config.h" - -namespace field_map = wavemap::examples::field_map; - -namespace { -bool approximatelyEqual(float lhs, float rhs, float tolerance = 1e-5f) { - return std::abs(lhs - rhs) <= tolerance; -} - -bool expect(const std::string& label, bool condition) { - std::cout << " " << label << ": " << (condition ? "pass" : "FAIL") - << "\n"; - return condition; -} -} // namespace - -int main() { - field_map::Map map(field_map::makeDefaultConfig()); - const wavemap::Index3D index(2, -1, 3); - map.continuousMap().setVoxelValue( - index, field_map::Voxel( - 0.7f, field_map::makeContinuousLayers(0.2f))); - - const field_map::Voxel before = map.continuousMap().getVoxelValue(index); - const wavemap::layered::LayerObservation observation( - wavemap::Point3D::Zero(), 1.2f); - const field_map::ReflectivityUpdatePolicy policy; - wavemap::layered::updateContinuousLayersAtVoxel( - map.continuousMap(), index, [&](field_map::ContinuousLayers& layers) { - auto& reflectivity = layers.get(); - reflectivity = policy(reflectivity, observation); - }); - - map.discreteLayers().get().setValue( - index, static_cast(field_map::ClassLabel::kObstacle)); - map.continuousMap().threshold(); - - const field_map::Voxel after = map.continuousMap().getVoxelValue(index); - const auto& configured_reflectivity_bounds = - map.continuousMap() - .getThresholdConfig() - .data.get(); - const auto class_value = - map.discreteLayers().get().getValue(index); - const auto schema = wavemap::layered::io::layeredMapSchema(map); - - bool ok = true; - std::cout << "Field LayeredMap generated-schema experiment\n"; - ok &= expect("occupancy preserved", - approximatelyEqual(before.occupancy, after.occupancy)); - ok &= expect("reflectivity thresholded to configured range", - approximatelyEqual( - after.data.get(), - configured_reflectivity_bounds.max)); - ok &= expect("class stored outside wavelet map", - class_value && - *class_value == - static_cast(field_map::ClassLabel::kObstacle)); - ok &= expect("schema has occupancy and reflectivity", - schema.continuous_layers.size() == 2u && - schema.continuous_layers[1].name == "reflectivity"); - ok &= expect("schema has one class layer", - schema.discrete_layers.size() == 1u && - schema.discrete_layers.front().name == "class"); - - const std::filesystem::path file_path = - "/tmp/field_layered_map_generated_schema.lwvmp"; - field_map::Map loaded(field_map::makeDefaultConfig()); - const auto* const continuous_map_before_load = - loaded.continuousMapPtr().get(); - const bool saved_and_loaded = - field_map::MapIo::save(file_path, map) && - field_map::MapIo::load(file_path, loaded); - const auto loaded_voxel = loaded.continuousMap().getVoxelValue(index); - const auto loaded_class = - loaded.discreteLayers().get().getValue(index); - ok &= expect( - "persistence round trip", - saved_and_loaded && - approximatelyEqual( - loaded_voxel.data.get(), - configured_reflectivity_bounds.max) && - loaded_class && - *loaded_class == - static_cast(field_map::ClassLabel::kObstacle)); - ok &= expect("load preserves continuous map allocation", - loaded.continuousMapPtr().get() == - continuous_map_before_load); - const auto& loaded_reflectivity_bounds = - loaded.continuousMap() - .getThresholdConfig() - .data.get(); - ok &= expect("load preserves runtime layer thresholds", - approximatelyEqual(loaded_reflectivity_bounds.min, - configured_reflectivity_bounds.min) && - approximatelyEqual(loaded_reflectivity_bounds.max, - configured_reflectivity_bounds.max)); - - return ok ? 0 : 1; -} diff --git a/examples/cpp/digital_twin/map_api/user_facing/field_layered_pipeline_experiment.cc b/examples/cpp/digital_twin/map_api/user_facing/field_layered_pipeline_experiment.cc deleted file mode 100644 index 5caf171b3..000000000 --- a/examples/cpp/digital_twin/map_api/user_facing/field_layered_pipeline_experiment.cc +++ /dev/null @@ -1,80 +0,0 @@ -#include -#include -#include -#include -#include - -#include -#include - -#include "../../common/field_layered_map_config.h" - -namespace field_map = wavemap::examples::field_map; - -namespace { -bool approximatelyEqual(float lhs, float rhs, float tolerance = 1e-5f) { - return std::abs(lhs - rhs) <= tolerance; -} - -bool expect(const std::string& label, bool condition) { - std::cout << " " << label << ": " << (condition ? "pass" : "FAIL") - << "\n"; - return condition; -} -} // namespace - -int main() { - field_map::Map map(field_map::makeDefaultConfig()); - wavemap::layered::LayeredPipeline pipeline(map); - - const wavemap::Point3D point(0.7f, -0.1f, 0.8f); - const wavemap::Index3D index = wavemap::convert::pointToNearestIndex( - point, 1.f / map.continuousMap().getMinCellWidth()); - map.continuousMap().setVoxelValue( - index, field_map::Voxel( - 0.6f, field_map::makeContinuousLayers(0.2f))); - - const std::vector> - reflectivity_observations{ - {point, 0.4f}, - {point, 0.65f}, - {wavemap::Point3D( - std::numeric_limits::quiet_NaN(), 0.f, 0.f), - 0.9f}}; - const auto reflectivity_result = - pipeline.integrate( - reflectivity_observations); - - const std::vector> - class_observations{ - {point, - static_cast(field_map::ClassLabel::kGround)}, - {point, - static_cast(field_map::ClassLabel::kObstacle)}}; - const auto class_result = - pipeline.integrate(class_observations); - - const auto voxel = map.continuousMap().getVoxelValue(index); - const auto class_value = - map.discreteLayers().get().getValue(index); - - bool ok = true; - std::cout << "Field LayeredMap Phase 3 pipeline experiment\n"; - ok &= expect("world position converted to voxel index", - reflectivity_result.updated_voxels == 1u); - ok &= expect("continuous observation integrated", - approximatelyEqual(voxel.data.get(), 0.65f)); - ok &= expect("occupancy preserved", - approximatelyEqual(voxel.occupancy, 0.6f)); - ok &= expect("invalid position rejected", - reflectivity_result.received == 3u && - reflectivity_result.integrated == 2u && - reflectivity_result.rejected == 1u); - ok &= expect( - "discrete policy and storage updated", - class_result.integrated == 2u && class_value && - *class_value == - static_cast(field_map::ClassLabel::kObstacle)); - - return ok ? 0 : 1; -} diff --git a/examples/cpp/digital_twin/old_experiments/blocks_experiment.cc b/examples/cpp/digital_twin/old_experiments/blocks_experiment.cc deleted file mode 100644 index 149ff2ea0..000000000 --- a/examples/cpp/digital_twin/old_experiments/blocks_experiment.cc +++ /dev/null @@ -1,124 +0,0 @@ -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include - -namespace { -template -bool publishMap(const HashedMapT& map, ros::Publisher& map_pub, - const std::string& map_frame, const std::string& label) { - wavemap_msgs::Map map_msg; - if (!wavemap::convert::mapToRosMsg(map, map_frame, ros::Time::now(), - map_msg)) { - std::cerr << "Failed to convert map to ROS msg for: " << label << "\n"; - return false; - } - - map_pub.publish(map_msg); - ros::spinOnce(); - std::cout << "Published " << label << " on " << map_pub.getTopic() << "\n"; - return true; -} - -template -bool eraseBlocksAndPublish(HashedMapT& map, const std::string& map_type_name, - ros::Publisher& map_pub, - const std::string& map_frame) { - std::cout << "Loaded map type: " << map_type_name << "\n"; - std::cout << "Initial block count: " << map.getHashMap().size() << "\n"; - - if (!publishMap(map, map_pub, map_frame, "original map")) { - return false; - } - - const std::vector blocks_to_erase{ - {0, 0, 0}, {1, 0, 0}, {1, 1, 0}, {0, 1, 0}, - {-1, 1, 0}, {-1, 0, 0}, {-1, -1, 0}, {0, -1, 0}, - {1, -1, 0}, {2, -1, 0}, {2, 0, 0}, {2, 1, 0}, - {2, 2, 0}, {1, 2, 0}, {0, 2, 0}, {-1, 2, 0}, - {-2, 2, 0}, {-2, 1, 0}, {-2, 0, 0}, {-2, -1, 0}, - {-2, -2, 0}, {-1, -2, 0}, {0, -2, 0}, {1, -2, 0}, - {2, -2, 0}, - }; - - for (const wavemap::Index3D& block_to_erase : blocks_to_erase) { - ros::Duration(1.0).sleep(); - - std::cout << "Erasing block " << block_to_erase.transpose() - << " | existed before erase: " << std::boolalpha - << map.hasBlock(block_to_erase) << "\n"; - const bool erased = map.eraseBlock(block_to_erase); - std::cout << " erased: " << erased - << " | remaining block count: " << map.getHashMap().size() - << "\n"; - - const std::string label = - "map after erasing block " + std::to_string(block_to_erase.x()) + " " + - std::to_string(block_to_erase.y()) + " " + - std::to_string(block_to_erase.z()); - if (!publishMap(map, map_pub, map_frame, label)) { - return false; - } - } - - return true; -} -} // namespace - -int main(int argc, char** argv) { - ros::init(argc, argv, "wavemap_blocks_experiment"); - ros::NodeHandle nh; - - const std::filesystem::path input_path = "/home/ci/data/maps/wavemap1.wvmp"; - const std::string map_topic = "/wavemap/map"; - const std::string map_frame = "map"; - - const int queue_size = 1; - const bool latch = true; - ros::Publisher map_pub = - nh.advertise(map_topic, queue_size, latch); - ros::Duration(0.5).sleep(); - - wavemap::MapBase::Ptr map; - if (!wavemap::io::fileToMap(input_path, map)) { - std::cerr << "Failed to load map from: " << input_path << "\n"; - return 1; - } - - if (!map) { - std::cerr << "Loaded map pointer is null.\n"; - return 1; - } - - if (auto hashed_chunked_map = - std::dynamic_pointer_cast( - map)) { - return eraseBlocksAndPublish(*hashed_chunked_map, - "hashed_chunked_wavelet_octree", map_pub, - map_frame) - ? 0 - : 1; - } - - if (auto hashed_map = - std::dynamic_pointer_cast(map)) { - return eraseBlocksAndPublish(*hashed_map, "hashed_wavelet_octree", map_pub, - map_frame) - ? 0 - : 1; - } - - std::cerr << "Loaded map does not expose hashed block APIs.\n"; - std::cerr << "Use a hashed_blocks, hashed_wavelet_octree, or " - "hashed_chunked_wavelet_octree map.\n"; - return 1; -} diff --git a/examples/cpp/digital_twin/old_experiments/experiment.cc b/examples/cpp/digital_twin/old_experiments/experiment.cc deleted file mode 100644 index a596cf20d..000000000 --- a/examples/cpp/digital_twin/old_experiments/experiment.cc +++ /dev/null @@ -1,58 +0,0 @@ -#include -#include - -#include -#include -#include -#include - -int main() { - const std::filesystem::path input_path = "/home/ci/data/maps/wavemap1.wvmp"; - const std::filesystem::path output_path = "/home/ci/data/maps/map_copy.wvmp"; - - wavemap::MapBase::Ptr map; - - if (!wavemap::io::fileToMap(input_path, map)) { - std::cerr << "Failed to load map from: " << input_path << "\n"; - return 1; - } - - if (!map) { - std::cerr << "Loaded map pointer is null.\n"; - return 1; - } - std::cout << "Loaded map from: " << input_path << "\n"; - - map->threshold(); - - size_t occupied_like = 0; - size_t free_like = 0; - - map->forEachLeaf([&](const wavemap::OctreeIndex&, float value) { - if (value > 0.f) { - ++occupied_like; - } else if (value < 0.f) { - ++free_like; - } - }); - - std::cout << "Occupied-like leaves: " << occupied_like << "\n"; - std::cout << "Free-like leaves: " << free_like << "\n"; - - if (std::dynamic_pointer_cast(map)) { - std::cout << "Loaded as hashed_wavelet_octree\n"; - } - - if (std::dynamic_pointer_cast(map)) { - std::cout << "Loaded as hashed_chunked_wavelet_octree\n"; - } - - if (!wavemap::io::mapToFile(*map, output_path)) { - std::cerr << "Failed to save map to: " << output_path << "\n"; - return 1; - } - - std::cout << "Saved map to: " << output_path << "\n"; - - return 0; -} diff --git a/examples/cpp/digital_twin/old_experiments/partial_save_experiment.cc b/examples/cpp/digital_twin/old_experiments/partial_save_experiment.cc deleted file mode 100644 index 79b1f9839..000000000 --- a/examples/cpp/digital_twin/old_experiments/partial_save_experiment.cc +++ /dev/null @@ -1,111 +0,0 @@ -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include - -namespace { -bool keepBlock(const wavemap::Index3D& block_index) { - return block_index.x() <= 1 && block_index.x() >= -1 && block_index.y() <= 1 && block_index.y() >= -1 && block_index.z() <= 1 && block_index.z() >= -1; -} - -template -bool publishMap(const MapT& map, ros::Publisher& map_pub, const std::string& map_frame, const std::string& label) { - wavemap_msgs::Map map_msg; - if (!wavemap::convert::mapToRosMsg(map, map_frame, ros::Time::now(), map_msg)) { - std::cerr << "Failed to convert " << label << " to ROS msg.\n"; - return false; - } - - map_pub.publish(map_msg); - ros::spinOnce(); - std::cout << "Published " << label << " on " << map_pub.getTopic() << "\n"; - return true; -} - -template -bool savePartialMap(HashedMapT& map, const std::filesystem::path& output_path) { - const size_t initial_block_count = map.getHashMap().size(); - - map.eraseBlockIf([](const wavemap::Index3D& block_index, const auto& /*block*/) { return !keepBlock(block_index); }); - - const size_t final_block_count = map.getHashMap().size(); - std::cout << "Kept " << final_block_count << " of " << initial_block_count << " blocks.\n"; - - if (!wavemap::io::mapToFile(map, output_path)) { - std::cerr << "Failed to save partial map to: " << output_path << "\n"; - return false; - } - - std::cout << "Saved partial map to: " << output_path << "\n"; - return true; -} - -template -int runExperiment(HashedMapT& map, ros::Publisher& map_pub, const std::string& map_frame, const std::filesystem::path& output_path, const std::string& map_type_name) { - std::cout << "Loaded map type: " << map_type_name << "\n"; - std::cout << "Original block count: " << map.getHashMap().size() << "\n"; - - if (!publishMap(map, map_pub, map_frame, "original map")) { - return 1; - } - - std::cout << "Showing original map...\n"; - ros::Duration(5.0).sleep(); - - if (!savePartialMap(map, output_path)) { - return 1; - } - - if (!publishMap(map, map_pub, map_frame, "partial map")) { - return 1; - } - - ros::spin(); - return 0; -} -} // namespace - -int main(int argc, char** argv) { - ros::init(argc, argv, "wavemap_partial_save_experiment"); - ros::NodeHandle nh; - - const std::filesystem::path input_path = "/home/ci/data/maps/wavemap1.wvmp"; - const std::filesystem::path output_path = "/home/ci/data/maps/wavemap_blocks.wvmp"; - const std::string map_topic = "/wavemap/map"; - const std::string map_frame = "map"; - - const int queue_size = 1; - const bool latch = true; - ros::Publisher map_pub = nh.advertise(map_topic, queue_size, latch); - ros::Duration(0.5).sleep(); - - wavemap::MapBase::Ptr map; - if (!wavemap::io::fileToMap(input_path, map)) { - std::cerr << "Failed to load map from: " << input_path << "\n"; - return 1; - } - - if (!map) { - std::cerr << "Loaded map pointer is null.\n"; - return 1; - } - - if (auto hashed_chunked_map = std::dynamic_pointer_cast(map)) { - return runExperiment(*hashed_chunked_map, map_pub, map_frame, output_path, "hashed_chunked_wavelet_octree"); - } - - if (auto hashed_map = std::dynamic_pointer_cast(map)) { - return runExperiment(*hashed_map, map_pub, map_frame, output_path, "hashed_wavelet_octree"); - } - - std::cerr << "Loaded map does not expose hashed block APIs.\n"; - std::cerr << "Use a hashed_wavelet_octree or hashed_chunked_wavelet_octree map.\n"; - return 1; -} diff --git a/examples/cpp/digital_twin/old_experiments/publish_experiment.cc b/examples/cpp/digital_twin/old_experiments/publish_experiment.cc deleted file mode 100644 index ae177b5be..000000000 --- a/examples/cpp/digital_twin/old_experiments/publish_experiment.cc +++ /dev/null @@ -1,84 +0,0 @@ -#include -#include -#include - -#include -#include -#include -#include -#include - -int main(int argc, char** argv) { - ros::init(argc, argv, "wavemap_publish_experiment"); - ros::NodeHandle nh; - - const std::filesystem::path input_path = "/home/ci/data/maps/wavemap1.wvmp"; - const std::string map_topic = "/wavemap/map"; - const std::string map_frame = "map"; - const std::filesystem::path output_path = - "/home/ci/data/maps/wavemap1_updated.wvmp"; - - wavemap::MapBase::Ptr map; - if (!wavemap::io::fileToMap(input_path, map)) { - std::cerr << "Failed to load map from: " << input_path << "\n"; - return 1; - } - - if (!map) { - std::cerr << "Loaded map pointer is null.\n"; - return 1; - } - - wavemap_msgs::Map map_msg; - if (!wavemap::convert::mapToRosMsg(*map, map_frame, ros::Time::now(), - map_msg)) { - std::cerr << "Failed to convert loaded map to wavemap_msgs::Map.\n"; - return 1; - } - - const int queue_size = 1; - const bool latch = true; - ros::Publisher map_pub = - nh.advertise(map_topic, queue_size, latch); - - ros::Duration(0.5).sleep(); - map_pub.publish(map_msg); - ros::spinOnce(); - - std::cout << "Before changes " << input_path << "\n"; - - ros::Duration(5.0).sleep(); - for (int x = 0; x <= 10; ++x) { - for (int y = 0; y <= 10; ++y) { - for (int z = 0; z <= 10; ++z) { - wavemap::Index3D index(x, y, z); - map->setCellValue(index, 5.0f); - } - } - } - - wavemap_msgs::Map updated_msg; - if (!wavemap::convert::mapToRosMsg(*map, map_frame, ros::Time::now(), - updated_msg)) { - std::cerr << "Failed to convert updated map to wavemap_msgs::Map.\n"; - return 1; - } - map_pub.publish(updated_msg); - - std::cout << "After changes " << input_path << "\n"; - - std::cout << "Loaded map from: " << input_path << "\n"; - std::cout << "Published map on: " << map_topic << "\n"; - std::cout << "RViz fixed frame should be: " << map_frame << "\n"; - - if (!wavemap::io::mapToFile(*map, output_path)) { - std::cerr << "Failed to save map to: " << output_path << "\n"; - return 1; - } - - std::cout << "Saved modified map to: " << output_path << "\n"; - - ros::spin(); - - return 0; -} diff --git a/examples/cpp/digital_twin/ros/field_layer_update_ros_experiment.cc b/examples/cpp/digital_twin/ros/field_layer_update_ros_experiment.cc deleted file mode 100644 index a6621c3ba..000000000 --- a/examples/cpp/digital_twin/ros/field_layer_update_ros_experiment.cc +++ /dev/null @@ -1,114 +0,0 @@ -#include -#include -#include - -#include -#include -#include -#include - -#include "../common/field_layered_map_ros_config.h" - -namespace field_map = wavemap::examples::field_map; - -namespace { -bool approximatelyEqual(float lhs, float rhs, float tolerance = 1e-5f) { - return std::abs(lhs - rhs) <= tolerance; -} - -bool expect(const std::string& label, bool condition) { - std::cout << " " << label << ": " << (condition ? "pass" : "FAIL") - << "\n"; - return condition; -} - -geometry_msgs::Point32 point(float x, float y, float z) { - geometry_msgs::Point32 result; - result.x = x; - result.y = y; - result.z = z; - return result; -} -} // namespace - -int main() { - field_map::Map map(field_map::makeDefaultConfig()); - wavemap::layered::LayeredPipeline pipeline(map); - - wavemap_msgs::LayeredMapUpdate update; - update.header.frame_id = "map"; - update.positions = {point(0.1f, 0.1f, 0.1f), - point(-0.4f, 0.2f, 0.6f)}; - - wavemap_msgs::Layer reflectivity; - reflectivity.name = "reflectivity"; - reflectivity.type = "float32"; - reflectivity.float32_values = {0.25f, 0.8f}; - update.layers.emplace_back(reflectivity); - - wavemap_msgs::Layer class_layer; - class_layer.name = "class"; - class_layer.type = "int32"; - class_layer.int32_values = { - static_cast(field_map::ClassLabel::kGround), - static_cast(field_map::ClassLabel::kObstacle)}; - update.layers.emplace_back(class_layer); - - std::string error; - const bool converted = - wavemap::convert::DescriptorLayerObservationRosConverter< - field_map::Map>::integrate(update, pipeline, &error); - - const float cell_width_inv = - 1.f / map.continuousMap().getMinCellWidth(); - const auto first_index = wavemap::convert::pointToNearestIndex( - wavemap::Point3D(0.1f, 0.1f, 0.1f), cell_width_inv); - const auto second_index = wavemap::convert::pointToNearestIndex( - wavemap::Point3D(-0.4f, 0.2f, 0.6f), cell_width_inv); - - bool ok = true; - std::cout << "Field layered ROS update Phase 4 experiment\n"; - ok &= expect("valid subset converted", converted && error.empty()); - ok &= expect( - "continuous layer routed through pipeline", - approximatelyEqual( - map.continuousMap().getVoxelValue(first_index).data.get(), 0.25f) && - approximatelyEqual( - map.continuousMap().getVoxelValue(second_index).data.get(), - 0.8f)); - const auto first_class = - map.discreteLayers().get().getValue(first_index); - const auto second_class = - map.discreteLayers().get().getValue(second_index); - ok &= expect( - "discrete layer routed through pipeline", - first_class && second_class && - *first_class == - static_cast(field_map::ClassLabel::kGround) && - *second_class == - static_cast(field_map::ClassLabel::kObstacle)); - - wavemap_msgs::LayeredMapUpdate malformed = update; - malformed.layers.front().float32_values = {0.9f}; - error.clear(); - const bool malformed_accepted = - wavemap::convert::DescriptorLayerObservationRosConverter< - field_map::Map>::integrate(malformed, pipeline, &error); - ok &= expect( - "malformed batch rejected without partial update", - !malformed_accepted && !error.empty() && - approximatelyEqual( - map.continuousMap().getVoxelValue(first_index).data.get(), - 0.25f)); - - wavemap_msgs::LayeredMapUpdate unknown = update; - unknown.layers.front().name = "not_in_schema"; - error.clear(); - const bool unknown_accepted = - wavemap::convert::DescriptorLayerObservationRosConverter< - field_map::Map>::integrate(unknown, pipeline, &error); - ok &= expect("unknown layer rejected", - !unknown_accepted && !error.empty()); - - return ok ? 0 : 1; -} diff --git a/examples/cpp/digital_twin/ros/field_layered_map_ros_experiment.cc b/examples/cpp/digital_twin/ros/field_layered_map_ros_experiment.cc deleted file mode 100644 index 600a1009f..000000000 --- a/examples/cpp/digital_twin/ros/field_layered_map_ros_experiment.cc +++ /dev/null @@ -1,54 +0,0 @@ -#include -#include - -#include -#include - -#include "../common/field_layered_map_ros_config.h" - -namespace field_map = wavemap::examples::field_map; -namespace field_map_ros = wavemap::examples::field_map::ros_config; - -namespace { -bool approximatelyEqual(float lhs, float rhs, float tolerance = 1e-5f) { - return std::abs(lhs - rhs) <= tolerance; -} -} // namespace - -int main() { - field_map::Map original(field_map::makeDefaultConfig()); - const wavemap::Index3D index(1, 2, 3); - original.continuousMap().setVoxelValue( - index, field_map::Voxel( - 0.8f, field_map::makeContinuousLayers(0.7f))); - original.discreteLayers().get().setValue( - index, static_cast(field_map::ClassLabel::kObstacle)); - - wavemap_msgs::LayeredMap message; - if (!wavemap::convert::layeredMapToRosMsg< - field_map::Map, field_map_ros::VoxelRosConverter>( - original, "map", ros::Time(10), message)) { - return 1; - } - - field_map::Map reconstructed(field_map::makeDefaultConfig()); - if (!wavemap::convert::rosMsgToLayeredMap< - field_map::Map, field_map_ros::VoxelRosConverter>( - message, reconstructed)) { - return 2; - } - - const auto voxel = reconstructed.continuousMap().getVoxelValue(index); - const auto class_value = - reconstructed.discreteLayers().get().getValue(index); - const bool ok = - message.header.frame_id == "map" && - message.continuous_map.layered_hashed_wavelet_octree.size() == 1u && - approximatelyEqual(voxel.data.get(), 0.7f) && - class_value && - *class_value == static_cast(field_map::ClassLabel::kObstacle); - - std::cout << "Field LayeredMap ROS round trip: " - << (ok ? "pass" : "FAIL") << "\n"; - return ok ? 0 : 3; -} diff --git a/examples/cpp/digital_twin/ros/layered_map_structured_ros_msg_experiment.cc b/examples/cpp/digital_twin/ros/layered_map_structured_ros_msg_experiment.cc deleted file mode 100644 index b277a3e01..000000000 --- a/examples/cpp/digital_twin/ros/layered_map_structured_ros_msg_experiment.cc +++ /dev/null @@ -1,105 +0,0 @@ -#include -#include -#include -#include - -#include "../common/example_layered_map_ros_config.h" -#include "../common/layered_ros_converter.h" - -namespace { -bool almostEqual(float lhs, float rhs) { - return std::abs(lhs - rhs) < 1e-5f; -} - -bool almostEqual(const LayeredVoxel& lhs, const LayeredVoxel& rhs) { - return almostEqual(lhs.occupancy, rhs.occupancy) && - almostEqual(lhs.data.rgb.r, rhs.data.rgb.r) && - almostEqual(lhs.data.rgb.g, rhs.data.rgb.g) && - almostEqual(lhs.data.rgb.b, rhs.data.rgb.b) && - almostEqual(lhs.data.traversability, rhs.data.traversability); -} - -template -bool sameValue(const std::optional& value, const ValueT& expected) { - return value && *value == expected; -} -} // namespace - -int main() { - ExampleLayeredMapConfig config; - config.continuous_map.min_cell_width = 0.1f; - config.continuous_map.min_log_odds = -2.f; - config.continuous_map.max_log_odds = 4.f; - config.continuous_map.tree_height = 3; - config.discrete_compression.block_height = 1; - - ExampleLayeredMap map(config); - std::vector populated_indices; - populateLayeredCube(map.continuousMap(), populated_indices); - - const wavemap::Index3D first_index = populated_indices.front(); - const wavemap::Index3D middle_index = populated_indices[populated_indices.size() / 2u]; - const wavemap::Index3D last_index = populated_indices.back(); - - map.discreteLayers().semantic.setValue(first_index, 1); - map.discreteLayers().semantic.setValue(middle_index, 2); - map.discreteLayers().semantic.setValue(last_index, 1); - map.discreteLayers().changed.setValue(first_index, false); - map.discreteLayers().changed.setValue(middle_index, true); - map.discreteLayers().changed.setValue(last_index, false); - - wavemap_msgs::LayeredMap msg; - if (!wavemap::convert::layeredMapToRosMsg( - map, "map", ros::Time(1, 0), msg)) { - std::cerr << "Failed to convert LayeredMap to ROS message.\n"; - return 1; - } - - ExampleLayeredMap loaded_map(config); - if (!wavemap::convert::rosMsgToLayeredMap( - msg, loaded_map)) { - std::cerr << "Failed to convert ROS message back to LayeredMap.\n"; - return 1; - } - - size_t verified_continuous = 0u; - for (const wavemap::Index3D& voxel_index : populated_indices) { - if (almostEqual(map.continuousMap().getVoxelValue(voxel_index), - loaded_map.continuousMap().getVoxelValue(voxel_index))) { - ++verified_continuous; - } - } - - const bool semantic_ok = - sameValue(loaded_map.discreteLayers().semantic.getValue(first_index), 1) && - sameValue(loaded_map.discreteLayers().semantic.getValue(middle_index), 2) && - sameValue(loaded_map.discreteLayers().semantic.getValue(last_index), 1); - const bool changed_ok = - sameValue(loaded_map.discreteLayers().changed.getValue(first_index), false) && - sameValue(loaded_map.discreteLayers().changed.getValue(middle_index), true) && - sameValue(loaded_map.discreteLayers().changed.getValue(last_index), false); - const bool unknown_ok = - !loaded_map.discreteLayers().semantic.getValue(wavemap::Index3D(0, 0, 0)); - - std::cout << "LayeredMap ROS message experiment\n"; - std::cout << "frame: " << msg.header.frame_id << "\n"; - std::cout << "continuous ROS map blocks: " - << msg.continuous_map.layered_hashed_wavelet_octree.front().blocks.size() - << "\n"; - std::cout << "discrete ROS layers: " << msg.discrete_layers.size() << "\n"; - std::cout << "continuous voxels verified: " << verified_continuous << "/" - << populated_indices.size() << "\n"; - std::cout << "semantic labels preserved: " << (semantic_ok ? "yes" : "no") - << "\n"; - std::cout << "changed flags preserved: " << (changed_ok ? "yes" : "no") - << "\n"; - std::cout << "unknown discrete voxel remains unknown: " - << (unknown_ok ? "yes" : "no") << "\n"; - - return verified_continuous == populated_indices.size() && semantic_ok && - changed_ok && unknown_ok - ? 0 - : 1; -} diff --git a/examples/cpp/digital_twin/ros/layered_ros_msg_experiment.cc b/examples/cpp/digital_twin/ros/layered_ros_msg_experiment.cc deleted file mode 100644 index 3599d31bf..000000000 --- a/examples/cpp/digital_twin/ros/layered_ros_msg_experiment.cc +++ /dev/null @@ -1,71 +0,0 @@ -#include -#include -#include - -#include - -#include "../common/layered_ros_converter.h" - -namespace { -bool almostEqual(float lhs, float rhs) { - return std::abs(lhs - rhs) < 1e-5f; -} - -bool almostEqual(const LayeredVoxel& lhs, const LayeredVoxel& rhs) { - return almostEqual(lhs.occupancy, rhs.occupancy) && almostEqual(lhs.data.rgb.r, rhs.data.rgb.r) && almostEqual(lhs.data.rgb.g, rhs.data.rgb.g) && almostEqual(lhs.data.rgb.b, rhs.data.rgb.b) && almostEqual(lhs.data.traversability, rhs.data.traversability); -} -} // namespace - -int main() { - wavemap::HashedWaveletOctreeConfig config; - config.min_cell_width = 0.1f; - config.min_log_odds = -2.f; - config.max_log_odds = 4.f; - config.tree_height = 3; - - ContinuousWaveletMap map(config); - std::vector populated_indices; - populateLayeredCube(map, populated_indices); - - // Convert the layered C++ map into the standard wavemap_msgs::Map wrapper, matching the normal wavemap publishing path. - wavemap_msgs::Map msg; - if (!wavemap::convert::mapToRosMsg(map, "map", ros::Time(1, 0), msg)) { - std::cerr << "Failed to convert layered map to wavemap_msgs::Map.\n"; - return 1; - } - - // Convert the wrapped ROS message back into a fresh C++ map to validate the full wrapper round-trip. - ContinuousWaveletMap::Ptr loaded_map; - if (!wavemap::convert::rosMsgToMap(msg, loaded_map)) { - std::cerr << "Failed to convert wavemap_msgs::Map back to a layered map.\n"; - return 1; - } - - if (!loaded_map) { - std::cerr << "Loaded map pointer is null.\n"; - return 1; - } - - size_t verified_voxels = 0u; - for (const wavemap::Index3D& voxel_index : populated_indices) { - const LayeredVoxel original_voxel = map.getVoxelValue(voxel_index); - const LayeredVoxel loaded_voxel = loaded_map->getVoxelValue(voxel_index); - if (almostEqual(original_voxel, loaded_voxel)) { - ++verified_voxels; - } else { - std::cerr << "Layered ROS voxel mismatch at index " << voxel_index.transpose() << "\n"; - printVoxel("Original", original_voxel); - printVoxel("Loaded", loaded_voxel); - } - } - - const wavemap::Index3D sample_voxel_index = wavemap::Index3D(8, 2, 2) + wavemap::Index3D(1, 1, 2); - printVoxel("Original sample voxel", map.getVoxelValue(sample_voxel_index)); - printVoxel("Loaded sample voxel", loaded_map->getVoxelValue(sample_voxel_index)); - std::cout << "ROS wrapper frame: " << msg.header.frame_id << "\n"; - std::cout << "ROS message layers: " << msg.layered_hashed_wavelet_octree.front().layer_names.size() << "\n"; - std::cout << "ROS message blocks: " << msg.layered_hashed_wavelet_octree.front().blocks.size() << "\n"; - std::cout << "Verified layered ROS voxels: " << verified_voxels << " / " << populated_indices.size() << "\n"; - - return verified_voxels == populated_indices.size() ? 0 : 1; -} diff --git a/examples/cpp/digital_twin/ros/native_sensor_layer_adapters_experiment.cc b/examples/cpp/digital_twin/ros/native_sensor_layer_adapters_experiment.cc deleted file mode 100644 index af326959f..000000000 --- a/examples/cpp/digital_twin/ros/native_sensor_layer_adapters_experiment.cc +++ /dev/null @@ -1,156 +0,0 @@ -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include - -#include "../common/field_layer_policies.h" - -namespace { -bool approximatelyEqual(float lhs, float rhs, float tolerance = 1e-5f) { - return std::abs(lhs - rhs) <= tolerance; -} -bool expect(const std::string& label, bool condition) { - std::cout << " " << label << ": " << (condition ? "pass" : "FAIL") << "\n"; - return condition; -} -sensor_msgs::PointField field(const std::string& name, uint32_t offset, - uint8_t datatype) { - sensor_msgs::PointField result; - result.name = name; - result.offset = offset; - result.datatype = datatype; - result.count = 1u; - return result; -} -template -void write(std::vector& data, size_t offset, T value) { - std::memcpy(data.data() + offset, &value, sizeof(value)); -} -} // namespace - -int main() { - sensor_msgs::PointCloud2 cloud; - cloud.header.frame_id = "os_sensor"; - cloud.height = 1u; - cloud.width = 3u; - cloud.point_step = 16u; - cloud.row_step = cloud.width * cloud.point_step; - cloud.fields = { - field("x", 0u, sensor_msgs::PointField::FLOAT32), - field("y", 4u, sensor_msgs::PointField::FLOAT32), - field("z", 8u, sensor_msgs::PointField::FLOAT32), - field("reflectivity", 12u, sensor_msgs::PointField::UINT16)}; - cloud.data.resize(cloud.row_step); - write(cloud.data, 0u, 1.f); - write(cloud.data, 4u, 2.f); - write(cloud.data, 8u, 3.f); - write(cloud.data, 12u, 128u); - write(cloud.data, 16u, -1.f); - write(cloud.data, 20u, -2.f); - write(cloud.data, 24u, -3.f); - write(cloud.data, 28u, 255u); - write(cloud.data, 32u, std::numeric_limits::quiet_NaN()); - write(cloud.data, 36u, 0.f); - write(cloud.data, 40u, 0.f); - write(cloud.data, 44u, 64u); - - const auto reflectivity = - wavemap::convert::pointcloudToReflectivityObservations(cloud); - const auto endpoint_cloud = wavemap::convert:: - pointcloudToReflectivityEndpointPointcloud< - wavemap::examples::field_map::ReflectivityLayer>(cloud); - - sensor_msgs::Image color; - color.header.frame_id = "camera_color_optical_frame"; - color.width = 2u; - color.height = 1u; - color.encoding = sensor_msgs::image_encodings::RGB8; - color.step = 6u; - color.data = {255u, 0u, 0u, 0u, 128u, 255u}; - - sensor_msgs::Image depth; - depth.header.frame_id = color.header.frame_id; - depth.width = color.width; - depth.height = color.height; - depth.encoding = sensor_msgs::image_encodings::TYPE_16UC1; - depth.step = 4u; - depth.data.resize(4u); - write(depth.data, 0u, 1000u); - write(depth.data, 2u, 2000u); - - sensor_msgs::CameraInfo camera_info; - camera_info.header.frame_id = color.header.frame_id; - camera_info.width = color.width; - camera_info.height = color.height; - camera_info.K[0] = 1.0; - camera_info.K[4] = 1.0; - camera_info.K[8] = 1.0; - - const auto colors = wavemap::convert::rgbdToColorObservations( - color, depth, camera_info); - - wavemap::undistortion::StampedPointcloud timestamped_cloud(0u, "sensor", 2u); - timestamped_cloud.emplace(1.f, 0.f, 0.f, 20u, 0u); - timestamped_cloud.emplace(2.f, 0.f, 0.f, 10u, 1u); - timestamped_cloud.sort(); - - bool ok = true; - std::cout << "Native sensor layer adapters Phase 5 experiment\n"; - ok &= expect( - "Ouster reflectivity decoded and normalized", - reflectivity && reflectivity.header.frame_id == "os_sensor" && - reflectivity.observations.size() == 2u && - approximatelyEqual(reflectivity.observations[0].value, - 128.f / 255.f) && - approximatelyEqual(reflectivity.observations[1].value, 1.f)); - ok &= expect( - "single-pass endpoint attributes remain index-aligned", - endpoint_cloud && endpoint_cloud.rejected == 1u && - endpoint_cloud.pointcloud.size() == 2u && - endpoint_cloud.pointcloud.hasConsistentSizes() && - endpoint_cloud.pointcloud.point(0u) == - wavemap::Point3D(1.f, 2.f, 3.f) && - approximatelyEqual( - endpoint_cloud.pointcloud.attribute< - wavemap::examples::field_map::ReflectivityLayer>(0u), - 128.f / 255.f) && - endpoint_cloud.pointcloud.point(1u) == - wavemap::Point3D(-1.f, -2.f, -3.f) && - approximatelyEqual( - endpoint_cloud.pointcloud.attribute< - wavemap::examples::field_map::ReflectivityLayer>(1u), - 1.f)); - ok &= expect( - "motion-undistortion sorting preserves the attribute permutation", - timestamped_cloud[0u].source_index == 1u && - timestamped_cloud[1u].source_index == 0u); - ok &= expect( - "LiDAR positions remain in sensor frame", - reflectivity.observations[0].position == - wavemap::Point3D(1.f, 2.f, 3.f)); - ok &= expect( - "RGB-D color decoded and normalized", - colors && colors.observations.size() == 2u && - colors.observations[0].value == - wavemap::layered::Rgb{1.f, 0.f, 0.f} && - approximatelyEqual(colors.observations[1].value.g, 128.f / 255.f) && - approximatelyEqual(colors.observations[1].value.b, 1.f)); - ok &= expect( - "aligned depth back-projected with CameraInfo", - colors.observations[0].position == wavemap::Point3D(0.f, 0.f, 1.f) && - colors.observations[1].position == wavemap::Point3D(2.f, 0.f, 2.f)); - ok &= expect("RGB-D positions remain in camera frame", - colors.header.frame_id == "camera_color_optical_frame"); - return ok ? 0 : 1; -} diff --git a/examples/cpp/digital_twin/ros/publish_layered_map_experiment.cc b/examples/cpp/digital_twin/ros/publish_layered_map_experiment.cc deleted file mode 100644 index e221ce6d0..000000000 --- a/examples/cpp/digital_twin/ros/publish_layered_map_experiment.cc +++ /dev/null @@ -1,50 +0,0 @@ -#include -#include - -#include -#include - -#include "../common/layered_ros_converter.h" - -int main(int argc, char** argv) { - ros::init(argc, argv, "wavemap_publish_layered_map_experiment"); - ros::NodeHandle nh; - - wavemap::HashedWaveletOctreeConfig config; - config.min_cell_width = 0.1f; - config.min_log_odds = -2.f; - config.max_log_odds = 4.f; - config.tree_height = 3; - - ContinuousWaveletMap map(config); - std::vector populated_indices; - populateLayeredCube(map, populated_indices); - - const std::string map_topic = "/wavemap/map"; - const std::string map_frame = "map"; - const int queue_size = 1; - const bool latch = true; - - // Publish through the same wrapper type used by the existing wavemap ROS path. - ros::Publisher map_pub = nh.advertise(map_topic, queue_size, latch); - ros::Duration(0.5).sleep(); - - wavemap_msgs::Map map_msg; - if (!wavemap::convert::mapToRosMsg(map, map_frame, ros::Time::now(), map_msg)) { - std::cerr << "Failed to convert layered map to wavemap_msgs::Map.\n"; - return 1; - } - - // The message carries the normal ROS header plus the layered map payload in layered_hashed_wavelet_octree. - map_pub.publish(map_msg); - ros::spinOnce(); - - std::cout << "Published layered map on: " << map_topic << "\n"; - std::cout << "Frame: " << map_frame << "\n"; - std::cout << "Populated voxels: " << populated_indices.size() << "\n"; - std::cout << "Layer count: " << map_msg.layered_hashed_wavelet_octree.front().layer_names.size() << "\n"; - std::cout << "Block count: " << map_msg.layered_hashed_wavelet_octree.front().blocks.size() << "\n"; - - ros::spin(); - return 0; -} diff --git a/examples/cpp/digital_twin/ros/publish_structured_layered_map_experiment.cc b/examples/cpp/digital_twin/ros/publish_structured_layered_map_experiment.cc deleted file mode 100644 index 40b9dd59b..000000000 --- a/examples/cpp/digital_twin/ros/publish_structured_layered_map_experiment.cc +++ /dev/null @@ -1,102 +0,0 @@ -#include -#include -#include - -#include -#include - -#include "../common/example_layered_map_ros_config.h" -#include "../common/layered_ros_converter.h" - -namespace { -void populateDiscreteLayers(ExampleLayeredMap& map, - const std::vector& indices) { - const wavemap::Index3D first_index = indices.front(); - const wavemap::Index3D middle_index = indices[indices.size() / 2u]; - const wavemap::Index3D last_index = indices.back(); - - map.discreteLayers().semantic.setValue(first_index, 1); - map.discreteLayers().semantic.setValue(middle_index, 2); - map.discreteLayers().semantic.setValue(last_index, 1); - map.discreteLayers().changed.setValue(first_index, false); - map.discreteLayers().changed.setValue(middle_index, true); - map.discreteLayers().changed.setValue(last_index, false); -} -} // namespace - -int main(int argc, char** argv) { - ros::init(argc, argv, "wavemap_publish_structured_layered_map_experiment"); - ros::NodeHandle nh; - ros::NodeHandle nh_private("~"); - - ExampleLayeredMapConfig config = - layered_map_config::loadExampleLayeredMapConfigFromRosParams(nh_private); - - ExampleLayeredMap map(config); - std::vector populated_indices; - populateLayeredCube(map.continuousMap(), populated_indices); - populateDiscreteLayers(map, populated_indices); - - wavemap_msgs::LayeredMap msg; - if (!wavemap::convert::layeredMapToRosMsg( - map, "map", ros::Time::now(), msg)) { - std::cerr << "Failed to convert LayeredMap to ROS message.\n"; - return 1; - } - - const std::string layered_map_topic = "/wavemap/layered_map"; - const std::string continuous_map_topic = "/wavemap/layered_map/continuous"; - const std::string continuous_map_compat_topic = "/wavemap/map"; - const int queue_size = 1; - const bool latch = true; - ros::Publisher layered_map_publisher = - nh.advertise(layered_map_topic, queue_size, latch); - ros::Publisher continuous_map_publisher = - nh.advertise(continuous_map_topic, queue_size, latch); - ros::Publisher continuous_map_compat_publisher = nh.advertise( - continuous_map_compat_topic, queue_size, latch); - std::map marker_publishers; - auto marker_publisher_factory = [&](const std::string& topic) -> ros::Publisher& { - auto [publisher_it, inserted] = marker_publishers.emplace(topic, ros::Publisher{}); - if (inserted) { - publisher_it->second = - nh.advertise(topic, queue_size, latch); - } - return publisher_it->second; - }; - ros::Duration(0.5).sleep(); - - layered_map_publisher.publish(msg); - continuous_map_publisher.publish(msg.continuous_map); - continuous_map_compat_publisher.publish(msg.continuous_map); - if (!layered_map_viz::publishDiscreteLayerMarkers( - map.discreteLayers(), msg.header.frame_id, msg.header.stamp, - marker_publisher_factory)) { - std::cerr << "Failed to publish discrete layer markers.\n"; - return 1; - } - ros::spinOnce(); - - std::cout << "Published LayeredMap on: " << layered_map_topic << "\n"; - std::cout << "Published continuous map on: " << continuous_map_topic << "\n"; - std::cout << "Published continuous compatibility map on: " - << continuous_map_compat_topic << "\n"; - for (const auto& [topic, publisher] : marker_publishers) { - (void)publisher; - std::cout << "Published discrete markers on: " << topic << "\n"; - } - std::cout << "Frame: " << msg.header.frame_id << "\n"; - std::cout << "Continuous ROS map blocks: " - << msg.continuous_map.layered_hashed_wavelet_octree.front().blocks.size() - << "\n"; - std::cout << "Discrete ROS layers: " << msg.discrete_layers.size() << "\n"; - std::cout << "Populated continuous voxels: " << populated_indices.size() << "\n"; - std::cout << "Semantic parent cells: " - << map.discreteLayers().semantic.parentCount() << "\n"; - std::cout << "Changed parent cells: " - << map.discreteLayers().changed.parentCount() << "\n"; - - ros::spin(); - return 0; -} diff --git a/examples/cpp/digital_twin/ros/subscribe_layered_map_experiment.cc b/examples/cpp/digital_twin/ros/subscribe_layered_map_experiment.cc deleted file mode 100644 index 987119b83..000000000 --- a/examples/cpp/digital_twin/ros/subscribe_layered_map_experiment.cc +++ /dev/null @@ -1,50 +0,0 @@ -#include - -#include -#include - -#include "../common/layered_ros_converter.h" - -namespace { -void layeredMapCallback(const wavemap_msgs::Map::ConstPtr& msg) { - // Convert the standard wavemap ROS wrapper back into the custom layered C++ map type. - ContinuousWaveletMap::Ptr map; - if (!wavemap::convert::rosMsgToMap(*msg, map)) { - std::cerr << "Failed to convert wavemap_msgs::Map back to a layered map.\n"; - return; - } - - if (!map) { - std::cerr << "Converted layered map pointer is null.\n"; - return; - } - - // This index is one of the voxels populated by publish_layered_map_experiment. - const wavemap::Index3D sample_voxel_index = wavemap::Index3D(8, 2, 2) + wavemap::Index3D(1, 1, 2); - const LayeredVoxel sample_voxel = map->getVoxelValue(sample_voxel_index); - - std::cout << "Received layered map frame: " << msg->header.frame_id << "\n"; - std::cout << "Received layered map stamp: " << msg->header.stamp << "\n"; - std::cout << "Block count: " << map->getHashMap().size() << "\n"; - std::cout << "Layer count: " << msg->layered_hashed_wavelet_octree.front().layer_names.size() << "\n"; - printVoxel("Received sample voxel", sample_voxel); - - // The example only needs one message to prove the subscriber path works. - ros::shutdown(); -} -} // namespace - -int main(int argc, char** argv) { - ros::init(argc, argv, "wavemap_subscribe_layered_map_experiment"); - ros::NodeHandle nh; - - const std::string map_topic = "/wavemap/map"; - const int queue_size = 1; - - // Subscribe to the same topic used by the existing wavemap map publisher path. - ros::Subscriber map_sub = nh.subscribe(map_topic, queue_size, layeredMapCallback); - - std::cout << "Waiting for layered map on: " << map_topic << "\n"; - ros::spin(); - return 0; -} diff --git a/examples/cpp/digital_twin/ros/subscribe_structured_layered_map_experiment.cc b/examples/cpp/digital_twin/ros/subscribe_structured_layered_map_experiment.cc deleted file mode 100644 index 78ccc8a98..000000000 --- a/examples/cpp/digital_twin/ros/subscribe_structured_layered_map_experiment.cc +++ /dev/null @@ -1,109 +0,0 @@ -#include -#include -#include -#include - -#include - -#include "../common/example_layered_map_ros_config.h" -#include "../common/layered_ros_converter.h" - -namespace { -ExampleLayeredMapConfig makeConfig() { - ExampleLayeredMapConfig config; - config.continuous_map.min_cell_width = 0.1f; - config.continuous_map.min_log_odds = -2.f; - config.continuous_map.max_log_odds = 4.f; - config.continuous_map.tree_height = 3; - config.discrete_compression.block_height = 1; - return config; -} - -bool almostEqual(float lhs, float rhs) { return std::abs(lhs - rhs) < 1e-5f; } - -bool almostEqual(const LayeredVoxel& lhs, const LayeredVoxel& rhs) { - return almostEqual(lhs.occupancy, rhs.occupancy) && - almostEqual(lhs.data.rgb.r, rhs.data.rgb.r) && - almostEqual(lhs.data.rgb.g, rhs.data.rgb.g) && - almostEqual(lhs.data.rgb.b, rhs.data.rgb.b) && - almostEqual(lhs.data.traversability, rhs.data.traversability); -} - -template -bool sameValue(const std::optional& value, const ValueT& expected) { - return value && *value == expected; -} - -void structuredLayeredMapCallback(const wavemap_msgs::LayeredMap::ConstPtr& msg) { - ExampleLayeredMapConfig config = makeConfig(); - - ExampleLayeredMap received_map(config); - if (!wavemap::convert::rosMsgToLayeredMap( - *msg, received_map)) { - std::cerr << "Failed to convert ROS message to LayeredMap.\n"; - ros::shutdown(); - return; - } - - ExampleLayeredMap expected_map(config); - std::vector populated_indices; - populateLayeredCube(expected_map.continuousMap(), populated_indices); - - const wavemap::Index3D first_index = populated_indices.front(); - const wavemap::Index3D middle_index = populated_indices[populated_indices.size() / 2u]; - const wavemap::Index3D last_index = populated_indices.back(); - - size_t verified_continuous = 0u; - for (const wavemap::Index3D& voxel_index : populated_indices) { - if (almostEqual(expected_map.continuousMap().getVoxelValue(voxel_index), - received_map.continuousMap().getVoxelValue(voxel_index))) { - ++verified_continuous; - } - } - - const bool semantic_ok = - sameValue(received_map.discreteLayers().semantic.getValue(first_index), 1) && - sameValue(received_map.discreteLayers().semantic.getValue(middle_index), 2) && - sameValue(received_map.discreteLayers().semantic.getValue(last_index), 1); - const bool changed_ok = - sameValue(received_map.discreteLayers().changed.getValue(first_index), false) && - sameValue(received_map.discreteLayers().changed.getValue(middle_index), true) && - sameValue(received_map.discreteLayers().changed.getValue(last_index), false); - const bool unknown_ok = - !received_map.discreteLayers().semantic.getValue(wavemap::Index3D(0, 0, 0)); - - std::cout << "Received LayeredMap frame: " << msg->header.frame_id - << "\n"; - std::cout << "Received LayeredMap stamp: " << msg->header.stamp - << "\n"; - std::cout << "Continuous ROS map blocks: " - << msg->continuous_map.layered_hashed_wavelet_octree.front().blocks.size() - << "\n"; - std::cout << "Discrete ROS layers: " << msg->discrete_layers.size() << "\n"; - std::cout << "continuous voxels verified: " << verified_continuous << "/" - << populated_indices.size() << "\n"; - std::cout << "semantic labels preserved: " << (semantic_ok ? "yes" : "no") - << "\n"; - std::cout << "changed flags preserved: " << (changed_ok ? "yes" : "no") - << "\n"; - std::cout << "unknown discrete voxel remains unknown: " - << (unknown_ok ? "yes" : "no") << "\n"; - - ros::shutdown(); -} -} // namespace - -int main(int argc, char** argv) { - ros::init(argc, argv, "wavemap_subscribe_structured_layered_map_experiment"); - ros::NodeHandle nh; - - const std::string topic = "/wavemap/layered_map"; - const int queue_size = 1; - ros::Subscriber subscriber = - nh.subscribe(topic, queue_size, structuredLayeredMapCallback); - - std::cout << "Waiting for LayeredMap on: " << topic << "\n"; - ros::spin(); - return 0; -} diff --git a/examples/cpp/digital_twin/voxel_data/new_voxel.cc b/examples/cpp/digital_twin/voxel_data/new_voxel.cc deleted file mode 100644 index c4ed2469f..000000000 --- a/examples/cpp/digital_twin/voxel_data/new_voxel.cc +++ /dev/null @@ -1,30 +0,0 @@ -#include - -#include - -#include "../common/layered_voxel_config.h" - -int main() { - wavemap::VoxelData<> occupancy_only; - occupancy_only = 1.0f; - occupancy_only += 0.5f; - - std::cout << "Occupancy only: " << occupancy_only.occupancy << "\n"; - - LayeredVoxel layered_voxel(0.8f, ContinuousLayers{rgb(1.f, 0.f, 0.f), 0.75f}); - layered_voxel += 0.2f; - printVoxel("Layered voxel", layered_voxel); - - wavemap::FloatingPoint occupancy_value = layered_voxel; - std::cout << "Implicit occupancy value: " << occupancy_value << "\n"; - - const LayeredVoxel voxel_a(0.4f, ContinuousLayers{rgb(1.f, 0.2f, 0.f), 0.3f}); - const LayeredVoxel voxel_b(0.6f, ContinuousLayers{rgb(0.f, 0.3f, 1.f), 0.5f}); - - printVoxel("Sum", voxel_a + voxel_b); - printVoxel("Difference", voxel_b - voxel_a); - printVoxel("Scaled", 0.5f * voxel_b); - printVoxel("Divided", voxel_b / 2.f); - - return 0; -} diff --git a/examples/cpp/digital_twin/voxel_data/voxel_haar_transform.cc b/examples/cpp/digital_twin/voxel_data/voxel_haar_transform.cc deleted file mode 100644 index 4da2d17dc..000000000 --- a/examples/cpp/digital_twin/voxel_data/voxel_haar_transform.cc +++ /dev/null @@ -1,35 +0,0 @@ -#include - -#include -#include - -#include "../common/layered_voxel_config.h" - -namespace { -using Coefficients = wavemap::HaarCoefficients; -using Transform = wavemap::HaarTransform; -} // namespace - -int main() { - Coefficients::CoefficientsArray child_voxels{ - LayeredVoxel{0.1f, ContinuousLayers{rgb(1.f, 0.f, 0.f), 0.1f}}, - LayeredVoxel{0.2f, ContinuousLayers{rgb(0.f, 1.f, 0.f), 0.2f}}, - LayeredVoxel{0.3f, ContinuousLayers{rgb(0.f, 0.f, 1.f), 0.3f}}, - LayeredVoxel{0.4f, ContinuousLayers{rgb(1.f, 1.f, 0.f), 0.4f}}, - LayeredVoxel{0.5f, ContinuousLayers{rgb(1.f, 0.f, 1.f), 0.5f}}, - LayeredVoxel{0.6f, ContinuousLayers{rgb(0.f, 1.f, 1.f), 0.6f}}, - LayeredVoxel{0.7f, ContinuousLayers{rgb(0.5f, 0.5f, 0.5f), 0.7f}}, - LayeredVoxel{0.8f, ContinuousLayers{rgb(1.f, 1.f, 1.f), 0.8f}}, - }; - - const Coefficients::Parent parent_coefficients = - Transform::forward(child_voxels); - const Coefficients::CoefficientsArray reconstructed_children = - Transform::backward(parent_coefficients); - - printVoxel("Parent scale", parent_coefficients.scale); - printVoxel("First reconstructed child", reconstructed_children[0]); - printVoxel("Last reconstructed child", reconstructed_children[7]); - - return 0; -} diff --git a/interfaces/ros1/wavemap_ros/CMakeLists.txt b/interfaces/ros1/wavemap_ros/CMakeLists.txt index 9e6693e9c..e7a845abe 100644 --- a/interfaces/ros1/wavemap_ros/CMakeLists.txt +++ b/interfaces/ros1/wavemap_ros/CMakeLists.txt @@ -44,10 +44,7 @@ add_library(${PROJECT_NAME} src/ros_server.cc) set_wavemap_target_properties(${PROJECT_NAME}) target_include_directories(${PROJECT_NAME} - PUBLIC include ${catkin_INCLUDE_DIRS} - PRIVATE - ${CMAKE_CURRENT_SOURCE_DIR}/../../../examples/cpp/digital_twin/common - ${CMAKE_CURRENT_SOURCE_DIR}/../../../examples/cpp/digital_twin/ouster_mapping) + PUBLIC include ${catkin_INCLUDE_DIRS}) target_link_libraries(${PROJECT_NAME} PUBLIC ${catkin_LIBRARIES} ${OpenCV_LIBRARIES}) @@ -57,55 +54,93 @@ set_wavemap_target_properties(ros_server) target_link_libraries(ros_server ${PROJECT_NAME}) add_executable(occupancy_layered_server - ../../../examples/cpp/digital_twin/ouster_mapping/occupancy_sensor_inputs.cc) + app/ouster_layered/occupancy_sensor_inputs.cc) set_wavemap_target_properties(occupancy_layered_server) target_include_directories(occupancy_layered_server PRIVATE - ${CMAKE_CURRENT_SOURCE_DIR}/../../../examples/cpp/digital_twin/common - ${CMAKE_CURRENT_SOURCE_DIR}/../../../examples/cpp/digital_twin/ouster_mapping) + ${CMAKE_CURRENT_SOURCE_DIR}/app/ouster_layered) target_link_libraries(occupancy_layered_server ${PROJECT_NAME}) +add_executable(occupancy_chunked_layered_server + app/ouster_layered/occupancy_chunked_sensor_inputs.cc) +set_wavemap_target_properties(occupancy_chunked_layered_server) +target_include_directories(occupancy_chunked_layered_server PRIVATE + ${CMAKE_CURRENT_SOURCE_DIR}/app/ouster_layered) +target_link_libraries(occupancy_chunked_layered_server ${PROJECT_NAME}) + add_executable(reflectivity_layered_server - ../../../examples/cpp/digital_twin/ouster_mapping/reflectivity_only_sensor_inputs.cc) + app/ouster_layered/reflectivity_only_sensor_inputs.cc) set_wavemap_target_properties(reflectivity_layered_server) target_include_directories(reflectivity_layered_server PRIVATE - ${CMAKE_CURRENT_SOURCE_DIR}/../../../examples/cpp/digital_twin/common - ${CMAKE_CURRENT_SOURCE_DIR}/../../../examples/cpp/digital_twin/ouster_mapping) + ${CMAKE_CURRENT_SOURCE_DIR}/app/ouster_layered) target_link_libraries(reflectivity_layered_server ${PROJECT_NAME}) +add_executable(signal_layered_server + app/ouster_layered/signal_sensor_inputs.cc) +set_wavemap_target_properties(signal_layered_server) +target_include_directories(signal_layered_server PRIVATE + ${CMAKE_CURRENT_SOURCE_DIR}/app/ouster_layered) +target_link_libraries(signal_layered_server ${PROJECT_NAME}) + +add_executable(signal_near_ir_layered_server + app/ouster_layered/signal_near_ir_sensor_inputs.cc) +set_wavemap_target_properties(signal_near_ir_layered_server) +target_include_directories(signal_near_ir_layered_server PRIVATE + ${CMAKE_CURRENT_SOURCE_DIR}/app/ouster_layered) +target_link_libraries(signal_near_ir_layered_server ${PROJECT_NAME}) + +add_executable(signal_class_layered_server + app/ouster_layered/signal_class_sensor_inputs.cc) +set_wavemap_target_properties(signal_class_layered_server) +target_include_directories(signal_class_layered_server PRIVATE + ${CMAKE_CURRENT_SOURCE_DIR}/app/ouster_layered) +target_link_libraries(signal_class_layered_server ${PROJECT_NAME}) + +add_executable(signal_near_ir_class_layered_server + app/ouster_layered/signal_near_ir_class_sensor_inputs.cc) +set_wavemap_target_properties(signal_near_ir_class_layered_server) +target_include_directories(signal_near_ir_class_layered_server PRIVATE + ${CMAKE_CURRENT_SOURCE_DIR}/app/ouster_layered) +target_link_libraries(signal_near_ir_class_layered_server ${PROJECT_NAME}) + +add_executable(reflectivity_chunked_layered_server + app/ouster_layered/reflectivity_chunked_sensor_inputs.cc) +set_wavemap_target_properties(reflectivity_chunked_layered_server) +target_include_directories(reflectivity_chunked_layered_server PRIVATE + ${CMAKE_CURRENT_SOURCE_DIR}/app/ouster_layered) +target_link_libraries(reflectivity_chunked_layered_server ${PROJECT_NAME}) + add_executable(reflectivity_class_layered_server - ../../../examples/cpp/digital_twin/ouster_mapping/ouster_sensor_inputs.cc) + app/ouster_layered/ouster_sensor_inputs.cc) set_wavemap_target_properties(reflectivity_class_layered_server) target_include_directories(reflectivity_class_layered_server PRIVATE - ${CMAKE_CURRENT_SOURCE_DIR}/../../../examples/cpp/digital_twin/common - ${CMAKE_CURRENT_SOURCE_DIR}/../../../examples/cpp/digital_twin/ouster_mapping) + ${CMAKE_CURRENT_SOURCE_DIR}/app/ouster_layered) target_link_libraries(reflectivity_class_layered_server ${PROJECT_NAME}) +add_executable(reflectivity_class_chunked_layered_server + app/ouster_layered/reflectivity_class_chunked_sensor_inputs.cc) +set_wavemap_target_properties(reflectivity_class_chunked_layered_server) +target_include_directories(reflectivity_class_chunked_layered_server PRIVATE + ${CMAKE_CURRENT_SOURCE_DIR}/app/ouster_layered) +target_link_libraries(reflectivity_class_chunked_layered_server ${PROJECT_NAME}) + +add_executable(inspect_ouster_benchmark_maps + app/ouster_layered/inspect_ouster_benchmark_maps.cc) +set_wavemap_target_properties(inspect_ouster_benchmark_maps) +target_include_directories(inspect_ouster_benchmark_maps PRIVATE + ${CMAKE_CURRENT_SOURCE_DIR}/app/ouster_layered) +target_link_libraries(inspect_ouster_benchmark_maps ${PROJECT_NAME}) + +add_executable(synthetic_rgb_layered_map_demo + app/synthetic_rgb_layered_map_demo.cc) +set_wavemap_target_properties(synthetic_rgb_layered_map_demo) +target_include_directories(synthetic_rgb_layered_map_demo PRIVATE + ${CMAKE_CURRENT_SOURCE_DIR}/app) +target_link_libraries(synthetic_rgb_layered_map_demo ${PROJECT_NAME}) + add_executable(rosbag_processor app/rosbag_processor.cc) set_wavemap_target_properties(rosbag_processor) target_link_libraries(rosbag_processor ${PROJECT_NAME}) -add_executable(publish_field_layer_updates_experiment - app/publish_field_layer_updates_experiment.cc) -set_wavemap_target_properties(publish_field_layer_updates_experiment) -target_link_libraries(publish_field_layer_updates_experiment ${PROJECT_NAME}) - -add_executable(field_layer_sensor_bridge - app/field_layer_sensor_bridge.cc) -set_wavemap_target_properties(field_layer_sensor_bridge) -target_link_libraries(field_layer_sensor_bridge ${PROJECT_NAME}) - -add_executable(layered_incremental_publish_experiment - app/layered_incremental_publish_experiment.cc) -set_wavemap_target_properties(layered_incremental_publish_experiment) -target_include_directories(layered_incremental_publish_experiment PRIVATE - ${CMAKE_CURRENT_SOURCE_DIR}/../../../examples/cpp/digital_twin/common) -target_link_libraries(layered_incremental_publish_experiment ${PROJECT_NAME}) - -add_executable(computed_point_field_binding_experiment - app/computed_point_field_binding_experiment.cc) -set_wavemap_target_properties(computed_point_field_binding_experiment) -target_link_libraries(computed_point_field_binding_experiment ${PROJECT_NAME}) - # Support installs # Mark targets for installation install(TARGETS ${PROJECT_NAME} diff --git a/interfaces/ros1/wavemap_ros/app/computed_point_field_binding_experiment.cc b/interfaces/ros1/wavemap_ros/app/computed_point_field_binding_experiment.cc deleted file mode 100644 index 27bc01a71..000000000 --- a/interfaces/ros1/wavemap_ros/app/computed_point_field_binding_experiment.cc +++ /dev/null @@ -1,204 +0,0 @@ -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include - -#include "wavemap_ros/inputs/pointcloud_endpoint_adapter.h" -#include "wavemap_ros/layered_ros_server_extension.h" - -namespace { -struct AveragedLayer - : wavemap::layered::schema::StatefulContinuousLayer< - float, wavemap::layered::WeightedMeanState, - wavemap::layered::WeightedMeanLayerUpdatePolicy> { - static constexpr std::string_view name = "averaged"; -}; - -struct ClassLayer - : wavemap::layered::schema::DiscreteLayer< - int, wavemap::layered::ReplaceLayerUpdatePolicy> { - static constexpr std::string_view name = "class"; -}; - -using AveragedDefinition = wavemap::layered::LayeredMapDefinition< - wavemap::layered::schema::LayerSchema>; -using AveragedRosConverter = wavemap::convert::DescriptorContinuousRosConverter< - AveragedDefinition::Voxel>; -using TestExtension = wavemap::LayeredRosServerExtension< - AveragedDefinition::Map, AveragedDefinition::MapIo, - AveragedRosConverter>; - -[[maybe_unused]] bool instantiateDiscreteEndpointIntegration( - TestExtension& extension, - const wavemap::PosedPointcloud<>& pointcloud, - const std::vector& classes) { - return extension.integrateEndpointLayer(pointcloud, classes); -} - -sensor_msgs::PointField makeField(const std::string& name, uint32_t offset, - uint8_t datatype) { - sensor_msgs::PointField field; - field.name = name; - field.offset = offset; - field.datatype = datatype; - field.count = 1u; - return field; -} - -template -void write(std::vector& data, size_t offset, ValueT value) { - std::memcpy(data.data() + offset, &value, sizeof(value)); -} - -bool approximatelyEqual(float lhs, float rhs, float tolerance = 1e-5f) { - return std::abs(lhs - rhs) <= tolerance; -} -} // namespace - -int main() { - sensor_msgs::PointCloud2 cloud; - cloud.height = 1u; - cloud.width = 3u; - cloud.point_step = 12u; - cloud.row_step = cloud.width * cloud.point_step; - cloud.fields = { - makeField("raw", 0u, sensor_msgs::PointField::UINT16), - makeField("gain", 4u, sensor_msgs::PointField::FLOAT32), - makeField("class", 8u, sensor_msgs::PointField::INT32)}; - cloud.data.resize(cloud.row_step); - - write(cloud.data, 0u, 100u); - write(cloud.data, 4u, 0.5f); - write(cloud.data, 8u, 4); - write(cloud.data, 12u, 200u); - write(cloud.data, 16u, - std::numeric_limits::quiet_NaN()); - write(cloud.data, 20u, 2); - write(cloud.data, 24u, 50u); - write(cloud.data, 28u, 2.f); - write(cloud.data, 32u, 7); - - std::vector> integrated_values; - wavemap::ComputedNumericPointcloudEndpointAdapter adapter( - {"raw", "gain"}, - [](const wavemap::NumericPointFieldValues& fields) - -> std::optional { - const float result = fields[0u] * 0.01f * fields[1u]; - return result <= 1.f ? std::optional(result) : std::nullopt; - }, - [&integrated_values]( - const wavemap::PosedPointcloud<>&, - const std::vector>& values) { - integrated_values = values; - }); - std::vector integrated_classes; - wavemap::NumericPointcloudEndpointAdapter class_adapter( - "class", 1.f, 0.f, 0, 255, - [&integrated_classes](const wavemap::PosedPointcloud<>&, - const std::vector& values) { - integrated_classes = values; - }); - - auto decoder = adapter.prepare(cloud, cloud.width); - if (!decoder) { - std::cerr << "Could not prepare computed point-field decoder.\n"; - return 1; - } - for (size_t index = 0u; index < cloud.width; ++index) { - if (!decoder->decodePoint(cloud, index * cloud.point_step)) { - return 1; - } - } - - auto channel = decoder->finish(); - auto class_decoder = class_adapter.prepare(cloud, cloud.width); - if (!class_decoder) { - std::cerr << "Could not prepare discrete point-field decoder.\n"; - return 1; - } - for (size_t index = 0u; index < cloud.width; ++index) { - if (!class_decoder->decodePoint(cloud, index * cloud.point_step)) { - return 1; - } - } - auto class_channel = class_decoder->finish(); - wavemap::PosedPointcloud<> posed_cloud; - posed_cloud.resize(3u); - std::vector sorted_points; - sorted_points.emplace_back(0.f, 0.f, 0.f, 0u, 2u); - sorted_points.emplace_back(0.f, 0.f, 0.f, 1u, 0u); - sorted_points.emplace_back(0.f, 0.f, 0.f, 2u, 1u); - - const bool integrated = channel->integrate(posed_cloud, sorted_points); - const bool class_integrated = - class_channel->integrate(posed_cloud, sorted_points); - AveragedDefinition::Voxel averaged_voxel; - averaged_voxel.data.get() = {2.6f, 4.f}; - auto averaged_layers = AveragedRosConverter::makeLayers(); - AveragedRosConverter::appendLayerValues(averaged_voxel, averaged_layers); - const auto restored_voxel = - AveragedRosConverter::readCellData(0.f, averaged_layers, 0u); - const auto restored_state = restored_voxel.data.get(); - std::vector endpoint_indices; - wavemap::layered::forEachPointcloudIntegrationIndex< - wavemap::layered::PointcloudLayerIntegrationMode::kEndpointOnly>( - wavemap::Point3D::Zero(), wavemap::Point3D(2.f, 0.f, 0.f), 1.f, - [&endpoint_indices](const wavemap::Index3D& index) { - endpoint_indices.emplace_back(index); - }); - std::vector ray_indices; - wavemap::layered::forEachPointcloudIntegrationIndex< - wavemap::layered::PointcloudLayerIntegrationMode::kAlongRay>( - wavemap::Point3D::Zero(), wavemap::Point3D(2.f, 0.f, 0.f), 1.f, - [&ray_indices](const wavemap::Index3D& index) { - ray_indices.emplace_back(index); - }); - const wavemap::Point3D origin = wavemap::Point3D::Zero(); - const wavemap::Index3D expected_origin = - wavemap::convert::pointToNearestIndex(origin, 1.f); - const wavemap::Index3D expected_endpoint = wavemap::convert::pointToNearestIndex( - wavemap::Point3D(2.f, 0.f, 0.f), 1.f); - - const bool passed = - integrated && class_integrated && integrated_values.size() == 3u && - integrated_values[0u] && - approximatelyEqual(*integrated_values[0u], 1.f) && - integrated_values[1u] && - approximatelyEqual(*integrated_values[1u], 0.5f) && - !integrated_values[2u] && integrated_classes == std::vector{7, 4, 2} && - endpoint_indices.size() == 1u && - endpoint_indices.front() == expected_endpoint && - ray_indices.size() > endpoint_indices.size() && - ray_indices.front() == expected_origin && - ray_indices.back() == expected_endpoint && averaged_layers.size() == 1u && - averaged_layers[0].type == "float32" && - averaged_layers[0].float32_values.size() == 1u && - approximatelyEqual(averaged_layers[0].float32_values[0], 0.65f) && - approximatelyEqual(restored_state.weighted_sum, 0.65f) && - approximatelyEqual(restored_state.total_weight, 1.f); - - std::cout << "Computed PointCloud2 field binding experiment: " - << (passed ? "pass" : "FAIL") << "\n"; - if (!passed) { - std::cout << "endpoint indices: " << endpoint_indices.size(); - for (const auto& index : endpoint_indices) { - std::cout << " [" << index.transpose() << "]"; - } - std::cout << "\nray indices: " << ray_indices.size(); - for (const auto& index : ray_indices) { - std::cout << " [" << index.transpose() << "]"; - } - std::cout << "\n"; - } - return passed ? 0 : 1; -} diff --git a/interfaces/ros1/wavemap_ros/app/field_layer_sensor_bridge.cc b/interfaces/ros1/wavemap_ros/app/field_layer_sensor_bridge.cc deleted file mode 100644 index 7814d5fc9..000000000 --- a/interfaces/ros1/wavemap_ros/app/field_layer_sensor_bridge.cc +++ /dev/null @@ -1,198 +0,0 @@ -#include -#include -#include -#include -#include - -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -namespace wavemap { -class FieldLayerSensorBridge { - public: - FieldLayerSensorBridge() - : nh_private_("~"), - transformer_(std::make_shared()) { - nh_private_.param("world_frame", world_frame_, std::string("odom")); - nh_private_.param("reflectivity_topic", reflectivity_topic_, - std::string("/ouster/points")); - nh_private_.param("color_topic", color_topic_, - std::string("/camera/color/image_raw")); - nh_private_.param("aligned_depth_topic", aligned_depth_topic_, - std::string( - "/camera/aligned_depth_to_color/image_raw")); - nh_private_.param("camera_info_topic", camera_info_topic_, - std::string("/camera/color/camera_info")); - nh_private_.param("reflectivity_enabled", reflectivity_enabled_, true); - nh_private_.param("color_enabled", color_enabled_, false); - - int point_stride = 1; - int pixel_stride = 4; - int queue_size = 10; - double sync_tolerance = 0.05; - nh_private_.param("reflectivity_point_stride", point_stride, point_stride); - nh_private_.param("color_pixel_stride", pixel_stride, pixel_stride); - nh_private_.param("sync_queue_size", queue_size, queue_size); - nh_private_.param("sync_tolerance", sync_tolerance, sync_tolerance); - reflectivity_config_.point_stride = - static_cast(std::max(1, point_stride)); - color_config_.pixel_stride = - static_cast(std::max(1, pixel_stride)); - - update_pub_ = nh_.advertise( - "/wavemap/layered_map_updates", 1); - - if (reflectivity_enabled_) { - reflectivity_sub_ = nh_.subscribe( - reflectivity_topic_, queue_size, - &FieldLayerSensorBridge::reflectivityCallback, this); - } - if (color_enabled_) { - color_sub_.subscribe(nh_, color_topic_, queue_size); - depth_sub_.subscribe(nh_, aligned_depth_topic_, queue_size); - camera_info_sub_.subscribe(nh_, camera_info_topic_, queue_size); - SyncPolicy policy(queue_size); - policy.setMaxIntervalDuration(ros::Duration(sync_tolerance)); - rgbd_sync_ = std::make_unique( - static_cast(policy), color_sub_, depth_sub_, - camera_info_sub_); - rgbd_sync_->registerCallback(boost::bind( - &FieldLayerSensorBridge::rgbdCallback, this, - boost::placeholders::_1, boost::placeholders::_2, - boost::placeholders::_3)); - } - } - - private: - using SyncPolicy = message_filters::sync_policies::ApproximateTime< - sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo>; - using Synchronizer = message_filters::Synchronizer; - - template - bool transformObservations( - std::vector>& observations, - const std::string& sensor_frame, const ros::Time& stamp) { - const auto T_W_S = - transformer_->lookupTransform(world_frame_, sensor_frame, stamp); - if (!T_W_S) { - ROS_WARN_STREAM_THROTTLE( - 2.0, "No transform from '" << sensor_frame << "' to '" - << world_frame_ - << "'; skipping layer observations."); - return false; - } - for (auto& observation : observations) { - observation.position = *T_W_S * observation.position; - } - return true; - } - - void reflectivityCallback(const sensor_msgs::PointCloud2& cloud) { - auto batch = convert::pointcloudToReflectivityObservations( - cloud, reflectivity_config_); - if (!batch) { - ROS_WARN_STREAM_THROTTLE( - 2.0, "Reflectivity conversion failed: " << batch.error); - return; - } - if (!transformObservations( - batch.observations, batch.header.frame_id, batch.header.stamp)) { - return; - } - - wavemap_msgs::LayeredMapUpdate update; - update.header = batch.header; - update.header.frame_id = world_frame_; - update.positions.reserve(batch.observations.size()); - wavemap_msgs::Layer layer; - layer.name = "reflectivity"; - layer.type = "float32"; - layer.float32_values.reserve(batch.observations.size()); - for (const auto& observation : batch.observations) { - geometry_msgs::Point32 position; - position.x = observation.position.x(); - position.y = observation.position.y(); - position.z = observation.position.z(); - update.positions.emplace_back(position); - layer.float32_values.emplace_back(observation.value); - } - update.layers.emplace_back(std::move(layer)); - update_pub_.publish(update); - } - - void rgbdCallback( - const sensor_msgs::ImageConstPtr& color, - const sensor_msgs::ImageConstPtr& depth, - const sensor_msgs::CameraInfoConstPtr& camera_info) { - auto batch = convert::rgbdToColorObservations( - *color, *depth, *camera_info, color_config_); - if (!batch) { - ROS_WARN_STREAM_THROTTLE(2.0, "RGB-D conversion failed: " << batch.error); - return; - } - if (!transformObservations( - batch.observations, batch.header.frame_id, batch.header.stamp)) { - return; - } - - wavemap_msgs::LayeredMapUpdate update; - update.header = batch.header; - update.header.frame_id = world_frame_; - update.positions.reserve(batch.observations.size()); - wavemap_msgs::Layer layer; - layer.name = "color"; - layer.type = "float32_rgb"; - layer.float32_values.reserve(3u * batch.observations.size()); - for (const auto& observation : batch.observations) { - geometry_msgs::Point32 position; - position.x = observation.position.x(); - position.y = observation.position.y(); - position.z = observation.position.z(); - update.positions.emplace_back(position); - layer.float32_values.emplace_back(observation.value.r); - layer.float32_values.emplace_back(observation.value.g); - layer.float32_values.emplace_back(observation.value.b); - } - update.layers.emplace_back(std::move(layer)); - update_pub_.publish(update); - } - - ros::NodeHandle nh_; - ros::NodeHandle nh_private_; - std::shared_ptr transformer_; - std::string world_frame_; - std::string reflectivity_topic_; - std::string color_topic_; - std::string aligned_depth_topic_; - std::string camera_info_topic_; - bool reflectivity_enabled_ = true; - bool color_enabled_ = false; - convert::ReflectivityObservationConfig reflectivity_config_; - convert::RgbdColorObservationConfig color_config_; - - ros::Publisher update_pub_; - ros::Subscriber reflectivity_sub_; - message_filters::Subscriber color_sub_; - message_filters::Subscriber depth_sub_; - message_filters::Subscriber camera_info_sub_; - std::unique_ptr rgbd_sync_; -}; -} // namespace wavemap - -int main(int argc, char** argv) { - ros::init(argc, argv, "field_layer_sensor_bridge"); - wavemap::FieldLayerSensorBridge bridge; - ros::spin(); - return 0; -} diff --git a/interfaces/ros1/wavemap_ros/app/layered_incremental_publish_experiment.cc b/interfaces/ros1/wavemap_ros/app/layered_incremental_publish_experiment.cc deleted file mode 100644 index 33ff92c1c..000000000 --- a/interfaces/ros1/wavemap_ros/app/layered_incremental_publish_experiment.cc +++ /dev/null @@ -1,145 +0,0 @@ -#include -#include -#include -#include -#include -#include - -#include -#include - -#include "field_layered_map_ros_config.h" -#include -#include "wavemap_ros/map_operations/layered_publish_map_operation.h" - -namespace field_map = wavemap::examples::field_map; - -int main(int argc, char** argv) { - ros::init(argc, argv, "layered_incremental_publish_experiment"); - ros::NodeHandle nh; - - // Exercise the same type-erased threshold operation used by the original - // Pipeline. Virtual dispatch must retain the typed layer configuration. - bool threshold_valid = false; - { - field_map::Map threshold_map(field_map::makeDefaultConfig()); - const wavemap::Index3D threshold_index(2, 3, 4); - threshold_map.continuousMap().setVoxelValue( - threshold_index, - field_map::Voxel( - 8.f, field_map::makeContinuousLayers(0.65f))); - wavemap::ThresholdMapOperationConfig threshold_config; - wavemap::ThresholdMapOperation threshold_operation( - threshold_config, threshold_map.continuousMapPtr()); - threshold_operation.run(true); - const auto thresholded = - threshold_map.continuousMap().getVoxelValue(threshold_index); - threshold_valid = - std::abs(thresholded.occupancy - 4.f) < 1e-5f && - std::abs(thresholded.data.get() - - 0.4f) < 1e-5f; - } - - std::mutex mutex; - std::vector messages; - auto subscriber = nh.subscribe( - "/layered_incremental_test", 10, - [&mutex, &messages](const wavemap_msgs::LayeredMap::ConstPtr& msg) { - std::scoped_lock lock(mutex); - messages.emplace_back(*msg); - }); - - const auto map_config = field_map::makeDefaultConfig(); - field_map::Map map(map_config); - wavemap::PublishMapOperationConfig config; - config.once_every = 0.01f; - config.max_num_blocks_per_msg = 1; - config.topic = "/layered_incremental_test"; - using Operation = wavemap::UnifiedLayeredPublishMapOperation< - field_map::Map, field_map::ros_config::VoxelRosConverter>; - Operation operation(config, &map, std::make_shared(1), - "map", nh); - - ros::AsyncSpinner spinner(1); - spinner.start(); - ros::WallDuration(0.1).sleep(); - - map.continuousMap().setVoxelValue( - wavemap::Index3D(0, 0, 0), - field_map::Voxel(0.5f, field_map::makeContinuousLayers(0.25f))); - auto& class_layer = - map.discreteLayers().get(); - class_layer.setValue(wavemap::Index3D(0, 0, 0), 1); - class_layer.setValue(wavemap::Index3D(1, 0, 0), 1); - class_layer.setValue(wavemap::Index3D(0, 1, 0), 2); - // Rewriting the same compressed parent must still produce one patch cell. - class_layer.setValue(wavemap::Index3D(0, 1, 0), 3); - operation.run(true); - ros::WallDuration(0.1).sleep(); - - // Tree height 6 means an offset of 64 lies in a different hashed block. - map.continuousMap().setVoxelValue( - wavemap::Index3D(64, 0, 0), - field_map::Voxel(0.7f, field_map::makeContinuousLayers(0.75f))); - class_layer.eraseValue(wavemap::Index3D(0, 0, 0)); - class_layer.eraseValue(wavemap::Index3D(1, 0, 0)); - class_layer.eraseValue(wavemap::Index3D(0, 1, 0)); - operation.run(true); - ros::WallDuration(0.1).sleep(); - - // A discrete-only edit must publish without touching a continuous block. - class_layer.setValue(wavemap::Index3D(4, 0, 0), 2); - operation.run(true); - ros::WallDuration(0.1).sleep(); - spinner.stop(); - - std::scoped_lock lock(mutex); - bool valid = threshold_valid && messages.size() == 3u; - if (valid) { - const auto& first = - messages[0].continuous_map.layered_hashed_wavelet_octree; - const auto& second = - messages[1].continuous_map.layered_hashed_wavelet_octree; - const auto& third = - messages[2].continuous_map.layered_hashed_wavelet_octree; - valid = first.size() == 1u && second.size() == 1u && third.size() == 1u && - !messages[0].is_full_update && !messages[1].is_full_update && - !messages[2].is_full_update && - first[0].blocks.size() == 1u && - first[0].allocated_block_indices.size() == 1u && - second[0].blocks.size() == 1u && - second[0].allocated_block_indices.size() == 2u && - second[0].layer_names == std::vector{"reflectivity"} && - second[0].layer_types == std::vector{"float32"} && - second[0].layer_min_values.size() == 1u && - second[0].layer_min_values[0].float32_values == - std::vector{0.f} && - second[0].layer_max_values.size() == 1u && - second[0].layer_max_values[0].float32_values == - std::vector{0.4f} && - messages[0].discrete_layers.size() == 1u && - messages[0].discrete_layers[0].name == "class" && - messages[0].discrete_layers[0].cells.size() == 1u && - messages[0].discrete_layers[0].cells[0].dominant_int32 == 1 && - messages[0].discrete_layers[0].cells[0].exception_offsets.size() == - 1u && - messages[0].discrete_layers[0] - .cells[0] - .exception_int32_values == std::vector{3} && - messages[1].discrete_layers.size() == 1u && - messages[1].discrete_layers[0].cells.empty() && - messages[1].discrete_layers[0].deleted_parent_indices.size() == 1u && - third[0].blocks.empty() && - third[0].allocated_block_indices.size() == 2u && - messages[2].discrete_layers.size() == 1u && - messages[2].discrete_layers[0].cells.size() == 1u; - } - - std::cout << "Layered incremental publish experiment\n" - << " messages received: " << messages.size() << "\n" - << " bounded continuous blocks and discrete patches: " - << (valid ? "pass" : "FAIL") << "\n" - << " typed occupancy/reflectivity thresholding: " - << (threshold_valid ? "pass" : "FAIL") << "\n"; - return valid ? 0 : 1; -} diff --git a/examples/cpp/digital_twin/io/inspect_ouster_benchmark_maps.cc b/interfaces/ros1/wavemap_ros/app/ouster_layered/inspect_ouster_benchmark_maps.cc similarity index 81% rename from examples/cpp/digital_twin/io/inspect_ouster_benchmark_maps.cc rename to interfaces/ros1/wavemap_ros/app/ouster_layered/inspect_ouster_benchmark_maps.cc index 9e8a3e7df..60a4e43af 100644 --- a/examples/cpp/digital_twin/io/inspect_ouster_benchmark_maps.cc +++ b/interfaces/ros1/wavemap_ros/app/ouster_layered/inspect_ouster_benchmark_maps.cc @@ -8,11 +8,11 @@ #include #include -#include +#include -#include "../ouster_mapping/occupancy_map_layers.h" -#include "../ouster_mapping/reflectivity_map_layers.h" -#include "../ouster_mapping/reflectivity_only_map_layers.h" +#include "occupancy_map_layers.h" +#include "reflectivity_map_layers.h" +#include "reflectivity_only_map_layers.h" namespace occupancy_map = wavemap::examples::occupancy_map; namespace reflectivity_only_map = wavemap::examples::reflectivity_only_map; @@ -193,8 +193,33 @@ int main(int argc, char** argv) { wavemap::layered::GeometricClass::kObstacle); } } + const size_t parent_count = class_layer.parentCount(); + const size_t observed_count = class_layer.observedValueCount(); + const size_t exception_count = class_layer.exceptionCount(); + const size_t dominant_matching_count = observed_count - exception_count; + // Exact size of the version-1 discrete section for this single int layer: + // section marker, layer metadata/configuration, and all cell records. + const size_t serialized_discrete_bytes = + 67u + 32u * parent_count + 4u * observed_count + + 8u * exception_count; std::cout << "Class statistics\n" - << " observed=" << class_layer.observedValueCount() + << " block_height=" << class_layer.config().block_height + << " block_side=" << class_layer.config().blockSideLength() + << " possible_children=" + << class_layer.config().blockVoxelCount() << "\n" + << " parents=" << parent_count << " observed=" << observed_count + << " dominant_matching=" << dominant_matching_count + << " exceptions=" << exception_count << "\n" + << " exception_ratio=" + << (observed_count ? static_cast(exception_count) / + static_cast(observed_count) + : 0.) + << " mean_observed_per_parent=" + << (parent_count ? static_cast(observed_count) / + static_cast(parent_count) + : 0.) + << "\n" + << " serialized_discrete_bytes=" << serialized_discrete_bytes << " ground=" << ground << " obstacle=" << obstacle << "\n"; return 0; } diff --git a/interfaces/ros1/wavemap_ros/app/ouster_layered/occupancy_chunked_sensor_inputs.cc b/interfaces/ros1/wavemap_ros/app/ouster_layered/occupancy_chunked_sensor_inputs.cc new file mode 100644 index 000000000..1e8f4072c --- /dev/null +++ b/interfaces/ros1/wavemap_ros/app/ouster_layered/occupancy_chunked_sensor_inputs.cc @@ -0,0 +1,44 @@ +#include "occupancy_map_layers.h" + +#include + +#include +#include + +namespace wavemap::examples::occupancy_map::chunked { + +using Definition = ChunkedDefinition; +using RosConverter = + convert::DescriptorContinuousRosConverter; +using ServerBuilder = LayeredRosServerBuilder; + +void registerSensorInputs(RosServer& /*server*/, + ServerBuilder::Extension& /*extension*/) { + // Occupancy continues to use Wavemap's original point-cloud integration. +} + +} // namespace wavemap::examples::occupancy_map::chunked + +int main(int argc, char** argv) { + ros::init(argc, argv, "wavemap_ros_server"); + google::InitGoogleLogging(argv[0]); + google::InstallFailureSignalHandler(); + FLAGS_alsologtostderr = true; + FLAGS_colorlogtostderr = true; + + ros::NodeHandle nh; + ros::NodeHandle nh_private("~"); + wavemap::RosServer server( + nh, nh_private, + [](wavemap::RosServer& server, ros::NodeHandle private_nh) { + namespace map = wavemap::examples::occupancy_map::chunked; + map::ServerBuilder::install( + server, private_nh, server.worldFrame(), + wavemap::examples::occupancy_map::makeMapConfig(), + map::registerSensorInputs); + ROS_INFO("Installed chunked layered occupancy-only benchmark."); + }); + + ros::spin(); + return 0; +} diff --git a/examples/cpp/digital_twin/ouster_mapping/occupancy_map_layers.h b/interfaces/ros1/wavemap_ros/app/ouster_layered/occupancy_map_layers.h similarity index 55% rename from examples/cpp/digital_twin/ouster_mapping/occupancy_map_layers.h rename to interfaces/ros1/wavemap_ros/app/ouster_layered/occupancy_map_layers.h index 771e3afe1..d4ead448a 100644 --- a/examples/cpp/digital_twin/ouster_mapping/occupancy_map_layers.h +++ b/interfaces/ros1/wavemap_ros/app/ouster_layered/occupancy_map_layers.h @@ -1,19 +1,25 @@ #ifndef WAVEMAP_EXAMPLES_DIGITAL_TWIN_OUSTER_OCCUPANCY_MAP_LAYERS_H_ #define WAVEMAP_EXAMPLES_DIGITAL_TWIN_OUSTER_OCCUPANCY_MAP_LAYERS_H_ -#include -#include -#include +#include +#include +#include namespace wavemap::examples::occupancy_map { // Occupancy is built into every layered map, so this schema intentionally has // no user-defined layers. It is benchmark variant B. using Schema = layered::schema::LayerSchema<>; -using Definition = layered::LayeredMapDefinition; +template +using DefinitionFor = + layered::LayeredMapDefinition; +using Definition = DefinitionFor; +using ChunkedDefinition = + DefinitionFor; -inline Definition::Config makeMapConfig() { - layered::LayeredMapConfigBuilder config; +template +inline typename DefinitionT::Config makeMapConfig() { + layered::LayeredMapConfigBuilder config; config.map() .minCellWidth(0.25f) .occupancyLogOdds(-2.f, 4.f) diff --git a/examples/cpp/digital_twin/ouster_mapping/occupancy_sensor_inputs.cc b/interfaces/ros1/wavemap_ros/app/ouster_layered/occupancy_sensor_inputs.cc similarity index 100% rename from examples/cpp/digital_twin/ouster_mapping/occupancy_sensor_inputs.cc rename to interfaces/ros1/wavemap_ros/app/ouster_layered/occupancy_sensor_inputs.cc diff --git a/examples/cpp/digital_twin/ouster_mapping/ouster_sensor_inputs.cc b/interfaces/ros1/wavemap_ros/app/ouster_layered/ouster_sensor_inputs.cc similarity index 88% rename from examples/cpp/digital_twin/ouster_mapping/ouster_sensor_inputs.cc rename to interfaces/ros1/wavemap_ros/app/ouster_layered/ouster_sensor_inputs.cc index 94ce7e737..ead884360 100644 --- a/examples/cpp/digital_twin/ouster_mapping/ouster_sensor_inputs.cc +++ b/interfaces/ros1/wavemap_ros/app/ouster_layered/ouster_sensor_inputs.cc @@ -46,12 +46,14 @@ int main(int argc, char** argv) { nh, nh_private, [](wavemap::RosServer& server, ros::NodeHandle private_nh) { namespace map = wavemap::examples::reflectivity_map; + int discrete_block_height = 0; + private_nh.param("discrete_block_height", discrete_block_height, 0); map::ServerBuilder::install(server, private_nh, server.worldFrame(), - map::makeMapConfig(), + map::makeMapConfig(discrete_block_height), map::registerSensorInputs); ROS_INFO( "Installed occupancy+reflectivity+class benchmark application " - "(D)."); + "(D) with discrete block height %d.", discrete_block_height); }); ros::spin(); diff --git a/interfaces/ros1/wavemap_ros/app/ouster_layered/reflectivity_chunked_sensor_inputs.cc b/interfaces/ros1/wavemap_ros/app/ouster_layered/reflectivity_chunked_sensor_inputs.cc new file mode 100644 index 000000000..484edd721 --- /dev/null +++ b/interfaces/ros1/wavemap_ros/app/ouster_layered/reflectivity_chunked_sensor_inputs.cc @@ -0,0 +1,51 @@ +#include "reflectivity_only_map_layers.h" + +#include + +#include +#include + +namespace wavemap::examples::reflectivity_only_map { +namespace chunked { + +using Definition = ChunkedDefinition; +using RosConverter = + convert::DescriptorContinuousRosConverter; +using ServerBuilder = LayeredRosServerBuilder; + +void registerSensorInputs(RosServer& server, + ServerBuilder::Extension& extension) { + server.bindNormalizedPointField( + extension, "reflectivity", 0.f, 255.f, + layered::EndpointRange{0.5f, 25.f}); +} + +} // namespace chunked +} // namespace wavemap::examples::reflectivity_only_map + +int main(int argc, char** argv) { + ros::init(argc, argv, "wavemap_ros_server"); + google::InitGoogleLogging(argv[0]); + google::InstallFailureSignalHandler(); + FLAGS_alsologtostderr = true; + FLAGS_colorlogtostderr = true; + + ros::NodeHandle nh; + ros::NodeHandle nh_private("~"); + wavemap::RosServer server( + nh, nh_private, + [](wavemap::RosServer& server, ros::NodeHandle private_nh) { + namespace map = + wavemap::examples::reflectivity_only_map::chunked; + map::ServerBuilder::install( + server, private_nh, server.worldFrame(), + wavemap::examples::reflectivity_only_map:: + makeMapConfig(), + map::registerSensorInputs); + ROS_INFO( + "Installed chunked occupancy+reflectivity application."); + }); + + ros::spin(); + return 0; +} diff --git a/interfaces/ros1/wavemap_ros/app/ouster_layered/reflectivity_class_chunked_sensor_inputs.cc b/interfaces/ros1/wavemap_ros/app/ouster_layered/reflectivity_class_chunked_sensor_inputs.cc new file mode 100644 index 000000000..f93eb68ed --- /dev/null +++ b/interfaces/ros1/wavemap_ros/app/ouster_layered/reflectivity_class_chunked_sensor_inputs.cc @@ -0,0 +1,58 @@ +#include "reflectivity_map_layers.h" + +#include + +#include +#include +#include + +namespace wavemap::examples::reflectivity_map::chunked { + +using Definition = ChunkedDefinition; +using RosConverter = + convert::DescriptorContinuousRosConverter; +using ServerBuilder = LayeredRosServerBuilder; + +void registerSensorInputs(RosServer& server, + ServerBuilder::Extension& extension) { + server.bindNormalizedPointField( + extension, "reflectivity", 0.f, 255.f, + layered::EndpointRange{0.5f, 25.f}); + + layered::LocalElevationClassifierConfig class_config; + class_config.grid_cell_width = 0.5f; + class_config.ground_tolerance = 0.2f; + class_config.obstacle_height = 0.35f; + class_config.min_points_per_cell = 3u; + server.bindGeometricClass( + extension, class_config, layered::EndpointRange{0.5f, 25.f}); +} + +} // namespace wavemap::examples::reflectivity_map::chunked + +int main(int argc, char** argv) { + ros::init(argc, argv, "wavemap_ros_server"); + google::InitGoogleLogging(argv[0]); + google::InstallFailureSignalHandler(); + FLAGS_alsologtostderr = true; + FLAGS_colorlogtostderr = true; + + ros::NodeHandle nh; + ros::NodeHandle nh_private("~"); + wavemap::RosServer server( + nh, nh_private, + [](wavemap::RosServer& server, ros::NodeHandle private_nh) { + namespace map = wavemap::examples::reflectivity_map::chunked; + map::ServerBuilder::install( + server, private_nh, server.worldFrame(), + wavemap::examples::reflectivity_map:: + makeMapConfig(), + map::registerSensorInputs); + ROS_INFO( + "Installed chunked occupancy+reflectivity+class benchmark " + "application."); + }); + + ros::spin(); + return 0; +} diff --git a/examples/cpp/digital_twin/ouster_mapping/reflectivity_map_layers.h b/interfaces/ros1/wavemap_ros/app/ouster_layered/reflectivity_map_layers.h similarity index 71% rename from examples/cpp/digital_twin/ouster_mapping/reflectivity_map_layers.h rename to interfaces/ros1/wavemap_ros/app/ouster_layered/reflectivity_map_layers.h index 20669fa0d..1a6eb88c1 100644 --- a/examples/cpp/digital_twin/ouster_mapping/reflectivity_map_layers.h +++ b/interfaces/ros1/wavemap_ros/app/ouster_layered/reflectivity_map_layers.h @@ -4,10 +4,10 @@ #include #include -#include -#include -#include -#include +#include +#include +#include +#include namespace wavemap::examples::reflectivity_map { @@ -39,21 +39,28 @@ struct ClassLayer : layered::schema::DiscreteLayer; -using Definition = layered::LayeredMapDefinition; +template +using DefinitionFor = + layered::LayeredMapDefinition; +using Definition = DefinitionFor; +using ChunkedDefinition = + DefinitionFor; -inline Definition::Config makeMapConfig() { - layered::LayeredMapConfigBuilder config; +template +inline typename DefinitionT::Config makeMapConfig( + int discrete_block_height = 0) { + layered::LayeredMapConfigBuilder config; config.map() .minCellWidth(0.25f) .occupancyLogOdds(-2.f, 4.f) .treeHeight(7) .onlyPruneBlocksIfUnusedFor(5.f); config.occupancy().pruningScale(1e-3f).pruningWeight(1.f); - config.layer() + config.template layer() .storageBounds(0.f, 1.f) .pruningScale(1e-3f) .pruningWeight(1.f); - config.discreteBlockHeight(0); + config.discreteBlockHeight(discrete_block_height); config.combinedPruningThreshold(1.f); return config.build(); } diff --git a/examples/cpp/digital_twin/ouster_mapping/reflectivity_only_map_layers.h b/interfaces/ros1/wavemap_ros/app/ouster_layered/reflectivity_only_map_layers.h similarity index 63% rename from examples/cpp/digital_twin/ouster_mapping/reflectivity_only_map_layers.h rename to interfaces/ros1/wavemap_ros/app/ouster_layered/reflectivity_only_map_layers.h index f230e2f20..6fec5239d 100644 --- a/examples/cpp/digital_twin/ouster_mapping/reflectivity_only_map_layers.h +++ b/interfaces/ros1/wavemap_ros/app/ouster_layered/reflectivity_only_map_layers.h @@ -7,17 +7,23 @@ namespace wavemap::examples::reflectivity_only_map { using ReflectivityLayer = reflectivity_map::ReflectivityLayer; using Schema = layered::schema::LayerSchema; -using Definition = layered::LayeredMapDefinition; +template +using DefinitionFor = + layered::LayeredMapDefinition; +using Definition = DefinitionFor; +using ChunkedDefinition = + DefinitionFor; -inline Definition::Config makeMapConfig() { - layered::LayeredMapConfigBuilder config; +template +inline typename DefinitionT::Config makeMapConfig() { + layered::LayeredMapConfigBuilder config; config.map() .minCellWidth(0.25f) .occupancyLogOdds(-2.f, 4.f) .treeHeight(7) .onlyPruneBlocksIfUnusedFor(5.f); config.occupancy().pruningScale(1e-3f).pruningWeight(1.f); - config.layer() + config.template layer() .storageBounds(0.f, 1.f) .pruningScale(1e-3f) .pruningWeight(1.f); diff --git a/examples/cpp/digital_twin/ouster_mapping/reflectivity_only_sensor_inputs.cc b/interfaces/ros1/wavemap_ros/app/ouster_layered/reflectivity_only_sensor_inputs.cc similarity index 100% rename from examples/cpp/digital_twin/ouster_mapping/reflectivity_only_sensor_inputs.cc rename to interfaces/ros1/wavemap_ros/app/ouster_layered/reflectivity_only_sensor_inputs.cc diff --git a/interfaces/ros1/wavemap_ros/app/ouster_layered/rgb_map_layers.h b/interfaces/ros1/wavemap_ros/app/ouster_layered/rgb_map_layers.h new file mode 100644 index 000000000..b3e220bc5 --- /dev/null +++ b/interfaces/ros1/wavemap_ros/app/ouster_layered/rgb_map_layers.h @@ -0,0 +1,43 @@ +#ifndef WAVEMAP_ROS_APP_OUSTER_LAYERED_RGB_MAP_LAYERS_H_ +#define WAVEMAP_ROS_APP_OUSTER_LAYERED_RGB_MAP_LAYERS_H_ + +#include + +#include +#include +#include +#include +#include + +namespace wavemap::examples::rgb_map { + +struct RgbLayer + : layered::schema::ContinuousLayer< + layered::Rgb, + layered::ReplaceLayerUpdatePolicy> { + static constexpr std::string_view name = "rgb"; +}; + +using Schema = layered::schema::LayerSchema; +using Definition = layered::LayeredMapDefinition; + +inline Definition::Config makeMapConfig() { + layered::LayeredMapConfigBuilder config; + config.map() + .minCellWidth(0.25f) + .occupancyLogOdds(-2.f, 4.f) + .treeHeight(7) + .onlyPruneBlocksIfUnusedFor(5.f); + config.occupancy().pruningScale(1e-3f).pruningWeight(1.f); + config.layer() + .storageBounds(layered::Rgb{0.f, 0.f, 0.f}, + layered::Rgb{1.f, 1.f, 1.f}) + .pruningScale(1e-3f) + .pruningWeight(1.f); + config.combinedPruningThreshold(1.f); + return config.build(); +} + +} // namespace wavemap::examples::rgb_map + +#endif // WAVEMAP_ROS_APP_OUSTER_LAYERED_RGB_MAP_LAYERS_H_ diff --git a/interfaces/ros1/wavemap_ros/app/ouster_layered/signal_class_sensor_inputs.cc b/interfaces/ros1/wavemap_ros/app/ouster_layered/signal_class_sensor_inputs.cc new file mode 100644 index 000000000..ace5a0168 --- /dev/null +++ b/interfaces/ros1/wavemap_ros/app/ouster_layered/signal_class_sensor_inputs.cc @@ -0,0 +1,61 @@ +#include "signal_map_layers.h" + +#include + +#include +#include + +namespace wavemap::examples::signal_map::signal_class_map { + +using Definition = ClassDefinition; +using RosConverter = + convert::DescriptorContinuousRosConverter; +using ServerBuilder = LayeredRosServerBuilder; + +void registerSensorInputs(RosServer& server, + ServerBuilder::Extension& extension) { + const layered::EndpointRange endpoint_range{0.5f, 25.f}; + server.bindNormalizedPointField( + extension, "reflectivity", 0.f, 255.f, endpoint_range); + server.bindNormalizedPointField( + extension, "intensity", 0.f, 65535.f, endpoint_range); + layered::LocalElevationClassifierConfig class_config; + class_config.grid_cell_width = 0.5f; + class_config.ground_tolerance = 0.2f; + class_config.obstacle_height = 0.35f; + class_config.min_points_per_cell = 3u; + server.bindGeometricClass( + extension, class_config, endpoint_range); +} + +} // namespace wavemap::examples::signal_map::signal_class_map + +int main(int argc, char** argv) { + ros::init(argc, argv, "wavemap_ros_server"); + google::InitGoogleLogging(argv[0]); + google::InstallFailureSignalHandler(); + FLAGS_alsologtostderr = true; + FLAGS_colorlogtostderr = true; + + ros::NodeHandle nh; + ros::NodeHandle nh_private("~"); + wavemap::RosServer server( + nh, nh_private, + [](wavemap::RosServer& server, ros::NodeHandle private_nh) { + namespace map = wavemap::examples::signal_map::signal_class_map; + int discrete_block_height = 0; + private_nh.param("discrete_block_height", discrete_block_height, 0); + map::ServerBuilder::install( + server, private_nh, server.worldFrame(), + wavemap::examples::signal_map::makeMapConfigFor( + discrete_block_height), + map::registerSensorInputs); + ROS_INFO( + "Installed occupancy+reflectivity+signal+class benchmark " + "application (G) with discrete block height %d.", + discrete_block_height); + }); + + ros::spin(); + return 0; +} diff --git a/interfaces/ros1/wavemap_ros/app/ouster_layered/signal_map_layers.h b/interfaces/ros1/wavemap_ros/app/ouster_layered/signal_map_layers.h new file mode 100644 index 000000000..e06028016 --- /dev/null +++ b/interfaces/ros1/wavemap_ros/app/ouster_layered/signal_map_layers.h @@ -0,0 +1,86 @@ +#ifndef WAVEMAP_EXAMPLES_OUSTER_SIGNAL_MAP_LAYERS_H_ +#define WAVEMAP_EXAMPLES_OUSTER_SIGNAL_MAP_LAYERS_H_ + +#include + +#include "reflectivity_map_layers.h" + +namespace wavemap::examples::signal_map { + +using ReflectivityLayer = reflectivity_map::ReflectivityLayer; +using ClassLayer = reflectivity_map::ClassLayer; +using SignalPolicy = layered::ExponentialScalarLayerUpdatePolicy<1, 5>; + +struct SignalLayer + : layered::schema::ContinuousLayer { + static constexpr layered::ScalarLayerVisualization visualization{ + {0.02f, 0.04f, 0.20f}, {0.00f, 1.00f, 1.00f}}; + static constexpr std::string_view name = "signal"; +}; + +struct NearIrLayer + : layered::schema::ContinuousLayer { + static constexpr layered::ScalarLayerVisualization visualization{ + {0.08f, 0.00f, 0.15f}, {1.00f, 0.85f, 0.00f}}; + static constexpr std::string_view name = "near_ir"; +}; + +// Occupancy remains built into every shared continuous voxel. +using Schema = + layered::schema::LayerSchema; +using NearIrSchema = layered::schema::LayerSchema< + ReflectivityLayer, SignalLayer, NearIrLayer>; +using ClassSchema = layered::schema::LayerSchema< + ReflectivityLayer, SignalLayer, ClassLayer>; +using NearIrClassSchema = layered::schema::LayerSchema< + ReflectivityLayer, SignalLayer, NearIrLayer, ClassLayer>; + +template +using DefinitionFor = + layered::LayeredMapDefinition; +using Definition = DefinitionFor; +using NearIrDefinition = DefinitionFor; +using ClassDefinition = DefinitionFor; +using NearIrClassDefinition = DefinitionFor; + +template +inline typename DefinitionT::Config makeMapConfigFor( + [[maybe_unused]] int discrete_block_height = 0) { + layered::LayeredMapConfigBuilder config; + config.map() + .minCellWidth(0.25f) + .occupancyLogOdds(-2.f, 4.f) + .treeHeight(7) + .onlyPruneBlocksIfUnusedFor(5.f); + config.occupancy().pruningScale(1e-3f).pruningWeight(1.f); + config.template layer() + .storageBounds(0.f, 1.f) + .pruningScale(1e-3f) + .pruningWeight(1.f); + config.template layer() + .storageBounds(0.f, 1.f) + .pruningScale(1e-3f) + .pruningWeight(1.f); + if constexpr ( + DefinitionT::Schema::template containsContinuous) { + config.template layer() + .storageBounds(0.f, 1.f) + .pruningScale(1e-3f) + .pruningWeight(1.f); + } + if constexpr ( + DefinitionT::Schema::template containsDiscrete) { + config.discreteBlockHeight(discrete_block_height); + } + config.combinedPruningThreshold(1.f); + return config.build(); +} + +inline Definition::Config makeMapConfig() { + return makeMapConfigFor(); +} + +} // namespace wavemap::examples::signal_map + +#endif // WAVEMAP_EXAMPLES_OUSTER_SIGNAL_MAP_LAYERS_H_ diff --git a/interfaces/ros1/wavemap_ros/app/ouster_layered/signal_near_ir_class_sensor_inputs.cc b/interfaces/ros1/wavemap_ros/app/ouster_layered/signal_near_ir_class_sensor_inputs.cc new file mode 100644 index 000000000..839f1ea41 --- /dev/null +++ b/interfaces/ros1/wavemap_ros/app/ouster_layered/signal_near_ir_class_sensor_inputs.cc @@ -0,0 +1,64 @@ +#include "signal_map_layers.h" + +#include + +#include +#include + +namespace wavemap::examples::signal_map::near_ir_class_map { + +using Definition = NearIrClassDefinition; +using RosConverter = + convert::DescriptorContinuousRosConverter; +using ServerBuilder = LayeredRosServerBuilder; + +void registerSensorInputs(RosServer& server, + ServerBuilder::Extension& extension) { + const layered::EndpointRange endpoint_range{0.5f, 25.f}; + server.bindNormalizedPointField( + extension, "reflectivity", 0.f, 255.f, endpoint_range); + server.bindNormalizedPointField( + extension, "intensity", 0.f, 65535.f, endpoint_range); + server.bindNormalizedPointField( + extension, "ambient", 0.f, 65535.f, endpoint_range); + + layered::LocalElevationClassifierConfig class_config; + class_config.grid_cell_width = 0.5f; + class_config.ground_tolerance = 0.2f; + class_config.obstacle_height = 0.35f; + class_config.min_points_per_cell = 3u; + server.bindGeometricClass( + extension, class_config, endpoint_range, -kHalfPi, kHalfPi); +} + +} // namespace wavemap::examples::signal_map::near_ir_class_map + +int main(int argc, char** argv) { + ros::init(argc, argv, "wavemap_ros_server"); + google::InitGoogleLogging(argv[0]); + google::InstallFailureSignalHandler(); + FLAGS_alsologtostderr = true; + FLAGS_colorlogtostderr = true; + + ros::NodeHandle nh; + ros::NodeHandle nh_private("~"); + wavemap::RosServer server( + nh, nh_private, + [](wavemap::RosServer& server, ros::NodeHandle private_nh) { + namespace map = wavemap::examples::signal_map::near_ir_class_map; + int discrete_block_height = 0; + private_nh.param("discrete_block_height", discrete_block_height, 0); + map::ServerBuilder::install( + server, private_nh, server.worldFrame(), + wavemap::examples::signal_map::makeMapConfigFor( + discrete_block_height), + map::registerSensorInputs); + ROS_INFO( + "Installed occupancy+reflectivity+signal+near_ir+class benchmark " + "application (H) with discrete block height %d.", + discrete_block_height); + }); + + ros::spin(); + return 0; +} diff --git a/interfaces/ros1/wavemap_ros/app/ouster_layered/signal_near_ir_sensor_inputs.cc b/interfaces/ros1/wavemap_ros/app/ouster_layered/signal_near_ir_sensor_inputs.cc new file mode 100644 index 000000000..6186f445e --- /dev/null +++ b/interfaces/ros1/wavemap_ros/app/ouster_layered/signal_near_ir_sensor_inputs.cc @@ -0,0 +1,50 @@ +#include "signal_map_layers.h" + +#include + +#include +#include + +namespace wavemap::examples::signal_map::near_ir_map { + +using Definition = NearIrDefinition; +using RosConverter = + convert::DescriptorContinuousRosConverter; +using ServerBuilder = LayeredRosServerBuilder; + +void registerSensorInputs(RosServer& server, + ServerBuilder::Extension& extension) { + const layered::EndpointRange endpoint_range{0.5f, 25.f}; + server.bindNormalizedPointField( + extension, "reflectivity", 0.f, 255.f, endpoint_range); + server.bindNormalizedPointField( + extension, "intensity", 0.f, 65535.f, endpoint_range); + server.bindNormalizedPointField( + extension, "ambient", 0.f, 65535.f, endpoint_range); +} + +} // namespace wavemap::examples::signal_map::near_ir_map + +int main(int argc, char** argv) { + ros::init(argc, argv, "wavemap_ros_server"); + google::InitGoogleLogging(argv[0]); + google::InstallFailureSignalHandler(); + FLAGS_alsologtostderr = true; + FLAGS_colorlogtostderr = true; + + ros::NodeHandle nh; + ros::NodeHandle nh_private("~"); + wavemap::RosServer server( + nh, nh_private, + [](wavemap::RosServer& server, ros::NodeHandle private_nh) { + namespace map = wavemap::examples::signal_map::near_ir_map; + map::ServerBuilder::install( + server, private_nh, server.worldFrame(), + wavemap::examples::signal_map::makeMapConfigFor(), + map::registerSensorInputs); + ROS_INFO("Installed occupancy+reflectivity+signal+near_ir benchmark application (F)."); + }); + + ros::spin(); + return 0; +} diff --git a/interfaces/ros1/wavemap_ros/app/ouster_layered/signal_sensor_inputs.cc b/interfaces/ros1/wavemap_ros/app/ouster_layered/signal_sensor_inputs.cc new file mode 100644 index 000000000..526d17a27 --- /dev/null +++ b/interfaces/ros1/wavemap_ros/app/ouster_layered/signal_sensor_inputs.cc @@ -0,0 +1,50 @@ +#include "signal_map_layers.h" + +#include + +#include +#include + +namespace wavemap::examples::signal_map { + +using RosConverter = + convert::DescriptorContinuousRosConverter; +using ServerBuilder = LayeredRosServerBuilder; + +void registerSensorInputs(RosServer& server, + ServerBuilder::Extension& extension) { + const layered::EndpointRange endpoint_range{0.5f, 25.f}; + server.bindNormalizedPointField( + extension, "reflectivity", 0.f, 255.f, endpoint_range); + // The bag's Ouster profile is SIG16. Preserve its full sensor-defined range + // in normalized storage rather than fitting bounds to this recording. + server.bindNormalizedPointField( + extension, "intensity", 0.f, 65535.f, endpoint_range); +} + +} // namespace wavemap::examples::signal_map + +int main(int argc, char** argv) { + ros::init(argc, argv, "wavemap_ros_server"); + google::InitGoogleLogging(argv[0]); + google::InstallFailureSignalHandler(); + FLAGS_alsologtostderr = true; + FLAGS_colorlogtostderr = true; + + ros::NodeHandle nh; + ros::NodeHandle nh_private("~"); + wavemap::RosServer server( + nh, nh_private, + [](wavemap::RosServer& server, ros::NodeHandle private_nh) { + namespace map = wavemap::examples::signal_map; + map::ServerBuilder::install(server, private_nh, server.worldFrame(), + map::makeMapConfig(), + map::registerSensorInputs); + ROS_INFO( + "Installed occupancy+reflectivity+signal benchmark application " + "(E)."); + }); + + ros::spin(); + return 0; +} diff --git a/interfaces/ros1/wavemap_ros/app/publish_field_layer_updates_experiment.cc b/interfaces/ros1/wavemap_ros/app/publish_field_layer_updates_experiment.cc deleted file mode 100644 index 0461e7838..000000000 --- a/interfaces/ros1/wavemap_ros/app/publish_field_layer_updates_experiment.cc +++ /dev/null @@ -1,47 +0,0 @@ -#include -#include - -namespace { -geometry_msgs::Point32 point(float x, float y, float z) { - geometry_msgs::Point32 result; - result.x = x; - result.y = y; - result.z = z; - return result; -} -} // namespace - -int main(int argc, char** argv) { - ros::init(argc, argv, "publish_field_layer_updates_experiment"); - ros::NodeHandle nh; - ros::NodeHandle nh_private("~"); - - std::string world_frame = "map"; - nh_private.param("world_frame", world_frame, world_frame); - - ros::Publisher publisher = - nh.advertise( - "/wavemap/layered_map_updates", 1, true); - - wavemap_msgs::LayeredMapUpdate update; - update.header.frame_id = world_frame; - update.header.stamp = ros::Time::now(); - update.positions = { - point(0.05f, 0.05f, 0.05f), - point(0.30f, 0.05f, 0.05f), - point(0.05f, 0.30f, 0.05f), - point(1.05f, 0.05f, 0.05f)}; - - wavemap_msgs::Layer class_layer; - class_layer.name = "class"; - class_layer.type = "int32"; - class_layer.int32_values = {1, 1, 2, 3}; - update.layers.emplace_back(class_layer); - - publisher.publish(update); - ROS_INFO_STREAM("Published " << update.positions.size() - << " class-only observations on " - << publisher.getTopic() << "."); - ros::spin(); - return 0; -} diff --git a/interfaces/ros1/wavemap_ros/app/synthetic_rgb_layered_map_demo.cc b/interfaces/ros1/wavemap_ros/app/synthetic_rgb_layered_map_demo.cc new file mode 100644 index 000000000..800a9db68 --- /dev/null +++ b/interfaces/ros1/wavemap_ros/app/synthetic_rgb_layered_map_demo.cc @@ -0,0 +1,200 @@ +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include "ouster_layered/rgb_map_layers.h" + +namespace wavemap::examples::rgb_map { +namespace { + +struct SceneVoxel { + Index3D index; + layered::Rgb color; +}; + +constexpr FloatingPoint kOccupiedLogOdds = 4.f; +constexpr int kFloorCells = 52; +constexpr int kPathHalfWidth = 3; + +const layered::Rgb kGrass{0.20f, 0.55f, 0.16f}; +const layered::Rgb kPath{0.46f, 0.27f, 0.12f}; +const layered::Rgb kTrunk{0.34f, 0.16f, 0.06f}; + +void appendFloor(std::vector& scene) { + const int min_coordinate = -kFloorCells / 2; + const int max_coordinate = min_coordinate + kFloorCells; + scene.reserve(kFloorCells * kFloorCells); + for (int y = min_coordinate; y < max_coordinate; ++y) { + const int path_center = + static_cast(std::round(2.5 * std::sin(y / 9.0))); + for (int x = min_coordinate; x < max_coordinate; ++x) { + const bool is_path = std::abs(x - path_center) <= kPathHalfWidth; + scene.push_back({Index3D{x, y, 0}, is_path ? kPath : kGrass}); + } + } +} + +void appendTree(const Index2D& center, std::mt19937& generator, + std::vector& scene) { + for (int z = 1; z <= 8; ++z) { + for (int dx = 0; dx < 2; ++dx) { + for (int dy = 0; dy < 2; ++dy) { + scene.push_back( + {Index3D{center.x() + dx, center.y() + dy, z}, kTrunk}); + } + } + } + + std::uniform_real_distribution random_value(0.f, 1.f); + std::uniform_real_distribution green_variation(-0.10f, 0.10f); + std::unordered_set added_leaf_indices; + for (int dz = -2; dz <= 3; ++dz) { + for (int dx = -4; dx <= 5; ++dx) { + for (int dy = -4; dy <= 5; ++dy) { + const float nx = (static_cast(dx) - 0.5f) / 4.6f; + const float ny = (static_cast(dy) - 0.5f) / 4.6f; + const float nz = (static_cast(dz) - 0.5f) / 3.2f; + const float radius = nx * nx + ny * ny + nz * nz; + const float keep_probability = + std::clamp(1.25f - radius, 0.f, 0.92f); + if (random_value(generator) > keep_probability) { + continue; + } + + const Index3D index{center.x() + dx, center.y() + dy, 9 + dz}; + if (!added_leaf_indices.emplace(index).second) { + continue; + } + const float variation = green_variation(generator); + scene.push_back( + {index, + layered::Rgb{std::clamp(0.12f + 0.4f * variation, 0.f, 1.f), + std::clamp(0.48f + variation, 0.f, 1.f), + std::clamp(0.10f + 0.25f * variation, 0.f, 1.f)}}); + } + } + } +} + +std::vector makeScene() { + std::vector scene; + appendFloor(scene); + + std::mt19937 generator(42u); + for (const Index2D& center : std::vector{ + {-18, -17}, {-17, 14}, {-11, -4}, {12, -17}, {16, 13}, {10, 4}}) { + appendTree(center, generator, scene); + } + return scene; +} + +} // namespace + +class SyntheticRgbLayeredMapDemo { + public: + SyntheticRgbLayeredMapDemo() + : private_nh_("~"), map_(makeMapConfig()), scene_(makeScene()) { + private_nh_.param("voxels_per_update", voxels_per_update_, 120); + private_nh_.param("update_period", update_period_, 0.08); + private_nh_.param("save_path", save_path_, std::string{}); + voxels_per_update_ = std::max(1, voxels_per_update_); + update_period_ = std::max(0.01, update_period_); + + publisher_ = private_nh_.advertise( + "layered_map", 1, true); + timer_ = private_nh_.createTimer( + ros::Duration(update_period_), + &SyntheticRgbLayeredMapDemo::insertNextVoxels, this); + + ROS_INFO_STREAM("Synthetic RGB Layered Wavemap contains " << scene_.size() + << " voxels and will be revealed in batches of " + << voxels_per_update_ << "."); + } + + private: + void insertNextVoxels(const ros::TimerEvent&) { + const size_t next_end = std::min( + next_voxel_ + static_cast(voxels_per_update_), scene_.size()); + for (; next_voxel_ < next_end; ++next_voxel_) { + Definition::ContinuousLayers layers; + layers.get() = scene_[next_voxel_].color; + map_.continuousMap().setVoxelValue( + scene_[next_voxel_].index, + Definition::Voxel{kOccupiedLogOdds, layers}); + } + publishMap(); + + if (next_voxel_ == scene_.size()) { + timer_.stop(); + map_.continuousMap().threshold(); + map_.continuousMap().prune(); + publishMap(); + saveMapIfRequested(); + ROS_INFO("Synthetic RGB Layered Wavemap is complete."); + } + } + + void publishMap() { + wavemap_msgs::LayeredMap message; + using RosConverter = + convert::DescriptorContinuousRosConverter; + if (!convert::layeredMapToRosMsg( + map_, "map", ros::Time::now(), message)) { + ROS_ERROR("Could not convert the synthetic RGB map to a ROS message."); + return; + } + publisher_.publish(message); + } + + void saveMapIfRequested() { + if (save_path_.empty()) { + return; + } + const std::filesystem::path output_path(save_path_); + if (output_path.has_parent_path()) { + std::filesystem::create_directories(output_path.parent_path()); + } + if (Definition::MapIo::save(output_path, map_)) { + ROS_INFO_STREAM("Saved synthetic RGB Layered Wavemap to " + << output_path); + } else { + ROS_ERROR_STREAM("Could not save synthetic RGB Layered Wavemap to " + << output_path); + } + } + + ros::NodeHandle private_nh_; + Definition::Map map_; + std::vector scene_; + size_t next_voxel_ = 0u; + int voxels_per_update_ = 120; + double update_period_ = 0.08; + std::string save_path_; + ros::Publisher publisher_; + ros::Timer timer_; +}; + +} // namespace wavemap::examples::rgb_map + +int main(int argc, char** argv) { + ros::init(argc, argv, "synthetic_rgb_layered_map"); + google::InitGoogleLogging(argv[0]); + google::InstallFailureSignalHandler(); + FLAGS_alsologtostderr = true; + FLAGS_colorlogtostderr = true; + + wavemap::examples::rgb_map::SyntheticRgbLayeredMapDemo demo; + ros::spin(); + return 0; +} diff --git a/interfaces/ros1/wavemap_ros/config/benchmark_ouster_chunked_layered.yaml b/interfaces/ros1/wavemap_ros/config/benchmark_ouster_chunked_layered.yaml new file mode 100644 index 000000000..07fba6e37 --- /dev/null +++ b/interfaces/ros1/wavemap_ros/config/benchmark_ouster_chunked_layered.yaml @@ -0,0 +1,43 @@ +general: + world_frame: "map" + logging_level: info + allow_reset_map_service: true + +# Layered map geometry and layer configuration are supplied by the selected +# typed server executable. +map_operations: [] + +measurement_integrators: + ouster_os1: + projection_model: + type: ouster_projector + lidar_origin_to_beam_origin: { millimeters: 16.721 } + lidar_origin_to_sensor_origin_z_offset: { millimeters: 38.195 } + elevation: + num_cells: 128 + min_angle: { degrees: -21.39 } + max_angle: { degrees: 21.25 } + azimuth: + num_cells: 1024 + min_angle: { degrees: -180.0 } + max_angle: { degrees: 180.0 } + measurement_model: + type: continuous_beam + angle_sigma: { degrees: 0.1 } + range_sigma: { meters: 0.05 } + scaling_free: 0.2 + scaling_occupied: 0.4 + integration_method: + type: hashed_chunked_wavelet_integrator + min_range: { meters: 0.5 } + max_range: { meters: 25.0 } + +inputs: + - type: pointcloud_topic + topic_name: "/ouster/points" + topic_type: ouster + measurement_integrator_names: ouster_os1 + undistort_motion: false + topic_queue_length: 10 + max_wait_for_pose: { seconds: 1.0 } + enable_benchmark_metrics: true diff --git a/interfaces/ros1/wavemap_ros/config/benchmark_ouster_chunked_occupancy_layered.yaml b/interfaces/ros1/wavemap_ros/config/benchmark_ouster_chunked_occupancy_layered.yaml new file mode 100644 index 000000000..56b0b7cc7 --- /dev/null +++ b/interfaces/ros1/wavemap_ros/config/benchmark_ouster_chunked_occupancy_layered.yaml @@ -0,0 +1,42 @@ +general: + world_frame: "map" + logging_level: debug + allow_reset_map_service: true + +# The layered map geometry is supplied by occupancy_map_layers.h. +# Keep ROS serialization and periodic maintenance outside the integration timer. +map_operations: [] + +measurement_integrators: + ouster_os1: + projection_model: + type: ouster_projector + lidar_origin_to_beam_origin: { millimeters: 16.721 } + lidar_origin_to_sensor_origin_z_offset: { millimeters: 38.195 } + elevation: + num_cells: 128 + min_angle: { degrees: -21.39 } + max_angle: { degrees: 21.25 } + azimuth: + num_cells: 1024 + min_angle: { degrees: -180.0 } + max_angle: { degrees: 180.0 } + measurement_model: + type: continuous_beam + angle_sigma: { degrees: 0.1 } + range_sigma: { meters: 0.05 } + scaling_free: 0.2 + scaling_occupied: 0.4 + integration_method: + type: hashed_chunked_wavelet_integrator + min_range: { meters: 0.5 } + max_range: { meters: 25.0 } + +inputs: + - type: pointcloud_topic + topic_name: "/ouster/points" + topic_type: ouster + measurement_integrator_names: ouster_os1 + undistort_motion: false + topic_queue_length: 10 + max_wait_for_pose: { seconds: 1.0 } diff --git a/interfaces/ros1/wavemap_ros/config/benchmark_ouster_chunked_occupancy_original.yaml b/interfaces/ros1/wavemap_ros/config/benchmark_ouster_chunked_occupancy_original.yaml new file mode 100644 index 000000000..b42a29b36 --- /dev/null +++ b/interfaces/ros1/wavemap_ros/config/benchmark_ouster_chunked_occupancy_original.yaml @@ -0,0 +1,50 @@ +general: + world_frame: "map" + logging_level: info + allow_reset_map_service: true + +map: + type: hashed_chunked_wavelet_octree + min_cell_width: { meters: 0.25 } + min_log_odds: -2.0 + max_log_odds: 4.0 + tree_height: 7 + only_prune_blocks_if_unused_for: { seconds: 5.0 } + +# Keep ROS serialization and periodic maintenance outside the integration timer. +map_operations: [] + +measurement_integrators: + ouster_os1: + projection_model: + type: ouster_projector + lidar_origin_to_beam_origin: { millimeters: 16.721 } + lidar_origin_to_sensor_origin_z_offset: { millimeters: 38.195 } + elevation: + num_cells: 128 + min_angle: { degrees: -21.39 } + max_angle: { degrees: 21.25 } + azimuth: + num_cells: 1024 + min_angle: { degrees: -180.0 } + max_angle: { degrees: 180.0 } + measurement_model: + type: continuous_beam + angle_sigma: { degrees: 0.1 } + range_sigma: { meters: 0.05 } + scaling_free: 0.2 + scaling_occupied: 0.4 + integration_method: + type: hashed_chunked_wavelet_integrator + min_range: { meters: 0.5 } + max_range: { meters: 25.0 } + +inputs: + - type: pointcloud_topic + topic_name: "/ouster/points" + topic_type: ouster + measurement_integrator_names: ouster_os1 + undistort_motion: false + topic_queue_length: 10 + max_wait_for_pose: { seconds: 1.0 } + enable_benchmark_metrics: true diff --git a/interfaces/ros1/wavemap_ros/config/benchmark_ouster_hashed_layered.yaml b/interfaces/ros1/wavemap_ros/config/benchmark_ouster_hashed_layered.yaml new file mode 100644 index 000000000..ef08f851b --- /dev/null +++ b/interfaces/ros1/wavemap_ros/config/benchmark_ouster_hashed_layered.yaml @@ -0,0 +1,43 @@ +general: + world_frame: "map" + logging_level: info + allow_reset_map_service: true + +# Layered map geometry and layer configuration are supplied by the selected +# typed server executable. +map_operations: [] + +measurement_integrators: + ouster_os1: + projection_model: + type: ouster_projector + lidar_origin_to_beam_origin: { millimeters: 16.721 } + lidar_origin_to_sensor_origin_z_offset: { millimeters: 38.195 } + elevation: + num_cells: 128 + min_angle: { degrees: -21.39 } + max_angle: { degrees: 21.25 } + azimuth: + num_cells: 1024 + min_angle: { degrees: -180.0 } + max_angle: { degrees: 180.0 } + measurement_model: + type: continuous_beam + angle_sigma: { degrees: 0.1 } + range_sigma: { meters: 0.05 } + scaling_free: 0.2 + scaling_occupied: 0.4 + integration_method: + type: hashed_wavelet_integrator + min_range: { meters: 0.5 } + max_range: { meters: 25.0 } + +inputs: + - type: pointcloud_topic + topic_name: "/ouster/points" + topic_type: ouster + measurement_integrator_names: ouster_os1 + undistort_motion: false + topic_queue_length: 10 + max_wait_for_pose: { seconds: 1.0 } + enable_benchmark_metrics: true diff --git a/interfaces/ros1/wavemap_ros/config/benchmark_ouster_hashed_occupancy_original.yaml b/interfaces/ros1/wavemap_ros/config/benchmark_ouster_hashed_occupancy_original.yaml new file mode 100644 index 000000000..7af2180ba --- /dev/null +++ b/interfaces/ros1/wavemap_ros/config/benchmark_ouster_hashed_occupancy_original.yaml @@ -0,0 +1,50 @@ +general: + world_frame: "map" + logging_level: info + allow_reset_map_service: true + +map: + type: hashed_wavelet_octree + min_cell_width: { meters: 0.25 } + min_log_odds: -2.0 + max_log_odds: 4.0 + tree_height: 7 + only_prune_blocks_if_unused_for: { seconds: 5.0 } + +# Keep ROS serialization and periodic maintenance outside the integration timer. +map_operations: [] + +measurement_integrators: + ouster_os1: + projection_model: + type: ouster_projector + lidar_origin_to_beam_origin: { millimeters: 16.721 } + lidar_origin_to_sensor_origin_z_offset: { millimeters: 38.195 } + elevation: + num_cells: 128 + min_angle: { degrees: -21.39 } + max_angle: { degrees: 21.25 } + azimuth: + num_cells: 1024 + min_angle: { degrees: -180.0 } + max_angle: { degrees: 180.0 } + measurement_model: + type: continuous_beam + angle_sigma: { degrees: 0.1 } + range_sigma: { meters: 0.05 } + scaling_free: 0.2 + scaling_occupied: 0.4 + integration_method: + type: hashed_wavelet_integrator + min_range: { meters: 0.5 } + max_range: { meters: 25.0 } + +inputs: + - type: pointcloud_topic + topic_name: "/ouster/points" + topic_type: ouster + measurement_integrator_names: ouster_os1 + undistort_motion: false + topic_queue_length: 10 + max_wait_for_pose: { seconds: 1.0 } + enable_benchmark_metrics: true diff --git a/interfaces/ros1/wavemap_ros/config/benchmark_ouster_variants.csv b/interfaces/ros1/wavemap_ros/config/benchmark_ouster_variants.csv new file mode 100644 index 000000000..0a2e44b5e --- /dev/null +++ b/interfaces/ros1/wavemap_ros/config/benchmark_ouster_variants.csv @@ -0,0 +1,19 @@ +variant,backend,layers,server_executable,param_file,output_directory,map_filename +A,hashed,occupancy,ros_server,benchmark_ouster_hashed_occupancy_original.yaml,A_original,map.wvmp +B,hashed,occupancy,occupancy_layered_server,benchmark_ouster_hashed_layered.yaml,B_layered_occupancy,map.lwvmp +C,hashed,occupancy+reflectivity,reflectivity_layered_server,benchmark_ouster_hashed_layered.yaml,C_layered_reflectivity,map.lwvmp +D,hashed,occupancy+reflectivity+class,reflectivity_class_layered_server,benchmark_ouster_hashed_layered.yaml,D_layered_reflectivity_class,map.lwvmp +D_h1,hashed,occupancy+reflectivity+class (discrete block height 1),reflectivity_class_layered_server,benchmark_ouster_hashed_layered.yaml,D_layered_reflectivity_class_h1,map.lwvmp +D_h2,hashed,occupancy+reflectivity+class (discrete block height 2),reflectivity_class_layered_server,benchmark_ouster_hashed_layered.yaml,D_layered_reflectivity_class_h2,map.lwvmp +D_h3,hashed,occupancy+reflectivity+class (discrete block height 3),reflectivity_class_layered_server,benchmark_ouster_hashed_layered.yaml,D_layered_reflectivity_class_h3,map.lwvmp +D_h4,hashed,occupancy+reflectivity+class (discrete block height 4),reflectivity_class_layered_server,benchmark_ouster_hashed_layered.yaml,D_layered_reflectivity_class_h4,map.lwvmp +D_h5,hashed,occupancy+reflectivity+class (discrete block height 5),reflectivity_class_layered_server,benchmark_ouster_hashed_layered.yaml,D_layered_reflectivity_class_h5,map.lwvmp +D_h6,hashed,occupancy+reflectivity+class (discrete block height 6),reflectivity_class_layered_server,benchmark_ouster_hashed_layered.yaml,D_layered_reflectivity_class_h6,map.lwvmp +E,hashed,occupancy+reflectivity+signal,signal_layered_server,benchmark_ouster_hashed_layered.yaml,E_layered_reflectivity_signal,map.lwvmp +F,hashed,occupancy+reflectivity+signal+near_ir,signal_near_ir_layered_server,benchmark_ouster_hashed_layered.yaml,F_layered_reflectivity_signal_near_ir,map.lwvmp +G,hashed,occupancy+reflectivity+signal+class,signal_class_layered_server,benchmark_ouster_hashed_layered.yaml,G_layered_reflectivity_signal_class,map.lwvmp +H,hashed,occupancy+reflectivity+signal+near_ir+class,signal_near_ir_class_layered_server,benchmark_ouster_hashed_layered.yaml,H_layered_reflectivity_signal_near_ir_class,map.lwvmp +chunked_A,chunked,occupancy,ros_server,benchmark_ouster_chunked_occupancy_original.yaml,chunked_A_original,map.wvmp +chunked_B,chunked,occupancy,occupancy_chunked_layered_server,benchmark_ouster_chunked_layered.yaml,chunked_B_layered_occupancy,map.lwvmp +chunked_C,chunked,occupancy+reflectivity,reflectivity_chunked_layered_server,benchmark_ouster_chunked_layered.yaml,chunked_C_layered_reflectivity,map.lwvmp +chunked_D,chunked,occupancy+reflectivity+class,reflectivity_class_chunked_layered_server,benchmark_ouster_chunked_layered.yaml,chunked_D_layered_reflectivity_class,map.lwvmp diff --git a/interfaces/ros1/wavemap_ros/config/rviz/synthetic_rgb_layered_map.rviz b/interfaces/ros1/wavemap_ros/config/rviz/synthetic_rgb_layered_map.rviz new file mode 100644 index 000000000..82e4b20c7 --- /dev/null +++ b/interfaces/ros1/wavemap_ros/config/rviz/synthetic_rgb_layered_map.rviz @@ -0,0 +1,106 @@ +Panels: + - Class: rviz/Displays + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /RGB Layered Wavemap1 + - /RGB Layered Wavemap1/Render voxels1 + Splitter Ratio: 0.5 + Tree Height: 650 + - Class: rviz/Views + Name: Views +Preferences: + PromptSaveOnExit: true +Toolbars: + toolButtonStyle: 2 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.03 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 40 + Reference Frame: + Value: true + - Class: wavemap_rviz_plugin/LayeredMap + Enabled: true + Name: Layered Wavemap + Queue Size: 10 + Source: Topic + Render voxels: + Alpha: 1 + Cell selector: Band + Color mode: Flat + Enable: true + Flat color: 255; 255; 255 + Frame rate: + Max update time: 20 + Layer: rgb + Layer color: + Bool true color: 255; 12; 12 + Scalar high color: 255; 255; 0 + Scalar low color: 0; 0; 255 + Scalar max: 1 + Scalar min: 0 + Show bool false: false + Max log odds: 1000000 + Min log odds: 0.001 + Termination height: 0 + Unknown log odds: 0.0001 + Topic: /synthetic_rgb_layered_map/layered_map + Unreliable: false + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Default Light: true + Fixed Frame: map + Frame Rate: 30 + Name: root + Tools: + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/MoveCamera + - Class: rviz/Select + - Class: rviz/FocusCamera + - Class: rviz/Measure + Value: true + Views: + Current: + Class: rviz/Orbit + Distance: 20 + Enable Stereo Rendering: + Stereo Eye Separation: 0.06 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Field of View: 0.785398 + Focal Point: + X: 0 + Y: 0 + Z: 1 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.01 + Pitch: 0.65 + Target Frame: + Yaw: 0.75 + Saved: ~ +Window Geometry: + Height: 900 + Width: 1440 diff --git a/interfaces/ros1/wavemap_ros/config/wavemap_ouster_all_layers_demo.yaml b/interfaces/ros1/wavemap_ros/config/wavemap_ouster_all_layers_demo.yaml new file mode 100644 index 000000000..cdbd78015 --- /dev/null +++ b/interfaces/ros1/wavemap_ros/config/wavemap_ouster_all_layers_demo.yaml @@ -0,0 +1,51 @@ +general: + world_frame: "map" + logging_level: debug + allow_reset_map_service: true + +map_operations: + - type: threshold_map + once_every: { seconds: 2.0 } + - type: prune_map + once_every: { seconds: 10.0 } + - type: publish_map + once_every: { seconds: 0.5 } + max_num_blocks_per_msg: 50 + topic: "layered_map" + +measurement_integrators: + ouster_os1: + projection_model: + type: ouster_projector + lidar_origin_to_beam_origin: { millimeters: 16.721 } + lidar_origin_to_sensor_origin_z_offset: { millimeters: 38.195 } + elevation: + num_cells: 128 + min_angle: { degrees: -21.39 } + max_angle: { degrees: 21.25 } + azimuth: + num_cells: 1024 + min_angle: { degrees: -180.0 } + max_angle: { degrees: 180.0 } + measurement_model: + type: continuous_beam + angle_sigma: { degrees: 0.1 } + range_sigma: { meters: 0.05 } + scaling_free: 0.2 + scaling_occupied: 0.4 + integration_method: + type: hashed_wavelet_integrator + min_range: { meters: 0.5 } + max_range: { meters: 25.0 } + +inputs: + - type: pointcloud_topic + topic_name: "/ouster/points" + topic_type: ouster + measurement_integrator_names: ouster_os1 + # IG-LIO publishes one map-to-body pose per processed scan. This bag's + # Ouster t field is not usable as Wavemap-relative per-point time, so asking + # Wavemap for intermediate poses drops the cloud. + undistort_motion: false + topic_queue_length: 10 + max_wait_for_pose: { seconds: 1.0 } diff --git a/interfaces/ros1/wavemap_ros/config/wavemap_ouster_reflectivity_chunked_layered.yaml b/interfaces/ros1/wavemap_ros/config/wavemap_ouster_reflectivity_chunked_layered.yaml new file mode 100644 index 000000000..aee2b4188 --- /dev/null +++ b/interfaces/ros1/wavemap_ros/config/wavemap_ouster_reflectivity_chunked_layered.yaml @@ -0,0 +1,48 @@ +general: + world_frame: "map" + logging_level: debug + allow_reset_map_service: true + +map_operations: + - type: threshold_map + once_every: { seconds: 2.0 } + - type: prune_map + once_every: { seconds: 10.0 } + - type: publish_map + once_every: { seconds: 0.5 } + max_num_blocks_per_msg: 50 + topic: "layered_map" + +measurement_integrators: + ouster_os1: + projection_model: + type: ouster_projector + lidar_origin_to_beam_origin: { millimeters: 16.721 } + lidar_origin_to_sensor_origin_z_offset: { millimeters: 38.195 } + elevation: + num_cells: 128 + min_angle: { degrees: -21.39 } + max_angle: { degrees: 21.25 } + azimuth: + num_cells: 1024 + min_angle: { degrees: -180.0 } + max_angle: { degrees: 180.0 } + measurement_model: + type: continuous_beam + angle_sigma: { degrees: 0.1 } + range_sigma: { meters: 0.05 } + scaling_free: 0.2 + scaling_occupied: 0.4 + integration_method: + type: hashed_chunked_wavelet_integrator + min_range: { meters: 0.5 } + max_range: { meters: 25.0 } + +inputs: + - type: pointcloud_topic + topic_name: "/ouster/points" + topic_type: ouster + measurement_integrator_names: ouster_os1 + undistort_motion: false + topic_queue_length: 10 + max_wait_for_pose: { seconds: 1.0 } diff --git a/interfaces/ros1/wavemap_ros/include/wavemap_ros/inputs/pointcloud_topic_input.h b/interfaces/ros1/wavemap_ros/include/wavemap_ros/inputs/pointcloud_topic_input.h index c1b1c1c95..16141e586 100644 --- a/interfaces/ros1/wavemap_ros/include/wavemap_ros/inputs/pointcloud_topic_input.h +++ b/interfaces/ros1/wavemap_ros/include/wavemap_ros/inputs/pointcloud_topic_input.h @@ -3,12 +3,14 @@ #include #include +#include #include #include #include #include #include +#include #include #include @@ -33,7 +35,7 @@ struct PointcloudTopicType : public TypeSelector { * Config struct for the pointcloud input handler. */ struct PointcloudTopicInputConfig - : public ConfigBase { //! Name of the ROS topic to subscribe to. std::string topic_name; @@ -77,6 +79,9 @@ struct PointcloudTopicInputConfig //! Name of the topic on which to republish the motion-undistorted //! pointclouds. Useful to share the undistorted pointclouds with other ROS //! nodes and for debugging. Disabled if not set. + //! Collect aggregate integration statistics and expose them through the + //! private benchmark_stats service. Disabled during normal operation. + bool enable_benchmark_metrics = false; std::string undistorted_pointcloud_topic_name; static MemberMap memberMap; @@ -135,6 +140,25 @@ class PointcloudTopicInput : public RosInputBase { std::queue pointcloud_queue_; std::vector> endpoint_adapters_; std::vector posed_cloud_callbacks_; + + struct BenchmarkMetrics { + size_t received_scans = 0u; + size_t integrated_scans = 0u; + size_t dropped_scans = 0u; + size_t pending_scans = 0u; + size_t received_points = 0u; + size_t integrated_points = 0u; + size_t maximum_queue_size = 0u; + std::vector integration_durations; + } benchmark_metrics_; + std::mutex benchmark_metrics_mutex_; + ros::ServiceServer benchmark_stats_srv_; + + void recordReceivedScan(size_t num_points); + void recordDroppedScan(); + void recordIntegratedScan(size_t num_points, double duration_seconds); + bool benchmarkStatsCallback(std_srvs::Trigger::Request& request, + std_srvs::Trigger::Response& response); void processQueue() override; static bool hasField(const sensor_msgs::PointCloud2& msg, diff --git a/interfaces/ros1/wavemap_ros/include/wavemap_ros/layered_hashed_wavelet_integrator_factory.h b/interfaces/ros1/wavemap_ros/include/wavemap_ros/layered_hashed_wavelet_integrator_factory.h index afc766e8b..f0108bdce 100644 --- a/interfaces/ros1/wavemap_ros/include/wavemap_ros/layered_hashed_wavelet_integrator_factory.h +++ b/interfaces/ros1/wavemap_ros/include/wavemap_ros/layered_hashed_wavelet_integrator_factory.h @@ -7,6 +7,7 @@ #include #include #include +#include #include #include #include @@ -18,15 +19,18 @@ namespace wavemap { // Typed counterpart of the original runtime IntegratorFactory boundary. The // original factory remains unchanged for occupancy-only maps; layered servers // use this path because their voxel type is known at compile time. -template -std::unique_ptr createLayeredHashedWaveletIntegrator( +namespace detail { +template +std::unique_ptr createLayeredWaveletIntegratorImpl( const param::Value& params, - typename HashedWaveletOctreeT::Ptr occupancy_map, + typename MapT::Ptr occupancy_map, IntegratorType expected_type, + const char* expected_type_name, std::shared_ptr thread_pool) { const auto type = IntegratorType::from(params, "integration_method"); - if (!type || type.value() != IntegratorType::kHashedWaveletIntegrator) { - LOG(ERROR) << "A layered hashed wavelet map requires integration_method " - "type hashed_wavelet_integrator."; + if (!type || type.value() != expected_type) { + LOG(ERROR) << "This layered continuous-map backend requires " + "integration_method type " + << expected_type_name << "."; return nullptr; } @@ -55,12 +59,39 @@ std::unique_ptr createLayeredHashedWaveletIntegrator( return nullptr; } - return std::make_unique>( + return std::make_unique( integrator_config.value(), std::move(projection_model), std::move(posed_range_image), std::move(beam_offset_image), std::move(measurement_model), std::move(occupancy_map), std::move(thread_pool)); } +} // namespace detail + +template +std::unique_ptr createLayeredWaveletIntegrator( + const param::Value& params, + typename HashedWaveletOctreeT::Ptr occupancy_map, + std::shared_ptr thread_pool) { + return detail::createLayeredWaveletIntegratorImpl< + HashedWaveletIntegratorT, + HashedWaveletOctreeT>( + params, std::move(occupancy_map), + IntegratorType::kHashedWaveletIntegrator, + "hashed_wavelet_integrator", std::move(thread_pool)); +} + +template +std::unique_ptr createLayeredWaveletIntegrator( + const param::Value& params, + typename HashedChunkedWaveletOctreeT::Ptr occupancy_map, + std::shared_ptr thread_pool) { + return detail::createLayeredWaveletIntegratorImpl< + HashedChunkedWaveletIntegratorT, + HashedChunkedWaveletOctreeT>( + params, std::move(occupancy_map), + IntegratorType::kHashedChunkedWaveletIntegrator, + "hashed_chunked_wavelet_integrator", std::move(thread_pool)); +} } // namespace wavemap diff --git a/interfaces/ros1/wavemap_ros/include/wavemap_ros/layered_ros_server_extension.h b/interfaces/ros1/wavemap_ros/include/wavemap_ros/layered_ros_server_extension.h index c294f905d..31d9382e1 100644 --- a/interfaces/ros1/wavemap_ros/include/wavemap_ros/layered_ros_server_extension.h +++ b/interfaces/ros1/wavemap_ros/include/wavemap_ros/layered_ros_server_extension.h @@ -9,12 +9,12 @@ #include #include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include #include #include #include diff --git a/interfaces/ros1/wavemap_ros/include/wavemap_ros/layered_server_builder.h b/interfaces/ros1/wavemap_ros/include/wavemap_ros/layered_server_builder.h index f351aa571..2e93dbb2e 100644 --- a/interfaces/ros1/wavemap_ros/include/wavemap_ros/layered_server_builder.h +++ b/interfaces/ros1/wavemap_ros/include/wavemap_ros/layered_server_builder.h @@ -47,7 +47,7 @@ class LayeredRosServerBuilder { server.layered_integrator_factory_ = [continuous_map](const param::Value& params, std::shared_ptr thread_pool) { - return createLayeredHashedWaveletIntegrator( + return createLayeredWaveletIntegrator( params, continuous_map, std::move(thread_pool)); }; diff --git a/interfaces/ros1/wavemap_ros/include/wavemap_ros/ros_server.h b/interfaces/ros1/wavemap_ros/include/wavemap_ros/ros_server.h index 580cabff0..a1d0e6121 100644 --- a/interfaces/ros1/wavemap_ros/include/wavemap_ros/ros_server.h +++ b/interfaces/ros1/wavemap_ros/include/wavemap_ros/ros_server.h @@ -2,6 +2,7 @@ #define WAVEMAP_ROS_ROS_SERVER_H_ #include +#include #include #include #include @@ -17,7 +18,7 @@ #include #include #include -#include +#include #include #include "wavemap_ros/inputs/ros_input_base.h" @@ -187,20 +188,33 @@ class RosServer { void bindGeometricClass( LayeredExtensionT& extension, layered::LocalElevationClassifierConfig classifier_config = {}, - layered::EndpointRange endpoint_range = {}) { + layered::EndpointRange endpoint_range = {}, + Radians min_azimuth = -kPi, + Radians max_azimuth = kPi) { using Value = layered::LayerValueT; static_assert(std::is_integral_v, "Geometric class layers require an integral value type."); addPointcloudEndpointAdapterRegistrar( [&extension, classifier = layered::LocalElevationClassifier( - classifier_config), endpoint_range]( + classifier_config), endpoint_range, min_azimuth, + max_azimuth]( PointcloudTopicInput& input) { input.addPosedCloudCallback( - [&extension, classifier, endpoint_range]( - const PosedPointcloud<>& pointcloud) { + [&extension, classifier, endpoint_range, min_azimuth, + max_azimuth](const PosedPointcloud<>& pointcloud) { + std::vector class_points; + class_points.reserve(pointcloud.size()); + for (const Point3D& point : pointcloud) { + const FloatingPoint azimuth = std::atan2(point.y(), point.x()); + if (min_azimuth <= azimuth && azimuth <= max_azimuth) { + class_points.emplace_back(point); + } + } + const PosedPointcloud<> class_pointcloud(pointcloud.getPose(), + class_points); const auto observations = layered::makeGeometricClassObservations( - pointcloud, classifier, endpoint_range); + class_pointcloud, classifier, endpoint_range); const auto result = extension.template integrateObservations< LayerTagT>(observations); diff --git a/interfaces/ros1/wavemap_ros/launch/datasets/external_ouster_layered_smoke.launch b/interfaces/ros1/wavemap_ros/launch/datasets/external_ouster_layered_smoke.launch deleted file mode 100644 index 40d17c4dc..000000000 --- a/interfaces/ros1/wavemap_ros/launch/datasets/external_ouster_layered_smoke.launch +++ /dev/null @@ -1,33 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/interfaces/ros1/wavemap_ros/launch/datasets/external_ouster_occupancy_validation.launch b/interfaces/ros1/wavemap_ros/launch/datasets/external_ouster_occupancy_validation.launch deleted file mode 100644 index 7d6acf0b8..000000000 --- a/interfaces/ros1/wavemap_ros/launch/datasets/external_ouster_occupancy_validation.launch +++ /dev/null @@ -1,29 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - diff --git a/interfaces/ros1/wavemap_ros/launch/synthetic_rgb_layered_map.launch b/interfaces/ros1/wavemap_ros/launch/synthetic_rgb_layered_map.launch new file mode 100644 index 000000000..f7e464185 --- /dev/null +++ b/interfaces/ros1/wavemap_ros/launch/synthetic_rgb_layered_map.launch @@ -0,0 +1,27 @@ + + + + + + + + + + + + + + + + diff --git a/interfaces/ros1/wavemap_ros/src/inputs/pointcloud_topic_input.cc b/interfaces/ros1/wavemap_ros/src/inputs/pointcloud_topic_input.cc index 45b66c8b3..fafc93052 100644 --- a/interfaces/ros1/wavemap_ros/src/inputs/pointcloud_topic_input.cc +++ b/interfaces/ros1/wavemap_ros/src/inputs/pointcloud_topic_input.cc @@ -1,6 +1,9 @@ #include "wavemap_ros/inputs/pointcloud_topic_input.h" +#include #include +#include +#include #include #include @@ -29,7 +32,8 @@ DECLARE_CONFIG_MEMBERS(PointcloudTopicInputConfig, (undistort_motion) (num_undistortion_interpolation_intervals_per_cloud) (projected_range_image_topic_name) - (undistorted_pointcloud_topic_name)); + (undistorted_pointcloud_topic_name) + (enable_benchmark_metrics)); bool PointcloudTopicInputConfig::isValid(bool verbose) const { bool all_valid = true; @@ -75,6 +79,12 @@ PointcloudTopicInput::PointcloudTopicInput( config_.undistorted_pointcloud_topic_name, config_.topic_queue_length); } + if (config_.enable_benchmark_metrics) { + benchmark_stats_srv_ = nh_private.advertiseService( + "benchmark_stats", &PointcloudTopicInput::benchmarkStatsCallback, + this); + benchmark_metrics_.integration_durations.reserve(10000u); + } } void PointcloudTopicInput::callback( @@ -186,6 +196,7 @@ void PointcloudTopicInput::callback( // Add it to the integration queue pointcloud_queue_.push( {std::move(stamped_pointcloud), std::move(endpoint_channels)}); + recordReceivedScan(num_points); } #ifdef LIVOX_AVAILABLE @@ -214,9 +225,100 @@ void PointcloudTopicInput::callback( // Add it to the integration queue pointcloud_queue_.push({std::move(stamped_pointcloud), {}}); + recordReceivedScan(pointcloud_msg.points.size()); } #endif +void PointcloudTopicInput::recordReceivedScan(size_t num_points) { + if (!config_.enable_benchmark_metrics) { + return; + } + const std::lock_guard lock(benchmark_metrics_mutex_); + ++benchmark_metrics_.received_scans; + ++benchmark_metrics_.pending_scans; + benchmark_metrics_.received_points += num_points; + benchmark_metrics_.maximum_queue_size = + std::max(benchmark_metrics_.maximum_queue_size, + benchmark_metrics_.pending_scans); +} + +void PointcloudTopicInput::recordDroppedScan() { + if (config_.enable_benchmark_metrics) { + const std::lock_guard lock(benchmark_metrics_mutex_); + ++benchmark_metrics_.dropped_scans; + --benchmark_metrics_.pending_scans; + } +} + +void PointcloudTopicInput::recordIntegratedScan( + size_t num_points, double duration_seconds) { + if (!config_.enable_benchmark_metrics) { + return; + } + const std::lock_guard lock(benchmark_metrics_mutex_); + ++benchmark_metrics_.integrated_scans; + --benchmark_metrics_.pending_scans; + benchmark_metrics_.integrated_points += num_points; + benchmark_metrics_.integration_durations.emplace_back(duration_seconds); +} + +bool PointcloudTopicInput::benchmarkStatsCallback( + std_srvs::Trigger::Request& /*request*/, + std_srvs::Trigger::Response& response) { + if (!config_.enable_benchmark_metrics) { + response.success = false; + response.message = "Benchmark metrics are disabled."; + return true; + } + + BenchmarkMetrics metrics; + { + const std::lock_guard lock(benchmark_metrics_mutex_); + metrics = benchmark_metrics_; + } + std::vector sorted = metrics.integration_durations; + std::sort(sorted.begin(), sorted.end()); + const auto percentile = [&sorted](double fraction) { + if (sorted.empty()) { + return 0.0; + } + const double position = fraction * static_cast(sorted.size() - 1u); + const size_t lower = static_cast(position); + const size_t upper = std::min(lower + 1u, sorted.size() - 1u); + const double weight = position - static_cast(lower); + return sorted[lower] * (1.0 - weight) + sorted[upper] * weight; + }; + const double total = + std::accumulate(sorted.begin(), sorted.end(), 0.0); + const double mean = sorted.empty() ? 0.0 : total / sorted.size(); + + std::ostringstream json; + json.precision(10); + json << "{" + << "\"received_scans\":" << metrics.received_scans << "," + << "\"integrated_scans\":" << metrics.integrated_scans + << "," + << "\"dropped_scans\":" << metrics.dropped_scans << "," + << "\"pending_scans\":" << metrics.pending_scans << "," + << "\"received_points\":" << metrics.received_points << "," + << "\"integrated_points\":" << metrics.integrated_points + << "," + << "\"maximum_queue_size\":" + << metrics.maximum_queue_size << "," + << "\"integration_time_s\":{" + << "\"total\":" << total << "," + << "\"min\":" << (sorted.empty() ? 0.0 : sorted.front()) << "," + << "\"mean\":" << mean << "," + << "\"median\":" << percentile(0.5) << "," + << "\"p95\":" << percentile(0.95) << "," + << "\"max\":" << (sorted.empty() ? 0.0 : sorted.back()) + << "}}"; + + response.success = true; + response.message = json.str(); + return true; +} + void PointcloudTopicInput::processQueue() { ProfilerZoneScoped; while (!pointcloud_queue_.empty()) { @@ -238,6 +340,7 @@ void PointcloudTopicInput::processQueue() { << pointcloud_queue_.back().pointcloud.getEndTime() << ". Dropping cloud."); pointcloud_queue_.pop(); + recordDroppedScan(); continue; } @@ -272,6 +375,7 @@ void PointcloudTopicInput::processQueue() { } pointcloud_queue_.pop(); + recordDroppedScan(); continue; } } else { @@ -317,6 +421,8 @@ void PointcloudTopicInput::processQueue() { // measurement have both been committed. pipeline_->runOperations(); integration_timer_.stop(); + recordIntegratedScan(posed_pointcloud.size(), + integration_timer_.getLastEpisodeDuration()); ROS_DEBUG_STREAM("Integrated new pointcloud in " << integration_timer_.getLastEpisodeDuration() << "s. Total integration time: " diff --git a/interfaces/ros1/wavemap_ros_conversions/include/wavemap_ros_conversions/descriptor_layer_observation_conversions.h b/interfaces/ros1/wavemap_ros_conversions/include/wavemap_ros_conversions/descriptor_layer_observation_conversions.h index 085adeaba..2ca706315 100644 --- a/interfaces/ros1/wavemap_ros_conversions/include/wavemap_ros_conversions/descriptor_layer_observation_conversions.h +++ b/interfaces/ros1/wavemap_ros_conversions/include/wavemap_ros_conversions/descriptor_layer_observation_conversions.h @@ -7,7 +7,7 @@ #include #include -#include +#include #include #include #include diff --git a/interfaces/ros1/wavemap_ros_conversions/include/wavemap_ros_conversions/descriptor_layered_map_conversions.h b/interfaces/ros1/wavemap_ros_conversions/include/wavemap_ros_conversions/descriptor_layered_map_conversions.h index 9ae4c6422..92970bad6 100644 --- a/interfaces/ros1/wavemap_ros_conversions/include/wavemap_ros_conversions/descriptor_layered_map_conversions.h +++ b/interfaces/ros1/wavemap_ros_conversions/include/wavemap_ros_conversions/descriptor_layered_map_conversions.h @@ -5,8 +5,8 @@ #include #include -#include -#include +#include +#include #include #include #include diff --git a/interfaces/ros1/wavemap_ros_conversions/include/wavemap_ros_conversions/discrete_layer_marker_conversions.h b/interfaces/ros1/wavemap_ros_conversions/include/wavemap_ros_conversions/discrete_layer_marker_conversions.h index efa9feae1..08fc1e7e5 100644 --- a/interfaces/ros1/wavemap_ros_conversions/include/wavemap_ros_conversions/discrete_layer_marker_conversions.h +++ b/interfaces/ros1/wavemap_ros_conversions/include/wavemap_ros_conversions/discrete_layer_marker_conversions.h @@ -14,7 +14,7 @@ #include #include -#include +#include namespace layered_map_viz { namespace detail { diff --git a/interfaces/ros1/wavemap_ros_conversions/include/wavemap_ros_conversions/impl/layered_map_msg_conversions_inl.h b/interfaces/ros1/wavemap_ros_conversions/include/wavemap_ros_conversions/impl/layered_map_msg_conversions_inl.h index a35d205c1..1cae061fc 100644 --- a/interfaces/ros1/wavemap_ros_conversions/include/wavemap_ros_conversions/impl/layered_map_msg_conversions_inl.h +++ b/interfaces/ros1/wavemap_ros_conversions/include/wavemap_ros_conversions/impl/layered_map_msg_conversions_inl.h @@ -76,7 +76,7 @@ void mapToRosMsg(const HashedWaveletOctreeT& map, wavemap_msgs::Layer for (const auto& block_index : include_blocks.value()) { if (const auto* block = map.getBlock(block_index); block) { auto& block_msg = msg.blocks[block_idx++]; - // Block conversion is independent per block, + // Block conversion is independent per block, // so it can safely run in the optional thread pool when the caller has one available. if (thread_pool) { thread_pool->add_task([block_index, block, min_log_odds, max_log_odds, @@ -132,7 +132,7 @@ void blockToRosMsg( auto& node_msg = msg.nodes.emplace_back(); node_msg.detail_layers = CellDataRosConverterT::makeLayers(); size_t coefficient_idx = 0u; - // Each wavelet node stores eight detail coefficients. + // Each wavelet node stores eight detail coefficients. // Occupancy is copied into the fixed array; all custom layer values are appended layer-major into detail_layers by CellDataRosConverterT for (const CellDataT& coefficient : node.data()) { node_msg.occupancy_detail_coefficients[coefficient_idx++] = @@ -162,6 +162,160 @@ void blockToRosMsg( } } +template +bool mapToRosMsg(const HashedChunkedWaveletOctreeT& map, const std::string& frame_id, const ros::Time& stamp, wavemap_msgs::Map& msg, std::optional> include_blocks, std::shared_ptr thread_pool) { + ProfilerZoneScoped; + // Layered equivalent of the existing MapBase wrapper + // Fills the common ROS header and stores the typed map inside wavemap_msgs::Map. + msg.header.stamp = stamp; + msg.header.frame_id = frame_id; + + // The custom voxel type needs the user-provided converter + auto& layered_map_msg = msg.layered_hashed_wavelet_octree.emplace_back(); + convert::mapToRosMsg(map, layered_map_msg, std::move(include_blocks), std::move(thread_pool)); + return true; +} + +template +void mapToRosMsg(const HashedChunkedWaveletOctreeT& map, wavemap_msgs::LayeredHashedWaveletOctree& msg, std::optional> include_blocks, std::shared_ptr thread_pool) { + ProfilerZoneScoped; + // Use a small margin when deciding whether child nodes are saturated + constexpr FloatingPoint kNumericalNoise = 1e-3f; + const auto min_log_odds = map.getMinLogOdds() + kNumericalNoise; + const auto max_log_odds = map.getMaxLogOdds() - kNumericalNoise; + + // Copy the map metadata first + msg.min_cell_width = map.getMinCellWidth(); + msg.min_log_odds = map.getMinLogOdds(); + msg.max_log_odds = map.getMaxLogOdds(); + msg.tree_height = map.getTreeHeight(); + msg.layer_names = CellDataRosConverterT::layerNames(); + msg.layer_types = CellDataRosConverterT::layerTypes(); + msg.layer_min_values = + CellDataRosConverterT::makeLayerMinimums(map.getThresholdConfig()); + msg.layer_max_values = + CellDataRosConverterT::makeLayerMaximums(map.getThresholdConfig()); + msg.layer_visualizations = + CellDataRosConverterT::layerVisualizations(); + + // Always publish the full list of allocated block indices + msg.allocated_block_indices.reserve(map.getHashMap().size()); + map.forEachBlock([&msg](const Index3D& block_index, const auto& /*block*/) { + auto& block_index_msg = msg.allocated_block_indices.emplace_back(); + block_index_msg.x = block_index.x(); + block_index_msg.y = block_index.y(); + block_index_msg.z = block_index.z(); + }); + + // If the caller requested a subset of blocks, discard indices that are not currently allocated + if (include_blocks) { + for (auto include_block_it = include_blocks->begin(); + include_block_it != include_blocks->end();) { + if (map.hasBlock(*include_block_it)) { + ++include_block_it; + } else { + include_block_it = include_blocks->erase(include_block_it); + } + } + } else { + include_blocks.emplace(); + map.forEachBlock( + [&include_blocks](const Index3D& block_index, const auto& /*block*/) { + include_blocks->emplace(block_index); + }); + } + + int block_idx = 0; + msg.blocks.resize(include_blocks->size()); + for (const auto& block_index : include_blocks.value()) { + if (const auto* block = map.getBlock(block_index); block) { + auto& block_msg = msg.blocks[block_idx++]; + // Block conversion is independent per block, + // so it can safely run in the optional thread pool when the caller has one available. + if (thread_pool) { + thread_pool->add_task([block_index, block, min_log_odds, max_log_odds, + &block_msg]() { + blockToRosMsg( + block_index, *block, min_log_odds, max_log_odds, block_msg); + }); + } else { + blockToRosMsg( + block_index, *block, min_log_odds, max_log_odds, block_msg); + } + } else { + ROS_ERROR("Block index not found. This should never happen."); + } + } + if (thread_pool) { + thread_pool->wait_all(); + } +} + +template +void blockToRosMsg( + const typename HashedChunkedWaveletOctreeT::BlockIndex& block_index, + const typename HashedChunkedWaveletOctreeT::Block& block, + FloatingPoint min_log_odds, FloatingPoint max_log_odds, + wavemap_msgs::LayeredHashedWaveletOctreeBlock& msg) { + using Block = typename HashedChunkedWaveletOctreeT::Block; + using NodeConstRefType = typename Block::OctreeType::NodeConstRefType; + + struct StackElement { + const CellDataT scale; + NodeConstRefType node; + }; + + // Store the block position and root scale separately from the internal wavelet nodes + // Occupancy is kept in the explicit occupancy field, while extra layers are encoded through the user-provided converter + msg.root_node_offset.x = block_index.x(); + msg.root_node_offset.y = block_index.y(); + msg.root_node_offset.z = block_index.z(); + msg.root_node_occupancy_scale_coefficient = static_cast(block.getRootScale()); + msg.root_node_layers = CellDataRosConverterT::makeLayers(); + CellDataRosConverterT::appendLayerValues(block.getRootScale(), msg.root_node_layers); + + // Traverse the block's wavelet tree without recursion + // Each stack entry carries the node and its reconstructed scale coefficient, to decide whether the children are saturated and should be serialized + std::stack stack; + stack.emplace(StackElement{block.getRootScale(), block.getRootNode()}); + while (!stack.empty()) { + const CellDataT scale = stack.top().scale; + const auto node = stack.top().node; + stack.pop(); + + auto& node_msg = msg.nodes.emplace_back(); + node_msg.detail_layers = CellDataRosConverterT::makeLayers(); + size_t coefficient_idx = 0u; + // Each wavelet node stores eight detail coefficients. + // Occupancy is copied into the fixed array; all custom layer values are appended layer-major into detail_layers by CellDataRosConverterT + for (const CellDataT& coefficient : node.data()) { + node_msg.occupancy_detail_coefficients[coefficient_idx++] = + static_cast(coefficient); + CellDataRosConverterT::appendLayerValues(coefficient, + node_msg.detail_layers); + } + + // Reconstruct child scales from the parent scale and detail coefficients + // Only children whose occupancy is not saturated need to be sent further + const auto child_scales = Block::Transform::backward({scale, node.data()}); + for (int relative_child_idx = OctreeIndex::kNumChildren - 1; + 0 <= relative_child_idx; --relative_child_idx) { + const auto child_scale = child_scales[relative_child_idx]; + const FloatingPoint child_occupancy = + static_cast(child_scale); + if (child_occupancy < min_log_odds || max_log_odds < child_occupancy) { + continue; + } + + if (auto child = node.getChild(relative_child_idx); child) { + stack.emplace(StackElement{child_scale, *child}); + // The bitset records which children have serialized node data. + node_msg.allocated_children_bitset += (1 << relative_child_idx); + } + } + } +} + template void rosMsgToMap(const wavemap_msgs::LayeredHashedWaveletOctree& msg, typename HashedWaveletOctreeT::Ptr& map) { diff --git a/interfaces/ros1/wavemap_ros_conversions/include/wavemap_ros_conversions/layer_observation_msg_conversions.h b/interfaces/ros1/wavemap_ros_conversions/include/wavemap_ros_conversions/layer_observation_msg_conversions.h index c61d62edd..4cc308cb8 100644 --- a/interfaces/ros1/wavemap_ros_conversions/include/wavemap_ros_conversions/layer_observation_msg_conversions.h +++ b/interfaces/ros1/wavemap_ros_conversions/include/wavemap_ros_conversions/layer_observation_msg_conversions.h @@ -5,7 +5,7 @@ #include #include -#include +#include namespace wavemap::convert { diff --git a/interfaces/ros1/wavemap_ros_conversions/include/wavemap_ros_conversions/layered_map_msg_conversions.h b/interfaces/ros1/wavemap_ros_conversions/include/wavemap_ros_conversions/layered_map_msg_conversions.h index eb0151e63..c376dad44 100644 --- a/interfaces/ros1/wavemap_ros_conversions/include/wavemap_ros_conversions/layered_map_msg_conversions.h +++ b/interfaces/ros1/wavemap_ros_conversions/include/wavemap_ros_conversions/layered_map_msg_conversions.h @@ -9,7 +9,7 @@ #include #include -#include +#include #include #include #include diff --git a/interfaces/ros1/wavemap_ros_conversions/include/wavemap_ros_conversions/map_msg_conversions.h b/interfaces/ros1/wavemap_ros_conversions/include/wavemap_ros_conversions/map_msg_conversions.h index 8af2aff6b..09e4bedf9 100644 --- a/interfaces/ros1/wavemap_ros_conversions/include/wavemap_ros_conversions/map_msg_conversions.h +++ b/interfaces/ros1/wavemap_ros_conversions/include/wavemap_ros_conversions/map_msg_conversions.h @@ -63,6 +63,15 @@ void mapToRosMsg(const HashedWaveletOctreeT& map, wavemap_msgs::Layer template void blockToRosMsg(const typename HashedWaveletOctreeT::BlockIndex& block_index, const typename HashedWaveletOctreeT::Block& block, FloatingPoint min_log_odds, FloatingPoint max_log_odds, wavemap_msgs::LayeredHashedWaveletOctreeBlock& msg); +template +bool mapToRosMsg(const HashedChunkedWaveletOctreeT& map, const std::string& frame_id, const ros::Time& stamp, wavemap_msgs::Map& msg, std::optional> include_blocks = std::nullopt, std::shared_ptr thread_pool = nullptr); + +template +void mapToRosMsg(const HashedChunkedWaveletOctreeT& map, wavemap_msgs::LayeredHashedWaveletOctree& msg, std::optional> include_blocks = std::nullopt, std::shared_ptr thread_pool = nullptr); + +template +void blockToRosMsg(const typename HashedChunkedWaveletOctreeT::BlockIndex& block_index, const typename HashedChunkedWaveletOctreeT::Block& block, FloatingPoint min_log_odds, FloatingPoint max_log_odds, wavemap_msgs::LayeredHashedWaveletOctreeBlock& msg); + template void rosMsgToMap(const wavemap_msgs::LayeredHashedWaveletOctree& msg, typename HashedWaveletOctreeT::Ptr& map); diff --git a/interfaces/ros1/wavemap_ros_conversions/include/wavemap_ros_conversions/pointcloud_layer_observation_conversions.h b/interfaces/ros1/wavemap_ros_conversions/include/wavemap_ros_conversions/pointcloud_layer_observation_conversions.h index 391d35df5..ee46e9111 100644 --- a/interfaces/ros1/wavemap_ros_conversions/include/wavemap_ros_conversions/pointcloud_layer_observation_conversions.h +++ b/interfaces/ros1/wavemap_ros_conversions/include/wavemap_ros_conversions/pointcloud_layer_observation_conversions.h @@ -11,7 +11,7 @@ #include #include -#include +#include #include namespace wavemap::convert { diff --git a/interfaces/ros1/wavemap_ros_conversions/test/src/test_map_msg_conversions.cc b/interfaces/ros1/wavemap_ros_conversions/test/src/test_map_msg_conversions.cc index d9a66500f..30876f96e 100644 --- a/interfaces/ros1/wavemap_ros_conversions/test/src/test_map_msg_conversions.cc +++ b/interfaces/ros1/wavemap_ros_conversions/test/src/test_map_msg_conversions.cc @@ -8,14 +8,34 @@ #include #include #include +#include +#include +#include #include #include #include #include #include "wavemap_ros_conversions/map_msg_conversions.h" +#include "wavemap_ros_conversions/descriptor_layered_map_conversions.h" namespace wavemap { +namespace { +struct TestReflectivityLayer + : layered::schema::ContinuousLayer< + FloatingPoint, + layered::ReplaceLayerUpdatePolicy> { + static constexpr std::string_view name = "reflectivity"; +}; + +using TestSchema = layered::schema::LayerSchema; +using ChunkedLayeredDefinition = layered::LayeredMapDefinition< + TestSchema, layered::HashedChunkedWaveletOctreeBackend>; +using RegularLayeredDefinition = layered::LayeredMapDefinition; +using TestLayerRosConverter = convert::DescriptorContinuousRosConverter< + ChunkedLayeredDefinition::Voxel>; +} // namespace + template class MapMsgConversionsTest : public FixtureBase, public GeometryGenerator, @@ -166,4 +186,44 @@ TYPED_TEST(MapMsgConversionsTest, InsertionAndLeafVisitor) { } } } + +TEST(LayeredMapMsgConversionsTest, + ChunkedMapUsesLogicalHashedWaveletRepresentation) { + ros::Time::init(); + + ChunkedLayeredDefinition::ContinuousMap::Config config; + config.min_cell_width = 0.2f; + config.min_log_odds = -2.f; + config.max_log_odds = 4.f; + config.tree_height = 4; + + ChunkedLayeredDefinition::ContinuousMap chunked_map(config); + const Index3D cell_index{3, -2, 5}; + ChunkedLayeredDefinition::Voxel voxel; + voxel.occupancy = 0.7f; + voxel.data.get() = 0.35f; + chunked_map.setVoxelValue(cell_index, voxel); + + wavemap_msgs::Map map_msg; + ASSERT_TRUE((convert::mapToRosMsg( + chunked_map, "odom", ros::Time(42), map_msg))); + ASSERT_EQ(map_msg.layered_hashed_wavelet_octree.size(), 1u); + const auto& layered_msg = map_msg.layered_hashed_wavelet_octree.front(); + EXPECT_EQ(layered_msg.layer_names, + std::vector({"reflectivity"})); + EXPECT_EQ(layered_msg.layer_types, + std::vector({"float32"})); + EXPECT_FALSE(layered_msg.blocks.empty()); + + RegularLayeredDefinition::ContinuousMap::Ptr regular_map; + ASSERT_TRUE((convert::rosMsgToMap(map_msg, + regular_map))); + ASSERT_TRUE(regular_map); + const auto reconstructed = regular_map->getVoxelValue(cell_index); + EXPECT_NEAR(reconstructed.occupancy, voxel.occupancy, 1e-5f); + EXPECT_NEAR(reconstructed.data.get(), + voxel.data.get(), 1e-5f); +} } // namespace wavemap diff --git a/interfaces/ros1/wavemap_rviz_plugin/CMakeLists.txt b/interfaces/ros1/wavemap_rviz_plugin/CMakeLists.txt index 335306b9b..baf6d395e 100644 --- a/interfaces/ros1/wavemap_rviz_plugin/CMakeLists.txt +++ b/interfaces/ros1/wavemap_rviz_plugin/CMakeLists.txt @@ -16,8 +16,7 @@ catkin_package(CATKIN_DEPENDS # For all targets include_directories( include - ${CMAKE_CURRENT_SOURCE_DIR}/../../../examples/cpp/digital_twin/common - ${CMAKE_CURRENT_SOURCE_DIR}/../../../examples/cpp/digital_twin/ouster_mapping + ${CMAKE_CURRENT_SOURCE_DIR}/../wavemap_ros/app/ouster_layered SYSTEM ${catkin_INCLUDE_DIRS}) # This setting causes Qt's "MOC" generation to happen automatically diff --git a/interfaces/ros1/wavemap_rviz_plugin/include/wavemap_rviz_plugin/layered_map_display.h b/interfaces/ros1/wavemap_rviz_plugin/include/wavemap_rviz_plugin/layered_map_display.h index 24622f893..2b608ebd8 100644 --- a/interfaces/ros1/wavemap_rviz_plugin/include/wavemap_rviz_plugin/layered_map_display.h +++ b/interfaces/ros1/wavemap_rviz_plugin/include/wavemap_rviz_plugin/layered_map_display.h @@ -13,6 +13,7 @@ #include #include +#include #include #include #include @@ -53,6 +54,7 @@ class LayeredMapDisplay void updateSourceModeCallback(); void loadMapFromDiskCallback(); void updateLayerSelectionCallback(); + void updateLegendVisibilityCallback(); private: struct DisplayLayer { @@ -97,6 +99,9 @@ class LayeredMapDisplay "Layer", "occupancy", "Layer to display. Populated from the received LayeredMap message.", &voxel_visual_properties_, SLOT(updateLayerSelectionCallback()), this}; + rviz::BoolProperty show_legend_property_{ + "Show Legend", true, "Whether to show the selected layer legend.", + this, SLOT(updateLegendVisibilityCallback()), this}; void processMessage(const wavemap_msgs::LayeredMap::ConstPtr& msg) override; bool loadMapFromDisk(const std::filesystem::path& filepath, std::string* error_message = nullptr); diff --git a/interfaces/ros1/wavemap_rviz_plugin/include/wavemap_rviz_plugin/visuals/voxel_visual.h b/interfaces/ros1/wavemap_rviz_plugin/include/wavemap_rviz_plugin/visuals/voxel_visual.h index 476bd9705..030e6026b 100644 --- a/interfaces/ros1/wavemap_rviz_plugin/include/wavemap_rviz_plugin/visuals/voxel_visual.h +++ b/interfaces/ros1/wavemap_rviz_plugin/include/wavemap_rviz_plugin/visuals/voxel_visual.h @@ -86,6 +86,8 @@ class VoxelVisual : public QObject { return scalar_max_property_.getFloat(); } Ogre::ColourValue scalarLowColor() const { return scalar_low_color_; } + VoxelColorMode colorMode() const { return voxel_color_mode_; } + Ogre::ColourValue flatColor() const { return voxel_flat_color_; } Ogre::ColourValue scalarHighColor() const { return scalar_high_color_; } void configureLayerAppearance( const std::string& layer_name, const std::string& layer_type, diff --git a/interfaces/ros1/wavemap_rviz_plugin/plugin_description.xml b/interfaces/ros1/wavemap_rviz_plugin/plugin_description.xml index 6f1ef582c..6c0d88762 100644 --- a/interfaces/ros1/wavemap_rviz_plugin/plugin_description.xml +++ b/interfaces/ros1/wavemap_rviz_plugin/plugin_description.xml @@ -15,16 +15,6 @@ wavemap_msgs/LayeredMap - - Schema handler for the legacy color/traversability example. - - - Schema handler for the reflectivity field map. - @@ -40,4 +30,14 @@ base_class_type="wavemap::rviz_plugin::LayeredMapFactory"> Schema handler for occupancy, reflectivity, and class maps. + + Schema handler for occupancy, reflectivity, signal, near-IR, and class maps with discrete block height 2. + + + Schema handler for occupancy and direct RGB color maps. + diff --git a/interfaces/ros1/wavemap_rviz_plugin/src/layered_map_display.cc b/interfaces/ros1/wavemap_rviz_plugin/src/layered_map_display.cc index b104ab6ed..1980727fb 100644 --- a/interfaces/ros1/wavemap_rviz_plugin/src/layered_map_display.cc +++ b/interfaces/ros1/wavemap_rviz_plugin/src/layered_map_display.cc @@ -30,8 +30,6 @@ #include #include -#include "example_layered_map_ros_config.h" -#include "layered_ros_converter.h" #include "wavemap_rviz_plugin/utils/alert_dialog.h" namespace wavemap::rviz_plugin { @@ -727,6 +725,11 @@ void LayeredMapDisplay::updateLegendOverlay(const wavemap_msgs::LayeredMap* msg) return; } + if (!show_legend_property_.getBool()) { + legend_overlay_->hide(); + return; + } + clearLegendOverlayElements(); struct LegendRow { @@ -750,11 +753,28 @@ void LayeredMapDisplay::updateLegendOverlay(const wavemap_msgs::LayeredMap* msg) if (layer->source == DisplayLayer::Source::kContinuous) { if (layer->name == "occupancy") { - subtitle = "occupancy / log-odds"; - rows.push_back({Ogre::ColourValue(0.12f, 0.32f, 0.95f, 1.f), - "lower probability"}); - rows.push_back({Ogre::ColourValue(0.95f, 0.18f, 0.10f, 1.f), - "higher probability"}); + const VoxelColorMode color_mode = + voxel_visual_ ? voxel_visual_->colorMode() + : VoxelColorMode{VoxelColorMode::kHeight}; + switch (color_mode.toTypeId()) { + case VoxelColorMode::kProbability: + subtitle = "occupancy / probability"; + rows.push_back({logOddsToColor(-20.f), "0% probability"}); + rows.push_back({logOddsToColor(0.f), "50% probability"}); + rows.push_back({logOddsToColor(20.f), "100% probability"}); + break; + case VoxelColorMode::kFlat: + subtitle = "occupancy / flat color"; + rows.push_back( + {voxel_visual_ ? voxel_visual_->flatColor() + : Ogre::ColourValue::Blue, + "all displayed voxels"}); + break; + case VoxelColorMode::kHeight: + default: + subtitle = "Color based on height"; + break; + } } else if (layer->type == "float32_rgb" || layer->type == "float32_vec3") { subtitle = "direct RGB color"; rows.push_back({Ogre::ColourValue(1.f, 0.f, 0.f, 1.f), "red channel"}); @@ -872,6 +892,10 @@ void LayeredMapDisplay::updateLegendOverlay(const wavemap_msgs::LayeredMap* msg) legend_overlay_->show(); } +void LayeredMapDisplay::updateLegendVisibilityCallback() { + updateLegendOverlay(latest_msg_ ? &latest_msg_.value() : nullptr); +} + void LayeredMapDisplay::updateSourceModeCallback() { ProfilerZoneScoped; const LayeredMapSourceMode old_source_mode = source_mode_; @@ -914,7 +938,7 @@ void LayeredMapDisplay::loadMapFromDiskCallback() { if (!loadMapFromDisk(filepath, &load_error)) { if (load_error.empty()) { load_error = "This display currently supports layered map files that " - "match the registered ExampleLayeredMap schema."; + "match a registered layered-map schema."; } AlertDialog alert{ "Could not load layered map", diff --git a/interfaces/ros1/wavemap_rviz_plugin/src/layered_map_factories.cc b/interfaces/ros1/wavemap_rviz_plugin/src/layered_map_factories.cc index 94c7fdeb1..cb7638ac3 100644 --- a/interfaces/ros1/wavemap_rviz_plugin/src/layered_map_factories.cc +++ b/interfaces/ros1/wavemap_rviz_plugin/src/layered_map_factories.cc @@ -3,20 +3,21 @@ #include #include +#include -#include "example_layered_map_ros_config.h" -#include "field_layered_map_ros_config.h" -#include "layered_ros_converter.h" #include "occupancy_map_layers.h" #include "reflectivity_map_layers.h" #include "reflectivity_only_map_layers.h" +#include "rgb_map_layers.h" +#include "signal_map_layers.h" #include "wavemap_rviz_plugin/layered_map_factory.h" namespace wavemap::rviz_plugin { -namespace field_map = wavemap::examples::field_map; namespace occupancy_map = wavemap::examples::occupancy_map; namespace reflectivity_map = wavemap::examples::reflectivity_map; namespace reflectivity_only_map = wavemap::examples::reflectivity_only_map; +namespace rgb_map = wavemap::examples::rgb_map; +namespace signal_map = wavemap::examples::signal_map; FloatingPoint normalizeScalar(FloatingPoint value, const LayerMetadata& metadata) { @@ -31,26 +32,6 @@ FloatingPoint normalizeScalar(FloatingPoint value, 0.f, 1.f); } -FloatingPoint normalizeComponent(FloatingPoint value, - const LayerMetadata& metadata, - size_t component) { - if (component >= metadata.min_values.size() || - component >= metadata.max_values.size() || - metadata.max_values[component] <= metadata.min_values[component]) { - return std::clamp(value, 0.f, 1.f); - } - return std::clamp((value - metadata.min_values[component]) / - (metadata.max_values[component] - - metadata.min_values[component]), - 0.f, 1.f); -} - -Ogre::ColourValue scalarGray(FloatingPoint normalized_value) { - const FloatingPoint visible_value = - 0.15f + 0.85f * std::clamp(normalized_value, 0.f, 1.f); - return Ogre::ColourValue(visible_value, visible_value, visible_value, 1.f); -} - Ogre::ColourValue scalarGradient(FloatingPoint normalized_value, const Ogre::ColourValue& low, const Ogre::ColourValue& high) { @@ -60,34 +41,7 @@ Ogre::ColourValue scalarGradient(FloatingPoint normalized_value, low.b + value * (high.b - low.b), 1.f); } -struct ExampleContinuousLayerColorProvider { - static bool getLayerColor(const std::string& layer_name, - const LayeredVoxel& voxel, - FloatingPoint /*occupancy*/, - FloatingPoint /*scalar_display_min*/, - FloatingPoint /*scalar_display_max*/, - const Ogre::ColourValue& /*scalar_low_color*/, - const Ogre::ColourValue& /*scalar_high_color*/, - const LayerMetadata& metadata, - Ogre::ColourValue& color) { - if (layer_name == "color") { - color = Ogre::ColourValue( - normalizeComponent(voxel.data.rgb.r, metadata, 0u), - normalizeComponent(voxel.data.rgb.g, metadata, 1u), - normalizeComponent(voxel.data.rgb.b, metadata, 2u), 1.f); - return true; - } - if (layer_name == "traversability") { - const FloatingPoint value = - normalizeScalar(voxel.data.traversability, metadata); - color = scalarGray(value); - return true; - } - return false; - } -}; - -struct FieldContinuousLayerColorProvider { +struct ReflectivityContinuousLayerColorProvider { template static bool getLayerColor(const std::string& layer_name, const VoxelT& voxel, @@ -104,55 +58,92 @@ struct FieldContinuousLayerColorProvider { LayerMetadata display_metadata; display_metadata.min_values = {scalar_display_min}; display_metadata.max_values = {scalar_display_max}; + const auto& state = + voxel.data.template get(); const FloatingPoint value = normalizeScalar( - voxel.data.template get(), + layered::LayerStateConversion< + reflectivity_map::ReflectivityLayer>::value(state), display_metadata); color = scalarGradient(value, scalar_low_color, scalar_high_color); return true; } }; -struct ReflectivityContinuousLayerColorProvider { - template - static bool getLayerColor(const std::string& layer_name, - const VoxelT& voxel, - FloatingPoint /*occupancy*/, - FloatingPoint scalar_display_min, - FloatingPoint scalar_display_max, - const Ogre::ColourValue& scalar_low_color, - const Ogre::ColourValue& scalar_high_color, - const LayerMetadata& /*metadata*/, - Ogre::ColourValue& color) { - if (layer_name != "reflectivity") { - return false; - } +struct SignalContinuousLayerColorProvider { + template + static Ogre::ColourValue colorFor( + const VoxelT& voxel, FloatingPoint scalar_display_min, + FloatingPoint scalar_display_max, + const Ogre::ColourValue& scalar_low_color, + const Ogre::ColourValue& scalar_high_color) { LayerMetadata display_metadata; display_metadata.min_values = {scalar_display_min}; display_metadata.max_values = {scalar_display_max}; - const auto& state = - voxel.data.template get(); + const auto& state = voxel.data.template get(); const FloatingPoint value = normalizeScalar( - layered::LayerStateConversion< - reflectivity_map::ReflectivityLayer>::value(state), + layered::LayerStateConversion::value(state), display_metadata); - color = scalarGradient(value, scalar_low_color, scalar_high_color); + return scalarGradient(value, scalar_low_color, scalar_high_color); + } + + template + static bool getLayerColor( + const std::string& layer_name, const VoxelT& voxel, + FloatingPoint /*occupancy*/, FloatingPoint scalar_display_min, + FloatingPoint scalar_display_max, + const Ogre::ColourValue& scalar_low_color, + const Ogre::ColourValue& scalar_high_color, + const LayerMetadata& /*metadata*/, Ogre::ColourValue& color) { + if (layer_name == "reflectivity") { + color = colorFor( + voxel, scalar_display_min, scalar_display_max, scalar_low_color, + scalar_high_color); + } else if (layer_name == "signal") { + color = colorFor( + voxel, scalar_display_min, scalar_display_max, scalar_low_color, + scalar_high_color); + } else if (layer_name == "near_ir") { + color = colorFor( + voxel, scalar_display_min, scalar_display_max, scalar_low_color, + scalar_high_color); + } else { + return false; + } return true; } }; -class ExampleLayeredMapFactory final - : public TypedLayeredMapFileFactory< - ContinuousWaveletMap, LayeredVoxelRosConverter, - ExampleContinuousLayerColorProvider, ExampleLayeredMap, - ExampleLayeredMapIo, - layered_map_config::makeDefaultExampleLayeredMapConfig> {}; +struct RgbContinuousLayerColorProvider { + template + static bool getLayerColor( + const std::string& layer_name, const VoxelT& voxel, + FloatingPoint /*occupancy*/, FloatingPoint /*scalar_display_min*/, + FloatingPoint /*scalar_display_max*/, + const Ogre::ColourValue& /*scalar_low_color*/, + const Ogre::ColourValue& /*scalar_high_color*/, + const LayerMetadata& /*metadata*/, Ogre::ColourValue& color) { + if (layer_name != "rgb") { + return false; + } + const layered::Rgb value = layered::LayerStateConversion< + rgb_map::RgbLayer>::value( + voxel.data.template get()); + color = Ogre::ColourValue(std::clamp(value.r, 0.f, 1.f), + std::clamp(value.g, 0.f, 1.f), + std::clamp(value.b, 0.f, 1.f), 1.f); + return true; + } +}; -class FieldLayeredMapFactory final - : public TypedLayeredMapFileFactory< - field_map::ContinuousMap, - field_map::ros_config::VoxelRosConverter, - FieldContinuousLayerColorProvider, field_map::Map, field_map::MapIo, - field_map::makeDefaultConfig> {}; +signal_map::NearIrClassDefinition::Config makeAllLayersH2MapConfig() { + return signal_map::makeMapConfigFor< + signal_map::NearIrClassDefinition>(2); +} + +reflectivity_map::Definition::Config makeReflectivityClassMapConfig() { + return reflectivity_map::makeMapConfig< + reflectivity_map::Definition>(0); +} class OccupancyLayeredMapFactory final : public TypedLayeredMapFileFactory< @@ -160,7 +151,8 @@ class OccupancyLayeredMapFactory final wavemap::convert::DescriptorContinuousRosConverter< occupancy_map::Definition::Voxel>, NoLayerColorProvider, occupancy_map::Definition::Map, - occupancy_map::Definition::MapIo, occupancy_map::makeMapConfig> {}; + occupancy_map::Definition::MapIo, + occupancy_map::makeMapConfig> {}; class ReflectivityLayeredMapFactory final : public TypedLayeredMapFileFactory< @@ -170,7 +162,8 @@ class ReflectivityLayeredMapFactory final ReflectivityContinuousLayerColorProvider, reflectivity_only_map::Definition::Map, reflectivity_only_map::Definition::MapIo, - reflectivity_only_map::makeMapConfig> {}; + reflectivity_only_map::makeMapConfig< + reflectivity_only_map::Definition>> {}; class ReflectivityClassLayeredMapFactory final : public TypedLayeredMapFileFactory< @@ -180,16 +173,35 @@ class ReflectivityClassLayeredMapFactory final ReflectivityContinuousLayerColorProvider, reflectivity_map::Definition::Map, reflectivity_map::Definition::MapIo, - reflectivity_map::makeMapConfig> {}; + makeReflectivityClassMapConfig> {}; + +class SignalNearIrClassLayeredMapFactory final + : public TypedLayeredMapFileFactory< + signal_map::NearIrClassDefinition::ContinuousMap, + wavemap::convert::DescriptorContinuousRosConverter< + signal_map::NearIrClassDefinition::Voxel>, + SignalContinuousLayerColorProvider, + signal_map::NearIrClassDefinition::Map, + signal_map::NearIrClassDefinition::MapIo, + makeAllLayersH2MapConfig> {}; + +class RgbLayeredMapFactory final + : public TypedLayeredMapFileFactory< + rgb_map::Definition::ContinuousMap, + wavemap::convert::DescriptorContinuousRosConverter< + rgb_map::Definition::Voxel>, + RgbContinuousLayerColorProvider, rgb_map::Definition::Map, + rgb_map::Definition::MapIo, rgb_map::makeMapConfig> {}; } // namespace wavemap::rviz_plugin -PLUGINLIB_EXPORT_CLASS(wavemap::rviz_plugin::ExampleLayeredMapFactory, - wavemap::rviz_plugin::LayeredMapFactory) -PLUGINLIB_EXPORT_CLASS(wavemap::rviz_plugin::FieldLayeredMapFactory, - wavemap::rviz_plugin::LayeredMapFactory) PLUGINLIB_EXPORT_CLASS(wavemap::rviz_plugin::OccupancyLayeredMapFactory, wavemap::rviz_plugin::LayeredMapFactory) PLUGINLIB_EXPORT_CLASS(wavemap::rviz_plugin::ReflectivityLayeredMapFactory, wavemap::rviz_plugin::LayeredMapFactory) PLUGINLIB_EXPORT_CLASS(wavemap::rviz_plugin::ReflectivityClassLayeredMapFactory, wavemap::rviz_plugin::LayeredMapFactory) +PLUGINLIB_EXPORT_CLASS( + wavemap::rviz_plugin::SignalNearIrClassLayeredMapFactory, + wavemap::rviz_plugin::LayeredMapFactory) +PLUGINLIB_EXPORT_CLASS(wavemap::rviz_plugin::RgbLayeredMapFactory, + wavemap::rviz_plugin::LayeredMapFactory) diff --git a/interfaces/ros1/wavemap_rviz_plugin/src/layered_map_factories.cc.orig b/interfaces/ros1/wavemap_rviz_plugin/src/layered_map_factories.cc.orig deleted file mode 100644 index ef36e1d3f..000000000 --- a/interfaces/ros1/wavemap_rviz_plugin/src/layered_map_factories.cc.orig +++ /dev/null @@ -1,169 +0,0 @@ -#include -#include - -#include -#include - -#include "example_layered_map_ros_config.h" -#include "field_layered_map_ros_config.h" -#include "layered_ros_converter.h" -#include "reflectivity_map_layers.h" -#include "wavemap_rviz_plugin/layered_map_factory.h" - -namespace wavemap::rviz_plugin { -namespace field_map = wavemap::examples::field_map; -namespace reflectivity_map = wavemap::examples::reflectivity_map; - -FloatingPoint normalizeScalar(FloatingPoint value, - const LayerMetadata& metadata) { - if (metadata.min_values.size() != 1u || - metadata.max_values.size() != 1u || - metadata.max_values.front() <= metadata.min_values.front()) { - return std::clamp(value, 0.f, 1.f); - } - return std::clamp((value - metadata.min_values.front()) / - (metadata.max_values.front() - - metadata.min_values.front()), - 0.f, 1.f); -} - -FloatingPoint normalizeComponent(FloatingPoint value, - const LayerMetadata& metadata, - size_t component) { - if (component >= metadata.min_values.size() || - component >= metadata.max_values.size() || - metadata.max_values[component] <= metadata.min_values[component]) { - return std::clamp(value, 0.f, 1.f); - } - return std::clamp((value - metadata.min_values[component]) / - (metadata.max_values[component] - - metadata.min_values[component]), - 0.f, 1.f); -} - -Ogre::ColourValue scalarGray(FloatingPoint normalized_value) { - const FloatingPoint visible_value = - 0.15f + 0.85f * std::clamp(normalized_value, 0.f, 1.f); - return Ogre::ColourValue(visible_value, visible_value, visible_value, 1.f); -} - -Ogre::ColourValue scalarGradient(FloatingPoint normalized_value, - const Ogre::ColourValue& low, - const Ogre::ColourValue& high) { - const FloatingPoint value = std::clamp(normalized_value, 0.f, 1.f); - return Ogre::ColourValue(low.r + value * (high.r - low.r), - low.g + value * (high.g - low.g), - low.b + value * (high.b - low.b), 1.f); -} - -struct ExampleContinuousLayerColorProvider { - static bool getLayerColor(const std::string& layer_name, - const LayeredVoxel& voxel, - FloatingPoint /*occupancy*/, - FloatingPoint /*scalar_display_min*/, - FloatingPoint /*scalar_display_max*/, - const Ogre::ColourValue& /*scalar_low_color*/, - const Ogre::ColourValue& /*scalar_high_color*/, - const LayerMetadata& metadata, - Ogre::ColourValue& color) { - if (layer_name == "color") { - color = Ogre::ColourValue( - normalizeComponent(voxel.data.rgb.r, metadata, 0u), - normalizeComponent(voxel.data.rgb.g, metadata, 1u), - normalizeComponent(voxel.data.rgb.b, metadata, 2u), 1.f); - return true; - } - if (layer_name == "traversability") { - const FloatingPoint value = - normalizeScalar(voxel.data.traversability, metadata); - color = scalarGray(value); - return true; - } - return false; - } -}; - -struct FieldContinuousLayerColorProvider { - template - static bool getLayerColor(const std::string& layer_name, - const VoxelT& voxel, - FloatingPoint /*occupancy*/, - FloatingPoint scalar_display_min, - FloatingPoint scalar_display_max, - const Ogre::ColourValue& scalar_low_color, - const Ogre::ColourValue& scalar_high_color, - const LayerMetadata& /*metadata*/, - Ogre::ColourValue& color) { - if (layer_name != "reflectivity") { - return false; - } - LayerMetadata display_metadata; - display_metadata.min_values = {scalar_display_min}; - display_metadata.max_values = {scalar_display_max}; - const FloatingPoint value = normalizeScalar( - voxel.data.template get(), - display_metadata); - color = scalarGradient(value, scalar_low_color, scalar_high_color); - return true; - } -}; - -struct ReflectivityContinuousLayerColorProvider { - template - static bool getLayerColor(const std::string& layer_name, - const VoxelT& voxel, - FloatingPoint /*occupancy*/, - FloatingPoint scalar_display_min, - FloatingPoint scalar_display_max, - const Ogre::ColourValue& scalar_low_color, - const Ogre::ColourValue& scalar_high_color, - const LayerMetadata& /*metadata*/, - Ogre::ColourValue& color) { - if (layer_name != "reflectivity") { - return false; - } - LayerMetadata display_metadata; - display_metadata.min_values = {scalar_display_min}; - display_metadata.max_values = {scalar_display_max}; - const auto& state = - voxel.data.template get(); - const FloatingPoint value = normalizeScalar( - layered::LayerStateConversion< - reflectivity_map::ReflectivityLayer>::value(state), - display_metadata); - color = scalarGradient(value, scalar_low_color, scalar_high_color); - return true; - } -}; - -class ExampleLayeredMapFactory final - : public TypedLayeredMapFileFactory< - ContinuousWaveletMap, LayeredVoxelRosConverter, - ExampleContinuousLayerColorProvider, ExampleLayeredMap, - ExampleLayeredMapIo, - layered_map_config::makeDefaultExampleLayeredMapConfig> {}; - -class FieldLayeredMapFactory final - : public TypedLayeredMapFileFactory< - field_map::ContinuousMap, - field_map::ros_config::VoxelRosConverter, - FieldContinuousLayerColorProvider, field_map::Map, field_map::MapIo, - field_map::makeDefaultConfig> {}; - -class ReflectivityLayeredMapFactory final - : public TypedLayeredMapFileFactory< - reflectivity_map::Definition::ContinuousMap, - wavemap::convert::DescriptorContinuousRosConverter< - reflectivity_map::Definition::Voxel>, - ReflectivityContinuousLayerColorProvider, - reflectivity_map::Definition::Map, - reflectivity_map::Definition::MapIo, - reflectivity_map::makeMapConfig> {}; -} // namespace wavemap::rviz_plugin - -PLUGINLIB_EXPORT_CLASS(wavemap::rviz_plugin::ExampleLayeredMapFactory, - wavemap::rviz_plugin::LayeredMapFactory) -PLUGINLIB_EXPORT_CLASS(wavemap::rviz_plugin::FieldLayeredMapFactory, - wavemap::rviz_plugin::LayeredMapFactory) -PLUGINLIB_EXPORT_CLASS(wavemap::rviz_plugin::ReflectivityLayeredMapFactory, - wavemap::rviz_plugin::LayeredMapFactory) 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 66632bc19..e93bbadf3 100644 --- a/interfaces/ros1/wavemap_rviz_plugin/src/visuals/voxel_visual.cc +++ b/interfaces/ros1/wavemap_rviz_plugin/src/visuals/voxel_visual.cc @@ -370,6 +370,9 @@ void VoxelVisual::colorModeUpdateCallback() { // Update the map if the color mode changed if (voxel_color_mode_ != old_color_mode) { updateMap(true); + if (layer_color_changed_callback_) { + layer_color_changed_callback_(); + } } } @@ -382,6 +385,9 @@ void VoxelVisual::flatColorUpdateCallback() { // Update the map if the color changed if (voxel_flat_color_ != old_flat_color) { updateMap(true); + if (layer_color_changed_callback_) { + layer_color_changed_callback_(); + } } } @@ -941,7 +947,7 @@ void VoxelVisual::appendLayeredBlockOccupancy(const wavemap_msgs::LayeredHashedW OctreeIndex node_index; FloatingPoint occupancy_scale; LayerValues layer_scales; - size_t node_msg_index; + bool render_descendants; }; LayerValues root_layer_scales; @@ -997,18 +1003,28 @@ void VoxelVisual::appendLayeredBlockOccupancy(const wavemap_msgs::LayeredHashedW size_t next_node_msg_index = 0u; std::stack stack; - stack.emplace(StackElement{OctreeIndex{layered_map_msg.tree_height, Index3D{block_msg.root_node_offset.x, block_msg.root_node_offset.y, block_msg.root_node_offset.z}}, block_msg.root_node_occupancy_scale_coefficient, std::move(root_layer_scales), next_node_msg_index++}); + stack.emplace(StackElement{ + OctreeIndex{layered_map_msg.tree_height, + Index3D{block_msg.root_node_offset.x, + block_msg.root_node_offset.y, + block_msg.root_node_offset.z}}, + block_msg.root_node_occupancy_scale_coefficient, + std::move(root_layer_scales), true}); while (!stack.empty()) { StackElement stack_element = stack.top(); stack.pop(); - if (block_msg.nodes.size() <= stack_element.node_msg_index) { + if (block_msg.nodes.size() <= next_node_msg_index) { ROS_WARN("Layered map block ended before all queued nodes were read."); return; } - const auto& node_msg = block_msg.nodes[stack_element.node_msg_index]; + // Nodes are serialized in depth-first preorder. Consume the next message + // when its corresponding stack entry is popped instead of assigning + // indices while siblings are discovered. A sibling's message only follows + // after the complete subtree of its preceding sibling. + const auto& node_msg = block_msg.nodes[next_node_msg_index++]; Coefficients::Details occupancy_details; std::copy_n(node_msg.occupancy_detail_coefficients.begin(), occupancy_details.size(), occupancy_details.begin()); @@ -1047,7 +1063,7 @@ void VoxelVisual::appendLayeredBlockOccupancy(const wavemap_msgs::LayeredHashedW OctreeIndex node_index; FloatingPoint occupancy_scale; LayerValues layer_scales; - size_t node_msg_index; + bool render_descendants; }; std::vector children_to_visit; @@ -1056,11 +1072,20 @@ void VoxelVisual::appendLayeredBlockOccupancy(const wavemap_msgs::LayeredHashedW const FloatingPoint child_occupancy_scale = child_occupancy_scales[child_idx]; const bool child_exists = bit_ops::is_bit_set(node_msg.allocated_children_bitset, child_idx); - if (child_exists && termination_height < child_node_index.height) { - children_to_visit.push_back({child_node_index, child_occupancy_scale, - std::move(child_layer_scales[child_idx]), - next_node_msg_index++}); - } else { + if (child_exists) { + const bool render_child_descendants = + stack_element.render_descendants && + termination_height < child_node_index.height; + children_to_visit.push_back( + {child_node_index, child_occupancy_scale, + std::move(child_layer_scales[child_idx]), + render_child_descendants}); + if (stack_element.render_descendants && + !render_child_descendants) { + append_leaf(child_node_index, child_occupancy_scale, + children_to_visit.back().layer_scales); + } + } else if (stack_element.render_descendants) { append_leaf(child_node_index, child_occupancy_scale, child_layer_scales[child_idx]); } @@ -1072,9 +1097,13 @@ void VoxelVisual::appendLayeredBlockOccupancy(const wavemap_msgs::LayeredHashedW stack.emplace(StackElement{child_it->node_index, child_it->occupancy_scale, std::move(child_it->layer_scales), - child_it->node_msg_index}); + child_it->render_descendants}); } } + + if (next_node_msg_index != block_msg.nodes.size()) { + ROS_WARN("Layered map block contains unread serialized nodes."); + } } void VoxelVisual::processBlockUpdateQueue(const Point3D& camera_position) { diff --git a/library/cpp/include/wavemap/core/integrator/projective/coarse_to_fine/hashed_chunked_wavelet_integrator.h b/library/cpp/include/wavemap/core/integrator/projective/coarse_to_fine/hashed_chunked_wavelet_integrator.h index a8ebabc56..a2ed8b552 100644 --- a/library/cpp/include/wavemap/core/integrator/projective/coarse_to_fine/hashed_chunked_wavelet_integrator.h +++ b/library/cpp/include/wavemap/core/integrator/projective/coarse_to_fine/hashed_chunked_wavelet_integrator.h @@ -1,6 +1,7 @@ #ifndef WAVEMAP_CORE_INTEGRATOR_PROJECTIVE_COARSE_TO_FINE_HASHED_CHUNKED_WAVELET_INTEGRATOR_H_ #define WAVEMAP_CORE_INTEGRATOR_PROJECTIVE_COARSE_TO_FINE_HASHED_CHUNKED_WAVELET_INTEGRATOR_H_ +#include #include #include #include @@ -13,15 +14,20 @@ #include "wavemap/core/utils/thread_pool.h" namespace wavemap { -class HashedChunkedWaveletIntegrator : public ProjectiveIntegrator { +template +class HashedChunkedWaveletIntegratorT : public ProjectiveIntegrator { public: - HashedChunkedWaveletIntegrator( + using MapType = HashedChunkedWaveletOctreeT; + using MapPtr = typename MapType::Ptr; + using Block = typename MapType::Block; + using BlockIndex = typename MapType::BlockIndex; + HashedChunkedWaveletIntegratorT( const ProjectiveIntegratorConfig& config, ProjectorBase::ConstPtr projection_model, PosedImage<>::Ptr posed_range_image, Image::Ptr beam_offset_image, MeasurementModelBase::ConstPtr measurement_model, - HashedChunkedWaveletOctree::Ptr occupancy_map, + MapPtr occupancy_map, std::shared_ptr thread_pool = nullptr) : ProjectiveIntegrator( config, std::move(projection_model), std::move(posed_range_image), @@ -31,10 +37,10 @@ class HashedChunkedWaveletIntegrator : public ProjectiveIntegrator { : std::make_shared()) {} private: - using BlockList = std::vector; - using OctreeType = HashedChunkedWaveletOctreeBlock::OctreeType; + using BlockList = std::vector; + using OctreeType = typename Block::OctreeType; - const HashedChunkedWaveletOctree::Ptr occupancy_map_; + const MapPtr occupancy_map_; const std::shared_ptr thread_pool_; std::shared_ptr range_image_intersector_; @@ -56,23 +62,38 @@ class HashedChunkedWaveletIntegrator : public ProjectiveIntegrator { std::log2(config_.max_update_resolution / min_cell_width_))) : 0; + static FloatingPoint occupancyOf(const CellDataT& value) { + return static_cast(value); + } + + static void addClampedOccupancyUpdate(CellDataT& value, + FloatingPoint update, + FloatingPoint min_value, + FloatingPoint max_value) { + value += update; + value = std::clamp(occupancyOf(value), min_value, max_value); + } + std::pair getFovMinMaxIndices( const Point3D& sensor_origin) const; void recursiveTester(const OctreeIndex& node_index, BlockList& update_job_list); void updateMap() override; - void updateBlock(HashedChunkedWaveletOctree::Block& block, - const HashedChunkedWaveletOctree::BlockIndex& block_index); + void updateBlock(Block& block, + const BlockIndex& block_index); - void updateNodeRecursive(OctreeType::NodeRefType node, + void updateNodeRecursive(typename OctreeType::NodeRefType node, const OctreeIndex& node_index, - FloatingPoint& node_value, + CellDataT& node_value, bool& block_needs_thresholding); void updateLeavesBatch(const OctreeIndex& parent_index, - FloatingPoint& parent_value, - OctreeType::NodeDataType& parent_details); + CellDataT& parent_value, + typename OctreeType::NodeDataType& parent_details); }; + +using HashedChunkedWaveletIntegrator = + HashedChunkedWaveletIntegratorT; } // namespace wavemap #include "wavemap/core/integrator/projective/coarse_to_fine/impl/hashed_chunked_wavelet_integrator_inl.h" diff --git a/library/cpp/include/wavemap/core/integrator/projective/coarse_to_fine/impl/hashed_chunked_wavelet_integrator_inl.h b/library/cpp/include/wavemap/core/integrator/projective/coarse_to_fine/impl/hashed_chunked_wavelet_integrator_inl.h index f6159ce93..d057e77a1 100644 --- a/library/cpp/include/wavemap/core/integrator/projective/coarse_to_fine/impl/hashed_chunked_wavelet_integrator_inl.h +++ b/library/cpp/include/wavemap/core/integrator/projective/coarse_to_fine/impl/hashed_chunked_wavelet_integrator_inl.h @@ -1,12 +1,173 @@ #ifndef WAVEMAP_CORE_INTEGRATOR_PROJECTIVE_COARSE_TO_FINE_IMPL_HASHED_CHUNKED_WAVELET_INTEGRATOR_INL_H_ #define WAVEMAP_CORE_INTEGRATOR_PROJECTIVE_COARSE_TO_FINE_IMPL_HASHED_CHUNKED_WAVELET_INTEGRATOR_INL_H_ +#include +#include #include +#include + namespace wavemap { -inline void HashedChunkedWaveletIntegrator::recursiveTester( // NOLINT +template +void HashedChunkedWaveletIntegratorT::updateMap() { + ProfilerZoneScoped; + // Update the range image intersector + { + ProfilerZoneScopedN("updateRangeImageIntersector"); + range_image_intersector_ = std::make_shared( + posed_range_image_, projection_model_, *measurement_model_, + config_.min_range, config_.max_range); + } + + // Find all the indices of blocks that need updating + BlockList blocks_to_update; + { + ProfilerZoneScopedN("selectBlocksToUpdate"); + const auto [fov_min_idx, fov_max_idx] = + getFovMinMaxIndices(posed_range_image_->getOrigin()); + for (const auto& block_index : + Grid(fov_min_idx.position, fov_max_idx.position)) { + recursiveTester(OctreeIndex{fov_min_idx.height, block_index}, + blocks_to_update); + } + } + + // Make sure the to-be-updated blocks are allocated + for (const auto& block_index : blocks_to_update) { + occupancy_map_->getOrAllocateBlock(block_index); + } + + // Update it with the threadpool + for (const auto& block_index : blocks_to_update) { + thread_pool_->add_task([this, block_index]() { + if (auto* block = occupancy_map_->getBlock(block_index); block) { + updateBlock(*block, block_index); + } + }); + } + thread_pool_->wait_all(); +} + +template +std::pair +HashedChunkedWaveletIntegratorT::getFovMinMaxIndices( + const Point3D& sensor_origin) const { + const int height = 1 + std::max(static_cast(std::ceil(std::log2( + config_.max_range / min_cell_width_))), + tree_height_); + const OctreeIndex fov_min_idx = convert::indexAndHeightToNodeIndex<3>( + convert::pointToFloorIndex<3>( + sensor_origin - Vector3D::Constant(config_.max_range), + min_cell_width_inv_) - + occupancy_map_->getBlockSize(), + height); + const OctreeIndex fov_max_idx = convert::indexAndHeightToNodeIndex<3>( + convert::pointToCeilIndex<3>( + sensor_origin + Vector3D::Constant(config_.max_range), + min_cell_width_inv_) + + occupancy_map_->getBlockSize(), + height); + return {fov_min_idx, fov_max_idx}; +} + +template +void HashedChunkedWaveletIntegratorT::updateBlock( + Block& block, + const BlockIndex& block_index) { + ProfilerZoneScoped; + block.setNeedsPruning(); + block.setLastUpdatedStamp(); + + bool block_needs_thresholding = block.getNeedsThresholding(); + const OctreeIndex root_node_index{tree_height_, block_index}; + updateNodeRecursive(block.getRootNode(), root_node_index, + block.getRootScale(), block_needs_thresholding); + block.setNeedsThresholding(block_needs_thresholding); +} + +template +void HashedChunkedWaveletIntegratorT::updateNodeRecursive( // NOLINT + typename OctreeType::NodeRefType node, + const OctreeIndex& node_index, CellDataT& node_value, + bool& block_needs_thresholding) { + // Decompress child values + auto& node_details = node.data(); + auto child_values = Block::Transform::backward( + {node_value, node_details}); + + // Handle each child + for (NdtreeIndexRelativeChild relative_child_idx = 0; + relative_child_idx < OctreeIndex::kNumChildren; ++relative_child_idx) { + const OctreeIndex child_index = + node_index.computeChildIndex(relative_child_idx); + CellDataT& child_value = child_values[relative_child_idx]; + + // Test whether it is fully occupied; free or unknown; or fully unknown + const AABB W_child_aabb = + convert::nodeIndexToAABB(child_index, min_cell_width_); + const UpdateType update_type = + range_image_intersector_->determineUpdateType( + W_child_aabb, posed_range_image_->getRotationMatrixInverse(), + posed_range_image_->getOrigin()); + + // If we're fully in unknown space, + // there's no need to evaluate this node or its children + if (update_type == UpdateType::kFullyUnobserved) { + continue; + } + + // We can also stop here if the cell will result in a free space update + // (or zero) and the map is already saturated free + if (update_type != UpdateType::kPossiblyOccupied && + occupancyOf(child_value) < min_log_odds_shrunk_) { + continue; + } + + // Test if the worst-case error for the intersection type at the current + // resolution falls within the acceptable approximation error + const FloatingPoint child_width = W_child_aabb.width<0>(); + const Point3D W_child_center = + W_child_aabb.min + Vector3D::Constant(child_width / 2.f); + const Point3D C_child_center = + posed_range_image_->getPoseInverse() * W_child_center; + const FloatingPoint d_C_child = + projection_model_->cartesianToSensorZ(C_child_center); + const FloatingPoint bounding_sphere_radius = + kUnitCubeHalfDiagonal * child_width; + if (measurement_model_->computeWorstCaseApproximationError( + update_type, d_C_child, bounding_sphere_radius) < + config_.termination_update_error) { + const FloatingPoint sample = computeUpdate(C_child_center); + child_value += sample; + block_needs_thresholding = true; + continue; + } + + // Since the approximation error would still be too big, refine + auto child_node = node.getOrAllocateChild(relative_child_idx); + auto& child_details = child_node.data(); + + // If we're at the leaf level, directly compute the update + if (child_index.height <= termination_height_ + 1) { + updateLeavesBatch(child_index, child_value, child_details); + } else { + // Otherwise, recurse + DCHECK_GE(child_index.height, 0); + updateNodeRecursive(child_node, child_index, child_value, + block_needs_thresholding); + } + } + + // Compress + const auto [new_value, new_details] = + Block::Transform::forward(child_values); + node_details = new_details; + node_value = new_value; +} +template +inline void HashedChunkedWaveletIntegratorT::recursiveTester( // NOLINT const OctreeIndex& node_index, - HashedChunkedWaveletIntegrator::BlockList& update_job_list) { + HashedChunkedWaveletIntegratorT::BlockList& update_job_list) { const AABB block_aabb = convert::nodeIndexToAABB(node_index, min_cell_width_); const UpdateType update_type = range_image_intersector_->determineUpdateType( @@ -24,7 +185,7 @@ inline void HashedChunkedWaveletIntegrator::recursiveTester( // NOLINT } if (const auto* block = occupancy_map_->getBlock(node_index.position); block) { - if (min_log_odds_shrunk_ <= block->getRootScale()) { + if (min_log_odds_shrunk_ <= occupancyOf(block->getRootScale())) { // Add the block to the job list update_job_list.emplace_back(node_index.position); } @@ -37,11 +198,12 @@ inline void HashedChunkedWaveletIntegrator::recursiveTester( // NOLINT } } -inline void HashedChunkedWaveletIntegrator::updateLeavesBatch( - const OctreeIndex& parent_index, FloatingPoint& parent_value, - HashedChunkedWaveletIntegrator::OctreeType::NodeDataType& parent_details) { +template +inline void HashedChunkedWaveletIntegratorT::updateLeavesBatch( + const OctreeIndex& parent_index, CellDataT& parent_value, + typename OctreeType::NodeDataType& parent_details) { // Decompress - auto child_values = HashedChunkedWaveletOctreeBlock::Transform::backward( + auto child_values = Block::Transform::backward( {parent_value, parent_details}); // Get child center points in world frame W @@ -61,22 +223,14 @@ inline void HashedChunkedWaveletIntegrator::updateLeavesBatch( // Compute updated values for (int child_idx = 0; child_idx < OctreeIndex::kNumChildren; ++child_idx) { const FloatingPoint sample = computeUpdate(child_centers.col(child_idx)); - FloatingPoint& child_value = child_values[child_idx]; - child_value = sample + child_value; - } - - // Threshold - for (int child_idx = 0; child_idx < OctreeIndex::kNumChildren; ++child_idx) { - FloatingPoint& child_value = child_values[child_idx]; - child_value = (child_value < min_log_odds_padded_) ? min_log_odds_padded_ - : child_value; - child_value = (max_log_odds_padded_ < child_value) ? max_log_odds_padded_ - : child_value; + CellDataT& child_value = child_values[child_idx]; + addClampedOccupancyUpdate(child_value, sample, + min_log_odds_padded_, max_log_odds_padded_); } // Compress const auto [new_value, new_details] = - HashedChunkedWaveletOctreeBlock::Transform::forward(child_values); + Block::Transform::forward(child_values); parent_details = new_details; parent_value = new_value; } 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 a733fc0f7..3404cebf2 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 @@ -2,6 +2,7 @@ #define WAVEMAP_CORE_MAP_HASHED_CHUNKED_WAVELET_OCTREE_H_ #include +#include #include #include "wavemap/core/common.h" @@ -14,14 +15,11 @@ namespace wavemap { /** - * Config struct for the hashed chunked wavelet octree volumetric data - * structure. + * Config struct for the hashed wavelet octree volumetric data structure. */ -struct HashedChunkedWaveletOctreeConfig - : ConfigBase { +struct HashedChunkedWaveletOctreeConfig : ConfigBase { static constexpr IndexElement kMaxSupportedTreeHeight = HashedChunkedWaveletOctreeBlock::kMaxSupportedTreeHeight; - //! Maximum resolution of the map, set as the width of the smallest cell that //! it can represent. Meters min_cell_width = 0.1f; @@ -42,10 +40,11 @@ struct HashedChunkedWaveletOctreeConfig // Constructors HashedChunkedWaveletOctreeConfig() = default; - HashedChunkedWaveletOctreeConfig( - FloatingPoint min_cell_width, FloatingPoint min_log_odds, - FloatingPoint max_log_odds, IndexElement tree_height, - FloatingPoint only_prune_blocks_if_unused_for) + HashedChunkedWaveletOctreeConfig(FloatingPoint min_cell_width, + FloatingPoint min_log_odds, + FloatingPoint max_log_odds, + IndexElement tree_height, + FloatingPoint only_prune_blocks_if_unused_for) : min_cell_width(min_cell_width), min_log_odds(min_log_odds), max_log_odds(max_log_odds), @@ -60,27 +59,41 @@ struct HashedChunkedWaveletOctreeConfig bool isValid(bool verbose) const override; }; -class HashedChunkedWaveletOctree : public MapBase { +template +class HashedChunkedWaveletOctreeT : public MapBase { public: - using Ptr = std::shared_ptr; - using ConstPtr = std::shared_ptr; + using Ptr = std::shared_ptr>; + using ConstPtr = std::shared_ptr>; using Config = HashedChunkedWaveletOctreeConfig; + using CellDataType = CellDataT; static constexpr bool kRequiresExplicitThresholding = true; using BlockIndex = Index3D; using CellIndex = OctreeIndex; - using Block = HashedChunkedWaveletOctreeBlock; + using Block = HashedChunkedWaveletOctreeBlockT; using BlockHashMap = SpatialHash; - - explicit HashedChunkedWaveletOctree( - const HashedChunkedWaveletOctreeConfig& config) - : MapBase(config), config_(config.checkValid()) {} + using Traits = CellDataTraits; + using ThresholdConfig = typename Traits::ThresholdConfig; + using PruningConfig = typename Traits::PruningConfig; + + explicit HashedChunkedWaveletOctreeT( + const HashedChunkedWaveletOctreeConfig& config, + ThresholdConfig threshold_config = ThresholdConfig{}, + PruningConfig pruning_config = PruningConfig{}) + : MapBase(config), + config_(config.checkValid()), + threshold_config_(threshold_config), + pruning_config_(pruning_config) {} // Copy construction is not supported - HashedChunkedWaveletOctree(const HashedChunkedWaveletOctree&) = delete; + HashedChunkedWaveletOctreeT(const HashedChunkedWaveletOctreeT&) = delete; MapType getMapType() const override { - return MapType::kHashedChunkedWaveletOctree; + if constexpr (std::is_same_v) { + return MapType::kHashedChunkedWaveletOctree; + } else { + return MapType::kLayeredHashedWaveletOctree; + } } bool empty() const override { return block_map_.empty(); } @@ -101,12 +114,19 @@ 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_; } + const ThresholdConfig& getThresholdConfig() const { + return threshold_config_; + } FloatingPoint getCellValue(const Index3D& index) const override; FloatingPoint getCellValue(const OctreeIndex& index) const; + CellDataT getVoxelValue(const Index3D& index) const; + CellDataT getVoxelValue(const OctreeIndex& index) const; void setCellValue(const Index3D& index, FloatingPoint new_value) override; + void setVoxelValue(const Index3D& index, const CellDataT& new_value); void addToCellValue(const Index3D& index, FloatingPoint update) override; + void addToVoxelValue(const Index3D& index, const CellDataT& update); bool hasBlock(const Index3D& block_index) const; bool eraseBlock(const BlockIndex& block_index); @@ -128,6 +148,9 @@ class HashedChunkedWaveletOctree : public MapBase { void forEachLeaf( typename MapBase::IndexedLeafVisitorFunction visitor_fn) const override; + template + void forEachVoxelLeaf(IndexedVoxelLeafVisitorFunction visitor_fn) const; + BlockIndex indexToBlockIndex(const OctreeIndex& node_index) const; CellIndex indexToCellIndex(OctreeIndex index) const; @@ -135,9 +158,13 @@ class HashedChunkedWaveletOctree : public MapBase { const HashedChunkedWaveletOctreeConfig config_; const IndexElement cells_per_block_side_ = int_math::exp2(config_.tree_height); + const ThresholdConfig threshold_config_; + const PruningConfig pruning_config_; BlockHashMap block_map_; }; + +using HashedChunkedWaveletOctree = HashedChunkedWaveletOctreeT; } // namespace wavemap #include "wavemap/core/map/impl/hashed_chunked_wavelet_octree_inl.h" diff --git a/library/cpp/include/wavemap/core/map/hashed_chunked_wavelet_octree_block.h b/library/cpp/include/wavemap/core/map/hashed_chunked_wavelet_octree_block.h index 5c457414e..74d8485a3 100644 --- a/library/cpp/include/wavemap/core/map/hashed_chunked_wavelet_octree_block.h +++ b/library/cpp/include/wavemap/core/map/hashed_chunked_wavelet_octree_block.h @@ -3,28 +3,39 @@ #include "wavemap/core/common.h" #include "wavemap/core/data_structure/chunked_ndtree/chunked_ndtree.h" +#include "wavemap/core/map/cell_types/cell_data_traits.h" #include "wavemap/core/map/cell_types/haar_coefficients.h" #include "wavemap/core/map/cell_types/haar_transform.h" +#include "wavemap/core/map/cell_types/voxel_data.h" #include "wavemap/core/map/map_base.h" #include "wavemap/core/utils/time/time.h" namespace wavemap { -class HashedChunkedWaveletOctreeBlock { +template +class HashedChunkedWaveletOctreeBlockT { public: static constexpr int kDim = 3; static constexpr int kChunkHeight = 3; static constexpr int kMaxSupportedTreeHeight = 9; + using CellDataType = CellDataT; using BlockIndex = Index3D; - using Coefficients = HaarCoefficients; - using Transform = HaarTransform; - using OctreeType = ChunkedOctree; - - explicit HashedChunkedWaveletOctreeBlock(IndexElement tree_height, - FloatingPoint min_log_odds, - FloatingPoint max_log_odds) + using Coefficients = HaarCoefficients; + using Transform = HaarTransform; + using OctreeType = ChunkedOctree; + using Traits = CellDataTraits; + using ThresholdConfig = typename Traits::ThresholdConfig; + using PruningConfig = typename Traits::PruningConfig; + + explicit HashedChunkedWaveletOctreeBlockT( + IndexElement tree_height, FloatingPoint min_log_odds, + FloatingPoint max_log_odds, + ThresholdConfig threshold_config = ThresholdConfig{}, + PruningConfig pruning_config = PruningConfig{}) : tree_height_(tree_height), min_log_odds_(min_log_odds), - max_log_odds_(max_log_odds) {} + max_log_odds_(max_log_odds), + threshold_config_(threshold_config), + pruning_config_(pruning_config) {} bool empty() const; size_t size() const { return ndtree_.size(); } @@ -33,33 +44,42 @@ class HashedChunkedWaveletOctreeBlock { void clear(); FloatingPoint getCellValue(const OctreeIndex& index) const; + CellDataT getVoxelValue(const OctreeIndex& index) const; void setCellValue(const OctreeIndex& index, FloatingPoint new_value); + void setVoxelValue(const OctreeIndex& index, const CellDataT& new_value); void addToCellValue(const OctreeIndex& index, FloatingPoint update); + void addToVoxelValue(const OctreeIndex& index, const CellDataT& update); void forEachLeaf(const BlockIndex& block_index, typename MapBase::IndexedLeafVisitorFunction visitor_fn, IndexElement termination_height = 0) const; - Coefficients::Scale& getRootScale() { return root_scale_coefficient_; } - const Coefficients::Scale& getRootScale() const { + template + void forEachVoxelLeaf(const BlockIndex& block_index, + IndexedVoxelLeafVisitorFunction visitor_fn, + IndexElement termination_height = 0) const; + + typename Coefficients::Scale& getRootScale() { return root_scale_coefficient_; } - OctreeType::NodeRefType getRootNode() { return ndtree_.getRootNode(); } - OctreeType::NodeConstRefType getRootNode() const { + const typename Coefficients::Scale& getRootScale() const { + return root_scale_coefficient_; + } + typename OctreeType::NodeRefType getRootNode() { return ndtree_.getRootNode(); } - OctreeType::ChunkType& getRootChunk() { return ndtree_.getRootChunk(); } - const OctreeType::ChunkType& getRootChunk() const { + typename OctreeType::NodeConstRefType getRootNode() const { + return ndtree_.getRootNode(); + } + typename OctreeType::ChunkType& getRootChunk() { return ndtree_.getRootChunk(); } + const typename OctreeType::ChunkType& getRootChunk() const { return ndtree_.getRootChunk(); } void setNeedsPruning(bool value = true) { needs_pruning_ = value; } bool getNeedsPruning() const { return needs_pruning_; } - bool& getNeedsPruning() { return needs_pruning_; } - void setNeedsThresholding(bool value = true) { needs_thresholding_ = value; } bool getNeedsThresholding() const { return needs_thresholding_; } - void setLastUpdatedStamp(Timestamp stamp = Time::now()) { last_updated_stamp_ = stamp; } @@ -68,11 +88,11 @@ class HashedChunkedWaveletOctreeBlock { template auto getChunkIterator() { - return ndtree_.getChunkIterator(); + return ndtree_.template getChunkIterator(); } template auto getChunkIterator() const { - return ndtree_.getChunkIterator(); + return ndtree_.template getChunkIterator(); } size_t getMemoryUsage() const { return ndtree_.getMemoryUsage(); } @@ -81,19 +101,22 @@ class HashedChunkedWaveletOctreeBlock { const IndexElement tree_height_; const FloatingPoint min_log_odds_; const FloatingPoint max_log_odds_; + const ThresholdConfig threshold_config_; + const PruningConfig pruning_config_; OctreeType ndtree_{tree_height_ - 1}; - Coefficients::Scale root_scale_coefficient_{}; + typename Coefficients::Scale root_scale_coefficient_{}; bool needs_thresholding_ = false; bool needs_pruning_ = false; Timestamp last_updated_stamp_ = Time::now(); - void recursiveThreshold(OctreeType::NodeRefType node, - Coefficients::Scale& node_scale_coefficient); - void recursivePrune( - HashedChunkedWaveletOctreeBlock::OctreeType::NodeRefType node); + void recursiveThreshold(typename OctreeType::NodeRefType node, + typename Coefficients::Scale& node_scale_coefficient); + void recursivePrune(typename OctreeType::NodeRefType node); }; + +using HashedChunkedWaveletOctreeBlock = HashedChunkedWaveletOctreeBlockT; } // namespace wavemap #include "wavemap/core/map/impl/hashed_chunked_wavelet_octree_block_inl.h" diff --git a/library/cpp/include/wavemap/core/map/impl/hashed_chunked_wavelet_octree_block_inl.h b/library/cpp/include/wavemap/core/map/impl/hashed_chunked_wavelet_octree_block_inl.h index 0cd6ae505..a4e28fc65 100644 --- a/library/cpp/include/wavemap/core/map/impl/hashed_chunked_wavelet_octree_block_inl.h +++ b/library/cpp/include/wavemap/core/map/impl/hashed_chunked_wavelet_octree_block_inl.h @@ -1,28 +1,96 @@ #ifndef WAVEMAP_CORE_MAP_IMPL_HASHED_CHUNKED_WAVELET_OCTREE_BLOCK_INL_H_ #define WAVEMAP_CORE_MAP_IMPL_HASHED_CHUNKED_WAVELET_OCTREE_BLOCK_INL_H_ +#include +#include +#include +#include +#include + +#include "wavemap/core/utils/profile/profiler_interface.h" #include "wavemap/core/utils/query/occupancy_classifier.h" namespace wavemap { -inline bool HashedChunkedWaveletOctreeBlock::empty() const { - // Check if all cells in the block are equal to zero - // NOTE: Aside from checking whether the block contains no detail - // coefficients, we also need to check whether its scale coefficient - // (average value over the whole block) is zero. - return ndtree_.empty() && - OccupancyClassifier::isUnobserved(root_scale_coefficient_); +namespace chunked_detail { +template +FloatingPoint getOccupancy(const CellDataT& value) { + return static_cast(value); +} + +template +void setOccupancy(CellDataT& value, FloatingPoint occupancy) { + value = occupancy; +} + +template +struct HasVoxelDataFields : std::false_type {}; + +template +struct HasVoxelDataFields< + T, std::void_t().occupancy), + decltype(std::declval().data)>> : std::true_type {}; + +template +struct HasOccupancyField : std::false_type {}; + +template +struct HasOccupancyField().occupancy)>> + : std::true_type {}; + +template +void thresholdCellData(CellDataT& value, FloatingPoint min_log_odds, + FloatingPoint max_log_odds, + const typename CellDataTraits::ThresholdConfig& + threshold_config) { + if constexpr (std::is_arithmetic_v) { + value = std::clamp(value, min_log_odds, max_log_odds); + } else { + if constexpr (HasVoxelDataFields::value) { + value.occupancy = std::clamp(value.occupancy, min_log_odds, max_log_odds); + } else if constexpr (HasOccupancyField::value) { + value.occupancy = std::clamp(value.occupancy, min_log_odds, max_log_odds); + } + CellDataTraits::threshold(value, threshold_config); + } +} + +template +bool detailsHaveNonzeroData( + const typename HaarCoefficients::Details& details, + const typename CellDataTraits::PruningConfig& pruning_config) { + return std::any_of(details.cbegin(), details.cend(), + [&pruning_config](const CellDataT& value) { + return CellDataTraits::isNonzero( + value, pruning_config); + }); } -inline FloatingPoint HashedChunkedWaveletOctreeBlock::getTimeSinceLastUpdated() +} // namespace chunked_detail + +template +bool HashedChunkedWaveletOctreeBlockT::empty() const { + return ndtree_.empty() && OccupancyClassifier::isUnobserved( + chunked_detail::getOccupancy(root_scale_coefficient_)); +} + +template +FloatingPoint HashedChunkedWaveletOctreeBlockT::getTimeSinceLastUpdated() const { return time::to_seconds(Time::now() - last_updated_stamp_); } -inline FloatingPoint HashedChunkedWaveletOctreeBlock::getCellValue( +template +FloatingPoint HashedChunkedWaveletOctreeBlockT::getCellValue( + const OctreeIndex& index) const { + return chunked_detail::getOccupancy(getVoxelValue(index)); +} + +template +CellDataT HashedChunkedWaveletOctreeBlockT::getVoxelValue( const OctreeIndex& index) const { const MortonIndex morton_code = convert::nodeIndexToMorton(index); - OctreeType::NodeConstPtrType node = &ndtree_.getRootNode(); - FloatingPoint value = root_scale_coefficient_; + typename OctreeType::NodeConstPtrType node = &ndtree_.getRootNode(); + CellDataT value = root_scale_coefficient_; for (int parent_height = tree_height_; node && index.height < parent_height; --parent_height) { const NdtreeIndexRelativeChild child_index = @@ -32,6 +100,230 @@ inline FloatingPoint HashedChunkedWaveletOctreeBlock::getCellValue( } return value; } + +template +void HashedChunkedWaveletOctreeBlockT::threshold() { + ProfilerZoneScoped; + if (getNeedsThresholding()) { + recursiveThreshold(ndtree_.getRootNode(), root_scale_coefficient_); + setNeedsThresholding(false); + } +} + +template +void HashedChunkedWaveletOctreeBlockT::prune() { + ProfilerZoneScoped; + if (getNeedsPruning()) { + threshold(); + recursivePrune(ndtree_.getRootNode()); + setNeedsPruning(false); + } +} + +template +void HashedChunkedWaveletOctreeBlockT::clear() { + ProfilerZoneScoped; + root_scale_coefficient_ = typename Coefficients::Scale{}; + ndtree_.clear(); + setLastUpdatedStamp(); +} + +template +void HashedChunkedWaveletOctreeBlockT::setCellValue( + const OctreeIndex& index, FloatingPoint new_value) { + CellDataT voxel = getVoxelValue(index); + chunked_detail::setOccupancy(voxel, new_value); + setVoxelValue(index, voxel); +} + +template +void HashedChunkedWaveletOctreeBlockT::setVoxelValue( + const OctreeIndex& index, const CellDataT& new_value) { + setNeedsPruning(); + setNeedsThresholding(); + setLastUpdatedStamp(); + const MortonIndex morton_code = convert::nodeIndexToMorton(index); + std::vector ancestors; + const int height_difference = tree_height_ - index.height; + ancestors.reserve(height_difference); + ancestors.emplace_back(ndtree_.getRootNode()); + CellDataT current_value = root_scale_coefficient_; + for (int parent_height = tree_height_; index.height + 1 < parent_height; + --parent_height) { + const NdtreeIndexRelativeChild child_index = + OctreeIndex::computeRelativeChildIndex(morton_code, parent_height); + typename OctreeType::NodeRefType current_parent = ancestors.back(); + current_value = Transform::backwardSingleChild( + {current_value, current_parent.data()}, child_index); + typename OctreeType::NodeRefType child = + current_parent.getOrAllocateChild(child_index); + ancestors.emplace_back(child); + } + DCHECK_EQ(ancestors.size(), height_difference); + + typename Coefficients::Parent coefficients{new_value - current_value, {}}; + for (int parent_height = index.height + 1; parent_height <= tree_height_; + ++parent_height) { + const NdtreeIndexRelativeChild child_index = + OctreeIndex::computeRelativeChildIndex(morton_code, parent_height); + typename OctreeType::NodeRefType current_node = ancestors.back(); + ancestors.pop_back(); + coefficients = + Transform::forwardSingleChild(coefficients.scale, child_index); + current_node.data() += coefficients.details; + } + + root_scale_coefficient_ += coefficients.scale; +} + +template +void HashedChunkedWaveletOctreeBlockT::addToCellValue( + const OctreeIndex& index, FloatingPoint update) { + addToVoxelValue(index, CellDataT{update}); +} + +template +void HashedChunkedWaveletOctreeBlockT::addToVoxelValue( + const OctreeIndex& index, const CellDataT& update) { + setNeedsPruning(); + setNeedsThresholding(); + setLastUpdatedStamp(); + const MortonIndex morton_code = convert::nodeIndexToMorton(index); + + std::vector ancestors; + const int height_difference = tree_height_ - index.height; + ancestors.reserve(height_difference); + ancestors.emplace_back(ndtree_.getRootNode()); + for (int parent_height = tree_height_; index.height + 1 < parent_height; + --parent_height) { + const NdtreeIndexRelativeChild child_index = + OctreeIndex::computeRelativeChildIndex(morton_code, parent_height); + typename OctreeType::NodeRefType current_parent = ancestors.back(); + typename OctreeType::NodeRefType child = + current_parent.getOrAllocateChild(child_index); + ancestors.emplace_back(child); + } + DCHECK_EQ(ancestors.size(), height_difference); + + typename Coefficients::Parent coefficients{update, {}}; + for (int parent_height = index.height + 1; parent_height <= tree_height_; + ++parent_height) { + typename OctreeType::NodeRefType current_node = ancestors.back(); + ancestors.pop_back(); + const NdtreeIndexRelativeChild child_index = + OctreeIndex::computeRelativeChildIndex(morton_code, parent_height); + coefficients = + Transform::forwardSingleChild(coefficients.scale, child_index); + current_node.data() += coefficients.details; + } + root_scale_coefficient_ += coefficients.scale; +} + +template +void HashedChunkedWaveletOctreeBlockT::forEachLeaf( + const BlockIndex& block_index, + MapBase::IndexedLeafVisitorFunction visitor_fn, + IndexElement termination_height) const { + forEachVoxelLeaf( + block_index, + [&visitor_fn](const OctreeIndex& node_index, + const CellDataT& voxel_value) { + visitor_fn(node_index, chunked_detail::getOccupancy(voxel_value)); + }, + termination_height); +} + +template +template +void HashedChunkedWaveletOctreeBlockT::forEachVoxelLeaf( + const BlockIndex& block_index, IndexedVoxelLeafVisitorFunction visitor_fn, + IndexElement termination_height) const { + ProfilerZoneScoped; + if (empty()) { + return; + } + + struct StackElement { + const OctreeIndex node_index; + typename OctreeType::NodeConstRefType node; + const typename Coefficients::Scale scale_coefficient{}; + }; + std::stack stack; + stack.emplace(StackElement{OctreeIndex{tree_height_, block_index}, + ndtree_.getRootNode(), root_scale_coefficient_}); + while (!stack.empty()) { + const OctreeIndex node_index = stack.top().node_index; + typename OctreeType::NodeConstRefType node = stack.top().node; + const typename Coefficients::Scale node_scale_coefficient = + stack.top().scale_coefficient; + stack.pop(); + + const typename Coefficients::CoefficientsArray child_scale_coefficients = + Transform::backward({node_scale_coefficient, {node.data()}}); + for (NdtreeIndexRelativeChild child_idx = 0; + child_idx < OctreeIndex::kNumChildren; ++child_idx) { + const OctreeIndex child_node_index = + node_index.computeChildIndex(child_idx); + const typename Coefficients::Scale child_scale_coefficient = + child_scale_coefficients[child_idx]; + typename OctreeType::NodeConstPtrType child_node = node.getChild(child_idx); + if (child_node && termination_height < child_node_index.height) { + stack.emplace(StackElement{child_node_index, *child_node, + child_scale_coefficient}); + } else { + visitor_fn(child_node_index, child_scale_coefficient); + } + } + } +} + +template +void HashedChunkedWaveletOctreeBlockT::recursiveThreshold( + typename OctreeType::NodeRefType node, + typename Coefficients::Scale& node_scale_coefficient) { + auto& node_detail_coefficients = node.data(); + typename Coefficients::CoefficientsArray child_scale_coefficients = + Transform::backward({node_scale_coefficient, node_detail_coefficients}); + + for (NdtreeIndexRelativeChild child_idx = 0; + child_idx < OctreeIndex::kNumChildren; ++child_idx) { + typename Coefficients::Scale& child_scale = child_scale_coefficients[child_idx]; + if (auto child_node = node.getChild(child_idx); child_node) { + recursiveThreshold(*child_node, child_scale); + } else { + chunked_detail::thresholdCellData(child_scale, min_log_odds_, max_log_odds_, + threshold_config_); + } + } + + const auto [new_scale, new_details] = + Transform::forward(child_scale_coefficients); + node_detail_coefficients = new_details; + node_scale_coefficient = new_scale; +} + +template +void HashedChunkedWaveletOctreeBlockT::recursivePrune( + typename OctreeType::NodeRefType node) { + bool has_at_least_one_child = false; + for (NdtreeIndexRelativeChild child_idx = 0; + child_idx < OctreeIndex::kNumChildren; ++child_idx) { + if (typename OctreeType::NodePtrType child_node = node.getChild(child_idx); + child_node) { + recursivePrune(*child_node); + if (!child_node->hasAtLeastOneChild() && + !chunked_detail::detailsHaveNonzeroData(child_node->data(), + pruning_config_)) { + node.eraseChild(child_idx); + } else { + has_at_least_one_child = true; + } + } + } + if (!has_at_least_one_child) { + node.hasAtLeastOneChild() = has_at_least_one_child; + } +} } // namespace wavemap #endif // WAVEMAP_CORE_MAP_IMPL_HASHED_CHUNKED_WAVELET_OCTREE_BLOCK_INL_H_ diff --git a/library/cpp/include/wavemap/core/map/impl/hashed_chunked_wavelet_octree_inl.h b/library/cpp/include/wavemap/core/map/impl/hashed_chunked_wavelet_octree_inl.h index f6182d909..a5c65ea66 100644 --- a/library/cpp/include/wavemap/core/map/impl/hashed_chunked_wavelet_octree_inl.h +++ b/library/cpp/include/wavemap/core/map/impl/hashed_chunked_wavelet_octree_inl.h @@ -2,9 +2,11 @@ #define WAVEMAP_CORE_MAP_IMPL_HASHED_CHUNKED_WAVELET_OCTREE_INL_H_ #include "wavemap/core/indexing/index_conversions.h" +#include "wavemap/core/utils/profile/profiler_interface.h" namespace wavemap { -inline size_t HashedChunkedWaveletOctree::size() const { +template +size_t HashedChunkedWaveletOctreeT::size() const { size_t size = 0u; forEachBlock([&size](const BlockIndex& /*block_index*/, const Block& block) { size += block.size(); @@ -12,7 +14,63 @@ inline size_t HashedChunkedWaveletOctree::size() const { return size; } -inline FloatingPoint HashedChunkedWaveletOctree::getCellValue( +template +void HashedChunkedWaveletOctreeT::threshold() { + ProfilerZoneScoped; + forEachBlock([](const BlockIndex& /*block_index*/, Block& block) { + block.threshold(); + }); +} + +template +void HashedChunkedWaveletOctreeT::prune() { + ProfilerZoneScoped; + block_map_.eraseBlockIf([](const BlockIndex& /*block_index*/, Block& block) { + block.prune(); + return block.empty(); + }); +} + +template +void HashedChunkedWaveletOctreeT::pruneSmart() { + ProfilerZoneScoped; + block_map_.eraseBlockIf( + [&config = config_](const BlockIndex& /*block_index*/, Block& block) { + if (config.only_prune_blocks_if_unused_for < + block.getTimeSinceLastUpdated()) { + block.prune(); + } + return block.empty(); + }); +} + +template +size_t HashedChunkedWaveletOctreeT::getMemoryUsage() const { + ProfilerZoneScoped; + // TODO(victorr): Also include the memory usage of the unordered map itself + size_t memory_usage = 0u; + forEachBlock( + [&memory_usage](const BlockIndex& /*block_index*/, const Block& block) { + memory_usage += block.getMemoryUsage(); + }); + return memory_usage; +} + +template +Index3D HashedChunkedWaveletOctreeT::getMinIndex() const { + return cells_per_block_side_ * getMinBlockIndex(); +} + +template +Index3D HashedChunkedWaveletOctreeT::getMaxIndex() const { + if (empty()) { + return Index3D::Zero(); + } + return cells_per_block_side_ * (getMaxBlockIndex().array() + 1) - 1; +} + +template +FloatingPoint HashedChunkedWaveletOctreeT::getCellValue( const Index3D& index) const { const BlockIndex block_index = convert::indexToBlockIndex(index, config_.tree_height); @@ -24,7 +82,8 @@ inline FloatingPoint HashedChunkedWaveletOctree::getCellValue( return block->getCellValue(cell_index); } -inline FloatingPoint HashedChunkedWaveletOctree::getCellValue( +template +FloatingPoint HashedChunkedWaveletOctreeT::getCellValue( const OctreeIndex& index) const { const BlockIndex block_index = indexToBlockIndex(index); const Block* block = getBlock(block_index); @@ -35,8 +94,34 @@ inline FloatingPoint HashedChunkedWaveletOctree::getCellValue( return block->getCellValue(cell_index); } -inline void HashedChunkedWaveletOctree::setCellValue(const Index3D& index, - FloatingPoint new_value) { +template +CellDataT HashedChunkedWaveletOctreeT::getVoxelValue( + const Index3D& index) const { + const BlockIndex block_index = + convert::indexToBlockIndex(index, config_.tree_height); + const Block* block = getBlock(block_index); + if (!block) { + return {}; + } + const CellIndex cell_index = indexToCellIndex({0, index}); + return block->getVoxelValue(cell_index); +} + +template +CellDataT HashedChunkedWaveletOctreeT::getVoxelValue( + const OctreeIndex& index) const { + const BlockIndex block_index = indexToBlockIndex(index); + const Block* block = getBlock(block_index); + if (!block) { + return {}; + } + const CellIndex cell_index = indexToCellIndex(index); + return block->getVoxelValue(cell_index); +} + +template +void HashedChunkedWaveletOctreeT::setCellValue(const Index3D& index, + FloatingPoint new_value) { const BlockIndex block_index = convert::indexToBlockIndex(index, config_.tree_height); auto& block = getOrAllocateBlock(block_index); @@ -44,8 +129,19 @@ inline void HashedChunkedWaveletOctree::setCellValue(const Index3D& index, block.setCellValue(cell_index, new_value); } -inline void HashedChunkedWaveletOctree::addToCellValue(const Index3D& index, - FloatingPoint update) { +template +void HashedChunkedWaveletOctreeT::setVoxelValue( + const Index3D& index, const CellDataT& new_value) { + const BlockIndex block_index = + convert::indexToBlockIndex(index, config_.tree_height); + auto& block = getOrAllocateBlock(block_index); + const CellIndex cell_index = indexToCellIndex({0, index}); + block.setVoxelValue(cell_index, new_value); +} + +template +void HashedChunkedWaveletOctreeT::addToCellValue(const Index3D& index, + FloatingPoint update) { const BlockIndex block_index = convert::indexToBlockIndex(index, config_.tree_height); auto& block = getOrAllocateBlock(block_index); @@ -53,59 +149,100 @@ inline void HashedChunkedWaveletOctree::addToCellValue(const Index3D& index, block.addToCellValue(cell_index, update); } -inline bool HashedChunkedWaveletOctree::hasBlock( +template +void HashedChunkedWaveletOctreeT::addToVoxelValue( + const Index3D& index, const CellDataT& update) { + const BlockIndex block_index = + convert::indexToBlockIndex(index, config_.tree_height); + auto& block = getOrAllocateBlock(block_index); + const CellIndex cell_index = indexToCellIndex({0, index}); + block.addToVoxelValue(cell_index, update); +} + +template +bool HashedChunkedWaveletOctreeT::hasBlock( const Index3D& block_index) const { return block_map_.hasBlock(block_index); } -inline bool HashedChunkedWaveletOctree::eraseBlock( - const HashedChunkedWaveletOctree::BlockIndex& block_index) { +template +bool HashedChunkedWaveletOctreeT::eraseBlock( + const typename HashedChunkedWaveletOctreeT::BlockIndex& block_index) { return block_map_.eraseBlock(block_index); } +template template -void HashedChunkedWaveletOctree::eraseBlockIf( +void HashedChunkedWaveletOctreeT::eraseBlockIf( IndexedBlockVisitor indicator_fn) { block_map_.eraseBlockIf(indicator_fn); } -inline HashedChunkedWaveletOctree::Block* HashedChunkedWaveletOctree::getBlock( - const Index3D& block_index) { +template +typename HashedChunkedWaveletOctreeT::Block* +HashedChunkedWaveletOctreeT::getBlock(const Index3D& block_index) { return block_map_.getBlock(block_index); } -inline const HashedChunkedWaveletOctree::Block* -HashedChunkedWaveletOctree::getBlock(const Index3D& block_index) const { +template +const typename HashedChunkedWaveletOctreeT::Block* +HashedChunkedWaveletOctreeT::getBlock(const Index3D& block_index) const { return block_map_.getBlock(block_index); } -inline HashedChunkedWaveletOctree::Block& -HashedChunkedWaveletOctree::getOrAllocateBlock(const Index3D& block_index) { - return block_map_.getOrAllocateBlock(block_index, config_.tree_height, - config_.min_log_odds, - config_.max_log_odds); +template +typename HashedChunkedWaveletOctreeT::Block& +HashedChunkedWaveletOctreeT::getOrAllocateBlock( + const Index3D& block_index) { + return block_map_.getOrAllocateBlock( + block_index, config_.tree_height, config_.min_log_odds, + config_.max_log_odds, threshold_config_, pruning_config_); } +template template -void HashedChunkedWaveletOctree::forEachBlock(IndexedBlockVisitor visitor_fn) { +void HashedChunkedWaveletOctreeT::forEachBlock( + IndexedBlockVisitor visitor_fn) { block_map_.forEachBlock(visitor_fn); } +template template -void HashedChunkedWaveletOctree::forEachBlock( +void HashedChunkedWaveletOctreeT::forEachBlock( IndexedBlockVisitor visitor_fn) const { block_map_.forEachBlock(visitor_fn); } -inline HashedChunkedWaveletOctree::BlockIndex -HashedChunkedWaveletOctree::indexToBlockIndex( +template +void HashedChunkedWaveletOctreeT::forEachLeaf( + MapBase::IndexedLeafVisitorFunction visitor_fn) const { + forEachBlock( + [&visitor_fn](const BlockIndex& block_index, const Block& block) { + block.forEachLeaf(block_index, visitor_fn); + }); +} + +template +template +void HashedChunkedWaveletOctreeT::forEachVoxelLeaf( + IndexedVoxelLeafVisitorFunction visitor_fn) const { + forEachBlock( + [&visitor_fn](const BlockIndex& block_index, const Block& block) { + block.forEachVoxelLeaf(block_index, visitor_fn); + }); +} + +template +typename HashedChunkedWaveletOctreeT::BlockIndex +HashedChunkedWaveletOctreeT::indexToBlockIndex( const OctreeIndex& node_index) const { const Index3D index = convert::nodeIndexToMinCornerIndex(node_index); return convert::indexToBlockIndex(index, config_.tree_height); } -inline HashedChunkedWaveletOctree::CellIndex -HashedChunkedWaveletOctree::indexToCellIndex(OctreeIndex index) const { +template +typename HashedChunkedWaveletOctreeT::CellIndex +HashedChunkedWaveletOctreeT::indexToCellIndex(OctreeIndex index) const { DCHECK_LE(index.height, config_.tree_height); const IndexElement height_difference = config_.tree_height - index.height; index.position = diff --git a/library/cpp/include/wavemap/io/impl/stream_conversions_inl.h b/library/cpp/include/wavemap/io/impl/stream_conversions_inl.h index 718c11e31..b72ba9b63 100644 --- a/library/cpp/include/wavemap/io/impl/stream_conversions_inl.h +++ b/library/cpp/include/wavemap/io/impl/stream_conversions_inl.h @@ -148,6 +148,106 @@ bool streamToMap(std::istream& istream, return istream.good(); } + +template +bool mapToStream(const HashedChunkedWaveletOctreeT& map, + std::ostream& ostream) { + if (!ostream.good()) { + return false; + } + + using Block = typename HashedChunkedWaveletOctreeT::Block; + using NodeConstRefType = typename Block::OctreeType::NodeConstRefType; + struct StackElement { + const CellDataT scale; + NodeConstRefType node; + }; + + constexpr FloatingPoint kNumericalNoise = 1e-3f; + const auto min_log_odds = map.getMinLogOdds() + kNumericalNoise; + const auto max_log_odds = map.getMaxLogOdds() - kNumericalNoise; + + streamable::StorageFormat storage_format = + streamable::StorageFormat::kLayeredHashedWaveletOctree; + storage_format.write(ostream); + + streamable::HashedWaveletOctreeHeader header; + header.min_cell_width = map.getMinCellWidth(); + header.min_log_odds = map.getMinLogOdds(); + header.max_log_odds = map.getMaxLogOdds(); + header.tree_height = map.getTreeHeight(); + header.num_blocks = map.getHashMap().size(); + header.write(ostream); + + map.forEachBlock([&ostream, min_log_odds, max_log_odds]( + const Index3D& block_index, const auto& block) { + if (!ostream.good()) { + return; + } + + streamable::Index3D streamable_block_index{ + block_index.x(), block_index.y(), block_index.z()}; + streamable_block_index.write(ostream); + CellDataSerializerT::write(ostream, block.getRootScale()); + + std::stack stack; + stack.emplace(StackElement{block.getRootScale(), block.getRootNode()}); + while (!stack.empty()) { + const CellDataT scale = stack.top().scale; + const auto node = stack.top().node; + stack.pop(); + + streamable::UInt8 allocated_children_bitset{}; + for (const CellDataT& coefficient : node.data()) { + CellDataSerializerT::write(ostream, coefficient); + } + + const auto child_scales = Block::Transform::backward({scale, node.data()}); + for (int relative_child_idx = OctreeIndex::kNumChildren - 1; + 0 <= relative_child_idx; --relative_child_idx) { + const auto child_scale = child_scales[relative_child_idx]; + const FloatingPoint child_occupancy = + static_cast(child_scale); + if (child_occupancy < min_log_odds || max_log_odds < child_occupancy) { + continue; + } + if (auto child = node.getChild(relative_child_idx); child) { + stack.emplace(StackElement{child_scale, *child}); + allocated_children_bitset += (1 << relative_child_idx); + } + } + + ostream.write(reinterpret_cast(&allocated_children_bitset), + sizeof(allocated_children_bitset)); + } + }); + return ostream.good(); +} + +template +bool streamToMap( + std::istream& istream, + typename HashedChunkedWaveletOctreeT::Ptr& map) { + typename HashedWaveletOctreeT::Ptr logical_map; + if (!streamToMap(istream, logical_map) || + !logical_map) { + return false; + } + + HashedChunkedWaveletOctreeConfig config; + config.min_cell_width = logical_map->getMinCellWidth(); + config.min_log_odds = logical_map->getMinLogOdds(); + config.max_log_odds = logical_map->getMaxLogOdds(); + config.tree_height = logical_map->getTreeHeight(); + map = std::make_shared>(config); + logical_map->forEachVoxelLeaf( + [&map](const OctreeIndex& node_index, const CellDataT& value) { + const auto block_index = map->indexToBlockIndex(node_index); + auto& block = map->getOrAllocateBlock(block_index); + block.setVoxelValue(map->indexToCellIndex(node_index), value); + }); + return true; +} } // namespace wavemap::io #endif // WAVEMAP_IO_IMPL_STREAM_CONVERSIONS_INL_H_ diff --git a/library/cpp/include/wavemap/io/stream_conversions.h b/library/cpp/include/wavemap/io/stream_conversions.h index 75789ab38..cbafd4b29 100644 --- a/library/cpp/include/wavemap/io/stream_conversions.h +++ b/library/cpp/include/wavemap/io/stream_conversions.h @@ -46,6 +46,15 @@ template bool streamToMap(std::istream& istream, typename HashedWaveletOctreeT::Ptr& map); +template +bool mapToStream(const HashedChunkedWaveletOctreeT& map, + std::ostream& ostream); + +template +bool streamToMap( + std::istream& istream, + typename HashedChunkedWaveletOctreeT::Ptr& map); + bool mapToStream(const HashedBlocks& map, std::ostream& ostream); bool streamToMap(std::istream& istream, HashedBlocks::Ptr& map); diff --git a/library/cpp/include/wavemap/layered/local_elevation_classifier.h b/library/cpp/include/wavemap/layered/classification/local_elevation_classifier.h similarity index 97% rename from library/cpp/include/wavemap/layered/local_elevation_classifier.h rename to library/cpp/include/wavemap/layered/classification/local_elevation_classifier.h index 4c94aa974..1b65f7d43 100644 --- a/library/cpp/include/wavemap/layered/local_elevation_classifier.h +++ b/library/cpp/include/wavemap/layered/classification/local_elevation_classifier.h @@ -11,8 +11,8 @@ #include #include -#include -#include +#include +#include namespace wavemap::layered { diff --git a/library/cpp/include/wavemap/layered/continuous_layer_updater.h b/library/cpp/include/wavemap/layered/integration/continuous_layer_updater.h similarity index 100% rename from library/cpp/include/wavemap/layered/continuous_layer_updater.h rename to library/cpp/include/wavemap/layered/integration/continuous_layer_updater.h diff --git a/library/cpp/include/wavemap/layered/endpoint_attribute_integrator.h b/library/cpp/include/wavemap/layered/integration/endpoint_attribute_integrator.h similarity index 91% rename from library/cpp/include/wavemap/layered/endpoint_attribute_integrator.h rename to library/cpp/include/wavemap/layered/integration/endpoint_attribute_integrator.h index c7b423170..5e1dbcf6b 100644 --- a/library/cpp/include/wavemap/layered/endpoint_attribute_integrator.h +++ b/library/cpp/include/wavemap/layered/integration/endpoint_attribute_integrator.h @@ -6,11 +6,11 @@ #include #include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include namespace wavemap::layered { diff --git a/library/cpp/include/wavemap/layered/endpoint_attribute_pointcloud.h b/library/cpp/include/wavemap/layered/integration/endpoint_attribute_pointcloud.h similarity index 98% rename from library/cpp/include/wavemap/layered/endpoint_attribute_pointcloud.h rename to library/cpp/include/wavemap/layered/integration/endpoint_attribute_pointcloud.h index 0cef3af84..b65a7e00f 100644 --- a/library/cpp/include/wavemap/layered/endpoint_attribute_pointcloud.h +++ b/library/cpp/include/wavemap/layered/integration/endpoint_attribute_pointcloud.h @@ -6,7 +6,7 @@ #include #include -#include +#include namespace wavemap::layered { namespace detail { diff --git a/library/cpp/include/wavemap/layered/endpoint_range.h b/library/cpp/include/wavemap/layered/integration/endpoint_range.h similarity index 100% rename from library/cpp/include/wavemap/layered/endpoint_range.h rename to library/cpp/include/wavemap/layered/integration/endpoint_range.h diff --git a/library/cpp/include/wavemap/layered/layer_integrator.h b/library/cpp/include/wavemap/layered/integration/layer_integrator.h similarity index 94% rename from library/cpp/include/wavemap/layered/layer_integrator.h rename to library/cpp/include/wavemap/layered/integration/layer_integrator.h index 33fb036a9..6bf5e5f26 100644 --- a/library/cpp/include/wavemap/layered/layer_integrator.h +++ b/library/cpp/include/wavemap/layered/integration/layer_integrator.h @@ -9,11 +9,11 @@ #include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include namespace wavemap::layered { diff --git a/library/cpp/include/wavemap/layered/layer_observation.h b/library/cpp/include/wavemap/layered/integration/layer_observation.h similarity index 100% rename from library/cpp/include/wavemap/layered/layer_observation.h rename to library/cpp/include/wavemap/layered/integration/layer_observation.h diff --git a/library/cpp/include/wavemap/layered/layer_observation_batch.h b/library/cpp/include/wavemap/layered/integration/layer_observation_batch.h similarity index 96% rename from library/cpp/include/wavemap/layered/layer_observation_batch.h rename to library/cpp/include/wavemap/layered/integration/layer_observation_batch.h index f3b80cc73..df598a0d4 100644 --- a/library/cpp/include/wavemap/layered/layer_observation_batch.h +++ b/library/cpp/include/wavemap/layered/integration/layer_observation_batch.h @@ -6,8 +6,8 @@ #include #include -#include -#include +#include +#include namespace wavemap::layered { namespace detail { diff --git a/library/cpp/include/wavemap/layered/layer_update_policy.h b/library/cpp/include/wavemap/layered/integration/layer_update_policy.h similarity index 99% rename from library/cpp/include/wavemap/layered/layer_update_policy.h rename to library/cpp/include/wavemap/layered/integration/layer_update_policy.h index ca51b1f50..68e334cf8 100644 --- a/library/cpp/include/wavemap/layered/layer_update_policy.h +++ b/library/cpp/include/wavemap/layered/integration/layer_update_policy.h @@ -8,7 +8,7 @@ #include -#include +#include #include namespace wavemap::layered { diff --git a/library/cpp/include/wavemap/layered/layered_pipeline.h b/library/cpp/include/wavemap/layered/integration/layered_pipeline.h similarity index 93% rename from library/cpp/include/wavemap/layered/layered_pipeline.h rename to library/cpp/include/wavemap/layered/integration/layered_pipeline.h index 5770bdbad..a66ac487a 100644 --- a/library/cpp/include/wavemap/layered/layered_pipeline.h +++ b/library/cpp/include/wavemap/layered/integration/layered_pipeline.h @@ -3,8 +3,8 @@ #include -#include -#include +#include +#include namespace wavemap::layered { diff --git a/library/cpp/include/wavemap/layered/pointcloud_layer_integration_mode.h b/library/cpp/include/wavemap/layered/integration/pointcloud_layer_integration_mode.h similarity index 100% rename from library/cpp/include/wavemap/layered/pointcloud_layer_integration_mode.h rename to library/cpp/include/wavemap/layered/integration/pointcloud_layer_integration_mode.h diff --git a/library/cpp/include/wavemap/layered/descriptor_layered_map_io.h b/library/cpp/include/wavemap/layered/io/descriptor_layered_map_io.h similarity index 93% rename from library/cpp/include/wavemap/layered/descriptor_layered_map_io.h rename to library/cpp/include/wavemap/layered/io/descriptor_layered_map_io.h index 12ab431d8..b2d469b03 100644 --- a/library/cpp/include/wavemap/layered/descriptor_layered_map_io.h +++ b/library/cpp/include/wavemap/layered/io/descriptor_layered_map_io.h @@ -5,11 +5,11 @@ #include #include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include namespace wavemap::layered { diff --git a/library/cpp/include/wavemap/layered/layer_stream_codec.h b/library/cpp/include/wavemap/layered/io/layer_stream_codec.h similarity index 100% rename from library/cpp/include/wavemap/layered/layer_stream_codec.h rename to library/cpp/include/wavemap/layered/io/layer_stream_codec.h diff --git a/library/cpp/include/wavemap/layered/layered_map_io.h b/library/cpp/include/wavemap/layered/io/layered_map_io.h similarity index 99% rename from library/cpp/include/wavemap/layered/layered_map_io.h rename to library/cpp/include/wavemap/layered/io/layered_map_io.h index acd733dca..59bc22e77 100644 --- a/library/cpp/include/wavemap/layered/layered_map_io.h +++ b/library/cpp/include/wavemap/layered/io/layered_map_io.h @@ -14,9 +14,9 @@ #include -#include -#include -#include +#include +#include +#include namespace wavemap::layered::io { namespace detail { diff --git a/library/cpp/include/wavemap/layered/layered_map_schema.h b/library/cpp/include/wavemap/layered/io/layered_map_schema.h similarity index 100% rename from library/cpp/include/wavemap/layered/layered_map_schema.h rename to library/cpp/include/wavemap/layered/io/layered_map_schema.h diff --git a/library/cpp/include/wavemap/layered/map/continuous_map_backend.h b/library/cpp/include/wavemap/layered/map/continuous_map_backend.h new file mode 100644 index 000000000..0c8f40db0 --- /dev/null +++ b/library/cpp/include/wavemap/layered/map/continuous_map_backend.h @@ -0,0 +1,21 @@ +#ifndef WAVEMAP_LAYERED_MAP_CONTINUOUS_MAP_BACKEND_H_ +#define WAVEMAP_LAYERED_MAP_CONTINUOUS_MAP_BACKEND_H_ + +#include +#include + +namespace wavemap::layered { + +struct HashedWaveletOctreeBackend { + template + using Map = wavemap::HashedWaveletOctreeT; +}; + +struct HashedChunkedWaveletOctreeBackend { + template + using Map = wavemap::HashedChunkedWaveletOctreeT; +}; + +} // namespace wavemap::layered + +#endif // WAVEMAP_LAYERED_MAP_CONTINUOUS_MAP_BACKEND_H_ diff --git a/library/cpp/include/wavemap/layered/discrete_layer.h b/library/cpp/include/wavemap/layered/map/discrete_layer.h similarity index 100% rename from library/cpp/include/wavemap/layered/discrete_layer.h rename to library/cpp/include/wavemap/layered/map/discrete_layer.h diff --git a/library/cpp/include/wavemap/layered/layered_map.h b/library/cpp/include/wavemap/layered/map/layered_map.h similarity index 79% rename from library/cpp/include/wavemap/layered/layered_map.h rename to library/cpp/include/wavemap/layered/map/layered_map.h index a89b40162..8c46ff7dd 100644 --- a/library/cpp/include/wavemap/layered/layered_map.h +++ b/library/cpp/include/wavemap/layered/map/layered_map.h @@ -5,9 +5,8 @@ #include #include -#include - -#include +#include +#include namespace wavemap::layered { @@ -15,28 +14,25 @@ template using LayeredMapVoxel = wavemap::VoxelData; -template -using ContinuousWaveletMapT = - wavemap::HashedWaveletOctreeT< - LayeredMapVoxel>; - template struct LayeredMapConfig { - wavemap::HashedWaveletOctreeConfig continuous_map; + typename ContinuousMapT::Config continuous_map; typename ContinuousMapT::ThresholdConfig continuous_threshold; typename ContinuousMapT::PruningConfig continuous_pruning; DiscreteCompressionConfig discrete_compression; }; template + typename DiscreteLayersT, + typename ContinuousMapBackendT = HashedWaveletOctreeBackend> class LayeredMap { public: using ContinuousLayers = ContinuousLayersT; using ContinuousPolicy = ContinuousPolicyT; using ContinuousVoxel = LayeredMapVoxel; - using ContinuousMap = ContinuousWaveletMapT; + using ContinuousMapBackend = ContinuousMapBackendT; + using ContinuousMap = typename ContinuousMapBackend::template Map< + ContinuousVoxel>; using Config = LayeredMapConfig; using DiscreteLayers = DiscreteLayersT; @@ -46,11 +42,11 @@ class LayeredMap { config.continuous_pruning)), discrete_layers_(config.discrete_compression) {} - explicit LayeredMap(const wavemap::HashedWaveletOctreeConfig& config) + explicit LayeredMap(const typename ContinuousMap::Config& config) : continuous_map_(std::make_shared(config)), discrete_layers_() {} - LayeredMap(const wavemap::HashedWaveletOctreeConfig& config, + LayeredMap(const typename ContinuousMap::Config& config, const typename ContinuousMap::ThresholdConfig& threshold_config, const typename ContinuousMap::PruningConfig& pruning_config) : continuous_map_(std::make_shared( diff --git a/library/cpp/include/wavemap/layered/layered_map_config_builder.h b/library/cpp/include/wavemap/layered/map/layered_map_config_builder.h similarity index 98% rename from library/cpp/include/wavemap/layered/layered_map_config_builder.h rename to library/cpp/include/wavemap/layered/map/layered_map_config_builder.h index 7381d5217..de19779d6 100644 --- a/library/cpp/include/wavemap/layered/layered_map_config_builder.h +++ b/library/cpp/include/wavemap/layered/map/layered_map_config_builder.h @@ -4,7 +4,7 @@ #include #include -#include +#include namespace wavemap::layered { diff --git a/library/cpp/include/wavemap/layered/layered_map_definition.h b/library/cpp/include/wavemap/layered/map/layered_map_definition.h similarity index 82% rename from library/cpp/include/wavemap/layered/layered_map_definition.h rename to library/cpp/include/wavemap/layered/map/layered_map_definition.h index 3a26fa9fa..b390da1fa 100644 --- a/library/cpp/include/wavemap/layered/layered_map_definition.h +++ b/library/cpp/include/wavemap/layered/map/layered_map_definition.h @@ -1,10 +1,10 @@ #ifndef WAVEMAP_LAYERED_LAYERED_MAP_DEFINITION_H_ #define WAVEMAP_LAYERED_LAYERED_MAP_DEFINITION_H_ -#include -#include -#include -#include +#include +#include +#include +#include namespace wavemap::layered { namespace detail { @@ -40,9 +40,11 @@ constexpr bool continuousLayersHaveStreamCodecs( // Derives the complete core map representation from a user layer schema. // Occupancy remains built into LayeredMapVoxel and is therefore intentionally // not listed in SchemaT. -template +template struct LayeredMapDefinition { using Schema = SchemaT; + using ContinuousMapBackend = ContinuousMapBackendT; using LayerTags = typename Schema::Layers; static_assert( @@ -60,8 +62,8 @@ struct LayeredMapDefinition { using ContinuousArithmetic = schema::ContinuousBundleArithmetic; using DiscreteLayers = schema::DiscreteLayerBundle; - using Map = - LayeredMap; + using Map = LayeredMap; using Config = typename Map::Config; using Voxel = typename Map::ContinuousVoxel; using ContinuousMap = typename Map::ContinuousMap; diff --git a/library/cpp/include/wavemap/layered/layer_bundle_traits.h b/library/cpp/include/wavemap/layered/schema/layer_bundle_traits.h similarity index 100% rename from library/cpp/include/wavemap/layered/layer_bundle_traits.h rename to library/cpp/include/wavemap/layered/schema/layer_bundle_traits.h diff --git a/library/cpp/include/wavemap/layered/layer_descriptor.h b/library/cpp/include/wavemap/layered/schema/layer_descriptor.h similarity index 98% rename from library/cpp/include/wavemap/layered/layer_descriptor.h rename to library/cpp/include/wavemap/layered/schema/layer_descriptor.h index 02136d52d..77ebef5ae 100644 --- a/library/cpp/include/wavemap/layered/layer_descriptor.h +++ b/library/cpp/include/wavemap/layered/schema/layer_descriptor.h @@ -5,7 +5,7 @@ #include #include -#include +#include namespace wavemap::layered { namespace detail { diff --git a/library/cpp/include/wavemap/layered/layer_schema.h b/library/cpp/include/wavemap/layered/schema/layer_schema.h similarity index 97% rename from library/cpp/include/wavemap/layered/layer_schema.h rename to library/cpp/include/wavemap/layered/schema/layer_schema.h index 24cb463ec..eb6956edc 100644 --- a/library/cpp/include/wavemap/layered/layer_schema.h +++ b/library/cpp/include/wavemap/layered/schema/layer_schema.h @@ -7,8 +7,8 @@ #include #include -#include -#include +#include +#include namespace wavemap::layered::schema { diff --git a/library/cpp/include/wavemap/layered/layer_traits.h b/library/cpp/include/wavemap/layered/schema/layer_traits.h similarity index 100% rename from library/cpp/include/wavemap/layered/layer_traits.h rename to library/cpp/include/wavemap/layered/schema/layer_traits.h diff --git a/library/cpp/include/wavemap/layered/schema_continuous_traits.h b/library/cpp/include/wavemap/layered/schema/schema_continuous_traits.h similarity index 99% rename from library/cpp/include/wavemap/layered/schema_continuous_traits.h rename to library/cpp/include/wavemap/layered/schema/schema_continuous_traits.h index 8e1178420..eb12f1ca4 100644 --- a/library/cpp/include/wavemap/layered/schema_continuous_traits.h +++ b/library/cpp/include/wavemap/layered/schema/schema_continuous_traits.h @@ -6,7 +6,7 @@ #include #include -#include +#include #include namespace wavemap::layered::schema { diff --git a/library/cpp/include/wavemap/layered/schema_layer_storage.h b/library/cpp/include/wavemap/layered/schema/schema_layer_storage.h similarity index 97% rename from library/cpp/include/wavemap/layered/schema_layer_storage.h rename to library/cpp/include/wavemap/layered/schema/schema_layer_storage.h index 74b7a8d2b..9bea2f32f 100644 --- a/library/cpp/include/wavemap/layered/schema_layer_storage.h +++ b/library/cpp/include/wavemap/layered/schema/schema_layer_storage.h @@ -5,9 +5,9 @@ #include #include -#include -#include -#include +#include +#include +#include namespace wavemap::layered::schema { namespace detail { diff --git a/library/cpp/src/core/integrator/projective/coarse_to_fine/hashed_chunked_wavelet_integrator.cc b/library/cpp/src/core/integrator/projective/coarse_to_fine/hashed_chunked_wavelet_integrator.cc index cc9709acc..293b185f0 100644 --- a/library/cpp/src/core/integrator/projective/coarse_to_fine/hashed_chunked_wavelet_integrator.cc +++ b/library/cpp/src/core/integrator/projective/coarse_to_fine/hashed_chunked_wavelet_integrator.cc @@ -1,163 +1 @@ #include "wavemap/core/integrator/projective/coarse_to_fine/hashed_chunked_wavelet_integrator.h" - -#include -#include -#include -#include - -#include - -namespace wavemap { -void HashedChunkedWaveletIntegrator::updateMap() { - ProfilerZoneScoped; - // Update the range image intersector - { - ProfilerZoneScopedN("updateRangeImageIntersector"); - range_image_intersector_ = std::make_shared( - posed_range_image_, projection_model_, *measurement_model_, - config_.min_range, config_.max_range); - } - - // Find all the indices of blocks that need updating - BlockList blocks_to_update; - { - ProfilerZoneScopedN("selectBlocksToUpdate"); - const auto [fov_min_idx, fov_max_idx] = - getFovMinMaxIndices(posed_range_image_->getOrigin()); - for (const auto& block_index : - Grid(fov_min_idx.position, fov_max_idx.position)) { - recursiveTester(OctreeIndex{fov_min_idx.height, block_index}, - blocks_to_update); - } - } - - // Make sure the to-be-updated blocks are allocated - for (const auto& block_index : blocks_to_update) { - occupancy_map_->getOrAllocateBlock(block_index); - } - - // Update it with the threadpool - for (const auto& block_index : blocks_to_update) { - thread_pool_->add_task([this, block_index]() { - if (auto* block = occupancy_map_->getBlock(block_index); block) { - updateBlock(*block, block_index); - } - }); - } - thread_pool_->wait_all(); -} - -std::pair -HashedChunkedWaveletIntegrator::getFovMinMaxIndices( - const Point3D& sensor_origin) const { - const int height = 1 + std::max(static_cast(std::ceil(std::log2( - config_.max_range / min_cell_width_))), - tree_height_); - const OctreeIndex fov_min_idx = convert::indexAndHeightToNodeIndex<3>( - convert::pointToFloorIndex<3>( - sensor_origin - Vector3D::Constant(config_.max_range), - min_cell_width_inv_) - - occupancy_map_->getBlockSize(), - height); - const OctreeIndex fov_max_idx = convert::indexAndHeightToNodeIndex<3>( - convert::pointToCeilIndex<3>( - sensor_origin + Vector3D::Constant(config_.max_range), - min_cell_width_inv_) + - occupancy_map_->getBlockSize(), - height); - return {fov_min_idx, fov_max_idx}; -} - -void HashedChunkedWaveletIntegrator::updateBlock( - HashedChunkedWaveletOctree::Block& block, - const HashedChunkedWaveletOctree::BlockIndex& block_index) { - ProfilerZoneScoped; - block.setNeedsPruning(); - block.setLastUpdatedStamp(); - - bool block_needs_thresholding = block.getNeedsThresholding(); - const OctreeIndex root_node_index{tree_height_, block_index}; - updateNodeRecursive(block.getRootNode(), root_node_index, - block.getRootScale(), block_needs_thresholding); - block.setNeedsThresholding(block_needs_thresholding); -} - -void HashedChunkedWaveletIntegrator::updateNodeRecursive( // NOLINT - HashedChunkedWaveletIntegrator::OctreeType::NodeRefType node, - const OctreeIndex& node_index, FloatingPoint& node_value, - bool& block_needs_thresholding) { - // Decompress child values - auto& node_details = node.data(); - auto child_values = HashedChunkedWaveletOctreeBlock::Transform::backward( - {node_value, node_details}); - - // Handle each child - for (NdtreeIndexRelativeChild relative_child_idx = 0; - relative_child_idx < OctreeIndex::kNumChildren; ++relative_child_idx) { - const OctreeIndex child_index = - node_index.computeChildIndex(relative_child_idx); - FloatingPoint& child_value = child_values[relative_child_idx]; - - // Test whether it is fully occupied; free or unknown; or fully unknown - const AABB W_child_aabb = - convert::nodeIndexToAABB(child_index, min_cell_width_); - const UpdateType update_type = - range_image_intersector_->determineUpdateType( - W_child_aabb, posed_range_image_->getRotationMatrixInverse(), - posed_range_image_->getOrigin()); - - // If we're fully in unknown space, - // there's no need to evaluate this node or its children - if (update_type == UpdateType::kFullyUnobserved) { - continue; - } - - // We can also stop here if the cell will result in a free space update - // (or zero) and the map is already saturated free - if (update_type != UpdateType::kPossiblyOccupied && - child_value < min_log_odds_shrunk_) { - continue; - } - - // Test if the worst-case error for the intersection type at the current - // resolution falls within the acceptable approximation error - const FloatingPoint child_width = W_child_aabb.width<0>(); - const Point3D W_child_center = - W_child_aabb.min + Vector3D::Constant(child_width / 2.f); - const Point3D C_child_center = - posed_range_image_->getPoseInverse() * W_child_center; - const FloatingPoint d_C_child = - projection_model_->cartesianToSensorZ(C_child_center); - const FloatingPoint bounding_sphere_radius = - kUnitCubeHalfDiagonal * child_width; - if (measurement_model_->computeWorstCaseApproximationError( - update_type, d_C_child, bounding_sphere_radius) < - config_.termination_update_error) { - const FloatingPoint sample = computeUpdate(C_child_center); - child_value += sample; - block_needs_thresholding = true; - continue; - } - - // Since the approximation error would still be too big, refine - auto child_node = node.getOrAllocateChild(relative_child_idx); - auto& child_details = child_node.data(); - - // If we're at the leaf level, directly compute the update - if (child_index.height <= termination_height_ + 1) { - updateLeavesBatch(child_index, child_value, child_details); - } else { - // Otherwise, recurse - DCHECK_GE(child_index.height, 0); - updateNodeRecursive(child_node, child_index, child_value, - block_needs_thresholding); - } - } - - // Compress - const auto [new_value, new_details] = - HashedChunkedWaveletOctreeBlock::Transform::forward(child_values); - node_details = new_details; - node_value = new_value; -} -} // namespace wavemap diff --git a/library/cpp/src/core/map/hashed_chunked_wavelet_octree.cc b/library/cpp/src/core/map/hashed_chunked_wavelet_octree.cc index 702a0eba6..4992dabbf 100644 --- a/library/cpp/src/core/map/hashed_chunked_wavelet_octree.cc +++ b/library/cpp/src/core/map/hashed_chunked_wavelet_octree.cc @@ -23,61 +23,4 @@ bool HashedChunkedWaveletOctreeConfig::isValid(bool verbose) const { return is_valid; } -void HashedChunkedWaveletOctree::threshold() { - ProfilerZoneScoped; - forEachBlock([](const BlockIndex& /*block_index*/, Block& block) { - block.threshold(); - }); -} - -void HashedChunkedWaveletOctree::prune() { - ProfilerZoneScoped; - block_map_.eraseBlockIf([](const BlockIndex& /*block_index*/, Block& block) { - block.prune(); - return block.empty(); - }); -} - -void HashedChunkedWaveletOctree::pruneSmart() { - ProfilerZoneScoped; - block_map_.eraseBlockIf( - [&config = config_](const BlockIndex& /*block_index*/, Block& block) { - if (config.only_prune_blocks_if_unused_for < - block.getTimeSinceLastUpdated()) { - block.prune(); - } - return block.empty(); - }); -} - -size_t HashedChunkedWaveletOctree::getMemoryUsage() const { - ProfilerZoneScoped; - // TODO(victorr): Also include the memory usage of the unordered map itself - size_t memory_usage = 0u; - forEachBlock( - [&memory_usage](const BlockIndex& /*block_index*/, const Block& block) { - memory_usage += block.getMemoryUsage(); - }); - return memory_usage; -} - -Index3D HashedChunkedWaveletOctree::getMinIndex() const { - return cells_per_block_side_ * getMinBlockIndex(); -} - -Index3D HashedChunkedWaveletOctree::getMaxIndex() const { - if (empty()) { - return Index3D::Zero(); - } - return cells_per_block_side_ * (getMaxBlockIndex().array() + 1) - 1; -} - -void HashedChunkedWaveletOctree::forEachLeaf( - MapBase::IndexedLeafVisitorFunction visitor_fn) const { - forEachBlock( - [&visitor_fn](const BlockIndex& block_index, const Block& block) { - block.forEachLeaf(block_index, visitor_fn); - }); -} - } // namespace wavemap diff --git a/library/cpp/src/core/map/hashed_chunked_wavelet_octree_block.cc b/library/cpp/src/core/map/hashed_chunked_wavelet_octree_block.cc index b5600653a..d1d468160 100644 --- a/library/cpp/src/core/map/hashed_chunked_wavelet_octree_block.cc +++ b/library/cpp/src/core/map/hashed_chunked_wavelet_octree_block.cc @@ -1,194 +1 @@ #include "wavemap/core/map/hashed_chunked_wavelet_octree_block.h" - -#include -#include -#include - -#include - -namespace wavemap { -void HashedChunkedWaveletOctreeBlock::threshold() { - ProfilerZoneScoped; - if (getNeedsThresholding()) { - recursiveThreshold(ndtree_.getRootNode(), root_scale_coefficient_); - setNeedsThresholding(false); - } -} - -void HashedChunkedWaveletOctreeBlock::prune() { - ProfilerZoneScoped; - if (getNeedsPruning()) { - threshold(); - recursivePrune(ndtree_.getRootNode()); - setNeedsPruning(false); - } -} - -void HashedChunkedWaveletOctreeBlock::clear() { - ProfilerZoneScoped; - root_scale_coefficient_ = Coefficients::Scale{}; - ndtree_.clear(); - setLastUpdatedStamp(); -} - -void HashedChunkedWaveletOctreeBlock::setCellValue(const OctreeIndex& index, - FloatingPoint new_value) { - setNeedsPruning(); - setNeedsThresholding(); - setLastUpdatedStamp(); - const MortonIndex morton_code = convert::nodeIndexToMorton(index); - std::vector ancestors; - const int height_difference = tree_height_ - index.height; - ancestors.reserve(height_difference); - ancestors.emplace_back(ndtree_.getRootNode()); - FloatingPoint current_value = root_scale_coefficient_; - for (int parent_height = tree_height_; index.height + 1 < parent_height; - --parent_height) { - const NdtreeIndexRelativeChild child_index = - OctreeIndex::computeRelativeChildIndex(morton_code, parent_height); - OctreeType::NodeRefType current_parent = ancestors.back(); - current_value = Transform::backwardSingleChild( - {current_value, current_parent.data()}, child_index); - OctreeType::NodeRefType child = - current_parent.getOrAllocateChild(child_index); - ancestors.emplace_back(child); - } - DCHECK_EQ(ancestors.size(), height_difference); - - Coefficients::Parent coefficients{new_value - current_value, {}}; - for (int parent_height = index.height + 1; parent_height <= tree_height_; - ++parent_height) { - const NdtreeIndexRelativeChild child_index = - OctreeIndex::computeRelativeChildIndex(morton_code, parent_height); - OctreeType::NodeRefType current_node = ancestors.back(); - ancestors.pop_back(); - coefficients = - Transform::forwardSingleChild(coefficients.scale, child_index); - current_node.data() += coefficients.details; - } - - root_scale_coefficient_ += coefficients.scale; -} - -void HashedChunkedWaveletOctreeBlock::addToCellValue(const OctreeIndex& index, - FloatingPoint update) { - setNeedsPruning(); - setNeedsThresholding(); - setLastUpdatedStamp(); - const MortonIndex morton_code = convert::nodeIndexToMorton(index); - - std::vector ancestors; - const int height_difference = tree_height_ - index.height; - ancestors.reserve(height_difference); - ancestors.emplace_back(ndtree_.getRootNode()); - for (int parent_height = tree_height_; index.height + 1 < parent_height; - --parent_height) { - const NdtreeIndexRelativeChild child_index = - OctreeIndex::computeRelativeChildIndex(morton_code, parent_height); - OctreeType::NodeRefType current_parent = ancestors.back(); - OctreeType::NodeRefType child = - current_parent.getOrAllocateChild(child_index); - ancestors.emplace_back(child); - } - DCHECK_EQ(ancestors.size(), height_difference); - - Coefficients::Parent coefficients{update, {}}; - for (int parent_height = index.height + 1; parent_height <= tree_height_; - ++parent_height) { - OctreeType::NodeRefType current_node = ancestors.back(); - ancestors.pop_back(); - const NdtreeIndexRelativeChild child_index = - OctreeIndex::computeRelativeChildIndex(morton_code, parent_height); - coefficients = - Transform::forwardSingleChild(coefficients.scale, child_index); - current_node.data() += coefficients.details; - } - root_scale_coefficient_ += coefficients.scale; -} - -void HashedChunkedWaveletOctreeBlock::forEachLeaf( - const BlockIndex& block_index, - MapBase::IndexedLeafVisitorFunction visitor_fn, - IndexElement termination_height) const { - ProfilerZoneScoped; - if (empty()) { - return; - } - - struct StackElement { - const OctreeIndex node_index; - OctreeType::NodeConstRefType node; - const Coefficients::Scale scale_coefficient{}; - }; - std::stack stack; - stack.emplace(StackElement{{tree_height_, block_index}, - ndtree_.getRootNode(), - root_scale_coefficient_}); - while (!stack.empty()) { - const OctreeIndex index = stack.top().node_index; - const auto node = stack.top().node; - const FloatingPoint scale_coefficient = stack.top().scale_coefficient; - stack.pop(); - - const Coefficients::CoefficientsArray child_scale_coefficients = - Transform::backward({scale_coefficient, {node.data()}}); - for (NdtreeIndexRelativeChild child_idx = 0; - child_idx < OctreeIndex::kNumChildren; ++child_idx) { - const OctreeIndex child_node_index = index.computeChildIndex(child_idx); - const FloatingPoint child_scale_coefficient = - child_scale_coefficients[child_idx]; - if (auto child_node = node.getChild(child_idx); - child_node && termination_height < child_node_index.height) { - stack.emplace(StackElement{child_node_index, *child_node, - child_scale_coefficient}); - } else { - visitor_fn(child_node_index, child_scale_coefficient); - } - } - } -} - -void HashedChunkedWaveletOctreeBlock::recursiveThreshold( // NOLINT - OctreeType::NodeRefType node, Coefficients::Scale& node_scale_coefficient) { - // Decompress child values - auto& node_detail_coefficients = node.data(); - Coefficients::CoefficientsArray child_scale_coefficients = - Transform::backward({node_scale_coefficient, node_detail_coefficients}); - - // Handle each child - for (NdtreeIndexRelativeChild child_idx = 0; - child_idx < OctreeIndex::kNumChildren; ++child_idx) { - Coefficients::Scale& child_scale = child_scale_coefficients[child_idx]; - if (auto child_node = node.getChild(child_idx); child_node) { - recursiveThreshold(*child_node, child_scale); - } else { - child_scale = std::clamp(child_scale, min_log_odds_, max_log_odds_); - } - } - - // Compress - const auto [new_scale, new_details] = - Transform::forward(child_scale_coefficients); - node_detail_coefficients = new_details; - node_scale_coefficient = new_scale; -} - -void HashedChunkedWaveletOctreeBlock::recursivePrune( // NOLINT - HashedChunkedWaveletOctreeBlock::OctreeType::NodeRefType node) { - bool has_at_least_one_child = false; - for (NdtreeIndexRelativeChild child_idx = 0; - child_idx < OctreeIndex::kNumChildren; ++child_idx) { - if (OctreeType::NodePtrType child_node = node.getChild(child_idx); - child_node) { - recursivePrune(*child_node); - if (!child_node->hasAtLeastOneChild() && - !child_node->hasNonzeroData(1e-3f)) { - node.eraseChild(child_idx); - } else { - has_at_least_one_child = true; - } - } - } - node.hasAtLeastOneChild() = has_at_least_one_child; -} -} // namespace wavemap diff --git a/library/cpp/test/src/core/integrator/test_pointcloud_integrators.cc b/library/cpp/test/src/core/integrator/test_pointcloud_integrators.cc index 7afd0e072..163012142 100644 --- a/library/cpp/test/src/core/integrator/test_pointcloud_integrators.cc +++ b/library/cpp/test/src/core/integrator/test_pointcloud_integrators.cc @@ -65,12 +65,17 @@ struct IntegratorDataStructurePair { using DataStructureType = DataStructureT; }; +using LayeredChunkedTestVoxel = VoxelData; + using IntegratorTypes = ::testing::Types< IntegratorDataStructurePair, IntegratorDataStructurePair, IntegratorDataStructurePair, IntegratorDataStructurePair>; + HashedChunkedWaveletOctree>, + IntegratorDataStructurePair< + HashedChunkedWaveletIntegratorT, + HashedChunkedWaveletOctreeT>>; TYPED_TEST_SUITE(PointcloudIntegratorTypedTest, IntegratorTypes, ); TYPED_TEST(PointcloudIntegratorTypedTest, diff --git a/library/cpp/test/src/core/map/test_layered_map_definition.cc b/library/cpp/test/src/core/map/test_layered_map_definition.cc index 56393c48c..188e96e8f 100644 --- a/library/cpp/test/src/core/map/test_layered_map_definition.cc +++ b/library/cpp/test/src/core/map/test_layered_map_definition.cc @@ -1,15 +1,16 @@ +#include #include #include #include #include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include namespace wavemap::layered { namespace { @@ -65,6 +66,9 @@ using DiscreteDefinition = using EmptyDefinition = LayeredMapDefinition>; using WeightedDefinition = LayeredMapDefinition>; +using ChunkedMixedDefinition = LayeredMapDefinition< + schema::LayerSchema, + HashedChunkedWaveletOctreeBackend>; static_assert(std::is_same_v); @@ -81,6 +85,9 @@ static_assert( std::is_same_v, WeightedMeanState>); static_assert(schema::kHasContinuousValueTraits); static_assert(kHasLayerStreamCodec); +static_assert(std::is_same_v< + ChunkedMixedDefinition::ContinuousMap, + HashedChunkedWaveletOctreeT>); template typename DefinitionT::Config makeConfig() { @@ -126,6 +133,55 @@ TEST(LayeredMapDefinition, GeneratesMixedContinuousAndDiscreteStorage) { EXPECT_EQ(*map.discreteLayers().get().getValue(index), 3); } +TEST(LayeredMapDefinition, SupportsChunkedContinuousBackend) { + ChunkedMixedDefinition::Map map(makeConfig()); + const Index3D index{-3, 5, 9}; + + ChunkedMixedDefinition::Voxel voxel; + voxel.occupancy = 0.6f; + voxel.data.get() = 0.35f; + map.continuousMap().setVoxelValue(index, voxel); + map.discreteLayers().get().setValue(index, 2); + map.continuousMap().threshold(); + map.continuousMap().prune(); + + const auto stored = map.continuousMap().getVoxelValue(index); + EXPECT_FLOAT_EQ(stored.occupancy, 0.6f); + EXPECT_FLOAT_EQ(stored.data.get(), 0.35f); + ASSERT_TRUE(map.discreteLayers().get().getValue(index)); + EXPECT_EQ(*map.discreteLayers().get().getValue(index), 2); +} + +TEST(LayeredMapDefinition, ChunkedBackendPersistenceRoundTrip) { + const auto config = makeConfig(); + ChunkedMixedDefinition::Map source(config); + ChunkedMixedDefinition::Map restored(config); + const Index3D index{-3, 5, 9}; + + ChunkedMixedDefinition::Voxel voxel; + voxel.occupancy = 0.6f; + voxel.data.get() = 0.35f; + source.continuousMap().setVoxelValue(index, voxel); + source.discreteLayers().get().setValue(index, 2); + + const auto file_path = + std::filesystem::temp_directory_path() / + "wavemap_chunked_layered_round_trip.wvmp"; + ASSERT_TRUE(ChunkedMixedDefinition::MapIo::save(file_path, source)); + std::string error_message; + ASSERT_TRUE(ChunkedMixedDefinition::MapIo::load( + file_path, restored, &error_message)) + << error_message; + std::filesystem::remove(file_path); + + const auto stored = restored.continuousMap().getVoxelValue(index); + EXPECT_NEAR(stored.occupancy, voxel.occupancy, 1e-5f); + EXPECT_NEAR(stored.data.get(), + voxel.data.get(), 1e-5f); + ASSERT_TRUE(restored.discreteLayers().get().getValue(index)); + EXPECT_EQ(*restored.discreteLayers().get().getValue(index), 2); +} + TEST(LayeredMapDefinition, IntegratesHeterogeneousObservationBatch) { MixedDefinition::Map map(makeConfig()); LayeredPipeline pipeline(map); diff --git a/library/cpp/test/src/core/map/test_map.cc b/library/cpp/test/src/core/map/test_map.cc index 2d397f860..f36d7e0f3 100644 --- a/library/cpp/test/src/core/map/test_map.cc +++ b/library/cpp/test/src/core/map/test_map.cc @@ -205,6 +205,33 @@ TYPED_TEST(MapTest, InsertionAndLeafVisitor) { } } +TEST(HashedChunkedWaveletOctreeTest, StoresTypedVoxelData) { + using Voxel = VoxelData; + HashedChunkedWaveletOctreeConfig config; + config.tree_height = 6; + HashedChunkedWaveletOctreeT map(config); + + const Index3D index(3, -2, 7); + map.setVoxelValue(index, Voxel{/*occupancy=*/1.25f, /*data=*/0.4f}); + + Voxel voxel = map.getVoxelValue(index); + EXPECT_FLOAT_EQ(voxel.occupancy, 1.25f); + EXPECT_FLOAT_EQ(voxel.data, 0.4f); + EXPECT_FLOAT_EQ(map.getCellValue(index), voxel.occupancy); + + map.addToVoxelValue(index, + Voxel{/*occupancy=*/0.5f, /*data=*/0.1f}); + voxel = map.getVoxelValue(index); + EXPECT_NEAR(voxel.occupancy, 1.75f, 1e-5f); + EXPECT_NEAR(voxel.data, 0.5f, 1e-5f); + + map.threshold(); + map.prune(); + voxel = map.getVoxelValue(index); + EXPECT_NEAR(voxel.occupancy, 1.75f, 1e-5f); + EXPECT_NEAR(voxel.data, 0.5f, 1e-5f); +} + // TODO(victorr): For classes derived from VolumetricOctreeInterface, test // NodeIndex based setters and getters (incl. whether values of // all children are updated but nothing spills to the diff --git a/library/cpp/test/src/core/utils/query/test_local_elevation_classifier.cc b/library/cpp/test/src/core/utils/query/test_local_elevation_classifier.cc index 70124607c..28ac297fc 100644 --- a/library/cpp/test/src/core/utils/query/test_local_elevation_classifier.cc +++ b/library/cpp/test/src/core/utils/query/test_local_elevation_classifier.cc @@ -1,6 +1,6 @@ #include -#include +#include namespace wavemap::layered { namespace { From aadc5cb188c3691b9e8d89be13ff37ad41964c2b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Guilherne=20Caba=C3=A7o?= Date: Fri, 11 Sep 2026 15:35:39 +0000 Subject: [PATCH 11/14] Document layered Wavemap setup and extension Add a layered project overview and demo video to the repository landing page. Document Docker and native setup, the RGB and Ouster workflows, persistence, custom layer definitions, sensor bindings, visualization, and troubleshooting. --- README.md | 37 ++- docs/pages/tutorials/index.rst | 1 + docs/pages/tutorials/layered_wavemap.rst | 403 +++++++++++++++++++++++ 3 files changed, 433 insertions(+), 8 deletions(-) create mode 100644 docs/pages/tutorials/layered_wavemap.rst diff --git a/README.md b/README.md index 737a112e6..96739124e 100644 --- a/README.md +++ b/README.md @@ -1,13 +1,33 @@ -# Wavemap -C++ -Python -ROS1 -Docs +# Layered Wavemap +C++ +Python +ROS1 +Docs Version -License -[![3D reconstruction of Newer College's Cloister](https://github.com/ethz-asl/wavemap/assets/6238939/e432d4ea-440d-4e9d-adf9-af3ae3b09a10)](https://www.youtube.com/live/ftQhK75Ri1E?si=9txTYyJ78wQuhyN-&t=733) +License -## Hierarchical, multi-resolution volumetric mapping +Layered Wavemap extends Wavemap's compressed multi-resolution occupancy map with typed environmental attributes. Continuous fields such as reflectivity, signal, near-infrared intensity, and RGB share the wavelet hierarchy with occupancy. Categorical fields such as semantic classes use aligned sparse side layers with lossless majority-with-exceptions compression. + +[Get started](docs/pages/tutorials/layered_wavemap.rst) · [Run the RGB demo](#try-the-layered-rgb-demo) · [Original Wavemap documentation](https://ethz-asl.github.io/wavemap/) + +[![Layered Wavemap demo](https://i.ytimg.com/vi/l24QWAxm7Zw/hqdefault.jpg)](https://youtu.be/l24QWAxm7Zw) + +*Watch the Layered Wavemap demo on YouTube.* + +## Try the layered RGB demo + +The synthetic forest demo needs no sensor data. On Ubuntu 20.04 with ROS Noetic: + +```bash +cd ~/catkin_ws +catkin build wavemap_all +source devel/setup.bash +roslaunch wavemap_ros synthetic_rgb_layered_map.launch +``` + +The launch file opens RViz and progressively builds a colored layered map. Docker setup, native installation, real Ouster usage, map persistence, and a layer-extension walkthrough are in the [Layered Wavemap guide](docs/pages/tutorials/layered_wavemap.rst). + +## Wavemap foundation Wavemap achieves state-of-the-art memory and computational efficiency by combining Haar wavelet compression and a coarse-to-fine measurement integration scheme. Advanced measurement models allow it to attain exceptionally high recall rates on challenging obstacles like thin objects. The framework is very flexible and supports several data structures, measurement integration methods, and sensor models out of the box. The ROS interface can, for example, easily be configured to fuse multiple sensor inputs, such as a LiDAR configured with a range of 20m and several depth cameras up to a resolution of 1cm, into a single multi-resolution occupancy grid map. @@ -21,6 +41,7 @@ Wavemap provides [C++](https://ethz-asl.github.io/wavemap/pages/tutorials/cpp) a The framework's documentation is available on [GitHub Pages](https://ethz-asl.github.io/wavemap/) for easy online access. A PDF version of each release’s documentation can also be found in the respective [release notes](https://github.com/ethz-asl/wavemap/releases). ### Table of contents +* [Layered Wavemap: setup, use, and extension](docs/pages/tutorials/layered_wavemap.rst) * [Installation](https://ethz-asl.github.io/wavemap/pages/installation) * [Demos](https://ethz-asl.github.io/wavemap/pages/demos) * [Tutorials](https://ethz-asl.github.io/wavemap/pages/tutorials) diff --git a/docs/pages/tutorials/index.rst b/docs/pages/tutorials/index.rst index 8858cef2a..f7badbdff 100644 --- a/docs/pages/tutorials/index.rst +++ b/docs/pages/tutorials/index.rst @@ -6,6 +6,7 @@ This section provides hands-on examples of how to use wavemap's C++ and Python A :caption: Interfaces :maxdepth: 1 + layered_wavemap cpp python ros1 diff --git a/docs/pages/tutorials/layered_wavemap.rst b/docs/pages/tutorials/layered_wavemap.rst new file mode 100644 index 000000000..f057eb5e4 --- /dev/null +++ b/docs/pages/tutorials/layered_wavemap.rst @@ -0,0 +1,403 @@ +.. _layered-wavemap-tutorial: + +Layered Wavemap +############### + +Layered Wavemap extends the original occupancy map with environmental +attributes that use the same spatial coordinates. This guide takes you from a +fresh checkout to a running RGB example, then shows how to use real point-cloud +fields and define a new layer. + +What is stored +************** + +Every layered map owns two related forms of storage: + +* Occupancy and wavelet-compatible continuous states share one wavelet octree. + Examples are reflectivity, signal strength, near-infrared intensity, and RGB. +* Categorical or finite-state values use sparse side layers aligned to the + minimum-resolution voxel indices. The current discrete backend stores a + dominant value per block, observed offsets, and exact exceptions. + +Occupancy always remains part of the continuous voxel and is not listed in a +user schema. Continuous and discrete describe the mathematical operations that +are valid for a value. An integer class ID is discrete because averaging class +IDs has no useful meaning. + +.. code-block:: text + + posed point cloud + +-- XYZ ----------------> original Wavemap occupancy integrator + +-- point attributes ---> layer update policy + +-- continuous: shared wavelet voxel + +-- discrete: aligned side layer + +Requirements +************ + +The supported ROS setup is Ubuntu 20.04 with ROS Noetic. Docker is the fastest +way to try the project when that environment is not already installed. Native +installation is more convenient while changing and rebuilding the code. + +Docker quick start +================== + +Install Docker and configure it for use without ``sudo``. Then clone this +repository and build the ROS image from the local checkout: + +.. code-block:: bash + + git clone https://github.com/GuilhermeCabaco/wavemap-digital-twin.git + cd wavemap-digital-twin + docker build --tag=wavemap_ros1 \ + --file tooling/docker/ros1/full.Dockerfile . + +The full Dockerfile copies the current checkout into a Catkin workspace, +installs its dependencies, and builds ``wavemap_all``. Downloading the upstream +Wavemap image alone does not include this repository's layered extensions. + +The supplied runner forwards an X11 display for RViz. It expects a host data +directory at ``/home/$USER/data``: + +.. code-block:: bash + + mkdir -p /home/$USER/data + chmod +x tooling/scripts/run_in_docker.sh + ./tooling/scripts/run_in_docker.sh \ + roslaunch wavemap_ros synthetic_rgb_layered_map.launch + +The runner is intended for Linux/X11. Other display systems require the Docker +GUI forwarding appropriate to that platform. + +Native ROS Noetic installation +============================== + +Install ROS Noetic, followed by the build tools: + +.. code-block:: bash + + sudo apt update + sudo apt install git build-essential python3-rosdep python3-catkin-tools + +Create and configure a workspace: + +.. code-block:: bash + + mkdir -p ~/catkin_ws/src + cd ~/catkin_ws + source /opt/ros/noetic/setup.bash + catkin init + catkin config --cmake-args -DCMAKE_BUILD_TYPE=Release + +Clone this repository into the source space: + +.. code-block:: bash + + cd ~/catkin_ws/src + git clone https://github.com/GuilhermeCabaco/wavemap-digital-twin.git wavemap + +Initialize rosdep once, install dependencies, and build: + +.. code-block:: bash + + sudo rosdep init + rosdep update + cd ~/catkin_ws + rosdep install -y --from-paths src --ignore-src + catkin build wavemap_all + source devel/setup.bash + +If rosdep is already initialized, continue with ``rosdep update``. Source +``devel/setup.bash`` in each new terminal. + +Run the dataset-free RGB demo +***************************** + +The synthetic demo constructs a small colored forest, publishes it as a +layered map, and opens RViz: + +.. code-block:: bash + + source ~/catkin_ws/devel/setup.bash + roslaunch wavemap_ros synthetic_rgb_layered_map.launch + +No bag, sensor driver, or localization system is needed. RViz subscribes to +``/synthetic_rgb_layered_map/layered_map``. Use +``Render voxels / Layer`` to switch between ``rgb`` and ``occupancy``. The node +prints a completion message after inserting all voxels. + +Control the animation with: + +.. code-block:: bash + + roslaunch wavemap_ros synthetic_rgb_layered_map.launch \ + voxels_per_update:=300 update_period:=0.03 + +Save the completed map with: + +.. code-block:: bash + + roslaunch wavemap_ros synthetic_rgb_layered_map.launch \ + save_path:=$HOME/synthetic_rgb.lwvmp + +After the completion message, stop the process with Ctrl-C. To inspect the file +later, add a ``LayeredMap`` display in RViz, change ``Source`` from ``Topic`` to +``File``, and select ``Load map from disk``. The built-in RGB schema handler +recognizes the saved file. + +Run a layered server with Ouster data +************************************* + +The included Ouster applications expect: + +* Point clouds on ``/ouster/points``. +* A valid TF transform from each cloud frame to the configured map frame. +* ``reflectivity`` values between 0 and 255. +* ``intensity`` and ``ambient`` values between 0 and 65535 when selected. + +The all-layer application stores occupancy, reflectivity, signal, near +infrared, and a geometrically derived ground/obstacle class: + +.. code-block:: bash + + source ~/catkin_ws/devel/setup.bash + rosparam load \ + $(rospack find wavemap_ros)/config/wavemap_ouster_all_layers_demo.yaml \ + /wavemap + rosrun wavemap_ros signal_near_ir_class_layered_server __name:=wavemap + +In another terminal, run the sensor, localization, or bag playback that +publishes the cloud and TF transforms. Open the supplied RViz configuration: + +.. code-block:: bash + + source ~/catkin_ws/devel/setup.bash + rviz -d $(rospack find wavemap_ros)/config/rviz/layered_map.rviz + +Choose ``occupancy``, ``reflectivity``, ``signal``, ``near_ir``, or ``class`` +from the display's Layer property. Class 1 is ground and class 2 is obstacle. +This is an example based on local elevation, not a learned semantic model. + +Available applications +====================== + +.. list-table:: + :header-rows: 1 + :widths: 45 55 + + * - Executable + - Additional layers + * - ``occupancy_layered_server`` + - None; exercises the layered lifecycle + * - ``reflectivity_layered_server`` + - reflectivity + * - ``reflectivity_class_layered_server`` + - reflectivity and class + * - ``signal_layered_server`` + - reflectivity and signal + * - ``signal_near_ir_layered_server`` + - reflectivity, signal, and near infrared + * - ``signal_class_layered_server`` + - reflectivity, signal, and class + * - ``signal_near_ir_class_layered_server`` + - reflectivity, signal, near infrared, and class + +Hashed and chunked versions also exist for the occupancy and reflectivity +examples. Their source is under +``interfaces/ros1/wavemap_ros/app/ouster_layered``. + +Save and restore layered maps +***************************** + +A layered server named ``/wavemap`` exposes these services: + +.. code-block:: bash + + rosservice call /wavemap/save_layered_map \ + "file_path: '$HOME/forest_map.lwvmp'" + + rosservice call /wavemap/load_layered_map \ + "file_path: '$HOME/forest_map.lwvmp'" + + rosservice call /wavemap/layered_map_request_full + +The ``.lwvmp`` format stores its version, ordered layer schema, typed continuous +wavelet map, and compressed discrete side layers. Loading rejects incompatible +schemas and map geometry. Start the same typed server used to create the file +before calling ``load_layered_map``. A successful load publishes the full map. + +Define and configure a layer +**************************** + +A layer is a compile-time type. This scalar example uses exponential smoothing +with an observation weight of 0.2: + +.. code-block:: c++ + + using ReflectivityPolicy = + wavemap::layered::ExponentialScalarLayerUpdatePolicy<1, 5>; + + struct ReflectivityLayer + : wavemap::layered::schema::ContinuousLayer< + float, ReflectivityPolicy> { + static constexpr std::string_view name = "reflectivity"; + static constexpr wavemap::layered::ScalarLayerVisualization + visualization{{0.05f, 0.05f, 0.05f}, {1.f, 1.f, 1.f}}; + }; + +A class layer uses discrete storage because arithmetic on identifiers would not +preserve their meaning: + +.. code-block:: c++ + + struct ClassLayer + : wavemap::layered::schema::DiscreteLayer< + int, wavemap::layered::ReplaceLayerUpdatePolicy> { + static constexpr std::string_view name = "class"; + static constexpr std::array categories{ + wavemap::layered::IntegerCategoryLabel{ + 1, "Ground", {0.2f, 0.8f, 0.2f}}, + wavemap::layered::IntegerCategoryLabel{ + 2, "Obstacle", {0.9f, 0.2f, 0.15f}}}; + }; + +Combine the tags into a schema and derive the map type: + +.. code-block:: c++ + + using Schema = wavemap::layered::schema::LayerSchema< + ReflectivityLayer, ClassLayer>; + using Definition = wavemap::layered::LayeredMapDefinition; + +Occupancy is included automatically. Pass +``wavemap::layered::HashedChunkedWaveletOctreeBackend`` as the second +``LayeredMapDefinition`` template argument to select the chunked backend. + +Configure map geometry, pruning, bounds, and discrete grouping: + +.. code-block:: c++ + + wavemap::layered::LayeredMapConfigBuilder config; + config.map() + .minCellWidth(0.25f) + .occupancyLogOdds(-2.f, 4.f) + .treeHeight(7) + .onlyPruneBlocksIfUnusedFor(5.f); + config.occupancy().pruningScale(1e-3f).pruningWeight(1.f); + config.layer() + .storageBounds(0.f, 1.f) + .pruningScale(1e-3f) + .pruningWeight(1.f); + config.discreteBlockHeight(2); + config.combinedPruningThreshold(1.f); + +A discrete block height of 2 groups 4 x 4 x 4 minimum-resolution positions. It +changes compression grouping, not map resolution. + +Choose an update policy +======================= + +Reusable policies live in +``library/cpp/include/wavemap/layered/integration/layer_update_policy.h``. They +include replacement, minimum, maximum, accumulation, exponential smoothing, +confidence blending, logical operations, and a stateful weighted mean. + +For ordinary layers, Value and State are identical. Use +``StatefulContinuousLayer`` when observations need a richer stored state. A +custom wavelet-compatible state must provide: + +* ``ContinuousValueTraits`` operations for addition, subtraction, scaling, + thresholding, and magnitude. +* A ``LayerStreamCodec`` for persistence. +* A ``LayerStateConversion`` when State differs from public Value. +* A ``LayerRosCodec`` when using the generic ROS converter. + +The RGB and weighted-mean types under +``library/cpp/include/wavemap/layered/types`` are compact examples. + +Bind sensor data +================ + +Install the typed map with ``LayeredRosServerBuilder``, then register how sensor +fields populate it. Numeric PointCloud2 fields can be normalized directly: + +.. code-block:: c++ + + server.bindNormalizedPointField( + extension, "reflectivity", 0.f, 255.f, + wavemap::layered::EndpointRange{0.5f, 25.f}); + +Other helpers include: + +* ``bindDirectPointField`` for values already in the layer's units. +* ``bindComputedPointField`` for values derived from numeric fields. +* ``bindGeometricClass`` for the included local-elevation classifier. +* A custom endpoint adapter for structured or application-specific decoding. + +Bindings default to endpoint-only integration. Continuous layers may select an +along-ray ``PointcloudLayerIntegrationMode``. Discrete fields are restricted to +endpoints. + +Expose and visualize the application +==================================== + +Add the executable to ``interfaces/ros1/wavemap_ros/CMakeLists.txt``, link it to +``wavemap_ros``, and rebuild: + +.. code-block:: bash + + cd ~/catkin_ws + catkin build wavemap_ros wavemap_ros_conversions wavemap_rviz_plugin + source devel/setup.bash + +Topic visualization obtains schema and color metadata from the ``LayeredMap`` +message. Loading a custom ``.lwvmp`` directly in RViz additionally requires a +typed ``LayeredMapFactory`` registration for the new schema. + +Verification and troubleshooting +******************************** + +Run the affected builds and tests after changing schemas, codecs, storage, or +ROS conversions: + +.. code-block:: bash + + cd ~/catkin_ws + catkin build wavemap wavemap_ros_conversions wavemap_ros \ + wavemap_rviz_plugin + catkin run_tests wavemap wavemap_ros_conversions wavemap_ros \ + wavemap_rviz_plugin + source /opt/ros/noetic/setup.bash + catkin_test_results --all build + +No map appears + Check the display topic, the fixed frame, TF availability, and whether the + node has published a map message. + +Point clouds are dropped + Verify cloud timestamps and frames, the map-to-sensor transform, and + ``max_wait_for_pose``. Motion undistortion needs usable per-point timing. + +An attribute remains empty + Inspect the PointCloud2 fields. Its name and numeric range must match the + registered binding. + +A saved map will not load + Use the same schema, backend, resolution, tree geometry, and discrete block + configuration that created it. + +Fine attribute detail disappears + Reduce the layer's pruning scale or increase its pruning weight. Continuous + fields contribute to one combined pruning decision. + +Relevant source locations +************************* + +* ``library/cpp/include/wavemap/layered/schema``: declarations and bundles. +* ``library/cpp/include/wavemap/layered/integration``: observations and update + policies. +* ``library/cpp/include/wavemap/layered/map``: map types, backends, and storage. +* ``library/cpp/include/wavemap/layered/io``: schema-aware persistence. +* ``interfaces/ros1/wavemap_ros/app/ouster_layered``: complete applications. +* ``interfaces/ros1/wavemap_ros_conversions``: ROS codecs. +* ``interfaces/ros1/wavemap_rviz_plugin``: topic and file visualization. From 06974424b7cd3822c082a9a2ada3c67b3378b20f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Guilherne=20Caba=C3=A7o?= Date: Fri, 11 Sep 2026 16:49:51 +0000 Subject: [PATCH 12/14] Load built-in layered map schemas dynamically --- .../wavemap_ros_conversions/CMakeLists.txt | 1 + .../layered_map_file_conversions.h | 23 + .../src/layered_map_file_conversions.cc | 481 ++++++++++++++++++ .../test/src/test_map_msg_conversions.cc | 103 +++- .../src/layered_map_display.cc | 15 +- 5 files changed, 620 insertions(+), 3 deletions(-) create mode 100644 interfaces/ros1/wavemap_ros_conversions/include/wavemap_ros_conversions/layered_map_file_conversions.h create mode 100644 interfaces/ros1/wavemap_ros_conversions/src/layered_map_file_conversions.cc diff --git a/interfaces/ros1/wavemap_ros_conversions/CMakeLists.txt b/interfaces/ros1/wavemap_ros_conversions/CMakeLists.txt index b986f3bdf..0076807e8 100644 --- a/interfaces/ros1/wavemap_ros_conversions/CMakeLists.txt +++ b/interfaces/ros1/wavemap_ros_conversions/CMakeLists.txt @@ -14,6 +14,7 @@ catkin_package( # Libraries add_library(${PROJECT_NAME} src/config_conversions.cc + src/layered_map_file_conversions.cc src/map_msg_conversions.cc src/time_conversions.cc) set_wavemap_target_properties(${PROJECT_NAME}) diff --git a/interfaces/ros1/wavemap_ros_conversions/include/wavemap_ros_conversions/layered_map_file_conversions.h b/interfaces/ros1/wavemap_ros_conversions/include/wavemap_ros_conversions/layered_map_file_conversions.h new file mode 100644 index 000000000..9ebfc2e2b --- /dev/null +++ b/interfaces/ros1/wavemap_ros_conversions/include/wavemap_ros_conversions/layered_map_file_conversions.h @@ -0,0 +1,23 @@ +#ifndef WAVEMAP_ROS_CONVERSIONS_LAYERED_MAP_FILE_CONVERSIONS_H_ +#define WAVEMAP_ROS_CONVERSIONS_LAYERED_MAP_FILE_CONVERSIONS_H_ + +#include +#include + +#include +#include + +namespace wavemap::convert { + +// Loads a version-1 .lwvmp file by interpreting the layer schema stored in the +// file. This path supports Wavemap's built-in stream codecs and does not +// require the original compile-time LayerSchema to be present in the process. +bool layeredMapFileToRosMsg(const std::filesystem::path& file_path, + const std::string& frame_id, + const ros::Time& stamp, + wavemap_msgs::LayeredMap& msg, + std::string* error_message = nullptr); + +} // namespace wavemap::convert + +#endif // WAVEMAP_ROS_CONVERSIONS_LAYERED_MAP_FILE_CONVERSIONS_H_ diff --git a/interfaces/ros1/wavemap_ros_conversions/src/layered_map_file_conversions.cc b/interfaces/ros1/wavemap_ros_conversions/src/layered_map_file_conversions.cc new file mode 100644 index 000000000..aad77282a --- /dev/null +++ b/interfaces/ros1/wavemap_ros_conversions/src/layered_map_file_conversions.cc @@ -0,0 +1,481 @@ +#include "wavemap_ros_conversions/layered_map_file_conversions.h" + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace wavemap::convert { +namespace { +using layered::LayerSchemaEntry; +using layered::LayeredMapSchema; + +constexpr uint64_t kMaxLayerCount = 1024u; +constexpr uint64_t kMaxNameLength = 4096u; +constexpr uint64_t kMaxBlockCount = 10'000'000u; +constexpr uint64_t kMaxNodeCount = 100'000'000u; +constexpr uint64_t kMaxDiscreteCellCount = 100'000'000u; +constexpr uint64_t kMaxOffsetsPerCell = 100'000'000u; + +bool fail(const std::string& message, std::string* error_message) { + if (error_message) { + *error_message = message; + } + return false; +} + +template +bool readPod(std::istream& stream, T& value) { + static_assert(std::is_trivially_copyable_v); + stream.read(reinterpret_cast(&value), sizeof(value)); + return static_cast(stream); +} + +bool readString(std::istream& stream, std::string& value) { + uint64_t size = 0u; + if (!readPod(stream, size) || kMaxNameLength < size) { + return false; + } + value.resize(static_cast(size)); + stream.read(value.data(), static_cast(size)); + return static_cast(stream); +} + +bool readSection(std::istream& stream, const std::string& expected) { + std::string actual; + return readString(stream, actual) && actual == expected; +} + +bool readSchemaEntries(std::istream& stream, + std::vector& entries) { + uint64_t count = 0u; + if (!readPod(stream, count) || kMaxLayerCount < count) { + return false; + } + entries.clear(); + entries.reserve(static_cast(count)); + for (uint64_t index = 0u; index < count; ++index) { + auto& entry = entries.emplace_back(); + if (!readString(stream, entry.name) || !readString(stream, entry.type)) { + return false; + } + } + return true; +} + +bool readSchema(std::istream& stream, LayeredMapSchema& schema) { + return readSchemaEntries(stream, schema.continuous_layers) && + readSchemaEntries(stream, schema.discrete_layers); +} + +std::string rosTypeForStreamType(const std::string& stream_type) { + if (stream_type == "float32_weighted_mean_state") { + return "float32"; + } + return stream_type; +} + +bool isSupportedContinuousType(const std::string& type) { + return type == "float32" || type == "float32_rgb" || + type == "float32_weighted_mean_state"; +} + +std::vector makeLayers( + const std::vector& layers) { + std::vector result; + result.reserve(layers.size()); + for (const auto& layer : layers) { + auto& layer_msg = result.emplace_back(); + layer_msg.name = layer.name; + layer_msg.type = rosTypeForStreamType(layer.type); + } + return result; +} + +bool appendContinuousValue(std::istream& stream, + const LayerSchemaEntry& schema, + wavemap_msgs::Layer& layer) { + if (schema.type == "float32") { + float value = 0.f; + if (!readPod(stream, value)) { + return false; + } + layer.float32_values.emplace_back(value); + return true; + } + if (schema.type == "float32_rgb") { + std::array value{}; + for (float& component : value) { + if (!readPod(stream, component)) { + return false; + } + layer.float32_values.emplace_back(component); + } + return true; + } + if (schema.type == "float32_weighted_mean_state") { + float weighted_sum = 0.f; + float total_weight = 0.f; + if (!readPod(stream, weighted_sum) || !readPod(stream, total_weight)) { + return false; + } + layer.float32_values.emplace_back( + 0.f < total_weight ? weighted_sum / total_weight : 0.f); + return true; + } + return false; +} + +bool readContinuousCell(std::istream& stream, + const std::vector& custom_layers, + float& occupancy, + std::vector& layers) { + if (!readPod(stream, occupancy)) { + return false; + } + for (size_t layer_index = 0u; layer_index < custom_layers.size(); + ++layer_index) { + if (!appendContinuousValue(stream, custom_layers[layer_index], + layers[layer_index])) { + return false; + } + } + return true; +} + +wavemap_msgs::Index3D toRosIndex( + const wavemap::io::streamable::Index3D& index) { + wavemap_msgs::Index3D msg; + msg.x = index.x; + msg.y = index.y; + msg.z = index.z; + return msg; +} + +unsigned int countSetBits(uint8_t value) { + unsigned int count = 0u; + while (value != 0u) { + count += value & 1u; + value >>= 1u; + } + return count; +} + +bool readContinuousMap(std::istream& stream, + const std::vector& custom_layers, + wavemap_msgs::LayeredHashedWaveletOctree& msg, + std::string* error_message) { + const auto storage_format = + wavemap::io::streamable::StorageFormat::read(stream); + if (!stream || storage_format != + wavemap::io::streamable::StorageFormat:: + kLayeredHashedWaveletOctree) { + return fail("The file does not contain a supported layered hashed-wavelet " + "continuous map.", error_message); + } + + const auto header = + wavemap::io::streamable::HashedWaveletOctreeHeader::read(stream); + if (!stream || !std::isfinite(header.min_cell_width) || + header.min_cell_width <= 0.f || !std::isfinite(header.min_log_odds) || + !std::isfinite(header.max_log_odds) || + header.max_log_odds < header.min_log_odds || header.tree_height < 0 || + 30 < header.tree_height || + kMaxBlockCount < header.num_blocks) { + return fail("The layered continuous-map header is invalid.", error_message); + } + msg.min_cell_width = header.min_cell_width; + msg.min_log_odds = header.min_log_odds; + msg.max_log_odds = header.max_log_odds; + msg.tree_height = header.tree_height; + msg.layer_names.reserve(custom_layers.size()); + msg.layer_types.reserve(custom_layers.size()); + for (const auto& layer : custom_layers) { + msg.layer_names.emplace_back(layer.name); + msg.layer_types.emplace_back(rosTypeForStreamType(layer.type)); + } + uint64_t total_node_count = 0u; + for (uint64_t block_index = 0u; block_index < header.num_blocks; + ++block_index) { + const auto serialized_index = + wavemap::io::streamable::Index3D::read(stream); + if (!stream) { + return fail("The layered map ended while reading a block index.", + error_message); + } + const auto ros_index = toRosIndex(serialized_index); + msg.allocated_block_indices.emplace_back(ros_index); + auto& block = msg.blocks.emplace_back(); + block.root_node_offset = ros_index; + block.root_node_layers = makeLayers(custom_layers); + if (!readContinuousCell(stream, custom_layers, + block.root_node_occupancy_scale_coefficient, + block.root_node_layers)) { + return fail("The layered map ended while reading a block root value.", + error_message); + } + + std::vector pending_node_heights{header.tree_height}; + while (!pending_node_heights.empty()) { + if (kMaxNodeCount <= total_node_count) { + return fail("The layered map contains too many wavelet nodes.", + error_message); + } + ++total_node_count; + const int32_t node_height = pending_node_heights.back(); + pending_node_heights.pop_back(); + auto& node = block.nodes.emplace_back(); + node.detail_layers = makeLayers(custom_layers); + for (size_t coefficient_index = 0u; + coefficient_index + 1u < + node.occupancy_detail_coefficients.size(); + ++coefficient_index) { + if (!readContinuousCell( + stream, custom_layers, + node.occupancy_detail_coefficients[coefficient_index], + node.detail_layers)) { + return fail("The layered map ended while reading wavelet " + "coefficients.", error_message); + } + } + if (!readPod(stream, node.allocated_children_bitset)) { + return fail("The layered map ended while reading its wavelet tree.", + error_message); + } + const unsigned int child_count = + countSetBits(node.allocated_children_bitset); + if (node_height == 0 && child_count != 0u) { + return fail("The layered map has children below its minimum cell " + "height.", error_message); + } + pending_node_heights.insert(pending_node_heights.end(), child_count, + node_height - 1); + } + } + return true; +} + +template +bool readDiscreteCells(std::istream& stream, + wavemap_msgs::DiscreteLayer& layer, + std::string* error_message) { + int32_t block_height = 0; + uint64_t cell_count = 0u; + if (!readPod(stream, block_height) || block_height < 0 || + 10 < block_height || !readPod(stream, cell_count) || + kMaxDiscreteCellCount < cell_count) { + return fail("A discrete-layer header is invalid.", error_message); + } + layer.block_height = block_height; + const int32_t max_offset = 1 << (3 * block_height); + for (uint64_t cell_index = 0u; cell_index < cell_count; ++cell_index) { + auto& cell = layer.cells.emplace_back(); + if (!readPod(stream, cell.parent_index.x) || + !readPod(stream, cell.parent_index.y) || + !readPod(stream, cell.parent_index.z)) { + return fail("The file ended while reading a discrete cell index.", + error_message); + } + ValueT dominant{}; + if (!readPod(stream, dominant)) { + return fail("The file ended while reading a discrete cell value.", + error_message); + } + if constexpr (std::is_same_v) { + cell.dominant_int32 = dominant; + } else { + if (1u < dominant) { + return fail("A boolean discrete layer contains a non-boolean value.", + error_message); + } + cell.dominant_uint8 = dominant; + } + + uint64_t observed_count = 0u; + if (!readPod(stream, observed_count) || + kMaxOffsetsPerCell < observed_count) { + return fail("A discrete cell's observed-offset list is invalid.", + error_message); + } + cell.observed_offsets.resize(static_cast(observed_count)); + for (int32_t& offset : cell.observed_offsets) { + if (!readPod(stream, offset) || offset < 0 || max_offset <= offset) { + return fail("A discrete cell contains an invalid observed offset.", + error_message); + } + } + + uint64_t exception_count = 0u; + if (!readPod(stream, exception_count) || + kMaxOffsetsPerCell < exception_count) { + return fail("A discrete cell's exception list is invalid.", + error_message); + } + cell.exception_offsets.resize(static_cast(exception_count)); + if constexpr (std::is_same_v) { + cell.exception_int32_values.reserve(static_cast(exception_count)); + } else { + cell.exception_uint8_values.reserve(static_cast(exception_count)); + } + for (size_t exception_index = 0u; + exception_index < static_cast(exception_count); + ++exception_index) { + ValueT value{}; + if (!readPod(stream, cell.exception_offsets[exception_index]) || + cell.exception_offsets[exception_index] < 0 || + max_offset <= cell.exception_offsets[exception_index] || + !readPod(stream, value)) { + return fail("A discrete cell contains an invalid exception.", + error_message); + } + if constexpr (std::is_same_v) { + cell.exception_int32_values.emplace_back(value); + } else { + if (1u < value) { + return fail("A boolean discrete layer contains a non-boolean value.", + error_message); + } + cell.exception_uint8_values.emplace_back(value); + } + } + } + return true; +} + +bool readDiscreteLayers(std::istream& stream, const LayeredMapSchema& schema, + std::vector& layers, + std::string* error_message) { + uint64_t layer_count = 0u; + if (!readPod(stream, layer_count) || + layer_count != schema.discrete_layers.size()) { + return fail("The discrete-layer data does not match the file schema.", + error_message); + } + layers.reserve(static_cast(layer_count)); + for (size_t layer_index = 0u; layer_index < static_cast(layer_count); + ++layer_index) { + std::string name; + std::string type; + if (!readString(stream, name) || !readString(stream, type) || + name != schema.discrete_layers[layer_index].name || + type != schema.discrete_layers[layer_index].type) { + return fail("The discrete-layer data does not match the file schema.", + error_message); + } + auto& layer = layers.emplace_back(); + layer.name = name; + layer.value_type = type; + if (type == "int") { + if (!readDiscreteCells(stream, layer, error_message)) { + return false; + } + } else if (type == "bool") { + if (!readDiscreteCells(stream, layer, error_message)) { + return false; + } + } else { + return fail("Unsupported discrete codec '" + type + "' for layer '" + + name + "'. Rebuild RViz with that layer's schema and " + "codec registered.", + error_message); + } + } + return true; +} +} // namespace + +bool layeredMapFileToRosMsg(const std::filesystem::path& file_path, + const std::string& frame_id, + const ros::Time& stamp, + wavemap_msgs::LayeredMap& msg, + std::string* error_message) { + std::ifstream stream(file_path, std::ios::binary); + if (!stream.is_open()) { + return fail("Could not open layered map file: " + file_path.string(), + error_message); + } + + std::string magic; + uint32_t version = 0u; + if (!readString(stream, magic) || magic != "LWVMP" || + !readPod(stream, version)) { + return fail("Invalid layered map file header.", error_message); + } + if (version != 1u) { + return fail("Unsupported layered map file version " + + std::to_string(version) + ".", + error_message); + } + + LayeredMapSchema schema; + if (!readSection(stream, "schema") || !readSchema(stream, schema)) { + return fail("Failed to read the layered map schema.", error_message); + } + if (schema.continuous_layers.empty() || + schema.continuous_layers.front() != + LayerSchemaEntry{"occupancy", "float32"}) { + return fail("The file does not have the required occupancy layer.", + error_message); + } + const std::vector custom_layers( + schema.continuous_layers.begin() + 1, + schema.continuous_layers.end()); + for (const auto& layer : custom_layers) { + if (!isSupportedContinuousType(layer.type)) { + return fail("Unsupported continuous codec '" + layer.type + + "' for layer '" + layer.name + + "'. Rebuild RViz with that layer's schema and codec " + "registered.", + error_message); + } + } + for (const auto& layer : schema.discrete_layers) { + if (layer.type != "int" && layer.type != "bool") { + return fail("Unsupported discrete codec '" + layer.type + + "' for layer '" + layer.name + + "'. Rebuild RViz with that layer's schema and codec " + "registered.", + error_message); + } + } + + wavemap_msgs::LayeredMap loaded; + loaded.header.frame_id = frame_id; + loaded.header.stamp = stamp; + loaded.continuous_map.header = loaded.header; + loaded.is_full_update = true; + if (!readSection(stream, "continuous_map")) { + return fail("Failed to find the layered continuous-map section.", + error_message); + } + auto& continuous = + loaded.continuous_map.layered_hashed_wavelet_octree.emplace_back(); + if (!readContinuousMap(stream, custom_layers, continuous, error_message)) { + return false; + } + if (!readSection(stream, "discrete_layers")) { + return fail("Failed to find the layered discrete-layers section.", + error_message); + } + if (!readDiscreteLayers(stream, schema, loaded.discrete_layers, + error_message)) { + return false; + } + msg = std::move(loaded); + return true; +} + +} // namespace wavemap::convert diff --git a/interfaces/ros1/wavemap_ros_conversions/test/src/test_map_msg_conversions.cc b/interfaces/ros1/wavemap_ros_conversions/test/src/test_map_msg_conversions.cc index 30876f96e..32155692c 100644 --- a/interfaces/ros1/wavemap_ros_conversions/test/src/test_map_msg_conversions.cc +++ b/interfaces/ros1/wavemap_ros_conversions/test/src/test_map_msg_conversions.cc @@ -1,5 +1,7 @@ +#include #include #include +#include #include #include @@ -11,13 +13,15 @@ #include #include #include +#include #include #include #include #include -#include "wavemap_ros_conversions/map_msg_conversions.h" #include "wavemap_ros_conversions/descriptor_layered_map_conversions.h" +#include "wavemap_ros_conversions/layered_map_file_conversions.h" +#include "wavemap_ros_conversions/map_msg_conversions.h" namespace wavemap { namespace { @@ -34,6 +38,39 @@ using ChunkedLayeredDefinition = layered::LayeredMapDefinition< using RegularLayeredDefinition = layered::LayeredMapDefinition; using TestLayerRosConverter = convert::DescriptorContinuousRosConverter< ChunkedLayeredDefinition::Voxel>; + +struct GenericRgbLayer + : layered::schema::ContinuousLayer< + layered::Rgb, + layered::ReplaceLayerUpdatePolicy> { + static constexpr std::string_view name = "generic_rgb"; +}; + +struct GenericAverageLayer + : layered::schema::StatefulContinuousLayer< + FloatingPoint, layered::WeightedMeanState, + layered::WeightedMeanLayerUpdatePolicy> { + static constexpr std::string_view name = "generic_average"; +}; + +struct GenericClassLayer + : layered::schema::DiscreteLayer< + int, layered::ReplaceLayerUpdatePolicy> { + static constexpr std::string_view name = "generic_class"; +}; + +struct GenericMaskLayer + : layered::schema::DiscreteLayer< + bool, layered::ReplaceLayerUpdatePolicy> { + static constexpr std::string_view name = "generic_mask"; +}; + +using GenericFileDefinition = layered::LayeredMapDefinition< + layered::schema::LayerSchema>; +using GenericFileRosConverter = convert::DescriptorContinuousRosConverter< + GenericFileDefinition::Voxel>; } // namespace template @@ -226,4 +263,68 @@ TEST(LayeredMapMsgConversionsTest, EXPECT_NEAR(reconstructed.data.get(), voxel.data.get(), 1e-5f); } + +TEST(LayeredMapFileConversionsTest, + LoadsBuiltInCodecsWithoutARegisteredRuntimeSchema) { + ros::Time::init(); + + GenericFileDefinition::Config config; + config.continuous_map.min_cell_width = 0.125f; + config.continuous_map.min_log_odds = -3.f; + config.continuous_map.max_log_odds = 5.f; + config.continuous_map.tree_height = 3; + config.discrete_compression.block_height = 2; + GenericFileDefinition::Map map(config); + + const Index3D first_index{1, 2, 3}; + const Index3D second_index{2, 2, 3}; + GenericFileDefinition::Voxel voxel; + voxel.occupancy = 0.8f; + voxel.data.get() = 0.35f; + voxel.data.get() = {0.2f, 0.4f, 0.6f}; + voxel.data.get() = {2.4f, 3.f}; + map.continuousMap().setVoxelValue(first_index, voxel); + map.discreteLayers().get().setValue(first_index, 4); + map.discreteLayers().get().setValue(second_index, 7); + map.discreteLayers().get().setValue(first_index, true); + map.discreteLayers().get().setValue(second_index, false); + + const auto file_path = std::filesystem::temp_directory_path() / + "wavemap_runtime_schema_conversion_test.lwvmp"; + ASSERT_TRUE(GenericFileDefinition::MapIo::save(file_path, map)); + + const ros::Time stamp(42); + wavemap_msgs::LayeredMap expected; + ASSERT_TRUE((convert::layeredMapToRosMsg( + map, "test_frame", stamp, expected))); + wavemap_msgs::LayeredMap actual; + std::string error_message; + const bool loaded = convert::layeredMapFileToRosMsg( + file_path, "test_frame", stamp, actual, &error_message); + std::filesystem::remove(file_path); + ASSERT_TRUE(loaded) << error_message; + + ASSERT_EQ(actual.continuous_map.layered_hashed_wavelet_octree.size(), 1u); + ASSERT_EQ(expected.continuous_map.layered_hashed_wavelet_octree.size(), 1u); + const auto& actual_continuous = + actual.continuous_map.layered_hashed_wavelet_octree.front(); + const auto& expected_continuous = + expected.continuous_map.layered_hashed_wavelet_octree.front(); + EXPECT_EQ(actual.header, expected.header); + EXPECT_TRUE(actual.is_full_update); + EXPECT_FLOAT_EQ(actual_continuous.min_cell_width, + expected_continuous.min_cell_width); + EXPECT_FLOAT_EQ(actual_continuous.min_log_odds, + expected_continuous.min_log_odds); + EXPECT_FLOAT_EQ(actual_continuous.max_log_odds, + expected_continuous.max_log_odds); + EXPECT_EQ(actual_continuous.tree_height, expected_continuous.tree_height); + EXPECT_EQ(actual_continuous.layer_names, expected_continuous.layer_names); + EXPECT_EQ(actual_continuous.layer_types, expected_continuous.layer_types); + EXPECT_EQ(actual_continuous.allocated_block_indices, + expected_continuous.allocated_block_indices); + EXPECT_EQ(actual_continuous.blocks, expected_continuous.blocks); + EXPECT_EQ(actual.discrete_layers, expected.discrete_layers); +} } // namespace wavemap diff --git a/interfaces/ros1/wavemap_rviz_plugin/src/layered_map_display.cc b/interfaces/ros1/wavemap_rviz_plugin/src/layered_map_display.cc index 1980727fb..574e643d4 100644 --- a/interfaces/ros1/wavemap_rviz_plugin/src/layered_map_display.cc +++ b/interfaces/ros1/wavemap_rviz_plugin/src/layered_map_display.cc @@ -28,6 +28,7 @@ #include #include #include +#include #include #include "wavemap_rviz_plugin/utils/alert_dialog.h" @@ -407,6 +408,17 @@ bool LayeredMapDisplay::loadMapFromDisk( *error_message = std::move(factory_error); } } + + wavemap_msgs::LayeredMap msg; + std::string generic_error; + if (convert::layeredMapFileToRosMsg(filepath, frame_id, ros::Time(0), msg, + &generic_error)) { + displayMessage(msg); + return true; + } + if (error_message) { + *error_message = std::move(generic_error); + } return false; } @@ -937,8 +949,7 @@ void LayeredMapDisplay::loadMapFromDiskCallback() { std::string load_error; if (!loadMapFromDisk(filepath, &load_error)) { if (load_error.empty()) { - load_error = "This display currently supports layered map files that " - "match a registered layered-map schema."; + load_error = "The file could not be decoded as a layered map."; } AlertDialog alert{ "Could not load layered map", From b22617ed5b6c4c7715374c0c1f13c09f05aea8e1 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Guilherne=20Caba=C3=A7o?= Date: Fri, 11 Sep 2026 16:54:49 +0000 Subject: [PATCH 13/14] Clarify layered map extension workflow --- docs/pages/tutorials/layered_wavemap.rst | 67 ++++++++++++++++++++---- 1 file changed, 58 insertions(+), 9 deletions(-) diff --git a/docs/pages/tutorials/layered_wavemap.rst b/docs/pages/tutorials/layered_wavemap.rst index f057eb5e4..fa26da4fb 100644 --- a/docs/pages/tutorials/layered_wavemap.rst +++ b/docs/pages/tutorials/layered_wavemap.rst @@ -20,9 +20,21 @@ Every layered map owns two related forms of storage: dominant value per block, observed offsets, and exact exceptions. Occupancy always remains part of the continuous voxel and is not listed in a -user schema. Continuous and discrete describe the mathematical operations that -are valid for a value. An integer class ID is discrete because averaging class -IDs has no useful meaning. +user schema. Here, continuous and discrete classify the algebra of the stored +state; all layers are still spatially voxelized. + +A layer is continuous when adding, subtracting, and scaling its stored state +have a meaningful interpretation. It must also be possible to bound the state +and measure the magnitude of a wavelet detail coefficient. Scalar sensor +measurements, RGB vectors, and additive weighted-mean statistics satisfy these +requirements and can share the Haar wavelet hierarchy with occupancy. + +A layer is discrete when interpolation or arithmetic would change its meaning. +Semantic class IDs, object IDs, and Boolean flags are examples: averaging class +1 and class 2 does not define a valid class. Their C++ representation does not +determine the choice; an integer or floating-point value still belongs in a +discrete layer when arithmetic on that value is semantically invalid. Such +values use aligned side storage and do not participate in wavelet pruning. .. code-block:: text @@ -145,18 +157,26 @@ later, add a ``LayeredMap`` display in RViz, change ``Source`` from ``Topic`` to ``File``, and select ``Load map from disk``. The built-in RGB schema handler recognizes the saved file. -Run a layered server with Ouster data -************************************* +Use the Ouster integration as a reference +****************************************** -The included Ouster applications expect: +The repository includes Ouster-specific applications as worked examples of a +real layered sensor integration. No Ouster bag or live sensor stream is bundled +with the repository, and the commands below do not start localization. They +show how to launch the mapping side once a compatible point cloud and TF source +are available. For another sensor or dataset, use the same structure and adapt +the topic, field names, normalization ranges, projection model, and transforms. + +The example applications expect: * Point clouds on ``/ouster/points``. * A valid TF transform from each cloud frame to the configured map frame. * ``reflectivity`` values between 0 and 255. * ``intensity`` and ``ambient`` values between 0 and 65535 when selected. -The all-layer application stores occupancy, reflectivity, signal, near -infrared, and a geometrically derived ground/obstacle class: +The all-layer reference application stores occupancy, reflectivity, signal, +near infrared, and a geometrically derived ground/obstacle class. With a +compatible live stream or bag already available, start it as follows: .. code-block:: bash @@ -300,7 +320,36 @@ Choose an update policy Reusable policies live in ``library/cpp/include/wavemap/layered/integration/layer_update_policy.h``. They include replacement, minimum, maximum, accumulation, exponential smoothing, -confidence blending, logical operations, and a stateful weighted mean. +confidence blending, logical operations, and a stateful weighted mean. These are +provided defaults rather than a closed set. A user can define a custom policy +without changing the map or integration pipeline. + +A stateless policy is a default-constructible callable that receives the current +value and a ``LayerObservation`` and returns the next value. A stateful +continuous policy follows the same pattern but receives and returns State: + +.. code-block:: c++ + + struct RateLimitedUpdate { + float operator()( + float current, + const wavemap::layered::LayerObservation& observation) const { + const float change = std::clamp( + observation.value - current, -0.1f, 0.1f); + return current + change; + } + }; + + struct HeightLayer + : wavemap::layered::schema::ContinuousLayer< + float, RateLimitedUpdate> { + static constexpr std::string_view name = "height"; + }; + +A discrete policy may instead accept ``std::optional`` as its current +value so it can distinguish an unobserved voxel from an observed value. The +compile-time schema checks the selected policy signature and reports an error if +it is incompatible with the layer. For ordinary layers, Value and State are identical. Use ``StatefulContinuousLayer`` when observations need a richer stored state. A From 888b72bf2288aaa5d4ecbdc52da6bdd3e9a48bca Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Guilherne=20Caba=C3=A7o?= Date: Fri, 11 Sep 2026 16:55:57 +0000 Subject: [PATCH 14/14] Remove obsolete layered policy design notes --- LAYERED_LAYER_POLICY_DESIGN_NOTES.txt | 196 -------------------------- 1 file changed, 196 deletions(-) delete mode 100644 LAYERED_LAYER_POLICY_DESIGN_NOTES.txt diff --git a/LAYERED_LAYER_POLICY_DESIGN_NOTES.txt b/LAYERED_LAYER_POLICY_DESIGN_NOTES.txt deleted file mode 100644 index e06d8a62f..000000000 --- a/LAYERED_LAYER_POLICY_DESIGN_NOTES.txt +++ /dev/null @@ -1,196 +0,0 @@ -LAYERED LAYER POLICY AND STORAGE DESIGN NOTES -============================================= - -Status ------- -These are design decisions and deferred ideas. They are not requirements for -the current occupancy + reflectivity + class example and are not implemented -unless explicitly stated otherwise. - - -1. Layer semantics and storage are separate concepts ----------------------------------------------------- - -The public meaning of a layer must not determine its storage automatically. -A future layer definition should independently select: - - * Value type: float, integer/category, bool, RGB, vector, structured state. - * Update policy: replace, logical operation, voting, hysteresis, etc. - * Storage policy: wavelet, majority + exceptions, sparse voxels, bit-packed - booleans, direct structured storage, etc. - * Pruning/compression policy. - * Serialization/transport codec when the generic codec is insufficient. - -The intended shape is conceptually: - - Layer - -Compile-time templates are preferred so integration does not gain virtual-call -overhead. - - -2. Continuous and discrete are not a closed list of storage backends ------------------------------------------------------------------- - -The current continuous implementation is specifically suitable for numeric -state that supports meaningful addition, subtraction, scaling, wavelet -reconstruction, thresholding, and pruning. - -The current discrete implementation uses spatial majority + exceptions. - -Structured numeric information can require another backend even when it is -mathematically continuous. Examples include: - - * Unit surface normals. - * Covariance matrices and NDT statistics. - * Probability distributions or mixtures. - * Multiple surfaces/hypotheses in one voxel. - -Prefer storing additive sufficient statistics where possible. For example, -NDT data can store count, position sum, and outer-product sum, then derive mean -and covariance when queried. Coupled pruning is required so independent -thresholding does not create inconsistent or invalid reconstructed state. - -Variable-length histories, graphs, connectivity, and per-frame metadata may -belong alongside the voxel map instead of becoming voxel layers. - - -3. Current meaning of majority + exceptions --------------------------------------------- - -Majority + exceptions is a spatial storage/compression method. A compressed -parent stores: - - * Its dominant child value. - * Which child offsets are observed. - * Explicit values for children that differ from the dominant value. - -It is not an observation update policy and is not restricted to replacement. -For example, the logical output of replace, logical OR, hysteresis, or temporal -voting can all be spatially stored with majority + exceptions. - -There are two different meanings of "majority" that must remain separate: - - * Temporal majority: observations over time vote for one voxel's value. - * Spatial majority: nearby child values are compressed under one parent. - - -4. The changed boolean layer ----------------------------- - -Majority + exceptions is valid for a spatial changed/not-changed boolean, -particularly when changed voxels are rare among mostly false voxels. - -A sparse or bit-packed boolean backend may eventually be more compact, but it -is an alternative storage policy, not a different semantic layer type. - -If "changed" represents an event or history rather than persistent state, a -boolean alone may be insufficient. It may need a last-changed timestamp or a -separate event/history system. - - -5. Per-layer discrete storage policies (deferred) -------------------------------------------------- - -The desired future map keeps heterogeneous side layers: - - class -> MajorityWithExceptionsStorage - changed -> MajorityWithExceptionsStorage or BitPackedBoolStorage - object_id -> SparseVoxelStorage - -Majority + exceptions should remain the default for backwards compatibility: - - DiscreteLayer> - -Required refactoring: - - 1. Extract majority + exceptions into a standalone storage policy. - 2. Make discrete configuration, including block height, per layer. - 3. Define a storage contract for get, set, erase, batch updates, iteration, - dirty-region tracking, and clearing only published dirty regions. - 4. Implement SparseVoxelStorage as a second backend to prove modularity. - 5. Decouple ROS and RViz from majority + exceptions internals. - 6. Provide a generic logical voxel patch and automatic visualization path. - 7. Allow optional optimized codecs for backends that should preserve their - compression over ROS or on disk. - 8. Store backend identifier and format version in persistent files. - -Users should not need custom ROS or RViz code for a normal custom storage -policy. Specialized codecs should only be necessary to preserve backend- -specific compression or hidden state. - - -6. Temporal majority with majority + exceptions (deferred) ----------------------------------------------------------- - -A true temporal majority requires per-voxel hidden integration state such as -vote counts. Keeping only the current winning integer is insufficient because -the history cannot be reconstructed from the winner. - -The processing sequence would be: - - observations for one voxel - -> update temporal vote state - -> calculate current winner - -> if winner changed, update logical class value - -> spatially recompress that value using majority + exceptions - -> mark/publish only the changed spatial parent - -Vote-state changes should not dirty the visible class layer when its winner -does not change. - -The architecture therefore needs a distinction between: - - * Public Value: the winning class ID. - * Stored integration State: vote counts or another stability state. - * Spatial StoragePolicy: how public class values are compressed. - -If mapping must resume exactly after loading, hidden vote state must also be -saved. Saving only winners is sufficient for visualization and queries, but -not for resuming exact temporal voting. - - -7. Efficiency decision for temporal class stability ----------------------------------------------------- - -Full vote histograms are not the preferred default for the current geometric -forest classifier. They require multiple counters per voxel and additional -update work. - -Lower-cost alternatives, in increasing complexity, are: - - * Replace/latest value. - * Class plus one contradictory-observation hysteresis counter. - * Candidate class plus confidence counter (streaming approximate majority). - * Two candidates and two counters. - * Decaying class confidence. - * Full per-class temporal vote histogram. - -For this project, if replacement later proves too noisy, the preferred next -experiment is a compact state: - - StableClassState { - int class_id; - uint8_t confidence; - } - -This has constant memory independent of the number of classes, cheap updates, -and should reduce class changes and discrete patch traffic. Full temporal -majority remains an optional advanced policy, not the default. - - -8. Suggested future implementation order ------------------------------------------ - - 1. Formalize custom stateless update-policy requirements. - 2. Add explicit public Value versus stored State support. - 3. Extract the existing majority + exceptions backend. - 4. Add per-layer storage configuration. - 5. Implement and test one second discrete backend. - 6. Introduce storage-independent logical patches for generic ROS/RViz use. - 7. Add optional optimized transport and file codecs. - 8. Experiment with compact class hysteresis only if replacement is too - unstable in real mapping tests. -