1 missing dataloader variety

.clang-format

@@ -7,4 +7,4 @@ BinPackParameters: false
 SortIncludes: true
 Standard: c++20
 DerivePointerAlignment: false
-PointerAlignment: Right
+PointerAlignment: Right

.cmake-format.yaml

@@ -1,4 +1,4 @@
 enable_markup: false
 line_width: 120
 format:
-  max_subgroups_hwrap: 5
+  max_subgroups_hwrap: 5

Dockerfile

@@ -34,12 +34,17 @@ RUN apt-get update && apt-get install -y -q --no-install-recommends \
     wget \
     libeigen3-dev \
     libomp-dev \
+    libtbb-dev \
     libgoogle-glog-dev \
     libgflags-dev \
     libatlas-base-dev \
     libsuitesparse-dev \
-    libpcl-dev \
-    python3-pip
+    python3-dev \
+    python3-pip \
+    python3-setuptools \
+    python3-distutils \
+    pybind11-dev \
+    liblzf-dev
 
 RUN pip install numpy ninja
 
@@ -52,7 +57,7 @@ RUN wget http://ceres-solver.org/ceres-solver-2.2.0.tar.gz && \
     cmake ../ceres-solver-2.2.0 && make -j3 && make test && make install
 
 # Irindescence
-RUN apt-get update && apt-get install -y -q --no-install-recommends curl gnupg
+RUN apt-get update && apt-get install -y -q --no-install-recommends curl gnupg libpng-dev libjpeg-dev
 RUN mkdir -m 0755 -p /etc/apt/keyrings/
 RUN curl -fsSL https://koide3.github.io/ppa/ubuntu2204/KEY.gpg | gpg --dearmor -o /etc/apt/keyrings/koide3_ppa.gpg
 RUN echo "deb [signed-by=/etc/apt/keyrings/koide3_ppa.gpg] https://koide3.github.io/ppa/ubuntu2204 ./" | tee /etc/apt/sources.list.d/koide3_ppa.list > /dev/null

Dockerfile.dev

@@ -34,12 +34,17 @@ RUN apt-get update && apt-get install -y -q --no-install-recommends \
     wget \
     libeigen3-dev \
     libomp-dev \
+    libtbb-dev \
     libgoogle-glog-dev \
     libgflags-dev \
     libatlas-base-dev \
     libsuitesparse-dev \
-    libpcl-dev \
-    python3-pip
+    python3-dev \
+    python3-pip \
+    python3-setuptools \
+    python3-distutils \
+    pybind11-dev \
+    liblzf-dev
 
 RUN pip install numpy ninja
 
@@ -52,7 +57,7 @@ RUN wget http://ceres-solver.org/ceres-solver-2.2.0.tar.gz && \
     cmake ../ceres-solver-2.2.0 && make -j3 && make test && make install
 
 # Irindescence
-RUN apt-get update && apt-get install -y -q --no-install-recommends curl gnupg
+RUN apt-get update && apt-get install -y -q --no-install-recommends curl gnupg libpng-dev libjpeg-dev
 RUN mkdir -m 0755 -p /etc/apt/keyrings/
 RUN curl -fsSL https://koide3.github.io/ppa/ubuntu2204/KEY.gpg | gpg --dearmor -o /etc/apt/keyrings/koide3_ppa.gpg
 RUN echo "deb [signed-by=/etc/apt/keyrings/koide3_ppa.gpg] https://koide3.github.io/ppa/ubuntu2204 ./" | tee /etc/apt/sources.list.d/koide3_ppa.list > /dev/null

README.md

@@ -3,9 +3,11 @@
     <h2>Zero-Drift Point Cloud Mapping using Map Priors</h2>
   <br>
 
