PCL常用的点云操作

1、PCL有序点云分析

对于有序点云可以通过 at 的方式访问，但是，需要注意的是，正确的访问方式是 at(w, h)。

//读写
#include <pcl/io/pcd_io.h> 
#include <pcl/point_types.h>  

typedef pcl::PointXYZ PointT;
typedef pcl::PointCloud<PointT> PointCloudT;
//读取点云示例
int main()
{
	PointCloudT::Ptr mpCloudSource;
	mpCloudSource.reset(new(PointCloudT));
	pcl::PCDReader reader;
	int sucess = reader.read(PointPath, *mpCloudSource);
	if (sucess == -1)
	{
		std::cout << "read point file failure!..." << std::endl;
		return -1;
	}
	//访问有序点云数据
	for (int i = 0; i < mpCloudSource->width; i++)
	{
		for (int j = 0; j < mpCloudSource->height; j++)
		{
			pcl::PointXYZ pointcloud = mpCloudSource->at(width, height);
		}
	}
}

2、PCL绘制直线

绘制直线(每条线段１个唯一的id符，不然的话只能画出一条线)：

//cloudOut->points.size():所有的直线点
for (int i = 0; i < cloudOut->points.size() - 1; i++)
{
    std::string id = "line_" + std::to_string(i);
    viewer.addLine(cloudOut->points[i], cloudOut->points[i + 1], 0.0, 0.0, 1.0, id, v1);
    viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_LINE_WIDTH, 6, id);
}
viewer.addText("points trajectory line", 10, 920, 16, bckgr_gray_level, bckgr_gray_level, txt_gray_lvl, "line", v1);

3、获取点云质心

获取点云的质心有两种方式，一种是pcl自带的API：pcl::compute3DCentroid(*cloud, centroid);，另一种是用数学的方式求所有点坐标的平均值。下面的代码实现了这两种方式。

#include <iostream>
#include <Eigen/Core>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/common/centroid.h>
#include <pcl/common/transforms.h>
 
using namespace std;
typedef pcl::PointCloud<pcl::PointXYZ> Cloud;
int main(int argc, char **argv)
{
    // 生成一个正方形点云
    Cloud::Ptr cloud(new Cloud);
    cloud->width = 10000;
    cloud->height = 1;
    cloud->is_dense = false;
    cloud->points.resize(cloud->width * cloud->height);  
    
    for (int i = 0; i < cloud->points.size(); i++)
    {
        cloud->points[i].x=1000* (rand() / (RAND_MAX + 1.0f));
        cloud->points[i].y=1000* (rand() / (RAND_MAX + 1.0f));
        cloud->points[i].z=0;
    }
    
    // 方式1：利用PCL函数计算质心
    Eigen::Vector4f centroid;                    // 质心
    pcl::compute3DCentroid(*cloud, centroid);    // 齐次坐标，（c0,c1,c2,1）
 
    // 方式2：利用公式计算质心
    pcl::PointXYZ p_c;
    p_c.x = 0; p_c.y = 0; p_c.z = 0;
    for (auto p : cloud->points) {
        p_c.x += p.x;
        p_c.y += p.y;
        p_c.z += p.z;
    }
 
    p_c.x /= cloud->points.size();
    p_c.y /= cloud->points.size();
    p_c.z /= cloud->points.size();
 
    // 结果对比
    cout << "pcl 函数计算点云质心结果：(" << centroid(0)<<","<<centroid(1)<<","<<centroid(2)<<")" << endl;
    cout << "按照公式计算点云质心结果：" << p_c<< endl;
    
    return 0;
}

4、判断点云是否是非数

pcl::PointXYZ pointcloud = cloudDownSampling->points[idx];
if (!isFinite(pointcloud)) //不是有效点
    continue;

5、移除无效点

// Object for storing the point cloud.
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
// Read a PCD file from disk.
if (pcl::io::loadPCDFile<pcl::PointXYZRGB>(argv[1], *cloud) != 0)
{
    return -1;
}
// The mapping tells you to what points of the old cloud the new ones correspond,
// but we will not use it.
std::vector<int> mapping;
pcl::removeNaNFromPointCloud(*cloud, *cloud, mapping);

6、PCL计算两点之间的欧式距离

for(int i=0;i<cloud->points.size()-1;i++)
{
     float dis=pcl::euclideanDistance(cloud->points[i],cloud->points[i+1]);
}

###################################未完待续###################################