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.LMedSRobustEstimator;
24 import com.irurueta.numerical.robust.LMedSRobustEstimatorListener;
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 collection of 2D points using
36 * LMedS algorithm
37 */
38 public class LMedSRadialDistortionRobustEstimator extends RadialDistortionRobustEstimator {
39
40 /**
41 * Default value to be used for stop threshold. Stop threshold can be used
42 * to keep the algorithm iterating in case that best estimated threshold
43 * using median of residuals is not small enough. Once a solution is found
44 * that generates a threshold below this value, the algorithm will stop.
45 * The stop threshold can be used to prevent the LMedS algorithm iterating
46 * too many times in cases where samples have a very similar accuracy.
47 * For instance, in cases where proportion of outliers is very small (close
48 * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
49 * iterate for a long time trying to find the best solution when indeed
50 * there is no need to do that if a reasonable threshold has already been
51 * reached.
52 * Because of this behaviour the stop threshold can be set to a value much
53 * lower than the one typically used in RANSAC, and yet the algorithm could
54 * still produce even smaller thresholds in estimated results.
55 */
56 public static final double DEFAULT_STOP_THRESHOLD = 1e-3;
57
58 /**
59 * Minimum allowed stop threshold value.
60 */
61 public static final double MIN_STOP_THRESHOLD = 0.0;
62
63 /**
64 * Threshold to be used to keep the algorithm iterating in case that best
65 * estimated threshold using median of residuals is not small enough. Once
66 * a solution is found that generates a threshold below this value, the
67 * algorithm will stop.
68 * The stop threshold can be used to prevent the LMedS algorithm iterating
69 * too many times in cases where samples have a very similar accuracy.
70 * For instance, in cases where proportion of outliers is very small (close
71 * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
72 * iterate for a long time trying to find the best solution when indeed
73 * there is no need to do that if a reasonable threshold has already been
74 * reached.
75 * Because of this behaviour the stop threshold can be set to a value much
76 * lower than the one typically used in RANSAC, and yet the algorithm could
77 * still produce even smaller thresholds in estimated results.
78 */
79 private double stopThreshold;
80
81 /**
82 * Constructor.
83 */
84 public LMedSRadialDistortionRobustEstimator() {
85 super();
86 stopThreshold = DEFAULT_STOP_THRESHOLD;
87 }
88
89 /**
90 * Constructor.
91 *
92 * @param listener listener to be notified of events such as when
93 * estimation starts, ends or its progress significantly changes.
94 */
95 public LMedSRadialDistortionRobustEstimator(final RadialDistortionRobustEstimatorListener listener) {
96 super(listener);
97 stopThreshold = DEFAULT_STOP_THRESHOLD;
98 }
99
100 /**
101 * Constructor.
102 *
103 * @param distortedPoints list of distorted points. Distorted points are
104 * obtained after radial distortion is applied to an undistorted point.
105 * @param undistortedPoints list of undistorted points.
106 * @throws IllegalArgumentException if provided lists of points don't have
107 * the same size or their size is smaller than MIN_NUMBER_OF_POINTS.
108 */
109 public LMedSRadialDistortionRobustEstimator(
110 final List<Point2D> distortedPoints, final List<Point2D> undistortedPoints) {
111 super(distortedPoints, undistortedPoints);
112 stopThreshold = DEFAULT_STOP_THRESHOLD;
113 }
114
115 /**
116 * Constructor.
117 *
118 * @param distortedPoints list of distorted points. Distorted points are
119 * obtained after radial distortion is applied to an undistorted point.
120 * @param undistortedPoints list of undistorted points.
121 * @param listener listener to be notified of events such as when
122 * estimation starts, ends or its progress significantly changes.
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 LMedSRadialDistortionRobustEstimator(final List<Point2D> distortedPoints,
127 final List<Point2D> undistortedPoints,
128 final RadialDistortionRobustEstimatorListener listener) {
129 super(distortedPoints, undistortedPoints, listener);
130 stopThreshold = DEFAULT_STOP_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 * @throws IllegalArgumentException if provided lists of points don't have
143 * the same size or their size is smaller than MIN_NUMBER_OF_POINTS.
144 */
145 public LMedSRadialDistortionRobustEstimator(final List<Point2D> distortedPoints,
146 final List<Point2D> undistortedPoints,
147 final Point2D distortionCenter) {
148 super(distortedPoints, undistortedPoints, distortionCenter);
149 stopThreshold = DEFAULT_STOP_THRESHOLD;
150 }
151
152 /**
153 * Constructor.
154 *
155 * @param distortedPoints list of distorted points. Distorted points are
156 * obtained after radial distortion is applied to an undistorted point.
157 * @param undistortedPoints list of undistorted points.
158 * @param distortionCenter radial distortion center. If null it is assumed
159 * to be the origin of coordinates, otherwise this is typically equal to
160 * the camera principal point.
161 * @param listener listener to be notified of events such as when
162 * estimation starts, ends or its progress significantly changes.
163 * @throws IllegalArgumentException if provided lists of points don't have
164 * the same size or their size is smaller than MIN_NUMBER_OF_POINTS.
165 */
166 public LMedSRadialDistortionRobustEstimator(final List<Point2D> distortedPoints,
167 final List<Point2D> undistortedPoints,
168 final Point2D distortionCenter,
169 final RadialDistortionRobustEstimatorListener listener) {
170 super(distortedPoints, undistortedPoints, distortionCenter, listener);
171 stopThreshold = DEFAULT_STOP_THRESHOLD;
172 }
173
174 /**
175 * Returns threshold to be used to keep the algorithm iterating in case that
176 * best estimated threshold using median of residuals is not small enough.
177 * Once a solution is found that generates a threshold below this value, the
178 * algorithm will stop.
179 * The stop threshold can be used to prevent the LMedS algorithm iterating
180 * too many times in cases where samples have a very similar accuracy.
181 * For instance, in cases where proportion of outliers is very small (close
182 * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
183 * iterate for a long time trying to find the best solution when indeed
184 * there is no need to do that if a reasonable threshold has already been
185 * reached.
186 * Because of this behaviour the stop threshold can be set to a value much
187 * lower than the one typically used in RANSAC, and yet the algorithm could
188 * still produce even smaller thresholds in estimated results.
189 *
190 * @return stop threshold to stop the algorithm prematurely when a certain
191 * accuracy has been reached.
192 */
193 public double getStopThreshold() {
194 return stopThreshold;
195 }
196
197 /**
198 * Sets threshold to be used to keep the algorithm iterating in case that
199 * best estimated threshold using median of residuals is not small enough.
200 * Once a solution is found that generates a threshold below this value, the
201 * algorithm will stop.
202 * The stop threshold can be used to prevent the LMedS algorithm iterating
203 * too many times in cases where samples have a very similar accuracy.
204 * For instance, in cases where proportion of outliers is very small (close
205 * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
206 * iterate for a long time trying to find the best solution when indeed
207 * there is no need to do that if a reasonable threshold has already been
208 * reached.
209 * Because of this behaviour the stop threshold can be set to a value much
210 * lower than the one typically used in RANSAC, and yet the algorithm could
211 * still produce even smaller thresholds in estimated results.
212 *
213 * @param stopThreshold stop threshold to stop the algorithm prematurely
214 * when a certain accuracy has been reached.
215 * @throws IllegalArgumentException if provided value is zero or negative.
216 * @throws LockedException if robust estimator is locked because an
217 * estimation is already in progress.
218 */
219 public void setStopThreshold(final double stopThreshold) throws LockedException {
220 if (isLocked()) {
221 throw new LockedException();
222 }
223 if (stopThreshold <= MIN_STOP_THRESHOLD) {
224 throw new IllegalArgumentException();
225 }
226
227 this.stopThreshold = stopThreshold;
228 }
229
230 /**
231 * Estimates a radial distortion using a robust estimator and
232 * the best set of matched 2D points found using the robust estimator.
233 *
234 * @return a radial distortion.
235 * @throws LockedException if robust estimator is locked because an
236 * estimation is already in progress.
237 * @throws NotReadyException if provided input data is not enough to start
238 * the estimation.
239 * @throws RobustEstimatorException if estimation fails for any reason
240 * (i.e. numerical instability, no solution available, etc).
241 */
242 @SuppressWarnings("DuplicatedCode")
243 @Override
244 public RadialDistortion estimate() throws LockedException, NotReadyException, RobustEstimatorException {
245 if (isLocked()) {
246 throw new LockedException();
247 }
248 if (!isReady()) {
249 throw new NotReadyException();
250 }
251
252 final var innerEstimator = new LMedSRobustEstimator<RadialDistortion>(new LMedSRobustEstimatorListener<>() {
253
254 // point to be reused when computing residuals
255 private final Point2D testPoint = Point2D.create(CoordinatesType.INHOMOGENEOUS_COORDINATES);
256
257 // non-robust radial distortion estimator
258 private final LMSERadialDistortionEstimator radialDistortionEstimator = new LMSERadialDistortionEstimator();
259
260 // subset of distorted (i.e. measured) points
261 private final List<Point2D> subsetDistorted = new ArrayList<>();
262
263 // subset of undistorted (i.e. ideal) points
264 private final List<Point2D> subsetUndistorted = new ArrayList<>();
265
266 @Override
267 public int getTotalSamples() {
268 return distortedPoints.size();
269 }
270
271 @Override
272 public int getSubsetSize() {
273 return RadialDistortionRobustEstimator.MIN_NUMBER_OF_POINTS;
274 }
275
276 @Override
277 public void estimatePreliminarSolutions(
278 final int[] samplesIndices, final List<RadialDistortion> solutions) {
279 subsetDistorted.clear();
280 subsetDistorted.add(distortedPoints.get(samplesIndices[0]));
281 subsetDistorted.add(distortedPoints.get(samplesIndices[1]));
282
283 subsetUndistorted.clear();
284 subsetUndistorted.add(undistortedPoints.get(samplesIndices[0]));
285 subsetUndistorted.add(undistortedPoints.get(samplesIndices[1]));
286
287 try {
288 radialDistortionEstimator.setPoints(distortedPoints, undistortedPoints);
289 radialDistortionEstimator.setPoints(subsetDistorted, subsetUndistorted);
290
291 final var distortion = radialDistortionEstimator.estimate();
292 solutions.add(distortion);
293 } catch (final Exception e) {
294 // if anything fails, no solution is added
295 }
296 }
297
298 @Override
299 public double computeResidual(final RadialDistortion currentEstimation, final int i) {
300 final var distortedPoint = distortedPoints.get(i);
301 final var undistortedPoint = undistortedPoints.get(i);
302
303 currentEstimation.distort(undistortedPoint, testPoint);
304
305 return testPoint.distanceTo(distortedPoint);
306 }
307
308 @Override
309 public boolean isReady() {
310 return LMedSRadialDistortionRobustEstimator.this.isReady();
311 }
312
313 @Override
314 public void onEstimateStart(final RobustEstimator<RadialDistortion> estimator) {
315 try {
316 radialDistortionEstimator.setLMSESolutionAllowed(false);
317 radialDistortionEstimator.setIntrinsic(getIntrinsic());
318 } catch (final Exception e) {
319 Logger.getLogger(LMedSRadialDistortionRobustEstimator.class.getName()).log(Level.WARNING,
320 "Could not set intrinsic parameters on radial distortion estimator", e);
321 }
322
323 if (listener != null) {
324 listener.onEstimateStart(LMedSRadialDistortionRobustEstimator.this);
325 }
326 }
327
328 @Override
329 public void onEstimateEnd(final RobustEstimator<RadialDistortion> estimator) {
330 if (listener != null) {
331 listener.onEstimateEnd(LMedSRadialDistortionRobustEstimator.this);
332 }
333 }
334
335 @Override
336 public void onEstimateNextIteration(
337 final RobustEstimator<RadialDistortion> estimator, final int iteration) {
338 if (listener != null) {
339 listener.onEstimateNextIteration(LMedSRadialDistortionRobustEstimator.this, iteration);
340 }
341 }
342
343 @Override
344 public void onEstimateProgressChange(
345 final RobustEstimator<RadialDistortion> estimator, final float progress) {
346 if (listener != null) {
347 listener.onEstimateProgressChange(LMedSRadialDistortionRobustEstimator.this, progress);
348 }
349 }
350 });
351
352 try {
353 locked = true;
354 innerEstimator.setConfidence(confidence);
355 innerEstimator.setMaxIterations(maxIterations);
356 innerEstimator.setProgressDelta(progressDelta);
357 innerEstimator.setStopThreshold(stopThreshold);
358 return innerEstimator.estimate();
359 } catch (final com.irurueta.numerical.LockedException e) {
360 throw new LockedException(e);
361 } catch (final com.irurueta.numerical.NotReadyException e) {
362 throw new NotReadyException(e);
363 } finally {
364 locked = false;
365 }
366 }
367
368 /**
369 * Returns method being used for robust estimation
370 *
371 * @return method being used for robust estimation
372 */
373 @Override
374 public RobustEstimatorMethod getMethod() {
375 return RobustEstimatorMethod.LMEDS;
376 }
377 }