v0.6.13 增加聚类结果中点数小于20的类别剔除功能；修改降采样后整个点云数量小于50退出（原来是200）

ReadMe.txt

@@ -80,4 +80,5 @@ v0.6.8
 v0.6.9 修改抓取砧木、穗苗的定义0--穗苗，1--砧木；穗苗和砧木抓取顺序相反
 v0.6.10 重写砧木、穗苗切后重识别算法，现在用切后侧面图，输出夹爪基面y值、切后3点y与基面y值的距离
 v0.6.11 增加茄科切后重识别异常退出判断
-v0.6.12 增加茄科抓取目标大小判断，太小的目标跳出；增加自动ply数据保存功能；茄科切后重识别图片保存功能
+v0.6.12 增加茄科抓取目标大小判断，太小的目标跳出；增加自动ply数据保存功能；茄科切后重识别图片保存功能
+v0.6.13 增加聚类结果中点数小于20的类别剔除功能；修改降采样后整个点云数量小于50退出（原来是200）

gcv_conf.yml

@@ -67,19 +67,19 @@ conf_parameters:
    sc_cut_pixel_ratio: 8.7499999999999994e-002
    rs_grab_xmin: -200
    rs_grab_xmax: 200
-   rs_grab_ymin: 0
+   rs_grab_ymin: -45
    rs_grab_ymax: 50
-   rs_grab_zmin: 200
-   rs_grab_zmax: 320
+   rs_grab_zmin: 300
+   rs_grab_zmax: 450
    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_ymin: -45
+   sc_grab_ymax: 50
+   sc_grab_zmin: 300
+   sc_grab_zmax: 450
    sc_grab_stem_diameter: 5.0
-   sc_grab_y_opt: -20
+   sc_grab_y_opt: -30
    
 

grab_point_rs.cpp

@@ -16,6 +16,8 @@
 #include <pcl\filters\radius_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 <math.h>
 
 #include "grab_point_rs.h"
@@ -196,7 +198,7 @@ namespace graft_cv {
 			buff << "VoxelGrid dowm_sampled point cloud " << cloud_dowm_sampled->width * cloud_dowm_sampled->height << " data points (if < 200, return)";
 			m_pLogger->INFO(buff.str());
 		}
