#include "cnn3d_segmentation.h"
#include "tf_wheel_3Dcnn_api.h"
#include <iostream>
#include <glog/logging.h>
#include <pcl/segmentation/extract_clusters.h>
#include <pcl/kdtree/kdtree.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/filters/statistical_outlier_removal.h>
#include <pcl/filters/project_inliers.h>
#include <pcl/surface/convex_hull.h>
#include <pcl/features/moment_of_inertia_estimation.h>
#include <pcl/common/centroid.h>
void save_rgb_cloud_txt(std::string txt, pcl::PointCloud<pcl::PointXYZRGB>::Ptr pCloud)
{
	std::ofstream os;
	os.open(txt, std::ios::out);
	for (int i = 0; i < pCloud->points.size(); i++)
	{
		pcl::PointXYZRGB point = pCloud->points[i];
		char buf[255];
		memset(buf, 0, 255);
		sprintf(buf, "%f %f %f %d %d %d\n", point.x, point.y, point.z, point.r, point.g, point.b);
		os.write(buf, strlen(buf));
	}
	os.close();
}

double Cnn3d_segmentation::distance(cv::Point2f p1, cv::Point2f p2)
{
	return sqrt(pow(p1.x - p2.x, 2.0) + pow(p1.y - p2.y, 2.0));
}

cv::RotatedRect minAreaRect_cloud(pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud)
{
	std::vector<cv::Point2f> points;
	for (int i = 0; i < cloud->size(); ++i)
	{
		pcl::PointXYZRGB point = cloud->points[i];
		points.push_back(cv::Point2f(point.x, point.y));
	}
	cv::RotatedRect rect= cv::minAreaRect(points);
	return rect;
}

cv::RotatedRect minAreaRect_cloud(std::vector<cv::Point2f> centers)
{
	cv::RotatedRect rect = cv::minAreaRect(centers);
	return rect;
}

Error_manager Cnn3d_segmentation::set_parameter(int l, int w, int h,int freq,int classes)
{
	m_lenth = l;
	m_width = w;
	m_height = h;
	m_freq = freq;
	m_nClass = classes;
	return SUCCESS;
}
Cnn3d_segmentation::Cnn3d_segmentation(int l, int w, int h,int freq,int classes)
{
	m_lenth = l;
	m_width = w;
	m_height = h;
	m_freq = freq;
	m_nClass = classes;
}

Cnn3d_segmentation::~Cnn3d_segmentation()
{
	//3dcnn 原校验功能保留,不加载网络
	//tf_wheel_3dcnn_close();
}

Error_manager Cnn3d_segmentation::init(std::string weights)
{
	//3dcnn 原校验功能保留,不加载网络
	return SUCCESS;
	/*if (0 != tf_wheel_3dcnn_init(weights, m_lenth,m_width,m_height))
	{
		std::string error_description="tf_wheel_3Dcnn model load failed :";
		error_description+=weights;
		return Error_manager(LOCATER_3DCNN_INIT_FAILED,MINOR_ERROR,error_description);
	}
    //空跑一次
	float* data = (float*)malloc(m_lenth*m_width*m_height*sizeof(float));
	float* pout = (float*)malloc(m_lenth*m_width*m_height*m_nClass*sizeof(float));

	if (!tf_wheel_3dcnn_predict(data, pout))
	{
		free(data);
		free(pout);
		return Error_manager(LOCATER_3DCNN_PREDICT_FAILED,MINOR_ERROR,"first locate 3dcnn failed");
	}
	free(data);
	free(pout);
	return SUCCESS;*/
}


void save_cloudT(std::string txt, pcl::PointCloud<pcl::PointXYZRGB>& pCloud)
{
	std::ofstream os;
	os.open(txt, std::ios::out);
	for (int i = 0; i < pCloud.points.size(); i++)
	{
		pcl::PointXYZRGB point = pCloud.points[i];
		char buf[255];
		memset(buf, 0, 255);
		sprintf(buf, "%f %f %f\n", point.x, point.y, point.z);
		os.write(buf, strlen(buf));
	}
	os.close();
}

