/*
通过点云数据识别抓取位置信息

1）获取点云
2）剔除离群点
3）降采样
4）植株检测
5）选出最前，最右侧植株
6）植株抓取位置检测

*/

#include <pcl\io\ply_io.h>
#include <pcl\visualization\cloud_viewer.h>
#include <pcl\filters\crop_box.h>
#include <pcl\filters\radius_outlier_removal.h>
#include <pcl\filters\voxel_grid.h>
#include <pcl\common\common.h>
#include <math.h>

#include "grab_point_rs.h"
#include "utils.h"

#define PI std::acos(-1)

namespace graft_cv {

	CRootStockGrabPoint::CRootStockGrabPoint(ConfigParam&cp, CGcvLogger*pLog/*=0*/)
		:m_cparam(cp),
		m_pLogger(pLog),
		m_dtype(0)
	{
	}

	CRootStockGrabPoint::~CRootStockGrabPoint() {}
	float* CRootStockGrabPoint::get_raw_point_cloud(int &data_size)
	{
		data_size = m_raw_cloud->width * m_raw_cloud->height;
		if (data_size == 0) {
			return 0;
		}
		else {
			pcl::PointXYZ* pp = m_raw_cloud->points.data();
			return (float*)pp->data;			
		}

	}
	int CRootStockGrabPoint::load_data(
		float*pPoint,
		int pixel_size,
		int pt_size,		
		const char* fn/* = 0*/)
	{
		int rst = 0;
		//1 get point cloud data
		if (pPoint != 0 && pt_size>0) {
			//read point
			m_raw_cloud.reset(new pcl::PointCloud<pcl::PointXYZ>);
			int step = pixel_size;
			for (int i = 0; i < pt_size; ++i) {
				pcl::PointXYZ pt = { pPoint[i*step], pPoint[i*step + 1] , pPoint[i*step + 2] };
				m_raw_cloud->push_back(pt);
			}
			rst = m_raw_cloud->width * m_raw_cloud->height;
			if (m_pLogger) {
				stringstream buff;
				buff << "load raw point cloud " << rst << " data points";
				m_pLogger->INFO(buff.str());
			}			
		}
		else if (fn != 0) {
			//read file
			rst = this->read_ply_file(fn);					
		}
		else {//error
			if (m_pLogger) {
				m_pLogger->ERRORINFO("no valid input");
				return (-1);
			}
		}	

		if (m_cparam.image_show) {
			viewer_cloud(m_raw_cloud, std::string("raw point cloud"));
		}
		return rst;
	}

