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.geometry.CoincidentPointsException;
19 import com.irurueta.geometry.CoordinatesType;
20 import com.irurueta.geometry.Point3D;
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 2D points using MSAC algorithm.
33 */
34 @SuppressWarnings("DuplicatedCode")
35 public class MSACPointCorrespondenceProjectiveTransformation3DRobustEstimator
36 extends PointCorrespondenceProjectiveTransformation3DRobustEstimator {
37
38 /**
39 * Constant defining default threshold to determine whether points are
40 * inliers or not.
41 * By default, 1.0 is considered a good value for cases where measures are
42 * done on pixels, since typically the minimum resolution is 1 pixel.
43 */
44 public static final double DEFAULT_THRESHOLD = 1.0;
45
46 /**
47 * Minimum value that can be set as threshold.
48 * Threshold must be strictly greater than 0.0.
49 */
50 public static final double MIN_THRESHOLD = 0.0;
51
52 /**
53 * Threshold to determine whether points are inliers or not when testing
54 * possible estimation solutions.
55 * The threshold refers to the amount of error (i.e. distance) a possible
56 * solution has on a matched pair of points.
57 */
58 private double threshold;
59
60 /**
61 * Constructor.
62 */
63 public MSACPointCorrespondenceProjectiveTransformation3DRobustEstimator() {
64 super();
65 threshold = DEFAULT_THRESHOLD;
66 }
67
68 /**
69 * Constructor with lists of points to be used to estimate a projective 3D
70 * transformation.
71 * Points in the list located at the same position are considered to be
72 * matched. Hence, both lists must have the same size, and their size must
73 * be greater or equal than MINIMUM_SIZE.
74 *
75 * @param inputPoints list of input points to be used to estimate a
76 * projective 3D transformation.
77 * @param outputPoints list of output points to be used to estimate a
78 * projective 3D transformation.
79 * @throws IllegalArgumentException if provided lists of points don't have
80 * the same size or their size is smaller than MINIMUM_SIZE.
81 */
82 public MSACPointCorrespondenceProjectiveTransformation3DRobustEstimator(
83 final List<Point3D> inputPoints, final List<Point3D> outputPoints) {
84 super(inputPoints, outputPoints);
85 threshold = DEFAULT_THRESHOLD;
86 }
87
88 /**
89 * Constructor.
90 *
91 * @param listener listener to be notified of events such as when estimation
92 * starts, ends or its progress significantly changes.
93 */
94 public MSACPointCorrespondenceProjectiveTransformation3DRobustEstimator(
95 final ProjectiveTransformation3DRobustEstimatorListener listener) {
96 super(listener);
97 threshold = DEFAULT_THRESHOLD;
98 }
99
100 /**
101 * Constructor with listener and lists of points to be used to estimate a
102 * projective 3D transformation.
103 * Points in the list located at the same position are considered to be
104 * matched. Hence, both lists must have the same size, and their size must
105 * be greater or equal than MINIMUM_SIZE.
106 *
107 * @param listener listener to be notified of events such as when estimation
108 * stars, ends or its progress significantly changes.
109 * @param inputPoints list of input points to be used to estimate a
110 * projective 3D transformation.
111 * @param outputPoints list of output points to be used to estimate a
112 * projective 3D transformation.
113 * @throws IllegalArgumentException if provided lists of points don't have
114 * the same size or their size is smaller than MINIMUM_SIZE.
115 */
116 public MSACPointCorrespondenceProjectiveTransformation3DRobustEstimator(
117 final ProjectiveTransformation3DRobustEstimatorListener listener,
118 final List<Point3D> inputPoints, final List<Point3D> outputPoints) {
119 super(listener, inputPoints, outputPoints);
120 threshold = DEFAULT_THRESHOLD;
121 }
122
123 /**
124 * Returns threshold to determine whether points are inliers or not when
125 * testing possible estimation solutions.
126 * The threshold refers to the amount of error (i.e. Euclidean distance) a
127 * possible solution has on a matched pair of points.
128 *
129 * @return threshold to determine whether points are inliers or not when
130 * testing possible estimation solutions.
131 */
132 public double getThreshold() {
133 return threshold;
134 }
135
136 /**
137 * Sets threshold to determine whether points are inliers or not when
138 * testing possible estimation solutions.
139 * The threshold refers to the amount of error (i.e. Euclidean distance) a
140 * possible solution has on a matched pair of points.
141 *
142 * @param threshold threshold to determine whether points are inliers or not
143 * @throws IllegalArgumentException if provided values is equal or less than
144 * zero.
145 * @throws LockedException if robust estimator is locked because an
146 * estimation is already in progress.
147 */
148 public void setThreshold(final double threshold) throws LockedException {
149 if (isLocked()) {
150 throw new LockedException();
151 }
152 if (threshold <= MIN_THRESHOLD) {
153 throw new IllegalArgumentException();
154 }
155 this.threshold = threshold;
156 }
157
158 /**
159 * Estimates a projective 3D transformation using a robust estimator and
160 * the best set of matched 3D point correspondences found using the robust
161 * estimator.
162 *
163 * @return a projective 3D transformation.
164 * @throws LockedException if robust estimator is locked because an
165 * estimation is already in progress.
166 * @throws NotReadyException if provided input data is not enough to start
167 * the estimation.
168 * @throws RobustEstimatorException if estimation fails for any reason
169 * (i.e. numerical instability, no solution available, etc).
170 */
171 @Override
172 public ProjectiveTransformation3D estimate() throws LockedException, NotReadyException, RobustEstimatorException {
173 if (isLocked()) {
174 throw new LockedException();
175 }
176 if (!isReady()) {
177 throw new NotReadyException();
178 }
179
180 final var innerEstimator = new MSACRobustEstimator<>(
181 new MSACRobustEstimatorListener<ProjectiveTransformation3D>() {
182
183 // point to be reused when computing residuals
184 private final Point3D testPoint = Point3D.create(CoordinatesType.HOMOGENEOUS_COORDINATES);
185
186 @Override
187 public double getThreshold() {
188 return threshold;
189 }
190
191 @Override
192 public int getTotalSamples() {
193 return inputPoints.size();
194 }
195
196 @Override
197 public int getSubsetSize() {
198 return ProjectiveTransformation3DRobustEstimator.MINIMUM_SIZE;
199 }
200
201 @Override
202 public void estimatePreliminarSolutions(
203 final int[] samplesIndices, final List<ProjectiveTransformation3D> solutions) {
204 final var inputPoint1 = inputPoints.get(samplesIndices[0]);
205 final var inputPoint2 = inputPoints.get(samplesIndices[1]);
206 final var inputPoint3 = inputPoints.get(samplesIndices[2]);
207 final var inputPoint4 = inputPoints.get(samplesIndices[3]);
208 final var inputPoint5 = inputPoints.get(samplesIndices[4]);
209
210 final var outputPoint1 = outputPoints.get(samplesIndices[0]);
211 final var outputPoint2 = outputPoints.get(samplesIndices[1]);
212 final var outputPoint3 = outputPoints.get(samplesIndices[2]);
213 final var outputPoint4 = outputPoints.get(samplesIndices[3]);
214 final var outputPoint5 = outputPoints.get(samplesIndices[4]);
215
216 try {
217 final var transformation = new ProjectiveTransformation3D(inputPoint1, inputPoint2,
218 inputPoint3, inputPoint4, inputPoint5, outputPoint1, outputPoint2, outputPoint3,
219 outputPoint4, outputPoint5);
220 solutions.add(transformation);
221 } catch (final CoincidentPointsException e) {
222 // if points are coincident, no solution is added
223 }
224 }
225
226 @Override
227 public double computeResidual(final ProjectiveTransformation3D currentEstimation, final int i) {
228 final var inputPoint = inputPoints.get(i);
229 final var outputPoint = outputPoints.get(i);
230
231 // transform input point and store result in mTestPoint
232 currentEstimation.transform(inputPoint, testPoint);
233
234 return outputPoint.distanceTo(testPoint);
235 }
236
237 @Override
238 public boolean isReady() {
239 return MSACPointCorrespondenceProjectiveTransformation3DRobustEstimator.this.isReady();
240 }
241
242 @Override
243 public void onEstimateStart(final RobustEstimator<ProjectiveTransformation3D> estimator) {
244 if (listener != null) {
245 listener.onEstimateStart(
246 MSACPointCorrespondenceProjectiveTransformation3DRobustEstimator.this);
247 }
248 }
249
250 @Override
251 public void onEstimateEnd(final RobustEstimator<ProjectiveTransformation3D> estimator) {
252 if (listener != null) {
253 listener.onEstimateEnd(
254 MSACPointCorrespondenceProjectiveTransformation3DRobustEstimator.this);
255 }
256 }
257
258 @Override
259 public void onEstimateNextIteration(
260 final RobustEstimator<ProjectiveTransformation3D> estimator, final int iteration) {
261 if (listener != null) {
262 listener.onEstimateNextIteration(
263 MSACPointCorrespondenceProjectiveTransformation3DRobustEstimator.this,
264 iteration);
265 }
266 }
267
268 @Override
269 public void onEstimateProgressChange(
270 final RobustEstimator<ProjectiveTransformation3D> estimator, final float progress) {
271 if (listener != null) {
272 listener.onEstimateProgressChange(
273 MSACPointCorrespondenceProjectiveTransformation3DRobustEstimator.this,
274 progress);
275 }
276 }
277 });
278
279 try {
280 locked = true;
281 inliersData = null;
282 innerEstimator.setConfidence(confidence);
283 innerEstimator.setMaxIterations(maxIterations);
284 innerEstimator.setProgressDelta(progressDelta);
285 final var transformation = innerEstimator.estimate();
286 inliersData = innerEstimator.getInliersData();
287 return attemptRefine(transformation);
288 } catch (final com.irurueta.numerical.LockedException e) {
289 throw new LockedException(e);
290 } catch (final com.irurueta.numerical.NotReadyException e) {
291 throw new NotReadyException(e);
292 } finally {
293 locked = false;
294 }
295 }
296
297 /**
298 * Returns method being used for robust estimation.
299 *
300 * @return method being used for robust estimation.
301 */
302 @Override
303 public RobustEstimatorMethod getMethod() {
304 return RobustEstimatorMethod.MSAC;
305 }
306
307 /**
308 * Gets standard deviation used for Levenberg-Marquardt fitting during
309 * refinement.
310 * Returned value gives an indication of how much variance each residual
311 * has.
312 * Typically, this value is related to the threshold used on each robust
313 * estimation, since residuals of found inliers are within the range of
314 * such threshold.
315 *
316 * @return standard deviation used for refinement.
317 */
318 @Override
319 protected double getRefinementStandardDeviation() {
320 return threshold;
321 }
322 }