Error_manager Cnn3d_segmentation::analytic_wheel(std::map<int,pcl::PointCloud<pcl::PointXYZRGB>::Ptr>& wheel_cloud_map, bool cloud_aggregation_flag,
												 Locate_information* p_locate_information,
												 bool save_flag, std::string save_dir)
{
	Error_manager t_error;
	if ( p_locate_information == NULL )
	{
		return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR,
							 " analytic_wheel POINTER_IS_NULL ");
	}

	//第一步 离群点过滤
	t_error = filter_cloud_with_outlier_for_map(wheel_cloud_map);
	if ( t_error != Error_code::SUCCESS)
	{
		return t_error;
	}

	//第二步,	//将map分为4个轮胎
	//注: 前面的雷达扫描并point sift之后, 可能只能识别3个轮胎, 此时允许3个轮胎 继续运行.
	std::vector<pcl::PointCloud<pcl::PointXYZRGB>::Ptr> wheel_source_vector; //拆分后的轮胎点云, 不同的轮胎点是分开的,
	t_error = split_wheel_cloud(wheel_cloud_map, cloud_aggregation_flag, wheel_source_vector, save_flag, save_dir);
	if ( t_error != Error_code::SUCCESS)
	{
		return t_error;
	}


	//第三步, pca过滤, 并计算每个轮子的中心点
	std::vector<pcl::PointCloud<pcl::PointXYZRGB>::Ptr> wheel_pca_vector;	//pca过滤后的点云
	t_error = filter_cloud_with_pca(wheel_source_vector, wheel_pca_vector, save_flag, save_dir);
	if ( t_error != Error_code::SUCCESS)
	{
		return t_error;
	}


	//第四步,提取关键矩形,
	cv::RotatedRect rect_of_wheel_center; 	//4个轮胎中心点构成的矩形
	cv::RotatedRect rect_of_wheel_cloud;	//所有点云构成的矩形
	t_error = extract_key_rect(wheel_pca_vector, rect_of_wheel_center, rect_of_wheel_cloud, save_flag, save_dir);
	if ( t_error != Error_code::SUCCESS)
	{
		return t_error;
	}

	//第五步,	//通过关键矩形, 计算出车辆的定位信息, (输出中心点的x和y, 前后轮距, 左右轮距, 旋转角)
	t_error = calculate_key_rect(rect_of_wheel_center, rect_of_wheel_cloud, p_locate_information);
	if ( t_error != Error_code::SUCCESS)
	{
		return t_error;
	}

	return Error_code::SUCCESS;
}


//过滤离群点,
Error_manager Cnn3d_segmentation::filter_cloud_with_outlier_for_map(std::map<int,pcl::PointCloud<pcl::PointXYZRGB>::Ptr>& wheel_cloud_map)
{
	Error_manager t_error;
	Error_manager t_result;

	//遍历map, 对点云进行 离群点过滤
	for (auto iter = wheel_cloud_map.begin(); iter != wheel_cloud_map.end(); ++iter)
	{
		//过滤离群点,
		t_error = filter_cloud_with_outlier(iter->second);
		if (t_error != Error_code::SUCCESS)
		{
			char buf[256] = {0};
			sprintf(buf, " map.id = %d, filter_cloud_with_outlier error", iter->first );
			t_error.add_error_description(buf, strlen(buf) );
			t_result.compare_and_cover_error(t_error);
			//注:这里不直接return,而是要把map全部执行完成
		}
	}
	if ( t_result != Error_code::SUCCESS)
	{
		return t_result;
	}
	return Error_code::SUCCESS;
}
//过滤离群点,
Error_manager Cnn3d_segmentation::filter_cloud_with_outlier(pcl::PointCloud<pcl::PointXYZRGB>::Ptr p_cloud)
{
	if(p_cloud.get()==NULL)
	{
		return Error_manager(LOCATER_3DCNN_INPUT_CLOUD_UNINIT,MINOR_ERROR,"Cnn3d_segmentation input cloud uninit");
	}
	if(p_cloud->size()<=0)
	{
		return Error_manager(LOCATER_3DCNN_INPUT_CLOUD_EMPTY,MINOR_ERROR,"Cnn3d_segmentation input cloud empty");
	}

	pcl::StatisticalOutlierRemoval<pcl::PointXYZRGB> sorfilter(true); // Initializing with true will allow us to extract the removed indices
	sorfilter.setInputCloud(p_cloud);
	sorfilter.setMeanK(20);
	sorfilter.setStddevMulThresh(2.0);
	sorfilter.filter(*p_cloud);

	return Error_code::SUCCESS;
}

