diff --git a/.github/workflows/pypi_publish.yml b/.github/workflows/pypi_publish.yml new file mode 100644 index 0000000..7b19a7b --- /dev/null +++ b/.github/workflows/pypi_publish.yml @@ -0,0 +1,35 @@ +name: Publish to PyPI.org +on: + release: + types: [published] + push: + branches: ["main"] + pull_request: + branches: ["main"] + +jobs: + build_sdist: + name: Build source distribution + runs-on: ubuntu-latest + steps: + - uses: actions/checkout@v3 + - name: Build sdist + run: pipx run build --sdist ${{github.workspace}} + - uses: actions/upload-artifact@v3 + with: + path: dist/*.tar.gz + + pypi: + if: github.event_name == 'release' + needs: build_sdist + runs-on: ubuntu-latest + steps: + - uses: actions/download-artifact@v3 + with: + name: artifact + path: dist + + - uses: pypa/gh-action-pypi-publish@release/v1 + with: + repository-url: https://test.pypi.org/legacy/ + password: ${{ secrets.TEST_PYPI_API_TOKEN }} diff --git a/.github/workflows/pythonbuild.yml b/.github/workflows/pythonbuild.yml new file mode 100644 index 0000000..0814787 --- /dev/null +++ b/.github/workflows/pythonbuild.yml @@ -0,0 +1,25 @@ +name: Python API Build +on: + push: + branches: ["main"] + pull_request: + branches: [] + +jobs: + python_package: + runs-on: ${{ matrix.os }} + strategy: + matrix: + os: [ubuntu-20.04, ubuntu-22.04] + steps: + - uses: actions/checkout@v4 + - name: Set up Python3 + uses: actions/setup-python@v5 + with: + python-version: "3.11" + - name: Install dependencies + run: | + python -m pip install --upgrade pip + - name: Build pip package + run: | + python -m pip install . diff --git a/.github/workflows/pythonbuild_devel.yml b/.github/workflows/pythonbuild_devel.yml index bf88f45..f056556 100644 --- a/.github/workflows/pythonbuild_devel.yml +++ b/.github/workflows/pythonbuild_devel.yml @@ -12,12 +12,14 @@ jobs: matrix: os: [ubuntu-20.04, ubuntu-22.04] steps: - - uses: actions/checkout@v3 + - uses: actions/checkout@v4 - name: Set up Python3 - uses: actions/setup-python@v3 + uses: actions/setup-python@v5 + with: + python-version: "3.11" - name: Install dependencies run: | python -m pip install --upgrade pip - name: Build pip package run: | - python -m pip install --verbose ./python/ + python -m pip install --verbose . diff --git a/mad-icp/CMakeLists.txt b/mad-icp/CMakeLists.txt deleted file mode 100755 index 425d137..0000000 --- a/mad-icp/CMakeLists.txt +++ /dev/null @@ -1,31 +0,0 @@ -cmake_minimum_required(VERSION 3.8 FATAL_ERROR) - -project(mad-icp LANGUAGES CXX) - -set(CMAKE_BUILD_TYPE Release) -set(CMAKE_CXX_STANDARD 17) -set(CMAKE_CXX_FLAGS -Werror) - -# this is required only to compile apps in C++, if you work in Python this can be set to off -# if need to compile C++ apps, when executing CMake run `cmake -DCOMPILE_CPP_APPS=ON ..` -option(COMPILE_CPP_APPS "Set to ON to compile C++ applications" OFF) - -# Eigen -find_package(Eigen3 3.3 REQUIRED NO_MODULE) - -# OpenMP -find_package(OpenMP REQUIRED) -set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}") - -# Python bindings -find_package(pybind11 REQUIRED) - -include_directories( - ${PROJECT_SOURCE_DIR}/src - ${EIGEN3_INCLUDE_DIR} - ${PYTHON_INCLUDE_DIRS} -) - -add_subdirectory(src) -# TODO only if one wants to build cpp runners -add_subdirectory(apps/cpp_runners) \ No newline at end of file diff --git a/mad-icp/apps/mad-icp.py b/mad-icp/apps/mad-icp.py deleted file mode 100755 index 8a55eff..0000000 --- a/mad-icp/apps/mad-icp.py +++ /dev/null @@ -1,161 +0,0 @@ -# Copyright 2024 R(obots) V(ision) and P(erception) group -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions are met: -# -# 1. Redistributions of source code must retain the above copyright notice, -# this list of conditions and the following disclaimer. -# -# 2. Redistributions in binary form must reproduce the above copyright notice, -# this list of conditions and the following disclaimer in the documentation -# and/or other materials provided with the distribution. -# -# 3. Neither the name of the copyright holder nor the names of its contributors -# may be used to endorse or promote products derived from this software -# without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE -# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR -# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF -# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS -# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -from pathlib import Path -import typer -from typing_extensions import Annotated -from rich.progress import track -from rich.console import Console -from enum import Enum -import os, sys, yaml -import numpy as np -from datetime import datetime -from utils.utils import write_transformed_pose -from utils.ros_reader import Ros1Reader -from utils.kitti_reader import KittiReader -from utils.visualizer import Visualizer - -sys.path.append("../build/src/odometry/") -# binded odometry -from pypeline import Pipeline, VectorEigen3d - - -console = Console() - -class InputDataInterface(str, Enum): - kitti = "kitti", - ros1 = "ros1", - # Can insert additional conversion formats - -InputDataInterface_lut = { - InputDataInterface.kitti: KittiReader, - InputDataInterface.ros1: Ros1Reader -} - -def main(data_path: Annotated[ - Path, typer.Option(help="path containing one or more rosbags (folder path)", show_default=False)], - estimate_path: Annotated[ - Path, typer.Option(help="trajectory estimate output path (folder path)", show_default=False)], - dataset_config: Annotated[ - Path, typer.Option(help="dataset configuration file", show_default=False)], - mad_icp_config: Annotated[ - Path, typer.Option(help="parameters for mad icp", show_default=True)] = "../configurations/params.cfg", - num_cores: Annotated[ - int, typer.Option(help="how many threads to use for icp (suggest maximum num)", show_default=True)] = 4, - num_keyframes: Annotated[ - int, typer.Option(help="max number of kf kept in the local map (suggest as num threads)", show_default=True)] = 4, - realtime: Annotated[ - bool, typer.Option(help="if true anytime realtime", show_default=True)] = False, - noviz: Annotated[ - bool, typer.Option(help="if true visualizer on", show_default=True)] = False) -> None: - - if not data_path.is_dir() or not estimate_path.is_dir() or not dataset_config.is_file(): - console.print("[red] Input dir or file not correct") - sys.exit(-1) - - visualizer = None - if not noviz: - visualizer = Visualizer() - - reader_type = InputDataInterface.kitti - if len(list(data_path.glob("*.bag"))) != 0: - console.print("[yellow] The dataset is in rosbag format") - reader_type = InputDataInterface.ros1 - else: - console.print("[yellow] The dataset is in kitti format") - - console.print("[green] Parsing dataset configuration file") - data_config_file = open(dataset_config, 'r') - data_cf = yaml.safe_load(data_config_file) - min_range = data_cf["min_range"] - max_range = data_cf["max_range"] - sensor_hz = data_cf["sensor_hz"] - deskew = data_cf["deskew"] - topic = None - if reader_type == InputDataInterface.ros1 : - topic = data_cf["rosbag_topic"] - lidar_to_base = np.array(data_cf["lidar_to_base"]) - - console.print("[green] Parsing mad-icp configuration file") - mad_icp_config_file = open(mad_icp_config, 'r') - mad_icp_cf = yaml.safe_load(mad_icp_config_file) - b_max = mad_icp_cf["b_max"] - b_min = mad_icp_cf["b_min"] - b_ratio = mad_icp_cf["b_ratio"] - p_th = mad_icp_cf["p_th"] - rho_ker = mad_icp_cf["rho_ker"] - n = mad_icp_cf["n"] - - # check some params for machine - if(realtime and num_keyframes > num_cores): - console.print("[red] If you chose realtime option, we suggest to chose a num_cores at least >= than the num_keyframes") - sys.exit(-1) - - console.print("[green] Setting up pipeline for odometry estimation") - pipeline = Pipeline(sensor_hz, deskew, b_max, rho_ker, p_th, b_min, b_ratio, num_keyframes, num_cores, realtime) - - estimate_file_name = estimate_path / "estimate.txt" - estimate_file = open(estimate_file_name, 'w') - - with InputDataInterface_lut[reader_type](data_path, min_range, max_range, topic=topic, sensor_hz=sensor_hz) as reader: - t_start = datetime.now() - for ts, points in track(reader, description="processing..."): - - print("Loading frame #", pipeline.currentID()) - - points = VectorEigen3d(points) - t_end = datetime.now() - t_delta = t_end - t_start - print("Time for reading points [ms]: ", t_delta.total_seconds() * 1000) - - t_start = datetime.now() - pipeline.compute(ts, points) - t_end = datetime.now() - t_delta = t_end - t_start - print("Time for odometry estimation [ms]: ", t_delta.total_seconds() * 1000) - - lidar_to_world = pipeline.currentPose() - write_transformed_pose(estimate_file, lidar_to_world, lidar_to_base) - - if not noviz: - t_start = datetime.now() - if pipeline.isMapUpdated(): - visualizer.update(pipeline.currentLeaves(), pipeline.modelLeaves(), lidar_to_world, pipeline.keyframePose()) - else: - visualizer.update(pipeline.currentLeaves(), None, lidar_to_world, None) - t_end = datetime.now() - t_delta = t_end - t_start - print("Time for visualization [ms]:", t_delta.total_seconds() * 1000, "\n") - - t_start = datetime.now() - - estimate_file.close() - - -if __name__ == '__main__': - typer.run(main) \ No newline at end of file diff --git a/mad-icp/src/odometry/CMakeLists.txt b/mad-icp/src/odometry/CMakeLists.txt deleted file mode 100755 index fe88177..0000000 --- a/mad-icp/src/odometry/CMakeLists.txt +++ /dev/null @@ -1,18 +0,0 @@ -add_library(odometry SHARED - vel_estimator.cpp - pipeline.cpp - mad_icp.cpp -) - -target_link_libraries(odometry - tools -) - -target_compile_features(odometry PUBLIC) - -pybind11_add_module(pypeline pybind/pypeline.cpp) -target_link_libraries(pypeline PUBLIC - odometry - tools -) - diff --git a/mad-icp/src/odometry/pybind/pypeline.cpp b/mad-icp/src/odometry/pybind/pypeline.cpp deleted file mode 100755 index ad235c2..0000000 --- a/mad-icp/src/odometry/pybind/pypeline.cpp +++ /dev/null @@ -1,50 +0,0 @@ -// pybind11 -#include -#include -#include -#include -#include -#include -#include -#include - -// std stuff -#include -#include -#include - -#include "../pipeline.h" -#include "eigen_stl_bindings.h" - -PYBIND11_MAKE_OPAQUE(std::vector); - -namespace py11 = pybind11; -using namespace py11::literals; - -PYBIND11_MODULE(pypeline, m) { - auto vector3dvector = pybind_eigen_vector_of_vector( - m, "VectorEigen3d", "std::vector", py11::py_array_to_vectors_double); - - auto pipeline = py11::class_(m, "Pipeline") - .def(py11::init(), - py11::arg("sensor_hz"), - py11::arg("deskew"), - py11::arg("b_max"), - py11::arg("rho_ker"), - py11::arg("p_th"), - py11::arg("b_min"), - py11::arg("b_ratio"), - py11::arg("num_keyframes"), - py11::arg("num_threads"), - py11::arg("realtime")) - .def("currentPose", &Pipeline::currentPose) - .def("trajectory", &Pipeline::trajectory) - .def("keyframePose", &Pipeline::keyframePose) - .def("isInitialized", &Pipeline::isInitialized) - .def("isMapUpdated", &Pipeline::isMapUpdated) - .def("currentID", &Pipeline::currentID) - .def("keyframeID", &Pipeline::keyframeID) - .def("modelLeaves", &Pipeline::modelLeaves) - .def("currentLeaves", &Pipeline::currentLeaves) - .def("compute", &Pipeline::compute); -} diff --git a/mad-icp/src/tools/CMakeLists.txt b/mad-icp/src/tools/CMakeLists.txt deleted file mode 100755 index dd7f8ad..0000000 --- a/mad-icp/src/tools/CMakeLists.txt +++ /dev/null @@ -1,6 +0,0 @@ -add_library(tools SHARED - mad_tree.cpp -) - -target_link_libraries(tools) -target_compile_features(tools PUBLIC) \ No newline at end of file diff --git a/mad-icp/.clang-format b/mad_icp/.clang-format similarity index 100% rename from mad-icp/.clang-format rename to mad_icp/.clang-format diff --git a/mad_icp/CMakeLists.txt b/mad_icp/CMakeLists.txt new file mode 100755 index 0000000..2fe2c52 --- /dev/null +++ b/mad_icp/CMakeLists.txt @@ -0,0 +1,54 @@ +# TODO: Change version to at least 3.11 and include FetchContent for Eigen +cmake_minimum_required(VERSION 3.8 FATAL_ERROR) + +project(mad_icp LANGUAGES CXX) + +set(CMAKE_BUILD_TYPE Release) +set(CMAKE_CXX_STANDARD 17) +set(CMAKE_CXX_FLAGS -Werror) + +# this is required only to compile apps in C++, if you work in Python this can be set to off +# if need to compile C++ apps, when executing CMake run `cmake -DCOMPILE_CPP_APPS=ON ..` +option(COMPILE_CPP_APPS "Set to ON to compile C++ applications" OFF) + +# Eigen +# TODO: Detect if Eigen exists in system, oterwhise, download it through FetchContent +# find_package(Eigen3 3.3 NO_MODULE) +if(NOT DEFINED Eigen3_FOUND) + message(CHECK_START "Fetching Eigen3") + list(APPEND CMAKE_MESSAGE_INDENT " ") + + include(FetchContent) + FetchContent_Declare( + Eigen + URL https://gitlab.com/libeigen/eigen/-/archive/3.4.0/eigen-3.4.0.tar.gz) + FetchContent_GetProperties(Eigen) + + if(NOT eigen_POPULATED) + FetchContent_Populate(Eigen) + add_subdirectory(${eigen_SOURCE_DIR} ${eigen_BINARY_DIR} EXCLUDE_FROM_ALL) + endif() + + list(POP_BACK CMAKE_MESSAGE_INDENT) + message(CHECK_PASS "fetched") +endif() + +# OpenMP +find_package(OpenMP REQUIRED) +set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}") +set(CMAKE_POSITION_INDEPENDENT_CODE ON) + +# Python bindings +find_package(pybind11 REQUIRED) + +include_directories( + ${PROJECT_SOURCE_DIR}/src + + # ${EIGEN3_INCLUDE_DIR} + ${PYTHON_INCLUDE_DIRS} +) + +add_subdirectory(src) + +# TODO only if one wants to build cpp runners +add_subdirectory(apps/cpp_runners) \ No newline at end of file diff --git a/mad_icp/__init__.py b/mad_icp/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/mad_icp/apps/__init__.py b/mad_icp/apps/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/mad-icp/apps/cpp_runners/CMakeLists.txt b/mad_icp/apps/cpp_runners/CMakeLists.txt similarity index 68% rename from mad-icp/apps/cpp_runners/CMakeLists.txt rename to mad_icp/apps/cpp_runners/CMakeLists.txt index fb5e888..cf792d3 100644 --- a/mad-icp/apps/cpp_runners/CMakeLists.txt +++ b/mad_icp/apps/cpp_runners/CMakeLists.txt @@ -1,4 +1,5 @@ -if(DEFINED COMPILE_CPP_APPS) +if(COMPILE_CPP_APPS) + message("Compiling CPP Apps") add_executable(bin_runner bin_runner.cpp) target_link_libraries(bin_runner odometry diff --git a/mad-icp/apps/cpp_runners/bin_runner.cpp b/mad_icp/apps/cpp_runners/bin_runner.cpp similarity index 100% rename from mad-icp/apps/cpp_runners/bin_runner.cpp rename to mad_icp/apps/cpp_runners/bin_runner.cpp diff --git a/mad-icp/apps/cpp_runners/cpp_utils/parse_cmd_line.h b/mad_icp/apps/cpp_runners/cpp_utils/parse_cmd_line.h similarity index 100% rename from mad-icp/apps/cpp_runners/cpp_utils/parse_cmd_line.h rename to mad_icp/apps/cpp_runners/cpp_utils/parse_cmd_line.h diff --git a/mad_icp/apps/mad_icp.py b/mad_icp/apps/mad_icp.py new file mode 100755 index 0000000..d4e46ed --- /dev/null +++ b/mad_icp/apps/mad_icp.py @@ -0,0 +1,185 @@ +# Copyright 2024 R(obots) V(ision) and P(erception) group +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# 1. Redistributions of source code must retain the above copyright notice, +# this list of conditions and the following disclaimer. +# +# 2. Redistributions in binary form must reproduce the above copyright notice, +# this list of conditions and the following disclaimer in the documentation +# and/or other materials provided with the distribution. +# +# 3. Neither the name of the copyright holder nor the names of its contributors +# may be used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. + +from pathlib import Path +import typer +from typing_extensions import Annotated +from rich.progress import track +from rich.console import Console +from enum import Enum +import os +import sys +import yaml +import numpy as np +from datetime import datetime +from mad_icp.apps.utils.utils import write_transformed_pose +from mad_icp.apps.utils.ros_reader import Ros1Reader +from mad_icp.apps.utils.kitti_reader import KittiReader +from mad_icp.apps.utils.visualizer import Visualizer +# binded Odometry +from mad_icp.src.pybind.pypeline import Pipeline, VectorEigen3d + + +console = Console() + + +class InputDataInterface(str, Enum): + kitti = "kitti", + ros1 = "ros1", + # Can insert additional conversion formats + + +InputDataInterface_lut = { + InputDataInterface.kitti: KittiReader, + InputDataInterface.ros1: Ros1Reader +} + + +def main(data_path: Annotated[ + Path, typer.Option(help="path containing one or more rosbags (folder path)", show_default=False)], + estimate_path: Annotated[ + Path, typer.Option(help="trajectory estimate output path (folder path)", show_default=False)], + dataset_config: Annotated[ + Path, typer.Option(help="dataset configuration file", show_default=False)], + mad_icp_config: Annotated[ + Path, typer.Option(help="parameters for mad icp", show_default=True)] = "../configurations/params.cfg", + num_cores: Annotated[ + int, typer.Option(help="how many threads to use for icp (suggest maximum num)", show_default=True)] = 4, + num_keyframes: Annotated[ + int, typer.Option(help="max number of kf kept in the local map (suggest as num threads)", show_default=True)] = 4, + realtime: Annotated[ + bool, typer.Option(help="if true anytime realtime", show_default=True)] = False, + noviz: Annotated[ + bool, typer.Option(help="if true visualizer on", show_default=True)] = False) -> None: + + if not data_path.exists(): + console.print(f"[red] {data_path} does not exist!") + sys.exit(-1) + if not estimate_path.is_dir(): + console.print( + f"[yellow] Output directory {estimate_path} does not exist. Creating new directory") + estimate_path.mkdir(parents=True, exist_ok=True) + + if not dataset_config.is_file(): + console.print( + f"[red] Dataset config file {dataset_config} does not exist!") + sys.exit(-1) + + visualizer = None + if not noviz: + visualizer = Visualizer() + + reader_type = InputDataInterface.kitti + if len(list(data_path.glob("*.bag"))) != 0: + console.print("[yellow] The dataset is in rosbag format") + reader_type = InputDataInterface.ros1 + else: + console.print("[yellow] The dataset is in kitti format") + + console.print("[green] Parsing dataset configuration file") + data_config_file = open(dataset_config, 'r') + data_cf = yaml.safe_load(data_config_file) + min_range = data_cf["min_range"] + max_range = data_cf["max_range"] + sensor_hz = data_cf["sensor_hz"] + deskew = data_cf["deskew"] + topic = None + if reader_type == InputDataInterface.ros1: + topic = data_cf["rosbag_topic"] + lidar_to_base = np.array(data_cf["lidar_to_base"]) + + console.print("[green] Parsing mad-icp configuration file") + mad_icp_config_file = open(mad_icp_config, 'r') + mad_icp_cf = yaml.safe_load(mad_icp_config_file) + b_max = mad_icp_cf["b_max"] + b_min = mad_icp_cf["b_min"] + b_ratio = mad_icp_cf["b_ratio"] + p_th = mad_icp_cf["p_th"] + rho_ker = mad_icp_cf["rho_ker"] + n = mad_icp_cf["n"] + + # check some params for machine + if (realtime and num_keyframes > num_cores): + console.print( + "[red] If you chose realtime option, we suggest to chose a num_cores at least >= than the num_keyframes") + sys.exit(-1) + + console.print("[green] Setting up pipeline for odometry estimation") + pipeline = Pipeline(sensor_hz, deskew, b_max, rho_ker, + p_th, b_min, b_ratio, num_keyframes, num_cores, realtime) + + estimate_file_name = estimate_path / "estimate.txt" + estimate_file = open(estimate_file_name, 'w') + + with InputDataInterface_lut[reader_type](data_path, min_range, max_range, topic=topic, sensor_hz=sensor_hz) as reader: + t_start = datetime.now() + for ts, points in track(reader, description="processing..."): + + print("Loading frame #", pipeline.currentID()) + + points = VectorEigen3d(points) + t_end = datetime.now() + t_delta = t_end - t_start + print("Time for reading points [ms]: ", + t_delta.total_seconds() * 1000) + + t_start = datetime.now() + pipeline.compute(ts, points) + t_end = datetime.now() + t_delta = t_end - t_start + print( + "Time for odometry estimation [ms]: ", t_delta.total_seconds() * 1000) + + lidar_to_world = pipeline.currentPose() + write_transformed_pose( + estimate_file, lidar_to_world, lidar_to_base) + + if not noviz: + t_start = datetime.now() + if pipeline.isMapUpdated(): + visualizer.update(pipeline.currentLeaves(), pipeline.modelLeaves( + ), lidar_to_world, pipeline.keyframePose()) + else: + visualizer.update(pipeline.currentLeaves(), + None, lidar_to_world, None) + t_end = datetime.now() + t_delta = t_end - t_start + print("Time for visualization [ms]:", + t_delta.total_seconds() * 1000, "\n") + + t_start = datetime.now() + + estimate_file.close() + + +def run(): + typer.run(main) + + +if __name__ == '__main__': + run() diff --git a/mad-icp/apps/utils/kitti_reader.py b/mad_icp/apps/utils/kitti_reader.py similarity index 100% rename from mad-icp/apps/utils/kitti_reader.py rename to mad_icp/apps/utils/kitti_reader.py diff --git a/mad-icp/apps/utils/point_cloud2.py b/mad_icp/apps/utils/point_cloud2.py similarity index 100% rename from mad-icp/apps/utils/point_cloud2.py rename to mad_icp/apps/utils/point_cloud2.py diff --git a/mad-icp/apps/utils/ros_reader.py b/mad_icp/apps/utils/ros_reader.py similarity index 85% rename from mad-icp/apps/utils/ros_reader.py rename to mad_icp/apps/utils/ros_reader.py index 3de8154..e71bb21 100644 --- a/mad-icp/apps/utils/ros_reader.py +++ b/mad_icp/apps/utils/ros_reader.py @@ -31,12 +31,13 @@ from pathlib import Path from typing import Tuple import natsort -from utils.point_cloud2 import read_point_cloud +from mad_icp.apps.utils.point_cloud2 import read_point_cloud import numpy as np + class Ros1Reader: - def __init__(self, data_dir: Path, min_range = 0, - max_range = 200, *args, **kwargs): + def __init__(self, data_dir: Path, min_range=0, + max_range=200, *args, **kwargs): """ :param data_dir: Directory containing rosbags or path to a rosbag file :param topics: Topic to read @@ -53,11 +54,12 @@ def __init__(self, data_dir: Path, min_range = 0, sys.exit(-1) if data_dir.is_file(): - #self.sequence_id = os.path.basename(data_dir).split(".")[0] + # self.sequence_id = os.path.basename(data_dir).split(".")[0] self.bag = AnyReader([data_dir]) else: - #self.sequence_id = os.path.basename(data_dir[0]).split(".")[0] - self.bag = AnyReader(natsort.natsorted([bag for bag in list(data_dir.glob("*.bag"))])) + # self.sequence_id = os.path.basename(data_dir[0]).split(".")[0] + self.bag = AnyReader(natsort.natsorted( + [bag for bag in list(data_dir.glob("*.bag"))])) print("Reading multiple .bag files in directory:") print("\n".join([path.name for path in self.bag.paths])) @@ -90,5 +92,6 @@ def __getitem__(self, item) -> Tuple[float, Tuple[np.ndarray, np.ndarray]]: connection, timestamp, rawdata = next(self.msgs) msg = self.bag.deserialize(rawdata, connection.msgtype) cloud_stamp = msg.header.stamp.sec + msg.header.stamp.nanosec * 1e-9 - points, _ = read_point_cloud(msg, min_range=self.min_range, max_range=self.max_range) - return timestamp, points \ No newline at end of file + points, _ = read_point_cloud( + msg, min_range=self.min_range, max_range=self.max_range) + return timestamp, points diff --git a/mad-icp/apps/utils/utils.py b/mad_icp/apps/utils/utils.py similarity index 100% rename from mad-icp/apps/utils/utils.py rename to mad_icp/apps/utils/utils.py diff --git a/mad-icp/apps/utils/visualizer.py b/mad_icp/apps/utils/visualizer.py similarity index 100% rename from mad-icp/apps/utils/visualizer.py rename to mad_icp/apps/utils/visualizer.py diff --git a/mad-icp/configurations/datasets/hilti_2021.cfg b/mad_icp/configurations/datasets/hilti_2021.cfg similarity index 100% rename from mad-icp/configurations/datasets/hilti_2021.cfg rename to mad_icp/configurations/datasets/hilti_2021.cfg diff --git a/mad-icp/configurations/datasets/kitti.cfg b/mad_icp/configurations/datasets/kitti.cfg similarity index 100% rename from mad-icp/configurations/datasets/kitti.cfg rename to mad_icp/configurations/datasets/kitti.cfg diff --git a/mad-icp/configurations/datasets/mulran.cfg b/mad_icp/configurations/datasets/mulran.cfg similarity index 100% rename from mad-icp/configurations/datasets/mulran.cfg rename to mad_icp/configurations/datasets/mulran.cfg diff --git a/mad-icp/configurations/datasets/newer_college_os0.cfg b/mad_icp/configurations/datasets/newer_college_os0.cfg similarity index 100% rename from mad-icp/configurations/datasets/newer_college_os0.cfg rename to mad_icp/configurations/datasets/newer_college_os0.cfg diff --git a/mad-icp/configurations/datasets/newer_college_os1.cfg b/mad_icp/configurations/datasets/newer_college_os1.cfg similarity index 100% rename from mad-icp/configurations/datasets/newer_college_os1.cfg rename to mad_icp/configurations/datasets/newer_college_os1.cfg diff --git a/mad-icp/configurations/datasets/vbr_os0.cfg b/mad_icp/configurations/datasets/vbr_os0.cfg similarity index 100% rename from mad-icp/configurations/datasets/vbr_os0.cfg rename to mad_icp/configurations/datasets/vbr_os0.cfg diff --git a/mad-icp/configurations/datasets/vbr_os1.cfg b/mad_icp/configurations/datasets/vbr_os1.cfg similarity index 100% rename from mad-icp/configurations/datasets/vbr_os1.cfg rename to mad_icp/configurations/datasets/vbr_os1.cfg diff --git a/mad-icp/configurations/params.cfg b/mad_icp/configurations/params.cfg similarity index 100% rename from mad-icp/configurations/params.cfg rename to mad_icp/configurations/params.cfg diff --git a/mad-icp/requirements.txt b/mad_icp/requirements.txt similarity index 100% rename from mad-icp/requirements.txt rename to mad_icp/requirements.txt diff --git a/mad-icp/src/CMakeLists.txt b/mad_icp/src/CMakeLists.txt similarity index 67% rename from mad-icp/src/CMakeLists.txt rename to mad_icp/src/CMakeLists.txt index 50cbd4e..91a5fd1 100755 --- a/mad-icp/src/CMakeLists.txt +++ b/mad_icp/src/CMakeLists.txt @@ -1,3 +1,3 @@ add_subdirectory(tools) add_subdirectory(odometry) - +add_subdirectory(pybind) diff --git a/mad_icp/src/odometry/CMakeLists.txt b/mad_icp/src/odometry/CMakeLists.txt new file mode 100755 index 0000000..9083530 --- /dev/null +++ b/mad_icp/src/odometry/CMakeLists.txt @@ -0,0 +1,11 @@ +add_library(odometry STATIC + vel_estimator.cpp + pipeline.cpp + mad_icp.cpp +) + +target_link_libraries(odometry + tools + Eigen3::Eigen +) +target_compile_features(odometry PUBLIC) \ No newline at end of file diff --git a/mad-icp/src/odometry/mad_icp.cpp b/mad_icp/src/odometry/mad_icp.cpp similarity index 100% rename from mad-icp/src/odometry/mad_icp.cpp rename to mad_icp/src/odometry/mad_icp.cpp diff --git a/mad-icp/src/odometry/mad_icp.h b/mad_icp/src/odometry/mad_icp.h similarity index 100% rename from mad-icp/src/odometry/mad_icp.h rename to mad_icp/src/odometry/mad_icp.h diff --git a/mad-icp/src/odometry/pipeline.cpp b/mad_icp/src/odometry/pipeline.cpp similarity index 100% rename from mad-icp/src/odometry/pipeline.cpp rename to mad_icp/src/odometry/pipeline.cpp diff --git a/mad-icp/src/odometry/pipeline.h b/mad_icp/src/odometry/pipeline.h similarity index 100% rename from mad-icp/src/odometry/pipeline.h rename to mad_icp/src/odometry/pipeline.h diff --git a/mad-icp/src/odometry/vel_estimator.cpp b/mad_icp/src/odometry/vel_estimator.cpp similarity index 100% rename from mad-icp/src/odometry/vel_estimator.cpp rename to mad_icp/src/odometry/vel_estimator.cpp diff --git a/mad-icp/src/odometry/vel_estimator.h b/mad_icp/src/odometry/vel_estimator.h similarity index 100% rename from mad-icp/src/odometry/vel_estimator.h rename to mad_icp/src/odometry/vel_estimator.h diff --git a/mad_icp/src/pybind/CMakeLists.txt b/mad_icp/src/pybind/CMakeLists.txt new file mode 100644 index 0000000..8a7ac6f --- /dev/null +++ b/mad_icp/src/pybind/CMakeLists.txt @@ -0,0 +1,8 @@ +pybind11_add_module(pypeline MODULE pypeline.cpp) +target_link_libraries(pypeline PRIVATE + Eigen3::Eigen + odometry + tools +) + +install(TARGETS pypeline DESTINATION .) \ No newline at end of file diff --git a/mad-icp/src/odometry/pybind/eigen_stl_bindings.h b/mad_icp/src/pybind/eigen_stl_bindings.h similarity index 100% rename from mad-icp/src/odometry/pybind/eigen_stl_bindings.h rename to mad_icp/src/pybind/eigen_stl_bindings.h diff --git a/mad_icp/src/pybind/pypeline.cpp b/mad_icp/src/pybind/pypeline.cpp new file mode 100755 index 0000000..bdbac5f --- /dev/null +++ b/mad_icp/src/pybind/pypeline.cpp @@ -0,0 +1,47 @@ +// pybind11 +#include +#include +#include +#include +#include +#include +#include +#include + +// std stuff +#include +#include +#include + +#include "eigen_stl_bindings.h" +#include "odometry/pipeline.h" + +PYBIND11_MAKE_OPAQUE(std::vector); + +namespace py11 = pybind11; +using namespace py11::literals; + +PYBIND11_MODULE(pypeline, m) { + auto vector3dvector = pybind_eigen_vector_of_vector( + m, "VectorEigen3d", "std::vector", + py11::py_array_to_vectors_double); + + auto pipeline = + py11::class_(m, "Pipeline") + .def(py11::init(), + py11::arg("sensor_hz"), py11::arg("deskew"), py11::arg("b_max"), + py11::arg("rho_ker"), py11::arg("p_th"), py11::arg("b_min"), + py11::arg("b_ratio"), py11::arg("num_keyframes"), + py11::arg("num_threads"), py11::arg("realtime")) + .def("currentPose", &Pipeline::currentPose) + .def("trajectory", &Pipeline::trajectory) + .def("keyframePose", &Pipeline::keyframePose) + .def("isInitialized", &Pipeline::isInitialized) + .def("isMapUpdated", &Pipeline::isMapUpdated) + .def("currentID", &Pipeline::currentID) + .def("keyframeID", &Pipeline::keyframeID) + .def("modelLeaves", &Pipeline::modelLeaves) + .def("currentLeaves", &Pipeline::currentLeaves) + .def("compute", &Pipeline::compute); +} diff --git a/mad_icp/src/tools/CMakeLists.txt b/mad_icp/src/tools/CMakeLists.txt new file mode 100755 index 0000000..5342c94 --- /dev/null +++ b/mad_icp/src/tools/CMakeLists.txt @@ -0,0 +1,5 @@ +add_library(tools STATIC + mad_tree.cpp +) +target_link_libraries(tools PUBLIC Eigen3::Eigen) +target_compile_features(tools PUBLIC) \ No newline at end of file diff --git a/mad-icp/src/tools/constants.h b/mad_icp/src/tools/constants.h similarity index 100% rename from mad-icp/src/tools/constants.h rename to mad_icp/src/tools/constants.h diff --git a/mad-icp/src/tools/frame.h b/mad_icp/src/tools/frame.h similarity index 100% rename from mad-icp/src/tools/frame.h rename to mad_icp/src/tools/frame.h diff --git a/mad-icp/src/tools/lie_algebra.h b/mad_icp/src/tools/lie_algebra.h similarity index 100% rename from mad-icp/src/tools/lie_algebra.h rename to mad_icp/src/tools/lie_algebra.h diff --git a/mad-icp/src/tools/mad_tree.cpp b/mad_icp/src/tools/mad_tree.cpp similarity index 100% rename from mad-icp/src/tools/mad_tree.cpp rename to mad_icp/src/tools/mad_tree.cpp diff --git a/mad-icp/src/tools/mad_tree.h b/mad_icp/src/tools/mad_tree.h similarity index 100% rename from mad-icp/src/tools/mad_tree.h rename to mad_icp/src/tools/mad_tree.h diff --git a/mad-icp/src/tools/utils.h b/mad_icp/src/tools/utils.h similarity index 100% rename from mad-icp/src/tools/utils.h rename to mad_icp/src/tools/utils.h diff --git a/pyproject.toml b/pyproject.toml new file mode 100644 index 0000000..a470106 --- /dev/null +++ b/pyproject.toml @@ -0,0 +1,48 @@ +[build-system] +requires = ["scikit-build-core", "pybind11"] +build-backend = "scikit_build_core.build" + +[project] +name = "mad-icp" +version = "0.0.1" +description = "It Is All About Matching Data -- Robust and Informed LiDAR Odometry" +readme = "README.md" +authors = [ + { name = "Simone Ferrari", email = "s.ferrari@diag.uniroma1.it" }, + { name = "Luca Di Giammarino", email = "digiammarino@diag.uniroma1.it" }, + { name = "Leonardo Brizi", email = "brizi@diag.uniroma1.it" }, + { name = "Emanuele Giacomini", email = "giacomini@diag.uniroma1.it" }, +] +keywords = ["LiDAR", "TODO"] +requires-python = ">=3.8" + +classifiers = [ + "Programming Language :: Python :: 3", + "License :: OSI Approved :: BSD License", + "Operating System :: Unix", + "Intended Audience :: Developers", + "Intended Audience :: Science/Research", +] +dependencies = [ + "PyYAML", + "numpy", + "rosbags", + "open3d", + "matplotlib", + "typer>=0.10.0", + "natsort", + "rich", +] + +[project.scripts] +mad_icp = "mad_icp.apps.mad_icp:run" + +[project.urls] +Homepage = "https://github.com/rvp-group/mad-icp" + +[tool.scikit-build] +build-dir = "build/{wheel_tag}" +cmake.source-dir = "mad_icp/" +cmake.verbose = false +cmake.version = ">=3.8" +wheel.install-dir = "mad_icp/src/pybind"