from copy import deepcopy import cv2 import math import matplotlib.pyplot as plt import numpy as np import open3d as o3d

### Reading the bins into np arrays frame_data = [] for i in range(10, 87): file_name = "./data/LiDAR/0000" + str(i) + ".bin" curr_frame = np.fromfile(file_name, dtype='float32') frame_data.append(curr_frame) frame_data = np.array(frame_data,dtype=object)

### Extracting the xyz coordinates from frame_data frame_xyz = [] for i in range(0,77): l = frame_data[i].shape[0] curr_frame_xyz = [] for j in range(0,l,4): curr_frame_xyz.append(np.array([frame_data[i][j], frame_data[i][j+1], frame_data[i][j+2]])) frame_xyz.append(np.array(curr_frame_xyz)) frame_xyz = np.array(frame_xyz,dtype=object)

### Reading data from odometry.txt and converting to pose odometry_file = open("./data/odometry.txt") number_of_lines = 77 odometry = [] for i in range(number_of_lines): line = odometry_file.readline() line_data = line.split(" ") line_matrix = list(map(float, text_arr)) line_matrix = np.array(line_matrix) line_matrix = np.reshape(line_matrix, (3,4)) odometry.append(num_arr) odometry = np.array(odometry)

### Visualising LiDAR Frame 0, 20, 40 as PCDs for i in range(0,3): frame_pcd = o3d.geometry.PointCloud() frame_pcd.points = o3d.utility.Vector3dVector(fram_xyz[i*20]) o3d.visualization.draw_geometries([frame_pcd])

### Transfrmation matrix from LiDAR frame to Camera frame lidar_to_camera_rot = np.transpose(np.array([ [0,0,1,0], [-1,0,0,0], [0,-1,0,0], [0,0,0,1] ]))

### Transforming frame data into camera frame camera_frame_xyz = [] for i in range(frame_xyz.shape[0]): curr_camera_frame_xyz = [] for j in range(frame_xyz[i].shape[0]): homogenous_point_xyz = np.array( [frame_xyz[i][j][0], frame_xyz[i][j][1], frame_xyz[i][j][2], 1]) camera_point_xyz = np.matmul(lidar_to_camera_rot, homogenous_point_xyz) curr_camera_frame_xyz.append(camera_point_xyz) curr_camera_frame_xyz = np.array(curr_camera_frame_xyz) camera_frame_xyz.append(curr_camera_frame_xyz) camera_frame_xyz = np.array(camera_frame_xyz)

### world = np.array([ [0,0,1, 0], [-1,0,0,0], [0,-1,0,0], [0,0,0,1] ])

### Convert Odometry Transformations matrices to Homogeneous form homogeneous = [] for i in range(0,77): curr_homogeneous = np.array([ [odometry[i][0][0], odometry[i][0][1], odometry[i][0][2], odometry[i][0][3]], [odometry[i][1][0], odometry[i][1][1], odometry[i][1][2], odometry[i][1][3]], [odometry[i][2][0], odometry[i][2][1], odometry[i][2][2], odometry[i][2][3]], [0, 0, 0, 1] ]) homogeneous.append(curr_homogeneous) homogenous = np.array(homogenous, dtype=object)

### Defining Cam-World Transformation Matrices & World-Cam Transformation Matrices cam_to_world_transform = [] world_to_cam_transform = [] for i in range(0,77): cam_to_world_transform.append(np.matmul(world, homogeneous[i])) world_to_cam_transform.append(np.linalg.inv(np.matmul(world, homogeneous[i])))

### Collecting all data points in common(world) frame common_frame_xyz = [] for i in range(0,77): l = camera_frame_xyz[i].shape[0] for j in range(0,l): common_frame_xyz.append(np.matmul(homogeneous[i], camera_frame_xyz[i][j])[0:3]) common_frame_xyz = np.array(common_frame_xyz, dtype=object)

### visualising common frame data point cloud common_frame_pcd = o3d.geometry.PointCloud() common_frame_pcd.points = o3d.utility.Vector3dVector(common_frame_xyz) o3d.visualization.draw_geometries([common_frame_pcd])

### Function to transform the registered point cloud from the world to the ith camera frame def convert_to_camera_i(i,pcd_w): o3d.visualization.draw_geometries([pcd_w.transform(world_to_cam[i])]) return pcd_w.transform(world_to_cam[i])

