全局ICP梯度解法推导及代码

问题描述

ICP(Iterative Closest Point)是点云精配准中最常用的方法之一。在上一篇博客中我复现了两两配准(pairwise registration)中的ICP算法。在实际应用中，经常需要同时配准多幅点云，这是就需要用到全局ICP算法在。其思路与两两配准的ICP算法相同，即通过寻找临近点构建误差的最小二乘公式，并计算出使得误差公式最小的旋转矩阵和平移矩阵。两者的不同在于误差公式中代求旋转矩阵和平移矩阵的个数。

由于我没有找到相关解析求解方法，故选用梯度下降法来求解最小二乘。

完整代码下载地址

全局ICP思路

首先使用点对点(point-to-point)的方式寻找邻近点，误差公式为：
$$E=\sum _{m=1}^K\sum _{n=1,n\ne m}^K\sum _{i=1}^{N_{mn}}||(R_m{P}_m^i+t_m)-(R_n{P}_n^i+t_n)|{|}^2$$
其中$K$是点云个数，$N_{mn}$是第$m$片点云和第$n$片点云邻近点的个数，$R_m$、$R_n$分别是第$m$和$n$片点云的旋转矩阵，$t_m$、$t_n$分别是第$m$和$n$片点云的平移矩阵。

全局ICP的目标——找到使误差$E$最小的旋转矩阵{$R_i$}($i=1\cdots K$)和平移矩阵{$t_i$}($i=1\cdots K$)

第一步：问题简化——将求解平移矩阵$t$分离出来

类比两两ICP配准，我首先将每片点云中的点减去质心坐标，用以消除平移矩阵$t$的影响。

每片点云的质心坐标:
$$\overline{P_m}=\frac{1}{N_m}\sum _{i=1}^{N_m}{P}_{mi}$$

定义点云集{$Q_i$}($i=1\cdots K$)
$$Q_m^i=P_m^i-\overline{P_m}$$
原公式可被简化为
$$E=\sum _{m=1}^K\sum _{n=1,n\ne m}^K\sum _{i=1}^{N_{mn}}||R_m{Q}_m^i-R_n{Q}_n^i|{|}^2$$

第二步：梯度下降法求解旋转矩阵

假设我们固定$i=1$的点云，则上述公式右边有$K-1$个待求变量{$R_2,R_3\cdots ,R_k$}。每次迭代过程中，更新后的变量求解公式为
$$R^{i+1}=\left[\begin{array}{c}{R}_2^{i+1}\\ R_3^{i+1}\\ ⋮\\ R_k^{i+1}\end{array}\right]=R^i-\alpha {▽}_R=\left[\begin{array}{c}\partial E/\partial R_2^i\\ \partial E/\partial R_3^i\\ ⋮\\ \partial E/\partial R_k^i\end{array}\right]$$
其中，梯度求解公式为：
$$\partial E/\partial R_m=\sum _{n=1,n\ne m}^K\sum _{i=1}^{N_{mn}}2R_m{Q}_m^i{Q}_m^{iT}-2R_n{Q}_n^i{Q}_m^{iT}$$
梯度推导过程在文章后面

第三步：求解平移矩阵

平移矩阵计算公式为：
$$t_m=\overline{P_1}-R_m\overline{P_m}$$

注意：第二步和第三步在每一次迭代过程中都要执行一遍。并且将更新后的R、t应用到点云集之后需要重新计算邻近点。

实际应用中，程序重复执行第二和第三步，直到收敛。

核心代码

完整代码下载地址

