Merge pull request #6 from TUMFTM/3-switch-to-gicp

Restructe with major changes to the core library

README.md

@@ -17,7 +17,7 @@
 
 ## Concept
 
-We use reference maps in combination with classical LiDAR odometry to enable drift-free localization/mapping. Our approach was developed for high-precision mapping. It enables georeferenced LiDAR-only point cloud mapping without GNSS. A detailed description of our pipeline can be found in the linked paper.  
+We use reference maps in combination with classical LiDAR odometry to enable drift-free localization/mapping. Our approach is designed for high precision mapping. It enables georeferenced LiDAR-only point cloud mapping without GNSS. A detailed description of our pipeline can be found in the linked paper.
 
 <img src=doc/pipeline_diagram.png alt="diagram" width="480" />
 
@@ -26,7 +26,7 @@ We use reference maps in combination with classical LiDAR odometry to enable dri
 <details>
 <summary>Install</summary>
 
-We provide a Docker image on Docker Hub, which will automatically pulled within the Run section, but you also have the option to build is locally.  
+We provide a Docker image on Docker Hub, which will automatically be pulled within the Run section, but you also have the option to build it locally.  
 ```sh
 ./docker/build_docker.sh # (optional)
 ```
@@ -35,7 +35,7 @@ We provide a Docker image on Docker Hub, which will automatically pulled within
 <details>
 <summary>Run</summary>
 
