v0.7.5 优化叶片剔除功能（欧式距离聚类没能分割叶片和茎造成误剔除，本次采用ror方法剔除叶片，但有些耗时）

ReadMe.txt

@@ -89,4 +89,5 @@ v0.7.0 支
 v0.7.1 增加voxelsize参数；修改茎密度最大值的1/3为最小茎密度的下限（原来为1/2，有些小苗识别不出来）
 v0.7.2 增加结果图片输出功能；日志增加image_id信息
 v0.7.3 修改抓取位置，提供茎节分叉的位置（以前提供可抓取的位置，避开茎节的点）
-v0.7.4 增加叶片剔除功能（在boxcrop和降采样后实现）
+v0.7.4 增加叶片剔除功能（在boxcrop和降采样后实现,直接采用欧式距离聚类）
+v0.7.5 优化叶片剔除功能（欧式距离聚类没能分割叶片和茎造成误剔除，本次采用ror方法剔除叶片，但有些耗时）

grab_point_rs.cpp

@@ -231,7 +231,9 @@ namespace graft_cv {
 		// 4 对截取的点云进行聚类分析，剔除叶片
 		pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_dowm_sampled(new pcl::PointCloud<pcl::PointXYZ>);
 		std::vector<int> non_leaf_indices;
-		leaf_filter(cloud_dowm_sampled_with_leaf, non_leaf_indices);
+		//leaf_filter(cloud_dowm_sampled_with_leaf, non_leaf_indices);
+		//leaf_filter_morph(cloud_dowm_sampled_with_leaf, non_leaf_indices);
+		leaf_filter_ror(cloud_dowm_sampled_with_leaf, non_leaf_indices);
 		pcl::copyPointCloud(*cloud_dowm_sampled_with_leaf, non_leaf_indices, *cloud_dowm_sampled);
 		if (m_pLogger) {
 			stringstream buff;
@@ -268,6 +270,24 @@ namespace graft_cv {
 		//对比两个宽度，变化很大的，说明周边有异物（叶柄或叶子），不适合作为抓取位置；否则就是抓取位置
 		//在众多抓取位置上，选择离指定高度最近的点作为抓取位置
 		// 
+		//显示位置
+		if (m_cparam.image_show) {
+			std::vector<pcl::PointXYZ> tmp;
+			tmp.push_back(xz_center);
+			pcl::PointCloud<pcl::PointXYZRGB>::Ptr tttp(new pcl::PointCloud<pcl::PointXYZRGB>);
+			pcl::copyPointCloud(*cloud_seedling_seed, *tttp);
+
+			for (auto& pt : *tttp) {
+				pt.r = 255;
+				pt.g = 255;
+				pt.b = 255;
+			}
+			viewer_cloud_cluster(tttp, tmp, string("first"));
+		}
+		
+
+
+
 		pcl::PointXYZ selected_pt;
 		int selected_pt_idx;
 		std::vector<int> optimal_seeds_idx;
@@ -541,7 +561,7 @@ namespace graft_cv {
 
 	//}
 	////////////////////////////////////////////////////////////
-	//叶子剔除
+	//叶子剔除, 假设叶子和茎是分离的，用欧式聚类分割
 void CRootStockGrabPoint::leaf_filter(
 	pcl::PointCloud<pcl::PointXYZ>::Ptr in_cloud,	//输入，降采样的点云
 	std::vector<int> &stem_cloud_idx				//输出，除了叶子的点云序号
@@ -616,6 +636,333 @@ void CRootStockGrabPoint::leaf_filter(
 		}
 	}
 }
+////////////////////////////////////////////////////////////////////////////////////
+//   基于3d空间形态学方法------------->
+//叶子剔除,通过形态学方法，腐蚀，得到叶子区域,将叶子区域内地的点云去掉
+void CRootStockGrabPoint::leaf_filter_morph(
+	pcl::PointCloud<pcl::PointXYZ>::Ptr in_cloud,	//input 输入点云数据
+	std::vector<int> &stem_cloud_idx				//output, 过滤后的点云序号
+)
+{
+	//获取点云范围
+	pcl::PointXYZ min_v;
+	pcl::PointXYZ max_v;
+	pcl::getMinMax3D(*in_cloud, min_v, max_v);
+
+	//定义3维图像和像素间隔
+	cv::Mat bin_3d_img;
+	float pix_step = m_cparam.rs_grab_stem_diameter / 4.0;
+	if (m_dtype == 0) { pix_step = m_cparam.sc_grab_stem_diameter / 4.0; }
+
+	//创建点云存储图像对应关系
+	std::map<int, std::vector<int>> imgidx_to_pcdidx;
+	gen_3d_image(in_cloud, min_v, max_v, pix_step, bin_3d_img, imgidx_to_pcdidx);
+
+	//腐蚀膨胀（开运算）
+	int iter = 9;
+	int threshold = 10;
+	cv::Mat in_img = bin_3d_img.clone();
+	for (int i = 0; i < iter; ++i) {
+		cv::Mat e_img = cv::Mat::zeros(3, in_img.size, CV_8UC1);
+		erosion_3d(in_img, threshold, e_img);
+		in_img = e_img.clone();
+	}
+	for (int i = 0; i < iter; ++i) {
+		cv::Mat e_img = cv::Mat::zeros(3, in_img.size, CV_8UC1);
+		dilation_3d(in_img, e_img);
+		in_img = e_img.clone();
+	}
+
+	//in_img是经过开运算的图像，其中像素位置的点云为叶子区域
+	std::vector<int> leaf_idx;
+	get_mass_obj_idx(in_img, imgidx_to_pcdidx, leaf_idx);
+	std::vector<int>::iterator lit;
+	for (int i = 0; i < in_cloud->points.size(); ++i) {
+		lit = std::find(leaf_idx.begin(), leaf_idx.end(), i);
+		if (lit == leaf_idx.end()) {
+			stem_cloud_idx.push_back(i);
+		}
+	}
+
+	//显示开运算的结果
+	if (m_cparam.image_show) {
+		pcl::PointCloud<pcl::PointXYZ>::Ptr stem_cloud(new pcl::PointCloud<pcl::PointXYZ>);
+		pcl::PointCloud<pcl::PointXYZ>::Ptr leaf_cloud(new pcl::PointCloud < pcl::PointXYZ>);
+		pcl::copyPointCloud(*in_cloud, stem_cloud_idx, *stem_cloud);
+		pcl::copyPointCloud(*in_cloud, leaf_idx, *leaf_cloud);
+
+		pcl::visualization::PCLVisualizer viewer("open cloud");
+		viewer.addCoordinateSystem();
+		viewer.addPointCloud<pcl::PointXYZ>(stem_cloud, "stem_cloud");
+		viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 1, 1, 1, "stem_cloud");
+		viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "stem_cloud");
+
+		viewer.addPointCloud<pcl::PointXYZ>(leaf_cloud, "leaf_cloud");
+		viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 1, 0, 0, "leaf_cloud");
+		viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "leaf_cloud");
+
+		while (!viewer.wasStopped()) {
+			viewer.spinOnce(100);
+			boost::this_thread::sleep(boost::posix_time::microseconds(100000));
+		}
+
+	}
+}
+//生成3d图像
+void CRootStockGrabPoint::gen_3d_image(
+	pcl::PointCloud<pcl::PointXYZ>::Ptr in_cloud,	//input 输入点云数据
+	pcl::PointXYZ& min_pt,							//input 图像下限
+	pcl::PointXYZ& max_pt,							//input 图像上限
+	float step,										//input 图像像素间隔
+	cv::Mat& bin_3d_img,							//output, 生成的binary图像
+	std::map<int, std::vector<int>>& id2pcdidx      //output, 图像素位置内含pcd点云序号
+)
+{	
+	int width = static_cast<int>((max_pt.x - min_pt.x) / step);
+	int height = static_cast<int>((max_pt.y - min_pt.y) / step);
+	int layer = static_cast<int>((max_pt.z - min_pt.z) / step);
+	
+	id2pcdidx.clear();
+
+	int dims[3];
+	dims[0] = layer;
+	dims[1] = height;
+	dims[2] = width;
+	
+	std::map<int, std::vector<int>>::iterator it;
+
+	bin_3d_img = cv::Mat::zeros(3, dims, CV_8UC1);
+	for (int i = 0; i < in_cloud->points.size();++i) {
+		pcl::PointXYZ&pt = in_cloud->points.at(i);
+		int w = static_cast<int>((pt.x - min_pt.x) / step);
+		int h = static_cast<int>((pt.y - min_pt.y) / step);
+		int l = static_cast<int>((pt.z - min_pt.z) / step);
+		if (w < 0 || w >= width) { continue; }
+		if (h < 0 || h >= height) { continue; }
+		if (l < 0 || l >= layer) { continue; }
+		bin_3d_img.at<unsigned char>(l, h, w) = 1;
+
+		int idx = l*height*width + h*width + w;
+		it = id2pcdidx.find(idx);
+		if (it == id2pcdidx.end()) {
+			std::vector<int> tmp;
+			tmp.push_back(i);
+			id2pcdidx.insert(std::pair<int, std::vector<int>>(idx,tmp));
+		}
+		else {
+			it->second.push_back(i);
+		}
+	}
+}
+
+//3*3*3内的膨胀
+void CRootStockGrabPoint::dilation_3d(
+	cv::Mat& in_3d_img,		//input, 输入图像
+	cv::Mat& out_3d_img		//output, 生成的图像
+)
+{
+	int width = in_3d_img.size[2];
+	int height = in_3d_img.size[1];
+	int layer = in_3d_img.size[0];
+	int x, y, z;
+	for (int l = 0; l < layer; ++l) {
+		for (int h = 0; h < height; ++h) {
+			for (int w = 0; w < width; ++w) {
+				if (in_3d_img.at<unsigned char>(l, h, w) == 0) { continue; }
+				
+				for (int dz = -1; dz <= 1; dz++) {
+					z = l + dz;
+					if (z < 0 || z >= layer) { continue; }
+					for (int dy = -1; dy <= 1; dy++) {
+						y = h + dy;
+						if (y < 0 || y >= height) { continue; }
+						for (int dx = -1; dx <= 1; dx++) {
+							x = w + dx;
+							if (x < 0 || x >= width) { continue; }
+							out_3d_img.at<unsigned char>(z, y, x) = 1;							
+						}
+					}
+				}
+			}
+		}
+	}
+
+}
+//3*3*3内的腐蚀
+void CRootStockGrabPoint::erosion_3d(
+	cv::Mat& in_3d_img,
+	int th,				//阈值
+	cv::Mat& out_3d_img
+)
+{	
+	int width = in_3d_img.size[2];
+	int height = in_3d_img.size[1];
+	int layer = in_3d_img.size[0];
+	int x, y, z;
+	for (int l = 0; l < layer; ++l) {		
+		for (int h = 0; h < height; ++h) {
+			for (int w = 0; w < width; ++w) {
+				if (in_3d_img.at<unsigned char>(l, h, w) == 0) { continue; }
+				int cnt = 0;
+				for (int dz = -1; dz <= 1; dz++) {
+					z = l + dz;
+					if (z < 0 || z >= layer) { continue; }
+					for (int dy = -1; dy <= 1; dy++) {
+						y = h + dy;
+						if (y < 0 || y >= height) { continue; }
+						for (int dx = -1; dx <= 1; dx++) {
+							x = w + dx;
+							if (x < 0 || x >= width) { continue; }
+							if (in_3d_img.at<unsigned char>(z, y, x) == 0) { continue; }
+							cnt++;
+						}
+					}
+				}
+				if (cnt >= th) {
+					out_3d_img.at<unsigned char>(l, h, w) = 1;
+				}
+			}
+		}
+	}
+}
+//得到点云序号
+void CRootStockGrabPoint::get_mass_obj_idx(
+	cv::Mat& open_3d_img,				//input, 开运算后的图像
+	std::map<int, std::vector<int>>& id2pcdidx, //input 保存的像素位置的点云序号
+	std::vector<int>& mass_idx			//output,
+)
+{
+	mass_idx.clear();
+	int width = open_3d_img.size[2];
+	int height = open_3d_img.size[1];
+	int layer = open_3d_img.size[0];
+	std::map<int, std::vector<int>>::iterator it;
+	
+	for (int l = 0; l < layer; ++l) {
+		for (int h = 0; h < height; ++h) {
+			for (int w = 0; w < width; ++w) {
+				if (open_3d_img.at<unsigned char>(l, h, w) == 0) { continue; }
+				int idx = l*height*width + h*width + w;
+				it = id2pcdidx.find(idx);
+				if (it != id2pcdidx.end()) {
+					for (int& j : it->second) {
+						mass_idx.push_back(j);
+					}
+				}
+			}
+		}
+	}
+	
+}
+//  <-------------基于3d空间形态学方法
+//////////////////////////////////////////////////////////////////////////////////////
+
+// 基于孤立点滤除的方法检测叶子区域
+void CRootStockGrabPoint::leaf_filter_ror(
+	pcl::PointCloud<pcl::PointXYZ>::Ptr in_cloud,	//input 输入点云数据
+	std::vector<int> &stem_cloud_idx				//output, 过滤后的点云序号
+)
+{
+	//计算点云平均间距
+	double mean_dist = 0.0;
+	cloud_mean_dist(in_cloud, mean_dist);
+
+	//计算点云过滤半径和点数阈值
+	double stem_diameter = m_cparam.rs_grab_stem_diameter;
+	if (m_dtype == 0) { stem_diameter = m_cparam.sc_grab_stem_diameter; }
+	
+	double remove_radius = stem_diameter;
+	int nb_points = stem_diameter * stem_diameter * 2.57 / mean_dist / mean_dist; // (2d*d + pi *d*d) /2
+
+	//获取叶片的点云
+	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_leaf(new pcl::PointCloud<pcl::PointXYZ>);
+	pcl::RadiusOutlierRemoval<pcl::PointXYZ> ror(true);
+	ror.setInputCloud(in_cloud);
+	ror.setRadiusSearch(remove_radius);
+	ror.setMinNeighborsInRadius(nb_points);
+	ror.filter(*cloud_leaf);
+
+	//通过叶片点云寻找附近remove_radius内的近邻点，当做叶子点云序号	
+	std::set<int> leaf_idx_set;
+	int nres;
+	std::vector<int> indices;
+	std::vector<float> sqr_distances;
+	pcl::search::KdTree<pcl::PointXYZ> tree;
+	tree.setInputCloud(in_cloud);
+	for (size_t i = 0; i < cloud_leaf->size(); ++i)
+	{		
+		nres = tree.radiusSearch(cloud_leaf->points.at(i), remove_radius, indices, sqr_distances);		
+		for (int& idx : indices) {
+			leaf_idx_set.insert(idx);
+		}
+	}
+
+	std::vector<int> leaf_idx;
+	leaf_idx.assign(leaf_idx_set.begin(), leaf_idx_set.end());	
+
+	//in_img是经过开运算的图像，其中像素位置的点云为叶子区域
+	std::vector<int>::iterator lit;
+	for (int i = 0; i < in_cloud->points.size(); ++i) {
+		lit = std::find(leaf_idx.begin(), leaf_idx.end(), i);
+		if (lit == leaf_idx.end()) {
+			stem_cloud_idx.push_back(i);
+		}
+	}
+
+	//显示开运算的结果
+	if (m_cparam.image_show) {
+		pcl::PointCloud<pcl::PointXYZ>::Ptr stem_cloud(new pcl::PointCloud<pcl::PointXYZ>);
+		pcl::PointCloud<pcl::PointXYZ>::Ptr leaf_cloud(new pcl::PointCloud < pcl::PointXYZ>);
+		pcl::copyPointCloud(*in_cloud, stem_cloud_idx, *stem_cloud);
+		pcl::copyPointCloud(*in_cloud, leaf_idx, *leaf_cloud);
+
+		pcl::visualization::PCLVisualizer viewer("open cloud");
+		viewer.addCoordinateSystem();
+		viewer.addPointCloud<pcl::PointXYZ>(stem_cloud, "stem_cloud");//????????????????
+		viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 1, 1, 1, "stem_cloud");
+		viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "stem_cloud");
+
+		viewer.addPointCloud<pcl::PointXYZ>(leaf_cloud, "leaf_cloud");
+		viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 1, 0, 0, "leaf_cloud");
+		viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "leaf_cloud");
+
+		while (!viewer.wasStopped()) {
+			viewer.spinOnce(100);
+			boost::this_thread::sleep(boost::posix_time::microseconds(100000));
+		}
+
+	}
+}
+void CRootStockGrabPoint::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 CRootStockGrabPoint::gen_all_seedling_positions(
 	pcl::PointXYZ&key_center,	//输入，已知的苗的坐标
 	std::vector<pcl::PointXYZ>&candidate_center	//输出，有倾斜苗的坐标
@@ -1849,17 +2196,12 @@ void CRootStockGrabPoint::line_filter(
 		float rx = 1.5;
 		float ry = 1.5;
 		float rz = 1.5;
-		float cx, cy, cz;
+		float cx, cy, cz, t;
 		float dz, dx;
-		//float dz,dx, dz_line, dx_line;
-		//float dist_min = 10000.0;
-
-
-
-		//for (size_t i = 0; i < in_line_cloud->points.size(); ++i) {
-		cy = ymin;
-		float t = (cy - line_model->values.at(1)) / line_model->values.at(4);
+		
+		cy = ymin;		
 		while(cy<ymax){
+			t = (cy - line_model->values.at(1)) / line_model->values.at(4);
 			cx = line_model->values.at(3) * t + line_model->values.at(0);			
 			cz = line_model->values.at(5) * t + line_model->values.at(2);
 			//////////////////////////////////////////////////////////////////
@@ -1928,7 +2270,22 @@ void CRootStockGrabPoint::line_filter(
 		pt.z = z_mu;
 
 		pt_idx = max_pos;	
-		
+
+		//显示位置
+		if (m_cparam.image_show) {
+			pcl::visualization::PCLVisualizer viewer("grab line");
+			viewer.addCoordinateSystem();
+			viewer.addPointCloud<pcl::PointXYZ>(in_line_cloud, "stem_cloud");//????????????????
+			viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 1, 1, 1, "stem_cloud");
+			viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "stem_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));
+			}
+		}		
 	}
 	void CRootStockGrabPoint::viewer_cloud(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, std::string&winname)
 	{

grab_point_rs.h

@@ -52,11 +52,58 @@ namespace graft_cv {
 		);*/
 		//<---------
 
-		//叶子剔除
+		/////////////////////////////////////////////////////////////////////////////////////
+		//叶子剔除,通过欧式距离聚类，然后判断每一个类别是否是叶子
+		//   出现的问题：叶子和茎通过叶柄连接时，会误判
 		void leaf_filter(
 			pcl::PointCloud<pcl::PointXYZ>::Ptr in_cloud, 
 			std::vector<int> &stem_cloud_idx
 			);
+		//////////////////////////////////////////////////////////////////////////////////////
+		//叶子剔除,通过形态学方法，腐蚀，得到叶子区域,将叶子区域内地的点云去掉
+		void leaf_filter_morph(
+			pcl::PointCloud<pcl::PointXYZ>::Ptr in_cloud,	//input 输入点云数据
+			std::vector<int> &stem_cloud_idx				//output, 过滤后的点云序号
+		);
+		//生成3d图像
+		void gen_3d_image(
+			pcl::PointCloud<pcl::PointXYZ>::Ptr in_cloud,	//input 输入点云数据
+			pcl::PointXYZ& min_pt,							//input 图像下限
+			pcl::PointXYZ& max_pt,							//input 图像上限
+			float step,										//input 图像像素间隔
+			cv::Mat& bin_3d_img,								//output, 生成的binary图像
+			std::map<int, std::vector<int>>& id2pcdidx
+			);
+		//3*3*3内的膨胀
+		void dilation_3d(
+			cv::Mat& in_3d_img,		//input, 输入图像
+			cv::Mat& out_3d_img		//output, 生成的图像
+		);
+		//3*3*3内的腐蚀
+		void erosion_3d(
+			cv::Mat& in_3d_img,
+			int th,				//阈值
+			cv::Mat& out_3d_img
+		);
+		//得到点云序号
+		void get_mass_obj_idx(
+			cv::Mat& open_3d_img,				//input, 开运算后的图像
+			std::map<int, std::vector<int>>& id2pcdidx, //input 保存的像素位置的点云序号
+			std::vector<int>& mass_idx			//output,
+		);
+		//////////////////////////////////////////////////////////////////////////////////////
+		//叶子剔除,通过形态学方法
+		void leaf_filter_ror(
+			pcl::PointCloud<pcl::PointXYZ>::Ptr in_cloud,	//input 输入点云数据
+			std::vector<int> &stem_cloud_idx				//output, 过滤后的点云序号
+		);
+		void cloud_mean_dist(
+			pcl::PointCloud<pcl::PointXYZ>::Ptr in_cloud,	//input 输入点云数据
+			double& mean_dist
+		);
+		//////////////////////////////////////////////////////////////////////////////////////
+
+
 
 		void gen_all_seedling_positions(
 			pcl::PointXYZ&key_center,	//输入，已知的苗的坐标

graft_cv_api.cpp

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