//拆分车轮, 将map分为4个轮胎
//wheel_cloud_map, 输入点云
//cloud_out_vector, 输出4轮点云vector, (vector是空的, 函数内部会添加4个点云)
Error_manager Cnn3d_segmentation::split_wheel_cloud(std::map<int,pcl::PointCloud<pcl::PointXYZRGB>::Ptr>& wheel_cloud_map, bool cloud_aggregation_flag,
													std::vector<pcl::PointCloud<pcl::PointXYZRGB>::Ptr> & wheel_cloud_vector,
													bool save_flag, std::string save_dir)
{
	Error_manager t_error;
	wheel_cloud_vector.clear();

	//如果输入的map只有一个cloud, 那么就要进行聚类, 拆分为4个轮胎
	if ( wheel_cloud_map.size() == 1 && cloud_aggregation_flag == true )
	{
		t_error = split_cloud_with_kmeans(wheel_cloud_map[0], wheel_cloud_vector, save_flag, save_dir);
		//注:此时wheel_clouds_vector里面可能有一个的点云为空的,
		if ( t_error != Error_code::SUCCESS )
		{
			return t_error;
		}
	}
		//如果输入的map有3~4个cloud, 那么就直接复制,
	else if ( (wheel_cloud_map.size() == CNN3D_WHEEL_NUMBER || wheel_cloud_map.size() == CNN3D_WHEEL_NUMBER - 1 )
			  && cloud_aggregation_flag == false )
	{
		for (auto iter = wheel_cloud_map.begin(); iter != wheel_cloud_map.end(); ++iter)
		{
			wheel_cloud_vector.push_back(iter->second);
			//注:此时的 clouds_vector 可能有3~4个点云,
		}
	}
	else
	{
		return Error_manager(Error_code::LOCATER_3DCNN_INPUT_CLOUD_MAP_ERROR, Error_level::MINOR_ERROR,
							 " split_wheel_cloud p_wheel_cloud_map size error ");
	}
	return Error_code::SUCCESS;
}
//聚类, 将一个车轮点云拆分为4个轮胎点云,
//p_cloud_in, 输入点云
//cloud_out_vector, 输出4轮点云vector, (vector是空的, 函数内部会添加4个点云)
Error_manager Cnn3d_segmentation::split_cloud_with_kmeans(pcl::PointCloud<pcl::PointXYZRGB>::Ptr p_cloud_in,
														  std::vector<pcl::PointCloud<pcl::PointXYZRGB>::Ptr> & cloud_out_vector,
														  bool save_flag, std::string save_dir)
{
	if(p_cloud_in.get()==NULL)
	{
		return Error_manager(LOCATER_3DCNN_INPUT_CLOUD_UNINIT,MINOR_ERROR,"Cnn3d_segmentation input cloud uninit");
	}
	if(p_cloud_in->size()<=0)
	{
		return Error_manager(LOCATER_3DCNN_INPUT_CLOUD_EMPTY,MINOR_ERROR,"Cnn3d_segmentation input cloud empty");
	}

	//为cloud_out_vector重新分配内存, 添加4个轮胎点云
	cloud_out_vector.clear();
	for(int i=0;i<CNN3D_WHEEL_NUMBER;++i)
	{
		pcl::PointCloud<pcl::PointXYZRGB>::Ptr t_cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
		cloud_out_vector.push_back(t_cloud);
	}

	//开始分割,
	//注: 实际上只考虑了x和y轴, 以水平面为标准,
	// 根据每个点到的4个角落的距离, 分为4个车轮

	//p_cloud_in转化为opencv的点云
	std::vector<cv::Point2f> cv_points;
	for (int i = 0; i < p_cloud_in->size(); ++i)
	{
		cv::Point2f point(p_cloud_in->points[i].x, p_cloud_in->points[i].y);
		cv_points.push_back(point);
	}
	//rect 为输入点云的外截最小矩形
	cv::RotatedRect rect = cv::minAreaRect(cv_points);
	//corner 为矩形的4个角落的点
	cv::Point2f corner[CNN3D_WHEEL_NUMBER];
	rect.points(corner);

	//遍历输入点云
	for (int i = 0; i < p_cloud_in->size(); ++i)
	{
		//计算出当前点距离哪个角落最近,
		double min_distance = 9999999.9;		//当前点距离角落的最小距离
		int cluste_index = 0;					//当前点最近角落的索引编号
		cv::Point2f point(p_cloud_in->points[i].x, p_cloud_in->points[i].y);
		for (int j = 0; j < CNN3D_WHEEL_NUMBER; ++j)
		{
			double dis=distance(point, corner[j]);
			if (dis < min_distance)
			{
				min_distance = dis;
				cluste_index = j;
			}
		}

		//根据点位的归属, 将其重新标色并填充到对应的输出点云
		if (cluste_index == 0)
		{
			p_cloud_in->points[i].r = 255;
			p_cloud_in->points[i].g = 0;
			p_cloud_in->points[i].b = 0;
		}
		else if (cluste_index == 1)
		{
			p_cloud_in->points[i].r = 0;
			p_cloud_in->points[i].g = 255;
			p_cloud_in->points[i].b = 0;
		}
		else if (cluste_index == 2)
		{
			p_cloud_in->points[i].r = 0;
			p_cloud_in->points[i].g = 0;
			p_cloud_in->points[i].b = 255;
		}
		else
		{
			p_cloud_in->points[i].r = 255;
			p_cloud_in->points[i].g = 255;
			p_cloud_in->points[i].b = 255;
		}
		cloud_out_vector[cluste_index]->push_back(p_cloud_in->points[i]);
	}

	//保存修改颜色后的点云.
	if ( save_flag )
	{
		std::string save_file=save_dir+"/cnn3d_with_kmeans.txt";
		save_rgb_cloud_txt(save_file, p_cloud_in);
	}

	return Error_code::SUCCESS;
}

