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)
