Merge remote-tracking branch 'origin/zzw_sift_wheel' into yct

.gitignore

@@ -29,7 +29,6 @@ obj/
 [Rr]elease*/
 _ReSharper*/
 [Tt]est[Rr]esult*
-<<<<<<< HEAD
 *.make 
 *.o
 *.bin
@@ -38,12 +37,11 @@ _ReSharper*/
 *.internal
 *.cbp
 *.check_cache
-=======
 .vs/
 #Nuget packages folder
 packages/
 build/
->>>>>>> origin/yct
 .vscode/**
 .idea/**
 cmake-build-debug/**
+.idea/**

CMakeLists.txt

@@ -10,6 +10,7 @@ FIND_PACKAGE(OpenCV REQUIRED)
 FIND_PACKAGE(PCL REQUIRED)
 FIND_PACKAGE(Protobuf REQUIRED)
 FIND_PACKAGE(Glog REQUIRED)
+FIND_PACKAGE(Ceres REQUIRED)
 set(CMAKE_MODULE_PATH "/usr/local/share/")
 set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wl,--no-as-needed")
 set(CMAKE_CXX_FLAGS "-std=c++11 -msse -msse2 -msse3 -msse4 -msse4.1 -msse4.2")
@@ -28,6 +29,7 @@ include_directories(
         /usr/local/include/snap7
   ${PCL_INCLUDE_DIRS}
   ${OpenCV_INCLUDE_DIRS}
+        ${CERES_INCLUDE_DIRS}
 )
 link_directories("/usr/local/lib")
 
@@ -47,6 +49,14 @@ aux_source_directory(${CMAKE_CURRENT_LIST_DIR}/wj_lidar WJLIDAR_SRC )
 #add_executable(plc_test  ./test/plc_test.cpp ${PLC_SRC} ${TASK_MANAGER_SRC} ${TOOL_SRC} ${ERROR_CODE_SRC})
 #target_link_libraries(plc_test ${OpenCV_LIBS}
 #        ${GLOG_LIBRARIES} ${PCL_LIBRARIES} ${PROTOBUF_LIBRARIES} ipopt modbus libnanomsg.so libnnxx.a libglog.a libgflags.a)
+
+add_executable(locate_sample  test/locate_sample.cpp ${LOCATE_SRC}  ${TASK_MANAGER_SRC} ${ERROR_SRC}
+        ${TOOL_SRC})
+target_link_libraries(locate_sample ${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 snap7 nnxx nanomsg)
+
 add_executable(fence_debug  test/plc_s7.cpp wj_lidar/wj_lidar_msg.pb.cc)
 target_link_libraries(fence_debug ${GLOG_LIBRARIES} ${PROTOBUF_LIBRARIES} /usr/local/lib/libglog.a
         /usr/local/lib/libgflags.a nnxx nanomsg)
@@ -54,7 +64,7 @@ target_link_libraries(fence_debug ${GLOG_LIBRARIES} ${PROTOBUF_LIBRARIES} /usr/l
 
 add_executable(locate  main.cpp ${LOCATE_SRC} ${PLC_SRC} ${TERMINOR_SRC} ${TASK_MANAGER_SRC} ${LASER_SRC} ${ERROR_SRC}
         ${TOOL_SRC} ${SYS_SRC} ${VERIFY_SRC} ${WJLIDAR_SRC})
-target_link_libraries(locate ${OpenCV_LIBS}
+target_link_libraries(locate ${OpenCV_LIBS} ${CERES_LIBRARIES}
         ${GLOG_LIBRARIES} ${PCL_LIBRARIES} ${PROTOBUF_LIBRARIES} ipopt libtensorflow_cc.so
         tf_3dcnn_api.so pointSIFT_API.so dark.so /usr/local/lib/libmodbus.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 snap7 nnxx nanomsg)

error_code/error_code.h

@@ -199,6 +199,7 @@ enum Error_code
     WJ_REGION_RECTANGLE_SIZE_ERROR,
     WJ_REGION_RECTANGLE_SYMMETRY_ERROR,
     WJ_REGION_CLUSTER_SIZE_ERROR,
+    WJ_REGION_CERES_SOLVE_ERROR,
 
     //wj manager error from 0x06040000-0x0604FFFF
     WJ_MANAGER_UNINITIALIZED=0x06040000,

laser/Laser.cpp

@@ -1,7 +1,7 @@
 
 
 #include "Laser.h"
-
+#include "laser_message.pb.h"
 
 Laser_base::Laser_base(int laser_id, Laser_proto::laser_parameter laser_param)
 :m_laser_id(laser_id)
@@ -204,7 +204,7 @@ Error_manager Laser_base::end_task()
 	if(mp_laser_task !=NULL)
 	{
 		//判断任务单的错误等级，
-		if ( mp_laser_task->get_task_error_manager().get_error_level() > Error_level::MINOR_ERROR)
+		if ( mp_laser_task->get_task_error_manager().get_error_level() <= Error_level::MINOR_ERROR)
 		{
 			if ( m_laser_statu == LASER_BUSY )
 			{
@@ -243,7 +243,7 @@ Error_manager Laser_base::set_open_save_path(std::string save_path,bool is_save)
 		char pts_file[255] = { 0 };
 		sprintf(pts_file, "%s/pts%d.txt", save_path.c_str(), m_laser_id+1);
 		m_points_log_tool.open(pts_file);
-		std::cout << "huli m_points_log_tool path "<< pts_file << std::endl;
+		//std::cout << "huli m_points_log_tool path "<< pts_file << std::endl;
 
 		char bin_file[255] = { 0 };
 		sprintf(bin_file, "%s/laser%d.data", save_path.c_str(), m_laser_id+1);
@@ -284,6 +284,7 @@ void Laser_base::thread_receive()
 	//接受雷达消息，每次循环只接受一个 Binary_buf
 	while (m_condition_receive.is_alive())
 	{
+
 		m_condition_receive.wait();
 		if ( m_condition_receive.is_alive() )
 		{
@@ -335,20 +336,20 @@ void Laser_base::thread_transform()
 			int huli_test = m_queue_laser_data.size();
 			//thread_receive存入前要手动分配内存，thread_transform取出后要记得释放。
 			bool t_pop_flag = m_queue_laser_data.try_pop(tp_binaty_buf);
-			if ( t_pop_flag )
+			/*if ( t_pop_flag )
 			{
 			    std::cout << "huli m_queue_laser_data.size() last = " << huli_test << std::endl;
 				std::cout << "huli m_queue_laser_data.size() now = " << m_queue_laser_data.size() << std::endl;
 
-			}
+			}*/
 
 			//第2步，处理数据，缓存转化缓存为三维点。
 			//如果获取到了 Binary_buf，或者上次有未完成的 m_last_data，那么就执行数据转化。
 			//注注注注注意了，扫描停止后，m_last_data一定要保证清空，不能有垃圾数据残留，这会阻塞线程的暂停
 			if (t_pop_flag || m_last_data.get_length() > 0)
 			{
-				std::cout << "huli thread_transform " << m_queue_laser_data.size() << std::endl;
-				std::cout << "huli t_pop_flag = "<< t_pop_flag << std::endl;
+				//std::cout << "huli thread_transform " << m_queue_laser_data.size() << std::endl;
+				//std::cout << "huli t_pop_flag = "<< t_pop_flag << std::endl;
 				//缓存类型
 				Buf_type t_buf_type = BUF_UNKNOW;
 				//雷达扫描结果，三维点云（雷达自身坐标系）
@@ -396,7 +397,7 @@ void Laser_base::thread_transform()
 							char buf[64] = {0};
 							sprintf(buf, "%f %f %f\n", t_point.x, t_point.y, t_point.z);
 							m_points_log_tool.write(buf, strlen(buf));
-//							std::cout << "huli m_points_log_tool.write" << std::endl;
+							//std::cout << "huli m_points_log_tool.write" << std::endl;
 						}
 
 						//此时 t_point 为雷达采集数据的结果，转换后的三维点云。
@@ -450,25 +451,26 @@ void Laser_base::thread_transform()
 
 
 //公开发布雷达信息的功能函数，
+#include "../tool/MeasureTopicPublisher.h"
 Error_manager Laser_base::publish_laser_to_message()
 {
-	return Error_code::SUCCESS;
-	/*
-	globalmsg::msg msg;
-	msg.set_msg_type(globalmsg::eLaser);
-
-	globalmsg::laserStatus status;
-	if(GetStatu()==eLaser_ready) status=globalmsg::eLaserConnected;
-	else if(GetStatu()==eLaser_disconnect) status=globalmsg::eLaserDisconnected;
-	else if(GetStatu()==eLaser_busy) status=globalmsg::eLaserBusy;
-	else  status=globalmsg::eLaserUnknown;
-
-	msg.mutable_laser_msg()->set_id(m_laser_id);
-	msg.mutable_laser_msg()->set_laser_status(status);
-	msg.mutable_laser_msg()->set_queue_data_count(m_queue_laser_data.size());
-	msg.mutable_laser_msg()->set_cloud_count(m_points_count);
+    //usleep(5*1000*1000);
+	laser_message::laserMsg msg;
+	laser_message::laserStatus status;
+	if(get_laser_statu()==LASER_READY) status=laser_message::eLaserConnected;
+	else if(get_laser_statu()==LASER_DISCONNECT) status=laser_message::eLaserDisconnected;
+	else if(get_laser_statu()==LASER_BUSY) status=laser_message::eLaserBusy;
+	else if(get_laser_statu()==LASER_FAULT) status=laser_message::eLaserDisconnected;
+	else  status=laser_message::eLaserUnknown;
+
+	msg.set_id(m_laser_id);
+	msg.set_laser_status(status);
+	msg.set_queue_data_count(m_queue_laser_data.size());
+	msg.set_cloud_count(m_points_count);
 	MeasureTopicPublisher::GetInstance()->Publish(msg.SerializeAsString());
-	 */
+
+	return SUCCESS;
+
 }
 //线程执行函数，公开发布雷达的相关信息，用作上位机的监视。
 void Laser_base::thread_publish(Laser_base *p_laser)

laser/LivoxLaser.cpp

@@ -72,7 +72,7 @@ Error_manager CLivoxLaser::execute_task(Task_Base* p_laser_task)
 
 	//检查消息内容是否正确，
 	//检查三维点云指针
