def find_loop_closures(traj, cum_dist): num_loop_closures = 0 filtered_loop_closures = 0 idx_list = [] for i in range( 0, traj.shape[0], 8 ): #check for loop closure points (compare current frame with all future points, and do this for every 20th frame) current_pose = traj[i] current_trans = current_pose[0:3, 3] current_rot = SO3.from_matrix(current_pose[0:3, 0:3], normalize=True).to_rpy() current_yaw = current_rot[2] current_cum_dist = cum_dist[i] loop_closure_idx = np.linalg.norm( np.abs(current_trans[0:3] - traj[i + 1:, 0:3, 3]), axis=1) <= 7 dist_idx = (cum_dist[i + 1:] - current_cum_dist) >= 10 loop_closure_idx = loop_closure_idx & dist_idx idx = np.where(loop_closure_idx == 1) if idx != np.array([]): for pose_idx in idx[0]: T = traj[i + 1:][pose_idx] yaw = SE3.from_matrix(T, normalize=True).rot.to_rpy()[2] yaw_diff = np.abs(np.abs(current_yaw) - np.abs(yaw)) in_range = ((yaw_diff <= 0.15)) or (np.abs( (np.pi - yaw_diff) <= 0.15)) filtered_loop_closures += in_range if in_range: idx_list.append(pose_idx + i) num_loop_closures += np.sum(loop_closure_idx) return num_loop_closures, filtered_loop_closures, idx_list
def add_noise(pose_list, trans_noise, rot_noise): ''' Add Noise to the pose measurements as a percentage of the translation and rotation magnitude''' #Create the translation noise vectors trans_noise_sigma = trans_noise * np.linalg.norm( np.array([pose.trans for pose in pose_list]), axis=1) trans_noise_vec = [ np.random.randn((3)) * trans_noise_sigma[i] for i in range(len(pose_list)) ] #Create the rotation noise vectors rot_noise_sigma = rot_noise * np.linalg.norm( np.array([pose.rot.log() for pose in pose_list]), axis=1) rot_noise_unit_vecs = np.random.rand(3, len(pose_list)) rot_noise_unit_vecs = rot_noise_unit_vecs / np.linalg.norm( rot_noise_unit_vecs, axis=0) rot_noise_vec = [ rot_noise_sigma[i] * np.random.rand() * rot_noise_unit_vecs[:, i] for i in range(len(pose_list)) ] #Add the noise to the poses noisy_poses = [ SE3(rot=pose.rot.dot(SO3.exp(rot_noise_vec[i])), trans=pose.trans + trans_noise_vec[i]) for i, pose in enumerate(pose_list) ] return noisy_poses
def do_tracking(basepath, seq, track_cond, vo, frames=None, outfile=None, rgb_dir='rgb'): track_data = tum_rgbd.LocalizationDataset(basepath, seq, track_cond, frames=frames, rgb_dir=rgb_dir) # Ground truth T_w_c_gt = [ SE3(SO3.from_quaternion(p[0], ordering='xyzw'), p[1]) for p in track_data.poses ] T_0_w = T_w_c_gt[0].inv() print('Tracking using {}/{}'.format(seq, track_cond)) vo.set_mode('track') start = time.perf_counter() for c_idx in range(len(track_data)): image = track_data.get_gray(c_idx) depth = track_data.get_depth(c_idx) try: vo.track(image, depth) # vo.track(image, depth, guess=T_w_c_gt[c_idx].inv()) # debug end = time.perf_counter() print('Image {}/{} ({:.2f} %) | {:.3f} s'.format( c_idx, len(track_data), 100. * c_idx / len(track_data), end - start), end='\r') except Exception as e: print('Error on {}/{}'.format(seq, track_cond)) print(e) print('Image {}/{} ({:.2f} %) | {:.3f} s'.format( c_idx, len(track_data), 100. * c_idx / len(track_data), end - start)) break start = end # Compute errors T_w_c_est = [T.inv() for T in vo.T_c_w] tm = TrajectoryMetrics(T_w_c_gt, T_w_c_est) # Save to file if outfile is not None: print('Saving to {}'.format(outfile)) tm.savemat(outfile) return tm, vo
def do_vo_mapping(basepath, seq, ref_cond, frames=None, outfile=None, rgb_dir='rgb'): ref_data = tum_rgbd.LocalizationDataset(basepath, seq, ref_cond, frames=frames, rgb_dir=rgb_dir) test_im = ref_data.get_gray(0) camera = get_camera(seq, test_im) # Ground truth T_w_c_gt = [ SE3(SO3.from_quaternion(p[0], ordering='xyzw'), p[1]) for p in ref_data.poses ] T_0_w = T_w_c_gt[0].inv() vo = DenseRGBDPipeline(camera, first_pose=T_0_w) vo.keyframe_trans_thresh = 0.2 # meters vo.keyframe_rot_thresh = 15. * np.pi / 180. # rad vo.depth_stiffness = 1. / 0.01 # 1/meters vo.intensity_stiffness = 1. / 0.1 # 1/ (intensity is in [0,1] ) vo.use_motion_model_guess = True print('Mapping using {}/{}'.format(seq, ref_cond)) vo.set_mode('map') start = time.perf_counter() for c_idx in range(len(ref_data)): image = ref_data.get_gray(c_idx) depth = ref_data.get_depth(c_idx) vo.track(image, depth) # vo.track(image, depth, guess=T_w_c_gt[c_idx].inv()) # debug end = time.perf_counter() print('Image {}/{} ({:.2f} %) | {:.3f} s'.format( c_idx, len(ref_data), 100. * c_idx / len(ref_data), end - start), end='\r') start = end # Compute errors T_w_c_est = [T.inv() for T in vo.T_c_w] tm = TrajectoryMetrics(T_w_c_gt, T_w_c_est) # Save to file if outfile is not None: print('Saving to {}'.format(outfile)) tm.savemat(outfile) return tm, vo
def evaluate_results_with_andreff(my_solver, T_v_rel, T_c_rel, scale, cons): # Solve with the Andreff method fail = 1000. # Extrinsic calibration inverse for evaluating error ext_cal_trans = SE3.from_matrix(my_solver.extcal).trans ext_cal_rot_inv = SE3.from_matrix(my_solver.extcal).rot.inv() # Set the pose data my_solver.set_T_v_rel(T_v_rel) T_c_rel_scaled = my_solver.set_T_c_rel(T_c_rel, scale) my_solver.andreff_solver.set_vals(T_v_rel, T_c_rel_scaled, None) X_andreff, scale_andreff, runtime_andreff = my_solver.andreff_solver.solve( schur=True) # Solve for the extrinsic calibration dual_time, dual_gt_value, dual_primal, dual_gap, dual_opt, dual_solution, dual_flag = my_solver.dual_solve( cons) rel_time, rel_gt_value, rel_primal, rel_gap, relax_opt, relax_solution, rel_flag = my_solver.relax_solve( cons) if dual_flag: #Solution was found # Store the estimation errors dual_trans_error = np.linalg.norm(ext_cal_trans - dual_solution[:3, 3]) dual_rot_error = np.linalg.norm((ext_cal_rot_inv.dot( SO3.from_matrix(dual_solution[:3, :3], normalize=True))).log()) dual_alpha_error = np.abs(scale - 1 / dual_opt[3]) dual_results_dict = { 'dual_time': dual_time, 'dual_gt_value': np.asscalar(dual_gt_value), 'dual_primal': np.asscalar(dual_primal), 'dual_gap': np.asscalar(dual_gap), 'dual_trans_error': dual_trans_error, 'dual_rot_error': dual_rot_error, 'dual_alpha_error': np.asscalar(dual_alpha_error) } print("DUAL SCALE: {:}".format(1 / dual_opt[3])) else: #Solution was not found dual_results_dict = { 'dual_time': fail, 'dual_gt_value': fail, 'dual_primal': fail, 'dual_gap': fail, 'dual_trans_error': fail, 'dual_rot_error': fail, 'dual_alpha_error': fail } if rel_flag: rel_trans_error = np.linalg.norm(ext_cal_trans - relax_solution[:3, 3]) rel_rot_error = np.linalg.norm((ext_cal_rot_inv.dot( SO3.from_matrix(relax_solution[:3, :3], normalize=True))).log()) rel_alpha_error = np.abs(scale - 1 / relax_opt[3]) rel_results_dict = { 'rel_time': rel_time, 'rel_gt_value': np.asscalar(rel_gt_value), 'rel_primal': np.asscalar(rel_primal), 'rel_gap': np.asscalar(rel_gap), 'rel_trans_error': rel_trans_error, 'rel_rot_error': rel_rot_error, 'rel_alpha_error': np.asscalar(rel_alpha_error) } else: #Solution was not found rel_results_dict = { 'rel_time': fail, 'rel_gt_value': fail, 'rel_primal': fail, 'rel_gap': fail, 'rel_trans_error': fail, 'rel_rot_error': fail, 'rel_alpha_error': fail } rel_results_dict.update(dual_results_dict) # Store Andreff estimation errors andreff_trans_error = np.linalg.norm(ext_cal_trans - X_andreff[0:3, 3]) andreff_rot_error = np.linalg.norm( (ext_cal_rot_inv.dot(SO3.from_matrix(X_andreff[:3, :3], normalize=True))).log()) andreff_alpha_error = np.abs(scale - 1 / scale_andreff) print("ANDREFF SCALE: {:}".format(scale_andreff)) # Store data and_dict = { "andreff_trans_error": andreff_trans_error, "andreff_rot_error": andreff_rot_error, "andreff_alpha_error": andreff_alpha_error, 'andreff_time': runtime_andreff } rel_results_dict.update(and_dict) return rel_results_dict
# import copy import numpy as np from liegroups import SE3, SO3 from pyslam.residuals import PoseResidual, PoseToPoseResidual from pyslam.problem import Options, Problem from pyslam.utils import invsqrt T_1_0_true = SE3.identity() T_2_0_true = SE3(SO3.identity(), -np.array([0.5, 0, 0])) T_3_0_true = SE3(SO3.identity(), -np.array([1, 0, 0])) T_4_0_true = SE3(SO3.rotz(np.pi / 2), -(SO3.rotz(np.pi / 2).dot(np.array([1, 0.5, 0])))) T_5_0_true = SE3(SO3.rotz(np.pi), -(SO3.rotz(np.pi).dot(np.array([0.5, 0.5, 0])))) T_6_0_true = SE3(SO3.rotz(-np.pi / 2), -(SO3.rotz(-np.pi / 2).dot(np.array([0.5, 0, 0])))) # T_6_0_true = copy.deepcopy(T_2_0_true) # Odometry T_1_0_obs = SE3.identity() T_2_1_obs = T_2_0_true.dot(T_1_0_true.inv()) T_3_2_obs = T_3_0_true.dot(T_2_0_true.inv()) T_4_3_obs = T_4_0_true.dot(T_3_0_true.inv()) T_5_4_obs = T_5_0_true.dot(T_4_0_true.inv()) T_6_5_obs = T_6_0_true.dot(T_5_0_true.inv()) # Loop closure T_6_2_obs = T_6_0_true.dot(T_2_0_true.inv())
def evaluate_results(my_solver, T_v_rel, T_c_rel, scale, cons): #Number to know that the solution failed fail = 1000 #Extrinsic calibration inverse for evaluating error ext_cal_trans = SE3.from_matrix(my_solver.extcal).trans ext_cal_rot_inv = SE3.from_matrix(my_solver.extcal).rot.inv() # Set the pose data my_solver.set_T_v_rel(T_v_rel) my_solver.set_T_c_rel(T_c_rel, scale) # Solve for the extrinsic calibration dual_time, dual_gt_value, dual_primal, dual_gap, dual_opt, dual_solution, dualflag = my_solver.dual_solve( cons) rel_time, rel_gt_value, rel_primal, rel_gap, relax_opt, relax_solution, relflag = my_solver.relax_solve( cons) #and_extcal, and_scale, and_runtime = my_solver.andreff_solver.solve() if dualflag: #Solution was found # Store the estimation errors dual_trans_error = np.linalg.norm(ext_cal_trans - dual_solution[:3, 3]) dual_rot_error = np.linalg.norm((ext_cal_rot_inv.dot( SO3.from_matrix(dual_solution[:3, :3], normalize=True))).log()) dual_alpha_error = np.abs(scale - 1 / dual_opt[3]) dual_results_dict = { 'dual_time': dual_time, 'dual_gt_value': np.asscalar(dual_gt_value), 'dual_primal': np.asscalar(dual_primal), 'dual_gap': np.asscalar(dual_gap), 'dual_trans_error': dual_trans_error, 'dual_rot_error': dual_rot_error, 'dual_alpha_error': np.asscalar(dual_alpha_error) } else: #Solution was not found dual_results_dict = { 'dual_time': fail, 'dual_gt_value': fail, 'dual_primal': fail, 'dual_gap': fail, 'dual_trans_error': fail, 'dual_rot_error': fail, 'dual_alpha_error': fail } if relflag: rel_trans_error = np.linalg.norm(ext_cal_trans - relax_solution[:3, 3]) rel_rot_error = np.linalg.norm((ext_cal_rot_inv.dot( SO3.from_matrix(relax_solution[:3, :3], normalize=True))).log()) rel_alpha_error = np.abs(scale - 1 / relax_opt[3]) rel_results_dict = { 'rel_time': rel_time, 'rel_gt_value': np.asscalar(rel_gt_value), 'rel_primal': np.asscalar(rel_primal), 'rel_gap': np.asscalar(rel_gap), 'rel_trans_error': rel_trans_error, 'rel_rot_error': rel_rot_error, 'rel_alpha_error': np.asscalar(rel_alpha_error) } else: #Solution was not found rel_results_dict = { 'rel_time': fail, 'rel_gt_value': fail, 'rel_primal': fail, 'rel_gap': fail, 'rel_trans_error': fail, 'rel_rot_error': fail, 'rel_alpha_error': fail } rel_results_dict.update(dual_results_dict) return rel_results_dict