1 /*
2 * Copyright (C) 2012 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.geometry;
17
18 import com.irurueta.algebra.AlgebraException;
19 import com.irurueta.algebra.Matrix;
20 import com.irurueta.algebra.SingularValueDecomposer;
21 import com.irurueta.algebra.WrongSizeException;
22
23 import java.io.Serializable;
24
25 /**
26 * This class contains implementation of a dual conic.
27 */
28 @SuppressWarnings("DuplicatedCode")
29 public class DualConic extends BaseConic implements Serializable {
30
31 /**
32 * Constructor.
33 */
34 public DualConic() {
35 super();
36 }
37
38 /**
39 * Constructor of this class. This constructor accepts every parameter
40 * describing a dual conic (parameters a, b, c, d, e, f).
41 *
42 * @param a Parameter A of the conic.
43 * @param b Parameter B of the conic.
44 * @param c Parameter C of the conic.
45 * @param d Parameter D of the conic.
46 * @param e Parameter E of the conic.
47 * @param f Parameter F of the conic.
48 */
49 public DualConic(final double a, final double b, final double c, final double d, final double e, final double f) {
50 super(a, b, c, d, e, f);
51 }
52
53 /**
54 * This method sets the matrix used to describe a dual conic.
55 * This matrix must be 3x3 and symmetric.
56 *
57 * @param m 3x3 Matrix describing the conic.
58 * @throws IllegalArgumentException Raised when the size of the matrix is
59 * not 3x3.
60 * @throws NonSymmetricMatrixException Raised when the conic matrix is not
61 * symmetric.
62 */
63 public DualConic(final Matrix m) throws NonSymmetricMatrixException {
64 super(m);
65 }
66
67 /**
68 * Instantiates a dual conic where provided lines belong to its locus.
69 *
70 * @param line1 1st line.
71 * @param line2 2nd line.
72 * @param line3 3rd line.
73 * @param line4 4th line.
74 * @param line5 5th line.
75 * @throws CoincidentLinesException Raised if provided lines are coincident
76 * (more than one line is equal) or produce a degenerate configuration.
77 */
78 public DualConic(final Line2D line1, final Line2D line2, final Line2D line3, final Line2D line4, final Line2D line5)
79 throws CoincidentLinesException {
80 setParametersFromLines(line1, line2, line3, line4, line5);
81 }
82
83 /**
84 * Checks if provided line is locus of this dual conic, or in other words,
85 * checks whether provided line lies within this conic, or whether provided
86 * line is tangent to the conic corresponding to this dual conic.
87 *
88 * @param line Line2D to be tested.
89 * @param threshold Threshold of tolerance to determine whether the line is
90 * locus or not. This is needed because of limited machine precision. If
91 * threshold is not provided, then DEFAULT_LOCUS_THRESHOLD is used instead.
92 * @return True if provided line is locus of this dual conic, false
93 * otherwise.
94 * @throws IllegalArgumentException Raised if provided threshold is negative.
95 */
96 public boolean isLocus(final Line2D line, final double threshold) {
97 if (threshold < MIN_THRESHOLD) {
98 throw new IllegalArgumentException();
99 }
100
101 try {
102 normalize();
103 final var dualC = asMatrix();
104 final var homLine = new Matrix(Line2D.LINE_NUMBER_PARAMS, 1);
105 line.normalize();
106 homLine.setElementAt(0, 0, line.getA());
107 homLine.setElementAt(1, 0, line.getB());
108 homLine.setElementAt(2, 0, line.getC());
109 final var locusMatrix = homLine.transposeAndReturnNew();
110 locusMatrix.multiply(dualC);
111 locusMatrix.multiply(homLine);
112
113 return Math.abs(locusMatrix.getElementAt(0, 0)) < threshold;
114 } catch (final WrongSizeException ignore) {
115 return false;
116 }
117 }
118
119 /**
120 * Checks if provided line is locus of this dual conic, or in other words,
121 * checks whether provided line lies within this conic, or whether provided
122 * line is tangent to the conic corresponding to this dual conic.
123 *
124 * @param line Line2D to be tested.
125 * @return True if provided line is locus of this dual conic, false
126 * otherwise.
127 * @see #isLocus(Line2D, double)
128 */
129 public boolean isLocus(final Line2D line) {
130 return isLocus(line, DEFAULT_LOCUS_THRESHOLD);
131 }
132
133 /**
134 * Computes the angle between two lines in radians.
135 *
136 * @param lineA First line to be tested.
137 * @param lineB Second line to be tested.
138 * @return Angle between the two provided lines in radians.
139 */
140 public double angleBetweenLines(final Line2D lineA, final Line2D lineB) {
141 try {
142 // retrieve conic as matrix
143 normalize();
144 final var dualC = asMatrix();
145 final var transHomLineA = new Matrix(1, Line2D.LINE_NUMBER_PARAMS);
146 lineA.normalize();
147 transHomLineA.setElementAt(0, 0, lineA.getA());
148 transHomLineA.setElementAt(0, 1, lineA.getB());
149 transHomLineA.setElementAt(0, 2, lineA.getC());
150
151
152 final var tmp = transHomLineA.multiplyAndReturnNew(dualC);
153 tmp.multiply(transHomLineA.transposeAndReturnNew()); //This is
154 // homLineA' * dualC * homLineA
155
156 final var normA = tmp.getElementAt(0, 0);
157
158 final var homLineB = new Matrix(Line2D.LINE_NUMBER_PARAMS, 1);
159 lineB.normalize();
160 homLineB.setElementAt(0, 0, lineB.getA());
161 homLineB.setElementAt(1, 0, lineB.getB());
162 homLineB.setElementAt(2, 0, lineB.getC());
163
164 homLineB.transpose(tmp);
165 tmp.multiply(dualC);
166 tmp.multiply(homLineB);
167
168 final var normB = tmp.getElementAt(0, 0);
169
170 transHomLineA.multiply(dualC);
171 transHomLineA.multiply(homLineB);
172 // This is homLineA' * dualC * homLineB
173
174 final var angleNumerator = transHomLineA.getElementAt(0, 0);
175
176 final var cosTheta = angleNumerator / Math.sqrt(normA * normB);
177 return Math.acos(cosTheta);
178 } catch (final WrongSizeException ignore) {
179 // This will never happen
180 return 0.0;
181 }
182 }
183
184 /**
185 * Checks if two lines are perpendicular attending to the geometry defined
186 * by this dual conic, or in other words, if lA' * dualC* * lB is zero.
187 *
188 * @param lineA First line to be checked.
189 * @param lineB Second line to be checked.
190 * @param threshold Threshold of tolerance to determine whether the lines
191 * are perpendicular or not. This is needed because of limited machine
192 * precision. If threshold is not provided, then
193 * DEFAULT_PERPENDICULAR_THRESHOLD is used instead.
194 * @return True if provided lines are perpendicular, false otherwise.
195 * @throws IllegalArgumentException Raised if provided threshold is negative.
196 */
197 public boolean arePerpendicularLines(final Line2D lineA, final Line2D lineB, final double threshold) {
198 try {
199 // retrieve conic as matrix
200 final var transHomLineA = new Matrix(1, Line2D.LINE_NUMBER_PARAMS);
201 lineA.normalize();
202 transHomLineA.setElementAt(0, 0, lineA.getA());
203 transHomLineA.setElementAt(0, 1, lineA.getB());
204 transHomLineA.setElementAt(0, 2, lineA.getC());
205
206 final var homLineB = new Matrix(Point2D.POINT2D_HOMOGENEOUS_COORDINATES_LENGTH, 1);
207 lineB.normalize();
208 homLineB.setElementAt(0, 0, lineB.getA());
209 homLineB.setElementAt(1, 0, lineB.getB());
210 homLineB.setElementAt(2, 0, lineB.getC());
211
212 normalize();
213 final var dualC = asMatrix();
214 transHomLineA.multiply(dualC);
215 transHomLineA.multiply(homLineB);
216 // This is homLineA' * dualC * homLineB
217
218 final var perpend = transHomLineA.getElementAt(0, 0);
219
220 return Math.abs(perpend) < threshold;
221 } catch (final WrongSizeException ignore) {
222 // This will never happen
223 return false;
224 }
225 }
226
227 /**
228 * Checks if two lines are perpendicular attending to the geometry defined
229 * by this dual conic, or in other words, if lA' * dualC* * lB is zero.
230 *
231 * @param lineA First line to be checked.
232 * @param lineB Second line to be checked.
233 * @return True if provided lines are perpendicular, false otherwise.
234 */
235 public boolean arePerpendicularLines(final Line2D lineA, final Line2D lineB) {
236 return arePerpendicularLines(lineA, lineB, DEFAULT_PERPENDICULAR_THRESHOLD);
237 }
238
239 /**
240 * Computes the conic corresponding to this dual conic.
241 *
242 * @return A new conic instance of this dual conic.
243 * @throws ConicNotAvailableException Raised if the rank of the dual conic
244 * matrix is not complete due to wrong parameters or numerical instability.
245 */
246 public Conic getConic() throws ConicNotAvailableException {
247 final var c = new Conic();
248 conic(c);
249 return c;
250 }
251
252 /**
253 * Computes the conic corresponding to this dual conic and stores the result
254 * in provided instance.
255 *
256 * @param conic Conic where result is stored.
257 * @throws ConicNotAvailableException Raised if the rank of the dual conic
258 * matrix is not complete due to wrong parameters or numerical instability.
259 */
260 public void conic(final Conic conic) throws ConicNotAvailableException {
261 final var dualConicMatrix = asMatrix();
262 try {
263 final var invMatrix = com.irurueta.algebra.Utils.inverse(dualConicMatrix);
264
265 // ensure that resulting matrix after inversion is symmetric
266 // by computing the mean of off-diagonal elements
267 final var a = invMatrix.getElementAt(0, 0);
268 final var b = 0.5 * (invMatrix.getElementAt(0, 1) + invMatrix.getElementAt(1, 0));
269 final var c = invMatrix.getElementAt(1, 1);
270 final var d = 0.5 * (invMatrix.getElementAt(0, 2) + invMatrix.getElementAt(2, 0));
271 final var e = 0.5 * (invMatrix.getElementAt(1, 2) + invMatrix.getElementAt(2, 1));
272 final var f = invMatrix.getElementAt(2, 2);
273 conic.setParameters(a, b, c, d, e, f);
274 } catch (final AlgebraException e) {
275 throw new ConicNotAvailableException(e);
276 }
277 }
278
279 /**
280 * Sets parameters of this dual conic so that provided lines lie within it
281 * (are locus).
282 *
283 * @param line1 1st line.
284 * @param line2 2nd line.
285 * @param line3 3rd line.
286 * @param line4 4th line.
287 * @param line5 5th line.
288 * @throws CoincidentLinesException Raised if lines are coincident or
289 * produce a degenerated configuration.
290 */
291 public final void setParametersFromLines(
292 final Line2D line1, final Line2D line2, final Line2D line3, final Line2D line4, final Line2D line5)
293 throws CoincidentLinesException {
294
295 try {
296 line1.normalize();
297 line2.normalize();
298 line3.normalize();
299 line4.normalize();
300 line5.normalize();
301
302 // estimate dual conic that lines inside provided 5 lines
303 final var m = new Matrix(5, 6);
304
305 var l1 = line1.getA();
306 var l2 = line1.getB();
307 var l3 = line1.getC();
308 m.setElementAt(0, 0, l1 * l1);
309 m.setElementAt(0, 1, 2.0 * l1 * l2);
310 m.setElementAt(0, 2, l2 * l2);
311 m.setElementAt(0, 3, 2.0 * l1 * l3);
312 m.setElementAt(0, 4, 2.0 * l2 * l3);
313 m.setElementAt(0, 5, l3 * l3);
314
315 l1 = line2.getA();
316 l2 = line2.getB();
317 l3 = line2.getC();
318 m.setElementAt(1, 0, l1 * l1);
319 m.setElementAt(1, 1, 2.0 * l1 * l2);
320 m.setElementAt(1, 2, l2 * l2);
321 m.setElementAt(1, 3, 2.0 * l1 * l3);
322 m.setElementAt(1, 4, 2.0 * l2 * l3);
323 m.setElementAt(1, 5, l3 * l3);
324
325 l1 = line3.getA();
326 l2 = line3.getB();
327 l3 = line3.getC();
328 m.setElementAt(2, 0, l1 * l1);
329 m.setElementAt(2, 1, 2.0 * l1 * l2);
330 m.setElementAt(2, 2, l2 * l2);
331 m.setElementAt(2, 3, 2.0 * l1 * l3);
332 m.setElementAt(2, 4, 2.0 * l2 * l3);
333 m.setElementAt(2, 5, l3 * l3);
334
335 l1 = line4.getA();
336 l2 = line4.getB();
337 l3 = line4.getC();
338 m.setElementAt(3, 0, l1 * l1);
339 m.setElementAt(3, 1, 2.0 * l1 * l2);
340 m.setElementAt(3, 2, l2 * l2);
341 m.setElementAt(3, 3, 2.0 * l1 * l3);
342 m.setElementAt(3, 4, 2.0 * l2 * l3);
343 m.setElementAt(3, 5, l3 * l3);
344
345 l1 = line5.getA();
346 l2 = line5.getB();
347 l3 = line5.getC();
348 m.setElementAt(4, 0, l1 * l1);
349 m.setElementAt(4, 1, 2.0 * l1 * l2);
350 m.setElementAt(4, 2, l2 * l2);
351 m.setElementAt(4, 3, 2.0 * l1 * l3);
352 m.setElementAt(4, 4, 2.0 * l2 * l3);
353 m.setElementAt(4, 5, l3 * l3);
354
355 // normalize each row to increase accuracy
356 final var row = new double[6];
357 double rowNorm;
358
359 for (var j = 0; j < 5; j++) {
360 m.getSubmatrixAsArray(j, 0, j, 5, row);
361 rowNorm = com.irurueta.algebra.Utils.normF(row);
362 for (var i = 0; i < 6; i++) {
363 m.setElementAt(j, i, m.getElementAt(j, i) / rowNorm);
364 }
365 }
366
367 final var decomposer = new SingularValueDecomposer(m);
368 decomposer.decompose();
369
370 if (decomposer.getRank() < 5) {
371 throw new CoincidentLinesException();
372 }
373
374 // the right null-space of m contains the parameters a, b, c, d, e ,f
375 // of the conic
376 final var v = decomposer.getV();
377
378 // l1^ + 2*l1*l2 + l2^2 + 2*l1*l3 + 2*l2*l3 + l3^2 = 0
379 final var a = v.getElementAt(0, 5);
380 final var b = v.getElementAt(1, 5);
381 final var c = v.getElementAt(2, 5);
382 final var d = v.getElementAt(3, 5);
383 final var e = v.getElementAt(4, 5);
384 final var f = v.getElementAt(5, 5);
385
386 setParameters(a, b, c, d, e, f);
387 } catch (final AlgebraException ex) {
388 throw new CoincidentLinesException(ex);
389 }
390 }
391
392 /**
393 * Creates a canonical instance of the dual absolute conic in the metric
394 * stratum.
395 * The intersection of the plane at infinity with the set of planes tangent
396 * to the dual absolute quadric produce the dual absolute conic.
397 * In other words, The dual absolute conic in the metric stratum is the set
398 * of lines tangent to the absolute conic that also lie in the plane at
399 * infinity.
400 * Both the absolute conic and the dual absolute conic define orthogonality
401 * in the metric stratum, and in a purely metric stratum (i.e. when camera
402 * is correctly calibrated), their canonical value is equal to the identity.
403 *
404 * @return a canonical instance of the dual absolute conic.
405 */
406 public static DualConic createCanonicalDualAbsoluteConic() {
407 return new DualConic(1.0, 0.0, 1.0, 0.0, 0.0, 1.0);
408 }
409 }