for i in range(0,77,10): convert_to_camera_i(i, deepcopy(common_frame_pcd))

### Create occupancy map occupancy_map = [] limits = [] vs = 1 #voxel size for i in range(0, 77): min_x = 100000000 max_x = -100000000 min_y = 100000000 max_y = -100000000 for p in range(frame_xyz[i].shape[0]): min_x = min(min_x, frame_xyz[i][p][0]) max_x = max(max_x, frame_xyz[i][p][0]) min_y = min(min_y, frame_xyz[i][p][1]) max_y = max(max_y, frame_xyz[i][p][1]) limits.append(np.array([math.floor(min_x * vs), math.floor(min_y * vs)])) curr_bin_map = np.zeros(( math.floor(max_x * vs) - math.floor(min_x * vs) +1, math.floor(max_y * vs) - math.floor(min_y* vs) +1 )) for j in range(xyz[i].shape[0]): curr_bin_map[ math.floor(frame_xyz[i][j][0] * vs) - limits[i][0]][ math.floor(frame_xyz[i][j][1] * vs) - limits[i][1]]+=1 occupancy_map.append(curr_bin_map) occupancy_map = np.array(occupancy_map, dtype=object)

### marking occupied/unoccupied based on thresold threshold = 10 binary_occupancy_map = deepcopy(occupancy_map) for i in range(occupancy_map.shape[0]): for x in range(occupancy_map[i].shape[0]): for y in range(occupancy_map[i].shape[1]): if(occupancy_map[i][x][y] >= threshold): binary_occupancy_map[i][x][y]=1 else: binary_occupancy_map[i][x][y]=0

# Visualisation of some occupancy maps for it in range(0, 77, 10): occupancy_map_xyz = [] for i in range(binary_occupancy_map[it].shape[0]): for j in range(binary_occupancy_map[it].shape[1]): if(binary_occupancy_map[it][i][j]): occupancy_map_xyz.append(np.array([limits[it][0] + i, limits[it][1] + j, 0])) occupancy_map_pcd = o3d.geometry.PointCloud() occupancy_map_pcd.points = o3d.utility.Vector3dVector(occupancy_map_xyz) voxel_grid = o3d.geometry.VoxelGrid.create_from_point_cloud(occupancy_map_pcd, voxel_size=1) o3d.visualization.draw_geometries([voxel_grid])

### Creating common occupancy map min_x = 100000000 max_x = -100000000 min_y = 100000000 max_y = -100000000 for f in range(common_frame_xyz.shape[0]): min_x = min(min_x, common_frame_xyz[f][0]) max_x = max(max_x, common_frame_xyz[f][0]) min_y = min(min_y, common_frame_xyz[f][1]) max_y = max(max_y, common_frame_xyz[f][1]) common_limits = np.array([math.floor(min_x * vs), math.floor(min_y * vs)]) common_occupancy_map = np.zeros(( math.floor(max_x * vs) - math.floor(min_x * vs) +1, math.floor(max_y * vs) - math.floor(min_y* vs) +1 )) for j in range(all_common.shape[0]): common_occupancy_map[ math.floor(all_common[j][0] * vs) - common_limits[0]][ math.floor(all_common[j][1] * vs) - common_limits[1]]+=1

### filtering common occupancy map threshold = 500 common_binary_occupancy_map = deepcopy(common_occupancy_map) for x in range(common_occupancy_map.shape[0]): for y in range(common_occupancy_map.shape[1]): if(common_occupancy_map[x][y] >= threshold): common_binary_occupancy_map[x][y]=1 else: common_binary_occupancy_map[x][y]=0

### visualising common occupancy map common_occupancy_map_xyz = [] for i in range(common_binary_occupancy_map.shape[0]): for j in range(common_binary_occupancy_map.shape[1]): if(common_binary_occupancy_map[i][j]): common_occupancy_map_xyz.append(np.array([common_limits[0] + i, common_limits[1] + j, 0])) common_occupancy_map_pcd = o3d.geometry.PointCloud() common_occupancy_map_pcd.points = o3d.utility.Vector3dVector(common_occupancy_map_xyz) voxel_grid = o3d.geometry.VoxelGrid.create_from_point_cloud(common_occupancy_map_pcd, voxel_size=1) o3d.visualization.draw_geometries([voxel_grid])