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