完成locate基本功能,错误处理

error.h

@@ -7,34 +7,50 @@ enum ERROR_CODE
 
     LASER_INIT_FAILED	=	0x010102FF,
 
-
-
     PLC_INIT_ERROR=0x02010000,
 
-
+    //Locater.cpp error from 0x0301000-0x030100FF
     LOCATER_TASK_INIT_CLOUD_EMPTY=0x03010000,
     LOCATER_TASK_ERROR,
+    LOCATER_TASK_INPUT_CLOUD_UNINIT,
+    LOCATER_INPUT_CLOUD_EMPTY,
+    LOCATER_YOLO_UNINIT,
+    LOCATER_POINTSIFT_UNINIT,
+    LOCATER_3DCNN_UNINIT,
+    LOCATER_INPUT_YOLO_CLOUD_EMPTY,
+    LOCATER_Y_OUT_RANGE_BY_PLC,
+    LOCATER_MEASURE_HEIGHT_CLOUD_UNINIT,
+    LOCATER_MEASURE_HEIGHT_CLOUD_EMPTY,
+    LOCATER_HEIGHT_OUT_RANGE,
+    LOCATER_ANGLE_OUT_RANGE,
+    LOCATER_INPUT_CLOUD_UNINIT,
+
 
     //point sift from 0x03010100-0x030101FF
     LOCATER_SIFT_INIT_FAILED=0x03010100,
+    LOCATER_SIFT_INPUT_CLOUD_UNINIT,
     LOCATER_SIFT_PREDICT_FAILED,
     LOCATER_SIFT_CREATE_INPUT_DATA_FAILED,
     LOCATER_SIFT_FILTE_OBS_FAILED,
+    LOCATER_SIFT_INPUT_BOX_PARAMETER_FAILED,
+    LOCATER_SIFT_INPUT_CLOUD_EMPTY,
+    LOCATER_SIFT_PREDICT_NO_CAR_POINT,
 
     //yolo from 0x03010200-0x030102FF
     LOCATER_YOLO_DETECT_FAILED=0x03010200,
     LOCATER_YOLO_DETECT_NO_TARGET,
+    LOCATER_YOLO_PARAMETER_INVALID,
+    LOCATER_YOLO_INPUT_CLOUD_UNINIT,
 
     //3dcnn from 0x03010300-0x030103FF
     LOCATER_3DCNN_INIT_FAILED=0x03010300,
+    LOCATER_3DCNN_INPUT_CLOUD_UNINIT,
     LOCATER_3DCNN_PREDICT_FAILED,
     LOCATER_3DCNN_VERIFY_RECT_FAILED_3,
     LOCATER_3DCNN_VERIFY_RECT_FAILED_4,
     LOCATER_3DCNN_KMEANS_FAILED,
-    LOCATER_3DCNN_IIU_FAILED
-
-
-
+    LOCATER_3DCNN_IIU_FAILED,
+    LOCATER_3DCNN_INPUT_CLOUD_EMPTY
 };
 
 #endif

locate/YoloDetector.cpp

@@ -9,8 +9,12 @@ void YoloDetector::free_img(image_t m)
     }
 }
 
