graft_cv/grab_occlusion.cpp

#include <sstream>
#include <opencv2\highgui\highgui.hpp>
#include "grab_occlusion.h"
#include "utils.h"


namespace graft_cv {
	CStemResultInfos::CStemResultInfos(double seedling_dist,
		int holes_number,
		double x_min,
		double x_max,
		double z_min,
		double z_max,
		std::string pcd_id, 
		CGcvLogger*pLog)
		: m_pLogger(pLog)
		, m_seedling_dist(seedling_dist)
		, m_holes_number(holes_number)
		, m_xmin(x_min)
		, m_xmax(x_max)
		, m_zmin(z_min)
		, m_zmax(z_max)
		//, m_append_counter(0)
		, m_pcdId(pcd_id)	
		, m_max_size(50)
	{
		gen_root_centers();
	}
	
	CStemResultInfos::~CStemResultInfos()
	{}

	void CStemResultInfos::append(
		CStemResult& sr
	)
	{

		m_infos.insert(m_infos.begin(), sr);
		if (m_infos.size() > m_max_size) {
			m_infos.pop_back();
		}
		//m_append_counter += 1;
		//每次都更新
		_update_root_centers(sr);
		//if (m_append_counter % 10 == 0) {
		//	//定期写入数据
		//	_update_root_centers();
		//	write_root_centers(m_json_filename);
		//}
	}
	void CStemResultInfos::clear()
	{
		m_infos.clear();
	}
	void CStemResultInfos::get_root_centers(
		std::vector<CStemResult>&rst
	)
	{		
		rst.clear();
		for (auto& sr : m_root_centers) {
			rst.push_back(sr);
		}		
	}
	void CStemResultInfos::_update_root_centers(CStemResult& sr) {
		//直接在m_root_centers中找到最接近的中心，如果小于指定距离，更新m_root_centers
		double d1 = m_seedling_dist / 4.0;
		
		double d_min = 1.0e6;
		int min_root_idx = -1;
		for (int i = 0; i < m_root_centers.size(); ++i) {
			double dist = m_root_centers.at(i).calcluate_distance(sr);
			if (dist < d_min) {
				d_min = dist;
				min_root_idx = i;
			}
		}
		if (d_min < d1 && min_root_idx >= 0) {
			//更新指定中心
			double mu_x, mu_y, mu_z;
			CStemResult& min_root = m_root_centers.at(min_root_idx);
			
			mu_x = min_root.root_x;			
			if (min_root.root_count == 0) {
				mu_y = sr.root_y;
				mu_z = sr.root_z;
			}
			else {
				mu_y = (min_root.root_y * min_root.root_count +
					sr.root_y * sr.root_count) / (double)(min_root.root_count + sr.root_count);
				mu_z = (min_root.root_z * min_root.root_count +
					sr.root_z * sr.root_count) / (double)(min_root.root_count + sr.root_count);

			}	
			min_root.root_x = mu_x;
			min_root.root_y = mu_y;
			min_root.root_z = mu_z;
			min_root.root_count += sr.root_count;
		}
	}

