v0.6.2 增加基于点云的茎节识别接口

ReadMe.txt

@@ -69,4 +69,5 @@ v0.5.9.20
 		  按测试，修改rs_cut_point_offset_ratio: 9.0000000000000000e-001
 		  修改穗苗识别方法（用于标定位置的夹子被遮挡）
 v0.6.0 修改旋转角度识别算法，采用顶部拍照，一次识别旋转角度
-v0.6.1 移植到vs2015平台（vc14），选用OpenCV v4.55
+v0.6.1 移植到vs2015平台（vc14），选用OpenCV v4.55
+v0.6.2 增加上苗点云识别夹取位置接口及功能

config.cpp

@@ -94,6 +94,15 @@ namespace graft_cv{
 			<< "rs_oa_pixel_ratio" << m_cparam->rs_oa_pixel_ratio
 			<< "rs_cut_pixel_ratio" << m_cparam->rs_cut_pixel_ratio
 			<< "sc_cut_pixel_ratio" << m_cparam->sc_cut_pixel_ratio
+
+			<< "rs_grab_xmin" << m_cparam->rs_grab_xmin
+			<< "rs_grab_xmax" << m_cparam->rs_grab_xmax
+			<< "rs_grab_ymin" << m_cparam->rs_grab_ymin
+			<< "rs_grab_ymax" << m_cparam->rs_grab_ymax
+			<< "rs_grab_zmin" << m_cparam->rs_grab_zmin
+			<< "rs_grab_zmax" << m_cparam->rs_grab_zmax
+			<< "rs_grab_stem_diameter" << m_cparam->rs_grab_stem_diameter
+
 			<< "}"; 	
 	};
 	void CGCvConfig::read(const cv::FileNode& node){ //Read serialization for this class
@@ -170,7 +179,15 @@ namespace graft_cv{
 
 		m_cparam->rs_oa_pixel_ratio  = (double)node["rs_oa_pixel_ratio"];
 		m_cparam->rs_cut_pixel_ratio  = (double)node["rs_cut_pixel_ratio"];
-		m_cparam->sc_cut_pixel_ratio  = (double)node["sc_cut_pixel_ratio"]; 		
+		m_cparam->sc_cut_pixel_ratio  = (double)node["sc_cut_pixel_ratio"]; 
+
+		m_cparam->rs_grab_xmin = (double)node["rs_grab_xmin"];
+		m_cparam->rs_grab_xmax = (double)node["rs_grab_xmax"];
+		m_cparam->rs_grab_ymin = (double)node["rs_grab_ymin"];
+		m_cparam->rs_grab_ymax = (double)node["rs_grab_ymax"];
+		m_cparam->rs_grab_zmin = (double)node["rs_grab_zmin"];
+		m_cparam->rs_grab_zmax = (double)node["rs_grab_zmax"];
+		m_cparam->rs_grab_stem_diameter = (double)node["rs_grab_stem_diameter"];
     
   }
 	string get_cparam_info(ConfigParam*m_cparam)
@@ -254,6 +271,15 @@ namespace graft_cv{
 			<< "rs_oa_pixel_ratio:\t" << m_cparam->rs_oa_pixel_ratio << endl
 			<< "rs_cut_pixel_ratio:\t" << m_cparam->rs_cut_pixel_ratio << endl
 			<< "sc_cut_pixel_ratio:\t" << m_cparam->sc_cut_pixel_ratio << endl
+
+			<< "rs_grab_xmin:\t" << m_cparam->rs_grab_xmin << endl
+			<< "rs_grab_xmax:\t" << m_cparam->rs_grab_xmax << endl
+			<< "rs_grab_ymin:\t" << m_cparam->rs_grab_ymin << endl
+			<< "rs_grab_ymax:\t" << m_cparam->rs_grab_ymax << endl
+			<< "rs_grab_zmin:\t" << m_cparam->rs_grab_zmin << endl
+			<< "rs_grab_zmax:\t" << m_cparam->rs_grab_zmax << endl
+			<< "rs_grab_stem_diameter:\t" << m_cparam->rs_grab_stem_diameter << endl
+
 			<< "}" << endl; 	
 		return buff.str();
 	}

data_def_api.h

@@ -103,11 +103,27 @@ typedef struct{
 	double rs_cut_pixel_ratio; // rootstock cutting camera  pixel ratio for millimeter, mm/piexel
 	double sc_cut_pixel_ratio; // scion cutting camera  pixel ratio for millimeter, mm/piexel
 	
+	// rootstock grab based points cloud
+	double rs_grab_xmin;
+	double rs_grab_xmax;
+	double rs_grab_ymin;
+	double rs_grab_ymax;
+	double rs_grab_zmin;
+	double rs_grab_zmax;
+
+	double rs_grab_stem_diameter;	
+
+
+
 } ConfigParam;
 
 typedef struct 
 {
 	//以下涉及到位置均为实际位置，毫米
+	double rs_grab_x;//砧木上苗抓取位置x，毫米
+	double rs_grab_y;//砧木上苗抓取位置y，毫米
+	double rs_grab_z;//砧木上苗抓取位置z，毫米
+
 	double rs_oa;//砧木最优角度
 	//double rs_oa_base;//砧木基质最优角度
 	//double rs_oa_stem_y_fork;//茎分叉点y，毫米

gcv_conf.yml

@@ -65,3 +65,11 @@ conf_parameters:
    rs_oa_pixel_ratio: 2.3333300000000001e-001
    rs_cut_pixel_ratio: 5.8479999999999997e-002
    sc_cut_pixel_ratio: 8.7499999999999994e-002
+   rs_grab_xmin: -200
+   rs_grab_xmax: 200
+   rs_grab_ymin: -150
+   rs_grab_ymax: 150
+   rs_grab_zmin: 200
+   rs_grab_zmax: 320
+   rs_grab_stem_diameter: 5.0
+

grab_point_rs.cpp

@@ -0,0 +1,621 @@
+/*
+通过点云数据识别抓取位置信息
+
+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\voxel_grid.h>
+#include <pcl\common\common.h>
+#include <math.h>
+
+#include "grab_point_rs.h"
+#include "utils.h"
+
+#define PI std::acos(-1)
+
+namespace graft_cv {
+
+	CRootStockGrabPoint::CRootStockGrabPoint(ConfigParam&cp, CGcvLogger*pLog/*=0*/)
+		:m_cparam(cp),
+		m_pLogger(pLog)
+	{
+	}
+
+	CRootStockGrabPoint::~CRootStockGrabPoint() {}
+	float* CRootStockGrabPoint::get_raw_point_cloud(int &data_size)
+	{
+		data_size = m_raw_cloud->width * m_raw_cloud->height;
+		if (data_size == 0) {
+			return 0;
+		}
+		else {
+			pcl::PointXYZ* pp = m_raw_cloud->points.data();
+			return (float*)pp->data;			
+		}
+
+	}
+	int CRootStockGrabPoint::load_data(
+		float*pPoint,
+		int pixel_size,
+		int pt_size,		
+		const char* fn/* = 0*/)
+	{
+		int rst = 0;
+		//1 get point cloud data
+		if (pPoint != 0 && pt_size>0) {
+			//read point
+			m_raw_cloud.reset(new pcl::PointCloud<pcl::PointXYZ>);
+			int step = pixel_size;
+			for (int i = 0; i < pt_size; ++i) {
+				pcl::PointXYZ pt = { pPoint[i*step], pPoint[i*step + 1] , pPoint[i*step + 2] };
+				m_raw_cloud->push_back(pt);
+			}
+			rst = m_raw_cloud->width * m_raw_cloud->height;
+			if (m_pLogger) {
+				stringstream buff;
+				buff << "load raw point cloud " << rst << " data points";
+				m_pLogger->INFO(buff.str());
+			}			
+		}
+		else if (fn != 0) {
+			//read file
+			rst = this->read_ply_file(fn);					
+		}
+		else {//error
+			if (m_pLogger) {
+				m_pLogger->ERRORINFO("no valid input");
+				return (-1);
+			}
+		}	
+
+		if (m_cparam.image_show) {
+			viewer_cloud(m_raw_cloud, std::string("raw point cloud"));
+		}
+		return rst;
+	}
+
+	int CRootStockGrabPoint::grab_point_detect(		
+		PositionInfo& posinfo
+		)
+	{
+		
+		//2 crop filter
+		pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_inbox(new pcl::PointCloud<pcl::PointXYZ>);
+		pcl::CropBox<pcl::PointXYZ> box_filter;
+		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));
+		box_filter.setNegative(false);
+		box_filter.setInputCloud(m_raw_cloud);
+		box_filter.filter(*cloud_inbox);
+
+		if (m_pLogger) {
+			stringstream buff;
+			buff << "CropBox croped point cloud " << cloud_inbox->width * cloud_inbox->height << " data points";
+			m_pLogger->INFO(buff.str());
+		}
+
+		if (m_cparam.image_show) {
+			viewer_cloud(cloud_inbox, std::string("cloud_inbox"));
+		}
+		//3 filtler with radius remove
+		int nb_points = 20;
+		double stem_radius = m_cparam.rs_grab_stem_diameter / 2.0;
+		pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_ror(new pcl::PointCloud<pcl::PointXYZ>);
+		pcl::RadiusOutlierRemoval<pcl::PointXYZ> ror;
+		ror.setInputCloud(cloud_inbox);
+		ror.setRadiusSearch(stem_radius);
+		ror.setMinNeighborsInRadius(nb_points);
+		ror.filter(*cloud_ror);
+
+		if (m_pLogger) {
+			stringstream buff;
+			buff << "RadiusOutlierRemoval filtered point cloud " << cloud_ror->width * cloud_ror->height << " data points. param stem_radius="<<
+				stem_radius<<", nb_points="<< nb_points;
+			m_pLogger->INFO(buff.str());
+		}
+
+		if (m_cparam.image_show) {
+			viewer_cloud(cloud_ror, std::string("cloud_ror"));
+		}
+
+		//3 voxel grid down sampling
+		pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_dowm_sampled(new pcl::PointCloud<pcl::PointXYZ>);
+		pcl::VoxelGrid<pcl::PointXYZ> outrem;
+		outrem.setInputCloud(cloud_ror);
+		outrem.setLeafSize(stem_radius, stem_radius, stem_radius);
+		outrem.filter(*cloud_dowm_sampled);
+
+		if (m_pLogger) {
+			stringstream buff;
+			buff << "VoxelGrid dowm_sampled point cloud " << cloud_dowm_sampled->width * cloud_dowm_sampled->height << " data points";
+			m_pLogger->INFO(buff.str());
+		}
+
+		if (m_cparam.image_show) {
+			viewer_cloud(cloud_dowm_sampled, std::string("cloud_dowm_sampled"));
+		}
+		//4 seedling position
+		std::vector<int> first_seedling_cloud_idx;
+		pcl::PointXYZ xz_center;
+		find_seedling_position(cloud_dowm_sampled, first_seedling_cloud_idx, xz_center);
+		if (m_pLogger) {
+			stringstream buff;
+			buff << "after find_seedling_position(), foud first seedling seeds points size " << first_seedling_cloud_idx .size();
+			m_pLogger->INFO(buff.str());
+		}
+		//5 nearest seedling grab point selection
+		pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_seedling_seed(new pcl::PointCloud<pcl::PointXYZ>);
+		pcl::copyPointCloud(*cloud_dowm_sampled, first_seedling_cloud_idx, *cloud_seedling_seed);
+		std::vector<int>mass_idx;
+		double dist_mean = compute_nearest_neighbor_distance(cloud_dowm_sampled);
+		std::vector<double> mass_indices;
+		crop_nn_analysis(cloud_ror, cloud_seedling_seed, dist_mean, mass_indices, mass_idx);
+		
+		double candidate_th = otsu(mass_indices);
+		std::vector<int> optimal_seeds_idx;
+		for (size_t j = 0; j < mass_idx.size(); ++j) {
+			if (mass_indices[mass_idx[j]] >= candidate_th) {
+				optimal_seeds_idx.push_back(mass_idx[j]);
+			}
+		}
+		pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_optimal_seed(new pcl::PointCloud<pcl::PointXYZ>);
+		pcl::copyPointCloud(*cloud_seedling_seed, optimal_seeds_idx, *cloud_optimal_seed);
+		pcl::PointXYZ selected_pt;
+		int selected_pt_idx;
+		get_optimal_seed(cloud_optimal_seed, selected_pt, selected_pt_idx);
+
+		posinfo.rs_grab_x = selected_pt.x;
+		posinfo.rs_grab_y = selected_pt.y;
+		posinfo.rs_grab_z = selected_pt.z;
+
+		////////////////////////////////////////////////////////////////////
+		//debug
+		if (m_cparam.image_show) {
+			pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_cand_demo(new pcl::PointCloud<pcl::PointXYZRGB>);
+			pcl::copyPointCloud(*cloud_dowm_sampled, *cloud_cand_demo);
+
+			for (auto& pt : *cloud_cand_demo) {
+				pt.r = 255;
+				pt.g = 255;
+				pt.b = 255;
+			}
+			int cnt = 0;
+			for (auto& idx : mass_idx) {
+				int p_idx = first_seedling_cloud_idx[idx];
+				/*if (p_idx == optimal_seeds_idx[selected_pt_idx]) {
+					cloud_cand_demo->points[p_idx].r = 0;
+					cloud_cand_demo->points[p_idx].g = 255;
+					cloud_cand_demo->points[p_idx].b = 0;
+				}
+				else {*/
+					cloud_cand_demo->points[p_idx].r = 255;
+					cloud_cand_demo->points[p_idx].g = 0;
+					cloud_cand_demo->points[p_idx].b = 0;
+				/*}	*/			
+				if (cnt > optimal_seeds_idx.size()) { break; }
+				cnt++;
+			}
+			pcl::PointXYZRGB pt_grab = {0,255,0};
+			pt_grab.x = selected_pt.x;
+			pt_grab.y = selected_pt.y;
+			pt_grab.z = selected_pt.z;
+
+			pcl::PointXYZRGB pt_grab_ = { 0,255,120 };
+			pt_grab_.x = selected_pt.x;
+			pt_grab_.y = selected_pt.y+0.2;
+			pt_grab_.z = selected_pt.z;
+			cloud_cand_demo->push_back(pt_grab);
+
+
+			viewer_cloud(cloud_cand_demo, std::string("cloud_cand_demo"));
+		}
+		return 0;
+	}
+	int CRootStockGrabPoint::read_ply_file(const char* fn)
+	{
+		m_raw_cloud.reset( new pcl::PointCloud<pcl::PointXYZ>);
+		if (pcl::io::loadPLYFile<pcl::PointXYZ>(fn, *m_raw_cloud) == -1) {
+			if (m_pLogger) {
+				m_pLogger->ERRORINFO("could not load file: "+std::string(fn));
+			}
+			return (-1);
+		}
+		if (m_pLogger) {
+			stringstream buff;
+			buff << "load raw point cloud " << m_raw_cloud->width * m_raw_cloud->height << " data points";
+			m_pLogger->INFO(buff.str());
+		}
+		return m_raw_cloud->width * m_raw_cloud->height;
+	}
+	double CRootStockGrabPoint::compute_nearest_neighbor_distance(pcl::PointCloud<pcl::PointXYZ>::Ptr pcd)
+	{
+		pcl::KdTreeFLANN<pcl::PointXYZ> tree;
+		tree.setInputCloud(pcd);
+
+		int k = 2;
+		double res = 0.0;
+		int n_points = 0;
+		for (size_t i = 0; i < pcd->size(); ++i) {
+			std::vector<int> idx(k);
+			std::vector<float> sqr_distances(k);
+
+			if (tree.nearestKSearch(i, k, idx, sqr_distances) == k) {
+				for (int id = 1; id < k; ++id) {
+					res += sqrt(sqr_distances[id]);
+					++n_points;
+				}
+			}
+		}
+		if (n_points > 0) {
+			res /= (double)n_points;
+		}
+		return res;
+	}
+	void CRootStockGrabPoint::find_seedling_position(
+		pcl::PointCloud<pcl::PointXYZ>::Ptr in_cloud,
+		std::vector<int> &first_seedling_cloud_idx,
+		pcl::PointXYZ&xz_center
+	)
+	{
+		pcl::PointCloud<pcl::PointXYZ>::Ptr cloud2d(new pcl::PointCloud < pcl::PointXYZ>);
+		pcl::copyPointCloud(*in_cloud, *cloud2d);
+		for (auto&pt : *cloud2d) {
+			pt.y = 0.0;
+		}
+
+		if(m_cparam.image_show){
+			viewer_cloud(cloud2d, std::string("cloud2d"));
+		}		
+
+		double radius = m_cparam.rs_grab_stem_diameter;
+		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[i], radius, idx, dist_sqr);
+			counter.push_back(k);
+		}
+		int th = (int)(otsu(counter));
+		std::vector<int> index;
+		for (size_t i = 0; i < counter.size(); ++i) {
+			if (counter[i] >= th) {
+				index.push_back(i);
+			}
+		}
+
+		pcl::PointCloud<pcl::PointXYZ>::Ptr cloud2d_pos(new pcl::PointCloud < pcl::PointXYZ>);
+		pcl::copyPointCloud(*cloud2d, index, *cloud2d_pos);
+
+		if (m_pLogger) {
+			stringstream buff;
+			buff << "get 2d seedling seed point cloud " << index.size()<< " data points with thrreshold "<<th;
+			m_pLogger->INFO(buff.str());
+		}
+		if (m_cparam.image_show) {
+			viewer_cloud(cloud2d_pos, std::string("cloud2d_pos"));
+		}
+		
+		//clustering
+		double d1 = m_cparam.rs_grab_stem_diameter;
+		double d2 = m_cparam.rs_grab_stem_diameter * 3.0;
+		std::vector<pcl::PointXYZ>cluster_center;
+		std::vector<std::vector<int>> cluster_member;
+		euclidean_clustering_ttsas(cloud2d_pos, d1, d2, cluster_center, cluster_member);
+
+		if (m_pLogger) {
+			stringstream buff;
+			buff << "euclidean_clustering_ttsas:  " << cluster_center.size() << " t1=" << d1
+				<< " t2=" << d2;
+			m_pLogger->INFO(buff.str());
+		}
+
+		//sort cluster center, get the frontest rightest one
+		std::vector<float> cluster_index;
+		for (auto&pt : cluster_center) {
+			float idx = pt.x - pt.z;
+			cluster_index.push_back(idx);
+		}
+		int first_idx = std::max_element(cluster_index.begin(), cluster_index.end()) - cluster_index.begin();
+
+		first_seedling_cloud_idx.clear();
+		for (auto&v : cluster_member[first_idx]) {
+			size_t i = index[v];
+			first_seedling_cloud_idx.push_back(i);
+		}
+		xz_center.x = cluster_center[first_idx].x;
+		xz_center.y = cluster_center[first_idx].y;
+		xz_center.z = cluster_center[first_idx].z;
+
+		if (m_pLogger) {
+			stringstream buff;
+			buff << "euclidean_clustering_ttsas， find cluster center(" << xz_center.x
+				<<", "<< xz_center.y<<", "<< xz_center.z<<")";
+			m_pLogger->INFO(buff.str());
+		}
+	}
+	void CRootStockGrabPoint::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[i].x;
+			cy = seed_cloud->points[i].y;
+			cz = seed_cloud->points[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
+	)
+	{
+		std::vector<int> cluster_weight;
+		std::vector<int> data_stat;
+
+		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[i] == 0 && new_while_first && exists_change == 0) {
+					new_while_first = false;
+					std::vector<int> idx;
+					idx.push_back(i);
+					clustr_member.push_back(idx);
+					pcl::PointXYZ center;
+					center.x = in_cloud->points[i].x;
+					center.y = in_cloud->points[i].y;
+					center.z = in_cloud->points[i].z;
+
+					cluster_center.push_back(center);
+					data_stat[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.size(); ++j) {
+						std::vector<float> distances_sub;
+						for (size_t jj = 0; jj < clustr_member[j].size(); ++jj) {
+							size_t ele_idx = clustr_member[j][jj];
+							double d = sqrt(
+								(in_cloud->points[i].x - in_cloud->points[ele_idx].x) * (in_cloud->points[i].x - in_cloud->points[ele_idx].x) +
+								(in_cloud->points[i].y - in_cloud->points[ele_idx].y) * (in_cloud->points[i].y - in_cloud->points[ele_idx].y) +
+								(in_cloud->points[i].z - in_cloud->points[ele_idx].z) * (in_cloud->points[i].z - in_cloud->points[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[min_idx] < d1) {
+						data_stat[i] = 1;
+						double w = cluster_weight[min_idx];
+						cluster_weight[min_idx] += 1;
+						clustr_member[min_idx].push_back(i);
+						cluster_center[min_idx].x = (cluster_center[min_idx].x * w + in_cloud->points[i].x) / (w + 1);
+						cluster_center[min_idx].y = (cluster_center[min_idx].y * w + in_cloud->points[i].y) / (w + 1);
+						cluster_center[min_idx].z = (cluster_center[min_idx].z * w + in_cloud->points[i].z) / (w + 1);
+						cur_change += 1;
+					}
+					else if (distances[min_idx] > d2) {
+						std::vector<int> idx;
+						idx.push_back(i);
+						clustr_member.push_back(idx);
+						pcl::PointXYZ center;
+						center.x = in_cloud->points[i].x;
+						center.y = in_cloud->points[i].y;
+						center.z = in_cloud->points[i].z;
+
+						cluster_center.push_back(center);
+						data_stat[i] = 1;
+						cur_change += 1;
+						cluster_weight.push_back(1);
+						m += 1;
+					}
+
+				}
+				else if (data_stat[i] == 1) {
+					cur_change += 1;
+				}
+			}
+			exists_change = fabs(cur_change - prev_change);
+			prev_change = cur_change;
+			cur_change = 0;
+		}
+	}
+	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[i].x;
+			}
+			else {
+				pts(i, 0) = in_cloud->points[i].z;
+			}
+			pts(i, 1) = in_cloud->points[i].y;
+		}
+		// centerlize
+		Eigen::MatrixXd mu = pts.colwise().mean();
+		Eigen::RowVectorXd mu_row = mu;
+		pts.rowwise() -= mu_row;
+
+		//calculate covariance
+		Eigen::MatrixXd C = pts.adjoint()*pts;
+		C = C.array() / (pts.cols() - 1);
+
+
+		//compute eig
+		Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> eig(C);
+		Eigen::MatrixXd d = eig.eigenvalues();
+		Eigen::MatrixXd v = eig.eigenvectors();
+
+		//projection
+		Eigen::MatrixXd p = pts * v;
+		Eigen::VectorXd minv = p.colwise().minCoeff();
+		Eigen::VectorXd maxv = p.colwise().maxCoeff();
+		Eigen::VectorXd range = maxv - minv;
+
+		dy_obb = range(1);
+		dx_obb = range(0);
+		angle_obb = std::atan2(v(1, 1), v(0, 1));
+		if (angle_obb < 0) { angle_obb = PI + angle_obb; }
+		angle_obb = angle_obb * 180 / PI;		
+	}
+
+	void CRootStockGrabPoint::get_optimal_seed(
+		pcl::PointCloud<pcl::PointXYZ>::Ptr in_cloud,
+		pcl::PointXYZ&pt,
+		int &pt_idx)
+	{
+		double d1 = m_cparam.rs_grab_stem_diameter;
+		double d2 = m_cparam.rs_grab_stem_diameter * 4.0;
+		std::vector<pcl::PointXYZ>cluster_center;
+		std::vector<std::vector<int>> cluster_member;
+		euclidean_clustering_ttsas(in_cloud, d1, d2, cluster_center, cluster_member);
+
+		float max_y = -1.0e6;
+		int max_idx = -1;
+		int member_size = 5;
+		for (int i = 0; i < cluster_member.size(); ++i) {
+			if (cluster_member[i].size() < member_size) {
+				continue;
+			}
+			if (cluster_center[i].y > max_y) {
+				max_y = cluster_center[i].y;
+				max_idx = i;
+			}
+		}
+		//find nearest pt
+		float nearest_dist = 1.0e6;
+		int nearest_idx = -1;
+		for (int i = 0; i < cluster_member[max_idx].size(); ++i) {
+			int idx = cluster_member[max_idx][i];
+			float dist = fabs(in_cloud->points[idx].x - cluster_center[max_idx].x) +
+				fabs(in_cloud->points[idx].y - cluster_center[max_idx].y) +
+				fabs(in_cloud->points[idx].z - cluster_center[max_idx].z);
+			if (dist < nearest_dist) {
+				nearest_dist = dist;
+				nearest_idx = idx;
+			}
+		}
+		pt.x = in_cloud->points[nearest_idx].x;
+		pt.y = in_cloud->points[nearest_idx].y;
+		pt.z = in_cloud->points[nearest_idx].z;
+		pt_idx = nearest_idx;
+	}
+	void CRootStockGrabPoint::viewer_cloud(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, std::string&winname)
+	{
+		pcl::visualization::CloudViewer viewer(winname);
+		//viewer.runOnVisualizationThreadOnce(viewerOneOff);	
+		viewer.showCloud(cloud);
+		while (!viewer.wasStopped()) {
+			boost::this_thread::sleep(boost::posix_time::microseconds(100000));
+		}
+	}
+	void CRootStockGrabPoint::viewer_cloud(pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud, std::string&winname)
+	{
+		pcl::visualization::CloudViewer viewer(winname);
+		//viewer.runOnVisualizationThreadOnce(viewerOneOff);	
+		viewer.showCloud(cloud);
+		while (!viewer.wasStopped()) {
+			boost::this_thread::sleep(boost::posix_time::microseconds(100000));
+		}
+	}
+
+};

