Skip to content

Commit 4dd5652

Browse files
yaoyx689duanbotu123mvieth
authored
Add a part of the new rigid registration method [FRICP] (#6199)
* add a part of the FRICP (robust transformation estimation) * update * Correct format * add Anderson acceleration fricp.hpp etc.UPdate cmakelist * fix bug * fix bug * fix bug in test * fix the bug of parameter * Update test * repush * fix bug in clang-tidy,merge the new update in pcl * fix bug in clang-tidy,merge the new update in pcl * fix bug in clang-tidy again * fix bug in clang-tidy again * fix bug in clang-tidy again * fix bug in clang-tidy again * Address all review comments in PR 6199 * Fix FRICP KdTree/Search mismatch in test_registration_api * Update registration/include/pcl/registration/fricp.h Co-authored-by: Markus Vieth <39675748+mvieth@users.noreply.github.com> * Apply PR6199 review fixes without build artifacts * Update registration/include/pcl/registration/fricp.h Co-authored-by: Markus Vieth <39675748+mvieth@users.noreply.github.com> * Address review comments: use #pragma once, pcl::index_t, squared residuals, Eigen::Index * Fix clang-tidy: use range-based for loop in findKNearestMedian * Use pcl::uindex_t for source indices per review feedback * Address reviewer feedback: add Ptr/ConstPtr, NaN asserts, and class documentation - Add Ptr/ConstPtr type aliases to FRICP class header - Add assert for finite source/target points (no NaN/Inf) - Document that FRICP ignores maxCorrespondenceDistance, correspondence estimators/rejectors, and that maxIterations controls inner-loop iterations only - Fix setDynamicWelschDecay() doc to note 0 maps to 0.5 - Remove redundant ctor init-list values (robust_function_, nu_end_ratio_ already have in-class defaults) * Apply clang-format to fricp.h * Fix file encoding: convert fricp.h from UTF-16 LE to UTF-8 * Fix: add in-class default for robust_function_ after removing from init-list Previously removed robust_function_ from constructor init-list along with nu_end_ratio_, but robust_function_ had no in-class initializer, leading to default-initialization of an enum class (indeterminate value). Add explicit in-class default to WELSCH to preserve correct behavior. * Integrate DefaultConvergenceCriteria into FRICP - Add ConvergenceTrigger enum to track why FRICP stopped - Add getLastConvergenceTrigger() for diagnostic queries - Integrate DefaultConvergenceCriteria as an additional convergence check alongside the existing transform delta - Populate pcl::Correspondences in updateCorrespondences so getFitnessScore() works correctly - Set transformation_ delta each iteration for criteria checks - Track convergence state on all error/early-return paths Based on larshg's proposal in PR discussion. * Use DefaultConvergenceCriteria per larshg review - Remove ConvergenceTrigger enum and getLastConvergenceTrigger() (larshg's test code, not needed for production) - Replace FRICP's own matrix delta check with DefaultConvergenceCriteria - Move delta_transform calculation after Anderson acceleration step - Use CONVERGENCE_CRITERIA_NO_CORRESPONDENCES on error paths - Populate correspondences_ so getFitnessScore() works --------- Co-authored-by: Guanghao Li <1909631936@qq.com> Co-authored-by: Markus Vieth <39675748+mvieth@users.noreply.github.com> Co-authored-by: yaoyx689 <yaoyx689@users.noreply.github.com>
1 parent 7972cba commit 4dd5652

7 files changed

Lines changed: 1492 additions & 0 deletions

File tree

‎registration/CMakeLists.txt‎

Lines changed: 5 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -44,6 +44,7 @@ set(incs
4444
"include/pcl/${SUBSYS_NAME}/ndt.h"
4545
"include/pcl/${SUBSYS_NAME}/ndt_2d.h"
4646
"include/pcl/${SUBSYS_NAME}/ppf_registration.h"
47+
"include/pcl/${SUBSYS_NAME}/anderson_acceleration.h"
4748

4849
"include/pcl/${SUBSYS_NAME}/impl/pairwise_graph_registration.hpp"
4950

@@ -60,10 +61,12 @@ set(incs
6061
"include/pcl/${SUBSYS_NAME}/transformation_estimation_point_to_plane_lls.h"
6162
"include/pcl/${SUBSYS_NAME}/transformation_estimation_point_to_plane_lls_weighted.h"
6263
"include/pcl/${SUBSYS_NAME}/transformation_estimation_symmetric_point_to_plane_lls.h"
64+
"include/pcl/${SUBSYS_NAME}/transformation_estimation_point_to_point_robust.h"
6365
"include/pcl/${SUBSYS_NAME}/transformation_validation.h"
6466
"include/pcl/${SUBSYS_NAME}/transformation_validation_euclidean.h"
6567
"include/pcl/${SUBSYS_NAME}/gicp.h"
6668
"include/pcl/${SUBSYS_NAME}/gicp6d.h"
69+
"include/pcl/${SUBSYS_NAME}/fricp.h"
6770
"include/pcl/${SUBSYS_NAME}/bfgs.h"
6871
"include/pcl/${SUBSYS_NAME}/warp_point_rigid.h"
6972
"include/pcl/${SUBSYS_NAME}/warp_point_rigid_6d.h"
@@ -110,8 +113,10 @@ set(impl_incs
110113
"include/pcl/${SUBSYS_NAME}/impl/transformation_estimation_point_to_plane_lls_weighted.hpp"
111114
"include/pcl/${SUBSYS_NAME}/impl/transformation_estimation_point_to_plane_weighted.hpp"
112115
"include/pcl/${SUBSYS_NAME}/impl/transformation_estimation_symmetric_point_to_plane_lls.hpp"
116+
"include/pcl/${SUBSYS_NAME}/impl/transformation_estimation_point_to_point_robust.hpp"
113117
"include/pcl/${SUBSYS_NAME}/impl/transformation_validation_euclidean.hpp"
114118
"include/pcl/${SUBSYS_NAME}/impl/gicp.hpp"
119+
"include/pcl/${SUBSYS_NAME}/impl/fricp.hpp"
115120
"include/pcl/${SUBSYS_NAME}/impl/sample_consensus_prerejective.hpp"
116121
"include/pcl/${SUBSYS_NAME}/impl/ia_fpcs.hpp"
117122
"include/pcl/${SUBSYS_NAME}/impl/ia_kfpcs.hpp"

‎registration/include/pcl/registration/anderson_acceleration.h‎

Lines changed: 186 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,186 @@
1+
#pragma once
2+
3+
#include <pcl/exceptions.h>
4+
#include <pcl/memory.h>
5+
6+
#include <Eigen/Core>
7+
#include <Eigen/QR>
8+
9+
#include <algorithm>
10+
#include <cstddef>
11+
12+
namespace pcl {
13+
namespace registration {
14+
15+
/**
16+
* \brief Lightweight Anderson acceleration helper used to speed up fixed-point
17+
* iterations in FRICP.
18+
*
19+
* The class stores a short history of the most recent residuals and solves the
20+
* normal equations following the scheme described in "Fast and Robust Iterative
21+
* Closest Point", Zhang et al., 2022.
22+
*
23+
* Only double precision is supported internally to maximise numerical stability.
24+
*/
25+
class AndersonAcceleration {
26+
public:
27+
AndersonAcceleration() = default;
28+
29+
/**
30+
* \brief Initialise the accelerator with a circular buffer of size \a history.
31+
*
32+
* \param[in] history Number of previous iterates to keep (m in the paper).
33+
* \param[in] dimension Dimensionality of the flattened state vector.
34+
* \param[in] initial_state Pointer to the initial state (expects
35+
* \a dimension doubles).
36+
*/
37+
inline void
38+
init(std::size_t history, std::size_t dimension, const double* initial_state)
39+
{
40+
if ((history == 0) || (dimension == 0)) {
41+
PCL_THROW_EXCEPTION(pcl::BadArgumentException,
42+
"AndersonAcceleration::init expects non-zero sizes");
43+
}
44+
45+
history_length_ = history;
46+
dimension_ = dimension;
47+
iteration_ = 0;
48+
column_index_ = 0;
49+
initialized_ = true;
50+
51+
current_u_.resize(dimension_);
52+
current_F_.resize(dimension_);
53+
prev_dG_.setZero(dimension_, history_length_);
54+
prev_dF_.setZero(dimension_, history_length_);
55+
normal_eq_matrix_.setZero(history_length_, history_length_);
56+
theta_.setZero(history_length_);
57+
dF_scale_.setZero(history_length_);
58+
59+
current_u_ = Eigen::Map<const Eigen::VectorXd>(initial_state, dimension_);
60+
}
61+
62+
inline bool
63+
isInitialized() const
64+
{
65+
return initialized_;
66+
}
67+
68+
inline std::size_t
69+
history() const
70+
{
71+
return history_length_;
72+
}
73+
74+
inline std::size_t
75+
dimension() const
76+
{
77+
return dimension_;
78+
}
79+
80+
inline void
81+
replace(const double* state)
82+
{
83+
if (!initialized_)
84+
return;
85+
current_u_ = Eigen::Map<const Eigen::VectorXd>(state, dimension_);
86+
}
87+
88+
inline void
89+
reset(const double* state)
90+
{
91+
if (!initialized_)
92+
return;
93+
iteration_ = 0;
94+
column_index_ = 0;
95+
current_u_ = Eigen::Map<const Eigen::VectorXd>(state, dimension_);
96+
}
97+
98+
/**
99+
* \brief Apply one Anderson acceleration update.
100+
*
101+
* \param[in] next_state Flattened state obtained from the fixed-point iteration.
102+
* \return Reference to the accelerated state vector.
103+
*/
104+
inline const Eigen::VectorXd&
105+
compute(const double* next_state)
106+
{
107+
if (!initialized_) {
108+
PCL_THROW_EXCEPTION(pcl::PCLException,
109+
"AndersonAcceleration::compute called before init");
110+
}
111+
112+
Eigen::Map<const Eigen::VectorXd> G(next_state, dimension_);
113+
current_F_ = G - current_u_;
114+
115+
constexpr double eps = 1e-14;
116+
117+
if (iteration_ == 0) {
118+
prev_dF_.col(0) = -current_F_;
119+
prev_dG_.col(0) = -G;
120+
current_u_ = G;
121+
}
122+
else {
123+
prev_dF_.col(column_index_) += current_F_;
124+
prev_dG_.col(column_index_) += G;
125+
126+
double scale = std::max(eps, prev_dF_.col(column_index_).norm());
127+
dF_scale_(column_index_) = scale;
128+
prev_dF_.col(column_index_) /= scale;
129+
130+
const std::size_t m_k = std::min(history_length_, iteration_);
131+
132+
if (m_k == 1) {
133+
theta_(0) = 0.0;
134+
const double dF_norm = prev_dF_.col(column_index_).norm();
135+
normal_eq_matrix_(0, 0) = dF_norm * dF_norm;
136+
if (dF_norm > eps) {
137+
theta_(0) = (prev_dF_.col(column_index_) / dF_norm).dot(current_F_ / dF_norm);
138+
}
139+
}
140+
else {
141+
// update Gram matrix row/column corresponding to the newest column
142+
const Eigen::RowVectorXd new_inner_prod =
143+
prev_dF_.col(column_index_).transpose() *
144+
prev_dF_.block(0, 0, dimension_, m_k);
145+
normal_eq_matrix_.block(column_index_, 0, 1, m_k) = new_inner_prod;
146+
normal_eq_matrix_.block(0, column_index_, m_k, 1) = new_inner_prod.transpose();
147+
148+
cod_.compute(normal_eq_matrix_.block(0, 0, m_k, m_k));
149+
theta_.head(m_k) =
150+
cod_.solve(prev_dF_.block(0, 0, dimension_, m_k).transpose() * current_F_);
151+
}
152+
153+
const Eigen::ArrayXd scaled_theta =
154+
theta_.head(m_k).array() / dF_scale_.head(m_k).array();
155+
current_u_ = G - prev_dG_.block(0, 0, dimension_, m_k) * scaled_theta.matrix();
156+
157+
column_index_ = (column_index_ + 1) % history_length_;
158+
prev_dF_.col(column_index_) = -current_F_;
159+
prev_dG_.col(column_index_) = -G;
160+
}
161+
162+
++iteration_;
163+
return current_u_;
164+
}
165+
166+
private:
167+
std::size_t history_length_{0};
168+
std::size_t dimension_{0};
169+
std::size_t iteration_{0};
170+
std::size_t column_index_{0};
171+
bool initialized_{false};
172+
173+
Eigen::VectorXd current_u_;
174+
Eigen::VectorXd current_F_;
175+
Eigen::MatrixXd prev_dG_;
176+
Eigen::MatrixXd prev_dF_;
177+
Eigen::MatrixXd normal_eq_matrix_;
178+
Eigen::VectorXd theta_;
179+
Eigen::VectorXd dF_scale_;
180+
Eigen::CompleteOrthogonalDecomposition<Eigen::MatrixXd> cod_;
181+
182+
PCL_MAKE_ALIGNED_OPERATOR_NEW
183+
};
184+
185+
} // namespace registration
186+
} // namespace pcl

0 commit comments

Comments
 (0)