	void CStemResultInfos::gen_root_centers()
	{
		//根据 m_seedling_dist, m_holes_number， m_xmin， m_xmax生成初始的穴位中心
		//以m_xmin， m_xmax的中间点为中心，分别找到间隔m_seedling_dist的m_holes_number个穴位中心
		//初始的z设成-1，等待更新赋值
		double x_mid = 0.5 * (m_xmin + m_xmax);
		double holes_mid = 0.5 * (m_holes_number - 1) * m_seedling_dist; 
		double x0 = x_mid - holes_mid;
		double z_mid = 0.5 * (m_zmin + m_zmax);
		m_root_centers.clear();
		for (int i=0; i<m_holes_number; ++i) {
			double x = x0 + i * m_seedling_dist;
			double y = 0;
			double z = z_mid;
			int size = 0;
			int count = 0;
			CStemResult sr = CStemResult(x, y, z, size, std::string(""), count);
			m_root_centers.push_back(sr);
		}
	}
	

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_global_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_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);
	m_history_histogram = cv::Mat::zeros(m_max_size, m_center_x.size(), CV_32F);

}
void CSeedlingStatus::append_hist(
	std::vector<int>&xhist		//input
	//std::vector<bool>&xstatus	//output
)
{
	assert(xhist.size() == m_hist_length);

	std::vector<float>center_count;
	calculate_center_size(xhist, center_count);
	assert(center_count.size() == m_center_x.size());

	m_global_cursor++;
	if (m_record_cursor < m_max_size-1) {
		m_record_cursor++;
		float pc_size = 0.0;
		for (int i = 0; i < center_count.size(); ++i) {			
			m_history_histogram.at<float>(m_record_cursor,i) = center_count.at(i);
			pc_size += center_count.at(i);
		}
		m_history_point_size.at<float>(m_record_cursor, 0) = pc_size;
		//get_status(xstatus);		
	}
	else {
		//数据上移一行，数据放在最后一行
		for (int row = 0; row < m_history_histogram.rows - 1; ++row) {
			for (int col = 0; col < m_history_histogram.cols; ++col) {
				m_history_histogram.at<float>(row, col) = m_history_histogram.at<float>(row + 1, col);
			}
		}
		for (int row = 0; row < m_history_point_size.rows - 1; ++row) {
			m_history_point_size.at<float>(row, 0) = m_history_point_size.at<float>(row+1, 0);
		}		

		float pc_size = 0.0;
		for (int i = 0; i < center_count.size(); ++i) {
			m_history_histogram.at<float>(m_history_histogram.rows - 1, i) = center_count.at(i);
			pc_size += center_count.at(i);
		}
		m_history_point_size.at<float>(m_history_point_size.rows - 1, 0) = pc_size;
		//get_status(xstatus);
	}
	

}
void CSeedlingStatus::calculate_center_size(std::vector<int>&xhist, std::vector<float>&x_count)
{
	x_count.clear();
	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);
		float  valid_pc_cnt = 0;
		for (int k = idx_low; k <= idx_up; ++k) {
			if (k < 0 || k >= xhist.size()) { continue; }
			valid_pc_cnt += xhist.at(k);
		}
		x_count.push_back(valid_pc_cnt);
	}
}
//void CSeedlingStatus::calculate_center_size(int cursor, std::vector<float>&x_count)
//{
//	x_count.clear();
//	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);
//		float  valid_pc_cnt = 0;
//		for (int k = idx_low; k <= idx_up; ++k) {
//			valid_pc_cnt += m_history_histogram.at<float>(cursor, i);
//		}
//		x_count.push_back(valid_pc_cnt);		
//	}
//}
//void CSeedlingStatus::get_status(std::vector<bool>&xstatus)
//{
//	//根据历史信息评估：
//	// 1）位置上是否有苗
//	// 2）历史上是否已经取走苗
//	// 3）是否有新进的苗
//
//	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;
//		
//		std::vector<float> x_count;
//		calculate_center_size(m_record_cursor, x_count);
//		for (int i = 0; i < x_count.size(); ++i) {
//			int idx_low = m_idx_low.at(i);
//			int idx_up = m_idx_up.at(i);			
//			xstatus.at(i) = x_count.at(i) > (idx_up - idx_low + 1) * th_hist;
//		}
//		//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;
//	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;
//	}
//	/*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_global_cursor) {
//			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 {
//				
//			}
//		}
//		else {
//			//数据上移一行，数据放在最后一行
//			for (int row = 0; row < m_history_status.rows - 1; ++row) {
//				for (int col = 0; col < m_history_status.cols; ++col) {
//					m_history_status.at<float>(row, col) = m_history_status.at<float>(row + 1, col);
//				}
//			}
//			/*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);
//		int cursor = m_record_cursor-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_global_cursor) {
//			for (int i = 0; i < m_history_status.cols; ++i) {
//				m_history_status.at<unsigned char>(m_record_cursor, i) = xstatus.at(i);
//			}
//		}
//		else{
//			//数据上移一行，数据放在最后一行
//			for (int row = 0; row < m_history_status.rows - 1; ++row) {
//				for (int col = 0; col < m_history_status.cols; ++col) {
//					m_history_status.at<float>(row, col) = m_history_status.at<float>(row + 1, col);
//				}
//			}
//			/*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_global_cursor) {
//		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{
//		//数据上移一行，数据放在最后一行
//		for (int row = 0; row < m_history_status.rows - 1; ++row) {
//			for (int col = 0; col < m_history_status.cols; ++col) {
//				m_history_status.at<float>(row, col) = m_history_status.at<float>(row + 1, col);
//			}
//		}
//		/*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;
//			}
//		}
//	}
//}

