def refine_registration_myownicp(source, target, trans_init, tolerance, max_iteration=50, num_icp_points=10000):
source_down = random_sampling(source, num_icp_points)
tarhet_down = random_sampling(target, num_icp_points)
rel_LO_pose, distacnces, iterations = icp(source_down, tarhet_down, init_pose=trans_init,
tolerance=tolerance,
max_iterations=max_iteration)
return rel_LO_pose
def updateMap(self,
curr_se3,
curr_local_ptcloud,
down_points=100,
submap_points=10000):
'''
创建并更新地图，正常情况下每一个位姿点都需要进行一次
:param curr_se3:
:param curr_local_ptcloud:
:param down_points:
:return:
'''
# 更新字段
self.curr_se3 = curr_se3
self.curr_local_ptcloud = curr_local_ptcloud
self.submap_points = submap_points
# 将点云坐标转化为齐次坐标（x,y,z）->(x,y,z,1)
self.curr_local_ptcloud = random_sampling(self.curr_local_ptcloud,
down_points)
tail = np.zeros((self.curr_local_ptcloud.shape[0], 1))
tail.fill(1)
self.curr_local_ptcloud = np.concatenate(
[self.curr_local_ptcloud, tail], axis=1)
self.ptcloud_list.append(self.curr_local_ptcloud)
curr_local_ptcloud_into_global_map = self.curr_se3 @ (
self.curr_local_ptcloud.T)
# concatenate the latest local pointcloud into global pointcloud
if (self.global_ptcloud is None):
self.global_ptcloud = curr_local_ptcloud_into_global_map.T[:, :3]
else:
self.global_ptcloud = np.concatenate([
self.global_ptcloud,
curr_local_ptcloud_into_global_map.T[:, :3]
])
# updata open3d_pointscloud
self.pointcloud.points = o3d.utility.Vector3dVector(
self.global_ptcloud)
# 子图相对位姿和点云维护
if self.sub_ptcloud_list.qsize() == self.k and (
not self.sub_ptcloud_list.empty()):
self.sub_ptcloud_list.get()
self.se3_list.get()
downsample_ptcloud = random_sampling(curr_local_ptcloud,
self.submap_points)
# 将点云坐标转化为齐次坐标（x,y,z）->(x,y,z,1)
tail = np.zeros((downsample_ptcloud.shape[0], 1))
tail.fill(1)
downsample_ptcloud = np.concatenate([downsample_ptcloud, tail], axis=1)
self.sub_ptcloud_list.put(downsample_ptcloud)
self.se3_list.put(curr_se3)
def getSubMap(self):
self.se3_to_curr_ptcloud_list = transform_to_curr_ptclound(
self.se3_list)
self.sub_ptcloud = integrateSubmap(self.sub_ptcloud_list,
self.se3_to_curr_ptcloud_list)
self.sub_ptcloud = random_sampling(self.sub_ptcloud,
self.submap_points)
return self.sub_ptcloud
def main():
from utils.UtilsMyData import test_framework_MyData 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)
ICP_iterations = np.zeros(len(framework))
VO_processing_time = np.zeros(len(framework))
ICP_iteration_time = 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))
'''绝对位姿列表初始化'''
# 对齐到雷达坐标系，VO模型输出的带有尺度的绝对位姿
abs_VO_poses = np.zeros((len(framework), 12))
# 对齐到雷达坐标系，LO模型输出的带有尺度的绝对位姿
abs_LO_poses = np.zeros((len(framework), 12))
# 位姿估计值，对齐到相机坐标系下，和真值直接比较（仅适用于有相机坐标系下的真值）
est_poses = np.zeros((len(framework), 12))
est_poses[0] = np.identity(4)[:3, :].reshape(-1, 12)[0]
'''帧间位姿列表初始化'''
# 对齐到相机坐标系，VO模型输出的带有尺度的帧间位姿
cur_VO_poses_C = np.zeros((len(framework), 12))
# 对齐到雷达坐标系，VO模型输出的带有尺度的帧间位姿
cur_VO_poses = np.zeros((len(framework), 6))
# 对齐到雷达坐标系，VO模型输出的带有尺度的帧间位姿
cur_LO_poses = np.zeros((len(framework), 12))
'''尺度因子'''
scale_factors = np.zeros((len(framework), 1))
abs_VO_pose = np.identity(4)[:3, :]
'''循环周期所需变量初始化'''
# 用来估计尺度因子
last_pose = np.identity(4)
last_gap2_pose = np.identity(4)
last_VO_pose = np.identity(4)
# 周期中涉及到的点云，当前，前一个，前前一个
curr_pts = None
last_pts = None
sec_last_pts = None
# 当前节点和前一个，前前一个节点的帧间预估
gap1_VO_pose = np.identity(4)
gap2_VO_pose = np.identity(4)
last_gap2_VO_pose = np.identity(4)
# L和C的转换矩阵,对齐输入位姿到雷达坐标系
Transform_matrix_L2C = np.identity(4)
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)
pointClouds = loadPointCloud(args.dataset_dir + "/sequences/" + args.sequence_idx + "/velodyne")
'''Pose Graph Manager (for back-end optimization) initialization'''
PGM = PoseGraphManager()
PGM.addPriorFactor()
num_frames = len(tqdm(framework))
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)
'''Mapping initialzation'''
if args.mapping is True:
Map = MappingManager()
# for save the result 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) + "_tolerance@" + str(
args.tolerance) + "_scm@" + str(args.scm_type) + "_thresh@" + str(args.loop_threshold) + ".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']
w, h = imgs[0].size
if (not args.no_resize) and (h != args.img_height or w != args.img_width):
imgs = [(np.array(img.resize((args.img_width, args.img_height)))).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)
VO_processing_time[j] = 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)))
# 将对[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:]
# gap2_VO_pose取final poses的第3项（0-2）
gap2_VO_pose[:3, :] = final_poses[2]
# cur_VO_pose取final poses的第2项（0-1）
gap1_VO_pose[:3, :] = final_poses[1]
# 尺度因子的确定：采用上一帧的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))
scale_factors[j] = scale_factor
last_VO_pose = copy.deepcopy(gap1_VO_pose) # 注意深拷贝
# 先尺度修正，再对齐坐标系，施密特正交化避免病态矩阵
gap2_VO_pose[:3, -1:] = gap2_VO_pose[:3, -1:] * scale_factor
gap2_VO_pose = Transform_matrix_C2L @ gap2_VO_pose @ np.linalg.inv(Transform_matrix_C2L)
gap2_VO_pose = GramSchmidtHelper(gap2_VO_pose)
gap1_VO_pose[:3, -1:] = gap1_VO_pose[:3, -1:] * scale_factor
cur_VO_poses_C[j] = gap1_VO_pose[:3, :].reshape(-1, 12)[0]
gap1_VO_pose = Transform_matrix_C2L @ gap1_VO_pose @ np.linalg.inv(Transform_matrix_C2L)
gap1_VO_pose = GramSchmidtHelper(gap1_VO_pose)
'''*************************LO部分******************************************'''
# 初始化
if j == 0:
last_pts = random_sampling(pointClouds[j], args.num_icp_points)
sec_last_pts = last_pts
SCM.addNode(j, last_pts)
if args.mapping is True:
Map.updateMap(curr_se3=PGM.curr_se3,curr_local_ptcloud=last_pts)
curr_pts = random_sampling(pointClouds[j + 1], args.num_icp_points)
from modules.ICP import icp
# 选择LO的初值预估，分别是无预估，上一帧位姿，VO位姿
if args.proposal == 0:
init_pose_1 = None
init_pose_2 = None
elif args.proposal == 1:
init_pose_1 = last_pose
init_pose_2 = last_gap2_pose
elif args.proposal == 2:
init_pose_1 = gap1_VO_pose
init_pose_2 = last_gap2_VO_pose
print("init_pose_1 ")
print(init_pose_1)
print("init_pose_2 ")
print(init_pose_2)
startTime = time.time()
icp_odom_transform_1, distacnces, iterations = icp(curr_pts, last_pts, init_pose=init_pose_1,
tolerance=args.tolerance,
max_iterations=50)
if j > 0:
icp_odom_transform_2, distacnces_2, iterations_2 = icp(last_pts, sec_last_pts, init_pose=init_pose_2,
tolerance=args.tolerance,
max_iterations=50)
else:
icp_odom_transform_2 = icp_odom_transform_1
distacnces_2 = distacnces
iterations_2 = iterations
ICP_iteration_time[j] = time.time() - startTime
ICP_iterations[j] = (iterations + iterations_2) / 2
print("last_pose ")
print(last_pose)
'''更新指针'''
# 点云
sec_last_pts = last_pts
last_pts = curr_pts
# gap2预估位姿
last_gap2_VO_pose = copy.deepcopy(gap2_VO_pose)
# icp 输出位姿
last_pose = icp_odom_transform_1
last_gap2_pose = icp_odom_transform_2
print("LO优化后的位姿,mean_dis: ", np.asarray(distacnces).mean())
print(icp_odom_transform_1)
print("icp_odom_transform_2")
print(icp_odom_transform_2)
'''Update loop detection nodes'''
SCM.addNode(j + 1, curr_pts)
'''Update the edges and nodes of pose graph'''
PGM.curr_node_idx = j + 1
PGM.curr_se3 = np.matmul(PGM.curr_se3, icp_odom_transform_1)
PGM.addOdometryFactor(icp_odom_transform_1, -1)
PGM.addOdometryFactor(icp_odom_transform_2, -2)
PGM.sec_prev_node_idx = PGM.prev_node_idx
PGM.prev_node_idx = PGM.curr_node_idx
# 建图更新
if args.mapping is True:
Map.updateMap(curr_se3=PGM.curr_se3,curr_local_ptcloud=curr_pts)
# 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(curr_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)
# 2-3/ updateMap
if args.mapping is True:
Map.optimizeGlobalMap(PGM.graph_optimized, PGM.curr_node_idx)
# 定时进行位姿图优化
if (PGM.curr_node_idx > 1 and PGM.curr_node_idx % args.optimization_period == 0):
# 2-2/ graph optimization
PGM.optimizePoseGraph()
# 2-2/ save optimized poses
ResultSaver.saveOptimizedPoseGraphResult(PGM.curr_node_idx, PGM.graph_optimized)
# 2-3/ updateMap
if args.mapping is True:
Map.optimizeGlobalMap(PGM.graph_optimized, PGM.curr_node_idx)
# 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.vizmapping is True:
Map.vizMapWithOpen3D()
# 对齐到雷达坐标系下，VO输出的绝对位姿
abs_VO_pose[:, :3] = gap1_VO_pose[:3, :3] @ abs_VO_pose[:, :3]
abs_VO_pose[:, -1:] += gap1_VO_pose[:3, -1:]
if args.output_dir is not None:
predictions_array[j] = final_poses
# cur_VO_poses[j]=cur_VO_pose[:3, :].reshape(-1, 12)[0]
cur_LO_poses[j] = icp_odom_transform_1[:3, :].reshape(-1, 12)[0]
abs_VO_poses[j] = abs_VO_pose[:3, :].reshape(-1, 12)[0]
abs_LO_poses[j] = PGM.curr_se3[:3, :].reshape(-1, 12)[0]
est_pose = Transform_matrix_L2C @ PGM.curr_se3 @ np.linalg.inv(Transform_matrix_L2C)
est_poses[j + 1] = est_pose[:3, :].reshape(-1, 12)[0]
if args.mapping is True:
Map.saveMap2File('map_' + args.sequence_idx + "_" + str(args.num_icp_points) + "_prop@" + str(
args.proposal) + "_tolerance@" + str(args.tolerance) + "_scm@" + str(args.scm_type) +
"_thresh@" + str(args.loop_threshold) + '.pcd')
if args.output_dir is not None:
# np.save(output_dir / 'predictions.npy', predictions_array)
np.savetxt(output_dir / 'scale_factors.txt', scale_factors)
np.savetxt(output_dir / 'cur_VO_poses_C.txt', cur_VO_poses_C)
np.savetxt(output_dir / 'cur_VO_poses.txt', cur_VO_poses)
np.savetxt(output_dir / 'abs_VO_poses.txt', abs_VO_poses)
np.savetxt(output_dir / 'cur_LO_poses.txt', cur_LO_poses)
np.savetxt(output_dir / 'abs_LO_poses.txt', abs_LO_poses)
np.savetxt(output_dir / 'iterations.txt', ICP_iterations)
np.savetxt(output_dir / 'est_kitti_{0}_poses.txt'.format(args.sequence_idx), est_poses)
# 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 = ICP_iterations.mean()
std_iterations = ICP_iterations.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 = ICP_iteration_time.mean()
std_iter_time = ICP_iteration_time.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))
def main():
'''加载训练后的模型'''
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:
from utils.UtilsKitti import test_framework_KITTI as test_framework
save_dir = os.path.join(args.output_dir, "kitti", args.sequence_idx,
'net_' + net_ID)
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 utils.UtilsMyData import test_framework_MyData as test_framework
save_dir = os.path.join(args.output_dir, "mydataset",
args.sequence_idx, 'net_' + net_ID)
downsample_img_height = args.img_height
downsample_img_width = args.img_width
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)
'''载入测试数据:图像和点云'''
dataset_dir = Path(args.dataset_dir)
sequences = [args.sequence_idx]
framework = test_framework(dataset_dir, sequences, seq_length)
pointcloud_dir = os.path.join(args.dataset_dir, 'sequences',
args.sequence_idx, 'velodyne')
pointClouds = loadPointCloud(pointcloud_dir)
print('{} snippets to test'.format(len(framework)))
'''误差初始化'''
errors = np.zeros((len(framework), 2), np.float32)
optimized_errors = np.zeros((len(framework), 2), np.float32)
##############################################################
'''输出到文件中的数据初始化'''
num_poses = len(framework) - (seq_length - 2)
'''效率'''
VO_processing_time = np.zeros(num_poses - 1)
ICP_iterations = np.zeros(num_poses - 1)
ICP_fitness = np.zeros(num_poses - 1)
ICP_iteration_time = np.zeros(num_poses - 1)
'''绝对位姿列表初始化'''
# 对齐到雷达坐标系，VO模型输出的带有尺度的绝对位姿
abs_VO_poses = np.zeros((num_poses, 12))
abs_VO_poses[0] = np.identity(4)[:3, :].reshape(12)
abs_VO_pose = np.identity(4)
# 位姿估计值，对齐到相机坐标系下，和真值直接比较（仅适用于有相机坐标系下的真值）
est_poses = np.zeros((num_poses, 12))
est_poses[0] = np.identity(4)[:3, :].reshape(12)
'''帧间位姿列表初始化'''
# 对齐到雷达坐标系，VO模型输出的带有尺度的帧间位姿
rel_VO_poses = np.zeros((num_poses - 1, 12))
'''尺度因子'''
scale_factors = np.zeros((num_poses - 1, 1))
# 用来估计尺度因子
last_rel_LO_pose = np.identity(4)
last_rel_VO_pose = np.identity(4)
##############################################################
'''创建输出文件夹位置及文件后缀'''
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()
suffix = "pts@" + str(args.num_icp_points) + \
"_prop@" + str(args.proposal) + \
"_tolerance@" + str(args.tolerance) + \
"_scm@" + str(args.scm_type) + \
"_thresh@" + str(args.loop_threshold)
if args.scan2submap:
suffix = suffix + '_scan2map@True'
'''Pose Graph Manager (for back-end optimization) initialization'''
PGM = PoseGraphManager()
PGM.addPriorFactor()
'''Saver初始化'''
num_frames = len(tqdm(framework))
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)
if args.loop:
'''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)
'''Mapping initialzation'''
if args.mapping is True:
Map = MappingManager(k=args.k)
# for save the result as a video
fig_idx = 1
fig = plt.figure(fig_idx)
writer = FFMpegWriter(fps=15)
vedio_name = suffix + ".mp4"
vedio_path = os.path.join(save_dir, vedio_name)
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, vedio_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 = [(np.array(
img.resize((downsample_img_width,
downsample_img_height)))).astype(np.float32)
for img in imgs]
imgs = [np.transpose(img, (2, 0, 1)) for img in imgs]
# numpy array 转troch tensor
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)
VO_processing_time[j] = time.time() - startTimeVO
final_poses = pose2tf_mat(args.rotation_mode, imgs, poses)
# cur_VO_pose取final poses的第2项，则是取T10,T21,T32。。。
rel_VO_pose = np.identity(4)
rel_VO_pose[:3, :] = final_poses[1]
# 尺度因子的确定：采用上一帧的LO输出位姿和VO输出位姿的尺度比值作为当前帧的尺度因子，初始尺度为1
# version1.0
if args.isKitti:
if j == 0:
scale_factor = 7
else:
scale_factor = math.sqrt(
np.sum(last_rel_LO_pose[:3, -1]**2) /
np.sum(last_rel_VO_pose[:3, -1]**2))
else:
scale_factor = 7
# version2.0 固定模型的尺度因子
# if args.isKitti:
# scale_factor = 35
# else:
# scale_factor = 7
scale_factors[j] = scale_factor
last_rel_VO_pose = copy.deepcopy(rel_VO_pose) # 注意深拷贝
# 先尺度修正，再对齐坐标系，施密特正交化避免病态矩阵
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)
rel_VO_poses[j] = rel_VO_pose[:3, :].reshape(12)
abs_VO_pose = np.matmul(abs_VO_pose, rel_VO_pose)
abs_VO_poses[j + 1] = abs_VO_pose[:3, :].reshape(12)
'''*************************LO部分******************************************'''
# 初始化
if j == 0:
last_pts = pointClouds[j]
if args.loop:
down_points = random_sampling(
last_pts, num_points=args.num_icp_points)
SCM.addNode(j, down_points)
if args.mapping is True:
Map.updateMap(curr_se3=PGM.curr_se3,
curr_local_ptcloud=last_pts,
down_points=args.map_down_points,
submap_points=args.num_icp_points)
curr_pts = pointClouds[j + 1]
# 选择LO的初值预估，分别是无预估，上一帧位姿，VO位姿
if args.proposal == 0:
init_pose = np.identity(4)
elif args.proposal == 1:
init_pose = last_rel_LO_pose
elif args.proposal == 2:
init_pose = rel_VO_pose
# print('init_pose')
# print(init_pose)
'''icp 类型选择2*2=4'''
startTime = time.time()
if args.scan2submap:
submap = Map.getSubMap()
if args.icp_version == 0:
curr_pts = random_sampling(curr_pts, args.num_icp_points)
rel_LO_pose, distacnces, iterations = icp(
curr_pts,
submap,
init_pose=init_pose,
tolerance=args.tolerance,
max_iterations=50)
elif args.icp_version == 1:
if args.isKitti:
curr_pts = random_sampling(curr_pts,
args.num_icp_points)
rel_LO_pose, fitness, inlier_rmse = p2l_icp(
curr_pts, submap, trans_init=init_pose, threshold=0.05)
else:
if args.icp_version == 0:
curr_pts = random_sampling(curr_pts, args.num_icp_points)
last_pts = random_sampling(last_pts, args.num_icp_points)
rel_LO_pose, distacnces, iterations = icp(
curr_pts,
last_pts,
init_pose=init_pose,
tolerance=args.tolerance,
max_iterations=50)
elif args.icp_version == 1:
if args.isKitti:
curr_pts = random_sampling(curr_pts,
args.num_icp_points)
last_pts = random_sampling(last_pts,
args.num_icp_points)
rel_LO_pose, fitness, inlier_rmse = p2l_icp(
curr_pts,
last_pts,
trans_init=init_pose,
threshold=0.05)
ICP_iteration_time[j] = time.time() - startTime
# print('rel_LO_pose1')
# print(rel_LO_pose)
if args.icp_version == 0:
ICP_iterations[j] = iterations
elif args.icp_version == 1:
ICP_fitness[j] = fitness
# 开始精匹配
if args.fine_matching == 1:
submap = Map.getSubMap()
if args.icp_version == 0:
rel_LO_pose, distacnces, iterations = icp(
curr_pts,
submap,
init_pose=rel_LO_pose,
tolerance=args.tolerance,
max_iterations=50)
elif args.icp_version == 1:
rel_LO_pose, fitness, inlier_rmse = p2l_icp(
curr_pts,
submap,
trans_init=rel_LO_pose,
threshold=0.05)
# print('rel_LO_pose2')
# print(rel_LO_pose)
ResultSaver.saveRelativePose(rel_LO_pose)
'''更新变量'''
last_pts = curr_pts
last_rel_LO_pose = rel_LO_pose
if args.loop:
'''Update loop detection nodes'''
down_points = random_sampling(curr_pts,
num_points=args.num_icp_points)
SCM.addNode(j + 1, down_points)
'''Update the edges and nodes of pose graph'''
PGM.curr_node_idx = j + 1
PGM.curr_se3 = np.matmul(PGM.curr_se3, rel_LO_pose)
PGM.addOdometryFactor(rel_LO_pose)
PGM.prev_node_idx = PGM.curr_node_idx
est_pose = Transform_matrix_L2C @ PGM.curr_se3 @ np.linalg.inv(
Transform_matrix_L2C)
est_poses[j + 1] = est_pose[:3, :].reshape(12)
# 建图更新
if args.mapping is True:
Map.updateMap(curr_se3=PGM.curr_se3,
curr_local_ptcloud=curr_pts,
down_points=args.map_down_points,
submap_points=args.num_icp_points)
# loop detection and optimize the graph
if (args.loop and 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)
if args.icp_version == 0:
loop_transform, _, _ = icp(
curr_pts,
loop_scan_down_pts,
init_pose=yawdeg2se3(yaw_diff_deg),
tolerance=args.tolerance,
max_iterations=50)
elif args.icp_version == 1:
loop_transform, _, _ = p2l_icp(
curr_pts,
loop_scan_down_pts,
trans_init=yawdeg2se3(yaw_diff_deg),
threshold=0.05)
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)
# 2-3/ updateMap
if args.mapping is True:
Map.optimizeGlobalMap(PGM.graph_optimized,
PGM.curr_node_idx)
# 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.vizmapping is True:
Map.vizMapWithOpen3D()
if args.mapping is True:
map_name = suffix + '.pcd'
map_path = os.path.join(save_dir, map_name)
Map.saveMap2File(map_path)
if args.output_dir is not None:
# np.save(output_dir / 'predictions.npy', predictions_array)
np.savetxt(save_dir / 'scale_factors_' + suffix + '.txt',
scale_factors)
np.savetxt(save_dir / 'rel_VO_poses.txt', rel_VO_poses)
np.savetxt(save_dir / 'abs_VO_poses.txt', abs_VO_poses)
rel_LO_poses_file = 'rel_LO_poses_' + suffix + '.txt'
abs_LO_poses_file = 'abs_LO_poses_' + suffix + '.txt'
ResultSaver.saveRelativePosesResult(rel_LO_poses_file)
ResultSaver.saveFinalPoseGraphResult(abs_LO_poses_file)
if args.icp_version == 0:
np.savetxt(save_dir / 'iterations_' + suffix + '.txt',
ICP_iterations)
elif args.icp_version == 1:
np.savetxt(save_dir / 'fitness_' + suffix + '.txt',
ICP_fitness)
np.savetxt(save_dir / 'iteration_time_' + suffix + '.txt',
ICP_iteration_time)
np.savetxt(save_dir / 'VO_processing_time.txt', VO_processing_time)
if args.isKitti:
np.savetxt(
save_dir / 'est_poses_' + suffix +
'.txt'.format(args.sequence_idx), est_poses)
# 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))
# 存储优化前后误差精度指标
if args.isKitti:
err_statics = np.array([
mean_errors, std_errors, optimized_mean_errors,
optimized_std_errors
])
np.savetxt(
save_dir / 'err_statics_' + suffix +
'.txt'.format(args.sequence_idx), err_statics)
# 迭代次数
mean_iterations = ICP_iterations.mean()
std_iterations = ICP_iterations.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 = ICP_iteration_time.mean()
std_iter_time = ICP_iteration_time.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))