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