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 __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 __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 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 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 __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 _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_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 __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.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.matcher_params = viso2.Matcher_parameters() """Parameters for libviso2 matcher""" self.matcher = viso2.Matcher(self.matcher_params) """libviso2 matcher""" self.matcher_mode = 0 """libviso2 matching mode 0=flow 1=stereo 2=quad""" self.ransac = FrameToFrameRANSAC(self.camera) """RANSAC outlier rejection""" self.reprojection_stiffness = np.diag([1., 1., 1.]) """Reprojection error stiffness matrix""" self.mode = 'map' """Create new keyframes or localize against existing ones? ['map'|'track']""" self.loss = L2Loss() """Loss function"""
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 se3_residual(self): from pyslam.residuals import PoseToPoseResidual return PoseToPoseResidual(SE3.identity(), np.eye(6))
def __init__(self, data, T_c_w=SE3.identity()): self.data = data """Image data (tuple or list)""" self.T_c_w = T_c_w """Keyframe pose, world-to-camera."""
def __init__(self, im_left, im_right, pyrlevels=0, T_c_w=SE3.identity()): super().__init__((im_left, im_right), im_left, pyrlevels, T_c_w)
def __init__(self, image, depth, pyrlevels=0, T_c_w=SE3.identity()): super().__init__((image, depth), image, pyrlevels, T_c_w)
def __init__(self, image, depth, T_c_w=SE3.identity()): super().__init__((image, depth), T_c_w)
def __init__(self, camera, first_pose=SE3.identity()): super().__init__(camera, first_pose) self.matcher_mode = 2 # stereo quad matching self.matcher.setIntrinsics(camera.fu, camera.cu, camera.cv, camera.b)
def __init__(self, event_bus: EventBus): self.command_rate_hz = 50 self.command_period_seconds = 1.0 / self.command_rate_hz self.n_cycle = 0 # self.record_counter = 0 # self.recording = False self.event_bus = event_bus self.event_bus.add_subscriptions(['pose/tractor/base/goal']) self.event_bus.add_event_callback(self._on_event) self.lock_out = False self.can_socket = CANSocket('can0', self.event_bus.event_loop()) self.steering = SteeringClient(self.event_bus) self.tractor_state = TractorState() self.odom_poses_tractor = TimeSeries(1.0) self.odom_pose_tractor = SE3.identity() self.config = TractorConfigManager.saved() self.kinematics = TractorKinematics(tractor_config=self.config) self.move_to_goal_controller = TractorMoveToGoalController() radius = self.config.wheel_radius.value gear_ratio = self.config.hub_motor_gear_ratio.value poll_pairs = self.config.hub_motor_poll_pairs.value self.right_motor = HubMotor( 'right_motor', radius, gear_ratio, poll_pairs, 7, self.can_socket, ) self.left_motor = HubMotor( 'left_motor', radius, gear_ratio, poll_pairs, 9, self.can_socket, ) if self.config.topology == TractorConfig.TOPOLOGY_FOUR_MOTOR_SKID_STEER: logger.info('Four Motor Skid Steer Mode') self.right_motor_aft = HubMotor( 'right_motor_aft', radius, gear_ratio, poll_pairs, 8, self.can_socket, ) self.left_motor_aft = HubMotor( 'left_motor_aft', radius, gear_ratio, poll_pairs, 10, self.can_socket, ) self.control_timer = Periodic( self.command_period_seconds, self.event_bus.event_loop(), self._command_loop, name='control_loop', ) self._last_odom_stamp = None
def __init__(self, camera, first_pose=SE3.identity()): super().__init__(camera, first_pose) self.depth_map_type = 'depth' self.depth_stiffness = 1 / 0.01
def __init__(self, event_bus): self.event_loop = asyncio.get_event_loop() self.command_rate_hz = 50 self.command_period_seconds = 1.0 / self.command_rate_hz self.n_cycle = 0 # self.record_counter = 0 # self.recording = False self.event_bus = event_bus self.event_bus.add_event_callback(self._on_event) self.lock_out = False self.can_socket = CANSocket('can0', self.event_loop) self.steering = SteeringClient(self.event_bus) self.tractor_state = TractorState() self.odom_poses_tractor = TimeSeries(1.0) self.odom_pose_tractor = SE3.identity() # TODO(ethanrublee|isherman) load from disk somehow. self.config = default_config() self.kinematics = TractorKinematics(tractor_config=self.config) self.move_to_goal_controller = TractorMoveToGoalController() radius = self.config.wheel_radius.value gear_ratio = self.config.hub_motor_gear_ratio.value poll_pairs = self.config.hub_motor_poll_pairs.value self.right_motor = HubMotor( 'right_motor', radius, gear_ratio, poll_pairs, 7, self.can_socket, ) self.left_motor = HubMotor( 'left_motor', radius, gear_ratio, poll_pairs, 9, self.can_socket, ) # TODO(isherman|ethanruble) add to configuration system # to enable four motors, just do: # $ mkdir -p /home/farmer/tractor-data/config # $ touch /home/farmer/tractor-data/config/HAS_FOUR_WHEELS has_four_motors = os.path.exists( '/home/farmer/tractor-data/config/HAS_FOUR_WHEELS') self.config.topology = TractorConfig.TOPOLOGY_TWO_MOTOR_DIFF_DRIVE if has_four_motors: logger.info('Four Motor Skid Steer Mode') self.config.topology = TractorConfig.TOPOLOGY_FOUR_MOTOR_SKID_STEER self.right_motor_aft = HubMotor( 'right_motor_aft', radius, gear_ratio, poll_pairs, 8, self.can_socket, ) self.left_motor_aft = HubMotor( 'left_motor_aft', radius, gear_ratio, poll_pairs, 10, self.can_socket, ) self.control_timer = Periodic( self.command_period_seconds, self.event_loop, self._command_loop, name='control_loop', ) self._last_odom_stamp = None
from pyslam.problem import Options, Problem from pyslam.utils import invsqrt # Reproducibility np.random.seed(42) # Landmarks (world frame is first camera frame) pts_w_GT = [ np.array([0., -1., 10.]), np.array([1., 1., 5.]), np.array([-1., 1., 15.]) ] # Trajectory T_cam_w_GT = [ SE3.identity(), SE3.exp(0.1 * np.ones(6)), SE3.exp(0.2 * np.ones(6)), SE3.exp(0.3 * np.ones(6)) ] # Camera camera = StereoCamera(640, 480, 1000, 1000, 0.25, 1280, 960) # Observations obs_var = [1, 1, 2] # [u,v,d] obs_stiffness = invsqrt(np.diagflat(obs_var)) obs = [[camera.project(T.dot(p)) for p in pts_w_GT] for T in T_cam_w_GT]
def __init__(self, camera, first_pose=SE3.identity()): super().__init__(camera, first_pose) self.matcher_mode = 0 # mono-to-mono
# 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())
drive = '0016' frame_range = range(0, 2) dataset = pykitti.raw(basedir, date, drive, frames=frame_range) # Parameters to estimate 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(dataset.oxts[0].T_w_imu).inv()) T_0_w.normalize() T_1_w = T_cam0_imu.dot(SE3.from_matrix(dataset.oxts[1].T_w_imu).inv()) T_1_w.normalize() T_1_0_true = T_1_w.dot(T_0_w.inv()) # params_init = {'T_1_0': T_1_0_true} params_init = {'T_1_0': SE3.identity()} # Scaling parameters pyrlevels = [3, 2, 1] params = params_init options = Options() options.allow_nondecreasing_steps = True options.max_nondecreasing_steps = 3 options.min_cost_decrease = 0.99 # options.max_iters = 100 # options.print_iter_summary = True for pyrlevel in pyrlevels: pyrfactor = 1. / 2**pyrlevel
def __init__(self, im_left, im_right, T_c_w=SE3.identity()): super().__init__((im_left, im_right), T_c_w)