-To use our approach, you need a reference map and an initial guess for the first pose.
+To use our approach, you need a reference map and an initial guess of the first pose.
 
 The easiest way to use our approach is with the provided Docker image.
 ```sh
@@ -49,6 +49,21 @@ The output of the algorithm are poses in the KITTI format.
 
 We also provide Python bindings. Have a look in the `python` folder, where we provide a test script.
 
+</details>
+<details>
+<summary>Configure</summary>
+
+The configuration of this pipeline can be changed in the `cpp/config` files. The naming suggest the intended usecase for the files. The most important parameters to play with if your results are not as good as expected are:
+
+| Parameter | Description | Default | Note |
+| :-------- | :-------- | :--------: | :-------- |
+| pipeline_.visualize | Toggle GUI | `true` | use `false` on headless servers |
+| preprocess_.downsampling_resolution | Scans are voxelized before usage | `1.5` | Reduce the size for increased robustness |
+| preprocess_.num_neighbors | Points for covariance calculation | `10` | Try both directions |
+| registration_.voxel_resolution | Voxelhashmap voxel size | `1.0` | Reduce the size for increased robustness | 
+| registration_.lambda | Optimization dampening factor | `1.0` | Increase to increase the robustness |
+
+
 </details>
 <details>
 <summary>Develop</summary>
@@ -82,7 +97,7 @@ pip install -e .
 
 ## Acknowledgement
 
-Great inspiration was taken from the following repositories. If you are using our work, please also leave a star at their repositories and cite their work.
+Great inspiration has come from the following repositories. If you use our work, please also leave a star in their repositories and cite their work.
 
 * [KISS-ICP](https://github.com/PRBonn/kiss-icp)
 * [small_gicp](https://github.com/koide3/small_gicp)

cpp/CMakeLists.txt

@@ -28,6 +28,7 @@ add_library(
   core/registration.cpp
   core/pose_graph.cpp
   core/prediction.cpp
+  core/preprocess.cpp
   io/kitti_loader.cpp
   io/map_loader.cpp
   pipeline/openlidarmap.cpp

cpp/config/config.hpp

@@ -3,6 +3,7 @@
 #include <iostream>
 
 #include "config/ceres_config.hpp"
+#include "config/kernel_config.hpp"
 #include "config/pipeline_config.hpp"
 #include "config/preprocess_config.hpp"
 #include "config/registration_config.hpp"
@@ -11,6 +12,7 @@ namespace openlidarmap::config {
 
 struct Config {
     CeresConfig ceres_{};
+    KernelConfig kernel_{};
     RegistrationConfig registration_{};
     PreProcessConfig preprocess_{};
     PipelineConfig pipeline_{};

cpp/config/kernel_config.hpp

@@ -0,0 +1,18 @@
+#pragma once
+
+#include <cstddef>
+
+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/pipeline_config.hpp

@@ -9,12 +9,14 @@ struct PipelineConfig {
     double rotation_threshold{};
     size_t sliding_window_size{};
     bool use_sliding_window{};
+    bool visualize{};
 
     PipelineConfig()
-        : translation_threshold(0.1),
-          rotation_threshold(0.1),
+        : translation_threshold(0.05),
+          rotation_threshold(0.05),
           sliding_window_size(250),
-          use_sliding_window(true) {}
+          use_sliding_window(true),
+          visualize(true) {}
 };
 
 }  // namespace openlidarmap::config

cpp/config/preprocess_config.hpp

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

cpp/config/registration_config.hpp

@@ -7,6 +7,7 @@ namespace openlidarmap::config {
 struct RegistrationConfig {
     double voxel_resolution{};
     double max_correspondence_distance{};
+    double max_dist_sq{};
     double rotation_eps{};
     double translation_eps{};
     double lambda{};
@@ -20,15 +21,16 @@ struct RegistrationConfig {
     RegistrationConfig()
         : voxel_resolution(1.0),
           max_correspondence_distance(6.0),
+          max_dist_sq(max_correspondence_distance * max_correspondence_distance),
           rotation_eps(0.1 * M_PI / 180.0),
           translation_eps(1e-3),
           lambda(1.0),
           max_iterations(1000),
           verbose(false),
-          map_overlap(0),
-          removal_horizon(1e9),
+          map_overlap(50),
+          removal_horizon(100),
           search_offset(27),
-          max_num_points_in_cell{100} {}
+          max_num_points_in_cell(20) {}
 };
 
 }  // namespace openlidarmap::config

cpp/core/pose_factor_abs.hpp

@@ -12,21 +12,22 @@ struct AbsPoseError {
 
     template <typename T>
     bool operator()(const T *const pose, T *residual) const {
-        // Position error
+        constexpr double STDDEV = 1.0;
+
         for (int i = 0; i < 3; ++i) {
-            residual[i] = pose[i] - T(abs_measure_[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> q_error = q1.normalized() * q2.normalized().conjugate();
+        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
-        residual[3] = T(2.0) * q_error.x();
-        residual[4] = T(2.0) * q_error.y();
-        residual[5] = T(2.0) * q_error.z();
+        residual[3] = T(2.0) * q_error.x() / 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/pose_factor_rel.hpp

@@ -12,29 +12,46 @@ struct RelPoseError {
 
     template <typename T>
     bool operator()(const T *const pose_i, const T *const pose_j, T *residual) const {
-        Eigen::Map<const Eigen::Matrix<T, 3, 1>> pos_i(pose_i);
-        Eigen::Map<const Eigen::Matrix<T, 3, 1>> pos_j(pose_j);
+        constexpr double STDDEV = 1.0;
 
-        // Extract quaternions (x,y,z,w order) to Eigen (w,x,y,z order)
+        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]);
-        Eigen::Quaternion<T> q_j(pose_j[6], pose_j[3], pose_j[4], pose_j[5]);
-
-        // Convert relative measurement
-        Eigen::Matrix<T, 3, 1> rel_pos{T(rel_measure_[0]), T(rel_measure_[1]), T(rel_measure_[2])};
-        Eigen::Quaternion<T> rel_q{T(rel_measure_[6]), T(rel_measure_[3]), T(rel_measure_[4]),
-                                   T(rel_measure_[5])};
-
-        // Position error
-        residual[0] = pos_j.x() - rel_pos.x();
-        residual[1] = pos_j.y() - rel_pos.y();
-        residual[2] = pos_j.z() - rel_pos.z();
+        T_i.linear() = q_i.normalized().toRotationMatrix();
 
-        // Quaternion error
-        Eigen::Quaternion<T> q_error = q_j.normalized() * rel_q.normalized().conjugate();
-
-        residual[3] = T(2.0) * q_error.x();
-        residual[4] = T(2.0) * q_error.y();
-        residual[5] = T(2.0) * q_error.z();
+        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])
+        );
+
+        const T w = T(rel_measure_[6]);
+        const T x = T(rel_measure_[3]);
+        const T y = T(rel_measure_[4]);
+        const T z = T(rel_measure_[5]);
+        Eigen::Quaternion<T> q_target(w, x, y, z);
+        T_target.linear() = q_target.normalized().toRotationMatrix();
+
+        auto T_error = T_target.inverse() * T_j;
+
+        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[5] = T(2.0) * q_error.z() / T(STDDEV);
 
         return true;
     }

cpp/core/pose_graph.cpp

@@ -25,12 +25,13 @@ void PoseGraph::addConstraint(size_t idx_from,
             new ceres::AutoDiffCostFunction<AbsPoseError, 6, 7>(new AbsPoseError(measurement)),
             new ceres::TukeyLoss(1.0), poses_[idx_to].data());
         abs_residual_blocks_.emplace_back(abs_block_id);
+        problem_.SetManifold(poses_[idx_to].data(), pose_manifold_);
     } else {
         ceres::ResidualBlockId rel_block_id = problem_.AddResidualBlock(
             new ceres::AutoDiffCostFunction<RelPoseError, 6, 7, 7>(new RelPoseError(measurement)),
             new ceres::CauchyLoss(1.0), poses_[idx_from].data(), poses_[idx_to].data());
         rel_residual_blocks_.emplace_back(rel_block_id);
-
+        problem_.SetManifold(poses_[idx_from].data(), pose_manifold_);
         problem_.SetManifold(poses_[idx_to].data(), pose_manifold_);
     }
 

cpp/core/pose_graph.hpp

@@ -21,11 +21,11 @@ class PoseGraph {
 
     ceres::Problem problem_{};
     ceres::Solver::Options solver_options_{};
+    const config::Config &config_;
     std::deque<Vector7d> &poses_;
     std::deque<ceres::ResidualBlockId> abs_residual_blocks_{};
     std::deque<ceres::ResidualBlockId> rel_residual_blocks_{};
     std::deque<size_t> window_indices_{};
-    const config::Config &config_;
     ceres::Manifold *pose_manifold_{};
     ceres::Solver::Summary summary_{};
 };

cpp/core/prediction.cpp

@@ -4,7 +4,7 @@ namespace openlidarmap {
 
 Vector7d ConstantDistancePredictor::predict(const Vector7d &current_pose,
                                             const Vector7d &previous_pose) {
-    Vector7d predicted_pose;
+    Vector7d predicted_pose{};
 
     // Predict translation
     predicted_pose.head<3>() = predictTranslation(current_pose, previous_pose);

cpp/core/preprocess.cpp

@@ -0,0 +1,16 @@
+#include "core/preprocess.hpp"
+
+#include <small_gicp/util/normal_estimation_tbb.hpp>
+#include <small_gicp/util/downsampling_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::estimate_covariances_tbb(*preprocessed_cloud, config_.preprocess_.num_neighbors);
+
+    return preprocessed_cloud;
+}
+
+}  // namespace openlidarmap

cpp/core/preprocess.hpp

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

cpp/core/registration.cpp

@@ -2,16 +2,17 @@
 
 #include <memory>
 #include <small_gicp/util/downsampling_tbb.hpp>
+#include <small_gicp/util/normal_estimation_tbb.hpp>
 
 namespace openlidarmap {
 
 Registration::Registration(const config::Config &config) : config_(config) {
-    configure_registration();
+    configure_registration(config_);
 }
 
-void Registration::configure_registration() {
-    registration_.rejector.max_dist_sq = config_.registration_.max_correspondence_distance *
-                                         config_.registration_.max_correspondence_distance;
+void Registration::configure_registration(const config::Config &config_) {
+    registration_.rejector.max_dist_sq = config_.registration_.max_dist_sq;
+    registration_.point_factor.robust_kernel.c = config_.kernel_.sigma;
     registration_.criteria.rotation_eps = config_.registration_.rotation_eps;
     registration_.criteria.translation_eps = config_.registration_.translation_eps;
     registration_.optimizer.max_iterations = config_.registration_.max_iterations;
@@ -25,28 +26,28 @@ bool Registration::initialize(const small_gicp::PointCloud::Ptr &map_cloud,
         return false;
     }
 
+    small_gicp::estimate_covariances_tbb(*map_cloud, config_.preprocess_.num_neighbors);
+
     target_map_ =
-        std::make_shared<small_gicp::IncrementalVoxelMap<small_gicp::FlatContainerPoints>>(
+        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);
 
     return true;
 }
 
-small_gicp::PointCloud::Ptr Registration::preprocess_cloud(
-    const small_gicp::PointCloud::Ptr &cloud) const {
-    return small_gicp::voxelgrid_sampling_tbb(*cloud, config_.preprocess_.downsampling_resolution);
-}
-
 small_gicp::RegistrationResult Registration::register_frame(
     const small_gicp::PointCloud::Ptr &frame, const Eigen::Isometry3d &initial_guess) {
-    auto preprocessed_frame = preprocess_cloud(frame);
     auto result =
-        registration_.align(*target_map_, *preprocessed_frame, *target_map_, initial_guess);
+        registration_.align(*target_map_, *frame, *target_map_, initial_guess);
 
     return result;
 }
 
+void Registration::update_config(const config::Config &config) {
+    configure_registration(config);
+}
 }  // namespace openlidarmap