v0.6.8 修改茄科抓取接口函数和参数，区分砧木和穗苗

ReadMe.txt

@@ -75,4 +75,5 @@ v0.6.3
 v0.6.4 增加算法参数读取文件进行更新的接口
 v0.6.5 修改代码，得到最左侧一株
 v0.6.6 增加剪裁后没有点的异常处理
-v0.6.7 修改点云显示代码，显示抓取点xy方向，及z平面上的xoy坐标系
+v0.6.7 修改点云显示代码，显示抓取点xy方向，及z平面上的xoy坐标系
+v0.6.8 增加穗苗的inbox定义，增加最优茎高的定义；修改vector随机读取[]为at方法；增加接口参数用于定义抓取苗是砧木还是穗苗；修改了接口函数名称

config.cpp

@@ -102,6 +102,16 @@ namespace graft_cv{
 			<< "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
+			<< "rs_grab_y_opt" << m_cparam->rs_grab_y_opt
+
+			<< "sc_grab_xmin" << m_cparam->sc_grab_xmin
+			<< "sc_grab_xmax" << m_cparam->sc_grab_xmax
+			<< "sc_grab_ymin" << m_cparam->sc_grab_ymin
+			<< "sc_grab_ymax" << m_cparam->sc_grab_ymax
+			<< "sc_grab_zmin" << m_cparam->sc_grab_zmin
+			<< "sc_grab_zmax" << m_cparam->sc_grab_zmax
+			<< "sc_grab_stem_diameter" << m_cparam->sc_grab_stem_diameter
+			<< "sc_grab_y_opt" << m_cparam->sc_grab_y_opt
 
 			<< "}"; 	
 	};
@@ -188,6 +198,16 @@ namespace graft_cv{
 		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"];
+		m_cparam->rs_grab_y_opt = (double)node["rs_grab_y_opt"];
+
+		m_cparam->sc_grab_xmin = (double)node["sc_grab_xmin"];
+		m_cparam->sc_grab_xmax = (double)node["sc_grab_xmax"];
+		m_cparam->sc_grab_ymin = (double)node["sc_grab_ymin"];
+		m_cparam->sc_grab_ymax = (double)node["sc_grab_ymax"];
+		m_cparam->sc_grab_zmin = (double)node["sc_grab_zmin"];
+		m_cparam->sc_grab_zmax = (double)node["sc_grab_zmax"];
+		m_cparam->sc_grab_stem_diameter = (double)node["sc_grab_stem_diameter"];
+		m_cparam->sc_grab_y_opt = (double)node["sc_grab_y_opt"];
     
   }
 	string get_cparam_info(ConfigParam*m_cparam)
