v0.8.5 叶子遮挡识别优化：修改叶子遮挡识别位置偏离问题，增加历史抓取信息约束

ReadMe.txt

@@ -113,4 +113,5 @@ v0.8.0 叶
 v0.8.1 叶子遮挡识别优化：通过x截取范围和穴位个数直接确定茎根中心位置
 v0.8.2 叶子遮挡识别优化：用叶子点云数量判断是否有遮挡，加上识别的茎的位置，统计植株的个数
 v0.8.3 叶子遮挡识别优化：解决叶子遮挡不计数问题;修改点云密度计算方法
-v0.8.4 叶子遮挡识别优化：小叶遮挡，茎没有识别出来，植株误判问题
+v0.8.4 叶子遮挡识别优化：小叶遮挡，茎没有识别出来，植株误判问题
+v0.8.5 叶子遮挡识别优化：修改叶子遮挡识别位置偏离问题，增加历史抓取信息约束

grab_occlusion.cpp

@@ -1,6 +1,7 @@
 #include <sstream>
 #include <opencv2\highgui\highgui.hpp>
 #include "grab_occlusion.h"
+#include "utils.h"
 
 
 namespace graft_cv {
@@ -451,6 +452,297 @@ namespace graft_cv {
 	//	}
 	//}
 
+CSeedlingStatus::CSeedlingStatus(
+	int dtype,
+	double step,
+	double x_min,
+	double x_max,	
+	double pc_mean_dist,
+	CGcvLogger*pLog)
+	: m_pLogger(pLog)
+	, m_dtype(dtype)
+	, m_bin_step(step)
+	, m_xmin(x_min)
+	, m_xmax(x_max)
+	, m_record_cursor(-1)
+	, m_max_size(50)
+	, m_pc_mean_dist(pc_mean_dist)
+{
+	int x0 = int(x_min);
+	int x1 = int(x_max);
+	m_hist_length = int((x1 - x0) / step);	
+	m_history_histogram = cv::Mat::zeros(m_max_size, m_hist_length, CV_32F);
+	m_history_point_size = cv::Mat::zeros(m_max_size, 1, CV_32F);	
+}
 
+CSeedlingStatus::~CSeedlingStatus()
+{}
+
+void CSeedlingStatus::set_x_centers(std::vector<double>&cx)
+{
+	m_center_x.clear();
+	for (auto&x : cx) {
+		m_center_x.push_back(x);
+	}
+	std::sort(m_center_x.begin(), m_center_x.end());
+
+	double seedling_distance = m_center_x.at(1) - m_center_x.at(0);
+	m_idx_low.clear();
+	m_idx_up.clear();
+	for (int i = 0; i < m_center_x.size(); ++i) {
+		int idx_low = int((m_center_x.at(i) - 0.5 * seedling_distance - m_xmin) / m_bin_step);
+		int idx_up = int((m_center_x.at(i) + 0.5 * seedling_distance - m_xmin) / m_bin_step);
+		m_idx_low.push_back(idx_low);
+		m_idx_up.push_back(idx_up);
+	}
+	m_history_status = cv::Mat::zeros(m_max_size, m_center_x.size(), CV_8U);
+
+}
+void CSeedlingStatus::append_hist(
+	std::vector<int>&xhist,		//input
+	std::vector<bool>&xstatus	//output
+)
+{
+	assert(xhist.size() == m_hist_length);
+	
+	if (m_record_cursor < m_max_size) {
+		m_record_cursor++;
+		float pc_size = 0.0;
+		for (int i = 0; i < xhist.size(); ++i) {
+			m_history_histogram.at<float>(m_record_cursor,i) = xhist.at(i);
+			pc_size += xhist.at(i);
+		}
+		m_history_point_size.at<float>(m_record_cursor, 0) = pc_size;
+		get_status(xstatus);		
+	}
+	else {
+		//数据上移一行，数据放在最后一行
+		memcpy_s(m_history_histogram.data,
+			m_history_histogram.step[0] * (m_max_size - 1),
+			m_history_histogram.data + m_history_histogram.step[0], 
+			m_history_histogram.step[0] * (m_max_size - 1));
+
+		memcpy_s(m_history_point_size.data,
+			m_history_point_size.step[0] * (m_max_size - 1),
+			m_history_point_size.data + m_history_point_size.step[0],
+			m_history_point_size.step[0] * (m_max_size - 1));
+
+		float pc_size = 0.0;
+		for (int i = 0; i < xhist.size(); ++i) {
+			m_history_histogram.at<float>(m_max_size - 1, i) = float(xhist.at(i));
+			pc_size += xhist.at(i);
+		}
+		m_history_point_size.at<float>(m_max_size - 1, 0) = pc_size;
+		get_status(xstatus);
+	}
+	
+
+}
+void CSeedlingStatus::get_status(std::vector<bool>&xstatus)
+{
+	xstatus.clear();
+	xstatus.assign(m_center_x.size(), false);
+
+	//1 如果没有记录输入，返回没有苗
+	if (m_record_cursor < 0) { return; }
+
+	//2 如果是第一次，没有参考，用自身的分布阈值判断是否有苗（可能不准确，但没有其他办法）
+	//点云分布情况分析
+	// 每个bin的数量阈值设定m_bin_step宽度，至少有10毫米的点云，才认为有苗
+	float th_hist = m_bin_step * 10 / m_pc_mean_dist / m_pc_mean_dist; 
+	if (m_record_cursor == 0) {
+		std::vector<bool> hist_status;		
+		hist_status.assign(m_hist_length, false);
+		for (int i = 0; i < m_hist_length; ++i) {
+			if (m_history_histogram.at<float>(m_record_cursor, i) > th_hist) {
+				hist_status.at(i) = true;
+			}
+		}
+		for (int i = 0; i < m_center_x.size(); ++i) {
+			int idx_low = m_idx_low.at(i);
+			int idx_up = m_idx_up.at(i);
+			int  valid_bin_cnt = 0;
+			for (int k = idx_low; k < idx_up; ++k) {
+				if (hist_status.at(k)) { valid_bin_cnt++; }
+			}
+			double valid_ratio = (double)valid_bin_cnt / (double)(idx_up - idx_low);
+			xstatus.at(i) = valid_ratio > 0.5;			
+		}
+		//update m_history_status
+		for (int i = 0; i < m_history_status.cols; ++i) {
+			m_history_status.at<unsigned char>(m_record_cursor, i) = xstatus.at(i);
+		}
+		return;
+	}
+
+	//3 2次或更多，通过前后2次差分析苗取走的情况
+	//3.1 计算被取走的点云位置分布
+	std::vector<float>hist_diff;
+	hist_diff.assign(m_hist_length, 0.0);
+	float sum_dn = 0.0;
+	float sum_n = 0.0;
+	float diff_cnt = 0.0;
+	if (m_record_cursor < m_max_size) {
+		for (int i = 0; i < m_hist_length; ++i) {
+			diff_cnt = m_history_histogram.at<float>(m_record_cursor - 1, i) - 
+				m_history_histogram.at<float>(m_record_cursor, i);
+			hist_diff.at(i) =  diff_cnt;
+			sum_n += diff_cnt;
+			sum_dn += diff_cnt * i;
+		}
+	}
+	else {
+		for (int i = 0; i < m_hist_length; ++i) {
+			diff_cnt = m_history_histogram.at<float>(m_max_size - 2, i) -
+				m_history_histogram.at<float>(m_max_size - 1, i);
+			hist_diff.at(i) = diff_cnt;
+			sum_n += diff_cnt;
+			sum_dn += diff_cnt * i;
+		}
+
+	}
+	
+	//3.2 统计增减点云的状态，区分点云增加，点云减小，点云没变化的部分
+	std::vector<int> hist_status_2d;	//3种状态记录： -1取走，0没变化，1上苗
+	hist_status_2d.assign(m_hist_length, 0);
+	int add_cnt = 0;
+	int sub_cnt = 0;
+	for (int i = 0; i < m_hist_length; ++i) {
+		if (hist_diff.at(i) > th_hist) {
+			hist_status_2d.at(i) = -1;
+			sub_cnt += 1;
+		}
+		if (hist_diff.at(i) < -th_hist) {
+			hist_status_2d.at(i) = 1;
+			add_cnt += 1;
+		}
+	}
+
+	//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的桶数
+	//3.3.1进一排苗
+	if (add_cnt > grid_one_seedling*3.0) {		
+		xstatus.assign(m_center_x.size(), true);
+		//update m_history_status
+		if (m_record_cursor < m_max_size) {
+			for (int i = 0; i < m_history_status.cols; ++i) {
+				m_history_status.at<unsigned char>(m_record_cursor, i) = xstatus.at(i);
+			}
+		}
+		else {
+			memcpy_s(m_history_status.data,
+				m_history_status.step[0] * (m_max_size - 1),
+				m_history_status.data + m_history_status.step[0],
+				m_history_status.step[0] * (m_max_size - 1));
+			
+			for (int i = 0; i < m_history_status.cols; ++i) {
+				m_history_status.at<unsigned char>(m_max_size-1, i) = xstatus.at(i);
+			}
+		}		
+		return;
+	}
+
+	std::vector<size_t>sorted_idx;
+	std::vector<float>sub_seedling_score;	//移出植株得分，记录每个穴位上点云变化得分
+	//3.3.2 变化很小，说明没有改变（没能成功抓走）
+	if (add_cnt + sub_cnt < 0.5 * grid_one_seedling) {
+		goto no_change;
+	}
+	//3.3.3 否则的话，就是抓走过一个苗
+	//找到被取走的苗的中心，然后根据dtype确定有苗的位置
+	//找覆盖范围最大的区域
+	double sub_cent_indx = sum_dn / sum_n;	//计算改变范围的中心，目前没用到	
+	sub_seedling_score.assign(m_center_x.size(), 0.0);
+	for (int idx = 0; idx < hist_status_2d.size(); ++idx) {
+		if (hist_status_2d.at(idx) >= 0) {
+			//这个histogram上没有移出，不统计， hist_status_2d的值域：-1取走，0没变化，1上苗
+			continue; 
+		}
+		for (int i = 0; i < m_center_x.size(); ++i) {
+			int idx_low = m_idx_low.at(i);
+			int idx_up = m_idx_up.at(i);
+			if (idx >= idx_low && idx < idx_up) {
+				sub_seedling_score.at(i) += 1.0;
+			}
+		}
+	}
+	int sub_pos = -1;
+	sorted_idx = sort_indexes_e(sub_seedling_score, false);
+	for (auto& idx : sorted_idx) {
+		if (sub_seedling_score.at(idx) < 0.25 * grid_one_seedling) {
+			//如果改变量，不到穴位范围的一半，不认为是移走的
+			continue; 
+		}
+		if (m_history_status.at<unsigned char>(m_record_cursor - 1, idx) == 0) { 
+			//如果这个位置上一帧就没有苗，那判别也是错误的
+			continue; 
+		}
+		sub_pos = idx;
+		break;//找到得分最高，并且满足条件的位置，就是被抓走的位置，跳出
+	}
+	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;
+			}
+		}
+		else {
+			//砧木
+			for (int kk = sub_pos + 1; kk < m_center_x.size(); ++kk) {
+				xstatus.at(kk) = true;
+			}
+		}
+		//update m_history_status
+		if (m_record_cursor < m_max_size) {
+			for (int i = 0; i < m_history_status.cols; ++i) {
+				m_history_status.at<unsigned char>(m_record_cursor, i) = xstatus.at(i);
+			}
+		}
+		else{
+			memcpy_s(m_history_status.data,
+				m_history_status.step[0] * (m_max_size - 1),
+				m_history_status.data + m_history_status.step[0],
+				m_history_status.step[0] * (m_max_size - 1));
+			for (int i = 0; i < m_history_status.cols; ++i) {
+				m_history_status.at<unsigned char>(m_max_size - 1, i) = xstatus.at(i);
+			}
+		}		
+		return;
+	}
+	else {
+		//如果没有找到有效位置，按没有变化处理
+		goto no_change;
+	}
+
+no_change:
+	//没有改变,用上一次的结果
+	xstatus.assign(m_center_x.size(), true);
+	//update m_history_status
+	if (m_record_cursor < m_max_size) {
+		for (int i = 0; i < m_history_status.cols; ++i) {
+			m_history_status.at<unsigned char>(m_record_cursor, i) = 
+				m_history_status.at<unsigned char>(m_record_cursor - 1, i);
+			if (m_history_status.at<unsigned char>(m_record_cursor - 1, i) == 0) {
+				xstatus.at(i) = false;
+			}
+		}
+	}
+	else{
+		memcpy_s(m_history_status.data,
+			m_history_status.step[0] * (m_max_size - 1),
+			m_history_status.data + m_history_status.step[0],
+			m_history_status.step[0] * (m_max_size - 1));
+		for (int i = 0; i < m_history_status.cols; ++i) {
+			m_history_status.at<unsigned char>(m_max_size - 1, i) = 
+				m_history_status.at<unsigned char>(m_max_size - 2, i);
+			if (m_history_status.at<unsigned char>(m_max_size - 2, i) == 0) {
+				xstatus.at(i) = false;
+			}
+		}
+	}
+}
 
 }

