Python Polynomial.partial_derivatives方法代码示例

# 需要导入模块: from polynomial import Polynomial [as 别名]
# 或者: from polynomial.Polynomial import partial_derivatives [as 别名]
def run_spline_epipolar():
    # Construct symbolic problem
    num_landmarks = 10
    num_frames = 3
    num_imu_readings = 8
    bezier_degree = 4
    out = 'out/epipolar_accel_bezier3'

    if not os.path.isdir(out):
        os.mkdir(out)

    # Both splines should start at 0,0,0
    frame_times = np.linspace(0, .9, num_frames)
    imu_times = np.linspace(0, 1, num_imu_readings)

    true_rot_controls = np.random.rand(bezier_degree-1, 3)
    true_pos_controls = np.random.rand(bezier_degree-1, 3)

    true_landmarks = np.random.randn(num_landmarks, 3)
    true_cayleys = np.array([zero_offset_bezier(true_rot_controls, t) for t in frame_times])
    true_positions = np.array([zero_offset_bezier(true_pos_controls, t) for t in frame_times])

    true_accels = np.array([zero_offset_bezier_second_deriv(true_pos_controls, t) for t in imu_times])

    true_qs = map(cayley_mat, true_cayleys)
    true_rotations = map(cayley, true_cayleys)

    true_uprojections = [[np.dot(R, x-p) for x in true_landmarks]
                         for R,p in zip(true_rotations, true_positions)]

    true_projections = [[normalized(zu) for zu in row] for row in true_uprojections]

    p0 = true_positions[0]
    q0 = true_qs[0]
    for i in range(1, num_frames):
        p = true_positions[i]
        q = true_qs[i]
        E = essential_matrix(q0, p0, q, p)
        for j in range(num_landmarks):
            z = true_projections[i][j]
            z0 = true_projections[0][j]
            #print np.dot(z, np.dot(E, z0))

    # construct symbolic versions of the above
    s_offs = 0
    p_offs = s_offs + (bezier_degree-1)*3
    num_vars = p_offs + (bezier_degree-1)*3

    sym_vars = [Polynomial.coordinate(i, num_vars, Fraction) for i in range(num_vars)]
    sym_rot_controls = np.reshape(sym_vars[s_offs:s_offs+(bezier_degree-1)*3], (bezier_degree-1, 3))
    sym_pos_controls = np.reshape(sym_vars[p_offs:p_offs+(bezier_degree-1)*3], (bezier_degree-1, 3))

    true_vars = np.hstack((true_rot_controls.flatten(),
                           true_pos_controls.flatten()))

    residuals = []

    # Accel residuals
    for i in range(num_imu_readings):
        sym_a = zero_offset_bezier_second_deriv(sym_pos_controls, imu_times[i])
        residual = sym_a - true_accels[i]
        residuals.extend(residual)

    # Epipolar residuals
    p0 = np.zeros(3)
    R0 = np.eye(3)
    for i in range(1, num_frames):
        sym_s = zero_offset_bezier(sym_rot_controls, frame_times[i])
        sym_p = zero_offset_bezier(sym_pos_controls, frame_times[i])
        sym_q = cayley_mat(sym_s)
        #sym_q = np.eye(3) * (1. - np.dot(sym_s, sym_s)) + 2.*skew(sym_s) + 2.*np.outer(sym_s, sym_s)
        sym_E = essential_matrix(R0, p0, sym_q, sym_p)
        for j in range(num_landmarks):
            z = true_projections[i][j]
            z0 = true_projections[0][j]
            residual = np.dot(z, np.dot(sym_E, z0))
            residuals.append(residual)

    print 'Num vars:',num_vars
    print 'Num residuals:',len(residuals)

    print 'Residuals:', len(residuals)
    cost = Polynomial(num_vars)
    for r in residuals:
        cost += r*r
        print '  %f   (degree=%d, length=%d)' % (r(*true_vars), r.total_degree, len(r))

    print '\nCost:'
    print '  Num terms: %d' % len(cost)
    print '  Degree: %d' % cost.total_degree

    print '\nGradients:'
    gradients = cost.partial_derivatives()
    for gradient in gradients:
        print '  %d  (degree=%d, length=%d)' % (gradient(*true_vars), gradient.total_degree, len(gradient))

    jacobians = [r.partial_derivatives() for r in residuals]

    J = np.array([[J_ij(*true_vars) for J_ij in row] for row in jacobians])

