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))