《PCL点云库学习&VS2010(X64)》Part 25 PCL点云操作函数汇集

《PCL点云库学习&VS2010(X64)》Part 25 PCL点云操作函数汇集

一、pcl::copyPointCloud()函数的用法：

1、将索引中的点云复制到pcl::PointXYZ中存储起来。

注：一般滤波算法、RANSAC算法、Cluster算法中都会有索引提取，当然在Cluster提取时要注意，有的Cluster里面是很多聚类后的点云，要分别转存，用迭代器和循环解决。

#include <pcl/common/impl/io.hpp>
...
std::vector<int> inliers_line
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
...
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_line(new pcl::PointCloud<pcl::PointXYZ>);
pcl::copyPointCloud(*cloud,inliers_line,*cloud_line);

2、PCL 不同类型的点云之间进行类型转换，也可以使用函数pcl::copyPointCloud()：

如将PointXYZ转换为PointXYZRGBA

#include <pcl/common/impl/io.h>

pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_xyz (new pcl::PointCloud<pcl::PointXYZ> ());
pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud_xyzrgba (new pcl::PointCloud<pcl::PointXYZRGBA> ());
pcl::copyPointCloud(*cloud_xyz, *cloud_xyzrgba);

该函数也可以用循环替代：

cloud_xyzrgba->points.resize(cloud_xyz->size());
for (size_t i = 0; i < cloud_xyz->points.size(); i++) {
cloud_xyzrgb->points[i].x = cloud_xyz->points[i].x;
cloud_xyzrgb->points[i].y = cloud_xyz->points[i].y;
cloud_xyzrgb->points[i].z = cloud_xyz->points[i].z;
}

二、将某些点转存为中间点云或者创建一个新的点云：

pcl::PointCloud<pcl::PointXYZ>::Ptr point_cloud(new pcl::PointCloud<pcl::PointXYZ>());
pcl::pointXYZ point;
point.x = 70.00;
point.y = -20.00;
point.z = 13.67;
point_cloud->points.push_back(point);

如果想将某些点从堆栈中弹出，可以利用pop_up()函数。

三、计算点云质心（重心）、曲率、法线、转换（仿射）矩阵

三维点云的质心、曲率、法线、转换（仿射）矩阵：

先定义三个点云，pcl1_ptrA, pcl1_ptrB and pcl1_ptrC:

pcl::PointCloud<pcl::PointXYZRGB>::Ptr pcl1_ptrA(new pcl::PointCloud<pcl::PointXYZRGB>); //pointer for color version of pointcloud
pcl::PointCloud<pcl::PointXYZRGB>::Ptr pcl1_ptrB(new pcl::PointCloud<pcl::PointXYZRGB>); //ptr to hold filtered Kinect image
pcl::PointCloud<pcl::PointXYZRGB>::Ptr pcl1_ptrC(new pcl::PointCloud<pcl::PointXYZRGB>); //ptr to hold filtered Kinect image
......
pcl::VoxelGrid<pcl::PointXYZRGB> vox;

vox.setInputCloud(pcl1_ptrA);

vox.setLeafSize(0.02f, 0.02f, 0.02f);

vox.filter(*pcl1_ptrB);

cout<<"done voxel filtering"<<endl;

cout<<"num bytes in original cloud data = "<<pcl1_ptrA->points.size()<<endl;

cout<<"num bytes in filtered cloud data = "<<pcl1_ptrB->points.size()<<endl; // ->data.size()<<endl;

Eigen::Vector4f xyz_centroid;

pcl::compute3DCentroid (*pcl1_ptrB, xyz_centroid);

float curvature;

Eigen::Vector4f plane_parameters;

pcl::computePointNormal(*pcl1_ptrB, plane_parameters, curvature); //pcl fnc to compute plane fit to point cloud

Eigen::Affine3f A(Eigen::Affine3f::Identity());

pcl::transformPointCloud(*pcl1_ptrB, *pcl1_ptrC, A);

The actual compute call from the NormalEstimation class does nothing internally but:

for each point p in cloud P

1. get the nearest neighbors of p
2. compute the surface normal n of p
3. check if n is consistently oriented towards the viewpoint and flip otherwise

The viewpoint is by default (0,0,0) and can be changed with:

setViewPoint (float vpx, float vpy, float vpz);

To compute a single point normal, use:

computePointNormal (const pcl::PointCloud<PointInT> &cloud, const std::vector<int> &indices, Eigen::Vector4f &plane
_parameters, float &curvature);

Where cloud is the input point cloud that contains the points, indices represents the set of k-nearest neighbors from cloud, and plane_parameters and curvature represent the output of the normal estimation, with plane_parameters holding the normal (nx, ny,
nz) on the first 3 coordinates, and the fourth coordinate is D = nc . p_plane (centroid here) + p. The output surface curvature is estimated as a relationship between the eigenvalues of the covariance matrix (as presented above), as:

