ReadMe.txt

@@ -114,4 +114,6 @@ v0.8.1 叶
 v0.8.2 叶子遮挡识别优化：用叶子点云数量判断是否有遮挡，加上识别的茎的位置，统计植株的个数
 v0.8.3 叶子遮挡识别优化：解决叶子遮挡不计数问题;修改点云密度计算方法
 v0.8.4 叶子遮挡识别优化：小叶遮挡，茎没有识别出来，植株误判问题
-v0.8.5 叶子遮挡识别优化：修改叶子遮挡识别位置偏离问题，增加历史抓取信息约束
+v0.8.5 叶子遮挡识别优化：修改叶子遮挡识别位置偏离问题，增加历史抓取信息约束
+v0.8.6 抓取位置调整
+v0.8.7 抓取顺序调整：先抓取有明确茎的位置，然后再去抓取遮挡位置的

grab_occlusion.cpp

@@ -620,7 +620,7 @@ void CSeedlingStatus::get_status(std::vector<bool>&xstatus)
 
 	//3.3 判断苗的整体情况
 	double seedling_distance = m_center_x.at(1) - m_center_x.at(0); //株间距离
-	double grid_one_seedling = seedling_distance / m_bin_step;		//没穴位占histogram的桶数
+	double grid_one_seedling = seedling_distance / m_bin_step;		//每穴位占histogram的桶数
 	//3.3.1进一排苗
 	if (add_cnt > grid_one_seedling*3.0) {		
 		xstatus.assign(m_center_x.size(), true);
@@ -683,18 +683,20 @@ void CSeedlingStatus::get_status(std::vector<bool>&xstatus)
 	}
 	if (sub_pos >= 0) {
 		xstatus.assign(m_center_x.size(), false);
-		if (m_dtype == 0) {
-			//穗苗
-			for (int kk = 0; kk <sub_pos; ++kk) {
-				xstatus.at(kk) = true;
-			}
+		int cursor = m_record_cursor - 1;
+		if (m_record_cursor < m_max_size) {
+			cursor = m_record_cursor - 1;		
 		}
 		else {
-			//砧木
-			for (int kk = sub_pos + 1; kk < m_center_x.size(); ++kk) {
-				xstatus.at(kk) = true;
+			cursor = m_max_size - 1;			
+		}
+		for (int i = 0; i < m_history_status.cols; ++i) {
+			if (m_history_status.at<unsigned char>(cursor, i) == 1) {
+				xstatus.at(i) = true;					
 			}
 		}
+		xstatus.at(sub_pos) = false;
+		
 		//update m_history_status
 		if (m_record_cursor < m_max_size) {
 			for (int i = 0; i < m_history_status.cols; ++i) {

grab_point_rs.cpp

@@ -339,7 +339,6 @@ namespace graft_cv {
 		get_point_x_hist(xhist_inbox);
 		m_pSeedlingStatus->append_hist(xhist_inbox, m_root_center_with_seedling_history);
 		
-		
 		////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 		// 4 对截取的点云进行ror滤除大面积联通区域，剔除叶片
 		pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_dowm_sampled(new pcl::PointCloud<pcl::PointXYZ>);
@@ -631,98 +630,98 @@ namespace graft_cv {
 			posinfo.rs_tortuosity = (double)stem_deflection;
 		}
 
-		// 1 找到基于rootcenter应该找的中心位置
-		//m_dtype == 0 找最大x位置， 否则找最小x位置
-		selected_idx = -1;
-		if (m_dtype == 0) {
-			for (int i = 0; i < m_root_center_with_seedling.size(); ++i) {
-				if (m_root_center_with_seedling[i]) { selected_idx = i; }
-			}
-		}
-		else {
-			for (int i = 0; i < m_root_center_with_seedling.size(); ++i) {
-				if (m_root_center_with_seedling[i]) {
-					selected_idx = i;
-					break;
-				}
-			}
-		}
-
-		if (selected_idx < 0) {
-			//可能没有中心（刚开始工作）
-			//按识别的结果			
-			return;
-		}
-
-		//2 如果识别的位置和rootcenter位置接近，就按识别位置
-		double seedling_distance = m_cparam.rs_grab_seedling_dist;		
-		if (m_dtype == 0) {
-			seedling_distance = m_cparam.sc_grab_seedling_dist;				
-		}
-		double root_x = m_root_centers.at(selected_idx).root_x;
-		if (m_dtype == 0) {
-			//穗苗找x最大的位置
-			if (selected_pt.x > (root_x - 0.5 * seedling_distance)) {
-				//找到x最大的位置上的苗，识别的苗位置大于有苗的位置，就认为识别结果更可信
-				return;
-			}
-			else {
-				//否则用指定根中心的位置
-				goto obstructed;
-			}
-		}
-		else {
-			//砧木，找x最小的位置
-			if (selected_pt.x < (root_x + 0.5 * seedling_distance)) {
-				//找到x最大的位置上的苗，识别的苗位置大于有苗的位置，就认为识别结果更可信
-				return;
-			}
-			else {
-				//否则用指定根中心的位置
-				goto obstructed;
-			}
-		}
-
-obstructed:
-		double grab_fork_ybt = m_cparam.rs_grab_fork_ybt;
-		double grab_offset = m_cparam.rs_grab_offset;
-		if (m_dtype == 0) {
-			grab_fork_ybt = m_cparam.sc_grab_fork_ybt;
-			grab_offset = m_cparam.sc_grab_offset;
-		}
-
-		//cx位置, 默认选择穴位中心,如果leaf中心有值，用叶子中心
-		double cx = m_root_centers.at(selected_idx).root_x;
-		double leaf_cx = m_root_center_leaf_cx.at(selected_idx);
-		if (fabs(leaf_cx - cx) < seedling_distance*0.33) {
-			cx = leaf_cx;
-		}
-
-		selected_pt_ref.x = cx;
-		selected_pt_ref.y = grab_fork_ybt;
-		selected_pt_ref.z = m_root_centers.at(selected_idx).root_z;
-		selected_pt = selected_pt_ref;
-		selected_pt.y += static_cast<int>(grab_offset);
-
-		double stem_width_mu_obstructed = 0.0;
-		double stem_deflection_obstructed = 0.0;
-		if (m_dtype == 0) {
-			posinfo.sc_grab_x = selected_pt.x;
-			posinfo.sc_grab_y = selected_pt.y;
-			posinfo.sc_grab_z = selected_pt.z;
-			posinfo.sc_count = (double)first_row_seedling_number;
-			posinfo.sc_width = stem_width_mu_obstructed;
-			posinfo.sc_tortuosity = stem_deflection_obstructed;
-		}
-		else {
-			posinfo.rs_grab_x = selected_pt.x;
-			posinfo.rs_grab_y = selected_pt.y;
-			posinfo.rs_grab_z = selected_pt.z;
-			posinfo.rs_count = (double)first_row_seedling_number;
-			posinfo.rs_width = stem_width_mu_obstructed;
-			posinfo.rs_tortuosity = stem_deflection_obstructed;
-		}
-
+//		// 1 找到基于rootcenter应该找的中心位置
+//		//m_dtype == 0 找最大x位置， 否则找最小x位置
+//		selected_idx = -1;
+//		if (m_dtype == 0) {
+//			for (int i = 0; i < m_root_center_with_seedling.size(); ++i) {
+//				if (m_root_center_with_seedling[i]) { selected_idx = i; }
+//			}
+//		}
+//		else {
+//			for (int i = 0; i < m_root_center_with_seedling.size(); ++i) {
+//				if (m_root_center_with_seedling[i]) {
+//					selected_idx = i;
+//					break;
+//				}
+//			}
+//		}
+//
+//		if (selected_idx < 0) {
+//			//可能没有中心（刚开始工作）
+//			//按识别的结果			
+//			return;
+//		}
+//
+//		//2 如果识别的位置和rootcenter位置接近，就按识别位置
+//		double seedling_distance = m_cparam.rs_grab_seedling_dist;		
+//		if (m_dtype == 0) {
+//			seedling_distance = m_cparam.sc_grab_seedling_dist;				
+//		}
+//		double root_x = m_root_centers.at(selected_idx).root_x;
+//		if (m_dtype == 0) {
+//			//穗苗找x最大的位置
+//			if (selected_pt.x > (root_x - 0.5 * seedling_distance)) {
+//				//找到x最大的位置上的苗，识别的苗位置大于有苗的位置，就认为识别结果更可信
+//				return;
+//			}
+//			else {
+//				//否则用指定根中心的位置
+//				goto obstructed;
+//			}
+//		}
+//		else {
+//			//砧木，找x最小的位置
+//			if (selected_pt.x < (root_x + 0.5 * seedling_distance)) {
+//				//找到x最大的位置上的苗，识别的苗位置大于有苗的位置，就认为识别结果更可信
+//				return;
+//			}
+//			else {
+//				//否则用指定根中心的位置
+//				goto obstructed;
+//			}
+//		}
+//
+//obstructed:
+//		double grab_fork_ybt = m_cparam.rs_grab_fork_ybt;
+//		double grab_offset = m_cparam.rs_grab_offset;
+//		if (m_dtype == 0) {
+//			grab_fork_ybt = m_cparam.sc_grab_fork_ybt;
+//			grab_offset = m_cparam.sc_grab_offset;
+//		}
+//
+//		//cx位置, 默认选择穴位中心,如果leaf中心有值，用叶子中心
+//		double cx = m_root_centers.at(selected_idx).root_x;
+//		double leaf_cx = m_root_center_leaf_cx.at(selected_idx);
+//		if (fabs(leaf_cx - cx) < seedling_distance*0.33) {
+//			cx = leaf_cx;
+//		}
+//
+//		selected_pt_ref.x = cx;
+//		selected_pt_ref.y = grab_fork_ybt;
+//		selected_pt_ref.z = m_root_centers.at(selected_idx).root_z;
+//		selected_pt = selected_pt_ref;
+//		selected_pt.y += static_cast<int>(grab_offset);
+//
+//		double stem_width_mu_obstructed = 0.0;
+//		double stem_deflection_obstructed = 0.0;
+//		if (m_dtype == 0) {
+//			posinfo.sc_grab_x = selected_pt.x;
+//			posinfo.sc_grab_y = selected_pt.y;
+//			posinfo.sc_grab_z = selected_pt.z;
+//			posinfo.sc_count = (double)first_row_seedling_number;
+//			posinfo.sc_width = stem_width_mu_obstructed;
+//			posinfo.sc_tortuosity = stem_deflection_obstructed;
+//		}
+//		else {
+//			posinfo.rs_grab_x = selected_pt.x;
+//			posinfo.rs_grab_y = selected_pt.y;
+//			posinfo.rs_grab_z = selected_pt.z;
+//			posinfo.rs_count = (double)first_row_seedling_number;
+//			posinfo.rs_width = stem_width_mu_obstructed;
+//			posinfo.rs_tortuosity = stem_deflection_obstructed;
+//		}
+//
 	}
 	//根据历史根的位置，计算对应位置点云数量，进而判断此位置是否有苗
 	void CRootStockGrabPoint::occluded_seedling_detect_by_leaf(
@@ -2568,29 +2567,46 @@ void CRootStockGrabPoint::line_filter(
 			grab_fork_ybt = m_cparam.sc_grab_fork_ybt;
 		}
 		bool out_of_range = false;
-		if ((max_pos + ymin) > grab_fork_yup || (max_pos + ymin) < grab_fork_ybt) {
+		if ((max_pos + ymin) > grab_fork_yup) {
 			out_of_range = true;
 			int original_max_pos = max_pos;
-			max_pos = int(grab_fork_ybt - ymin + 0.5);
+			max_pos = int(grab_fork_yup - ymin + 0.5);
 			max_pos_ref = max_pos;
 			if (m_pLogger) {
 				stringstream buff;
 				buff << m_pcdId << ": warning，self fork postiont = " << original_max_pos <<
-					", USE bottom limit fork postiont " << max_pos <<
+					", USE up limit fork postiont " << max_pos <<
 					", valid fork postiont range:[" << int(grab_fork_ybt - ymin + 0.5) <<
 					", " << int(grab_fork_yup - ymin + 0.5) << "]";
 				m_pLogger->INFO(buff.str());
 			}
 		}
 		else {
-			if (m_pLogger) {
-				stringstream buff;
-				buff << m_pcdId << ": self fork postiont = " << max_pos <<
-					", valid fork postiont range:[" << int(grab_fork_ybt - ymin + 0.5)<<
-					", "<< int(grab_fork_yup - ymin + 0.5) << "]";
-				m_pLogger->INFO(buff.str());
+
+			if ((max_pos + ymin) < grab_fork_ybt) {
+				out_of_range = true;
+				int original_max_pos = max_pos;
+				max_pos = int(grab_fork_ybt - ymin + 0.5);
+				max_pos_ref = max_pos;
+				if (m_pLogger) {
+					stringstream buff;
+					buff << m_pcdId << ": warning，self fork postiont = " << original_max_pos <<
+						", USE bottom limit fork postiont " << max_pos <<
+						", valid fork postiont range:[" << int(grab_fork_ybt - ymin + 0.5) <<
+						", " << int(grab_fork_yup - ymin + 0.5) << "]";
+					m_pLogger->INFO(buff.str());
+				}
 			}
-		}	
+			else {
+				if (m_pLogger) {
+					stringstream buff;
+					buff << m_pcdId << ": self fork postiont = " << max_pos <<
+						", valid fork postiont range:[" << int(grab_fork_ybt - ymin + 0.5) <<
+						", " << int(grab_fork_yup - ymin + 0.5) << "]";
+					m_pLogger->INFO(buff.str());
+				}
+			}
+		}
 		
 
 		//5 按指定量偏移

grab_point_rs.h

@@ -34,7 +34,7 @@ namespace graft_cv {
 		std::vector<double> m_root_center_leaf_cx;//穴位上，如有叶子，叶子中心，或按整体点云中心，默认1.0e6
 		std::vector<bool> m_root_center_with_seedling;	//m_root_centers位置上是否含有植株
 		std::vector<bool> m_root_center_with_seedling_history;	//m_root_centers位置上是否含有植株(通过历史信息获取)
-
+		
 		//用于记录第一排z均值，用来辅助判别1、2排的苗
 		float m_1st_row_zmean_rs = -1.0;
 		float m_1st_row_zmean_sc = -1.0;

graft_cv_api.cpp

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