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.MSACRobustEstimator;
24 import com.irurueta.numerical.robust.MSACRobustEstimatorListener;
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 * MSAC algorithm.
37 */
38 public class MSACRadialDistortionRobustEstimator 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 MSACRadialDistortionRobustEstimator() {
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 MSACRadialDistortionRobustEstimator(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 MSACRadialDistortionRobustEstimator(final List<Point2D> distortedPoints,
91 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 MSACRadialDistortionRobustEstimator(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 MSACRadialDistortionRobustEstimator(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 MSACRadialDistortionRobustEstimator(
148 final List<Point2D> distortedPoints, final List<Point2D> undistortedPoints, final Point2D distortionCenter,
149 final RadialDistortionRobustEstimatorListener listener) {
150 super(distortedPoints, undistortedPoints, distortionCenter, listener);
151 threshold = DEFAULT_THRESHOLD;
152 }
153
154 /**
155 * Returns threshold to determine whether points are inliers or not when
156 * testing possible estimation solutions.
157 * The threshold refers to the amount of error (i.e. Euclidean distance) a
158 * possible solution has on projected 2D points.
159 *
160 * @return threshold to determine whether points are inliers or not when
161 * testing possible estimation solutions.
162 */
163 public double getThreshold() {
164 return threshold;
165 }
166
167 /**
168 * Sets threshold to determine whether points are inliers or not when
169 * testing possible estimation solutions.
170 * The threshold refers to the amount of error (i.e. Euclidean distance) a
171 * possible solution has on projected 2D points.
172 *
173 * @param threshold threshold to be set.
174 * @throws IllegalArgumentException if provided value is equal or less than
175 * zero.
176 * @throws LockedException if robust estimator is locked because an
177 * estimation is already in progress.
178 */
179 public void setThreshold(final double threshold) throws LockedException {
180 if (isLocked()) {
181 throw new LockedException();
182 }
183 if (threshold <= MIN_THRESHOLD) {
184 throw new IllegalArgumentException();
185 }
186 this.threshold = threshold;
187 }
188
189 /**
190 * Estimates a radial distortion using a robust estimator and
191 * the best set of matched 2D points found using the robust estimator.
192 *
193 * @return a radial distortion.
194 * @throws LockedException if robust estimator is locked because an
195 * estimation is already in progress.
196 * @throws NotReadyException if provided input data is not enough to start
197 * the estimation.
198 * @throws RobustEstimatorException if estimation fails for any reason
199 * (i.e. numerical instability, no solution available, etc).
200 */
201 @SuppressWarnings("DuplicatedCode")
202 @Override
203 public RadialDistortion estimate() throws LockedException, NotReadyException, RobustEstimatorException {
204 if (isLocked()) {
205 throw new LockedException();
206 }
207 if (!isReady()) {
208 throw new NotReadyException();
209 }
210
211 final var innerEstimator = new MSACRobustEstimator<RadialDistortion>(new MSACRobustEstimatorListener<>() {
212
213 // point to be reused when computing residuals
214 private final Point2D testPoint = Point2D.create(CoordinatesType.INHOMOGENEOUS_COORDINATES);
215
216 // non-robust radial distortion estimator
217 private final LMSERadialDistortionEstimator radialDistortionEstimator = new LMSERadialDistortionEstimator();
218
219 // subset of distorted (i.e. measured) points
220 private final List<Point2D> subsetDistorted = new ArrayList<>();
221
222 // subset of undistorted (i.e. ideal) points
223 private final List<Point2D> subsetUndistorted = new ArrayList<>();
224
225 @Override
226 public double getThreshold() {
227 return threshold;
228 }
229
230 @Override
231 public int getTotalSamples() {
232 return distortedPoints.size();
233 }
234
235 @Override
236 public int getSubsetSize() {
237 return RadialDistortionRobustEstimator.MIN_NUMBER_OF_POINTS;
238 }
239
240 @Override
241 public void estimatePreliminarSolutions(
242 final int[] samplesIndices, final List<RadialDistortion> solutions) {
243 subsetDistorted.clear();
244 subsetDistorted.add(distortedPoints.get(samplesIndices[0]));
245 subsetDistorted.add(distortedPoints.get(samplesIndices[1]));
246
247 subsetUndistorted.clear();
248 subsetUndistorted.add(undistortedPoints.get(samplesIndices[0]));
249 subsetUndistorted.add(undistortedPoints.get(samplesIndices[1]));
250
251 try {
252 radialDistortionEstimator.setPoints(distortedPoints, undistortedPoints);
253 radialDistortionEstimator.setPoints(subsetDistorted, subsetUndistorted);
254
255 final var distortion = radialDistortionEstimator.estimate();
256 solutions.add(distortion);
257 } catch (final Exception e) {
258 // if anything fails, no solution is added
259 }
260 }
261
262 @Override
263 public double computeResidual(final RadialDistortion currentEstimation, final int i) {
264 final var distortedPoint = distortedPoints.get(i);
265 final var undistortedPoint = undistortedPoints.get(i);
266
267 currentEstimation.distort(undistortedPoint, testPoint);
268
269 return testPoint.distanceTo(distortedPoint);
270 }
271
272 @Override
273 public boolean isReady() {
274 return MSACRadialDistortionRobustEstimator.this.isReady();
275 }
276
277 @Override
278 public void onEstimateStart(final RobustEstimator<RadialDistortion> estimator) {
279 try {
280 radialDistortionEstimator.setLMSESolutionAllowed(false);
281 radialDistortionEstimator.setIntrinsic(getIntrinsic());
282 } catch (final Exception e) {
283 Logger.getLogger(MSACRadialDistortionRobustEstimator.class.getName()).log(Level.WARNING,
284 "Could not set intrinsic parameters on radial distortion estimator", e);
285 }
286
287 if (listener != null) {
288 listener.onEstimateStart(MSACRadialDistortionRobustEstimator.this);
289 }
290 }
291
292 @Override
293 public void onEstimateEnd(final RobustEstimator<RadialDistortion> estimator) {
294 if (listener != null) {
295 listener.onEstimateEnd(
296 MSACRadialDistortionRobustEstimator.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(MSACRadialDistortionRobustEstimator.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(MSACRadialDistortionRobustEstimator.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.MSAC;
340 }
341 }