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),
		m_1st_row_zmean_rs(-1.0),
		m_1st_row_zmean_sc(-1.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,
		int dtype,
		const char* fn/* = 0*/)
	{
		int rst = 0;
		m_dtype = dtype;
		//generate image id
		if (m_dtype == 0) {
			m_pcdId = getImgId(img_type::sola_sc_pcd);
		}
		else {
			m_pcdId = getImgId(img_type::sola_rs_pcd);
		}

		//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 << m_pcdId <<": 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(m_pcdId + ": no valid input");
				return (-1);
			}
		}	

		

		if (m_ppImgSaver && *m_ppImgSaver) {
			(*m_ppImgSaver)->saveBinPly(m_raw_cloud, m_pcdId);
		}
		if (m_cparam.image_show) {
			viewer_cloud(m_raw_cloud, m_pcdId + std::string(": raw point cloud"));
		}
		return rst;
	}

	int CRootStockGrabPoint::grab_point_detect(
		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_pcdId << ": 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(m_pcdId + ": begin crop");
		}
		pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_inbox(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, 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 << m_pcdId << ": 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(m_pcdId + ": end crop");
		}
		////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
		//2 filtler with radius remove
		if (m_pLogger) {
			m_pLogger->INFO(m_pcdId + ": begin ror");
		}
		int nb_points = 20;
		double stem_radius = m_cparam.rs_grab_stem_diameter / 2.0;
		if(m_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 << m_pcdId <<": 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(m_pcdId + ": end ror");
		}
		////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
		//3 voxel grid down sampling
		if (m_pLogger) {
			m_pLogger->INFO(m_pcdId + ": 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);
		double voxel_size = m_cparam.rs_grab_voxel_size;
		if (m_dtype == 0) { voxel_size = m_cparam.sc_grab_voxel_size; }
		outrem.setLeafSize(voxel_size, voxel_size, voxel_size);
		outrem.filter(*cloud_dowm_sampled);

		if (m_pLogger) {
			stringstream buff;
			buff << m_pcdId <<": 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(m_pcdId + ": end down");
		}

		////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
		//4 seedling position，找到第一个位置上的植株
		std::vector<int> first_seedling_cloud_idx;	//存储找到的第一个位置上植株茎上直线点的index
		pcl::PointXYZ xz_center;					//存储找到的第一个位置上植株根部坐标
		pcl::ModelCoefficients::Ptr line_model;		//存储找到的第一个位置上植株茎直线模型
		if (m_pLogger) {
			m_pLogger->INFO(m_pcdId + ": begin find seedling position");
		}		
		find_seedling_position_key(cloud_dowm_sampled, first_seedling_cloud_idx, xz_center, line_model);
		if (m_pLogger) {
			stringstream buff;
			buff << m_pcdId <<": 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(m_pcdId + ": 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,		//input
			cloud_seedling_seed,	//input
			line_model,				//input
			selected_pt,			//output
			selected_pt_idx,		//output
			optimal_seeds_idx);		//output
		
		if (selected_pt_idx < 0) {	
			if (m_pLogger) {
				m_pLogger->ERRORINFO(m_pcdId + ": Not found valid fork point");
			}
			return 1;
		}
		if (m_pLogger) {
			m_pLogger->INFO(m_pcdId + ": end get_optimal_seed");
		}
		if (m_dtype == 0) {
			posinfo.sc_grab_x = selected_pt.x;
			posinfo.sc_grab_y = selected_pt.y;
			posinfo.sc_grab_z = selected_pt.z;
		}
		else {
			posinfo.rs_grab_x = selected_pt.x;
			posinfo.rs_grab_y = selected_pt.y;
			posinfo.rs_grab_z = selected_pt.z;
		}
		////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
		//6 保存处理结果到图片
		cv::Mat rst_img = cv::Mat::zeros(cv::Size(1280,640),CV_8UC1);
		gen_result_img(cloud_dowm_sampled, selected_pt, rst_img);
		if (m_ppImgSaver && *m_ppImgSaver) {
			(*m_ppImgSaver)->saveImage(rst_img, m_pcdId + "_rst_0");
		}
		////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
		//7 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"));
			imshow_wait("rst_img", rst_img);
		}
		return 0;
	}

	//生成结果图片
	void CRootStockGrabPoint::gen_result_img(
		pcl::PointCloud<pcl::PointXYZ>::Ptr in_cloud,	//输入，整体点云cloud_dowm_sampled, 
		pcl::PointXYZ& selected_pt,				//输入，selected_pt, 
		cv::Mat& rst_img						//输出，图片， 640*1280
	)
	{
		if (rst_img.empty()) { return; }
		if (rst_img.rows != 640 && rst_img.cols != 1280) { return; }
		int cx = 640;		//图像中心点0
		int cy = 320;		//图像中心点0
		float res = 0.33333;	//分辨率，1个像素0.333mm
		//绘制坐标轴
		int arrow_len = 20;
		cv::line(rst_img, cv::Point(0, cy), cv::Point(cx, cy), cv::Scalar(128));
		cv::line(rst_img, cv::Point(0, cy), cv::Point(arrow_len, cy - int(arrow_len/2)), cv::Scalar(128));
		cv::line(rst_img, cv::Point(0, cy), cv::Point(arrow_len, cy + int(arrow_len/2)), cv::Scalar(128));

		cv::line(rst_img, cv::Point(cx, 0), cv::Point(cx, cy), cv::Scalar(128));
		cv::line(rst_img, cv::Point(cx, 0), cv::Point(cx - int(arrow_len/2), arrow_len), cv::Scalar(128));
		cv::line(rst_img, cv::Point(cx, 0), cv::Point(cx + int(arrow_len/2), arrow_len), cv::Scalar(128));

		cv::putText(rst_img, std::string("x"), cv::Point(20, cy-10), cv::FONT_HERSHEY_SIMPLEX, 1, cv::Scalar(128));
		cv::putText(rst_img, std::string("y"), cv::Point(cx+10, 20), cv::FONT_HERSHEY_SIMPLEX, 1, cv::Scalar(128));
		
		//绘制所有点
		int x, y;
		for (auto&pt : in_cloud->points) {
			x = cx - int(pt.x / res + 0.5);
			y = cy - int(pt.y / res + 0.5);
			if (x < 0 || x >= rst_img.cols) { continue; }
			if (y < 0 || y >= rst_img.rows) { continue; }
			rst_img.at<unsigned char>(y, x) = 255;
		}
		//绘制抓取点坐标
		int grab_x = cx - int(selected_pt.x / res + 0.5);
		int grab_y = cy - int(selected_pt.y / res + 0.5);
		int radius = 30;
		cv::line(rst_img, cv::Point(grab_x - radius, grab_y - radius), cv::Point(grab_x + radius, grab_y + radius), cv::Scalar(128));
		cv::line(rst_img, cv::Point(grab_x - radius, grab_y + radius), cv::Point(grab_x + radius, grab_y - radius), cv::Scalar(128));
	}
	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(m_pcdId + ": could not load file: "+std::string(fn));
			}
			return (-1);
		}
		if (m_pLogger) {
			stringstream buff;
			buff << m_pcdId <<": 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::gen_all_seedling_positions(
	pcl::PointXYZ&key_center,	//输入，已知的苗的坐标
	std::vector<pcl::PointXYZ>&candidate_center	//输出，有倾斜苗的坐标
)
{
	//生成所有的植株位置，并排序
	candidate_center.clear();
	
	//找到z最小的那一株，并找出和它一排的那些
	float step_harf = m_cparam.rs_grab_seedling_dist / 2.0;
	float xmin = m_cparam.rs_grab_xmin;
	float xmax = m_cparam.rs_grab_xmax;
	float zmin = m_cparam.rs_grab_zmin;
	float zmax = m_cparam.rs_grab_zmax;
	int col_from = -8;
	int col_to = 8;
	int col_step = 1;

	if (m_dtype == 0) {
		step_harf = m_cparam.sc_grab_seedling_dist / 2.0;
		xmin = m_cparam.sc_grab_xmin;
		xmax = m_cparam.sc_grab_xmax;
		zmin = m_cparam.sc_grab_zmin;
		zmax = m_cparam.sc_grab_zmax;		
	}

	// 生成所有可能的植株位置中心点	
	for (int row = -8; row <= 4; row += 1) {
		float cz = key_center.z + row * step_harf/2.0;
		if (cz < zmin || cz > zmax) { continue; }
		for (int col = 4*col_from; col <= 4*col_to; col += col_step) {
			float cx = key_center.x + col * step_harf/2;			
			if (cx < xmin || cx > xmax) { continue; }			
			//保存
			pcl::PointXYZ pc(cx, 0.0, cz);
			candidate_center.push_back(pc);
		}
	}
}

