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.geometry.Line2D;
19 import com.irurueta.geometry.PinholeCamera;
20 import com.irurueta.geometry.Plane;
21 import com.irurueta.numerical.robust.PROSACRobustEstimator;
22 import com.irurueta.numerical.robust.PROSACRobustEstimatorListener;
23 import com.irurueta.numerical.robust.RobustEstimator;
24 import com.irurueta.numerical.robust.RobustEstimatorException;
25 import com.irurueta.numerical.robust.RobustEstimatorMethod;
26
27 import java.util.ArrayList;
28 import java.util.List;
29
30 /**
31 * Finds the best pinhole camera for provided collections of matched lines and
32 * planes using PROSAC algorithm.
33 */
34 @SuppressWarnings("DuplicatedCode")
35 public class PROSACDLTLinePlaneCorrespondencePinholeCameraRobustEstimator
36 extends DLTLinePlaneCorrespondencePinholeCameraRobustEstimator {
37
38 /**
39 * Constant defining default threshold to determine whether planes are
40 * inliers or not.
41 * Residuals to determine whether planes are inliers or not are computed by
42 * comparing two planes algebraically (e.g. doing the dot product of their
43 * parameters).
44 * A residual of 0 indicates that dot product was 1 or -1 and lines were
45 * equal.
46 * A residual of 1 indicates that dot product was 0 and lines were
47 * orthogonal.
48 * If dot product between lines is -1, then although their director vectors
49 * are opposed, lines are considered equal, since sign changes are not taken
50 * into account and their residuals will be 0.
51 */
52 public static final double DEFAULT_THRESHOLD = 1e-6;
53
54 /**
55 * Minimum value that can be set as threshold.
56 * Threshold must be strictly greater than 0.0.
57 */
58 public static final double MIN_THRESHOLD = 0.0;
59
60 /**
61 * Indicates that by default inliers will only be computed but not kept.
62 */
63 public static final boolean DEFAULT_COMPUTE_AND_KEEP_INLIERS = false;
64
65 /**
66 * Indicates that by default residuals will only be computed but not kept.
67 */
68 public static final boolean DEFAULT_COMPUTE_AND_KEEP_RESIDUALS = false;
69
70 /**
71 * Threshold to determine whether planes are inliers or not when testing
72 * possible estimation solutions.
73 * The threshold refers to the amount of error a possible solution has on a
74 * plane respect the back-projected plane of a line using estimated camera.
75 */
76 private double threshold;
77
78 /**
79 * Quality scores corresponding to each pair of matched points.
80 * The larger the score value the better the quality of the matching.
81 */
82 private double[] qualityScores;
83
84 /**
85 * Indicates whether inliers must be computed and kept.
86 */
87 private boolean computeAndKeepInliers;
88
89 /**
90 * Indicates whether residuals must be computed and kept.
91 */
92 private boolean computeAndKeepResiduals;
93
94 /**
95 * Constructor.
96 */
97 public PROSACDLTLinePlaneCorrespondencePinholeCameraRobustEstimator() {
98 super();
99 threshold = DEFAULT_THRESHOLD;
100 computeAndKeepInliers = DEFAULT_COMPUTE_AND_KEEP_INLIERS;
101 computeAndKeepResiduals = DEFAULT_COMPUTE_AND_KEEP_RESIDUALS;
102 }
103
104 /**
105 * Constructor with lists of matched planes and 2D lines to estimate a
106 * pinhole camera.
107 * Points and lines in the lists located at the same position are considered
108 * to be matched. Hence, both lists must have the same size, and their size
109 * must be greater or equal than MIN_NUMBER_OF_LINE_PLANE_CORRESPONDENCES
110 * (4 matches).
111 *
112 * @param planes list of planes used to estimate a pinhole camera.
113 * @param lines list of corresponding projected 2D lines used to estimate
114 * a pinhole camera.
115 * @throws IllegalArgumentException if provided lists don't have the same
116 * size or their size is smaller than required minimum size (4 matches).
117 */
118 public PROSACDLTLinePlaneCorrespondencePinholeCameraRobustEstimator(
119 final List<Plane> planes, final List<Line2D> lines) {
120 super(planes, lines);
121 threshold = DEFAULT_THRESHOLD;
122 computeAndKeepInliers = DEFAULT_COMPUTE_AND_KEEP_INLIERS;
123 computeAndKeepResiduals = DEFAULT_COMPUTE_AND_KEEP_RESIDUALS;
124 }
125
126 /**
127 * Constructor with listener.
128 *
129 * @param listener listener to be notified of events such as when estimation
130 * starts, ends or its progress significantly changes.
131 */
132 public PROSACDLTLinePlaneCorrespondencePinholeCameraRobustEstimator(
133 final PinholeCameraRobustEstimatorListener listener) {
134 super(listener);
135 threshold = DEFAULT_THRESHOLD;
136 computeAndKeepInliers = DEFAULT_COMPUTE_AND_KEEP_INLIERS;
137 computeAndKeepResiduals = DEFAULT_COMPUTE_AND_KEEP_RESIDUALS;
138 }
139
140 /**
141 * Constructor with listener and lists of matched planes and 2D lines to
142 * estimate a pinhole camera.
143 * Points and lines in the lists located at the same position are considered
144 * to be matched. Hence, both lists must have the same size, and their size
145 * must be greater or equal than MIN_NUMBER_OF_LINE_PLANE_CORRESPONDENCES
146 * (4 matches).
147 *
148 * @param listener listener to be notified of events such as when estimation
149 * starts, ends or its progress significantly changes.
150 * @param planes list of planes used to estimate a pinhole camera.
151 * @param lines list of corresponding projected 2D lines used to estimate
152 * a pinhole camera.
153 * @throws IllegalArgumentException if provided lists don't have the same
154 * size or their size is smaller than required minimum size (4 matches).
155 */
156 public PROSACDLTLinePlaneCorrespondencePinholeCameraRobustEstimator(
157 final PinholeCameraRobustEstimatorListener listener, final List<Plane> planes, final List<Line2D> lines) {
158 super(listener, planes, lines);
159 threshold = DEFAULT_THRESHOLD;
160 computeAndKeepInliers = DEFAULT_COMPUTE_AND_KEEP_INLIERS;
161 computeAndKeepResiduals = DEFAULT_COMPUTE_AND_KEEP_RESIDUALS;
162 }
163
164 /**
165 * Constructor.
166 *
167 * @param qualityScores quality scores corresponding to each pair of matched
168 * points.
169 * @throws IllegalArgumentException if provided quality scores length is
170 * smaller than MIN_NUMBER_OF_LINE_PLANE_CORRESPONDENCES (i.e. 4 samples).
171 */
172 public PROSACDLTLinePlaneCorrespondencePinholeCameraRobustEstimator(final double[] qualityScores) {
173 super();
174 threshold = DEFAULT_THRESHOLD;
175 computeAndKeepInliers = DEFAULT_COMPUTE_AND_KEEP_INLIERS;
176 computeAndKeepResiduals = DEFAULT_COMPUTE_AND_KEEP_RESIDUALS;
177 internalSetQualityScores(qualityScores);
178 }
179
180 /**
181 * Constructor with lists of matched planes and 2D lines to estimate a
182 * pinhole camera.
183 * Points and lines in the lists located at the same position are considered
184 * to be matched. Hence, both lists must have the same size, and their size
185 * must be greater or equal than MIN_NUMBER_OF_LINE_PLANE_CORRESPONDENCES
186 * (4 matches).
187 *
188 * @param planes list of planes used to estimate a pinhole camera.
189 * @param lines list of corresponding projected 2D lines used to estimate
190 * a pinhole camera.
191 * @param qualityScores quality scores corresponding to each pair of matched
192 * points.
193 * @throws IllegalArgumentException if provided lists or quality scores
194 * don't have the same size or their size is smaller than required minimum
195 * size (4 matches).
196 */
197 public PROSACDLTLinePlaneCorrespondencePinholeCameraRobustEstimator(
198 final List<Plane> planes, final List<Line2D> lines, final double[] qualityScores) {
199 super(planes, lines);
200
201 if (qualityScores.length != planes.size()) {
202 throw new IllegalArgumentException();
203 }
204
205 threshold = DEFAULT_THRESHOLD;
206 computeAndKeepInliers = DEFAULT_COMPUTE_AND_KEEP_INLIERS;
207 computeAndKeepResiduals = DEFAULT_COMPUTE_AND_KEEP_RESIDUALS;
208 internalSetQualityScores(qualityScores);
209 }
210
211 /**
212 * Constructor with listener.
213 *
214 * @param listener listener to be notified of events such as when estimation
215 * starts, ends or its progress significantly changes.
216 * @param qualityScores quality scores corresponding to each pair of matched
217 * points.
218 * @throws IllegalArgumentException if provided quality scores length is
219 * smaller than MIN_NUMBER_OF_LINE_PLANE_CORRESPONDENCES (i.e. 4 samples).
220 */
221 public PROSACDLTLinePlaneCorrespondencePinholeCameraRobustEstimator(
222 final PinholeCameraRobustEstimatorListener listener, final double[] qualityScores) {
223 super(listener);
224 threshold = DEFAULT_THRESHOLD;
225 computeAndKeepInliers = DEFAULT_COMPUTE_AND_KEEP_INLIERS;
226 computeAndKeepResiduals = DEFAULT_COMPUTE_AND_KEEP_RESIDUALS;
227 internalSetQualityScores(qualityScores);
228 }
229
230 /**
231 * Constructor with listener and lists of matched planes and 2D lines to
232 * estimate a pinhole camera.
233 * Points and lines in the lists located at the same position are considered
234 * to be matched. Hence, both lists must have the same size, and their size
235 * must be greater or equal than MIN_NUMBER_OF_LINE_PLANE_CORRESPONDENCES
236 * (4 matches).
237 *
238 * @param listener listener to be notified of events such as when estimation
239 * starts, ends or its progress significantly changes.
240 * @param planes list of planes used to estimate a pinhole camera.
241 * @param lines list of corresponding projected 2D lines used to estimate
242 * a pinhole camera.
243 * @param qualityScores quality scores corresponding to each pair of matched
244 * points.
245 * @throws IllegalArgumentException if provided lists don't have the same
246 * size or their size is smaller than required minimum size (4 matches).
247 */
248 public PROSACDLTLinePlaneCorrespondencePinholeCameraRobustEstimator(
249 final PinholeCameraRobustEstimatorListener listener, final List<Plane> planes, final List<Line2D> lines,
250 final double[] qualityScores) {
251 super(listener, planes, lines);
252
253 if (qualityScores.length != planes.size()) {
254 throw new IllegalArgumentException();
255 }
256
257 threshold = DEFAULT_THRESHOLD;
258 computeAndKeepInliers = DEFAULT_COMPUTE_AND_KEEP_INLIERS;
259 computeAndKeepResiduals = DEFAULT_COMPUTE_AND_KEEP_RESIDUALS;
260 internalSetQualityScores(qualityScores);
261 }
262
263 /**
264 * Returns threshold to determine whether planes are inliers or not when
265 * testing possible estimation solutions.
266 * The threshold refers to the amount of error a possible solution has on a
267 * plane respect the back-projected plane of a line using estimated camera
268 * Residuals to determine whether planes are inliers or not are computed by
269 * comparing two planes algebraically (e.g. doing the dot product of their
270 * parameters).
271 * A residual of 0 indicates that dot product was 1 or -1 and planes were
272 * equal.
273 * A residual of 1 indicates that dot product was 0 and planes were
274 * orthogonal.
275 * If dot product between planes is -1, then although their director vectors
276 * are opposed, planes are considered equal, since sign changes are not
277 * taken into account and their residuals will be 0.
278 *
279 * @return threshold to determine whether matched planes are inliers or not.
280 */
281 public double getThreshold() {
282 return threshold;
283 }
284
285 /**
286 * Sets threshold to determine whether planes are inliers or not when
287 * testing possible estimation solutions.
288 * The threshold refers to the amount of error a possible solution has on a
289 * plane respect the back-projected plane of a line using estimated camera
290 * Residuals to determine whether planes are inliers or not are computed by
291 * comparing two planes algebraically (e.g. doing the dot product of their
292 * parameters).
293 * A residual of 0 indicates that dot product was 1 or -1 and planes were
294 * equal.
295 * A residual of 1 indicates that dot product was 0 and planes were
296 * orthogonal.
297 * If dot product between planes is -1, then although their director vectors
298 * are opposed, planes are considered equal, since sign changes are not
299 * taken into account and their residuals will be 0.
300 *
301 * @param threshold threshold to determine whether matched planes are
302 * inliers or not.
303 * @throws IllegalArgumentException if provided value is equal or less than
304 * zero.
305 * @throws LockedException if robust estimator is locked because an
306 * estimation is already in progress.
307 */
308 public void setThreshold(final double threshold) throws LockedException {
309 if (isLocked()) {
310 throw new LockedException();
311 }
312 if (threshold <= MIN_THRESHOLD) {
313 throw new IllegalArgumentException();
314 }
315 this.threshold = threshold;
316 }
317
318 /**
319 * Returns quality scores corresponding to each pair of matched points.
320 * The larger the score value the better the quality of the matching.
321 *
322 * @return quality scores corresponding to each pair of matched points.
323 */
324 @Override
325 public double[] getQualityScores() {
326 return qualityScores;
327 }
328
329 /**
330 * Sets quality scores corresponding to each pair of matched points.
331 * The larger the score value the better the quality of the matching.
332 *
333 * @param qualityScores quality scores corresponding to each pair of matched
334 * points.
335 * @throws LockedException if robust estimator is locked because an
336 * estimation is already in progress.
337 * @throws IllegalArgumentException if provided quality scores length is
338 * smaller than MIN_NUMBER_OF_LINE_PLANE_CORRESPONDENCES (i.e. 4 samples).
339 */
340 @Override
341 public void setQualityScores(final double[] qualityScores) throws LockedException {
342 if (isLocked()) {
343 throw new LockedException();
344 }
345 internalSetQualityScores(qualityScores);
346 }
347
348 /**
349 * Indicates if estimator is ready to start the affine 2D transformation
350 * estimation.
351 * This is true when input data (i.e. lists of matched points and quality
352 * scores) are provided and a minimum of MINIMUM_SIZE points are available.
353 *
354 * @return true if estimator is ready, false otherwise.
355 */
356 @Override
357 public boolean isReady() {
358 return super.isReady() && qualityScores != null && qualityScores.length == planes.size();
359 }
360
361 /**
362 * Indicates whether inliers must be computed and kept.
363 *
364 * @return true if inliers must be computed and kept, false if inliers
365 * only need to be computed but not kept.
366 */
367 public boolean isComputeAndKeepInliersEnabled() {
368 return computeAndKeepInliers;
369 }
370
371 /**
372 * Specifies whether inliers must be computed and kept.
373 *
374 * @param computeAndKeepInliers true if inliers must be computed and kept,
375 * false if inliers only need to be computed but not kept.
376 * @throws LockedException if estimator is locked.
377 */
378 public void setComputeAndKeepInliersEnabled(final boolean computeAndKeepInliers) throws LockedException {
379 if (isLocked()) {
380 throw new LockedException();
381 }
382 this.computeAndKeepInliers = computeAndKeepInliers;
383 }
384
385 /**
386 * Indicates whether residuals must be computed and kept.
387 *
388 * @return true if residuals must be computed and kept, false if residuals
389 * only need to be computed but not kept.
390 */
391 public boolean isComputeAndKeepResidualsEnabled() {
392 return computeAndKeepResiduals;
393 }
394
395 /**
396 * Specifies whether residuals must be computed and kept.
397 *
398 * @param computeAndKeepResiduals true if residuals must be computed and
399 * kept, false if residuals only need to be computed but not kept.
400 * @throws LockedException if estimator is locked.
401 */
402 public void setComputeAndKeepResidualsEnabled(final boolean computeAndKeepResiduals) throws LockedException {
403 if (isLocked()) {
404 throw new LockedException();
405 }
406 this.computeAndKeepResiduals = computeAndKeepResiduals;
407 }
408
409 /**
410 * Estimates a pinhole camera using a robust estimator and
411 * the best set of matched 2D line/3D plane correspondences found using the
412 * robust estimator.
413 *
414 * @return a pinhole camera.
415 * @throws LockedException if robust estimator is locked because an
416 * estimation is already in progress.
417 * @throws NotReadyException if provided input data is not enough to start
418 * the estimation.
419 * @throws RobustEstimatorException if estimation fails for any reason
420 * (i.e. numerical instability, no solution available, etc).
421 */
422 @Override
423 public PinholeCamera estimate() throws LockedException, NotReadyException, RobustEstimatorException {
424 if (isLocked()) {
425 throw new LockedException();
426 }
427 if (!isReady()) {
428 throw new NotReadyException();
429 }
430
431 // pinhole camera estimator using DLT (Direct Linear Transform) algorithm
432 final var nonRobustEstimator = new DLTLinePlaneCorrespondencePinholeCameraEstimator();
433
434 nonRobustEstimator.setLMSESolutionAllowed(false);
435
436 // suggestions
437 nonRobustEstimator.setSuggestSkewnessValueEnabled(isSuggestSkewnessValueEnabled());
438 nonRobustEstimator.setSuggestedSkewnessValue(getSuggestedSkewnessValue());
439 nonRobustEstimator.setSuggestHorizontalFocalLengthEnabled(isSuggestHorizontalFocalLengthEnabled());
440 nonRobustEstimator.setSuggestedHorizontalFocalLengthValue(getSuggestedHorizontalFocalLengthValue());
441 nonRobustEstimator.setSuggestVerticalFocalLengthEnabled(isSuggestVerticalFocalLengthEnabled());
442 nonRobustEstimator.setSuggestedVerticalFocalLengthValue(getSuggestedVerticalFocalLengthValue());
443 nonRobustEstimator.setSuggestAspectRatioEnabled(isSuggestAspectRatioEnabled());
444 nonRobustEstimator.setSuggestedAspectRatioValue(getSuggestedAspectRatioValue());
445 nonRobustEstimator.setSuggestPrincipalPointEnabled(isSuggestPrincipalPointEnabled());
446 nonRobustEstimator.setSuggestedPrincipalPointValue(getSuggestedPrincipalPointValue());
447 nonRobustEstimator.setSuggestRotationEnabled(isSuggestRotationEnabled());
448 nonRobustEstimator.setSuggestedRotationValue(getSuggestedRotationValue());
449 nonRobustEstimator.setSuggestCenterEnabled(isSuggestCenterEnabled());
450 nonRobustEstimator.setSuggestedCenterValue(getSuggestedCenterValue());
451
452 final var innerEstimator = new PROSACRobustEstimator<>(new PROSACRobustEstimatorListener<PinholeCamera>() {
453
454 // 3D planes for a subset of samples
455 private final List<Plane> subsetPlanes = new ArrayList<>();
456
457 // 2D lines for a subset of samples
458 private final List<Line2D> subsetLines = new ArrayList<>();
459
460 @Override
461 public double getThreshold() {
462 return threshold;
463 }
464
465 @Override
466 public int getTotalSamples() {
467 return planes.size();
468 }
469
470 @Override
471 public int getSubsetSize() {
472 return LinePlaneCorrespondencePinholeCameraEstimator.MIN_NUMBER_OF_LINE_PLANE_CORRESPONDENCES;
473 }
474
475 @Override
476 public void estimatePreliminarSolutions(final int[] samplesIndices, final List<PinholeCamera> solutions) {
477 subsetPlanes.clear();
478 subsetPlanes.add(planes.get(samplesIndices[0]));
479 subsetPlanes.add(planes.get(samplesIndices[1]));
480 subsetPlanes.add(planes.get(samplesIndices[2]));
481 subsetPlanes.add(planes.get(samplesIndices[3]));
482
483 subsetLines.clear();
484 subsetLines.add(lines.get(samplesIndices[0]));
485 subsetLines.add(lines.get(samplesIndices[1]));
486 subsetLines.add(lines.get(samplesIndices[2]));
487 subsetLines.add(lines.get(samplesIndices[3]));
488
489 try {
490 nonRobustEstimator.setLists(subsetPlanes, subsetLines);
491
492 final var cam = nonRobustEstimator.estimate();
493 solutions.add(cam);
494 } catch (final Exception e) {
495 // if lines/planes configuration is degenerate, no solution
496 // is added
497 }
498 }
499
500 @Override
501 public double computeResidual(final PinholeCamera currentEstimation, final int i) {
502 final var inputLine = lines.get(i);
503 final var inputPlane = planes.get(i);
504
505 return singleBackprojectionResidual(currentEstimation, inputLine, inputPlane);
506 }
507
508 @Override
509 public boolean isReady() {
510 return PROSACDLTLinePlaneCorrespondencePinholeCameraRobustEstimator.this.isReady();
511 }
512
513 @Override
514 public void onEstimateStart(final RobustEstimator<PinholeCamera> estimator) {
515 if (listener != null) {
516 listener.onEstimateStart(
517 PROSACDLTLinePlaneCorrespondencePinholeCameraRobustEstimator.this);
518 }
519 }
520
521 @Override
522 public void onEstimateEnd(final RobustEstimator<PinholeCamera> estimator) {
523 if (listener != null) {
524 listener.onEstimateEnd(PROSACDLTLinePlaneCorrespondencePinholeCameraRobustEstimator.this);
525 }
526 }
527
528 @Override
529 public void onEstimateNextIteration(final RobustEstimator<PinholeCamera> estimator, final int iteration) {
530 if (listener != null) {
531 listener.onEstimateNextIteration(
532 PROSACDLTLinePlaneCorrespondencePinholeCameraRobustEstimator.this, iteration);
533 }
534 }
535
536 @Override
537 public void onEstimateProgressChange(final RobustEstimator<PinholeCamera> estimator, final float progress) {
538 if (listener != null) {
539 listener.onEstimateProgressChange(
540 PROSACDLTLinePlaneCorrespondencePinholeCameraRobustEstimator.this, progress);
541 }
542 }
543
544 @Override
545 public double[] getQualityScores() {
546 return qualityScores;
547 }
548 });
549
550 try {
551 locked = true;
552 inliersData = null;
553 innerEstimator.setComputeAndKeepInliersEnabled(computeAndKeepInliers || refineResult);
554 innerEstimator.setComputeAndKeepResidualsEnabled(computeAndKeepResiduals || refineResult);
555 innerEstimator.setConfidence(confidence);
556 innerEstimator.setMaxIterations(maxIterations);
557 innerEstimator.setProgressDelta(progressDelta);
558 final var result = innerEstimator.estimate();
559 inliersData = innerEstimator.getInliersData();
560 return attemptRefine(result, nonRobustEstimator.getMaxSuggestionWeight());
561
562 } catch (final com.irurueta.numerical.LockedException e) {
563 throw new LockedException(e);
564 } catch (final com.irurueta.numerical.NotReadyException e) {
565 throw new NotReadyException(e);
566 } finally {
567 locked = false;
568 }
569 }
570
571 /**
572 * Returns method being used for robust estimation.
573 *
574 * @return method being used for robust estimation.
575 */
576 @Override
577 public RobustEstimatorMethod getMethod() {
578 return RobustEstimatorMethod.PROSAC;
579 }
580
581 /**
582 * Gets standard deviation used for Levenberg-Marquardt fitting during
583 * refinement.
584 * Returned value gives an indication of how much variance each residual
585 * has.
586 * Typically, this value is related to the threshold used on each robust
587 * estimation, since residuals of found inliers are within the range of
588 * such threshold.
589 *
590 * @return standard deviation used for refinement.
591 */
592 @Override
593 protected double getRefinementStandardDeviation() {
594 return threshold;
595 }
596
597 /**
598 * Sets quality scores corresponding to each pair of matched points.
599 * This method is used internally and does not check whether instance is
600 * locked or not.
601 *
602 * @param qualityScores quality scores to be set.
603 * @throws IllegalArgumentException if provided quality scores length is
604 * smaller than MINIMUM_SIZE.
605 */
606 private void internalSetQualityScores(final double[] qualityScores) {
607 if (qualityScores.length < MIN_NUMBER_OF_LINE_PLANE_CORRESPONDENCES) {
608 throw new IllegalArgumentException();
609 }
610
611 this.qualityScores = qualityScores;
612 }
613 }