PCL点云边缘检测源码解析（过程记录）---AngleCriterion （附C++代码）

一、概述

在PCL中集成了一个非常经典的点云边缘检测算法，这个算法也在 PCL边缘检测 这篇博客中讲解了。那篇文章中只介绍了AC算法的思想步骤以及它的接口调用。那么它的内部具体是如何实现的呢，如果知道了它的具体实现，那么在某些情况下，就可以直接在它的源码基础上做一些小的改动，就能够更好地适应不同的需求了。这里只对AC边缘检测算法做个源码解析及修改后调用的过程记录，若以后需要解析其他PCL功能，也可以借鉴一下。

二、PCL边缘检测源码（AC）定位过程

1. 首先，定位什么功能的源码，前提是已经知道了它调用的接口是什么，属于哪个模块（类的名字）。比如，对于AC边缘检测来说，调用的接口代码为pcl::BoundaryEstimation<PointT, PointNT, PointT>，那么就知道了，这个计算边缘的类名就叫BoundaryEstimation了，那么打开地址https://pointclouds.org/，进入PCL的主页，如下图：
   
   然后点击 Docs按钮
   
   然后就进入了documentation界面，然后在右侧Search搜索框搜索上面类的名称BoundaryEstimation，然后点击对应的搜索结果，
   
   然后就进到了BoundaryEstimation类中。
   
2. 接下来目标就是找到对应的哪个函数是用来计算边缘点的。在调用接口的时候，用的是pcl::BoundaryEstimation<PointT, PointNT, PointT>::compute()这个函数，那么自然而然地，就是要找到compute()这个函数了。可以直接在网页中搜索（按 Ctrl+F）compute，
   
   找到之后点进去，
   
   上图说了在feature.hpp文件的第194行定义了这个函数，直接点击feature.hpp，然后找到定义这个函数的代码
   
   发现里面又调用了一个computeFeature()的函数，可以在搜索框搜索这个函数
   
   点进去之后看到函数的说明，如下图，在boundary.hpp文件的116行定义了此函数
   
   再点进去，这样就找到了这个函数的实现了
   

但是，读了一下这个实现代码，看到里面有几个比较重要的函数，分别是searchForNeighbors(), getCoordinateSystemOnplane 和 isBoundaryPoint()，其中searchForNeighbors()就是利用Kdtree找近邻的，不多讲。isBoundaryPoint()函数也可以在这个hpp文件中找到实现的代码，如下图。

可以看到，isBoundaryPoint()这个函数有两个，进行了重载，区别就是一个输入是点的索引，另一个是点。而另一个函数getCoordinateSystemOnPlane()也可以在搜索框中直接搜索（反正遇到什么函数需要看看的，直接在搜索框搜索就行了）。

点进去，说了在boundary.h的159行定义了，

直接点击boundary.h文件

好，到此为止，所有的代码都定位出来了。

三、重写代码

定位到这些代码之后，就可以照着computeFeature()函数自己写一个边缘检测功能的代码了，再看一遍computeFeature()函数的代码。

仔细分析一下，知道整个流程就是遍历每个点，对每个点找近邻，并调用getCoordinateSystemOnPlane()对每个点计算u 和 v这两个向量，然后调用isBoundaryPoint()函数，判断每个点是否是边缘点。

知道了整个流程后，就可以自己动手实现一下这个功能了。

代码如下
“boundary_estimation.hpp”

#include <pcl/features/boundary.h>
#include <pcl/features/normal_3d.h>
#include <Eigen/Eigen>

