PCL点云库——PCA粗配准

PCA粗配准

　　主成分分析法( Principal Component Analysis，简称PCA)是使数据简化的算法，通过揭露数据内部的主要分布方向，减少了数据集的维数，从而保留了点云集中贡献最大的特征，更好地解释数据的变化规律。PCA 算法进行点云配准是以主方向为依据，根据两点云的主方向计算出对应的三个旋转和三个平移量，可大致对齐两点云数据。
　　PCA算法流程如下：
　　（1）原始特征矩阵归一化处理（假设M和样本，每个样本n个特征，则对M*N的X数据，进行零均值化，即减去这一列的均值）；
　　（2）求取归一化处理后特征矩阵的协方差矩阵；
　　（3）计算协方差矩阵的特征值及其对应的特征向量；
　　（4）将特征向量按对应特征值大小从上到下按行排列成矩阵，取前k行组成矩阵P；
　　（5）Y=PX即为降维到k维后的数据。
　　文末给出了配准代码，配准效果如图1、图2、图3、图4所示

#include <pcl/visualization/cloud_viewer.h>
#include <pcl/io/pcd_io.h>
#include <pcl/common/transforms.h>

//计算向量u与向量v之间的夹角
void calRotation(Eigen::Vector3f u, Eigen::Vector3f v, double &angle, Eigen::Vector3f &vec);
//获取罗德里格斯旋转矩阵
Eigen::Matrix4f RodriguesMatrixTranslation(Eigen::Vector3f n, double angle);

int main()
{
	pcl::PointCloud<pcl::PointXYZ>::Ptr target(new pcl::PointCloud<pcl::PointXYZ>());//目标点云
	pcl::PointCloud<pcl::PointXYZ>::Ptr source(new pcl::PointCloud<pcl::PointXYZ>());//源点云
	pcl::io::loadPCDFile("cloud\\target.pcd", *target);
	pcl::io::loadPCDFile("cloud\\source.pcd", *source);

	Eigen::Vector4f pcaCentroidtarget;//容量为4的列向量
	pcl::compute3DCentroid(*target, pcaCentroidtarget);//计算目标点云质心
	Eigen::Matrix3f covariance;//创建一个3行3列的矩阵，里面每个元素均为float类型
	pcl::computeCovarianceMatrixNormalized(*target, pcaCentroidtarget, covariance);//计算目标点云协方差矩阵
	Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> eigen_solver(covariance, Eigen::ComputeEigenvectors);//构造一个计算特定矩阵的类对象
	Eigen::Matrix3f eigenVectorsPCA = eigen_solver.eigenvectors();//eigenvectors计算特征向量
	Eigen::Vector3f eigenValuesPCA = eigen_solver.eigenvalues();//eigenvalues计算特征值

	Eigen::Vector4f pcaCentroidsource;
	pcl::compute3DCentroid(*source, pcaCentroidsource);//计算源点云质心
	Eigen::Matrix3f model_covariance;
	pcl::computeCovarianceMatrixNormalized(*source, pcaCentroidsource, model_covariance);//计算源点云协方差矩阵
	Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> modeleigen_solver(model_covariance, Eigen::ComputeEigenvectors);
	Eigen::Matrix3f model_eigenVectorsPCA = modeleigen_solver.eigenvectors();
	Eigen::Vector3f model_eigenValuesPCA = modeleigen_solver.eigenvalues();

	pcl::PointCloud<pcl::PointXYZ>::Ptr target_t(new pcl::PointCloud<pcl::PointXYZ>);
	//平移目标点云，将目标点云通过质心平移到原点
	Eigen::Matrix4f translation_t = Eigen::Matrix4f::Identity();
	//设置矩阵的元素
	translation_t(0, 3) = -pcaCentroidtarget[0];
	translation_t(1, 3) = -pcaCentroidtarget[1];
	translation_t(2, 3) = -pcaCentroidtarget[2];
	pcl::transformPointCloud(*target, *target_t, translation_t);

	pcl::PointCloud<pcl::PointXYZ>::Ptr source_t(new pcl::PointCloud<pcl::PointXYZ>);
	//平移源点云，将源点云通过质心平移到原点
	Eigen::Matrix4f translation_s = Eigen::Matrix4f::Identity();
	//设置矩阵的元素
	translation_s(0, 3) = -pcaCentroidsource[0];
	translation_s(1, 3) = -pcaCentroidsource[1];
	translation_s(2, 3) = -pcaCentroidsource[2];
	pcl::transformPointCloud(*source, *source_t, translation_s);

	pcl::PointCloud<pcl::PointXYZ>::Ptr translationCloud(new pcl::PointCloud<pcl::PointXYZ>);
	Eigen::Vector3f n0, n1, n2;
	double angle0, angle1, angle2;
	calRotation(eigenVectorsPCA.col(0), model_eigenVectorsPCA.col(0), angle0, n0);
	calRotation(eigenVectorsPCA.col(1), model_eigenVectorsPCA.col(1), angle1, n1);
	calRotation(eigenVectorsPCA.col(2), model_eigenVectorsPCA.col(2), angle2, n2);
	Eigen::Matrix4f transform0(Eigen::Matrix4f::Identity());
	Eigen::Matrix4f transform1(Eigen::Matrix4f::Identity());
	Eigen::Matrix4f transform2(Eigen::Matrix4f::Identity());
	transform0 = RodriguesMatrixTranslation(n0, angle0);
	transform1 = RodriguesMatrixTranslation(n1, angle1);
	transform2 = RodriguesMatrixTranslation(n2, angle2);
	pcl::transformPointCloud(*source_t, *translationCloud, transform2);  //源点云整体旋转，最大主方向对齐
	pcl::transformPointCloud(*translationCloud, *translationCloud, transform0);  //源点云整体旋转，最小主方向对齐
	//pcl::transformPointCloud(*translationCloud, *translationCloud, transform1);  //源点云整体旋转，

	pcl::visualization::PCLVisualizer viewer("PCLVisualizer");
	viewer.initCameraParameters();

	int v1(0);
	viewer.createViewPort(0.0, 0.0, 0.5, 1.0, v1);
	viewer.setBackgroundColor(128.0 / 255.0, 138.0 / 255.0, 135.0 / 255.0, v1);
	viewer.addText("Cloud before transforming", 10, 10, "v1 test", v1);
	viewer.addCoordinateSystem(0.5, v1);
	pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> color_v1(target, 0, 255, 0);
	viewer.addPointCloud<pcl::PointXYZ>(target, color_v1, "color_v1", v1);
	viewer.addPointCloud<pcl::PointXYZ>(source, "source", v1);

	int v2(0);
	viewer.createViewPort(0.5, 0.0, 1.0, 1.0, v2);
	viewer.setBackgroundColor(128.0 / 255.0, 138.0 / 255.0, 135.0 / 255.0, v2);
	viewer.addText("Cloud after transforming", 10, 10, "v2 test", v2);
	viewer.addCoordinateSystem(0.5, v2);
	pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> color_v2(target_t, 0, 255, 0);
	viewer.addPointCloud<pcl::PointXYZ>(target_t, color_v2, "color_v2", v2);
	viewer.addPointCloud<pcl::PointXYZ>(translationCloud, "translationCloud", v2);

	while (!viewer.wasStopped())
	{
		//在此处可以添加其他处理  
		viewer.spinOnce(100);
	}
	return 0;
}

