1 /*
2 * Copyright (C) 2016 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.SingularValueDecomposer;
21 import com.irurueta.algebra.Utils;
22 import com.irurueta.algebra.WrongSizeException;
23 import com.irurueta.geometry.DualQuadric;
24 import com.irurueta.geometry.NonSymmetricMatrixException;
25 import com.irurueta.geometry.Plane;
26 import com.irurueta.geometry.ProjectiveTransformation3D;
27 import com.irurueta.numerical.NumericalException;
28 import com.irurueta.numerical.roots.FirstDegreePolynomialRootsEstimator;
29 import com.irurueta.numerical.roots.SecondDegreePolynomialRootsEstimator;
30
31 import java.io.Serializable;
32
33 /**
34 * The dual absolute quadric is the dual quadric tangent to the plane at
35 * infinity.
36 * The absolute quadric (which is its inverse) contains all planes located
37 * at infinity (x,y,z,0), hence the absolute quadric fulfills
38 * (x,y,z,0)'*Q*(x,y,z,0).
39 * Consequently, the dual absolute quadric fulfills P*Q^-1*P, where P is the
40 * plane at infinity (0,0,0,1) in the metric stratum.
41 * This means that in the metric stratus the dual absolute quadric is equal
42 * (up to scale) to:
43 * [1 0 0 0]
44 * Q = [0 1 0 0]
45 * [0 0 1 0]
46 * [0 0 0 0]
47 */
48 public class DualAbsoluteQuadric extends DualQuadric implements Serializable {
49
50 /**
51 * Constructor.
52 * Initializes the Dual Absolute Quadric assuming metric stratum, where it
53 * is equal to the identity except for the last element which is zero.
54 */
55 public DualAbsoluteQuadric() {
56 super(1.0, 1.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0);
57 }
58
59 /**
60 * Constructor of this class. This constructor accepts every parameter
61 * describing a dual quadric (parameters a, b, c, d, e, f, g, h, i, j).
62 *
63 * @param a Parameter A of the quadric.
64 * @param b Parameter B of the quadric.
65 * @param c Parameter C of the quadric.
66 * @param d Parameter D of the quadric.
67 * @param e Parameter E of the quadric.
68 * @param f Parameter F of the quadric.
69 * @param g Parameter G of the quadric.
70 * @param h Parameter H of the quadric.
71 * @param i Parameter I of the quadric.
72 * @param j Parameter J of the quadric.
73 */
74 public DualAbsoluteQuadric(
75 final double a, final double b, final double c, final double d, final double e,
76 final double f, final double g, final double h, final double i, final double j) {
77 super(a, b, c, d, e, f, g, h, i, j);
78 }
79
80 /**
81 * Constructor from provided dual image of absolute conic and plane at
82 * infinity on an arbitrary projective stratum.
83 *
84 * @param diac dual image of absolute conic in an arbitrary projective
85 * stratum.
86 * @param planeAtInfinity plane at infinity in an arbitrary projective
87 * stratum.
88 */
89 public DualAbsoluteQuadric(final DualImageOfAbsoluteConic diac, final Plane planeAtInfinity) {
90 super();
91 setDualImageOfAbsoluteConicAndPlaneAtInfinity(diac, planeAtInfinity);
92 }
93
94 /**
95 * Constructor using provided dual image of absolute conic on an arbitrary
96 * affine stratum while keeping the plane at infinity typically used in
97 * a metric stratum (0,0,0,1).
98 *
99 * @param diac dual image of absolute conic.
100 */
101 public DualAbsoluteQuadric(final DualImageOfAbsoluteConic diac) {
102 super();
103
104 diac.normalize();
105
106 final var a = diac.getA();
107 final var b = diac.getC();
108 final var c = diac.getF();
109 final var d = diac.getB();
110 final var e = diac.getE();
111 final var f = diac.getD();
112 setParameters(a, b, c, d, e, f, 0.0, 0.0, 0.0, 0.0);
113 }
114
115 /**
116 * Constructor using provided plane at infinity while using the unitary
117 * dual image of absolute conic (the identity).
118 *
119 * @param planeAtInfinity plane at infinity.
120 */
121 public DualAbsoluteQuadric(final Plane planeAtInfinity) {
122 super();
123
124 var planeA = planeAtInfinity.getA();
125 var planeB = planeAtInfinity.getB();
126 var planeC = planeAtInfinity.getC();
127 final var planeD = planeAtInfinity.getD();
128
129 // normalize plane components so that last one is the unit
130 planeA /= planeD;
131 planeB /= planeD;
132 planeC /= planeD;
133
134 final var g = -planeA;
135 final var h = -planeB;
136 final var i = -planeC;
137 final var j = planeA * planeA + planeB * planeB + planeC * planeC;
138
139 setParameters(1.0, 1.0, 1.0, 0.0, 0.0, 0.0, g, h, i, j);
140 }
141
142 /**
143 * Constructor of the Dual Absolute Quadric in an arbitrary projective
144 * stratum using a transformation from metric stratum to such projective
145 * space.
146 *
147 * @param metricToProjectiveTransformation transformation from metric to
148 * a projective space.
149 * @throws InvalidTransformationException if provided transformation is
150 * numerically unstable.
151 */
152 public DualAbsoluteQuadric(final ProjectiveTransformation3D metricToProjectiveTransformation)
153 throws InvalidTransformationException {
154 setMetricToProjectiveTransformation(metricToProjectiveTransformation);
155 }
156
157 /**
158 * Sets this dual absolute quadric from provided metric to projective space
159 * transformation.
160 *
161 * @param metricToProjectiveTransformation transformation from metric to a
162 * projective space.
163 * @throws InvalidTransformationException if provided transformation is
164 * numerically unstable.
165 */
166 public final void setMetricToProjectiveTransformation(
167 final ProjectiveTransformation3D metricToProjectiveTransformation) throws InvalidTransformationException {
168 metricToProjectiveTransformation.normalize();
169
170 try {
171 // transformation
172 final var t = metricToProjectiveTransformation.asMatrix();
173
174 // identity except for the last element. This is the DAQ in metric
175 // stratum
176 final var i = Matrix.identity(BASEQUADRIC_MATRIX_ROW_SIZE, BASEQUADRIC_MATRIX_COLUMN_SIZE);
177 i.setElementAt(3, 3, 0.0);
178
179 // transformation transposed
180 final var tt = t.transposeAndReturnNew();
181
182 // make product t * i * tt
183 t.multiply(i);
184 t.multiply(tt);
185
186 setParameters(t);
187 } catch (final NonSymmetricMatrixException e) {
188 throw new InvalidTransformationException(e);
189 } catch (final WrongSizeException ignore) {
190 // never thrown
191 }
192 }
193
194 /**
195 * Obtains the metric to projective stratum transformation defining this
196 * Dual Absolute Quadric.
197 *
198 * @return a new metric to projective space transformation.
199 * @throws InvalidTransformationException if transformation cannot be
200 * determined because dual absolute quadric is numerically
201 * unstable.
202 */
203 public ProjectiveTransformation3D getMetricToProjectiveTransformation() throws InvalidTransformationException {
204 final var result = new ProjectiveTransformation3D();
205 getMetricToProjectiveTransformation(result);
206 return result;
207 }
208
209 /**
210 * Obtains the metric to projective stratum transformation defining this
211 * Dual Absolute Quadric.
212 *
213 * @param result instance where metric to projective space transformation
214 * will be stored.
215 * @throws InvalidTransformationException if transformation cannot be
216 * determined because dual absolute quadric is numerically
217 * unstable.
218 */
219 public void getMetricToProjectiveTransformation(final ProjectiveTransformation3D result)
220 throws InvalidTransformationException {
221 // DAQ can be decomposed as DAQ = H * I * H^t, where
222 // I = [1 0 0 0]
223 // [0 1 0 0]
224 // [0 0 1 0]
225 // [0 0 0 0]
226 // Hence I can be seen as the eigen values of DAQ's eigen decomposition
227 // and H are the eigenvectors.
228 // From this decomposition, it can be seen that:
229 // - DAQ is singular (has rank 3)
230 // - DAQ is symmetric positive definite (or negative depending on scale
231 // sign, but all eigen values have the same sign).
232 // We know that symmetric positive definite matrices have square roots
233 // such as:
234 // DAQ = M*M^t
235 // Hence the SVD of the square root is equal to:
236 // M = U*S*V^t
237 // And consequently DAQ is:
238 // DAQ = (U*S*V^t)*(U*S*V^t)^t = U*S*V^t*V*S*U^t = U*S*S*U^t
239 // where S is diagonal and contains the singular values of M
240 // and U are the singular vectors of M
241 // Since S is diagonal, then S*S contains the squared singular values on
242 // its diagonal and matrix S*S can be seen as the eigen values of DAQ,
243 // while "U" are the eigen vectors of DAQ
244 // Since we don't care about scale, we can normalize the eigen values of
245 // DAQ, and transformation H is equal to matrix U (up to scale)
246
247 try {
248 final var daqMatrix = asMatrix();
249 final var decomposer = new SingularValueDecomposer(daqMatrix);
250 decomposer.decompose();
251 // since daq matrix will always be symmetric U = V
252 final var u = decomposer.getU();
253 // we need to undo the effect of possible different singular values
254 // because we want H * I * H^t, so that middle matrix is the identity
255 // except for the last element which is zero.
256 // For that reason we multiply each column of U by the square root
257 // of each singular value
258 final var w = decomposer.getSingularValues();
259 for (var i = 0; i < BASEQUADRIC_MATRIX_COLUMN_SIZE - 1; i++) {
260 final var scalar = Math.sqrt(w[i]);
261 for (var j = 0; j < BASEQUADRIC_MATRIX_ROW_SIZE; j++) {
262 u.setElementAt(j, i, scalar * u.getElementAt(j, i));
263 }
264 }
265 result.setT(u);
266 } catch (final AlgebraException e) {
267 throw new InvalidTransformationException(e);
268 }
269 }
270
271 /**
272 * Sets this dual absolute quadric from provided dual image of absolute
273 * conic and plane at infinity on an arbitrary projective stratum.
274 *
275 * @param diac dual image of absolute conic to be set.
276 * @param planeAtInfinity plane at infinity to be set.
277 */
278 public final void setDualImageOfAbsoluteConicAndPlaneAtInfinity(
279 final DualImageOfAbsoluteConic diac, final Plane planeAtInfinity) {
280
281 var planeA = planeAtInfinity.getA();
282 var planeB = planeAtInfinity.getB();
283 var planeC = planeAtInfinity.getC();
284 final var planeD = planeAtInfinity.getD();
285
286 // normalize plane components so that last one is the unit
287 planeA /= planeD;
288 planeB /= planeD;
289 planeC /= planeD;
290
291 final var a = diac.getA();
292 final var b = diac.getC();
293 final var c = diac.getF();
294 final var d = diac.getB();
295 final var e = diac.getE();
296 final var f = diac.getD();
297
298 final var g = -(diac.getA() * planeA + diac.getB() * planeB + diac.getD() * planeC);
299 final var h = -(diac.getB() * planeA + diac.getC() * planeB + diac.getE() * planeC);
300 final var i = -(diac.getD() * planeA + diac.getE() * planeB + diac.getF() * planeC);
301 final var j = -(planeA * g + planeB * h + planeC * i);
302
303 setParameters(a, b, c, d, e, f, g, h, i, j);
304 }
305
306 /**
307 * Gets dual image of absolute conic associated to this dual absolute
308 * quadric in an arbitrary projective stratum.
309 *
310 * @return dual image of absolute conic associated to this dual absolute
311 * quadric.
312 */
313 public DualImageOfAbsoluteConic getDualImageOfAbsoluteConic() {
314 final var result = new DualImageOfAbsoluteConic();
315 getDualImageOfAbsoluteConic(result);
316 return result;
317 }
318
319 /**
320 * Gets dual image of absolute conic associated to this dual absolute
321 * quadric in an arbitrary projective stratum and stores the result into
322 * provided instance.
323 *
324 * @param result instance where dual image of absolute conic will be stored.
325 */
326 public void getDualImageOfAbsoluteConic(final DualImageOfAbsoluteConic result) {
327 final var a = getA();
328 final var b = getD();
329 final var c = getB();
330 final var d = getF();
331 final var e = getE();
332 final var f = getC();
333 result.setParameters(a, b, c, d, e, f);
334 }
335
336 /**
337 * Sets dual image of absolute conic while keeping current plane at infinity
338 * in an arbitrary projective stratum.
339 *
340 * @param diac dual image of absolute conic to be set.
341 * @throws InvalidPlaneAtInfinityException if plane at infinity to be
342 * preserved when setting DIAC cannot be determined.
343 */
344 public final void setDualImageOfAbsoluteConic(final DualImageOfAbsoluteConic diac)
345 throws InvalidPlaneAtInfinityException {
346 final var planeAtInfinity = getPlaneAtInfinity();
347 setDualImageOfAbsoluteConicAndPlaneAtInfinity(diac, planeAtInfinity);
348 }
349
350 /**
351 * Gets plane at infinity associated to this dual absolute quadric in an
352 * arbitrary projective stratum.
353 *
354 * @return plane at infinity associated to this dual absolute quadric.
355 * @throws InvalidPlaneAtInfinityException if plane at infinity cannot be
356 * determined.
357 */
358 public Plane getPlaneAtInfinity() throws InvalidPlaneAtInfinityException {
359 final var result = new Plane();
360 getPlaneAtInfinity(result);
361 return result;
362 }
363
364 /**
365 * Gets plane at infinity associated to this dual absolute quadric in an
366 * arbitrary projective stratum.
367 *
368 * @param result instance where plane at infinity will be stored.
369 * @throws InvalidPlaneAtInfinityException if plane at infinity cannot be
370 * determined.
371 */
372 public void getPlaneAtInfinity(final Plane result) throws InvalidPlaneAtInfinityException {
373 try {
374 final var diac = getDualImageOfAbsoluteConic();
375 final var m = diac.asMatrix();
376 Utils.inverse(m, m);
377
378 final var g = getG();
379 final var h = getH();
380 final var i = getI();
381
382 final var planeA = -m.getElementAt(0, 0) * g
383 - m.getElementAt(0, 1) * h
384 - m.getElementAt(0, 2) * i;
385
386 final var planeB = -m.getElementAt(1, 0) * g
387 - m.getElementAt(1, 1) * h
388 - m.getElementAt(1, 2) * i;
389
390 final var planeC = -m.getElementAt(2, 0) * g
391 - m.getElementAt(2, 1) * h
392 - m.getElementAt(2, 2) * i;
393
394 // plane at infinity must be tangent to dual absolute quadric, hence
395 // P^T*Q^-1*P = 0
396 // [planeA planeB planeC planeD]*[a d f g][planeA]
397 // [d b e h][planeB] = 0
398 // [f e c i][planeC]
399 // [g h i j][planeD]
400
401 // [planeA planeB planeC planeD]*[a*planeA + d*planeB + f*planeC + g*planeD]
402 // [d*planeA + b*planeB + e*planeC + h*planeD] = 0
403 // [f*planeA + e*planeB + c*planeC + i*planeD]
404 // [g*planeA + h*planeB + i*planeC + j*planeD]
405
406 // planeA*(a*planeA + d*planeB + f*planeC) +
407 // planeB*(d*planeA + b*planeB + e*planeC) +
408 // planeC*(f*planeA + e*planeB + c*planeC) +
409 // 2*planeD*(g*planeA + h*planeB + i*planeC) + j*planeD*planeD = 0
410
411 // hence we create the 2nd degree polynomial shown above and solve
412 // planeD value
413 final var a = getA();
414 final var b = getB();
415 final var c = getC();
416 final var d = getD();
417 final var e = getE();
418 final var f = getF();
419 final var j = getJ();
420 final var polyParams = new double[]{
421 planeA * (a * planeA + d * planeB + f * planeC)
422 + planeB * (d * planeA + b * planeB + e * planeC)
423 + planeC * (f * planeA + e * planeB + c * planeC),
424 2.0 * (g * planeA + h * planeB + i * planeC),
425 j
426 };
427
428 double planeD;
429 if (SecondDegreePolynomialRootsEstimator.isSecondDegree(polyParams)) {
430 // second degree
431 final var estimator = new SecondDegreePolynomialRootsEstimator(polyParams);
432
433
434 final var hasDoubleRoot = estimator.hasDoubleRoot();
435
436 // a double REAL root (same root happening twice) must be present
437 // because only one plane at infinity should be defined
438 if (!hasDoubleRoot) {
439 throw new InvalidPlaneAtInfinityException("more than one possible solution");
440 }
441
442 estimator.estimate();
443
444 final var roots = estimator.getRoots();
445 planeD = roots[0].getReal();
446 } else {
447 // polynomial is not second degree, attempt to solve as 1 degree
448 if (FirstDegreePolynomialRootsEstimator.isFirstDegree(polyParams)) {
449 // first degree
450 final var estimatorFirst = new FirstDegreePolynomialRootsEstimator(
451 new double[]{polyParams[0], polyParams[1]});
452
453 estimatorFirst.estimate();
454
455 final var roots = estimatorFirst.getRoots();
456 planeD = roots[0].getReal();
457 } else {
458 // degenerate polynomial (i.e. metric stratum)
459 planeD = 1.0;
460 }
461 }
462 result.setParameters(planeA, planeB, planeC, planeD);
463
464 } catch (final AlgebraException | NumericalException e) {
465 throw new InvalidPlaneAtInfinityException(e);
466 }
467 }
468
469 /**
470 * Sets provided plane at infinity at an arbitrary projective stratum while
471 * keeping current dual image of absolute conic.
472 *
473 * @param planeAtInfinity plane at infinity to be set.
474 */
475 public final void setPlaneAtInfinity(final Plane planeAtInfinity) {
476 final var diac = getDualImageOfAbsoluteConic();
477 setDualImageOfAbsoluteConicAndPlaneAtInfinity(diac, planeAtInfinity);
478 }
479 }