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 <pcl\segmentation\extract_clusters.h>
#include <math.h>
#include <pcl\features\normal_3d.h>
#include <pcl\features\boundary.h>

#include "grab_point_rs.h"
#include "utils.h"
#include "peak_finder.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),
		m_cloud_mean_dist(0.0),
		m_pImginfoResult(0),
		m_pStemInfos(0)
	{		
	}
	void CRootStockGrabPoint::clear_imginfo() {
		if (m_pImginfoResult) {
			imginfo_release(&m_pImginfoResult);
			m_pImginfoResult = 0;
		}
		
	}

	CRootStockGrabPoint::~CRootStockGrabPoint() {
		this->clear_imginfo();
		if (m_pStemInfos) {
			delete m_pStemInfos;
			m_pStemInfos = 0;
		}
	}
	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*/)
	{
		//数据加载功能实现，并生成imageid，保存原始数据到文件
		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) {
			pcl::visualization::PCLVisualizer viewer(m_pcdId + std::string(": raw point cloud"));
			viewer.setBackgroundColor(0.35, 0.35, 0.35);
			viewer.addCoordinateSystem();
			viewer.addPointCloud<pcl::PointXYZ>(m_raw_cloud, "raw_cloud");
			viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 1, 1, 1, "raw_cloud");
			viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "raw_cloud");

			float xmin, ymin, zmin, xmax, ymax, zmax;
			xmin = m_cparam.rs_grab_xmin;
			ymin = m_cparam.rs_grab_ymin; 
			zmin = m_cparam.rs_grab_zmin; 
			xmax = m_cparam.rs_grab_xmax; 
			ymax = m_cparam.rs_grab_ymax; 
			zmax = m_cparam.rs_grab_zmax;
			if (m_dtype == 0) {
				xmin = m_cparam.sc_grab_xmin;
				ymin = m_cparam.sc_grab_ymin;
				zmin = m_cparam.sc_grab_zmin;
				xmax = m_cparam.sc_grab_xmax;
				ymax = m_cparam.sc_grab_ymax;
				zmax = m_cparam.sc_grab_zmax;
			}

			viewer.addCube(xmin, xmax,ymin, ymax,  zmin, zmax, 0.75, 0.0, 0.0, "AABB_");
			viewer.setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_REPRESENTATION,
				pcl::visualization::PCL_VISUALIZER_REPRESENTATION_WIREFRAME, "AABB_");

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

		if (m_pStemInfos==0) {
			double seedling_distance = m_cparam.rs_grab_seedling_dist;
			int holes_number = m_cparam.rs_grab_holes_number;
			double x_min = m_cparam.rs_grab_xmin;
			double x_max = m_cparam.rs_grab_xmax;
			double z_min = m_cparam.rs_grab_zmin;
			double z_max = m_cparam.rs_grab_zmax;
			if (m_dtype == 0) {
				seedling_distance = m_cparam.sc_grab_seedling_dist;
				holes_number = m_cparam.sc_grab_holes_number;
				x_min = m_cparam.sc_grab_xmin;
				x_max = m_cparam.sc_grab_xmax;
				z_min = m_cparam.sc_grab_zmin;
				z_max = m_cparam.sc_grab_zmax;
			}
			m_pStemInfos = new CStemResultInfos(
				seedling_distance, holes_number,
				x_min, x_max, z_min, z_max,
				m_pcdId, m_pLogger);
		}
		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<< "(if < 50, return)";
			m_pLogger->INFO(buff.str());
		}
		if (cloud_ror->width * cloud_ror->height < 50) {
			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");
		}

		////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
		//?? 用截取到的点云，初步找到植株的位置中心
		m_root_centers.clear();
		m_root_center_with_seedling.clear();
		if (m_pStemInfos) {			
			m_pStemInfos->get_root_centers(m_root_centers);
			if (m_pLogger) {
				stringstream buff;
				buff << m_pcdId << ": root positions "<< m_root_centers.size()<<" centers\n";
				for (auto& sr : m_root_centers) {
					buff << "\tx=" << sr.root_x << ", y=" << sr.root_y << ", z=" << sr.root_z
						<< ", cnt=" << sr.root_count
						<< ", pcd_size=" << sr.stem_size << "\n";
				}
				m_pLogger->INFO(buff.str());
			}
		}
		
		////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
		//3 原来的降采样没有意义，改成统计平均距离
		m_cloud_mean_dist = 0.0;
		cloud_mean_dist(cloud_ror, m_cloud_mean_dist);
		if (m_pLogger) {
			stringstream buff;
			buff << m_pcdId <<": cloud_mean_dist = " << m_cloud_mean_dist;
			m_pLogger->INFO(buff.str());
		}
		
		////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
		// 4 对截取的点云进行ror滤除大面积联通区域，剔除叶片
		pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_dowm_sampled(new pcl::PointCloud<pcl::PointXYZ>);
		std::vector<int> non_leaf_indices;
		std::vector<int> leaf_indices;
		leaf_filter_ror(cloud_ror, non_leaf_indices, leaf_indices);
		pcl::copyPointCloud(*cloud_ror, non_leaf_indices, *cloud_dowm_sampled);
		if (m_pLogger) {
			stringstream buff;
			buff << m_pcdId << ": after leaf_filter 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;
		}

		////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
		//判断m_root_centers位置上是否有叶片遮挡
		occluded_seedling_detect_by_leaf(cloud_ror, leaf_indices);
		

		////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
		//5 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");
		}

		int first_row_seedling_number = 0;
		bool fund_seedling = find_seedling_position_key(cloud_dowm_sampled, first_seedling_cloud_idx, xz_center, line_model, first_row_seedling_number);
		if (!fund_seedling && first_row_seedling_number == 0) {
			if (m_pLogger) {
				stringstream buff;
				buff << m_pcdId << ": Not found seedlings" ;
				m_pLogger->INFO(buff.str());
			}
			return 1;
		}
		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());		
			m_pLogger->INFO(m_pcdId + ": end find seedling position");
		}

		////////////////////////////////////////////////////////////////////////////////////////////////////////////////
		//5.1 如果没有找到茎，在root_center 中找一个位置
		pcl::PointXYZ selected_pt;//抓取点坐标，根据茎节点的偏移
		pcl::PointXYZ selected_pt_ref;	//返回茎节点，作为抓取点偏的基点		
		if (!fund_seedling) {
			int selected_center_idx;
			no_seedling_detected_post_process(
				first_row_seedling_number,		//input
				selected_center_idx,			//output
				selected_pt,					//output
				selected_pt_ref,				//output
				posinfo);
			if (selected_center_idx < 0) {
				if (m_pLogger) {
					stringstream buff;
					buff << m_pcdId << ": Not found seedlings after no_seedling_detected_post_process()";
					m_pLogger->INFO(buff.str());
				}
				return 1;
			}
		}
		else {
			////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
			//6 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的宽度
			//对比两个宽度，变化很大的，说明周边有异物（叶柄或叶子），不适合作为抓取位置；否则就是抓取位置
			//在众多抓取位置上，选择离指定高度最近的点作为抓取位置
			// 
			//显示位置
			if (m_cparam.image_show) {
				std::vector<pcl::PointXYZ> tmp;
				tmp.push_back(xz_center);
				pcl::PointCloud<pcl::PointXYZRGB>::Ptr tttp(new pcl::PointCloud<pcl::PointXYZRGB>);
				pcl::copyPointCloud(*cloud_seedling_seed, *tttp);

				for (auto& pt : *tttp) {
					pt.r = 255;
					pt.g = 255;
					pt.b = 255;
				}
				viewer_cloud_cluster(tttp, tmp, string("first"));
			}
			
			float stem_width_mu;
			float stem_deflection;
			get_optimal_seed_compare(
				cloud_dowm_sampled,		//input
				cloud_seedling_seed,	//input
				line_model,				//input
				selected_pt,			//output
				selected_pt_ref,		//output				
				stem_width_mu,			//output
				stem_deflection			//output
			);		//output

			if (selected_pt.z < 0) {
				int selected_center_idx;
				no_seedling_detected_post_process(
					first_row_seedling_number,		//input
					selected_center_idx,			//output
					selected_pt,					//output
					selected_pt_ref,				//output
					posinfo);

				if (selected_center_idx < 0) {
					if (m_pLogger) {
						stringstream buff;
						buff << m_pcdId << ": Not found valid fork point after no_seedling_detected_post_process()";
						m_pLogger->INFO(buff.str());
					}
					return 1;
				}
			}
			else {
				int selected_center_idx;
				had_seedling_detected_post_process(
					first_row_seedling_number,		//input
					stem_width_mu,				//input
					stem_deflection,				//input
					selected_center_idx,					//output, 选择root_center的序号
					selected_pt,			//input-output, 检测到的目标抓取点
					selected_pt_ref,		//input-output, 检测到的目标抓取参考点
					posinfo);
			}
			
			if (m_pLogger) {
				m_pLogger->INFO(m_pcdId + ": end get_optimal_seed");
			}
		}
		////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
		//7 保存处理结果到图片
		cv::Mat rst_img = cv::Mat::zeros(cv::Size(1280,640),CV_8UC3);
		gen_result_img(cloud_ror, cloud_dowm_sampled, selected_pt, selected_pt_ref, posinfo,rst_img);
		if (m_ppImgSaver && *m_ppImgSaver) {
			(*m_ppImgSaver)->saveImage(rst_img, m_pcdId + "_rst_0");
		}
		////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
		//8 页面显示结果
		this->clear_imginfo();
		m_pImginfoResult = mat2imginfo(rst_img);
		posinfo.pp_images[0] = m_pImginfoResult;

		////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
		//9 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;
			}			
			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, selected_pt_ref, xz_center, std::string("cloud_cand_demo"));
			imshow_wait("rst_img", rst_img);
		}
		return 0;
	}
	//没有检测到苗的后处理
	void CRootStockGrabPoint::no_seedling_detected_post_process(
		int first_row_seedling_number,		//input
		int& selected_idx,					//output, 选择root_center的序号
		pcl::PointXYZ& selected_pt,			//output
		pcl::PointXYZ& selected_pt_ref,		//output
		PositionInfo& posinfo				//output
	)
	{
		//m_dtype == 0 找最大x位置， 否则找最小x位置
		selected_idx = -1;
		if (m_dtype == 0) {
			for (int i = 0; i < m_root_center_with_seedling.size(); ++i) {
				if (m_root_center_with_seedling[i]) { selected_idx = i; }
			}
		}
		else {
			for (int i = 0; i < m_root_center_with_seedling.size(); ++i) {
				if (m_root_center_with_seedling[i]) {
					selected_idx = i;
					break;
				}
			}
		}

		if (selected_idx < 0) {
			return;
		}

		double grab_fork_ybt = m_cparam.rs_grab_fork_ybt;
		double grab_offset = m_cparam.rs_grab_offset;
		if (m_dtype == 0) {
			grab_fork_ybt = m_cparam.sc_grab_fork_ybt;
			grab_offset = m_cparam.sc_grab_offset;
		}
		
		selected_pt_ref.x = m_root_centers.at(selected_idx).root_x;
		selected_pt_ref.y = grab_fork_ybt;
		selected_pt_ref.z = m_root_centers.at(selected_idx).root_z;
		selected_pt = selected_pt_ref;
		selected_pt.y += static_cast<int>(grab_offset);

		double stem_width_mu = 0.0;
		double stem_deflection = 0.0;
		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;
			posinfo.sc_count = (double)first_row_seedling_number;
			posinfo.sc_width = (double)stem_width_mu;
			posinfo.sc_tortuosity = (double)stem_deflection;
		}
		else {
			posinfo.rs_grab_x = selected_pt.x;
			posinfo.rs_grab_y = selected_pt.y;
			posinfo.rs_grab_z = selected_pt.z;
			posinfo.rs_count = (double)first_row_seedling_number;
			posinfo.rs_width = (double)stem_width_mu;
			posinfo.rs_tortuosity = (double)stem_deflection;
		}

	}

	//检测到苗的情况，后处理
	void CRootStockGrabPoint::had_seedling_detected_post_process(
		int first_row_seedling_number,		//input
		float stem_width_mu,				//input
	    float stem_deflection,				//input
		int& selected_idx,					//output, 选择root_center的序号
		pcl::PointXYZ& selected_pt,			//input-output, 检测到的目标抓取点
		pcl::PointXYZ& selected_pt_ref,		//input-output, 检测到的目标抓取参考点
		PositionInfo& posinfo				//output
	)
	{
		//通过selected_pt找到对应的gen中心位置

		//0 首选识别到的位置
		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;
			posinfo.sc_count = (double)first_row_seedling_number;
			posinfo.sc_width = (double)stem_width_mu;
			posinfo.sc_tortuosity = (double)stem_deflection;
		}
		else {
			posinfo.rs_grab_x = selected_pt.x;
			posinfo.rs_grab_y = selected_pt.y;
			posinfo.rs_grab_z = selected_pt.z;
			posinfo.rs_count = (double)first_row_seedling_number;
			posinfo.rs_width = (double)stem_width_mu;
			posinfo.rs_tortuosity = (double)stem_deflection;
		}

		// 1 找到基于rootcenter应该找的中心位置
		//m_dtype == 0 找最大x位置， 否则找最小x位置
		selected_idx = -1;
		if (m_dtype == 0) {
			for (int i = 0; i < m_root_center_with_seedling.size(); ++i) {
				if (m_root_center_with_seedling[i]) { selected_idx = i; }
			}
		}
		else {
			for (int i = 0; i < m_root_center_with_seedling.size(); ++i) {
				if (m_root_center_with_seedling[i]) {
					selected_idx = i;
					break;
				}
			}
		}

		if (selected_idx < 0) {
			//可能没有中心（刚开始工作）
			//按识别的结果			
			return;
		}

		//2 如果识别的位置和rootcenter位置接近，就按识别位置
		double seedling_distance = m_cparam.rs_grab_seedling_dist;		
		if (m_dtype == 0) {
			seedling_distance = m_cparam.sc_grab_seedling_dist;				
		}
		double root_x = m_root_centers.at(selected_idx).root_x;
		if (m_dtype == 0) {
			//穗苗找x最大的位置
			if (selected_pt.x > (root_x - 0.5 * seedling_distance)) {
				//找到x最大的位置上的苗，识别的苗位置大于有苗的位置，就认为识别结果更可信
				return;
			}
			else {
				//否则用指定根中心的位置
				goto obstructed;
			}
		}
		else {
			//砧木，找x最小的位置
			if (selected_pt.x < (root_x + 0.5 * seedling_distance)) {
				//找到x最大的位置上的苗，识别的苗位置大于有苗的位置，就认为识别结果更可信
				return;
			}
			else {
				//否则用指定根中心的位置
				goto obstructed;
			}
		}

