PCL入门（七）：点云数据输入输出、两个点云的拼接等简单操作

下面主要参考双愚的代码

1. 点云数据文件的创建

随机创建名为test.pcd的点云数据，如下

• test_pcd.cpp

#include 
#include 
#include  

int main(int argc, char** argv)
{
	//步骤一：创建点云对象
	pcl::PointCloud<pcl::PointXYZ> cloud;
	
	//步骤二：输入点云参数
	cloud.width = 5;
	cloud.height = 1;
	cloud.is_dense = false;
	cloud.points.resize(cloud.width * cloud.height);
	
	//步骤三：用随机数填充点云
	for (size_t i=0; i<cloud.points.size(); ++i)
	{
		cloud.points[i].x = 1024 * rand() / (RAND_MAX + 1.0f);
		cloud.points[i].y = 1024 * rand() / (RAND_MAX + 1.0f);
		cloud.points[i].z = 1024 * rand() / (RAND_MAX + 1.0f);
	}
	
	//步骤四：保存点云数据为.pcd文件
	pcl::io::savePCDFileASCII("../test.pcd", cloud);
	
	//步骤五：打印
	std::cerr << "saved " << cloud.points.size() << " data points to test.pcd" << std::endl;
	
	for (size_t i=0; i<cloud.points.size(); ++i)
	{
		std::cerr << " " << cloud.points[i].x << " " << cloud.points[i].y << " " << cloud.points[i].z << std::endl;
	}
	
	return 0;
}

• CMakeLists.txt

cmake_minimum_required (VERSION 2.6 FATAL_ERROR)
project(write_pcd)
find_package(PCL 1.3 REQUIRED)

add_executable(write_pcd write_pcd.cpp)
target_link_libraries(write_pcd ${PCL_LIBRARIES})

• 结果

生成的test.pcd文件内容如下

2. 点云数据文件的读取

读取前面创建的test.pcd文件，如下

• read_pcd.cpp

#include 
#include 
#include 

int main(int argc, char** argv)
{
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
	
	if (pcl::io::loadPCDFile<pcl::PointXYZ>("../test.pcd", *cloud) == -1)
	{
		PCL_ERROR("could not read file test.pcd\n");
	}
	
	std::cout << "Loaded "
              << cloud->width * cloud->height // 宽*高
              << " data points from test_pcd.pcd with the following fields: "
              << std::endl;
        
        for (size_t i=0; i < cloud->points.size(); ++i)
        {
        	std::cout << "    " << cloud->points[i].x
                  << " " << cloud->points[i].y
                  << " " << cloud->points[i].z << std::endl;
        }
        
        return 0;
}

• CMakeLists.txt

cmake_minimum_required (VERSION 2.6 FATAL_ERROR)
project(read_pcd)
find_package(PCL 1.3 REQUIRED)

add_executable(read_pcd read_pcd.cpp)
target_link_libraries(read_pcd ${PCL_LIBRARIES})

3. 两个点云的拼接

参考博客《【pcl入门教程系列】之点云Concatenate》，点云的拼接存在两种操作，一种是增加点的数量，一种是增加点的维度。

• 操作1：增加点的数量。此时要求两个点云字段和维度完全相同，数量可以不同，将两个点云的相加意味着新点云数量的增加，类似于两个集合的并集，如下图所示
  
• 操作2：增加点的维度。此时要求两个点云的数量相同，字段和维度可以不同，将两个点云拼接意味着每个店的维度增加，比如点云A用来表示某个物体的形状，每个点用三维向量表示$(x,y,z)$，点云B用来表示物体表面的法向量，对应点用三维向量表示$(x_n,y_n,z_n)$，这样，点云A和B拼接，则表示对应点维度增加，从单纯表示坐标的三维，变成表示坐标和法向量的6维$(x,y,z,x_n,y_n,z_n)$，如下图
  
  例子：创建一个包含5个点的点云cloud_a，同时创建点云cloud_b，他们之间做拼接形成的新点云cloud_c。

下面的代码生成二进制文件concatenate_pcd，当执行concatenate_pcd -p时，执行操作1，即增加点的数量，此时cloud_b为3个点的点云，新点云cloud_c为包含8个点的点云；当执行concatenate_pcd -f时，执行操作2，即增加点的维度，此时cloud_b为n_cloud_b，包含5个点，新点云cloud_c为包含5个点的点云，每个点的维度增加。

• concatenate_pcd.cpp

#include 
#include 
#include  

