普爱现场版本,wj采用滤波结果，ceres模型微调，轮距采集比例微调

terminor/terminal_command_executor.cpp

@@ -501,8 +501,8 @@ Error_manager Terminor_Command_Executor::scanning_measuring() {
     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 = 0.1 * dj_locate_information.locate_wheel_base +
-                                              0.9 * wj_locate_information.locate_wheel_base;
+    m_measure_information.locate_wheel_base = 0.01 * dj_locate_information.locate_wheel_base +
+                                              0.99 * wj_locate_information.locate_wheel_base;
 //    m_measure_information.locate_wheel_base += 400;
     //m_measure_information.locate_wheel_base=m_measure_information.locate_wheel_base>3000?3000:m_measure_information.locate_wheel_base;
     m_measure_information.locate_length = dj_locate_information.locate_length;

wj_lidar/detect_wheel_ceres.cpp

@@ -78,10 +78,10 @@ public:
         T width = variable[4];
         T theta_front = variable[5];
         T theta_front_weight = T(0.8);
-        T wheel_length_img_ratio = variable[6];
-        T wheel_width_length_ratio = variable[7];
+//        T wheel_length_img_ratio = variable[6];
+        T wheel_width_length_ratio = variable[6];
         // [1/3, 5/3]
-        T limited_wheel_length_img_ratio = T(0.6) / (T(1) + ceres::exp(T(-wheel_length_img_ratio))) + T(0.9);
+        T limited_wheel_length_img_ratio = T(0.85);// = T(0.7) / (T(1) + ceres::exp(T(-wheel_length_img_ratio))) + T(0.85);
         // [0.25, 0.35]
         T limited_wheel_length_ratio = T(0.25) / (T(1) + ceres::exp(T(-wheel_width_length_ratio))) + T(0.25);
 
@@ -188,22 +188,29 @@ public:
 		//printf("dev: %.6f\n", costs[i]);
 	}
 
-        char buf[30];
-        memset(buf, 0, 30);
-        sprintf(buf, "%.12f", costs[0]);
-        m_costs_lf = std::stod(buf) / left_front_num;
+            char buf[30];
+            memset(buf, 0, 30);
+            sprintf(buf, "%.12f", costs[0]);
+            double t_costs_lf = std::stod(buf) / left_front_num;
 
-        memset(buf, 0, 30);
-        sprintf(buf, "%.12f", costs[1]);
-        m_costs_rf = std::stod(buf) / right_front_num;
+            memset(buf, 0, 30);
+            sprintf(buf, "%.12f", costs[1]);
+            double t_costs_rf = std::stod(buf) / right_front_num;
 
-        memset(buf, 0, 30);
-        sprintf(buf, "%.12f", costs[2]);
-        m_costs_lr = std::stod(buf) / left_rear_num;
+            memset(buf, 0, 30);
+            sprintf(buf, "%.12f", costs[2]);
+            double t_costs_lr = std::stod(buf) / left_rear_num;
 
-        memset(buf, 0, 30);
-        sprintf(buf, "%.12f", costs[3]);
-        m_costs_rr = std::stod(buf) / right_rear_num;
+            memset(buf, 0, 30);
+            sprintf(buf, "%.12f", costs[3]);
+            double t_costs_rr = std::stod(buf) / right_rear_num;
+            if(t_costs_lf >= 0.001 && t_costs_rf >= 0.001 && t_costs_lr >= 0.001 && t_costs_rr >= 0.001)
+            {
+                m_costs_lf=t_costs_lf;
+                m_costs_rf=t_costs_rf;
+                m_costs_lr=t_costs_lr;
+                m_costs_rr=t_costs_rr;
+            }
         // m_costs_lf = costs[0];
 	//printf("%.6f %.6f %.6f %.6f\n",m_costs_lf, m_costs_rf, m_costs_lr, m_costs_rr);
         return true;
