Projection Velodyne data into image plane in NAVER LABS dataset

project_vel2img.py

import os
from PIL import Image, ImageDraw
import h5py
import numpy as np
from helper.intrinsics import *
import matplotlib.pyplot as plt
cmap = plt.cm.jet
import cv2
from helper.tf_helpers import get_T_c_l
import open3d

CAMERA_IDs = {"2019-04-16_15-35-46": ["22970285", "22970286", "22970288", "22970289", "22970290", "22970291"],
              "2019-04-16_16-14-48": ["22970285", "22970286", "22970288", "22970289", "22970290", "22970291",
                                      "AC01324954", "AC01324955", "AC01324968", "AC01324969"],
              "2019-08-20_10-41-18": ["40027089", "40029628", "40030065", "40031951", "40033113", "40033116",
                                      "AC01324954", "AC01324955", "AC01324968", "AC01324969"],
              "2019-08-21_09-49-05": ["40027089", "40029628", "40030065", "40031951", "40033113", "40033116",
                                      "AC01324954", "AC01324955", "AC01324968", "AC01324969"],
              "2019-04-16_14-35-00": ["22970285", "22970286", "22970288", "22970289", "22970290", "22970291",
                                      "AC01324954", "AC01324955", "AC01324968", "AC01324969"],
              "2019-08-20_11-32-05": ["40027089", "40029628", "40030065", "40031951", "40033113", "40033116",
                                      "AC01324954", "AC01324955", "AC01324968", "AC01324969"],
              "2019-08-21_12-10-13": ["40027089", "40029628", "40030065", "40031951", "40033113", "40033116",
                                      "AC01324954", "AC01324955", "AC01324968", "AC01324969"]}

print(cv2.__version__)

def get_timestamp(dataname):
    ts = dataname.split("_")[-1].split(".")[0]
    return ts

def project():
    pass


def imgs2dict(root, folder_class):
    folder_path = root + folder_class + "/images"
    cam_ids = CAMERA_IDs[folder_class]
    print(cam_ids)
    times_per_id = {}
    for cam_id in cam_ids:
        times_per_id[cam_id] = []

    imgs = os.listdir(folder_path)
    for img in imgs:
        camera_id, time_w_jpg = img.split("_")
        time = int(time_w_jpg.split(".")[0])
        times_per_id[camera_id].append(time)

    num_total = 0
    for key in times_per_id.keys():
        num_imgs = len(times_per_id[key])
        num_total += num_imgs
        print(num_imgs)
        times_per_id[key].sort()
    print("Total: ", num_total)
    return times_per_id


def lidars2dict(root, folder_class):
    folder_path = root + folder_class + "/pointclouds_data"
    lidar_ids = ["lidar0", "lidar1"]
    times_per_id = {}
    for lidar_id in lidar_ids:
        times_per_id[lidar_id] = []

    pcds = os.listdir(folder_path)
    for pcd in pcds:
        lidar_id, time_w_jpg = pcd.split("_")
        time = int(time_w_jpg.split(".")[0])
        times_per_id[lidar_id].append(time)

    num_total = 0
    for key in times_per_id.keys():
        num_imgs = len(times_per_id[key])
        num_total += num_imgs
        print(num_imgs)
        times_per_id[key].sort()
    print("Total: ", num_total)
    return times_per_id

def search_nearest_ts(cam_ts, lidar_ts):

    global T_STEP
    i = 0
    min_length = len(cam_ts)
    nearest_index = 0
    nearest_ts = []
    for i in range(min_length):
        for j in range(nearest_index, len(lidar_ts)-1):
            tc = cam_ts[i]
            t1, t2 = lidar_ts[j:j+2]
            t_diff1 = tc - t1
            t_diff2 = tc - t2
            # In case of intial stage
            if t_diff1 * t_diff2 <= 0:  # each has different sign
                if abs(t_diff1) <= abs(t_diff2):
                    nearest_ts.append(lidar_ts[j])
                elif abs(t_diff1) > abs(t_diff2):
                    nearest_ts.append(lidar_ts[j+1])
                else:
                    raise ValueError("Sth wrong?")
                nearest_index = j+1
                break
            else:
                if t_diff2 >= 0:
                    continue  # t_lidar <<< t_cam
                elif t_diff1 < 0:
                    break    # t_lidar >>>> t_cam

    assert len(cam_ts) == len(nearest_ts)

    return nearest_ts

def get_pose(id, timestamp):
    global h5_gt
    timestamp_key = id + "_stamp"
    pose_key = id + "_pose"

    stamps = h5_gt.get(timestamp_key)[()]
    rows, cols = np.where(stamps == int(timestamp))
    row = rows[0]
    poses_src = h5_gt.get(pose_key)[()]
    pose = poses_src[row:row + 1, :]

    return pose

def get_pc_homo(pc_path):
    l0 = open3d.io.read_point_cloud(pc_path)
    pc = np.asarray(l0.points)
    pcs_np = np.transpose(pc)
    len_pc = pcs_np.shape[1]
    ones = np.ones((1, len_pc))
    pcs_homo = np.concatenate((pcs_np, ones), axis=0)

    return pcs_homo


def is_in_the_region(pose_col, limit_lower, limit_upper):
    upper = pose_col >= limit_lower
    lower = pose_col < limit_upper
    is_in = np.bitwise_and(upper, lower)
    return is_in


def remove_backward_pcs(pcs, depth):
    is_front = depth[0, :] > 0
    depth_f = depth[:, is_front]
    pcs_f = pcs[:, is_front]
    return pcs_f, depth_f

def remove_outside_pixels(pixels, h, w):
    x_in = is_in_the_region(pixels[0, :], 0, w)
    y_in = is_in_the_region(pixels[1, :], 0, h)
    is_in = np.bitwise_and(x_in, y_in)
    pixels_filtered = pixels[:, is_in]
    return pixels_filtered, is_in


