diff --git a/pyproject.toml b/pyproject.toml index 08417d9..de80c09 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -7,7 +7,7 @@ authors = [ { name = "Benedikt Mersch", email = "benedikt.mersch@gmail.com" }, ] 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 diff --git a/scripts/precache.py b/scripts/precache.py index c6f6293..2a307ab 100755 --- a/scripts/precache.py +++ b/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( diff --git a/src/mos4d/__init__.py b/src/mos4d/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/src/mos4d/odometry.py b/src/mos4d/odometry.py index 2757e85..85fe4bc 100644 --- a/src/mos4d/odometry.py +++ b/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 diff --git a/src/mos4d/pybind/3rdparty/eigen/LICENSE b/src/mos4d/pybind/3rdparty/eigen/LICENSE new file mode 100644 index 0000000..de5b632 --- /dev/null +++ b/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. diff --git a/src/mos4d/pybind/3rdparty/eigen/eigen.cmake b/src/mos4d/pybind/3rdparty/eigen/eigen.cmake new file mode 100644 index 0000000..315b400 --- /dev/null +++ b/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() diff --git a/src/mos4d/pybind/3rdparty/find_dependencies.cmake b/src/mos4d/pybind/3rdparty/find_dependencies.cmake new file mode 100644 index 0000000..1f58629 --- /dev/null +++ b/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() diff --git a/src/mos4d/pybind/3rdparty/sophus/LICENSE b/src/mos4d/pybind/3rdparty/sophus/LICENSE new file mode 100644 index 0000000..9f9cc4d --- /dev/null +++ b/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 diff --git a/src/mos4d/pybind/3rdparty/sophus/sophus.cmake b/src/mos4d/pybind/3rdparty/sophus/sophus.cmake new file mode 100644 index 0000000..c552401 --- /dev/null +++ b/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) diff --git a/src/mos4d/pybind/3rdparty/tbb/LICENSE b/src/mos4d/pybind/3rdparty/tbb/LICENSE new file mode 100644 index 0000000..261eeb9 --- /dev/null +++ b/src/mos4d/pybind/3rdparty/tbb/LICENSE @@ -0,0 +1,201 @@ + Apache License + Version 2.0, January 2004 + http://www.apache.org/licenses/ + + TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION + + 1. Definitions. + + "License" shall mean the terms and conditions for use, reproduction, + and distribution as defined by Sections 1 through 9 of this document. + + "Licensor" shall mean the copyright owner or entity authorized by + the copyright owner that is granting the License. + + "Legal Entity" shall mean the union of the acting entity and all + other entities that control, are controlled by, or are under common + control with that entity. For the purposes of this definition, + "control" means (i) the power, direct or indirect, to cause the + direction or management of such entity, whether by contract or + otherwise, or (ii) ownership of fifty percent (50%) or more of the + outstanding shares, or (iii) beneficial ownership of such entity. + + "You" (or "Your") shall mean an individual or Legal Entity + exercising permissions granted by this License. + + "Source" form shall mean the preferred form for making modifications, + including but not limited to software source code, documentation + source, and configuration files. + + "Object" form shall mean any form resulting from mechanical + transformation or translation of a Source form, including but + not limited to compiled object code, generated documentation, + and conversions to other media types. + + "Work" shall mean the work of authorship, whether in Source or + Object form, made available under the License, as indicated by a + copyright notice that is included in or attached to the work + (an example is provided in the Appendix below). + + "Derivative Works" shall mean any work, whether in Source or Object + form, that is based on (or derived from) the Work and for which the + editorial revisions, annotations, elaborations, or other modifications + represent, as a whole, an original work of authorship. For the purposes + of this License, Derivative Works shall not include works that remain + separable from, or merely link (or bind by name) to the interfaces of, + the Work and Derivative Works thereof. + + "Contribution" shall mean any work of authorship, including + the original version of the Work and any modifications or additions + to that Work or Derivative Works thereof, that is intentionally + submitted to Licensor for inclusion in the Work by the copyright owner + or by an individual or Legal Entity authorized to submit on behalf of + the copyright owner. For the purposes of this definition, "submitted" + means any form of electronic, verbal, or written communication sent + to the Licensor or its representatives, including but not limited to + communication on electronic mailing lists, source code control systems, + and issue tracking systems that are managed by, or on behalf of, the + Licensor for the purpose of discussing and improving the Work, but + excluding communication that is conspicuously marked or otherwise + designated in writing by the copyright owner as "Not a Contribution." + + "Contributor" shall mean Licensor and any individual or Legal Entity + on behalf of whom a Contribution has been received by Licensor and + subsequently incorporated within the Work. + + 2. Grant of Copyright License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + copyright license to reproduce, prepare Derivative Works of, + publicly display, publicly perform, sublicense, and distribute the + Work and such Derivative Works in Source or Object form. + + 3. Grant of Patent License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + (except as stated in this section) patent license to make, have made, + use, offer to sell, sell, import, and otherwise transfer the Work, + where such license applies only to those patent claims licensable + by such Contributor that are necessarily infringed by their + Contribution(s) alone or by combination of their Contribution(s) + with the Work to which such Contribution(s) was submitted. If You + institute patent litigation against any entity (including a + cross-claim or counterclaim in a lawsuit) alleging that the Work + or a Contribution incorporated within the Work constitutes direct + or contributory patent infringement, then any patent licenses + granted to You under this License for that Work shall terminate + as of the date such litigation is filed. + + 4. Redistribution. You may reproduce and distribute copies of the + Work or Derivative Works thereof in any medium, with or without + modifications, and in Source or Object form, provided that You + meet the following conditions: + + (a) You must give any other recipients of the Work or + Derivative Works a copy of this License; and + + (b) You must cause any modified files to carry prominent notices + stating that You changed the files; and + + (c) You must retain, in the Source form of any Derivative Works + that You distribute, all copyright, patent, trademark, and + attribution notices from the Source form of the Work, + excluding those notices that do not pertain to any part of + the Derivative Works; and + + (d) If the Work includes a "NOTICE" text file as part of its + distribution, then any Derivative Works that You distribute must + include a readable copy of the attribution notices contained + within such NOTICE file, excluding those notices that do not + pertain to any part of the Derivative Works, in at least one + of the following places: within a NOTICE text file distributed + as part of the Derivative Works; within the Source form or + documentation, if provided along with the Derivative Works; or, + within a display generated by the Derivative Works, if and + wherever such third-party notices normally appear. The contents + of the NOTICE file are for informational purposes only and + do not modify the License. You may add Your own attribution + notices within Derivative Works that You distribute, alongside + or as an addendum to the NOTICE text from the Work, provided + that such additional attribution notices cannot be construed + as modifying the License. + + You may add Your own copyright statement to Your modifications and + may provide additional or different license terms and conditions + for use, reproduction, or distribution of Your modifications, or + for any such Derivative Works as a whole, provided Your use, + reproduction, and distribution of the Work otherwise complies with + the conditions stated in this License. + + 5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. + + 6. Trademarks. This License does not grant permission to use the trade + names, trademarks, service marks, or product names of the Licensor, + except as required for reasonable and customary use in describing the + origin of the Work and reproducing the content of the NOTICE file. + + 7. Disclaimer of Warranty. Unless required by applicable law or + agreed to in writing, Licensor provides the Work (and each + Contributor provides its Contributions) on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or + implied, including, without limitation, any warranties or conditions + of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A + PARTICULAR PURPOSE. You are solely responsible for determining the + appropriateness of using or redistributing the Work and assume any + risks associated with Your exercise of permissions under this License. + + 8. Limitation of Liability. In no event and under no legal theory, + whether in tort (including negligence), contract, or otherwise, + unless required by applicable law (such as deliberate and grossly + negligent acts) or agreed to in writing, shall any Contributor be + liable to You for damages, including any direct, indirect, special, + incidental, or consequential damages of any character arising as a + result of this License or out of the use or inability to use the + Work (including but not limited to damages for loss of goodwill, + work stoppage, computer failure or malfunction, or any and all + other commercial damages or losses), even if such Contributor + has been advised of the possibility of such damages. + + 9. Accepting Warranty or Additional Liability. While redistributing + the Work or Derivative Works thereof, You may choose to offer, + and charge a fee for, acceptance of support, warranty, indemnity, + or other liability obligations and/or rights consistent with this + License. However, in accepting such obligations, You may act only + on Your own behalf and on Your sole responsibility, not on behalf + of any other Contributor, and only if You agree to indemnify, + defend, and hold each Contributor harmless for any liability + incurred by, or claims asserted against, such Contributor by reason + of your accepting any such warranty or additional liability. + + END OF TERMS AND CONDITIONS + + APPENDIX: How to apply the Apache License to your work. + + To apply the Apache License to your work, attach the following + boilerplate notice, with the fields enclosed by brackets "[]" + replaced with your own identifying information. (Don't include + the brackets!) The text should be enclosed in the appropriate + comment syntax for the file format. We also recommend that a + file or class name and description of purpose be included on the + same "printed page" as the copyright notice for easier + identification within third-party archives. + + Copyright [yyyy] [name of copyright owner] + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. diff --git a/src/mos4d/pybind/3rdparty/tbb/tbb.cmake b/src/mos4d/pybind/3rdparty/tbb/tbb.cmake new file mode 100644 index 0000000..7d95b44 --- /dev/null +++ b/src/mos4d/pybind/3rdparty/tbb/tbb.cmake @@ -0,0 +1,40 @@ +# 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. +# option(BUILD_SHARED_LIBS ON) +option(BUILD_SHARED_LIBS OFF) +option(TBBMALLOC_BUILD OFF) +option(TBB_EXAMPLES OFF) +option(TBB_STRICT OFF) +option(TBB_TEST OFF) + +include(FetchContent) +FetchContent_Declare(tbb SYSTEM URL https://github.com/oneapi-src/oneTBB/archive/refs/tags/v2021.8.0.tar.gz) +if(NOT tbb_POPULATED) + FetchContent_Populate(tbb) + add_subdirectory(${tbb_SOURCE_DIR} ${tbb_BINARY_DIR} EXCLUDE_FROM_ALL) +endif() + +if(${CMAKE_VERSION} VERSION_LESS 3.25) + get_target_property(tbb_include_dirs tbb INTERFACE_INCLUDE_DIRECTORIES) + set_target_properties(tbb PROPERTIES INTERFACE_SYSTEM_INCLUDE_DIRECTORIES "${tbb_include_dirs}") +endif() diff --git a/src/mos4d/pybind/CMakeLists.txt b/src/mos4d/pybind/CMakeLists.txt new file mode 100644 index 0000000..d7a55dc --- /dev/null +++ b/src/mos4d/pybind/CMakeLists.txt @@ -0,0 +1,46 @@ +# 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. +cmake_minimum_required(VERSION 3.16...3.26) +project(mos4d_cpp VERSION 1.0.0 LANGUAGES CXX) + +# Setup build options +option(USE_SYSTEM_EIGEN3 "Use system pre-installed Eigen" ON) +option(USE_SYSTEM_SOPHUS "Use system pre-installed Sophus" ON) +option(USE_SYSTEM_TBB "Use system pre-installed oneAPI/tbb" ON) + +# Set build type (repeat here for C++ only consumers) +set(CMAKE_BUILD_TYPE Release) +set(CMAKE_EXPORT_COMPILE_COMMANDS ON) +set(CMAKE_POSITION_INDEPENDENT_CODE ON) + +include(3rdparty/find_dependencies.cmake) + +set(PYBIND11_NEWPYTHON ON) +find_package(Python COMPONENTS Interpreter Development.Module REQUIRED) +find_package(pybind11 CONFIG REQUIRED) + +# Python bindings +pybind11_add_module(mos4d_pybind MODULE mos4d_pybind.cpp Deskew.cpp) +target_compile_features(mos4d_pybind PRIVATE cxx_std_17) +target_link_libraries(mos4d_pybind PUBLIC Eigen3::Eigen TBB::tbb Sophus::Sophus) +install(TARGETS mos4d_pybind LIBRARY DESTINATION .) diff --git a/src/mos4d/pybind/Deskew.cpp b/src/mos4d/pybind/Deskew.cpp new file mode 100644 index 0000000..610b9e7 --- /dev/null +++ b/src/mos4d/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 mos4d { + +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 mos4d diff --git a/src/mos4d/pybind/Deskew.hpp b/src/mos4d/pybind/Deskew.hpp new file mode 100644 index 0000000..2d94e5d --- /dev/null +++ b/src/mos4d/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 mos4d { + +std::vector Deskew(const std::vector &frame, + const std::vector ×tamps, + const Sophus::SE3d &relative_motion); + +} // namespace mos4d diff --git a/src/mos4d/pybind/mos4d_pybind.cpp b/src/mos4d/pybind/mos4d_pybind.cpp new file mode 100644 index 0000000..d2747a1 --- /dev/null +++ b/src/mos4d/pybind/mos4d_pybind.cpp @@ -0,0 +1,56 @@ +// 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 +#include +#include +#include +#include + +#include +#include +#include + +#include "Deskew.hpp" +#include "stl_vector_eigen.h" + +namespace py = pybind11; +using namespace py::literals; + +PYBIND11_MAKE_OPAQUE(std::vector); + +namespace mos4d { +PYBIND11_MODULE(mos4d_pybind, m) { + auto vector3dvector = pybind_eigen_vector_of_vector( + 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); +} +} // namespace mos4d diff --git a/src/mos4d/pybind/stl_vector_eigen.h b/src/mos4d/pybind/stl_vector_eigen.h new file mode 100644 index 0000000..db45e3e --- /dev/null +++ b/src/mos4d/pybind/stl_vector_eigen.h @@ -0,0 +1,117 @@ +// ---------------------------------------------------------------------------- +// NOTE: This fily has been adapted from the Open3D project, but copyright +// still belongs to Open3D. All rights reserved +// ---------------------------------------------------------------------------- +// - Open3D: www.open3d.org - +// ---------------------------------------------------------------------------- +// The MIT License (MIT) +// +// Copyright (c) 2018-2021 www.open3d.org +// +// 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 + +// pollute namespace with py +#include +namespace py = pybind11; +#include +#include +#include +#include +#include + +namespace pybind11 { + +template , typename... Args> +py::class_ bind_vector_without_repr(py::module &m, + std::string const &name, + Args &&...args) { + // hack function to disable __repr__ for the convenient function + // bind_vector() + using Class_ = py::class_; + Class_ cl(m, name.c_str(), std::forward(args)...); + cl.def(py::init<>()); + cl.def( + "__bool__", [](const Vector &v) -> bool { return !v.empty(); }, + "Check whether the list is nonempty"); + cl.def("__len__", &Vector::size); + return cl; +} + +// - This function is used by Pybind for std::vector constructor. +// This optional constructor is added to avoid too many Python <-> C++ API +// calls when the vector size is large using the default biding method. +// Pybind matches np.float64 array to py::array_t buffer. +// - Directly using templates for the py::array_t and py::array_t +// and etc. doesn't work. The current solution is to explicitly implement +// bindings for each py array types. +template +std::vector py_array_to_vectors_double( + py::array_t array) { + int64_t eigen_vector_size = EigenVector::SizeAtCompileTime; + if (array.ndim() != 2 || array.shape(1) != eigen_vector_size) { + throw py::cast_error(); + } + std::vector eigen_vectors(array.shape(0)); + auto array_unchecked = array.mutable_unchecked<2>(); + for (auto i = 0; i < array_unchecked.shape(0); ++i) { + eigen_vectors[i] = Eigen::Map(&array_unchecked(i, 0)); + } + return eigen_vectors; +} + +} // namespace pybind11 + +template , + typename holder_type = std::unique_ptr, + typename InitFunc> +py::class_ pybind_eigen_vector_of_vector(py::module &m, + const std::string &bind_name, + const std::string &repr_name, + InitFunc init_func) { + using Scalar = typename EigenVector::Scalar; + auto vec = py::bind_vector_without_repr>( + m, bind_name, py::buffer_protocol(), py::module_local()); + vec.def(py::init(init_func)); + vec.def_buffer([](std::vector &v) -> py::buffer_info { + size_t rows = EigenVector::RowsAtCompileTime; + return py::buffer_info(v.data(), sizeof(Scalar), py::format_descriptor::format(), 2, + {v.size(), rows}, {sizeof(EigenVector), sizeof(Scalar)}); + }); + vec.def("__repr__", [repr_name](const std::vector &v) { + return repr_name + std::string(" with ") + std::to_string(v.size()) + + std::string(" elements.\n") + std::string("Use numpy.asarray() to access data."); + }); + vec.def("__copy__", [](std::vector &v) { return std::vector(v); }); + vec.def("__deepcopy__", + [](std::vector &v) { return std::vector(v); }); + + // py::detail must be after custom constructor + using Class_ = py::class_>; + py::detail::vector_if_copy_constructible(vec); + py::detail::vector_if_equal_operator(vec); + py::detail::vector_modifiers(vec); + py::detail::vector_accessor(vec); + + return vec; +}