graft_cv/grab_occlusion.cpp

#include <sstream>
#include <opencv2\highgui\highgui.hpp>
#include "grab_occlusion.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::_update_root_centers() {
	//	//依据m_infos中的数据，苗间距m_seedling_dist，聚类生成茎的平均位置，茎的点云平均数量
	//	//更新m_root_centers

	//	//以root_center为基础进行聚类，如果root_center是空的，因当前数据进行聚类
	//	std::vector<CStemResult> root_centers;
	//	double d1 = m_seedling_dist / 2.0;
	//	double d2 = d1*1.75;
	//	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud2d_pos(new pcl::PointCloud < pcl::PointXYZ>);
	//	for (auto&sr : m_infos) {
	//		cloud2d_pos->points.push_back(pcl::PointXYZ((float)sr.root_x, 0.0, (float)sr.root_z));
	//	}
	//	cloud2d_pos->width = cloud2d_pos->points.size();
	//	std::vector<pcl::PointXYZ>cluster_center;
	//	std::vector<std::vector<int>> cluster_member;
	//	euclidean_clustering_ttsas(cloud2d_pos, d1, d2, cluster_center, cluster_member);

	//	for (size_t i = 0; i < cluster_center.size(); ++i) {
	//		double x = (double)cluster_center.at(i).x;
	//		double y = (double)cluster_center.at(i).y;
	//		double z = (double)cluster_center.at(i).z;
	//		int count = (int)cluster_member.at(i).size();
	//		int pc_size = 0.0;
	//		for (auto& idx : cluster_member.at(i)) {
	//			pc_size += m_infos.at(idx).stem_size;
	//		}
	//		pc_size = (int)(pc_size / (float)count);

	//		CStemResult sr = CStemResult(x, y, z, pc_size, std::string(""), count);
	//		root_centers.push_back(sr);
	//	}

	//	if (m_root_centers.size() == 0) {
	//		//如果没有历史数据，直接赋值
	//		m_root_centers = root_centers;
	//	}
	//	else {
	//		if (m_infos.size() > 50) {
	//			//如果当前数据量较大，用当前的结果
	//			m_root_centers = root_centers;
	//		}
	//		else {
	//			//否则将当前结果合并到m_root_centers
	//			//m_root_centers
	//			for (auto& cent : root_centers) {					
	//				double dist_min = d2;
	//				int min_root_idx = 0;

	//				for (int j = 0; j < m_root_centers.size();++j) {
	//					CStemResult& root = m_root_centers.at(j);
	//					double dist = root.calcluate_distance(cent);
	//					if (dist < dist_min) {
	//						dist_min = dist;
	//						min_root_idx = j;
	//					}
	//				}
	//				if (dist_min < d1) {
	//					//merge
	//					double mu_x, mu_y,mu_z;
	//					CStemResult& min_root = m_root_centers.at(min_root_idx);
	//					mu_x = (min_root.root_x * min_root.root_count +
	//						cent.root_x * cent.root_count) / (double)(min_root.root_count + cent.root_count);

	//					mu_y = (min_root.root_y * min_root.root_count +
	//						cent.root_y * cent.root_count) / (double)(min_root.root_count + cent.root_count);

	//					mu_z = (min_root.root_z * min_root.root_count +
	//						cent.root_z * cent.root_count) / (double)(min_root.root_count + cent.root_count);

	//					min_root.root_x = mu_x;
	//					min_root.root_y = mu_y;
	//					min_root.root_z = mu_z;
	//					min_root.root_count += cent.root_count;
	//				}
	//				else {
	//					//add root
	//					m_root_centers.push_back(cent);
	//				}
	//			}
	//		}
	//	}
	//	//m_root_centers 排序、剔除
	//	std::sort(m_root_centers.begin(), m_root_centers.end(),
	//		[](const CStemResult& sr1, const CStemResult& sr2) {return sr1.root_x < sr2.root_x; });
	//	//nms		
	//	_filter_root_centers(d1, d2);
	//}
	//void CStemResultInfos::_filter_root_centers(double d1, double d2)
	//{
	//	//对生成的根中心m_root_centers进行过滤，剔除异常
	//	//1 z值，通过中值，距离中值远的剔除

	//	if (m_root_centers.size() > 3) {
	//		std::vector<double>root_z;
	//		for (auto&rc : m_root_centers) {
	//			root_z.push_back(rc.root_z);
	//		}
	//		std::sort(root_z.begin(), root_z.end());
	//		double mid_z = 0.0;
	//		if (root_z.size() % 2 == 1) {
	//			int idx = (m_root_centers.size() - 1) / 2;
	//			mid_z = root_z.at(idx);
	//		}
	//		else {
	//			int idx = root_z.size() / 2;
	//			mid_z = 0.5 * (root_z.at(idx) + root_z.at(idx - 1));
	//		}
	//		std::vector<CStemResult> valid_root_centers;
	//		for (auto&rc : m_root_centers) {
	//			if (fabs(rc.root_z - mid_z) < d2) {
	//				valid_root_centers.push_back(rc);
	//			}
	//		}
	//		m_root_centers = valid_root_centers;
	//	}

	//	//2 按x做分组，排除权重小的
	//	if (m_root_centers.size() > 3) {
	//		std::vector<double> center_dist;
	//		for (int i = 0; i < m_root_centers.size(); ++i) {
	//			double x = m_root_centers.at(i).root_x;
	//			double mod_sum = 0.0;
	//			for (int j = 0; j < m_root_centers.size(); ++j) {
	//				double dist = fabs(m_root_centers.at(j).root_x - x);
	//				double mod = fmod(dist, m_seedling_dist);
	//				double mod_inv = m_seedling_dist - mod;
	//				mod = min(mod, mod_inv);
	//				mod_sum += mod;
	//			}
	//			mod_sum /= (m_root_centers.size() - 1);
	//			center_dist.push_back(mod_sum);
	//		}

	//		std::vector<CStemResult> valid_root_centers;
	//		for (int i = 0; i < center_dist.size(); ++i) {
	//			double dist = center_dist.at(i);
	//			if (dist < d1) {
	//				valid_root_centers.push_back(m_root_centers.at(i));
	//			}
	//		}
	//		m_root_centers = valid_root_centers;
	//	}
	//	//3 数量少的剔除
	//	int total_count = 0;
	//	for (auto&rc : m_root_centers) {
	//		total_count += rc.root_count;
	//	}
	//	if (total_count > 50) {
	//		bool need_del = false;
	//		for (auto& rc : m_root_centers) {
	//			if (rc.root_count <= 2) {
	//				need_del = true;
	//				break;
	//			}
	//		}
	//		if (need_del) {
	//			std::vector<CStemResult> valid_root_centers;
	//			for (int i = 0; i < m_root_centers.size(); ++i) {					
	//				if (m_root_centers.at(i).root_count > 2) {
	//					valid_root_centers.push_back(m_root_centers.at(i));
	//				}
	//			}
	//			m_root_centers = valid_root_centers;
	//		}
	//		
	//	}
	//}
	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);
		}
	}

	//void CStemResultInfos::set_json_filename(std::string& filename) {
	//	m_json_filename = std::string(filename);
	//}
	//void CStemResultInfos::read_root_centers(
	//	std::string& filename
	//) 
	//{
	//	//读取历史数据，每次启动时，去取
	//	m_json_filename = std::string(filename);
	//	//1 文件是否存在
	//	ifstream f(filename.c_str());
	//	if (!f.good()) {
	//		//文件不存在
	//		if (m_pLogger) {
	//			stringstream buff;
	//			buff << m_pcdId << ": json file not exists:" << filename;
	//			m_pLogger->INFO(buff.str());
	//		}
	//		return;
	//	}	

	//	cv::FileStorage fs(filename, cv::FileStorage::READ);
	//	cv::FileNode root_centers = fs["root_centers"];
	//	cv::FileNodeIterator it = root_centers.begin(), it_end = root_centers.end();
	//	m_root_centers.clear();
	//	for (; it != it_end; ++it) {
	//		double x = (double)(*it)["x"];
	//		double y = (double)(*it)["y"];
	//		double z = (double)(*it)["z"];
	//		int size = (int)(*it)["size"];
	//		std::string bid = (std::string)(*it)["batch_id"];
	//		int count = (int)(*it)["count"];
	//		CStemResult sr = CStemResult(x,y,z,size, bid, count);
	//		m_root_centers.push_back(sr);
	//	}
	//	fs.release();

	//}
	//void CStemResultInfos::write_root_centers(
	//	std::string& filename
	//)
	//{
	//	cv::FileStorage fs(filename, cv::FileStorage::WRITE);
	//	fs << "root_centers" << "[";
	//	for (auto& sr : m_root_centers) {		
	//		fs << "{" << "x" << sr.root_x
	//			<< "y" << sr.root_y
	//			<< "z" << sr.root_z
	//			<< "size" << sr.stem_size
	//			<< "batch_id" << sr.batch_id	
	//			<< "count" << sr.root_count
	//			<< "}";
	//	}
	//	fs << "]";
	//	fs.release();

	//}

	//void CStemResultInfos::euclidean_clustering_ttsas(
	//	pcl::PointCloud<pcl::PointXYZ>::Ptr in_cloud,
	//	double d1, double d2,
	//	std::vector<pcl::PointXYZ>&cluster_center,
	//	std::vector<std::vector<int>> &clustr_member
	//)
	//{
	//	if (m_pLogger) {
	//		stringstream buff;
	//		buff << m_pcdId << ": root center estimate: euclidean_clustering_ttsas() begin...";
	//		m_pLogger->INFO(buff.str());
	//	}
	//	std::vector<int> cluster_weight;
	//	std::vector<int> data_stat;

	//	std::vector<pcl::PointXYZ>cluster_center_raw;
	//	std::vector<std::vector<int>> clustr_member_raw;

	//	for (size_t i = 0; i < in_cloud->points.size(); ++i) { data_stat.push_back(0); }
	//	size_t data_len = in_cloud->points.size();
	//	int exists_change = 0;
	//	int prev_change = 0;
	//	int cur_change = 0;
	//	int m = 0;
	//	while (std::find(data_stat.begin(), data_stat.end(), 0) != data_stat.end()) {
	//		bool new_while_first = true;
	//		for (size_t i = 0; i < data_len; ++i) {
	//			if (data_stat.at(i) == 0 && new_while_first && exists_change == 0) {
	//				new_while_first = false;
	//				std::vector<int> idx;
	//				idx.push_back(i);
	//				clustr_member_raw.push_back(idx);
	//				pcl::PointXYZ center;
	//				center.x = in_cloud->points.at(i).x;
	//				center.y = in_cloud->points.at(i).y;
	//				center.z = in_cloud->points.at(i).z;

	//				cluster_center_raw.push_back(center);
	//				data_stat.at(i) = 1;
	//				cur_change += 1;
	//				cluster_weight.push_back(1);
	//				m += 1;
	//			}
	//			else if (data_stat[i] == 0) {
	//				std::vector<float> distances;
	//				for (size_t j = 0; j < clustr_member_raw.size(); ++j) {
	//					std::vector<float> distances_sub;
	//					for (size_t jj = 0; jj < clustr_member_raw.at(j).size(); ++jj) {
	//						size_t ele_idx = clustr_member_raw.at(j).at(jj);
	//						double d = sqrt(
	//							(in_cloud->points.at(i).x - in_cloud->points.at(ele_idx).x) * (in_cloud->points.at(i).x - in_cloud->points.at(ele_idx).x) +
	//							(in_cloud->points.at(i).y - in_cloud->points.at(ele_idx).y) * (in_cloud->points.at(i).y - in_cloud->points.at(ele_idx).y) +
	//							(in_cloud->points.at(i).z - in_cloud->points.at(ele_idx).z) * (in_cloud->points.at(i).z - in_cloud->points.at(ele_idx).z));
	//						distances_sub.push_back(d);
	//					}
	//					double min_dist = *std::min_element(distances_sub.begin(), distances_sub.end());
	//					distances.push_back(min_dist);
	//				}
	//				int min_idx = std::min_element(distances.begin(), distances.end()) - distances.begin();
	//				if (distances.at(min_idx) < d1) {
	//					data_stat.at(i) = 1;
	//					double w = cluster_weight.at(min_idx);
	//					cluster_weight.at(min_idx) += 1;
	//					clustr_member_raw.at(min_idx).push_back(i);
	//					cluster_center_raw.at(min_idx).x = (cluster_center_raw.at(min_idx).x * w + in_cloud->points.at(i).x) / (w + 1);
	//					cluster_center_raw.at(min_idx).y = (cluster_center_raw.at(min_idx).y * w + in_cloud->points.at(i).y) / (w + 1);
	//					cluster_center_raw.at(min_idx).z = (cluster_center_raw.at(min_idx).z * w + in_cloud->points.at(i).z) / (w + 1);
	//					cur_change += 1;
	//				}
	//				else if (distances.at(min_idx) > d2) {
	//					std::vector<int> idx;
	//					idx.push_back(i);
	//					clustr_member_raw.push_back(idx);
	//					pcl::PointXYZ center;
	//					center.x = in_cloud->points.at(i).x;
	//					center.y = in_cloud->points.at(i).y;
	//					center.z = in_cloud->points.at(i).z;

	//					cluster_center_raw.push_back(center);
	//					data_stat.at(i) = 1;
	//					cur_change += 1;
	//					cluster_weight.push_back(1);
	//					m += 1;
	//				}

	//			}
	//			else if (data_stat.at(i) == 1) {
	//				cur_change += 1;
	//			}
	//		}
	//		exists_change = fabs(cur_change - prev_change);
	//		prev_change = cur_change;
	//		cur_change = 0;
	//	}

	//	// copy result
	//	for (size_t i = 0; i < clustr_member_raw.size(); ++i) {
	//		//if (clustr_member_raw.at(i).size() < 20) { continue; }
	//		cluster_center.push_back(cluster_center_raw.at(i));
	//		clustr_member.push_back(clustr_member_raw.at(i));
	//	}
	//	if (m_pLogger) {
	//		stringstream buff;
	//		buff << m_pcdId << ":  root center estimate: euclidean_clustering_ttsas() end";
	//		m_pLogger->INFO(buff.str());
	//	}
	//}



}