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

#include "chessboard_calibration.h"
#include "utils.h"

#define PI std::acos(-1)

namespace graft_cv {

	CChessboardCalibration::CChessboardCalibration(ConfigParam&cp, CGcvLogger*pLog/*=0*/)
		:m_cparam(cp),
		m_pLogger(pLog),
		m_dtype(0),
		m_pcdId(""),
		m_ppImgSaver(0),		
		m_cloud_mean_dist(0.0)
	{
	}

	CChessboardCalibration::~CChessboardCalibration() {}
	float* CChessboardCalibration::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 CChessboardCalibration::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.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));
			}
		}
		return rst;
	}

	int CChessboardCalibration::cross_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");
		}
		////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
		//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());
		}

		//获取点云范围
		pcl::PointXYZ min_v;
		pcl::PointXYZ max_v;
		pcl::getMinMax3D(*cloud_ror, min_v, max_v);

		//4 边界检测
		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_ror);
		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_ror->size());
		pcl::BoundaryEstimation < pcl::PointXYZ, pcl::Normal, pcl::Boundary>boundary_estimation;
		boundary_estimation.setInputCloud(cloud_ror);
		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_ror->size());
		for (size_t i = 0; i < cloud_ror->size(); ++i) {
			cloud_visual->points.at(i).x = cloud_ror->points.at(i).x;
			cloud_visual->points.at(i).y = cloud_ror->points.at(i).y;
			cloud_visual->points.at(i).z = cloud_ror->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_ror->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"));
		}

		//# 直线抽取
		float line_radius = stem_radius * 0.75;
		std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> lines;
		std::vector<pcl::ModelCoefficients::Ptr> lines_model;
		pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_remain(new pcl::PointCloud<pcl::PointXYZ>);
		pcl::copyPointCloud(*cloud_lines, *cloud_remain);

		while (cloud_remain->size() > 100) {
			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(line_radius);
			seg.setInputCloud(cloud_remain);
			seg.segment(*inliers, *coefficients);



			pcl::PointCloud<pcl::PointXYZ>::Ptr stem_cloud(new pcl::PointCloud<pcl::PointXYZ>);
			pcl::copyPointCloud(*cloud_remain, *inliers, *stem_cloud);

			if (stem_cloud->size() > 100) {
				lines.push_back(stem_cloud);
				lines_model.push_back(coefficients);
			}
			

			//显示得到的直线
			//if (m_cparam.image_show) {
			//	pcl::visualization::PCLVisualizer viewer("line");
			//	viewer.addCoordinateSystem();
			//	viewer.addPointCloud<pcl::PointXYZ>(cloud_remain, "cloud_remain");//????????????????
			//	viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 1, 1, 1, "cloud_remain");
			//	viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "cloud_remain");

			//	viewer.addPointCloud<pcl::PointXYZ>(stem_cloud, "line_cloud");//????????????????
			//	viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 1, 0, 0, "line_cloud");
			//	viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "line_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));
			//	}
			//}
			//更新cloud_remain
			pcl::PointCloud<pcl::PointXYZ>::Ptr tmp_cloud(new pcl::PointCloud<pcl::PointXYZ>);
			std::vector<int> remain_idx;
			for (size_t i = 0; i < cloud_remain->size(); ++i) {
				if (std::find(inliers->indices.begin(), inliers->indices.end(), i) == inliers->indices.end()) {
					remain_idx.push_back(i);
				}
			}
			pcl::copyPointCloud(*cloud_remain, remain_idx, *tmp_cloud);
			cloud_remain = tmp_cloud;
		}
		
		//计算 直线间的交点
		std::vector<pcl::PointXYZ> cross_points;
		int padding = 10;
		for (int i = 0; i < lines.size() - 1; ++i) {
			for (int j = i + 1; j < lines.size(); ++j) {
				//计算两个点云简单最小距离 https://blog.csdn.net/qq_29600745/article/details/111310214
				double dist = min_dist_of_lines(lines_model.at(i), lines_model.at(j));
				if (dist > 5) { continue; }
				//计算公垂线中点坐标： https://blog.csdn.net/qq_42648534/article/details/124100036

				pcl::PointXYZ cross_pt;
				virtual_cross_of_lines(lines_model.at(i), lines_model.at(j), cross_pt);

				if (cross_pt.x > max_v.x-padding || cross_pt.x<min_v.x+padding ||
					cross_pt.y>max_v.y - padding || cross_pt.y<min_v.y + padding ||
					cross_pt.z>max_v.z - padding || cross_pt.z < min_v.z + padding)
				{
					continue;
				}

				cross_points.push_back(cross_pt);
			}
		}

		// 显示在点云中
		if (m_cparam.image_show) {
			pcl::visualization::PCLVisualizer viewer("cross");
			viewer.addCoordinateSystem();
			viewer.addPointCloud<pcl::PointXYZ>(cloud_lines, "cloud_lines");//????????????????
			viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 1, 1, 1, "cloud_lines");
			viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "cloud_lines");

			pcl::PointCloud<pcl::PointXYZ>::Ptr cross_cloud(new pcl::PointCloud<pcl::PointXYZ>);
			for (auto& cp : cross_points) {
				cross_cloud->points.push_back(cp);
			}

			viewer.addPointCloud<pcl::PointXYZ>(cross_cloud, "cross_cloud");//????????????????
			viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 1, 0, 0, "cross_cloud");
			viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "cross_cloud");

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


		//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
		////7 保存处理结果到图片
		cv::Mat rst_img = cv::Mat::zeros(cv::Size(1280, 1280), CV_8UC3);
		gen_result_img(cloud_ror, cloud_lines, cross_points, rst_img);
		if (m_ppImgSaver && *m_ppImgSaver) {
			(*m_ppImgSaver)->saveImage(rst_img, m_pcdId + "_rst_0");
		}
		//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
		////8 debug 显示结果
		if (m_cparam.image_show) {			
			imshow_wait("rst_img", rst_img);
		}
		return 0;
	}
	double CChessboardCalibration::min_dist_of_lines(
		pcl::ModelCoefficients::Ptr line_1,
		pcl::ModelCoefficients::Ptr line_2
	)
	{
		pcl::PointXYZ m;
		m.x = line_1->values.at(0) - line_2->values.at(0);
		m.y = line_1->values.at(1) - line_2->values.at(1);
		m.z = line_1->values.at(2) - line_2->values.at(2);

		pcl::PointXYZ e;
		e.x = line_1->values.at(4) * line_2->values.at(5) - line_1->values.at(5) * line_2->values.at(4);
		e.y = line_1->values.at(5) * line_2->values.at(3) - line_1->values.at(3) * line_2->values.at(5);
		e.z = line_1->values.at(3) * line_2->values.at(4) - line_1->values.at(4) * line_2->values.at(3);


		double dist = std::fabs(m.x * e.x + m.y * e.y + m.z * e.z);
		double q = std::sqrt(e.x*e.x + e.y*e.y+ e.z*e.z);
		dist /= q;
		return dist;
	}
	void CChessboardCalibration::virtual_cross_of_lines(
		pcl::ModelCoefficients::Ptr line_l, //input
		pcl::ModelCoefficients::Ptr line_r,//input
		pcl::PointXYZ& cross_pt//output
	)
	{
		//计算两条直线的虚交点--公垂线的中点
		pcl::PointXYZ pl, pr;//直线上的点
		pl.x = line_l->values.at(0);
		pl.y = line_l->values.at(1);
		pl.z = line_l->values.at(2);

		pr.x = line_r->values.at(0);
		pr.y = line_r->values.at(1);
		pr.z = line_r->values.at(2);



		pcl::PointXYZ p_rl, ll,lr;
		p_rl.x = line_r->values.at(0) - line_l->values.at(0);
		p_rl.y = line_r->values.at(1) - line_l->values.at(1);
		p_rl.z = line_r->values.at(2) - line_l->values.at(2);

		ll.x = line_l->values.at(3);
		ll.y = line_l->values.at(4);
		ll.z = line_l->values.at(5);

		lr.x = line_r->values.at(3);
		lr.y = line_r->values.at(4);
		lr.z = line_r->values.at(5);

		float pll = p_rl.x * ll.x + p_rl.y * ll.y + p_rl.z * ll.z;
		float plr = p_rl.x * lr.x + p_rl.y * lr.y + p_rl.z * lr.z;
		float ll_lr = ll.x * lr.x + ll.y * lr.y + ll.z * lr.z;

		float t1 = (pll - plr * ll_lr) / (1.0 - ll_lr*ll_lr);
		float t2 = (pll * ll_lr - plr ) / (1.0 - ll_lr*ll_lr);

		pcl::PointXYZ ml, mr;
		ml.x = pl.x + t1 * ll.x;
		ml.y = pl.y + t1 * ll.y;
		ml.z = pl.z + t1 * ll.z;

		mr.x = pr.x + t2 * lr.x;
		mr.y = pr.y + t2 * lr.y;
		mr.z = pr.z + t2 * lr.z;

		cross_pt.x = 0.5 * (ml.x + mr.x);
		cross_pt.y = 0.5 * (ml.y + mr.y);
		cross_pt.z = 0.5 * (ml.z + mr.z);
	}

	//生成结果图片
	void CChessboardCalibration::gen_result_img(
		pcl::PointCloud<pcl::PointXYZ>::Ptr in_cloud_raw,//输入，未经过滤的整体点云in_cloud_raw, 
		pcl::PointCloud<pcl::PointXYZ>::Ptr in_cloud_edge,	//输入，边缘点云, 
		std::vector<pcl::PointXYZ>& cross_points,				//输入，交叉点,		
		cv::Mat& rst_img						//输出，图片， 1280*1280
	)
	{
		if (rst_img.empty()) { return; }
		if (rst_img.rows != 1280 && rst_img.cols != 1280) { return; }
		int cx = 640;		//图像中心点0
		int cy = 640;		//图像中心点0
		float res = 0.2;	//分辨率，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] = 64;
			rst_img.at<cv::Vec3b>(y, x)[1] = 64;
			rst_img.at<cv::Vec3b>(y, x)[2] = 64;
		}

		for (auto&pt : in_cloud_edge->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] = 180;
			rst_img.at<cv::Vec3b>(y, x)[2] = 180;
		}
		//绘制抓取点坐标
		int radius = 10;
		int row_sep = 15;
		float fsize = 0.5;
		for (int i = 0; i < cross_points.size(); ++i) {
			pcl::PointXYZ&pt = cross_points.at(i);
			int grab_x = cx - int(pt.x / res + 0.5);
			int grab_y = cy - int(pt.y / res + 0.5);
			
			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));

			stringstream buff;

			buff << i << ":";
			cv::putText(rst_img, buff.str(), cv::Point(grab_x + 10, grab_y), cv::FONT_HERSHEY_SIMPLEX, fsize, cv::Scalar(0, 220, 0));
			buff.str("");
			buff.setf(ios::fixed);
			buff.precision(2);
			buff << pt.x;
			cv::putText(rst_img, buff.str(), cv::Point(grab_x +10, grab_y+ row_sep), cv::FONT_HERSHEY_SIMPLEX, fsize, cv::Scalar(0, 220, 0));
			buff.str("");
			buff << pt.y;
			cv::putText(rst_img, buff.str(), cv::Point(grab_x + 10, grab_y+ 2* row_sep), cv::FONT_HERSHEY_SIMPLEX, fsize, cv::Scalar(0, 220, 0));
			buff.str("");
			buff << pt.z;
			cv::putText(rst_img, buff.str(), cv::Point(grab_x + 10, grab_y + 3 * row_sep), cv::FONT_HERSHEY_SIMPLEX, fsize, cv::Scalar(0, 220, 0));

		}
		
		

	}
	int CChessboardCalibration::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 CChessboardCalibration::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 CChessboardCalibration::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 CChessboardCalibration::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 CChessboardCalibration::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 CChessboardCalibration::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 CChessboardCalibration::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 CChessboardCalibration::viewer_cloud_cluster_box(
		pcl::PointCloud<pcl::PointXYZ>::Ptr cloud,
		std::vector<pcl::PointXYZ>&cluster_center,
		std::vector<pcl::PointXYZ>&cluster_box,
		std::vector<std::vector<int> >& clt_line_idx,
		std::string&winname)
	{
		pcl::visualization::PCLVisualizer viewer(winname);
		viewer.addCoordinateSystem();
		viewer.addPointCloud<pcl::PointXYZ>(cloud, "all_cloud");
		viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 1, 1, 1, "all_cloud");
		viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "all_cloud");

		int cnt = 0;
		char buf[8];
		for (size_t i = 0; i < cluster_center.size(); ++i) {
			pcl::PointXYZ &p = cluster_center.at(i);
			pcl::PointXYZ px0, px1, py1, py0;
			px0.x = p.x - 5.0;
			px0.y = p.y;
			px0.z = p.z;
			px1.x = p.x + 5.0;
			px1.y = p.y;
			px1.z = p.z;

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

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

			//box
			pcl::PointXYZ & min_pt = cluster_box.at(2 * i);
			pcl::PointXYZ & max_pt = cluster_box.at(2 * i + 1);
			viewer.addCube(min_pt.x, max_pt.x, min_pt.y, max_pt.y, min_pt.z, max_pt.z, 0.5, 0.5, 0.0, "AABB_" + string(_itoa(cnt, buf, 10)));
			viewer.setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_REPRESENTATION,
				pcl::visualization::PCL_VISUALIZER_REPRESENTATION_WIREFRAME, "AABB_" + string(_itoa(cnt, buf, 10)));
			if (clt_line_idx.size()>i && clt_line_idx.at(i).size() > 0) {
				pcl::PointCloud<pcl::PointXYZ>::Ptr line_cloud(new pcl::PointCloud<pcl::PointXYZ>);
				pcl::copyPointCloud(*cloud, clt_line_idx.at(i), *line_cloud);
				viewer.addPointCloud(line_cloud, "fit_line" + string(_itoa(cnt, buf, 10)));
				viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 0, 1, 0, "fit_line" + string(_itoa(cnt, buf, 10)));
				viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "fit_line" + string(_itoa(cnt, buf, 10)));
			}
			cnt += 1;
		}
		while (!viewer.wasStopped()) {
			viewer.spinOnce(100);
			boost::this_thread::sleep(boost::posix_time::microseconds(100000));
		}

	}

};