inverse_depth.py 文件源码

def linearity_index_xyz_parameterization():
    """Linearity index of XYZ Parameterization"""
    d, d1, alpha, sigma_d = sympy.symbols("d,d1,alpha,sigma_d")
    alpha = sympy.symbols("alpha")
    u = (d * sympy.sin(alpha)) / (d1 + d * sympy.cos(alpha))

    # first order derivative of u
    u_p = sympy.diff(u, d)
    u_p = sympy.simplify(u_p)

    # second order derivative of u
    u_pp = sympy.diff(u_p, d)
    u_pp = sympy.simplify(u_pp)

    # Linearity index
    L = (u_pp * 2 * sigma_d) / (u_p)
    L = sympy.simplify(L)

    print()
    print("u: ", u)
    print("u': ", u_p)
    print("u'': ", u_pp)
    # print("L = ", L)
    print("L = ", L.subs(d, 0))
    print()


# def derive_J_jacobian():
#     r_W_C = sympy.symbols("r_W_C")
#
#     y =