wj wheel algorithm optimized. add result and loss struct, interface slightly changed.

src/findwheel/src/detect_wheel_ceres.cpp

@@ -234,8 +234,7 @@ 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)
+bool detect_wheel_ceres::detect(std::vector<pcl::PointCloud<pcl::PointXYZ>> cloud_vec, Detect_result &detect_result)
 {
     //清理点云
     m_left_front_cloud.clear();
@@ -250,6 +249,8 @@ bool detect_wheel_ceres::detect(std::vector<pcl::PointCloud<pcl::PointXYZ>> clou
         cloud_all+=cloud_vec[i];
     }
 
+    // std::cout<<"cloud size: "<<cloud_all.size()<<std::endl;
+
     if(cloud_all.size()<20)
         return false;
 
@@ -283,7 +284,7 @@ bool detect_wheel_ceres::detect(std::vector<pcl::PointCloud<pcl::PointXYZ>> clou
         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);
+    // printf("rect theta: %.3f\n",angle_x);
     //第二步, 将没分点云旋转回去,计算点云重心所在象限
     for(int i=0;i<cloud_vec.size();++i)
     {
@@ -330,15 +331,22 @@ bool detect_wheel_ceres::detect(std::vector<pcl::PointCloud<pcl::PointXYZ>> clou
         }
 
     }
-    // printf("origin: x:%.3f,y:%.3f,theta:%.3f\nlength:%.3f,width:%.3f\ntheta_front:%.3f\n",
-    //        center_x, center_y, angle_x, wheel_base, width, front_theta * 180.0 / M_PI);
-    cx=center_x;
-    cy=center_y;
+
+    // // 仅优化一次，不调整图像比例与角度初值。
+    // 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;
     // 已计算次数，初始传入值正常，之后solve函数会修改初始值，需要恢复角度单位为弧度
     int calc_count=0;
     // double final_theta = 0;
-    // 误差数组，保存左前、右前、左后、右后、整体平均误差
-    double whole_loss[5];
+    // 误差结构体，保存左前、右前、左后、右后、整体平均误差
+    Loss_result loss_result;
     // 平均误差值，用于获取最小整体平均误差
     double avg_loss = 100;
     // 定义图像列数，控制图像大小
@@ -347,251 +355,99 @@ bool detect_wheel_ceres::detect(std::vector<pcl::PointCloud<pcl::PointXYZ>> clou
     int optimized_map_rows = 200;
     // 优化结果
     bool solve_result = false;
-    for (int j = 4; j < 10; j++)
+    double total_solve_time = 0;
+    for (int j = 2; j < 4; j++)
     {
-        double input_vars[] = {cx, cy, theta, wheel_base, width, front_theta};
-        double map_rows = map_cols * 1.0 / (j*0.5) ;
+        // 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 = -5; i < 6; i++)
         {
-            input_vars[2] = (-angle_x + i) * M_PI / 180.0;
-            // printf("middle x:%.3f  y: %.3f  wheel:%.3f  width:%.3f    theta : %.3f   front theta: %.3f\n",input_vars[0],input_vars[1],input_vars[3],input_vars[4],input_vars[2],input_vars[5]);
-            // double t_loss = 1;
-            // bool current_result = Solve(input_vars[0], input_vars[1], input_vars[2], input_vars[3], input_vars[4], input_vars[5], t_loss);
-            double t_loss[5];
-            double t_whole_loss=100;
+            t_detect_result.theta = (-angle_x + i) * M_PI / 180.0;
+            // printf("double x:%.3f  y: %.3f  wheel:%.3f  width:%.3f    theta : %.3f   front theta: %.3f\n",input_vars[0],input_vars[1],input_vars[3],input_vars[4],input_vars[2],input_vars[5]);
+            Loss_result t_loss_result;
+            t_loss_result.total_avg_loss = 1000;
             //输出角度已变化，需恢复成弧度
             if(calc_count > 0)
             {
-                input_vars[2] = input_vars[2] * (-M_PI) / 180.0;
-                input_vars[5] = input_vars[5] * (-M_PI) / 180.0;
+                // t_detect_result.theta *= (-M_PI) / 180.0;
+                t_detect_result.front_theta *= (-M_PI) / 180.0;
             }
-            bool current_result = Solve(input_vars[0], input_vars[1], input_vars[2], input_vars[3], input_vars[4], input_vars[5], t_loss);
+            auto t1=std::chrono::system_clock::now();
+            bool current_result = Solve(t_detect_result, t_loss_result);
+            auto t2=std::chrono::system_clock::now();
+            auto duration=t2-t1;
+            double second=std::chrono::duration_cast<std::chrono::milliseconds>(duration).count()/1000.0;
+            total_solve_time+=second;
+            // std::cout<<" time "<<second<<std::endl;
+            // std::cout<<"current_result: "<<current_result<<std::endl;
             if(!current_result)
                 continue;
-            t_whole_loss = t_loss[0] + t_loss[1] + t_loss[2] + t_loss[3];
             // LOG(INFO) << "avg loss, current theta: " << t_loss << ", " << input_vars[2];
-            if (avg_loss > t_loss[4])
+            if (avg_loss > t_loss_result.total_avg_loss)
             {
-                avg_loss = t_loss[4];
+                avg_loss = t_loss_result.total_avg_loss;
                 // final_theta = -input_vars[2] * M_PI / 180.0;
-                cx = input_vars[0];
-                cy = input_vars[1];
-                theta = input_vars[2];
-                wheel_base = input_vars[3];
-                width = input_vars[4];
-                front_theta = input_vars[5];// * M_PI / 180.0;
+                detect_result = t_detect_result;
                 solve_result = current_result;
-                memcpy(whole_loss, t_loss, 5*sizeof(double));
+                loss_result = t_loss_result;
                 optimized_map_rows = map_rows;
                 calc_count++;
             }
         }
-    }
-    
+    } 
+    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", whole_loss[0], whole_loss[1], whole_loss[2], whole_loss[3], whole_loss[4]);
+        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);
-    // double input_vars[] = {cx, cy, theta, wheel_base, width, front_theta};
-    // input_vars[2] = input_vars[2] * (-M_PI) / 180.0;
-    // input_vars[5] = input_vars[5] * (-M_PI) / 180.0;
-    // double t_loss[5];
+    // 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(input_vars[0], input_vars[1], input_vars[2], input_vars[3], input_vars[4], input_vars[5], t_loss, true);
-
-    // theta = -final_theta * M_PI / 180.0;
-    //printf("final x:%.3f  y: %.3f  wheel:%.3f  width:%.3f    theta : %.3f   front theta: %.3f\n",cx,cy,wheel_base,width,theta,front_theta);
-    return solve_result;//Solve(cx, cy, theta, wheel_base, width, front_theta, avg_loss);
-
-
-    //清理点云
-   /* 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;
+    // Solve(detect_result, t_loss_result, true);
 
-        }
+    return solve_result;
 
-    }
-    cx=center_x;
-    cy=center_y;
-    theta=-angle_x*M_PI/180.0;
-    double loss=0;
-    return Solve(cx,cy,theta,wheel_base,width,front_theta,loss);*/
 }
 
