def process_images(root_dir, camera_description, models_dir): print "Processing images, ", camera_description if 'stereo' in camera_description: timestamps_path = os.path.join(root_dir, 'stereo.timestamps') else: timestamps_path = os.path.join(root_dir, camera_description + '.timestamps') if not os.path.isfile(timestamps_path): raise IOError("Could not find timestamps file") images_dir = os.path.join(root_dir, camera_description) model = CameraModel(models_dir, images_dir) current_chunk = 0 timestamps_file = open(timestamps_path) idx = 0 for line in timestamps_file: tokens = line.split() if len(tokens) < 2: break timestamp_us = long(tokens[0]) timestamp_ns = timestamp_us * long(1000) datetime = dt.utcfromtimestamp(int(tokens[0]) / 1000000) chunk = int(tokens[1]) filename = os.path.join(images_dir, tokens[0] + '.png') if not os.path.isfile(filename): if chunk != current_chunk: print("Chunk " + str(chunk) + " not found") current_chunk = chunk continue current_chunk = chunk filename_processed = os.path.join(images_dir, tokens[0] + '_ds2_undistorted.png') if not os.path.exists(filename_processed): img = load_image(filename, model) im_gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY) im_gray_downscaled = cv2.resize( im_gray, (im_gray.shape[1] / 2, im_gray.shape[0] / 2)) cv2.imwrite(filename_processed, im_gray_downscaled) if idx % 100 == 0: print "At ", idx idx += 1 print "Done with ", camera_description
def import_camera_trajectory(camera_timestamp_file, ins_data_file, camera_dir, models_dir = 'robotcar-dataset-sdk/models'): # Import image rectification and debayering from SDK camera_model = CameraModel(models_dir, camera_dir) # Get timestamps from camera data camera_timestamps = np.genfromtxt(camera_timestamp_file) camera_timestamps = camera_timestamps[:,0].flatten() start_time = camera_timestamps[0] end_time = camera_timestamps[-1] # Check if camera trajectory has already been computed date_of_run = datetime.utcfromtimestamp(start_time*1e-6).strftime('%Y-%m-%d') trajectory_file = 'trajectory/ned_trajectory_{}_{}.npy'.format(camera_model.camera, date_of_run) print('Search for precomputed camera trajectory ' + trajectory_file + '...') try: camera_trajectory = np.load(trajectory_file) print('Done!') except: print('Not found! Create camera trajectory...') # Get trajectory corresponding to camera data camera_timestamp_idx = 0 camera_trajectory = np.empty((7,0)) with open(ins_data_file, 'r') as ins_file: reader = csv.DictReader(ins_file) for row in reader: if int(row['timestamp']) > end_time: break if int(row['timestamp']) < start_time: continue # Get closest corresponding camera timestamp timestamp, camera_timestamp_idx = get_closest_camera_timestamp( int(row['timestamp']), camera_timestamps, camera_timestamp_idx) camera_state = np.array([float(row['northing']), float(row['easting']), float(row['down']), float(row['roll']), float(row['pitch']), float(row['yaw']), timestamp]).reshape(7,1) camera_trajectory = np.append(camera_trajectory, camera_state, axis=1) # Save trajectory print('Save camera trajectory to ' + trajectory_file + '...') np.save(trajectory_file, camera_trajectory) print('Done!') return camera_trajectory, camera_model
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 init_camera_model_posture(): global CAMERA_MODEL global G_CAMERA_POSESOURCE models_dir = "/data/lyh/lab/robotcar-dataset-sdk/models/" CAMERA_MODEL = CameraModel(models_dir, "stereo_centre") #read the camera and ins extrinsics extrinsics_path = "/data/lyh/lab/robotcar-dataset-sdk/extrinsics/stereo.txt" print(extrinsics_path) with open(extrinsics_path) as extrinsics_file: extrinsics = [float(x) for x in next(extrinsics_file).split(' ')] G_camera_vehicle = build_se3_transform(extrinsics) print(G_camera_vehicle) extrinsics_path = "/data/lyh/lab/robotcar-dataset-sdk/extrinsics/ins.txt" print(extrinsics_path) with open(extrinsics_path) as extrinsics_file: extrinsics = [float(x) for x in next(extrinsics_file).split(' ')] G_ins_vehicle = build_se3_transform(extrinsics) print(G_ins_vehicle) G_CAMERA_POSESOURCE = G_camera_vehicle * G_ins_vehicle
from build_pointcloud import build_pointcloud from transform import build_se3_transform from image import load_image from camera_model import CameraModel parser = argparse.ArgumentParser(description='Project LIDAR data into camera image') parser.add_argument('--image_dir', type=str, help='Directory containing images') parser.add_argument('--laser_dir', type=str, help='Directory containing LIDAR scans') parser.add_argument('--poses_file', type=str, help='File containing either INS or VO poses') parser.add_argument('--models_dir', type=str, help='Directory containing camera models') parser.add_argument('--extrinsics_dir', type=str, help='Directory containing sensor extrinsics') parser.add_argument('--image_idx', type=int, help='Index of image to display') args = parser.parse_args() model = CameraModel(args.models_dir, args.image_dir) extrinsics_path = os.path.join(args.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)\.csv', args.poses_file).group(1) if poses_type == 'ins': with open(os.path.join(args.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
args = parser.parse_args() camera = re.search('(stereo|mono_(left|right|rear))', args.dir).group(0) timestamps_path = os.path.join( os.path.join(args.dir, os.pardir, camera + '.timestamps')) if not os.path.isfile(timestamps_path): timestamps_path = os.path.join(args.dir, os.pardir, os.pardir, camera + '.timestamps') if not os.path.isfile(timestamps_path): raise IOError("Could not find timestamps file") model = None if args.models_dir: model = CameraModel(args.models_dir, args.dir) current_chunk = 0 timestamps_file = open(timestamps_path) i = 0 for line in timestamps_file: tokens = line.split() datetime = dt.utcfromtimestamp(int(tokens[0]) / 1000000) chunk = int(tokens[1]) filename = os.path.join(args.dir, tokens[0] + '.png') if not os.path.isfile(filename): if chunk != current_chunk: print("Chunk " + str(chunk) + " not found") current_chunk = chunk continue
def main(_config, seed): global EPOCH, weights if _config['weight'] is not None: weights = _config['weight'] dataset_class = DatasetVisibilityKittiSingle img_shape = (384, 1280) split = 'test' if _config['random_initial_pose']: split = 'test_random' maps_folder = 'local_maps' if _config['maps_folder'] is not None: maps_folder = _config['maps_folder'] if _config['test_sequence'] is None: raise TypeError('test_sequences cannot be None') else: if isinstance(_config['test_sequence'], int): _config['test_sequence'] = f"{_config['test_sequence']:02d}" dataset_val = dataset_class(_config['data_folder'], max_r=_config['max_r'], max_t=_config['max_t'], split=split, use_reflectance=_config['use_reflectance'], maps_folder=maps_folder, test_sequence=_config['test_sequence']) np.random.seed(seed) torch.random.manual_seed(seed) def init_fn(x): return _init_fn(x, seed) num_worker = 6 batch_size = 1 TestImgLoader = torch.utils.data.DataLoader(dataset=dataset_val, shuffle=False, batch_size=batch_size, num_workers=num_worker, worker_init_fn=init_fn, collate_fn=merge_inputs, drop_last=False, pin_memory=False) print(len(TestImgLoader)) models = [] for i in range(len(weights)): if _config['network'].startswith('PWC'): feat = 1 md = 4 split = _config['network'].split('_') for item in split[1:]: if item.startswith('f'): feat = int(item[-1]) elif item.startswith('md'): md = int(item[2:]) assert 0 < feat < 7, "Feature Number from PWC have to be between 1 and 6" assert 0 < md, "md must be positive" model = CMRNet(img_shape, use_feat_from=feat, md=md, use_reflectance=_config['use_reflectance']) else: raise TypeError("Network unknown") checkpoint = torch.load(weights[i], map_location='cpu') saved_state_dict = checkpoint['state_dict'] model.load_state_dict(saved_state_dict) model = model.to(device) model.eval() models.append(model) if i == 0: _config['occlusion_threshold'] = checkpoint['config'][ 'occlusion_threshold'] _config['occlusion_kernel'] = checkpoint['config'][ 'occlusion_kernel'] else: assert _config['occlusion_threshold'] == checkpoint['config'][ 'occlusion_threshold'] assert _config['occlusion_kernel'] == checkpoint['config'][ 'occlusion_kernel'] if _config['save_log']: log_file = f'./results_for_paper/log_seq{_config["test_sequence"]}.csv' log_file = open(log_file, 'w') log_file = csv.writer(log_file) header = ['frame'] for i in range(len(weights) + 1): header += [ f'iter{i}_error_t', f'iter{i}_error_r', f'iter{i}_error_x', f'iter{i}_error_y', f'iter{i}_error_z', f'iter{i}_error_r', f'iter{i}_error_p', f'iter{i}_error_y' ] log_file.writerow(header) show = _config['show'] show = True errors_r = [] errors_t = [] errors_t2 = [] errors_rpy = [] all_RTs = [] prev_tr_error = None prev_rot_error = None for i in range(len(weights) + 1): errors_r.append([]) errors_t.append([]) errors_t2.append([]) errors_rpy.append([]) for batch_idx, sample in enumerate(TestImgLoader): log_string = [str(batch_idx)] lidar_input = [] rgb_input = [] shape_pad = [0, 0, 0, 0] if batch_idx == 0 or not _config['use_prev_output']: # Qui dare posizione di input del frame corrente rispetto alla GT sample['tr_error'] = sample['tr_error'].cuda() sample['rot_error'] = sample['rot_error'].cuda() else: sample['tr_error'] = prev_tr_error sample['rot_error'] = prev_rot_error for idx in range(len(sample['rgb'])): real_shape = [ sample['rgb'][idx].shape[1], sample['rgb'][idx].shape[2], sample['rgb'][idx].shape[0] ] # ProjectPointCloud in RT-pose sample['point_cloud'][idx] = sample['point_cloud'][idx].cuda() pc_rotated = sample['point_cloud'][idx].clone() reflectance = None if _config['use_reflectance']: reflectance = sample['reflectance'][idx].cuda() R = mathutils.Quaternion(sample['rot_error'][idx]) T = mathutils.Vector(sample['tr_error'][idx]) pc_rotated = rotate_back(pc_rotated, R, T) cam_params = sample['calib'][idx].cuda() cam_model = CameraModel() cam_model.focal_length = cam_params[:2] cam_model.principal_point = cam_params[2:] uv, depth, points, refl = cam_model.project_pytorch( pc_rotated, real_shape, reflectance) uv = uv.t().int() depth_img = torch.zeros(real_shape[:2], device='cuda', dtype=torch.float) depth_img += 1000. depth_img = visibility.depth_image(uv, depth, depth_img, uv.shape[0], real_shape[1], real_shape[0]) depth_img[depth_img == 1000.] = 0. projected_points = torch.zeros_like(depth_img, device='cuda') projected_points = visibility.visibility2( depth_img, cam_params, projected_points, depth_img.shape[1], depth_img.shape[0], _config['occlusion_threshold'], _config['occlusion_kernel']) if _config['use_reflectance']: uv = uv.long() indexes = projected_points[uv[:, 1], uv[:, 0]] == depth refl_img = torch.zeros(real_shape[:2], device='cuda', dtype=torch.float) refl_img[uv[indexes, 1], uv[indexes, 0]] = refl[0, indexes] projected_points /= 100. if not _config['use_reflectance']: projected_points = projected_points.unsqueeze(0) else: projected_points = torch.stack((projected_points, refl_img)) rgb = sample['rgb'][idx].cuda() shape_pad[3] = (img_shape[0] - rgb.shape[1]) shape_pad[1] = (img_shape[1] - rgb.shape[2]) rgb = F.pad(rgb, shape_pad) projected_points = F.pad(projected_points, shape_pad) rgb_input.append(rgb) lidar_input.append(projected_points) lidar_input = torch.stack(lidar_input) rgb_input = torch.stack(rgb_input) if show: out0 = overlay_imgs(rgb, lidar_input) cv2.imshow("INPUT", out0[:, :, [2, 1, 0]]) cv2.waitKey(1) pc_GT = sample['point_cloud'][idx].clone() uv, depth, _, refl = cam_model.project_pytorch(pc_GT, real_shape) uv = uv.t().int() depth_img = torch.zeros(real_shape[:2], device='cuda', dtype=torch.float) depth_img += 1000. depth_img = visibility.depth_image(uv, depth, depth_img, uv.shape[0], real_shape[1], real_shape[0]) depth_img[depth_img == 1000.] = 0. projected_points = torch.zeros_like(depth_img, device='cuda') projected_points = visibility.visibility2( depth_img, cam_params, projected_points, depth_img.shape[1], depth_img.shape[0], _config['occlusion_threshold'], _config['occlusion_kernel']) projected_points /= 100. projected_points = F.pad(projected_points, shape_pad) lidar_GT = projected_points.unsqueeze(0).unsqueeze(0) out1 = overlay_imgs(rgb_input[0], lidar_GT) cv2.imshow("GT", out1[:, :, [2, 1, 0]]) # plt.figure() # plt.imshow(out1) # if batch_idx == 0: # # import ipdb; ipdb.set_trace() # out2 = overlay_imgs(sample['rgb'][0], lidar_input[:,:,:,1241]) # plt.figure() # plt.imshow(out2) # io.imshow(lidar_input[0][0].cpu().numpy(), cmap='jet') # io.show() rgb = rgb_input.to(device) lidar = lidar_input.to(device) target_transl = sample['tr_error'].to(device) target_rot = sample['rot_error'].to(device) point_cloud = sample['point_cloud'][0].to(device) reflectance = None if _config['use_reflectance']: reflectance = sample['reflectance'][0].to(device) camera_model = cam_model R = quat2mat(target_rot[0]) T = tvector2mat(target_transl[0]) RT1_inv = torch.mm(T, R) RT1 = RT1_inv.clone().inverse() rotated_point_cloud = rotate_forward(point_cloud, RT1) RTs = [RT1] T_composed = RT1[:3, 3] R_composed = quaternion_from_matrix(RT1) errors_t[0].append(T_composed.norm().item()) errors_t2[0].append(T_composed) errors_r[0].append( quaternion_distance( R_composed.unsqueeze(0), torch.tensor([1., 0., 0., 0.], device=R_composed.device).unsqueeze(0), R_composed.device)) # rpy_error = quaternion_to_tait_bryan(R_composed) rpy_error = mat2xyzrpy(RT1)[3:] rpy_error *= (180.0 / 3.141592) errors_rpy[0].append(rpy_error) log_string += [ str(errors_t[0][-1]), str(errors_r[0][-1]), str(errors_t2[0][-1][0].item()), str(errors_t2[0][-1][1].item()), str(errors_t2[0][-1][2].item()), str(errors_rpy[0][-1][0].item()), str(errors_rpy[0][-1][1].item()), str(errors_rpy[0][-1][2].item()) ] if batch_idx == 0.: print(f'Initial T_erorr: {errors_t[0]}') print(f'Initial R_erorr: {errors_r[0]}') start = 0 # Run model with torch.no_grad(): for iteration in range(start, len(weights)): # Run the i-th network T_predicted, R_predicted = models[iteration](rgb, lidar) if _config['rot_transl_separated'] and iteration == 0: T_predicted = torch.tensor([[0., 0., 0.]], device='cuda') if _config['rot_transl_separated'] and iteration == 1: R_predicted = torch.tensor([[1., 0., 0., 0.]], device='cuda') # Project the points in the new pose predicted by the i-th network R_predicted = quat2mat(R_predicted[0]) T_predicted = tvector2mat(T_predicted[0]) RT_predicted = torch.mm(T_predicted, R_predicted) RTs.append(torch.mm(RTs[iteration], RT_predicted)) rotated_point_cloud = rotate_forward(rotated_point_cloud, RT_predicted) uv2, depth2, _, refl = camera_model.project_pytorch( rotated_point_cloud, real_shape, reflectance) uv2 = uv2.t().int() depth_img2 = torch.zeros(real_shape[:2], device=device) depth_img2 += 1000. depth_img2 = visibility.depth_image(uv2, depth2, depth_img2, uv2.shape[0], real_shape[1], real_shape[0]) depth_img2[depth_img2 == 1000.] = 0. out_cuda2 = torch.zeros_like(depth_img2, device=device) out_cuda2 = visibility.visibility2( depth_img2, cam_params, out_cuda2, depth_img2.shape[1], depth_img2.shape[0], _config['occlusion_threshold'], _config['occlusion_kernel']) if _config['use_reflectance']: uv = uv.long() indexes = projected_points[uv[:, 1], uv[:, 0]] == depth refl_img = torch.zeros(real_shape[:2], device='cuda', dtype=torch.float) refl_img[uv[indexes, 1], uv[indexes, 0]] = refl[0, indexes] refl_img = F.pad(refl_img, shape_pad) out_cuda2 = F.pad(out_cuda2, shape_pad) lidar = out_cuda2.clone() lidar /= 100. if not _config['use_reflectance']: lidar = lidar.unsqueeze(0) else: lidar = torch.stack((lidar, refl_img)) lidar = lidar.unsqueeze(0) if show: out3 = overlay_imgs(rgb[0], lidar, idx=batch_idx) cv2.imshow(f'Iter_{iteration}', out3[:, :, [2, 1, 0]]) cv2.waitKey(1) # if iter == 1: # plt.figure() # plt.imshow(out3) # io.imshow(lidar.cpu().numpy()[0,0], cmap='jet') # io.show() T_composed = RTs[iteration + 1][:3, 3] R_composed = quaternion_from_matrix(RTs[iteration + 1]) errors_t[iteration + 1].append(T_composed.norm().item()) errors_t2[iteration + 1].append(T_composed) errors_r[iteration + 1].append( quaternion_distance( R_composed.unsqueeze(0), torch.tensor([1., 0., 0., 0.], device=R_composed.device).unsqueeze(0), R_composed.device)) # rpy_error = quaternion_to_tait_bryan(R_composed) rpy_error = mat2xyzrpy(RTs[iteration + 1])[3:] rpy_error *= (180.0 / 3.141592) errors_rpy[iteration + 1].append(rpy_error) log_string += [ str(errors_t[iteration + 1][-1]), str(errors_r[iteration + 1][-1]), str(errors_t2[iteration + 1][-1][0].item()), str(errors_t2[iteration + 1][-1][1].item()), str(errors_t2[iteration + 1][-1][2].item()), str(errors_rpy[iteration + 1][-1][0].item()), str(errors_rpy[iteration + 1][-1][1].item()), str(errors_rpy[iteration + 1][-1][2].item()) ] all_RTs.append(RTs[-1]) prev_RT = RTs[-1].inverse() prev_tr_error = prev_RT[:3, 3].unsqueeze(0) prev_rot_error = quaternion_from_matrix(prev_RT).unsqueeze(0) # Qui prev_rt è quanto si discosta l'output della rete rispetto alla GT if _config['save_log']: log_file.writerow(log_string) if _config['save_log']: log_file.close() print("Iterative refinement: ") for i in range(len(weights) + 1): errors_r[i] = torch.tensor(errors_r[i]) * (180.0 / 3.141592) errors_t[i] = torch.tensor(errors_t[i]) * 100 print( f"Iteration {i}: \tMean Translation Error: {errors_t[i].mean():.4f} cm " f" Mean Rotation Error: {errors_r[i].mean():.4f} °") print( f"Iteration {i}: \tMedian Translation Error: {errors_t[i].median():.4f} cm " f" Median Rotation Error: {errors_r[i].median():.4f} °\n") print("-------------------------------------------------------") print("Timings:") for i in range(len(errors_t2)): errors_t2[i] = torch.stack(errors_t2[i]) errors_rpy[i] = torch.stack(errors_rpy[i]) plt.plot(errors_t2[-1][:, 0].cpu().numpy()) plt.show() plt.plot(errors_t2[-1][:, 1].cpu().numpy()) plt.show() plt.plot(errors_t2[-1][:, 2].cpu().numpy()) plt.show() if _config["save_name"] is not None: torch.save( torch.stack(errors_t).cpu().numpy(), f'./results_for_paper/{_config["save_name"]}_errors_t') torch.save( torch.stack(errors_r).cpu().numpy(), f'./results_for_paper/{_config["save_name"]}_errors_r') torch.save( torch.stack(errors_t2).cpu().numpy(), f'./results_for_paper/{_config["save_name"]}_errors_t2') torch.save( torch.stack(errors_rpy).cpu().numpy(), f'./results_for_paper/{_config["save_name"]}_errors_rpy') print("End!")
def main(): args = get_arguments() camera = re.search('(stereo|mono_(left|right|rear))', args.dir).group(0) timestamps_path = os.path.join( os.path.join(args.dir, os.pardir, camera + '.timestamps')) if not os.path.isfile(timestamps_path): timestamps_path = os.path.join(args.dir, os.pardir, os.pardir, camera + '.timestamps') if not os.path.isfile(timestamps_path): raise IOError("Could not find timestamps file") model = None if args.models_dir: model = CameraModel(args.models_dir, args.dir) output_dir = os.curdir if args.output_dir: output_dir = args.output_dir if not os.path.isdir(output_dir): raise IOError(output_dir + "is not an existing folder") result_list = [] count = 0 dictionary = {} t_records = [] p_records = [] angles_records = [] intrinsic_matrix = None with open(args.poses_file) as vo_file: vo_reader = csv.reader(vo_file) headers = next(vo_file) for row in vo_reader: src_image_name = row[0] dst_image_name = row[1] src_image_filename = os.path.join(args.dir, src_image_name + '.png') dst_image_filename = os.path.join(args.dir, dst_image_name + '.png') if not os.path.isfile(src_image_filename) or not os.path.isfile( dst_image_filename): continue if dst_image_name not in dictionary: img, orig_resolution = process_image( load_image(dst_image_filename, model), args.crop, args.scale) dictionary[dst_image_name] = count count = count + 1 result_list.append(list(img)) if src_image_name not in dictionary: img, orig_resolution = process_image( load_image(src_image_filename, model), args.crop, args.scale) dictionary[src_image_name] = count count = count + 1 result_list.append(list(img)) focal_length, principal_point = get_intrinsics_parameters( model.get_focal_length(), model.get_principal_point(), orig_resolution, args.crop, args.scale) src_image_idx = dictionary[src_image_name] dst_image_idx = dictionary[dst_image_name] xyzrpy = [float(v) for v in row[2:8]] rel_pose = build_se3_transform(xyzrpy) t_matrix = rel_pose[0:3] # 3x4 matrix intrinsic_matrix = build_intrinsic_matrix(focal_length, principal_point) p_matrix = intrinsic_matrix * t_matrix t_records.append((t_matrix, src_image_idx, dst_image_idx)) p_records.append((p_matrix, src_image_idx, dst_image_idx)) angles_records.append((xyzrpy, src_image_idx, dst_image_idx)) transf = np.array(t_records, dtype=[('T', ('float64', (3, 4))), ('src_idx', 'int32'), ('dst_idx', 'int32')]) proy = np.array(p_records, dtype=[('P', ('float64', (3, 4))), ('src_idx', 'int32'), ('dst_idx', 'int32')]) angles = np.array(angles_records, dtype=[('ang', ('float64', 6)), ('src_idx', 'int32'), ('dst_idx', 'int32')]) # Solo lo guardo una vez porque es constante para todo el dataset (o deberia serlo) if intrinsic_matrix is not None: save_txt(os.path.join(output_dir, "intrinsic_matrix"), intrinsic_matrix) save_txt(os.path.join(output_dir, "intrinsic_parameters"), [focal_length, principal_point]) #path = os.path.normpath(args.dir) #folders = path.split(os.sep) #compressed_file_path = os.path.join(output_dir, folders[-3]) result = list_to_array(result_list) save_txt(os.path.join(output_dir, 'images_shape'), result.shape, fmt='%i') print result.shape compressed_file_path = os.path.join(output_dir, 'images') savez_compressed(compressed_file_path, result) savez_compressed(os.path.join(output_dir, 't'), transf) savez_compressed(os.path.join(output_dir, 'p'), proy) savez_compressed(os.path.join(output_dir, 'angles'), angles)
# Making save # fourcc = cv2.VideoWriter_fourcc('m', 'p', '4', 'v') # vw = cv2.VideoWriter('laserrange_image.mp4', fourcc, 30, (1280, 960)) lidar = re.search( '(lms_front|lms_rear|ldmrs|velodyne_left|velodyne_right)', my_params.laser_dir).group(0) lidar_timestamps_path = os.path.join(my_params.dataset_patch, lidar + '.timestamps') camera = re.search('(stereo|mono_(left|right|rear))', my_params.image_dir).group(0) camera_timestamps_path = os.path.join( os.path.join(my_params.dataset_patch, camera + '.timestamps')) model = CameraModel(my_params.model_dir, my_params.image_dir) poses_file = my_params.poses_file extrinsics_dir = my_params.extrinsics_dir with open( os.path.join( os.path.join(my_params.extrinsics_dir, camera + '.txt'))) as extrinsics_file: extrinsics = [float(x) for x in next(extrinsics_file).split(' ')] G_camera_vehicle = build_se3_transform(extrinsics) G_camera_posesource = None # VO frame and vehicle frame are the same G_camera_posesource = G_camera_vehicle timestamp = 0
def main(_config, _run, seed): global EPOCH print(_config['loss']) if _config['test_sequence'] is None: raise TypeError('test_sequences cannot be None') else: _config['test_sequence'] = f"{_config['test_sequence']:02d}" print("Test Sequence: ", _config['test_sequence']) dataset_class = DatasetVisibilityKittiSingle occlusion_threshold = _config['occlusion_threshold'] img_shape = (384, 1280) _config["savemodel"] = os.path.join(_config["savemodel"], _config['dataset']) maps_folder = 'local_maps' if _config['maps_folder'] is not None: maps_folder = _config['maps_folder'] dataset = dataset_class(_config['data_folder'], max_r=_config['max_r'], max_t=_config['max_t'], split='train', use_reflectance=_config['use_reflectance'], maps_folder=maps_folder, test_sequence=_config['test_sequence']) dataset_val = dataset_class(_config['data_folder'], max_r=_config['max_r'], max_t=_config['max_t'], split='test', use_reflectance=_config['use_reflectance'], maps_folder=maps_folder, test_sequence=_config['test_sequence']) _config["savemodel"] = os.path.join(_config["savemodel"], _config['test_sequence']) if not os.path.exists(_config["savemodel"]): os.mkdir(_config["savemodel"]) np.random.seed(seed) torch.random.manual_seed(seed) def init_fn(x): return _init_fn(x, seed) dataset_size = len(dataset) # Training and test set creation num_worker = _config['num_worker'] batch_size = _config['batch_size'] TrainImgLoader = torch.utils.data.DataLoader(dataset=dataset, shuffle=True, batch_size=batch_size, num_workers=num_worker, worker_init_fn=init_fn, collate_fn=merge_inputs, drop_last=False, pin_memory=True) TestImgLoader = torch.utils.data.DataLoader(dataset=dataset_val, shuffle=False, batch_size=batch_size, num_workers=num_worker, worker_init_fn=init_fn, collate_fn=merge_inputs, drop_last=False, pin_memory=True) print(len(TrainImgLoader)) print(len(TestImgLoader)) if _config['loss'] == 'simple': loss_fn = ProposedLoss(_config['rescale_transl'], _config['rescale_rot']) elif _config['loss'] == 'geometric': loss_fn = GeometricLoss() loss_fn = loss_fn.to(device) elif _config['loss'] == 'points_distance': loss_fn = DistancePoints3D() elif _config['loss'] == 'L1': loss_fn = L1Loss(_config['rescale_transl'], _config['rescale_rot']) else: raise ValueError("Unknown Loss Function") #runs = datetime.now().strftime('%b%d_%H-%M-%S') + "/" #train_writer = SummaryWriter('./logs/' + runs) #ex.info["tensorflow"] = {} #ex.info["tensorflow"]["logdirs"] = ['./logs/' + runs] if _config['network'].startswith('PWC'): feat = 1 md = 4 split = _config['network'].split('_') for item in split[1:]: if item.startswith('f'): feat = int(item[-1]) elif item.startswith('md'): md = int(item[2:]) assert 0 < feat < 7, "Feature Number from PWC have to be between 1 and 6" assert 0 < md, "md must be positive" model = CMRNet(img_shape, use_feat_from=feat, md=md, use_reflectance=_config['use_reflectance'], dropout=_config['dropout']) else: raise TypeError("Network unknown") if _config['weights'] is not None: print(f"Loading weights from {_config['weights']}") checkpoint = torch.load(_config['weights'], map_location='cpu') saved_state_dict = checkpoint['state_dict'] model.load_state_dict(saved_state_dict) model = model.to(device) print(dataset_size) print('Number of model parameters: {}'.format(sum([p.data.nelement() for p in model.parameters()]))) parameters = list(filter(lambda p: p.requires_grad, model.parameters())) if _config['loss'] == 'geometric': parameters += list(loss_fn.parameters()) if _config['optimizer'] == 'adam': optimizer = optim.Adam(parameters, lr=_config['BASE_LEARNING_RATE'], weight_decay=5e-6) # Probably this scheduler is not used scheduler = torch.optim.lr_scheduler.MultiStepLR(optimizer, milestones=[20, 50, 70], gamma=0.5) else: optimizer = optim.SGD(parameters, lr=_config['BASE_LEARNING_RATE'], momentum=0.9, weight_decay=5e-6, nesterov=True) starting_epoch = 0 if _config['weights'] is not None and _config['resume']: checkpoint = torch.load(_config['weights'], map_location='cpu') opt_state_dict = checkpoint['optimizer'] optimizer.load_state_dict(opt_state_dict) starting_epoch = checkpoint['epoch'] # Allow mixed-precision if needed # model, optimizer = apex.amp.initialize(model, optimizer, opt_level=_config["precision"]) start_full_time = time.time() BEST_VAL_LOSS = 10000. old_save_filename = None total_iter = 0 for epoch in range(starting_epoch, _config['epochs'] + 1): EPOCH = epoch print('This is %d-th epoch' % epoch) epoch_start_time = time.time() total_train_loss = 0 local_loss = 0. if _config['optimizer'] != 'adam': _run.log_scalar("LR", _config['BASE_LEARNING_RATE'] * math.exp((1 - epoch) * 4e-2), epoch) for param_group in optimizer.param_groups: param_group['lr'] = _config['BASE_LEARNING_RATE'] * \ math.exp((1 - epoch) * 4e-2) else: #scheduler.step(epoch%100) _run.log_scalar("LR", scheduler.get_lr()[0]) ## Training ## time_for_50ep = time.time() for batch_idx, sample in enumerate(TrainImgLoader): #print(f'batch {batch_idx+1}/{len(TrainImgLoader)}', end='\r') start_time = time.time() lidar_input = [] rgb_input = [] sample['tr_error'] = sample['tr_error'].cuda() sample['rot_error'] = sample['rot_error'].cuda() start_preprocess = time.time() for idx in range(len(sample['rgb'])): # ProjectPointCloud in RT-pose real_shape = [sample['rgb'][idx].shape[1], sample['rgb'][idx].shape[2], sample['rgb'][idx].shape[0]] sample['point_cloud'][idx] = sample['point_cloud'][idx].cuda() pc_rotated = sample['point_cloud'][idx].clone() reflectance = None if _config['use_reflectance']: reflectance = sample['reflectance'][idx].cuda() R = mathutils.Quaternion(sample['rot_error'][idx]).to_matrix() R.resize_4x4() T = mathutils.Matrix.Translation(sample['tr_error'][idx]) RT = T * R pc_rotated = rotate_back(pc_rotated, RT) if _config['max_depth'] < 100.: pc_rotated = pc_rotated[:, pc_rotated[0, :] < _config['max_depth']].clone() cam_params = sample['calib'][idx].cuda() cam_model = CameraModel() cam_model.focal_length = cam_params[:2] cam_model.principal_point = cam_params[2:] uv, depth, _, refl = cam_model.project_pytorch(pc_rotated, real_shape, reflectance) uv = uv.t().int() depth_img = torch.zeros(real_shape[:2], device='cuda', dtype=torch.float) depth_img += 1000. depth_img = visibility.depth_image(uv, depth, depth_img, uv.shape[0], real_shape[1], real_shape[0]) depth_img[depth_img == 1000.] = 0. depth_img_no_occlusion = torch.zeros_like(depth_img, device='cuda') depth_img_no_occlusion = visibility.visibility2(depth_img, cam_params, depth_img_no_occlusion, depth_img.shape[1], depth_img.shape[0], occlusion_threshold, _config['occlusion_kernel']) if _config['use_reflectance']: # This need to be checked uv = uv.long() indexes = depth_img_no_occlusion[uv[:,1], uv[:,0]] == depth refl_img = torch.zeros(real_shape[:2], device='cuda', dtype=torch.float) refl_img[uv[indexes,1], uv[indexes,0]] = refl[0, indexes] depth_img_no_occlusion /= _config['max_depth'] if not _config['use_reflectance']: depth_img_no_occlusion = depth_img_no_occlusion.unsqueeze(0) else: depth_img_no_occlusion = torch.stack((depth_img_no_occlusion, refl_img)) # PAD ONLY ON RIGHT AND BOTTOM SIDE rgb = sample['rgb'][idx].cuda() shape_pad = [0, 0, 0, 0] shape_pad[3] = (img_shape[0] - rgb.shape[1]) # // 2 shape_pad[1] = (img_shape[1] - rgb.shape[2]) # // 2 + 1 rgb = F.pad(rgb, shape_pad) depth_img_no_occlusion = F.pad(depth_img_no_occlusion, shape_pad) rgb_input.append(rgb) lidar_input.append(depth_img_no_occlusion) lidar_input = torch.stack(lidar_input) rgb_input = torch.stack(rgb_input) end_preprocess = time.time() loss = train(model, optimizer, rgb_input, lidar_input, sample['tr_error'], sample['rot_error'], loss_fn, sample['point_cloud']) if loss != loss: raise ValueError("Loss is NaN") #train_writer.add_scalar("Loss", loss, total_iter) local_loss += loss if batch_idx % 50 == 0 and batch_idx != 0: print(f'Iter {batch_idx}/{len(TrainImgLoader)} training loss = {local_loss/50:.3f}, ' f'time = {(time.time() - start_time)/lidar_input.shape[0]:.4f}, ' #f'time_preprocess = {(end_preprocess-start_preprocess)/lidar_input.shape[0]:.4f}, ' f'time for 50 iter: {time.time()-time_for_50ep:.4f}') time_for_50ep = time.time() _run.log_scalar("Loss", local_loss/50, total_iter) local_loss = 0. total_train_loss += loss * len(sample['rgb']) total_iter += len(sample['rgb']) print("------------------------------------") print('epoch %d total training loss = %.3f' % (epoch, total_train_loss / len(dataset))) print('Total epoch time = %.2f' % (time.time() - epoch_start_time)) print("------------------------------------") _run.log_scalar("Total training loss", total_train_loss / len(dataset), epoch) ## Test ## total_test_loss = 0. total_test_t = 0. total_test_r = 0. local_loss = 0.0 for batch_idx, sample in enumerate(TestImgLoader): # print(f'batch {batch_idx + 1}/{len(TestImgLoader)}', end='\r') start_time = time.time() lidar_input = [] rgb_input = [] #sample['rgb'] = sample['rgb'].cuda() sample['tr_error'] = sample['tr_error'].cuda() sample['rot_error'] = sample['rot_error'].cuda() for idx in range(len(sample['rgb'])): # ProjectPointCloud in RT-pose real_shape = [sample['rgb'][idx].shape[1], sample['rgb'][idx].shape[2], sample['rgb'][idx].shape[0]] sample['point_cloud'][idx] = sample['point_cloud'][idx].cuda() pc_rotated = sample['point_cloud'][idx].clone() reflectance = None if _config['use_reflectance']: reflectance = sample['reflectance'][idx].cuda() R = mathutils.Quaternion(sample['rot_error'][idx]).to_matrix() R.resize_4x4() T = mathutils.Matrix.Translation(sample['tr_error'][idx]) RT = T * R pc_rotated = rotate_back(pc_rotated, RT) if _config['max_depth'] < 100.: pc_rotated = pc_rotated[:, pc_rotated[0, :] < _config['max_depth']].clone() cam_params = sample['calib'][idx].cuda() cam_model = CameraModel() cam_model.focal_length = cam_params[:2] cam_model.principal_point = cam_params[2:] uv, depth, _, refl = cam_model.project_pytorch(pc_rotated, real_shape, reflectance) uv = uv.t().int() depth_img = torch.zeros(real_shape[:2], device='cuda', dtype=torch.float) depth_img += 1000. depth_img = visibility.depth_image(uv, depth, depth_img, uv.shape[0], real_shape[1], real_shape[0]) depth_img[depth_img == 1000.] = 0. depth_img_no_occlusion = torch.zeros_like(depth_img, device='cuda') depth_img_no_occlusion = visibility.visibility2(depth_img, cam_params, depth_img_no_occlusion, depth_img.shape[1], depth_img.shape[0], occlusion_threshold, _config['occlusion_kernel']) if _config['use_reflectance']: uv = uv.long() indexes = depth_img_no_occlusion[uv[:,1], uv[:,0]] == depth refl_img = torch.zeros(real_shape[:2], device='cuda', dtype=torch.float) refl_img[uv[indexes,1], uv[indexes,0]] = refl[0, indexes] depth_img_no_occlusion /= _config['max_depth'] if not _config['use_reflectance']: depth_img_no_occlusion = depth_img_no_occlusion.unsqueeze(0) else: depth_img_no_occlusion = torch.stack((depth_img_no_occlusion, refl_img)) rgb = sample['rgb'][idx].cuda() shape_pad = [0, 0, 0, 0] shape_pad[3] = (img_shape[0] - rgb.shape[1]) shape_pad[1] = (img_shape[1] - rgb.shape[2]) rgb = F.pad(rgb, shape_pad) depth_img_no_occlusion = F.pad(depth_img_no_occlusion, shape_pad) rgb_input.append(rgb) lidar_input.append(depth_img_no_occlusion) lidar_input = torch.stack(lidar_input) rgb_input = torch.stack(rgb_input) loss, trasl_e, rot_e = test(model, rgb_input, lidar_input, sample['tr_error'], sample['rot_error'], loss_fn, dataset_val.model, sample['point_cloud']) if loss != loss: raise ValueError("Loss is NaN") total_test_t += trasl_e total_test_r += rot_e local_loss += loss if batch_idx % 50 == 0 and batch_idx != 0: print('Iter %d test loss = %.3f , time = %.2f' % (batch_idx, local_loss/50., (time.time() - start_time)/lidar_input.shape[0])) local_loss = 0.0 total_test_loss += loss * len(sample['rgb']) print("------------------------------------") print('total test loss = %.3f' % (total_test_loss / len(dataset_val))) print(f'total traslation error: {total_test_t / len(dataset_val)} cm') print(f'total rotation error: {total_test_r / len(dataset_val)} °') print("------------------------------------") #train_writer.add_scalar("Val_Loss", total_test_loss / len(dataset_val), epoch) #train_writer.add_scalar("Val_t_error", total_test_t / len(dataset_val), epoch) #train_writer.add_scalar("Val_r_error", total_test_r / len(dataset_val), epoch) _run.log_scalar("Val_Loss", total_test_loss / len(dataset_val), epoch) _run.log_scalar("Val_t_error", total_test_t / len(dataset_val), epoch) _run.log_scalar("Val_r_error", total_test_r / len(dataset_val), epoch) # SAVE val_loss = total_test_loss / len(dataset_val) if val_loss < BEST_VAL_LOSS: BEST_VAL_LOSS = val_loss #_run.result = BEST_VAL_LOSS if _config['rescale_transl'] > 0: _run.result = total_test_t / len(dataset_val) else: _run.result = total_test_r / len(dataset_val) savefilename = f'{_config["savemodel"]}/checkpoint_r{_config["max_r"]:.2f}_t{_config["max_t"]:.2f}_e{epoch}_{val_loss:.3f}.tar' torch.save({ 'config': _config, 'epoch': epoch, 'state_dict': model.state_dict(), 'optimizer': optimizer.state_dict(), 'train_loss': total_train_loss / len(dataset), 'test_loss': total_test_loss / len(dataset_val), }, savefilename) print(f'Model saved as {savefilename}') if old_save_filename is not None: if os.path.exists(old_save_filename): os.remove(old_save_filename) old_save_filename = savefilename print('full training time = %.2f HR' % ((time.time() - start_full_time) / 3600)) return _run.result
def __init__(self, dataset_dir, transform=None, augmentation=False, maps_folder='local_maps', use_reflectance=False, max_t=2., max_r=10., split='test', device='cpu', test_sequence='00'): super(DatasetVisibilityKittiSingle, self).__init__() self.use_reflectance = use_reflectance self.maps_folder = maps_folder self.device = device self.max_r = max_r self.max_t = max_t self.augmentation = augmentation self.root_dir = dataset_dir self.transform = transform self.split = split self.GTs_R = {} self.GTs_T = {} self.all_files = [] self.model = CameraModel() self.model.focal_length = [7.18856e+02, 7.18856e+02] self.model.principal_point = [6.071928e+02, 1.852157e+02] # for dir in ['00', '03', '05', '06', '07', '08', '09']: for dir in ['00']: self.GTs_R[dir] = [] self.GTs_T[dir] = [] df_locations = pd.read_csv(os.path.join(dataset_dir, dir, 'poses.csv'), sep=',', dtype={'timestamp': str}) for index, row in df_locations.iterrows(): if not os.path.exists( os.path.join(dataset_dir, dir, maps_folder, str(row['timestamp']) + '.h5')): continue if not os.path.exists( os.path.join(dataset_dir, dir, 'image_2', str(row['timestamp']) + '.png')): continue if dir == test_sequence and split.startswith('test'): self.all_files.append( os.path.join(dir, str(row['timestamp']))) elif (not dir == test_sequence) and split == 'train': self.all_files.append( os.path.join(dir, str(row['timestamp']))) GT_R = np.array([row['qw'], row['qx'], row['qy'], row['qz']]) GT_T = np.array([row['x'], row['y'], row['z']]) self.GTs_R[dir].append(GT_R) self.GTs_T[dir].append(GT_T) self.test_RT = [] if split == 'test': test_RT_file = os.path.join( dataset_dir, f'test_RT_seq{test_sequence}_{max_r:.2f}_{max_t:.2f}.csv') if os.path.exists(test_RT_file): print(f'TEST SET: Using this file: {test_RT_file}') df_test_RT = pd.read_csv(test_RT_file, sep=',') for index, row in df_test_RT.iterrows(): self.test_RT.append(list(row)) else: print(f'TEST SET - Not found: {test_RT_file}') print("Generating a new one") test_RT_file = open(test_RT_file, 'w') test_RT_file = csv.writer(test_RT_file, delimiter=',') test_RT_file.writerow( ['id', 'tx', 'ty', 'tz', 'rx', 'ry', 'rz']) for i in range(len(self.all_files)): rotz = np.random.uniform(-max_r, max_r) * (3.141592 / 180.0) roty = np.random.uniform(-max_r, max_r) * (3.141592 / 180.0) rotx = np.random.uniform(-max_r, max_r) * (3.141592 / 180.0) transl_x = np.random.uniform(-max_t, max_t) transl_y = np.random.uniform(-max_t, max_t) transl_z = np.random.uniform(-max_t, min(max_t, 1.)) test_RT_file.writerow( [i, transl_x, transl_y, transl_z, rotx, roty, rotz]) self.test_RT.append( [i, transl_x, transl_y, transl_z, rotx, roty, rotz]) assert len(self.test_RT) == len( self.all_files), "Something wrong with test RTs"
def main(): #read the pointcloud pointcloud_filename = "/data/lyh/lab/pcaifeat_RobotCar_v3_1/1400505794141322.txt" pointcloud = np.loadtxt(pointcloud_filename, delimiter=' ') pointcloud = np.hstack([pointcloud, np.ones((pointcloud.shape[0], 1))]) ''' for i in range(pointcloud.shape[0]): print(" %.3f %.3f %.3f %.3f"%(pointcloud[i,0],pointcloud[i,1],pointcloud[i,2],pointcloud[i,3])) exit() ''' #load the camera model models_dir = "/data/lyh/lab/robotcar-dataset-sdk/models/" model = CameraModel(models_dir, "mono_left") #load the camera global pose imgpos_path = "/data/lyh/lab/pcaifeat_RobotCar_v3_1/1400505794141322_imgpos.txt" print(imgpos_path) imgpos = {} with open(imgpos_path) as imgpos_file: for line in imgpos_file: pos = [x for x in line.split(' ')] for i in range(len(pos) - 2): pos[i + 1] = float(pos[i + 1]) imgpos[pos[0]] = np.reshape(np.array(pos[1:-1]), [4, 4]) ''' for key in imgpos.keys(): print(key) print(imgpos[key]) exit() ''' #read the camera and ins extrinsics extrinsics_path = "/data/lyh/lab/robotcar-dataset-sdk/extrinsics/mono_left.txt" print(extrinsics_path) with open(extrinsics_path) as extrinsics_file: extrinsics = [float(x) for x in next(extrinsics_file).split(' ')] G_camera_vehicle = build_se3_transform(extrinsics) print(G_camera_vehicle) extrinsics_path = "/data/lyh/lab/robotcar-dataset-sdk/extrinsics/ins.txt" print(extrinsics_path) with open(extrinsics_path) as extrinsics_file: extrinsics = [float(x) for x in next(extrinsics_file).split(' ')] G_ins_vehicle = build_se3_transform(extrinsics) print(G_ins_vehicle) G_camera_posesource = G_camera_vehicle * G_ins_vehicle #translate pointcloud to image coordinate pointcloud = np.dot(np.linalg.inv(imgpos["mono_left"]), pointcloud.T) pointcloud = np.dot(G_camera_posesource, pointcloud) #project pointcloud to image image_path = "/data/lyh/lab/pcaifeat_RobotCar_v3_1/1400505794141322_mono_left.png" #image = load_image(image_path, model) image = load_image(image_path) uv = model.project(pointcloud, [1024, 1024]) lut = model.bilinear_lut[:, 1::-1].T.reshape((2, 1024, 1024)) u = map_coordinates(lut[0, :, :], uv, order=1) v = map_coordinates(lut[1, :, :], uv, order=1) uv = np.array([u, v]) print(uv.shape) transform_matrix = np.zeros([64, 4096]) for i in range(uv.shape[1]): if uv[0, i] == 0 and uv[1, i] == 0: continue cur_uv = np.rint(uv[:, i] / 128) row = cur_uv[1] * 8 + cur_uv[0] transform_matrix[int(row), i] = 1 aa = np.sum(transform_matrix, 1).reshape([8, 8]) print(np.sum(aa)) plt.figure(1) plt.imshow(aa) #plt.show() #exit() plt.figure(2) #plt.imshow(image) #uv = np.rint(uv/32) plt.scatter(np.ravel(uv[0, :]), np.ravel(uv[1, :]), s=2, edgecolors='none', cmap='jet') plt.xlim(0, 1024) plt.ylim(1024, 0) plt.xticks([]) plt.yticks([]) plt.show()
from build_pointcloud import build_pointcloud from transform import build_se3_transform from image import load_image from camera_model import CameraModel import my_params image_dir = my_params.image_dir laser_dir = my_params.lmsfront_dir poses_file = my_params.poses_file models_dir = my_params.model_dir extrinsics_dir = my_params.extrinsics_dir image_idx = 1572 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 # print("G_camera_vehicle",G_camera_vehicle) 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(
def iterate_images(root_dir, camera_description, models_dir, full_size=True): if 'stereo' in camera_description: timestamps_path = os.path.join(root_dir, 'stereo.timestamps') else: timestamps_path = os.path.join(root_dir, camera_description + '.timestamps') if not os.path.isfile(timestamps_path): raise IOError("Could not find timestamps file") images_dir = os.path.join(root_dir, camera_description) model = CameraModel(models_dir, images_dir) current_chunk = 0 timestamps_file = open(timestamps_path) i = 0 for line in timestamps_file: tokens = line.split() if len(tokens) < 2: break timestamp_us = long(tokens[0]) timestamp_ns = timestamp_us * long(1000) if full_size: filename_processed = os.path.join(images_dir, tokens[0] + '_undistorted.png') else: filename_processed = os.path.join( images_dir, tokens[0] + '_ds2_undistorted.png') if os.path.exists(filename_processed): im = cv2.imread(filename_processed, cv2.IMREAD_GRAYSCALE) else: datetime = dt.utcfromtimestamp(int(tokens[0]) / 1000000) chunk = int(tokens[1]) filename = os.path.join(images_dir, tokens[0] + '.png') if not os.path.isfile(filename): if chunk != current_chunk: print("Chunk " + str(chunk) + " not found") current_chunk = chunk continue current_chunk = chunk img = load_image(filename, model) im_gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY) filename_gray = os.path.join(images_dir, tokens[0] + '_undistorted.png') cv2.imwrite(filename_gray, im_gray) im_gray_downscaled = cv2.resize( im_gray, (im_gray.shape[1] / 2, im_gray.shape[0] / 2)) cv2.imwrite(filename_processed, im_gray_downscaled) if full_size: im = im_gray else: im = im_gray_downscaled if i % 100 == 0: print "At i ", i i += 1 yield timestamp_ns, im
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)