@@ -620,18 +627,18 @@ bool detect_wheel_ceres::Solve(Detect_result &detect_result, Loss_result &loss_r
     double init_width=1.55;
     double init_theta_front=0*M_PI/180.0;
     // 车轮长图比, 比例取值1/3-5/3, 当前变量无限制
-    double init_wheel_length_img_ratio = 0.5;
+//    double init_wheel_length_img_ratio = 0.1;
     // 车轮宽长比, 默认长为1,比例取值范围0.2-0.8, 当前变量范围无限制
-    double init_wheel_width_length_ratio = 0.5;
+    double init_wheel_width_length_ratio = -0.5;
 
-    double variable[] = {cx, cy, init_theta, init_wheel_base, init_width, init_theta_front, init_wheel_length_img_ratio, init_wheel_width_length_ratio};
+    double variable[] = {cx, cy, init_theta, init_wheel_base, init_width, init_theta_front, init_wheel_width_length_ratio};
     // printf("solve x:%.3f  y: %.3f  wheel:%.3f  width:%.3f    theta : %.3f   front theta: %.3f\n", variable[0], variable[1], variable[3], variable[4], variable[2], variable[5]);
     // 第二部分：构建寻优问题
     ceres::Problem problem;
     CostFunctor *cost_func = new CostFunctor(m_left_front_cloud,m_right_front_cloud,m_left_rear_cloud,m_right_rear_cloud,m_map,SCALE);
     //使用自动求导，将之前的代价函数结构体传入，第一个1是输出维度，即残差的维度，第二个1是输入维度，即待寻优参数x的维度。
     ceres::CostFunction* cost_function =new
-            ceres::AutoDiffCostFunction<CostFunctor, ceres::DYNAMIC, 8>(
+            ceres::AutoDiffCostFunction<CostFunctor, ceres::DYNAMIC, 7>(
                     cost_func,
                     m_left_front_cloud.size()+m_right_front_cloud.size()+m_left_rear_cloud.size()+m_right_rear_cloud.size()+2);
     problem.AddResidualBlock(cost_function, NULL, variable); //向问题中添加误差项，本问题比较简单，添加一个就行。
@@ -672,7 +679,7 @@ bool detect_wheel_ceres::Solve(Detect_result &detect_result, Loss_result &loss_r
     //检验
     // printf("loss: %.5f\n", loss);
     // printf("middle x:%.3f  y: %.3f  wheel:%.3f  width:%.3f    theta : %.3f   front theta: %.3f\n", variable[0], variable[1], variable[3], variable[4], variable[2], variable[5]);
-    if(loss>0.005)
+    if(loss>0.0115)
     {
         error_info.append("总loss过大 ").append(std::to_string(loss)).append("\t");
 //        LOG(WARNING) <<"总loss过大 "<<loss;
@@ -724,7 +731,7 @@ bool detect_wheel_ceres::Solve(Detect_result &detect_result, Loss_result &loss_r
             loss_result.rb_loss = loss;
         }
         // 判断每个轮子平均loss是否满足条件
-        double single_wheel_loss_threshold = 0.5;
+        double single_wheel_loss_threshold = 0.135;
         if(loss_result.lf_loss > single_wheel_loss_threshold || loss_result.rf_loss > single_wheel_loss_threshold  || loss_result.lb_loss > single_wheel_loss_threshold || loss_result.rb_loss > single_wheel_loss_threshold)
         {
             error_info.append("四轮loss过大 ").append(std::to_string(loss_result.lf_loss)).append(", ")

wj_lidar/region_worker.cpp

@@ -95,10 +95,10 @@ Error_manager Region_worker::get_wheel_result(pcl::PointCloud<pcl::PointXYZ>::Pt
     {
         LOG(INFO) << "worker getting result";
 		result = m_detector->detect(cloud_in, x, y, c, wheelbase, width, front_theta);
-		// 增加滤波后结果，暂时只打印对比结果
+		// 增加滤波后结果，赋值传出
         Common_data::Car_wheel_information t_wheel_info;
-        Error_manager ec = Measure_filter::get_instance_references().get_filtered_wheel_information(get_id(), t_wheel_info);
-        if(ec ==SUCCESS) {
+        result = Measure_filter::get_instance_references().get_filtered_wheel_information(get_id(), t_wheel_info);
+        if(result ==SUCCESS) {
             char buf[512] = {0};
             sprintf(buf,
                     "\nx %.3f->%.3f\n"
@@ -110,9 +110,15 @@ Error_manager Region_worker::get_wheel_result(pcl::PointCloud<pcl::PointXYZ>::Pt
                     x, t_wheel_info.center_x, y, t_wheel_info.center_y, c, t_wheel_info.car_angle, wheelbase,
                     t_wheel_info.wheel_base, width, t_wheel_info.wheel_width, front_theta, t_wheel_info.front_theta
             );
-            LOG(WARNING) << buf;
+            LOG(INFO) << buf;
+            x = t_wheel_info.center_x;
+            y = t_wheel_info.center_y;
+            c = t_wheel_info.car_angle;
+            wheelbase = t_wheel_info.wheel_base;
+            width = t_wheel_info.wheel_width;
+            front_theta = t_wheel_info.front_theta;
         }else{
-            LOG(WARNING) << ec.to_string();
+            LOG(WARNING) << result.to_string();
         }
     }
     else