pcl库中GICP算法是通过gicp.h、gicp.hpp两个文件实现的，gicp.h为声明文件，gicp.hpp是函数的具体实现文件，pcl库中实现的GICP算法是基于BFGS(拟牛顿法)优化方法。本文将解析源代码的逻辑实现，GICP算法本身的原理请自行阅读相关文献。
由于算法实现的源代码继承于ICP算法，所以阅读本部分前请先阅读ICP部分的源代码解读。PCL库源代码阅读—配准模块(Registration)—ICP算法
文中有任何错误或疑惑的地方可在评论区留言，看到均会回复！！！
gicp.h文件
gicp算法的实现类GeneralizedIterativeClosestPoint继承于经典ICP算法的实现类IterativeClosestPoint，是ICP算法的变体，其具体实现的代码基于IterativeClosestPoint类的基础上。
class GeneralizedIterativeClosestPoint
: public IterativeClosestPoint<PointSource, PointTarget, Scalar> {
public:
using IterativeClosestPoint<PointSource, PointTarget, Scalar>::reg_name_;
...
}下面对gicp.h文件中主要变量和函数进行了解释说明，其中构造函数实现了关键变量的初始化，并通过函数对象实现了基于BFGS优化算法的变换矩阵估计。
本部分为了快速阅读，简化了函数的书写方式，不常用的函数本部分将略去。
public:
using 各种变量，方便本类直接使用
GeneralizedIterativeClosestPoint()：… 构造函数及初始化列表
{ rigid_transformation_estimation_ = [this](,,,,)
{estimateRigidTransformationBFGS(,,,,);}; 实现了基于BFGS优化算法的变换矩阵估计
}
setInputSource(& ) override; 设置源点云
setInputTarget(& ) override; 设置目标点云
void estimateRigidTransformationBFGS(,,,,); 计算变换矩阵(BFGS法)
computeRDerivative(,,); 计算目标函数相对于旋转角度的梯度值
void setRotationEpsilon(); 设置旋转变化的阈值
void setCorrespondenceRandomness(); 设置计算协方差阵的最近邻点数
void setMaximumOptimizerIterations(); 设置优化算法的最大迭代次数，default:20
void setTranslationGradientTolerance(); 设置优化终止的最小平移梯度阈值
void setRotationGradientTolerance(); 设置优化终止的最小旋转梯度阈值
protected:
int k_correspondences_; 计算协方差阵的邻近点数量，default:20;
double gicp_epsilon_; 定义论文中计算协方差阵所用到的常数epsilon
double rotation_epsilon_; 定义旋转误差
MatricesVectorPtr input_covariances_; 定义源点云的协方差阵
MatricesVectorPtr target_covariances_; 定义目标点云的协方差阵
std::vector<Eigen::Matrix3d> mahalanobis_; 定义马氏距离的存储矩阵
int max_inner_iterations_; 定义优化算法的最大迭代次数
double translation_gradient_tolerance_; 定义优化算法终止时的最小平移梯度阈值
double rotation_gradient_tolerance_; 定义优化算法终止时的最小旋转梯度阈值
void computeCovariances(,,); 计算点云的协方差阵
通过vector的迭代器遍历点云，为点搜索K邻域，计算协方差阵，将协方差阵SVD分解重构，得到最终的协方差阵
double matricesInnerProd(,); 计算两个动态矩阵相乘的迹
void computeTransformation(,) override; 计算变换矩阵，输出变换后的点云
计算源点云、目标点云的协方差阵，通过while迭代求解T，每次迭代都要进行for循环
for()循环：将源点云中的每个点的坐标在当前T下变换，然后搜索最近邻点计算出(RC_i^AR^T+C_i^B)-1的和，存储配对点对的索引
cnt:配对点对的个数
bool searchForNeighbors(,,); 为一个点搜索最近邻点
void applyState(,); 将欧拉角转为旋转矩阵，并返回该旋转矩阵和另一旋转矩阵参数的积，实质为两参数相乘，更新旋转矩阵
std::function<void(,,,,)> rigid_transformation_estimation_;定义了一个函数对象，实现了变换矩阵的估计
这里单独介绍一下优化函数的结构体 OptimizationFunctorWithIndices，operator()重载了调用操作符（），即gicp_(gicp)＝gicp_.operator(gicp)；
BFGSDummyFunctor<double, 6>()初始化时确定了父结构体的x的类型是Vector6d
存疑：(gicp)与(Vector6d)两个参数是如何转换的？？？
struct OptimizationFunctorWithIndices : public BFGSDummyFunctor<double, 6> {
OptimizationFunctorWithIndices(const GeneralizedIterativeClosestPoint* gicp)
: BFGSDummyFunctor<double, 6>(), gicp_(gicp)
{}
double
operator()(const Vector6d& x) override;
void 求解x处的梯度值df
df(const Vector6d& x, Vector6d& df) override;
void 求解x处的函数值f和梯度值df
fdf(const Vector6d& x, double& f, Vector6d& df) override;
BFGSSpace::Status 检查梯度状态
checkGradient(const Vector6d& g) override;
const GeneralizedIterativeClosestPoint* gicp_;
};gicp.hpp文件
computeTransformation()；该函数是GICP算法实现的关键部分，实现了变换矩阵的计算。guess是给定变换矩阵T的初值，默认为单位矩阵，默认值的由来可在registration.h文件中的align()配准函数中得出。output的初值来自于align()配准函数，初值为源点云。
template <typename PointSource, typename PointTarget, typename Scalar>
inline void
GeneralizedIterativeClosestPoint<PointSource, PointTarget, Scalar>::
computeTransformation(PointCloudSource& output, const Matrix4& guess)
{}下面具体介绍computeTransformation()函数内部的代码实现过程：
首先是迭代求解变换矩阵前的变量初始化过程，最终用来迭代求解的output(代表源点云)是经过初始位姿guess变换后的。求解源点云、目标点云的协方差阵只需要求解一次，因为点云的旋转平移变换不会影响内部的点云相对结构关系。
// 调用了父类中的初始化
pcl::IterativeClosestPoint<PointSource, PointTarget, Scalar>::initComputeReciprocal();
double delta = 0; 定义了前后两次变换矩阵的差值
const std::size_t N = indices_->size(); 变量N存储源点云的数量
mahalanobis_.resize(N, Eigen::Matrix3d::Identity()); 将马氏距离的存储矩阵的大小设置为N，初值设置为单位矩阵
//计算目标点云的协方差阵，具体可参考computeCovariances()函数的源代码解读
if ((!target_covariances_) || (target_covariances_->empty())) {
target_covariances_.reset(new MatricesVector);
computeCovariances<PointTarget>(target_, tree_, *target_covariances_);
}
//计算源点云的协方差阵
if ((!input_covariances_) || (input_covariances_->empty())) {
input_covariances_.reset(new MatricesVector);
computeCovariances<PointSource>(input_, tree_reciprocal_, *input_covariances_);
}
base_transformation_ = Matrix4::Identity();
nr_iterations_ = 0; 设置当前迭代次数为0
converged_ = false; 设置收敛状态为false
double dist_threshold = corr_dist_threshold_ * corr_dist_threshold_; 定义变量存储搜索最近邻点的距离阈值的平方
pcl::Indices nn_indices(1); 定义变量存储一个索引值
std::vector<float> nn_dists(1); 定义变量存储一个距离值
pcl::transformPointCloud(output, output, guess); 将源点云进行初始变换通过收敛状态控制迭代求解变换矩阵
while (!converged_) {}while循环中迭代求解的具体过程如下：继承而来的tranformation_的初值为单位矩阵，每迭代一次都要进行配对点对的搜索建立。
需特别说明：transformation_代表了多次迭代累积的变换矩阵。
std::size_t cnt = 0; 配对点对的个数
pcl::Indices source_indices(indices_->size()); 定义索引变量用来存储配对点对中源点云的索引，初值暂定为源点云的数量
pcl::Indices target_indices(indices_->size()); 定义索引变量用来存储配对点对中目标点云的索引，初值暂定为源点云的数量
/ 变量transform_R存储当前变换矩阵transformation_和变换初值guess的乘积，成为了最新的变换矩阵 /
Eigen::Matrix4d transform_R = Eigen::Matrix4d::Zero(); 初值赋为0
for (std::size_t i = 0; i < 4; i++)
for (std::size_t j = 0; j < 4; j++)
for (std::size_t k = 0; k < 4; k++)
transform_R(i, j) += static_cast<double>(transformation_(i, k)) *
static_cast<double>(guess(k, j));
Eigen::Matrix3d R = transform_R.topLeftCorner<3, 3>(); R存储变换矩阵中左上角3*3的旋转矩阵下文中用到了GICP算法求解变换矩阵T的部分公式：
for()循环通过遍历源点云，搜索出配对点对并计算变换矩阵T的一部分
M引用了mahalanobis_[i]的类型对象，可通过M访问和修改mahalanobis_[i]中的数据。
for (std::size_t i = 0; i < N; i++) { 遍历源点云
PointSource query = output[i]; 变量存储当前源点云的遍历点，是4维齐次坐标向量
query.getVector4fMap() = 矩阵*向量，得到了变换后新的4维坐标向量
transformation_.template cast<float>() * query.getVector4fMap();
/ 遍历点搜索不到最近点时抛出错误，此时已经完成了最近点搜索过程 /
if (!searchForNeighbors(query, nn_indices, nn_dists)) {
...
return;
}
/ 检查该点搜索到的最近邻点是否满足阈值要求 /
if (nn_dists[0] < dist_threshold) {
Eigen::Matrix3d& C1 = (*input_covariances_)[i]; 引用该点的协方差阵
Eigen::Matrix3d& C2 = (*target_covariances_)[nn_indices[0]];引用该点配对的目标点的协方差矩阵
Eigen::Matrix3d& M = mahalanobis_[i]; 引用该配对点对的马氏距离
/ 计算变换矩阵T的一部分 /
M = R * C1;
Eigen::Matrix3d temp = M * R.transpose();
temp += C2;
M = temp.inverse();
source_indices[cnt] = static_cast<int>(i); 存储配对点对的源索引
target_indices[cnt] = nn_indices[0]; 存储配对点对的目标索引
cnt++;
}
}source_indices.resize(cnt); 将源点云索引重置大小
target_indices.resize(cnt); 将目标点云索引重置大小
previous_transformation_ = transformation_; 将当前变换矩阵赋予前一次变换矩阵对配对的点对基于BFGS优化算法计算变换矩阵，得到的transformation_是多次迭代累积后的变换矩阵。
try {
rigid_transformation_estimation_(
output, source_indices, *target_, target_indices, transformation_);计算基于BFGS优化的变换矩阵
/ 为每次迭代均计算delta，该变量存储前后两次变换矩阵的最大差值 /
delta = 0.;
for (int k = 0; k < 4; k++) {
for (int l = 0; l < 4; l++) {
double ratio = 1;
if (k < 3 && l < 3) 旋转部分
ratio = 1. / rotation_epsilon_;
else
ratio = 1. / transformation_epsilon_; 平移部分
double c_delta =
ratio * std::abs(previous_transformation_(k, l) - transformation_(k, l));
if (c_delta > delta)
delta = c_delta;
}
}
} catch (PCLException& e) {
...
break;
}收敛与否的判断条件只有两个：迭代次数和前后两次变换矩阵的变化差值
nr_iterations_++; 当前迭代次数+1
// 是否满足收敛条件的判断
if (nr_iterations_ >= max_iterations_ || delta < 1) {
converged_ = true;
...
}
else
PCL_DEBUG("[pcl::%s::computeTransformation] Convergence failed\n",
getClassName().c_str());
}
while()循环结束后，意味着收敛成功，从而计算出最终的变换矩阵，并输出最终变换矩阵下的源点云。
final_transformation_ = previous_transformation_ * guess; 最终的变换矩阵
pcl::transformPointCloud(*input_, output, final_transformation_);输出最终变换矩阵T下的源点云总结
gicp算法实现的核心是computeTransformation()函数，该函数是实现点云配准估计变换矩阵的关键，也是区分不同配准算法的核心代码块，如果需要优化算法，可修改此部分实现。
本文主要聚焦于GICP算法的实现逻辑，但在computeTransformation()函数中计算最优变换矩阵时，需要重点研究该行代码：
rigid_transformation_estimation_(output, source_indices, *target_, target_indices, transformation_);该行代码背后的estimateRigidTransformationBFGS()函数使用拟牛顿法求解非线性优化问题，其需要用到大量的数学知识，故在此不做探究。
38万+
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
