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

导入所需库

#include 
#include 
#include 
#include 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. 法向量实例化，若输入为空指针，则进行空间分配，若输入法向量非空，则使其为大小为0if (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 cloudEigen::SelfAdjointEigenSolver<Eigen::Matrix3f> eigen_solver(covariance, Eigen::ComputeEigenvectors);//define an eigen solvereigenVectorsPCA = eigen_solver.eigenvectors();//compute eigen vectorseigenValuesPCA = eigen_solver.eigenvalues();//compute eigen values;Normal normal(eigenVectorsPCA(0), eigenVectorsPCA(1), eigenVectorsPCA(2));normals->push_back(normal);}elsenormals->push_back(Normal());}}

本文来自互联网用户投稿，文章观点仅代表作者本人，不代表本站立场，不承担相关法律责任。如若转载，请注明出处。 如若内容造成侵权/违法违规/事实不符，请点击【内容举报】进行投诉反馈！