v0.7.10 增加茎节点位置绘制输出；茎节判别仅按dx方向大小判断（dz误差较大）

ReadMe.txt

@@ -94,4 +94,5 @@ v0.7.5
 v0.7.6 优化叶片剔除功能,修改ror,nb_point的数量系数，通过配置文件sc_grab_ror_ratio参数传入
 v0.7.7 优化茎判别问题：上下2片叶子，利用有效高度剔除；偏离z中心剔除，采用历史z均值
 v0.7.8 修改接穗抓点靠上一点
-v0.7.9 修改接穗抓点靠上一点，增加了grab_offset，用来在找到茎节后的上下偏移得到抓取点，+值向上偏，-值向下偏
+v0.7.9 修改接穗抓点靠上一点，增加了grab_offset，用来在找到茎节后的上下偏移得到抓取点，+值向上偏，-值向下偏
+v0.7.10 增加茎节点位置绘制输出；茎节判别仅按dx方向大小判断（dz误差较大）

gcv_conf.yml

@@ -65,8 +65,8 @@ conf_parameters:
    rs_oa_pixel_ratio: 2.3333300000000001e-001
    rs_cut_pixel_ratio: 5.8479999999999997e-002
    sc_cut_pixel_ratio: 8.7499999999999994e-002
-   rs_grab_xmin: -50
-   rs_grab_xmax: 200
+   rs_grab_xmin: -100
+   rs_grab_xmax: 250
    rs_grab_ymin: -45
    rs_grab_ymax: 0
    rs_grab_zmin: 300
@@ -77,8 +77,8 @@ conf_parameters:
    rs_grab_stem_min_pts: 50
    rs_grab_ror_ratio: 0.8
    rs_grab_offset: -5.0
-   sc_grab_xmin: -50
-   sc_grab_xmax: 200
+   sc_grab_xmin: -100
+   sc_grab_xmax: 220
    sc_grab_ymin: -45
    sc_grab_ymax: 45
    sc_grab_zmin: 300

grab_point_rs.cpp

@@ -298,18 +298,18 @@ namespace graft_cv {
 			viewer_cloud_cluster(tttp, tmp, string("first"));
 		}
 
-		pcl::PointXYZ selected_pt;
-		int selected_pt_idx;
+		pcl::PointXYZ selected_pt;//抓取点坐标，根据茎节点的偏移
+		pcl::PointXYZ selected_pt_ref;	//返回茎节点，作为抓取点偏的基点
 		std::vector<int> optimal_seeds_idx;
 		get_optimal_seed_compare(
 			cloud_dowm_sampled,		//input
 			cloud_seedling_seed,	//input
 			line_model,				//input
 			selected_pt,			//output
-			selected_pt_idx,		//output
+			selected_pt_ref,		//output
 			optimal_seeds_idx);		//output
 		
-		if (selected_pt_idx < 0) {	
+		if (selected_pt.z < 0) {
 			if (m_pLogger) {
 				m_pLogger->ERRORINFO(m_pcdId + ": Not found valid fork point");
 			}
@@ -331,7 +331,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, rst_img);
+		gen_result_img(cloud_ror, cloud_dowm_sampled, selected_pt, selected_pt_ref,rst_img);
 		if (m_ppImgSaver && *m_ppImgSaver) {
 			(*m_ppImgSaver)->saveImage(rst_img, m_pcdId + "_rst_0");
 		}
@@ -375,7 +375,7 @@ namespace graft_cv {
 
 
 			//viewer_cloud(cloud_cand_demo, std::string("cloud_cand_demo"));
-			viewer_cloud_debug(cloud_cand_demo, selected_pt, xz_center, std::string("cloud_cand_demo"));
+			viewer_cloud_debug(cloud_cand_demo, selected_pt, selected_pt_ref, xz_center, std::string("cloud_cand_demo"));
 			imshow_wait("rst_img", rst_img);
 		}
 		return 0;
@@ -386,6 +386,7 @@ 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, 
 		cv::Mat& rst_img						//输出，图片， 640*1280
 	)
 	{
@@ -434,6 +435,13 @@ namespace graft_cv {
 		int radius = 30;
 		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));