@@ -279,6 +299,16 @@ namespace graft_cv{
 			<< "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
+			<< "rs_grab_y_opt:\t" << m_cparam->rs_grab_y_opt << endl
+
+			<< "sc_grab_xmin:\t" << m_cparam->sc_grab_xmin << endl
+			<< "sc_grab_xmax:\t" << m_cparam->sc_grab_xmax << endl
+			<< "sc_grab_ymin:\t" << m_cparam->sc_grab_ymin << endl
+			<< "sc_grab_ymax:\t" << m_cparam->sc_grab_ymax << endl
+			<< "sc_grab_zmin:\t" << m_cparam->sc_grab_zmin << endl
+			<< "sc_grab_zmax:\t" << m_cparam->sc_grab_zmax << endl
+			<< "sc_grab_stem_diameter:\t" << m_cparam->sc_grab_stem_diameter << endl
+			<< "sc_grab_y_opt:\t" << m_cparam->sc_grab_y_opt << endl
 
 			<< "}" << endl; 	
 		return buff.str();

data_def_api.h

@@ -112,6 +112,18 @@ typedef struct{
 	double rs_grab_zmax;
 
 	double rs_grab_stem_diameter;	
+	double rs_grab_y_opt;
+
+	// scion grab based points cloud
+	double sc_grab_xmin;
+	double sc_grab_xmax;
+	double sc_grab_ymin;
+	double sc_grab_ymax;
+	double sc_grab_zmin;
+	double sc_grab_zmax;
+
+	double sc_grab_stem_diameter;
+	double sc_grab_y_opt;
 
 
 

gcv_conf.yml

@@ -72,4 +72,14 @@ conf_parameters:
    rs_grab_zmin: 200
    rs_grab_zmax: 320
    rs_grab_stem_diameter: 5.0
+   rs_grab_y_opt: -20
+   sc_grab_xmin: -200
+   sc_grab_xmax: 200
+   sc_grab_ymin: -150
+   sc_grab_ymax: 150
+   sc_grab_zmin: 200
+   sc_grab_zmax: 320
+   sc_grab_stem_diameter: 5.0
+   sc_grab_y_opt: -20
+   
 

grab_point_rs.cpp

@@ -27,7 +27,8 @@ namespace graft_cv {
 
 	CRootStockGrabPoint::CRootStockGrabPoint(ConfigParam&cp, CGcvLogger*pLog/*=0*/)
 		:m_cparam(cp),
-		m_pLogger(pLog)
+		m_pLogger(pLog),
+		m_dtype(0)
 	{
 	}
 
@@ -84,16 +85,36 @@ namespace graft_cv {
 		return rst;
 	}
 
-	int CRootStockGrabPoint::grab_point_detect(		
+	int CRootStockGrabPoint::grab_point_detect(	
+		int dtype,
 		PositionInfo& posinfo
 		)
 	{
-		
+		// dtype == 0, rootstock
+		// dtype != 0, scion
+		if (m_raw_cloud->width * m_raw_cloud->height < 1) {
+			if (m_pLogger) {
+				stringstream buff;
+				buff << "m_raw_cloud point cloud " << m_raw_cloud->width * m_raw_cloud->height << " data points";
+				m_pLogger->ERRORINFO(buff.str());
+			}
+			return 1;
+		}
 		//2 crop filter
+		if (m_pLogger) {
+			m_pLogger->INFO("begin crop");			
+		}
 		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));
+		m_dtype = dtype;
+		if (dtype == 0) {
+			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 {
+			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);
@@ -110,9 +131,18 @@ namespace graft_cv {
 		if (m_cparam.image_show) {
 			viewer_cloud(cloud_inbox, std::string("cloud_inbox"));
 		}
+		if (m_pLogger) {
+			m_pLogger->INFO("end crop");
+		}
 		//3 filtler with radius remove
+		if (m_pLogger) {
+			m_pLogger->INFO("begin ror");
+		}
 		int nb_points = 20;
 		double stem_radius = m_cparam.rs_grab_stem_diameter / 2.0;
+		if(dtype != 0){
+			stem_radius = m_cparam.sc_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);
@@ -133,8 +163,14 @@ namespace graft_cv {
 		if (m_cparam.image_show) {
 			viewer_cloud(cloud_ror, std::string("cloud_ror"));
 		}
+		if (m_pLogger) {
+			m_pLogger->INFO("end ror");
+		}
 
 		//3 voxel grid down sampling
+		if (m_pLogger) {
+			m_pLogger->INFO("begin down");
+		}
 		pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_dowm_sampled(new pcl::PointCloud<pcl::PointXYZ>);
 		pcl::VoxelGrid<pcl::PointXYZ> outrem;
 		outrem.setInputCloud(cloud_ror);
@@ -146,40 +182,66 @@ namespace graft_cv {
 			buff << "VoxelGrid dowm_sampled point cloud " << cloud_dowm_sampled->width * cloud_dowm_sampled->height << " data points";
 			m_pLogger->INFO(buff.str());
 		}
+		if (cloud_dowm_sampled->width * cloud_dowm_sampled->height < 1) {
+			return 1;
+		}
 
 		if (m_cparam.image_show) {
 			viewer_cloud(cloud_dowm_sampled, std::string("cloud_dowm_sampled"));
 		}
+		if (m_pLogger) {
+			m_pLogger->INFO("end down");
+		}
 		//4 seedling position
 		std::vector<int> first_seedling_cloud_idx;
 		pcl::PointXYZ xz_center;
+		if (m_pLogger) {
+			m_pLogger->INFO("begin find seedling position");
+		}
 		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());
 		}
+		if (m_pLogger) {
+			m_pLogger->INFO("end find seedling position");
+		}
 		//5 nearest seedling grab point selection
 		pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_seedling_seed(new pcl::PointCloud<pcl::PointXYZ>);
 		pcl::copyPointCloud(*cloud_dowm_sampled, first_seedling_cloud_idx, *cloud_seedling_seed);
 		std::vector<int>mass_idx;
 		double dist_mean = compute_nearest_neighbor_distance(cloud_dowm_sampled);
 		std::vector<double> mass_indices;
+		if (m_pLogger) {
+			m_pLogger->INFO("begin crop nn_analysis");
+		}
 		crop_nn_analysis(cloud_ror, cloud_seedling_seed, dist_mean, mass_indices, mass_idx);
-		
+		if (m_pLogger) {
+			m_pLogger->INFO("end crop nn_analysis");
+		}
+
 		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]);
+			if (mass_indices.at(mass_idx.at(j)) >= candidate_th) {
+				optimal_seeds_idx.push_back(mass_idx.at(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;
+		if (m_pLogger) {
+			m_pLogger->INFO("begin get_optimal_seed");
+		}
 		get_optimal_seed(cloud_optimal_seed, selected_pt, selected_pt_idx);
-
+		if (selected_pt_idx < 0) {			
+			return 1;
+		}
+		if (m_pLogger) {
+			m_pLogger->INFO("end get_optimal_seed");
+		}
 		posinfo.rs_grab_x = selected_pt.x;
 		posinfo.rs_grab_y = selected_pt.y;
 		posinfo.rs_grab_z = selected_pt.z;
@@ -197,16 +259,16 @@ namespace graft_cv {
 			}
 			int cnt = 0;
 			for (auto& idx : mass_idx) {
-				int p_idx = first_seedling_cloud_idx[idx];
+				int p_idx = first_seedling_cloud_idx.at(idx);
 				/*if (p_idx == optimal_seeds_idx[selected_pt_idx]) {
 					cloud_cand_demo->points[p_idx].r = 0;
 					cloud_cand_demo->points[p_idx].g = 255;
 					cloud_cand_demo->points[p_idx].b = 0;
 				}
 				else {*/
-					cloud_cand_demo->points[p_idx].r = 255;
-					cloud_cand_demo->points[p_idx].g = 0;
-					cloud_cand_demo->points[p_idx].b = 0;
+					cloud_cand_demo->points.at(p_idx).r = 255;
+					cloud_cand_demo->points.at(p_idx).g = 0;
+					cloud_cand_demo->points.at(p_idx).b = 0;
 				/*}	*/			
 				if (cnt > optimal_seeds_idx.size()) { break; }
 				cnt++;
@@ -258,7 +320,7 @@ namespace graft_cv {
 
 			if (tree.nearestKSearch(i, k, idx, sqr_distances) == k) {
 				for (int id = 1; id < k; ++id) {
-					res += sqrt(sqr_distances[id]);
+					res += sqrt(sqr_distances.at(id));
 					++n_points;
 				}
 			}
@@ -285,19 +347,22 @@ namespace graft_cv {
 		}		
 
 		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[i], radius, idx, dist_sqr);
+			int k = kdtree.radiusSearch(cloud2d->points.at(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) {
+			if (counter.at(i) >= th) {
 				index.push_back(i);
 			}
 		}
@@ -317,6 +382,10 @@ namespace graft_cv {
 		//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);
@@ -337,13 +406,13 @@ namespace graft_cv {
 		int first_idx = std::min_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];
+		for (auto&v : cluster_member.at(first_idx)) {
+			size_t i = index.at(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;
+		xz_center.x = cluster_center.at(first_idx).x;
+		xz_center.y = cluster_center.at(first_idx).y;
+		xz_center.z = cluster_center.at(first_idx).z;
 
 		if (m_pLogger) {
 			stringstream buff;
@@ -374,9 +443,9 @@ namespace graft_cv {
 		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;
+			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));
@@ -438,6 +507,11 @@ namespace graft_cv {
 		std::vector<std::vector<int>> &clustr_member
 	)
 	{
+		if (m_pLogger) {
+			stringstream buff;
+			buff << "euclidean_clustering_ttsas() begin...";
+			m_pLogger->INFO(buff.str());
+		}
 		std::vector<int> cluster_weight;
 		std::vector<int> data_stat;
 
@@ -450,18 +524,18 @@ namespace graft_cv {
 		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) {
+				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.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;
+					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.push_back(center);
-					data_stat[i] = 1;
+					data_stat.at(i) = 1;
 					cur_change += 1;
 					cluster_weight.push_back(1);
 					m += 1;
@@ -470,46 +544,46 @@ namespace graft_cv {
 					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];
+						for (size_t jj = 0; jj < clustr_member.at(j).size(); ++jj) {
+							size_t ele_idx = clustr_member.at(j).at(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));
+								(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[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);
+					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.at(min_idx).push_back(i);
+						cluster_center.at(min_idx).x = (cluster_center.at(min_idx).x * w + in_cloud->points.at(i).x) / (w + 1);
+						cluster_center.at(min_idx).y = (cluster_center.at(min_idx).y * w + in_cloud->points.at(i).y) / (w + 1);
+						cluster_center.at(min_idx).z = (cluster_center.at(min_idx).z * w + in_cloud->points.at(i).z) / (w + 1);
 						cur_change += 1;
 					}
-					else if (distances[min_idx] > d2) {
+					else if (distances.at(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;
+						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.push_back(center);
-						data_stat[i] = 1;
+						data_stat.at(i) = 1;
 						cur_change += 1;
 						cluster_weight.push_back(1);
 						m += 1;
 					}
 
 				}
-				else if (data_stat[i] == 1) {
+				else if (data_stat.at(i)== 1) {
 					cur_change += 1;
 				}
 			}
@@ -517,6 +591,11 @@ namespace graft_cv {
 			prev_change = cur_change;
 			cur_change = 0;
 		}
+		if (m_pLogger) {
+			stringstream buff;
+			buff << "euclidean_clustering_ttsas() end";
+			m_pLogger->INFO(buff.str());
+		}
 	}
 	void CRootStockGrabPoint::cal_obb_2d(
 		pcl::PointCloud<pcl::PointXYZ>::Ptr in_cloud,
@@ -530,12 +609,12 @@ namespace graft_cv {
 		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;
+				pts(i, 0) = in_cloud->points.at(i).x;
 			}
 			else {
-				pts(i, 0) = in_cloud->points[i].z;
+				pts(i, 0) = in_cloud->points.at(i).z;
 			}
-			pts(i, 1) = in_cloud->points[i].y;
+			pts(i, 1) = in_cloud->points.at(i).y;
 		}
 		// centerlize
 		Eigen::MatrixXd mu = pts.colwise().mean();
@@ -565,46 +644,119 @@ namespace graft_cv {
 		angle_obb = angle_obb * 180 / PI;		
 	}
 
+	//void CRootStockGrabPoint::get_optimal_seed(
+	//	pcl::PointCloud<pcl::PointXYZ>::Ptr in_cloud,
+	//	pcl::PointXYZ&pt,
+	//	int &pt_idx)
+	//{
+	//	double d1 = m_cparam.rs_grab_stem_diameter;
+	//	double d2 = m_cparam.rs_grab_stem_diameter * 4.0;
+	//	if (m_dtype != 0) {
+	//		d1 = m_cparam.sc_grab_stem_diameter;
+	//		d2 = m_cparam.sc_grab_stem_diameter * 4.0;
+	//	}
+	//	std::vector<pcl::PointXYZ>cluster_center;
+	//	std::vector<std::vector<int>> cluster_member;
+	//	euclidean_clustering_ttsas(in_cloud, d1, d2, cluster_center, cluster_member);
+
+	//	float min_y_dist = 1.0e6;
+	//	int opt_idx = -1;
+	//	int member_size = 5;
+	//	float y_opt = m_cparam.rs_grab_y_opt;
+	//	if (m_dtype != 0) {
+	//		y_opt = m_cparam.sc_grab_y_opt;
+	//	}
+	//	for (int i = 0; i < cluster_member.size(); ++i) {
+	//		if (cluster_member.at(i).size() < member_size) {
+	//			continue;
+	//		}
+	//		if (fabs(cluster_center.at(i).y -y_opt) < min_y_dist) {
+	//			min_y_dist = fabs(cluster_center.at(i).y - y_opt);
+	//			opt_idx = i;
+	//		}
+	//	}
+	//	if (opt_idx < 0) {
+	//		if (m_pLogger) {
+	//			stringstream buff;
+	//			buff << "get_optimal_seed failed, get invalid optimal cluster id";
+	//			m_pLogger->ERRORINFO(buff.str());
+	//		}
+	//		return;
+	//	}
+	//	//find nearest pt
+	//	float nearest_dist = 1.0e6;
+	//	int nearest_idx = -1;
+	//	for (int i = 0; i < cluster_member.at(opt_idx).size(); ++i) {
+	//		int idx = cluster_member.at(opt_idx).at(i);
+	//		float dist = fabs(in_cloud->points.at(idx).x - cluster_center.at(opt_idx).x) +
+	//			fabs(in_cloud->points.at(idx).y - cluster_center.at(opt_idx).y) +
+	//			fabs(in_cloud->points.at(idx).z - cluster_center.at(opt_idx).z);
+	//		if (dist < nearest_dist) {
+	//			nearest_dist = dist;
+	//			nearest_idx = idx;
+	//		}
+	//	}
+	//	pt.x = in_cloud->points.at(nearest_idx).x;
+	//	pt.y = in_cloud->points.at(nearest_idx).y;
+	//	pt.z = in_cloud->points.at(nearest_idx).z;
+	//	pt_idx = nearest_idx;
+	//}
 	void CRootStockGrabPoint::get_optimal_seed(
 		pcl::PointCloud<pcl::PointXYZ>::Ptr in_cloud,
 		pcl::PointXYZ&pt,
 		int &pt_idx)
 	{
-		double d1 = m_cparam.rs_grab_stem_diameter;
+		/*double d1 = m_cparam.rs_grab_stem_diameter;
 		double d2 = m_cparam.rs_grab_stem_diameter * 4.0;
+		if (m_dtype != 0) {
+			d1 = m_cparam.sc_grab_stem_diameter;
+			d2 = m_cparam.sc_grab_stem_diameter * 4.0;
+		}
 		std::vector<pcl::PointXYZ>cluster_center;
 		std::vector<std::vector<int>> cluster_member;
-		euclidean_clustering_ttsas(in_cloud, d1, d2, cluster_center, cluster_member);
+		euclidean_clustering_ttsas(in_cloud, d1, d2, cluster_center, cluster_member);*/
 
-		float max_y = -1.0e6;
-		int max_idx = -1;
+		float min_y_dist = 1.0e6;
+		int opt_idx = -1;
 		int member_size = 5;
-		for (int i = 0; i < cluster_member.size(); ++i) {
-			if (cluster_member[i].size() < member_size) {
+		float y_opt = m_cparam.rs_grab_y_opt;
+		if (m_dtype != 0) {
+			y_opt = m_cparam.sc_grab_y_opt;
+		}
+		for (int i = 0; i < in_cloud->points.size(); ++i) {
+			/*if (cluster_member.at(i).size() < member_size) {
 				continue;
+			}*/
+			if (fabs(in_cloud->points.at(i).y - y_opt) < min_y_dist) {
+				min_y_dist = fabs(in_cloud->points.at(i).y - y_opt);
+				opt_idx = i;
 			}
-			if (cluster_center[i].y > max_y) {
-				max_y = cluster_center[i].y;
-				max_idx = i;
+		}
+		if (opt_idx < 0) {
+			if (m_pLogger) {
+				stringstream buff;
+				buff << "get_optimal_seed failed, get invalid optimal cluster id";
+				m_pLogger->ERRORINFO(buff.str());
 			}
+			return;
 		}
 		//find nearest pt
-		float nearest_dist = 1.0e6;
+		/*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);
+		for (int i = 0; i < cluster_member.at(opt_idx).size(); ++i) {
+			int idx = cluster_member.at(opt_idx).at(i);
+			float dist = fabs(in_cloud->points.at(idx).x - cluster_center.at(opt_idx).x) +
+				fabs(in_cloud->points.at(idx).y - cluster_center.at(opt_idx).y) +
+				fabs(in_cloud->points.at(idx).z - cluster_center.at(opt_idx).z);
 			if (dist < nearest_dist) {
 				nearest_dist = dist;
 				nearest_idx = idx;
 			}
-		}
-		pt.x = in_cloud->points[nearest_idx].x;
-		pt.y = in_cloud->points[nearest_idx].y;
-		pt.z = in_cloud->points[nearest_idx].z;
-		pt_idx = nearest_idx;
+		}*/
+		pt.x = in_cloud->points.at(opt_idx).x;
+		pt.y = in_cloud->points.at(opt_idx).y;
+		pt.z = in_cloud->points.at(opt_idx).z;
+		pt_idx = opt_idx;
 	}
 	void CRootStockGrabPoint::viewer_cloud(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, std::string&winname)
 	{

grab_point_rs.h

@@ -11,13 +11,14 @@ namespace graft_cv {
 	public:
 		CRootStockGrabPoint(ConfigParam&c, CGcvLogger*pLog = 0);
 		~CRootStockGrabPoint();
-		int grab_point_detect( PositionInfo& posinfo);
+		int grab_point_detect( int dtype, 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;
+		int m_dtype;
 
 		pcl::PointCloud<pcl::PointXYZ>::Ptr m_raw_cloud;
 

graft_cv_api.cpp

@@ -19,7 +19,7 @@ extern CRITICAL_SECTION g_cs;
 namespace graft_cv
 {
 
-	char *g_version_str = "0.6.7";
+	char *g_version_str = "0.6.8";
 
 	//configure
 	string g_conf_file = "./gcv_conf.yml";	
@@ -293,10 +293,11 @@ namespace graft_cv
 		return 0;
 	};*/
 	//
-	int rs_grab_point(
+	int sola_grab_point(
 		float* points,
 		int pixel_size,
 		int pt_size, 
+		int dtype,
 		PositionInfo& posinfo,
 		const char* fn/*=0*/
 	)
@@ -308,7 +309,7 @@ namespace graft_cv
 				g_logger.ERRORINFO("invalid points");
 				return 1;
 			}
-			int oa = g_rs_gp.grab_point_detect(posinfo);
+			int oa = g_rs_gp.grab_point_detect(dtype, posinfo);
 			if (oa != 0) {
 				g_logger.ERRORINFO("no points");
 				return 1;

graft_cv_api.h

@@ -63,11 +63,13 @@ GCV_API void cv_get_param(ConfigParam&);
 GCV_API void get_version(char* buf);
 
 
-//11 砧木上苗,找到抓取位置
+//11 茄科上苗,找到抓取位置
 //
 //  输入： points ---      输入， 指向点云的指针，点云为3值一组，结构：[pt0.x, pt0.y, pt0.z, pt1.x, pt1.y,pt1.z], 有时是4值一组（视采集设备数据情况）
 //         pixel_size ---  输入，一个点云的维度（一般3或4）
 //         pt_size-------- 输入， 点云数量，上例中数量为2
+//         dtype  -------- 输入，砧木--0， 穗苗--1
+//
 //         posinfo ------- 输出
 //         fn ------------ 输入， points指向0，且fn可用时，读取文件中的数据（用于测试），仅支持ply格式
 //				posinfo.rs_pre_x;
@@ -75,7 +77,7 @@ GCV_API void get_version(char* buf);
 //				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);
+GCV_API int sola_grab_point(float* points, int pixel_size, int pt_size, int dtype, PositionInfo& posinfo, const char* fn=0);