v0.7.23 识别接口返回图片，返回茎的弯曲度，当前苗的数量

ReadMe.txt

@@ -107,4 +107,5 @@ v0.7.18
 v0.7.19 优化茎节位置识别,投影到2为平面寻找分叉点，rs_grab_fork_th和sc_grab_fork_th不在起作用，放弃历史均值位置
 v0.7.20 优化茎节位置识别,在v0.7.19基础上加上历史均值位置约束，rs_grab_fork_th和sc_grab_fork_th是茎节粗和茎粗的比值系数
 v0.7.21 优化茎节位置识别,增加偏离平均茎节高度的距离参数rs_grab_to_meanfork_max_dist和sc_grab_to_meanfork_max_dist
-v0.7.22 优化茎节位置识别,废弃基于历史平均茎节高度的约束，人工指定范围（高度上限，下限），不在范围内时，按下限位置
+v0.7.22 优化茎节位置识别,废弃基于历史平均茎节高度的约束，人工指定范围（高度上限，下限），不在范围内时，按下限位置
+v0.7.23 识别接口返回图片，返回茎的弯曲度，当前苗的数量

data_def_api.h

@@ -4,7 +4,7 @@ namespace graft_cv{
 typedef unsigned char byte;
 typedef struct 
 {
-    int angle;
+    int channel;
     int width;
     int height;
     byte *data;
@@ -147,10 +147,16 @@ typedef struct
 	double rs_grab_x;//砧木上苗抓取位置x，毫米
 	double rs_grab_y;//砧木上苗抓取位置y，毫米
 	double rs_grab_z;//砧木上苗抓取位置z，毫米
+	double rs_width;		//茎的宽度（直径），毫米
+	double rs_tortuosity;	//弯曲度，离茎中心轴线最大距离，毫米
+	double rs_count;		//当前第一排共有多少株
 
 	double sc_grab_x;//穗苗上苗抓取位置x，毫米
 	double sc_grab_y;//穗苗上苗抓取位置y，毫米
 	double sc_grab_z;//穗苗上苗抓取位置z，毫米
+	double sc_width;		//茎的宽度（直径），毫米
+	double sc_tortuosity;	//弯曲度，离茎中心轴线最大距离，毫米
+	double sc_count;		//当前第一排共有多少株
 
 	double rs_oa;//砧木最优角度
 	//double rs_oa_base;//砧木基质最优角度

grab_point_rs.cpp

@@ -40,11 +40,20 @@ namespace graft_cv {
 		m_ppImgSaver(0),
 		m_1st_row_zmean_rs(-1.0),
 		m_1st_row_zmean_sc(-1.0),
-		m_cloud_mean_dist(0.0)		
+		m_cloud_mean_dist(0.0),
+		m_pImginfoResult(0)
 	{
 	}
+	void CRootStockGrabPoint::clear_imginfo() {
+		if (m_pImginfoResult) {
+			imginfo_release(&m_pImginfoResult);
+			m_pImginfoResult = 0;
+		}		
+	}
 
-	CRootStockGrabPoint::~CRootStockGrabPoint() {}
+	CRootStockGrabPoint::~CRootStockGrabPoint() {
+		this->clear_imginfo();
+	}
 	float* CRootStockGrabPoint::get_raw_point_cloud(int &data_size)
 	{
 		data_size = m_raw_cloud->width * m_raw_cloud->height;
@@ -262,8 +271,9 @@ namespace graft_cv {
 		pcl::ModelCoefficients::Ptr line_model;		//存储找到的第一个位置上植株茎直线模型
 		if (m_pLogger) {
 			m_pLogger->INFO(m_pcdId + ": begin find seedling position");
-		}		
-		bool fund_seedling = find_seedling_position_key(cloud_dowm_sampled, first_seedling_cloud_idx, xz_center, line_model);
+		}
+		int first_row_seedling_number = 0;
+		bool fund_seedling = find_seedling_position_key(cloud_dowm_sampled, first_seedling_cloud_idx, xz_center, line_model, first_row_seedling_number);
 		if (!fund_seedling) {
 			if (m_pLogger) {
 				stringstream buff;
@@ -305,13 +315,18 @@ namespace graft_cv {
 		pcl::PointXYZ selected_pt;//抓取点坐标，根据茎节点的偏移
 		pcl::PointXYZ selected_pt_ref;	//返回茎节点，作为抓取点偏的基点
 		std::vector<int> optimal_seeds_idx;
+		float stem_width_mu;
+		float stem_deflection;
 		get_optimal_seed_compare(
 			cloud_dowm_sampled,		//input
 			cloud_seedling_seed,	//input
 			line_model,				//input
 			selected_pt,			//output
 			selected_pt_ref,		//output
-			optimal_seeds_idx);		//output
+			optimal_seeds_idx,		//output
+			stem_width_mu,			//output
+			stem_deflection			//output
+			);		//output
 		
 		if (selected_pt.z < 0) {
 			if (m_pLogger) {
@@ -326,11 +341,17 @@ namespace graft_cv {
 			posinfo.sc_grab_x = selected_pt.x;
 			posinfo.sc_grab_y = selected_pt.y;
 			posinfo.sc_grab_z = selected_pt.z;
+			posinfo.sc_count = (double)first_row_seedling_number;
+			posinfo.sc_width = (double)stem_width_mu;
+			posinfo.sc_tortuosity = (double)stem_deflection;
 		}
 		else {
 			posinfo.rs_grab_x = selected_pt.x;
 			posinfo.rs_grab_y = selected_pt.y;
 			posinfo.rs_grab_z = selected_pt.z;
+			posinfo.rs_count = (double)first_row_seedling_number;
+			posinfo.rs_width = (double)stem_width_mu;
+			posinfo.rs_tortuosity = (double)stem_deflection;
 		}
 		////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 		//7 保存处理结果到图片
@@ -340,7 +361,13 @@ namespace graft_cv {
 			(*m_ppImgSaver)->saveImage(rst_img, m_pcdId + "_rst_0");
 		}
 		////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-		//7 debug 显示结果
+		//8 页面显示结果
+		this->clear_imginfo();
+		m_pImginfoResult = mat2imginfo(rst_img);
+		posinfo.pp_images[0] = m_pImginfoResult;
+
+		////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		//9 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);
@@ -445,6 +472,10 @@ namespace graft_cv {
 		int fork_y = cy - int(selected_pt_ref.y / res + 0.5);
 		radius = 15;
 		cv::line(rst_img, cv::Point(fork_x - radius, fork_y), cv::Point(fork_x + radius, fork_y), cv::Scalar(153, 51, 255));
+
+		//在图片中写入文件名字
+		cv::putText(rst_img, m_pcdId, cv::Point(20, 20), cv::FONT_HERSHEY_SIMPLEX, 0.5, cv::Scalar(128, 128, 128));
+		
 		
 	}
 	int CRootStockGrabPoint::read_ply_file(const char* fn)
@@ -1272,9 +1303,11 @@ void CRootStockGrabPoint::line_filter(
 		pcl::PointCloud<pcl::PointXYZ>::Ptr in_cloud,
 		std::vector<int> &first_seedling_cloud_idx,
 		pcl::PointXYZ&xz_center,
-		pcl::ModelCoefficients::Ptr& line_model
+		pcl::ModelCoefficients::Ptr& line_model,
+		int& first_row_size
 	)
-	{
+	{	
+		first_row_size = 0;
 		// 确定植株inbox范围
 		float hole_step = m_cparam.rs_grab_seedling_dist - 5.0; //穴盘中穴间距
 		if (m_dtype == 0) {
@@ -1570,6 +1603,7 @@ void CRootStockGrabPoint::line_filter(
 		xz_center.y = target_filtered_root.at(first_idx).y;
 		xz_center.z = target_filtered_root.at(first_idx).z;
 		line_model = target_filtered_models.at(first_idx);
+		first_row_size = first_row_index.size();
 
 		if (m_pLogger) {
 			stringstream buff;
@@ -2309,14 +2343,19 @@ void CRootStockGrabPoint::line_filter(
 		pcl::ModelCoefficients::Ptr line_model,				//input
 		pcl::PointXYZ&pt,									//  返回抓取的点坐标，基于pt_ref的偏移点
 		pcl::PointXYZ &pt_ref,								//	返回点茎节的参考点
-		std::vector<int>& valid_line_index					//  返回有效直线点index
+		std::vector<int>& valid_line_index,					//  返回有效直线点index
+		float& stem_width_mu,									// 茎宽度，直径
+		float& stem_deflection								//茎的最大挠度，最大弯曲处的偏离直径轴心的距离，毫米
 	)
 	{
+		// stem_deflection 的计算方法：计算点云到拟合轴线的平均距离
 		valid_line_index.clear();
 		float th_ratio = 1.5;		//原始点云和直线点云邻域尺寸增加原来的20%，不能当做抓取点
 		pt.x = -1000.0;
 		pt.y = -1000.0;
 		pt.z = -1000.0;
+		stem_width_mu = 0.0;
+		stem_deflection = 0.0;
 
 
 		float ymin, ymax;
@@ -2389,12 +2428,13 @@ void CRootStockGrabPoint::line_filter(
 			pt_ref.z = z_mu;			
 			return;
 		}*/
-
 		// 如果opt_y_valid==false，就是用户没有指定抓取的y高度	
 		float max_dist_to_boundary = 0.0;
 		int max_idx_to_boundary = -1;
 		find_fork(in_line_cloud, max_dist_to_boundary, max_idx_to_boundary);
 
+		stem_deflection = static_cast<float>(calculate_deflection(in_line_cloud, line_model));
+
 		cy = ymin;	
 		//计算茎上的直径
 		while(cy<ymax){
@@ -2439,8 +2479,12 @@ void CRootStockGrabPoint::line_filter(
 			}
 		}
 		float mu = get_hist_mean(valid_stem_width);
+		stem_width_mu = mu;
 		float stdv = get_hist_std(valid_stem_width, mu);
 
+		//0)计算点云到轴线的最大距离
+
+
 		//1) original max position
 		int max_pos = std::max_element(stem_width.begin(), stem_width.end()) - stem_width.begin();
 		int max_pos_ref = max_pos;
@@ -2610,6 +2654,35 @@ void CRootStockGrabPoint::line_filter(
 		}		
 	}
 
+	//计算茎的弯曲度：点云到直线的距离95分位
+	double CRootStockGrabPoint::calculate_deflection(
+		pcl::PointCloud<pcl::PointXYZ>::Ptr in_line_cloud,	//input
+		pcl::ModelCoefficients::Ptr line_model			//input
+	)
+	{
+		double deflection = 0.0;
+		std::vector<double> dists;
+		Eigen::Vector4f line_pt = { line_model->values[0],line_model->values[1],line_model->values[2],0.0 };
+		Eigen::Vector4f line_dir = { line_model->values[3],line_model->values[4],line_model->values[5],0.0 };
+		for (auto ptc : in_line_cloud->points) {
+			//Get the square distance from a point to a line (represented by a point and a direction)
+			Eigen::Vector4f pt = { ptc.x, ptc.y, ptc.z, 0.0 };
+			double dist = (line_dir.cross3(line_pt - pt)).squaredNorm() / line_dir.squaredNorm();
+
+			dists.push_back(sqrt(dist));
+		}
+
+		//deflection = get_hist_mean(dists);
+
+		//95 percentile
+		if (dists.size() > 0) {
+			int idx_95 = int(0.95 * dists.size());
+			std::sort(dists.begin(), dists.end());
+			deflection = dists.at(idx_95);
+		}
+		return deflection;
+	}
+
 	void  CRootStockGrabPoint::find_fork(
 		pcl::PointCloud<pcl::PointXYZ>::Ptr in_line_cloud,
 		float& max_dist,

grab_point_rs.h

@@ -30,10 +30,16 @@ namespace graft_cv {
 
 		CImStoreManager** m_ppImgSaver;
 
+		//返回图片，用于调试
+		ImgInfo* m_pImginfoResult;//识别结果的图片
+		
+
 		pcl::PointCloud<pcl::PointXYZ>::Ptr m_raw_cloud;
 		double m_cloud_mean_dist;	
 
 
+		void clear_imginfo();
+
 		int read_ply_file(const char* fn);
 		double compute_nearest_neighbor_distance(pcl::PointCloud<pcl::PointXYZ>::Ptr);		
 
@@ -102,7 +108,8 @@ namespace graft_cv {
 			pcl::PointCloud<pcl::PointXYZ>::Ptr in_cloud,
 			std::vector<int> &first_seedling_cloud_idx,
 			pcl::PointXYZ&xz_center,
-			pcl::ModelCoefficients::Ptr& lmodel
+			pcl::ModelCoefficients::Ptr& lmodel,
+			int& first_row_size //返回第一排植株的数量
 		);
 		// 邻域最小抑制
 		void nms_box(
@@ -169,7 +176,14 @@ namespace graft_cv {
 			pcl::ModelCoefficients::Ptr line_model,			//input
 			pcl::PointXYZ&pt,								//输出，
 			pcl::PointXYZ &pt_ref,							//输出，	返回点茎节的参考点
-			std::vector<int>& valid_line_index				//输出，
+			std::vector<int>& valid_line_index,				//输出，
+			float& stem_width_mu,								//输出,茎宽度，直径
+			float& stem_deflection							//输出,茎的最大挠度，最大弯曲处的偏离直径轴心的距离，毫米
+		);
+		//计算茎的弯曲度：点云到直线距离的95分位
+		double calculate_deflection(
+			pcl::PointCloud<pcl::PointXYZ>::Ptr in_line_cloud,	//input
+			pcl::ModelCoefficients::Ptr line_model			//input
 		);
 		//在一株苗的空间范围内找出直线（茎，假设茎是直线分布的），并返回直线上的点index
 		void find_line_in_cube(

graft_cv_api.cpp

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

utils.cpp

@@ -11,28 +11,36 @@ namespace graft_cv{
 	{
 		assert(imginfo->data);
 		assert(imginfo->height>0 && imginfo->width>0);
-		cv::Mat m = cv::Mat(imginfo->height,imginfo->width,CV_8UC1);
-		for(int h=0; h<imginfo->height; ++h)
+		assert(imginfo->channel == 1 || imginfo->channel == 3);
+		cv::Mat m;
+		if (imginfo->channel == 1) {
+			m = cv::Mat(imginfo->height, imginfo->width, CV_8UC1);
+		}
+		if (imginfo->channel == 3) {
+			m = cv::Mat(imginfo->height, imginfo->width, CV_8UC3);
+		}
+
+		for (int h = 0; h<imginfo->height; ++h)
 		{
 			memcpy((void*)(m.ptr(h)),
-				(void*)(imginfo->data+h*imginfo->width),
-				imginfo->width);
+				(void*)(imginfo->data + h*imginfo->width * imginfo->channel),
+				imginfo->width * imginfo->channel);
 		};
 		return m;
 	};
 
 	ImgInfo* mat2imginfo(const cv::Mat&img){
-		assert(img.channels()==1);
+		assert(img.channels() == 1 || img.channels() == 3);
 		ImgInfo* imginfo = new ImgInfo();
-		imginfo->angle=0;
-		imginfo->height=img.rows;
+		imginfo->channel = img.channels();
+		imginfo->height = img.rows;
 		imginfo->width = img.cols;
 		imginfo->data = 0;
-		imginfo->data = new byte[imginfo->height * imginfo->width];
-		memset(imginfo->data, 0,imginfo->height * imginfo->width);	
+		imginfo->data = new byte[imginfo->height * imginfo->width * imginfo->channel];
+		memset(imginfo->data, 0, imginfo->height * imginfo->width * imginfo->channel);
 		//IplImage ipl_img = cvIplImage(img);
-		for(int i =0; i<imginfo->height;++i){
-			memcpy(imginfo->data+i*imginfo->width, img.ptr(i), imginfo->width);
+		for (int i = 0; i<imginfo->height; ++i) {
+			memcpy(imginfo->data + i*imginfo->width * imginfo->channel, img.ptr(i), imginfo->width * imginfo->channel);
 		}
 
 		return imginfo;