#.........这里部分代码省略.........

# 需要导入模块: from polynomial import Polynomial [as 别名]
# 或者: from polynomial.Polynomial import partial_derivatives [as 别名]

#.........这里部分代码省略.........
    assert len(true_vars) == len(sym_vars)

    residuals = []

    #
    # Accel residuals
    #
    print '\nAccel residuals:'
    for i in range(num_imu_readings):
        true_R = true_imu_rotations[i]
        sym_global_accel = zero_offset_bezier_second_deriv(sym_pos_controls, imu_times[i])
        sym_accel = np.dot(true_R, sym_global_accel + sym_gravity) + sym_accel_bias
        residual = sym_accel - true_accels[i]
        for i in range(3):
            print '  Degree of global accel = %d, local accel = %d, residual = %d' % \
                  (sym_global_accel[i].total_degree, sym_accel[i].total_degree, residual[i].total_degree)
        residuals.extend(residual)

    #
    # Epipolar residuals
    #
    p0 = np.zeros(3)
    R0 = np.eye(3)
    for i in range(1, num_frames):
        true_s = true_cayleys[i]
        true_R = cayley_mat(true_s)
        sym_p = zero_offset_bezier(sym_pos_controls, frame_times[i])
        sym_E = essential_matrix(R0, p0, true_R, sym_p)
        for j in range(num_landmarks):
            z = true_projections[i][j]
            z0 = true_projections[0][j]
            residual = np.dot(z, np.dot(sym_E, z0))
            residuals.append(residual)

    print '\nNum vars:', num_vars
    print 'Num residuals:', len(residuals)

    print '\nResiduals:', len(residuals)
    cost = Polynomial(num_vars)
    for r in residuals:
        cost += r*r
        print '  %f   (degree=%d, length=%d)' % (r(*true_vars), r.total_degree, len(r))

    print '\nCost:'
    print '  Num terms: %d' % len(cost)
    print '  Degree: %d' % cost.total_degree
    for term in cost:
        print '    ',term

    print '\nGradients:'
    gradients = cost.partial_derivatives()
    for gradient in gradients:
        print '  %d  (degree=%d, length=%d)' % (gradient(*true_vars), gradient.total_degree, len(gradient))

    jacobians = np.array([r.partial_derivatives() for r in residuals])

    J = evaluate_array(jacobians, *true_vars)

    U, S, V = np.linalg.svd(J)

    print '\nJacobian singular values:'
    print J.shape
    print S

    print '\nHessian eigenvalues:'
    H = np.dot(J.T, J)
    print H.shape
    print np.linalg.eigvals(H)

    null_space_dims = sum(np.abs(S) < 1e-5)
    print '\nNull space dimensions:', null_space_dims
    if null_space_dims > 0:
        for i in range(null_space_dims):
            print '  ',V[-i]

    null_monomial = (0,) * num_vars
    coordinate_monomials = [list(var.monomials)[0] for var in sym_vars]
    A, _ = matrix_form(gradients, coordinate_monomials)
    b, _ = matrix_form(gradients, [null_monomial])
    b = np.squeeze(b)

    AA, bb, kk = quadratic_form(cost)

    estimated_vars = np.squeeze(numpy.linalg.solve(AA*2, -b))

    print '\nEstimated:'
    print estimated_vars

    print '\nGround truth:'
    print true_vars

    print '\nError:'
    print np.linalg.norm(estimated_vars - true_vars)

    # Output to file
    write_polynomials(cost, out+'/cost.txt')
    write_polynomials(residuals, out+'/residuals.txt')
    write_polynomials(gradients, out+'/gradients.txt')
    write_polynomials(jacobians.flat, out+'/jacobians.txt')
    write_solution(true_vars, out+'/solution.txt')

