PCL PCA主成分分析计算点云最小包围盒（Common_PCA）

注意：本示例为手写计算点云最小包围盒

与PCL自带的计算OBB包围盒的区别为：

本示例（1）适合计算实际的小场景物体的最小包围盒。如【人、树、车子】等；

（2）计算更快；

工作中若需计算点云最小包围盒，一般使用此示例（就算会损失一些精确性）。

前置知识：包围盒及特征向量等、协方差矩阵及PCA主成分分析等

1.原理

通过计算点云的特征值和特征向量，按照特征值和特征向量的相关参数，计算得到最小包围盒的参数值。

（1）计算点云的协方差矩阵，得到特征值和特征向量T。

（2）三个特征向量T1,T2,T3为当前点云的最小包围盒状态下的xyz轴。

按照特征向量T的xyz方向，旋转点云，使点云的XYZ（特征向量的XYZ方向）轴和系统的XYZ（即x(1,0,0)，y(0,1,0)，z(0,0,1)）轴对齐。并记录所使用的旋转矩阵M。

此时，旋转后的点云的姿态即为最小包围盒姿态。

类似于下图，想象将这个小象的坐标轴旋转的和途中的坐标轴对齐。

（3）按照计算AABB盒的方法，计算旋转后的点云包围盒。

（4）计算上文旋转矩阵M的逆矩阵，将AABB盒的各参数（如顶点）转换到原始点云的位置坐标内。

得到的包围盒参数即为最小包围盒参数。

2.使用场景

需要计算最小包围盒的场景。

之前计算悬垂绝缘子的方向和形态的时候用过。

3.注意事项

（1）实际工作中，使用该方法计算最小包围盒。

（2）适合小场景，类似于某个小物体的最小包围盒计算，不适合大场景，大物体的最小包围盒计算。（不准确）

（3）不适合扁平点云的最小包围盒计算（协方差矩阵计算会不准确）。

4.关键函数

本示例函数复杂，学习的话，需要整篇代码理一下，不能只学某些函数。

5.代码

方法一为调用PCL接口，方法二为手动计算协方差矩阵，得到最小包围盒。

#include <iostream>
#include <pcl/common/pca.h>
#include <pcl/io/pcd_io.h>
#include <pcl/common/common.h>
#include <boost/thread/thread.hpp>
#include <pcl/visualization/pcl_visualizer.h>
// #define COMMON_PCA