-void detect_wheel_ceres::transform_src(double cx,double cy,double theta,double length,double width,double theta_front)
+// 根据测量结果，生成四轮各中心十字星点云
+void detect_wheel_ceres::transform_src(Detect_result detect_result)
 {
 
     //整车旋转
-    Eigen::Rotation2D<double> rotation(-theta);
+    Eigen::Rotation2D<double> rotation(-detect_result.theta);
     Eigen::Matrix<double, 2, 2> rotation_matrix = rotation.toRotationMatrix();
 
     //左前偏移
-    Eigen::Matrix<double, 2, 1> wheel_center_normal_left_front(length / 2.0, width / 2.0);
+    Eigen::Matrix<double, 2, 1> wheel_center_normal_left_front(detect_result.wheel_base / 2.0, detect_result.width / 2.0);
     //右前偏移
-    Eigen::Matrix<double, 2, 1> wheel_center_normal_right_front(length / 2.0, -width / 2.0);
+    Eigen::Matrix<double, 2, 1> wheel_center_normal_right_front(detect_result.wheel_base / 2.0, -detect_result.width / 2.0);
     //左后偏移
-    Eigen::Matrix<double, 2, 1> wheel_center_normal_left_rear(-length / 2.0, width / 2.0);
+    Eigen::Matrix<double, 2, 1> wheel_center_normal_left_rear(-detect_result.wheel_base / 2.0, detect_result.width / 2.0);
     //右后偏移
-    Eigen::Matrix<double, 2, 1> wheel_center_normal_right_rear(-length / 2.0, -width / 2.0);
+    Eigen::Matrix<double, 2, 1> wheel_center_normal_right_rear(-detect_result.wheel_base / 2.0, -detect_result.width / 2.0);
 
 
-    Eigen::Matrix<double, 2, 1> translate_car(cx,cy);
+    Eigen::Matrix<double, 2, 1> translate_car(detect_result.cx,detect_result.cy);
 
     const Eigen::Matrix<double, 2, 1> lt_center = rotation_matrix * wheel_center_normal_left_front+translate_car;
     const Eigen::Matrix<double, 2, 1> rt_center = rotation_matrix * wheel_center_normal_right_front+translate_car;
     const Eigen::Matrix<double, 2, 1> lb_center = rotation_matrix * wheel_center_normal_left_rear+translate_car;
     const Eigen::Matrix<double, 2, 1> rb_center = rotation_matrix * wheel_center_normal_right_rear+translate_car;
 
-    create_mark(lt_center(0,0),lt_center(1,0),-theta-theta_front,m_left_front_cloud_out);
-    create_mark(rt_center(0,0),rt_center(1,0),-theta-theta_front,m_right_front_cloud_out);
-    create_mark(lb_center(0,0),lb_center(1,0),-theta,m_left_rear_cloud_out);
-    create_mark(rb_center(0,0),rb_center(1,0),-theta,m_right_rear_cloud_out);
-
-
-
-    //左前轮
-    /*m_left_front_cloud_out.clear();
-    for (int i = 0; i < m_left_front_cloud.size(); ++i) {
-        const Eigen::Matrix<double, 2, 1> point((double(m_left_front_cloud[i].x) - cx),
-                                           (double(m_left_front_cloud[i].y) - cy));
-        //减去经过车辆旋转计算的左前中心
-        const Eigen::Matrix<double, 2, 1> tanslate_point = rotation_matrix * point - wheel_center_normal_left_front;
-        //旋转
-        const Eigen::Matrix<double, 2, 1> point_rotation = rotation_matrix_front * tanslate_point;
-        pcl::PointXYZ point_out(point_rotation(0,0)+17.0,point_rotation(1,0)+1,0);
-        m_left_front_cloud_out.push_back(point_out);
-    }
-    //右前轮
-    int right_front_num = m_right_front_cloud.size();
-    m_right_front_cloud_out.clear();
-    for (int i = 0; i < m_right_front_cloud.size(); ++i) {
-        const Eigen::Matrix<double, 2, 1> point((double(m_right_front_cloud[i].x) - cx),
-                                           (double(m_right_front_cloud[i].y) - cy));
-        //减去经过车辆旋转计算的左前中心
-        const Eigen::Matrix<double, 2, 1> tanslate_point = rotation_matrix * point - wheel_center_normal_right_front;
-        //旋转
-        const Eigen::Matrix<double, 2, 1> point_rotation = rotation_matrix_front * tanslate_point;
-        pcl::PointXYZ point_out(point_rotation(0,0)+17.0,point_rotation(1,0)-1.0,0);
-        m_right_front_cloud_out.push_back(point_out);
-
-    }
-    //左后轮
-    m_left_rear_cloud_out.clear();
-    for (int i = 0; i < m_left_rear_cloud.size(); ++i) {
-        const Eigen::Matrix<double, 2, 1> point((double(m_left_rear_cloud[i].x) - cx),
-                                           (double(m_left_rear_cloud[i].y) - cy));
-        //减去经过车辆旋转计算的左前中心
-        const Eigen::Matrix<double, 2, 1> tanslate_point = rotation_matrix * point - wheel_center_normal_left_rear;
-
-        pcl::PointXYZ point_out(tanslate_point(0,0)+17.0+1,tanslate_point(1,0),0);
-        m_left_rear_cloud_out.push_back(point_out);
-
-    }
-
-    //右后轮
-    m_right_rear_cloud_out.clear();
-    for (int i = 0; i < m_right_rear_cloud.size(); ++i) {
-        const Eigen::Matrix<double, 2, 1> point((double(m_right_rear_cloud[i].x) - cx),
-                                           (double(m_right_rear_cloud[i].y) - cy));
-        //减去经过车辆旋转计算的左前中心
-        const Eigen::Matrix<double, 2, 1> tanslate_point = rotation_matrix * point - wheel_center_normal_right_rear;
-        pcl::PointXYZ point_out(tanslate_point(0,0)+17.0-1,tanslate_point(1,0),0);
-        m_right_rear_cloud_out.push_back(point_out);
-
-    }*/
-
+    create_mark(lt_center(0,0),lt_center(1,0),-detect_result.theta-detect_result.front_theta,m_left_front_cloud_out);
+    create_mark(rt_center(0,0),rt_center(1,0),-detect_result.theta-detect_result.front_theta,m_right_front_cloud_out);
+    create_mark(lb_center(0,0),lb_center(1,0),-detect_result.theta,m_left_rear_cloud_out);
+    create_mark(rb_center(0,0),rb_center(1,0),-detect_result.theta,m_right_rear_cloud_out);
 }
 
