v0.7.2 增加结果图片输出功能；日志增加image_id信息

ReadMe.txt

@@ -86,4 +86,5 @@ v0.6.14
 v0.6.15 修改抓取点识别方法，通过比较直线点云和原始点云相同位置邻域内xz的范围，确定此点是否是无干扰点的茎
 v0.6.16 加强苗的识别，增加倾斜苗的检测,修改排序错乱问题
 v0.7.0 支持多线程调用业务接口
-v0.7.1 增加voxelsize参数；修改茎密度最大值的1/3为最小茎密度的下限（原来为1/2，有些小苗识别不出来）
+v0.7.1 增加voxelsize参数；修改茎密度最大值的1/3为最小茎密度的下限（原来为1/2，有些小苗识别不出来）
+v0.7.2 增加结果图片输出功能；日志增加image_id信息

grab_point_rs.cpp

@@ -55,10 +55,20 @@ namespace graft_cv {
 	int CRootStockGrabPoint::load_data(
 		float*pPoint,
 		int pixel_size,
-		int pt_size,		
+		int pt_size,
+		int dtype,
 		const char* fn/* = 0*/)
 	{
 		int rst = 0;
+		m_dtype = dtype;
+		//generate image id
+		if (m_dtype == 0) {
+			m_pcdId = getImgId(img_type::sola_sc_pcd);
+		}
+		else {
+			m_pcdId = getImgId(img_type::sola_rs_pcd);
+		}
+
 		//1 get point cloud data
 		if (pPoint != 0 && pt_size>0) {
 			//read point
@@ -71,7 +81,7 @@ namespace graft_cv {
 			rst = m_raw_cloud->width * m_raw_cloud->height;
 			if (m_pLogger) {
 				stringstream buff;
-				buff << "load raw point cloud " << rst << " data points";
+				buff << m_pcdId <<": load raw point cloud " << rst << " data points";
 				m_pLogger->INFO(buff.str());
 			}			
 		}
@@ -81,29 +91,23 @@ namespace graft_cv {
 		}
 		else {//error
 			if (m_pLogger) {
-				m_pLogger->ERRORINFO("no valid input");
+				m_pLogger->ERRORINFO(m_pcdId + ": no valid input");
 				return (-1);
 			}
 		}	
 
-		if (m_dtype == 0) {
-			m_pcdId = getImgId(img_type::sola_sc_pcd);
-		}
-		else {
-			m_pcdId = getImgId(img_type::sola_rs_pcd);
-		}
+		
 
 		if (m_ppImgSaver && *m_ppImgSaver) {
 			(*m_ppImgSaver)->saveBinPly(m_raw_cloud, m_pcdId);
 		}
 		if (m_cparam.image_show) {
-			viewer_cloud(m_raw_cloud, std::string("raw point cloud"));			
+			viewer_cloud(m_raw_cloud, m_pcdId + std::string(": raw point cloud"));
 		}
 		return rst;
 	}
 
-	int CRootStockGrabPoint::grab_point_detect(	
-		int dtype,
+	int CRootStockGrabPoint::grab_point_detect(
 		PositionInfo& posinfo
 		)
 	{
@@ -114,7 +118,7 @@ namespace graft_cv {
 		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";
+				buff << m_pcdId << ": m_raw_cloud point cloud " << m_raw_cloud->width * m_raw_cloud->height << " data points";
 				m_pLogger->ERRORINFO(buff.str());
 			}
 			return 1;
@@ -122,12 +126,11 @@ namespace graft_cv {
 		////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 		//1 crop filter
 		if (m_pLogger) {
-			m_pLogger->INFO("begin crop");			
+			m_pLogger->INFO(m_pcdId + ": begin crop");
 		}
 		pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_inbox(new pcl::PointCloud<pcl::PointXYZ>);
-		pcl::CropBox<pcl::PointXYZ> box_filter;
-		m_dtype = dtype;
-		if (dtype != 0) {//rootstock
+		pcl::CropBox<pcl::PointXYZ> box_filter;		
+		if (m_dtype != 0) {//rootstock
 			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));
 		}
@@ -141,7 +144,7 @@ namespace graft_cv {
 
 		if (m_pLogger) {
 			stringstream buff;
-			buff << "CropBox croped point cloud " << cloud_inbox->width * cloud_inbox->height << " data points";
+			buff << m_pcdId << ": CropBox croped point cloud " << cloud_inbox->width * cloud_inbox->height << " data points";
 			m_pLogger->INFO(buff.str());
 		}
 		if (cloud_inbox->width * cloud_inbox->height < 1) {
@@ -152,16 +155,16 @@ namespace graft_cv {
 			viewer_cloud(cloud_inbox, std::string("cloud_inbox"));
 		}
 		if (m_pLogger) {
-			m_pLogger->INFO("end crop");
+			m_pLogger->INFO(m_pcdId + ": end crop");
 		}
 		////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 		//2 filtler with radius remove
 		if (m_pLogger) {
-			m_pLogger->INFO("begin ror");
+			m_pLogger->INFO(m_pcdId + ": begin ror");
 		}
 		int nb_points = 20;
 		double stem_radius = m_cparam.rs_grab_stem_diameter / 2.0;
-		if(dtype == 0){
+		if(m_dtype == 0){
 			stem_radius = m_cparam.sc_grab_stem_diameter / 2.0;
 		}
 		double remove_radius = stem_radius * 2.0;
@@ -174,7 +177,7 @@ namespace graft_cv {
 		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="<<
+			buff << m_pcdId <<": 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());
 		}
@@ -186,12 +189,12 @@ namespace graft_cv {
 			viewer_cloud(cloud_ror, std::string("cloud_ror"));
 		}*/
 		if (m_pLogger) {
-			m_pLogger->INFO("end ror");
+			m_pLogger->INFO(m_pcdId + ": end ror");
 		}
 		////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 		//3 voxel grid down sampling
 		if (m_pLogger) {
-			m_pLogger->INFO("begin down");
+			m_pLogger->INFO(m_pcdId + ": begin down");
 		}
 		pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_dowm_sampled(new pcl::PointCloud<pcl::PointXYZ>);
 		pcl::VoxelGrid<pcl::PointXYZ> outrem;
@@ -204,7 +207,7 @@ namespace graft_cv {
 
 		if (m_pLogger) {
 			stringstream buff;
-			buff << "VoxelGrid dowm_sampled point cloud " << cloud_dowm_sampled->width * cloud_dowm_sampled->height << " data points (if < 50, return)";
+			buff << m_pcdId <<": VoxelGrid dowm_sampled point cloud " << cloud_dowm_sampled->width * cloud_dowm_sampled->height << " data points (if < 50, return)";
 			m_pLogger->INFO(buff.str());
 		}
 		if (cloud_dowm_sampled->width * cloud_dowm_sampled->height < 50) {
@@ -215,7 +218,7 @@ namespace graft_cv {
 			viewer_cloud(cloud_dowm_sampled, std::string("cloud_dowm_sampled"));
 		}*/
 		if (m_pLogger) {
-			m_pLogger->INFO("end down");
+			m_pLogger->INFO(m_pcdId + ": end down");
 		}
 
 		////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
@@ -223,16 +226,16 @@ namespace graft_cv {
 		std::vector<int> first_seedling_cloud_idx;	//存储找到的第一个位置上植株茎上直线点的index
 		pcl::PointXYZ xz_center;					//存储找到的第一个位置上植株根部坐标
 		if (m_pLogger) {
-			m_pLogger->INFO("begin find seedling position");
+			m_pLogger->INFO(m_pcdId + ": begin find seedling position");
 		}		
 		find_seedling_position_key(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();
+			buff << m_pcdId <<": 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");
+			m_pLogger->INFO(m_pcdId + ": end find seedling position");
 		}
 
 		////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
@@ -250,12 +253,12 @@ namespace graft_cv {
 		
 		if (selected_pt_idx < 0) {	
 			if (m_pLogger) {
-				m_pLogger->ERRORINFO("Not found valid grab point");
+				m_pLogger->ERRORINFO(m_pcdId + ": Not found valid grab point");
 			}
 			return 1;
 		}
 		if (m_pLogger) {
-			m_pLogger->INFO("end get_optimal_seed");
+			m_pLogger->INFO(m_pcdId + ": end get_optimal_seed");
 		}
 		if (m_dtype == 0) {
 			posinfo.sc_grab_x = selected_pt.x;
@@ -267,9 +270,15 @@ namespace graft_cv {
 			posinfo.rs_grab_y = selected_pt.y;
 			posinfo.rs_grab_z = selected_pt.z;
 		}
-
 		////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-		//6 debug 显示结果
+		//6 保存处理结果到图片
+		cv::Mat rst_img = cv::Mat::zeros(cv::Size(1280,640),CV_8UC1);
+		gen_result_img(cloud_dowm_sampled, selected_pt, rst_img);
+		if (m_ppImgSaver && *m_ppImgSaver) {
+			(*m_ppImgSaver)->saveImage(rst_img, m_pcdId + "_rst_0");
+		}
+		////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		//7 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);
@@ -309,21 +318,64 @@ namespace graft_cv {
 
 			//viewer_cloud(cloud_cand_demo, std::string("cloud_cand_demo"));
 			viewer_cloud_debug(cloud_cand_demo, selected_pt, xz_center, std::string("cloud_cand_demo"));
+			imshow_wait("rst_img", rst_img);
 		}
 		return 0;
 	}
+
+	//生成结果图片
+	void CRootStockGrabPoint::gen_result_img(
+		pcl::PointCloud<pcl::PointXYZ>::Ptr in_cloud,	//输入，整体点云cloud_dowm_sampled, 
+		pcl::PointXYZ& selected_pt,				//输入，selected_pt, 
+		cv::Mat& rst_img						//输出，图片， 640*1280
+	)
+	{
+		if (rst_img.empty()) { return; }
+		if (rst_img.rows != 640 && rst_img.cols != 1280) { return; }
+		int cx = 640;		//图像中心点0
+		int cy = 320;		//图像中心点0
+		float res = 0.33333;	//分辨率，1个像素0.333mm
+		//绘制坐标轴
+		int arrow_len = 20;
+		cv::line(rst_img, cv::Point(0, cy), cv::Point(cx, cy), cv::Scalar(128));
+		cv::line(rst_img, cv::Point(0, cy), cv::Point(arrow_len, cy - int(arrow_len/2)), cv::Scalar(128));
+		cv::line(rst_img, cv::Point(0, cy), cv::Point(arrow_len, cy + int(arrow_len/2)), cv::Scalar(128));
+
+		cv::line(rst_img, cv::Point(cx, 0), cv::Point(cx, cy), cv::Scalar(128));
+		cv::line(rst_img, cv::Point(cx, 0), cv::Point(cx - int(arrow_len/2), arrow_len), cv::Scalar(128));
+		cv::line(rst_img, cv::Point(cx, 0), cv::Point(cx + int(arrow_len/2), arrow_len), cv::Scalar(128));
+
+		cv::putText(rst_img, std::string("x"), cv::Point(20, cy-10), cv::FONT_HERSHEY_SIMPLEX, 1, cv::Scalar(128));
+		cv::putText(rst_img, std::string("y"), cv::Point(cx+10, 20), cv::FONT_HERSHEY_SIMPLEX, 1, cv::Scalar(128));
+		
+		//绘制所有点
+		int x, y;
+		for (auto&pt : in_cloud->points) {
+			x = cx - int(pt.x / res + 0.5);
+			y = cy - int(pt.y / res + 0.5);
+			if (x < 0 || x >= rst_img.cols) { continue; }
+			if (y < 0 || y >= rst_img.rows) { continue; }
+			rst_img.at<unsigned char>(y, x) = 255;
+		}
+		//绘制抓取点坐标
+		int grab_x = cx - int(selected_pt.x / res + 0.5);
+		int grab_y = cy - int(selected_pt.y / res + 0.5);
+		int radius = 30;
+		cv::line(rst_img, cv::Point(grab_x - radius, grab_y - radius), cv::Point(grab_x + radius, grab_y + radius), cv::Scalar(128));
+		cv::line(rst_img, cv::Point(grab_x - radius, grab_y + radius), cv::Point(grab_x + radius, grab_y - radius), cv::Scalar(128));
+	}
 	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));
+				m_pLogger->ERRORINFO(m_pcdId + ": 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";
+			buff << m_pcdId <<": 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;
@@ -895,7 +947,7 @@ void CRootStockGrabPoint::line_filter(
 
 		if (m_pLogger) {
 			stringstream buff;
-			buff << "euclidean_clustering_ttsas， find cluster center(" << xz_center.x
+			buff << m_pcdId << ": euclidean_clustering_ttsas， find cluster center(" << xz_center.x
 				<<", "<< xz_center.y<<", "<< xz_center.z<<")";
 			m_pLogger->INFO(buff.str());
 		}
@@ -1011,7 +1063,7 @@ void CRootStockGrabPoint::line_filter(
 
 		if (m_pLogger) {
 			stringstream buff;
-			buff << "get 2d seedling seed point cloud " << index.size() << " data points with thrreshold " << th;
+			buff << m_pcdId <<": get 2d seedling seed point cloud " << index.size() << " data points with thrreshold " << th;
 			m_pLogger->INFO(buff.str());
 		}
 		if (m_cparam.image_show) {
@@ -1031,7 +1083,7 @@ void CRootStockGrabPoint::line_filter(
 
 		if (m_pLogger) {
 			stringstream buff;
-			buff << "euclidean_clustering_ttsas:  " << cluster_center.size() << " t1=" << d1
+			buff << m_pcdId <<": euclidean_clustering_ttsas:  " << cluster_center.size() << " t1=" << d1
 				<< " t2=" << d2;
 			m_pLogger->INFO(buff.str());
 		}
@@ -1133,7 +1185,7 @@ void CRootStockGrabPoint::line_filter(
 
 		if (m_pLogger) {
 			stringstream buff;
-			buff << "euclidean_clustering_ttsas， find cluster center(" << xz_center.x
+			buff << m_pcdId <<": euclidean_clustering_ttsas， find cluster center(" << xz_center.x
 				<< ", " << xz_center.y << ", " << xz_center.z << ")";
 			m_pLogger->INFO(buff.str());
 		}
@@ -1287,7 +1339,7 @@ void CRootStockGrabPoint::line_filter(
 	{
 		if (m_pLogger) {
 			stringstream buff;
-			buff << "euclidean_clustering_ttsas() begin...";
+			buff << m_pcdId <<": euclidean_clustering_ttsas() begin...";
 			m_pLogger->INFO(buff.str());
 		}
 		std::vector<int> cluster_weight;
@@ -1381,7 +1433,7 @@ void CRootStockGrabPoint::line_filter(
 		}
 		if (m_pLogger) {
 			stringstream buff;
-			buff << "euclidean_clustering_ttsas() end";
+			buff << m_pcdId <<": euclidean_clustering_ttsas() end";
 			m_pLogger->INFO(buff.str());
 		}
 	}
@@ -1523,7 +1575,7 @@ void CRootStockGrabPoint::line_filter(
 		if (opt_idx < 0) {
 			if (m_pLogger) {
 				stringstream buff;
-				buff << "get_optimal_seed failed, get invalid optimal cluster id";
+				buff << m_pcdId <<": get_optimal_seed failed, get invalid optimal cluster id";
 				m_pLogger->ERRORINFO(buff.str());
 			}
 			return;

grab_point_rs.h

@@ -12,9 +12,9 @@ namespace graft_cv {
 	public:
 		CRootStockGrabPoint(ConfigParam&c, CGcvLogger*pLog = 0);
 		~CRootStockGrabPoint();
-		int grab_point_detect( int dtype, PositionInfo& posinfo);
+		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);
+		int load_data(float*pPoint, int pixel_size, int pt_size, int dtype, const char* fn = 0);
 		void set_image_saver(CImStoreManager** ppis) { m_ppImgSaver = ppis; }
 	private:
 		//global configure parameters
@@ -130,6 +130,13 @@ namespace graft_cv {
 			std::vector<int>& out_idx				//输出，直线上点的index， 基于输入整体点云
 			);
 
+		//生成结果图片
+		void gen_result_img(
+			pcl::PointCloud<pcl::PointXYZ>::Ptr,	//输入，整体点云cloud_dowm_sampled, 
+			pcl::PointXYZ& selected_pt,				//输入，selected_pt, 
+			cv::Mat& rst_img						//输出，图片， 640*1280
+		);
+
 		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

@@ -19,7 +19,7 @@ extern CRITICAL_SECTION g_cs;
 namespace graft_cv
 {
 
-	char *g_version_str = "0.7.1";
+	char *g_version_str = "0.7.2";
 
 	//configure
 	string g_conf_file = "./gcv_conf.yml";	
@@ -309,12 +309,12 @@ namespace graft_cv
 		memset(&posinfo, 0, sizeof(PositionInfo));
 		int dtype = 1;
 		try {
-			int rst = g_sola_grab_rs.load_data(points, pixel_size, pt_size, fn);
+			int rst = g_sola_grab_rs.load_data(points, pixel_size, pt_size, dtype, fn);
 			if (rst <= 0) {
 				g_logger.ERRORINFO("invalid points");
 				return 1;
 			}
-			int oa = g_sola_grab_rs.grab_point_detect(dtype, posinfo);
+			int oa = g_sola_grab_rs.grab_point_detect(posinfo);
 			if (oa != 0) {
 				g_logger.ERRORINFO("no points");
 				return 1;
@@ -347,12 +347,12 @@ namespace graft_cv
 		memset(&posinfo, 0, sizeof(PositionInfo));
 		int dtype = 0;
 		try {
-			int rst = g_sola_grab_sc.load_data(points, pixel_size, pt_size, fn);
+			int rst = g_sola_grab_sc.load_data(points, pixel_size, pt_size, dtype, fn);
 			if (rst <= 0) {
 				g_logger.ERRORINFO("invalid points");
 				return 1;
 			}
-			int oa = g_sola_grab_sc.grab_point_detect(dtype, posinfo);
+			int oa = g_sola_grab_sc.grab_point_detect(posinfo);
 			if (oa != 0) {
 				g_logger.ERRORINFO("no points");
 				return 1;

imstorage_manager.cpp

@@ -114,7 +114,7 @@ int WINAPI TheadFuncSavePcd(LPVOID lpParam)
 		try {
 			EnterCriticalSection(&g_cs_pcd);
 			PcdParam& pcd_info = pcdQ->front();
-			pcl::io::savePLYFile(pcd_info.pcd_name, pcd_info.pcd, true);			
+			pcl::io::savePLYFile(pcd_info.pcd_name, *pcd_info.pcd, true);			
 			//cout<<im_info.img_name<<"\t"<<imgQ->size()<<endl;
 			pcdQ->pop();
 		}
@@ -253,8 +253,9 @@ int CImStoreManager::saveBinPly(
 	EnterCriticalSection(&g_cs_pcd);
 	string tar_file = m_storeDir + "/" + name_id + ".ply";		
 	PcdParam pcdp;
+	pcdp.pcd.reset(new pcl::PointCloud<pcl::PointXYZ>);
 	pcdp.pcd_name = tar_file;
-	pcl::copyPointCloud(*pcd, pcdp.pcd);	
+	pcl::copyPointCloud(*pcd, *pcdp.pcd);	
 	
 	m_pcds.push(pcdp);
 	//cout<<"=========="<<pcdp.pcd_name<<endl;

imstorage_manager.h

@@ -21,7 +21,7 @@ typedef struct
 typedef struct
 {
 	string pcd_name;
-	pcl::PointCloud<pcl::PointXYZ> pcd;
+	pcl::PointCloud<pcl::PointXYZ>::Ptr pcd;
 } PcdParam;