-		if (cloud_dowm_sampled->width * cloud_dowm_sampled->height < 200) {
+		if (cloud_dowm_sampled->width * cloud_dowm_sampled->height < 50) {
 			return 1;
 		}
 
@@ -212,6 +214,7 @@ namespace graft_cv {
 		if (m_pLogger) {
 			m_pLogger->INFO("begin find seedling position");
 		}
+		//find_seedling_position_line_based(cloud_ror, first_seedling_cloud_idx, xz_center);
 		find_seedling_position(cloud_dowm_sampled, first_seedling_cloud_idx, xz_center);
 		if (m_pLogger) {
 			stringstream buff;
@@ -344,6 +347,115 @@ namespace graft_cv {
 		}
 		return res;
 	}
+	//void CRootStockGrabPoint::find_tray_top_edge(
+	//pcl::PointCloud<pcl::PointXYZ>::Ptr in_cloud,
+	//float & tray_top_edge_y
+	//)
+	//{
+	//	tray_top_edge_y = -1000.0;
+	//	//以毫米为单位构建vector
+	//	pcl::PointXYZ min_point_aabb;
+	//	pcl::PointXYZ max_point_aabb;
+	//	pcl::MomentOfInertiaEstimation<pcl::PointXYZ> feature_extractor;
+	//	feature_extractor.setInputCloud(in_cloud);
+	//	feature_extractor.compute();
+	//	feature_extractor.getAABB(min_point_aabb, max_point_aabb);
+	//	float miny = min_point_aabb.y;
+	//	float maxy = max_point_aabb.y;
+	//	if(maxy - miny <5.0){
+	//		tray_top_edge_y = maxy;
+	//		return;
+	//	}
+	//	std::vector<int> y_cnt_hist(int(maxy - miny), 0);
+	//	for(auto& pt : in_cloud->points){
+	//		int idx =  (int)(pt.y - miny);
+	//		if(idx<0){continue;}
+	//		if (idx >= y_cnt_hist.size()) { continue; }
+	//		y_cnt_hist.at(idx) += 1;
+	//	}
+	//	//从上半部分找到最大值，作为平面上沿
+	//	int idx_part = (int)(y_cnt_hist.size() / 2);
+	//	int idx_edge = std::max_element(y_cnt_hist.begin(), y_cnt_hist.begin() + idx_part) - y_cnt_hist.begin();
+	//	tray_top_edge_y = miny + (float)(idx_edge + 2.0);
+	//}
+
+	//void CRootStockGrabPoint::find_seedling_position_line_based(
+	//	pcl::PointCloud<pcl::PointXYZ>::Ptr in_cloud,
+	//	std::vector<int> &first_seedling_cloud_idx,
+	//	pcl::PointXYZ&xz_center
+	//)
+	//{
+	//	//找到穴盘上沿z，最优抓取的z，再在最优抓取的z基础上加上3作为有效范围
+	//	float tray_y = -1000.0;
+	//	float top_y = 0.0;
+	//	find_tray_top_edge(in_cloud, tray_y);
+	//	if (tray_y < -500.0) {
+	//		if (m_dtype == 0) {
+	//			//scion
+	//			tray_y = m_cparam.sc_grab_y_opt-2.0;
+	//		}
+	//		else {
+	//			tray_y = m_cparam.rs_grab_y_opt-2.0;
+	//		}			
+	//	}
+	//	//up limit		
+	//	if (m_dtype == 0) {
+	//		top_y = m_cparam.sc_grab_zmax;
+	//		if (top_y > m_cparam.sc_grab_y_opt + 3.0) {
+	//			top_y = m_cparam.sc_grab_y_opt + 3.0;
+	//		}
+	//	}
+	//	else {
+	//		top_y = m_cparam.rs_grab_zmax;
+	//		if (top_y > m_cparam.rs_grab_y_opt + 3.0) {
+	//			top_y = m_cparam.rs_grab_y_opt + 3.0;
+	//		}
+	//	}
+	//	//sub cloud
+	//	pcl::PointCloud<pcl::PointXYZ>::Ptr sub_cloud(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, tray_y, m_cparam.rs_grab_zmin, 1));
+	//		box_filter.setMax(Eigen::Vector4f(m_cparam.rs_grab_xmax, top_y, m_cparam.rs_grab_zmax, 1));
+	//	}
+	//	else {//scion
+	//		box_filter.setMin(Eigen::Vector4f(m_cparam.sc_grab_xmin, tray_y, m_cparam.sc_grab_zmin, 1));
+	//		box_filter.setMax(Eigen::Vector4f(m_cparam.sc_grab_xmax, top_y, m_cparam.sc_grab_zmax, 1));
+	//	}
+	//	box_filter.setNegative(false);
+	//	box_filter.setInputCloud(in_cloud);
+	//	box_filter.filter(*sub_cloud);
+
+	//	if (m_cparam.image_show) {			
+	//		viewer_cloud(sub_cloud, std::string("sub inbox"));
+	//	}
+
+ //       //在sub_cloud中进行直线检测
+	//	segement_line(sub_cloud);
+
+	//}
+	//void CRootStockGrabPoint::segement_line(
+	//	pcl::PointCloud<pcl::PointXYZ>::Ptr in_cloud
+	//)
+	//{
+	//	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.setMethodType(pcl::SAC_RANSAC);
+	//	seg.setDistanceThreshold(0.5);
+	//	seg.setInputCloud(in_cloud);
+	//	seg.segment(*inliers, *coefficients);
+
+	//	if (m_cparam.image_show) {
+	//		pcl::PointCloud<pcl::PointXYZ>::Ptr line_cloud(new pcl::PointCloud<pcl::PointXYZ>);
+	//		pcl::copyPointCloud(*in_cloud, *inliers, *line_cloud);
+	//		viewer_cloud(line_cloud, std::string("cloud_line"));
+	//	}
+
+	//}
+	////////////////////////////////////////////////////////////
 	void CRootStockGrabPoint::find_seedling_position(
 		pcl::PointCloud<pcl::PointXYZ>::Ptr in_cloud,
 		std::vector<int> &first_seedling_cloud_idx,
@@ -532,6 +644,9 @@ namespace graft_cv {
 		std::vector<int> cluster_weight;
 		std::vector<int> data_stat;
 
+		std::vector<pcl::PointXYZ>cluster_center_raw;
+		std::vector<std::vector<int>> clustr_member_raw;
+
 		for (size_t i = 0; i < in_cloud->points.size(); ++i) { data_stat.push_back(0); }
 		size_t data_len = in_cloud->points.size();
 		int exists_change = 0;
@@ -545,13 +660,13 @@ namespace graft_cv {
 					new_while_first = false;
 					std::vector<int> idx;
 					idx.push_back(i);
-					clustr_member.push_back(idx);
+					clustr_member_raw.push_back(idx);
 					pcl::PointXYZ center;
 					center.x = in_cloud->points.at(i).x;
 					center.y = in_cloud->points.at(i).y;
 					center.z = in_cloud->points.at(i).z;
 
-					cluster_center.push_back(center);
+					cluster_center_raw.push_back(center);
 					data_stat.at(i) = 1;
 					cur_change += 1;
 					cluster_weight.push_back(1);
@@ -559,10 +674,10 @@ namespace graft_cv {
 				}
 				else if (data_stat[i] == 0) {
 					std::vector<float> distances;
-					for (size_t j = 0; j < clustr_member.size(); ++j) {
+					for (size_t j = 0; j < clustr_member_raw.size(); ++j) {
 						std::vector<float> distances_sub;
-						for (size_t jj = 0; jj < clustr_member.at(j).size(); ++jj) {
-							size_t ele_idx = clustr_member.at(j).at(jj);
+						for (size_t jj = 0; jj < clustr_member_raw.at(j).size(); ++jj) {
+							size_t ele_idx = clustr_member_raw.at(j).at(jj);
 							double d = sqrt(
 								(in_cloud->points.at(i).x - in_cloud->points.at(ele_idx).x) * (in_cloud->points.at(i).x - in_cloud->points.at(ele_idx).x) +
 								(in_cloud->points.at(i).y - in_cloud->points.at(ele_idx).y) * (in_cloud->points.at(i).y - in_cloud->points.at(ele_idx).y) +
@@ -577,22 +692,22 @@ namespace graft_cv {
 						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);
+						clustr_member_raw.at(min_idx).push_back(i);
+						cluster_center_raw.at(min_idx).x = (cluster_center_raw.at(min_idx).x * w + in_cloud->points.at(i).x) / (w + 1);
+						cluster_center_raw.at(min_idx).y = (cluster_center_raw.at(min_idx).y * w + in_cloud->points.at(i).y) / (w + 1);
+						cluster_center_raw.at(min_idx).z = (cluster_center_raw.at(min_idx).z * w + in_cloud->points.at(i).z) / (w + 1);
 						cur_change += 1;
 					}
 					else if (distances.at(min_idx) > d2) {
 						std::vector<int> idx;
 						idx.push_back(i);
-						clustr_member.push_back(idx);
+						clustr_member_raw.push_back(idx);
 						pcl::PointXYZ center;
 						center.x = in_cloud->points.at(i).x;
 						center.y = in_cloud->points.at(i).y;
 						center.z = in_cloud->points.at(i).z;
 
-						cluster_center.push_back(center);
+						cluster_center_raw.push_back(center);
 						data_stat.at(i) = 1;
 						cur_change += 1;
 						cluster_weight.push_back(1);
@@ -608,6 +723,13 @@ namespace graft_cv {
 			prev_change = cur_change;
 			cur_change = 0;
 		}
+
+		// copy result
+		for (size_t i = 0; i < clustr_member_raw.size(); ++i) {
+			if (clustr_member_raw.at(i).size() < 20) { continue; }
+			cluster_center.push_back(cluster_center_raw.at(i));
+			clustr_member.push_back(clustr_member_raw.at(i));
+		}
 		if (m_pLogger) {
 			stringstream buff;
 			buff << "euclidean_clustering_ttsas() end";

grab_point_rs.h

@@ -30,6 +30,23 @@ namespace graft_cv {
 
 		int read_ply_file(const char* fn);
 		double compute_nearest_neighbor_distance(pcl::PointCloud<pcl::PointXYZ>::Ptr);
+
+		////////////////////////////////////////////////
+		//---> 2023-8-8ÓÅ»¯£¬seedling orderʶ±ð´íÎóÎÊÌâ
+		/*void CRootStockGrabPoint::find_tray_top_edge(
+			pcl::PointCloud<pcl::PointXYZ>::Ptr in_cloud,
+			float & tray_top_edge_y
+		);
+		void CRootStockGrabPoint::find_seedling_position_line_based(
+			pcl::PointCloud<pcl::PointXYZ>::Ptr in_cloud,
+			std::vector<int> &first_seedling_cloud_idx,
+			pcl::PointXYZ&xz_center
+		);
+		void segement_line(
+			pcl::PointCloud<pcl::PointXYZ>::Ptr in_cloud
+		);*/
+		//<---------
+		/////////////////////////////////////////////////
 		void find_seedling_position(
 			pcl::PointCloud<pcl::PointXYZ>::Ptr in_cloud,
 			std::vector<int> &first_seedling_cloud_idx,

graft_cv_api.cpp

@@ -19,7 +19,7 @@ extern CRITICAL_SECTION g_cs;
 namespace graft_cv
 {
 
-	char *g_version_str = "0.6.12";
+	char *g_version_str = "0.6.13";
 
 	//configure
 	string g_conf_file = "./gcv_conf.yml";