赞
踩
KITTI 中的lidar数据是以二进制文件.bin存储的。每个点包含4个值: x,y,z坐标值和反射强度值。
import numpy as np # 用来处理数据 import matplotlib.pyplot as plt res = np.fromfile("/000001.bin", dtype=np.float32).reshape(-1, 4) x, y, z = res[:, 0], res[:,1], res[:,2] ax = plt.subplot(projection='3d') # 创建一个三维的绘图工程 ax.set_title('3d_image_show') # 设置本图名称 ax.scatter(x, y, z, c=z, # height data for color cmap='rainbow', marker="x") # 绘制数据点 c: 'r'红色,'y'黄色,等颜色 ax.set_xlabel('X') # 设置x坐标轴 ax.set_ylabel('Y') # 设置y坐标轴 ax.set_zlabel('Z') # 设置z坐标轴 plt.show()
可视化的效果是下图的样子, 其实不是很直观。
这里我用到的是MeshLab和CloudCompare。
大多数的工具都不支持直接打开二进制文件, 所以需要先预处理一下。 txt格式比较简单,因此把二进制文件转成txt。txt中的每一行表示一个点。
简单的转换代码:
res = np.fromfile("000001.bin", dtype=np.float32).reshape(-1, 4)
with open('000001.txt','w') as fp:
[fp.write(str(item[0])+','+str(item[1]) +',' +str(item[2]) +'\n') for item in res]
这个可视化的效果就还可以了。
CloudCompare软件, 如果直接从file里面去选择打开文件的话, 会发现没有对txt格式的支持。 后来发现, 直接把txt文件拖入这个软件的界面就可以打开 了。
打开的效果如下:
这个效果要更好一些。
点云数据和图像数据是有对应关系的, 把点云数据和图像展现在同一个图像上, 可以更直观的看到数据之间的对应关系。
由于点云数据和相机的图像数据不是在同一个坐标系下, 因此要想画在一个图上, 必须把点云数据对应到图像上。 主要有3次转换, 分别是: 激光雷达坐标系–>参考相机坐标系–>图像. 详细原理可参见:KITTI数据集激光雷达点云3D-Box可视化Matlab代码
代码中注释写的比较清楚了, 具体实现可参考下面的代码:
import numpy as np import matplotlib.pyplot as plt import cv2 import copy img_file = "000008.png" lidar_file = "000008.bin" calib_file = "000008.txt" def show_lidar(lidar_file): points = np.fromfile(lidar_file, dtype=np.float32).reshape(-1,4) x, y, z = points[:, 0], points[:, 1], points[:, 2] ax = plt.subplot(projection='3d') ax.set_title('lidar_image') ax.scatter(x, y, z, c=z, # height data for color cmap='rainbow', marker="x") ax.set_xlabel('X') # set axes ax.set_ylabel('Y') ax.set_zlabel('Z') plt.show() def save_lidar_txt(lidar_file): points = np.fromfile(lidar_file, dtype=np.float32).reshape(-1, 4) with open('000008.txt', 'w') as fp: [fp.write(str(item[0]) + ',' + str(item[1]) + ',' + str(item[2]) + '\n') for item in points] def show_image(img_file): img = cv2.imread(img_file) img = img[:, :, ::-1] # convert BGR to RGB plt.imshow(img) # plt.show() def get_calib_from_file(calib_file): """Read the calib txt""" with open(calib_file) as f: lines = f.readlines() obj = lines[2].strip().split(' ')[1:] P2 = np.array(obj, dtype=np.float32) obj = lines[3].strip().split(' ')[1:] P3 = np.array(obj, dtype=np.float32) obj = lines[4].strip().split(' ')[1:] R0 = np.array(obj, dtype=np.float32) obj = lines[5].strip().split(' ')[1:] Tr_velo_to_cam = np.array(obj, dtype=np.float32) return {'P2': P2.reshape(3, 4), 'P3': P3.reshape(3, 4), 'R0': R0.reshape(3, 3), 'Tr_velo2cam': Tr_velo_to_cam.reshape(3, 4)} def _3d_matmul(mat1, mat2): return np.einsum("ijk, ikn ->ijn", mat1, mat2) def project_lidar_to_img(calib_file, lidar_file): """convert the lidar point from lidar coordinate system to camera coordinate system.""" points = np.fromfile(lidar_file, dtype=np.float32).reshape(-1, 4) valid_idx = (points[:,0]>5).nonzero() # the area of x<5 is the blind spot of camera points = points[valid_idx] points = points[::5] # sample every 5 points so the image will be clear points[:, 3] = 1 points = np.expand_dims(points, axis=2) points_num = len(points) calib = get_calib_from_file(calib_file) P2 = calib["P2"] P2_batch = np.expand_dims(P2, axis=0).repeat(points_num, axis=0) # [3,4]->[points_num, 3, 4] R0 = calib["R0"] R0_pad = np.eye(4) R0_pad[0:3, 0:3] = R0 R0_pad_batch = np.expand_dims(R0_pad, axis=0).repeat(points_num, axis=0) # [4,4]->[points_num, 4, 4] Tr_velo2cam = calib["Tr_velo2cam"] Tr_velo2cam_pad = np.eye(4) Tr_velo2cam_pad[0:3, :] = Tr_velo2cam Tr_velo2cam_pad_batch = np.expand_dims(Tr_velo2cam_pad, axis=0).repeat(points_num, axis=0) # proj = P2 @ (R0_pad @ (Tr_velo2cam_pad @ point)) for single point proj = _3d_matmul(Tr_velo2cam_pad_batch, points) proj = _3d_matmul(R0_pad_batch, proj) proj = _3d_matmul(P2_batch, proj) #shape is [batch, 3,1] proj = proj[:, :, 0] # shape is [batch, 3] norm = copy.deepcopy(proj) depth = proj[:,2] norm[:, 0] = depth norm[:, 1] = depth proj = proj/norm return proj, depth if __name__ == "__main__": show_lidar(lidar_file) save_lidar_txt(lidar_file) show_image(img_file) # proj the lidar to camera and plot them together proj, _ = project_lidar_to_img(calib_file, lidar_file) X = np.clip(proj[:, 0], 0, 1242) #the image size is [375, 1242] Y = np.clip(proj[:, 1], 0, 375) col_idx = np.round(64*5/X) plt.scatter(X, Y, c=col_idx, cmap='rainbow',marker="x") plt.show()
最终的效果是:
Copyright © 2003-2013 www.wpsshop.cn 版权所有,并保留所有权利。