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_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);
	m_global_cursor++;
	if (m_record_cursor < m_max_size-1) {
		m_record_cursor++;
		float pc_size = 0.0;
		for (int i = 0; i < xhist.size(); ++i) {
			if (i >= m_hist_length) { continue; }
			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 {
		//数据上移一行，数据放在最后一行
		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);
		}
		/*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_history_histogram.rows - 1, i) = float(xhist.at(i));
			pc_size += xhist.at(i);
		}
		m_history_point_size.at<float>(m_history_point_size.rows - 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;
	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;
			}
		}
	}
}

}