提交计算平面的逻辑

script/calc_plane.py

+import os
+import open3d as o3d
+import json
+import glob
+import numpy as np
+import math
+import random
+import shutil as sh
+import copy
+from scipy.spatial.transform import Rotation as R
+import cv2
+from scipy.spatial import Delaunay
+import scipy
+
+from seg_ww_ground import ground_segmentation
+
+#绕z轴旋转：欧拉角到旋转矩阵
+def rotz(t):
+  """Rotation about the z-axis."""
+  c = np.cos(t)
+  s = np.sin(t)
+  return np.array([[c, -s,  0],
+                    [s,  c,  0],
+                    [0,  0,  1]])
+
+def compute_rot(A,B,C):
+  origin_direction = np.array([0,0,1])
+  nor = np.array([A,B,C])
+  mod = np.linalg.norm(nor)
+  nor = nor / mod
+  angle = np.arccos(np.dot(nor,origin_direction))
+  print(angle,angle* 180/ np.pi)
+
+  p_rot = np.cross(nor,origin_direction)
+  p_rot = p_rot / np.linalg.norm(p_rot)
+
+  sin_ang = np.sin(angle)
+  cos_ang = np.cos(angle)
+  rotMat = np.zeros([4,4])
+  rotMat[0,0] = cos_ang + p_rot[0] * p_rot[0] * (1 - cos_ang)
+  rotMat[0,1] = p_rot[0] * p_rot[1] * ( 1 - cos_ang - p_rot[2] * sin_ang)
+  rotMat[0,2] = p_rot[1] * sin_ang + p_rot[0] * p_rot[2] * (1-cos_ang)
+
+  rotMat[1,0] = p_rot[2] * sin_ang + p_rot[0] * p_rot[1] * (1 - cos_ang)
+  rotMat[1,1] = cos_ang + p_rot[1] * p_rot[1] * (1 - cos_ang)
+  rotMat[1,2] = -p_rot[0] * sin_ang + p_rot[1] * p_rot[2] * ( 1 - cos_ang)
+
+  rotMat[2,0] = -p_rot[1] * sin_ang + p_rot[0] * p_rot[2] * (1 - cos_ang)
+  rotMat[2,1] = p_rot[0] * sin_ang + p_rot[1] * p_rot[2] * ( 1 - cos_ang)
+  rotMat[2,2] = cos_ang + p_rot[2] * p_rot[2] * (1 - cos_ang)
+
+  rotMat[3,3] = 1
+  return angle,rotMat
+
+rotMat = {}
+
+def gen_rotMat():
+  global rotMat
+
+  #生成N7_1这段数据的全局转换矩阵
+  A,B,C,D = 5.10151553e-04,1.30740918e-03,1.89664435e-01,9.81847968e-01
+  #A,B,C,D = -9.39865821e-04,  1.04783823e-03,  1.85715376e-01,  9.82602574e-01
+  ang,hori_rotMat = compute_rot(A,B,C)
+  print(ang,hori_rotMat)
+
+  #ext_theta =  0.5 * np.pi
+  ext_theta =  90 / 180.0 * np.pi
+  kitti_Rot = np.zeros([4,4])
+  kitti_Rot[:3,:3] = rotz(ext_theta)
+  kitti_Rot[2,3] = -1
+  kitti_Rot[3,3] = 1
+  print(kitti_Rot)
+
+  tmp = np.dot(kitti_Rot,hori_rotMat)
+  rotMat['N7_1'] = tmp
+  print(tmp)
+  inverse_tmp = np.linalg.inv(tmp)
+  print(inverse_tmp)
+
+  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,
+      1.0]])
+  rotMat['N2_1'] = N2_1_mat
+
+  N2_2_mat = np.array([[0.07510626206747571, -0.9946697513726462, 0.07064796601834904, 0.0],
+    [0.9968676257061473, 0.07313389683723709, -0.03010597868179836, 0.0], [0.024778745271707994,
+      0.07268781767035316, 0.997046887034447, -0.7000000000000002], [0.0, 0.0, 0.0,
+      1.0]])
+  rotMat['N2_2'] = N2_2_mat
+
+  N2_3_mat = np.array([[0.07568437565247627, -0.9968308870840116, -0.02449607800031248,
+      0.0], [0.9971317454228593, 0.07565174868629727, 0.002257252902847979, 0.0],
+    [-0.0003969282768393359, -0.024596655789107878, 0.9996973776958381, 0.9000000000000004],
+    [0.0, 0.0, 0.0, 1.0]])
+  rotMat['N2_3'] = N2_3_mat
+
+  N2_4_mat = np.array([[0.07516802438907474, -0.996779910756179, 0.0279209172905782, 0.0],
+    [0.9961881280280445, 0.07630731297561882, 0.042265914901034, 0.0], [-0.044260385057339445,
+      0.024637441006419476, 0.9987161833149752, 0.09999999999999964], [0.0, 0.0, 0.0,
+      1.0]])
+  rotMat['N2_4'] = N2_4_mat
+
+  N3_1_mat = np.array([[0.10282893175566048, -0.9927983688656276, 0.06146226136206685, 0.0],
+    [0.9946815801416514, 0.10226437626202323, -0.01226994191701453, 0.0], [0.0058961784994428,
+      0.06239708427073043, 0.9980339868729993, 0.7844127162859094], [0.0, 0.0, 0.0,
+      1.0]])
+  rotMat['N3_1'] = N3_1_mat
+
+  N3_2_mat = np.array([[0.9995180204153945, 0.028485544660163925, -0.012341013341895022,
+      10.0], [-0.02812481342444661, 0.9991986491976914, 0.02847901528053492, 0.0],
+    [0.013142364122603362, -0.02811820027887063, 0.9995182064766736, 0.7000000000000002],
+    [0.0, 0.0, 0.0, 1.0]])
+  rotMat['N3_2'] = N3_2_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,
+      1.0]])
+  rotMat['N4_1'] = N4_1_mat
+
+  N4_2_mat = np.array([[0.9988523295913587, 0.04671359708300838, -0.010576555085760117,
+      10.0], [-0.04648606911938285, 0.9987018755893003, 0.020823281975752502, 0.0],
+    [0.011535555805583356, -0.02030772124045653, 0.9997272265024476, 0.7999999999999998],
+    [0.0, 0.0, 0.0, 1.0]])
+  rotMat['N4_2'] = N4_2_mat
+
+  N5_1_mat = np.array([[-0.08986377281989212, -0.9954705408424266, 0.031030704946254054,
+      0.0], [0.9958706079834863, -0.09021596031025624, -0.010139657780214443, 0.0],
+    [0.012893195460257717, 0.029991379097749554, 0.9994669993004465, 0.5747271756415682],
+    [0.0, 0.0, 0.0, 1.0]])
+  rotMat['N5_1'] = N5_1_mat
+  
+  N5_2_mat = np.array([[-0.09764111729780293, -0.9944741458664936, 0.038566636046517395,
+      0.0], [0.9945996324606676, -0.09887670911792698, -0.03154310547669095, 0.0],
+    [0.035182144930937416, 0.03527845797532399, 0.9987580523234553, 1.2727123965813156],
+    [0.0, 0.0, 0.0, 1.0]])
+  rotMat['N5_2'] = N5_2_mat
+
+  N5_3_mat = np.array([[0.11376454710271973, -0.9928370794231558, 0.03649878826770976, 0.0],
+    [0.9918680257870178, 0.11560991762950971, 0.05321810187391049, 0.0], [-0.05705652674214411,
+      0.030147647805359817, 0.9979156638152981, -0.404451678835378], [0.0, 0.0, 0.0,
+      1.0]])
+  rotMat['N5_3'] = N5_3_mat
+
+  N6_1_mat = np.array([[0.9994973004805413, -0.03086596626934255, 0.007241440351907325,
+      10.0], [0.03082860898771179, 0.9995110809882194, 0.0052149640117460745, 0.0],
+    [-0.007398864777328993, -0.004989098918726587, 0.9999601820532585, 0.9000000000000004],
+    [0.0, 0.0, 0.0, 1.0]])
+  rotMat['N6_1'] = N6_1_mat
+
+  N6_2_mat = np.array([[0.9988722862129534, -0.04214125427326351, -0.021869396973478106,
+      10.0], [0.0409985649537926, 0.99789214163878, -0.05030299521200135, 0.0], [
+      0.023943130694148426, 0.04934965393845363, 0.9984945358632253, 0.9000000000000004],
+    [0.0, 0.0, 0.0, 1.0]])
+  rotMat['N6_2'] = N6_2_mat
+
+  N6_3_mat = np.array([[-0.04889424680162646, -0.9987854830493128, -0.006075481845301646,
+      0.0], [0.9987878973799329, -0.048858057728727665, -0.005968772264798985, 0.0],
+    [0.00566468684698101, -0.00635995636205657, 0.9999637304812602, -0.6553131698852468],
+    [0.0, 0.0, 0.0, 1.0]])
+  rotMat['N6_3'] = N6_3_mat
+
+  N8_1_mat = np.array([[0.06742395876958965, -0.9977243948218384, 0.00020435565303055666,
+      0.0], [0.9976344970690704, 0.06741513105551401, -0.013439135482274529, 0.0],
+    [0.013394776652850061, 0.001109991965790168, 0.9999096698583608, 0.7999999999999998],
+    [0.0, 0.0, 0.0, 1.0]])
+  rotMat['N8_1'] = N8_1_mat
+
+  N9_1_mat = np.array([[-0.19809481806349166, -0.9794499176397107, 0.03789857374567175,
+      0.0], [0.9799804151970427, -0.19712000934332657, 0.02796583177233762, 0.0],
+    [-0.01992056441529652, 0.04267974639159458, 0.998890189340813, 0.9980420980703562],
+    [0.0, 0.0, 0.0, 1.0]])
+  rotMat['N9_1'] = N9_1_mat
+
+  N9_2_mat = np.array([[-0.03297378854159967, -0.9961526066188378, 0.08119552694397052,
+      0.0], [0.9993813319576447, -0.03186786767058145, 0.014879258875841538, 0.0],
+    [-0.01223448420563244, 0.0816359194020958, 0.9965871231656551, -0.11066447945547608],
+    [0.0, 0.0, 0.0, 1.0]])
+  rotMat['N9_2'] = N9_2_mat
+
+  N10_1_mat = np.array([[-0.10138137931389381, -0.9920819078535333, 0.07413031794148582,
+      0.0], [0.9946640529021992, -0.10251230933181928, -0.011603805394292926, 0.0],
+    [0.019111195477614955, 0.07255835269044347, 0.9971810505932539, -0.09999999999999964],
+    [0.0, 0.0, 0.0, 1.0]])
+  rotMat['N10_1'] = N10_1_mat
+
+  N10_2_mat = np.array([[0.051858864388487746, -0.9973430294431006, 0.05116189798663601,
+      0.0], [0.9980146776291314, 0.049924120800806875, -0.0383964243008054, 0.0],
+    [0.03574019335646457, 0.05305152008684534, 0.9979519902256409, 1.259040914953271],
+    [0.0, 0.0, 0.0, 1.0]])
+  rotMat['N10_2'] = N10_2_mat
+
+gen_rotMat()
+
+#按照文本来解析pcd点云文件
+#解析过程中将原始lidar坐标系转换为新虚拟lidar坐标系
+#转换方式：
+#        ln(x) = -l(y);ln(y) = l(x);ln(z) = l(z)
+#        intensity = intensity / 255.0
+def parse_pandarmind_pcd(pandar_pcd_path,rotMat):
+  '''
+  pandar_fp = open(pandar_pcd_path,'r')
+  origin_data = []
+  pc = []
+  for line in pandar_fp.readlines()[11:]:
+    #TODO cur lidar -> virtual kitti lidar
+    x = float(line.split(' ')[1]) * -1 + 5  #将原点移到铁丝网后5米，这样可以把剔除掉的对象拉回来
+    y = float(line.split(' ')[0]) * 1 + 22 #将原点右移22米
+    z = float(line.split(' ')[2])
+
+    intensity = int(line.split(' ')[3])
+    origin_data.append([x,y,z,intensity/255.0])
+    pc.append([x,y,z])
+
+  origin_data = np.array(origin_data,dtype=np.float32)
+  pc = np.array(pc,dtype=np.float32)
+  pcd = o3d.geometry.PointCloud()
+  pcd.points = o3d.utility.Vector3dVector(pc)
+  return origin_data, pcd
+  '''
+  pcd = o3d.io.read_point_cloud(pandar_pcd_path)
+  converted_pcd = copy.deepcopy(pcd)
+  xyz = np.array(pcd.points)
+
+  # convert points to horizontal plane
+  xyz_h = copy.deepcopy(xyz)
+  xyz_h = np.transpose(np.dot(rotMat[:3,:3], np.transpose(xyz_h))) + rotMat[:3,3]
+  xyzi = np.zeros([xyz_h.shape[0],4])
+  xyzi[:,:3] = xyz_h[:,:]
+  converted_pcd.points = o3d.utility.Vector3dVector(xyzi[:,:3])
+  '''
+  xyzi[:,0] = xyz[:,0] - 15
+  xyzi[:,1] = xyz[:,1] - 7
+  xyzi[:,2] = xyz[:,2]
+  pcd.points = o3d.utility.Vector3dVector(xyzi[:,:3])
+  '''
+
+  return xyzi,pcd,converted_pcd
+
+import numpy
+class NumpyEncoder(json.JSONEncoder):  
+    def default(self, obj):  
+        if isinstance(obj, (numpy.int_, numpy.intc, numpy.intp, numpy.int8,  
+            numpy.int16, numpy.int32, numpy.int64, numpy.uint8,  
+            numpy.uint16, numpy.uint32, numpy.uint64)):  
+            return int(obj)  
+        elif isinstance(obj, (numpy.float_, numpy.float16, numpy.float32,numpy.float64)):  
+            return float(obj)  
+        elif isinstance(obj, (numpy.ndarray,)):  
+            return obj.tolist()  
+        return json.JSONEncoder.default(self, obj) 
+
+if __name__ == '__main__':
+
+    generate_root_path = "D:/work/git_workspace/jfxmap_python/script/generate/"
+    dirsAll = os.listdir(generate_root_path)
+    dirs =[]
+    for file_ in dirsAll:
+        if os.path.isdir(generate_root_path + file_) == True:
+            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])
+            g_xyzi,g_pcd,g_converted_pcd = parse_pandarmind_pcd(base_file_path, rotMat[dir])
+            # 计算点云图的地面平面
+            converted_points = np.array(g_converted_pcd.points)
+            b = np.where( (converted_points[:,2] <= -5.0) & (converted_points[:,2] > -7) )
+            cropped_cropped = converted_points[b]
+            print(cropped_cropped.shape)
+
+            seg,m = ground_segmentation(data=cropped_cropped[:,:3])   
+            print(m)
+            plane_jsn_path = os.path.join(generate_root_path + dir,"plane.json")
+            with open(plane_jsn_path,'w') as file_o:
+                json.dump(m,file_o,cls=NumpyEncoder,indent=4)
+
+
+

script/generate/N2_1/plane.json

+[
+    0.0003636187809244265,
+    -0.0005781262674784084,
+    0.15716614157720674,
+    0.987571940414031
+]
\ No newline at end of file

script/generate/N2_3/plane.json

+[
+    -0.00011929093378262973,
+    0.0006075874705990641,
+    0.16106436965517343,
+    0.9869437093546523
+]
\ No newline at end of file

script/generate/N3_1/plane.json

+[
+    0.0008628313253618763,
+    0.004304676791955863,
+    0.15973366550116072,
+    0.9871503843819258
+]
\ No newline at end of file

script/generate/N3_2/plane.json

+[
+    0.0024586087473623722,
+    -3.997675895635217e-05,
+    0.1572215142136272,
+    0.987560301507336
+]
\ No newline at end of file

script/generate/N5_1/plane.json

+[
+    -0.00028697653577780606,
+    0.0012995321221822814,
+    0.16069402933624308,
+    0.9870033727381148
+]
\ No newline at end of file

script/generate/N7_1/plane.json

+[
+    -9.024346469878065e-05,
+    -0.001337693103673818,
+    0.15857135647827952,
+    0.9873466095236849
+]
\ No newline at end of file