v0.7.8 修改接穗抓点靠上一点

ReadMe.txt

@@ -91,4 +91,6 @@ v0.7.2
 v0.7.3 修改抓取位置，提供茎节分叉的位置（以前提供可抓取的位置，避开茎节的点）
 v0.7.4 增加叶片剔除功能（在boxcrop和降采样后实现,直接采用欧式距离聚类）
 v0.7.5 优化叶片剔除功能（欧式距离聚类没能分割叶片和茎造成误剔除，本次采用ror方法剔除叶片，但有些耗时）
-v0.7.6 优化叶片剔除功能,修改ror,nb_point的数量系数，通过配置文件sc_grab_ror_ratio参数传入
+v0.7.6 优化叶片剔除功能,修改ror,nb_point的数量系数，通过配置文件sc_grab_ror_ratio参数传入
+v0.7.7 优化茎判别问题：上下2片叶子，利用有效高度剔除；偏离z中心剔除，采用历史z均值
+v0.7.8 修改接穗抓点靠上一点

gcv_conf.yml

@@ -1,6 +1,6 @@
 %YAML:1.0
 conf_parameters:
-   image_show: 1
+   image_show: 0
    image_return: 1
    image_row_grid: 20
    image_col_grid: 100
@@ -75,7 +75,7 @@ conf_parameters:
    rs_grab_y_opt: -30
    rs_grab_seedling_dist: 40.0
    rs_grab_stem_min_pts: 50
-   rs_grab_ror_ratio: 0.75
+   rs_grab_ror_ratio: 0.8
    sc_grab_xmin: -200
    sc_grab_xmax: 200
    sc_grab_ymin: -45
@@ -86,6 +86,6 @@ conf_parameters:
    sc_grab_y_opt: -20
    sc_grab_seedling_dist: 40.0
    sc_grab_stem_min_pts: 45
-   sc_grab_ror_ratio: 0.75
+   sc_grab_ror_ratio: 0.8
    
 

grab_point_rs.cpp

