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

double IIU(std::vector<cv::Point>& poly1, std::vector<cv::Point>& poly2, float max_x, float max_y)
{
	int width = int(max_x) + 100;
	int height = int(max_y) + 100;
	//��ͼ1
	cv::Mat src1 = cv::Mat::zeros(cv::Size(width, height), CV_8UC1);
	cv::Mat src2 = cv::Mat::zeros(cv::Size(width, height), CV_8UC1);
	std::vector<std::vector<cv::Point> > polys1;
	std::vector<std::vector<cv::Point> > polys2;
	polys1.push_back(poly1);
	polys2.push_back(poly2);
	cv::fillPoly(src1, polys1, cv::Scalar(1));
	cv::fillPoly(src2, polys2, cv::Scalar(1));

	cv::Mat I = src1&src2;
	//cv::Mat U = src1 | src2;
	cv::Scalar S_I = cv::sum(I);
	cv::Scalar S_U = cv::sum(src1);

	if (S_U[0] == 0) return 0;

	return double(S_I[0]) / double(S_U[0]);

}

float* Cnn3d_segmentation::generate_tensor(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, float min_x, float max_x,
	float min_y, float max_y, float min_z, float max_z)
{
	int size = m_lenth*m_width*m_height;
	float* data = (float*)malloc(size*sizeof(float));
	memset(data, 0, size*sizeof(float));
	for (int i = 0; i < cloud->size(); ++i)
	{
		pcl::PointXYZ point = cloud->points[i];
		int new_x = (point.x - min_x) / (max_x - min_x)*m_lenth;
		int new_y = (point.y - min_y) / (max_y - min_y)*m_width;
		int new_z = (point.z - min_z) / (max_z - min_z)*m_height;
		if (new_x < 0 || new_x >= m_lenth || new_y < 0 || new_y >= m_width || new_z < 0 || new_z >= m_height)
		{
			continue;
		}
		*(data + new_x*(m_width*m_height) + new_y*m_height + new_z) = 1.0;
	}
	return data;
}

std::vector<pcl::PointCloud<pcl::PointXYZRGB>::Ptr> Cnn3d_segmentation::decodeCloud(pcl::PointCloud<pcl::PointXYZ>& cloud,
		float* data, float min_x, float max_x, float min_y, float max_y, float min_z, float max_z)
{
	std::vector<pcl::PointCloud<pcl::PointXYZRGB>::Ptr> clouds;
	for (int i = 0; i < m_nClass - 1; ++i)
		clouds.push_back(pcl::PointCloud<pcl::PointXYZRGB>::Ptr(new pcl::PointCloud<pcl::PointXYZRGB>));
	for (int i = 0; i < cloud.size(); ++i)
	{
		pcl::PointXYZ point = cloud.points[i];
		int new_x = (point.x - min_x) / (max_x - min_x)*m_lenth;
		int new_y = (point.y - min_y) / (max_y - min_y)*m_width;
		int new_z = (point.z - min_z) / (max_z - min_z)*m_height;
		if (new_x < 0 || new_x >= m_lenth || new_y < 0 || new_y >= m_width || new_z < 0 || new_z >= m_height)
		{
			continue;
		}
		int max_index = 0;
		float max_prob = *(data + new_x*(m_width*m_height*m_nClass) + new_y*m_height*m_nClass + new_z*m_nClass + 0);

		for (int j = 0; j < m_nClass; ++j)
		{
			if (*(data + new_x*(m_width*m_height*m_nClass) + new_y*m_height*m_nClass + new_z*m_nClass + j) > max_prob)
			{
				max_prob = *(data + new_x*(m_width*m_height*m_nClass) + new_y*m_height*m_nClass + new_z*m_nClass + j);
				max_index = j;
			}
		}

		pcl::PointXYZRGB point_color;
		point_color.x = point.x;
		point_color.y = point.y;
		point_color.z = point.z;
		point_color.r = 255;
		point_color.g = 255;
		point_color.b = 255;
		switch (max_index)
		{
		case 1:
			point_color.r = 0;
			point_color.g = 255;
			point_color.b = 0;
			clouds[0]->push_back(point_color);
			break;
		case 2:
			point_color.r = 255;
			point_color.g = 0;
			point_color.b = 0;
			clouds[1]->push_back(point_color);
			break;
		default:break;
		}

	}
	return clouds;
}

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,NORMAL,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,NORMAL,"first locate 3dcnn failed");
	}
	free(data);
	free(pout);
	return SUCCESS;*/
}

