PCL点云 CSV格式转 PLY格式

大家好，我是青空。

最近在做项目的时候，遇到了需要把CSV 转换成点云文件的情况，主要是为了从csv 转换成的点云文件和从深度图转换成的点云文件做一个对比校验作用。

CSV 文件大致如下，本文的CSV 文件是由 基恩士激光传感器采集所的。大家如果是其他的传感器所得的数据，可能对应的关系有所不同的。

表格中行表示X轴方向的点，列表示Y轴方向的点，表格中的数值表示Z轴深度值。
每一个X与Y的间距是由传感器本身的精度和伺服电机的步长决定的。

#include <iostream>
#include <fstream>
#include <string>

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

#include <opencv2/opencv.hpp>

//typedef pcl::PointXYZRGB point_t;
typedef pcl::PointXYZ point_t;
typedef pcl::PointCloud<point_t> pointcloud_t;


int endWith(std::string str, std::string sub) {
    return str.rfind(sub) == (str.length() - sub.length()) ? 1:0;
}

int main(int argc, char** argv) {

    std::string readFile = argv[1];

    std::cout << "readFile= " << readFile << std::endl;
    if (endWith(readFile, ".csv") == 0) {
        std::cout << "please input .csv file" << std::endl;
        system("pause");
        return 0;
    }
    
    std::ifstream csvfs(readFile, std::ios::binary | std::ios::in);
    
    std::string lineStr;
    std::string str;
    
    int yStep = 0;
    
    pointcloud_t::Ptr cloud(new pointcloud_t);
    point_t point;
    
    cv::Mat csvMat(3200, 3200, CV_32FC1);
    
    while (std::getline(csvfs, lineStr)) {
        std::stringstream ss(lineStr);
        int xStep = 0;
        while (std::getline(ss, str, ',')) {
            csvMat.at<float>(yStep, xStep) = std::stof(str);
            xStep++;
        }
        yStep++;
    }
    std::cout << "row= " << yStep << std::endl;
    
    auto row = csvMat.rows > yStep ? yStep : csvMat.rows;
    
    for (int r = 0; r < row; r++) {
        for (int c = 0; c < csvMat.cols; c++) {
            auto val = csvMat.at<float>(r, c);
            if (val < -50) {
                continue;
            }
            point.z = val / 1000;
            point.x = 0.01 * c / 1000;
            point.y = 0.01 * r / 1000;
            
            cloud->points.push_back(point);
        }
    }
    
    std::cout << "point cloud size = " << cloud->points.size() << std::endl;
    
    cloud->height = 1;
    cloud->width = cloud->points.size();
    pcl::io::savePLYFileBinary("cloudcsv.ply", *cloud);
    
    std::cout << "save point cloud ok, file name: cloudcsv.ply" << std::endl;
    system("pause");
    return 0;
}