@@ -36,7 +36,8 @@ namespace graft_cv {
 		m_pcdId(""),
 		m_ppImgSaver(0),
 		m_1st_row_zmean_rs(-1.0),
-		m_1st_row_zmean_sc(-1.0)
+		m_1st_row_zmean_sc(-1.0),
+		m_cloud_mean_dist(0.0)
 	{
 	}
 
@@ -197,47 +198,17 @@ namespace graft_cv {
 			m_pLogger->INFO(m_pcdId + ": end ror");
 		}
 		////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-		//3 voxel grid down sampling， 降采样
-		//if (m_pLogger) {
-		//	m_pLogger->INFO(m_pcdId + ": begin down");
-		//}
-		//pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_dowm_sampled_with_leaf(new pcl::PointCloud<pcl::PointXYZ>);		
-		//pcl::VoxelGrid<pcl::PointXYZ> outrem;
-		//outrem.setInputCloud(cloud_ror);
-		////outrem.setLeafSize(stem_radius, stem_radius, stem_radius);
-		//double voxel_size = m_cparam.rs_grab_voxel_size;
-		//if (m_dtype == 0) { voxel_size = m_cparam.sc_grab_voxel_size; }
-		//outrem.setLeafSize(voxel_size, voxel_size, voxel_size);
-		//outrem.filter(*cloud_dowm_sampled_with_leaf);
-
-		double mean_dist = 0.0;
-		cloud_mean_dist(cloud_ror, mean_dist);
+		//3 原来的降采样没有意义，改成统计平均距离
+		m_cloud_mean_dist = 0.0;
+		cloud_mean_dist(cloud_ror, m_cloud_mean_dist);
 		if (m_pLogger) {
 			stringstream buff;
-			buff << m_pcdId <<": cloud_mean_dist = " << mean_dist;
+			buff << m_pcdId <<": cloud_mean_dist = " << m_cloud_mean_dist;
 			m_pLogger->INFO(buff.str());
 		}
 		
-
-		//if (m_pLogger) {
-		//	stringstream buff;
-		//	buff << m_pcdId <<": VoxelGrid dowm_sampled point cloud " 
-		//		<< cloud_dowm_sampled_with_leaf->width * cloud_dowm_sampled_with_leaf->height
-		//		<< " data points (if < 50, return)";
-		//	m_pLogger->INFO(buff.str());
-		//}
-		//if (cloud_dowm_sampled_with_leaf->width * cloud_dowm_sampled_with_leaf->height < 50) {
-		//	return 1;
-		//}
-
-		///*if (m_cparam.image_show) {
-		//	viewer_cloud(cloud_dowm_sampled, std::string("cloud_dowm_sampled"));
-		//}*/
-		//if (m_pLogger) {
-		//	m_pLogger->INFO(m_pcdId + ": end down");
-		//}
 		////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-		// 4 对截取的点云进行聚类分析，剔除叶片
+		// 4 对截取的点云进行ror滤除大面积联通区域，剔除叶片
 		pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_dowm_sampled(new pcl::PointCloud<pcl::PointXYZ>);
 		std::vector<int> non_leaf_indices;
 		//leaf_filter(cloud_ror, non_leaf_indices);
@@ -261,13 +232,19 @@ namespace graft_cv {
 		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, line_model);
+		bool fund_seedling = find_seedling_position_key(cloud_dowm_sampled, first_seedling_cloud_idx, xz_center, line_model);
+		if (!fund_seedling) {
+			if (m_pLogger) {
+				stringstream buff;
+				buff << m_pcdId << ": Not found seedlings" ;
+				m_pLogger->INFO(buff.str());
+			}
+			return 1;
+		}
 		if (m_pLogger) {
 			stringstream buff;
 			buff << m_pcdId <<": after find_seedling_position(), foud first seedling seeds points size " << first_seedling_cloud_idx.size();
-			m_pLogger->INFO(buff.str());
-		}
-		if (m_pLogger) {
+			m_pLogger->INFO(buff.str());		
 			m_pLogger->INFO(m_pcdId + ": end find seedling position");
 		}
 
@@ -293,9 +270,6 @@ namespace graft_cv {
 			}
 			viewer_cloud_cluster(tttp, tmp, string("first"));
 		}
-		
-
-
 
 		pcl::PointXYZ selected_pt;
 		int selected_pt_idx;