-ERROR_CODE YoloDetector::SetRegion(float minx, float maxx, float miny, float maxy,float freq)
+ERROR_CODE YoloDetector::set_region(float minx, float maxx, float miny, float maxy,float freq)
 {
+    if(m_maxx<=m_minx || m_maxy<=m_miny || m_freq<=0)
+    {
+        return LOCATER_YOLO_PARAMETER_INVALID;
+    }
     m_minx = minx;
     m_maxx = maxx;
     m_miny = miny;
@@ -98,9 +102,9 @@ ERROR_CODE YoloDetector::detect(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in, st
             box_y.w = (float(box.w)/float(w)*(m_maxx - m_minx));
             box_y.h = (float(box.h)/float(h)*(m_maxy - m_miny));
 
-			LOG(WARNING) << "\tyolo minx:" << m_minx << ",maxx:" << m_maxx << ",miny:" << m_miny << ",maxy:" << m_maxy;
-			LOG(WARNING) << "box " << i << " x:" << box.x << " ,y:" << box.y << " ,w:" << box.w << " ,h:" << box.h;
-			LOG(WARNING) << "box_t " << i << " x:" << box_y.x << " ,y:" << box_y.y << " ,w:" << box_y.w << " ,h:" << box_y.h;
+			//LOG(WARNING) << "\tyolo minx:" << m_minx << ",maxx:" << m_maxx << ",miny:" << m_miny << ",maxy:" << m_maxy;
+			//LOG(WARNING) << "box " << i << " x:" << box.x << " ,y:" << box.y << " ,w:" << box.w << " ,h:" << box.h;
+			LOG(INFO) << "yolo box " << i << " x:" << box_y.x << " ,y:" << box_y.y << " ,w:" << box_y.w << " ,h:" << box_y.h;
 
 			boxes.push_back(box_y);
 			//����boxes

locate/YoloDetector.h

@@ -15,17 +15,28 @@ typedef struct YOLO_BOX
     float x, y, w, h;
 }YoloBox;
 
+/*
+ * yolo v2网络识别类, 输入图像大小为416*416.
+ */
 class YoloDetector
 {
 public:
 	YoloDetector(std::string cfg,std::string weights,float minx,float maxx,float miny,float maxy,float freq);
-    ERROR_CODE SetRegion(float minx, float maxx, float miny, float maxy,float freq);
+	//设置输入点云的边界,识别的时候根据边界投影到图像
+	//输入边界的长款比必须是1:1,否则会出现图像扭曲
+	//freq:表示多长一个像素点
+    ERROR_CODE set_region(float minx, float maxx, float miny, float maxy,float freq);
 	virtual ~YoloDetector();
-
+    //识别函数, 将点云数据转换成图像格式保存,该函数已经过期(第一版算法,点云由两个雷达扫描)
+    //l:左边雷达扫描数据; r:右边雷达扫描数据
+    //save_dir:中间数据保存路径
 	ERROR_CODE detect(cv::Mat& l, cv::Mat& r, std::vector<YoloBox>& boxes,std::string save_dir);
+	//将点云投影成图像, 识别车辆外接矩形框
 	ERROR_CODE detect(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in, std::vector<YoloBox>& boxes, std::string save_dir);
 protected:
+    //释放图像内存
     void free_img(image_t img);
+    //opencv图像类型转换成 image_t 格式,并调整大小
     std::shared_ptr<image_t> mat_to_image_resize(cv::Mat mat) const
     {
         if (mat.data == NULL) return std::shared_ptr<image_t>(NULL);

locate/cnn3d_segmentation.cpp

@@ -150,7 +150,7 @@ cv::RotatedRect minAreaRect_cloud(std::vector<cv::Point2f> centers)
 	return rect;
 }
 
-ERROR_CODE Cnn3d_segmentation::SetParam(int l, int w, int h,int freq,int classes)
+ERROR_CODE Cnn3d_segmentation::set_parameter(int l, int w, int h,int freq,int classes)
 {
     m_lenth = l;
     m_width = w;
@@ -173,7 +173,7 @@ Cnn3d_segmentation::~Cnn3d_segmentation()
 	tf_wheel_3dcnn_close();
 }
 
-ERROR_CODE Cnn3d_segmentation::Init(std::string weights)
+ERROR_CODE Cnn3d_segmentation::init(std::string weights)
 {
 
 	if (0 != tf_wheel_3dcnn_init(weights, m_lenth,m_width,m_height))
@@ -197,9 +197,8 @@ ERROR_CODE Cnn3d_segmentation::Init(std::string weights)
 	return SUCCESS;
 }
 
-std::vector<cv::Point2f> Cnn3d_segmentation::kmeans(pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud, std::string file_name)
+std::vector<cv::Point2f> Cnn3d_segmentation::kmeans(pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud, std::string file_path)
 {
-	////���4��
 	std::vector<cv::Point2f> cv_points;
 	for (int i = 0; i < cloud->size(); ++i)
 	{
@@ -268,7 +267,8 @@ std::vector<cv::Point2f> Cnn3d_segmentation::kmeans(pcl::PointCloud<pcl::PointXY
 		rets.push_back(cv::Point2f(avg_x, avg_y));
 
 	}
-	save_rgb_cloud_txt(file_name, cloud);
+	std::string save_file=file_path+"/cnn3d.txt";
+	save_rgb_cloud_txt(save_file, cloud);
 	return rets;
 
 
@@ -355,8 +355,7 @@ ERROR_CODE Cnn3d_segmentation::isRect(std::vector<cv::Point2f>& points)
         }
         double cosa = (l1 * l1 + l2 * l2 - max_l * max_l) / (2.0 * l1 * l2);
         if (fabs(cosa) >= 0.13) {
-            LOG(INFO) << " angle cos >0.13 =" << cosa << "  i=" << max_index;
-            LOG(INFO) << "L1:" << l1 << "  L2:" << l2 << "  L3:" << max_l;
+            LOG(ERROR) << " angle cos >0.13 =" << cosa << "  i=" << max_index;
             return LOCATER_3DCNN_VERIFY_RECT_FAILED_4;
         }
 
@@ -405,8 +404,7 @@ ERROR_CODE Cnn3d_segmentation::isRect(std::vector<cv::Point2f>& points)
         }
         double cosa = (l1 * l1 + l2 * l2 - max_l * max_l) / (2.0 * l1 * l2);
         if (fabs(cosa) >= 0.12) {
-            LOG(INFO) << "3 wheels angle cos >0.12 =" << cosa << "  i=" << max_index;
-            LOG(INFO) << "L1:" << l1 << "  L2:" << l2 << "  L3:" << max_l;
+            LOG(ERROR) << "3 wheels angle cos >0.12 =" << cosa << "  i=" << max_index;
             return LOCATER_3DCNN_VERIFY_RECT_FAILED_3;
         }
 
@@ -449,9 +447,15 @@ void save_cloudT(std::string txt, pcl::PointCloud<pcl::PointXYZ>& pCloud)
     os.close();
 }
 
-ERROR_CODE Cnn3d_segmentation::Predict(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, cv::RotatedRect& rect, std::string cluster_file_path)
+ERROR_CODE Cnn3d_segmentation::predict(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, cv::RotatedRect& rect,
+    std::string cluster_file_path)
 {
-	clock_t clock0 = clock();
+    if(cloud->size()==0)
+    {
+        LOG(ERROR)<<" 3dcnn predict input cloud empty";
+        return LOCATER_3DCNN_INPUT_CLOUD_EMPTY;
+    }
+
 	pcl::PointXYZ minp, maxp;
 	pcl::getMinMax3D(*cloud, minp, maxp);
 	float center_x = (minp.x + maxp.x) / 2.0;
@@ -473,7 +477,7 @@ ERROR_CODE Cnn3d_segmentation::Predict(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud
 		free(pout);
 		return LOCATER_3DCNN_PREDICT_FAILED;
 	}
-	clock_t clock2 = clock();
+
 	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);
@@ -487,15 +491,15 @@ ERROR_CODE Cnn3d_segmentation::Predict(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud
 
 	if (cloudout.size() == 0)
 	{
-		LOG(INFO) << "tf pred classes ==0";
+		LOG(ERROR) << "tf pred classes ==0";
 		return LOCATER_3DCNN_PREDICT_FAILED;
 	}
 	if (cloudout[0]->size() == 0)
 	{
-		LOG(INFO) << "tf pred class[wheel].points size ==0";
+		LOG(ERROR) << "tf pred class[wheel].points size ==0";
 		return LOCATER_3DCNN_PREDICT_FAILED;
 	}
-	//������Ⱥ��
+    //离群点过滤
 	pcl::StatisticalOutlierRemoval<pcl::PointXYZRGB> sorfilter(true); // Initializing with true will allow us to extract the removed indices
 	sorfilter.setInputCloud(cloudout[0]);
 	sorfilter.setMeanK(15);
@@ -504,11 +508,10 @@ ERROR_CODE Cnn3d_segmentation::Predict(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud
 
 	if (cloudout[0]->size() < 4 )
 	{
-		LOG(INFO) <<  "tf pred wheel points size <4";
+		LOG(ERROR) <<  "tf pred wheel points size <4";
 		return LOCATER_3DCNN_PREDICT_FAILED;
 	}
 	//kmeans
-
 	std::vector<cv::Point2f> centers = kmeans(cloudout[0], cluster_file_path);
 	std::vector<cv::Point2f> wheel_points;
 	for(int i=0;i<cloudout[0]->size();++i)
@@ -517,13 +520,13 @@ ERROR_CODE Cnn3d_segmentation::Predict(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud
     }
 	if (centers.size() != 4 && centers.size()!=3)
 	{
-		LOG(INFO) << "\t 3dcnn cluster center size != 4 & 3";
+		LOG(ERROR) << "\t 3dcnn cluster center size != 4 & 3";
 		return LOCATER_3DCNN_KMEANS_FAILED;
 	}
 	ERROR_CODE code=isRect(centers);
 	if(code!=SUCCESS)
 	{
-		LOG(INFO) << "\t 3dcnn rectangle verify failed   ";
+		LOG(ERROR) << "\t 3dcnn rectangle verify failed   ";
 		return code;
 	}
     if(centers.size()==3)
@@ -534,10 +537,10 @@ ERROR_CODE Cnn3d_segmentation::Predict(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud
     cv::RotatedRect rect_wheels=minAreaRect_cloud(wheel_points);
     rect.center=rect_wheels.center;
 	bool IOU = check_box(rect, cloud);
-	if(IOU==false) return LOCATER_3DCNN_IIU_FAILED;
-
-	clock_t clock3 = clock();
-	//LOG(INFO) << "   ALL TIME:" << clock3 - clock0 << " \t tf_TIME:" << clock2 - clock1 << "\t";
+	if(IOU==false){
+        return LOCATER_3DCNN_IIU_FAILED;
+	}
+    LOG(INFO)<<"3dcnn rotate rect center :"<<rect.center<<"   size : "<<rect.size;
 
 	return SUCCESS;
 }
@@ -585,12 +588,12 @@ bool Cnn3d_segmentation::check_box(cv::RotatedRect& box, pcl::PointCloud<pcl::Po
 	//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)
 	{
-		LOG(INFO) << " \t IOU failed";
+		LOG(ERROR) << " \t IOU failed";
 		return false;
 	}
 	if (std::max(box.size.width, box.size.height) < 1900 || std::max(box.size.width, box.size.height) > 3600)
 	{
-		LOG(INFO) << "\tlen not in 1900-3400";
+		LOG(ERROR) << "\tlen not in 1900-3400";
 		return false;
 	}
 	return true;

locate/cnn3d_segmentation.h

@@ -7,26 +7,31 @@
 #include <string>
 #include "../error.h"
 
+/*
+ * 3dcnn网络识别车两中轮胎点
+ */
 class  Cnn3d_segmentation
 {
 public:
 	Cnn3d_segmentation(int l,int w,int h,int freq,int nClass);
 	//设置3dcnn网络参数
-    ERROR_CODE SetParam(int l, int w, int h,int freq,int classes);
+    ERROR_CODE set_parameter(int l, int w, int h,int freq,int classes);
 	virtual ~Cnn3d_segmentation();
 	//初始化网络参数
 	//weights:参数文件
-	virtual ERROR_CODE Init(std::string weights);
+	virtual ERROR_CODE init(std::string weights);
 
 	//预测
 	//cloud:输入点云
 	//rect:输出旋转矩形框
 	//save_dir:中间文件保存路径
-	virtual ERROR_CODE Predict(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, cv::RotatedRect& rect, std::string save_dir);
+	virtual ERROR_CODE predict(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, cv::RotatedRect& rect, std::string save_dir);
 
 protected:
+    //根据设置的参数将点云转换成网络输入数格式
 	float* 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);
+    //将识别结果转换成点云
 	std::vector<pcl::PointCloud<pcl::PointXYZRGB>::Ptr> 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);
 protected:

locate/locate_sample.cpp

@@ -1,10 +1,13 @@
 //
 // Created by zx on 2019/12/30.
 //
+/*
+ * locater 模块测试例子
+ */
 #include <fcntl.h>
 #include "locate_task.h"
 #include "locater.h"
-
+#include <glog/logging.h>
 #include <google/protobuf/io/coded_stream.h>
 #include <google/protobuf/io/zero_copy_stream_impl.h>
 #include <google/protobuf/text_format.h>
@@ -26,10 +29,60 @@ bool read_proto_param(std::string path, ::google::protobuf::Message& param)
     close(fd);
     return success;
 }
+bool string2point(std::string str,pcl::PointXYZ& point)
+{
+    std::istringstream iss;
+    iss.str(str);
+    float value;
+    float data[3]={0};
+    for(int i=0;i<3;++i)
+    {
+        if(iss>>value)
+        {
+            data[i]=value;
+        }
+        else
+        {
+            return false;
+        }
+    }
+    point.x=data[0];
+    point.y=data[1];
+    point.z=data[2];
+    return true;
+}
 
+pcl::PointCloud<pcl::PointXYZ>::Ptr ReadTxtCloud(std::string file)
+{
+    std::ifstream fin(file.c_str());
+    const int line_length=64;
+    char str[line_length]={0};
+    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
+    while(fin.getline(str,line_length))
+    {
+        pcl::PointXYZ point;
+        if(string2point(str,point))
+        {
+            cloud->push_back(point);
+        }
+    }
+    return cloud;
+}
 
 int main(int argc,char* argv[])
 {
+    std::string input_file="20191217-215217.txt";
+    std::string out_path="./test";
+    if(argc>2)
+    {
+        input_file=argv[1];
+        input_file+=".txt";
+    }
+    if(argc>3)
+    {
+        out_path=argv[2];
+    }
+    ERROR_CODE  code;
     Locater* locater=new Locater();
     LidarMeasure::LocateParameter parameter;
     if(false==read_proto_param("./setting/locate.prototxt",parameter))
@@ -38,6 +91,21 @@ int main(int argc,char* argv[])
     }
     locater->init(parameter);
     Locate_task* task=new Locate_task();
-    locater->execute_task(task,5);
+
+    std::string cloud_path="/home/zx/zzw/code/Detection_src/LidarCppAD/extract/black_car_sample/";
+    cloud_path+=input_file;
+    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud;
+
+    cloud=ReadTxtCloud(cloud_path);
+    code=task->set_locate_cloud(cloud);
+    if(SUCCESS!=code)
+    {
+        return 0;
+    }
+    task->set_save_path(out_path);
+    if(SUCCESS==locater->execute_task(task,5))
+    {
+        LOG(INFO)<<" LOCATE SUCCESS";
+    }
     return 0;
 }

locate/locate_task.cpp

@@ -4,7 +4,7 @@
 
 #include "locate_task.h"
 #include "../error.h"
-
+#include <glog/logging.h>
 Locate_task::Locate_task()
 {
     m_task_type=LOCATE_TASK;
@@ -16,6 +16,11 @@ Locate_task::~Locate_task()
 
 ERROR_CODE Locate_task::set_locate_cloud(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud)
 {
+    if(cloud.get()==0)
+    {
+        LOG(ERROR)<<"locate task set input cloud uninit";
+        return LOCATER_TASK_INPUT_CLOUD_UNINIT;
+    }
     if(cloud->size()==0) {
         return LOCATER_TASK_INIT_CLOUD_EMPTY;
     }
@@ -33,8 +38,19 @@ Locate_information Locate_task::get_locate_information()
     return m_locate_information;
 }
 
-int Locate_task::set_locate_information(Locate_information information)
+ERROR_CODE Locate_task::set_locate_information(Locate_information information)
 {
     memcpy(&m_locate_information,&information,sizeof(Locate_information));
     return SUCCESS;
+}
+
+std::string Locate_task::get_save_path()
+{
+    return m_save_path;
+}
+
+ERROR_CODE Locate_task::set_save_path(std::string path)
+{
+    m_save_path=path;
+    return SUCCESS;
 }

locate/locate_task.h

@@ -39,12 +39,18 @@ public:
     //获取测量结果
     Locate_information get_locate_information();
     //设置测量结果
-    int set_locate_information(Locate_information information);
+    ERROR_CODE set_locate_information(Locate_information information);
+    //获取文件存放路径
+    std::string get_save_path();
+    //设置文件村昂路径
+    ERROR_CODE set_save_path(std::string path);
 protected:
     //测量算法输入点云
     pcl::PointCloud<pcl::PointXYZ>::Ptr         m_locate_cloud_ptr;
     //测量结果
     Locate_information                          m_locate_information;
+    //测量中间文件存放路径
+    std::string                                 m_save_path;
 };
 
 

locate/locater.cpp

@@ -3,8 +3,10 @@
 //
 
 #include "locater.h"
+#include <pcl/filters//voxel_grid.h>
+#include <pcl/filters/passthrough.h>
 #include <glog/logging.h>
-Locater::Locater()
+Locater::Locater():mp_yolo_detector(0),mp_point_sift(0),mp_cnn3d(0)
 {
 }
 Locater::~Locater()
@@ -29,10 +31,10 @@ ERROR_CODE Locater::init(LidarMeasure::LocateParameter parameter)
         std::string	graph_file =  graph;
         std::string	cpkt_file =  cpkt;
         LOG(INFO)<<"init  pointSift net ...";
-        code=mp_point_sift->Init(graph_file,cpkt_file);
+        code=mp_point_sift->init(graph_file,cpkt_file);
         if(code!=SUCCESS)
         {
-            LOG(ERROR)<<"Init pointSIFT net Failed ... exit";
+            LOG(ERROR)<<"Init pointSIFT net Failed ... "<<code;
             return code;
         }
     }
