Add param to force planarity

config/advanced.yaml

@@ -21,3 +21,4 @@ registration:
   max_num_iterations: 500 # <- optional
   convergence_criterion: 0.0001 # <- optional
   max_num_threads: 0 # <- optional, 0 means automatic
+  planar: false # if true, 2D registration is forced (z, roll, and pitch are zeroed)

cpp/kiss_icp/core/Registration.cpp

@@ -88,13 +88,23 @@ Correspondences DataAssociation(const std::vector<Eigen::Vector3d> &points,
     return correspondences;
 }
 
-LinearSystem BuildLinearSystem(const Correspondences &correspondences, const double kernel_scale) {
-    auto compute_jacobian_and_residual = [](const auto &correspondence) {
+LinearSystem BuildLinearSystem(const Correspondences &correspondences,
+                               const double kernel_scale,
+                               const bool planar) {
+    auto compute_jacobian_and_residual = [planar](const auto &correspondence) {
         const auto &[source, target] = correspondence;
-        const Eigen::Vector3d residual = source - target;
+        Eigen::Vector3d residual = source - target;
+        if (planar) {
+            residual.z() = 0;
+        }
         Eigen::Matrix3_6d J_r;
         J_r.block<3, 3>(0, 0) = Eigen::Matrix3d::Identity();
         J_r.block<3, 3>(0, 3) = -1.0 * Sophus::SO3d::hat(source);
+        if (planar) {
+            J_r.row(2).setZero();  // z
+            J_r.col(3).setZero();  // roll
+            J_r.col(4).setZero();  // pitch
+        }
         return std::make_tuple(J_r, residual);
     };
 
@@ -134,12 +144,16 @@ LinearSystem BuildLinearSystem(const Correspondences &correspondences, const dou
 
 namespace kiss_icp {
 
-Registration::Registration(int max_num_iteration, double convergence_criterion, int max_num_threads)
+Registration::Registration(int max_num_iteration,
+                           double convergence_criterion,
+                           int max_num_threads,
+                           bool planar)
     : max_num_iterations_(max_num_iteration),
       convergence_criterion_(convergence_criterion),
       // Only manipulate the number of threads if the user specifies something greater than 0
       max_num_threads_(max_num_threads > 0 ? max_num_threads
-                                           : tbb::this_task_arena::max_concurrency()) {
+                                           : tbb::this_task_arena::max_concurrency()),
+      planar_(planar) {
     // This global variable requires static duration storage to be able to manipulate the max
     // concurrency from TBB across the entire class
     static const auto tbb_control_settings = tbb::global_control(
@@ -163,8 +177,13 @@ Sophus::SE3d Registration::AlignPointsToMap(const std::vector<Eigen::Vector3d> &
         // Equation (10)
         const auto correspondences = DataAssociation(source, voxel_map, max_distance);
         // Equation (11)
-        const auto &[JTJ, JTr] = BuildLinearSystem(correspondences, kernel_scale);
-        const Eigen::Vector6d dx = JTJ.ldlt().solve(-JTr);
+        const auto &[JTJ, JTr] = BuildLinearSystem(correspondences, kernel_scale, planar_);
+        Eigen::Vector6d dx = JTJ.ldlt().solve(-JTr);
+        if (planar_) {
+            dx[2] = 0.0;  // z
+            dx[3] = 0.0;  // roll
+            dx[4] = 0.0;  // pitch
+        }
         const Sophus::SE3d estimation = Sophus::SE3d::exp(dx);
         // Equation (12)
         TransformPoints(estimation, source);
@@ -174,7 +193,17 @@ Sophus::SE3d Registration::AlignPointsToMap(const std::vector<Eigen::Vector3d> &
         if (dx.norm() < convergence_criterion_) break;
     }
     // Spit the final transformation
-    return T_icp * initial_guess;
+    auto final_transformation = T_icp * initial_guess;
+    if (planar_) {
+        Eigen::Matrix3d R = final_transformation.rotationMatrix();
+        double yaw = std::atan2(R(1, 0), R(0, 0));
+        Eigen::Matrix3d R_2D = Eigen::Matrix3d::Identity();
+        R_2D << std::cos(yaw), -std::sin(yaw), 0, std::sin(yaw), std::cos(yaw), 0, 0, 0, 1;
+        Eigen::Vector3d t = final_transformation.translation();
+        t.z() = 0.0;
+        final_transformation = Sophus::SE3d(R_2D, t);
+    }
+    return final_transformation;
 }
 
 }  // namespace kiss_icp

cpp/kiss_icp/core/Registration.hpp

@@ -31,7 +31,10 @@
 namespace kiss_icp {
 
 struct Registration {
-    explicit Registration(int max_num_iteration, double convergence_criterion, int max_num_threads);
+    explicit Registration(int max_num_iteration,
+                          double convergence_criterion,
+                          int max_num_threads,
+                          bool planar);
 
     Sophus::SE3d AlignPointsToMap(const std::vector<Eigen::Vector3d> &frame,
                                   const VoxelHashMap &voxel_map,
@@ -42,5 +45,6 @@ struct Registration {
     int max_num_iterations_;
     double convergence_criterion_;
     int max_num_threads_;
+    bool planar_;
 };
 }  // namespace kiss_icp

cpp/kiss_icp/pipeline/KissICP.hpp

@@ -50,6 +50,9 @@ struct KISSConfig {
 
     // Motion compensation
     bool deskew = false;
+
+    // 2D Mode
+    bool planar = false;
 };
 
 class KissICP {
@@ -60,8 +63,10 @@ class KissICP {
 public:
     explicit KissICP(const KISSConfig &config)
         : config_(config),
-          registration_(
-              config.max_num_iterations, config.convergence_criterion, config.max_num_threads),
+          registration_(config.max_num_iterations,
+                        config.convergence_criterion,
+                        config.max_num_threads,
+                        config.planar),
           local_map_(config.voxel_size, config.max_range, config.max_points_per_voxel),
           adaptive_threshold_(config.initial_threshold, config.min_motion_th, config.max_range) {}
 

python/kiss_icp/config/config.py

@@ -40,6 +40,7 @@ class RegistrationConfig(BaseModel):
     max_num_iterations: Optional[int] = 500
     convergence_criterion: Optional[float] = 0.0001
     max_num_threads: Optional[int] = 0  # 0 means automatic
+    planar: Optional[bool] = False
 
 
 class AdaptiveThresholdConfig(BaseModel):

python/kiss_icp/pybind/kiss_icp_pybind.cpp

@@ -77,8 +77,8 @@ PYBIND11_MODULE(kiss_icp_pybind, m) {
     // Point Cloud registration
     py::class_<Registration> internal_registration(m, "_Registration", "Don't use this");
     internal_registration
-        .def(py::init<int, double, int>(), "max_num_iterations"_a, "convergence_criterion"_a,
-             "max_num_threads"_a)
+        .def(py::init<int, double, int, bool>(), "max_num_iterations"_a, "convergence_criterion"_a,
+             "max_num_threads"_a, "planar"_a)
         .def(
             "_align_points_to_map",
             [](Registration &self, const std::vector<Eigen::Vector3d> &points,

python/kiss_icp/registration.py

@@ -41,6 +41,7 @@ def __init__(
         max_num_iterations: int,
         convergence_criterion: float,
         max_num_threads: int = 0,
+        planar: bool = False,
     ):
         self._registration = kiss_icp_pybind._Registration(
             max_num_iterations=max_num_iterations,