Projecting LIDAR pointcloud to camera image without dgp library

[m-zain-khawaja]

Description

Hello, I am trying to perform a simple projection of the LIDAR points into the camera frame of the 1st camera without relying on the dgp library. This should be a simple transformation from world coordinates -> camera coordinates -> image coordinates - however, the projection is not working as expected and LIDAR points are misaligned with the image. I am sharing code below, any help or guidance would be most appreciated - thanks!

parseCalib - parsing calibration file to get transformation matrices

def parseCalib(calib_files):
    
    calib_logs = []
    world_to_camera_transforms = []
    camera_intrinsic_matrices = []
    focal_lengths = []
    cy_vals = []


    for i in range (0, len(calib_files)):
        
        # Get filepath to calibration file
        calib_filepath = str(calib_files[i])
        
        # Storing log associated with calibration file
        calib_log = calib_filepath[26:-58]
        calib_logs.append(calib_log)

        # Reading calibration file JSON
        with open(calib_filepath, 'r') as file:
            data = json.load(file)
            camera_rotation = data['extrinsics'][1]['rotation']
            camera_translation = data['extrinsics'][1]['translation']
            camera_intrinsic = data['intrinsics'][1]

      
            # Get the world to camera transformation matrix
            ###############################################
            # Rotation
            qx = camera_rotation['qx']
            qy = camera_rotation['qy']
            qz = camera_rotation['qz']
            qw = camera_rotation['qw']
            quat = Quaternion(qw, qx, qy, qz)
            world_to_camera_rotation_matrix = quat.rotation_matrix

            # Translation
            tvec=np.float32([0., 0., 0.])
            tvec[0] = camera_translation['x']
            tvec[1] = camera_translation['y']
            tvec[2] = camera_translation['z']
           
            # Full transform
            world_to_camera_projection_matrix = np.eye(4)
            world_to_camera_projection_matrix[:3,:3] = world_to_camera_rotation_matrix
            world_to_camera_projection_matrix[:3, 3] = tvec
            
            # Get camera to image projection matrix
            #######################################
            camera_to_image_projection_matrix = np.eye(3)
            camera_to_image_projection_matrix[0, 0] = camera_intrinsic['fx']
            camera_to_image_projection_matrix[1, 1] = camera_intrinsic['fy']
            camera_to_image_projection_matrix[0, 2] = camera_intrinsic['cx']
            camera_to_image_projection_matrix[1, 2] = camera_intrinsic['cy']
            camera_to_image_projection_matrix[0, 1] = camera_intrinsic['skew']

            # Append to list
            world_to_camera_transforms.append(world_to_camera_projection_matrix)
            camera_intrinsic_matrices.append(camera_to_image_projection_matrix)
            focal_lengths.append(camera_intrinsic['fy'])
            cy_vals.append(camera_intrinsic['cy'])

    return calib_logs, world_to_camera_transforms, camera_intrinsic_matrices, focal_lengths, cy_vals

projectPointcloudToImage - projecting 3D world point in homogenous coordinates to image

def projectPoincloudToImage(image, pointcloud, world_to_cam_transform, camera_intrinsic, img_height, img_width):
    
    draw = ImageDraw.Draw(image)

    for i in range(0, len(pointcloud)):

      
        world_point = np.float32([0., 0., 0., 1.])
        world_point[0] = pointcloud[i][0]
        world_point[1] = pointcloud[i][1]
        world_point[2] = pointcloud[i][2]
        
        camera_point = world_to_cam_transform.dot(world_point)
        image_point = camera_intrinsic.dot(camera_point[0:3])
        
        image_projection_x = image_point[0]/image_point[2] 
        image_projection_y = image_point[1]/image_point[2]
    
        x_val = image_projection_x
        y_val = image_projection_y
        draw.ellipse((y_val-5, x_val-5, y_val+5, x_val+5), fill=(255, 125, 0))

The data is being read and processed as follows:

   # Filepath for data loading
   root_data_path = '/mnt/media/ddad_train_val/'

   # Reading dataset labels and images and sorting returned list in alphabetical order
   depth_maps = sorted([f for f in pathlib.Path(root_data_path).glob("*/point_cloud/LIDAR/*.npz")])
   images = sorted([f for f in pathlib.Path(root_data_path).glob("*/rgb/CAMERA_01/*.png")])
   calib_files = sorted([f for f in pathlib.Path(root_data_path).glob("*/calibration/*.json")])

   calib_logs, world_to_camera_transforms, camera_intrinsic_matrices, focal_lengths, cy_vals = \
            parseCalib(calib_files)
        
        # Looping through data 
        for index in range(0, 1):
            
            # Image
            image = Image.open(str(images[index]))
            img_width, img_height = image.size

            # Pointcloud
            pointcloud = np.load(str(depth_maps[index]))['data']
            print(pointcloud)

            print(str(depth_maps[index]), str(images[index]))

            # Data log and its index
            data_log = str(images[index])[26:32]
            log_index = calib_logs.index(data_log)
            
            # Projection of pointlcoud point to image coordinates       
            projectPoincloudToImage(image, pointcloud, world_to_camera_transforms[log_index], 
                       camera_intrinsic_matrices[log_index], img_height, img_width)
            
            # Visualization
            plt.figure()
            plt.imshow(image)