@@ -56,35 +58,313 @@ ERROR_CODE Locater::init(LidarMeasure::LocateParameter parameter)
     int nClass = parameter.net_3dcnn_parameter().nclass();
     int freq = parameter.net_3dcnn_parameter().freq();
 
-    LOG(INFO)<<"Creating 3dcnn ...";
     mp_cnn3d = new Cnn3d_segmentation(l, w, h, freq, nClass);
 
     LOG(INFO)<<"Init 3dcnn ...";
     std::string	weights = parameter.net_3dcnn_parameter().weights_file();
-    code=mp_cnn3d->Init(weights);
+    code=mp_cnn3d->init(weights);
     if(SUCCESS!=code)
     {
-        LOG(ERROR)<<"Init 3dcnn failed exit : ";
+        LOG(ERROR)<<"Init 3dcnn failed  : "<<code;
         return code;
     }
     return SUCCESS;
 
 }
 
-ERROR_CODE Locater::execute_task(Locate_task* task,double time_out)
+ERROR_CODE Locater::execute_task(Task_Base* task,double time_out)
 {
+    LOG(INFO)<<"NEW LOCATE TASK ==============================================================================";
+    Locate_information locate_information;
+    locate_information.locate_correct=false;
+
     //判断task是否合法
     if(task==0) return LOCATER_TASK_ERROR;
     if(task->get_task_type()!=LOCATE_TASK)
     {
+        LOG(ERROR)<<"locate Task type error";
+        task->update_statu(TASK_OVER,"locate task type error");
         return  LOCATER_TASK_ERROR;
     }
+    Locate_task* locate_task=(Locate_task*)task;
+    ERROR_CODE code;
+    ///第一步,获取task中input点云,文件保存路径
+    pcl::PointCloud<pcl::PointXYZ>::Ptr t_cloud_input=locate_task->get_locate_cloud();
+    std::string save_path=locate_task->get_save_path();
+    if(t_cloud_input.get()==0)
+    {
+        LOG(ERROR)<<"Task set cloud is uninit";
+        locate_task->update_statu(TASK_OVER,"Task set cloud is uninit");
+        return LOCATER_INPUT_CLOUD_UNINIT;
+    }
+    //第二步,调用locate
+    code=locate(t_cloud_input,locate_information,save_path);
+    if(SUCCESS!=code)
+    {
+        locate_task->update_statu(TASK_OVER,"Locate Failed");
+        return code;
+    }
+    locate_task->set_locate_information(locate_information);
+    locate_task->update_statu(TASK_OVER,"Locate OK");
+    return code;
+
+}
+
+ERROR_CODE Locater::locate_yolo(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in,
+                       std::vector<YoloBox>& boxes, std::string work_dir)
+{
+    if(cloud_in.get()==0)
+    {
+        LOG(ERROR)<<"yolo input cloud uninit";
+        return LOCATER_YOLO_INPUT_CLOUD_UNINIT;
+    }
+    if(cloud_in->size()==0)
+    {
+        LOG(ERROR)<<"locate_yolo input cloud empty";
+        return LOCATER_INPUT_YOLO_CLOUD_EMPTY;
+    }
+    if(mp_yolo_detector==0)
+    {
+        LOG(ERROR)<<"locate_yolo yolo is not init";
+        return LOCATER_YOLO_UNINIT;
+    }
+    //1,根据点云区域,调整yolo边界参数,边界参数必须保证长宽 1:1
+    pcl::PointXYZ min_point,max_point;
+    pcl::getMinMax3D(*cloud_in,min_point,max_point);
+    float yolo_minx=min_point.x;
+    float yolo_maxx=max_point.x;
+    float yolo_miny=min_point.y;
+    float yolo_maxy=max_point.y;
+    float length=yolo_maxx-yolo_minx;
+    float width=yolo_maxy-yolo_miny;
+    if(length>2*width) yolo_maxy=(length/2.0)+yolo_miny;
+    else if(length<2*width) yolo_maxx=(2*width)+yolo_minx;
+
+    LOG(INFO)<<("set yolo parameter l:%.3f  r:%.3f     t:%.3f  b:%.3f\n",
+           yolo_minx,yolo_maxx,yolo_miny,yolo_maxy);
+    mp_yolo_detector->set_region(yolo_minx,yolo_maxx,yolo_miny,yolo_maxy,m_locate_parameter.yolo_parameter().freq());
+    //2,识别车辆位置
+    boxes.clear();
+    ERROR_CODE code=mp_yolo_detector->detect(cloud_in,boxes,work_dir);
+    if(code!=SUCCESS)
+    {
+        LOG(ERROR)<<"Yolo detect failed : "<<code;
+    }
+    return code;
+}
+
+ERROR_CODE 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)
+{
+    if(cloud_in.get()==0)
+    {
+        LOG(ERROR)<<"sift input cloud uninit";
+        return LOCATER_SIFT_INPUT_CLOUD_UNINIT;
+    }
+    if(cloud_in->size()==0)
+    {
+        LOG(ERROR)<<"locate_sift input cloud empty";
+        return LOCATER_SIFT_INPUT_CLOUD_EMPTY;
+    }
+    if(mp_point_sift==0)
+    {
+        LOG(ERROR)<<"Point Sift unInit ";
+        return LOCATER_POINTSIFT_UNINIT;
+    }
+    //第一步,根据点云边界调整PointSIft 参数
+    pcl::PointXYZ min_point,max_point;
+    pcl::getMinMax3D(*cloud_in,min_point,max_point);
+    ERROR_CODE code ;
+    code=mp_point_sift->set_region(min_point,max_point);
+    if(code!=SUCCESS)
+    {
+        return code;
+    }
+    //第二步,点云抽稀,抽稀后的点云大约在8192左右,
+    // 如果有多辆车(多个yolo框),取第一个框
+    YoloBox yolo_box=boxes[0];
+    pcl::PointXYZ minp, maxp;
+    minp.x = yolo_box.x;
+    minp.y = yolo_box.y;
+    maxp.x = minp.x + yolo_box.w;
+    maxp.y = minp.y + yolo_box.h;
+
+    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_input_sift(new pcl::PointCloud<pcl::PointXYZ>());
+    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_downsample(new pcl::PointCloud<pcl::PointXYZ>());
+    ///	///体素网格 下采样
+    pcl::VoxelGrid<pcl::PointXYZ> vgfilter;
+    vgfilter.setInputCloud(cloud_in);
+    vgfilter.setLeafSize(60.f, 60.f, 50.f);
+    vgfilter.filter(*cloud_downsample);
+    //限制 x y
+    pcl::PassThrough<pcl::PointXYZ> passX;
+    pcl::PassThrough<pcl::PointXYZ> passY;
+    passX.setInputCloud(cloud_downsample);
+    passX.setFilterFieldName("x");
+    passX.setFilterLimits(minp.x, maxp.x);
+    passX.filter(*cloud_in);
+
+    passY.setInputCloud(cloud_in);
+    passY.setFilterFieldName("y");
+    passY.setFilterLimits(minp.y, maxp.y);
+    passY.filter(*cloud_in);
+
+    //第三步,分割车辆点云
+    std::vector<pcl::PointCloud<pcl::PointXYZRGB>::Ptr> segmentation_clouds;
+    code=mp_point_sift->seg(cloud_in, segmentation_clouds, work_dir);
+    if(code!=SUCCESS)
+    {
+        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;
+    return SUCCESS;
+}
+
+ERROR_CODE Locater::locate_wheel_rotate_rect(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud,cv::RotatedRect& rotate_rect,
+                                    std::string work_dir)
+{
+    if(cloud.get()==0)
+    {
+        LOG(ERROR)<<"3dcnn input cloud uninit";
+        return LOCATER_3DCNN_INPUT_CLOUD_UNINIT;
+    }
+    if(cloud->size()==0)
+    {
+        LOG(ERROR)<<"locate_3dcnn input cloud empty";
+        return LOCATER_3DCNN_INPUT_CLOUD_EMPTY;
+    }
+    if(mp_cnn3d==0)
+    {
+        LOG(ERROR)<<"locate_wheel 3dcnn is not init";
+        return LOCATER_3DCNN_UNINIT;
+    }
+    //识别车轮
+    ERROR_CODE code;
+    code=mp_cnn3d->predict(cloud,rotate_rect,work_dir);
+    if(code!=SUCCESS)
+    {
+        return code;
+    }
+    //根据plc限制判断合法性
+    if(rotate_rect.center.y>=930 || rotate_rect.center.y<=200)
+    {
+        LOG(ERROR)<<" Y is out of range by plc (200  , 900)  Y:"<<rotate_rect.center.x;
+        return LOCATER_Y_OUT_RANGE_BY_PLC;
+    }
+    return SUCCESS;
+
 
-    ///获取task中input点云
-    pcl::PointCloud<pcl::PointXYZ>::Ptr t_cloud_input=task->get_locate_cloud();
-    std::cout<<" execute locate task";
-    usleep(1000*1000);
+}
 
+ERROR_CODE Locater::measure_height(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_car,float& height)
+{
+    if(cloud_car.get()==0)
+    {
+        LOG(ERROR)<<"measure height input cloud uninit";
+        return LOCATER_MEASURE_HEIGHT_CLOUD_UNINIT;
+    }
+    if(cloud_car->size()==0)
+    {
+        return LOCATER_MEASURE_HEIGHT_CLOUD_EMPTY;
+    }
+    pcl::PointXYZ min_point,max_point;
+    pcl::getMinMax3D(*cloud_car,min_point,max_point);
+    //限制车高的范围,检验结果
+    height=max_point.z;
+    if(height<1000||height>2000)
+    {
+        return LOCATER_HEIGHT_OUT_RANGE;
+    }
     return SUCCESS;
+}
 
+ERROR_CODE Locater::locate(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in,
+                  Locate_information& locate_information, std::string work_dir)
+{
+    if(cloud_in.get()==0)
+    {
+        LOG(ERROR)<<"locate input cloud is not init";
+        return LOCATER_INPUT_CLOUD_UNINIT;
+    }
+    if(cloud_in->size()==0) {
+        LOG(ERROR)<<"input cloud is empty";
+        return LOCATER_INPUT_CLOUD_EMPTY;
+    }
+    //置初始结果为错误
+    locate_information.locate_correct=false;
+    ERROR_CODE code;
+    std::lock_guard<std::mutex> t_lock(m_mutex_lock);
+    //第一步 yolo定位车辆外接矩形
+    std::vector<YoloBox> t_yolo_boxes;
+    code=locate_yolo(cloud_in,t_yolo_boxes,work_dir);
+    if(SUCCESS!=code)
+    {
+        return code;
+    }
+
+    //第二步 ,根据yolo框结合PointSift识别车身点云
+    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_car(new pcl::PointCloud<pcl::PointXYZ>);
+    code=locate_sift(cloud_in,t_yolo_boxes,cloud_car,work_dir);
+    if(code!=SUCCESS)
+    {
+        return code;
+    }
+
+    //第三步,计算车辆高度
+    float height_car=0;
+    code=measure_height(cloud_car,height_car);
+    if(SUCCESS!=code)
+    {
+        return code;
+    }
+
+    //第四步,识别轮胎,并根据轮胎数量计算旋转矩形
+    cv::RotatedRect rotate_rect;
+    code=locate_wheel_rotate_rect(cloud_car,rotate_rect,work_dir);
+    if(SUCCESS!=code)
+    {
+        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));
+    //角度检验, 根据plc限制,角度应该在(80-100,260-280)
+    if( !((angle_x>80&&angle_x<100) || (angle_x>260&&angle_x<280)) )
+    {
+        return LOCATER_ANGLE_OUT_RANGE;
+    }
+
+    //第六步,结果赋值
+    //角度以度为单位
+    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_hight=height_car;
+    locate_information.locate_wheel_base=std::max(rotate_rect.size.width,rotate_rect.size.height);
+    locate_information.locate_correct=true;
+    LOG(INFO) << "Locate SUCCESS :" << " center:" << "[" << locate_information.locate_x
+                << "," << locate_information.locate_y<< "] size= "
+              << locate_information.locate_length << ", "
+              << locate_information.locate_width << "," << locate_information.locate_hight
+              << "  angle=" << locate_information.locate_theta ;
+
+    return SUCCESS;
 }

