def estimate_trajectory_householder(spline_template,
observed_accel_timestamps,
observed_accel_orientations,
observed_accel_readings,
observed_frame_timestamps,
observed_frame_orientations,
observed_features,
imu_to_camera=np.eye(3),
camera_matrix=np.eye(3),
accel_weight=1.):
assert isinstance(spline_template, spline.SplineTemplate)
num_tracks = max(f.track_id for f in observed_features) + 1
accel_bias_offset = spline_template.control_size
gravity_offset = spline_template.control_size + 3
structure_offset = spline_template.control_size + 6
num_vars = structure_offset + num_tracks * 3
j_blocks = []
r_blocks = []
# Add terms for accel residuals
print 'Constructing linear systems for %d accel readings...' % len(observed_accel_readings)
accel_coefficients = spline_template.coefficients_d2(observed_accel_timestamps)
for r, a, c in zip(observed_accel_orientations, observed_accel_readings, accel_coefficients):
amat = spline.diagify(c, 3)
j = np.zeros((3, num_vars))
j[:, :spline_template.control_size] = np.dot(r, amat)
j[:, gravity_offset:gravity_offset+3] = r
j[:, accel_bias_offset:accel_bias_offset+3] = np.eye(3)
j_blocks.append(j * accel_weight)
r_blocks.append(a * accel_weight)
# Add terms for features
print 'Constructing linear systems for %d features...' % len(observed_features)
pos_coefficients = spline_template.coefficients(observed_frame_timestamps)
pos_multidim_coefs = [spline.diagify(x, 3) for x in pos_coefficients]
for feature in observed_features:
z = calibrated(feature.position, camera_matrix)
h = householder(z)
r = observed_frame_orientations[feature.frame_id]
pmat = pos_multidim_coefs[feature.frame_id]
point_offset = structure_offset + feature.track_id*3
j = np.zeros((2, num_vars))
j[:, :spline_template.control_size] = -np.dot(h, np.dot(imu_to_camera, np.dot(r, pmat)))
j[:, point_offset:point_offset+3] = np.dot(h, np.dot(imu_to_camera, r))
j_blocks.append(j)
r_blocks.append(np.zeros(2))
# Assemble full linear system
j = np.vstack(j_blocks)
r = np.hstack(r_blocks)
# Eliminate global position
j = j[:, 3:]
# Solve
print 'Solving linear system of size %d x %d' % j.shape
solution, _, _, _ = np.linalg.lstsq(j, r)
# Replace global position
solution = np.hstack((np.zeros(3), solution))
# Extract individual variables from solution
position_controls = solution[:spline_template.control_size].reshape((-1, 3))
position_curve = spline.Spline(spline_template, position_controls)
gravity = solution[gravity_offset:gravity_offset+3]
accel_bias = solution[accel_bias_offset:accel_bias_offset+3]
landmarks = solution[structure_offset:].reshape((-1, 3))
return structures.PositionEstimate(position_curve, gravity, accel_bias, landmarks)
def construct_problem_inf(spline_template,
observed_accel_timestamps,
observed_accel_orientations,
observed_accel_readings,
observed_frame_timestamps,
observed_frame_orientations,
observed_features,
imu_to_camera=np.eye(3),
camera_matrix=np.eye(3),
feature_tolerance=1e-2,
accel_tolerance=1e-3,
gravity_magnitude=9.8,
max_bias_magnitude=.1):
# Sanity checks
assert isinstance(spline_template, spline.SplineTemplate)
assert len(observed_accel_orientations) == len(observed_accel_readings)
assert len(observed_accel_timestamps) == len(observed_accel_readings)
assert len(observed_frame_timestamps) == len(observed_frame_orientations)
assert all(0 <= f.frame_id < len(observed_frame_timestamps) for f in observed_features)
assert np.ndim(observed_accel_timestamps) == 1
assert np.ndim(observed_frame_timestamps) == 1
# Compute offsets
position_offset = 0
position_len = spline_template.control_size
gravity_offset = position_offset + position_len
accel_bias_offset = gravity_offset + 3
structure_offset = accel_bias_offset + 3
track_ids = set(f.track_id for f in observed_features)
num_frames = len(observed_frame_timestamps)
num_tracks = max(track_ids) + 1
num_vars = structure_offset + num_tracks * 3
# Make sure each track has at least one observation
counts_by_frame = np.zeros(num_frames, int)
counts_by_track = np.zeros(num_tracks, int)
for f in observed_features:
counts_by_frame[f.frame_id] += 1
counts_by_track[f.track_id] += 1
assert np.all(counts_by_frame > 0),\
'These frames had zero features: ' + ','.join(map(str, np.flatnonzero(counts_by_frame == 0)))
assert np.all(counts_by_track > 0),\
'These tracks had zero features: ' + ','.join(map(str, np.flatnonzero(counts_by_track == 0)))
# Track IDs should be exactly 0..n-1
assert all(track_id < num_tracks for track_id in track_ids)
# Initialize the problem
objective = np.zeros(num_vars)
problem = socp.SocpProblem(objective, [])
# Construct gravity constraints
a_gravity = np.zeros((3, num_vars))
a_gravity[:, gravity_offset:gravity_offset+3] = np.eye(3)
d_gravity = gravity_magnitude
problem.add_constraint(a=a_gravity, d=d_gravity)
# Construct accel bias constraints
a_bias = np.zeros((3, num_vars))
a_bias[:, accel_bias_offset:accel_bias_offset+3] = np.eye(3)
d_bias = max_bias_magnitude
problem.add_constraint(a=a_bias, d=d_bias)
# Construct accel constraints
print 'Constructing constraints for %d accel readings...' % len(observed_accel_readings)
accel_coefficients = spline_template.coefficients_d2(observed_accel_timestamps)
for r, a, c in zip(observed_accel_orientations, observed_accel_readings, accel_coefficients):
amat = spline.diagify(c, 3)
j = np.zeros((3, num_vars))
j[:, :position_len] = np.dot(r, amat)
j[:, gravity_offset:gravity_offset+3] = r
j[:, accel_bias_offset:accel_bias_offset+3] = np.eye(3)
r = -a
problem.add_constraint(a=j, b=r, d=accel_tolerance)
# Construct vision constraints
print 'Constructing constraints for %d features...' % len(observed_features)
pos_coefficients = spline_template.coefficients(observed_frame_timestamps)
pos_multidim_coefs = [spline.diagify(x, 3) for x in pos_coefficients]
for feature in observed_features:
r = observed_frame_orientations[feature.frame_id]
pmat = pos_multidim_coefs[feature.frame_id]
point_offset = structure_offset + feature.track_id*3
assert point_offset + 3 <= num_vars, 'track id was %d, num vars was %d' % (feature.track_id, num_vars)
k_rc_r = np.dot(camera_matrix, np.dot(imu_to_camera, r))
ymat = np.zeros((3, num_vars))
ymat[:, :position_len] = -np.dot(k_rc_r, pmat)
ymat[:, point_offset:point_offset+3] = k_rc_r
a_feature = ymat[:2] - np.outer(feature.position, ymat[2])
c_feature = ymat[2] * feature_tolerance
problem.add_constraint(a=a_feature, c=c_feature)
return problem
def construct_problem_mixed(spline_template,
observed_accel_timestamps,
observed_accel_orientations,
observed_accel_readings,
observed_frame_timestamps,
observed_frame_orientations,
observed_features,
imu_to_camera=np.eye(3),
camera_matrix=np.eye(3),
feature_tolerance=1e-2,
gravity_magnitude=9.8,
max_bias_magnitude=.1):
if len(observed_features) < 5:
raise InsufficientObservationsError()
# Sanity checks
assert isinstance(spline_template, spline.SplineTemplate)
assert len(observed_accel_orientations) == len(observed_accel_readings)
assert len(observed_accel_timestamps) == len(observed_accel_readings)
assert len(observed_frame_timestamps) == len(observed_frame_orientations)
assert all(0 <= f.frame_id < len(observed_frame_timestamps) for f in observed_features)
assert np.ndim(observed_accel_timestamps) == 1
assert np.ndim(observed_frame_timestamps) == 1
# Compute offsets
position_offset = 0
position_len = spline_template.control_size
gravity_offset = position_offset + position_len
accel_bias_offset = gravity_offset + 3
structure_offset = accel_bias_offset + 3
track_ids = set(f.track_id for f in observed_features)
num_aux_vars = 1 # one extra variable representing the objective
num_frames = len(observed_frame_timestamps)
num_tracks = max(track_ids) + 1
num_vars = structure_offset + num_tracks * 3 + num_aux_vars
# Make sure each track has at least one observation
counts_by_frame = np.zeros(num_frames, int)
counts_by_track = np.zeros(num_tracks, int)
for f in observed_features:
counts_by_frame[f.frame_id] += 1
counts_by_track[f.track_id] += 1
if not np.all(counts_by_frame > 0):
raise InsufficientObservationsError(
'These frames had zero features: ' + ','.join(map(str, np.flatnonzero(counts_by_frame == 0))))
if not np.all(counts_by_track > 0):
raise InsufficientObservationsError(
'These tracks had zero features: ' + ','.join(map(str, np.flatnonzero(counts_by_track == 0))))
# Track IDs should be exactly 0..n-1
assert all(track_id < num_tracks for track_id in track_ids)
# Initialize the problem
objective = utils.unit(num_vars-1, num_vars) # the last variable is the objective we minimize
problem = socp.SocpProblem(objective)
# Construct accel constraints
print 'Constructing constraints for %d accel readings...' % len(observed_accel_readings)
accel_coefficients = spline_template.coefficients_d2(observed_accel_timestamps)
accel_j_blocks = []
accel_r_blocks = []
for r, a, c in zip(observed_accel_orientations, observed_accel_readings, accel_coefficients):
amat = spline.diagify(c, 3)
j = np.zeros((3, num_vars))
j[:, :position_len] = np.dot(r, amat)
j[:, gravity_offset:gravity_offset+3] = r
j[:, accel_bias_offset:accel_bias_offset+3] = np.eye(3)
accel_j_blocks.append(j)
accel_r_blocks.append(a)
# Form the least squares objective || J*x + r ||^2
accel_j = np.vstack(accel_j_blocks)
accel_r = np.hstack(accel_r_blocks)
# Form the quadratic objective: x' J' J x + b' x + c <= objective ("objective" is the variable we minimize)
accel_c = np.dot(accel_r, accel_r)
accel_b = -2. * np.dot(accel_j.T, accel_r)
accel_b[-1] = -1.
# Convert to an SOCP objective
problem.add_constraint(*soc_constraint_from_quadratic_constraint(accel_j, accel_b, accel_c))
# Construct gravity constraints
a_gravity = np.zeros((3, num_vars))
a_gravity[:, gravity_offset:gravity_offset+3] = np.eye(3)
d_gravity = gravity_magnitude
problem.add_constraint(a=a_gravity, d=d_gravity)
# Construct accel bias constraints
a_bias = np.zeros((3, num_vars))
a_bias[:, accel_bias_offset:accel_bias_offset+3] = np.eye(3)
d_bias = max_bias_magnitude
problem.add_constraint(a=a_bias, d=d_bias)
# Construct vision constraints
print 'Constructing constraints for %d features...' % len(observed_features)
pos_coefficients = spline_template.coefficients(observed_frame_timestamps)
pos_multidim_coefs = [spline.diagify(x, 3) for x in pos_coefficients]
for feature in observed_features:
r = observed_frame_orientations[feature.frame_id]
pmat = pos_multidim_coefs[feature.frame_id]
point_offset = structure_offset + feature.track_id*3
assert point_offset + 3 <= num_vars, 'track id was %d, num vars was %d' % (feature.track_id, num_vars)
k_rc_r = np.dot(camera_matrix, np.dot(imu_to_camera, r))
ymat = np.zeros((3, num_vars))
ymat[:, :position_len] = -np.dot(k_rc_r, pmat)
ymat[:, point_offset:point_offset+3] = k_rc_r
a_feature = ymat[:2] - np.outer(feature.position, ymat[2])
c_feature = ymat[2] * feature_tolerance
problem.add_constraint(a=a_feature, c=c_feature)
return problem
