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