From 34b7e71fb96f4e342a7621bdb61c71844b8e9a19 Mon Sep 17 00:00:00 2001 From: Luca Lobefaro Date: Fri, 2 Aug 2024 10:02:41 +0200 Subject: [PATCH] Trajectory as points, not anymore a curve --- python/kiss_icp/tools/visualizer.py | 35 ++++++++++++----------------- 1 file changed, 14 insertions(+), 21 deletions(-) diff --git a/python/kiss_icp/tools/visualizer.py b/python/kiss_icp/tools/visualizer.py index e297e80e..5de2e3b0 100644 --- a/python/kiss_icp/tools/visualizer.py +++ b/python/kiss_icp/tools/visualizer.py @@ -70,19 +70,18 @@ def __init__(self, config: KISSConfig): # Initialize GUI controls self._background_color = BACKGROUND_COLOR + self._frame_size = FRAME_PTS_SIZE + self._keypoints_size = KEYPOINTS_PTS_SIZE + self._map_size = MAP_PTS_SIZE 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) @@ -154,14 +153,20 @@ def _update_geometries(self, source, keypoints, target_map, pose): # TRAJECTORY (only visible in global view) self._trajectory.append(pose[:3, 3]) - n_poses = len(self._trajectory) - if n_poses > 1: - self._trajectory_edges.append([n_poses - 2, n_poses - 1]) - else: - self._trajectory_edges.append([0, 0]) if self._global_view: self._register_trajectory() + def _register_trajectory(self): + trajectory_cloud = self._ps.register_point_cloud( + "trajectory", + np.asarray(self._trajectory), + color=TRAJECTORY_COLOR, + ) + trajectory_cloud.set_radius(0.3, relative=False) + + def _unregister_trajectory(self): + self._ps.remove_point_cloud("trajectory") + # GUI Callbacks --------------------------------------------------------------------------- def _start_pause_callback(self): button_name = PAUSE_BUTTON if self._play_mode else START_BUTTON @@ -233,18 +238,6 @@ def _background_color_callback(self): 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):