去除3dcnn网络, 改为pointsift直接分割车轮

CMakeLists.txt

@@ -40,8 +40,11 @@ aux_source_directory(${CMAKE_CURRENT_LIST_DIR}/terminor TERMINOR_SRC )
 aux_source_directory(${CMAKE_CURRENT_LIST_DIR}/verify VERIFY_SRC )
 aux_source_directory(${CMAKE_CURRENT_LIST_DIR}/system_manager SYS_SRC )
 
-#add_executable(LidarMeasure ./main.cpp ${LASER_SRC} ${PLC_SRC} ${TERMINOR_SRC} ${LOCATE_SRC} ${TASK_MANAGER_SRC})
-#
+add_executable(Test_locate ./test/locate_sample.cpp ${ERROR_SRC} ${TOOL_SRC} ${LASER_SRC} ${LOCATE_SRC} ${TASK_MANAGER_SRC})
+target_link_libraries(Test_locate ${OpenCV_LIBS}
+        ${GLOG_LIBRARIES} ${PCL_LIBRARIES} ${PROTOBUF_LIBRARIES} libtensorflow_cc.so
+        tf_3dcnn_api.so pointSIFT_API.so dark.so /usr/local/lib/libglog.a
+        /usr/local/lib/libgflags.a /usr/local/lib/liblivox_sdk_static.a /usr/local/apr/lib/libapr-1.a nnxx nanomsg)
 
 
 add_executable(locate  main.cpp ${LOCATE_SRC} ${PLC_SRC} ${TERMINOR_SRC} ${TASK_MANAGER_SRC} ${LASER_SRC} ${ERROR_SRC}

error_code/error_code.h

@@ -136,6 +136,7 @@ enum Error_code
     LOCATER_3DCNN_KMEANS_FAILED,
     LOCATER_3DCNN_IIU_FAILED,
     LOCATER_3DCNN_INPUT_CLOUD_EMPTY,
+    LOCATER_3DCNN_PCA_OUT_CLOUD_EMPTY,
 
     //System_manager error from 0x04010000-0x0401FFFF
     SYSTEM_READ_PARAMETER_ERROR=0x04010100,

locate/cnn3d_segmentation.cpp

@@ -8,7 +8,8 @@
 #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;
@@ -169,13 +170,15 @@ Cnn3d_segmentation::Cnn3d_segmentation(int l, int w, int h,int freq,int classes)
 
 Cnn3d_segmentation::~Cnn3d_segmentation()
 {
-	tf_wheel_3dcnn_close();
+    //3dcnn 原校验功能保留,不加载网络
+	//tf_wheel_3dcnn_close();
 }
 
 Error_manager Cnn3d_segmentation::init(std::string weights)
 {
-
-	if (0 != tf_wheel_3dcnn_init(weights, m_lenth,m_width,m_height))
+    //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;
@@ -193,11 +196,20 @@ Error_manager Cnn3d_segmentation::init(std::string weights)
 	}
 	free(data);
 	free(pout);
-	return SUCCESS;
+	return SUCCESS;*/
 }
 