+// 创建车轮中心十字星点云
 void detect_wheel_ceres::create_mark(double x,double y,double theta,pcl::PointCloud<pcl::PointXYZ>& cloud_out)
 {
     cloud_out.clear();
@@ -627,15 +483,14 @@ void detect_wheel_ceres::create_mark(double x,double y,double theta,pcl::PointCl
         cloud_out.push_back(point_out);
 
     }
-
 }
 
-bool detect_wheel_ceres::Solve(double& x,double& y,double& theta,double& wheel_base,double& width,double& front_theta,double &avg_loss)
+bool detect_wheel_ceres::Solve(Detect_result &detect_result, double &avg_loss)
 {
     double SCALE=300.0;
-    double cx=x;
-    double cy=y;
-    double init_theta=theta;
+    double cx=detect_result.cx;
+    double cy=detect_result.cy;
+    double init_theta=detect_result.theta;
     double init_wheel_base=2.7;
     double init_width=1.55;
     double init_theta_front=0*M_PI/180.0;
@@ -669,51 +524,54 @@ bool detect_wheel_ceres::Solve(double& x,double& y,double& theta,double& wheel_b
 
     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);
+    detect_result.cx=variable[0];
+    detect_result.cy=variable[1];
+    detect_result.theta=(-variable[2])*180.0/M_PI;
+    detect_result.wheel_base=variable[3];
+    detect_result.width=variable[4];
+    detect_result.front_theta=-(variable[5]*180.0/M_PI);
 
-    if(theta>180.0)
-        theta=theta-180.0;
-    if(theta<0)
-        theta+=180.0;
+    if(detect_result.theta>180.0)
+        detect_result.theta=detect_result.theta-180.0;
+    if(detect_result.theta<0)
+        detect_result.theta+=180.0;
 
     //检验
     if(loss>0.01)
         return false;
-    if (width < 1.350 || width > 2.000 || wheel_base > 3.000 || wheel_base < 2.200)
+    if (detect_result.width < 1.350 || detect_result.width > 2.000 || detect_result.wheel_base > 3.000 || detect_result.wheel_base < 2.200)
     {
         return false;
     }
 
-    width+=0.15;//车宽+10cm
+    detect_result.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);
 
     // //added by yct
     avg_loss = loss; // 将loss传出
 