+  ![C++](https://img.shields.io/badge/-C++-blue?logo=cplusplus)
+  ![Python](https://img.shields.io/badge/Python-3670A0?logo=python&logoColor=ffdd54)
   [![Docker](https://badgen.net/badge/icon/docker?icon=docker&label)](https://www.docker.com/)
   ![License](https://img.shields.io/badge/license-Apache%202.0-blue)
-  ![Version](https://img.shields.io/badge/version-0.1.0-blue)
+  ![Version](https://img.shields.io/badge/version-0.2.0-blue)
   [![arXiv](https://img.shields.io/badge/arXiv-1234.56789-b31b1b.svg)](https://arxiv.org/abs/2501.11111)
 
   <br align="center">
@@ -35,9 +37,11 @@ We provide a Docker image on Docker Hub, which will automatically be pulled with
 <details>
 <summary>Run</summary>
 
-To use our approach, you need a reference map and an initial guess of the first pose.
+To use our approach, you need a reference map and an initial guess of the first pose.  
+More details on reference maps can be found in our paper.
 
-The easiest way to use our approach is with the provided Docker image.
+The easiest way to use our approach is with the provided Docker image.  
+We currently support point cloud files in `.bin`(KITTI), `.pcd`, `.ply` and `.xyz`.
 ```sh
 ./docker/run_docker.sh <map_path> <scan_path> <output_path> <x> <y> <z> <qx> <qy> <qz> <qw>
 
@@ -89,11 +93,7 @@ pip install -e .
 
 ## Limitations
 
-* Currently only the KITTI .bin dataloader is implemented
-* The reference map has to be in the .pcd format
-* Detailed instructions on how to create refrence maps is missing
-* Currently the visualization is active on default
-* Move to nanobind
+* Detailed instructions on how to create refrence maps are missing
 
 ## Acknowledgement
 
@@ -102,6 +102,7 @@ Great inspiration has come from the following repositories. If you use our work,
 * [KISS-ICP](https://github.com/PRBonn/kiss-icp)
 * [small_gicp](https://github.com/koide3/small_gicp)
 * [Iridescence](https://github.com/koide3/iridescence)
+* [Open3D](https://github.com/isl-org/Open3D)
 
 
 ## Citation

cpp/CMakeLists.txt

@@ -14,9 +14,11 @@ set(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS_RELEASE} -O3")
 
 find_package(Eigen3 REQUIRED)
 find_package(Ceres REQUIRED)
-find_package(PCL REQUIRED)
 find_package(Iridescence REQUIRED)
 
+find_path(LIBLZF_INCLUDE_DIR lzf.h PATH_SUFFIXES liblzf)
+find_library(LIBLZF_LIBRARY NAMES lzf liblzf)
+
 # Fetch external dependencies
 include(FetchContent)
 FetchContent_Declare(small_gicp GIT_REPOSITORY https://github.com/ga58lar/small_gicp)
@@ -29,18 +31,21 @@ add_library(
   core/pose_graph.cpp
   core/prediction.cpp
   core/preprocess.cpp
-  io/kitti_loader.cpp
-  io/map_loader.cpp
+  io/format/kitti.cpp
+  io/format/xyz.cpp
+  io/format/pcd.cpp
+  io/format/ply.cpp
+  io/loader_factory.cpp
   pipeline/openlidarmap.cpp
   utils/file_utils.cpp
   utils/pose_utils.cpp
   utils/helpers.cpp)
 
-target_include_directories(openlidarmap_lib PUBLIC ${CMAKE_CURRENT_SOURCE_DIR} PRIVATE ${PCL_INCLUDE_DIRS}
-                                                                                           ${small_gicp_INCLUDE_DIRS})
+target_include_directories(openlidarmap_lib PUBLIC ${CMAKE_CURRENT_SOURCE_DIR} PRIVATE ${small_gicp_INCLUDE_DIRS}
+                                                                                       ${LIBLZF_INCLUDE_DIR})
 
-target_link_libraries(openlidarmap_lib PUBLIC Eigen3::Eigen small_gicp ${PCL_LIBRARIES} Ceres::ceres
-                      PRIVATE Iridescence::Iridescence stdc++fs)
+target_link_libraries(openlidarmap_lib PUBLIC Eigen3::Eigen small_gicp Ceres::ceres PRIVATE Iridescence::Iridescence
+                                                                                            stdc++fs ${LIBLZF_LIBRARY})
 
 # Add executable
 add_executable(openlidarmap_cpp apps/main.cpp)

cpp/config/kernel_config.hpp

@@ -4,15 +4,12 @@
 
 namespace openlidarmap::config {
 
-    struct KernelConfig {
-        double model_sse{};
-        size_t num_samples{};
-        double sigma{};
-
-        KernelConfig()
-            :  model_sse(1.0),
-               num_samples(1),
-               sigma(1.0) {}
-    };
-
-    }  // namespace openlidarmap::config
+struct KernelConfig {
+    double model_sse{};
+    size_t num_samples{};
+    double sigma{};
+
+    KernelConfig() : model_sse(1.0), num_samples(1), sigma(1.0) {}
+};
+
+}  // namespace openlidarmap::config

cpp/config/preprocess_config.hpp

@@ -8,8 +8,8 @@ struct PreProcessConfig {
     double downsampling_resolution{};
     int num_neighbors{};
 
-    PreProcessConfig() : min_range(5.0), max_range(100.0),
-        downsampling_resolution(1.5), num_neighbors(10) {}
+    PreProcessConfig()
+        : min_range(5.0), max_range(100.0), downsampling_resolution(1.5), num_neighbors(10) {}
 };
 
 }  // namespace openlidarmap::config

cpp/config/types.hpp

@@ -4,14 +4,8 @@
 #include <Eigen/Core>
 #include <Eigen/Geometry>
 
-// PCL
-#include <pcl/point_cloud.h>
-#include <pcl/point_types.h>
-
 namespace openlidarmap {
 
-using PointT = pcl::PointXYZ;
-using PointCloudT = pcl::PointCloud<PointT>;
 using Vector7d = Eigen::Matrix<double, 7, 1>;
 
 }  // namespace openlidarmap

cpp/core/pose_factor_abs.hpp

@@ -13,15 +13,15 @@ struct AbsPoseError {
     template <typename T>
     bool operator()(const T *const pose, T *residual) const {
         constexpr double STDDEV = 1.0;
-        
+
         for (int i = 0; i < 3; ++i) {
             residual[i] = (pose[i] - T(abs_measure_[i])) / T(STDDEV);
         }
 
         // Quaternion error
         Eigen::Quaternion<T> q1(pose[6], pose[3], pose[4], pose[5]);
-        Eigen::Quaternion<T> q2{T(abs_measure_[6]), T(abs_measure_[3]), 
-                                T(abs_measure_[4]), T(abs_measure_[5])};
+        Eigen::Quaternion<T> q2{T(abs_measure_[6]), T(abs_measure_[3]), T(abs_measure_[4]),
+                                T(abs_measure_[5])};
         Eigen::Quaternion<T> q_error = q1.normalized() * q2.normalized().inverse();
 
         // Convert quaternion error to residual

cpp/core/pose_factor_rel.hpp

@@ -14,22 +14,22 @@ struct RelPoseError {
     bool operator()(const T *const pose_i, const T *const pose_j, T *residual) const {
         constexpr double STDDEV = 1.0;
 
-        Eigen::Transform<T,3,Eigen::Isometry> T_i = Eigen::Transform<T,3,Eigen::Isometry>::Identity();
-        T_i.translation() = Eigen::Map<const Eigen::Matrix<T,3,1>>(pose_i);
+        Eigen::Transform<T, 3, Eigen::Isometry> T_i =
+            Eigen::Transform<T, 3, Eigen::Isometry>::Identity();
+        T_i.translation() = Eigen::Map<const Eigen::Matrix<T, 3, 1>>(pose_i);
         Eigen::Quaternion<T> q_i(pose_i[6], pose_i[3], pose_i[4], pose_i[5]);
         T_i.linear() = q_i.normalized().toRotationMatrix();
 
-        Eigen::Transform<T,3,Eigen::Isometry> T_j = Eigen::Transform<T,3,Eigen::Isometry>::Identity();
-        T_j.translation() = Eigen::Map<const Eigen::Matrix<T,3,1>>(pose_j);
+        Eigen::Transform<T, 3, Eigen::Isometry> T_j =
+            Eigen::Transform<T, 3, Eigen::Isometry>::Identity();
+        T_j.translation() = Eigen::Map<const Eigen::Matrix<T, 3, 1>>(pose_j);
         Eigen::Quaternion<T> q_j(pose_j[6], pose_j[3], pose_j[4], pose_j[5]);
         T_j.linear() = q_j.normalized().toRotationMatrix();
 
-        Eigen::Transform<T,3,Eigen::Isometry> T_target = Eigen::Transform<T,3,Eigen::Isometry>::Identity();
-        T_target.translation() = Eigen::Matrix<T,3,1>(
-            T(rel_measure_[0]),
-            T(rel_measure_[1]), 
-            T(rel_measure_[2])
-        );
+        Eigen::Transform<T, 3, Eigen::Isometry> T_target =
+            Eigen::Transform<T, 3, Eigen::Isometry>::Identity();
+        T_target.translation() =
+            Eigen::Matrix<T, 3, 1>(T(rel_measure_[0]), T(rel_measure_[1]), T(rel_measure_[2]));
 
         const T w = T(rel_measure_[6]);
         const T x = T(rel_measure_[3]);
@@ -40,17 +40,17 @@ struct RelPoseError {
 
         auto T_error = T_target.inverse() * T_j;
 
-        Eigen::Matrix<T,3,1> pos_error = T_error.translation();
-        
+        Eigen::Matrix<T, 3, 1> pos_error = T_error.translation();
+
         Eigen::Quaternion<T> q_error(T_error.linear());
         q_error.normalize();
 
         residual[0] = pos_error.x() / T(STDDEV);
         residual[1] = pos_error.y() / T(STDDEV);
         residual[2] = pos_error.z() / T(STDDEV);
-        
+
         residual[3] = T(2.0) * q_error.x() / T(STDDEV);
-        residual[4] = T(2.0) * q_error.y() / T(STDDEV); 
+        residual[4] = T(2.0) * q_error.y() / T(STDDEV);
         residual[5] = T(2.0) * q_error.z() / T(STDDEV);
 
         return true;

cpp/core/preprocess.cpp

@@ -1,16 +1,18 @@
 #include "core/preprocess.hpp"
 
-#include <small_gicp/util/normal_estimation_tbb.hpp>
 #include <small_gicp/util/downsampling_tbb.hpp>
+#include <small_gicp/util/normal_estimation_tbb.hpp>
 
 namespace openlidarmap {
 
-small_gicp::PointCloud::Ptr Preprocess::preprocess_cloud(const small_gicp::PointCloud::Ptr &cloud) const {
-    auto preprocessed_cloud = small_gicp::voxelgrid_sampling_tbb(*cloud, config_.preprocess_.downsampling_resolution);
+small_gicp::PointCloud::Ptr Preprocess::preprocess_cloud(
+    const small_gicp::PointCloud::Ptr &cloud) const {
+    auto preprocessed_cloud =
+        small_gicp::voxelgrid_sampling_tbb(*cloud, config_.preprocess_.downsampling_resolution);
 
     small_gicp::estimate_covariances_tbb(*preprocessed_cloud, config_.preprocess_.num_neighbors);
 
     return preprocessed_cloud;
 }
 
-}  // namespace openlidarmap
+}  // namespace openlidarmap

cpp/core/preprocess.hpp

@@ -1,18 +1,19 @@
 #pragma once
 
-#include "config/config.hpp"
 #include <small_gicp/points/point_cloud.hpp>
 
+#include "config/config.hpp"
+
 namespace openlidarmap {
 
 class Preprocess {
 public:
-    explicit Preprocess(const config::Config& config) : config_(config) {}
+    explicit Preprocess(const config::Config &config) : config_(config) {}
 
-    small_gicp::PointCloud::Ptr preprocess_cloud(const small_gicp::PointCloud::Ptr& cloud) const;
+    small_gicp::PointCloud::Ptr preprocess_cloud(const small_gicp::PointCloud::Ptr &cloud) const;
 
 private:
     config::Config config_;
 };
 
-} // namespace openlidarmap
+}  // namespace openlidarmap

cpp/core/registration.cpp

@@ -28,10 +28,10 @@ bool Registration::initialize(const small_gicp::PointCloud::Ptr &map_cloud,
 
     small_gicp::estimate_covariances_tbb(*map_cloud, config_.preprocess_.num_neighbors);
 
-    target_map_ =
-        std::make_shared<small_gicp::IncrementalVoxelMap<small_gicp::FlatContainerCov>>(
-            config_.registration_.voxel_resolution);
-    target_map_->voxel_setting.max_num_points_in_cell = config_.registration_.max_num_points_in_cell;
+    target_map_ = std::make_shared<small_gicp::IncrementalVoxelMap<small_gicp::FlatContainerCov>>(
+        config_.registration_.voxel_resolution);
+    target_map_->voxel_setting.max_num_points_in_cell =
+        config_.registration_.max_num_points_in_cell;
     target_map_->removal_horizon = config_.registration_.removal_horizon;
     target_map_->set_search_offsets(config_.registration_.search_offset);
     target_map_->distance_insert(*map_cloud, initial_guess);
@@ -41,13 +41,10 @@ bool Registration::initialize(const small_gicp::PointCloud::Ptr &map_cloud,
 
 small_gicp::RegistrationResult Registration::register_frame(
     const small_gicp::PointCloud::Ptr &frame, const Eigen::Isometry3d &initial_guess) {
-    auto result =
-        registration_.align(*target_map_, *frame, *target_map_, initial_guess);
+    auto result = registration_.align(*target_map_, *frame, *target_map_, initial_guess);
 
     return result;
 }
 
-void Registration::update_config(const config::Config &config) {
-    configure_registration(config);
-}
+void Registration::update_config(const config::Config &config) { configure_registration(config); }
 }  // namespace openlidarmap