---

前言

提示：这里可以添加本文要记录的大概内容：

例如：随着人工智能的不断发展，机器学习这门技术也越来越重要，很多人都开启了学习机器学习，本文就介绍了机器学习的基础内容。

---

提示：以下是本篇文章正文内容，下面案例可供参考

一、激光雷达-图像坐标系转换关系

1.数据格式

数据包含4个部分，即激光雷达数据velodyne、图像数据image_2、校准数据calib和标注数据label_2。

1.激光雷达数据（data_object_velodyne）

velodyne文件夹下存储了点云文件，以bin格式存储。velodyne文件是激光雷达的测量数据（绕其垂直轴（逆时针）连续旋转），激光雷达参数如下：

1 × Velodyne HDL-64E rotating 3D laser scanner, 
10 Hz, 64 beams, 0.09 degree angular resolution, 
2 cm distance accuracy, collecting ∼ 1.3 million points/second,
field of view: 360◦ horizontal, 26.8◦ vertical, range: 120 m
1
2
3
4

以“000000.bin”文件为例，点云数据以浮点二进制文件格式存储，每行包含8个数据，每个数据由四位十六进制数表示（浮点数），每个数据通过空格隔开。一个点云数据由四个浮点数数据构成，分别表示点云的x、y、z、r（强度 or 反射值）。

KITTI激光雷达文件夹下的训练点云数量有7481个，即7481个bin文件，共13.2GB大小。测试点云数量有7518个，即7518个bin文件，共13.4GB大小。

可视化

利用python中的open3d实现 点云数据可视化（.bin文件）

import numpy as np
import struct
import open3d

def read_bin_velodyne(path):
    '''read bin file and transfer to array data'''
    pc_list=[]
    with open(path,'rb') as f:
        content=f.read()
        pc_iter=struct.iter_unpack('ffff',content)
        for idx,point in enumerate(pc_iter):
            pc_list.append([point[0],point[1],point[2]])
    return np.asarray(pc_list,dtype=np.float32)

def main():
    pc_path='/KITTI_DATASET_ROOT/testing/velodyne/000045.bin'
    example = np.fromfile(pc_path, dtype=np.float32, count=-1).reshape(-1, 4)
    example_xyz=example[:,:3]
    example_xyz=example_xyz[example_xyz[:,2]>-3]

    # From numpy to Open3D
    pcd = open3d.open3d.geometry.PointCloud()
    pcd.points= open3d.open3d.utility.Vector3dVector(example_xyz)
    vis_ = open3d.visualization.Visualizer()
    vis_.create_window()
    vis_.add_geometry(pcd)
    render_options = vis_.get_render_option()
    render_options.point_size = 1
    render_options.background_color = np.array([0, 0, 0])
    vis_.run()
    vis_.destroy_window()


if __name__=="__main__":
    main()

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36

2.标注数据label_2.

标签存储在data_object_label_2文件夹中，存储为txt文本文件，即data_object_label_2/training/label_2/xxxxxx.txt。标签中仅不含了7481个训练场景的标注数据，而没有测试场景的标注数据。

标注文件中16个属性，即16列。但我们只能够看到前15列数据，因为第16列是针对测试场景下目标的置信度得分，也可以认为训练场景中得分全部为1但是没有专门标注出来。下图是000001.txt的标注内容和对应属性介绍。

第1列
目标类比别（type），共有8种类别，分别是Car、Van、Truck、Pedestrian、Person_sitting、Cyclist、Tram、Misc或’DontCare。DontCare表示某些区域是有目标的，但是由于一些原因没有做标注，比如距离激光雷达过远。但实际算法可能会检测到该目标，但没有标注，这样会被当作false positive （FP）。这是不合理的。用DontCare标注后，评估时将会自动忽略这个区域的预测结果，相当于没有检测到目标，这样就不会增加FP的数量了。此外，在 2D 与 3D Detection Benchmark 中只针对 Car、Pedestrain、Cyclist 这三类。

第2列
截断程度（truncated），表示处于边缘目标的截断程度，取值范围为0~1，0表示没有截断，取值越大表示截断程度越大。处于边缘的目标可能只有部分出现在视野当中，这种情况被称为截断。

第3列
遮挡程度（occlude），取值为（0，1，2，3）。0表示完全可见，1表示小部分遮挡，2表示大部分遮挡，3表示未知（遮挡过大）。