-    transform_src(x,y,variable[2],variable[3],variable[4],variable[5]);
+    Detect_result t_detect_result = detect_result;
+    t_detect_result.theta *= -M_PI/180.0;
+    t_detect_result.front_theta *= -M_PI/180.0;
+    transform_src(t_detect_result);
 
     return true;
 
 }
 
 
-bool detect_wheel_ceres::Solve(double& x,double& y,double& theta,double& wheel_base,double& width,double& front_theta,double *losses, bool save_img)
+bool detect_wheel_ceres::Solve(Detect_result &detect_result, Loss_result &loss_result, bool save_img)
 {
     double SCALE=300.0;
-    double cx=x;
-    double cy=y;
-    double init_theta=theta;
+    double cx=detect_result.cx;
+    double cy=detect_result.cy;
+    double init_theta=detect_result.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};
-
+    // 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);
@@ -736,216 +594,227 @@ bool detect_wheel_ceres::Solve(double& x,double& y,double& theta,double& wheel_b
     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);*/
-
+    // debug_img(detect_result, loss_result, true);
     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);
+    detect_result.cx=variable[0];
+    detect_result.cy=variable[1];
+    detect_result.theta=(-variable[2])*180.0/M_PI;
+    detect_result.wheel_base=variable[3];
+    detect_result.width=variable[4];
+    detect_result.front_theta=-(variable[5]*180.0/M_PI);
 
-    if(theta>180.0)
-        theta=theta-180.0;
-    if(theta<0)
-        theta+=180.0;
+    if(detect_result.theta>180.0)
+        detect_result.theta=detect_result.theta-180.0;
+    if(detect_result.theta<0)
+        detect_result.theta+=180.0;
 
     //检验
+    // 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.01)
+    {
+        LOG(WARNING) <<"总loss过大 "<<loss;
         return false;
-    if (width < 1.350 || width > 2.000 || wheel_base > 3.000 || wheel_base < 2.200)
+    }
+    if (detect_result.width < 1.350 || detect_result.width > 2.000 || detect_result.wheel_base > 3.000 || detect_result.wheel_base < 2.200)
     {
+        LOG(WARNING) <<"宽度(1.35, 2.00) 轴距(2.20, 3.00): "<<detect_result.width<<", "<<detect_result.wheel_base;
         return false;
     }
 
-
-    width+=0.15;//车宽+10cm
+    detect_result.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);
 
     // //added by yct
-    if(theta > 120 || theta < 60)
+    if(detect_result.theta > 120 || detect_result.theta < 60)
     {
-        LOG(WARNING) <<"总角度错误 "<<theta;
+        LOG(WARNING) <<"总角度错误 "<<detect_result.theta;
         return false;
     }
-    if(fabs(front_theta)>35)
+    if(fabs(detect_result.front_theta)>35)
     {
-        LOG(WARNING) <<"前轮角度错误 "<<front_theta;
+        LOG(WARNING) <<"前轮角度错误 "<<detect_result.front_theta;
         return false;
     }
     // 将loss传出
     if(cost_func!=nullptr)
     {
-        // std::cout << "000"<< std::endl;
+        
         double costs_lf, costs_rf, costs_lr, costs_rr;
         cost_func->get_costs(costs_lf, costs_rf, costs_lr, costs_rr);
         // std::cout << "111"<< std::endl;
-        losses[0] = costs_lf;
-        losses[1] = costs_rf;
-        losses[2] = costs_lr;
-        losses[3] = costs_rr;
-        losses[4] = loss;
+        loss_result.lf_loss = costs_lf;
+        loss_result.rf_loss = costs_rf;
+        loss_result.lb_loss = costs_lr;
+        loss_result.rb_loss = costs_rr;
+        loss_result.total_avg_loss = loss;
         // std::cout << "222"<< std::endl;
         if (costs_lf <= 1e-6 && costs_rf <= 1e-6 && costs_lr <= 1e-6 && costs_rr <= 1e-6)
         {
-            for (size_t i = 0; i < 4; i++)
-            {
-                losses[i] = loss;
-            }
+            loss_result.lf_loss = loss;
+            loss_result.rf_loss = loss;
+            loss_result.lb_loss = loss;
+            loss_result.rb_loss = loss;
         }
         // 判断每个轮子平均loss是否满足条件
-        for (size_t i = 0; i < 4; i++)
+        double single_wheel_loss_threshold = 0.09;
+        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)
         {
-            if(losses[i] > 0.09)
-            {
-                LOG(WARNING) <<"四轮loss过大";
-                return false;
-            }
+            LOG(WARNING) <<"四轮loss过大";
+            return false;
         }
-        // std::cout << "333"<< std::endl;
-        // std::cout<<"costs for four wheels: ";
-        // std::cout<<costs_lf<<" ";
-        // std::cout<<costs_rf<<" ";
-        // std::cout<<costs_lr<<" ";
-        // std::cout<<costs_rr<<" "<<std::endl;
 
         // std::cout<<"save img: "<<save_img<<std::endl;
