diff --git a/.gitignore b/.gitignore
index 651ff07..0f5f5b9 100644
--- a/.gitignore
+++ b/.gitignore
@@ -124,3 +124,6 @@ __pypackages__/
# VSCode
.vscode
+
+# Polyscope
+*.ini
diff --git a/README.md b/README.md
index 6132370..529d2d4 100644
--- a/README.md
+++ b/README.md
@@ -24,7 +24,7 @@ Effectively Detecting Loop Closures using Point Cloud Density Maps.
## Use MapClosures in your C++ project
-
+
1. Include the following snippet in your project's `CMakeLists.txt`:
```cmake
set(USE_SYSTEM_EIGEN3 ON CACHE BOOL "use system eigen3")
@@ -33,7 +33,7 @@ set(USE_SYSTEM_OPENCV ON CACHE BOOL "use system opencv")
include(FetchContent)
FetchContent_Declare(
- map_closures
+ map_closures
GIT_REPOSITORY https://github.com/PRBonn/MapClosures.git
GIT_TAG main
SOURCE_SUBDIR cpp
@@ -52,7 +52,7 @@ target_link_libraries(my_target PUBLIC map_closures)
#include
```
-## Install the Python API and CLI
+## Install the Python API and CLI
1. First, install the necessary system dependencies
```sh
sudo apt-get install --no-install-recommends -y build-essential cmake pybind11-dev libeigen3-dev libopencv-dev libtbb-dev
@@ -61,7 +61,7 @@ target_link_libraries(my_target PUBLIC map_closures)
```sh
pip install kiss-icp
```
-3. Then run:
+3. Then run:
```sh
make
```
diff --git a/python/map_closures/pipeline.py b/python/map_closures/pipeline.py
index 105e8d5..a145a34 100644
--- a/python/map_closures/pipeline.py
+++ b/python/map_closures/pipeline.py
@@ -76,6 +76,7 @@ def __init__(
self.closures = []
self.local_maps = []
+ self.density_maps = []
self.odom_poses = np.zeros((self._n_scans, 4, 4))
self.closure_overlap_threshold = 0.5
@@ -140,7 +141,10 @@ def _run_pipeline(self):
frame_to_map_pose = np.linalg.inv(current_map_pose) @ current_frame_pose
self.voxel_local_map.add_points(transform_points(frame_downsample, frame_to_map_pose))
self.visualizer.update_registration(
- source, keypoints, self.voxel_local_map, current_frame_pose, frame_to_map_pose
+ source,
+ self.voxel_local_map.point_cloud(),
+ current_frame_pose,
+ frame_to_map_pose,
)
if np.linalg.norm(frame_to_map_pose[:3, -1]) > self._map_range or (
@@ -160,6 +164,7 @@ def _run_pipeline(self):
np.copy(poses_in_local_map),
)
)
+ self.density_maps.append(self.map_closures.get_density_map_from_id(map_idx))
if closure.number_of_inliers > self.closure_config.inliers_threshold:
reference_local_map = self.local_maps[closure.source_id]
@@ -186,6 +191,8 @@ def _run_pipeline(self):
self.visualizer.update_closures(
reference_local_map.pointcloud,
query_local_map.pointcloud,
+ self.density_maps[closure.source_id],
+ self.density_maps[closure.target_id],
np.asarray(closure.pose),
[
reference_local_map.scan_indices[0],
@@ -209,7 +216,7 @@ def _run_pipeline(self):
scan_indices_in_local_map.append(scan_idx)
poses_in_local_map.append(current_frame_pose)
- self.visualizer.pause_vis()
+ # self.visualizer.pause_vis()
def _log_to_file(self):
np.savetxt(os.path.join(self._results_dir, "map_closures.txt"), np.asarray(self.closures))
diff --git a/python/map_closures/tools/visualizer.py b/python/map_closures/tools/visualizer.py
index 9821660..a4b54c8 100644
--- a/python/map_closures/tools/visualizer.py
+++ b/python/map_closures/tools/visualizer.py
@@ -20,49 +20,76 @@
# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
# SOFTWARE.
-import copy
+import importlib
import os
from abc import ABC
from dataclasses import dataclass, field
-from functools import partial
-from typing import Callable, List
import numpy as np
-import open3d as o3d
+from matplotlib import pyplot as plt
+
+# Button names
+START_BUTTON = "Play [SPACE]"
+PAUSE_BUTTON = "Hold [SPACE]"
+NEXT_FRAME_BUTTON = "Step Frame [N]"
+LOCAL_VIEW_BUTTON = "Local View [V]"
+GLOBAL_VIEW_BUTTON = "Global View [V]"
+CENTER_VIEWPOINT_BUTTON = "Center Viewpoint [C]"
+PREV_CLOSURE_BUTTON = "Prev Closure [P]"
+NEXT_CLOSURE_BUTTON = "Next Closure [N]"
+ALIGN_CLOSURE_BUTTON = "Align [A]"
+CLOSURES_VIEW_BUTTON = "Switch to MapClosures View [M]"
+REGISTRATION_VIEW_BUTTON = "Switch to Registration View [M]"
+OPEN_DENSITY_VIEW_BUTTON = "Open Density Maps Viewer [D]"
+QUIT_DENSITY_VIEW_BUTTON = "Quit Density Maps Viewer [D]"
+QUIT_BUTTON = "Quit [Q]"
+
+# Colors
+BACKGROUND_COLOR = [1.0, 1.0, 1.0]
+SOURCE_COLOR = [0.8470, 0.1058, 0.3764]
+TARGET_COLOR = [0.0, 0.3019, 0.2509]
+TRAJECTORY_COLOR = [0.1176, 0.5333, 0.8980]
+
+# Size constants
+SOURCE_PTS_SIZE = 0.06
+TARGET_PTS_SIZE = 0.08
+I = np.eye(4)
-CYAN = np.array([0.24, 0.898, 1.0])
-GREEN = np.array([0.0, 1.0, 0.0])
RED = np.array([0.5, 0.0, 0.0])
-BLACK = np.array([0.0, 0.0, 0.0])
BLUE = np.array([0.4, 0.5, 0.9])
-GRAY = np.array([0.4, 0.4, 0.4])
-WHITE = np.array([1.0, 1.0, 1.0])
-SPHERE_SIZE = 0.20
-
-
-@dataclass
-class RegistrationData:
- source: o3d.geometry.PointCloud = o3d.geometry.PointCloud()
- keypoints: o3d.geometry.PointCloud = o3d.geometry.PointCloud()
- target: o3d.geometry.PointCloud = o3d.geometry.PointCloud()
- frames: list = field(default_factory=list)
@dataclass
class LoopClosureData:
- loop_closures: o3d.geometry.LineSet = o3d.geometry.LineSet()
- closure_points: list = field(default_factory=list)
- closure_lines: list = field(default_factory=list)
-
- closures_exist: bool = False
-
- sources: list = field(default_factory=list)
- targets: list = field(default_factory=list)
+ num_closures: int = 0
+ current_closure_id: int = 0
closure_poses: list = field(default_factory=list)
+ closure_edges: list = field(default_factory=list)
+ source_local_maps: list = field(default_factory=list)
+ target_local_maps: list = field(default_factory=list)
+ source_density_maps: list = field(default_factory=list)
+ target_density_maps: list = field(default_factory=list)
- current_closure_id: int = 0
- current_source: o3d.geometry.PointCloud = o3d.geometry.PointCloud()
- current_target: o3d.geometry.PointCloud = o3d.geometry.PointCloud()
+
+@dataclass
+class VisualizerStateMachine:
+ block_execution: bool = True
+ play_mode: bool = False
+ view_frame: bool = True
+ view_local_map: bool = True
+ global_view: bool = False
+
+ view_closures: bool = False
+ view_closure_query: bool = True
+ view_closure_reference: bool = True
+ align_closures: bool = True
+ view_density_map: bool = False
+
+ background_color: list = field(default_factory=lambda: BACKGROUND_COLOR)
+ source_points_size: float = SOURCE_PTS_SIZE
+ map_points_size: float = TARGET_PTS_SIZE
+ query_points_size: float = SOURCE_PTS_SIZE
+ reference_points_size: float = TARGET_PTS_SIZE
class StubVisualizer(ABC):
@@ -80,323 +107,477 @@ def update_closures(self, *kwargs):
class Visualizer(StubVisualizer):
- # Public Interaface ----------------------------------------------------------------------------
def __init__(self):
- # Initialize GUI controls
- self.block_vis = True
- self.play_crun = False
- self.reset_bounding_box = True
-
- # Create registration data
- self.registration_data = RegistrationData()
+ # Initialize GUI
+ try:
+ 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 States
+ self._states = VisualizerStateMachine()
+ self._ref_density_viewer = None
+ self._query_density_viewer = None
+ self.fig = None
+
+ # Create data
+ self._trajectory = []
+ self._last_frame_pose = I
self.loop_closures_data = LoopClosureData()
+ self._last_frame_to_local_map_pose = I
- # Initialize visualizer
- self.registration_vis = o3d.visualization.VisualizerWithKeyCallback()
- self.closure_vis = o3d.visualization.VisualizerWithKeyCallback()
- self._registration_key_callbacks()
self._initialize_registration_visualizers()
- # Visualization options
- self.render_map = False
- self.render_source = True
- self.render_keypoints = False
- self.global_view = True
- self.render_trajectory = True
-
- # Cache the state of the visualizer
- self.state = (
- self.render_map,
- self.render_keypoints,
- self.render_source,
- )
-
- def pause_vis(self):
- self.play_crun = False
- while True:
- self.registration_vis.poll_events()
- self.registration_vis.update_renderer()
- self.closure_vis.poll_events()
- self.closure_vis.update_renderer()
- if self.play_crun:
- break
-
- def update_registration(self, source, keypoints, target_map, frame_pose, map_pose):
- target = target_map.point_cloud()
- self._update_registraion(source, keypoints, target, frame_pose, map_pose)
- while self.block_vis:
- self.registration_vis.poll_events()
- self.registration_vis.update_renderer()
- self.closure_vis.poll_events()
- self.closure_vis.update_renderer()
- if self.play_crun:
+ def update_registration(self, source, local_map, frame_pose, frame_to_local_map_pose):
+ self._update_registraion(source, local_map, frame_pose, frame_to_local_map_pose)
+ self._last_frame_pose = frame_pose
+ self._last_frame_to_local_map_pose = frame_to_local_map_pose
+ while self._states.block_execution:
+ self._ps.frame_tick()
+ if self._states.play_mode:
break
- self.block_vis = not self.block_vis
-
- def update_closures(self, source, target, closure_pose, closure_indices):
- self._update_closures(source, target, closure_pose, closure_indices)
- if self.loop_closures_data.closures_exist == False:
- self._initialize_closure_visualizers()
- self._loopclosure_key_callbacks()
- self.loop_closures_data.closures_exist = True
-
- # Render trajectory, only if it make sense (global view)
- if self.render_trajectory and self.global_view:
- self.registration_vis.update_geometry(self.loop_closures_data.loop_closures)
-
- # Private Interaface ---------------------------------------------------------------------------
- def _initialize_registration_visualizers(self):
- w_name = "Registration Visualizer"
- self.registration_vis.create_window(window_name=w_name, width=1920, height=1080)
- self.registration_vis.add_geometry(self.registration_data.source, reset_bounding_box=False)
- self._set_black_background(self.registration_vis)
- self.registration_vis.get_render_option().point_size = 1
- print(
- f"{w_name} initialized. Press:\n"
- "\t[SPACE] to pause/start\n"
- "\t [ESC] to exit\n"
- "\t [N] to step\n"
- "\t [F] to toggle on/off the input cloud to the pipeline\n"
- "\t [K] to toggle on/off the subsbampled frame\n"
- "\t [M] to toggle on/off the local map\n"
- "\t [V] to toggle ego/global viewpoint\n"
- "\t [T] to toggle the trajectory view(only available in global view)\n"
- "\t [C] to center the viewpoint\n"
- "\t [W] to toggle a white background\n"
- "\t [B] to toggle a black background\n"
+ self._states.block_execution = not self._states.block_execution
+
+ def update_closures(
+ self,
+ reference_local_map,
+ query_local_map,
+ reference_density_map,
+ query_density_map,
+ closure_pose,
+ closure_indices,
+ ):
+ self.loop_closures_data.num_closures += 1
+ self._update_closures(
+ reference_local_map,
+ query_local_map,
+ reference_density_map,
+ query_density_map,
+ closure_pose,
+ closure_indices,
)
- def _initialize_closure_visualizers(self):
- w_name = "Loop Closure Visualizer"
- self.closure_vis.create_window(window_name=w_name, width=1920, height=1080)
- self.closure_vis.add_geometry(
- self.loop_closures_data.current_source, reset_bounding_box=False
+ def _initialize_registration_visualizers(self):
+ self._ps.set_program_name("MapClosures 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)
+
+ # --- GUI Callbacks ------------------------------------------------
+ def _main_gui_callback(self):
+ if not self._states.view_closures:
+ self._start_pause_callback()
+ if not self._states.play_mode:
+ self._gui.SameLine()
+ self._next_frame_callback()
+ self._gui.Separator()
+ self._registration_source_callback()
+ self._registration_localmap_callback()
+ else:
+ self._previous_next_closure_callback()
+ self._gui.SameLine()
+ self._density_map_callback()
+ self._gui.Separator()
+ self._closure_query_map_callback()
+ self._closure_reference_map_callback()
+ self._gui.Separator()
+ self._closure_alignment_callback()
+ self._gui.SameLine()
+ self._closure_navigate_callback()
+ if self._states.view_density_map:
+ plt.gcf().canvas.draw()
+ plt.gcf().canvas.start_event_loop(1e-6)
+ self._gui.Separator()
+ self._background_color_callback()
+ self._gui.Separator()
+ self._global_view_callback()
+ self._gui.SameLine()
+ self._center_viewpoint_callback()
+ self._gui.SameLine()
+ self._quit_callback()
+ if self.loop_closures_data.num_closures:
+ self._closure_window_callback()
+
+ def _closure_window_callback(self):
+ window_pos = self._gui.GetWindowPos()
+ window_width = self._gui.GetWindowWidth()
+ window_height = self._gui.GetWindowHeight()
+ self._gui.Begin(f"No. of Closures: {self.loop_closures_data.num_closures}", open=True)
+ self._gui.SetWindowPos((window_pos[0], window_pos[1] + window_height + 20))
+ self._gui.SetWindowSize((window_width, 4 * self._gui.GetTextLineHeight()))
+ self._switch_view_callback()
+ self._gui.End()
+
+ def _start_pause_callback(self):
+ button_name = PAUSE_BUTTON if self._states.play_mode else START_BUTTON
+ if self._gui.Button(button_name) or self._gui.IsKeyPressed(self._gui.ImGuiKey_Space):
+ self._states.play_mode = not self._states.play_mode
+
+ def _next_frame_callback(self):
+ if self._gui.Button(NEXT_FRAME_BUTTON) or self._gui.IsKeyPressed(self._gui.ImGuiKey_N):
+ self._states.block_execution = not self._states.block_execution
+
+ def _registration_source_callback(self):
+ changed, self._states.source_points_size = self._gui.SliderFloat(
+ "##frame_size", self._states.source_points_size, v_min=0.01, v_max=0.6
)
- self.closure_vis.add_geometry(
- self.loop_closures_data.current_target, reset_bounding_box=False
+ if changed:
+ self._ps.get_point_cloud("current_frame").set_radius(
+ self._states.source_points_size, relative=False
+ )
+ self._gui.SameLine()
+ changed, self._states.view_frame = self._gui.Checkbox("Frame", self._states.view_frame)
+ if changed:
+ self._ps.get_point_cloud("current_frame").set_enabled(self._states.view_frame)
+
+ def _registration_localmap_callback(self):
+ changed, self._states.map_points_size = self._gui.SliderFloat(
+ "##map_size", self._states.map_points_size, v_min=0.01, v_max=0.6
)
- self.registration_vis.add_geometry(
- self.loop_closures_data.loop_closures, reset_bounding_box=False
+ if changed:
+ self._ps.get_point_cloud("local_map").set_radius(
+ self._states.map_points_size, relative=False
+ )
+ self._gui.SameLine()
+ changed, self._states.view_local_map = self._gui.Checkbox(
+ "Local Map", self._states.view_local_map
)
- self._set_black_background(self.closure_vis)
- self.closure_vis.get_render_option().point_size = 1
- print(
- f"{w_name} initialized. Press:\n"
- "\t[SPACE] to pause/start\n"
- "\t [P] to step to previous closure\n"
- "\t [N] to step to next closure\n"
- "\t [C] to center the viewpoint\n"
- "\t [W] to toggle a white background\n"
- "\t [B] to toggle a black background\n"
+ if changed:
+ self._ps.get_point_cloud("local_map").set_enabled(self._states.view_local_map)
+
+ def _update_registraion(self, source, local_map, frame_pose, frame_to_local_map_pose):
+ source_cloud = self._ps.register_point_cloud(
+ "current_frame",
+ source,
+ color=SOURCE_COLOR,
+ point_render_mode="quad",
)
-
- def _registration_key_callback(self, keys: List, callback: Callable):
- for key in keys:
- self.registration_vis.register_key_callback(ord(str(key)), partial(callback))
-
- def _loopclosure_key_callback(self, keys: List, callback: Callable):
- for key in keys:
- self.closure_vis.register_key_callback(ord(str(key)), partial(callback))
-
- def _registration_key_callbacks(self):
- self._registration_key_callback(["Ā", "Q", "\x1b"], self._quit)
- self._registration_key_callback([" "], self._start_stop)
- self._registration_key_callback(["N"], self._next_frame)
- self._registration_key_callback(["V"], self._toggle_view)
- self._registration_key_callback(["C"], self._center_viewpoint)
- self._registration_key_callback(["F"], self._toggle_source)
- self._registration_key_callback(["K"], self._toggle_keypoints)
- self._registration_key_callback(["M"], self._toggle_map)
- self._registration_key_callback(["T"], self._toggle_trajectory)
- self._registration_key_callback(["B"], self._set_black_background)
- self._registration_key_callback(["W"], self._set_white_background)
-
- def _loopclosure_key_callbacks(self):
- self._loopclosure_key_callback([" "], self._start_stop)
- self._loopclosure_key_callback(["P"], self._prev_closure)
- self._loopclosure_key_callback(["N"], self._next_closure)
- self._loopclosure_key_callback(["C"], self._center_viewpoint)
- self._loopclosure_key_callback(["B"], self._set_black_background)
- self._loopclosure_key_callback(["W"], self._set_white_background)
-
- def _set_black_background(self, vis):
- vis.get_render_option().background_color = [0.0, 0.0, 0.0]
-
- def _set_white_background(self, vis):
- vis.get_render_option().background_color = [1.0, 1.0, 1.0]
-
- def _quit(self, vis):
- print("Destroying Visualizer")
- vis.destroy_window()
- os._exit(0)
-
- def _next_frame(self, vis):
- self.block_vis = not self.block_vis
-
- def _prev_closure(self, vis):
- self.loop_closures_data.current_closure_id = (
- self.loop_closures_data.current_closure_id - 1
- ) % len(self.loop_closures_data.closure_poses)
- self._update_closure(self.loop_closures_data.current_closure_id)
-
- def _next_closure(self, vis):
- self.loop_closures_data.current_closure_id = (
- self.loop_closures_data.current_closure_id + 1
- ) % len(self.loop_closures_data.closure_poses)
- self._update_closure(self.loop_closures_data.current_closure_id)
-
- def _start_stop(self, vis):
- self.play_crun = not self.play_crun
-
- def _toggle_source(self, vis):
- if self.render_keypoints:
- self.render_keypoints = False
- self.render_source = True
- self.registration_vis.remove_geometry(
- self.registration_data.keypoints, reset_bounding_box=False
- )
+ source_cloud.set_radius(self._states.source_points_size, relative=False)
+ if self._states.global_view:
+ source_cloud.set_transform(frame_pose)
else:
- self.render_source = not self.render_source
-
- if self.render_source:
- self.registration_vis.add_geometry(
- self.registration_data.source, reset_bounding_box=False
- )
+ source_cloud.set_transform(I)
+ source_cloud.set_enabled(self._states.view_frame)
+
+ map_cloud = self._ps.register_point_cloud(
+ "local_map",
+ local_map,
+ color=TARGET_COLOR,
+ point_render_mode="quad",
+ )
+ map_cloud.set_radius(self._states.map_points_size, relative=False)
+ if self._states.global_view:
+ map_cloud.set_transform(frame_pose @ np.linalg.inv(frame_to_local_map_pose))
else:
- self.registration_vis.remove_geometry(
- self.registration_data.source, reset_bounding_box=False
- )
- return False
-
- def _toggle_keypoints(self, vis):
- if self.render_source:
- self.render_source = False
- self.render_keypoints = True
- self.registration_vis.remove_geometry(
- self.registration_data.source, reset_bounding_box=False
+ map_cloud.set_transform(np.linalg.inv(frame_to_local_map_pose))
+ map_cloud.set_enabled(self._states.view_local_map)
+
+ self._trajectory.append(frame_pose)
+ if self._states.global_view:
+ self._register_trajectory()
+
+ def _register_trajectory(self):
+ trajectory_cloud = self._ps.register_point_cloud(
+ "trajectory",
+ np.asarray(self._trajectory)[:, :3, -1],
+ color=BLUE,
+ )
+ trajectory_cloud.set_radius(0.3, relative=False)
+ if self.loop_closures_data.num_closures:
+ closure_lines = self._ps.register_curve_network(
+ "loop closures",
+ np.array(self._trajectory)[:, :3, -1],
+ np.array(self.loop_closures_data.closure_edges),
+ color=RED,
)
- else:
- self.render_keypoints = not self.render_keypoints
+ closure_lines.set_radius(0.1, relative=False)
- if self.render_keypoints:
- self.registration_vis.add_geometry(
- self.registration_data.keypoints, reset_bounding_box=False
- )
- else:
- self.registration_vis.remove_geometry(
- self.registration_data.keypoints, reset_bounding_box=False
+ def _unregister_trajectory(self):
+ self._ps.remove_point_cloud("trajectory")
+ if self.loop_closures_data.num_closures:
+ self._ps.remove_curve_network("loop closures")
+
+ def _switch_view_callback(self):
+ BUTTON_NAME = (
+ REGISTRATION_VIEW_BUTTON if self._states.view_closures else CLOSURES_VIEW_BUTTON
+ )
+ if self._gui.Button(BUTTON_NAME) or self._gui.IsKeyPressed(self._gui.ImGuiKey_M):
+ self._states.view_closures = not self._states.view_closures
+ if self._states.view_closures:
+ self._states.view_frame = False
+ self._states.view_local_map = False
+ self._states.view_closure_query = True
+ self._states.view_closure_reference = True
+ self._ps.get_point_cloud("current_frame").set_enabled(self._states.view_frame)
+ self._ps.get_point_cloud("local_map").set_enabled(self._states.view_local_map)
+ self._states.play_mode = False
+ self._render_closure()
+ if self._states.view_density_map:
+ plt.gcf().canvas.draw()
+ plt.gcf().canvas.start_event_loop(1e-6)
+ else:
+ self._states.view_frame = True
+ self._states.view_local_map = True
+ self._states.view_closure_query = False
+ self._states.view_closure_reference = False
+ self._render_closure()
+ self._ps.get_point_cloud("current_frame").set_enabled(self._states.view_frame)
+ self._ps.get_point_cloud("local_map").set_enabled(self._states.view_local_map)
+ self._ref_density_viewer = None
+ self._query_density_viewer = None
+ if self._states.view_density_map:
+ self._states.view_density_map = False
+ plt.close("all")
+
+ def _closure_query_map_callback(self):
+ changed, self._states.query_points_size = self._gui.SliderFloat(
+ "##query_size", self._states.query_points_size, v_min=0.01, v_max=0.6
+ )
+ if changed:
+ self._ps.get_point_cloud("query_map").set_radius(
+ self._states.query_points_size, relative=False
)
- return False
+ self._gui.SameLine()
+ changed, self._states.view_closure_query = self._gui.Checkbox(
+ "Query Map", self._states.view_closure_query
+ )
+ if changed:
+ self._ps.get_point_cloud("query_map").set_enabled(self._states.view_closure_query)
- def _toggle_map(self, vis):
- self.render_map = not self.render_map
- if self.render_map:
- self.registration_vis.add_geometry(
- self.registration_data.target, reset_bounding_box=False
+ def _closure_reference_map_callback(self):
+ changed, self._states.reference_points_size = self._gui.SliderFloat(
+ "##reference_size", self._states.reference_points_size, v_min=0.01, v_max=0.6
+ )
+ if changed:
+ self._ps.get_point_cloud("reference_map").set_radius(
+ self._states.reference_points_size, relative=False
)
- else:
- self.registration_vis.remove_geometry(
- self.registration_data.target, reset_bounding_box=False
+ self._gui.SameLine()
+ changed, self._states.view_closure_reference = self._gui.Checkbox(
+ "Reference Map", self._states.view_closure_reference
+ )
+ if changed:
+ self._ps.get_point_cloud("reference_map").set_enabled(
+ self._states.view_closure_reference
)
- return False
-
- def _toggle_view(self, vis):
- self.global_view = not self.global_view
- self._trajectory_handle()
-
- def _center_viewpoint(self, vis):
- vis.reset_view_point(True)
-
- def _toggle_trajectory(self, vis):
- if not self.global_view:
- return False
- self.render_trajectory = not self.render_trajectory
- self._trajectory_handle()
- return False
-
- def _trajectory_handle(self):
- if self.render_trajectory and self.global_view:
- for frame in self.registration_data.frames:
- self.registration_vis.add_geometry(frame, reset_bounding_box=False)
- if self.loop_closures_data.closures_exist:
- self.registration_vis.add_geometry(
- self.loop_closures_data.loop_closures, reset_bounding_box=False
+
+ def _closure_alignment_callback(self):
+ changed, self._states.align_closures = self._gui.Checkbox(
+ ALIGN_CLOSURE_BUTTON, self._states.align_closures
+ )
+ if changed:
+ self._render_closure()
+ if self._gui.IsKeyPressed(self._gui.ImGuiKey_A):
+ self._states.align_closures = not self._states.align_closures
+ self._render_closure()
+
+ def _closure_navigate_callback(self):
+ changed, self.loop_closures_data.current_closure_id = self._gui.SliderInt(
+ f"###Closure Index",
+ self.loop_closures_data.current_closure_id,
+ v_min=0,
+ v_max=self.loop_closures_data.num_closures - 1,
+ format="Closure Id: %d",
+ )
+ if changed:
+ self._render_closure()
+ if self._states.view_density_map:
+ self._ref_density_viewer.set_data(
+ self.loop_closures_data.source_density_maps[
+ self.loop_closures_data.current_closure_id
+ ]
)
- else:
- for frame in self.registration_data.frames:
- self.registration_vis.remove_geometry(frame, reset_bounding_box=False)
- if self.loop_closures_data.closures_exist:
- self.registration_vis.remove_geometry(
- self.loop_closures_data.loop_closures, reset_bounding_box=False
+ self._query_density_viewer.set_data(
+ self.loop_closures_data.target_density_maps[
+ self.loop_closures_data.current_closure_id
+ ]
)
-
- def _update_registraion(self, source, keypoints, target, pose, frame_to_map_pose):
- # Source hot frame
- if self.render_source:
- self.registration_data.source.points = o3d.utility.Vector3dVector(source)
- self.registration_data.source.paint_uniform_color(CYAN)
- if self.global_view:
- self.registration_data.source.transform(pose)
- self.registration_vis.update_geometry(self.registration_data.source)
-
- # Keypoints
- if self.render_keypoints:
- self.registration_data.keypoints.points = o3d.utility.Vector3dVector(keypoints)
- self.registration_data.keypoints.paint_uniform_color(CYAN)
- if self.global_view:
- self.registration_data.keypoints.transform(pose)
- self.registration_vis.update_geometry(self.registration_data.keypoints)
-
- # Target Map
- if self.render_map:
- target = copy.deepcopy(target)
- self.registration_data.target.points = o3d.utility.Vector3dVector(target)
- if self.global_view:
- self.registration_data.target.paint_uniform_color(GRAY)
- self.registration_data.target.transform(pose @ np.linalg.inv(frame_to_map_pose))
+ plt.gcf().canvas.draw()
+ plt.gcf().canvas.start_event_loop(1e-6)
+
+ def _density_map_callback(self):
+ BUTTON = (
+ OPEN_DENSITY_VIEW_BUTTON
+ if not self._states.view_density_map
+ else QUIT_DENSITY_VIEW_BUTTON
+ )
+ if self._gui.Button(BUTTON) or self._gui.IsKeyPressed(self._gui.ImGuiKey_D):
+ self._states.view_density_map = not self._states.view_density_map
+ if self._states.view_density_map:
+ plt.ion()
+ self.fig = plt.figure()
+ plt.show(block=False)
+ ax_ref = self.fig.add_subplot(1, 2, 1)
+ self._ref_density_viewer = ax_ref.imshow(
+ self.loop_closures_data.source_density_maps[
+ self.loop_closures_data.current_closure_id
+ ],
+ cmap="gray",
+ )
+ ax_ref.set_title("Reference Density Map")
+
+ ax_query = self.fig.add_subplot(1, 2, 2)
+ self._query_density_viewer = ax_query.imshow(
+ self.loop_closures_data.target_density_maps[
+ self.loop_closures_data.current_closure_id
+ ],
+ cmap="gray",
+ )
+ ax_query.set_title("Query Density Map")
+ plt.gcf().canvas.draw()
+ plt.gcf().canvas.start_event_loop(1e-6)
else:
- self.registration_data.target.transform(np.linalg.inv(frame_to_map_pose))
- self.registration_vis.update_geometry(self.registration_data.target)
-
- # Update always a list with all the trajectories
- new_frame = o3d.geometry.TriangleMesh.create_sphere(SPHERE_SIZE)
- new_frame.paint_uniform_color(BLUE)
- new_frame.compute_vertex_normals()
- new_frame.transform(pose)
- self.registration_data.frames.append(new_frame)
- self.loop_closures_data.closure_points.append(pose[:3, -1])
-
- # Render trajectory, only if it make sense (global view)
- if self.render_trajectory and self.global_view:
- self.registration_vis.add_geometry(
- self.registration_data.frames[-1], reset_bounding_box=False
- )
-
- if self.reset_bounding_box:
- self.registration_vis.reset_view_point(True)
- self.reset_bounding_box = False
-
- def _update_closures(self, source, target, closure_pose, closure_indices):
- self.loop_closures_data.sources.append(o3d.utility.Vector3dVector(source))
- self.loop_closures_data.targets.append(o3d.utility.Vector3dVector(target))
+ plt.close("all")
+
+ def _previous_next_closure_callback(self):
+ if self._gui.Button(PREV_CLOSURE_BUTTON) or self._gui.IsKeyPressed(self._gui.ImGuiKey_P):
+ self.loop_closures_data.current_closure_id = (
+ self.loop_closures_data.current_closure_id - 1
+ ) % self.loop_closures_data.num_closures
+ if self._states.view_density_map:
+ self._ref_density_viewer.set_data(
+ self.loop_closures_data.source_density_maps[
+ self.loop_closures_data.current_closure_id
+ ]
+ )
+ self._query_density_viewer.set_data(
+ self.loop_closures_data.target_density_maps[
+ self.loop_closures_data.current_closure_id
+ ]
+ )
+ plt.gcf().canvas.draw()
+ plt.gcf().canvas.start_event_loop(1e-6)
+
+ self._gui.SameLine()
+ if self._gui.Button(NEXT_CLOSURE_BUTTON) or self._gui.IsKeyPressed(self._gui.ImGuiKey_N):
+ self.loop_closures_data.current_closure_id = (
+ self.loop_closures_data.current_closure_id + 1
+ ) % self.loop_closures_data.num_closures
+ if self._states.view_density_map:
+ self._ref_density_viewer.set_data(
+ self.loop_closures_data.source_density_maps[
+ self.loop_closures_data.current_closure_id
+ ]
+ )
+ self._query_density_viewer.set_data(
+ self.loop_closures_data.target_density_maps[
+ self.loop_closures_data.current_closure_id
+ ]
+ )
+ plt.gcf().canvas.draw()
+ plt.gcf().canvas.start_event_loop(1e-6)
+ self._render_closure()
+
+ def _update_closures(
+ self,
+ reference_local_map,
+ query_local_map,
+ reference_density_map,
+ query_density_map,
+ closure_pose,
+ closure_indices,
+ ):
+ self.loop_closures_data.source_local_maps.append(reference_local_map)
+ self.loop_closures_data.target_local_maps.append(query_local_map)
+ self.loop_closures_data.source_density_maps.append(reference_density_map)
+ self.loop_closures_data.target_density_maps.append(query_density_map)
self.loop_closures_data.closure_poses.append(closure_pose)
- self._update_closure(len(self.loop_closures_data.closure_poses) - 1)
-
- self.loop_closures_data.closure_lines.append(closure_indices)
- self.loop_closures_data.loop_closures.points = o3d.utility.Vector3dVector(
- self.loop_closures_data.closure_points
+ self.loop_closures_data.closure_edges.append(closure_indices)
+ self.loop_closures_data.current_closure_id = self.loop_closures_data.num_closures - 1
+ if self._states.view_closures:
+ self._render_closure()
+
+ def _render_closure(self):
+ idx = self.loop_closures_data.current_closure_id
+ ref_map_pose = self._trajectory[self.loop_closures_data.closure_edges[idx][0]]
+ query_map_pose = self._trajectory[self.loop_closures_data.closure_edges[idx][1]]
+ query_map = self._ps.register_point_cloud(
+ "query_map",
+ self.loop_closures_data.target_local_maps[idx],
+ color=TARGET_COLOR,
+ point_render_mode="quad",
)
- self.loop_closures_data.loop_closures.lines = o3d.utility.Vector2iVector(
- self.loop_closures_data.closure_lines
+ query_map.set_radius(self._states.query_points_size, relative=False)
+ if self._states.global_view:
+ query_map.set_transform(query_map_pose)
+ else:
+ query_map.set_transform(I)
+ query_map.set_enabled(self._states.view_closure_query)
+
+ reference_map = self._ps.register_point_cloud(
+ "reference_map",
+ self.loop_closures_data.source_local_maps[idx],
+ color=SOURCE_COLOR,
+ point_render_mode="quad",
)
- self.loop_closures_data.loop_closures.paint_uniform_color(RED)
-
- def _update_closure(self, idx):
- self.loop_closures_data.current_source.points = self.loop_closures_data.sources[idx]
- self.loop_closures_data.current_source.transform(self.loop_closures_data.closure_poses[idx])
- self.loop_closures_data.current_source.paint_uniform_color(RED)
- self.loop_closures_data.current_target.points = self.loop_closures_data.targets[idx]
- self.loop_closures_data.current_target.paint_uniform_color(GREEN)
+ reference_map.set_radius(self._states.reference_points_size, relative=False)
+ if self._states.global_view:
+ if self._states.align_closures:
+ reference_map.set_transform(
+ query_map_pose @ self.loop_closures_data.closure_poses[idx]
+ )
+ else:
+ reference_map.set_transform(ref_map_pose)
+ else:
+ if self._states.align_closures:
+ reference_map.set_transform(self.loop_closures_data.closure_poses[idx])
+ else:
+ reference_map.set_transform(I)
+ reference_map.set_enabled(self._states.view_closure_reference)
- self.closure_vis.update_geometry(self.loop_closures_data.current_source)
- self.closure_vis.update_geometry(self.loop_closures_data.current_target)
+ def _background_color_callback(self):
+ changed, self._states.background_color = self._gui.ColorEdit3(
+ "Background Color",
+ self._states.background_color,
+ )
+ if changed:
+ self._ps.set_background_color(self._states.background_color)
+
+ def _global_view_callback(self):
+ button_name = LOCAL_VIEW_BUTTON if self._states.global_view else GLOBAL_VIEW_BUTTON
+ if self._gui.Button(button_name) or self._gui.IsKeyPressed(self._gui.ImGuiKey_V):
+ self._states.global_view = not self._states.global_view
+ if self._states.global_view:
+ self._ps.get_point_cloud("current_frame").set_transform(self._last_frame_pose)
+ self._ps.get_point_cloud("local_map").set_transform(
+ self._last_frame_pose @ np.linalg.inv(self._last_frame_to_local_map_pose)
+ )
+ self._register_trajectory()
+ else:
+ self._ps.get_point_cloud("current_frame").set_transform(I)
+ self._ps.get_point_cloud("local_map").set_transform(
+ np.linalg.inv(self._last_frame_to_local_map_pose)
+ )
+ self._unregister_trajectory()
+ if self._states.view_closures:
+ self._render_closure()
+ self._ps.reset_camera_to_home_view()
+
+ 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 _quit_callback(self):
+ posX = (
+ self._gui.GetCursorPosX()
+ + self._gui.GetColumnWidth()
+ - self._gui.CalcTextSize(QUIT_BUTTON)[0]
+ - self._gui.GetScrollX()
+ - self._gui.ImGuiStyleVar_ItemSpacing
+ )
+ self._gui.SetCursorPosX(posX)
+ 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)
diff --git a/python/pyproject.toml b/python/pyproject.toml
index 2908518..87f064b 100644
--- a/python/pyproject.toml
+++ b/python/pyproject.toml
@@ -32,7 +32,7 @@ all = [
"ouster-sdk",
"pyntcloud",
]
-visualizer = ["open3d>=0.13"]
+visualizer = ["polyscope", "matplotlib>3.9.2"]
[project.scripts]
map_closure_pipeline = "map_closures.tools.cmd:run"