homographies.py 文件源码

def compute_foes_svd_irls(matches_all_frames_, homographies_refined):
    """
    Compute initial set of FoEs.
    """

    matches_all_frames = filter_features(matches_all_frames_, homographies_refined)

    def estimate_foe_svd_norm_irls(points1, points2, init=None):
        """
        Estimate focus of expansion using least squares
        """
        A,b = points2system(points1[:,0],points1[:,1],points2[:,0],points2[:,1],norm=False)
        T = np.c_[A,-b]

        weights = np.ones((T.shape[0],1))

        mad = lambda x : 1.48 * np.median(np.abs(x - np.median(x)))

        for it in range(100):
            Tw = T * np.sqrt(weights)
            # Compute correction matrix
            M = np.array([[0.0,0.0,0.0],[0.0,0.0,0.0],[0.0,0.0,0.0]])
            for i,p1 in enumerate(points1):
                M += weights[i]**2 * np.array([[1.0,0.0,-p1[0]],[0.0,1.0,-p1[1]],[-p1[0],-p1[1],p1[0]**2 + p1[1]**2]])

            Mcor = sqrtm(np.linalg.inv(M))

            X = Mcor.dot(Tw.T.dot(Tw)).dot(Mcor)
            U,S,V = np.linalg.svd(X)

            err = T.dot(Mcor).dot(V[-1,:])
            sigma = mad(err)
            weights[:,0] = 1.0 / (1 + (err/sigma)**2)

            weights /= weights.sum()

        ep1 = V[2,:]
        ep1 = Mcor.dot(ep1)
        ep1 = ep1[:2] / ep1[2]
        return ep1

    epipoles = []
    n_frames = matches_all_frames.shape[2]
    for i in range(1,n_frames):
        pt1 = matches_all_frames[:,:,0]
        pt2 = matches_all_frames[:,:,i]
        H = homographies_refined[i-1]
        pt2_H = remove_homography(pt2,H)
        epipoles.append(estimate_foe_svd_norm_irls(pt1,pt2_H))

    return epipoles