1 lidar點云數據可視化

KITTI 中的lidar數據是以二進制文件.bin存儲的。每個點包含4個值： x,y,z坐標值和反射強度值。

1.1 方式1： 通過matplotlib畫圖

import numpy as np # 用來處理數據 import matplotlib.pyplot as pltres = 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 colorcmap='rainbow',marker="x") # 繪制數據點 c: 'r'紅色，'y'黃色，等顏色ax.set_xlabel('X') # 設置x坐標軸 ax.set_ylabel('Y') # 設置y坐標軸 ax.set_zlabel('Z') # 設置z坐標軸plt.show()

可視化的效果是下圖的樣子， 其實不是很直觀。

1.2 用點云可視化工具打開

這里我用到的是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文件拖入這個軟件的界面就可以打開 了。
打開的效果如下：

這個效果要更好一些。

2 點云數據投影到圖像上

點云數據和圖像數據是有對應關系的， 把點云數據和圖像展現在同一個圖像上， 可以更直觀的看到數據之間的對應關系。
由于點云數據和相機的圖像數據不是在同一個坐標系下， 因此要想畫在一個圖上， 必須把點云數據對應到圖像上。 主要有3次轉換， 分別是： 激光雷達坐標系–>參考相機坐標系–>圖像. 詳細原理可參見：KITTI數據集激光雷達點云3D-Box可視化Matlab代碼

代碼中注釋寫的比較清楚了， 具體實現可參考下面的代碼：

import numpy as np import matplotlib.pyplot as plt import cv2 import copyimg_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 colorcmap='rainbow',marker="x")ax.set_xlabel('X') # set axesax.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 RGBplt.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 camerapoints = points[valid_idx]points = points[::5] # sample every 5 points so the image will be clearpoints[:, 3] = 1points = 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] = R0R0_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_velo2camTr_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 pointproj = _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] = depthnorm[:, 1] = depthproj = proj/normreturn proj, depthif __name__ == "__main__":show_lidar(lidar_file)save_lidar_txt(lidar_file)show_image(img_file)# proj the lidar to camera and plot them togetherproj, _ = 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()

最終的效果是：

總結

以上是生活随笔為你收集整理的KITTI数据集可视化的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。