From 776bfe2b9e176a17f349f7f6fa76c9d62cd0a0b1 Mon Sep 17 00:00:00 2001 From: Sergei Sergienko Date: Fri, 18 Oct 2024 18:55:59 +0300 Subject: [PATCH 01/16] feat: Added depth extraction functionality --- examples/python/raycast/depth_exctraction.py | 52 ++++++++++ library/python/CMakeLists.txt | 3 +- library/python/include/pywavemap/raycast.h | 12 +++ library/python/src/pywavemap.cc | 4 + library/python/src/pywavemap/__init__.py | 1 + library/python/src/raycast.cc | 99 ++++++++++++++++++++ 6 files changed, 170 insertions(+), 1 deletion(-) create mode 100644 examples/python/raycast/depth_exctraction.py create mode 100644 library/python/include/pywavemap/raycast.h create mode 100644 library/python/src/raycast.cc diff --git a/examples/python/raycast/depth_exctraction.py b/examples/python/raycast/depth_exctraction.py new file mode 100644 index 000000000..068f56b7e --- /dev/null +++ b/examples/python/raycast/depth_exctraction.py @@ -0,0 +1,52 @@ +""" +Depth map extraction from HashedWaveletOctree map at given camera pose and intrinsics +""" + +import time +from pathlib import Path + +import numpy as np +from PIL import Image +import pywavemap as wm + +def save_depth_as_png(depth_map: np.ndarray, out_path: Path): + depth_min = np.min(depth_map) + depth_max = np.max(depth_map) + + # Avoid division by zero in case all values are the same + if depth_max - depth_min > 0: + depth_map_normalized = (depth_map - depth_min) / (depth_max - depth_min) + else: + depth_map_normalized = np.zeros_like(depth_map) + + # Convert floats (meters) to uint8 and save to png + depth_map_8bit = (depth_map_normalized * 255).astype(np.uint8) + image = Image.fromarray(depth_map_8bit) + image.save(out_path) + +if __name__ == "__main__": + map_path = Path.home() / "data/panoptic_mapping/flat_dataset/run2/your_map.wvmp" + out_path = Path(__file__).parent / "depth.png" + camera_cfg = wm.PinholeCameraProjectorConfig( + width=1280, + height=720, + fx=526.21539307, + fy=526.21539307, + cx=642.309021, + cy=368.69949341, + ) # Note: these are intrinsics for Zed 2i + + # Load map from file + map = wm.Map.load(map_path) + + # Create pose + rotation = wm.Rotation(np.eye(3)) + translation = np.zeros(3) + pose = wm.Pose(rotation, translation) + + # Extract depth + t1 = time.perf_counter() + depth = wm.get_depth(map, pose, camera_cfg, 0.1, 10) + t2 = time.perf_counter() + print(f"Depth map of size {camera_cfg.width}x{camera_cfg.height} created in {t2-t1:.02f} seconds") + save_depth_as_png(depth, out_path) \ No newline at end of file diff --git a/library/python/CMakeLists.txt b/library/python/CMakeLists.txt index 23ad9ab77..49a0e7a0d 100644 --- a/library/python/CMakeLists.txt +++ b/library/python/CMakeLists.txt @@ -63,7 +63,8 @@ nanobind_add_module(_pywavemap_bindings STABLE_ABI src/maps.cc src/measurements.cc src/param.cc - src/pipeline.cc) + src/pipeline.cc + src/raycast.cc) set_wavemap_target_properties(_pywavemap_bindings) target_include_directories(_pywavemap_bindings PRIVATE include) target_link_libraries(_pywavemap_bindings PRIVATE diff --git a/library/python/include/pywavemap/raycast.h b/library/python/include/pywavemap/raycast.h new file mode 100644 index 000000000..61fbf9b18 --- /dev/null +++ b/library/python/include/pywavemap/raycast.h @@ -0,0 +1,12 @@ +#ifndef PYWAVEMAP_RAYCAST_H_ +#define PYWAVEMAP_RAYCAST_H_ + +#include + +namespace nb = nanobind; + +namespace wavemap { +void add_raycast_bindings(nb::module_& m); +} // namespace wavemap + +#endif // PYWAVEMAP_RAYCAST_H_ diff --git a/library/python/src/pywavemap.cc b/library/python/src/pywavemap.cc index 2cc7ba0ae..c44791623 100644 --- a/library/python/src/pywavemap.cc +++ b/library/python/src/pywavemap.cc @@ -7,6 +7,7 @@ #include "pywavemap/measurements.h" #include "pywavemap/param.h" #include "pywavemap/pipeline.h" +#include "pywavemap/raycast.h" using namespace wavemap; // NOLINT namespace nb = nanobind; @@ -53,4 +54,7 @@ NB_MODULE(_pywavemap_bindings, m) { // Bindings for measurement integration and map update pipelines add_pipeline_bindings(m); + + // Bindings for raycasting + add_raycast_bindings(m); } diff --git a/library/python/src/pywavemap/__init__.py b/library/python/src/pywavemap/__init__.py index f1db669c3..0c4078ab3 100644 --- a/library/python/src/pywavemap/__init__.py +++ b/library/python/src/pywavemap/__init__.py @@ -9,6 +9,7 @@ HashedChunkedWaveletOctree, InterpolationMode) from ._pywavemap_bindings import Pipeline +from ._pywavemap_bindings import raycast, PinholeCameraProjectorConfig, get_depth # Binding submodules from ._pywavemap_bindings import logging, param, convert diff --git a/library/python/src/raycast.cc b/library/python/src/raycast.cc new file mode 100644 index 000000000..9bf4cd694 --- /dev/null +++ b/library/python/src/raycast.cc @@ -0,0 +1,99 @@ +#include "pywavemap/raycast.h" + +#include // to use eigen2numpy seamlessly +#include +#include +#include +#include +#include "wavemap/core/utils/iterate/ray_iterator.h" +#include +#include + +using namespace nb::literals; // NOLINT + +namespace wavemap { +FloatingPoint raycast( + const HashedWaveletOctree& map, + Point3D start_point, + Point3D end_point, + FloatingPoint threshold +) { + const FloatingPoint mcw = map.getMinCellWidth(); + const Ray ray(start_point, end_point, mcw); + for (const Index3D& ray_voxel_index : ray) { + if (map.getCellValue(ray_voxel_index) > threshold) { + const Point3D voxel_center = convert::indexToCenterPoint(ray_voxel_index, mcw); + return (voxel_center - start_point).norm(); + } + } + return (end_point - start_point).norm(); +} + +FloatingPoint raycast_fast( + QueryAccelerator& query_accelerator, + Point3D start_point, + Point3D end_point, + FloatingPoint threshold, + FloatingPoint min_cell_width +) { + const Ray ray(start_point, end_point, min_cell_width); + for (const Index3D& ray_voxel_index : ray) { + if (query_accelerator.getCellValue(ray_voxel_index) > threshold) { + const Point3D voxel_center = convert::indexToCenterPoint(ray_voxel_index, min_cell_width); + return (voxel_center - start_point).norm(); + } + } + return (end_point - start_point).norm(); +} + +void add_raycast_bindings(nb::module_& m) { + nb::class_( + m, + "PinholeCameraProjectorConfig", + "Describes pinhole camera intrinsics" + ) + .def(nb::init(), "fx"_a, "fy"_a, "cx"_a, "cy"_a, "height"_a, "width"_a) + .def_rw("width", &PinholeCameraProjectorConfig::width) + .def_rw("height", &PinholeCameraProjectorConfig::height) + .def_rw("fx", &PinholeCameraProjectorConfig::fx) + .def_rw("fy", &PinholeCameraProjectorConfig::fy) + .def_rw("cx", &PinholeCameraProjectorConfig::cx) + .def_rw("cy", &PinholeCameraProjectorConfig::cy) + .def("__repr__", [](const PinholeCameraProjectorConfig& self) { + return nb::str("PinholeCameraProjectorConfig(width={}, height={}, fx={}, fy={}, cx={}, cy={})") + .format(self.width, self.height, self.fx, self.fy, self.cx, self.cy); + }); + + m.def( + "raycast", + &raycast, + "Raycast and get first point with occopancy higher than threshold" + ); + + m.def( + "raycast_fast", + &raycast_fast, // TODO: unusable without QueryAccelerator binding + "Raycast and get first point with occopancy higher than threshold using QueryAccelerator for efficiency" + ); + + m.def( + "get_depth", + [](const HashedWaveletOctree& map, Transformation3D pose, PinholeCameraProjectorConfig cam_cfg, FloatingPoint threshold, FloatingPoint max_range){ + Image depth_image(cam_cfg.width, cam_cfg.height); + QueryAccelerator query_accelerator(map); + const FloatingPoint mcw = map.getMinCellWidth(); + const PinholeCameraProjector projection_model(cam_cfg); + auto start_point = pose.getPosition(); + for (const Index2D& index: Grid<2>(Index2D::Zero(), depth_image.getDimensions() - Index2D::Ones())) { + const Vector2D image_xy = projection_model.indexToImage(index); + const Point3D C_point = projection_model.sensorToCartesian({image_xy, max_range}); + const Point3D end_point = pose * C_point; + FloatingPoint depth = raycast_fast(query_accelerator, start_point, end_point, threshold, mcw); + depth_image.at(index) = depth; + } + return depth_image.getData().transpose().eval(); + }, + "Extract depth from octree map at using given camera pose and intrinsics" + ); +} +} // namespace wavemap From c352f73b71a7250c491b1db662eda6ade253dc0f Mon Sep 17 00:00:00 2001 From: Victor Reijgwart Date: Thu, 24 Oct 2024 18:01:09 +0200 Subject: [PATCH 02/16] Run pre-commit and address warnings --- examples/python/raycast/depth_exctraction.py | 23 ++-- library/python/src/raycast.cc | 118 +++++++++---------- 2 files changed, 73 insertions(+), 68 deletions(-) diff --git a/examples/python/raycast/depth_exctraction.py b/examples/python/raycast/depth_exctraction.py index 068f56b7e..899bb552f 100644 --- a/examples/python/raycast/depth_exctraction.py +++ b/examples/python/raycast/depth_exctraction.py @@ -9,23 +9,27 @@ from PIL import Image import pywavemap as wm -def save_depth_as_png(depth_map: np.ndarray, out_path: Path): + +def save_depth_as_png(depth_map: np.ndarray, output_path: Path): depth_min = np.min(depth_map) depth_max = np.max(depth_map) # Avoid division by zero in case all values are the same if depth_max - depth_min > 0: - depth_map_normalized = (depth_map - depth_min) / (depth_max - depth_min) + depth_map_normalized = (depth_map - depth_min) / (depth_max - + depth_min) else: depth_map_normalized = np.zeros_like(depth_map) # Convert floats (meters) to uint8 and save to png depth_map_8bit = (depth_map_normalized * 255).astype(np.uint8) image = Image.fromarray(depth_map_8bit) - image.save(out_path) + image.save(output_path) + if __name__ == "__main__": - map_path = Path.home() / "data/panoptic_mapping/flat_dataset/run2/your_map.wvmp" + map_path = Path.home() \ + / "data/panoptic_mapping/flat_dataset/run2/your_map.wvmp" out_path = Path(__file__).parent / "depth.png" camera_cfg = wm.PinholeCameraProjectorConfig( width=1280, @@ -34,10 +38,10 @@ def save_depth_as_png(depth_map: np.ndarray, out_path: Path): fy=526.21539307, cx=642.309021, cy=368.69949341, - ) # Note: these are intrinsics for Zed 2i + ) # Note: these are intrinsics for Zed 2i # Load map from file - map = wm.Map.load(map_path) + your_map = wm.Map.load(map_path) # Create pose rotation = wm.Rotation(np.eye(3)) @@ -46,7 +50,8 @@ def save_depth_as_png(depth_map: np.ndarray, out_path: Path): # Extract depth t1 = time.perf_counter() - depth = wm.get_depth(map, pose, camera_cfg, 0.1, 10) + depth = wm.get_depth(your_map, pose, camera_cfg, 0.1, 10) t2 = time.perf_counter() - print(f"Depth map of size {camera_cfg.width}x{camera_cfg.height} created in {t2-t1:.02f} seconds") - save_depth_as_png(depth, out_path) \ No newline at end of file + print(f"Depth your_map of size {camera_cfg.width}x{camera_cfg.height} " + f"created in {t2 - t1:.02f} seconds") + save_depth_as_png(depth, out_path) diff --git a/library/python/src/raycast.cc b/library/python/src/raycast.cc index 9bf4cd694..f429f1c69 100644 --- a/library/python/src/raycast.cc +++ b/library/python/src/raycast.cc @@ -5,24 +5,22 @@ #include #include #include -#include "wavemap/core/utils/iterate/ray_iterator.h" -#include #include +#include + +#include "wavemap/core/utils/iterate/ray_iterator.h" using namespace nb::literals; // NOLINT namespace wavemap { -FloatingPoint raycast( - const HashedWaveletOctree& map, - Point3D start_point, - Point3D end_point, - FloatingPoint threshold -) { +FloatingPoint raycast(const HashedWaveletOctree& map, Point3D start_point, + Point3D end_point, FloatingPoint threshold) { const FloatingPoint mcw = map.getMinCellWidth(); const Ray ray(start_point, end_point, mcw); for (const Index3D& ray_voxel_index : ray) { if (map.getCellValue(ray_voxel_index) > threshold) { - const Point3D voxel_center = convert::indexToCenterPoint(ray_voxel_index, mcw); + const Point3D voxel_center = + convert::indexToCenterPoint(ray_voxel_index, mcw); return (voxel_center - start_point).norm(); } } @@ -30,16 +28,14 @@ FloatingPoint raycast( } FloatingPoint raycast_fast( - QueryAccelerator& query_accelerator, - Point3D start_point, - Point3D end_point, - FloatingPoint threshold, - FloatingPoint min_cell_width -) { + QueryAccelerator& query_accelerator, + Point3D start_point, Point3D end_point, FloatingPoint threshold, + FloatingPoint min_cell_width) { const Ray ray(start_point, end_point, min_cell_width); for (const Index3D& ray_voxel_index : ray) { if (query_accelerator.getCellValue(ray_voxel_index) > threshold) { - const Point3D voxel_center = convert::indexToCenterPoint(ray_voxel_index, min_cell_width); + const Point3D voxel_center = + convert::indexToCenterPoint(ray_voxel_index, min_cell_width); return (voxel_center - start_point).norm(); } } @@ -48,52 +44,56 @@ FloatingPoint raycast_fast( void add_raycast_bindings(nb::module_& m) { nb::class_( - m, - "PinholeCameraProjectorConfig", - "Describes pinhole camera intrinsics" - ) - .def(nb::init(), "fx"_a, "fy"_a, "cx"_a, "cy"_a, "height"_a, "width"_a) - .def_rw("width", &PinholeCameraProjectorConfig::width) - .def_rw("height", &PinholeCameraProjectorConfig::height) - .def_rw("fx", &PinholeCameraProjectorConfig::fx) - .def_rw("fy", &PinholeCameraProjectorConfig::fy) - .def_rw("cx", &PinholeCameraProjectorConfig::cx) - .def_rw("cy", &PinholeCameraProjectorConfig::cy) - .def("__repr__", [](const PinholeCameraProjectorConfig& self) { - return nb::str("PinholeCameraProjectorConfig(width={}, height={}, fx={}, fy={}, cx={}, cy={})") - .format(self.width, self.height, self.fx, self.fy, self.cx, self.cy); - }); + m, "PinholeCameraProjectorConfig", "Describes pinhole camera intrinsics") + .def(nb::init(), + "fx"_a, "fy"_a, "cx"_a, "cy"_a, "height"_a, "width"_a) + .def_rw("width", &PinholeCameraProjectorConfig::width) + .def_rw("height", &PinholeCameraProjectorConfig::height) + .def_rw("fx", &PinholeCameraProjectorConfig::fx) + .def_rw("fy", &PinholeCameraProjectorConfig::fy) + .def_rw("cx", &PinholeCameraProjectorConfig::cx) + .def_rw("cy", &PinholeCameraProjectorConfig::cy) + .def("__repr__", [](const PinholeCameraProjectorConfig& self) { + return nb::str( + "PinholeCameraProjectorConfig(width={}, height={}, fx={}, " + "fy={}, cx={}, cy={})") + .format(self.width, self.height, self.fx, self.fy, self.cx, + self.cy); + }); - m.def( - "raycast", - &raycast, - "Raycast and get first point with occopancy higher than threshold" - ); + m.def("raycast", &raycast, + "Raycast and get first point with occopancy higher than threshold"); - m.def( - "raycast_fast", - &raycast_fast, // TODO: unusable without QueryAccelerator binding - "Raycast and get first point with occopancy higher than threshold using QueryAccelerator for efficiency" - ); + m.def("raycast_fast", + &raycast_fast, // TODO: unusable without QueryAccelerator binding + "Raycast and get first point with occopancy higher than threshold " + "using QueryAccelerator for efficiency"); m.def( - "get_depth", - [](const HashedWaveletOctree& map, Transformation3D pose, PinholeCameraProjectorConfig cam_cfg, FloatingPoint threshold, FloatingPoint max_range){ - Image depth_image(cam_cfg.width, cam_cfg.height); - QueryAccelerator query_accelerator(map); - const FloatingPoint mcw = map.getMinCellWidth(); - const PinholeCameraProjector projection_model(cam_cfg); - auto start_point = pose.getPosition(); - for (const Index2D& index: Grid<2>(Index2D::Zero(), depth_image.getDimensions() - Index2D::Ones())) { - const Vector2D image_xy = projection_model.indexToImage(index); - const Point3D C_point = projection_model.sensorToCartesian({image_xy, max_range}); - const Point3D end_point = pose * C_point; - FloatingPoint depth = raycast_fast(query_accelerator, start_point, end_point, threshold, mcw); - depth_image.at(index) = depth; - } - return depth_image.getData().transpose().eval(); - }, - "Extract depth from octree map at using given camera pose and intrinsics" - ); + "get_depth", + [](const HashedWaveletOctree& map, Transformation3D pose, + PinholeCameraProjectorConfig cam_cfg, FloatingPoint threshold, + FloatingPoint max_range) { + Image depth_image(cam_cfg.width, cam_cfg.height); + QueryAccelerator query_accelerator(map); + const FloatingPoint mcw = map.getMinCellWidth(); + const PinholeCameraProjector projection_model(cam_cfg); + auto start_point = pose.getPosition(); + for (const Index2D& index : + Grid<2>(Index2D::Zero(), + depth_image.getDimensions() - Index2D::Ones())) { + const Vector2D image_xy = projection_model.indexToImage(index); + const Point3D C_point = + projection_model.sensorToCartesian({image_xy, max_range}); + const Point3D end_point = pose * C_point; + FloatingPoint depth = raycast_fast(query_accelerator, start_point, + end_point, threshold, mcw); + depth_image.at(index) = depth; + } + return depth_image.getData().transpose().eval(); + }, + "Extract depth from octree map at using given camera pose and " + "intrinsics"); } } // namespace wavemap From fea5914d14fccea42c41e2f78de881d47a83611b Mon Sep 17 00:00:00 2001 From: Victor Reijgwart Date: Thu, 24 Oct 2024 19:10:44 +0200 Subject: [PATCH 03/16] Draft support for multi-threading --- library/python/src/raycast.cc | 54 ++++++++++++++++++++++++----------- 1 file changed, 38 insertions(+), 16 deletions(-) diff --git a/library/python/src/raycast.cc b/library/python/src/raycast.cc index f429f1c69..e5539d85e 100644 --- a/library/python/src/raycast.cc +++ b/library/python/src/raycast.cc @@ -7,14 +7,16 @@ #include #include #include +#include #include "wavemap/core/utils/iterate/ray_iterator.h" using namespace nb::literals; // NOLINT namespace wavemap { -FloatingPoint raycast(const HashedWaveletOctree& map, Point3D start_point, - Point3D end_point, FloatingPoint threshold) { +FloatingPoint raycast(const HashedWaveletOctree& map, + const Point3D& start_point, const Point3D& end_point, + FloatingPoint threshold) { const FloatingPoint mcw = map.getMinCellWidth(); const Ray ray(start_point, end_point, mcw); for (const Index3D& ray_voxel_index : ray) { @@ -29,8 +31,8 @@ FloatingPoint raycast(const HashedWaveletOctree& map, Point3D start_point, FloatingPoint raycast_fast( QueryAccelerator& query_accelerator, - Point3D start_point, Point3D end_point, FloatingPoint threshold, - FloatingPoint min_cell_width) { + const Point3D& start_point, const Point3D& end_point, + FloatingPoint threshold, FloatingPoint min_cell_width) { const Ray ray(start_point, end_point, min_cell_width); for (const Index3D& ray_voxel_index : ray) { if (query_accelerator.getCellValue(ray_voxel_index) > threshold) { @@ -72,25 +74,45 @@ void add_raycast_bindings(nb::module_& m) { m.def( "get_depth", - [](const HashedWaveletOctree& map, Transformation3D pose, - PinholeCameraProjectorConfig cam_cfg, FloatingPoint threshold, + [](const HashedWaveletOctree& map, const Transformation3D& pose, + const PinholeCameraProjectorConfig& cam_cfg, FloatingPoint threshold, FloatingPoint max_range) { + // NOTE: This way of parallelizing it is not very efficient, as it + // creates a very large number of jobs (1 per pixel), but already + // leads to a nice speedup. The next step to improve it would + // probably be to split the image into tiles and spawn 1 job per + // tile. The tile size should be such that there are enough tiles + // to distribute the work evenly across all cores even if some + // tiles take much shorter than others, while still being few + // enough to minimize the overhead of dispatching jobs and create + // local QueryAccelerator instances for each job. Maybe 10x as + // many tiles as there are workers? + ThreadPool thread_pool; // By default, the pool will spawn as many + // workers as the system's reported + // std::thread::hardware_concurrency(). Image depth_image(cam_cfg.width, cam_cfg.height); - QueryAccelerator query_accelerator(map); - const FloatingPoint mcw = map.getMinCellWidth(); + const FloatingPoint min_cell_width = map.getMinCellWidth(); const PinholeCameraProjector projection_model(cam_cfg); - auto start_point = pose.getPosition(); + const Point3D& start_point = pose.getPosition(); for (const Index2D& index : Grid<2>(Index2D::Zero(), depth_image.getDimensions() - Index2D::Ones())) { - const Vector2D image_xy = projection_model.indexToImage(index); - const Point3D C_point = - projection_model.sensorToCartesian({image_xy, max_range}); - const Point3D end_point = pose * C_point; - FloatingPoint depth = raycast_fast(query_accelerator, start_point, - end_point, threshold, mcw); - depth_image.at(index) = depth; + FloatingPoint& depth_pixel = depth_image.at(index); + thread_pool.add_task([&map, &projection_model, &pose, &start_point, + &depth_pixel, index, max_range, threshold, + min_cell_width]() { + QueryAccelerator query_accelerator(map); + const Vector2D image_xy = projection_model.indexToImage(index); + const Point3D C_point = + projection_model.sensorToCartesian({image_xy, max_range}); + const Point3D end_point = pose * C_point; + FloatingPoint depth = + raycast_fast(query_accelerator, start_point, end_point, + threshold, min_cell_width); + depth_pixel = depth; + }); } + thread_pool.wait_all(); return depth_image.getData().transpose().eval(); }, "Extract depth from octree map at using given camera pose and " From 97525ec1718079db501ad0b6833568b927e17d69 Mon Sep 17 00:00:00 2001 From: Victor Reijgwart Date: Mon, 28 Oct 2024 15:51:38 +0100 Subject: [PATCH 04/16] Group parallel ray casts into tiles to reduce job dispatch overhead --- .../wavemap/core/utils/math/int_math.h | 10 +++ library/python/src/raycast.cc | 71 +++++++++---------- 2 files changed, 44 insertions(+), 37 deletions(-) diff --git a/library/cpp/include/wavemap/core/utils/math/int_math.h b/library/cpp/include/wavemap/core/utils/math/int_math.h index f48431b18..06c5cc05a 100644 --- a/library/cpp/include/wavemap/core/utils/math/int_math.h +++ b/library/cpp/include/wavemap/core/utils/math/int_math.h @@ -111,6 +111,16 @@ constexpr T div_round_up(T numerator, T denominator) { return numerator / denominator + (numerator % denominator != 0); } +template +Eigen::Matrix div_round_up(Eigen::Matrix vector, + int denominator) { + DCHECK_GE(denominator, 0); + for (int dim_idx = 0; dim_idx < dim; ++dim_idx) { + vector[dim_idx] = div_round_up(vector[dim_idx], denominator); + } + return vector; +} + template constexpr T factorial(T x) { T result = 1; diff --git a/library/python/src/raycast.cc b/library/python/src/raycast.cc index e5539d85e..adcb5d0b2 100644 --- a/library/python/src/raycast.cc +++ b/library/python/src/raycast.cc @@ -16,11 +16,11 @@ using namespace nb::literals; // NOLINT namespace wavemap { FloatingPoint raycast(const HashedWaveletOctree& map, const Point3D& start_point, const Point3D& end_point, - FloatingPoint threshold) { + FloatingPoint log_odds_threshold) { const FloatingPoint mcw = map.getMinCellWidth(); const Ray ray(start_point, end_point, mcw); for (const Index3D& ray_voxel_index : ray) { - if (map.getCellValue(ray_voxel_index) > threshold) { + if (log_odds_threshold < map.getCellValue(ray_voxel_index)) { const Point3D voxel_center = convert::indexToCenterPoint(ray_voxel_index, mcw); return (voxel_center - start_point).norm(); @@ -32,10 +32,11 @@ FloatingPoint raycast(const HashedWaveletOctree& map, FloatingPoint raycast_fast( QueryAccelerator& query_accelerator, const Point3D& start_point, const Point3D& end_point, - FloatingPoint threshold, FloatingPoint min_cell_width) { + FloatingPoint log_odds_threshold) { + const FloatingPoint min_cell_width = query_accelerator.getMinCellWidth(); const Ray ray(start_point, end_point, min_cell_width); for (const Index3D& ray_voxel_index : ray) { - if (query_accelerator.getCellValue(ray_voxel_index) > threshold) { + if (log_odds_threshold < query_accelerator.getCellValue(ray_voxel_index)) { const Point3D voxel_center = convert::indexToCenterPoint(ray_voxel_index, min_cell_width); return (voxel_center - start_point).norm(); @@ -75,44 +76,40 @@ void add_raycast_bindings(nb::module_& m) { m.def( "get_depth", [](const HashedWaveletOctree& map, const Transformation3D& pose, - const PinholeCameraProjectorConfig& cam_cfg, FloatingPoint threshold, - FloatingPoint max_range) { - // NOTE: This way of parallelizing it is not very efficient, as it - // creates a very large number of jobs (1 per pixel), but already - // leads to a nice speedup. The next step to improve it would - // probably be to split the image into tiles and spawn 1 job per - // tile. The tile size should be such that there are enough tiles - // to distribute the work evenly across all cores even if some - // tiles take much shorter than others, while still being few - // enough to minimize the overhead of dispatching jobs and create - // local QueryAccelerator instances for each job. Maybe 10x as - // many tiles as there are workers? - ThreadPool thread_pool; // By default, the pool will spawn as many - // workers as the system's reported - // std::thread::hardware_concurrency(). - Image depth_image(cam_cfg.width, cam_cfg.height); - const FloatingPoint min_cell_width = map.getMinCellWidth(); + const PinholeCameraProjectorConfig& cam_cfg, + FloatingPoint log_odds_threshold, FloatingPoint max_range) { const PinholeCameraProjector projection_model(cam_cfg); - const Point3D& start_point = pose.getPosition(); - for (const Index2D& index : - Grid<2>(Index2D::Zero(), - depth_image.getDimensions() - Index2D::Ones())) { - FloatingPoint& depth_pixel = depth_image.at(index); - thread_pool.add_task([&map, &projection_model, &pose, &start_point, - &depth_pixel, index, max_range, threshold, - min_cell_width]() { + Image depth_image(projection_model.getDimensions()); + const Point3D& W_start_point = pose.getPosition(); + + ThreadPool thread_pool; + constexpr int kPatchWidth = 64; + const Index2D num_patches = + int_math::div_round_up(depth_image.getDimensions(), kPatchWidth); + for (const Index2D& patch_index : + Grid<2>(Index2D::Zero(), num_patches - Index2D::Ones())) { + thread_pool.add_task([&map, &projection_model, &pose, &W_start_point, + &depth_image, patch_index, kPatchWidth, + max_range, log_odds_threshold]() { QueryAccelerator query_accelerator(map); - const Vector2D image_xy = projection_model.indexToImage(index); - const Point3D C_point = - projection_model.sensorToCartesian({image_xy, max_range}); - const Point3D end_point = pose * C_point; - FloatingPoint depth = - raycast_fast(query_accelerator, start_point, end_point, - threshold, min_cell_width); - depth_pixel = depth; + const Index2D patch_min = patch_index * kPatchWidth; + const Index2D patch_max = + (patch_min.array() + kPatchWidth) + .min(depth_image.getDimensions().array()) - + 1; + for (const Index2D& index : Grid<2>(patch_min, patch_max)) { + FloatingPoint& depth_pixel = depth_image.at(index); + const Vector2D image_xy = projection_model.indexToImage(index); + const Point3D C_end_point = + projection_model.sensorToCartesian({image_xy, max_range}); + const Point3D W_end_point = pose * C_end_point; + depth_pixel = raycast_fast(query_accelerator, W_start_point, + W_end_point, log_odds_threshold); + } }); } thread_pool.wait_all(); + return depth_image.getData().transpose().eval(); }, "Extract depth from octree map at using given camera pose and " From b6a10c9e5e1c0a21ae4095571a6600c5b711fc0e Mon Sep 17 00:00:00 2001 From: Victor Reijgwart Date: Thu, 5 Dec 2024 14:19:40 +0100 Subject: [PATCH 05/16] Cleaner handling of features provided by optional dependencies in CMake --- interfaces/ros1/wavemap_ros/CMakeLists.txt | 18 ++++++++++-------- 1 file changed, 10 insertions(+), 8 deletions(-) diff --git a/interfaces/ros1/wavemap_ros/CMakeLists.txt b/interfaces/ros1/wavemap_ros/CMakeLists.txt index e90bc7449..8987c6e1e 100644 --- a/interfaces/ros1/wavemap_ros/CMakeLists.txt +++ b/interfaces/ros1/wavemap_ros/CMakeLists.txt @@ -10,6 +10,9 @@ find_package(catkin REQUIRED COMPONENTS roscpp rosbag cv_bridge image_transport tf2_ros std_srvs sensor_msgs visualization_msgs) +# Optional dependencies +find_package(livox_ros_driver2 QUIET) + # Register catkin package catkin_package( INCLUDE_DIRS include @@ -19,14 +22,6 @@ catkin_package( roscpp rosbag cv_bridge image_transport tf2_ros std_srvs sensor_msgs visualization_msgs) - -# Optional dependencies -find_package(livox_ros_driver2 QUIET) -if (livox_ros_driver2_FOUND) - include_directories(${livox_ros_driver2_INCLUDE_DIRS}) - add_compile_definitions(LIVOX_AVAILABLE) -endif () - # Libraries add_library(${PROJECT_NAME} src/inputs/depth_image_topic_input.cc @@ -48,6 +43,13 @@ target_include_directories(${PROJECT_NAME} target_link_libraries(${PROJECT_NAME} PUBLIC ${catkin_LIBRARIES} ${OpenCV_LIBRARIES}) +# Optional Livox support +if (livox_ros_driver2_FOUND) + target_include_directories(${PROJECT_NAME} PUBLIC + ${livox_ros_driver2_INCLUDE_DIRS}) + target_compile_definitions(${PROJECT_NAME} PUBLIC LIVOX_AVAILABLE) +endif () + # Binaries add_executable(ros_server app/ros_server.cc) set_wavemap_target_properties(ros_server) From 385961cf7d30595eb3ee967fbd07bc1dd7cd99aa Mon Sep 17 00:00:00 2001 From: Victor Reijgwart Date: Thu, 5 Dec 2024 14:26:05 +0100 Subject: [PATCH 06/16] Refactor param loaders and converters for consistency and extensibility --- interfaces/ros1/wavemap_ros/src/ros_server.cc | 19 ++-- .../config_conversions.h | 16 +-- .../src/config_conversions.cc | 102 +++++++++--------- .../src/wavemap_map_display.cc | 2 +- .../wavemap/io/{ => map}/file_conversions.h | 7 +- .../io/{ => map}/impl/streamable_types_impl.h | 6 +- .../wavemap/io/{ => map}/stream_conversions.h | 8 +- .../wavemap/io/{ => map}/streamable_types.h | 8 +- library/cpp/src/io/CMakeLists.txt | 4 +- .../cpp/src/io/{ => map}/file_conversions.cc | 4 +- .../src/io/{ => map}/stream_conversions.cc | 4 +- .../cpp/test/src/io/test_file_conversions.cc | 3 +- library/python/include/pywavemap/param.h | 6 +- library/python/src/param.cc | 29 +++-- 14 files changed, 110 insertions(+), 108 deletions(-) rename library/cpp/include/wavemap/io/{ => map}/file_conversions.h (62%) rename library/cpp/include/wavemap/io/{ => map}/impl/streamable_types_impl.h (97%) rename library/cpp/include/wavemap/io/{ => map}/stream_conversions.h (80%) rename library/cpp/include/wavemap/io/{ => map}/streamable_types.h (93%) rename library/cpp/src/io/{ => map}/file_conversions.cc (94%) rename library/cpp/src/io/{ => map}/stream_conversions.cc (99%) diff --git a/interfaces/ros1/wavemap_ros/src/ros_server.cc b/interfaces/ros1/wavemap_ros/src/ros_server.cc index 3d94171a4..054d285c2 100644 --- a/interfaces/ros1/wavemap_ros/src/ros_server.cc +++ b/interfaces/ros1/wavemap_ros/src/ros_server.cc @@ -5,7 +5,7 @@ #include #include -#include +#include #include #include #include @@ -33,10 +33,10 @@ bool RosServerConfig::isValid(bool verbose) const { // NOTE: If RosServerConfig::from(...) fails, accessing its value will throw // an exception and end the program. RosServer::RosServer(ros::NodeHandle nh, ros::NodeHandle nh_private) - : RosServer(nh, nh_private, - RosServerConfig::from( - param::convert::toParamValue(nh_private, "general")) - .value()) {} + : RosServer( + nh, nh_private, + RosServerConfig::from(convert::rosToParams(nh_private, "general")) + .value()) {} RosServer::RosServer(ros::NodeHandle nh, ros::NodeHandle nh_private, const RosServerConfig& config) @@ -47,8 +47,7 @@ RosServer::RosServer(ros::NodeHandle nh, ros::NodeHandle nh_private, config_.logging_level.applyToRosConsole(); // Setup data structure - const auto data_structure_params = - param::convert::toParamValue(nh_private, "map"); + const auto data_structure_params = convert::rosToParams(nh_private, "map"); occupancy_map_ = MapFactory::create(data_structure_params, MapType::kHashedBlocks); CHECK_NOTNULL(occupancy_map_); @@ -65,14 +64,14 @@ RosServer::RosServer(ros::NodeHandle nh, ros::NodeHandle nh_private, // Add map operations to pipeline const param::Array map_operation_param_array = - param::convert::toParamArray(nh_private, "map_operations"); + convert::rosToParamArray(nh_private, "map_operations"); for (const auto& operation_params : map_operation_param_array) { addOperation(operation_params, nh_private); } // Add measurement integrators to pipeline const param::Map measurement_integrator_param_map = - param::convert::toParamMap(nh_private, "measurement_integrators"); + convert::rosToParamMap(nh_private, "measurement_integrators"); for (const auto& [integrator_name, integrator_params] : measurement_integrator_param_map) { pipeline_->addIntegrator(integrator_name, integrator_params); @@ -80,7 +79,7 @@ RosServer::RosServer(ros::NodeHandle nh, ros::NodeHandle nh_private, // Setup measurement inputs const param::Array input_param_array = - param::convert::toParamArray(nh_private, "inputs"); + convert::rosToParamArray(nh_private, "inputs"); for (const auto& integrator_params : input_param_array) { addInput(integrator_params, nh, nh_private); } diff --git a/interfaces/ros1/wavemap_ros_conversions/include/wavemap_ros_conversions/config_conversions.h b/interfaces/ros1/wavemap_ros_conversions/include/wavemap_ros_conversions/config_conversions.h index 42204b47d..0031cb376 100644 --- a/interfaces/ros1/wavemap_ros_conversions/include/wavemap_ros_conversions/config_conversions.h +++ b/interfaces/ros1/wavemap_ros_conversions/include/wavemap_ros_conversions/config_conversions.h @@ -7,14 +7,14 @@ #include #include -namespace wavemap::param::convert { -param::Map toParamMap(const ros::NodeHandle& nh, const std::string& ns); -param::Array toParamArray(const ros::NodeHandle& nh, const std::string& ns); -param::Value toParamValue(const ros::NodeHandle& nh, const std::string& ns); +namespace wavemap::convert { +param::Map xmlRpcToParamMap(const XmlRpc::XmlRpcValue& xml_rpc_value); +param::Array xmlRpcToParamArray(const XmlRpc::XmlRpcValue& xml_rpc_value); +param::Value xmlRpcToParams(const XmlRpc::XmlRpcValue& xml_rpc_value); -param::Map toParamMap(const XmlRpc::XmlRpcValue& xml_rpc_value); -param::Array toParamArray(const XmlRpc::XmlRpcValue& xml_rpc_value); -param::Value toParamValue(const XmlRpc::XmlRpcValue& xml_rpc_value); -} // namespace wavemap::param::convert +param::Map rosToParamMap(const ros::NodeHandle& nh, const std::string& ns); +param::Array rosToParamArray(const ros::NodeHandle& nh, const std::string& ns); +param::Value rosToParams(const ros::NodeHandle& nh, const std::string& ns); +} // namespace wavemap::convert #endif // WAVEMAP_ROS_CONVERSIONS_CONFIG_CONVERSIONS_H_ diff --git a/interfaces/ros1/wavemap_ros_conversions/src/config_conversions.cc b/interfaces/ros1/wavemap_ros_conversions/src/config_conversions.cc index b321cb02e..256b74211 100644 --- a/interfaces/ros1/wavemap_ros_conversions/src/config_conversions.cc +++ b/interfaces/ros1/wavemap_ros_conversions/src/config_conversions.cc @@ -2,84 +2,51 @@ #include -namespace wavemap::param::convert { -param::Map toParamMap(const ros::NodeHandle& nh, const std::string& ns) { - XmlRpc::XmlRpcValue xml_rpc_value; - if (nh.getParam(ns, xml_rpc_value)) { - return toParamMap(xml_rpc_value); - } - - ROS_WARN_STREAM("Could not load ROS params under namespace " - << nh.resolveName(ns)); - return {}; -} - -param::Array toParamArray(const ros::NodeHandle& nh, const std::string& ns) { - XmlRpc::XmlRpcValue xml_rpc_value; - if (nh.getParam(ns, xml_rpc_value)) { - return toParamArray(xml_rpc_value); - } - - ROS_WARN_STREAM("Could not load ROS params under namespace " - << nh.resolveName(ns)); - return {}; -} - -param::Value toParamValue(const ros::NodeHandle& nh, const std::string& ns) { - XmlRpc::XmlRpcValue xml_rpc_value; - if (nh.getParam(ns, xml_rpc_value)) { - return toParamValue(xml_rpc_value); - } - - ROS_WARN_STREAM("Could not load ROS params under namespace " - << nh.resolveName(ns)); - return param::Value{param::Map{}}; // Return an empty map -} - -param::Map toParamMap( // NOLINT +namespace wavemap::convert { +param::Map xmlRpcToParamMap( // NOLINT const XmlRpc::XmlRpcValue& xml_rpc_value) { if (xml_rpc_value.getType() != XmlRpc::XmlRpcValue::TypeStruct) { - ROS_WARN("Expected param map."); + ROS_WARN("Expected ROS param map."); return {}; } param::Map param_map; for (const auto& kv : xml_rpc_value) { - param_map.emplace(kv.first, toParamValue(kv.second)); + param_map.emplace(kv.first, xmlRpcToParams(kv.second)); } return param_map; } -param::Array toParamArray( // NOLINT +param::Array xmlRpcToParamArray( // NOLINT const XmlRpc::XmlRpcValue& xml_rpc_value) { if (xml_rpc_value.getType() != XmlRpc::XmlRpcValue::TypeArray) { - ROS_WARN("Expected param array."); + ROS_WARN("Expected ROS param array."); return {}; } param::Array array; array.reserve(xml_rpc_value.size()); for (int idx = 0; idx < xml_rpc_value.size(); ++idx) { // NOLINT - array.template emplace_back(toParamValue(xml_rpc_value[idx])); + array.emplace_back(xmlRpcToParams(xml_rpc_value[idx])); } return array; } -param::Value toParamValue( // NOLINT +param::Value xmlRpcToParams( // NOLINT const XmlRpc::XmlRpcValue& xml_rpc_value) { switch (xml_rpc_value.getType()) { + case XmlRpc::XmlRpcValue::TypeStruct: + return param::Value{xmlRpcToParamMap(xml_rpc_value)}; + case XmlRpc::XmlRpcValue::TypeArray: + return param::Value{xmlRpcToParamArray(xml_rpc_value)}; case XmlRpc::XmlRpcValue::TypeBoolean: - return param::Value(static_cast(xml_rpc_value)); + return param::Value{static_cast(xml_rpc_value)}; case XmlRpc::XmlRpcValue::TypeInt: - return param::Value(static_cast(xml_rpc_value)); + return param::Value{static_cast(xml_rpc_value)}; case XmlRpc::XmlRpcValue::TypeDouble: - return param::Value(static_cast(xml_rpc_value)); + return param::Value{static_cast(xml_rpc_value)}; case XmlRpc::XmlRpcValue::TypeString: - return param::Value(static_cast(xml_rpc_value)); - case XmlRpc::XmlRpcValue::TypeArray: - return param::Value(toParamArray(xml_rpc_value)); - case XmlRpc::XmlRpcValue::TypeStruct: - return param::Value(toParamMap(xml_rpc_value)); + return param::Value{static_cast(xml_rpc_value)}; case XmlRpc::XmlRpcValue::TypeInvalid: ROS_ERROR("Encountered invalid type while parsing ROS params."); break; @@ -99,6 +66,39 @@ param::Value toParamValue( // NOLINT } // On error, return an empty array - return param::Value(param::Array{}); + return param::Value{param::Array{}}; +} + +param::Map rosToParamMap(const ros::NodeHandle& nh, const std::string& ns) { + XmlRpc::XmlRpcValue xml_rpc_value; + if (nh.getParam(ns, xml_rpc_value)) { + return xmlRpcToParamMap(xml_rpc_value); + } + + ROS_WARN_STREAM("Could not load ROS params under namespace " + << nh.resolveName(ns)); + return {}; +} + +param::Array rosToParamArray(const ros::NodeHandle& nh, const std::string& ns) { + XmlRpc::XmlRpcValue xml_rpc_value; + if (nh.getParam(ns, xml_rpc_value)) { + return xmlRpcToParamArray(xml_rpc_value); + } + + ROS_WARN_STREAM("Could not load ROS params under namespace " + << nh.resolveName(ns)); + return {}; +} + +param::Value rosToParams(const ros::NodeHandle& nh, const std::string& ns) { + XmlRpc::XmlRpcValue xml_rpc_value; + if (nh.getParam(ns, xml_rpc_value)) { + return xmlRpcToParams(xml_rpc_value); + } + + ROS_WARN_STREAM("Could not load ROS params under namespace " + << nh.resolveName(ns)); + return param::Value{param::Map{}}; // Return an empty map } -} // namespace wavemap::param::convert +} // namespace wavemap::convert 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..bcf61337c 100644 --- a/interfaces/ros1/wavemap_rviz_plugin/src/wavemap_map_display.cc +++ b/interfaces/ros1/wavemap_rviz_plugin/src/wavemap_map_display.cc @@ -10,7 +10,7 @@ #include #include #include -#include +#include #include #include "wavemap_rviz_plugin/utils/alert_dialog.h" diff --git a/library/cpp/include/wavemap/io/file_conversions.h b/library/cpp/include/wavemap/io/map/file_conversions.h similarity index 62% rename from library/cpp/include/wavemap/io/file_conversions.h rename to library/cpp/include/wavemap/io/map/file_conversions.h index 6e0cb70f2..c34b5ab72 100644 --- a/library/cpp/include/wavemap/io/file_conversions.h +++ b/library/cpp/include/wavemap/io/map/file_conversions.h @@ -1,14 +1,13 @@ -#ifndef WAVEMAP_IO_FILE_CONVERSIONS_H_ -#define WAVEMAP_IO_FILE_CONVERSIONS_H_ +#ifndef WAVEMAP_IO_MAP_FILE_CONVERSIONS_H_ +#define WAVEMAP_IO_MAP_FILE_CONVERSIONS_H_ #include #include "wavemap/core/map/map_base.h" -#include "wavemap/io/stream_conversions.h" 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); } // namespace wavemap::io -#endif // WAVEMAP_IO_FILE_CONVERSIONS_H_ +#endif // WAVEMAP_IO_MAP_FILE_CONVERSIONS_H_ diff --git a/library/cpp/include/wavemap/io/impl/streamable_types_impl.h b/library/cpp/include/wavemap/io/map/impl/streamable_types_impl.h similarity index 97% rename from library/cpp/include/wavemap/io/impl/streamable_types_impl.h rename to library/cpp/include/wavemap/io/map/impl/streamable_types_impl.h index 249437a04..6722a3c66 100644 --- a/library/cpp/include/wavemap/io/impl/streamable_types_impl.h +++ b/library/cpp/include/wavemap/io/map/impl/streamable_types_impl.h @@ -1,5 +1,5 @@ -#ifndef WAVEMAP_IO_IMPL_STREAMABLE_TYPES_IMPL_H_ -#define WAVEMAP_IO_IMPL_STREAMABLE_TYPES_IMPL_H_ +#ifndef WAVEMAP_IO_MAP_IMPL_STREAMABLE_TYPES_IMPL_H_ +#define WAVEMAP_IO_MAP_IMPL_STREAMABLE_TYPES_IMPL_H_ namespace wavemap::io::streamable { void Index3D::write(std::ostream& ostream) const { @@ -157,4 +157,4 @@ StorageFormat StorageFormat::peek(std::istream& istream) { } } // namespace wavemap::io::streamable -#endif // WAVEMAP_IO_IMPL_STREAMABLE_TYPES_IMPL_H_ +#endif // WAVEMAP_IO_MAP_IMPL_STREAMABLE_TYPES_IMPL_H_ diff --git a/library/cpp/include/wavemap/io/stream_conversions.h b/library/cpp/include/wavemap/io/map/stream_conversions.h similarity index 80% rename from library/cpp/include/wavemap/io/stream_conversions.h rename to library/cpp/include/wavemap/io/map/stream_conversions.h index f3341999b..3c02b226f 100644 --- a/library/cpp/include/wavemap/io/stream_conversions.h +++ b/library/cpp/include/wavemap/io/map/stream_conversions.h @@ -1,16 +1,14 @@ -#ifndef WAVEMAP_IO_STREAM_CONVERSIONS_H_ -#define WAVEMAP_IO_STREAM_CONVERSIONS_H_ +#ifndef WAVEMAP_IO_MAP_STREAM_CONVERSIONS_H_ +#define WAVEMAP_IO_MAP_STREAM_CONVERSIONS_H_ #include #include #include "wavemap/core/common.h" -#include "wavemap/core/map/cell_types/haar_coefficients.h" #include "wavemap/core/map/hashed_blocks.h" #include "wavemap/core/map/hashed_chunked_wavelet_octree.h" #include "wavemap/core/map/hashed_wavelet_octree.h" #include "wavemap/core/map/wavelet_octree.h" -#include "wavemap/io/streamable_types.h" namespace wavemap::io { bool mapToStream(const MapBase& map, std::ostream& ostream); @@ -28,4 +26,4 @@ bool streamToMap(std::istream& istream, HashedWaveletOctree::Ptr& map); bool mapToStream(const HashedChunkedWaveletOctree& map, std::ostream& ostream); } // namespace wavemap::io -#endif // WAVEMAP_IO_STREAM_CONVERSIONS_H_ +#endif // WAVEMAP_IO_MAP_STREAM_CONVERSIONS_H_ diff --git a/library/cpp/include/wavemap/io/streamable_types.h b/library/cpp/include/wavemap/io/map/streamable_types.h similarity index 93% rename from library/cpp/include/wavemap/io/streamable_types.h rename to library/cpp/include/wavemap/io/map/streamable_types.h index 3a82d09c5..f27eb54a7 100644 --- a/library/cpp/include/wavemap/io/streamable_types.h +++ b/library/cpp/include/wavemap/io/map/streamable_types.h @@ -1,5 +1,5 @@ -#ifndef WAVEMAP_IO_STREAMABLE_TYPES_H_ -#define WAVEMAP_IO_STREAMABLE_TYPES_H_ +#ifndef WAVEMAP_IO_MAP_STREAMABLE_TYPES_H_ +#define WAVEMAP_IO_MAP_STREAMABLE_TYPES_H_ #include #include @@ -99,6 +99,6 @@ struct StorageFormat : TypeSelector { }; } // namespace wavemap::io::streamable -#include "wavemap/io/impl/streamable_types_impl.h" +#include "wavemap/io/map/impl/streamable_types_impl.h" -#endif // WAVEMAP_IO_STREAMABLE_TYPES_H_ +#endif // WAVEMAP_IO_MAP_STREAMABLE_TYPES_H_ diff --git a/library/cpp/src/io/CMakeLists.txt b/library/cpp/src/io/CMakeLists.txt index 8a8fb375c..ea5d08d58 100644 --- a/library/cpp/src/io/CMakeLists.txt +++ b/library/cpp/src/io/CMakeLists.txt @@ -8,7 +8,9 @@ add_wavemap_include_directories(wavemap_io) target_link_libraries(wavemap_io PUBLIC Eigen3::Eigen glog wavemap_core) # Set sources -target_sources(wavemap_io PRIVATE file_conversions.cc stream_conversions.cc) +target_sources(wavemap_io PRIVATE + map/file_conversions.cc + map/stream_conversions.cc) # Support installs if (GENERATE_WAVEMAP_INSTALL_RULES) diff --git a/library/cpp/src/io/file_conversions.cc b/library/cpp/src/io/map/file_conversions.cc similarity index 94% rename from library/cpp/src/io/file_conversions.cc rename to library/cpp/src/io/map/file_conversions.cc index 0c8fa9aba..16508a156 100644 --- a/library/cpp/src/io/file_conversions.cc +++ b/library/cpp/src/io/map/file_conversions.cc @@ -1,7 +1,9 @@ -#include "wavemap/io/file_conversions.h" +#include "wavemap/io/map/file_conversions.h" #include +#include "wavemap/io/map/stream_conversions.h" + namespace wavemap::io { bool mapToFile(const MapBase& map, const std::filesystem::path& file_path) { if (file_path.empty()) { diff --git a/library/cpp/src/io/stream_conversions.cc b/library/cpp/src/io/map/stream_conversions.cc similarity index 99% rename from library/cpp/src/io/stream_conversions.cc rename to library/cpp/src/io/map/stream_conversions.cc index 364260678..af2cbaa80 100644 --- a/library/cpp/src/io/stream_conversions.cc +++ b/library/cpp/src/io/map/stream_conversions.cc @@ -1,9 +1,11 @@ -#include "wavemap/io/stream_conversions.h" +#include "wavemap/io/map/stream_conversions.h" #include #include #include +#include "wavemap/io/map/streamable_types.h" + namespace wavemap::io { bool mapToStream(const MapBase& map, std::ostream& ostream) { // Call the appropriate mapToStream converter based on the map's derived type diff --git a/library/cpp/test/src/io/test_file_conversions.cc b/library/cpp/test/src/io/test_file_conversions.cc index 44cd077ff..6e7753c12 100644 --- a/library/cpp/test/src/io/test_file_conversions.cc +++ b/library/cpp/test/src/io/test_file_conversions.cc @@ -4,11 +4,12 @@ #include #include "wavemap/core/common.h" +#include "wavemap/core/map/hashed_blocks.h" #include "wavemap/core/map/hashed_chunked_wavelet_octree.h" #include "wavemap/core/map/hashed_wavelet_octree.h" #include "wavemap/core/map/map_base.h" #include "wavemap/core/map/wavelet_octree.h" -#include "wavemap/io/file_conversions.h" +#include "wavemap/io/map/file_conversions.h" #include "wavemap/test/config_generator.h" #include "wavemap/test/fixture_base.h" #include "wavemap/test/geometry_generator.h" diff --git a/library/python/include/pywavemap/param.h b/library/python/include/pywavemap/param.h index 27ff9f56f..e103519c6 100644 --- a/library/python/include/pywavemap/param.h +++ b/library/python/include/pywavemap/param.h @@ -8,9 +8,9 @@ namespace nb = nanobind; namespace wavemap { namespace convert { -param::Map toParamMap(const nb::handle& py_value); -param::Array toParamArray(const nb::handle& py_value); -param::Value toParamValue(const nb::handle& py_value); +param::Map pyToParamMap(const nb::handle& py_value); +param::Array pyToParamArray(const nb::handle& py_value); +param::Value pyToParams(const nb::handle& py_value); } // namespace convert void add_param_module(nb::module_& m_param); diff --git a/library/python/src/param.cc b/library/python/src/param.cc index 734efeb6f..fe7c79afa 100644 --- a/library/python/src/param.cc +++ b/library/python/src/param.cc @@ -4,7 +4,7 @@ namespace wavemap { namespace convert { -param::Map toParamMap(const nb::handle& py_value) { // NOLINT +param::Map pyToParamMap(const nb::handle& py_value) { // NOLINT nb::dict py_dict; if (!nb::try_cast(py_value, py_dict)) { LOG(WARNING) << "Expected python dict, but got " @@ -14,9 +14,8 @@ param::Map toParamMap(const nb::handle& py_value) { // NOLINT param::Map param_map; for (const auto& [py_key, py_dict_value] : py_dict) { - nb::str py_key_str; - if (nb::try_cast(py_key, py_key_str)) { - param_map.emplace(py_key_str.c_str(), toParamValue(py_dict_value)); + if (nb::str py_key_str; nb::try_cast(py_key, py_key_str)) { + param_map.emplace(py_key_str.c_str(), pyToParams(py_dict_value)); } else { LOG(WARNING) << "Ignoring dict entry. Key not convertible to string for " "element with key " @@ -27,7 +26,7 @@ param::Map toParamMap(const nb::handle& py_value) { // NOLINT return param_map; } -param::Array toParamArray(const nb::handle& py_value) { // NOLINT +param::Array pyToParamArray(const nb::handle& py_value) { // NOLINT nb::list py_list; if (!nb::try_cast(py_value, py_list)) { LOG(WARNING) << "Expected python list, but got " @@ -37,13 +36,19 @@ param::Array toParamArray(const nb::handle& py_value) { // NOLINT param::Array array; array.reserve(nb::len(py_list)); - for (const auto& py_element : py_list) { // NOLINT - array.emplace_back(toParamValue(py_element)); + for (const auto& py_element : py_list) { + array.emplace_back(pyToParams(py_element)); } return array; } -param::Value toParamValue(const nb::handle& py_value) { // NOLINT +param::Value pyToParams(const nb::handle& py_value) { // NOLINT + if (nb::isinstance(py_value)) { + return param::Value(pyToParamMap(py_value)); + } + if (nb::isinstance(py_value)) { + return param::Value(pyToParamArray(py_value)); + } if (nb::bool_ py_bool; nb::try_cast(py_value, py_bool)) { return param::Value{static_cast(py_bool)}; } @@ -56,12 +61,6 @@ param::Value toParamValue(const nb::handle& py_value) { // NOLINT if (nb::str py_str; nb::try_cast(py_value, py_str)) { return param::Value{std::string{py_str.c_str()}}; } - if (nb::isinstance(py_value)) { - return param::Value(toParamArray(py_value)); - } - if (nb::isinstance(py_value)) { - return param::Value(toParamMap(py_value)); - } // On error, return an empty array LOG(ERROR) << "Encountered unsupported type while parsing python param " @@ -78,7 +77,7 @@ void add_param_module(nb::module_& m_param) { "can therefore hold the information needed to initialize an entire " "config, or even a hierarchy of nested configs.") .def("__init__", [](param::Value* t, nb::handle py_value) { - new (t) param::Value{convert::toParamValue(py_value)}; + new (t) param::Value{convert::pyToParams(py_value)}; }); nb::implicitly_convertible(); From d4e8edbc91696e7d1b84abe227c31a5e35e1d02e Mon Sep 17 00:00:00 2001 From: Victor Reijgwart Date: Thu, 5 Dec 2024 14:41:07 +0100 Subject: [PATCH 07/16] Add optional yaml parsing support Currently, support is automatically enabled if yaml-cpp is available. However, the system is modular and can easily be extended to work with additional parsers. --- library/cpp/cmake/find-wavemap-deps.cmake | 1 + .../wavemap/io/config/file_conversions.h | 13 ++++ .../wavemap/io/config/stream_conversions.h | 12 +++ .../wavemap/io/config/yaml_cpp_conversions.h | 13 ++++ library/cpp/src/io/CMakeLists.txt | 10 +++ library/cpp/src/io/config/file_conversions.cc | 33 ++++++++ .../cpp/src/io/config/stream_conversions.cc | 25 ++++++ .../cpp/src/io/config/yaml_cpp_conversions.cc | 76 +++++++++++++++++++ 8 files changed, 183 insertions(+) create mode 100644 library/cpp/include/wavemap/io/config/file_conversions.h create mode 100644 library/cpp/include/wavemap/io/config/stream_conversions.h create mode 100644 library/cpp/include/wavemap/io/config/yaml_cpp_conversions.h create mode 100644 library/cpp/src/io/config/file_conversions.cc create mode 100644 library/cpp/src/io/config/stream_conversions.cc create mode 100644 library/cpp/src/io/config/yaml_cpp_conversions.cc diff --git a/library/cpp/cmake/find-wavemap-deps.cmake b/library/cpp/cmake/find-wavemap-deps.cmake index 695f0c98e..367f9068e 100644 --- a/library/cpp/cmake/find-wavemap-deps.cmake +++ b/library/cpp/cmake/find-wavemap-deps.cmake @@ -56,3 +56,4 @@ endif () # Optional dependencies find_package(tracy QUIET) +find_package(yaml-cpp QUIET) diff --git a/library/cpp/include/wavemap/io/config/file_conversions.h b/library/cpp/include/wavemap/io/config/file_conversions.h new file mode 100644 index 000000000..d7073a26a --- /dev/null +++ b/library/cpp/include/wavemap/io/config/file_conversions.h @@ -0,0 +1,13 @@ +#ifndef WAVEMAP_IO_CONFIG_FILE_CONVERSIONS_H_ +#define WAVEMAP_IO_CONFIG_FILE_CONVERSIONS_H_ + +#include + +#include + +namespace wavemap::io { +bool yamlFileToParams(const std::filesystem::path& file_path, + param::Value& params); +} // namespace wavemap::io + +#endif // WAVEMAP_IO_CONFIG_FILE_CONVERSIONS_H_ diff --git a/library/cpp/include/wavemap/io/config/stream_conversions.h b/library/cpp/include/wavemap/io/config/stream_conversions.h new file mode 100644 index 000000000..0c9ba5bad --- /dev/null +++ b/library/cpp/include/wavemap/io/config/stream_conversions.h @@ -0,0 +1,12 @@ +#ifndef WAVEMAP_IO_CONFIG_STREAM_CONVERSIONS_H_ +#define WAVEMAP_IO_CONFIG_STREAM_CONVERSIONS_H_ + +#include + +#include + +namespace wavemap::io { +bool yamlStreamToParams(std::istream& istream, param::Value& params); +} + +#endif // WAVEMAP_IO_CONFIG_STREAM_CONVERSIONS_H_ diff --git a/library/cpp/include/wavemap/io/config/yaml_cpp_conversions.h b/library/cpp/include/wavemap/io/config/yaml_cpp_conversions.h new file mode 100644 index 000000000..3f26c50d7 --- /dev/null +++ b/library/cpp/include/wavemap/io/config/yaml_cpp_conversions.h @@ -0,0 +1,13 @@ +#ifndef WAVEMAP_IO_CONFIG_YAML_CPP_CONVERSIONS_H_ +#define WAVEMAP_IO_CONFIG_YAML_CPP_CONVERSIONS_H_ + +#include +#include + +namespace wavemap::convert { +param::Map yamlToParamMap(const YAML::Node& yaml_cpp_value); +param::Array yamlToParamArray(const YAML::Node& yaml_cpp_value); +param::Value yamlToParams(const YAML::Node& yaml_cpp_value); +} // namespace wavemap::convert + +#endif // WAVEMAP_IO_CONFIG_YAML_CPP_CONVERSIONS_H_ diff --git a/library/cpp/src/io/CMakeLists.txt b/library/cpp/src/io/CMakeLists.txt index ea5d08d58..e903c7ba6 100644 --- a/library/cpp/src/io/CMakeLists.txt +++ b/library/cpp/src/io/CMakeLists.txt @@ -9,9 +9,19 @@ target_link_libraries(wavemap_io PUBLIC Eigen3::Eigen glog wavemap_core) # Set sources target_sources(wavemap_io PRIVATE + config/file_conversions.cc + config/stream_conversions.cc map/file_conversions.cc map/stream_conversions.cc) +# Optional YAML support +if (yaml-cpp_FOUND) + target_compile_definitions(wavemap_io PUBLIC YAML_CPP_AVAILABLE) + target_link_libraries(wavemap_io PUBLIC ${YAML_CPP_LIBRARIES}) + target_sources(wavemap_io PRIVATE + config/yaml_cpp_conversions.cc) +endif () + # Support installs if (GENERATE_WAVEMAP_INSTALL_RULES) # Mark headers for installation diff --git a/library/cpp/src/io/config/file_conversions.cc b/library/cpp/src/io/config/file_conversions.cc new file mode 100644 index 000000000..27d46ef19 --- /dev/null +++ b/library/cpp/src/io/config/file_conversions.cc @@ -0,0 +1,33 @@ +#include "wavemap/io/config/file_conversions.h" + +#include + +#include "wavemap/io/config/stream_conversions.h" + +namespace wavemap::io { +bool fileToParams(const std::filesystem::path& file_path, + param::Value& params) { + if (file_path.empty()) { + LOG(WARNING) + << "Could not open file for reading. Specified file path is empty."; + return false; + } + + // Open the file for reading + 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; + } + + // Deserialize from bytestream + if (!yamlStreamToParams(file_istream, params)) { + LOG(WARNING) << "Failed to parse map from file " << file_path << "."; + return false; + } + + return true; +} +} // namespace wavemap::io diff --git a/library/cpp/src/io/config/stream_conversions.cc b/library/cpp/src/io/config/stream_conversions.cc new file mode 100644 index 000000000..2c551c9a0 --- /dev/null +++ b/library/cpp/src/io/config/stream_conversions.cc @@ -0,0 +1,25 @@ +#include "wavemap/io/config/stream_conversions.h" + +#ifdef YAML_CPP_AVAILABLE +#include + +#include "wavemap/io/config/yaml_cpp_conversions.h" +#endif + +namespace wavemap::io { +bool yamlStreamToParams(std::istream& istream, param::Value& params) { +#ifdef YAML_CPP_AVAILABLE + try { + YAML::Node yaml = YAML::Load(istream); + params = convert::yamlToParams(yaml); + return true; + } catch (YAML::ParserException&) { + LOG(WARNING) << "Failed to parse bytestream using yaml-cpp."; + return false; + } +#endif + LOG(ERROR) << "No YAML parser is available. Install yaml-cpp or add an " + "interface to your preferred parser in wavemap/io/config."; + return false; +} +} // namespace wavemap::io diff --git a/library/cpp/src/io/config/yaml_cpp_conversions.cc b/library/cpp/src/io/config/yaml_cpp_conversions.cc new file mode 100644 index 000000000..51ebe5355 --- /dev/null +++ b/library/cpp/src/io/config/yaml_cpp_conversions.cc @@ -0,0 +1,76 @@ +#include "wavemap/io/config/yaml_cpp_conversions.h" + +#include + +namespace wavemap::convert { +param::Map yamlToParamMap(const YAML::Node& yaml_cpp_value) { // NOLINT + if (!yaml_cpp_value.IsMap()) { + LOG(WARNING) << "Expected YAML param map."; + return {}; + } + + param::Map param_map; + for (const auto& kv : yaml_cpp_value) { + if (std::string key; YAML::convert::decode(kv.first, key)) { + param_map.emplace(key, yamlToParams(kv.second)); + } else { + LOG(WARNING) << "Ignoring YAML map entry. Key not convertible to string."; + } + } + return param_map; +} + +param::Array yamlToParamArray(const YAML::Node& yaml_cpp_value) { // NOLINT + if (!yaml_cpp_value.IsSequence()) { + LOG(WARNING) << "Expected YAML param sequence."; + return {}; + } + + param::Array array; + for (const auto& kv : yaml_cpp_value) { + array.emplace_back(yamlToParams(kv)); + } + return array; +} + +param::Value yamlToParams(const YAML::Node& yaml_cpp_value) { // NOLINT + if (yaml_cpp_value.IsDefined()) { + switch (yaml_cpp_value.Type()) { + case YAML::NodeType::Map: + return param::Value{yamlToParamMap(yaml_cpp_value)}; + case YAML::NodeType::Sequence: + return param::Value{yamlToParamArray(yaml_cpp_value)}; + case YAML::NodeType::Scalar: + if (bool value; YAML::convert::decode(yaml_cpp_value, value)) { + return param::Value{value}; + } + if (int value; YAML::convert::decode(yaml_cpp_value, value)) { + return param::Value{value}; + } + if (double value; + YAML::convert::decode(yaml_cpp_value, value)) { + return param::Value{value}; + } + if (std::string value; + YAML::convert::decode(yaml_cpp_value, value)) { + return param::Value{value}; + } + LOG(ERROR) << "Encountered unknown type while parsing YAML params."; + break; + case YAML::NodeType::Undefined: + LOG(ERROR) << "Encountered undefined type while parsing YAML params."; + break; + case YAML::NodeType::Null: + LOG(ERROR) << "Encountered null type while parsing YAML params."; + break; + default: + break; + } + } else { + LOG(ERROR) << "Encountered undefined node while parsing YAML params."; + } + + // On error, return an empty array + return param::Value{param::Array{}}; +} +} // namespace wavemap::convert From ddbab4344e493a6b1a1f7d38aa4c37af303f1103 Mon Sep 17 00:00:00 2001 From: Victor Reijgwart Date: Thu, 5 Dec 2024 16:03:08 +0100 Subject: [PATCH 08/16] Communicate failure using std::optional --- .../wavemap/io/config/file_conversions.h | 4 ++-- .../wavemap/io/config/stream_conversions.h | 2 +- library/cpp/src/io/config/file_conversions.cc | 17 ++++++++--------- library/cpp/src/io/config/stream_conversions.cc | 9 ++++----- 4 files changed, 15 insertions(+), 17 deletions(-) diff --git a/library/cpp/include/wavemap/io/config/file_conversions.h b/library/cpp/include/wavemap/io/config/file_conversions.h index d7073a26a..6c0a8f89e 100644 --- a/library/cpp/include/wavemap/io/config/file_conversions.h +++ b/library/cpp/include/wavemap/io/config/file_conversions.h @@ -6,8 +6,8 @@ #include namespace wavemap::io { -bool yamlFileToParams(const std::filesystem::path& file_path, - param::Value& params); +std::optional yamlFileToParams( + const std::filesystem::path& file_path); } // namespace wavemap::io #endif // WAVEMAP_IO_CONFIG_FILE_CONVERSIONS_H_ diff --git a/library/cpp/include/wavemap/io/config/stream_conversions.h b/library/cpp/include/wavemap/io/config/stream_conversions.h index 0c9ba5bad..eef5417db 100644 --- a/library/cpp/include/wavemap/io/config/stream_conversions.h +++ b/library/cpp/include/wavemap/io/config/stream_conversions.h @@ -6,7 +6,7 @@ #include namespace wavemap::io { -bool yamlStreamToParams(std::istream& istream, param::Value& params); +std::optional yamlStreamToParams(std::istream& istream); } #endif // WAVEMAP_IO_CONFIG_STREAM_CONVERSIONS_H_ diff --git a/library/cpp/src/io/config/file_conversions.cc b/library/cpp/src/io/config/file_conversions.cc index 27d46ef19..b651af30d 100644 --- a/library/cpp/src/io/config/file_conversions.cc +++ b/library/cpp/src/io/config/file_conversions.cc @@ -5,12 +5,12 @@ #include "wavemap/io/config/stream_conversions.h" namespace wavemap::io { -bool fileToParams(const std::filesystem::path& file_path, - param::Value& params) { +std::optional yamlFileToParams( + const std::filesystem::path& file_path) { if (file_path.empty()) { LOG(WARNING) << "Could not open file for reading. Specified file path is empty."; - return false; + return std::nullopt; } // Open the file for reading @@ -19,15 +19,14 @@ bool fileToParams(const std::filesystem::path& file_path, if (!file_istream.is_open()) { LOG(WARNING) << "Could not open file " << file_path << " for reading. Error: " << strerror(errno); - return false; + return std::nullopt; } // Deserialize from bytestream - if (!yamlStreamToParams(file_istream, params)) { - LOG(WARNING) << "Failed to parse map from file " << file_path << "."; - return false; + if (auto params = yamlStreamToParams(file_istream); params) { + return params; } - - return true; + LOG(WARNING) << "Failed to parse map from file " << file_path << "."; + return std::nullopt; } } // namespace wavemap::io diff --git a/library/cpp/src/io/config/stream_conversions.cc b/library/cpp/src/io/config/stream_conversions.cc index 2c551c9a0..0e3691917 100644 --- a/library/cpp/src/io/config/stream_conversions.cc +++ b/library/cpp/src/io/config/stream_conversions.cc @@ -7,19 +7,18 @@ #endif namespace wavemap::io { -bool yamlStreamToParams(std::istream& istream, param::Value& params) { +std::optional yamlStreamToParams(std::istream& istream) { #ifdef YAML_CPP_AVAILABLE try { YAML::Node yaml = YAML::Load(istream); - params = convert::yamlToParams(yaml); - return true; + return convert::yamlToParams(yaml); } catch (YAML::ParserException&) { LOG(WARNING) << "Failed to parse bytestream using yaml-cpp."; - return false; + return std::nullopt; } #endif LOG(ERROR) << "No YAML parser is available. Install yaml-cpp or add an " "interface to your preferred parser in wavemap/io/config."; - return false; + return std::nullopt; } } // namespace wavemap::io From b2d1f4fc6b13fae878704fe8527c9c902e207aa4 Mon Sep 17 00:00:00 2001 From: Victor Reijgwart Date: Thu, 5 Dec 2024 16:20:51 +0100 Subject: [PATCH 09/16] Add tests for param loading from yaml files --- library/cpp/test/data/config_file.yaml | 52 +++++++++++++++ library/cpp/test/src/io/CMakeLists.txt | 6 +- .../src/io/test_config_file_conversions.cc | 66 +++++++++++++++++++ ...rsions.cc => test_map_file_conversions.cc} | 0 4 files changed, 123 insertions(+), 1 deletion(-) create mode 100644 library/cpp/test/data/config_file.yaml create mode 100644 library/cpp/test/src/io/test_config_file_conversions.cc rename library/cpp/test/src/io/{test_file_conversions.cc => test_map_file_conversions.cc} (100%) diff --git a/library/cpp/test/data/config_file.yaml b/library/cpp/test/data/config_file.yaml new file mode 100644 index 000000000..3f9d71236 --- /dev/null +++ b/library/cpp/test/data/config_file.yaml @@ -0,0 +1,52 @@ +general: + world_frame: "odom" + logging_level: debug + allow_reset_map_service: true + +map: + type: hashed_chunked_wavelet_octree + min_cell_width: { meters: 0.1 } + +map_operations: + - type: threshold_map + once_every: { seconds: 2.0 } + - type: prune_map + once_every: { seconds: 10.0 } + - type: publish_map + once_every: { seconds: 2.0 } + +measurement_integrators: + ouster_os0: + projection_model: + type: ouster_projector + lidar_origin_to_beam_origin: { millimeters: 27.67 } + lidar_origin_to_sensor_origin_z_offset: { millimeters: 36.18 } + elevation: + num_cells: 128 + min_angle: { degrees: -45.73 } + max_angle: { degrees: 46.27 } + azimuth: + num_cells: 1024 + min_angle: { degrees: -180.0 } + max_angle: { degrees: 180.0 } + measurement_model: + type: continuous_beam + angle_sigma: { degrees: 0.035 } + # NOTE: For best results, we recommend setting range_sigma to + # max(min_cell_width / 2, ouster_noise) where ouster_noise = 0.05 + range_sigma: { meters: 0.05 } + scaling_free: 0.2 + scaling_occupied: 0.4 + integration_method: + type: hashed_chunked_wavelet_integrator + min_range: { meters: 1.0 } + max_range: { meters: 15.0 } + +inputs: + - type: pointcloud_topic + topic_name: "/os_cloud_node/points" + topic_type: ouster + measurement_integrator_names: ouster_os0 + undistort_motion: true + topic_queue_length: 10 + max_wait_for_pose: { seconds: 1.0 } diff --git a/library/cpp/test/src/io/CMakeLists.txt b/library/cpp/test/src/io/CMakeLists.txt index 5cf212161..8a28eec9c 100644 --- a/library/cpp/test/src/io/CMakeLists.txt +++ b/library/cpp/test/src/io/CMakeLists.txt @@ -2,7 +2,11 @@ add_executable(test_wavemap_io) target_include_directories(test_wavemap_io PRIVATE ${PROJECT_SOURCE_DIR}/test/include) -target_sources(test_wavemap_io PRIVATE test_file_conversions.cc) +target_sources(test_wavemap_io PRIVATE + test_config_file_conversions.cc + test_map_file_conversions.cc) +target_compile_definitions(test_wavemap_io PRIVATE + DATADIR="${CMAKE_CURRENT_SOURCE_DIR}/../../data") set_wavemap_target_properties(test_wavemap_io) target_link_libraries(test_wavemap_io wavemap_core wavemap_io GTest::gtest_main) diff --git a/library/cpp/test/src/io/test_config_file_conversions.cc b/library/cpp/test/src/io/test_config_file_conversions.cc new file mode 100644 index 000000000..82a30eb4e --- /dev/null +++ b/library/cpp/test/src/io/test_config_file_conversions.cc @@ -0,0 +1,66 @@ +#include + +#include + +#include "wavemap/io/config/file_conversions.h" +#include "wavemap/test/fixture_base.h" + +namespace wavemap { +using ConfigFileConversionTest = FixtureBase; + +TEST_F(ConfigFileConversionTest, Reading) { + std::filesystem::path data_dir = DATADIR; + std::filesystem::path config_file_path = data_dir / "config_file.yaml"; + + const auto params = io::yamlFileToParams(config_file_path); + +#ifndef YAML_CPP_AVAILABLE + EXPECT_EQ(params, std::nullopt); + return; +#endif + + ASSERT_TRUE(params.has_value()); + EXPECT_TRUE(params->holds()); + EXPECT_TRUE(params->hasChild("general")); + EXPECT_TRUE(params->hasChild("map")); + EXPECT_TRUE(params->hasChild("map_operations")); + EXPECT_TRUE(params->hasChild("measurement_integrators")); + EXPECT_TRUE(params->hasChild("inputs")); + + // Test map loading + const auto map = params->getChildAs("map"); + ASSERT_TRUE(map.has_value()); + EXPECT_EQ(map->size(), 2); + + // Test array loading + const auto map_operations = + params->getChildAs("map_operations"); + ASSERT_TRUE(map_operations.has_value()) + << "Could not convert value for key \"map_operations\" to a " + "param::Array."; + EXPECT_EQ(map_operations->size(), 3); + + // Test loading primitive types + const auto inputs = params->getChildAs("inputs"); + ASSERT_TRUE(inputs.has_value()); + const auto& input = inputs->operator[](0); + // Booleans + const auto undistort_motion = input.getChildAs("undistort_motion"); + ASSERT_TRUE(undistort_motion.has_value()); + EXPECT_EQ(undistort_motion.value(), true); + // Integers + const auto topic_queue_length = input.getChildAs("topic_queue_length"); + ASSERT_TRUE(topic_queue_length.has_value()); + EXPECT_EQ(topic_queue_length.value(), 10); + // Floating points + const auto max_wait_for_pose = input.getChild("max_wait_for_pose"); + ASSERT_TRUE(max_wait_for_pose.has_value()); + const auto seconds = max_wait_for_pose->getChildAs("seconds"); + ASSERT_TRUE(seconds.has_value()); + EXPECT_EQ(seconds.value(), 1.f); + // Strings + const auto topic_name = input.getChildAs("topic_name"); + ASSERT_TRUE(topic_name.has_value()); + EXPECT_EQ(topic_name.value(), "/os_cloud_node/points"); +} +} // namespace wavemap diff --git a/library/cpp/test/src/io/test_file_conversions.cc b/library/cpp/test/src/io/test_map_file_conversions.cc similarity index 100% rename from library/cpp/test/src/io/test_file_conversions.cc rename to library/cpp/test/src/io/test_map_file_conversions.cc From 7e39068986f2eea15958275bdb1010828d039082 Mon Sep 17 00:00:00 2001 From: Victor Reijgwart Date: Thu, 5 Dec 2024 16:40:54 +0100 Subject: [PATCH 10/16] Fix outdated include path --- library/python/src/maps.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/library/python/src/maps.cc b/library/python/src/maps.cc index 6aa29c3bd..b224d077e 100644 --- a/library/python/src/maps.cc +++ b/library/python/src/maps.cc @@ -11,7 +11,7 @@ #include #include #include -#include +#include using namespace nb::literals; // NOLINT From 1fc20a81c33bdd5d52816102774d0dddf1cebb98 Mon Sep 17 00:00:00 2001 From: Victor Reijgwart Date: Thu, 5 Dec 2024 17:44:56 +0100 Subject: [PATCH 11/16] Add an example on how to run the mapping pipeline through the C++ API --- examples/cpp/CMakeLists.txt | 1 + examples/cpp/mapping/.gitignore | 1 + examples/cpp/mapping/CMakeLists.txt | 7 +++ examples/cpp/mapping/example_config.yaml | 33 ++++++++++ examples/cpp/mapping/example_pipeline.cc | 76 ++++++++++++++++++++++++ 5 files changed, 118 insertions(+) create mode 100644 examples/cpp/mapping/.gitignore create mode 100644 examples/cpp/mapping/CMakeLists.txt create mode 100644 examples/cpp/mapping/example_config.yaml create mode 100644 examples/cpp/mapping/example_pipeline.cc diff --git a/examples/cpp/CMakeLists.txt b/examples/cpp/CMakeLists.txt index 428126512..cac35a6e2 100644 --- a/examples/cpp/CMakeLists.txt +++ b/examples/cpp/CMakeLists.txt @@ -32,5 +32,6 @@ endif () # Add each set of examples add_subdirectory(io) +add_subdirectory(mapping) add_subdirectory(queries) add_subdirectory(planning) diff --git a/examples/cpp/mapping/.gitignore b/examples/cpp/mapping/.gitignore new file mode 100644 index 000000000..668b7e19a --- /dev/null +++ b/examples/cpp/mapping/.gitignore @@ -0,0 +1 @@ +example_map.wvmp diff --git a/examples/cpp/mapping/CMakeLists.txt b/examples/cpp/mapping/CMakeLists.txt new file mode 100644 index 000000000..4d65c6b40 --- /dev/null +++ b/examples/cpp/mapping/CMakeLists.txt @@ -0,0 +1,7 @@ +# Binaries +add_executable(example_pipeline example_pipeline.cc) +set_wavemap_target_properties(example_pipeline) +target_link_libraries(example_pipeline PUBLIC + wavemap::wavemap_core + wavemap::wavemap_io + wavemap::wavemap_pipeline) diff --git a/examples/cpp/mapping/example_config.yaml b/examples/cpp/mapping/example_config.yaml new file mode 100644 index 000000000..84b8a3700 --- /dev/null +++ b/examples/cpp/mapping/example_config.yaml @@ -0,0 +1,33 @@ +# NOTE: More examples can be found in the `interfaces/ros1/wavemap_ros/config` +# directory, and all available params are documented at: +# https://ethz-asl.github.io/wavemap/pages/parameters + +map: + type: hashed_chunked_wavelet_octree + min_cell_width: { meters: 0.02 } + +map_operations: + - type: threshold_map + once_every: { seconds: 2.0 } + - type: prune_map + once_every: { seconds: 10.0 } + +measurement_integrators: + your_camera: + projection_model: + type: pinhole_camera_projector + width: 640 + height: 480 + fx: 320.0 + fy: 320.0 + cx: 320.0 + cy: 240.0 + measurement_model: + type: continuous_ray + range_sigma: { meters: 0.01 } + scaling_free: 0.2 + scaling_occupied: 0.4 + integration_method: + type: hashed_chunked_wavelet_integrator + min_range: { meters: 0.1 } + max_range: { meters: 5.0 } diff --git a/examples/cpp/mapping/example_pipeline.cc b/examples/cpp/mapping/example_pipeline.cc new file mode 100644 index 000000000..aaf2b4568 --- /dev/null +++ b/examples/cpp/mapping/example_pipeline.cc @@ -0,0 +1,76 @@ +#include +#include + +#include +#include +#include +#include + +using namespace wavemap; +int main(int, char** argv) { + // Settings + const std::string config_name = "example_config.yaml"; + const std::string output_map_name = "example_map.wvmp"; + const std::filesystem::path current_dir = + std::filesystem::canonical(__FILE__).parent_path(); + const std::filesystem::path config_path = current_dir / config_name; + const std::filesystem::path output_map_path = current_dir / output_map_name; + + // Load the config + std::cout << "Loading config file: " << config_path << std::endl; + const auto params = io::yamlFileToParams(config_path); + CHECK(params.has_value()); + + // Create the map + const auto map_config = params->getChild("map"); + CHECK(map_config.has_value()); + MapBase::Ptr map = MapFactory::create(map_config.value()); + CHECK_NOTNULL(map); + + // Create measurement integration pipeline + Pipeline pipeline{map}; + + // Add map operations to pipeline + const auto map_operations = + params->getChildAs("map_operations"); + CHECK(map_operations); + for (const auto& operation_params : map_operations.value()) { + pipeline.addOperation(operation_params); + } + + // Add measurement integrators to pipeline + const auto measurement_integrators = + params->getChildAs("measurement_integrators"); + CHECK(measurement_integrators); + for (const auto& [integrator_name, integrator_params] : + measurement_integrators.value()) { + pipeline.addIntegrator(integrator_name, integrator_params); + } + + // Define a depth camera image for illustration + const int width = 640; + const int height = 480; + Image<> depth_image{width, height}; + depth_image.setToConstant(2.f); + + // Define a depth camera pose for illustration + Transformation3D T_W_C{}; + // Set the camera's [x, y, z] position + T_W_C.getPosition() = {0.f, 0.f, 0.f}; + // Set the camera's orientation + // For example as a quaternion's [w, x, y, z] coefficients + T_W_C.getRotation() = Rotation3D{0.5f, -0.5f, -0.5f, 0.5f}; + // NOTE: Alternatively, the rotation can also be loaded from a rotation + // matrix, or T_W_C can be initialized from a transformation matrix. + + // Integrate a depth image + pipeline.runPipeline({"your_camera"}, PosedImage<>{T_W_C, depth_image}); + + // See if the map was updated + const size_t map_size_KB = map->getMemoryUsage() / 1024; + std::cout << "Created map of size: " << map_size_KB << " KB" << std::endl; + + // Save map + std::cout << "Saving it to: " << output_map_path << std::endl; + io::mapToFile(*map, output_map_path); +} From 43d1a5066003bd2637d7e618e6b6434ba0955b2b Mon Sep 17 00:00:00 2001 From: Victor Reijgwart Date: Thu, 5 Dec 2024 17:52:20 +0100 Subject: [PATCH 12/16] Update the documentation --- docs/pages/tutorials/cpp.rst | 15 +++++++++++++++ examples/cpp/mapping/example_pipeline.cc | 6 +++--- 2 files changed, 18 insertions(+), 3 deletions(-) diff --git a/docs/pages/tutorials/cpp.rst b/docs/pages/tutorials/cpp.rst index 43ab08f12..709f916dc 100644 --- a/docs/pages/tutorials/cpp.rst +++ b/docs/pages/tutorials/cpp.rst @@ -40,6 +40,21 @@ Code examples ************* In the following sections, you'll find sample code for common tasks. If you'd like to request examples for additional tasks or contribute new examples, please don't hesitate to `contact us `_. +Mapping +======= +The only requirements to build wavemap maps are that you have a set of + +1. depth measurements, +2. sensor pose (estimates) for each measurement. + +We usually use depth measurements from depth cameras or 3D LiDARs, but any source would work as long as a corresponding :ref:`projection ` and :ref:`measurement ` model is available. To help you get started quickly, we provide example configs for various sensor setups :gh_file:`here `. An overview of all the available settings is provided on the :doc:`parameters page <../parameters/index>`. + +Example pipeline +---------------- + +.. literalinclude:: ../../../examples/cpp/mapping/example_pipeline.cc + :language: cpp + Serializing maps ================ In this section, we'll demonstrate how to serialize and deserialize maps using wavemap's lightweight and efficient binary format. This format is consistent across wavemap's C++, Python, and ROS interfaces. For instance, you can create maps on a robot with ROS and later load them into a rendering engine plugin that only depends on wavemap's C++ library. diff --git a/examples/cpp/mapping/example_pipeline.cc b/examples/cpp/mapping/example_pipeline.cc index aaf2b4568..5325786af 100644 --- a/examples/cpp/mapping/example_pipeline.cc +++ b/examples/cpp/mapping/example_pipeline.cc @@ -63,14 +63,14 @@ int main(int, char** argv) { // NOTE: Alternatively, the rotation can also be loaded from a rotation // matrix, or T_W_C can be initialized from a transformation matrix. - // Integrate a depth image + // Integrate the measurement pipeline.runPipeline({"your_camera"}, PosedImage<>{T_W_C, depth_image}); - // See if the map was updated + // Measure the map's size const size_t map_size_KB = map->getMemoryUsage() / 1024; std::cout << "Created map of size: " << map_size_KB << " KB" << std::endl; - // Save map + // Save the map to disk std::cout << "Saving it to: " << output_map_path << std::endl; io::mapToFile(*map, output_map_path); } From f3846f9fb0ab37a32a447f38cb1e664fe33368e0 Mon Sep 17 00:00:00 2001 From: Victor Reijgwart Date: Thu, 5 Dec 2024 17:54:27 +0100 Subject: [PATCH 13/16] Address GCC CI warnings --- library/cpp/src/io/config/stream_conversions.cc | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/library/cpp/src/io/config/stream_conversions.cc b/library/cpp/src/io/config/stream_conversions.cc index 0e3691917..54df5bec5 100644 --- a/library/cpp/src/io/config/stream_conversions.cc +++ b/library/cpp/src/io/config/stream_conversions.cc @@ -7,7 +7,8 @@ #endif namespace wavemap::io { -std::optional yamlStreamToParams(std::istream& istream) { +std::optional yamlStreamToParams( + [[maybe_unused]] std::istream& istream) { #ifdef YAML_CPP_AVAILABLE try { YAML::Node yaml = YAML::Load(istream); From 8f28aad72ed314e4e99fd181bf20fe027673ae3a Mon Sep 17 00:00:00 2001 From: Victor Reijgwart Date: Thu, 5 Dec 2024 20:13:23 +0100 Subject: [PATCH 14/16] Update factories to consistently pass subconfigs to subfactories --- .../src/core/integrator/integrator_factory.cc | 23 +++++++++++++++---- .../measurement_model_factory.cc | 11 +++------ .../projection_model/projector_factory.cc | 13 ++++------- 3 files changed, 25 insertions(+), 22 deletions(-) diff --git a/library/cpp/src/core/integrator/integrator_factory.cc b/library/cpp/src/core/integrator/integrator_factory.cc index 524a54dac..be9b21b96 100644 --- a/library/cpp/src/core/integrator/integrator_factory.cc +++ b/library/cpp/src/core/integrator/integrator_factory.cc @@ -60,10 +60,16 @@ std::unique_ptr IntegratorFactory::create( } // Create the projection model + const auto projector_params = params.getChild("projection_model"); + if (!projector_params) { + LOG(ERROR) << "No params with key \"projection_model\". " + "Projection model could not be created."; + return nullptr; + } std::shared_ptr projection_model = - ProjectorFactory::create(params); + ProjectorFactory::create(projector_params.value()); if (!projection_model) { - LOG(ERROR) << "Projection model could not be created."; + LOG(ERROR) << "Creating projection model failed."; return nullptr; } @@ -75,11 +81,18 @@ std::unique_ptr IntegratorFactory::create( std::make_shared>(projection_model->getDimensions()); // Create the measurement model + const auto measurement_params = params.getChild("measurement_model"); + if (!measurement_params) { + LOG(ERROR) << "No params with key \"measurement_model\". " + "Measurement model could not be created."; + return nullptr; + } std::shared_ptr measurement_model = - MeasurementModelFactory::create(params, projection_model, - posed_range_image, beam_offset_image); + MeasurementModelFactory::create(measurement_params.value(), + projection_model, posed_range_image, + beam_offset_image); if (!measurement_model) { - LOG(ERROR) << "Measurement model could not be created."; + LOG(ERROR) << "Creating measurement model failed."; return nullptr; } diff --git a/library/cpp/src/core/integrator/measurement_model/measurement_model_factory.cc b/library/cpp/src/core/integrator/measurement_model/measurement_model_factory.cc index ac5b32b42..a057d1f59 100644 --- a/library/cpp/src/core/integrator/measurement_model/measurement_model_factory.cc +++ b/library/cpp/src/core/integrator/measurement_model/measurement_model_factory.cc @@ -11,8 +11,7 @@ std::unique_ptr wavemap::MeasurementModelFactory::create( const param::Value& params, ProjectorBase::ConstPtr projection_model, Image<>::ConstPtr range_image, Image::ConstPtr beam_offset_image, std::optional default_measurement_model_type) { - if (const auto type = MeasurementModelType::from(params, "measurement_model"); - type) { + if (const auto type = MeasurementModelType::from(params); type) { return create(type.value(), std::move(projection_model), std::move(range_image), std::move(beam_offset_image), params); } @@ -36,9 +35,7 @@ std::unique_ptr wavemap::MeasurementModelFactory::create( Image::ConstPtr beam_offset_image, const param::Value& params) { switch (measurement_model_type) { case MeasurementModelType::kContinuousRay: { - if (const auto config = - ContinuousRayConfig::from(params, "measurement_model"); - config) { + if (const auto config = ContinuousRayConfig::from(params); config) { return std::make_unique(config.value(), std::move(projection_model), std::move(range_image)); @@ -49,9 +46,7 @@ std::unique_ptr wavemap::MeasurementModelFactory::create( } } case MeasurementModelType::kContinuousBeam: { - if (const auto config = - ContinuousBeamConfig::from(params, "measurement_model"); - config) { + if (const auto config = ContinuousBeamConfig::from(params); config) { return std::make_unique( config.value(), std::move(projection_model), std::move(range_image), std::move(beam_offset_image)); diff --git a/library/cpp/src/core/integrator/projection_model/projector_factory.cc b/library/cpp/src/core/integrator/projection_model/projector_factory.cc index 1a132e5ba..b41d8ba28 100644 --- a/library/cpp/src/core/integrator/projection_model/projector_factory.cc +++ b/library/cpp/src/core/integrator/projection_model/projector_factory.cc @@ -10,7 +10,7 @@ namespace wavemap { std::unique_ptr wavemap::ProjectorFactory::create( const param::Value& params, std::optional default_projector_type) { - if (const auto type = ProjectorType::from(params, "projection_model"); type) { + if (const auto type = ProjectorType::from(params); type) { return create(type.value(), params); } @@ -28,9 +28,7 @@ std::unique_ptr wavemap::ProjectorFactory::create( ProjectorType projector_type, const param::Value& params) { switch (projector_type) { case ProjectorType::kSphericalProjector: { - if (const auto config = - SphericalProjectorConfig::from(params, "projection_model"); - config) { + if (const auto config = SphericalProjectorConfig::from(params); config) { return std::make_unique(config.value()); } else { LOG(ERROR) << "Spherical projector config could not be loaded."; @@ -38,9 +36,7 @@ std::unique_ptr wavemap::ProjectorFactory::create( } } case ProjectorType::kOusterProjector: { - if (const auto config = - OusterProjectorConfig::from(params, "projection_model"); - config) { + if (const auto config = OusterProjectorConfig::from(params); config) { return std::make_unique(config.value()); } else { LOG(ERROR) << "Ouster projector config could not be loaded."; @@ -48,8 +44,7 @@ std::unique_ptr wavemap::ProjectorFactory::create( } } case ProjectorType::kPinholeCameraProjector: { - if (const auto config = - PinholeCameraProjectorConfig::from(params, "projection_model"); + if (const auto config = PinholeCameraProjectorConfig::from(params); config) { return std::make_unique(config.value()); } else { From 21a6143db134edeb018489bb6782847ab033d2a6 Mon Sep 17 00:00:00 2001 From: Victor Reijgwart Date: Thu, 5 Dec 2024 21:29:37 +0100 Subject: [PATCH 15/16] Move code to C++ library and add support for other projection models --- .../raycast_depth_image.py} | 40 ++++-- .../core/utils/render/impl/raycast_inl.h | 34 +++++ .../wavemap/core/utils/render/raycast.h | 25 ++++ .../core/utils/render/raycasting_renderer.h | 43 +++++++ library/cpp/src/core/CMakeLists.txt | 1 + .../core/utils/render/raycasting_renderer.cc | 78 ++++++++++++ library/python/CMakeLists.txt | 2 +- library/python/include/pywavemap/raycast.h | 12 -- library/python/include/pywavemap/render.h | 12 ++ library/python/src/measurements.cc | 22 +++- library/python/src/pywavemap.cc | 6 +- library/python/src/pywavemap/__init__.py | 4 +- library/python/src/raycast.cc | 118 ------------------ library/python/src/render.cc | 24 ++++ 14 files changed, 272 insertions(+), 149 deletions(-) rename examples/python/{raycast/depth_exctraction.py => render/raycast_depth_image.py} (53%) create mode 100644 library/cpp/include/wavemap/core/utils/render/impl/raycast_inl.h create mode 100644 library/cpp/include/wavemap/core/utils/render/raycast.h create mode 100644 library/cpp/include/wavemap/core/utils/render/raycasting_renderer.h create mode 100644 library/cpp/src/core/utils/render/raycasting_renderer.cc delete mode 100644 library/python/include/pywavemap/raycast.h create mode 100644 library/python/include/pywavemap/render.h delete mode 100644 library/python/src/raycast.cc create mode 100644 library/python/src/render.cc diff --git a/examples/python/raycast/depth_exctraction.py b/examples/python/render/raycast_depth_image.py similarity index 53% rename from examples/python/raycast/depth_exctraction.py rename to examples/python/render/raycast_depth_image.py index 899bb552f..99419b68f 100644 --- a/examples/python/raycast/depth_exctraction.py +++ b/examples/python/render/raycast_depth_image.py @@ -1,5 +1,5 @@ """ -Depth map extraction from HashedWaveletOctree map at given camera pose and intrinsics +Extract depth maps from wavemap maps at a given sensor pose and intrinsics """ import time @@ -31,18 +31,31 @@ def save_depth_as_png(depth_map: np.ndarray, output_path: Path): map_path = Path.home() \ / "data/panoptic_mapping/flat_dataset/run2/your_map.wvmp" out_path = Path(__file__).parent / "depth.png" - camera_cfg = wm.PinholeCameraProjectorConfig( - width=1280, - height=720, - fx=526.21539307, - fy=526.21539307, - cx=642.309021, - cy=368.69949341, - ) # Note: these are intrinsics for Zed 2i + + # Configure the virtual sensor's projection model + # NOTE: These intrinsics match the Zed 2i depth camera. + projection_model = wm.Projector.create({ + "type": "pinhole_camera_projector", + "width": 1280, + "height": 720, + "fx": 526.21539307, + "fy": 526.21539307, + "cx": 642.309021, + "cy": 368.69949341 + }) # Load map from file your_map = wm.Map.load(map_path) + # Create the depth image renderer + log_odds_occupancy_threshold = 0.1 + min_range = 0.1 + max_range = 6.0 + default_depth_value = -1.0 + renderer = wm.RaycastingRenderer(your_map, projection_model, + log_odds_occupancy_threshold, min_range, + max_range, default_depth_value) + # Create pose rotation = wm.Rotation(np.eye(3)) translation = np.zeros(3) @@ -50,8 +63,11 @@ def save_depth_as_png(depth_map: np.ndarray, output_path: Path): # Extract depth t1 = time.perf_counter() - depth = wm.get_depth(your_map, pose, camera_cfg, 0.1, 10) + depth_image = renderer.render(pose) t2 = time.perf_counter() - print(f"Depth your_map of size {camera_cfg.width}x{camera_cfg.height} " + print(f"Depth image of size {depth_image.width}x{depth_image.height} " f"created in {t2 - t1:.02f} seconds") - save_depth_as_png(depth, out_path) + save_depth_as_png(depth_image.data, out_path) + + # Avoids leak warnings on old Python versions with lazy garbage collectors + del renderer, depth_image diff --git a/library/cpp/include/wavemap/core/utils/render/impl/raycast_inl.h b/library/cpp/include/wavemap/core/utils/render/impl/raycast_inl.h new file mode 100644 index 000000000..07c5779f3 --- /dev/null +++ b/library/cpp/include/wavemap/core/utils/render/impl/raycast_inl.h @@ -0,0 +1,34 @@ +#ifndef WAVEMAP_CORE_UTILS_RENDER_IMPL_RAYCAST_INL_H_ +#define WAVEMAP_CORE_UTILS_RENDER_IMPL_RAYCAST_INL_H_ + +namespace wavemap::raycast { +template +std::optional first_collision_index( + MapT& map, const Point3D& W_start_point, const Point3D& W_end_point, + FloatingPoint log_odds_occupancy_threshold) { + const Ray ray(W_start_point, W_end_point, map.getMinCellWidth()); + for (const Index3D& ray_voxel_index : ray) { + if (log_odds_occupancy_threshold < map.getCellValue(ray_voxel_index)) { + return ray_voxel_index; + } + } + return std::nullopt; +} + +template +std::optional first_collision_distance( + MapT& map, const Point3D& W_start_point, const Point3D& W_end_point, + FloatingPoint log_odds_occupancy_threshold) { + const auto colliding_index = first_collision_index( + map, W_start_point, W_end_point, log_odds_occupancy_threshold); + if (colliding_index) { + const FloatingPoint min_cell_width = map.getMinCellWidth(); + const Point3D voxel_center = + convert::indexToCenterPoint(colliding_index.value(), min_cell_width); + return (voxel_center - W_start_point).norm(); + } + return std::nullopt; +} +} // namespace wavemap::raycast + +#endif // WAVEMAP_CORE_UTILS_RENDER_IMPL_RAYCAST_INL_H_ diff --git a/library/cpp/include/wavemap/core/utils/render/raycast.h b/library/cpp/include/wavemap/core/utils/render/raycast.h new file mode 100644 index 000000000..5e44dde9b --- /dev/null +++ b/library/cpp/include/wavemap/core/utils/render/raycast.h @@ -0,0 +1,25 @@ +#ifndef WAVEMAP_CORE_UTILS_RENDER_RAYCAST_H_ +#define WAVEMAP_CORE_UTILS_RENDER_RAYCAST_H_ + +#include "wavemap/core/common.h" +#include "wavemap/core/utils/iterate/ray_iterator.h" + +namespace wavemap::raycast { +// NOTE: The map can either be a raw map, or a QueryAccelerator instance. We +// strongly recommend passing a QueryAccelerator to get good performance. +template +std::optional first_collision_index( + MapT& map, const Point3D& W_start_point, const Point3D& W_end_point, + FloatingPoint log_odds_occupancy_threshold); + +// NOTE: The map can either be a raw map, or a QueryAccelerator instance. We +// strongly recommend passing a QueryAccelerator to get good performance. +template +std::optional first_collision_distance( + MapT& map, const Point3D& W_start_point, const Point3D& W_end_point, + FloatingPoint log_odds_occupancy_threshold); +} // namespace wavemap::raycast + +#include "wavemap/core/utils/render/impl/raycast_inl.h" + +#endif // WAVEMAP_CORE_UTILS_RENDER_RAYCAST_H_ diff --git a/library/cpp/include/wavemap/core/utils/render/raycasting_renderer.h b/library/cpp/include/wavemap/core/utils/render/raycasting_renderer.h new file mode 100644 index 000000000..8df0163b2 --- /dev/null +++ b/library/cpp/include/wavemap/core/utils/render/raycasting_renderer.h @@ -0,0 +1,43 @@ +#ifndef WAVEMAP_CORE_UTILS_RENDER_RAYCASTING_RENDERER_H_ +#define WAVEMAP_CORE_UTILS_RENDER_RAYCASTING_RENDERER_H_ + +#include + +#include "wavemap/core/data_structure/image.h" +#include "wavemap/core/integrator/projection_model/projector_base.h" +#include "wavemap/core/map/map_base.h" +#include "wavemap/core/utils/thread_pool.h" + +namespace wavemap { +class RaycastingRenderer { + public: + RaycastingRenderer(MapBase::ConstPtr occupancy_map, + ProjectorBase::ConstPtr projection_model, + FloatingPoint log_odds_occupancy_threshold = 1e-3f, + FloatingPoint min_range = 0.f, + FloatingPoint max_range = 10.f, + FloatingPoint default_depth_value = -1.f, + std::shared_ptr thread_pool = nullptr); + + const Image<>& render(const Transformation3D& T_W_C); + + private: + static constexpr int kPatchWidth = 64; + + const MapBase::ConstPtr map_; + const std::shared_ptr thread_pool_; + + const ProjectorBase::ConstPtr projection_model_; + const FloatingPoint min_range_; + const FloatingPoint max_range_; + const FloatingPoint log_odds_occupancy_threshold_; + + Image depth_image_; + + template + void renderPatch(const MapT& map, const Transformation3D& T_W_C, + const Index2D& patch_index); +}; +} // namespace wavemap + +#endif // WAVEMAP_CORE_UTILS_RENDER_RAYCASTING_RENDERER_H_ diff --git a/library/cpp/src/core/CMakeLists.txt b/library/cpp/src/core/CMakeLists.txt index a48abab05..30c312034 100644 --- a/library/cpp/src/core/CMakeLists.txt +++ b/library/cpp/src/core/CMakeLists.txt @@ -46,6 +46,7 @@ target_sources(wavemap_core PRIVATE utils/query/classified_map.cc utils/query/query_accelerator.cc utils/query/point_sampler.cc + utils/render/raycasting_renderer.cc utils/sdf/full_euclidean_sdf_generator.cc utils/sdf/quasi_euclidean_sdf_generator.cc utils/time/stopwatch.cc diff --git a/library/cpp/src/core/utils/render/raycasting_renderer.cc b/library/cpp/src/core/utils/render/raycasting_renderer.cc new file mode 100644 index 000000000..54273e06d --- /dev/null +++ b/library/cpp/src/core/utils/render/raycasting_renderer.cc @@ -0,0 +1,78 @@ +#include "wavemap/core/utils/render/raycasting_renderer.h" + +#include +#include + +#include "wavemap/core/utils/iterate/grid_iterator.h" +#include "wavemap/core/utils/query/query_accelerator.h" +#include "wavemap/core/utils/render/raycast.h" + +namespace wavemap { +RaycastingRenderer::RaycastingRenderer( + MapBase::ConstPtr occupancy_map, ProjectorBase::ConstPtr projection_model, + FloatingPoint log_odds_occupancy_threshold, FloatingPoint min_range, + FloatingPoint max_range, FloatingPoint default_depth_value, + std::shared_ptr thread_pool) + : map_(CHECK_NOTNULL(occupancy_map)), + thread_pool_(thread_pool ? std::move(thread_pool) + : std::make_shared()), + projection_model_(CHECK_NOTNULL(projection_model)), + min_range_(min_range), + max_range_(max_range), + log_odds_occupancy_threshold_(log_odds_occupancy_threshold), + depth_image_(projection_model_->getDimensions(), default_depth_value) {} + +template +void RaycastingRenderer::renderPatch(const MapT& map, + const Transformation3D& T_W_C, + const Index2D& patch_index) { + QueryAccelerator query_accelerator(map); + + const Index2D patch_min = patch_index * kPatchWidth; + const Index2D patch_max = (patch_min.array() + kPatchWidth) + .min(depth_image_.getDimensions().array()) - + 1; + + for (const Index2D& index : Grid(patch_min, patch_max)) { + FloatingPoint& depth_pixel = depth_image_.at(index); + const Vector2D image_xy = projection_model_->indexToImage(index); + const Point3D C_start_point = + projection_model_->sensorToCartesian({image_xy, min_range_}); + const Point3D C_end_point = + projection_model_->sensorToCartesian({image_xy, max_range_}); + const Point3D W_start_point = T_W_C * C_start_point; + const Point3D W_end_point = T_W_C * C_end_point; + const auto d_start_collision = raycast::first_collision_distance( + query_accelerator, W_start_point, W_end_point, + log_odds_occupancy_threshold_); + if (d_start_collision) { + depth_pixel = d_start_collision.value(); + } + } +} + +const Image<>& RaycastingRenderer::render(const Transformation3D& T_W_C) { + depth_image_.resetToInitialValue(); + + const Index2D num_patches = + int_math::div_round_up(depth_image_.getDimensions(), kPatchWidth); + for (const Index2D& patch_index : + Grid<2>(Index2D::Zero(), num_patches - Index2D::Ones())) { + thread_pool_->add_task([this, &T_W_C, patch_index]() { + if (const auto* map = + dynamic_cast(map_.get()); + map) { + renderPatch(*map, T_W_C, patch_index); + } + if (const auto* map = + dynamic_cast(map_.get()); + map) { + renderPatch(*map, T_W_C, patch_index); + } + }); + } + thread_pool_->wait_all(); + + return depth_image_; +} +} // namespace wavemap diff --git a/library/python/CMakeLists.txt b/library/python/CMakeLists.txt index 125ee920a..702629604 100644 --- a/library/python/CMakeLists.txt +++ b/library/python/CMakeLists.txt @@ -64,7 +64,7 @@ nanobind_add_module(_pywavemap_bindings STABLE_ABI src/measurements.cc src/param.cc src/pipeline.cc - src/raycast.cc) + src/render.cc) set_wavemap_target_properties(_pywavemap_bindings) target_include_directories(_pywavemap_bindings PRIVATE include) target_link_libraries(_pywavemap_bindings PRIVATE diff --git a/library/python/include/pywavemap/raycast.h b/library/python/include/pywavemap/raycast.h deleted file mode 100644 index 61fbf9b18..000000000 --- a/library/python/include/pywavemap/raycast.h +++ /dev/null @@ -1,12 +0,0 @@ -#ifndef PYWAVEMAP_RAYCAST_H_ -#define PYWAVEMAP_RAYCAST_H_ - -#include - -namespace nb = nanobind; - -namespace wavemap { -void add_raycast_bindings(nb::module_& m); -} // namespace wavemap - -#endif // PYWAVEMAP_RAYCAST_H_ diff --git a/library/python/include/pywavemap/render.h b/library/python/include/pywavemap/render.h new file mode 100644 index 000000000..f889f25b9 --- /dev/null +++ b/library/python/include/pywavemap/render.h @@ -0,0 +1,12 @@ +#ifndef PYWAVEMAP_RENDER_H_ +#define PYWAVEMAP_RENDER_H_ + +#include + +namespace nb = nanobind; + +namespace wavemap { +void add_render_bindings(nb::module_& m); +} // namespace wavemap + +#endif // PYWAVEMAP_RENDER_H_ diff --git a/library/python/src/measurements.cc b/library/python/src/measurements.cc index 1c51f7052..d421ba485 100644 --- a/library/python/src/measurements.cc +++ b/library/python/src/measurements.cc @@ -1,9 +1,14 @@ #include "pywavemap/measurements.h" +#include + #include +#include #include #include #include +#include +#include using namespace nb::literals; // NOLINT @@ -33,9 +38,24 @@ void add_measurement_bindings(nb::module_& m) { // Images nb::class_>(m, "Image", "A class to store depth images.") - .def(nb::init::Data>(), "pixel_matrix"_a); + .def(nb::init::Data>(), "pixel_matrix"_a) + .def_prop_ro("width", &Image<>::getNumRows, + "The image's width in pixels.") + .def_prop_ro("height", &Image<>::getNumColumns, + "The image's height in pixels.") + .def_prop_ro("data", nb::overload_cast<>(&Image<>::getData, nb::const_)); nb::class_>( m, "PosedImage", "A class to store depth images with an associated pose.") .def(nb::init>(), "pose"_a, "image"_a); + + // Projection models + nb::class_(m, "Projector") + .def_static( + "create", + [](const param::Value& params) -> std::shared_ptr { + return ProjectorFactory::create(params); + }, + nb::sig("def create(parameters: dict) -> Projector"), "parameters"_a, + "Create a new projection model based on the given settings."); } } // namespace wavemap diff --git a/library/python/src/pywavemap.cc b/library/python/src/pywavemap.cc index c44791623..65d8e2212 100644 --- a/library/python/src/pywavemap.cc +++ b/library/python/src/pywavemap.cc @@ -7,7 +7,7 @@ #include "pywavemap/measurements.h" #include "pywavemap/param.h" #include "pywavemap/pipeline.h" -#include "pywavemap/raycast.h" +#include "pywavemap/render.h" using namespace wavemap; // NOLINT namespace nb = nanobind; @@ -55,6 +55,6 @@ NB_MODULE(_pywavemap_bindings, m) { // Bindings for measurement integration and map update pipelines add_pipeline_bindings(m); - // Bindings for raycasting - add_raycast_bindings(m); + // Bindings for rendering + add_render_bindings(m); } diff --git a/library/python/src/pywavemap/__init__.py b/library/python/src/pywavemap/__init__.py index 0c4078ab3..bfdde4583 100644 --- a/library/python/src/pywavemap/__init__.py +++ b/library/python/src/pywavemap/__init__.py @@ -4,12 +4,12 @@ # Binding types from ._pywavemap_bindings import OctreeIndex from ._pywavemap_bindings import (Rotation, Pose, Pointcloud, PosedPointcloud, - Image, PosedImage) + Image, PosedImage, Projector) from ._pywavemap_bindings import (Map, HashedWaveletOctree, HashedChunkedWaveletOctree, InterpolationMode) from ._pywavemap_bindings import Pipeline -from ._pywavemap_bindings import raycast, PinholeCameraProjectorConfig, get_depth +from ._pywavemap_bindings import RaycastingRenderer # Binding submodules from ._pywavemap_bindings import logging, param, convert diff --git a/library/python/src/raycast.cc b/library/python/src/raycast.cc deleted file mode 100644 index adcb5d0b2..000000000 --- a/library/python/src/raycast.cc +++ /dev/null @@ -1,118 +0,0 @@ -#include "pywavemap/raycast.h" - -#include // to use eigen2numpy seamlessly -#include -#include -#include -#include -#include -#include -#include - -#include "wavemap/core/utils/iterate/ray_iterator.h" - -using namespace nb::literals; // NOLINT - -namespace wavemap { -FloatingPoint raycast(const HashedWaveletOctree& map, - const Point3D& start_point, const Point3D& end_point, - FloatingPoint log_odds_threshold) { - const FloatingPoint mcw = map.getMinCellWidth(); - const Ray ray(start_point, end_point, mcw); - for (const Index3D& ray_voxel_index : ray) { - if (log_odds_threshold < map.getCellValue(ray_voxel_index)) { - const Point3D voxel_center = - convert::indexToCenterPoint(ray_voxel_index, mcw); - return (voxel_center - start_point).norm(); - } - } - return (end_point - start_point).norm(); -} - -FloatingPoint raycast_fast( - QueryAccelerator& query_accelerator, - const Point3D& start_point, const Point3D& end_point, - FloatingPoint log_odds_threshold) { - const FloatingPoint min_cell_width = query_accelerator.getMinCellWidth(); - const Ray ray(start_point, end_point, min_cell_width); - for (const Index3D& ray_voxel_index : ray) { - if (log_odds_threshold < query_accelerator.getCellValue(ray_voxel_index)) { - const Point3D voxel_center = - convert::indexToCenterPoint(ray_voxel_index, min_cell_width); - return (voxel_center - start_point).norm(); - } - } - return (end_point - start_point).norm(); -} - -void add_raycast_bindings(nb::module_& m) { - nb::class_( - m, "PinholeCameraProjectorConfig", "Describes pinhole camera intrinsics") - .def(nb::init(), - "fx"_a, "fy"_a, "cx"_a, "cy"_a, "height"_a, "width"_a) - .def_rw("width", &PinholeCameraProjectorConfig::width) - .def_rw("height", &PinholeCameraProjectorConfig::height) - .def_rw("fx", &PinholeCameraProjectorConfig::fx) - .def_rw("fy", &PinholeCameraProjectorConfig::fy) - .def_rw("cx", &PinholeCameraProjectorConfig::cx) - .def_rw("cy", &PinholeCameraProjectorConfig::cy) - .def("__repr__", [](const PinholeCameraProjectorConfig& self) { - return nb::str( - "PinholeCameraProjectorConfig(width={}, height={}, fx={}, " - "fy={}, cx={}, cy={})") - .format(self.width, self.height, self.fx, self.fy, self.cx, - self.cy); - }); - - m.def("raycast", &raycast, - "Raycast and get first point with occopancy higher than threshold"); - - m.def("raycast_fast", - &raycast_fast, // TODO: unusable without QueryAccelerator binding - "Raycast and get first point with occopancy higher than threshold " - "using QueryAccelerator for efficiency"); - - m.def( - "get_depth", - [](const HashedWaveletOctree& map, const Transformation3D& pose, - const PinholeCameraProjectorConfig& cam_cfg, - FloatingPoint log_odds_threshold, FloatingPoint max_range) { - const PinholeCameraProjector projection_model(cam_cfg); - Image depth_image(projection_model.getDimensions()); - const Point3D& W_start_point = pose.getPosition(); - - ThreadPool thread_pool; - constexpr int kPatchWidth = 64; - const Index2D num_patches = - int_math::div_round_up(depth_image.getDimensions(), kPatchWidth); - for (const Index2D& patch_index : - Grid<2>(Index2D::Zero(), num_patches - Index2D::Ones())) { - thread_pool.add_task([&map, &projection_model, &pose, &W_start_point, - &depth_image, patch_index, kPatchWidth, - max_range, log_odds_threshold]() { - QueryAccelerator query_accelerator(map); - const Index2D patch_min = patch_index * kPatchWidth; - const Index2D patch_max = - (patch_min.array() + kPatchWidth) - .min(depth_image.getDimensions().array()) - - 1; - for (const Index2D& index : Grid<2>(patch_min, patch_max)) { - FloatingPoint& depth_pixel = depth_image.at(index); - const Vector2D image_xy = projection_model.indexToImage(index); - const Point3D C_end_point = - projection_model.sensorToCartesian({image_xy, max_range}); - const Point3D W_end_point = pose * C_end_point; - depth_pixel = raycast_fast(query_accelerator, W_start_point, - W_end_point, log_odds_threshold); - } - }); - } - thread_pool.wait_all(); - - return depth_image.getData().transpose().eval(); - }, - "Extract depth from octree map at using given camera pose and " - "intrinsics"); -} -} // namespace wavemap diff --git a/library/python/src/render.cc b/library/python/src/render.cc new file mode 100644 index 000000000..60e190f5f --- /dev/null +++ b/library/python/src/render.cc @@ -0,0 +1,24 @@ +#include "pywavemap/render.h" + +#include + +#include +#include +#include +#include + +using namespace nb::literals; // NOLINT + +namespace wavemap { +void add_render_bindings(nb::module_& m) { + nb::class_(m, "RaycastingRenderer", + "Render depth images using ray casting.") + .def(nb::init, + std::shared_ptr, FloatingPoint, + FloatingPoint, FloatingPoint, FloatingPoint>(), + "occupancy_map"_a, "projection_model"_a, + "log_odds_occupancy_threshold"_a = 1e-3f, "min_range"_a = 0.f, + "max_range"_a = 10.f, "default_depth_value"_a = -1.f) + .def("render", &RaycastingRenderer::render, "pose"_a); +} +} // namespace wavemap From 730cb2808fa4584ca739a46d8cb2d6cf234b49b7 Mon Sep 17 00:00:00 2001 From: Victor Reijgwart Date: Fri, 6 Dec 2024 12:02:05 +0100 Subject: [PATCH 16/16] Remove option to use a non-zero min_range in the ray casting renderer --- examples/python/render/raycast_depth_image.py | 14 ++++++++++---- .../core/utils/render/raycasting_renderer.h | 2 -- .../src/core/utils/render/raycasting_renderer.cc | 10 +++------- library/python/src/render.cc | 6 +++--- 4 files changed, 16 insertions(+), 16 deletions(-) diff --git a/examples/python/render/raycast_depth_image.py b/examples/python/render/raycast_depth_image.py index 99419b68f..ec2ad5dd3 100644 --- a/examples/python/render/raycast_depth_image.py +++ b/examples/python/render/raycast_depth_image.py @@ -33,7 +33,14 @@ def save_depth_as_png(depth_map: np.ndarray, output_path: Path): out_path = Path(__file__).parent / "depth.png" # Configure the virtual sensor's projection model - # NOTE: These intrinsics match the Zed 2i depth camera. + # NOTE: The projection model can be configured through a Python dictionary. + # Diagnostics for invalid configurations are printed to the terminal. + # For an overview of all the available models and their params, see: + # pylint: disable=line-too-long + # https://ethz-asl.github.io/wavemap/pages/parameters/measurement_integrators.html#projection-models + # Examples for a broad selection of common sensors can be found in the + # `interfaces/ros1/wavemap_ros/config` directory. + # The intrinsics below match the Zed 2i depth camera. projection_model = wm.Projector.create({ "type": "pinhole_camera_projector", "width": 1280, @@ -49,12 +56,11 @@ def save_depth_as_png(depth_map: np.ndarray, output_path: Path): # Create the depth image renderer log_odds_occupancy_threshold = 0.1 - min_range = 0.1 max_range = 6.0 default_depth_value = -1.0 renderer = wm.RaycastingRenderer(your_map, projection_model, - log_odds_occupancy_threshold, min_range, - max_range, default_depth_value) + log_odds_occupancy_threshold, max_range, + default_depth_value) # Create pose rotation = wm.Rotation(np.eye(3)) diff --git a/library/cpp/include/wavemap/core/utils/render/raycasting_renderer.h b/library/cpp/include/wavemap/core/utils/render/raycasting_renderer.h index 8df0163b2..d47d4a845 100644 --- a/library/cpp/include/wavemap/core/utils/render/raycasting_renderer.h +++ b/library/cpp/include/wavemap/core/utils/render/raycasting_renderer.h @@ -14,7 +14,6 @@ class RaycastingRenderer { RaycastingRenderer(MapBase::ConstPtr occupancy_map, ProjectorBase::ConstPtr projection_model, FloatingPoint log_odds_occupancy_threshold = 1e-3f, - FloatingPoint min_range = 0.f, FloatingPoint max_range = 10.f, FloatingPoint default_depth_value = -1.f, std::shared_ptr thread_pool = nullptr); @@ -28,7 +27,6 @@ class RaycastingRenderer { const std::shared_ptr thread_pool_; const ProjectorBase::ConstPtr projection_model_; - const FloatingPoint min_range_; const FloatingPoint max_range_; const FloatingPoint log_odds_occupancy_threshold_; diff --git a/library/cpp/src/core/utils/render/raycasting_renderer.cc b/library/cpp/src/core/utils/render/raycasting_renderer.cc index 54273e06d..f566479f5 100644 --- a/library/cpp/src/core/utils/render/raycasting_renderer.cc +++ b/library/cpp/src/core/utils/render/raycasting_renderer.cc @@ -10,14 +10,12 @@ namespace wavemap { RaycastingRenderer::RaycastingRenderer( MapBase::ConstPtr occupancy_map, ProjectorBase::ConstPtr projection_model, - FloatingPoint log_odds_occupancy_threshold, FloatingPoint min_range, - FloatingPoint max_range, FloatingPoint default_depth_value, - std::shared_ptr thread_pool) + FloatingPoint log_odds_occupancy_threshold, FloatingPoint max_range, + FloatingPoint default_depth_value, std::shared_ptr thread_pool) : map_(CHECK_NOTNULL(occupancy_map)), thread_pool_(thread_pool ? std::move(thread_pool) : std::make_shared()), projection_model_(CHECK_NOTNULL(projection_model)), - min_range_(min_range), max_range_(max_range), log_odds_occupancy_threshold_(log_odds_occupancy_threshold), depth_image_(projection_model_->getDimensions(), default_depth_value) {} @@ -36,11 +34,9 @@ void RaycastingRenderer::renderPatch(const MapT& map, for (const Index2D& index : Grid(patch_min, patch_max)) { FloatingPoint& depth_pixel = depth_image_.at(index); const Vector2D image_xy = projection_model_->indexToImage(index); - const Point3D C_start_point = - projection_model_->sensorToCartesian({image_xy, min_range_}); + const Point3D& W_start_point = T_W_C.getPosition(); const Point3D C_end_point = projection_model_->sensorToCartesian({image_xy, max_range_}); - const Point3D W_start_point = T_W_C * C_start_point; const Point3D W_end_point = T_W_C * C_end_point; const auto d_start_collision = raycast::first_collision_distance( query_accelerator, W_start_point, W_end_point, diff --git a/library/python/src/render.cc b/library/python/src/render.cc index 60e190f5f..9dfabc33e 100644 --- a/library/python/src/render.cc +++ b/library/python/src/render.cc @@ -15,10 +15,10 @@ void add_render_bindings(nb::module_& m) { "Render depth images using ray casting.") .def(nb::init, std::shared_ptr, FloatingPoint, - FloatingPoint, FloatingPoint, FloatingPoint>(), + FloatingPoint, FloatingPoint>(), "occupancy_map"_a, "projection_model"_a, - "log_odds_occupancy_threshold"_a = 1e-3f, "min_range"_a = 0.f, - "max_range"_a = 10.f, "default_depth_value"_a = -1.f) + "log_odds_occupancy_threshold"_a = 1e-3f, "max_range"_a = 10.f, + "default_depth_value"_a = -1.f) .def("render", &RaycastingRenderer::render, "pose"_a); } } // namespace wavemap