|
|
@@ -396,15 +396,17 @@ namespace graft_cv {
|
|
|
|
|
|
////////////////////////////////////////////////////////////////////////////////////////////////////////////////
|
|
|
//5.1 如果没有找到茎，在root_center 中找一个位置
|
|
|
- pcl::PointXYZ selected_pt;//抓取点坐标，根据茎节点的偏移
|
|
|
- pcl::PointXYZ selected_pt_ref; //返回茎节点，作为抓取点偏的基点
|
|
|
+ pcl::PointXYZ fork_selected_pt;//抓取点坐标，根据茎节点的偏移
|
|
|
+ pcl::PointXYZ fork_selected_pt_ref; //返回茎节点，作为抓取点偏的基点
|
|
|
+ pcl::PointXYZ rs_grab_pt; //v0.8.8修改砧木抓取位置为固定高度，通过yup和ybt均值确定
|
|
|
if (!fund_seedling) {
|
|
|
int selected_center_idx;
|
|
|
no_seedling_detected_post_process(
|
|
|
first_row_seedling_number, //input
|
|
|
selected_center_idx, //output
|
|
|
- selected_pt, //output
|
|
|
- selected_pt_ref, //output
|
|
|
+ fork_selected_pt, //output
|
|
|
+ fork_selected_pt_ref, //output
|
|
|
+ rs_grab_pt, //output
|
|
|
posinfo);
|
|
|
if (selected_center_idx < 0) {
|
|
|
if (m_pLogger) {
|
|
|
@@ -445,19 +447,21 @@ namespace graft_cv {
|
|
|
cloud_dowm_sampled, //input
|
|
|
cloud_seedling_seed, //input
|
|
|
line_model, //input
|
|
|
- selected_pt, //output
|
|
|
- selected_pt_ref, //output
|
|
|
+ fork_selected_pt, //output
|
|
|
+ fork_selected_pt_ref, //output
|
|
|
+ rs_grab_pt, //output
|
|
|
stem_width_mu, //output
|
|
|
stem_deflection //output
|
|
|
); //output
|
|
|
|
|
|
- if (selected_pt.z < 0) {
|
|
|
+ if (fork_selected_pt.z < 0) {
|
|
|
int selected_center_idx;
|
|
|
no_seedling_detected_post_process(
|
|
|
first_row_seedling_number, //input
|
|
|
selected_center_idx, //output
|
|
|
- selected_pt, //output
|
|
|
- selected_pt_ref, //output
|
|
|
+ fork_selected_pt, //output
|
|
|
+ fork_selected_pt_ref, //output
|
|
|
+ rs_grab_pt, //output
|
|
|
posinfo);
|
|
|
|
|
|
if (selected_center_idx < 0) {
|
|
|
@@ -476,8 +480,9 @@ namespace graft_cv {
|
|
|
stem_width_mu, //input
|
|
|
stem_deflection, //input
|
|
|
selected_center_idx, //output, 选择root_center的序号
|
|
|
- selected_pt, //input-output, 检测到的目标抓取点
|
|
|
- selected_pt_ref, //input-output, 检测到的目标抓取参考点
|
|
|
+ fork_selected_pt, //input-output, 检测到的目标抓取点
|
|
|
+ fork_selected_pt_ref, //input-output, 检测到的目标抓取参考点
|
|
|
+ rs_grab_pt,
|
|
|
posinfo);
|
|
|
}
|
|
|
|
|
|
@@ -488,7 +493,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, selected_pt, selected_pt_ref, posinfo,rst_img);
|
|
|
+ gen_result_img(cloud_ror, cloud_dowm_sampled, fork_selected_pt, fork_selected_pt_ref, rs_grab_pt, posinfo,rst_img);
|
|
|
if (m_ppImgSaver && *m_ppImgSaver) {
|
|
|
(*m_ppImgSaver)->saveImage(rst_img, m_pcdId + "_rst_0");
|
|
|
}
|
|
|
@@ -510,19 +515,19 @@ namespace graft_cv {
|
|
|
pt.b = 255;
|
|
|
}
|
|
|
pcl::PointXYZRGB pt_grab = {0,255,0};
|
|
|
- pt_grab.x = selected_pt.x;
|
|
|
- pt_grab.y = selected_pt.y;
|
|
|
- pt_grab.z = selected_pt.z;
|
|
|
+ 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 = selected_pt.x;
|
|
|
- pt_grab_.y = selected_pt.y+0.2;
|
|
|
- pt_grab_.z = selected_pt.z;
|
|
|
+ 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, selected_pt, selected_pt_ref, xz_center, 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"));
|
|
|
imshow_wait("rst_img", rst_img);
|
|
|
}
|
|
|
return 0;
|
|
|
@@ -533,9 +538,13 @@ namespace graft_cv {
|
|
|
int& selected_idx, //output, 选择root_center的序号
|
|
|
pcl::PointXYZ& selected_pt, //output
|
|
|
pcl::PointXYZ& selected_pt_ref, //output
|
|
|
+ pcl::PointXYZ& rs_grab_pt, //output
|
|
|
PositionInfo& posinfo //output
|
|
|
)
|
|
|
{
|
|
|
+ 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) {
|
|
|
@@ -578,6 +587,15 @@ namespace graft_cv {
|
|
|
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) {
|
|
|
@@ -589,9 +607,12 @@ namespace graft_cv {
|
|
|
posinfo.sc_tortuosity = (double)stem_deflection;
|
|
|
}
|
|
|
else {
|
|
|
- posinfo.rs_grab_x = selected_pt.x;
|
|
|
- posinfo.rs_grab_y = selected_pt.y;
|
|
|
- posinfo.rs_grab_z = selected_pt.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_count = (double)first_row_seedling_number;
|
|
|
posinfo.rs_width = (double)stem_width_mu;
|
|
|
posinfo.rs_tortuosity = (double)stem_deflection;
|
|
|
@@ -605,8 +626,9 @@ namespace graft_cv {
|
|
|
float stem_width_mu, //input
|
|
|
float stem_deflection, //input
|
|
|
int& selected_idx, //output, 选择root_center的序号
|
|
|
- pcl::PointXYZ& selected_pt, //input-output, 检测到的目标抓取点
|
|
|
- pcl::PointXYZ& selected_pt_ref, //input-output, 检测到的目标抓取参考点
|
|
|
+ pcl::PointXYZ& selected_pt, //input-output, 检测到的基于茎节抓取点
|
|
|
+ pcl::PointXYZ& selected_pt_ref, //input-output, 检测到的茎节点
|
|
|
+ pcl::PointXYZ& rs_grab_pt, //input-output, 检测到的目标抓取点
|
|
|
PositionInfo& posinfo //output
|
|
|
)
|
|
|
{
|
|
|
@@ -616,15 +638,18 @@ namespace graft_cv {
|
|
|
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_grab_z = selected_pt.z;
|
|
|
posinfo.sc_count = (double)first_row_seedling_number;
|
|
|
posinfo.sc_width = (double)stem_width_mu;
|
|
|
posinfo.sc_tortuosity = (double)stem_deflection;
|
|
|
}
|
|
|
else {
|
|
|
- posinfo.rs_grab_x = selected_pt.x;
|
|
|
- posinfo.rs_grab_y = selected_pt.y;
|
|
|
- posinfo.rs_grab_z = selected_pt.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.x;
|
|
|
+ posinfo.rs_fork_y = selected_pt.y;
|
|
|
+ posinfo.rs_fork_z = selected_pt.z;
|
|
|
posinfo.rs_count = (double)first_row_seedling_number;
|
|
|
posinfo.rs_width = (double)stem_width_mu;
|
|
|
posinfo.rs_tortuosity = (double)stem_deflection;
|
|
|
@@ -1001,7 +1026,8 @@ namespace graft_cv {
|
|
|
pcl::PointCloud<pcl::PointXYZ>::Ptr in_cloud_raw,//输入，未经过滤的整体点云in_cloud_raw,
|
|
|
pcl::PointCloud<pcl::PointXYZ>::Ptr in_cloud, //输入，整体点云cloud_dowm_sampled,
|
|
|
pcl::PointXYZ& selected_pt, //输入，selected_pt,
|
|
|
- pcl::PointXYZ& selected_pt_ref, //输入，selected_pt_ref,
|
|
|
+ pcl::PointXYZ& selected_pt_ref, //输入，selected_pt_ref,
|
|
|
+ pcl::PointXYZ& rs_grab_pt, //输入，砧木固定抓取点,
|
|
|
const PositionInfo& posinfo, //输入，
|
|
|
cv::Mat& rst_img //输出，图片， 640*1280
|
|
|
)
|
|
|
@@ -1049,6 +1075,11 @@ namespace graft_cv {
|
|
|
int grab_x = cx - int(selected_pt.x / res + 0.5);
|
|
|
int grab_y = cy - int(selected_pt.y / res + 0.5);
|
|
|
int radius = 30;
|
|
|
+ if (m_dtype == 1) {
|
|
|
+ //如果是砧木，固定y值的抓取点
|
|
|
+ grab_x = cx - int(rs_grab_pt.x / res + 0.5);
|
|
|
+ grab_y = cy - int(rs_grab_pt.y / res + 0.5);
|
|
|
+ }
|
|
|
cv::line(rst_img, cv::Point(grab_x - radius, grab_y - radius), cv::Point(grab_x + radius, grab_y + radius), cv::Scalar(0, 215, 255));
|
|
|
cv::line(rst_img, cv::Point(grab_x - radius, grab_y + radius), cv::Point(grab_x + radius, grab_y - radius), cv::Scalar(0, 215, 255));
|
|
|
//如果是遮挡，画一个圆
|
|
|
@@ -1783,6 +1814,8 @@ void CRootStockGrabPoint::line_filter(
|
|
|
final_box.push_back(max_point_aabb_ex);
|
|
|
}
|
|
|
viewer_cloud_cluster_box(in_cloud, target_filtered_root, final_box, target_member, std::string("line_filter"));
|
|
|
+ std::vector<std::vector<int>>target_member_empty;
|
|
|
+ viewer_cloud_cluster_box(m_raw_cloud, target_filtered_root, final_box, target_member_empty, std::string("stem_box_in_raw_pc"));
|
|
|
}
|
|
|
|
|
|
//保存茎的根部坐标和点云数量
|
|
|
@@ -2456,7 +2489,8 @@ void CRootStockGrabPoint::line_filter(
|
|
|
pcl::PointCloud<pcl::PointXYZ>::Ptr in_line_cloud, //input 茎上直线点云
|
|
|
pcl::ModelCoefficients::Ptr line_model, //input
|
|
|
pcl::PointXYZ&pt, // 返回抓取的点坐标，基于pt_ref的偏移点
|
|
|
- pcl::PointXYZ &pt_ref, // 返回点茎节的参考点
|
|
|
+ pcl::PointXYZ &pt_ref, // 返回点茎节的参考点
|
|
|
+ pcl::PointXYZ&pt_rs_grab, // 返回砧木指定的固定高度位置坐标
|
|
|
float& stem_width_mu, // 茎宽度，直径
|
|
|
float& stem_deflection //茎的最大挠度，最大弯曲处的偏离直径轴心的距离，毫米
|
|
|
)
|
|
|
@@ -2468,6 +2502,10 @@ void CRootStockGrabPoint::line_filter(
|
|
|
pt.z = -1000.0;
|
|
|
stem_width_mu = 0.0;
|
|
|
stem_deflection = 0.0;
|
|
|
+ pt_rs_grab.x = -1000.0;
|
|
|
+ pt_rs_grab.y = -1000.0;
|
|
|
+ pt_rs_grab.z = -1000.0;
|
|
|
+
|
|
|
|
|
|
|
|
|
float ymin, ymax;
|
|
|
@@ -2652,6 +2690,31 @@ void CRootStockGrabPoint::line_filter(
|
|
|
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");
|
chenhongjiang