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

#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);
                }
            }
        }

    }

}