with ceres optimized, 20200812

wj_lidar/detect_wheel_ceres.cpp

@@ -41,25 +41,33 @@ class CostFunctor {
 private:
     cv::Mat  m_map;
     double m_scale;
+    mutable double m_costs_lf, m_costs_rf, m_costs_lr, m_costs_rr;
     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)
+    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;
+        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;
+        m_costs_lf = 0.0;
+        m_costs_rf = 0.0;
+        m_costs_lr = 0.0;
+        m_costs_rr = 0.0;
     }
     template <typename T>
     bool operator()(const T* const variable, T* residual) const {
+        // 每一轮重新初始化残差值
+        T costs[4]={T(0),T(0),T(0),T(0)};
+
         T cx = variable[0];
         T cy = variable[1];
         T theta = variable[2];
@@ -102,6 +110,7 @@ public:
             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]);
+            costs[0] += residual[i];
 
         }
         //右前轮
@@ -116,7 +125,7 @@ public:
             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]);
-
+            costs[1] += residual[left_front_num+i];
         }
         //左后轮
         int left_rear_num = m_left_rear_cloud.size();
@@ -130,7 +139,7 @@ public:
             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]);
-
+            costs[2] += residual[left_front_num + right_front_num + i];
         }
 
         //右后轮
@@ -144,14 +153,36 @@ public:
             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]);
-
+            costs[3] += residual[left_front_num + right_front_num + left_rear_num + i];
         }
 
-        return true;
-    }
+        char buf[30];
+        memset(buf, 0, 30);
+        sprintf(buf, "%.7f", costs[0]);
+        m_costs_lf = std::stod(buf) / left_front_num;
+
+        memset(buf, 0, 30);
+        sprintf(buf, "%.7f", costs[1]);
+        m_costs_rf = std::stod(buf) / right_front_num;
+
+        memset(buf, 0, 30);
+        sprintf(buf, "%.7f", costs[2]);
+        m_costs_lr = std::stod(buf) / left_rear_num;
 
-private:
 
+        memset(buf, 0, 30);
+        sprintf(buf, "%.7f", costs[3]);
+        m_costs_rr = std::stod(buf) / m_right_rear_cloud.size();
+        // m_costs_lf = costs[0];
+        return true;
+    }
+    void get_costs(double &lf, double &rf, double &lr, double &rr)
+    {
+        lf = m_costs_lf;
+        rf = m_costs_rf;
+        lr = m_costs_lr;
+        rr = m_costs_rr;
+    }
 };
 
 bool detect_wheel_ceres::update_mat(int rows, int cols)
@@ -165,18 +196,28 @@ bool detect_wheel_ceres::update_mat(int rows, int cols)
 
     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);
+        double x = n / double(L);
+        double sigma = 0.06;
+        double miu = 0.0;
+        double scale = 0.02;
+        double translation = -0.05;
+        double gauss_value = (scale/(sqrt(2*M_PI)*sigma))*exp(-pow(x-miu, 2)/(2*sigma*sigma))+translation;
+        double quadratic_value = x-0.08;
         // LOG(INFO) << rect.tl().x<<", "<<rect.tl().y<<", "<<rect.br().x<<", "<<rect.br().y;
         // 对内外分别取色
-        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::rectangle(map,rect,std::max(gauss_value, quadratic_value));
+
+        // if(n<K*2){
+        //     cv::rectangle(map,rect,1.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));
@@ -193,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();
@@ -209,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;
 
@@ -242,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)
     {
@@ -289,54 +331,175 @@ 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;
-    double final_theta = 0;
-    double avg_loss = 100;   
+
+    // // 仅优化一次，不调整图像比例与角度初值。
+    // 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;
+    // 误差结构体，保存左前、右前、左后、右后、整体平均误差
+    Loss_result loss_result;
+    // 平均误差值，用于获取最小整体平均误差
+    double avg_loss = 100;
+    // 定义图像列数，控制图像大小
     int map_cols = 800;
+    // 优化后图像行数，用于保存优化后结果图像
+    int optimized_map_rows = 200;
+    // 优化结果
     bool solve_result = false;
-    for (int j = 2; j < 5; j++)
+    double total_solve_time = 0;
+    bool stop_sign = false;
+    for (int j = 2; j < 4&&!stop_sign; j++)
     {
-        double input_vars[] = {cx, cy, theta, wheel_base, width, front_theta};
+        // 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++)
+        for (int i = -5; i < 6&&!stop_sign; i++)
         {
-            double t_loss = 1;
-            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]);
-            bool current_result = Solve(input_vars[0], input_vars[1], input_vars[2], input_vars[3], input_vars[4], input_vars[5], t_loss);
+            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)
+            {
+                // t_detect_result.theta *= (-M_PI) / 180.0;
+                t_detect_result.front_theta *= (-M_PI) / 180.0;
+            }
+            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;
+            static double second=0.0;
+            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;
             // LOG(INFO) << "avg loss, current theta: " << t_loss << ", " << input_vars[2];
-            if (avg_loss > t_loss)
+            if (avg_loss > t_loss_result.total_avg_loss)
             {
-                avg_loss = t_loss;
+                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;
+                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);
     
