以PLY binary little-endian format格式保存RGB3D点云

PCL库中的3D点云包含的域相当丰富，但大多数时候我们用不到，在一些插件和脚本中使用PCL导出的PLY文件，这些多余的域可能会导致报错，因此我写了一个简单的PLY binary little-endian format格式，包含了XYZ的三维坐标信息和每个点的RGB信息。运行需要PCL库读取点云文件。

#include <iostream>          
#include <pcl/io/pcd_io.h>  
#include <pcl/point_types.h> 
#include <pcl/io/ply_io.h>  
#include <pcl/filters/statistical_outlier_removal.h>
#include <pcl/filters/filter.h>

void removeNaN(pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud)//移除无效点
{
	pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_temp(new pcl::PointCloud<pcl::PointXYZRGB>);//实例化一个点云对象指针
	for (size_t i = 0; i < cloud->points.size(); i++)
	{
		if (pcl::isFinite(cloud->points[i]))
		{
			cloud_temp->points.push_back(cloud->points[i]);
		}
	}
	cloud->clear();
	cloud_temp->width = (int)cloud_temp->points.size();
	cloud_temp->height = 1;//设置点云形状和类型，无序点云
	pcl::copyPointCloud(*cloud_temp, *cloud);
}

std::string FilePathandName(const std::string &file_path)//提取打开文件除后缀的部分，作为保存文件的前缀
{
	std::string file_path_without_suffix;
	file_path_without_suffix = file_path.substr(0, file_path.find_last_of('.'));
	return file_path_without_suffix;
}

struct PositionData
{
	float x, y, z;
	PositionData() {
		x = y = z = 0.0f;
	}
};

struct colorData
{
	unsigned char r, g, b;
	colorData() {
		r = g = b = 0;
	}
};

int main(int argc, char** argv)
{
	if (argc < 2)
	{
		std::cout << "The Input is error!" << std::endl;
		return (-1);
	}//输入不符合要求时报错
	std::string ori_filename;
	ori_filename = argv[1];
	pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
	pcl::io::loadPCDFile(ori_filename, *cloud);
	std::cout << "Read PCD file Success" << std::endl;
	removeNaN(cloud);
	int vertexCount = cloud->points.size();
	//pcl::io::savePLYFileBinary("Captured_Frame1.ply", *cloud);
	std::string file_path_without_suffix;
	file_path_without_suffix = FilePathandName(argv[1]);

	std::string new_filename;
	new_filename = file_path_without_suffix + ".ply";

	FILE *outputPly;
	struct PositionData P;
	struct colorData C;
	outputPly = fopen(new_filename.c_str(), "wb");

	fprintf(outputPly, "ply\n");
	fprintf(outputPly, "format binary_little_endian 1.0\n");
	fprintf(outputPly, "comment This contains a Splatted Point Cloud\n");
	fprintf(outputPly, "element vertex %d\n", cloud->points.size());
	fprintf(outputPly, "property float x\n");
	fprintf(outputPly, "property float y\n");
	fprintf(outputPly, "property float z\n");
	fprintf(outputPly, "property uchar red\n");
	fprintf(outputPly, "property uchar green\n");
	fprintf(outputPly, "property uchar blue\n");
	fprintf(outputPly, "end_header\n");

	//write vertices and normals
	for (long int i = 0; i < vertexCount; i++)
	{
		P.x = cloud->points[i].x;
		P.y = cloud->points[i].y;
		P.z = cloud->points[i].z;
		C.r = cloud->points[i].r;
		C.g = cloud->points[i].g;
		C.b = cloud->points[i].b;
		fwrite(&P, sizeof(struct PositionData), 1, outputPly);
		fwrite(&C, sizeof(struct colorData), 1, outputPly);
	}
	
	fclose(outputPly);
	std::cout << "Save PLY file Success" << std::endl;
	return 0;
}