一、概述
在PCL中集成了一个非常经典的点云边缘检测算法,这个算法也在 PCL边缘检测 这篇博客中讲解了。那篇文章中只介绍了AC算法的思想步骤以及它的接口调用。那么它的内部具体是如何实现的呢,如果知道了它的具体实现,那么在某些情况下,就可以直接在它的源码基础上做一些小的改动,就能够更好地适应不同的需求了。这里只对AC边缘检测算法做个源码解析及修改后调用的过程记录,若以后需要解析其他PCL功能,也可以借鉴一下。
二、PCL边缘检测源码(AC)定位过程
- 首先,定位什么功能的源码,前提是已经知道了它调用的接口是什么,属于哪个模块(类的名字)。比如,对于AC边缘检测来说,调用的接口代码为
pcl::BoundaryEstimation<PointT, PointNT, PointT>
,那么就知道了,这个计算边缘的类名就叫BoundaryEstimation
了,那么打开地址https://pointclouds.org/
,进入PCL的主页,如下图:
然后点击 Docs按钮
然后就进入了documentation界面,然后在右侧Search搜索框搜索上面类的名称BoundaryEstimation
,然后点击对应的搜索结果,
然后就进到了BoundaryEstimation
类中。
- 接下来目标就是找到对应的哪个函数是用来计算边缘点的。在调用接口的时候,用的是
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边缘检测效果