在Rope3D数据集可视化时，与kitti数据集不同，需要用到denorm文件。denorm文件表示的是地面方程Ax+By+Cz+D=0，如果应用于其它路测数据集，可能没有denorm文件，就无法进行有效的可视化。那么如何通过现有数据集，求出该数据就是本文要解决的问题。
注：需要考虑隐藏约束条件AA+BB+CC=1。
kitti可视化脚本代码：
import os
import shutil
import time
from multiprocessing import Process,Queue
from multiprocessing import cpu_count
import cv2
import numpy as np
pwd = os.getcwd()
img_dir = pwd+"/datasets/kitti/testing/image_2/"
label_dir = pwd+"/datasets/kitti/testing/label_2/"
#label_dir = pwd+"/output/exp/inference/kitti_test/data/"
calib_dir = pwd+"/datasets/kitti/testing/calib/"
save_dir = pwd+"/datasets/kitti/testing//visualize_original/"
def Get_label(label_path,calib_path):
with open(label_path, 'r') as f:
data = [] #分别对应car\xmin\ymin\xmax\ymax\h\w\l\x\y\z
#每行分开读取
for line in f:
data_line = line.strip("\n").split() #去除首尾换行符，并按空格划分
data.append([data_line[0], data_line[4], data_line[5], data_line[6], data_line[7], data_line[8], data_line[9], data_line[10], data_line[11], data_line[12], data_line[13], data_line[14]])
print('-----', label_path)
with open(label_path, 'r') as f:
label_lines = f.readlines()
# print(label_path+' before:'+str(len(label_lines)))
data3 = []
for i in range(len(label_lines)):
h,w,l,x,y,z,yaw = float(data[i][5]), float(data[i][6]), float(data[i][7]), float(data[i][8]), float(data[i][9]), float(data[i][10]), float(data[i][11])
R = np.array([[np.cos(yaw), 0, np.sin(yaw)], [0,1,0], [-np.sin(yaw),0,np.cos(yaw)]])
if h>0.0 and w >0.0 and l>0.0:
x_corners = [l/2, l/2, -l/2, -l/2, l/2, l/2, -l/2, -l/2]
y_corners = [0,0,0,0,-h,-h,-h,-h]
z_corners = [w/2, -w/2, -w/2, w/2, w/2, -w/2, -w/2, w/2]
corners_3d_cam2 = np.dot(R, np.vstack([x_corners, y_corners, z_corners]))
corners_3d_cam2 += np.vstack([x, y, z])
data3.append(corners_3d_cam2)
# print(corners_3d_cam2)
return data3
def Save_viz_image(image_path, label_path, calib_path):
data3 = Get_label(label_path, calib_path) #返回值为类别、坐上右下坐标、8个角点像素坐标
image = cv2.imread(image_path)
print(len(data3))
with open(calib_path, 'r') as f2:
alllines = f2.readlines()
space_data2 = []
data2 = []
for i in range(len(alllines[0])):
if alllines[0][i] == ' ':
space_data2.append(i)
data2.append([alllines[0][space_data2[0]+1:space_data2[1]], alllines[0][space_data2[2]+1:space_data2[3]], alllines[0][space_data2[5]+1:space_data2[6]], alllines[0][space_data2[6]+1:space_data2[7]]])
fx, u0, fy, v0 = float(data2[0][0]), float(data2[0][1]), float(data2[0][2]), float(data2[0][3])
print([fx,u0,fy,v0])
# fx, u0, fy, v0 = -462.207, 538.294, 473.902, 276.378
for i in range(len(data3)):
data = data3[i]
data_x,data_y,data_z = data[0], data[1], data[2]
e1_x,e2_x,e3_x,e4_x,e5_x,e6_x,e7_x,e8_x = float(data_x[0]), data_x[1], data_x[2], data_x[3], data_x[4], data_x[5], data_x[6], data_x[7]
e1_y,e2_y,e3_y,e4_y,e5_y,e6_y,e7_y,e8_y = float(data_y[0]), data_y[1], data_y[2], data_y[3], data_y[4], data_y[5], data_y[6], data_y[7]
e1_z,e2_z,e3_z,e4_z,e5_z,e6_z,e7_z,e8_z = float(data_z[0]), data_z[1], data_z[2], data_z[3], data_z[4], data_z[5], data_z[6], data_z[7]
e1_img_u, e1_img_v = int(float((fx*e1_x+u0*e1_z)/e1_z)), int(float((fy*e1_y+v0*e1_z)/e1_z))
e2_img_u, e2_img_v = int(float((fx*e2_x+u0*e2_z)/e2_z)), int(float((fy*e2_y+v0*e2_z)/e2_z))
e3_img_u, e3_img_v = int(float((fx*e3_x+u0*e3_z)/e3_z)), int(float((fy*e3_y+v0*e3_z)/e3_z))
e4_img_u, e4_img_v = int(float((fx*e4_x+u0*e4_z)/e4_z)), int(float((fy*e4_y+v0*e4_z)/e4_z))
e5_img_u, e5_img_v = int(float((fx*e5_x+u0*e5_z)/e5_z)), int(float((fy*e5_y+v0*e5_z)/e5_z))
e6_img_u, e6_img_v = int(float((fx*e6_x+u0*e6_z)/e6_z)), int(float((fy*e6_y+v0*e6_z)/e6_z))
e7_img_u, e7_img_v = int(float((fx*e7_x+u0*e7_z)/e7_z)), int(float((fy*e7_y+v0*e7_z)/e7_z))
e8_img_u, e8_img_v = int(float((fx*e8_x+u0*e8_z)/e8_z)), int(float((fy*e8_y+v0*e8_z)/e8_z))
cv2.line(image, (e1_img_u,e1_img_v),(e2_img_u,e2_img_v), (255,255,255), 1)
cv2.line(image, (e1_img_u,e1_img_v),(e4_img_u,e4_img_v), (255,255,255), 1)
cv2.line(image, (e2_img_u,e2_img_v),(e3_img_u,e3_img_v), (255,255,255), 1)
cv2.line(image, (e3_img_u,e3_img_v),(e4_img_u,e4_img_v), (255,255,255), 1)
cv2.line(image, (e1_img_u,e1_img_v),(e5_img_u,e5_img_v), (255,255,255), 1)
cv2.line(image, (e2_img_u,e2_img_v),(e6_img_u,e6_img_v), (255,255,255), 1)
cv2.line(image, (e3_img_u,e3_img_v),(e7_img_u,e7_img_v), (255,255,255), 1)
cv2.line(image, (e4_img_u,e4_img_v),(e8_img_u,e8_img_v), (255,255,255), 1)
cv2.line(image, (e5_img_u,e5_img_v),(e6_img_u,e6_img_v), (255,255,255), 1)
cv2.line(image, (e5_img_u,e5_img_v),(e8_img_u,e8_img_v), (255,255,255), 1)
cv2.line(image, (e6_img_u,e6_img_v),(e7_img_u,e7_img_v), (255,255,255), 1)
cv2.line(image, (e7_img_u,e7_img_v),(e8_img_u,e8_img_v), (255,255,255), 1)
# print([e1_img_u, e1_img_v,e2_img_u, e2_img_v, e3_img_u, e3_img_v, e4_img_u, e4_img_v, e5_img_u, e5_img_v, e6_img_u, e6_img_v, e7_img_u, e7_img_v, e8_img_u, e8_img_v])
filename = image_path.split('/')[-1]
name = os.path.join(save_dir, filename)
cv2.imwrite(name, image)
# cv2.imshow('img', image)
# cv2.waitKey(0)
def Visualize(img_dir, label_dir):
label_list = os.listdir(label_dir)
for i in label_list:
label_path = os.path.join(label_dir, i)
image_path = os.path.join(img_dir, i.replace('.txt', '.jpg'))
calib_path = os.path.join(calib_dir, i)
Save_viz_image(image_path, label_path, calib_path)
if __name__ == "__main__":
if os.path.isdir(save_dir):
shutil.rmtree(save_dir)
os.mkdir(save_dir)
Visualize(img_dir, label_dir)
Rope3D可视化脚本代码
import os
import cv2
import numpy as np
import math
import sys
# import config
import glob as gb
# import pdb
# import yaml
import shutil
from pyquaternion import Quaternion
# stop python from writing so much bytecode
sys.dont_write_bytecode = True
sys.path.append(os.getcwd())
np.set_printoptions(suppress=True)
# pwd = os.getcwd()
img_dir = ".\\datasets\\testing\\image"
label_dir = ".\\datasets\\training\\label"
calib_dir = ".\\datasets\\testing\\calib"
denorm_dir = ".\\datasets\\testing\\denorm_1"
save_dir = ".\\datasets\\testing\\visualize_original"
color_list = {'car': (0, 0, 255),
'truck': (0, 255, 255),
'van': (255, 0, 255),
'bus': (255, 255, 0),
'cyclist': (0, 128, 128),
'motorcyclist': (128, 0, 128),
'tricyclist': (128, 128, 0),
'pedestrian': (0, 128, 255),
'barrow': (255, 0, 128)}
class Data:
def __init__(self, obj_type="unset", truncation=-1, occlusion=-1, \
obs_angle=-10, x1=-1, y1=-1, x2=-1, y2=-1, w=-1, h=-1, l=-1, \
X=-1000, Y=-1000, Z=-1000, yaw=-10, score=-1000, detect_id=-1, \
vx=0, vy=0, vz=0):
"""init object data"""
self.obj_type = obj_type
self.truncation = truncation
self.occlusion = occlusion
self.obs_angle = obs_angle
self.x1 = x1
self.y1 = y1
self.x2 = x2
self.y2 = y2
self.w = w
self.h = h
self.l = l
self.X = X
self.Y = Y
self.Z = Z
self.vx = vx
self.vy = vy
self.vz = vz
self.yaw = yaw
self.score = score
self.ignored = False
self.valid = False
self.detect_id = detect_id
def __str__(self):
""" str """
attrs = vars(self)
return '\n'.join("%s: %s" % item for item in attrs.items())
def progress(count, total, status=''):
bar_len = 60
filled_len = int(round(bar_len * count / float(total)))
percents = round(100.0 * count / float(total), 1)
bar = '=' * filled_len + '>' + '-' * (bar_len - filled_len)
sys.stdout.write('[%s] %s%s ...%s\r' % (bar, percents, '%', status))
sys.stdout.flush()
#从label文件中加载检测内容
def load_detect_data(filename):
data = []
with open(filename) as infile:
index = 0
for line in infile:
# (objectType,truncation,occlusion,alpha,x1,y1,x2,y2,h,w,l,X,Y,Z,ry)
line = line.strip()
fields = line.split(" ")
t_data = Data()
# get fields from table
t_data.obj_type = fields[0].lower() # object type [car, pedestrian, cyclist, ...]
t_data.truncation = float(fields[1]) # truncation [0..1]
t_data.occlusion = int(float(fields[2])) # occlusion [0,1,2]
t_data.obs_angle = float(fields[3]) # observation angle [rad]
t_data.x1 = int(float(fields[4])) # left [px]
t_data.y1 = int(float(fields[5])) # top [px]
t_data.x2 = int(float(fields[6])) # right [px]
t_data.y2 = int(float(fields[7])) # bottom [px]
t_data.h = float(fields[8]) # height [m]
t_data.w = float(fields[9]) # width [m]
t_data.l = float(fields[10]) # length [m]
t_data.X = float(fields[11]) # X [m]
t_data.Y = float(fields[12]) # Y [m]
t_data.Z = float(fields[13]) # Z [m]
t_data.yaw = float(fields[14]) # yaw angle [rad]
if len(fields) >= 16:
t_data.score = float(fields[15]) # detection score
else:
t_data.score = 1
t_data.detect_id = index
data.append(t_data)
index = index + 1
return data
#读取v2x denorm文件内容
def load_denorm_data(denormfile):
text_file = open(denormfile, 'r')
for line in text_file:
parsed = line.split('\n')[0].split(' ')
if parsed is not None and len(parsed) > 3:
de_norm = []
de_norm.append(float(parsed[0]))
de_norm.append(float(parsed[1]))
de_norm.append(float(parsed[2]))
text_file.close()
return np.array(de_norm)
#计算相机坐标系到地面的转化
def compute_c2g_trans(de_norm):
ground_z_axis = de_norm
cam_xaxis = np.array([1.0, 0.0, 0.0])
ground_x_axis = cam_xaxis - cam_xaxis.dot(ground_z_axis) * ground_z_axis
ground_x_axis = ground_x_axis / np.linalg.norm(ground_x_axis)
ground_y_axis = np.cross(ground_z_axis, ground_x_axis)
ground_y_axis = ground_y_axis / np.linalg.norm(ground_y_axis)
c2g_trans = np.vstack([ground_x_axis, ground_y_axis, ground_z_axis]) #(3, 3)
return c2g_trans
#读取相机校准文件P2
def read_kitti_cal(calfile):
text_file = open(calfile, 'r')
for line in text_file:
parsed = line.split('\n')[0].split(' ')
# cls x y w h occ x y w h ign ang
if parsed is not None and parsed[0] == 'P2:':
p2 = np.zeros([4, 4], dtype=float)
p2[0, 0] = parsed[1]
p2[0, 1] = parsed[2]
p2[0, 2] = parsed[3]
p2[0, 3] = parsed[4]
p2[1, 0] = parsed[5]
p2[1, 1] = parsed[6]
p2[1, 2] = parsed[7]
p2[1, 3] = parsed[8]
p2[2, 0] = parsed[9]
p2[2, 1] = parsed[10]
p2[2, 2] = parsed[11]
p2[2, 3] = parsed[12]
p2[3, 3] = 1
text_file.close()
return p2
#Projects a 3D box into 2D vertices using the camera2ground tranformation
def project_3d_ground(p2, de_bottom_center, w3d, h3d, l3d, ry3d, de_norm, c2g_trans):
"""
Args:
p2 (nparray): projection matrix of size 4x3
de_bottom_center: bottom center XYZ-coord of the object
w3d: width of object
h3d: height of object
l3d: length of object
ry3d: rotation w.r.t y-axis
de_norm: [a,b,c] denotes the ground equation plane
c2g_trans: camera_to_ground translation
"""
de_bottom_center_in_ground = c2g_trans.dot(de_bottom_center) #convert de_bottom_center in Camera coord into Ground coord
bottom_center_ground = np.array(de_bottom_center_in_ground)
bottom_center_ground = bottom_center_ground.reshape((3, 1))
theta = np.matrix([math.cos(ry3d), 0, -math.sin(ry3d)]).reshape(3, 1)
theta0 = c2g_trans[:3, :3] * theta #first column
yaw_world_res = math.atan2(theta0[1], theta0[0])
g2c_trans = np.linalg.inv(c2g_trans)
verts3d = get_camera_3d_8points_g2c(w3d, h3d, l3d, yaw_world_res,
bottom_center_ground, g2c_trans, p2, isCenter=False)
verts3d = np.array(verts3d)
return verts3d
def get_camera_3d_8points_g2c(w3d, h3d, l3d, yaw_ground, center_ground,
g2c_trans, p2,
isCenter=True):
"""
function: projection 3D to 2D
w3d: width of object
h3d: height of object
l3d: length of object
yaw_world: yaw angle in world coordinate
center_world: the center or the bottom-center of the object in world-coord
g2c_trans: ground2camera / world2camera transformation
p2: projection matrix of size 4x3 (camera intrinsics)
isCenter:
1: center,
0: bottom
"""
ground_r = np.matrix([[math.cos(yaw_ground), -math.sin(yaw_ground), 0],
[math.sin(yaw_ground), math.cos(yaw_ground), 0],
[0, 0, 1]])
#l, w, h = obj_size
w = w3d
l = l3d
h = h3d
if isCenter:
corners_3d_ground = np.matrix([[l / 2, l / 2, -l / 2, -l / 2, l / 2, l / 2, -l / 2, -l / 2],
[w / 2, -w / 2, -w / 2, w / 2, w / 2, -w / 2, -w / 2, w / 2],
[-h / 2, -h / 2, -h / 2, -h / 2, h / 2, h / 2, h / 2, h / 2]])
else:#bottom center, ground: z axis is up
corners_3d_ground = np.matrix([[l / 2, l / 2, -l / 2, -l / 2, l / 2, l / 2, -l / 2, -l / 2],
[w / 2, -w / 2, -w / 2, w / 2, w / 2, -w / 2, -w / 2, w / 2],
[0, 0, 0, 0, h, h, h, h]])
corners_3d_ground = np.matrix(ground_r) * np.matrix(corners_3d_ground) + np.matrix(center_ground) #[3, 8]
if g2c_trans.shape[0] == 4: #world2camera transformation
ones = np.ones(8).reshape(1, 8).tolist()
corners_3d_cam = g2c_trans * np.matrix(corners_3d_ground.tolist() + ones)
corners_3d_cam = corners_3d_cam[:3, :]
else: #only consider the rotation
corners_3d_cam = np.matrix(g2c_trans) * corners_3d_ground #[3, 8]
pt = p2[:3, :3] * corners_3d_cam
corners_2d = pt / pt[2]
corners_2d_all = corners_2d.reshape(-1)
if True in np.isnan(corners_2d_all):
print("Invalid projection")
return None
corners_2d = corners_2d[0:2].T.tolist()
for i in range(8):
corners_2d[i][0] = int(corners_2d[i][0])
corners_2d[i][1] = int(corners_2d[i][1])
return corners_2d
#显示2D和3D框
def show_box_with_roll(name_list, thresh=0.5):
for i, name in enumerate(name_list):
name = name.strip()
name = name.split('\\')
name = name[-1].split('.')[0]
img_path = os.path.join(img_dir, '%s.jpg' % (name))
progress(i, len(name_list))
detection_file = os.path.join(label_dir, '%s.txt' % (name))
result = load_detect_data(detection_file)
denorm_file = os.path.join(denorm_dir, '%s.txt' % (name))
de_norm = load_denorm_data(denorm_file) #[ax+by+cz+d=0]
c2g_trans = compute_c2g_trans(de_norm)
calfile = os.path.join(calib_dir, '%s.txt' % (name))
p2 = read_kitti_cal(calfile)
img = cv2.imread(img_path)
h, w, c = img.shape
for result_index in range(len(result)):
t = result[result_index]
if t.score < thresh:
continue
if t.obj_type not in color_list.keys():
continue
color_type = color_list[t.obj_type]
cv2.rectangle(img, (t.x1, t.y1), (t.x2, t.y2), (255, 255, 255), 1) #2D标注框
if t.w <= 0.05 and t.l <= 0.05 and t.h <= 0.05: #无效的annos
continue
cam_bottom_center = [t.X, t.Y, t.Z] # 相机坐标系的底面中心
verts3d = project_3d_ground(p2, np.array(cam_bottom_center), t.w, t.h, t.l, t.yaw, de_norm, c2g_trans)
if verts3d is None:
continue
verts3d = verts3d.astype(np.int32)
#draw projection
cv2.line(img, tuple(verts3d[2]), tuple(verts3d[1]), color_type, 2)
cv2.line(img, tuple(verts3d[1]), tuple(verts3d[0]), color_type, 2)
cv2.line(img, tuple(verts3d[0]), tuple(verts3d[3]), color_type, 2)
cv2.line(img, tuple(verts3d[2]), tuple(verts3d[3]), color_type, 2)
cv2.line(img, tuple(verts3d[7]), tuple(verts3d[4]), color_type, 2)
cv2.line(img, tuple(verts3d[4]), tuple(verts3d[5]), color_type, 2)
cv2.line(img, tuple(verts3d[5]), tuple(verts3d[6]), color_type, 2)
cv2.line(img, tuple(verts3d[6]), tuple(verts3d[7]), color_type, 2)
cv2.line(img, tuple(verts3d[7]), tuple(verts3d[3]), color_type, 2)
cv2.line(img, tuple(verts3d[1]), tuple(verts3d[5]), color_type, 2)
cv2.line(img, tuple(verts3d[0]), tuple(verts3d[4]), color_type, 2)
cv2.line(img, tuple(verts3d[2]), tuple(verts3d[6]), color_type, 2)
cv2.imwrite('%s/%s.jpg' % (save_dir, name), img)
if __name__ == "__main__":
name_list = gb.glob(label_dir+"\\*")
print(name_list)
if os.path.isdir(save_dir):
shutil.rmtree(save_dir)
os.mkdir(save_dir)
show_box_with_roll(name_list)
根据Rope3D数据集中label文件求解denorm文件，并将三维点展示在平面上
import glob as gb
import matplotlib.pyplot as plt
import numpy as np
import os
import shutil
import math
label_dir = ".\\datasets\\training\\label\\"
denorm_dir = ".\\datasets\\testing\\denorm_1\\"
groundplt_dir = ".\\datasets\\testing\\groundplt\\"
def get_label(label_path):
with open(label_path, 'r') as f:
data = [] #分别对应x\y\z
x, y, z =[], [], []
#每行分开读取
for line in f:
data_line = line.strip("\n").split() #去除首尾换行符，并按空格划分
#如果三维信息有效
if float(data_line[8])>0.0 and float(data_line[9])>0.0 and float(data_line[10])>0.0:
data.append([data_line[11], data_line[12], data_line[13]])
x.append(float(data_line[11]))
y.append(float(data_line[12]))
z.append(float(data_line[13]))
return data, x, y, z
def get_abc(label_path, name, flag=True):
data, x, y, z = get_label(label_path)
R, Z = [], []
for j in range(len(data)):
R.append([float(data[j][0]), float(data[j][1]), 1])
Z.append([float(data[j][2])])
R = np.mat(R)
A = np.dot(np.dot(np.linalg.inv(np.dot(R.T, R)), R.T), Z)
A = np.array(A, dtype='float32').flatten()
if flag==True:
fig1 = plt.figure()
ax1 = plt.axes(projection='3d')
ax1.set_xlabel("x")
ax1.set_ylabel("y")
ax1.set_zlabel("z")
# ax1.grid() # 关闭网格
x_p = [np.min(x), np.max(x)]
y_p = [np.min(y), np.max(y)]
x_p, y_p = np.meshgrid(x_p, y_p)
a, b, c= A[0], A[1], A[2]
z_p = a * x_p + b * y_p + c
xx = [(np.min(x) + np.max(x)) / 2, (np.min(x) + np.max(x)) / 2 + a]
yy = [(np.min(y) + np.max(y)) / 2, (np.min(y) + np.max(y)) / 2 + b]
zz = [(np.min(z) + np.max(z)) / 2, (np.min(z) + np.max(z)) / 2 - 1]
x.append((np.min(x) + np.max(x)) / 2)
y.append((np.min(y) + np.max(y)) / 2)
z.append((np.min(z) + np.max(z)) / 2)
ax1.scatter(x, y, z, c='r', marker='o') # 散点图
ax1.plot_wireframe(x_p, y_p, z_p, rstride=10, cstride=10) # 线框图
ax1.plot3D(xx, yy, zz, c='b', linestyle='--')
if not os.path.exists(groundplt_dir):
os.makedirs(groundplt_dir)
plt_path=groundplt_dir+name+".png"
plt.title(name)
plt.savefig(plt_path)
print('groungplt is saved in .\\datasets\\testing\\groundplt')
return A[0], A[1], A[2]
def get_denorm(name_list):
for name in name_list:
label_path = label_dir+name
name = name.strip()
name = name.split('\\')
name = name[-1].split('.')[0]
denorm_path = denorm_dir+name
a, b, c = get_abc(label_path, name)
C = -1.0/math.sqrt(a*a + b*b+1)
A, B, D = -a*C, -b*C, -c*C
denorm_file = open(denorm_path, 'w')
denorm_file.write(str(A)+' '+str(B)+' '+str(C)+' '+str(D))
denorm_file.close()
print('denorm is saved in .\\datasets\\testing\\denorm_1')
if __name__ == '__main__':
name_list = os.listdir(label_dir)
if os.path.isdir(denorm_dir):
shutil.rmtree(denorm_dir)
os.mkdir(denorm_dir)
if os.path.isdir(groundplt_dir):
shutil.rmtree(groundplt_dir)
os.mkdir(groundplt_dir)
get_denorm(name_list)
相关知识点连接参考：
https://blog.csdn.net/weixin_44729155/article/details/128343928
https://zhuanlan.zhihu.com/p/390294059
686
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
