point_sift_segmentation.cpp 19 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582583584585586587588589590591592593594595596597598599600601602603
  1. #include "point_sift_segmentation.h"
  2. #include <glog/logging.h>
  3. #include <fstream>
  4. #include <algorithm>
  5. #include <pcl/filters//voxel_grid.h>
  6. #include <pcl/filters/passthrough.h>
  7. #include <time.h>
  8. #include <sys/time.h>
  9. #include <chrono>
  10. using namespace std;
  11. using namespace chrono;
  12. void save_rgb_cloud_txt(std::string txt, pcl::PointCloud<pcl::PointXYZRGB>& cloud)
  13. {
  14. std::ofstream os;
  15. os.open(txt, std::ios::out);
  16. for (int i = 0; i < cloud.points.size(); i++)
  17. {
  18. pcl::PointXYZRGB point = cloud.points[i];
  19. char buf[255];
  20. memset(buf, 0, 255);
  21. sprintf(buf, "%f %f %f %d %d %d\n", point.x, point.y, point.z, point.r, point.g, point.b);
  22. os.write(buf, strlen(buf));
  23. }
  24. os.close();
  25. }
  26. void save_rgb_cloud_txt(std::string txt, std::vector<pcl::PointCloud<pcl::PointXYZRGB>::Ptr>& cloud_vector)
  27. {
  28. std::ofstream os;
  29. os.open(txt, std::ios::out);
  30. for (int j = 0; j < cloud_vector.size(); ++j)
  31. {
  32. for (int i = 0; i < cloud_vector[j]->points.size(); i++)
  33. {
  34. pcl::PointXYZRGB point = cloud_vector[j]->points[i];
  35. char buf[255];
  36. memset(buf, 0, 255);
  37. sprintf(buf, "%f %f %f %d %d %d\n", point.x, point.y, point.z, point.r, point.g, point.b);
  38. os.write(buf, strlen(buf));
  39. }
  40. }
  41. os.close();
  42. }
  43. Point_sift_segmentation::Point_sift_segmentation(int point_size, int cls, float freq, pcl::PointXYZ minp, pcl::PointXYZ maxp)
  44. :PointSifter(point_size, cls)
  45. {
  46. m_point_num = point_size;
  47. m_cls_num = cls;
  48. m_freq = freq;
  49. m_minp = minp;
  50. m_maxp = maxp;
  51. }
  52. Point_sift_segmentation::Point_sift_segmentation(int point_size,int cls,float freq, Cloud_box cloud_box)
  53. :PointSifter(point_size, cls)
  54. {
  55. m_point_num = point_size;
  56. m_cls_num = cls;
  57. m_freq = freq;
  58. m_cloud_box_limit = cloud_box;
  59. }
  60. Point_sift_segmentation::~Point_sift_segmentation()
  61. {
  62. }
  63. Error_manager Point_sift_segmentation::init(std::string graph, std::string cpkt)
  64. {
  65. if (!Load(graph, cpkt))
  66. {
  67. std::string error_string="pointSIFT Init ERROR:";
  68. error_string+=LastError();
  69. return Error_manager(LOCATER_SIFT_INIT_FAILED,MINOR_ERROR,error_string);
  70. }
  71. //创建空数据,第一次初始化后空跑
  72. float* cloud_data = (float*)malloc(m_point_num * 3 * sizeof(float));
  73. float* output = (float*)malloc(m_point_num * m_cls_num * sizeof(float));
  74. auto start = system_clock::now();
  75. if (false == Predict(cloud_data, output))
  76. {
  77. free(cloud_data);
  78. free(output);
  79. std::string error_string="pointSIFT int first predict ERROR:";
  80. error_string+=LastError();
  81. return Error_manager(LOCATER_SIFT_PREDICT_FAILED,MINOR_ERROR,error_string);
  82. }
  83. else
  84. {
  85. free(cloud_data);
  86. free(output);
  87. }
  88. auto end = system_clock::now();
  89. auto duration = duration_cast<microseconds>(end - start);
  90. cout << "花费了"
  91. << double(duration.count()) * microseconds::period::num / microseconds::period::den << "秒" << endl;
  92. return SUCCESS;
  93. }
  94. //分割函数,输入点云,输出各类的点云,并保存中间文件方便查看识别结果.
  95. //cloud:输入点云
  96. //clouds:输出分类好的多个点云(附带颜色,红色为杂物,绿色为车,白色为地面)(此时里面没有内存)
  97. Error_manager Point_sift_segmentation::segmentation(pcl::PointCloud<pcl::PointXYZ>::Ptr p_cloud_in,
  98. std::vector<pcl::PointCloud<pcl::PointXYZRGB>::Ptr>& cloud_vector,
  99. bool save_flag, std::string save_dir)
  100. {
  101. if(p_cloud_in.get()==NULL)
  102. {
  103. return Error_manager(LOCATER_SIFT_INPUT_CLOUD_UNINIT,MINOR_ERROR,"sift input cloud uninit");
  104. }
  105. if(p_cloud_in->size()<=0)
  106. {
  107. return Error_manager(LOCATER_SIFT_INPUT_CLOUD_EMPTY,MINOR_ERROR,"locate_sift input cloud empty");
  108. }
  109. Error_manager t_error;
  110. std::cout << "p_cloud_in = "<<p_cloud_in->size() << std::endl;
  111. //第一步
  112. // 使用体素网格, 抽稀点云, //将点云空间分为小方格,每个小方格只保留一个点.
  113. t_error = filter_cloud_with_voxel_grid(p_cloud_in);
  114. if ( t_error != Error_code::SUCCESS )
  115. {
  116. return t_error;
  117. }
  118. std::cout << "p_cloud_in = "<<p_cloud_in->size() << std::endl;
  119. //第二步
  120. // 使用box限定点云范围,只保留box范围内部的点, 剔除外部的点云
  121. t_error = filter_cloud_with_box(p_cloud_in);
  122. if ( t_error != Error_code::SUCCESS )
  123. {
  124. return t_error;
  125. }
  126. std::cout << "p_cloud_in = "<<p_cloud_in->size() << std::endl;
  127. //第三步 /
  128. // /通过输入点云,更新区域范围, 范围不一定是box的边界,可能比box要小.
  129. t_error = update_region(p_cloud_in);
  130. if ( t_error != Error_code::SUCCESS )
  131. {
  132. return t_error;
  133. }
  134. std::cout << "p_cloud_in = "<<p_cloud_in->size() << std::endl;
  135. //第四步
  136. // 抽稀点云, //自己写的算法, 重新生成新的点云.
  137. t_error = filter_cloud_with_my_grid(p_cloud_in);
  138. if ( t_error != Error_code::SUCCESS )
  139. {
  140. return t_error;
  141. }
  142. std::cout << "p_cloud_in = "<<p_cloud_in->size() << std::endl;
  143. //第五步
  144. pcl::PointCloud<pcl::PointXYZ>::Ptr p_cloud_select(new pcl::PointCloud<pcl::PointXYZ>);
  145. //从点云中选出固定数量的随机点, 默认8192个点
  146. t_error = filter_cloud_with_select(p_cloud_in, p_cloud_select);
  147. if ( t_error != Error_code::SUCCESS )
  148. {
  149. return t_error;
  150. }
  151. std::cout << "p_cloud_select = "<<p_cloud_select->size() << std::endl;
  152. //第六步
  153. //把pcl点云转化为float, 因为TensorFlow算法只能识别float
  154. //提前分配内存, 后续记得回收....
  155. float* p_data_point = (float*)malloc(m_point_num * 3 * sizeof(float)); //8192*3 float, 8192个三维点
  156. float* p_data_type = (float*)malloc(m_point_num*m_cls_num*sizeof(float)); //8192*2 float, 8192个类别百分比(车轮和车身2类)
  157. memset(p_data_point, 0, m_point_num * 3 * sizeof(float));
  158. memset(p_data_type, 0, m_point_num*m_cls_num * sizeof(float));
  159. //将点云数据转换到float*
  160. t_error = translate_cloud_to_float(p_cloud_select, p_data_point);
  161. if ( t_error != Error_code::SUCCESS )
  162. {
  163. free(p_data_point);
  164. free(p_data_type);
  165. return t_error;
  166. }
  167. //第七步
  168. //tensonflow预测点云, 计算出每个三维点的类别百分比,
  169. auto start = system_clock::now();
  170. if (!Predict(p_data_point, p_data_type))
  171. {
  172. free(p_data_point);
  173. free(p_data_type);
  174. return Error_manager(LOCATER_SIFT_PREDICT_FAILED,MINOR_ERROR,"pointSIFT predict ERROR");
  175. }
  176. auto end = system_clock::now();
  177. auto duration = duration_cast<microseconds>(end - start);
  178. cout << "花费了"
  179. << double(duration.count()) * microseconds::period::num / microseconds::period::den << "秒" << endl;
  180. cout << double(duration.count()) << " "
  181. << microseconds::period::num << " " << microseconds::period::den << "秒" << endl;
  182. //第八步
  183. //恢复点云,并填充到cloud_vector
  184. t_error = recovery_float_to_cloud(p_data_type, p_data_point, cloud_vector);
  185. if ( t_error != Error_code::SUCCESS )
  186. {
  187. free(p_data_point);
  188. free(p_data_type);
  189. return t_error;
  190. }
  191. //第九步
  192. //保存点云数据
  193. if ( save_flag )
  194. {
  195. t_error = save_cloud(cloud_vector,save_dir);
  196. if ( t_error != Error_code::SUCCESS )
  197. {
  198. free(p_data_point);
  199. free(p_data_type);
  200. return t_error;
  201. }
  202. }
  203. //第十步
  204. free(p_data_point);
  205. free(p_data_type);
  206. return Error_code::SUCCESS;
  207. }
  208. //使用体素网格, 抽稀点云, //将点云空间分为小方格,每个小方格只保留一个点.
  209. Error_manager Point_sift_segmentation::filter_cloud_with_voxel_grid(pcl::PointCloud<pcl::PointXYZ>::Ptr p_cloud)
  210. {
  211. if(p_cloud.get()==0)
  212. {
  213. return Error_manager(LOCATER_SIFT_INPUT_CLOUD_UNINIT,MINOR_ERROR,"sift input cloud uninit");
  214. }
  215. if(p_cloud->size()<=0)
  216. {
  217. return Error_manager(LOCATER_SIFT_INPUT_CLOUD_EMPTY,MINOR_ERROR,"locate_sift input cloud empty");
  218. }
  219. //体素网格 下采样
  220. pcl::VoxelGrid<pcl::PointXYZ> vgfilter;
  221. vgfilter.setInputCloud(p_cloud);
  222. vgfilter.setLeafSize(0.02f, 0.02f, 0.02f);
  223. vgfilter.filter(*p_cloud);
  224. return Error_code::SUCCESS;
  225. }
  226. //使用box限定点云范围,只保留box范围内部的点, 剔除外部的点云
  227. Error_manager Point_sift_segmentation::filter_cloud_with_box(pcl::PointCloud<pcl::PointXYZ>::Ptr p_cloud)
  228. {
  229. if(p_cloud.get()==0)
  230. {
  231. return Error_manager(LOCATER_SIFT_INPUT_CLOUD_UNINIT,MINOR_ERROR,"sift input cloud uninit");
  232. }
  233. if(p_cloud->size()<=0)
  234. {
  235. return Error_manager(LOCATER_SIFT_INPUT_CLOUD_EMPTY,MINOR_ERROR,"locate_sift input cloud empty");
  236. }
  237. //限制 x y
  238. pcl::PassThrough<pcl::PointXYZ> passX;
  239. pcl::PassThrough<pcl::PointXYZ> passY;
  240. pcl::PassThrough<pcl::PointXYZ> passZ;
  241. passX.setInputCloud(p_cloud);
  242. passX.setFilterFieldName("x");
  243. passX.setFilterLimits(m_cloud_box_limit.x_min, m_cloud_box_limit.x_max);
  244. passX.filter(*p_cloud);
  245. passY.setInputCloud(p_cloud);
  246. passY.setFilterFieldName("y");
  247. passY.setFilterLimits(m_cloud_box_limit.y_min, m_cloud_box_limit.y_max);
  248. passY.filter(*p_cloud);
  249. passZ.setInputCloud(p_cloud);
  250. passZ.setFilterFieldName("z");
  251. passZ.setFilterLimits(m_cloud_box_limit.z_min, m_cloud_box_limit.z_max);
  252. passZ.filter(*p_cloud);
  253. return Error_code::SUCCESS;
  254. }
  255. //通过输入点云,更新边界区域范围
  256. Error_manager Point_sift_segmentation::update_region(pcl::PointCloud<pcl::PointXYZ>::Ptr p_cloud)
  257. {
  258. pcl::PointXYZ min_point,max_point;
  259. pcl::getMinMax3D(*p_cloud,min_point,max_point);
  260. if(max_point.x<=min_point.x || max_point.y<=min_point.y)
  261. {
  262. return Error_manager(LOCATER_SIFT_INPUT_BOX_PARAMETER_FAILED,MINOR_ERROR,
  263. "Point sift update_region errror :m_maxp.x<=m_minp.x || m_maxp.y<=m_minp.y ");
  264. }
  265. m_minp = min_point;
  266. m_maxp = max_point;
  267. return Error_code::SUCCESS;
  268. }
  269. //抽稀点云, //自己写的算法, 重新生成新的点云.
  270. Error_manager Point_sift_segmentation::filter_cloud_with_my_grid(pcl::PointCloud<pcl::PointXYZ>::Ptr p_cloud)
  271. {
  272. if(p_cloud.get()==NULL)
  273. {
  274. return Error_manager(LOCATER_SIFT_INPUT_CLOUD_UNINIT,MINOR_ERROR,"sift input cloud uninit");
  275. }
  276. if(p_cloud->size()<=0)
  277. {
  278. return Error_manager(LOCATER_SIFT_INPUT_CLOUD_EMPTY,MINOR_ERROR,"locate_sift input cloud empty");
  279. }
  280. Error_manager t_error;
  281. //可以按照输入点云的数量, 然后动态修改 m_freq , 让其在抽稀之后,点云数量接近8192
  282. //以后在写.
  283. //按照m_freq的分辨率, 将点云拆分为很多网格
  284. //网格大小 m_freq 的3次方, 网格总数 grid_number
  285. int t_grid_length = int((m_maxp.x - m_minp.x) / m_freq) + 1;
  286. int t_grid_width = int((m_maxp.y - m_minp.y) / m_freq) + 1;
  287. int t_grid_height = int((m_maxp.z - m_minp.z) / m_freq) + 1;
  288. int t_grid_number = t_grid_length * t_grid_width * t_grid_height;
  289. // std::cout << " t_grid_length = "<<t_grid_length << " t_grid_width = "<<t_grid_width<< " t_grid_height = "<<t_grid_height<< std::endl;
  290. // std::cout << "---------------------------"<< std::endl;
  291. //创建网格->三维点的map, 每个网格里面只保留一个点,
  292. map<int, pcl::PointXYZ> grid_point_map;
  293. //遍历输入点云, 将每个点的索引编号写入对应的网格.
  294. for (int i = 0; i < p_cloud->size(); ++i)
  295. {
  296. //找到对应的网格.
  297. pcl::PointXYZ point = p_cloud->points[i];
  298. int id_x = (point.x - m_minp.x) / m_freq;
  299. int id_y = (point.y - m_minp.y) / m_freq;
  300. int id_z = (point.z - m_minp.z) / m_freq;
  301. // std::cout << "------------point:"<< point << std::endl;
  302. // std::cout << "------------m_minp:"<< m_minp << std::endl;
  303. // std::cout << "------------m_freq:"<< m_freq << std::endl;
  304. // std::cout << " id_x = "<<id_x << " id_y = "<<id_y<< " id_y = "<<id_y<< std::endl;
  305. 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)
  306. {
  307. //无效点.不要.
  308. continue;
  309. }
  310. grid_point_map[id_x + id_y*t_grid_length + id_z*t_grid_length*t_grid_width] = point;
  311. //注:如果有多个点属于同一个网格, 那个后者就会覆盖前者, 最终保留一个点.
  312. }
  313. //检查map
  314. if ( grid_point_map.size() >0 )
  315. {
  316. //清空点云, 然后将map里面的点 重新加入p_cloud
  317. p_cloud->clear();
  318. for ( auto iter = grid_point_map.begin(); iter != grid_point_map.end(); iter++ )
  319. {
  320. p_cloud->push_back(iter->second);
  321. }
  322. }
  323. else
  324. {
  325. return Error_manager(Error_code::LOCATER_SIFT_GRID_ERROR, Error_level::MINOR_ERROR,
  326. " filter_cloud_with_my_grid error ");
  327. }
  328. /*
  329. //创建网格的数据, 内存大小为t_grid_number, 储存内容为是否存在点云的标记位.
  330. bool* tp_grid = (bool*)malloc(t_grid_number*sizeof(bool));
  331. memset(tp_grid, 0, t_grid_number*sizeof(bool));
  332. //遍历输入点云, 将每个点映射到输入网格, 每个网格最多保留一个点,多余的删除.
  333. for(pcl::PointCloud<pcl::PointXYZ>::iterator iter = p_cloud->begin(); iter !=p_cloud->end(); )
  334. {
  335. //找到对应的网格.
  336. int id_x = (iter->x - m_minp.x) / m_freq;
  337. int id_y = (iter->y - m_minp.y) / m_freq;
  338. int id_z = (iter->z - m_minp.z) / m_freq;
  339. 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)
  340. {
  341. //无效点, 直接删除
  342. iter = p_cloud->erase(iter);//删除当前节点,自动指向下一个节点
  343. }
  344. else if(*(tp_grid + id_x + id_y*t_grid_length + id_z*t_grid_length*t_grid_width) == true)
  345. {
  346. //标记位为true,就表示这个网格已经有点了, 那么直接删除后者,保留前者
  347. iter = p_cloud->erase(iter);//删除当前节点,自动指向下一个节点
  348. }
  349. else
  350. {
  351. *(tp_grid + id_x + id_y*t_grid_length + id_z*t_grid_length*t_grid_width) = true;
  352. iter++;
  353. }
  354. }
  355. free(tp_grid);
  356. */
  357. return Error_code::SUCCESS;
  358. }
  359. //从点云中选出固定数量的随机点, 默认8192个点
  360. 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)
  361. {
  362. if(p_cloud_in.get()==NULL || p_cloud_out.get()==NULL)
  363. {
  364. return Error_manager(LOCATER_SIFT_INPUT_CLOUD_UNINIT,MINOR_ERROR,"sift input cloud uninit");
  365. }
  366. if(p_cloud_in->size()<=0)
  367. {
  368. return Error_manager(LOCATER_SIFT_INPUT_CLOUD_EMPTY,MINOR_ERROR,"locate_sift input cloud empty");
  369. }
  370. Error_manager t_error;
  371. //从输入点云 p_cloud_in 里面选出 m_point_num 个点, 然后加入 p_cloud_out
  372. //输入个数大于8192
  373. if (p_cloud_in->size() > m_point_num)
  374. {
  375. //将点云随机打乱
  376. std::random_shuffle(p_cloud_in->points.begin(), p_cloud_in->points.end());
  377. //选出输入点云前面8192个点.
  378. p_cloud_out->points.assign(p_cloud_in->points.begin(), p_cloud_in->points.begin() + m_point_num);
  379. }
  380. //输入个数小鱼8192
  381. else if (p_cloud_in->size() < m_point_num)
  382. {
  383. int add = m_point_num - p_cloud_in->size();
  384. //注意了, 输入点云不能少于4096, 否则点云分布太少, 会严重影响到TensorFlow的图像识别.
  385. if (add > p_cloud_in->size())
  386. {
  387. return Error_manager(Error_code::LOCATER_SIFT_CLOUD_VERY_LITTLE, Error_level::MINOR_ERROR,
  388. " filter_cloud_with_select p_cloud_in->size() very little ");
  389. }
  390. //将点云随机打乱
  391. std::random_shuffle(p_cloud_in->points.begin(), p_cloud_in->points.end());
  392. //选出输入点云8192个点, 不够的在加一次,
  393. p_cloud_out->points.assign(p_cloud_in->points.begin(), p_cloud_in->points.begin() + add);
  394. p_cloud_out->points.insert(p_cloud_out->points.begin(), p_cloud_in->points.begin(), p_cloud_in->points.end());
  395. }
  396. //输入个数等于8192
  397. else
  398. {
  399. //此时就不需要打乱了, 直接全部复制.
  400. p_cloud_out->points.assign(p_cloud_in->points.begin(), p_cloud_in->points.end());
  401. }
  402. if (p_cloud_out->points.size() != m_point_num)
  403. {
  404. return Error_manager(Error_code::LOCATER_SIFT_SELECT_ERROR, Error_level::MINOR_ERROR,
  405. " Point_sift_segmentation::filter_cloud_with_select error ");
  406. }
  407. return Error_code::SUCCESS;
  408. }
  409. //将点云数据转换到float*
  410. Error_manager Point_sift_segmentation::translate_cloud_to_float(pcl::PointCloud<pcl::PointXYZ>::Ptr p_cloud_in, float* p_data_out)
  411. {
  412. if(p_cloud_in.get()==NULL)
  413. {
  414. return Error_manager(LOCATER_SIFT_INPUT_CLOUD_UNINIT,MINOR_ERROR,"sift input cloud uninit");
  415. }
  416. if(p_cloud_in->size() != m_point_num)
  417. {
  418. return Error_manager(Error_code::PARAMETER_ERROR, Error_level::MINOR_ERROR,
  419. " Point_sift_segmentation::translate_cloud_to_float p_cloud_in->size() error ");
  420. }
  421. if ( p_data_out == NULL )
  422. {
  423. return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR,
  424. " translate_cloud_to_float p_data_out POINTER_IS_NULL ");
  425. }
  426. for (int i = 0; i < m_point_num; ++i)
  427. {
  428. pcl::PointXYZ point = p_cloud_in->points[i];
  429. *(p_data_out + i * 3) = point.x;
  430. *(p_data_out + i * 3+1) = point.y;
  431. *(p_data_out + i * 3+2) = point.z ;
  432. //注:三维点云的数据 不用做缩放和平移,
  433. // 这个在雷达扫描时就进行了标定和转化, 雷达传输过来的坐标就已经是公共坐标系了, (单位米)
  434. }
  435. return Error_code::SUCCESS;
  436. }
  437. //恢复点云, 将float*转换成点云
  438. //p_data_type:输入点云对应的类别,大小为 点数*类别
  439. //p_data_point:输入点云数据(xyz)
  440. //cloud_vector::输出带颜色的点云vector
  441. 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)
  442. {
  443. if ( p_data_type == NULL || p_data_point == NULL )
  444. {
  445. return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR,
  446. " recovery_float_to_cloud p_data_type or p_data_point POINTER_IS_NULL ");
  447. }
  448. //为cloud_vector 添加m_cls_num个点云, 提前分配内存
  449. for (int k = 0; k < m_cls_num; ++k)
  450. {
  451. pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_rgb(new pcl::PointCloud<pcl::PointXYZRGB>);
  452. cloud_vector.push_back(cloud_rgb);
  453. }
  454. //遍历data数据,
  455. for (int i = 0; i < m_point_num; ++i)
  456. {
  457. pcl::PointXYZRGB t_point;
  458. t_point.x = *(p_data_point + i * 3);
  459. t_point.y = *(p_data_point + i * 3 + 1);
  460. t_point.z = *(p_data_point + i * 3 + 2);
  461. if (t_point.x > m_maxp.x || t_point.x<m_minp.x
  462. || t_point.y>m_maxp.y || t_point.y<m_minp.y
  463. || t_point.z>m_maxp.z || t_point.z < m_minp.z)
  464. {
  465. continue;
  466. }
  467. //当前点 的类别百分比的指针
  468. float* tp_type_percent = p_data_type + i*m_cls_num;
  469. //当前点 的类别, 初始化为0
  470. int t_cls = 0;
  471. //当前点 的类别, 百分比的最大值
  472. float t_type_percent_max = tp_type_percent[0];
  473. //循环比较, 找出百分比最大的类别
  474. for (int j = 1; j < m_cls_num; j++) //前面已经把第0个用来初始化了, 后续从第下一个开始比较
  475. {
  476. if (tp_type_percent[j] > t_type_percent_max)
  477. {
  478. t_type_percent_max = tp_type_percent[j];
  479. t_cls = j;
  480. }
  481. }
  482. //根据点的类别, 添加颜色, 并且分别加入到各自的点云里面
  483. switch ( t_cls )
  484. {
  485. case 0: //第0类, 绿色, 轮胎
  486. t_point.r = 0;
  487. t_point.g = 255;
  488. t_point.b = 0;
  489. break;
  490. case 1://第1类, 白色, 车身
  491. t_point.r = 255;
  492. t_point.g = 255;
  493. t_point.b = 255;
  494. break;
  495. // case 2:
  496. // ;
  497. // break;
  498. default:
  499. break;
  500. }
  501. cloud_vector[t_cls]->push_back(t_point);
  502. }
  503. //校验点云的数量, 要求size>0
  504. if(cloud_vector[0]->size() <=0)
  505. {
  506. return Error_manager(Error_code::LOCATER_SIFT_PREDICT_NO_WHEEL_POINT, Error_level::MINOR_ERROR,
  507. " recovery_float_to_cloud NO_WHEEL_POINT ");
  508. }
  509. if(cloud_vector[1]->size() <=0)
  510. {
  511. return Error_manager(Error_code::LOCATER_SIFT_PREDICT_NO_CAR_POINT, Error_level::MINOR_ERROR,
  512. " recovery_float_to_cloud NO_CAR_POINT ");
  513. }
  514. return Error_code::SUCCESS;
  515. }
  516. //保存点云数据
  517. Error_manager Point_sift_segmentation::save_cloud(std::vector<pcl::PointCloud<pcl::PointXYZRGB>::Ptr>& cloud_vector,std::string save_dir)
  518. {
  519. if(cloud_vector.size()>0) {
  520. std::string sift_in = save_dir + "/SIFT_cls_0.txt";
  521. save_rgb_cloud_txt(sift_in, *cloud_vector[0]);
  522. }
  523. if(cloud_vector.size()>1) {
  524. std::string sift_in = save_dir + "/SIFT_cls_1.txt";
  525. save_rgb_cloud_txt(sift_in, *cloud_vector[1]);
  526. }
  527. if(cloud_vector.size()>2) {
  528. std::string sift_in = save_dir + "/SIFT_cls_2.txt";
  529. save_rgb_cloud_txt(sift_in, *cloud_vector[2]);
  530. }
  531. std::string sift_in = save_dir + "/SIFT_TOTAL.txt";
  532. save_rgb_cloud_txt(sift_in, cloud_vector);
  533. return Error_code::SUCCESS;
  534. }