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.algebra.AlgebraException;
19 import com.irurueta.geometry.CoincidentPlanesException;
20 import com.irurueta.geometry.Plane;
21 import com.irurueta.geometry.ProjectiveTransformation3D;
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.List;
29
30 /**
31 * Finds the best projective 3D transformation for provided collections of
32 * matched 3D planes using MSAC algorithm.
33 */
34 @SuppressWarnings("DuplicatedCode")
35 public class MSACPlaneCorrespondenceProjectiveTransformation3DRobustEstimator
36 extends PlaneCorrespondenceProjectiveTransformation3DRobustEstimator {
37
38 /**
39 * Constant defining default threshold to determine whether planes are
40 * inliers or not.
41 * Residuals to determine whether planes are inliers or not are computed by
42 * comparing two planes algebraically (e.g. doing the dot product of their
43 * parameters).
44 * A residual of 0 indicates that dot product was 1 or -1 and planes were
45 * equal.
46 * A residual of 1 indicates that dot product was 0 and planes were
47 * orthogonal.
48 * If dot product between planes is -1, then although their director vectors
49 * are opposed, planes are considered equal, since sign changes are not
50 * taken into account and their residuals will be 0.
51 */
52 public static final double DEFAULT_THRESHOLD = 1e-6;
53
54 /**
55 * Minimum value that can be set as threshold.
56 * Threshold must be strictly greater than 0.0.
57 */
58 public static final double MIN_THRESHOLD = 0.0;
59
60 /**
61 * Threshold to determine whether planes are inliers or not when testing
62 * possible estimation solutions.
63 * The threshold refers to the amount of error (i.e. distance and director
64 * vector angle difference) a possible solution has on a matched pair of
65 * planes.
66 */
67 private double threshold;
68
69 /**
70 * Constructor.
71 */
72 public MSACPlaneCorrespondenceProjectiveTransformation3DRobustEstimator() {
73 super();
74 threshold = DEFAULT_THRESHOLD;
75 }
76
77 /**
78 * Constructor with lists of planes to be used to estimate a projective 3D
79 * transformation.
80 * Planes in the list located at the same position are considered to be
81 * matched. Hence, both lists must have the same size, and their size must
82 * be greater or equal than MINIMUM_SIZE.
83 *
84 * @param inputPlanes list of input planes to be used to estimate a
85 * projective 3D transformation.
86 * @param outputPlanes list of output planes to be used to estimate a
87 * projective 3D transformation.
88 * @throws IllegalArgumentException if provided lists of planes don't have
89 * the same size or their size is smaller than MINIMUM_SIZE.
90 */
91 public MSACPlaneCorrespondenceProjectiveTransformation3DRobustEstimator(
92 final List<Plane> inputPlanes, final List<Plane> outputPlanes) {
93 super(inputPlanes, outputPlanes);
94 threshold = DEFAULT_THRESHOLD;
95 }
96
97 /**
98 * Constructor.
99 *
100 * @param listener listener to be notified of events such as when estimation
101 * starts, ends or its progress significantly changes.
102 */
103 public MSACPlaneCorrespondenceProjectiveTransformation3DRobustEstimator(
104 final ProjectiveTransformation3DRobustEstimatorListener listener) {
105 super(listener);
106 threshold = DEFAULT_THRESHOLD;
107 }
108
109 /**
110 * Constructor with listener and lists of planes to be used to estimate a
111 * projective 3D transformation.
112 * Planes in the list located at the same position are considered to be
113 * matched. Hence, both lists must have the same size, and their size must
114 * be greater or equal than MINIMUM_SIZE.
115 *
116 * @param listener listener to be notified of events such as when estimation
117 * starts, ends or its progress significantly changes.
118 * @param inputPlanes list of input planes to be used to estimate a projective
119 * 3D transformation.
120 * @param outputPlanes list of output planes to be used to estimate a
121 * projective 3D transformation.
122 * @throws IllegalArgumentException if provided lists of planes don't have
123 * the same size or their size is smaller than MINIMUM_SIZE.
124 */
125 public MSACPlaneCorrespondenceProjectiveTransformation3DRobustEstimator(
126 final ProjectiveTransformation3DRobustEstimatorListener listener,
127 final List<Plane> inputPlanes, final List<Plane> outputPlanes) {
128 super(listener, inputPlanes, outputPlanes);
129 threshold = DEFAULT_THRESHOLD;
130 }
131
132 /**
133 * Returns threshold to determine whether planes are inliers or not when
134 * testing possible estimation solutions.
135 * Residuals to determine whether planes are inliers or not are computed by
136 * comparing two lines algebraically (e.g. doing the dot product of their
137 * parameters).
138 * A residual of 0 indicates that dot product was 1 or -1 and planes were
139 * equal.
140 * A residual of 1 indicates that dot product was 0 and planes were
141 * orthogonal.
142 * If dot product between planes is -1, then although their director vectors
143 * are opposed, planes are considered equal, since sign changes are not
144 * taken into account and their residuals will be 0.
145 *
146 * @return threshold to determine whether matched planes are inliers or not.
147 */
148 public double getThreshold() {
149 return threshold;
150 }
151
152 /**
153 * Sets threshold to determine whether planes are inliers or not when
154 * testing possible estimation solutions.
155 * Residuals to determine whether planes are inliers or not are computed by
156 * comparing two lines algebraically (e.g. doing the dot product of their
157 * parameters).
158 * A residual of 0 indicates that dot product was 1 or -1 and planes were
159 * equal.
160 * A residual of 1 indicates that dot product was 0 and planes were
161 * orthogonal.
162 * If dot product between planes is -1, then although their director vectors
163 * are opposed, planes are considered equal, since sign changes are not
164 * taken into account and their residuals will be 0.
165 *
166 * @param threshold threshold to determine whether matched planes are
167 * inliers or not.
168 * @throws IllegalArgumentException if provided value is equal or less than
169 * zero.
170 * @throws LockedException if robust estimator is locked because an
171 * estimation is already in progress.
172 */
173 public void setThreshold(final double threshold) throws LockedException {
174 if (isLocked()) {
175 throw new LockedException();
176 }
177 if (threshold <= MIN_THRESHOLD) {
178 throw new IllegalArgumentException();
179 }
180 this.threshold = threshold;
181 }
182
183 /**
184 * Estimates a projective 3D transformation using a robust estimator and
185 * the best set of matched 3D planes correspondences found using the robust
186 * estimator.
187 *
188 * @return a projective 3D transformation.
189 * @throws LockedException if robust estimator is locked because an
190 * estimation is already in progress.
191 * @throws NotReadyException if provided input data is not enough to start
192 * the estimation.
193 * @throws RobustEstimatorException if estimation fails for any reason
194 * (i.e. numerical instability, no solution available, etc).
195 */
196 @Override
197 public ProjectiveTransformation3D estimate() throws LockedException, NotReadyException, RobustEstimatorException {
198 if (isLocked()) {
199 throw new LockedException();
200 }
201 if (!isReady()) {
202 throw new NotReadyException();
203 }
204
205 final var innerEstimator = new MSACRobustEstimator<>(
206 new MSACRobustEstimatorListener<ProjectiveTransformation3D>() {
207
208 // plane to be reused when computing residuals
209 private final Plane testPlane = new Plane();
210
211 @Override
212 public double getThreshold() {
213 return threshold;
214 }
215
216 @Override
217 public int getTotalSamples() {
218 return inputPlanes.size();
219 }
220
221 @Override
222 public int getSubsetSize() {
223 return ProjectiveTransformation3DRobustEstimator.MINIMUM_SIZE;
224 }
225
226 @Override
227 public void estimatePreliminarSolutions(
228 final int[] samplesIndices, final List<ProjectiveTransformation3D> solutions) {
229 final var inputPlane1 = inputPlanes.get(samplesIndices[0]);
230 final var inputPlane2 = inputPlanes.get(samplesIndices[1]);
231 final var inputPlane3 = inputPlanes.get(samplesIndices[2]);
232 final var inputPlane4 = inputPlanes.get(samplesIndices[3]);
233 final var inputPlane5 = inputPlanes.get(samplesIndices[4]);
234
235 final var outputPlane1 = outputPlanes.get(samplesIndices[0]);
236 final var outputPlane2 = outputPlanes.get(samplesIndices[1]);
237 final var outputPlane3 = outputPlanes.get(samplesIndices[2]);
238 final var outputPlane4 = outputPlanes.get(samplesIndices[3]);
239 final var outputPlane5 = outputPlanes.get(samplesIndices[4]);
240
241 try {
242 final var transformation = new ProjectiveTransformation3D(inputPlane1, inputPlane2,
243 inputPlane3, inputPlane4, inputPlane5, outputPlane1, outputPlane2, outputPlane3,
244 outputPlane4, outputPlane5);
245 solutions.add(transformation);
246 } catch (final CoincidentPlanesException e) {
247 // if lines are coincident, no solution is added
248 }
249 }
250
251 @Override
252 public double computeResidual(final ProjectiveTransformation3D currentEstimation, final int i) {
253 final var inputPlane = inputPlanes.get(i);
254 final var outputPlane = outputPlanes.get(i);
255
256 // transform input plane and store result in mTestPlane
257 try {
258 currentEstimation.transform(inputPlane, testPlane);
259
260 return getResidual(outputPlane, testPlane);
261 } catch (final AlgebraException e) {
262 // this happens when internal matrix of affine transformation
263 // cannot be reverse (i.e. transformation is not well-defined,
264 // numerical instabilities, etc.)
265 return Double.MAX_VALUE;
266 }
267 }
268
269 @Override
270 public boolean isReady() {
271 return MSACPlaneCorrespondenceProjectiveTransformation3DRobustEstimator.this.isReady();
272 }
273
274 @Override
275 public void onEstimateStart(final RobustEstimator<ProjectiveTransformation3D> estimator) {
276 if (listener != null) {
277 listener.onEstimateStart(
278 MSACPlaneCorrespondenceProjectiveTransformation3DRobustEstimator.this);
279 }
280 }
281
282 @Override
283 public void onEstimateEnd(final RobustEstimator<ProjectiveTransformation3D> estimator) {
284 if (listener != null) {
285 listener.onEstimateEnd(
286 MSACPlaneCorrespondenceProjectiveTransformation3DRobustEstimator.this);
287 }
288 }
289
290 @Override
291 public void onEstimateNextIteration(
292 final RobustEstimator<ProjectiveTransformation3D> estimator, final int iteration) {
293 if (listener != null) {
294 listener.onEstimateNextIteration(
295 MSACPlaneCorrespondenceProjectiveTransformation3DRobustEstimator.this,
296 iteration);
297 }
298 }
299
300 @Override
301 public void onEstimateProgressChange(
302 final RobustEstimator<ProjectiveTransformation3D> estimator, final float progress) {
303 if (listener != null) {
304 listener.onEstimateProgressChange(
305 MSACPlaneCorrespondenceProjectiveTransformation3DRobustEstimator.this,
306 progress);
307 }
308 }
309 });
310
311 try {
312 locked = true;
313 inliersData = null;
314 innerEstimator.setConfidence(confidence);
315 innerEstimator.setMaxIterations(maxIterations);
316 innerEstimator.setProgressDelta(progressDelta);
317 final var transformation = innerEstimator.estimate();
318 inliersData = innerEstimator.getInliersData();
319 return attemptRefine(transformation);
320 } catch (final com.irurueta.numerical.LockedException e) {
321 throw new LockedException(e);
322 } catch (final com.irurueta.numerical.NotReadyException e) {
323 throw new NotReadyException(e);
324 } finally {
325 locked = false;
326 }
327 }
328
329 /**
330 * Returns method being used for robust estimation.
331 *
332 * @return method being used for robust estimation.
333 */
334 @Override
335 public RobustEstimatorMethod getMethod() {
336 return RobustEstimatorMethod.MSAC;
337 }
338
339 /**
340 * Gets standard deviation used for Levenberg-Marquardt fitting during
341 * refinement.
342 * Returned value gives an indication of how much variance each residual
343 * has.
344 * Typically, this value is related to the threshold used on each robust
345 * estimation, since residuals of found inliers are within the range of
346 * such threshold.
347 *
348 * @return standard deviation used for refinement.
349 */
350 @Override
351 protected double getRefinementStandardDeviation() {
352 return threshold;
353 }
354 }