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 最大與最小主方向對齊