graft_cv/grab_point_rs.h

#pragma once

#include <pcl\point_types.h>
#include <pcl\point_cloud.h>
#include <pcl\segmentation\sac_segmentation.h>
#include "data_def_api.h"
#include "data_def.h"
#include "logger.h"
#include "imstorage_manager.h"

namespace graft_cv {
	class CRootStockGrabPoint {
	public:
		CRootStockGrabPoint(ConfigParam&c, CGcvLogger*pLog = 0);
		~CRootStockGrabPoint();
		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, int dtype, const char* fn = 0);
		void set_image_saver(CImStoreManager** ppis) { m_ppImgSaver = ppis; }
	private:
		//global configure parameters
		ConfigParam& m_cparam;
		CGcvLogger * m_pLogger;
		int m_dtype;
		string m_pcdId;

		//用于记录第一排z均值，用来辅助判别1、2排的苗
		float m_1st_row_zmean_rs = -1.0;
		float m_1st_row_zmean_sc = -1.0;

		CImStoreManager** m_ppImgSaver;

		pcl::PointCloud<pcl::PointXYZ>::Ptr m_raw_cloud;
		double m_cloud_mean_dist;

		//历史茎节位置，用于计算平均茎节位置
		std::vector<int>m_stem_fork_ys;
		int m_stem_fork_ys_size;



		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 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  CRootStockGrabPoint::find_fork(
			pcl::PointCloud<pcl::PointXYZ>::Ptr in_line_cloud,
			float& max_dist,//最大距离
			int& max_idx //距离边界最大的点序号
		);


		void gen_all_seedling_positions(
			pcl::PointXYZ&key_center,	//输入，已知的苗的坐标
			std::vector<pcl::PointXYZ>&candidate_center	//输出，有倾斜苗的坐标
		);
		bool find_seedling_position_key(
			pcl::PointCloud<pcl::PointXYZ>::Ptr in_cloud,
			std::vector<int> &first_seedling_cloud_idx,
			pcl::PointXYZ&xz_center,
			pcl::ModelCoefficients::Ptr& lmodel
		);
		// 邻域最小抑制
		void nms_box(
			std::vector<pcl::PointXYZ>&clt_root_v,	//目标点的可能位置
			std::vector<float>&valid_box_cc_dist,	//目标点的重心到中心的距离，距离越近越好
			float hole_step_radius,					//目标点搜索半径
			std::vector<pcl::PointXYZ>& target_toot // 返回值
		);
		//通过指定位置内,取部分点云分析是否存在真正的茎，真茎位置保存到target_filtered
		void line_filter(pcl::PointCloud<pcl::PointXYZ>::Ptr in_cloud,  //输入点云
			float radius,												//输入，取点半径
			float ymin,													//输入，y最小值
			float ymax,													//输入，y最大值
			std::vector<pcl::PointXYZ>&target_root,						//输入，位置
			std::vector<pcl::PointXYZ>&target_filtered,					//输出，位置
			std::vector<pcl::PointXYZ>&target_filtered_root,			//输出，茎上根部点坐标
			std::vector<std::vector<int>>&target_filtered_element,		//输出，茎上点index
			std::vector<pcl::ModelCoefficients::Ptr>& target_filtered_models//输出，茎直线模型
		);
		void get_line_project_hist(
			pcl::PointCloud<pcl::PointXYZ>::Ptr in_line_cloud,	//input	茎上直线点云
			pcl::ModelCoefficients::Ptr line_model,				//input		
			std::vector<int>& count_hist					    //  返回有效直线1mm内点云数量
		);

		/////////////////////////////////////////////////
		void find_seedling_position(
			pcl::PointCloud<pcl::PointXYZ>::Ptr in_cloud,
			std::vector<int> &first_seedling_cloud_idx,
			pcl::PointXYZ&xz_center
		);
		void crop_nn_analysis(
			pcl::PointCloud<pcl::PointXYZ>::Ptr in_cloud,
			pcl::PointCloud<pcl::PointXYZ>::Ptr seed_cloud,
			double dist_mean,
			std::vector<double>&mass_indices,
			std::vector<int>& idx
		);

