【手写ICP】ICP -SVD 手动实现与例程（上）

文章目录

1. 写在前面
1. 配和代码的 SVD-ICP 流程梳理
- 1.1 SVD-ICP 算法 5 步走
- 1.2 预处理
- 1.3 点云变换
- 1.4 通过 KD-tree 找最近匹配点
- 1.5 SVD 求解
2. 几个简单的例子
- 2.1 基于PCL中的ICP进行点云匹配
- 2.2 基于PCL中的NDT进行点云匹配
- 2.3 基于手动ICP进行点云匹配
- 2.4 完整例程与结果对比

1. 写在前面

点云配准就是给定两帧点云，一帧固定为目标点云，一帧需要匹配的源点云，下文简称 S (Source) 和 T (Target) 点云。求最优的变换矩阵 T4x4T_{4x4}T4x4 希望将源 S 完美变换到目标 T 上去，那么我们就知道了 S 和 T 之间的三维变换关系 TtsT_{ts}Tts 或者 TstT_{st}Tst ，在这里我们统一采用左乘形式，即对于目标 T 和源 S 点云，他俩之间的变换有如下关系。
S=Tst∗TT=Tts∗SS = T_{st} * T \\ T = T_{ts} * S S=Tst∗TT=Tts∗S
本文主要是利用部分PCL函数，实现手写SVD-ICP，ICP也可以利用非线性优化来实现，稳定性要优于SVD解析方法。
特别注意，PCL库中实现了ICP，NDT等注册算法，跟我一起默念三遍，
- PCL中点云注册输出的矩阵是 Source to Target，也就是左乘中的 TtsT_{ts}Tts 矩阵。
- PCL中点云注册输出的矩阵是 Source to Target，也就是左乘中的 TtsT_{ts}Tts 矩阵。
- PCL中点云注册输出的矩阵是 Source to Target，也就是左乘中的 TtsT_{ts}Tts 矩阵。
简单展开分析一下，注意两个输入 Source 和 Target，在实际使用中可能会搞混，最后 getFinalTransformation 实际输出的是矩阵是 Source to Target 也就是 TtsT_{ts}Tts 矩阵。
这是非常符合人的理解逻辑的，从点云注册的目的来思考，之所以叫源 Source 和目标 Target ，就是把源 SSS 点云变换到目标点云 TTT 上去，设经过 ICP 计算变换后的点云为 SregS_{reg}Sreg ，我们的目标就是让 SregS_{reg}Sreg 和 TTT 尽可能完全重合，那么有如下关系。其中 TtsTrueT_{ts}^{True}TtsTrue 表示真实的变换关系，我们无法得知，TtsEstimateT_{ts}^{Estimate}TtsEstimate 表示估计的变换关系，errorerrorerror 表示变换的误差（点云间距离等）。
T=TtsTrue∗STtsEstimate=ICP(Source,Target)Sreg=TtsEstimate∗Serror=dist(T,Sreg)T = T_{ts}^{True} * S \\ T_{ts}^{Estimate} = ICP(Source,Target) \\ S_{reg} = T_{ts}^{Estimate} * S \\ error = dist(T,S_{reg}) T=TtsTrue∗STtsEstimate=ICP(Source,Target)Sreg=TtsEstimate∗Serror=dist(T,Sreg)
从 PCL 的 ICP 调用来看，cloud_source_registered 对应输出注册后的点云 SregS_{reg}Sreg ，transformation 对应估计的变换 TtsEstimateT_{ts}^{Estimate}TtsEstimate 矩阵。也就是说，PCL 的返回变换矩阵永远按照 TtsT_{ts}Tts 矩阵返回，而源点云就是你调用 setInputSource 传入的点云，目标点云就是 setInputTarget 传入的点云。以激光里程计为例，我们点云帧间注册的常见方法是把当前新采集的点云注册到局部点云地图中去，那么自然，当前点云就是 Source ，局部地图就是 Target 。
```
pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
icp.setInputSource(cl_t);
icp.setInputTarget(cl_s);
icp.align (cloud_source_registered);// S_{reg} = cloud_source_registered
// Obtain the transformation that aligned cloud_source to cloud_source_registered
Eigen::Matrix4f transformation = icp.getFinalTransformation ();// T_ts(Estimate)
```