-	if (mp_laser_task->get_task_point_cloud().get() == NULL)
+	if (mp_laser_task->get_task_point_cloud()== NULL)
 	{
 		t_result.error_manager_reset(Error_code::POINTER_IS_NULL,
 											Error_level::MINOR_ERROR,
@@ -172,7 +172,7 @@ bool CLivoxLaser::receive_buf_to_queue(Binary_buf& binary_buf)
 	{
 		binary_buf = *tp_livox_buf;
 		delete tp_livox_buf;
-		std::cout << "huli m_queue_livox_data.try_pop" << std::endl;
+		//std::cout << "huli m_queue_livox_data.try_pop" << std::endl;
 		return true;
 	}
 	return false;
@@ -383,15 +383,15 @@ void CLivoxLaser::LidarDataCallback(uint8_t handle, LivoxEthPacket *data, uint32
 		{
 			if (livox->IsScanComplete())
 			{
-				std::cout << "huli qwe point count = " << livox->g_count[livox->m_handle] << std::endl;
+				//std::cout << "huli qwe point count = " << livox->g_count[livox->m_handle] << std::endl;
 				livox->stop_scan();
 				return;
 			}
-			else
+			/*else
 			{
-				std::cout << "huli asd point count = " << livox->g_count[livox->m_handle] << std::endl;
+				//std::cout << "huli asd point count = " << livox->g_count[livox->m_handle] << std::endl;
 
-			}
+			}*/
 //			std::cout << "huli LidarDataCallback " << std::endl;
 
 			if (g_count[handle] >= livox->m_frame_maxnum)
@@ -401,24 +401,16 @@ void CLivoxLaser::LidarDataCallback(uint8_t handle, LivoxEthPacket *data, uint32
 
 			Binary_buf* data_bin = new Binary_buf((char*)p_point_data, data_num * sizeof(LivoxRawPoint));
 			livox->m_queue_livox_data.push(data_bin);
-			std::cout << "huli m_queue_livox_data.push " << std::endl;
+			//std::cout << "huli m_queue_livox_data.push " << std::endl;
 
 			g_count[handle]++;
 
 		}
-	/*	else
-		{
-			std::cout << "huli 1z error " << "data = "<< data  << std::endl;
-			std::cout << "huli 1z error " << "livox = "<< livox  << std::endl;
-
-			//?????????????????
-			livox->m_laser_statu = LASER_READY;
-			usleep(10*1000);
-		}*/
+        livox->m_laser_statu = LASER_READY;
 	}
-	else
+	/*else
 	{
-		std::cout << "huli 2z error " << "data = "<< data  << std::endl;
-		std::cout << "huli 2z error " << "livox = "<< livox  << std::endl;
-	}
+		//std::cout << "huli 2z error " << "data = "<< data  << std::endl;
+		//std::cout << "huli 2z error " << "livox = "<< livox  << std::endl;
+	}*/
 }

laser/LivoxMid100Laser.cpp

@@ -84,14 +84,19 @@ Error_manager CLivoxMid100Laser::execute_task(Task_Base* p_laser_task)
 		return Error_manager(Error_code::LIVOX_TASK_TYPE_ERROR, Error_level::MINOR_ERROR,
 							 "laser task type error ！= LASER_TASK");
 	}
-
+    //检查当前状态
+    t_error=check_laser();
+	if(t_error!=SUCCESS)
+    {
+	    return t_error;
+    }
 	//接受任务，并将任务的状态改为TASK_SIGNED已签收
 	mp_laser_task = (Laser_task *) p_laser_task;
 	mp_laser_task->set_task_statu(TASK_SIGNED);
 
 	//检查消息内容是否正确，
 	//检查三维点云指针
-	if (mp_laser_task->get_task_point_cloud().get() == NULL)
+	if (mp_laser_task->get_task_point_cloud() == NULL)
 	{
 		t_result.error_manager_reset(Error_code::POINTER_IS_NULL,
 									 Error_level::MINOR_ERROR,

laser/laser_task_command.cpp

@@ -32,10 +32,10 @@ Laser_task::~Laser_task()
 // input:p_task_cloud_lock 三维点云的数据保护锁
 //注：task_frame_maxnum默认为0，如果为0，则使用laser默认的点云的采集帧数最大值 TASK_FRAME_MAXNUM_DEFAULT
 Error_manager Laser_task::task_init(Task_statu task_statu,
-                                    pcl::PointCloud<pcl::PointXYZ>::Ptr p_task_point_cloud,
+                                    pcl::PointCloud<pcl::PointXYZ>* p_task_point_cloud,
                                     std::mutex* p_task_cloud_lock)
 {
-    if(p_task_point_cloud.get() == NULL)
+    if(p_task_point_cloud == NULL)
     {
         return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR,
                              "Laser_task::task_init p_task_point_cloud is null");
@@ -66,10 +66,10 @@ Error_manager Laser_task::task_init(Task_statu task_statu,
 Error_manager Laser_task::task_init(Task_statu task_statu,
                                     std::string & task_statu_information,
                                     unsigned int task_frame_maxnum,
-                                    pcl::PointCloud<pcl::PointXYZ>::Ptr p_task_point_cloud,
+                                    pcl::PointCloud<pcl::PointXYZ>* p_task_point_cloud,
                                     std::mutex* p_task_cloud_lock)
 {
-    if(p_task_point_cloud.get() == NULL)
+    if(p_task_point_cloud == NULL)
     {
         return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR,
                              "Laser_task::task_init p_task_point_cloud is null");
@@ -105,7 +105,7 @@ Error_manager Laser_task::task_push_point(pcl::PointXYZ point_xyz)
         return Error_manager(POINTER_IS_NULL,MINOR_ERROR,
                              "push_point laser task input lock is null");
     }
-    if(mp_task_point_cloud.get()==NULL)
+    if(mp_task_point_cloud==NULL)
     {
         return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR,
                              "Laser_task::task_push_point p_task_point_cloud is null");
@@ -131,7 +131,7 @@ std::string Laser_task::get_task_save_path()
     return m_task_save_path;
 }
 //获取 三维点云容器的智能指针
-pcl::PointCloud<pcl::PointXYZ>::Ptr Laser_task::get_task_point_cloud()
+pcl::PointCloud<pcl::PointXYZ>* Laser_task::get_task_point_cloud()
 {
     return mp_task_point_cloud;
 }
@@ -161,7 +161,7 @@ void Laser_task::set_task_save_path(std::string task_save_path)
     m_task_save_path=task_save_path;
 }
 //设置 三维点云容器的智能指针
-void Laser_task::set_task_point_cloud(pcl::PointCloud<pcl::PointXYZ>::Ptr p_task_point_cloud)
+void Laser_task::set_task_point_cloud(pcl::PointCloud<pcl::PointXYZ>* p_task_point_cloud)
 {
     mp_task_point_cloud = p_task_point_cloud;
 }

laser/laser_task_command.h

@@ -31,7 +31,7 @@ public:
     //构造函数，构造函数锁定任务类型为LASER_TASK，后续不允许更改
     Laser_task();
     //析构函数
-    ~Laser_task();
+    virtual ~Laser_task();
 
     //初始化任务单，必须初始化之后才可以使用，（必选参数）
     // input:task_statu 任务状态
@@ -39,7 +39,7 @@ public:
     // input:p_task_cloud_lock 三维点云的数据保护锁
     //注：task_frame_maxnum默认为0，如果为0，则使用laser默认的点云的采集帧数最大值 TASK_FRAME_MAXNUM_DEFAULT
     Error_manager task_init(Task_statu task_statu,
-                            pcl::PointCloud<pcl::PointXYZ>::Ptr p_task_point_cloud,
+                            pcl::PointCloud<pcl::PointXYZ>* p_task_point_cloud,
                             std::mutex* p_task_cloud_lock);
 
     //初始化任务单，必须初始化之后才可以使用，（可选参数）
@@ -52,7 +52,7 @@ public:
     Error_manager task_init(Task_statu task_statu,
                             std::string & task_statu_information,
                             unsigned int task_frame_maxnum,
-                            pcl::PointCloud<pcl::PointXYZ>::Ptr p_task_point_cloud,
+                            pcl::PointCloud<pcl::PointXYZ>* p_task_point_cloud,
                             std::mutex* p_task_cloud_lock);
 
     //push点云，把 point_xyz 添加到 p_task_point_cloud 里面。
@@ -65,7 +65,7 @@ public:
     //获取采集的点云保存路径
     std::string get_task_save_path();
     //获取 三维点云容器的智能指针
-    pcl::PointCloud<pcl::PointXYZ>::Ptr get_task_point_cloud();
+    pcl::PointCloud<pcl::PointXYZ>* get_task_point_cloud();
 
 
 
@@ -79,7 +79,7 @@ public:
     //设置采集的点云保存路径
     void set_task_save_path(std::string task_save_path);
     //设置 三维点云容器的智能指针
-    void set_task_point_cloud(pcl::PointCloud<pcl::PointXYZ>::Ptr p_task_point_cloud);
+    void set_task_point_cloud(pcl::PointCloud<pcl::PointXYZ>* p_task_point_cloud);
     //设置 错误码
     void set_task_error_manager(Error_manager & error_manager);
 
@@ -101,7 +101,7 @@ protected:
     std::mutex*                     mp_task_cloud_lock;
     //采集结果，三维点云容器的智能指针，任务输出
     //这里只是指针，实际内存由应用层管理，初始化时必须保证内存有效。
-    pcl::PointCloud<pcl::PointXYZ>::Ptr        mp_task_point_cloud;
+    pcl::PointCloud<pcl::PointXYZ>*        mp_task_point_cloud;
 
 
 };

locate/locate_task.h

@@ -31,7 +31,7 @@ class Locate_task: public Task_Base
 {
 public:
     Locate_task();
-    ~Locate_task();
+    virtual  ~Locate_task();
     //设置测量输入点云
     Error_manager set_locate_cloud(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud);
     //获取输入点云

plc/plc_communicator.cpp

@@ -202,6 +202,8 @@ Error_manager Plc_Communicator::write_result_to_plc(struct measure_result result
         int retry_times = 3;
         int plc_write_return_code = -1;
         while(retry_times-->0 && plc_write_return_code<0) {
+		// 更新时间
+		m_plc_region_status[result.terminal_id - 1].last_time_point = std::chrono::steady_clock::now();
             // 写入数据
             m_plc_mutex.lock();
             plc_write_return_code = m_plc_wrapper.write_registers(offset, result_length, result_info_temp);
@@ -441,4 +443,4 @@ void Plc_Communicator::plc_update_thread(Plc_Communicator *plc_communicator)
 
         usleep(1000 * PLC_SLEEP_IN_MILLISECONDS);
     }
-}
+}

task/task_command_manager.h

@@ -29,7 +29,7 @@ enum Task_statu
 class Task_Base
 {
 public:
-    ~Task_Base();
+    virtual ~Task_Base();
     //初始化任务单,初始任务单类型为 UNKONW_TASK
     virtual Error_manager init();
     //更新任务单

terminor/terminal_command_executor.cpp

@@ -7,6 +7,8 @@
 #include "terminor_msg.pb.h"
 #include "MeasureRequest.h"
 
+std::mutex Terminor_Command_Executor::ms_mutex_scan_measure;
+
 //////以下两个函数用于测试,读取指定点云
 bool string2point(std::string str,pcl::PointXYZ& point)
 {
@@ -293,237 +295,237 @@ void Terminor_Command_Executor::set_save_root_path(std::string root)
     m_save_root_path=root;
 }
 
-Error_manager Terminor_Command_Executor::scanning_measuring()
-{
+Error_manager Terminor_Command_Executor::scanning_measuring() {
+    std::lock_guard<std::mutex> lock(ms_mutex_scan_measure);
+    Error_manager code;
+    LOG(INFO) << ">>>>>>>>>>>> Enter terminator ID:" << m_terminor_parameter.id();
     //计时起点
-    auto t_start_point=std::chrono::system_clock::now();
+    auto t_start_point = std::chrono::system_clock::now();
 
     //准备目录
     time_t tt;
-    time( &tt );
-    tt = tt + 8*3600;  // transform the time zone
-    tm* t= gmtime( &tt );
-    char path[255]={0};
-    sprintf(path,"%s/%4d/%02d/%02d",m_save_root_path.c_str(),
+    time(&tt);
+    tt = tt + 8 * 3600;  // transform the time zone
+    tm *t = gmtime(&tt);
+    char path[255] = {0};
+    sprintf(path, "%s/%4d/%02d/%02d", m_save_root_path.c_str(),
             t->tm_year + 1900,
             t->tm_mon + 1,
             t->tm_mday);
     PathCreator path_creator;
     path_creator.CreateDatePath(path);
-    std::string project_path=path_creator.GetCurPath();
+    std::string project_path = path_creator.GetCurPath();
+
 
-    Error_manager code;
     //第一步,启动雷达扫描点云,切换当前状态为扫描中
     //根据配置筛选雷达
-    pcl::PointCloud<pcl::PointXYZ>::Ptr scan_cloud(new pcl::PointCloud<pcl::PointXYZ>);
+    pcl::PointCloud<pcl::PointXYZ> scan_cloud;
     std::mutex cloud_lock;
-    std::vector<Task_Base*> laser_task_vector;
-
-    std::vector<Laser_base*> tp_lasers;
-    for(int i=0;i<m_terminor_parameter.laser_parameter_size();++i)
-    {
-        int laser_id=m_terminor_parameter.laser_parameter(i).id();
-        int frame_count=m_terminor_parameter.laser_parameter(i).frame_count();
-        if(frame_count<=0)
-        {
-            LOG(WARNING)<<"terminal parameter laser["<<laser_id<<"] frame count is <0";
+    std::vector<Laser_task *> laser_task_vector;
+
+    std::vector<Laser_base *> tp_lasers;
+    for (int i = 0; i < m_terminor_parameter.laser_parameter_size(); ++i) {
+        int laser_id = m_terminor_parameter.laser_parameter(i).id();
+        int frame_count = m_terminor_parameter.laser_parameter(i).frame_count();
+        if (frame_count <= 0) {
+            LOG(WARNING) << "terminal parameter laser[" << laser_id << "] frame count is <0";
             continue;
         }
-        if(laser_id>=0&&laser_id<mp_laser_vector.size())
-        {
+        if (laser_id >= 0 && laser_id < mp_laser_vector.size()) {
             //判断该id的雷达是否已经添加进去, 放置配置中出现重复的雷达编号
-            bool tb_repeat=false;
-            for(int j=0;j<tp_lasers.size();++j)
-            {
-                if(tp_lasers[j]==mp_laser_vector[laser_id])
-                {
-                    tb_repeat=true;
+            bool tb_repeat = false;
+            for (int j = 0; j < tp_lasers.size(); ++j) {
+                if (tp_lasers[j] == mp_laser_vector[laser_id]) {
+                    tb_repeat = true;
                     break;
                 }
             }
-            if(tb_repeat==false)
-            {
+            if (tb_repeat == false) {
                 tp_lasers.push_back(mp_laser_vector[laser_id]);
                 //创建扫描任务,
-                Laser_task* laser_task=new Laser_task();
+                Laser_task *laser_task = new Laser_task();
                 //
-                laser_task->task_init(TASK_CREATED,scan_cloud,&cloud_lock);
+                laser_task->task_init(TASK_CREATED, &scan_cloud, &cloud_lock);
                 laser_task->set_task_frame_maxnum(frame_count);
                 laser_task->set_save_path(project_path);
                 laser_task_vector.push_back(laser_task);
                 //发送任务单给雷达
-                code=tp_lasers[i]->execute_task(laser_task);
-                if(code!=SUCCESS)
-                {
+                code = tp_lasers[i]->execute_task(laser_task);
+                if (code != SUCCESS) {
                     return code;
                 }
             }
         }
     }
-    if(tp_lasers.size()==0)
-    {
-        return Error_manager(TERMINOR_NOT_CONTAINS_LASER,NORMAL,"terminal not contains laser");
+    if (tp_lasers.size() == 0) {
+        return Error_manager(TERMINOR_NOT_CONTAINS_LASER, NORMAL, "terminal not contains laser");
     }
-    m_terminor_statu=TERMINOR_SCANNING;
+    m_terminor_statu = TERMINOR_SCANNING;
+
     //等待雷达完成任务单
-    while(1)
-    {
+    while (1) {
         //判断是否强制退出
-        if(mb_force_quit==true)
-        {
-            char description[255]={0};
-            sprintf(description,"ternimal is force quit terminal id : %d",m_terminor_parameter.id());
-            return Error_manager(TERMINOR_FORCE_QUIT,NORMAL,description);
+        if (mb_force_quit == true) {
+            char description[255] = {0};
+            sprintf(description, "ternimal is force quit terminal id : %d", m_terminor_parameter.id());
+            return Error_manager(TERMINOR_FORCE_QUIT, NORMAL, description);
         }
         //判断雷达任务单是否全部完成
-        bool tb_laser_complete=true;
-        for(int i=0;i<laser_task_vector.size();++i)
-        {
-            tb_laser_complete &=(TASK_OVER==laser_task_vector[i]->get_statu());
+        bool tb_laser_complete = true;
+        for (int i = 0; i < laser_task_vector.size(); ++i) {
+            tb_laser_complete &= (TASK_OVER == laser_task_vector[i]->get_statu());
         }
-        if(tb_laser_complete)
-        {
+        if (tb_laser_complete) {
             break;
         }
         //计算扫描时间,若超时,并且没有点,则返回错误.
-        auto t_end_point=std::chrono::system_clock::now();
-        auto duration=std::chrono::duration_cast<std::chrono::milliseconds>(t_end_point - t_start_point);
-        double second=double(duration.count()) *
-            std::chrono::milliseconds::period::num / std::chrono::milliseconds::period::den;
-        if(second>m_timeout_second)
-        {
+        auto t_end_point = std::chrono::system_clock::now();
+        auto duration = std::chrono::duration_cast<std::chrono::milliseconds>(t_end_point - t_start_point);
+        double second = double(duration.count()) *
+                        std::chrono::milliseconds::period::num / std::chrono::milliseconds::period::den;
+        if (second > m_timeout_second) {
             //扫描超时,点云中没有点,则返回错误
-            if(scan_cloud->size()==0)
-            {
-                return Error_manager(TERMINOR_LASER_TIMEOUT,MAJOR_ERROR,"laser scanning timeout");
+            if (scan_cloud.size() == 0) {
+                return Error_manager(TERMINOR_LASER_TIMEOUT, MAJOR_ERROR, "laser scanning timeout");
             }
 
 
         }
-        usleep(1000*100);
+        usleep(1000 * 100);
     }
     //检查雷达任务完成情况,是否是正常完成
-    LOG(INFO)<<" laser scanning over cloud size:"<<scan_cloud->size();
+    LOG(INFO) << " laser scanning over cloud size:" << scan_cloud.size();
     //释放扫描任务单
-    for(int i=0;i<laser_task_vector.size();++i)
-    {
-        if(laser_task_vector[i]!=0)
-        {
+    for (int i = 0; i < laser_task_vector.size(); ++i) {
+        if (laser_task_vector[i] != 0) {
             delete laser_task_vector[i];
-            laser_task_vector[i]=NULL;
+            laser_task_vector[i] = NULL;
         }
     }
+
     //第二步,根据区域筛选点云
     pcl::PointCloud<pcl::PointXYZ>::Ptr locate_cloud(new pcl::PointCloud<pcl::PointXYZ>);
     pcl::PassThrough<pcl::PointXYZ> passX;
     pcl::PassThrough<pcl::PointXYZ> passY;
     pcl::PassThrough<pcl::PointXYZ> passZ;
-    passX.setInputCloud(scan_cloud);
+    passX.setInputCloud(scan_cloud.makeShared());
     passX.setFilterFieldName("x");
-    passX.setFilterLimits(m_terminor_parameter.area_3d().min_x(),m_terminor_parameter.area_3d().max_x());
+    passX.setFilterLimits(m_terminor_parameter.area_3d().min_x(), m_terminor_parameter.area_3d().max_x());
     passX.filter(*locate_cloud);
 
     passY.setInputCloud(locate_cloud);
     passY.setFilterFieldName("y");
-    passY.setFilterLimits(m_terminor_parameter.area_3d().min_y(),m_terminor_parameter.area_3d().max_y());
+    passY.setFilterLimits(m_terminor_parameter.area_3d().min_y(), m_terminor_parameter.area_3d().max_y());
     passY.filter(*locate_cloud);
 
     passY.setInputCloud(locate_cloud);
     passY.setFilterFieldName("z");
-    passY.setFilterLimits(m_terminor_parameter.area_3d().min_z(),m_terminor_parameter.area_3d().max_z());
+    passY.setFilterLimits(m_terminor_parameter.area_3d().min_z(), m_terminor_parameter.area_3d().max_z());
     passY.filter(*locate_cloud);
-    LOG(INFO)<<"筛选点云点数 : "<<locate_cloud->size();
+    LOG(INFO) << "筛选点云点数 : " << locate_cloud->size();
+
+
 
     //第三步,创建测量任务, 进入测量中
-    Locate_task* locate_task=new Locate_task();
+    Locate_task *locate_task = new Locate_task();
     locate_task->set_locate_cloud(locate_cloud);
     locate_task->set_save_path(project_path);
-    m_terminor_statu=TERMINOR_MEASURING;
-    code=mp_locater->execute_task(locate_task);
-    m_measure_information.locate_correct=false;
+    m_terminor_statu = TERMINOR_MEASURING;
+    code = mp_locater->execute_task(locate_task);
+    m_measure_information.locate_correct = false;
     //获取测量结果
-    Locate_information dj_locate_information=locate_task->get_locate_information();
+    Locate_information dj_locate_information = locate_task->get_locate_information();
     //释放locate task
-    if(locate_task!=NULL)
-    {
+    if (locate_task != NULL) {
         delete locate_task;
-        locate_task=NULL;
+        locate_task = NULL;
     }
-    if(code!=SUCCESS)
-    {
+    if (code != SUCCESS) {
         return code;
     }
 
+
     //进入万集雷达流程--------
-    Wj_lidar_Task* wj_task=new Wj_lidar_Task();
-    code=wj_task->init();
-    if(code!=SUCCESS)
-    {
+    Wj_lidar_Task *wj_task = new Wj_lidar_Task();
+    code = wj_task->init();
+    if (code != SUCCESS) {
+        delete wj_task;
         return code;
     }
     wj_command t_wj_command;
-    t_wj_command.terminal_id=m_terminor_parameter.id();
-    t_wj_command.command_time=std::chrono::steady_clock::now();
-    t_wj_command.timeout_in_milliseconds=2000;
+    t_wj_command.terminal_id = m_terminor_parameter.id();
+    t_wj_command.command_time = std::chrono::steady_clock::now();
+    t_wj_command.timeout_in_milliseconds = 2000;
     wj_task->set_command(t_wj_command);
-    code=mp_wj_lidar->execute_task(wj_task);
-    if(code!=SUCCESS)
-    {
+    code = mp_wj_lidar->execute_task(wj_task);
+    if (code != SUCCESS) {
+        delete wj_task;
         return code;
     }
     ///万集测量正确,对比两数据
     wj_measure_result wj_result;
-    code=wj_task->get_result(wj_result);
-    if(code!=SUCCESS)
-    {
+    code = wj_task->get_result(wj_result);
+    if (code != SUCCESS) {
+        delete wj_task;
         return code;
     }
     Locate_information wj_locate_information;
-    wj_locate_information.locate_x=wj_result.x;
-    wj_locate_information.locate_y=wj_result.y;
-    wj_locate_information.locate_theta=wj_result.angle;
-    wj_locate_information.locate_wheel_base=wj_result.wheel_base;
-    wj_locate_information.locate_width=wj_result.width;
+    wj_locate_information.locate_x = wj_result.x;
+    wj_locate_information.locate_y = wj_result.y;
+    wj_locate_information.locate_theta = wj_result.angle;
+    wj_locate_information.locate_wheel_base = wj_result.wheel_base;
+    wj_locate_information.locate_width = wj_result.width;
     //对比两个数据
-    float offset_x=fabs(dj_locate_information.locate_x-wj_locate_information.locate_x);
-    float offset_y=fabs(dj_locate_information.locate_y-wj_locate_information.locate_y);
-    float offset_angle=fabs(dj_locate_information.locate_theta-wj_locate_information.locate_theta);
-    float offset_width=fabs(dj_locate_information.locate_width-wj_locate_information.locate_width);
-    float offset_wheel_base=fabs(dj_locate_information.locate_width-wj_locate_information.locate_width);
-    if(offset_x>100 ||offset_y>100 ||offset_angle>2 || offset_wheel_base>200 ||offset_width>100)
-    {
-        return Error_manager(TERMINOR_CHECK_RESULTS_ERROR,NORMAL,"check results failed ");
+    float offset_x = fabs(dj_locate_information.locate_x - wj_locate_information.locate_x);
+    float offset_y = fabs(dj_locate_information.locate_y - wj_locate_information.locate_y);
+    float offset_angle = fabs(dj_locate_information.locate_theta - wj_locate_information.locate_theta);
+    float offset_width = fabs(dj_locate_information.locate_width - wj_locate_information.locate_width);
+    float offset_wheel_base = fabs(
+            dj_locate_information.locate_wheel_base - wj_locate_information.locate_wheel_base);
+    if (offset_x > 100 || offset_y > 100 || offset_angle > 2 || offset_wheel_base > 200 || offset_width > 200) {
+        LOG(WARNING) << "chekc failed. (offset x>100, y>100, angle>2, wheel_base>200, width>200): " << offset_x
+                     << " " << offset_y << " " << offset_angle << " " << offset_wheel_base << " " << offset_width;
+        return Error_manager(TERMINOR_CHECK_RESULTS_ERROR, NORMAL, "check results failed ");
     }
     ///根据两个结果选择最优结果,中心点选择万集雷达,角度选择两值加权,轴距已以2750为基准作卡尔曼滤波
     ///车长以大疆雷达为准,车宽以万集雷达为准,高以大疆雷达为准
-    m_measure_information.locate_x=wj_locate_information.locate_x;
-    m_measure_information.locate_y=wj_locate_information.locate_y;
-    m_measure_information.locate_theta=0.5*(wj_locate_information.locate_theta+dj_locate_information.locate_theta);
-    float dj_distance=fabs(dj_locate_information.locate_wheel_base-2750);
-    float wj_distance=fabs(wj_locate_information.locate_wheel_base-2750);
-    float weight_dj=wj_distance/(dj_distance+wj_distance);
-    float weight_wj=dj_distance/(dj_distance+wj_distance);
-    m_measure_information.locate_wheel_base=weight_dj*dj_locate_information.locate_wheel_base+
-        weight_wj*wj_locate_information.locate_wheel_base;
-    m_measure_information.locate_length=dj_locate_information.locate_length;
-    m_measure_information.locate_width=wj_locate_information.locate_width;
-    m_measure_information.locate_hight=dj_locate_information.locate_hight;
-    m_measure_information.locate_correct=true;
+    m_measure_information.locate_x = wj_locate_information.locate_x;
+    m_measure_information.locate_y = wj_locate_information.locate_y;
+    if (offset_angle < 2)
+        m_measure_information.locate_theta =
+                0.5 * wj_locate_information.locate_theta + 0.5 * dj_locate_information.locate_theta;
+    else
+        m_measure_information.locate_theta =
+                0.1 * wj_locate_information.locate_theta + 0.9 * dj_locate_information.locate_theta;
+
+    float dj_distance = fabs(dj_locate_information.locate_wheel_base - 2750);
+    float wj_distance = fabs(wj_locate_information.locate_wheel_base - 2750);
+    float weight_dj = wj_distance / (dj_distance + wj_distance);
+    float weight_wj = dj_distance / (dj_distance + wj_distance);
+    m_measure_information.locate_wheel_base = weight_dj * dj_locate_information.locate_wheel_base +
+                                              weight_wj * wj_locate_information.locate_wheel_base;
+    m_measure_information.locate_length = dj_locate_information.locate_length;
+    m_measure_information.locate_width = wj_locate_information.locate_width;
+    m_measure_information.locate_hight = dj_locate_information.locate_hight;
+    m_measure_information.locate_correct = true;
     ////如果测量正确,检验结果
-    if(mp_verify_tool!=NULL) {
+    if (mp_verify_tool != NULL) {
 
         cv::RotatedRect rotated_rect;
         rotated_rect.center = cv::Point2f(m_measure_information.locate_x, m_measure_information.locate_y);
         rotated_rect.angle = m_measure_information.locate_theta;
-        float ext_length=720;
-        float robot_width=2650.0;
-        float new_length=m_measure_information.locate_length+ext_length*2.0;
-        rotated_rect=create_rotate_rect(new_length,robot_width,m_measure_information.locate_theta,
-                                        m_measure_information.locate_x, m_measure_information.locate_y);
-        code = mp_verify_tool->verify(rotated_rect,m_measure_information.locate_hight, false);
+        float ext_length = 720;
+        float robot_width = 2650.0;
+        float new_length = m_measure_information.locate_length + ext_length * 2.0;
+        rotated_rect = create_rotate_rect(new_length, robot_width, m_measure_information.locate_theta,
+                                          m_measure_information.locate_x, m_measure_information.locate_y);
+        code = mp_verify_tool->verify(rotated_rect, m_measure_information.locate_hight, false);
         m_measure_information.locate_correct = (code == SUCCESS);
+        delete wj_task;
         return code;
 
+    } else {
+        delete wj_task;
     }
 
     return code;
@@ -558,4 +560,4 @@ cv::RotatedRect Terminor_Command_Executor::create_rotate_rect(float length,float
         point_vec.push_back(cv::Point2f(x,y));
     }
     return cv::minAreaRect(point_vec);
-}
+}

terminor/terminal_command_executor.h

@@ -13,6 +13,7 @@
 #include "../wj_lidar/fence_controller.h"
 #include "Terminor_parameter.pb.h"
 #include <thread>
+#include <mutex>
 /*
  * 终端指令执行状态,枚举
  * 列举终端指令从进入流程到测量完成过程中的进度状态
@@ -111,6 +112,9 @@ protected:
     //检验结果工具
     Verify_result*              mp_verify_tool;
 
+    //
+    static std::mutex                  ms_mutex_scan_measure;
+
 };
 
 #endif //TERMINOR_H

test/locate_sample.cpp

@@ -7,6 +7,7 @@
 #include <fcntl.h>
 #include "../locate/locate_task.h"
 #include "../locate/locater.h"
+#include <pcl/filters/passthrough.h>
 #include <glog/logging.h>
 #include <google/protobuf/io/coded_stream.h>
 #include <google/protobuf/io/zero_copy_stream_impl.h>
@@ -25,7 +26,7 @@ void init_glog()
 {
     FLAGS_max_log_size = 100;
     FLAGS_logbufsecs = 0;
-    google::InitGoogleLogging("LidarMeasurement");
+    google::InitGoogleLogging("LidarMeasurement_test");
     google::SetStderrLogging(google::INFO);
     google::InstallFailureSignalHandler();
     FLAGS_colorlogtostderr = true;        // Set log color
@@ -101,27 +102,28 @@ bool string2point(std::string str,pcl::PointXYZ& point)
     return true;
 }
 
-pcl::PointCloud<pcl::PointXYZ>::Ptr ReadTxtCloud(std::string file)
+void ReadTxtCloud(std::string file, pcl::PointCloud<pcl::PointXYZ> &cloud)
 {
+    cloud.clear();
     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>);
+    // 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);
+            cloud.push_back(point);
         }
     }
-    return cloud;
+    // return *cloud;
 }
 
 int main(int argc,char* argv[])
 {
     init_glog();
-    std::string input_file="balck_suv";
+    std::string input_file="cloud";
     std::string out_path="./test";
     if(argc>=2)
     {
@@ -143,45 +145,82 @@ int main(int argc,char* argv[])
     {
         LOG(ERROR)<<code.to_string();
     }
-    Locate_task* task=new Locate_task();
+    
 
-    std::string cloud_path="/home/zx/data/samples/src_txt/";
+    std::string cloud_path="/home/zx/zzw/code/LidarMeasure/build/";
     cloud_path+=input_file;
     std::vector<std::string> files;
     list_dir(cloud_path.c_str(),files);
     int count=files.size();
-    int correct_size=0;
-    for(int i=0;i<files.size();++i) {
-
-        pcl::PointCloud<pcl::PointXYZ>::Ptr cloud;
-        std::string t_file=cloud_path+"/"+files[i];
-        cloud = ReadTxtCloud(t_file);
-        std::cout << "  input file: " << t_file << std::endl;
-        code = task->set_locate_cloud(cloud);
-        if (code != SUCCESS) {
-            LOG(ERROR) << code.to_string();
-            return 0;
-        }
-        task->set_save_path(out_path);
-        code = locater->execute_task(task, 5);
-        if (code == SUCCESS) {
-            LOG(INFO) << " LOCATE SUCCESS";
-            correct_size++;
-        }
-        else {
-            LOG(ERROR) << code.to_string();
-            //移动文件
-            usleep(300);
-            std::string src="./test/cnn3d.txt";
-            std::string dst="./error/";
-            dst+=files[i];
-            std::string name="mv "+src+" "+dst;
-            const char *des_name=name.c_str();
-
-            system(des_name); //调用系统命令
-            usleep(200);
+    int correct_size = 0;
+    while (1)
+    {
+        int count = 20;
+        while(count-->0){
+
+            for (int i = 0; i < files.size(); ++i)
+            {
+
+                pcl::PointCloud<pcl::PointXYZ> cloud;
+                std::string t_file = cloud_path + "/" + files[i];
+                ReadTxtCloud(t_file, cloud);
+                std::cout << "  input file: " << t_file << std::endl;
+
+                //第二步,根据区域筛选点云
+                pcl::PointCloud<pcl::PointXYZ>::Ptr locate_cloud(new pcl::PointCloud<pcl::PointXYZ>);
+                pcl::PassThrough<pcl::PointXYZ> passX;
+                pcl::PassThrough<pcl::PointXYZ> passY;
+                pcl::PassThrough<pcl::PointXYZ> passZ;
+                passX.setInputCloud(cloud.makeShared());
+                passX.setFilterFieldName("x");
+                passX.setFilterLimits(-100000, 100000);
+                passX.filter(*locate_cloud);
+
+                passY.setInputCloud(locate_cloud);
+                passY.setFilterFieldName("y");
+                passY.setFilterLimits(-100000, 100000);
+                passY.filter(*locate_cloud);
+
+                passY.setInputCloud(locate_cloud);
+                passY.setFilterFieldName("z");
+                passY.setFilterLimits(-100000, 100000);
+                passY.filter(*locate_cloud);
+                LOG(INFO) << "筛选点云点数 : " << locate_cloud->size();
+
+                Locate_task* task=new Locate_task();
+                code = task->set_locate_cloud(locate_cloud);
+                if (code != SUCCESS)
+                {
+                    LOG(ERROR) << code.to_string();
+                    return 0;
+                }
+                task->set_save_path(out_path);
+                code = locater->execute_task(task, 5);
+				Locate_information dj_locate_information = task->get_locate_information();
+                if (code == SUCCESS)
+                {
+                    LOG(INFO) << " LOCATE SUCCESS";
+                    correct_size++;
+                }
+                else
+                {
+                    LOG(ERROR) << code.to_string();
+                    //移动文件
+                    usleep(300);
+                    std::string src = "./test/cnn3d.txt";
+                    std::string dst = "./error/";
+                    dst += files[i];
+                    std::string name = "mv " + src + " " + dst;
+                    const char *des_name = name.c_str();
+
+                    system(des_name); //调用系统命令
+                    usleep(200);
+                }
+                delete task;
+            }
 
         }
+        getchar();
     }
     if(count>0)
     {

wj_lidar/detect_wheel_ceres.cpp

@@ -0,0 +1,352 @@
+//
+// Created by zx on 2020/7/1.
+//
+
+#include "detect_wheel_ceres.h"
+#include <ceres/cubic_interpolation.h>
+#include <pcl/common/transforms.h>
+
+
+constexpr float kMinProbability = 0.0f;
+constexpr float kMaxProbability = 1.f - kMinProbability;
+constexpr float kMaxCorrespondenceCost = 1.f - kMinProbability;
+constexpr int kPadding = INT_MAX / 4;
+class GridArrayAdapter {
+public:
+    enum { DATA_DIMENSION = 1 };
+
+    explicit GridArrayAdapter(const cv::Mat& grid) : grid_(grid) {}
+
+    void GetValue(const int row, const int column, double* const value) const {
+        if (row < kPadding || column < kPadding || row >= NumRows() - kPadding ||
+            column >= NumCols() - kPadding) {
+            *value = kMaxCorrespondenceCost;
+        } else {
+            *value = static_cast<double>(grid_.at<double >(row - kPadding, column - kPadding));
+        }
+    }
+
+    int NumRows() const {
+        return grid_.rows + 2 * kPadding;
+    }
+
+    int NumCols() const {
+        return grid_.cols + 2 * kPadding;
+    }
+private:
+    const cv::Mat& grid_;
+};
+
+class CostFunctor {
+private:
+    cv::Mat  m_map;
+    double m_scale;
+    pcl::PointCloud<pcl::PointXYZ>          m_left_front_cloud;     //左前点云
+    pcl::PointCloud<pcl::PointXYZ>          m_right_front_cloud;    //右前点云
+    pcl::PointCloud<pcl::PointXYZ>          m_left_rear_cloud;      //左后点云
+    pcl::PointCloud<pcl::PointXYZ>          m_right_rear_cloud;     //右后点云
+
+public:
+    CostFunctor(pcl::PointCloud<pcl::PointXYZ> left_front,pcl::PointCloud<pcl::PointXYZ> right_front,
+                pcl::PointCloud<pcl::PointXYZ> left_rear,pcl::PointCloud<pcl::PointXYZ> right_rear,
+                cv::Mat& map,double scale)
+    {
+        m_map=map;
+        m_scale=scale;
+        m_left_front_cloud=left_front;
+        m_right_front_cloud=right_front;
+        m_left_rear_cloud=left_rear;
+        m_right_rear_cloud=right_rear;
+    }
+    template <typename T>
+    bool operator()(const T* const variable, T* residual) const {
+        T cx = variable[0];
+        T cy = variable[1];
+        T theta = variable[2];
+        T length = variable[3];
+        T width = variable[4];
+        T theta_front = variable[5];
+
+        //整车旋转
+        Eigen::Rotation2D<T> rotation(theta);
+        Eigen::Matrix<T, 2, 2> rotation_matrix = rotation.toRotationMatrix();
+
+        //左前偏移
+        Eigen::Matrix<T, 2, 1> wheel_center_normal_left_front(length / 2.0, width / 2.0);
+        //右前偏移
+        Eigen::Matrix<T, 2, 1> wheel_center_normal_right_front(length / 2.0, -width / 2.0);
+        //左后偏移
+        Eigen::Matrix<T, 2, 1> wheel_center_normal_left_rear(-length / 2.0, width / 2.0);
+        //右后偏移
+        Eigen::Matrix<T, 2, 1> wheel_center_normal_right_rear(-length / 2.0, -width / 2.0);
+
+        //前轮旋转
+        Eigen::Rotation2D<T> rotation_front(theta_front);
+        Eigen::Matrix<T, 2, 2> rotation_matrix_front = rotation_front.toRotationMatrix();
+
+
+        const GridArrayAdapter adapter(m_map);
+        ceres::BiCubicInterpolator<GridArrayAdapter> interpolator(adapter);
+
+        double rows = m_map.rows;
+        double cols = m_map.cols;
+        //左前轮
+        int left_front_num = m_left_front_cloud.size();
+        for (int i = 0; i < m_left_front_cloud.size(); ++i) {
+            const Eigen::Matrix<T, 2, 1> point((T(m_left_front_cloud[i].x) - cx),
+                                               (T(m_left_front_cloud[i].y) - cy));
+            //减去经过车辆旋转计算的左前中心
+            const Eigen::Matrix<T, 2, 1> tanslate_point = rotation_matrix * point - wheel_center_normal_left_front;
+            //旋转
+            const Eigen::Matrix<T, 2, 1> point_rotation = rotation_matrix_front * tanslate_point;
+            interpolator.Evaluate(point_rotation(1, 0) * m_scale + rows / 2.0 + 0.5 + T(kPadding),
+                                  point_rotation(0, 0) * m_scale + cols / 2.0 + 0.5 + T(kPadding),
+                                  &residual[i]);
+
+        }
+        //右前轮
+        int right_front_num = m_right_front_cloud.size();
+        for (int i = 0; i < m_right_front_cloud.size(); ++i) {
+            const Eigen::Matrix<T, 2, 1> point((T(m_right_front_cloud[i].x) - cx),
+                                               (T(m_right_front_cloud[i].y) - cy));
+            //减去经过车辆旋转计算的左前中心
+            const Eigen::Matrix<T, 2, 1> tanslate_point = rotation_matrix * point - wheel_center_normal_right_front;
+            //旋转
+            const Eigen::Matrix<T, 2, 1> point_rotation = rotation_matrix_front * tanslate_point;
+            interpolator.Evaluate(point_rotation(1, 0) * m_scale + rows / 2.0 + 0.5 + T(kPadding),
+                                  point_rotation(0, 0) * m_scale + cols / 2.0 + 0.5 + T(kPadding),
+                                  &residual[left_front_num + i]);
+
+        }
+        //左后轮
+        int left_rear_num = m_left_rear_cloud.size();
+
+        for (int i = 0; i < m_left_rear_cloud.size(); ++i) {
+            const Eigen::Matrix<T, 2, 1> point((T(m_left_rear_cloud[i].x) - cx),
+                                               (T(m_left_rear_cloud[i].y) - cy));
+            //减去经过车辆旋转计算的左前中心
+            const Eigen::Matrix<T, 2, 1> tanslate_point = rotation_matrix * point - wheel_center_normal_left_rear;
+
+            interpolator.Evaluate(tanslate_point(1, 0) * m_scale + rows / 2.0 + 0.5 + T(kPadding),
+                                  tanslate_point(0, 0) * m_scale + cols / 2.0 + 0.5 + T(kPadding),
+                                  &residual[left_front_num + right_front_num + i]);
+
+        }
+
+        //右后轮
+
+        for (int i = 0; i < m_right_rear_cloud.size(); ++i) {
+            const Eigen::Matrix<T, 2, 1> point((T(m_right_rear_cloud[i].x) - cx),
+                                               (T(m_right_rear_cloud[i].y) - cy));
+            //减去经过车辆旋转计算的左前中心
+            const Eigen::Matrix<T, 2, 1> tanslate_point = rotation_matrix * point - wheel_center_normal_right_rear;
+
+            interpolator.Evaluate(tanslate_point(1, 0) * m_scale + rows / 2.0 + 0.5 + T(kPadding),
+                                  tanslate_point(0, 0) * m_scale + cols / 2.0 + 0.5 + T(kPadding),
+                                  &residual[left_front_num + right_front_num + left_rear_num + i]);
+
+        }
+
+        return true;
+    }
+
+private:
+
+};
+
+detect_wheel_ceres::detect_wheel_ceres()
+{
+    /////创建地图
+    int cols=800;
+    int rows=200;
+    int L=std::min(rows,cols);
+    cv::Mat map=cv::Mat::ones(L,L,CV_64F);
+    //map=map*255;
+    cv::Point center(L/2,L/2);
+
+
+    float K=L*0.08;
+    for(int n=0;n<L;n+=2)
+    {
+        cv::Size size(n+2,n+2);
+        cv::Rect rect(center-cv::Point(n/2,n/2),size);
+        if(n<K*2)
+            cv::rectangle(map,rect,3.0*float(K*2-n)/float(L));
+        else
+            cv::rectangle(map,rect,float(n-K*2)/float(L));
+
+    }
+    cv::resize(map,map,cv::Size(cols,rows));
+    cv::GaussianBlur(map,m_map,cv::Size(7,7),3,3);
+}
+detect_wheel_ceres::~detect_wheel_ceres(){}
+
+
+
+bool detect_wheel_ceres::detect(std::vector<pcl::PointCloud<pcl::PointXYZ>> cloud_vec,
+                                double& cx,double& cy,double& theta,double& wheel_base,double& width,double& front_theta)
+{
+    //清理点云
+    m_left_front_cloud.clear();
+    m_right_front_cloud.clear();
+    m_left_rear_cloud.clear();
+    m_right_rear_cloud.clear();
+    //重新计算点云,按方位分割
+    //第一步,计算整体中心,主轴方向
+    pcl::PointCloud<pcl::PointXYZ> cloud_all;
+    for(int i=0;i<cloud_vec.size();++i)
+    {
+        cloud_all+=cloud_vec[i];
+    }
+
+    if(cloud_all.size()<20)
+        return false;
+
+    Eigen::Vector4f centroid;
+    pcl::compute3DCentroid(cloud_all, centroid);
+    double center_x=centroid[0];
+    double center_y=centroid[1];
+    //计算外接旋转矩形
+    std::vector<cv::Point2f> points_cv;
+    for(int i=0;i<cloud_all.size();++i)
+    {
+        points_cv.push_back(cv::Point2f(cloud_all[i].x,cloud_all[i].y));
+    }
+    cv::RotatedRect rotate_rect=cv::minAreaRect(points_cv);
+    //计算旋转矩形与X轴的夹角
+    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);
+    // 寻找长边，倾角为长边与x轴夹角
+    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 / M_PI * acos(vec.x / sqrt(vec.x * vec.x + vec.y * vec.y));
+    //printf("rect theta: %.3f\n",angle_x);
+    //第二步, 将没分点云旋转回去,计算点云重心所在象限
+    for(int i=0;i<cloud_vec.size();++i)
+    {
+        pcl::PointCloud<pcl::PointXYZ> cloud=cloud_vec[i];
+        Eigen::Affine3f traslation = Eigen::Affine3f::Identity();//初始化变换矩阵为单位矩阵
+        // 平移
+        traslation.translation() << -center_x, -center_y, 0.0;
+        pcl::PointCloud<pcl::PointXYZ> translate_cloud;
+        pcl::transformPointCloud(cloud, translate_cloud, traslation);
+
+        // 旋转; Z 轴上旋转angle_x 弧度,Y轴上旋转0弧度
+        Eigen::Affine3f rotation = Eigen::Affine3f::Identity();
+        rotation.rotate(Eigen::AngleAxisf(-angle_x*M_PI/180.0, Eigen::Vector3f::UnitZ()));
+
+        pcl::PointCloud<pcl::PointXYZ> transformed_cloud;
+        pcl::transformPointCloud(translate_cloud, transformed_cloud, rotation);
+
+        //计算重心
+        Eigen::Vector4f centroid;
+        pcl::compute3DCentroid(transformed_cloud, centroid);
+        double x=centroid[0];
+        double y=centroid[1];
+        //计算象限
+        if(x>0&&y>0)
+        {
+            //第一象限
+            m_left_front_cloud=cloud;
+        }
+        if(x>0 && y<0)
+        {
+            //第四象限
+            m_right_front_cloud=cloud;
+        }
+        if(x<0 && y>0)
+        {
+            //第二象限
+            m_left_rear_cloud=cloud;
+        }
+        if(x<0 && y<0)
+        {
+            //第三象限
+            m_right_rear_cloud=cloud;
+
+        }
+
+    }
+    cx=center_x;
+    cy=center_y;
+    theta=-angle_x*M_PI/180.0;
+    return Solve(cx,cy,theta,wheel_base,width,front_theta);
+}
+
+bool detect_wheel_ceres::Solve(double& x,double& y,double& theta,double& wheel_base,double& width,double& front_theta)
+{
+    double SCALE=300.0;
+    double cx=x;
+    double cy=y;
+    double init_theta=theta;
+    double init_wheel_base=2.7;
+    double init_width=1.55;
+    double init_theta_front=0*M_PI/180.0;
+
+    double variable[] = {cx,cy,init_theta,init_wheel_base,init_width,init_theta_front};
+
+    // 第二部分：构建寻优问题
+    ceres::Problem problem;
+    //使用自动求导，将之前的代价函数结构体传入，第一个1是输出维度，即残差的维度，第二个1是输入维度，即待寻优参数x的维度。
+    ceres::CostFunction* cost_function =new
+            ceres::AutoDiffCostFunction<CostFunctor, ceres::DYNAMIC, 6>(
+                    new CostFunctor(m_left_front_cloud,m_right_front_cloud,m_left_rear_cloud,m_right_rear_cloud,m_map,SCALE),
+                    m_left_front_cloud.size()+m_right_front_cloud.size()+m_left_rear_cloud.size()+m_right_rear_cloud.size());
+    problem.AddResidualBlock(cost_function, NULL, variable); //向问题中添加误差项，本问题比较简单，添加一个就行。
+
+
+    //第三部分： 配置并运行求解器
+    ceres::Solver::Options options;
+    options.use_nonmonotonic_steps=false;
+    options.linear_solver_type = ceres::DENSE_QR; //配置增量方程的解法
+    options.max_num_iterations=20;
+    options.num_threads=1;
+    options.minimizer_progress_to_stdout = false;//输出到cout
+    ceres::Solver::Summary summary;//优化信息
+    ceres::Solve(options, &problem, &summary);//求解!!!
+
+    //std::cout << summary.BriefReport() << "\n";//输出优化的简要信息
+
+    /*printf("x:%.3f,y:%.3f,theta:%.3f\nlength:%.3f,width:%.3f\ntheta_front:%.3f\n",
+           x[0],x[1],x[2]*180.0/M_PI,x[3],x[4],x[5]*180.0/M_PI);*/
+
+    double loss=summary.final_cost/(m_left_front_cloud.size()+m_right_front_cloud.size()+m_left_rear_cloud.size()+m_right_rear_cloud.size());
+
+
+    x=variable[0];
+    y=variable[1];
+    theta=(-variable[2])*180.0/M_PI;
+    wheel_base=variable[3];
+    width=variable[4];
+    front_theta=-(variable[5]*180.0/M_PI);
+
+    if(theta>180.0)
+        theta=theta-180.0;
+    if(theta<0)
+        theta+=180.0;
+
+    //检验
+    if(loss>0.03)
+        return false;
+    if (width < 1.350 || width > 2.000 || wheel_base > 3.000 || wheel_base < 2.200)
+    {
+        return false;
+    }
+
+    width+=0.15;//车宽+10cm
+    //printf(" x:%.3f  y: %.3f  wheel:%.3f  width:%.3f    theta : %.3f   front theta: %.3f\n",x,y,wheel_base,width,theta,front_theta);
+    return true;
+
+}

wj_lidar/detect_wheel_ceres.h

@@ -0,0 +1,36 @@
+//
+// Created by zx on 2020/7/1.
+//
+
+#ifndef CERES_TEST_DETECT_WHEEL_CERES_H
+#define CERES_TEST_DETECT_WHEEL_CERES_H
+
+#include <ceres/ceres.h>
+#include <glog/logging.h>
+#include <pcl/point_types.h>
+#include <pcl/point_cloud.h>
+#include <pcl/common/centroid.h>
+#include <opencv2/opencv.hpp>
+
+class detect_wheel_ceres {
+public:
+    detect_wheel_ceres();
+    ~detect_wheel_ceres();
+    bool detect(std::vector<pcl::PointCloud<pcl::PointXYZ>> cloud_vec,
+                double& cx,double& cy,double& theta,double& wheel_base,double& width,double& front_theta);
+
+protected:
+    bool Solve(double& x,double& y,double& theta,double& wheel_base,double& width,double& front_theta);
+
+private:
+    pcl::PointCloud<pcl::PointXYZ>          m_left_front_cloud;     //左前点云
+    pcl::PointCloud<pcl::PointXYZ>          m_right_front_cloud;    //右前点云
+    pcl::PointCloud<pcl::PointXYZ>          m_left_rear_cloud;      //左后点云
+    pcl::PointCloud<pcl::PointXYZ>          m_right_rear_cloud;     //右后点云
+
+public:
+    cv::Mat                                 m_map;                  //保存一份地图用作匹配
+};
+
+
+#endif //CERES_TEST_DETECT_WHEEL_CERES_H

wj_lidar/fence_controller.cpp

@@ -314,7 +314,7 @@ Error_manager Fence_controller::execute_task(Task_Base* task)
                 bool correctness = false;
 
                 LOG(INFO) << "--------callback find terminal------" << terminal_id;
-                double x = 0, y = 0, c = 0, wheelbase = 0, width = 0;
+                double x = 0, y = 0, c = 0, wheelbase = 0, width = 0,front_theta=0;
                 // 获取最新点云并保存到total_cloud中
 
                 pcl::PointCloud<pcl::PointXYZ>::Ptr total_cloud(new pcl::PointCloud<pcl::PointXYZ>);
@@ -330,7 +330,7 @@ Error_manager Fence_controller::execute_task(Task_Base* task)
                 LOG(INFO) << "--------callback get cloud, size: " << total_cloud->size();
 
                 // 获取检测结果
-                Error_manager code = m_region_workers_vector[i]->get_wheel_result(total_cloud, x, y, c, wheelbase, width);
+                Error_manager code = m_region_workers_vector[i]->get_wheel_result(total_cloud, x, y, c, wheelbase, width,front_theta);
                 time_t tt = time(0);
                 struct tm *tc = localtime(&tt);
                 char buf[255] = {0};
@@ -464,6 +464,10 @@ void Fence_controller::wheel_msg_recv_thread(Fence_controller *fc)
                 fc->m_msg_queue.push(task);
                 LOG(INFO) << "-------recv msg " << task.cmd.c_str() << "-------";
             }
+		if(c1 != nullptr)
+		{
+			delete c1;
+		}
         }
     }
     LOG(INFO) << "任务消息接收线程退出";
@@ -531,7 +535,7 @@ void Fence_controller::wheel_msg_handling_thread(Fence_controller *fc)
                         if (terminal_id >= 0 && fc->m_region_workers_vector[i]->get_id() == terminal_id)
                         {
                             LOG(INFO) << "--------callback find terminal------" << terminal_id;
-                            double x = 0, y = 0, c = 0, wheelbase = 0, width = 0;
+                            double x = 0, y = 0, c = 0, wheelbase = 0, width = 0,front_theta=0;
                             // 获取最新点云并保存到total_cloud中
 //                            fc->m_cloud_mutex.lock();
                             pcl::PointCloud<pcl::PointXYZ>::Ptr total_cloud(new pcl::PointCloud<pcl::PointXYZ>);
@@ -551,7 +555,7 @@ void Fence_controller::wheel_msg_handling_thread(Fence_controller *fc)
                             char whole_filename[255];
                             memset(whole_filename, 0, 255);
                             // 获取检测结果
-                            Error_manager code = fc->m_region_workers_vector[i]->get_wheel_result(total_cloud, x, y, c, wheelbase, width);
+                            Error_manager code = fc->m_region_workers_vector[i]->get_wheel_result(total_cloud, x, y, c, wheelbase, width,front_theta);
                             // 根据是否成功生成对应文件名
                             if (code == SUCCESS)
                             {
@@ -581,6 +585,7 @@ void Fence_controller::wheel_msg_handling_thread(Fence_controller *fc)
                             result.set_c(c);
                             result.set_wheel_base(wheelbase * 1000.0);
                             result.set_width(width * 1000.0);
+                            result.set_front_theta(front_theta*180.0/M_PI);
                             if (result.correctness())
                                 break;
                         }
@@ -602,4 +607,4 @@ void Fence_controller::wheel_msg_handling_thread(Fence_controller *fc)
             delete task.sock_controller;
         }
     }
-}
+}

wj_lidar/globalmsg.pb.cc

@@ -241,10 +241,11 @@ const ::google::protobuf::uint32 TableStruct::offsets[] GOOGLE_PROTOBUF_ATTRIBUT
   GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(::globalmsg::resultInfo, width_),
   GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(::globalmsg::resultInfo, height_),
   GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(::globalmsg::resultInfo, error_),
+  GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(::globalmsg::resultInfo, front_theta_),
   0,
   2,
   3,
-  11,
+  12,
   4,
   5,
   6,
@@ -253,6 +254,7 @@ const ::google::protobuf::uint32 TableStruct::offsets[] GOOGLE_PROTOBUF_ATTRIBUT
   9,
   10,
   1,
+  11,
   GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(::globalmsg::algMsg, _has_bits_),
   GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(::globalmsg::algMsg, _internal_metadata_),
   ~0u,  // no _extensions_
@@ -278,9 +280,9 @@ static const ::google::protobuf::internal::MigrationSchema schemas[] GOOGLE_PROT
   { 0, 10, sizeof(::globalmsg::msg)},
   { 15, 24, sizeof(::globalmsg::laserMsg)},
   { 28, 35, sizeof(::globalmsg::plcMsg)},
-  { 37, 54, sizeof(::globalmsg::resultInfo)},
-  { 66, 74, sizeof(::globalmsg::algMsg)},
-  { 77, 84, sizeof(::globalmsg::SysMsg)},
+  { 37, 55, sizeof(::globalmsg::resultInfo)},
+  { 68, 76, sizeof(::globalmsg::algMsg)},
+  { 79, 86, sizeof(::globalmsg::SysMsg)},
 };
 
 static ::google::protobuf::Message const * const file_default_instances[] = {
@@ -324,27 +326,27 @@ void AddDescriptorsImpl() {
       "\020queue_data_count\030\002 \001(\005\022\023\n\013cloud_count\030\003"
       " \001(\005\022\n\n\002id\030\004 \002(\005\"F\n\006plcMsg\022(\n\nplc_status"
       "\030\001 \001(\0162\024.globalmsg.plcStatus\022\022\n\nplc_valu"
-      "es\030\002 \003(\005\"\320\001\n\nresultInfo\022\016\n\004time\030\001 \002(\t:\000\022"
+      "es\030\002 \003(\005\"\345\001\n\nresultInfo\022\016\n\004time\030\001 \002(\t:\000\022"
       "\023\n\013correctness\030\002 \002(\010\022\025\n\rpark_space_id\030\003 "
       "\001(\005\022\021\n\tlaser_ids\030\004 \001(\005\022\t\n\001x\030\005 \001(\001\022\t\n\001y\030\006"
       " \001(\001\022\t\n\001c\030\007 \001(\001\022\022\n\nwheel_base\030\010 \001(\001\022\016\n\006l"
       "ength\030\t \001(\001\022\r\n\005width\030\n \001(\001\022\016\n\006height\030\013 \001"
-      "(\001\022\017\n\005error\030\014 \001(\t:\000\"^\n\006algMsg\022%\n\006result\030"
-      "\001 \003(\0132\025.globalmsg.resultInfo\022\031\n\021thread_q"
-      "ueue_size\030\002 \001(\005\022\022\n\010log_path\030\003 \001(\t:\000\"E\n\006S"
-      "ysMsg\022\r\n\003log\030\001 \002(\t:\000\022,\n\005level\030\002 \001(\0162\023.gl"
-      "obalmsg.logLevel:\010eSysInfo*0\n\004type\022\010\n\004eP"
-      "LC\020\000\022\n\n\006eLaser\020\001\022\010\n\004eAlg\020\002\022\010\n\004eLog\020\003*]\n\013"
-      "laserStatus\022\023\n\017eLaserConnected\020\000\022\026\n\022eLas"
-      "erDisconnected\020\001\022\016\n\neLaserBusy\020\002\022\021\n\reLas"
-      "erUnknown\020\003*V\n\tplcStatus\022\021\n\rePLCConnecte"
-      "d\020\000\022\024\n\020ePLCDisconnected\020\001\022\017\n\013ePLCRefused"
-      "\020\002\022\017\n\013ePLCUnknown\020\003*E\n\010logLevel\022\014\n\010eSysI"
-      "nfo\020\000\022\013\n\007eSysLog\020\001\022\017\n\013eSysWarning\020\002\022\r\n\te"
-      "SysError\020\003"
+      "(\001\022\017\n\005error\030\014 \001(\t:\000\022\023\n\013front_theta\030\r \001(\001"
+      "\"^\n\006algMsg\022%\n\006result\030\001 \003(\0132\025.globalmsg.r"
+      "esultInfo\022\031\n\021thread_queue_size\030\002 \001(\005\022\022\n\010"
+      "log_path\030\003 \001(\t:\000\"E\n\006SysMsg\022\r\n\003log\030\001 \002(\t:"
+      "\000\022,\n\005level\030\002 \001(\0162\023.globalmsg.logLevel:\010e"
+      "SysInfo*0\n\004type\022\010\n\004ePLC\020\000\022\n\n\006eLaser\020\001\022\010\n"
+      "\004eAlg\020\002\022\010\n\004eLog\020\003*]\n\013laserStatus\022\023\n\017eLas"
+      "erConnected\020\000\022\026\n\022eLaserDisconnected\020\001\022\016\n"
+      "\neLaserBusy\020\002\022\021\n\reLaserUnknown\020\003*V\n\tplcS"
+      "tatus\022\021\n\rePLCConnected\020\000\022\024\n\020ePLCDisconne"
+      "cted\020\001\022\017\n\013ePLCRefused\020\002\022\017\n\013ePLCUnknown\020\003"
+      "*E\n\010logLevel\022\014\n\010eSysInfo\020\000\022\013\n\007eSysLog\020\001\022"
+      "\017\n\013eSysWarning\020\002\022\r\n\teSysError\020\003"
   };
   ::google::protobuf::DescriptorPool::InternalAddGeneratedFile(
-      descriptor, 1090);
+      descriptor, 1111);
   ::google::protobuf::MessageFactory::InternalRegisterGeneratedFile(
     "globalmsg.proto", &protobuf_RegisterTypes);
 }
@@ -1575,6 +1577,7 @@ const int resultInfo::kLengthFieldNumber;
 const int resultInfo::kWidthFieldNumber;
 const int resultInfo::kHeightFieldNumber;
 const int resultInfo::kErrorFieldNumber;
+const int resultInfo::kFrontThetaFieldNumber;
 #endif  // !defined(_MSC_VER) || _MSC_VER >= 1900
 
 resultInfo::resultInfo()
@@ -1669,7 +1672,7 @@ void resultInfo::Clear() {
         reinterpret_cast<char*>(&wheel_base_) -
         reinterpret_cast<char*>(&correctness_)) + sizeof(wheel_base_));
   }
-  if (cached_has_bits & 3840u) {
+  if (cached_has_bits & 7936u) {
     ::memset(&length_, 0, static_cast<size_t>(
         reinterpret_cast<char*>(&laser_ids_) -
         reinterpret_cast<char*>(&length_)) + sizeof(laser_ids_));
@@ -1860,6 +1863,20 @@ bool resultInfo::MergePartialFromCodedStream(
         break;
       }
 
+      // optional double front_theta = 13;
+      case 13: {
+        if (static_cast< ::google::protobuf::uint8>(tag) ==
+            static_cast< ::google::protobuf::uint8>(105u /* 105 & 0xFF */)) {
+          set_has_front_theta();
+          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
+                   double, ::google::protobuf::internal::WireFormatLite::TYPE_DOUBLE>(
+                 input, &front_theta_)));
+        } else {
+          goto handle_unusual;
+        }
+        break;
+      }
+
       default: {
       handle_unusual:
         if (tag == 0) {
@@ -1908,7 +1925,7 @@ void resultInfo::SerializeWithCachedSizes(
   }
 
   // optional int32 laser_ids = 4;
-  if (cached_has_bits & 0x00000800u) {
+  if (cached_has_bits & 0x00001000u) {
     ::google::protobuf::internal::WireFormatLite::WriteInt32(4, this->laser_ids(), output);
   }
 
@@ -1957,6 +1974,11 @@ void resultInfo::SerializeWithCachedSizes(
       12, this->error(), output);
   }
 
+  // optional double front_theta = 13;
+  if (cached_has_bits & 0x00000800u) {
+    ::google::protobuf::internal::WireFormatLite::WriteDouble(13, this->front_theta(), output);
+  }
+
   if (_internal_metadata_.have_unknown_fields()) {
     ::google::protobuf::internal::WireFormat::SerializeUnknownFields(
         _internal_metadata_.unknown_fields(), output);
@@ -1994,7 +2016,7 @@ void resultInfo::SerializeWithCachedSizes(
   }
 
   // optional int32 laser_ids = 4;
-  if (cached_has_bits & 0x00000800u) {
+  if (cached_has_bits & 0x00001000u) {
     target = ::google::protobuf::internal::WireFormatLite::WriteInt32ToArray(4, this->laser_ids(), target);
   }
 
@@ -2044,6 +2066,11 @@ void resultInfo::SerializeWithCachedSizes(
         12, this->error(), target);
   }
 
+  // optional double front_theta = 13;
+  if (cached_has_bits & 0x00000800u) {
+    target = ::google::protobuf::internal::WireFormatLite::WriteDoubleToArray(13, this->front_theta(), target);
+  }
+
   if (_internal_metadata_.have_unknown_fields()) {
     target = ::google::protobuf::internal::WireFormat::SerializeUnknownFieldsToArray(
         _internal_metadata_.unknown_fields(), target);
@@ -2127,7 +2154,7 @@ size_t resultInfo::ByteSizeLong() const {
     }
 
   }
-  if (_has_bits_[8 / 32] & 3840u) {
+  if (_has_bits_[8 / 32] & 7936u) {
     // optional double length = 9;
     if (has_length()) {
       total_size += 1 + 8;
@@ -2143,6 +2170,11 @@ size_t resultInfo::ByteSizeLong() const {
       total_size += 1 + 8;
     }
 
+    // optional double front_theta = 13;
+    if (has_front_theta()) {
+      total_size += 1 + 8;
+    }
+
     // optional int32 laser_ids = 4;
     if (has_laser_ids()) {
       total_size += 1 +
@@ -2210,7 +2242,7 @@ void resultInfo::MergeFrom(const resultInfo& from) {
     }
     _has_bits_[0] |= cached_has_bits;
   }
-  if (cached_has_bits & 3840u) {
+  if (cached_has_bits & 7936u) {
     if (cached_has_bits & 0x00000100u) {
       length_ = from.length_;
     }
@@ -2221,6 +2253,9 @@ void resultInfo::MergeFrom(const resultInfo& from) {
       height_ = from.height_;
     }
     if (cached_has_bits & 0x00000800u) {
+      front_theta_ = from.front_theta_;
+    }
+    if (cached_has_bits & 0x00001000u) {
       laser_ids_ = from.laser_ids_;
     }
     _has_bits_[0] |= cached_has_bits;
@@ -2263,6 +2298,7 @@ void resultInfo::InternalSwap(resultInfo* other) {
   swap(length_, other->length_);
   swap(width_, other->width_);
   swap(height_, other->height_);
+  swap(front_theta_, other->front_theta_);
   swap(laser_ids_, other->laser_ids_);
   swap(_has_bits_[0], other->_has_bits_[0]);
   _internal_metadata_.Swap(&other->_internal_metadata_);

wj_lidar/globalmsg.pb.h

@@ -775,6 +775,13 @@ class resultInfo : public ::google::protobuf::Message /* @@protoc_insertion_poin
   double height() const;
   void set_height(double value);
 
+  // optional double front_theta = 13;
+  bool has_front_theta() const;
+  void clear_front_theta();
+  static const int kFrontThetaFieldNumber = 13;
+  double front_theta() const;
+  void set_front_theta(double value);
+
   // optional int32 laser_ids = 4;
   bool has_laser_ids() const;
   void clear_laser_ids();
@@ -808,6 +815,8 @@ class resultInfo : public ::google::protobuf::Message /* @@protoc_insertion_poin
   void clear_has_height();
   void set_has_error();
   void clear_has_error();
+  void set_has_front_theta();
+  void clear_has_front_theta();
 
   // helper for ByteSizeLong()
   size_t RequiredFieldsByteSizeFallback() const;
@@ -826,6 +835,7 @@ class resultInfo : public ::google::protobuf::Message /* @@protoc_insertion_poin
   double length_;
   double width_;
   double height_;
+  double front_theta_;
   ::google::protobuf::int32 laser_ids_;
   friend struct ::protobuf_globalmsg_2eproto::TableStruct;
   friend void ::protobuf_globalmsg_2eproto::InitDefaultsresultInfoImpl();
@@ -1628,13 +1638,13 @@ inline void resultInfo::set_park_space_id(::google::protobuf::int32 value) {
 
 // optional int32 laser_ids = 4;
 inline bool resultInfo::has_laser_ids() const {
-  return (_has_bits_[0] & 0x00000800u) != 0;
+  return (_has_bits_[0] & 0x00001000u) != 0;
 }
 inline void resultInfo::set_has_laser_ids() {
-  _has_bits_[0] |= 0x00000800u;
+  _has_bits_[0] |= 0x00001000u;
 }
 inline void resultInfo::clear_has_laser_ids() {
-  _has_bits_[0] &= ~0x00000800u;
+  _has_bits_[0] &= ~0x00001000u;
 }
 inline void resultInfo::clear_laser_ids() {
   laser_ids_ = 0;
@@ -1881,6 +1891,30 @@ inline void resultInfo::set_allocated_error(::std::string* error) {
   // @@protoc_insertion_point(field_set_allocated:globalmsg.resultInfo.error)
 }
 
+// optional double front_theta = 13;
+inline bool resultInfo::has_front_theta() const {
+  return (_has_bits_[0] & 0x00000800u) != 0;
+}
+inline void resultInfo::set_has_front_theta() {
+  _has_bits_[0] |= 0x00000800u;
+}
+inline void resultInfo::clear_has_front_theta() {
+  _has_bits_[0] &= ~0x00000800u;
+}
+inline void resultInfo::clear_front_theta() {
+  front_theta_ = 0;
+  clear_has_front_theta();
+}
+inline double resultInfo::front_theta() const {
+  // @@protoc_insertion_point(field_get:globalmsg.resultInfo.front_theta)
+  return front_theta_;
+}
+inline void resultInfo::set_front_theta(double value) {
+  set_has_front_theta();
+  front_theta_ = value;
+  // @@protoc_insertion_point(field_set:globalmsg.resultInfo.front_theta)
+}
+
 // -------------------------------------------------------------------
 
 // algMsg

wj_lidar/globalmsg.proto

@@ -61,6 +61,7 @@ message resultInfo
 	optional double width=10;
 	optional double height=11;
 	optional string error=12 [default=""];
+	optional double front_theta=13;
 }
 message algMsg
 {

wj_lidar/plc_data.h

@@ -48,7 +48,7 @@ private:
     StdCondition m_cond_exit;                // 系统关闭标志
     std::thread *mp_update_thread;           // plc更新线程
     S7PLC m_plc;                             // S7协议句柄
-    const int m_update_interval_milli = 200; // plc更新频率
+    const int m_update_interval_milli = 100; // plc更新频率
 
     // 有参构造函数
     Plc_data(std::string ip);

wj_lidar/region_detect.cpp

@@ -236,8 +236,62 @@ Error_manager Region_detector::isRect(std::vector<cv::Point2f> &points, bool pri
     }
 }
 
+/*
+ * 仅仅聚类
+ */
+Error_manager Region_detector::clustering_only(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in,
+        std::vector<pcl::PointCloud<pcl::PointXYZ>> &seg_clouds, bool print)
+{
+    // 1.判断参数合法性
+    if (cloud_in->size() <= 0)
+        return Error_manager(Error_code::WJ_REGION_EMPTY_CLOUD);
 
+    // 2.聚类
+    pcl::search::KdTree<pcl::PointXYZ>::Ptr kdtree(new pcl::search::KdTree<pcl::PointXYZ>);
+    kdtree->setInputCloud(cloud_in);
 
+    pcl::EuclideanClusterExtraction<pcl::PointXYZ> clustering;
+    // 设置聚类的最小值 2cm (small values may cause objects to be divided
+    // in several clusters, whereas big values may join objects in a same cluster).
+    clustering.setClusterTolerance(0.2);
+    // 设置聚类的小点数和最大点云数
+    clustering.setMinClusterSize(10);
+    clustering.setMaxClusterSize(500);
+    clustering.setSearchMethod(kdtree);
+    clustering.setInputCloud(cloud_in);
+    std::vector<pcl::PointIndices> clusters;
+    clustering.extract(clusters);
+    if(clusters.size() <= 0)
+        return Error_manager(Error_code::WJ_REGION_EMPTY_CLOUD);
+
+    for (int i = 0; i < clusters.size(); ++i)
+    {
+        seg_clouds.push_back(pcl::PointCloud<pcl::PointXYZ>());
+    }
+
+    int j = 0;
+    for (std::vector<pcl::PointIndices>::const_iterator it = clusters.begin(); it != clusters.end(); ++it)
+    {
+        pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_cluster(new pcl::PointCloud<pcl::PointXYZ>);
+        //创建新的点云数据集cloud_cluster，将所有当前聚类写入到点云数据集中
+        for (std::vector<int>::const_iterator pit = it->indices.begin(); pit != it->indices.end(); ++pit)
+        {
+            cloud_cluster->points.push_back(cloud_in->points[*pit]);
+            cloud_cluster->width = cloud_cluster->points.size();
+            cloud_cluster->height = 1;
+            cloud_cluster->is_dense = true;
+        }
+        seg_clouds[j] = *cloud_cluster;
+        j++;
+    }
+
+    if (print)
+    {
+        LOG(INFO) << " cluster classes:" << clusters.size();
+    }
+
+    return SUCCESS;
+}
 
 
 /**
@@ -344,6 +398,31 @@ Error_manager Region_detector::detect(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_
 }
 
 
+/**
+ * ceres优化
+ * 优化变量:中心点、角度、轮距与宽度的四轮点云检测函数
+ * */
+Error_manager Region_detector::detect(pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud_in, double &x, double &y, double &c,
+        double &wheelbase, double &width, double& front_wheel_theta,bool print)
+{
+    Error_manager result = Error_manager(Error_code::SUCCESS);
+    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered(pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>));
+    // 1.预处理
+    result = preprocess(cloud_in, cloud_filtered);
+    if (result != SUCCESS)
+        return result;
+
+    // 2.聚类
+    std::vector<pcl::PointCloud<pcl::PointXYZ>> seg_clouds;
+    result = clustering_only(cloud_filtered, seg_clouds, print);
+    if (result != SUCCESS)
+        return result;
+
+    if(!m_detector_ceres.detect(seg_clouds,x,y,c,wheelbase,width,front_wheel_theta))
+        return WJ_REGION_CERES_SOLVE_ERROR;
+
+    return Error_manager(Error_code::SUCCESS);
+}
 
 
 /**

wj_lidar/region_detect.h

@@ -41,7 +41,7 @@ using google::protobuf::Message;
 
 #include "../tool/StdCondition.h"
 #include "opencv2/opencv.hpp"
-
+#include "detect_wheel_ceres.h"
 /**
  * 万集区域检测算法类
  * */
@@ -56,6 +56,9 @@ public:
     Error_manager detect(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in);
     // 检测传入点云，识别为四类返回中心点、角度、轮距与宽度
     Error_manager detect(pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud_in, double &x, double &y, double &c, double &wheelbase, double &width, bool print=true);
+    //通过ceres检测中心,旋转,轴距,宽度,前轮旋转
+    Error_manager detect(pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud_in, double &x, double &y, double &c,double& front_wheel_theta,
+            double &wheelbase, double &width, bool print=true);
     // 获得区域id
     int get_region_id();
 
@@ -63,13 +66,20 @@ private:
     // 预处理，直通滤波限制点云范围
     Error_manager preprocess(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in, pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_out);
     // 点云聚类，寻找四类点云并检验是否近似矩形
-    Error_manager clustering(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in, std::vector<cv::Point2f>& corner_points, std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> &seg_clouds, bool print=false);
+    Error_manager clustering(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in, std::vector<cv::Point2f>& corner_points,
+            std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> &seg_clouds, bool print=false);
+    //仅仅聚类
+    Error_manager clustering_only(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in,
+                             std::vector<pcl::PointCloud<pcl::PointXYZ>> &seg_clouds, bool print=false);
     // 判断是否足够近似矩形
     Error_manager isRect(std::vector<cv::Point2f>& points, bool print=false);
 
 private:
     wj::Region m_region_param;      // 区域范围参数
     std::atomic<int> m_region_id;   // 区域id
+
+    detect_wheel_ceres              m_detector_ceres;   //ceres优化对象
+
 };
 
 #endif //REGION_DETECT_H

wj_lidar/region_worker.cpp

@@ -80,6 +80,28 @@ Error_manager Region_worker::get_wheel_result(pcl::PointCloud<pcl::PointXYZ>::Pt
     return result;
 }
 
+
+/**
+ * ceres结果
+ * 获取轮距等测量结果,包括前轮旋转
+ * */
+Error_manager Region_worker::get_wheel_result(pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud_in, double &x, double &y, double &c,
+        double &wheelbase, double &width,double& front_theta)
+{
+    Error_manager result;
+    std::lock_guard<std::mutex> lck(m_mutex);
+    if (m_detector)
+    {
+        LOG(INFO) << "worker getting result";
+        result = m_detector->detect(cloud_in, x, y, c, wheelbase, width,front_theta);
+    }
+    else
+    {
+        result = Error_manager(Error_code::POINTER_IS_NULL);
+    }
+    return result;
+}
+
 /**
  * 外部调用更新区域检测用点云
  * */
@@ -122,8 +144,8 @@ void Region_worker::detect_loop(Region_worker *worker)
             int code = REGION_WORKER_RESULT_DEFAULT;
             int border_status = REGION_WORKER_RESULT_DEFAULT;
             worker->mb_cloud_updated = false;
-            double x,y,angle,wheelbase,width;
-            Error_manager result = worker->m_detector->detect(worker->m_cloud, x, y, angle, wheelbase, width, false);
+            double x,y,angle,wheelbase,width,front_theta;
+            Error_manager result = worker->m_detector->detect(worker->m_cloud, x, y, angle, wheelbase, width,front_theta, false);
 
             if(worker->mp_verify_handle == nullptr)
             {
@@ -178,7 +200,7 @@ void Region_worker::detect_loop(Region_worker *worker)
             if (p!=0 && duration > 500 && ((worker->m_last_sent_code != worker->m_last_read_code) || worker->m_last_border_status != border_status) && worker->m_read_code_count > 3)
             {
                 worker->m_last_sent_code = worker->m_last_read_code;
-                worker->m_last_border_status = border_status;
+		        worker->m_last_border_status = border_status;
                 p->update_data(code, border_status, worker->m_detector->get_region_id());
                 worker->m_update_plc_time = std::chrono::steady_clock::now();
             }
@@ -217,4 +239,4 @@ cv::RotatedRect Region_worker::create_rotate_rect(float length,float width,float
         point_vec.push_back(cv::Point2f(x,y));
     }
     return cv::minAreaRect(point_vec);
-}
+}

wj_lidar/region_worker.h

@@ -43,6 +43,11 @@ public:
     int get_id();
     // 获得中心点、角度等测量数据
     Error_manager get_wheel_result(pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud_in, double &x, double &y, double &c, double &wheelbase, double &width);
+
+    // 获得中心点、角度等测量数据,前轮旋转
+    Error_manager get_wheel_result(pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud_in, double &x, double &y, double &c,
+            double &wheelbase, double &width,double& front_theta);
+
 private:
     static cv::RotatedRect create_rotate_rect(float length,float width,float angle,float x,float y);
 private:
@@ -52,7 +57,7 @@ private:
     std::chrono::steady_clock::time_point m_update_plc_time; // 更新plc状态时刻
     int m_last_sent_code;                              // 上次写入plc的状态值
     int m_last_read_code;                              // 上次检查获得的状态值
-    int m_last_border_status;                           // 上次超界提示
+    int m_last_border_status;				// 上次超界提示
     std::atomic<int> m_read_code_count;                      // 检查后重复获取相同状态值次数
 
     StdCondition m_cond_exit;     // 系统退出标志