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.calibration.estimators;
17
18 import com.irurueta.ar.calibration.RadialDistortion;
19 import com.irurueta.geometry.CoordinatesType;
20 import com.irurueta.geometry.Point2D;
21 import com.irurueta.geometry.estimators.LockedException;
22 import com.irurueta.geometry.estimators.NotReadyException;
23 import com.irurueta.numerical.robust.RANSACRobustEstimator;
24 import com.irurueta.numerical.robust.RANSACRobustEstimatorListener;
25 import com.irurueta.numerical.robust.RobustEstimator;
26 import com.irurueta.numerical.robust.RobustEstimatorException;
27 import com.irurueta.numerical.robust.RobustEstimatorMethod;
28
29 import java.util.ArrayList;
30 import java.util.List;
31 import java.util.logging.Level;
32 import java.util.logging.Logger;
33
34 /**
35 * Finds the best radial distortion for provided collections of 2D points using
36 * RANSAC algorithm
37 */
38 public class RANSACRadialDistortionRobustEstimator extends RadialDistortionRobustEstimator {
39
40 /**
41 * Constant defining default threshold to determine whether points are
42 * inliers or not.
43 * By default, 1.0 is considered a good value for cases where measures are
44 * done on pixels, since typically the minimum resolution is 1 pixel.
45 */
46 public static final double DEFAULT_THRESHOLD = 1.0;
47
48 /**
49 * Minimum value that can be set as threshold.
50 * Threshold must be strictly greater than 0.0.
51 */
52 public static final double MIN_THRESHOLD = 0.0;
53
54 /**
55 * Threshold to determine whether points are inliers or not when testing
56 * possible estimation solutions.
57 * The threshold refers to the amount of error (i.e. distance) a possible
58 * solution has on a matched pair of points.
59 */
60 private double threshold;
61
62 /**
63 * Constructor.
64 */
65 public RANSACRadialDistortionRobustEstimator() {
66 super();
67 threshold = DEFAULT_THRESHOLD;
68 }
69
70 /**
71 * Constructor.
72 *
73 * @param listener listener to be notified of events such as when
74 * estimation starts, ends or its progress significantly changes.
75 */
76 public RANSACRadialDistortionRobustEstimator(final RadialDistortionRobustEstimatorListener listener) {
77 super(listener);
78 threshold = DEFAULT_THRESHOLD;
79 }
80
81 /**
82 * Constructor.
83 *
84 * @param distortedPoints list of distorted points. Distorted points are
85 * obtained after radial distortion is applied to an undistorted point.
86 * @param undistortedPoints list of undistorted points.
87 * @throws IllegalArgumentException if provided lists of points don't have
88 * the same size or their size is smaller than MIN_NUMBER_OF_POINTS.
89 */
90 public RANSACRadialDistortionRobustEstimator(
91 final List<Point2D> distortedPoints, final List<Point2D> undistortedPoints) {
92 super(distortedPoints, undistortedPoints);
93 threshold = DEFAULT_THRESHOLD;
94 }
95
96 /**
97 * Constructor.
98 *
99 * @param distortedPoints list of distorted points. Distorted points are
100 * obtained after radial distortion is applied to an undistorted point.
101 * @param undistortedPoints list of undistorted points.
102 * @param listener listener to be notified of events such as when
103 * estimation starts, ends or its progress significantly changes.
104 * @throws IllegalArgumentException if provided lists of points don't have
105 * the same size or their size is smaller than MIN_NUMBER_OF_POINTS.
106 */
107 public RANSACRadialDistortionRobustEstimator(final List<Point2D> distortedPoints,
108 final List<Point2D> undistortedPoints,
109 final RadialDistortionRobustEstimatorListener listener) {
110 super(distortedPoints, undistortedPoints, listener);
111 threshold = DEFAULT_THRESHOLD;
112 }
113
114 /**
115 * Constructor.
116 *
117 * @param distortedPoints list of distorted points. Distorted points are
118 * obtained after radial distortion is applied to an undistorted point.
119 * @param undistortedPoints list of undistorted points.
120 * @param distortionCenter radial distortion center. If null it is assumed
121 * to be the origin of coordinates, otherwise this is typically equal to
122 * the camera principal point.
123 * @throws IllegalArgumentException if provided lists of points don't have
124 * the same size or their size is smaller than MIN_NUMBER_OF_POINTS.
125 */
126 public RANSACRadialDistortionRobustEstimator(final List<Point2D> distortedPoints,
127 final List<Point2D> undistortedPoints,
128 final Point2D distortionCenter) {
129 super(distortedPoints, undistortedPoints, distortionCenter);
130 threshold = DEFAULT_THRESHOLD;
131 }
132
133 /**
134 * Constructor.
135 *
136 * @param distortedPoints list of distorted points. Distorted points are
137 * obtained after radial distortion is applied to an undistorted point.
138 * @param undistortedPoints list of undistorted points.
139 * @param distortionCenter radial distortion center. If null it is assumed
140 * to be the origin of coordinates, otherwise this is typically equal to
141 * the camera principal point.
142 * @param listener listener to be notified of events such as when
143 * estimation starts, ends or its progress significantly changes.
144 * @throws IllegalArgumentException if provided lists of points don't have
145 * the same size or their size is smaller than MIN_NUMBER_OF_POINTS.
146 */
147 public RANSACRadialDistortionRobustEstimator(final List<Point2D> distortedPoints,
148 final List<Point2D> undistortedPoints,
149 final Point2D distortionCenter,
150 final RadialDistortionRobustEstimatorListener listener) {
151 super(distortedPoints, undistortedPoints, distortionCenter, listener);
152 threshold = DEFAULT_THRESHOLD;
153 }
154
155 /**
156 * Returns threshold to determine whether points are inliers or not when
157 * testing possible estimation solutions.
158 * The threshold refers to the amount of error (i.e. Euclidean distance) a
159 * possible solution has on projected 2D points.
160 *
161 * @return threshold to determine whether points are inliers or not when
162 * testing possible estimation solutions.
163 */
164 public double getThreshold() {
165 return threshold;
166 }
167
168 /**
169 * Sets threshold to determine whether points are inliers or not when
170 * testing possible estimation solutions.
171 * The threshold refers to the amount of error (i.e. Euclidean distance) a
172 * possible solution has on projected 2D points.
173 *
174 * @param threshold threshold to be set.
175 * @throws IllegalArgumentException if provided value is equal or less than
176 * zero.
177 * @throws LockedException if robust estimator is locked because an
178 * estimation is already in progress.
179 */
180 public void setThreshold(final double threshold) throws LockedException {
181 if (isLocked()) {
182 throw new LockedException();
183 }
184 if (threshold <= MIN_THRESHOLD) {
185 throw new IllegalArgumentException();
186 }
187 this.threshold = threshold;
188 }
189
190 /**
191 * Estimates a radial distortion using a robust estimator and
192 * the best set of matched 2D points found using the robust estimator.
193 *
194 * @return a radial distortion.
195 * @throws LockedException if robust estimator is locked because an
196 * estimation is already in progress.
197 * @throws NotReadyException if provided input data is not enough to start
198 * the estimation.
199 * @throws RobustEstimatorException if estimation fails for any reason
200 * (i.e. numerical instability, no solution available, etc).
201 */
202 @SuppressWarnings("DuplicatedCode")
203 @Override
204 public RadialDistortion estimate() throws LockedException, NotReadyException, RobustEstimatorException {
205 if (isLocked()) {
206 throw new LockedException();
207 }
208 if (!isReady()) {
209 throw new NotReadyException();
210 }
211
212 final var innerEstimator = new RANSACRobustEstimator<RadialDistortion>(new RANSACRobustEstimatorListener<>() {
213
214 // point to be reused when computing residuals
215 private final Point2D testPoint = Point2D.create(CoordinatesType.INHOMOGENEOUS_COORDINATES);
216
217 // non-robust radial distortion estimator
218 private final LMSERadialDistortionEstimator radialDistortionEstimator = new LMSERadialDistortionEstimator();
219
220 // subset of distorted (i.e. measured) points
221 private final List<Point2D> subsetDistorted = new ArrayList<>();
222
223 // subset of undistorted (i.e. ideal) points
224 private final List<Point2D> subsetUndistorted = new ArrayList<>();
225
226 @Override
227 public double getThreshold() {
228 return threshold;
229 }
230
231 @Override
232 public int getTotalSamples() {
233 return distortedPoints.size();
234 }
235
236 @Override
237 public int getSubsetSize() {
238 return MIN_NUMBER_OF_POINTS;
239 }
240
241 @Override
242 public void estimatePreliminarSolutions(
243 final int[] samplesIndices, final List<RadialDistortion> solutions) {
244 subsetDistorted.clear();
245 subsetDistorted.add(distortedPoints.get(samplesIndices[0]));
246 subsetDistorted.add(distortedPoints.get(samplesIndices[1]));
247
248 subsetUndistorted.clear();
249 subsetUndistorted.add(undistortedPoints.get(samplesIndices[0]));
250 subsetUndistorted.add(undistortedPoints.get(samplesIndices[1]));
251
252 try {
253 radialDistortionEstimator.setPoints(distortedPoints, undistortedPoints);
254 radialDistortionEstimator.setPoints(subsetDistorted, subsetUndistorted);
255
256 final var distortion = radialDistortionEstimator.estimate();
257 solutions.add(distortion);
258 } catch (final Exception e) {
259 // if anything fails, no solution is added
260 }
261 }
262
263 @Override
264 public double computeResidual(final RadialDistortion currentEstimation, final int i) {
265 final var distortedPoint = distortedPoints.get(i);
266 final var undistortedPoint = undistortedPoints.get(i);
267
268 currentEstimation.distort(undistortedPoint, testPoint);
269
270 return testPoint.distanceTo(distortedPoint);
271 }
272
273 @Override
274 public boolean isReady() {
275 return RANSACRadialDistortionRobustEstimator.this.isReady();
276 }
277
278 @Override
279 public void onEstimateStart(final RobustEstimator<RadialDistortion> estimator) {
280 try {
281 radialDistortionEstimator.setLMSESolutionAllowed(false);
282 radialDistortionEstimator.setIntrinsic(getIntrinsic());
283 } catch (final Exception e) {
284 Logger.getLogger(RANSACRadialDistortionRobustEstimator.class.getName()).log(Level.WARNING,
285 "Could not set intrinsic parameters on radial distortion estimator", e);
286 }
287
288 if (listener != null) {
289 listener.onEstimateStart(RANSACRadialDistortionRobustEstimator.this);
290 }
291 }
292
293 @Override
294 public void onEstimateEnd(final RobustEstimator<RadialDistortion> estimator) {
295 if (listener != null) {
296 listener.onEstimateEnd(RANSACRadialDistortionRobustEstimator.this);
297 }
298 }
299
300 @Override
301 public void onEstimateNextIteration(
302 final RobustEstimator<RadialDistortion> estimator, final int iteration) {
303 if (listener != null) {
304 listener.onEstimateNextIteration(RANSACRadialDistortionRobustEstimator.this, iteration);
305 }
306 }
307
308 @Override
309 public void onEstimateProgressChange(
310 final RobustEstimator<RadialDistortion> estimator, final float progress) {
311 if (listener != null) {
312 listener.onEstimateProgressChange(RANSACRadialDistortionRobustEstimator.this, progress);
313 }
314 }
315 });
316
317 try {
318 locked = true;
319 innerEstimator.setConfidence(confidence);
320 innerEstimator.setMaxIterations(maxIterations);
321 innerEstimator.setProgressDelta(progressDelta);
322 return innerEstimator.estimate();
323 } catch (final com.irurueta.numerical.LockedException e) {
324 throw new LockedException(e);
325 } catch (final com.irurueta.numerical.NotReadyException e) {
326 throw new NotReadyException(e);
327 } finally {
328 locked = false;
329 }
330 }
331
332 /**
333 * Returns method being used for robust estimation
334 *
335 * @return method being used for robust estimation
336 */
337 @Override
338 public RobustEstimatorMethod getMethod() {
339 return RobustEstimatorMethod.RANSAC;
340 }
341 }