|
|
@@ -33,7 +33,9 @@ namespace graft_cv {
|
|
|
m_pLogger(pLog),
|
|
|
m_dtype(0),
|
|
|
m_pcdId(""),
|
|
|
- m_ppImgSaver(0)
|
|
|
+ m_ppImgSaver(0),
|
|
|
+ m_1st_row_zmean_rs(-1.0),
|
|
|
+ m_1st_row_zmean_sc(-1.0)
|
|
|
{
|
|
|
}
|
|
|
|
|
|
@@ -219,9 +221,8 @@ namespace graft_cv {
|
|
|
pcl::PointXYZ xz_center; //存储找到的第一个位置上植株根部坐标
|
|
|
if (m_pLogger) {
|
|
|
m_pLogger->INFO("begin find seedling position");
|
|
|
- }
|
|
|
- //find_seedling_position_line_based(cloud_ror, first_seedling_cloud_idx, xz_center);
|
|
|
- find_seedling_position(cloud_dowm_sampled, first_seedling_cloud_idx, xz_center);
|
|
|
+ }
|
|
|
+ find_seedling_position_key(cloud_dowm_sampled, first_seedling_cloud_idx, xz_center);
|
|
|
if (m_pLogger) {
|
|
|
stringstream buff;
|
|
|
buff << "after find_seedling_position(), foud first seedling seeds points size " << first_seedling_cloud_idx.size();
|
|
|
@@ -450,16 +451,518 @@ namespace graft_cv {
|
|
|
|
|
|
//}
|
|
|
////////////////////////////////////////////////////////////
|
|
|
- void CRootStockGrabPoint::find_seedling_position(
|
|
|
+void CRootStockGrabPoint::gen_all_seedling_positions(
|
|
|
+ pcl::PointXYZ&key_center, //输入，已知的苗的坐标
|
|
|
+ std::vector<pcl::PointXYZ>&candidate_center //输出，有倾斜苗的坐标
|
|
|
+)
|
|
|
+{
|
|
|
+ //生成所有的植株位置，并排序
|
|
|
+ candidate_center.clear();
|
|
|
+
|
|
|
+ //找到z最小的那一株，并找出和它一排的那些
|
|
|
+ float step_harf = m_cparam.rs_grab_seedling_dist / 2.0;
|
|
|
+ float xmin = m_cparam.rs_grab_xmin;
|
|
|
+ float xmax = m_cparam.rs_grab_xmax;
|
|
|
+ float zmin = m_cparam.rs_grab_zmin;
|
|
|
+ float zmax = m_cparam.rs_grab_zmax;
|
|
|
+ int col_from = -8;
|
|
|
+ int col_to = 8;
|
|
|
+ int col_step = 1;
|
|
|
+
|
|
|
+ if (m_dtype == 0) {
|
|
|
+ step_harf = m_cparam.sc_grab_seedling_dist / 2.0;
|
|
|
+ xmin = m_cparam.sc_grab_xmin;
|
|
|
+ xmax = m_cparam.sc_grab_xmax;
|
|
|
+ zmin = m_cparam.sc_grab_zmin;
|
|
|
+ zmax = m_cparam.sc_grab_zmax;
|
|
|
+ }
|
|
|
+
|
|
|
+ // 生成所有可能的植株位置中心点
|
|
|
+ for (int row = -8; row <= 4; row += 1) {
|
|
|
+ float cz = key_center.z + row * step_harf/2.0;
|
|
|
+ if (cz < zmin || cz > zmax) { continue; }
|
|
|
+ for (int col = 4*col_from; col <= 4*col_to; col += col_step) {
|
|
|
+ float cx = key_center.x + col * step_harf/2;
|
|
|
+ if (cx < xmin || cx > xmax) { continue; }
|
|
|
+ //保存
|
|
|
+ pcl::PointXYZ pc(cx, 0.0, cz);
|
|
|
+ candidate_center.push_back(pc);
|
|
|
+ }
|
|
|
+ }
|
|
|
+}
|
|
|
+
|
|
|
+void CRootStockGrabPoint::nms_box(
|
|
|
+ std::vector<pcl::PointXYZ>&clt_root_v,
|
|
|
+ std::vector<float>&valid_box_cc_dist,
|
|
|
+ float hole_step_radius,
|
|
|
+ std::vector<pcl::PointXYZ>& target_toot)
|
|
|
+{
|
|
|
+ target_toot.clear();
|
|
|
+ pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
|
|
|
+ for (auto&p : clt_root_v) { cloud->points.push_back(p); }
|
|
|
+
|
|
|
+ pcl::KdTreeFLANN<pcl::PointXYZ> kdtree;
|
|
|
+ kdtree.setInputCloud(cloud);
|
|
|
+ std::vector<int>idx;
|
|
|
+ std::vector<float>dist_sqr;
|
|
|
+ std::vector<int> tar_idx;
|
|
|
+ for (size_t i = 0; i < cloud->points.size(); ++i) {
|
|
|
+ int k = kdtree.radiusSearch(cloud->points.at(i), hole_step_radius, idx, dist_sqr);
|
|
|
+ std::vector<float> cc_tmp;
|
|
|
+ for (auto& j : idx) { cc_tmp.push_back(valid_box_cc_dist.at(j)); }
|
|
|
+ int min_id = std::min_element(cc_tmp.begin(), cc_tmp.end()) - cc_tmp.begin();
|
|
|
+ if (min_id == 0) {
|
|
|
+ tar_idx.push_back(i);
|
|
|
+ }
|
|
|
+ }
|
|
|
+ for (auto& i : tar_idx) {
|
|
|
+ target_toot.push_back(clt_root_v.at(i));
|
|
|
+ }
|
|
|
+
|
|
|
+}
|
|
|
+//通过指定位置内,取部分点云分析是否存在真正的茎，真茎位置保存到target_filtered
|
|
|
+void CRootStockGrabPoint::line_filter(
|
|
|
+ pcl::PointCloud<pcl::PointXYZ>::Ptr in_cloud, //输入点云
|
|
|
+ float radius, //输入，取点半径
|
|
|
+ float ymin,
|
|
|
+ float ymax,
|
|
|
+ std::vector<pcl::PointXYZ>&target_root, //输入，位置
|
|
|
+ std::vector<pcl::PointXYZ>&target_filtered, //输出，位置
|
|
|
+ std::vector<pcl::PointXYZ>&target_filtered_root, //输出，茎上根部点坐标
|
|
|
+ std::vector<std::vector<int>>&target_filtered_element //输出，茎上点index
|
|
|
+
|
|
|
+
|
|
|
+)
|
|
|
+{
|
|
|
+ target_filtered.clear();
|
|
|
+ target_filtered_element.clear();
|
|
|
+ target_filtered_root.clear();
|
|
|
+ if (target_root.size() == 0) { return; }
|
|
|
+
|
|
|
+ for (auto&p : target_root) {
|
|
|
+ // 构建box，获取植株点云
|
|
|
+ pcl::PointCloud<pcl::PointXYZ>::Ptr seedling_inbox(new pcl::PointCloud<pcl::PointXYZ>);
|
|
|
+ pcl::CropBox<pcl::PointXYZ> box_filter;
|
|
|
+ std::vector<int>inbox_idx;
|
|
|
+ pcl::PointXYZ min_point_aabb_ex;
|
|
|
+ pcl::PointXYZ max_point_aabb_ex;
|
|
|
+
|
|
|
+ min_point_aabb_ex.x = p.x - radius;
|
|
|
+ min_point_aabb_ex.y = ymin;
|
|
|
+ min_point_aabb_ex.z = p.z - radius;
|
|
|
+ max_point_aabb_ex.x = p.x + radius;
|
|
|
+ max_point_aabb_ex.y = ymax;
|
|
|
+ max_point_aabb_ex.z = p.z + radius;
|
|
|
+
|
|
|
+ box_filter.setMin(Eigen::Vector4f(min_point_aabb_ex.x, min_point_aabb_ex.y, min_point_aabb_ex.z, 1));
|
|
|
+ box_filter.setMax(Eigen::Vector4f(max_point_aabb_ex.x, max_point_aabb_ex.y, max_point_aabb_ex.z, 1));
|
|
|
+
|
|
|
+ box_filter.setNegative(false);
|
|
|
+ box_filter.setInputCloud(in_cloud);
|
|
|
+ box_filter.filter(inbox_idx);
|
|
|
+ pcl::copyPointCloud(*in_cloud, inbox_idx, *seedling_inbox);
|
|
|
+
|
|
|
+ //植株点云直线查找
|
|
|
+ //找到inbox点云中的直线
|
|
|
+ float stem_radius = m_cparam.rs_grab_stem_diameter / 2.0;
|
|
|
+ if (m_dtype == 0) {
|
|
|
+ stem_radius = m_cparam.sc_grab_stem_diameter / 2.0;
|
|
|
+ }
|
|
|
+ pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
|
|
|
+ pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
|
|
|
+ pcl::SACSegmentation<pcl::PointXYZ> seg;
|
|
|
+ seg.setOptimizeCoefficients(true);
|
|
|
+ seg.setModelType(pcl::SACMODEL_LINE);
|
|
|
+ seg.setDistanceThreshold(stem_radius);
|
|
|
+ seg.setInputCloud(seedling_inbox);
|
|
|
+ seg.segment(*inliers, *coefficients);
|
|
|
+
|
|
|
+
|
|
|
+ pcl::PointCloud<pcl::PointXYZ>::Ptr stem_cloud(new pcl::PointCloud<pcl::PointXYZ>);
|
|
|
+ pcl::copyPointCloud(*seedling_inbox, *inliers, *stem_cloud);
|
|
|
+
|
|
|
+ //点数过滤
|
|
|
+ int min_stem_pts = m_cparam.rs_grab_stem_min_pts;
|
|
|
+ if (m_dtype == 0) { min_stem_pts = m_cparam.sc_grab_stem_min_pts; }
|
|
|
+ if (inliers->indices.size() < int(min_stem_pts / 2)) { continue; }
|
|
|
+ //y方向分布范围滤波
|
|
|
+ pcl::PointXYZ min_v;
|
|
|
+ pcl::PointXYZ max_v;
|
|
|
+ pcl::getMinMax3D(*stem_cloud, min_v, max_v);
|
|
|
+ float dy = max_v.y - min_v.y;
|
|
|
+ if (dy / (ymax - ymin) < 0.5) { continue; }
|
|
|
+ //y方向分布中心滤波
|
|
|
+ float dy_c = 0.5*(max_v.y + min_v.y);
|
|
|
+ if ((dy_c - ymin) / (ymax - ymin) > 0.75) { continue; }
|
|
|
+ //倾斜角度过滤
|
|
|
+ float xz = std::sqrt(coefficients->values.at(3) * coefficients->values.at(3) +
|
|
|
+ coefficients->values.at(5) * coefficients->values.at(5));
|
|
|
+ float a = std::atan2f(coefficients->values.at(4), xz);
|
|
|
+ a = std::fabs(a) * 180.0 / 3.14159;
|
|
|
+ if (a < 45.0) { continue; }
|
|
|
+ target_filtered.push_back(p);
|
|
|
+
|
|
|
+ float min_y = 10000.0;
|
|
|
+ int min_y_idx = -1;
|
|
|
+ for (size_t j = 0; j < stem_cloud->points.size();++j) {
|
|
|
+ pcl::PointXYZ &spt = stem_cloud->at(j);
|
|
|
+ if (spt.y < min_y) {
|
|
|
+ min_y = spt.y;
|
|
|
+ min_y_idx = j;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ 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);
|
|
|
+ target_filtered_root.push_back(tr_pt);
|
|
|
+
|
|
|
+ int idx_raw = 0;
|
|
|
+ std::vector<int> out_idx;
|
|
|
+ for (auto& idx : inliers->indices) {
|
|
|
+ idx_raw = inbox_idx.at(idx);
|
|
|
+ out_idx.push_back(idx_raw);
|
|
|
+ }
|
|
|
+ target_filtered_element.push_back(out_idx);
|
|
|
+
|
|
|
+ // debug show
|
|
|
+ /*if (m_cparam.image_show) {
|
|
|
+ pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_line(new pcl::PointCloud<pcl::PointXYZ>);
|
|
|
+ pcl::copyPointCloud(*seedling_inbox, *inliers, *cloud_line);
|
|
|
+
|
|
|
+ pcl::visualization::PCLVisualizer::Ptr viewer(new pcl::visualization::PCLVisualizer("line in cluster"));
|
|
|
+ viewer->addPointCloud<pcl::PointXYZ>(seedling_inbox, "cluster");
|
|
|
+ viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 1, 1, 1, "cluster");
|
|
|
+ viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "cluster");
|
|
|
+
|
|
|
+ viewer->addPointCloud(cloud_line, "fit_line");
|
|
|
+ viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 0, 1, 0, "fit_line");
|
|
|
+ viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "fit_line");
|
|
|
+ while (!viewer->wasStopped()) {
|
|
|
+ viewer->spinOnce(100);
|
|
|
+ }
|
|
|
+ }*/
|
|
|
+ }
|
|
|
+}
|
|
|
+
|
|
|
+
|
|
|
+ void CRootStockGrabPoint::find_seedling_position_key(
|
|
|
pcl::PointCloud<pcl::PointXYZ>::Ptr in_cloud,
|
|
|
std::vector<int> &first_seedling_cloud_idx,
|
|
|
pcl::PointXYZ&xz_center
|
|
|
)
|
|
|
{
|
|
|
+ // 确定植株inbox范围
|
|
|
+ float hole_step = m_cparam.rs_grab_seedling_dist - 5.0; //穴盘中穴间距
|
|
|
+ if (m_dtype == 0) {
|
|
|
+ hole_step = m_cparam.sc_grab_seedling_dist - 5.0;
|
|
|
+ }
|
|
|
+ float hole_step_radius = hole_step / 2.0;
|
|
|
+
|
|
|
+ // 点云降维到xz平面，y=0
|
|
|
+ pcl::PointCloud<pcl::PointXYZ>::Ptr cloud2d(new pcl::PointCloud < pcl::PointXYZ>);
|
|
|
+ pcl::copyPointCloud(*in_cloud, *cloud2d);
|
|
|
+ for (auto&pt : *cloud2d) {
|
|
|
+ pt.y = 0.0;
|
|
|
+ }
|
|
|
+
|
|
|
+ // 在xz平面内统计点的密度
|
|
|
+ double radius = m_cparam.rs_grab_stem_diameter;
|
|
|
+ if (m_dtype == 0) {
|
|
|
+ radius = m_cparam.sc_grab_stem_diameter;
|
|
|
+ }
|
|
|
+ std::vector<int> counter;
|
|
|
+ pcl::KdTreeFLANN<pcl::PointXYZ> kdtree;
|
|
|
+ kdtree.setInputCloud(cloud2d);
|
|
|
+ std::vector<int>idx;
|
|
|
+ std::vector<float>dist_sqr;
|
|
|
+ for (size_t i = 0; i < cloud2d->points.size(); ++i) {
|
|
|
+ int k = kdtree.radiusSearch(cloud2d->points.at(i), radius, idx, dist_sqr);
|
|
|
+ counter.push_back(k);
|
|
|
+ }
|
|
|
+ int max_density_idx = std::max_element(counter.begin(), counter.end()) - counter.begin();
|
|
|
+ int max_density = counter.at(max_density_idx);
|
|
|
+
|
|
|
+ if (m_cparam.image_show) {
|
|
|
+ //显示密度最大的点的位置
|
|
|
+ pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_tmp(new pcl::PointCloud<pcl::PointXYZRGB>);
|
|
|
+ pcl::copyPointCloud(*in_cloud, *cloud_tmp);
|
|
|
+ for (auto& pt : *cloud_tmp) {
|
|
|
+ pt.r = 255;
|
|
|
+ pt.g = 255;
|
|
|
+ pt.b = 255;
|
|
|
+ }
|
|
|
+ std::vector<pcl::PointXYZ>cc;
|
|
|
+ cc.push_back(in_cloud->points.at(max_density_idx));
|
|
|
+ viewer_cloud_cluster(cloud_tmp, cc, string("key"));
|
|
|
+ }
|
|
|
+ // 生成植株的中心及box
|
|
|
+ std::vector<pcl::PointXYZ>clt_root;
|
|
|
+ std::vector<pcl::PointXYZ> clt_box;
|
|
|
+ float ymin = m_cparam.rs_grab_ymin;
|
|
|
+ float ymax = m_cparam.rs_grab_ymax;
|
|
|
+ if (m_dtype == 0) {
|
|
|
+ ymin = m_cparam.sc_grab_ymin;
|
|
|
+ ymax = m_cparam.sc_grab_ymax;
|
|
|
+ }
|
|
|
+ gen_all_seedling_positions(in_cloud->points.at(max_density_idx), clt_root);
|
|
|
+
|
|
|
+ // 每个位置点云情况，过滤
|
|
|
+ std::vector<int> valid_index; //初步有效的矩形index
|
|
|
+ std::vector<int> valid_box_pts; //立方体内点云数量
|
|
|
+ std::vector<float>valid_box_cc_dist;//重心和矩形中心距离
|
|
|
+ for (size_t i = 0; i < clt_root.size();++i) {
|
|
|
+ pcl::PointXYZ &p = clt_root.at(i);
|
|
|
+ pcl::PointCloud<pcl::PointXYZ>::Ptr seedling_inbox(new pcl::PointCloud<pcl::PointXYZ>);
|
|
|
+ pcl::CropBox<pcl::PointXYZ> box_filter;
|
|
|
+ std::vector<int>inbox_idx;
|
|
|
+ pcl::PointXYZ min_point_aabb_ex;
|
|
|
+ pcl::PointXYZ max_point_aabb_ex;
|
|
|
+
|
|
|
+ min_point_aabb_ex.x = p.x - hole_step_radius;
|
|
|
+ min_point_aabb_ex.y = ymin;
|
|
|
+ min_point_aabb_ex.z = p.z - hole_step_radius;
|
|
|
+ max_point_aabb_ex.x = p.x + hole_step_radius;
|
|
|
+ max_point_aabb_ex.y = ymax;
|
|
|
+ max_point_aabb_ex.z = p.z + hole_step_radius;
|
|
|
+
|
|
|
+ box_filter.setMin(Eigen::Vector4f(min_point_aabb_ex.x, min_point_aabb_ex.y, min_point_aabb_ex.z, 1));
|
|
|
+ box_filter.setMax(Eigen::Vector4f(max_point_aabb_ex.x, max_point_aabb_ex.y, max_point_aabb_ex.z, 1));
|
|
|
+
|
|
|
+ box_filter.setNegative(false);
|
|
|
+ box_filter.setInputCloud(in_cloud);
|
|
|
+ box_filter.filter(inbox_idx);
|
|
|
+ pcl::copyPointCloud(*in_cloud, inbox_idx, *seedling_inbox);
|
|
|
+ //点数过滤
|
|
|
+ if (inbox_idx.size() < int(max_density/2)) { continue; }
|
|
|
+ //y方向分布范围滤波
|
|
|
+ pcl::PointXYZ min_v;
|
|
|
+ pcl::PointXYZ max_v;
|
|
|
+ pcl::getMinMax3D(*seedling_inbox, min_v, max_v);
|
|
|
+ float dy = max_v.y - min_v.y;
|
|
|
+ if (dy / (ymax - ymin) < 0.5) { continue; }
|
|
|
+ //y方向分布中心滤波
|
|
|
+ float dy_c = 0.5*(max_v.y + min_v.y);
|
|
|
+ if ((dy_c-ymin) / (ymax - ymin) > 0.75) { continue; }
|
|
|
+ //计算中心
|
|
|
+ Eigen::Vector4f mean;
|
|
|
+ pcl::compute3DCentroid(*seedling_inbox, mean);
|
|
|
+ double cc_dist = std::sqrt((mean[0] - p.x)*(mean[0] - p.x) + (mean[2] - p.z)*(mean[2] - p.z));
|
|
|
+ valid_index.push_back(i);
|
|
|
+ valid_box_pts.push_back(inbox_idx.size());
|
|
|
+ valid_box_cc_dist.push_back((float)cc_dist);
|
|
|
+ }
|
|
|
+ // 找到局部最大值，找出真正的位置
|
|
|
+ std::vector<pcl::PointXYZ> clt_root_v;
|
|
|
+ std::vector<pcl::PointXYZ> clt_box_v;
|
|
|
+ for (auto&i : valid_index) {
|
|
|
+ pcl::PointXYZ&p = clt_root.at(i);
|
|
|
+ pcl::PointXYZ p_show(p.x, ymin, p.z);
|
|
|
+ clt_root_v.push_back(p_show);
|
|
|
+ pcl::PointXYZ min_point_aabb_ex;
|
|
|
+ pcl::PointXYZ max_point_aabb_ex;
|
|
|
+ min_point_aabb_ex.x = p.x - hole_step_radius;
|
|
|
+ min_point_aabb_ex.y = ymin;
|
|
|
+ min_point_aabb_ex.z = p.z - hole_step_radius;
|
|
|
+ max_point_aabb_ex.x = p.x + hole_step_radius;
|
|
|
+ max_point_aabb_ex.y = ymax;
|
|
|
+ max_point_aabb_ex.z = p.z + hole_step_radius;
|
|
|
+ clt_box_v.push_back(min_point_aabb_ex);
|
|
|
+ clt_box_v.push_back(max_point_aabb_ex);
|
|
|
+ }
|
|
|
+ // 显示
|
|
|
+ if (m_cparam.image_show) {
|
|
|
+ std::vector<std::vector<int> > clt_line_idx;
|
|
|
+ viewer_cloud_cluster_box(in_cloud, clt_root_v, clt_box_v, clt_line_idx, std::string("valid_box"));
|
|
|
+ }
|
|
|
+ std::vector<pcl::PointXYZ> target_root;
|
|
|
+ nms_box(clt_root_v, valid_box_cc_dist, hole_step_radius, target_root);
|
|
|
+
|
|
|
+ // 显示
|
|
|
+ if (m_cparam.image_show) {
|
|
|
+ std::vector<std::vector<int> > clt_line_idx;
|
|
|
+ std::vector<pcl::PointXYZ>clt_box_tmp;
|
|
|
+ for (auto&p : target_root) {
|
|
|
+ pcl::PointXYZ p_show(p.x, ymin, p.z);
|
|
|
+ clt_root_v.push_back(p_show);
|
|
|
+ pcl::PointXYZ min_point_aabb_ex;
|
|
|
+ pcl::PointXYZ max_point_aabb_ex;
|
|
|
+ min_point_aabb_ex.x = p.x - hole_step_radius;
|
|
|
+ min_point_aabb_ex.y = ymin;
|
|
|
+ min_point_aabb_ex.z = p.z - hole_step_radius;
|
|
|
+ max_point_aabb_ex.x = p.x + hole_step_radius;
|
|
|
+ max_point_aabb_ex.y = ymax;
|
|
|
+ max_point_aabb_ex.z = p.z + hole_step_radius;
|
|
|
+ clt_box_tmp.push_back(min_point_aabb_ex);
|
|
|
+ clt_box_tmp.push_back(max_point_aabb_ex);
|
|
|
+ }
|
|
|
+
|
|
|
+ viewer_cloud_cluster_box(in_cloud, target_root, clt_box_tmp, clt_line_idx, std::string("nms_box"));
|
|
|
+ }
|
|
|
+ // 根据得到的位置，直线检测，并过滤
|
|
|
+ std::vector<pcl::PointXYZ>target_filtered;
|
|
|
+ std::vector<std::vector<int>> target_member;
|
|
|
+ std::vector<pcl::PointXYZ>target_filtered_root;
|
|
|
+ line_filter(in_cloud, hole_step_radius,ymin,ymax, target_root, target_filtered, target_filtered_root, target_member);
|
|
|
+
|
|
|
+ if (m_cparam.image_show) {
|
|
|
+ std::vector<pcl::PointXYZ> final_box;
|
|
|
+ for (auto&pt : target_filtered_root) {
|
|
|
+ //扩展矩形范围
|
|
|
+ pcl::PointXYZ min_point_aabb_ex;
|
|
|
+ pcl::PointXYZ max_point_aabb_ex;
|
|
|
+ min_point_aabb_ex.x = pt.x - hole_step_radius;
|
|
|
+ min_point_aabb_ex.y = ymin;
|
|
|
+ min_point_aabb_ex.z = pt.z - hole_step_radius;
|
|
|
+
|
|
|
+ max_point_aabb_ex.x = pt.x + hole_step_radius;
|
|
|
+ max_point_aabb_ex.y = ymax;
|
|
|
+ max_point_aabb_ex.z = pt.z + hole_step_radius;
|
|
|
+ final_box.push_back(min_point_aabb_ex);
|
|
|
+ final_box.push_back(max_point_aabb_ex);
|
|
|
+ }
|
|
|
+ std::vector<std::vector<int> > clt_line_idx_;
|
|
|
+ viewer_cloud_cluster_box(in_cloud, target_filtered_root, final_box, clt_line_idx_, std::string("line_filter"));
|
|
|
+ }
|
|
|
+
|
|
|
+ //sort cluster center, get the frontest leftest one
|
|
|
+ //找最小z，然后计算平均z
|
|
|
+ float min_root_z = 10000.0;
|
|
|
+ for(auto&pt : target_filtered_root) {
|
|
|
+ if (pt.z < min_root_z) { min_root_z = pt.z; }
|
|
|
+ }
|
|
|
+ //通过最小z，保守的找到和他一排的植株
|
|
|
+ std::vector<int> first_row_index;
|
|
|
+ float mean_z = 0.0;
|
|
|
+ for (int idx_r = 0; idx_r < target_filtered_root.size();++idx_r) {
|
|
|
+ pcl::PointXYZ&pt = target_filtered_root.at(idx_r);
|
|
|
+ if (std::fabs(pt.z - min_root_z) < hole_step) {
|
|
|
+ first_row_index.push_back(idx_r);
|
|
|
+ mean_z += pt.z;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ //如果第一排的植株大于3，计算平均值
|
|
|
+ float first_row_num = (float)first_row_index.size();
|
|
|
+ mean_z /= first_row_num;
|
|
|
+ if (first_row_num >= 4) {
|
|
|
+ if (m_dtype == 0) {
|
|
|
+ m_1st_row_zmean_sc = mean_z;
|
|
|
+ }
|
|
|
+ else{ m_1st_row_zmean_rs = mean_z; }
|
|
|
+ }
|
|
|
+ else {
|
|
|
+ if( m_dtype == 0) {
|
|
|
+ if (m_1st_row_zmean_sc > 0) { mean_z = m_1st_row_zmean_sc; }
|
|
|
+ }
|
|
|
+ else {
|
|
|
+ if (m_1st_row_zmean_rs > 0) { mean_z = m_1st_row_zmean_rs; }
|
|
|
+ }
|
|
|
+ }
|
|
|
+ //通过mean_z再次寻找第一排的植株
|
|
|
+ first_row_index.clear();
|
|
|
+ for (int idx_r = 0; idx_r < target_filtered_root.size(); ++idx_r) {
|
|
|
+ pcl::PointXYZ&pt = target_filtered_root.at(idx_r);
|
|
|
+ if (std::fabs(pt.z - mean_z) < hole_step) {
|
|
|
+ first_row_index.push_back(idx_r);
|
|
|
+ }
|
|
|
+ }
|
|
|
+ std::vector<float> cluster_index;
|
|
|
+ for (int i=0; i<first_row_index.size();++i){
|
|
|
+ int raw_idx = first_row_index.at(i);
|
|
|
+ pcl::PointXYZ&pt = target_filtered_root.at(raw_idx);
|
|
|
+ cluster_index.push_back(pt.x);
|
|
|
+ }
|
|
|
+ int first_idx = std::min_element(cluster_index.begin(), cluster_index.end()) - cluster_index.begin();
|
|
|
+ if (m_dtype == 0) {
|
|
|
+ first_idx = std::max_element(cluster_index.begin(), cluster_index.end()) - cluster_index.begin();
|
|
|
+ }
|
|
|
+ first_idx = first_row_index.at(first_idx);
|
|
|
+
|
|
|
+ first_seedling_cloud_idx.clear();
|
|
|
+ for (auto&v : target_member.at(first_idx)) {
|
|
|
+ first_seedling_cloud_idx.push_back(v);
|
|
|
+ }
|
|
|
+ xz_center.x = target_filtered_root.at(first_idx).x;
|
|
|
+ xz_center.y = target_filtered_root.at(first_idx).y;
|
|
|
+ xz_center.z = target_filtered_root.at(first_idx).z;
|
|
|
+
|
|
|
+ if (m_pLogger) {
|
|
|
+ stringstream buff;
|
|
|
+ buff << "euclidean_clustering_ttsas， find cluster center(" << xz_center.x
|
|
|
+ <<", "<< xz_center.y<<", "<< xz_center.z<<")";
|
|
|
+ m_pLogger->INFO(buff.str());
|
|
|
+ }
|
|
|
+ }
|
|
|
+ void CRootStockGrabPoint::tilted_seedling_discover(
|
|
|
+ std::vector<pcl::PointXYZ>&cluster_center, //输入，已知的苗的坐标
|
|
|
+ std::vector<pcl::PointXYZ>&tilted_center //输出，有倾斜苗的坐标
|
|
|
+ )
|
|
|
+ {
|
|
|
+ tilted_center.clear();
|
|
|
+ if (cluster_center.size() == 0) { return; }
|
|
|
+ //找到z最小的那一株，并找出和它一排的那些
|
|
|
+ float step_harf = m_cparam.rs_grab_seedling_dist / 2.0;
|
|
|
+ float xmin = m_cparam.rs_grab_xmin;
|
|
|
+ float xmax = m_cparam.rs_grab_xmax;
|
|
|
+ float zmin = m_cparam.rs_grab_zmin;
|
|
|
+ float zmax = m_cparam.rs_grab_zmax;
|
|
|
+ if (m_dtype == 0) {
|
|
|
+ step_harf = m_cparam.sc_grab_seedling_dist / 2.0;
|
|
|
+ xmin = m_cparam.sc_grab_xmin;
|
|
|
+ xmax = m_cparam.sc_grab_xmax;
|
|
|
+ zmin = m_cparam.sc_grab_zmin;
|
|
|
+ zmax = m_cparam.sc_grab_zmax;
|
|
|
+ }
|
|
|
+ auto minz_it = std::min_element(cluster_center.begin(), cluster_center.end(),
|
|
|
+ [](const pcl::PointXYZ& p1, const pcl::PointXYZ& p2)-> bool {return p1.z < p2.z; });
|
|
|
+ float minz = minz_it->z;
|
|
|
+ float meanz = 0.0;
|
|
|
+ float cnt = 0.0;
|
|
|
+ for (auto&p : cluster_center) {
|
|
|
+ if (fabs(p.z - minz) < step_harf) { meanz += p.z; cnt += 1.0; }
|
|
|
+ }
|
|
|
+ if(cnt>0.0){meanz /= cnt;}
|
|
|
+ else { meanz = minz; }
|
|
|
+ // 生成所有可能的植株位置中心点
|
|
|
+ std::vector<pcl::PointXYZ>candidate_centers;
|
|
|
+ for (int row = -1; row <= 1; row += 1) {
|
|
|
+ for (int col = -6; col <= 6; col += 1) {
|
|
|
+ float cx = minz_it->x + col * 2.0 * step_harf;
|
|
|
+ float cz = meanz + row * 2.0 * step_harf;
|
|
|
+ if (cx < xmin || cx > xmax) { continue; }
|
|
|
+ if (cz < zmin || cz > zmax) { continue; }
|
|
|
+ // 邻域内已经存在植株的跳过
|
|
|
+ bool with_seedling = false;
|
|
|
+ for (auto&p : cluster_center) {
|
|
|
+ float d = std::sqrt((p.x - cx) * (p.x - cx) + (p.z - cz) * (p.z - cz));
|
|
|
+ if (d < 2.0*step_harf) {
|
|
|
+ with_seedling=true;
|
|
|
+ break;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ if (!with_seedling) {//保存
|
|
|
+ pcl::PointXYZ pc(cx,0.0,cz);
|
|
|
+ candidate_centers.push_back(pc);
|
|
|
+ }
|
|
|
+ }
|
|
|
+ }
|
|
|
+ //针对每一个candidat_centers的邻域进行检查，检测是否有茎目标
|
|
|
+ for (auto&pc : candidate_centers) {
|
|
|
+ }
|
|
|
+
|
|
|
+ }
|
|
|
|
|
|
- float hole_step = 40.0; //穴盘中穴间距
|
|
|
+ void CRootStockGrabPoint::find_seedling_position(
|
|
|
+ pcl::PointCloud<pcl::PointXYZ>::Ptr in_cloud,
|
|
|
+ std::vector<int> &first_seedling_cloud_idx,
|
|
|
+ pcl::PointXYZ&xz_center
|
|
|
+ )
|
|
|
+ {
|
|
|
+ // 确定植株inbox范围
|
|
|
+ float hole_step = m_cparam.rs_grab_seedling_dist - 10.0; //穴盘中穴间距
|
|
|
+ if (m_dtype == 0) {
|
|
|
+ hole_step = m_cparam.sc_grab_seedling_dist - 10.0;
|
|
|
+ }
|
|
|
float hole_step_radius = hole_step / 2.0;
|
|
|
|
|
|
+ // 点云降维到xz平面，y=0
|
|
|
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud2d(new pcl::PointCloud < pcl::PointXYZ>);
|
|
|
pcl::copyPointCloud(*in_cloud, *cloud2d);
|
|
|
for (auto&pt : *cloud2d) {
|
|
|
@@ -467,9 +970,10 @@ namespace graft_cv {
|
|
|
}
|
|
|
|
|
|
/*if(m_cparam.image_show){
|
|
|
- viewer_cloud(cloud2d, std::string("cloud2d"));
|
|
|
+ viewer_cloud(cloud2d, std::string("cloud2d"));
|
|
|
} */
|
|
|
|
|
|
+ // 在xz平面内统计点的密度
|
|
|
double radius = m_cparam.rs_grab_stem_diameter;
|
|
|
if (m_dtype == 0) {
|
|
|
radius = m_cparam.sc_grab_stem_diameter;
|
|
|
@@ -483,27 +987,28 @@ namespace graft_cv {
|
|
|
int k = kdtree.radiusSearch(cloud2d->points.at(i), radius, idx, dist_sqr);
|
|
|
counter.push_back(k);
|
|
|
}
|
|
|
- int th = (int)(otsu(counter));
|
|
|
+ // 计算根据密度分割的自动阈值
|
|
|
+ int th = (int)(otsu(counter));
|
|
|
+ // 得到茎目标的点云序号，和对应的点云cloud2d_pos
|
|
|
std::vector<int> index;
|
|
|
for (size_t i = 0; i < counter.size(); ++i) {
|
|
|
if (counter.at(i) >= th) {
|
|
|
index.push_back(i);
|
|
|
}
|
|
|
}
|
|
|
-
|
|
|
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud2d_pos(new pcl::PointCloud < pcl::PointXYZ>);
|
|
|
pcl::copyPointCloud(*cloud2d, index, *cloud2d_pos);
|
|
|
|
|
|
if (m_pLogger) {
|
|
|
stringstream buff;
|
|
|
- buff << "get 2d seedling seed point cloud " << index.size()<< " data points with thrreshold "<<th;
|
|
|
+ buff << "get 2d seedling seed point cloud " << index.size() << " data points with thrreshold " << th;
|
|
|
m_pLogger->INFO(buff.str());
|
|
|
}
|
|
|
if (m_cparam.image_show) {
|
|
|
viewer_cloud(cloud2d_pos, std::string("cloud2d_pos"));
|
|
|
}
|
|
|
-
|
|
|
- //clustering
|
|
|
+
|
|
|
+ //对茎的点云进行clustering，得到不同的茎的分组
|
|
|
double d1 = m_cparam.rs_grab_stem_diameter;
|
|
|
double d2 = m_cparam.rs_grab_stem_diameter * 3.0;
|
|
|
if (m_dtype == 0) {
|
|
|
@@ -530,8 +1035,12 @@ namespace graft_cv {
|
|
|
pt.g = 255;
|
|
|
pt.b = 255;
|
|
|
}
|
|
|
- viewer_cloud_cluster(cloud_cluster, cluster_center, std::string("cloud2d_cluster_pos"));
|
|
|
+ viewer_cloud_cluster(cloud_cluster, cluster_center, std::string("cloud2d_cluster_pos"));
|
|
|
}
|
|
|
+ // 扩展检测第一排的可能位置，找到倾斜的苗
|
|
|
+ std::vector<pcl::PointXYZ> tilted_center;
|
|
|
+ tilted_seedling_discover(cluster_center, tilted_center);
|
|
|
+
|
|
|
//对每个类别进行检测，剔除不是茎的类别
|
|
|
std::vector<pcl::PointXYZ> clt_root;
|
|
|
std::vector<pcl::PointXYZ> clt_box;
|
|
|
@@ -573,7 +1082,7 @@ namespace graft_cv {
|
|
|
//找到line_idx中y最小的那个点的idx
|
|
|
int line_root_idx = -1;
|
|
|
float line_root_y_min = 10000.0;
|
|
|
- for (size_t lidx = 0; lidx < line_idx.size();++lidx) {
|
|
|
+ for (size_t lidx = 0; lidx < line_idx.size(); ++lidx) {
|
|
|
int raw_idx = line_idx.at(lidx);
|
|
|
if (in_cloud->at(raw_idx).y < line_root_y_min) {
|
|
|
line_root_y_min = in_cloud->at(raw_idx).y;
|
|
|
@@ -583,14 +1092,14 @@ namespace graft_cv {
|
|
|
//找到底部中心点，然后找到根部坐标
|
|
|
pcl::PointXYZ pt_root;
|
|
|
pt_root = in_cloud->at(line_root_idx);
|
|
|
-
|
|
|
+
|
|
|
clt_root.push_back(pt_root);
|
|
|
clt_box.push_back(min_point_aabb_ex);
|
|
|
clt_box.push_back(max_point_aabb_ex);
|
|
|
//viewer_cloud(clt_cloud, std::string("cluster"));
|
|
|
}
|
|
|
- if (m_cparam.image_show) {
|
|
|
- viewer_cloud_cluster_box(in_cloud, clt_root, clt_box, clt_line_idx, std::string("cloud2d_cluster_box"));
|
|
|
+ if (m_cparam.image_show) {
|
|
|
+ viewer_cloud_cluster_box(in_cloud, clt_root, clt_box, clt_line_idx, std::string("cloud2d_cluster_box"));
|
|
|
}
|
|
|
|
|
|
//sort cluster center, get the frontest leftest one
|
|
|
@@ -605,7 +1114,7 @@ namespace graft_cv {
|
|
|
}
|
|
|
|
|
|
first_seedling_cloud_idx.clear();
|
|
|
- for (auto&v : clt_line_idx.at(first_idx)) {
|
|
|
+ for (auto&v : clt_line_idx.at(first_idx)) {
|
|
|
first_seedling_cloud_idx.push_back(v);
|
|
|
}
|
|
|
xz_center.x = clt_root.at(first_idx).x;
|
|
|
@@ -615,10 +1124,12 @@ namespace graft_cv {
|
|
|
if (m_pLogger) {
|
|
|
stringstream buff;
|
|
|
buff << "euclidean_clustering_ttsas， find cluster center(" << xz_center.x
|
|
|
- <<", "<< xz_center.y<<", "<< xz_center.z<<")";
|
|
|
+ << ", " << xz_center.y << ", " << xz_center.z << ")";
|
|
|
m_pLogger->INFO(buff.str());
|
|
|
}
|
|
|
}
|
|
|
+
|
|
|
+
|
|
|
void CRootStockGrabPoint::find_line_in_cube(
|
|
|
pcl::PointCloud<pcl::PointXYZ>::Ptr in_cloud, //输入，整体点云
|
|
|
pcl::PointXYZ& min_pt_ex, //输入，
|
|
|
@@ -1263,7 +1774,7 @@ namespace graft_cv {
|
|
|
pcl::visualization::PCL_VISUALIZER_REPRESENTATION_WIREFRAME, "AABB_" + string(_itoa(cnt, buf, 10)));
|
|
|
|
|
|
pcl::PointCloud<pcl::PointXYZ>::Ptr line_cloud(new pcl::PointCloud<pcl::PointXYZ>);
|
|
|
- pcl::copyPointCloud(*cloud, clt_line_idx.at(i), *line_cloud);
|
|
|
+ //pcl::copyPointCloud(*cloud, clt_line_idx.at(i), *line_cloud);
|
|
|
viewer.addPointCloud(line_cloud, "fit_line" + string(_itoa(cnt, buf, 10)));
|
|
|
viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 0, 1, 0, "fit_line" + string(_itoa(cnt, buf, 10)));
|
|
|
viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "fit_line" + string(_itoa(cnt, buf, 10)));
|