obstructed:
		double grab_fork_ybt = m_cparam.rs_grab_fork_ybt;
		double grab_offset = m_cparam.rs_grab_offset;
		if (m_dtype == 0) {
			grab_fork_ybt = m_cparam.sc_grab_fork_ybt;
			grab_offset = m_cparam.sc_grab_offset;
		}

		selected_pt_ref.x = m_root_centers.at(selected_idx).root_x;
		selected_pt_ref.y = grab_fork_ybt;
		selected_pt_ref.z = m_root_centers.at(selected_idx).root_z;
		selected_pt = selected_pt_ref;
		selected_pt.y += static_cast<int>(grab_offset);

		double stem_width_mu_obstructed = 0.0;
		double stem_deflection_obstructed = 0.0;
		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;
			posinfo.sc_count = (double)first_row_seedling_number;
			posinfo.sc_width = stem_width_mu_obstructed;
			posinfo.sc_tortuosity = stem_deflection_obstructed;
		}
		else {
			posinfo.rs_grab_x = selected_pt.x;
			posinfo.rs_grab_y = selected_pt.y;
			posinfo.rs_grab_z = selected_pt.z;
			posinfo.rs_count = (double)first_row_seedling_number;
			posinfo.rs_width = stem_width_mu_obstructed;
			posinfo.rs_tortuosity = stem_deflection_obstructed;
		}

	}
	//根据历史根的位置，计算对应位置点云数量，进而判断此位置是否有苗
	void CRootStockGrabPoint::occluded_seedling_detect_by_leaf(
		pcl::PointCloud<pcl::PointXYZ>::Ptr in_cloud,	//input 输入点云数据			
		std::vector<int>& leaf_idx
	)
	{		
		m_root_center_with_seedling.clear();
		m_root_center_with_seedling.assign(m_root_centers.size(), false);
		std::vector<int> pcd_cnt;
		//int th_pcd_size = m_cparam.rs_grab_seedling_min_pts;
		double stem_diameter = m_cparam.rs_grab_stem_diameter;
		double y_min = m_cparam.rs_grab_ymin;
		double y_max = m_cparam.rs_grab_ymax;

		if (m_dtype == 0) {
			//th_pcd_size = m_cparam.sc_grab_seedling_min_pts;
			stem_diameter = m_cparam.sc_grab_stem_diameter;
			y_min = m_cparam.sc_grab_ymin;
			y_max = m_cparam.sc_grab_ymax;
		}
		int th_pcd_size = stem_diameter * (y_max - y_min) * m_cloud_mean_dist;
		std::vector<pcl::PointXYZ> aabb_mins_maxs;		
		for (int i = 0; i < m_root_centers.size(); ++i) {
			CStemResult& rc = m_root_centers.at(i);
			pcl::PointXYZ aabb_min;
			pcl::PointXYZ aabb_max;
			int cnt = get_point_count_inbox(rc, aabb_min, aabb_max, in_cloud, leaf_idx);
			pcd_cnt.push_back(cnt);			
			bool has_seedling = cnt > th_pcd_size;
			m_root_center_with_seedling.at(i) = has_seedling;
			aabb_mins_maxs.push_back(aabb_min);
			aabb_mins_maxs.push_back(aabb_max);			
		}

		if (m_pLogger) {
			stringstream buff;
			buff << m_pcdId << ": root positions points size " << m_root_centers.size() << " centers\n";
			for (int i = 0; i < m_root_centers.size();++i) {
				CStemResult& sr = m_root_centers.at(i);

				buff << "\tx=" << sr.root_x << ", z=" << sr.root_z					
					<< ", stem_pcd_size=" << sr.stem_size 
					<< ", raw_pcd_size=" << pcd_cnt.at(i) << "\n";
			}
			m_pLogger->INFO(buff.str());
		}

		// 显示			
		if (m_cparam.image_show) {
			std::vector<std::vector<int> > clt_line_idx;			
			std::vector<pcl::PointXYZ>target_root;
			double ymin = m_cparam.rs_grab_ymin;
			if (m_dtype == 0) { ymin = m_cparam.sc_grab_ymin; }
			for (auto&rc : m_root_centers) {			
				pcl::PointXYZ p(rc.root_x, ymin, rc.root_z);
				target_root.push_back(p);				
			}

			viewer_cloud_cluster_box(m_raw_cloud, target_root, aabb_mins_maxs, clt_line_idx, std::string("seedling root centers"));
		}		

	}
	//int CRootStockGrabPoint::get_point_count_inbox(const CStemResult& sr, 
	//	pcl::PointXYZ& aabb_min,
	//	pcl::PointXYZ& aabb_max)
	//{
	//	double seedling_distance = m_cparam.rs_grab_seedling_dist;
	//	double min_y = m_cparam.rs_grab_ymin;
	//	if (m_dtype == 0) {
	//		seedling_distance = m_cparam.sc_grab_seedling_dist;
	//		min_y = m_cparam.sc_grab_ymin;
	//	}
	//	double min_x = sr.root_x - 0.5 * seedling_distance;
	//	double max_x = sr.root_x + 0.5 * seedling_distance;
	//	double min_z = sr.root_z - 0.75 * seedling_distance;
	//	double max_z = sr.root_z + 0.25 * seedling_distance;
	//	double max_y = min_y + 150.0; //假定苗高150mm

	//	int count = 0;
	//	for (auto&pt : m_raw_cloud->points) {
	//		if(pt.y >= min_y && pt.y <= max_y &&
	//			pt.x >=min_x && pt.x<=max_x &&
	//			pt.z >= min_z && pt.z <= max_z)
	//		{ 
	//			count++;
	//		}
	//	}
	//	aabb_min.x = min_x;
	//	aabb_min.y = min_y;
	//	aabb_min.z = min_z;
	//	aabb_max.x = max_x;
	//	aabb_max.y = max_y;
	//	aabb_max.z = max_z;

	//	return count;
	//}

	int CRootStockGrabPoint::get_point_count_inbox(const CStemResult& sr,
		pcl::PointXYZ& aabb_min,
		pcl::PointXYZ& aabb_max,
		pcl::PointCloud<pcl::PointXYZ>::Ptr in_cloud,	//input 输入点云数据			
		std::vector<int>& leaf_idx)
	{
		double seedling_distance = m_cparam.rs_grab_seedling_dist;
		double min_y = m_cparam.rs_grab_ymin;
		double max_y = m_cparam.rs_grab_ymax;
		if (m_dtype == 0) {
			seedling_distance = m_cparam.sc_grab_seedling_dist;
			min_y = m_cparam.sc_grab_ymin;
			max_y = m_cparam.sc_grab_ymax;
		}
		double min_x = sr.root_x - 0.5 * seedling_distance;
		double max_x = sr.root_x + 0.5 * seedling_distance;
		double min_z = sr.root_z - 0.75 * seedling_distance;
		double max_z = sr.root_z + 0.25 * seedling_distance;		

		int count = 0;
		//for (auto&pt : m_raw_cloud->points) {
		for (auto&i : leaf_idx) {
			pcl::PointXYZ& pt = in_cloud->points.at(i);
			if (pt.y >= min_y && pt.y <= max_y &&
				pt.x >= min_x && pt.x <= max_x &&
				pt.z >= min_z && pt.z <= max_z)
			{
				count++;
			}
		}
		aabb_min.x = min_x;
		aabb_min.y = min_y;
		aabb_min.z = min_z;
		aabb_max.x = max_x;
		aabb_max.y = max_y;
		aabb_max.z = max_z;

		return count;
	}

	//生成结果图片
	void CRootStockGrabPoint::gen_result_img(
		pcl::PointCloud<pcl::PointXYZ>::Ptr in_cloud_raw,//输入，未经过滤的整体点云in_cloud_raw, 
		pcl::PointCloud<pcl::PointXYZ>::Ptr in_cloud,	//输入，整体点云cloud_dowm_sampled, 
		pcl::PointXYZ& selected_pt,				//输入，selected_pt, 
		pcl::PointXYZ& selected_pt_ref,        //输入，selected_pt_ref, 
		const PositionInfo& posinfo,					//输入，
		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,128,128));
		cv::line(rst_img, cv::Point(0, cy), cv::Point(arrow_len, cy - int(arrow_len/2)), cv::Scalar(128, 128, 128));
		cv::line(rst_img, cv::Point(0, cy), cv::Point(arrow_len, cy + int(arrow_len/2)), cv::Scalar(128, 128, 128));

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

		cv::putText(rst_img, std::string("x"), cv::Point(20, cy-10), cv::FONT_HERSHEY_SIMPLEX, 1, cv::Scalar(128, 128, 128));
		cv::putText(rst_img, std::string("y"), cv::Point(cx+10, 20), cv::FONT_HERSHEY_SIMPLEX, 1, cv::Scalar(128, 128, 128));
		
		//绘制所有点
		int x, y;
		for (auto&pt : in_cloud_raw->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<cv::Vec3b>(y, x)[0] = 60; 
			rst_img.at<cv::Vec3b>(y, x)[1] = 20;
			rst_img.at<cv::Vec3b>(y, x)[2] = 220;
		}

		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<cv::Vec3b>(y, x)[0] = 0;
			rst_img.at<cv::Vec3b>(y, x)[1] = 225; 
			rst_img.at<cv::Vec3b>(y, x)[2] = 0;
		}
		//绘制抓取点坐标
		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(0, 215, 255));
		cv::line(rst_img, cv::Point(grab_x - radius, grab_y + radius), cv::Point(grab_x + radius, grab_y - radius), cv::Scalar(0, 215, 255));
		//如果是遮挡，画一个圆
		if ((m_dtype == 0 && posinfo.sc_width == 0) || (m_dtype == 1 && posinfo.rs_width == 0)) {
			cv::circle(rst_img, cv::Point(grab_x, grab_y), radius, cv::Scalar(0, 215, 255));
		}


		//绘制茎节点坐标
		int fork_x = cx - int(selected_pt_ref.x / res + 0.5);
		int fork_y = cy - int(selected_pt_ref.y / res + 0.5);
		radius = 15;
		cv::line(rst_img, cv::Point(fork_x - radius, fork_y), cv::Point(fork_x + radius, fork_y), cv::Scalar(153, 51, 255));

		//在图片中写入文件名字
		cv::putText(rst_img, m_pcdId, cv::Point(20, 20), cv::FONT_HERSHEY_SIMPLEX, 0.5, cv::Scalar(128, 128, 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::leaf_filter(
	pcl::PointCloud<pcl::PointXYZ>::Ptr in_cloud,	//输入，降采样的点云
	std::vector<int> &stem_cloud_idx				//输出，除了叶子的点云序号
) 
{
	//采用欧式距离聚类，对每一类的点云进行分析，剔除叶子
	stem_cloud_idx.clear();

	std::vector<pcl::PointIndices> cluster_indices;
	pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>);
	tree->setInputCloud(in_cloud);
	

	pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;
	ec.setClusterTolerance(2.5);
	ec.setMinClusterSize(30);
	ec.setMaxClusterSize(20000);
	ec.setSearchMethod(tree);
	ec.setInputCloud(in_cloud);
	ec.extract(cluster_indices);
	
	for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin();
		it != cluster_indices.end(); ++it) {
		pcl::PointCloud<pcl::PointXYZ>::Ptr cluster_cloud(new pcl::PointCloud<pcl::PointXYZ>);
		pcl::copyPointCloud(*in_cloud, *it, *cluster_cloud);		

		//计算点云的pca
		Eigen::Vector4f pcaCenteroid;
		pcl::compute3DCentroid(*cluster_cloud, pcaCenteroid);
		Eigen::Matrix3f covariance;
		pcl::computeCovarianceMatrix(*cluster_cloud, pcaCenteroid, covariance);
		Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> eigen_solver(covariance, Eigen::ComputeEigenvectors);
		Eigen::Vector3f eigenValuesPCA = eigen_solver.eigenvalues();		
		float e1, e2, e3;
		e1 = std::sqrt(eigenValuesPCA(2));
		e2 = std::sqrt(eigenValuesPCA(1));
		e3 = std::sqrt(eigenValuesPCA(0));

		float a1d = (e1 - e2) / e1;
		float a2d = (e2 - e3) / e1;
		float a3d = e3 / e1;

		if (m_pLogger) {
			stringstream buff;
			buff << m_pcdId << ": ads=(" << a1d << "," << a2d << "," << a3d << "), pca_eigem_values=(" << e1
				<< "," << e2 << "," << e3 << "), cluster_size=" << it->indices.size();
			m_pLogger->INFO(buff.str());		
		}	
		if (m_cparam.image_show) {
			if (a1d < 0.75) {
				viewer_cloud(cluster_cloud, string("cluster_leaf"));
			}
			else {
				viewer_cloud(cluster_cloud, string("cluster_stem"));
			}
		}
		/*
		特征值归一化处理：   a1d = (e1 - e2) / e1 
		                    a2d = (e2 - e3) / e1 
							a3d = e3 / e1 
							
							ei = sqrt(lamda_i)
							
							当点云线状分布时，a1d = 1, a2d = 0, a3d = 0
							当点云面状分布时，a1d = 0, a2d = 1, a3d = 0
							当点云球状分布时，a1d = 0, a2d = 0, a3d = 1
		*/

		if (a1d < 0.75) { continue; }
		for (int idx : it->indices) {
			stem_cloud_idx.push_back(idx);
		}
	}
}

// 基于孤立点滤除的方法检测叶子区域
void CRootStockGrabPoint::leaf_filter_ror(
	pcl::PointCloud<pcl::PointXYZ>::Ptr in_cloud,	//input 输入点云数据
	std::vector<int> &stem_cloud_idx,				//output, 过滤后的点云序号
	std::vector<int>& leaf_idx						//output, 叶子的点云序号
)
{
	//计算点云平均间距
	double mean_dist = 0.0;
	cloud_mean_dist(in_cloud, mean_dist);

	//计算点云过滤半径和点数阈值
	double stem_diameter = m_cparam.rs_grab_stem_diameter;
	if (m_dtype == 0) { stem_diameter = m_cparam.sc_grab_stem_diameter; }

	double remove_radius = stem_diameter;
	double ror_ratio = m_cparam.rs_grab_ror_ratio;
	if (m_dtype == 0){ror_ratio = m_cparam.sc_grab_ror_ratio;}
	int nb_points = ror_ratio*  stem_diameter * stem_diameter * 2.0 / mean_dist / mean_dist; // (2d*d + pi *d*d) /2
	if (m_pLogger) {
		stringstream buff;
		buff << m_pcdId << ": ror_ratio=" << ror_ratio << ", ror filter nb_points=" << nb_points;
		m_pLogger->INFO(buff.str());
	}

	//获取叶片的点云
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_leaf(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::RadiusOutlierRemoval<pcl::PointXYZ> ror(true);
	ror.setInputCloud(in_cloud);
	ror.setRadiusSearch(remove_radius);
	ror.setMinNeighborsInRadius(nb_points);
	ror.filter(*cloud_leaf);

	//通过叶片点云寻找附近remove_radius内的近邻点，当做叶子点云序号	
	std::set<int> leaf_idx_set;
	int nres;
	std::vector<int> indices;
	std::vector<float> sqr_distances;
	pcl::search::KdTree<pcl::PointXYZ> tree;
	tree.setInputCloud(in_cloud);
	for (size_t i = 0; i < cloud_leaf->size(); ++i)
	{		
		nres = tree.radiusSearch(cloud_leaf->points.at(i), remove_radius, indices, sqr_distances);		
		for (int& idx : indices) {
			leaf_idx_set.insert(idx);
		}
	}

	leaf_idx.clear();
	leaf_idx.assign(leaf_idx_set.begin(), leaf_idx_set.end());	

	//in_img是经过开运算的图像，其中像素位置的点云为叶子区域
	std::vector<int>::iterator lit;
	for (int i = 0; i < in_cloud->points.size(); ++i) {
		lit = std::find(leaf_idx.begin(), leaf_idx.end(), i);
		if (lit == leaf_idx.end()) {
			stem_cloud_idx.push_back(i);
		}
	}

	//显示开运算的结果
	if (m_cparam.image_show) {
		pcl::PointCloud<pcl::PointXYZ>::Ptr stem_cloud(new pcl::PointCloud<pcl::PointXYZ>);
		pcl::PointCloud<pcl::PointXYZ>::Ptr leaf_cloud(new pcl::PointCloud < pcl::PointXYZ>);
		pcl::copyPointCloud(*in_cloud, stem_cloud_idx, *stem_cloud);
		pcl::copyPointCloud(*in_cloud, leaf_idx, *leaf_cloud);

		pcl::visualization::PCLVisualizer viewer("open cloud");
		viewer.addCoordinateSystem();
		viewer.addPointCloud<pcl::PointXYZ>(stem_cloud, "stem_cloud");//????????????????
		viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 1, 1, 1, "stem_cloud");
		viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "stem_cloud");

		viewer.addPointCloud<pcl::PointXYZ>(leaf_cloud, "leaf_cloud");
		viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 1, 0, 0, "leaf_cloud");
		viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "leaf_cloud");

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

	}
}
void CRootStockGrabPoint::cloud_mean_dist(
	pcl::PointCloud<pcl::PointXYZ>::Ptr in_cloud,	//input 输入点云数据
	double& mean_dist
)
{	
	mean_dist = 0.0;
	int n_points = 0;
	int nres;
	std::vector<int> indices(2);
	std::vector<float> sqr_distances(2);
	pcl::search::KdTree<pcl::PointXYZ> tree;
	tree.setInputCloud(in_cloud);
	for (size_t i = 0; i < in_cloud->size(); ++i)
	{		
		//Considering the second neighbor since the first is the point itself.
		nres = tree.nearestKSearch(i, 2, indices, sqr_distances);
		if (nres == 2)
		{
			mean_dist +=std::sqrt(sqr_distances[1]);
			++n_points;
		}
	}
	if (n_points != 0)
	{
		mean_dist /= n_points;
	}
}