第4列
观测角度（alpha）,取值范围为（-pi, pi）。是在相机坐标系下，以相机原点为中心，相机原点到物体中心的连线为半径，将物体绕相机y轴旋转至相机z轴，此时物体方向与相机x轴的夹角。这相当于将物体中心旋转到正前方后，计算其与车身方向的夹角。

第5-8列
二维检测框（bbox），目标二维矩形框坐标，分别对应left、top、right、bottom，即左上（xy）和右下的坐标（xy）。

第9-11列
三维物体的尺寸（dimensions），分别对应高度、宽度、长度，以米为单位。

第12-14列
中心坐标（location），三维物体中心在相机坐标系下的位置坐标（x，y，z），单位为米。

第15列
旋转角（rotation_y），取值范围为（-pi, pi）。表示车体朝向，绕相机坐标系y轴的弧度值，即物体前进方向与相机坐标系x轴的夹角。rolation_y与alpha的关系为alpha=rotation_y - theta，theta为物体中心与车体前进方向上的夹角。alpha的效果是从正前方看目标行驶方向与车身方向的夹角，如果物体不在正前方，那么旋转物体或者坐标系使得能从正前方看到目标，旋转的角度为theta。

第16列
置信度分数（score），仅在测试评估的时候才需要用到。置信度越高，表示目标越存在的概率越大。

3.图像数据image_2

KITTI数据集种共包含了4相机数据，2个灰度相机和2个彩色相机，其中image_2存储了左侧彩色相机采集的RGB图像数据（RGB）。

存储方式为png格式。KITTI相机的分辨率是1392x512，而image_2种存储的图像是矫正后的图像，分辨率为1242x375。训练集共7481张图片；测试集共7518张图片。

4.标定校准数据calib

标定校准文件主要作用是把 激光雷达坐标系 测得的点云坐标 转换到相机坐标中 去，相关参数存在data object calib中，共包含7481个训练标定文件和7518个测试标定文件。标定文件的存储方式为txt文本文件

以训练文件中的000000.txt标定校准文件为例，其内容如下图所示:

其中，0、1、2、3分别代表左边灰度相机、右边灰度相机、左边彩色相机和右边彩色相机。

1. 内参矩阵

P0-P3分别表示4个相机的内参矩阵，或投影矩阵, 大小为 3x4。相机内参矩阵是为了计算点云空间位置坐标在相机坐标系下的坐标，即把点云坐标投影到相机坐标系。将相机的内参矩阵乘以点云在世界坐标系中的坐标即可得到点云在相机坐标系中的坐标。

如果需要进一步将点云在相机坐标系下的坐标投影到像平面，还需要除以Z值，以及内参矩阵的推导请参考：https://blog.csdn.net/qq_33801763/article/details/77033064。

2. 外参矩阵

根据上述介绍，我们知道存在三种坐标系世界坐标系、相机坐标系、激光雷达坐标系。世界坐标系反映了物体的真实位置坐标，也是作为相机坐标系和激光雷达坐标系之间相互变换的过渡坐标系。

点云位置坐标投影到相机坐标系前，需要转换到世界坐标系下，对应的矩阵为外参矩阵。外参矩阵为Tr_velo_to_cam ，大小为3x4，包含了旋转矩阵 R 和 平移向量 T。将相机的外参矩阵乘以点云坐标即可得到点云在世界坐标系中的坐标。

3.R0校准矩阵

R0_rect 为0号相机的修正矩阵，大小为3x3，目的是为了使4个相机成像达到共面的效果，保证4个相机光心在同一个xoy平面上。在进行外参矩阵变化之后，需要于R0_rect相乘得到相机坐标系下的坐标。

4.点云坐标到相机坐标

综上所述，点云坐标在相机坐标系中的坐标等于

内参矩阵 * 外参矩阵 * R0校准矩阵 * 点云坐标

     P * R0_rect *Tr_velo_to_cam * x
1

例如，要将Velodyne激光雷达坐标系中的点x投影到左侧的彩色图像中y，使用公式：

     y = P2 * R0_rect *Tr_velo_to_cam * x
1

当计算出z<0的时候表明该点在相机的后面 。

按照上述过程得到的结果是点云在相机坐标系中的坐标，如果需要将点云坐标投影到像平面还需要除以Z。参考2.1节。示例程序可以参考 ：https://blog.csdn.net/suiyingy/article/details/124817919。

2.KITTI数据集中的三个坐标系：

1. 激光雷达坐标系 (下图中的蓝色坐标系)

2. 相机坐标系 (下图中的红色坐标系)
   

