>

## 基于 SVD 分解进行 ICP 配准

/// 设置 ICP 匹配参数，主要包括：最大关联距离，相邻两次迭代间的最大接收位姿变化程度以及匹配得分变化，以及最大迭代次数
bool ICPRegistration::SetRegistraionParam(float max_correspodence_dist,
float trans_eps,
float fitness_eps,
int max_iter) {
param_.max_correspodence_dist = max_correspodence_dist;
param_.trans_eps = trans_eps;
param_.fitness_eps = fitness_eps;
param_.max_iter = max_iter;

return true;
}

/// 设置匹配目标点云，用于建立 KD 树方便后续快速搜索最近点
bool ICPRegistration::SetInputTarget(const CloudData::Cloud_Ptr& input_target) {

target_kd_tree_ptr_->setInputCloud(input_target);
return true;
}

/// 进行 SVD 分解进行 ICP 配准，获得两个点云之间的相对位姿变化
bool ICPRegistration::MatchWithSVD(const CloudData::Cloud_Ptr& input_source,
const Eigen::Matrix4f& predict_pose,
Eigen::Matrix4f& result_pose)

{
Eigen::Matrix4f esitimated_pose = predict_pose;
bool converge = false;
int iter = 0;

// keep updating pose until converge or reach max iteration
while (iter < param_.max_iter && !converge) {
iter++;

CloudData::Cloud transformed_cloud;
pcl::transformPointCloud(*input_source, transformed_cloud, esitimated_pose);

std::vector<float> source_points_xyz;
std::vector<float> target_points_xyz;

// 预留足够空间，避免频繁空间分配
source_points_xyz.reserve(transformed_cloud.points.size() * 3);
target_points_xyz.reserve(transformed_cloud.points.size() * 3);

// 循环的过程同时计算上一次迭代最后的每个点平均关联距离
float score = 0.0;
for (size_t i = 0; i < input_source->points.size(); ++i) {
const CloudData::Point& transformed_point = transformed_cloud.points[i];

std::vector<int> indices(1);
std::vector<float> distances(1);

target_kd_tree_ptr_->nearestKSearch(transformed_point, 1, indices, distances);

score += distances[0];

if (distances[0] > param_.max_correspodence_dist) continue;

source_points_xyz.push_back(input_source->points[i].x);
source_points_xyz.push_back(input_source->points[i].y);
source_points_xyz.push_back(input_source->points[i].z);

target_points_xyz.push_back(target_kd_tree_ptr_->getInputCloud()->points[indices[0]].x);
target_points_xyz.push_back(target_kd_tree_ptr_->getInputCloud()->points[indices[0]].y);
target_points_xyz.push_back(target_kd_tree_ptr_->getInputCloud()->points[indices[0]].z);
}

// condition 1: fitness score converge -- 匹配距离收敛
score /= (transformed_cloud.points.size());
if (std::fabs(score_ - score) < param_.fitness_eps) {
converge = true;
break;
}
score_ = score;

// 使用 Eigen::Map 来避免多余空间使用
Eigen::Map<Eigen::Matrix3Xf> source_points(source_points_xyz.data(), 3, source_points_xyz.size() / 3);
Eigen::Map<Eigen::Matrix3Xf> target_points(target_points_xyz.data(), 3, target_points_xyz.size() / 3);

Eigen::Vector3f source_mean = source_points.rowwise().mean();
Eigen::Vector3f target_mean = target_points.rowwise().mean();

source_points.colwise() -= source_mean;
target_points.colwise() -= target_mean;

Eigen::Matrix3f H = source_points * target_points.transpose();
Eigen::JacobiSVD<Eigen::Matrix3f> svd(H, Eigen::ComputeFullU | Eigen::ComputeFullV);

Eigen::Vector3f prev_translation = esitimated_pose.block<3, 1>(0, 3);

Eigen::Matrix3f updated_rotation = svd.matrixV() * svd.matrixU().transpose();
Eigen::Vector3f updated_translation = target_mean - updated_rotation * source_mean;

esitimated_pose.block<3, 3>(0, 0) = updated_rotation;
esitimated_pose.block<3, 1>(0, 3) = updated_translation;

// condition 2: translation converge -- 这里只比较了平移没有考虑旋转的变化程度
if ((updated_translation - prev_translation).squaredNorm() < param_.trans_eps) {
converge = true;
}
}
result_pose = esitimated_pose;

return true;
}


