wj with ceres optimization. output angle unstable. need to roll back.

.~lock.智象停车测量软件设计文档.docx#

@@ -0,0 +1 @@
+,zx,zx-MIC,04.07.2020 17:39,file:///home/zx/.config/libreoffice/4;

terminor/terminal_command_executor.cpp

@@ -482,8 +482,8 @@ Error_manager Terminor_Command_Executor::scanning_measuring() {
     float offset_width = fabs(dj_locate_information.locate_width - wj_locate_information.locate_width);
     float offset_wheel_base = fabs(
             dj_locate_information.locate_wheel_base - wj_locate_information.locate_wheel_base);
-    if (offset_x > 100 || offset_y > 150 || offset_angle > 5 || offset_wheel_base > 250 || offset_width > 250) {
-        LOG(WARNING) << "chekc failed. (offset x>100, y>150, angle>5, wheel_base>250, width>250): " << offset_x
+    if (offset_x > 100 || offset_y > 150 || offset_angle > 5 || offset_wheel_base > 550 || offset_width > 250) {
+        LOG(WARNING) << "chekc failed. (offset x>100, y>150, angle>5, wheel_base>550, width>250): " << offset_x
                      << " " << offset_y << " " << offset_angle << " " << offset_wheel_base << " " << offset_width;
         return Error_manager(TERMINOR_CHECK_RESULTS_ERROR, NORMAL, "check results failed ");
     }

wj_lidar/detect_wheel_ceres.cpp

@@ -154,11 +154,9 @@ private:
 
 };
 
-detect_wheel_ceres::detect_wheel_ceres()
+bool detect_wheel_ceres::update_mat(int rows, int cols)
 {
     /////创建地图
-    int cols=800;
-    int rows=200;
     int L=std::min(rows,cols);
     cv::Mat map=cv::Mat::ones(L,L,CV_64F);
     //map=map*255;
@@ -166,19 +164,31 @@ detect_wheel_ceres::detect_wheel_ceres()
 
 
     float K=L*0.08;
+    // 从中心开始向外画矩形
     for(int n=0;n<L;n+=2)
     {
         cv::Size size(n+2,n+2);
         cv::Rect rect(center-cv::Point(n/2,n/2),size);
-        if(n<K*2)
+        // 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
+        }
+        else{
             cv::rectangle(map,rect,float(n-K*2)/float(L));
+        }
 
     }
     cv::resize(map,map,cv::Size(cols,rows));
     cv::GaussianBlur(map,m_map,cv::Size(7,7),3,3);
 }
+
+detect_wheel_ceres::detect_wheel_ceres()
+{
+    int cols=800;
+    int rows=200;
+    update_mat(rows, cols);
+}
 detect_wheel_ceres::~detect_wheel_ceres(){}
 
 
@@ -279,13 +289,49 @@ 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;
-    theta=-angle_x*M_PI/180.0;
-    return Solve(cx,cy,theta,wheel_base,width,front_theta);
+    double final_theta = 0;
+    double avg_loss = 100;   
+    int map_cols = 800;
+    bool solve_result = false;
+    for (int j = 2; j < 5; j++)
+    {
+        double input_vars[] = {cx, cy, theta, wheel_base, width, front_theta};
+        double map_rows = map_cols * 1.0 / j ;
+        update_mat(map_rows, map_cols);
+        // 寻找最小loss值对应的初始旋转角
+        for (int i = -5; i < 6; 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);
+            // LOG(INFO) << "avg loss, current theta: " << t_loss << ", " << input_vars[2];
+            if (avg_loss > t_loss)
+            {
+                avg_loss = t_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;
+                solve_result = current_result;
+            }
+        }
+    }
+    
+    // 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);
 }
 
-bool detect_wheel_ceres::Solve(double& x,double& y,double& theta,double& wheel_base,double& width,double& front_theta)
+bool detect_wheel_ceres::Solve(double& x,double& y,double& theta,double& wheel_base,double& width,double& front_theta,double &avg_loss)
 {
     double SCALE=300.0;
     double cx=x;
@@ -311,20 +357,19 @@ bool detect_wheel_ceres::Solve(double& x,double& y,double& theta,double& wheel_b
     ceres::Solver::Options options;
     options.use_nonmonotonic_steps=false;
     options.linear_solver_type = ceres::DENSE_QR; //配置增量方程的解法
-    options.max_num_iterations=20;
+    options.max_num_iterations=60;
     options.num_threads=1;
     options.minimizer_progress_to_stdout = false;//输出到cout
     ceres::Solver::Summary summary;//优化信息
     ceres::Solve(options, &problem, &summary);//求解!!!
 
-    //std::cout << summary.BriefReport() << "\n";//输出优化的简要信息
+    // std::cout << summary.BriefReport() << "\n";//输出优化的简要信息
 
     /*printf("x:%.3f,y:%.3f,theta:%.3f\nlength:%.3f,width:%.3f\ntheta_front:%.3f\n",
            x[0],x[1],x[2]*180.0/M_PI,x[3],x[4],x[5]*180.0/M_PI);*/
 
     double loss=summary.final_cost/(m_left_front_cloud.size()+m_right_front_cloud.size()+m_left_rear_cloud.size()+m_right_rear_cloud.size());
 
-
     x=variable[0];
     y=variable[1];
     theta=(-variable[2])*180.0/M_PI;
@@ -346,7 +391,11 @@ bool detect_wheel_ceres::Solve(double& x,double& y,double& theta,double& wheel_b
     }
 
     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);
+    // 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传出
+
     return true;
 
-}
+}

wj_lidar/detect_wheel_ceres.h

@@ -20,15 +20,14 @@ public:
                 double& cx,double& cy,double& theta,double& wheel_base,double& width,double& front_theta);
 
 protected:
-    bool Solve(double& x,double& y,double& theta,double& wheel_base,double& width,double& front_theta);
+    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);
 
 private:
     pcl::PointCloud<pcl::PointXYZ>          m_left_front_cloud;     //左前点云
     pcl::PointCloud<pcl::PointXYZ>          m_right_front_cloud;    //右前点云
     pcl::PointCloud<pcl::PointXYZ>          m_left_rear_cloud;      //左后点云
     pcl::PointCloud<pcl::PointXYZ>          m_right_rear_cloud;     //右后点云
-
-public:
     cv::Mat                                 m_map;                  //保存一份地图用作匹配
 };
 

wj_lidar/region_detect.cpp

@@ -253,7 +253,7 @@ Error_manager Region_detector::clustering_only(pcl::PointCloud<pcl::PointXYZ>::P
     pcl::EuclideanClusterExtraction<pcl::PointXYZ> clustering;
     // 设置聚类的最小值 2cm (small values may cause objects to be divided
     // in several clusters, whereas big values may join objects in a same cluster).
-    clustering.setClusterTolerance(0.2);
+    clustering.setClusterTolerance(0.5);
     // 设置聚类的小点数和最大点云数
     clustering.setMinClusterSize(10);
     clustering.setMaxClusterSize(500);

wj_lidar/region_worker.cpp

@@ -93,7 +93,9 @@ Error_manager Region_worker::get_wheel_result(pcl::PointCloud<pcl::PointXYZ>::Pt
     if (m_detector)
     {
         LOG(INFO) << "worker getting result";
-        result = m_detector->detect(cloud_in, x, y, c, wheelbase, width,front_theta);
+		//result = m_detector->detect(cloud_in, x, y, c, wheelbase, width, front_theta);
+		//if(result != SUCCESS)
+			result = m_detector->detect(cloud_in, x, y, c, wheelbase, width);
     }
     else
     {