int main(int argc, char** argv)
{
	if (argc != 2)
	{
		std::cerr << "please specify command line arg -f or -p" << std::endl;
		exit(0);
	}
	
	// 步骤1：定义所需要的点云对象
	pcl::PointCloud<pcl::PointXYZ> cloud_a, cloud_b, cloud_c;
	pcl::PointCloud<pcl::Normal> n_cloud_b;
	pcl::PointCloud<pcl::PointNormal> p_n_cloud_c;
	
	// 步骤2：对点云cloud_a赋参数
	cloud_a.width = 5;
	cloud_a.height = cloud_b.height = n_cloud_b.height = 1;
	cloud_a.points.resize(cloud_a.width * cloud_a.height);
	
	// 步骤3：对点云cloud_b赋参数（这里cloud_b可能为cloud_b，也可能为法向量点云n_cloud_b）
	if (strcmp(argv[1], "-p") == 0)
	{
		cloud_b.width = 3;
		cloud_b.points.resize(cloud_b.width * cloud_b.height);
	}
	else
	{
		n_cloud_b.width = 5;
		n_cloud_b.points.resize(n_cloud_b.width * n_cloud_b.height);
	}
	
	// 步骤4：填充点云cloud_a
	for (size_t i=0; i < cloud_a.points.size(); ++i)
	{
		cloud_a.points[i].x = 1024 * rand() / (RAND_MAX + 1.0f);
		cloud_a.points[i].y = 1024 * rand() / (RAND_MAX + 1.0f);
		cloud_a.points[i].z = 1024 * rand() / (RAND_MAX + 1.0f);
		
	}
	
	// 步骤5：填充点云cloud_b（这里cloud_b可能为cloud_b，也可能为法向量点云n_cloud_b）
	if (strcmp(argv[1], "-p") == 0)
	{
		for (size_t i=0; i < cloud_b.points.size(); ++i)
		{
			cloud_b.points[i].x = 1024 * rand() / (RAND_MAX + 1.0f);
			cloud_b.points[i].y = 1024 * rand() / (RAND_MAX + 1.0f);
			cloud_b.points[i].z = 1024 * rand() / (RAND_MAX + 1.0f);
			
		}
	}
	else
	{
		for (size_t i=0; i < n_cloud_b.points.size(); ++i)
		{
			n_cloud_b.points[i].normal[0] = 1024 * rand() / (RAND_MAX + 1.0f);
			n_cloud_b.points[i].normal[1] = 1024 * rand() / (RAND_MAX + 1.0f);
			n_cloud_b.points[i].normal[2] = 1024 * rand() / (RAND_MAX + 1.0f);
		}
	}
	
	// 步骤6：输出点云cloud_a
	std::cerr << "cloud A: " << std::endl;
	for (size_t i=0; i<cloud_a.points.size(); ++i)
	{
		std::cerr << " " << cloud_a.points[i].x << " " << cloud_a.points[i].y << " " << cloud_a.points[i].z << std::endl;
		
	}
	
	// 步骤7：输出点云cloud_b
	std::cerr << "cloud B: " << std::endl;
	if (strcmp(argv[1], "-p") == 0)
	{
		for (size_t i=0; i<cloud_b.points.size(); ++i)
		{
			std::cerr << " " << cloud_b.points[i].x << " " << cloud_b.points[i].y << " " << cloud_b.points[i].z << std::endl;
			
		}
	}
	else
	{
		for (size_t i=0; i<n_cloud_b.points.size(); ++i)
		{
			std::cerr << " " << n_cloud_b.points[i].normal[0] << " " << n_cloud_b.points[i].normal[1] << " " << n_cloud_b.points[i].normal[2] << std::endl;
			
		}
	}
	
	// 步骤8：点云cloud_a和点云cloud_b进行拼接，生成cloud_c（拼接操作根据超参数，可能执行操作1，也可能执行操作2）
	if (strcmp(argv[1], "-p") == 0)
	{
		cloud_c = cloud_a;
		cloud_c += cloud_b;
		std::cerr << "cloud C: " << std::endl;
		for (size_t i=0; i<cloud_c.points.size(); ++i)
		{
			std::cerr << " " << cloud_c.points[i].x << " " << cloud_c.points[i].y << " " << cloud_c.points[i].z << std::endl;	
		}
	}
	else
	{
		pcl::concatenateFields(cloud_a, n_cloud_b, p_n_cloud_c);
		std::cerr << "cloud C: " << std::endl;
		for (size_t i = 0; i < p_n_cloud_c.points.size(); ++i)
		{
            		std::cerr << "    " << p_n_cloud_c.points[i].x << " " << p_n_cloud_c.points[i].y << " " << p_n_cloud_c.points[i].z << " " << p_n_cloud_c.points[i].normal[0] << " " << p_n_cloud_c.points[i].normal[1] << " " << p_n_cloud_c.points[i].normal[2] << std::endl;
            	}
	}
	
	return 0;
}

• CMakeLists.txt

cmake_minimum_required (VERSION 2.6 FATAL_ERROR)
project(concatenate_pcd)
find_package(PCL 1.3 REQUIRED)

add_executable(concatenate_pcd concatenate_pcd.cpp)
target_link_libraries(concatenate_pcd ${PCL_LIBRARIES})

• 结果