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.AlgebraException;
19 import com.irurueta.algebra.Matrix;
20 import com.irurueta.geometry.DualQuadric;
21 import com.irurueta.geometry.Plane;
22 import com.irurueta.numerical.robust.RobustEstimatorException;
23 import com.irurueta.numerical.robust.RobustEstimatorMethod;
24
25 import java.util.List;
26
27 /**
28 * This is an abstract class for algorithms to robustly find the best dual
29 * quadric that fits in a collection of 3D planes.
30 * Implementations of this class should be able to detect and discard outliers
31 * in order to find the best solution.
32 */
33 @SuppressWarnings("DuplicatedCode")
34 public abstract class DualQuadricRobustEstimator {
35 /**
36 * Minimum number of 3D planes required to estimate a Dual Quadric.
37 */
38 public static final int MINIMUM_SIZE = 9;
39
40 /**
41 * Default amount of progress variation before notifying a change in
42 * estimation progress. By default, this is set to 5%.
43 */
44 public static final float DEFAULT_PROGRESS_DELTA = 0.05f;
45
46 /**
47 * Minimum allowed value for progress delta.
48 */
49 public static final float MIN_PROGRESS_DELTA = 0.0f;
50
51 /**
52 * Maximum allowed value for progress delta.
53 */
54 public static final float MAX_PROGRESS_DELTA = 1.0f;
55
56 /**
57 * Constant defining default confidence of the estimated result, which is
58 * 99%. This means that with a probability of 99% estimation will be
59 * accurate because chosen sub-samples will be inliers.
60 */
61 public static final double DEFAULT_CONFIDENCE = 0.99;
62
63 /**
64 * Default maximum allowed number of iterations.
65 */
66 public static final int DEFAULT_MAX_ITERATIONS = 5000;
67
68 /**
69 * Minimum allowed confidence value.
70 */
71 public static final double MIN_CONFIDENCE = 0.0;
72
73 /**
74 * Maximum allowed confidence value.
75 */
76 public static final double MAX_CONFIDENCE = 1.0;
77
78 /**
79 * Minimum allowed number of iterations.
80 */
81 public static final int MIN_ITERATIONS = 1;
82
83 /**
84 * Default robust estimator method when none is provided.
85 */
86 public static final RobustEstimatorMethod DEFAULT_ROBUST_METHOD = RobustEstimatorMethod.PROMEDS;
87
88 /**
89 * Listener to be notified of events such as when estimation starts, ends
90 * or its progress significantly changes.
91 */
92 protected DualQuadricRobustEstimatorListener listener;
93
94 /**
95 * Indicates if this estimator is locked because an estimation is being
96 * computed.
97 */
98 protected volatile boolean locked;
99
100 /**
101 * Amount of progress variation before notifying a progress change during
102 * estimation.
103 */
104 protected float progressDelta;
105
106 /**
107 * Amount of confidence expressed as a value between 0.0 and 1.0 (which is
108 * equivalent to 100%). The amount of confidence indicates the probability
109 * that the estimated result is correct. Usually this value will be close
110 * to 1.0, but not exactly 1.0.
111 */
112 protected double confidence;
113
114 /**
115 * Maximum allowed number of iterations. When the maximum number of
116 * iterations is exceeded, result will not be available, however an
117 * approximate result will be available for retrieval.
118 */
119 protected int maxIterations;
120
121 /**
122 * List of planes to be used to estimate a dual quadric. Provided list must
123 * have a size greater or equal than MINIMUM_SIZE.
124 */
125 protected List<Plane> planes;
126
127 /**
128 * Matrix representation of a 3D plane to be reused when computing
129 * residuals
130 */
131 private Matrix testPlane;
132
133 /**
134 * Matrix representation of a dual quadric to be reused when computing
135 * residuals.
136 */
137 private Matrix testDualQ;
138
139 /**
140 * Constructor.
141 */
142 protected DualQuadricRobustEstimator() {
143 progressDelta = DEFAULT_PROGRESS_DELTA;
144 confidence = DEFAULT_CONFIDENCE;
145 maxIterations = DEFAULT_MAX_ITERATIONS;
146 }
147
148 /**
149 * Constructor.
150 *
151 * @param listener listener to be notified of events such as when estimation
152 * starts, ends or its progress significantly changes.
153 */
154 protected DualQuadricRobustEstimator(final DualQuadricRobustEstimatorListener listener) {
155 this.listener = listener;
156 progressDelta = DEFAULT_PROGRESS_DELTA;
157 confidence = DEFAULT_CONFIDENCE;
158 maxIterations = DEFAULT_MAX_ITERATIONS;
159 }
160
161 /**
162 * Constructor with lines.
163 *
164 * @param planes planes to estimate a dual quadric.
165 * @throws IllegalArgumentException if provided list of planes don't have
166 * a size greater or equal than MINIMUM_SIZE.
167 */
168 protected DualQuadricRobustEstimator(final List<Plane> planes) {
169 progressDelta = DEFAULT_PROGRESS_DELTA;
170 confidence = DEFAULT_CONFIDENCE;
171 maxIterations = DEFAULT_MAX_ITERATIONS;
172 internalSetPlanes(planes);
173 }
174
175 /**
176 * Constructor.
177 *
178 * @param listener listener to be notified of events such as when estimation
179 * starts, ends or its progress significantly changes.
180 * @param planes planes to estimate a dual quadric.
181 * @throws IllegalArgumentException if provided list of planes don't have a
182 * size greater or equal than MINIMUM_SIZE.
183 */
184 protected DualQuadricRobustEstimator(final DualQuadricRobustEstimatorListener listener, final List<Plane> planes) {
185 this.listener = listener;
186 progressDelta = DEFAULT_PROGRESS_DELTA;
187 confidence = DEFAULT_CONFIDENCE;
188 maxIterations = DEFAULT_MAX_ITERATIONS;
189 internalSetPlanes(planes);
190 }
191
192 /**
193 * Returns reference to listener to be notified of events such as when
194 * estimation starts, ends or its progress significantly changes.
195 *
196 * @return listener to be notified of events.
197 */
198 public DualQuadricRobustEstimatorListener getListener() {
199 return listener;
200 }
201
202 /**
203 * Sets listener to be notified of events such as when estimation starts,
204 * ends or its progress significantly changes.
205 *
206 * @param listener listener to be notified of events.
207 * @throws LockedException if robust estimator is locked.
208 */
209 public void setListener(final DualQuadricRobustEstimatorListener listener) throws LockedException {
210 if (isLocked()) {
211 throw new LockedException();
212 }
213 this.listener = listener;
214 }
215
216 /**
217 * Indicates whether listener has been provided and is available for
218 * retrieval.
219 *
220 * @return true if available, false otherwise.
221 */
222 public boolean isListenerAvailable() {
223 return listener != null;
224 }
225
226 /**
227 * Indicates if this instance is locked because estimation is being computed.
228 *
229 * @return true if locked, false otherwise.
230 */
231 public boolean isLocked() {
232 return locked;
233 }
234
235 /**
236 * Returns amount of progress variation before notifying a progress change
237 * during estimation.
238 *
239 * @return amount of progress variation before notifying a progress change
240 * during estimation.
241 */
242 public float getProgressDelta() {
243 return progressDelta;
244 }
245
246 /**
247 * Sets amount of progress variation before notifying a progress change
248 * during estimation.
249 *
250 * @param progressDelta amount of progress variation before notifying a
251 * progress change during estimation.
252 * @throws IllegalArgumentException if progress delta is less than zero or
253 * greater than 1.
254 * @throws LockedException if this estimator is locked because an estimation
255 * is being computed.
256 */
257 public void setProgressDelta(final float progressDelta) throws LockedException {
258 if (isLocked()) {
259 throw new LockedException();
260 }
261 if (progressDelta < MIN_PROGRESS_DELTA || progressDelta > MAX_PROGRESS_DELTA) {
262 throw new IllegalArgumentException();
263 }
264 this.progressDelta = progressDelta;
265 }
266
267 /**
268 * Returns amount of confidence expressed as a value between 0.0 and 1.0
269 * (which is equivalent to 100%). The amount of confidence indicates that
270 * probability that the estimated result is correct. Usually this value will
271 * be close to 1.0, but not exactly 1.0.
272 *
273 * @return amount of confidence as a value between 0.0 and 1.0.
274 */
275 public double getConfidence() {
276 return confidence;
277 }
278
279 /**
280 * Sets amount of confidence expressed as a value between 0.0 and 1.0 (which
281 * is equivalent to 100%). The amount of confidence indicates the
282 * probability that the estimated result is correct. Usually this value will
283 * be close to 1.0, but not exactly 1.0.
284 *
285 * @param confidence confidence to be set as a value between 0.0 and 1.0.
286 * @throws IllegalArgumentException if provided value is not between 0.0 and
287 * 1.0.
288 * @throws LockedException if this estimator is locked because an estimator
289 * is being computed.
290 */
291 public void setConfidence(final double confidence) throws LockedException {
292 if (isLocked()) {
293 throw new LockedException();
294 }
295 if (confidence < MIN_CONFIDENCE || confidence > MAX_CONFIDENCE) {
296 throw new IllegalArgumentException();
297 }
298 this.confidence = confidence;
299 }
300
301 /**
302 * Returns maximum allowed number of iterations. If maximum allowed number
303 * of iterations is achieved without converging to a result when calling
304 * estimate(), a RobustEstimatorException will be raised.
305 *
306 * @return maximum allowed number of iterations.
307 */
308 public int getMaxIterations() {
309 return maxIterations;
310 }
311
312 /**
313 * Sets maximum allowed number of iterations. When the maximum number of
314 * iterations is exceeded, result will not be available, however an
315 * approximate result will be available for retrieval.
316 *
317 * @param maxIterations maximum allowed number of iterations to be set.
318 * @throws IllegalArgumentException if provided value is less than 1.
319 * @throws LockedException if this estimator is locked because an estimation
320 * is being computed.
321 */
322 public void setMaxIterations(final int maxIterations) throws LockedException {
323 if (isLocked()) {
324 throw new LockedException();
325 }
326 if (maxIterations < MIN_ITERATIONS) {
327 throw new IllegalArgumentException();
328 }
329 this.maxIterations = maxIterations;
330 }
331
332 /**
333 * Returns list of planes to be used to estimate a dual quadric.
334 * Provided list have a size greater or equal than MINIMUM_SIZE.
335 *
336 * @return list of planes to be used to estimate a dual quadric.
337 */
338 public List<Plane> getPlanes() {
339 return planes;
340 }
341
342 /**
343 * Sets list of planes to be used to estimate a dual quadric.
344 * Provided list must have a size greater or equal than MINIMUM_SIZE.
345 *
346 * @param planes list of planes to be used to estimate a dual quadric.
347 * @throws IllegalArgumentException if provided list of planes doesn't have
348 * a size greater or equal than MINIMUM_SIZE.
349 * @throws LockedException if estimator is locked because a computation is
350 * already in progress.
351 */
352 public void setPlanes(final List<Plane> planes) throws LockedException {
353 if (isLocked()) {
354 throw new LockedException();
355 }
356 internalSetPlanes(planes);
357 }
358
359 /**
360 * Indicates if estimator is ready to start the dual quadric estimation.
361 * This is true when MINIMUM_SIZE lines are available.
362 *
363 * @return true if estimator is ready, false otherwise.
364 */
365 public boolean isReady() {
366 return planes != null && planes.size() >= MINIMUM_SIZE;
367 }
368
369 /**
370 * Returns quality scores corresponding to each plane.
371 * The larger the score value the better the quality of the plane measure.
372 * This implementation always return null.
373 * Subclasses using quality scores must implement proper behaviour.
374 *
375 * @return quality scores corresponding to each plane.
376 */
377 public double[] getQualityScores() {
378 return null;
379 }
380
381 /**
382 * Sets quality scores corresponding to each plane.
383 * The larger the score value the better the quality of the matching.
384 * This implementation makes no action.
385 * Subclasses using quality scores must implement proper behaviour.
386 *
387 * @param qualityScores quality scores corresponding to each plane.
388 * @throws LockedException if robust estimator is locked because an
389 * estimation is already in progress.
390 * @throws IllegalArgumentException if provided quality scores length is
391 * smaller than MINIMUM_SIZE (i.e. 9 samples).
392 */
393 public void setQualityScores(final double[] qualityScores) throws LockedException {
394 }
395
396 /**
397 * Creates a dual quadric robust estimator based on plane samples and using
398 * provided robust estimator method.
399 *
400 * @param method method of a robust estimator algorithm to estimate bes dual
401 * quadric.
402 * @return an instance of a dual quadric robust estimator.
403 */
404 public static DualQuadricRobustEstimator create(final RobustEstimatorMethod method) {
405 return switch (method) {
406 case LMEDS -> new LMedSDualQuadricRobustEstimator();
407 case MSAC -> new MSACDualQuadricRobustEstimator();
408 case PROSAC -> new PROSACDualQuadricRobustEstimator();
409 case PROMEDS -> new PROMedSDualQuadricRobustEstimator();
410 default -> new RANSACDualQuadricRobustEstimator();
411 };
412 }
413
414 /**
415 * Creates a dual quadric robust estimator based on plane samples and using
416 * provided planes and robust estimator method.
417 *
418 * @param planes 3D planes to estimate a dual quadric.
419 * @param method method of a robust estimator algorithm to estimate the best
420 * dual quadric.
421 * @return an instance of a dual quadric robust estimator.
422 * @throws IllegalArgumentException if provided list of planes don't have a
423 * size greater or equal than MINIMUM_SIZE.
424 */
425 public static DualQuadricRobustEstimator create(final List<Plane> planes, final RobustEstimatorMethod method) {
426 return switch (method) {
427 case LMEDS -> new LMedSDualQuadricRobustEstimator(planes);
428 case MSAC -> new MSACDualQuadricRobustEstimator(planes);
429 case PROSAC -> new PROSACDualQuadricRobustEstimator(planes);
430 case PROMEDS -> new PROMedSDualQuadricRobustEstimator(planes);
431 default -> new RANSACDualQuadricRobustEstimator(planes);
432 };
433 }
434
435 /**
436 * Creates a dual quadric robust estimator based on plane samples and using
437 * provided listener.
438 *
439 * @param listener listener to be notified of events such as when estimation
440 * starts, ends or its progress significantly changes.
441 * @param method method of a robust estimator algorithm to estimate the best
442 * dual quadric.
443 * @return an instance of a dual quadric robust estimator.
444 */
445 public static DualQuadricRobustEstimator create(
446 final DualQuadricRobustEstimatorListener listener, final RobustEstimatorMethod method) {
447 return switch (method) {
448 case LMEDS -> new LMedSDualQuadricRobustEstimator(listener);
449 case MSAC -> new MSACDualQuadricRobustEstimator(listener);
450 case PROSAC -> new PROSACDualQuadricRobustEstimator(listener);
451 case PROMEDS -> new PROMedSDualQuadricRobustEstimator(listener);
452 default -> new RANSACDualQuadricRobustEstimator(listener);
453 };
454 }
455
456 /**
457 * Creates a dual quadric robust estimator based on plane samples and using
458 * provided listener and planes.
459 *
460 * @param listener listener to be notified of events such as when estimation
461 * starts, ends or its progress significantly changes.
462 * @param planes 3D planes to estimate a dual quadric.
463 * @param method method of a robust estimator algorithm to estimate the best
464 * dual quadric.
465 * @return an instance of a dual quadric robust estimator.
466 * @throws IllegalArgumentException if provided list of planes don't have a
467 * size greater or equal than MINIMUM_SIZE.
468 */
469 public static DualQuadricRobustEstimator create(
470 final DualQuadricRobustEstimatorListener listener, final List<Plane> planes,
471 final RobustEstimatorMethod method) {
472 return switch (method) {
473 case LMEDS -> new LMedSDualQuadricRobustEstimator(listener, planes);
474 case MSAC -> new MSACDualQuadricRobustEstimator(listener, planes);
475 case PROSAC -> new PROSACDualQuadricRobustEstimator(listener, planes);
476 case PROMEDS -> new PROMedSDualQuadricRobustEstimator(listener, planes);
477 default -> new RANSACDualQuadricRobustEstimator(listener, planes);
478 };
479 }
480
481 /**
482 * Creates a dual quadric robust estimator based on plane samples and using
483 * provided robust estimator method.
484 *
485 * @param qualityScores quality scores corresponding to each provided plane.
486 * @param method method of a robust estimator algorithm to estimate bes dual
487 * quadric.
488 * @return an instance of a dual quadric robust estimator.
489 * @throws IllegalArgumentException if provided quality scores length is
490 * smaller than MINIMUM_SIZE (i.e. 9 planes).
491 */
492 public static DualQuadricRobustEstimator create(final double[] qualityScores, final RobustEstimatorMethod method) {
493 return switch (method) {
494 case LMEDS -> new LMedSDualQuadricRobustEstimator();
495 case MSAC -> new MSACDualQuadricRobustEstimator();
496 case PROSAC -> new PROSACDualQuadricRobustEstimator(qualityScores);
497 case PROMEDS -> new PROMedSDualQuadricRobustEstimator(qualityScores);
498 default -> new RANSACDualQuadricRobustEstimator();
499 };
500 }
501
502 /**
503 * Creates a dual quadric robust estimator method based on plane samples and
504 * using provided planes and robust estimator method.
505 *
506 * @param planes 3D planes to estimate a dual quadric.
507 * @param qualityScores quality scores corresponding to each provided plane.
508 * @param method method of a robust estimator algorithm to estimate the best
509 * dual quadric.
510 * @return an instance of a dual quadric robust estimator.
511 * @throws IllegalArgumentException if provided list of planes don't have
512 * the same size as the list of provided quality scores, or if their size
513 * is not greater or equal than MINIMUM_SIZE.
514 */
515 public static DualQuadricRobustEstimator create(
516 final List<Plane> planes, final double[] qualityScores, final RobustEstimatorMethod method) {
517 return switch (method) {
518 case LMEDS -> new LMedSDualQuadricRobustEstimator(planes);
519 case MSAC -> new MSACDualQuadricRobustEstimator(planes);
520 case PROSAC -> new PROSACDualQuadricRobustEstimator(planes, qualityScores);
521 case PROMEDS -> new PROMedSDualQuadricRobustEstimator(planes, qualityScores);
522 default -> new RANSACDualQuadricRobustEstimator(planes);
523 };
524 }
525
526 /**
527 * Creates a dual quadric robust estimator based on plane samples and using
528 * provided listener.
529 *
530 * @param listener listener to be notified of events such as when estimation
531 * starts, ends or its progress significantly changes.
532 * @param qualityScores quality scores corresponding to each provided plane.
533 * @param method method of a robust estimator algorithm to estimate the best
534 * dual quadric.
535 * @return an instance of a dual quadric robust estimator.
536 * @throws IllegalArgumentException if provided quality scores length is
537 * smaller than MINIMUM_SIZE (i.e. 9 planes).
538 */
539 public static DualQuadricRobustEstimator create(
540 final DualQuadricRobustEstimatorListener listener, final double[] qualityScores,
541 final RobustEstimatorMethod method) {
542 return switch (method) {
543 case LMEDS -> new LMedSDualQuadricRobustEstimator(listener);
544 case MSAC -> new MSACDualQuadricRobustEstimator(listener);
545 case PROSAC -> new PROSACDualQuadricRobustEstimator(listener, qualityScores);
546 case PROMEDS -> new PROMedSDualQuadricRobustEstimator(listener, qualityScores);
547 default -> new RANSACDualQuadricRobustEstimator(listener);
548 };
549 }
550
551 /**
552 * Creates a dual conic robust estimator based on 3D plane samples and using
553 * provided listener and planes.
554 *
555 * @param listener listener to be notified of events such as when estimation
556 * starts, ends or its progress significantly changes.
557 * @param planes 3D planes to estimate a dual quadric.
558 * @param qualityScores quality scores corresponding to each provided plane.
559 * @param method method of a robust estimator algorithm to estimate the best
560 * dual quadric.
561 * @return an instance of a dual quadric robust estimator.
562 * @throws IllegalArgumentException if provided list of planes don't have
563 * the same size as the list of provided quality scores, or it their size is
564 * not greater or equal than MINIMUM_SIZE.
565 */
566 public static DualQuadricRobustEstimator create(
567 final DualQuadricRobustEstimatorListener listener, final List<Plane> planes,
568 final double[] qualityScores, final RobustEstimatorMethod method) {
569 return switch (method) {
570 case LMEDS -> new LMedSDualQuadricRobustEstimator(listener, planes);
571 case MSAC -> new MSACDualQuadricRobustEstimator(listener, planes);
572 case PROSAC -> new PROSACDualQuadricRobustEstimator(listener, planes, qualityScores);
573 case PROMEDS -> new PROMedSDualQuadricRobustEstimator(listener, planes, qualityScores);
574 default -> new RANSACDualQuadricRobustEstimator(listener, planes);
575 };
576 }
577
578 /**
579 * Creates a dual quadric robust estimator based on plane samples and using
580 * default robust estimator method.
581 *
582 * @return an instance of a dual quadric robust estimator.
583 */
584 public static DualQuadricRobustEstimator create() {
585 return create(DEFAULT_ROBUST_METHOD);
586 }
587
588 /**
589 * Creates a dual quadric robust estimator based on plane samples and using
590 * provided planes and default robust estimator method.
591 *
592 * @param planes 3D planes to estimate a dual quadric.
593 * @return an instance of a dual quadric robust estimator.
594 * @throws IllegalArgumentException if provided list of planes doesn't have
595 * a size greater or equal than MINIMUM_SIZE.
596 */
597 public static DualQuadricRobustEstimator create(final List<Plane> planes) {
598 return create(planes, DEFAULT_ROBUST_METHOD);
599 }
600
601 /**
602 * Creates a dual quadric robust estimator based on plane samples and using
603 * provided listener and default robust estimator method.
604 *
605 * @param listener listener to be notified of events such as when estimation
606 * starts, ends or its progress significantly changes.
607 * @return an instance of a dual quadric robust estimator.
608 */
609 public static DualQuadricRobustEstimator create(final DualQuadricRobustEstimatorListener listener) {
610 return create(listener, DEFAULT_ROBUST_METHOD);
611 }
612
613 /**
614 * Creates a dual quadric robust estimator based on plane samples and using
615 * provided listener and lines and default robust estimator method.
616 *
617 * @param listener listener to be notified of events such as when estimation
618 * starts, ends or its progress significantly changes.
619 * @param planes 3D planes to estimate a dual quadric.
620 * @return an instance of a dual conic robust estimator.
621 * @throws IllegalArgumentException if provided list of planes doesn't have
622 * a size greater or equal than MINIMUM_SIZE.
623 */
624 public static DualQuadricRobustEstimator create(
625 final DualQuadricRobustEstimatorListener listener, final List<Plane> planes) {
626 return create(listener, planes, DEFAULT_ROBUST_METHOD);
627 }
628
629 /**
630 * Creates a dual quadric robust estimator based on 3D plane samples and
631 * using default robust estimator method.
632 *
633 * @param qualityScores quality scores corresponding to each provided plane.
634 * @return an instance of a dual quadric robust estimator.
635 * @throws IllegalArgumentException if provided quality scores length is
636 * smaller than MINIMUM_SIZE (i.e. 9 planes).
637 */
638 public static DualQuadricRobustEstimator create(final double[] qualityScores) {
639 return create(qualityScores, DEFAULT_ROBUST_METHOD);
640 }
641
642 /**
643 * Creates a dual quadric robust estimator based on plane samples and using
644 * provided planes and default estimator method.
645 *
646 * @param planes 3D planes to estimate a dual quadric.
647 * @param qualityScores quality scores corresponding to each provided plane.
648 * @return an instance of a dual conic robust estimator.
649 * @throws IllegalArgumentException if provided list of planes don't have the
650 * same size as the list of provided quality scores, or if their size is not
651 * greater or equal than MINIMUM_SIZE.
652 */
653 public static DualQuadricRobustEstimator create(final List<Plane> planes, final double[] qualityScores) {
654 return create(planes, qualityScores, DEFAULT_ROBUST_METHOD);
655 }
656
657 /**
658 * Creates a dual quadric robust estimator based on plane samples and using
659 * provided listener and default estimator method.
660 *
661 * @param listener listener to be notified of events such as when estimation
662 * starts, ends or its progress significantly changes.
663 * @param qualityScores quality scores corresponding to each provided plane
664 * @return an instance of a dual quadric robust estimator.
665 * @throws IllegalArgumentException if provided quality scores length is
666 * smaller than MINIMUM_SIZE (i.e. 9 planes).
667 */
668 public static DualQuadricRobustEstimator create(
669 final DualQuadricRobustEstimatorListener listener, final double[] qualityScores) {
670 return create(listener, qualityScores, DEFAULT_ROBUST_METHOD);
671 }
672
673 /**
674 * Creates a dual quadric robust estimator based on plane samples and using
675 * provided listener and planes and default estimator method.
676 *
677 * @param listener listener to be notified of events such as when estimation
678 * starts, ends or its progress significantly changes.
679 * @param planes 3D planes to estimate a dual quadric.
680 * @param qualityScores quality scores corresponding to each provided plane
681 * @return an instance of a dual quadric robust estimator.
682 * @throws IllegalArgumentException if provided list of planes don't have
683 * the same size as the list of provided quality scores, or if their size is
684 * not greater or equal than MINIMUM_SIZE.
685 */
686 public static DualQuadricRobustEstimator create(
687 final DualQuadricRobustEstimatorListener listener, final List<Plane> planes, final double[] qualityScores) {
688 return create(listener, planes, qualityScores, DEFAULT_ROBUST_METHOD);
689 }
690
691 /**
692 * Estimates a dual conic using a robust estimator and the best set of 3D
693 * planes that fit into the locus of the estimated dual quadric found using
694 * the robust estimator.
695 *
696 * @return a dual quadric.
697 * @throws LockedException if robust estimator is locked because an
698 * estimation is already in progress.
699 * @throws NotReadyException if provided input data is not enough to start
700 * the estimation.
701 * @throws RobustEstimatorException if estimation fails for any reason (i.e.
702 * numerical instability, no solution available, etc).
703 */
704 public abstract DualQuadric estimate() throws LockedException, NotReadyException, RobustEstimatorException;
705
706 /**
707 * Returns method being used for robust estimation.
708 *
709 * @return method being used for robust estimation.
710 */
711 public abstract RobustEstimatorMethod getMethod();
712
713 /**
714 * Internal method to set list of planes to be used to estimate a dual
715 * quadric.
716 * This method does not check whether estimator is locked or not.
717 *
718 * @param planes list of planes to be used to estimate a dual quadric.
719 * @throws IllegalArgumentException if provided list of planes doesn't have
720 * a size greater or equal than MINIMUM_SIZE.
721 */
722 private void internalSetPlanes(final List<Plane> planes) {
723 if (planes.size() < MINIMUM_SIZE) {
724 throw new IllegalArgumentException();
725 }
726 this.planes = planes;
727 }
728
729 /**
730 * Computes the residual between a dual quadric and a 3D plane.
731 *
732 * @param dq a dual quadric.
733 * @param plane a 3D plane.
734 * @return residual.
735 */
736 protected double residual(final DualQuadric dq, final Plane plane) {
737 dq.normalize();
738 try {
739 if (testDualQ == null) {
740 testDualQ = dq.asMatrix();
741 } else {
742 dq.asMatrix(testDualQ);
743 }
744
745 if (testPlane == null) {
746 testPlane = new Matrix(Plane.PLANE_NUMBER_PARAMS, 1);
747 }
748 plane.normalize();
749 testPlane.setElementAt(0, 0, plane.getA());
750 testPlane.setElementAt(1, 0, plane.getB());
751 testPlane.setElementAt(2, 0, plane.getC());
752 testPlane.setElementAt(3, 0, plane.getD());
753 final var locusMatrix = testPlane.transposeAndReturnNew();
754 locusMatrix.multiply(testDualQ);
755 locusMatrix.multiply(testPlane);
756 return Math.abs(locusMatrix.getElementAt(0, 0));
757 } catch (final AlgebraException e) {
758 return Double.MAX_VALUE;
759 }
760 }
761 }