void GloballyICP::align(vector<pcl::PointCloud<pcl::PointXYZ>>& clouds_out) {
		for (int indexIter = 0; indexIter < _iterNum; ++indexIter) {
			cout << "iteration : " << indexIter << endl;
			FindCentroid();
			for (size_t indexCloud = 0; indexCloud < _cloudsNum; ++indexCloud) {
				int centroidX = _Centroids[indexCloud].x;
				int centroidY = _Centroids[indexCloud].y;
				int centroidZ = _Centroids[indexCloud].z;
				for (size_t indexPoint = 0; indexPoint < _clouds_ICP[indexCloud].points.size(); ++indexPoint) {
					_Qclouds_ICP[indexCloud][indexPoint].x = _clouds_ICP[indexCloud][indexPoint].x - centroidX;
					_Qclouds_ICP[indexCloud][indexPoint].y = _clouds_ICP[indexCloud][indexPoint].y - centroidY;
					_Qclouds_ICP[indexCloud][indexPoint].z = _clouds_ICP[indexCloud][indexPoint].z - centroidZ;
				}
			}
			/* step one : find correspondences */
			FindCorrespondence();
			cout << "MSE : " << _meanSquaredError << endl;
			/* step two : 梯度法求解旋转矩阵R */
			// 遍历计算梯度
			for (int indexR = 0; indexR < _cloudsNum; ++indexR) {
				//cout << indexR << endl;
				if (indexR == _Ifix) {
					_delta_Rotation_Matrices[indexR] = Eigen::Matrix3f::Zero();
					continue;
				}
				Eigen::Matrix3f tempDeltaRotation = Eigen::Matrix3f::Zero();
				int pointNum = 0;
				for (int indexCloud = 0; indexCloud < _cloudsNum; ++indexCloud) {
					if (indexCloud == indexR)
						continue;
					vector<size_t>correspondenceM = _correspondence_index[indexR][indexCloud];
					vector<size_t>correspondenceN = _correspondence_index[indexCloud][indexR];
					for (size_t indexPoint = 0; indexPoint < _correspondenceNum[indexR][indexCloud]; ++indexPoint) {
						//cout << _Qclouds_ICP[indexR].points[correspondenceM[indexPoint]] << endl;
						Eigen::Vector3f Mpoint(_Qclouds_ICP[indexR].points[correspondenceM[indexPoint]].x,
							_Qclouds_ICP[indexR].points[correspondenceM[indexPoint]].y,
							_Qclouds_ICP[indexR].points[correspondenceM[indexPoint]].z);
						//cout << _Qclouds_ICP[indexCloud].points[correspondenceN[indexPoint]] << endl;
						Eigen::Vector3f Npoint(_Qclouds_ICP[indexCloud].points[correspondenceN[indexPoint]].x,
							_Qclouds_ICP[indexCloud].points[correspondenceN[indexPoint]].y,
							_Qclouds_ICP[indexCloud].points[correspondenceN[indexPoint]].z);
						//cout << "Mpoint:" << endl << Mpoint << endl;
						//cout << "Npoint:" << endl << Npoint << endl;
						tempDeltaRotation += 2 * (_Rotation_Matrices[indexR] * Mpoint * Mpoint.transpose() -
							_Rotation_Matrices[indexCloud] * Npoint * Mpoint.transpose());
						++pointNum;
					}
				}
				if (pointNum == 0) {
					_delta_Rotation_Matrices[indexR] = Eigen::Matrix3f::Zero();
				}
				else {
					_delta_Rotation_Matrices[indexR] = tempDeltaRotation / pointNum;
				}
				//cout << "delta rotation Matrix:" << indexR << endl << _delta_Rotation_Matrices[indexR] << endl;
			}
			// 计算更新后的旋转矩阵 R
			for (int indexR = 0; indexR < _cloudsNum; ++indexR) {
				_Rotation_Matrices[indexR] = _Rotation_Matrices[indexR] - _alpha * _delta_Rotation_Matrices[indexR];
			}
			//cout << "RotationMatrix:" << endl << _Rotation_Matrices[1] << endl;
			// 计算平移矩阵
			Eigen::Vector3f centroidF(_Centroids[_Ifix].x, _Centroids[_Ifix].y, _Centroids[_Ifix].z);
			for (int indexT = 0; indexT < _cloudsNum; ++indexT) {
				if (indexT == _Ifix)
					_Transformaton_Matrices[indexT] = Eigen::Vector3f::Zero();
				else {
					Eigen::Vector3f centroidM(_Centroids[indexT].x, _Centroids[indexT].y, _Centroids[indexT].z);
					_Transformaton_Matrices[indexT] = centroidF - _Rotation_Matrices[indexT] * centroidM;
					//cout << "Transformation matrix " << indexT << " : " << endl;
					//cout << _Transformaton_Matrices[indexT] << endl;
				}
			}
			/* step3 : 将R，T应用到 _clouds_ICP  */
			UpdateCloudsPosition();
		}
		clouds_out = _clouds_ICP;
	}