3. 图像坐标系 (下图：相机采集的图像)
   

3.点云数据转鸟瞰图（BEV）

import numpy as np
from PIL import Image
import matplotlib.pyplot as plt
 
# -------------------------------1.点云读取----------------------------
pc_path= '/home/xzz/Downloads/mini_kitti/mini kitti/data object veloyne/training/000004.bin'
pointcloud = np.fromfile(pc_path, dtype=np.float32, count=-1).reshape([-1, 4])           # (115976, 4)


# -----------------------------2.设置鸟瞰图范围---------------------------
side_range = (-40, 40)  # 左右距离
fwd_range = (0, 70.4)  # 后前距离
 
x_points = pointcloud[:, 0]            # (115976)
y_points = pointcloud[:, 1]            # (115976)
z_points = pointcloud[:, 2]            # (115976)
 

# ------------------------------3.获得区域内的点----------------------------
f_filt = np.logical_and(x_points > fwd_range[0], x_points < fwd_range[1])      # (115976): [True, False, True...]
s_filt = np.logical_and(y_points > side_range[0], y_points < side_range[1])
filter = np.logical_and(f_filt, s_filt)                                        # (115976): [True, False, True...]
indices = np.argwhere(filter).flatten()                                        # (59732) : [0, 2, ...115976, 115975]
x_points = x_points[indices]
y_points = y_points[indices]
z_points = z_points[indices]
 
# ----------------------------4. 把坐标单位从米，调整到厘米-------------------------
res = 0.1  # 分辨率0.1m
x_img = (-y_points / res).astype(np.int32)
y_img = (-x_points / res).astype(np.int32)


# -----------------------------5.调整坐标原点(到（0，0）)----------------------------
x_img -= int(np.floor(side_range[0]) / res)
y_img += int(np.floor(fwd_range[1]) / res)
print(x_img.min(), x_img.max(), y_img.min(), x_img.max())
 

# -----------------------------6.填充像素值（用z值）-------------------------
height_range = (-2, 0.5)                                   # z值范围选取
pixel_value = np.clip(a=z_points, a_max=height_range[1], a_min=height_range[0])
 
 
def scale_to_255(a, min, max, dtype=np.uint8):
	return ((a - min) / float(max - min) * 255).astype(dtype)
 
 
pixel_value = scale_to_255(pixel_value, height_range[0], height_range[1])   # z值映射到到(0，255)
 
# -----------------------------7.创建图像数组-------------------------------------
x_max = 1 + int((side_range[1] - side_range[0]) / res)
y_max = 1 + int((fwd_range[1] - fwd_range[0]) / res)
im = np.zeros([y_max, x_max], dtype=np.uint8)
im[y_img, x_img] = pixel_value
 


# -------------------------8.鸟瞰图可视化（灰度/彩色）------------------------
# im2 = Image.fromarray(im)
# im2.show()
 
# imshow （彩色）
plt.imshow(im, cmap="nipy_spectral", vmin=0, vmax=255)
plt.show()
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65

4.将标注画在鸟瞰图（BEV）上

from __future__ import division
import os
import numpy as np
import cv2
import math
 
 # voxel size
vd = 0.4
vh = 0.2
vw = 0.2
 
# points cloud range
xrange = (0, 70.4)
yrange = (-40, 40)
zrange = (-3, 1)
 
# voxel grid
W = math.ceil((xrange[1] - xrange[0]) / vw)    # 352
H = math.ceil((yrange[1] - yrange[0]) / vh)    # 400
D = math.ceil((zrange[1] - zrange[0]) / vd)    # 10
 
def _quantize_coords(x, y):
    xx = H - int((y - yrange[0]) / vh)
    yy = W - int((x - xrange[0]) / vw)
    return xx, yy
 