std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> Cnn3d_segmentation::kmeans(
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud,std::string file_path)
{
    std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> kmens_out_clouds;
    for(int i=0;i<4;++i)
    {
        pcl::PointCloud<pcl::PointXYZ>::Ptr t_cloud(new pcl::PointCloud<pcl::PointXYZ>);
        kmens_out_clouds.push_back(t_cloud);
    }

    //开始分割
	std::vector<cv::Point2f> cv_points;
	for (int i = 0; i < cloud->size(); ++i)
	{
		cv::Point2f point(cloud->points[i].x, cloud->points[i].y);
		cv_points.push_back(point);
	}
	cv::RotatedRect rect = cv::minAreaRect(cv_points);
	cv::Point2f corner[4];
	rect.points(corner);

	std::vector<cv::Point2f> clusters[4];
	double sumX[4] = { 0.0};
	double sumY[4] = { 0.0 };
	for (int i = 0; i < cloud->size(); ++i)
	{
		double min_distance = 9999999.9;
		int cluste_index = 0;
		cv::Point2f point(cloud->points[i].x, cloud->points[i].y);
		for (int j = 0; j < 4; ++j)
		{
			double dis=distance(point, corner[j]);
			if (dis < min_distance)
			{
				min_distance = dis;
				cluste_index = j;
			}
		}
		clusters[cluste_index].push_back(point);
		sumX[cluste_index] += point.x;
		sumY[cluste_index] += point.y;

        pcl::PointXYZ t_point3d;
        t_point3d.x=cloud->points[i].x;
        t_point3d.y=cloud->points[i].y;
        t_point3d.z=cloud->points[i].z;
        if(cluste_index>=0&&cluste_index<4)
        {
            kmens_out_clouds[cluste_index]->push_back(t_point3d);
        }

		if (cluste_index == 0)
		{
			cloud->points[i].r = 255;
			cloud->points[i].g = 0;
			cloud->points[i].b = 0;
		}
		else if (cluste_index == 1)
		{
			cloud->points[i].r = 0;
			cloud->points[i].g = 255;
			cloud->points[i].b = 0;
		}
		else if (cluste_index == 2)
		{
			cloud->points[i].r = 0;
			cloud->points[i].g = 0;
			cloud->points[i].b = 255;
		}
		else
		{

			cloud->points[i].r = 255;
			cloud->points[i].g = 255;
			cloud->points[i].b = 255;
		}
	}
	std::vector<cv::Point2f> rets;
	for (int i = 0; i < 4; ++i)
	{
		if (clusters[i].size() == 0)
		{
			continue;
		}
		float avg_x = sumX[i] / float(clusters[i].size());
		float avg_y = sumY[i] / float(clusters[i].size());
		rets.push_back(cv::Point2f(avg_x, avg_y));

	}
	std::string save_file=file_path+"/cnn3d.txt";
	save_rgb_cloud_txt(save_file, cloud);
	return kmens_out_clouds;


	/*int clusterCount = 4;
	int sampleCount = cloud->size();

	cv::Mat points(sampleCount, 1, CV_32FC2), labels;   //��������������ʵ����Ϊ2ͨ������������Ԫ������ΪPoint2f
	for (int i = 0; i < sampleCount; ++i)
	{
		points.at<cv::Point2f>(i) = cv::Point2f(cloud->points[i].x, cloud->points[i].y);
	}

	clusterCount = MIN(clusterCount, sampleCount);
	cv::Mat centers(clusterCount, 1, points.type());    //�����洢���������ĵ�
	cv::RNG rng(12345); //�����������


	cv::kmeans(points, clusterCount, labels,
		cv::TermCriteria(CV_TERMCRIT_EPS + CV_TERMCRIT_ITER, 500, 15.0),
		5, cv::KMEANS_PP_CENTERS, centers);



	for (int i = 0; i < sampleCount; i++)
	{
		int clusterIdx = labels.at<int>(i);
		pcl::PointXYZ point;
		point.x=cloud->points[i].x;
        point.y=cloud->points[i].y;
        point.z=cloud->points[i].z;
        if(clusterIdx>=0&&clusterIdx<4)
        {
            kmens_out_clouds[clusterIdx]->push_back(point);
        }
		if (clusterIdx == 0)
		{
			cloud->points[i].r = 255;
			cloud->points[i].g = 0;
			cloud->points[i].b = 0;

		}
		else if (clusterIdx == 1)
		{
			cloud->points[i].r = 0;
			cloud->points[i].g = 255;
			cloud->points[i].b = 0;
		}
		else if (clusterIdx == 2)
		{
			cloud->points[i].r = 0;
			cloud->points[i].g = 0;
			cloud->points[i].b = 255;
		}
		else
		{

			cloud->points[i].r = 255;
			cloud->points[i].g = 255;
			cloud->points[i].b = 255;
		}

	}
    std::string save_file=file_path+"/cnn3d.txt";
	save_rgb_cloud_txt(save_file, cloud);*/
	return kmens_out_clouds;
}