-    // LOG(INFO) << "avg loss, final theta: "<<avg_loss <<", "<<final_theta;
-    // 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);
+    // // 生成并保存图片
+    // 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);
+
+    return solve_result;
+
 }
 
-bool detect_wheel_ceres::Solve(double& x,double& y,double& theta,double& wheel_base,double& width,double& front_theta,double &avg_loss)
+// 根据测量结果，生成四轮各中心十字星点云
+void detect_wheel_ceres::transform_src(Detect_result detect_result)
+{
+
+    //整车旋转
+    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 / 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(-detect_result.wheel_base / 2.0, detect_result.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(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),-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();
+    pcl::PointCloud<pcl::PointXYZ> cloud;
+
+    //std::cout<<"1111111111"<<std::endl;
+    int width=30;
+    int height=10;
+    double step=0.015;
+
+    for(int i=0;i<width;++i)
+    {
+        pcl::PointXYZ point((i-width/2)*step,0,0);
+        cloud.push_back(point);
+    }
+    for(int i=0;i<height;++i)
+    {
+        pcl::PointXYZ point(0,(i-height/2)*step,0);
+        cloud.push_back(point);
+    }
+    //std::cout<<"22222222222"<<std::endl;
+    //整车旋转
+    Eigen::Rotation2D<double> rotation(theta);
+    Eigen::Matrix<double, 2, 2> rotation_matrix = rotation.toRotationMatrix();
+
+    for(int i=0;i<cloud.size();++i) {
+
+        const Eigen::Matrix<double, 2, 1> point((double(cloud[i].x)),
+                                                (double(cloud[i].y)));
+        //减去经过车辆旋转计算的左前中心
+        const Eigen::Matrix<double, 2, 1> tanslate_point = rotation_matrix * point;
+        pcl::PointXYZ point_out(tanslate_point(0,0)+x,tanslate_point(1,0)+y,0);
+        cloud_out.push_back(point_out);
+
+    }
+}
+
+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;
@@ -370,32 +533,297 @@ 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.03)
+    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传出
 
+    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(Detect_result &detect_result, Loss_result &loss_result, bool save_img)
+{
+    double SCALE=300.0;
+    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);
+    //使用自动求导，将之前的代价函数结构体传入，第一个1是输出维度，即残差的维度，第二个1是输入维度，即待寻优参数x的维度。
+    ceres::CostFunction* cost_function =new
+            ceres::AutoDiffCostFunction<CostFunctor, ceres::DYNAMIC, 6>(
+                    cost_func,
+                    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=60;
+    options.num_threads=3;
+    options.minimizer_progress_to_stdout = false;//输出到cout
+    ceres::Solver::Summary summary;//优化信息
+    ceres::Solve(options, &problem, &summary);//求解!!!
+
+    // std::cout << summary.BriefReport() << "\n";//输出优化的简要信息
+    // 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());
+
+    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(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 (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;
+    }
+
+    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(detect_result.theta > 120 || detect_result.theta < 60)
+    {
+//        LOG(WARNING) <<"总角度错误 "<<detect_result.theta;
+        return false;
+    }
+    if(fabs(detect_result.front_theta)>35)
+    {
+//        LOG(WARNING) <<"前轮角度错误 "<<detect_result.front_theta;
+        return false;
+    }
+    // 将loss传出
+    if(cost_func!=nullptr)
+    {
+        
+        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;
+        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)
+        {
+            loss_result.lf_loss = loss;
+            loss_result.rf_loss = loss;
+            loss_result.lb_loss = loss;
+            loss_result.rb_loss = loss;
+        }
+        // 判断每个轮子平均loss是否满足条件
+        double single_wheel_loss_threshold = 0.099;
+        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)
+        {
+//            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;
+//        debug_img(detect_result, loss_result, save_img);
+    }
+    else
+    {
+        return false;
+    }
+
+    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);
+    }
+}

wj_lidar/detect_wheel_ceres.h

@@ -11,24 +11,79 @@
 #include <pcl/point_cloud.h>
 #include <pcl/common/centroid.h>
 #include <opencv2/opencv.hpp>
+#include <iostream>
+#include <string>
+#include <chrono>
+#include "boost/format.hpp"
 
 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(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(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="");
 
-private:
+    void create_mark(double x,double y,double theta,pcl::PointCloud<pcl::PointXYZ>& cloud);
+public:
+    pcl::PointCloud<pcl::PointXYZ>          m_left_front_cloud_out;     //左前点云
+    pcl::PointCloud<pcl::PointXYZ>          m_right_front_cloud_out;    //右前点云
+    pcl::PointCloud<pcl::PointXYZ>          m_left_rear_cloud_out;      //左后点云
+    pcl::PointCloud<pcl::PointXYZ>          m_right_rear_cloud_out;     //右后点云
+
+protected:
     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;     //右后点云
     cv::Mat                                 m_map;                  //保存一份地图用作匹配
+
+
 };
 
 

wj_lidar/region_detect.cpp

@@ -418,7 +418,8 @@ Error_manager Region_detector::detect(pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud
     if (result != SUCCESS)
         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 WJ_REGION_CERES_SOLVE_ERROR;
 
     return Error_manager(Error_code::SUCCESS);