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.MSACRobustEstimator;
23 import com.irurueta.numerical.robust.MSACRobustEstimatorListener;
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) for provided collection of
33 * homographies (2D transformations) using MSAC algorithm.
34 */
35 public class MSACImageOfAbsoluteConicRobustEstimator extends ImageOfAbsoluteConicRobustEstimator {
36
37 /**
38 * Constant defining default threshold to determine whether homographies are
39 * inliers or not.
40 * Threshold is defined by equations h1'*IAC*h2 = 0 and
41 * h1'*IAC*h1 = h2'*IAC*h2 --< h1'*IAC*h1 - h2'*IAC*h2 = 0, where
42 * h1 and h2 are the 1st and 2nd columns of an homography (2D
43 * transformation).
44 * These equations are derived from the fact that rotation matrices are
45 * orthonormal.
46 */
47 public static final double DEFAULT_THRESHOLD = 1e-6;
48
49 /**
50 * Minimum value that can be set as threshold.
51 * Threshold must be strictly greater than 0.0.
52 */
53 public static final double MIN_THRESHOLD = 0.0;
54
55 /**
56 * Threshold to determine whether homographies are inliers or not when
57 * testing possible estimation solutions.
58 * The threshold refers to the amount of error a possible solution has on
59 * the ortho-normality assumption of rotation matrices.
60 */
61 private double threshold;
62
63 /**
64 * Constructor.
65 */
66 public MSACImageOfAbsoluteConicRobustEstimator() {
67 super();
68 threshold = DEFAULT_THRESHOLD;
69 }
70
71 /**
72 * Constructor.
73 *
74 * @param listener listener to be notified of events such as when
75 * estimation starts, ends or its progress significantly changes.
76 */
77 public MSACImageOfAbsoluteConicRobustEstimator(final ImageOfAbsoluteConicRobustEstimatorListener listener) {
78 super(listener);
79 threshold = DEFAULT_THRESHOLD;
80 }
81
82 /**
83 * Constructor.
84 *
85 * @param homographies list of homographies (2D transformations) used to
86 * estimate the image of absolute conic (IAC), which can be used to obtain
87 * pinhole camera intrinsic parameters.
88 * @throws IllegalArgumentException if not enough homographies are provided
89 * for default settings. Hence, at least 1 homography must be provided.
90 */
91 public MSACImageOfAbsoluteConicRobustEstimator(final List<Transformation2D> homographies) {
92 super(homographies);
93 threshold = DEFAULT_THRESHOLD;
94 }
95
96 /**
97 * Constructor.
98 *
99 * @param homographies list of homographies (2D transformations) used to
100 * estimate the image of absolute conic (IAC), which can be used to obtain
101 * pinhole camera intrinsic parameters.
102 * @param listener listener to be notified of events such as when estimation
103 * starts, ends or estimation progress changes.
104 * @throws IllegalArgumentException if not enough homographies are provided
105 * for default settings. Hence, at least 1 homography must be provided.
106 */
107 public MSACImageOfAbsoluteConicRobustEstimator(
108 final List<Transformation2D> homographies, final ImageOfAbsoluteConicRobustEstimatorListener listener) {
109 super(homographies, listener);
110 threshold = DEFAULT_THRESHOLD;
111 }
112
113 /**
114 * Returns threshold to determine whether homographies are inliers or not
115 * when testing possible estimation solutions.
116 * The threshold refers to the amount of error a possible solution has on
117 * the ortho-normality assumption of rotation matrices.
118 *
119 * @return threshold to determine whether homographies are inliers or not
120 * when testing possible estimation solutions.
121 */
122 public double getThreshold() {
123 return threshold;
124 }
125
126 /**
127 * Sets threshold to determine whether homographies are inliers or not when
128 * testing possible estimation solutions.
129 * The threshold refers to the amount of error a possible solution has on
130 * the ortho-normality assumption of rotation matrices.
131 *
132 * @param threshold threshold to determine whether homographies are inliers
133 * or not when testing possible estimation solutions.
134 * @throws IllegalArgumentException if provided value is equal or less than
135 * zero.
136 * @throws LockedException if robust estimator is locked because an
137 * estimation is already in progress.
138 */
139 public void setThreshold(final double threshold) throws LockedException {
140 if (isLocked()) {
141 throw new LockedException();
142 }
143 if (threshold <= MIN_THRESHOLD) {
144 throw new IllegalArgumentException();
145 }
146 this.threshold = threshold;
147 }
148
149 /**
150 * Estimates Image of Absolute Conic (IAC).
151 *
152 * @return estimated IAC.
153 * @throws LockedException if robust estimator is locked because an
154 * estimation is already in progress.
155 * @throws NotReadyException if provided input data is not enough to start
156 * the estimation.
157 * @throws RobustEstimatorException if estimation fails for any reason
158 * (i.e. numerical instability, no solution available, etc).
159 */
160 @SuppressWarnings("DuplicatedCode")
161 @Override
162 public ImageOfAbsoluteConic estimate() throws LockedException, NotReadyException, RobustEstimatorException {
163 if (isLocked()) {
164 throw new LockedException();
165 }
166 if (!isReady()) {
167 throw new NotReadyException();
168 }
169
170 final var innerEstimator = new MSACRobustEstimator<ImageOfAbsoluteConic>(new MSACRobustEstimatorListener<>() {
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 (final 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 MSACImageOfAbsoluteConicRobustEstimator.this.isReady();
217 }
218
219 @Override
220 public void onEstimateStart(final RobustEstimator<ImageOfAbsoluteConic> estimator) {
221 if (listener != null) {
222 listener.onEstimateStart(MSACImageOfAbsoluteConicRobustEstimator.this);
223 }
224 }
225
226 @Override
227 public void onEstimateEnd(final RobustEstimator<ImageOfAbsoluteConic> estimator) {
228 if (listener != null) {
229 listener.onEstimateEnd(MSACImageOfAbsoluteConicRobustEstimator.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(MSACImageOfAbsoluteConicRobustEstimator.this, iteration);
238 }
239 }
240
241 @Override
242 public void onEstimateProgressChange(
243 final RobustEstimator<ImageOfAbsoluteConic> estimator, final float progress) {
244 if (listener != null) {
245 listener.onEstimateProgressChange(MSACImageOfAbsoluteConicRobustEstimator.this, progress);
246 }
247 }
248 });
249
250 try {
251 locked = true;
252 innerEstimator.setConfidence(confidence);
253 innerEstimator.setMaxIterations(maxIterations);
254 innerEstimator.setProgressDelta(progressDelta);
255 return innerEstimator.estimate();
256 } catch (final com.irurueta.numerical.LockedException e) {
257 throw new LockedException(e);
258 } catch (final com.irurueta.numerical.NotReadyException e) {
259 throw new NotReadyException(e);
260 } finally {
261 locked = false;
262 }
263 }
264
265 /**
266 * Returns method being used for robust estimation
267 *
268 * @return method being used for robust estimation
269 */
270 @Override
271 public RobustEstimatorMethod getMethod() {
272 return RobustEstimatorMethod.MSAC;
273 }
274 }