grab_point_rs.h

@@ -0,0 +1,62 @@
+#pragma once
+
+#include <pcl\point_types.h>
+#include <pcl\point_cloud.h>
+#include "data_def_api.h"
+#include "data_def.h"
+#include "logger.h"
+
+namespace graft_cv {
+	class CRootStockGrabPoint {
+	public:
+		CRootStockGrabPoint(ConfigParam&c, CGcvLogger*pLog = 0);
+		~CRootStockGrabPoint();
+		int grab_point_detect( PositionInfo& posinfo);
+		float* get_raw_point_cloud(int &data_size);
+		int load_data(float*pPoint, int pixel_size, int pt_size, const char* fn = 0);
+	private:
+		//global configure parameters
+		ConfigParam& m_cparam;
+		CGcvLogger * m_pLogger;
+
+		pcl::PointCloud<pcl::PointXYZ>::Ptr m_raw_cloud;
+
+
+		int read_ply_file(const char* fn);
+		double compute_nearest_neighbor_distance(pcl::PointCloud<pcl::PointXYZ>::Ptr);
+		void find_seedling_position(
+			pcl::PointCloud<pcl::PointXYZ>::Ptr in_cloud,
+			std::vector<int> &first_seedling_cloud_idx,
+			pcl::PointXYZ&xz_center
+		);
+		void 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>& idx
+		);
+
+		void 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
+		);
+		void cal_obb_2d(
+			pcl::PointCloud<pcl::PointXYZ>::Ptr in_cloud,
+			int axis,
+			double &dx_obb,
+			double &dy_obb,
+			double &angle_obb);
+
+		void get_optimal_seed(
+			pcl::PointCloud<pcl::PointXYZ>::Ptr,
+			pcl::PointXYZ&pt,
+			int &pt_idx);
+
+		void viewer_cloud(pcl::PointCloud<pcl::PointXYZ>::Ptr, std::string&winname);
+		void viewer_cloud(pcl::PointCloud<pcl::PointXYZRGB>::Ptr, std::string&winname);
+	};
+
+};