+
+		//绘制茎节点坐标
+		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));
+		
 	}
 	int CRootStockGrabPoint::read_ply_file(const char* fn)
 	{
@@ -2295,14 +2303,17 @@ void CRootStockGrabPoint::line_filter(
 		pcl::PointCloud<pcl::PointXYZ>::Ptr in_cloud,		//input	全部有效点云
 		pcl::PointCloud<pcl::PointXYZ>::Ptr in_line_cloud,	//input	茎上直线点云
 		pcl::ModelCoefficients::Ptr line_model,				//input
-		pcl::PointXYZ&pt,									//  返回抓取的点坐标
-		int &pt_idx,										//	返回点index
+		pcl::PointXYZ&pt,									//  返回抓取的点坐标，基于pt_ref的偏移点
+		pcl::PointXYZ &pt_ref,										//	返回点茎节的参考点
 		std::vector<int>& valid_line_index					//  返回有效直线点index
 	)
 	{
 		valid_line_index.clear();
 		float th_ratio = 2.5;		//原始点云和直线点云邻域尺寸增加原来的20%，不能当做抓取点
-		pt_idx = -1;
+		pt.x = -1000.0;
+		pt.y = -1000.0;
+		pt.z = -1000.0;
+
 
 		float ymin, ymax;
 		ymin = m_cparam.rs_grab_ymin;
@@ -2352,15 +2363,17 @@ void CRootStockGrabPoint::line_filter(
 
 				Eigen::Vector4f min_point;
 				Eigen::Vector4f max_point;
-				pcl::getMinMax3D(*cloud_inbox, min_point, max_point);
-				dz = max_point(2) - min_point(2);
+				pcl::getMinMax3D(*cloud_inbox, min_point, max_point);				
 				dx = max_point(0) - min_point(0);
+				stem_width.push_back(dx);
+
+				/*dz = max_point(2) - min_point(2);
 				if (dx > dz) {
 					stem_width.push_back(dx);
 				}
 				else {
 					stem_width.push_back(dz);
-				}				
+				}			*/	
 			}
 			else {
 				stem_width.push_back(0);
@@ -2380,6 +2393,8 @@ void CRootStockGrabPoint::line_filter(
 		float stdv = get_hist_std(valid_stem_width, mu);
 
 		int max_pos = std::max_element(stem_width.begin(), stem_width.end()) - stem_width.begin();
+		int max_pos_ref = max_pos;
+
 		float max_val = stem_width.at(max_pos);
 		float th = mu + th_ratio * stdv;
 		if(m_dtype == 0){
@@ -2404,6 +2419,7 @@ void CRootStockGrabPoint::line_filter(
 				}
 				max_pos = i;
 			}
+			max_pos_ref = max_pos;
 			max_pos += static_cast<int>(m_cparam.rs_grab_offset);			
 		}
 		else{
@@ -2418,6 +2434,7 @@ void CRootStockGrabPoint::line_filter(
 				}
 				max_pos = i;
 			}
+			max_pos_ref = max_pos;
 			max_pos += static_cast<int>(m_cparam.rs_grab_offset);
 		}
 		if (max_pos < 0) { max_pos = 0;	}
@@ -2446,7 +2463,9 @@ void CRootStockGrabPoint::line_filter(
 		pt.y = cy;
 		pt.z = z_mu;
 
-		pt_idx = max_pos;	
+		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;
 
 		//显示位置
 		if (m_cparam.image_show) {
@@ -2527,7 +2546,8 @@ void CRootStockGrabPoint::line_filter(
 	}
 	void CRootStockGrabPoint::viewer_cloud_debug(
 		pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud, 
-		pcl::PointXYZ &p, 
+		pcl::PointXYZ &p,		//抓取点
+		pcl::PointXYZ &p_ref,//分叉点
 		pcl::PointXYZ &root_pt,
 		std::string&winname)
 	{
@@ -2564,11 +2584,27 @@ void CRootStockGrabPoint::line_filter(
 		root_px1.z = root_pt.z;
 
 		root_py0.x = root_pt.x;
-		root_py0.y = root_pt.y - 5.0;
-		root_py0.z = root_pt.z;
+		root_py0.y = root_pt.y;
+		root_py0.z = root_pt.z - 5.0;
 		root_py1.x = root_pt.x;
-		root_py1.y = root_pt.y + 5.0;
-		root_py1.z = root_pt.z;
+		root_py1.y = root_pt.y;
+		root_py1.z = root_pt.z + 5.0;
+
+		//茎节点
+		pcl::PointXYZ fork_px0, fork_px1, fork_py0, fork_py1;
+		fork_px0.x = p_ref.x - 5.0;
+		fork_px0.y = p_ref.y;
+		fork_px0.z = p_ref.z;
+		fork_px1.x = p_ref.x + 5.0;
+		fork_px1.y = p_ref.y;
+		fork_px1.z = p_ref.z;
+
+		fork_py0.x = p_ref.x;
+		fork_py0.y = p_ref.y;
+		fork_py0.z = p_ref.z - 5.0;
+		fork_py1.x = p_ref.x;
+		fork_py1.y = p_ref.y;
+		fork_py1.z = p_ref.z + 5.0;
 
 
 		viewer.addLine(p0, x1, 255, 0, 0, "x");
@@ -2580,6 +2616,9 @@ void CRootStockGrabPoint::line_filter(
 		viewer.addLine(root_px0, root_px1, 255, 0, 0, "rootx");
 		viewer.addLine(root_py0, root_py1, 0, 255, 0, "rooty");
 
+		viewer.addLine(fork_px0, fork_px1, 255, 0, 0, "forkx");
+		viewer.addLine(fork_py0, fork_py1, 0, 255, 0, "forky");
+
 		while (!viewer.wasStopped()) {
 			viewer.spinOnce(100);
  			boost::this_thread::sleep(boost::posix_time::microseconds(100000));

grab_point_rs.h

@@ -190,9 +190,9 @@ namespace graft_cv {
 			pcl::PointCloud<pcl::PointXYZ>::Ptr in_cloud,		//input
 			pcl::PointCloud<pcl::PointXYZ>::Ptr in_line_cloud,	//input
 			pcl::ModelCoefficients::Ptr line_model,			//input
-			pcl::PointXYZ&pt,
-			int &pt_idx,
-			std::vector<int>& valid_line_index
+			pcl::PointXYZ&pt,								//输出，
+			pcl::PointXYZ &pt_ref,							//输出，	返回点茎节的参考点
+			std::vector<int>& valid_line_index				//输出，
 		);
 		//在一株苗的空间范围内找出直线（茎，假设茎是直线分布的），并返回直线上的点index
 		void find_line_in_cube(
@@ -207,6 +207,7 @@ namespace graft_cv {
 			pcl::PointCloud<pcl::PointXYZ>::Ptr in_cloud_raw,//输入，未经过滤的整体点云in_cloud_raw, 
 			pcl::PointCloud<pcl::PointXYZ>::Ptr,	//输入，整体点云cloud_dowm_sampled, 
 			pcl::PointXYZ& selected_pt,				//输入，selected_pt, 
+			pcl::PointXYZ& selected_pt_ref,        //输入，selected_pt_ref,
 			cv::Mat& rst_img						//输出，图片， 640*1280
 		);
 
@@ -215,7 +216,8 @@ namespace graft_cv {
 
 		void viewer_cloud_debug(
 			pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud, 
-			pcl::PointXYZ&p,
+			pcl::PointXYZ&p,//抓取点
+			pcl::PointXYZ &p_ref,//分叉点
 			pcl::PointXYZ &root_pt,
 			std::string&winname);
 

graft_cv_api.cpp

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