1 /*
2 * Copyright (C) 2015 Alberto Irurueta Carro (alberto@irurueta.com)
3 *
4 * Licensed under the Apache License, Version 2.0 (the "License");
5 * you may not use this file except in compliance with the License.
6 * You may obtain a copy of the License at
7 *
8 * http://www.apache.org/licenses/LICENSE-2.0
9 *
10 * Unless required by applicable law or agreed to in writing, software
11 * distributed under the License is distributed on an "AS IS" BASIS,
12 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13 * See the License for the specific language governing permissions and
14 * limitations under the License.
15 */
16 package com.irurueta.geometry.estimators;
17
18 import com.irurueta.algebra.Matrix;
19 import com.irurueta.geometry.CoordinatesType;
20 import com.irurueta.geometry.HomogeneousPoint3D;
21 import com.irurueta.geometry.InhomogeneousPoint3D;
22 import com.irurueta.geometry.Plane;
23 import com.irurueta.geometry.Point3D;
24 import com.irurueta.geometry.refiners.HomogeneousPoint3DRefiner;
25 import com.irurueta.geometry.refiners.InhomogeneousPoint3DRefiner;
26 import com.irurueta.geometry.refiners.Point3DRefiner;
27 import com.irurueta.numerical.robust.InliersData;
28 import com.irurueta.numerical.robust.RobustEstimatorException;
29 import com.irurueta.numerical.robust.RobustEstimatorMethod;
30
31 import java.util.List;
32
33 /**
34 * This is an abstract class for algorithms to robustly find the best 3D point
35 * that intersects in a collection of 3D planes.
36 * Implementations of this class should be able to detect and discard outliers
37 * in order to find the best solution.
38 */
39 @SuppressWarnings("DuplicatedCode")
40 public abstract class Point3DRobustEstimator {
41 /**
42 * Minimum number of 3D planes required to estimate a point.
43 */
44 public static final int MINIMUM_SIZE = 3;
45
46 /**
47 * Default amount of progress variation before notifying a change in
48 * estimation progress. By default, this is set to 5%.
49 */
50 public static final float DEFAULT_PROGRESS_DELTA = 0.05f;
51
52 /**
53 * Minimum allowed value for progress delta.
54 */
55 public static final float MIN_PROGRESS_DELTA = 0.0f;
56
57 /**
58 * Maximum allowed value for progress delta.
59 */
60 public static final float MAX_PROGRESS_DELTA = 1.0f;
61
62 /**
63 * Constant defining default confidence of the estimated result, which is
64 * 99%. This means that with a probability of 99% estimation will be
65 * accurate because chosen sub-samples will be inliers.
66 */
67 public static final double DEFAULT_CONFIDENCE = 0.99;
68
69 /**
70 * Default maximum allowed number of iterations.
71 */
72 public static final int DEFAULT_MAX_ITERATIONS = 5000;
73
74 /**
75 * Minimum allowed confidence value.
76 */
77 public static final double MIN_CONFIDENCE = 0.0;
78
79 /**
80 * Maximum allowed confidence value.
81 */
82 public static final double MAX_CONFIDENCE = 1.0;
83
84 /**
85 * Minimum allowed number of iterations.
86 */
87 public static final int MIN_ITERATIONS = 1;
88
89 /**
90 * Default robust estimator method when none is provided.
91 */
92 public static final RobustEstimatorMethod DEFAULT_ROBUST_METHOD = RobustEstimatorMethod.PROMEDS;
93
94 /**
95 * Indicates that result is refined by default using Levenberg-Marquardt
96 * fitting algorithm over found inliers.
97 */
98 public static final boolean DEFAULT_REFINE_RESULT = true;
99
100 /**
101 * Indicates that covariance is not kept by default after refining result.
102 */
103 public static final boolean DEFAULT_KEEP_COVARIANCE = false;
104
105 /**
106 * Listener to be notified of events such as when estimation starts, ends
107 * or its progress significantly changes.
108 */
109 protected Point3DRobustEstimatorListener listener;
110
111 /**
112 * Indicates if this estimator is locked because an estimation is being
113 * computed.
114 */
115 protected volatile boolean locked;
116
117 /**
118 * Amount of progress variation before notifying a progress change during
119 * estimation.
120 */
121 protected float progressDelta;
122
123 /**
124 * Amount of confidence expressed as a value between 0.0 and 1.0 (which is
125 * equivalent to 100%). The amount of confidence indicates the probability
126 * that the estimated result is correct. Usually this value will be close
127 * to 1.0, but not exactly 1.0.
128 */
129 protected double confidence;
130
131 /**
132 * Maximum allowed number of iterations. When the maximum number of
133 * iterations is exceeded, result will not be available, however an
134 * approximate result will be available for retrieval.
135 */
136 protected int maxIterations;
137
138 /**
139 * List of lines to be used to estimate a 3D point. Provided list must have
140 * a size greater or equal than MINIMUM_SIZE.
141 */
142 protected List<Plane> planes;
143
144 /**
145 * Data related to inliers found after estimation.
146 */
147 protected InliersData inliersData;
148
149 /**
150 * Indicates whether result must be refined using Levenberg-Marquardt
151 * fitting algorithm over found inliers.
152 * If true, inliers will be computed and kept in any implementation
153 * regardless of the settings.
154 */
155 protected boolean refineResult;
156
157 /**
158 * Coordinates type to use for refinement. When using inhomogeneous
159 * coordinates a 3x3 covariance matrix is estimated. When using homogeneous
160 * coordinates a 4x4 covariance matrix is estimated.
161 */
162 private CoordinatesType refinementCoordinatesType = CoordinatesType.INHOMOGENEOUS_COORDINATES;
163
164 /**
165 * Indicates whether covariance must be kept after refining result.
166 * This setting is only taken into account if result is refined.
167 */
168 private boolean keepCovariance;
169
170 /**
171 * Estimated covariance of estimated 3D point.
172 * This is only available when result has been refined and covariance is
173 * kept.
174 */
175 private Matrix covariance;
176
177 /**
178 * Constructor.
179 */
180 protected Point3DRobustEstimator() {
181 progressDelta = DEFAULT_PROGRESS_DELTA;
182 confidence = DEFAULT_CONFIDENCE;
183 maxIterations = DEFAULT_MAX_ITERATIONS;
184 refineResult = DEFAULT_REFINE_RESULT;
185 keepCovariance = DEFAULT_KEEP_COVARIANCE;
186 }
187
188 /**
189 * Constructor.
190 *
191 * @param listener listener to be notified of events such as when estimation
192 * starts, ends or its progress significantly changes.
193 */
194 protected Point3DRobustEstimator(final Point3DRobustEstimatorListener listener) {
195 this.listener = listener;
196 progressDelta = DEFAULT_PROGRESS_DELTA;
197 confidence = DEFAULT_CONFIDENCE;
198 maxIterations = DEFAULT_MAX_ITERATIONS;
199 refineResult = DEFAULT_REFINE_RESULT;
200 keepCovariance = DEFAULT_KEEP_COVARIANCE;
201 }
202
203 /**
204 * Constructor with lines.
205 *
206 * @param planes 3D planes to estimate a 3D point.
207 * @throws IllegalArgumentException if provided list of lines don't have
208 * a size greater or equal than MINIMUM_SIZE.
209 */
210 protected Point3DRobustEstimator(final List<Plane> planes) {
211 progressDelta = DEFAULT_PROGRESS_DELTA;
212 confidence = DEFAULT_CONFIDENCE;
213 maxIterations = DEFAULT_MAX_ITERATIONS;
214 internalSetPlanes(planes);
215 refineResult = DEFAULT_REFINE_RESULT;
216 keepCovariance = DEFAULT_KEEP_COVARIANCE;
217 }
218
219 /**
220 * Constructor.
221 *
222 * @param planes 3D planes to estimate a 3D point.
223 * @param listener listener to be notified of events such as when estimation
224 * starts, ends or its progress significantly changes.
225 * @throws IllegalArgumentException if provided list of lines don't have
226 * a size greater or equal than MINIMUM_SIZE.
227 */
228 protected Point3DRobustEstimator(final Point3DRobustEstimatorListener listener, final List<Plane> planes) {
229 this.listener = listener;
230 progressDelta = DEFAULT_PROGRESS_DELTA;
231 confidence = DEFAULT_CONFIDENCE;
232 maxIterations = DEFAULT_MAX_ITERATIONS;
233 internalSetPlanes(planes);
234 refineResult = DEFAULT_REFINE_RESULT;
235 keepCovariance = DEFAULT_KEEP_COVARIANCE;
236 }
237
238
239 /**
240 * Returns reference to listener to be notified of events such as when
241 * estimation starts, ends or its progress significantly changes.
242 *
243 * @return listener to be notified of events.
244 */
245 public Point3DRobustEstimatorListener getListener() {
246 return listener;
247 }
248
249 /**
250 * Sets listener to be notified of events such as when estimation starts,
251 * ends or its progress significantly changes.
252 *
253 * @param listener listener to be notified of events.
254 * @throws LockedException if robust estimator is locked.
255 */
256 public void setListener(final Point3DRobustEstimatorListener listener) throws LockedException {
257 if (isLocked()) {
258 throw new LockedException();
259 }
260 this.listener = listener;
261 }
262
263 /**
264 * Indicates whether listener has been provided and is available for
265 * retrieval.
266 *
267 * @return true if available, false otherwise.
268 */
269 public boolean isListenerAvailable() {
270 return listener != null;
271 }
272
273 /**
274 * Indicates if this instance is locked because estimation is being computed
275 *
276 * @return true if locked, false otherwise.
277 */
278 public boolean isLocked() {
279 return locked;
280 }
281
282 /**
283 * Returns amount of progress variation before notifying a progress change
284 * during estimation.
285 *
286 * @return amount of progress variation before notifying a progress change
287 * during estimation.
288 */
289 public float getProgressDelta() {
290 return progressDelta;
291 }
292
293 /**
294 * Sets amount of progress variation before notifying a progress change
295 * during estimation.
296 *
297 * @param progressDelta amount of progress variation before notifying a
298 * progress change during estimation.
299 * @throws IllegalArgumentException if progress delta is less than zero or
300 * greater than 1.
301 * @throws LockedException if this estimator is locked because an estimation
302 * is being computed.
303 */
304 public void setProgressDelta(final float progressDelta) throws LockedException {
305 if (isLocked()) {
306 throw new LockedException();
307 }
308 if (progressDelta < MIN_PROGRESS_DELTA || progressDelta > MAX_PROGRESS_DELTA) {
309 throw new IllegalArgumentException();
310 }
311 this.progressDelta = progressDelta;
312 }
313
314 /**
315 * Returns amount of confidence expressed as a value between 0.0 and 1.0
316 * (which is equivalent to 100%). The amount of confidence indicates the
317 * probability that the estimated result is correct. Usually this value will
318 * be close to 1.0, but not exactly 1.0.
319 *
320 * @return amount of confidence as a value between 0.0 and 1.0.
321 */
322 public double getConfidence() {
323 return confidence;
324 }
325
326 /**
327 * Sets amount of confidence expressed as a value between 0.0 and 1.0 (which
328 * is equivalent to 100%). The amount of confidence indicates the
329 * probability that the estimated result is correct. Usually this value will
330 * be close to 1.0, but not exactly 1.0.
331 *
332 * @param confidence confidence to be set as a value between 0.0 and 1.0.
333 * @throws IllegalArgumentException if provided value is not between 0.0 and
334 * 1.0.
335 * @throws LockedException if this estimator is locked because an estimator
336 * is being computed.
337 */
338 public void setConfidence(final double confidence) throws LockedException {
339 if (isLocked()) {
340 throw new LockedException();
341 }
342 if (confidence < MIN_CONFIDENCE || confidence > MAX_CONFIDENCE) {
343 throw new IllegalArgumentException();
344 }
345 this.confidence = confidence;
346 }
347
348 /**
349 * Returns maximum allowed number of iterations. If maximum allowed number
350 * of iterations is achieved without converging to a result when calling
351 * estimate(), a RobustEstimatorException will be raised.
352 *
353 * @return maximum allowed number of iterations.
354 */
355 public int getMaxIterations() {
356 return maxIterations;
357 }
358
359 /**
360 * Sets maximum allowed number of iterations. When the maximum number of
361 * iterations is exceeded, result will not be available, however an
362 * approximate result will be available for retrieval.
363 *
364 * @param maxIterations maximum allowed number of iterations to be set.
365 * @throws IllegalArgumentException if provided value is less than 1.
366 * @throws LockedException if this estimator is locked because an estimation
367 * is being computed.
368 */
369 public void setMaxIterations(final int maxIterations) throws LockedException {
370 if (isLocked()) {
371 throw new LockedException();
372 }
373 if (maxIterations < MIN_ITERATIONS) {
374 throw new IllegalArgumentException();
375 }
376 this.maxIterations = maxIterations;
377 }
378
379 /**
380 * Gets data related to inliers found after estimation.
381 *
382 * @return data related to inliers found after estimation.
383 */
384 public InliersData getInliersData() {
385 return inliersData;
386 }
387
388 /**
389 * Indicates whether result must be refined using Levenberg-Marquardt
390 * fitting algorithm over found inliers.
391 * If true, inliers will be computed and kept in any implementation
392 * regardless of the settings.
393 *
394 * @return true to refine result, false to simply use result found by
395 * robust estimator without further refining.
396 */
397 public boolean isResultRefined() {
398 return refineResult;
399 }
400
401 /**
402 * Specifies whether result must be refined using Levenberg-Marquardt
403 * fitting algorithm over found inliers.
404 *
405 * @param refineResult true to refine result, false to simply use result
406 * found by robust estimator without further refining.
407 * @throws LockedException if estimator is locked.
408 */
409 public void setResultRefined(final boolean refineResult) throws LockedException {
410 if (isLocked()) {
411 throw new LockedException();
412 }
413 this.refineResult = refineResult;
414 }
415
416 /**
417 * Gets coordinates type to use for refinement. When using inhomogeneous
418 * coordinates a 3x3 covariance matrix is estimated. When using homogeneous
419 * coordinates a 4x4 covariance matrix is estimated.
420 *
421 * @return coordinates type to use for refinement.
422 */
423 public CoordinatesType getRefinementCoordinatesType() {
424 return refinementCoordinatesType;
425 }
426
427 /**
428 * Sets coordinates type to use for refinement. When using inhomogeneous
429 * coordinates a 3x3 covariance matrix is estimated. When using homogeneous
430 * coordinates a 4x4 covariance matrix is estimated.
431 *
432 * @param refinementCoordinatesType coordinates type to use for refinement.
433 * @throws LockedException if estimator is locked.
434 */
435 public void setRefinementCoordinatesType(final CoordinatesType refinementCoordinatesType) throws LockedException {
436 if (isLocked()) {
437 throw new LockedException();
438 }
439 this.refinementCoordinatesType = refinementCoordinatesType;
440 }
441
442 /**
443 * Indicates whether covariance must be kept after refining result.
444 * This setting is only taken into account if result is refined.
445 *
446 * @return true if covariance must be kept after refining result, false
447 * otherwise.
448 */
449 public boolean isCovarianceKept() {
450 return keepCovariance;
451 }
452
453 /**
454 * Specifies whether covariance must be kept after refining result.
455 * This setting is only taken into account if result is refined.
456 *
457 * @param keepCovariance true if covariance must be kept after refining
458 * result, false otherwise.
459 * @throws LockedException if estimator is locked.
460 */
461 public void setCovarianceKept(final boolean keepCovariance) throws LockedException {
462 if (isLocked()) {
463 throw new LockedException();
464 }
465 this.keepCovariance = keepCovariance;
466 }
467
468 /**
469 * Returns list of planes to be used to estimate a 3D point.
470 * Provided list must have a size greater or equal than MINIMUM_SIZE.
471 *
472 * @return list of planes to be used to estimate a 3D point.
473 */
474 public List<Plane> getPlanes() {
475 return planes;
476 }
477
478 /**
479 * Sets list of planes to be used to estimate a 3D point.
480 * Provided list must have a size greater or equal than MINIMUM_SIZE.
481 *
482 * @param planes list of planes to be used to estimate a 3D point.
483 * @throws IllegalArgumentException if provided list of planes doesn't have
484 * a size greater or equal than MINIMUM_SIZE.
485 * @throws LockedException if estimator is locked because a computation is
486 * already in progress.
487 */
488 public void setPlanes(final List<Plane> planes) throws LockedException {
489 if (isLocked()) {
490 throw new LockedException();
491 }
492 internalSetPlanes(planes);
493 }
494
495 /**
496 * Indicates if estimator is ready to start the 3D point estimation.
497 * This is true when a minimum if MINIMUM_SIZE lines are available.
498 *
499 * @return true if estimator is ready, false otherwise.
500 */
501 public boolean isReady() {
502 return planes != null && planes.size() >= MINIMUM_SIZE;
503 }
504
505 /**
506 * Returns quality scores corresponding to each line.
507 * The larger the score value the better the quality of the line measure.
508 * This implementation always returns null.
509 * Subclasses using quality scores must implement proper behaviour.
510 *
511 * @return quality scores corresponding to each point.
512 */
513 public double[] getQualityScores() {
514 return null;
515 }
516
517 /**
518 * Sets quality scores corresponding to each line.
519 * The larger the score value the better the quality of the line measure.
520 * This implementation makes no action.
521 * Subclasses using quality scores must implement proper behaviour.
522 *
523 * @param qualityScores quality scores corresponding to each sampled line.
524 * @throws LockedException if robust estimator is locked because an
525 * estimation is already in progress.
526 * @throws IllegalArgumentException if provided quality scores length is
527 * smaller than MINIMUM_SIZE (i.e. 3 samples).
528 */
529 public void setQualityScores(final double[] qualityScores) throws LockedException {
530 }
531
532 /**
533 * Gets estimated covariance of estimated 3D point if available.
534 * This is only available when result has been refined and covariance is
535 * kept.
536 *
537 * @return estimated covariance or null.
538 */
539 public Matrix getCovariance() {
540 return covariance;
541 }
542
543 /**
544 * Creates a 3D point robust estimator based on 3D line samples and using
545 * provided robust estimator method.
546 *
547 * @param method method of a robust estimator algorithm to estimate the best
548 * 3D point.
549 * @return an instance of a 3D point robust estimator.
550 */
551 public static Point3DRobustEstimator create(final RobustEstimatorMethod method) {
552 return switch (method) {
553 case LMEDS -> new LMedSPoint3DRobustEstimator();
554 case MSAC -> new MSACPoint3DRobustEstimator();
555 case PROSAC -> new PROSACPoint3DRobustEstimator();
556 case PROMEDS -> new PROMedSPoint3DRobustEstimator();
557 default -> new RANSACPoint3DRobustEstimator();
558 };
559 }
560
561 /**
562 * Creates a 3D point robust estimator based on 3D plane samples and using
563 * provided planes and robust estimator method.
564 *
565 * @param planes 3D planes to estimate a 3D point.
566 * @param method method of a robust estimator algorithm to estimate the best
567 * 3D point.
568 * @return an instance of a 3D point robust estimator.
569 * @throws IllegalArgumentException if provided list of lines don't have a
570 * size greater or equal than MINIMUM_SIZE.
571 */
572 public static Point3DRobustEstimator create(final List<Plane> planes, final RobustEstimatorMethod method) {
573 return switch (method) {
574 case LMEDS -> new LMedSPoint3DRobustEstimator(planes);
575 case MSAC -> new MSACPoint3DRobustEstimator(planes);
576 case PROSAC -> new PROSACPoint3DRobustEstimator(planes);
577 case PROMEDS -> new PROMedSPoint3DRobustEstimator(planes);
578 default -> new RANSACPoint3DRobustEstimator(planes);
579 };
580 }
581
582 /**
583 * Creates a 3D point robust estimator based on 3D plane samples and using
584 * provided listener.
585 *
586 * @param listener listener to be notified of events such as when estimation
587 * starts, ends or its progress significantly changes.
588 * @param method method of a robust estimator algorithm to estimate the best
589 * 3D point.
590 * @return an instance of a 3D point robust estimator.
591 */
592 public static Point3DRobustEstimator create(
593 final Point3DRobustEstimatorListener listener, final RobustEstimatorMethod method) {
594 return switch (method) {
595 case LMEDS -> new LMedSPoint3DRobustEstimator(listener);
596 case MSAC -> new MSACPoint3DRobustEstimator(listener);
597 case PROSAC -> new PROSACPoint3DRobustEstimator(listener);
598 case PROMEDS -> new PROMedSPoint3DRobustEstimator(listener);
599 default -> new RANSACPoint3DRobustEstimator(listener);
600 };
601 }
602
603 /**
604 * Creates a 3D point robust estimator based on 3D plane samples and using
605 * provided listener and planes.
606 *
607 * @param listener listener to be notified of events such as when estimation
608 * starts, ends or its progress significantly changes.
609 * @param planes 3D planes to estimate a 3D point.
610 * @param method method of a robust estimator algorithm to estimate the best
611 * 3D point.
612 * @return an instance of a 3D point robust estimator.
613 * @throws IllegalArgumentException if provided list of lines don't have a
614 * size greater or equal than MINIMUM_SIZE.
615 */
616 public static Point3DRobustEstimator create(
617 final Point3DRobustEstimatorListener listener, List<Plane> planes, final RobustEstimatorMethod method) {
618 return switch (method) {
619 case LMEDS -> new LMedSPoint3DRobustEstimator(listener, planes);
620 case MSAC -> new MSACPoint3DRobustEstimator(listener, planes);
621 case PROSAC -> new PROSACPoint3DRobustEstimator(listener, planes);
622 case PROMEDS -> new PROMedSPoint3DRobustEstimator(listener, planes);
623 default -> new RANSACPoint3DRobustEstimator(listener, planes);
624 };
625 }
626
627 /**
628 * Creates a 3D point robust estimator based on 3D plane samples and using
629 * provided robust estimator method.
630 *
631 * @param qualityScores quality scores corresponding to each provided plane.
632 * @param method method of a robust estimator algorithm to estimate the best
633 * 3D point.
634 * @return an instance of a 3D point robust estimator.
635 * @throws IllegalArgumentException if provided quality scores length is
636 * smaller than MINIMUM_SIZE (i.e. 2 lines).
637 */
638 public static Point3DRobustEstimator create(final double[] qualityScores, final RobustEstimatorMethod method) {
639 return switch (method) {
640 case LMEDS -> new LMedSPoint3DRobustEstimator();
641 case MSAC -> new MSACPoint3DRobustEstimator();
642 case PROSAC -> new PROSACPoint3DRobustEstimator(qualityScores);
643 case PROMEDS -> new PROMedSPoint3DRobustEstimator(qualityScores);
644 default -> new RANSACPoint3DRobustEstimator();
645 };
646 }
647
648 /**
649 * Creates a 3D point robust estimator based on 3D plane samples and using
650 * provided planes and robust estimator method.
651 *
652 * @param planes 3D planes to estimate a 3D point.
653 * @param qualityScores quality scores corresponding to each provided plane.
654 * @param method method of a robust estimator algorithm to estimate the best
655 * 3D point.
656 * @return an instance of a 3D point robust estimator.
657 * @throws IllegalArgumentException if provided list of lines doesn't have
658 * the same size as the list of provided quality scores, or it their size
659 * is not greater or equal than MINIMUM_SIZE.
660 */
661 public static Point3DRobustEstimator create(
662 final List<Plane> planes, final double[] qualityScores, final RobustEstimatorMethod method) {
663 return switch (method) {
664 case LMEDS -> new LMedSPoint3DRobustEstimator(planes);
665 case MSAC -> new MSACPoint3DRobustEstimator(planes);
666 case PROSAC -> new PROSACPoint3DRobustEstimator(planes, qualityScores);
667 case PROMEDS -> new PROMedSPoint3DRobustEstimator(planes, qualityScores);
668 default -> new RANSACPoint3DRobustEstimator(planes);
669 };
670 }
671
672 /**
673 * Creates a 3D point robust estimator based on 3D plane samples and using
674 * provided listener.
675 *
676 * @param listener listener to be notified of events such as when estimation
677 * starts, ends or its progress significantly changes.
678 * @param qualityScores quality scores corresponding to each provided plane.
679 * @param method method of a robust estimator algorithm to estimate the best
680 * 3D point.
681 * @return an instance of a 3D point robust estimator.
682 * @throws IllegalArgumentException if provided quality scores length is
683 * smaller than MINIMUM_SIZE (i.e. 2 lines).
684 */
685 public static Point3DRobustEstimator create(
686 final Point3DRobustEstimatorListener listener, final double[] qualityScores,
687 final RobustEstimatorMethod method) {
688 return switch (method) {
689 case LMEDS -> new LMedSPoint3DRobustEstimator(listener);
690 case MSAC -> new MSACPoint3DRobustEstimator(listener);
691 case PROSAC -> new PROSACPoint3DRobustEstimator(listener, qualityScores);
692 case PROMEDS -> new PROMedSPoint3DRobustEstimator(listener, qualityScores);
693 default -> new RANSACPoint3DRobustEstimator(listener);
694 };
695 }
696
697 /**
698 * Creates a 3D point robust estimator based on 3D plane samples and using
699 * provided listener and planes.
700 *
701 * @param listener listener to be notified of events such as when estimation
702 * starts, ends or its progress significantly changes.
703 * @param planes 3D planes to estimate a 3D point.
704 * @param qualityScores quality scores corresponding to each provided point.
705 * @param method method of a robust estimator algorithm to estimate the best
706 * 3D point.
707 * @return an instance of a 3D point robust estimator.
708 * @throws IllegalArgumentException if provided list of planes doesn't have
709 * the same size as the list of provided quality scores, or it their size
710 * is not greater or equal than MINIMUM_SIZE.
711 */
712 public static Point3DRobustEstimator create(
713 final Point3DRobustEstimatorListener listener, final List<Plane> planes, final double[] qualityScores,
714 final RobustEstimatorMethod method) {
715 return switch (method) {
716 case LMEDS -> new LMedSPoint3DRobustEstimator(listener, planes);
717 case MSAC -> new MSACPoint3DRobustEstimator(listener, planes);
718 case PROSAC -> new PROSACPoint3DRobustEstimator(listener, planes, qualityScores);
719 case PROMEDS -> new PROMedSPoint3DRobustEstimator(listener, planes, qualityScores);
720 default -> new RANSACPoint3DRobustEstimator(listener, planes);
721 };
722 }
723
724 /**
725 * Creates a 3D point robust estimator based on 3D plane samples and using
726 * default robust estimator method.
727 *
728 * @return an instance of a 3D point robust estimator.
729 */
730 public static Point3DRobustEstimator create() {
731 return create(DEFAULT_ROBUST_METHOD);
732 }
733
734 /**
735 * Creates a 3D point robust estimator based on 3D plane samples and using
736 * provided planes and default robust estimator method.
737 *
738 * @param planes 3D planes to estimate a 3D point.
739 * @return an instance of a 3D point robust estimator.
740 * @throws IllegalArgumentException if provided list of lines doesn't have a
741 * size greater or equal than MINIMUM_SIZE.
742 */
743 public static Point3DRobustEstimator create(final List<Plane> planes) {
744 return create(planes, DEFAULT_ROBUST_METHOD);
745 }
746
747 /**
748 * Creates a 3D point robust estimator based on 3D plane samples and using
749 * provided listener and default robust estimator method.
750 *
751 * @param listener listener to be notified of events such as when estimation
752 * starts, ends or its progress significantly changes.
753 * @return an instance of a 3D point robust estimator.
754 */
755 public static Point3DRobustEstimator create(final Point3DRobustEstimatorListener listener) {
756 return create(listener, DEFAULT_ROBUST_METHOD);
757 }
758
759 /**
760 * Creates a 3D point robust estimator based on 3D plane samples and using
761 * provided listener and planes and default robust estimator method.
762 *
763 * @param listener listener to be notified of events such as when estimation
764 * starts, ends or its progress significantly changes.
765 * @param planes 3D planes to estimate a point.
766 * @return an instance of a 3D point robust estimator.
767 * @throws IllegalArgumentException if provided list of lines don't have a
768 * size greater or equal than MINIMUM_SIZE.
769 */
770 public static Point3DRobustEstimator create(
771 final Point3DRobustEstimatorListener listener, final List<Plane> planes) {
772 return create(listener, planes, DEFAULT_ROBUST_METHOD);
773 }
774
775 /**
776 * Creates a 3D point robust estimator based on 3D plane samples and using
777 * default robust estimator method.
778 *
779 * @param qualityScores quality scores corresponding to each provided plane
780 * @return an instance of a 3D point robust estimator.
781 * @throws IllegalArgumentException if provided quality scores length is
782 * smaller than MINIMUM_SIZE (i.e. 3 planes).
783 */
784 public static Point3DRobustEstimator create(final double[] qualityScores) {
785 return create(qualityScores, DEFAULT_ROBUST_METHOD);
786 }
787
788 /**
789 * Creates a 3D point robust estimator based on 3D plane samples and using
790 * provided planes and default estimator method.
791 *
792 * @param planes 3D planes to estimate a 3D point.
793 * @param qualityScores quality scores corresponding to each provided plane.
794 * @return an instance of a 3D point robust estimator.
795 * @throws IllegalArgumentException if provided list of planes doesn't have
796 * the same size as the list of provided quality scores, or if their size
797 * is not greater or equal than MINIMUM_SIZE.
798 */
799 public static Point3DRobustEstimator create(final List<Plane> planes, final double[] qualityScores) {
800 return create(planes, qualityScores, DEFAULT_ROBUST_METHOD);
801 }
802
803 /**
804 * Creates a 3D point robust estimator based on 3D plane samples and using
805 * provided listener and default estimator method.
806 *
807 * @param listener listener to be notified of events such as when estimation
808 * starts, ends or its progress significantly changes.
809 * @param qualityScores quality scores corresponding to each provided plane
810 * @return an instance of a circle robust estimator.
811 * @throws IllegalArgumentException if provided quality scores length is
812 * smaller than MINIMUM_SIZE (i.e. 3 planes).
813 */
814 public static Point3DRobustEstimator create(
815 final Point3DRobustEstimatorListener listener, final double[] qualityScores) {
816 return create(listener, qualityScores, DEFAULT_ROBUST_METHOD);
817 }
818
819 /**
820 * Creates a 3D point robust estimator based on 3D plane samples and using
821 * provided listener and planes and default estimator method.
822 *
823 * @param listener listener to be notified of events such as when estimation
824 * starts, ends or its progress significantly changes.
825 * @param planes 3D planes to estimate a 3D point.
826 * @param qualityScores quality scores corresponding to each provided plane
827 * @return an instance of a 3D point robust estimator.
828 * @throws IllegalArgumentException if provided list of lines don't have
829 * the same size as the list of provided quality scores, or if their size
830 * is not greater or equal than MINIMUM_SIZE.
831 */
832 public static Point3DRobustEstimator create(
833 final Point3DRobustEstimatorListener listener, final List<Plane> planes, final double[] qualityScores) {
834 return create(listener, planes, qualityScores, DEFAULT_ROBUST_METHOD);
835 }
836
837 /**
838 * Estimates a 3D point using a robust estimator and the best set of 3D
839 * planes that intersect into the estimated 3D point.
840 *
841 * @return a 3D point.
842 * @throws LockedException if robust estimator is locked because an
843 * estimation is already in progress.
844 * @throws NotReadyException if provided input data is not enough to start
845 * the estimation.
846 * @throws RobustEstimatorException if estimation fails for any reason
847 * (i.e. numerical instability, no solution available, etc).
848 */
849 public abstract Point3D estimate() throws LockedException, NotReadyException, RobustEstimatorException;
850
851 /**
852 * Returns method being used for robust estimation.
853 *
854 * @return method being used for robust estimation.
855 */
856 public abstract RobustEstimatorMethod getMethod();
857
858 /**
859 * Computes the residual between a 3D point and a plane.
860 *
861 * @param p a 3D point.
862 * @param plane a 3D plane.
863 * @return residual.
864 */
865 protected double residual(final Point3D p, final Plane plane) {
866 p.normalize();
867 plane.normalize();
868
869 return Math.abs(plane.signedDistance(p));
870 }
871
872 /**
873 * Attempts to refine provided solution if refinement is requested.
874 * This method returns a refined solution or the same provided solution
875 * if refinement is not requested or has failed.
876 * If refinement is enabled, and it is requested to keep covariance, this
877 * method will also keep covariance of refined point.
878 *
879 * @param point point estimated by a robust estimator without refinement.
880 * @return solution after refinement (if requested) or the provided non-refined
881 * solution if not requested or if refinement failed.
882 */
883 protected Point3D attemptRefine(final Point3D point) {
884 if (refineResult) {
885 try {
886 Point3DRefiner<? extends Point3D> refiner;
887 Point3D result;
888 final boolean improved;
889 switch (refinementCoordinatesType) {
890 case HOMOGENEOUS_COORDINATES:
891 HomogeneousPoint3D homP;
892 if (point.getType() == CoordinatesType.HOMOGENEOUS_COORDINATES) {
893 homP = (HomogeneousPoint3D) point;
894 } else {
895 homP = new HomogeneousPoint3D(point);
896 }
897 final HomogeneousPoint3DRefiner homRefiner = new HomogeneousPoint3DRefiner(homP, keepCovariance,
898 getInliersData(), planes, getRefinementStandardDeviation());
899 refiner = homRefiner;
900 final var homResult = new HomogeneousPoint3D();
901 improved = homRefiner.refine(homResult);
902 result = homResult;
903 break;
904
905 case INHOMOGENEOUS_COORDINATES:
906 default:
907 InhomogeneousPoint3D inhomP;
908 if (point.getType() == CoordinatesType.INHOMOGENEOUS_COORDINATES) {
909 inhomP = (InhomogeneousPoint3D) point;
910 } else {
911 inhomP = new InhomogeneousPoint3D(point);
912 }
913 final InhomogeneousPoint3DRefiner inhomRefiner = new InhomogeneousPoint3DRefiner(inhomP,
914 keepCovariance, getInliersData(), planes, getRefinementStandardDeviation());
915 refiner = inhomRefiner;
916 final var inhomResult = new InhomogeneousPoint3D();
917 improved = inhomRefiner.refine(inhomResult);
918 result = inhomResult;
919 break;
920 }
921
922 if (keepCovariance) {
923 // keep covariance
924 covariance = refiner.getCovariance();
925 }
926
927 return improved ? result : point;
928 } catch (final Exception e) {
929 // refinement failed, so we return input value
930 return point;
931 }
932 } else {
933 return point;
934 }
935 }
936
937 /**
938 * Gets standard deviation used for Levenberg-Marquardt fitting during
939 * refinement.
940 * Returned value gives an indication of how much variance each residual
941 * has.
942 * Typically, this value is related to the threshold used on each robust
943 * estimation, since residuals of found inliers are within the range of
944 * such threshold.
945 *
946 * @return standard deviation used for refinement.
947 */
948 protected abstract double getRefinementStandardDeviation();
949
950 /**
951 * Internal method to set list of 3D planes to be used to estimate a 3D
952 * point.
953 * This method does not check whether estimator is locked or not.
954 *
955 * @param planes list of planes to be used to estimate a 3D point.
956 * @throws IllegalArgumentException if provided list of planes doesn't have
957 * a size greater or equal than MINIMUM_SIZE.
958 */
959 private void internalSetPlanes(final List<Plane> planes) {
960 if (planes.size() < MINIMUM_SIZE) {
961 throw new IllegalArgumentException();
962 }
963 this.planes = planes;
964 }
965 }