	int CRootStockGrabPoint::grab_point_detect(	
		int dtype,
		PositionInfo& posinfo
		)
	{
		// dtype == 0, rootstock
		// dtype != 0, scion
		if (m_raw_cloud->width * m_raw_cloud->height < 1) {
			if (m_pLogger) {
				stringstream buff;
				buff << "m_raw_cloud point cloud " << m_raw_cloud->width * m_raw_cloud->height << " data points";
				m_pLogger->ERRORINFO(buff.str());
			}
			return 1;
		}
		//2 crop filter
		if (m_pLogger) {
			m_pLogger->INFO("begin crop");			
		}
		pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_inbox(new pcl::PointCloud<pcl::PointXYZ>);
		pcl::CropBox<pcl::PointXYZ> box_filter;
		m_dtype = dtype;
		if (dtype == 0) {
			box_filter.setMin(Eigen::Vector4f(m_cparam.rs_grab_xmin, m_cparam.rs_grab_ymin, m_cparam.rs_grab_zmin, 1));
			box_filter.setMax(Eigen::Vector4f(m_cparam.rs_grab_xmax, m_cparam.rs_grab_ymax, m_cparam.rs_grab_zmax, 1));
		}
		else {
			box_filter.setMin(Eigen::Vector4f(m_cparam.sc_grab_xmin, m_cparam.sc_grab_ymin, m_cparam.sc_grab_zmin, 1));
			box_filter.setMax(Eigen::Vector4f(m_cparam.sc_grab_xmax, m_cparam.sc_grab_ymax, m_cparam.sc_grab_zmax, 1));
		}
		box_filter.setNegative(false);
		box_filter.setInputCloud(m_raw_cloud);
		box_filter.filter(*cloud_inbox);

		if (m_pLogger) {
			stringstream buff;
			buff << "CropBox croped point cloud " << cloud_inbox->width * cloud_inbox->height << " data points";
			m_pLogger->INFO(buff.str());
		}
		if (cloud_inbox->width * cloud_inbox->height < 1) {
			return 1;
		}

		if (m_cparam.image_show) {
			viewer_cloud(cloud_inbox, std::string("cloud_inbox"));
		}
		if (m_pLogger) {
			m_pLogger->INFO("end crop");
		}
		//3 filtler with radius remove
		if (m_pLogger) {
			m_pLogger->INFO("begin ror");
		}
		int nb_points = 20;
		double stem_radius = m_cparam.rs_grab_stem_diameter / 2.0;
		if(dtype != 0){
			stem_radius = m_cparam.sc_grab_stem_diameter / 2.0;
		}
		pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_ror(new pcl::PointCloud<pcl::PointXYZ>);
		pcl::RadiusOutlierRemoval<pcl::PointXYZ> ror;
		ror.setInputCloud(cloud_inbox);
		ror.setRadiusSearch(stem_radius);
		ror.setMinNeighborsInRadius(nb_points);
		ror.filter(*cloud_ror);

		if (m_pLogger) {
			stringstream buff;
			buff << "RadiusOutlierRemoval filtered point cloud " << cloud_ror->width * cloud_ror->height << " data points. param stem_radius="<<
				stem_radius<<", nb_points="<< nb_points;
			m_pLogger->INFO(buff.str());
		}
		if (cloud_ror->width * cloud_ror->height < 1) {
			return 1;
		}

		if (m_cparam.image_show) {
			viewer_cloud(cloud_ror, std::string("cloud_ror"));
		}
		if (m_pLogger) {
			m_pLogger->INFO("end ror");
		}

		//3 voxel grid down sampling
		if (m_pLogger) {
			m_pLogger->INFO("begin down");
		}
		pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_dowm_sampled(new pcl::PointCloud<pcl::PointXYZ>);
		pcl::VoxelGrid<pcl::PointXYZ> outrem;
		outrem.setInputCloud(cloud_ror);
		outrem.setLeafSize(stem_radius, stem_radius, stem_radius);
		outrem.filter(*cloud_dowm_sampled);

		if (m_pLogger) {
			stringstream buff;
			buff << "VoxelGrid dowm_sampled point cloud " << cloud_dowm_sampled->width * cloud_dowm_sampled->height << " data points";
			m_pLogger->INFO(buff.str());
		}
		if (cloud_dowm_sampled->width * cloud_dowm_sampled->height < 1) {
			return 1;
		}

		if (m_cparam.image_show) {
			viewer_cloud(cloud_dowm_sampled, std::string("cloud_dowm_sampled"));
		}
		if (m_pLogger) {
			m_pLogger->INFO("end down");
		}
		//4 seedling position
		std::vector<int> first_seedling_cloud_idx;
		pcl::PointXYZ xz_center;
		if (m_pLogger) {
			m_pLogger->INFO("begin find seedling position");
		}
		find_seedling_position(cloud_dowm_sampled, first_seedling_cloud_idx, xz_center);
		if (m_pLogger) {
			stringstream buff;
			buff << "after find_seedling_position(), foud first seedling seeds points size " << first_seedling_cloud_idx .size();
			m_pLogger->INFO(buff.str());
		}
		if (m_pLogger) {
			m_pLogger->INFO("end find seedling position");
		}
		//5 nearest seedling grab point selection
		pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_seedling_seed(new pcl::PointCloud<pcl::PointXYZ>);
		pcl::copyPointCloud(*cloud_dowm_sampled, first_seedling_cloud_idx, *cloud_seedling_seed);
		std::vector<int>mass_idx;
		double dist_mean = compute_nearest_neighbor_distance(cloud_dowm_sampled);
		std::vector<double> mass_indices;
		if (m_pLogger) {
			m_pLogger->INFO("begin crop nn_analysis");
		}
		crop_nn_analysis(cloud_ror, cloud_seedling_seed, dist_mean, mass_indices, mass_idx);
		if (m_pLogger) {
			m_pLogger->INFO("end crop nn_analysis");
		}

		double candidate_th = otsu(mass_indices);
		std::vector<int> optimal_seeds_idx;
		for (size_t j = 0; j < mass_idx.size(); ++j) {
			if (mass_indices.at(mass_idx.at(j)) >= candidate_th) {
				optimal_seeds_idx.push_back(mass_idx.at(j));
			}
		}
		pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_optimal_seed(new pcl::PointCloud<pcl::PointXYZ>);
		pcl::copyPointCloud(*cloud_seedling_seed, optimal_seeds_idx, *cloud_optimal_seed);
		pcl::PointXYZ selected_pt;
		int selected_pt_idx;
		if (m_pLogger) {
			m_pLogger->INFO("begin get_optimal_seed");
		}
		get_optimal_seed(cloud_optimal_seed, selected_pt, selected_pt_idx);
		if (selected_pt_idx < 0) {			
			return 1;
		}
		if (m_pLogger) {
			m_pLogger->INFO("end get_optimal_seed");
		}
		posinfo.rs_grab_x = selected_pt.x;
		posinfo.rs_grab_y = selected_pt.y;
		posinfo.rs_grab_z = selected_pt.z;

		////////////////////////////////////////////////////////////////////
		//debug
		if (m_cparam.image_show) {
			pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_cand_demo(new pcl::PointCloud<pcl::PointXYZRGB>);
			pcl::copyPointCloud(*cloud_dowm_sampled, *cloud_cand_demo);

			for (auto& pt : *cloud_cand_demo) {
				pt.r = 255;
				pt.g = 255;
				pt.b = 255;
			}
			int cnt = 0;
			for (auto& idx : mass_idx) {
				int p_idx = first_seedling_cloud_idx.at(idx);
				/*if (p_idx == optimal_seeds_idx[selected_pt_idx]) {
					cloud_cand_demo->points[p_idx].r = 0;
					cloud_cand_demo->points[p_idx].g = 255;
					cloud_cand_demo->points[p_idx].b = 0;
				}
				else {*/
					cloud_cand_demo->points.at(p_idx).r = 255;
					cloud_cand_demo->points.at(p_idx).g = 0;
					cloud_cand_demo->points.at(p_idx).b = 0;
				/*}	*/			
				if (cnt > optimal_seeds_idx.size()) { break; }
				cnt++;
			}
			pcl::PointXYZRGB pt_grab = {0,255,0};
			pt_grab.x = selected_pt.x;
			pt_grab.y = selected_pt.y;
			pt_grab.z = selected_pt.z;

			pcl::PointXYZRGB pt_grab_ = { 0,255,120 };
			pt_grab_.x = selected_pt.x;
			pt_grab_.y = selected_pt.y+0.2;
			pt_grab_.z = selected_pt.z;
			cloud_cand_demo->push_back(pt_grab);


			//viewer_cloud(cloud_cand_demo, std::string("cloud_cand_demo"));
			viewer_cloud_debug(cloud_cand_demo, selected_pt, std::string("cloud_cand_demo"));
		}
		return 0;
	}
	int CRootStockGrabPoint::read_ply_file(const char* fn)
	{
		m_raw_cloud.reset( new pcl::PointCloud<pcl::PointXYZ>);
		if (pcl::io::loadPLYFile<pcl::PointXYZ>(fn, *m_raw_cloud) == -1) {
			if (m_pLogger) {
				m_pLogger->ERRORINFO("could not load file: "+std::string(fn));
			}
			return (-1);
		}
		if (m_pLogger) {
			stringstream buff;
			buff << "load raw point cloud " << m_raw_cloud->width * m_raw_cloud->height << " data points";
			m_pLogger->INFO(buff.str());
		}
		return m_raw_cloud->width * m_raw_cloud->height;
	}
	double CRootStockGrabPoint::compute_nearest_neighbor_distance(pcl::PointCloud<pcl::PointXYZ>::Ptr pcd)
	{
		pcl::KdTreeFLANN<pcl::PointXYZ> tree;
		tree.setInputCloud(pcd);

		int k = 2;
		double res = 0.0;
		int n_points = 0;
		for (size_t i = 0; i < pcd->size(); ++i) {
			std::vector<int> idx(k);
			std::vector<float> sqr_distances(k);

			if (tree.nearestKSearch(i, k, idx, sqr_distances) == k) {
				for (int id = 1; id < k; ++id) {
					res += sqrt(sqr_distances.at(id));
					++n_points;
				}
			}
		}
		if (n_points > 0) {
			res /= (double)n_points;
		}
		return res;
	}
	void CRootStockGrabPoint::find_seedling_position(
		pcl::PointCloud<pcl::PointXYZ>::Ptr in_cloud,
		std::vector<int> &first_seedling_cloud_idx,
		pcl::PointXYZ&xz_center
	)
	{
		pcl::PointCloud<pcl::PointXYZ>::Ptr cloud2d(new pcl::PointCloud < pcl::PointXYZ>);
		pcl::copyPointCloud(*in_cloud, *cloud2d);
		for (auto&pt : *cloud2d) {
			pt.y = 0.0;
		}

		if(m_cparam.image_show){
			viewer_cloud(cloud2d, std::string("cloud2d"));
		}		

		double radius = m_cparam.rs_grab_stem_diameter;
		if (m_dtype != 0) {
			radius = m_cparam.sc_grab_stem_diameter;
		}
		std::vector<int> counter;
		pcl::KdTreeFLANN<pcl::PointXYZ> kdtree;
		kdtree.setInputCloud(cloud2d);
		std::vector<int>idx;
		std::vector<float>dist_sqr;
		for (size_t i = 0; i < cloud2d->points.size(); ++i) {
			int k = kdtree.radiusSearch(cloud2d->points.at(i), radius, idx, dist_sqr);
			counter.push_back(k);
		}
		int th = (int)(otsu(counter));
		std::vector<int> index;
		for (size_t i = 0; i < counter.size(); ++i) {
			if (counter.at(i) >= th) {
				index.push_back(i);
			}
		}

		pcl::PointCloud<pcl::PointXYZ>::Ptr cloud2d_pos(new pcl::PointCloud < pcl::PointXYZ>);
		pcl::copyPointCloud(*cloud2d, index, *cloud2d_pos);

		if (m_pLogger) {
			stringstream buff;
			buff << "get 2d seedling seed point cloud " << index.size()<< " data points with thrreshold "<<th;
			m_pLogger->INFO(buff.str());
		}
		if (m_cparam.image_show) {
			viewer_cloud(cloud2d_pos, std::string("cloud2d_pos"));
		}
		
		//clustering
		double d1 = m_cparam.rs_grab_stem_diameter;
		double d2 = m_cparam.rs_grab_stem_diameter * 3.0;
		if (m_dtype != 0) {
			d1 = m_cparam.sc_grab_stem_diameter;
			d2 = m_cparam.sc_grab_stem_diameter * 3.0;
		}
		std::vector<pcl::PointXYZ>cluster_center;
		std::vector<std::vector<int>> cluster_member;
		euclidean_clustering_ttsas(cloud2d_pos, d1, d2, cluster_center, cluster_member);

		if (m_pLogger) {
			stringstream buff;
			buff << "euclidean_clustering_ttsas:  " << cluster_center.size() << " t1=" << d1
				<< " t2=" << d2;
			m_pLogger->INFO(buff.str());
		}

		//sort cluster center, get the frontest leftest one
		std::vector<float> cluster_index;
		for (auto&pt : cluster_center) {
			float idx = pt.x - pt.z;
			cluster_index.push_back(idx);
		}
		int first_idx = std::min_element(cluster_index.begin(), cluster_index.end()) - cluster_index.begin();

		first_seedling_cloud_idx.clear();
		for (auto&v : cluster_member.at(first_idx)) {
			size_t i = index.at(v);
			first_seedling_cloud_idx.push_back(i);
		}
		xz_center.x = cluster_center.at(first_idx).x;
		xz_center.y = cluster_center.at(first_idx).y;
		xz_center.z = cluster_center.at(first_idx).z;

		if (m_pLogger) {
			stringstream buff;
			buff << "euclidean_clustering_ttsas， find cluster center(" << xz_center.x
				<<", "<< xz_center.y<<", "<< xz_center.z<<")";
			m_pLogger->INFO(buff.str());
		}
	}
	void CRootStockGrabPoint::crop_nn_analysis(
		pcl::PointCloud<pcl::PointXYZ>::Ptr in_cloud,
		pcl::PointCloud<pcl::PointXYZ>::Ptr seed_cloud,
		double dist_mean,
		std::vector<double>&mass_indices,
		std::vector<int>& candidate_idx
	)
	{
		candidate_idx.clear();
		pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_inbox(new pcl::PointCloud<pcl::PointXYZ>);
		pcl::CropBox<pcl::PointXYZ> box_filter;
		box_filter.setNegative(false);
		box_filter.setInputCloud(in_cloud);				

		double radius = 5;
		double rx = 0.8;
		double ry = 1.0;
		double rz = 0.8;
		double cx, cy, cz;
		double dz;

		for (size_t i = 0; i < seed_cloud->points.size(); ++i) {
			cx = seed_cloud->points.at(i).x;
			cy = seed_cloud->points.at(i).y;
			cz = seed_cloud->points.at(i).z;

			box_filter.setMin(Eigen::Vector4f(cx - rx*radius, cy - ry*radius, cz - rz*radius, 1));
			box_filter.setMax(Eigen::Vector4f(cx + rx*radius, cy + ry*radius, cz + rz*radius, 1));
			box_filter.filter(*cloud_inbox);

			//dz
			Eigen::Vector4f min_point;
			Eigen::Vector4f max_point;
			pcl::getMinMax3D(*cloud_inbox, min_point, max_point);
			dz = max_point(2) - min_point(2);

			//project to xy-plane
			pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_inbox_xy(new pcl::PointCloud<pcl::PointXYZ>);
			pcl::copyPointCloud(*cloud_inbox, *cloud_inbox_xy);
			for (auto&pt : *cloud_inbox_xy) { pt.z = 0.0; }

			//pca
			double dx_obb;
			double dy_obb;
			double angle_obb;
			cal_obb_2d(cloud_inbox_xy, 0, dx_obb, dy_obb, angle_obb);			
			try {

				double dx_grid = dx_obb / dist_mean;
				double dy_grid = dy_obb / dist_mean;
				double dz_grid = dz / dist_mean;
				double fill_ratio = cloud_inbox->points.size() / dx_grid / dy_grid / dz_grid;
				double y_ratio = dy_obb / dx_obb/2;
				y_ratio = pow(y_ratio, 2);				

				double a_ratio = cos((angle_obb - 90)*PI / 180.0);
				a_ratio = pow(a_ratio, 3);

				double mass_index = fill_ratio*y_ratio*a_ratio;
				mass_indices.push_back(mass_index);
				
				if (m_pLogger) {
					stringstream buff;
					buff << i << "/" << seed_cloud->points.size() << " dx=" << dx_obb << ", dy=" << dy_obb << ", fill_ratio=" << fill_ratio
						<< ", y_ratio=" << y_ratio << ", a_ratio=" << a_ratio << ", mass_index=" << mass_index;
					m_pLogger->INFO(buff.str());
				}
			}
			catch (...) {
				mass_indices.push_back(0);
			}

		}

		// sort by mass_indices
		std::vector<size_t> sort_idx = sort_indexes_e(mass_indices, false);
		for (auto&v : sort_idx) { candidate_idx.push_back((int)v); }
	}

