欢迎您访问程序员文章站本站旨在为大家提供分享程序员计算机编程知识!
您现在的位置是: 首页

将mpts格式点云转换为ply和pcd格式

程序员文章站 2022-03-30 22:02:29
...
#include <pcl/ModelCoefficients.h>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/io/ply_io.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/features/normal_3d.h>
#include <pcl/kdtree/kdtree.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/segmentation/extract_clusters.h>
#include <boost/filesystem.hpp>
#include <string>
#include <vector>
#include <fstream>
#include <sstream>


int main (int argc, char** argv)
{
    std::string input_dir_path(argv[1]);
    namespace fs = boost::filesystem;
    fs::path pointcloud_dir_path(input_dir_path);

    std::cout << "path: " << input_dir_path << std::endl;
    if (input_dir_path.empty() || !fs::exists(pointcloud_dir_path) || !fs::is_directory(pointcloud_dir_path))
        PCL_THROW_EXCEPTION (pcl::IOException, "No valid pointcloud directory given!\n");

    std::vector<std::string> result;
    fs::directory_iterator iter(pointcloud_dir_path);
    fs::directory_iterator end; 



    fs::path ply_cloud_dir_path = "./ply_cloud";
    if (fs::exists(ply_cloud_dir_path))
    {
        fs::remove_all(ply_cloud_dir_path);
    }

    assert(!fs::exists(ply_cloud_dir_path));
    fs::create_directory(ply_cloud_dir_path);

    for(; iter != end ; ++iter)
    {
        fs::path current_file_path;
        if (fs::is_regular_file(iter->status()) )
        {
            if (fs::extension(*iter) == ".mpts")
            {
                current_file_path = iter->path ();
                std::cout << "Processing: " << current_file_path << std::endl;

                fs::path file_name = current_file_path.filename();


                std::ifstream ifs(current_file_path.string());
                if (!ifs.is_open())
                {
                    std::cout << "Error opening file"<<std::endl;
                    break;
                }

                std::string previous_line_str("");
                std::string cur_line_str;
                std::string cur_field;
                int point_num = 0;
                bool has_normal = false;
                bool has_color = false;

                std::ofstream out_position_file;
                std::ofstream out_color_file;
                std::ofstream out_normal_file;

                while (getline(ifs,cur_line_str))
                {
                    if (isdigit(cur_line_str[0]) || cur_line_str[0] == '-')
                    {
                        if(!previous_line_str.empty() && previous_line_str.compare("pointnumber:")== 0)
                        {
                            std::stringstream ss;
                            ss<<cur_line_str;
                            ss>>point_num;
                        }
                        else if(cur_field.compare("points") == 0 &&  out_position_file.is_open())
                        {
                            out_position_file<<cur_line_str<<std::endl;
                        }
                        else if(has_color && cur_field.compare("colors") == 0 && out_color_file.is_open())
                        {
                            out_color_file<<cur_line_str<<std::endl;
                        }
                        else if(has_normal && cur_field.compare("normals") == 0 && out_normal_file.is_open())
                        {
                            out_normal_file<<cur_line_str<<std::endl;
                        }
                    }
                    else if (cur_line_str.compare("points:") == 0)
                    {
                        cur_field = "points";
                        fs::path tmp_pos_file_name = current_file_path.filename().replace_extension(".pos");
                        fs::path tmp_pos_file_path = ply_cloud_dir_path/tmp_pos_file_name;
                        out_position_file.open(tmp_pos_file_path.string());
                        if (!out_position_file.is_open())
                        {
                            std::cout << "Error opening file"<<std::endl;
                        }
                    }
                    else if(cur_line_str.compare("colors:") == 0)
                    {
                        cur_field = "colors";
                        has_color = true;
                        fs::path tmp_color_file_name = current_file_path.filename().replace_extension(".clr");
                        fs::path tmp_color_file_path = ply_cloud_dir_path/tmp_color_file_name;
                        out_color_file.open(tmp_color_file_path.string());
                        if (!out_color_file.is_open())
                        {
                            std::cout << "Error opening file"<<std::endl;
                        }
                    }
                    else if(cur_line_str.compare("normals:") == 0)
                    {
                        cur_field = "normals";
                        has_normal = true;
                        fs::path tmp_normal_file_name = current_file_path.filename().replace_extension(".nor");
                        fs::path tmp_normal_file_path = ply_cloud_dir_path/tmp_normal_file_name;
                        out_normal_file.open(tmp_normal_file_path.string());
                        if (!out_normal_file.is_open())
                        {
                            std::cout << "Error opening file"<<std::endl;
                        }
                    }

                    previous_line_str = cur_line_str;

                }

                ifs.close();
                if (out_position_file.is_open())
                {
                    out_position_file.close();
                }
                if (out_color_file.is_open())
                {
                    out_color_file.close();
                }
                if (out_normal_file.is_open())
                {
                    out_normal_file.close();
                }

                std::cout<<"there are "<<point_num<<" points in this pointcloud."<<std::endl;



                fs::path tmp_pos_file_name = current_file_path.filename().replace_extension(".pos");
                fs::path tmp_pos_file_path = ply_cloud_dir_path/tmp_pos_file_name;

                fs::path tmp_color_file_name = current_file_path.filename().replace_extension(".clr");
                fs::path tmp_color_file_path = ply_cloud_dir_path/tmp_color_file_name;

                fs::path tmp_normal_file_name = current_file_path.filename().replace_extension(".nor");
                fs::path tmp_normal_file_path = ply_cloud_dir_path/tmp_normal_file_name;

                std::ifstream in_position_file;
                std::ifstream in_color_file;
                std::ifstream in_normal_file;


                in_position_file.open(tmp_pos_file_path.string());
                if (!in_position_file.is_open())
                {
                    std::cout << "Error opening file"<<std::endl;
                }
                if (has_color)
                {
                    in_color_file.open(tmp_color_file_path.string());
                    if (!in_color_file.is_open())
                    {
                        std::cout << "Error opening file"<<std::endl;
                    }       
                }
                if (has_normal)
                {
                    in_normal_file.open(tmp_normal_file_path.string());
                    if (!in_normal_file.is_open())
                    {
                        std::cout << "Error opening file"<<std::endl;
                    }       
                }


                std::string position_str;
                std::string color_str;
                std::string normal_str;
                pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
                while (getline(in_position_file,position_str))
                {
                    pcl::PointXYZRGBNormal p;
                    std::stringstream ss;
                    ss << position_str;
                    ss >> p.x;
                    ss >> p.y;
                    ss >> p.z;
                    if (has_normal)
                    {
                        getline(in_normal_file,normal_str);
                        ss.clear();
                        ss << normal_str;
                        ss >> p.normal_x;
                        ss >> p.normal_y;
                        ss >> p.normal_z;
                    }
                    if (has_color)
                    {
                        getline(in_color_file,color_str);
                        ss.clear();
                        ss << color_str;
                        float fred;
                        float fgreen;
                        float fblue;

                        ss >> fred;
                        ss >> fgreen;
                        ss >> fblue;

                        uint8_t  r = fred * 255, g = fgreen * 255, b = fblue * 255;    // Example: Red color
                        uint32_t rgb = ((uint32_t)r << 16 | (uint32_t)g << 8 | (uint32_t)b);
                        p.rgb = *reinterpret_cast<float*>(&rgb);;
                    }

                    cloud->push_back(p);

                }

                if (in_position_file.is_open())
                {
                    in_position_file.close();
                }
                if (in_normal_file.is_open())
                {
                    in_normal_file.close();
                }
                if (in_color_file.is_open())
                {
                    in_color_file.close();
                }

                fs::path tmp_pcd_file_name = current_file_path.filename().replace_extension(".pcd");
                fs::path tmp_pcd_file_path = ply_cloud_dir_path / tmp_pcd_file_name;
                pcl::io::savePCDFileASCII(tmp_pcd_file_path.string(),*cloud);

                fs::path tmp_ply_file_name = current_file_path.filename().replace_extension(".ply");
                fs::path tmp_ply_file_path = ply_cloud_dir_path / tmp_ply_file_name;
                pcl::io::savePLYFileASCII(tmp_ply_file_path.string(),*cloud);


                if (fs::exists(tmp_pos_file_path))
                {
                    fs::remove(tmp_pos_file_path);
                }
                if(fs::exists(tmp_color_file_path))
                {
                    fs::remove((tmp_color_file_path));
                }
                if (fs::exists(tmp_normal_file_path))
                {
                    fs::remove(tmp_normal_file_path);
                }
            }
        }

    }

}