v0.7.11 增加棋盘格识别，用于标定辅助

ReadMe.txt

@@ -95,4 +95,5 @@ v0.7.6
 v0.7.7 优化茎判别问题：上下2片叶子，利用有效高度剔除；偏离z中心剔除，采用历史z均值
 v0.7.8 修改接穗抓点靠上一点
 v0.7.9 修改接穗抓点靠上一点，增加了grab_offset，用来在找到茎节后的上下偏移得到抓取点，+值向上偏，-值向下偏
-v0.7.10 增加茎节点位置绘制输出；茎节判别仅按dx方向大小判断（dz误差较大）
+v0.7.10 增加茎节点位置绘制输出；茎节判别仅按dx方向大小判断（dz误差较大）
+v0.7.11 增加棋盘格识别，用于标定辅助

chessboard_calibration.cpp

@@ -0,0 +1,815 @@
+#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));
+		}
+
+	}
+
+};

chessboard_calibration.h

@@ -0,0 +1,76 @@
+#pragma once
+
+#include <pcl\point_types.h>
+#include <pcl\point_cloud.h>
+#include <pcl\segmentation\sac_segmentation.h>
+#include "data_def_api.h"
+#include "data_def.h"
+#include "logger.h"
+#include "imstorage_manager.h"
+
+namespace graft_cv {
+	class CChessboardCalibration {
+	public:
+		CChessboardCalibration(ConfigParam&c, CGcvLogger*pLog = 0);
+		~CChessboardCalibration();
+		int cross_detect(PositionInfo& posinfo);
+		float* get_raw_point_cloud(int &data_size);
+		int load_data(float*pPoint, int pixel_size, int pt_size, int dtype, const char* fn = 0);
+		void set_image_saver(CImStoreManager** ppis) { m_ppImgSaver = ppis; }
+	private:
+		//global configure parameters
+		ConfigParam& m_cparam;
+		CGcvLogger * m_pLogger;
+		int m_dtype;
+		string m_pcdId;
+
+		CImStoreManager** m_ppImgSaver;
+
+		pcl::PointCloud<pcl::PointXYZ>::Ptr m_raw_cloud;
+		double m_cloud_mean_dist;
+
+		double min_dist_of_lines(
+			pcl::ModelCoefficients::Ptr line_1,//input
+			pcl::ModelCoefficients::Ptr line_2//input
+			);
+		void virtual_cross_of_lines(
+			pcl::ModelCoefficients::Ptr line_1, //input
+			pcl::ModelCoefficients::Ptr line_2,//input
+			pcl::PointXYZ& cross_pt//output
+		);
+
+
+		int read_ply_file(const char* fn);
+		double compute_nearest_neighbor_distance(pcl::PointCloud<pcl::PointXYZ>::Ptr);
+		void cloud_mean_dist(
+			pcl::PointCloud<pcl::PointXYZ>::Ptr in_cloud,	//input 输入点云数据
+			double& mean_dist
+		);
+		void 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
+		);
+		
+
+		void viewer_cloud(pcl::PointCloud<pcl::PointXYZ>::Ptr, std::string&winname);
+		void viewer_cloud(pcl::PointCloud<pcl::PointXYZRGB>::Ptr, std::string&winname);
+
+		void viewer_cloud_debug(
+			pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud,
+			pcl::PointXYZ&p,//抓取点
+			pcl::PointXYZ &p_ref,//分叉点
+			pcl::PointXYZ &root_pt,
+			std::string&winname);
+
+		void viewer_cloud_cluster(pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud, std::vector<pcl::PointXYZ>cluster_center, std::string&winname);
+		void 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);
+	};
+
+};

graft_cv_api.cpp

@@ -14,12 +14,13 @@
 #include "cut_point_rs_reid.h"
 
 #include "grab_point_rs.h"
+#include "chessboard_calibration.h"
 
 extern CRITICAL_SECTION g_cs;
 namespace graft_cv
 {
 
-	char *g_version_str = "0.7.10";
+	char *g_version_str = "0.7.11";
 
 	//configure
 	string g_conf_file = "./gcv_conf.yml";	
@@ -50,6 +51,8 @@ namespace graft_cv
 	CSolaCutPointReid g_sola_reid_rs = CSolaCutPointReid(g_cp, 1, &g_logger);
 	CSolaCutPointReid g_sola_reid_sc = CSolaCutPointReid(g_cp, 0, &g_logger);
 
+	CChessboardCalibration g_chessboard = CChessboardCalibration(g_cp, &g_logger);
+
 	//??¶àÏß³ÌÓÐÎÊÌâ
 	map<string, cv::Mat> g_img_cache;
 	
@@ -374,6 +377,38 @@ namespace graft_cv
 
 	}
 
+	int chessboard_calibration(float* points, int pixel_size, int pt_size, PositionInfo& posinfo, const char* fn)
+	{
+		memset(&posinfo, 0, sizeof(PositionInfo));
+		int dtype = 0;
+		try {
+			int rst = g_chessboard.load_data(points, pixel_size, pt_size, dtype, fn);
+			if (rst <= 0) {
+				g_logger.ERRORINFO("invalid points");
+				return 1;
+			}
+			int oa = g_chessboard.cross_detect(posinfo);
+			if (oa != 0) {
+				g_logger.ERRORINFO("no points");
+				return 1;
+			}
+		}
+		catch (std::exception &err) {
+			g_logger.ERRORINFO(err.what());
+			return 1;
+		}
+		catch (string& msg) {
+			g_logger.ERRORINFO(msg);
+			return 1;
+		}
+		catch (...) {
+			g_logger.ERRORINFO("unknown error");
+			return 1;
+		}
+		return 0;
+	 }
+
+
 	//9
 	int rs_oa_get_result(
 		ImgInfo* imginfo,

graft_cv_api.h

@@ -80,6 +80,7 @@ GCV_API void get_version(char* buf);
 GCV_API int sola_grab_point_rs(float* points, int pixel_size, int pt_size, PositionInfo& posinfo, const char* fn=0);
 GCV_API int sola_grab_point_sc(float* points, int pixel_size, int pt_size, PositionInfo& posinfo, const char* fn = 0);
 
+GCV_API int chessboard_calibration(float* points, int pixel_size, int pt_size, PositionInfo& posinfo, const char* fn = 0);
 
 //12  砧木最优角度识别,追加图像，要求图像的角度按顺序，第一帧图像角度为0，最后一帧图像角度为180
 //   每次追加图片，通过posinfo返回：