-        if (save_img)
-        {
-            cv::Mat lf = m_map.clone();
-            cv::Mat rf = m_map.clone();
-            cv::Mat lb = m_map.clone();
-            cv::Mat rb = m_map.clone();
-            cv::Mat total_img = cv::Mat::zeros(m_map.cols*2, m_map.rows*2, m_map.type());
-            cv::Mat cvted_img = cv::Mat::zeros(m_map.cols*2, m_map.rows*2, CV_8U);
-            //整车旋转
-            Eigen::Rotation2D<double> rotation(variable[2]);
-
-            Eigen::Matrix<double, 2, 2> rotation_matrix = rotation.toRotationMatrix();
-
-            //左前偏移
-            Eigen::Matrix<double, 2, 1> wheel_center_normal_left_front(wheel_base / 2.0, (width - 0.15) / 2.0);
-            //右前偏移
-            Eigen::Matrix<double, 2, 1> wheel_center_normal_right_front(wheel_base / 2.0, (-width + 0.15) / 2.0);
-            //左后偏移
-            Eigen::Matrix<double, 2, 1> wheel_center_normal_left_rear(-wheel_base / 2.0, (width - 0.15) / 2.0);
-            //右后偏移
-            Eigen::Matrix<double, 2, 1> wheel_center_normal_right_rear(-wheel_base / 2.0, (-width + 0.15) / 2.0);
-
-            //前轮旋转
-            Eigen::Rotation2D<double> rotation_front(variable[5]);
-            Eigen::Matrix<double, 2, 2> rotation_matrix_front = rotation_front.toRotationMatrix();
-
-            // 左前轮
-            for (size_t i = 0; i < m_left_front_cloud.size(); i++)
-            {
-                Eigen::Matrix<double, 2, 1> point((double(m_left_front_cloud[i].x) - x),
-                                                  (double(m_left_front_cloud[i].y) - y));
-                //减去经过车辆旋转计算的左前中心
-                Eigen::Matrix<double, 2, 1> tanslate_point = rotation_matrix * point - wheel_center_normal_left_front;
-                //旋转
-                Eigen::Matrix<double, 2, 1> point_rotation = rotation_matrix_front * tanslate_point;
-                int r = (int)(point_rotation(1, 0) * SCALE + m_map.rows / 2.0 + 0.5);
-                int c = (int)(point_rotation(0, 0) * SCALE + m_map.cols / 2.0 + 0.5);
-                cv::circle(lf, cv::Point(c, r), 1, cv::Scalar(150));
-                // std::cout<<"map r,c "<<m_map.rows<<", "<<m_map.cols<<std::endl;
-                // std::cout<<"r,c "<<r<<", "<<c<<std::endl;
-                // lf.at<uchar>(r, c) = 150;
-            }
-            //右前轮
-            for (int i = 0; i < m_right_front_cloud.size(); ++i)
-            {
-                Eigen::Matrix<double, 2, 1> point((double(m_right_front_cloud[i].x) - x),
-                                                  (double(m_right_front_cloud[i].y) - y));
-                //减去经过车辆旋转计算的左前中心
-                Eigen::Matrix<double, 2, 1> tanslate_point = rotation_matrix * point - wheel_center_normal_right_front;
-                //旋转
-                Eigen::Matrix<double, 2, 1> point_rotation = rotation_matrix_front * tanslate_point;
-                int r = (int)(point_rotation(1, 0) * SCALE + m_map.rows / 2.0 + 0.5);
-                int c = (int)(point_rotation(0, 0) * SCALE + m_map.cols / 2.0 + 0.5);
-                cv::circle(rf, cv::Point(c, r), 1, cv::Scalar(150));
-            }
-            //左后轮
-            for (int i = 0; i < m_left_rear_cloud.size(); ++i)
-            {
-                Eigen::Matrix<double, 2, 1> point((double(m_left_rear_cloud[i].x) - x),
-                                                  (double(m_left_rear_cloud[i].y) - y));
-                //减去经过车辆旋转计算的左前中心
-                Eigen::Matrix<double, 2, 1> tanslate_point = rotation_matrix * point - wheel_center_normal_left_rear;
-                int r = (int)(tanslate_point(1, 0) * SCALE + m_map.rows / 2.0 + 0.5);
-                int c = (int)(tanslate_point(0, 0) * SCALE + m_map.cols / 2.0 + 0.5);
-                cv::circle(lb, cv::Point(c, r), 1, cv::Scalar(150));
-            }
-
-            //右后轮
-            for (int i = 0; i < m_right_rear_cloud.size(); ++i)
-            {
-                Eigen::Matrix<double, 2, 1> point((double(m_right_rear_cloud[i].x) - x),
-                                                  (double(m_right_rear_cloud[i].y) - y));
-                //减去经过车辆旋转计算的左前中心
-                Eigen::Matrix<double, 2, 1> tanslate_point = rotation_matrix * point - wheel_center_normal_right_rear;
-                int r = (int)(tanslate_point(1, 0) * SCALE + m_map.rows / 2.0 + 0.5);
-                int c = (int)(tanslate_point(0, 0) * SCALE + m_map.cols / 2.0 + 0.5);
-                cv::circle(rb, cv::Point(c, r), 1, cv::Scalar(150));
-            }
-
-            // cv::Mat rot90;// = (cv::Mat_<double>(2, 3)<<cos(90), -sin(90), 0, sin(90), cos(90), 0);
-            // rot90 = cv::getRotationMatrix2D(cv::Point2f(lf.cols/2, lf.rows/2), 90, 1);
-            // // std::cout<<rot90<<std::endl;
-            // cv::warpAffine(lf, lf, rot90, cv::Size(lf.cols, lf.rows));
-            // cv::flip()
-            // cv::namedWindow("left front", cv::WINDOW_FREERATIO);
-            // cv::namedWindow("right front", cv::WINDOW_FREERATIO);
-            // cv::namedWindow("left back", cv::WINDOW_FREERATIO);
-            // cv::namedWindow("right back", cv::WINDOW_FREERATIO);
-            cv::flip(lf, lf, -1);
-            cv::flip(rf, rf, -1);
-            cv::flip(lb, lb, -1);
-            cv::flip(rb, rb, -1);
-            cv::Mat lft=lf.t(), rft=rf.t(), lbt=lb.t(), rbt=rb.t();
-            // cv::imshow("left front", lf.t());
-            // cv::imshow("right front", rf.t());
-            // cv::imshow("left back", lb.t());
-            // cv::imshow("right back", rb.t());
-            // 写入各轮平均误差
-            cv::putText(lft, (boost::format("lf %.6f") %(losses[0])).str().c_str(),
-                        cv::Point(lft.cols / 6, lft.rows / 4),
-                        cv::FONT_HERSHEY_COMPLEX, 1, cv::Scalar(0, 0, 0));
-            cv::putText(rft, (boost::format("rf %.6f") %(losses[1])).str().c_str(),
-                        cv::Point(rft.cols / 6, rft.rows / 4),
-                        cv::FONT_HERSHEY_COMPLEX, 1, cv::Scalar(0, 0, 0));
-            cv::putText(lbt, (boost::format("lb %.6f") %(losses[2])).str().c_str(),
-                        cv::Point(lbt.cols / 6, lbt.rows / 4),
-                        cv::FONT_HERSHEY_COMPLEX, 1, cv::Scalar(0, 0, 0));
-            cv::putText(rbt, (boost::format("rb %.6f") %(losses[3])).str().c_str(),
-                        cv::Point(rbt.cols / 6, rbt.rows / 4),
-                        cv::FONT_HERSHEY_COMPLEX, 1, cv::Scalar(0, 0, 0));
-
-            lft.copyTo(total_img(cv::Rect(0, 0, m_map.rows, m_map.cols)));
-            rft.copyTo(total_img(cv::Rect(m_map.rows, 0, m_map.rows, m_map.cols)));
-            lbt.copyTo(total_img(cv::Rect(0, m_map.cols, m_map.rows, m_map.cols)));
-            rbt.copyTo(total_img(cv::Rect(m_map.rows, m_map.cols, m_map.rows, m_map.cols)));
-            // cv::namedWindow("total img", CV_WINDOW_FREERATIO);
-            // cv::imshow("total img", total_img);
-            int total_milli = (std::chrono::time_point_cast<std::chrono::milliseconds>(std::chrono::system_clock::now())).time_since_epoch().count();
-            std::string img_filename = std::string("/home/youchen/Documents/measure/MainStructure/elecfence_ws/img/img").append(std::to_string(total_milli)).append(".jpg");
-            LOG(INFO) << "write to " << img_filename.c_str();
-            // cv::cvtColor(total_img*255, cvted_img, CV_8U);
-            cv::convertScaleAbs(total_img*255, cvted_img);
-            cv::imwrite(img_filename, cvted_img);
-            // cv::waitKey();
-        }
+        debug_img(detect_result, loss_result, save_img);
     }
     else
     {
         return false;
     }
 