def filter_outliers(pcs_hnorm, depth, h, w):
    pcs_f1, depth_f1 = remove_backward_pcs(pcs_hnorm, depth)
    pcs_f2, is_in = remove_outside_pixels(pcs_f1, h, w)
    depth_f2 = depth_f1[:, is_in]
    return pcs_f2, depth_f2


def project_depth(pixel, depth, h, w):
    pixel = pixel.astype(np.int)
    num_pcs = pixel.shape[1]

    proj_d = np.zeros((h, w))
    for i in range(num_pcs):
        x = pixel[0, i]
        y = pixel[1, i]
        d = depth[0, i]
        proj_d[y, x] = d
    return proj_d



def calc_depth(pcs, P_mat):
    ''' Refer to multiview geometry p.162 '''
    M = P_mat[:3, :3]
    det_M = np.linalg.det(M)
    sign = None
    if det_M > 0:
        sign = 1
    else:
        sign = -1
    m3 = M[:, 2]
    m3_2norm_value = np.sqrt(np.sum(np.power(m3, 2)))

    w = pcs[-1:, :].copy()

    depth = sign * w / m3_2norm_value

    return depth


def exaggerate(pixel, sparse_d_viz, target, r=5):
    num_pix = pixel.shape[1]
    viz_new = target.copy()
    draw = ImageDraw.Draw(viz_new)
    for i in range(num_pix):
        x = pixel[0, i]
        y = pixel[1, i]
        loc = x, y
        color = sparse_d_viz.getpixel(loc)
        draw.ellipse((x-r, y-r, x+r, y+r), fill=color, outline=color)
    # viz_new.show()
    return viz_new


def colored_depthmap(depth, d_min=None, d_max=None):
    if d_min is None:
        d_min = np.min(depth)
    if d_max is None:
        d_max = np.max(depth)
    depth_relative = (depth - d_min) / (d_max - d_min)
    colored = 255 * cmap(depth_relative)[:, :, :3]  # H, W, C

    return colored.astype(np.uint8)

def get_concat_h(im1, im2):
    dst = Image.new('RGB', (im1.width + im2.width, im1.height))
    dst.paste(im1, (0, 0))
    dst.paste(im2, (im1.width, 0))
    return dst


if __name__ == "__main__":
    mode = "train"
    root = "/home/shapelim/ws/dataset/1f/{}/".format(mode)
    DIR_1F_TRAIN = ["2019-04-16_14-35-00", "2019-08-20_11-32-05"]
    # for target_dir in DIR_1F_TRAIN:
    target_path = os.path.join(root, DIR_1F_TRAIN[0])
    times_per_cam = imgs2dict(root, DIR_1F_TRAIN[0])
    times_per_lidar = lidars2dict(root, DIR_1F_TRAIN[0])
    gt_path = os.path.join(target_path, 'groundtruth.hdf5')
    param_path = os.path.join(target_path, 'camera_parameters.txt')
    intrinsics = parse_camera_params(param_path)

    cam_keys = times_per_cam.keys()
    lidar_keys = times_per_lidar.keys()

    lidar_id = "lidar0"
    k = 1
    is_initial = True
    for camera_id in cam_keys:
        if "AC" in camera_id:
            continue
        cam_ts_list = times_per_cam[camera_id]
        nearest_ts = search_nearest_ts(cam_ts_list, times_per_lidar[lidar_id])

        h5_gt = h5py.File(gt_path, mode="r")

        pc_path = os.path.join(target_path, "pointclouds_data", lidar_id + "_" + str(nearest_ts[k]) + ".pcd")
        img_path = os.path.join(target_path, "images", camera_id + "_" + str(cam_ts_list[k]) + ".jpg")

        timestamp_key = camera_id + "_stamp"
        pose_key = camera_id + "_pose"
        # get corresponding poses of camera and lidar
        pose_c = get_pose(camera_id, cam_ts_list[k])
        pose_l = get_pose(lidar_id, nearest_ts[k])

        T_c_l = get_T_c_l(pose_c, pose_l)

        K = intrinsics[camera_id].K
        D = intrinsics[camera_id].D
        h = intrinsics[camera_id].h
        w = intrinsics[camera_id].w
        P_mat = np.matmul(K, T_c_l)

        pcs = get_pc_homo(pc_path)
        pcs_c_l = np.matmul(T_c_l, pcs)
        pcs_img_plane = np.matmul(K, pcs_c_l)

        depth = calc_depth(pcs_img_plane, P_mat)
        z = pcs_img_plane[-1:, :].copy()
        pcs_hnorm = pcs_img_plane / z

        assert depth.shape[1] == pcs_hnorm.shape[1]

        pixels_filtered, depths_filtered = filter_outliers(pcs_hnorm, depth, h, w)

        proj_d = project_depth(pixels_filtered, depths_filtered, h, w)

        # visualization
        viz = colored_depthmap(proj_d, 0, np.amax(depths_filtered))
        viz = Image.fromarray(viz)
        img = Image.open(img_path)
        viz_exaggerate = exaggerate(pixels_filtered, viz, img)
        w, h = viz_exaggerate.size
        ratio = 0.2
        if is_initial:
            viz_merge = viz_exaggerate.resize((int(w * ratio), int(h * ratio)))
            is_initial = False
        else:
            viz_ = viz_exaggerate.resize((int(w * ratio), int(h * ratio)))
            viz_merge = get_concat_h(viz_, viz_merge)
    viz_merge.show()
    # from labs_dataloaders.load_inputs import viz_pcd
    # viz_pcd(pc_path)
    viz_merge.save("test.png")
    h5_gt.close()