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