locate/locater.h

@@ -10,6 +10,9 @@
 #include "point_sift_segmentation.h"
 #include "YoloDetector.h"
 #include "cnn3d_segmentation.h"
+#include <mutex>
+
+
 class Locater
 {
 public:
@@ -21,11 +24,40 @@ public:
     //执行任务
     //task:测量任务
     //time_out:超时时间,单位秒
-    ERROR_CODE execute_task(Locate_task* task,double time_out=5);
+    ERROR_CODE execute_task(Task_Base* task,double time_out=5);
+protected:
+    //测量算法函数
+    //cloud_in:输入点云
+    //locate_information:测量结果
+    //work_dir:中间文件保存路径
+    ERROR_CODE locate(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in,
+        Locate_information& locate_information, std::string work_dir);
+    //yolo定位车辆外接矩形
+    //cloud_in:输入点云,定位算法内部不会修改点云内容
+    //boxes:定位到的外接box框
+    //work_dir:中间文件输出路径
+    ERROR_CODE locate_yolo(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in,
+        std::vector<YoloBox>& boxes, std::string work_dir);
+    //根据yolo识别结果, 利用PointSift从场景中分割车辆点云
+    //cloud:输入点云
+    //boxes:yolo识别结果
+    //cloud_target:车辆点云
+    //work_dir:中间文件保存路径
+    ERROR_CODE locate_sift(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, std::vector<YoloBox> boxes,
+                           pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud_targe, std::string work_dir);
+    //输入汽车点云,识别轮胎,并找到轮胎的外接旋转矩形;
+    ERROR_CODE locate_wheel_rotate_rect(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud,cv::RotatedRect& rotate_rect,
+        std::string work_dir);
+    //根据汽车点云计算车高
+    //cloud_car:车辆点云
+    ERROR_CODE measure_height(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_car,float& height);
 private:
+    LidarMeasure::LocateParameter           m_locate_parameter;
     Point_sift_segmentation*                mp_point_sift;
     YoloDetector*                           mp_yolo_detector;
     Cnn3d_segmentation*                     mp_cnn3d;
+
+    std::mutex						        m_mutex_lock;
 };
 
 

