|
|
@@ -134,13 +134,14 @@ namespace graft_cv {
|
|
|
viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 1, 1, 1, "raw_cloud");
|
|
|
viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "raw_cloud");
|
|
|
|
|
|
- float xmin, ymin, zmin, xmax, ymax, zmax;
|
|
|
+ float xmin, ymin, zmin, xmax, ymax, zmax, zcent;
|
|
|
xmin = m_cparam.rs_grab_xmin;
|
|
|
ymin = m_cparam.rs_grab_ymin;
|
|
|
zmin = m_cparam.rs_grab_zmin;
|
|
|
xmax = m_cparam.rs_grab_xmax;
|
|
|
ymax = m_cparam.rs_grab_ymax;
|
|
|
zmax = m_cparam.rs_grab_zmax;
|
|
|
+ zcent = m_cparam.rs_grab_zcent;
|
|
|
if (m_dtype == 0) {
|
|
|
xmin = m_cparam.sc_grab_xmin;
|
|
|
ymin = m_cparam.sc_grab_ymin;
|
|
|
@@ -148,12 +149,17 @@ namespace graft_cv {
|
|
|
xmax = m_cparam.sc_grab_xmax;
|
|
|
ymax = m_cparam.sc_grab_ymax;
|
|
|
zmax = m_cparam.sc_grab_zmax;
|
|
|
+ zcent = m_cparam.sc_grab_zcent;
|
|
|
}
|
|
|
|
|
|
viewer.addCube(xmin, xmax,ymin, ymax, zmin, zmax, 0.75, 0.0, 0.0, "AABB_");
|
|
|
viewer.setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_REPRESENTATION,
|
|
|
pcl::visualization::PCL_VISUALIZER_REPRESENTATION_WIREFRAME, "AABB_");
|
|
|
|
|
|
+ viewer.addCube(xmin, xmax, ymin, ymax, zmin, zcent, 0.75, 0.0, 0.0, "AABB_front");
|
|
|
+ viewer.setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_REPRESENTATION,
|
|
|
+ pcl::visualization::PCL_VISUALIZER_REPRESENTATION_WIREFRAME, "AABB_front");
|
|
|
+
|
|
|
pcl::PointXYZ px0, px1, py1, pz1;
|
|
|
px0.x = 0;
|
|
|
px0.y = 0;
|
|
|
@@ -187,6 +193,7 @@ namespace graft_cv {
|
|
|
double x_max = m_cparam.rs_grab_xmax;
|
|
|
double z_min = m_cparam.rs_grab_zmin;
|
|
|
double z_max = m_cparam.rs_grab_zmax;
|
|
|
+ double z_cent = m_cparam.rs_grab_zcent;
|
|
|
if (m_dtype == 0) {
|
|
|
seedling_distance = m_cparam.sc_grab_seedling_dist;
|
|
|
holes_number = m_cparam.sc_grab_holes_number;
|
|
|
@@ -194,10 +201,11 @@ namespace graft_cv {
|
|
|
x_max = m_cparam.sc_grab_xmax;
|
|
|
z_min = m_cparam.sc_grab_zmin;
|
|
|
z_max = m_cparam.sc_grab_zmax;
|
|
|
+ z_cent = m_cparam.sc_grab_zcent;
|
|
|
}
|
|
|
m_pStemInfos = new CStemResultInfos(
|
|
|
seedling_distance, holes_number,
|
|
|
- x_min, x_max, z_min, z_max,
|
|
|
+ x_min, x_max, z_min, z_max, z_cent,
|
|
|
m_pcdId, m_pLogger);
|
|
|
}
|
|
|
return rst;
|
|
|
@@ -384,7 +392,12 @@ namespace graft_cv {
|
|
|
}
|
|
|
|
|
|
int first_row_seedling_number = 0;
|
|
|
- bool fund_seedling = find_seedling_position_key(cloud_dowm_sampled, first_seedling_cloud_idx, xz_center, line_model, first_row_seedling_number);
|
|
|
+ bool fund_seedling = find_seedling_position_key(
|
|
|
+ cloud_dowm_sampled,
|
|
|
+ first_seedling_cloud_idx,
|
|
|
+ xz_center, line_model,
|
|
|
+ first_row_seedling_number);
|
|
|
+
|
|
|
if (!fund_seedling && first_row_seedling_number == 0) {
|
|
|
if (m_pLogger) {
|
|
|
stringstream buff;
|
|
|
@@ -707,7 +720,7 @@ namespace graft_cv {
|
|
|
}
|
|
|
//根据历史根的位置，计算对应位置点云数量，进而判断此位置是否有苗
|
|
|
void CRootStockGrabPoint::occluded_seedling_detect_by_leaf(
|
|
|
- pcl::PointCloud<pcl::PointXYZ>::Ptr in_cloud, //input 输入点云数据
|
|
|
+ pcl::PointCloud<pcl::PointXYZ>::Ptr in_cloud, //input 输入点云数据,包含叶子和茎的点云
|
|
|
std::vector<int>& leaf_idx
|
|
|
)
|
|
|
{
|
|
|
@@ -952,9 +965,9 @@ namespace graft_cv {
|
|
|
}
|
|
|
|
|
|
void CRootStockGrabPoint::get_leaf_point_count_inbox(
|
|
|
- const CStemResult& sr,//input
|
|
|
- pcl::PointCloud<pcl::PointXYZ>::Ptr in_cloud, //input 输入点云数据
|
|
|
- std::vector<int>& leaf_idx, //input
|
|
|
+ const CStemResult& sr, //input 指定的苗的中心
|
|
|
+ pcl::PointCloud<pcl::PointXYZ>::Ptr in_cloud, //input 输入点云数据,包含叶子和茎的点云
|
|
|
+ std::vector<int>& leaf_idx, //input，叶子点云index
|
|
|
pcl::PointXYZ& aabb_min,//output
|
|
|
pcl::PointXYZ& aabb_max,//output
|
|
|
int& total_cnt, //output
|
|
|
@@ -968,16 +981,17 @@ namespace graft_cv {
|
|
|
double seedling_distance = m_cparam.rs_grab_seedling_dist;
|
|
|
double min_y = m_cparam.rs_grab_ymin;
|
|
|
double max_y = m_cparam.rs_grab_ymax;
|
|
|
+ double min_z = m_cparam.rs_grab_zmin;
|
|
|
double stem_diameter = m_cparam.rs_grab_stem_diameter;
|
|
|
if (m_dtype == 0) {
|
|
|
seedling_distance = m_cparam.sc_grab_seedling_dist;
|
|
|
min_y = m_cparam.sc_grab_ymin;
|
|
|
max_y = m_cparam.sc_grab_ymax;
|
|
|
+ min_z = m_cparam.sc_grab_zmin;
|
|
|
stem_diameter = m_cparam.sc_grab_stem_diameter;
|
|
|
}
|
|
|
double min_x = sr.root_x - 0.5 * seedling_distance;
|
|
|
- double max_x = sr.root_x + 0.5 * seedling_distance;
|
|
|
- double min_z = sr.root_z - 0.75 * seedling_distance;
|
|
|
+ double max_x = sr.root_x + 0.5 * seedling_distance;
|
|
|
double max_z = sr.root_z + 0.25 * seedling_distance;
|
|
|
aabb_min.x = min_x;
|
|
|
aabb_min.y = min_y;
|
|
|
@@ -1436,30 +1450,35 @@ void CRootStockGrabPoint::nms_box(
|
|
|
}
|
|
|
//通过指定位置内,取部分点云分析是否存在真正的茎，真茎位置保存到target_filtered
|
|
|
void CRootStockGrabPoint::line_filter(
|
|
|
- pcl::PointCloud<pcl::PointXYZ>::Ptr in_cloud, //输入点云
|
|
|
+ pcl::PointCloud<pcl::PointXYZ>::Ptr in_cloud, //输入点云,仅含茎的点云
|
|
|
float radius, //输入，取点半径
|
|
|
- float ymin,
|
|
|
- float ymax,
|
|
|
+ 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
|
|
|
- std::vector<pcl::ModelCoefficients::Ptr>& target_filtered_models//输出，茎直线模型
|
|
|
-
|
|
|
+ std::vector<pcl::PointXYZ>&target_filtered, //输出，box底部中心位置
|
|
|
+ std::vector<pcl::PointXYZ>&target_filtered_root, //输出，茎上根部点坐标
|
|
|
+ std::vector<std::vector<int>>&target_filtered_element, //输出，茎上点index
|
|
|
+ std::vector<pcl::ModelCoefficients::Ptr>& target_filtered_models, //输出，茎直线模型
|
|
|
+ std::vector<int>& target_filtered_occlusion_status //输出，茎状态，是否遮挡，1--有茎，2--叶子遮挡
|
|
|
)
|
|
|
{
|
|
|
target_filtered.clear();
|
|
|
target_filtered_element.clear();
|
|
|
target_filtered_root.clear();
|
|
|
+ target_filtered_occlusion_status.clear();
|
|
|
if (target_root.size() == 0) { return; }
|
|
|
|
|
|
- double stem_height_ratio = m_cparam.rs_grab_valid_stem_ratio;
|
|
|
+ double stem_ratio_th = m_cparam.rs_grab_valid_stem_ratio;
|
|
|
+ double leaf_ratio_th = m_cparam.rs_grab_valid_occlusion_ratio;
|
|
|
+ double overall_ratio_th = m_cparam.rs_grab_valid_overall_ratio;
|
|
|
float stem_radius = m_cparam.rs_grab_stem_diameter / 2.0;
|
|
|
int min_stem_pts = m_cparam.rs_grab_stem_min_pts;
|
|
|
if (m_dtype == 0) {
|
|
|
stem_radius = m_cparam.sc_grab_stem_diameter / 2.0;
|
|
|
min_stem_pts = m_cparam.sc_grab_stem_min_pts;
|
|
|
- stem_height_ratio = m_cparam.sc_grab_valid_stem_ratio;
|
|
|
+ stem_ratio_th = m_cparam.sc_grab_valid_stem_ratio;
|
|
|
+ leaf_ratio_th = m_cparam.sc_grab_valid_occlusion_ratio;
|
|
|
+ overall_ratio_th = m_cparam.sc_grab_valid_overall_ratio;
|
|
|
}
|
|
|
|
|
|
for (auto&p : target_root) {
|
|
|
@@ -1523,22 +1542,22 @@ void CRootStockGrabPoint::line_filter(
|
|
|
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<y_length_th || dy / (ymax - ymin) < stem_height_ratio) { continue; }
|
|
|
+ //float dy = max_v.y - min_v.y;
|
|
|
+ //if (dy<y_length_th || dy / (ymax - ymin) < stem_height_ratio) { continue; }
|
|
|
//y方向分布中心滤波
|
|
|
- float dy_c = 0.5*(max_v.y + min_v.y);
|
|
|
- if ((dy_c - ymin) / (ymax - ymin) > 0.75) { continue; }
|
|
|
+ //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 < 60.0) { continue; }
|
|
|
- target_filtered.push_back(p);
|
|
|
+ if (a < 60.0) { continue; }
|
|
|
|
|
|
//有效茎长计算
|
|
|
- std::vector<int> stem_y_count_hist;
|
|
|
- get_line_project_hist(stem_cloud, coefficients, stem_y_count_hist);
|
|
|
+ std::vector<int> stem_y_count_hist;//茎在y方向的点云数量统计
|
|
|
+ std::vector<int> leaf_y_count_hist;//叶子在y方向的点云数量统计，随着茎轴线，茎位置到zmin间的点云数量统计
|
|
|
+ get_line_project_hist(stem_cloud, coefficients, stem_y_count_hist, leaf_y_count_hist);
|
|
|
std::vector<int> stem_y_count_hist_valid;
|
|
|
for (auto&v : stem_y_count_hist) {
|
|
|
if (v > 2) {
|
|
|
@@ -1547,28 +1566,71 @@ void CRootStockGrabPoint::line_filter(
|
|
|
}
|
|
|
double valid_mu = get_hist_mean(stem_y_count_hist_valid);
|
|
|
int cnt_th = static_cast<int>(valid_mu/2.0);
|
|
|
- if (cnt_th < 3) { cnt_th = 2; }
|
|
|
-
|
|
|
- float valid_length = 0.0;
|
|
|
+ if (cnt_th < 3) { cnt_th = 2; }
|
|
|
|
|
|
std::vector<int> stem_y_mask;
|
|
|
+ std::vector<int> leaf_y_mask;
|
|
|
|
|
|
- for (auto& c : stem_y_count_hist) {
|
|
|
- if (c > cnt_th) {
|
|
|
- valid_length += 1.0;
|
|
|
+ for (int i = 0; i < stem_y_count_hist.size();++i) {
|
|
|
+ int stem_cnt = stem_y_count_hist.at(i);
|
|
|
+ int leaf_cnt = leaf_y_count_hist.at(i);
|
|
|
+ if (stem_cnt > cnt_th) {
|
|
|
stem_y_mask.push_back(1);
|
|
|
+ leaf_y_mask.push_back(0);
|
|
|
}
|
|
|
else {
|
|
|
stem_y_mask.push_back(0);
|
|
|
+ if (leaf_cnt > cnt_th) {
|
|
|
+ leaf_y_mask.push_back(1);
|
|
|
+ }
|
|
|
+ else {
|
|
|
+ leaf_y_mask.push_back(0);
|
|
|
+ }
|
|
|
}
|
|
|
+
|
|
|
}
|
|
|
//填补空
|
|
|
for (int i = 1; i < stem_y_mask.size() - 1; ++i) {
|
|
|
if (stem_y_mask.at(i) == 0 && stem_y_mask.at(i - 1) == 1 && stem_y_mask.at(i + 1) == 1) {
|
|
|
stem_y_mask.at(i) = 1;
|
|
|
+ leaf_y_mask.at(i) = 0;
|
|
|
}
|
|
|
}
|
|
|
- //找出最长的连续段的长度
|
|
|
+ float valid_length = 0.0;
|
|
|
+ float valid_length_leaf = 0.0;
|
|
|
+ for (int i = 1; i < stem_y_mask.size(); ++i) {
|
|
|
+ valid_length += stem_y_mask.at(i);
|
|
|
+ }
|
|
|
+ for (int i = 1; i < leaf_y_mask.size(); ++i) {
|
|
|
+ valid_length_leaf += leaf_y_mask.at(i);
|
|
|
+ }
|
|
|
+ float overall_ratio = (valid_length + valid_length_leaf) / (ymax - ymin);
|
|
|
+ float stem_ratio = valid_length / (ymax - ymin);
|
|
|
+ float leaf_ratio = valid_length_leaf / (ymax - ymin);
|
|
|
+ if (m_pLogger) {
|
|
|
+ stringstream buff;
|
|
|
+ buff << m_pcdId << ": overall(stem + leaf) ratio = " << overall_ratio
|
|
|
+ << ", stem_ratio = " << stem_ratio
|
|
|
+ << ", leaf_ratio = " << leaf_ratio;
|
|
|
+ m_pLogger->INFO(buff.str());
|
|
|
+ }
|
|
|
+ if (overall_ratio < overall_ratio_th) {
|
|
|
+ continue;
|
|
|
+ }
|
|
|
+ if (stem_ratio < stem_ratio_th) {
|
|
|
+ if (leaf_ratio > leaf_ratio_th) {
|
|
|
+ //有显著的叶子
|
|
|
+ target_filtered_occlusion_status.push_back(2);
|
|
|
+ }
|
|
|
+ else {
|
|
|
+ continue;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ //有显著的茎
|
|
|
+ target_filtered_occlusion_status.push_back(1);
|
|
|
+
|
|
|
+
|
|
|
+ /*//找出最长的连续段的长度
|
|
|
int longest_segment = 0;
|
|
|
int start = 0;
|
|
|
for (int i = 0; i < stem_y_mask.size(); ++i) {
|
|
|
@@ -1600,6 +1662,7 @@ void CRootStockGrabPoint::line_filter(
|
|
|
}
|
|
|
|
|
|
if (longest_segment<10.0 || valid_length / (ymax - ymin) < stem_height_ratio) { continue; }
|
|
|
+ */
|
|
|
|
|
|
|
|
|
float min_y = 10000.0;
|
|
|
@@ -1622,6 +1685,9 @@ void CRootStockGrabPoint::line_filter(
|
|
|
}
|
|
|
target_filtered_element.push_back(out_idx);
|
|
|
target_filtered_models.push_back(coefficients);
|
|
|
+ target_filtered.push_back(p);
|
|
|
+
|
|
|
+
|
|
|
|
|
|
// debug show
|
|
|
/*if (m_cparam.image_show) {
|
|
|
@@ -1674,6 +1740,12 @@ void CRootStockGrabPoint::line_filter(
|
|
|
)
|
|
|
{
|
|
|
first_row_size = 0;
|
|
|
+ //0）in_cloud是仅含茎的点云
|
|
|
+ //1）将判别成茎的点云在xz平面投影，找到密度高的点
|
|
|
+ //2）基于密度最高的点，生成可能是茎位置的备选box
|
|
|
+ //3）备选box通过点云分布，进行过滤
|
|
|
+ //4）nms过滤距离较近的box
|
|
|
+ //5）每个框内的点云直线分析，找出符合要求的茎
|
|
|
|
|
|
// 确定植株inbox范围
|
|
|
float hole_step = m_cparam.rs_grab_seedling_dist - 5.0; //穴盘中穴间距
|
|
|
@@ -1689,7 +1761,7 @@ void CRootStockGrabPoint::line_filter(
|
|
|
radius = m_cparam.sc_grab_stem_diameter;
|
|
|
}
|
|
|
|
|
|
- float hole_step_radius = 2.0 * hole_step / 3.0;
|
|
|
+ float hole_step_radius = 0.5 * hole_step;
|
|
|
|
|
|
// 点云降维到xz平面，y=0
|
|
|
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud2d(new pcl::PointCloud < pcl::PointXYZ>);
|
|
|
@@ -1820,7 +1892,8 @@ void CRootStockGrabPoint::line_filter(
|
|
|
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);
|
|
|
+ float min_distance = 0.6 * hole_step;
|
|
|
+ nms_box(clt_root_v, valid_box_cc_dist, min_distance, target_root);
|
|
|
|
|
|
// 显示
|
|
|
if (m_cparam.image_show) {
|
|
|
@@ -1846,17 +1919,32 @@ void CRootStockGrabPoint::line_filter(
|
|
|
///////////////////////////////////////////////////////////////////////////////////////////////////////////
|
|
|
///////////////////////////////////////////////////////////////////////////////////////////////////////////
|
|
|
// 根据得到的位置，直线检测，并过滤
|
|
|
+ std::vector<pcl::PointXYZ>target_filtered_;
|
|
|
+ std::vector<std::vector<int>> target_member_;//输出，茎上点index
|
|
|
+ std::vector<pcl::PointXYZ>target_filtered_root_;
|
|
|
+ std::vector<pcl::ModelCoefficients::Ptr> target_filtered_models_; //茎直线模型
|
|
|
+ std::vector<int> target_filtered_stem_status; //输出，茎状态，是否遮挡，1--有茎，2--叶子遮挡
|
|
|
+ line_filter(in_cloud, hole_step_radius,ymin,ymax, target_root, //input
|
|
|
+ target_filtered_, //output
|
|
|
+ target_filtered_root_, //output
|
|
|
+ target_member_, //output
|
|
|
+ target_filtered_models_, //output
|
|
|
+ target_filtered_stem_status //output
|
|
|
+ );
|
|
|
std::vector<pcl::PointXYZ>target_filtered;
|
|
|
std::vector<std::vector<int>> target_member;//输出，茎上点index
|
|
|
std::vector<pcl::PointXYZ>target_filtered_root;
|
|
|
std::vector<pcl::ModelCoefficients::Ptr> target_filtered_models; //茎直线模型
|
|
|
- line_filter(in_cloud, hole_step_radius,ymin,ymax, target_root, target_filtered,
|
|
|
- target_filtered_root,
|
|
|
- target_member,
|
|
|
- target_filtered_models);
|
|
|
-
|
|
|
+ for (int i = 0; i < target_filtered_stem_status.size(); ++i) {
|
|
|
+ if (target_filtered_stem_status.at(i) == 1) {
|
|
|
+ target_filtered.push_back(target_filtered_.at(i));
|
|
|
+ target_member.push_back(target_member_.at(i));
|
|
|
+ target_filtered_root.push_back(target_filtered_root_.at(i));
|
|
|
+ target_filtered_models.push_back(target_filtered_models_.at(i));
|
|
|
+ }
|
|
|
+ }
|
|
|
//根据实际得到的茎的位置，更新当前茎状态
|
|
|
- m_pSeedlingStatus->real_result_update(target_filtered_root);
|
|
|
+ m_pSeedlingStatus->real_result_update(target_filtered_root_, target_filtered_stem_status);
|
|
|
m_pSeedlingStatus->occlusion_result_update(m_root_center_with_occlusion);
|
|
|
m_pSeedlingStatus->get_stem_status(m_root_center_with_seedling);
|
|
|
if (m_pLogger) {
|
|
|
@@ -3216,17 +3304,25 @@ void CRootStockGrabPoint::line_filter(
|
|
|
void CRootStockGrabPoint::get_line_project_hist(
|
|
|
pcl::PointCloud<pcl::PointXYZ>::Ptr in_line_cloud, //input 茎上直线点云
|
|
|
pcl::ModelCoefficients::Ptr line_model, //input
|
|
|
- std::vector<int>& count_hist // 返回有效直线1mm内点云数量
|
|
|
+ std::vector<int>& count_hist, // 返回有效直线1mm内点云数量
|
|
|
+ std::vector<int>& leaf_count_hist // 返回有效直线1mm内点云数量,原始点云上做统计
|
|
|
)
|
|
|
{
|
|
|
+ //计算苗上的点云，y方向的统计，是否有点
|
|
|
+ //计算苗的前端，y方向遮挡叶子的数量
|
|
|
count_hist.clear();
|
|
|
+ leaf_count_hist.clear();
|
|
|
|
|
|
- float ymin, ymax;
|
|
|
+ float ymin, ymax, zmin, radius;
|
|
|
ymin = m_cparam.rs_grab_ymin;
|
|
|
ymax = m_cparam.rs_grab_ymax;
|
|
|
+ zmin = m_cparam.rs_grab_zmin;
|
|
|
+ radius = m_cparam.rs_grab_stem_diameter;
|
|
|
if (m_dtype == 0) {
|
|
|
ymin = m_cparam.sc_grab_ymin;
|
|
|
ymax = m_cparam.sc_grab_ymax;
|
|
|
+ zmin = m_cparam.sc_grab_zmin;
|
|
|
+ radius = m_cparam.sc_grab_stem_diameter;
|
|
|
}
|
|
|
|
|
|
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_inbox(new pcl::PointCloud<pcl::PointXYZ>);
|
|
|
@@ -3234,10 +3330,11 @@ void CRootStockGrabPoint::line_filter(
|
|
|
box_filter_line.setNegative(false);
|
|
|
box_filter_line.setInputCloud(in_line_cloud);
|
|
|
|
|
|
- float radius = m_cparam.rs_grab_stem_diameter;
|
|
|
- if (m_dtype == 0) {
|
|
|
- radius = m_cparam.sc_grab_stem_diameter;
|
|
|
- }
|
|
|
+ pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_inbox_front_leaf(new pcl::PointCloud<pcl::PointXYZ>);
|
|
|
+ pcl::CropBox<pcl::PointXYZ> box_filter_front_leaf;
|
|
|
+ box_filter_front_leaf.setNegative(false);
|
|
|
+ box_filter_front_leaf.setInputCloud(m_raw_cloud);
|
|
|
+
|
|
|
float rx = 1.5;
|
|
|
float rz = 1.5;
|
|
|
float cx, cy, cz, t;
|
|
|
@@ -3252,6 +3349,12 @@ void CRootStockGrabPoint::line_filter(
|
|
|
box_filter_line.setMax(Eigen::Vector4f(cx + rx*radius, cy + 0.5, cz + rz*radius, 1));
|
|
|
box_filter_line.filter(*cloud_inbox);
|
|
|
count_hist.push_back(cloud_inbox->points.size());
|
|
|
+
|
|
|
+ box_filter_front_leaf.setMin(Eigen::Vector4f(cx - 2*rx*radius, cy - 0.5, zmin, 1));
|
|
|
+ box_filter_front_leaf.setMax(Eigen::Vector4f(cx + 2*rx*radius, cy + 0.5, cz, 1));
|
|
|
+ box_filter_front_leaf.filter(*cloud_inbox_front_leaf);
|
|
|
+ leaf_count_hist.push_back(cloud_inbox_front_leaf->points.size());
|
|
|
+
|
|
|
cy += 1.0;
|
|
|
}
|
|
|
}
|
chenhongjiang