//pca, 使用pca算法对每个车轮的点云进行过滤,
Error_manager Cnn3d_segmentation::filter_cloud_with_pca(std::vector<pcl::PointCloud<pcl::PointXYZRGB>::Ptr> & cloud_in_vector,
													   std::vector<pcl::PointCloud<pcl::PointXYZRGB>::Ptr> & cloud_out_vector,
													   bool save_flag, std::string save_dir)
{
	Error_manager t_error;
	cloud_out_vector.clear();

	for (int i = 0; i < cloud_in_vector.size(); ++i)
	{
		if ( cloud_in_vector[i]->size() != 0 )
		{
			pcl::PointCloud<pcl::PointXYZRGB>::Ptr tp_cloud_pca(new pcl::PointCloud<pcl::PointXYZRGB>);
			//对点云作pca分析,然后在最小轴方向过滤, 防止轮胎左右两边出现噪声
			t_error = pca_minist_axis_filter(cloud_in_vector[i],tp_cloud_pca);
			if ( t_error != Error_code::SUCCESS )
			{
				return t_error;
			}
			cloud_out_vector.push_back(tp_cloud_pca);

			if ( save_flag )
			{
				char buf[255]={0};
				sprintf(buf,"%s/cnn3d_pca_wheel_%d.txt",save_dir.c_str(),i);
				save_cloudT(buf,*tp_cloud_pca);
			}
		}
	}
	//验证 vector 的大小
	if ( cloud_out_vector.size() <  CNN3D_WHEEL_NUMBER - 1)
	{
		return Error_manager(Error_code::LOCATER_3DCNN_PCA_OUT_ERROR, Error_level::MINOR_ERROR,
							 " wheel_pca_vector.size() < 3  ");
	}
	return Error_code::SUCCESS;
}
//对点云作pca分析,然后在最小轴方向过滤, 防止轮胎左右两边出现噪声
Error_manager Cnn3d_segmentation::pca_minist_axis_filter(pcl::PointCloud<pcl::PointXYZRGB>::Ptr p_cloud_in,
														 pcl::PointCloud<pcl::PointXYZRGB>::Ptr p_cloud_out)
{
	if(p_cloud_in.get()==NULL)
	{
		return Error_manager(LOCATER_3DCNN_INPUT_CLOUD_UNINIT,MINOR_ERROR,"Cnn3d_segmentation input cloud uninit");
	}
	if(p_cloud_in->size()<=0)
	{
		return Error_manager(LOCATER_3DCNN_INPUT_CLOUD_EMPTY,MINOR_ERROR,"Cnn3d_segmentation input cloud empty");
	}

	//pca寻找主轴
	pcl::MomentOfInertiaEstimation <pcl::PointXYZRGB> feature_extractor;
	feature_extractor.setInputCloud (p_cloud_in);
	feature_extractor.compute ();

	std::vector <float> moment_of_inertia;
	std::vector <float> eccentricity;
	pcl::PointXYZRGB min_point_AABB;
	pcl::PointXYZRGB max_point_AABB;
	pcl::PointXYZRGB min_point_OBB;
	pcl::PointXYZRGB max_point_OBB;
	pcl::PointXYZRGB position_OBB;
	Eigen::Matrix3f rotational_matrix_OBB;
	float major_value, middle_value, minor_value;
	Eigen::Vector3f major_vector, middle_vector, minor_vector;
	Eigen::Vector3f mass_center;

	feature_extractor.getMomentOfInertia (moment_of_inertia);
	feature_extractor.getEccentricity (eccentricity);
	feature_extractor.getAABB (min_point_AABB, max_point_AABB);
	feature_extractor.getOBB (min_point_OBB, max_point_OBB, position_OBB, rotational_matrix_OBB);
	feature_extractor.getEigenValues (major_value, middle_value, minor_value);
	feature_extractor.getEigenVectors (major_vector, middle_vector, minor_vector);
	feature_extractor.getMassCenter (mass_center);

	//根据minor_vector 最小主轴方向,以及中心点, 计算轴所在三维平面
	float x=mass_center[0];
	float y=mass_center[1];
	float z=mass_center[2];
	float a=minor_vector[0];
	float b=minor_vector[1];
	float c=minor_vector[2];
	float d=-(a*x+b*y+c*z);

	//计算各点距离平面的距离,距离操过轮胎的一半厚,舍弃(100mm)
	float S=sqrt(a*a+b*b+c*c);
	for(int i=0;i<p_cloud_in->size();++i)
	{
		pcl::PointXYZRGB point=p_cloud_in->points[i];
		if(fabs(point.x*a+point.y*b+point.z*c+d)/S <40 )
		{
			p_cloud_out->push_back(point);
		}
	}

	/*if(cloud_out->size()==0)
	{
		return Error_manager(LOCATER_3DCNN_PCA_OUT_CLOUD_EMPTY,MINOR_ERROR,"wheel pca filter out cloud empty");
	}*/
	return SUCCESS;
}

