pykitti的使用

最近在用kitti的 kitti_raw的数据集，一直在和pykitti打交道， pykitti这个包里面整理了经常使用的一些接口.

比如 pose,信息，比如把点云信息根据每个时刻的pose,就能够得到一个大的点云地图.而且比较稠密，因为用的是原始的.
下面就以 点云拼接为例，来熟悉pykitti中的接口.

• 创建kitti的数据集的对象

import pykitti
# dataset = pykitti.raw(basedir, date, drive, frames=list(range(0, 200, 4)))
basedir = '/Users/john/Downloads'
# Specify the dataset to load
date = '2011_09_30'
drive = '0016'
dataset = pykitti.raw(basedir, date, drive)

# 这样使用的就是 kitti raw 中的sync的数据集.

• 拿到各个sensor对就应的数据

cam3 = list(iter(dataset.cam3)) # 用的是image03， image03是RGB的.
oxts = dataset.oxts
velos = dataset.velo
assert len(cam3) == len(oxts)

• 拿到传感器的参数，以及相对pose

def inverse_pose(pose):
    """
    pose: 4x4,
    """
    inv_pose = np.eye(4)
    rvec = pose[:3,:3]
    tvec = pose[:3,3].reshape(3,1)
    inv_rvec = rvec.T
    t = - np.dot(inv_rvec, tvec)
    inv_pose[:3, :3] = inv_rvec
    inv_pose[:3, [3]] = t
    return inv_pose

T_cam3_imu = dataset.calib.T_cam3_imu   # 从imu的坐标系到cam3的坐标系的变换. 这是一个4*4的
T_velo_imu = dataset.calib.T_velo_imu   # 从imu坐标系到velo坐标系的转换.
T_imu_velo = inverse_pose(T_velo_imu)

#如果只是需要点云拼接的话，下面的不太需要, 下面的我是想给点云上色.
K_cam3 = dataset.calib.K_cam3  # 3号样机的内参信息
T_cam3_velo = dataset.calib.T_cam3_velo[:3, :4]
proj_matrix = np.dot(K_cam3, T_cam3_velo)

• 得到车辆的pose信息

其实车辆的Pose信息，可以从车上的imu信息得到

for idx, (img, oxt, velo) in enumerate(zip(cam3, oxts, velos)):
    T_w_imu = oxt.T_w_imu
    # 这个就是从imu坐标系到世界坐标系下的变换,如果想知道车此时的位置，就可以用 T_w_imu.dot(np.array(0,0,0,1)) 来得到. 这一点在求车辆的轨迹的时候就会用到.
    T_w_velo = np.dot(T_w_imu, T_imu_velo)
    # 这个就是从velo的坐标系到世界坐标系的变换.  要注意这里的世界坐标系其实是以车辆的出发点作为坐标原点来看的，如果知道了车辆的出发点的位置的话，就真的是世界坐标系下的位置了，即东北上坐标系下的位置。
    velo_points = velo.copy()
    velo_points[:, 3] = 1.  # 本来第4个维度是intensity,但是这里这个并没有用，所以就直接转成齐次的了.
    velo_points = velo_points.T
    point_w = np.dot(T_w_velo, velo_points).T[:, :3]
    points_total.append(point_w)
    
    # 下面是给点云颜色.
       img = np.asarray(img)
    H, W = img.shape[:2]
    points_image = np.dot(proj_matrix, velo_points).T
    points_image = points_image / points_image[:, [2]]
    color_temp = []
    for i, pt in enumerate(points_image):
        u, v, flag = pt
        u = int(u/flag)
        v = int(v/flag)
        if flag>0 and (0<= u and u < W) and (0<=v and v<H):
            color = img[v, u, :] / 255.
        else:
            color = np.array([64,2,128])/ 255
        color_temp.append(color)
      color_temp = np.array(color_temp).reshape(-1, 3)
 
      colors_total.append(color_temp)

之后就可以使用 open3d来看得到了点云了.

看，车道线都非常的清晰，这里为了方便，只使用了10张图片，太多了，打开不方便.