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