-std::vector<cv::Point2f> Cnn3d_segmentation::kmeans(pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud, std::string file_path)
+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)
 	{
@@ -228,6 +240,16 @@ std::vector<cv::Point2f> Cnn3d_segmentation::kmeans(pcl::PointCloud<pcl::PointXY
 		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;
@@ -268,7 +290,7 @@ std::vector<cv::Point2f> Cnn3d_segmentation::kmeans(pcl::PointCloud<pcl::PointXY
 	}
 	std::string save_file=file_path+"/cnn3d.txt";
 	save_rgb_cloud_txt(save_file, cloud);
-	return rets;
+	return kmens_out_clouds;
 
 
 	/*int clusterCount = 4;
@@ -283,24 +305,31 @@ std::vector<cv::Point2f> Cnn3d_segmentation::kmeans(pcl::PointCloud<pcl::PointXY
 	clusterCount = MIN(clusterCount, sampleCount);
 	cv::Mat centers(clusterCount, 1, points.type());    //�����洢���������ĵ�
 	cv::RNG rng(12345); //�����������
-	/ * generate random sample from multigaussian distribution * /
+
 
 	cv::kmeans(points, clusterCount, labels,
 		cv::TermCriteria(CV_TERMCRIT_EPS + CV_TERMCRIT_ITER, 500, 15.0),
-		5, cv::KMEANS_PP_CENTERS, centers);  //����3�Σ�ȡ�����õ��ǴΣ�����ij�ʼ������PP�ض�������㷨��
+		5, cv::KMEANS_PP_CENTERS, centers);
+
 
-	std::vector<cv::Point2f> rets;
-	for (int i = 0; i < clusterCount; ++i)
-		rets.push_back(centers.at<cv::Point2f>(i));
 
 	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)
 		{
@@ -323,8 +352,9 @@ std::vector<cv::Point2f> Cnn3d_segmentation::kmeans(pcl::PointCloud<pcl::PointXY
 		}
 
 	}
-	save_rgb_cloud_txt(sava_path + "/kmeans.txt", cloud);
-	return rets;*/
+    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)
@@ -451,98 +481,192 @@ void save_cloudT(std::string txt, pcl::PointCloud<pcl::PointXYZ>& pCloud)
     os.close();
 }
 
-Error_manager Cnn3d_segmentation::predict(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, cv::RotatedRect& rect,
-    std::string cluster_file_path)
+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");
     }
 
-	pcl::PointXYZ minp, maxp;
-	pcl::getMinMax3D(*cloud, minp, maxp);
-	float center_x = (minp.x + maxp.x) / 2.0;
-	float center_y = (minp.y + maxp.y) / 2.0;
-	float center_z = (minp.z + maxp.z) / 2.0;
-	float min_x = center_x - m_lenth / 2.0*m_freq;
-	float max_x = center_x + m_lenth / 2.0*m_freq;
-	float min_y = center_y - m_width / 2.0*m_freq;
-	float max_y = center_y + m_width / 2.0*m_freq;
-	float min_z = center_z - m_height / 2.0*m_freq;
-	float max_z = center_z + m_height / 2.0*m_freq;
-
-	float* data = generate_tensor(cloud, min_x, max_x, min_y, max_y, min_z, max_z);
-	float* pout = (float*)malloc(m_lenth*m_width*m_height*m_nClass*sizeof(float));
 	clock_t clock1 = clock();
-	if (!tf_wheel_3dcnn_predict(data, pout))
-	{
-		free(data);
-		free(pout);
-		return Error_manager(LOCATER_3DCNN_PREDICT_FAILED,NORMAL,"3dcnn first predict failed");
-	}
 
-	std::vector<pcl::PointCloud<pcl::PointXYZRGB>::Ptr> cloudout = decodeCloud(*cloud, pout, min_x, max_x, min_y, max_y, min_z, max_z);
-	free(data);
-	free(pout);
+    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>);
-	for (int i = 0; i < cloudout.size(); ++i)
-	{
-		last_cloud->operator+=(*cloudout[i]);
-	}
 
-	if (cloudout.size() == 0)
-	{
-		return Error_manager(LOCATER_3DCNN_PREDICT_FAILED,NORMAL,"tf pred classes ==0");
-	}
-	if (cloudout[0]->size() == 0)
+	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[0]);
-	sorfilter.setMeanK(15);
-	sorfilter.setStddevMulThresh(4.0);
-	sorfilter.filter(*cloudout[0]);
+	sorfilter.setInputCloud(cloudout);
+	sorfilter.setMeanK(20);
+	sorfilter.setStddevMulThresh(2.0);
+	sorfilter.filter(*cloudout);
 
-	if (cloudout[0]->size() < 4 )
+	if (cloudout->size() < 4 )
 	{
 		return Error_manager(LOCATER_3DCNN_PREDICT_FAILED,NORMAL,"tf pred wheel points size <4");
 	}
 	//kmeans
-	std::vector<cv::Point2f> centers = kmeans(cloudout[0], cluster_file_path);
+    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<cloudout[0]->size();++i)
+	for(int i=0;i<pca_filter_clouds.size();++i)
     {
-	    wheel_points.push_back(cv::Point2f(cloudout[0]->points[i].x,cloudout[0]->points[i].y));
+	    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]);
     }
