1 /*
2 * Copyright (C) 2015 Alberto Irurueta Carro (alberto@irurueta.com)
3 *
4 * Licensed under the Apache License, Version 2.0 (the "License");
5 * you may not use this file except in compliance with the License.
6 * You may obtain a copy of the License at
7 *
8 * http://www.apache.org/licenses/LICENSE-2.0
9 *
10 * Unless required by applicable law or agreed to in writing, software
11 * distributed under the License is distributed on an "AS IS" BASIS,
12 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13 * See the License for the specific language governing permissions and
14 * limitations under the License.
15 */
16 package com.irurueta.geometry.estimators;
17
18 import com.irurueta.geometry.AffineTransformation3D;
19 import com.irurueta.geometry.Plane;
20 import com.irurueta.geometry.refiners.PlaneCorrespondenceAffineTransformation3DRefiner;
21 import com.irurueta.numerical.robust.RobustEstimatorMethod;
22
23 import java.util.List;
24
25 /**
26 * This is an abstract class for algorithms to robustly find the best affine
27 * 3D transformation for collections of matching planes.
28 * Implementations of this class should be able to detect and discard outliers
29 * in order to find the best solution.
30 */
31 public abstract class PlaneCorrespondenceAffineTransformation3DRobustEstimator
32 extends AffineTransformation3DRobustEstimator {
33
34 /**
35 * Default robust estimator method when none is provided.
36 */
37 public static final RobustEstimatorMethod DEFAULT_ROBUST_METHOD = RobustEstimatorMethod.PROMEDS;
38
39 /**
40 * List of planes to be used to estimate an affine 3D transformation.
41 * Each line in the list of input lines must be matched with the
42 * corresponding line in the list of output lines located at the same
43 * position. Hence, both input lines and output lines must have the
44 * same size, and their size must be greater or equal than MINIMUM_SIZE.
45 */
46 protected List<Plane> inputPlanes;
47
48 /**
49 * List of planes to be used to estimate an affine 3D transformation.
50 * Each point in the list of output lines must be matched with the
51 * corresponding line in the list of input lines located at the same
52 * position. Hence, both input lines and output lines must have the
53 * same size, and their size must be greater or equal than MINIMUM_SIZE.
54 */
55 protected List<Plane> outputPlanes;
56
57 /**
58 * Constructor.
59 */
60 protected PlaneCorrespondenceAffineTransformation3DRobustEstimator() {
61 super();
62 }
63
64 /**
65 * Constructor with lists of planes to be used to estimate an affine 3D
66 * transformation.
67 * Planes in the list located at the same position are considered to be
68 * matched. Hence, both lists must have the same size, and their size must
69 * be greater or equal than MINIMUM_SIZE.
70 *
71 * @param inputPlanes list of input planes to be used to estimate an affine
72 * 3D transformation.
73 * @param outputPlanes list of output planes ot be used to estimate an
74 * affine 3D transformation.
75 * @throws IllegalArgumentException if provided lists of planes don't have
76 * the same size or their size is smaller than MINIMUM_SIZE.
77 */
78 protected PlaneCorrespondenceAffineTransformation3DRobustEstimator(
79 final List<Plane> inputPlanes, final List<Plane> outputPlanes) {
80 super();
81 internalSetPlanes(inputPlanes, outputPlanes);
82 }
83
84 /**
85 * Constructor.
86 *
87 * @param listener listener to be notified of events such as when estimation
88 * stars, ends or its progress significantly changes.
89 */
90 protected PlaneCorrespondenceAffineTransformation3DRobustEstimator(
91 final AffineTransformation3DRobustEstimatorListener listener) {
92 super(listener);
93 }
94
95 /**
96 * Constructor with listener and lists of planes to be used to estimate
97 * affine 3D transformation.
98 * Planes in the list located at the same position are considered to be
99 * matched. Hence, both lists must have the same size, and their size must
100 * be greater or equal than MINIMUM_SIZE.
101 *
102 * @param listener listener to be notified of events such as when estimation
103 * starts, ends or its progress significantly changes.
104 * @param inputPlanes list of input planes to be used to estimate an affine
105 * 3D transformation.
106 * @param outputPlanes list of output planes to be used to estimate an
107 * affine 3D transformation.
108 * @throws IllegalArgumentException if provided lists of planes don't have
109 * the same size or their size is smaller than MINIMUM_SIZE.
110 */
111 protected PlaneCorrespondenceAffineTransformation3DRobustEstimator(
112 final AffineTransformation3DRobustEstimatorListener listener,
113 final List<Plane> inputPlanes, final List<Plane> outputPlanes) {
114 super(listener);
115 internalSetPlanes(inputPlanes, outputPlanes);
116 }
117
118 /**
119 * Returns list of input planes to be used to estimate an affine 3D
120 * transformation.
121 * Each plane in the list of input planes must be matched with the
122 * corresponding planes in the list of output planes located at the same
123 * position. Hence, both input planes and output planes must have the same
124 * size, and their size must be greater or equal than MINIMUM_SIZE.
125 *
126 * @return list of input planes to be used to estimate an affine 3D
127 * transformation.
128 */
129 public List<Plane> getInputPlanes() {
130 return inputPlanes;
131 }
132
133 /**
134 * Returns list of output planes to be used to estimate an affine 3D
135 * transformation.
136 * Each plane in the list of output planes must be matched with the
137 * corresponding plane in the list of input planes located at the same
138 * position. Hence, both input planes and output planes must have the same
139 * size, and their size must be greater or equal than MINIMUM_SIZE.
140 *
141 * @return list of output planes to be used to estimate an affine 3D
142 * transformation.
143 */
144 public List<Plane> getOutputPlanes() {
145 return outputPlanes;
146 }
147
148 /**
149 * Sets lists of planes to be used to estimate an affine 3D transformation.
150 * Planes in the list located at the same position are considered to be
151 * matched. Hence, both lists must have the same size, and their size must
152 * be greater or equal than MINIMUM_SIZE.
153 *
154 * @param inputPlanes list of input planes to be used to estimate an affine
155 * 3D transformation.
156 * @param outputPlanes list of output planes to be used to estimate an
157 * affine 3D transformation.
158 * @throws IllegalArgumentException if provided lists of planes don't have
159 * the same size or their size is smaller than MINIMUM_SIZE.
160 * @throws LockedException if estimator is locked because a computation is
161 * already in progress.
162 */
163 public final void setPlanes(final List<Plane> inputPlanes, final List<Plane> outputPlanes) throws LockedException {
164 if (isLocked()) {
165 throw new LockedException();
166 }
167 internalSetPlanes(inputPlanes, outputPlanes);
168 }
169
170 /**
171 * Indicates if estimator is ready to start the affine 3D transformation
172 * estimation.
173 * This is true when input data (i.e. lists of matched planes) are provided
174 * and a minimum of MINIMUM_SIZE lines are available.
175 *
176 * @return true if estimator is ready, false otherwise.
177 */
178 public boolean isReady() {
179 return inputPlanes != null && outputPlanes != null && inputPlanes.size() == outputPlanes.size()
180 && inputPlanes.size() >= MINIMUM_SIZE;
181 }
182
183 /**
184 * Returns quality scores corresponding to each pair of matched planes.
185 * The larger the score value the better the quality of the matching.
186 * This implementation always returns null.
187 * Subclasses using quality scores must implement proper behaviour.
188 *
189 * @return quality scores corresponding to each pair of matched points.
190 */
191 public double[] getQualityScores() {
192 return null;
193 }
194
195 /**
196 * Sets quality scores corresponding to each pair of matched planes.
197 * The larger the score value the better the quality of the matching.
198 * This implementation makes no action.
199 * Subclasses using quality scores must implement proper behaviour.
200 *
201 * @param qualityScores quality scores corresponding to each pair of matched
202 * points.
203 * @throws LockedException if robust estimator is locked because an
204 * estimation is already in progress.
205 * @throws IllegalArgumentException if provided quality scores length is
206 * smaller than MINIMUM_SIZE (i.e. 3 samples).
207 */
208 public void setQualityScores(final double[] qualityScores) throws LockedException {
209 }
210
211 /**
212 * Creates an affine 3D transformation estimator based on 3D plane
213 * correspondences and using provided robust estimator method.
214 *
215 * @param method method of a robust estimator algorithm to estimate
216 * the best affine 3D transformation.
217 * @return an instance of affine 3D transformation estimator.
218 */
219 public static PlaneCorrespondenceAffineTransformation3DRobustEstimator create(final RobustEstimatorMethod method) {
220 return switch (method) {
221 case LMEDS -> new LMedSPlaneCorrespondenceAffineTransformation3DRobustEstimator();
222 case MSAC -> new MSACPlaneCorrespondenceAffineTransformation3DRobustEstimator();
223 case PROSAC -> new PROSACPlaneCorrespondenceAffineTransformation3DRobustEstimator();
224 case PROMEDS -> new PROMedSPlaneCorrespondenceAffineTransformation3DRobustEstimator();
225 default -> new RANSACPlaneCorrespondenceAffineTransformation3DRobustEstimator();
226 };
227 }
228
229 /**
230 * Creates an affine 3D transformation estimator based on 3D plane
231 * correspondences and using provided robust estimator method.
232 *
233 * @param inputPlanes list of input planes to be used to estimate an
234 * affine 3D transformation.
235 * @param outputPlanes list of output planes to be used to estimate an
236 * affine 3D transformation.
237 * @param method method of a robust estimator algorithm to estimate
238 * the best affine 3D transformation.
239 * @return an instance of affine 3D transformation estimator.
240 * @throws IllegalArgumentException if provided lists of lines don't have
241 * the same size or their size is smaller than MINIMUM_SIZE.
242 */
243 public static PlaneCorrespondenceAffineTransformation3DRobustEstimator create(
244 final List<Plane> inputPlanes, final List<Plane> outputPlanes, final RobustEstimatorMethod method) {
245 return switch (method) {
246 case LMEDS -> new LMedSPlaneCorrespondenceAffineTransformation3DRobustEstimator(inputPlanes, outputPlanes);
247 case MSAC -> new MSACPlaneCorrespondenceAffineTransformation3DRobustEstimator(inputPlanes, outputPlanes);
248 case PROSAC -> new PROSACPlaneCorrespondenceAffineTransformation3DRobustEstimator(
249 inputPlanes, outputPlanes);
250 case PROMEDS -> new PROMedSPlaneCorrespondenceAffineTransformation3DRobustEstimator(
251 inputPlanes, outputPlanes);
252 default -> new RANSACPlaneCorrespondenceAffineTransformation3DRobustEstimator(inputPlanes, outputPlanes);
253 };
254 }
255
256 /**
257 * Creates an affine 3D transformation estimator based on 3D plane
258 * correspondences and using provided robust estimator method.
259 *
260 * @param listener listener to be notified of events such as when estimation
261 * starts, ends or its progress significantly changes.
262 * @param method method of a robust estimator algorithm to estimate the best
263 * affine 3D transformation.
264 * @return an instance of affine 3D transformation estimator.
265 */
266 public static PlaneCorrespondenceAffineTransformation3DRobustEstimator create(
267 final AffineTransformation3DRobustEstimatorListener listener, final RobustEstimatorMethod method) {
268 return switch (method) {
269 case LMEDS -> new LMedSPlaneCorrespondenceAffineTransformation3DRobustEstimator(listener);
270 case MSAC -> new MSACPlaneCorrespondenceAffineTransformation3DRobustEstimator(listener);
271 case PROSAC -> new PROSACPlaneCorrespondenceAffineTransformation3DRobustEstimator(listener);
272 case PROMEDS -> new PROMedSPlaneCorrespondenceAffineTransformation3DRobustEstimator(listener);
273 default -> new RANSACPlaneCorrespondenceAffineTransformation3DRobustEstimator(listener);
274 };
275 }
276
277 /**
278 * Creates an affine 3D transformation estimator based on 3D line
279 * correspondences and using provided robust estimator method.
280 *
281 * @param listener listener to be notified of events such as when estimation
282 * starts, ends or its progress significantly changes.
283 * @param inputPlanes list of input lines to be used to estimate an affine
284 * 3D transformation.
285 * @param outputPlanes list of output lines to be used to estimate an affine
286 * 3D transformation.
287 * @param method method of a robust estimator algorithm to estimate best
288 * affine 3D transformation.
289 * @return an instance of affine 3D transformation estimator.
290 * @throws IllegalArgumentException if provided lists of lines don't have
291 * the same size or their size is smaller than MINIMUM_SIZE.
292 */
293 public static PlaneCorrespondenceAffineTransformation3DRobustEstimator create(
294 final AffineTransformation3DRobustEstimatorListener listener,
295 final List<Plane> inputPlanes, final List<Plane> outputPlanes, final RobustEstimatorMethod method) {
296 return switch (method) {
297 case LMEDS -> new LMedSPlaneCorrespondenceAffineTransformation3DRobustEstimator(
298 listener, inputPlanes, outputPlanes);
299 case MSAC -> new MSACPlaneCorrespondenceAffineTransformation3DRobustEstimator(
300 listener, inputPlanes, outputPlanes);
301 case PROSAC -> new PROSACPlaneCorrespondenceAffineTransformation3DRobustEstimator(
302 listener, inputPlanes, outputPlanes);
303 case PROMEDS -> new PROMedSPlaneCorrespondenceAffineTransformation3DRobustEstimator(
304 listener, inputPlanes, outputPlanes);
305 default -> new RANSACPlaneCorrespondenceAffineTransformation3DRobustEstimator(
306 listener, inputPlanes, outputPlanes);
307 };
308 }
309
310 /**
311 * Creates an affine 3D transformation estimator based on 3D plane
312 * correspondences and using provided robust estimator method.
313 *
314 * @param qualityScores quality scores corresponding to each pair of matched
315 * planes.
316 * @param method method of a robust estimator algorithm to estimate the best
317 * affine 3D transformation.
318 * @return an instance of affine 3D transformation estimator.
319 */
320 public static PlaneCorrespondenceAffineTransformation3DRobustEstimator create(
321 final double[] qualityScores, final RobustEstimatorMethod method) {
322 return switch (method) {
323 case LMEDS -> new LMedSPlaneCorrespondenceAffineTransformation3DRobustEstimator();
324 case MSAC -> new MSACPlaneCorrespondenceAffineTransformation3DRobustEstimator();
325 case PROSAC -> new PROSACPlaneCorrespondenceAffineTransformation3DRobustEstimator(qualityScores);
326 case PROMEDS -> new PROMedSPlaneCorrespondenceAffineTransformation3DRobustEstimator(qualityScores);
327 default -> new RANSACPlaneCorrespondenceAffineTransformation3DRobustEstimator();
328 };
329 }
330
331 /**
332 * Creates an affine 3D transformation estimator based on plane
333 * correspondences and using provided robust estimator method.
334 *
335 * @param inputPlanes list of input planes to be used to estimate an
336 * affine 3D transformation.
337 * @param outputPlanes list of output planes to be used to estimate an
338 * affine 3D transformation.
339 * @param qualityScores quality scores corresponding to each pair of matched
340 * planes.
341 * @param method method of a robust estimator algorithm to estimate the best
342 * affine 3D transformation.
343 * @return an instance of affine 3D transformation estimator.
344 * @throws IllegalArgumentException if provided lists of lines don't have
345 * the same size or their size is smaller than MINIMUM_SIZE.
346 */
347 public static PlaneCorrespondenceAffineTransformation3DRobustEstimator create(
348 final List<Plane> inputPlanes, final List<Plane> outputPlanes, final double[] qualityScores,
349 final RobustEstimatorMethod method) {
350 return switch (method) {
351 case LMEDS -> new LMedSPlaneCorrespondenceAffineTransformation3DRobustEstimator(inputPlanes, outputPlanes);
352 case MSAC -> new MSACPlaneCorrespondenceAffineTransformation3DRobustEstimator(inputPlanes, outputPlanes);
353 case PROSAC -> new PROSACPlaneCorrespondenceAffineTransformation3DRobustEstimator(
354 inputPlanes, outputPlanes, qualityScores);
355 case PROMEDS -> new PROMedSPlaneCorrespondenceAffineTransformation3DRobustEstimator(
356 inputPlanes, outputPlanes, qualityScores);
357 default -> new RANSACPlaneCorrespondenceAffineTransformation3DRobustEstimator(inputPlanes, outputPlanes);
358 };
359 }
360
361 /**
362 * Creates an affine 3D transformation estimator based on plane
363 * correspondences and using provided robust estimator method.
364 *
365 * @param listener listener to be notified of events such as when estimation
366 * starts, ends or its progress significantly changes.
367 * @param qualityScores quality scores corresponding to each pair of matched
368 * lines.
369 * @param method method of a robust estimator algorithm to estimate the best
370 * affine 3D transformation.
371 * @return an instance of affine 3D transformation estimator.
372 */
373 public static PlaneCorrespondenceAffineTransformation3DRobustEstimator create(
374 final AffineTransformation3DRobustEstimatorListener listener, final double[] qualityScores,
375 final RobustEstimatorMethod method) {
376 return switch (method) {
377 case LMEDS -> new LMedSPlaneCorrespondenceAffineTransformation3DRobustEstimator(listener);
378 case MSAC -> new MSACPlaneCorrespondenceAffineTransformation3DRobustEstimator(listener);
379 case PROSAC -> new PROSACPlaneCorrespondenceAffineTransformation3DRobustEstimator(listener, qualityScores);
380 case PROMEDS -> new PROMedSPlaneCorrespondenceAffineTransformation3DRobustEstimator(
381 listener, qualityScores);
382 default -> new RANSACPlaneCorrespondenceAffineTransformation3DRobustEstimator(listener);
383 };
384 }
385
386 /**
387 * Creates an affine 3D transformation estimator based on plane
388 * correspondences and using provided robust estimator method.
389 *
390 * @param listener listener to be notified of events such as when estimation
391 * starts, ends or its progress significantly changes.
392 * @param inputPlanes list of input planes to be used to estimate an affine
393 * 3D transformation.
394 * @param outputPlanes list of output planes to be used to estimate an affine
395 * 3D transformation.
396 * @param qualityScores quality scores corresponding to each pair of matched
397 * planes.
398 * @param method method of a robust estimator algorithm to estimate the best
399 * affine 3D transformation.
400 * @return an instance of affine 3D transformation estimator.
401 * @throws IllegalArgumentException if provided lists of lines don't have
402 * the same size or their size is smaller than MINIMUM_SIZE.
403 */
404 public static PlaneCorrespondenceAffineTransformation3DRobustEstimator create(
405 final AffineTransformation3DRobustEstimatorListener listener,
406 final List<Plane> inputPlanes, final List<Plane> outputPlanes,
407 final double[] qualityScores, final RobustEstimatorMethod method) {
408 return switch (method) {
409 case LMEDS -> new LMedSPlaneCorrespondenceAffineTransformation3DRobustEstimator(
410 listener, inputPlanes, outputPlanes);
411 case MSAC -> new MSACPlaneCorrespondenceAffineTransformation3DRobustEstimator(
412 listener, inputPlanes, outputPlanes);
413 case PROSAC -> new PROSACPlaneCorrespondenceAffineTransformation3DRobustEstimator(
414 listener, inputPlanes, outputPlanes, qualityScores);
415 case PROMEDS -> new PROMedSPlaneCorrespondenceAffineTransformation3DRobustEstimator(
416 listener, inputPlanes, outputPlanes, qualityScores);
417 default -> new RANSACPlaneCorrespondenceAffineTransformation3DRobustEstimator(
418 listener, inputPlanes, outputPlanes);
419 };
420 }
421
422 /**
423 * Creates an affine 3D transformation estimator based on plane
424 * correspondences and using default robust estimator method.
425 *
426 * @return an instance of affine 3D transformation estimator.
427 */
428 public static PlaneCorrespondenceAffineTransformation3DRobustEstimator create() {
429 return create(DEFAULT_ROBUST_METHOD);
430 }
431
432 /**
433 * Creates an affine 3D transformation estimator based on plane
434 * correspondences and using default robust estimator method.
435 *
436 * @param inputPlanes list of input planes to be used to estimate an
437 * affine 3D transformation.
438 * @param outputPlanes list of output planes to be used to estimate an
439 * affine 3D transformation.
440 * @return an instance of affine 3D transformation estimator.
441 * @throws IllegalArgumentException if provided lists of planes don't have
442 * the same size or their size is smaller than MINIMUM_SIZE.
443 */
444 public static PlaneCorrespondenceAffineTransformation3DRobustEstimator create(
445 final List<Plane> inputPlanes, final List<Plane> outputPlanes) {
446 return create(inputPlanes, outputPlanes, DEFAULT_ROBUST_METHOD);
447 }
448
449 /**
450 * Creates an affine 3D transformation estimator based on plane
451 * correspondences and using default robust estimator method.
452 *
453 * @param listener listener to be notified of events such as when estimation
454 * starts, ends or its progress significantly changes.
455 * @return an instance of affine 3D transformation estimator.
456 */
457 public static PlaneCorrespondenceAffineTransformation3DRobustEstimator create(
458 final AffineTransformation3DRobustEstimatorListener listener) {
459 return create(listener, DEFAULT_ROBUST_METHOD);
460 }
461
462 /**
463 * Creates an affine 3D transformation estimator based on plane
464 * correspondences and using default robust estimator method.
465 *
466 * @param listener listener to be notified of events such as when estimation
467 * starts, ends or its progress significantly changes.
468 * @param inputPlanes list of input planes to be used to estimate an affine
469 * 3D transformation.
470 * @param outputPlanes list of output planes to be used to estimate an
471 * affine 3D transformation.
472 * @return an instance of affine 3D transformation estimator.
473 * @throws IllegalArgumentException if provided lists of lines don't have
474 * the same size or their size is smaller than MINIMUM_SIZE.
475 */
476 public static PlaneCorrespondenceAffineTransformation3DRobustEstimator create(
477 final AffineTransformation3DRobustEstimatorListener listener,
478 final List<Plane> inputPlanes, final List<Plane> outputPlanes) {
479 return create(listener, inputPlanes, outputPlanes, DEFAULT_ROBUST_METHOD);
480 }
481
482 /**
483 * Creates an affine 3D transformation estimator based on plane
484 * correspondences and using default robust estimator method.
485 *
486 * @param qualityScores quality scores corresponding to each pair of matched
487 * planes.
488 * @return an instance of affine 3D transformation estimator.
489 */
490 public static PlaneCorrespondenceAffineTransformation3DRobustEstimator create(final double[] qualityScores) {
491 return create(qualityScores, DEFAULT_ROBUST_METHOD);
492 }
493
494 /**
495 * Creates an affine 3D transformation estimator based on 3D line
496 * correspondences and using default robust estimator method.
497 *
498 * @param inputPlanes list of input planes to be used to estimate an affine
499 * 3D transformation.
500 * @param outputPlanes list of output planes to be used to estimate an affine
501 * 3D transformation.
502 * @param qualityScores quality scores corresponding to each pair of matched
503 * planes.
504 * @return an instance of affine 3D transformation estimator.
505 * @throws IllegalArgumentException if provided lists of lines don't have
506 * the same size or their size is smaller than MINIMUM_SIZE.
507 */
508 public static PlaneCorrespondenceAffineTransformation3DRobustEstimator create(
509 final List<Plane> inputPlanes, final List<Plane> outputPlanes, final double[] qualityScores) {
510 return create(inputPlanes, outputPlanes, qualityScores, DEFAULT_ROBUST_METHOD);
511 }
512
513 /**
514 * Creates an affine 3D transformation estimator based on 3D line
515 * correspondences and using default robust estimator method.
516 *
517 * @param listener listener to be notified of events such as when estimation
518 * starts, ends or its progress significantly changes.
519 * @param qualityScores quality scores corresponding to each pair of matched
520 * points.
521 * @return an instance of affine 3D transformation estimator.
522 */
523 public static PlaneCorrespondenceAffineTransformation3DRobustEstimator create(
524 final AffineTransformation3DRobustEstimatorListener listener, final double[] qualityScores) {
525 return create(listener, qualityScores, DEFAULT_ROBUST_METHOD);
526 }
527
528 /**
529 * Creates an affine 3D transformation estimator based on plane
530 * correspondences and using default robust estimator method.
531 *
532 * @param listener listener to be notified of events such as when estimation
533 * starts, ends or its progress significantly changes.
534 * @param inputPlanes list of input planes to be used to estimate an affine
535 * 3D transformation.
536 * @param outputPlanes list of output planes to be used to estimate an
537 * affine 3D transformation.
538 * @param qualityScores quality scores corresponding to each pair of matched
539 * lines.
540 * @return an instance of affine 3D transformation estimator.
541 * @throws IllegalArgumentException if provided lists of lines don't have
542 * the same size or their size is smaller than MINIMUM_SIZE.
543 */
544 public static PlaneCorrespondenceAffineTransformation3DRobustEstimator create(
545 final AffineTransformation3DRobustEstimatorListener listener, final List<Plane> inputPlanes,
546 final List<Plane> outputPlanes, final double[] qualityScores) {
547 return create(listener, inputPlanes, outputPlanes, qualityScores, DEFAULT_ROBUST_METHOD);
548 }
549
550 /**
551 * Internal method to set lists of planes to be used to estimate an affine
552 * 3D transformation.
553 * This method does not check whether estimator is locked or not.
554 *
555 * @param inputPlanes list of input planes to be used to estimate an affine
556 * 3D transformation.
557 * @param outputPlanes list of output planes to be used to estimate an
558 * affine 3D transformation.
559 * @throws IllegalArgumentException if provided lists of lines don't have
560 * the same size or their size is smaller than MINIMUM_SIZE.
561 */
562 private void internalSetPlanes(final List<Plane> inputPlanes, final List<Plane> outputPlanes) {
563 if (inputPlanes.size() < MINIMUM_SIZE) {
564 throw new IllegalArgumentException();
565 }
566 if (inputPlanes.size() != outputPlanes.size()) {
567 throw new IllegalArgumentException();
568 }
569 this.inputPlanes = inputPlanes;
570 this.outputPlanes = outputPlanes;
571 }
572
573 /**
574 * Computes residual by comparing two lines algebraically by doing the
575 * dot product of their parameters.
576 * A residual of 0 indicates that dot product was 1 or -1 and lines were
577 * equal.
578 * A residual of 1 indicates that dot product was 0 and lines were
579 * orthogonal.
580 * If dot product was -1, then although their director vectors are opposed,
581 * lines are considered equal, since sign changes are not taken into account.
582 *
583 * @param plane originally sampled output plane.
584 * @param transformedPlane estimated output plane obtained after using
585 * estimated transformation.
586 * @return computed residual.
587 */
588 @SuppressWarnings("DuplicatedCode")
589 protected static double getResidual(final Plane plane, final Plane transformedPlane) {
590 plane.normalize();
591 transformedPlane.normalize();
592
593 final var dotProduct = Math.abs(plane.getA() * transformedPlane.getA()
594 + plane.getB() * transformedPlane.getB() + plane.getC() * transformedPlane.getC()
595 + plane.getD() * transformedPlane.getD());
596 return 1.0 - dotProduct;
597 }
598
599 /**
600 * Attempts to refine provided solution if refinement is requested.
601 * This method returns a refined solution of the same provided solution
602 * if refinement is not requested or has failed.
603 * If refinement is enabled, and it is requested to keep covariance, this
604 * method will also keep covariance of refined transformation.
605 *
606 * @param transformation transformation estimated by a robust estimator
607 * without refinement.
608 * @return solution after refinement (if requested) or the provided
609 * non-refined solution if not requested or refinement failed.
610 */
611 @SuppressWarnings("DuplicatedCode")
612 protected AffineTransformation3D attemptRefine(final AffineTransformation3D transformation) {
613 if (refineResult) {
614 final var refiner = new PlaneCorrespondenceAffineTransformation3DRefiner(transformation, keepCovariance,
615 getInliersData(), inputPlanes, outputPlanes, getRefinementStandardDeviation());
616
617 try {
618 final var result = new AffineTransformation3D();
619 final var improved = refiner.refine(result);
620
621 if (keepCovariance) {
622 // keep covariance
623 covariance = refiner.getCovariance();
624 }
625
626 return improved ? result : transformation;
627 } catch (final Exception e) {
628 //final refinement failed, so we return input value
629 return transformation;
630 }
631 } else {
632 return transformation;
633 }
634 }
635 }