int main()
{
	/****************添加点云********************/
	// 原始点云
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::io::loadPCDFile("D:/code/csdn/data/person4.pcd", *cloud);   // 加载原始点云数据
	

	// 计算pca主成分分析（方法一）
#ifdef COMMON_PCA
	pcl::PCA<pcl::PointXYZ> pca;												// 计算PCA主成分分析
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloudPcaProj(new pcl::PointCloud<pcl::PointXYZ>);		// 投影后的结果点云
	pca.setInputCloud(cloud);
	pca.project(*cloud, *cloudPcaProj);											// 计算投影后点云
	std::cout << "特征向量: " << pca.getEigenVectors() << std::endl;			// 计算特征向量
	std::cout << "特征值: " << pca.getEigenValues() << std::endl;				// 计算特征值
#endif

	// 计算PCA主成分分析（方法二）
	Eigen::Vector4f pcaCentroid;						
	pcl::compute3DCentroid(*cloud, pcaCentroid);											// 计算点云质心
	Eigen::Matrix3f covariance;
	pcl::computeCovarianceMatrixNormalized(*cloud, pcaCentroid, covariance);				// 计算协方差矩阵
	Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> eigen_solver(covariance, Eigen::ComputeEigenvectors);
	Eigen::Matrix3f eigenVectorsPCA = eigen_solver.eigenvectors();							// 计算特征向量
	Eigen::Vector3f eigenValuesPCA = eigen_solver.eigenvalues();							// 计算特征值
	eigenVectorsPCA.col(2) = eigenVectorsPCA.col(0).cross(eigenVectorsPCA.col(1));			// 校正主方向间垂直（两两叉乘，类似于OpenGL中的计算相机的流程）
	eigenVectorsPCA.col(0) = eigenVectorsPCA.col(1).cross(eigenVectorsPCA.col(2));
	eigenVectorsPCA.col(1) = eigenVectorsPCA.col(2).cross(eigenVectorsPCA.col(0));

	// PCA的应用（计算最小包围盒）
	// 计算协方差矩阵的变换矩阵及逆变换矩阵
	Eigen::Matrix4f tm = Eigen::Matrix4f::Identity();										// 变换矩阵
	Eigen::Matrix4f tm_inv = Eigen::Matrix4f::Identity();									// 逆变换矩阵
	tm.block<3, 3>(0, 0) = eigenVectorsPCA.transpose();										// 计算协方差矩阵所得的变换矩阵
	tm.block<3, 1>(0, 3) = -1.0f * (eigenVectorsPCA.transpose()) * (pcaCentroid.head<3>());
	tm_inv = tm.inverse();																	// 计算协方差矩阵得到的逆变换矩阵

	// 计算最小包围盒
	pcl::PointCloud<pcl::PointXYZ>::Ptr transformedCloud(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::transformPointCloud(*cloud, *transformedCloud, tm);								// 将点云转换为协方差矩阵所在的位置（也就是将点云转正，其主方向的XYZ轴和坐标系的XYZ轴平行，通过对此计算包围盒，得到最终结果）

	// 计算此时的点云包围盒(按照协方差矩阵转正之后的包围盒)
	pcl::PointXYZ min_p1, max_p1;										// 转换后包围盒最大最小位置的参数
	pcl::getMinMax3D(*transformedCloud, min_p1, max_p1);
	Eigen::Vector3f centerTrans = (min_p1.getVector3fMap() + max_p1.getVector3fMap()) / 2.0;			// 转换后中心点
	Eigen::Vector3f centerOriginal;										// 原始中心点
	pcl::transformPoint(centerTrans, centerOriginal, Eigen::Affine3f(tm_inv));				// 通过逆矩阵转换回去

	// 生成点云最小包围盒参数
	// 顶点计算
	pcl::PointCloud<pcl::PointXYZ>::Ptr transVertexCloud(new pcl::PointCloud<pcl::PointXYZ>);			// 先计算变换后点云包围盒的6个顶点
	transVertexCloud->width = 6;
	transVertexCloud->height = 1;
	transVertexCloud->is_dense = false;
	transVertexCloud->points.resize(transVertexCloud->width * transVertexCloud->height);
	transVertexCloud->points[0].x = max_p1.x;
	transVertexCloud->points[0].y = max_p1.y;
	transVertexCloud->points[0].z = max_p1.z;
	transVertexCloud->points[1].x = max_p1.x;
	transVertexCloud->points[1].y = max_p1.y;
	transVertexCloud->points[1].z = min_p1.z;
	transVertexCloud->points[2].x = max_p1.x;
	transVertexCloud->points[2].y = min_p1.y;
	transVertexCloud->points[2].z = min_p1.z;
	transVertexCloud->points[3].x = min_p1.x;
	transVertexCloud->points[3].y = max_p1.y;
	transVertexCloud->points[3].z = max_p1.z;
	transVertexCloud->points[4].x = min_p1.x;
	transVertexCloud->points[4].y = min_p1.y;
	transVertexCloud->points[4].z = max_p1.z;
	transVertexCloud->points[5].x = min_p1.x;
	transVertexCloud->points[5].y = min_p1.y;
	transVertexCloud->points[5].z = min_p1.z;

	pcl::PointCloud<pcl::PointXYZ>::Ptr minVertexCloud(new pcl::PointCloud<pcl::PointXYZ>);			// 最小包围盒的顶点
	pcl::transformPointCloud(*transVertexCloud, *minVertexCloud, tm_inv);

	// 中心点
	const Eigen::Vector3f bboxCenter(centerOriginal);							// 最小包围盒中心点
	const Eigen::Quaternionf bboxQua(tm_inv.block<3, 3>(0, 0));					// 从垂直于xyz的轴逆转换回来的四元数
	double width = (max_p1.getVector3fMap() - min_p1.getVector3fMap())(0);		// 宽
	double height = (max_p1.getVector3fMap() - min_p1.getVector3fMap())(1);		// 高
	double depth = (max_p1.getVector3fMap() - min_p1.getVector3fMap())(2);		// 深

	/****************展示********************/
	// 点云
	pcl::visualization::PCLVisualizer viewer;
	pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> cloudColor(cloud, 255, 255, 0);
	viewer.addPointCloud(cloud, cloudColor, "cloud");
	// 顶点
	pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> vertexColor(cloud, 255, 0, 0);
	viewer.addPointCloud(minVertexCloud, vertexColor, "vertex");
	// 包围盒
	viewer.addCube(bboxCenter, bboxQua, width, height, depth, "minBbox");
	viewer.setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_REPRESENTATION, pcl::visualization::PCL_VISUALIZER_REPRESENTATION_WIREFRAME, "minBbox");
	viewer.setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 0.0, 1.0, 0.0, "minBbox");	
	// 原始坐标轴
	viewer.addCoordinateSystem(0.5);

	while (!viewer.wasStopped())
	{
		viewer.spinOnce();
	}

	return 0;
}

6.结果展示

坐标轴为系统坐标轴，即x(1,0,0)，y(0,1,0)，z(0,0,1)。

绿色框线即为计算出的最小包围盒。