四、将三维点云（pcl::PointXYZ）转换为二维向量（Eigen::Vector2d）

上一篇文章中的拟合二维曲线有提及该转换函数。

void
PointCloud2Vector2d (pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, pcl::on_nurbs::vector_vec2d &data)
{
for (unsigned i = 0; i < cloud->size (); i++)
{
pcl::PointXYZ &p = cloud->at (i);
if (!pcl_isnan (p.x) && !pcl_isnan (p.y))
data.push_back (Eigen::Vector2d (p.x, p.y));
}
}

也可以用循环解决，但是矩阵的效率可能高点。

五、点云可视化属性函数使用

PointCloudColorHandlerRGBField

PointCloudColorHandlerCustom

setPointCloudRenderingProperties

addPointCloudNormals

1、PointCloudColorHandlerRGBField使用方法：

...
int v1(0);
viewer->createViewPort (0.0, 0.0, 0.5, 1.0, v1);
viewer->setBackgroundColor (0, 0, 0, v1);
viewer->addText ("Radius: 0.01", 10, 10, "v1 text", v1);
pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> rgb (cloud);
viewer->addPointCloud<pcl::PointXYZRGB> (cloud, rgb, "sample cloud1", v1);
...

2、PointCloudColorHandlerCustom使用方法：

...
int v2(0);
viewer->createViewPort (0.5, 0.0, 1.0, 1.0, v2);
viewer->setBackgroundColor (0.3, 0.3, 0.3, v2);
viewer->addText ("Radius: 0.1", 10, 10, "v2 text", v2);
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZRGB> single_color (cloud, 0, 255, 0);
viewer->addPointCloud<pcl::PointXYZRGB> (cloud, single_color, "sample cloud2", v2);
...

3、setPointCloudRenderingProperties使用方法：

...
viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "sample cloud1");
viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "sample cloud2");
viewer->addCoordinateSystem (1.0);

4、addPointCloudNormals使用方法：

...
viewer->addPointCloudNormals<pcl::PointXYZRGB, pcl::Normal> (cloud, normals1, 10, 0.05, "normals1", v1);
viewer->addPointCloudNormals<pcl::PointXYZRGB, pcl::Normal> (cloud, normals2, 10, 0.05, "normals2", v2);
...

六、法向量估计——计算协方差矩阵

1、计算协方差矩阵

// Placeholder for the 3x3 covariance matrix at each surface patch
Eigen::Matrix3f covariance_matrix;
// 16-bytes aligned placeholder for the XYZ centroid of a surface patch
Eigen::Vector4f xyz_centroid;

// Estimate the XYZ centroid
compute3DCentroid (cloud, xyz_centroid);

// Compute the 3x3 covariance matrix
computeCovarianceMatrix (cloud, xyz_centroid, covariance_matrix);

2、**计算单个点法线**

法线估计类NormalEstimation的实际计算调用程序内部执行以下操作：

对点云P中的每个点p
  1.得到p点的最近邻元素
  2.计算p点的表面法线n
  3.检查n的方向是否一致指向视点，如果不是则翻转

视点坐标默认为（0,0,0），可以使用以下代码进行更换：

setViewPoint (float vpx, float vpy, float vpz);

计算单个点的法线，使用：

computePointNormal (const pcl::PointCloud<PointInT>&cloud, const std::vector<int>&indices, Eigen::Vector4f &plane_parameters, float&curvature);

此处，cloud是包含点的输入点云，indices是点的k-最近邻元素集索引，plane_parameters和curvature是法线估计的输出，plane_parameters前三个坐标中，以(nx, ny, nz)来表示法线，第四个坐标D = nc . p_plane (centroid here) + p。输出表面曲率curvature通过协方差矩阵的特征值之间的运算估计得到，如：

七、提取点云的索引pcl::ExtractIndices

该函数的作用是从点云中提取索引。
使用方法：

pcl::ExtractIndices<PointType> eifilter (true); // Initializing with true will allow us to extract the removed indices
eifilter.setInputCloud (cloud_in);
eifilter.setIndices (indices_in);
eifilter.filter (*cloud_out);
// The resulting cloud_out contains all points of cloud_in that are indexed by indices_in
indices_rem = eifilter.getRemovedIndices ();
// The indices_rem array indexes all points of cloud_in that are not indexed by indices_in
eifilter.setNegative (true);
eifilter.filter (*indices_out);
// Alternatively: the indices_out array is identical to indices_rem
eifilter.setNegative (false);
eifilter.setUserFilterValue (1337.0);
eifilter.filterDirectly (cloud_in);
// This will directly modify cloud_in instead of creating a copy of the cloud
// It will overwrite all fields of the filtered points by the user value: 1337