locate/pointSIFT_API.h

@@ -17,13 +17,28 @@
 #endif
 #endif
 
+/*
+ * PointSift 点云分割类
+ * 加载PointSift网络,分割点云
+ */
+
 class LIB_SIFT_API PointSifter
 {
 public:
+    //初始化PointSift
+    //point_num:pointsift输入点数
+    //cls_num:PointSift输出类别数
 	PointSifter(int point_num, int cls_num);
 	~PointSifter();
+	//加载网络参数
+	//meta:tensorflow网络结构定义文件
+	//cpkt:tensorflow网络权重
 	bool Load(std::string meta, std::string cpkt);
+	//预测
+	//data:输入数据,大小为  输入点数*3
+	//output:输出数据,大小为  输入点数*类别数
 	bool Predict(float* data, float* output);
+	//错误原因
 	std::string LastError();
 private:
 	PointSifter();

locate/point_sift_segmentation.cpp

@@ -27,17 +27,24 @@ Point_sift_segmentation::Point_sift_segmentation(int point_size, int cls, float
 	m_maxp = maxp;
 }
 
-ERROR_CODE Point_sift_segmentation::ResetRegion(pcl::PointXYZ minp, pcl::PointXYZ maxp)
+ERROR_CODE Point_sift_segmentation::set_region(pcl::PointXYZ minp, pcl::PointXYZ maxp)
 {
     m_minp = minp;
     m_maxp = maxp;
+    if(m_maxp.x<=m_minp.x || m_maxp.y<=m_minp.y)
+    {
+        LOG(ERROR)<<"Point sift set region invalid :m_maxp.x<=m_minp.x || m_maxp.y<=m_minp.y  "
+                    <<LOCATER_SIFT_INPUT_BOX_PARAMETER_FAILED;
+        return LOCATER_SIFT_INPUT_BOX_PARAMETER_FAILED;
+    }
+    return SUCCESS;
 }
 
 Point_sift_segmentation::~Point_sift_segmentation()
 {
 }
 
-ERROR_CODE Point_sift_segmentation::Init(std::string graph, std::string cpkt)
+ERROR_CODE Point_sift_segmentation::init(std::string graph, std::string cpkt)
 {
 	if (!Load(graph, cpkt))
 	{
@@ -142,13 +149,18 @@ bool Point_sift_segmentation::Create_data(pcl::PointCloud<pcl::PointXYZ>::Ptr cl
 	return true;
 }
 
-ERROR_CODE Point_sift_segmentation::Seg(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, std::vector<pcl::PointCloud<pcl::PointXYZRGB>::Ptr>& cloud_seg, std::string save_dir)
+ERROR_CODE Point_sift_segmentation::seg(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud,
+    std::vector<pcl::PointCloud<pcl::PointXYZRGB>::Ptr>& cloud_seg, std::string save_dir)
 {
+    if(cloud->size()==0)
+    {
+        LOG(ERROR)<<"PointSift input cloud empty : "<<LOCATER_SIFT_INPUT_CLOUD_EMPTY;
+        return LOCATER_SIFT_INPUT_CLOUD_EMPTY;
+    }
 	float* data = (float*)malloc(m_point_num * 3 * sizeof(float));
 	float* output = (float*)malloc(m_point_num*m_cls_num*sizeof(float));
 	memset(data, 0, m_point_num * 3 * sizeof(float));
 	memset(output, 0, m_point_num*m_cls_num * sizeof(float));
-	///׼������
 	LOG(INFO) << "cloud size:" << cloud->size()<<"  /  "<<m_point_num;
 	if (!Create_data(cloud, data))
 	{
@@ -175,9 +187,16 @@ ERROR_CODE Point_sift_segmentation::Seg(pcl::PointCloud<pcl::PointXYZ>::Ptr clou
 	}
 	free(output);
 	free(data);
-	//����������
+	//确保有车辆点云存在,车辆类别 1
+	int segmentation_class_size=cloud_seg.size();
+	if(segmentation_class_size<2)
+    {
+	    LOG(ERROR)<<"PointSift detect no car point";
+	    return LOCATER_SIFT_PREDICT_NO_CAR_POINT;
+    }
+	///保存中间文件
 	pcl::PointCloud<pcl::PointXYZRGB>::Ptr seg(new pcl::PointCloud<pcl::PointXYZRGB>);
-	for (int i = 0; i < m_cls_num; ++i)
+	for (int i = 0; i < segmentation_class_size; ++i)
 	{
 		seg->operator +=(*cloud_seg[i]);
 	}

locate/point_sift_segmentation.h

@@ -6,20 +6,37 @@
 #include <pcl/point_types.h>
 #include <pcl/common/common.h>
 
-class LIB_SIFT_API Point_sift_segmentation : public PointSifter
+/*
+ * 封装PointSift点云分割网络,输入点云,分割出汽车,检验结果
+ * 功能包括将pcl点云数据格式转换成原始数据格式float*
+ * 将识别数据转换成点云数据并用不同颜色区分
+ */
+class Point_sift_segmentation : public PointSifter
 {
 public:
 	Point_sift_segmentation(int point_size,int cls,float freq,pcl::PointXYZ minp,pcl::PointXYZ maxp);
-
-    ERROR_CODE ResetRegion(pcl::PointXYZ minp, pcl::PointXYZ maxp);
-	virtual ERROR_CODE Init(std::string graph,std::string cpkt);
-	virtual ERROR_CODE Seg(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, std::vector<pcl::PointCloud<pcl::PointXYZRGB>::Ptr>& clouds, std::string save_dir);
+    //设置要识别的点云区域
+    ERROR_CODE set_region(pcl::PointXYZ minp, pcl::PointXYZ maxp);
+    //初始化网络, 加载网络权重
+	virtual ERROR_CODE init(std::string graph,std::string cpkt);
+	//分割函数,输入点云,输出各类的点云,并保存中间文件方便查看识别结果.
+	//cloud:输入点云
+	//clouds:输出分类好的多个点云(附带颜色,红色为杂物,绿色为车,白色为地面)
+	virtual ERROR_CODE seg(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud,
+	    std::vector<pcl::PointCloud<pcl::PointXYZRGB>::Ptr>& clouds, std::string save_dir);
 
 	virtual ~Point_sift_segmentation();
 protected:
+    //将点云数据转换到float*
+    //cloud:输入点云
+    //output:转换结果保存地址,函数内不负责内存的分配与释放
 	bool	Create_data(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, float* output);
+    //将float*转换成点云
+    //output:点云对应的类别,大小为  点数*类别
+    //cloud:点云数据(xyz)
+    //cloud_seg::输入带颜色的点云
 	bool    RecoveryCloud(float* output, float* cloud, std::vector<pcl::PointCloud<pcl::PointXYZRGB>::Ptr>& cloud_seg);
-	////  ��Ŀ������ˣ� ���˵��ϰ��︽����Ŀ���
+
 	bool	FilterObs(std::vector<pcl::PointCloud<pcl::PointXYZRGB>::Ptr>& cloud_seg, std::string save_dir);
 protected:
 	float		        m_freq;

locate/tf_wheel_3Dcnn_api.h

@@ -1,3 +1,6 @@
+/*
+ * 3dcnn 识别api
+ */
 #ifndef __tf_wheel_3Dcnn__H
 #define __tf_wheel_3Dcnn__H
 #include <string>
@@ -17,9 +20,11 @@
 #endif
 #endif
 #endif
-
+//初始化3dcnn网络参数,长宽高,权重文件
 LIB_3DCNN_API int		 tf_wheel_3dcnn_init(std::string model_file,int length, int width, int height);
+//3dcnn网络预测
 LIB_3DCNN_API bool	 tf_wheel_3dcnn_predict(float* data,float* pOut);
+//关闭网络
 LIB_3DCNN_API int		 tf_wheel_3dcnn_close();
 
 #endif