v0.7.21 优化茎节位置识别,增加偏离平均茎节高度的距离参数rs_grab_to_meanfork_max_dist和sc_grab_to_meanfork_max_dist

ReadMe.txt

@@ -104,4 +104,6 @@ v0.7.15
 v0.7.16 优化茎节位置识别,增加平均茎节高度约束，10mm范围
 v0.7.17 优化茎节位置识别,不存在显著茎节的用平均茎节高度替代，否则用识别到的茎节位置
 v0.7.18 优化茎节位置识别,评估方法改为顶点高度和中值高度差，增加阈值控制是否有效
-v0.7.19 优化茎节位置识别,投影到2为平面寻找分叉点，rs_grab_fork_th和sc_grab_fork_th不在起作用，放弃历史均值位置
+v0.7.19 优化茎节位置识别,投影到2为平面寻找分叉点，rs_grab_fork_th和sc_grab_fork_th不在起作用，放弃历史均值位置
+v0.7.20 优化茎节位置识别,在v0.7.19基础上加上历史均值位置约束，rs_grab_fork_th和sc_grab_fork_th是茎节粗和茎粗的比值系数
+v0.7.21 优化茎节位置识别,增加偏离平均茎节高度的距离参数rs_grab_to_meanfork_max_dist和sc_grab_to_meanfork_max_dist

config.cpp

