#include "point_sift_segmentation.h" #include #include #include #include #include #include #include #include using namespace std; using namespace chrono; void save_rgb_cloud_txt(std::string txt, pcl::PointCloud& cloud) { std::ofstream os; os.open(txt, std::ios::out); for (int i = 0; i < cloud.points.size(); i++) { pcl::PointXYZRGB point = cloud.points[i]; char buf[255]; memset(buf, 0, 255); sprintf(buf, "%f %f %f %d %d %d\n", point.x, point.y, point.z, point.r, point.g, point.b); os.write(buf, strlen(buf)); } os.close(); } void save_rgb_cloud_txt(std::string txt, std::vector::Ptr>& cloud_vector) { std::ofstream os; os.open(txt, std::ios::out); for (int j = 0; j < cloud_vector.size(); ++j) { for (int i = 0; i < cloud_vector[j]->points.size(); i++) { pcl::PointXYZRGB point = cloud_vector[j]->points[i]; char buf[255]; memset(buf, 0, 255); sprintf(buf, "%f %f %f %d %d %d\n", point.x, point.y, point.z, point.r, point.g, point.b); os.write(buf, strlen(buf)); } } os.close(); } Point_sift_segmentation::Point_sift_segmentation(int point_size, int cls, float freq, pcl::PointXYZ minp, pcl::PointXYZ maxp) :PointSifter(point_size, cls) { m_point_num = point_size; m_cls_num = cls; m_freq = freq; m_minp = minp; m_maxp = maxp; } Point_sift_segmentation::Point_sift_segmentation(int point_size,int cls,float freq, Cloud_box cloud_box) :PointSifter(point_size, cls) { m_point_num = point_size; m_cls_num = cls; m_freq = freq; m_cloud_box_limit = cloud_box; } Point_sift_segmentation::~Point_sift_segmentation() { } Error_manager Point_sift_segmentation::init(std::string graph, std::string cpkt) { if (!Load(graph, cpkt)) { std::string error_string="pointSIFT Init ERROR:"; error_string+=LastError(); return Error_manager(LOCATER_SIFT_INIT_FAILED,MINOR_ERROR,error_string); } //创建空数据,第一次初始化后空跑 float* cloud_data = (float*)malloc(m_point_num * 3 * sizeof(float)); float* output = (float*)malloc(m_point_num * m_cls_num * sizeof(float)); auto start = system_clock::now(); if (false == Predict(cloud_data, output)) { free(cloud_data); free(output); std::string error_string="pointSIFT int first predict ERROR:"; error_string+=LastError(); return Error_manager(LOCATER_SIFT_PREDICT_FAILED,MINOR_ERROR,error_string); } else { free(cloud_data); free(output); } auto end = system_clock::now(); auto duration = duration_cast(end - start); cout << "花费了" << double(duration.count()) * microseconds::period::num / microseconds::period::den << "秒" << endl; return SUCCESS; } //分割函数,输入点云,输出各类的点云,并保存中间文件方便查看识别结果. //cloud:输入点云 //clouds:输出分类好的多个点云(附带颜色,红色为杂物,绿色为车,白色为地面)(此时里面没有内存) Error_manager Point_sift_segmentation::segmentation(pcl::PointCloud::Ptr p_cloud_in, std::vector::Ptr>& cloud_vector, bool save_flag, std::string save_dir) { if(p_cloud_in.get()==NULL) { return Error_manager(LOCATER_SIFT_INPUT_CLOUD_UNINIT,MINOR_ERROR,"sift input cloud uninit"); } if(p_cloud_in->size()<=0) { return Error_manager(LOCATER_SIFT_INPUT_CLOUD_EMPTY,MINOR_ERROR,"locate_sift input cloud empty"); } Error_manager t_error; std::cout << "p_cloud_in = "<size() << std::endl; //第一步 // 使用体素网格, 抽稀点云, //将点云空间分为小方格,每个小方格只保留一个点. t_error = filter_cloud_with_voxel_grid(p_cloud_in); if ( t_error != Error_code::SUCCESS ) { return t_error; } std::cout << "p_cloud_in = "<size() << std::endl; //第二步 // 使用box限定点云范围,只保留box范围内部的点, 剔除外部的点云 t_error = filter_cloud_with_box(p_cloud_in); if ( t_error != Error_code::SUCCESS ) { return t_error; } std::cout << "p_cloud_in = "<size() << std::endl; //第三步 / // /通过输入点云,更新区域范围, 范围不一定是box的边界,可能比box要小. t_error = update_region(p_cloud_in); if ( t_error != Error_code::SUCCESS ) { return t_error; } std::cout << "p_cloud_in = "<size() << std::endl; //第四步 // 抽稀点云, //自己写的算法, 重新生成新的点云. t_error = filter_cloud_with_my_grid(p_cloud_in); if ( t_error != Error_code::SUCCESS ) { return t_error; } std::cout << "p_cloud_in = "<size() << std::endl; //第五步 pcl::PointCloud::Ptr p_cloud_select(new pcl::PointCloud); //从点云中选出固定数量的随机点, 默认8192个点 t_error = filter_cloud_with_select(p_cloud_in, p_cloud_select); if ( t_error != Error_code::SUCCESS ) { return t_error; } std::cout << "p_cloud_select = "<size() << std::endl; //第六步 //把pcl点云转化为float, 因为TensorFlow算法只能识别float //提前分配内存, 后续记得回收.... float* p_data_point = (float*)malloc(m_point_num * 3 * sizeof(float)); //8192*3 float, 8192个三维点 float* p_data_type = (float*)malloc(m_point_num*m_cls_num*sizeof(float)); //8192*2 float, 8192个类别百分比(车轮和车身2类) memset(p_data_point, 0, m_point_num * 3 * sizeof(float)); memset(p_data_type, 0, m_point_num*m_cls_num * sizeof(float)); //将点云数据转换到float* t_error = translate_cloud_to_float(p_cloud_select, p_data_point); if ( t_error != Error_code::SUCCESS ) { free(p_data_point); free(p_data_type); return t_error; } //第七步 //tensonflow预测点云, 计算出每个三维点的类别百分比, auto start = system_clock::now(); if (!Predict(p_data_point, p_data_type)) { free(p_data_point); free(p_data_type); return Error_manager(LOCATER_SIFT_PREDICT_FAILED,MINOR_ERROR,"pointSIFT predict ERROR"); } auto end = system_clock::now(); auto duration = duration_cast(end - start); cout << "花费了" << double(duration.count()) * microseconds::period::num / microseconds::period::den << "秒" << endl; cout << double(duration.count()) << " " << microseconds::period::num << " " << microseconds::period::den << "秒" << endl; //第八步 //恢复点云,并填充到cloud_vector t_error = recovery_float_to_cloud(p_data_type, p_data_point, cloud_vector); if ( t_error != Error_code::SUCCESS ) { free(p_data_point); free(p_data_type); return t_error; } //第九步 //保存点云数据 if ( save_flag ) { t_error = save_cloud(cloud_vector,save_dir); if ( t_error != Error_code::SUCCESS ) { free(p_data_point); free(p_data_type); return t_error; } } //第十步 free(p_data_point); free(p_data_type); return Error_code::SUCCESS; } //使用体素网格, 抽稀点云, //将点云空间分为小方格,每个小方格只保留一个点. Error_manager Point_sift_segmentation::filter_cloud_with_voxel_grid(pcl::PointCloud::Ptr p_cloud) { if(p_cloud.get()==0) { return Error_manager(LOCATER_SIFT_INPUT_CLOUD_UNINIT,MINOR_ERROR,"sift input cloud uninit"); } if(p_cloud->size()<=0) { return Error_manager(LOCATER_SIFT_INPUT_CLOUD_EMPTY,MINOR_ERROR,"locate_sift input cloud empty"); } //体素网格 下采样 pcl::VoxelGrid vgfilter; vgfilter.setInputCloud(p_cloud); vgfilter.setLeafSize(0.02f, 0.02f, 0.02f); vgfilter.filter(*p_cloud); return Error_code::SUCCESS; } //使用box限定点云范围,只保留box范围内部的点, 剔除外部的点云 Error_manager Point_sift_segmentation::filter_cloud_with_box(pcl::PointCloud::Ptr p_cloud) { if(p_cloud.get()==0) { return Error_manager(LOCATER_SIFT_INPUT_CLOUD_UNINIT,MINOR_ERROR,"sift input cloud uninit"); } if(p_cloud->size()<=0) { return Error_manager(LOCATER_SIFT_INPUT_CLOUD_EMPTY,MINOR_ERROR,"locate_sift input cloud empty"); } //限制 x y pcl::PassThrough passX; pcl::PassThrough passY; pcl::PassThrough passZ; passX.setInputCloud(p_cloud); passX.setFilterFieldName("x"); passX.setFilterLimits(m_cloud_box_limit.x_min, m_cloud_box_limit.x_max); passX.filter(*p_cloud); passY.setInputCloud(p_cloud); passY.setFilterFieldName("y"); passY.setFilterLimits(m_cloud_box_limit.y_min, m_cloud_box_limit.y_max); passY.filter(*p_cloud); passZ.setInputCloud(p_cloud); passZ.setFilterFieldName("z"); passZ.setFilterLimits(m_cloud_box_limit.z_min, m_cloud_box_limit.z_max); passZ.filter(*p_cloud); return Error_code::SUCCESS; } //通过输入点云,更新边界区域范围 Error_manager Point_sift_segmentation::update_region(pcl::PointCloud::Ptr p_cloud) { pcl::PointXYZ min_point,max_point; pcl::getMinMax3D(*p_cloud,min_point,max_point); if(max_point.x<=min_point.x || max_point.y<=min_point.y) { return Error_manager(LOCATER_SIFT_INPUT_BOX_PARAMETER_FAILED,MINOR_ERROR, "Point sift update_region errror :m_maxp.x<=m_minp.x || m_maxp.y<=m_minp.y "); } m_minp = min_point; m_maxp = max_point; return Error_code::SUCCESS; } //抽稀点云, //自己写的算法, 重新生成新的点云. Error_manager Point_sift_segmentation::filter_cloud_with_my_grid(pcl::PointCloud::Ptr p_cloud) { if(p_cloud.get()==NULL) { return Error_manager(LOCATER_SIFT_INPUT_CLOUD_UNINIT,MINOR_ERROR,"sift input cloud uninit"); } if(p_cloud->size()<=0) { return Error_manager(LOCATER_SIFT_INPUT_CLOUD_EMPTY,MINOR_ERROR,"locate_sift input cloud empty"); } Error_manager t_error; //可以按照输入点云的数量, 然后动态修改 m_freq , 让其在抽稀之后,点云数量接近8192 //以后在写. //按照m_freq的分辨率, 将点云拆分为很多网格 //网格大小 m_freq 的3次方, 网格总数 grid_number int t_grid_length = int((m_maxp.x - m_minp.x) / m_freq) + 1; int t_grid_width = int((m_maxp.y - m_minp.y) / m_freq) + 1; int t_grid_height = int((m_maxp.z - m_minp.z) / m_freq) + 1; int t_grid_number = t_grid_length * t_grid_width * t_grid_height; // std::cout << " t_grid_length = "<三维点的map, 每个网格里面只保留一个点, map grid_point_map; //遍历输入点云, 将每个点的索引编号写入对应的网格. for (int i = 0; i < p_cloud->size(); ++i) { //找到对应的网格. pcl::PointXYZ point = p_cloud->points[i]; int id_x = (point.x - m_minp.x) / m_freq; int id_y = (point.y - m_minp.y) / m_freq; int id_z = (point.z - m_minp.z) / m_freq; // std::cout << "------------point:"<< point << std::endl; // std::cout << "------------m_minp:"<< m_minp << std::endl; // std::cout << "------------m_freq:"<< m_freq << std::endl; // std::cout << " id_x = "<::iterator iter = p_cloud->begin(); iter !=p_cloud->end(); ) { //找到对应的网格. int id_x = (iter->x - m_minp.x) / m_freq; int id_y = (iter->y - m_minp.y) / m_freq; int id_z = (iter->z - m_minp.z) / m_freq; if (id_x < 0 || id_x >= t_grid_length || id_y < 0 || id_y >= t_grid_width || id_z < 0 || id_z >= t_grid_height) { //无效点, 直接删除 iter = p_cloud->erase(iter);//删除当前节点,自动指向下一个节点 } else if(*(tp_grid + id_x + id_y*t_grid_length + id_z*t_grid_length*t_grid_width) == true) { //标记位为true,就表示这个网格已经有点了, 那么直接删除后者,保留前者 iter = p_cloud->erase(iter);//删除当前节点,自动指向下一个节点 } else { *(tp_grid + id_x + id_y*t_grid_length + id_z*t_grid_length*t_grid_width) = true; iter++; } } free(tp_grid); */ return Error_code::SUCCESS; } //从点云中选出固定数量的随机点, 默认8192个点 Error_manager Point_sift_segmentation::filter_cloud_with_select(pcl::PointCloud::Ptr p_cloud_in, pcl::PointCloud::Ptr p_cloud_out) { if(p_cloud_in.get()==NULL || p_cloud_out.get()==NULL) { return Error_manager(LOCATER_SIFT_INPUT_CLOUD_UNINIT,MINOR_ERROR,"sift input cloud uninit"); } if(p_cloud_in->size()<=0) { return Error_manager(LOCATER_SIFT_INPUT_CLOUD_EMPTY,MINOR_ERROR,"locate_sift input cloud empty"); } Error_manager t_error; //从输入点云 p_cloud_in 里面选出 m_point_num 个点, 然后加入 p_cloud_out //输入个数大于8192 if (p_cloud_in->size() > m_point_num) { //将点云随机打乱 std::random_shuffle(p_cloud_in->points.begin(), p_cloud_in->points.end()); //选出输入点云前面8192个点. p_cloud_out->points.assign(p_cloud_in->points.begin(), p_cloud_in->points.begin() + m_point_num); } //输入个数小鱼8192 else if (p_cloud_in->size() < m_point_num) { int add = m_point_num - p_cloud_in->size(); //注意了, 输入点云不能少于4096, 否则点云分布太少, 会严重影响到TensorFlow的图像识别. if (add > p_cloud_in->size()) { return Error_manager(Error_code::LOCATER_SIFT_CLOUD_VERY_LITTLE, Error_level::MINOR_ERROR, " filter_cloud_with_select p_cloud_in->size() very little "); } //将点云随机打乱 std::random_shuffle(p_cloud_in->points.begin(), p_cloud_in->points.end()); //选出输入点云8192个点, 不够的在加一次, p_cloud_out->points.assign(p_cloud_in->points.begin(), p_cloud_in->points.begin() + add); p_cloud_out->points.insert(p_cloud_out->points.begin(), p_cloud_in->points.begin(), p_cloud_in->points.end()); } //输入个数等于8192 else { //此时就不需要打乱了, 直接全部复制. p_cloud_out->points.assign(p_cloud_in->points.begin(), p_cloud_in->points.end()); } if (p_cloud_out->points.size() != m_point_num) { return Error_manager(Error_code::LOCATER_SIFT_SELECT_ERROR, Error_level::MINOR_ERROR, " Point_sift_segmentation::filter_cloud_with_select error "); } return Error_code::SUCCESS; } //将点云数据转换到float* Error_manager Point_sift_segmentation::translate_cloud_to_float(pcl::PointCloud::Ptr p_cloud_in, float* p_data_out) { if(p_cloud_in.get()==NULL) { return Error_manager(LOCATER_SIFT_INPUT_CLOUD_UNINIT,MINOR_ERROR,"sift input cloud uninit"); } if(p_cloud_in->size() != m_point_num) { return Error_manager(Error_code::PARAMETER_ERROR, Error_level::MINOR_ERROR, " Point_sift_segmentation::translate_cloud_to_float p_cloud_in->size() error "); } if ( p_data_out == NULL ) { return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR, " translate_cloud_to_float p_data_out POINTER_IS_NULL "); } for (int i = 0; i < m_point_num; ++i) { pcl::PointXYZ point = p_cloud_in->points[i]; *(p_data_out + i * 3) = point.x; *(p_data_out + i * 3+1) = point.y; *(p_data_out + i * 3+2) = point.z ; //注:三维点云的数据 不用做缩放和平移, // 这个在雷达扫描时就进行了标定和转化, 雷达传输过来的坐标就已经是公共坐标系了, (单位米) } return Error_code::SUCCESS; } //恢复点云, 将float*转换成点云 //p_data_type:输入点云对应的类别,大小为 点数*类别 //p_data_point:输入点云数据(xyz) //cloud_vector::输出带颜色的点云vector Error_manager Point_sift_segmentation::recovery_float_to_cloud(float* p_data_type, float* p_data_point, std::vector::Ptr>& cloud_vector) { if ( p_data_type == NULL || p_data_point == NULL ) { return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR, " recovery_float_to_cloud p_data_type or p_data_point POINTER_IS_NULL "); } //为cloud_vector 添加m_cls_num个点云, 提前分配内存 for (int k = 0; k < m_cls_num; ++k) { pcl::PointCloud::Ptr cloud_rgb(new pcl::PointCloud); cloud_vector.push_back(cloud_rgb); } //遍历data数据, for (int i = 0; i < m_point_num; ++i) { pcl::PointXYZRGB t_point; t_point.x = *(p_data_point + i * 3); t_point.y = *(p_data_point + i * 3 + 1); t_point.z = *(p_data_point + i * 3 + 2); if (t_point.x > m_maxp.x || t_point.xm_maxp.y || t_point.ym_maxp.z || t_point.z < m_minp.z) { continue; } //当前点 的类别百分比的指针 float* tp_type_percent = p_data_type + i*m_cls_num; //当前点 的类别, 初始化为0 int t_cls = 0; //当前点 的类别, 百分比的最大值 float t_type_percent_max = tp_type_percent[0]; //循环比较, 找出百分比最大的类别 for (int j = 1; j < m_cls_num; j++) //前面已经把第0个用来初始化了, 后续从第下一个开始比较 { if (tp_type_percent[j] > t_type_percent_max) { t_type_percent_max = tp_type_percent[j]; t_cls = j; } } //根据点的类别, 添加颜色, 并且分别加入到各自的点云里面 switch ( t_cls ) { case 0: //第0类, 绿色, 轮胎 t_point.r = 0; t_point.g = 255; t_point.b = 0; break; case 1://第1类, 白色, 车身 t_point.r = 255; t_point.g = 255; t_point.b = 255; break; // case 2: // ; // break; default: break; } cloud_vector[t_cls]->push_back(t_point); } //校验点云的数量, 要求size>0 if(cloud_vector[0]->size() <=0) { return Error_manager(Error_code::LOCATER_SIFT_PREDICT_NO_WHEEL_POINT, Error_level::MINOR_ERROR, " recovery_float_to_cloud NO_WHEEL_POINT "); } if(cloud_vector[1]->size() <=0) { return Error_manager(Error_code::LOCATER_SIFT_PREDICT_NO_CAR_POINT, Error_level::MINOR_ERROR, " recovery_float_to_cloud NO_CAR_POINT "); } return Error_code::SUCCESS; } //保存点云数据 Error_manager Point_sift_segmentation::save_cloud(std::vector::Ptr>& cloud_vector,std::string save_dir) { if(cloud_vector.size()>0) { std::string sift_in = save_dir + "/SIFT_cls_0.txt"; save_rgb_cloud_txt(sift_in, *cloud_vector[0]); } if(cloud_vector.size()>1) { std::string sift_in = save_dir + "/SIFT_cls_1.txt"; save_rgb_cloud_txt(sift_in, *cloud_vector[1]); } if(cloud_vector.size()>2) { std::string sift_in = save_dir + "/SIFT_cls_2.txt"; save_rgb_cloud_txt(sift_in, *cloud_vector[2]); } std::string sift_in = save_dir + "/SIFT_TOTAL.txt"; save_rgb_cloud_txt(sift_in, cloud_vector); return Error_code::SUCCESS; }