1 /*
2 * Copyright (C) 2015 Alberto Irurueta Carro (alberto@irurueta.com)
3 *
4 * Licensed under the Apache License, Version 2.0 (the "License");
5 * you may not use this file except in compliance with the License.
6 * You may obtain a copy of the License at
7 *
8 * http://www.apache.org/licenses/LICENSE-2.0
9 *
10 * Unless required by applicable law or agreed to in writing, software
11 * distributed under the License is distributed on an "AS IS" BASIS,
12 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13 * See the License for the specific language governing permissions and
14 * limitations under the License.
15 */
16 package com.irurueta.ar.epipolar.estimators;
17
18 import com.irurueta.ar.epipolar.FundamentalMatrix;
19 import com.irurueta.geometry.Point2D;
20 import com.irurueta.geometry.estimators.LockedException;
21 import com.irurueta.geometry.estimators.NotReadyException;
22 import com.irurueta.numerical.robust.MSACRobustEstimator;
23 import com.irurueta.numerical.robust.MSACRobustEstimatorListener;
24 import com.irurueta.numerical.robust.RobustEstimator;
25 import com.irurueta.numerical.robust.RobustEstimatorException;
26 import com.irurueta.numerical.robust.RobustEstimatorMethod;
27
28 import java.util.ArrayList;
29 import java.util.List;
30
31 /**
32 * Finds the best fundamental matrix for provided collections of matched 2D
33 * points using RANSAC algorithm.
34 */
35 public class MSACFundamentalMatrixRobustEstimator extends FundamentalMatrixRobustEstimator {
36
37 /**
38 * Constant defining default threshold to determine whether points are
39 * inliers or not.
40 * By default, 1.0 is considered a good value for cases where measures are
41 * done in pixels, since typically the minimum resolution is 1 pixel.
42 */
43 public static final double DEFAULT_THRESHOLD = 1.0;
44
45 /**
46 * Minimum value that can be set as threshold.
47 * Threshold must be strictly greater than 0.0.
48 */
49 public static final double MIN_THRESHOLD = 0.0;
50
51 /**
52 * Threshold to determine whether pairs of matched points are inliers or not
53 * when testing possible estimation solutions.
54 * The threshold refers to the amount of error (i.e. distance) a given
55 * point has respect to the epipolar line generated by its matched point.
56 */
57 private double threshold;
58
59 /**
60 * Constructor.
61 *
62 * @param fundMatrixEstimatorMethod method for non-robust fundamental matrix
63 * estimator.
64 */
65 public MSACFundamentalMatrixRobustEstimator(final FundamentalMatrixEstimatorMethod fundMatrixEstimatorMethod) {
66 super(fundMatrixEstimatorMethod);
67 threshold = DEFAULT_THRESHOLD;
68 }
69
70 /**
71 * Constructor.
72 *
73 * @param fundMatrixEstimatorMethod method for non-robust fundamental matrix
74 * estimator.
75 * @param listener listener to be notified of events such as when
76 * estimation starts, ends or its progress significantly changes.
77 */
78 public MSACFundamentalMatrixRobustEstimator(
79 final FundamentalMatrixEstimatorMethod fundMatrixEstimatorMethod,
80 final FundamentalMatrixRobustEstimatorListener listener) {
81 super(fundMatrixEstimatorMethod, listener);
82 threshold = DEFAULT_THRESHOLD;
83 }
84
85 /**
86 * Constructor.
87 *
88 * @param fundMatrixEstimatorMethod method for non-robust fundamental matrix
89 * estimator.
90 * @param leftPoints 2D points on left view.
91 * @param rightPoints 2D points on right view.
92 * @throws IllegalArgumentException if provided list of points do not have
93 * the same length or their length is less than 7 points.
94 */
95 public MSACFundamentalMatrixRobustEstimator(
96 final FundamentalMatrixEstimatorMethod fundMatrixEstimatorMethod,
97 final List<Point2D> leftPoints, final List<Point2D> rightPoints) {
98 super(fundMatrixEstimatorMethod, leftPoints, rightPoints);
99 threshold = DEFAULT_THRESHOLD;
100 }
101
102 /**
103 * Constructor.
104 *
105 * @param fundMatrixEstimatorMethod method for non-robust fundamental matrix
106 * estimator.
107 * @param leftPoints 2D points on left view.
108 * @param rightPoints 2D points on right view.
109 * @param listener listener to be notified of events such as when estimation
110 * starts, ends or its progress significantly changes.
111 * @throws IllegalArgumentException if provided list of points do not have
112 * the same length or their length is less than 7 points.
113 */
114 public MSACFundamentalMatrixRobustEstimator(
115 final FundamentalMatrixEstimatorMethod fundMatrixEstimatorMethod, final List<Point2D> leftPoints,
116 final List<Point2D> rightPoints, final FundamentalMatrixRobustEstimatorListener listener) {
117 super(fundMatrixEstimatorMethod, leftPoints, rightPoints, listener);
118 threshold = DEFAULT_THRESHOLD;
119 }
120
121 /**
122 * Constructor.
123 */
124 public MSACFundamentalMatrixRobustEstimator() {
125 this(DEFAULT_FUNDAMENTAL_MATRIX_ESTIMATOR_METHOD);
126 }
127
128 /**
129 * Constructor.
130 *
131 * @param listener listener to be notified of events such as when
132 * estimation starts, ends or its progress significantly changes.
133 */
134 public MSACFundamentalMatrixRobustEstimator(final FundamentalMatrixRobustEstimatorListener listener) {
135 this(DEFAULT_FUNDAMENTAL_MATRIX_ESTIMATOR_METHOD, listener);
136 }
137
138 /**
139 * Constructor.
140 *
141 * @param leftPoints 2D points on left view.
142 * @param rightPoints 2D points on right view.
143 * @throws IllegalArgumentException if provided list of points do not have
144 * the same length or their length is less than 7 points.
145 */
146 public MSACFundamentalMatrixRobustEstimator(final List<Point2D> leftPoints, final List<Point2D> rightPoints) {
147 this(DEFAULT_FUNDAMENTAL_MATRIX_ESTIMATOR_METHOD, leftPoints, rightPoints);
148 }
149
150 /**
151 * Constructor.
152 *
153 * @param leftPoints 2D points on left view.
154 * @param rightPoints 2D points on right view.
155 * @param listener listener to be notified of events such as when estimation
156 * starts, ends or its progress significantly changes.
157 * @throws IllegalArgumentException if provided list of points do not have
158 * the same length or their length is less than 7 points.
159 */
160 public MSACFundamentalMatrixRobustEstimator(
161 final List<Point2D> leftPoints, final List<Point2D> rightPoints,
162 final FundamentalMatrixRobustEstimatorListener listener) {
163 this(DEFAULT_FUNDAMENTAL_MATRIX_ESTIMATOR_METHOD, leftPoints, rightPoints, listener);
164 }
165
166 /**
167 * Returns threshold to determine whether matched pairs of points are
168 * inliers or not when testing possible estimation solutions.
169 * The threshold refers to the amount of error (i.e. distance) a given
170 * point has respect to the epipolar line generated by its matched point.
171 *
172 * @return threshold to determine whether matched pairs of points are
173 * inliers or not.
174 */
175 public double getThreshold() {
176 return threshold;
177 }
178
179 /**
180 * Sets threshold to determine whether matched pairs of points are inliers
181 * or not when testing possible estimation solutions.
182 *
183 * @param threshold threshold to be set.
184 * @throws IllegalArgumentException if provided value is equal or less than
185 * zero.
186 * @throws LockedException if robust estimator is locked because an
187 * estimation is already in progress.
188 */
189 public void setThreshold(final double threshold) throws LockedException {
190 if (isLocked()) {
191 throw new LockedException();
192 }
193 if (threshold <= MIN_THRESHOLD) {
194 throw new IllegalArgumentException();
195 }
196 this.threshold = threshold;
197 }
198
199 /**
200 * Estimates a radial distortion using a robust estimator and
201 * the best set of matched 2D points found using the robust estimator.
202 *
203 * @return a radial distortion.
204 * @throws LockedException if robust estimator is locked because an
205 * estimation is already in progress.
206 * @throws NotReadyException if provided input data is not enough to start
207 * the estimation.
208 * @throws RobustEstimatorException if estimation fails for any reason
209 * (i.e. numerical instability, no solution available, etc).
210 */
211 @SuppressWarnings("DuplicatedCode")
212 @Override
213 public FundamentalMatrix estimate() throws LockedException, NotReadyException, RobustEstimatorException {
214 if (isLocked()) {
215 throw new LockedException();
216 }
217 if (!isReady()) {
218 throw new NotReadyException();
219 }
220
221 final var innerEstimator = new MSACRobustEstimator<FundamentalMatrix>(new MSACRobustEstimatorListener<>() {
222
223 // subset of left points
224 private final List<Point2D> subsetLeftPoints = new ArrayList<>();
225
226 // subset of right points
227 private final List<Point2D> subsetRightPoints = new ArrayList<>();
228
229 @Override
230 public double getThreshold() {
231 return threshold;
232 }
233
234 @Override
235 public int getTotalSamples() {
236 return leftPoints.size();
237 }
238
239 @Override
240 public int getSubsetSize() {
241 return getMinRequiredPoints();
242 }
243
244 @Override
245 public void estimatePreliminarSolutions(
246 final int[] samplesIndices, final List<FundamentalMatrix> solutions) {
247
248 subsetLeftPoints.clear();
249 subsetRightPoints.clear();
250 for (final var samplesIndex : samplesIndices) {
251 subsetLeftPoints.add(leftPoints.get(samplesIndex));
252 subsetRightPoints.add(rightPoints.get(samplesIndex));
253 }
254
255 nonRobustEstimate(solutions, subsetLeftPoints, subsetRightPoints);
256 }
257
258 @Override
259 public double computeResidual(final FundamentalMatrix currentEstimation, final int i) {
260 final var leftPoint = leftPoints.get(i);
261 final var rightPoint = rightPoints.get(i);
262 return residual(currentEstimation, leftPoint, rightPoint);
263 }
264
265 @Override
266 public boolean isReady() {
267 return MSACFundamentalMatrixRobustEstimator.this.isReady();
268 }
269
270 @Override
271 public void onEstimateStart(final RobustEstimator<FundamentalMatrix> estimator) {
272 if (listener != null) {
273 listener.onEstimateStart(MSACFundamentalMatrixRobustEstimator.this);
274 }
275 }
276
277 @Override
278 public void onEstimateEnd(final RobustEstimator<FundamentalMatrix> estimator) {
279 if (listener != null) {
280 listener.onEstimateEnd(MSACFundamentalMatrixRobustEstimator.this);
281 }
282 }
283
284 @Override
285 public void onEstimateNextIteration(
286 final RobustEstimator<FundamentalMatrix> estimator, final int iteration) {
287 if (listener != null) {
288 listener.onEstimateNextIteration(MSACFundamentalMatrixRobustEstimator.this, iteration);
289 }
290 }
291
292 @Override
293 public void onEstimateProgressChange(
294 final RobustEstimator<FundamentalMatrix> estimator, final float progress) {
295 if (listener != null) {
296 listener.onEstimateProgressChange(MSACFundamentalMatrixRobustEstimator.this, progress);
297 }
298 }
299 });
300
301 try {
302 locked = true;
303 inliersData = null;
304 innerEstimator.setConfidence(confidence);
305 innerEstimator.setMaxIterations(maxIterations);
306 innerEstimator.setProgressDelta(progressDelta);
307 final var result = innerEstimator.estimate();
308 inliersData = innerEstimator.getInliersData();
309 return attemptRefine(result);
310 } catch (final com.irurueta.numerical.LockedException e) {
311 throw new LockedException(e);
312 } catch (final com.irurueta.numerical.NotReadyException e) {
313 throw new NotReadyException(e);
314 } finally {
315 locked = false;
316 }
317 }
318
319 /**
320 * Returns method being used for robust estimation.
321 *
322 * @return method being used for robust estimation.
323 */
324 @Override
325 public RobustEstimatorMethod getMethod() {
326 return RobustEstimatorMethod.MSAC;
327 }
328
329 /**
330 * Gets standard deviation used for Levenberg-Marquardt fitting during
331 * refinement.
332 * Returned value gives an indication of how much variance each residual
333 * has.
334 * Typically, this value is related to the threshold used on each robust
335 * estimation, since residuals of found inliers are within the range of
336 * such threshold.
337 *
338 * @return standard deviation used for refinement.
339 */
340 @Override
341 protected double getRefinementStandardDeviation() {
342 return threshold;
343 }
344 }