//提取关键矩形,
//cloud_in_vector, 输入点云
//rect_of_wheel_center, 输出4轮中心点构成的矩形
//rect_of_wheel_cloud, 输出所有车轮点构成的矩形
Error_manager Cnn3d_segmentation::extract_key_rect(std::vector<pcl::PointCloud<pcl::PointXYZRGB>::Ptr> & cloud_in_vector,
												   cv::RotatedRect & rect_of_wheel_center, cv::RotatedRect & rect_of_wheel_cloud,
												   bool save_flag, std::string save_dir)
{
	//验证 vector 的大小
	if ( cloud_in_vector.size() <  CNN3D_WHEEL_NUMBER - 1)
	{
		return Error_manager(Error_code::LOCATER_3DCNN_EXTRACT_RECT_ERROR, Error_level::MINOR_ERROR,
							 " cloud_in_vector.size() < 3  ");
	}

	Error_manager t_error;

	//4个轮子的中心点, 只要x和y轴的坐标,
	std::vector<cv::Point2f> wheel_center_vector;
	//取出轮胎的中心点
	for (int i = 0; i < cloud_in_vector.size(); ++i)
	{
		if ( cloud_in_vector[i]->size() != 0 )
		{
			//提取每个车轮中心点
			Eigen::Vector4f centroid;
			pcl::compute3DCentroid(*(cloud_in_vector[i]), centroid);

			wheel_center_vector.push_back(cv::Point2f(centroid[0],centroid[1]));
		}
	}
	//验证 vector 的大小
	int t_vector_size = wheel_center_vector.size();
	if ( wheel_center_vector.size() <  CNN3D_WHEEL_NUMBER - 1)
	{
		return Error_manager(Error_code::LOCATER_3DCNN_EXTRACT_RECT_ERROR, Error_level::MINOR_ERROR,
							 " wheel_center_vector.size() < 3  ");
	}

	//检查并修复关键点, 此时可以通过3个轮胎模拟出第四个轮胎的中心点位置, 并添加到 vector
//	t_error = check_and_repair_center(wheel_center_vector);
	if ( t_error != Error_code::SUCCESS )
	{
		return t_error;
	}
	//rect_of_wheel_center, 输出4轮中心点构成的矩形
	rect_of_wheel_center = minAreaRect_cloud(wheel_center_vector);
//	t_error = check_rect_size(rect_of_wheel_center);
	if ( t_error != Error_code::SUCCESS )
	{
		return t_error;
	}

	//将输入的车轮点云, 汇总为 cv::Point2f的vector
	std::vector<cv::Point2f> wheel_points_vector;
	for(int i=0;i<cloud_in_vector.size();++i)
	{
		for(int j=0;j<cloud_in_vector[i]->size();++j)
			wheel_points_vector.push_back(cv::Point2f(cloud_in_vector[i]->points[j].x,
													  cloud_in_vector[i]->points[j].y));
	}
	//如果前面check_and_repair_center为wheel_center_vector添加了第四个中心点, 那么也要加入到wheel_points_vector
	if(t_vector_size == CNN3D_WHEEL_NUMBER - 1 &&  wheel_center_vector.size() == CNN3D_WHEEL_NUMBER)
	{
		wheel_points_vector.push_back(wheel_center_vector[CNN3D_WHEEL_NUMBER - 1]);
	}

	//rect_of_wheel_cloud, 输出所有车轮点构成的矩形
	rect_of_wheel_cloud = minAreaRect_cloud(wheel_points_vector);


	return Error_code::SUCCESS;
}