#过滤指定范围之外的点和目标框
def get_filtered_lidar(lidar, boxes3d=None):
    # lidar:(115976, 4) boxes3d: (2, 8, 3)
    xrange = (0, 70.4)
    yrange = (-40, 40)
    zrange = (-3, 1)
    pxs = lidar[:, 0]
    pys = lidar[:, 1]
    pzs = lidar[:, 2]
    filter_x = np.where((pxs >= xrange[0]) & (pxs < xrange[1]))[0]      # (59820) : [0, 2, ...105689]
    filter_y = np.where((pys >= yrange[0]) & (pys < yrange[1]))[0]      # (115833)
    filter_z = np.where((pzs >= zrange[0]) & (pzs < zrange[1]))[0]      # (113897): [ 162, 165,...115976 ]
    filter_xy = np.intersect1d(filter_x, filter_y)                      # (59737)
    filter_xyz = np.intersect1d(filter_xy, filter_z)                    # (58590)
    if boxes3d is not None:
        box_x = (boxes3d[:, :, 0] >= xrange[0]) & (boxes3d[:, :, 0] < xrange[1])        # (2, 8): [True. F,..True]
        box_y = (boxes3d[:, :, 1] >= yrange[0]) & (boxes3d[:, :, 1] < yrange[1])
        box_z = (boxes3d[:, :, 2] >= zrange[0]) & (boxes3d[:, :, 2] < zrange[1])
        box_xyz = np.sum(box_x & box_y & box_z,axis=1)
        return lidar[filter_xyz], boxes3d[box_xyz>0]
    return lidar[filter_xyz]
 
def lidar_to_bev(lidar):
    pxs = lidar[:, 0]             # ( 58590 )
    pys = lidar[:, 1]
    pzs = lidar[:, 2]
    prs = lidar[:, 3]
 
    qxs=((pxs-xrange[0])/vw).astype(np.int32)
    qys=((pys-yrange[0])/vh).astype(np.int32)
    qzs=((pzs-zrange[0])/vd).astype(np.int32)
 
    print('height,width,channel=%d,%d,%d'%(W, H, D))
    top = np.zeros(shape=(W, H, D), dtype=np.float32)
    mask = np.ones(shape=(W, H, D), dtype=np.float32)* -5
    bev = np.zeros(shape=(W, H, 3), dtype=np.float32)
    bev[:, : ,0] = np.ones(shape=(W, H), dtype=np.float32)* -5
 
    for i in range(len(pxs)):
        #统计高度方向上每个体素的个数
        bev[-qxs[i], -qys[i], -1]= 1+ bev[-qxs[i], -qys[i], -1]
        if pzs[i]>mask[-qxs[i], -qys[i],qzs[i]]:
            #记录每个体素中点的最大高度值
            top[-qxs[i], -qys[i], qzs[i]] = max(0,pzs[i]-zrange[0])
            #更新最大高度值
            mask[-qxs[i], -qys[i],qzs[i]]=pzs[i]
        if pzs[i]>bev[-qxs[i], -qys[i], 0]:
            #记录高度方向上的最大高度值
            bev[-qxs[i], -qys[i], 0]=pzs[i]
            #记录高度方向上最高点的强度值
            bev[-qxs[i], -qys[i], 1]=prs[i]
 
    bev[:,:,-1] = np.log(bev[:,:,-1]+1)/math.log(64)              # 数值缩小
 
 
    bev_image = bev - np.min(bev.reshape(-1, 3), 0)
    bev_image_image = (bev_image/np.max(bev_image.reshape(-1, 3), 0)*255).astype(np.uint8)
    return  bev[:, :, 0], bev[:, :, 1], bev[:, :, 2]              # bev[:, :, 0]表示体素中最高点的高度值; 最高点的强度; 体素中点的密度即点的数量。
 
def  draw_polygons(image, polygons,color=(255,255,255), thickness=1, darken=1):
    img = image.copy() * darken        # polygons: (n,4,2) 
    for polygon in polygons:
        tup0, tup1, tup2, tup3 = [_quantize_coords(*tup) for tup in polygon]  # 选择3维点云（8，3）中的（4，2），可直接画在2D体素图上
        cv2.line(img, tup0, tup1, color, thickness, cv2.LINE_AA)
        cv2.line(img, tup1, tup2, color, thickness, cv2.LINE_AA)
        cv2.line(img, tup2, tup3, color, thickness, cv2.LINE_AA)
        cv2.line(img, tup3, tup0, color, thickness, cv2.LINE_AA)
    return img
 
def load_kitti_calib(calib_file):
    """
    load projection matrix
    """
    with open(calib_file) as fi:
        lines = fi.readlines()
        assert (len(lines) == 8)
    obj = lines[0].strip().split(' ')[1:]
    P0 = np.array(obj, dtype=np.float32)
    obj = lines[1].strip().split(' ')[1:]
    P1 = np.array(obj, dtype=np.float32)
    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)
    obj = lines[6].strip().split(' ')[1:]
    Tr_imu_to_velo = np.array(obj, dtype=np.float32)
    return {'P2': P2.reshape(3, 4),
            'R0': R0.reshape(3, 3),
            'Tr_velo2cam': Tr_velo_to_cam.reshape(3, 4)}
 
 
