Merge pull request #52 from PRBonn/gupta/deskewing

Deskew Scans in the Global Mapping Run

Author: saurabh1002

kiss_slam/kiss_slam_pybind/kiss_slam_pybind.cpp

@@ -74,7 +74,7 @@ PYBIND11_MODULE(kiss_slam_pybind, m) {
         .def("_per_voxel_point_and_normal", &VoxelMap::PerVoxelPointAndNormal);
 
     py::class_<OccupancyMapper> grid_mapper(m, "_OccupancyMapper", "Don't use this");
-    grid_mapper.def(py::init<float, float>(), "resolution"_a, "max_range"_a)
+    grid_mapper.def(py::init<float>(), "resolution"_a)
         .def("_integrate_frame", &OccupancyMapper::IntegrateFrame, "pointcloud"_a, "pose"_a)
         .def("_get_active_voxels", &OccupancyMapper::GetOccupancyInformation)
         .def("_get_occupied_voxels", &OccupancyMapper::GetOccupiedVoxels, "probability_threshold"_a)

kiss_slam/kiss_slam_pybind/occupancy_mapper/occupancy_mapper.cpp

@@ -41,21 +41,18 @@ inline float LogOddsOccupied(const float probability) {
 }  // namespace
 
 namespace occupancy_mapper {
-OccupancyMapper::OccupancyMapper(const float resolution, const float max_range)
-    : max_range_(max_range), map_(resolution), accessor_(map_.createAccessor()) {}
+OccupancyMapper::OccupancyMapper(const float resolution)
+    : map_(resolution), accessor_(map_.createAccessor()) {}
 
 void OccupancyMapper::IntegrateFrame(const Vector3fVector &pointcloud,
                                      const Eigen::Matrix4f &pose) {
     const Eigen::Matrix3f &R = pose.block<3, 3>(0, 0);
     const Eigen::Vector3f &t = pose.block<3, 1>(0, 3);
     const auto start_coord = map_.posToCoord(t);
     std::for_each(pointcloud.cbegin(), pointcloud.cend(), [&](const Eigen::Vector3f &point) {
-        const auto point_range = point.norm();
-        if (point_range < max_range_) {
-            const Eigen::Vector3f point_tf = R * point + t;
-            const auto end_coord = map_.posToCoord(point_tf);
-            Bresenham3DLine(start_coord, end_coord);
-        }
+        const Eigen::Vector3f point_tf = R * point + t;
+        const auto end_coord = map_.posToCoord(point_tf);
+        Bresenham3DLine(start_coord, end_coord);
     });
 }
 

kiss_slam/kiss_slam_pybind/occupancy_mapper/occupancy_mapper.hpp

@@ -35,7 +35,7 @@ using Vector3iVector = std::vector<Eigen::Vector3i>;
 namespace occupancy_mapper {
 class OccupancyMapper {
 public:
-    OccupancyMapper(const float resolution, const float max_range);
+    OccupancyMapper(const float resolution);
     ~OccupancyMapper() = default;
 
     void IntegrateFrame(const Vector3fVector &pointcloud, const Eigen::Matrix4f &pose);
@@ -50,7 +50,6 @@ class OccupancyMapper {
     void Bresenham3DLine(const Bonxai::CoordT &start_coord, const Bonxai::CoordT &end_coord);
     void UpdateVoxelOccupancy(const Bonxai::CoordT &coord, const float value);
 
-    float max_range_ = 0.0f;
     Bonxai::VoxelGrid<float> map_;
     using AccessorType = typename Bonxai::VoxelGrid<float>::Accessor;
     AccessorType accessor_;

kiss_slam/occupancy_mapper.py

@@ -38,9 +38,7 @@ def __init__(
         config: OccupancyMapperConfig,
     ):
         self.config = config
-        self.occupancy_mapping_pipeline = kiss_slam_pybind._OccupancyMapper(
-            self.config.resolution, self.config.max_range
-        )
+        self.occupancy_mapping_pipeline = kiss_slam_pybind._OccupancyMapper(self.config.resolution)
 
     def integrate_frame(self, frame: np.ndarray, pose: np.ndarray):
         frame_downsampled = voxel_down_sample(frame, self.config.resolution).astype(np.float32)

kiss_slam/pipeline.py

@@ -27,7 +27,6 @@
 
 import numpy as np
 from kiss_icp.pipeline import OdometryPipeline
-from map_closures import map_closures
 from tqdm import tqdm, trange
 
 from kiss_slam.config import load_config
@@ -83,18 +82,26 @@ def _run_pipeline(self):
 
     def _global_mapping(self):
         if self.refuse_scans:
+            from kiss_icp.preprocess import get_preprocessor
+
             if hasattr(self._dataset, "reset"):
                 self._dataset.reset()
-            ref_ground_alignment = map_closures.align_map_to_local_ground(
-                self.kiss_slam.local_map_graph[0].pcd.point.positions.cpu().numpy(),
-                self.slam_config.odometry.mapping.voxel_size,
+            ref_ground_alignment = self.kiss_slam.closer.detector.get_ground_alignment_from_id(0)
+            deskewing_deltas = np.vstack(
+                (
+                    np.eye(4)[None],
+                    np.eye(4)[None],
+                    np.linalg.inv(self.poses[:-2]) @ self.poses[1:-1],
+                )
             )
+            preprocessor = get_preprocessor(self.config)
             occupancy_grid_mapper = OccupancyGridMapper(self.slam_config.occupancy_mapper)
             print("KissSLAM| Computing Occupancy Grid")
             for idx in trange(self._first, self._last, unit=" frames", dynamic_ncols=True):
-                scan, _ = self._next(idx)
+                scan, timestamps = self._next(idx)
+                deskewed_scan = preprocessor.preprocess(scan, timestamps, deskewing_deltas[idx])
                 occupancy_grid_mapper.integrate_frame(
-                    scan, ref_ground_alignment @ self.poses[idx - self._first]
+                    deskewed_scan, ref_ground_alignment @ self.poses[idx - self._first]
                 )
             occupancy_grid_mapper.compute_3d_occupancy_information()
             occupancy_grid_mapper.compute_2d_occupancy_information()