Add a part of the new rigid registration method [FRICP]

registration/CMakeLists.txt

@@ -44,6 +44,7 @@ set(incs
   "include/pcl/${SUBSYS_NAME}/ndt.h"
   "include/pcl/${SUBSYS_NAME}/ndt_2d.h"
   "include/pcl/${SUBSYS_NAME}/ppf_registration.h"
+  "include/pcl/${SUBSYS_NAME}/anderson_acceleration.h"
 
   "include/pcl/${SUBSYS_NAME}/impl/pairwise_graph_registration.hpp"
 
@@ -60,10 +61,12 @@ set(incs
   "include/pcl/${SUBSYS_NAME}/transformation_estimation_point_to_plane_lls.h"
   "include/pcl/${SUBSYS_NAME}/transformation_estimation_point_to_plane_lls_weighted.h"
   "include/pcl/${SUBSYS_NAME}/transformation_estimation_symmetric_point_to_plane_lls.h"
+  "include/pcl/${SUBSYS_NAME}/transformation_estimation_point_to_point_robust.h"
   "include/pcl/${SUBSYS_NAME}/transformation_validation.h"
   "include/pcl/${SUBSYS_NAME}/transformation_validation_euclidean.h"
   "include/pcl/${SUBSYS_NAME}/gicp.h"
   "include/pcl/${SUBSYS_NAME}/gicp6d.h"
+  "include/pcl/${SUBSYS_NAME}/fricp.h"
   "include/pcl/${SUBSYS_NAME}/bfgs.h"
   "include/pcl/${SUBSYS_NAME}/warp_point_rigid.h"
   "include/pcl/${SUBSYS_NAME}/warp_point_rigid_6d.h"
@@ -110,8 +113,10 @@ set(impl_incs
   "include/pcl/${SUBSYS_NAME}/impl/transformation_estimation_point_to_plane_lls_weighted.hpp"
   "include/pcl/${SUBSYS_NAME}/impl/transformation_estimation_point_to_plane_weighted.hpp"
   "include/pcl/${SUBSYS_NAME}/impl/transformation_estimation_symmetric_point_to_plane_lls.hpp"
+  "include/pcl/${SUBSYS_NAME}/impl/transformation_estimation_point_to_point_robust.hpp"
   "include/pcl/${SUBSYS_NAME}/impl/transformation_validation_euclidean.hpp"
   "include/pcl/${SUBSYS_NAME}/impl/gicp.hpp"
+  "include/pcl/${SUBSYS_NAME}/impl/fricp.hpp"
   "include/pcl/${SUBSYS_NAME}/impl/sample_consensus_prerejective.hpp"
   "include/pcl/${SUBSYS_NAME}/impl/ia_fpcs.hpp"
   "include/pcl/${SUBSYS_NAME}/impl/ia_kfpcs.hpp"

registration/include/pcl/registration/anderson_acceleration.h

@@ -0,0 +1,186 @@
+#pragma once
+
+#include <pcl/exceptions.h>
+#include <pcl/memory.h>
+
+#include <Eigen/Core>
+#include <Eigen/QR>
+
+#include <algorithm>
+#include <cstddef>
+
+namespace pcl {
+namespace registration {
+
+/**
+ * \brief Lightweight Anderson acceleration helper used to speed up fixed-point
+ *        iterations in FRICP.
+ *
+ * The class stores a short history of the most recent residuals and solves the
+ * normal equations following the scheme described in "Fast and Robust Iterative
+ * Closest Point", Zhang et al., 2022.
+ *
+ * Only double precision is supported internally to maximise numerical stability.
+ */
+class AndersonAcceleration {
+public:
+  AndersonAcceleration() = default;
+
+  /**
+   * \brief Initialise the accelerator with a circular buffer of size \a history.
+   *
+   * \param[in] history    Number of previous iterates to keep (m in the paper).
+   * \param[in] dimension  Dimensionality of the flattened state vector.
+   * \param[in] initial_state Pointer to the initial state (expects
+   *                          \a dimension doubles).
+   */
+  inline void
+  init(std::size_t history, std::size_t dimension, const double* initial_state)
+  {
+    if ((history == 0) || (dimension == 0)) {
+      PCL_THROW_EXCEPTION(pcl::BadArgumentException,
+                          "AndersonAcceleration::init expects non-zero sizes");
+    }
+
+    history_length_ = history;
+    dimension_ = dimension;
+    iteration_ = 0;
+    column_index_ = 0;
+    initialized_ = true;
+
+    current_u_.resize(dimension_);
+    current_F_.resize(dimension_);
+    prev_dG_.setZero(dimension_, history_length_);
+    prev_dF_.setZero(dimension_, history_length_);
+    normal_eq_matrix_.setZero(history_length_, history_length_);
+    theta_.setZero(history_length_);
+    dF_scale_.setZero(history_length_);
+
+    current_u_ = Eigen::Map<const Eigen::VectorXd>(initial_state, dimension_);
+  }
+
+  inline bool
+  isInitialized() const
+  {
+    return initialized_;
+  }
+
+  inline std::size_t
+  history() const
+  {
+    return history_length_;
+  }
+
+  inline std::size_t
+  dimension() const
+  {
+    return dimension_;
+  }
+
+  inline void
+  replace(const double* state)
+  {
+    if (!initialized_)
+      return;
+    current_u_ = Eigen::Map<const Eigen::VectorXd>(state, dimension_);
+  }
+
+  inline void
+  reset(const double* state)
+  {
+    if (!initialized_)
+      return;
+    iteration_ = 0;
+    column_index_ = 0;
+    current_u_ = Eigen::Map<const Eigen::VectorXd>(state, dimension_);
+  }
+
+  /**
+   * \brief Apply one Anderson acceleration update.
+   *
+   * \param[in] next_state Flattened state obtained from the fixed-point iteration.
+   * \return Reference to the accelerated state vector.
+   */
+  inline const Eigen::VectorXd&
+  compute(const double* next_state)
+  {
+    if (!initialized_) {
+      PCL_THROW_EXCEPTION(pcl::PCLException,
+                          "AndersonAcceleration::compute called before init");
+    }
+
+    Eigen::Map<const Eigen::VectorXd> G(next_state, dimension_);
+    current_F_ = G - current_u_;
+
+    constexpr double eps = 1e-14;
+
+    if (iteration_ == 0) {
+      prev_dF_.col(0) = -current_F_;
+      prev_dG_.col(0) = -G;
+      current_u_ = G;
+    }
+    else {
+      prev_dF_.col(column_index_) += current_F_;
+      prev_dG_.col(column_index_) += G;
+
+      double scale = std::max(eps, prev_dF_.col(column_index_).norm());
+      dF_scale_(column_index_) = scale;
+      prev_dF_.col(column_index_) /= scale;
+
+      const std::size_t m_k = std::min(history_length_, iteration_);
+
+      if (m_k == 1) {
+        theta_(0) = 0.0;
+        const double dF_norm = prev_dF_.col(column_index_).norm();
+        normal_eq_matrix_(0, 0) = dF_norm * dF_norm;
+        if (dF_norm > eps) {
+          theta_(0) = (prev_dF_.col(column_index_) / dF_norm).dot(current_F_ / dF_norm);
+        }
+      }
+      else {
+        // update Gram matrix row/column corresponding to the newest column
+        const Eigen::RowVectorXd new_inner_prod =
+            prev_dF_.col(column_index_).transpose() *
+            prev_dF_.block(0, 0, dimension_, m_k);
+        normal_eq_matrix_.block(column_index_, 0, 1, m_k) = new_inner_prod;
+        normal_eq_matrix_.block(0, column_index_, m_k, 1) = new_inner_prod.transpose();
+
+        cod_.compute(normal_eq_matrix_.block(0, 0, m_k, m_k));
+        theta_.head(m_k) =
+            cod_.solve(prev_dF_.block(0, 0, dimension_, m_k).transpose() * current_F_);
+      }
+
+      const Eigen::ArrayXd scaled_theta =
+          theta_.head(m_k).array() / dF_scale_.head(m_k).array();
+      current_u_ = G - prev_dG_.block(0, 0, dimension_, m_k) * scaled_theta.matrix();
+
+      column_index_ = (column_index_ + 1) % history_length_;
+      prev_dF_.col(column_index_) = -current_F_;
+      prev_dG_.col(column_index_) = -G;
+    }
+
+    ++iteration_;
+    return current_u_;
+  }
+
+private:
+  std::size_t history_length_{0};
+  std::size_t dimension_{0};
+  std::size_t iteration_{0};
+  std::size_t column_index_{0};
+  bool initialized_{false};
+
+  Eigen::VectorXd current_u_;
+  Eigen::VectorXd current_F_;
+  Eigen::MatrixXd prev_dG_;
+  Eigen::MatrixXd prev_dF_;
+  Eigen::MatrixXd normal_eq_matrix_;
+  Eigen::VectorXd theta_;
+  Eigen::VectorXd dF_scale_;
+  Eigen::CompleteOrthogonalDecomposition<Eigen::MatrixXd> cod_;
+
+  PCL_MAKE_ALIGNED_OPERATOR_NEW
+};
+
+} // namespace registration
+} // namespace pcl

registration/include/pcl/registration/fricp.h

@@ -0,0 +1,171 @@
+/*
+ * SPDX-License-Identifier: BSD-3-Clause
+ *
+ *  Point Cloud Library (PCL) - www.pointclouds.org
+ *  Copyright (c) 2010-, Open Perception, Inc.
+ *
+ *  All rights reserved.
+ */
+
+#pragma once
+
+#include <pcl/registration/anderson_acceleration.h>
+#include <pcl/registration/icp.h>
+#include <pcl/search/kdtree.h>
+#include <pcl/search/search.h>
+#include <pcl/pcl_macros.h>
+#include <pcl/point_types.h>
+
+#include <Eigen/Core>
+
+#include <cmath>
+#include <vector>
+
+namespace pcl {
+/**
+ * \brief FastRobustIterativeClosestPoint implements the FRICP variant described in
+ *        "Fast and Robust Iterative Closest Point", Zhang et al., 2021.
+ *
+ * The solver relies on Welsch reweighting for robustness and optional Anderson
+ * acceleration for faster convergence.
+ *
+ * \code
+ * pcl::FastRobustIterativeClosestPoint<PointT, PointT> reg;
+ * reg.setInputSource (src); // src and tgt are clouds that must be created before
+ * reg.setInputTarget (tgt);
+ * reg.setMaximumIterations (60); // parameters may have to be tuned, depending on the point clouds
+ * reg.setTransformationEpsilon (1e-8);
+ * pcl::PointCloud<PointT> output;
+ * reg.align (output);
+ * \endcode
+ * \ingroup registration
+ */
+template <typename PointSource, typename PointTarget, typename Scalar = float>
+class FastRobustIterativeClosestPoint
+: public IterativeClosestPoint<PointSource, PointTarget, Scalar> {
+public:
+  using Base = IterativeClosestPoint<PointSource, PointTarget, Scalar>;
+  using typename Base::Matrix4;
+  using typename Base::PointCloudSource;
+  using typename Base::PointCloudTarget;
+
+  enum class RobustFunction { NONE, WELSCH };
+
+  FastRobustIterativeClosestPoint();
+
+  void
+  setRobustFunction(RobustFunction f);
+
+  [[nodiscard]] RobustFunction
+  getRobustFunction() const;
+
+  /** \brief Enable or disable Anderson acceleration in the FRICP optimization loop.
+   *
+   * When enabled, convergence can be faster on some datasets but may become less
+   * stable. The default is disabled to keep behavior predictable.
+   */
+  void
+  setUseAndersonAcceleration(bool enabled);
+
+  [[nodiscard]] bool
+  getUseAndersonAcceleration() const;
+
+  /** \brief Set the history size used by Anderson acceleration.
+   *
+   * Larger values may improve acceleration quality but can increase instability and
+   * memory usage. Values smaller than 1 are clamped to 1.
+   */
+  void
+  setAndersonHistorySize(std::size_t history);
+
+  [[nodiscard]] std::size_t
+  getAndersonHistorySize() const;
+
+  /** \brief Set the initial Welsch scale ratio used in dynamic robust weighting.
+   *
+   * Larger values start with weaker down-weighting of outliers. Values are clamped
+   * to a small positive threshold.
+   */
+  void
+  setDynamicWelschBeginRatio(double ratio);
+
+  /** \brief Set the final Welsch scale ratio used in dynamic robust weighting.
+   *
+   * Smaller values end with stronger outlier suppression. Values are clamped to a
+   * small positive threshold.
+   */
+  void
+  setDynamicWelschEndRatio(double ratio);
+
+  /** \brief Set the multiplicative decay applied to the dynamic Welsch scale.
+   *
+   * Valid range is [0, 1]. Smaller values reduce the scale faster per outer
+   * iteration, while larger values keep it closer to the current value.
+   */
+  void
+  setDynamicWelschDecay(double ratio);
+
+protected:
+  void
+  computeTransformation(PointCloudSource& output, const Matrix4& guess) override;
+
+private:
+  using Matrix4d = Eigen::Matrix<double, 4, 4>;
+  using Matrix3Xd = Eigen::Matrix<double, 3, Eigen::Dynamic>;
+  using VectorXd = Eigen::VectorXd;
+  using Vector3d = Eigen::Vector3d;
+  using AndersonAccelerationType = registration::AndersonAcceleration;
+
+  Matrix4d
+  convertGuessToCentered(const Matrix4& guess,
+                         const Vector3d& source_mean,
+                         const Vector3d& target_mean) const;
+
+  Matrix4d
+  convertCenteredToActual(const Matrix4d& transform,
+                          const Vector3d& source_mean,
+                          const Vector3d& target_mean) const;
+
+  bool
+  updateCorrespondences(const Matrix4d& transform,
+                        const Matrix3Xd& source,
+                        const Matrix3Xd& target,
+                        pcl::search::Search<pcl::PointXYZ>& tree,
+                        Matrix3Xd& matched_targets,
+                        VectorXd& residuals) const;
+
+  double
+  computeEnergy(const VectorXd& residuals, double nu) const;
+
+  VectorXd
+  computeWeights(const VectorXd& residuals, double nu) const;
+
+  Matrix4d
+  computeWeightedRigidTransform(const Matrix3Xd& source,
+                                const Matrix3Xd& target,
+                                const VectorXd& weights) const;
+
+  double
+  findKNearestMedian(const pcl::PointCloud<pcl::PointXYZ>& cloud,
+                     pcl::search::Search<pcl::PointXYZ>& tree,
+                     int neighbors) const;
+
+  Matrix4d
+  matrixLog(const Matrix4d& transform) const;
+
+  RobustFunction robust_function_;
+  bool use_anderson_ = false;
+  std::size_t anderson_history_ = 5;
+  double nu_begin_ratio_ = 3.0;
+  double nu_end_ratio_ = 1.0 / (3.0 * std::sqrt(3.0));
+  double nu_decay_ratio_ = 0.5;
+
+  static constexpr double same_threshold_ = 1e-6;
+
+  AndersonAccelerationType anderson_;
+
+  PCL_MAKE_ALIGNED_OPERATOR_NEW
+};
+} // namespace pcl
+
+#include <pcl/registration/impl/fricp.hpp>