int CSeedlingStatus::get_status(int& max_idx, float&max_change)
{
	//根据历史信息评估：
	// 1）评估是否新上苗
	// 2）判断被取走的那个苗	

	//状态 1 ---此次为新上苗， 0 无显著变化， -1 上一次取苗成功
	int status = 1; 	

	//1 如果没有记录输入，返回没有苗
	if (m_record_cursor <= 0) { 
		return status; 
	}

	float change_threshold = 500.0;//点云变化，是否有苗新加或取走的阈值
	float change_count = m_history_point_size.at<float>(m_record_cursor, 0) - m_history_point_size.at<float>(m_record_cursor - 1, 0);
	if (change_count > 2.0 * change_threshold) {
		status = 1;		
	}
	else {
		if (change_count < -change_threshold) {
			status = -1;
		}
		else {
			status = 0;
		}
	}
	max_change = 0;
	max_idx = 0;
	for (int i = 0; i < m_history_histogram.cols; ++i) {
		float d = m_history_histogram.at<float>(m_record_cursor, i) - m_history_histogram.at<float>(m_record_cursor - 1, i);
		if (fabs(d) > fabs(max_change)) {
			max_change = d;
			max_idx = i;
		}
	}
	if (max_change < -change_threshold) {
		status = -1;
	}
	return status;
}


void CSeedlingStatus::real_result_update(
	std::vector<pcl::PointXYZ>& root
)
{	
	m_stem_status.clear();
	m_stem_status.assign(m_center_x.size(), 0);
	
	for (auto& p : root) {
		int mini = -1;
		float mind = 1000.0;
		for (int i = 0; i < m_center_x.size();++i) {
			float d = fabs(m_center_x.at(i) - p.x);
			if (d < mind) {
				mind = d;
				mini = i;
			}
		}
		if (mini >= 0) {
			m_stem_status.at(mini) = 1;
		}
	}
}

void CSeedlingStatus::occlusion_result_update(
	std::vector<int>& leaf_occlusion
)
{
	assert(m_center_x.size() == leaf_occlusion.size());
	assert(m_center_x.size() == m_stem_status.size());
	for (int i = 0; i < m_center_x.size(); ++i) {		
		if (m_stem_status.at(i)==0 && leaf_occlusion.at(i)>0) {
			m_stem_status.at(i) = 2;			
		}
	}
	
}
//获取植株的状态，是否有苗
// 在real_result_update（）和occlusion_result_update（）调用后
// 调用此函数，获取茎的状态
void CSeedlingStatus::get_stem_status(
	std::vector<int>&stem_status
)
{
	//状态 1 ---此次为新上苗， 0 无显著变化， -1 上一次取苗成功
	float max_change_points_count;
	int max_change_center_idx;
	int status = get_status(max_change_center_idx, max_change_points_count);
	if (status > 0) {
		m_center_grabed_record.clear();
		m_center_grabed_record.assign(m_center_x.size(), -1);
	}

	stem_status.clear();
	if (status >= 0) {
		//用当前识别的结果		
	}
	else {
		m_center_grabed_record.at(max_change_center_idx) = 0;

		//如果max_change_center_idx位置的苗上一次有苗，本次为2，那么久将2改成1
		if (max_change_points_count < 0) {
			if (m_history_status.at<unsigned char>(m_record_cursor - 1, max_change_center_idx) != 0) {
				if (m_stem_status.at(max_change_center_idx) == 2) {
					m_stem_status.at(max_change_center_idx) = 0;
				}
			}
		}
	}

	//更新根据： m_center_grabed_record更新m_stem_status中的叶子遮挡
	for (int i = 0; i < m_stem_status.size(); ++i) {
		if (m_center_grabed_record.at(i) == 0 && m_stem_status.at(i) == 2) {
			m_stem_status.at(i) = 0;
		}		
	}

	for (int i = 0; i < m_stem_status.size(); ++i) {
		stem_status.push_back(m_stem_status.at(i));
	}
	
	//更新m_history_status
	if (m_record_cursor == m_global_cursor) {
		for (int i = 0; i < m_history_status.cols; ++i) {
			m_history_status.at<unsigned char>(m_record_cursor, i) = m_stem_status.at(i);
		}
	}
	else{
		//数据上移一行，数据放在最后一行
		for (int row = 0; row < m_history_status.rows - 1; ++row) {
			for (int col = 0; col < m_history_status.cols; ++col) {
				m_history_status.at<float>(row, col) = m_history_status.at<float>(row + 1, col);
			}
		}		
		for (int i = 0; i < m_history_status.cols; ++i) {
			m_history_status.at<unsigned char>(m_max_size - 1, i) = m_stem_status.at(i);
		}
	}	
}


}