点云随笔（二）主成分分析计算三维物体长宽高

主成分分析的主要原理如下所示：

       首先将点集中的x,y,z值构造为三维矩阵M:

      然后对矩阵M进行均值化：

      再进行协方差矩阵C的计算：

      计算协方差矩阵的特征值和特征向量，分别将特征值对应的特征向量e单位化：

      然后对其进行基变换： 

     在一维坐标系上对比所得到的矩阵N的最大最小值做差即可得到当前轴向长度，然后对长宽对比后得出长、宽为多少，高度的化则是特征向量第三个特征值所对应的特征向量。

 

#include <algorithm>  
#include <string>
#include <cstdlib>
#include <cstdio>
#include <Eigen\Dense>
#include <Eigen/Eigenvalues>
#include <map>
#include <math.h>
#include<list>
#include <pcl/io/ply_io.h> 
#include <pcl/surface/gp3.h>
#include <pcl/common/common.h>
#include <pcl/common/time.h>
#include <pcl/console/parse.h>
#include <pcl/common/transforms.h>
#include <pcl/io/pcd_io.h>
#include <pcl/filters/conditional_removal.h>
#include <pcl/filters/normal_space.h>
#include <pcl/filters/project_inliers.h>  // 投影滤波 
#include <pcl/geometry/planar_polygon.h>//定义多边形
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/point_cloud.h>
#include <pcl/ModelCoefficients.h>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/features/normal_3d.h>
#include <pcl/kdtree/kdtree.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/segmentation/extract_clusters.h>
#include <pcl/surface/concave_hull.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/sample_consensus/ransac.h>
#include <pcl/sample_consensus/sac_model_plane.h>
#include <pcl/features/boundary.h>
#include <vector>



using Eigen::MatrixXd;
using Eigen::Matrix2d;
using Eigen::Matrix3d;
using namespace Eigen;
using namespace std;

typedef pcl::PointXYZRGB PointT;
typedef pcl::PointCloud<PointT> PointCloudT;

bool compare(double a, double b) {
	return a > b;
}

void sort_double(double* begin, double* end)
{
	sort(begin, end, [](double x, double y) { return x < y; });
}
void
PCA_3D(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr& cloud,double xyz[3])//主成分分析
{
	double width{ 0 };
	int cols = cloud->size();
	MatrixXd M(3, cols);
	Matrix3d C(3, 3);

	double xaverage{ 0 }, yaverage{ 0 }, zaverage{ 0 };
	for (int j = 0; j < cloud->size(); j++) {
		xaverage += cloud->points[j].x;
		yaverage += cloud->points[j].y;
		zaverage += cloud->points[j].z;
	}
	xaverage = xaverage / cloud->size();
	yaverage = yaverage / cloud->size();
	zaverage = zaverage / cloud->size();
	for (int j = 0; j < cloud->size(); j++) {
		M(0, j) = cloud->points[j].x - xaverage;
		M(1, j) = cloud->points[j].y - yaverage;
		M(2, j) = cloud->points[j].z - yaverage;
	}
	C = M * M.transpose();
	C = C / cloud->size();
	//计算特征值和特征向量

	EigenSolver<MatrixXd> es(C);
	MatrixXcd evecs = es.eigenvectors();
	MatrixXd D;//D是特征向量
	D = evecs.real();
	MatrixXcd evals = es.eigenvalues();
	MatrixXd D_temp, N;//N是特征值
	D_temp = evals.real();
	N = D_temp.asDiagonal();
	/*int index{ 0 };
	double maxeigenvalues = N(0, 0);
	for (int i{ 0 }; i < N.rows(); i++)
	{
		if (N(i, i) > maxeigenvalues)
		{
			maxeigenvalues = N(i, i);
			index = i;
		}
	}
	xyz[0] = D(0, index); xyz[1] = D(1, index); xyz[2] = D(2, index);*/
	double nums[3];
	for (int i{ 0 }; i < 3; i++)
	{
		MatrixXd XL(1, 3);

		XL(0, 0) = D(0, i);
		XL(0, 1) = D(1, i);
		XL(0, 2) = D(2, i);
		double w00 = XL(0, 0), w01 = XL(0, 1), w02 = XL(0, 2);
		double ww = sqrt(pow(w00, 2) + pow(w01, 2) + pow(w02, 2));
		XL(0, 0) = w00 / ww;
		XL(0, 1) = w01 / ww;
		XL(0, 2) = w02 / ww;

		MatrixXd BAN(1, cols);
		BAN = XL * M;
		double max_d = BAN.maxCoeff(), min_d = BAN.minCoeff();
		nums[i] = max_d - min_d;
	}
	/*for (int i{ 0 }; i < 3; i++) {
		xyz[i] = nums[i];
	}
	sort(xyz, xyz + 3, compare);*/

	xyz[2] = nums[2];
	if (nums[0] > nums[1]) 
	{
		xyz[0] = nums[0]; xyz[1] = nums[1];
	}
	else 
	{
		xyz[0] = nums[1]; xyz[1] = nums[0];
	}
	//xyz[0]为长度；xyz[1]为宽度;xyz[2]为高度
}



int main() 
{
		pcl::PCDReader reader;
		pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
		std::stringstream ss_f;
		ss_f << "D:\\learning_RE\\simulation_data\\DC_.pcd";
		reader.read(ss_f.str(), *cloud);

		std::cout << "The cloud size is: " << cloud->points.size() << " data points." << std::endl;

		double xyz[3];
		PCA_3D(cloud, xyz);
		cout <<"长: "<< xyz[0] << "  宽: " << xyz[1] << "  高: " << xyz[2] << endl;

	return 0;
}

代码里面的头文件不是全部都用，只是懒得删了。