|
|
@@ -10,6 +10,7 @@
|
|
|
|
|
|
*/
|
|
|
|
|
|
+#include <algorithm>
|
|
|
#include <pcl\io\ply_io.h>
|
|
|
#include <pcl\visualization\cloud_viewer.h>
|
|
|
#include <pcl\filters\crop_box.h>
|
|
|
@@ -399,6 +400,7 @@ namespace graft_cv {
|
|
|
pcl::PointXYZ fork_selected_pt;//抓取点坐标，根据茎节点的偏移
|
|
|
pcl::PointXYZ fork_selected_pt_ref; //返回茎节点，作为抓取点偏的基点
|
|
|
pcl::PointXYZ rs_grab_pt; //v0.8.8修改砧木抓取位置为固定高度，通过yup和ybt均值确定
|
|
|
+ std::vector<float>fork_ys;
|
|
|
if (!fund_seedling) {
|
|
|
int selected_center_idx;
|
|
|
no_seedling_detected_post_process(
|
|
|
@@ -442,7 +444,7 @@ namespace graft_cv {
|
|
|
}
|
|
|
|
|
|
float stem_width_mu;
|
|
|
- float stem_deflection;
|
|
|
+ float stem_deflection;
|
|
|
get_optimal_seed_compare(
|
|
|
cloud_dowm_sampled, //input
|
|
|
cloud_seedling_seed, //input
|
|
|
@@ -450,11 +452,12 @@ namespace graft_cv {
|
|
|
fork_selected_pt, //output
|
|
|
fork_selected_pt_ref, //output
|
|
|
rs_grab_pt, //output
|
|
|
+ fork_ys, // 返回，fork y的高度
|
|
|
stem_width_mu, //output
|
|
|
stem_deflection //output
|
|
|
); //output
|
|
|
|
|
|
- if (fork_selected_pt.z < 0) {
|
|
|
+ if (m_dtype==0 && fork_selected_pt.z < 0) {
|
|
|
int selected_center_idx;
|
|
|
no_seedling_detected_post_process(
|
|
|
first_row_seedling_number, //input
|
|
|
@@ -483,6 +486,7 @@ namespace graft_cv {
|
|
|
fork_selected_pt, //input-output, 检测到的目标抓取点
|
|
|
fork_selected_pt_ref, //input-output, 检测到的目标抓取参考点
|
|
|
rs_grab_pt,
|
|
|
+ fork_ys,
|
|
|
posinfo);
|
|
|
}
|
|
|
|
|
|
@@ -493,7 +497,7 @@ namespace graft_cv {
|
|
|
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
|
|
|
//7 保存处理结果到图片
|
|
|
cv::Mat rst_img = cv::Mat::zeros(cv::Size(1280,640),CV_8UC3);
|
|
|
- gen_result_img(cloud_ror, cloud_dowm_sampled, fork_selected_pt, fork_selected_pt_ref, rs_grab_pt, posinfo,rst_img);
|
|
|
+ gen_result_img(cloud_ror, cloud_dowm_sampled, fork_selected_pt, fork_selected_pt_ref, rs_grab_pt, fork_ys, posinfo,rst_img);
|
|
|
if (m_ppImgSaver && *m_ppImgSaver) {
|
|
|
(*m_ppImgSaver)->saveImage(rst_img, m_pcdId + "_rst_0");
|
|
|
}
|
|
|
@@ -513,21 +517,40 @@ namespace graft_cv {
|
|
|
pt.r = 255;
|
|
|
pt.g = 255;
|
|
|
pt.b = 255;
|
|
|
- }
|
|
|
- pcl::PointXYZRGB pt_grab = {0,255,0};
|
|
|
- pt_grab.x = fork_selected_pt.x;
|
|
|
- pt_grab.y = fork_selected_pt.y;
|
|
|
- pt_grab.z = fork_selected_pt.z;
|
|
|
+ }
|
|
|
+ if (m_dtype == 0) {
|
|
|
+ pcl::PointXYZRGB pt_grab = { 0,255,0 };
|
|
|
+ pt_grab.x = fork_selected_pt.x;
|
|
|
+ pt_grab.y = fork_selected_pt.y;
|
|
|
+ pt_grab.z = fork_selected_pt.z;
|
|
|
+
|
|
|
+ pcl::PointXYZRGB pt_grab_ = { 0,255,120 };
|
|
|
+ pt_grab_.x = fork_selected_pt.x;
|
|
|
+ pt_grab_.y = fork_selected_pt.y + 0.2;
|
|
|
+ pt_grab_.z = fork_selected_pt.z;
|
|
|
+ cloud_cand_demo->push_back(pt_grab);
|
|
|
+
|
|
|
+
|
|
|
+ //viewer_cloud(cloud_cand_demo, std::string("cloud_cand_demo"));
|
|
|
+ viewer_cloud_debug(cloud_cand_demo, fork_selected_pt, fork_selected_pt_ref, xz_center, std::string("cloud_cand_demo"));
|
|
|
+ }
|
|
|
+ else {
|
|
|
+ pcl::PointXYZRGB pt_grab = { 0,255,0 };
|
|
|
+ pt_grab.x = rs_grab_pt.x;
|
|
|
+ pt_grab.y = rs_grab_pt.y;
|
|
|
+ pt_grab.z = rs_grab_pt.z;
|
|
|
|
|
|
- pcl::PointXYZRGB pt_grab_ = { 0,255,120 };
|
|
|
- pt_grab_.x = fork_selected_pt.x;
|
|
|
- pt_grab_.y = fork_selected_pt.y + 0.2;
|
|
|
- pt_grab_.z = fork_selected_pt.z;
|
|
|
- cloud_cand_demo->push_back(pt_grab);
|
|
|
+ pcl::PointXYZRGB pt_grab_ = { 0,255,120 };
|
|
|
+ pt_grab_.x = rs_grab_pt.x;
|
|
|
+ pt_grab_.y = rs_grab_pt.y + 0.2;
|
|
|
+ pt_grab_.z = rs_grab_pt.z;
|
|
|
+ cloud_cand_demo->push_back(pt_grab);
|
|
|
|
|
|
|
|
|
- //viewer_cloud(cloud_cand_demo, std::string("cloud_cand_demo"));
|
|
|
- viewer_cloud_debug(cloud_cand_demo, fork_selected_pt, fork_selected_pt_ref, xz_center, std::string("cloud_cand_demo"));
|
|
|
+ //viewer_cloud(cloud_cand_demo, std::string("cloud_cand_demo"));
|
|
|
+ viewer_cloud_debug(cloud_cand_demo, rs_grab_pt, rs_grab_pt, xz_center, std::string("cloud_cand_demo"));
|
|
|
+ }
|
|
|
+
|
|
|
imshow_wait("rst_img", rst_img);
|
|
|
}
|
|
|
return 0;
|
|
|
@@ -542,9 +565,18 @@ namespace graft_cv {
|
|
|
PositionInfo& posinfo //output
|
|
|
)
|
|
|
{
|
|
|
+ selected_pt.x = -1000.0;
|
|
|
+ selected_pt.y = -1000.0;
|
|
|
+ selected_pt.z = -1000.0;
|
|
|
+
|
|
|
+ selected_pt_ref.x = -1000.0;
|
|
|
+ selected_pt_ref.y = -1000.0;
|
|
|
+ selected_pt_ref.z = -1000.0;
|
|
|
+
|
|
|
rs_grab_pt.x = -1000.0;
|
|
|
rs_grab_pt.y = -1000.0;
|
|
|
rs_grab_pt.z = -1000.0;
|
|
|
+
|
|
|
//m_dtype == 0 找最大x位置， 否则找最小x位置
|
|
|
selected_idx = -1;
|
|
|
if (m_dtype == 0) {
|
|
|
@@ -568,10 +600,12 @@ namespace graft_cv {
|
|
|
double grab_fork_ybt = m_cparam.rs_grab_fork_ybt;
|
|
|
double grab_offset = m_cparam.rs_grab_offset;
|
|
|
double grab_seedling_dist = m_cparam.rs_grab_seedling_dist;
|
|
|
+ double ymin = m_cparam.rs_grab_ymin;
|
|
|
if (m_dtype == 0) {
|
|
|
grab_fork_ybt = m_cparam.sc_grab_fork_ybt;
|
|
|
- grab_offset = m_cparam.sc_grab_offset;
|
|
|
+ grab_offset = m_cparam.sc_grab_offset_bt;
|
|
|
grab_seedling_dist = m_cparam.sc_grab_seedling_dist;
|
|
|
+ ymin = m_cparam.sc_grab_ymin;
|
|
|
}
|
|
|
|
|
|
//cx位置, 默认选择穴位中心,如果leaf中心有值，用叶子中心
|
|
|
@@ -580,25 +614,15 @@ namespace graft_cv {
|
|
|
if (fabs(leaf_cx - cx) < grab_seedling_dist*0.33) {
|
|
|
cx = leaf_cx;
|
|
|
}
|
|
|
-
|
|
|
- selected_pt_ref.x = cx;
|
|
|
- selected_pt_ref.y = grab_fork_ybt;
|
|
|
- selected_pt_ref.z = m_root_centers.at(selected_idx).root_z;
|
|
|
- selected_pt = selected_pt_ref;
|
|
|
- selected_pt.y += static_cast<int>(grab_offset);
|
|
|
-
|
|
|
- //砧木指定y高度上的确定位置
|
|
|
- if (m_dtype == 1) {
|
|
|
- float grab_y = 0.5 * (m_cparam.rs_grab_fork_ybt + m_cparam.rs_grab_fork_yup);
|
|
|
- rs_grab_pt.x = cx;
|
|
|
- rs_grab_pt.y = grab_y;
|
|
|
- rs_grab_pt.z = m_root_centers.at(selected_idx).root_z;
|
|
|
- }
|
|
|
-
|
|
|
-
|
|
|
double stem_width_mu = 0.0;
|
|
|
double stem_deflection = 0.0;
|
|
|
if (m_dtype == 0) {
|
|
|
+ selected_pt_ref.x = cx;
|
|
|
+ selected_pt_ref.y = ymin;
|
|
|
+ selected_pt_ref.z = m_root_centers.at(selected_idx).root_z;
|
|
|
+ selected_pt = selected_pt_ref;
|
|
|
+ selected_pt.y += static_cast<int>(grab_offset);
|
|
|
+
|
|
|
posinfo.sc_grab_x = selected_pt.x;
|
|
|
posinfo.sc_grab_y = selected_pt.y;
|
|
|
posinfo.sc_grab_z = selected_pt.z;
|
|
|
@@ -607,12 +631,18 @@ namespace graft_cv {
|
|
|
posinfo.sc_tortuosity = (double)stem_deflection;
|
|
|
}
|
|
|
else {
|
|
|
+ //砧木指定y高度上的确定位置
|
|
|
+ float grab_y = 0.5 * (m_cparam.rs_grab_fork_ybt + m_cparam.rs_grab_fork_yup);
|
|
|
+ rs_grab_pt.x = cx;
|
|
|
+ rs_grab_pt.y = grab_y;
|
|
|
+ rs_grab_pt.z = m_root_centers.at(selected_idx).root_z;
|
|
|
+
|
|
|
posinfo.rs_grab_x = rs_grab_pt.x;
|
|
|
posinfo.rs_grab_y = rs_grab_pt.y;
|
|
|
posinfo.rs_grab_z = rs_grab_pt.z;
|
|
|
- posinfo.rs_fork_x = selected_pt_ref.x;
|
|
|
- posinfo.rs_fork_y = selected_pt_ref.y;
|
|
|
- posinfo.rs_fork_z = selected_pt_ref.z;
|
|
|
+ posinfo.rs_fork_y1 = -1000.0;
|
|
|
+ posinfo.rs_fork_y2 = -1000.0;
|
|
|
+ posinfo.rs_fork_y3 = -1000.0;
|
|
|
posinfo.rs_count = (double)first_row_seedling_number;
|
|
|
posinfo.rs_width = (double)stem_width_mu;
|
|
|
posinfo.rs_tortuosity = (double)stem_deflection;
|
|
|
@@ -629,6 +659,7 @@ namespace graft_cv {
|
|
|
pcl::PointXYZ& selected_pt, //input-output, 检测到的基于茎节抓取点
|
|
|
pcl::PointXYZ& selected_pt_ref, //input-output, 检测到的茎节点
|
|
|
pcl::PointXYZ& rs_grab_pt, //input-output, 检测到的目标抓取点
|
|
|
+ std::vector<float>& fork_ys,
|
|
|
PositionInfo& posinfo //output
|
|
|
)
|
|
|
{
|
|
|
@@ -647,106 +678,28 @@ namespace graft_cv {
|
|
|
posinfo.rs_grab_x = rs_grab_pt.x;
|
|
|
posinfo.rs_grab_y = rs_grab_pt.y;
|
|
|
posinfo.rs_grab_z = rs_grab_pt.z;
|
|
|
- posinfo.rs_fork_x = selected_pt.x;
|
|
|
- posinfo.rs_fork_y = selected_pt.y;
|
|
|
- posinfo.rs_fork_z = selected_pt.z;
|
|
|
+ posinfo.rs_fork_y1 = -1000.0;
|
|
|
+ posinfo.rs_fork_y2 = -1000.0;
|
|
|
+ posinfo.rs_fork_y3 = -1000.0;
|
|
|
+ int fsize = fork_ys.size();
|
|
|
+ if (fsize > 3) { fsize = 3; }
|
|
|
+ if (fsize == 1) {
|
|
|
+ posinfo.rs_fork_y1 = fork_ys.at(0);
|
|
|
+ }
|
|
|
+ if (fsize == 2) {
|
|
|
+ posinfo.rs_fork_y1 = fork_ys.at(0);
|
|
|
+ posinfo.rs_fork_y2 = fork_ys.at(1);
|
|
|
+ }
|
|
|
+ if (fsize == 3) {
|
|
|
+ posinfo.rs_fork_y1 = fork_ys.at(0);
|
|
|
+ posinfo.rs_fork_y2 = fork_ys.at(1);
|
|
|
+ posinfo.rs_fork_y3 = fork_ys.at(2);
|
|
|
+ }
|
|
|
+
|
|
|
posinfo.rs_count = (double)first_row_seedling_number;
|
|
|
posinfo.rs_width = (double)stem_width_mu;
|
|
|
posinfo.rs_tortuosity = (double)stem_deflection;
|
|
|
}
|
|
|
-
|
|
|
-// // 1 找到基于rootcenter应该找的中心位置
|
|
|
-// //m_dtype == 0 找最大x位置， 否则找最小x位置
|
|
|
-// selected_idx = -1;
|
|
|
-// if (m_dtype == 0) {
|
|
|
-// for (int i = 0; i < m_root_center_with_seedling.size(); ++i) {
|
|
|
-// if (m_root_center_with_seedling[i]) { selected_idx = i; }
|
|
|
-// }
|
|
|
-// }
|
|
|
-// else {
|
|
|
-// for (int i = 0; i < m_root_center_with_seedling.size(); ++i) {
|
|
|
-// if (m_root_center_with_seedling[i]) {
|
|
|
-// selected_idx = i;
|
|
|
-// break;
|
|
|
-// }
|
|
|
-// }
|
|
|
-// }
|
|
|
-//
|
|
|
-// if (selected_idx < 0) {
|
|
|
-// //可能没有中心（刚开始工作）
|
|
|
-// //按识别的结果
|
|
|
-// return;
|
|
|
-// }
|
|
|
-//
|
|
|
-// //2 如果识别的位置和rootcenter位置接近，就按识别位置
|
|
|
-// double seedling_distance = m_cparam.rs_grab_seedling_dist;
|
|
|
-// if (m_dtype == 0) {
|
|
|
-// seedling_distance = m_cparam.sc_grab_seedling_dist;
|
|
|
-// }
|
|
|
-// double root_x = m_root_centers.at(selected_idx).root_x;
|
|
|
-// if (m_dtype == 0) {
|
|
|
-// //穗苗找x最大的位置
|
|
|
-// if (selected_pt.x > (root_x - 0.5 * seedling_distance)) {
|
|
|
-// //找到x最大的位置上的苗，识别的苗位置大于有苗的位置，就认为识别结果更可信
|
|
|
-// return;
|
|
|
-// }
|
|
|
-// else {
|
|
|
-// //否则用指定根中心的位置
|
|
|
-// goto obstructed;
|
|
|
-// }
|
|
|
-// }
|
|
|
-// else {
|
|
|
-// //砧木，找x最小的位置
|
|
|
-// if (selected_pt.x < (root_x + 0.5 * seedling_distance)) {
|
|
|
-// //找到x最大的位置上的苗，识别的苗位置大于有苗的位置，就认为识别结果更可信
|
|
|
-// return;
|
|
|
-// }
|
|
|
-// else {
|
|
|
-// //否则用指定根中心的位置
|
|
|
-// goto obstructed;
|
|
|
-// }
|
|
|
-// }
|
|
|
-//
|
|
|
-//obstructed:
|
|
|
-// double grab_fork_ybt = m_cparam.rs_grab_fork_ybt;
|
|
|
-// double grab_offset = m_cparam.rs_grab_offset;
|
|
|
-// if (m_dtype == 0) {
|
|
|
-// grab_fork_ybt = m_cparam.sc_grab_fork_ybt;
|
|
|
-// grab_offset = m_cparam.sc_grab_offset;
|
|
|
-// }
|
|
|
-//
|
|
|
-// //cx位置, 默认选择穴位中心,如果leaf中心有值，用叶子中心
|
|
|
-// double cx = m_root_centers.at(selected_idx).root_x;
|
|
|
-// double leaf_cx = m_root_center_leaf_cx.at(selected_idx);
|
|
|
-// if (fabs(leaf_cx - cx) < seedling_distance*0.33) {
|
|
|
-// cx = leaf_cx;
|
|
|
-// }
|
|
|
-//
|
|
|
-// selected_pt_ref.x = cx;
|
|
|
-// selected_pt_ref.y = grab_fork_ybt;
|
|
|
-// selected_pt_ref.z = m_root_centers.at(selected_idx).root_z;
|
|
|
-// selected_pt = selected_pt_ref;
|
|
|
-// selected_pt.y += static_cast<int>(grab_offset);
|
|
|
-//
|
|
|
-// double stem_width_mu_obstructed = 0.0;
|
|
|
-// double stem_deflection_obstructed = 0.0;
|
|
|
-// if (m_dtype == 0) {
|
|
|
-// posinfo.sc_grab_x = selected_pt.x;
|
|
|
-// posinfo.sc_grab_y = selected_pt.y;
|
|
|
-// posinfo.sc_grab_z = selected_pt.z;
|
|
|
-// posinfo.sc_count = (double)first_row_seedling_number;
|
|
|
-// posinfo.sc_width = stem_width_mu_obstructed;
|
|
|
-// posinfo.sc_tortuosity = stem_deflection_obstructed;
|
|
|
-// }
|
|
|
-// else {
|
|
|
-// posinfo.rs_grab_x = selected_pt.x;
|
|
|
-// posinfo.rs_grab_y = selected_pt.y;
|
|
|
-// posinfo.rs_grab_z = selected_pt.z;
|
|
|
-// posinfo.rs_count = (double)first_row_seedling_number;
|
|
|
-// posinfo.rs_width = stem_width_mu_obstructed;
|
|
|
-// posinfo.rs_tortuosity = stem_deflection_obstructed;
|
|
|
-// }
|
|
|
-//
|
|
|
}
|
|
|
//根据历史根的位置，计算对应位置点云数量，进而判断此位置是否有苗
|
|
|
void CRootStockGrabPoint::occluded_seedling_detect_by_leaf(
|
|
|
@@ -1028,6 +981,7 @@ namespace graft_cv {
|
|
|
pcl::PointXYZ& selected_pt, //输入，selected_pt,
|
|
|
pcl::PointXYZ& selected_pt_ref, //输入，selected_pt_ref,
|
|
|
pcl::PointXYZ& rs_grab_pt, //输入，砧木固定抓取点,
|
|
|
+ std::vector<float>&fork_ys, //输入，forkys
|
|
|
const PositionInfo& posinfo, //输入，
|
|
|
cv::Mat& rst_img //输出，图片， 640*1280
|
|
|
)
|
|
|
@@ -1089,10 +1043,20 @@ namespace graft_cv {
|
|
|
|
|
|
|
|
|
//绘制茎节点坐标
|
|
|
- int fork_x = cx - int(selected_pt_ref.x / res + 0.5);
|
|
|
- int fork_y = cy - int(selected_pt_ref.y / res + 0.5);
|
|
|
radius = 15;
|
|
|
- cv::line(rst_img, cv::Point(fork_x - radius, fork_y), cv::Point(fork_x + radius, fork_y), cv::Scalar(153, 51, 255));
|
|
|
+ if (m_dtype == 0) {
|
|
|
+ int fork_x = cx - int(selected_pt_ref.x / res + 0.5);
|
|
|
+ int fork_y = cy - int(selected_pt_ref.y / res + 0.5);
|
|
|
+
|
|
|
+ cv::line(rst_img, cv::Point(fork_x - radius, fork_y), cv::Point(fork_x + radius, fork_y), cv::Scalar(153, 51, 255));
|
|
|
+ }
|
|
|
+ else {
|
|
|
+ for (auto&fy : fork_ys) {
|
|
|
+ int fork_x = grab_x;
|
|
|
+ int fork_y = cy - int(fy / res + 0.5);
|
|
|
+ cv::line(rst_img, cv::Point(fork_x - radius, fork_y), cv::Point(fork_x + radius, fork_y), cv::Scalar(153, 51, 255));
|
|
|
+ }
|
|
|
+ }
|
|
|
|
|
|
//在图片中写入文件名字
|
|
|
cv::putText(rst_img, m_pcdId, cv::Point(20, 20), cv::FONT_HERSHEY_SIMPLEX, 0.5, cv::Scalar(128, 128, 128));
|
|
|
@@ -2491,20 +2455,23 @@ void CRootStockGrabPoint::line_filter(
|
|
|
pcl::PointXYZ&pt, // 返回抓取的点坐标，基于pt_ref的偏移点
|
|
|
pcl::PointXYZ &pt_ref, // 返回点茎节的参考点
|
|
|
pcl::PointXYZ&pt_rs_grab, // 返回砧木指定的固定高度位置坐标
|
|
|
+ std::vector<float>& fork_ys, // 返回，fork y的高度
|
|
|
float& stem_width_mu, // 茎宽度，直径
|
|
|
float& stem_deflection //茎的最大挠度，最大弯曲处的偏离直径轴心的距离，毫米
|
|
|
)
|
|
|
- {
|
|
|
- // stem_deflection 的计算方法：计算点云到拟合轴线的平均距离
|
|
|
- float th_ratio = 1.5; //原始点云和直线点云邻域尺寸增加原来的20%，不能当做抓取点
|
|
|
+ {
|
|
|
pt.x = -1000.0;
|
|
|
pt.y = -1000.0;
|
|
|
pt.z = -1000.0;
|
|
|
- stem_width_mu = 0.0;
|
|
|
- stem_deflection = 0.0;
|
|
|
+ pt_ref.x = -1000.0;
|
|
|
+ pt_ref.y = -1000.0;
|
|
|
+ pt_ref.z = -1000.0;
|
|
|
pt_rs_grab.x = -1000.0;
|
|
|
pt_rs_grab.y = -1000.0;
|
|
|
pt_rs_grab.z = -1000.0;
|
|
|
+ fork_ys.clear();
|
|
|
+ stem_width_mu = 0.0;
|
|
|
+ stem_deflection = 0.0;
|
|
|
|
|
|
|
|
|
|
|
|
@@ -2537,10 +2504,12 @@ void CRootStockGrabPoint::line_filter(
|
|
|
float rz = 1.5;
|
|
|
float dz, dx;
|
|
|
|
|
|
- // 如果opt_y_valid==false，就是用户没有指定抓取的y高度
|
|
|
- float max_dist_to_boundary = 0.0;
|
|
|
- int max_idx_to_boundary = -1;
|
|
|
- find_fork(in_line_cloud, max_dist_to_boundary, max_idx_to_boundary);
|
|
|
+ // 如果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));
|
|
|
|
|
|
@@ -2557,7 +2526,6 @@ void CRootStockGrabPoint::line_filter(
|
|
|
box_filter.filter(*cloud_inbox);
|
|
|
|
|
|
if (cloud_inbox->points.size() > 10) {
|
|
|
-
|
|
|
Eigen::Vector4f min_point;
|
|
|
Eigen::Vector4f max_point;
|
|
|
pcl::getMinMax3D(*cloud_inbox, min_point, max_point);
|
|
|
@@ -2582,19 +2550,6 @@ void CRootStockGrabPoint::line_filter(
|
|
|
float mu = get_hist_mean(valid_stem_width);
|
|
|
stem_width_mu = mu;
|
|
|
float stdv = get_hist_std(valid_stem_width, mu);
|
|
|
-
|
|
|
- //0)计算点云到轴线的最大距离
|
|
|
-
|
|
|
-
|
|
|
- //1) original max position
|
|
|
- int max_pos = std::max_element(stem_width.begin(), stem_width.end()) - stem_width.begin();
|
|
|
- int max_pos_ref = max_pos;
|
|
|
-
|
|
|
- //3 如果fork方法得到位置信息，进行更新，并记录历史
|
|
|
- if (max_idx_to_boundary >= 0) {
|
|
|
- max_pos = int(in_line_cloud->points.at(max_idx_to_boundary).y + 0.5 - ymin);
|
|
|
- max_pos_ref = max_pos;
|
|
|
- }
|
|
|
|
|
|
|
|
|
//4 用茎节上下限高度值进行约束，如果超限，用最低限高度作为茎节高度
|
|
|
@@ -2604,69 +2559,85 @@ void CRootStockGrabPoint::line_filter(
|
|
|
grab_fork_yup = m_cparam.sc_grab_fork_yup;
|
|
|
grab_fork_ybt = m_cparam.sc_grab_fork_ybt;
|
|
|
}
|
|
|
- bool out_of_range = false;
|
|
|
- if ((max_pos + ymin) > grab_fork_yup) {
|
|
|
- out_of_range = true;
|
|
|
- int original_max_pos = max_pos;
|
|
|
- max_pos = int(grab_fork_yup - ymin + 0.5);
|
|
|
- max_pos_ref = max_pos;
|
|
|
- if (m_pLogger) {
|
|
|
- stringstream buff;
|
|
|
- buff << m_pcdId << ": warning，self fork postiont = " << original_max_pos <<
|
|
|
- ", USE up limit fork postiont " << max_pos <<
|
|
|
- ", valid fork postiont range:[" << int(grab_fork_ybt - ymin + 0.5) <<
|
|
|
- ", " << int(grab_fork_yup - ymin + 0.5) << "]";
|
|
|
- m_pLogger->INFO(buff.str());
|
|
|
+
|
|
|
+ float z_mu = 0.0;
|
|
|
+ float x_mu = 0.0;
|
|
|
+ Eigen::Vector4f min_point;
|
|
|
+ Eigen::Vector4f max_point;
|
|
|
+ if (m_dtype == 1) {
|
|
|
+ float grab_y = 0.5 * (m_cparam.rs_grab_fork_ybt + m_cparam.rs_grab_fork_yup);
|
|
|
+ int grab_y_pos = int(grab_y - m_cparam.rs_grab_ymin);
|
|
|
+ cx = online_points.at(grab_y_pos).x;
|
|
|
+ cy = online_points.at(grab_y_pos).y;
|
|
|
+ cz = online_points.at(grab_y_pos).z;
|
|
|
+ box_filter_line.setMin(Eigen::Vector4f(cx - rx*radius, cy - 2, cz - rz*radius, 1));
|
|
|
+ box_filter_line.setMax(Eigen::Vector4f(cx + rx*radius, cy + 2, cz + rz*radius, 1));
|
|
|
+ box_filter_line.filter(*cloud_inbox_line);
|
|
|
+
|
|
|
+ z_mu = cz;
|
|
|
+ x_mu = cx;
|
|
|
+ if (cloud_inbox_line->points.size() > 5) {
|
|
|
+ pcl::getMinMax3D(*cloud_inbox_line, min_point, max_point);
|
|
|
+ z_mu = 0.5 * (max_point(2) + min_point(2));
|
|
|
+ x_mu = 0.5 * (max_point(0) + min_point(0));
|
|
|
}
|
|
|
- }
|
|
|
- else {
|
|
|
|
|
|
- if ((max_pos + ymin) < grab_fork_ybt) {
|
|
|
- out_of_range = true;
|
|
|
- int original_max_pos = max_pos;
|
|
|
- max_pos = int(grab_fork_ybt - ymin + 0.5);
|
|
|
- max_pos_ref = max_pos;
|
|
|
- if (m_pLogger) {
|
|
|
- stringstream buff;
|
|
|
- buff << m_pcdId << ": warning，self fork postiont = " << original_max_pos <<
|
|
|
- ", USE bottom limit fork postiont " << max_pos <<
|
|
|
- ", valid fork postiont range:[" << int(grab_fork_ybt - ymin + 0.5) <<
|
|
|
- ", " << int(grab_fork_yup - ymin + 0.5) << "]";
|
|
|
- m_pLogger->INFO(buff.str());
|
|
|
+ pt_rs_grab.x = x_mu;
|
|
|
+ pt_rs_grab.y = cy;
|
|
|
+ pt_rs_grab.z = z_mu;
|
|
|
+
|
|
|
+ //显示位置
|
|
|
+ if (m_cparam.image_show) {
|
|
|
+ pcl::visualization::PCLVisualizer viewer("grab line");
|
|
|
+ viewer.addCoordinateSystem();
|
|
|
+ viewer.addPointCloud<pcl::PointXYZ>(in_line_cloud, "stem_cloud");//????????????????
|
|
|
+ viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 1, 1, 1, "stem_cloud");
|
|
|
+ viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "stem_cloud");
|
|
|
+
|
|
|
+ viewer.addLine(online_points.front(), online_points.back(), 1.0, 0.0, 0.0);
|
|
|
+
|
|
|
+ while (!viewer.wasStopped()) {
|
|
|
+ viewer.spinOnce(100);
|
|
|
+ boost::this_thread::sleep(boost::posix_time::microseconds(100000));
|
|
|
}
|
|
|
}
|
|
|
+
|
|
|
+ return;
|
|
|
+ }
|
|
|
+ //穗苗处理
|
|
|
+ int max_pos = 0;
|
|
|
+ int max_pos_ref = max_pos;
|
|
|
+ double offset = 0.0;
|
|
|
+ double fork_y = 0.0;
|
|
|
+ if (fork_positions.size() == 0) {
|
|
|
+ max_pos = 0;
|
|
|
+ max_pos_ref = max_pos;
|
|
|
+ offset = m_cparam.sc_grab_offset_nofork;
|
|
|
+ }
|
|
|
+ else {
|
|
|
+ max_pos = *std::min_element(fork_positions.begin(), fork_positions.end());
|
|
|
+ max_pos_ref = max_pos;
|
|
|
+ fork_y = max_pos + ymin;
|
|
|
+ if (fork_y > grab_fork_yup) {
|
|
|
+ offset = m_cparam.sc_grab_offset_up;
|
|
|
+ }
|
|
|
else {
|
|
|
- if (m_pLogger) {
|
|
|
- stringstream buff;
|
|
|
- buff << m_pcdId << ": self fork postiont = " << max_pos <<
|
|
|
- ", valid fork postiont range:[" << int(grab_fork_ybt - ymin + 0.5) <<
|
|
|
- ", " << int(grab_fork_yup - ymin + 0.5) << "]";
|
|
|
- m_pLogger->INFO(buff.str());
|
|
|
+ if (fork_y < grab_fork_ybt) {
|
|
|
+ offset = m_cparam.sc_grab_offset_bt;
|
|
|
+ }
|
|
|
+ else {
|
|
|
+ offset = m_cparam.sc_grab_offset_normal;
|
|
|
}
|
|
|
}
|
|
|
}
|
|
|
-
|
|
|
|
|
|
//5 按指定量偏移
|
|
|
- if(m_dtype == 0){
|
|
|
- max_pos_ref = max_pos;
|
|
|
- max_pos += static_cast<int>(m_cparam.sc_grab_offset);
|
|
|
- }
|
|
|
- else{
|
|
|
- max_pos_ref = max_pos;
|
|
|
- max_pos += static_cast<int>(m_cparam.rs_grab_offset);
|
|
|
- }
|
|
|
+ max_pos += static_cast<int>(offset);
|
|
|
if (max_pos < 0) { max_pos = 0; }
|
|
|
if (max_pos >= online_points.size()) { max_pos = online_points.size() - 1; }
|
|
|
- if (out_of_range) {
|
|
|
- max_pos_ref = max_pos;
|
|
|
- }
|
|
|
|
|
|
-
|
|
|
/////////////////////////////////////////////////////////////////////
|
|
|
- //直线点云，获取指定区域的dx_line，dz_line
|
|
|
- Eigen::Vector4f min_point;
|
|
|
- Eigen::Vector4f max_point;
|
|
|
+ //直线点云，获取指定区域的dx_line，dz_line
|
|
|
cx = online_points.at(max_pos).x;
|
|
|
cy = online_points.at(max_pos).y;
|
|
|
cz = online_points.at(max_pos).z;
|
|
|
@@ -2674,8 +2645,8 @@ void CRootStockGrabPoint::line_filter(
|
|
|
box_filter_line.setMax(Eigen::Vector4f(cx + rx*radius, cy + 2, cz + rz*radius, 1));
|
|
|
box_filter_line.filter(*cloud_inbox_line);
|
|
|
|
|
|
- float z_mu = cz;
|
|
|
- float x_mu = cx;
|
|
|
+ z_mu = cz;
|
|
|
+ x_mu = cx;
|
|
|
if (cloud_inbox_line->points.size() > 5) {
|
|
|
pcl::getMinMax3D(*cloud_inbox_line, min_point, max_point);
|
|
|
z_mu = 0.5 * (max_point(2) + min_point(2));
|
|
|
@@ -2689,32 +2660,7 @@ void CRootStockGrabPoint::line_filter(
|
|
|
pt_ref.x = online_points.at(max_pos_ref).x;
|
|
|
pt_ref.y = online_points.at(max_pos_ref).y;
|
|
|
pt_ref.z = online_points.at(max_pos_ref).z;
|
|
|
-
|
|
|
- //砧木指定y高度上的确定位置
|
|
|
- if (m_dtype == 1) {
|
|
|
- float grab_y = 0.5 * (m_cparam.rs_grab_fork_ybt + m_cparam.rs_grab_fork_yup);
|
|
|
- int grab_y_pos = int(grab_y - m_cparam.rs_grab_ymin);
|
|
|
- cx = online_points.at(grab_y_pos).x;
|
|
|
- cy = online_points.at(grab_y_pos).y;
|
|
|
- cz = online_points.at(grab_y_pos).z;
|
|
|
- box_filter_line.setMin(Eigen::Vector4f(cx - rx*radius, cy - 2, cz - rz*radius, 1));
|
|
|
- box_filter_line.setMax(Eigen::Vector4f(cx + rx*radius, cy + 2, cz + rz*radius, 1));
|
|
|
- box_filter_line.filter(*cloud_inbox_line);
|
|
|
-
|
|
|
- z_mu = cz;
|
|
|
- x_mu = cx;
|
|
|
- if (cloud_inbox_line->points.size() > 5) {
|
|
|
- pcl::getMinMax3D(*cloud_inbox_line, min_point, max_point);
|
|
|
- z_mu = 0.5 * (max_point(2) + min_point(2));
|
|
|
- x_mu = 0.5 * (max_point(0) + min_point(0));
|
|
|
- }
|
|
|
-
|
|
|
- pt_rs_grab.x = x_mu;
|
|
|
- pt_rs_grab.y = cy;
|
|
|
- pt_rs_grab.z = z_mu;
|
|
|
-
|
|
|
- }
|
|
|
-
|
|
|
+
|
|
|
//显示位置
|
|
|
if (m_cparam.image_show) {
|
|
|
pcl::visualization::PCLVisualizer viewer("grab line");
|
|
|
@@ -2763,10 +2709,12 @@ void CRootStockGrabPoint::line_filter(
|
|
|
|
|
|
void CRootStockGrabPoint::find_fork(
|
|
|
pcl::PointCloud<pcl::PointXYZ>::Ptr in_line_cloud,
|
|
|
- float& max_dist,
|
|
|
- int& max_idx
|
|
|
+ //float& max_dist,
|
|
|
+ //int& max_idx,
|
|
|
+ std::vector<int>&fork_pos //输出， 多个fork位置
|
|
|
)
|
|
|
{
|
|
|
+ fork_pos.clear();
|
|
|
//1 project to xy plane
|
|
|
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_line_2d(new pcl::PointCloud<pcl::PointXYZ>);
|
|
|
pcl::copyPointCloud(*in_line_cloud, *cloud_line_2d);
|
|
|
@@ -2822,8 +2770,16 @@ void CRootStockGrabPoint::line_filter(
|
|
|
pcl::search::KdTree<pcl::PointXYZ>::Ptr bound_kdtree_ptr(new pcl::search::KdTree<pcl::PointXYZ>);
|
|
|
bound_kdtree_ptr->setInputCloud(cloud_lines);
|
|
|
|
|
|
- max_idx = -1;
|
|
|
- max_dist = 0.0;
|
|
|
+ //构建基于ymin的vector记录每一个毫米中的最大距离边缘的距离
|
|
|
+ double ymin = m_cparam.rs_grab_ymin;
|
|
|
+ double ymax = m_cparam.rs_grab_ymax;
|
|
|
+ if (m_dtype == 0) {
|
|
|
+ ymin = m_cparam.sc_grab_ymin;
|
|
|
+ ymax = m_cparam.sc_grab_ymax;
|
|
|
+ }
|
|
|
+ int length = int(ymax - ymin);
|
|
|
+ std::vector<float> stem_diameters;
|
|
|
+ stem_diameters.assign(length, 0.0);
|
|
|
int k = 1;
|
|
|
for (int i = 0; i < cloud_line_2d->size(); ++i) {
|
|
|
if (boundaries->points.at(i).boundary_point != 0) { continue; }
|
|
|
@@ -2831,26 +2787,61 @@ void CRootStockGrabPoint::line_filter(
|
|
|
std::vector<float> sqr_distances(k);
|
|
|
int cnt = bound_kdtree_ptr->nearestKSearch(cloud_line_2d->points.at(i), k,idx,sqr_distances);
|
|
|
if (cnt > 0) {
|
|
|
- if (sqr_distances.at(0) > max_dist) {
|
|
|
- max_dist = sqr_distances.at(0);
|
|
|
- max_idx = i;
|
|
|
+ float stem_r = sqrt(sqr_distances.at(0));
|
|
|
+ int idx = int(cloud_line_2d->points.at(i).y - ymin);
|
|
|
+ if (idx >= length) { continue; }
|
|
|
+ if (stem_diameters.at(idx) < stem_r) {
|
|
|
+ stem_diameters.at(idx) = stem_r;
|
|
|
}
|
|
|
}
|
|
|
}
|
|
|
+ //计算平均半径
|
|
|
+ float mean_r = 0.0;
|
|
|
+ float cnt = 0.0;
|
|
|
+ for (auto&r : stem_diameters) {
|
|
|
+ if (r > 0.0) {
|
|
|
+ mean_r += r;
|
|
|
+ cnt += 1.0;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ if (cnt > 0.0) {
|
|
|
+ mean_r /= cnt;
|
|
|
+ }
|
|
|
+ float th_fork = 1.5;
|
|
|
+ int del_radius = 4;
|
|
|
+ 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);
|
|
|
+ for (int k = -del_radius; k <= del_radius; ++k) {
|
|
|
+ int j = k + midx;
|
|
|
+ if (j < 0 || j >= length) {
|
|
|
+ continue;
|
|
|
+ }
|
|
|
+ stem_diameters.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");
|
|
|
|
|
|
- if (max_idx >= 0) {
|
|
|
+ for(auto&h: fork_pos) {
|
|
|
pcl::PointXYZ p0,p1;
|
|
|
- p0.x = cloud_line_2d->points.at(max_idx).x-5;
|
|
|
- p0.y = cloud_line_2d->points.at(max_idx).y;
|
|
|
- p0.z = cloud_line_2d->points.at(max_idx).z;
|
|
|
+ p0.x = cloud_line_2d->points.at(0).x-5;
|
|
|
+ p0.y = ymin + h;
|
|
|
+ p0.z = cloud_line_2d->points.at(0).z;
|
|
|
|
|
|
- p1.x = cloud_line_2d->points.at(max_idx).x + 5;
|
|
|
- p1.y = cloud_line_2d->points.at(max_idx).y;
|
|
|
- p1.z = cloud_line_2d->points.at(max_idx).z;
|
|
|
+ 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);
|
|
|
}
|
|
|
|
chenhongjiang