graft_cv/grab_point_rs.cpp

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

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\statistical_outlier_removal.h>
#include <pcl\filters\voxel_grid.h>
#include <pcl\common\common.h>
#include <pcl\features\moment_of_inertia_estimation.h>
#include <pcl\segmentation\sac_segmentation.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),
		m_pcdId(""),
		m_ppImgSaver(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_dtype == 0) {
			m_pcdId = getImgId(img_type::sola_sc_pcd);
		}
		else {
			m_pcdId = getImgId(img_type::sola_rs_pcd);
		}

		if (m_ppImgSaver && *m_ppImgSaver) {
			(*m_ppImgSaver)->saveBinPly(m_raw_cloud, m_pcdId);
		}
		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, scion
		// dtype != 0, rootstock
		// 输入，穗苗--0， 砧木--1

		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;
		}
		////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
		//1 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) {//rootstock
			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 {//scion
			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");
		}
		////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
		//2 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;
		}
		double remove_radius = stem_radius * 2.0;

		pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_ror(new pcl::PointCloud<pcl::PointXYZ>);
		pcl::RadiusOutlierRemoval<pcl::PointXYZ> ror(false); 
		ror.setInputCloud(cloud_inbox);
		ror.setRadiusSearch(remove_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 (if < 50, return)";
			m_pLogger->INFO(buff.str());
		}
		if (cloud_dowm_sampled->width * cloud_dowm_sampled->height < 50) {
			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;	//存储找到的第一个位置上植株茎上直线点的index
		pcl::PointXYZ xz_center;					//存储找到的第一个位置上植株根部坐标
		if (m_pLogger) {
			m_pLogger->INFO("begin find seedling position");
		}
		//find_seedling_position_line_based(cloud_ror, first_seedling_cloud_idx, xz_center);
		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);
		//改进思路：将茎直线上cloud_seedling_seed的点，提取周围邻域xz的宽度，在相同邻域cloud_dowm_sampled点云内提取xz的宽度
		//对比两个宽度，变化很大的，说明周边有异物（叶柄或叶子），不适合作为抓取位置；否则就是抓取位置
		//在众多抓取位置上，选择离指定高度最近的点作为抓取位置
		// 
		pcl::PointXYZ selected_pt;
		int selected_pt_idx;
		std::vector<int> optimal_seeds_idx;
		get_optimal_seed_compare(cloud_dowm_sampled, cloud_seedling_seed, selected_pt, selected_pt_idx, optimal_seeds_idx);
		
		if (selected_pt_idx < 0) {	
			if (m_pLogger) {
				m_pLogger->ERRORINFO("Not found valid grab point");
			}
			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;

		////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
		//6 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 : optimal_seeds_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, xz_center, 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_tray_top_edge(
	//pcl::PointCloud<pcl::PointXYZ>::Ptr in_cloud,
	//float & tray_top_edge_y
	//)
	//{
	//	tray_top_edge_y = -1000.0;
	//	//以毫米为单位构建vector
	//	pcl::PointXYZ min_point_aabb;
	//	pcl::PointXYZ max_point_aabb;
	//	pcl::MomentOfInertiaEstimation<pcl::PointXYZ> feature_extractor;
	//	feature_extractor.setInputCloud(in_cloud);
	//	feature_extractor.compute();
	//	feature_extractor.getAABB(min_point_aabb, max_point_aabb);
	//	float miny = min_point_aabb.y;
	//	float maxy = max_point_aabb.y;
	//	if(maxy - miny <5.0){
	//		tray_top_edge_y = maxy;
	//		return;
	//	}
	//	std::vector<int> y_cnt_hist(int(maxy - miny), 0);
	//	for(auto& pt : in_cloud->points){
	//		int idx =  (int)(pt.y - miny);
	//		if(idx<0){continue;}
	//		if (idx >= y_cnt_hist.size()) { continue; }
	//		y_cnt_hist.at(idx) += 1;
	//	}
	//	//从上半部分找到最大值，作为平面上沿
	//	int idx_part = (int)(y_cnt_hist.size() / 2);
	//	int idx_edge = std::max_element(y_cnt_hist.begin(), y_cnt_hist.begin() + idx_part) - y_cnt_hist.begin();
	//	tray_top_edge_y = miny + (float)(idx_edge + 2.0);
	//}

	//void CRootStockGrabPoint::find_seedling_position_line_based(
	//	pcl::PointCloud<pcl::PointXYZ>::Ptr in_cloud,
	//	std::vector<int> &first_seedling_cloud_idx,
	//	pcl::PointXYZ&xz_center
	//)
	//{
	//	//找到穴盘上沿z，最优抓取的z，再在最优抓取的z基础上加上3作为有效范围
	//	float tray_y = -1000.0;
	//	float top_y = 0.0;
	//	find_tray_top_edge(in_cloud, tray_y);
	//	if (tray_y < -500.0) {
	//		if (m_dtype == 0) {
	//			//scion
	//			tray_y = m_cparam.sc_grab_y_opt-2.0;
	//		}
	//		else {
	//			tray_y = m_cparam.rs_grab_y_opt-2.0;
	//		}			
	//	}
	//	//up limit		
	//	if (m_dtype == 0) {
	//		top_y = m_cparam.sc_grab_zmax;
	//		if (top_y > m_cparam.sc_grab_y_opt + 3.0) {
	//			top_y = m_cparam.sc_grab_y_opt + 3.0;
	//		}
	//	}
	//	else {
	//		top_y = m_cparam.rs_grab_zmax;
	//		if (top_y > m_cparam.rs_grab_y_opt + 3.0) {
	//			top_y = m_cparam.rs_grab_y_opt + 3.0;
	//		}
	//	}
	//	//sub cloud
	//	pcl::PointCloud<pcl::PointXYZ>::Ptr sub_cloud(new pcl::PointCloud<pcl::PointXYZ>);
	//	pcl::CropBox<pcl::PointXYZ> box_filter;		
	//	if (m_dtype != 0) {//rootstock
	//		box_filter.setMin(Eigen::Vector4f(m_cparam.rs_grab_xmin, tray_y, m_cparam.rs_grab_zmin, 1));
	//		box_filter.setMax(Eigen::Vector4f(m_cparam.rs_grab_xmax, top_y, m_cparam.rs_grab_zmax, 1));
	//	}
	//	else {//scion
	//		box_filter.setMin(Eigen::Vector4f(m_cparam.sc_grab_xmin, tray_y, m_cparam.sc_grab_zmin, 1));
	//		box_filter.setMax(Eigen::Vector4f(m_cparam.sc_grab_xmax, top_y, m_cparam.sc_grab_zmax, 1));
	//	}
	//	box_filter.setNegative(false);
	//	box_filter.setInputCloud(in_cloud);
	//	box_filter.filter(*sub_cloud);

	//	if (m_cparam.image_show) {			
	//		viewer_cloud(sub_cloud, std::string("sub inbox"));
	//	}

 //       //在sub_cloud中进行直线检测
	//	segement_line(sub_cloud);

	//}
	//void CRootStockGrabPoint::segement_line(
	//	pcl::PointCloud<pcl::PointXYZ>::Ptr in_cloud
	//)
	//{
	//	pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
	//	pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
	//	pcl::SACSegmentation<pcl::PointXYZ> seg;
	//	seg.setOptimizeCoefficients(true);
	//	seg.setModelType(pcl::SACMODEL_LINE);
	//	seg.setMethodType(pcl::SAC_RANSAC);
	//	seg.setDistanceThreshold(0.5);
	//	seg.setInputCloud(in_cloud);
	//	seg.segment(*inliers, *coefficients);

	//	if (m_cparam.image_show) {
	//		pcl::PointCloud<pcl::PointXYZ>::Ptr line_cloud(new pcl::PointCloud<pcl::PointXYZ>);
	//		pcl::copyPointCloud(*in_cloud, *inliers, *line_cloud);
	//		viewer_cloud(line_cloud, std::string("cloud_line"));
	//	}

	//}
	////////////////////////////////////////////////////////////
	void CRootStockGrabPoint::find_seedling_position(		
		pcl::PointCloud<pcl::PointXYZ>::Ptr in_cloud,
		std::vector<int> &first_seedling_cloud_idx,
		pcl::PointXYZ&xz_center
	)
	{

		float hole_step = 40.0; //穴盘中穴间距
		float hole_step_radius = hole_step / 2.0;

		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());
		}

		if (m_cparam.image_show) {
			pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_cluster(new pcl::PointCloud<pcl::PointXYZRGB>);
			pcl::copyPointCloud(*cloud2d_pos, *cloud_cluster);

			for (auto& pt : *cloud_cluster) {
				pt.r = 255;
				pt.g = 255;
				pt.b = 255;
			}
			viewer_cloud_cluster(cloud_cluster, cluster_center,  std::string("cloud2d_cluster_pos"));
		}
		//对每个类别进行检测，剔除不是茎的类别
		std::vector<pcl::PointXYZ> clt_root;
		std::vector<pcl::PointXYZ> clt_box;
		std::vector<std::vector<int> > clt_line_idx;

		for (size_t i = 0; i < cluster_center.size(); ++i) {
			pcl::PointIndices cluster_idxs;
			for (auto idx_clt : cluster_member.at(i)) {
				int idx_raw = index.at(idx_clt);
				cluster_idxs.indices.push_back(idx_raw);
			}
			pcl::PointCloud<pcl::PointXYZ>::Ptr clt_cloud(new pcl::PointCloud < pcl::PointXYZ>);
			pcl::copyPointCloud(*in_cloud, cluster_idxs, *clt_cloud);
			//计算每一个茎的外接矩形
			pcl::PointXYZ min_point_aabb;
			pcl::PointXYZ max_point_aabb;
			pcl::MomentOfInertiaEstimation<pcl::PointXYZ> feature_extractor;
			feature_extractor.setInputCloud(clt_cloud);
			feature_extractor.compute();
			feature_extractor.getAABB(min_point_aabb, max_point_aabb);
			//扩展矩形范围
			pcl::PointXYZ min_point_aabb_ex;
			pcl::PointXYZ max_point_aabb_ex;
			min_point_aabb_ex.x = 0.5*(min_point_aabb.x + max_point_aabb.x) - hole_step_radius;
			min_point_aabb_ex.y = m_cparam.sc_grab_ymin;
			if (m_dtype != 0) { min_point_aabb_ex.y = m_cparam.rs_grab_ymin; }
			min_point_aabb_ex.z = 0.5*(min_point_aabb.z + max_point_aabb.z) - hole_step_radius;

			max_point_aabb_ex.x = 0.5*(min_point_aabb.x + max_point_aabb.x) + hole_step_radius;
			max_point_aabb_ex.y = m_cparam.sc_grab_ymax;
			if (m_dtype != 0) { max_point_aabb_ex.y = m_cparam.rs_grab_ymax; }
			max_point_aabb_ex.z = 0.5*(min_point_aabb.z + max_point_aabb.z) + hole_step_radius;

			/////////////////////////////////
			//扩展矩形内直线检测
			std::vector<int>line_idx;
			find_line_in_cube(in_cloud, min_point_aabb_ex, max_point_aabb_ex, line_idx);
			clt_line_idx.push_back(line_idx);
			//找到line_idx中y最小的那个点的idx
			int line_root_idx = -1;
			float line_root_y_min = 10000.0;
			for (size_t lidx = 0; lidx < line_idx.size();++lidx) {
				int raw_idx = line_idx.at(lidx);
				if (in_cloud->at(raw_idx).y < line_root_y_min) {
					line_root_y_min = in_cloud->at(raw_idx).y;
					line_root_idx = raw_idx;
				}
			}
			//找到底部中心点，然后找到根部坐标			
			pcl::PointXYZ pt_root;
			pt_root = in_cloud->at(line_root_idx);
		
			clt_root.push_back(pt_root);
			clt_box.push_back(min_point_aabb_ex);
			clt_box.push_back(max_point_aabb_ex);
			//viewer_cloud(clt_cloud, std::string("cluster"));			
		}
		if (m_cparam.image_show) {			
			viewer_cloud_cluster_box(in_cloud, clt_root, clt_box, clt_line_idx, std::string("cloud2d_cluster_box"));			
		}

		//sort cluster center, get the frontest leftest one
		std::vector<float> cluster_index;
		for (auto&pt : clt_root) {
			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();
		if (m_dtype == 0) {
			first_idx = std::max_element(cluster_index.begin(), cluster_index.end()) - cluster_index.begin();
		}

		first_seedling_cloud_idx.clear();
		for (auto&v : clt_line_idx.at(first_idx)) {			
			first_seedling_cloud_idx.push_back(v);
		}
		xz_center.x = clt_root.at(first_idx).x;
		xz_center.y = clt_root.at(first_idx).y;
		xz_center.z = clt_root.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::find_line_in_cube(
		pcl::PointCloud<pcl::PointXYZ>::Ptr in_cloud,	//输入，整体点云
		pcl::PointXYZ& min_pt_ex,				//输入，
		pcl::PointXYZ& max_pt_ex,				//输入，
		std::vector<int>& out_idx				//输出，直线上点的index， 基于输入整体点云
	)
	{
		out_idx.clear();
		pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_inbox(new pcl::PointCloud<pcl::PointXYZ>);
		pcl::CropBox<pcl::PointXYZ> box_filter;
		std::vector<int>inbox_idx;
		box_filter.setMin(Eigen::Vector4f(min_pt_ex.x, min_pt_ex.y, min_pt_ex.z, 1));
		box_filter.setMax(Eigen::Vector4f(max_pt_ex.x, max_pt_ex.y, max_pt_ex.z, 1));
		
		box_filter.setNegative(false);
		box_filter.setInputCloud(in_cloud);		
		box_filter.filter(inbox_idx);
		pcl::copyPointCloud(*in_cloud, inbox_idx, *cloud_inbox);	
		

		//找到inbox点云中的直线
		float stem_radius = m_cparam.rs_grab_stem_diameter / 2.0;
		if (m_dtype == 0) {
			stem_radius = m_cparam.sc_grab_stem_diameter / 2.0;
		}
		pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
		pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
		pcl::SACSegmentation<pcl::PointXYZ> seg;
		seg.setOptimizeCoefficients(true);
		seg.setModelType(pcl::SACMODEL_LINE);
		seg.setDistanceThreshold(stem_radius);
		seg.setInputCloud(cloud_inbox);
		seg.segment(*inliers, *coefficients);	
		
		int idx_raw = 0;
		for (auto& idx : inliers->indices) {
			idx_raw = inbox_idx.at(idx);
			out_idx.push_back(idx_raw);
		}


		if (m_cparam.image_show) {
			pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_line(new pcl::PointCloud<pcl::PointXYZ>);
			pcl::copyPointCloud(*cloud_inbox, *inliers, *cloud_line);

			pcl::visualization::PCLVisualizer::Ptr viewer(new pcl::visualization::PCLVisualizer("line in cluster"));
			viewer->addPointCloud<pcl::PointXYZ>(cloud_inbox, "cluster");
			viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 1, 1, 1, "cluster");
			viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "cluster");

			viewer->addPointCloud(cloud_line, "fit_line");
			viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 0, 1, 0, "fit_line");
			viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "fit_line");
			while (!viewer->wasStopped()) {
				viewer->spinOnce(100);
			}
		}
	}

	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;

		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 << "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;
	}
	//通过比较直线点云和原始点云相同位置邻域内xz的范围，确定此点是否是无干扰点的茎
	void CRootStockGrabPoint::get_optimal_seed_compare(
		pcl::PointCloud<pcl::PointXYZ>::Ptr in_cloud,		//	全部有效点云
		pcl::PointCloud<pcl::PointXYZ>::Ptr in_line_cloud,	//	茎上直线点云
		pcl::PointXYZ&pt,									//  返回抓取的点坐标
		int &pt_idx,										//	返回点index
		std::vector<int>& valid_line_index					//  返回有效直线点index
	)
	{
		valid_line_index.clear();
		float th = 0.75;		//原始点云和直线点云邻域尺寸增加原来的20%，不能当做抓取点
		pt_idx = -1;

		pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_inbox(new pcl::PointCloud<pcl::PointXYZ>);
		pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_inbox_line(new pcl::PointCloud<pcl::PointXYZ>);

		pcl::CropBox<pcl::PointXYZ> box_filter;
		box_filter.setNegative(false);
		box_filter.setInputCloud(in_cloud);

		pcl::CropBox<pcl::PointXYZ> box_filter_line;
		box_filter_line.setNegative(false);
		box_filter_line.setInputCloud(in_line_cloud);

		float radius = m_cparam.rs_grab_stem_diameter;
		float opt_y = m_cparam.rs_grab_y_opt;
		if (m_dtype == 0) {
			radius = m_cparam.sc_grab_stem_diameter;
			opt_y = m_cparam.sc_grab_y_opt;
		}
		float rx = 1.5;
		float ry = 1.5;
		float rz = 1.5;
		float cx, cy, cz;
		float dz,dx, dz_line, dx_line;
		float dist_min = 10000.0;

		for (size_t i = 0; i < in_line_cloud->points.size(); ++i) {
			cx = in_line_cloud->points.at(i).x;
			cy = in_line_cloud->points.at(i).y;
			cz = in_line_cloud->points.at(i).z;
			//////////////////////////////////////////////////////////////////
			//原始点云，获取指定区域的dx，dz
			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);

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

			/////////////////////////////////////////////////////////////////////
			//直线点云，获取指定区域的dx_line，dz_line
			box_filter_line.setMin(Eigen::Vector4f(cx - rx*radius, cy - ry*radius, cz - rz*radius, 1));
			box_filter_line.setMax(Eigen::Vector4f(cx + rx*radius, cy + ry*radius, cz + rz*radius, 1));
			box_filter_line.filter(*cloud_inbox_line);
			pcl::getMinMax3D(*cloud_inbox_line, min_point, max_point);
			dz_line = max_point(2) - min_point(2);
			dx_line = max_point(0) - min_point(0);

			float ratio_ex = (dx + dz - dz_line - dx_line) / (dz_line + dx_line);
			if (ratio_ex < th) {
				valid_line_index.push_back(i);
				if (fabs(cy - opt_y) < dist_min) {
					dist_min = fabs(cy - opt_y);
					pt_idx = i;
				}
			}

		}
		if (pt_idx >= 0) {
			pt = in_line_cloud->points.at(pt_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, 
		pcl::PointXYZ &root_pt,
		std::string&winname)
	{
		pcl::visualization::PCLVisualizer viewer(winname);
		//viewer.runOnVisualizationThreadOnce(viewerOneOff);	
		viewer.addPointCloud(cloud);
		viewer.addCoordinateSystem();
		pcl::PointXYZ p0, x1, y1,p00,x0,y0, root_px0, root_px1, root_py0,root_py1;
		p0.x = p.x;
		p0.y = p.y;
		p0.z = p.z;
		x1.x = p.x + 100.0;
		x1.y = p.y;
		x1.z = p.z;
		y1.x = p.x;
		y1.y = p.y + 20.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;
		
		root_px0.x = root_pt.x - 5.0;
		root_px0.y = root_pt.y;
		root_px0.z = root_pt.z;
		root_px1.x = root_pt.x + 5.0;
		root_px1.y = root_pt.y;
		root_px1.z = root_pt.z;

		root_py0.x = root_pt.x;
		root_py0.y = root_pt.y - 5.0;
		root_py0.z = root_pt.z;
		root_py1.x = root_pt.x;
		root_py1.y = root_pt.y + 5.0;
		root_py1.z = root_pt.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");

		viewer.addLine(root_px0, root_px1, 255, 0, 0, "rootx");
		viewer.addLine(root_py0, root_py1, 0, 255, 0, "rooty");

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

	void CRootStockGrabPoint::viewer_cloud_cluster(
		pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud, 
		std::vector<pcl::PointXYZ>cluster_center, 
		std::string&winname)
	{
		pcl::visualization::PCLVisualizer viewer(winname);		
		viewer.addPointCloud(cloud);
		viewer.addCoordinateSystem();
		int cnt = 0;
		char buf[8];
		for (auto& p : cluster_center) {
			pcl::PointXYZ px0, px1, py1, py0;
			px0.x = p.x-5.0;
			px0.y = p.y;
			px0.z = p.z;
			px1.x = p.x + 5.0;
			px1.y = p.y;
			px1.z = p.z;

			py0.x = p.x;
			py0.y = p.y - 5.0;
			py0.z = p.z;
			py1.x = p.x;
			py1.y = p.y + 5.0;
			py1.z = p.z;

			viewer.addLine(px0, px1, 255, 0, 0, "x" + string(_itoa(cnt, buf,10)));
			viewer.addLine(py0, py1, 0, 255, 0, "y" + string(_itoa(cnt, buf, 10)));
			cnt += 1;
		}
		

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

	}
	void CRootStockGrabPoint::viewer_cloud_cluster_box(
		pcl::PointCloud<pcl::PointXYZ>::Ptr cloud,
		std::vector<pcl::PointXYZ>&cluster_center,
		std::vector<pcl::PointXYZ>&cluster_box,
		std::vector<std::vector<int> >& clt_line_idx,
		std::string&winname)
	{
		pcl::visualization::PCLVisualizer viewer(winname);
		viewer.addCoordinateSystem();
		viewer.addPointCloud<pcl::PointXYZ>(cloud, "all_cloud");
		viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 1, 1, 1, "all_cloud");
		viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "all_cloud");
		
		int cnt = 0;
		char buf[8];
		for (size_t i = 0; i < cluster_center.size();++i) {
			pcl::PointXYZ &p = cluster_center.at(i);
			pcl::PointXYZ px0, px1, py1, py0;
			px0.x = p.x - 5.0;
			px0.y = p.y;
			px0.z = p.z;
			px1.x = p.x + 5.0;
			px1.y = p.y;
			px1.z = p.z;

			py0.x = p.x;
			py0.y = p.y - 5.0;
			py0.z = p.z;
			py1.x = p.x;
			py1.y = p.y + 5.0;
			py1.z = p.z;

			viewer.addLine(px0, px1, 255, 0, 0, "x" + string(_itoa(cnt, buf, 10)));
			viewer.addLine(py0, py1, 0, 255, 0, "y" + string(_itoa(cnt, buf, 10)));

			//box
			pcl::PointXYZ & min_pt = cluster_box.at(2 * i);
			pcl::PointXYZ & max_pt = cluster_box.at(2 * i + 1);
			viewer.addCube(min_pt.x, max_pt.x, min_pt.y, max_pt.y, min_pt.z, max_pt.z, 0.5,0.5,0.0,"AABB_"+string(_itoa(cnt, buf, 10)));
			viewer.setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_REPRESENTATION,
				pcl::visualization::PCL_VISUALIZER_REPRESENTATION_WIREFRAME, "AABB_" + string(_itoa(cnt, buf, 10)));

			pcl::PointCloud<pcl::PointXYZ>::Ptr line_cloud(new pcl::PointCloud<pcl::PointXYZ>);
			pcl::copyPointCloud(*cloud, clt_line_idx.at(i), *line_cloud);
			viewer.addPointCloud(line_cloud, "fit_line" + string(_itoa(cnt, buf, 10)));
			viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 0, 1, 0, "fit_line" + string(_itoa(cnt, buf, 10)));
			viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "fit_line" + string(_itoa(cnt, buf, 10)));

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

	}

};