修改结果中车宽数据，增加body_width,计算真实车宽

CMakeLists.txt

@@ -28,8 +28,6 @@ include_directories(
 )
 
 aux_source_directory(${CMAKE_CURRENT_LIST_DIR}/src/tool TOOL_SRC )
-
-link_directories("/usr/local/lib")
 aux_source_directory(${CMAKE_CURRENT_LIST_DIR}/src/error_code ERROR_SRC )
 
 add_executable(vlp16_sample  src/vlp16_sample.cpp ${ERROR_SRC} ${TOOL_SRC}

build/sys.prototxt

@@ -0,0 +1,23 @@
+
+
+lidar
+{
+	ip:"192.168.2.201"
+	port:2367
+}
+
+lidar
+{
+	ip:"192.168.2.202"
+	port:2368
+}
+
+box
+{
+	minx:0
+	maxx:50
+	miny:-100
+	maxy:100
+	minz:-1
+	maxz:100
+}

src/detect_wheel_ceres.cpp

@@ -372,13 +372,6 @@ bool detect_wheel_ceres::detect(std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr>
 
     }
 
-    // // 仅优化一次，不调整图像比例与角度初值。
-    // detect_result.cx=center_x;
-    // detect_result.cy=center_y;
-    // detect_result.theta=-angle_x*M_PI/180.0;
-    // Loss_result one_shot_loss_result;
-    // return Solve(detect_result, one_shot_loss_result);
-
     // 多次优化，获取最佳优化结果
     detect_result.cx=center_x;
     detect_result.cy=center_y;
@@ -397,13 +390,7 @@ bool detect_wheel_ceres::detect(std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr>
     bool solve_result = false;
     double total_solve_time = 0;
     bool stop_sign = false;
-    // for (int j = 2; j < 4&&!stop_sign; j++)
-    // {
-    //     // double input_vars[] = {detect_result.cx, detect_result.cy, detect_result.theta, detect_result.wheel_base, detect_result.width, detect_result.front_theta};
-    //     double map_rows = map_cols * 1.0 / j ;
-    //     update_mat(map_rows, map_cols);
 
-    // !!! 将车轮比例添加到优化模型中
         Detect_result t_detect_result = detect_result;
        // 寻找最小loss值对应的初始旋转角
         for (int i = -3; i < 4&&!stop_sign; i+=6)
@@ -415,7 +402,6 @@ bool detect_wheel_ceres::detect(std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr>
             //输出角度已变化，需恢复成弧度
             if(calc_count > 0)
             {
-                // t_detect_result.theta *= (-M_PI) / 180.0;
                 t_detect_result.front_theta *= (-M_PI) / 180.0;
             }
 	    std::string t_error_info;
@@ -435,34 +421,37 @@ bool detect_wheel_ceres::detect(std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr>
                 loss_result = t_loss_result;
                 // optimized_map_rows = map_rows;
                 calc_count++;
-                // // 新增，优化时间足够长则认为已找到正确结果
-                // if(second > 0.02)
-                // {
-                //     stop_sign = true;
-                //     break;
-                // }
+
             }
         }
-    // }
-    // std::cout<<"solve time "<<total_solve_time<<std::endl;
-    // LOG(INFO) << "final avg cost for four wheels: "<<whole_loss[0]<<" "<<whole_loss[1]<<" "<<whole_loss[2]<<" "<<whole_loss[3]<<" ";
-
-//    if(solve_result)
-//        printf("final avg cost for four wheels: %7.6f %7.6f %7.6f %7.6f---%7.6f\n", loss_result.lf_loss, loss_result.rf_loss, loss_result.lb_loss, loss_result.rb_loss, loss_result.total_avg_loss);
-    
-    // // 生成并保存图片
-    // update_mat(optimized_map_rows, map_cols);
-    // Detect_result t_detect_result = detect_result;
-    // t_detect_result.theta *= (-M_PI) / 180.0;
-    // t_detect_result.front_theta *= (-M_PI) / 180.0;
-    // Loss_result t_loss_result;
-    // // LOG(INFO) <<"going to show img ";
-    // Solve(detect_result, t_loss_result, true);
-
-//     printf(" x:%.3f  y: %.3f  wheel:%.3f  width:%.3f    theta : %.3f   front theta: %.3f\n",
-//         detect_result.cx,detect_result.cy,detect_result.wheel_base,detect_result.width,detect_result.theta,detect_result.front_theta);
+
     if(solve_result)
-        error_info="";
+    {
+        error_info = "";
+        //按照车身角，旋转后轮， 计算车宽
+        pcl::PointCloud<pcl::PointXYZ>::Ptr rear_cloud(new pcl::PointCloud<pcl::PointXYZ>);
+        *rear_cloud+=m_left_rear_cloud;
+        *rear_cloud+=m_right_rear_cloud;
+        if(rear_cloud->size()>0)
+        {
+            double body_theta = detect_result.theta / 180.0 * M_PI;
+            Eigen::Vector2d vec(sin(body_theta), cos(body_theta));
+
+            double dot_min = 9999999., dot_max = -9999999.9;
+            for (int i = 0; i < rear_cloud->size(); ++i)
+            {
+                pcl::PointXYZ pt = rear_cloud->points[i];
+                double dot = pt.x * vec[0] + pt.y * vec[1];
+                if (dot > dot_max)
+                    dot_max = dot;
+                if (dot < dot_min)
+                    dot_min = dot;
+            }
+            detect_result.body_width=fabs(dot_max-dot_min);
+        }
+
+    }
+
     return solve_result;
 
 }
@@ -686,21 +675,18 @@ bool detect_wheel_ceres::Solve(Detect_result &detect_result, Loss_result &loss_r
         return false;
     }
 
-    detect_result.width += 0.22; //车宽一边+11cm
-//     printf(" x:%.3f  y: %.3f  wheel:%.3f  width:%.3f    theta : %.3f   front theta: %.3f\n",
-//             detect_result.cx,detect_result.cy,detect_result.wheel_base,detect_result.width,detect_result.theta,detect_result.front_theta);
+   // detect_result.width += 0.22; //车宽一边+11cm
 
     // //added by yct
     if(detect_result.theta > 120 || detect_result.theta < 60)
     {
-        error_info.append("总角度错误 ").append(std::to_string(detect_result.theta)).append("\t");
+        error_info.append("车身角度错误 ").append(std::to_string(detect_result.theta)).append("\t");
 //        LOG(WARNING) <<"总角度错误 "<<detect_result.theta;
         return false;
     }
     if(fabs(detect_result.front_theta)>35)
     {
         error_info.append("前轮角度错误 ").append(std::to_string(detect_result.front_theta)).append("\t");
-//        LOG(WARNING) <<"前轮角度错误 "<<detect_result.front_theta;
         return false;
     }
     // 将loss传出
@@ -732,8 +718,6 @@ bool detect_wheel_ceres::Solve(Detect_result &detect_result, Loss_result &loss_r
                     .append(std::to_string(loss_result.rf_loss)).append(", ")
                     .append(std::to_string(loss_result.lb_loss)).append(", ")
                     .append(std::to_string(loss_result.rb_loss)).append("\t");
-//            LOG(WARNING)<<error_info;
-//            LOG(WARNING) <<"四轮loss过大"<<loss_result.lf_loss<<", "<<loss_result.rf_loss<<", "<<loss_result.lb_loss<<", "<<loss_result.rb_loss;
             return false;
         }
         //std::cout<<"save img: "<<save_img<<std::endl;
@@ -747,7 +731,7 @@ bool detect_wheel_ceres::Solve(Detect_result &detect_result, Loss_result &loss_r
     Detect_result t_detect_result = detect_result;
     t_detect_result.theta *= -M_PI/180.0;
     t_detect_result.front_theta *= -M_PI/180.0;
-    t_detect_result.width -= 0.22;
+    //t_detect_result.width -= 0.22;
     transform_src(t_detect_result);
 
 

src/detect_wheel_ceres.h

@@ -26,7 +26,8 @@ public:
         double theta;
         double wheel_base;
         double width;
-        double front_theta; 
+        double body_width;
+        double front_theta;
         bool success; 
         Detect_result& operator=(const Detect_result& detect_result)
         {
@@ -37,6 +38,7 @@ public:
             this->width = detect_result.width;
             this->front_theta = detect_result.front_theta;
             this->success = detect_result.success;
+            this->body_width=detect_result.body_width;
             return *this;
         }
     };

src/ground_region.cpp

@@ -10,7 +10,7 @@
 #include <fcntl.h>
 
 //欧式聚类*******************************************************
-std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> segmentation(pcl::PointCloud<pcl::PointXYZ>::Ptr sor_cloud)
+std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> Ground_region::segmentation(pcl::PointCloud<pcl::PointXYZ>::Ptr sor_cloud)
 {
     std::vector<pcl::PointIndices> ece_inlier;
     pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>);
@@ -33,13 +33,13 @@ std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> segmentation(pcl::PointCloud<pc
     return segmentation_clouds;
 }
 
-double distance(cv::Point2f p1, cv::Point2f p2)
+double Ground_region::distance(cv::Point2f p1, cv::Point2f p2)
 {
     return sqrt(pow(p1.x - p2.x, 2.0) + pow(p1.y - p2.y, 2.0));
 }
 
 
-bool isRect(std::vector<cv::Point2f>& points)
+bool Ground_region::isRect(std::vector<cv::Point2f>& points)
 {
     if (points.size() == 4)
     {
@@ -285,7 +285,7 @@ Error_manager Ground_region::init(std::string configure_file)
 Error_manager Ground_region::init(ground_region::Configure configure)
 {
     //创建雷达
-    m_lidars.resize(configure.lidar_size());
+    /*m_lidars.resize(configure.lidar_size());
     for(int i=0;i<configure.lidar_size();++i)
     {
         const boost::asio::ip::address address = boost::asio::ip::address::from_string( configure.lidar(i).ip() );
@@ -296,7 +296,7 @@ Error_manager Ground_region::init(ground_region::Configure configure)
             sprintf(description,"lidar [%d] open failed  ip:%s ,port:%d",i,configure.lidar(i).ip().c_str(),port);
             return Error_manager(FAILED,NORMAL,description);
         }
-    }
+    }*/
     m_configure=configure;
     m_running_thread=new std::thread(std::bind(thread_measure_func,this));
     return SUCCESS;
@@ -382,6 +382,7 @@ Error_manager Ground_region::sync_capture(pcl::PointCloud<pcl::PointXYZ>::Ptr cl
 
 void Ground_region::thread_measure_func(Ground_region* p)
 {
+    return ;
     //保持 10 fps
     while(false==p->m_exit_condition.wait_for_millisecond(100))
     {
@@ -439,7 +440,6 @@ Error_manager Ground_region::detect(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud,
     while(fabs(center_z-last_center_z)>0.01)
     {
         center_z=(start_z+max_z)/2.0;
-        std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> seg_clouds;
         detect_wheel_ceres::Detect_result result;
         bool ret = classify_ceres_detect(cloud, center_z,result);
         if(ret)
@@ -465,6 +465,6 @@ Error_manager Ground_region::detect(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud,
         return Error_manager(FAILED,NORMAL,"nor car");
     }
     ///  to be
-    last_result=results[0];
+    last_result=results[results.size()-1];
     return SUCCESS;
 }

src/ground_region.h

@@ -32,6 +32,9 @@ class Ground_region
     Error_manager init(std::string configure_file);
     Error_manager init(ground_region::Configure configure);
 
+    Error_manager detect(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud,const ground_region::Box& box,
+                         detect_wheel_ceres::Detect_result& result);
+
  private:
     bool read_proto_param(std::string file, ::google::protobuf::Message& parameter);
     Error_manager sync_capture(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud,double timeout_second=1.0);
@@ -39,11 +42,14 @@ class Ground_region
 
     bool classify_ceres_detect(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud,double thresh_z,
                                detect_wheel_ceres::Detect_result& result);
-    Error_manager detect(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud,const ground_region::Box& box,
-            detect_wheel_ceres::Detect_result& result);
+
     static void thread_measure_func(Ground_region* p);
 
 
+
+    std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> segmentation(pcl::PointCloud<pcl::PointXYZ>::Ptr sor_cloud);
+    double distance(cv::Point2f p1, cv::Point2f p2);
+    bool isRect(std::vector<cv::Point2f>& points);
  private:
     std::vector<velodyne::VLP16Capture*>     m_lidars;
     ground_region::Configure                m_configure;