def track(self, trackframe, guess=None): """ Track an image. Args: trackframe : frame to track guess : optional initial guess of the motion """ if len(self.keyframes) == 0: # First frame, so don't track anything yet trackframe.compute_pyramids() self.keyframes.append(trackframe) self.active_keyframe_idx = 0 active_keyframe = self.keyframes[0] else: # Default behaviour for second frame and beyond active_keyframe = self.keyframes[self.active_keyframe_idx] if guess is None: # Default initial guess is previous pose relative to keyframe if len(self.T_c_w) == 0: # We just started relocalizing guess = SE3.identity() else: guess = self.T_c_w[-1].dot(active_keyframe.T_c_w.inv()) # Better initial guess is previous pose + previous motion if self.use_motion_model_guess and len(self.T_c_w) > 1: guess = self.T_c_w[-1].dot(self.T_c_w[-2].inv().dot(guess)) else: guess = guess.dot(active_keyframe.T_c_w.inv()) # Estimate pose change from keyframe to tracking frame T_track_ref = self._compute_frame_to_frame_motion( active_keyframe, trackframe, guess) T_track_ref.normalize() # Numerical instability problems otherwise self.T_c_w.append(T_track_ref.dot(active_keyframe.T_c_w)) # Threshold the distance from the active keyframe to drop a new one se3_vec = SE3.log(T_track_ref) trans_dist = np.linalg.norm(se3_vec[0:3]) rot_dist = np.linalg.norm(se3_vec[3:6]) if trans_dist > self.keyframe_trans_thresh or \ rot_dist > self.keyframe_rot_thresh: if self.mode is 'map': trackframe.T_c_w = self.T_c_w[-1] trackframe.compute_pyramids() self.keyframes.append(trackframe) print('Dropped new keyframe. ' 'Trans dist was {:.3f}. Rot dist was {:.3f}.'.format( trans_dist, rot_dist)) self.active_keyframe_idx += 1 print('Active keyframe idx: {}'.format( self.active_keyframe_idx))
def test_bundle_adjust(self, options, camera, points, poses, observations): from liegroups import SE3 from pyslam.residuals import ReprojectionResidual problem = Problem(options) obs_var = [1, 1, 2] # [u,v,d] obs_stiffness = invsqrt(np.diagflat(obs_var)) for i, this_pose_obs in enumerate(observations): for j, o in enumerate(this_pose_obs): residual = ReprojectionResidual(camera, o, obs_stiffness) problem.add_residual_block( residual, ['T_cam{}_w'.format(i), 'pt{}_w'.format(j)]) params_true = {} params_init = {} for i, pt in enumerate(points): pid = 'pt{}_w'.format(i) params_true.update({pid: pt}) params_init.update({pid: camera.triangulate( observations[0][i] + 10. * np.random.rand(3))}) for i, pose in enumerate(poses): pid = 'T_cam{}_w'.format(i) params_true.update({pid: pose}) params_init.update({pid: SE3.identity()}) problem.initialize_params(params_init) problem.set_parameters_constant('T_cam0_w') params_final = problem.solve() for key in params_true.keys(): p_est = params_final[key] p_true = params_true[key] if isinstance(p_est, SE3): err = SE3.log(p_est.inv().dot(p_true)) else: err = p_est - p_true assert np.linalg.norm(err) < 1e-4
def track(self, trackframe): """ Track an image. Args: trackframe : frame to track """ if len(self.keyframes) == 0: # First frame, so don't track anything yet self.keyframes.append(trackframe) self.active_keyframe_idx = 0 active_keyframe = self.keyframes[0] else: # Default behaviour for second frame and beyond active_keyframe = self.keyframes[self.active_keyframe_idx] # Estimate pose change from keyframe to tracking frame T_track_ref = self._compute_frame_to_frame_motion( active_keyframe, trackframe) T_track_ref.normalize() # Numerical instability problems otherwise self.T_c_w.append(T_track_ref.dot(active_keyframe.T_c_w)) # Threshold the distance from the active keyframe to drop a new one se3_vec = SE3.log(T_track_ref) trans_dist = np.linalg.norm(se3_vec[0:3]) rot_dist = np.linalg.norm(se3_vec[3:6]) if trans_dist > self.keyframe_trans_thresh or \ rot_dist > self.keyframe_rot_thresh: if self.mode is 'map': trackframe.T_c_w = self.T_c_w[-1] self.keyframes.append(trackframe) print('Dropped new keyframe. ' 'Trans dist was {:.3f}. Rot dist was {:.3f}.'.format( trans_dist, rot_dist)) self.active_keyframe_idx += 1 print('Active keyframe idx: {}'.format( self.active_keyframe_idx))
residual = PhotometricResidualSE3(camera, im_ref, disp_ref, im_track, jac_ref, 1., 1.) # Timing debug # niters = 100 # start = time.perf_counter() # for _ in range(niters): # cost.evaluate([params_init['T_1_0']]) # end = time.perf_counter() # print('cost.evaluate avg {} s', (end - start) / niters) # start = time.perf_counter() # for _ in range(niters): # cost.evaluate([params_init['T_1_0']], [True]) # end = time.perf_counter() # print('cost.evaluate w jac avg {} s', (end - start) / niters) # Optimize # start = time.perf_counter() problem = Problem(options) problem.add_residual_block(residual, ['T_1_0']) problem.initialize_params(params) params = problem.solve() # end = time.perf_counter() # print('Elapsed time: {} s'.format(end - start)) print('Error in T_1_w: {}'.format( SE3.log(T_1_w.dot((params['T_1_0'].dot(T_0_w)).inv()))))
residual = PhotometricResidual(camera, im_ref, disp_ref, jac_ref, im_track, 1.) # Timing debug # niters = 100 # start = time.perf_counter() # for _ in range(niters): # cost.evaluate([params_init['T_1_0']]) # end = time.perf_counter() # print('cost.evaluate avg {} s', (end - start) / niters) # start = time.perf_counter() # for _ in range(niters): # cost.evaluate([params_init['T_1_0']], [True]) # end = time.perf_counter() # print('cost.evaluate w jac avg {} s', (end - start) / niters) # Optimize # start = time.perf_counter() problem = Problem(options) problem.add_residual_block(residual, ['T_1_0']) problem.initialize_params(params) params = problem.solve() # end = time.perf_counter() # print('Elapsed time: {} s'.format(end - start)) print('Error in T_1_w: {}'.format( SE3.log(T_1_w * (params['T_1_0'] * T_0_w).inv())))
def optimize_transformation( previous_ids: np.ndarray, previous_points: np.ndarray, current_ids: np.ndarray, current_points: np.ndarray) -> Tuple[Quaternion, np.ndarray]: common_points = set(previous_ids).intersection(set(current_ids)) if len(common_points) < 3: print( 'WARNING: Fewer than 3 common points. Unable to find transfomration' ) return Quaternion(), np.zeros(3) previous_points_filter = np.array([ i for i in range(len(previous_ids)) if previous_ids[i] in common_points ]) current_points_filter = np.array([ i for i in range(len(current_ids)) if current_ids[i] in common_points ]) previous_points = previous_points[:, previous_points_filter] current_points = current_points[:, current_points_filter] _, n_points = previous_points.shape xi = np.zeros(6) plot_points(previous_points, current_points) weights = np.ones(len(common_points)) for attempt in range(5): for iteration in range(10): A = np.zeros((3 * n_points, 6)) b = np.zeros(3 * n_points) T = SE3.exp(xi) R = T.rot.as_matrix() t = T.trans iteration_points = R @ previous_points + t[:, np.newaxis] errors = np.sqrt( np.sum(np.square(current_points - iteration_points), axis=0)) k = 1.345 * np.std(errors) huber_weights = np.array( list(map(lambda x: 1.0 if x <= k else k / abs(x), errors))) * weights huber_weights = np.repeat(huber_weights, 3) W = np.diag(huber_weights) for i in range(n_points): x1, x2, x3 = iteration_points[:, i] A[i * 3:i * 3 + 3, :] = np.array([[1, 0, 0, 0, x3, -x2], [0, 1, 0, -x3, 0, x1], [0, 0, 1, x2, -x1, 0]]) b[i * 3:i * 3 + 3] = current_points[:, i] - iteration_points[:, i] AA = A.T @ W @ A AA_inv = np.linalg.inv(AA) xi_delta = AA_inv @ A.T @ W @ b xi = SE3.log(SE3.exp(xi) * SE3.exp(xi_delta)) if np.linalg.norm(xi_delta) < 1e-8: break for i in range(len(common_points)): weights[i] = 0 if errors[i] > np.average( errors) + 1 * np.std(errors) else 1 print(weights) T = SE3.exp(xi) R = T.rot.as_matrix() t = T.trans print( f'error: {np.max(errors)}, {np.min(errors)}, {np.average(errors)}, {np.std(errors)}' ) plot_points(iteration_points, current_points) return Quaternion(matrix=R), t
'T_3_0': T_3_0_init, 'T_4_0': T_4_0_init, 'T_5_0': T_5_0_init, 'T_6_0': T_6_0_init } params_true = { 'T_1_0': T_1_0_true, 'T_2_0': T_2_0_true, 'T_3_0': T_3_0_true, 'T_4_0': T_4_0_true, 'T_5_0': T_5_0_true, 'T_6_0': T_6_0_true } problem.initialize_params(params_init) params_final = problem.solve() print(problem.summary(format='full')) print("Initial Error:") for key in params_true.keys(): print('{}: {}'.format( key, SE3.log(params_init[key].inv().dot(params_true[key])))) print() print("Final Error:") for key in params_true.keys(): print('{}: {}'.format( key, SE3.log(params_final[key].inv().dot(params_true[key]))))
problem.initialize_params(params_init) problem.set_parameters_constant('T_cam0_w') # import ipdb # ipdb.set_trace() params_final = problem.solve() print(problem.summary(format='full')) # Compute errors print("Initial Error:") for key in params_true.keys(): p_est = params_init[key] p_true = params_true[key] if isinstance(p_est, SE3): err = SE3.log(p_est.inv().dot(p_true)) else: err = p_est - p_true print('{}: {}'.format(key, err)) print() print("Final Error:") for key in params_true.keys(): p_est = params_final[key] p_true = params_true[key] if isinstance(p_est, SE3): err = SE3.log(p_est.inv().dot(p_true)) else:
for i in range(len(obs) - 1): T_c1_w = T_cam_w_GT[i] T_c2_w = T_cam_w_GT[i + 1] pid = 'T_cam{}_cam{}'.format(i + 1, i) params_true.update({pid: T_c2_w * T_c1_w.inv()}) params_init.update({pid: SE3.identity()}) problem.initialize_params(params_init) params_final = problem.solve() print() # Compute errors print("Initial Error:") for key in params_true.keys(): p_est = params_init[key] p_true = params_true[key] err = SE3.log(p_est.inv() * p_true) print('{}: {}'.format(key, err)) print() print("Final Error:") for key in params_true.keys(): p_est = params_final[key] p_true = params_true[key] err = SE3.log(p_est.inv() * p_true) print('{}: {}'.format(key, err))