void CRootStockGrabPoint::nms_box(
	std::vector<pcl::PointXYZ>&clt_root_v,	
	std::vector<float>&valid_box_cc_dist,
	float hole_step_radius, 
	std::vector<pcl::PointXYZ>& target_toot)
{	
	target_toot.clear();
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
	for (auto&p : clt_root_v) { cloud->points.push_back(p); }
	
	pcl::KdTreeFLANN<pcl::PointXYZ> kdtree;
	kdtree.setInputCloud(cloud);
	std::vector<int>idx;
	std::vector<float>dist_sqr;
	std::vector<int> tar_idx;
	for (size_t i = 0; i < cloud->points.size(); ++i) {
		int k = kdtree.radiusSearch(cloud->points.at(i), hole_step_radius, idx, dist_sqr);
		std::vector<float> cc_tmp;
		for (auto& j : idx) { cc_tmp.push_back(valid_box_cc_dist.at(j)); }
		int min_id = std::min_element(cc_tmp.begin(), cc_tmp.end()) - cc_tmp.begin();
		if (min_id == 0) {
			tar_idx.push_back(i);
		}		
	}
	for (auto& i : tar_idx) {
		target_toot.push_back(clt_root_v.at(i));
	}

}
//通过指定位置内,取部分点云分析是否存在真正的茎，真茎位置保存到target_filtered
void CRootStockGrabPoint::line_filter(
	pcl::PointCloud<pcl::PointXYZ>::Ptr in_cloud,  //输入点云
	float radius,												//输入，取点半径
	float ymin,
	float ymax,
	std::vector<pcl::PointXYZ>&target_root,						//输入，位置
	std::vector<pcl::PointXYZ>&target_filtered,					//输出，位置
	std::vector<pcl::PointXYZ>&target_filtered_root,			//输出，茎上根部点坐标
	std::vector<std::vector<int>>&target_filtered_element,		//输出，茎上点index
	std::vector<pcl::ModelCoefficients::Ptr>& target_filtered_models//输出，茎直线模型

)
{
	target_filtered.clear();
	target_filtered_element.clear();
	target_filtered_root.clear();
	if (target_root.size() == 0) { return; }

	for (auto&p : target_root) {
		// 构建box，获取植株点云
		pcl::PointCloud<pcl::PointXYZ>::Ptr seedling_inbox(new pcl::PointCloud<pcl::PointXYZ>);
		pcl::CropBox<pcl::PointXYZ> box_filter;
		std::vector<int>inbox_idx;
		pcl::PointXYZ min_point_aabb_ex;
		pcl::PointXYZ max_point_aabb_ex;

		min_point_aabb_ex.x = p.x - radius;
		min_point_aabb_ex.y = ymin;
		min_point_aabb_ex.z = p.z - radius;
		max_point_aabb_ex.x = p.x + radius;
		max_point_aabb_ex.y = ymax;
		max_point_aabb_ex.z = p.z + radius;

		box_filter.setMin(Eigen::Vector4f(min_point_aabb_ex.x, min_point_aabb_ex.y, min_point_aabb_ex.z, 1));
		box_filter.setMax(Eigen::Vector4f(max_point_aabb_ex.x, max_point_aabb_ex.y, max_point_aabb_ex.z, 1));

		box_filter.setNegative(false);
		box_filter.setInputCloud(in_cloud);
		box_filter.filter(inbox_idx);
		pcl::copyPointCloud(*in_cloud, inbox_idx, *seedling_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(seedling_inbox);
		seg.segment(*inliers, *coefficients);


		pcl::PointCloud<pcl::PointXYZ>::Ptr stem_cloud(new pcl::PointCloud<pcl::PointXYZ>);
		pcl::copyPointCloud(*seedling_inbox, *inliers, *stem_cloud);
		
		//点数过滤
		int min_stem_pts = m_cparam.rs_grab_stem_min_pts;
		if (m_dtype == 0) { min_stem_pts = m_cparam.sc_grab_stem_min_pts; }
		if (inliers->indices.size() < int(min_stem_pts / 2)) { continue; }
		//y方向分布范围滤波
		pcl::PointXYZ min_v;
		pcl::PointXYZ max_v;
		pcl::getMinMax3D(*stem_cloud, min_v, max_v);
		float dy = max_v.y - min_v.y;
		if (dy / (ymax - ymin) < 0.5) { continue; }
		//y方向分布中心滤波
		float dy_c = 0.5*(max_v.y + min_v.y);
		if ((dy_c - ymin) / (ymax - ymin) > 0.75) { continue; }
		//倾斜角度过滤
		float xz = std::sqrt(coefficients->values.at(3) * coefficients->values.at(3) +
			coefficients->values.at(5) * coefficients->values.at(5));
		float a = std::atan2f(coefficients->values.at(4), xz);
		a = std::fabs(a) * 180.0 / 3.14159;
		if (a < 45.0) { continue; }
		target_filtered.push_back(p);

		float min_y = 10000.0;
		int min_y_idx = -1;
		for (size_t j = 0; j < stem_cloud->points.size();++j) {
			pcl::PointXYZ &spt = stem_cloud->at(j);
			if (spt.y < min_y) {
				min_y = spt.y;
				min_y_idx = j;
			}
		}
		pcl::PointXYZ tr_pt(stem_cloud->points.at(min_y_idx).x, stem_cloud->points.at(min_y_idx).y, stem_cloud->points.at(min_y_idx).z);
		target_filtered_root.push_back(tr_pt);
		
		int idx_raw = 0;
		std::vector<int> out_idx;
		for (auto& idx : inliers->indices) {
			idx_raw = inbox_idx.at(idx);
			out_idx.push_back(idx_raw);
		}
		target_filtered_element.push_back(out_idx);
		target_filtered_models.push_back(coefficients);
		
		// debug show
		/*if (m_cparam.image_show) {
			pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_line(new pcl::PointCloud<pcl::PointXYZ>);
			pcl::copyPointCloud(*seedling_inbox, *inliers, *cloud_line);

			pcl::visualization::PCLVisualizer::Ptr viewer(new pcl::visualization::PCLVisualizer("line in cluster"));
			viewer->addPointCloud<pcl::PointXYZ>(seedling_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::find_seedling_position_key(		
		pcl::PointCloud<pcl::PointXYZ>::Ptr in_cloud,
		std::vector<int> &first_seedling_cloud_idx,
		pcl::PointXYZ&xz_center,
		pcl::ModelCoefficients::Ptr& line_model
	)
	{
		// 确定植株inbox范围
		float hole_step = m_cparam.rs_grab_seedling_dist - 5.0; //穴盘中穴间距
		if (m_dtype == 0) {
			hole_step = m_cparam.sc_grab_seedling_dist - 5.0;
		}
		float hole_step_radius = hole_step / 2.0;

		// 点云降维到xz平面，y=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;
		}	

		// 在xz平面内统计点的密度
		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 max_density_idx = std::max_element(counter.begin(), counter.end()) - counter.begin();
		int max_density = counter.at(max_density_idx);
		
		if (m_cparam.image_show) {
			//显示密度最大的点的位置
			pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_tmp(new pcl::PointCloud<pcl::PointXYZRGB>);
			pcl::copyPointCloud(*in_cloud, *cloud_tmp);
			for (auto& pt : *cloud_tmp) {
				pt.r = 255;
				pt.g = 255;
				pt.b = 255;
			}
			std::vector<pcl::PointXYZ>cc;
			cc.push_back(in_cloud->points.at(max_density_idx));
			viewer_cloud_cluster(cloud_tmp, cc, string("key"));
		}
		//  生成植株的中心及box
		std::vector<pcl::PointXYZ>clt_root;
		std::vector<pcl::PointXYZ> clt_box;
		float ymin = m_cparam.rs_grab_ymin;
		float ymax = m_cparam.rs_grab_ymax;
		if (m_dtype == 0) {			
			ymin = m_cparam.sc_grab_ymin;
			ymax = m_cparam.sc_grab_ymax;			
		}
		gen_all_seedling_positions(in_cloud->points.at(max_density_idx), clt_root);		

		// 每个位置点云情况，过滤		
		std::vector<int> valid_index;		//初步有效的矩形index
		std::vector<int> valid_box_pts;		//立方体内点云数量
		std::vector<float>valid_box_cc_dist;//重心和矩形中心距离
		for (size_t i = 0; i < clt_root.size();++i) {
			pcl::PointXYZ &p = clt_root.at(i);
			pcl::PointCloud<pcl::PointXYZ>::Ptr seedling_inbox(new pcl::PointCloud<pcl::PointXYZ>);
			pcl::CropBox<pcl::PointXYZ> box_filter;
			std::vector<int>inbox_idx;
			pcl::PointXYZ min_point_aabb_ex;
			pcl::PointXYZ max_point_aabb_ex;

			min_point_aabb_ex.x = p.x - hole_step_radius;
			min_point_aabb_ex.y = ymin;
			min_point_aabb_ex.z = p.z - hole_step_radius;
			max_point_aabb_ex.x = p.x + hole_step_radius;
			max_point_aabb_ex.y = ymax;
			max_point_aabb_ex.z = p.z + hole_step_radius;

			box_filter.setMin(Eigen::Vector4f(min_point_aabb_ex.x, min_point_aabb_ex.y, min_point_aabb_ex.z, 1));
			box_filter.setMax(Eigen::Vector4f(max_point_aabb_ex.x, max_point_aabb_ex.y, max_point_aabb_ex.z, 1));

			box_filter.setNegative(false);
			box_filter.setInputCloud(in_cloud);
			box_filter.filter(inbox_idx);
			pcl::copyPointCloud(*in_cloud, inbox_idx, *seedling_inbox);
			//点数过滤
			if (inbox_idx.size() < int(max_density/3)) { continue; }
			//y方向分布范围滤波
			pcl::PointXYZ min_v;
			pcl::PointXYZ max_v;
			pcl::getMinMax3D(*seedling_inbox, min_v, max_v);
			float dy = max_v.y - min_v.y;
			if (dy / (ymax - ymin) < 0.35) { continue; }
			//y方向分布中心滤波
			float dy_c = 0.5*(max_v.y + min_v.y);
			if ((dy_c-ymin) / (ymax - ymin) > 0.75) { continue; }
			//计算中心
			Eigen::Vector4f mean;
			pcl::compute3DCentroid(*seedling_inbox, mean);
			double cc_dist = std::sqrt((mean[0] - p.x)*(mean[0] - p.x) + (mean[2] - p.z)*(mean[2] - p.z));
			valid_index.push_back(i);
			valid_box_pts.push_back(inbox_idx.size());
			valid_box_cc_dist.push_back((float)cc_dist);			
		}
		// 找到局部最大值，找出真正的位置
		std::vector<pcl::PointXYZ> clt_root_v;
		std::vector<pcl::PointXYZ> clt_box_v;
		for (auto&i : valid_index) {
			pcl::PointXYZ&p = clt_root.at(i);
			pcl::PointXYZ p_show(p.x, ymin, p.z);
			clt_root_v.push_back(p_show);
			pcl::PointXYZ min_point_aabb_ex;
			pcl::PointXYZ max_point_aabb_ex;
			min_point_aabb_ex.x = p.x - hole_step_radius;
			min_point_aabb_ex.y = ymin;
			min_point_aabb_ex.z = p.z - hole_step_radius;
			max_point_aabb_ex.x = p.x + hole_step_radius;
			max_point_aabb_ex.y = ymax;
			max_point_aabb_ex.z = p.z + hole_step_radius;
			clt_box_v.push_back(min_point_aabb_ex);
			clt_box_v.push_back(max_point_aabb_ex);
		}
		// 显示			
		if (m_cparam.image_show) {
			std::vector<std::vector<int> > clt_line_idx;
			viewer_cloud_cluster_box(in_cloud, clt_root_v, clt_box_v, clt_line_idx, std::string("valid_box"));
		}
		std::vector<pcl::PointXYZ> target_root;
		nms_box(clt_root_v, valid_box_cc_dist, hole_step_radius, target_root);
		
		// 显示			
		if (m_cparam.image_show) {
			std::vector<std::vector<int> > clt_line_idx;
			std::vector<pcl::PointXYZ>clt_box_tmp;
			for (auto&p : target_root) {
				pcl::PointXYZ p_show(p.x, ymin, p.z);
				clt_root_v.push_back(p_show);
				pcl::PointXYZ min_point_aabb_ex;
				pcl::PointXYZ max_point_aabb_ex;
				min_point_aabb_ex.x = p.x - hole_step_radius;
				min_point_aabb_ex.y = ymin;
				min_point_aabb_ex.z = p.z - hole_step_radius;
				max_point_aabb_ex.x = p.x + hole_step_radius;
				max_point_aabb_ex.y = ymax;
				max_point_aabb_ex.z = p.z + hole_step_radius;
				clt_box_tmp.push_back(min_point_aabb_ex);
				clt_box_tmp.push_back(max_point_aabb_ex);
			}

			viewer_cloud_cluster_box(in_cloud, target_root, clt_box_tmp, clt_line_idx, std::string("nms_box"));
		}
		// 根据得到的位置，直线检测，并过滤
		std::vector<pcl::PointXYZ>target_filtered;
		std::vector<std::vector<int>> target_member;//输出，茎上点index
		std::vector<pcl::PointXYZ>target_filtered_root;
		std::vector<pcl::ModelCoefficients::Ptr> target_filtered_models; //茎直线模型
		line_filter(in_cloud, hole_step_radius,ymin,ymax, target_root, target_filtered, 
			target_filtered_root, 
			target_member,
			target_filtered_models);

		if (m_cparam.image_show) {
			std::vector<pcl::PointXYZ> final_box;
			for (auto&pt : target_filtered_root) {
				//扩展矩形范围
				pcl::PointXYZ min_point_aabb_ex;
				pcl::PointXYZ max_point_aabb_ex;
				min_point_aabb_ex.x = pt.x - hole_step_radius;
				min_point_aabb_ex.y = ymin;				
				min_point_aabb_ex.z = pt.z - hole_step_radius;

				max_point_aabb_ex.x = pt.x + hole_step_radius;
				max_point_aabb_ex.y = ymax;
				max_point_aabb_ex.z = pt.z + hole_step_radius;
				final_box.push_back(min_point_aabb_ex);
				final_box.push_back(max_point_aabb_ex);
			}			
			viewer_cloud_cluster_box(in_cloud, target_filtered_root, final_box, target_member, std::string("line_filter"));
		}

		//sort cluster center, get the frontest leftest one
		//找最小z，然后计算平均z
		float min_root_z = 10000.0;
		for(auto&pt : target_filtered_root) {
			if (pt.z < min_root_z) { min_root_z = pt.z; }
		}
		//通过最小z，保守的找到和他一排的植株
		std::vector<int> first_row_index;
		float mean_z = 0.0;
		for (int idx_r = 0; idx_r < target_filtered_root.size();++idx_r) {
			pcl::PointXYZ&pt = target_filtered_root.at(idx_r);
			if (std::fabs(pt.z - min_root_z) < hole_step) { 
				first_row_index.push_back(idx_r); 
				mean_z += pt.z;
			}
		}
		//如果第一排的植株大于3，计算平均值
		float first_row_num = (float)first_row_index.size();
		mean_z /= first_row_num;
		if (first_row_num >= 4) {
			if (m_dtype == 0) {
				m_1st_row_zmean_sc = mean_z;
			}
			else{ m_1st_row_zmean_rs = mean_z; }
		}
		else {
			if( m_dtype == 0) {
				if (m_1st_row_zmean_sc > 0) { mean_z = m_1st_row_zmean_sc; }
			}
			else {
				if (m_1st_row_zmean_rs > 0) { mean_z = m_1st_row_zmean_rs; }
			}
		}
		//通过mean_z再次寻找第一排的植株
		first_row_index.clear();		
		for (int idx_r = 0; idx_r < target_filtered_root.size(); ++idx_r) {
			pcl::PointXYZ&pt = target_filtered_root.at(idx_r);
			if (std::fabs(pt.z - mean_z) < hole_step) {
				first_row_index.push_back(idx_r);				
			}
		}
		std::vector<float> cluster_index;
		for (int i=0; i<first_row_index.size();++i){
			int raw_idx = first_row_index.at(i);
			pcl::PointXYZ&pt = target_filtered_root.at(raw_idx);			
			cluster_index.push_back(pt.x);
		}
		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_idx = first_row_index.at(first_idx);

		first_seedling_cloud_idx.clear();
		for (auto&v : target_member.at(first_idx)) {
			first_seedling_cloud_idx.push_back(v);
		}
		xz_center.x = target_filtered_root.at(first_idx).x;
		xz_center.y = target_filtered_root.at(first_idx).y;
		xz_center.z = target_filtered_root.at(first_idx).z;
		line_model = target_filtered_models.at(first_idx);

		if (m_pLogger) {
			stringstream buff;
			buff << m_pcdId << ": euclidean_clustering_ttsas， find cluster center(" << xz_center.x
				<<", "<< xz_center.y<<", "<< xz_center.z<<")";
			m_pLogger->INFO(buff.str());
		}
	}
	void CRootStockGrabPoint::tilted_seedling_discover(
		std::vector<pcl::PointXYZ>&cluster_center,	//输入，已知的苗的坐标
		std::vector<pcl::PointXYZ>&tilted_center	//输出，有倾斜苗的坐标
	)
	{
		tilted_center.clear();
		if (cluster_center.size() == 0) { return; }
		//找到z最小的那一株，并找出和它一排的那些
		float step_harf = m_cparam.rs_grab_seedling_dist / 2.0;
		float xmin = m_cparam.rs_grab_xmin;		
		float xmax = m_cparam.rs_grab_xmax;
		float zmin = m_cparam.rs_grab_zmin;
		float zmax = m_cparam.rs_grab_zmax;
		if (m_dtype == 0) { 
			step_harf = m_cparam.sc_grab_seedling_dist / 2.0;
			xmin = m_cparam.sc_grab_xmin;
			xmax = m_cparam.sc_grab_xmax;
			zmin = m_cparam.sc_grab_zmin;
			zmax = m_cparam.sc_grab_zmax;
		}
		auto minz_it = std::min_element(cluster_center.begin(), cluster_center.end(),
			[](const pcl::PointXYZ& p1, const pcl::PointXYZ& p2)-> bool {return p1.z < p2.z; });
		float minz = minz_it->z;
		float meanz = 0.0;
		float cnt = 0.0;
		for (auto&p : cluster_center) {
			if (fabs(p.z - minz) < step_harf) { meanz += p.z; cnt += 1.0; }
		}
		if(cnt>0.0){meanz /= cnt;}
		else { meanz = minz; }
		// 生成所有可能的植株位置中心点
		std::vector<pcl::PointXYZ>candidate_centers;
		for (int row = -1; row <= 1; row += 1) {
			for (int col = -6; col <= 6; col += 1) {
				float cx = minz_it->x + col * 2.0 * step_harf;
				float cz = meanz + row * 2.0 * step_harf;
				if (cx < xmin || cx > xmax) { continue; }
				if (cz < zmin || cz > zmax) { continue; }
				// 邻域内已经存在植株的跳过
				bool with_seedling = false;
				for (auto&p : cluster_center) {
					float d = std::sqrt((p.x - cx) * (p.x - cx) + (p.z - cz) * (p.z - cz));
					if (d < 2.0*step_harf) {
						with_seedling=true; 
						break;
					}
				}
				if (!with_seedling) {//保存
					pcl::PointXYZ pc(cx,0.0,cz);
					candidate_centers.push_back(pc);
				}
			}
		}
		//针对每一个candidat_centers的邻域进行检查，检测是否有茎目标
		for (auto&pc : candidate_centers) {
		}
		
	}

	void CRootStockGrabPoint::find_seedling_position(
		pcl::PointCloud<pcl::PointXYZ>::Ptr in_cloud,
		std::vector<int> &first_seedling_cloud_idx,
		pcl::PointXYZ&xz_center
	)
	{
		// 确定植株inbox范围
		float hole_step = m_cparam.rs_grab_seedling_dist - 10.0; //穴盘中穴间距
		if (m_dtype == 0) {
			hole_step = m_cparam.sc_grab_seedling_dist - 10.0;
		}
		float hole_step_radius = hole_step / 2.0;

		// 点云降维到xz平面，y=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"));
		}		*/

		// 在xz平面内统计点的密度
		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));
		// 得到茎目标的点云序号，和对应的点云cloud2d_pos
		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 << m_pcdId <<": 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 << m_pcdId <<": 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> tilted_center;
		tilted_seedling_discover(cluster_center, tilted_center);

		//对每个类别进行检测，剔除不是茎的类别
		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 << m_pcdId <<": 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 << m_pcdId <<": 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 <<": 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 << m_pcdId <<": 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::get_optimal_seed_compare(
		pcl::PointCloud<pcl::PointXYZ>::Ptr in_cloud,		//input	全部有效点云
		pcl::PointCloud<pcl::PointXYZ>::Ptr in_line_cloud,	//input	茎上直线点云
		pcl::ModelCoefficients::Ptr line_model,				//input
		pcl::PointXYZ&pt,									//  返回抓取的点坐标
		int &pt_idx,										//	返回点index
		std::vector<int>& valid_line_index					//  返回有效直线点index
	)
	{
		valid_line_index.clear();
		float th_ratio = 1.0;		//原始点云和直线点云邻域尺寸增加原来的20%，不能当做抓取点
		pt_idx = -1;

		float ymin, ymax;
		ymin = m_cparam.rs_grab_ymin;
		ymax = m_cparam.rs_grab_ymax;
		if (m_dtype == 0) {
			ymin = m_cparam.sc_grab_ymin;
			ymax = m_cparam.sc_grab_ymax;
		}

		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>);
		std::vector<float> stem_width;
		std::vector<pcl::PointXYZ>online_points;

		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;
		//float dz,dx, dz_line, dx_line;
		//float dist_min = 10000.0;



		//for (size_t i = 0; i < in_line_cloud->points.size(); ++i) {
		cy = ymin;
		float t = (cy - line_model->values.at(1)) / line_model->values.at(4);
		while(cy<ymax){
			cx = line_model->values.at(3) * t + line_model->values.at(0);			
			cz = line_model->values.at(5) * t + line_model->values.at(2);
			//////////////////////////////////////////////////////////////////
			//原始点云，获取指定区域的dx，dz
			box_filter.setMin(Eigen::Vector4f(cx - rx*radius, cy - 2, cz - rz*radius, 1));
			box_filter.setMax(Eigen::Vector4f(cx + rx*radius, cy + 2, cz + rz*radius, 1));
			box_filter.filter(*cloud_inbox);

			if (cloud_inbox->points.size() > 10) {

				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);
				if (dx > dz) {
					stem_width.push_back(dx);
				}
				else {
					stem_width.push_back(dz);
				}
				
			}
			else {
				stem_width.push_back(0);
			}
			pcl::PointXYZ tmp_pt(cx,cy,cz);
			online_points.push_back(tmp_pt);

			cy += 1.0;
		}
		std::vector<float>valid_stem_width;
		for (auto&w : stem_width) { valid_stem_width.push_back(w); }
		float mu = get_hist_mean(valid_stem_width);
		float stdv = get_hist_std(valid_stem_width, mu);

		int max_pos = std::max_element(stem_width.begin(), stem_width.end()) - stem_width.begin();
		float max_val = stem_width.at(max_pos);
		float th = mu + th_ratio * stdv;
		if (th < max_val) {
			int i = 0;
			for (; i < stem_width.size(); ++i) {
				if (stem_width.at(i) >= th) {
					break;
				}
			}
			max_pos = i;
		}

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

		pt.x = x_mu;
		pt.y = cy;
		pt.z = z_mu;

		pt_idx = max_pos;	
		
	}
	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)));
			if (clt_line_idx.size()>i && clt_line_idx.at(i).size() > 0) {
				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));
		}

	}

};