## 基于高斯牛顿法进行 ICP 配准

$$\boldsymbol{e} = \boldsymbol{Rp}_i + \boldsymbol{t} - \boldsymbol{p}'_i$$


\begin{aligned} \frac{\partial\boldsymbol{e}_i}{\partial\boldsymbol{R}} &= -\boldsymbol{Rp}^\wedge\\ \frac{\partial\boldsymbol{e}_i}{\partial\boldsymbol{t}} &= \boldsymbol{I}\\ \boldsymbol{J}_i &= \begin{bmatrix} \frac{\partial\boldsymbol{e}_i}{\partial\boldsymbol{R}} & \frac{\partial\boldsymbol{e}_i}{\partial\boldsymbol{t}} \end{bmatrix} \in \mathbb{R}^{3\times 6} \end{aligned}


\begin{aligned} \boldsymbol{H}\delta\boldsymbol{x} &= \boldsymbol{b}\\ \boldsymbol{H} &= \sum_{i}\boldsymbol{J}_i^T\boldsymbol{J}_i \in \mathbb{R}^{6\times6}\\ \boldsymbol{b} &= -\sum_{i}\boldsymbol{J}_i^T\boldsymbol{e}_i \in \mathbb{R}^{6\times1} \end{aligned}


\begin{aligned} \boldsymbol{R}' &= \boldsymbol{R}\exp{(\delta\boldsymbol{x}_{0:2})}\\ \boldsymbol{t}' &= \boldsymbol{t} + \delta\boldsymbol{x}_{3:5} \end{aligned}


bool ICPRegistration::MatchWithGassianNewton(const CloudData::Cloud_Ptr& input_source,
const Eigen::Matrix4f& predict_pose,
Eigen::Matrix4f& result_pose) {
Eigen::Matrix4f esitimated_pose = predict_pose;
bool converge = false;
int iter = 0;

// keep updating pose until converge or reach max iteration
while (iter < param_.max_iter && !converge) {
iter++;

CloudData::Cloud transformed_cloud;
pcl::transformPointCloud(*input_source, transformed_cloud, esitimated_pose);

// 初始化 H, b
Eigen::Matrix<float, 6, 6> H = Eigen::Matrix<float, 6, 6>::Zero();  // J^TJ
Eigen::Matrix<float, 6, 1> b = Eigen::Matrix<float, 6, 1>::Zero();  // J^Te

// 对每个点计算在目标点云中的最近点，计算其雅可比和残差，构建 H，b
float score = 0.0;
for (size_t i = 0; i < input_source->points.size(); ++i) {
const CloudData::Point& transformed_point = transformed_cloud.points[i];

std::vector<int> indices(1);
std::vector<float> distances(1);

target_kd_tree_ptr_->nearestKSearch(transformed_point, 1, indices, distances);

const CloudData::Point& original_point = input_source->points[i];
const CloudData::Point& nearest_point = target_kd_tree_ptr_->getInputCloud()->points[indices[0]];
score += distances[0];

if (distances[0] > param_.max_correspodence_dist) continue;

Eigen::Vector3f source_pt(original_point.x, original_point.y, original_point.z);
Eigen::Vector3f transformed_pt(transformed_point.x, transformed_point.y, transformed_point.z);
Eigen::Vector3f target_pt(nearest_point.x, nearest_point.y, nearest_point.z);

// 计算雅可比矩阵
Eigen::Matrix<float, 3, 6> jacobian = Eigen::Matrix<float, 3, 6>::Zero();
jacobian.block<3, 3>(0, 0) = -esitimated_pose.block<3, 3>(0, 0) * Sophus::SO3f::hat(source_pt);
jacobian.block<3, 3>(0, 3) = Eigen::Matrix3f::Identity();
// 计算残差
Eigen::Vector3f resiual = transformed_pt - target_pt;

// 更新 H, b
H += jacobian.transpose() * jacobian;
b += -jacobian.transpose() * resiual;
}

// condition 1: fitness score converge
score /= (transformed_cloud.points.size());
if (std::fabs(score_ - score) < param_.fitness_eps) {
converge = true;
break;
}
score_ = score;

// 求解 Hx = b
Eigen::Matrix<float, 6, 1> delta_x = H.householderQr().solve(b);

// 更新 R, t
esitimated_pose.block<3, 3>(0, 0) *= Sophus::SO3f::exp(delta_x.block<3, 1>(0, 0)).matrix();
esitimated_pose.block<3, 1>(0, 3) += delta_x.block<3, 1>(3, 0);

// condition 2: transformation converge
if (delta_x.squaredNorm() < param_.trans_eps) {
converge = true;
}
}

// used final mean distance (across all points) as fitness score
result_pose = esitimated_pose;

return true;
}


