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

原创已于 2022-05-07 16:07:54 修改 · 1.9k 阅读

10 ·

CC 4.0 BY-SA版权

文章标签：

#c++ #计算机视觉 #自动驾驶

于 2022-05-07 15:36:56 首次发布

该代码段展示了如何利用PCL库中的PCA算法计算点云数据的法向量。首先，通过KdTree构建索引，然后对每个点的K邻近点进行PCA分解，计算其法向量。最后，将所有点的法向量存储到PointCloud<Normal>对象中。此过程对于理解和处理3D点云的表面特性至关重要。

导入所需库

#include <pcl/point_types.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/common/transforms.h>
#include <Eigen/Eigenvalues>

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

核心代码

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

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

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


}