graft_cv_api.cpp

@@ -13,11 +13,13 @@
 #include "imstorage_manager.h"
 #include "cut_point_rs_reid.h"
 
+#include "grab_point_rs.h"
+
 extern CRITICAL_SECTION g_cs;
 namespace graft_cv
 {
 
-	char *g_version_str = "0.6.1";
+	char *g_version_str = "0.6.2";
 
 	//configure
 	string g_conf_file = "./gcv_conf.yml";	
@@ -42,6 +44,8 @@ namespace graft_cv
 	CRootStockCutPointReid g_rs_cp_reid = CRootStockCutPointReid(g_cp,&g_logger);
 	CScionCutPoint g_sc_cp = CScionCutPoint(g_cp,&g_logger);
 
+	CRootStockGrabPoint g_rs_gp(g_cp, &g_logger);
+
 	//
 	map<string, cv::Mat> g_img_cache;
 	
@@ -263,6 +267,40 @@ namespace graft_cv
 		}
 		return 0;
 	};*/
+	//
+	int rs_grab_point(
+		float* points,
+		int pixel_size,
+		int pt_size, 
+		PositionInfo& posinfo,
+		const char* fn/*=0*/
+	)
+	{
+		memset(&posinfo, 0, sizeof(PositionInfo));
+		try {
+			int rst = g_rs_gp.load_data(points, pixel_size, pt_size, fn);
+			if (rst <= 0) {
+				g_logger.ERRORINFO("invalid points");
+				return 1;
+			}
+			int oa = g_rs_gp.grab_point_detect(posinfo);
+		}
+		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(

graft_cv_api.h

@@ -62,9 +62,21 @@ GCV_API void cv_get_param(ConfigParam&);
 GCV_API void get_version(char* buf);
 
 
-//11 砧木最优角度识别初始化，每一株均需初始化一次
-//  返回： 0- 正常； 其他- 失败 
-//GCV_API int rs_oa_init();
+//11 砧木上苗,找到抓取位置
+//
+//  输入： points ---      输入， 指向点云的指针，点云为3值一组，结构：[pt0.x, pt0.y, pt0.z, pt1.x, pt1.y,pt1.z], 有时是4值一组（视采集设备数据情况）
+//         pixel_size ---  输入，一个点云的维度（一般3或4）
+//         pt_size-------- 输入， 点云数量，上例中数量为2
+//         posinfo ------- 输出
+//         fn ------------ 输入， points指向0，且fn可用时，读取文件中的数据（用于测试），仅支持ply格式
+//				posinfo.rs_pre_x;
+//				posinfo.rs_pre_y;
+//				posinfo.rs_pre_z;
+//				其余数据为0
+//  返回： 0- 正常； 其他- 失败
+GCV_API int rs_grab_point(float* points, int pixel_size, int pt_size, PositionInfo& posinfo, const char* fn=0);
+
+
 
 //12  砧木最优角度识别,追加图像，要求图像的角度按顺序，第一帧图像角度为0，最后一帧图像角度为180
 //   每次追加图片，通过posinfo返回：

utils.h

@@ -388,6 +388,47 @@ namespace graft_cv{
 		
 		for(size_t j=0;j<hist.size();++j){hist[j]=tmp[j];}
 	}
+	template<typename T>
+	double otsu(std::vector<T>&data)
+	{
+		std::vector<T>data_sort(data);
+		std::sort(data_sort.begin(), data_sort.end());
+		double t0 = data_sort[int(data_sort.size() / 2)];
+		double t1 = data_sort.back();
+		double t = t0;
+		double max_g = 0;
+		double max_t = t0;
+		std::vector<T>d0;
+		std::vector<T>d1;
+		double n = data.size();
+		while (t < t1) {
+			d0.clear();
+			d1.clear();
+			for (auto& v : data_sort) {
+				if (v < t) { d0.push_back(v); }
+				else { d1.push_back(v); }
+			}
+			double g = get_hist_mean(d0) - get_hist_mean(d1);
+			g *= g;
+			g = g* d0.size() * d1.size() / n / n;
+			if (g > max_g) {
+				max_g = g;
+				max_t = t;
+			}
+			t += 1;
+		}
+		return max_t;
+
+	}
+	template<typename T>
+	double get_hist_mean(std::vector<T>&data) {
+		double mu = 0;
+		for (auto &v : data) { mu += v; }
+		if (data.size() > 0) {
+			mu /= data.size();
+		}
+		return mu;
+	}