1 | % RANSAC - Robustly fits a model to data with the RANSAC algorithm |
---|
2 | % |
---|
3 | % Usage: |
---|
4 | % |
---|
5 | % [M, inliers] = ransac(x, fittingfn, distfn, degenfn s, t, feedback) |
---|
6 | % [F, inliers, NewDistrib, fail] = ... |
---|
7 | % ransac(defaultPara, x, Depth, ... |
---|
8 | % fittingfn, distfn, degenfn, s, t, distrib, T, feedback, disp, FlagDist); |
---|
9 | % |
---|
10 | % Arguments: |
---|
11 | % defaultPara - useful default parameters (like, camera intrinsic matrix) |
---|
12 | % |
---|
13 | % x - Data sets to which we are seeking to fit a model M |
---|
14 | % It is assumed that x is of size [d x Npts] |
---|
15 | % where d is the dimensionality of the data and Npts is |
---|
16 | % the number of data points. |
---|
17 | % |
---|
18 | % Depth - depth imformation to support more accurate ransac |
---|
19 | % |
---|
20 | % fittingfn - Handle to a function that fits a model to s |
---|
21 | % data from x. It is assumed that the function is of the |
---|
22 | % form: |
---|
23 | % M = fittingfn(x) |
---|
24 | % Note it is possible that the fitting function can return |
---|
25 | % multiple models (for example up to 3 fundamental matrices |
---|
26 | % can be fitted to 7 matched points). In this case it is |
---|
27 | % assumed that the fitting function returns a cell array of |
---|
28 | % models. |
---|
29 | % If this function cannot fit a model it should return M as |
---|
30 | % an empty matrix. |
---|
31 | % |
---|
32 | % distfn - Handle to a function that evaluates the |
---|
33 | % distances from the model to data x. |
---|
34 | % It is assumed that the function is of the form: |
---|
35 | % [inliers, M] = distfn(M, x, t) |
---|
36 | % This function must evaluate the distances between points |
---|
37 | % and the model returning the indices of elements in x that |
---|
38 | % are inliers, that is, the points that are within distance |
---|
39 | % 't' of the model. Additionally, if M is a cell array of |
---|
40 | % possible models 'distfn' will return the model that has the |
---|
41 | % most inliers. If there is only one model this function |
---|
42 | % must still copy the model to the output. After this call M |
---|
43 | % will be a non-cell object representing only one model. |
---|
44 | % |
---|
45 | % degenfn - Handle to a function that determines whether a |
---|
46 | % set of datapoints will produce a degenerate model. |
---|
47 | % This is used to discard random samples that do not |
---|
48 | % result in useful models. |
---|
49 | % It is assumed that degenfn is a boolean function of |
---|
50 | % the form: |
---|
51 | % r = degenfn(x) |
---|
52 | % It may be that you cannot devise a test for degeneracy in |
---|
53 | % which case you should write a dummy function that always |
---|
54 | % returns a value of 1 (true) and rely on 'fittingfn' to return |
---|
55 | % an empty model should the data set be degenerate. |
---|
56 | % |
---|
57 | % s - The minimum number of samples from x required by |
---|
58 | % fittingfn to fit a model. |
---|
59 | % |
---|
60 | % t - The distance threshold between a data point and the model |
---|
61 | % used to decide whether the point is an inlier or not. |
---|
62 | % |
---|
63 | % distrib - initial distribution (default uniform dist) |
---|
64 | % |
---|
65 | % T - affine transform 3 by 3 matrix applied on x |
---|
66 | % |
---|
67 | % feedback - An optional flag 0/1. If set to one the trial count and the |
---|
68 | % estimated total number of trials required is printed out at |
---|
69 | % each step. Defaults to 0. |
---|
70 | % |
---|
71 | % disp - if true, display the matches found when done. |
---|
72 | % |
---|
73 | % FlagDist - if true, calculate the reprojection error |
---|
74 | % |
---|
75 | % Returns: |
---|
76 | % M - The model having the greatest number of inliers. |
---|
77 | % inliers - An array of indices of the elements of x that were |
---|
78 | % the inliers for the best model. |
---|
79 | % NewDist - New Distribution after Ransac (Outliers have zero distribution) |
---|
80 | % fail - true if Ransac fail to find any solution is not degenerated |
---|
81 | % |
---|
82 | % For an example of the use of this function see RANSACFITHOMOGRAPHY or |
---|
83 | % RANSACFITPLANE |
---|
84 | |
---|
85 | % References: |
---|
86 | % M.A. Fishler and R.C. Boles. "Random sample concensus: A paradigm |
---|
87 | % for model fitting with applications to image analysis and automated |
---|
88 | % cartography". Comm. Assoc. Comp, Mach., Vol 24, No 6, pp 381-395, 1981 |
---|
89 | % |
---|
90 | % Richard Hartley and Andrew Zisserman. "Multiple View Geometry in |
---|
91 | % Computer Vision". pp 101-113. Cambridge University Press, 2001 |
---|
92 | |
---|
93 | % Peter Kovesi |
---|
94 | % School of Computer Science & Software Engineering |
---|
95 | % The University of Western Australia |
---|
96 | % pk at csse uwa edu au |
---|
97 | % http://www.csse.uwa.edu.au/~pk |
---|
98 | % |
---|
99 | % May 2003 - Original version |
---|
100 | % February 2004 - Tidied up. |
---|
101 | % August 2005 - Specification of distfn changed to allow model fitter to |
---|
102 | % return multiple models from which the best must be selected. |
---|
103 | % |
---|
104 | % additional parameter distrib is a vector of non-negative numbers that |
---|
105 | % specifies a (not necessarily normalized) probability distribution over |
---|
106 | % the different possible matches - passed to the sample function in the lightspeed |
---|
107 | % package in order to sample possible matches. |
---|
108 | % (added by Jeff Michels) |
---|
109 | % Additional NewDist estimated after Ransac is formed by using Depth |
---|
110 | % information |
---|
111 | % (added by Min Sun) |
---|
112 | %function [M, inliers, fail] = ransac(x, fittingfn, distfn, degenfn, s, t, feedback, distrib) |
---|
113 | function [M, inliers, NewDistrib, fail] = ... |
---|
114 | ransac(defaultPara, x, Depth, ... |
---|
115 | fittingfn, distfn, degenfn, s, t, distrib, T, feedback, disp, FlagDist) |
---|
116 | |
---|
117 | if nargin < 10 |
---|
118 | feedback = 0; |
---|
119 | disp = 0; |
---|
120 | end |
---|
121 | fail=0; |
---|
122 | [rows, npts] = size(x); |
---|
123 | |
---|
124 | %p = 0.999; % Desired probability of choosing at least one sample |
---|
125 | p = 0.9999; % Desired probability of choosing at least one sample |
---|
126 | % free from outliers |
---|
127 | |
---|
128 | %maxTrials = 20000; % Maximum number of trials before we give up. |
---|
129 | %maxDataTrials = 100; % Max number of attempts to select a non-degenerate |
---|
130 | maxTrials = 20000; % Maximum number of trials before we give up. |
---|
131 | maxDataTrials = 100; % Max number of attempts to select a non-degenerate |
---|
132 | % data set. |
---|
133 | |
---|
134 | bestM = NaN; % Sentinel value allowing detection of solution failure. |
---|
135 | trialcount = 0; |
---|
136 | bestscore = 0; |
---|
137 | N = 1; % Dummy initialisation for number of trials. |
---|
138 | |
---|
139 | while N > trialcount |
---|
140 | |
---|
141 | % Select at random s datapoints to form a trial model, M. |
---|
142 | % In selecting these points we have to check that they are not in |
---|
143 | % a degenerate configuration. |
---|
144 | degenerate = 1; |
---|
145 | count = 1; |
---|
146 | while degenerate |
---|
147 | % Generate s random indicies in the range 1..npts |
---|
148 | %ind = ceil(rand(1,s)*npts); |
---|
149 | % JM replaced above line with sample() to sample non-uniformly |
---|
150 | ind = sample(distrib, s); |
---|
151 | |
---|
152 | % Test that these points are not a degenerate configuration. |
---|
153 | degenerate = feval(degenfn, x(:,ind)); |
---|
154 | |
---|
155 | if ~degenerate |
---|
156 | % Fit model to this random selection of data points. |
---|
157 | % Note that M may represent a set of models that fit the data in |
---|
158 | % this case M will be a cell array of models |
---|
159 | M = feval(fittingfn, x(:,ind)); |
---|
160 | |
---|
161 | % Depending on your problem it might be that the only way you |
---|
162 | % can determine whether a data set is degenerate or not is to |
---|
163 | % try to fit a model and see if it succeeds. If it fails we |
---|
164 | % reset degenerate to true. |
---|
165 | if isempty(M) |
---|
166 | degenerate = 1; |
---|
167 | end |
---|
168 | end |
---|
169 | |
---|
170 | % Safeguard against being stuck in this loop forever |
---|
171 | count = count + 1; |
---|
172 | if count > maxDataTrials |
---|
173 | warning('Unable to select a nondegenerate data set'); |
---|
174 | break |
---|
175 | end |
---|
176 | end |
---|
177 | |
---|
178 | % Once we are out here we should have some kind of model... |
---|
179 | % Evaluate distances between points and model returning the indices |
---|
180 | % of elements in x that are inliers. Additionally, if M is a cell |
---|
181 | % array of possible models 'distfn' will return the model that has |
---|
182 | % the most inliers. After this call M will be a non-cell object |
---|
183 | % representing only one model. |
---|
184 | [inliers, M, tempReProjError] = ... |
---|
185 | feval(distfn, M, x, t, defaultPara, Depth, T, FlagDist); |
---|
186 | warning('called distfn in ransac'); |
---|
187 | if ~isempty(tempReProjError) |
---|
188 | ReProjErrorM(trialcount+1,:) = tempReProjError; |
---|
189 | else |
---|
190 | ReProjErrorM = []; |
---|
191 | end |
---|
192 | |
---|
193 | % Find the number of inliers to this model. |
---|
194 | ninliers = length(inliers); |
---|
195 | |
---|
196 | % weightedNInliers weights each inlier by the prior probability |
---|
197 | % that it should be an inlier based on the parameter distrib |
---|
198 | % added by JM |
---|
199 | weightedNInliers = sum(distrib(inliers)); |
---|
200 | |
---|
201 | %if ninliers > bestscore % Largest set of inliers so far... |
---|
202 | % bestscore = ninliers; % Record data for this model |
---|
203 | if weightedNInliers > bestscore |
---|
204 | bestscore = weightedNInliers; |
---|
205 | bestinliers = inliers; |
---|
206 | bestM = M; |
---|
207 | |
---|
208 | % Update estimate of N, the number of trials to ensure we pick, |
---|
209 | % with probability p, a data set with no outliers. |
---|
210 | fracinliers = weightedNInliers;%ninliers/npts; |
---|
211 | pNoOutliers = 1 - fracinliers^s; |
---|
212 | pNoOutliers = max(eps, pNoOutliers); % Avoid division by -Inf |
---|
213 | pNoOutliers = min(1-eps, pNoOutliers);% Avoid division by 0. |
---|
214 | N = log(1-p)/log(pNoOutliers); |
---|
215 | end |
---|
216 | |
---|
217 | trialcount = trialcount+1; |
---|
218 | if feedback&&mod(trialcount,100)==0&& trialcount>1 |
---|
219 | fprintf('trial %d out of %d \r',trialcount, ceil(N)); |
---|
220 | end |
---|
221 | |
---|
222 | % Safeguard against being stuck in this loop forever |
---|
223 | if trialcount > maxTrials |
---|
224 | warning( ... |
---|
225 | sprintf('ransac reached the maximum number of %d trials',... |
---|
226 | maxTrials)); |
---|
227 | fail=1; |
---|
228 | break |
---|
229 | end |
---|
230 | end |
---|
231 | fprintf('\n'); |
---|
232 | |
---|
233 | if ~isnan(bestM) % We got a solution |
---|
234 | M = bestM; |
---|
235 | inliers = bestinliers; |
---|
236 | else |
---|
237 | warning('ransac was unable to find a useful solution'); |
---|
238 | fail=1; |
---|
239 | end |
---|
240 | |
---|
241 | % Processing NewDist |
---|
242 | if FlagDist |
---|
243 | ReProjError = median(ReProjErrorM,1); |
---|
244 | if disp |
---|
245 | figure; hist(ReProjError(ReProjError ~= Inf)); % plot hist of the ReProjection error |
---|
246 | end |
---|
247 | VarGammDist = var(ReProjError(ReProjError ~= Inf)); |
---|
248 | NewDistrib(size(ReProjError)) = 0; |
---|
249 | NewDistrib(ReProjError(ReProjError ~= Inf)) = ... |
---|
250 | gampdf(ReProjError(ReProjError ~= Inf), 1, VarGammDist); |
---|
251 | else |
---|
252 | NewDistrib = []; |
---|
253 | end |
---|