v0.6.15 修改抓取点识别方法，通过比较直线点云和原始点云相同位置邻域内xz的范围，确定此点是否是无干扰点的茎

ReadMe.txt

@@ -82,4 +82,5 @@ v0.6.10
 v0.6.11 增加茄科切后重识别异常退出判断
 v0.6.12 增加茄科抓取目标大小判断，太小的目标跳出；增加自动ply数据保存功能；茄科切后重识别图片保存功能
 v0.6.13 增加聚类结果中点数小于20的类别剔除功能；修改降采样后整个点云数量小于50退出（原来是200）
-v0.6.14 修改茎识别方法，通过2d高密度点找到茎的位置，然后提取株的最大空间内点云，用直线分割的方法得到茎的位置，避免识别位置错误
+v0.6.14 修改茎识别方法，通过2d高密度点找到茎的位置，然后提取株的最大空间内点云，用直线分割的方法得到茎的位置，避免识别位置错误
+v0.6.15 修改抓取点识别方法，通过比较直线点云和原始点云相同位置邻域内xz的范围，确定此点是否是无干扰点的茎

grab_point_rs.cpp

@@ -117,7 +117,8 @@ namespace graft_cv {
 			}
 			return 1;
 		}
-		//2 crop filter
+		////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		//1 crop filter
 		if (m_pLogger) {
 			m_pLogger->INFO("begin crop");			
 		}
@@ -151,7 +152,8 @@ namespace graft_cv {
 		if (m_pLogger) {
 			m_pLogger->INFO("end crop");
 		}
-		//3 filtler with radius remove
+		////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		//2 filtler with radius remove
 		if (m_pLogger) {
 			m_pLogger->INFO("begin ror");
 		}
@@ -184,7 +186,7 @@ namespace graft_cv {
 		if (m_pLogger) {
 			m_pLogger->INFO("end ror");
 		}
-		///////////////////////////////////////////////////////////////////////////////
+		////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 		//3 voxel grid down sampling
 		if (m_pLogger) {
 			m_pLogger->INFO("begin down");
@@ -211,10 +213,10 @@ namespace graft_cv {
 			m_pLogger->INFO("end down");
 		}
 
-		///////////////////////////////////////////////////////////////////////////////
-		//4 seedling position
-		std::vector<int> first_seedling_cloud_idx;
-		pcl::PointXYZ xz_center;
+		////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		//4 seedling position，找到第一个位置上的植株
+		std::vector<int> first_seedling_cloud_idx;	//存储找到的第一个位置上植株茎上直线点的index
+		pcl::PointXYZ xz_center;					//存储找到的第一个位置上植株根部坐标
 		if (m_pLogger) {
 			m_pLogger->INFO("begin find seedling position");
 		}
@@ -229,37 +231,23 @@ namespace graft_cv {
 			m_pLogger->INFO("end find seedling position");
 		}
 
-		///////////////////////////////////////////////////////////////////////////////
-		//5 nearest seedling grab point selection
+		////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		//5 nearest seedling grab point selection，找到最接近指定高度的最优抓取点坐标
 		pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_seedling_seed(new pcl::PointCloud<pcl::PointXYZ>);
 		pcl::copyPointCloud(*cloud_dowm_sampled, first_seedling_cloud_idx, *cloud_seedling_seed);
-		std::vector<int>mass_idx;
-		double dist_mean = compute_nearest_neighbor_distance(cloud_dowm_sampled);
-		std::vector<double> mass_indices;
-		if (m_pLogger) {
-			m_pLogger->INFO("begin crop nn_analysis");
-		}
-		crop_nn_analysis(cloud_ror, cloud_seedling_seed, dist_mean, mass_indices, mass_idx);
-		if (m_pLogger) {
-			m_pLogger->INFO("end crop nn_analysis");
-		}
-
-		double candidate_th = otsu(mass_indices);
-		std::vector<int> optimal_seeds_idx;
-		for (size_t j = 0; j < mass_idx.size(); ++j) {
-			if (mass_indices.at(mass_idx.at(j)) >= candidate_th) {
-				optimal_seeds_idx.push_back(mass_idx.at(j));
-			}
-		}
-		pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_optimal_seed(new pcl::PointCloud<pcl::PointXYZ>);
-		pcl::copyPointCloud(*cloud_seedling_seed, optimal_seeds_idx, *cloud_optimal_seed);
+		//改进思路：将茎直线上cloud_seedling_seed的点，提取周围邻域xz的宽度，在相同邻域cloud_dowm_sampled点云内提取xz的宽度
+		//对比两个宽度，变化很大的，说明周边有异物（叶柄或叶子），不适合作为抓取位置；否则就是抓取位置
+		//在众多抓取位置上，选择离指定高度最近的点作为抓取位置
+		// 
 		pcl::PointXYZ selected_pt;
 		int selected_pt_idx;
-		if (m_pLogger) {
-			m_pLogger->INFO("begin get_optimal_seed");
-		}
-		get_optimal_seed(cloud_optimal_seed, selected_pt, selected_pt_idx);
-		if (selected_pt_idx < 0) {			
+		std::vector<int> optimal_seeds_idx;
+		get_optimal_seed_compare(cloud_dowm_sampled, cloud_seedling_seed, selected_pt, selected_pt_idx, optimal_seeds_idx);
+		
+		if (selected_pt_idx < 0) {	
+			if (m_pLogger) {
+				m_pLogger->ERRORINFO("Not found valid grab point");
+			}
 			return 1;
 		}
 		if (m_pLogger) {
@@ -269,8 +257,8 @@ namespace graft_cv {
 		posinfo.rs_grab_y = selected_pt.y;
 		posinfo.rs_grab_z = selected_pt.z;
 
-		////////////////////////////////////////////////////////////////////
-		//debug
+		////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		//6 debug 显示结果
 		if (m_cparam.image_show) {
 			pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_cand_demo(new pcl::PointCloud<pcl::PointXYZRGB>);
 			pcl::copyPointCloud(*cloud_dowm_sampled, *cloud_cand_demo);
@@ -281,7 +269,7 @@ namespace graft_cv {
 				pt.b = 255;
 			}
 			int cnt = 0;
-			for (auto& idx : mass_idx) {
+			for (auto& idx : optimal_seeds_idx) {
 				int p_idx = first_seedling_cloud_idx.at(idx);
 				/*if (p_idx == optimal_seeds_idx[selected_pt_idx]) {
 					cloud_cand_demo->points[p_idx].r = 0;
@@ -1037,6 +1025,84 @@ namespace graft_cv {
 		pt.z = in_cloud->points.at(opt_idx).z;
 		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::viewer_cloud(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, std::string&winname)
 	{
 		pcl::visualization::CloudViewer viewer(winname);

grab_point_rs.h

@@ -77,6 +77,15 @@ namespace graft_cv {
 			pcl::PointCloud<pcl::PointXYZ>::Ptr,
 			pcl::PointXYZ&pt,
 			int &pt_idx);
+
+		//通过比较直线点云和原始点云相同位置邻域内xz的范围，确定此点是否是无干扰点的茎
+		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
+		);
 		//在一株苗的空间范围内找出直线（茎，假设茎是直线分布的），并返回直线上的点index
 		void find_line_in_cube(
 			pcl::PointCloud<pcl::PointXYZ>::Ptr,	//输入，整体点云

graft_cv_api.cpp

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