-    transform_src(x,y,variable[2],variable[3],variable[4],variable[5]);
+    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.15;
+    transform_src(t_detect_result);
 
     return true;
+}
+
+// 显示/保存图片供调试用
+void detect_wheel_ceres::debug_img(Detect_result detect_result, Loss_result loss_result, bool save_img, std::string out_img_path)
+{
+    double SCALE=300.0;
+    cv::Mat lf = m_map.clone();
+    cv::Mat rf = m_map.clone();
+    cv::Mat lb = m_map.clone();
+    cv::Mat rb = m_map.clone();
+    //整车旋转
+    Eigen::Rotation2D<double> rotation(detect_result.theta);
+
+    Eigen::Matrix<double, 2, 2> rotation_matrix = rotation.toRotationMatrix();
+
+    //左前偏移
+    Eigen::Matrix<double, 2, 1> wheel_center_normal_left_front(detect_result.wheel_base / 2.0, (detect_result.width - 0.15) / 2.0);
+    //右前偏移
+    Eigen::Matrix<double, 2, 1> wheel_center_normal_right_front(detect_result.wheel_base / 2.0, (-detect_result.width + 0.15) / 2.0);
+    //左后偏移
+    Eigen::Matrix<double, 2, 1> wheel_center_normal_left_rear(-detect_result.wheel_base / 2.0, (detect_result.width - 0.15) / 2.0);
+    //右后偏移
+    Eigen::Matrix<double, 2, 1> wheel_center_normal_right_rear(-detect_result.wheel_base / 2.0, (-detect_result.width + 0.15) / 2.0);
+
+    //前轮旋转
+    Eigen::Rotation2D<double> rotation_front(detect_result.front_theta);
+    Eigen::Matrix<double, 2, 2> rotation_matrix_front = rotation_front.toRotationMatrix();
+
+    // 左前轮
+    for (size_t i = 0; i < m_left_front_cloud.size(); i++)
+    {
+        Eigen::Matrix<double, 2, 1> point((double(m_left_front_cloud[i].x) - detect_result.cx),
+                                          (double(m_left_front_cloud[i].y) - detect_result.cy));
+        //减去经过车辆旋转计算的左前中心
+        Eigen::Matrix<double, 2, 1> tanslate_point = rotation_matrix * point - wheel_center_normal_left_front;
+        //旋转
+        Eigen::Matrix<double, 2, 1> point_rotation = rotation_matrix_front * tanslate_point;
+        int r = (int)(point_rotation(1, 0) * SCALE + m_map.rows / 2.0 + 0.5);
+        int c = (int)(point_rotation(0, 0) * SCALE + m_map.cols / 2.0 + 0.5);
+        cv::circle(lf, cv::Point(c, r), 1, cv::Scalar(150));
+        // std::cout<<"map r,c "<<m_map.rows<<", "<<m_map.cols<<std::endl;
+        // std::cout<<"r,c "<<r<<", "<<c<<std::endl;
+        // lf.at<uchar>(r, c) = 150;
+    }
+    //右前轮
+    for (int i = 0; i < m_right_front_cloud.size(); ++i)
+    {
+        Eigen::Matrix<double, 2, 1> point((double(m_right_front_cloud[i].x) - detect_result.cx),
+                                          (double(m_right_front_cloud[i].y) - detect_result.cy));
+        //减去经过车辆旋转计算的左前中心
+        Eigen::Matrix<double, 2, 1> tanslate_point = rotation_matrix * point - wheel_center_normal_right_front;
+        //旋转
+        Eigen::Matrix<double, 2, 1> point_rotation = rotation_matrix_front * tanslate_point;
+        int r = (int)(point_rotation(1, 0) * SCALE + m_map.rows / 2.0 + 0.5);
+        int c = (int)(point_rotation(0, 0) * SCALE + m_map.cols / 2.0 + 0.5);
+        cv::circle(rf, cv::Point(c, r), 1, cv::Scalar(150));
+    }
+    //左后轮
+    for (int i = 0; i < m_left_rear_cloud.size(); ++i)
+    {
+        Eigen::Matrix<double, 2, 1> point((double(m_left_rear_cloud[i].x) - detect_result.cx),
+                                          (double(m_left_rear_cloud[i].y) - detect_result.cy));
+        //减去经过车辆旋转计算的左前中心
+        Eigen::Matrix<double, 2, 1> tanslate_point = rotation_matrix * point - wheel_center_normal_left_rear;
+        int r = (int)(tanslate_point(1, 0) * SCALE + m_map.rows / 2.0 + 0.5);
+        int c = (int)(tanslate_point(0, 0) * SCALE + m_map.cols / 2.0 + 0.5);
+        cv::circle(lb, cv::Point(c, r), 1, cv::Scalar(150));
+    }
+
+    //右后轮
+    for (int i = 0; i < m_right_rear_cloud.size(); ++i)
+    {
+        Eigen::Matrix<double, 2, 1> point((double(m_right_rear_cloud[i].x) - detect_result.cx),
+                                          (double(m_right_rear_cloud[i].y) - detect_result.cy));
+        //减去经过车辆旋转计算的左前中心
+        Eigen::Matrix<double, 2, 1> tanslate_point = rotation_matrix * point - wheel_center_normal_right_rear;
+        int r = (int)(tanslate_point(1, 0) * SCALE + m_map.rows / 2.0 + 0.5);
+        int c = (int)(tanslate_point(0, 0) * SCALE + m_map.cols / 2.0 + 0.5);
+        cv::circle(rb, cv::Point(c, r), 1, cv::Scalar(150));
+    }
+
+    // cv::Mat rot90;// = (cv::Mat_<double>(2, 3)<<cos(90), -sin(90), 0, sin(90), cos(90), 0);
+    // rot90 = cv::getRotationMatrix2D(cv::Point2f(lf.cols/2, lf.rows/2), 90, 1);
+    // // std::cout<<rot90<<std::endl;
+    // cv::warpAffine(lf, lf, rot90, cv::Size(lf.cols, lf.rows));
+    // cv::flip()
+    // cv::namedWindow("left front", cv::WINDOW_FREERATIO);
+    // cv::namedWindow("right front", cv::WINDOW_FREERATIO);
+    // cv::namedWindow("left back", cv::WINDOW_FREERATIO);
+    // cv::namedWindow("right back", cv::WINDOW_FREERATIO);
+    cv::flip(lf, lf, -1);
+    cv::flip(rf, rf, -1);
+    cv::flip(lb, lb, -1);
+    cv::flip(rb, rb, -1);
+    cv::Mat lft = lf.t(), rft = rf.t(), lbt = lb.t(), rbt = rb.t();
+    // cv::imshow("left front", lf.t());
+    // cv::imshow("right front", rf.t());
+    // cv::imshow("left back", lb.t());
+    // cv::imshow("right back", rb.t());
+    // 写入各轮平均误差
+    cv::putText(lft, (boost::format("lf %.6f") % (loss_result.lf_loss)).str().c_str(),
+                cv::Point(lft.cols / 6, lft.rows / 4),
+                cv::FONT_HERSHEY_COMPLEX, 1, cv::Scalar(0, 0, 0));
+    cv::putText(rft, (boost::format("rf %.6f") % (loss_result.rf_loss)).str().c_str(),
+                cv::Point(rft.cols / 6, rft.rows / 4),
+                cv::FONT_HERSHEY_COMPLEX, 1, cv::Scalar(0, 0, 0));
+    cv::putText(lbt, (boost::format("lb %.6f") % (loss_result.lb_loss)).str().c_str(),
+                cv::Point(lbt.cols / 6, lbt.rows / 4),
+                cv::FONT_HERSHEY_COMPLEX, 1, cv::Scalar(0, 0, 0));
+    cv::putText(rbt, (boost::format("rb %.6f") % (loss_result.rb_loss)).str().c_str(),
+                cv::Point(rbt.cols / 6, rbt.rows / 4),
+                cv::FONT_HERSHEY_COMPLEX, 1, cv::Scalar(0, 0, 0));
+
+    cv::Mat total_img = cv::Mat::zeros(m_map.cols * 2, m_map.rows * 2, m_map.type());
+    lft.copyTo(total_img(cv::Rect(0, 0, m_map.rows, m_map.cols)));
+    rft.copyTo(total_img(cv::Rect(m_map.rows, 0, m_map.rows, m_map.cols)));
+    lbt.copyTo(total_img(cv::Rect(0, m_map.cols, m_map.rows, m_map.cols)));
+    rbt.copyTo(total_img(cv::Rect(m_map.rows, m_map.cols, m_map.rows, m_map.cols)));
+    // cv::namedWindow("total img", CV_WINDOW_FREERATIO);
+    // cv::imshow("total img", total_img);
+    // cv::waitKey(20);
+    if(save_img)
+    {
+        cv::Mat cvted_img = cv::Mat::zeros(m_map.cols * 2, m_map.rows * 2, CV_8U);
+        int total_milli = (std::chrono::time_point_cast<std::chrono::milliseconds>(std::chrono::system_clock::now())).time_since_epoch().count();
+        std::string img_filename="";
+        if(out_img_path.empty()){
+            img_filename = std::string("/home/youchen/Documents/measure/MainStructure/elecfence_ws/img/img").append(std::to_string(total_milli)).append(".jpg");
+        }
+        else{
+            img_filename = out_img_path;
+        }
+        LOG(INFO) << "write to " << img_filename.c_str();
+        // cv::cvtColor(total_img*255, cvted_img, CV_8U);
+        cv::convertScaleAbs(total_img * 255, cvted_img);
+        cv::imwrite(img_filename, cvted_img);
+    }
 }