grab_occlusion.h

@@ -91,4 +91,42 @@ namespace graft_cv {
 		//);
 		void gen_root_centers();
 	};
+
+	//植株状态，记录植株点云分布情况，用于确定植株是否被取走，穴位是否有苗
+	class CSeedlingStatus {
+	public:
+		CSeedlingStatus(
+			int dtype,		//1-砧木， 0-穗苗
+			double step,	//x方向直方图点云分布，每个bin的宽度（默认5毫米）
+			double x_min,	//x的下限
+			double x_max,	//x的上限
+			double pc_mean_dist,	//点云，两点间平均距离
+			CGcvLogger*pLog = 0);
+		~CSeedlingStatus();
+		void set_x_centers(std::vector<double>&cx);
+		void append_hist(std::vector<int>&xhist,	//input
+			std::vector<bool>&xstatus);				//output
+		
+	protected:
+		CGcvLogger * m_pLogger;
+		int m_max_size;	//最大记录长度
+		int m_record_cursor;
+		cv::Mat m_history_histogram;	//历史点云x方向分布直方图
+		cv::Mat m_history_point_size;	//历史点云框内数量
+		cv::Mat m_history_status;		//历史穴位是有苗状态
+		std::vector<int> m_idx_low;		//穴位对应的序号低位
+		std::vector<int> m_idx_up;		//穴位对应的序号低位
+
+		int m_dtype;					//1-砧木， 0-穗苗
+		double m_xmin;					//x的下限
+		double m_xmax;					//x的上限
+		double m_bin_step;				//x方向直方图点云分布，每个bin的宽度（默认5毫米）
+		double m_pc_mean_dist;			//点云，两点间平均距离
+		std::vector<double>m_center_x;
+		int m_hist_length;
+
+		void get_status(std::vector<bool>&xstatus);
+		
+		
+	};
 }

