点云配准经典算法ICP复现 C++ Win10 pcl1.11.1

背景

ICP算法是点云配准(registration)领域的主流算法，在学习过程中我尝试使用C++复现了ICP算法。我参考的是经典ICP论"P. Besl, N. McKay. ‘A Method for Registration of 3-D Shapes,’ IEEE Trans. on Pattern Analysis and Machine Intel., vol. 14, no. 2, pp. 239-256,1992"。这篇论文中使用的求解最小二乘的方法为单位四元数法，建议论文和代码搭配阅读。

环境

windows10 + pcl1.11.1

思路

• 计算邻近点：这一步骤设置了两种模式：暴力枚举(BRUTE_FORCE)和kd树(KD_TREE)。两种模式的思路顾名思义，需要说明的是当点云规模稍大时，暴力枚举所消耗的时间是不能被接受的。
• 计算旋转和平移矩阵：ICP算法中求解最小二乘一般有四种，分别为单位四元数、奇异值分解、正交矩阵法和对偶四元数。我使用的是论文中提到的单位四元数法，之后也复现了常用的奇异值分解法(SVD)
• 对目标点云进行旋转和平移操作
• 重复上述三个步骤直至终止：终止准则一般有三种，一是设置迭代次数；二是设置均方差阈值，当某一迭代过程中均方差小于该阈值则认为收敛；三是设置均方差微分阈值，当某次迭代过程中均方差与上次迭代中均方差的差值小于该阈值则认为收敛。我在程序中采用的是第一和第二种准则。

代码

计算临近点

	// first step : find the clostest points
		void ComputeCorrespondence(PointTargetPtr& cloud_ICP){
			if (methodCP == BURTE_FORCE) { // 使用穷举法计算邻近点
				// clear the correspondence vector generated at last iteration
				//vector<int>().swap(Index_corr_src);
				//vector<int>().swap(Index_corr_tar);
				//vector<int>().swap(SquareError);
				for (size_t index_source = 0; index_source < cloud_source->points.size(); ++index_source) {
					Scalar minDistance = CorrThreshold;
					size_t minTargetIndex = 0;
					for (size_t index_target = 0; index_target < cloud_target->points.size(); ++index_target) {
						cout << "calculating now... index_source = " << index_source << " index_target = " << index_target << endl;
						//Scalar distance = ComputeDistance3d(
						//	cloud_source->points[index_source].x,
						//	cloud_source->points[index_source].y,
						//	cloud_source->points[index_source].z,
						//	cloud_ICP->points[index_target].x,
						//	cloud_ICP->points[index_target].y,
						//	cloud_ICP->points[index_target].z);
						Scalar test = pow(cloud_source->points[index_source].x - cloud_ICP->points[index_target].x, 2);
						float deltaX = cloud_source->points[index_source].x - cloud_ICP->points[index_target].x;
						float deltaY = cloud_source->points[index_source].y - cloud_ICP->points[index_target].y;
						float deltaZ = cloud_source->points[index_source].z - cloud_ICP->points[index_target].z;
						float distance = pow(float(pow(deltaX, 2) + pow(deltaY, 2) + pow(deltaZ, 2)), 0.5);
						if (distance < minDistance) {
							minDistance = distance;
							minTargetIndex = index_target;
						}
					}
					if (minDistance < CorrThreshold) {
						Index_corr_src.push_back(index_source);
						Index_corr_tar.push_back(minTargetIndex);
						SquareError.push_back(minDistance);
					}
				}
				// calculate the mean square error
				Scalar SquareErrorSum = 0;
				for (size_t i = 0; i < SquareError.size(); ++i) {
					SquareErrorSum += SquareError[i];
				}
				SquareMeanError = SquareErrorSum / SquareError.size();
			}
			else if (methodCP == KD_TREE) { // 使用kdtree 暴力枚举实在是太慢了
				// 由于源点云不变化，所以对源点云建立kdtree
				pcl::KdTreeFLANN<pcl::PointXYZ>kdtree;
				kdtree.setInputCloud(cloud_ICP);
				Index_corr_src.resize(0);
				Index_corr_tar.resize(0);
				SquareError.resize(0);
				for (size_t i = 0; i < cloud_source->points.size(); ++i) {
					pcl::PointXYZ searchPoint;
					searchPoint.x = cloud_source->points[i].x;
					searchPoint.y = cloud_source->points[i].y;
					searchPoint.z = cloud_source->points[i].z;
					int k = 1;
					vector<int> Idx(k);
					vector<Scalar> SD(k);
					if (kdtree.nearestKSearch(searchPoint, k, Idx, SD) > 0) {
						if (SD[0] < CorrThreshold) {
							Index_corr_src.push_back(i);
							Index_corr_tar.push_back(Idx[0]);
							SquareError.push_back(SD[0]);
						}
					}
				/*	if (it == 1) {
						cout << i << " " << Idx[0] << " " << SD[0] << endl;
					}*/
					
				}
				// 计算均方差
				if (SquareError.size() == 0) {
					throw"there si something wrong, SquareError is zero...";
				}
				Scalar SquaerErrorSum = 0;
				for (size_t i = 0; i < SquareError.size(); ++i) {
					SquaerErrorSum += SquareError[i];
				}
				SquareMeanError = SquaerErrorSum / SquareError.size();
			}
		}

