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"
#include "grab_occlusion.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;

		//历史植株位置信息
		CStemResultInfos* m_pStemInfos;
		CSeedlingStatus* m_pSeedlingStatus;
		std::string m_stem_info_file;
		std::vector<CStemResult> m_root_centers;  //通过m_pStemInfos获取到历史根中心位置
		std::vector<int> m_root_center_pcd_size;  //穴位上整体点云数量
		std::vector<double> m_root_center_leaf_cx;//穴位上，如有叶子，叶子中心，或按整体点云中心，默认1.0e6
		std::vector<bool> m_root_center_with_seedling;	//m_root_centers位置上是否含有植株
		std::vector<bool> m_root_center_with_seedling_history;	//m_root_centers位置上是否含有植株(通过历史信息获取)
		
		//用于记录第一排z均值，用来辅助判别1、2排的苗
		float m_1st_row_zmean_rs = -1.0;
		float m_1st_row_zmean_sc = -1.0;

		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);		

		/////////////////////////////////////////////////////////////////////////////////////
		//叶子剔除,通过欧式距离聚类，然后判断每一个类别是否是叶子
		//   出现的问题：叶子和茎通过叶柄连接时，会误判
		void leaf_filter(
			pcl::PointCloud<pcl::PointXYZ>::Ptr in_cloud, 
			std::vector<int> &stem_cloud_idx
			);
		
		void leaf_filter_ror(
			pcl::PointCloud<pcl::PointXYZ>::Ptr in_cloud,	//input 输入点云数据
			std::vector<int> &stem_cloud_idx,				//output, 过滤后的点云序号
			std::vector<int>& leaf_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 stem_width_ex_mu,//输入，stem_width_ex中有效值的均值
			const std::vector<float>& stem_width_ex,//输入， 通过外沿矩形得到的x方向茎粗
			std::vector<int>&fork_pos, //输出， 多个fork位置
			float& stem_width_mu //输出， xy投影上x方向的茎粗均值
		);


		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,
			int& first_row_size //返回第一排植株的数量
		);
		// 邻域最小抑制
		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 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,		//input
			pcl::PointCloud<pcl::PointXYZ>::Ptr in_line_cloud,	//input
			pcl::ModelCoefficients::Ptr line_model,			//input
			pcl::PointXYZ&pt,								//输出，
			pcl::PointXYZ &pt_ref,							//输出，	返回点茎节的参考点			
			pcl::PointXYZ&pt_rs_grab,					    //输出，	返回砧木指定的固定高度位置坐标
			std::vector<float>& fork_ys,							//	返回，fork y的高度
			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(
			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,
			pcl::PointXYZ& rs_grab_pt,        //输入，砧木固定抓取点,
			std::vector<float>&fork_ys,			//输入，forkys
			const PositionInfo& posinfo,					//输入，
			cv::Mat& rst_img						//输出，图片， 640*1280
		);
		//根据历史根的位置，计算对应位置点云数量，进而判断此位置是否有苗
		void occluded_seedling_detect_by_leaf(
			pcl::PointCloud<pcl::PointXYZ>::Ptr in_cloud,	//input 输入点云数据			
			std::vector<int>& leaf_idx);	//input

		//统计inbox点云在x方向的分布情况
		void get_point_x_hist(
			std::vector<int>& x_hist	//output			
		);
		void get_leaf_point_count_inbox(
			const CStemResult& sr, //input
			pcl::PointCloud<pcl::PointXYZ>::Ptr in_cloud,	//input 输入点云数据			
			std::vector<int>& leaf_idx,				//input 叶子点云index
			pcl::PointXYZ& aabb_min,	//output
			pcl::PointXYZ& aabb_max,	//output
			int& total_cnt,		//output
			int& leaf_cnt,		//output
			double& total_cx,	//output
			double& leaf_cx		//output
		);
		//没有检测到苗的情况，后处理
		void no_seedling_detected_post_process(
			int first_row_seedling_number,		//input
			int& selected_idx,					//output, 选择root_center的序号
			pcl::PointXYZ& selected_pt,			//output
			pcl::PointXYZ& selected_pt_ref,		//output
			pcl::PointXYZ& rs_grab_pt,			//output
			PositionInfo& posinfo				//output
		);
		//检测到苗的情况，后处理
		void had_seedling_detected_post_process(
			int first_row_seedling_number,		//input
			float stem_width_mu,				//input
			float stem_deflection,				//input
			int& selected_idx,					//output, 选择root_center的序号
			pcl::PointXYZ& selected_pt,			//input-output
			pcl::PointXYZ& selected_pt_ref,		//input-output
			pcl::PointXYZ& rs_grab_pt,			//input-output, 检测到的目标抓取参考点
			std::vector<float>&fork_ys,
			PositionInfo& posinfo				//output
		);
		//基于noise计算
		void calculate_noise(
			std::vector<float>&diamters,	//input
			int security_radius,			//input
			int noise_radius,				//input
			std::vector<float>&noise		//output
		);

		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);
	};

};