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