计算旋转和平移矩阵(即求解最小二乘)

	// second step : compute the registration
		void ComputeRegistration(PointTargetPtr& cloud_ICP) {
			if (methodMS == QUATERNION) {
				// 计算源点云和icp点云 邻近点的中心
				pcl::PointXYZ ave_source;
				pcl::PointXYZ ave_ICP;
				Scalar tempX = 0;
				Scalar tempY = 0;
				Scalar tempZ = 0;
				for (size_t i = 0; i < Index_corr_src.size(); ++i) {
					tempX += cloud_source->points[Index_corr_src[i]].x;
					tempY += cloud_source->points[Index_corr_src[i]].y;
					tempZ += cloud_source->points[Index_corr_src[i]].z;
				}
				ave_source.x = tempX / Index_corr_src.size();
				ave_source.y = tempY / Index_corr_src.size();
				ave_source.z = tempZ / Index_corr_src.size();
				tempX = 0;
				tempY = 0;
				tempZ = 0;
				for (size_t i = 0; i < Index_corr_tar.size(); ++i) {
					tempX += cloud_ICP->points[Index_corr_tar[i]].x;
					tempY += cloud_ICP->points[Index_corr_tar[i]].y;
					tempZ += cloud_ICP->points[Index_corr_tar[i]].z;
				}
				ave_ICP.x = tempX / Index_corr_tar.size();
				ave_ICP.y = tempY / Index_corr_tar.size();
				ave_ICP.z = tempZ / Index_corr_tar.size();
#if SHOW_DEB == 1
				cout << "center of source cloud : (" << ave_source.x << "," << ave_source.y
					<< "," << ave_source.z << ")" << endl;
				cout << "center of target cloud : (" << ave_ICP.x << "," << ave_ICP.y 
					<< "," << ave_ICP.z << ")" << endl;
#endif
				// calculate the cross-covariance matrix (3x3)
				Eigen::Matrix3d CroCovarM = Eigen::Matrix3d::Zero();// cross-covariance matrix
				for (size_t i = 0; i < Index_corr_src.size(); ++i) {
					CroCovarM += PointDot(cloud_ICP->points[Index_corr_tar[i]],
						cloud_source->points[Index_corr_src[i]]);
				}
#if SHOW_DEB
				cout << " the temp crocover:" << endl;
				PrintMatrix3(CroCovarM / Index_corr_src.size());
				cout << " the ave_ICP:";
				cout << "(" << ave_ICP.x << "," << ave_ICP.y << "," << ave_ICP.z << ")" << endl;
				cout << "the ave_source:";
				cout << "(" << ave_source.x << "," << ave_source.y << "," << ave_source.z << ")" << endl;
				cout << "the matrix by miu:" << endl;
				PrintMatrix3(PointDot(ave_ICP, ave_source));	
#endif
				CroCovarM = CroCovarM / Index_corr_src.size() - PointDot(ave_ICP, ave_source);
#if SHOW_DEB == 1
				cout << "the size of corr points  : " << Index_corr_src.size() << endl;
				cout << "the cross covariance matrix :" << endl;
				PrintMatrix3(CroCovarM);
#endif
				// calculate the big Q matrix (4x4)
					// first find the vector Delta (3X1)
				Eigen::Matrix3d antiCroCrovarM = CroCovarM - CroCovarM.transpose();
				vector<double> delta(3, 0);
				delta[0] = antiCroCrovarM(1, 2);
				delta[1] = antiCroCrovarM(2, 0);
				delta[2] = antiCroCrovarM(0, 1);
				Eigen::Matrix4d Q = Eigen::Matrix4d::Zero(); // big Q matrix (4x4)
				Q(0, 0) = CroCovarM.trace();
				Q.block<3, 3>(1, 1) = CroCovarM + CroCovarM.transpose() - CroCovarM.trace() * Eigen::Matrix3d::Identity();
				Q(1, 0) = delta[0]; Q(2, 0) = delta[1]; Q(3, 0) = delta[2];
				Q(0, 1) = delta[0]; Q(0, 2) = delta[1]; Q(0, 3) = delta[2];
#if SHOW_DEB == 1
				cout << "the big Q matricx:" << endl;
				PrintMatrix4(Q);
#endif
				// capture the unit eigenvector corresponding to the maximun eigenvalue of matrix Q
				Eigen::EigenSolver<Eigen::Matrix4d>ev(Q,true);
				    // contract the index of the maximum eigenvalue
				float maxEV = MIN;
				int IndMaxEV = -1;
				for (int i = 0; i < 4; ++i) {
					float tempEV = ev.eigenvalues()[i].real();
					if (tempEV > maxEV) {
						maxEV = tempEV;
						IndMaxEV = i;
					}
				}
					// calculate the unit eigenvector
				
				unitEV[0] = ev.eigenvectors()(0,IndMaxEV).real();
				unitEV[1] = ev.eigenvectors()(1,IndMaxEV).real();
				unitEV[2] = ev.eigenvectors()(2,IndMaxEV).real();
				unitEV[3] = ev.eigenvectors()(3,IndMaxEV).real();
				float ModUnitEV = sqrt(pow(unitEV[0], 2) + pow(unitEV[1], 2) + pow(unitEV[2], 2) + pow(unitEV[3], 2));
				unitEV[0] /= ModUnitEV; unitEV[1] /= ModUnitEV; unitEV[2] /= ModUnitEV; unitEV[3] /= ModUnitEV;
#if SHOW_DEB
				cout << "the eigenvalue: " << endl << ev.eigenvalues() << endl;
				cout << "the eigenvector: " << endl << ev.eigenvectors() << endl;
				cout << "the index of max eigenvalue: " << IndMaxEV << endl;
				cout << "the corresponding eigenvector : ";
				for (int i = 0; i < unitEV.size(); ++i) {
					cout << unitEV[i] << " ";
				}
				cout << endl;
#endif
				// calculate the corresponding rotation matrix and translation matrix
				QuaterionToRotation(unitEV, rotation_matrix);
#if SHOW_DEB
				cout << "rotation acquired by paper:" << endl;
				PrintMatrix3(rotation_matrix.cast<double>());
#endif
				vector<float> miuX(3, 0);
				vector<float> miuP(3, 0);
				miuX[0] = ave_source.x; miuX[1] = ave_source.y; miuX[2] = ave_source.z;
				miuP[0] = ave_ICP.x; miuP[1] = ave_ICP.y; miuP[2] = ave_ICP.z;
				vector<float>temp(3, 0);
				temp = RotationXVector3(rotation_matrix, miuP);
				
				translation_matrix[0] = miuX[0] - temp[0];
				translation_matrix[1] = miuX[1] - temp[1];
				translation_matrix[2] = miuX[2] - temp[2];
			}
			else {

			}
		}

匹配

		// alian
		void align(PointTargetPtr& cloud_ICP) {
			*cloud_ICP = *cloud_target;
			int iteration_time = 0;
			if (methodMS == QUATERNION) {
				for (int i = 0; i < m_iterations; ++i) {
					Scalar lastSquareMeanError = SquareMeanError;
					ComputeCorrespondence(cloud_ICP);
#if SHOW_DEB
					PrintSME();
#endif
					if (abs(lastSquareMeanError - SquareMeanError) < epsilon) {
						break;
					}
					++iteration_time;
					ComputeRegistration(cloud_ICP);
					ApplyRegistration(cloud_ICP);
				}
				final_iterations = iteration_time;
#if SHOW_DEB
				cout << "total iteration time : " << iteration_time << endl;
				cout << "the last square mean error : " << SquareMeanError << endl;
#endif
			}
			else if () {
				}
			}
		}

效果

我用自己复现的程序和pcl中封装的库进行测试，效果几乎相同，pcl的速度更快。

最新代码下载