grab_point_rs.cpp

@@ -42,7 +42,8 @@ namespace graft_cv {
 		m_1st_row_zmean_sc(-1.0),
 		m_cloud_mean_dist(0.0),
 		m_pImginfoResult(0),
-		m_pStemInfos(0)
+		m_pStemInfos(0),
+		m_pSeedlingStatus(0)
 	{		
 	}
 	void CRootStockGrabPoint::clear_imginfo() {
@@ -59,6 +60,10 @@ namespace graft_cv {
 			delete m_pStemInfos;
 			m_pStemInfos = 0;
 		}
+		if (m_pSeedlingStatus) {
+			delete m_pSeedlingStatus;
+			m_pSeedlingStatus = 0;
+		}
 	}
 	float* CRootStockGrabPoint::get_raw_point_cloud(int &data_size)
 	{
@@ -193,7 +198,7 @@ namespace graft_cv {
 				seedling_distance, holes_number,
 				x_min, x_max, z_min, z_max,
 				m_pcdId, m_pLogger);
-		}
+		}		
 		return rst;
 	}
 
@@ -312,6 +317,28 @@ namespace graft_cv {
 			buff << m_pcdId <<": cloud_mean_dist = " << m_cloud_mean_dist;
 			m_pLogger->INFO(buff.str());
 		}
