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