		void euclidean_clustering_ttsas(
			pcl::PointCloud<pcl::PointXYZ>::Ptr in_cloud,
			double d1, double d2,
			std::vector<pcl::PointXYZ>&cluster_center,
			std::vector<std::vector<int>> &clustr_member
		);
		void cal_obb_2d(
			pcl::PointCloud<pcl::PointXYZ>::Ptr in_cloud,
			int axis,
			double &dx_obb,
			double &dy_obb,
			double &angle_obb);

		void get_optimal_seed(
			pcl::PointCloud<pcl::PointXYZ>::Ptr,
			pcl::PointXYZ&pt,
			int &pt_idx);
        //已知检测出苗的位置，找出有可能有苗的位置
		void tilted_seedling_discover(
			std::vector<pcl::PointXYZ>&cluster_center,	//输入，已知的苗的坐标
			std::vector<pcl::PointXYZ>&tilted_center	//输出，有倾斜苗的坐标
		);

		//通过比较直线点云和原始点云相同位置邻域内xz的范围，确定此点是否是无干扰点的茎
		/*void get_optimal_seed_compare(
			pcl::PointCloud<pcl::PointXYZ>::Ptr in_cloud,
			pcl::PointCloud<pcl::PointXYZ>::Ptr in_line_cloud,
			pcl::PointXYZ&pt,
			int &pt_idx,
			std::vector<int>& valid_line_index
		);*/
		//通过比较直线点云和原始点云相同位置邻域内xz的范围，确定此点是否是无干扰点的茎
		void get_optimal_seed_compare(
			pcl::PointCloud<pcl::PointXYZ>::Ptr in_cloud,		//input
			pcl::PointCloud<pcl::PointXYZ>::Ptr in_line_cloud,	//input
			pcl::ModelCoefficients::Ptr line_model,			//input
			pcl::PointXYZ&pt,								//输出，
			pcl::PointXYZ &pt_ref,							//输出，	返回点茎节的参考点
			std::vector<int>& valid_line_index				//输出，
		);
		//在一株苗的空间范围内找出直线（茎，假设茎是直线分布的），并返回直线上的点index
		void find_line_in_cube(
			pcl::PointCloud<pcl::PointXYZ>::Ptr,	//输入，整体点云
			pcl::PointXYZ& min_pt_ex,				//输入，
			pcl::PointXYZ& max_pt_ex,				//输入，
			std::vector<int>& out_idx				//输出，直线上点的index， 基于输入整体点云
			);

		//生成结果图片
		void gen_result_img(
			pcl::PointCloud<pcl::PointXYZ>::Ptr in_cloud_raw,//输入，未经过滤的整体点云in_cloud_raw, 
			pcl::PointCloud<pcl::PointXYZ>::Ptr,	//输入，整体点云cloud_dowm_sampled, 
			pcl::PointXYZ& selected_pt,				//输入，selected_pt, 
			pcl::PointXYZ& selected_pt_ref,        //输入，selected_pt_ref,
			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);

		void viewer_cloud_debug(
			pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud, 
			pcl::PointXYZ&p,//抓取点
			pcl::PointXYZ &p_ref,//分叉点
			pcl::PointXYZ &root_pt,
			std::string&winname);

		void viewer_cloud_cluster(pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud, std::vector<pcl::PointXYZ>cluster_center, std::string&winname);
		void viewer_cloud_cluster_box(
			pcl::PointCloud<pcl::PointXYZ>::Ptr cloud,
			std::vector<pcl::PointXYZ>&cluster_center,
			std::vector<pcl::PointXYZ>&cluster_box,
			std::vector<std::vector<int> >& clt_line_idx,
			std::string&winname);
	};

};