lidar 3 calib

setting/calib/3back_pole.txt

@@ -0,0 +1,11 @@
+-2.69199991 3.69499993 0.08000000
+-2.68199992 3.69099998 0.88700002
+-2.68099999 3.69300008 1.05400002
+-2.67600012 3.68700004 -0.08000000
+-2.67700005 3.69000006 1.22200000
+-2.67700005 3.70099998 0.08000000
+-2.66100001 3.68899989 0.88400000
+-2.67300010 3.70900011 1.05499995
+-2.65899992 3.69199991 1.21899998
+-2.66000009 3.70499992 0.08000000
+-2.65899992 3.70600009 0.23899999

setting/calib/3front_pole.txt

@@ -0,0 +1,12 @@
+2.76600003 3.57200003 0.23700000
+2.76099992 3.56100011 0.55299997
+2.76500010 3.56299996 0.71399999
+2.75999999 3.55599999 0.87500000
+2.76900005 3.56399989 1.04200006
+2.76399994 3.55500007 1.20700002
+2.77800012 3.55999994 0.23700000
+2.77399993 3.55399990 0.39399999
+2.77699995 3.55599999 0.55400002
+2.78200006 3.55900002 0.87800002
+2.79600000 3.55900002 0.23700000
+2.79099989 3.55100012 0.39500001

setting/velodyne_manager.prototxt

@@ -81,6 +81,26 @@ velodyne_lidars
 }
 
 # 3 lidar 192.168.1.204
+velodyne_lidars
+{
+    ip:""
+    port:2371
+    model:"VLP16"
+    calibrationFile:"../setting/VLP16db.yaml"
+    lidar_id:3
+    max_range:8.0
+    min_range:0.1
+    min_angle:0
+    max_angle:360
+    rpm:600
+    calib
+    {
+        r:-0.06283
+        p:0.48588
+        y:91.4077
+        cz:0.071
+    }
+}
 
 # 2 lidar 192.168.1.205
 velodyne_lidars
@@ -251,52 +271,96 @@ region
 }
 
 # 3 region
+region
+{
+    minx:-1.5
+	maxx:1.5
+	miny:-2.6
+	maxy:2.3
+	minz:0.0
+	maxz:0.5
+    region_id:3
+    turnplate_cx:0.0
+    turnplate_cy:0.0
+    border_minx:-1.3
+    border_maxx:1.3
+    plc_offsetx:0.0
+    plc_offsety:0.0
+    plc_offset_degree:0.0
+    plc_border_miny:-7.51
+    car_min_width:1.55
+    car_max_width:1.92
+    car_min_wheelbase:2.3
+    car_max_wheelbase:3.15
+    turnplate_angle_limit_anti_clockwise:5.3
+    turnplate_angle_limit_clockwise:5.3
 
-# # 2 region
-# region
-# {
-#     minx:-1.5
-# 	maxx:1.5
-# 	miny:-2.6
-# 	maxy:2.3
-# 	minz:0.03
-# 	maxz:0.5
-#     region_id:2
-#     turnplate_cx:0.0
-#     turnplate_cy:0.0
-#     border_minx:-1.3
-#     border_maxx:1.3
-#     plc_offsetx:0.0
-#     plc_offsety:0.0
-#     plc_offset_degree:0.0
-#     plc_border_miny:-7.51
-#     car_min_width:1.55
-#     car_max_width:1.92
-#     car_min_wheelbase:2.3
-#     car_max_wheelbase:3.15
-#     turnplate_angle_limit_anti_clockwise:5.3
-#     turnplate_angle_limit_clockwise:5.3
+    # 3-->4: -3.80394 (-0.01385+0.02)
+    lidar_exts
+    {
+        lidar_id:4
+        calib
+        {
+            cx:1.902
+            cy:-0.0031
+        }
+    }
+    lidar_exts
+    {
+        lidar_id:3
+        calib
+        {
+            cx:-1.902
+            cy:0.0031
+        }
+    }
+}
 
