From 232275aeca4b0b207fe5e69860666bfe7ed28806 Mon Sep 17 00:00:00 2001 From: Ignacio Vizzo Date: Mon, 5 Aug 2024 16:51:22 +0200 Subject: [PATCH 1/5] Add draft visualization for the voxels of the VoxelHashMap --- cpp/kiss_icp/core/VoxelHashMap.cpp | 11 ++++++++ cpp/kiss_icp/core/VoxelHashMap.hpp | 1 + python/kiss_icp/mapping.py | 4 +++ python/kiss_icp/pybind/kiss_icp_pybind.cpp | 3 +- python/kiss_icp/tools/visualizer.py | 32 +++++++++++++++++++--- 5 files changed, 46 insertions(+), 5 deletions(-) diff --git a/cpp/kiss_icp/core/VoxelHashMap.cpp b/cpp/kiss_icp/core/VoxelHashMap.cpp index a762bf40..a5cb6ef8 100644 --- a/cpp/kiss_icp/core/VoxelHashMap.cpp +++ b/cpp/kiss_icp/core/VoxelHashMap.cpp @@ -56,6 +56,17 @@ std::vector VoxelHashMap::Pointcloud() const { return points; } +// Get the indices of the occupided voxels as points, mainly used for visualization +std::vector VoxelHashMap::GetVoxels() const { + std::vector voxels; + voxels.reserve(map_.size()); + for (const auto &[voxel, voxel_block] : map_) { + (void)voxel_block; + voxels.emplace_back(voxel); + } + return voxels; +} + void VoxelHashMap::Update(const std::vector &points, const Eigen::Vector3d &origin) { AddPoints(points); diff --git a/cpp/kiss_icp/core/VoxelHashMap.hpp b/cpp/kiss_icp/core/VoxelHashMap.hpp index 98664cd4..f1b66a3e 100644 --- a/cpp/kiss_icp/core/VoxelHashMap.hpp +++ b/cpp/kiss_icp/core/VoxelHashMap.hpp @@ -49,6 +49,7 @@ struct VoxelHashMap { void RemovePointsFarFromLocation(const Eigen::Vector3d &origin); std::vector Pointcloud() const; std::vector GetPoints(const std::vector &query_voxels) const; + std::vector GetVoxels() const; double voxel_size_; double max_distance_; diff --git a/python/kiss_icp/mapping.py b/python/kiss_icp/mapping.py index ba1dfb73..7c902cc6 100644 --- a/python/kiss_icp/mapping.py +++ b/python/kiss_icp/mapping.py @@ -66,3 +66,7 @@ def remove_far_away_points(self, origin): def point_cloud(self) -> np.ndarray: """Return the internal representaion as a np.array (pointcloud).""" return np.asarray(self._internal_map._point_cloud()) + + def get_voxels(self) -> np.ndarray: + """Return the occupied voxels, in indices (i,j,k( as a np.array.""" + return np.asarray(self._internal_map._get_voxels()) diff --git a/python/kiss_icp/pybind/kiss_icp_pybind.cpp b/python/kiss_icp/pybind/kiss_icp_pybind.cpp index 2291c04f..61edceec 100644 --- a/python/kiss_icp/pybind/kiss_icp_pybind.cpp +++ b/python/kiss_icp/pybind/kiss_icp_pybind.cpp @@ -72,7 +72,8 @@ PYBIND11_MODULE(kiss_icp_pybind, m) { "points"_a, "pose"_a) .def("_add_points", &VoxelHashMap::AddPoints, "points"_a) .def("_remove_far_away_points", &VoxelHashMap::RemovePointsFarFromLocation, "origin"_a) - .def("_point_cloud", &VoxelHashMap::Pointcloud); + .def("_point_cloud", &VoxelHashMap::Pointcloud) + .def("_get_voxels", &VoxelHashMap::GetVoxels); // Point Cloud registration py::class_ internal_registration(m, "_Registration", "Don't use this"); diff --git a/python/kiss_icp/tools/visualizer.py b/python/kiss_icp/tools/visualizer.py index 3e9c9257..12ba31a9 100644 --- a/python/kiss_icp/tools/visualizer.py +++ b/python/kiss_icp/tools/visualizer.py @@ -34,6 +34,7 @@ BLACK = np.array([0, 0, 0]) / 255.0 BLUE = np.array([0.4, 0.5, 0.9]) GRAY = np.array([0.4, 0.4, 0.4]) +LIGHT_GRAY = np.array([0.9, 0.9, 0.9]) SPHERE_SIZE = 0.20 @@ -63,6 +64,8 @@ def __init__(self): self.source = self.o3d.geometry.PointCloud() self.keypoints = self.o3d.geometry.PointCloud() self.target = self.o3d.geometry.PointCloud() + self.voxel_map = self.o3d.geometry.VoxelGrid() + self.voxel_map.voxel_size = 1.0 self.frames = [] # Initialize visualizer @@ -74,6 +77,7 @@ def __init__(self): self.render_map = True self.render_source = True self.render_keypoints = False + self.render_voxel_grid = True self.global_view = False self.render_trajectory = True # Cache the state of the visualizer @@ -81,11 +85,11 @@ def __init__(self): self.render_map, self.render_keypoints, self.render_source, + self.render_voxel_grid, ) def update(self, source, keypoints, target_map, pose): - target = target_map.point_cloud() - self._update_geometries(source, keypoints, target, pose) + self._update_geometries(source, keypoints, target_map, pose) while self.block_vis: self.vis.poll_events() self.vis.update_renderer() @@ -100,8 +104,10 @@ def _initialize_visualizer(self): self.vis.add_geometry(self.source) self.vis.add_geometry(self.keypoints) self.vis.add_geometry(self.target) + self.vis.add_geometry(self.voxel_map) self._set_black_background(self.vis) self.vis.get_render_option().point_size = 1 + self.vis.get_render_option().mesh_show_wireframe = True print( f"{w_name} initialized. Press:\n" "\t[SPACE] to pause/start\n" @@ -130,6 +136,7 @@ def _register_key_callbacks(self): self._register_key_callback(["F"], self._toggle_source) self._register_key_callback(["K"], self._toggle_keypoints) self._register_key_callback(["M"], self._toggle_map) + self._register_key_callback(["G"], self._toggle_voxel_grid) self._register_key_callback(["T"], self._toggle_trajectory) self._register_key_callback(["B"], self._set_black_background) self._register_key_callback(["W"], self._set_white_background) @@ -172,6 +179,10 @@ def _toggle_map(self, vis): self.render_map = not self.render_map return False + def _toggle_voxel_grid(self, vis): + self.render_voxel_grid = not self.render_voxel_grid + return False + def _toggle_view(self, vis): self.global_view = not self.global_view self._trajectory_handle() @@ -194,7 +205,7 @@ def _trajectory_handle(self): for frame in self.frames: self.vis.remove_geometry(frame, reset_bounding_box=False) - def _update_geometries(self, source, keypoints, target, pose): + def _update_geometries(self, source, keypoints, target_map, pose): # Source hot frame if self.render_source: self.source.points = self.o3d.utility.Vector3dVector(source) @@ -215,7 +226,7 @@ def _update_geometries(self, source, keypoints, target, pose): # Target Map if self.render_map: - target = copy.deepcopy(target) + target = target_map.point_cloud() self.target.points = self.o3d.utility.Vector3dVector(target) if self.global_view: self.target.paint_uniform_color(GRAY) @@ -224,6 +235,18 @@ def _update_geometries(self, source, keypoints, target, pose): else: self.target.points = self.o3d.utility.Vector3dVector() + # VoxelHashMap + if self.render_voxel_grid: + self.voxel_map.clear() + self.voxel_map.voxel_size = 1.0 + inv_pose = np.linalg.inv(pose) + for voxel in target_map.get_voxels(): + if not self.global_view: + voxel = np.dot(inv_pose, np.append(voxel, 1))[:3].astype(np.int32) + self.voxel_map.add_voxel(self.o3d.geometry.Voxel(voxel, LIGHT_GRAY)) + else: + self.voxel_map.clear() + # Update always a list with all the trajectories new_frame = self.o3d.geometry.TriangleMesh.create_sphere(SPHERE_SIZE) new_frame.paint_uniform_color(BLUE) @@ -237,6 +260,7 @@ def _update_geometries(self, source, keypoints, target, pose): self.vis.update_geometry(self.keypoints) self.vis.update_geometry(self.source) self.vis.update_geometry(self.target) + self.vis.update_geometry(self.voxel_map) if self.reset_bounding_box: self.vis.reset_view_point(True) self.reset_bounding_box = False From 4dddcd29654accd47f7576acfba7d8740c543e43 Mon Sep 17 00:00:00 2001 From: Ignacio Vizzo Date: Mon, 5 Aug 2024 17:34:15 +0200 Subject: [PATCH 2/5] Hack voxel size around visualizer for debugging --- python/kiss_icp/pipeline.py | 1 + python/kiss_icp/tools/visualizer.py | 11 ++++++++--- 2 files changed, 9 insertions(+), 3 deletions(-) diff --git a/python/kiss_icp/pipeline.py b/python/kiss_icp/pipeline.py index c3cbb169..2bff0d0f 100644 --- a/python/kiss_icp/pipeline.py +++ b/python/kiss_icp/pipeline.py @@ -77,6 +77,7 @@ def __init__( # Visualizer self.visualizer = RegistrationVisualizer() if visualize else StubVisualizer() + self.visualizer.voxel_size = self.config.mapping.voxel_size if hasattr(self._dataset, "use_global_visualizer"): self.visualizer.global_view = self._dataset.use_global_visualizer diff --git a/python/kiss_icp/tools/visualizer.py b/python/kiss_icp/tools/visualizer.py index 12ba31a9..bfe342f7 100644 --- a/python/kiss_icp/tools/visualizer.py +++ b/python/kiss_icp/tools/visualizer.py @@ -40,6 +40,7 @@ class StubVisualizer(ABC): def __init__(self): + self.voxel_size = 1.0 pass def update(self, source, keypoints, target_map, pose): @@ -65,7 +66,8 @@ def __init__(self): self.keypoints = self.o3d.geometry.PointCloud() self.target = self.o3d.geometry.PointCloud() self.voxel_map = self.o3d.geometry.VoxelGrid() - self.voxel_map.voxel_size = 1.0 + self.voxel_size = 1.0 + self.voxel_map.voxel_size = self.voxel_size self.frames = [] # Initialize visualizer @@ -238,11 +240,14 @@ def _update_geometries(self, source, keypoints, target_map, pose): # VoxelHashMap if self.render_voxel_grid: self.voxel_map.clear() - self.voxel_map.voxel_size = 1.0 + self.voxel_map.voxel_size = self.voxel_size inv_pose = np.linalg.inv(pose) for voxel in target_map.get_voxels(): if not self.global_view: - voxel = np.dot(inv_pose, np.append(voxel, 1))[:3].astype(np.int32) + voxel = ( + np.dot(inv_pose, np.append(voxel * self.voxel_size, 1))[:3] + / self.voxel_size + ).astype(np.int32) self.voxel_map.add_voxel(self.o3d.geometry.Voxel(voxel, LIGHT_GRAY)) else: self.voxel_map.clear() From 346886495d91634a07179637a66de5dc5ad68022 Mon Sep 17 00:00:00 2001 From: Ignacio Vizzo Date: Mon, 5 Aug 2024 18:11:03 +0200 Subject: [PATCH 3/5] We can't rotate the voxels to a local view because of Open3D This feature is missing, and also a bit hard to implement in terms of rendering, etc. As open3d, and also we, assume the voxel grid is always aligned with the origin --- python/kiss_icp/tools/visualizer.py | 22 ++++++++++++---------- 1 file changed, 12 insertions(+), 10 deletions(-) diff --git a/python/kiss_icp/tools/visualizer.py b/python/kiss_icp/tools/visualizer.py index bfe342f7..d05dce2d 100644 --- a/python/kiss_icp/tools/visualizer.py +++ b/python/kiss_icp/tools/visualizer.py @@ -79,7 +79,7 @@ def __init__(self): self.render_map = True self.render_source = True self.render_keypoints = False - self.render_voxel_grid = True + self.render_voxel_grid = False self.global_view = False self.render_trajectory = True # Cache the state of the visualizer @@ -182,6 +182,12 @@ def _toggle_map(self, vis): return False def _toggle_voxel_grid(self, vis): + if not self.global_view and not self.render_voxel_grid: + print( + "[WARNING] Can't render voxel grid in local view. " + "Open3D does not support VoxelGrid rotation" + ) + return self.render_voxel_grid = not self.render_voxel_grid return False @@ -231,23 +237,19 @@ def _update_geometries(self, source, keypoints, target_map, pose): target = target_map.point_cloud() self.target.points = self.o3d.utility.Vector3dVector(target) if self.global_view: - self.target.paint_uniform_color(GRAY) + # self.target.paint_uniform_color(GRAY) + pass else: self.target.transform(np.linalg.inv(pose)) else: self.target.points = self.o3d.utility.Vector3dVector() # VoxelHashMap - if self.render_voxel_grid: + if self.render_voxel_grid and self.global_view: self.voxel_map.clear() self.voxel_map.voxel_size = self.voxel_size - inv_pose = np.linalg.inv(pose) - for voxel in target_map.get_voxels(): - if not self.global_view: - voxel = ( - np.dot(inv_pose, np.append(voxel * self.voxel_size, 1))[:3] - / self.voxel_size - ).astype(np.int32) + voxels = target_map.get_voxels() + for voxel in voxels: self.voxel_map.add_voxel(self.o3d.geometry.Voxel(voxel, LIGHT_GRAY)) else: self.voxel_map.clear() From d7ab6c4343146009c609973473c0bd0f03f4e486 Mon Sep 17 00:00:00 2001 From: Ignacio Vizzo Date: Tue, 6 Aug 2024 11:26:18 +0200 Subject: [PATCH 4/5] Update cpp/kiss_icp/core/VoxelHashMap.cpp Co-authored-by: Luca Lobefaro --- cpp/kiss_icp/core/VoxelHashMap.cpp | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/cpp/kiss_icp/core/VoxelHashMap.cpp b/cpp/kiss_icp/core/VoxelHashMap.cpp index a5cb6ef8..5f9d4df6 100644 --- a/cpp/kiss_icp/core/VoxelHashMap.cpp +++ b/cpp/kiss_icp/core/VoxelHashMap.cpp @@ -58,6 +58,11 @@ std::vector VoxelHashMap::Pointcloud() const { // Get the indices of the occupided voxels as points, mainly used for visualization std::vector VoxelHashMap::GetVoxels() const { +std::vector voxels(map_.size()); + std::transform(map_.cbegin(), map_.cend(), voxels.begin(), + [](const auto &map_element) { return map_element.first; }); + return voxels; +} std::vector voxels; voxels.reserve(map_.size()); for (const auto &[voxel, voxel_block] : map_) { From 0d08b5b34e9146e06bf15f91f94ed17771e701d2 Mon Sep 17 00:00:00 2001 From: Ignacio Vizzo Date: Tue, 6 Aug 2024 11:27:13 +0200 Subject: [PATCH 5/5] fix wrong commit --- cpp/kiss_icp/core/VoxelHashMap.cpp | 10 +--------- 1 file changed, 1 insertion(+), 9 deletions(-) diff --git a/cpp/kiss_icp/core/VoxelHashMap.cpp b/cpp/kiss_icp/core/VoxelHashMap.cpp index 5f9d4df6..10248c05 100644 --- a/cpp/kiss_icp/core/VoxelHashMap.cpp +++ b/cpp/kiss_icp/core/VoxelHashMap.cpp @@ -58,19 +58,11 @@ std::vector VoxelHashMap::Pointcloud() const { // Get the indices of the occupided voxels as points, mainly used for visualization std::vector VoxelHashMap::GetVoxels() const { -std::vector voxels(map_.size()); + std::vector voxels(map_.size()); std::transform(map_.cbegin(), map_.cend(), voxels.begin(), [](const auto &map_element) { return map_element.first; }); return voxels; } - std::vector voxels; - voxels.reserve(map_.size()); - for (const auto &[voxel, voxel_block] : map_) { - (void)voxel_block; - voxels.emplace_back(voxel); - } - return voxels; -} void VoxelHashMap::Update(const std::vector &points, const Eigen::Vector3d &origin) {