-	rect = minAreaRect_cloud(centers);
+
+    cv::RotatedRect rect_center = minAreaRect_cloud(centers);
     cv::RotatedRect rect_wheels=minAreaRect_cloud(wheel_points);
-    rect.center=rect_wheels.center;
-	bool IOU = check_box(rect, cloud);
-	if(IOU==false){
+    bool IOU = check_box(rect_center, cloud);
+    if(IOU==false){
         return Error_manager(LOCATER_3DCNN_IIU_FAILED,NORMAL,"IIU check box failed");
-	}
-    LOG(INFO)<<"3dcnn rotate rect center :"<<rect.center<<"   size : "<<rect.size;
+    }
+	//开始赋值
+    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>);

locate/cnn3d_segmentation.h

@@ -25,7 +25,9 @@ public:
 	//cloud:输入点云
 	//rect:输出旋转矩形框
 	//save_dir:中间文件保存路径
-	virtual Error_manager predict(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, cv::RotatedRect& rect, std::string save_dir);
+	virtual Error_manager predict(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud,
+	    float& center_x,float& center_y,
+	    float& wheel_base,float& width,float& angle, std::string save_dir);
 
 protected:
     //根据设置的参数将点云转换成网络输入数格式
@@ -40,10 +42,18 @@ protected:
 	Error_manager isRect(std::vector<cv::Point2f>& points);
 	//kmeans算法聚类,拆分四个轮子,返回四轮中心
 	//cloud:输入点云
-	std::vector<cv::Point2f> kmeans(pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud, std::string cluster_file_path);
+    std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> kmeans(
+        pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud, std::string cluster_file_path);
 	bool check_box(cv::RotatedRect& box, pcl::PointCloud<pcl::PointXYZ>::Ptr cloud);
 	///IIU判断
 	bool check_IOU(cv::RotatedRect& box, pcl::PointCloud<pcl::PointXYZ>::Ptr cloud);
+    /*
+     * 对点云作pca分析,然后在最小轴方向过滤, 防止轮胎左右两边出现噪声
+     * cloud_in:输入点云
+     * cloud_out:输出点云
+     */
+    Error_manager pca_minist_axis_filter(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in,
+                                                             pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_out);
 protected:
 	int m_lenth;
 	int m_width;

locate/locater.cpp

