123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582583584585586587588589590591592593594595596597598599600601602603 |
- #include "point_sift_segmentation.h"
- #include <glog/logging.h>
- #include <fstream>
- #include <algorithm>
- #include <pcl/filters//voxel_grid.h>
- #include <pcl/filters/passthrough.h>
- #include <time.h>
- #include <sys/time.h>
- #include <chrono>
- using namespace std;
- using namespace chrono;
- void save_rgb_cloud_txt(std::string txt, pcl::PointCloud<pcl::PointXYZRGB>& 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<pcl::PointCloud<pcl::PointXYZRGB>::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<microseconds>(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<pcl::PointXYZ>::Ptr p_cloud_in,
- std::vector<pcl::PointCloud<pcl::PointXYZRGB>::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 = "<<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 = "<<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 = "<<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 = "<<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 = "<<p_cloud_in->size() << std::endl;
- //第五步
- pcl::PointCloud<pcl::PointXYZ>::Ptr p_cloud_select(new pcl::PointCloud<pcl::PointXYZ>);
- //从点云中选出固定数量的随机点, 默认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 = "<<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<microseconds>(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<pcl::PointXYZ>::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<pcl::PointXYZ> 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<pcl::PointXYZ>::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<pcl::PointXYZ> passX;
- pcl::PassThrough<pcl::PointXYZ> passY;
- pcl::PassThrough<pcl::PointXYZ> 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<pcl::PointXYZ>::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<pcl::PointXYZ>::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 = "<<t_grid_length << " t_grid_width = "<<t_grid_width<< " t_grid_height = "<<t_grid_height<< std::endl;
- // std::cout << "---------------------------"<< std::endl;
- //创建网格->三维点的map, 每个网格里面只保留一个点,
- map<int, pcl::PointXYZ> 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 = "<<id_x << " id_y = "<<id_y<< " id_y = "<<id_y<< std::endl;
- 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)
- {
- //无效点.不要.
- continue;
- }
- grid_point_map[id_x + id_y*t_grid_length + id_z*t_grid_length*t_grid_width] = point;
- //注:如果有多个点属于同一个网格, 那个后者就会覆盖前者, 最终保留一个点.
- }
- //检查map
- if ( grid_point_map.size() >0 )
- {
- //清空点云, 然后将map里面的点 重新加入p_cloud
- p_cloud->clear();
- for ( auto iter = grid_point_map.begin(); iter != grid_point_map.end(); iter++ )
- {
- p_cloud->push_back(iter->second);
- }
- }
- else
- {
- return Error_manager(Error_code::LOCATER_SIFT_GRID_ERROR, Error_level::MINOR_ERROR,
- " filter_cloud_with_my_grid error ");
- }
- /*
- //创建网格的数据, 内存大小为t_grid_number, 储存内容为是否存在点云的标记位.
- bool* tp_grid = (bool*)malloc(t_grid_number*sizeof(bool));
- memset(tp_grid, 0, t_grid_number*sizeof(bool));
- //遍历输入点云, 将每个点映射到输入网格, 每个网格最多保留一个点,多余的删除.
- for(pcl::PointCloud<pcl::PointXYZ>::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<pcl::PointXYZ>::Ptr p_cloud_in, pcl::PointCloud<pcl::PointXYZ>::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<pcl::PointXYZ>::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<pcl::PointCloud<pcl::PointXYZRGB>::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<pcl::PointXYZRGB>::Ptr cloud_rgb(new pcl::PointCloud<pcl::PointXYZRGB>);
- 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.x<m_minp.x
- || t_point.y>m_maxp.y || t_point.y<m_minp.y
- || t_point.z>m_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<pcl::PointCloud<pcl::PointXYZRGB>::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;
- }
|