def box3d_cam_to_velo(box3d, Tr):
    # Tr: Tr_velo_to_cam(3, 4)
    def project_cam2velo(cam, Tr):
        T = np.zeros([4, 4], dtype=np.float32)
        T[:3, :] = Tr
        T[3, 3] = 1
        T_inv = np.linalg.inv(T)            # 算矩阵的(乘法)逆
        lidar_loc_ = np.dot(T_inv, cam)
        lidar_loc = lidar_loc_[:3]
        return lidar_loc.reshape(1, 3)
 
    def ry_to_rz(ry):
        angle = -ry - np.pi / 2
        if angle >= np.pi:
            angle -= np.pi
        if angle < -np.pi:
            angle = 2*np.pi + angle
        return angle
 
    h,w,l,tx,ty,tz,ry = [float(i) for i in box3d]
    cam = np.ones([4, 1])
    cam[0] = tx
    cam[1] = ty
    cam[2] = tz
    t_lidar = project_cam2velo(cam, Tr)               # (相機坐標系)坐標中心 × Tr_velo_to_cam -> (1,3)
    Box = np.array([[-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]])
    rz = ry_to_rz(ry)
    rotMat = np.array([
        [np.cos(rz), -np.sin(rz), 0.0],
        [np.sin(rz), np.cos(rz), 0.0],
        [0.0, 0.0, 1.0]])
    velo_box = np.dot(rotMat, Box)                     # (3, 8)
    cornerPosInVelo = velo_box + np.tile(t_lidar, (8, 1)).T
    box3d_corner = cornerPosInVelo.transpose()
    return box3d_corner.astype(np.float32)             # (8, 3)
 
def load_kitti_label(label_file, Tr):
    # Tr: Tr_velo_to_cam(3, 4)
    with open(label_file,'r') as f:
        lines = f.readlines()
    gt_boxes3d_corner = []
    num_obj = len(lines)
    for j in range(num_obj):
        obj = lines[j].strip().split(' ')
        obj_class = obj[0].strip()
        if obj_class not in ['Car']:   
            continue                                      # 只顯示車輛目標
        box3d_corner = box3d_cam_to_velo(obj[8:], Tr)     # input:(h,w,l,x,y,z,r) (Tr)   out:(8, 3)
        gt_boxes3d_corner.append(box3d_corner)
    gt_boxes3d_corner = np.array(gt_boxes3d_corner).reshape(-1,8,3)
    return gt_boxes3d_corner                              # (2, 8, 3)
 
def test():
    lidar_file = '/home/xzz/Downloads/mini_kitti/mini kitti/data object veloyne/training/000004.bin'
    calib_file = '/home/xzz/Downloads/mini_kitti/mini kitti/data object calib/training/000004.txt'
    label_file = '/home/xzz/Downloads/mini_kitti/mini kitti/data_object_label_2/training/000004.txt'
 
    #加载雷达数据
    print("Processing: ", lidar_file)
    lidar = np.fromfile(lidar_file, dtype=np.float32)
    lidar = lidar.reshape((-1, 4))                      # (115976, 4)
 
    #加载标注文件
    calib = load_kitti_calib(calib_file)                # return: P2:(3, 4), R0:(3, 3), Tr_velo2cam:(3, 4)
    #标注转三维目标检测框
    gt_box3d = load_kitti_label(label_file, calib['Tr_velo2cam'])    # (n, 8, 3)
 
    #过滤指定范围之外的点和目标框
    lidar, gt_box3d = get_filtered_lidar(lidar, gt_box3d)          # (58590, 4)  (2, 8, 3)
 
    hight_image, height_r_image, density_image = lidar_to_bev(lidar)    # 体素中最高点的高度值; 最高点的强度; 体素中点的密度即点的数量。
    hight_with_box = draw_polygons(hight_image,gt_box3d[:,:4,:2])
    height_r_with_box = draw_polygons(height_r_image,gt_box3d[:,:4,:2])
    density_with_box = draw_polygons(density_image,gt_box3d[:,:4,:2])
    cv2.imshow('hight', hight_with_box)
    cv2.imshow('height_r', height_r_with_box)
    cv2.imshow('density', density_with_box)
    cv2.imwrite('hight.png', hight_with_box)
    cv2.imwrite('height_r.png', height_r_with_box)
    cv2.imwrite('density.png', density_with_box)
    cv2.waitKey(0)
 
if __name__ == '__main__':
    test()
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207

效果图：