update region4 plc offset,

add car pose validation,
add chutian config file.

.gitignore

@@ -142,5 +142,4 @@ CCCoreLibExport.h
 *.pb
 *ckpt*
 *.weights
-*.pb*
-*.prototxt
+*.pb*

CMakeLists.txt

@@ -5,7 +5,7 @@ cmake_minimum_required(VERSION 3.5)
 set (CMAKE_CXX_STANDARD 11)
 set(CMAKE_BUILD_TYPE "Release")
 
-#set(PCL_DIR "/home/youchen/pcl-1.8/share/pcl-1.8")
+set(PCL_DIR "/home/youchen/pcl-1.8/share/pcl-1.8")
 #nanomsg
 find_package(PkgConfig REQUIRED)
 pkg_check_modules(nanomsg REQUIRED nanomsg)

setting/velodyne_manager.prototxt

@@ -117,6 +117,10 @@ region
 }
 
 # 2 region
+# 利用共用雷达标定plc偏移量
+# plc_4 = plc_5 + calib5in5 - calib5in4
+# plc_4x = 1.913 + (-1.9018) - (1.9095) = -1.8983
+# plc_4y = 5.998 + (0.0175) - (-0.019388) = 6.03489
 region
 {
     minx:-1.5
@@ -130,8 +134,8 @@ region
     turnplate_cy:0.0
     border_minx:-1.3
     border_maxx:1.3
-    plc_offsetx:0.0
-    plc_offsety:0.0
+    plc_offsetx:-1.8983
+    plc_offsety:6.03489
     # 4-->5: -3.81895 0.038776
     lidar_exts
     {

setting/velodyne_manager_chutian.prototxt

@@ -0,0 +1,138 @@
+fence_data_path:"/home/youchen/extra_space/chutian/fence_wj"
+#fence_log_path:"/home/zx/yct/MainStructure/new_electronic_fence/log"
+left_model_path:"/home/youchen/extra_space/chutian/measure/puai_wj_2021/setting/left_model.txt"
+right_model_path:"/home/youchen/extra_space/chutian/measure/puai_wj_2021/setting/right_model.txt"
+distribution_mode:false
+
+# 1 lidar
+velodyne_lidars
+{
+    ip:""
+    port:2368
+    model:"VLP16"
+    calibrationFile:"../setting/VLP16db.yaml"
+    lidar_id:0
+    max_range:8.0
+    min_range:0.1
+    min_angle:0
+    max_angle:360
+    rpm:600
+    calib
+    {
+         r:-0.276397
+         p:-0.11717
+         y:89.6003
+         cz:0.05467
+        #r:-0.623165
+        #p:0.601821
+        #y:87.0198
+        #cz:0.101527
+    }
+}
+
+# 2 lidar
+velodyne_lidars
+{
+    ip:""
+    port:2369
+    model:"VLP16"
+    calibrationFile:"../setting/VLP16db.yaml"
+    lidar_id:1
+    max_range:8.0
+    min_range:0.1
+    min_angle:0
+    max_angle:360
+    rpm:600
+    calib
+    {
+        r:-0.202484
+        p:-0.106456
+        y:88.58717
+        cz:0.071634
+        #r:0.462994
+        #p:1.29624
+        #y:96.0048
+        #cz:0.09496
+    }
+}
+
+# # 3 lidar
+# velodyne_lidars
+# {
+#     ip:""
+#     port:2370
+#     model:"VLP16"
+#     calibrationFile:"../setting/VLP16db.yaml"
+#     lidar_id:2
+#     max_range:5.0
+#     min_range:0.1
+#     min_angle:0
+#     max_angle:360
+#     rpm:600
+#     calib
+#     {
+#         y:1.57
+#     }
+# }
+
+# 1 region
+region
+{
+    minx:-1.6
+	maxx:1.6
+	miny:-2.6
+	maxy:2.6
+	minz:0.02
+	maxz:0.2
+    region_id:0
+    lidar_exts
+    {
+        lidar_id:0
+        calib
+        {
+            cx:1.9018
+            cy:-0.0175
+        }
+    }
+    lidar_exts
+    {
+        lidar_id:1
+        calib
+        {
+            #cx:-4.021775
+            #cy:-0.039429
+            cx:-1.9018
+            cy:0.0175
+        }
+    }
+}
+
+# # 2 region
+# region
+# {
+#     minx:-3.1
+# 	maxx:0
+# 	miny:-2.75
+# 	maxy:3.08
+# 	minz:0.01
+# 	maxz:0.2
+#     region_id:1
+#     lidar_exts
+#     {
+#         lidar_id:1
+#         calib
+#         {
+#             cx:0.1
+#             cy:0.3
+#         }
+#     }
+#     lidar_exts
+#     {
+#         lidar_id:2
+#         calib
+#         {
+#             cx:0.1
+#             cy:0.3
+#         }
+#     }
+# }

setting/velodyne_manager_puai_26x.prototxt

@@ -0,0 +1,146 @@
+fence_data_path:"/home/youchen/extra_space/chutian/fence_wj"
+#fence_log_path:"/home/zx/yct/MainStructure/new_electronic_fence/log"
+left_model_path:"/home/youchen/extra_space/chutian/measure/chutian_velo_ws/src/chutian_velo/setting/left_model.txt"
+right_model_path:"/home/youchen/extra_space/chutian/measure/chutian_velo_ws/src/chutian_velo/setting/right_model.txt"
+distribution_mode:false
+
+# 1 lidar
+velodyne_lidars
+{
+    ip:""
+    port:2368
+    model:"VLP16"
+    calibrationFile:"../setting/VLP16db.yaml"
+    lidar_id:0
+    max_range:5.0
+    min_range:0.1
+    min_angle:0
+    max_angle:360
+    rpm:600
+    calib
+    {
+        # r:0.0310211
+        # p:-0.46
+        # y:0.514386
+        r:0.0268911
+        p:-0.46026
+        y:88.7017
+        cz:0.0897851
+
+        # r:-0.623165
+        # p:0.601821
+        # y:87.0198
+        # cz:0.0
+    }
+}
+
+# 2 lidar
+velodyne_lidars
+{
+    ip:""
+    port:2369
+    model:"VLP16"
+    calibrationFile:"../setting/VLP16db.yaml"
+    lidar_id:1
+    max_range:5.0
+    min_range:0.1
+    min_angle:0
+    max_angle:360
+    rpm:600
+    calib
+    {
+        # r:-0.471486
+        # p:-0.599778
+        # y:0.00246781
+        r:-0.471538
+        p:-0.599737
+        y:87.4874
+        cz:0.0986272
+
+        # r:0.462994
+        # p:1.29624
+        # y:96.0048
+        # cz:0.0
+    }
+}
+
+# # 3 lidar
+# velodyne_lidars
+# {
+#     ip:""
+#     port:2370
+#     model:"VLP16"
+#     calibrationFile:"../setting/VLP16db.yaml"
+#     lidar_id:2
+#     max_range:5.0
+#     min_range:0.1
+#     min_angle:0
+#     max_angle:360
+#     rpm:600
+#     calib
+#     {
+#         y:1.57
+#     }
+# }
+
+# 1 region
+region
+{
+    minx:0.5
+	maxx:3.1
+	miny:-2.0
+	maxy:2.75
+	minz:0.045
+	maxz:0.2
+    region_id:0
+    lidar_exts
+    {
+        lidar_id:0
+        calib
+        {
+            cx:0.0
+            cy:0.0
+        }
+    }
+    lidar_exts
+    {
+        lidar_id:1
+        calib
+        {
+            cx:3.5511
+            cy:-0.1036
+            # cx:0.05668
+            # cy:-3.5723
+        }
+    }
+}
+
+# # 2 region
+# region
+# {
+#     minx:-3.1
+# 	maxx:0
+# 	miny:-2.75
+# 	maxy:3.08
+# 	minz:0.01
+# 	maxz:0.2
+#     region_id:1
+#     lidar_exts
+#     {
+#         lidar_id:1
+#         calib
+#         {
+#             cx:0.1
+#             cy:0.3
+#         }
+#     }
+#     lidar_exts
+#     {
+#         lidar_id:2
+#         calib
+#         {
+#             cx:0.1
+#             cy:0.3
+#         }
+#     }
+# }

setting/velodyne_manager_puai_env.prototxt

@@ -0,0 +1,128 @@
+fence_data_path:"/home/youchen/extra_space/chutian/fence_wj"
+#fence_log_path:"/home/zx/yct/MainStructure/new_electronic_fence/log"
+left_model_path:"/home/youchen/extra_space/chutian/measure/chutian_velo_ws/src/chutian_velo/setting/left_model.txt"
+right_model_path:"/home/youchen/extra_space/chutian/measure/chutian_velo_ws/src/chutian_velo/setting/right_model.txt"
+distribution_mode:false
+
+# 1 lidar
+velodyne_lidars
+{
+    ip:""
+    port:2368
+    model:"VLP16"
+    calibrationFile:"../setting/VLP16db.yaml"
+    lidar_id:0
+    max_range:5.0
+    min_range:0.1
+    min_angle:0
+    max_angle:360
+    rpm:600
+    calib
+    {
+		y:89.255064
+		cx:0.0
+		cy:0.0
+    }
+}
+
+# 2 lidar
+velodyne_lidars
+{
+    ip:""
+    port:2369
+    model:"VLP16"
+    calibrationFile:"../setting/VLP16db.yaml"
+    lidar_id:1
+    max_range:5.0
+    min_range:0.1
+    min_angle:0
+    max_angle:360
+    rpm:600
+    calib
+    {
+		y:86.1979
+		cx:3.368067
+		cy:-0.07205
+    }
+}
+
+# # 3 lidar
+# velodyne_lidars
+# {
+#     ip:""
+#     port:2370
+#     model:"VLP16"
+#     calibrationFile:"../setting/VLP16db.yaml"
+#     lidar_id:2
+#     max_range:5.0
+#     min_range:0.1
+#     min_angle:0
+#     max_angle:360
+#     rpm:600
+#     calib
+#     {
+#         y:1.57
+#     }
+# }
+
+# 1 region
+region
+{
+	minx:0.2
+	maxx:3.25
+	miny:-2.35
+	maxy:2.7
+	minz:0.02
+	maxz:0.2
+    region_id:0
+    lidar_exts
+    {
+        lidar_id:0
+        calib
+        {
+            cx:0.0
+            cy:0.0
+        }
+    }
+    lidar_exts
+    {
+        lidar_id:1
+        calib
+        {
+            cx:0.0
+            cy:0.0
+            # cx:0.05668
+            # cy:-3.5723
+        }
+    }
+}
+
+# # 2 region
+# region
+# {
+#     minx:-3.1
+# 	maxx:0
+# 	miny:-2.75
+# 	maxy:3.08
+# 	minz:0.01
+# 	maxz:0.2
+#     region_id:1
+#     lidar_exts
+#     {
+#         lidar_id:1
+#         calib
+#         {
+#             cx:0.1
+#             cy:0.3
+#         }
+#     }
+#     lidar_exts
+#     {
+#         lidar_id:2
+#         calib
+#         {
+#             cx:0.1
+#             cy:0.3
+#         }
+#     }
+# }

velodyne_lidar/ground_region.cpp

@@ -381,8 +381,8 @@ Error_manager Ground_region::detect(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, d
 
     // 3.*********位姿优化，获取中心xy与角度*********
     pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_detect_z(new pcl::PointCloud<pcl::PointXYZ>);
-    double x, y, theta, width, z_value=0.2;
-    if(!Car_pose_detector::get_instance_references().detect_pose(cloud_filtered, cloud_detect_z, x, y, theta, width, z_value, false))
+    double car_pose_x, car_pose_y, car_pose_theta, car_pose_width, z_value=0.2;
+    if(!Car_pose_detector::get_instance_references().detect_pose(cloud_filtered, cloud_detect_z, car_pose_x, car_pose_y, car_pose_theta, car_pose_width, z_value, false))
     {
         return Error_manager(VELODYNE_REGION_CERES_SOLVE_ERROR, NEGLIGIBLE_ERROR, "find general car pose value failed.");
     }
@@ -393,6 +393,10 @@ Error_manager Ground_region::detect(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, d
 
     // 4.*********xoz优化获取底盘高度*********
     // 重新取包含地面点的点云,用于底盘优化
+    double z_solver_x = car_pose_x;
+    double z_solver_y = car_pose_y;
+    double z_solver_theta = car_pose_theta;
+    double z_solver_width = 1.0;
     chassis_ceres_solver t_solver;
     pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_z_solver(new pcl::PointCloud<pcl::PointXYZ>);
     for (int i = 0; i < cloud->size(); ++i)
@@ -409,18 +413,17 @@ Error_manager Ground_region::detect(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, d
     sor.setStddevMulThresh(3.0); //标准差倍数
     sor.setNegative(false);      //保留未滤波点（内点）
     sor.filter(*cloud_z_solver); //保存滤波结果到cloud_filter
-
-    double mid_z = 0.05, height = 0.08;
     // 去中心，角度调正
-    Car_pose_detector::get_instance_references().inv_trans_cloud(cloud_z_solver, x, y, theta);
+    Car_pose_detector::get_instance_references().inv_trans_cloud(cloud_z_solver, z_solver_x, z_solver_y, z_solver_theta);
 
     if (cloud_z_solver->size() == 0)
         return Error_manager(VELODYNE_REGION_EMPTY_CLOUD, NORMAL, "z solver no point");
 
-    x = 0.0;
-    width = 1.0;
+    // 给予底盘z中心与高度初值
+    double mid_z = 0.05, height = 0.08;
+    z_solver_x = 0.0;
     // Error_manager ec = t_solver.solve(cloud_z_solver, x, mid_z, width, height);
-    Error_manager ec = t_solver.solve_mat(cloud_z_solver, x, mid_z, width, height, false);
+    Error_manager ec = t_solver.solve_mat(cloud_z_solver, z_solver_x, mid_z, z_solver_width, height, false);
     // 切除大于height高度以外点，并显示width直线
     // 根据z值切原始点云
     pcl::PassThrough<pcl::PointXYZ> pass;
@@ -523,6 +526,15 @@ Error_manager Ground_region::detect(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, d
     // last_result.cy -= y;
     // last_result.theta -= theta;
 
+    // 角度校验，理论上car pose检测使用车身点云，应与车轮点云识别得到的车身 x偏移(0.025)与角度(<1°)一致
+    double car_pose_theta_deg = 90 - car_pose_theta * 180.0 / M_PI;
+    if (fabs(car_pose_x - last_result.cx) > 0.025 || fabs(car_pose_theta_deg - last_result.theta) > 1)
+    {
+        char valid_info[255];
+        sprintf(valid_info, "validation failed for x and theta, carpose: (%.3f, %.3f), result: (%.3f, %.3f)", car_pose_x, car_pose_theta_deg, last_result.cx, last_result.theta);
+        return Error_manager(VELODYNE_REGION_CERES_SOLVE_ERROR, NEGLIGIBLE_ERROR, valid_info);
+    }
+
     return SUCCESS;
 }
 
@@ -673,7 +685,7 @@ void Ground_region::thread_measure_func()
             else
             {
                 m_car_wheel_information.correctness = false;
-                LOG(ERROR) << ec.to_string();
+                LOG_IF(ERROR, m_region.region_id() == 4) << ec.to_string();
             }
             m_detect_update_time = std::chrono::system_clock::now();
         }