Browse Source

22-7-16 locater.cpp xc banben include setting

yct 2 years ago
parent
commit
80e6302694

+ 5 - 3
locate/locater.cpp

@@ -288,7 +288,8 @@ Error_manager Locater::measure_height(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_
     pcl::getMinMax3D(*cloud_car,min_point,max_point);
     //限制车高的范围,检验结果
     height=max_point.z;
-    if(height>=1900 || height<=1000)
+    // 20220111 changed by yct, height 1950
+    if(height>=1950 || height<=1000)
     {
         char description[255]={0};
         sprintf(description,"height is out of range, retry :%.2f",height);
@@ -375,7 +376,7 @@ Error_manager Locater::locate(pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud_src,
     t_wheel_centers.push_back(Eigen::Vector2d((width+height_calc_offsetx)/2.0, wheel_base/2.0));
     t_wheel_centers.push_back(Eigen::Vector2d(-(width+height_calc_offsetx)/2.0, -wheel_base/2.0));
     t_wheel_centers.push_back(Eigen::Vector2d((width+height_calc_offsetx)/2.0, -wheel_base/2.0));
-    Eigen::Rotation2Dd t_rot((angle-90.0)*M_PI/180.0);
+    Eigen::Rotation2Dd t_rot((90.0-angle)*M_PI/180.0);
     double min_x=INT_MAX, max_x = 0.0;
     for (int i = 0; i < t_wheel_centers.size(); ++i) {
         t_wheel_centers[i] = t_rot.toRotationMatrix() * t_wheel_centers[i];
@@ -413,12 +414,13 @@ Error_manager Locater::locate(pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud_src,
 
     //第六步,结果赋值
     //角度以度为单位
+    // 20211223 added by yct, car height filtered become valid.
     locate_information.locate_x=center_x;
     locate_information.locate_y=center_y;
     locate_information.locate_theta = angle;
     locate_information.locate_length=wheel_base;
     locate_information.locate_width=width;
-    locate_information.locate_hight=height_car;
+    locate_information.locate_hight=height_car_filtered;
     locate_information.locate_wheel_base=wheel_base;
     locate_information.locate_correct=true;
     LOG(INFO) << "Locate SUCCESS :" << " center:" << "[" << locate_information.locate_x

+ 0 - 0
setting/20210506


+ 147 - 0
setting/laser (copy).prototxt

@@ -0,0 +1,147 @@
+
+#2号雷达
+laser_parameters
+{
+	type:"LivoxMid100"
+	is_save_txt:false
+	sn:"1LVDG1F006V2WU"
+	frame_num:500
+    mat_r00:0.0135042 
+    mat_r01:0.9988871 
+    mat_r02:0.0451919 
+    mat_r03:12198.162 
+    mat_r10:-0.3850948
+    mat_r11:0.0469052 
+    mat_r12:-0.9216841
+    mat_r13:3545.251  
+    mat_r20:-0.9227782
+    mat_r21:-0.0049566
+    mat_r22:0.3852997 
+    mat_r23:6738.30800
+}
+#3号雷达
+
+laser_parameters
+{
+	type:"LivoxMid100"
+	is_save_txt:false
+	sn:"1LVDG1S006N93N"
+	frame_num:1000
+    mat_r00:0.0229183
+    mat_r01:-0.9985044
+    mat_r02:0.0496393
+    mat_r03:5287.829
+    mat_r10:0.2763783
+    mat_r11:0.0540452
+    mat_r12:0.9595283
+    mat_r13:-2399.535
+    mat_r20:-0.9607757
+    mat_r21:-0.0082715
+    mat_r22:0.2772034
+    mat_r23:6674.29500
+}
+#4号雷达
+laser_parameters
+{
+	type:"LivoxMid100"
+	is_save_txt:false
+	sn:"1LVDG1S006G462"
+	frame_num:1000
+    mat_r00:0.0126959
+    mat_r01:0.9992238
+    mat_r02:-0.0372968
+    mat_r03:5264.606 
+    mat_r10:-0.3767520
+    mat_r11:-0.0297706
+    mat_r12:-0.9258357
+    mat_r13:3559.158
+    mat_r20:-0.9262271
+    mat_r21:0.0258060
+    mat_r22:0.3760815
+    mat_r23:6721.93300
+}
+#5号雷达
+laser_parameters
+{
+	type:"LivoxMid100"
+	is_save_txt:false
+	sn:"1LVDG1S006MNJG"
+	frame_num:1100
+    mat_r00:0.3164810
+    mat_r01:-0.9480029
+    mat_r02:0.0336194
+    mat_r03:-1620.378
+    mat_r10:0.2050821
+    mat_r11:0.1029815
+    mat_r12:0.9733119
+    mat_r13:-2364.715
+    mat_r20:-0.9261647
+    mat_r21:-0.3011400
+    mat_r22:0.2270101
+    mat_r23:6703.779000
+}
+
+#6号雷达
+laser_parameters
+{
+	type:"LivoxMid100"
+	is_save_txt:false
+	sn:"1LVDFCE0050446"
+	frame_num:1000
+    mat_r00:0.2753770
+    mat_r01:0.9612699
+    mat_r02:0.0113092
+    mat_r03:-1641.832
+    mat_r10:-0.4067668
+    mat_r11:0.1271702
+    mat_r12:-0.9046370
+    mat_r13:3503.771
+    mat_r20:-0.8710384
+    mat_r21:0.2445160
+    mat_r22:0.4260324
+    mat_r23:6717.29800
+}
+
+#7号雷达
+laser_parameters
+{
+	type:"Horizon"
+	is_save_txt:false
+	sn:"1HDDH3200100691"
+	frame_num:1000
+    mat_r00:-0.357077
+    mat_r01:-0.930381
+    mat_r02:-0.08299
+    mat_r03:19022
+    mat_r10:0.217961
+    mat_r11:-0.16939
+    mat_r12:0.961145
+    mat_r13:-2316
+    mat_r20:-0.908289
+    mat_r21:0.325113
+    mat_r22:0.263272
+    mat_r23:6700
+}
+
+
+#8号雷达
+laser_parameters
+{
+	type:"LivoxMid100"
+	is_save_txt:false
+	sn:"1LVDG1S006Y27R"
+	frame_num:1000
+    mat_r00:-0.2963634
+    mat_r01:0.9547403
+    mat_r02:-0.0252988
+    mat_r03:19077.359
+    mat_r10:-0.2261306
+    mat_r11:-0.0958803
+    mat_r12:-0.9693665
+    mat_r13:3570.490
+    mat_r20:-0.9279189
+    mat_r21:-0.2815638
+    mat_r22:0.2443113
+    mat_r23:6680.68600
+}
+

+ 170 - 0
setting/laser.prototxt

@@ -0,0 +1,170 @@
+
+#1号雷达
+laser_parameters
+{
+	type:"Horizon"
+	is_save_txt:false
+	sn:"3WEDJ5400101001"
+	frame_num:1000
+    mat_r00:0.109071 
+    mat_r01:-0.993999 
+    mat_r02:0.00830208 
+    mat_r03:12200.0 
+    mat_r10:0.253139
+    mat_r11:0.0358515 
+    mat_r12:0.966765
+    mat_r13:-2318.0  
+    mat_r20:-0.961262
+    mat_r21:-0.103345
+    mat_r22:0.25553 
+    mat_r23:6712.0
+}
+
+#2号雷达
+laser_parameters
+{
+	type:"LivoxMid100"
+	is_save_txt:false
+	sn:"1LVDG1F006V2WU"
+	frame_num:500
+    mat_r00:0.0135042 
+    mat_r01:0.9988871 
+    mat_r02:0.0451919 
+    mat_r03:12198.162 
+    mat_r10:-0.3850948
+    mat_r11:0.0469052 
+    mat_r12:-0.9216841
+    mat_r13:3545.251  
+    mat_r20:-0.9227782
+    mat_r21:-0.0049566
+    mat_r22:0.3852997 
+    mat_r23:6738.30800
+}
+#3号雷达
+
+laser_parameters
+{
+	type:"LivoxMid100"
+	is_save_txt:false
+	sn:"1LVDG1S006N93N"
+	frame_num:1000
+    mat_r00:0.0229183
+    mat_r01:-0.9985044
+    mat_r02:0.0496393
+    mat_r03:5287.829
+    mat_r10:0.2763783
+    mat_r11:0.0540452
+    mat_r12:0.9595283
+    mat_r13:-2399.535
+    mat_r20:-0.9607757
+    mat_r21:-0.0082715
+    mat_r22:0.2772034
+    mat_r23:6674.29500
+}
+
+#4号雷达
+laser_parameters
+{
+	type:"Horizon"
+	is_save_txt:false
+	sn:"3WEDJ4700100871"
+	frame_num:1000
+    mat_r00:0.0433356
+    mat_r01:0.998893
+    mat_r02:0.0182734
+    mat_r03:5318
+    mat_r10:-0.262337
+    mat_r11:0.029026
+    mat_r12:-0.96454
+    mat_r13:3502
+    mat_r20:-0.964003
+    mat_r21:0.0370051
+    mat_r22:0.263305
+    mat_r23:6721
+}
+
+#5号雷达
+laser_parameters
+{
+	type:"LivoxMid100"
+	is_save_txt:false
+	sn:"1LVDG1S006MNJG"
+	frame_num:1100
+    mat_r00:0.3164810
+    mat_r01:-0.9480029
+    mat_r02:0.0336194
+    mat_r03:-1620.378
+    mat_r10:0.2050821
+    mat_r11:0.1029815
+    mat_r12:0.9733119
+    mat_r13:-2364.715
+    mat_r20:-0.9261647
+    mat_r21:-0.3011400
+    mat_r22:0.2270101
+    mat_r23:6703.779000
+}
+
+#6号雷达
+laser_parameters
+{
+	type:"LivoxMid100"
+	is_save_txt:false
+	sn:"1LVDFCE0050446"
+	frame_num:1000
+    mat_r00:0.2753770
+    mat_r01:0.9612699
+    mat_r02:0.0113092
+    mat_r03:-1641.832
+    mat_r10:-0.4067668
+    mat_r11:0.1271702
+    mat_r12:-0.9046370
+    mat_r13:3503.771
+    mat_r20:-0.8710384
+    mat_r21:0.2445160
+    mat_r22:0.4260324
+    mat_r23:6717.29800
+}
+
+#7号雷达
+laser_parameters
+{
+	type:"Horizon"
+	is_save_txt:false
+	sn:"1HDDH3200100691"
+	frame_num:1000
+    mat_r00:-0.357077
+    mat_r01:-0.930381
+    mat_r02:-0.08299
+    mat_r03:19022
+    mat_r10:0.217961
+    mat_r11:-0.16939
+    mat_r12:0.961145
+    mat_r13:-2316
+    mat_r20:-0.908289
+    mat_r21:0.325113
+    mat_r22:0.263272
+    mat_r23:6700
+}
+
+
+#8号雷达
+laser_parameters
+{
+	type:"LivoxMid100"
+	is_save_txt:false
+	sn:"1LVDG1S006Y27R"
+	frame_num:1000
+    mat_r00:-0.2963634
+    mat_r01:0.9547403
+    mat_r02:-0.0252988
+    mat_r03:19077.359
+    mat_r10:-0.2261306
+    mat_r11:-0.0958803
+    mat_r12:-0.9693665
+    mat_r13:3570.490
+    mat_r20:-0.9279189
+    mat_r21:-0.2815638
+    mat_r22:0.2443113
+    mat_r23:6680.68600
+}
+

+ 366 - 0
setting/laser_backup

@@ -0,0 +1,366 @@
+
+
+20210506
+#1号雷达
+laser_parameters
+{
+	type:"LivoxMid100"
+	is_save_txt:false
+	sn:"1LVDG410064MEB"
+	frame_num:700
+    mat_r00:-0.0046481
+    mat_r01:-0.9979388
+    mat_r02:0.0640112
+    mat_r03:12200.509
+    mat_r10:0.3061189
+    mat_r11:0.0595189
+    mat_r12:0.9501309
+    mat_r13:-2345.693
+    mat_r20:-0.9519821
+    mat_r21:0.0240113
+    mat_r22:0.3052112
+    mat_r23:6720.61400
+}
+
+
+mat_r00:0.263272
+    mat_r01:-0.930381
+    mat_r02:-0.08299
+    mat_r03:19022
+    mat_r10:0.217961
+    mat_r11:-0.16939
+    mat_r12:0.961145
+    mat_r13:-2316
+    mat_r20:-0.908289
+    mat_r21:0.325113
+    mat_r22:0.263272
+    mat_r23:6700
+
+
+
+
+#7号雷达
+laser_parameters
+{
+	type:"LivoxMid100"
+	is_save_txt:false
+	sn:"1LVDG190069Q14"
+	frame_num:1100
+    mat_r00:-0.357077
+    mat_r01:-930381
+    mat_r02:-0.08299
+    mat_r03:19022
+    mat_r10:0.217961
+    mat_r11:-0.16939
+    mat_r12:0.961145
+    mat_r13:-2316
+    mat_r20:-0.908289
+    mat_r21:0.325113
+    mat_r22:0.263272
+    mat_r23:6700
+}
+
+
+
+-----------------------------------------
+
+#2号雷达
+laser_parameters
+{
+	type:"LivoxMid100"
+	is_save_txt:false
+	sn:"1LVDG1F006V2WU"
+	frame_num:500
+    mat_r00:0.0135042 
+    mat_r01:0.9988871 
+    mat_r02:0.0451919 
+    mat_r03:12198.162 
+    mat_r10:-0.3850948
+    mat_r11:0.0469052 
+    mat_r12:-0.9216841
+    mat_r13:3545.251  
+    mat_r20:-0.9227782
+    mat_r21:-0.0049566
+    mat_r22:0.3852997 
+    mat_r23:6738.30800
+}
+#3号雷达
+3#   94.4597   73.8993    3.29085
+laser_parameters
+{
+	type:"LivoxMid100"
+	is_save_txt:false
+	sn:"1LVDG1S006N93N"
+	frame_num:1000
+    mat_r00:0.0229183
+    mat_r01:-0.9985044
+    mat_r02:0.0496393
+    mat_r03:5287.829
+    mat_r10:0.2763783
+    mat_r11:0.0540452
+    mat_r12:0.9595283
+    mat_r13:-2399.535
+    mat_r20:-0.9607757
+    mat_r21:-0.0082715
+    mat_r22:0.2772034
+    mat_r23:6674.29500
+}
+#4号雷达
+laser_parameters
+{
+	type:"Horizon"
+	is_save_txt:false
+	sn:"3WEDJ4700100871"
+	frame_num:1000
+    mat_r00:0.0444021
+    mat_r01:0.998855
+    mat_r02:0.0178093
+    mat_r03:5315 
+    mat_r10:-0.261135
+    mat_r11:0.0288117
+    mat_r12:-0.964872
+    mat_r13:3559.158
+    mat_r20:-0.964281
+    mat_r21:0.0381917
+    mat_r22:0.262115
+    mat_r23:6701
+}
+#5号雷达
+# 32.9435  67.8446  -52.9897
+laser_parameters
+{
+	type:"LivoxMid100"
+	is_save_txt:false
+	sn:"1LVDG1S006MNJG"
+	frame_num:1100
+    mat_r00:0.3164810
+    mat_r01:-0.9480029
+    mat_r02:0.0336194
+    mat_r03:-1620.378
+    mat_r10:0.2050821
+    mat_r11:0.1029815
+    mat_r12:0.9733119
+    mat_r13:-2364.715
+    mat_r20:-0.9261647
+    mat_r21:-0.3011400
+    mat_r22:0.2270101
+    mat_r23:6703.779000
+}
+
+#6号雷达
+# 124.098  119.42  -150.147
+laser_parameters
+{
+	type:"LivoxMid100"
+	is_save_txt:false
+	sn:"1LVDFCE0050446"
+	frame_num:1000
+    mat_r00:0.2753770
+    mat_r01:0.9612699
+    mat_r02:0.0113092
+    mat_r03:-1641.832
+    mat_r10:-0.4067668
+    mat_r11:0.1271702
+    mat_r12:-0.9046370
+    mat_r13:3503.771
+    mat_r20:-0.8710384
+    mat_r21:0.2445160
+    mat_r22:0.4260324
+    mat_r23:6717.29800
+}
+
+
+#8号雷达
+#37.3443  111.887  130.948
+laser_parameters
+{
+	type:"LivoxMid100"
+	is_save_txt:false
+	sn:"1LVDG1S006Y27R"
+	frame_num:1000
+    mat_r00:-0.2963634
+    mat_r01:0.9547403
+    mat_r02:-0.0252988
+    mat_r03:19077.359
+    mat_r10:-0.2261306
+    mat_r11:-0.0958803
+    mat_r12:-0.9693665
+    mat_r13:3570.490
+    mat_r20:-0.9279189
+    mat_r21:-0.2815638
+    mat_r22:0.2443113
+    mat_r23:6680.68600
+}
+
+########################################### 20210708 ######################################
+
+#1号雷达
+laser_parameters
+{
+	type:"Horizon"
+	is_save_txt:false
+	sn:"3WEDJ5400101001"
+	frame_num:1000
+    mat_r00:0.109071 
+    mat_r01:-0.993999 
+    mat_r02:0.00830208 
+    mat_r03:12200.0 
+    mat_r10:0.253139
+    mat_r11:0.0358515 
+    mat_r12:0.966765
+    mat_r13:-2318.0  
+    mat_r20:-0.961262
+    mat_r21:-0.103345
+    mat_r22:0.25553 
+    mat_r23:6712.0
+}
+
+#2号雷达
+laser_parameters
+{
+	type:"LivoxMid100"
+	is_save_txt:false
+	sn:"1LVDG1F006V2WU"
+	frame_num:500
+    mat_r00:0.0135042 
+    mat_r01:0.9988871 
+    mat_r02:0.0451919 
+    mat_r03:12198.162 
+    mat_r10:-0.3850948
+    mat_r11:0.0469052 
+    mat_r12:-0.9216841
+    mat_r13:3545.251  
+    mat_r20:-0.9227782
+    mat_r21:-0.0049566
+    mat_r22:0.3852997 
+    mat_r23:6738.30800
+}
+#3号雷达
+
+laser_parameters
+{
+	type:"LivoxMid100"
+	is_save_txt:false
+	sn:"1LVDG1S006N93N"
+	frame_num:1000
+    mat_r00:0.0229183
+    mat_r01:-0.9985044
+    mat_r02:0.0496393
+    mat_r03:5287.829
+    mat_r10:0.2763783
+    mat_r11:0.0540452
+    mat_r12:0.9595283
+    mat_r13:-2399.535
+    mat_r20:-0.9607757
+    mat_r21:-0.0082715
+    mat_r22:0.2772034
+    mat_r23:6674.29500
+}
+
+#4号雷达
+laser_parameters
+{
+	type:"Horizon"
+	is_save_txt:false
+	sn:"3WEDJ4700100871"
+	frame_num:1000
+    mat_r00:0.0433356
+    mat_r01:0.998893
+    mat_r02:0.0182734
+    mat_r03:5318
+    mat_r10:-0.262337
+    mat_r11:0.029026
+    mat_r12:-0.96454
+    mat_r13:3502
+    mat_r20:-0.964003
+    mat_r21:0.0370051
+    mat_r22:0.263305
+    mat_r23:6721
+}
+
+#5号雷达
+laser_parameters
+{
+	type:"LivoxMid100"
+	is_save_txt:false
+	sn:"1LVDG1S006MNJG"
+	frame_num:1100
+    mat_r00:0.3164810
+    mat_r01:-0.9480029
+    mat_r02:0.0336194
+    mat_r03:-1620.378
+    mat_r10:0.2050821
+    mat_r11:0.1029815
+    mat_r12:0.9733119
+    mat_r13:-2364.715
+    mat_r20:-0.9261647
+    mat_r21:-0.3011400
+    mat_r22:0.2270101
+    mat_r23:6703.779000
+}
+
+#6号雷达
+laser_parameters
+{
+	type:"LivoxMid100"
+	is_save_txt:false
+	sn:"1LVDFCE0050446"
+	frame_num:1000
+    mat_r00:0.2753770
+    mat_r01:0.9612699
+    mat_r02:0.0113092
+    mat_r03:-1641.832
+    mat_r10:-0.4067668
+    mat_r11:0.1271702
+    mat_r12:-0.9046370
+    mat_r13:3503.771
+    mat_r20:-0.8710384
+    mat_r21:0.2445160
+    mat_r22:0.4260324
+    mat_r23:6717.29800
+}
+
+#7号雷达
+laser_parameters
+{
+	type:"Horizon"
+	is_save_txt:false
+	sn:"1HDDH3200100691"
+	frame_num:1000
+    mat_r00:-0.357077
+    mat_r01:-0.930381
+    mat_r02:-0.08299
+    mat_r03:19022
+    mat_r10:0.217961
+    mat_r11:-0.16939
+    mat_r12:0.961145
+    mat_r13:-2316
+    mat_r20:-0.908289
+    mat_r21:0.325113
+    mat_r22:0.263272
+    mat_r23:6700
+}
+
+
+#8号雷达
+laser_parameters
+{
+	type:"LivoxMid100"
+	is_save_txt:false
+	sn:"1LVDG1S006Y27R"
+	frame_num:1000
+    mat_r00:-0.2963634
+    mat_r01:0.9547403
+    mat_r02:-0.0252988
+    mat_r03:19077.359
+    mat_r10:-0.2261306
+    mat_r11:-0.0958803
+    mat_r12:-0.9693665
+    mat_r13:3570.490
+    mat_r20:-0.9279189
+    mat_r21:-0.2815638
+    mat_r22:0.2443113
+    mat_r23:6680.68600
+}
+
+

+ 147 - 0
setting/laser_copy210516.prototxt

@@ -0,0 +1,147 @@
+
+#2号雷达
+laser_parameters
+{
+	type:"LivoxMid100"
+	is_save_txt:false
+	sn:"1LVDG1F006V2WU"
+	frame_num:500
+    mat_r00:0.0135042 
+    mat_r01:0.9988871 
+    mat_r02:0.0451919 
+    mat_r03:12198.162 
+    mat_r10:-0.3850948
+    mat_r11:0.0469052 
+    mat_r12:-0.9216841
+    mat_r13:3545.251  
+    mat_r20:-0.9227782
+    mat_r21:-0.0049566
+    mat_r22:0.3852997 
+    mat_r23:6738.30800
+}
+#3号雷达
+
+laser_parameters
+{
+	type:"LivoxMid100"
+	is_save_txt:false
+	sn:"1LVDG1S006N93N"
+	frame_num:1000
+    mat_r00:0.0229183
+    mat_r01:-0.9985044
+    mat_r02:0.0496393
+    mat_r03:5287.829
+    mat_r10:0.2763783
+    mat_r11:0.0540452
+    mat_r12:0.9595283
+    mat_r13:-2399.535
+    mat_r20:-0.9607757
+    mat_r21:-0.0082715
+    mat_r22:0.2772034
+    mat_r23:6674.29500
+}
+#4号雷达
+laser_parameters
+{
+	type:"LivoxMid100"
+	is_save_txt:false
+	sn:"1LVDG1S006G462"
+	frame_num:1000
+    mat_r00:0.0126959
+    mat_r01:0.9992238
+    mat_r02:-0.0372968
+    mat_r03:5264.606 
+    mat_r10:-0.3767520
+    mat_r11:-0.0297706
+    mat_r12:-0.9258357
+    mat_r13:3559.158
+    mat_r20:-0.9262271
+    mat_r21:0.0258060
+    mat_r22:0.3760815
+    mat_r23:6721.93300
+}
+#5号雷达
+laser_parameters
+{
+	type:"LivoxMid100"
+	is_save_txt:false
+	sn:"1LVDG1S006MNJG"
+	frame_num:1100
+    mat_r00:0.3164810
+    mat_r01:-0.9480029
+    mat_r02:0.0336194
+    mat_r03:-1620.378
+    mat_r10:0.2050821
+    mat_r11:0.1029815
+    mat_r12:0.9733119
+    mat_r13:-2364.715
+    mat_r20:-0.9261647
+    mat_r21:-0.3011400
+    mat_r22:0.2270101
+    mat_r23:6703.779000
+}
+
+#6号雷达
+laser_parameters
+{
+	type:"LivoxMid100"
+	is_save_txt:false
+	sn:"1LVDFCE0050446"
+	frame_num:1000
+    mat_r00:0.2753770
+    mat_r01:0.9612699
+    mat_r02:0.0113092
+    mat_r03:-1641.832
+    mat_r10:-0.4067668
+    mat_r11:0.1271702
+    mat_r12:-0.9046370
+    mat_r13:3503.771
+    mat_r20:-0.8710384
+    mat_r21:0.2445160
+    mat_r22:0.4260324
+    mat_r23:6717.29800
+}
+
+#7号雷达
+laser_parameters
+{
+	type:"Horizon"
+	is_save_txt:false
+	sn:"1HDDH3200100691"
+	frame_num:1000
+    mat_r00:-0.357077
+    mat_r01:-0.930381
+    mat_r02:-0.08299
+    mat_r03:19022
+    mat_r10:0.217961
+    mat_r11:-0.16939
+    mat_r12:0.961145
+    mat_r13:-2316
+    mat_r20:-0.908289
+    mat_r21:0.325113
+    mat_r22:0.263272
+    mat_r23:6700
+}
+
+
+#8号雷达
+laser_parameters
+{
+	type:"LivoxMid100"
+	is_save_txt:false
+	sn:"1LVDG1S006Y27R"
+	frame_num:1000
+    mat_r00:-0.2963634
+    mat_r01:0.9547403
+    mat_r02:-0.0252988
+    mat_r03:19077.359
+    mat_r10:-0.2261306
+    mat_r11:-0.0958803
+    mat_r12:-0.9693665
+    mat_r13:3570.490
+    mat_r20:-0.9279189
+    mat_r21:-0.2815638
+    mat_r22:0.2443113
+    mat_r23:6680.68600
+}
+

+ 79 - 0
setting/limit.prototxt

@@ -0,0 +1,79 @@
+center_min_x:0
+center_max_x:18800
+center_min_y:260
+center_max_y:900
+
+corner_min_y:-2300
+corner_max_y:3450
+
+height:2000
+
+theta_range
+{
+	min_theta:80
+	max_theta:100
+}
+
+theta_range
+{
+	min_theta:260
+	max_theta:280
+}
+
+#1号栏杆
+railing_parameter
+{
+	pa:1
+	pb:0
+	pc:2265
+	railing_width:30
+}
+#2号栏杆
+railing_parameter
+{
+	pa:1
+	pb:0
+	pc:-2006
+	railing_width:30
+}
+#3号栏杆
+railing_parameter
+{
+	pa:1
+	pb:0
+	pc:-5350
+	railing_width:30
+}
+#4号栏杆
+railing_parameter
+{
+	pa:1
+	pb:0
+	pc:-8724
+	railing_width:30
+}
+#5号栏杆
+railing_parameter
+{
+	pa:1
+	pb:0
+	pc:-12215
+	railing_width:30
+}
+#6号栏杆
+railing_parameter
+{
+	pa:1
+	pb:0
+	pc:-15680
+	railing_width:30
+}
+#7号栏杆
+railing_parameter
+{
+	pa:1
+	pb:0
+	pc:-19000
+	railing_width:30
+}
+

+ 47 - 0
setting/locate.prototxt

@@ -0,0 +1,47 @@
+area{
+    x_min:-1227.5
+    x_max:1802.9
+    y_min:-2789.8
+    y_max:3777.19
+    z_min:0
+    z_max:1800
+}
+
+net_3dcnn_parameter
+{
+    length:224
+    width:224
+    height:96
+    freq:25
+    nclass:3
+    weights_file:"./setting/3dcnn_model.pb"
+}
+
+seg_parameter
+{
+    point_size:8192
+    cls_num:3
+    freq:50.0
+    area
+    {
+        x_min:-1227.50
+    	x_max:1802.9
+    	y_min:-2789.8
+    	y_max:3777.19
+    	z_min:0
+    	z_max:1800
+    }
+    graph:"./setting/seg_model_9000.ckpt.meta"
+    cpkt:"./setting/seg_model_9000.ckpt"
+}
+
+yolo_parameter
+{
+    cfg:"./setting/yolov3-spot2.cfg"
+    weights:"./setting/yolov3-spot2_12000.weights"
+    min_x:-1227.5
+    max_x:10802.9
+    min_y:-2789.8
+    max_y:3777.19
+    freq:25.
+}

+ 3 - 0
setting/plc (copy).prototxt

@@ -0,0 +1,3 @@
+ip:"192.168.0.1"
+port:30001
+slave_id:1

+ 3 - 0
setting/plc.prototxt

@@ -0,0 +1,3 @@
+ip:"192.168.0.1"
+port:30001
+slave_id:1

+ 333 - 0
setting/terminal.prototxt

@@ -0,0 +1,333 @@
+save_root_path:"/home/zx/data/scans"
+#1号终端
+terminor_parameters
+{
+	id:0
+	area_3d
+	{
+		
+		min_x:-1100
+		max_x:2400
+		min_y:-2218
+		max_y:3270
+		min_z:-20
+		max_z:1800
+	}
+	laser_parameter
+	{
+		id:0
+		frame_count:1000
+	}
+	laser_parameter
+	{
+		id:1
+		frame_count:1000
+	}
+	laser_parameter
+	{
+		id:2
+		frame_count:1000
+	}
+	laser_parameter
+	{
+		id:3
+		frame_count:1000
+	}
+	laser_parameter
+	{
+		id:4
+		frame_count:1000
+	}
+	laser_parameter
+	{
+		id:5
+		frame_count:1000
+	}
+	laser_parameter
+	{
+		id:6
+		frame_count:1000
+	}
+	laser_parameter
+	{
+		id:7
+		frame_count:1000
+	}
+	
+}
+#2号终端
+terminor_parameters
+{
+	id:1
+	area_3d
+	{
+		
+		min_x:2100
+		max_x:5600
+		min_y:-2218
+		max_y:3270
+		min_z:-20
+		max_z:1800
+	}
+	laser_parameter
+	{
+		id:0
+		frame_count:1000
+	}
+	laser_parameter
+	{
+		id:1
+		frame_count:1000
+	}
+	laser_parameter
+	{
+		id:2
+		frame_count:1000
+	}
+	laser_parameter
+	{
+		id:3
+		frame_count:1000
+	}
+	laser_parameter
+	{
+		id:4
+		frame_count:1000
+	}
+	laser_parameter
+	{
+		id:5
+		frame_count:1000
+	}
+	laser_parameter
+	{
+		id:6
+		frame_count:1000
+	}
+	laser_parameter
+	{
+		id:7
+		frame_count:1000
+	}
+}
+#3号终端
+terminor_parameters
+{
+	id:2
+	area_3d
+	{
+		
+		min_x:5300
+		max_x:8800
+		min_y:-2218
+		max_y:3270
+		min_z:-20
+		max_z:1800
+	}
+	laser_parameter
+	{
+		id:0
+		frame_count:1000
+	}
+	laser_parameter
+	{
+		id:1
+		frame_count:1000
+	}
+	laser_parameter
+	{
+		id:2
+		frame_count:1000
+	}
+	laser_parameter
+	{
+		id:3
+		frame_count:1000
+	}
+	laser_parameter
+	{
+		id:4
+		frame_count:1000
+	}
+	laser_parameter
+	{
+		id:5
+		frame_count:1000
+	}
+	laser_parameter
+	{
+		id:6
+		frame_count:1000
+	}
+	laser_parameter
+	{
+		id:7
+		frame_count:1000
+	}
+	
+}
+#4号终端
+terminor_parameters
+{
+	id:3
+	area_3d
+	{
+		
+		min_x:8500
+		max_x:12000
+		min_y:-2218
+		max_y:3270
+		min_z:-20
+		max_z:1800
+	}
+	laser_parameter
+	{
+		id:0
+		frame_count:1000
+	}
+	laser_parameter
+	{
+		id:1
+		frame_count:1000
+	}
+	laser_parameter
+	{
+		id:2
+		frame_count:1000
+	}
+	laser_parameter
+	{
+		id:3
+		frame_count:1000
+	}
+	laser_parameter
+	{
+		id:4
+		frame_count:1000
+	}
+	laser_parameter
+	{
+		id:5
+		frame_count:3000
+	}
+	laser_parameter
+	{
+		id:6
+		frame_count:1000
+	}
+	laser_parameter
+	{
+		id:7
+		frame_count:1000
+	}
+}
+#5号终端
+terminor_parameters
+{
+	id:4
+	area_3d
+	{
+		
+		min_x:11700
+		max_x:15200
+		min_y:-2218
+		max_y:3270
+		min_z:-20
+		max_z:1800
+	}
+	laser_parameter
+	{
+		id:0
+		frame_count:1000
+	}
+	laser_parameter
+	{
+		id:1
+		frame_count:1000
+	}
+	laser_parameter
+	{
+		id:2
+		frame_count:1000
+	}
+	laser_parameter
+	{
+		id:3
+		frame_count:1000
+	}
+	laser_parameter
+	{
+		id:4
+		frame_count:1000
+	}
+	laser_parameter
+	{
+		id:5
+		frame_count:1000
+	}
+	laser_parameter
+	{
+		id:6
+		frame_count:1000
+	}
+	laser_parameter
+	{
+		id:7
+		frame_count:1000
+	}
+}
+#6号终端
+terminor_parameters
+{
+	id:5
+	area_3d
+	{
+		
+		min_x:15600
+		max_x:18800
+		min_y:-2218
+		max_y:3270
+		min_z:-20
+		max_z:2000
+	}
+	laser_parameter
+	{
+		id:0
+		frame_count:1000
+	}
+	laser_parameter
+	{
+		id:1
+		frame_count:1000
+	}
+	laser_parameter
+	{
+		id:2
+		frame_count:1000
+	}
+	laser_parameter
+	{
+		id:3
+		frame_count:1000
+	}
+	laser_parameter
+	{
+		id:4
+		frame_count:1000
+	}
+	laser_parameter
+	{
+		id:5
+		frame_count:3000
+	}
+	laser_parameter
+	{
+		id:6
+		frame_count:1000
+	}
+	laser_parameter
+	{
+		id:7
+		frame_count:1000
+	}
+}

+ 517 - 0
setting/wj_manager_settings (copy).prototxt

@@ -0,0 +1,517 @@
+fence_data_path:"/home/zx/data/fence_wj"
+#fence_log_path:"/home/zx/yct/MainStructure/new_electronic_fence/log"
+
+#1
+wj_lidar
+{
+    angle_min:-2.35619449
+    angle_max:2.35619449
+    angle_increment:0.00582
+    time_increment:0.000062
+    range_min:0
+    range_max:30
+    net_config
+    {
+        ip_address:"192.168.2.201"
+        port:2110
+    }
+    transform
+    {
+        m00:-0.100059
+		m01:0.9949814
+		m02:-1.470
+		m10:0.9949814
+		m11:0.1000593
+		m12:-2.285   
+    }
+    scan_limit
+    {
+        dist_limit:8
+        minx:0.1
+        maxx:5.5
+        miny:-4
+        maxy:4
+    }
+}
+
+#2
+wj_lidar
+{
+    angle_min:-2.35619449
+    angle_max:2.35619449
+    angle_increment:0.00582
+    time_increment:0.000062
+    range_min:0
+    range_max:30
+    net_config
+    {
+        ip_address:"192.168.2.202"
+        port:2110
+    }
+    transform
+    {
+        m00:-0.0123952
+		m01:-0.9999232
+		m02:1.851
+		m10:-0.9999232
+		m11:0.0123952
+		m12:3.48100000
+    }
+    scan_limit
+    {
+        dist_limit:8
+        minx:0.1
+        maxx:5.5
+        miny:-4
+        maxy:4
+    }
+}
+
+#3
+wj_lidar
+{
+    angle_min:-2.35619449
+    angle_max:2.35619449
+    angle_increment:0.00582
+    time_increment:0.000062
+    range_min:0
+    range_max:30
+    net_config
+    {
+        ip_address:"192.168.2.203"
+        port:2110
+    }
+    transform
+    {
+		m00:0.034899
+		m01:0.999391
+		m02:5.229
+		m10:0.999391
+		m11:-0.034899
+		m12:-2.3500000
+    }
+    scan_limit
+    {
+        dist_limit:8
+        minx:0.1
+        maxx:5.5
+        miny:-4
+        maxy:4
+    }
+}
+
+#4
+wj_lidar
+{
+    angle_min:-2.35619449
+    angle_max:2.35619449
+    angle_increment:0.00582
+    time_increment:0.000062
+    range_min:0
+    range_max:30
+    net_config
+    {
+        ip_address:"192.168.2.204"
+        port:2110
+    }
+    transform
+    {
+        m00:0.0549574
+		m01:-0.9984888
+		m02:8.717
+		m10:-0.9984888
+		m11:-0.0549574
+		m12:3.54400000
+    }
+    scan_limit
+    {
+        dist_limit:8
+        minx:0.1
+        maxx:5.5
+        miny:-4
+        maxy:4
+    }
+}
+
+#5
+wj_lidar
+{
+    angle_min:-2.35619449
+    angle_max:2.35619449
+    angle_increment:0.00582
+    time_increment:0.000062
+    range_min:0
+    range_max:30
+    net_config
+    {
+        ip_address:"192.168.2.205"
+        port:2110
+    }
+    transform
+    {
+    	m00:0.005236
+		m01:0.999986
+		m02:12.136
+		m10:0.999986
+		m11:-0.005236
+		m12:-2.3190000
+    }
+    scan_limit
+    {
+        dist_limit:8
+        minx:0.1
+        maxx:5.5
+        miny:-4
+        maxy:4
+    }
+}
+
+#6
+wj_lidar
+{
+    angle_min:-2.35619449
+    angle_max:2.35619449
+    angle_increment:0.00582
+    time_increment:0.000062
+    range_min:0
+    range_max:30
+    net_config
+    {
+        ip_address:"192.168.2.206"
+        port:2110
+    }
+    transform
+    {
+        m00:0.085926
+		m01:-0.9963016
+		m02:15.675
+		m10:-0.9963016
+		m11:-0.0859263
+		m12:3.48400000
+    }
+    scan_limit
+    {
+        dist_limit:8
+        minx:0.1
+        maxx:5.5
+        miny:-4
+        maxy:4
+    }
+}
+
+#7
+wj_lidar
+{
+    angle_min:-2.35619449
+    angle_max:2.35619449
+    angle_increment:0.00582
+    time_increment:0.000062
+    range_min:0
+    range_max:30
+    net_config
+    {
+        ip_address:"192.168.2.207"
+        port:2110
+    }
+    transform
+    {
+        m00:-0.0609897
+		m01:0.9981384
+		m02:19.069
+		m10:0.9981384
+		m11:0.0609897
+		m12:-2.3750000
+    }
+    scan_limit
+    {
+        dist_limit:8
+        minx:0.1
+        maxx:5.5
+        miny:-4
+        maxy:4
+    }
+}
+
+#8
+wj_lidar
+{
+    angle_min:-2.35619449
+    angle_max:2.35619449
+    angle_increment:0.00582
+    time_increment:0.000062
+    range_min:0
+    range_max:30
+    net_config
+    {
+        ip_address:"192.168.2.208"
+        port:2110
+    }
+    transform
+    {
+        m00:0.0473273
+		m01:-0.9988794
+		m02:-1.398
+		m10:-0.9988794
+		m11:-0.0473273
+		m12:3.35600000
+    }
+    scan_limit
+    {
+        dist_limit:8
+        minx:0.1
+        maxx:5.5
+        miny:-4
+        maxy:4
+    }
+}
+
+#9
+wj_lidar
+{
+    angle_min:-2.35619449
+    angle_max:2.35619449
+    angle_increment:0.00582
+    time_increment:0.000062
+    range_min:0
+    range_max:30
+    net_config
+    {
+        ip_address:"192.168.2.209"
+        port:2110
+    }
+    transform
+    {
+        m00:-0.0773542
+		m01:0.9970038
+		m02:1.742
+		m10:0.9970038
+		m11:0.0773542
+		m12:-2.5350000
+    }
+    scan_limit
+    {
+        dist_limit:8
+        minx:0.1
+        maxx:5.5
+        miny:-4
+        maxy:4
+    }
+}
+
+#10
+wj_lidar
+{
+    angle_min:-2.35619449
+    angle_max:2.35619449
+    angle_increment:0.00582
+    time_increment:0.000062
+    range_min:0
+    range_max:30
+    net_config
+    {
+        ip_address:"192.168.2.210"
+        port:2110
+    }
+    transform
+    {
+        m00:0.0255674
+		m01:-0.9996730
+		m02:5.233
+		m10:-0.9996730
+		m11:-0.0255674
+		m12:3.44200000
+    }
+    scan_limit
+    {
+        dist_limit:8
+        minx:0.1
+        maxx:5.5
+        miny:-4
+        maxy:4
+    }
+}
+
+#11
+wj_lidar
+{
+    angle_min:-2.35619449
+    angle_max:2.35619449
+    angle_increment:0.00582
+    time_increment:0.000062
+    range_min:0
+    range_max:30
+    net_config
+    {
+        ip_address:"192.168.2.211"
+        port:2110
+    }
+    transform
+    {
+        m00:0.1196470
+		m01:0.9928164
+		m02:8.617
+		m10:0.9928164
+		m11:-0.1196470
+		m12:-2.5510000
+    }
+    scan_limit
+    {
+        dist_limit:8
+        minx:0.1
+        maxx:5.5
+        miny:-4
+        maxy:3.2
+    }
+}
+
+#12
+wj_lidar
+{
+    angle_min:-2.35619449
+    angle_max:2.35619449
+    angle_increment:0.00582
+    time_increment:0.000062
+    range_min:0
+    range_max:30
+    net_config
+    {
+        ip_address:"192.168.2.212"
+        port:2110
+    }
+    transform
+    {
+        m00:0.0158848
+		m01:-0.9998739
+		m02:12.162
+		m10:-0.9998739
+		m11:-0.0158848
+		m12:3.43600000
+    }
+    scan_limit
+    {
+        dist_limit:8
+        minx:0.1
+        maxx:5.5
+        miny:-4
+        maxy:4
+    }
+}
+
+#13
+wj_lidar
+{
+    angle_min:-2.35619449
+    angle_max:2.35619449
+    angle_increment:0.00582
+    time_increment:0.000062
+    range_min:0
+    range_max:30
+    net_config
+    {
+        ip_address:"192.168.2.213"
+        port:2110
+    }
+    transform
+    {
+        m00:-0.0459619
+		m01:0.9989432
+		m02:15.555
+		m10:0.9989432
+		m11:0.0459619
+		m12:-2.5410000
+    }
+    scan_limit
+    {
+        dist_limit:8
+        minx:0.1
+        maxx:5.5
+        miny:-4
+        maxy:3.25
+    }
+}
+
+#14
+wj_lidar
+{
+    angle_min:-2.35619449
+    angle_max:2.35619449
+    angle_increment:0.00582
+    time_increment:0.000062
+    range_min:0
+    range_max:30
+    net_config
+    {
+        ip_address:"192.168.2.214"
+        port:2110
+    }
+    transform
+    {
+        m00:-0.1357627
+		m01:-0.9907414
+		m02:18.616
+		m10:-0.9907414
+		m11:0.1357627
+		m12:3.48700000
+    }
+    scan_limit
+    {
+        dist_limit:8
+        minx:0.1
+        maxx:5
+        miny:-4
+        maxy:4
+    }
+}
+
+
+#1
+regions
+{
+	minx:-0.7
+	maxx:1.8
+	miny:-2.43
+	maxy:3.3
+}
+
+#2
+regions
+{
+	minx:2.5
+	maxx:5.15
+	miny:-2.4
+	maxy:3.43
+}
+
+#3
+regions
+{
+	minx:5.4
+	maxx:8.65
+	miny:-2.40
+	maxy:3.43
+}
+#4
+regions
+{
+    minx:8.8
+    maxx:12
+    miny:-2.40
+    maxy:3.45
+}
+#5
+regions
+{
+    minx:12.25
+    maxx:15.4
+    miny:-2.40
+    maxy:3.28
+}
+#6
+regions
+{
+    minx:15.8
+    maxx:18.8
+    miny:-2.40
+    maxy:3.45
+}
+

+ 454 - 0
setting/wj_manager_settings.prototxt

@@ -0,0 +1,454 @@
+fence_data_path:"/home/zx/data/fence_wj"
+#fence_log_path:"/home/zx/yct/MainStructure/new_electronic_fence/log"
+
+
+#2
+wj_lidar
+{
+    angle_min:-2.35619449
+    angle_max:2.35619449
+    angle_increment:0.00582
+    time_increment:0.000062
+    range_min:0
+    range_max:30
+    net_config
+    {
+        ip_address:"192.168.2.202"
+        port:2110
+    }
+    transform
+    {
+        m00:-0.0123952
+		m01:-0.9999232
+		m02:1.851
+		m10:-0.9999232
+		m11:0.0123952
+		m12:3.48100000
+    }
+    scan_limit
+    {
+        dist_limit:8
+        minx:0.1
+        maxx:5.5
+        miny:-4
+        maxy:4
+    }
+}
+
+#3
+wj_lidar
+{
+    angle_min:-2.35619449
+    angle_max:2.35619449
+    angle_increment:0.00582
+    time_increment:0.000062
+    range_min:0
+    range_max:30
+    net_config
+    {
+        ip_address:"192.168.2.203"
+        port:2110
+    }
+    transform
+	{
+		m00:0.034899
+		m01:0.999391
+		m02:5.229
+		m10:0.999391
+		m11:-0.034899
+		m12:-2.31500000
+	}
+    scan_limit
+    {
+        dist_limit:8
+        minx:0.1
+        maxx:5.5
+        miny:-4
+        maxy:4
+    }
+}
+
+#4
+wj_lidar
+{
+    angle_min:-2.35619449
+    angle_max:2.35619449
+    angle_increment:0.00582
+    time_increment:0.000062
+    range_min:0
+    range_max:30
+    net_config
+    {
+        ip_address:"192.168.2.204"
+        port:2110
+    }
+    transform
+	{
+		m00:0.0549574
+		m01:-0.9984888
+		m02:8.747
+		m10:-0.9984888
+		m11:-0.0549574
+		m12:3.53000000
+	}
+    scan_limit
+    {
+        dist_limit:8
+        minx:0.1
+        maxx:5.5
+        miny:-4
+        maxy:4
+    }
+}
+
+#5
+wj_lidar
+{
+    angle_min:-2.35619449
+    angle_max:2.35619449
+    angle_increment:0.00582
+    time_increment:0.000062
+    range_min:0
+    range_max:30
+    net_config
+    {
+        ip_address:"192.168.2.205"
+        port:2110
+    }
+    transform
+    {
+    	m00:0.005236
+		m01:0.999986
+		m02:12.136
+		m10:0.999986
+		m11:-0.005236
+		m12:-2.3190000
+    }
+    scan_limit
+    {
+        dist_limit:8
+        minx:0.1
+        maxx:5.5
+        miny:-4
+        maxy:4
+    }
+}
+
+#6
+wj_lidar
+{
+    angle_min:-2.35619449
+    angle_max:2.35619449
+    angle_increment:0.00582
+    time_increment:0.000062
+    range_min:0
+    range_max:30
+    net_config
+    {
+        ip_address:"192.168.2.206"
+        port:2110
+    }
+    transform
+    {
+        m00:0.085926
+		m01:-0.9963016
+		m02:15.675
+		m10:-0.9963016
+		m11:-0.0859263
+		m12:3.48400000
+    }
+    scan_limit
+    {
+        dist_limit:8
+        minx:0.1
+        maxx:5.5
+        miny:-4
+        maxy:4
+    }
+}
+
+#7
+wj_lidar
+{
+    angle_min:-2.35619449
+    angle_max:2.35619449
+    angle_increment:0.00582
+    time_increment:0.000062
+    range_min:0
+    range_max:30
+    net_config
+    {
+        ip_address:"192.168.2.207"
+        port:2110
+    }
+    transform
+    {
+        m00:-0.0609897
+		m01:0.9981384
+		m02:19.069
+		m10:0.9981384
+		m11:0.0609897
+		m12:-2.3750000
+    }
+    scan_limit
+    {
+        dist_limit:8
+        minx:0.1
+        maxx:5.5
+        miny:-4
+        maxy:4
+    }
+}
+
+
+
+#9
+wj_lidar
+{
+    angle_min:-2.35619449
+    angle_max:2.35619449
+    angle_increment:0.00582
+    time_increment:0.000062
+    range_min:0
+    range_max:30
+    net_config
+    {
+        ip_address:"192.168.2.209"
+        port:2110
+    }
+    transform
+    {
+        m00:-0.0773542
+		m01:0.9970038
+		m02:1.742
+		m10:0.9970038
+		m11:0.0773542
+		m12:-2.5350000
+    }
+    scan_limit
+    {
+        dist_limit:8
+        minx:0.1
+        maxx:5.5
+        miny:-4
+        maxy:4
+    }
+}
+
+#10
+wj_lidar
+{
+    angle_min:-2.35619449
+    angle_max:2.35619449
+    angle_increment:0.00582
+    time_increment:0.000062
+    range_min:0
+    range_max:30
+    net_config
+    {
+        ip_address:"192.168.2.210"
+        port:2110
+    }
+    transform
+    {
+        m00:0.0255674
+		m01:-0.9996730
+		m02:5.233
+		m10:-0.9996730
+		m11:-0.0255674
+		m12:3.44200000
+    }
+    scan_limit
+    {
+        dist_limit:8
+        minx:0.1
+        maxx:5.5
+        miny:-4
+        maxy:4
+    }
+}
+
+#11
+wj_lidar
+{
+    angle_min:-2.35619449
+    angle_max:2.35619449
+    angle_increment:0.00582
+    time_increment:0.000062
+    range_min:0
+    range_max:30
+    net_config
+    {
+        ip_address:"192.168.2.211"
+        port:2110
+    }
+    transform
+    {
+        m00:0.1196470
+		m01:0.9928164
+		m02:8.617
+		m10:0.9928164
+		m11:-0.1196470
+		m12:-2.5510000
+    }
+    scan_limit
+    {
+        dist_limit:8
+        minx:0.1
+        maxx:5.5
+        miny:-4
+        maxy:3.2
+    }
+}
+
+#12
+wj_lidar
+{
+    angle_min:-2.35619449
+    angle_max:2.35619449
+    angle_increment:0.00582
+    time_increment:0.000062
+    range_min:0
+    range_max:30
+    net_config
+    {
+        ip_address:"192.168.2.212"
+        port:2110
+    }
+    transform
+    {
+        m00:0.0158848
+		m01:-0.9998739
+		m02:12.162
+		m10:-0.9998739
+		m11:-0.0158848
+		m12:3.43600000
+    }
+    scan_limit
+    {
+        dist_limit:8
+        minx:0.1
+        maxx:5.5
+        miny:-4
+        maxy:4
+    }
+}
+
+#13
+wj_lidar
+{
+    angle_min:-2.35619449
+    angle_max:2.35619449
+    angle_increment:0.00582
+    time_increment:0.000062
+    range_min:0
+    range_max:30
+    net_config
+    {
+        ip_address:"192.168.2.213"
+        port:2110
+    }
+    transform
+    {
+        m00:-0.0459619
+		m01:0.9989432
+		m02:15.555
+		m10:0.9989432
+		m11:0.0459619
+		m12:-2.5410000
+    }
+    scan_limit
+    {
+        dist_limit:8
+        minx:0.1
+        maxx:5.5
+        miny:-4
+        maxy:3.25
+    }
+}
+
+#14
+wj_lidar
+{
+    angle_min:-2.35619449
+    angle_max:2.35619449
+    angle_increment:0.00582
+    time_increment:0.000062
+    range_min:0
+    range_max:30
+    net_config
+    {
+        ip_address:"192.168.2.214"
+        port:2110
+    }
+    transform
+    {
+        m00:-0.1357627
+		m01:-0.9907414
+		m02:18.616
+		m10:-0.9907414
+		m11:0.1357627
+		m12:3.48700000
+    }
+    scan_limit
+    {
+        dist_limit:8
+        minx:0.1
+        maxx:5
+        miny:-4
+        maxy:4
+    }
+}
+
+
+#1
+regions
+{
+	minx:-0.7
+	maxx:1.8
+	miny:-2.43
+	maxy:3.3
+}
+
+#2
+regions
+{
+	minx:2.5
+	maxx:5.15
+	miny:-2.4
+	maxy:3.43
+}
+
+#3
+regions
+{
+	minx:5.4
+	maxx:8.65
+	miny:-2.40
+	maxy:3.43
+}
+#4
+regions
+{
+    minx:8.8
+    maxx:12
+    miny:-2.40
+    maxy:3.45
+}
+#5
+regions
+{
+    minx:12.25
+    maxx:15.4
+    miny:-2.40
+    maxy:3.28
+}
+#6
+regions
+{
+    minx:15.8
+    maxx:18.8
+    miny:-2.40
+    maxy:3.45
+}
+

+ 789 - 0
setting/yolov3-spot2.cfg

@@ -0,0 +1,789 @@
+[net]
+# Testing
+# batch=1
+# subdivisions=1
+# Training
+batch=8
+subdivisions=1
+width=416
+height=416
+channels=3
+momentum=0.9
+decay=0.0005
+angle=0
+saturation = 1.5
+exposure = 1.5
+hue=.1
+
+learning_rate=0.001
+burn_in=1000
+max_batches=50000
+policy=steps
+steps=5000,30000,30500,41000
+scales=1.0,.3,.2,.1
+
+[convolutional]
+batch_normalize=1
+filters=32
+size=3
+stride=1
+pad=1
+activation=leaky
+
+# Downsample
+
+[convolutional]
+batch_normalize=1
+filters=64
+size=3
+stride=2
+pad=1
+activation=leaky
+
+[convolutional]
+batch_normalize=1
+filters=32
+size=1
+stride=1
+pad=1
+activation=leaky
+
+[convolutional]
+batch_normalize=1
+filters=64
+size=3
+stride=1
+pad=1
+activation=leaky
+
+[shortcut]
+from=-3
+activation=linear
+
+# Downsample
+
+[convolutional]
+batch_normalize=1
+filters=128
+size=3
+stride=2
+pad=1
+activation=leaky
+
+[convolutional]
+batch_normalize=1
+filters=64
+size=1
+stride=1
+pad=1
+activation=leaky
+
+[convolutional]
+batch_normalize=1
+filters=128
+size=3
+stride=1
+pad=1
+activation=leaky
+
+[shortcut]
+from=-3
+activation=linear
+
+[convolutional]
+batch_normalize=1
+filters=64
+size=1
+stride=1
+pad=1
+activation=leaky
+
+[convolutional]
+batch_normalize=1
+filters=128
+size=3
+stride=1
+pad=1
+activation=leaky
+
+[shortcut]
+from=-3
+activation=linear
+
+# Downsample
+
+[convolutional]
+batch_normalize=1
+filters=256
+size=3
+stride=2
+pad=1
+activation=leaky
+
+[convolutional]
+batch_normalize=1
+filters=128
+size=1
+stride=1
+pad=1
+activation=leaky
+
+[convolutional]
+batch_normalize=1
+filters=256
+size=3
+stride=1
+pad=1
+activation=leaky
+
+[shortcut]
+from=-3
+activation=linear
+
+[convolutional]
+batch_normalize=1
+filters=128
+size=1
+stride=1
+pad=1
+activation=leaky
+
+[convolutional]
+batch_normalize=1
+filters=256
+size=3
+stride=1
+pad=1
+activation=leaky
+
+[shortcut]
+from=-3
+activation=linear
+
+[convolutional]
+batch_normalize=1
+filters=128
+size=1
+stride=1
+pad=1
+activation=leaky
+
+[convolutional]
+batch_normalize=1
+filters=256
+size=3
+stride=1
+pad=1
+activation=leaky
+
+[shortcut]
+from=-3
+activation=linear
+
+[convolutional]
+batch_normalize=1
+filters=128
+size=1
+stride=1
+pad=1
+activation=leaky
+
+[convolutional]
+batch_normalize=1
+filters=256
+size=3
+stride=1
+pad=1
+activation=leaky
+
+[shortcut]
+from=-3
+activation=linear
+
+
+[convolutional]
+batch_normalize=1
+filters=128
+size=1
+stride=1
+pad=1
+activation=leaky
+
+[convolutional]
+batch_normalize=1
+filters=256
+size=3
+stride=1
+pad=1
+activation=leaky
+
+[shortcut]
+from=-3
+activation=linear
+
+[convolutional]
+batch_normalize=1
+filters=128
+size=1
+stride=1
+pad=1
+activation=leaky
+
+[convolutional]
+batch_normalize=1
+filters=256
+size=3
+stride=1
+pad=1
+activation=leaky
+
+[shortcut]
+from=-3
+activation=linear
+
+[convolutional]
+batch_normalize=1
+filters=128
+size=1
+stride=1
+pad=1
+activation=leaky
+
+[convolutional]
+batch_normalize=1
+filters=256
+size=3
+stride=1
+pad=1
+activation=leaky
+
+[shortcut]
+from=-3
+activation=linear
+
+[convolutional]
+batch_normalize=1
+filters=128
+size=1
+stride=1
+pad=1
+activation=leaky
+
+[convolutional]
+batch_normalize=1
+filters=256
+size=3
+stride=1
+pad=1
+activation=leaky
+
+[shortcut]
+from=-3
+activation=linear
+
+# Downsample
+
+[convolutional]
+batch_normalize=1
+filters=512
+size=3
+stride=2
+pad=1
+activation=leaky
+
+[convolutional]
+batch_normalize=1
+filters=256
+size=1
+stride=1
+pad=1
+activation=leaky
+
+[convolutional]
+batch_normalize=1
+filters=512
+size=3
+stride=1
+pad=1
+activation=leaky
+
+[shortcut]
+from=-3
+activation=linear
+
+
+[convolutional]
+batch_normalize=1
+filters=256
+size=1
+stride=1
+pad=1
+activation=leaky
+
+[convolutional]
+batch_normalize=1
+filters=512
+size=3
+stride=1
+pad=1
+activation=leaky
+
+[shortcut]
+from=-3
+activation=linear
+
+
+[convolutional]
+batch_normalize=1
+filters=256
+size=1
+stride=1
+pad=1
+activation=leaky
+
+[convolutional]
+batch_normalize=1
+filters=512
+size=3
+stride=1
+pad=1
+activation=leaky
+
+[shortcut]
+from=-3
+activation=linear
+
+
+[convolutional]
+batch_normalize=1
+filters=256
+size=1
+stride=1
+pad=1
+activation=leaky
+
+[convolutional]
+batch_normalize=1
+filters=512
+size=3
+stride=1
+pad=1
+activation=leaky
+
+[shortcut]
+from=-3
+activation=linear
+
+[convolutional]
+batch_normalize=1
+filters=256
+size=1
+stride=1
+pad=1
+activation=leaky
+
+[convolutional]
+batch_normalize=1
+filters=512
+size=3
+stride=1
+pad=1
+activation=leaky
+
+[shortcut]
+from=-3
+activation=linear
+
+
+[convolutional]
+batch_normalize=1
+filters=256
+size=1
+stride=1
+pad=1
+activation=leaky
+
+[convolutional]
+batch_normalize=1
+filters=512
+size=3
+stride=1
+pad=1
+activation=leaky
+
+[shortcut]
+from=-3
+activation=linear
+
+
+[convolutional]
+batch_normalize=1
+filters=256
+size=1
+stride=1
+pad=1
+activation=leaky
+
+[convolutional]
+batch_normalize=1
+filters=512
+size=3
+stride=1
+pad=1
+activation=leaky
+
+[shortcut]
+from=-3
+activation=linear
+
+[convolutional]
+batch_normalize=1
+filters=256
+size=1
+stride=1
+pad=1
+activation=leaky
+
+[convolutional]
+batch_normalize=1
+filters=512
+size=3
+stride=1
+pad=1
+activation=leaky
+
+[shortcut]
+from=-3
+activation=linear
+
+# Downsample
+
+[convolutional]
+batch_normalize=1
+filters=1024
+size=3
+stride=2
+pad=1
+activation=leaky
+
+[convolutional]
+batch_normalize=1
+filters=512
+size=1
+stride=1
+pad=1
+activation=leaky
+
+[convolutional]
+batch_normalize=1
+filters=1024
+size=3
+stride=1
+pad=1
+activation=leaky
+
+[shortcut]
+from=-3
+activation=linear
+
+[convolutional]
+batch_normalize=1
+filters=512
+size=1
+stride=1
+pad=1
+activation=leaky
+
+[convolutional]
+batch_normalize=1
+filters=1024
+size=3
+stride=1
+pad=1
+activation=leaky
+
+[shortcut]
+from=-3
+activation=linear
+
+[convolutional]
+batch_normalize=1
+filters=512
+size=1
+stride=1
+pad=1
+activation=leaky
+
+[convolutional]
+batch_normalize=1
+filters=1024
+size=3
+stride=1
+pad=1
+activation=leaky
+
+[shortcut]
+from=-3
+activation=linear
+
+[convolutional]
+batch_normalize=1
+filters=512
+size=1
+stride=1
+pad=1
+activation=leaky
+
+[convolutional]
+batch_normalize=1
+filters=1024
+size=3
+stride=1
+pad=1
+activation=leaky
+
+[shortcut]
+from=-3
+activation=linear
+
+######################
+
+[convolutional]
+batch_normalize=1
+filters=512
+size=1
+stride=1
+pad=1
+activation=leaky
+
+[convolutional]
+batch_normalize=1
+size=3
+stride=1
+pad=1
+filters=1024
+activation=leaky
+
+[convolutional]
+batch_normalize=1
+filters=512
+size=1
+stride=1
+pad=1
+activation=leaky
+
+[convolutional]
+batch_normalize=1
+size=3
+stride=1
+pad=1
+filters=1024
+activation=leaky
+
+[convolutional]
+batch_normalize=1
+filters=512
+size=1
+stride=1
+pad=1
+activation=leaky
+
+[convolutional]
+batch_normalize=1
+size=3
+stride=1
+pad=1
+filters=1024
+activation=leaky
+
+[convolutional]
+size=1
+stride=1
+pad=1
+filters=18
+activation=linear
+
+
+[yolo]
+mask = 6,7,8
+anchors = 79.4242,162.0001, 172.0031,91.3043, 97.5744,165.3259, 136.7621,130.6138, 119.1326,156.7095, 133.2467,146.9565, 166.3998,122.6419, 132.2911,156.4967, 150.5914,143.3059
+classes=1
+num=9
+jitter=.2
+ignore_thresh = .5
+truth_thresh = 1
+random=0
+
+
+[route]
+layers = -4
+
+[convolutional]
+batch_normalize=1
+filters=256
+size=1
+stride=1
+pad=1
+activation=leaky
+
+[upsample]
+stride=2
+
+[route]
+layers = -1, 61
+
+
+
+[convolutional]
+batch_normalize=1
+filters=256
+size=1
+stride=1
+pad=1
+activation=leaky
+
+[convolutional]
+batch_normalize=1
+size=3
+stride=1
+pad=1
+filters=512
+activation=leaky
+
+[convolutional]
+batch_normalize=1
+filters=256
+size=1
+stride=1
+pad=1
+activation=leaky
+
+[convolutional]
+batch_normalize=1
+size=3
+stride=1
+pad=1
+filters=512
+activation=leaky
+
+[convolutional]
+batch_normalize=1
+filters=256
+size=1
+stride=1
+pad=1
+activation=leaky
+
+[convolutional]
+batch_normalize=1
+size=3
+stride=1
+pad=1
+filters=512
+activation=leaky
+
+[convolutional]
+size=1
+stride=1
+pad=1
+filters=18
+activation=linear
+
+
+[yolo]
+mask = 3,4,5
+anchors = 79.4242,162.0001, 172.0031,91.3043, 97.5744,165.3259, 136.7621,130.6138, 119.1326,156.7095, 133.2467,146.9565, 166.3998,122.6419, 132.2911,156.4967, 150.5914,143.3059
+classes=1
+num=9
+jitter=.2
+ignore_thresh = .5
+truth_thresh = 1
+random=0
+
+
+
+[route]
+layers = -4
+
+[convolutional]
+batch_normalize=1
+filters=128
+size=1
+stride=1
+pad=1
+activation=leaky
+
+[upsample]
+stride=2
+
+[route]
+layers = -1, 36
+
+
+
+[convolutional]
+batch_normalize=1
+filters=128
+size=1
+stride=1
+pad=1
+activation=leaky
+
+[convolutional]
+batch_normalize=1
+size=3
+stride=1
+pad=1
+filters=256
+activation=leaky
+
+[convolutional]
+batch_normalize=1
+filters=128
+size=1
+stride=1
+pad=1
+activation=leaky
+
+[convolutional]
+batch_normalize=1
+size=3
+stride=1
+pad=1
+filters=256
+activation=leaky
+
+[convolutional]
+batch_normalize=1
+filters=128
+size=1
+stride=1
+pad=1
+activation=leaky
+
+[convolutional]
+batch_normalize=1
+size=3
+stride=1
+pad=1
+filters=256
+activation=leaky
+
+[convolutional]
+size=1
+stride=1	
+pad=1
+filters=18
+activation=linear
+
+
+[yolo]
+mask = 0,1,2
+anchors = 79.4242,162.0001, 172.0031,91.3043, 97.5744,165.3259, 136.7621,130.6138, 119.1326,156.7095, 133.2467,146.9565, 166.3998,122.6419, 132.2911,156.4967, 150.5914,143.3059
+classes=1
+num=9
+jitter=.2
+ignore_thresh = .5
+truth_thresh = 1
+random=0
+

+ 5 - 0
terminor/terminal_command_executor.cpp

@@ -541,17 +541,22 @@ Error_manager Terminor_Command_Executor::scanning_measuring() {
     // 6号位标定地面偏低,高度增加21mm
     // 20210603 2号位加高10mm, 4号位加高5mm
     // 20210925 6号位加高改为13
+    // 20220621 2号位轴距-10, 4号位y+12mm,轴距+20mm, 6号位y+7mm
     if(m_terminor_parameter.id()==5)
     {
         m_measure_information.locate_hight += 13;
+        m_measure_information.locate_y += 7;
     }
     else if(m_terminor_parameter.id()==3)
     {
         m_measure_information.locate_hight += 5;
+        m_measure_information.locate_y +=12;
+        m_measure_information.locate_wheel_base += 20;
     }
     else if(m_terminor_parameter.id()==1)
     {
         m_measure_information.locate_hight += 10;
+        m_measure_information.locate_wheel_base -= 10;
     }
     m_measure_information.locate_correct = true;