src/findwheel/src/detect_wheel_ceres.h

@@ -18,16 +18,56 @@
 
 class detect_wheel_ceres {
 public:
+    struct Detect_result
+    {
+        public: 
+        double cx;
+        double cy;
+        double theta;
+        double wheel_base;
+        double width;
+        double front_theta; 
+        bool success; 
+        Detect_result& operator=(const Detect_result detect_result)
+        {
+            this->cx = detect_result.cx;
+            this->cy = detect_result.cy;
+            this->theta = detect_result.theta;
+            this->wheel_base = detect_result.wheel_base;
+            this->width = detect_result.width;
+            this->front_theta = detect_result.front_theta;
+            this->success = detect_result.success;
+            return *this;
+        }
+    };
+    struct Loss_result
+    {
+        public: 
+        double lf_loss;
+        double rf_loss;
+        double lb_loss;
+        double rb_loss;
+        double total_avg_loss;
+        Loss_result& operator=(const Loss_result& loss_result)
+        {
+            this->lf_loss = loss_result.lf_loss;
+            this->rf_loss = loss_result.rf_loss;
+            this->lb_loss = loss_result.lb_loss;
+            this->rb_loss = loss_result.rb_loss;
+            this->total_avg_loss = loss_result.total_avg_loss;
+            return *this;
+        }
+    };
     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);
