PCL：计算指定点到点云的最远一点坐标

Posted 2021-09-11 没事就要敲代码

文章目录

• • 1 函数原型
  • 2 实现代码
  • 3 输出结果

1 函数原型

pcl::getMaxDistance (const pcl::PointCloud< PointT > &cloud, const Eigen::Vector4f &pivot_pt, Eigen::Vector4f &max_pt)

2 实现代码

关键代码：

pcl::getMaxDistance(*cloud, pivot_pt, max_pt);

源码：

template<typename PointT> inline void
pcl::getMaxDistance (const pcl::PointCloud<PointT> &cloud, const Eigen::Vector4f &pivot_pt, Eigen::Vector4f &max_pt)
{
  float max_dist = -FLT_MAX;
  int max_idx = -1;
  float dist;
  const Eigen::Vector3f pivot_pt3 = pivot_pt.head<3> ();

  // If the data is dense, we don't need to check for NaN
  if (cloud.is_dense)
  {
    for (size_t i = 0; i < cloud.points.size (); ++i)
    {
      pcl::Vector3fMapConst pt = cloud.points[i].getVector3fMap ();
      dist = (pivot_pt3 - pt).norm ();
      if (dist > max_dist)
      {
        max_idx = int (i);
        max_dist = dist;
      }
    }
  }
  // NaN or Inf values could exist => check for them
  else
  {
    for (size_t i = 0; i < cloud.points.size (); ++i)
    {
      // Check if the point is invalid
      if (!pcl_isfinite (cloud.points[i].x) || !pcl_isfinite (cloud.points[i].y) || !pcl_isfinite (cloud.points[i].z))
        continue;
      pcl::Vector3fMapConst pt = cloud.points[i].getVector3fMap ();
      dist = (pivot_pt3 - pt).norm ();
      if (dist > max_dist)
      {
        max_idx = int (i);
        max_dist = dist;
      }
    }
  }

  if(max_idx != -1)
    max_pt = cloud.points[max_idx].getVector4fMap ();
  else
    max_pt = Eigen::Vector4f(std::numeric_limits<float>::quiet_NaN(),std::numeric_limits<float>::quiet_NaN(),std::numeric_limits<float>::quiet_NaN(),std::numeric_limits<float>::quiet_NaN());
}

完整代码：

#include <iostream>
#include <pcl\\io\\pcd_io.h>
#include <pcl\\common\\common.h>	

using namespace std;

typedef pcl::PointXYZ PointT;

int main()
{
	//---------------------------- 加载点云 --------------------------
	pcl::PointCloud<PointT>::Ptr cloud(new pcl::PointCloud<PointT>);
	if (pcl::io::loadPCDFile("test.pcd", *cloud) < 0)
	{
		PCL_ERROR("->点云文件不存在！\\a\\n");
		system("pause");
		return -1;
	}
	cout << "->加载了 " << cloud->points.size() << " 个数据点" << endl;
	//================================================================


	//--------------------- 计算一点到点云的最远一点 -------------------
	Eigen::Vector4f max_pt;
	Eigen::Vector4f pivot_pt;	//指定一点（x,y,z,1）
	pivot_pt << 0.34, 2.84, 0.45, 1;
	pcl::getMaxDistance(*cloud, pivot_pt, max_pt);
	
	cout << "\\n->最远一点为：" << endl;
	cout << max_pt << endl;
	//================================================================

	return 0;
}

3 输出结果

->加载了 1348 个数据点

->最远一点为：
 13.1488
0.316289
 3.52778
       1