Manual Camera-Lidar Calibrator

manual_calib.md

This script is a tool to manual calibrate image and lidar point cloud.

Requirements:

numpy, scipy>=1.2.0, cv2 (opencv),

Input/Output

The input is specified by the variable image_in and cloud_in, the value of these two variables have to be either a path to the file, or a path to the folder containing the data. Point cloud should be stored in txt format with points value starting from the second row. (This could be modified in the code) The output contains three translation parameters (x,y,z) and three rotation parameters (roll,pitch,yaw). The calibration output will be printed when the calibration process finishes.

Usage

Just directly run the script and a window showing the image and projected point cloud will pop up.

• a / d rotate point cloud left / right

• w / s rotate point cloud up / down

• e / q rotate point cloud clock-wise / counterclock-wise

• j / l move point cloud left / right

• i / k move point cloud forward / backward

• u / o move point cloud up / down

• esc stop calibration (and print the result)

• space continue to the next frame (if the input is a folder)

• - / = adjust the movement speed

• [ / ] adjust the transparency

• < / > adjust the point size

manual_calib.py

import os
import cv2
import numpy as np
from scipy.spatial.transform import Rotation as R
import matplotlib.pyplot as plt

image_in = "/home/jacobz/Calibration/working_zone/181202_1s/b5"
cloud_in = "/home/jacobz/Calibration/working_zone/181202_1s/b5"
image_intrinsic = np.array([
    [2166.642447,   0.000000,   985.636040  ],
    [0.000000,      2171.700780,502.607759  ],
    [0.000000,      0.000000,   1.000000    ],
])
image_distort = np.array([-0.630460,0.312053,0.001022,-0.005729])
# rt_init = np.array([0,0,0,np.pi/2,-np.pi/2,0]) # [x y z euler_x euler_y euler_z]
rt_init = np.array([-0.10644462,-0.29416358,-1.16898709,-0.77940823,-1.56247815,2.35042207])

def proj(p, h, k, sz):
    # project
    xyz1 = np.insert(p[:,:3], 3, values=1, axis=1)
    proj = h.dot(xyz1.T)
    zloc = proj[2,:]
    proj = k.dot(proj)
    xloc = proj[0,:] / proj[2,:]
    yloc = proj[1,:] / proj[2,:]

    # calculate depth error
    width, height  = sz
    mask = (xloc > 0) & (xloc < width) & (yloc > 0) & (yloc < height) & (zloc > 1)
    ymask, xmask = yloc[mask].astype(int), xloc[mask].astype(int)
    return ymask, xmask, zloc[mask]

def scatter(img, xyz, s=5, a=0.5):
    dots = img.copy()
    ym, xm, zm = xyz
    zmax, zmin = np.max(zm), np.min(zm)
    zm = 256*(zm - zmin)/(zmax-zmin)
    zm = cv2.applyColorMap(zm.astype('u1'), cv2.COLORMAP_JET)
    for x,y,z in zip(xm,ym,zm):
        color = tuple([int(c) for c in z[0]])
        dots = cv2.circle(dots,(x,y), s, color, -1)
    ret = img.copy()
    cv2.addWeighted(dots, a, ret, 1-a, 0, ret)
    return ret

def get_homo(vec):
    HT,HR = vec[:3], vec[3:]
    HR = R.from_euler('xyz', HR).as_dcm()
    H = np.eye(4)
    H[:3,:3] = HR # rotation
    H[:3, 3] = HT # translation
    return H
    
#################################

if os.path.isfile(image_in):
    image_list = [image_in]
    cloud_list = [cloud_in]
else:
    image_names = sorted(fname[:-4] for fname in os.listdir(image_in) if fname.endswith('.png'))
    cloud_names = sorted(fname[:-4] for fname in os.listdir(image_in) if fname.endswith('.pcd'))
    assert image_names == cloud_names
    image_list = [os.path.join(image_in, fname + '.png') for fname in image_names]
    cloud_list = [os.path.join(cloud_in, fname + '.pcd') for fname in cloud_names]

hwindow = "Projected image"
cv2.namedWindow(hwindow, cv2.WINDOW_NORMAL)
cv2.resizeWindow(hwindow, 1000, 600)
    
dloc = 0.1
dang = 0.01
alpha = 0.7
size = 4

for image_dir, cloud_dir in zip(image_list, cloud_list):
    # read image and cloud
    image = cv2.imread(image_dir)
    cloud = np.loadtxt(cloud_dir, skiprows=11)
    image = cv2.undistort(image, image_intrinsic, image_distort, None, image_intrinsic)
    kfix = np.hstack((image_intrinsic, np.zeros((3,1))))

    hcur = rt_init
    calib_next = True

    # main loop
    while True:
        pimage = scatter(image, proj(cloud, get_homo(hcur), kfix, image.shape[1::-1]), s=size, a=alpha)
        cv2.imshow(hwindow, pimage)

        key = cv2.waitKeyEx(0)
        if key == 27: # Esc key to stop
            calib_next = False
            break
        elif key == ord(' '): break # next picture
        elif key == -1: continue
        elif key == ord('j'): hcur[0] += dloc # move left
        elif key == ord('l'): hcur[0] -= dloc # move right
        elif key == ord('i'): hcur[2] -= dloc # move forward
        elif key == ord('k'): hcur[2] += dloc # move backward
        elif key == ord('u'): hcur[1] += dloc # move up
        elif key == ord('o'): hcur[1] -= dloc # move down
        elif key == ord('a'): # rotate left
            delta = R.from_euler('y', dang)
            newang = delta * R.from_euler('xyz', hcur[3:])
            hcur[3:] = newang.as_euler('xyz')
        elif key == ord('d'): # rotate right
            delta = R.from_euler('y', -dang)
            newang = delta * R.from_euler('xyz', hcur[3:])
            hcur[3:] = newang.as_euler('xyz')
        elif key == ord('w'): # rotate up
            delta = R.from_euler('x', -dang)
            newang = delta * R.from_euler('xyz', hcur[3:])
            hcur[3:] = newang.as_euler('xyz')
        elif key == ord('s'): # rotate down
            delta = R.from_euler('x', dang)
            newang = delta * R.from_euler('xyz', hcur[3:])
            hcur[3:] = newang.as_euler('xyz')
        elif key == ord('q'): # rotate counter-clock
            delta = R.from_euler('z', -dang)
            newang = delta * R.from_euler('xyz', hcur[3:])
            hcur[3:] = newang.as_euler('xyz')
        elif key == ord('e'): # rotate clock
            delta = R.from_euler('z', dang)
            newang = delta * R.from_euler('xyz', hcur[3:])
            hcur[3:] = newang.as_euler('xyz')
        elif key == 61: # quicker
            dloc *= 1.1
            dang *= 1.1
            print("Current location delta: %f, Current angular delta: %f" % (dloc, dang))
        elif key == 45: # slower
            dloc *= 0.9
            dang *= 0.9
            print("Current location delta: %f, Current angular delta: %f" % (dloc, dang))
        elif key == ord('['): alpha -= 0.1 # alpha smaller
        elif key == ord(']'): alpha += 0.1 # alpha bigger
        elif key == ord(','): size -= 1 # size smaller
        elif key == ord('.'): size += 1 # size bigger

        else: print("Unrecognized key:", key)
    
    print("Adjusted parameters:", np.array2string(hcur, separator=','))
    rt_init = hcur

    if not calib_next:
        break

print("Final parameters:", np.array2string(rt_init, separator=','))
cv2.destroyAllWindows()