1.输入点云
因为几乎所有的PCL类都是继承自基类pcl::PCLBase
,pcl::Feature
类以两种不同的方式接受输入数据:
- 一个完整的点云数据集,通过
setInputCloud(PointCloudConstPtr &)
给出-此函数必须设置后续特征算子此能正常计算。任何可以进行特征描述子估计的类,为给定的输入点云中的每个点估计一个特征向量。 - 点云数据集的一个子集,通过
setInputCloud(PointCloudConstPtr &)
和setIndices(IndicesConstPtr &)
给出-setIndices是可选设置。任何特征估计类都将试图在输入点云中给出索引值的所有点估计一个特征。默认情况下,如果没有给出索引值,点云中所有的点都将参与计算。
除此之外,要使用的点集合邻域可以通过附加的调用来明确setSearchSurface(PointCloudConstPtr &)
。这个调用是可选的,如果搜索曲面没有给出,则输入点云数据集为默认的搜索空间。因为总是需要setInputCloud()
,我们可以使用<setInputCloud(),setIndices(),setSearchSurface>
来创建四个组合。比如我们有两个点云,P={p_1, p_2, …p_n} 和Q={q_1, q_2, …, q_n}。下面的图片表示了这四种情况。
- setIndices() = false, setSearchSurface() = false - 这毫无疑问是PCL中应用的最广的例子,用户仅需要输入一个点云数据集,并且为点云中的所有点估计一个特征向量。 无论一组索引和(或)搜索点云是否给定,我们都不希望保存不同的实现副本,无论何时,即使
indices=false
,PCL都会创建一组内部索引(std::vector<int>
),这个内部索引指向整个数据集(indices = 1…N,其中N是点云中点的数量)。这与上面的图片的最左边的例子对应。首先我们估计了p1的最近邻元素,然后估计了p2的最近邻元素,知道我们估计完P中所有点。 - setIndices() = true, setSearchSruface() = false - 如前所述,这种特征估计方法仅给那些给定索引值的点估计特征。在上图中这对应了第二种情况。这里,我们假设p2的索引值没有给出来,所以没有估计p2点的近邻以及特征。
- setIndices() = false, setSearchSurface() = true - 如第一种情况,对于输入中所有点进行特征估计,但是在setSearchSurface()中给出的采样面点云将用来为输入点获取最近邻元素,而不是输入点云本身。这对应了上图中的第三个。如果Q={q_1, q_2}是输入的另一个点云,不同于P,并且P是Q的搜索表面,那么q1和q2的近邻将从P中计算。
- setIndices() = true, setSearchSurface() = true - 这可能是最不常用的情况,其中索引值和搜索曲面都给出。在这种情况下,将使用setSearchSurface()中给出的搜索曲面信息,仅对<input, indices>中的子集估计特征。在上图中的最右例子。这里我们假设q2的索引没有在Q的已给索引向量中,因此在q2处,没有估计其近邻和特征。
当使用setSearchSurface()时,最有用的例子是当我们的输入数据集非常密集时,但我们又不想估计其中所有点的特征,而是在使用pcl_keypoints中的方法发现的一些关键点,或者在点云的降采样版本(例如,使用pcl::VoxelGrid<T>
过滤器获得的)。在本例中,我们通过setInputCloud()传递下采样/关键点输入,并将原始数据作为setSearchSurface()传递。
2.法线估计实例
一旦确定,查询点的相邻点可用于估计局部特征表示,该局部特征表示捕获查询点周围的底层采样曲面的几何图形。描述曲面几何的一个重要问题是首先推断其在坐标系中的方向,即估计其法线。曲面法线是曲面的重要属性,在许多领域(如计算机图形学应用程序)中大量使用,以应用生成阴影和其他视觉效果的正确光源。
下面的代码对输入数据的所有点估计曲面法向量。
#include <pcl/point_types.h>
#include <pcl/features/normal_3d.h>
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
... read, pass in or create a point cloud ...
// Create the normal estimation class, and pass the input dataset to it
pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne;
ne.setInputCloud (cloud);
// Create an empty kdtree representation, and pass it to the normal estimation object.
// Its content will be filled inside the object, based on the given input dataset (as no other search surface is given).
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ> ());
ne.setSearchMethod (tree);
// Output datasets
pcl::PointCloud<pcl::Normal>::Ptr cloud_normals (new pcl::PointCloud<pcl::Normal>);
// Use all neighbors in a sphere of radius 3cm
ne.setRadiusSearch (0.03);
// Compute the features
ne.compute (*cloud_normals);
// cloud_normals->size () should have the same size as the input cloud->size ()
}
下面的代码对输入数据的部分点估计曲面法向量。
#include <pcl/point_types.h>
#include <pcl/features/normal_3d.h>
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
... read, pass in or create a point cloud ...
// Create a set of indices to be used. For simplicity, we're going to be using the first 10% of the points in cloud
std::vector<int> indices (std::floor (cloud->size () / 10));
for (std::size_t i = 0; i < indices.size (); ++i) indices[i] = i;
// Create the normal estimation class, and pass the input dataset to it
pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne;
ne.setInputCloud (cloud);
// Pass the indices
pcl::shared_ptr<std::vector<int> > indicesptr (new std::vector<int> (indices));
ne.setIndices (indicesptr);
// Create an empty kdtree representation, and pass it to the normal estimation object.
// Its content will be filled inside the object, based on the given input dataset (as no other search surface is given).
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ> ());
ne.setSearchMethod (tree);
// Output datasets
pcl::PointCloud<pcl::Normal>::Ptr cloud_normals (new pcl::PointCloud<pcl::Normal>);
// Use all neighbors in a sphere of radius 3cm
ne.setRadiusSearch (0.03);
// Compute the features
ne.compute (*cloud_normals);
// cloud_normals->size () should have the same size as the input indicesptr->size ()
}
最后,下面的代码片段将为输入数据集中的所有点估计一组曲面法线,但将使用另一个数据集估计它们的最近邻居。如前所述,一个很好的用例是当输入是曲面的降采样版本时。
#include <pcl/point_types.h>
#include <pcl/features/normal_3d.h>
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_downsampled (new pcl::PointCloud<pcl::PointXYZ>);
... read, pass in or create a point cloud ...
... create a downsampled version of it ...
// Create the normal estimation class, and pass the input dataset to it
pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne;
ne.setInputCloud (cloud_downsampled);
// Pass the original data (before downsampling) as the search surface
ne.setSearchSurface (cloud);
// Create an empty kdtree representation, and pass it to the normal estimation object.
// Its content will be filled inside the object, based on the given surface dataset.
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ> ());
ne.setSearchMethod (tree);
// Output datasets
pcl::PointCloud<pcl::Normal>::Ptr cloud_normals (new pcl::PointCloud<pcl::Normal>);
// Use all neighbors in a sphere of radius 3cm
ne.setRadiusSearch (0.03);
// Compute the features
ne.compute (*cloud_normals);
// cloud_normals->size () should have the same size as the input cloud_downsampled->size ()
}