+
+		if (m_pSeedlingStatus == 0) {
+			
+			double x_min = m_cparam.rs_grab_xmin;
+			double x_max = m_cparam.rs_grab_xmax;
+			
+			if (m_dtype == 0) {				
+				x_min = m_cparam.sc_grab_xmin;
+				x_max = m_cparam.sc_grab_xmax;				
+			}
+			m_pSeedlingStatus = new CSeedlingStatus(m_dtype, 5.0, x_min, x_max, m_cloud_mean_dist, m_pLogger);
+			std::vector<double>root_cxs;
+			for (auto&sr : m_root_centers) {
+				root_cxs.push_back(sr.root_x);
+			}
+			m_pSeedlingStatus->set_x_centers(root_cxs);
+		}
+
+		std::vector<int>xhist_inbox;		
+		get_point_x_hist(xhist_inbox);
+		m_pSeedlingStatus->append_hist(xhist_inbox, m_root_center_with_seedling_history);
+		
 		
 		////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 		// 4 对截取的点云进行ror滤除大面积联通区域，剔除叶片
@@ -333,6 +360,13 @@ namespace graft_cv {
 		////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 		//判断m_root_centers位置上是否有叶片遮挡
 		occluded_seedling_detect_by_leaf(cloud_ror, leaf_indices);
+		//用m_root_center_with_seedling_history对叶子遮挡的结果进行过滤
+		/*for (int j = 0; j < m_root_center_with_seedling.size(); ++j) {
+			if (m_root_center_with_seedling.at(j) && !m_root_center_with_seedling_history.at(j)) {
+				m_root_center_with_seedling.at(j) = false;
+			}
+		}*/
+		m_root_center_with_seedling = m_root_center_with_seedling_history;
 		
 
 		////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
@@ -525,12 +559,21 @@ namespace graft_cv {
 
 		double grab_fork_ybt = m_cparam.rs_grab_fork_ybt;
 		double grab_offset = m_cparam.rs_grab_offset;
+		double grab_seedling_dist = m_cparam.rs_grab_seedling_dist;
 		if (m_dtype == 0) {
 			grab_fork_ybt = m_cparam.sc_grab_fork_ybt;
 			grab_offset = m_cparam.sc_grab_offset;
+			grab_seedling_dist = m_cparam.sc_grab_seedling_dist;
+		}
+
+		//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) < grab_seedling_dist*0.33) {
+			cx = leaf_cx;
 		}
 		
-		selected_pt_ref.x = m_root_centers.at(selected_idx).root_x;
+		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;
@@ -648,7 +691,14 @@ obstructed:
 			grab_offset = m_cparam.sc_grab_offset;
 		}
 
