calib region 2, change wheel width calc method.

need further test

setting/velodyne_manager.prototxt

@@ -277,7 +277,7 @@ region
 	maxx:1.37
 	miny:-2.6
 	maxy:2.35
-	minz:0.02
+	minz:0.03
 	maxz:0.5
     region_id:3
     turnplate_cx:0.0
@@ -319,11 +319,11 @@ region
 # 2 region
 region
 {
-    minx:-1.4
-	maxx:1.4
+    minx:-1.3
+	maxx:1.37
 	miny:-2.6
 	maxy:2.35
-	minz:0.02
+	minz:0.03
 	maxz:0.5
     region_id:2
     turnplate_cx:0.0
@@ -360,8 +360,8 @@ region
         {
             cx:-1.9389
             cy:-0.01395
-            cz:0.05
-            # p:-1.0
+            cz:0.08
+            p:-1.5
         }
     }
 }

velodyne_lidar/ground_region.cpp

@@ -442,6 +442,9 @@ Error_manager Ground_region::detect(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, d
             cloud_z_solver->push_back(pt);
         }
     }
+    if (cloud_z_solver->size() == 0){
+        return Error_manager(VELODYNE_REGION_EMPTY_CLOUD, NEGLIGIBLE_ERROR, "cloud z no point for outlier removal");
+    }
     //离群点过滤
     sor.setInputCloud(cloud_z_solver);
     sor.setMeanK(5);            //K近邻搜索点个数
