Add separate de-skewing (#32)

* Add separate deskewing

* Fix deskewing

* Add copy and update dependency

* Move into odometry

* Use cache@v4

* Test this

* Cosmetics

* Remove again

* Clean up

* Move back here

* Just use the default

.github/workflows/cpp.yml

@@ -42,7 +42,7 @@ jobs:
     steps:
       - uses: actions/checkout@v3
       - name: Cache dependencies
-        uses: actions/cache@v2
+        uses: actions/cache@v4
         with:
           path: ~/.apt/cache
           key: ${{ runner.os }}-apt-${{ hashFiles('**/ubuntu_dependencies.yml') }}

pyproject.toml

@@ -8,7 +8,7 @@ authors = [
 ]
 license_files = "LICENSE"
 dependencies = [
-    "kiss-icp<=1.1.0",
+    "kiss-icp>=1.2.0",
     "diskcache>=5.3.0",
     "pytorch_lightning>=1.6.4",
 ]

src/mapmos/mapping.py

@@ -59,7 +59,7 @@ def remove_voxels_far_from_location(self, location: np.ndarray):
         self._internal_map._remove_far_away_points(location)
 
     def update_belief(self, points: np.ndarray, logits: np.ndarray):
-        self._internal_map._update_belief(mapmos_pybind._Vector3dVector(points), logits)
+        self._internal_map._update_belief(mapmos_pybind._Vector3dVector(points), logits.ravel())
 
     def get_belief(self, points: np.ndarray):
         belief = self._internal_map._get_belief(mapmos_pybind._Vector3dVector(points))

src/mapmos/odometry.py

@@ -20,14 +20,13 @@
 # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 # SOFTWARE.
 
-from typing import Type
-
 import numpy as np
 from kiss_icp.config import KISSConfig
 from kiss_icp.kiss_icp import KissICP, get_registration
 
 from mapmos.config import DataConfig, OdometryConfig
 from mapmos.mapping import VoxelHashMap
+from mapmos.pybind import mapmos_pybind
 from mapmos.registration import get_registration
 
 
@@ -61,10 +60,7 @@ def __init__(
 
     def register_points(self, points, timestamps, scan_index):
         # Apply motion compensation
-        points = self.compensator.deskew_scan(points, timestamps, self.last_delta)
-
-        # Preprocess the input cloud
-        points_prep = self.preprocess(points)
+        points_prep = self.preprocessor.preprocess(points, timestamps, self.last_delta)
 
         # Voxelize
         source, points_downsample = self.voxelize(points_prep)
@@ -83,6 +79,8 @@ def register_points(self, points, timestamps, scan_index):
             kernel=sigma / 3,
         )
 
+        point_deskewed = self.deskew(points, timestamps, self.last_delta)
+
         # Compute the difference between the prediction and the actual estimate
         model_deviation = np.linalg.inv(initial_guess) @ new_pose
 
@@ -92,17 +90,29 @@ def register_points(self, points, timestamps, scan_index):
         self.last_delta = np.linalg.inv(self.last_pose) @ new_pose
         self.last_pose = new_pose
 
-        points_reg = self.transform(points, self.last_pose)
-        return np.asarray(points_reg)
+        return self.transform(point_deskewed, self.last_pose)
 
     def get_map_points(self):
         map_points, map_timestamps = self.local_map.point_cloud_with_timestamps()
         return map_points.reshape(-1, 3), map_timestamps.reshape(-1, 1)
 
+    def current_location(self):
+        return self.last_pose[:3, 3]
+
     def transform(self, points, pose):
         points_hom = np.hstack((points, np.ones((len(points), 1))))
         points = (pose @ points_hom.T).T[:, :3]
         return points
 
-    def current_location(self):
-        return self.last_pose[:3, 3]
+    def deskew(self, points, timestamps, relative_motion):
+        return (
+            np.asarray(
+                mapmos_pybind._deskew(
+                    frame=mapmos_pybind._Vector3dVector(points),
+                    timestamps=timestamps,
+                    relative_motion=relative_motion,
+                )
+            )
+            if self.config.data.deskew
+            else points
+        )

src/mapmos/paper_pipeline.py

@@ -72,6 +72,7 @@ def _run_pipeline(self):
         for scan_index in pbar:
             local_scan, timestamps, gt_labels = self._next(scan_index)
             map_points, map_indices = self.odometry.get_map_points()
+
             scan_points = self.odometry.register_points(local_scan, timestamps, scan_index)
             self.poses[scan_index - self._first] = self.odometry.last_pose
 

src/mapmos/pipeline.py

@@ -126,6 +126,7 @@ def _run_pipeline(self):
         for scan_index in pbar:
             local_scan, timestamps, gt_labels = self._next(scan_index)
             map_points, map_indices = self.odometry.get_map_points()
+
             scan_points = self.odometry.register_points(local_scan, timestamps, scan_index)
             self.poses[scan_index - self._first] = self.odometry.last_pose
 

src/mapmos/pybind/CMakeLists.txt

@@ -41,7 +41,7 @@ find_package(Python COMPONENTS Interpreter Development.Module REQUIRED)
 find_package(pybind11 CONFIG REQUIRED)
 
 # Python bindings
-pybind11_add_module(mapmos_pybind MODULE mapmos_pybind.cpp VoxelHashMap.cpp Registration.cpp)
+pybind11_add_module(mapmos_pybind MODULE mapmos_pybind.cpp VoxelHashMap.cpp Registration.cpp Deskew.cpp)
 target_compile_features(mapmos_pybind PRIVATE cxx_std_17)
 target_link_libraries(mapmos_pybind PUBLIC Eigen3::Eigen tsl::robin_map TBB::tbb Sophus::Sophus)
 install(TARGETS mapmos_pybind LIBRARY DESTINATION .)

src/mapmos/pybind/Deskew.cpp

@@ -0,0 +1,60 @@
+// MIT License
+//
+// Copyright (c) 2022 Ignacio Vizzo, Tiziano Guadagnino, Benedikt Mersch, Cyrill
+// Stachniss.
+//
+// Permission is hereby granted, free of charge, to any person obtaining a copy
+// of this software and associated documentation files (the "Software"), to deal
+// in the Software without restriction, including without limitation the rights
+// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+// copies of the Software, and to permit persons to whom the Software is
+// furnished to do so, subject to the following conditions:
+//
+// The above copyright notice and this permission notice shall be included in all
+// copies or substantial portions of the Software.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+// 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.
+#include "Deskew.hpp"
+
+#include <tbb/blocked_range.h>
+#include <tbb/parallel_for.h>
+
+#include <Eigen/Core>
+#include <Eigen/Geometry>
+#include <algorithm>
+#include <functional>
+#include <vector>
+
+namespace mapmos {
+
+std::vector<Eigen::Vector3d> Deskew(const std::vector<Eigen::Vector3d> &frame,
+                                    const std::vector<double> &timestamps,
+                                    const Sophus::SE3d &relative_motion) {
+    const std::vector<Eigen::Vector3d> &deskewed_frame = [&]() {
+        const auto &omega = relative_motion.log();
+        const Sophus::SE3d &inverse_motion = relative_motion.inverse();
+        std::vector<Eigen::Vector3d> deskewed_frame(frame.size());
+        tbb::parallel_for(
+            // Index Range
+            tbb::blocked_range<size_t>{0, deskewed_frame.size()},
+            // Parallel Compute
+            [&](const tbb::blocked_range<size_t> &r) {
+                for (size_t idx = r.begin(); idx < r.end(); ++idx) {
+                    const auto &point = frame.at(idx);
+                    const auto &stamp = timestamps.at(idx);
+                    const auto pose = inverse_motion * Sophus::SE3d::exp(stamp * omega);
+                    deskewed_frame.at(idx) = pose * point;
+                };
+            });
+        return deskewed_frame;
+    }();
+    return deskewed_frame;
+}
+
+}  // namespace mapmos

src/mapmos/pybind/Deskew.hpp

@@ -0,0 +1,37 @@
+// MIT License
+//
+// Copyright (c) 2022 Ignacio Vizzo, Tiziano Guadagnino, Benedikt Mersch, Cyrill
+// Stachniss.
+//
+// Permission is hereby granted, free of charge, to any person obtaining a copy
+// of this software and associated documentation files (the "Software"), to deal
+// in the Software without restriction, including without limitation the rights
+// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+// copies of the Software, and to permit persons to whom the Software is
+// furnished to do so, subject to the following conditions:
+//
+// The above copyright notice and this permission notice shall be included in all
+// copies or substantial portions of the Software.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+// 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.
+#pragma once
+#include <tbb/global_control.h>
+#include <tbb/task_arena.h>
+
+#include <Eigen/Core>
+#include <sophus/se3.hpp>
+#include <vector>
+
+namespace mapmos {
+
+std::vector<Eigen::Vector3d> Deskew(const std::vector<Eigen::Vector3d> &frame,
+                                    const std::vector<double> &timestamps,
+                                    const Sophus::SE3d &relative_motion);
+
+}  // namespace mapmos

src/mapmos/pybind/mapmos_pybind.cpp

@@ -30,6 +30,7 @@
 #include <memory>
 #include <vector>
 
+#include "Deskew.hpp"
 #include "Registration.hpp"
 #include "VoxelHashMap.hpp"
 #include "stl_vector_eigen.h"
@@ -45,6 +46,15 @@ PYBIND11_MODULE(mapmos_pybind, m) {
         m, "_Vector3dVector", "std::vector<Eigen::Vector3d>",
         py::py_array_to_vectors_double<Eigen::Vector3d>);
 
+    m.def(
+        "_deskew",
+        [](const std::vector<Eigen::Vector3d> &points, const std::vector<double> &timestamps,
+           const Eigen::Matrix4d &relative_motion) {
+            Sophus::SE3d motion(relative_motion);
+            return Deskew(points, timestamps, motion);
+        },
+        "frame"_a, "timestamps"_a, "relative_motion"_a);
+
     // Map representation
     py::class_<VoxelHashMap> internal_map(m, "_VoxelHashMap", "Don't use this");
     internal_map