# 需要导入模块: from polynomial import Polynomial [as 别名]
# 或者: from polynomial.Polynomial import partial_derivatives [as 别名]

#.........这里部分代码省略.........
    num_accel_bias_vars = 3
    num_gravity_vars = 3

    block_sizes = [num_position_vars, num_orientation_vars, num_accel_bias_vars, num_gravity_vars]
    num_vars = sum(block_sizes)

    sym_vars = [Polynomial.coordinate(i, num_vars, Fraction) for i in range(num_vars)]
    sym_pos_controls, sym_orient_controls, sym_accel_bias, sym_gravity = map(np.array, chop(sym_vars, block_sizes))

    sym_pos_controls = sym_pos_controls.reshape((-1, 3))
    sym_orient_controls = sym_orient_controls.reshape((-1, 3))

    assert len(true_vars) == len(sym_vars)

    #
    # Accel residuals
    #
    residuals = []

    print 'Accel residuals:'
    for i, t in enumerate(accel_times):
        sym_cayley = zero_offset_bezier(sym_orient_controls, t)
        sym_orient = cayley_mat(sym_cayley)
        sym_denom = cayley_denom(sym_cayley)
        sym_global_accel = zero_offset_bezier_second_deriv(sym_pos_controls, t)
        sym_accel = np.dot(sym_orient, sym_global_accel + sym_gravity) + sym_denom * sym_accel_bias
        residual = sym_accel - sym_denom * observed_accels[i]
        residuals.extend(residual)
        for r in residual:
            print '  %f   (degree=%d, length=%d)' % (r(*true_vars), r.total_degree, len(r))

    #
    # Epipolar residuals
    #

    print 'Epipolar residuals:'
    for i, ti in enumerate(frame_times):
        if i == 0: continue
        sym_Ri = cayley_mat(zero_offset_bezier(sym_orient_controls, ti))
        sym_pi = zero_offset_bezier(sym_pos_controls, ti)
        sym_E = essential_matrix_from_relative_pose(sym_Ri, sym_pi)
        for k in range(num_landmarks):
            z1 = observed_features[0][k]
            zi = observed_features[i][k]
            residual = np.dot(zi, np.dot(sym_E, z1))
            residuals.append(residual)
            r = residual
            print '  %f   (degree=%d, length=%d)' % (r(*true_vars), r.total_degree, len(r))

    #
    # Construct cost
    #

    cost = Polynomial(num_vars)
    for r in residuals:
        cost += r*r

    gradients = cost.partial_derivatives()

    print '\nNum vars:', num_vars
    print 'Num residuals:', len(residuals)
    print '\nCost:'
    print '  Num terms: %d' % len(cost)
    print '  Degree: %d' % cost.total_degree


    #
    # Output to file
    #
    write_polynomials(cost, out+'/cost.txt')
    write_polynomials(residuals, out+'/residuals.txt')
    write_polynomials(gradients, out+'/gradients.txt')
    write_solution(true_vars, out+'/solution.txt')

    np.savetxt(out+'/feature_measurements.txt', observed_features.reshape((-1, 3)))
    np.savetxt(out+'/accel_measurements.txt', observed_accels)
    np.savetxt(out+'/problem_size.txt', [num_frames, num_landmarks, num_imu_readings])
    np.savetxt(out+'/frame_times.txt', frame_times)
    np.savetxt(out+'/accel_times.txt', accel_times)

    np.savetxt(out+'/true_pos_controls.txt', true_pos_controls)
    np.savetxt(out+'/true_orient_controls.txt', true_orient_controls)
    np.savetxt(out+'/true_accel_bias.txt', true_accel_bias)
    np.savetxt(out+'/true_gravity.txt', true_gravity)


    return

    #
    # Plot
    #
    fig = plt.figure(figsize=(14,6))
    ax = fig.add_subplot(1, 2, 1, projection='3d')

    ts = np.linspace(0, 1, 100)
    true_ps = np.array([zero_offset_bezier(true_pos_controls, t) for t in ts])

    ax.plot(true_ps[:, 0], true_ps[:, 1], true_ps[:, 2], '-b')

    plt.show()