grab_point_rs.cpp 59 KB

1234567891011121314151617181920212223242526272829303132333435363738394041424344454647484950515253545556575859606162636465666768697071727374757677787980818283848586878889909192939495969798991001011021031041051061071081091101111121131141151161171181191201211221231241251261271281291301311321331341351361371381391401411421431441451461471481491501511521531541551561571581591601611621631641651661671681691701711721731741751761771781791801811821831841851861871881891901911921931941951961971981992002012022032042052062072082092102112122132142152162172182192202212222232242252262272282292302312322332342352362372382392402412422432442452462472482492502512522532542552562572582592602612622632642652662672682692702712722732742752762772782792802812822832842852862872882892902912922932942952962972982993003013023033043053063073083093103113123133143153163173183193203213223233243253263273283293303313323333343353363373383393403413423433443453463473483493503513523533543553563573583593603613623633643653663673683693703713723733743753763773783793803813823833843853863873883893903913923933943953963973983994004014024034044054064074084094104114124134144154164174184194204214224234244254264274284294304314324334344354364374384394404414424434444454464474484494504514524534544554564574584594604614624634644654664674684694704714724734744754764774784794804814824834844854864874884894904914924934944954964974984995005015025035045055065075085095105115125135145155165175185195205215225235245255265275285295305315325335345355365375385395405415425435445455465475485495505515525535545555565575585595605615625635645655665675685695705715725735745755765775785795805815825835845855865875885895905915925935945955965975985996006016026036046056066076086096106116126136146156166176186196206216226236246256266276286296306316326336346356366376386396406416426436446456466476486496506516526536546556566576586596606616626636646656666676686696706716726736746756766776786796806816826836846856866876886896906916926936946956966976986997007017027037047057067077087097107117127137147157167177187197207217227237247257267277287297307317327337347357367377387397407417427437447457467477487497507517527537547557567577587597607617627637647657667677687697707717727737747757767777787797807817827837847857867877887897907917927937947957967977987998008018028038048058068078088098108118128138148158168178188198208218228238248258268278288298308318328338348358368378388398408418428438448458468478488498508518528538548558568578588598608618628638648658668678688698708718728738748758768778788798808818828838848858868878888898908918928938948958968978988999009019029039049059069079089099109119129139149159169179189199209219229239249259269279289299309319329339349359369379389399409419429439449459469479489499509519529539549559569579589599609619629639649659669679689699709719729739749759769779789799809819829839849859869879889899909919929939949959969979989991000100110021003100410051006100710081009101010111012101310141015101610171018101910201021102210231024102510261027102810291030103110321033103410351036103710381039104010411042104310441045104610471048104910501051105210531054105510561057105810591060106110621063106410651066106710681069107010711072107310741075107610771078107910801081108210831084108510861087108810891090109110921093109410951096109710981099110011011102110311041105110611071108110911101111111211131114111511161117111811191120112111221123112411251126112711281129113011311132113311341135113611371138113911401141114211431144114511461147114811491150115111521153115411551156115711581159116011611162116311641165116611671168116911701171117211731174117511761177117811791180118111821183118411851186118711881189119011911192119311941195119611971198119912001201120212031204120512061207120812091210121112121213121412151216121712181219122012211222122312241225122612271228122912301231123212331234123512361237123812391240124112421243124412451246124712481249125012511252125312541255125612571258125912601261126212631264126512661267126812691270127112721273127412751276127712781279128012811282128312841285128612871288128912901291129212931294129512961297129812991300130113021303130413051306130713081309131013111312131313141315131613171318131913201321132213231324132513261327132813291330133113321333133413351336133713381339134013411342134313441345134613471348134913501351135213531354135513561357135813591360136113621363136413651366136713681369137013711372137313741375137613771378137913801381138213831384138513861387138813891390139113921393139413951396139713981399140014011402140314041405140614071408140914101411141214131414141514161417141814191420142114221423142414251426142714281429143014311432143314341435143614371438143914401441144214431444144514461447144814491450145114521453145414551456145714581459146014611462146314641465146614671468146914701471147214731474147514761477147814791480148114821483148414851486148714881489149014911492149314941495149614971498149915001501150215031504150515061507150815091510151115121513151415151516151715181519152015211522152315241525152615271528152915301531153215331534153515361537153815391540154115421543154415451546154715481549155015511552155315541555155615571558155915601561156215631564156515661567156815691570157115721573157415751576157715781579158015811582158315841585158615871588158915901591159215931594159515961597159815991600160116021603160416051606160716081609161016111612161316141615161616171618161916201621162216231624162516261627162816291630163116321633163416351636163716381639164016411642164316441645164616471648164916501651165216531654165516561657165816591660166116621663166416651666166716681669167016711672167316741675167616771678167916801681168216831684168516861687168816891690169116921693169416951696169716981699170017011702170317041705170617071708170917101711171217131714171517161717171817191720172117221723172417251726172717281729173017311732173317341735173617371738173917401741174217431744174517461747174817491750175117521753175417551756175717581759176017611762176317641765176617671768176917701771177217731774177517761777177817791780178117821783178417851786178717881789179017911792179317941795179617971798
  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\statistical_outlier_removal.h>
  15. #include <pcl\filters\voxel_grid.h>
  16. #include <pcl\common\common.h>
  17. #include <pcl\features\moment_of_inertia_estimation.h>
  18. #include <pcl\segmentation\sac_segmentation.h>
  19. #include <math.h>
  20. #include "grab_point_rs.h"
  21. #include "utils.h"
  22. #define PI std::acos(-1)
  23. namespace graft_cv {
  24. CRootStockGrabPoint::CRootStockGrabPoint(ConfigParam&cp, CGcvLogger*pLog/*=0*/)
  25. :m_cparam(cp),
  26. m_pLogger(pLog),
  27. m_dtype(0),
  28. m_pcdId(""),
  29. m_ppImgSaver(0),
  30. m_1st_row_zmean_rs(-1.0),
  31. m_1st_row_zmean_sc(-1.0)
  32. {
  33. }
  34. CRootStockGrabPoint::~CRootStockGrabPoint() {}
  35. float* CRootStockGrabPoint::get_raw_point_cloud(int &data_size)
  36. {
  37. data_size = m_raw_cloud->width * m_raw_cloud->height;
  38. if (data_size == 0) {
  39. return 0;
  40. }
  41. else {
  42. pcl::PointXYZ* pp = m_raw_cloud->points.data();
  43. return (float*)pp->data;
  44. }
  45. }
  46. int CRootStockGrabPoint::load_data(
  47. float*pPoint,
  48. int pixel_size,
  49. int pt_size,
  50. const char* fn/* = 0*/)
  51. {
  52. int rst = 0;
  53. //1 get point cloud data
  54. if (pPoint != 0 && pt_size>0) {
  55. //read point
  56. m_raw_cloud.reset(new pcl::PointCloud<pcl::PointXYZ>);
  57. int step = pixel_size;
  58. for (int i = 0; i < pt_size; ++i) {
  59. pcl::PointXYZ pt = { pPoint[i*step], pPoint[i*step + 1] , pPoint[i*step + 2] };
  60. m_raw_cloud->push_back(pt);
  61. }
  62. rst = m_raw_cloud->width * m_raw_cloud->height;
  63. if (m_pLogger) {
  64. stringstream buff;
  65. buff << "load raw point cloud " << rst << " data points";
  66. m_pLogger->INFO(buff.str());
  67. }
  68. }
  69. else if (fn != 0) {
  70. //read file
  71. rst = this->read_ply_file(fn);
  72. }
  73. else {//error
  74. if (m_pLogger) {
  75. m_pLogger->ERRORINFO("no valid input");
  76. return (-1);
  77. }
  78. }
  79. if (m_dtype == 0) {
  80. m_pcdId = getImgId(img_type::sola_sc_pcd);
  81. }
  82. else {
  83. m_pcdId = getImgId(img_type::sola_rs_pcd);
  84. }
  85. if (m_ppImgSaver && *m_ppImgSaver) {
  86. (*m_ppImgSaver)->saveBinPly(m_raw_cloud, m_pcdId);
  87. }
  88. if (m_cparam.image_show) {
  89. viewer_cloud(m_raw_cloud, std::string("raw point cloud"));
  90. }
  91. return rst;
  92. }
  93. int CRootStockGrabPoint::grab_point_detect(
  94. int dtype,
  95. PositionInfo& posinfo
  96. )
  97. {
  98. // dtype == 0, scion
  99. // dtype != 0, rootstock
  100. // 输入,穗苗--0, 砧木--1
  101. if (m_raw_cloud->width * m_raw_cloud->height < 1) {
  102. if (m_pLogger) {
  103. stringstream buff;
  104. buff << "m_raw_cloud point cloud " << m_raw_cloud->width * m_raw_cloud->height << " data points";
  105. m_pLogger->ERRORINFO(buff.str());
  106. }
  107. return 1;
  108. }
  109. ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  110. //1 crop filter
  111. if (m_pLogger) {
  112. m_pLogger->INFO("begin crop");
  113. }
  114. pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_inbox(new pcl::PointCloud<pcl::PointXYZ>);
  115. pcl::CropBox<pcl::PointXYZ> box_filter;
  116. m_dtype = dtype;
  117. if (dtype != 0) {//rootstock
  118. box_filter.setMin(Eigen::Vector4f(m_cparam.rs_grab_xmin, m_cparam.rs_grab_ymin, m_cparam.rs_grab_zmin, 1));
  119. box_filter.setMax(Eigen::Vector4f(m_cparam.rs_grab_xmax, m_cparam.rs_grab_ymax, m_cparam.rs_grab_zmax, 1));
  120. }
  121. else {//scion
  122. box_filter.setMin(Eigen::Vector4f(m_cparam.sc_grab_xmin, m_cparam.sc_grab_ymin, m_cparam.sc_grab_zmin, 1));
  123. box_filter.setMax(Eigen::Vector4f(m_cparam.sc_grab_xmax, m_cparam.sc_grab_ymax, m_cparam.sc_grab_zmax, 1));
  124. }
  125. box_filter.setNegative(false);
  126. box_filter.setInputCloud(m_raw_cloud);
  127. box_filter.filter(*cloud_inbox);
  128. if (m_pLogger) {
  129. stringstream buff;
  130. buff << "CropBox croped point cloud " << cloud_inbox->width * cloud_inbox->height << " data points";
  131. m_pLogger->INFO(buff.str());
  132. }
  133. if (cloud_inbox->width * cloud_inbox->height < 1) {
  134. return 1;
  135. }
  136. if (m_cparam.image_show) {
  137. viewer_cloud(cloud_inbox, std::string("cloud_inbox"));
  138. }
  139. if (m_pLogger) {
  140. m_pLogger->INFO("end crop");
  141. }
  142. ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  143. //2 filtler with radius remove
  144. if (m_pLogger) {
  145. m_pLogger->INFO("begin ror");
  146. }
  147. int nb_points = 20;
  148. double stem_radius = m_cparam.rs_grab_stem_diameter / 2.0;
  149. if(dtype == 0){
  150. stem_radius = m_cparam.sc_grab_stem_diameter / 2.0;
  151. }
  152. double remove_radius = stem_radius * 2.0;
  153. pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_ror(new pcl::PointCloud<pcl::PointXYZ>);
  154. pcl::RadiusOutlierRemoval<pcl::PointXYZ> ror(false);
  155. ror.setInputCloud(cloud_inbox);
  156. ror.setRadiusSearch(remove_radius);
  157. ror.setMinNeighborsInRadius(nb_points);
  158. ror.filter(*cloud_ror);
  159. if (m_pLogger) {
  160. stringstream buff;
  161. buff << "RadiusOutlierRemoval filtered point cloud " << cloud_ror->width * cloud_ror->height << " data points. param stem_radius="<<
  162. stem_radius<<", nb_points="<< nb_points;
  163. m_pLogger->INFO(buff.str());
  164. }
  165. if (cloud_ror->width * cloud_ror->height < 1) {
  166. return 1;
  167. }
  168. /*if (m_cparam.image_show) {
  169. viewer_cloud(cloud_ror, std::string("cloud_ror"));
  170. }*/
  171. if (m_pLogger) {
  172. m_pLogger->INFO("end ror");
  173. }
  174. ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  175. //3 voxel grid down sampling
  176. if (m_pLogger) {
  177. m_pLogger->INFO("begin down");
  178. }
  179. pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_dowm_sampled(new pcl::PointCloud<pcl::PointXYZ>);
  180. pcl::VoxelGrid<pcl::PointXYZ> outrem;
  181. outrem.setInputCloud(cloud_ror);
  182. outrem.setLeafSize(stem_radius, stem_radius, stem_radius);
  183. outrem.filter(*cloud_dowm_sampled);
  184. if (m_pLogger) {
  185. stringstream buff;
  186. buff << "VoxelGrid dowm_sampled point cloud " << cloud_dowm_sampled->width * cloud_dowm_sampled->height << " data points (if < 50, return)";
  187. m_pLogger->INFO(buff.str());
  188. }
  189. if (cloud_dowm_sampled->width * cloud_dowm_sampled->height < 50) {
  190. return 1;
  191. }
  192. /*if (m_cparam.image_show) {
  193. viewer_cloud(cloud_dowm_sampled, std::string("cloud_dowm_sampled"));
  194. }*/
  195. if (m_pLogger) {
  196. m_pLogger->INFO("end down");
  197. }
  198. ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  199. //4 seedling position,找到第一个位置上的植株
  200. std::vector<int> first_seedling_cloud_idx; //存储找到的第一个位置上植株茎上直线点的index
  201. pcl::PointXYZ xz_center; //存储找到的第一个位置上植株根部坐标
  202. if (m_pLogger) {
  203. m_pLogger->INFO("begin find seedling position");
  204. }
  205. find_seedling_position_key(cloud_dowm_sampled, first_seedling_cloud_idx, xz_center);
  206. if (m_pLogger) {
  207. stringstream buff;
  208. buff << "after find_seedling_position(), foud first seedling seeds points size " << first_seedling_cloud_idx.size();
  209. m_pLogger->INFO(buff.str());
  210. }
  211. if (m_pLogger) {
  212. m_pLogger->INFO("end find seedling position");
  213. }
  214. ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  215. //5 nearest seedling grab point selection,找到最接近指定高度的最优抓取点坐标
  216. pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_seedling_seed(new pcl::PointCloud<pcl::PointXYZ>);
  217. pcl::copyPointCloud(*cloud_dowm_sampled, first_seedling_cloud_idx, *cloud_seedling_seed);
  218. //改进思路:将茎直线上cloud_seedling_seed的点,提取周围邻域xz的宽度,在相同邻域cloud_dowm_sampled点云内提取xz的宽度
  219. //对比两个宽度,变化很大的,说明周边有异物(叶柄或叶子),不适合作为抓取位置;否则就是抓取位置
  220. //在众多抓取位置上,选择离指定高度最近的点作为抓取位置
  221. //
  222. pcl::PointXYZ selected_pt;
  223. int selected_pt_idx;
  224. std::vector<int> optimal_seeds_idx;
  225. get_optimal_seed_compare(cloud_dowm_sampled, cloud_seedling_seed, selected_pt, selected_pt_idx, optimal_seeds_idx);
  226. if (selected_pt_idx < 0) {
  227. if (m_pLogger) {
  228. m_pLogger->ERRORINFO("Not found valid grab point");
  229. }
  230. return 1;
  231. }
  232. if (m_pLogger) {
  233. m_pLogger->INFO("end get_optimal_seed");
  234. }
  235. if (m_dtype == 0) {
  236. posinfo.sc_grab_x = selected_pt.x;
  237. posinfo.sc_grab_y = selected_pt.y;
  238. posinfo.sc_grab_z = selected_pt.z;
  239. }
  240. else {
  241. posinfo.rs_grab_x = selected_pt.x;
  242. posinfo.rs_grab_y = selected_pt.y;
  243. posinfo.rs_grab_z = selected_pt.z;
  244. }
  245. ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  246. //6 debug 显示结果
  247. if (m_cparam.image_show) {
  248. pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_cand_demo(new pcl::PointCloud<pcl::PointXYZRGB>);
  249. pcl::copyPointCloud(*cloud_dowm_sampled, *cloud_cand_demo);
  250. for (auto& pt : *cloud_cand_demo) {
  251. pt.r = 255;
  252. pt.g = 255;
  253. pt.b = 255;
  254. }
  255. int cnt = 0;
  256. for (auto& idx : optimal_seeds_idx) {
  257. int p_idx = first_seedling_cloud_idx.at(idx);
  258. /*if (p_idx == optimal_seeds_idx[selected_pt_idx]) {
  259. cloud_cand_demo->points[p_idx].r = 0;
  260. cloud_cand_demo->points[p_idx].g = 255;
  261. cloud_cand_demo->points[p_idx].b = 0;
  262. }
  263. else {*/
  264. cloud_cand_demo->points.at(p_idx).r = 255;
  265. cloud_cand_demo->points.at(p_idx).g = 0;
  266. cloud_cand_demo->points.at(p_idx).b = 0;
  267. /*} */
  268. if (cnt > optimal_seeds_idx.size()) { break; }
  269. cnt++;
  270. }
  271. pcl::PointXYZRGB pt_grab = {0,255,0};
  272. pt_grab.x = selected_pt.x;
  273. pt_grab.y = selected_pt.y;
  274. pt_grab.z = selected_pt.z;
  275. pcl::PointXYZRGB pt_grab_ = { 0,255,120 };
  276. pt_grab_.x = selected_pt.x;
  277. pt_grab_.y = selected_pt.y+0.2;
  278. pt_grab_.z = selected_pt.z;
  279. cloud_cand_demo->push_back(pt_grab);
  280. //viewer_cloud(cloud_cand_demo, std::string("cloud_cand_demo"));
  281. viewer_cloud_debug(cloud_cand_demo, selected_pt, xz_center, std::string("cloud_cand_demo"));
  282. }
  283. return 0;
  284. }
  285. int CRootStockGrabPoint::read_ply_file(const char* fn)
  286. {
  287. m_raw_cloud.reset( new pcl::PointCloud<pcl::PointXYZ>);
  288. if (pcl::io::loadPLYFile<pcl::PointXYZ>(fn, *m_raw_cloud) == -1) {
  289. if (m_pLogger) {
  290. m_pLogger->ERRORINFO("could not load file: "+std::string(fn));
  291. }
  292. return (-1);
  293. }
  294. if (m_pLogger) {
  295. stringstream buff;
  296. buff << "load raw point cloud " << m_raw_cloud->width * m_raw_cloud->height << " data points";
  297. m_pLogger->INFO(buff.str());
  298. }
  299. return m_raw_cloud->width * m_raw_cloud->height;
  300. }
  301. double CRootStockGrabPoint::compute_nearest_neighbor_distance(pcl::PointCloud<pcl::PointXYZ>::Ptr pcd)
  302. {
  303. pcl::KdTreeFLANN<pcl::PointXYZ> tree;
  304. tree.setInputCloud(pcd);
  305. int k = 2;
  306. double res = 0.0;
  307. int n_points = 0;
  308. for (size_t i = 0; i < pcd->size(); ++i) {
  309. std::vector<int> idx(k);
  310. std::vector<float> sqr_distances(k);
  311. if (tree.nearestKSearch(i, k, idx, sqr_distances) == k) {
  312. for (int id = 1; id < k; ++id) {
  313. res += sqrt(sqr_distances.at(id));
  314. ++n_points;
  315. }
  316. }
  317. }
  318. if (n_points > 0) {
  319. res /= (double)n_points;
  320. }
  321. return res;
  322. }
  323. //void CRootStockGrabPoint::find_tray_top_edge(
  324. //pcl::PointCloud<pcl::PointXYZ>::Ptr in_cloud,
  325. //float & tray_top_edge_y
  326. //)
  327. //{
  328. // tray_top_edge_y = -1000.0;
  329. // //以毫米为单位构建vector
  330. // pcl::PointXYZ min_point_aabb;
  331. // pcl::PointXYZ max_point_aabb;
  332. // pcl::MomentOfInertiaEstimation<pcl::PointXYZ> feature_extractor;
  333. // feature_extractor.setInputCloud(in_cloud);
  334. // feature_extractor.compute();
  335. // feature_extractor.getAABB(min_point_aabb, max_point_aabb);
  336. // float miny = min_point_aabb.y;
  337. // float maxy = max_point_aabb.y;
  338. // if(maxy - miny <5.0){
  339. // tray_top_edge_y = maxy;
  340. // return;
  341. // }
  342. // std::vector<int> y_cnt_hist(int(maxy - miny), 0);
  343. // for(auto& pt : in_cloud->points){
  344. // int idx = (int)(pt.y - miny);
  345. // if(idx<0){continue;}
  346. // if (idx >= y_cnt_hist.size()) { continue; }
  347. // y_cnt_hist.at(idx) += 1;
  348. // }
  349. // //从上半部分找到最大值,作为平面上沿
  350. // int idx_part = (int)(y_cnt_hist.size() / 2);
  351. // int idx_edge = std::max_element(y_cnt_hist.begin(), y_cnt_hist.begin() + idx_part) - y_cnt_hist.begin();
  352. // tray_top_edge_y = miny + (float)(idx_edge + 2.0);
  353. //}
  354. //void CRootStockGrabPoint::find_seedling_position_line_based(
  355. // pcl::PointCloud<pcl::PointXYZ>::Ptr in_cloud,
  356. // std::vector<int> &first_seedling_cloud_idx,
  357. // pcl::PointXYZ&xz_center
  358. //)
  359. //{
  360. // //找到穴盘上沿z,最优抓取的z,再在最优抓取的z基础上加上3作为有效范围
  361. // float tray_y = -1000.0;
  362. // float top_y = 0.0;
  363. // find_tray_top_edge(in_cloud, tray_y);
  364. // if (tray_y < -500.0) {
  365. // if (m_dtype == 0) {
  366. // //scion
  367. // tray_y = m_cparam.sc_grab_y_opt-2.0;
  368. // }
  369. // else {
  370. // tray_y = m_cparam.rs_grab_y_opt-2.0;
  371. // }
  372. // }
  373. // //up limit
  374. // if (m_dtype == 0) {
  375. // top_y = m_cparam.sc_grab_zmax;
  376. // if (top_y > m_cparam.sc_grab_y_opt + 3.0) {
  377. // top_y = m_cparam.sc_grab_y_opt + 3.0;
  378. // }
  379. // }
  380. // else {
  381. // top_y = m_cparam.rs_grab_zmax;
  382. // if (top_y > m_cparam.rs_grab_y_opt + 3.0) {
  383. // top_y = m_cparam.rs_grab_y_opt + 3.0;
  384. // }
  385. // }
  386. // //sub cloud
  387. // pcl::PointCloud<pcl::PointXYZ>::Ptr sub_cloud(new pcl::PointCloud<pcl::PointXYZ>);
  388. // pcl::CropBox<pcl::PointXYZ> box_filter;
  389. // if (m_dtype != 0) {//rootstock
  390. // box_filter.setMin(Eigen::Vector4f(m_cparam.rs_grab_xmin, tray_y, m_cparam.rs_grab_zmin, 1));
  391. // box_filter.setMax(Eigen::Vector4f(m_cparam.rs_grab_xmax, top_y, m_cparam.rs_grab_zmax, 1));
  392. // }
  393. // else {//scion
  394. // box_filter.setMin(Eigen::Vector4f(m_cparam.sc_grab_xmin, tray_y, m_cparam.sc_grab_zmin, 1));
  395. // box_filter.setMax(Eigen::Vector4f(m_cparam.sc_grab_xmax, top_y, m_cparam.sc_grab_zmax, 1));
  396. // }
  397. // box_filter.setNegative(false);
  398. // box_filter.setInputCloud(in_cloud);
  399. // box_filter.filter(*sub_cloud);
  400. // if (m_cparam.image_show) {
  401. // viewer_cloud(sub_cloud, std::string("sub inbox"));
  402. // }
  403. // //在sub_cloud中进行直线检测
  404. // segement_line(sub_cloud);
  405. //}
  406. //void CRootStockGrabPoint::segement_line(
  407. // pcl::PointCloud<pcl::PointXYZ>::Ptr in_cloud
  408. //)
  409. //{
  410. // pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
  411. // pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
  412. // pcl::SACSegmentation<pcl::PointXYZ> seg;
  413. // seg.setOptimizeCoefficients(true);
  414. // seg.setModelType(pcl::SACMODEL_LINE);
  415. // seg.setMethodType(pcl::SAC_RANSAC);
  416. // seg.setDistanceThreshold(0.5);
  417. // seg.setInputCloud(in_cloud);
  418. // seg.segment(*inliers, *coefficients);
  419. // if (m_cparam.image_show) {
  420. // pcl::PointCloud<pcl::PointXYZ>::Ptr line_cloud(new pcl::PointCloud<pcl::PointXYZ>);
  421. // pcl::copyPointCloud(*in_cloud, *inliers, *line_cloud);
  422. // viewer_cloud(line_cloud, std::string("cloud_line"));
  423. // }
  424. //}
  425. ////////////////////////////////////////////////////////////
  426. void CRootStockGrabPoint::gen_all_seedling_positions(
  427. pcl::PointXYZ&key_center, //输入,已知的苗的坐标
  428. std::vector<pcl::PointXYZ>&candidate_center //输出,有倾斜苗的坐标
  429. )
  430. {
  431. //生成所有的植株位置,并排序
  432. candidate_center.clear();
  433. //找到z最小的那一株,并找出和它一排的那些
  434. float step_harf = m_cparam.rs_grab_seedling_dist / 2.0;
  435. float xmin = m_cparam.rs_grab_xmin;
  436. float xmax = m_cparam.rs_grab_xmax;
  437. float zmin = m_cparam.rs_grab_zmin;
  438. float zmax = m_cparam.rs_grab_zmax;
  439. int col_from = -8;
  440. int col_to = 8;
  441. int col_step = 1;
  442. if (m_dtype == 0) {
  443. step_harf = m_cparam.sc_grab_seedling_dist / 2.0;
  444. xmin = m_cparam.sc_grab_xmin;
  445. xmax = m_cparam.sc_grab_xmax;
  446. zmin = m_cparam.sc_grab_zmin;
  447. zmax = m_cparam.sc_grab_zmax;
  448. }
  449. // 生成所有可能的植株位置中心点
  450. for (int row = -8; row <= 4; row += 1) {
  451. float cz = key_center.z + row * step_harf/2.0;
  452. if (cz < zmin || cz > zmax) { continue; }
  453. for (int col = 4*col_from; col <= 4*col_to; col += col_step) {
  454. float cx = key_center.x + col * step_harf/2;
  455. if (cx < xmin || cx > xmax) { continue; }
  456. //保存
  457. pcl::PointXYZ pc(cx, 0.0, cz);
  458. candidate_center.push_back(pc);
  459. }
  460. }
  461. }
  462. void CRootStockGrabPoint::nms_box(
  463. std::vector<pcl::PointXYZ>&clt_root_v,
  464. std::vector<float>&valid_box_cc_dist,
  465. float hole_step_radius,
  466. std::vector<pcl::PointXYZ>& target_toot)
  467. {
  468. target_toot.clear();
  469. pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
  470. for (auto&p : clt_root_v) { cloud->points.push_back(p); }
  471. pcl::KdTreeFLANN<pcl::PointXYZ> kdtree;
  472. kdtree.setInputCloud(cloud);
  473. std::vector<int>idx;
  474. std::vector<float>dist_sqr;
  475. std::vector<int> tar_idx;
  476. for (size_t i = 0; i < cloud->points.size(); ++i) {
  477. int k = kdtree.radiusSearch(cloud->points.at(i), hole_step_radius, idx, dist_sqr);
  478. std::vector<float> cc_tmp;
  479. for (auto& j : idx) { cc_tmp.push_back(valid_box_cc_dist.at(j)); }
  480. int min_id = std::min_element(cc_tmp.begin(), cc_tmp.end()) - cc_tmp.begin();
  481. if (min_id == 0) {
  482. tar_idx.push_back(i);
  483. }
  484. }
  485. for (auto& i : tar_idx) {
  486. target_toot.push_back(clt_root_v.at(i));
  487. }
  488. }
  489. //通过指定位置内,取部分点云分析是否存在真正的茎,真茎位置保存到target_filtered
  490. void CRootStockGrabPoint::line_filter(
  491. pcl::PointCloud<pcl::PointXYZ>::Ptr in_cloud, //输入点云
  492. float radius, //输入,取点半径
  493. float ymin,
  494. float ymax,
  495. std::vector<pcl::PointXYZ>&target_root, //输入,位置
  496. std::vector<pcl::PointXYZ>&target_filtered, //输出,位置
  497. std::vector<pcl::PointXYZ>&target_filtered_root, //输出,茎上根部点坐标
  498. std::vector<std::vector<int>>&target_filtered_element //输出,茎上点index
  499. )
  500. {
  501. target_filtered.clear();
  502. target_filtered_element.clear();
  503. target_filtered_root.clear();
  504. if (target_root.size() == 0) { return; }
  505. for (auto&p : target_root) {
  506. // 构建box,获取植株点云
  507. pcl::PointCloud<pcl::PointXYZ>::Ptr seedling_inbox(new pcl::PointCloud<pcl::PointXYZ>);
  508. pcl::CropBox<pcl::PointXYZ> box_filter;
  509. std::vector<int>inbox_idx;
  510. pcl::PointXYZ min_point_aabb_ex;
  511. pcl::PointXYZ max_point_aabb_ex;
  512. min_point_aabb_ex.x = p.x - radius;
  513. min_point_aabb_ex.y = ymin;
  514. min_point_aabb_ex.z = p.z - radius;
  515. max_point_aabb_ex.x = p.x + radius;
  516. max_point_aabb_ex.y = ymax;
  517. max_point_aabb_ex.z = p.z + radius;
  518. box_filter.setMin(Eigen::Vector4f(min_point_aabb_ex.x, min_point_aabb_ex.y, min_point_aabb_ex.z, 1));
  519. box_filter.setMax(Eigen::Vector4f(max_point_aabb_ex.x, max_point_aabb_ex.y, max_point_aabb_ex.z, 1));
  520. box_filter.setNegative(false);
  521. box_filter.setInputCloud(in_cloud);
  522. box_filter.filter(inbox_idx);
  523. pcl::copyPointCloud(*in_cloud, inbox_idx, *seedling_inbox);
  524. //植株点云直线查找
  525. //找到inbox点云中的直线
  526. float stem_radius = m_cparam.rs_grab_stem_diameter / 2.0;
  527. if (m_dtype == 0) {
  528. stem_radius = m_cparam.sc_grab_stem_diameter / 2.0;
  529. }
  530. pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
  531. pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
  532. pcl::SACSegmentation<pcl::PointXYZ> seg;
  533. seg.setOptimizeCoefficients(true);
  534. seg.setModelType(pcl::SACMODEL_LINE);
  535. seg.setDistanceThreshold(stem_radius);
  536. seg.setInputCloud(seedling_inbox);
  537. seg.segment(*inliers, *coefficients);
  538. pcl::PointCloud<pcl::PointXYZ>::Ptr stem_cloud(new pcl::PointCloud<pcl::PointXYZ>);
  539. pcl::copyPointCloud(*seedling_inbox, *inliers, *stem_cloud);
  540. //点数过滤
  541. int min_stem_pts = m_cparam.rs_grab_stem_min_pts;
  542. if (m_dtype == 0) { min_stem_pts = m_cparam.sc_grab_stem_min_pts; }
  543. if (inliers->indices.size() < int(min_stem_pts / 2)) { continue; }
  544. //y方向分布范围滤波
  545. pcl::PointXYZ min_v;
  546. pcl::PointXYZ max_v;
  547. pcl::getMinMax3D(*stem_cloud, min_v, max_v);
  548. float dy = max_v.y - min_v.y;
  549. if (dy / (ymax - ymin) < 0.5) { continue; }
  550. //y方向分布中心滤波
  551. float dy_c = 0.5*(max_v.y + min_v.y);
  552. if ((dy_c - ymin) / (ymax - ymin) > 0.75) { continue; }
  553. //倾斜角度过滤
  554. float xz = std::sqrt(coefficients->values.at(3) * coefficients->values.at(3) +
  555. coefficients->values.at(5) * coefficients->values.at(5));
  556. float a = std::atan2f(coefficients->values.at(4), xz);
  557. a = std::fabs(a) * 180.0 / 3.14159;
  558. if (a < 45.0) { continue; }
  559. target_filtered.push_back(p);
  560. float min_y = 10000.0;
  561. int min_y_idx = -1;
  562. for (size_t j = 0; j < stem_cloud->points.size();++j) {
  563. pcl::PointXYZ &spt = stem_cloud->at(j);
  564. if (spt.y < min_y) {
  565. min_y = spt.y;
  566. min_y_idx = j;
  567. }
  568. }
  569. pcl::PointXYZ tr_pt(stem_cloud->points.at(min_y_idx).x, stem_cloud->points.at(min_y_idx).y, stem_cloud->points.at(min_y_idx).z);
  570. target_filtered_root.push_back(tr_pt);
  571. int idx_raw = 0;
  572. std::vector<int> out_idx;
  573. for (auto& idx : inliers->indices) {
  574. idx_raw = inbox_idx.at(idx);
  575. out_idx.push_back(idx_raw);
  576. }
  577. target_filtered_element.push_back(out_idx);
  578. // debug show
  579. /*if (m_cparam.image_show) {
  580. pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_line(new pcl::PointCloud<pcl::PointXYZ>);
  581. pcl::copyPointCloud(*seedling_inbox, *inliers, *cloud_line);
  582. pcl::visualization::PCLVisualizer::Ptr viewer(new pcl::visualization::PCLVisualizer("line in cluster"));
  583. viewer->addPointCloud<pcl::PointXYZ>(seedling_inbox, "cluster");
  584. viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 1, 1, 1, "cluster");
  585. viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "cluster");
  586. viewer->addPointCloud(cloud_line, "fit_line");
  587. viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 0, 1, 0, "fit_line");
  588. viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "fit_line");
  589. while (!viewer->wasStopped()) {
  590. viewer->spinOnce(100);
  591. }
  592. }*/
  593. }
  594. }
  595. void CRootStockGrabPoint::find_seedling_position_key(
  596. pcl::PointCloud<pcl::PointXYZ>::Ptr in_cloud,
  597. std::vector<int> &first_seedling_cloud_idx,
  598. pcl::PointXYZ&xz_center
  599. )
  600. {
  601. // 确定植株inbox范围
  602. float hole_step = m_cparam.rs_grab_seedling_dist - 5.0; //穴盘中穴间距
  603. if (m_dtype == 0) {
  604. hole_step = m_cparam.sc_grab_seedling_dist - 5.0;
  605. }
  606. float hole_step_radius = hole_step / 2.0;
  607. // 点云降维到xz平面,y=0
  608. pcl::PointCloud<pcl::PointXYZ>::Ptr cloud2d(new pcl::PointCloud < pcl::PointXYZ>);
  609. pcl::copyPointCloud(*in_cloud, *cloud2d);
  610. for (auto&pt : *cloud2d) {
  611. pt.y = 0.0;
  612. }
  613. // 在xz平面内统计点的密度
  614. double radius = m_cparam.rs_grab_stem_diameter;
  615. if (m_dtype == 0) {
  616. radius = m_cparam.sc_grab_stem_diameter;
  617. }
  618. std::vector<int> counter;
  619. pcl::KdTreeFLANN<pcl::PointXYZ> kdtree;
  620. kdtree.setInputCloud(cloud2d);
  621. std::vector<int>idx;
  622. std::vector<float>dist_sqr;
  623. for (size_t i = 0; i < cloud2d->points.size(); ++i) {
  624. int k = kdtree.radiusSearch(cloud2d->points.at(i), radius, idx, dist_sqr);
  625. counter.push_back(k);
  626. }
  627. int max_density_idx = std::max_element(counter.begin(), counter.end()) - counter.begin();
  628. int max_density = counter.at(max_density_idx);
  629. if (m_cparam.image_show) {
  630. //显示密度最大的点的位置
  631. pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_tmp(new pcl::PointCloud<pcl::PointXYZRGB>);
  632. pcl::copyPointCloud(*in_cloud, *cloud_tmp);
  633. for (auto& pt : *cloud_tmp) {
  634. pt.r = 255;
  635. pt.g = 255;
  636. pt.b = 255;
  637. }
  638. std::vector<pcl::PointXYZ>cc;
  639. cc.push_back(in_cloud->points.at(max_density_idx));
  640. viewer_cloud_cluster(cloud_tmp, cc, string("key"));
  641. }
  642. // 生成植株的中心及box
  643. std::vector<pcl::PointXYZ>clt_root;
  644. std::vector<pcl::PointXYZ> clt_box;
  645. float ymin = m_cparam.rs_grab_ymin;
  646. float ymax = m_cparam.rs_grab_ymax;
  647. if (m_dtype == 0) {
  648. ymin = m_cparam.sc_grab_ymin;
  649. ymax = m_cparam.sc_grab_ymax;
  650. }
  651. gen_all_seedling_positions(in_cloud->points.at(max_density_idx), clt_root);
  652. // 每个位置点云情况,过滤
  653. std::vector<int> valid_index; //初步有效的矩形index
  654. std::vector<int> valid_box_pts; //立方体内点云数量
  655. std::vector<float>valid_box_cc_dist;//重心和矩形中心距离
  656. for (size_t i = 0; i < clt_root.size();++i) {
  657. pcl::PointXYZ &p = clt_root.at(i);
  658. pcl::PointCloud<pcl::PointXYZ>::Ptr seedling_inbox(new pcl::PointCloud<pcl::PointXYZ>);
  659. pcl::CropBox<pcl::PointXYZ> box_filter;
  660. std::vector<int>inbox_idx;
  661. pcl::PointXYZ min_point_aabb_ex;
  662. pcl::PointXYZ max_point_aabb_ex;
  663. min_point_aabb_ex.x = p.x - hole_step_radius;
  664. min_point_aabb_ex.y = ymin;
  665. min_point_aabb_ex.z = p.z - hole_step_radius;
  666. max_point_aabb_ex.x = p.x + hole_step_radius;
  667. max_point_aabb_ex.y = ymax;
  668. max_point_aabb_ex.z = p.z + hole_step_radius;
  669. box_filter.setMin(Eigen::Vector4f(min_point_aabb_ex.x, min_point_aabb_ex.y, min_point_aabb_ex.z, 1));
  670. box_filter.setMax(Eigen::Vector4f(max_point_aabb_ex.x, max_point_aabb_ex.y, max_point_aabb_ex.z, 1));
  671. box_filter.setNegative(false);
  672. box_filter.setInputCloud(in_cloud);
  673. box_filter.filter(inbox_idx);
  674. pcl::copyPointCloud(*in_cloud, inbox_idx, *seedling_inbox);
  675. //点数过滤
  676. if (inbox_idx.size() < int(max_density/2)) { continue; }
  677. //y方向分布范围滤波
  678. pcl::PointXYZ min_v;
  679. pcl::PointXYZ max_v;
  680. pcl::getMinMax3D(*seedling_inbox, min_v, max_v);
  681. float dy = max_v.y - min_v.y;
  682. if (dy / (ymax - ymin) < 0.5) { continue; }
  683. //y方向分布中心滤波
  684. float dy_c = 0.5*(max_v.y + min_v.y);
  685. if ((dy_c-ymin) / (ymax - ymin) > 0.75) { continue; }
  686. //计算中心
  687. Eigen::Vector4f mean;
  688. pcl::compute3DCentroid(*seedling_inbox, mean);
  689. double cc_dist = std::sqrt((mean[0] - p.x)*(mean[0] - p.x) + (mean[2] - p.z)*(mean[2] - p.z));
  690. valid_index.push_back(i);
  691. valid_box_pts.push_back(inbox_idx.size());
  692. valid_box_cc_dist.push_back((float)cc_dist);
  693. }
  694. // 找到局部最大值,找出真正的位置
  695. std::vector<pcl::PointXYZ> clt_root_v;
  696. std::vector<pcl::PointXYZ> clt_box_v;
  697. for (auto&i : valid_index) {
  698. pcl::PointXYZ&p = clt_root.at(i);
  699. pcl::PointXYZ p_show(p.x, ymin, p.z);
  700. clt_root_v.push_back(p_show);
  701. pcl::PointXYZ min_point_aabb_ex;
  702. pcl::PointXYZ max_point_aabb_ex;
  703. min_point_aabb_ex.x = p.x - hole_step_radius;
  704. min_point_aabb_ex.y = ymin;
  705. min_point_aabb_ex.z = p.z - hole_step_radius;
  706. max_point_aabb_ex.x = p.x + hole_step_radius;
  707. max_point_aabb_ex.y = ymax;
  708. max_point_aabb_ex.z = p.z + hole_step_radius;
  709. clt_box_v.push_back(min_point_aabb_ex);
  710. clt_box_v.push_back(max_point_aabb_ex);
  711. }
  712. // 显示
  713. if (m_cparam.image_show) {
  714. std::vector<std::vector<int> > clt_line_idx;
  715. viewer_cloud_cluster_box(in_cloud, clt_root_v, clt_box_v, clt_line_idx, std::string("valid_box"));
  716. }
  717. std::vector<pcl::PointXYZ> target_root;
  718. nms_box(clt_root_v, valid_box_cc_dist, hole_step_radius, target_root);
  719. // 显示
  720. if (m_cparam.image_show) {
  721. std::vector<std::vector<int> > clt_line_idx;
  722. std::vector<pcl::PointXYZ>clt_box_tmp;
  723. for (auto&p : target_root) {
  724. pcl::PointXYZ p_show(p.x, ymin, p.z);
  725. clt_root_v.push_back(p_show);
  726. pcl::PointXYZ min_point_aabb_ex;
  727. pcl::PointXYZ max_point_aabb_ex;
  728. min_point_aabb_ex.x = p.x - hole_step_radius;
  729. min_point_aabb_ex.y = ymin;
  730. min_point_aabb_ex.z = p.z - hole_step_radius;
  731. max_point_aabb_ex.x = p.x + hole_step_radius;
  732. max_point_aabb_ex.y = ymax;
  733. max_point_aabb_ex.z = p.z + hole_step_radius;
  734. clt_box_tmp.push_back(min_point_aabb_ex);
  735. clt_box_tmp.push_back(max_point_aabb_ex);
  736. }
  737. viewer_cloud_cluster_box(in_cloud, target_root, clt_box_tmp, clt_line_idx, std::string("nms_box"));
  738. }
  739. // 根据得到的位置,直线检测,并过滤
  740. std::vector<pcl::PointXYZ>target_filtered;
  741. std::vector<std::vector<int>> target_member;
  742. std::vector<pcl::PointXYZ>target_filtered_root;
  743. line_filter(in_cloud, hole_step_radius,ymin,ymax, target_root, target_filtered, target_filtered_root, target_member);
  744. if (m_cparam.image_show) {
  745. std::vector<pcl::PointXYZ> final_box;
  746. for (auto&pt : target_filtered_root) {
  747. //扩展矩形范围
  748. pcl::PointXYZ min_point_aabb_ex;
  749. pcl::PointXYZ max_point_aabb_ex;
  750. min_point_aabb_ex.x = pt.x - hole_step_radius;
  751. min_point_aabb_ex.y = ymin;
  752. min_point_aabb_ex.z = pt.z - hole_step_radius;
  753. max_point_aabb_ex.x = pt.x + hole_step_radius;
  754. max_point_aabb_ex.y = ymax;
  755. max_point_aabb_ex.z = pt.z + hole_step_radius;
  756. final_box.push_back(min_point_aabb_ex);
  757. final_box.push_back(max_point_aabb_ex);
  758. }
  759. std::vector<std::vector<int> > clt_line_idx_;
  760. viewer_cloud_cluster_box(in_cloud, target_filtered_root, final_box, clt_line_idx_, std::string("line_filter"));
  761. }
  762. //sort cluster center, get the frontest leftest one
  763. //找最小z,然后计算平均z
  764. float min_root_z = 10000.0;
  765. for(auto&pt : target_filtered_root) {
  766. if (pt.z < min_root_z) { min_root_z = pt.z; }
  767. }
  768. //通过最小z,保守的找到和他一排的植株
  769. std::vector<int> first_row_index;
  770. float mean_z = 0.0;
  771. for (int idx_r = 0; idx_r < target_filtered_root.size();++idx_r) {
  772. pcl::PointXYZ&pt = target_filtered_root.at(idx_r);
  773. if (std::fabs(pt.z - min_root_z) < hole_step) {
  774. first_row_index.push_back(idx_r);
  775. mean_z += pt.z;
  776. }
  777. }
  778. //如果第一排的植株大于3,计算平均值
  779. float first_row_num = (float)first_row_index.size();
  780. mean_z /= first_row_num;
  781. if (first_row_num >= 4) {
  782. if (m_dtype == 0) {
  783. m_1st_row_zmean_sc = mean_z;
  784. }
  785. else{ m_1st_row_zmean_rs = mean_z; }
  786. }
  787. else {
  788. if( m_dtype == 0) {
  789. if (m_1st_row_zmean_sc > 0) { mean_z = m_1st_row_zmean_sc; }
  790. }
  791. else {
  792. if (m_1st_row_zmean_rs > 0) { mean_z = m_1st_row_zmean_rs; }
  793. }
  794. }
  795. //通过mean_z再次寻找第一排的植株
  796. first_row_index.clear();
  797. for (int idx_r = 0; idx_r < target_filtered_root.size(); ++idx_r) {
  798. pcl::PointXYZ&pt = target_filtered_root.at(idx_r);
  799. if (std::fabs(pt.z - mean_z) < hole_step) {
  800. first_row_index.push_back(idx_r);
  801. }
  802. }
  803. std::vector<float> cluster_index;
  804. for (int i=0; i<first_row_index.size();++i){
  805. int raw_idx = first_row_index.at(i);
  806. pcl::PointXYZ&pt = target_filtered_root.at(raw_idx);
  807. cluster_index.push_back(pt.x);
  808. }
  809. int first_idx = std::min_element(cluster_index.begin(), cluster_index.end()) - cluster_index.begin();
  810. if (m_dtype == 0) {
  811. first_idx = std::max_element(cluster_index.begin(), cluster_index.end()) - cluster_index.begin();
  812. }
  813. first_idx = first_row_index.at(first_idx);
  814. first_seedling_cloud_idx.clear();
  815. for (auto&v : target_member.at(first_idx)) {
  816. first_seedling_cloud_idx.push_back(v);
  817. }
  818. xz_center.x = target_filtered_root.at(first_idx).x;
  819. xz_center.y = target_filtered_root.at(first_idx).y;
  820. xz_center.z = target_filtered_root.at(first_idx).z;
  821. if (m_pLogger) {
  822. stringstream buff;
  823. buff << "euclidean_clustering_ttsas, find cluster center(" << xz_center.x
  824. <<", "<< xz_center.y<<", "<< xz_center.z<<")";
  825. m_pLogger->INFO(buff.str());
  826. }
  827. }
  828. void CRootStockGrabPoint::tilted_seedling_discover(
  829. std::vector<pcl::PointXYZ>&cluster_center, //输入,已知的苗的坐标
  830. std::vector<pcl::PointXYZ>&tilted_center //输出,有倾斜苗的坐标
  831. )
  832. {
  833. tilted_center.clear();
  834. if (cluster_center.size() == 0) { return; }
  835. //找到z最小的那一株,并找出和它一排的那些
  836. float step_harf = m_cparam.rs_grab_seedling_dist / 2.0;
  837. float xmin = m_cparam.rs_grab_xmin;
  838. float xmax = m_cparam.rs_grab_xmax;
  839. float zmin = m_cparam.rs_grab_zmin;
  840. float zmax = m_cparam.rs_grab_zmax;
  841. if (m_dtype == 0) {
  842. step_harf = m_cparam.sc_grab_seedling_dist / 2.0;
  843. xmin = m_cparam.sc_grab_xmin;
  844. xmax = m_cparam.sc_grab_xmax;
  845. zmin = m_cparam.sc_grab_zmin;
  846. zmax = m_cparam.sc_grab_zmax;
  847. }
  848. auto minz_it = std::min_element(cluster_center.begin(), cluster_center.end(),
  849. [](const pcl::PointXYZ& p1, const pcl::PointXYZ& p2)-> bool {return p1.z < p2.z; });
  850. float minz = minz_it->z;
  851. float meanz = 0.0;
  852. float cnt = 0.0;
  853. for (auto&p : cluster_center) {
  854. if (fabs(p.z - minz) < step_harf) { meanz += p.z; cnt += 1.0; }
  855. }
  856. if(cnt>0.0){meanz /= cnt;}
  857. else { meanz = minz; }
  858. // 生成所有可能的植株位置中心点
  859. std::vector<pcl::PointXYZ>candidate_centers;
  860. for (int row = -1; row <= 1; row += 1) {
  861. for (int col = -6; col <= 6; col += 1) {
  862. float cx = minz_it->x + col * 2.0 * step_harf;
  863. float cz = meanz + row * 2.0 * step_harf;
  864. if (cx < xmin || cx > xmax) { continue; }
  865. if (cz < zmin || cz > zmax) { continue; }
  866. // 邻域内已经存在植株的跳过
  867. bool with_seedling = false;
  868. for (auto&p : cluster_center) {
  869. float d = std::sqrt((p.x - cx) * (p.x - cx) + (p.z - cz) * (p.z - cz));
  870. if (d < 2.0*step_harf) {
  871. with_seedling=true;
  872. break;
  873. }
  874. }
  875. if (!with_seedling) {//保存
  876. pcl::PointXYZ pc(cx,0.0,cz);
  877. candidate_centers.push_back(pc);
  878. }
  879. }
  880. }
  881. //针对每一个candidat_centers的邻域进行检查,检测是否有茎目标
  882. for (auto&pc : candidate_centers) {
  883. }
  884. }
  885. void CRootStockGrabPoint::find_seedling_position(
  886. pcl::PointCloud<pcl::PointXYZ>::Ptr in_cloud,
  887. std::vector<int> &first_seedling_cloud_idx,
  888. pcl::PointXYZ&xz_center
  889. )
  890. {
  891. // 确定植株inbox范围
  892. float hole_step = m_cparam.rs_grab_seedling_dist - 10.0; //穴盘中穴间距
  893. if (m_dtype == 0) {
  894. hole_step = m_cparam.sc_grab_seedling_dist - 10.0;
  895. }
  896. float hole_step_radius = hole_step / 2.0;
  897. // 点云降维到xz平面,y=0
  898. pcl::PointCloud<pcl::PointXYZ>::Ptr cloud2d(new pcl::PointCloud < pcl::PointXYZ>);
  899. pcl::copyPointCloud(*in_cloud, *cloud2d);
  900. for (auto&pt : *cloud2d) {
  901. pt.y = 0.0;
  902. }
  903. /*if(m_cparam.image_show){
  904. viewer_cloud(cloud2d, std::string("cloud2d"));
  905. } */
  906. // 在xz平面内统计点的密度
  907. double radius = m_cparam.rs_grab_stem_diameter;
  908. if (m_dtype == 0) {
  909. radius = m_cparam.sc_grab_stem_diameter;
  910. }
  911. std::vector<int> counter;
  912. pcl::KdTreeFLANN<pcl::PointXYZ> kdtree;
  913. kdtree.setInputCloud(cloud2d);
  914. std::vector<int>idx;
  915. std::vector<float>dist_sqr;
  916. for (size_t i = 0; i < cloud2d->points.size(); ++i) {
  917. int k = kdtree.radiusSearch(cloud2d->points.at(i), radius, idx, dist_sqr);
  918. counter.push_back(k);
  919. }
  920. // 计算根据密度分割的自动阈值
  921. int th = (int)(otsu(counter));
  922. // 得到茎目标的点云序号,和对应的点云cloud2d_pos
  923. std::vector<int> index;
  924. for (size_t i = 0; i < counter.size(); ++i) {
  925. if (counter.at(i) >= th) {
  926. index.push_back(i);
  927. }
  928. }
  929. pcl::PointCloud<pcl::PointXYZ>::Ptr cloud2d_pos(new pcl::PointCloud < pcl::PointXYZ>);
  930. pcl::copyPointCloud(*cloud2d, index, *cloud2d_pos);
  931. if (m_pLogger) {
  932. stringstream buff;
  933. buff << "get 2d seedling seed point cloud " << index.size() << " data points with thrreshold " << th;
  934. m_pLogger->INFO(buff.str());
  935. }
  936. if (m_cparam.image_show) {
  937. viewer_cloud(cloud2d_pos, std::string("cloud2d_pos"));
  938. }
  939. //对茎的点云进行clustering,得到不同的茎的分组
  940. double d1 = m_cparam.rs_grab_stem_diameter;
  941. double d2 = m_cparam.rs_grab_stem_diameter * 3.0;
  942. if (m_dtype == 0) {
  943. d1 = m_cparam.sc_grab_stem_diameter;
  944. d2 = m_cparam.sc_grab_stem_diameter * 3.0;
  945. }
  946. std::vector<pcl::PointXYZ>cluster_center;
  947. std::vector<std::vector<int>> cluster_member;
  948. euclidean_clustering_ttsas(cloud2d_pos, d1, d2, cluster_center, cluster_member);
  949. if (m_pLogger) {
  950. stringstream buff;
  951. buff << "euclidean_clustering_ttsas: " << cluster_center.size() << " t1=" << d1
  952. << " t2=" << d2;
  953. m_pLogger->INFO(buff.str());
  954. }
  955. if (m_cparam.image_show) {
  956. pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_cluster(new pcl::PointCloud<pcl::PointXYZRGB>);
  957. pcl::copyPointCloud(*cloud2d_pos, *cloud_cluster);
  958. for (auto& pt : *cloud_cluster) {
  959. pt.r = 255;
  960. pt.g = 255;
  961. pt.b = 255;
  962. }
  963. viewer_cloud_cluster(cloud_cluster, cluster_center, std::string("cloud2d_cluster_pos"));
  964. }
  965. // 扩展检测第一排的可能位置,找到倾斜的苗
  966. std::vector<pcl::PointXYZ> tilted_center;
  967. tilted_seedling_discover(cluster_center, tilted_center);
  968. //对每个类别进行检测,剔除不是茎的类别
  969. std::vector<pcl::PointXYZ> clt_root;
  970. std::vector<pcl::PointXYZ> clt_box;
  971. std::vector<std::vector<int> > clt_line_idx;
  972. for (size_t i = 0; i < cluster_center.size(); ++i) {
  973. pcl::PointIndices cluster_idxs;
  974. for (auto idx_clt : cluster_member.at(i)) {
  975. int idx_raw = index.at(idx_clt);
  976. cluster_idxs.indices.push_back(idx_raw);
  977. }
  978. pcl::PointCloud<pcl::PointXYZ>::Ptr clt_cloud(new pcl::PointCloud < pcl::PointXYZ>);
  979. pcl::copyPointCloud(*in_cloud, cluster_idxs, *clt_cloud);
  980. //计算每一个茎的外接矩形
  981. pcl::PointXYZ min_point_aabb;
  982. pcl::PointXYZ max_point_aabb;
  983. pcl::MomentOfInertiaEstimation<pcl::PointXYZ> feature_extractor;
  984. feature_extractor.setInputCloud(clt_cloud);
  985. feature_extractor.compute();
  986. feature_extractor.getAABB(min_point_aabb, max_point_aabb);
  987. //扩展矩形范围
  988. pcl::PointXYZ min_point_aabb_ex;
  989. pcl::PointXYZ max_point_aabb_ex;
  990. min_point_aabb_ex.x = 0.5*(min_point_aabb.x + max_point_aabb.x) - hole_step_radius;
  991. min_point_aabb_ex.y = m_cparam.sc_grab_ymin;
  992. if (m_dtype != 0) { min_point_aabb_ex.y = m_cparam.rs_grab_ymin; }
  993. min_point_aabb_ex.z = 0.5*(min_point_aabb.z + max_point_aabb.z) - hole_step_radius;
  994. max_point_aabb_ex.x = 0.5*(min_point_aabb.x + max_point_aabb.x) + hole_step_radius;
  995. max_point_aabb_ex.y = m_cparam.sc_grab_ymax;
  996. if (m_dtype != 0) { max_point_aabb_ex.y = m_cparam.rs_grab_ymax; }
  997. max_point_aabb_ex.z = 0.5*(min_point_aabb.z + max_point_aabb.z) + hole_step_radius;
  998. /////////////////////////////////
  999. //扩展矩形内直线检测
  1000. std::vector<int>line_idx;
  1001. find_line_in_cube(in_cloud, min_point_aabb_ex, max_point_aabb_ex, line_idx);
  1002. clt_line_idx.push_back(line_idx);
  1003. //找到line_idx中y最小的那个点的idx
  1004. int line_root_idx = -1;
  1005. float line_root_y_min = 10000.0;
  1006. for (size_t lidx = 0; lidx < line_idx.size(); ++lidx) {
  1007. int raw_idx = line_idx.at(lidx);
  1008. if (in_cloud->at(raw_idx).y < line_root_y_min) {
  1009. line_root_y_min = in_cloud->at(raw_idx).y;
  1010. line_root_idx = raw_idx;
  1011. }
  1012. }
  1013. //找到底部中心点,然后找到根部坐标
  1014. pcl::PointXYZ pt_root;
  1015. pt_root = in_cloud->at(line_root_idx);
  1016. clt_root.push_back(pt_root);
  1017. clt_box.push_back(min_point_aabb_ex);
  1018. clt_box.push_back(max_point_aabb_ex);
  1019. //viewer_cloud(clt_cloud, std::string("cluster"));
  1020. }
  1021. if (m_cparam.image_show) {
  1022. viewer_cloud_cluster_box(in_cloud, clt_root, clt_box, clt_line_idx, std::string("cloud2d_cluster_box"));
  1023. }
  1024. //sort cluster center, get the frontest leftest one
  1025. std::vector<float> cluster_index;
  1026. for (auto&pt : clt_root) {
  1027. float idx = pt.x - pt.z;
  1028. cluster_index.push_back(idx);
  1029. }
  1030. int first_idx = std::min_element(cluster_index.begin(), cluster_index.end()) - cluster_index.begin();
  1031. if (m_dtype == 0) {
  1032. first_idx = std::max_element(cluster_index.begin(), cluster_index.end()) - cluster_index.begin();
  1033. }
  1034. first_seedling_cloud_idx.clear();
  1035. for (auto&v : clt_line_idx.at(first_idx)) {
  1036. first_seedling_cloud_idx.push_back(v);
  1037. }
  1038. xz_center.x = clt_root.at(first_idx).x;
  1039. xz_center.y = clt_root.at(first_idx).y;
  1040. xz_center.z = clt_root.at(first_idx).z;
  1041. if (m_pLogger) {
  1042. stringstream buff;
  1043. buff << "euclidean_clustering_ttsas, find cluster center(" << xz_center.x
  1044. << ", " << xz_center.y << ", " << xz_center.z << ")";
  1045. m_pLogger->INFO(buff.str());
  1046. }
  1047. }
  1048. void CRootStockGrabPoint::find_line_in_cube(
  1049. pcl::PointCloud<pcl::PointXYZ>::Ptr in_cloud, //输入,整体点云
  1050. pcl::PointXYZ& min_pt_ex, //输入,
  1051. pcl::PointXYZ& max_pt_ex, //输入,
  1052. std::vector<int>& out_idx //输出,直线上点的index, 基于输入整体点云
  1053. )
  1054. {
  1055. out_idx.clear();
  1056. pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_inbox(new pcl::PointCloud<pcl::PointXYZ>);
  1057. pcl::CropBox<pcl::PointXYZ> box_filter;
  1058. std::vector<int>inbox_idx;
  1059. box_filter.setMin(Eigen::Vector4f(min_pt_ex.x, min_pt_ex.y, min_pt_ex.z, 1));
  1060. box_filter.setMax(Eigen::Vector4f(max_pt_ex.x, max_pt_ex.y, max_pt_ex.z, 1));
  1061. box_filter.setNegative(false);
  1062. box_filter.setInputCloud(in_cloud);
  1063. box_filter.filter(inbox_idx);
  1064. pcl::copyPointCloud(*in_cloud, inbox_idx, *cloud_inbox);
  1065. //找到inbox点云中的直线
  1066. float stem_radius = m_cparam.rs_grab_stem_diameter / 2.0;
  1067. if (m_dtype == 0) {
  1068. stem_radius = m_cparam.sc_grab_stem_diameter / 2.0;
  1069. }
  1070. pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
  1071. pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
  1072. pcl::SACSegmentation<pcl::PointXYZ> seg;
  1073. seg.setOptimizeCoefficients(true);
  1074. seg.setModelType(pcl::SACMODEL_LINE);
  1075. seg.setDistanceThreshold(stem_radius);
  1076. seg.setInputCloud(cloud_inbox);
  1077. seg.segment(*inliers, *coefficients);
  1078. int idx_raw = 0;
  1079. for (auto& idx : inliers->indices) {
  1080. idx_raw = inbox_idx.at(idx);
  1081. out_idx.push_back(idx_raw);
  1082. }
  1083. if (m_cparam.image_show) {
  1084. pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_line(new pcl::PointCloud<pcl::PointXYZ>);
  1085. pcl::copyPointCloud(*cloud_inbox, *inliers, *cloud_line);
  1086. pcl::visualization::PCLVisualizer::Ptr viewer(new pcl::visualization::PCLVisualizer("line in cluster"));
  1087. viewer->addPointCloud<pcl::PointXYZ>(cloud_inbox, "cluster");
  1088. viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 1, 1, 1, "cluster");
  1089. viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "cluster");
  1090. viewer->addPointCloud(cloud_line, "fit_line");
  1091. viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 0, 1, 0, "fit_line");
  1092. viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "fit_line");
  1093. while (!viewer->wasStopped()) {
  1094. viewer->spinOnce(100);
  1095. }
  1096. }
  1097. }
  1098. void CRootStockGrabPoint::crop_nn_analysis(
  1099. pcl::PointCloud<pcl::PointXYZ>::Ptr in_cloud,
  1100. pcl::PointCloud<pcl::PointXYZ>::Ptr seed_cloud,
  1101. double dist_mean,
  1102. std::vector<double>&mass_indices,
  1103. std::vector<int>& candidate_idx
  1104. )
  1105. {
  1106. candidate_idx.clear();
  1107. pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_inbox(new pcl::PointCloud<pcl::PointXYZ>);
  1108. pcl::CropBox<pcl::PointXYZ> box_filter;
  1109. box_filter.setNegative(false);
  1110. box_filter.setInputCloud(in_cloud);
  1111. double radius = 5;
  1112. double rx = 0.8;
  1113. double ry = 1.0;
  1114. double rz = 0.8;
  1115. double cx, cy, cz;
  1116. double dz;
  1117. for (size_t i = 0; i < seed_cloud->points.size(); ++i) {
  1118. cx = seed_cloud->points.at(i).x;
  1119. cy = seed_cloud->points.at(i).y;
  1120. cz = seed_cloud->points.at(i).z;
  1121. box_filter.setMin(Eigen::Vector4f(cx - rx*radius, cy - ry*radius, cz - rz*radius, 1));
  1122. box_filter.setMax(Eigen::Vector4f(cx + rx*radius, cy + ry*radius, cz + rz*radius, 1));
  1123. box_filter.filter(*cloud_inbox);
  1124. //dz
  1125. Eigen::Vector4f min_point;
  1126. Eigen::Vector4f max_point;
  1127. pcl::getMinMax3D(*cloud_inbox, min_point, max_point);
  1128. dz = max_point(2) - min_point(2);
  1129. //project to xy-plane
  1130. pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_inbox_xy(new pcl::PointCloud<pcl::PointXYZ>);
  1131. pcl::copyPointCloud(*cloud_inbox, *cloud_inbox_xy);
  1132. for (auto&pt : *cloud_inbox_xy) { pt.z = 0.0; }
  1133. //pca
  1134. double dx_obb;
  1135. double dy_obb;
  1136. double angle_obb;
  1137. cal_obb_2d(cloud_inbox_xy, 0, dx_obb, dy_obb, angle_obb);
  1138. try {
  1139. double dx_grid = dx_obb / dist_mean;
  1140. double dy_grid = dy_obb / dist_mean;
  1141. double dz_grid = dz / dist_mean;
  1142. double fill_ratio = cloud_inbox->points.size() / dx_grid / dy_grid / dz_grid;
  1143. double y_ratio = dy_obb / dx_obb/2;
  1144. y_ratio = pow(y_ratio, 2);
  1145. double a_ratio = cos((angle_obb - 90)*PI / 180.0);
  1146. a_ratio = pow(a_ratio, 3);
  1147. double mass_index = fill_ratio*y_ratio*a_ratio;
  1148. mass_indices.push_back(mass_index);
  1149. if (m_pLogger) {
  1150. stringstream buff;
  1151. buff << i << "/" << seed_cloud->points.size() << " dx=" << dx_obb << ", dy=" << dy_obb << ", fill_ratio=" << fill_ratio
  1152. << ", y_ratio=" << y_ratio << ", a_ratio=" << a_ratio << ", mass_index=" << mass_index;
  1153. m_pLogger->INFO(buff.str());
  1154. }
  1155. }
  1156. catch (...) {
  1157. mass_indices.push_back(0);
  1158. }
  1159. }
  1160. // sort by mass_indices
  1161. std::vector<size_t> sort_idx = sort_indexes_e(mass_indices, false);
  1162. for (auto&v : sort_idx) { candidate_idx.push_back((int)v); }
  1163. }
  1164. void CRootStockGrabPoint::euclidean_clustering_ttsas(
  1165. pcl::PointCloud<pcl::PointXYZ>::Ptr in_cloud,
  1166. double d1, double d2,
  1167. std::vector<pcl::PointXYZ>&cluster_center,
  1168. std::vector<std::vector<int>> &clustr_member
  1169. )
  1170. {
  1171. if (m_pLogger) {
  1172. stringstream buff;
  1173. buff << "euclidean_clustering_ttsas() begin...";
  1174. m_pLogger->INFO(buff.str());
  1175. }
  1176. std::vector<int> cluster_weight;
  1177. std::vector<int> data_stat;
  1178. std::vector<pcl::PointXYZ>cluster_center_raw;
  1179. std::vector<std::vector<int>> clustr_member_raw;
  1180. for (size_t i = 0; i < in_cloud->points.size(); ++i) { data_stat.push_back(0); }
  1181. size_t data_len = in_cloud->points.size();
  1182. int exists_change = 0;
  1183. int prev_change = 0;
  1184. int cur_change = 0;
  1185. int m = 0;
  1186. while (std::find(data_stat.begin(), data_stat.end(), 0) != data_stat.end()) {
  1187. bool new_while_first = true;
  1188. for (size_t i = 0; i < data_len; ++i) {
  1189. if (data_stat.at(i) == 0 && new_while_first && exists_change == 0) {
  1190. new_while_first = false;
  1191. std::vector<int> idx;
  1192. idx.push_back(i);
  1193. clustr_member_raw.push_back(idx);
  1194. pcl::PointXYZ center;
  1195. center.x = in_cloud->points.at(i).x;
  1196. center.y = in_cloud->points.at(i).y;
  1197. center.z = in_cloud->points.at(i).z;
  1198. cluster_center_raw.push_back(center);
  1199. data_stat.at(i) = 1;
  1200. cur_change += 1;
  1201. cluster_weight.push_back(1);
  1202. m += 1;
  1203. }
  1204. else if (data_stat[i] == 0) {
  1205. std::vector<float> distances;
  1206. for (size_t j = 0; j < clustr_member_raw.size(); ++j) {
  1207. std::vector<float> distances_sub;
  1208. for (size_t jj = 0; jj < clustr_member_raw.at(j).size(); ++jj) {
  1209. size_t ele_idx = clustr_member_raw.at(j).at(jj);
  1210. double d = sqrt(
  1211. (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) +
  1212. (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) +
  1213. (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));
  1214. distances_sub.push_back(d);
  1215. }
  1216. double min_dist = *std::min_element(distances_sub.begin(), distances_sub.end());
  1217. distances.push_back(min_dist);
  1218. }
  1219. int min_idx = std::min_element(distances.begin(), distances.end()) - distances.begin();
  1220. if (distances.at(min_idx) < d1) {
  1221. data_stat.at(i) = 1;
  1222. double w = cluster_weight.at(min_idx);
  1223. cluster_weight.at(min_idx) += 1;
  1224. clustr_member_raw.at(min_idx).push_back(i);
  1225. cluster_center_raw.at(min_idx).x = (cluster_center_raw.at(min_idx).x * w + in_cloud->points.at(i).x) / (w + 1);
  1226. cluster_center_raw.at(min_idx).y = (cluster_center_raw.at(min_idx).y * w + in_cloud->points.at(i).y) / (w + 1);
  1227. cluster_center_raw.at(min_idx).z = (cluster_center_raw.at(min_idx).z * w + in_cloud->points.at(i).z) / (w + 1);
  1228. cur_change += 1;
  1229. }
  1230. else if (distances.at(min_idx) > d2) {
  1231. std::vector<int> idx;
  1232. idx.push_back(i);
  1233. clustr_member_raw.push_back(idx);
  1234. pcl::PointXYZ center;
  1235. center.x = in_cloud->points.at(i).x;
  1236. center.y = in_cloud->points.at(i).y;
  1237. center.z = in_cloud->points.at(i).z;
  1238. cluster_center_raw.push_back(center);
  1239. data_stat.at(i) = 1;
  1240. cur_change += 1;
  1241. cluster_weight.push_back(1);
  1242. m += 1;
  1243. }
  1244. }
  1245. else if (data_stat.at(i)== 1) {
  1246. cur_change += 1;
  1247. }
  1248. }
  1249. exists_change = fabs(cur_change - prev_change);
  1250. prev_change = cur_change;
  1251. cur_change = 0;
  1252. }
  1253. // copy result
  1254. for (size_t i = 0; i < clustr_member_raw.size(); ++i) {
  1255. if (clustr_member_raw.at(i).size() < 20) { continue; }
  1256. cluster_center.push_back(cluster_center_raw.at(i));
  1257. clustr_member.push_back(clustr_member_raw.at(i));
  1258. }
  1259. if (m_pLogger) {
  1260. stringstream buff;
  1261. buff << "euclidean_clustering_ttsas() end";
  1262. m_pLogger->INFO(buff.str());
  1263. }
  1264. }
  1265. void CRootStockGrabPoint::cal_obb_2d(
  1266. pcl::PointCloud<pcl::PointXYZ>::Ptr in_cloud,
  1267. int axis, //0--xy, 1--zy
  1268. double &dx_obb,
  1269. double &dy_obb,
  1270. double &angle_obb
  1271. )
  1272. {
  1273. // asign value
  1274. Eigen::MatrixXd pts(in_cloud->points.size(), 2);
  1275. for (size_t i = 0; i < in_cloud->points.size(); ++i) {
  1276. if (axis == 0) {
  1277. pts(i, 0) = in_cloud->points.at(i).x;
  1278. }
  1279. else {
  1280. pts(i, 0) = in_cloud->points.at(i).z;
  1281. }
  1282. pts(i, 1) = in_cloud->points.at(i).y;
  1283. }
  1284. // centerlize
  1285. Eigen::MatrixXd mu = pts.colwise().mean();
  1286. Eigen::RowVectorXd mu_row = mu;
  1287. pts.rowwise() -= mu_row;
  1288. //calculate covariance
  1289. Eigen::MatrixXd C = pts.adjoint()*pts;
  1290. C = C.array() / (pts.cols() - 1);
  1291. //compute eig
  1292. Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> eig(C);
  1293. Eigen::MatrixXd d = eig.eigenvalues();
  1294. Eigen::MatrixXd v = eig.eigenvectors();
  1295. //projection
  1296. Eigen::MatrixXd p = pts * v;
  1297. Eigen::VectorXd minv = p.colwise().minCoeff();
  1298. Eigen::VectorXd maxv = p.colwise().maxCoeff();
  1299. Eigen::VectorXd range = maxv - minv;
  1300. dy_obb = range(1);
  1301. dx_obb = range(0);
  1302. angle_obb = std::atan2(v(1, 1), v(0, 1));
  1303. if (angle_obb < 0) { angle_obb = PI + angle_obb; }
  1304. angle_obb = angle_obb * 180 / PI;
  1305. }
  1306. //void CRootStockGrabPoint::get_optimal_seed(
  1307. // pcl::PointCloud<pcl::PointXYZ>::Ptr in_cloud,
  1308. // pcl::PointXYZ&pt,
  1309. // int &pt_idx)
  1310. //{
  1311. // double d1 = m_cparam.rs_grab_stem_diameter;
  1312. // double d2 = m_cparam.rs_grab_stem_diameter * 4.0;
  1313. // if (m_dtype != 0) {
  1314. // d1 = m_cparam.sc_grab_stem_diameter;
  1315. // d2 = m_cparam.sc_grab_stem_diameter * 4.0;
  1316. // }
  1317. // std::vector<pcl::PointXYZ>cluster_center;
  1318. // std::vector<std::vector<int>> cluster_member;
  1319. // euclidean_clustering_ttsas(in_cloud, d1, d2, cluster_center, cluster_member);
  1320. // float min_y_dist = 1.0e6;
  1321. // int opt_idx = -1;
  1322. // int member_size = 5;
  1323. // float y_opt = m_cparam.rs_grab_y_opt;
  1324. // if (m_dtype != 0) {
  1325. // y_opt = m_cparam.sc_grab_y_opt;
  1326. // }
  1327. // for (int i = 0; i < cluster_member.size(); ++i) {
  1328. // if (cluster_member.at(i).size() < member_size) {
  1329. // continue;
  1330. // }
  1331. // if (fabs(cluster_center.at(i).y -y_opt) < min_y_dist) {
  1332. // min_y_dist = fabs(cluster_center.at(i).y - y_opt);
  1333. // opt_idx = i;
  1334. // }
  1335. // }
  1336. // if (opt_idx < 0) {
  1337. // if (m_pLogger) {
  1338. // stringstream buff;
  1339. // buff << "get_optimal_seed failed, get invalid optimal cluster id";
  1340. // m_pLogger->ERRORINFO(buff.str());
  1341. // }
  1342. // return;
  1343. // }
  1344. // //find nearest pt
  1345. // float nearest_dist = 1.0e6;
  1346. // int nearest_idx = -1;
  1347. // for (int i = 0; i < cluster_member.at(opt_idx).size(); ++i) {
  1348. // int idx = cluster_member.at(opt_idx).at(i);
  1349. // float dist = fabs(in_cloud->points.at(idx).x - cluster_center.at(opt_idx).x) +
  1350. // fabs(in_cloud->points.at(idx).y - cluster_center.at(opt_idx).y) +
  1351. // fabs(in_cloud->points.at(idx).z - cluster_center.at(opt_idx).z);
  1352. // if (dist < nearest_dist) {
  1353. // nearest_dist = dist;
  1354. // nearest_idx = idx;
  1355. // }
  1356. // }
  1357. // pt.x = in_cloud->points.at(nearest_idx).x;
  1358. // pt.y = in_cloud->points.at(nearest_idx).y;
  1359. // pt.z = in_cloud->points.at(nearest_idx).z;
  1360. // pt_idx = nearest_idx;
  1361. //}
  1362. void CRootStockGrabPoint::get_optimal_seed(
  1363. pcl::PointCloud<pcl::PointXYZ>::Ptr in_cloud,
  1364. pcl::PointXYZ&pt,
  1365. int &pt_idx)
  1366. {
  1367. /*double d1 = m_cparam.rs_grab_stem_diameter;
  1368. double d2 = m_cparam.rs_grab_stem_diameter * 4.0;
  1369. if (m_dtype != 0) {
  1370. d1 = m_cparam.sc_grab_stem_diameter;
  1371. d2 = m_cparam.sc_grab_stem_diameter * 4.0;
  1372. }
  1373. std::vector<pcl::PointXYZ>cluster_center;
  1374. std::vector<std::vector<int>> cluster_member;
  1375. euclidean_clustering_ttsas(in_cloud, d1, d2, cluster_center, cluster_member);*/
  1376. float min_y_dist = 1.0e6;
  1377. int opt_idx = -1;
  1378. int member_size = 5;
  1379. float y_opt = m_cparam.rs_grab_y_opt;
  1380. if (m_dtype == 0) {
  1381. y_opt = m_cparam.sc_grab_y_opt;
  1382. }
  1383. for (int i = 0; i < in_cloud->points.size(); ++i) {
  1384. /*if (cluster_member.at(i).size() < member_size) {
  1385. continue;
  1386. }*/
  1387. if (fabs(in_cloud->points.at(i).y - y_opt) < min_y_dist) {
  1388. min_y_dist = fabs(in_cloud->points.at(i).y - y_opt);
  1389. opt_idx = i;
  1390. }
  1391. }
  1392. if (opt_idx < 0) {
  1393. if (m_pLogger) {
  1394. stringstream buff;
  1395. buff << "get_optimal_seed failed, get invalid optimal cluster id";
  1396. m_pLogger->ERRORINFO(buff.str());
  1397. }
  1398. return;
  1399. }
  1400. //find nearest pt
  1401. /*float nearest_dist = 1.0e6;
  1402. int nearest_idx = -1;
  1403. for (int i = 0; i < cluster_member.at(opt_idx).size(); ++i) {
  1404. int idx = cluster_member.at(opt_idx).at(i);
  1405. float dist = fabs(in_cloud->points.at(idx).x - cluster_center.at(opt_idx).x) +
  1406. fabs(in_cloud->points.at(idx).y - cluster_center.at(opt_idx).y) +
  1407. fabs(in_cloud->points.at(idx).z - cluster_center.at(opt_idx).z);
  1408. if (dist < nearest_dist) {
  1409. nearest_dist = dist;
  1410. nearest_idx = idx;
  1411. }
  1412. }*/
  1413. pt.x = in_cloud->points.at(opt_idx).x;
  1414. pt.y = in_cloud->points.at(opt_idx).y;
  1415. pt.z = in_cloud->points.at(opt_idx).z;
  1416. pt_idx = opt_idx;
  1417. }
  1418. //通过比较直线点云和原始点云相同位置邻域内xz的范围,确定此点是否是无干扰点的茎
  1419. void CRootStockGrabPoint::get_optimal_seed_compare(
  1420. pcl::PointCloud<pcl::PointXYZ>::Ptr in_cloud, // 全部有效点云
  1421. pcl::PointCloud<pcl::PointXYZ>::Ptr in_line_cloud, // 茎上直线点云
  1422. pcl::PointXYZ&pt, // 返回抓取的点坐标
  1423. int &pt_idx, // 返回点index
  1424. std::vector<int>& valid_line_index // 返回有效直线点index
  1425. )
  1426. {
  1427. valid_line_index.clear();
  1428. float th = 0.75; //原始点云和直线点云邻域尺寸增加原来的20%,不能当做抓取点
  1429. pt_idx = -1;
  1430. pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_inbox(new pcl::PointCloud<pcl::PointXYZ>);
  1431. pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_inbox_line(new pcl::PointCloud<pcl::PointXYZ>);
  1432. pcl::CropBox<pcl::PointXYZ> box_filter;
  1433. box_filter.setNegative(false);
  1434. box_filter.setInputCloud(in_cloud);
  1435. pcl::CropBox<pcl::PointXYZ> box_filter_line;
  1436. box_filter_line.setNegative(false);
  1437. box_filter_line.setInputCloud(in_line_cloud);
  1438. float radius = m_cparam.rs_grab_stem_diameter;
  1439. float opt_y = m_cparam.rs_grab_y_opt;
  1440. if (m_dtype == 0) {
  1441. radius = m_cparam.sc_grab_stem_diameter;
  1442. opt_y = m_cparam.sc_grab_y_opt;
  1443. }
  1444. float rx = 1.5;
  1445. float ry = 1.5;
  1446. float rz = 1.5;
  1447. float cx, cy, cz;
  1448. float dz,dx, dz_line, dx_line;
  1449. float dist_min = 10000.0;
  1450. for (size_t i = 0; i < in_line_cloud->points.size(); ++i) {
  1451. cx = in_line_cloud->points.at(i).x;
  1452. cy = in_line_cloud->points.at(i).y;
  1453. cz = in_line_cloud->points.at(i).z;
  1454. //////////////////////////////////////////////////////////////////
  1455. //原始点云,获取指定区域的dx,dz
  1456. box_filter.setMin(Eigen::Vector4f(cx - rx*radius, cy - ry*radius, cz - rz*radius, 1));
  1457. box_filter.setMax(Eigen::Vector4f(cx + rx*radius, cy + ry*radius, cz + rz*radius, 1));
  1458. box_filter.filter(*cloud_inbox);
  1459. Eigen::Vector4f min_point;
  1460. Eigen::Vector4f max_point;
  1461. pcl::getMinMax3D(*cloud_inbox, min_point, max_point);
  1462. dz = max_point(2) - min_point(2);
  1463. dx = max_point(0) - min_point(0);
  1464. /////////////////////////////////////////////////////////////////////
  1465. //直线点云,获取指定区域的dx_line,dz_line
  1466. box_filter_line.setMin(Eigen::Vector4f(cx - rx*radius, cy - ry*radius, cz - rz*radius, 1));
  1467. box_filter_line.setMax(Eigen::Vector4f(cx + rx*radius, cy + ry*radius, cz + rz*radius, 1));
  1468. box_filter_line.filter(*cloud_inbox_line);
  1469. pcl::getMinMax3D(*cloud_inbox_line, min_point, max_point);
  1470. dz_line = max_point(2) - min_point(2);
  1471. dx_line = max_point(0) - min_point(0);
  1472. float ratio_ex = (dx + dz - dz_line - dx_line) / (dz_line + dx_line);
  1473. if (ratio_ex < th) {
  1474. valid_line_index.push_back(i);
  1475. if (fabs(cy - opt_y) < dist_min) {
  1476. dist_min = fabs(cy - opt_y);
  1477. pt_idx = i;
  1478. }
  1479. }
  1480. }
  1481. if (pt_idx >= 0) {
  1482. pt = in_line_cloud->points.at(pt_idx);
  1483. }
  1484. }
  1485. void CRootStockGrabPoint::viewer_cloud(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, std::string&winname)
  1486. {
  1487. pcl::visualization::CloudViewer viewer(winname);
  1488. //viewer.runOnVisualizationThreadOnce(viewerOneOff);
  1489. viewer.showCloud(cloud);
  1490. while (!viewer.wasStopped()) {
  1491. boost::this_thread::sleep(boost::posix_time::microseconds(100000));
  1492. }
  1493. }
  1494. void CRootStockGrabPoint::viewer_cloud(pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud, std::string&winname)
  1495. {
  1496. pcl::visualization::CloudViewer viewer(winname);
  1497. //viewer.runOnVisualizationThreadOnce(viewerOneOff);
  1498. viewer.showCloud(cloud);
  1499. while (!viewer.wasStopped()) {
  1500. boost::this_thread::sleep(boost::posix_time::microseconds(100000));
  1501. }
  1502. }
  1503. void CRootStockGrabPoint::viewer_cloud_debug(
  1504. pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud,
  1505. pcl::PointXYZ &p,
  1506. pcl::PointXYZ &root_pt,
  1507. std::string&winname)
  1508. {
  1509. pcl::visualization::PCLVisualizer viewer(winname);
  1510. //viewer.runOnVisualizationThreadOnce(viewerOneOff);
  1511. viewer.addPointCloud(cloud);
  1512. viewer.addCoordinateSystem();
  1513. pcl::PointXYZ p0, x1, y1,p00,x0,y0, root_px0, root_px1, root_py0,root_py1;
  1514. p0.x = p.x;
  1515. p0.y = p.y;
  1516. p0.z = p.z;
  1517. x1.x = p.x + 100.0;
  1518. x1.y = p.y;
  1519. x1.z = p.z;
  1520. y1.x = p.x;
  1521. y1.y = p.y + 20.0;
  1522. y1.z = p.z;
  1523. p00.x = 0.0;
  1524. p00.y = 0.0;
  1525. p00.z = p.z;
  1526. x0.x = 600.0;
  1527. x0.y = 0;
  1528. x0.z = p.z;
  1529. y0.x = 0.0;
  1530. y0.y = 300.0;
  1531. y0.z = p.z;
  1532. root_px0.x = root_pt.x - 5.0;
  1533. root_px0.y = root_pt.y;
  1534. root_px0.z = root_pt.z;
  1535. root_px1.x = root_pt.x + 5.0;
  1536. root_px1.y = root_pt.y;
  1537. root_px1.z = root_pt.z;
  1538. root_py0.x = root_pt.x;
  1539. root_py0.y = root_pt.y - 5.0;
  1540. root_py0.z = root_pt.z;
  1541. root_py1.x = root_pt.x;
  1542. root_py1.y = root_pt.y + 5.0;
  1543. root_py1.z = root_pt.z;
  1544. viewer.addLine(p0, x1, 255, 0, 0, "x");
  1545. viewer.addLine(p0, y1, 0, 255, 0, "y");
  1546. viewer.addLine(p00, x0, 255, 0, 0, "x0");
  1547. viewer.addLine(p00, y0, 0, 255, 0, "y0");
  1548. viewer.addLine(root_px0, root_px1, 255, 0, 0, "rootx");
  1549. viewer.addLine(root_py0, root_py1, 0, 255, 0, "rooty");
  1550. while (!viewer.wasStopped()) {
  1551. viewer.spinOnce(100);
  1552. boost::this_thread::sleep(boost::posix_time::microseconds(100000));
  1553. }
  1554. }
  1555. void CRootStockGrabPoint::viewer_cloud_cluster(
  1556. pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud,
  1557. std::vector<pcl::PointXYZ>cluster_center,
  1558. std::string&winname)
  1559. {
  1560. pcl::visualization::PCLVisualizer viewer(winname);
  1561. viewer.addPointCloud(cloud);
  1562. viewer.addCoordinateSystem();
  1563. int cnt = 0;
  1564. char buf[8];
  1565. for (auto& p : cluster_center) {
  1566. pcl::PointXYZ px0, px1, py1, py0;
  1567. px0.x = p.x-5.0;
  1568. px0.y = p.y;
  1569. px0.z = p.z;
  1570. px1.x = p.x + 5.0;
  1571. px1.y = p.y;
  1572. px1.z = p.z;
  1573. py0.x = p.x;
  1574. py0.y = p.y - 5.0;
  1575. py0.z = p.z;
  1576. py1.x = p.x;
  1577. py1.y = p.y + 5.0;
  1578. py1.z = p.z;
  1579. viewer.addLine(px0, px1, 255, 0, 0, "x" + string(_itoa(cnt, buf,10)));
  1580. viewer.addLine(py0, py1, 0, 255, 0, "y" + string(_itoa(cnt, buf, 10)));
  1581. cnt += 1;
  1582. }
  1583. while (!viewer.wasStopped()) {
  1584. viewer.spinOnce(100);
  1585. boost::this_thread::sleep(boost::posix_time::microseconds(100000));
  1586. }
  1587. }
  1588. void CRootStockGrabPoint::viewer_cloud_cluster_box(
  1589. pcl::PointCloud<pcl::PointXYZ>::Ptr cloud,
  1590. std::vector<pcl::PointXYZ>&cluster_center,
  1591. std::vector<pcl::PointXYZ>&cluster_box,
  1592. std::vector<std::vector<int> >& clt_line_idx,
  1593. std::string&winname)
  1594. {
  1595. pcl::visualization::PCLVisualizer viewer(winname);
  1596. viewer.addCoordinateSystem();
  1597. viewer.addPointCloud<pcl::PointXYZ>(cloud, "all_cloud");
  1598. viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 1, 1, 1, "all_cloud");
  1599. viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "all_cloud");
  1600. int cnt = 0;
  1601. char buf[8];
  1602. for (size_t i = 0; i < cluster_center.size();++i) {
  1603. pcl::PointXYZ &p = cluster_center.at(i);
  1604. pcl::PointXYZ px0, px1, py1, py0;
  1605. px0.x = p.x - 5.0;
  1606. px0.y = p.y;
  1607. px0.z = p.z;
  1608. px1.x = p.x + 5.0;
  1609. px1.y = p.y;
  1610. px1.z = p.z;
  1611. py0.x = p.x;
  1612. py0.y = p.y - 5.0;
  1613. py0.z = p.z;
  1614. py1.x = p.x;
  1615. py1.y = p.y + 5.0;
  1616. py1.z = p.z;
  1617. viewer.addLine(px0, px1, 255, 0, 0, "x" + string(_itoa(cnt, buf, 10)));
  1618. viewer.addLine(py0, py1, 0, 255, 0, "y" + string(_itoa(cnt, buf, 10)));
  1619. //box
  1620. pcl::PointXYZ & min_pt = cluster_box.at(2 * i);
  1621. pcl::PointXYZ & max_pt = cluster_box.at(2 * i + 1);
  1622. viewer.addCube(min_pt.x, max_pt.x, min_pt.y, max_pt.y, min_pt.z, max_pt.z, 0.5,0.5,0.0,"AABB_"+string(_itoa(cnt, buf, 10)));
  1623. viewer.setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_REPRESENTATION,
  1624. pcl::visualization::PCL_VISUALIZER_REPRESENTATION_WIREFRAME, "AABB_" + string(_itoa(cnt, buf, 10)));
  1625. pcl::PointCloud<pcl::PointXYZ>::Ptr line_cloud(new pcl::PointCloud<pcl::PointXYZ>);
  1626. //pcl::copyPointCloud(*cloud, clt_line_idx.at(i), *line_cloud);
  1627. viewer.addPointCloud(line_cloud, "fit_line" + string(_itoa(cnt, buf, 10)));
  1628. viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 0, 1, 0, "fit_line" + string(_itoa(cnt, buf, 10)));
  1629. viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "fit_line" + string(_itoa(cnt, buf, 10)));
  1630. cnt += 1;
  1631. }
  1632. while (!viewer.wasStopped()) {
  1633. viewer.spinOnce(100);
  1634. boost::this_thread::sleep(boost::posix_time::microseconds(100000));
  1635. }
  1636. }
  1637. };