-#     # 2-->3: 
-#     lidar_exts
-#     {
-#         lidar_id:3
-#         calib
-#         {
-#             cx:0.0
-#             cy:0.0
-#         }
-#     }
-#     lidar_exts
-#     {
-#         lidar_id:2
-#         calib
-#         {
-#             cx:0.0
-#             cy:0.0
-#         }
-#     }
-# }
+# 2 region
+region
+{
+    minx:-1.5
+	maxx:1.5
+	miny:-2.6
+	maxy:2.3
+	minz:0.0
+	maxz:0.5
+    region_id:2
+    turnplate_cx:0.0
+    turnplate_cy:0.0
+    border_minx:-1.3
+    border_maxx:1.3
+    plc_offsetx:0.0
+    plc_offsety:0.0
+    plc_offset_degree:-90
+    plc_border_miny:-7.51
+    car_min_width:1.55
+    car_max_width:1.92
+    car_min_wheelbase:2.3
+    car_max_wheelbase:3.15
+    turnplate_angle_limit_anti_clockwise:5.3
+    turnplate_angle_limit_clockwise:5.3
+
+    # 2-->3: -3.87778 (-0.052905-0.025)
+    lidar_exts
+    {
+        lidar_id:3
+        calib
+        {
+            cx:1.9389
+            cy:0.01395
+        }
+    }
+    lidar_exts
+    {
+        lidar_id:2
+        calib
+        {
+            cx:-1.9389
+            cy:-0.01395
+        }
+    }
+}
 
 # 1 region
 # plcx + -0.06

tests/lidar_calib_tools.cpp

@@ -227,8 +227,8 @@ void matrix_trans()
 
 int main()
 {
-    matrix_trans();
-    return 0;
+    // matrix_trans();
+    // return 0;
 
     // calib process:
     // 1. cc cut and fit ground, get normal and z
@@ -244,15 +244,19 @@ int main()
     // lidar 2 normal [10:43:07] 	- normal: (0.001131,-0.024040,0.999710) z -0.0461051
     // 86.3796  1.37069 -0.15165
 
+    // lidar 3 normal [11:18:44] 	- normal: (0.000833,0.010742,0.999942) z -0.071
+    // 91.4077  -0.614125 -0.0628335
+    // pitch 1.1 ---- 0.485875
+
     pcl::PointCloud<pcl::PointXYZ>::Ptr t_front, t_back;
     t_front = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);
     t_back = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);
-    if (!read_pointcloud("/home/youchen/extra_space/chutian/measure/puai_wj_2021/setting/calib/2front_pole.txt", t_front))
+    if (!read_pointcloud("/home/zx/yct/chutian_measure_2021/setting/calib/3front_pole.txt", t_front))
     {
         std::cout << "read front failed." << std::endl;
         return -1;
     }
-    if(!read_pointcloud("/home/youchen/extra_space/chutian/measure/puai_wj_2021/setting/calib/2back_pole.txt", t_back))
+    if(!read_pointcloud("/home/zx/yct/chutian_measure_2021/setting/calib/3back_pole.txt", t_back))
     {
         std::cout << "read back failed." << std::endl;
         return -1;
@@ -263,8 +267,8 @@ int main()
     // 经过测试，地面法向量不适宜使用杆子获取，还是使用地面环点云拟合获得
     // find_intrinsic((front_direction + back_direction) / 2.0, y_direction);
     // find_intrinsic(Eigen::Vector3d(-0.000392,-0.011935,0.999929), y_direction);
-    find_intrinsic(Eigen::Vector3d(0.001131,-0.024040,0.999710), y_direction);
-
+    // find_intrinsic(Eigen::Vector3d(0.001131,-0.024040,0.999710), y_direction);
+    find_intrinsic(Eigen::Vector3d(0.000833,0.010742,0.999942), y_direction);
     return 0;
 
     std::cout << "------------------------ start calib lidar 1 ----------------------" << std::endl;

velodyne_lidar/region_status_checker.h

@@ -255,7 +255,7 @@ public:
         // std::cout << "111" << std::endl;
         pcl::PointCloud<pcl::PointXYZ>::Ptr t_result_cloud = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);
         Eigen::Matrix4f t_pred_pose, t_res_pose;
-        t_pred_pose.setIdentity();
+        t_pred_pose = Eigen::Matrix4f::Identity();
         if (!mp_icp_svd->ScanMatch(t_curr_cloud_inside_filtered, t_pred_pose, t_result_cloud, t_res_pose))
         {
             return Error_manager(WJ_FILTER_FLUCTUATING, MINOR_ERROR, (std::string("icp匹配失败，终端") + std::to_string(terminal_id)).c_str());