class AC
{
public:
	void edge_detection(const pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud_ptr, pcl::PointCloud<pcl::Boundary>::Ptr &output)
	{
		pcl::PointCloud<pcl::Normal>::Ptr normal_ptr(new pcl::PointCloud<pcl::Normal>);
		normal_ptr = this->compute_normals(cloud_ptr);
		std::vector<int> neighbor_idx;
		std::vector<float> neighbor_dist;
		pcl::search::KdTree<pcl::PointXYZ> kdtree;
		kdtree.setInputCloud(cloud_ptr);

		Eigen::Vector4f u = Eigen::Vector4f::Zero(), v = Eigen::Vector4f::Zero();
		output->resize(cloud_ptr->size());

		for (size_t i = 0; i < cloud_ptr->size(); i++)
		{
			kdtree.nearestKSearch(cloud_ptr->points[i], 40, neighbor_idx, neighbor_dist);
			this->getCoordinateSystemOnPlane(normal_ptr->points[i], u, v);
			output->points[i].boundary_point = this->isBoundaryPoint(cloud_ptr, cloud_ptr->points[i], neighbor_idx, u, v, 0.9);
		}
		
	}
private:
	pcl::PointCloud<pcl::Normal>::Ptr compute_normals(const pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud_ptr)
	{
		pcl::PointCloud<pcl::Normal>::Ptr normal_ptr(new pcl::PointCloud<pcl::Normal>);
		pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne;
		ne.setInputCloud(cloud_ptr);
		pcl::search::KdTree<pcl::PointXYZ>::Ptr kdtree(new pcl::search::KdTree<pcl::PointXYZ>);
		ne.setSearchMethod(kdtree);
		ne.setKSearch(30);
		ne.compute(*normal_ptr);
		return normal_ptr;
	}

private:
	void getCoordinateSystemOnPlane(const pcl::Normal &p_coeff, Eigen::Vector4f &u, Eigen::Vector4f &v)
	{
		pcl::Vector4fMapConst p_coeff_v = p_coeff.getNormalVector4fMap();
		v = p_coeff_v.unitOrthogonal();
		u = p_coeff_v.cross3(v);
	}

	bool isBoundaryPoint(
		const pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud, const pcl::PointXYZ &q_point,
		const std::vector<int> &indices,
		const Eigen::Vector4f &u, const Eigen::Vector4f &v,
		const float angle_threshold)
	{
		if (indices.size() < 3) return false;
		if (!std::isfinite(q_point.x) || !std::isfinite(q_point.y) || !std::isfinite(q_point.z)) return false;
		std::vector<float> angles(indices.size());
		float max_dif = FLT_MIN, dif;
		int cp = 0;
		for (const auto &index : indices)
		{
			if (!std::isfinite(cloud->points[index].x) ||
				!std::isfinite(cloud->points[index].y) ||
				!std::isfinite(cloud->points[index].z)) continue;
			Eigen::Vector4f delta = cloud->points[index].getVector4fMap() - q_point.getVector4fMap();
			if (delta == Eigen::Vector4f::Zero()) continue;
			angles[cp++] = std::atan2(v.dot(delta), u.dot(delta));
		}
		if (cp == 0) return false;
		angles.resize(cp);
		std::sort(angles.begin(), angles.end());
		for (size_t i = 0; i < angles.size(); i++)
		{
			dif = angles[i + 1] - angles[i];
			max_dif = std::max(max_dif, dif);
		}
		dif = 2 * static_cast<float> (M_PI) - angles.back() + angles[0];
		max_dif = std::max(max_dif, dif);
		return (max_dif > angle_threshold);
	}
};

“main.cpp”

#include <pcl/io/ply_io.h>
#include <pcl/visualization/cloud_viewer.h>
#include "boundary_estimation.hpp"

int main()
{
	std::string filename = "***.ply";
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_ptr(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::PLYReader reader;
	reader.read(filename, *cloud_ptr);
	
	pcl::PointCloud<pcl::Boundary>::Ptr output(new pcl::PointCloud<pcl::Boundary>);
	AC ac;
	ac.edge_detection(cloud_ptr, output);
	pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudRGB(new pcl::PointCloud<pcl::PointXYZRGB>);
	cloudRGB->resize(cloud_ptr->size());
	for (size_t i = 0; i < output->size(); i++)
	{
		cloudRGB->points[i].x = cloud_ptr->points[i].x;
		cloudRGB->points[i].y = cloud_ptr->points[i].y;
		cloudRGB->points[i].z = cloud_ptr->points[i].z;
		if (output->points[i].boundary_point)
		{
			cloudRGB->points[i].r = 0;
			cloudRGB->points[i].g = 255;
			cloudRGB->points[i].b = 0;
		}
		else
		{
			cloudRGB->points[i].r = 150;
			cloudRGB->points[i].g = 150;
			cloudRGB->points[i].b = 150;
		}
	}
	pcl::visualization::CloudViewer viewer("viewer");
	viewer.showCloud(cloudRGB);
	system("PAUSE");
}

AC边缘检测效果