@@ -156,7 +156,8 @@ Error_manager Locater::locate_yolo(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in,
 }
 
 Error_manager Locater::locate_sift(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in, std::vector<YoloBox> boxes,
-                       pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud_targe, std::string work_dir)
+                                   pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud_wheel,pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud_car,
+                                   std::string work_dir)
 {
     if(cloud_in.get()==0)
     {
@@ -198,6 +199,7 @@ Error_manager Locater::locate_sift(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in,
     //限制 x y
     pcl::PassThrough<pcl::PointXYZ> passX;
     pcl::PassThrough<pcl::PointXYZ> passY;
+    pcl::PassThrough<pcl::PointXYZ> passZ;
     passX.setInputCloud(cloud_downsample);
     passX.setFilterFieldName("x");
     passX.setFilterLimits(minp.x, maxp.x);
@@ -208,6 +210,11 @@ Error_manager Locater::locate_sift(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in,
     passY.setFilterLimits(minp.y, maxp.y);
     passY.filter(*cloud_in);
 
+    passY.setInputCloud(cloud_in);
+    passY.setFilterFieldName("z");
+    passY.setFilterLimits(40, 2000);
+    passY.filter(*cloud_in);
+
     //第三步,分割车辆点云
     std::vector<pcl::PointCloud<pcl::PointXYZRGB>::Ptr> segmentation_clouds;
     code=mp_point_sift->seg(cloud_in, segmentation_clouds, work_dir);
@@ -215,13 +222,23 @@ Error_manager Locater::locate_sift(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in,
     {
         return code;
     }
-    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_car(new pcl::PointCloud<pcl::PointXYZ>);
-    pcl::copyPointCloud(*segmentation_clouds[1], *cloud_car);
-    cloud_targe=cloud_car;
+    if(segmentation_clouds[0]->size()==0||segmentation_clouds[2]->size()==0)
+    {
+        return Error_manager(LOCATER_SIFT_PREDICT_NO_CAR_POINT,NORMAL,"sift predict no car or wheel");
+    }
+    pcl::PointCloud<pcl::PointXYZ>::Ptr t_cloud_car(new pcl::PointCloud<pcl::PointXYZ>);
+    pcl::PointCloud<pcl::PointXYZ>::Ptr t_cloud_wheel(new pcl::PointCloud<pcl::PointXYZ>);
+    //第0类即是轮胎点云,第二类为车身点云
+    pcl::copyPointCloud(*segmentation_clouds[0], *t_cloud_wheel);
+    pcl::copyPointCloud(*segmentation_clouds[2], *t_cloud_car);
+    cloud_wheel=t_cloud_wheel;
+    cloud_car=t_cloud_car;
     return SUCCESS;
 }
 
-Error_manager Locater::locate_wheel_rotate_rect(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud,cv::RotatedRect& rotate_rect,
+Error_manager Locater::locate_wheel(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud,
+                                                float& center_x,float& center_y,
+                                                float& wheel_base,float& width,float& angle,
                                     std::string work_dir)
 {
     if(cloud.get()==0)
@@ -238,16 +255,17 @@ Error_manager Locater::locate_wheel_rotate_rect(pcl::PointCloud<pcl::PointXYZ>::
     }
     //识别车轮
     Error_manager code;
-    code=mp_cnn3d->predict(cloud,rotate_rect,work_dir);
+
+    code=mp_cnn3d->predict(cloud,center_x,center_y,wheel_base,width,angle,work_dir);
     if(code!=SUCCESS)
     {
         return code;
     }
     //根据plc限制判断合法性
-    if(rotate_rect.center.y>=930 || rotate_rect.center.y<=200)
+    if(center_y>=930 || center_y<=200)
     {
         char description[255]={0};
-        sprintf(description,"Y is out of range by plc (200  , 900)  Y:%.2f",rotate_rect.center.y);
+        sprintf(description,"Y is out of range by plc (200  , 900)  Y:%.2f",center_y);
         return Error_manager(LOCATER_Y_OUT_RANGE_BY_PLC,NORMAL,description);
     }
     return SUCCESS;
@@ -294,9 +312,10 @@ Error_manager Locater::locate(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in,
         return code;
     }
 
-    //第二步 ,根据yolo框结合PointSift识别车身点云
+    //第二步 ,根据yolo框结合PointSift识别车身点云,第0类即轮胎,第二类为车身
+    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_wheel(new pcl::PointCloud<pcl::PointXYZ>);
     pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_car(new pcl::PointCloud<pcl::PointXYZ>);
-    code=locate_sift(cloud_in,t_yolo_boxes,cloud_car,work_dir);
+    code=locate_sift(cloud_in,t_yolo_boxes,cloud_wheel,cloud_car,work_dir);
     if(code!=SUCCESS)
     {
         return code;
@@ -312,39 +331,26 @@ Error_manager Locater::locate(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in,
     locate_information.locate_hight=height_car;
 
     //第四步,识别轮胎,并根据轮胎数量计算旋转矩形
-    cv::RotatedRect rotate_rect;
-    code=locate_wheel_rotate_rect(cloud_car,rotate_rect,work_dir);
+    float center_x;
+    float center_y;
+    float wheel_base;
+    float width;
+    float angle;
+    code=locate_wheel(cloud_wheel,center_x,center_y,wheel_base,width,angle,work_dir);
     if(code!=SUCCESS)
     {
         return code;
     }
 
-    //第五步,根据rotate_rect计算长边与x轴的角度
-    const int C_PI=3.14159265;
-    cv::Point2f vec;
-    cv::Point2f vertice[4];
-    rotate_rect.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;
-    }
-    float angle_x = 180.0 / C_PI * acos(vec.x / sqrt(vec.x * vec.x + vec.y * vec.y));
-
     //第六步,结果赋值
     //角度以度为单位
-    locate_information.locate_x=rotate_rect.center.x;
-    locate_information.locate_y=rotate_rect.center.y;
-    locate_information.locate_theta = angle_x;
-    locate_information.locate_length=std::max(rotate_rect.size.width,rotate_rect.size.height);
-    locate_information.locate_width=std::min(rotate_rect.size.width,rotate_rect.size.height);
+    locate_information.locate_x=center_x;
+    locate_information.locate_y=center_y;
+    locate_information.locate_theta = angle;
+    locate_information.locate_length=wheel_base;
+    locate_information.locate_width=width;
     locate_information.locate_hight=height_car;
-    locate_information.locate_wheel_base=std::max(rotate_rect.size.width,rotate_rect.size.height);
+    locate_information.locate_wheel_base=wheel_base;
     locate_information.locate_correct=true;
     LOG(INFO) << "Locate SUCCESS :" << " center:" << "[" << locate_information.locate_x
                 << "," << locate_information.locate_y<< "] size= "

locate/locater.h

@@ -45,10 +45,11 @@ protected:
     //cloud_target:车辆点云
     //work_dir:中间文件保存路径
     Error_manager locate_sift(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, std::vector<YoloBox> boxes,
-                           pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud_targe, std::string work_dir);
-    //输入汽车点云,识别轮胎,并找到轮胎的外接旋转矩形;
-    Error_manager locate_wheel_rotate_rect(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud,cv::RotatedRect& rotate_rect,
-        std::string work_dir);
+                           pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud_wheel,pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud_car,
+                           std::string work_dir);
+    //输入汽车点云,识别轮胎,并计算轴长,宽,中心,旋转角
+    Error_manager locate_wheel(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud,float& center_x,float& center_y,
+                                           float& wheel_base,float& width,float& angle,std::string work_dir);
     //根据汽车点云计算车高
     //cloud_car:车辆点云
     Error_manager measure_height(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_car,float& height);

locate/point_sift_segmentation.cpp

@@ -186,9 +186,9 @@ Error_manager Point_sift_segmentation::seg(pcl::PointCloud<pcl::PointXYZ>::Ptr c
 	}
 	free(output);
 	free(data);
-	//确保有车辆点云存在,车辆类别 1
+	//确保有车辆点云及轮胎存在,车辆类别 2,轮胎类别0
 	int segmentation_class_size=cloud_seg.size();
-	if(segmentation_class_size<2)
+	if(segmentation_class_size<3)
     {
 	    return Error_manager(LOCATER_SIFT_PREDICT_NO_CAR_POINT,NORMAL,"PointSift detect no car point");
     }
@@ -269,6 +269,7 @@ bool Point_sift_segmentation::RecoveryCloud(float* output, float* cloud, std::ve
 	return true;
 }
 
+
 bool Point_sift_segmentation::FilterObs(std::vector<pcl::PointCloud<pcl::PointXYZRGB>::Ptr>& cloud_seg,std::string save_dir)
 {
 	/*if (cloud_seg.size() != m_cls_num)

test/locate_sample.cpp

@@ -73,12 +73,12 @@ int main(int argc,char* argv[])
 {
     std::string input_file="20191217-215217.txt";
     std::string out_path="./test";
-    if(argc>2)
+    if(argc>=2)
     {
         input_file=argv[1];
         input_file+=".txt";
     }
-    if(argc>3)
+    if(argc>=3)
     {
         out_path=argv[2];
     }
@@ -96,11 +96,12 @@ int main(int argc,char* argv[])
     }
     Locate_task* task=new Locate_task();
 
-    std::string cloud_path="/home/zx/zzw/code/Detection_src/LidarCppAD/extract/black_car_sample/";
+    std::string cloud_path="/home/zx/data/samples/src_txt/balck_suv/";
     cloud_path+=input_file;
     pcl::PointCloud<pcl::PointXYZ>::Ptr cloud;
 
     cloud=ReadTxtCloud(cloud_path);
+    std::cout<<"  input file: "<<cloud_path<<std::endl;
     code=task->set_locate_cloud(cloud);
     if(code!=SUCCESS)
     {