	void CRootStockGrabPoint::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 << "euclidean_clustering_ttsas() begin...";
			m_pLogger->INFO(buff.str());
		}
		std::vector<int> cluster_weight;
		std::vector<int> data_stat;

		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.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.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.size(); ++j) {
						std::vector<float> distances_sub;
						for (size_t jj = 0; jj < clustr_member.at(j).size(); ++jj) {
							size_t ele_idx = clustr_member.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.at(min_idx).push_back(i);
						cluster_center.at(min_idx).x = (cluster_center.at(min_idx).x * w + in_cloud->points.at(i).x) / (w + 1);
						cluster_center.at(min_idx).y = (cluster_center.at(min_idx).y * w + in_cloud->points.at(i).y) / (w + 1);
						cluster_center.at(min_idx).z = (cluster_center.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.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.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;
		}
		if (m_pLogger) {
			stringstream buff;
			buff << "euclidean_clustering_ttsas() end";
			m_pLogger->INFO(buff.str());
		}
	}
	void CRootStockGrabPoint::cal_obb_2d(
		pcl::PointCloud<pcl::PointXYZ>::Ptr in_cloud,
		int axis, //0--xy,  1--zy
		double &dx_obb,
		double &dy_obb,
		double &angle_obb
	)
	{
		// asign value
		Eigen::MatrixXd pts(in_cloud->points.size(), 2);
		for (size_t i = 0; i < in_cloud->points.size(); ++i) {
			if (axis == 0) {
				pts(i, 0) = in_cloud->points.at(i).x;
			}
			else {
				pts(i, 0) = in_cloud->points.at(i).z;
			}
			pts(i, 1) = in_cloud->points.at(i).y;
		}
		// centerlize
		Eigen::MatrixXd mu = pts.colwise().mean();
		Eigen::RowVectorXd mu_row = mu;
		pts.rowwise() -= mu_row;

		//calculate covariance
		Eigen::MatrixXd C = pts.adjoint()*pts;
		C = C.array() / (pts.cols() - 1);


		//compute eig
		Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> eig(C);
		Eigen::MatrixXd d = eig.eigenvalues();
		Eigen::MatrixXd v = eig.eigenvectors();

		//projection
		Eigen::MatrixXd p = pts * v;
		Eigen::VectorXd minv = p.colwise().minCoeff();
		Eigen::VectorXd maxv = p.colwise().maxCoeff();
		Eigen::VectorXd range = maxv - minv;

		dy_obb = range(1);
		dx_obb = range(0);
		angle_obb = std::atan2(v(1, 1), v(0, 1));
		if (angle_obb < 0) { angle_obb = PI + angle_obb; }
		angle_obb = angle_obb * 180 / PI;		
	}

	//void CRootStockGrabPoint::get_optimal_seed(
	//	pcl::PointCloud<pcl::PointXYZ>::Ptr in_cloud,
	//	pcl::PointXYZ&pt,
	//	int &pt_idx)
	//{
	//	double d1 = m_cparam.rs_grab_stem_diameter;
	//	double d2 = m_cparam.rs_grab_stem_diameter * 4.0;
	//	if (m_dtype != 0) {
	//		d1 = m_cparam.sc_grab_stem_diameter;
	//		d2 = m_cparam.sc_grab_stem_diameter * 4.0;
	//	}
	//	std::vector<pcl::PointXYZ>cluster_center;
	//	std::vector<std::vector<int>> cluster_member;
	//	euclidean_clustering_ttsas(in_cloud, d1, d2, cluster_center, cluster_member);

	//	float min_y_dist = 1.0e6;
	//	int opt_idx = -1;
	//	int member_size = 5;
	//	float y_opt = m_cparam.rs_grab_y_opt;
	//	if (m_dtype != 0) {
	//		y_opt = m_cparam.sc_grab_y_opt;
	//	}
	//	for (int i = 0; i < cluster_member.size(); ++i) {
	//		if (cluster_member.at(i).size() < member_size) {
	//			continue;
	//		}
	//		if (fabs(cluster_center.at(i).y -y_opt) < min_y_dist) {
	//			min_y_dist = fabs(cluster_center.at(i).y - y_opt);
	//			opt_idx = i;
	//		}
	//	}
	//	if (opt_idx < 0) {
	//		if (m_pLogger) {
	//			stringstream buff;
	//			buff << "get_optimal_seed failed, get invalid optimal cluster id";
	//			m_pLogger->ERRORINFO(buff.str());
	//		}
	//		return;
	//	}
	//	//find nearest pt
	//	float nearest_dist = 1.0e6;
	//	int nearest_idx = -1;
	//	for (int i = 0; i < cluster_member.at(opt_idx).size(); ++i) {
	//		int idx = cluster_member.at(opt_idx).at(i);
	//		float dist = fabs(in_cloud->points.at(idx).x - cluster_center.at(opt_idx).x) +
	//			fabs(in_cloud->points.at(idx).y - cluster_center.at(opt_idx).y) +
	//			fabs(in_cloud->points.at(idx).z - cluster_center.at(opt_idx).z);
	//		if (dist < nearest_dist) {
	//			nearest_dist = dist;
	//			nearest_idx = idx;
	//		}
	//	}
	//	pt.x = in_cloud->points.at(nearest_idx).x;
	//	pt.y = in_cloud->points.at(nearest_idx).y;
	//	pt.z = in_cloud->points.at(nearest_idx).z;
	//	pt_idx = nearest_idx;
	//}
	void CRootStockGrabPoint::get_optimal_seed(
		pcl::PointCloud<pcl::PointXYZ>::Ptr in_cloud,
		pcl::PointXYZ&pt,
		int &pt_idx)
	{
		/*double d1 = m_cparam.rs_grab_stem_diameter;
		double d2 = m_cparam.rs_grab_stem_diameter * 4.0;
		if (m_dtype != 0) {
			d1 = m_cparam.sc_grab_stem_diameter;
			d2 = m_cparam.sc_grab_stem_diameter * 4.0;
		}
		std::vector<pcl::PointXYZ>cluster_center;
		std::vector<std::vector<int>> cluster_member;
		euclidean_clustering_ttsas(in_cloud, d1, d2, cluster_center, cluster_member);*/

		float min_y_dist = 1.0e6;
		int opt_idx = -1;
		int member_size = 5;
		float y_opt = m_cparam.rs_grab_y_opt;
		if (m_dtype != 0) {
			y_opt = m_cparam.sc_grab_y_opt;
		}
		for (int i = 0; i < in_cloud->points.size(); ++i) {
			/*if (cluster_member.at(i).size() < member_size) {
				continue;
			}*/
			if (fabs(in_cloud->points.at(i).y - y_opt) < min_y_dist) {
				min_y_dist = fabs(in_cloud->points.at(i).y - y_opt);
				opt_idx = i;
			}
		}
		if (opt_idx < 0) {
			if (m_pLogger) {
				stringstream buff;
				buff << "get_optimal_seed failed, get invalid optimal cluster id";
				m_pLogger->ERRORINFO(buff.str());
			}
			return;
		}
		//find nearest pt
		/*float nearest_dist = 1.0e6;
		int nearest_idx = -1;
		for (int i = 0; i < cluster_member.at(opt_idx).size(); ++i) {
			int idx = cluster_member.at(opt_idx).at(i);
			float dist = fabs(in_cloud->points.at(idx).x - cluster_center.at(opt_idx).x) +
				fabs(in_cloud->points.at(idx).y - cluster_center.at(opt_idx).y) +
				fabs(in_cloud->points.at(idx).z - cluster_center.at(opt_idx).z);
			if (dist < nearest_dist) {
				nearest_dist = dist;
				nearest_idx = idx;
			}
		}*/
		pt.x = in_cloud->points.at(opt_idx).x;
		pt.y = in_cloud->points.at(opt_idx).y;
		pt.z = in_cloud->points.at(opt_idx).z;
		pt_idx = opt_idx;
	}
	void CRootStockGrabPoint::viewer_cloud(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, std::string&winname)
	{
		pcl::visualization::CloudViewer viewer(winname);
		//viewer.runOnVisualizationThreadOnce(viewerOneOff);	
		viewer.showCloud(cloud);
		while (!viewer.wasStopped()) {
			boost::this_thread::sleep(boost::posix_time::microseconds(100000));
		}
	}
	void CRootStockGrabPoint::viewer_cloud(pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud, std::string&winname)
	{
		pcl::visualization::CloudViewer viewer(winname);
		//viewer.runOnVisualizationThreadOnce(viewerOneOff);	
		viewer.showCloud(cloud);
		while (!viewer.wasStopped()) {
			boost::this_thread::sleep(boost::posix_time::microseconds(100000));
		}
	}
	void CRootStockGrabPoint::viewer_cloud_debug(pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud, pcl::PointXYZ &p, std::string&winname)
	{
		pcl::visualization::PCLVisualizer viewer(winname);
		//viewer.runOnVisualizationThreadOnce(viewerOneOff);	
		viewer.addPointCloud(cloud);
		viewer.addCoordinateSystem();
		pcl::PointXYZ p0, x1, y1,p00,x0,y0;
		p0.x = p.x;
		p0.y = p.y;
		p0.z = p.z;
		x1.x = p.x + 400.0;
		x1.y = p.y;
		x1.z = p.z;
		y1.x = p.x;
		y1.y = p.y + 200.0;
		y1.z = p.z;

		p00.x = 0.0;
		p00.y = 0.0;
		p00.z = p.z;
		x0.x = 600.0;
		x0.y = 0;
		x0.z = p.z;
		y0.x = 0.0;
		y0.y = 300.0;
		y0.z = p.z;

		viewer.addLine(p0, x1, 255, 0, 0, "x");
		viewer.addLine(p0, y1, 0, 255, 0, "y");

		viewer.addLine(p00, x0, 255, 0, 0, "x0");
		viewer.addLine(p00, y0, 0, 255, 0, "y0");

		while (!viewer.wasStopped()) {
			viewer.spinOnce(100);
 			boost::this_thread::sleep(boost::posix_time::microseconds(100000));
		}
	}

};