Numerical-Analysis/ransac.py at master · SCKIMOSU/Numerical-Analysis · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
import numpy as np
import math
import matplotlib.pyplot as plt


def get_sampling_number(sample_size, p=0.99, e=0.5):
    '''
    sample size : Number of sample size per every iterations
    p : Desired probability of choosing at least one sample free of outliers
    e : Estimated probability that a point is an outlier
    '''
    # Calculate the required number of iterations based on the formula
    n_iterations_calculated = math.ceil(math.log(1 - p) / math.log(1 - (1 - e) ** sample_size))
    print(f"Calculated number of iterations: {n_iterations_calculated}")
    return n_iterations_calculated


def get_inlier_threshold(thresholds, data, polynomial_degree, sample_size,
                         min_iteration, max_iteration, stop_inlier_ratio, verbose=False):
    early_stop_flag = False
    inlier_threshold = None
    for threshold in thresholds:
        best_fit = None
        best_error = 0
        for i in range(max_iteration):
            # Randomly select sample points
            subset = data[np.random.choice(len(data), sample_size, replace=False)]
            x_sample, y_sample = subset[:, 0], subset[:, 1]

            # Fit a line to the sample points
            p = np.polyfit(x_sample, y_sample, polynomial_degree)

            # Compute error
            y_pred = np.polyval(p, X)
            error = np.abs(y - y_pred)

            # Count inliers
            inliers = error < threshold
            n_inliers = np.sum(inliers)

            # Update best fit if the current model is better
            if n_inliers > best_error:
                print("threshold : {}, index : {}, n_inliers : {}".format(threshold, i, n_inliers))
                best_fit = p
                best_error = n_inliers

                if (i > min_iteration) and (n_inliers / len(data)) >= stop_inlier_ratio:
                    early_stop_flag = True
                    inlier_threshold = threshold

            if early_stop_flag:
                break

        if verbose:
            # Best curve
            y_best = np.polyval(best_fit, X)

            # Plotting
            plt.scatter(X, y, label='Data Points')
            plt.plot(X, y_best, color='red', label='RANSAC Fit')
            plt.legend()
            plt.show()

        if early_stop_flag:
            break

    return inlier_threshold


def get_model_with_ransac(data, polynomial_degree, threshold, sample_size,
                          min_iteration, max_iteration, stop_inlier_ratio, verbose=False):
    early_stop_flag = False
    inlier_threshold = None
    best_fit = None
    best_error = 0
    for i in range(max_iteration):
        # Randomly select sample points
        subset = data[np.random.choice(len(data), sample_size, replace=False)]
        x_sample, y_sample = subset[:, 0], subset[:, 1]

        # Fit a line to the sample points
        p = np.polyfit(x_sample, y_sample, polynomial_degree)

        # Compute error
        y_pred = np.polyval(p, X)
        error = np.abs(y - y_pred)

        # Count inliers
        inliers = error < threshold
        n_inliers = np.sum(inliers)

        # Update best fit if the current model is better
        if n_inliers > best_error:
            best_fit = p
            best_error = n_inliers

            if (i > min_iteration) and (n_inliers / len(data)) >= stop_inlier_ratio:
                early_stop_flag = True
                inlier_threshold = threshold
                print("index : {}, n_inliers : {}".format(i, n_inliers))

        if early_stop_flag:
            break

    if verbose:
        # Best curve
        y_best = np.polyval(best_fit, X)

        # Plotting
        plt.scatter(X, y, label='Data Points')
        plt.plot(X, y_best, color='red', label='RANSAC Fit')
        plt.legend()
        plt.show()

    return early_stop_flag, best_fit


if __name__ == "__main__":
    # Generate synthetic data
    np.random.seed(0)
    n_points = 100
    X = np.linspace(0, 10, n_points)
    y = 3 * X + 10 + np.random.normal(0, 3, n_points)

    # Add outliers
    n_outliers = 20
    X[-n_outliers:] += int(30 * np.random.rand())
    y[-n_outliers:] -= int(50 * np.random.rand())
    X = np.expand_dims(X, -1)
    y = np.expand_dims(y, -1)
    data = np.hstack([X, y])

    threshold_cadidates = [1, 2, 4, 8, 16, 32, 64, 128]
    threshold_cadidates.sort()

    sample_size = 2
    max_iteration = get_sampling_number(sample_size)
    threshold = get_inlier_threshold(threshold_cadidates, data, polynomial_degree=1, sample_size=sample_size,
                                     min_iteration=-1, max_iteration=max_iteration, stop_inlier_ratio=0.50,
                                     verbose=True)


    # Generate synthetic data
    np.random.seed(0)
    n_points = 100
    X = np.linspace(-10, 10, n_points)
    y = 2 * X ** 2 + 3 * X + 4 + np.random.normal(0, 10, n_points)

    # Add outliers
    n_outliers = 20
    X[-n_outliers:] += int(30 * np.random.rand())
    y[-n_outliers:] -= int(500 * np.random.rand())

    X = np.expand_dims(X, -1)
    y = np.expand_dims(y, -1)
    data = np.hstack([X, y])

    threshold_cadidates = [1, 2, 4, 8, 16, 32, 64, 128]
    threshold_cadidates.sort()

    sample_size = 3
    max_iteration = get_sampling_number(sample_size)
    threshold = get_inlier_threshold(
        threshold_cadidates, data, polynomial_degree=2, sample_size=sample_size,
        min_iteration=-1, max_iteration=max_iteration, stop_inlier_ratio=0.50, verbose=True)