def main():
output_file = Path(args.output_file)
tracker_file = Path(args.tracker_file)
pretrained_weights = Path(args.pretrained_weights)
dataset_dir = Path(args.dataset_dir)
# Load pretrained model
weights = torch.load(pretrained_weights, 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)
# Set Kitti framework for sequence number 09 with 5-snippet samples.
from kitti_eval.pose_evaluation_utils import test_framework_KITTI as test_framework
framework = test_framework(dataset_dir, ['09'], 5)
attacker = Attacker(framework, pose_net)
noise_mask = np.load(tracker_file)
pertubation = attacker.generate(noise_mask, 691, 731)
np.save(output_file, pertubation)
print('Attacked!')
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():
global tgt_img, disp_net
args = parser.parse_args()
'''加载训练后的模型'''
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)
# 网络模型的MD5 ID
net_ID = MD5_ID(args.pretrained_posenet)
# L和C的转换矩阵,对齐输入位姿到雷达坐标系
Transform_matrix_L2C = np.identity(4)
'''Kitti switch'''
if args.isKitti:
if not args.isDynamic:
from kitti_eval.pose_evaluation_utils import test_framework_KITTI as test_framework
else:
from kitti_eval.pose_evaluation_utils_forDynamicTest import test_framework_KITTI as test_framework
save_dir = os.path.join(args.output_dir, "kitti", args.sequences[0],
'net_' + net_ID)
if args.trainedOnMydataset:
downsample_img_height = args.img_height
downsample_img_width = args.img_width
else:
# on kitti train set
downsample_img_height = 128
downsample_img_width = 416
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)
else:
from mydataset_eval.pose_evaluation_utils import test_framework_MYDATASET as test_framework
save_dir = os.path.join(args.output_dir, "mydataset",
args.sequences[0], 'net_' + net_ID)
if args.trainedOnMydataset:
downsample_img_height = args.img_height
downsample_img_width = args.img_width
else:
# on kitti train set
downsample_img_height = 128
downsample_img_width = 416
Transform_matrix_L2C[:3, :3] = np.array(
[[-1.51482698e-02, -9.99886648e-01, 5.36310553e-03],
[-4.65337018e-03, -5.36307196e-03, -9.99969412e-01],
[9.99870070e-01, -1.56647995e-02, -4.48880010e-03]])
Transform_matrix_L2C[:3, -1:] = np.array(
[4.29029924e-03, -6.08539196e-02, -9.20346161e-02]).reshape(3, 1)
Transform_matrix_L2C = GramSchmidtHelper(Transform_matrix_L2C)
Transform_matrix_C2L = np.linalg.inv(Transform_matrix_L2C)
# *************************可删除*********************************
# 为了进行动态场景下的Mask评估，这里需要引入disp net
if args.isDynamic:
from models import DispNetS
disp_net = DispNetS().to(device)
weights = torch.load(args.pretrained_dispnet)
disp_net.load_state_dict(weights['state_dict'])
disp_net.eval()
# normalize = custom_transforms.Normalize(mean=[0.5, 0.5, 0.5],
# std=[0.5, 0.5, 0.5])
# valid_transform = custom_transforms.Compose([custom_transforms.ArrayToTensor(), normalize])
# from datasets.sequence_folders import SequenceFolder
# val_set = SequenceFolder(
# '/home/sda/mydataset/preprocessing/formatted/data/',
# transform=valid_transform,
# seed=0,
# train=False,
# sequence_length=3,
# )
# val_loader = torch.utils.data.DataLoader(
# val_set, batch_size=1, shuffle=False,
# num_workers=4, pin_memory=True)
#
# intrinsics = None
# for i, (tgt_img, ref_imgs, intrinsics, intrinsics_inv) in enumerate(val_loader):
# intrinsics = intrinsics.to(device)
# break
# *************************************************************************
'''载入测试数据集'''
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)
'''输出到文件夹中的数据'''
num_poses = len(framework) - (seq_length - 2)
predictions_array = np.zeros((len(framework), seq_length, 3, 4))
processing_time = np.zeros((num_poses - 1, 1))
# 输出文件夹
save_dir = Path(save_dir)
print('Output files wiil be saved in： ' + save_dir)
if not os.path.exists(save_dir): save_dir.makedirs_p()
# Pose Graph Manager (for back-end optimization) initialization
PGM = PoseGraphManager()
PGM.addPriorFactor()
# Result saver
num_frames = len(framework)
ResultSaver = PoseGraphResultSaver(init_pose=PGM.curr_se3,
save_gap=args.save_gap,
num_frames=num_frames,
seq_idx=args.sequences[0],
save_dir=save_dir)
# for save the results as a video
fig_idx = 1
fig = plt.figure(fig_idx)
writer = FFMpegWriter(fps=15)
video_path = save_dir + '/' + args.sequences[0] + ".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_path,
num_frames_to_save): # this video saving part is optional
for j, sample in enumerate(tqdm(framework)):
'''
VO部分
'''
imgs = sample['imgs']
w, h = imgs[0].size
if (not args.no_resize) and (h != downsample_img_height
or w != downsample_img_width):
imgs = [
imresize(img, (downsample_img_height,
downsample_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)
processing_time[j] = (time.time() - startTimeVO) / (seq_length - 1)
# **************************可删除********************************
if args.isDynamic:
'''测试Photo mask的效果'''
if args.isKitti:
intrinsics = [[
2.416744631239935472e+02, 0.000000000000000000e+00,
2.041680103059581199e+02
],
[
0.000000000000000000e+00,
2.462848682666666491e+02,
5.900083200000000261e+01
],
[
0.000000000000000000e+00,
0.000000000000000000e+00,
1.000000000000000000e+00
]]
else:
intrinsics = [[279.1911, 0.0000, 210.8265],
[0.0000, 279.3980, 172.3114],
[0.0000, 0.0000, 1.0000]]
PhotoMask_Output(_, disp_net, intrinsics, j, poses, ref_imgs,
save_dir)
# ***************************************************************
final_poses = pose2tf_mat(args.rotation_mode, imgs, poses)
predictions_array[j] = final_poses
# rel_VO_pose取final poses的第2项，整体则是取T10,T21,T32。。。
rel_VO_pose = np.identity(4)
rel_VO_pose[:3, :] = final_poses[1]
# 引入尺度因子对单目VO输出的位姿进行修正,并进行坐标系对齐到雷达坐标系
scale_factor = 7
rel_VO_pose[:3, -1:] = rel_VO_pose[:3, -1:] * scale_factor
rel_VO_pose = Transform_matrix_C2L @ rel_VO_pose @ np.linalg.inv(
Transform_matrix_C2L)
rel_VO_pose = GramSchmidtHelper(rel_VO_pose)
ResultSaver.saveRelativePose(rel_VO_pose)
PGM.curr_node_idx = j + 1
PGM.curr_se3 = np.matmul(PGM.curr_se3, rel_VO_pose)
PGM.addOdometryFactor(rel_VO_pose)
PGM.prev_node_idx = PGM.curr_node_idx
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.isKitti:
ATE, RE = compute_pose_error(sample['poses'], final_poses)
errors[j] = ATE, RE
'''save output files'''
if save_dir is not None:
# np.save(save_dir / 'predictions.npy', predictions_array)
ResultSaver.saveFinalPoseGraphResult(filename='abs_VO_poses.txt')
ResultSaver.saveRelativePosesResult(filename='rel_VO_poses.txt')
np.savetxt(save_dir / 'processing_time.txt', processing_time)
if args.isKitti:
np.savetxt(save_dir / 'errors.txt', errors)
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))
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()
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
#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 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 = 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)))
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))
global_pose = np.identity(4) #
pose_list = [global_pose[0:3, :].reshape(1, 12)]
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)
if j == 0:
poses = poses.cpu().data[0]
#print(poses.size()) #[2,6]
# poses = torch.cat([poses[:len(imgs)//2], torch.zeros(1,6).float(), poses[len(imgs)//2:]])
# print(poses.size()) #[3,6]
pose = poses[0, :].unsqueeze(0)
pose_mat = pose_vec2mat(pose).squeeze(0).cpu().numpy() #[B, 3, 4]
pose_mat = np.vstack([pose_mat, np.array([0, 0, 0, 1])])
global_pose = global_pose @ (pose_mat)
scale_factor = np.sum(
sample['poses'][0, :, -1] * pose_mat[0:3, -1]) / np.sum(
pose_mat[0:3, -1]**2)
global_pose = np.hstack(
[global_pose[:, :-1], scale_factor * global_pose[:, -1:]])
pose_list.append(global_pose[0:3, :].reshape(1, 12))
#------------------------------------------------------
pose = poses[1, :].unsqueeze(0)
pose_mat = pose_vec2mat(pose).squeeze(0).cpu().numpy() #[B, 3, 4]
pose_mat = np.vstack([pose_mat, np.array([0, 0, 0, 1])])
global_pose = global_pose @ np.linalg.inv(pose_mat)
pose_gt = np.vstack(
[sample['poses'][1, :, :],
np.array([0, 0, 0, 1])])
scale_factor = np.sum(
np.linalg.inv(pose_gt)[:3, -1] * pose_mat[0:3, -1]) / np.sum(
pose_mat[0:3, -1]**2)
global_pose = np.hstack(
[global_pose[:, :-1], scale_factor * global_pose[:, -1:]])
pose_list.append(global_pose[0:3, :].reshape(1, 12))
else:
poses = poses.cpu().data[0]
#print(poses.size()) #[2,6]
# poses = torch.cat([poses[:len(imgs)//2], torch.zeros(1,6).float(), poses[len(imgs)//2:]])
# print(poses.size()) #[3,6]
pose = poses[1, :].unsqueeze(0)
pose_mat = pose_vec2mat(pose).squeeze(0).cpu().numpy() #[B, 3, 4]
pose_mat = np.vstack([pose_mat, np.array([0, 0, 0, 1])])
global_pose = global_pose @ np.linalg.inv(pose_mat)
pose_gt = np.vstack(
[sample['poses'][1, :, :],
np.array([0, 0, 0, 1])])
scale_factor = np.sum(
np.linalg.inv(pose_gt)[:3, -1] * pose_mat[0:3, -1]) / np.sum(
pose_mat[0:3, -1]**2)
print(scale_factor)
global_pose = np.hstack(
[global_pose[:, :-1], scale_factor * global_pose[:, -1:]])
pose_list.append(global_pose[0:3, :].reshape(1, 12))
pose_list = np.concatenate(pose_list, axis=0)
filename = Path(args.output_dir + "09_pred" + ".txt")
np.savetxt(filename, pose_list, delimiter=' ', fmt='%1.8e')
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()
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)