Error_manager Cnn3d_segmentation::isRect(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,NORMAL,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 || wheel_base >3300 ||wheel_base < 2100 l:%.1f,w:%.1f",length,width);
            return Error_manager(LOCATER_3DCNN_VERIFY_RECT_FAILED_4,NORMAL,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,NORMAL,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,NORMAL,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,NORMAL,description);
        }

    }

}

void save_cloudT(std::string txt, pcl::PointCloud<pcl::PointXYZ>& pCloud)
{
    std::ofstream os;
    os.open(txt, std::ios::out);
    for (int i = 0; i < pCloud.points.size(); i++)
    {
        pcl::PointXYZ 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::predict(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud,
                                          float& center_x,float& center_y,float& wheel_base,
    float& width,float& angle,std::string cluster_file_path)
{
    if(cloud.get()==NULL)
    {
        return Error_manager(LOCATER_3DCNN_INPUT_CLOUD_EMPTY,NORMAL,"3dcnn predict input cloud uninit");
    }
    if(cloud->size()==0)
    {
        return Error_manager(LOCATER_3DCNN_INPUT_CLOUD_EMPTY,NORMAL,"3dcnn predict input cloud empty");
    }

	clock_t clock1 = clock();

    pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudout(new pcl::PointCloud<pcl::PointXYZRGB>);
    pcl::copyPointCloud(*cloud,*cloudout);


	pcl::PointCloud<pcl::PointXYZRGB>::Ptr last_cloud(new pcl::PointCloud<pcl::PointXYZRGB>);

	if (cloudout->size() == 0)
	{
		return Error_manager(LOCATER_3DCNN_PREDICT_FAILED,NORMAL,"tf pred class[wheel].points size ==0");
	}
    //离群点过滤
	pcl::StatisticalOutlierRemoval<pcl::PointXYZRGB> sorfilter(true); // Initializing with true will allow us to extract the removed indices
	sorfilter.setInputCloud(cloudout);
	sorfilter.setMeanK(20);
	sorfilter.setStddevMulThresh(2.0);
	sorfilter.filter(*cloudout);

	if (cloudout->size() < 4 )
	{
		return Error_manager(LOCATER_3DCNN_PREDICT_FAILED,NORMAL,"tf pred wheel points size <4");
	}
	//kmeans
    std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> kmeans_out_clouds = kmeans(cloudout, cluster_file_path);
	//对kmeans结果点云作pca filter
    std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> pca_filter_clouds;
    for(int i=0;i<4;++i)
    {
        pcl::PointCloud<pcl::PointXYZ>::Ptr t_cloud(new pcl::PointCloud<pcl::PointXYZ>);
        pca_filter_clouds.push_back(t_cloud);
    }
	for(int i=0;i<kmeans_out_clouds.size();++i)
    {
	    pca_minist_axis_filter(kmeans_out_clouds[i],pca_filter_clouds[i]);

        char buf[255]={0};
        sprintf(buf,"%s/pca_wheel_%d.txt",cluster_file_path.c_str(),i);
        save_cloudT(buf,*pca_filter_clouds[i]);
    }
    //计算pca filter点云中心
    std::vector<cv::Point2f> centers;
    for(int i=0;i<pca_filter_clouds.size();++i)
    {
        if(pca_filter_clouds[i]->size()!=0) {
            Eigen::Vector4f centroid;
            pcl::compute3DCentroid(*pca_filter_clouds[i], centroid);
            centers.push_back(cv::Point2f(centroid[0],centroid[1]));
        }

    }
    //将pca过滤后的轮胎点转换成opencv点,用于计算角度与宽
	std::vector<cv::Point2f> wheel_points;
	for(int i=0;i<pca_filter_clouds.size();++i)
    {
	    for(int j=0;j<pca_filter_clouds[i]->size();++j)
	        wheel_points.push_back(cv::Point2f(pca_filter_clouds[i]->points[j].x,
                                               pca_filter_clouds[i]->points[j].y));
    }
	if (centers.size() != 4 && centers.size()!=3)
	{
		return Error_manager(LOCATER_3DCNN_KMEANS_FAILED,NORMAL,"3dcnn cluster center size != 4 & 3");
	}
	//取四轮中心计算轴长
	Error_manager code=isRect(centers);
	if(code!=SUCCESS)
	{
		return code;
	}

    if(centers.size()==3)
    {
        wheel_points.push_back(centers[centers.size()-1]);
    }

    cv::RotatedRect rect_center = minAreaRect_cloud(centers);
    cv::RotatedRect rect_wheels=minAreaRect_cloud(wheel_points);
    bool IOU = check_box(rect_center, cloud);
    if(IOU==false){
        return Error_manager(LOCATER_3DCNN_IIU_FAILED,NORMAL,"IIU check box failed");
    }
	//开始赋值
    center_x=rect_center.center.x;
    center_y=rect_center.center.y;
    wheel_base=std::max(rect_center.size.height,rect_center.size.width);
    width=std::min(rect_center.size.height,rect_center.size.width);
    //根据 rect_wheel 计算角度
    const double C_PI=3.14159265;
    cv::Point2f vec;
    cv::Point2f vertice[4];
    rect_wheels.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;
    }
    angle = 180.0 / C_PI * acos(vec.x / sqrt(vec.x * vec.x + vec.y * vec.y));


    LOG(INFO)<<"3dcnn rotate rect center :"<<rect_center.center<<"   size : "<<rect_center.size<<"  angle:"<<angle;

	return SUCCESS;
}

Error_manager Cnn3d_segmentation::pca_minist_axis_filter(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in,
                                                              pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_out)
{
    if(cloud_in.get()==NULL)
    {
        return Error_manager(PARAMETER_ERROR,NORMAL,"pca_minist_axis_filter  input cloud uninit");
    }
    if(cloud_in->size()==0)
    {
        return Error_manager(PARAMETER_ERROR,NORMAL,"pca_minist_axis_filter  input cloud empty");
    }

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

    std::vector <float> moment_of_inertia;
    std::vector <float> eccentricity;
    pcl::PointXYZ min_point_AABB;
    pcl::PointXYZ max_point_AABB;
    pcl::PointXYZ min_point_OBB;
    pcl::PointXYZ max_point_OBB;
    pcl::PointXYZ 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<cloud_in->size();++i)
    {
        pcl::PointXYZ point=cloud_in->points[i];
        if(fabs(point.x*a+point.y*b+point.z*c+d)/S <40 )
        {
            cloud_out->push_back(point);
        }
    }

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

bool Cnn3d_segmentation::check_box(cv::RotatedRect& box, pcl::PointCloud<pcl::PointXYZ>::Ptr cloud)
{
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_projected_XY(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients());
	coefficients->values.resize(4);
	coefficients->values[0] = 0.;
	coefficients->values[1] = 0.;
	coefficients->values[2] = 1.0;
	coefficients->values[3] = 0.;

	// Create the filtering object
	pcl::ProjectInliers<pcl::PointXYZ> proj;
	proj.setModelType(pcl::SACMODEL_PLANE);
	proj.setInputCloud(cloud);
	proj.setModelCoefficients(coefficients);
	proj.filter(*cloud_projected_XY);

	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_convexhull(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::ConvexHull<pcl::PointXYZ> cconvexhull;
	cconvexhull.setInputCloud(cloud_projected_XY);
	cconvexhull.setDimension(2);
	cconvexhull.reconstruct(*cloud_convexhull);
	//����IOU
	pcl::PointXYZ minPt, maxPt;
	pcl::getMinMax3D(*cloud_convexhull, minPt, maxPt);
	cv::Point2f vertice[4];
	box.points(vertice);
	std::vector<cv::Point> poly;
	poly.push_back(vertice[0]);
	poly.push_back(vertice[1]);
	poly.push_back(vertice[2]);
	poly.push_back(vertice[3]);

	std::vector<cv::Point> poly1;
	for (int i = 0; i < cloud_convexhull->points.size(); ++i)
	{
		pcl::PointXYZ pt = cloud_convexhull->points[i];
		cv::Point2f point(pt.x, pt.y);
		poly1.push_back(point);
	}
	//double area_percent = IIU(poly, poly1, maxPt.x, maxPt.y);
	if (/*area_percent < 0.95 ||*/ std::min(box.size.width, box.size.height)>2000.0 || std::min(box.size.width, box.size.height)<1200.0)
	{
		return false;
	}
	if (std::max(box.size.width, box.size.height) < 1900 || std::max(box.size.width, box.size.height) > 3600)
	{
		LOG(ERROR) << "\tlen not in 1900-3400";
		return false;
	}
	return true;
}