//计算向量u与向量v之间的夹角
void calRotation(Eigen::Vector3f u, Eigen::Vector3f v, double &angle, Eigen::Vector3f &vec)
{
	angle = acos(u.dot(v) / (u.norm()*v.norm()));
	if (angle > M_PI / 2)
	{
		u = -u;
		angle = M_PI - angle;
	}
	float i, j, k;
	i = u(1)*v(2) - u(2)*v(1), j = v(0)*u(2) - v(2)*u(0), k = u(0)*v(1) - u(1)*v(0);
	vec << i, j, k;
	double value = sqrt(i*i + j*j + k*k);
	vec(0) = vec(0) / value;
	vec(1) = vec(1) / value;
	vec(2) = vec(2) / value;
}

//获取罗德里格斯旋转矩阵
Eigen::Matrix4f RodriguesMatrixTranslation(Eigen::Vector3f n, double angle)
{
	//罗德里格斯公式求旋转矩阵
	Eigen::Matrix4f x_transform(Eigen::Matrix4f::Identity());
	x_transform(0, 0) = cos(angle) + n(0)*n(0)*(1 - cos(angle));
	x_transform(1, 0) = n(2)*sin(angle) + n(0)*n(1)*(1 - cos(angle));
	x_transform(2, 0) = -n(1)*sin(angle) + n(0)*n(2)*(1 - cos(angle));
	x_transform(0, 1) = n(0)*n(1)*(1 - cos(angle)) - n(2)*sin(angle);
	x_transform(1, 1) = cos(angle) + n(1)*n(1)*(1 - cos(angle));
	x_transform(2, 1) = n(0)*sin(angle) + n(1)*n(2)*(1 - cos(angle));
	x_transform(0, 2) = n(1)*sin(angle) + n(0)*n(2)*(1 - cos(angle));
	x_transform(1, 2) = -n(0)*sin(angle) + n(1)*n(2)*(1 - cos(angle));
	x_transform(2, 2) = cos(angle) + n(2)*n(2)*(1 - cos(angle));

	return  x_transform;
}