def compute_target(self, idx, target_idx, source_idx): ## Compute ts # dt = np.abs(self.ts_samples[idx,target_idx] - self.ts_samples[idx, source_idx]) dt = self.ts_samples[idx, target_idx] - self.ts_samples[idx, source_idx] #compute Tau_gt T2 = SE3.from_matrix(self.gt_samples[idx, target_idx, :, :], normalize=True).inv() T1 = SE3.from_matrix(self.gt_samples[idx, source_idx, :, :], normalize=True) dT_gt = T2.dot( T1 ) #pose change from source to a target (for reconstructing source from target) gt_lie_alg = dT_gt.log() #compute Tau_vo T2 = SE3.from_matrix(self.vo_samples[idx, target_idx, :, :], normalize=True).inv() T1 = SE3.from_matrix(self.vo_samples[idx, source_idx, :, :], normalize=True) dT_vo = T2.dot(T1) vo_lie_alg = dT_vo.log() if self.config['estimator_type'] == 'mono': if np.linalg.norm(vo_lie_alg[0:3]) >= 1e-8: scale = np.linalg.norm(gt_lie_alg[0:3]) / np.linalg.norm( vo_lie_alg[0:3]) scale = 1 vo_lie_alg[0:3] = scale * vo_lie_alg[0:3] dT_vo = SE3.exp(vo_lie_alg) gt_correction = (dT_gt.dot(dT_vo.inv())).log() return gt_lie_alg, vo_lie_alg, gt_correction, dt
def __init__(self): self.model = TractorKinematics() self.controller = TractorMoveToGoalController(self.model) self.v_left = 0.0 self.v_right = 0.0 self.world_pose_tractor = SE3.identity() self.world_pose_tractor_goal = SE3.identity()
def test_se3(self, options): from liegroups import SE3 from pyslam.residuals import PoseResidual, PoseToPoseResidual problem = Problem(options) odom = SE3.exp(0.1 * np.ones(6)) odom_stiffness = invsqrt(1e-3 * np.eye(SE3.dof)) T0_stiffness = invsqrt(1e-6 * np.eye(SE3.dof)) odom_covar = np.linalg.inv(np.dot(odom_stiffness, odom_stiffness)) T0_covar = np.linalg.inv(np.dot(T0_stiffness, T0_stiffness)) residual0 = PoseResidual(SE3.identity(), T0_stiffness) residual1 = PoseToPoseResidual(odom, odom_stiffness) params_init = {'T0': SE3.identity(), 'T1': SE3.identity()} problem.add_residual_block(residual0, 'T0') problem.add_residual_block(residual1, ['T0', 'T1']) problem.initialize_params(params_init) problem.solve() problem.compute_covariance() estimated_covar = problem.get_covariance_block('T1', 'T1') expected_covar = odom_covar + odom.adjoint().dot(T0_covar.dot(odom.adjoint().T)) assert np.allclose(estimated_covar, expected_covar)
def load_brookshire_data(data_filename): data_dict = sio.loadmat(data_filename) pose_array = data_dict['T_v_rel_list'] T_v_rel = [SE3.from_matrix(pose_array[:, :, i]) for i in range(pose_array.shape[2])] pose_array = data_dict['T_c_rel_list'] T_c_rel = [SE3.from_matrix(pose_array[:, :, i]) for i in range(pose_array.shape[2])] extcal = data_dict['extcal'] return T_v_rel, T_c_rel, extcal
def load_data_1(data_filename): #Data loader for Emmett's Synethetic datasets data_dict = sio.loadmat(data_filename) pose_array = data_dict['T_vki_list'] T_vki = [SE3.from_matrix(pose_array[i, :, :]) for i in range(pose_array.shape[0])] pose_array = data_dict['T_ci_list'] T_ci = [SE3.from_matrix(pose_array[i, :, :]) for i in range(pose_array.shape[0])] extcal = data_dict['extcal'] return T_vki, T_ci, extcal
def poses(self): from liegroups import SE3 T_cam_w_true = [ SE3.identity(), SE3.exp(0.1 * np.ones(6)), SE3.exp(0.2 * np.ones(6)), SE3.exp(0.3 * np.ones(6)) ] return T_cam_w_true
def evolve_world_pose_tractor( self, world_pose_tractor: SE3, v_left: float, v_right: float, delta_t: float, ) -> SE3: v, w = self.wheel_velocity_to_unicycle(v_left, v_right) tractor_pose_t = SE3.exp([v * delta_t, 0, 0, 0, 0, w * delta_t]) return world_pose_tractor.dot(tractor_pose_t)
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 compute_target(self, poses, target_idx, source_idx): T_c2_w = SE3.from_matrix(poses[target_idx], normalize=True) T_c1_w = SE3.from_matrix(poses[source_idx], normalize=True) T_c2_c1 = T_c2_w.dot(T_c1_w.inv()) gt_lie_alg = T_c2_c1.log() vo_lie_alg = np.copy(gt_lie_alg) gt_correction = np.zeros(gt_lie_alg.shape) dt = 0 return gt_lie_alg, vo_lie_alg, gt_correction, dt
def run_vo_kitti(basedir, date, drive, frames, outfile=None): # Load KITTI data dataset = pykitti.raw(basedir, date, drive, frames=frames, imformat='cv2') first_oxts = dataset.oxts[0] T_cam0_imu = SE3.from_matrix(dataset.calib.T_cam0_imu) T_cam0_imu.normalize() T_0_w = T_cam0_imu.dot(SE3.from_matrix(first_oxts.T_w_imu).inv()) T_0_w.normalize() # Create the camera test_im = np.array(next(dataset.cam0)) fu = dataset.calib.K_cam0[0, 0] fv = dataset.calib.K_cam0[1, 1] cu = dataset.calib.K_cam0[0, 2] cv = dataset.calib.K_cam0[1, 2] b = dataset.calib.b_gray h, w = test_im.shape camera = StereoCamera(cu, cv, fu, fv, b, w, h) # Ground truth T_w_c_gt = [ SE3.from_matrix(o.T_w_imu).dot(T_cam0_imu.inv()) for o in dataset.oxts ] # Pipeline vo = DenseStereoPipeline(camera, first_pose=T_0_w) # Skip the highest resolution level vo.pyrlevel_sequence.pop() vo.pyr_cameras.pop() start = time.perf_counter() for c_idx, impair in enumerate(dataset.gray): vo.track(np.array(impair[0]), np.array(impair[1])) # vo.track(impair[0], impair[1], guess=T_w_c_gt[c_idx].inv()) end = time.perf_counter() print('Image {}/{} | {:.3f} s'.format(c_idx, len(dataset), end - start)) 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) # Clean up del vo return tm
def _compute_frame_to_frame_motion(self, ref_frame, track_frame, guess=SE3.identity()): # params = {'T_1_0': guess} params = {'R_1_0': guess.rot, 't_1_0_1': guess.trans} for (pyrlevel, pyr_camera) in zip(self.pyrlevel_sequence, self.pyr_cameras): pyrfactor = 2**-pyrlevel im_jacobian = ref_frame.jacobian[pyrlevel] # ESM # im_jacobian = 0.5 * (ref_frame.jacobian[pyrlevel] + # track_frame.jacobian[pyrlevel]) if self.depth_map_type is 'disparity': depth_ref = ref_frame.disparity[pyrlevel] # Disparity is in pixels, so we need to scale it according to the pyramid level depth_stiffness = self.depth_stiffness / pyrfactor else: depth_ref = ref_frame.depth[pyrlevel] depth_stiffness = self.depth_stiffness residual = PhotometricResidualSE3( pyr_camera, ref_frame.im_pyr[pyrlevel], depth_ref, track_frame.im_pyr[pyrlevel], im_jacobian, self.intensity_stiffness, depth_stiffness, self.min_grad) problem = Problem(self.motion_options) # problem.add_residual_block(residual, ['T_1_0'], loss=self.loss) problem.add_residual_block(residual, ['R_1_0', 't_1_0_1'], loss=self.loss) problem.initialize_params(params) if pyrlevel > 2: problem.set_parameters_constant('t_1_0_1') # else: # problem.set_parameters_constant('R_1_0') params = problem.solve() # print(problem.summary(format='brief')) # # DEBUG: Store residuals for later # try: # self.residuals = np.hstack( # [self.residuals, residual.evaluate([guess])]) # except AttributeError: # self.residuals = residual.evaluate([guess]) # return params['T_1_0'] return SE3(params['R_1_0'], params['t_1_0_1'])
def test_jacobians_se3(self, se3_residual): T1 = SE3.exp([1, 2, 3, 4, 5, 6]) T2 = SE3.exp([7, 8, 9, 10, 11, 12]) _, jac1 = se3_residual.evaluate([T1, T2], compute_jacobians=[True, True]) _, jac2 = se3_residual.evaluate([T1, T2], compute_jacobians=[True, False]) _, jac3 = se3_residual.evaluate([T1, T2], compute_jacobians=[False, True]) assert len(jac1) == len(jac2) == len(jac3) == 2 assert jac1[0].shape == (6, 6) and jac1[1].shape == (6, 6) assert jac2[0].shape == (6, 6) and jac2[1] is None assert jac3[0] is None and jac3[1].shape == (6, 6)
def generate_trajectory_metrics(gt_traj, est_traj, name='',seq='', mode=''): gt_traj_se3 = [SE3.from_matrix(T,normalize=True) for T in gt_traj] est_traj_se3 = [SE3.from_matrix(T, normalize=True) for T in est_traj] tm = TrajectoryMetrics(gt_traj_se3, est_traj_se3, convention = 'Twv') est_mATE_trans, est_mATE_rot = tm.mean_err() est_mATE_rot = est_mATE_rot*180/np.pi print("{} ({}) mean trans. error: {} | mean rot. error: {}".format(name, mode, est_mATE_trans, est_mATE_rot)) seg_lengths = list(range(100,801,100)) _, est_seg_errs = tm.segment_errors(seg_lengths, rot_unit='rad') est_seg_err_trans = np.mean(est_seg_errs[:,1])*100 est_seg_err_rot = 100*np.mean(est_seg_errs[:,2])*180/np.pi print("{} ({}) mean Segment Errors: {} (trans, %) | {} (rot, deg/100m)".format(name, mode, est_seg_err_trans, est_seg_err_rot) ) return tm, (seq, name, mode, est_mATE_trans.round(3), est_mATE_rot.round(3), est_seg_err_trans.round(3), est_seg_err_rot.round(3))
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 evaluate(self, params, compute_jacobians=None): T_cam_w = params[0] pt_w = params[1] pt_cam = T_cam_w.dot(pt_w) if compute_jacobians: jacobians = [None for _ in enumerate(params)] predicted_obs, cam_jacobian = self.camera.project( pt_cam, compute_jacobians=True) residual = np.dot(self.stiffness, predicted_obs - self.obs) if compute_jacobians[0]: jacobians[0] = np.dot(self.stiffness, cam_jacobian.dot(SE3.odot(pt_cam))) if compute_jacobians[1]: jacobians[1] = np.dot( self.stiffness, cam_jacobian.dot(T_cam_w.rot.as_matrix())) return residual, jacobians residual = np.dot(self.stiffness, self.camera.project(pt_cam) - self.obs) return residual
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 proto_to_se3(proto: geometry_pb2.SE3Pose) -> SE3: rot = SE3.RotationType.from_quaternion(np.array([proto.rotation.w, proto.rotation.x, proto.rotation.y, proto.rotation.z]), ordering='wxyz') trans = np.array([ proto.position.x, proto.position.y, proto.position.z, ]) return SE3(rot=rot, trans=trans)
def __init__(self, data, pyrimage, pyrlevels, T_c_w=SE3.identity()): super().__init__(data, T_c_w) self.pyrlevels = pyrlevels """Number of pyramid levels to downsample""" self.compute_image_pyramid(pyrimage) """Image pyramid"""
def compute_trajectory(pose_vec, gt_traj, method='odom'): est_traj = [gt_traj[0]] cum_dist = [0] for i in range(0, pose_vec.shape[0]): #classically estimated traj dT = SE3.exp(pose_vec[i]) new_est = SE3.as_matrix( (dT.dot(SE3.from_matrix(est_traj[i], normalize=True).inv())).inv()) est_traj.append(new_est) cum_dist.append(cum_dist[i] + np.linalg.norm(dT.trans)) gt_traj_se3 = [SE3.from_matrix(T, normalize=True) for T in gt_traj] est_traj_se3 = [SE3.from_matrix(T, normalize=True) for T in est_traj] tm_est = TrajectoryMetrics(gt_traj_se3, est_traj_se3, convention='Twv') est_mean_trans, est_mean_rot = tm_est.mean_err() est_mean_rot = (est_mean_rot * 180 / np.pi).round(3) est_mean_trans = est_mean_trans.round(3) seg_lengths = list(range(100, 801, 100)) _, seg_errs_est = tm_est.segment_errors(seg_lengths, rot_unit='rad') print('trans. rel. err: {}, rot. rel. err: {}'.format( np.mean(tm_est.rel_errors()[0]), np.mean(tm_est.rel_errors()[1]))) rot_seg_err = (100 * np.mean(seg_errs_est[:, 2]) * 180 / np.pi).round(3) trans_seg_err = (np.mean(seg_errs_est[:, 1]) * 100).round(3) if np.isnan(trans_seg_err): max_dist = cum_dist[-1] - cum_dist[-1] % 100 + 1 - 100 print('max dist', max_dist) seg_lengths = list(range(100, int(max_dist), 100)) _, seg_errs_est = tm_est.segment_errors(seg_lengths, rot_unit='rad') rot_seg_err = (100 * np.mean(seg_errs_est[:, 2]) * 180 / np.pi).round(3) trans_seg_err = (np.mean(seg_errs_est[:, 1]) * 100).round(3) print("{} mean trans. error: {} | mean rot. error: {}".format( method, est_mean_trans, est_mean_rot)) print("{} mean Segment Errors: {} (trans, %) | {} (rot, deg/100m)".format( method, trans_seg_err, rot_seg_err)) errors = (est_mean_trans, est_mean_rot, trans_seg_err, rot_seg_err) return np.array(est_traj), np.array(gt_traj), errors, np.array(cum_dist)
def do_vo_mapping(basepath, seq, ref_cond, frames=None, outfile=None, rgb_dir='rgb'): ref_data = vkitti.LocalizationDataset(basepath, seq, ref_cond, frames=frames, rgb_dir=rgb_dir) test_im = ref_data.get_gray(0) camera = get_camera(seq, test_im) camera.maxdepth = 200. # Ground truth T_w_c_gt = [SE3.from_matrix(p, normalize=True) 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 = 3. # meters vo.keyframe_trans_thresh = 2. # meters vo.keyframe_rot_thresh = 15. * np.pi / 180. # rad vo.depth_stiffness = 1. / 0.01 # 1/meters vo.intensity_stiffness = 1. / 0.2 # 1/ (intensity is in [0,1] ) # vo.intensity_stiffness = 1. / 0.1 vo.use_motion_model_guess = True # vo.min_grad = 0.2 # vo.loss = HuberLoss(5.0) 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) depth[depth >= camera.maxdepth] = -1. 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 test_evaluate_stereo(self, stereo_residual): T_cam_w = SE3.exp([1, 2, 3, 4, 5, 6]) pt_good_w = T_cam_w.inv().dot( stereo_residual.camera.triangulate(stereo_residual.obs)) pt_bad_w = pt_good_w + [1, 1, 1] assert np.allclose(stereo_residual.evaluate([T_cam_w, pt_good_w]), np.zeros(3)) assert not np.allclose(stereo_residual.evaluate([T_cam_w, pt_bad_w]), np.zeros(3))
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 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 evolve_world_pose_tractor( self, world_pose_tractor: SE3, v_left: float, v_right: float, delta_t: float, ) -> SE3: tractor_pose_delta = self.compute_tractor_pose_delta( v_left, v_right, delta_t) return world_pose_tractor.dot(tractor_pose_delta)
def __init__(self, camera, first_pose=SE3.identity()): self.camera = camera """Camera model""" self.first_pose = first_pose """First pose""" self.keyframes = [] """List of keyframes""" self.T_c_w = [first_pose] """List of camera poses""" self.motion_options = Options() """Optimizer parameters for motion estimation""" # Default optimizer parameters for motion estimation self.motion_options.allow_nondecreasing_steps = True self.motion_options.max_nondecreasing_steps = 5 self.motion_options.min_cost_decrease = 0.99 self.motion_options.max_iters = 30 self.motion_options.num_threads = 1 self.motion_options.linesearch_max_iters = 0 self.pyrlevels = 4 """Number of image pyramid levels for coarse-to-fine optimization""" self.pyrlevel_sequence = list(range(self.pyrlevels)) self.pyrlevel_sequence.reverse() self.keyframe_trans_thresh = 3.0 # meters """Translational distance threshold to drop new keyframes""" self.keyframe_rot_thresh = 0.3 # rad """Rotational distance threshold to drop new keyframes""" self.intensity_stiffness = 1. / 0.01 """Photometric measurement stiffness""" self.depth_stiffness = 1. / 0.01 """Depth or disparity measurement stiffness""" self.min_grad = 0.1 """Minimum image gradient magnitude to use a given pixel""" self.depth_map_type = 'depth' """Is the depth map depth, inverse depth, disparity? ['depth','disparity'] supported""" self.mode = 'map' """Create new keyframes or localize against existing ones? ['map'|'track']""" self.use_motion_model_guess = True """Use constant motion model for initial guess.""" # self.loss = L2Loss() self.loss = HuberLoss(10.0) # self.loss = TukeyLoss(5.0) # self.loss = CauchyLoss(5.0) # self.loss = TDistributionLoss(5.0) # Kerl et al. ICRA 2013 # self.loss = TDistributionLoss(3.0) """Loss function""" self._make_pyramid_cameras()
def __init__(self, event_loop): self.event_loop = event_loop self.command_rate_hz = 50 self.command_period_seconds = 1.0 / self.command_rate_hz self.event_bus = get_event_bus('farm_ng.pose_vis_toy') self.odom_pose_tractor = SE3.identity() self.tractor_pose_wheel_left = SE3.exp( (0.0, 1.0, 0, 0, 0, 0)).dot(SE3.exp((0.0, 0.0, 0, -np.pi / 2, 0, 0))) self.tractor_pose_wheel_right = SE3.exp( (0.0, -1.0, 0, 0, 0, 0)).dot(SE3.exp((0.0, 0.0, 0, -np.pi / 2, 0, 0))) self.kinematic_model = TractorKinematics(default_config()) self.control_timer = Periodic( self.command_period_seconds, self.event_loop, self._command_loop, name='control_loop', )
def _command_loop(self, n_periods): pose_msg = NamedSE3Pose() left_speed = 1.0 right_speed = 0.5 self.odom_pose_tractor = self.kinematic_model.evolve_world_pose_tractor( self.odom_pose_tractor, left_speed, right_speed, n_periods * self.command_period_seconds, ) pose_msg.a_pose_b.CopyFrom(se3_to_proto(self.odom_pose_tractor)) pose_msg.frame_a = 'odometry/wheel' pose_msg.frame_b = 'tractor/base' self.event_bus.send(make_event('pose/tractor/base', pose_msg)) pose_msg = NamedSE3Pose() radius = self.kinematic_model.wheel_radius self.tractor_pose_wheel_left = self.tractor_pose_wheel_left.dot( SE3.exp((0, 0, 0, 0, 0, left_speed / radius * self.command_period_seconds * n_periods))) pose_msg.a_pose_b.CopyFrom(se3_to_proto(self.tractor_pose_wheel_left)) pose_msg.frame_a = 'tractor/base' pose_msg.frame_b = 'tractor/wheel/left' self.event_bus.send(make_event('pose/tractor/wheel/left', pose_msg)) pose_msg = NamedSE3Pose() self.tractor_pose_wheel_right = self.tractor_pose_wheel_right.dot( SE3.exp((0, 0, 0, 0, 0, right_speed / radius * self.command_period_seconds * n_periods))) pose_msg.a_pose_b.CopyFrom(se3_to_proto(self.tractor_pose_wheel_right)) pose_msg.frame_a = 'tractor/base' pose_msg.frame_b = 'tractor/wheel/right' self.event_bus.send(make_event('pose/tractor/wheel/right', pose_msg)) pose_msg = NamedSE3Pose() pose_msg.a_pose_b.position.z = 1.0 pose_msg.a_pose_b.position.y = 0.5 pose_msg.frame_a = 'tractor/base' pose_msg.frame_b = 'tractor/camera' self.event_bus.send(make_event('pose/tractor/camera', pose_msg))
def perform_ransac(self): """Main RANSAC Routine""" max_inliers = 0 # Select random ids for minimal sets rand_idx = np.random.randint(self.num_pts, size=(self.ransac_iters, self.num_min_set_pts)) pts_1_sample_stacked = self.pts_1[rand_idx] pts_2_sample_stacked = self.pts_2[rand_idx] # Parallel transform computation # Compute transforms in parallel # start = time.perf_counter() T_21_stacked = compute_transform_fast(pts_1_sample_stacked, pts_2_sample_stacked, SE3_SHAPE) # end = time.perf_counter() # print('comp, transform | {}'.format(end - start)) # Parallel cost computation # start = time.perf_counter() # inlier_masks_stacked = compute_ransac_cost_fast( # T_21_stacked, self.pts_1, self.obs_2, cam_params, inlier_thresh) inlier_masks_stacked = self.compute_ransac_cost( T_21_stacked, self.pts_1, self.obs_2, self.camera, self.ransac_thresh) # end = time.perf_counter() # print('comp, masks | {}'.format(end - start)) # start = time.perf_counter() inlier_nums = np.sum(inlier_masks_stacked, axis=1) most_inliers_idx = np.argmax(inlier_nums) T_21_best = SE3.from_matrix(T_21_stacked[most_inliers_idx, :, :]) max_inliers = inlier_nums[most_inliers_idx] inlier_indices_best = np.where( inlier_masks_stacked[most_inliers_idx, :])[0] # end = time.perf_counter() # print('comp, rest | {}'.format(end - start)) if max_inliers < 5: raise ValueError( " RANSAC failed to find more than 5 inliers. Try adjusting the thresholds." ) # print('After {} RANSAC iters, found best transform with {} / {} inliers.'.format( # self.ransac_iters, max_inliers, self.num_pts)) obs_1_inliers = self.obs_1[inlier_indices_best] obs_2_inliers = self.obs_2[inlier_indices_best] return (T_21_best, obs_1_inliers, obs_2_inliers, inlier_indices_best)
def matlab_comparison(): data = sio.loadmat('simple_case.mat') T1 = data['T1'] T2 = data['T2'] T_v_rel = [SE3.from_matrix(T1[:, :, 0]), SE3.from_matrix(T1[:, :, 1])] T_c_rel = [SE3.from_matrix(T2[:, :, 0]), SE3.from_matrix(T2[:, :, 1])] my_solver = solver() my_solver.set_T_v_rel(T_v_rel) my_solver.set_T_c_rel(T_c_rel) dual_time, dual_gt, dual_primal, dual_gap, dual_opt, dual_solution, dual_flag = my_solver.dual_solve( 'R', verbose=True) rel_time, rel_gt, rel_primal, rel_gap, relax_opt, relax_solution, rel_flag = my_solver.relax_solve( 'R') print(1 / dual_opt[3]) print(1 / relax_opt[3]) print(dual_solution) print(relax_solution) return
def test_jacobians_stereo(self, stereo_residual): T_cam_w = SE3.exp([1, 2, 3, 4, 5, 6]) pt_w = T_cam_w.inv().dot( stereo_residual.camera.triangulate(stereo_residual.obs)) _, jac1 = stereo_residual.evaluate([T_cam_w, pt_w], compute_jacobians=[True, True]) _, jac2 = stereo_residual.evaluate([T_cam_w, pt_w], compute_jacobians=[True, False]) _, jac3 = stereo_residual.evaluate([T_cam_w, pt_w], compute_jacobians=[False, True]) assert len(jac1) == len(jac2) == len(jac3) == 2 assert jac1[0].shape == (3, 6) and jac1[1].shape == (3, 3) assert jac2[0].shape == (3, 6) and jac2[1] is None assert jac3[0] is None and jac3[1].shape == (3, 3)