Skip to content

J
jfxmap_python
Commit e6f31465 authored by oscar's avatar oscar
Browse files
Options

提交代码修改

parent 21abad1e
Hide whitespace changes
Inline Side-by-side
Showing with 440 additions and 6 deletions
script/to_kitti_Nodes.py
View file @ e6f31465
...@@ -483,10 +483,10 @@ if __name__ == '__main__': ...@@ -483,10 +483,10 @@ if __name__ == '__main__':
total_ind = 0 total_ind = 0
for child_ind,child_dir in enumerate(children_dir): for child_ind,child_dir in enumerate(children_dir):
pcd_list = glob.glob(os.path.join(root_path,child_dir)+'\\pcd\\*.pcd') pcd_list = glob.glob(os.path.join(root_path,child_dir)+'\\pcd\\*.pcd') #获取子目录下的所有pcd文件列表，其中每个元素都有完整的路径和文件名
#读标注的json文件 #读标注的json文件
anno = {} anno = {} #保存标记的信息列表，每个元素key是文件名，value是所有标记的信息列表
jsn_file = os.path.join(os.path.join(root_path,child_dir), child_dir + ".json") jsn_file = os.path.join(os.path.join(root_path,child_dir), child_dir + ".json")
with open(jsn_file, 'r',encoding='utf-8',errors='ignore') as fp: with open(jsn_file, 'r',encoding='utf-8',errors='ignore') as fp:
jsn = json.load(fp) jsn = json.load(fp)
......
script/trans_data.py
View file @ e6f31465
import os import os
import open3d as o3d
import json import json
import glob 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
def compute_pitch(A,B,C):
origin_direction = [0,0,1]
nor = [A,B,C]
angle = np.arccos(np.dot(origin_direction,nor) / np.linalg.norm(nor))
print(angle,angle* 180/ np.pi)
return angle
#绕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()
#根据quart和计算出的原始坐标系的corners_3d来
#计算转换到新虚拟坐标系下的heading和heading平面
def my_compute_heading_plane(quart,corners_3d):
rot = R.from_quat(quart)
euler = rot.as_euler('zxy', degrees=False) #航向角
#点的方向按照逆时针顺序给出，保证算出的法向量垂直于平面向外
six_planes = [[corners_3d[1],corners_3d[5],corners_3d[6],corners_3d[2]],\
[corners_3d[0],corners_3d[4],corners_3d[5],corners_3d[1]],\
[corners_3d[3],corners_3d[7],corners_3d[4],corners_3d[0]],\
[corners_3d[2],corners_3d[6],corners_3d[7],corners_3d[3]],\
[corners_3d[0],corners_3d[1],corners_3d[2],corners_3d[3]],\
[corners_3d[7],corners_3d[6],corners_3d[5],corners_3d[4]] ]
normals = []
for it in six_planes:
it = np.array(it)
nor = np.cross(it[1] - it[2],it[0] - it[1])
normals.append(nor)
origin_direction = [1,0,0]
ret_ind = -1
for ind,nor in enumerate(normals):
direction = np.arccos(np.dot(origin_direction,nor) / np.linalg.norm(nor))
if abs(direction - abs(euler[0])) < 0.5:
ret_ind = ind
#print('debug:',direction,ind)
break
try:
assert(ret_ind == 0)
except:
print("wrong annotation")
return False, [], []
#new_heading = origin_lidar_heading_to_new_virtual(euler[0])
new_heading = euler[0]
new_heading_plane = copy.deepcopy(six_planes[ret_ind])
'''
nhp_len = len(new_heading_plane)
for ii in range(nhp_len):
#TODO cur lidar -> virtual kitti lidar
new_heading_plane[ii][0] = new_heading_plane[ii][0] - 15.0
new_heading_plane[ii][1] = new_heading_plane[ii][1] - 7.0
'''
return True,new_heading, new_heading_plane
#根据标注的3d框的center、quart、size计算3d bbox的8个顶点
#此处计算的是原始lidar坐标系下的坐标
def my_compute_box_3d(center, quart, size):
# P' = T * R * S * p
#其中p为box3d局部坐标，P'为变换后的世界坐标，假设初始box3d局部坐标系与lidar世界坐标系重合
#标准的box边长为1，正方体
x_corners = [-1,1,1,-1,-1,1,1,-1]
y_corners = [1,1,-1,-1,1,1,-1,-1]
z_corners = [1,1,1,1,-1,-1,-1,-1]
tmp = np.vstack([x_corners, y_corners, z_corners]) / 2
corners_3d = np.ones([4,8])
corners_3d[:3,:] = tmp
S = np.diag([size[0],size[1],size[2],1])
rot = R.from_quat(quart)
TR = rot.as_matrix()
Trans = np.zeros([4,4])
Trans[:3,:3] = TR
Trans[0,3] = center[0]
Trans[1,3] = center[1]
Trans[2,3] = center[2]
tmp4x4 = np.dot(S,corners_3d)
world_corners_3d = np.dot(Trans,tmp4x4)
return np.transpose(world_corners_3d[:3,:])
#读入原始lidar坐标系的标注信息
#将原始lidar坐标系转化为标准kitti坐标系
#转换方式：
# 所有corners_3d、center坐标按照“ln(x) = -l(y);ln(y) = l(x);ln(z) = l(z)”来转换
# 新的虚拟new_heading = np.pi + heading if -np.pi < heading < 0;
# 新的虚拟new_heading = heading - np.pi if 0 < heading < np.pi;
########
#保持不变的信息：
# category、size
def read_all_3d_bboxes(box3d,rotMat):
bboxes = []
for item in box3d:
center = [item['box3D']['translation']['x'],item['box3D']['translation']['y'],item['box3D']['translation']['z']]
center = np.array(center, np.float32)
center = np.transpose(np.dot(rotMat[:3,:3],np.transpose(center))) + rotMat[:3,3]
quart = [item['box3D']['rotation']['x'],item['box3D']['rotation']['y'],item['box3D']['rotation']['z'],item['box3D']['rotation']['w']]
origin_rot = R.from_quat(quart)
origin_rot_mat = origin_rot.as_matrix()
new_rot_mat = np.dot(rotMat[:3,:3],origin_rot_mat)
tmp = R.from_matrix(new_rot_mat)
quart = tmp.as_quat()
size = [item['box3D']['size']['x'],item['box3D']['size']['y'],item['box3D']['size']['z']]
size = np.array(size, np.float32)
if size[0] < 0.4 or size[1] < 0.4 or size[2] < 0.4:
continue
#计算原始lidar坐标系中的corners_3d
corners_3d = my_compute_box_3d(center, quart, size)
category = item['category']
assert_flag, heading, heading_plane = my_compute_heading_plane(quart,corners_3d)
if assert_flag == False:
return False, []
#以上为原始lidar坐标系的数据，下面要把bbox转成虚拟lidar坐标系的数据
#TODO cur lidar -> virtual kitti lidar
#new x = old x - 15 new y = old y - 7
#corners_3d[:,0] = corners_3d[:,0] - 15.0
#corners_3d[:,1] = corners_3d[:,1] - 7.0
#TODO cur lidar -> virtual kitti lidar
#center[0] = center[0] - 15.0
#center[1] = center[1] - 7.0
#特别注意，下面的heading和heading_plane是虚拟lidar坐标系下的heading
bboxes.append([category, corners_3d, heading, heading_plane, center, quart, size])
return True, bboxes
#按照文本来解析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
#根据corners_3d来生成open3d渲染要用的12条边
def gen_o3d_3dbboxes(corners_):
bbox_lines = [[0, 1], [1, 2], [2, 3], [3, 0], [4, 5], [5, 6], [6, 7], [7, 4], [0, 4], [1, 5], [2, 6], [3, 7]]
colors = [[1, 0, 0] for _ in range(len(bbox_lines))] #red
bbox = o3d.geometry.LineSet()
bbox.lines = o3d.utility.Vector2iVector(bbox_lines)
bbox.colors = o3d.utility.Vector3dVector(colors)
bbox.points = o3d.utility.Vector3dVector(corners_)
return bbox
def custom_draw_geometry_with_key_callback(pcd, file_path):
def save_pcd(vis):
#io.write_point_cloud(file_path, pcd, write_ascii=True)
#print(file_path)
return False
key_to_callback = {}
key_to_callback[ord("S")] = save_pcd
o3d.visualization.draw_geometries_with_key_callbacks(pcd, key_to_callback)
if __name__ == '__main__': if __name__ == '__main__':
jsn_file = "./data_aug_samples/N10_1.json" root_path = "D:/work/git_workspace/jfxmap_python/script/data_aug_samples/"
child_dir = "N10_1"
#读标注的json文件
anno = {}
jsn_file = os.path.join(root_path, child_dir + ".json")
print(jsn_file)
vis = True
with open(jsn_file, 'r',encoding='utf-8',errors='ignore') as fp: with open(jsn_file, 'r',encoding='utf-8',errors='ignore') as fp:
jsn = json.load(fp) jsn = json.load(fp)
print(jsn) annos = jsn["annotations"]
for an in annos:
\ No newline at end of file fileuri = "10-1_pcds/" + an['fileuri'].split("/")[-1]
fileuri_path = os.path.join(root_path,fileuri)
# print(fileuri_path)
if os.path.isfile(fileuri_path):
print(fileuri_path," ",fileuri)
anno[fileuri_path] = an['labels_box3D']
for file_path,boxx in anno.items():
# print(file_path)
# print(boxx)
bbox_flag,bboxes = read_all_3d_bboxes(boxx, np.identity(4))
if bbox_flag == False or len(bboxes) < 1:
continue
# print(bbox_flag)
# print(bboxes)
# 地面转成水平面，地面点的z值几乎差不多
xyzi,pcd,converted_pcd = parse_pandarmind_pcd(file_path, rotMat[child_dir])
print(xyzi)
print(pcd)
print(converted_pcd)
merge_geos = [pcd]
for bbox in bboxes:
#渲染bbox
merge_geos += [gen_o3d_3dbboxes(bbox[1])]
heading_point = (bbox[3][0] + bbox[3][2]) / 2
heading_pcd = o3d.geometry.PointCloud()
heading_pcd.points = o3d.utility.Vector3dVector(np.array([heading_point]))
colors = [[0, 1, 0]]
heading_pcd.colors = o3d.utility.Vector3dVector(colors)
#渲染车辆形式方向的点
merge_geos += [heading_pcd]
if vis:
axis_pcd = o3d.geometry.TriangleMesh.create_coordinate_frame(size=5, origin=[0, 0, 0])
merge_geos += [axis_pcd]
#pcd.paint_uniform_color([1.00, 0, 0])
custom_draw_geometry_with_key_callback(merge_geos, file_path)
# pcd_list = glob.glob("D:/work/git_workspace/jfxmap_python/script/data_aug_samples/10-1_pcds/*.pcd")
# print(pcd_list)
\ No newline at end of file
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment