Kissualizer Update: Inspection Mode and more Keybindings

.gitignore

@@ -6,6 +6,7 @@ _skbuild/
 .gitlab-ci-local/
 .polyscope.ini
 imgui.ini
+kisshot_*
 
 # Created by https://www.toptal.com/developers/gitignore/api/python,c++
 # Edit at https://www.toptal.com/developers/gitignore?templates=python,c++

cpp/kiss_icp/core/VoxelHashMap.cpp

@@ -95,6 +95,14 @@ std::vector<Eigen::Vector3d> VoxelHashMap::Pointcloud() const {
     return points;
 }
 
+// Get the indices of the occupided voxels as points, mainly used for visualization
+std::vector<Voxel> VoxelHashMap::GetVoxels() const {
+    std::vector<Voxel> voxels(map_.size());
+    std::transform(map_.cbegin(), map_.cend(), voxels.begin(),
+                   [](const auto &map_element) { return map_element.first; });
+    return voxels;
+}
+
 void VoxelHashMap::Update(const std::vector<Eigen::Vector3d> &points,
                           const Eigen::Vector3d &origin) {
     AddPoints(points);

cpp/kiss_icp/core/VoxelHashMap.hpp

@@ -48,6 +48,7 @@ struct VoxelHashMap {
     void AddPoints(const std::vector<Eigen::Vector3d> &points);
     void RemovePointsFarFromLocation(const Eigen::Vector3d &origin);
     std::vector<Eigen::Vector3d> Pointcloud() const;
+    std::vector<Voxel> GetVoxels() const;
     std::tuple<Eigen::Vector3d, double> GetClosestNeighbor(const Eigen::Vector3d &point) const;
 
     double voxel_size_;

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())

python/kiss_icp/pipeline.py

@@ -80,7 +80,9 @@ def __init__(
         self._vis_infos = {
             "max_range": self.config.data.max_range,
             "min_range": self.config.data.min_range,
+            "voxel_size": self.config.mapping.voxel_size,
         }
+        self.visualizer.set_voxel_size(self.config.mapping.voxel_size)
         if hasattr(self._dataset, "use_global_visualizer"):
             self.visualizer._global_view = self._dataset.use_global_visualizer
 

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_<Registration> internal_registration(m, "_Registration", "Don't use this");

python/kiss_icp/tools/visualizer.py

@@ -34,6 +34,10 @@
 LOCAL_VIEW_BUTTON = "LOCAL VIEW\n\t\t [G]"
 GLOBAL_VIEW_BUTTON = "GLOBAL VIEW\n\t\t  [G]"
 CENTER_VIEWPOINT_BUTTON = "CENTER VIEWPOINT\n\t\t\t\t[C]"
+SHOW_VOXEL_GRID_BUTTON = "SHOW VOXEL GRID\n\t\t\t\t[V]"
+HIDE_VOXEL_GRID_BUTTON = "HIDE VOXEL GRID\n\t\t\t  [V]"
+ORTHO_ON_BUTTON = "ORTHO VIEW ON\n\t\t\t [O]"
+ORTHO_OFF_BUTTON = "ORTHO VIEW OFF\n\t\t\t  [O]"
 QUIT_BUTTON = "QUIT\n  [Q]"
 
 # Colors
@@ -42,6 +46,7 @@
 KEYPOINTS_COLOR = [1, 0.7568, 0.0274]
 LOCAL_MAP_COLOR = [0.0, 0.3019, 0.2509]
 TRAJECTORY_COLOR = [0.1176, 0.5333, 0.8980]
+VOXEL_GRID_COLOR = [0.9, 0.9, 0.9]
 
 # Size constants
 FRAME_PTS_SIZE = 0.06
@@ -53,7 +58,10 @@ class StubVisualizer(ABC):
     def __init__(self):
         pass
 
-    def update(self, source, keypoints, target_map, pose, vis_infos):
+    def update(self, source, keypoints, local_map, pose, vis_infos):
+        pass
+
+    def set_voxel_size(self, voxel_size):
         pass
 
 
@@ -77,27 +85,36 @@ def __init__(self):
         self._toggle_frame = True
         self._toggle_keypoints = True
         self._toggle_map = True
+        self._toggle_voxel_grid = False
+        self._toggle_ortho = False
         self._global_view = False
 
         # Create data
         self._trajectory = []
         self._last_pose = np.eye(4)
         self._vis_infos = dict()
         self._selected_pose = ""
+        self._voxel_grid_nodes = np.array([])
+        self._voxel_grid_edges = np.array([])
+        self._last_local_map = None
+        self._voxel_size = 1.0
 
         # Initialize Visualizer
         self._initialize_visualizer()
 
-    def update(self, source, keypoints, target_map, pose, vis_infos: dict):
+    def update(self, source, keypoints, local_map, pose, vis_infos: dict):
         self._vis_infos = dict(sorted(vis_infos.items(), key=lambda item: len(item[0])))
-        self._update_geometries(source, keypoints, target_map, pose)
         self._last_pose = pose
+        self._update_geometries(source, keypoints, local_map, pose)
         while self._block_execution:
             self._ps.frame_tick()
             if self._play_mode:
                 break
         self._block_execution = not self._block_execution
 
+    def set_voxel_size(self, voxel_size):
+        self._voxel_size = voxel_size
+
     # Private Interface ---------------------------------------------------------------------------
     def _initialize_visualizer(self):
         self._ps.set_program_name("KissICP Visualizer")
@@ -107,8 +124,9 @@ def _initialize_visualizer(self):
         self._ps.set_verbosity(0)
         self._ps.set_user_callback(self._main_gui_callback)
         self._ps.set_build_default_gui_panels(False)
+        self._ps.set_SSAA_factor(4)
 
-    def _update_geometries(self, source, keypoints, target_map, pose):
+    def _update_geometries(self, source, keypoints, local_map, pose):
         # CURRENT FRAME
         frame_cloud = self._ps.register_point_cloud(
             "current_frame",
@@ -137,7 +155,7 @@ def _update_geometries(self, source, keypoints, target_map, pose):
         # LOCAL MAP
         map_cloud = self._ps.register_point_cloud(
             "local_map",
-            target_map.point_cloud(),
+            local_map.point_cloud(),
             color=LOCAL_MAP_COLOR,
             point_render_mode="quad",
         )
@@ -148,6 +166,11 @@ def _update_geometries(self, source, keypoints, target_map, pose):
             map_cloud.set_transform(np.linalg.inv(pose))
         map_cloud.set_enabled(self._toggle_map)
 
+        # VOXEL GRID (only if toggled)
+        self._last_local_map = local_map
+        if self._toggle_voxel_grid:
+            self._register_voxel_grid()
+
         # TRAJECTORY (only visible in global view)
         self._trajectory.append(pose[:3, 3])
         if self._global_view:
@@ -164,11 +187,80 @@ def _register_trajectory(self):
     def _unregister_trajectory(self):
         self._ps.remove_point_cloud("trajectory")
 
+    def _register_voxel_grid(self):
+        if self._last_local_map is None:
+            return
+        voxels = self._last_local_map.get_voxels()
+        self._voxel_grid_nodes = np.zeros((voxels.shape[0] * 8, 3), dtype=np.float64)
+        self._voxel_grid_edges = np.zeros((voxels.shape[0] * 12, 2), dtype=np.int64)
+
+        for idx, voxel in enumerate(voxels):
+            self._generate_new_voxel(voxel, idx)
+
+        voxel_grid = self._ps.register_curve_network(
+            "voxel_grid", self._voxel_grid_nodes, self._voxel_grid_edges, color=VOXEL_GRID_COLOR
+        )
+        voxel_grid.set_radius(0.01, relative=False)
+        if self._global_view:
+            voxel_grid.set_transform(np.eye(4))
+        else:
+            voxel_grid.set_transform(np.linalg.inv(self._last_pose))
+
+    def _unregister_voxel_grid(self):
+        if self._ps.has_curve_network("voxel_grid"):
+            self._ps.remove_curve_network("voxel_grid")
+
+    def _generate_new_voxel(self, voxel: np.ndarray, idx: int):
+        verts = np.array(
+            [
+                [0, 0, 0],
+                [1, 0, 0],
+                [1, 1, 0],
+                [0, 1, 0],
+                [0, 0, 1],
+                [1, 0, 1],
+                [1, 1, 1],
+                [0, 1, 1],
+            ]
+        ).astype(np.float64)
+        edges = np.array(
+            [
+                [0, 1],
+                [1, 2],
+                [2, 3],
+                [0, 3],
+                [0, 4],
+                [5, 4],
+                [1, 5],
+                [5, 6],
+                [2, 6],
+                [6, 7],
+                [3, 7],
+                [4, 7],
+            ]
+        ).astype(np.int64)
+        verts = verts + voxel
+        verts = verts * self._voxel_size
+        edges += idx * 8
+        self._voxel_grid_nodes[idx * 8 : idx * 8 + 8] = verts
+        self._voxel_grid_edges[idx * 12 : idx * 12 + 12] = edges
+
     # GUI Callbacks ---------------------------------------------------------------------------
     def _start_pause_callback(self):
         button_name = PAUSE_BUTTON if self._play_mode else START_BUTTON
         if self._gui.Button(button_name) or self._gui.IsKeyPressed(self._gui.ImGuiKey_Space):
             self._play_mode = not self._play_mode
+            if self._play_mode:
+                self._toggle_voxel_grid = False
+                self._unregister_voxel_grid()
+                self._ps.set_SSAA_factor(1)
+                if self._toggle_ortho:
+                    self._ps.reset_camera_to_home_view()  # to reset FoV
+                    self._ps.set_view_projection_mode("perspective")
+                    self._ps.set_navigation_style("turntable")
+                    self._toggle_ortho = False
+            else:
+                self._ps.set_SSAA_factor(4)
 
     def _next_frame_callback(self):
         if self._gui.Button(NEXT_FRAME_BUTTON) or self._gui.IsKeyPressed(self._gui.ImGuiKey_N):
@@ -185,8 +277,6 @@ def _vis_infos_callback(self):
         if self._gui.TreeNodeEx("Odometry Information", self._gui.ImGuiTreeNodeFlags_DefaultOpen):
             for key in self._vis_infos:
                 self._gui.TextUnformatted(f"{key}: {self._vis_infos[key]}")
-            if not self._play_mode and self._global_view:
-                self._gui.TextUnformatted(f"Selected Pose: {self._selected_pose}")
             self._gui.TreePop()
 
     def _center_viewpoint_callback(self):
@@ -203,8 +293,11 @@ def _toggle_buttons_andslides_callback(self):
         if changed:
             self._ps.get_point_cloud("current_frame").set_radius(self._frame_size, relative=False)
         self._gui.SameLine()
-        changed, self._toggle_frame = self._gui.Checkbox("Frame Cloud", self._toggle_frame)
-        if changed:
+        f_key_pressed = self._gui.IsKeyPressed(self._gui.ImGuiKey_F)
+        if f_key_pressed:
+            self._toggle_frame = not self._toggle_frame
+        changed, self._toggle_frame = self._gui.Checkbox("[F] Frame Cloud", self._toggle_frame)
+        if changed or f_key_pressed:
             self._ps.get_point_cloud("current_frame").set_enabled(self._toggle_frame)
 
         # KEYPOINTS
@@ -214,8 +307,13 @@ def _toggle_buttons_andslides_callback(self):
         if changed:
             self._ps.get_point_cloud("keypoints").set_radius(self._keypoints_size, relative=False)
         self._gui.SameLine()
-        changed, self._toggle_keypoints = self._gui.Checkbox("Keypoints", self._toggle_keypoints)
-        if changed:
+        k_key_pressed = self._gui.IsKeyPressed(self._gui.ImGuiKey_K)
+        if k_key_pressed:
+            self._toggle_keypoints = not self._toggle_keypoints
+        changed, self._toggle_keypoints = self._gui.Checkbox(
+            "[K] Keypoints", self._toggle_keypoints
+        )
+        if changed or k_key_pressed:
             self._ps.get_point_cloud("keypoints").set_enabled(self._toggle_keypoints)
 
         # LOCAL MAP
@@ -225,8 +323,11 @@ def _toggle_buttons_andslides_callback(self):
         if changed:
             self._ps.get_point_cloud("local_map").set_radius(self._map_size, relative=False)
         self._gui.SameLine()
-        changed, self._toggle_map = self._gui.Checkbox("Local Map", self._toggle_map)
-        if changed:
+        m_key_pressed = self._gui.IsKeyPressed(self._gui.ImGuiKey_M)
+        if m_key_pressed:
+            self._toggle_map = not self._toggle_map
+        changed, self._toggle_map = self._gui.Checkbox("[M] Local Map", self._toggle_map)
+        if changed or m_key_pressed:
             self._ps.get_point_cloud("local_map").set_enabled(self._toggle_map)
 
     def _background_color_callback(self):
@@ -237,6 +338,39 @@ def _background_color_callback(self):
         if changed:
             self._ps.set_background_color(self._background_color)
 
+    def _inspection_callback(self):
+        info_string = "Double-click on the trajectory to visualize it (only global view):"
+        if self._gui.TreeNodeEx("Inspection", self._gui.ImGuiTreeNodeFlags_DefaultOpen):
+            voxel_grid_button_name = (
+                HIDE_VOXEL_GRID_BUTTON if self._toggle_voxel_grid else SHOW_VOXEL_GRID_BUTTON
+            )
+            if self._gui.Button(voxel_grid_button_name) or self._gui.IsKeyPressed(
+                self._gui.ImGuiKey_V
+            ):
+                self._toggle_voxel_grid = not self._toggle_voxel_grid
+                if self._toggle_voxel_grid:
+                    self._register_voxel_grid()
+                else:
+                    self._unregister_voxel_grid()
+
+            ortho_button_name = ORTHO_OFF_BUTTON if self._toggle_ortho else ORTHO_ON_BUTTON
+            self._gui.SameLine()
+            if self._gui.Button(ortho_button_name) or self._gui.IsKeyPressed(self._gui.ImGuiKey_O):
+                self._toggle_ortho = not self._toggle_ortho
+                if self._toggle_ortho:
+                    self._ps.set_view_projection_mode("orthographic")
+                    self._ps.set_navigation_style("planar")
+                else:
+                    self._ps.reset_camera_to_home_view()  # to reset Fov
+                    self._ps.set_view_projection_mode("perspective")
+                    self._ps.set_navigation_style("turntable")
+                self._ps.reset_camera_to_home_view()
+
+            self._gui.TextUnformatted(info_string)
+            if self._selected_pose != "":
+                self._gui.TextUnformatted(f"\t\tSelected Pose: {self._selected_pose}")
+            self._gui.TreePop()
+
     def _global_view_callback(self):
         button_name = LOCAL_VIEW_BUTTON if self._global_view else GLOBAL_VIEW_BUTTON
         if self._gui.Button(button_name) or self._gui.IsKeyPressed(self._gui.ImGuiKey_G):
@@ -246,11 +380,17 @@ def _global_view_callback(self):
                 self._ps.get_point_cloud("keypoints").set_transform(self._last_pose)
                 self._ps.get_point_cloud("local_map").set_transform(np.eye(4))
                 self._register_trajectory()
+                if self._toggle_voxel_grid:
+                    self._ps.get_curve_network("voxel_grid").set_transform(np.eye(4))
             else:
                 self._ps.get_point_cloud("current_frame").set_transform(np.eye(4))
                 self._ps.get_point_cloud("keypoints").set_transform(np.eye(4))
                 self._ps.get_point_cloud("local_map").set_transform(np.linalg.inv(self._last_pose))
                 self._unregister_trajectory()
+                if self._toggle_voxel_grid:
+                    self._ps.get_curve_network("voxel_grid").set_transform(
+                        np.linalg.inv(self._last_pose)
+                    )
             self._ps.reset_camera_to_home_view()
 
     def _quit_callback(self):
@@ -271,7 +411,7 @@ def _trajectory_pick_callback(self):
             name, idx = self._ps.get_selection()
             if name == "trajectory" and self._ps.has_point_cloud(name):
                 pose = self._trajectory[idx]
-                self._selected_pose = f"x: {pose[0]:7.3f}, y: {pose[1]:7.3f}, z: {pose[2]:7.3f}>"
+                self._selected_pose = f"x: {pose[0]:7.3f}, y: {pose[1]:7.3f}, z: {pose[2]:7.3f}"
             else:
                 self._selected_pose = ""
 
@@ -288,11 +428,14 @@ def _main_gui_callback(self):
         self._gui.Separator()
         self._toggle_buttons_andslides_callback()
         self._background_color_callback()
+        if not self._play_mode:
+            self._gui.Separator()
+            self._inspection_callback()
+            self._gui.Separator()
+            self._trajectory_pick_callback()
         self._global_view_callback()
         self._gui.SameLine()
         self._center_viewpoint_callback()
         self._gui.Separator()
         self._quit_callback()
 
-        # Mouse callbacks
-        self._trajectory_pick_callback()