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