-		selected_pt_ref.x = m_root_centers.at(selected_idx).root_x;
+		//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;
@@ -684,6 +734,10 @@ obstructed:
 		//所以改成整个空间点云数量做判别--2024-3-2
 		m_root_center_with_seedling.clear();
 		m_root_center_with_seedling.assign(m_root_centers.size(), false);
+
+		m_root_center_leaf_cx.clear();
+		m_root_center_leaf_cx.assign(m_root_centers.size(), 1.0e6);
+
 		std::vector<int> pcd_cnt;
 		//int th_pcd_size = m_cparam.rs_grab_seedling_min_pts;
 		double stem_diameter = m_cparam.rs_grab_stem_diameter;
@@ -696,15 +750,34 @@ obstructed:
 			y_min = m_cparam.sc_grab_ymin;
 			y_max = m_cparam.sc_grab_ymax;
 		}
-		int th_pcd_size = 0.333 * stem_diameter * (y_max - y_min) / m_cloud_mean_dist / m_cloud_mean_dist;
-		std::vector<pcl::PointXYZ> aabb_mins_maxs;		
+		int th_pcd_size = int(0.333 * stem_diameter * (y_max - y_min) / m_cloud_mean_dist / m_cloud_mean_dist);
+		std::vector<pcl::PointXYZ> aabb_mins_maxs;	
+		int total_cnt, leaf_cnt; //穴位上inbox的点云数量，叶子点云数量
+		double total_cx, leaf_cx;//穴位上inbox的点云中位x，叶子点云中位x
 		for (int i = 0; i < m_root_centers.size(); ++i) {
 			CStemResult& rc = m_root_centers.at(i);
 			pcl::PointXYZ aabb_min;
-			pcl::PointXYZ aabb_max;
-			int cnt = get_point_count_inbox(rc, aabb_min, aabb_max, in_cloud);
-			pcd_cnt.push_back(cnt);			
-			bool has_seedling = cnt > th_pcd_size;
+			pcl::PointXYZ aabb_max;			
+			get_leaf_point_count_inbox(rc, in_cloud, leaf_idx, 
+				aabb_min, aabb_max,
+				total_cnt, leaf_cnt,
+				total_cx, leaf_cx);
+
+			double leaf_ratio = 0.0;
+			if (total_cnt == 0) {
+				leaf_ratio = 0.0;
+			}
+			else {
+				leaf_ratio = double(leaf_cnt) / double(total_cnt);
+			}
+			if (leaf_ratio > 0.25) {
+				m_root_center_leaf_cx.at(i) = leaf_cx;
+			}
+			else {
+				m_root_center_leaf_cx.at(i) = total_cx;
+			}
+			pcd_cnt.push_back(total_cnt);
+			bool has_seedling = total_cnt > th_pcd_size;
 			m_root_center_with_seedling.at(i) = has_seedling;
 			aabb_mins_maxs.push_back(aabb_min);
 			aabb_mins_maxs.push_back(aabb_max);			
@@ -712,7 +785,7 @@ obstructed:
 
 		if (m_pLogger) {
 			stringstream buff;
-			buff << m_pcdId << ": root positions points size " << m_root_centers.size() << " centers\n";
+			buff << m_pcdId << ": root positions points size [inbox] " << m_root_centers.size() << " centers, size_threshold: "<< th_pcd_size<<"\n";
 			for (int i = 0; i < m_root_centers.size();++i) {
 				CStemResult& sr = m_root_centers.at(i);
 
@@ -773,12 +846,99 @@ obstructed:
 	//	return count;
 	//}
 
-	int CRootStockGrabPoint::get_point_count_inbox(const CStemResult& sr,
-		pcl::PointXYZ& aabb_min,
-		pcl::PointXYZ& aabb_max,
-		pcl::PointCloud<pcl::PointXYZ>::Ptr in_cloud	//input 输入点云数据			
+	//统计inbox点云在x方向的分布情况
+	void CRootStockGrabPoint::get_point_x_hist(
+		std::vector<int>& x_hist	//output			
+	)
+	{
+		double seedling_distance = m_cparam.rs_grab_seedling_dist;
+		double x_min = m_cparam.rs_grab_xmin;
+		double x_max = m_cparam.rs_grab_xmax;
+		double y_min = m_cparam.rs_grab_ymin;
+		double y_max = y_min + 150.0;
+		double z_min = m_cparam.rs_grab_zmin - seedling_distance;
+		double z_max = m_cparam.rs_grab_zmax + seedling_distance/4.0;
+
+		if (m_dtype == 0) {
+			//穗苗
+			seedling_distance = m_cparam.sc_grab_seedling_dist;
+			x_min = m_cparam.sc_grab_xmin;
+			x_max = m_cparam.sc_grab_xmax;
+			y_min = m_cparam.sc_grab_ymin;
+			y_max = y_min + 150.0;
+			z_min = m_cparam.sc_grab_zmin - seedling_distance;
+			z_max = m_cparam.sc_grab_zmax;
+		}
+		double binw = 5;
+		int x0 = int(x_min);
+		int x1 = int(x_max);
+		int h_size = int((x1 - x0)/binw);
+		int idx = 0;
+		x_hist.assign(h_size, 0);
+
+		for (auto&pt : m_raw_cloud->points) {
+			if(pt.y >= y_min && pt.y < y_max &&
+				pt.x >=x_min && pt.x < x_max &&
+				pt.z >= z_min && pt.z < z_max)
+			{ 
+				idx = int((pt.x - x0) / binw);
+				x_hist.at(idx)++;
+			}
+		}
+
+		//显示结果
+		if (m_cparam.image_show) {
+			pcl::visualization::PCLVisualizer viewer(m_pcdId + std::string(": x histogram"));
+			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");
+			viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "raw_cloud");			
+
+			viewer.addCube(x_min, x_max, y_min, y_max, z_min, z_max, 0.75, 0.0, 0.0, "AABB_");
+			viewer.setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_REPRESENTATION,
+				pcl::visualization::PCL_VISUALIZER_REPRESENTATION_WIREFRAME, "AABB_");
+
+			pcl::PointXYZ px0, px1, py1, pz1;
+			px0.x = 0;
+			px0.y = 0;
+			px0.z = 0;
+			px1.x = 10.0;
+			px1.y = 0;
+			px1.z = 0;
+
+
+			py1.x = 0;
+			py1.y = 10.0;
+			py1.z = 0;
+			pz1.x = 0;
+			pz1.y = 0;
+			pz1.z = 10.0;
+
+			viewer.addLine(px0, px1, 255, 0, 0, "x");
+			viewer.addLine(px0, py1, 0, 255, 0, "y");
+			viewer.addLine(px0, pz1, 0, 0, 255, "z");
+
+			while (!viewer.wasStopped()) {
+				viewer.spinOnce(100);
+				boost::this_thread::sleep(boost::posix_time::microseconds(100000));
+			}
+		}
+	}
+
+	void CRootStockGrabPoint::get_leaf_point_count_inbox(
+		const CStemResult& sr,//input		
+		pcl::PointCloud<pcl::PointXYZ>::Ptr in_cloud,	//input 输入点云数据	
+		std::vector<int>& leaf_idx,	//input
+		pcl::PointXYZ& aabb_min,//output
+		pcl::PointXYZ& aabb_max,//output		
+		int& total_cnt,		//output
+		int& leaf_cnt,		//output
+		double& total_cx,	//output
+		double& leaf_cx		//output
 		)
 	{
+		//计算每一个穴位叶子遮挡的点云数量（以及整体点云数量），并计算x方向的中心
 		double seedling_distance = m_cparam.rs_grab_seedling_dist;
 		double min_y = m_cparam.rs_grab_ymin;
 		double max_y = m_cparam.rs_grab_ymax;
@@ -790,25 +950,51 @@ obstructed:
 		double min_x = sr.root_x - 0.5 * seedling_distance;
 		double max_x = sr.root_x + 0.5 * seedling_distance;
 		double min_z = sr.root_z - 0.75 * seedling_distance;
-		double max_z = sr.root_z + 0.25 * seedling_distance;		
+		double max_z = sr.root_z + 0.25 * seedling_distance;
+		aabb_min.x = min_x;
+		aabb_min.y = min_y;
+		aabb_min.z = min_z;
+		aabb_max.x = max_x;
+		aabb_max.y = max_y;
+		aabb_max.z = max_z;
+
+		total_cnt = 0;
+		leaf_cnt = 0;
+		total_cx = 1.0e6;
+		leaf_cx = 1.0e6;
+		std::vector<double>xs;
 
-		int count = 0;
 		for (auto&pt : in_cloud->points) {			
 			if (pt.y >= min_y && pt.y <= max_y &&
 				pt.x >= min_x && pt.x <= max_x &&
 				pt.z >= min_z && pt.z <= max_z)
 			{
-				count++;
+				total_cnt++;
+				xs.push_back(pt.x);
 			}
 		}
-		aabb_min.x = min_x;
-		aabb_min.y = min_y;
-		aabb_min.z = min_z;
-		aabb_max.x = max_x;
-		aabb_max.y = max_y;
-		aabb_max.z = max_z;
+		if (total_cnt == 0) { return; }
+		int med_idx = int(0.5 * xs.size());
+		std::sort(xs.begin(), xs.end());
+		total_cx = xs.at(med_idx);
 
-		return count;
+		//leaf center
+		xs.clear();
+		for (auto&i : leaf_idx) {
+			pcl::PointXYZ& pt = in_cloud->points.at(i);
+			if (pt.y >= min_y && pt.y <= max_y &&
+				pt.x >= min_x && pt.x <= max_x &&
+				pt.z >= min_z && pt.z <= max_z)
+			{
+				leaf_cnt++;
+				xs.push_back(pt.x);
+			}
+		}
+		if (leaf_cnt == 0) { return; }
+		med_idx = int(0.5 * xs.size());
+		std::sort(xs.begin(), xs.end());
+		leaf_cx = xs.at(med_idx);
+		return;
 	}
 
 	//生成结果图片

grab_point_rs.h

@@ -27,10 +27,13 @@ namespace graft_cv {
 
 		//历史植株位置信息
 		CStemResultInfos* m_pStemInfos;
+		CSeedlingStatus* m_pSeedlingStatus;
 		std::string m_stem_info_file;
 		std::vector<CStemResult> m_root_centers;  //通过m_pStemInfos获取到历史根中心位置
-		std::vector<int> m_root_center_pcd_size;
+		std::vector<int> m_root_center_pcd_size;  //穴位上整体点云数量
+		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;
@@ -181,11 +184,22 @@ namespace graft_cv {
 		void occluded_seedling_detect_by_leaf(
 			pcl::PointCloud<pcl::PointXYZ>::Ptr in_cloud,	//input 输入点云数据			
 			std::vector<int>& leaf_idx);	//input
-		int get_point_count_inbox(const CStemResult& sr, //input
+
+		//统计inbox点云在x方向的分布情况
+		void get_point_x_hist(
+			std::vector<int>& x_hist	//output			
+		);
+		void get_leaf_point_count_inbox(
+			const CStemResult& sr, //input
+			pcl::PointCloud<pcl::PointXYZ>::Ptr in_cloud,	//input 输入点云数据			
+			std::vector<int>& leaf_idx,				//input 叶子点云index
 			pcl::PointXYZ& aabb_min,	//output
 			pcl::PointXYZ& aabb_max,	//output
-			pcl::PointCloud<pcl::PointXYZ>::Ptr in_cloud	//input 输入点云数据			
-			);
+			int& total_cnt,		//output
+			int& leaf_cnt,		//output
+			double& total_cx,	//output
+			double& leaf_cx		//output
+		);
 		//没有检测到苗的情况，后处理
 		void no_seedling_detected_post_process(
 			int first_row_seedling_number,		//input
@@ -215,7 +229,10 @@ namespace graft_cv {
 			pcl::PointXYZ &root_pt,
 			std::string&winname);
 
-		void viewer_cloud_cluster(pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud, std::vector<pcl::PointXYZ>cluster_center, std::string&winname);
+		void viewer_cloud_cluster(
+			pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud,
+			std::vector<pcl::PointXYZ>cluster_center, 
+			std::string&winname);
 		void viewer_cloud_cluster_box(
 			pcl::PointCloud<pcl::PointXYZ>::Ptr cloud,
 			std::vector<pcl::PointXYZ>&cluster_center,

graft_cv_api.cpp

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