@@ -503,6 +506,8 @@ Error_manager Ground_region::detect(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, d
         // LOG_IF(WARNING, m_region.region_id() == 0) << "chassis z: " << chassis_z << ", midz: " << mid_z - height / 2.0;
         // 初值中x使用第一步优化的值
         result.cx = car_pose_x;
+        // 传入整车角度，用于准确判断轮宽，此处需要的是点云的角度，通常逆时针90度到x轴
+        result.theta = -(M_PI_2 - car_pose_theta);
         ret = classify_ceres_detect(cloud_filtered, chassis_z, result);
         // changed by yct, 根据底盘z识别，失败则向下移动直至成功或超过底盘内空中点
         if(ret)

velodyne_lidar/match3d/detect_wheel_ceres3d.cpp

@@ -473,8 +473,9 @@ bool detect_wheel_ceres3d::detect(std::vector<pcl::PointCloud<pcl::PointXYZ>::Pt
 
     Detect_result t_detect_result = detect_result;
     // 寻找最小loss值对应的初始旋转角
+    // LOG(WARNING) <<cloud_all.size()<< ", car angle: "<<t_detect_result.theta<<", "<<(-angle_x ) * M_PI / 180.0;
+    // t_detect_result.theta = (-angle_x ) * M_PI / 180.0;
 
-    t_detect_result.theta = (-angle_x ) * M_PI / 180.0;
     Loss_result t_loss_result;
     t_loss_result.total_avg_loss = 1000;
     //输出角度已变化，需恢复成弧度
@@ -614,7 +615,7 @@ void detect_wheel_ceres3d::save_debug_data(std::string dir )
 }
 
 // 外接矩形估算车轮宽度，以此调整模型比例
-bool detect_wheel_ceres3d::wheel_size_estimate(double &wheel_width, double &wheel_length, float &ratio)
+bool detect_wheel_ceres3d::wheel_size_estimate(double &wheel_width, double &wheel_length, double car_angle, float &ratio)
 {
     // 分别对四轮寻找外接矩形
     int rect_res = 0;
@@ -642,16 +643,54 @@ bool detect_wheel_ceres3d::wheel_size_estimate(double &wheel_width, double &whee
     avg_width /= 2.0;
     avg_length /= 2.0;
     // std::cout << "avg width: " << avg_width << ", avg length: " << avg_length << std::endl;
-    wheel_width = avg_width;
+
+    // 以投影方式计算轮宽
+    Eigen::Vector2d car_angle_vec(cos(-car_angle), sin(-car_angle));
+    double left_min=INT_MAX, left_max=-INT_MAX, right_min=INT_MAX, right_max=-INT_MAX;
+    if(m_left_rear_cloud.size()==0 || m_right_rear_cloud.size()==0)
+    {
+        ratio = 1.0f;
+        return false;
+    }
+    for (size_t i = 0; i < m_left_rear_cloud.size(); i++)
+    {
+        Eigen::Vector2d point_vec(m_left_rear_cloud[i].x, m_left_rear_cloud[i].y);
+        double res = car_angle_vec.dot(point_vec);
+        // std::cout<<"lres: "<<res<<std::endl;
+        if(res<left_min)
+            left_min=res;
+        if(res>left_max)
+            left_max=res;
+    }
+    for (size_t i = 0; i < m_right_rear_cloud.size(); i++)
+    {
+        Eigen::Vector2d point_vec(m_right_rear_cloud[i].x, m_right_rear_cloud[i].y);
+        double res = car_angle_vec.dot(point_vec);
+        // std::cout<<"rres: "<<res<<std::endl;
+        if(res<right_min)
+            right_min=res;
+        if(res>right_max)
+            right_max=res;
+    }
+    double new_wheel_width = (left_max+right_max-left_min-right_min)/2.0;
+    // std::cout << "avg width: " << avg_width << ", new width: " << new_wheel_width << std::endl;
+    
+    wheel_width = new_wheel_width;
     wheel_length = avg_length;
 
-    // 轮宽范围，实际0.185-0.3，通常会少0.03-0.04，因此设定范围0.16-0.26
-    // 比例以半径0.64为模板，范围0.61-0.69
-    float t_model_ratio = float((0.6 + ((wheel_width - 0.16) * 0.1 / 0.1)) / 0.64);
-    t_model_ratio = std::max(0.6f/0.64f, t_model_ratio);
-    t_model_ratio = std::min(0.7f/0.64f, t_model_ratio);
-    ratio = 1.0f / t_model_ratio;
+    // ratio = 1.0f;
 
+    // 轮宽范围，实际0.185-0.3，通常会少0.03-0.04，（新方法不会少），因此设定范围0.175-0.275
+    // 比例以半径0.64为模板，范围0.6-0.7
+    if(wheel_width<0.18||wheel_width>0.3)
+    {
+        ratio = 1.0f;
+    }else{
+        float t_model_ratio = float((0.6 + ((wheel_width - 0.175) * 0.1 / 0.1)) / 0.64);
+        t_model_ratio = std::max(0.6f/0.64f, t_model_ratio);
+        t_model_ratio = std::min(0.7f/0.64f, t_model_ratio);
+        ratio = 1.0f / t_model_ratio;
+    }
     return true;
 }
 
@@ -787,7 +826,7 @@ bool detect_wheel_ceres3d::Solve(Detect_result &detect_result, Loss_result &loss
 {
     // 20220118 added by yct. 计算比例传入模型
     float model_ratio = 1.0f;
-    bool ws_res = wheel_size_estimate(detect_result.single_wheel_width, detect_result.single_wheel_length, model_ratio);
+    // bool ws_res = wheel_size_estimate(detect_result.single_wheel_width, detect_result.single_wheel_length, detect_result.theta, model_ratio);
     // std::cout << "change cloud with ratio: " << model_ratio << std::endl;
 
     double cx=detect_result.cx;

velodyne_lidar/match3d/detect_wheel_ceres3d.h

@@ -89,7 +89,7 @@ protected:
     bool wheebase_estimate(double &wheelbase, double &wheel_width, double &wheel_length);
 
     // 外接矩形估算车轮宽度，以此调整模型比例，此比例变换点云贴合模型
-    bool wheel_size_estimate(double &wheel_width, double &wheel_length, float &ratio);
+    bool wheel_size_estimate(double &wheel_width, double &wheel_length, double car_angle, float &ratio);
 
     // 根据点云计算外接矩形
     bool get_wheel_rect(pcl::PointCloud<pcl::PointXYZ> cloud, cv::RotatedRect &rect, double &theta, double &length, double &width);