New visualizer (#21)

* Take new visualizer

* Also fix here

* Add back

.gitignore

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

src/mapmos/mapping.py

@@ -50,6 +50,10 @@ def point_cloud_with_timestamps(self):
         map_points, map_timestamps = self._internal_map._point_cloud_with_timestamps()
         return np.asarray(map_points), np.asarray(map_timestamps)
 
+    def voxels_with_belief(self):
+        voxels, belief = self._internal_map._voxels_with_belief()
+        return np.asarray(voxels), np.asarray(belief)
+
     def remove_voxels_far_from_location(self, location: np.ndarray):
         self._internal_map._remove_far_away_points(location)
 

src/mapmos/paper_pipeline.py

@@ -158,19 +158,14 @@ def _run_pipeline(self):
                 torch.tensor(gt_labels, dtype=torch.int32),
             )
 
-            belief_labels_scan = belief_labels_with_map
-            if self.visualize:
-                belief_map = self.belief.get_belief(map_points)
-                belief_labels_map = self.model.to_label(belief_map)
-                self.visualizer.update(
-                    scan_points,
-                    map_points,
-                    pred_labels_scan,
-                    pred_labels_map,
-                    belief_labels_scan,
-                    belief_labels_map,
-                    self.odometry.last_pose,
-                )
+            self.visualizer.update(
+                scan_points,
+                map_points,
+                pred_labels_scan,
+                pred_labels_map,
+                self.belief,
+                self.odometry.last_pose,
+            )
 
             # Probabilistic volumetric fusion with scan and moving map predictions and delay
             self.buffer.append([scan_index, scan_points, gt_labels])

src/mapmos/pipeline.py

@@ -98,7 +98,7 @@ def __init__(
         # Visualizer
         self.visualize = visualize
         self.visualizer = MapMOSVisualizer() if visualize else StubVisualizer()
-
+        self.visualizer.set_voxel_size(self.config.mos.voxel_size_belief)
         self.ply_writer = PlyWriter() if save_ply else StubWriter()
         self.kitti_writer = KITTIWriter() if save_kitti else StubWriter()
 
@@ -174,22 +174,17 @@ def _run_pipeline(self):
 
             start_time = time.perf_counter_ns()
             self.belief.update_belief(points_stacked, logits_stacked)
-            belief_scan = self.belief.get_belief(scan_points)
+            self.belief.get_belief(scan_points)
             self.times_belief.append(time.perf_counter_ns() - start_time)
-            belief_labels_scan = self.model.to_label(belief_scan)
-
-            if self.visualize:
-                belief_map = self.belief.get_belief(map_points)
-                belief_labels_map = self.model.to_label(belief_map)
-                self.visualizer.update(
-                    scan_points,
-                    map_points,
-                    pred_labels_scan,
-                    pred_labels_map,
-                    belief_labels_scan,
-                    belief_labels_map,
-                    self.odometry.last_pose,
-                )
+
+            self.visualizer.update(
+                scan_points,
+                map_points,
+                pred_labels_scan,
+                pred_labels_map,
+                self.belief,
+                self.odometry.last_pose,
+            )
 
             # Evaluate and save with delay
             self.buffer.append([scan_index, scan_points, gt_labels])

src/mapmos/pybind/VoxelHashMap.cpp

@@ -77,6 +77,17 @@ std::tuple<std::vector<Eigen::Vector3d>, std::vector<int>> VoxelHashMap::Pointcl
     return std::make_tuple(points, timestamps);
 }
 
+std::tuple<std::vector<VoxelHashMap::Voxel>, std::vector<double>> VoxelHashMap::VoxelsWithBelief()
+    const {
+    std::vector<Voxel> voxels;
+    std::vector<double> belief;
+    for (auto map_element : map_) {
+        voxels.push_back(map_element.first);
+        belief.push_back(map_element.second.belief.value_);
+    }
+    return make_tuple(voxels, belief);
+}
+
 void VoxelHashMap::Update(const std::vector<Eigen::Vector3d> &points,
                           const Eigen::Vector3d &origin,
                           const int timestamp) {

src/mapmos/pybind/VoxelHashMap.hpp

@@ -97,6 +97,7 @@ struct VoxelHashMap {
     void RemovePointsFarFromLocation(const Eigen::Vector3d &origin);
     std::vector<Eigen::Vector3d> Pointcloud() const;
     std::tuple<std::vector<Eigen::Vector3d>, std::vector<int>> PointcloudWithTimestamps() const;
+    std::tuple<std::vector<Voxel>, std::vector<double>> VoxelsWithBelief() const;
     std::vector<Eigen::Vector3d> GetPoints(const std::vector<Voxel> &query_voxels) const;
 
     double voxel_size_;

src/mapmos/pybind/mapmos_pybind.cpp

@@ -68,6 +68,7 @@ PYBIND11_MODULE(mapmos_pybind, m) {
         .def("_remove_far_away_points", &VoxelHashMap::RemovePointsFarFromLocation, "origin"_a)
         .def("_point_cloud", &VoxelHashMap::Pointcloud)
         .def("_point_cloud_with_timestamps", &VoxelHashMap::PointcloudWithTimestamps)
+        .def("_voxels_with_belief", &VoxelHashMap::VoxelsWithBelief)
         .def("_update_belief", &VoxelHashMap::UpdateBelief, "points"_a, "updates"_a)
         .def("_get_belief", &VoxelHashMap::GetBelief, "points"_a);