v0.7.19 优化茎节位置识别,投影到2为平面寻找分叉点，rs_grab_fork_th和sc_grab_fork_th不在起作用，放弃历史均值位置

ReadMe.txt

@@ -103,4 +103,5 @@ v0.7.14
 v0.7.15 优化茎节位置识别
 v0.7.16 优化茎节位置识别,增加平均茎节高度约束，10mm范围
 v0.7.17 优化茎节位置识别,不存在显著茎节的用平均茎节高度替代，否则用识别到的茎节位置
-v0.7.18 优化茎节位置识别,评估方法改为顶点高度和中值高度差，增加阈值控制是否有效
+v0.7.18 优化茎节位置识别,评估方法改为顶点高度和中值高度差，增加阈值控制是否有效
+v0.7.19 优化茎节位置识别,投影到2为平面寻找分叉点，rs_grab_fork_th和sc_grab_fork_th不在起作用，放弃历史均值位置

grab_point_rs.cpp

@@ -21,6 +21,8 @@
 #include <pcl\segmentation\sac_segmentation.h>
 #include <pcl\segmentation\extract_clusters.h>
 #include <math.h>
+#include <pcl\features\normal_3d.h>
+#include <pcl\features\boundary.h>
 
 #include "grab_point_rs.h"
 #include "utils.h"
@@ -2388,7 +2390,11 @@ void CRootStockGrabPoint::line_filter(
 			return;
 		}
 
