-
Notifications
You must be signed in to change notification settings - Fork 0
/
ransac_fit_affine.m
106 lines (92 loc) · 4.2 KB
/
ransac_fit_affine.m
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
function [A_best,t_best,k_loop,nbr_inlier_best] = ransac_fit_affine(pts, pts_tilde,d, threshold,local_opt_using)
datasize = length(pts);
A_best = zeros(2,2);
t_best = zeros(2,1);
nbr_inlier_best = 0;
new_best_found = false;
%These parameter is to adjust the iteration
e = 0.05; %Initial inlier rate
mu = 0.1^2; %minimum probability of missing correct model %Not sure about this
k_max = round(log(mu)/log(1-e^(3+d)));
k_loop = 0;
while k_loop < k_max
% while k_loop < 1000
temp = randperm(datasize,3+d);
index_of_point_to_estimate = temp(1:3);
index_of_Tdd = temp(4:end);
pts_temp = pts(1:2,index_of_point_to_estimate);
pts_tilde_temp = pts_tilde(1:2,index_of_point_to_estimate);
[A_temp, t_temp] = estimate_affine(pts_temp, pts_tilde_temp);
%check for NaN
% check_A = sum(sum(isnan(A_temp)));
% check_t = sum(sum(isnan(t_temp)));
% check_for_NaN = check_A + check_t == 0;
%check for Tdd
if d == 0
check_for_Tdd = true;
else
pts_Tdd = pts(1:2,index_of_Tdd);
pts_tilde_Tdd = pts_tilde(1:2,index_of_Tdd);
residual_lgths_Tdd = residual_lgths(A_temp,t_temp,pts_Tdd,pts_tilde_Tdd);
check_threshold_Tdd = residual_lgths_Tdd < threshold;
check_for_Tdd = sum(check_threshold_Tdd) == d;
end
if check_for_Tdd
% if check_for_NaN && check_for_Tdd
residual_lgths_temp = residual_lgths(A_temp,t_temp,pts,pts_tilde);
check_threshold = residual_lgths_temp < threshold;
nbr_inlier = sum(check_threshold);
if nbr_inlier > nbr_inlier_best
A_best = A_temp;
t_best = t_temp;
nbr_inlier_best = nbr_inlier;
e = nbr_inlier/datasize;
k_max = round(log(mu)/log(1-e^(3+d)));
%Local optimization
% new_best_found = false;
if local_opt_using
%Store inlier
index_inlier = find(check_threshold);
pts_inlier = pts(:,index_inlier);
pts_tilde_inlier = pts_tilde(:,index_inlier);
A_new_best = zeros(2,2);
t_new_best = zeros(2,1);
nbr_inlier_new_best = nbr_inlier_best;
%iterate 10 times
for k_local = 1:10
if nbr_inlier_best<12
s_pts_inlier = pts_inlier;
s_pts_tilde_inlier = pts_tilde_inlier;
else
index_for_local = randperm(nbr_inlier_best,12);
s_pts_inlier = pts_inlier(:,index_for_local);
s_pts_tilde_inlier = pts_tilde_inlier(:,index_for_local);
end
[A_local_temp,t_local_temp] = least_squares_affine(s_pts_inlier, s_pts_tilde_inlier);
residual_lgths_local_temp = residual_lgths(A_local_temp,t_local_temp,pts,pts_tilde);
check_threshold_local = residual_lgths_local_temp < threshold;
nbr_inlier_local = sum(check_threshold_local);
if nbr_inlier_local > nbr_inlier_new_best
A_new_best = A_local_temp;
t_new_best = t_local_temp;
nbr_inlier_new_best = nbr_inlier_local;
if nbr_inlier_new_best > nbr_inlier_best
e = nbr_inlier_new_best/datasize;
k_max = round(log(mu)/log(1-e^(3+d)));
end
new_best_found = true;
end
end
end
end
end
k_loop = k_loop + 1;
end
if new_best_found
if nbr_inlier_best < nbr_inlier_new_best
A_best = A_new_best;
t_best = t_new_best;
nbr_inlier_best = nbr_inlier_new_best;
end
end
end