| 65 | |
| 66 | |
| 67 | class ClosuresVisualizer: |
| 68 | def __init__(self, ps, gui, localmap_data): |
| 69 | self._ps = ps |
| 70 | self._gui = gui |
| 71 | |
| 72 | # Initialize GUI States |
| 73 | self.states = ClosuresStateMachine() |
| 74 | self.ref_density_viewer = None |
| 75 | self.query_density_viewer = None |
| 76 | self.fig = None |
| 77 | |
| 78 | # Create data |
| 79 | self.localmap_data = localmap_data |
| 80 | self.data = LoopClosureData() |
| 81 | |
| 82 | def update_closures(self, alignment_pose, closure_edge): |
| 83 | self.data.closure_edges.append(closure_edge) |
| 84 | self.data.alignment_pose.append(alignment_pose) |
| 85 | self.data.size += 1 |
| 86 | self.data.current_id = self.data.size - 1 |
| 87 | if self.states.global_view: |
| 88 | self._register_trajectory() |
| 89 | if self.states.toggle_view: |
| 90 | self._render_closure() |
| 91 | |
| 92 | def matplotlib_eventloop(self): |
| 93 | plt.gcf().canvas.draw() |
| 94 | plt.gcf().canvas.start_event_loop(1e-6) |
| 95 | |
| 96 | def close_density_map_fig(self): |
| 97 | plt.close(self.fig) |
| 98 | |
| 99 | def _query_map_callback(self): |
| 100 | changed, self.states.query_points_size = self._gui.SliderFloat( |
| 101 | "##query_size", self.states.query_points_size, v_min=0.01, v_max=0.6 |
| 102 | ) |
| 103 | if changed: |
| 104 | self._ps.get_point_cloud("query_map").set_radius( |
| 105 | self.states.query_points_size, relative=False |
| 106 | ) |
| 107 | self._gui.SameLine() |
| 108 | changed, self.states.view_query = self._gui.Checkbox("Query Map", self.states.view_query) |
| 109 | if changed: |
| 110 | self._ps.get_point_cloud("query_map").set_enabled(self.states.view_query) |
| 111 | |
| 112 | def _reference_map_callback(self): |
| 113 | changed, self.states.reference_points_size = self._gui.SliderFloat( |
| 114 | "##reference_size", self.states.reference_points_size, v_min=0.01, v_max=0.6 |
| 115 | ) |
| 116 | if changed: |
| 117 | self._ps.get_point_cloud("reference_map").set_radius( |
| 118 | self.states.reference_points_size, relative=False |
| 119 | ) |
| 120 | self._gui.SameLine() |
| 121 | changed, self.states.view_reference = self._gui.Checkbox( |
| 122 | "Reference Map", self.states.view_reference |
| 123 | ) |
| 124 | if changed: |