camera_utils.py 文件源码

def sampson_error(F, pts1, pts2): 
    """
    Computes the sampson error for F, and points pts1, pts2. Sampson
    error is the first order approximation to the geometric error.
    Remember that this is a squared error.

    (x'^{T} * F * x)^2
    -----------------
    (F * x)_1^2 + (F * x)_2^2 + (F^T * x')_1^2 + (F^T * x')_2^2

    where (F * x)_i^2 is the square of the i-th entry of the vector Fx
    """
    x1, x2 = unproject_points(pts1).T, unproject_points(pts2).T
    Fx1 = np.dot(F, x1)
    Fx2 = np.dot(F, x2)

    # Sampson distance as error measure
    denom = Fx1[0]**2 + Fx1[1]**2 + Fx2[0]**2 + Fx2[1]**2
    return ( np.diag(x1.T.dot(Fx2)) )**2 / denom