def getAbsolutePoses(poses): """ Return absolute poses from poses snippets (relative poses) """ poses = np.array(poses) for i, pose in enumerate(poses): pose = pose.cpu()[0] pose = torch.cat( [pose[:5 // 2], torch.zeros(1, 6).float(), pose[5 // 2:]]) inv_transform_matrices = pose_vec2mat(pose, rotation_mode='euler').double() rot_matrices = torch.inverse(inv_transform_matrices[:, :, :3]) tr_vectors = -rot_matrices @ inv_transform_matrices[:, :, -1:] transform_matrices = torch.cat([rot_matrices, tr_vectors], axis=-1) first_inv_transform = inv_transform_matrices[0] final_poses = first_inv_transform[:, :3] @ transform_matrices final_poses[:, :, -1:] += first_inv_transform[:, -1:] poses[i] = final_poses for i in range(1, len(poses)): r = poses[i - 1][1] poses[i] = r[:, :3] @ poses[i] poses[i][:, :, -1] = poses[i][:, :, -1] + r[:, -1] return poses[-1][:, :, -1]
def pose_vec2points(file): ''' 把所有的pose变成其次坐标点 :return: ''' #origin = torch.tensor([0,0,0]) poses_all = np.load(file) #[b,4,6] frame_poses_list = [[] for i in range(poses_all.shape[0])] for i in range(poses_all.shape[0]): #b for j in range(i, i + poses_all.shape[1]): #0~5,1~6 if j < poses_all.shape[0]: frame_poses_list[j].append(poses_all[i, j - i, :]) else: break #去除空值 for i in range(2, len(frame_poses_list)): if i == 2: frame_poses_list[i].pop(0) elif i == 3: frame_poses_list[i].pop(1) else: frame_poses_list[i].pop(2) #求和平均,得到一个list batch_pose_vec = None #at last [b(96),6] for i in range(len(frame_poses_list)): nump = np.zeros(6) for j in range(len(frame_poses_list[i])): nump += np.array(frame_poses_list[i][j]) nump /= len(frame_poses_list[i]) if i == 0: batch_pose_vec = nump.reshape(1, -1) else: batch_pose_vec = np.concatenate( [batch_pose_vec, nump.reshape(1, -1)]) #6d-tensor 2 matrix batch_pose_vec = torch.tensor(batch_pose_vec) batch_pose_mat = pose_vec2mat(batch_pose_vec) origin = torch.tensor([[0.], [0.], [0.], [1.]]).double() point = origin points = None #last [b,4]齐次坐标 for i in range(batch_pose_mat.shape[0]): point = batch_pose_mat[i] @ point point = torch.cat([point, torch.ones([1, 1]).double()]) if i == 0: points = point.unsqueeze(0) else: points = torch.cat([points, point.unsqueeze(0)]) ret_file_name = dataset_name + '_corrds.npy' np.save(ret_file_name, points.detach().numpy()) return ret_file_name
def pose2mat(pose): """ param: pose: only one single pose """ if pose.shape == (4, 4): return pose if pose.shape[0] == 6: from inverse_warp import pose_vec2mat pose_mat = pose_vec2mat(torch.tensor( pose[np.newaxis, ...])).squeeze(0).cpu().numpy() pose_mat = np.vstack([pose_mat, np.array([0, 0, 0, 1])]) if len(pose.shape) == 2 and pose.shape[1] == 6: from inverse_warp import pose_vec2mat pose_mat = pose_vec2mat( torch.tensor(pose)).squeeze(0).cpu().numpy() pose_mat = np.vstack([pose_mat, np.array([0, 0, 0, 1])]) if pose.shape == (3, 4): pose_mat = np.vstack([pose, np.array([0, 0, 0, 1])]) return pose_mat pass
def pose2tf_mat(rotation_mode, imgs, poses): poses = poses.cpu()[0] poses = torch.cat([poses[:len(imgs) // 2], torch.zeros(1, 6).float(), poses[len(imgs) // 2:]]) inv_transform_matrices = pose_vec2mat(poses, rotation_mode=rotation_mode).numpy().astype(np.float64) rot_matrices = np.linalg.inv(inv_transform_matrices[:, :, :3]) tr_vectors = -rot_matrices @ inv_transform_matrices[:, :, -1:] transform_matrices = np.concatenate([rot_matrices, tr_vectors], axis=-1) # 将对[0 1 2]中间1的转换矩阵变成对0的位姿转换:T(0->0),T(1->0),T(2->0) first_inv_transform = inv_transform_matrices[0] final_poses = first_inv_transform[:, :3] @ transform_matrices final_poses[:, :, -1:] += first_inv_transform[:, -1:] return final_poses
def one_scale(depth, explainability_mask): assert(explainability_mask is None or depth.size()[2:] == explainability_mask.size()[2:]) tgt_img = ref_imgs[0] reconstruction_loss = 0 b, _, h, w = depth.size() downscale = tgt_img.size(2)/h ref_img = ref_imgs[-1] tgt_img_scaled = F.interpolate(tgt_img, (h, w), mode='area') ref_imgs_scaled = F.interpolate(ref_img, (h, w), mode='area') intrinsics_scaled = torch.cat((intrinsics[:, 0:2]/downscale, intrinsics[:, 2:]), dim=1) #current_pose = pose[:, i] pose21 = pose[:,0] pose23 = pose[:,1] T21 = homomat(pose_vec2mat(pose21)) T23 = homomat(pose_vec2mat(pose21)) T12 = T21.inverse() T13 = torch.bmm(T23, T12) T13 = T13[:,:3] ref_img_warped, valid_points = inverse_warp(ref_img, depth[:,0], T13, intrinsics_scaled, 'mat', padding_mode) diff = (tgt_img_scaled - ref_img_warped) * valid_points.unsqueeze(1).float() if explainability_mask is not None: diff = diff * explainability_mask[:,i:i+1].expand_as(diff) reconstruction_loss += diff.abs().mean() assert((reconstruction_loss == reconstruction_loss).item() == 1) return reconstruction_loss
def main(): args = parser.parse_args() weights_pose = torch.load(args.pretrained_posenet) pose_net = models.PoseResNet().to(device) pose_net.load_state_dict(weights_pose['state_dict'], strict=False) pose_net.eval() image_dir = Path(args.dataset_dir + args.sequence + "/image_2/") output_dir = Path(args.output_dir) output_dir.makedirs_p() test_files = sum( [image_dir.files('*.{}'.format(ext)) for ext in args.img_exts], []) test_files.sort() print('{} files to test'.format(len(test_files))) print(test_files) global_pose = np.eye(4) poses = [global_pose[0:3, :].reshape(1, 12)] n = len(test_files) tensor_img1 = load_tensor_image(test_files[0], args) for iter in tqdm(range(n - 1)): tensor_img2 = load_tensor_image(test_files[iter + 1], args) pose = pose_net(tensor_img1, tensor_img2) pose_mat = pose_vec2mat(pose).squeeze(0).cpu().numpy() pose_mat = np.vstack([pose_mat, np.array([0, 0, 0, 1])]) global_pose = global_pose @ np.linalg.inv(pose_mat) poses.append(global_pose[0:3, :].reshape(1, 12)) # update tensor_img1 = tensor_img2 poses = np.concatenate(poses, axis=0) filename = Path(args.output_dir + args.sequence + ".txt") np.savetxt(filename, poses, delimiter=' ', fmt='%1.8e')
def validate_with_gt(args, val_loader, depth_net, pose_net, epoch, logger, output_writers=[], **env): global device batch_time = AverageMeter() depth_error_names = ['abs diff', 'abs rel', 'sq rel', 'a1', 'a2', 'a3'] stab_depth_errors = AverageMeter(i=len(depth_error_names)) unstab_depth_errors = AverageMeter(i=len(depth_error_names)) pose_error_names = ['Absolute Trajectory Error', 'Rotation Error'] pose_errors = AverageMeter(i=len(pose_error_names)) # switch to evaluate mode depth_net.eval() pose_net.eval() end = time.time() logger.valid_bar.update(0) for i, sample in enumerate(val_loader): log_output = i < len(output_writers) imgs = torch.stack(sample['imgs'], dim=1).to(device) batch_size, seq, c, h, w = imgs.size() intrinsics = sample['intrinsics'].to(device) intrinsics_inv = sample['intrinsics_inv'].to(device) if args.network_input_size is not None: imgs = F.interpolate(imgs, (c, *args.network_input_size), mode='area') downscale = h / args.network_input_size[0] intrinsics = torch.cat( (intrinsics[:, 0:2] / downscale, intrinsics[:, 2:]), dim=1) intrinsics_inv = torch.cat( (intrinsics_inv[:, :, 0:2] * downscale, intrinsics_inv[:, :, 2:]), dim=2) GT_depth = sample['depth'].to(device) GT_pose = sample['pose'].to(device) mid_index = (args.sequence_length - 1) // 2 tgt_img = imgs[:, mid_index] if epoch == 1 and log_output: for j, img in enumerate(sample['imgs']): output_writers[i].add_image('val Input', tensor2array(img[0]), j) depth_to_show = GT_depth[0].cpu() # KITTI Like data routine to discard invalid data depth_to_show[depth_to_show == 0] = 1000 disp_to_show = (1 / depth_to_show).clamp(0, 10) output_writers[i].add_image( 'val target Disparity Normalized', tensor2array(disp_to_show, max_value=None, colormap='bone'), epoch) poses = pose_net(imgs) pose_matrices = pose_vec2mat(poses, args.rotation_mode) # [B, seq, 3, 4] inverted_pose_matrices = invert_mat(pose_matrices) pose_errors.update( compute_pose_error(GT_pose[:, :-1], inverted_pose_matrices.data[:, :-1])) tgt_poses = pose_matrices[:, mid_index] # [B, 3, 4] compensated_predicted_poses = compensate_pose(pose_matrices, tgt_poses) compensated_GT_poses = compensate_pose(GT_pose, GT_pose[:, mid_index]) for j in range(args.sequence_length): if j == mid_index: if log_output and epoch == 1: output_writers[i].add_image( 'val Input Stabilized', tensor2array(sample['imgs'][j][0]), j) continue '''compute displacement magnitude for each element of batch, and rescale depth accordingly.''' prior_img = imgs[:, j] displacement = compensated_GT_poses[:, j, :, -1] # [B,3] displacement_magnitude = displacement.norm(p=2, dim=1) # [B] current_GT_depth = GT_depth * args.nominal_displacement / displacement_magnitude.view( -1, 1, 1) prior_predicted_pose = compensated_predicted_poses[:, j] # [B, 3, 4] prior_GT_pose = compensated_GT_poses[:, j] prior_predicted_rot = prior_predicted_pose[:, :, :-1] prior_GT_rot = prior_GT_pose[:, :, :-1].transpose(1, 2) prior_compensated_from_GT = inverse_rotate(prior_img, prior_GT_rot, intrinsics, intrinsics_inv) if log_output and epoch == 1: depth_to_show = current_GT_depth[0] output_writers[i].add_image( 'val target Depth {}'.format(j), tensor2array(depth_to_show, max_value=args.max_depth), epoch) output_writers[i].add_image( 'val Input Stabilized', tensor2array(prior_compensated_from_GT[0]), j) prior_compensated_from_prediction = inverse_rotate( prior_img, prior_predicted_rot, intrinsics, intrinsics_inv) predicted_input_pair = torch.cat( [prior_compensated_from_prediction, tgt_img], dim=1) # [B, 6, W, H] GT_input_pair = torch.cat([prior_compensated_from_GT, tgt_img], dim=1) # [B, 6, W, H] # This is the depth from footage stabilized with GT pose, it should be better than depth from raw footage without any GT info raw_depth_stab = depth_net(GT_input_pair) raw_depth_unstab = depth_net(predicted_input_pair) # Upsample depth so that it matches GT size scale_factor = GT_depth.size(-1) // raw_depth_stab.size(-1) depth_stab = F.interpolate(raw_depth_stab, scale_factor=scale_factor, mode='bilinear', align_corners=False) depth_unstab = F.interpolate(raw_depth_unstab, scale_factor=scale_factor, mode='bilinear', align_corners=False) for k, depth in enumerate([depth_stab, depth_unstab]): disparity = 1 / depth errors = stab_depth_errors if k == 0 else unstab_depth_errors errors.update( compute_depth_errors(current_GT_depth, depth, crop=True)) if log_output: prefix = 'stabilized' if k == 0 else 'unstabilized' output_writers[i].add_image( 'val {} Dispnet Output Normalized {}'.format( prefix, j), tensor2array(disparity[0], max_value=None, colormap='bone'), epoch) output_writers[i].add_image( 'val {} Depth Output {}'.format(prefix, j), tensor2array(depth[0], max_value=args.max_depth), epoch) # measure elapsed time batch_time.update(time.time() - end) end = time.time() logger.valid_bar.update(i + 1) if i % args.print_freq == 0: logger.valid_writer.write( 'valid: Time {} ATE Error {:.4f} ({:.4f}), Unstab Rel Abs Error {:.4f} ({:.4f})' .format(batch_time, pose_errors.val[0], pose_errors.avg[0], unstab_depth_errors.val[1], unstab_depth_errors.avg[1])) logger.valid_bar.update(len(val_loader)) errors = (*pose_errors.avg, *unstab_depth_errors.avg, *stab_depth_errors.avg) error_names = (*pose_error_names, *['unstab {}'.format(e) for e in depth_error_names], *['stab {}'.format(e) for e in depth_error_names]) return OrderedDict(zip(error_names, errors))
def validate_without_gt(args, val_loader, depth_net, pose_net, epoch, logger, output_writers=[], **env): global device batch_time = AverageMeter() losses = AverageMeter(i=3, precision=4) w1, w2, w3 = args.photo_loss_weight, args.smooth_loss_weight, args.ssim if args.log_output: poses_values = np.zeros(((len(val_loader) - 1) * args.test_batch_size * (args.sequence_length - 1), 6)) disp_values = np.zeros( ((len(val_loader) - 1) * args.test_batch_size * 3)) # switch to evaluate mode depth_net.eval() pose_net.eval() upsample_depth_net = models.UpSampleNet(depth_net, args.network_input_size) end = time.time() logger.valid_bar.update(0) for i, sample in enumerate(val_loader): log_output = i < len(output_writers) imgs = torch.stack(sample['imgs'], dim=1).to(device) intrinsics = sample['intrinsics'].to(device) intrinsics_inv = sample['intrinsics_inv'].to(device) if epoch == 1 and log_output: for j, img in enumerate(sample['imgs']): output_writers[i].add_image('val Input', tensor2array(img[0]), j) batch_size, seq = imgs.size()[:2] if args.network_input_size is not None: h, w = args.network_input_size downsample_imgs = F.interpolate(imgs, (3, h, w), mode='area') poses = pose_net(downsample_imgs) # [B, seq, 6] else: poses = pose_net(imgs) pose_matrices = pose_vec2mat(poses, args.rotation_mode) # [B, seq, 3, 4] mid_index = (args.sequence_length - 1) // 2 tgt_imgs = imgs[:, mid_index] # [B, 3, H, W] tgt_poses = pose_matrices[:, mid_index] # [B, 3, 4] compensated_poses = compensate_pose( pose_matrices, tgt_poses) # [B, seq, 3, 4] tgt_poses are now neutral pose ref_indices = list(range(args.sequence_length)) ref_indices.remove(mid_index) loss_1 = 0 loss_2 = 0 for ref_index in ref_indices: prior_imgs = imgs[:, ref_index] prior_poses = compensated_poses[:, ref_index] # [B, 3, 4] prior_imgs_compensated = inverse_rotate(prior_imgs, prior_poses[:, :, :3], intrinsics, intrinsics_inv) input_pair = torch.cat([prior_imgs_compensated, tgt_imgs], dim=1) # [B, 6, W, H] predicted_magnitude = prior_poses[:, :, -1:].norm( p=2, dim=1, keepdim=True).unsqueeze(1) # [B, 1, 1, 1] scale_factor = args.nominal_displacement / predicted_magnitude normalized_translation = compensated_poses[:, :, :, -1:] * scale_factor # [B, seq, 3, 1] new_pose_matrices = torch.cat( [compensated_poses[:, :, :, :-1], normalized_translation], dim=-1) depth = upsample_depth_net(input_pair) disparity = 1 / depth total_indices = torch.arange(seq).long().unsqueeze(0).expand( batch_size, seq).to(device) tgt_id = total_indices[:, mid_index] ref_indices = total_indices[ total_indices != tgt_id.unsqueeze(1)].view( batch_size, seq - 1) photo_loss, diff_maps, warped_imgs = photometric_reconstruction_loss( imgs, tgt_id, ref_indices, depth, new_pose_matrices, intrinsics, intrinsics_inv, args.rotation_mode, ssim_weight=w3) loss_1 += photo_loss if log_output: output_writers[i].add_image( 'val Dispnet Output Normalized {}'.format(ref_index), tensor2array(disparity[0], max_value=None, colormap='bone'), epoch) output_writers[i].add_image( 'val Depth Output {}'.format(ref_index), tensor2array(depth[0].cpu(), max_value=args.max_depth), epoch) for j, (diff, warped) in enumerate(zip(diff_maps, warped_imgs)): output_writers[i].add_image( 'val Warped Outputs {} {}'.format(j, ref_index), tensor2array(warped[0]), epoch) output_writers[i].add_image( 'val Diff Outputs {} {}'.format(j, ref_index), tensor2array(diff[0].abs() - 1), epoch) loss_2 += texture_aware_smooth_loss( disparity, tgt_imgs if args.texture_loss else None) if args.log_output and i < len(val_loader) - 1: step = args.test_batch_size * (args.sequence_length - 1) poses_values[i * step:(i + 1) * step] = poses[:, :-1].cpu().view( -1, 6).numpy() step = args.test_batch_size * 3 disp_unraveled = disparity.cpu().view(args.test_batch_size, -1) disp_values[i * step:(i + 1) * step] = torch.cat([ disp_unraveled.min(-1)[0], disp_unraveled.median(-1)[0], disp_unraveled.max(-1)[0] ]).numpy() loss = w1 * loss_1 + w2 * loss_2 losses.update([loss.item(), loss_1.item(), loss_2.item()]) # measure elapsed time batch_time.update(time.time() - end) end = time.time() logger.valid_bar.update(i + 1) if i % args.print_freq == 0: logger.valid_writer.write('valid: Time {} Loss {}'.format( batch_time, losses)) if args.log_output: rot_coeffs = ['rx', 'ry', 'rz'] if args.rotation_mode == 'euler' else [ 'qx', 'qy', 'qz' ] tr_coeffs = ['tx', 'ty', 'tz'] for k, (coeff_name) in enumerate(tr_coeffs + rot_coeffs): output_writers[0].add_histogram('val poses_{}'.format(coeff_name), poses_values[:, k], epoch) output_writers[0].add_histogram('disp_values', disp_values, epoch) logger.valid_bar.update(len(val_loader)) return OrderedDict( zip(['Total loss', 'Photo loss', 'Smooth loss'], losses.avg))
def train_one_epoch(args, train_loader, depth_net, pose_net, optimizer, epoch, n_iter, logger, training_writer, **env): global device logger.reset_train_bar() batch_time = AverageMeter() data_time = AverageMeter() losses = AverageMeter(precision=4) w1, w2, w3 = args.photo_loss_weight, args.smooth_loss_weight, args.ssim e1, e2 = args.training_milestones # switch to train mode depth_net.train() pose_net.train() upsample_depth_net = models.UpSampleNet(depth_net, args.network_input_size) end = time.time() logger.train_bar.update(0) for i, sample in enumerate(train_loader): log_losses = i > 0 and n_iter % args.print_freq == 0 log_output = args.training_output_freq > 0 and n_iter % args.training_output_freq == 0 # measure data loading time data_time.update(time.time() - end) imgs = torch.stack(sample['imgs'], dim=1).to(device) intrinsics = sample['intrinsics'].to(device) intrinsics_inv = sample['intrinsics_inv'].to(device) batch_size, seq = imgs.size()[:2] if args.network_input_size is not None: h, w = args.network_input_size downsample_imgs = F.interpolate(imgs, (3, h, w), mode='area') poses = pose_net(downsample_imgs) # [B, seq, 6] else: poses = pose_net(imgs) pose_matrices = pose_vec2mat(poses, args.rotation_mode) # [B, seq, 3, 4] total_indices = torch.arange(seq).long().to(device).unsqueeze( 0).expand(batch_size, seq) batch_range = torch.arange(batch_size).long().to(device) ''' for each element of the batch select a random picture in the sequence to which we will compute the depth, all poses are then converted so that pose of this very picture is exactly identity. At first this image is always in the middle of the sequence''' if epoch > e2: tgt_id = torch.floor(torch.rand(batch_size) * seq).long().to(device) else: tgt_id = torch.zeros(batch_size).long().to( device) + args.sequence_length // 2 ''' Select what other picture we are going to feed DepthNet, it must not be the same as tgt_id. At first, it's always first picture of the sequence, it is randomly chosen when first training milestone is reached ''' ref_indices = total_indices[total_indices != tgt_id.unsqueeze(1)].view( batch_size, seq - 1) if epoch > e1: prior_id = torch.floor(torch.rand(batch_size) * (seq - 1)).long().to(device) else: prior_id = torch.zeros(batch_size).long().to(device) prior_id = ref_indices[batch_range, prior_id] tgt_imgs = imgs[batch_range, tgt_id] # [B, 3, H, W] tgt_poses = pose_matrices[batch_range, tgt_id] # [B, 3, 4] prior_imgs = imgs[batch_range, prior_id] compensated_poses = compensate_pose( pose_matrices, tgt_poses) # [B, seq, 3, 4] tgt_poses are now neutral pose prior_poses = compensated_poses[batch_range, prior_id] # [B, 3, 4] if args.supervise_pose: from_GT = invert_mat(sample['pose']).to(device) compensated_GT_poses = compensate_pose( from_GT, from_GT[batch_range, tgt_id]) prior_GT_poses = compensated_GT_poses[batch_range, prior_id] prior_imgs_compensated = inverse_rotate(prior_imgs, prior_GT_poses[:, :, :-1], intrinsics, intrinsics_inv) else: prior_imgs_compensated = inverse_rotate(prior_imgs, prior_poses[:, :, :-1], intrinsics, intrinsics_inv) input_pair = torch.cat([prior_imgs_compensated, tgt_imgs], dim=1) # [B, 6, W, H] depth = upsample_depth_net(input_pair) # depth = [sample['depth'].to(device).unsqueeze(1) * 3 / abs(tgt_id[0] - prior_id[0])] # depth.append(torch.nn.functional.interpolate(depth[0], scale_factor=2)) disparities = [1 / d for d in depth] predicted_magnitude = prior_poses[:, :, -1:].norm(p=2, dim=1, keepdim=True).unsqueeze(1) scale_factor = args.nominal_displacement / (predicted_magnitude + 1e-5) normalized_translation = compensated_poses[:, :, :, -1:] * scale_factor # [B, seq_length-1, 3] new_pose_matrices = torch.cat( [compensated_poses[:, :, :, :-1], normalized_translation], dim=-1) biggest_scale = depth[0].size(-1) loss_1 = 0 for k, scaled_depth in enumerate(depth): size_ratio = scaled_depth.size(-1) / biggest_scale loss, diff_maps, warped_imgs = photometric_reconstruction_loss( imgs, tgt_id, ref_indices, scaled_depth, new_pose_matrices, intrinsics, intrinsics_inv, args.rotation_mode, ssim_weight=w3) loss_1 += loss * size_ratio if log_output: training_writer.add_image( 'train Dispnet Output Normalized scale {}'.format(k), tensor2array(disparities[k][0], max_value=None, colormap='bone'), n_iter) training_writer.add_image( 'train Depth Output scale {}'.format(k), tensor2array(scaled_depth[0], max_value=args.max_depth), n_iter) for j, (diff, warped) in enumerate(zip(diff_maps, warped_imgs)): training_writer.add_image( 'train Warped Outputs {} {}'.format(k, j), tensor2array(warped[0]), n_iter) training_writer.add_image( 'train Diff Outputs {} {}'.format(k, j), tensor2array(diff.abs()[0] - 1), n_iter) loss_2 = texture_aware_smooth_loss( depth, tgt_imgs if args.texture_loss else None) loss = w1 * loss_1 + w2 * loss_2 if args.supervise_pose: loss += (from_GT[:, :, :, :3] - pose_matrices[:, :, :, :3]).abs().mean() if log_losses: training_writer.add_scalar('photometric_error', loss_1.item(), n_iter) training_writer.add_scalar('disparity_smoothness_loss', loss_2.item(), n_iter) training_writer.add_scalar('total_loss', loss.item(), n_iter) if log_output: nominal_translation_magnitude = poses[:, -2, :3].norm(p=2, dim=-1) # last pose is always identity and penultimate translation magnitude is always 1, so you don't need to log them for j in range(args.sequence_length - 2): trans_mag = poses[:, j, :3].norm(p=2, dim=-1) training_writer.add_histogram( 'tr {}'.format(j), (trans_mag / nominal_translation_magnitude).detach().cpu().numpy(), n_iter) for j in range(args.sequence_length - 1): # TODO log a better value : this is magnitude of vector (yaw, pitch, roll) which is not a physical value rot_mag = poses[:, j, 3:].norm(p=2, dim=-1) training_writer.add_histogram('rot {}'.format(j), rot_mag.detach().cpu().numpy(), n_iter) training_writer.add_image('train Input', tensor2array(tgt_imgs[0]), n_iter) # record loss for average meter losses.update(loss.item(), args.batch_size) # compute gradient and do Adam step optimizer.zero_grad() loss.backward() optimizer.step() # measure elapsed time batch_time.update(time.time() - end) end = time.time() with open(args.save_path / args.log_full, 'a') as csvfile: writer = csv.writer(csvfile, delimiter='\t') writer.writerow([loss.item(), loss_1.item(), loss_2.item()]) logger.train_bar.update(i + 1) if i % args.print_freq == 0: logger.train_writer.write('Train: Time {} Data {} Loss {}'.format( batch_time, data_time, losses)) if i >= args.epoch_size - 1: break n_iter += 1 return losses.avg[0], n_iter
def main(): global tgt_pc, tgt_img args = parser.parse_args() from kitti_eval.VOLO_data_utils import test_framework_KITTI as test_framework weights = torch.load(args.pretrained_posenet) seq_length = int(weights['state_dict']['conv1.0.weight'].size(1)/3) pose_net = PoseExpNet(nb_ref_imgs=seq_length - 1, output_exp=False).to(device) pose_net.load_state_dict(weights['state_dict'], strict=False) dataset_dir = Path(args.dataset_dir) framework = test_framework(dataset_dir, args.sequences, seq_length) print('{} snippets to test'.format(len(framework))) errors = np.zeros((len(framework), 2), np.float32) optimized_errors = np.zeros((len(framework), 2), np.float32) if args.output_dir is not None: output_dir = Path(args.output_dir) output_dir.makedirs_p() predictions_array = np.zeros((len(framework), seq_length, 3, 4)) f=open('//') for j, sample in enumerate(tqdm(framework)): ''' VO部分 并计算和真值的差值 ''' imgs = sample['imgs'] h,w,_ = imgs[0].shape if (not args.no_resize) and (h != args.img_height or w != args.img_width): imgs = [imresize(img, (args.img_height, args.img_width)).astype(np.float32) for img in imgs] imgs = [np.transpose(img, (2,0,1)) for img in imgs] ref_imgs = [] for i, img in enumerate(imgs): img = torch.from_numpy(img).unsqueeze(0) img = ((img/255 - 0.5)/0.5).to(device) if i == len(imgs)//2: tgt_img = img else: ref_imgs.append(img) timeCostVO=0 startTimeVO=time.time() _, poses = pose_net(tgt_img, ref_imgs) timeCostVO=time.time()-startTimeVO poses = poses.cpu()[0] poses = torch.cat([poses[:len(imgs)//2], torch.zeros(1,6).float(), poses[len(imgs)//2:]]) inv_transform_matrices = pose_vec2mat(poses, rotation_mode=args.rotation_mode).numpy().astype(np.float64) rot_matrices = np.linalg.inv(inv_transform_matrices[:,:,:3]) tr_vectors = -rot_matrices @ inv_transform_matrices[:,:,-1:] transform_matrices = np.concatenate([rot_matrices, tr_vectors], axis=-1) print('**********DeepVO result: time_cost {:.3} s'.format(timeCostVO/(len(imgs)-1))) #print(transform_matrices) first_inv_transform = inv_transform_matrices[0] final_poses = first_inv_transform[:,:3] @ transform_matrices final_poses[:,:,-1:] += first_inv_transform[:,-1:] # print('first') # print(first_inv_transform) print('poses') print(final_poses) if args.output_dir is not None: predictions_array[j] = final_poses ATE, RE = compute_pose_error(sample['poses'], final_poses) errors[j] = ATE, RE ''' LO部分 以VO的结果作为预估,并计算和真值的差值 ''' pointclouds=sample['pointclouds'] from VOLO import LO #pointcluds是可以直接处理的 for i, pc in enumerate(pointclouds): if i == len(pointclouds)//2: tgt_pc =pointclouds[i] optimized_transform_matrices=[] timeCostLO=0 startTimeLO=time.time() totalIterations=0 for i,pc in enumerate(pointclouds): pose_proposal=np.identity(4) pose_proposal[:3,:]=transform_matrices[i] print('======pose proposal for LO=====') print(pose_proposal) T,distacnces,iterations=LO(pc,tgt_pc,init_pose=pose_proposal, max_iterations=50, tolerance=0.001,LO='icp') optimized_transform_matrices.append(T) totalIterations+=iterations print('iterations:\n') print(iterations) timeCostLO=time.time()-startTimeLO optimized_transform_matrices=np.asarray(optimized_transform_matrices) print('*****LO result: time_cost {:.3} s'.format(timeCostLO/(len(pointclouds)-1))+' average iterations: {}' .format(totalIterations/(len(pointclouds)-1))) # print(optimized_transform_matrices) #TODO 打通VO-LO pipeline: 需要将转换矩阵格式对齐; 评估VO的预估对LO的增益:效率上和精度上; 评估过程可视化 #TODO 利用数据集有对应的图像,点云和位姿真值的数据集(Kitti的odomerty) inv_optimized_rot_matrices = np.linalg.inv(optimized_transform_matrices[:,:3,:3]) inv_optimized_tr_vectors = -inv_optimized_rot_matrices @ optimized_transform_matrices[:,:3,-1:] inv_optimized_transform_matrices = np.concatenate([inv_optimized_rot_matrices, inv_optimized_tr_vectors], axis=-1) first_inv_optimized_transform = inv_optimized_transform_matrices[0] final_optimized_poses = first_inv_optimized_transform[:,:3] @ optimized_transform_matrices[:,:3,:] final_optimized_poses[:,:,-1:] += first_inv_optimized_transform[:,-1:] # print('first') # print(first_inv_optimized_transform) print('poses') print(final_optimized_poses) if args.output_dir is not None: predictions_array[j] = final_poses optimized_ATE, optimized_RE = compute_pose_error(sample['poses'], final_optimized_poses) optimized_errors[j] = optimized_ATE, optimized_RE print('==============\n===============\n') mean_errors = errors.mean(0) std_errors = errors.std(0) error_names = ['ATE','RE'] print('') print("Results") print("\t {:>10}, {:>10}".format(*error_names)) print("mean \t {:10.4f}, {:10.4f}".format(*mean_errors)) print("std \t {:10.4f}, {:10.4f}".format(*std_errors)) optimized_mean_errors = optimized_errors.mean(0) optimized_std_errors = optimized_errors.std(0) optimized_error_names = ['optimized_ATE','optimized_RE'] print('') print("optimized_Results") print("\t {:>10}, {:>10}".format(*optimized_error_names)) print("mean \t {:10.4f}, {:10.4f}".format(*optimized_mean_errors)) print("std \t {:10.4f}, {:10.4f}".format(*optimized_std_errors)) if args.output_dir is not None: np.save(output_dir/'predictions.npy', predictions_array)
def adjust_shifts(args, train_set, adjust_loader, depth_net, pose_net, epoch, logger, training_writer, **env): batch_time = AverageMeter() data_time = AverageMeter() new_shifts = AverageMeter(args.sequence_length - 1, precision=2) pose_net.eval() depth_net.eval() upsample_depth_net = models.UpSampleNet(depth_net, args.network_input_size) end = time.time() mid_index = (args.sequence_length - 1) // 2 # we contrain mean value of depth net output from pair 0 and mid_index target_values = np.arange( -mid_index, mid_index + 1) / (args.target_mean_depth * mid_index) target_values = 1 / np.abs( np.concatenate( [target_values[:mid_index], target_values[mid_index + 1:]])) logger.reset_train_bar(len(adjust_loader)) for i, sample in enumerate(adjust_loader): index = sample['index'] # measure data loading time data_time.update(time.time() - end) imgs = torch.stack(sample['imgs'], dim=1).to(device) intrinsics = sample['intrinsics'].to(device) intrinsics_inv = sample['intrinsics_inv'].to(device) # compute output batch_size, seq = imgs.size()[:2] if args.network_input_size is not None: h, w = args.network_input_size downsample_imgs = F.interpolate(imgs, (3, h, w), mode='area') poses = pose_net(downsample_imgs) # [B, seq, 6] else: poses = pose_net(imgs) pose_matrices = pose_vec2mat(poses, args.rotation_mode) # [B, seq, 3, 4] tgt_imgs = imgs[:, mid_index] # [B, 3, H, W] tgt_poses = pose_matrices[:, mid_index] # [B, 3, 4] compensated_poses = compensate_pose( pose_matrices, tgt_poses) # [B, seq, 3, 4] tgt_poses are now neutral pose ref_indices = list(range(args.sequence_length)) ref_indices.remove(mid_index) mean_depth_batch = [] for ref_index in ref_indices: prior_imgs = imgs[:, ref_index] prior_poses = compensated_poses[:, ref_index] # [B, 3, 4] prior_imgs_compensated = inverse_rotate(prior_imgs, prior_poses[:, :, :3], intrinsics, intrinsics_inv) input_pair = torch.cat([prior_imgs_compensated, tgt_imgs], dim=1) # [B, 6, W, H] depth = upsample_depth_net(input_pair) # [B, 1, H, W] mean_depth = depth.view(batch_size, -1).mean(-1).cpu().numpy() # B mean_depth_batch.append(mean_depth) for j, mean_values in zip(index, np.stack(mean_depth_batch, axis=-1)): ratio = mean_values / target_values # if mean value is too high, raise the shift, lower otherwise train_set.reset_shifts(j, ratio[:mid_index], ratio[mid_index:]) new_shifts.update(train_set.get_shifts(j)) # measure elapsed time batch_time.update(time.time() - end) end = time.time() logger.train_bar.update(i) if i % args.print_freq == 0: logger.train_writer.write('Adjustement:' 'Time {} Data {} shifts {}'.format( batch_time, data_time, new_shifts)) for i, shift in enumerate(new_shifts.avg): training_writer.add_scalar('shifts{}'.format(i), shift, epoch) return new_shifts.avg
def main(): args = parser.parse_args() # from kitti_eval.VOLO_data_utils import test_framework_KITTI as test_framework from kitti_eval.pose_evaluation_utils import test_framework_KITTI as test_framework weights = torch.load(args.pretrained_posenet) seq_length = int(weights['state_dict']['conv1.0.weight'].size(1) / 3) pose_net = PoseExpNet(nb_ref_imgs=seq_length - 1, output_exp=False).to(device) pose_net.load_state_dict(weights['state_dict'], strict=False) dataset_dir = Path(args.dataset_dir) sequences=[args.sequence_idx] framework = test_framework(dataset_dir, sequences, seq_length) print('{} snippets to test'.format(len(framework))) errors = np.zeros((len(framework), 2), np.float32) optimized_errors = np.zeros((len(framework), 2), np.float32) iteration_arr = np.zeros(len(framework)) LO_iter_times = np.zeros(len(framework)) if args.output_dir is not None: output_dir = Path(args.output_dir) output_dir.makedirs_p() predictions_array = np.zeros((len(framework), seq_length, 3, 4)) abs_VO_poses = np.zeros((len(framework), 12)) abs_VO_pose = np.identity(4) last_pose = np.identity(4) last_VO_pose = np.identity(4) # L和C的转换矩阵,对齐输入位姿到雷达坐标系 Transform_matrix_L2C = np.identity(4) Transform_matrix_L2C[:3, :3] = np.array([[7.533745e-03, -9.999714e-01, -6.166020e-04], [1.480249e-02, 7.280733e-04, -9.998902e-01], [9.998621e-01, 7.523790e-03, 1.480755e-02]]) Transform_matrix_L2C[:3, -1:] = np.array([-4.069766e-03, -7.631618e-02, -2.717806e-01]).reshape(3, 1) Transform_matrix_C2L = np.linalg.inv(Transform_matrix_L2C) pointClouds = loadPointCloud(args.dataset_dir + "/sequences/" + args.sequence_idx + "/velodyne") # *************可视化准备*********************** num_frames = len(tqdm(framework)) # Pose Graph Manager (for back-end optimization) initialization PGM = PoseGraphManager() PGM.addPriorFactor() # Result saver save_dir = "result/" + args.sequence_idx if not os.path.exists(save_dir): os.makedirs(save_dir) ResultSaver = PoseGraphResultSaver(init_pose=PGM.curr_se3, save_gap=args.save_gap, num_frames=num_frames, seq_idx=args.sequence_idx, save_dir=save_dir) # Scan Context Manager (for loop detection) initialization SCM = ScanContextManager(shape=[args.num_rings, args.num_sectors], num_candidates=args.num_candidates, threshold=args.loop_threshold) # for save the results as a video fig_idx = 1 fig = plt.figure(fig_idx) writer = FFMpegWriter(fps=15) video_name = args.sequence_idx + "_" + str(args.num_icp_points) + "_prop@" + str(args.proposal) + "_tol@" + str( args.tolerance) + ".mp4" num_frames_to_skip_to_show = 5 num_frames_to_save = np.floor(num_frames / num_frames_to_skip_to_show) with writer.saving(fig, video_name, num_frames_to_save): # this video saving part is optional for j, sample in enumerate(tqdm(framework)): ''' ***************************************VO部分******************************************* ''' imgs = sample['imgs'] h, w, _ = imgs[0].shape if (not args.no_resize) and (h != args.img_height or w != args.img_width): imgs = [imresize(img, (args.img_height, args.img_width)).astype(np.float32) for img in imgs] imgs = [np.transpose(img, (2, 0, 1)) for img in imgs] ref_imgs = [] for i, img in enumerate(imgs): img = torch.from_numpy(img).unsqueeze(0) img = ((img / 255 - 0.5) / 0.5).to(device) if i == len(imgs) // 2: tgt_img = img else: ref_imgs.append(img) startTimeVO = time.time() _, poses = pose_net(tgt_img, ref_imgs) timeCostVO = time.time() - startTimeVO poses = poses.cpu()[0] poses = torch.cat([poses[:len(imgs) // 2], torch.zeros(1, 6).float(), poses[len(imgs) // 2:]]) inv_transform_matrices = pose_vec2mat(poses, rotation_mode=args.rotation_mode).numpy().astype(np.float64) rot_matrices = np.linalg.inv(inv_transform_matrices[:, :, :3]) tr_vectors = -rot_matrices @ inv_transform_matrices[:, :, -1:] transform_matrices = np.concatenate([rot_matrices, tr_vectors], axis=-1) print('**********DeepVO result: time_cost {:.3} s'.format(timeCostVO / (len(imgs) - 1))) # print(transform_matrices) # 将对[0 1 2]中间1的转换矩阵变成对0的位姿转换 first_inv_transform = inv_transform_matrices[0] final_poses = first_inv_transform[:, :3] @ transform_matrices final_poses[:, :, -1:] += first_inv_transform[:, -1:] # print('first') # print(first_inv_transform) print('poses') print(final_poses) # cur_VO_pose取final poses的第2项,则是取T10,T21,T32。。。 cur_VO_pose = np.identity(4) cur_VO_pose[:3, :] = final_poses[1] print("对齐前未有尺度修正的帧间位姿") print(cur_VO_pose) print("last_pose") print(last_pose) print("last_VO_pose") print(last_VO_pose) #尺度因子的确定:采用上一帧的LO输出位姿和VO输出位姿的尺度比值作为当前帧的尺度因子,初始尺度为1 if j == 0: scale_factor = 7 else: scale_factor = math.sqrt(np.sum(last_pose[:3, -1] ** 2) / np.sum(last_VO_pose[:3, -1] ** 2)) print("分子", np.sum(last_pose[:3, -1] ** 2)) print("分母", np.sum(last_VO_pose[:3, -1] ** 2)) last_VO_pose = copy.deepcopy(cur_VO_pose) # 注意深拷贝 print("尺度因子:", scale_factor) # 先尺度修正,再对齐 cur_VO_pose[:3, -1:] = cur_VO_pose[:3, -1:] * scale_factor print("尺度修正后...") print(cur_VO_pose) cur_VO_pose = Transform_matrix_C2L @ cur_VO_pose @ np.linalg.inv(Transform_matrix_C2L) print("对齐到雷达坐标系帧间位姿") print(cur_VO_pose) '''*************************LO部分******************************************''' tgt_pc = random_sampling(pointClouds[j], 5000) pc = random_sampling(pointClouds[j + 1], 5000) from point_cloud_processing.icpImpl import icp if args.proposal == 0: init_pose = None elif args.proposal == 1: init_pose = last_pose elif args.proposal == 2: init_pose = cur_VO_pose startTimeLO = time.time() odom_transform, distacnces, iterations = icp(pc, tgt_pc, init_pose=init_pose, tolerance=args.tolerance, max_iterations=50) iter_time = time.time() - startTimeLO LO_iter_times[j] = iter_time iteration_arr[j] = iterations last_pose = odom_transform print("LO优化后的位姿,mean_dis: ", np.asarray(distacnces).mean()) print(odom_transform) print("LO迭代次数:", iterations) PGM.curr_node_idx = j # make start with 0 if (PGM.curr_node_idx == 0): PGM.prev_node_idx = PGM.curr_node_idx continue # update the current (moved) pose PGM.curr_se3 = np.matmul(PGM.curr_se3, odom_transform) # add the odometry factor to the graph # PGM.addOdometryFactor(cur_VO_pose) # renewal the prev information PGM.prev_node_idx = PGM.curr_node_idx # loop detection and optimize the graph if (PGM.curr_node_idx > 1 and PGM.curr_node_idx % args.try_gap_loop_detection == 0): # 1/ loop detection loop_idx, loop_dist, yaw_diff_deg = SCM.detectLoop() if (loop_idx == None): # NOT FOUND pass # else: # print("Loop event detected: ", PGM.curr_node_idx, loop_idx, loop_dist) # # 2-1/ add the loop factor # loop_scan_down_pts = SCM.getPtcloud(loop_idx) # loop_transform, _, _ = ICP.icp(curr_scan_down_pts, loop_scan_down_pts, # init_pose=yawdeg2se3(yaw_diff_deg), max_iterations=20) # PGM.addLoopFactor(loop_transform, loop_idx) # # # 2-2/ graph optimization # PGM.optimizePoseGraph() # # # 2-2/ save optimized poses # ResultSaver.saveOptimizedPoseGraphResult(PGM.curr_node_idx, PGM.graph_optimized) # save the ICP odometry pose result (no loop closure) ResultSaver.saveUnoptimizedPoseGraphResult(PGM.curr_se3, PGM.curr_node_idx) if (j % num_frames_to_skip_to_show == 0): ResultSaver.vizCurrentTrajectory(fig_idx=fig_idx) writer.grab_frame() if args.output_dir is not None: predictions_array[j] = final_poses abs_VO_poses[j] = abs_VO_pose[:3, :].reshape(-1, 12)[0] ATE, RE = compute_pose_error(sample['poses'], final_poses) errors[j] = ATE, RE optimized_ATE, optimized_RE = compute_LO_pose_error(sample['poses'], odom_transform, Transform_matrix_L2C) optimized_errors[j] = optimized_ATE, optimized_RE # VO输出位姿的精度指标 mean_errors = errors.mean(0) std_errors = errors.std(0) error_names = ['ATE', 'RE'] print('') print("VO_Results") print("\t {:>10}, {:>10}".format(*error_names)) print("mean \t {:10.4f}, {:10.4f}".format(*mean_errors)) print("std \t {:10.4f}, {:10.4f}".format(*std_errors)) # LO二次优化后的精度指标 optimized_mean_errors = optimized_errors.mean(0) optimized_std_errors = optimized_errors.std(0) optimized_error_names = ['optimized_ATE', 'optimized_RE'] print('') print("LO_optimized_Results") print("\t {:>10}, {:>10}".format(*optimized_error_names)) print("mean \t {:10.4f}, {:10.4f}".format(*optimized_mean_errors)) print("std \t {:10.4f}, {:10.4f}".format(*optimized_std_errors)) # 迭代次数 mean_iterations = iteration_arr.mean() std_iterations = iteration_arr.std() _names = ['iteration'] print('') print("LO迭代次数") print("\t {:>10}".format(*_names)) print("mean \t {:10.4f}".format(mean_iterations)) print("std \t {:10.4f}".format(std_iterations)) # 迭代时间 mean_iter_time = LO_iter_times.mean() std_iter_time = LO_iter_times.std() _names = ['iter_time'] print('') print("LO迭代时间:单位/s") print("\t {:>10}".format(*_names)) print("mean \t {:10.4f}".format(mean_iter_time)) print("std \t {:10.4f}".format(std_iter_time)) if args.output_dir is not None: np.save(output_dir / 'predictions.npy', predictions_array) np.savetxt(output_dir / 'abs_VO_poses.txt', abs_VO_poses)
def main(): args = parser.parse_args() from sintel_eval.pose_evaluation_utils import test_framework_Sintel as test_framework weights = torch.load(args.pretrained_posenet) seq_length = int(weights['state_dict']['conv1.0.weight'].size(1) / 3) pose_net = getattr(models, args.posenet)(nb_ref_imgs=seq_length - 1).cuda() pose_net.load_state_dict(weights['state_dict'], strict=False) dataset_dir = Path(args.dataset_dir) framework = test_framework(dataset_dir, args.sequences, seq_length) print('{} snippets to test'.format(len(framework))) RE = np.zeros((len(framework)), np.float32) if args.output_dir is not None: output_dir = Path(args.output_dir) output_dir.makedirs_p() predictions_array = np.zeros((len(framework), seq_length, 3, 4)) for j, sample in enumerate(tqdm(framework)): imgs = sample['imgs'] h, w, _ = imgs[0].shape if (not args.no_resize) and (h != args.img_height or w != args.img_width): imgs = [ imresize(img, (args.img_height, args.img_width)).astype(np.float32) for img in imgs ] imgs = [np.transpose(img, (2, 0, 1)) for img in imgs] ref_imgs_var = [] for i, img in enumerate(imgs): img = torch.from_numpy(img).unsqueeze(0) img = ((img / 255 - 0.5) / 0.5).cuda() img_var = Variable(img, volatile=True) if i == len(imgs) // 2: tgt_img_var = img_var else: ref_imgs_var.append(Variable(img, volatile=True)) if args.posenet in ["PoseNet6", "PoseNetB6"]: poses = pose_net(tgt_img_var, ref_imgs_var) else: _, poses = pose_net(tgt_img_var, ref_imgs_var) poses = poses.cpu().data[0] poses = torch.cat([ poses[:len(imgs) // 2], torch.zeros(1, 6).float(), poses[len(imgs) // 2:] ]) inv_transform_matrices = pose_vec2mat( Variable(poses), rotation_mode=args.rotation_mode).data.numpy().astype(np.float64) rot_matrices = np.linalg.inv(inv_transform_matrices[:, :, :3]) tr_vectors = -rot_matrices @ inv_transform_matrices[:, :, -1:] transform_matrices = np.concatenate([rot_matrices, tr_vectors], axis=-1) first_inv_transform = inv_transform_matrices[0] final_poses = first_inv_transform[:, :3] @ transform_matrices final_poses[:, :, -1:] += first_inv_transform[:, -1:] if args.output_dir is not None: predictions_array[j] = final_poses RE[j] = compute_pose_error(sample['poses'], final_poses) print('') print("Results") print("\t {:>10}".format('RE')) print("mean \t {:10.4f}".format(RE.mean())) print("std \t {:10.4f}".format(RE.std())) if args.output_dir is not None: np.save(output_dir / 'predictions.npy', predictions_array)
def train(args, train_loader, disp_net, pose_exp_net, optimizer, epoch_size, logger, tb_writer): global n_iter, device batch_time = AverageMeter() data_time = AverageMeter() losses = AverageMeter(precision=4) w1, w2, w3 = args.photo_loss_weight, args.mask_loss_weight, args.smooth_loss_weight # switch to train mode disp_net.train() pose_exp_net.train() end = time.time() logger.train_bar.update(0) for i, (tgt_img, ref_imgs, intrinsics, intrinsics_inv) in enumerate(train_loader): log_losses = i > 0 and n_iter % args.print_freq == 0 log_output = args.training_output_freq > 0 and n_iter % args.training_output_freq == 0 # measure data loading time data_time.update(time.time() - end) tgt_img = tgt_img.to(device) ref_imgs = [img.to(device) for img in ref_imgs] intrinsics = intrinsics.to(device) # compute output disparities = disp_net(tgt_img) depth = [1 / disp for disp in disparities] # print("***",len(depth),depth[0].size()) explainability_mask, pose = pose_exp_net(tgt_img, ref_imgs) loss_1, warped, diff = photometric_reconstruction_loss( tgt_img, ref_imgs, intrinsics, depth, explainability_mask, pose, args.rotation_mode, args.padding_mode) if w2 > 0: loss_2 = explainability_loss(explainability_mask) else: loss_2 = 0 loss_3 = smooth_loss(depth) loss = w1 * loss_1 + w2 * loss_2 + w3 * loss_3 if args.with_photocon_loss: batch_size = pose.size()[0] homo_row = torch.tensor([[0, 0, 0, 1]], dtype=torch.float).to(device) homo_row = homo_row.unsqueeze(0).expand(batch_size, -1, -1) T21 = pose_vec2mat(pose[:, 0]) T21 = torch.cat((T21, homo_row), 1) T12 = torch.inverse(T21) T23 = pose_vec2mat(pose[:, 1]) T23 = torch.cat((T23, homo_row), 1) T13 = torch.matmul(T23, T12) #[B, 4, 4] # print("----",T13.size()) # target = 1 and ref = 3 ref_img_warped, valid_points = inverse_warp_posemat( ref_imgs[1], depth[0][:, 0], T13, intrinsics, args.rotation_mode, args.padding_mode) diff = (ref_imgs[0] - ref_img_warped) * valid_points.unsqueeze(1).float() loss_4 = diff.abs().mean() loss += loss_4 if log_losses: tb_writer.add_scalar('photometric_error', loss_1.item(), n_iter) if w2 > 0: tb_writer.add_scalar('explanability_loss', loss_2.item(), n_iter) tb_writer.add_scalar('disparity_smoothness_loss', loss_3.item(), n_iter) tb_writer.add_scalar('total_loss', loss.item(), n_iter) if log_output: tb_writer.add_image('train Input', tensor2array(tgt_img[0]), n_iter) for k, scaled_maps in enumerate( zip(depth, disparities, warped, diff, explainability_mask)): log_output_tensorboard(tb_writer, "train", 0, " {}".format(k), n_iter, *scaled_maps) # record loss and EPE losses.update(loss.item(), args.batch_size) # compute gradient and do Adam step optimizer.zero_grad() loss.backward() optimizer.step() # measure elapsed time batch_time.update(time.time() - end) end = time.time() with open(args.save_path / args.log_full, 'a') as csvfile: writer = csv.writer(csvfile, delimiter='\t') writer.writerow([ loss.item(), loss_1.item(), loss_2.item() if w2 > 0 else 0, loss_3.item() ]) logger.train_bar.update(i + 1) if i % args.print_freq == 0: logger.train_writer.write('Train: Time {} Data {} Loss {}'.format( batch_time, data_time, losses)) if i >= epoch_size - 1: break n_iter += 1 return losses.avg[0]
def main(): args = parser.parse_args() weights = torch.load(args.pretrained_posenet) pose_net = models.PoseNet().to(device) pose_net.load_state_dict(weights['state_dict'], strict=False) pose_net.eval() seq_length = 5 dataset_dir = Path(args.dataset_dir) framework = test_framework(dataset_dir, args.sequences, seq_length) print('{} snippets to test'.format(len(framework))) errors = np.zeros((len(framework), 2), np.float32) if args.output_dir is not None: output_dir = Path(args.output_dir) output_dir.makedirs_p() predictions_array = np.zeros((len(framework), seq_length, 3, 4)) for j, sample in enumerate(tqdm(framework)): imgs = sample['imgs'] h, w, _ = imgs[0].shape if (not args.no_resize) and (h != args.img_height or w != args.img_width): imgs = [ imresize(img, (args.img_height, args.img_width)).astype(np.float32) for img in imgs ] imgs = [np.transpose(img, (2, 0, 1)) for img in imgs] tensor_imgs = [] for i, img in enumerate(imgs): img = ((torch.from_numpy(img).unsqueeze(0) / 255 - 0.5) / 0.5).to(device) tensor_imgs.append(img) global_pose = np.identity(4) poses = [] poses.append(global_pose[0:3, :]) for iter in range(seq_length - 1): pose = pose_net(tensor_imgs[iter], tensor_imgs[iter + 1]) pose_mat = pose_vec2mat(pose).squeeze(0).cpu().numpy() pose_mat = np.vstack([pose_mat, np.array([0, 0, 0, 1])]) global_pose = global_pose @ np.linalg.inv(pose_mat) poses.append(global_pose[0:3, :]) final_poses = np.stack(poses, axis=0) if args.output_dir is not None: predictions_array[j] = final_poses ATE, RE = compute_pose_error(sample['poses'], final_poses) errors[j] = ATE, RE mean_errors = errors.mean(0) std_errors = errors.std(0) error_names = ['ATE', 'RE'] print('') print("Results") print("\t {:>10}, {:>10}".format(*error_names)) print("mean \t {:10.4f}, {:10.4f}".format(*mean_errors)) print("std \t {:10.4f}, {:10.4f}".format(*std_errors)) if args.output_dir is not None: np.save(output_dir / 'predictions.npy', predictions_array)
def main(): args = parser.parse_args() attack = False if args.perturbation and args.tracker_file: attack = True perturbation = np.load(Path(args.perturbation)) noise_mask = np.load(Path(args.tracker_file)) from kitti_eval.pose_evaluation_utils import test_framework_KITTI as test_framework weights = torch.load(args.pretrained_posenet, map_location=device) seq_length = int(weights['state_dict']['conv1.0.weight'].size(1) / 3) pose_net = PoseExpNet(nb_ref_imgs=seq_length - 1, output_exp=False).to(device) pose_net.load_state_dict(weights['state_dict'], strict=False) dataset_dir = Path(args.dataset_dir) framework = test_framework(dataset_dir, args.sequences, seq_length) print('{} snippets to test'.format(len(framework))) errors = np.zeros((len(framework), 2), np.float32) if args.output_dir is not None: output_dir = Path(args.output_dir) output_dir.makedirs_p() predictions_array = np.zeros((len(framework), seq_length, 3, 4)) ground_truth_array = np.zeros((len(framework), seq_length, 3, 4)) for j, sample in enumerate(tqdm(framework)): imgs = sample['imgs'] h, w, _ = imgs[0].shape if (not args.no_resize) and (h != args.img_height or w != args.img_width): imgs = [ imresize(img, (args.img_height, args.img_width)).astype(np.float32) for img in imgs ] imgs = [np.transpose(img, (2, 0, 1)) for img in imgs] ref_imgs = [] for i, img in enumerate(imgs): img = torch.from_numpy(img).unsqueeze(0) img = ((img / 255 - 0.5) / 0.5).to(device) if i == len(imgs) // 2: tgt_img = img else: ref_imgs.append(img) if attack: # Add noise to target image if j + 2 >= first_frame and j + 2 < last_frame: curr_mask = noise_mask[j - first_frame + 2].astype(np.int) w = curr_mask[2] - curr_mask[0] h = curr_mask[3] - curr_mask[1] noise_box = resize2d(perturbation, (h, w)) tgt_img[0][:, curr_mask[1]:curr_mask[3], curr_mask[0]:curr_mask[2]] += noise_box tgt_img[0] = tgt_img[0].clamp(-1, 1) # Add noise to reference images for k in range(5): ref_idx = k if k == 2: # Skip target image continue if k > 2: # Since it is numbered: ref1, ref2, tgt, ref3, ref4 ref_idx = k - 1 if j + k >= first_frame and j + k < last_frame: curr_mask = noise_mask[j - first_frame + k].astype(np.int) w = curr_mask[2] - curr_mask[0] h = curr_mask[3] - curr_mask[1] noise_box = resize2d(perturbation, (h, w)) ref_imgs[ref_idx][ 0][:, curr_mask[1]:curr_mask[3], curr_mask[0]:curr_mask[2]] += noise_box ref_imgs[ref_idx] = ref_imgs[ref_idx].clamp(-1, 1) _, poses = pose_net(tgt_img, ref_imgs) poses = poses.cpu()[0] poses = torch.cat([ poses[:len(imgs) // 2], torch.zeros(1, 6).float(), poses[len(imgs) // 2:] ]) inv_transform_matrices = pose_vec2mat( poses, rotation_mode=args.rotation_mode).numpy().astype(np.float64) rot_matrices = np.linalg.inv(inv_transform_matrices[:, :, :3]) tr_vectors = -rot_matrices @ inv_transform_matrices[:, :, -1:] transform_matrices = np.concatenate([rot_matrices, tr_vectors], axis=-1) first_inv_transform = inv_transform_matrices[0] final_poses = first_inv_transform[:, :3] @ transform_matrices final_poses[:, :, -1:] += first_inv_transform[:, -1:] if args.output_dir is not None: ground_truth_array[j] = sample['poses'] predictions_array[j] = final_poses ATE, RE = compute_pose_error(sample['poses'], final_poses) errors[j] = ATE, RE mean_errors = errors.mean(0) std_errors = errors.std(0) error_names = ['ATE', 'RE'] print('') print("Results") print("\t {:>10}, {:>10}".format(*error_names)) print("mean \t {:10.4f}, {:10.4f}".format(*mean_errors)) print("std \t {:10.4f}, {:10.4f}".format(*std_errors)) if args.output_dir is not None: np.save(output_dir / 'ground_truth.npy', ground_truth_array) np.save(output_dir / 'predictions_perturbed.npy', predictions_array)
def main(): args = parser.parse_args() weights_pose = torch.load(args.pretrained_posenet) pose_net = models.PoseResNet().to(device) pose_net.load_state_dict(weights_pose['state_dict'], strict=False) pose_net.eval() image_dir = Path(args.dataset_dir) output_dir = Path(args.output_dir) output_dir.makedirs_p() test_files = sum( [image_dir.files('*.{}'.format(ext)) for ext in args.img_exts], []) test_files.sort() print('{} files to test'.format(len(test_files))) #print(test_files) global_pose = np.eye(4) if 'advio-04' in args.dataset_dir: global_pose = np.array([[ 0.2337503561460601, 0.71124984004749991, -0.66293618387207998, 0.0037387620000000001 ], [ 0.10487759004941999, 0.65940272844750003, 0.74443851971943986, 0.082704390000000003 ], [ 0.96662371684119996, -0.24353990138999998, 0.079541459813400106, -0.023607989999999999 ], [0.0, 0.0, 0.0, 1.0]]) # advio-04 elif 'advio-23' in args.dataset_dir: global_pose = np.array([[ -0.02024017794777988, -0.7758527702428, 0.630589204478, -0.005759389 ], [0.11342629147720007, 0.62486446287, 0.7724499038534, -0.09049783], [ -0.993340194646, 0.08715994761339996, 0.0753547505262201, -0.001374606 ], [0.0, 0.0, 0.0, 1.0]]) # advio-23 print(str(global_pose)) poses = [global_pose[0:3, :].reshape(1, 12)] n = len(test_files) tensor_img1 = load_tensor_image(test_files[0], args) for iter in tqdm(range(n - 1)): tensor_img2 = load_tensor_image(test_files[iter + 1], args) pose = pose_net(tensor_img1, tensor_img2) pose_mat = pose_vec2mat(pose).squeeze(0).cpu().numpy() pose_mat = np.vstack([pose_mat, np.array([0, 0, 0, 1])]) global_pose = global_pose @ np.linalg.inv(pose_mat) poses.append(global_pose[0:3, :].reshape(1, 12)) # update tensor_img1 = tensor_img2 poses = np.concatenate(poses, axis=0) filename = Path(args.output_dir + args.sequence + ".txt") np.savetxt(filename, poses, delimiter=' ', fmt='%1.8e')
def validate_without_gt(args, val_loader, depth_net, pose_net, epoch, logger, tb_writer, sample_nb_to_log, **env): global device batch_time = AverageMeter() losses = AverageMeter(i=3, precision=4) w1, w2, w3 = args.photo_loss_weight, args.smooth_loss_weight, args.ssim if args.log_output: poses_values = np.zeros(((len(val_loader) - 1) * args.test_batch_size * (args.sequence_length - 1), 6)) disp_values = np.zeros( ((len(val_loader) - 1) * args.test_batch_size * 3)) # switch to evaluate mode depth_net.eval() pose_net.eval() end = time.time() logger.valid_bar.update(0) for i, sample in enumerate(val_loader): log_output = i < sample_nb_to_log imgs = torch.stack(sample['imgs'], dim=1).to(device) intrinsics = sample['intrinsics'].to(device) if epoch == 1 and log_output: for j, img in enumerate(sample['imgs']): tb_writer.add_image('val Input/{}'.format(i), tensor2array(img[0]), j) batch_size, seq = imgs.size()[:2] poses = pose_net(imgs) pose_matrices = pose_vec2mat(poses, args.rotation_mode) # [B, seq, 3, 4] mid_index = (args.sequence_length - 1) // 2 tgt_imgs = imgs[:, mid_index] # [B, 3, H, W] tgt_poses = pose_matrices[:, mid_index] # [B, 3, 4] compensated_poses = compensate_pose( pose_matrices, tgt_poses) # [B, seq, 3, 4] tgt_poses are now neutral pose ref_ids = list(range(args.sequence_length)) ref_ids.remove(mid_index) loss_1 = 0 loss_2 = 0 for ref_index in ref_ids: prior_imgs = imgs[:, ref_index] prior_poses = compensated_poses[:, ref_index] # [B, 3, 4] prior_imgs_compensated = inverse_rotate(prior_imgs, prior_poses[:, :, :3], intrinsics) input_pair = torch.cat([prior_imgs_compensated, tgt_imgs], dim=1) # [B, 6, W, H] predicted_magnitude = prior_poses[:, :, -1:].norm( p=2, dim=1, keepdim=True).unsqueeze(1) # [B, 1, 1, 1] scale_factor = args.nominal_displacement / predicted_magnitude normalized_translation = compensated_poses[:, :, :, -1:] * scale_factor # [B, seq, 3, 1] new_pose_matrices = torch.cat( [compensated_poses[:, :, :, :-1], normalized_translation], dim=-1) depth = depth_net(input_pair) disparity = 1 / depth tgt_id = torch.full((batch_size, ), ref_index, dtype=torch.int64, device=device) ref_ids_tensor = torch.tensor(ref_ids, dtype=torch.int64, device=device).expand( batch_size, -1) photo_loss, *to_log = photometric_reconstruction_loss( imgs, tgt_id, ref_ids_tensor, depth, new_pose_matrices, intrinsics, args.rotation_mode, ssim_weight=w3, upsample=args.upscale) loss_1 += photo_loss if log_output: log_output_tensorboard(tb_writer, "train", i, ref_index, epoch, depth[0], disparity[0], *to_log) loss_2 += grad_diffusion_loss(disparity, tgt_imgs, args.kappa) if args.log_output and i < len(val_loader) - 1: step = args.test_batch_size * (args.sequence_length - 1) poses_values[i * step:(i + 1) * step] = poses[:, :-1].cpu().view( -1, 6).numpy() step = args.test_batch_size * 3 disp_unraveled = disparity.cpu().view(args.test_batch_size, -1) disp_values[i * step:(i + 1) * step] = torch.cat([ disp_unraveled.min(-1)[0], disp_unraveled.median(-1)[0], disp_unraveled.max(-1)[0] ]).numpy() loss = w1 * loss_1 + w2 * loss_2 losses.update([loss.item(), loss_1.item(), loss_2.item()]) # measure elapsed time batch_time.update(time.time() - end) end = time.time() logger.valid_bar.update(i + 1) if i % args.print_freq == 0: logger.valid_writer.write('valid: Time {} Loss {}'.format( batch_time, losses)) if args.log_output: rot_coeffs = ['rx', 'ry', 'rz'] if args.rotation_mode == 'euler' else [ 'qx', 'qy', 'qz' ] tr_coeffs = ['tx', 'ty', 'tz'] for k, (coeff_name) in enumerate(tr_coeffs + rot_coeffs): tb_writer.add_histogram('val poses_{}'.format(coeff_name), poses_values[:, k], epoch) tb_writer.add_histogram('disp_values', disp_values, epoch) logger.valid_bar.update(len(val_loader)) return OrderedDict( zip(['Total loss', 'Photo loss', 'Smooth loss'], losses.avg))
def train_one_epoch(args, train_loader, depth_net, pose_net, optimizer, epoch, n_iter, logger, tb_writer, **env): global device logger.reset_train_bar() batch_time = AverageMeter() data_time = AverageMeter() losses = AverageMeter(precision=4) w1, w2, w3 = args.photo_loss_weight, args.smooth_loss_weight, args.ssim e1, e2 = args.training_milestones # switch to train mode depth_net.train() pose_net.train() end = time.time() logger.train_bar.update(0) for i, sample in enumerate(train_loader): log_losses = i > 0 and n_iter % args.print_freq == 0 log_output = args.training_output_freq > 0 and n_iter % args.training_output_freq == 0 # measure data loading time data_time.update(time.time() - end) imgs = torch.stack(sample['imgs'], dim=1).to(device) intrinsics = sample['intrinsics'].to(device) batch_size, seq = imgs.size()[:2] if args.network_input_size is not None: h, w = args.network_input_size downsample_imgs = F.interpolate(imgs, (3, h, w), mode='area') poses = pose_net(downsample_imgs) # [B, seq, 6] else: poses = pose_net(imgs) pose_matrices = pose_vec2mat(poses, args.rotation_mode) # [B, seq, 3, 4] total_indices = torch.arange(seq, dtype=torch.int64, device=device).expand(batch_size, seq) batch_range = torch.arange(batch_size, dtype=torch.int64, device=device) ''' for each element of the batch select a random picture in the sequence to which we will compute the depth, all poses are then converted so that pose of this very picture is exactly identity. At first this image is always in the middle of the sequence''' if epoch > e2: tgt_id = torch.randint(0, seq, (batch_size, ), device=device) else: tgt_id = torch.full_like(batch_range, args.sequence_length // 2) ref_ids = total_indices[total_indices != tgt_id.unsqueeze(1)].view( batch_size, seq - 1) ''' Select what other picture we are going to feed DepthNet, it must not be the same as tgt_id. At first, it's always first picture of the sequence, it is randomly chosen when first training milestone is reached ''' if epoch > e1: probs = torch.ones_like(total_indices, dtype=torch.float32) probs[batch_range, tgt_id] = args.same_ratio prior_id = torch.multinomial(probs, 1)[:, 0] else: prior_id = torch.zeros_like(batch_range) # Treat the case of prior_id == tgt_id and the depth must be max_depth, regardless of apparent movement tgt_imgs = imgs[batch_range, tgt_id] # [B, 3, H, W] tgt_poses = pose_matrices[batch_range, tgt_id] # [B, 3, 4] prior_imgs = imgs[batch_range, prior_id] compensated_poses = compensate_pose( pose_matrices, tgt_poses) # [B, seq, 3, 4] tgt_poses are now neutral pose prior_poses = compensated_poses[batch_range, prior_id] # [B, 3, 4] if args.supervise_pose: from_GT = invert_mat(sample['pose']).to(device) compensated_GT_poses = compensate_pose( from_GT, from_GT[batch_range, tgt_id]) prior_GT_poses = compensated_GT_poses[batch_range, prior_id] prior_imgs_compensated = inverse_rotate(prior_imgs, prior_GT_poses[:, :, :-1], intrinsics) else: prior_imgs_compensated = inverse_rotate(prior_imgs, prior_poses[:, :, :-1], intrinsics) input_pair = torch.cat([prior_imgs_compensated, tgt_imgs], dim=1) # [B, 6, W, H] depth = depth_net(input_pair) # depth = [sample['depth'].to(device).unsqueeze(1) * 3 / abs(tgt_id[0] - prior_id[0])] # depth.append(torch.nn.functional.interpolate(depth[0], scale_factor=2)) disparities = [1 / d for d in depth] predicted_magnitude = prior_poses[:, :, -1:].norm(p=2, dim=1, keepdim=True).unsqueeze(1) scale_factor = args.nominal_displacement / (predicted_magnitude + 1e-5) normalized_translation = compensated_poses[:, :, :, -1:] * scale_factor # [B, seq_length-1, 3] new_pose_matrices = torch.cat( [compensated_poses[:, :, :, :-1], normalized_translation], dim=-1) biggest_scale = depth[0].size(-1) # Construct valid sequence to compute photometric error, # make the rest converge to max_depth because nothing moved vb = batch_range[prior_id != tgt_id] same_range = batch_range[prior_id == tgt_id] # batch of still pairs loss_1 = 0 loss_1_same = 0 for k, scaled_depth in enumerate(depth): size_ratio = scaled_depth.size(-1) / biggest_scale if len(same_range) > 0: # Frames are identical. The corresponding depth must be infinite. Here, we set it to max depth still_depth = scaled_depth[same_range] loss_same = F.smooth_l1_loss(still_depth / args.max_depth, torch.ones_like(still_depth)) else: loss_same = 0 loss_valid, *to_log = photometric_reconstruction_loss( imgs[vb], tgt_id[vb], ref_ids[vb], scaled_depth[vb], new_pose_matrices[vb], intrinsics[vb], args.rotation_mode, ssim_weight=w3, upsample=args.upscale) loss_1 += loss_valid * size_ratio loss_1_same += loss_same * size_ratio if log_output and len(vb) > 0: log_output_tensorboard(tb_writer, "train", 0, k, n_iter, scaled_depth[0], disparities[k][0], *to_log) loss_2 = grad_diffusion_loss(disparities, tgt_imgs, args.kappa) loss = w1 * (loss_1 + loss_1_same) + w2 * loss_2 if args.supervise_pose: loss += (from_GT[:, :, :, :3] - pose_matrices[:, :, :, :3]).abs().mean() if log_losses: tb_writer.add_scalar('photometric_error', loss_1.item(), n_iter) tb_writer.add_scalar('disparity_smoothness_loss', loss_2.item(), n_iter) tb_writer.add_scalar('total_loss', loss.item(), n_iter) if log_output and len(vb) > 0: valid_poses = poses[vb] nominal_translation_magnitude = valid_poses[:, -2, :3].norm(p=2, dim=-1) # Log the translation magnitude relative to translation magnitude between last and penultimate frames # for a perfectly constant displacement magnitude, you should get ratio of 2,3,4 and so forth. # last pose is always identity and penultimate translation magnitude is always 1, so you don't need to log them for j in range(args.sequence_length - 2): trans_mag = valid_poses[:, j, :3].norm(p=2, dim=-1) tb_writer.add_histogram( 'tr {}'.format(j), (trans_mag / nominal_translation_magnitude).detach().cpu().numpy(), n_iter) for j in range(args.sequence_length - 1): # TODO log a better value : this is magnitude of vector (yaw, pitch, roll) which is not a physical value rot_mag = valid_poses[:, j, 3:].norm(p=2, dim=-1) tb_writer.add_histogram('rot {}'.format(j), rot_mag.detach().cpu().numpy(), n_iter) tb_writer.add_image('train Input', tensor2array(tgt_imgs[0]), n_iter) # record loss for average meter losses.update(loss.item(), args.batch_size) # compute gradient and do Adam step optimizer.zero_grad() loss.backward() optimizer.step() # measure elapsed time batch_time.update(time.time() - end) end = time.time() with open(args.save_path / args.log_full, 'a') as csvfile: writer = csv.writer(csvfile, delimiter='\t') writer.writerow([loss.item(), loss_1.item(), loss_2.item()]) logger.train_bar.update(i + 1) if i % args.print_freq == 0: logger.train_writer.write('Train: Time {} Data {} Loss {}'.format( batch_time, data_time, losses)) if i >= args.epoch_size - 1: break n_iter += 1 return losses.avg[0], n_iter
def main(): args = parser.parse_args() limit_1 = 8 limit_2 = 28 sequences = os.listdir(args.dataset_dir) for seq in sequences: if '.txt' not in seq: print(seq) args.sequence = seq device = torch.device("cuda") if torch.cuda.is_available() else torch.device("cpu") image_dir = Path(args.dataset_dir + args.sequence) test_files = sum([image_dir.files('*.{}'.format(ext)) for ext in args.img_exts], []) test_files.sort() test_files = test_files print('{} files to test'.format(len(test_files))) camera_matrix = np.loadtxt(Path(args.dataset_dir + args.sequence + "/cam.txt")).astype(np.float32) # print(camera_matrix) rvec = np.array([0., 0., 0]) tvec = np.array([0., 0., 0]) rvec, _ = cv2.Rodrigues(rvec) NUM_SHOW_POINTS = 70 # homogeneous point [x, y, z, w] corresponds to the three-dimensional point [x/w, y/w, z/w]. project_points = np.array([[0, 1.7, 3, 1]]).reshape(1, 1, 4) # homogeneous point [x, y, z, w] corresponds to the three-dimensional point [x/w, y/w, z/w]. project_points_l = np.array([[-0.8, 1.7, 3, 1]]).reshape(1, 1, 4) # homogeneous point [x, y, z, w] corresponds to the three-dimensional point [x/w, y/w, z/w]. project_points_r = np.array([[+0.8, 1.7, 3, 1]]).reshape(1, 1, 4) path = args.dataset_dir + args.sequence + "/frame{0:06d}.png" df = pd.read_csv(args.dataset_dir.replace('_frames', '') + 'info/{}-0-static-deleted.csv'.format(seq), sep=",") print(df) global_pose = np.identity(4) poses = [global_pose[0:3, :].reshape(1, 12)] for i in range(len(df)): v = df['linear_speed'][i] dt = 0.1 r = get_radius(df['real_steer_angle'][i] / WHEEL_STEER_RATIO) alpha = v * dt / r rot = [0, alpha, 0] px, py = rotate_point(0, 0, alpha, -r, 0) trans = [px + r, 0, py] pose = torch.tensor(trans + rot).reshape(1, 6) pose_mat = pose_vec2mat(pose).squeeze(0).cpu().numpy() pose_mat = np.vstack([pose_mat, np.array([0, 0, 0, 1])]) global_pose = global_pose @ np.linalg.inv(pose_mat) poses.append(global_pose[0:3, :].reshape(1, 12)) n = len(poses) poses = np.array(poses).reshape(n, 3, 4) x = np.zeros((n, 1, 4)) x[:, :, -1] = 1 poses = np.concatenate([poses, x], axis=1) for i in tqdm(range(n - 1)): crt_pose = np.stack(inv(poses[i]).dot(x) for x in poses[i:]) world_points = project_points.dot(crt_pose.transpose((0, 2, 1)))[0, 0] world_points_l = project_points_l.dot(crt_pose.transpose((0, 2, 1)))[0, 0] world_points_r = project_points_r.dot(crt_pose.transpose((0, 2, 1)))[0, 0] show_img = cv2.imread(path.format(i)).astype(np.float32) / 255. world_points_show = np.concatenate([ world_points[:NUM_SHOW_POINTS][:, :3], world_points_l[:NUM_SHOW_POINTS][:, :3], world_points_r[:NUM_SHOW_POINTS][:, :3] ]) rvec2 = np.eye(3) # it is almost the identity matrix show_points = cv2.projectPoints(world_points_show.astype(np.float64), rvec2, tvec, camera_matrix, None)[0] show_points_l = cv2.projectPoints(world_points_l[:NUM_SHOW_POINTS][:, :3].astype(np.float64), rvec2, tvec, camera_matrix, None)[0] show_points_r = cv2.projectPoints(world_points_r[:NUM_SHOW_POINTS][:, :3].astype(np.float64), rvec2, tvec, camera_matrix, None)[0] show_points = show_points.astype(np.int)[:, 0] show_points_l = show_points_l.astype(np.int)[:, 0] show_points_r = show_points_r.astype(np.int)[:, 0] overlay = np.zeros_like(show_img) overlay_limited = np.zeros_like(show_img) # overlay[:, :, 0] = 255 ok = True # cv2.imshow('img', show_img) # distances / distances.max()) # cv2.waitKey(0) for it, p1, p2, p3, p4 in zip(range(len(show_points_l) - 1), show_points_l[:-1], show_points_r[:-1], show_points_l[1:], show_points_r[1:]): x1, y1 = p1 x2, y2 = p2 x3, y3 = p3 x4, y4 = p4 pts = np.array([(x1, y1), (x3, y3), (x4, y4), (x2, y2)]) overlay = cv2.drawContours(overlay, [pts], 0, (0, 255, 0), cv2.FILLED) alpha = 1.0 show_img_og = np.copy(show_img) # show_img = cv2.addWeighted(overlay_limited, alpha, show_img, 1, 0) show_img_og = cv2.addWeighted(overlay, alpha, show_img_og, 1, 0) if np.sum(overlay) > 0.0: pass # cv2.imwrite(args.segmentation_path + "labels/GTLabels/" + path.format(i).replace('/', '\\'), distances) cv2.imshow('res', show_img_og) cv2.waitKey(0)
def main(): args = parser.parse_args() device = torch.device("cuda") if torch.cuda.is_available() else torch.device("cpu") if args.gt_type == 'KITTI': from kitti_eval.pose_evaluation_utils import test_framework_KITTI as test_framework elif args.gt_type == 'stillbox': from stillbox_eval.pose_evaluation_utils import test_framework_stillbox as test_framework weights = torch.load(args.pretrained_posenet) seq_length = int(weights['state_dict']['conv1.0.weight'].size(1)/3) pose_net = PoseNet(seq_length=seq_length).to(device) pose_net.load_state_dict(weights['state_dict'], strict=False) dataset_dir = Path(args.dataset_dir) framework = test_framework(dataset_dir, args.sequences, seq_length) print('{} snippets to test'.format(len(framework))) errors = np.zeros((len(framework), 2), np.float32) if args.output_dir is not None: output_dir = Path(args.output_dir) output_dir.makedirs_p() predictions_array = np.zeros((len(framework), seq_length, 3, 4)) for j, sample in enumerate(tqdm(framework)): imgs = sample['imgs'] h,w,_ = imgs[0].shape if (not args.no_resize) and (h != args.img_height or w != args.img_width): imgs = [imresize(img, (args.img_height, args.img_width)).astype(np.float32) for img in imgs] imgs = [torch.from_numpy(np.transpose(img, (2,0,1))) for img in imgs] imgs = torch.stack(imgs).unsqueeze(0).to(device) imgs = 2*(imgs/255 - 0.5) poses = pose_net(imgs) inv_transform_matrices = pose_vec2mat(poses, rotation_mode=args.rotation_mode) transform_matrices = invert_mat(inv_transform_matrices) # rot_matrices = np.linalg.inv(inv_transform_matrices[:,:,:3]) # tr_vectors = rot_matrices @ inv_transform_matrices[:,:,-1:] # transform_matrices = np.concatenate([rot_matrices, tr_vectors], axis=-1) # first_transform = transform_matrices[0] # final_poses = np.linalg.inv(first_transform[:,:3]) @ transform_matrices # final_poses[:,:,-1:] -= np.linalg.inv(first_transform[:,:3]) @ first_transform[:,-1:] final_poses = compensate_pose(transform_matrices, transform_matrices[:,0])[0].cpu().numpy() if args.output_dir is not None: predictions_array[j] = final_poses ATE, RE = compute_pose_error(sample['poses'][1:], final_poses[1:]) errors[j] = ATE, RE mean_errors = errors.mean(0) std_errors = errors.std(0) error_names = ['ATE','RE'] print('') print("Results") print("\t {:>10}, {:>10}".format(*error_names)) print("mean \t {:10.4f}, {:10.4f}".format(*mean_errors)) print("std \t {:10.4f}, {:10.4f}".format(*std_errors)) if args.output_dir is not None: np.save(output_dir/'predictions.npy', predictions_array)
def main(): args = parser.parse_args() from kitti_eval.pose_evaluation_utils import test_framework_KITTI as test_framework weights = torch.load(args.pretrained_posenet) seq_length = int(weights['state_dict']['conv1.0.weight'].size(1) / 3) pose_net = PoseExpNet(nb_ref_imgs=seq_length - 1, output_exp=False).to(device) pose_net.load_state_dict(weights['state_dict'], strict=False) dataset_dir = Path(args.dataset_dir) framework = test_framework(dataset_dir, args.sequences, seq_length) print('{} snippets to test'.format(len(framework))) errors = np.zeros((len(framework), 2), np.float32) if args.output_dir is not None: output_dir = Path(args.output_dir) output_dir.makedirs_p() predictions_array = np.zeros((len(framework), seq_length, 3, 4)) for j, sample in enumerate(tqdm(framework)): imgs = sample['imgs'] h, w, _ = imgs[0].shape if (not args.no_resize) and (h != args.img_height or w != args.img_width): imgs = [ imresize(img, (args.img_height, args.img_width)).astype(np.float32) for img in imgs ] imgs = [np.transpose(img, (2, 0, 1)) for img in imgs] ref_imgs = [] for i, img in enumerate(imgs): img = torch.from_numpy(img).unsqueeze(0) img = ((img / 255 - 0.5) / 0.5).to(device) if i == len(imgs) // 2: tgt_img = img else: ref_imgs.append(img) _, poses = pose_net(tgt_img, ref_imgs) poses = poses.cpu()[0] poses = torch.cat([ poses[:len(imgs) // 2], torch.zeros(1, 6).float(), poses[len(imgs) // 2:] ]) inv_transform_matrices = pose_vec2mat( poses, rotation_mode=args.rotation_mode).numpy().astype(np.float64) rot_matrices = np.linalg.inv(inv_transform_matrices[:, :, :3]) tr_vectors = -rot_matrices @ inv_transform_matrices[:, :, -1:] transform_matrices = np.concatenate([rot_matrices, tr_vectors], axis=-1) first_inv_transform = inv_transform_matrices[0] final_poses = first_inv_transform[:, :3] @ transform_matrices final_poses[:, :, -1:] += first_inv_transform[:, -1:] if args.output_dir is not None: predictions_array[j] = final_poses ATE, RE = compute_pose_error(sample['poses'], final_poses) errors[j] = ATE, RE mean_errors = errors.mean(0) std_errors = errors.std(0) error_names = ['ATE', 'RE'] print('') print("Results") print("\t {:>10}, {:>10}".format(*error_names)) print("mean \t {:10.4f}, {:10.4f}".format(*mean_errors)) print("std \t {:10.4f}, {:10.4f}".format(*std_errors)) if args.output_dir is not None: np.save(output_dir / 'predictions.npy', predictions_array)
def main(): args = parser.parse_args() from kitti_eval.pose_evaluation_utils import test_framework_KITTI as test_framework #net init weights = torch.load(args.pretrained_posenet)#权重参数载入,return orderedDict seq_length = int(weights['state_dict']['conv1.0.weight'].size(1)/3) #conv1.0.weight .shape = (16,15,7,7),这里注意哈, 网络结构定义的是 #in_planse = 15, out_plans = 16, kernel_size =7,7 #但是这里dict存储的时候就是反的???与conv定义前两个是颠倒的!!! #seq_lenth ==5由于模型如此 pose_net = PoseExpNet(nb_ref_imgs=seq_length - 1, output_exp=False).to(device) pose_net.load_state_dict(weights['state_dict'], strict=False)#载入模型参数 dataset_dir = Path(args.dataset_dir) framework = test_framework(dataset_dir, args.sequences, seq_length) print('{} snippets to test'.format(len(framework))) errors = np.zeros((len(framework), 2), np.float32) if args.output_dir is not None: output_dir = Path(args.output_dir) output_dir.makedirs_p() predictions_array = np.zeros((len(framework), seq_length, 3, 4)) # main cycle for j, sample in enumerate(tqdm(framework)) :#j from 0~1591#tqdm(obj)调用__iter__ if j>100: break; imgs = sample['imgs']#[375,1242,3] h,w,_ = imgs[0].shape if (not args.no_resize) and (h != args.img_height or w != args.img_width): imgs = [imresize(img, (args.img_height, args.img_width)).astype(np.float32) for img in imgs] #[128,416,3] imgs = [np.transpose(img, (2,0,1)) for img in imgs]#[3,128,416],201 通道提前 ref_imgs = [] for i, img in enumerate(imgs): img = torch.from_numpy(img).unsqueeze(0) img = ((img/255 - 0.5)/0.5).to(device) if i == len(imgs)//2: tgt_img = img else: ref_imgs.append(img) #pose predict #tgt_img size [1,3,h,w] #ref :list of [1,3,h,w], lenth 5 _, poses = pose_net(tgt_img, ref_imgs)#return exp_mask,pose,# # 这里的1是因为在训练的时候需要batch_size输入,但其他时候不需要 #pose tensorsize =( 1,num_ref_imgs(4),6) poses = poses.cpu()[0]#(4,6) poses = torch.cat([poses[:len(imgs)//2], torch.zeros(1,6).float(), poses[len(imgs)//2:]])#中间插入全0, 代表关键帧 #相对于自己自运动为0,[5,6] inv_transform_matrices = pose_vec2mat(poses, rotation_mode=args.rotation_mode).numpy().astype(np.float64) #shape = 5,3,4 rot_matrices = np.linalg.inv(inv_transform_matrices[:,:,:3]) tr_vectors = -rot_matrices @ inv_transform_matrices[:,:,-1:] transform_matrices = np.concatenate([rot_matrices, tr_vectors], axis=-1) first_inv_transform = inv_transform_matrices[0] final_poses = first_inv_transform[:,:3] @ transform_matrices final_poses[:,:,-1:] += first_inv_transform[:,-1:]#5,3,4 if args.output_dir is not None:#forwad pass 结果记录一下,留着输出 predictions_array[j] = final_poses ATE, RE = compute_pose_error(sample['poses'], final_poses) errors[j] = ATE, RE mean_errors = errors.mean(0) std_errors = errors.std(0) error_names = ['ATE','RE'] print('') print("Results") print("\t {:>10}, {:>10}".format(*error_names)) print("mean \t {:10.4f}, {:10.4f}".format(*mean_errors)) print("std \t {:10.4f}, {:10.4f}".format(*std_errors)) if args.output_dir is not None: np.save(output_dir/'predictions.npy', predictions_array)
def validate_with_gt_pose(args, val_loader, disp_net, pose_exp_net, epoch, logger, tb_writer, sample_nb_to_log=3): global device batch_time = AverageMeter() depth_error_names = ['abs_diff', 'abs_rel', 'sq_rel', 'a1', 'a2', 'a3'] depth_errors = AverageMeter(i=len(depth_error_names), precision=4) pose_error_names = ['ATE', 'RTE'] pose_errors = AverageMeter(i=2, precision=4) log_outputs = sample_nb_to_log > 0 # Output the logs throughout the whole dataset batches_to_log = list( np.linspace(0, len(val_loader), sample_nb_to_log).astype(int)) poses_values = np.zeros( ((len(val_loader) - 1) * args.batch_size * (args.sequence_length - 1), 6)) disp_values = np.zeros(((len(val_loader) - 1) * args.batch_size * 3)) # switch to evaluate mode disp_net.eval() pose_exp_net.eval() end = time.time() logger.valid_bar.update(0) for i, (tgt_img, ref_imgs, gt_depth, gt_poses) in enumerate(val_loader): tgt_img = tgt_img.to(device) gt_depth = gt_depth.to(device) gt_poses = gt_poses.to(device) ref_imgs = [img.to(device) for img in ref_imgs] b = tgt_img.shape[0] # compute output output_disp = disp_net(tgt_img) output_depth = 1 / output_disp explainability_mask, output_poses = pose_exp_net(tgt_img, ref_imgs) reordered_output_poses = torch.cat([ output_poses[:, :gt_poses.shape[1] // 2], torch.zeros(b, 1, 6).to(output_poses), output_poses[:, gt_poses.shape[1] // 2:] ], dim=1) # pose_vec2mat only takes B, 6 tensors, so we simulate a batch dimension of B * seq_length unravelled_poses = reordered_output_poses.reshape(-1, 6) unravelled_matrices = pose_vec2mat(unravelled_poses, rotation_mode=args.rotation_mode) inv_transform_matrices = unravelled_matrices.reshape(b, -1, 3, 4) rot_matrices = inv_transform_matrices[..., :3].transpose(-2, -1) tr_vectors = -rot_matrices @ inv_transform_matrices[..., -1:] transform_matrices = torch.cat([rot_matrices, tr_vectors], axis=-1) first_inv_transform = inv_transform_matrices.reshape(b, -1, 3, 4)[:, :1] final_poses = first_inv_transform[..., :3] @ transform_matrices final_poses[..., -1:] += first_inv_transform[..., -1:] final_poses = final_poses.reshape(b, -1, 3, 4) if log_outputs and i in batches_to_log: # log first output of wanted batches index = batches_to_log.index(i) if epoch == 0: for j, ref in enumerate(ref_imgs): tb_writer.add_image('val Input {}/{}'.format(j, index), tensor2array(tgt_img[0]), 0) tb_writer.add_image('val Input {}/{}'.format(j, index), tensor2array(ref[0]), 1) log_output_tensorboard(tb_writer, 'val', index, '', epoch, output_depth, output_disp, None, None, explainability_mask) if log_outputs and i < len(val_loader) - 1: step = args.batch_size * (args.sequence_length - 1) poses_values[i * step:(i + 1) * step] = output_poses.cpu().view( -1, 6).numpy() step = args.batch_size * 3 disp_unraveled = output_disp.cpu().view(args.batch_size, -1) disp_values[i * step:(i + 1) * step] = torch.cat([ disp_unraveled.min(-1)[0], disp_unraveled.median(-1)[0], disp_unraveled.max(-1)[0] ]).numpy() depth_errors.update(compute_depth_errors(gt_depth, output_depth[:, 0])) pose_errors.update(compute_pose_errors(gt_poses, final_poses)) # measure elapsed time batch_time.update(time.time() - end) end = time.time() logger.valid_bar.update(i + 1) if i % args.print_freq == 0: logger.valid_writer.write( 'valid: Time {} Abs Error {:.4f} ({:.4f}), ATE {:.4f} ({:.4f})' .format(batch_time, depth_errors.val[0], depth_errors.avg[0], pose_errors.val[0], pose_errors.avg[0])) if log_outputs: prefix = 'valid poses' coeffs_names = ['tx', 'ty', 'tz'] if args.rotation_mode == 'euler': coeffs_names.extend(['rx', 'ry', 'rz']) elif args.rotation_mode == 'quat': coeffs_names.extend(['qx', 'qy', 'qz']) for i in range(poses_values.shape[1]): tb_writer.add_histogram('{} {}'.format(prefix, coeffs_names[i]), poses_values[:, i], epoch) tb_writer.add_histogram('disp_values', disp_values, epoch) logger.valid_bar.update(len(val_loader)) return depth_errors.avg + pose_errors.avg, depth_error_names + pose_error_names
def validate_without_gt(args, val_loader, disp_net, pose_exp_net, epoch, logger, tb_writer, sample_nb_to_log=3): global device batch_time = AverageMeter() losses = AverageMeter(i=3, precision=4) log_outputs = sample_nb_to_log > 0 w1, w2, w3 = args.photo_loss_weight, args.mask_loss_weight, args.smooth_loss_weight poses = np.zeros( ((len(val_loader) - 1) * args.batch_size * (args.sequence_length - 1), 6)) disp_values = np.zeros(((len(val_loader) - 1) * args.batch_size * 3)) # switch to evaluate mode disp_net.eval() pose_exp_net.eval() end = time.time() logger.valid_bar.update(0) for i, (tgt_img, ref_imgs, intrinsics, intrinsics_inv) in enumerate(val_loader): tgt_img = tgt_img.to(device) ref_imgs = [img.to(device) for img in ref_imgs] intrinsics = intrinsics.to(device) intrinsics_inv = intrinsics_inv.to(device) # compute output disp = disp_net(tgt_img) depth = 1 / disp explainability_mask, pose = pose_exp_net(tgt_img, ref_imgs) loss_1, warped, diff = photometric_reconstruction_loss( tgt_img, ref_imgs, intrinsics, depth, explainability_mask, pose, args.rotation_mode, args.padding_mode) loss_1 = loss_1.item() if w2 > 0: loss_2 = explainability_loss(explainability_mask).item() else: loss_2 = 0 loss_3 = smooth_loss(depth).item() if log_outputs and i < sample_nb_to_log - 1: # log first output of first batches if epoch == 0: for j, ref in enumerate(ref_imgs): tb_writer.add_image('val Input {}/{}'.format(j, i), tensor2array(tgt_img[0]), 0) tb_writer.add_image('val Input {}/{}'.format(j, i), tensor2array(ref[0]), 1) log_output_tensorboard(tb_writer, 'val', i, '', epoch, 1. / disp, disp, warped[0], diff[0], explainability_mask) if log_outputs and i < len(val_loader) - 1: step = args.batch_size * (args.sequence_length - 1) poses[i * step:(i + 1) * step] = pose.cpu().view(-1, 6).numpy() step = args.batch_size * 3 disp_unraveled = disp.cpu().view(args.batch_size, -1) disp_values[i * step:(i + 1) * step] = torch.cat([ disp_unraveled.min(-1)[0], disp_unraveled.median(-1)[0], disp_unraveled.max(-1)[0] ]).numpy() loss = w1 * loss_1 + w2 * loss_2 + w3 * loss_3 if args.with_photocon_loss: batch_size = pose.size()[0] homo_row = torch.tensor([[0, 0, 0, 1]], dtype=torch.float).to(device) homo_row = homo_row.unsqueeze(0).expand(batch_size, -1, -1) T21 = pose_vec2mat(pose[:, 0]) T21 = torch.cat((T21, homo_row), 1) T12 = torch.inverse(T21) T23 = pose_vec2mat(pose[:, 1]) T23 = torch.cat((T23, homo_row), 1) T13 = torch.matmul(T23, T12) #[B,4,4] # print("----",T13.size()) # target = 1(ref_imgs[0]) and ref = 3(ref_imgs[1]) ref_img_warped, valid_points = inverse_warp_posemat( ref_imgs[1], depth[:, 0], T13, intrinsics, args.rotation_mode, args.padding_mode) diff = (ref_imgs[0] - ref_img_warped) * valid_points.unsqueeze(1).float() loss_4 = diff.abs().mean() loss += loss_4 losses.update([loss, loss_1, loss_2]) # measure elapsed time batch_time.update(time.time() - end) end = time.time() logger.valid_bar.update(i + 1) if i % args.print_freq == 0: logger.valid_writer.write('valid: Time {} Loss {}'.format( batch_time, losses)) if log_outputs: prefix = 'valid poses' coeffs_names = ['tx', 'ty', 'tz'] if args.rotation_mode == 'euler': coeffs_names.extend(['rx', 'ry', 'rz']) elif args.rotation_mode == 'quat': coeffs_names.extend(['qx', 'qy', 'qz']) for i in range(poses.shape[1]): tb_writer.add_histogram('{} {}'.format(prefix, coeffs_names[i]), poses[:, i], epoch) tb_writer.add_histogram('disp_values', disp_values, epoch) logger.valid_bar.update(len(val_loader)) return losses.avg, [ 'Validation Total loss', 'Validation Photo loss', 'Validation Exp loss' ]
def main(): global best_error, worst_error device = torch.device("cuda") if torch.cuda.is_available() else torch.device("cpu") args = parser.parse_args() if args.gt_type == 'KITTI': from kitti_eval.depth_evaluation_utils import test_framework_KITTI as test_framework elif args.gt_type == 'stillbox': from stillbox_eval.depth_evaluation_utils import test_framework_stillbox as test_framework weights = torch.load(args.pretrained_depthnet) depth_net = DepthNet(depth_activation="elu", batch_norm='bn' in weights.keys() and weights['bn']).to(device) depth_net.load_state_dict(weights['state_dict']) depth_net.eval() if args.pretrained_posenet is None: args.stabilize_from_GT = True print('no PoseNet specified, stab will be done from ground truth') seq_length = 5 else: weights = torch.load(args.pretrained_posenet) seq_length = int(weights['state_dict']['conv1.0.weight'].size(1)/3) pose_net = PoseNet(seq_length=seq_length).to(device) pose_net.load_state_dict(weights['state_dict'], strict=False) dataset_dir = Path(args.dataset_dir) if args.dataset_list is not None: with open(args.dataset_list, 'r') as f: test_files = list(f.read().splitlines()) else: test_files = [file.relpathto(dataset_dir) for file in sum([dataset_dir.files('*.{}'.format(ext)) for ext in args.img_exts], [])] framework = test_framework(dataset_dir, test_files, seq_length, args.min_depth, args.max_depth) print('{} files to test'.format(len(test_files))) errors = np.zeros((7, len(test_files)), np.float32) args.output_dir = Path(args.output_dir) args.output_dir.makedirs_p() for j, sample in enumerate(tqdm(framework)): imgs = sample['imgs'] intrinsics = sample['intrinsics'].copy() h,w,_ = imgs[0].shape if (not args.no_resize) and (h != args.img_height or w != args.img_width): imgs = [imresize(img, (args.img_height, args.img_width)).astype(np.float32) for img in imgs] intrinsics[0] *= args.img_width/w intrinsics[1] *= args.img_height/h intrinsics_inv = np.linalg.inv(intrinsics) intrinsics = torch.from_numpy(intrinsics).unsqueeze(0).to(device) intrinsics_inv = torch.from_numpy(intrinsics_inv).unsqueeze(0).to(device) imgs = [torch.from_numpy(np.transpose(img, (2,0,1))) for img in imgs] imgs = torch.stack(imgs).unsqueeze(0).to(device) imgs = 2*(imgs/255 - 0.5) tgt_img = imgs[:,sample['tgt_index']] # Construct a batch of all possible stabilized pairs, with PoseNet or with GT orientation, will take the output closest to target mean depth if args.stabilize_from_GT: poses_GT = Variable(torch.from_numpy(sample['poses']).cuda()).unsqueeze(0) inv_poses_GT = invert_mat(poses_GT) tgt_pose = inv_poses_GT[:,sample['tgt_index']] inv_transform_matrices_tgt = compensate_pose(inv_poses_GT, tgt_pose) else: poses = pose_net(imgs) inv_transform_matrices = pose_vec2mat(poses, rotation_mode=args.rotation_mode) tgt_pose = inv_transform_matrices[:,sample['tgt_index']] inv_transform_matrices_tgt = compensate_pose(inv_transform_matrices, tgt_pose) stabilized_pairs = [] corresponding_displ = [] for i in range(seq_length): if i == sample['tgt_index']: continue img = imgs[:,i] img_pose = inv_transform_matrices_tgt[:,i] stab_img = inverse_rotate(img, img_pose[:,:,:3], intrinsics, intrinsics_inv) pair = torch.cat([stab_img, tgt_img], dim=1) # [1, 6, H, W] stabilized_pairs.append(pair) GT_translations = sample['poses'][:,:,-1] real_displacement = np.linalg.norm(GT_translations[sample['tgt_index']] - GT_translations[i]) corresponding_displ.append(real_displacement) stab_batch = torch.cat(stabilized_pairs) # [seq, 6, H, W] depth_maps = depth_net(stab_batch) # [seq, 1 , H/4, W/4] selected_depth, selected_index = select_best_map(depth_maps, target_mean_depthnet_output) pred_depth = selected_depth.cpu().data.numpy() * corresponding_displ[selected_index] / args.nominal_displacement if args.save_output: if j == 0: predictions = np.zeros((len(test_files), *pred_depth.shape)) predictions[j] = 1/pred_depth gt_depth = sample['gt_depth'] pred_depth_zoomed = zoom(pred_depth, (gt_depth.shape[0]/pred_depth.shape[0], gt_depth.shape[1]/pred_depth.shape[1]) ).clip(args.min_depth, args.max_depth) if sample['mask'] is not None: pred_depth_zoomed_masked = pred_depth_zoomed[sample['mask']] gt_depth = gt_depth[sample['mask']] errors[:,j] = compute_errors(gt_depth, pred_depth_zoomed_masked) if args.log_best_worst: if best_error > errors[0,j]: best_error = errors[0,j] log_result(pred_depth_zoomed, sample['gt_depth'], stab_batch, selected_index, args.output_dir, 'best') if worst_error < errors[0,j]: worst_error = errors[0,j] log_result(pred_depth_zoomed, sample['gt_depth'], stab_batch, selected_index, args.output_dir, 'worst') mean_errors = errors.mean(1) error_names = ['abs_rel','sq_rel','rms','log_rms','a1','a2','a3'] print("Results : ") print("{:>10}, {:>10}, {:>10}, {:>10}, {:>10}, {:>10}, {:>10}".format(*error_names)) print("{:10.4f}, {:10.4f}, {:10.4f}, {:10.4f}, {:10.4f}, {:10.4f}, {:10.4f}".format(*mean_errors)) if args.save_output: np.save(args.output_dir/'predictions.npy', predictions)
def compose_transforms(self, frames, translation_scale): print(self.camera_matrix) global_pose = np.identity(4) poses = [global_pose[0:3, :].reshape(1, 12)] img1 = frames[0] tensor_img1 = self.preprocess_img(img1) for i in tqdm(range(len(frames) - 1)): img2 = frames[i + 1] tensor_img2 = self.preprocess_img(img2) pose = self.pose_net(tensor_img1, tensor_img2) pose_mat = pose_vec2mat(pose).squeeze(0).cpu().detach().numpy() pose_mat = np.vstack([pose_mat, np.array([0, 0, 0, 1])]) global_pose = global_pose @ np.linalg.inv(pose_mat) poses.append(global_pose[0:3, :].reshape(1, 12)) # update tensor_img1 = tensor_img2 transforms = np.concatenate(poses, axis=0) #concatenate all transforms into a np array pose = np.array(transforms).reshape((len(transforms), 3, 4)) x = np.zeros((len(pose), 1, 4)) x[:, :, -1] = 1 # add the last row to the pose arrays pose = np.concatenate([pose, x], axis=1) # scale the translations pose[:, :, -1] = translation_scale * pose[:, :, -1] # this is with the ground truth pose # df_gt = pd.read_csv("/HDD1_2TB/storage/KITTI/data_odometry_color/dataset/poses/09.txt", sep=" ", header=None) # pose_gt = df_gt.values.reshape((len(df_gt), 3, 4)) # # x_gt = np.zeros((len(pose_gt), 1, 4)) # # x_gt[:, :, -1] = 1 # # pose_gt = np.concatenate([pose_gt, x_gt], axis=1) # compute the relative pose between the first pose and every following one crt_pose = np.stack(inv(pose[0]).dot(x) for x in pose[0:]) rvec = np.array([0., 0., 0]) tvec = np.array([0., 0., 0]) rvec, _ = cv2.Rodrigues(rvec) # the _l and _r points correspond to the left and right wheels # homogeneous point [x, y, z, w] corresponds to the three-dimensional point [x/w, y/w, z/w]. project_points = np.array([[0, 1.7, 3, 1]]).reshape(1, 1, 4) # homogeneous point [x, y, z, w] corresponds to the three-dimensional point [x/w, y/w, z/w]. project_points_l = np.array([[-0.8, 1.7, 3, 1]]).reshape(1, 1, 4) # homogeneous point [x, y, z, w] corresponds to the three-dimensional point [x/w, y/w, z/w]. project_points_r = np.array([[+0.8, 1.7, 3, 1]]).reshape(1, 1, 4) world_points = project_points.dot(crt_pose.transpose((0, 2, 1)))[0, 0] world_points_l = project_points_l.dot(crt_pose.transpose((0, 2, 1)))[0, 0] world_points_r = project_points_r.dot(crt_pose.transpose((0, 2, 1)))[0, 0] # show points are for displaying the world points on a frame world_points_show = np.concatenate([ world_points[:, :3], world_points_l[:, :3], world_points_r[:, :3] ]) rvec2 = crt_pose[0][:3, :3] # it is almost the identity matrix show_points = cv2.projectPoints(world_points_show.astype(np.float64), rvec2, tvec, self.camera_matrix, None)[0] show_points_l = cv2.projectPoints( world_points_l[:, :3].astype(np.float64), rvec2, tvec, self.camera_matrix, None)[0] show_points_r = cv2.projectPoints( world_points_r[:, :3].astype(np.float64), rvec2, tvec, self.camera_matrix, None)[0] show_points = show_points.astype(np.int)[:, 0] show_points_l = show_points_l.astype(np.int)[:, 0] show_points_r = show_points_r.astype(np.int)[:, 0] img1 = imresize(frames[0], (self.img_height, self.img_width)).astype(np.float32) overlay = np.zeros_like(img1) for it, p1, p2, p3, p4 in zip(range(len(show_points_l) - 1), show_points_l[:-1], show_points_r[:-1], show_points_l[1:], show_points_r[1:]): x1, y1 = p1 x2, y2 = p2 x3, y3 = p3 x4, y4 = p4 pts = np.array([(x1, y1), (x3, y3), (x4, y4), (x2, y2)]) overlay = cv2.drawContours(overlay, [pts], 0, (0, 255, 0), cv2.FILLED) alpha = 1.0 show_img = cv2.addWeighted(overlay, alpha, img1 / 255.0, 1, 0) cv2.imshow('path', show_img) cv2.waitKey(0) return world_points, world_points_l, world_points_r