Kissualizer Update: Inspection Mode and more Keybindings

.gitignore

@@ -6,6 +6,7 @@ _skbuild/
 .gitlab-ci-local/
 .polyscope.ini
 imgui.ini
+kisshot_*
 
 # Created by https://www.toptal.com/developers/gitignore/api/python,c++
 # Edit at https://www.toptal.com/developers/gitignore?templates=python,c++

cpp/kiss_icp/core/Registration.cpp

@@ -161,9 +161,9 @@ Sophus::SE3d Registration::AlignPointsToMap(const std::vector<Eigen::Vector3d> &
     Sophus::SE3d T_icp = Sophus::SE3d();
     for (int j = 0; j < max_num_iterations_; ++j) {
         // Equation (10)
-        const auto correspondences = DataAssociation(source, voxel_map, max_distance);
+        last_correspondences_ = DataAssociation(source, voxel_map, max_distance);
         // Equation (11)
-        const auto &[JTJ, JTr] = BuildLinearSystem(correspondences, kernel_scale);
+        const auto &[JTJ, JTr] = BuildLinearSystem(last_correspondences_, kernel_scale);
         const Eigen::Vector6d dx = JTJ.ldlt().solve(-JTr);
         const Sophus::SE3d estimation = Sophus::SE3d::exp(dx);
         // Equation (12)

cpp/kiss_icp/core/Registration.hpp

@@ -28,6 +28,8 @@
 
 #include "VoxelHashMap.hpp"
 
+using Correspondences = std::vector<std::pair<Eigen::Vector3d, Eigen::Vector3d>>;
+
 namespace kiss_icp {
 
 struct Registration {
@@ -42,5 +44,6 @@ struct Registration {
     int max_num_iterations_;
     double convergence_criterion_;
     int max_num_threads_;
+    Correspondences last_correspondences_;
 };
 }  // namespace kiss_icp

cpp/kiss_icp/core/VoxelHashMap.cpp

@@ -85,6 +85,14 @@ std::vector<Eigen::Vector3d> VoxelHashMap::Pointcloud() const {
     return points;
 }
 
+// Get the indices of the occupided voxels as points, mainly used for visualization
+std::vector<Voxel> VoxelHashMap::GetVoxels() const {
+    std::vector<Voxel> voxels(map_.size());
+    std::transform(map_.cbegin(), map_.cend(), voxels.begin(),
+                   [](const auto &map_element) { return map_element.first; });
+    return voxels;
+}
+
 void VoxelHashMap::Update(const std::vector<Eigen::Vector3d> &points,
                           const Eigen::Vector3d &origin) {
     AddPoints(points);

cpp/kiss_icp/core/VoxelHashMap.hpp

@@ -48,6 +48,7 @@ struct VoxelHashMap {
     void AddPoints(const std::vector<Eigen::Vector3d> &points);
     void RemovePointsFarFromLocation(const Eigen::Vector3d &origin);
     std::vector<Eigen::Vector3d> Pointcloud() const;
+    std::vector<Voxel> GetVoxels() const;
     std::tuple<Eigen::Vector3d, double> GetClosestNeighbor(const Eigen::Vector3d &query) const;
 
     double voxel_size_;

python/kiss_icp/mapping.py

@@ -66,3 +66,7 @@ def remove_far_away_points(self, origin):
     def point_cloud(self) -> np.ndarray:
         """Return the internal representaion as a np.array (pointcloud)."""
         return np.asarray(self._internal_map._point_cloud())
+
+    def get_voxels(self) -> np.ndarray:
+        """Return the occupied voxels, in indices (i,j,k) as a np.array."""
+        return np.asarray(self._internal_map._get_voxels())

python/kiss_icp/pipeline.py

@@ -80,7 +80,9 @@ def __init__(
         self._vis_infos = {
             "max_range": self.config.data.max_range,
             "min_range": self.config.data.min_range,
+            "voxel_size": self.config.mapping.voxel_size,
         }
+        self.visualizer.set_voxel_size(self.config.mapping.voxel_size)
         if hasattr(self._dataset, "use_global_visualizer"):
             self.visualizer._global_view = self._dataset.use_global_visualizer
 
@@ -109,8 +111,7 @@ def _run_pipeline(self):
             self.visualizer.update(
                 source,
                 keypoints,
-                self.odometry.local_map,
-                self.odometry.last_pose,
+                self.odometry,
                 self._vis_infos,
             )
 

python/kiss_icp/pybind/kiss_icp_pybind.cpp

@@ -72,7 +72,8 @@ PYBIND11_MODULE(kiss_icp_pybind, m) {
             "points"_a, "pose"_a)
         .def("_add_points", &VoxelHashMap::AddPoints, "points"_a)
         .def("_remove_far_away_points", &VoxelHashMap::RemovePointsFarFromLocation, "origin"_a)
-        .def("_point_cloud", &VoxelHashMap::Pointcloud);
+        .def("_point_cloud", &VoxelHashMap::Pointcloud)
+        .def("_get_voxels", &VoxelHashMap::GetVoxels);
 
     // Point Cloud registration
     py::class_<Registration> internal_registration(m, "_Registration", "Don't use this");
@@ -91,7 +92,8 @@ PYBIND11_MODULE(kiss_icp_pybind, m) {
                     .matrix();
             },
             "points"_a, "voxel_map"_a, "initial_guess"_a, "max_correspondance_distance"_a,
-            "kernel"_a);
+            "kernel"_a)
+        .def("_get_correspondences", [](Registration &self) { return self.last_correspondences_; });
 
     // AdaptiveThreshold bindings
     py::class_<AdaptiveThreshold> adaptive_threshold(m, "_AdaptiveThreshold", "Don't use this");

python/kiss_icp/registration.py

@@ -63,3 +63,6 @@ def align_points_to_map(
             max_correspondance_distance=max_correspondance_distance,
             kernel=kernel,
         )
+
+    def get_correspondences(self) -> np.ndarray:
+        return self._registration._get_correspondences()