+    bool detect(std::vector<pcl::PointCloud<pcl::PointXYZ>> cloud_vec, Detect_result &detect_result);
 
 protected:
-    void transform_src(double x,double y,double theta,double wheel_base,double width,double front_theta);
+    void transform_src(Detect_result detect_result);
     bool update_mat(int rows, int cols);
-    bool Solve(double& x,double& y,double& theta,double& wheel_base,double& width,double& front_theta,double &avg_loss);
-    bool Solve(double& x,double& y,double& theta,double& wheel_base,double& width,double& front_theta,double *losses, bool save_img=false);
+    bool Solve(Detect_result &detect_result, double &avg_loss);
+    bool Solve(Detect_result &detect_result, Loss_result &loss_result, bool save_img=false);
+    void debug_img(Detect_result detect_result, Loss_result loss_result, bool save_img=false, std::string out_img_path="");
 
     void create_mark(double x,double y,double theta,pcl::PointCloud<pcl::PointXYZ>& cloud);
 public:

src/findwheel/src/region_detect.cpp

@@ -350,7 +350,8 @@ bool Region_detector::detect(pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud_in, doub
     if (result != true)
         return result;
 
-    if(!m_detector_ceres.detect(seg_clouds,x,y,c,wheelbase,width,front_wheel_theta))
+    detect_wheel_ceres::Detect_result detect_result{x,y,c,wheelbase,width,front_wheel_theta};
+    if(!m_detector_ceres.detect(seg_clouds, detect_result))
         return false;
 
     return true;

src/findwheel/src/region_worker.cpp

@@ -136,7 +136,7 @@ void Region_worker::detect_loop(Region_worker *worker)
             auto t2=std::chrono::system_clock::now();
             auto duration=t2-t1;
             double second=std::chrono::duration_cast<std::chrono::milliseconds>(duration).count()/1000.0;
-            std::cout<<" time "<<second<<std::endl;
+            std::cout<<"------------------ total time --------------- "<<second<<std::endl;
 
             worker->publish_origin_cloud(*worker->m_cloud);
             if(worker->get_id()==0)