1 /*
2 * Copyright (C) 2017 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.refiners;
17
18 import com.irurueta.geometry.CameraException;
19 import com.irurueta.geometry.InhomogeneousPoint2D;
20 import com.irurueta.geometry.InhomogeneousPoint3D;
21 import com.irurueta.geometry.NotAvailableException;
22 import com.irurueta.geometry.PinholeCamera;
23 import com.irurueta.geometry.PinholeCameraIntrinsicParameters;
24 import com.irurueta.geometry.Quaternion;
25 import com.irurueta.geometry.estimators.LockedException;
26 import com.irurueta.geometry.estimators.NotReadyException;
27 import com.irurueta.numerical.robust.InliersData;
28
29 import java.util.BitSet;
30 import java.util.List;
31
32 /**
33 * Base class for pinhole camera refiners.
34 * Implementations of this class refine a pinhole camera by taking into account
35 * an initial estimation, inlier point or line matches and their residuals.
36 * This class can be used to find a solution that minimizes error of inliers in
37 * LMSE terms.
38 * Typically, a refiner is used by a robust estimator, however it can also be
39 * useful in some other situations.
40 *
41 * @param <S1> type of matched samples in 1st set.
42 * @param <S2> type of matched samples in 2nd set.
43 */
44 @SuppressWarnings("DuplicatedCode")
45 public abstract class PinholeCameraRefiner<S1, S2> extends PairMatchesAndInliersDataRefiner<PinholeCamera, S1, S2> {
46
47 /**
48 * Default value indicating whether skewness value is suggested or not.
49 * By default, this is disabled.
50 */
51 public static final boolean DEFAULT_SUGGEST_SKEWNESS_VALUE_ENABLED = false;
52
53 /**
54 * Default value of skewness to be suggested when suggestion is enabled.
55 * By default suggested skewness is zero.
56 */
57 public static final double DEFAULT_SUGGESTED_SKEWNESS_VALUE = 0.0;
58
59 /**
60 * Default value indicating whether horizontal focal length value is
61 * suggested or not. By default, this is disabled.
62 */
63 public static final boolean DEFAULT_SUGGEST_HORIZONTAL_FOCAL_LENGTH_ENABLED = false;
64
65 /**
66 * Default value indicating whether vertical focal length value is suggested
67 * or not. By default, this is disabled.
68 */
69 public static final boolean DEFAULT_SUGGEST_VERTICAL_FOCAL_LENGTH_ENABLED = false;
70
71 /**
72 * Default value indicating whether aspect ratio is suggested or not. By
73 * default, this is disabled.
74 */
75 public static final boolean DEFAULT_SUGGEST_ASPECT_RATIO_ENABLED = false;
76
77 /**
78 * Default value of aspect ratio to be suggested when suggestion is enabled.
79 * By default, suggested aspect ratio is 1.0, although also -1.0 is a typical
80 * value when vertical coordinates increase downwards.
81 */
82 public static final double DEFAULT_SUGGESTED_ASPECT_RATIO_VALUE = 1.0;
83
84 /**
85 * Default value indicating whether principal point is suggested or not. By
86 * default, this is disabled.
87 */
88 public static final boolean DEFAULT_SUGGEST_PRINCIPAL_POINT_ENABLED = false;
89
90 /**
91 * Default value indicating whether rotation is suggested or not. By default,
92 * this is disabled.
93 */
94 public static final boolean DEFAULT_SUGGEST_ROTATION_ENABLED = false;
95
96 /**
97 * Default value indicating whether center is suggested or not. By default,
98 * this is disabled.
99 */
100 public static final boolean DEFAULT_SUGGEST_CENTER_ENABLED = false;
101
102 /**
103 * Standard deviation used for Levenberg-Marquardt fitting during
104 * refinement.
105 * Returned value gives an indication of how much variance each residual
106 * has.
107 * Typically, this value is related to the threshold used on each robust
108 * estimation, since residuals of found inliers are within the range of
109 * such threshold.
110 */
111 protected double refinementStandardDeviation;
112
113 /**
114 * Indicates whether skewness value is suggested or not. When enabled, the
115 * estimator will attempt to enforce suggested value in an iterative manner
116 * starting from an initially estimated camera.
117 * Even when suggestion is enabled, the iterative algorithm might not reach
118 * suggested value if the initial value largely differs from the suggested
119 * value.
120 */
121 private boolean suggestSkewnessValueEnabled = DEFAULT_SUGGEST_SKEWNESS_VALUE_ENABLED;
122
123 /**
124 * Suggested skewness value to be reached when suggestion is enabled.
125 * Suggested value should be close to the initially estimated value
126 * otherwise the iterative refinement might not converge to provided
127 * value.
128 */
129 private double suggestedSkewnessValue = DEFAULT_SUGGESTED_SKEWNESS_VALUE;
130
131 /**
132 * Indicates whether horizontal focal length is suggested or not. When
133 * enabled, the estimator will attempt to enforce suggested value in an
134 * iterative manner starting from an initially estimated camera.
135 * Even when suggestion is enabled, the iterative algorithm might not reach
136 * suggested value if the initial value largely differs from the suggested
137 * value.
138 */
139 private boolean suggestHorizontalFocalLengthEnabled = DEFAULT_SUGGEST_HORIZONTAL_FOCAL_LENGTH_ENABLED;
140
141 /**
142 * Suggested horizontal focal length value to be reached when suggestion is
143 * enabled.
144 * Suggested value should be close to the initially estimated value
145 * otherwise the iterative refinement might not converge to provided value.
146 */
147 private double suggestedHorizontalFocalLengthValue;
148
149 /**
150 * Indicates whether vertical focal length is suggested or not. When
151 * enabled, the estimator will attempt to enforce suggested value in an
152 * iterative manner starting from an initially estimated camera.
153 * Even when suggestion is enabled, the iterative algorithm might not reach
154 * suggested value if the initial value largely differs from the suggested
155 * value.
156 */
157 private boolean suggestVerticalFocalLengthEnabled = DEFAULT_SUGGEST_VERTICAL_FOCAL_LENGTH_ENABLED;
158
159 /**
160 * Suggested vertical focal length value to be reached when suggestion is
161 * enabled.
162 * Suggested value should be close to the initially estimated value
163 * otherwise the iterative refinement might not converge to provided value.
164 */
165 private double suggestedVerticalFocalLengthValue;
166
167 /**
168 * Indicates whether aspect ratio is suggested or not. When enabled, the
169 * estimator will attempt to enforce suggested value in an iterative manner
170 * starting from an initially estimated camera.
171 * Even when suggestion is enabled, the iterative algorithm might not reach
172 * suggested value if the initial value largely differs from the suggested
173 * value.
174 */
175 private boolean suggestAspectRatioEnabled = DEFAULT_SUGGEST_ASPECT_RATIO_ENABLED;
176
177 /**
178 * Suggested aspect ratio value to be reached when suggestion is enabled.
179 * Suggested value should be close to the initially estimated value
180 * otherwise the iterative refinement might not converge to provided value.
181 */
182 private double suggestedAspectRatioValue = DEFAULT_SUGGESTED_ASPECT_RATIO_VALUE;
183
184 /**
185 * Indicates whether principal point is suggested or not. When enabled, the
186 * estimator will attempt to enforce suggested value in an iterative manner
187 * starting from an initially estimated camera.
188 * Even when suggestion is enabled, the iterative algorithm might not reach
189 * suggested value if the initial value largely differs from the suggested
190 * value.
191 */
192 private boolean suggestPrincipalPointEnabled = DEFAULT_SUGGEST_PRINCIPAL_POINT_ENABLED;
193
194 /**
195 * Suggested principal point value to be reached when suggestion is enabled.
196 * Suggested value should be close to the initially estimated value
197 * otherwise the iterative refinement might not converge to provided value.
198 */
199 private InhomogeneousPoint2D suggestedPrincipalPointValue;
200
201 /**
202 * Indicates whether camera rotation is suggested or not. When enabled, the
203 * estimator will attempt to enforce suggested value in an iterative manner
204 * starting from an initially estimated camera.
205 * Even when suggestion is enabled, the iterative algorithm might not reach
206 * suggested value if the initial value largely differs from the suggested
207 * value.
208 */
209 private boolean suggestRotationEnabled = DEFAULT_SUGGEST_ROTATION_ENABLED;
210
211 /**
212 * Suggested rotation to be reached when suggestion is enabled.
213 * Suggested value should be close to the initially estimated value
214 * otherwise the iterative refinement might not converge to provided value.
215 */
216 private Quaternion suggestedRotationValue;
217
218 /**
219 * Indicates whether camera center is suggested or not. When enabled, the
220 * estimator will attempt to enforce suggested value in an iterative manner
221 * starting from an initially estimated camera.
222 * Even when suggestion is enabled, the iterative algorithm might not reach
223 * suggested value if the initial value largely differs from the suggested
224 * value.
225 */
226 private boolean suggestCenterEnabled;
227
228 /**
229 * Suggested center to be reached when suggestion is enabled.
230 * Suggested value should be close to the initially estimated value
231 * otherwise the iterative refinement might not converge to provided value.
232 */
233 private InhomogeneousPoint3D suggestedCenterValue;
234
235 /**
236 * Instance to be reused to compute residual for intrinsic parameters.
237 */
238 private PinholeCameraIntrinsicParameters residualIntrinsic;
239
240 /**
241 * Instance to be reused to compute residual on principal point.
242 */
243 private InhomogeneousPoint2D residualPrincipalPoint;
244
245 /**
246 * Instance to be reused to compute residual on rotation.
247 */
248 private Quaternion residualRotation;
249
250 /**
251 * Instance to be reused to compute center.
252 */
253 private InhomogeneousPoint3D residualCenter;
254
255 /**
256 * Constructor.
257 */
258 protected PinholeCameraRefiner() {
259 }
260
261 /**
262 * Constructor.
263 *
264 * @param initialEstimation initial estimation to be set.
265 * @param keepCovariance true if covariance of estimation must be kept after
266 * refinement, false otherwise.
267 * @param inliers set indicating which of the provided matches are inliers.
268 * @param residuals residuals for matched samples.
269 * @param numInliers number of inliers on initial estimation.
270 * @param samples1 1st set of paired samples.
271 * @param samples2 2nd set of paired samples.
272 * @param refinementStandardDeviation standard deviation used for
273 * Levenberg-Marquardt fitting.
274 */
275 protected PinholeCameraRefiner(
276 final PinholeCamera initialEstimation, final boolean keepCovariance, final BitSet inliers,
277 final double[] residuals, final int numInliers, final List<S1> samples1, final List<S2> samples2,
278 final double refinementStandardDeviation) {
279 super(initialEstimation, keepCovariance, inliers, residuals, numInliers, samples1, samples2);
280 this.refinementStandardDeviation = refinementStandardDeviation;
281 }
282
283 /**
284 * Constructor.
285 *
286 * @param initialEstimation initial estimation to be set.
287 * @param keepCovariance true if covariance of estimation must be kept after
288 * refinement, false otherwise.
289 * @param inliersData inlier data, typically obtained from a robust
290 * estimator.
291 * @param samples1 1st set of paired samples.
292 * @param samples2 2nd set of paired samples.
293 * @param refinementStandardDeviation standard deviation used for
294 * Levenberg-Marquardt fitting.
295 */
296 protected PinholeCameraRefiner(
297 final PinholeCamera initialEstimation, final boolean keepCovariance, final InliersData inliersData,
298 final List<S1> samples1, final List<S2> samples2, final double refinementStandardDeviation) {
299 super(initialEstimation, keepCovariance, inliersData, samples1, samples2);
300 this.refinementStandardDeviation = refinementStandardDeviation;
301 }
302
303 /**
304 * Gets standard deviation used for Levenberg-Marquardt fitting during
305 * refinement.
306 * Returned value gives an indication of how much variance each residual
307 * has.
308 * Typically, this value is related to the threshold used on each robust
309 * estimation, since residuals of found inliers are within the range of such
310 * threshold.
311 *
312 * @return standard deviation used for refinement.
313 */
314 public double getRefinementStandardDeviation() {
315 return refinementStandardDeviation;
316 }
317
318 /**
319 * Sets standard deviation used for Levenberg-Marquardt fitting during
320 * refinement.
321 * Returned value gives an indication of how much variance each residual
322 * has.
323 * Typically, this value is related to the threshold used on each robust
324 * estimation, since residuals of found inliers are within the range of such
325 * threshold.
326 *
327 * @param refinementStandardDeviation standard deviation used for
328 * refinement.
329 * @throws LockedException if estimator is locked.
330 */
331 public void setRefinementStandardDeviation(final double refinementStandardDeviation) throws LockedException {
332 if (isLocked()) {
333 throw new LockedException();
334 }
335 this.refinementStandardDeviation = refinementStandardDeviation;
336 }
337
338 /**
339 * Indicates whether skewness value is suggested or not. When enabled, the
340 * estimator will attempt to enforce suggested value in an iterative manner
341 * starting from an initially estimated camera.
342 * Even when suggestion is enabled, the iterative algorithm might not reach
343 * suggested value if the initial value largely differs from the suggested
344 * value.
345 *
346 * @return true if skewness value is suggested, false otherwise.
347 */
348 public boolean isSuggestSkewnessValueEnabled() {
349 return suggestSkewnessValueEnabled;
350 }
351
352 /**
353 * Specifies whether skewness value is suggested or not. When enabled, the
354 * estimator will attempt to enforce suggested value in an iterative manner
355 * starting from an initially estimated camera.
356 * Even when suggestion is enabled, the iterative algorithm might not reach
357 * suggested value if the initial value largely differs from the suggested
358 * value.
359 *
360 * @param suggestSkewnessValueEnabled true if skewness value is suggested,
361 * false otherwise.
362 * @throws LockedException if estimator is locked.
363 */
364 public void setSuggestSkewnessValueEnabled(final boolean suggestSkewnessValueEnabled) throws LockedException {
365 if (isLocked()) {
366 throw new LockedException();
367 }
368 this.suggestSkewnessValueEnabled = suggestSkewnessValueEnabled;
369 }
370
371 /**
372 * Gets suggested skewness value to be reached when suggestion is enabled.
373 * Suggested value should be close to the initially estimated value
374 * otherwise the iterative refinement might not converge to provided value.
375 *
376 * @return suggested skewness value.
377 */
378 public double getSuggestedSkewnessValue() {
379 return suggestedSkewnessValue;
380 }
381
382 /**
383 * Sets suggested skewness value to be reached when suggestion is enabled.
384 * Suggested value should be close to the initially estimated value
385 * otherwise the iterative refinement might not converge to provided value.
386 *
387 * @param suggestedSkewnessValue suggested skewness value.
388 * @throws LockedException if estimator is locked.
389 */
390 public void setSuggestedSkewnessValue(final double suggestedSkewnessValue) throws LockedException {
391 if (isLocked()) {
392 throw new LockedException();
393 }
394 this.suggestedSkewnessValue = suggestedSkewnessValue;
395 }
396
397 /**
398 * Indicates whether horizontal focal length is suggested or not. When
399 * enabled, the estimator will attempt to enforce suggested value in an
400 * iterative manner starting from an initially estimated camera.
401 * Even when suggestion is enabled, the iterative algorithm might not reach
402 * suggested value if the initial value largely differs from the suggested
403 * value.
404 *
405 * @return true if horizontal focal length is suggested, false otherwise.
406 */
407 public boolean isSuggestHorizontalFocalLengthEnabled() {
408 return suggestHorizontalFocalLengthEnabled;
409 }
410
411 /**
412 * Specifies whether horizontal focal length is suggested or not. When
413 * enabled, the estimator will attempt to enforce suggested value in an
414 * iterative manner starting from an initially estimated camera.
415 * Even when suggestion is enabled, the iterative algorithm might not reach
416 * suggested value if the initial value largely differs from the suggested
417 * value.
418 *
419 * @param suggestHorizontalFocalLengthEnabled true if horizontal focal
420 * length is suggested, false otherwise.
421 * @throws LockedException if estimator is locked.
422 */
423 public void setSuggestHorizontalFocalLengthEnabled(final boolean suggestHorizontalFocalLengthEnabled)
424 throws LockedException {
425 if (isLocked()) {
426 throw new LockedException();
427 }
428 this.suggestHorizontalFocalLengthEnabled = suggestHorizontalFocalLengthEnabled;
429 }
430
431 /**
432 * Gets suggested horizontal focal length value to be reached when
433 * suggestion is enabled.
434 * Suggested value should be close to the initially estimated value
435 * otherwise the iterative refinement might not converge to provided value.
436 *
437 * @return suggested horizontal focal length value.
438 */
439 public double getSuggestedHorizontalFocalLengthValue() {
440 return suggestedHorizontalFocalLengthValue;
441 }
442
443 /**
444 * Sets suggested horizontal focal length value to be reached when
445 * suggestion is enabled.
446 * Suggested value should be close to the initially estimated value
447 * otherwise the iterative refinement might not converge to provided value.
448 *
449 * @param suggestedHorizontalFocalLengthValue suggested horizontal focal
450 * length value.
451 * @throws LockedException if estimator is locked.
452 */
453 public void setSuggestedHorizontalFocalLengthValue(final double suggestedHorizontalFocalLengthValue)
454 throws LockedException {
455 if (isLocked()) {
456 throw new LockedException();
457 }
458 this.suggestedHorizontalFocalLengthValue = suggestedHorizontalFocalLengthValue;
459 }
460
461 /**
462 * Indicates whether vertical focal length is suggested or not. When
463 * enabled, the estimator will attempt to enforce suggested value in an
464 * iterative manner starting from an initially estimated camera.
465 * Even when suggestion is enabled, the iterative algorithm might not reach
466 * suggested value if the initial value largely differs from the suggested
467 * value.
468 *
469 * @return true if vertical focal length is suggested, false otherwise.
470 */
471 public boolean isSuggestVerticalFocalLengthEnabled() {
472 return suggestVerticalFocalLengthEnabled;
473 }
474
475 /**
476 * Specifies whether vertical focal length is suggested or not. When
477 * enabled, the estimator will attempt to enforce suggested value in an
478 * iterative manner starting from an initially estimated camera.
479 * Even when suggestion is enabled, the iterative algorithm might not reach
480 * suggested value if the initial value largely differs from the suggested
481 * value.
482 *
483 * @param suggestVerticalFocalLengthEnabled true if vertical focal length is
484 * suggested, false otherwise.
485 * @throws LockedException if estimator is locked.
486 */
487 public void setSuggestVerticalFocalLengthEnabled(final boolean suggestVerticalFocalLengthEnabled)
488 throws LockedException {
489 if (isLocked()) {
490 throw new LockedException();
491 }
492 this.suggestVerticalFocalLengthEnabled = suggestVerticalFocalLengthEnabled;
493 }
494
495 /**
496 * Gets suggested vertical focal length value to be reached when suggestion
497 * is enabled.
498 * Suggested value should be close to the initially estimated value
499 * otherwise the iterative refinement might not converge to provided value.
500 *
501 * @return suggested vertical focal length.
502 */
503 public double getSuggestedVerticalFocalLengthValue() {
504 return suggestedVerticalFocalLengthValue;
505 }
506
507 /**
508 * Sets suggested vertical focal length value to be reached when suggestion
509 * is enabled.
510 * Suggested value should be close to the initially estimated value
511 * otherwise the iterative refinement might not converge to provided value.
512 *
513 * @param suggestedVerticalFocalLengthValue suggested vertical focal length.
514 * @throws LockedException if estimator is locked.
515 */
516 public void setSuggestedVerticalFocalLengthValue(final double suggestedVerticalFocalLengthValue)
517 throws LockedException {
518 if (isLocked()) {
519 throw new LockedException();
520 }
521 this.suggestedVerticalFocalLengthValue = suggestedVerticalFocalLengthValue;
522 }
523
524 /**
525 * Indicates whether aspect ratio is suggested or not. When enabled, the
526 * estimator will attempt to enforce suggested value in an iterative manner
527 * starting from an initially estimated camera.
528 * Even when suggestion is enabled, the iterative algorithm might not reach
529 * suggested value if the initial value largely differs from the suggested
530 * value.
531 *
532 * @return true if aspect ratio is suggested, false otherwise.
533 */
534 public boolean isSuggestAspectRatioEnabled() {
535 return suggestAspectRatioEnabled;
536 }
537
538 /**
539 * Specifies whether aspect ratio is suggested or not. When enabled, the
540 * estimator will attempt to enforce suggested value in an iterative manner
541 * starting from an initially estimated camera.
542 * Even when suggestion is enabled, the iterative algorithm might not reach
543 * suggested value if the initial value largely differs from the suggested
544 * value.
545 *
546 * @param suggestAspectRatioEnabled true if aspect ratio is suggested, false
547 * otherwise.
548 * @throws LockedException if estimator is locked.
549 */
550 public void setSuggestAspectRatioEnabled(final boolean suggestAspectRatioEnabled) throws LockedException {
551 if (isLocked()) {
552 throw new LockedException();
553 }
554 this.suggestAspectRatioEnabled = suggestAspectRatioEnabled;
555 }
556
557 /**
558 * Gets suggested aspect ratio value to be reached when suggestion is
559 * enabled. Suggested value should be close to the initially estimated value
560 * otherwise the iterative refinement might not converge to provided value.
561 *
562 * @return suggested aspect ratio value.
563 */
564 public double getSuggestedAspectRatioValue() {
565 return suggestedAspectRatioValue;
566 }
567
568 /**
569 * Sets suggested aspect ratio value to be reached when suggestion is
570 * enabled. Suggested value should be close to the initially estimated value
571 * otherwise the iterative refinement might not converge to provided value.
572 *
573 * @param suggestedAspectRatioValue suggested aspect ratio value.
574 * @throws LockedException if estimator is locked.
575 */
576 public void setSuggestedAspectRatioValue(final double suggestedAspectRatioValue) throws LockedException {
577 if (isLocked()) {
578 throw new LockedException();
579 }
580 this.suggestedAspectRatioValue = suggestedAspectRatioValue;
581 }
582
583 /**
584 * Indicates whether principal point is suggested or not. When enabled, the
585 * estimator will attempt to enforce suggested value in an iterative manner
586 * starting from an initially estimated camera.
587 * Even when suggestion is enabled, the iterative algorithm might not reach
588 * suggested value if the initial value largely differs from the suggested
589 * value.
590 *
591 * @return true if principal point is suggested, false otherwise.
592 */
593 public boolean isSuggestPrincipalPointEnabled() {
594 return suggestPrincipalPointEnabled;
595 }
596
597 /**
598 * Specifies whether principal point is suggested or not. When enabled, the
599 * estimator will attempt to enforce suggested value in an iterative manner
600 * starting from an initially estimated camera.
601 * Even when suggestion is enabled, the iterative algorithm might not reach
602 * suggested value if the initial value largely differs from the suggested
603 * value.
604 *
605 * @param suggestPrincipalPointEnabled true if principal point is suggested,
606 * false otherwise.
607 * @throws LockedException if estimator is locked.
608 */
609 public void setSuggestPrincipalPointEnabled(final boolean suggestPrincipalPointEnabled) throws LockedException {
610 if (isLocked()) {
611 throw new LockedException();
612 }
613 this.suggestPrincipalPointEnabled = suggestPrincipalPointEnabled;
614 if (suggestPrincipalPointEnabled && suggestedPrincipalPointValue == null) {
615 suggestedPrincipalPointValue = new InhomogeneousPoint2D();
616 }
617 }
618
619 /**
620 * Gets suggested principal point value to be reached when suggestion is
621 * enabled. Suggested value should be close to the initially estimated value
622 * otherwise the iterative refinement might not converge to provided value.
623 *
624 * @return suggested principal point value to be reached when suggestion is
625 * enabled.
626 */
627 public InhomogeneousPoint2D getSuggestedPrincipalPointValue() {
628 return suggestedPrincipalPointValue;
629 }
630
631 /**
632 * Sets suggested principal point value to be reached when suggestion is
633 * enabled. Suggested value should be close to the initially estimated value
634 * otherwise the iterative refinement might not converge to provided value.
635 *
636 * @param suggestedPrincipalPointValue suggested principal point value to be
637 * reached when suggestion is enabled.
638 * @throws LockedException if estimator is locked.
639 */
640 public void setSuggestedPrincipalPointValue(final InhomogeneousPoint2D suggestedPrincipalPointValue)
641 throws LockedException {
642 if (isLocked()) {
643 throw new LockedException();
644 }
645 this.suggestedPrincipalPointValue = suggestedPrincipalPointValue;
646 }
647
648 /**
649 * Indicates whether camera rotation is suggested or not. When enabled, the
650 * estimator will attempt to enforce suggested value in an iterative manner
651 * starting from an initially estimated camera.
652 * Even when suggestion is enabled, the iterative algorithm might not reach
653 * suggested value if the initial value largely differs from the suggested
654 * value.
655 *
656 * @return true if camera rotation is suggested, false otherwise.
657 */
658 public boolean isSuggestRotationEnabled() {
659 return suggestRotationEnabled;
660 }
661
662 /**
663 * Specifies whether camera rotation is suggested or not. When enabled, the
664 * estimator will attempt to enforce suggested value in an iterative manner
665 * starting from an initially estimated camera.
666 * Even when suggestion is enabled, the iterative algorithm might not reach
667 * suggested value if the initial value largely differs from the suggested
668 * value.
669 *
670 * @param suggestRotationEnabled true if camera rotation is suggested, false
671 * otherwise.
672 * @throws LockedException if estimator is locked.
673 */
674 public void setSuggestRotationEnabled(final boolean suggestRotationEnabled) throws LockedException {
675 if (isLocked()) {
676 throw new LockedException();
677 }
678 this.suggestRotationEnabled = suggestRotationEnabled;
679 if (suggestRotationEnabled && suggestedRotationValue == null) {
680 suggestedRotationValue = new Quaternion();
681 }
682
683 }
684
685 /**
686 * Gets suggested rotation to be reached when suggestion is enabled.
687 * Suggested value should be close to the initially estimated value
688 * otherwise the iterative refinement might not converge to provided value.
689 *
690 * @return suggested rotation to be reached when suggestion is enabled.
691 */
692 public Quaternion getSuggestedRotationValue() {
693 return suggestedRotationValue;
694 }
695
696 /**
697 * Sets suggested rotation to be reached when suggestion is enabled.
698 * Suggested value should be close to the initially estimated value
699 * otherwise the iterative refinement might not converge to provided value.
700 *
701 * @param suggestedRotationValue suggested rotation to be reached when
702 * suggestion is enabled.
703 * @throws LockedException if estimator is locked.
704 */
705 public void setSuggestedRotationValue(final Quaternion suggestedRotationValue) throws LockedException {
706 if (isLocked()) {
707 throw new LockedException();
708 }
709 this.suggestedRotationValue = suggestedRotationValue;
710 }
711
712 /**
713 * Indicates whether camera center is suggested or not. When enabled, the
714 * estimator will attempt to enforce suggested value in an iterative manner
715 * starting from an initially estimated camera.
716 * Even when suggestion is enabled, the iterative algorithm might not reach
717 * suggested value if the initial value largely differs from the suggested
718 * value.
719 *
720 * @return true if camera center is suggested, false otherwise.
721 */
722 public boolean isSuggestCenterEnabled() {
723 return suggestCenterEnabled;
724 }
725
726 /**
727 * Specifies whether camera center is suggested or not. When enabled, the
728 * estimator will attempt to enforce suggested value in an iterative manner
729 * starting from an initially estimated camera.
730 * Even when suggestion is enabled, the iterative algorithm might not reach
731 * suggested value if the initial value largely differs from the suggested
732 * value.
733 *
734 * @param suggestCenterEnabled true if camera is suggested, false otherwise.
735 * @throws LockedException if estimator is locked.
736 */
737 public void setSuggestCenterEnabled(final boolean suggestCenterEnabled) throws LockedException {
738 if (isLocked()) {
739 throw new LockedException();
740 }
741 this.suggestCenterEnabled = suggestCenterEnabled;
742 if (suggestCenterEnabled && suggestedCenterValue == null) {
743 suggestedCenterValue = new InhomogeneousPoint3D();
744 }
745 }
746
747 /**
748 * Gets suggested center to be reached when suggestion is enabled.
749 * Suggested value should be close to the initially estimated value
750 * otherwise the iterative refinement might not converge to provided value.
751 *
752 * @return suggested center to be reached when suggestion is enabled.
753 */
754 public InhomogeneousPoint3D getSuggestedCenterValue() {
755 return suggestedCenterValue;
756 }
757
758 /**
759 * Sets suggested center to be reached when suggestion is enabled.
760 * Suggested value should be close to the initially estimated value
761 * otherwise the iterative refinement might not converge to provided value.
762 *
763 * @param suggestedCenterValue suggested center to be reached when
764 * suggestion is enabled.
765 * @throws LockedException if estimator is locked.
766 */
767 public void setSuggestedCenterValue(final InhomogeneousPoint3D suggestedCenterValue) throws LockedException {
768 if (isLocked()) {
769 throw new LockedException();
770 }
771 this.suggestedCenterValue = suggestedCenterValue;
772 }
773
774 /**
775 * Refines provided initial estimation.
776 *
777 * @return refines estimation.
778 * @throws NotReadyException if not enough input data has been provided.
779 * @throws LockedException if estimator is locked because refinement is
780 * already in progress.
781 * @throws RefinerException if refinement fails for some reason (e.g. unable
782 * to converge to a result).
783 */
784 @Override
785 public PinholeCamera refine() throws NotReadyException, LockedException, RefinerException {
786 final var result = new PinholeCamera();
787 refine(result);
788 return result;
789 }
790
791 /**
792 * Residual term for any required suggestions.
793 *
794 * @param params parameters being optimized. In the following order:
795 * skewness, horizontal focal length, vertical focal length,
796 * horizontal principal point, vertical principal point, quaternion A,
797 * quaternion B, quaternion C, quaternion D, center x, center y, center z.
798 * @param weight weight to apply to obtained residual. This weight increases
799 * on each iteration to help into achieving required suggested values.
800 * @return term for any required suggestion.
801 */
802 protected double suggestionResidual(final double[] params, final double weight) {
803 double residual = 0.0;
804
805 if (suggestSkewnessValueEnabled) {
806 residual += Math.pow(params[0] - suggestedSkewnessValue, 2.0);
807 }
808
809 if (suggestHorizontalFocalLengthEnabled) {
810 residual += Math.pow(params[1] - suggestedHorizontalFocalLengthValue, 2.0);
811 }
812
813 if (suggestVerticalFocalLengthEnabled) {
814 residual += Math.pow(params[2] - suggestedVerticalFocalLengthValue, 2.0);
815 }
816
817 if (suggestAspectRatioEnabled) {
818 final var aspectRatio = params[2] / params[1];
819 residual += Math.pow(aspectRatio - suggestedAspectRatioValue, 2.0);
820 }
821
822 if (suggestPrincipalPointEnabled) {
823 if (residualPrincipalPoint == null) {
824 residualPrincipalPoint = new InhomogeneousPoint2D(params[3], params[4]);
825 } else {
826 residualPrincipalPoint.setInhomogeneousCoordinates(params[3], params[4]);
827 }
828
829 residual += Math.pow(residualPrincipalPoint.distanceTo(suggestedPrincipalPointValue), 2.0);
830 }
831
832 if (suggestRotationEnabled) {
833 if (residualRotation == null) {
834 residualRotation = new Quaternion(params[5], params[6], params[7], params[8]);
835 } else {
836 residualRotation.setA(params[5]);
837 residualRotation.setB(params[6]);
838 residualRotation.setC(params[7]);
839 residualRotation.setD(params[8]);
840 }
841 residualRotation.normalize();
842 suggestedRotationValue.normalize();
843 residual += Math.pow(residualRotation.getA() - suggestedRotationValue.getA(), 2.0)
844 + Math.pow(residualRotation.getB() - suggestedRotationValue.getB(), 2.0)
845 + Math.pow(residualRotation.getC() - suggestedRotationValue.getC(), 2.0)
846 + Math.pow(residualRotation.getD() - suggestedRotationValue.getD(), 2.0);
847 }
848
849 if (suggestCenterEnabled) {
850 if (residualCenter == null) {
851 residualCenter = new InhomogeneousPoint3D(params[9], params[10], params[11]);
852 } else {
853 residualCenter.setInhomogeneousCoordinates(params[9], params[10], params[11]);
854 }
855 residual += Math.pow(residualCenter.distanceTo(suggestedCenterValue), 2.0);
856 }
857
858 return weight * residual;
859 }
860
861 /**
862 * Sets array of parameters into a pinhole camera.
863 * This method is used internally during refinement.
864 *
865 * @param params parameters to be set. In the following order:
866 * skewness, horizontal focal length, vertical focal length,
867 * horizontal principal point, vertical principal point, quaternion A,
868 * quaternion B, quaternion C, quaternion D, center x, center y, center z.
869 * @param result instance where parameters will be set.
870 */
871 protected void parametersToCamera(final double[] params, final PinholeCamera result) {
872
873 if (residualIntrinsic == null) {
874 residualIntrinsic = new PinholeCameraIntrinsicParameters();
875 }
876 residualIntrinsic.setSkewness(params[0]);
877 residualIntrinsic.setHorizontalFocalLength(params[1]);
878 residualIntrinsic.setVerticalFocalLength(params[2]);
879 residualIntrinsic.setHorizontalPrincipalPoint(params[3]);
880 residualIntrinsic.setVerticalPrincipalPoint(params[4]);
881
882 if (residualRotation == null) {
883 residualRotation = new Quaternion(params[5], params[6], params[7], params[8]);
884 } else {
885 residualRotation.setA(params[5]);
886 residualRotation.setB(params[6]);
887 residualRotation.setC(params[7]);
888 residualRotation.setD(params[8]);
889 }
890 residualRotation.normalize();
891
892 if (residualCenter == null) {
893 residualCenter = new InhomogeneousPoint3D(params[9], params[10], params[11]);
894 } else {
895 residualCenter.setInhomogeneousCoordinates(params[9], params[10], params[11]);
896 }
897 residualCenter.normalize();
898
899 result.setIntrinsicAndExtrinsicParameters(residualIntrinsic, residualRotation, residualCenter);
900 result.normalize();
901 }
902
903 /**
904 * Sets camera parameters into array of parameters.
905 * This method is used internally during refinement.
906 *
907 * @param camera camera to obtain parameters to be set into array.
908 * @param result array where extracted parameters are stored. In the
909 * following order:
910 * skewness, horizontal focal length, vertical focal length,
911 * horizontal principal point, vertical principal point, quaternion A,
912 * quaternion B, quaternion C, quaternion D, center x, center y, center z.
913 * @throws CameraException if camera cannot be decomposed.
914 * @throws NotAvailableException if any camera component cannot be
915 * retrieved.
916 */
917 protected void cameraToParameters(final PinholeCamera camera, final double[] result) throws CameraException,
918 NotAvailableException {
919
920 camera.decompose();
921
922 final var intrinsic = camera.getIntrinsicParameters();
923 result[0] = intrinsic.getSkewness();
924 result[1] = intrinsic.getHorizontalFocalLength();
925 result[2] = intrinsic.getVerticalFocalLength();
926 result[3] = intrinsic.getHorizontalPrincipalPoint();
927 result[4] = intrinsic.getVerticalPrincipalPoint();
928
929 final var rotation = camera.getCameraRotation();
930 if (residualRotation == null) {
931 residualRotation = rotation.toQuaternion();
932 } else {
933 rotation.toQuaternion(residualRotation);
934 }
935 residualRotation.normalize();
936
937 result[5] = residualRotation.getA();
938 result[6] = residualRotation.getB();
939 result[7] = residualRotation.getC();
940 result[8] = residualRotation.getD();
941
942 final var center = camera.getCameraCenter();
943
944 result[9] = center.getInhomX();
945 result[10] = center.getInhomY();
946 result[11] = center.getInhomZ();
947 }
948
949 /**
950 * Indicates whether obtained solution requires refinement to apply provided
951 * suggestions.
952 *
953 * @return true if solution requires refinement to apply provided
954 * suggestions, false otherwise.
955 */
956 protected boolean hasSuggestions() {
957 return hasIntrinsicSuggestions() || hasExtrinsicSuggestions();
958 }
959
960 /**
961 * Indicates whether suggestions for any intrinsic parameter are required
962 * or not.
963 *
964 * @return true if suggestions for any intrinsic parameters are required,
965 * false otherwise.
966 */
967 private boolean hasIntrinsicSuggestions() {
968 return suggestSkewnessValueEnabled || suggestHorizontalFocalLengthEnabled || suggestVerticalFocalLengthEnabled
969 || suggestAspectRatioEnabled;
970 }
971
972 /**
973 * Indicates whether suggestions for any extrinsic parameter are required
974 * or not.
975 *
976 * @return true if suggestions for any extrinsic parameter are required,
977 * false otherwise.
978 */
979 private boolean hasExtrinsicSuggestions() {
980 return suggestPrincipalPointEnabled || suggestRotationEnabled || suggestCenterEnabled;
981 }
982 }