v0.7.3 修改抓取位置，提供茎节分叉的位置（以前提供可抓取的位置，避开茎节的点）

ReadMe.txt

@@ -87,4 +87,5 @@ v0.6.15
 v0.6.16 加强苗的识别，增加倾斜苗的检测,修改排序错乱问题
 v0.7.0 支持多线程调用业务接口
 v0.7.1 增加voxelsize参数；修改茎密度最大值的1/3为最小茎密度的下限（原来为1/2，有些小苗识别不出来）
-v0.7.2 增加结果图片输出功能；日志增加image_id信息
+v0.7.2 增加结果图片输出功能；日志增加image_id信息
+v0.7.3 修改抓取位置，提供茎节分叉的位置（以前提供可抓取的位置，避开茎节的点）

grab_point_rs.cpp

@@ -225,10 +225,11 @@ namespace graft_cv {
 		//4 seedling position，找到第一个位置上的植株
 		std::vector<int> first_seedling_cloud_idx;	//存储找到的第一个位置上植株茎上直线点的index
 		pcl::PointXYZ xz_center;					//存储找到的第一个位置上植株根部坐标
+		pcl::ModelCoefficients::Ptr line_model;		//存储找到的第一个位置上植株茎直线模型
 		if (m_pLogger) {
 			m_pLogger->INFO(m_pcdId + ": begin find seedling position");
 		}		
-		find_seedling_position_key(cloud_dowm_sampled, first_seedling_cloud_idx, xz_center);
+		find_seedling_position_key(cloud_dowm_sampled, first_seedling_cloud_idx, xz_center, line_model);
 		if (m_pLogger) {
 			stringstream buff;
 			buff << m_pcdId <<": after find_seedling_position(), foud first seedling seeds points size " << first_seedling_cloud_idx.size();
@@ -249,11 +250,17 @@ namespace graft_cv {
 		pcl::PointXYZ selected_pt;
 		int selected_pt_idx;
 		std::vector<int> optimal_seeds_idx;
-		get_optimal_seed_compare(cloud_dowm_sampled, cloud_seedling_seed, selected_pt, selected_pt_idx, 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
+			optimal_seeds_idx);		//output
 		
 		if (selected_pt_idx < 0) {	
 			if (m_pLogger) {
-				m_pLogger->ERRORINFO(m_pcdId + ": Not found valid grab point");
+				m_pLogger->ERRORINFO(m_pcdId + ": Not found valid fork point");
 			}
 			return 1;
 		}
@@ -591,8 +598,8 @@ void CRootStockGrabPoint::line_filter(
 	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<std::vector<int>>&target_filtered_element,		//输出，茎上点index
+	std::vector<pcl::ModelCoefficients::Ptr>& target_filtered_models//输出，茎直线模型
 
 )
 {
@@ -683,6 +690,7 @@ void CRootStockGrabPoint::line_filter(
 			out_idx.push_back(idx_raw);
 		}
 		target_filtered_element.push_back(out_idx);
+		target_filtered_models.push_back(coefficients);
 		
 		// debug show
 		/*if (m_cparam.image_show) {
@@ -708,7 +716,8 @@ void CRootStockGrabPoint::line_filter(
 	void CRootStockGrabPoint::find_seedling_position_key(		
 		pcl::PointCloud<pcl::PointXYZ>::Ptr in_cloud,
 		std::vector<int> &first_seedling_cloud_idx,
-		pcl::PointXYZ&xz_center
+		pcl::PointXYZ&xz_center,
+		pcl::ModelCoefficients::Ptr& line_model
 	)
 	{
 		// 确定植株inbox范围
@@ -860,9 +869,13 @@ void CRootStockGrabPoint::line_filter(
 		}
 		// 根据得到的位置，直线检测，并过滤
 		std::vector<pcl::PointXYZ>target_filtered;
-		std::vector<std::vector<int>> target_member;
+		std::vector<std::vector<int>> target_member;//输出，茎上点index
 		std::vector<pcl::PointXYZ>target_filtered_root;
-		line_filter(in_cloud, hole_step_radius,ymin,ymax, target_root, target_filtered, target_filtered_root, target_member);
+		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);
 
 		if (m_cparam.image_show) {
 			std::vector<pcl::PointXYZ> final_box;
@@ -879,9 +892,8 @@ void CRootStockGrabPoint::line_filter(
 				max_point_aabb_ex.z = pt.z + hole_step_radius;
 				final_box.push_back(min_point_aabb_ex);
 				final_box.push_back(max_point_aabb_ex);
-			}
-			std::vector<std::vector<int> > clt_line_idx_;
-			viewer_cloud_cluster_box(in_cloud, target_filtered_root, final_box, clt_line_idx_, std::string("line_filter"));
+			}			
+			viewer_cloud_cluster_box(in_cloud, target_filtered_root, final_box, target_member, std::string("line_filter"));
 		}
 
 		//sort cluster center, get the frontest leftest one
@@ -944,6 +956,7 @@ void CRootStockGrabPoint::line_filter(
 		xz_center.x = target_filtered_root.at(first_idx).x;
 		xz_center.y = target_filtered_root.at(first_idx).y;
 		xz_center.z = target_filtered_root.at(first_idx).z;
+		line_model = target_filtered_models.at(first_idx);
 
 		if (m_pLogger) {
 			stringstream buff;
@@ -1599,20 +1612,108 @@ void CRootStockGrabPoint::line_filter(
 		pt_idx = opt_idx;
 	}
 	//通过比较直线点云和原始点云相同位置邻域内xz的范围，确定此点是否是无干扰点的茎
+	//void CRootStockGrabPoint::get_optimal_seed_compare(
+	//	pcl::PointCloud<pcl::PointXYZ>::Ptr in_cloud,		//	全部有效点云
+	//	pcl::PointCloud<pcl::PointXYZ>::Ptr in_line_cloud,	//	茎上直线点云
+	//	pcl::PointXYZ&pt,									//  返回抓取的点坐标
+	//	int &pt_idx,										//	返回点index
+	//	std::vector<int>& valid_line_index					//  返回有效直线点index
+	//)
+	//{
+	//	valid_line_index.clear();
+	//	float th = 0.75;		//原始点云和直线点云邻域尺寸增加原来的20%，不能当做抓取点
+	//	pt_idx = -1;
+
+	//	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_inbox(new pcl::PointCloud<pcl::PointXYZ>);
+	//	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_inbox_line(new pcl::PointCloud<pcl::PointXYZ>);
+
+	//	pcl::CropBox<pcl::PointXYZ> box_filter;
+	//	box_filter.setNegative(false);
+	//	box_filter.setInputCloud(in_cloud);
+
+	//	pcl::CropBox<pcl::PointXYZ> box_filter_line;
+	//	box_filter_line.setNegative(false);
+	//	box_filter_line.setInputCloud(in_line_cloud);
+
+	//	float radius = m_cparam.rs_grab_stem_diameter;
+	//	float opt_y = m_cparam.rs_grab_y_opt;
+	//	if (m_dtype == 0) {
+	//		radius = m_cparam.sc_grab_stem_diameter;
+	//		opt_y = m_cparam.sc_grab_y_opt;
+	//	}
+	//	float rx = 1.5;
+	//	float ry = 1.5;
+	//	float rz = 1.5;
+	//	float cx, cy, cz;
+	//	float dz,dx, dz_line, dx_line;
+	//	float dist_min = 10000.0;
+
+	//	for (size_t i = 0; i < in_line_cloud->points.size(); ++i) {
+	//		cx = in_line_cloud->points.at(i).x;
+	//		cy = in_line_cloud->points.at(i).y;
+	//		cz = in_line_cloud->points.at(i).z;
+	//		//////////////////////////////////////////////////////////////////
+	//		//原始点云，获取指定区域的dx，dz
+	//		box_filter.setMin(Eigen::Vector4f(cx - rx*radius, cy - ry*radius, cz - rz*radius, 1));
+	//		box_filter.setMax(Eigen::Vector4f(cx + rx*radius, cy + ry*radius, cz + rz*radius, 1));
+	//		box_filter.filter(*cloud_inbox);
+
+	//		Eigen::Vector4f min_point;
+	//		Eigen::Vector4f max_point;
+	//		pcl::getMinMax3D(*cloud_inbox, min_point, max_point);
+	//		dz = max_point(2) - min_point(2);
+	//		dx = max_point(0) - min_point(0);
+
+	//		/////////////////////////////////////////////////////////////////////
+	//		//直线点云，获取指定区域的dx_line，dz_line
+	//		box_filter_line.setMin(Eigen::Vector4f(cx - rx*radius, cy - ry*radius, cz - rz*radius, 1));
+	//		box_filter_line.setMax(Eigen::Vector4f(cx + rx*radius, cy + ry*radius, cz + rz*radius, 1));
+	//		box_filter_line.filter(*cloud_inbox_line);
+	//		pcl::getMinMax3D(*cloud_inbox_line, min_point, max_point);
+	//		dz_line = max_point(2) - min_point(2);
+	//		dx_line = max_point(0) - min_point(0);
+
+	//		float ratio_ex = (dx + dz - dz_line - dx_line) / (dz_line + dx_line);
+	//		if (ratio_ex > th) {
+	//			valid_line_index.push_back(i);
+	//			if (fabs(cy - opt_y) < dist_min) {
+	//				dist_min = fabs(cy - opt_y);
+	//				pt_idx = i;
+	//			}
+	//		}
+
+	//	}
+	//	if (pt_idx >= 0) {
+	//		pt = in_line_cloud->points.at(pt_idx);
+	//	}
+	//	
+	//}
+
 	void CRootStockGrabPoint::get_optimal_seed_compare(
-		pcl::PointCloud<pcl::PointXYZ>::Ptr in_cloud,		//	全部有效点云
-		pcl::PointCloud<pcl::PointXYZ>::Ptr in_line_cloud,	//	茎上直线点云
+		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
 		std::vector<int>& valid_line_index					//  返回有效直线点index
 	)
 	{
 		valid_line_index.clear();
-		float th = 0.75;		//原始点云和直线点云邻域尺寸增加原来的20%，不能当做抓取点
+		float th_ratio = 1.0;		//原始点云和直线点云邻域尺寸增加原来的20%，不能当做抓取点
 		pt_idx = -1;
 
+		float ymin, ymax;
+		ymin = m_cparam.rs_grab_ymin;
+		ymax = m_cparam.rs_grab_ymax;
+		if (m_dtype == 0) {
+			ymin = m_cparam.sc_grab_ymin;
+			ymax = m_cparam.sc_grab_ymax;
+		}
+
 		pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_inbox(new pcl::PointCloud<pcl::PointXYZ>);
 		pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_inbox_line(new pcl::PointCloud<pcl::PointXYZ>);
+		std::vector<float> stem_width;
+		std::vector<pcl::PointXYZ>online_points;
 
 		pcl::CropBox<pcl::PointXYZ> box_filter;
 		box_filter.setNegative(false);
@@ -1632,50 +1733,86 @@ void CRootStockGrabPoint::line_filter(
 		float ry = 1.5;
 		float rz = 1.5;
 		float cx, cy, cz;
-		float dz,dx, dz_line, dx_line;
-		float dist_min = 10000.0;
+		float dz, dx;
+		//float dz,dx, dz_line, dx_line;
+		//float dist_min = 10000.0;
+
+
 
-		for (size_t i = 0; i < in_line_cloud->points.size(); ++i) {
-			cx = in_line_cloud->points.at(i).x;
-			cy = in_line_cloud->points.at(i).y;
-			cz = in_line_cloud->points.at(i).z;
+		//for (size_t i = 0; i < in_line_cloud->points.size(); ++i) {
+		cy = ymin;
+		float t = (cy - line_model->values.at(1)) / line_model->values.at(4);
+		while(cy<ymax){
+			cx = line_model->values.at(3) * t + line_model->values.at(0);			
+			cz = line_model->values.at(5) * t + line_model->values.at(2);
 			//////////////////////////////////////////////////////////////////
 			//原始点云，获取指定区域的dx，dz
-			box_filter.setMin(Eigen::Vector4f(cx - rx*radius, cy - ry*radius, cz - rz*radius, 1));
-			box_filter.setMax(Eigen::Vector4f(cx + rx*radius, cy + ry*radius, cz + rz*radius, 1));
+			box_filter.setMin(Eigen::Vector4f(cx - rx*radius, cy - 2, cz - rz*radius, 1));
+			box_filter.setMax(Eigen::Vector4f(cx + rx*radius, cy + 2, cz + rz*radius, 1));
 			box_filter.filter(*cloud_inbox);
 
-			Eigen::Vector4f min_point;
-			Eigen::Vector4f max_point;
-			pcl::getMinMax3D(*cloud_inbox, min_point, max_point);
-			dz = max_point(2) - min_point(2);
-			dx = max_point(0) - min_point(0);
-
-			/////////////////////////////////////////////////////////////////////
-			//直线点云，获取指定区域的dx_line，dz_line
-			box_filter_line.setMin(Eigen::Vector4f(cx - rx*radius, cy - ry*radius, cz - rz*radius, 1));
-			box_filter_line.setMax(Eigen::Vector4f(cx + rx*radius, cy + ry*radius, cz + rz*radius, 1));
-			box_filter_line.filter(*cloud_inbox_line);
-			pcl::getMinMax3D(*cloud_inbox_line, min_point, max_point);
-			dz_line = max_point(2) - min_point(2);
-			dx_line = max_point(0) - min_point(0);
-
-			float ratio_ex = (dx + dz - dz_line - dx_line) / (dz_line + dx_line);
-			if (ratio_ex < th) {
-				valid_line_index.push_back(i);
-				if (fabs(cy - opt_y) < dist_min) {
-					dist_min = fabs(cy - opt_y);
-					pt_idx = i;
+			if (cloud_inbox->points.size() > 10) {
+
+				Eigen::Vector4f min_point;
+				Eigen::Vector4f max_point;
+				pcl::getMinMax3D(*cloud_inbox, min_point, max_point);
+				dz = max_point(2) - min_point(2);
+				dx = max_point(0) - min_point(0);
+				if (dx > dz) {
+					stem_width.push_back(dx);
+				}
+				else {
+					stem_width.push_back(dz);
 				}
+				
 			}
-
-		}
-		if (pt_idx >= 0) {
-			pt = in_line_cloud->points.at(pt_idx);
-		}
+			else {
+				stem_width.push_back(0);
+			}
+			pcl::PointXYZ tmp_pt(cx,cy,cz);
+			online_points.push_back(tmp_pt);
+
+			cy += 1.0;
+		}
+		std::vector<float>valid_stem_width;
+		for (auto&w : stem_width) { valid_stem_width.push_back(w); }
+		float mu = get_hist_mean(valid_stem_width);
+		float stdv = get_hist_std(valid_stem_width, mu);
+
+		int max_pos = std::max_element(stem_width.begin(), stem_width.end()) - stem_width.begin();
+		float max_val = stem_width.at(max_pos);
+		float th = mu + th_ratio * stdv;
+		if (th < max_val) {
+			int i = 0;
+			for (; i < stem_width.size(); ++i) {
+				if (stem_width.at(i) >= th) {
+					break;
+				}
+			}
+			max_pos = i;
+		}
+
+		/////////////////////////////////////////////////////////////////////
+		//直线点云，获取指定区域的dx_line，dz_line
+		Eigen::Vector4f min_point;
+		Eigen::Vector4f max_point;
+		cx = online_points.at(max_pos).x;
+		cy = online_points.at(max_pos).y;
+		cz = online_points.at(max_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);
+		pcl::getMinMax3D(*cloud_inbox_line, min_point, max_point);
+		float z_mu = 0.5 * (max_point(2) + min_point(2));
+		float x_mu = 0.5 * (max_point(0) + min_point(0));
+
+		pt.x = x_mu;
+		pt.y = cy;
+		pt.z = z_mu;
+
+		pt_idx = max_pos;	
 		
 	}
-
 	void CRootStockGrabPoint::viewer_cloud(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, std::string&winname)
 	{
 		pcl::visualization::CloudViewer viewer(winname);
@@ -1834,13 +1971,13 @@ void CRootStockGrabPoint::line_filter(
 			viewer.addCube(min_pt.x, max_pt.x, min_pt.y, max_pt.y, min_pt.z, max_pt.z, 0.5,0.5,0.0,"AABB_"+string(_itoa(cnt, buf, 10)));
 			viewer.setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_REPRESENTATION,
 				pcl::visualization::PCL_VISUALIZER_REPRESENTATION_WIREFRAME, "AABB_" + string(_itoa(cnt, buf, 10)));
-
-			pcl::PointCloud<pcl::PointXYZ>::Ptr line_cloud(new pcl::PointCloud<pcl::PointXYZ>);
-			//pcl::copyPointCloud(*cloud, clt_line_idx.at(i), *line_cloud);
-			viewer.addPointCloud(line_cloud, "fit_line" + string(_itoa(cnt, buf, 10)));
-			viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 0, 1, 0, "fit_line" + string(_itoa(cnt, buf, 10)));
-			viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "fit_line" + string(_itoa(cnt, buf, 10)));
-
+			if (clt_line_idx.size()>i && clt_line_idx.at(i).size() > 0) {
+				pcl::PointCloud<pcl::PointXYZ>::Ptr line_cloud(new pcl::PointCloud<pcl::PointXYZ>);
+				pcl::copyPointCloud(*cloud, clt_line_idx.at(i), *line_cloud);
+				viewer.addPointCloud(line_cloud, "fit_line" + string(_itoa(cnt, buf, 10)));
+				viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 0, 1, 0, "fit_line" + string(_itoa(cnt, buf, 10)));
+				viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "fit_line" + string(_itoa(cnt, buf, 10)));
+			}
 			cnt += 1;
 		}
 		while (!viewer.wasStopped()) {

grab_point_rs.h

@@ -2,6 +2,7 @@
 
 #include <pcl\point_types.h>
 #include <pcl\point_cloud.h>
+#include <pcl\segmentation\sac_segmentation.h>
 #include "data_def_api.h"
 #include "data_def.h"
 #include "logger.h"
@@ -57,7 +58,8 @@ namespace graft_cv {
 		void find_seedling_position_key(
 			pcl::PointCloud<pcl::PointXYZ>::Ptr in_cloud,
 			std::vector<int> &first_seedling_cloud_idx,
-			pcl::PointXYZ&xz_center
+			pcl::PointXYZ&xz_center,
+			pcl::ModelCoefficients::Ptr& lmodel
 		);
 		// 邻域最小抑制
 		void nms_box(
@@ -74,7 +76,8 @@ namespace graft_cv {
 			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<std::vector<int>>&target_filtered_element,		//输出，茎上点index
+			std::vector<pcl::ModelCoefficients::Ptr>& target_filtered_models//输出，茎直线模型
 		);
 
 		/////////////////////////////////////////////////
@@ -115,12 +118,21 @@ namespace graft_cv {
 		);
 
 		//通过比较直线点云和原始点云相同位置邻域内xz的范围，确定此点是否是无干扰点的茎
-		void get_optimal_seed_compare(
+		/*void get_optimal_seed_compare(
 			pcl::PointCloud<pcl::PointXYZ>::Ptr in_cloud,
 			pcl::PointCloud<pcl::PointXYZ>::Ptr in_line_cloud,
 			pcl::PointXYZ&pt,
 			int &pt_idx,
 			std::vector<int>& valid_line_index
+		);*/
+		//通过比较直线点云和原始点云相同位置邻域内xz的范围，确定此点是否是无干扰点的茎
+		void get_optimal_seed_compare(
+			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
 		);
 		//在一株苗的空间范围内找出直线（茎，假设茎是直线分布的），并返回直线上的点index
 		void find_line_in_cube(

graft_cv_api.cpp

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

utils.h

@@ -434,6 +434,17 @@ namespace graft_cv{
 		return mu;
 	}
 
+	template<typename T>
+	double get_hist_std(std::vector<T>&data, T mu) {
+		double std_v = 0;
+		for (auto &v : data) { std_v += (v-mu)*(v - mu); }
+		if (data.size() > 0) {
+			std_v /= data.size();
+			std_v = sqrt(std_v);
+		}
+		return std_v;
+	}
+
 	template<typename T>
 	int trend_detect_pos(const vector<T>&data, int data_th, int step=15) {
 		int pos = -1;