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