实验效果

我对随机生成的点云进行人为平移和旋转，共使用四片点云(颜色不同)进行全局ICP配准，在步长为0.001时迭代300次的效果如下图：上图左边为待配准的原始点云，右边为配准完毕的点云。在右图中，PCL的可视化模块已经将四片点云中的对应点显示为一个点，这表明配准效果还是很好的。最后一次迭代的均方差为0.014。

梯度法求解推导

消除平移矩阵后的误差公式：
$$\begin{gathered} E=\sum _{m=1}^K\sum _{n=1,n\ne m}^K\sum _{i=1}^{N_{mn}}||R_m{Q}_m^i-R_n{Q}_n^i|{|}^2 \\ =\sum _{m=1}^K\sum _{n=1,n\ne m}^K\sum _{i=1}^{N_{mn}}(R_m{Q}_m^i-R_n{Q}_n^i{)}^T(R_m{Q}_m^i-R_n{Q}_n^i) \\ =\sum _{m=1}^K\sum _{n=1,n\ne m}^K\sum _{i=1}^{N_{mn}}(Q_m^{iT}{R}_m^T-Q_n^{iT}{R}_n^T)(R_m{Q}_m^i-R_n{Q}_n^i) \end{gathered}$$
展开后，求和符号内部为
$$(Q_m^{iT}{R}_m^T-Q_n^{iT}{R}_n^T)(R_m{Q}_m^i-R_n{Q}_n^i)=$$
$$Q_m^{iT}{R}_m^T{R}_m{Q}_m^i-Q_m^{iT}{R}_m^T{R}_n{Q}_n^i-Q_n^{iT}{R}_n^T{R}_m{Q}_m^i+Q_n^{iT}{R}_n^T{R}_n{Q}_n^i$$
所以，偏导$\partial E/\partial R_m$为
$$\partial E/\partial R_m=\sum _{n=1,n\ne m}^K\sum _{i=1}^{N_{mn}}\frac{\partial Q_m^{iT}{R}_m^T{R}_m{Q}_m^i}{\partial R_m}-\frac{\partial Q_m^{iT}{R}_m^T{R}_n{Q}_n^i}{\partial R_m}-\frac{\partial Q_n^{iT}{R}_n^T{R}_m{Q}_m^i}{\partial R_m}$$
于是，现在遇到的问题就是三个偏导数如何求解。
在矩阵对矩阵的求导中，有三个公式，分别为
$$\frac{\partial a^T{X}^{T}Xb}{\partial X}=Xb{a}^T+Xa{b}^T$$
$$\frac{\partial a^T{X}^{T}b}{\partial X}=b{a}^T$$
$$\frac{\partial a^{T}Xb}{\partial X}=a{b}^T$$
将这三个公式应用到偏导求解过程则有：
$$\frac{\partial Q_m^{iT}{R}_m^T{R}_m{Q}_m^i}{\partial R_m}=2R_m{Q}_m^i{Q}_m^{iT}$$
$$\frac{\partial Q_m^{iT}{R}_m^T{R}_n{Q}_n^i}{\partial R_m}=R_n{Q}_n^i{Q}_m^{iT}$$
$$\frac{\partial Q_n^{iT}{R}_n^T{R}_m{Q}_m^i}{\partial R_m}=R_n{Q}_n^i{Q}_m^{iT}$$
即
$$\partial E/\partial R_m=\sum _{n=1,n\ne m}^K\sum _{i=1}^{N_{mn}}2R_m{Q}_m^i{Q}_m^{iT}-2R_n{Q}_n^i{Q}_m^{iT}$$
推导完毕
完整代码下载地址