def test_zero_offset_bezier_mat(self):
bezier_order = 4
t = .5
f = lambda x: bezier.zero_offset_bezier(x.reshape((bezier_order, 3)), t)
J_numeric = numdifftools.Jacobian(f)(np.zeros(bezier_order*3))
J_analytic = bezier.zero_offset_bezier_mat(t, bezier_order, 3)
np.testing.assert_array_almost_equal(J_numeric, J_analytic)
def epipolar_jacobian(bezier_order, ti, tj, zi, zj, Ri, Rj):
Rrel = np.dot(Rj, Ri.T)
zzt = np.outer(zj, zi).flatten()
Ai = bezier.zero_offset_bezier_mat(ti, bezier_order, 3)
Aj = bezier.zero_offset_bezier_mat(tj, bezier_order, 3)
return dots(zzt, diagify(Rrel, 3), skew_jacobian(), np.dot(Ri, Aj - Ai))
def reprojection_jacobian(bezier_order, timestamp, feature, orientation):
bezier_mat = zero_offset_bezier_mat(timestamp, bezier_order, 3)
return -np.dot(orientation, bezier_mat), orientation, -feature
def construct_problem(bezier_degree,
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):
# Compute offsets
position_offset = 0
position_len = (bezier_degree-1)*3
accel_bias_offset = position_offset + position_len
gravity_offset = accel_bias_offset + 3
structure_offset = gravity_offset + 3
num_vars = structure_offset + 3 * observed_features.shape[1]
# 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
for t, r, a in zip(observed_accel_timestamps, observed_accel_orientations, observed_accel_readings):
amat = bezier.zero_offset_bezier_second_deriv_mat(t, bezier_degree-1, 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 structure constraints
for i, (t, r, zs) in enumerate(zip(observed_frame_timestamps, observed_frame_orientations, observed_features)):
for j, z in enumerate(zs):
point_offset = structure_offset + j*3
pmat = bezier.zero_offset_bezier_mat(t, bezier_degree-1, 3)
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(z, ymat[2])
c_feature = ymat[2] * feature_tolerance
problem.add_constraint(a=a_feature, c=c_feature)
return problem