def get_uvd(image_dir, laser_dir, poses_file, models_dir, extrinsics_dir, image_idx): model = CameraModel(models_dir, image_dir) extrinsics_path = os.path.join(extrinsics_dir, model.camera + '.txt') with open(extrinsics_path) as extrinsics_file: extrinsics = [float(x) for x in next(extrinsics_file).split(' ')] G_camera_vehicle = build_se3_transform(extrinsics) G_camera_posesource = None poses_type = re.search('(vo|ins|rtk)\.csv', poses_file).group(1) if poses_type in ['ins', 'rtk']: with open(os.path.join(extrinsics_dir, 'ins.txt')) as extrinsics_file: extrinsics = next(extrinsics_file) G_camera_posesource = G_camera_vehicle * build_se3_transform([float(x) for x in extrinsics.split(' ')]) else: # VO frame and vehicle frame are the same G_camera_posesource = G_camera_vehicle timestamps_path = os.path.join(image_dir, os.pardir, model.camera + '.timestamps') if not os.path.isfile(timestamps_path): timestamps_path = os.path.join(image_dir, os.pardir, os.pardir, model.camera + '.timestamps') timestamp = 0 with open(timestamps_path) as timestamps_file: for i, line in enumerate(timestamps_file): if i == image_idx: timestamp = int(line.split(' ')[0]) pointcloud, reflectance = build_pointcloud(laser_dir, poses_file, extrinsics_dir, timestamp - 1e7, timestamp + 1e7, timestamp) pointcloud = np.dot(G_camera_posesource, pointcloud) #print(pointcloud) #print('pose', G_camera_posesource.shape) #print('pc', pointcloud[0].shape) image_path = os.path.join(image_dir, str(timestamp) + '.png') image = load_image(image_path, model) uv, depth = model.project(pointcloud, image.shape) # plt.imshow(image) # plt.scatter(np.ravel(uv[0, :]), np.ravel(uv[1, :]), s=2, c=depth, edgecolors='none', cmap='jet') # plt.xlim(0, image.shape[1]) # plt.ylim(image.shape[0], 0) # plt.xticks([]) # plt.yticks([]) # plt.show() return uv, depth, timestamp, model
def get_next_split(i_start, max_trajectory_size, trajectory_ned, lidar_dir, extrinsics_dir='robotcar-dataset-sdk/extrinsics'): split = namedtuple('Split', 'i_end, end_of_route, pcl_ned, reflectance') state_first_frame = trajectory_ned[:,i_start] i_max = min(i_start+max_trajectory_size, trajectory_ned.shape[1]) prev_pos = state_first_frame[:3] dist_to_20 = 20 split.i_end = None i_pcl_end = None n_submaps_20 = 0 for i in range(i_start+1,i_max): curr_pos = trajectory_ned[[0,1,2],i] dist_to_20 -= np.linalg.norm(prev_pos - curr_pos) prev_pos = curr_pos if dist_to_20 < 0: if i_pcl_end is not None: split.i_end = i_pcl_end n_submaps_20 += 1 i_pcl_end = i dist_to_20 = 20 print('Get next split: i_start {} i_end {} i_pcl_end {} n_submaps_20 {}'. format(i_start, split.i_end, i_pcl_end, n_submaps_20)) if split.i_end is None or i_pcl_end is None: split.end_of_route = True else: split.end_of_route = False start_time = state_first_frame[6] end_time = trajectory_ned[6,i_pcl_end] # Build pointcloud in vehicle reference frame print('Build PCL...') pcl_split_veh, split.reflectance = sdk_pcl.build_pointcloud(lidar_dir, ins_data_file, extrinsics_dir, start_time, end_time) print('Done!') # Transform pointlcoud to NED system state_first_frame = state_first_frame.flatten() split.pcl_ned = pcl_trafo(pcl_split_veh, trans_newref=state_first_frame[:3], rot=state_first_frame[3:]) return split
else: # VO frame and vehicle frame are the same G_camera_posesource = G_camera_vehicle timestamps_path = os.path.join(args.image_dir, os.pardir, model.camera + '.timestamps') if not os.path.isfile(timestamps_path): timestamps_path = os.path.join(args.image_dir, os.pardir, os.pardir, model.camera + '.timestamps') timestamp = 0 with open(timestamps_path) as timestamps_file: for i, line in enumerate(timestamps_file): if i == args.image_idx: timestamp = int(line.split(' ')[0]) pointcloud, reflectance = build_pointcloud(args.laser_dir, args.poses_file, args.extrinsics_dir, timestamp - 1e7, timestamp + 1e7, timestamp) pointcloud = np.dot(G_camera_posesource, pointcloud) image_path = os.path.join(args.image_dir, str(timestamp) + '.png') image = load_image(image_path, model) uv, depth = model.project(pointcloud, image.shape) plt.imshow(image) plt.hold(True) plt.scatter(np.ravel(uv[0, :]), np.ravel(uv[1, :]), s=2, c=depth, edgecolors='none', cmap='jet') plt.xlim(0, image.shape[1]) plt.ylim(image.shape[0], 0) plt.xticks([]) plt.yticks([])
def generate_vo_submap(args): # Get trajectory and submap metadata trajectory_ned = import_trajectory_ned(ins_data_file, lidar_timestamp_file) pcl_dir = 'data/reference/submaps_{}m'.format(args.length) metadata_filename = pcl_dir + '/metadata.csv' pcl_metadata = get_pcl_metadata(metadata_filename, seg_idx=args.index) assert pcl_metadata is not None # Get start and end time for VO trajectory generation (begin 30m from submap # start, end where submap ends) i_start = np.argwhere(trajectory_ned[6,:] == pcl_metadata['timestamp_start'])[0,0] dist_to_vo_start = 30 prev_pos = trajectory_ned[:3,i_start] for i_vo_start in range(i_start-1,0,-1): curr_pos = trajectory_ned[[0,1,2],i_vo_start] dist_to_vo_start -= np.linalg.norm(prev_pos - curr_pos) prev_pos = curr_pos if dist_to_vo_start < 0: break dist_to_vo_end = args.length prev_pos = trajectory_ned[:3,i_start] for i_vo_end in range(i_start+1,trajectory_ned.shape[1]): curr_pos = trajectory_ned[[0,1,2],i_vo_end] dist_to_vo_end -= np.linalg.norm(prev_pos - curr_pos) prev_pos = curr_pos if dist_to_vo_end < 0: break # Get timestamp of image considering offset i_img = i_start if args.camera == 'center' and args.offset > 0: dist_to_img = args.offset prev_pos = trajectory_ned[:3,i_start] for i_img in range(i_start-1,0,-1): curr_pos = trajectory_ned[[0,1,2],i_img] dist_to_img -= np.linalg.norm(prev_pos - curr_pos) prev_pos = curr_pos if dist_to_img < 0: break elif args.camera in ['left', 'right']: dist_to_img = args.length / 2 prev_pos = trajectory_ned[:3,i_start] for i_img in range(i_start+1,trajectory_ned.shape[1]): curr_pos = trajectory_ned[[0,1,2],i_img] dist_to_img -= np.linalg.norm(prev_pos - curr_pos) prev_pos = curr_pos if dist_to_img < 0: break # Build pointcloud based on visual odometry timestamp_start = trajectory_ned[6,i_vo_start] timestamp_img = trajectory_ned[6,i_img] timestamp_end = trajectory_ned[6,i_vo_end] print('Build PCL from VO with {} measurements...'.format(i_vo_end-i_vo_start)) pointcloud, refl = sdk_pcl.build_pointcloud(lidar_dir, vo_data_file, extrinsics_dir, timestamp_start, timestamp_end, timestamp_img) print('Done!') # Transform pointcloud to NED and generate submap reference_state = trajectory_ned[:,i_img].flatten() pointcloud_ned = pcl_trafo(pointcloud, trans_newref=reference_state[:3], rot=reference_state[3:]) i_center = np.argwhere(trajectory_ned[6,:] == pcl_metadata['timestamp_center'])[0,0] submap_ned, _ = get_pcl_segment(trajectory_ned, pointcloud_ned, i_center, float(args.length), alignment='trajectory', width=40) submap = pcl_trafo(submap_ned, trans_oldref=-reference_state[:3], rot=-reference_state[3:]) return i_img, submap
timestamp = int(line.split(' ')[0]) Ltimestamps.append(timestamp) #Get timestamps for camera interpolation, interpolate, and lidar point clouds stamp = [] stamp.append(Ctimestamps[0]) camera_poses = [] pointc = [] subset = np.asarray(Ltimestamps[1000:1100]) for i in range(len(subset) - 1): stamp.append(stamp[i] + subset[i + 1] - subset[i]) camera_poses.append(interpolate_vo_poses(vopath, [stamp[i + 1]], stamp[i])) pointc.append( np.asarray( build_pointcloud(lmsFrontDir, vopath, extrinsics, subset[i], subset[i])[0]).transpose()) #Get transformations between lidar scans, and the error+iterations LidarPoses, c = icpLidar(subset) ''' #Plot error against iterations y = [] for i in range (len(c)): y.append(i+1) plt.scatter(y,c) plt.title("Reduction in squared error deviation by Iterative closest point") print(len(c)) plt.xticks(y) plt.xlabel("iterations") plt.ylabel("Mean error")
image = load_image(image_path, model) with open(lidar_timestamps_path) as lidar_timestamps_file: for j, row in enumerate(lidar_timestamps_file): lidar_timestamps = int(row.split(' ')[0]) if (lidar_timestamps > image_timestamp): start_time = image_timestamp - 2e6 end_time = image_timestamp + 2e6 break lidar_timestamps_file.close() pointcloud, reflectance = build_pointcloud(my_params.laser_dir, poses_file, extrinsics_dir, start_time, end_time, lidar_timestamps) pointcloud = np.dot(G_camera_posesource, pointcloud) uv, depth = model.project(pointcloud, image.shape) frame_path = os.path.join(my_params.reprocess_image_dir + '//' + str(image_timestamp) + '.png') frame = cv2.imread(frame_path) # must processed imagte # img2 = load_image(img2_path,model) pointcloud = np.zeros_like(frame) for k in range(uv.shape[1]): x_p = (int)(np.ravel(uv[:, k])[0]) y_p = (int)(np.ravel(uv[:, k])[1])
'.timestamps') timestamp = 0 with open(timestamps_path) as timestamps_file: for i, line in enumerate(timestamps_file): if i == image_idx: # print('index:', i) timestamp = int(line.split(' ')[0]) break # print('index timestamp:', timestamp) # print('start time:', start_time) # print('end time:', end_time) pointcloud, reflectance = build_pointcloud(laser_dir, poses_file, extrinsics_dir, timestamp - 5e7, timestamp + 5e7, timestamp) pointcloud = np.dot(G_camera_posesource, pointcloud) image_path = os.path.join(image_dir, str(timestamp) + '.png') image = load_image(image_path, model) uv, depth = model.project(pointcloud, image.shape) plt.figure(1) plt.imshow(image) plt.scatter(np.ravel(uv[0, :]), np.ravel(uv[1, :]), s=2, c=depth,
def generate_pointcloud(): WRITE_IMAGE = 1 DRAW_MAP = 1 # Data input image_dir = my_params.image_dir ldrms_dir = my_params.laser_dir lmsfront_dir = my_params.lmsfront_dir lmsrear_dir = my_params.lmsrear_dir poses_file = my_params.poses_file models_dir = my_params.model_dir extrinsics_dir = my_params.extrinsics_dir output_img_dir = my_params.output_dir2 + 'pl_img_' + my_params.dataset_no + '//' output_ldrms_dir = my_params.output_dir2 + 'pl_ldrms_' + my_params.dataset_no + '//' output_front_dir = my_params.output_dir2 + 'pl_front_' + my_params.dataset_no + '//' output_rear_dir = my_params.output_dir2 + 'pl_rear_' + my_params.dataset_no + '//' map_ldrms_dir = my_params.output_dir2 + 'map_ldrms_' + my_params.dataset_no + '//' map_front_dir = my_params.output_dir2 + 'map_front_' + my_params.dataset_no + '//' map_rear_dir = my_params.output_dir2 + 'map_rear_' + my_params.dataset_no + '//' model = CameraModel(models_dir, image_dir) extrinsics_path = os.path.join(extrinsics_dir, model.camera + '.txt') with open(extrinsics_path) as extrinsics_file: extrinsics = [float(x) for x in next(extrinsics_file).split(' ')] G_camera_vehicle = build_se3_transform(extrinsics) G_camera_posesource = None poses_type = re.search('(vo|ins|rtk)\.csv', poses_file).group(1) if poses_type in ['ins', 'rtk']: with open(os.path.join(extrinsics_dir, 'ins.txt')) as extrinsics_file: extrinsics = next(extrinsics_file) G_camera_posesource = G_camera_vehicle * build_se3_transform([float(x) for x in extrinsics.split(' ')]) else: # VO frame and vehicle frame are the same G_camera_posesource = G_camera_vehicle image_size = (960,1280,3) timestamps_path = os.path.join(my_params.dataset_patch + model.camera + '.timestamps') timestamp = 0 plt.figure() with open(timestamps_path) as timestamps_file: for i, line in enumerate(timestamps_file): if i < 799: # print('open image index', i) # timestamp = int(line.split(' ')[0]) # break continue timestamp = int(line.split(' ')[0]) start_time = timestamp - 1e6 frame_path = os.path.join(my_params.reprocess_image_dir + '//' + str(timestamp) + '.png') frame = cv2.imread(frame_path) print('process image ',i,'-',timestamp) if i < 4: if(WRITE_IMAGE): savefile_name = output_dir + '\\lms_front_img_' + my_params.dataset_no + '\\' + str(timestamp) + '.png' cv2.imwrite(savefile_name, frame) continue pl_ldrms = np.zeros((960,1280),dtype=int) pl_front = np.zeros((960,1280),dtype=int) # LDRMS ldrms_pointcloud, _ = build_pointcloud(ldrms_dir, poses_file, extrinsics_dir, start_time, timestamp + 2e6, timestamp) ldrms_pointcloud = np.dot(G_camera_posesource, ldrms_pointcloud) uv, depth = model.project(ldrms_pointcloud,image_size) x_p = np.ravel(uv[0,:]) y_p = np.ravel(uv[1,:]) z_p = np.ravel(depth) map_ldrms = (np.array(ldrms_pointcloud[0:3,:])).transpose() map_ldrms_filename = map_ldrms_dir + str(timestamp) + '.csv' np.savetxt(map_ldrms_filename, map_ldrms, delimiter=",") if (DRAW_MAP): map_x = [numeric_map_x for numeric_map_x in np.array(ldrms_pointcloud[0,:])] map_y = [numeric_map_y for numeric_map_y in np.array(ldrms_pointcloud[1,:])] map_z = np.array(ldrms_pointcloud[2,:]) plt.scatter((map_y),(map_x),c='b',marker='.', zorder=1) # LDRMS pointcloud to CSV # for k in range(uv.shape[1]): # pl_ldrms[int(y_p[k]),int(x_p[k])] = int(100*depth[k]) # ldrms_filename = output_ldrms_dir + str(timestamp) + '.csv' # np.savetxt(ldrms_filename, pl_ldrms, delimiter=",") # LMS-FRONT front_pointcloud, _ = build_pointcloud(lmsfront_dir, poses_file, extrinsics_dir, start_time, timestamp + 1e6, timestamp) front_pointcloud = np.dot(G_camera_posesource, front_pointcloud) wh,xrange = model.project(front_pointcloud,image_size) x_f = np.ravel(wh[0,:]) y_f = np.ravel(wh[1,:]) z_f = np.ravel(xrange) map_front = (np.array(front_pointcloud[0:3,:])).transpose() map_front_filename = map_front_dir + str(timestamp) + '.csv' np.savetxt(map_front_filename, map_front, delimiter=",") if (DRAW_MAP): map_x = [numeric_map_x for numeric_map_x in np.array(front_pointcloud[0,:])] map_y = [numeric_map_y for numeric_map_y in np.array(front_pointcloud[1,:])] map_z = [-numeric_map_z for numeric_map_z in np.array(front_pointcloud[2,:])] plt.scatter((map_y),(map_z),c='r',marker='.', zorder=1) # LMS-FRONT pointcloud to CSV # for k in range(wh.shape[1]): # pl_ldrms[int(y_f[k]),int(x_f[k])] = int(100*xrange[k]) # front_filename = output_front_dir + str(timestamp) + '.csv' # np.savetxt(front_filename, pl_ldrms, delimiter=",") # LMS-REAR rear_pointcloud, _ = build_pointcloud(lmsrear_dir, poses_file, extrinsics_dir, start_time, timestamp + 2e6, timestamp) rear_pointcloud = np.dot(G_camera_posesource, rear_pointcloud) map_rear = (np.array(rear_pointcloud[0:3,:])).transpose() map_rear_filename = map_rear_dir + str(timestamp) + '.csv' np.savetxt(map_rear_filename, map_rear, delimiter=",") if (DRAW_MAP): map_x = [numeric_map_x for numeric_map_x in np.array(rear_pointcloud[0,:])] map_y = [-numeric_map_y for numeric_map_y in np.array(rear_pointcloud[1,:])] map_z = [numeric_map_z for numeric_map_z in np.array(rear_pointcloud[2,:])] plt.scatter((map_y),(map_z),c='g',marker='.', zorder=1) if (WRITE_IMAGE): for k in range(uv.shape[1]): color = (int(255-8*depth[k]),255-3*depth[k],50+3*depth[k]) frame= cv2.circle(frame, (int(x_p[k]), int(y_p[k])), 1, color, 1) for k in range(wh.shape[1]): color = (int(255-8*xrange[k]),255-3*xrange[k],50+3*xrange[k]) frame= cv2.circle(frame, (int(x_f[k]), int(y_f[k])), 1, color, 1) cv2.imshow('frame',frame) image_filename = output_img_dir + str(timestamp) + '.png' cv2.imwrite(image_filename, frame) cv2.waitKey(1) # plt.show() plt.pause(0.1)