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