提交更新

script/calc_plane.py

@@ -77,6 +77,17 @@ def gen_rotMat():
   inverse_tmp = np.linalg.inv(tmp)
   print(inverse_tmp)
 
+  N7_1_mat = np.array([[-0.03414222029249569, -0.9993794541775686, 0.008661140874264117,
+      0.0], [0.9993821935410669, -0.0340674395723183, 0.008639490380331674, 0.0],
+    [-0.008339066287306893, 0.00895076134927045, 0.999925169122532, -0.7000000000000002],
+    [0.0, 0.0, 0.0, 1.0]])
+  rotMat['N7_1'] = N7_1_mat
+
+  N1_1_mat = np.array([[-0.0983520936644168, -0.9931814642282579, 0.06258949420820718, 0.0],
+    [0.9948432078839998, -0.09656107514718422, 0.031031443625662798, 0.0], [-0.024776145763589476,
+      0.06531874064794312, 0.9975568177909808, -1.0], [0.0, 0.0, 0.0, 1.0]])
+  rotMat['N1_1'] = N1_1_mat
+
   N2_1_mat = np.array([[0.05045908306549516, -0.997741728436212, 0.04433197799906369, 0.0],
     [0.9959209468126726, 0.05359234819149573, 0.07259013648610471, 0.0], [-0.0748020630460102,
       0.04048831377621809, 0.9963761076077744, 0.2859260540868016], [0.0, 0.0, 0.0,
@@ -112,7 +123,11 @@ def gen_rotMat():
     [0.013142364122603362, -0.02811820027887063, 0.9995182064766736, 0.7000000000000002],
     [0.0, 0.0, 0.0, 1.0]])
   rotMat['N3_2'] = N3_2_mat
-
+  N3_3_mat = np.array([[-0.035337857194336984, -0.998492540408635, 0.041998602324638, 0.0],
+    [0.9993752894121967, -0.03528491485549016, 0.002001423475641178, 0.0], [-0.0005164893035519965,
+      0.04204309137006046, 0.9991156648290768, 0.7000000000000002], [0.0, 0.0, 0.0,
+      1.0]])
+  rotMat['N3_3'] = N3_3_mat
   N4_1_mat = np.array([[-0.444341735964156, -0.8947739313681952, 0.044045810518876544, 0.0],
     [0.8953720752482885, -0.4419458502434798, 0.054705688169885476, 0.0], [-0.029483360492542885,
       0.06374540921880009, 0.9975305781065533, -0.09999999999999964], [0.0, 0.0, 0.0,
@@ -261,8 +276,9 @@ if __name__ == '__main__':
             dirs.append(file_)
     for dir in dirs:
         pcd_list = glob.glob(os.path.join(generate_root_path + dir,"*.pcd"))
-        if len(pcd_list) >=1 :
-            base_file_path = os.path.join(generate_root_path + dir, pcd_list[0])
+        num = 1
+        for pcd_file in pcd_list:
+            base_file_path = os.path.join(generate_root_path + dir, pcd_file)
             g_xyzi,g_pcd,g_converted_pcd = parse_pandarmind_pcd(base_file_path, rotMat[dir])
             # 计算点云图的地面平面
             converted_points = np.array(g_converted_pcd.points)
@@ -272,9 +288,10 @@ if __name__ == '__main__':
 
             seg,m = ground_segmentation(data=cropped_cropped[:,:3])   
             print(m)
-            plane_jsn_path = os.path.join(generate_root_path + dir,"plane.json")
+            plane_jsn_path = os.path.join(generate_root_path + dir,str(num) + "plane.json")
             with open(plane_jsn_path,'w') as file_o:
                 json.dump(m,file_o,cls=NumpyEncoder,indent=4)
+            num += 1
 
 
 

script/generate/N10_1/plane.json

+[
+    -0.0005609857775679018,
+    0.00023758167846802948,
+    0.16787892283940983,
+    0.9858074335874035
+]
\ No newline at end of file

script/generate/N10_1/readme.txt

+效果不好，有车，还下雨。
\ No newline at end of file

script/generate/N10_2/plane.json

+[
+    0.0002424398318833359,
+    0.0005664752101506272,
+    0.16455671953140444,
+    0.98636742970651
+]
\ No newline at end of file

script/generate/N10_2/readme.txt

+1644452108.945200000.pcd效果还行
+1644452110.645129000.pcd效果还行
\ No newline at end of file

script/generate/N1_1/1plane.json

+[
+    -0.0005685242554344202,
+    0.0007067777276572896,
+    0.16504212074054406,
+    0.9862861023186317
+]
\ No newline at end of file

script/generate/N1_1/2plane.json

+[
+    -0.0013618400738119518,
+    0.0006375676031618303,
+    0.16327845961621784,
+    0.9865788785112521
+]
\ No newline at end of file

script/generate/N1_1/readme.txt

+1644558525.008252000.pcd效果不好，路上有车
+1644558526.108550000.pcd效果不好，路上有车
\ No newline at end of file

script/generate/N2_1/1plane.json

+[
+    -0.000234359649692424,
+    0.0006531031619645841,
+    0.1617938840843481,
+    0.9868243296578753
+]
\ No newline at end of file

script/generate/N2_1/2plane.json

+[
+    0.0004672502846234136,
+    -0.000556000465562388,
+    0.15975232419469493,
+    0.9871568606128648
+]
\ No newline at end of file

script/generate/N2_1/readme.txt

+1644445895.469666000.pcd效果不好
+1644445895.569650000.pcd效果不好，还不如上一个
\ No newline at end of file

script/generate/N2_2/1plane.json

+[
+    0.0004347697731658683,
+    -0.00042212636372115876,
+    0.16413500770262993,
+    0.9864376979977169
+]
\ No newline at end of file

script/generate/N2_2/2plane.json

+[
+    0.0015072879157140059,
+    0.0009305050305421803,
+    0.16847083403875954,
+    0.9857050473249146
+]
\ No newline at end of file

script/generate/N2_3/1644446771.601334000plane.json

+[
+    -2.9341730050906774e-05,
+    0.0006318593355426668,
+    0.16073807488473535,
+    0.9869968952206446
+]
\ No newline at end of file

script/generate/N2_3/1644446773.401255000plane.json

+[
+    -2.705443088050278e-05,
+    0.000847692650058053,
+    0.16199273879474216,
+    0.9867915855250323
+]
\ No newline at end of file

script/generate/N2_4/1644447116.283077000plane.json

+[
+    -0.0002080034704835912,
+    -4.3735655539618086e-05,
+    0.16659523993940795,
+    0.9860253449335264
+]
\ No newline at end of file

script/generate/N2_4/1644447117.682559000plane.json

+[
+    -0.00012988148376349472,
+    -0.0007372906367781346,
+    0.1638369552925771,
+    0.9864871472116478
+]
\ No newline at end of file

script/generate/N3_1/1644447918.579127000plane.json

+[
+    0.0009152797824259646,
+    0.004139287871106833,
+    0.1602906952989946,
+    0.9870607486671754
+]
\ No newline at end of file

script/generate/N3_1/1644447918.879316000plane.json

+[
+    0.0010140206876672188,
+    0.004501221827956577,
+    0.16014085040630574,
+    0.9870833899905549
+]
\ No newline at end of file

script/generate/N3_1/readme.txt

+效果不好，有车，还下雨。
\ No newline at end of file

script/generate/N3_2/1644529991.627412000plane.json

+[
+    0.001958597855960349,
+    -6.376508051751396e-05,
+    0.1568864088258193,
+    0.9876147095675474
+]
\ No newline at end of file

script/generate/N3_2/1644529993.427344000plane.json

+[
+    0.004130432596080459,
+    -0.0002901323345926954,
+    0.15809164526426778,
+    0.9874157620007059
+]
\ No newline at end of file

script/generate/N3_3/1644530167.693675000plane.json

+[
+    -5.263943434574616e-06,
+    4.144231579933179e-05,
+    0.16142532844392385,
+    0.9868849282421921
+]
\ No newline at end of file

script/generate/N3_3/1644530170.893063000plane.json

+[
+    -0.00018174382220382534,
+    0.00014806951766390634,
+    0.16187358353020917,
+    0.9868114754094064
+]
\ No newline at end of file

script/generate/N3_3/readme.txt

+效果不好，有车，还下雨。
\ No newline at end of file

script/generate/N4_1/1644449372.414357000plane.json

+[
+    0.00040659855786502716,
+    0.0003301964822562061,
+    0.16354097426152084,
+    0.9865364034770783
+]
\ No newline at end of file

script/generate/N4_1/1644449372.814330000plane.json

+[
+    0.000778132468652352,
+    0.00019230205169502854,
+    0.16400355151573803,
+    0.9864594226931013
+]
\ No newline at end of file

script/generate/N4_1/readme.txt

+效果不好，有车，还下雨。
\ No newline at end of file

script/generate/N5_1/1644530408.884777000plane.json

+[
+    0.0010946873783587471,
+    -0.0007428646451492257,
+    0.16883161330105195,
+    0.9856440210145987
+]
\ No newline at end of file

script/generate/N5_1/1644530409.885106000plane.json

+[
+    0.000746720579032738,
+    -0.0010894298913198654,
+    0.16791953412785998,
+    0.98579982025215
+]
\ No newline at end of file

script/generate/N5_1/readme.txt

+效果不好，有车，还下雨。
\ No newline at end of file

script/generate/N5_2/1644530564.574210000plane.json

+[
+    -0.00022570058130914696,
+    0.00012775020039755918,
+    0.18627113935890693,
+    0.9824983437039823
+]
\ No newline at end of file

script/generate/N5_2/1644530566.174198000plane.json

+[
+    0.0013149704512133843,
+    -0.000715829184069232,
+    0.17361200962421916,
+    0.984812991666708
+]
\ No newline at end of file

script/generate/N5_2/readme.txt

+效果不好，有车，还下雨。
\ No newline at end of file

script/generate/N5_3/1644531092.168831000plane.json

+[
+    0.001235348810612942,
+    0.00738573989339522,
+    0.1528593307683465,
+    0.9882195857989238
+]
\ No newline at end of file

script/generate/N5_3/1644531092.968780000plane.json

+[
+    0.0015323986393459832,
+    0.0011464296042591213,
+    0.18055233848945046,
+    0.9835635162609294
+]
\ No newline at end of file

script/generate/N5_3/readme.txt

+效果不好，有车，还下雨。
\ No newline at end of file

script/generate/N6_1/1644450293.927524000plane.json

+[
+    0.004935488427294054,
+    -0.000403084187866453,
+    0.17110865664462205,
+    0.9852397201181012
+]
\ No newline at end of file

script/generate/N6_1/1644450294.728019000plane.json

+[
+    0.00564065584575377,
+    -0.0005070118010227757,
+    0.17005814198780267,
+    0.9854177562254091
+]
\ No newline at end of file

script/generate/N6_1/readme.txt

+效果不好，有车，还下雨。
\ No newline at end of file

script/generate/N6_2/1644445284.895852000plane.json

+[
+    0.0031352145212830127,
+    -0.00027819197787784836,
+    0.16387873286933452,
+    0.9864754705274076
+]
\ No newline at end of file

script/generate/N6_2/1644445285.695575000plane.json

+[
+    0.00292538683691183,
+    1.2833185924426993e-05,
+    0.16222570716069598,
+    0.98674934095918
+]
\ No newline at end of file

script/generate/N6_2/readme.txt

+效果不好，有车，还下雨。
\ No newline at end of file

script/generate/N6_3/1644531314.829136000plane.json

+[
+    0.001556405577725973,
+    -0.0011561553734968396,
+    0.17111897469093362,
+    0.9852484648083211
+]
\ No newline at end of file

script/generate/N6_3/1644531315.429224000plane.json

+[
+    0.0015026776063407335,
+    9.92765474964887e-05,
+    0.1710073757877932,
+    0.9852685976576899
+]
\ No newline at end of file

script/generate/N7_1/1643385027.128334000plane.json

+[
+    -0.00015585077470143939,
+    0.00018502932843773555,
+    0.16646975849082976,
+    0.9860465308405524
+]
\ No newline at end of file

script/generate/N7_1/1644449111.296929000plane.json

+[
+    0.0001266899117589442,
+    4.3614029698881496e-05,
+    0.1649256118265583,
+    0.9863059994804443
+]
\ No newline at end of file

script/generate/N7_1/1644449111.496915000plane.json

+[
+    -0.000428463371829486,
+    2.5804330418613063e-05,
+    0.16344760256614188,
+    0.9865519230981489
+]
\ No newline at end of file

script/generate/N8_1/1644450365.476402000plane.json

+[
+    -0.0012219332363556886,
+    0.0009004374110446045,
+    0.15507919497906159,
+    0.9879008752786288
+]
\ No newline at end of file

script/generate/N8_1/1644450366.776369000plane.json

+[
+    -0.0014051129189692764,
+    0.0014423722106598202,
+    0.15197616699684363,
+    0.9883821072262682
+]
\ No newline at end of file

script/generate/N9_1/1644450761.150720000plane.json

+[
+    0.0003187577470733684,
+    -6.194217425740103e-05,
+    0.16582555844161367,
+    0.9861550480143539
+]
\ No newline at end of file

script/generate/N9_1/1644450762.750515000plane.json

+[
+    -0.00024145023556524812,
+    0.0010288839816513641,
+    0.16504248927229037,
+    0.9862858915316298
+]
\ No newline at end of file

script/generate/N9_2/1644451049.391497000plane.json

+[
+    0.00010610328873611283,
+    -0.0005013784477689031,
+    0.15286135030590242,
+    0.988247511984928
+]
\ No newline at end of file

script/generate/N9_2/1644451050.591059000plane.json

+[
+    0.00021009125291800339,
+    -0.0006281627782210075,
+    0.15250378903266407,
+    0.9883026639667983
+]
\ No newline at end of file

script/trans_and_export_data_multi_auto.py

@@ -138,7 +138,11 @@ def gen_rotMat():
     [0.013142364122603362, -0.02811820027887063, 0.9995182064766736, 0.7000000000000002],
     [0.0, 0.0, 0.0, 1.0]])
   rotMat['N3_2'] = N3_2_mat
-
+  N3_3_mat = np.array([[-0.035337857194336984, -0.998492540408635, 0.041998602324638, 0.0],
+    [0.9993752894121967, -0.03528491485549016, 0.002001423475641178, 0.0], [-0.0005164893035519965,
+      0.04204309137006046, 0.9991156648290768, 0.7000000000000002], [0.0, 0.0, 0.0,
+      1.0]])
+  rotMat['N3_3'] = N3_3_mat
   N4_1_mat = np.array([[-0.444341735964156, -0.8947739313681952, 0.044045810518876544, 0.0],
     [0.8953720752482885, -0.4419458502434798, 0.054705688169885476, 0.0], [-0.029483360492542885,
       0.06374540921880009, 0.9975305781065533, -0.09999999999999964], [0.0, 0.0, 0.0,