1. 配和代码的 SVD-ICP 流程梳理

1.1 SVD-ICP 算法 5 步走

<1> 预处理，即滤波、清理野值等。
<2> 点云变换，根据上一步ICP求解的 R,tR,tR,t 位姿或者初始变换矩阵 TTT，对当前的源点云进行变换。ICP是一个迭代的过程，这一步就是迭代的关键，是让源点云一步步逼真目标点云的关键步骤。
<3> 找匹配点，在变换后的点云中寻找与目标点云中最近的点的匹配关系，并剔除不合理的点对。在这里，由于每一步源 S 点云都会经过变换（为了离T点云越来越近），而目标 T 点云是固定不动的，因此我们一般用 Kd-tree 储存目标 T 点云，然后在源点云中遍历点 SiS_iSi ，搜索在 T 中最近的点 TiT_iTi 。
<4> SVD求解，在现有匹配关系下，求质心-去质心-求H-求SVD-求R,t ，走完一套流程
<5> 收敛判断，一般的收敛条件有迭代次数、点云距离、矩阵 TTT 增量等。只要达到任意条件即可停止迭代。

1.2 预处理

点云预处理，可以利用PCL的VoxelGrid滤波器进行降采样，或利用统计滤波去除一些离群值，如下所示，注意以下代码只是做说明之用，本文最后提供了完整的例程。

pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_raw(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filter(new pcl::PointCloud<pcl::PointXYZ>);
//体素降采样------------------------------------------------------------
pcl::VoxelGrid<pcl::PointXYZ> voxel_filter;
voxel_filter.setInputCloud(cloud_raw);
voxel_filter.setLeafSize (0.1f, 0.1f, 0.1f);// 单位：m
voxel_filter.filter(*cloud_filter);
//统计滤波--------------------------------------------------------------
pcl::StatisticalOutlierRemoval<pcl::PointXYZ> stat_filter;
stat_filter.setInputCloud(cloud_raw);
stat_filter.setMeanK(50);               //K近邻搜索点个数
stat_filter.setStddevMulThresh(1.0);    //标准差倍数
stat_filter.setNegative(false);         //保留未滤波点（外点），false就是只要内点
stat_filter.filter(*cloud_filter);      //保存滤波结果到cloud_filter

1.3 点云变换

在预处理之后，直接利用上一步变换阵或者初始估计来变换源点云，代码如下：

pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_S(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_S_Trans(new pcl::PointCloud<pcl::PointXYZ>);
Eigen::Matrix4f transform = Eigen::Matrix4f::Identity(); //可以根据实际情况来赋初值或从上一步ICP得到
pcl::transformPointCloud(*cloud_S,*cloud_S_Trans,transform);

1.4 通过 KD-tree 找最近匹配点

一般利用到 KD-Tree 结构来快速搜索，pcl 中常用 KdTreeFLANN 中的 nearestKSearch 来找最近点，这里我们的策略是，目标点云一般是固定不动的（例如在激光里程计前端的局部地图），并不会每次扫描都更新，因此把 TTT 建为 kd-tree 而在 SSS 点云内部遍历来找点比较节省时间。 nearestKSearch 函数的输出 indexs，distances 就是 k 个最近点的索引和距离。

pcl::KdTreeFLANN<pcl::PointXYZ> kd_tree;
kd_tree.setInputCloud(cl_t);float max_correspond_distance_ = 0.001;
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_S_Trans(new pcl::PointCloud<pcl::PointXYZ>);
pcl::transformPointCloud(*cl_s, *cloud_S_Trans, transform);
std::vector<int> match_indexs;
for (auto i = 0; i < cloud_S_Trans->size(); ++i)
{std::vector<float> k_distances(2);std::vector<int> k_indexs(2);kd_tree.nearestKSearch(cloud_S_Trans->at(i), 1, k_indexs, k_distances);if (k_distances[0] < max_correspond_distance_){match_indexs.emplace_back(k_indexs[0]);}
}pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_match_from_S_trans(new pcl::PointCloud<pcl::PointXYZ>);
pcl::copyPointCloud(*cloud_S_Trans, match_indexs, *cloud_match_from_S_trans);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_match_from_T(new pcl::PointCloud<pcl::PointXYZ>);
pcl::copyPointCloud(*cl_t, match_indexs, *cloud_match_from_T);

1.5 SVD 求解

经过上面几步后，我们得到了完全匹配的两组点云 T (cloud_match_from_T) 和 S (cloud_match_from_S_trans)，注意这里的 S 和 T 都已经不是原始的点云，已经经过了预处理、变换和匹配。那么 point-to-point ICP 的目标函数如下，求最优的旋转和平移。默认 S 和 T 的点已经匹配完毕。
R∗，t∗=argminR,t1∣NS∣∑i=1NS∣∣Ti(x,y,z)−[RTS∗Si(x,y,z)+tTS]∣∣2R^*，t^*=argmin_{R,t} \frac{1}{|N_S|}\sum_{i=1}^{N_S}{||T_i(x,y,z)-[R_{TS}*S_i(x,y,z) + t_{TS}] ||^2} R∗，t∗=argminR,t∣NS∣1i=1∑NS∣∣Ti(x,y,z)−[RTS∗Si(x,y,z)+tTS]∣∣2
计算源和目标点云质心 μS,μT\mu_S, \mu_TμS,μT 。

μS=1NS∑i=1NSSi(xi,yi,zi)μT=1NT∑i=1NTTi(xi,yi,zi)\mu_S = \frac{1}{N_S} \sum_{i=1}^{N_S}{S_i}(x_i,y_i,z_i) \\ \mu_T = \frac{1}{N_T} \sum_{i=1}^{N_T}{T_i}(x_i,y_i,z_i) μS=NS1i=1∑NSSi(xi,yi,zi)μT=NT1i=1∑NTTi(xi,yi,zi)

基于 PCL 计算点云质心。

// 3d center of two clouds
Eigen::Vector4f mu_S_temp, mu_T_temp;
pcl::compute3DCentroid(*cloud_match_from_S_trans, mu_S_temp);
pcl::compute3DCentroid(*cloud_match_from_T, mu_T_temp);
Eigen::Vector3f mu_S(mu_S_temp[0], mu_S_temp[1], mu_S_temp[2]);
Eigen::Vector3f mu_T(mu_T_temp[0], mu_T_temp[1], mu_T_temp[2]);

原始点云减去对应质心，得到去质心的点云 $ S^c , T^c $ 。即匹配后的 S 和 T 点云转换到质心坐标系。
Sc=S−μSTc=T−μTS^c = S - \mu_S \\ T^c = T - \mu_T Sc=S−μSTc=T−μT
计算矩阵 HHH ，是去质心之后的点相乘得来，维度是 3x3 。

H3×3=∑i=1NSSic(x,y,z)3×1∗Tic(x,y,z)1×3T=S3×NC∗TN×3CTH_{3\times3} = \sum_{i=1}^{N_S}{S^c_i}(x,y,z)_{3\times1}*{T^c_i}(x,y,z)_{1\times3}^T = S^C_{3\times{N}}*T^{CT}_{{N}\times3} H3×3=i=1∑NSSic(x,y,z)3×1∗Tic(x,y,z)1×3T=S3×NC∗TN×3CT

基于 PCL 的 HHH 矩阵计算

// H += (S_i) * (T_i^T)
Eigen::Matrix3f H_icp = Eigen::Matrix3f::Identity();
for (auto i = 0; i < match_indexs.size(); ++i)
{Eigen::Vector3f s_ci(cloud_match_from_S_trans->at(i).x - mu_S[0],cloud_match_from_S_trans->at(i).y - mu_S[1],cloud_match_from_S_trans->at(i).z - mu_S[2]);Eigen::Vector3f t_ci(cloud_match_from_T->at(i).x - mu_T[0],cloud_match_from_T->at(i).y - mu_T[1],cloud_match_from_T->at(i).z - mu_T[2]);Eigen::Matrix3f H_temp = s_ci * t_ci.transpose();H_icp += H_temp;
}

对 HHH 求 SVD 分解，根据公式求得 R∗,t∗R^*,t^*R∗,t∗
H=UΣVTRTS∗=V∗UTtTS∗=μT−RTS∗∗μSH = U \Sigma V^T \\ R^*_{TS} = V*U^T \\ t^*_{TS} = \mu_T - R^*_{TS}*\mu_S H=UΣVTRTS∗=V∗UTtTS∗=μT−RTS∗∗μS

直接利用 Eigen 计算 SVD 分解，并求取旋转和平移。

Eigen::JacobiSVD<Eigen::MatrixXf> svd(H_icp, Eigen::ComputeThinU | Eigen::ComputeThinV);
Eigen::Matrix3f R_ts = svd.matrixU() * (svd.matrixV().transpose());
Eigen::Vector3f t_ts = mu_T - R_ts * mu_S;// T = R_ts * S + t_ts

2. 几个简单的例子

几个例子中的收敛都没有严谨考虑，其中参数定义详见 PCL-Doc，手写 ICP 中仅利用了迭代次数作为收敛条件。

2.1 基于PCL中的ICP进行点云匹配

ICP 匹配，注意这里的时间计算是比较随性的，严谨的方法请自行实现。

void ICP_PCL(pcl::PointCloud<pcl::PointXYZ>::Ptr &cl_s,pcl::PointCloud<pcl::PointXYZ>::Ptr &cl_t,pcl::PointCloud<pcl::PointXYZ>::Ptr &cl_final,Eigen::Matrix4f &T_ts,double &time_cost)
{clock_t startT, endT;startT = clock();pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;//trs source to  target = T_tsicp.setInputSource(cl_s);icp.setInputTarget(cl_t);icp.setMaxCorrespondenceDistance(10);//importanticp.setMaximumIterations(50);icp.setTransformationEpsilon(1e-10);icp.setEuclideanFitnessEpsilon(0.001);icp.align(*cl_final);T_ts = icp.getFinalTransformation();endT = clock();time_cost = (double) (endT - startT) / CLOCKS_PER_SEC;
}

2.2 基于PCL中的NDT进行点云匹配

NDT 匹配，这里用了体素滤波，注意这里的时间计算是比较随性的，严谨的方法请自行实现。

void NDT_PCL(pcl::PointCloud<pcl::PointXYZ>::Ptr &cl_s,pcl::PointCloud<pcl::PointXYZ>::Ptr &cl_t,pcl::PointCloud<pcl::PointXYZ>::Ptr &cl_final,Eigen::Matrix4f &T_ts,double &time_cost)
{clock_t startT, endT;startT = clock();pcl::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ> ndt;pcl::PointCloud<pcl::PointXYZ>::Ptr cl_s_filt(new pcl::PointCloud<pcl::PointXYZ>);pcl::PointCloud<pcl::PointXYZ>::Ptr cl_t_filt(new pcl::PointCloud<pcl::PointXYZ>);pcl::VoxelGrid<pcl::PointXYZ> voxel_filter;voxel_filter.setLeafSize(0.1f, 0.1f, 0.1f);voxel_filter.setInputCloud(cl_s);voxel_filter.filter(*cl_s_filt);voxel_filter.setInputCloud(cl_t);voxel_filter.filter(*cl_t_filt);ndt.setInputSource(cl_s_filt);ndt.setInputTarget(cl_t_filt);ndt.setStepSize(0.1);ndt.setResolution(1.0);ndt.setMaximumIterations(50);ndt.setTransformationEpsilon(1e-10);ndt.align(*cl_final);T_ts = ndt.getFinalTransformation();endT = clock();time_cost = (double) (endT - startT) / CLOCKS_PER_SEC;
}

2.3 基于手动ICP进行点云匹配

手写 ICP 匹配，注意这里的时间计算是比较随性的，严谨的方法请自行实现。

void ICP_MANUAL(pcl::PointCloud<pcl::PointXYZ>::Ptr &cl_s,pcl::PointCloud<pcl::PointXYZ>::Ptr &cl_t,pcl::PointCloud<pcl::PointXYZ>::Ptr &cl_final,Eigen::Matrix4f &T_ts,double &time_cost)
{clock_t startT, endT;startT = clock();// target cloud is fixed, so kd-tree create for targetpcl::KdTreeFLANN<pcl::PointXYZ> kd_tree;kd_tree.setInputCloud(cl_t);int nICP_Step = 10; //you can change this param for different situationsfloat max_correspond_distance_ = 0.001; //you can change this param for different situationsEigen::Matrix4f transform = Eigen::Matrix4f::Identity(); //init T_stfor (int i = 0; i < nICP_Step; ++i){pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_S_Trans(new pcl::PointCloud<pcl::PointXYZ>);// let us transform [cl_s] to [cloud_S_Trans] by [T_st](transform)// T_st changed for each interation, make [cloud_S_Trans] more and more close to [cl_s]pcl::transformPointCloud(*cl_s, *cloud_S_Trans, transform);std::vector<int> match_indexs;for (auto i = 0; i < cloud_S_Trans->size(); ++i){std::vector<float> k_distances(2);std::vector<int> k_indexs(2);kd_tree.nearestKSearch(cloud_S_Trans->at(i), 1, k_indexs, k_distances);if (k_distances[0] < max_correspond_distance_){match_indexs.emplace_back(k_indexs[0]);}}// matched cloudspcl::PointCloud<pcl::PointXYZ>::Ptr cloud_match_from_S_trans(new pcl::PointCloud<pcl::PointXYZ>);pcl::copyPointCloud(*cloud_S_Trans, match_indexs, *cloud_match_from_S_trans);pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_match_from_T(new pcl::PointCloud<pcl::PointXYZ>);pcl::copyPointCloud(*cl_t, match_indexs, *cloud_match_from_T);// 3d center of two cloudsEigen::Vector4f mu_S_temp, mu_T_temp;pcl::compute3DCentroid(*cloud_match_from_S_trans, mu_S_temp);pcl::compute3DCentroid(*cloud_match_from_T, mu_T_temp);Eigen::Vector3f mu_S(mu_S_temp[0], mu_S_temp[1], mu_S_temp[2]);Eigen::Vector3f mu_T(mu_T_temp[0], mu_T_temp[1], mu_T_temp[2]);// H += (S_i) * (T_i^T)Eigen::Matrix3f H_icp = Eigen::Matrix3f::Identity();for (auto i = 0; i < match_indexs.size(); ++i){Eigen::Vector3f s_ci(cloud_match_from_S_trans->at(i).x - mu_S[0],cloud_match_from_S_trans->at(i).y - mu_S[1],cloud_match_from_S_trans->at(i).z - mu_S[2]);Eigen::Vector3f t_ci(cloud_match_from_T->at(i).x - mu_T[0],cloud_match_from_T->at(i).y - mu_T[1],cloud_match_from_T->at(i).z - mu_T[2]);Eigen::Matrix3f H_temp = s_ci * t_ci.transpose();H_icp += H_temp;}// H = SVDEigen::JacobiSVD<Eigen::MatrixXf> svd(H_icp, Eigen::ComputeThinU | Eigen::ComputeThinV);Eigen::Matrix3f R_ts = svd.matrixU() * (svd.matrixV().transpose());Eigen::Vector3f t_ts = mu_T - R_ts * mu_S;// T = R_ts * S + t_tsEigen::Quaternionf q_ts(R_ts);transform.block<3, 3>(0,0) *= q_ts.normalized().toRotationMatrix().inverse();//we use left form, so we need .inverse()transform.block<3, 1>(0, 3) += t_ts;// We got a new [transform] here, that is, we are more closer to the [source] than last [transform].// This is why we call it Iterative method.}T_ts = transform;//transform = T_ts// Target = T_t * Sourcepcl::transformPointCloud(*cl_s, *cl_final, T_ts);endT = clock();time_cost = (double) (endT - startT) / CLOCKS_PER_SEC;
}

2.4 完整例程与结果对比

为了避免文章过于冗长，分成上下部，详见下部。