//检查并修复关键点, 此时可以通过3个轮胎模拟出第四个轮胎的中心点位置, 并添加到 vector
Error_manager Cnn3d_segmentation::check_and_repair_center(std::vector<cv::Point2f> & cloud_centers)
{
	//取四轮中心计算轴长
	return centers_is_rect(cloud_centers);
}
//判断矩形框是否找到
//points:输入轮子中心点, 只能是3或者4个
Error_manager Cnn3d_segmentation::centers_is_rect(std::vector<cv::Point2f>& points)
{
	if (points.size() == 4)
	{
		double L[3] = {0.0};
		L[0] = distance(points[0], points[1]);
		L[1] = distance(points[1], points[2]);
		L[2] = distance(points[0], points[2]);
		double max_l = L[0];
		double l1 = L[1];
		double l2 = L[2];
		int max_index = 0;
		cv::Point2f ps = points[0], pt = points[1];
		cv::Point2f pc = points[2];
		for (int i = 1; i < 3; ++i) {
			if (L[i] > max_l) {
				max_index = i;
				max_l = L[i];
				l1 = L[abs(i + 1) % 3];
				l2 = L[abs(i + 2) % 3];
				ps = points[i % 3];
				pt = points[(i + 1) % 3];
				pc = points[(i + 2) % 3];
			}
		}
		double cosa = (l1 * l1 + l2 * l2 - max_l * max_l) / (2.0 * l1 * l2);
		if (fabs(cosa) >= 0.13) {
			char description[255]={0};
			sprintf(description,"angle cos value(%.2f) >0.13 ",cosa);
			return Error_manager(LOCATER_3DCNN_VERIFY_RECT_FAILED_4,MINOR_ERROR,description);
		}

		float width=std::min(l1,l2);
		float length=std::max(l1,l2);
		if(width<1400 || width >2000 || length >3300 ||length < 2200)
		{
			char description[255]={0};
			sprintf(description,"width<1400 || width >2100 || length >3300 ||length < 2100 l:%.1f,w:%.1f",length,width);
			return Error_manager(LOCATER_3DCNN_VERIFY_RECT_FAILED_4,MINOR_ERROR,description);
		}

		double d = distance(pc, points[3]);
		cv::Point2f center1 = (ps + pt) * 0.5;
		cv::Point2f center2 = (pc + points[3]) * 0.5;
		if (fabs(d - max_l) > max_l * 0.1 || distance(center1, center2) > 150) {
			LOG(INFO) << "d:" << d << " maxl:" << max_l << "  center1:" << center1 << "  center2:" << center2;
			char description[255]={0};
			sprintf(description,"Verify failed-4  fabs(d - max_l) > max_l * 0.1 || distance(center1, center2) > 150 ");
			return Error_manager(LOCATER_3DCNN_VERIFY_RECT_FAILED_4,MINOR_ERROR,description);
		}
		LOG(INFO) << " rectangle verify OK  cos angle=" << cosa << "  length off=" << fabs(d - max_l)
				  << "  center distance=" << distance(center1, center2);
		return SUCCESS;
	}
	else if(points.size()==3)
	{
		double L[3] = {0.0};
		L[0] = distance(points[0], points[1]);
		L[1] = distance(points[1], points[2]);
		L[2] = distance(points[0], points[2]);
		double max_l = L[0];
		double l1 = L[1];
		double l2 = L[2];
		int max_index = 0;
		cv::Point2f ps = points[0], pt = points[1];
		cv::Point2f pc = points[2];
		for (int i = 1; i < 3; ++i) {
			if (L[i] > max_l) {
				max_index = i;
				max_l = L[i];
				l1 = L[abs(i + 1) % 3];
				l2 = L[abs(i + 2) % 3];
				ps = points[i % 3];
				pt = points[(i + 1) % 3];
				pc = points[(i + 2) % 3];
			}
		}
		double cosa = (l1 * l1 + l2 * l2 - max_l * max_l) / (2.0 * l1 * l2);
		if (fabs(cosa) >= 0.12) {
			char description[255]={0};
			sprintf(description,"3 wheels angle cos value(%.2f) >0.12 ",cosa);
			return Error_manager(LOCATER_3DCNN_VERIFY_RECT_FAILED_3,MINOR_ERROR,description);
		}

		double l=std::max(l1,l2);
		double w=std::min(l1,l2);
		if(l>2100 && l<3300 && w>1400 && w<2100)
		{
			//生成第四个点
			cv::Point2f vec1=ps-pc;
			cv::Point2f vec2=pt-pc;
			cv::Point2f point4=(vec1+vec2)+pc;
			points.push_back(point4);
			LOG(INFO) << "3 wheels rectangle verify OK  cos angle=" << cosa << "  L=" << l
					  << "  w=" << w;
			return SUCCESS;
		}
		else
		{

			char description[255]={0};
			sprintf(description,"3 wheels rectangle verify Failed  cos angle=%.2f,L=%.1f, w=%.1f",cosa,l,w);
			return Error_manager(LOCATER_3DCNN_VERIFY_RECT_FAILED_3,MINOR_ERROR,description);
		}

	}
	return Error_code::SUCCESS;
}

