C++ 保存pcd、ply格式的点云文件

1. 说明

1. pcd、ply格式的点云都可以用PCL库进行很方便的保存，这里没写
2. 由于PCL库是很重型的库，很多小程序使用PCL库很不方便，所以下面写出C++的轻量化实现
3. 实现思路：根据文件的内容填值
4. pcd、ply格式都分别有ascii形式和二进制形式的保存方式
   4.1. ascii形式方便人阅读，但是文件体积稍微大一些
   4.2. 二进制形式文件体积小，但是人不易读

2. PCD

2.1 ascii 形式

ascii 形式的PCD文件内容样式如下：

image

参照这个内容样式可写C++代码如下：

std::vector<PointXyzRgb<float>> points;  // PointXyzRgb为自定义类，有x/y/z/r/g/b信息

points_len = points.size();
std::string pc_name = "./pointcloud" + std::to_string(cnt) + ".pcd";
std::ofstream fout_pc_name(pc_name);

fout_pc_name << "# .PCD v0.7 - Point Cloud Data file format" << std::endl;
fout_pc_name << "VERSION 0.7" << std::endl;
fout_pc_name << "FIELDS x y z" << std::endl;
fout_pc_name << "SIZE 4 4 4" << std::endl;
fout_pc_name << "TYPE F F F" << std::endl;
fout_pc_name << "COUNT 1 1 1" << std::endl;
fout_pc_name << "WIDTH " << points_len << std::endl;
fout_pc_name << "HEIGHT 1" << std::endl;
fout_pc_name << "VIEWPOINT 0 0 0 1 0 0 0" << std::endl;
fout_pc_name << "POINTS " << points_len << std::endl;
fout_pc_name << "DATA ascii" << std::endl;

for (int n = 0; n < points_len; n++) {
    fout_pc_name << points[n].x << " " << points[n].y << " " << points[n].z << std::endl;
}
fout_pc_name.close();

如果需要颜色，则在文件头多增加几行，for循环里加上rgb的信息

2.2 二进制 形式

二进制形式的PCD文件内容样式如下：

image

参照这个内容样式可写C++代码如下：

std::vector<PointXyzRgb<float>> points;  // 里面存有点云。 PointXyzRgb为自定义类，有x/y/z/r/g/b信息

points_len = points.size();
std::string pc_name = "./pointcloud" + std::to_string(cnt) + ".pcd";

FILE *fp = fopen(pc_name.c_str(), "wb");
fprintf(fp, "# .PCD v0.7 - Point Cloud Data file format\n");
fprintf(fp, "VERSION 0.7\n");
fprintf(fp, "FIELDS x y z\n");
fprintf(fp, "SIZE 4 4 4\n");
fprintf(fp, "TYPE F F F\n");
fprintf(fp, "COUNT 1 1 1\n");
fprintf(fp, "WIDTH %d\n", points_len);
fprintf(fp, "HEIGHT 1\n");
fprintf(fp, "VIEWPOINT 0 0 0 1 0 0 0\n");
fprintf(fp, "POINTS %d\n", points_len);
fprintf(fp, "DATA binary\n");

for (int n = 0; n < points_len; n++) {
    float tmp2[3] = { points[n].x, points[n].y, points[n].z };
    fwrite(tmp2, sizeof(float), 3, fp);
}
fclose(fp);

如果需要颜色，则在文件头多增加几行，for循环里加上rgb的信息

3. PLY

3.1 ascii 形式

ascii 形式的PLY文件内容样式如下：

image

参照这个内容样式可写C++代码如下：

std::vector<PointXyzRgb<float>> points;  // 里面存有点云。 PointXyzRgb为自定义类，有x/y/z/r/g/b信息

points_len = points.size();
std::string pc_name = "./pointcloud" + std::to_string(cnt) + ".ply";
std::ofstream fout_pc_name(pc_name);

fout_pc_name << "ply" << std::endl;
fout_pc_name << "format ascii 1.0" << std::endl;
fout_pc_name << "element vertex " << points_len << std::endl;
fout_pc_name << "property float x" << std::endl;
fout_pc_name << "property float y" << std::endl;
fout_pc_name << "property float z" << std::endl;
fout_pc_name << "element face 0" << std::endl;
fout_pc_name << "property list uchar int vertex_index" << std::endl;
fout_pc_name << "end_header" << std::endl;

