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;
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.estimators.LMSERadialDistortionEstimator;
22 import com.irurueta.ar.calibration.estimators.RadialDistortionEstimatorException;
23 import com.irurueta.geometry.GeometryException;
24 import com.irurueta.geometry.InhomogeneousPoint2D;
25 import com.irurueta.geometry.PinholeCameraIntrinsicParameters;
26 import com.irurueta.geometry.Point2D;
27
28 import java.io.Serializable;
29 import java.util.ArrayList;
30
31 /**
32 * Class implementing Brown's radial distortion.
33 * This kind of distortion is usually modelled after the distortion introduced
34 * by lenses on cameras when taking short range pictures using wide angle
35 * lenses.
36 * Brown's radial distortion typically follow an expression such as:
37 * xd = xu + (xu - xc)*(K1*r^2 + K2*r^4 + ...)
38 * yd = yu + (yu - yc)*(K1*r^2 + K2*r^4 + ...),
39 * where (xu, yu) stands for the undistorted point coordinates,
40 * (xd, yd) stands for the distorted point coordinates,
41 * (xc, yc) stands for the distortion center,
42 * r^2 is typically (xu - xc)^2 + (yu - yc)^2 stands for the squared distance
43 * of the distorted point respect to the distortion center. r^2 is computed
44 * taking into account provided intrinsic parameters
45 * And K1, K2 are the distortion parameters. Usually K1 dominates and K2 is
46 * much smaller. Further terms are usually neglected as they are not meaningful
47 * and typically produce numerical instabilities, but can also be provided in
48 * array form.
49 * <p>
50 * NOTE: in order to be able to converge to correct values when computing
51 * distortions, RadialDistortion should work with normalized point coordinates
52 * between -1.0 and 1.0.
53 */
54 @SuppressWarnings("DuplicatedCode")
55 public class RadialDistortion extends Distortion implements Serializable {
56
57 /**
58 * Defines default focal length if none is defined.
59 */
60 public static final double DEFAULT_FOCAL_LENGTH = 1.0;
61
62 /**
63 * Defines default skewness if none is defined.
64 */
65 public static final double DEFAULT_SKEW = 0.0;
66
67 /**
68 * Default maximum number of iterations to do when attempting to un-distort
69 * a point if convergence is not reached.
70 */
71 public static final int DEFAULT_MAX_ITERS = 20;
72
73 /**
74 * Default tolerance to consider point convergence when un-distorting a point.
75 */
76 public static final double DEFAULT_TOLERANCE = 1e-5;
77
78 /**
79 * Radial distortion center.
80 */
81 private Point2D center;
82
83 /**
84 * Radial distortion parameters.
85 */
86 private double[] kParams;
87
88 /**
89 * Horizontal focal length expressed in pixels.
90 */
91 private double horizontalFocalLength;
92
93 /**
94 * Vertical focal length expressed in pixels.
95 */
96 private double verticalFocalLength;
97
98 /**
99 * Skew in pixels.
100 */
101 private double skew;
102
103 /**
104 * Inverse of intrinsic parameters' matrix.
105 */
106 private Matrix kInv;
107
108 /**
109 * Constructor.
110 */
111 public RadialDistortion() {
112 try {
113 setIntrinsic(null, DEFAULT_FOCAL_LENGTH, DEFAULT_FOCAL_LENGTH, DEFAULT_SKEW);
114 } catch (final RadialDistortionException ignore) {
115 // never happens
116 }
117 }
118
119 /**
120 * Constructor with radial distortion parameters and center assumed to be
121 * at the origin of coordinates (0, 0).
122 *
123 * @param k1 first degree distortion parameter.
124 * @param k2 second degree distortion parameter.
125 */
126 public RadialDistortion(final double k1, final double k2) {
127 kParams = new double[]{k1, k2};
128 try {
129 setIntrinsic(null, DEFAULT_FOCAL_LENGTH, DEFAULT_FOCAL_LENGTH, DEFAULT_SKEW);
130 } catch (final RadialDistortionException ignore) {
131 // never happens
132 }
133 }
134
135 /**
136 * Constructor with radial distortion parameters and center assumed to be
137 * at the origin of coordinates (0, 0).
138 *
139 * @param kParams radial distortion parameters of any length.
140 * @throws IllegalArgumentException if radial distortion parameters is null.
141 */
142 public RadialDistortion(final double[] kParams) {
143 if (kParams == null) {
144 throw new IllegalArgumentException();
145 }
146 this.kParams = kParams;
147 try {
148 setIntrinsic(null, DEFAULT_FOCAL_LENGTH, DEFAULT_FOCAL_LENGTH, DEFAULT_SKEW);
149 } catch (final RadialDistortionException ignore) {
150 // never happens
151 }
152 }
153
154 /**
155 * Constructor with radial distortion parameters and center.
156 *
157 * @param k1 first degree distortion parameter.
158 * @param k2 second degree distortion parameter.
159 * @param center center of radial distortion. If null it is assumed to be
160 * at the origin of coordinates (0, 0), which is the typical value.
161 */
162 public RadialDistortion(final double k1, final double k2, final Point2D center) {
163 this(k1, k2);
164 try {
165 setIntrinsic(center, DEFAULT_FOCAL_LENGTH, DEFAULT_FOCAL_LENGTH, DEFAULT_SKEW);
166 } catch (final RadialDistortionException ignore) {
167 // never happens
168 }
169 }
170
171 /**
172 * Constructor with radial distortion parameters and center.
173 *
174 * @param kParams radial distortion parameters of any length.
175 * @param center center of radial distortion. If null it is assumed to be
176 * at the origin of coordinates (0, 0), which is the typical value.
177 * @throws IllegalArgumentException if radial distortion parameters is null.
178 */
179 public RadialDistortion(final double[] kParams, final Point2D center) {
180 this(kParams);
181 try {
182 setIntrinsic(center, DEFAULT_FOCAL_LENGTH, DEFAULT_FOCAL_LENGTH, DEFAULT_SKEW);
183 } catch (final RadialDistortionException ignore) {
184 // never happens
185 }
186 }
187
188 /**
189 * Constructor with radial distortion parameters, center and other
190 * camera intrinsic parameters.
191 *
192 * @param k1 first degree distortion parameter.
193 * @param k2 second degree distortion parameter.
194 * @param center center of radial distortion. If null it is assumed to be at
195 * the origin of coordinates (0, 0), which is the typical value.
196 * @param horizontalFocalLength horizontal focal length expressed in pixels.
197 * @param verticalFocalLength vertical focal length expressed in pixels.
198 * @param skew skew expressed in pixels.
199 * @throws RadialDistortionException if provided focal lengths are
200 * degenerate (i.e. zero).
201 */
202 public RadialDistortion(
203 final double k1, final double k2, final Point2D center, final double horizontalFocalLength,
204 final double verticalFocalLength, final double skew) throws RadialDistortionException {
205 this(k1, k2);
206 setIntrinsic(center, horizontalFocalLength, verticalFocalLength, skew);
207 }
208
209 /**
210 * Constructor with radial distortion parameters, center and other camera
211 * intrinsic parameters.
212 *
213 * @param kParams radial distortion parameters of any length.
214 * @param center center of radial distortion. If null it is assumed to be at
215 * the origin of coordinates (0, 0), which is the typical value.
216 * @param horizontalFocalLength horizontal focal length expressed in pixels.
217 * @param verticalFocalLength vertical focal length expressed in pixels.
218 * @param skew skew expressed in pixels.
219 * @throws RadialDistortionException if provided focal lengths are
220 * degenerate (i.e. zero).
221 * @throws IllegalArgumentException if radial distortion parameters is null.
222 */
223 public RadialDistortion(final double[] kParams, final Point2D center, final double horizontalFocalLength,
224 final double verticalFocalLength, final double skew) throws RadialDistortionException {
225 this(kParams);
226 setIntrinsic(center, horizontalFocalLength, verticalFocalLength, skew);
227 }
228
229 /**
230 * Constructor with points and distortion center.
231 *
232 * @param distortedPoint1 1st distorted point (i.e. measured).
233 * @param distortedPoint2 2nd distorted point (i.e. measured).
234 * @param undistortedPoint1 1st undistorted point (i.e. ideal).
235 * @param undistortedPoint2 2nd undistorted point (i.e. undistorted).
236 * @param distortionCenter distortion center or null if center is at origin
237 * of coordinates (which is the typical value).
238 * @throws RadialDistortionException if distortion could not be estimated.
239 */
240 public RadialDistortion(final Point2D distortedPoint1, final Point2D distortedPoint2,
241 final Point2D undistortedPoint1, final Point2D undistortedPoint2,
242 final Point2D distortionCenter) throws RadialDistortionException {
243 super();
244
245 setFromPointsAndCenter(distortedPoint1, distortedPoint2, undistortedPoint1, undistortedPoint2,
246 distortionCenter);
247 }
248
249 /**
250 * Estimates this radial distortion from points and distortion center.
251 *
252 * @param distortedPoint1 1st distorted point (i.e. measured).
253 * @param distortedPoint2 2nd distorted point (i.e. measured).
254 * @param undistortedPoint1 1st undistorted point (i.e. ideal).
255 * @param undistortedPoint2 2nd undistorted point (i.e. undistorted).
256 * @param distortionCenter distortion center or null if center is at origin
257 * of coordinates (which is the typical value).
258 * @throws RadialDistortionException if distortion could not be estimated.
259 */
260 public final void setFromPointsAndCenter(
261 final Point2D distortedPoint1, final Point2D distortedPoint2,
262 final Point2D undistortedPoint1, final Point2D undistortedPoint2, final Point2D distortionCenter)
263 throws RadialDistortionException {
264
265 final var distortedPoints = new ArrayList<Point2D>();
266 final var undistortedPoints = new ArrayList<Point2D>();
267
268 distortedPoints.add(distortedPoint1);
269 distortedPoints.add(distortedPoint2);
270
271 undistortedPoints.add(undistortedPoint1);
272 undistortedPoints.add(undistortedPoint2);
273
274 try {
275 final var estimator = new LMSERadialDistortionEstimator(distortedPoints, undistortedPoints,
276 distortionCenter);
277 estimator.setLMSESolutionAllowed(false);
278 final var distortion = estimator.estimate();
279
280 kParams = distortion.kParams;
281 center = distortion.center;
282 } catch (final GeometryException | RadialDistortionEstimatorException e) {
283 throw new RadialDistortionException(e);
284 }
285 }
286
287 /**
288 * Returns radial distortion center.
289 *
290 * @return radial distortion center.
291 */
292 public Point2D getCenter() {
293 return center;
294 }
295
296 /**
297 * Sets radial distortion center.
298 *
299 * @param center radial distortion center to be set.
300 */
301 public void setCenter(final Point2D center) {
302 try {
303 setIntrinsic(center, horizontalFocalLength, verticalFocalLength, skew);
304 } catch (final RadialDistortionException ignore) {
305 // never happens
306 }
307 }
308
309 /**
310 * Returns horizontal focal length expressed in pixels.
311 *
312 * @return horizontal focal length expressed in pixels.
313 */
314 public double getHorizontalFocalLength() {
315 return horizontalFocalLength;
316 }
317
318 /**
319 * Sets horizontal focal length expressed in pixels.
320 *
321 * @param horizontalFocalLength horizontal focal length expressed in pixels.
322 * @throws RadialDistortionException if provided value is degenerate (i.e.
323 * zero).
324 */
325 public void setHorizontalFocalLength(final double horizontalFocalLength) throws RadialDistortionException {
326 setIntrinsic(center, horizontalFocalLength, verticalFocalLength, skew);
327 }
328
329 /**
330 * Returns vertical focal length expressed in pixels.
331 *
332 * @return vertical focal length expressed in pixels.
333 */
334 public double getVerticalFocalLength() {
335 return verticalFocalLength;
336 }
337
338 /**
339 * Sets vertical focal length expressed in pixels.
340 *
341 * @param verticalFocalLength vertical focal length expressed in pixels.
342 * @throws RadialDistortionException if provided value is degenerate (i.e.
343 * zero).
344 */
345 public void setVerticalFocalLength(final double verticalFocalLength) throws RadialDistortionException {
346 setIntrinsic(center, horizontalFocalLength, verticalFocalLength, skew);
347 }
348
349 /**
350 * Returns skew expressed in pixels.
351 *
352 * @return skew expressed in pixels.
353 */
354 public double getSkew() {
355 return skew;
356 }
357
358 /**
359 * Sets skew expressed in pixels.
360 *
361 * @param skew skew expressed in pixels.
362 */
363 public void setSkew(final double skew) {
364 try {
365 setIntrinsic(center, horizontalFocalLength, verticalFocalLength, skew);
366 } catch (final RadialDistortionException ignore) {
367 // never happens
368 }
369 }
370
371 /**
372 * Returns pinhole camera intrinsic parameters associated to this distortion.
373 *
374 * @return pinhole camera intrinsic parameters associated to this distortion.
375 */
376 public PinholeCameraIntrinsicParameters getIntrinsic() {
377 return new PinholeCameraIntrinsicParameters(horizontalFocalLength,
378 verticalFocalLength,
379 center != null ? center.getInhomX() : 0.0,
380 center != null ? center.getInhomY() : 0.0,
381 skew);
382 }
383
384 /**
385 * Sets intrinsic parameters from pinhole camera.
386 *
387 * @param intrinsic intrinsic parameters to be set.
388 * @throws RadialDistortionException if focal length is degenerate (i.e.
389 * zero).
390 */
391 public void setIntrinsic(final PinholeCameraIntrinsicParameters intrinsic) throws RadialDistortionException {
392 setIntrinsic(new InhomogeneousPoint2D(
393 intrinsic.getHorizontalPrincipalPoint(), intrinsic.getVerticalPrincipalPoint()),
394 intrinsic.getHorizontalFocalLength(),
395 intrinsic.getVerticalFocalLength(),
396 intrinsic.getSkewness());
397 }
398
399 /**
400 * Sets intrinsic parameters.
401 *
402 * @param center radial distortion center.
403 * @param horizontalFocalLength horizontal focal length expressed in pixels.
404 * @param verticalFocalLength vertical focal length expressed in pixels.
405 * @param skew skew expressed in pixels.
406 * @throws RadialDistortionException if focal length is degenerate (i.e.
407 * zero).
408 */
409 public final void setIntrinsic(
410 final Point2D center, final double horizontalFocalLength, final double verticalFocalLength,
411 final double skew) throws RadialDistortionException {
412 this.center = center;
413 this.horizontalFocalLength = horizontalFocalLength;
414 this.verticalFocalLength = verticalFocalLength;
415 this.skew = skew;
416
417 try {
418 if (kInv == null) {
419 kInv = new Matrix(3, 3);
420 }
421
422 // initially matrix is zero
423 final var k = new Matrix(3, 3);
424
425 k.setElementAt(0, 0, horizontalFocalLength);
426 k.setElementAt(1, 1, verticalFocalLength);
427 k.setElementAt(0, 1, skew);
428 if (this.center != null) {
429 // if center is not provided, values are zero
430 k.setElementAt(0, 2, this.center.getInhomX());
431 k.setElementAt(1, 2, this.center.getInhomY());
432 }
433 k.setElementAt(2, 2, 1.0);
434
435 Utils.inverse(k, kInv);
436 } catch (final AlgebraException e) {
437 throw new RadialDistortionException(e);
438 }
439 }
440
441 /**
442 * Returns first degree distortion parameter or zero if not available.
443 *
444 * @return first degree distortion parameter or zero if not available.
445 */
446 public double getK1() {
447 return kParams != null && kParams.length > 0 ? kParams[0] : 0.0;
448 }
449
450 /**
451 * Sets first degree distortion parameter.
452 *
453 * @param k1 first degree distortion parameter.
454 */
455 public void setK1(final double k1) {
456 if (kParams == null || kParams.length < 1) {
457 kParams = new double[]{k1};
458 }
459 }
460
461 /**
462 * Returns second degree distortion parameter or zero if not available.
463 *
464 * @return second degree distortion parameter or zero if not available.
465 */
466 public double getK2() {
467 return kParams != null && kParams.length > 1 ? kParams[1] : 0.0;
468 }
469
470 /**
471 * Sets second degree distortion parameter.
472 *
473 * @param k2 second degree distortion parameter to be set.
474 */
475 public void setK2(final double k2) {
476 if (kParams == null || kParams.length < 2) {
477 final var kp = new double[2];
478 kp[0] = getK1();
479 this.kParams = kp;
480 }
481 kParams[1] = k2;
482 }
483
484 /**
485 * Returns all radial distortion parameters. Typically only first and second
486 * degree radial distortion parameters are used.
487 *
488 * @return all radial distortion parameters.
489 */
490 public double[] getKParams() {
491 return kParams;
492 }
493
494 /**
495 * Sets all radial distortion parameters. With this method more than 2
496 * radial distortion parameters can be set if needed.
497 *
498 * @param kParams radial distortion parameters to be set.
499 */
500 public void setKParams(final double[] kParams) {
501 this.kParams = kParams;
502 }
503
504 /**
505 * Distorts provided 2D point and stores result into provided distorted
506 * point.
507 *
508 * @param undistortedPoint undistorted point to be undistorted.
509 * @param distortedPoint distorted point where result is stored.
510 */
511 @Override
512 public void distort(final Point2D undistortedPoint, final Point2D distortedPoint) {
513
514 undistortedPoint.normalize();
515
516 final var uHomX = undistortedPoint.getHomX();
517 final var uHomY = undistortedPoint.getHomY();
518 final var uHomW = undistortedPoint.getHomW();
519
520 // multiply mKinv with homogeneous undistorted point coordinates
521 // to normalize them respect to principal point and image size
522 final var uNormHomX = kInv.getElementAt(0, 0) * uHomX
523 + kInv.getElementAt(0, 1) * uHomY
524 + kInv.getElementAt(0, 2) * uHomW;
525 final double uNormHomY = kInv.getElementAt(1, 0) * uHomX
526 + kInv.getElementAt(1, 1) * uHomY
527 + kInv.getElementAt(1, 2) * uHomW;
528 final double uNormHomW = kInv.getElementAt(2, 0) * uHomX
529 + kInv.getElementAt(2, 1) * uHomY
530 + kInv.getElementAt(2, 2) * uHomW;
531
532 final var uNormInhomX = uNormHomX / uNormHomW;
533 final var uNormInhomY = uNormHomY / uNormHomW;
534
535 final var r2 = uNormInhomX * uNormInhomX + uNormInhomY * uNormInhomY;
536 var r = r2;
537
538 var sum = 0.0;
539 if (kParams != null) {
540 for (final var kParam : kParams) {
541 sum += kParam * r;
542 r *= r2;
543 }
544 }
545
546 final var uInhomX = uHomX / uHomW;
547 final var uInhomY = uHomY / uHomW;
548
549 var centerX = 0.0;
550 var centerY = 0.0;
551 if (center != null) {
552 centerX = center.getInhomX();
553 centerY = center.getInhomY();
554 }
555
556 final var dInhomX = uInhomX + (uInhomX - centerX) * sum;
557 final var dInhomY = uInhomY + (uInhomY - centerY) * sum;
558
559 distortedPoint.setInhomogeneousCoordinates(dInhomX, dInhomY);
560 }
561
562 /**
563 * Un-distorts provided 2D point and stores result into provided undistorted
564 * point
565 *
566 * @param distortedPoint distorted point to be undistorted
567 * @param undistortedPoint undistorted point where result is stored
568 */
569 @Override
570 public void undistort(final Point2D distortedPoint, final Point2D undistortedPoint) {
571 undistort(distortedPoint, undistortedPoint, DEFAULT_MAX_ITERS, DEFAULT_TOLERANCE);
572 }
573
574 /**
575 * Un-distorts provided 2D point and stores result into provided undistorted
576 * point.
577 *
578 * @param distortedPoint distorted point to be undistorted.
579 * @param undistortedPoint undistorted point where result is stored.
580 * @param maxIters maximum number of iterations to un-distort a point in case
581 * that convergence is not reached.
582 * @param tolerance tolerance to indicate that convergence has been reached.
583 */
584 public void undistort(final Point2D distortedPoint, final Point2D undistortedPoint, final int maxIters,
585 final double tolerance) {
586 if (maxIters <= 0) {
587 throw new IllegalArgumentException();
588 }
589 if (tolerance <= 0.0) {
590 throw new IllegalArgumentException();
591 }
592
593 distortedPoint.normalize();
594
595 final var dHomX = distortedPoint.getHomX();
596 final var dHomY = distortedPoint.getHomY();
597 final var dHomW = distortedPoint.getHomW();
598
599 // initial estimate of undistorted point
600 undistortedPoint.setHomogeneousCoordinates(dHomX, dHomY, dHomW);
601
602 final var uHomX = undistortedPoint.getHomX();
603 final var uHomY = undistortedPoint.getHomY();
604 final var uHomW = undistortedPoint.getHomW();
605
606 // multiply mKinv with homogeneous undistorted point coordinates
607 final var uHomXDenorm = kInv.getElementAt(0, 0) * uHomX
608 + kInv.getElementAt(0, 1) * uHomY
609 + kInv.getElementAt(0, 2) * uHomW;
610 final double uHomYDenorm = kInv.getElementAt(1, 0) * uHomX
611 + kInv.getElementAt(1, 1) * uHomY
612 + kInv.getElementAt(1, 2) * uHomW;
613 final double uHomWDenorm = kInv.getElementAt(2, 0) * uHomX
614 + kInv.getElementAt(2, 1) * uHomY
615 + kInv.getElementAt(2, 2) * uHomW;
616
617 final var origX = uHomXDenorm / uHomWDenorm;
618 final var origY = uHomYDenorm / uHomWDenorm;
619 var uInhomX = origX;
620 var uInhomY = origY;
621
622 // radial distortion magnitude
623 var sum = 0.0;
624 var prevSum = 0.0;
625 for (var iter = 0; iter < maxIters; iter++) {
626 // estimate the radial distance
627 final var r2 = uInhomX * uInhomX + uInhomY * uInhomY;
628 var r = r2;
629
630 sum = 0.0;
631 if (kParams != null) {
632 for (final var kParam : kParams) {
633 sum += kParam * r;
634 r *= r2;
635 }
636 }
637
638 uInhomX = origX / (1.0 + sum);
639 uInhomY = origY / (1.0 + sum);
640
641 if (Math.abs(prevSum - sum) <= tolerance) {
642 break;
643 } else {
644 prevSum = sum;
645 }
646 }
647
648 final var dInhomX = dHomX / dHomW;
649 final var dInhomY = dHomY / dHomW;
650
651 var centerX = 0.0;
652 var centerY = 0.0;
653 if (center != null) {
654 centerX = center.getInhomX();
655 centerY = center.getInhomY();
656 }
657
658 uInhomX = (dInhomX + centerX * sum) / (1.0 + sum);
659 uInhomY = (dInhomY + centerY * sum) / (1.0 + sum);
660
661 undistortedPoint.setInhomogeneousCoordinates(uInhomX, uInhomY);
662 }
663
664 /**
665 * Indicates whether this instance can distort points.
666 * This implementation always returns false, hence attempting to distort
667 * a point will result in a NotSupportedException being raised.
668 *
669 * @return true if points can be distorted, false otherwise.
670 */
671 @Override
672 public boolean canDistort() {
673 return true;
674 }
675
676 /**
677 * Indicates whether this instance can un-distort points.
678 * This implementation always returns true.
679 *
680 * @return true if points can be undistorted, false otherwise.
681 */
682 @Override
683 public boolean canUndistort() {
684 return true;
685 }
686
687
688 /**
689 * Returns kind of distortion.
690 *
691 * @return kind of distortion.
692 */
693 @Override
694 public DistortionKind getKind() {
695 return DistortionKind.BROWN_RADIAL_DISTORTION;
696 }
697 }