From b1b5e6f3e11bb77d4267950281f414a3d58532b4 Mon Sep 17 00:00:00 2001 From: Benedikt Mersch Date: Fri, 17 Jan 2025 11:20:47 +0100 Subject: [PATCH] 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 | 2 +- pyproject.toml | 2 +- src/mapmos/mapping.py | 2 +- src/mapmos/odometry.py | 30 ++++++++++----- src/mapmos/paper_pipeline.py | 1 + src/mapmos/pipeline.py | 1 + src/mapmos/pybind/CMakeLists.txt | 2 +- src/mapmos/pybind/Deskew.cpp | 60 +++++++++++++++++++++++++++++ src/mapmos/pybind/Deskew.hpp | 37 ++++++++++++++++++ src/mapmos/pybind/mapmos_pybind.cpp | 10 +++++ 10 files changed, 133 insertions(+), 14 deletions(-) create mode 100644 src/mapmos/pybind/Deskew.cpp create mode 100644 src/mapmos/pybind/Deskew.hpp diff --git a/.github/workflows/cpp.yml b/.github/workflows/cpp.yml index a3bbfe7..fb64c7c 100644 --- a/.github/workflows/cpp.yml +++ b/.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') }} diff --git a/pyproject.toml b/pyproject.toml index 883c95d..e75ac01 100644 --- a/pyproject.toml +++ b/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", ] diff --git a/src/mapmos/mapping.py b/src/mapmos/mapping.py index b92edaf..837577c 100644 --- a/src/mapmos/mapping.py +++ b/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)) diff --git a/src/mapmos/odometry.py b/src/mapmos/odometry.py index b9ecbea..1fb2cf4 100644 --- a/src/mapmos/odometry.py +++ b/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 + ) diff --git a/src/mapmos/paper_pipeline.py b/src/mapmos/paper_pipeline.py index 76841d2..cf31f82 100644 --- a/src/mapmos/paper_pipeline.py +++ b/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 diff --git a/src/mapmos/pipeline.py b/src/mapmos/pipeline.py index f33a300..e351a1a 100644 --- a/src/mapmos/pipeline.py +++ b/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 diff --git a/src/mapmos/pybind/CMakeLists.txt b/src/mapmos/pybind/CMakeLists.txt index 91a6020..ae7920e 100644 --- a/src/mapmos/pybind/CMakeLists.txt +++ b/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 .) diff --git a/src/mapmos/pybind/Deskew.cpp b/src/mapmos/pybind/Deskew.cpp new file mode 100644 index 0000000..3c96b2b --- /dev/null +++ b/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 +#include + +#include +#include +#include +#include +#include + +namespace mapmos { + +std::vector Deskew(const std::vector &frame, + const std::vector ×tamps, + const Sophus::SE3d &relative_motion) { + const std::vector &deskewed_frame = [&]() { + const auto &omega = relative_motion.log(); + const Sophus::SE3d &inverse_motion = relative_motion.inverse(); + std::vector deskewed_frame(frame.size()); + tbb::parallel_for( + // Index Range + tbb::blocked_range{0, deskewed_frame.size()}, + // Parallel Compute + [&](const tbb::blocked_range &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 diff --git a/src/mapmos/pybind/Deskew.hpp b/src/mapmos/pybind/Deskew.hpp new file mode 100644 index 0000000..c05bd71 --- /dev/null +++ b/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 +#include + +#include +#include +#include + +namespace mapmos { + +std::vector Deskew(const std::vector &frame, + const std::vector ×tamps, + const Sophus::SE3d &relative_motion); + +} // namespace mapmos diff --git a/src/mapmos/pybind/mapmos_pybind.cpp b/src/mapmos/pybind/mapmos_pybind.cpp index 59daf19..74e9634 100644 --- a/src/mapmos/pybind/mapmos_pybind.cpp +++ b/src/mapmos/pybind/mapmos_pybind.cpp @@ -30,6 +30,7 @@ #include #include +#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", py::py_array_to_vectors_double); + m.def( + "_deskew", + [](const std::vector &points, const std::vector ×tamps, + 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_ internal_map(m, "_VoxelHashMap", "Don't use this"); internal_map