diff --git a/python/kiss_icp/tools/visualizer.py b/python/kiss_icp/tools/visualizer.py index d30215cf..e297e80e 100644 --- a/python/kiss_icp/tools/visualizer.py +++ b/python/kiss_icp/tools/visualizer.py @@ -58,281 +58,245 @@ def update(self, source, keypoints, target_map, pose, last_time): pass -def start_pause_callback(): - button_name = PAUSE_BUTTON if Kissualizer.play_mode else START_BUTTON - if Kissualizer.polyscope.imgui.Button(button_name) or Kissualizer.polyscope.imgui.IsKeyPressed( - Kissualizer.polyscope.imgui.ImGuiKey_Space - ): - Kissualizer.play_mode = not Kissualizer.play_mode - - -def next_frame_callback(): - if Kissualizer.polyscope.imgui.Button( - NEXT_FRAME_BUTTON - ) or Kissualizer.polyscope.imgui.IsKeyPressed(Kissualizer.polyscope.imgui.ImGuiKey_N): - Kissualizer.block_execution = not Kissualizer.block_execution - - -def screenshot_callback(): - # TODO: this is just for demo, set a more valid path - if Kissualizer.polyscope.imgui.Button( - SCREENSHOT_BUTTON - ) or Kissualizer.polyscope.imgui.IsKeyPressed(Kissualizer.polyscope.imgui.ImGuiKey_S): - image_filename = "screenshot.jpg" - Kissualizer.polyscope.screenshot(image_filename) - Kissualizer.polyscope.info(f"Screenshot save at: {image_filename}") - - -def fps_callback(): - if Kissualizer.play_mode: - total_time_s = np.sum(Kissualizer.times) * 1e-9 - current_fps = float(len(Kissualizer.times) / total_time_s) if total_time_s > 0 else 0 - Kissualizer.polyscope.imgui.TextUnformatted(f"FPS: {int(np.floor(current_fps))}") - - -def center_viewpoint_callback(): - if Kissualizer.polyscope.imgui.Button( - CENTER_VIEWPOINT_BUTTON - ) or Kissualizer.polyscope.imgui.IsKeyPressed(Kissualizer.polyscope.imgui.ImGuiKey_C): - Kissualizer.polyscope.reset_camera_to_home_view() - - -def toggle_buttons_andslides_callback(): - # FRAME - changed, Kissualizer.frame_size = Kissualizer.polyscope.imgui.SliderFloat( - "##frame_size", Kissualizer.frame_size, v_min=0.01, v_max=0.6 - ) - if changed: - Kissualizer.polyscope.get_point_cloud("current_frame").set_radius( - Kissualizer.frame_size, relative=False - ) - Kissualizer.polyscope.imgui.SameLine() - changed, Kissualizer.toggle_frame = Kissualizer.polyscope.imgui.Checkbox( - "Frame Cloud", Kissualizer.toggle_frame - ) - if changed: - Kissualizer.polyscope.get_point_cloud("current_frame").set_enabled(Kissualizer.toggle_frame) - - # KEYPOINTS - changed, Kissualizer.keypoints_size = Kissualizer.polyscope.imgui.SliderFloat( - "##keypoints_size", Kissualizer.keypoints_size, v_min=0.01, v_max=0.6 - ) - if changed: - Kissualizer.polyscope.get_point_cloud("keypoints").set_radius( - Kissualizer.keypoints_size, relative=False - ) - Kissualizer.polyscope.imgui.SameLine() - changed, Kissualizer.toggle_keypoints = Kissualizer.polyscope.imgui.Checkbox( - "Keypoints", Kissualizer.toggle_keypoints - ) - if changed: - Kissualizer.polyscope.get_point_cloud("keypoints").set_enabled(Kissualizer.toggle_keypoints) - - # LOCAL MAP - changed, Kissualizer.map_size = Kissualizer.polyscope.imgui.SliderFloat( - "##map_size", Kissualizer.map_size, v_min=0.01, v_max=0.6 - ) - if changed: - Kissualizer.polyscope.get_point_cloud("local_map").set_radius( - Kissualizer.map_size, relative=False - ) - Kissualizer.polyscope.imgui.SameLine() - changed, Kissualizer.toggle_map = Kissualizer.polyscope.imgui.Checkbox( - "Local Map", Kissualizer.toggle_map - ) - if changed: - Kissualizer.polyscope.get_point_cloud("local_map").set_enabled(Kissualizer.toggle_map) - - -def background_color_callback(): - changed, Kissualizer.background_color = Kissualizer.polyscope.imgui.ColorEdit3( - "Background Color", - Kissualizer.background_color, - ) - if changed: - Kissualizer.polyscope.set_background_color(Kissualizer.background_color) - - -def register_trajectory(): - trajectory_curve = Kissualizer.polyscope.register_curve_network( - "trajectory", - np.asarray(Kissualizer.trajectory), - np.asarray(Kissualizer.trajectory_edges), - color=TRAJECTORY_COLOR, - ) - trajectory_curve.set_radius(0.3, relative=False) - - -def unregister_trajectory(): - Kissualizer.polyscope.remove_curve_network("trajectory") - - -def global_view_callback(): - button_name = LOCAL_VIEW_BUTTON if Kissualizer.global_view else GLOBAL_VIEW_BUTTON - if Kissualizer.polyscope.imgui.Button(button_name) or Kissualizer.polyscope.imgui.IsKeyPressed( - Kissualizer.polyscope.imgui.ImGuiKey_G - ): - Kissualizer.global_view = not Kissualizer.global_view - if Kissualizer.global_view: - Kissualizer.polyscope.get_point_cloud("current_frame").set_transform( - Kissualizer.last_pose - ) - Kissualizer.polyscope.get_point_cloud("keypoints").set_transform(Kissualizer.last_pose) - Kissualizer.polyscope.get_point_cloud("local_map").set_transform(np.eye(4)) - register_trajectory() - else: - Kissualizer.polyscope.get_point_cloud("current_frame").set_transform(np.eye(4)) - Kissualizer.polyscope.get_point_cloud("keypoints").set_transform(np.eye(4)) - Kissualizer.polyscope.get_point_cloud("local_map").set_transform( - np.linalg.inv(Kissualizer.last_pose) - ) - unregister_trajectory() - Kissualizer.polyscope.reset_camera_to_home_view() - - -def configuration_callback(): - if Kissualizer.polyscope.imgui.TreeNode("Parameters"): - config = Kissualizer.config - Kissualizer.polyscope.imgui.TextUnformatted(f"Voxel size: {config.mapping.voxel_size}") - Kissualizer.polyscope.imgui.TextUnformatted( - f"# Points per voxel: {config.mapping.max_points_per_voxel}" - ) - Kissualizer.polyscope.imgui.TextUnformatted(f"Max range: {config.data.max_range}") - Kissualizer.polyscope.imgui.TextUnformatted(f"Min range: {config.data.min_range}") - Kissualizer.polyscope.imgui.TreePop() - - -def quit_callback(): - if ( - Kissualizer.polyscope.imgui.Button(QUIT_BUTTON) - or Kissualizer.polyscope.imgui.IsKeyPressed(Kissualizer.polyscope.imgui.ImGuiKey_Escape) - or Kissualizer.polyscope.imgui.IsKeyPressed(Kissualizer.polyscope.imgui.ImGuiKey_Q) - ): - print("Destroying Visualizer") - Kissualizer.polyscope.unshow() - os._exit(0) - - -def main_gui_callback(): - start_pause_callback() - if not Kissualizer.play_mode: - Kissualizer.polyscope.imgui.SameLine() - next_frame_callback() - Kissualizer.polyscope.imgui.SameLine() - screenshot_callback() - Kissualizer.polyscope.imgui.SameLine() - fps_callback() - Kissualizer.polyscope.imgui.Separator() - configuration_callback() - Kissualizer.polyscope.imgui.Separator() - toggle_buttons_andslides_callback() - background_color_callback() - global_view_callback() - Kissualizer.polyscope.imgui.SameLine() - center_viewpoint_callback() - Kissualizer.polyscope.imgui.Separator() - quit_callback() - - class Kissualizer(StubVisualizer): - # Static parameters - polyscope = None - config = None - background_color = BACKGROUND_COLOR - block_execution = True - play_mode = False - frame_size = FRAME_PTS_SIZE - toggle_frame = True - keypoints_size = KEYPOINTS_PTS_SIZE - toggle_keypoints = True - map_size = MAP_PTS_SIZE - toggle_map = True - global_view = False - trajectory = [] - trajectory_edges = [] - times = [] - last_pose = np.eye(4) - # Public Interface ---------------------------------------------------------------------------- def __init__(self, config: KISSConfig): try: - Kissualizer.polyscope = importlib.import_module("polyscope") + self._ps = importlib.import_module("polyscope") + self._gui = self._ps.imgui except ModuleNotFoundError as err: print(f'polyscope is not installed on your system, run "pip install polyscope"') exit(1) + # Initialize GUI controls + self._background_color = BACKGROUND_COLOR + self._block_execution = True + self._play_mode = False + self._frame_size = FRAME_PTS_SIZE + self._toggle_frame = True + self._keypoints_size = KEYPOINTS_PTS_SIZE + self._toggle_keypoints = True + self._map_size = MAP_PTS_SIZE + self._toggle_map = True + self._global_view = False + + # Create data + self._trajectory = [] + self._trajectory_edges = [] + self._times = [] + self._last_pose = np.eye(4) + # Initialize Visualizer - Kissualizer.polyscope.set_program_name("KissICP Visualizer") - Kissualizer.polyscope.init() self._initialize_visualizer() # Initialize parameters - Kissualizer.config = config + self._config = config def update(self, source, keypoints, target_map, pose, last_time): - Kissualizer.times.append(last_time) + self._times.append(last_time) self._update_geometries(source, keypoints, target_map, pose) - Kissualizer.last_pose = pose - while Kissualizer.block_execution: - Kissualizer.polyscope.frame_tick() - if Kissualizer.play_mode: + self._last_pose = pose + while self._block_execution: + self._ps.frame_tick() + if self._play_mode: break - Kissualizer.block_execution = not Kissualizer.block_execution + self._block_execution = not self._block_execution # Private Interface --------------------------------------------------------------------------- def _initialize_visualizer(self): - Kissualizer.polyscope.set_ground_plane_mode("none") - Kissualizer.polyscope.set_background_color(BACKGROUND_COLOR) - Kissualizer.polyscope.set_verbosity(0) - Kissualizer.polyscope.set_user_callback(main_gui_callback) - Kissualizer.polyscope.set_build_default_gui_panels(False) + self._ps.set_program_name("KissICP Visualizer") + self._ps.init() + self._ps.set_ground_plane_mode("none") + self._ps.set_background_color(BACKGROUND_COLOR) + self._ps.set_verbosity(0) + self._ps.set_user_callback(self._main_gui_callback) + self._ps.set_build_default_gui_panels(False) def _update_geometries(self, source, keypoints, target_map, pose): # CURRENT FRAME - frame_cloud = Kissualizer.polyscope.register_point_cloud( + frame_cloud = self._ps.register_point_cloud( "current_frame", source, color=FRAME_COLOR, point_render_mode="quad", ) - frame_cloud.set_radius(Kissualizer.frame_size, relative=False) - if Kissualizer.global_view: + frame_cloud.set_radius(self._frame_size, relative=False) + if self._global_view: frame_cloud.set_transform(pose) else: frame_cloud.set_transform(np.eye(4)) - frame_cloud.set_enabled(Kissualizer.toggle_frame) + frame_cloud.set_enabled(self._toggle_frame) # KEYPOINTS - keypoints_cloud = Kissualizer.polyscope.register_point_cloud( + keypoints_cloud = self._ps.register_point_cloud( "keypoints", keypoints, color=KEYPOINTS_COLOR, point_render_mode="quad" ) - keypoints_cloud.set_radius(Kissualizer.keypoints_size, relative=False) - if Kissualizer.global_view: + keypoints_cloud.set_radius(self._keypoints_size, relative=False) + if self._global_view: keypoints_cloud.set_transform(pose) else: keypoints_cloud.set_transform(np.eye(4)) - keypoints_cloud.set_enabled(Kissualizer.toggle_keypoints) + keypoints_cloud.set_enabled(self._toggle_keypoints) # LOCAL MAP - map_cloud = Kissualizer.polyscope.register_point_cloud( + map_cloud = self._ps.register_point_cloud( "local_map", target_map.point_cloud(), color=LOCAL_MAP_COLOR, point_render_mode="quad", ) - map_cloud.set_radius(Kissualizer.map_size, relative=False) - if Kissualizer.global_view: + map_cloud.set_radius(self._map_size, relative=False) + if self._global_view: map_cloud.set_transform(np.eye(4)) else: map_cloud.set_transform(np.linalg.inv(pose)) - map_cloud.set_enabled(Kissualizer.toggle_map) + map_cloud.set_enabled(self._toggle_map) # TRAJECTORY (only visible in global view) - Kissualizer.trajectory.append(pose[:3, 3]) - n_poses = len(Kissualizer.trajectory) + self._trajectory.append(pose[:3, 3]) + n_poses = len(self._trajectory) if n_poses > 1: - Kissualizer.trajectory_edges.append([n_poses - 2, n_poses - 1]) + self._trajectory_edges.append([n_poses - 2, n_poses - 1]) else: - Kissualizer.trajectory_edges.append([0, 0]) - if Kissualizer.global_view: - register_trajectory() + self._trajectory_edges.append([0, 0]) + if self._global_view: + self._register_trajectory() + + # 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 + + def _next_frame_callback(self): + if self._gui.Button(NEXT_FRAME_BUTTON) or self._gui.IsKeyPressed(self._gui.ImGuiKey_N): + self._block_execution = not self._block_execution + + def _screenshot_callback(self): + # TODO: this is just for demo, set a more valid path + if self._gui.Button(SCREENSHOT_BUTTON) or self._gui.IsKeyPressed(self._gui.ImGuiKey_S): + image_filename = "screenshot.jpg" + self._ps.screenshot(image_filename) + self._ps.info(f"Screenshot save at: {image_filename}") + + def _fps_callback(self): + if self._play_mode: + total_time_s = np.sum(self._times) * 1e-9 + current_fps = float(len(self._times) / total_time_s) if total_time_s > 0 else 0 + self._gui.TextUnformatted(f"FPS: {int(np.floor(current_fps))}") + + def _center_viewpoint_callback(self): + if self._gui.Button(CENTER_VIEWPOINT_BUTTON) or self._gui.IsKeyPressed( + self._gui.ImGuiKey_C + ): + self._ps.reset_camera_to_home_view() + + def _toggle_buttons_andslides_callback(self): + # FRAME + changed, self._frame_size = self._gui.SliderFloat( + "##frame_size", self._frame_size, v_min=0.01, v_max=0.6 + ) + 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: + self._ps.get_point_cloud("current_frame").set_enabled(self._toggle_frame) + + # KEYPOINTS + changed, self._keypoints_size = self._gui.SliderFloat( + "##keypoints_size", self._keypoints_size, v_min=0.01, v_max=0.6 + ) + 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: + self._ps.get_point_cloud("keypoints").set_enabled(self._toggle_keypoints) + + # LOCAL MAP + changed, self._map_size = self._gui.SliderFloat( + "##map_size", self._map_size, v_min=0.01, v_max=0.6 + ) + 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: + self._ps.get_point_cloud("local_map").set_enabled(self._toggle_map) + + def _background_color_callback(self): + changed, self._background_color = self._gui.ColorEdit3( + "Background Color", + self._background_color, + ) + if changed: + self._ps.set_background_color(self._background_color) + + def _register_trajectory(self): + trajectory_curve = self._ps.register_curve_network( + "trajectory", + np.asarray(self._trajectory), + np.asarray(self._trajectory_edges), + color=TRAJECTORY_COLOR, + ) + trajectory_curve.set_radius(0.3, relative=False) + + def _unregister_trajectory(self): + self._ps.remove_curve_network("trajectory") + + 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): + self._global_view = not self._global_view + if self._global_view: + self._ps.get_point_cloud("current_frame").set_transform(self._last_pose) + 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() + 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() + self._ps.reset_camera_to_home_view() + + def _configuration_callback(self): + if self._gui.TreeNode("Parameters"): + self._gui.TextUnformatted(f"Voxel size: {self._config.mapping.voxel_size}") + self._gui.TextUnformatted( + f"# Points per voxel: {self._config.mapping.max_points_per_voxel}" + ) + self._gui.TextUnformatted(f"Max range: {self._config.data.max_range}") + self._gui.TextUnformatted(f"Min range: {self._config.data.min_range}") + self._gui.TreePop() + + def _quit_callback(self): + if ( + self._gui.Button(QUIT_BUTTON) + or self._gui.IsKeyPressed(self._gui.ImGuiKey_Escape) + or self._gui.IsKeyPressed(self._gui.ImGuiKey_Q) + ): + print("Destroying Visualizer") + self._ps.unshow() + os._exit(0) + + def _main_gui_callback(self): + self._start_pause_callback() + if not self._play_mode: + self._gui.SameLine() + self._next_frame_callback() + self._gui.SameLine() + self._screenshot_callback() + self._gui.SameLine() + self._fps_callback() + self._gui.Separator() + self._configuration_callback() + self._gui.Separator() + self._toggle_buttons_andslides_callback() + self._background_color_callback() + self._global_view_callback() + self._gui.SameLine() + self._center_viewpoint_callback() + self._gui.Separator() + self._quit_callback()