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.geometry.estimators;
17
18 import com.irurueta.algebra.Matrix;
19 import com.irurueta.algebra.SingularValueDecomposer;
20 import com.irurueta.algebra.Utils;
21 import com.irurueta.geometry.Line2D;
22 import com.irurueta.geometry.NotAvailableException;
23 import com.irurueta.geometry.PinholeCamera;
24 import com.irurueta.geometry.Plane;
25 import com.irurueta.numerical.robust.WeightSelection;
26
27 import java.util.List;
28
29 /**
30 * This class implements pinhole camera estimator using a weighted algorithm and
31 * point correspondences.
32 */
33 @SuppressWarnings("DuplicatedCode")
34 public class WeightedLinePlaneCorrespondencePinholeCameraEstimator extends
35 LinePlaneCorrespondencePinholeCameraEstimator {
36
37 /**
38 * Default number of correspondences to be weighted and taken into account.
39 */
40 public static final int DEFAULT_MAX_CORRESPONDENCES = 50;
41
42 /**
43 * Indicates if weights are sorted by default so that largest weighted
44 * correspondences are used first.
45 */
46 public static final boolean DEFAULT_SORT_WEIGHTS = true;
47
48 /**
49 * Defines tiny value considered as machine precision.
50 */
51 public static final double EPS = 1e-8;
52
53 /**
54 * Maximum number of correspondences to be weighted and taken into account.
55 */
56 private int maxCorrespondences;
57
58 /**
59 * Indicates if weights are sorted by default so that largest weighted
60 * correspondences are used first.
61 */
62 private boolean sortWeights;
63
64 /**
65 * Array containing weights for all point correspondences.
66 */
67 private double[] weights;
68
69 /**
70 * Constructor.
71 */
72 public WeightedLinePlaneCorrespondencePinholeCameraEstimator() {
73 super();
74 maxCorrespondences = DEFAULT_MAX_CORRESPONDENCES;
75 sortWeights = DEFAULT_SORT_WEIGHTS;
76 weights = null;
77 }
78
79 /**
80 * Constructor with listener.
81 *
82 * @param listener listener to be notified of events such as when estimation
83 * starts, ends or estimation progress changes.
84 */
85 public WeightedLinePlaneCorrespondencePinholeCameraEstimator(final PinholeCameraEstimatorListener listener) {
86 super(listener);
87 maxCorrespondences = DEFAULT_MAX_CORRESPONDENCES;
88 sortWeights = DEFAULT_SORT_WEIGHTS;
89 weights = null;
90 }
91
92 /**
93 * Constructor.
94 *
95 * @param planes list of corresponding 3D planes.
96 * @param lines2D list of corresponding 2D lines.
97 * @throws IllegalArgumentException if any of the lists are null.
98 * @throws WrongListSizesException if provided lists of correspondences
99 * don't have the same size and enough correspondences.
100 */
101 public WeightedLinePlaneCorrespondencePinholeCameraEstimator(final List<Plane> planes, final List<Line2D> lines2D)
102 throws WrongListSizesException {
103 super(planes, lines2D);
104 maxCorrespondences = DEFAULT_MAX_CORRESPONDENCES;
105 sortWeights = DEFAULT_SORT_WEIGHTS;
106 weights = null;
107 }
108
109 /**
110 * Constructor.
111 *
112 * @param planes list of corresponding 3D planes.
113 * @param lines2D list of corresponding 2D lines.
114 * @param listener listener to be notified of events such as when estimation
115 * starts, ends or estimation progress changes.
116 * @throws IllegalArgumentException if any of the lists are null.
117 * @throws WrongListSizesException if provided lists of correspondences
118 * don't have the same size and enough correspondences.
119 */
120 public WeightedLinePlaneCorrespondencePinholeCameraEstimator(
121 final List<Plane> planes, final List<Line2D> lines2D, final PinholeCameraEstimatorListener listener)
122 throws WrongListSizesException {
123 super(planes, lines2D, listener);
124 maxCorrespondences = DEFAULT_MAX_CORRESPONDENCES;
125 sortWeights = DEFAULT_SORT_WEIGHTS;
126 weights = null;
127 }
128
129 /**
130 * Constructor.
131 *
132 * @param planes list of corresponding 3D planes.
133 * @param lines2D list of corresponding 2D lines.
134 * @param weights array containing a weight amount for each correspondence.
135 * The larger the value of a weight, the most significant the
136 * correspondence will be.
137 * @throws IllegalArgumentException if any of the lists are null.
138 * @throws WrongListSizesException if provided lists of correspondences
139 * don't have the same size and enough correspondences.
140 */
141 public WeightedLinePlaneCorrespondencePinholeCameraEstimator(
142 final List<Plane> planes, final List<Line2D> lines2D, final double[] weights)
143 throws WrongListSizesException {
144 super();
145 maxCorrespondences = DEFAULT_MAX_CORRESPONDENCES;
146 sortWeights = DEFAULT_SORT_WEIGHTS;
147 this.weights = null;
148 internalSetListsAndWeights(planes, lines2D, weights);
149 }
150
151 /**
152 * Constructor.
153 *
154 * @param planes list of corresponding 3D planes.
155 * @param lines2D list of corresponding 2D lines.
156 * @param weights array containing a weight amount for each correspondence.
157 * The larger the value of a weight, the most significant the
158 * correspondence will be.
159 * @param listener listener to be notified of events such as when estimation
160 * starts, ends or estimation progress changes.
161 * @throws IllegalArgumentException if any of the lists are null.
162 * @throws WrongListSizesException if provided lists of correspondences
163 * don't have the same size and enough correspondences.
164 */
165 public WeightedLinePlaneCorrespondencePinholeCameraEstimator(
166 final List<Plane> planes, final List<Line2D> lines2D, final double[] weights,
167 final PinholeCameraEstimatorListener listener) throws WrongListSizesException {
168 super(listener);
169 maxCorrespondences = DEFAULT_MAX_CORRESPONDENCES;
170 sortWeights = DEFAULT_SORT_WEIGHTS;
171 this.weights = null;
172 internalSetListsAndWeights(planes, lines2D, weights);
173 }
174
175 /**
176 * Internal method to set list of corresponding points (it does not check
177 * if estimator is locked).
178 *
179 * @param planes list of corresponding 3D planes.
180 * @param lines2D list of corresponding 2D lines.
181 * @param weights array containing a weight amount for each correspondence.
182 * The larger the value of a weight, the most significant the
183 * correspondence will be.
184 * @throws IllegalArgumentException if any of the lists or arrays are null.
185 * @throws WrongListSizesException if provided lists of correspondences
186 * don't have the same size and enough correspondences.
187 */
188 private void internalSetListsAndWeights(
189 final List<Plane> planes, final List<Line2D> lines2D, final double[] weights)
190 throws WrongListSizesException {
191
192 if (planes == null || lines2D == null || weights == null) {
193 throw new IllegalArgumentException();
194 }
195
196 if (!areValidListsAndWeights(planes, lines2D, weights)) {
197 throw new WrongListSizesException();
198 }
199
200 this.planes = planes;
201 this.lines2D = lines2D;
202 this.weights = weights;
203 }
204
205 /**
206 * Sets list of corresponding planes and lines.
207 *
208 * @param planes list of corresponding 3D planes.
209 * @param lines2D list of corresponding 2D lines.
210 * @param weights array containing a weight amount for each correspondence.
211 * The larger the value of a weight, the most significant the
212 * correspondence will be.
213 * @throws LockedException if estimator is locked.
214 * @throws IllegalArgumentException if any of the lists are null.
215 * @throws WrongListSizesException if provided lists of correspondences
216 * don't have the same size and enough correspondences.
217 */
218 public void setListsAndWeights(
219 final List<Plane> planes, final List<Line2D> lines2D, final double[] weights) throws LockedException,
220 WrongListSizesException {
221 if (isLocked()) {
222 throw new LockedException();
223 }
224
225 internalSetListsAndWeights(planes, lines2D, weights);
226 }
227
228 /**
229 * Indicates if lists of corresponding planes and lines are valid.
230 * Lists are considered valid if they have the same number of
231 * correspondences and both have more than the required minimum of
232 * correspondences (which is 4).
233 *
234 * @param planes list of corresponding 3D planes.
235 * @param lines2D list of corresponding 2D lines.
236 * @param weights array containing a weight amount for each correspondence.
237 * The larger the value of a weight, the most significant the
238 * correspondence will be.
239 * @return true if corresponding planes and lines are valid, false otherwise.
240 */
241 public static boolean areValidListsAndWeights(
242 final List<Plane> planes, final List<Line2D> lines2D, final double[] weights) {
243 if (planes == null || lines2D == null || weights == null) {
244 return false;
245 }
246 return planes.size() == lines2D.size() && lines2D.size() == weights.length
247 && planes.size() >= MIN_NUMBER_OF_LINE_PLANE_CORRESPONDENCES;
248 }
249
250 /**
251 * Returns array containing a weight amount for each correspondence.
252 * The larger the value of a weight, the most significant the
253 * correspondence will be.
254 *
255 * @return array containing weights for each correspondence.
256 * @throws NotAvailableException if weights are not available.
257 */
258 public double[] getWeights() throws NotAvailableException {
259 if (!areWeightsAvailable()) {
260 throw new NotAvailableException();
261 }
262 return weights;
263 }
264
265 /**
266 * Returns boolean indicating whether weights have been provided and are
267 * available for retrieval.
268 *
269 * @return true if weights are available, false otherwise.
270 */
271 public boolean areWeightsAvailable() {
272 return weights != null;
273 }
274
275 /**
276 * Returns maximum number of correspondences to be weighted and taken into
277 * account.
278 *
279 * @return maximum number of points to be weighted.
280 */
281 public int getMaxCorrespondences() {
282 return maxCorrespondences;
283 }
284
285 /**
286 * Sets maximum number of correspondences to be weighted and taken into
287 * account.
288 *
289 * @param maxCorrespondences maximum number of correspondences to be
290 * weighted.
291 * @throws IllegalArgumentException if provided value is less than the
292 * minimum allowed number of point correspondences.
293 * @throws LockedException if this instance is locked.
294 */
295 public void setMaxCorrespondences(final int maxCorrespondences) throws LockedException {
296 if (isLocked()) {
297 throw new LockedException();
298 }
299 if (maxCorrespondences < MIN_NUMBER_OF_LINE_PLANE_CORRESPONDENCES) {
300 throw new IllegalArgumentException();
301 }
302
303 this.maxCorrespondences = maxCorrespondences;
304 }
305
306 /**
307 * Indicates if weights are sorted by so that largest weighted
308 * correspondences are used first.
309 *
310 * @return true if weights are sorted, false otherwise.
311 */
312 public boolean isSortWeightsEnabled() {
313 return sortWeights;
314 }
315
316 /**
317 * Specifies whether weights are sorted by so that largest weighted
318 * correspondences are used first.
319 *
320 * @param sortWeights true if weights are sorted, false otherwise.
321 * @throws LockedException if this instance is locked.
322 */
323 public void setSortWeightsEnabled(final boolean sortWeights) throws LockedException {
324 if (isLocked()) {
325 throw new LockedException();
326 }
327
328 this.sortWeights = sortWeights;
329 }
330
331 /**
332 * Indicates if this estimator is ready to start the estimation.
333 * Estimator will be ready once both lists and weights are available.
334 *
335 * @return true if estimator is ready, false otherwise.
336 */
337 @Override
338 public boolean isReady() {
339 return areListsAvailable() && areWeightsAvailable();
340 }
341
342 /**
343 * Estimates a pinhole camera.
344 *
345 * @return estimated pinhole camera.
346 * @throws LockedException if estimator is locked.
347 * @throws NotReadyException if input has not yet been provided.
348 * @throws PinholeCameraEstimatorException if an error occurs during
349 * estimation, usually because input data is not valid.
350 */
351 @Override
352 public PinholeCamera estimate() throws LockedException, NotReadyException, PinholeCameraEstimatorException {
353
354 if (isLocked()) {
355 throw new LockedException();
356 }
357 if (!isReady()) {
358 throw new NotReadyException();
359 }
360
361 try {
362 locked = true;
363 if (listener != null) {
364 listener.onEstimateStart(this);
365 }
366
367 final var selection = WeightSelection.selectWeights(weights, sortWeights, maxCorrespondences);
368 final var selected = selection.getSelected();
369
370 final var a = new Matrix(12, 12);
371 final var row = new Matrix(3, 12);
372 final var transRow = new Matrix(12, 3);
373 final var tmp = new Matrix(12, 12);
374
375 final var iterator2D = lines2D.iterator();
376 final var iterator3D = planes.iterator();
377
378 var index = 0;
379 var nMatches = 0;
380 var previousNorm = 1.0;
381 double rowNorm;
382 while (iterator2D.hasNext() && iterator3D.hasNext()) {
383 final var line2D = iterator2D.next();
384 final var plane = iterator3D.next();
385
386 if (selected[index]) {
387 final var weight = weights[index];
388
389 if (Math.abs(weight) < EPS) {
390 // skip, because weight is too small
391 index++;
392 continue;
393 }
394
395 // normalize lines and planes to increase accuracy
396 line2D.normalize();
397 plane.normalize();
398
399 final var la = line2D.getA();
400 final var lb = line2D.getB();
401 final var lc = line2D.getC();
402
403 final var pA = plane.getA();
404 final var pB = plane.getB();
405 final var pC = plane.getC();
406 final var pD = plane.getD();
407
408 // first row
409 row.setElementAt(0, 0, -pD * la * weight);
410 row.setElementAt(0, 1, -pD * lb * weight);
411 row.setElementAt(0, 2, -pD * lc * weight);
412
413 // columns 3, 4, 5, 6, 7, 8 are left with zero values
414
415 row.setElementAt(0, 9, pA * la * weight);
416 row.setElementAt(0, 10, pA * lb * weight);
417 row.setElementAt(0, 11, pA * lc * weight);
418
419 // normalize row
420 rowNorm = Math.sqrt(Math.pow(row.getElementAt(0, 0), 2.0)
421 + Math.pow(row.getElementAt(0, 1), 2.0)
422 + Math.pow(row.getElementAt(0, 2), 2.0)
423 + Math.pow(row.getElementAt(0, 9), 2.0)
424 + Math.pow(row.getElementAt(0, 10), 2.0)
425 + Math.pow(row.getElementAt(0, 11), 2.0));
426
427 row.setElementAt(0, 0, row.getElementAt(0, 0) / rowNorm);
428 row.setElementAt(0, 1, row.getElementAt(0, 1) / rowNorm);
429 row.setElementAt(0, 2, row.getElementAt(0, 2) / rowNorm);
430 row.setElementAt(0, 9, row.getElementAt(0, 9) / rowNorm);
431 row.setElementAt(0, 10, row.getElementAt(0, 10) / rowNorm);
432 row.setElementAt(0, 11, row.getElementAt(0, 11) / rowNorm);
433
434 // second row
435
436 // columns 0, 1, 2 are left with zero values
437
438 row.setElementAt(1, 3, -pD * la * weight);
439 row.setElementAt(1, 4, -pD * lb * weight);
440 row.setElementAt(1, 5, -pD * lc * weight);
441
442 // columns 6, 7, 8 are left with zero values
443
444 row.setElementAt(1, 9, pB * la * weight);
445 row.setElementAt(1, 10, pB * lb * weight);
446 row.setElementAt(1, 11, pB * lc * weight);
447
448 // normalize row
449 rowNorm = Math.sqrt(Math.pow(row.getElementAt(1, 3), 2.0)
450 + Math.pow(row.getElementAt(1, 4), 2.0)
451 + Math.pow(row.getElementAt(1, 5), 2.0)
452 + Math.pow(row.getElementAt(1, 9), 2.0)
453 + Math.pow(row.getElementAt(1, 10), 2.0)
454 + Math.pow(row.getElementAt(1, 11), 2.0));
455
456 row.setElementAt(1, 3, row.getElementAt(1, 3) / rowNorm);
457 row.setElementAt(1, 4, row.getElementAt(1, 4) / rowNorm);
458 row.setElementAt(1, 5, row.getElementAt(1, 5) / rowNorm);
459 row.setElementAt(1, 9, row.getElementAt(1, 9) / rowNorm);
460 row.setElementAt(1, 10, row.getElementAt(1, 10) / rowNorm);
461 row.setElementAt(1, 11, row.getElementAt(1, 11) / rowNorm);
462
463 // third row
464
465 // columns 0, 1, 2, 3, 4, 5 are left with zero values
466
467 row.setElementAt(2, 6, -pD * la * weight);
468 row.setElementAt(2, 7, -pD * lb * weight);
469 row.setElementAt(2, 8, -pD * lc * weight);
470
471 row.setElementAt(2, 9, pC * la * weight);
472 row.setElementAt(2, 10, pC * lb * weight);
473 row.setElementAt(2, 11, pC * lc * weight);
474
475 // normalize row
476 rowNorm = Math.sqrt(Math.pow(row.getElementAt(2, 6), 2.0)
477 + Math.pow(row.getElementAt(2, 7), 2.0)
478 + Math.pow(row.getElementAt(2, 8), 2.0)
479 + Math.pow(row.getElementAt(2, 9), 2.0)
480 + Math.pow(row.getElementAt(2, 10), 2.0)
481 + Math.pow(row.getElementAt(2, 11), 2.0));
482
483 row.setElementAt(2, 6, row.getElementAt(2, 6) / rowNorm);
484 row.setElementAt(2, 7, row.getElementAt(2, 7) / rowNorm);
485 row.setElementAt(2, 8, row.getElementAt(2, 8) / rowNorm);
486 row.setElementAt(2, 9, row.getElementAt(2, 9) / rowNorm);
487 row.setElementAt(2, 10, row.getElementAt(2, 10) / rowNorm);
488 row.setElementAt(2, 11, row.getElementAt(2, 11) / rowNorm);
489
490 // transRow = row'
491 row.transpose(transRow);
492 // tmp = row' * row
493 transRow.multiply(row, tmp);
494
495 tmp.multiplyByScalar(1.0 / previousNorm);
496
497 // a += 1.0 / previousNorm * tmp
498 a.add(tmp);
499 // normalize
500 previousNorm = Utils.normF(a);
501 a.multiplyByScalar(1.0 / previousNorm);
502
503 nMatches++;
504 }
505 index++;
506 }
507
508 if (nMatches < MIN_NUMBER_OF_LINE_PLANE_CORRESPONDENCES) {
509 throw new PinholeCameraEstimatorException();
510 }
511
512 final var decomposer = new SingularValueDecomposer(a);
513 decomposer.decompose();
514
515 if (decomposer.getNullity() > 1) {
516 // line/plane configuration is degenerate and exists a linear
517 // combination of possible pinhole cameras (i.e. solution is not
518 // unique up to scale)
519 throw new PinholeCameraEstimatorException();
520 }
521
522 final var v = decomposer.getV();
523
524 // use last column of V as pinhole camera vector
525
526 // the last column of V contains pinhole camera matrix ordered by
527 // columns as: P11, P21, P31, P12, P22, P32, P13, P23, P33, P14, P24,
528 // P34, hence we reorder p
529 final var pinholeCameraMatrix = new Matrix(PinholeCamera.PINHOLE_CAMERA_MATRIX_ROWS,
530 PinholeCamera.PINHOLE_CAMERA_MATRIX_COLS);
531
532 pinholeCameraMatrix.setElementAt(0, 0, v.getElementAt(0, 11));
533 pinholeCameraMatrix.setElementAt(1, 0, v.getElementAt(1, 11));
534 pinholeCameraMatrix.setElementAt(2, 0, v.getElementAt(2, 11));
535
536 pinholeCameraMatrix.setElementAt(0, 1, v.getElementAt(3, 11));
537 pinholeCameraMatrix.setElementAt(1, 1, v.getElementAt(4, 11));
538 pinholeCameraMatrix.setElementAt(2, 1, v.getElementAt(5, 11));
539
540 pinholeCameraMatrix.setElementAt(0, 2, v.getElementAt(6, 11));
541 pinholeCameraMatrix.setElementAt(1, 2, v.getElementAt(7, 11));
542 pinholeCameraMatrix.setElementAt(2, 2, v.getElementAt(8, 11));
543
544 pinholeCameraMatrix.setElementAt(0, 3, v.getElementAt(9, 11));
545 pinholeCameraMatrix.setElementAt(1, 3, v.getElementAt(10, 11));
546 pinholeCameraMatrix.setElementAt(2, 3, v.getElementAt(11, 11));
547
548 // because pinholeCameraMatrix has been obtained as the last column
549 // of V, then its Frobenius norm will be 1 because SVD already
550 // returns normalized singular vector
551
552 final var camera = new PinholeCamera(pinholeCameraMatrix);
553
554 if (listener != null) {
555 listener.onEstimateEnd(this);
556 }
557
558 return attemptRefine(camera);
559
560 } catch (final PinholeCameraEstimatorException e) {
561 // ensure it is no longer locked
562 locked = false;
563 throw e;
564 } catch (final Exception e) {
565 // ensure it is no longer locked
566 locked = false;
567 throw new PinholeCameraEstimatorException(e);
568 } finally {
569 locked = false;
570 }
571 }
572
573 /**
574 * Returns type of pinhole camera estimator.
575 *
576 * @return type of pinhole camera estimator.
577 */
578 @Override
579 public PinholeCameraEstimatorType getType() {
580 return PinholeCameraEstimatorType.WEIGHTED_LINE_PLANE_PINHOLE_CAMERA_ESTIMATOR;
581 }
582
583 }