//检查矩形的大小
Error_manager Cnn3d_segmentation::check_rect_size(cv::RotatedRect& rect)
{
	if ( std::min(rect.size.width, rect.size.height)>2.000 || std::min(rect.size.width, rect.size.height)<1.200)
	{
		return Error_manager(Error_code::LOCATER_3DCNN_RECT_SIZE_ERROR, Error_level::MINOR_ERROR,
							 " check_rect_size error ");
	}
	if (std::max(rect.size.width, rect.size.height) < 1.900 || std::max(rect.size.width, rect.size.height) > 3.600)
	{
		return Error_manager(Error_code::LOCATER_3DCNN_RECT_SIZE_ERROR, Error_level::MINOR_ERROR,
							" check_rect_size error ");
	}
	return Error_code::SUCCESS;
}


//通过关键矩形, 计算出车辆的定位信息, (输出中心点的x和y, 前后轮距, 左右轮距, 旋转角)
Error_manager Cnn3d_segmentation::calculate_key_rect(cv::RotatedRect & rect_of_wheel_center, cv::RotatedRect & rect_of_wheel_cloud,
								 Locate_information* p_locate_information)
{
	if ( p_locate_information == NULL )
	{
	    return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR,
	    					" calculate_key_rect input parameter POINTER_IS_NULL ");
	}

	//计算中心点
	p_locate_information->locate_x=rect_of_wheel_center.center.x;
	p_locate_information->locate_y=rect_of_wheel_center.center.y;
	//计算轮距, 矩形的长边为前后轮距, 短边为左右轮距
	p_locate_information->locate_wheel_base=std::max(rect_of_wheel_center.size.height,rect_of_wheel_center.size.width);
	p_locate_information->locate_wheel_width=std::min(rect_of_wheel_center.size.height,rect_of_wheel_center.size.width);
	//计算旋转角
	const double C_PI=3.14159265;
	cv::Point2f vec;
	cv::Point2f vertice[4];
	rect_of_wheel_cloud.points(vertice);
	float len1 = pow(vertice[0].x - vertice[1].x, 2.0) + pow(vertice[0].y - vertice[1].y, 2.0);
	float len2 = pow(vertice[1].x - vertice[2].x, 2.0) + pow(vertice[1].y - vertice[2].y, 2.0);
	if (len1 > len2) {
		vec.x = vertice[0].x - vertice[1].x;
		vec.y = vertice[0].y - vertice[1].y;
	}
	else
	{
		vec.x = vertice[1].x - vertice[2].x;
		vec.y = vertice[1].y - vertice[2].y;
	}

	p_locate_information->locate_angle = 180.0 / C_PI * acos(vec.x / sqrt(vec.x * vec.x + vec.y * vec.y));

	LOG(INFO)<<"3dcnn rotate rect center :"<<rect_of_wheel_center.center<<"   size : "<<rect_of_wheel_center.size<<"  angle:"<<p_locate_information->locate_angle;

	return SUCCESS;
}