Add separate deskewing, taken from MapMOS

pyproject.toml

@@ -7,7 +7,7 @@ authors = [
     { name = "Benedikt Mersch", email = "[email protected]" },
 ]
 dependencies = [
-    "kiss-icp>=1.0.0",
+    "kiss-icp>=1.2.0",
     "diskcache>=5.3.0",
     "pytorch_lightning>=1.6.4",
 ]
@@ -28,8 +28,21 @@ mos4d_pipeline = "mos4d.cli:app"
 Homepage = "https://github.com/PRBonn/4DMOS"
 
 [build-system]
-requires = ["setuptools >= 61.0"]
-build-backend = "setuptools.build_meta"
+requires = [
+    "scikit_build_core","pybind11",
+]
+build-backend = "scikit_build_core.build"
+
+[tool.scikit-build]
+build-dir = "build"
+cmake.verbose = false
+cmake.minimum-version = "3.16"
+cmake.source-dir = "src/mos4d/pybind/"
+editable.mode = "redirect"
+editable.rebuild = true
+editable.verbose = true
+sdist.exclude = ["pybind/"]
+wheel.install-dir = "mos4d/pybind/"
 
 [tool.black]
 line-length = 100

scripts/precache.py

@@ -68,7 +68,7 @@ def precache(
     from mos4d.datasets.mos4d_dataset import collate_fn
 
     cfg = load_config(config)
-    sequences = list(sequence) if len(sequence) > 0 else cfg.training.train + cfg.training.val
+    sequences = list(sequence) if sequence != None else cfg.training.train + cfg.training.val
 
     data_iterable = DataLoader(
         Dataset(

src/mos4d/odometry.py

@@ -25,6 +25,7 @@
 from kiss_icp.kiss_icp import KissICP
 
 from mos4d.config import DataConfig, OdometryConfig
+from mos4d.pybind import mos4d_pybind
 
 
 def parse_config(config_data: DataConfig, config_odometry: OdometryConfig):
@@ -50,10 +51,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)
@@ -72,6 +70,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
 
@@ -81,14 +81,26 @@ 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 transform(self, points, pose):
         points_hom = np.hstack((points, np.ones((len(points), 1))))
         points = (pose @ points_hom.T).T[:, :3]
         return points
 
+    def deskew(self, points, timestamps, relative_motion):
+        return (
+            np.asarray(
+                mos4d_pybind._deskew(
+                    frame=mos4d_pybind._Vector3dVector(points),
+                    timestamps=timestamps,
+                    relative_motion=relative_motion,
+                )
+            )
+            if self.config.data.deskew
+            else points
+        )
+
     def current_pose(self):
         return self.last_pose
 

src/mos4d/pybind/3rdparty/eigen/LICENSE

@@ -0,0 +1,18 @@
+Eigen is primarily MPL2 licensed. See COPYING.MPL2 and these links:
+  http://www.mozilla.org/MPL/2.0/
+  http://www.mozilla.org/MPL/2.0/FAQ.html
+
+Some files contain third-party code under BSD or LGPL licenses, whence the other
+COPYING.* files here.
+
+All the LGPL code is either LGPL 2.1-only, or LGPL 2.1-or-later.
+For this reason, the COPYING.LGPL file contains the LGPL 2.1 text.
+
+If you want to guarantee that the Eigen code that you are #including is licensed
+under the MPL2 and possibly more permissive licenses (like BSD), #define this
+preprocessor symbol:
+  EIGEN_MPL2_ONLY
+For example, with most compilers, you could add this to your project CXXFLAGS:
+  -DEIGEN_MPL2_ONLY
+This will cause a compilation error to be generated if you #include any code that is
+LGPL licensed.

src/mos4d/pybind/3rdparty/eigen/eigen.cmake

@@ -0,0 +1,41 @@
+# 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.
+
+# TODO: Yet another manual release dne by nacho. This should be updated whenever the Eigen team
+# release a new version that is not 3.4. That version does not include this necessary changes:
+# - https://gitlab.com/libeigen/eigen/-/merge_requests/893/diffs
+
+set(EIGEN_BUILD_DOC OFF CACHE BOOL "Don't build Eigen docs")
+set(EIGEN_BUILD_TESTING OFF CACHE BOOL "Don't build Eigen tests")
+set(EIGEN_BUILD_PKGCONFIG OFF CACHE BOOL "Don't build Eigen pkg-config")
+set(EIGEN_BUILD_BLAS OFF CACHE BOOL "Don't build blas module")
+set(EIGEN_BUILD_LAPACK OFF CACHE BOOL "Don't build lapack module")
+
+include(FetchContent)
+FetchContent_Declare(eigen SYSTEM URL https://github.com/nachovizzo/eigen/archive/refs/tags/3.4.90.tar.gz)
+FetchContent_MakeAvailable(eigen)
+
+if(${CMAKE_VERSION} VERSION_LESS 3.25)
+  get_target_property(eigen_include_dirs eigen INTERFACE_INCLUDE_DIRECTORIES)
+  set_target_properties(eigen PROPERTIES INTERFACE_SYSTEM_INCLUDE_DIRECTORIES "${eigen_include_dirs}")
+endif()

src/mos4d/pybind/3rdparty/find_dependencies.cmake

@@ -0,0 +1,52 @@
+# 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.
+
+# Silence timestamp warning
+if(CMAKE_VERSION VERSION_GREATER 3.24)
+  cmake_policy(SET CMP0135 OLD)
+endif()
+
+if(USE_SYSTEM_EIGEN3)
+  find_package(Eigen3 QUIET NO_MODULE)
+endif()
+if(NOT USE_SYSTEM_EIGEN3 OR NOT TARGET Eigen3::Eigen)
+  set(USE_SYSTEM_EIGEN3 OFF)
+  include(${CMAKE_CURRENT_LIST_DIR}/eigen/eigen.cmake)
+endif()
+
+if(USE_SYSTEM_SOPHUS)
+  find_package(Sophus QUIET NO_MODULE)
+endif()
+if(NOT USE_SYSTEM_SOPHUS OR NOT TARGET Sophus::Sophus)
+  set(USE_SYSTEM_SOPHUS OFF)
+  include(${CMAKE_CURRENT_LIST_DIR}/sophus/sophus.cmake)
+endif()
+
+# tbb needs to be statically linked, so, also do it always :)
+if(USE_SYSTEM_TBB)
+  find_package(TBB QUIET NO_MODULE)
+endif()
+if(NOT USE_SYSTEM_TBB OR NOT TARGET TBB::tbb)
+  set(USE_SYSTEM_TBB OFF)
+  include(${CMAKE_CURRENT_LIST_DIR}/tbb/tbb.cmake)
+endif()

src/mos4d/pybind/3rdparty/sophus/LICENSE

@@ -0,0 +1,19 @@
+Copyright (c) 2008-2015 Jesse Beder.
+
+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 SOFTWAR

src/mos4d/pybind/3rdparty/sophus/sophus.cmake

@@ -0,0 +1,30 @@
+# MIT License
+#
+# # Copyright (c) 2023 Saurabh Gupta, Ignacio Vizzo, Cyrill Stachniss, University of Bonn
+#
+# 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(FetchContent)
+
+set(SOPHUS_USE_BASIC_LOGGING ON CACHE BOOL "Don't use fmt for Sophus libraru")
+set(BUILD_SOPHUS_TESTS OFF CACHE BOOL "Don't build Sophus tests")
+set(BUILD_SOPHUS_EXAMPLES OFF CACHE BOOL "Don't build Sophus Examples")
+
+# TODO: after https://github.com/strasdat/Sophus/pull/502 gets merged go back to mainstream
+FetchContent_Declare(sophus SYSTEM URL https://github.com/nachovizzo/Sophus/archive/refs/tags/1.22.11.tar.gz)
+FetchContent_MakeAvailable(sophus)