PCL 计算距离最大的两个点并保存

目录

• 一、原理介绍
• • 1. 问题定义
  • 2. 计算方法
• 二、代码实现
• 三、结果展示

一、原理介绍

1. 问题定义

给定包含n nn个点的三维点云集合P = { p 0 , p 1 , . . . , p n − 1 } P = \{p_0, p_1, ..., p_{n-1}\}P={p0​,p1​,...,pn−1​}，点云的最大距离定义为所有点对之间欧氏距离的全局最大值：
d m a x = max ⁡ { ∥ p i − p j ∥ 2 ∣ 0 ≤ i , j < n } d_{max} = \max\{\|p_i - p_j\|_2 \mid 0 \le i, j < n\}dmax​=max{∥pi​−pj​∥2​∣0≤i,j<n}
对应的两个点p i p_ipi​、p j p_jpj​即为距离最远的点对。

2. 计算方法

getMaxSegment采用暴力枚举法求解，核心思想是遍历所有不重复的点对，逐一计算空间距离并记录最大值与对应端点。为了提升计算效率，算法做了两处关键的工程优化：

1. 利用距离对称性减少循环次数：点对( ( i , j ) ) ((i,j))((i,j))与( ( j , i ) ) ((j,i))((j,i))距离相同，因此内层循环从i ii开始遍历，仅计算j ≥ i j \geq ij≥i的点对，计算量直接减半。
2. 平方距离比较，最后统一开方：比较距离大小时，平方距离的大小关系与真实欧氏距离完全一致。算法全程先比较平方距离，仅在最终返回结果时做一次开方运算，大幅减少sqrt函数的调用开销。

算法整体时间复杂度为O ( n 2 ) O(n^2)O(n2)，空间复杂度为( O ( 1 ) ) (O(1))(O(1))（仅使用有限变量记录最大值与索引）。

二、代码实现

#include<iostream>#include<pcl/io/pcd_io.h>#include<pcl/point_types.h>#include<pcl/common/distances.h>#include<boost/thread/thread.hpp>#include<pcl/visualization/pcl_visualizer.h>usingnamespacestd;intmain(intargc,char**argv){// 加载原始点云pcl::PointCloud<pcl::PointXYZ>::Ptrcloud(newpcl::PointCloud<pcl::PointXYZ>);if(pcl::io::loadPCDFile<pcl::PointXYZ>("temp//vault_raw_34_convex.pcd",*cloud)==-1){PCL_ERROR("加载点云失败，请检查文件路径是否正确！\n");return-1;}pcl::PointXYZ pmin,pmax;// 计算点云中距离最大的两个端点，返回最大距离doublemax_distance=pcl::getMaxSegment(*cloud,pmin,pmax);cout<<"点云集合中的最大距离为："<<max_distance<<" 米"<<endl;// -------------------------- 保存最远两点为PCD文件 --------------------------// 创建只包含两个最远点的新点云pcl::PointCloud<pcl::PointXYZ>::Ptrextreme_points_cloud(newpcl::PointCloud<pcl::PointXYZ>);extreme_points_cloud->push_back(pmin);// 第一个端点extreme_points_cloud->push_back(pmax);// 第二个端点// 显式设置点云属性（规范写法，push_back 也会自动维护）extreme_points_cloud->width=2;extreme_points_cloud->height=1;extreme_points_cloud->is_dense=true;// 保存为PCD文件string save_path="temp//max_distance_points.pcd";intsave_result=pcl::io::savePCDFileBinary(save_path,*extreme_points_cloud);if(save_result==0){cout<<"已将距离最大的两个点成功保存到："<<save_path<<endl;}else{cerr<<"保存PCD文件失败，错误码："<<save_result<<endl;return-1;}// -------------------------- 结果可视化 --------------------------boost::shared_ptr<pcl::visualization::PCLVisualizer>viewer(newpcl::visualization::PCLVisualizer("Viewer"));viewer->setBackgroundColor(0,0,0);viewer->setWindowName("getMaxSegment");pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>single_color(cloud,0,0,255);// 蓝色viewer->addPointCloud<pcl::PointXYZ>(cloud,single_color,"sample cloud");// 绘制最远两点之间的箭头和标注viewer->addArrow<pcl::PointXYZ>(pmin,pmax,0,255,0,true,"arrow",0);viewer->addText3D("Point1",pmin,0.05,255,0,0);viewer->addText3D("Point2",pmax,0.05,255,0,0);while(!viewer->wasStopped()){viewer->spinOnce(100);boost::this_thread::sleep(boost::posix_time::microseconds(100000));}return0;}

三、结果展示