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.CholeskyDecomposer;
20 import com.irurueta.algebra.Matrix;
21 import com.irurueta.algebra.Utils;
22 import com.irurueta.geometry.Conic;
23 import com.irurueta.geometry.DualConic;
24 import com.irurueta.geometry.DualConicNotAvailableException;
25 import com.irurueta.geometry.InvalidPinholeCameraIntrinsicParametersException;
26 import com.irurueta.geometry.NonSymmetricMatrixException;
27 import com.irurueta.geometry.PinholeCameraIntrinsicParameters;
28
29 import java.io.Serializable;
30
31 /**
32 * The image of the absolute conic, is the projection of the absolute
33 * quadric using a given pinhole camera.
34 * Because the absolute quadric cannot be computed from the dual absolute
35 * quadric because the latter is degenerate, then this instance assumes
36 * that we always work on an ideal metric stratum.
37 * In such cases, the image of the absolute conic (IAC), is directly related
38 * to the inverse of pinhole camera intrinsic parameters.
39 */
40 public class ImageOfAbsoluteConic extends Conic implements Serializable {
41
42 /**
43 * Constructor.
44 * When working on a metric stratum, the IAC is related by the pinhole
45 * camera intrinsic parameters as C=(K^-1)'*(K^-1).
46 *
47 * @param k pinhole camera intrinsic parameters.
48 * @throws InvalidPinholeCameraIntrinsicParametersException if intrinsic
49 * parameters cannot be inverted (i.e. this
50 * might happen when values are incorrectly set
51 * such as when focal length is zero, etc.).
52 * Typically, this will never be thrown.
53 */
54 public ImageOfAbsoluteConic(final PinholeCameraIntrinsicParameters k)
55 throws InvalidPinholeCameraIntrinsicParametersException {
56 super();
57 setFromPinholeCameraIntrinsicParameters(k);
58 }
59
60 /**
61 * Constructor of this class. This constructor accepts every parameter
62 * describing a conic (parameters a, b, c, d, e, f).
63 *
64 * @param a Parameter A of the conic.
65 * @param b Parameter B of the conic.
66 * @param c Parameter C of the conic.
67 * @param d Parameter D of the conic.
68 * @param e Parameter E of the conic.
69 * @param f Parameter F of the conic.
70 */
71 public ImageOfAbsoluteConic(
72 final double a, final double b, final double c, final double d, final double e, final double f) {
73 super(a, b, c, d, e, f);
74 }
75
76 /**
77 * This method sets the matrix used to describe a conic.
78 * This matrix must be 3x3 and symmetric.
79 *
80 * @param m 3x3 Matrix describing the conic.
81 * @throws IllegalArgumentException Raised when the size of the matrix is
82 * not 3x3.
83 * @throws NonSymmetricMatrixException Raised when the conic matrix is not
84 * symmetric.
85 */
86 public ImageOfAbsoluteConic(final Matrix m) throws NonSymmetricMatrixException {
87 super(m);
88 }
89
90 /**
91 * Constructor without arguments.
92 */
93 protected ImageOfAbsoluteConic() {
94 super();
95 }
96
97 /**
98 * Computes the dual conic of this conic.
99 * The dual conic is equal to the inverse of the conic matrix.
100 *
101 * @return A new DualConic corresponding to the dual conic of this instance.
102 * @throws DualConicNotAvailableException Raised if the dual conic does not
103 * exist because this conic instance is degenerate (its inverse
104 * cannot be computed).
105 */
106 @Override
107 public DualConic getDualConic() throws DualConicNotAvailableException {
108 final var dualConic = new DualImageOfAbsoluteConic();
109 dualConic(dualConic);
110 return dualConic;
111 }
112
113 /**
114 * Sets IAC parameters from pinhole camera intrinsic parameters when we
115 * are working in a metric stratum, which is equal to C=(K^-1)'*(K^-1).
116 *
117 * @param k pinhole camera intrinsic parameters.
118 * @throws InvalidPinholeCameraIntrinsicParametersException if intrinsic parameters cannot be inverted
119 * (i.e. this might happen when values are
120 * incorrectly set such as when focal length
121 * is zero, etc.). Typically, this will never
122 * be thrown.
123 */
124 public final void setFromPinholeCameraIntrinsicParameters(
125 final PinholeCameraIntrinsicParameters k) throws InvalidPinholeCameraIntrinsicParametersException {
126 final var kMatrix = k.getInternalMatrix();
127 try {
128 final var invKMatrix = Utils.inverse(kMatrix);
129 setParameters(invKMatrix.transposeAndReturnNew().multiplyAndReturnNew(invKMatrix));
130 } catch (final AlgebraException e) {
131 throw new InvalidPinholeCameraIntrinsicParametersException(e);
132 } catch (final NonSymmetricMatrixException ignore) {
133 // this will never happen
134 }
135 }
136
137 /**
138 * Assuming that we are working in a metric stratum this method obtains the
139 * internal parameters of a pinhole camera analytically from IAC.
140 * This method should be preferred over Cholesky decomposition as it is
141 * more numerically stable.
142 *
143 * @return the internal parameters of a pinhole camera.
144 * @throws InvalidPinholeCameraIntrinsicParametersException if pinhole camera intrinsic parameters
145 * cannot be obtained from this conic instance.
146 */
147 public PinholeCameraIntrinsicParameters getIntrinsicParameters()
148 throws InvalidPinholeCameraIntrinsicParametersException {
149 try {
150 normalize();
151
152 // A conic is defined as [A B D]
153 // [B C E]
154 // [D E F]
155 final var b11 = getA();
156 final var b12 = getB();
157 final var b22 = getC();
158 final var b13 = getD();
159 final var b23 = getE();
160 final var b33 = getF();
161
162 // alpha = horizontal focal distance
163 // beta = vertical focal distance
164 // gamma = skewness
165 // u0 = horizontal principal point
166 // v0 = vertical principal point
167 // lambda = scale
168 final var v0 = (b12 * b13 - b11 * b23) / (b11 * b22 - b12 * b12);
169 if (Double.isNaN(v0) || Double.isInfinite(v0)) {
170 throw new InvalidPinholeCameraIntrinsicParametersException();
171 }
172
173 // lambda is an arbitrary scale, because conics are defined up to
174 // scale, and we are obtaining intrinsic parameters from IAC (Image
175 // of Absolute Conic)
176 var lambda = b33 - (b13 * b13 + v0 * (b12 * b13 - b11 * b23)) / b11;
177 // fix sign of lambda so that squared roots are always positive
178 lambda *= Math.signum(lambda) * Math.signum(b11);
179
180 if (Double.isNaN(lambda) || Double.isInfinite(lambda)) {
181 throw new InvalidPinholeCameraIntrinsicParametersException();
182 }
183
184 final var alpha = Math.sqrt(lambda / b11);
185 if (Double.isNaN(alpha) || Double.isInfinite(alpha)) {
186 throw new InvalidPinholeCameraIntrinsicParametersException();
187 }
188
189 final var beta = Math.sqrt(lambda * b11 / (b11 * b22 - b12 * b12));
190 if (Double.isNaN(beta) || Double.isInfinite(beta)) {
191 throw new InvalidPinholeCameraIntrinsicParametersException();
192 }
193
194 final var gamma = -b12 * alpha * alpha * beta / lambda;
195 if (Double.isNaN(gamma) || Double.isInfinite(gamma)) {
196 throw new InvalidPinholeCameraIntrinsicParametersException();
197 }
198
199 final var u0 = gamma * v0 / beta - b13 * alpha * alpha / lambda;
200 if (Double.isNaN(u0) || Double.isInfinite(u0)) {
201 throw new InvalidPinholeCameraIntrinsicParametersException();
202 }
203
204 return new PinholeCameraIntrinsicParameters(alpha, beta, u0, v0, gamma);
205 } catch (final ArithmeticException e) {
206 throw new InvalidPinholeCameraIntrinsicParametersException(e);
207 }
208 }
209
210 /**
211 * Assuming that we are working in a metric stratum this method obtains the
212 * internal parameters of a pinhole camera by means of Cholesky
213 * decomposition.
214 *
215 * @return the internal parameters of a pinhole camera.
216 * @throws InvalidPinholeCameraIntrinsicParametersException if pinhole
217 * camera intrinsic parameters cannot be
218 * obtained from this conic instance.
219 */
220 public PinholeCameraIntrinsicParameters getIntrinsicParametersCholesky()
221 throws InvalidPinholeCameraIntrinsicParametersException {
222 try {
223 normalize();
224
225 final var m = asMatrix();
226 final var decomposer = new CholeskyDecomposer(m);
227 decomposer.decompose();
228 final var inverseInternalParamsMatrix = decomposer.getR();
229 final var internalParamsMatrix = com.irurueta.algebra.Utils.inverse(inverseInternalParamsMatrix);
230 return new PinholeCameraIntrinsicParameters(internalParamsMatrix);
231 } catch (final AlgebraException e) {
232 throw new InvalidPinholeCameraIntrinsicParametersException(e);
233 }
234 }
235 }