Example #1
0
    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)
Example #2
0
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