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