C++使用Eigen库求解点云法向量全部代码

#include 
#include 
#include 
#include 

using namespace std;
using namespace pcl;
using namespace Eigen;

void pcaNormal(const PointCloud::Ptr& cloud, const int& k_neighbors, PointCloud::Ptr& normals)
//cloud为所求取的点云，k_neighbors为输入的Kd树的k值，normals为点云法向量
{
	//1.构建kd树索引
	KdTreeFLANN::Ptr kdtree(new KdTreeFLANN);
	kdtree->setInputCloud(cloud);

	//2. 法向量实例化，若输入为空指针，则进行空间分配，若输入法向量非空，则使其为大小为0
	if (normals == NULL)
		normals = PointCloud::Ptr(new PointCloud);
	if (!normals->empty())
		normals->resize(0);
		
	//3.对输入点云中的每个点进行遍历，对于每个点，使用PCA求取法向量
	for (const auto& point : *cloud)
	{
		//3.1 初始化两个向量，用来记录K邻近点的编号以及距离平方
		PointXYZ searchPoint = point; 
		vector KNNIndices(k_neighbors);
		vector KNNSquareDistance(k_neighbors);

		//3.2 对K领域进行PCA分解，计算法向量
		if (kdtree->nearestKSearch(searchPoint, k_neighbors, KNNIndices, KNNSquareDistance) > 0)
		{
			//如果上面的值大于0，则搜索成功
			PointCloud::Ptr neighborPoints(new PointCloud);
			//提取现在点的邻近点至neighborPoints中
			pcl::copyPointCloud(*cloud, KNNIndices, *neighborPoints);

			Eigen::Vector4f pcaCentroid;
			Eigen::Matrix3f covariance;
			Eigen::Matrix3f eigenVectorsPCA;
			Eigen::Vector3f eigenValuesPCA;

			compute3DCentroid(*neighborPoints, pcaCentroid);//计算重心
			computeCovarianceMatrixNormalized(*neighborPoints, pcaCentroid, covariance);//compute the covariance matrix of point cloud
			Eigen::SelfAdjointEigenSolver eigen_solver(covariance, Eigen::ComputeEigenvectors);//define an eigen solver
			eigenVectorsPCA = eigen_solver.eigenvectors();//compute eigen vectors
			eigenValuesPCA = eigen_solver.eigenvalues();//compute eigen values;

			Normal normal(eigenVectorsPCA(0), eigenVectorsPCA(1), eigenVectorsPCA(2));
			normals->push_back(normal);
		}
		else
			normals->push_back(Normal());
	}


}