@@ -1121,17 +1095,18 @@ void CRootStockGrabPoint::line_filter(
 
 		pcl::PointCloud<pcl::PointXYZ>::Ptr stem_cloud(new pcl::PointCloud<pcl::PointXYZ>);
 		pcl::copyPointCloud(*seedling_inbox, *inliers, *stem_cloud);
-		
+
 		//点数过滤
 		int min_stem_pts = m_cparam.rs_grab_stem_min_pts;
 		if (m_dtype == 0) { min_stem_pts = m_cparam.sc_grab_stem_min_pts; }
 		if (inliers->indices.size() < int(min_stem_pts / 2)) { continue; }
 		//y方向分布范围滤波
+		float y_length_th = 10.0;
 		pcl::PointXYZ min_v;
 		pcl::PointXYZ max_v;
 		pcl::getMinMax3D(*stem_cloud, min_v, max_v);
 		float dy = max_v.y - min_v.y;
-		if (dy / (ymax - ymin) < 0.5) { continue; }
+		if (dy<y_length_th && dy / (ymax - ymin) < 0.25) { continue; }
 		//y方向分布中心滤波
 		float dy_c = 0.5*(max_v.y + min_v.y);
 		if ((dy_c - ymin) / (ymax - ymin) > 0.75) { continue; }
@@ -1143,6 +1118,75 @@ void CRootStockGrabPoint::line_filter(
 		if (a < 45.0) { continue; }
 		target_filtered.push_back(p);
 
+		//有效茎长计算
+		std::vector<int> stem_y_count_hist;
+		get_line_project_hist(stem_cloud, coefficients, stem_y_count_hist);
+		/*float stem_diameter = m_cparam.rs_grab_stem_diameter;
+		if (m_dtype == 0) { stem_diameter = m_cparam.sc_grab_stem_diameter; }
+		int cnt_th = static_cast<int>(stem_diameter / m_cloud_mean_dist / m_cloud_mean_dist / 2.0);*/
+		std::vector<int> stem_y_count_hist_valid;
+		for (auto&v : stem_y_count_hist) {
+			if (v > 2) {
+				stem_y_count_hist_valid.push_back(v);
+			} 
+		}
+		double valid_mu = get_hist_mean(stem_y_count_hist_valid);
+		int cnt_th = static_cast<int>(valid_mu/2.0);
+		if (cnt_th < 3) { cnt_th = 2; }
+
+		float valid_length = 0.0;
+		
+		std::vector<int> stem_y_mask;
+
+		for (auto& c : stem_y_count_hist) {
+			if (c > cnt_th) { 
+				valid_length += 1.0; 
+				stem_y_mask.push_back(1);
+			}
+			else {
+				stem_y_mask.push_back(0);
+			}
+		}
+		//填补空
+		for (int i = 1; i < stem_y_mask.size() - 1; ++i) {
+			if (stem_y_mask.at(i) == 0 && stem_y_mask.at(i - 1) == 1 && stem_y_mask.at(i + 1) == 1) {
+				stem_y_mask.at(i) = 1;
+			}
+		}
+		//找出最长的连续段的长度
+		int longest_segment = 0;
+		int start = 0;
+		for (int i = 0; i < stem_y_mask.size(); ++i) {
+			if (i == 0 ) {
+				if (stem_y_mask.at(i) == 1) {
+					start = i;
+				}				
+				continue;
+			}
+			
+			
+			if (i == stem_y_mask.size() - 1 ) {
+				if (stem_y_mask.at(i) == 1) {
+					int length = i - start + 1;
+					if (length > longest_segment) { longest_segment = length; }
+				}				
+				continue;
+			}
+			if (stem_y_mask.at(i - 1) == 0 && stem_y_mask.at(i) == 1) {
+				start = i;
+				continue;
+			}
+			if (stem_y_mask.at(i - 1) == 1 && stem_y_mask.at(i) == 0) {
+				int length = i - start;
+				if (length > longest_segment) { longest_segment = length; }
+				continue;
+			}
+			
+		}
+
+		if (longest_segment<10.0 && valid_length / (ymax - ymin) < 0.35) { continue; }
+
+
 		float min_y = 10000.0;
 		int min_y_idx = -1;
 		for (size_t j = 0; j < stem_cloud->points.size();++j) {
@@ -1185,7 +1229,7 @@ void CRootStockGrabPoint::line_filter(
 }
 
 
-	void CRootStockGrabPoint::find_seedling_position_key(		
+	bool CRootStockGrabPoint::find_seedling_position_key(		
 		pcl::PointCloud<pcl::PointXYZ>::Ptr in_cloud,
 		std::vector<int> &first_seedling_cloud_idx,
 		pcl::PointXYZ&xz_center,
@@ -1360,6 +1404,8 @@ void CRootStockGrabPoint::line_filter(
 
 			viewer_cloud_cluster_box(in_cloud, target_root, clt_box_tmp, clt_line_idx, std::string("nms_box"));
 		}
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////
 		// 根据得到的位置，直线检测，并过滤
 		std::vector<pcl::PointXYZ>target_filtered;
 		std::vector<std::vector<int>> target_member;//输出，茎上点index
@@ -1370,6 +1416,15 @@ void CRootStockGrabPoint::line_filter(
 			target_member,
 			target_filtered_models);
 
+		if (target_filtered_root.size() == 0) {
+			if (m_pLogger) {
+				stringstream buff;
+				buff << m_pcdId << ": line_filter reuslt cluster is empty" ;
+				m_pLogger->INFO(buff.str());
+			}
+			return false;
+		}
+
 		if (m_cparam.image_show) {
 			std::vector<pcl::PointXYZ> final_box;
 			for (auto&pt : target_filtered_root) {
@@ -1388,23 +1443,30 @@ void CRootStockGrabPoint::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
 		//找最小z，然后计算平均z
-		float min_root_z = 10000.0;
+		float mean_root_z_all = 0.0;
 		for(auto&pt : target_filtered_root) {
-			if (pt.z < min_root_z) { min_root_z = pt.z; }
+			mean_root_z_all += pt.z; 
 		}
+		mean_root_z_all /= target_filtered_root.size();
 		//通过最小z，保守的找到和他一排的植株
 		std::vector<int> first_row_index;
 		float mean_z = 0.0;
 		for (int idx_r = 0; idx_r < target_filtered_root.size();++idx_r) {
 			pcl::PointXYZ&pt = target_filtered_root.at(idx_r);
-			if (std::fabs(pt.z - min_root_z) < hole_step) { 
+			if (std::fabs(pt.z - mean_root_z_all) < hole_step_radius) {
 				first_row_index.push_back(idx_r); 
 				mean_z += pt.z;
 			}
 		}
+		if (m_pLogger) {
+			stringstream buff;
+			buff << m_pcdId << ": current seedling number:"<< first_row_index.size()
+				<<", mean_z:"<< mean_z;
+			m_pLogger->INFO(buff.str());
+		}
 		//如果第一排的植株大于3，计算平均值
 		float first_row_num = (float)first_row_index.size();
 		mean_z /= first_row_num;
@@ -1416,26 +1478,45 @@ void CRootStockGrabPoint::line_filter(
 		}
 		else {
 			if( m_dtype == 0) {
-				if (m_1st_row_zmean_sc > 0) { mean_z = m_1st_row_zmean_sc; }
+				if (m_1st_row_zmean_sc > 0) {
+					mean_z = m_1st_row_zmean_sc; }
 			}
 			else {
-				if (m_1st_row_zmean_rs > 0) { mean_z = m_1st_row_zmean_rs; }
+				if (m_1st_row_zmean_rs > 0) { 
+					mean_z = m_1st_row_zmean_rs; }
+			}
+			if (m_pLogger) {
+				stringstream buff;
+				buff << m_pcdId << ": update with historic mean_z:" << mean_z;
+				m_pLogger->INFO(buff.str());
 			}
 		}
 		//通过mean_z再次寻找第一排的植株
 		first_row_index.clear();		
 		for (int idx_r = 0; idx_r < target_filtered_root.size(); ++idx_r) {
 			pcl::PointXYZ&pt = target_filtered_root.at(idx_r);
-			if (std::fabs(pt.z - mean_z) < hole_step) {
+			if (std::fabs(pt.z - mean_z) < hole_step_radius) {
 				first_row_index.push_back(idx_r);				
 			}
 		}
+		//找第一排的植株没有满足要求的
+		if (first_row_index.size()==0) {
+			if (m_pLogger) {
+				stringstream buff;
+				buff << m_pcdId << ": NOT found valid seedlings";
+				m_pLogger->INFO(buff.str());
+			}
+			return false;
+		}
+
+		///////////////////////////////////////////////////////////
 		std::vector<float> cluster_index;
 		for (int i=0; i<first_row_index.size();++i){
 			int raw_idx = first_row_index.at(i);
 			pcl::PointXYZ&pt = target_filtered_root.at(raw_idx);			
 			cluster_index.push_back(pt.x);
 		}
+		
 		int first_idx = std::min_element(cluster_index.begin(), cluster_index.end()) - cluster_index.begin();
 		if (m_dtype == 0) {
 			first_idx = std::max_element(cluster_index.begin(), cluster_index.end()) - cluster_index.begin();
@@ -1457,6 +1538,7 @@ void CRootStockGrabPoint::line_filter(
 				<<", "<< xz_center.y<<", "<< xz_center.z<<")";
 			m_pLogger->INFO(buff.str());
 		}
+		return true;
 	}
 	void CRootStockGrabPoint::tilted_seedling_discover(
 		std::vector<pcl::PointXYZ>&cluster_center,	//输入，已知的苗的坐标
@@ -2270,14 +2352,31 @@ void CRootStockGrabPoint::line_filter(
 		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;
+		if(m_dtype == 0){
+			//穗苗尽量找高点
+			if(max_pos>15){
+				if (th < max_val) {
+				int i = max_pos-10;
+				for (; i < max_pos; ++i) {
+					if (stem_width.at(i) >= th) {
+						break;
+					}
+				}
+				max_pos = i;
+			}
+			}
+		}
+		else{
+			//砧木可以按低点
+			if (th < max_val) {
+				int i = 0;
+				for (; i < stem_width.size(); ++i) {
+					if (stem_width.at(i) >= th) {
+						break;
+					}
 				}
+				max_pos = i;
 			}
-			max_pos = i;
 		}
 
 		/////////////////////////////////////////////////////////////////////
@@ -2316,6 +2415,49 @@ void CRootStockGrabPoint::line_filter(
 			}
 		}		
 	}
+
+	void CRootStockGrabPoint::get_line_project_hist(		
+		pcl::PointCloud<pcl::PointXYZ>::Ptr in_line_cloud,	//input	茎上直线点云
+		pcl::ModelCoefficients::Ptr line_model,				//input		
+		std::vector<int>& count_hist					    //  返回有效直线1mm内点云数量
+	)
+	{
+		count_hist.clear();		
+
+		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::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;		
+		if (m_dtype == 0) {
+			radius = m_cparam.sc_grab_stem_diameter;			
+		}
+		float rx = 1.5;		
+		float rz = 1.5;
+		float cx, cy, cz, t;
+
+		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);
+			cz = line_model->values.at(5) * t + line_model->values.at(2);
+			
+			box_filter_line.setMin(Eigen::Vector4f(cx - rx*radius, cy - 0.5, cz - rz*radius, 1));
+			box_filter_line.setMax(Eigen::Vector4f(cx + rx*radius, cy + 0.5, cz + rz*radius, 1));
+			box_filter_line.filter(*cloud_inbox);
+			count_hist.push_back(cloud_inbox->points.size());
+			cy += 1.0;
+		}		
+	}
 	void CRootStockGrabPoint::viewer_cloud(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, std::string&winname)
 	{
 		pcl::visualization::CloudViewer viewer(winname);

grab_point_rs.h

@@ -31,6 +31,7 @@ namespace graft_cv {
 		CImStoreManager** m_ppImgSaver;
 
 		pcl::PointCloud<pcl::PointXYZ>::Ptr m_raw_cloud;
+		double m_cloud_mean_dist;
 
 
 		int read_ply_file(const char* fn);
@@ -109,7 +110,7 @@ namespace graft_cv {
 			pcl::PointXYZ&key_center,	//输入，已知的苗的坐标
 			std::vector<pcl::PointXYZ>&candidate_center	//输出，有倾斜苗的坐标
 		);
-		void find_seedling_position_key(
+		bool find_seedling_position_key(
 			pcl::PointCloud<pcl::PointXYZ>::Ptr in_cloud,
 			std::vector<int> &first_seedling_cloud_idx,
 			pcl::PointXYZ&xz_center,
@@ -133,6 +134,11 @@ namespace graft_cv {
 			std::vector<std::vector<int>>&target_filtered_element,		//输出，茎上点index
 			std::vector<pcl::ModelCoefficients::Ptr>& target_filtered_models//输出，茎直线模型
 		);
+		void get_line_project_hist(
+			pcl::PointCloud<pcl::PointXYZ>::Ptr in_line_cloud,	//input	茎上直线点云
+			pcl::ModelCoefficients::Ptr line_model,				//input		
+			std::vector<int>& count_hist					    //  返回有效直线1mm内点云数量
+		);
 
 		/////////////////////////////////////////////////
 		void find_seedling_position(

graft_cv_api.cpp

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