将ply点云文件中的各个点按最小生成树的顺序连成线段的C++代码
以下是基于PCL库的C++代码：
#include <iostream>
#include <pcl/io/ply_io.h>
#include <pcl/point_types.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/common/centroid.h>
#include <pcl/common/common.h>
#include <pcl/common/distances.h>
#include <pcl/common/transforms.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/segmentation/extract_clusters.h>
#include <pcl/filters/statistical_outlier_removal.h>
#include <pcl/registration/icp.h>
#include <pcl/registration/ndt.h>
#include <pcl/registration/gicp.h>
#include <pcl/registration/transforms.h>
#include <pcl/features/normal_3d_omp.h>
#include <pcl/features/fpfh_omp.h>
#include <pcl/features/shot_omp.h>
#include <pcl/registration/correspondence_rejection_sample_consensus.h>
#include <pcl/registration/transformation_estimation_svd.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/surface/mls.h>
int main(int argc, char **argv)
{
if (argc < 2)
{
std::cout << "Please specify the PLY file as input." << std::endl;
return -1;
}
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
if (pcl::io::loadPLYFile<pcl::PointXYZ>(argv[1], *cloud) == -1)
{
std::cout << "Failed to load PLY file." << std::endl;
return -1;
}
// Compute normals
pcl::NormalEstimationOMP<pcl::PointXYZ, pcl::Normal> ne;
ne.setInputCloud(cloud);
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>());
ne.setSearchMethod(tree);
pcl::PointCloud<pcl::Normal>::Ptr normals(new pcl::PointCloud<pcl::Normal>);
ne.setRadiusSearch(0.05);
ne.compute(*normals);
// Compute FPFH features
pcl::FPFHEstimationOMP<pcl::PointXYZ, pcl::Normal, pcl::FPFHSignature33> fpfh;
fpfh.setInputCloud(cloud);
fpfh.setInputNormals(normals);
fpfh.setSearchMethod(tree);
pcl::PointCloud<pcl::FPFHSignature33>::Ptr fpfhs(new pcl::PointCloud<pcl::FPFHSignature33>());
fpfh.setRadiusSearch(0.05);
fpfh.compute(*fpfhs);
// Compute correspondences
pcl::CorrespondencesPtr correspondences(new pcl::Correspondences());
pcl::KdTreeFLANN<pcl::FPFHSignature33> match_search;
match_search.setInputCloud(fpfhs);
for (size_t i = 0; i < fpfhs->size(); ++i)
{
std::vector<int> neigh_indices(1);
std::vector<float> neigh_sqr_dists(1);
if (!pcl_isfinite((*fpfhs)[i].histogram[0])) //skipping NaNs
{
continue;
}
int found_neighs = match_search.nearestKSearch(*fpfhs, i, 1, neigh_indices, neigh_sqr_dists);
if (found_neighs == 1 && neigh_sqr_dists[0] < 0.25f) // 0.25 squared distance threshold
{
pcl::Correspondence correspondence(neigh_indices[0], static_cast<int>(i), neigh_sqr_dists[0]);
correspondences->push_back(correspondence);
}
}
// Reject correspondences based on RANSAC
pcl::registration::CorrespondenceRejectorSampleConsensus<pcl::PointXYZ> sac;
sac.setInputSource(cloud);
sac.setInlierThreshold(0.05);
sac.setMaximumIterations(10000);
pcl::CorrespondencesPtr correspondences_filtered(new pcl::Correspondences());
sac.getRemainingCorrespondences(*correspondences, *correspondences_filtered);
// Extract lines from correspondences
std::vector<pcl::PointXYZ> points;
std::vector<std::pair<int, int>> lines;
for (size_t i = 0; i < correspondences_filtered->size(); ++i)
{
int idx1 = (*correspondences_filtered)[i].index_query;
int idx2 = (*correspondences_filtered)[i].index_match;
pcl::PointXYZ p1 = cloud->points[idx1];
pcl::PointXYZ p2 = cloud->points[idx2];
bool added1 = false;
bool added2 = false;
int idx1_new = -1;
int idx2_new = -1;
for (size_t j = 0; j < points.size(); ++j)
{
pcl::PointXYZ p = points[j];
if (!added1 && pcl::euclideanDistance(p1, p) < 0.05)
{
added1 = true;
idx1_new = j;
}
if (!added2 && pcl::euclideanDistance(p2, p) < 0.05)
{
added2 = true;
idx2_new = j;
}
if (added1 && added2)
{
break;
}
}
if (!added1)
{
points.push_back(p1);
idx1_new = points.size() - 1;
}
if (!added2)
{
points.push_back(p2);
idx2_new = points.size() - 1;
}
lines.push_back(std::make_pair(idx1_new, idx2_new));
}
// Compute minimum spanning tree
struct Edge
{
int u, v;
double w;
bool operator<(const Edge &other) const
{
return w < other.w;
}
};
std::vector<Edge> edges;
for (size_t i = 0; i < lines.size(); ++i)
{
int u = lines[i].first;
int v = lines[i].second;
double w = pcl::euclideanDistance(points[u], points[v]);
edges.push_back({u, v, w});
}
std::vector<Edge> mst;
std::vector<int> parent(points.size(), -1);
std::sort(edges.begin(), edges.end());
for (size_t i = 0; i < edges.size(); ++i)
{
int u = edges[i].u;
int v = edges[i].v;
double w = edges[i].w;
int parent_u = u;
while (parent[parent_u] != -1)
{
parent_u = parent[parent_u];
}
int parent_v = v;
while (parent[parent_v] != -1)
{
parent_v = parent[parent_v];
}
if (parent_u != parent_v)
{
mst.push_back(edges[i]);
parent[parent_u] = parent_v;
}
}
// Visualize results
pcl::visualization::PCLVisualizer viewer("PLY Viewer");
viewer.addCoordinateSystem(0.1);
viewer.initCameraParameters();
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_points(new pcl::PointCloud<pcl::PointXYZ>);
for (size_t i = 0; i < points.size(); ++i)
{
cloud_points->push_back(points[i]);
}
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_lines(new pcl::PointCloud<pcl::PointXYZ>);
for (size_t i = 0; i < mst.size(); ++i)
{
int u = mst[i].u;
int v = mst[i].v;
pcl::PointXYZ p1 = points[u];
pcl::PointXYZ p2 = points[v];
cloud_lines->push_back(p1);
cloud_lines->push_back(p2);
std::stringstream ss;
ss << "line_" << i;
viewer.addLine<pcl::PointXYZ>(p1, p2, ss.str());
}
viewer.addPointCloud<pcl::PointXYZ>(cloud_points, "points");
viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "points");
viewer.addPointCloud<pcl::PointXYZ>(cloud_lines, "lines");
viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 1, 0, 0, "lines");
viewer.spin();
return 0;
}
该代码会读取PLY文件，计算点云的法向量和FPFH特征，通过RANSAC算法进行特征匹配，然后根据匹配得到的点对计算最小生成树，并可视化结果。其中pcl::euclideanDistance函数用于计算两个点之间的欧几里得距离，pcl::visualization::PCLVisualizer类用于可视化点云和线段
原文地址: https://www.cveoy.top/t/topic/eCuE 著作权归作者所有。请勿转载和采集!