-		// 如果opt_y_valid==false，就是用户没有指定抓取的y高度		
+		// 如果opt_y_valid==false，就是用户没有指定抓取的y高度	
+		float max_dist_to_boundary = 0.0;
+		int max_idx_to_boundary = -1;
+		find_fork(in_line_cloud, max_dist_to_boundary, max_idx_to_boundary);
+
 		cy = ymin;	
 		//计算茎上的直径
 		while(cy<ymax){
@@ -2435,19 +2441,20 @@ void CRootStockGrabPoint::line_filter(
 		float mu = get_hist_mean(valid_stem_width);
 		float stdv = get_hist_std(valid_stem_width, mu);
 
+		//1) original max position
 		int max_pos = std::max_element(stem_width.begin(), stem_width.end()) - stem_width.begin();
 		int max_pos_ref = max_pos;
 
-		//peak finder
-		std::vector<int> peak_indices;
+		//2) peak finder, optimal
+		/*std::vector<int> peak_indices;
 		std::vector<float> peak_power;
 		findPeaks(stem_width, peak_indices, false);
 		indexPeaks(stem_width, peak_indices, mu, peak_power);
 		float max_power = 0.0;
-		if(peak_indices.size() > 0) {
+		if (peak_indices.size() > 0) {
 			int tmp_max_pos = peak_indices[0];
 			max_power = peak_power[0];
-			for (int i = 0; i < peak_indices.size();++i) {
+			for (int i = 0; i < peak_indices.size(); ++i) {
 				int pid = peak_indices.at(i);
 				if (peak_power[i] > max_power) {
 					tmp_max_pos = pid;
@@ -2455,26 +2462,20 @@ void CRootStockGrabPoint::line_filter(
 				}
 			}
 			max_pos = tmp_max_pos;
-			max_pos_ref = max_pos;			
-		}
-		//保存历史茎节位置
-		float max_power_th = m_cparam.rs_grab_fork_th;
-		if (m_dtype == 0) { max_power_th = m_cparam.sc_grab_fork_th; }
+			max_pos_ref = max_pos;
+		}*/
 
-		if (max_power > max_power_th) {
-			m_stem_fork_ys.insert(m_stem_fork_ys.begin(), max_pos);
-			if (m_stem_fork_ys.size() > m_stem_fork_ys_size) { m_stem_fork_ys.pop_back(); }
-			
-		}
-		int max_pos_mu = 0;
-		if (m_stem_fork_ys.size() > 0) {
-			max_pos_mu = int(get_hist_mean<int>(m_stem_fork_ys) + 0.5);
-		}
 
-		//float max_val = stem_width.at(max_pos);
-		//float th = mu + th_ratio * stdv;
+		/*float max_power_th = m_cparam.rs_grab_fork_th;
+		if (m_dtype == 0) { max_power_th = m_cparam.sc_grab_fork_th; }*/
+
+		if (max_idx_to_boundary >= 0) {
+			max_pos = int(in_line_cloud->points.at(max_idx_to_boundary).y + 0.5 - ymin);
+			max_pos_ref = max_pos;
+		}
+		
 		if(m_dtype == 0){			
-			if (max_power > max_power_th) {
+			/*if (max_power > max_power_th) {
 				max_pos_ref = max_pos;
 				max_pos += static_cast<int>(m_cparam.sc_grab_offset);
 			}
@@ -2487,10 +2488,12 @@ void CRootStockGrabPoint::line_filter(
 					max_pos_ref = max_pos;
 					max_pos += static_cast<int>(m_cparam.sc_grab_offset);
 				}
-			}			
+			}			*/
+			max_pos_ref = max_pos;
+			max_pos += static_cast<int>(m_cparam.sc_grab_offset);
 		}
 		else{			
-			if (max_power > max_power_th) {
+			/*if (max_power > max_power_th) {
 				max_pos_ref = max_pos;
 				max_pos += static_cast<int>(m_cparam.rs_grab_offset);
 			}
@@ -2503,7 +2506,9 @@ void CRootStockGrabPoint::line_filter(
 					max_pos_ref = max_pos;
 					max_pos += static_cast<int>(m_cparam.rs_grab_offset);
 				}
-			}			
+			}			*/
+			max_pos_ref = max_pos;
+			max_pos += static_cast<int>(m_cparam.rs_grab_offset);
 		}
 		if (max_pos < 0) { max_pos = 0;	}
 		if (max_pos >= online_points.size()) { max_pos = online_points.size() - 1; }
@@ -2558,6 +2563,107 @@ void CRootStockGrabPoint::line_filter(
 		}		
 	}
 
+	void  CRootStockGrabPoint::find_fork(
+		pcl::PointCloud<pcl::PointXYZ>::Ptr in_line_cloud,
+		float& max_dist,
+		int& max_idx
+	) 
+	{
+		//1 project to xy plane
+		pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_line_2d(new pcl::PointCloud<pcl::PointXYZ>);
+		pcl::copyPointCloud(*in_line_cloud, *cloud_line_2d);
+		for (pcl::PointXYZ&p : cloud_line_2d->points) { p.z = 0.0; }
+
+		//2 边界检测
+		pcl::search::KdTree<pcl::PointXYZ>::Ptr kdtree_ptr(new pcl::search::KdTree<pcl::PointXYZ>);
+		pcl::PointCloud<pcl::Normal>::Ptr normal(new pcl::PointCloud<pcl::Normal>);
+		pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne;
+		ne.setInputCloud(cloud_line_2d);
+		ne.setSearchMethod(kdtree_ptr);
+		ne.setRadiusSearch(3.0);
+		ne.compute(*normal);
+
+		pcl::PointCloud<pcl::Boundary>::Ptr boundaries(new pcl::PointCloud<pcl::Boundary>);
+		boundaries->resize(cloud_line_2d->size());
+		pcl::BoundaryEstimation < pcl::PointXYZ, pcl::Normal, pcl::Boundary>boundary_estimation;
+		boundary_estimation.setInputCloud(cloud_line_2d);
+		boundary_estimation.setInputNormals(normal);
+
+		boundary_estimation.setSearchMethod(kdtree_ptr);
+		boundary_estimation.setKSearch(30);
+		boundary_estimation.setAngleThreshold(M_PI*0.6);
+		boundary_estimation.compute(*boundaries);
+
+		pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_lines(new pcl::PointCloud<pcl::PointXYZ>);
+
+		pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_visual(new pcl::PointCloud<pcl::PointXYZRGB>);
+		cloud_visual->resize(cloud_line_2d->size());
+		for (size_t i = 0; i < cloud_line_2d->size(); ++i) {
+			cloud_visual->points.at(i).x = cloud_line_2d->points.at(i).x;
+			cloud_visual->points.at(i).y = cloud_line_2d->points.at(i).y;
+			cloud_visual->points.at(i).z = cloud_line_2d->points.at(i).z;
+			if (boundaries->points.at(i).boundary_point != 0) {
+				cloud_visual->points.at(i).r = 255;
+				cloud_visual->points.at(i).g = 0;
+				cloud_visual->points.at(i).b = 0;
+				cloud_lines->points.push_back(cloud_line_2d->points.at(i));
+			}
+			else {
+				cloud_visual->points.at(i).r = 255;
+				cloud_visual->points.at(i).g = 255;
+				cloud_visual->points.at(i).b = 255;
+			}
+		}
+
+		/*if (m_cparam.image_show) {
+			viewer_cloud(cloud_visual, string("boundary"));
+			viewer_cloud(cloud_lines, string("cloud_lines"));
+		}
+*/
+		//3 find points with max distance to boundaries point
+		pcl::search::KdTree<pcl::PointXYZ>::Ptr bound_kdtree_ptr(new pcl::search::KdTree<pcl::PointXYZ>);
+		bound_kdtree_ptr->setInputCloud(cloud_lines);
+
+		max_idx = -1;
+		max_dist = 0.0;
+		int k = 1;
+		for (int i = 0; i < cloud_line_2d->size(); ++i) {
+			if (boundaries->points.at(i).boundary_point != 0) { continue; }
+			std::vector<int> idx(k);
+			std::vector<float> sqr_distances(k);
+			int cnt = bound_kdtree_ptr->nearestKSearch(cloud_line_2d->points.at(i), k,idx,sqr_distances);
+			if (cnt > 0) {
+				if (sqr_distances.at(0) > max_dist) {
+					max_dist = sqr_distances.at(0);
+					max_idx = i;
+				}
+			}
+		}
+		if (m_cparam.image_show) {
+			pcl::visualization::PCLVisualizer viewer("boundary line");
+			viewer.addCoordinateSystem();
+			viewer.addPointCloud<pcl::PointXYZRGB>(cloud_visual, "boundary");
+			
+			if (max_idx >= 0) {
+				pcl::PointXYZ p0,p1;
+				p0.x = cloud_line_2d->points.at(max_idx).x-5;
+				p0.y = cloud_line_2d->points.at(max_idx).y;
+				p0.z = cloud_line_2d->points.at(max_idx).z;
+
+				p1.x = cloud_line_2d->points.at(max_idx).x + 5;
+				p1.y = cloud_line_2d->points.at(max_idx).y;
+				p1.z = cloud_line_2d->points.at(max_idx).z;
+				viewer.addLine(p0, p1, 1.0, 0.0, 0.0);
+			}
+			
+
+			while (!viewer.wasStopped()) {
+				viewer.spinOnce(100);
+				boost::this_thread::sleep(boost::posix_time::microseconds(100000));
+			}
+		}
+	}
+
 	void CRootStockGrabPoint::get_line_project_hist(		
 		pcl::PointCloud<pcl::PointXYZ>::Ptr in_line_cloud,	//input	茎上直线点云
 		pcl::ModelCoefficients::Ptr line_model,				//input		

grab_point_rs.h

@@ -108,7 +108,11 @@ namespace graft_cv {
 			double& mean_dist
 		);
 		//////////////////////////////////////////////////////////////////////////////////////
-
+		void  CRootStockGrabPoint::find_fork(
+			pcl::PointCloud<pcl::PointXYZ>::Ptr in_line_cloud,
+			float& max_dist,//最大距离
+			int& max_idx //距离边界最大的点序号
+		);
 
 
 		void gen_all_seedling_positions(

graft_cv_api.cpp

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