Visualizer based on Polyscope (#29)

* Registration Visualizer works

* Closures also visualized now, more improvement needed

* This visualizer looks good, density maps added

* fix closure trace in visualizer

* Second imgui window for closures

* Screenshot raus

* cosmetic changes

* Final layout of gui, fix some issues in maytplotlib

---------

Co-authored-by: Luca Lobefaro <[email protected]>

.gitignore

@@ -124,3 +124,6 @@ __pypackages__/
 
 # VSCode
 .vscode
+
+# Polyscope
+*.ini

README.md

@@ -24,7 +24,7 @@ Effectively Detecting Loop Closures using Point Cloud Density Maps.
 <hr />
 
 ## 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 <map_closures/MapClosures.hpp>
 ```
 
-## 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
     ```

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