for (int n = 0; n < points_len; n++) {
  fout_pc_name << points[n].x << " " << points[n].y << " " << points[n].z << std::endl;
}
fout_pc_name.close();

需要RGB信息，则如下：

std::vector<PointXyzRgb<float>> points;  // 里面存有点云。 PointXyzRgb为自定义类，有x/y/z/r/g/b信息

points_len = points.size();
std::string pc_name = "./pointcloud" + std::to_string(cnt) + ".ply";
std::ofstream fout_pc_name(pc_name);

fout_pc_name << "ply" << std::endl;
fout_pc_name << "format ascii 1.0" << std::endl;
fout_pc_name << "element vertex " << points_len << std::endl;
fout_pc_name << "property float x" << std::endl;
fout_pc_name << "property float y" << std::endl;
fout_pc_name << "property float z" << std::endl;
fout_pc_name << "property uchar red" << std::endl;
fout_pc_name << "property uchar green" << std::endl;
fout_pc_name << "property uchar blue" << std::endl;
fout_pc_name << "element face 0" << std::endl;
fout_pc_name << "property list uchar int vertex_index" << std::endl;
fout_pc_name << "end_header" << std::endl;

for (int n = 0; n < points_len; n++) {
    fout_pc_name << points[n].x << " " << points[n].y << " " << points[n].z << " " <<  
        points[n].r << " " << points[n].g << " " << points[n].b << std::endl;
}

fout_pc_name.close();

3.2 二进制 形式

代码来自右方链接： https://zhuanlan.zhihu.com/p/78151412

二进制形式的PLY文件内容样式如下：

image

参照这个内容样式可写C++代码如下：

void saveBinPts(std::vector<float> &Pts, char *fileName)
{
    int elementNum = Pts.size() / 3;
    FILE *fp = fopen(fileName, "wb");
    fprintf(fp, "ply\n");
    fprintf(fp, "format binary_little_endian 1.0\n");
    fprintf(fp, "comment File generated\n");
    fprintf(fp, "element vertex %d\n", elementNum);
    fprintf(fp, "property float x\n");
    fprintf(fp, "property float y\n");
    fprintf(fp, "property float z\n");
    fprintf(fp, "end_header\n");

    for (int i = 0; i < elementNum; i++) {
        float tmp2[3] = { Pts[3 * i + 0], Pts[3 * i + 1], Pts[3 * i + 2] };
        fwrite(tmp2, sizeof(float), 3, fp);
    }
    fclose(fp);
}

上面的帖子也写了加载的程序，如下(未验证)：

void loadBinPts(char * filename)
{
    FILE *fp;

    fp = fopen(filename, "rb");

    char strLine[1024];

    char end_flag[] = "end_header ";
    char num_flag[] = "element vertex ";
    char *p;
    char num[100];


    if (fp == NULL) {
        printf("Error:Open input.c file fail!\n");
        return;
    }

    while (!feof(fp)) {  //循环读取每一行，直到文件尾
        fgets(strLine, 1024, fp);

        if (strlen(strLine) == (strlen(end_flag))) {
            break;
        }

        if ((p = strstr(strLine, num_flag)) != NULL) {
            int start = strlen(num_flag);
            int sub_len = strlen(strLine) - strlen(num_flag);

            for (int i = 0; i < sub_len; i++)  {
                num[i] = strLine[start + i];    //从第start+i个元素开始向数组内赋值
            }
            numPts = atoi(num);
        }
    }

    float *pts = (float*)malloc(numPts * 3 * sizeof(float));
    float cnt = numPts * 3;
    fread(pts, sizeof(float), cnt, fp);
    fclose(fp);

    for (int i = 0; i < numPts; i++) {
        //注意点的访问方式
        Pts.push_back(pts[3 * i + 0]);
        Pts.push_back(pts[3 * i + 1]);
        Pts.push_back(pts[3 * i + 2]);
    }
}

ply各字段含义说明
http://paulbourke.net/dataformats/ply/
http://gamma.cs.unc.edu/POWERPLANT/papers/ply.pdf