## PCL 库中的 ICP，NDT 接口

PCL 库中也自带了 ICP 和 NDT 的算法接口，下面简单介绍一下如何使用。

• PCL-ICP

// 初始化 ICP 接口
icp_ptr_.reset(new pcl::IterativeClosestPoint<CloudData::Point, CloudData::Point>());

// 参数设置
icp_ptr_->setMaxCorrespondenceDistance(max_correspodence_dist);
icp_ptr_->setMaximumIterations(max_iter);
icp_ptr_->setTransformationEpsilon(trans_eps);
icp_ptr_->setEuclideanFitnessEpsilon(fitness_eps);

// 目标点云设置
// CloudData::Cloud_Ptr input_target; 获取目标点云，可以是上一帧点云，或者其他形式的点云地图
icp_ptr_->setInputTarget(input_target);

// 点云配准，获取匹配结果
// input_source (用来匹配的点云，通常是最新一帧点云)
// Eigen::Matrix4f predict_pose 预测位姿（初始值）
// CloudData::Cloud_Ptr result_cloud_ptr // 使用 ICP 估计后的位姿转换的匹配点云
// Eigen::Matrix4f result_pose ICP 最终位姿估计
icp_ptr_->setInputSource(input_source);
icp_ptr_->align(*result_cloud_ptr, predict_pose);
result_pose = icp_ptr_->getFinalTransformation();
score_ = icp_ptr_->getFitnessScore();   // 最后匹配残差（所有关联点对的平均距离）

• PCL-NDT

PCL 库中的 NDT 和 ICP 接口基本类似，如下所示：

// 初始化 NDT 接口
ndt_ptr_.reset(new pcl::NormalDistributionsTransform<CloudData::Point, CloudData::Point>());

// 参数设置，包括分辨率，迭代步长等
ndt_ptr_->setResolution(res);
ndt_ptr_->setStepSize(step_size);
ndt_ptr_->setTransformationEpsilon(trans_eps);
ndt_ptr_->setMaximumIterations(max_iter);

// 目标点云设置
// CloudData::Cloud_Ptr input_target; 获取目标点云，可以是上一帧点云，或者其他形式的点云地图
ndt_ptr_->setInputTarget(input_target);

// 点云配准，获取匹配结果
// input_source (用来匹配的点云，通常是最新一帧点云)
// Eigen::Matrix4f predict_pose 预测位姿（初始值）
// CloudData::Cloud_Ptr result_cloud_ptr // 使用 ICP 估计后的位姿转换的匹配点云
// Eigen::Matrix4f result_pose ICP 最终位姿估计
ndt_ptr_->setInputSource(input_source);
ndt_ptr_->align(*result_cloud_ptr, predict_pose);
result_pose = ndt_ptr_->getFinalTransformation();
ndt_ptr_->getFitnessScore();  // 最后匹配残差（所有关联点对的平均距离）


## 测试

• 第一帧不进行匹配，初始化局部地图
• 在局部地图累计若干帧之前不对点云进行下采样，直接将点云转换至世界坐标系中加入局部地图
• 每一帧通过和局部地图进行匹配获得相对位姿变换，并计算其世界坐标
• 每一帧点云的初始位姿预测值，为上一帧点云的世界坐标与上一帧估计的相对位姿变化（即假设载体在进行匀速运动，因此每两帧之间的相对运动基本保持不变）

### 性能比较

Built with Hugo
Theme Stack designed by Jimmy