1 /*
2 * Copyright (C) 2016 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.ar.calibration.estimators;
17
18 import com.irurueta.ar.calibration.DualAbsoluteQuadric;
19 import com.irurueta.geometry.PinholeCamera;
20 import com.irurueta.geometry.estimators.LockedException;
21 import com.irurueta.geometry.estimators.NotReadyException;
22 import com.irurueta.numerical.robust.RANSACRobustEstimator;
23 import com.irurueta.numerical.robust.RANSACRobustEstimatorListener;
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 dual absolute quadric (DAQ) using RANSAC algorithm.
33 */
34 public class RANSACDualAbsoluteQuadricRobustEstimator extends DualAbsoluteQuadricRobustEstimator {
35
36 /**
37 * Constant defining default threshold to determine whether cameras are
38 * inliers or not.
39 * Threshold is defined by the equations used to estimate the DAQ depending
40 * on the required settings (zero skewness, principal point at origin, and
41 * known aspect ratio).
42 */
43 public static final double DEFAULT_THRESHOLD = 1e-3;
44
45 /**
46 * Minimum value that can be set as threshold.
47 * Threshold must be strictly greater than 0.0.
48 */
49 public static final double MIN_THRESHOLD = 0.0;
50
51 /**
52 * Threshold to determine whether cameras are inliers or not when
53 * testing possible estimation solutions.
54 * The threshold refers to the amount of error a possible solution has.
55 */
56 private double threshold;
57
58 /**
59 * Constructor.
60 */
61 public RANSACDualAbsoluteQuadricRobustEstimator() {
62 super();
63 threshold = DEFAULT_THRESHOLD;
64 }
65
66 /**
67 * Constructor.
68 *
69 * @param listener listener to be notified of events such as when estimation
70 * starts, ends or its progress significantly changes.
71 */
72 public RANSACDualAbsoluteQuadricRobustEstimator(final DualAbsoluteQuadricRobustEstimatorListener listener) {
73 super(listener);
74 threshold = DEFAULT_THRESHOLD;
75 }
76
77 /**
78 * Constructor.
79 *
80 * @param cameras list of cameras used to estimate the dual absolute quadric
81 * (DAQ), which can be used to obtain pinhole camera intrinsic parameters.
82 * @throws IllegalArgumentException if not enough cameras are provided for
83 * default settings. Hence, at least 2 cameras must be provided.
84 */
85 public RANSACDualAbsoluteQuadricRobustEstimator(final List<PinholeCamera> cameras) {
86 super(cameras);
87 threshold = DEFAULT_THRESHOLD;
88 }
89
90 /**
91 * Constructor.
92 *
93 * @param cameras list of cameras used to estimate the dual absolute quadric
94 * (DAQ), which can be used to obtain pinhole camera intrinsic parameters.
95 * @param listener listener to be notified of events such as when estimation
96 * starts, ends or estimation progress changes.
97 * @throws IllegalArgumentException if not enough cameras are provided for
98 * default settings. Hence, at least 2 cameras must be provided.
99 */
100 public RANSACDualAbsoluteQuadricRobustEstimator(
101 final List<PinholeCamera> cameras, final DualAbsoluteQuadricRobustEstimatorListener listener) {
102 super(cameras, listener);
103 threshold = DEFAULT_THRESHOLD;
104 }
105
106 /**
107 * Returns threshold to determine whether cameras are inliers or not
108 * when testing possible estimation solutions.
109 * The threshold refers to the amount of error a possible solution has.
110 *
111 * @return threshold to determine whether cameras are inliers or not.
112 */
113 public double getThreshold() {
114 return threshold;
115 }
116
117 /**
118 * Sets threshold to determine whether cameras are inliers or not when
119 * testing possible estimation solutions.
120 * The threshold refers to the amount of error a possible solution has.
121 *
122 * @param threshold threshold to determine whether cameras are inliers or
123 * not when testing possible estimation solutions.
124 * @throws IllegalArgumentException if provided value is equal or less than
125 * zero.
126 * @throws LockedException if robust estimator is locked because an
127 * estimation is already in progress.
128 */
129 public void setThreshold(final double threshold) throws LockedException {
130 if (isLocked()) {
131 throw new LockedException();
132 }
133 if (threshold <= MIN_THRESHOLD) {
134 throw new IllegalArgumentException();
135 }
136 this.threshold = threshold;
137 }
138
139 /**
140 * Estimates the Dual Absolute Quadric using provided cameras.
141 *
142 * @return estimated Dual Absolute Quadric (DAQ).
143 * @throws LockedException if robust estimator is locked.
144 * @throws NotReadyException if no valid input data has already been
145 * provided.
146 * @throws RobustEstimatorException if estimation fails for any reason
147 * (i.e. numerical instability, no solution available, etc).
148 */
149 @SuppressWarnings("DuplicatedCode")
150 @Override
151 public DualAbsoluteQuadric estimate() throws LockedException, NotReadyException, RobustEstimatorException {
152 if (isLocked()) {
153 throw new LockedException();
154 }
155 if (!isReady()) {
156 throw new NotReadyException();
157 }
158
159 final var innerEstimator = new RANSACRobustEstimator<DualAbsoluteQuadric>(
160 new RANSACRobustEstimatorListener<>() {
161
162 // subset of cameras picked on each iteration
163 private final List<PinholeCamera> subsetCameras = new ArrayList<>();
164
165 @Override
166 public double getThreshold() {
167 return threshold;
168 }
169
170 @Override
171 public int getTotalSamples() {
172 return cameras.size();
173 }
174
175 @Override
176 public int getSubsetSize() {
177 return daqEstimator.getMinNumberOfRequiredCameras();
178 }
179
180 @Override
181 public void estimatePreliminarSolutions(
182 final int[] samplesIndices, final List<DualAbsoluteQuadric> solutions) {
183 subsetCameras.clear();
184 for (final var samplesIndex : samplesIndices) {
185 subsetCameras.add(cameras.get(samplesIndex));
186 }
187
188 try {
189 daqEstimator.setLMSESolutionAllowed(false);
190 daqEstimator.setCameras(subsetCameras);
191
192 final var daq = daqEstimator.estimate();
193 solutions.add(daq);
194 } catch (final Exception e) {
195 // if anything fails, no solution is added
196 }
197 }
198
199 @Override
200 public double computeResidual(final DualAbsoluteQuadric currentEstimation, final int i) {
201 return residual(currentEstimation, cameras.get(i));
202 }
203
204 @Override
205 public boolean isReady() {
206 return RANSACDualAbsoluteQuadricRobustEstimator.this.isReady();
207 }
208
209 @Override
210 public void onEstimateStart(final RobustEstimator<DualAbsoluteQuadric> estimator) {
211 if (listener != null) {
212 listener.onEstimateStart(RANSACDualAbsoluteQuadricRobustEstimator.this);
213 }
214 }
215
216 @Override
217 public void onEstimateEnd(final RobustEstimator<DualAbsoluteQuadric> estimator) {
218 if (listener != null) {
219 listener.onEstimateEnd(RANSACDualAbsoluteQuadricRobustEstimator.this);
220 }
221 }
222
223 @Override
224 public void onEstimateNextIteration(
225 final RobustEstimator<DualAbsoluteQuadric> estimator, final int iteration) {
226 if (listener != null) {
227 listener.onEstimateNextIteration(
228 RANSACDualAbsoluteQuadricRobustEstimator.this, iteration);
229 }
230 }
231
232 @Override
233 public void onEstimateProgressChange(
234 final RobustEstimator<DualAbsoluteQuadric> estimator, final float progress) {
235 if (listener != null) {
236 listener.onEstimateProgressChange(
237 RANSACDualAbsoluteQuadricRobustEstimator.this, progress);
238 }
239 }
240 });
241
242 try {
243 locked = true;
244 innerEstimator.setConfidence(confidence);
245 innerEstimator.setMaxIterations(maxIterations);
246 innerEstimator.setProgressDelta(progressDelta);
247 return innerEstimator.estimate();
248 } catch (final com.irurueta.numerical.LockedException e) {
249 throw new LockedException(e);
250 } catch (final com.irurueta.numerical.NotReadyException e) {
251 throw new NotReadyException(e);
252 } finally {
253 locked = false;
254 }
255 }
256
257 /**
258 * Returns method being used for robust estimation.
259 *
260 * @return method being used for robust estimation.
261 */
262 @Override
263 public RobustEstimatorMethod getMethod() {
264 return RobustEstimatorMethod.RANSAC;
265 }
266 }