|
|
@@ -2478,9 +2478,11 @@ void CRootStockGrabPoint::line_filter(
|
|
|
float ymin, ymax;
|
|
|
ymin = m_cparam.rs_grab_ymin;
|
|
|
ymax = m_cparam.rs_grab_ymax;
|
|
|
+ float radius = m_cparam.rs_grab_stem_diameter;
|
|
|
if (m_dtype == 0) {
|
|
|
ymin = m_cparam.sc_grab_ymin;
|
|
|
ymax = m_cparam.sc_grab_ymax;
|
|
|
+ radius = m_cparam.sc_grab_stem_diameter;
|
|
|
}
|
|
|
|
|
|
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_inbox(new pcl::PointCloud<pcl::PointXYZ>);
|
|
|
@@ -2496,22 +2498,14 @@ void CRootStockGrabPoint::line_filter(
|
|
|
box_filter_line.setNegative(false);
|
|
|
box_filter_line.setInputCloud(in_line_cloud);
|
|
|
|
|
|
- float radius = m_cparam.rs_grab_stem_diameter;
|
|
|
+
|
|
|
|
|
|
float cx, cy, cz, t;
|
|
|
float rx = 1.5;
|
|
|
float ry = 1.5;
|
|
|
float rz = 1.5;
|
|
|
- float dz, dx;
|
|
|
+ float dz, dx;
|
|
|
|
|
|
- // 如果opt_y_valid==false，就是用户没有指定抓取的y高度
|
|
|
- std::vector<int> fork_positions;
|
|
|
- find_fork(in_line_cloud, fork_positions);
|
|
|
- for (auto&y : fork_positions) {
|
|
|
- fork_ys.push_back(y + ymin);
|
|
|
- }
|
|
|
-
|
|
|
- stem_deflection = static_cast<float>(calculate_deflection(in_line_cloud, line_model));
|
|
|
|
|
|
cy = ymin;
|
|
|
//计算茎上的直径
|
|
|
@@ -2551,6 +2545,14 @@ void CRootStockGrabPoint::line_filter(
|
|
|
stem_width_mu = mu;
|
|
|
float stdv = get_hist_std(valid_stem_width, mu);
|
|
|
|
|
|
+ //找到茎节
|
|
|
+ std::vector<int> fork_positions;
|
|
|
+ find_fork(in_line_cloud, stem_width_mu, stem_width, fork_positions);
|
|
|
+ for (auto&y : fork_positions) {
|
|
|
+ fork_ys.push_back(y + ymin);
|
|
|
+ }
|
|
|
+
|
|
|
+ stem_deflection = static_cast<float>(calculate_deflection(in_line_cloud, line_model));
|
|
|
|
|
|
//4 用茎节上下限高度值进行约束，如果超限，用最低限高度作为茎节高度
|
|
|
double grab_fork_yup = m_cparam.rs_grab_fork_yup;
|
|
|
@@ -2711,6 +2713,8 @@ void CRootStockGrabPoint::line_filter(
|
|
|
pcl::PointCloud<pcl::PointXYZ>::Ptr in_line_cloud,
|
|
|
//float& max_dist,
|
|
|
//int& max_idx,
|
|
|
+ float stem_width_mu,
|
|
|
+ const std::vector<float>& stem_width_ex,//输入， 通过外沿矩形得到的x方向茎粗
|
|
|
std::vector<int>&fork_pos //输出， 多个fork位置
|
|
|
)
|
|
|
{
|
|
|
@@ -2773,9 +2777,13 @@ void CRootStockGrabPoint::line_filter(
|
|
|
//构建基于ymin的vector记录每一个毫米中的最大距离边缘的距离
|
|
|
double ymin = m_cparam.rs_grab_ymin;
|
|
|
double ymax = m_cparam.rs_grab_ymax;
|
|
|
+ double stemd = m_cparam.rs_grab_stem_diameter;
|
|
|
+ double th_fork = m_cparam.rs_grab_fork_ratio;
|
|
|
if (m_dtype == 0) {
|
|
|
ymin = m_cparam.sc_grab_ymin;
|
|
|
ymax = m_cparam.sc_grab_ymax;
|
|
|
+ stemd = m_cparam.sc_grab_stem_diameter;
|
|
|
+ th_fork = m_cparam.sc_grab_fork_ratio;
|
|
|
}
|
|
|
int length = int(ymax - ymin);
|
|
|
std::vector<float> stem_diameters;
|
|
|
@@ -2807,13 +2815,33 @@ void CRootStockGrabPoint::line_filter(
|
|
|
if (cnt > 0.0) {
|
|
|
mean_r /= cnt;
|
|
|
}
|
|
|
- float th_fork = 1.5;
|
|
|
+
|
|
|
int del_radius = 4;
|
|
|
+ int security_r = (int)(stemd / 2.0 + 0.5);//保护半径
|
|
|
+ int noise_r = int(stemd); //噪声半径
|
|
|
+ std::vector<float> stem_noise;
|
|
|
+ calculate_noise(stem_diameters, security_r, noise_r, stem_noise);
|
|
|
while (true) {
|
|
|
//找到最大值的位置和最大值
|
|
|
int midx = max_element(stem_diameters.begin(), stem_diameters.end()) - stem_diameters.begin();
|
|
|
if (stem_diameters.at(midx) > th_fork * mean_r) {
|
|
|
- fork_pos.push_back(midx);
|
|
|
+ //大于周围的值的均值的1.5倍，才能成为茎节
|
|
|
+ if (stem_diameters.at(midx) > th_fork * stem_noise.at(midx)) {
|
|
|
+ //检测fork_pos中的近邻是否存在
|
|
|
+ bool has_nn = false;
|
|
|
+ for (auto& fy : fork_pos) {
|
|
|
+ if (fabs(fy - midx) < 20) {
|
|
|
+ has_nn = true;
|
|
|
+ break;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ //如果没有近邻存在，保存
|
|
|
+ if (!has_nn) {
|
|
|
+ fork_pos.push_back(midx);
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ //清除发现的最大直径周围的数据
|
|
|
for (int k = -del_radius; k <= del_radius; ++k) {
|
|
|
int j = k + midx;
|
|
|
if (j < 0 || j >= length) {
|
|
|
@@ -2826,13 +2854,45 @@ void CRootStockGrabPoint::line_filter(
|
|
|
break;
|
|
|
}
|
|
|
}
|
|
|
+ //再通过stem_width_ex找茎节
|
|
|
+ std::vector<float> stem_width_ex_cp(stem_width_ex);
|
|
|
+ while (true) {
|
|
|
+ //找到最大值的位置和最大值
|
|
|
+ int midx = max_element(stem_width_ex_cp.begin(), stem_width_ex_cp.end()) - stem_width_ex_cp.begin();
|
|
|
+ if (stem_width_ex_cp.at(midx) > th_fork * stem_width_mu) {
|
|
|
+ //检测fork_pos中的近邻是否存在
|
|
|
+ bool has_nn = false;
|
|
|
+ for (auto& fy : fork_pos) {
|
|
|
+ if (fabs(fy - midx) < 20) {
|
|
|
+ has_nn = true;
|
|
|
+ break;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ //如果没有近邻存在，保存
|
|
|
+ if (!has_nn) {
|
|
|
+ fork_pos.push_back(midx);
|
|
|
+ }
|
|
|
+
|
|
|
|
|
|
+ //清除发现的最大直径周围的数据
|
|
|
+ for (int k = -del_radius; k <= del_radius; ++k) {
|
|
|
+ int j = k + midx;
|
|
|
+ if (j < 0 || j >= stem_width_ex_cp.size()) {
|
|
|
+ continue;
|
|
|
+ }
|
|
|
+ stem_width_ex_cp.at(j) = 0.0;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ else {
|
|
|
+ break;
|
|
|
+ }
|
|
|
+ }
|
|
|
|
|
|
if (m_cparam.image_show) {
|
|
|
pcl::visualization::PCLVisualizer viewer("boundary line");
|
|
|
viewer.addCoordinateSystem();
|
|
|
viewer.addPointCloud<pcl::PointXYZRGB>(cloud_visual, "boundary");
|
|
|
-
|
|
|
+ char buf[8];
|
|
|
for(auto&h: fork_pos) {
|
|
|
pcl::PointXYZ p0,p1;
|
|
|
p0.x = cloud_line_2d->points.at(0).x-5;
|
|
|
@@ -2842,7 +2902,7 @@ void CRootStockGrabPoint::line_filter(
|
|
|
p1.x = cloud_line_2d->points.at(0).x + 5;
|
|
|
p1.y = ymin+h;
|
|
|
p1.z = cloud_line_2d->points.at(0).z;
|
|
|
- viewer.addLine(p0, p1, 1.0, 0.0, 0.0);
|
|
|
+ viewer.addLine(p0, p1, 1.0, 0.0, 0.0, "fork_y_" + string(_itoa(h, buf, 10)));
|
|
|
}
|
|
|
|
|
|
|
|
|
@@ -2852,6 +2912,36 @@ void CRootStockGrabPoint::line_filter(
|
|
|
}
|
|
|
}
|
|
|
}
|
|
|
+ void CRootStockGrabPoint::calculate_noise(
|
|
|
+ std::vector<float>&diamters, //input
|
|
|
+ int security_radius, //input
|
|
|
+ int noise_radius, //input
|
|
|
+ std::vector<float>&noise //output
|
|
|
+ )
|
|
|
+ {
|
|
|
+ noise.clear();
|
|
|
+ float mu_noise = 0.0;
|
|
|
+ float cnt = 0.0;
|
|
|
+ int k;
|
|
|
+ for (int i = 0; i < diamters.size(); ++i) {
|
|
|
+ mu_noise = 0.0;
|
|
|
+ cnt = 0.0;
|
|
|
+ for (int di = -(security_radius + noise_radius); di <= (security_radius + noise_radius); ++di)
|
|
|
+ {
|
|
|
+ if (di >= -security_radius && di <= security_radius) { continue; }
|
|
|
+ k = i + di;
|
|
|
+ if (k < 0 || k >= diamters.size()) { continue; }
|
|
|
+ if (diamters.at(k) <= 1.0e-6) { continue; }
|
|
|
+
|
|
|
+ mu_noise += diamters.at(k);
|
|
|
+ cnt += 1.0;
|
|
|
+ }
|
|
|
+ if (cnt > 0.0) {
|
|
|
+ mu_noise /= cnt;
|
|
|
+ }
|
|
|
+ noise.push_back(mu_noise);
|
|
|
+ }
|
|
|
+ }
|
|
|
|
|
|
void CRootStockGrabPoint::get_line_project_hist(
|
|
|
pcl::PointCloud<pcl::PointXYZ>::Ptr in_line_cloud, //input 茎上直线点云
|
chenhongjiang