def normalized_dlt(match_list):
    """
    Calculates a homography using Direct Linear Transformation algorithm, normalizing
    image point sets independently.
    
    match_list: a list of point matches
        [[(x1, y1), (x1', y1')], [(x2, y2), (x2', y2')], ...]
    """
    if logger.isEnabledFor(DEBUG):
        logger.debug("Normalized DLT started: match list = %s", match_list)

    matches_count = len(match_list)

    points1, points2 = np.split(np.array(match_list), 2, axis=1)
    points1 = points1.reshape((matches_count, 2)).T
    points2 = points2.reshape((matches_count, 2)).T

    if logger.isEnabledFor(DEBUG):
        logger.debug("\nPoints1 =\n%s\nPoints2 =\n%s", points1, points2)

    T1 = similarity_transform_matr(points1)
    T2 = similarity_transform_matr(points2)
    points1 = np.dot(T1, point_transforms.nd_to_homogeneous(points1))
    points2 = np.dot(T2, point_transforms.nd_to_homogeneous(points2))

    if logger.isEnabledFor(DEBUG):
        logger.debug("\nPoints1 =\n%s\nPoints2 =\n%s", points1, points2)

    points1 = point_transforms.nd_to_cartesian(points1).T.reshape((matches_count, 1, 2))
    points2 = point_transforms.nd_to_cartesian(points2).T.reshape((matches_count, 1, 2))

    normalized_points = np.hstack((points1, points2))

    if logger.isEnabledFor(DEBUG):
        logger.debug("\nNormalized points: %s", normalized_points)

    H = dlt(normalized_points)
    return np.dot(np.linalg.inv(T2), np.dot(H, T1))
Beispiel #2
0
    def estimate(self):
        if logger.isEnabledFor(INFO):
            logger.info('=================================================================')
            logger.info("RANSAC estimation started, matches: %s, threshold: %s", 
                        self.__initial_matches, self.dist_threshold)
            
        matches = self.__initial_matches
        get_sample_points = self.__get_noncollinear_sample_points

        max_inlier_count = curr_std = 0
        threshold = self.dist_threshold
        matches_count = self.matches_count
        float_matches_count = float(matches_count)
        inliers = None
        best_h = None
        
        nd_matches = np.array(matches)
        points1, points2 = np.split(nd_matches, 2, axis=1)
        points1 = points1.reshape((matches_count, 2)).T
        points2 = points2.reshape((matches_count, 2)).T
        hg_points1 = pt.nd_to_homogeneous(points1)
        hg_points2 = pt.nd_to_homogeneous(points2)
        
        adaptive_thresh_enabled = self.adaptive_threshold_enabled
        thresh_change_freq = self.threshold_change_frequency
        thresh_change_step = self.threshold_change_step
        
        N = RansacHomographyEstimator.INITIAL_NUMBER_OF_SAMPLES
        p = RansacHomographyEstimator.P
        i = 1
        
        while i <= N:
            i = i + 1
            
            if adaptive_thresh_enabled and (i % thresh_change_freq == 0):
                threshold += thresh_change_step
             
            match_list = get_sample_points()         
            curr_h = normalized_dlt(match_list)
            inv_h = np.linalg.inv(curr_h)
            
            projected_points1 = pt.nd_to_cartesian(np.dot(curr_h, hg_points1))
            projected_points2 = pt.nd_to_cartesian(np.dot(inv_h, hg_points2))
            
            distances1 = pt.get_distances(projected_points1, points2)
            distances2 = pt.get_distances(projected_points2, points1)
            distances = distances1 + distances2
            
            inlier_indices = np.where(distances < threshold)[0]
            inlier_distances = distances[inlier_indices]
            curr_std = np.std(inlier_distances)            
            inlier_count = inlier_indices.shape[0]
            
            if (logger.isEnabledFor(DEBUG)):
                logger.debug('inlier count: {0}, max inlier count: {1}, matches_count = {2}'.format(
                            inlier_count, max_inlier_count, matches_count))
                
            if (inlier_count > max_inlier_count or 
                    (inlier_count == max_inlier_count and 
                     curr_std < RansacHomographyEstimator.MIN_STD)):
                max_inlier_count = inlier_count
                best_h = curr_h                
                inliers = [matches[i] for i in list(inlier_indices)]
            
            eps = 1.0 - inlier_count/float_matches_count
            if eps == 1.0:
                N = N + 1
                continue
            N = int(math.log(1.0-p)/math.log(1.0 - (1.0-eps)**4))                
            
            if (logger.isEnabledFor(DEBUG)):
                logger.debug('i = {0}, N = {1}'.format(i, N))
        # End while    
        self.__H = best_h
        self.__inliers = inliers
        
        if logger.isEnabledFor(INFO):
            logger.info('RANSAC estimation finished successfully, number of inliers: %s, final threshold: %s', 
                         len(self.inliers), threshold)
            logger.info('=================================================================')