v0.7.14 增加指定抓取位置的功能，抓取位置在ymax向下指定距离为抓取位置

ReadMe.txt

@@ -98,4 +98,5 @@ v0.7.9
 v0.7.10 增加茎节点位置绘制输出；茎节判别仅按dx方向大小判断（dz误差较大）
 v0.7.11 增加棋盘格识别，用于标定辅助
 v0.7.12 增加茎节peakfind功能
-v0.7.13 修改sc_grab_offset不起作用的bug
+v0.7.13 修改sc_grab_offset不起作用的bug
+v0.7.14 增加指定抓取位置的功能，抓取位置在ymax向下指定距离为抓取位置

grab_point_rs.cpp

@@ -2339,17 +2339,57 @@ void CRootStockGrabPoint::line_filter(
 
 		float radius = m_cparam.rs_grab_stem_diameter;
 		float opt_y = m_cparam.rs_grab_y_opt;
+		bool opt_y_valid = false;
+		if (opt_y >= m_cparam.rs_grab_ymin && opt_y <= m_cparam.rs_grab_ymax) {
+			opt_y_valid = true;
+		}
 		if (m_dtype == 0) {
 			radius = m_cparam.sc_grab_stem_diameter;
 			opt_y = m_cparam.sc_grab_y_opt;
+			if (opt_y >= m_cparam.sc_grab_ymin && opt_y <= m_cparam.sc_grab_ymax) {
+				opt_y_valid = true;
+			}
 		}
+
+		float cx, cy, cz, t;
 		float rx = 1.5;
 		float ry = 1.5;
 		float rz = 1.5;
-		float cx, cy, cz, t;
 		float dz, dx;
-		
-		cy = ymin;		
+		//用户指定抓取的y高度,按抓取高度找到对应的点， pt_ref参考位置为上顶点，pt为抓取点
+		if (opt_y_valid) {
+			cy = opt_y;
+			t = (cy - line_model->values.at(1)) / line_model->values.at(4);
+			cx = line_model->values.at(3) * t + line_model->values.at(0);
+			cz = line_model->values.at(5) * t + line_model->values.at(2);
+
+			Eigen::Vector4f min_point;
+			Eigen::Vector4f max_point;
+			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);
+
+			float z_mu = cz;
+			float 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.x = x_mu;
+			pt.y = cy;
+			pt.z = z_mu;
+
+			pt_ref.x = x_mu;
+			pt_ref.y = cy;
+			pt_ref.z = z_mu;			
+			return;
+		}
+
+		// 如果opt_y_valid==false，就是用户没有指定抓取的y高度		
+		cy = ymin;	
+		//计算茎上的直径
 		while(cy<ymax){
 			t = (cy - line_model->values.at(1)) / line_model->values.at(4);
 			cx = line_model->values.at(3) * t + line_model->values.at(0);			
@@ -2384,6 +2424,7 @@ void CRootStockGrabPoint::line_filter(
 
 			cy += 1.0;
 		}
+		//得到有效直径数值，并计算均值、标准差
 		std::vector<float>valid_stem_width;
 		for (auto&w : stem_width) {
 			if (w > 0) {

graft_cv_api.cpp

@@ -20,7 +20,7 @@ extern CRITICAL_SECTION g_cs;
 namespace graft_cv
 {
 
-	char *g_version_str = "0.7.13";
+	char *g_version_str = "0.7.14";
 
 	//configure
 	string g_conf_file = "./gcv_conf.yml";