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