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.algebra.AlgebraException;
19 import com.irurueta.algebra.Matrix;
20 import com.irurueta.algebra.Utils;
21 import com.irurueta.ar.calibration.RadialDistortion;
22 import com.irurueta.ar.calibration.RadialDistortionException;
23 import com.irurueta.geometry.Point2D;
24 import com.irurueta.geometry.estimators.LockedException;
25 import com.irurueta.geometry.estimators.NotReadyException;
26
27 import java.util.List;
28
29 /**
30 * This class defines an LMSE (the Least Mean Square Error) estimator of radial
31 * distortion.
32 * Equations to determine a RadialDistortion instance for a single point are
33 * linear dependent, for that reason, at least 2 points are required for the
34 * estimation.
35 * Even though x and y equations are linear dependent, both equations are taken
36 * into account in case that sampled data contains errors, so that an LMSE error
37 * can be obtained.
38 */
39 public class LMSERadialDistortionEstimator extends RadialDistortionEstimator {
40 /**
41 * Indicates if by default an LMSE (the Least Mean Square Error) solution is
42 * allowed if more correspondences than the minimum are provided.
43 */
44 public static final boolean DEFAULT_ALLOW_LMSE_SOLUTION = false;
45
46 /**
47 * Indicates if an LMSE (the Least Mean Square Error) solution is allowed if
48 * more correspondences than the minimum are provided. If false, the
49 * exceeding correspondences will be ignored and only the 6 first
50 * correspondences will be used.
51 */
52 private boolean allowLMSESolution;
53
54 /**
55 * Constructor.
56 */
57 public LMSERadialDistortionEstimator() {
58 super();
59 allowLMSESolution = DEFAULT_ALLOW_LMSE_SOLUTION;
60 }
61
62 /**
63 * Constructor with listener.
64 *
65 * @param listener listener to be notified of events such as when estimation
66 * starts, ends or estimation progress changes.
67 */
68 public LMSERadialDistortionEstimator(final RadialDistortionEstimatorListener listener) {
69 super(listener);
70 allowLMSESolution = DEFAULT_ALLOW_LMSE_SOLUTION;
71 }
72
73 /**
74 * Constructor.
75 *
76 * @param distortedPoints list of distorted points. Distorted points are
77 * obtained after radial distortion is applied to an undistorted point.
78 * @param undistortedPoints list of undistorted points.
79 * @throws IllegalArgumentException if provided lists of points don't have
80 * the same size.
81 */
82 public LMSERadialDistortionEstimator(final List<Point2D> distortedPoints, final List<Point2D> undistortedPoints) {
83 super(distortedPoints, undistortedPoints);
84 allowLMSESolution = DEFAULT_ALLOW_LMSE_SOLUTION;
85 }
86
87 /**
88 * Constructor.
89 *
90 * @param distortedPoints list of distorted points. Distorted points are
91 * obtained after radial distortion is applied to an undistorted point.
92 * @param undistortedPoints list of undistorted points.
93 * @param listener listener to be notified of events such as when estimation
94 * starts, ends or estimation progress changes.
95 * @throws IllegalArgumentException if provided lists of points don't have
96 * the same size.
97 */
98
99 public LMSERadialDistortionEstimator(final List<Point2D> distortedPoints, final List<Point2D> undistortedPoints,
100 final RadialDistortionEstimatorListener listener) {
101 super(distortedPoints, undistortedPoints, listener);
102 allowLMSESolution = DEFAULT_ALLOW_LMSE_SOLUTION;
103 }
104
105 /**
106 * Constructor with distortion center.
107 *
108 * @param distortionCenter Distortion center. This is usually equal to the
109 * principal point of an estimated camera. If not set it is assumed to be at
110 * the origin of coordinates (0,0).
111 */
112 public LMSERadialDistortionEstimator(final Point2D distortionCenter) {
113 super(distortionCenter);
114 }
115
116 /**
117 * Constructor with listener and distortion center.
118 *
119 * @param distortionCenter Distortion center. This is usually equal to the
120 * principal point of an estimated camera. If not set it is assumed to be at
121 * the origin of coordinates (0,0).
122 * @param listener listener to be notified of events such as when estimation
123 * starts, ends or estimation progress changes.
124 */
125 public LMSERadialDistortionEstimator(
126 final Point2D distortionCenter, final RadialDistortionEstimatorListener listener) {
127 super(distortionCenter, listener);
128 }
129
130 /**
131 * Constructor with points and distortion center.
132 *
133 * @param distortedPoints list of distorted points. Distorted points are
134 * obtained after radial distortion is applied to an undistorted point.
135 * @param undistortedPoints list of undistorted points.
136 * @param distortionCenter Distortion center. This is usually equal to the
137 * principal point of an estimated camera. If not set it is assumed to be at
138 * the origin of coordinates (0,0).
139 * @throws IllegalArgumentException if provided lists of points don't have
140 * the same size.
141 */
142 public LMSERadialDistortionEstimator(final List<Point2D> distortedPoints, final List<Point2D> undistortedPoints,
143 final Point2D distortionCenter) {
144 super(distortedPoints, undistortedPoints, distortionCenter);
145 }
146
147 /**
148 * Constructor
149 *
150 * @param distortedPoints list of distorted points. Distorted points are
151 * obtained after radial distortion is applied to an undistorted point.
152 * @param undistortedPoints list of undistorted points.
153 * @param distortionCenter Distortion center. This is usually equal to the
154 * principal point of an estimated camera. If not set it is assumed to be at
155 * the origin of coordinates (0,0).
156 * @param listener listener to be notified of events such as when estimation
157 * starts, ends or estimation progress changes.
158 * @throws IllegalArgumentException if provided lists of points don't have
159 * the same size.
160 */
161 public LMSERadialDistortionEstimator(
162 final List<Point2D> distortedPoints, final List<Point2D> undistortedPoints, final Point2D distortionCenter,
163 final RadialDistortionEstimatorListener listener) {
164 super(distortedPoints, undistortedPoints, distortionCenter, listener);
165 }
166
167 /**
168 * Indicates if an LMSE (the Least Mean Square Error) solution is allowed if
169 * more correspondences than the minimum are provided. If false, the
170 * exceeding correspondences will be ignored and only the 6 first
171 * correspondences will be used.
172 *
173 * @return true if LMSE solution is allowed, false otherwise.
174 */
175 public boolean isLMSESolutionAllowed() {
176 return allowLMSESolution;
177 }
178
179 /**
180 * Specifies if an LMSE (the Least Mean Square Error) solution is allowed if
181 * more correspondences than the minimum are provided. If false, the
182 * exceeding correspondences will be ignored and only the 6 first
183 * correspondences will be used.
184 *
185 * @param allowed true if LMSE solution is allowed, false otherwise.
186 * @throws LockedException if estimator is locked.
187 */
188 public void setLMSESolutionAllowed(final boolean allowed) throws LockedException {
189 if (isLocked()) {
190 throw new LockedException();
191 }
192 allowLMSESolution = allowed;
193 }
194
195 /**
196 * Estimates a radial distortion.
197 *
198 * @return estimated radial distortion.
199 * @throws LockedException if estimator is loked.
200 * @throws NotReadyException if input has not yet been provided.
201 * @throws RadialDistortionEstimatorException if an error occurs during
202 * estimation, usually because input data is not valid.
203 */
204 @SuppressWarnings("DuplicatedCode")
205 @Override
206 public RadialDistortion estimate() throws LockedException, NotReadyException, RadialDistortionEstimatorException {
207 if (isLocked()) {
208 throw new LockedException();
209 }
210 if (!isReady()) {
211 throw new NotReadyException();
212 }
213
214 try {
215 locked = true;
216 if (listener != null) {
217 listener.onEstimateStart(this);
218 }
219
220 final var nPoints = distortedPoints.size();
221
222 int numRows;
223 if (isLMSESolutionAllowed()) {
224 // initialize new matrix having two rows per point
225 numRows = 2 * nPoints;
226 } else {
227 // when LMSE is not allowed, restrict matrix to two rows (minimum
228 // value required for a solution)
229 numRows = 2 * getMinNumberOfMatchedPoints();
230 }
231
232 final var aMatrix = new Matrix(numRows, numKParams);
233 final var b = new double[numRows];
234
235 final var iteratorDistorted = distortedPoints.iterator();
236 final var iteratorUndistorted = undistortedPoints.iterator();
237
238 Point2D distorted;
239 Point2D undistorted;
240 var counter = 0;
241
242 // undistorted normalized homogeneous coordinates
243 double uNormHomX;
244 double uNormHomY;
245 double uNormHomW;
246 // undistorted normalized inhomogeneous coordinates
247 double uNormInhomX;
248 double uNormInhomY;
249 // undistorted denormalized homogeneous coordinates
250 double uDenormHomX;
251 double uDenormHomY;
252 double uDenormHomW;
253 // undistorted denormalized inhomogeneous coordinates
254 double uDenormInhomX;
255 double uDenormInhomY;
256 // distorted inhomogeneous coordinates
257 double dInhomX;
258 double dInhomY;
259 double rowNormX;
260 double rowNormY;
261
262 // radial distortion center
263 var centerX = 0.0;
264 var centerY = 0.0;
265 if (distortionCenter != null) {
266 centerX = distortionCenter.getInhomX();
267 centerY = distortionCenter.getInhomY();
268 }
269
270 // radial distance of undistorted normalized (calibration independent)
271 // coordinates
272 double r2;
273 double a;
274 double value;
275
276 while (iteratorDistorted.hasNext() && iteratorUndistorted.hasNext()) {
277 distorted = iteratorDistorted.next();
278 undistorted = iteratorUndistorted.next();
279
280 undistorted.normalize();
281
282 uDenormHomX = undistorted.getHomX();
283 uDenormHomY = undistorted.getHomY();
284 uDenormHomW = undistorted.getHomW();
285
286 uDenormInhomX = uDenormHomX / uDenormHomW;
287 uDenormInhomY = uDenormHomY / uDenormHomW;
288
289 // multiply intrinsic parameters by undistorted point
290 uNormHomX = kInv.getElementAt(0, 0) * uDenormHomX
291 + kInv.getElementAt(0, 1) * uDenormHomY
292 + kInv.getElementAt(0, 2) * uDenormHomW;
293 uNormHomY = kInv.getElementAt(1, 0) * uDenormHomX
294 + kInv.getElementAt(1, 1) * uDenormHomY
295 + kInv.getElementAt(1, 2) * uDenormHomW;
296 uNormHomW = kInv.getElementAt(2, 0) * uDenormHomX
297 + kInv.getElementAt(2, 1) * uDenormHomY
298 + kInv.getElementAt(2, 2) * uDenormHomW;
299
300 uNormInhomX = uNormHomX / uNormHomW;
301 uNormInhomY = uNormHomY / uNormHomW;
302
303 r2 = uNormInhomX * uNormInhomX + uNormInhomY * uNormInhomY;
304
305 dInhomX = distorted.getInhomX();
306 dInhomY = distorted.getInhomY();
307
308 a = 1.0;
309 rowNormX = rowNormY = 0.0;
310 for (var i = 0; i < numKParams; i++) {
311 a *= r2;
312
313 // x and y coordinates generate linear dependent equations, for
314 // that reason we need more than one point
315
316 // x coordinates
317 value = (uDenormInhomX - centerX) * a;
318 aMatrix.setElementAt(2 * counter, i, value);
319
320 rowNormX += Math.pow(value, 2.0);
321
322 // y coordinates
323 value = (uDenormInhomY - centerY) * a;
324 aMatrix.setElementAt(2 * counter + 1, i, value);
325
326 rowNormY += Math.pow(value, 2.0);
327 }
328
329 // x coordinates
330 value = dInhomX - uDenormInhomX;
331 b[2 * counter] = value;
332
333 rowNormX += Math.pow(value, 2.0);
334
335 // y coordinates
336 value = dInhomY - uDenormInhomY;
337 b[2 * counter + 1] = value;
338
339 rowNormY += Math.pow(value, 2.0);
340
341 // normalize rows to increase accuracy
342 for (var i = 0; i < numKParams; i++) {
343 aMatrix.setElementAt(2 * counter, i,
344 aMatrix.getElementAt(2 * counter, i) / rowNormX);
345 aMatrix.setElementAt(2 * counter + 1, i,
346 aMatrix.getElementAt(2 * counter + 1, i) / rowNormY);
347 }
348
349 b[2 * counter] /= rowNormX;
350 b[2 * counter + 1] /= rowNormY;
351
352 counter++;
353
354 if (!isLMSESolutionAllowed() && (counter >= getMinNumberOfMatchedPoints())) {
355 break;
356 }
357 }
358
359 final var params = Utils.solve(aMatrix, b);
360
361 final var distortion = new RadialDistortion(params, distortionCenter, horizontalFocalLength,
362 verticalFocalLength, skew);
363
364 if (listener != null) {
365 listener.onEstimateEnd(this);
366 }
367
368 return distortion;
369 } catch (final AlgebraException | RadialDistortionException e) {
370 throw new RadialDistortionEstimatorException(e);
371 } finally {
372 locked = false;
373 }
374 }
375
376 /**
377 * Returns type of radial distortion estimator.
378 *
379 * @return type of radial distortion estimator.
380 */
381 @Override
382 public RadialDistortionEstimatorType getType() {
383 return RadialDistortionEstimatorType.LMSE_RADIAL_DISTORTION_ESTIMATOR;
384 }
385 }