grab_point_rs.cpp 25 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582583584585586587588589590591592593594595596597598599600601602603604605606607608609610611612613614615616617618619620621622623624625626627628629630631632633634635636637638639640641642643644645646647648649650651652653654655656657658659660661662663664665666667668669670671672673674675676677678679680681682683684685686687688689690691692693694695696697698699700701702703704705706707708709710711712713714715716717718719720721722723724725726727728729730731732733734735736737738739740741742743744745746747748749750751752753754755756757758759760761762763764765766767768769770771772773774775776777778779780781782783784785786787788789790791792793794795796797798799800801802803804805806807808809810811812813814815816817818819820821822823824825826827828829830831832833834835
  1. /*
  2. 通过点云数据识别抓取位置信息
  3. 1)获取点云
  4. 2)剔除离群点
  5. 3)降采样
  6. 4)植株检测
  7. 5)选出最前,最右侧植株
  8. 6)植株抓取位置检测
  9. */
  10. #include <pcl\io\ply_io.h>
  11. #include <pcl\visualization\cloud_viewer.h>
  12. #include <pcl\filters\crop_box.h>
  13. #include <pcl\filters\radius_outlier_removal.h>
  14. #include <pcl\filters\voxel_grid.h>
  15. #include <pcl\common\common.h>
  16. #include <math.h>
  17. #include "grab_point_rs.h"
  18. #include "utils.h"
  19. #define PI std::acos(-1)
  20. namespace graft_cv {
  21. CRootStockGrabPoint::CRootStockGrabPoint(ConfigParam&cp, CGcvLogger*pLog/*=0*/)
  22. :m_cparam(cp),
  23. m_pLogger(pLog),
  24. m_dtype(0),
  25. m_pcdId(""),
  26. m_ppImgSaver(0)
  27. {
  28. }
  29. CRootStockGrabPoint::~CRootStockGrabPoint() {}
  30. float* CRootStockGrabPoint::get_raw_point_cloud(int &data_size)
  31. {
  32. data_size = m_raw_cloud->width * m_raw_cloud->height;
  33. if (data_size == 0) {
  34. return 0;
  35. }
  36. else {
  37. pcl::PointXYZ* pp = m_raw_cloud->points.data();
  38. return (float*)pp->data;
  39. }
  40. }
  41. int CRootStockGrabPoint::load_data(
  42. float*pPoint,
  43. int pixel_size,
  44. int pt_size,
  45. const char* fn/* = 0*/)
  46. {
  47. int rst = 0;
  48. //1 get point cloud data
  49. if (pPoint != 0 && pt_size>0) {
  50. //read point
  51. m_raw_cloud.reset(new pcl::PointCloud<pcl::PointXYZ>);
  52. int step = pixel_size;
  53. for (int i = 0; i < pt_size; ++i) {
  54. pcl::PointXYZ pt = { pPoint[i*step], pPoint[i*step + 1] , pPoint[i*step + 2] };
  55. m_raw_cloud->push_back(pt);
  56. }
  57. rst = m_raw_cloud->width * m_raw_cloud->height;
  58. if (m_pLogger) {
  59. stringstream buff;
  60. buff << "load raw point cloud " << rst << " data points";
  61. m_pLogger->INFO(buff.str());
  62. }
  63. }
  64. else if (fn != 0) {
  65. //read file
  66. rst = this->read_ply_file(fn);
  67. }
  68. else {//error
  69. if (m_pLogger) {
  70. m_pLogger->ERRORINFO("no valid input");
  71. return (-1);
  72. }
  73. }
  74. if (m_dtype == 0) {
  75. m_pcdId = getImgId(img_type::sola_sc_pcd);
  76. }
  77. else {
  78. m_pcdId = getImgId(img_type::sola_rs_pcd);
  79. }
  80. if (m_ppImgSaver && *m_ppImgSaver) {
  81. (*m_ppImgSaver)->saveBinPly(m_raw_cloud, m_pcdId);
  82. }
  83. if (m_cparam.image_show) {
  84. viewer_cloud(m_raw_cloud, std::string("raw point cloud"));
  85. }
  86. return rst;
  87. }
  88. int CRootStockGrabPoint::grab_point_detect(
  89. int dtype,
  90. PositionInfo& posinfo
  91. )
  92. {
  93. // dtype == 0, scion
  94. // dtype != 0, rootstock
  95. // 输入,穗苗--0, 砧木--1
  96. if (m_raw_cloud->width * m_raw_cloud->height < 1) {
  97. if (m_pLogger) {
  98. stringstream buff;
  99. buff << "m_raw_cloud point cloud " << m_raw_cloud->width * m_raw_cloud->height << " data points";
  100. m_pLogger->ERRORINFO(buff.str());
  101. }
  102. return 1;
  103. }
  104. //2 crop filter
  105. if (m_pLogger) {
  106. m_pLogger->INFO("begin crop");
  107. }
  108. pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_inbox(new pcl::PointCloud<pcl::PointXYZ>);
  109. pcl::CropBox<pcl::PointXYZ> box_filter;
  110. m_dtype = dtype;
  111. if (dtype != 0) {//rootstock
  112. box_filter.setMin(Eigen::Vector4f(m_cparam.rs_grab_xmin, m_cparam.rs_grab_ymin, m_cparam.rs_grab_zmin, 1));
  113. box_filter.setMax(Eigen::Vector4f(m_cparam.rs_grab_xmax, m_cparam.rs_grab_ymax, m_cparam.rs_grab_zmax, 1));
  114. }
  115. else {//scion
  116. box_filter.setMin(Eigen::Vector4f(m_cparam.sc_grab_xmin, m_cparam.sc_grab_ymin, m_cparam.sc_grab_zmin, 1));
  117. box_filter.setMax(Eigen::Vector4f(m_cparam.sc_grab_xmax, m_cparam.sc_grab_ymax, m_cparam.sc_grab_zmax, 1));
  118. }
  119. box_filter.setNegative(false);
  120. box_filter.setInputCloud(m_raw_cloud);
  121. box_filter.filter(*cloud_inbox);
  122. if (m_pLogger) {
  123. stringstream buff;
  124. buff << "CropBox croped point cloud " << cloud_inbox->width * cloud_inbox->height << " data points";
  125. m_pLogger->INFO(buff.str());
  126. }
  127. if (cloud_inbox->width * cloud_inbox->height < 1) {
  128. return 1;
  129. }
  130. if (m_cparam.image_show) {
  131. viewer_cloud(cloud_inbox, std::string("cloud_inbox"));
  132. }
  133. if (m_pLogger) {
  134. m_pLogger->INFO("end crop");
  135. }
  136. //3 filtler with radius remove
  137. if (m_pLogger) {
  138. m_pLogger->INFO("begin ror");
  139. }
  140. int nb_points = 20;
  141. double stem_radius = m_cparam.rs_grab_stem_diameter / 2.0;
  142. if(dtype == 0){
  143. stem_radius = m_cparam.sc_grab_stem_diameter / 2.0;
  144. }
  145. pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_ror(new pcl::PointCloud<pcl::PointXYZ>);
  146. pcl::RadiusOutlierRemoval<pcl::PointXYZ> ror;
  147. ror.setInputCloud(cloud_inbox);
  148. ror.setRadiusSearch(stem_radius);
  149. ror.setMinNeighborsInRadius(nb_points);
  150. ror.filter(*cloud_ror);
  151. if (m_pLogger) {
  152. stringstream buff;
  153. buff << "RadiusOutlierRemoval filtered point cloud " << cloud_ror->width * cloud_ror->height << " data points. param stem_radius="<<
  154. stem_radius<<", nb_points="<< nb_points;
  155. m_pLogger->INFO(buff.str());
  156. }
  157. if (cloud_ror->width * cloud_ror->height < 1) {
  158. return 1;
  159. }
  160. if (m_cparam.image_show) {
  161. viewer_cloud(cloud_ror, std::string("cloud_ror"));
  162. }
  163. if (m_pLogger) {
  164. m_pLogger->INFO("end ror");
  165. }
  166. //3 voxel grid down sampling
  167. if (m_pLogger) {
  168. m_pLogger->INFO("begin down");
  169. }
  170. pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_dowm_sampled(new pcl::PointCloud<pcl::PointXYZ>);
  171. pcl::VoxelGrid<pcl::PointXYZ> outrem;
  172. outrem.setInputCloud(cloud_ror);
  173. outrem.setLeafSize(stem_radius, stem_radius, stem_radius);
  174. outrem.filter(*cloud_dowm_sampled);
  175. if (m_pLogger) {
  176. stringstream buff;
  177. buff << "VoxelGrid dowm_sampled point cloud " << cloud_dowm_sampled->width * cloud_dowm_sampled->height << " data points (if < 200, return)";
  178. m_pLogger->INFO(buff.str());
  179. }
  180. if (cloud_dowm_sampled->width * cloud_dowm_sampled->height < 200) {
  181. return 1;
  182. }
  183. if (m_cparam.image_show) {
  184. viewer_cloud(cloud_dowm_sampled, std::string("cloud_dowm_sampled"));
  185. }
  186. if (m_pLogger) {
  187. m_pLogger->INFO("end down");
  188. }
  189. //4 seedling position
  190. std::vector<int> first_seedling_cloud_idx;
  191. pcl::PointXYZ xz_center;
  192. if (m_pLogger) {
  193. m_pLogger->INFO("begin find seedling position");
  194. }
  195. find_seedling_position(cloud_dowm_sampled, first_seedling_cloud_idx, xz_center);
  196. if (m_pLogger) {
  197. stringstream buff;
  198. buff << "after find_seedling_position(), foud first seedling seeds points size " << first_seedling_cloud_idx .size();
  199. m_pLogger->INFO(buff.str());
  200. }
  201. if (m_pLogger) {
  202. m_pLogger->INFO("end find seedling position");
  203. }
  204. //5 nearest seedling grab point selection
  205. pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_seedling_seed(new pcl::PointCloud<pcl::PointXYZ>);
  206. pcl::copyPointCloud(*cloud_dowm_sampled, first_seedling_cloud_idx, *cloud_seedling_seed);
  207. std::vector<int>mass_idx;
  208. double dist_mean = compute_nearest_neighbor_distance(cloud_dowm_sampled);
  209. std::vector<double> mass_indices;
  210. if (m_pLogger) {
  211. m_pLogger->INFO("begin crop nn_analysis");
  212. }
  213. crop_nn_analysis(cloud_ror, cloud_seedling_seed, dist_mean, mass_indices, mass_idx);
  214. if (m_pLogger) {
  215. m_pLogger->INFO("end crop nn_analysis");
  216. }
  217. double candidate_th = otsu(mass_indices);
  218. std::vector<int> optimal_seeds_idx;
  219. for (size_t j = 0; j < mass_idx.size(); ++j) {
  220. if (mass_indices.at(mass_idx.at(j)) >= candidate_th) {
  221. optimal_seeds_idx.push_back(mass_idx.at(j));
  222. }
  223. }
  224. pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_optimal_seed(new pcl::PointCloud<pcl::PointXYZ>);
  225. pcl::copyPointCloud(*cloud_seedling_seed, optimal_seeds_idx, *cloud_optimal_seed);
  226. pcl::PointXYZ selected_pt;
  227. int selected_pt_idx;
  228. if (m_pLogger) {
  229. m_pLogger->INFO("begin get_optimal_seed");
  230. }
  231. get_optimal_seed(cloud_optimal_seed, selected_pt, selected_pt_idx);
  232. if (selected_pt_idx < 0) {
  233. return 1;
  234. }
  235. if (m_pLogger) {
  236. m_pLogger->INFO("end get_optimal_seed");
  237. }
  238. posinfo.rs_grab_x = selected_pt.x;
  239. posinfo.rs_grab_y = selected_pt.y;
  240. posinfo.rs_grab_z = selected_pt.z;
  241. ////////////////////////////////////////////////////////////////////
  242. //debug
  243. if (m_cparam.image_show) {
  244. pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_cand_demo(new pcl::PointCloud<pcl::PointXYZRGB>);
  245. pcl::copyPointCloud(*cloud_dowm_sampled, *cloud_cand_demo);
  246. for (auto& pt : *cloud_cand_demo) {
  247. pt.r = 255;
  248. pt.g = 255;
  249. pt.b = 255;
  250. }
  251. int cnt = 0;
  252. for (auto& idx : mass_idx) {
  253. int p_idx = first_seedling_cloud_idx.at(idx);
  254. /*if (p_idx == optimal_seeds_idx[selected_pt_idx]) {
  255. cloud_cand_demo->points[p_idx].r = 0;
  256. cloud_cand_demo->points[p_idx].g = 255;
  257. cloud_cand_demo->points[p_idx].b = 0;
  258. }
  259. else {*/
  260. cloud_cand_demo->points.at(p_idx).r = 255;
  261. cloud_cand_demo->points.at(p_idx).g = 0;
  262. cloud_cand_demo->points.at(p_idx).b = 0;
  263. /*} */
  264. if (cnt > optimal_seeds_idx.size()) { break; }
  265. cnt++;
  266. }
  267. pcl::PointXYZRGB pt_grab = {0,255,0};
  268. pt_grab.x = selected_pt.x;
  269. pt_grab.y = selected_pt.y;
  270. pt_grab.z = selected_pt.z;
  271. pcl::PointXYZRGB pt_grab_ = { 0,255,120 };
  272. pt_grab_.x = selected_pt.x;
  273. pt_grab_.y = selected_pt.y+0.2;
  274. pt_grab_.z = selected_pt.z;
  275. cloud_cand_demo->push_back(pt_grab);
  276. //viewer_cloud(cloud_cand_demo, std::string("cloud_cand_demo"));
  277. viewer_cloud_debug(cloud_cand_demo, selected_pt, std::string("cloud_cand_demo"));
  278. }
  279. return 0;
  280. }
  281. int CRootStockGrabPoint::read_ply_file(const char* fn)
  282. {
  283. m_raw_cloud.reset( new pcl::PointCloud<pcl::PointXYZ>);
  284. if (pcl::io::loadPLYFile<pcl::PointXYZ>(fn, *m_raw_cloud) == -1) {
  285. if (m_pLogger) {
  286. m_pLogger->ERRORINFO("could not load file: "+std::string(fn));
  287. }
  288. return (-1);
  289. }
  290. if (m_pLogger) {
  291. stringstream buff;
  292. buff << "load raw point cloud " << m_raw_cloud->width * m_raw_cloud->height << " data points";
  293. m_pLogger->INFO(buff.str());
  294. }
  295. return m_raw_cloud->width * m_raw_cloud->height;
  296. }
  297. double CRootStockGrabPoint::compute_nearest_neighbor_distance(pcl::PointCloud<pcl::PointXYZ>::Ptr pcd)
  298. {
  299. pcl::KdTreeFLANN<pcl::PointXYZ> tree;
  300. tree.setInputCloud(pcd);
  301. int k = 2;
  302. double res = 0.0;
  303. int n_points = 0;
  304. for (size_t i = 0; i < pcd->size(); ++i) {
  305. std::vector<int> idx(k);
  306. std::vector<float> sqr_distances(k);
  307. if (tree.nearestKSearch(i, k, idx, sqr_distances) == k) {
  308. for (int id = 1; id < k; ++id) {
  309. res += sqrt(sqr_distances.at(id));
  310. ++n_points;
  311. }
  312. }
  313. }
  314. if (n_points > 0) {
  315. res /= (double)n_points;
  316. }
  317. return res;
  318. }
  319. void CRootStockGrabPoint::find_seedling_position(
  320. pcl::PointCloud<pcl::PointXYZ>::Ptr in_cloud,
  321. std::vector<int> &first_seedling_cloud_idx,
  322. pcl::PointXYZ&xz_center
  323. )
  324. {
  325. pcl::PointCloud<pcl::PointXYZ>::Ptr cloud2d(new pcl::PointCloud < pcl::PointXYZ>);
  326. pcl::copyPointCloud(*in_cloud, *cloud2d);
  327. for (auto&pt : *cloud2d) {
  328. pt.y = 0.0;
  329. }
  330. if(m_cparam.image_show){
  331. viewer_cloud(cloud2d, std::string("cloud2d"));
  332. }
  333. double radius = m_cparam.rs_grab_stem_diameter;
  334. if (m_dtype == 0) {
  335. radius = m_cparam.sc_grab_stem_diameter;
  336. }
  337. std::vector<int> counter;
  338. pcl::KdTreeFLANN<pcl::PointXYZ> kdtree;
  339. kdtree.setInputCloud(cloud2d);
  340. std::vector<int>idx;
  341. std::vector<float>dist_sqr;
  342. for (size_t i = 0; i < cloud2d->points.size(); ++i) {
  343. int k = kdtree.radiusSearch(cloud2d->points.at(i), radius, idx, dist_sqr);
  344. counter.push_back(k);
  345. }
  346. int th = (int)(otsu(counter));
  347. std::vector<int> index;
  348. for (size_t i = 0; i < counter.size(); ++i) {
  349. if (counter.at(i) >= th) {
  350. index.push_back(i);
  351. }
  352. }
  353. pcl::PointCloud<pcl::PointXYZ>::Ptr cloud2d_pos(new pcl::PointCloud < pcl::PointXYZ>);
  354. pcl::copyPointCloud(*cloud2d, index, *cloud2d_pos);
  355. if (m_pLogger) {
  356. stringstream buff;
  357. buff << "get 2d seedling seed point cloud " << index.size()<< " data points with thrreshold "<<th;
  358. m_pLogger->INFO(buff.str());
  359. }
  360. if (m_cparam.image_show) {
  361. viewer_cloud(cloud2d_pos, std::string("cloud2d_pos"));
  362. }
  363. //clustering
  364. double d1 = m_cparam.rs_grab_stem_diameter;
  365. double d2 = m_cparam.rs_grab_stem_diameter * 3.0;
  366. if (m_dtype == 0) {
  367. d1 = m_cparam.sc_grab_stem_diameter;
  368. d2 = m_cparam.sc_grab_stem_diameter * 3.0;
  369. }
  370. std::vector<pcl::PointXYZ>cluster_center;
  371. std::vector<std::vector<int>> cluster_member;
  372. euclidean_clustering_ttsas(cloud2d_pos, d1, d2, cluster_center, cluster_member);
  373. if (m_pLogger) {
  374. stringstream buff;
  375. buff << "euclidean_clustering_ttsas: " << cluster_center.size() << " t1=" << d1
  376. << " t2=" << d2;
  377. m_pLogger->INFO(buff.str());
  378. }
  379. //sort cluster center, get the frontest leftest one
  380. std::vector<float> cluster_index;
  381. for (auto&pt : cluster_center) {
  382. float idx = pt.x - pt.z;
  383. cluster_index.push_back(idx);
  384. }
  385. int first_idx = std::min_element(cluster_index.begin(), cluster_index.end()) - cluster_index.begin();
  386. if (m_dtype == 0) {
  387. first_idx = std::max_element(cluster_index.begin(), cluster_index.end()) - cluster_index.begin();
  388. }
  389. first_seedling_cloud_idx.clear();
  390. for (auto&v : cluster_member.at(first_idx)) {
  391. size_t i = index.at(v);
  392. first_seedling_cloud_idx.push_back(i);
  393. }
  394. xz_center.x = cluster_center.at(first_idx).x;
  395. xz_center.y = cluster_center.at(first_idx).y;
  396. xz_center.z = cluster_center.at(first_idx).z;
  397. if (m_pLogger) {
  398. stringstream buff;
  399. buff << "euclidean_clustering_ttsas, find cluster center(" << xz_center.x
  400. <<", "<< xz_center.y<<", "<< xz_center.z<<")";
  401. m_pLogger->INFO(buff.str());
  402. }
  403. }
  404. void CRootStockGrabPoint::crop_nn_analysis(
  405. pcl::PointCloud<pcl::PointXYZ>::Ptr in_cloud,
  406. pcl::PointCloud<pcl::PointXYZ>::Ptr seed_cloud,
  407. double dist_mean,
  408. std::vector<double>&mass_indices,
  409. std::vector<int>& candidate_idx
  410. )
  411. {
  412. candidate_idx.clear();
  413. pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_inbox(new pcl::PointCloud<pcl::PointXYZ>);
  414. pcl::CropBox<pcl::PointXYZ> box_filter;
  415. box_filter.setNegative(false);
  416. box_filter.setInputCloud(in_cloud);
  417. double radius = 5;
  418. double rx = 0.8;
  419. double ry = 1.0;
  420. double rz = 0.8;
  421. double cx, cy, cz;
  422. double dz;
  423. for (size_t i = 0; i < seed_cloud->points.size(); ++i) {
  424. cx = seed_cloud->points.at(i).x;
  425. cy = seed_cloud->points.at(i).y;
  426. cz = seed_cloud->points.at(i).z;
  427. box_filter.setMin(Eigen::Vector4f(cx - rx*radius, cy - ry*radius, cz - rz*radius, 1));
  428. box_filter.setMax(Eigen::Vector4f(cx + rx*radius, cy + ry*radius, cz + rz*radius, 1));
  429. box_filter.filter(*cloud_inbox);
  430. //dz
  431. Eigen::Vector4f min_point;
  432. Eigen::Vector4f max_point;
  433. pcl::getMinMax3D(*cloud_inbox, min_point, max_point);
  434. dz = max_point(2) - min_point(2);
  435. //project to xy-plane
  436. pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_inbox_xy(new pcl::PointCloud<pcl::PointXYZ>);
  437. pcl::copyPointCloud(*cloud_inbox, *cloud_inbox_xy);
  438. for (auto&pt : *cloud_inbox_xy) { pt.z = 0.0; }
  439. //pca
  440. double dx_obb;
  441. double dy_obb;
  442. double angle_obb;
  443. cal_obb_2d(cloud_inbox_xy, 0, dx_obb, dy_obb, angle_obb);
  444. try {
  445. double dx_grid = dx_obb / dist_mean;
  446. double dy_grid = dy_obb / dist_mean;
  447. double dz_grid = dz / dist_mean;
  448. double fill_ratio = cloud_inbox->points.size() / dx_grid / dy_grid / dz_grid;
  449. double y_ratio = dy_obb / dx_obb/2;
  450. y_ratio = pow(y_ratio, 2);
  451. double a_ratio = cos((angle_obb - 90)*PI / 180.0);
  452. a_ratio = pow(a_ratio, 3);
  453. double mass_index = fill_ratio*y_ratio*a_ratio;
  454. mass_indices.push_back(mass_index);
  455. if (m_pLogger) {
  456. stringstream buff;
  457. buff << i << "/" << seed_cloud->points.size() << " dx=" << dx_obb << ", dy=" << dy_obb << ", fill_ratio=" << fill_ratio
  458. << ", y_ratio=" << y_ratio << ", a_ratio=" << a_ratio << ", mass_index=" << mass_index;
  459. m_pLogger->INFO(buff.str());
  460. }
  461. }
  462. catch (...) {
  463. mass_indices.push_back(0);
  464. }
  465. }
  466. // sort by mass_indices
  467. std::vector<size_t> sort_idx = sort_indexes_e(mass_indices, false);
  468. for (auto&v : sort_idx) { candidate_idx.push_back((int)v); }
  469. }
  470. void CRootStockGrabPoint::euclidean_clustering_ttsas(
  471. pcl::PointCloud<pcl::PointXYZ>::Ptr in_cloud,
  472. double d1, double d2,
  473. std::vector<pcl::PointXYZ>&cluster_center,
  474. std::vector<std::vector<int>> &clustr_member
  475. )
  476. {
  477. if (m_pLogger) {
  478. stringstream buff;
  479. buff << "euclidean_clustering_ttsas() begin...";
  480. m_pLogger->INFO(buff.str());
  481. }
  482. std::vector<int> cluster_weight;
  483. std::vector<int> data_stat;
  484. for (size_t i = 0; i < in_cloud->points.size(); ++i) { data_stat.push_back(0); }
  485. size_t data_len = in_cloud->points.size();
  486. int exists_change = 0;
  487. int prev_change = 0;
  488. int cur_change = 0;
  489. int m = 0;
  490. while (std::find(data_stat.begin(), data_stat.end(), 0) != data_stat.end()) {
  491. bool new_while_first = true;
  492. for (size_t i = 0; i < data_len; ++i) {
  493. if (data_stat.at(i) == 0 && new_while_first && exists_change == 0) {
  494. new_while_first = false;
  495. std::vector<int> idx;
  496. idx.push_back(i);
  497. clustr_member.push_back(idx);
  498. pcl::PointXYZ center;
  499. center.x = in_cloud->points.at(i).x;
  500. center.y = in_cloud->points.at(i).y;
  501. center.z = in_cloud->points.at(i).z;
  502. cluster_center.push_back(center);
  503. data_stat.at(i) = 1;
  504. cur_change += 1;
  505. cluster_weight.push_back(1);
  506. m += 1;
  507. }
  508. else if (data_stat[i] == 0) {
  509. std::vector<float> distances;
  510. for (size_t j = 0; j < clustr_member.size(); ++j) {
  511. std::vector<float> distances_sub;
  512. for (size_t jj = 0; jj < clustr_member.at(j).size(); ++jj) {
  513. size_t ele_idx = clustr_member.at(j).at(jj);
  514. double d = sqrt(
  515. (in_cloud->points.at(i).x - in_cloud->points.at(ele_idx).x) * (in_cloud->points.at(i).x - in_cloud->points.at(ele_idx).x) +
  516. (in_cloud->points.at(i).y - in_cloud->points.at(ele_idx).y) * (in_cloud->points.at(i).y - in_cloud->points.at(ele_idx).y) +
  517. (in_cloud->points.at(i).z - in_cloud->points.at(ele_idx).z) * (in_cloud->points.at(i).z - in_cloud->points.at(ele_idx).z));
  518. distances_sub.push_back(d);
  519. }
  520. double min_dist = *std::min_element(distances_sub.begin(), distances_sub.end());
  521. distances.push_back(min_dist);
  522. }
  523. int min_idx = std::min_element(distances.begin(), distances.end()) - distances.begin();
  524. if (distances.at(min_idx) < d1) {
  525. data_stat.at(i) = 1;
  526. double w = cluster_weight.at(min_idx);
  527. cluster_weight.at(min_idx) += 1;
  528. clustr_member.at(min_idx).push_back(i);
  529. cluster_center.at(min_idx).x = (cluster_center.at(min_idx).x * w + in_cloud->points.at(i).x) / (w + 1);
  530. cluster_center.at(min_idx).y = (cluster_center.at(min_idx).y * w + in_cloud->points.at(i).y) / (w + 1);
  531. cluster_center.at(min_idx).z = (cluster_center.at(min_idx).z * w + in_cloud->points.at(i).z) / (w + 1);
  532. cur_change += 1;
  533. }
  534. else if (distances.at(min_idx) > d2) {
  535. std::vector<int> idx;
  536. idx.push_back(i);
  537. clustr_member.push_back(idx);
  538. pcl::PointXYZ center;
  539. center.x = in_cloud->points.at(i).x;
  540. center.y = in_cloud->points.at(i).y;
  541. center.z = in_cloud->points.at(i).z;
  542. cluster_center.push_back(center);
  543. data_stat.at(i) = 1;
  544. cur_change += 1;
  545. cluster_weight.push_back(1);
  546. m += 1;
  547. }
  548. }
  549. else if (data_stat.at(i)== 1) {
  550. cur_change += 1;
  551. }
  552. }
  553. exists_change = fabs(cur_change - prev_change);
  554. prev_change = cur_change;
  555. cur_change = 0;
  556. }
  557. if (m_pLogger) {
  558. stringstream buff;
  559. buff << "euclidean_clustering_ttsas() end";
  560. m_pLogger->INFO(buff.str());
  561. }
  562. }
  563. void CRootStockGrabPoint::cal_obb_2d(
  564. pcl::PointCloud<pcl::PointXYZ>::Ptr in_cloud,
  565. int axis, //0--xy, 1--zy
  566. double &dx_obb,
  567. double &dy_obb,
  568. double &angle_obb
  569. )
  570. {
  571. // asign value
  572. Eigen::MatrixXd pts(in_cloud->points.size(), 2);
  573. for (size_t i = 0; i < in_cloud->points.size(); ++i) {
  574. if (axis == 0) {
  575. pts(i, 0) = in_cloud->points.at(i).x;
  576. }
  577. else {
  578. pts(i, 0) = in_cloud->points.at(i).z;
  579. }
  580. pts(i, 1) = in_cloud->points.at(i).y;
  581. }
  582. // centerlize
  583. Eigen::MatrixXd mu = pts.colwise().mean();
  584. Eigen::RowVectorXd mu_row = mu;
  585. pts.rowwise() -= mu_row;
  586. //calculate covariance
  587. Eigen::MatrixXd C = pts.adjoint()*pts;
  588. C = C.array() / (pts.cols() - 1);
  589. //compute eig
  590. Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> eig(C);
  591. Eigen::MatrixXd d = eig.eigenvalues();
  592. Eigen::MatrixXd v = eig.eigenvectors();
  593. //projection
  594. Eigen::MatrixXd p = pts * v;
  595. Eigen::VectorXd minv = p.colwise().minCoeff();
  596. Eigen::VectorXd maxv = p.colwise().maxCoeff();
  597. Eigen::VectorXd range = maxv - minv;
  598. dy_obb = range(1);
  599. dx_obb = range(0);
  600. angle_obb = std::atan2(v(1, 1), v(0, 1));
  601. if (angle_obb < 0) { angle_obb = PI + angle_obb; }
  602. angle_obb = angle_obb * 180 / PI;
  603. }
  604. //void CRootStockGrabPoint::get_optimal_seed(
  605. // pcl::PointCloud<pcl::PointXYZ>::Ptr in_cloud,
  606. // pcl::PointXYZ&pt,
  607. // int &pt_idx)
  608. //{
  609. // double d1 = m_cparam.rs_grab_stem_diameter;
  610. // double d2 = m_cparam.rs_grab_stem_diameter * 4.0;
  611. // if (m_dtype != 0) {
  612. // d1 = m_cparam.sc_grab_stem_diameter;
  613. // d2 = m_cparam.sc_grab_stem_diameter * 4.0;
  614. // }
  615. // std::vector<pcl::PointXYZ>cluster_center;
  616. // std::vector<std::vector<int>> cluster_member;
  617. // euclidean_clustering_ttsas(in_cloud, d1, d2, cluster_center, cluster_member);
  618. // float min_y_dist = 1.0e6;
  619. // int opt_idx = -1;
  620. // int member_size = 5;
  621. // float y_opt = m_cparam.rs_grab_y_opt;
  622. // if (m_dtype != 0) {
  623. // y_opt = m_cparam.sc_grab_y_opt;
  624. // }
  625. // for (int i = 0; i < cluster_member.size(); ++i) {
  626. // if (cluster_member.at(i).size() < member_size) {
  627. // continue;
  628. // }
  629. // if (fabs(cluster_center.at(i).y -y_opt) < min_y_dist) {
  630. // min_y_dist = fabs(cluster_center.at(i).y - y_opt);
  631. // opt_idx = i;
  632. // }
  633. // }
  634. // if (opt_idx < 0) {
  635. // if (m_pLogger) {
  636. // stringstream buff;
  637. // buff << "get_optimal_seed failed, get invalid optimal cluster id";
  638. // m_pLogger->ERRORINFO(buff.str());
  639. // }
  640. // return;
  641. // }
  642. // //find nearest pt
  643. // float nearest_dist = 1.0e6;
  644. // int nearest_idx = -1;
  645. // for (int i = 0; i < cluster_member.at(opt_idx).size(); ++i) {
  646. // int idx = cluster_member.at(opt_idx).at(i);
  647. // float dist = fabs(in_cloud->points.at(idx).x - cluster_center.at(opt_idx).x) +
  648. // fabs(in_cloud->points.at(idx).y - cluster_center.at(opt_idx).y) +
  649. // fabs(in_cloud->points.at(idx).z - cluster_center.at(opt_idx).z);
  650. // if (dist < nearest_dist) {
  651. // nearest_dist = dist;
  652. // nearest_idx = idx;
  653. // }
  654. // }
  655. // pt.x = in_cloud->points.at(nearest_idx).x;
  656. // pt.y = in_cloud->points.at(nearest_idx).y;
  657. // pt.z = in_cloud->points.at(nearest_idx).z;
  658. // pt_idx = nearest_idx;
  659. //}
  660. void CRootStockGrabPoint::get_optimal_seed(
  661. pcl::PointCloud<pcl::PointXYZ>::Ptr in_cloud,
  662. pcl::PointXYZ&pt,
  663. int &pt_idx)
  664. {
  665. /*double d1 = m_cparam.rs_grab_stem_diameter;
  666. double d2 = m_cparam.rs_grab_stem_diameter * 4.0;
  667. if (m_dtype != 0) {
  668. d1 = m_cparam.sc_grab_stem_diameter;
  669. d2 = m_cparam.sc_grab_stem_diameter * 4.0;
  670. }
  671. std::vector<pcl::PointXYZ>cluster_center;
  672. std::vector<std::vector<int>> cluster_member;
  673. euclidean_clustering_ttsas(in_cloud, d1, d2, cluster_center, cluster_member);*/
  674. float min_y_dist = 1.0e6;
  675. int opt_idx = -1;
  676. int member_size = 5;
  677. float y_opt = m_cparam.rs_grab_y_opt;
  678. if (m_dtype == 0) {
  679. y_opt = m_cparam.sc_grab_y_opt;
  680. }
  681. for (int i = 0; i < in_cloud->points.size(); ++i) {
  682. /*if (cluster_member.at(i).size() < member_size) {
  683. continue;
  684. }*/
  685. if (fabs(in_cloud->points.at(i).y - y_opt) < min_y_dist) {
  686. min_y_dist = fabs(in_cloud->points.at(i).y - y_opt);
  687. opt_idx = i;
  688. }
  689. }
  690. if (opt_idx < 0) {
  691. if (m_pLogger) {
  692. stringstream buff;
  693. buff << "get_optimal_seed failed, get invalid optimal cluster id";
  694. m_pLogger->ERRORINFO(buff.str());
  695. }
  696. return;
  697. }
  698. //find nearest pt
  699. /*float nearest_dist = 1.0e6;
  700. int nearest_idx = -1;
  701. for (int i = 0; i < cluster_member.at(opt_idx).size(); ++i) {
  702. int idx = cluster_member.at(opt_idx).at(i);
  703. float dist = fabs(in_cloud->points.at(idx).x - cluster_center.at(opt_idx).x) +
  704. fabs(in_cloud->points.at(idx).y - cluster_center.at(opt_idx).y) +
  705. fabs(in_cloud->points.at(idx).z - cluster_center.at(opt_idx).z);
  706. if (dist < nearest_dist) {
  707. nearest_dist = dist;
  708. nearest_idx = idx;
  709. }
  710. }*/
  711. pt.x = in_cloud->points.at(opt_idx).x;
  712. pt.y = in_cloud->points.at(opt_idx).y;
  713. pt.z = in_cloud->points.at(opt_idx).z;
  714. pt_idx = opt_idx;
  715. }
  716. void CRootStockGrabPoint::viewer_cloud(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, std::string&winname)
  717. {
  718. pcl::visualization::CloudViewer viewer(winname);
  719. //viewer.runOnVisualizationThreadOnce(viewerOneOff);
  720. viewer.showCloud(cloud);
  721. while (!viewer.wasStopped()) {
  722. boost::this_thread::sleep(boost::posix_time::microseconds(100000));
  723. }
  724. }
  725. void CRootStockGrabPoint::viewer_cloud(pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud, std::string&winname)
  726. {
  727. pcl::visualization::CloudViewer viewer(winname);
  728. //viewer.runOnVisualizationThreadOnce(viewerOneOff);
  729. viewer.showCloud(cloud);
  730. while (!viewer.wasStopped()) {
  731. boost::this_thread::sleep(boost::posix_time::microseconds(100000));
  732. }
  733. }
  734. void CRootStockGrabPoint::viewer_cloud_debug(pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud, pcl::PointXYZ &p, std::string&winname)
  735. {
  736. pcl::visualization::PCLVisualizer viewer(winname);
  737. //viewer.runOnVisualizationThreadOnce(viewerOneOff);
  738. viewer.addPointCloud(cloud);
  739. viewer.addCoordinateSystem();
  740. pcl::PointXYZ p0, x1, y1,p00,x0,y0;
  741. p0.x = p.x;
  742. p0.y = p.y;
  743. p0.z = p.z;
  744. x1.x = p.x + 400.0;
  745. x1.y = p.y;
  746. x1.z = p.z;
  747. y1.x = p.x;
  748. y1.y = p.y + 200.0;
  749. y1.z = p.z;
  750. p00.x = 0.0;
  751. p00.y = 0.0;
  752. p00.z = p.z;
  753. x0.x = 600.0;
  754. x0.y = 0;
  755. x0.z = p.z;
  756. y0.x = 0.0;
  757. y0.y = 300.0;
  758. y0.z = p.z;
  759. viewer.addLine(p0, x1, 255, 0, 0, "x");
  760. viewer.addLine(p0, y1, 0, 255, 0, "y");
  761. viewer.addLine(p00, x0, 255, 0, 0, "x0");
  762. viewer.addLine(p00, y0, 0, 255, 0, "y0");
  763. while (!viewer.wasStopped()) {
  764. viewer.spinOnce(100);
  765. boost::this_thread::sleep(boost::posix_time::microseconds(100000));
  766. }
  767. }
  768. };