//////////////////////////////////////////////////////////////////////////////////////////

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方向分布范围滤波
		float y_length_th = 10.0;
		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<y_length_th && dy / (ymax - ymin) < 0.25) { 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);

		//有效茎长计算
		std::vector<int> stem_y_count_hist;
		get_line_project_hist(stem_cloud, coefficients, stem_y_count_hist);		
		std::vector<int> stem_y_count_hist_valid;
		for (auto&v : stem_y_count_hist) {
			if (v > 2) {
				stem_y_count_hist_valid.push_back(v);
			} 
		}
		double valid_mu = get_hist_mean(stem_y_count_hist_valid);
		int cnt_th = static_cast<int>(valid_mu/2.0);
		if (cnt_th < 3) { cnt_th = 2; }

		float valid_length = 0.0;
		
		std::vector<int> stem_y_mask;

		for (auto& c : stem_y_count_hist) {
			if (c > cnt_th) { 
				valid_length += 1.0; 
				stem_y_mask.push_back(1);
			}
			else {
				stem_y_mask.push_back(0);
			}
		}
		//填补空
		for (int i = 1; i < stem_y_mask.size() - 1; ++i) {
			if (stem_y_mask.at(i) == 0 && stem_y_mask.at(i - 1) == 1 && stem_y_mask.at(i + 1) == 1) {
				stem_y_mask.at(i) = 1;
			}
		}
		//找出最长的连续段的长度
		int longest_segment = 0;
		int start = 0;
		for (int i = 0; i < stem_y_mask.size(); ++i) {
			if (i == 0 ) {
				if (stem_y_mask.at(i) == 1) {
					start = i;
				}				
				continue;
			}
			
			
			if (i == stem_y_mask.size() - 1 ) {
				if (stem_y_mask.at(i) == 1) {
					int length = i - start + 1;
					if (length > longest_segment) { longest_segment = length; }
				}				
				continue;
			}
			if (stem_y_mask.at(i - 1) == 0 && stem_y_mask.at(i) == 1) {
				start = i;
				continue;
			}
			if (stem_y_mask.at(i - 1) == 1 && stem_y_mask.at(i) == 0) {
				int length = i - start;
				if (length > longest_segment) { longest_segment = length; }
				continue;
			}
			
		}

		if (longest_segment<10.0 && valid_length / (ymax - ymin) < 0.35) { continue; }


		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);
			}
		}*/
	}
}


	bool 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,
		int& first_row_size
	)
	{	
		first_row_size = 0;
		// 确定植株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);	
		// 显示生成的每一个候选框				
		/*if (m_cparam.image_show) {
			std::vector<std::vector<int> > clt_line_idx;
			std::vector<pcl::PointXYZ> clt_root_v;
			std::vector<pcl::PointXYZ> clt_box_v;
			for (auto&p : clt_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_v.push_back(min_point_aabb_ex);
				clt_box_v.push_back(max_point_aabb_ex);
			}
			viewer_cloud_cluster_box(in_cloud, clt_root_v, clt_box_v, clt_line_idx, std::string("candidate_box"));
		}*/

		// 每个位置点云情况，过滤		
		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 (target_filtered_root.size() == 0) {
			if (m_pLogger) {
				stringstream buff;
				buff << m_pcdId << ": line_filter reuslt cluster is empty" ;
				m_pLogger->INFO(buff.str());
			}
			return false;
		}

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

		//保存茎的根部坐标和点云数量
		if (m_pStemInfos) {
			for (size_t k = 0; k < target_filtered_root.size(); ++k) {
				CStemResult sr = CStemResult(target_filtered_root.at(k).x,
					target_filtered_root.at(k).y,
					target_filtered_root.at(k).z,
					target_member.at(k).size(),
					m_pcdId,
					1);
				m_pStemInfos->append(sr);
			}
		}
		
		//sort cluster center, get the frontest leftest one
		//找最小z，然后计算平均z
		float mean_root_z_all = 0.0;
		for(auto&pt : target_filtered_root) {
			mean_root_z_all += pt.z; 
		}
		mean_root_z_all /= target_filtered_root.size();
		//通过最小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 - mean_root_z_all) < hole_step_radius) {
				first_row_index.push_back(idx_r); 
				mean_z += pt.z;
			}
		}
		if (m_pLogger) {
			stringstream buff;
			buff << m_pcdId << ": current seedling number:"<< first_row_index.size()
				<<", mean_z:"<< mean_z;
			m_pLogger->INFO(buff.str());
		}
		//如果第一排的植株大于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; }
			}
			if (m_pLogger) {
				stringstream buff;
				buff << m_pcdId << ": update with historic mean_z:" << mean_z;
				m_pLogger->INFO(buff.str());
			}
		}
		//通过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_radius) {
				first_row_index.push_back(idx_r);				
			}
		}
		//找第一排的植株没有满足要求的
		if (first_row_index.size()==0) {
			if (m_pLogger) {
				stringstream buff;
				buff << m_pcdId << ": NOT found valid seedlings";
				m_pLogger->INFO(buff.str());
			}
			return false;
		}

		///////////////////////////////////////////////////////////
		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);
			//update m_root_center_with_seedling
			double min_dist = hole_step;
			int min_i = -1;
			for (int rc_i = 0; rc_i < m_root_centers.size(); ++rc_i)
			{
				double dist = fabs(pt.x - m_root_centers.at(rc_i).root_x);
				if (dist < min_dist) {
					min_dist = dist;
					min_i = rc_i;
				}
			}
			if (min_i >= 0) {
				m_root_center_with_seedling.at(min_i) = true;
			}
		}
		//统计苗数量
		first_row_size = 0;
		for (auto&has_seedling : m_root_center_with_seedling) {
			if (has_seedling) { first_row_size += 1; }
		}

		
		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());
		}
		return true;
	}
	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_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,									//  返回抓取的点坐标，基于pt_ref的偏移点
		pcl::PointXYZ &pt_ref,								//	返回点茎节的参考点		
		float& stem_width_mu,									// 茎宽度，直径
		float& stem_deflection								//茎的最大挠度，最大弯曲处的偏离直径轴心的距离，毫米
	)
	{
		// stem_deflection 的计算方法：计算点云到拟合轴线的平均距离	
		float th_ratio = 1.5;		//原始点云和直线点云邻域尺寸增加原来的20%，不能当做抓取点
		pt.x = -1000.0;
		pt.y = -1000.0;
		pt.z = -1000.0;
		stem_width_mu = 0.0;
		stem_deflection = 0.0;


		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 cx, cy, cz, t;
		float rx = 1.5;
		float ry = 1.5;
		float rz = 1.5;
		float dz, dx;
		
		// 如果opt_y_valid==false，就是用户没有指定抓取的y高度	
		float max_dist_to_boundary = 0.0;
		int max_idx_to_boundary = -1;
		find_fork(in_line_cloud, max_dist_to_boundary, max_idx_to_boundary);

		stem_deflection = static_cast<float>(calculate_deflection(in_line_cloud, line_model));

		cy = ymin;	
		//计算茎上的直径
		while(cy<ymax){
			t = (cy - line_model->values.at(1)) / line_model->values.at(4);
			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);				
				dx = max_point(0) - min_point(0);
				stem_width.push_back(dx);				
			}
			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) {
			if (w > 0) {
				valid_stem_width.push_back(w);
			}
		}
		float mu = get_hist_mean(valid_stem_width);
		stem_width_mu = mu;
		float stdv = get_hist_std(valid_stem_width, mu);

		//0)计算点云到轴线的最大距离


		//1) original max position
		int max_pos = std::max_element(stem_width.begin(), stem_width.end()) - stem_width.begin();
		int max_pos_ref = max_pos;		

		//3 如果fork方法得到位置信息，进行更新，并记录历史
		if (max_idx_to_boundary >= 0) {
			max_pos = int(in_line_cloud->points.at(max_idx_to_boundary).y + 0.5 - ymin);
			max_pos_ref = max_pos;			
		}
		

		//4 用茎节上下限高度值进行约束，如果超限，用最低限高度作为茎节高度
		double grab_fork_yup = m_cparam.rs_grab_fork_yup;
		double grab_fork_ybt = m_cparam.rs_grab_fork_ybt;
		if (m_dtype == 0) {
			grab_fork_yup = m_cparam.sc_grab_fork_yup;
			grab_fork_ybt = m_cparam.sc_grab_fork_ybt;
		}
		bool out_of_range = false;
		if ((max_pos + ymin) > grab_fork_yup || (max_pos + ymin) < grab_fork_ybt) {
			out_of_range = true;
			int original_max_pos = max_pos;
			max_pos = int(grab_fork_ybt - ymin + 0.5);
			max_pos_ref = max_pos;
			if (m_pLogger) {
				stringstream buff;
				buff << m_pcdId << ": warning，self fork postiont = " << original_max_pos <<
					", USE bottom limit fork postiont " << max_pos <<
					", valid fork postiont range:[" << int(grab_fork_ybt - ymin + 0.5) <<
					", " << int(grab_fork_yup - ymin + 0.5) << "]";
				m_pLogger->INFO(buff.str());
			}
		}
		else {
			if (m_pLogger) {
				stringstream buff;
				buff << m_pcdId << ": self fork postiont = " << max_pos <<
					", valid fork postiont range:[" << int(grab_fork_ybt - ymin + 0.5)<<
					", "<< int(grab_fork_yup - ymin + 0.5) << "]";
				m_pLogger->INFO(buff.str());
			}
		}	
		

		//5 按指定量偏移
		if(m_dtype == 0){
			max_pos_ref = max_pos;
			max_pos += static_cast<int>(m_cparam.sc_grab_offset);
		}
		else{			
			max_pos_ref = max_pos;
			max_pos += static_cast<int>(m_cparam.rs_grab_offset);
		}
		if (max_pos < 0) { max_pos = 0;	}
		if (max_pos >= online_points.size()) { max_pos = online_points.size() - 1; }
		if (out_of_range) {
			max_pos_ref = max_pos;
		}

		
		/////////////////////////////////////////////////////////////////////
		//直线点云，获取指定区域的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);

  		float z_mu = cz;
		float x_mu = cx;
		if (cloud_inbox_line->points.size() > 5) {
			pcl::getMinMax3D(*cloud_inbox_line, min_point, max_point);
			z_mu = 0.5 * (max_point(2) + min_point(2));
			x_mu = 0.5 * (max_point(0) + min_point(0));
		}

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

		pt_ref.x = online_points.at(max_pos_ref).x;
		pt_ref.y = online_points.at(max_pos_ref).y;
		pt_ref.z = online_points.at(max_pos_ref).z;

		//显示位置
		if (m_cparam.image_show) {
			pcl::visualization::PCLVisualizer viewer("grab line");
			viewer.addCoordinateSystem();
			viewer.addPointCloud<pcl::PointXYZ>(in_line_cloud, "stem_cloud");//????????????????
			viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 1, 1, 1, "stem_cloud");
			viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "stem_cloud");			

			viewer.addLine(online_points.front(), online_points.back(), 1.0, 0.0, 0.0);

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

	//计算茎的弯曲度：点云到直线的距离95分位
	double CRootStockGrabPoint::calculate_deflection(
		pcl::PointCloud<pcl::PointXYZ>::Ptr in_line_cloud,	//input
		pcl::ModelCoefficients::Ptr line_model			//input
	)
	{
		double deflection = 0.0;
		std::vector<double> dists;
		Eigen::Vector4f line_pt = { line_model->values[0],line_model->values[1],line_model->values[2],0.0 };
		Eigen::Vector4f line_dir = { line_model->values[3],line_model->values[4],line_model->values[5],0.0 };
		for (auto ptc : in_line_cloud->points) {
			//Get the square distance from a point to a line (represented by a point and a direction)
			Eigen::Vector4f pt = { ptc.x, ptc.y, ptc.z, 0.0 };
			double dist = (line_dir.cross3(line_pt - pt)).squaredNorm() / line_dir.squaredNorm();

			dists.push_back(sqrt(dist));
		}

		//deflection = get_hist_mean(dists);

		//95 percentile
		if (dists.size() > 0) {
			int idx_95 = int(0.95 * dists.size());
			std::sort(dists.begin(), dists.end());
			deflection = dists.at(idx_95);
		}
		return deflection;
	}

	void  CRootStockGrabPoint::find_fork(
		pcl::PointCloud<pcl::PointXYZ>::Ptr in_line_cloud,
		float& max_dist,
		int& max_idx
	) 
	{
		//1 project to xy plane
		pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_line_2d(new pcl::PointCloud<pcl::PointXYZ>);
		pcl::copyPointCloud(*in_line_cloud, *cloud_line_2d);
		for (pcl::PointXYZ&p : cloud_line_2d->points) { p.z = 0.0; }

		//2 边界检测
		pcl::search::KdTree<pcl::PointXYZ>::Ptr kdtree_ptr(new pcl::search::KdTree<pcl::PointXYZ>);
		pcl::PointCloud<pcl::Normal>::Ptr normal(new pcl::PointCloud<pcl::Normal>);
		pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne;
		ne.setInputCloud(cloud_line_2d);
		ne.setSearchMethod(kdtree_ptr);
		ne.setRadiusSearch(3.0);
		ne.compute(*normal);

		pcl::PointCloud<pcl::Boundary>::Ptr boundaries(new pcl::PointCloud<pcl::Boundary>);
		boundaries->resize(cloud_line_2d->size());
		pcl::BoundaryEstimation < pcl::PointXYZ, pcl::Normal, pcl::Boundary>boundary_estimation;
		boundary_estimation.setInputCloud(cloud_line_2d);
		boundary_estimation.setInputNormals(normal);

		boundary_estimation.setSearchMethod(kdtree_ptr);
		boundary_estimation.setKSearch(30);
		boundary_estimation.setAngleThreshold(M_PI*0.6);
		boundary_estimation.compute(*boundaries);

		pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_lines(new pcl::PointCloud<pcl::PointXYZ>);

		pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_visual(new pcl::PointCloud<pcl::PointXYZRGB>);
		cloud_visual->resize(cloud_line_2d->size());
		for (size_t i = 0; i < cloud_line_2d->size(); ++i) {
			cloud_visual->points.at(i).x = cloud_line_2d->points.at(i).x;
			cloud_visual->points.at(i).y = cloud_line_2d->points.at(i).y;
			cloud_visual->points.at(i).z = cloud_line_2d->points.at(i).z;
			if (boundaries->points.at(i).boundary_point != 0) {
				cloud_visual->points.at(i).r = 255;
				cloud_visual->points.at(i).g = 0;
				cloud_visual->points.at(i).b = 0;
				cloud_lines->points.push_back(cloud_line_2d->points.at(i));
			}
			else {
				cloud_visual->points.at(i).r = 255;
				cloud_visual->points.at(i).g = 255;
				cloud_visual->points.at(i).b = 255;
			}
		}

		/*if (m_cparam.image_show) {
			viewer_cloud(cloud_visual, string("boundary"));
			viewer_cloud(cloud_lines, string("cloud_lines"));
		}
*/
		//3 find points with max distance to boundaries point
		pcl::search::KdTree<pcl::PointXYZ>::Ptr bound_kdtree_ptr(new pcl::search::KdTree<pcl::PointXYZ>);
		bound_kdtree_ptr->setInputCloud(cloud_lines);

		max_idx = -1;
		max_dist = 0.0;
		int k = 1;
		for (int i = 0; i < cloud_line_2d->size(); ++i) {
			if (boundaries->points.at(i).boundary_point != 0) { continue; }
			std::vector<int> idx(k);
			std::vector<float> sqr_distances(k);
			int cnt = bound_kdtree_ptr->nearestKSearch(cloud_line_2d->points.at(i), k,idx,sqr_distances);
			if (cnt > 0) {
				if (sqr_distances.at(0) > max_dist) {
					max_dist = sqr_distances.at(0);
					max_idx = i;
				}
			}
		}
		if (m_cparam.image_show) {
			pcl::visualization::PCLVisualizer viewer("boundary line");
			viewer.addCoordinateSystem();
			viewer.addPointCloud<pcl::PointXYZRGB>(cloud_visual, "boundary");
			
			if (max_idx >= 0) {
				pcl::PointXYZ p0,p1;
				p0.x = cloud_line_2d->points.at(max_idx).x-5;
				p0.y = cloud_line_2d->points.at(max_idx).y;
				p0.z = cloud_line_2d->points.at(max_idx).z;

				p1.x = cloud_line_2d->points.at(max_idx).x + 5;
				p1.y = cloud_line_2d->points.at(max_idx).y;
				p1.z = cloud_line_2d->points.at(max_idx).z;
				viewer.addLine(p0, p1, 1.0, 0.0, 0.0);
			}
			

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

	void CRootStockGrabPoint::get_line_project_hist(		
		pcl::PointCloud<pcl::PointXYZ>::Ptr in_line_cloud,	//input	茎上直线点云
		pcl::ModelCoefficients::Ptr line_model,				//input		
		std::vector<int>& count_hist					    //  返回有效直线1mm内点云数量
	)
	{
		count_hist.clear();		

		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::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;		
		if (m_dtype == 0) {
			radius = m_cparam.sc_grab_stem_diameter;			
		}
		float rx = 1.5;		
		float rz = 1.5;
		float cx, cy, cz, t;

		cy = ymin;
		while (cy<ymax) {
			t = (cy - line_model->values.at(1)) / line_model->values.at(4);
			cx = line_model->values.at(3) * t + line_model->values.at(0);
			cz = line_model->values.at(5) * t + line_model->values.at(2);
			
			box_filter_line.setMin(Eigen::Vector4f(cx - rx*radius, cy - 0.5, cz - rz*radius, 1));
			box_filter_line.setMax(Eigen::Vector4f(cx + rx*radius, cy + 0.5, cz + rz*radius, 1));
			box_filter_line.filter(*cloud_inbox);
			count_hist.push_back(cloud_inbox->points.size());
			cy += 1.0;
		}		
	}
	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 &p_ref,//分叉点
		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;
		root_py0.z = root_pt.z - 5.0;
		root_py1.x = root_pt.x;
		root_py1.y = root_pt.y;
		root_py1.z = root_pt.z + 5.0;

		//茎节点
		pcl::PointXYZ fork_px0, fork_px1, fork_py0, fork_py1;
		fork_px0.x = p_ref.x - 5.0;
		fork_px0.y = p_ref.y;
		fork_px0.z = p_ref.z;
		fork_px1.x = p_ref.x + 5.0;
		fork_px1.y = p_ref.y;
		fork_px1.z = p_ref.z;

		fork_py0.x = p_ref.x;
		fork_py0.y = p_ref.y;
		fork_py0.z = p_ref.z - 5.0;
		fork_py1.x = p_ref.x;
		fork_py1.y = p_ref.y;
		fork_py1.z = p_ref.z + 5.0;


		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");

		viewer.addLine(fork_px0, fork_px1, 255, 0, 0, "forkx");
		viewer.addLine(fork_py0, fork_py1, 0, 255, 0, "forky");

		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;
			px0.y = p.y;
			px0.z = p.z;
			px1.x = p.x + 10.0;
			px1.y = p.y;
			px1.z = p.z;

			py0.x = p.x;
			py0.y = p.y;
			py0.z = p.z;
			py1.x = p.x;
			py1.y = p.y + 10.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));
		}

	}

};