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.estimators;
17
18 import com.irurueta.geometry.CoordinatesType;
19 import com.irurueta.geometry.PinholeCamera;
20 import com.irurueta.geometry.PinholeCameraIntrinsicParameters;
21 import com.irurueta.geometry.Point2D;
22 import com.irurueta.geometry.Point3D;
23 import com.irurueta.numerical.robust.RANSACRobustEstimator;
24 import com.irurueta.numerical.robust.RANSACRobustEstimatorListener;
25 import com.irurueta.numerical.robust.RobustEstimator;
26 import com.irurueta.numerical.robust.RobustEstimatorException;
27 import com.irurueta.numerical.robust.RobustEstimatorMethod;
28
29 import java.util.ArrayList;
30 import java.util.List;
31
32 /**
33 * Finds the best pinhole camera for provided collections of matched 2D/3D
34 * points using RANSAC + EPnP algorithms.
35 */
36 @SuppressWarnings("DuplicatedCode")
37 public class RANSACEPnPPointCorrespondencePinholeCameraRobustEstimator extends
38 EPnPPointCorrespondencePinholeCameraRobustEstimator {
39
40 /**
41 * Constant defining default threshold to determine whether points are
42 * inliers or not.
43 * By default, 1.0 is considered a good value for cases where measures are
44 * done on pixels, since typically the minimum resolution is 1 pixel.
45 */
46 public static final double DEFAULT_THRESHOLD = 1.0;
47
48 /**
49 * Minimum value that can be set as threshold.
50 * Threshold must be strictly greater than 0.0.
51 */
52 public static final double MIN_THRESHOLD = 0.0;
53
54 /**
55 * Indicates that by default inliers will only be computed but not kept.
56 */
57 public static final boolean DEFAULT_COMPUTE_AND_KEEP_INLIERS = false;
58
59 /**
60 * Indicates that by default residuals will only be computed but not kept.
61 */
62 public static final boolean DEFAULT_COMPUTE_AND_KEEP_RESIDUALS = false;
63
64 /**
65 * Threshold to determine whether points are inliers or not when testing
66 * possible estimation solutions.
67 * The threshold refers to the amount of error (i.e. distance) a possible
68 * solution has on a matched pair of points.
69 */
70 private double threshold;
71
72 /**
73 * Indicates whether inliers must be computed and kept.
74 */
75 private boolean computeAndKeepInliers;
76
77 /**
78 * Indicates whether residuals must be computed and kept.
79 */
80 private boolean computeAndKeepResiduals;
81
82 /**
83 * Constructor.
84 */
85 public RANSACEPnPPointCorrespondencePinholeCameraRobustEstimator() {
86 super();
87 threshold = DEFAULT_THRESHOLD;
88 computeAndKeepInliers = DEFAULT_COMPUTE_AND_KEEP_INLIERS;
89 computeAndKeepResiduals = DEFAULT_COMPUTE_AND_KEEP_RESIDUALS;
90 }
91
92 /**
93 * Constructor with listener.
94 *
95 * @param listener listener to be notified of events such as when estimation
96 * starts, ends or its progress significantly changes.
97 */
98 public RANSACEPnPPointCorrespondencePinholeCameraRobustEstimator(
99 final PinholeCameraRobustEstimatorListener listener) {
100 super(listener);
101 threshold = DEFAULT_THRESHOLD;
102 computeAndKeepInliers = DEFAULT_COMPUTE_AND_KEEP_INLIERS;
103 computeAndKeepResiduals = DEFAULT_COMPUTE_AND_KEEP_RESIDUALS;
104 }
105
106 /**
107 * Constructor with lists of points to be used to estimate a pinhole camera.
108 * Points in the lists located at the same position are considered to be
109 * matched. Hence, both lists must have the same size, and their size must
110 * be greater or equal than MIN_NUMBER_OF_POINT_CORRESPONDENCES (6 points).
111 *
112 * @param points3D list of 3D points used to estimate a pinhole camera.
113 * @param points2D list of corresponding projected 2D points used to
114 * estimate a pinhole camera.
115 * @throws IllegalArgumentException if provided lists of points don't have
116 * the same size or their size is smaller than required minimum size (6
117 * correspondences).
118 */
119 public RANSACEPnPPointCorrespondencePinholeCameraRobustEstimator(
120 final List<Point3D> points3D, final List<Point2D> points2D) {
121 super(points3D, points2D);
122 threshold = DEFAULT_THRESHOLD;
123 computeAndKeepInliers = DEFAULT_COMPUTE_AND_KEEP_INLIERS;
124 computeAndKeepResiduals = DEFAULT_COMPUTE_AND_KEEP_RESIDUALS;
125 }
126
127 /**
128 * Constructor with listener and lists of points to be used to estimate a
129 * pinhole camera.
130 * Points in the lists located at the same position are considered to be
131 * matched. Hence, both lists must have the same size, and their size must
132 * be greater or equal than MIN_NUMBER_OF_POINT_CORRESPONDENCES (6 points).
133 *
134 * @param listener listener to be notified of events such as when estimation
135 * starts, ends or its progress significantly changes.
136 * @param points3D list of 3D points used to estimate a pinhole camera.
137 * @param points2D list of corresponding projected 2D points used to
138 * estimate a pinhole camera.
139 * @throws IllegalArgumentException if provided lists of points don't have
140 * the same size or their size is smaller than required minimum size (6
141 * correspondences).
142 */
143 public RANSACEPnPPointCorrespondencePinholeCameraRobustEstimator(
144 final PinholeCameraRobustEstimatorListener listener,
145 final List<Point3D> points3D, final List<Point2D> points2D) {
146 super(listener, points3D, points2D);
147 threshold = DEFAULT_THRESHOLD;
148 computeAndKeepInliers = DEFAULT_COMPUTE_AND_KEEP_INLIERS;
149 computeAndKeepResiduals = DEFAULT_COMPUTE_AND_KEEP_RESIDUALS;
150 }
151
152 /**
153 * Constructor with intrinsic parameters.
154 *
155 * @param intrinsic intrinsic parameters of camera to be estimated.
156 */
157 public RANSACEPnPPointCorrespondencePinholeCameraRobustEstimator(final PinholeCameraIntrinsicParameters intrinsic) {
158 super(intrinsic);
159 threshold = DEFAULT_THRESHOLD;
160 computeAndKeepInliers = DEFAULT_COMPUTE_AND_KEEP_INLIERS;
161 computeAndKeepResiduals = DEFAULT_COMPUTE_AND_KEEP_RESIDUALS;
162 }
163
164 /**
165 * Constructor with intrinsic parameters and listener.
166 *
167 * @param listener listener to be notified of events such as when estimation
168 * starts, ends or its progress significantly changes.
169 * @param intrinsic intrinsic parameters of camera to be estimated.
170 */
171 public RANSACEPnPPointCorrespondencePinholeCameraRobustEstimator(
172 final PinholeCameraRobustEstimatorListener listener, final PinholeCameraIntrinsicParameters intrinsic) {
173 super(listener, intrinsic);
174 threshold = DEFAULT_THRESHOLD;
175 computeAndKeepInliers = DEFAULT_COMPUTE_AND_KEEP_INLIERS;
176 computeAndKeepResiduals = DEFAULT_COMPUTE_AND_KEEP_RESIDUALS;
177 }
178
179 /**
180 * Constructor with lists of points to be used to estimate a pinhole camera
181 * and intrinsic parameters.
182 * Points in the lists located at the same position are considered to be
183 * matched. Hence, both lists must have the same size, and their size must
184 * be greater or equal than MIN_NUMBER_OF_POINT_CORRESPONDENCES (6 points).
185 *
186 * @param intrinsic intrinsic parameters of camera to be estimated.
187 * @param points3D list of 3D points used to estimate a pinhole camera.
188 * @param points2D list of corresponding projected 2D points used to
189 * estimate a pinhole camera.
190 * @throws IllegalArgumentException if provided lists of points don't have
191 * the same size or their size is smaller than required minimum size (6
192 * correspondences).
193 */
194 public RANSACEPnPPointCorrespondencePinholeCameraRobustEstimator(
195 final PinholeCameraIntrinsicParameters intrinsic, final List<Point3D> points3D,
196 final List<Point2D> points2D) {
197 super(intrinsic, points3D, points2D);
198 threshold = DEFAULT_THRESHOLD;
199 computeAndKeepInliers = DEFAULT_COMPUTE_AND_KEEP_INLIERS;
200 computeAndKeepResiduals = DEFAULT_COMPUTE_AND_KEEP_RESIDUALS;
201 }
202
203 /**
204 * Constructor with listener and lists of points to be used to estimate a
205 * pinhole camera and intrinsic parameters.
206 * Points in the lists located at the same position are considered to be
207 * matched. Hence, both lists must have the same size, and their size must
208 * be greater or equal than MIN_NUMBER_OF_POINT_CORRESPONDENCES (6 points).
209 *
210 * @param listener listener to be notified of events such as when estimation
211 * starts, ends or its progress significantly changes.
212 * @param intrinsic intrinsic parameters of camera to be estimated.
213 * @param points3D list of 3D points used to estimate a pinhole camera.
214 * @param points2D list of corresponding projected 2D points used to
215 * estimate a pinhole camera.
216 * @throws IllegalArgumentException if provided lists of points don't have
217 * the same size or their size is smaller than required minimum size (6
218 * correspondences).
219 */
220 public RANSACEPnPPointCorrespondencePinholeCameraRobustEstimator(
221 final PinholeCameraRobustEstimatorListener listener, final PinholeCameraIntrinsicParameters intrinsic,
222 final List<Point3D> points3D, final List<Point2D> points2D) {
223 super(listener, intrinsic, points3D, points2D);
224 threshold = DEFAULT_THRESHOLD;
225 computeAndKeepInliers = DEFAULT_COMPUTE_AND_KEEP_INLIERS;
226 computeAndKeepResiduals = DEFAULT_COMPUTE_AND_KEEP_RESIDUALS;
227 }
228
229 /**
230 * Returns threshold to determine whether points are inliers or not when
231 * testing possible estimation solutions.
232 * The threshold refers to the amount of error (i.e. Euclidean distance) a
233 * possible solution has on projected 2D points.
234 *
235 * @return threshold to determine whether points are inliers or not when
236 * testing possible estimation solutions.
237 */
238 public double getThreshold() {
239 return threshold;
240 }
241
242 /**
243 * Sets threshold to determine whether points are inliers or not when
244 * testing possible estimation solutions.
245 * The threshold refers to the amount of error (i.e. Euclidean distance) a
246 * possible solution has on projected 2D points.
247 *
248 * @param threshold threshold to be set.
249 * @throws IllegalArgumentException if provided value is equal or less than zero.
250 * @throws LockedException if robust estimator is locked because an
251 * estimation is already in progress.
252 */
253 public void setThreshold(final double threshold) throws LockedException {
254 if (isLocked()) {
255 throw new LockedException();
256 }
257 if (threshold <= MIN_THRESHOLD) {
258 throw new IllegalArgumentException();
259 }
260 this.threshold = threshold;
261 }
262
263 /**
264 * Indicates whether inliers must be computed and kept.
265 *
266 * @return true if inliers must be computed and kept, false if inliers
267 * only need to be computed but not kept.
268 */
269 public boolean isComputeAndKeepInliersEnabled() {
270 return computeAndKeepInliers;
271 }
272
273 /**
274 * Specifies whether inliers must be computed and kept.
275 *
276 * @param computeAndKeepInliers true if inliers must be computed and kept,
277 * false if inliers only need to be computed but not kept.
278 * @throws LockedException if estimator is locked.
279 */
280 public void setComputeAndKeepInliersEnabled(final boolean computeAndKeepInliers) throws LockedException {
281 if (isLocked()) {
282 throw new LockedException();
283 }
284 this.computeAndKeepInliers = computeAndKeepInliers;
285 }
286
287 /**
288 * Indicates whether residuals must be computed and kept.
289 *
290 * @return true if residuals must be computed and kept, false if residuals
291 * only need to be computed but not kept.
292 */
293 public boolean isComputeAndKeepResidualsEnabled() {
294 return computeAndKeepResiduals;
295 }
296
297 /**
298 * Specifies whether residuals must be computed and kept.
299 *
300 * @param computeAndKeepResiduals true if residuals must be computed and
301 * kept, false if residuals only need to be computed but not kept.
302 * @throws LockedException if estimator is locked.
303 */
304 public void setComputeAndKeepResidualsEnabled(final boolean computeAndKeepResiduals) throws LockedException {
305 if (isLocked()) {
306 throw new LockedException();
307 }
308 this.computeAndKeepResiduals = computeAndKeepResiduals;
309 }
310
311 /**
312 * Estimates a pinhole camera using a robust estimator and
313 * the best set of matched 2D/3D point correspondences or 2D line/3D plane
314 * correspondences found using the robust estimator.
315 *
316 * @return a pinhole camera.
317 * @throws LockedException if robust estimator is locked because an
318 * estimation is already in progress.
319 * @throws NotReadyException if provided input data is not enough to start
320 * the estimation.
321 * @throws RobustEstimatorException if estimation fails for any reason
322 * (i.e. numerical instability, no solution available, etc).
323 */
324 @Override
325 public PinholeCamera estimate() throws LockedException, NotReadyException, RobustEstimatorException {
326 if (isLocked()) {
327 throw new LockedException();
328 }
329 if (!isReady()) {
330 throw new NotReadyException();
331 }
332
333 // pinhole camera estimator using EPnP (Efficient Perspective-n-Point) algorithm
334 final var nonRobustEstimator = new EPnPPointCorrespondencePinholeCameraEstimator(intrinsic);
335
336 nonRobustEstimator.setPlanarConfigurationAllowed(planarConfigurationAllowed);
337 nonRobustEstimator.setNullspaceDimension2Allowed(nullspaceDimension2Allowed);
338 nonRobustEstimator.setNullspaceDimension3Allowed(nullspaceDimension3Allowed);
339 nonRobustEstimator.setPlanarThreshold(planarThreshold);
340
341 // suggestions
342 nonRobustEstimator.setSuggestSkewnessValueEnabled(isSuggestSkewnessValueEnabled());
343 nonRobustEstimator.setSuggestedSkewnessValue(getSuggestedSkewnessValue());
344 nonRobustEstimator.setSuggestHorizontalFocalLengthEnabled(isSuggestHorizontalFocalLengthEnabled());
345 nonRobustEstimator.setSuggestedHorizontalFocalLengthValue(getSuggestedHorizontalFocalLengthValue());
346 nonRobustEstimator.setSuggestVerticalFocalLengthEnabled(isSuggestVerticalFocalLengthEnabled());
347 nonRobustEstimator.setSuggestedVerticalFocalLengthValue(getSuggestedVerticalFocalLengthValue());
348 nonRobustEstimator.setSuggestAspectRatioEnabled(isSuggestAspectRatioEnabled());
349 nonRobustEstimator.setSuggestedAspectRatioValue(getSuggestedAspectRatioValue());
350 nonRobustEstimator.setSuggestPrincipalPointEnabled(isSuggestPrincipalPointEnabled());
351 nonRobustEstimator.setSuggestedPrincipalPointValue(getSuggestedPrincipalPointValue());
352 nonRobustEstimator.setSuggestRotationEnabled(isSuggestRotationEnabled());
353 nonRobustEstimator.setSuggestedRotationValue(getSuggestedRotationValue());
354 nonRobustEstimator.setSuggestCenterEnabled(isSuggestCenterEnabled());
355 nonRobustEstimator.setSuggestedCenterValue(getSuggestedCenterValue());
356
357 final var innerEstimator = new RANSACRobustEstimator<>(new RANSACRobustEstimatorListener<PinholeCamera>() {
358
359 // point to be reused when computing residuals
360 private final Point2D testPoint = Point2D.create(CoordinatesType.HOMOGENEOUS_COORDINATES);
361
362 // 3D points for a subset of samples
363 private final List<Point3D> subset3D = new ArrayList<>();
364
365 // 2D points for a subset of samples
366 private final List<Point2D> subset2D = new ArrayList<>();
367
368 @Override
369 public double getThreshold() {
370 return threshold;
371 }
372
373 @Override
374 public int getTotalSamples() {
375 return points3D.size();
376 }
377
378 @Override
379 public int getSubsetSize() {
380 return PointCorrespondencePinholeCameraEstimator.MIN_NUMBER_OF_POINT_CORRESPONDENCES;
381 }
382
383 @Override
384 public void estimatePreliminarSolutions(final int[] samplesIndices, final List<PinholeCamera> solutions) {
385 subset3D.clear();
386 subset3D.add(points3D.get(samplesIndices[0]));
387 subset3D.add(points3D.get(samplesIndices[1]));
388 subset3D.add(points3D.get(samplesIndices[2]));
389 subset3D.add(points3D.get(samplesIndices[3]));
390 subset3D.add(points3D.get(samplesIndices[4]));
391 subset3D.add(points3D.get(samplesIndices[5]));
392
393 subset2D.clear();
394 subset2D.add(points2D.get(samplesIndices[0]));
395 subset2D.add(points2D.get(samplesIndices[1]));
396 subset2D.add(points2D.get(samplesIndices[2]));
397 subset2D.add(points2D.get(samplesIndices[3]));
398 subset2D.add(points2D.get(samplesIndices[4]));
399 subset2D.add(points2D.get(samplesIndices[5]));
400
401 try {
402 nonRobustEstimator.setLists(subset3D, subset2D);
403
404 final var cam = nonRobustEstimator.estimate();
405 solutions.add(cam);
406 } catch (final Exception e) {
407 // if points configuration is degenerate, no solution is added
408 }
409 }
410
411 @Override
412 public double computeResidual(final PinholeCamera currentEstimation, int i) {
413 // pick i-th points
414 final var point3D = points3D.get(i);
415 final var point2D = points2D.get(i);
416
417 // project point3D into test point
418 currentEstimation.project(point3D, testPoint);
419
420 // compare test point and 2D point
421 return testPoint.distanceTo(point2D);
422 }
423
424 @Override
425 public boolean isReady() {
426 return RANSACEPnPPointCorrespondencePinholeCameraRobustEstimator.this.isReady();
427 }
428
429 @Override
430 public void onEstimateStart(final RobustEstimator<PinholeCamera> estimator) {
431 if (listener != null) {
432 listener.onEstimateStart(RANSACEPnPPointCorrespondencePinholeCameraRobustEstimator.this);
433 }
434 }
435
436 @Override
437 public void onEstimateEnd(final RobustEstimator<PinholeCamera> estimator) {
438 if (listener != null) {
439 listener.onEstimateEnd(RANSACEPnPPointCorrespondencePinholeCameraRobustEstimator.this);
440 }
441 }
442
443 @Override
444 public void onEstimateNextIteration(final RobustEstimator<PinholeCamera> estimator, final int iteration) {
445 if (listener != null) {
446 listener.onEstimateNextIteration(
447 RANSACEPnPPointCorrespondencePinholeCameraRobustEstimator.this, iteration);
448 }
449 }
450
451 @Override
452 public void onEstimateProgressChange(final RobustEstimator<PinholeCamera> estimator, final float progress) {
453 if (listener != null) {
454 listener.onEstimateProgressChange(
455 RANSACEPnPPointCorrespondencePinholeCameraRobustEstimator.this, progress);
456 }
457 }
458 });
459
460 try {
461 locked = true;
462 inliersData = null;
463 innerEstimator.setComputeAndKeepInliersEnabled(computeAndKeepInliers || refineResult);
464 innerEstimator.setComputeAndKeepResidualsEnabled(computeAndKeepResiduals || refineResult);
465 innerEstimator.setConfidence(confidence);
466 innerEstimator.setMaxIterations(maxIterations);
467 innerEstimator.setProgressDelta(progressDelta);
468 final var result = innerEstimator.estimate();
469 inliersData = innerEstimator.getInliersData();
470 return attemptRefine(result, nonRobustEstimator.getMaxSuggestionWeight());
471 } catch (final com.irurueta.numerical.LockedException e) {
472 throw new LockedException(e);
473 } catch (final com.irurueta.numerical.NotReadyException e) {
474 throw new NotReadyException(e);
475 } finally {
476 locked = false;
477 }
478 }
479
480 /**
481 * Returns method being used for robust estimation.
482 *
483 * @return method being used for robust estimation.
484 */
485 @Override
486 public RobustEstimatorMethod getMethod() {
487 return RobustEstimatorMethod.RANSAC;
488 }
489
490 /**
491 * Gets standard deviation used for Levenberg-Marquardt fitting during
492 * refinement.
493 * Returned value gives an indication of how much variance each residual
494 * has.
495 * Typically, this value is related to the threshold used on each robust
496 * estimation, since residuals of found inliers are within the range of
497 * such threshold.
498 *
499 * @return standard deviation used for refinement.
500 */
501 @Override
502 protected double getRefinementStandardDeviation() {
503 return threshold;
504 }
505 }