project_vel_to_cam.py

# !/usr/bin/python
#
# Demonstrates how to project velodyne points to camera imagery.

# Requires a binary velodyne sync file (image-synced) , undistorted image and a dir, containing camera calibration files.
#
# Provided by NCLT dataset authors, modified by Aljosa Osep (osep@vision.rwth-aachen.de)
#
# To use:
#
#    python project_vel_to_cam.py vel img cam_num
#
#       vel:  The velodyne binary file (timestamp.bin)
#       img:  The undistorted image (timestamp.tiff)
#       calib_dir: Dir containing camera calibration files.
#       cam_num:  The index (0 through 5) of the camera
#

import sys
import struct
import matplotlib.pyplot as plt
import matplotlib.image as mpimg
import numpy as np
import os

#from undistort import *

def convert(x_s, y_s, z_s):

    scaling = 0.005 # 5 mm
    offset = -100.0

    x = x_s * scaling + offset
    y = y_s * scaling + offset
    z = z_s * scaling + offset

    return x, y, z

def load_vel_hits(filename):

    f_bin = open(filename, "r")

    hits = []

    while True:

        x_str = f_bin.read(2)
        if x_str == '': # eof
            break

        x = struct.unpack('<H', x_str)[0]
        y = struct.unpack('<H', f_bin.read(2))[0]
        z = struct.unpack('<H', f_bin.read(2))[0]
        i = struct.unpack('B', f_bin.read(1))[0]
        l = struct.unpack('B', f_bin.read(1))[0]

        x, y, z = convert(x, y, z)

        # Load in homogenous
        hits += [[x, y, z, 1]]

    f_bin.close()
    hits = np.asarray(hits)

    return hits.transpose()

def ssc_to_homo(ssc):

    # Convert 6-DOF ssc coordinate transformation to 4x4 homogeneous matrix
    # transformation

    sr = np.sin(np.pi/180.0 * ssc[3])
    cr = np.cos(np.pi/180.0 * ssc[3])

    sp = np.sin(np.pi/180.0 * ssc[4])
    cp = np.cos(np.pi/180.0 * ssc[4])

    sh = np.sin(np.pi/180.0 * ssc[5])
    ch = np.cos(np.pi/180.0 * ssc[5])

    H = np.zeros((4, 4))

    H[0, 0] = ch*cp
    H[0, 1] = -sh*cr + ch*sp*sr
    H[0, 2] = sh*sr + ch*sp*cr
    H[1, 0] = sh*cp
    H[1, 1] = ch*cr + sh*sp*sr
    H[1, 2] = -ch*sr + sh*sp*cr
    H[2, 0] = -sp
    H[2, 1] = cp*sr
    H[2, 2] = cp*cr

    H[0, 3] = ssc[0]
    H[1, 3] = ssc[1]
    H[2, 3] = ssc[2]

    H[3, 3] = 1

    return H

def project_vel_to_cam(hits, cam_num, calib_dir):

    # Load camera parameters
    K = np.loadtxt(os.path.join(calib_dir, 'K_cam%d.csv' % (cam_num)), delimiter=',')
    x_lb3_c = np.loadtxt(os.path.join(calib_dir,'x_lb3_c%d.csv' % (cam_num)), delimiter=',')

    # Other coordinate transforms we need
    x_body_lb3 = [0.035, 0.002, -1.23, -179.93, -0.23, 0.50]

    # Now do the projection
    T_lb3_c = ssc_to_homo(x_lb3_c)
    T_body_lb3 = ssc_to_homo(x_body_lb3)

    T_lb3_body = np.linalg.inv(T_body_lb3)
    T_c_lb3 = np.linalg.inv(T_lb3_c)

    T_c_body = np.matmul(T_c_lb3, T_lb3_body)

    hits_c = np.matmul(T_c_body, hits)
    hits_im = np.matmul(K, hits_c[0:3, :])

    return hits_im

def main(args):

    if len(args)<5:
        print  """Incorrect usage.

        To use:

           python project_vel_to_cam.py vel img cam_num

              vel:  The velodyne binary file (timestamp.bin)
              img:  The undistorted image (timestamp.tiff)
              calib_dir: Dir containing camera calib files.
          cam_num:  The index (0 through 5) of the camera
        """
        return 1


    # Load velodyne points
    hits_body = load_vel_hits(args[1])

    # Load image
    image = mpimg.imread(args[2])

    # Calib dir
    calib_dir = args[3]

    if not os.path.isdir(calib_dir):
        print ('Error, dir %s does not exist!'%calib_dir)
        sys.exit()

    cam_num = int(args[4])

    hits_image = project_vel_to_cam(hits_body, cam_num, calib_dir)

    x_im = hits_image[0, :]/hits_image[2, :]
    y_im = hits_image[1, :]/hits_image[2, :]
    z_im = hits_image[2, :]

    idx_infront = z_im>0
    x_im = x_im[idx_infront]
    y_im = y_im[idx_infront]
    z_im = z_im[idx_infront]

    plt.figure(1)
    plt.imshow(image)
    plt.hold(True)
    plt.scatter(x_im, y_im, c=z_im, s=5, linewidths=0)
    plt.xlim(0, 1616)
    plt.ylim(0, 1232)
    plt.show()

    return 0

if __name__ == '__main__':
    sys.exit(main(sys.argv))