将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);
}
}
}
}
}
下一篇: 基数排序原理及其实现