@@ -93,6 +93,7 @@ namespace graft_cv{
 			<< "rs_grab_ror_ratio" << m_cparam->rs_grab_ror_ratio
 			<< "rs_grab_offset" << m_cparam->rs_grab_offset
 			<< "rs_grab_fork_th" << m_cparam->rs_grab_fork_th
+			<< "rs_grab_to_meanfork_max_dist" << m_cparam->rs_grab_to_meanfork_max_dist
 
 			<< "sc_grab_xmin" << m_cparam->sc_grab_xmin
 			<< "sc_grab_xmax" << m_cparam->sc_grab_xmax
@@ -107,6 +108,7 @@ namespace graft_cv{
 			<< "sc_grab_ror_ratio" << m_cparam->sc_grab_ror_ratio
 			<< "sc_grab_offset" << m_cparam->sc_grab_offset
 			<< "sc_grab_fork_th" << m_cparam->sc_grab_fork_th
+			<< "sc_grab_to_meanfork_max_dist" << m_cparam->sc_grab_to_meanfork_max_dist
 			<< "}"; 	
 	};
 	void CGCvConfig::read(const cv::FileNode& node){ //Read serialization for this class
@@ -185,6 +187,7 @@ namespace graft_cv{
 		m_cparam->rs_grab_ror_ratio = (double)node["rs_grab_ror_ratio"];
 		m_cparam->rs_grab_offset = (double)node["rs_grab_offset"];
 		m_cparam->rs_grab_fork_th = (double)node["rs_grab_fork_th"];
+		m_cparam->rs_grab_to_meanfork_max_dist = (double)node["rs_grab_to_meanfork_max_dist"];
 
 		m_cparam->sc_grab_xmin = (double)node["sc_grab_xmin"];
 		m_cparam->sc_grab_xmax = (double)node["sc_grab_xmax"];
@@ -199,6 +202,7 @@ namespace graft_cv{
 		m_cparam->sc_grab_ror_ratio = (double)node["sc_grab_ror_ratio"];
 		m_cparam->sc_grab_offset = (double)node["sc_grab_offset"];
 		m_cparam->sc_grab_fork_th = (double)node["sc_grab_fork_th"];
+		m_cparam->sc_grab_to_meanfork_max_dist = (double)node["sc_grab_to_meanfork_max_dist"];
   }
 	string get_cparam_info(ConfigParam*m_cparam)
 	{
@@ -280,7 +284,8 @@ namespace graft_cv{
 			<< "rs_grab_stem_min_pts:\t" << m_cparam->rs_grab_stem_min_pts << endl
 			<< "rs_grab_ror_ratio:\t" << m_cparam->rs_grab_ror_ratio << endl
 			<< "rs_grab_offset:\t" << m_cparam->rs_grab_offset << endl
-			<< "rs_grab_fork_th:\t" << m_cparam->rs_grab_fork_th << endl			
+			<< "rs_grab_fork_th:\t" << m_cparam->rs_grab_fork_th << endl
+			<< "rs_grab_to_meanfork_max_dist:\t" << m_cparam->rs_grab_to_meanfork_max_dist << endl
 
 			<< "sc_grab_xmin:\t" << m_cparam->sc_grab_xmin << endl
 			<< "sc_grab_xmax:\t" << m_cparam->sc_grab_xmax << endl
@@ -295,6 +300,7 @@ namespace graft_cv{
 			<< "sc_grab_ror_ratio:\t" << m_cparam->sc_grab_ror_ratio << endl
 			<< "sc_grab_offset:\t" << m_cparam->sc_grab_offset << endl
 			<< "sc_grab_fork_th:\t" << m_cparam->sc_grab_fork_th << endl
+			<< "sc_grab_to_meanfork_max_dist:\t" << m_cparam->sc_grab_to_meanfork_max_dist << endl
 			<< "}" << endl; 	
 		return buff.str();
 	}

data_def_api.h

@@ -118,6 +118,7 @@ typedef struct{
 	double rs_grab_ror_ratio;
 	double rs_grab_offset;
 	double rs_grab_fork_th;
+	double rs_grab_to_meanfork_max_dist;
 
 	// scion grab based points cloud
 	double sc_grab_xmin;
@@ -134,6 +135,7 @@ typedef struct{
 	double sc_grab_ror_ratio;
 	double sc_grab_offset;
 	double sc_grab_fork_th;
+	double sc_grab_to_meanfork_max_dist;
 
 
 } ConfigParam;

gcv_conf.yml

@@ -90,7 +90,7 @@ conf_parameters:
    rs_grab_stem_min_pts: 50
    rs_grab_ror_ratio: 0.85
    rs_grab_offset: 0
-   rs_grab_fork_th: 0.7
+   rs_grab_fork_th: 1.5
 
 
    sc_grab_xmin: -200
@@ -111,6 +111,6 @@ conf_parameters:
    sc_grab_stem_min_pts: 45
    sc_grab_ror_ratio: 0.85
    sc_grab_offset: 0
-   sc_grab_fork_th: 0.7
+   sc_grab_fork_th: 1.5
    
 

grab_point_rs.cpp

@@ -41,7 +41,8 @@ namespace graft_cv {
 		m_1st_row_zmean_rs(-1.0),
 		m_1st_row_zmean_sc(-1.0),
 		m_cloud_mean_dist(0.0),
-		m_stem_fork_ys_size(20)
+		m_stem_fork_ys_size(20),
+		m_stem_fork_pos_mu(0)
 	{
 	}
 
@@ -108,6 +109,7 @@ namespace graft_cv {
 		}
 		if (m_cparam.image_show) {
 			pcl::visualization::PCLVisualizer viewer(m_pcdId + std::string(": raw point cloud"));
+			viewer.setBackgroundColor(0.35, 0.35, 0.35);
 			viewer.addCoordinateSystem();
 			viewer.addPointCloud<pcl::PointXYZ>(m_raw_cloud, "raw_cloud");
 			viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 1, 1, 1, "raw_cloud");
@@ -2465,60 +2467,78 @@ void CRootStockGrabPoint::line_filter(
 			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; }*/
+		//3 如果fork方法得到位置信息，进行更新，并记录历史
+		float max_power_th = m_cparam.rs_grab_fork_th;
+		float max_dist_to_mean_fork = m_cparam.rs_grab_to_meanfork_max_dist;
+		if (m_dtype == 0) { 
+			max_power_th = m_cparam.sc_grab_fork_th; 
+			max_dist_to_mean_fork = m_cparam.sc_grab_to_meanfork_max_dist;
+		}
 
 		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 (2.0*max_dist_to_boundary > max_power_th * mu) {
+				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(); }
+				m_stem_fork_pos_mu = int(get_hist_mean<int>(m_stem_fork_ys) + 0.5);
+
+				if (m_pLogger) {
+					stringstream buff;
+					float ratio = 0.0;
+					if (mu > 1.0e-3) { ratio = 2.0*max_dist_to_boundary / mu; }
+					buff << m_pcdId << ": update mean fork postiont is " << m_stem_fork_pos_mu<<
+						", fork width = "<< 2.0*max_dist_to_boundary<<
+						", stem mean widht = "<< mu<<
+						", ratio is = "<< ratio;
+					m_pLogger->INFO(buff.str());
+				}
+			}			
 		}
 		
-		if(m_dtype == 0){			
-			/*if (max_power > max_power_th) {
+
+		//4 用历史均值进行约束，如果偏差大于max_dist_to_mean_fork mm，采用历史均值
+		bool out_of_range = false;
+		if (m_stem_fork_pos_mu > 0) {
+			if (fabs(max_pos - (float)m_stem_fork_pos_mu) > max_dist_to_mean_fork) {
+				out_of_range = true;
+				int original_max_pos = max_pos;
+				max_pos = m_stem_fork_pos_mu;
 				max_pos_ref = max_pos;
-				max_pos += static_cast<int>(m_cparam.sc_grab_offset);
+				if (m_pLogger) {
+					stringstream buff;
+					buff << m_pcdId << ": warning，self fork postiont = " << original_max_pos <<
+						", USE mean fork postiont " << m_stem_fork_pos_mu;
+					m_pLogger->INFO(buff.str());
+				}
 			}
 			else {
-				if (max_pos_mu > 0) {
-					max_pos_ref = max_pos_mu;
-					max_pos = max_pos_mu;
-				}
-				else {
-					max_pos_ref = max_pos;
-					max_pos += static_cast<int>(m_cparam.sc_grab_offset);
+				if (m_pLogger) {
+					stringstream buff;
+					buff << m_pcdId << ": self fork postiont = "<< max_pos<<
+						", reference mean fork postiont = " << m_stem_fork_pos_mu;
+					m_pLogger->INFO(buff.str());
 				}
-			}			*/
+			}
+		}
+
+		//5 按指定量偏移
+		if(m_dtype == 0){
 			max_pos_ref = max_pos;
 			max_pos += static_cast<int>(m_cparam.sc_grab_offset);
 		}
 		else{			
-			/*if (max_power > max_power_th) {
-				max_pos_ref = max_pos;
-				max_pos += static_cast<int>(m_cparam.rs_grab_offset);
-			}
-			else {
-				if (max_pos_mu > 0) {
-					max_pos_ref = max_pos_mu;
-					max_pos = max_pos_mu;
-				}
-				else {
-					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; }
+		if (out_of_range) {
+			max_pos_ref = max_pos;
+		}
 
-		////用历史均值进行约束，如果偏差大于10mm，采用历史均值
-		//if (abs(max_pos - max_pos_mu) > 10) {
-		//	max_pos = max_pos_mu;
-		//	max_pos_ref = max_pos;
-		//}
-
+		
 		/////////////////////////////////////////////////////////////////////
 		//直线点云，获取指定区域的dx_line，dz_line
 		Eigen::Vector4f min_point;

grab_point_rs.h

@@ -36,6 +36,7 @@ namespace graft_cv {
 		//历史茎节位置，用于计算平均茎节位置
 		std::vector<int>m_stem_fork_ys;
 		int m_stem_fork_ys_size;
+		int m_stem_fork_pos_mu;
 
 
 

graft_cv_api.cpp

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