1 /*
2 * Copyright (C) 2017 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.algebra.Matrix;
20 import com.irurueta.algebra.SingularValueDecomposer;
21 import com.irurueta.algebra.Utils;
22 import com.irurueta.geometry.CoincidentPointsException;
23 import com.irurueta.geometry.EuclideanTransformation3D;
24 import com.irurueta.geometry.InvalidRotationMatrixException;
25 import com.irurueta.geometry.MatrixRotation3D;
26 import com.irurueta.geometry.Point3D;
27
28 import java.util.List;
29
30 /**
31 * Estimator of a 3D Euclidean transformation based on point correspondences.
32 * This estimator uses Kabsch algorithm.
33 * A minimum of 4 non-coincident matched 3D input/output points is required for
34 * estimation.
35 * For some point configurations 3 points are enough to find a valid solution.
36 * If more points are provided an LMSE (Least Mean Squared Error) solution will
37 * be found.
38 * Based on:
39 * <a href="https://en.wikipedia.org/wiki/Kabsch_algorithm">https://en.wikipedia.org/wiki/Kabsch_algorithm</a>
40 * <a href="http://nghiaho.com/?page_id=671">http://nghiaho.com/?page_id=671</a>
41 */
42 @SuppressWarnings("DuplicatedCode")
43 public class EuclideanTransformation3DEstimator {
44
45 /**
46 * Minimum required number of matched points.
47 */
48 public static final int MINIMUM_SIZE = 4;
49
50 /**
51 * For some point configurations a solution can be found with only 3 points.
52 */
53 public static final int WEAK_MINIMUM_SIZE = 3;
54
55 /**
56 * 3D input points.
57 */
58 private List<Point3D> inputPoints;
59
60 /**
61 * 3D output points.
62 */
63 private List<Point3D> outputPoints;
64
65 /**
66 * Listener to be notified of events such as when estimation starts or ends.
67 */
68 private EuclideanTransformation3DEstimatorListener listener;
69
70 /**
71 * Indicates whether estimation can start with only 3 points or not.
72 * True allows 3 points, false requires 4.
73 */
74 private boolean weakMinimumSizeAllowed;
75
76 /**
77 * Indicates if this estimator is locked because an estimation is being
78 * computed.
79 */
80 private boolean locked;
81
82 /**
83 * Constructor.
84 */
85 public EuclideanTransformation3DEstimator() {
86 }
87
88 /**
89 * Constructor.
90 *
91 * @param inputPoints 3D input points.
92 * @param outputPoints 3D output points.
93 * @throws IllegalArgumentException if provided lists of points don't have
94 * the same size or their size is smaller than 4.
95 */
96 public EuclideanTransformation3DEstimator(
97 final List<Point3D> inputPoints, final List<Point3D> outputPoints) {
98 internalSetPoints(inputPoints, outputPoints);
99 }
100
101 /**
102 * Constructor.
103 *
104 * @param listener listener to be notified of events such as when estimation
105 * starts or ends.
106 */
107 public EuclideanTransformation3DEstimator(final EuclideanTransformation3DEstimatorListener listener) {
108 this.listener = listener;
109 }
110
111 /**
112 * Constructor.
113 *
114 * @param listener listener to be notified of events such as when estimation
115 * starts or ends.
116 * @param inputPoints 3D input points.
117 * @param outputPoints 3D output points.
118 * @throws IllegalArgumentException if provided lists of points don't have
119 * the same size or their size is smaller than 4.
120 */
121 public EuclideanTransformation3DEstimator(
122 final EuclideanTransformation3DEstimatorListener listener,
123 final List<Point3D> inputPoints, final List<Point3D> outputPoints) {
124 this.listener = listener;
125 internalSetPoints(inputPoints, outputPoints);
126 }
127
128 /**
129 * Constructor.
130 *
131 * @param weakMinimumSizeAllowed true allows 3 points, false requires 4.
132 */
133 public EuclideanTransformation3DEstimator(final boolean weakMinimumSizeAllowed) {
134 this.weakMinimumSizeAllowed = weakMinimumSizeAllowed;
135 }
136
137 /**
138 * Constructor.
139 *
140 * @param inputPoints 3D input points.
141 * @param outputPoints 3D output points.
142 * @param weakMinimumSizeAllowed true allows 3 points, false requires 4.
143 * @throws IllegalArgumentException if provided lists of points don't have
144 * the same size or their size is smaller than 4.
145 */
146 public EuclideanTransformation3DEstimator(
147 final List<Point3D> inputPoints, final List<Point3D> outputPoints, final boolean weakMinimumSizeAllowed) {
148 this.weakMinimumSizeAllowed = weakMinimumSizeAllowed;
149 internalSetPoints(inputPoints, outputPoints);
150 }
151
152 /**
153 * Constructor.
154 *
155 * @param listener listener to be notified of events such as when estimation
156 * starts or ends.
157 * @param weakMinimumSizeAllowed true allows 3 points, false requires 4.
158 */
159 public EuclideanTransformation3DEstimator(
160 final EuclideanTransformation3DEstimatorListener listener, final boolean weakMinimumSizeAllowed) {
161 this.weakMinimumSizeAllowed = weakMinimumSizeAllowed;
162 this.listener = listener;
163 }
164
165 /**
166 * Constructor.
167 *
168 * @param listener listener to be notified of events such as when estimation
169 * starts or ends.
170 * @param inputPoints 3D input points.
171 * @param outputPoints 3D output points.
172 * @param weakMinimumSizeAllowed true allows 3 points, false requires 4.
173 * @throws IllegalArgumentException if provided lists of points don't have
174 * the same size or their size is smaller than 4.
175 */
176 public EuclideanTransformation3DEstimator(
177 final EuclideanTransformation3DEstimatorListener listener,
178 final List<Point3D> inputPoints, final List<Point3D> outputPoints, final boolean weakMinimumSizeAllowed) {
179 this.weakMinimumSizeAllowed = weakMinimumSizeAllowed;
180 this.listener = listener;
181 internalSetPoints(inputPoints, outputPoints);
182 }
183
184 /**
185 * Returns list of input points to be used to estimate an Euclidean 3D
186 * transformation.
187 * Each point in the list of input points must be matched with the
188 * corresponding point in the list of output points located at the same
189 * position. Hence, both input points and output points must have the same
190 * size, and their size must be greater or equal than #getMinimumPoints.
191 *
192 * @return list of input points to be used to estimate an Euclidean 3D
193 * transformation.
194 */
195 public List<Point3D> getInputPoints() {
196 return inputPoints;
197 }
198
199 /**
200 * Returns list of output points ot be used to estimate an Euclidean 3D
201 * transformation.
202 * Each point in the list of output points must be matched with the
203 * corresponding point in the list of input points located at the same
204 * position. Hence, both input points and output points must have the same
205 * size, and their size must be greater or equal than #getMinimumPoints.
206 *
207 * @return list of output points to be used to estimate an Euclidean 3D
208 * transformation.
209 */
210 public List<Point3D> getOutputPoints() {
211 return outputPoints;
212 }
213
214 /**
215 * Sets list of points to be used to estimate an Euclidean 3D
216 * transformation.
217 * Points in the list located at the same position are considered to be
218 * matched. Hence, both lists must have the same size, and their size must
219 * be greater or equal than #getMinimumPoints.
220 *
221 * @param inputPoints list of input points to be used to estimate an
222 * Euclidean 3D transformation.
223 * @param outputPoints list of output points to be used to estimate an
224 * Euclidean 3D transformation.
225 * @throws IllegalArgumentException if provided lists of points don't have
226 * the same size or their size is smaller than #getMinimumPoints.
227 * @throws LockedException if estimator is locked because a computation is
228 * already in progress.
229 */
230 public void setPoints(final List<Point3D> inputPoints, final List<Point3D> outputPoints) throws LockedException {
231 if (isLocked()) {
232 throw new LockedException();
233 }
234 internalSetPoints(inputPoints, outputPoints);
235 }
236
237 /**
238 * Returns reference to listener to be notified of events such as when
239 * estimation starts or ends.
240 *
241 * @return listener to be notified of events.
242 */
243 public EuclideanTransformation3DEstimatorListener getListener() {
244 return listener;
245 }
246
247 /**
248 * Sets listener to be notified of events such as when estimation starts or
249 * ends.
250 *
251 * @param listener listener to be notified of events.
252 * @throws LockedException if estimator is locked.
253 */
254 public void setListener(final EuclideanTransformation3DEstimatorListener listener) throws LockedException {
255 if (isLocked()) {
256 throw new LockedException();
257 }
258 this.listener = listener;
259 }
260
261 /**
262 * Indicates whether estimation can start with only 3 points or not.
263 *
264 * @return true allows 3 points, false requires 4.
265 */
266 public boolean isWeakMinimumSizeAllowed() {
267 return weakMinimumSizeAllowed;
268 }
269
270 /**
271 * Specifies whether estimation can start with only 3 points or not.
272 *
273 * @param weakMinimumSizeAllowed true allows 3 points, false requires 4.
274 * @throws LockedException if estimator is locked.
275 */
276 public void setWeakMinimumSizeAllowed(final boolean weakMinimumSizeAllowed) throws LockedException {
277 if (isLocked()) {
278 throw new LockedException();
279 }
280 this.weakMinimumSizeAllowed = weakMinimumSizeAllowed;
281 }
282
283 /**
284 * Required minimum number of point correspondences to start the estimation.
285 * Can be either 3 or 4.
286 *
287 * @return minimum number of point correspondences.
288 */
289 public int getMinimumPoints() {
290 return weakMinimumSizeAllowed ? WEAK_MINIMUM_SIZE : MINIMUM_SIZE;
291 }
292
293 /**
294 * Indicates whether listener has been provided and is available for
295 * retrieval.
296 *
297 * @return true if available, false otherwise.
298 */
299 public boolean isListenerAvailable() {
300 return listener != null;
301 }
302
303 /**
304 * Indicates if this instance is locked because estimation is being
305 * computed.
306 *
307 * @return true if locked, false otherwise.
308 */
309 public boolean isLocked() {
310 return locked;
311 }
312
313 /**
314 * Indicates if estimator is ready to start the Euclidean 3D transformation
315 * estimation.
316 * This is true when input data (i.e. lists of matched points) are provided
317 * and a minimum of MINIMUM_SIZE points are available.
318 *
319 * @return true if estimator is ready, false otherwise.
320 */
321 public boolean isReady() {
322 return inputPoints != null && outputPoints != null && inputPoints.size() == outputPoints.size()
323 && inputPoints.size() >= getMinimumPoints();
324 }
325
326 /**
327 * Estimates an Euclidean 3D transformation using the list of matched input
328 * and output 3D points.
329 * A minimum of 4 matched non-coincident points is required. If more points
330 * are provided an LMSE (Least Mean Squared Error) solution will be found.
331 *
332 * @return estimated euclidean 3D transformation.
333 * @throws LockedException if estimator is locked.
334 * @throws NotReadyException if not enough data has been provided.
335 * @throws CoincidentPointsException raised if transformation cannot be
336 * estimated for some reason (point configuration degeneracy, duplicate
337 * points or numerical instabilities).
338 */
339 public EuclideanTransformation3D estimate() throws LockedException, NotReadyException, CoincidentPointsException {
340 final var result = new EuclideanTransformation3D();
341 estimate(result);
342 return result;
343 }
344
345 /**
346 * Estimates an Euclidean 3D transformation using the list of matched input
347 * and output 3D points.
348 * A minimum of 4 matched non-coincident points is required. If more points
349 * are provided an LMSE (Least Mean Squared Error) solution will be found.
350 *
351 * @param result instance where result will be stored.
352 * @throws LockedException if estimator is locked.
353 * @throws NotReadyException if not enough data has been provided.
354 * @throws CoincidentPointsException raised if transformation cannot be
355 * estimated for some reason (point configuration degeneracy, duplicate
356 * points or numerical instabilities).
357 */
358 public void estimate(final EuclideanTransformation3D result) throws LockedException, NotReadyException,
359 CoincidentPointsException {
360 if (isLocked()) {
361 throw new LockedException();
362 }
363 if (!isReady()) {
364 throw new NotReadyException();
365 }
366
367 try {
368 locked = true;
369
370 if (listener != null) {
371 listener.onEstimateStart(this);
372 }
373
374 final var inCentroid = computeCentroid(inputPoints);
375 final var outCentroid = computeCentroid(outputPoints);
376
377 final var m = new Matrix(Point3D.POINT3D_INHOMOGENEOUS_COORDINATES_LENGTH,
378 Point3D.POINT3D_INHOMOGENEOUS_COORDINATES_LENGTH);
379
380 final var n = inputPoints.size();
381 final var col = new Matrix(Point3D.POINT3D_INHOMOGENEOUS_COORDINATES_LENGTH, 1);
382 final var row = new Matrix(1, Point3D.POINT3D_INHOMOGENEOUS_COORDINATES_LENGTH);
383 final var tmp = new Matrix(Point3D.POINT3D_INHOMOGENEOUS_COORDINATES_LENGTH,
384 Point3D.POINT3D_INHOMOGENEOUS_COORDINATES_LENGTH);
385 for (var i = 0; i < n; i++) {
386 final var inputPoint = inputPoints.get(i);
387 final var outputPoint = outputPoints.get(i);
388
389 col.setElementAtIndex(0, inputPoint.getInhomX() - inCentroid.getElementAtIndex(0));
390 col.setElementAtIndex(1, inputPoint.getInhomY() - inCentroid.getElementAtIndex(1));
391 col.setElementAtIndex(2, inputPoint.getInhomZ() - inCentroid.getElementAtIndex(2));
392
393 row.setElementAtIndex(0, outputPoint.getInhomX() - outCentroid.getElementAtIndex(0));
394 row.setElementAtIndex(1, outputPoint.getInhomY() - outCentroid.getElementAtIndex(1));
395 row.setElementAtIndex(2, outputPoint.getInhomZ() - outCentroid.getElementAtIndex(2));
396
397 col.multiply(row, tmp);
398 m.add(tmp);
399 }
400
401 final var decomposer = new SingularValueDecomposer(m);
402 decomposer.decompose();
403
404 if (!weakMinimumSizeAllowed && decomposer.getNullity() > 0) {
405 throw new CoincidentPointsException();
406 }
407
408 final var u = decomposer.getU();
409 final var v = decomposer.getV();
410
411 // rotation R = V*U^T
412 u.transpose();
413 v.multiply(u);
414
415 if (Utils.det(v) < 0.0) {
416 // multiply 3rd column of R by -1
417 v.setElementAt(0, 2, -v.getElementAt(0, 2));
418 v.setElementAt(1, 2, -v.getElementAt(1, 2));
419 v.setElementAt(2, 2, -v.getElementAt(2, 2));
420 }
421
422 final var rotation = new MatrixRotation3D(v);
423
424 // translation
425 final var t = v.multiplyAndReturnNew(inCentroid);
426 t.multiplyByScalar(-1.0);
427 t.add(outCentroid);
428
429 result.setRotation(rotation);
430 result.setTranslation(t.getBuffer());
431
432 if (listener != null) {
433 listener.onEstimateEnd(this);
434 }
435
436 } catch (final AlgebraException | InvalidRotationMatrixException e) {
437 throw new CoincidentPointsException(e);
438 } finally {
439 locked = false;
440 }
441 }
442
443 /**
444 * Computes centroid of provided list of points using inhomogeneous
445 * coordinates.
446 *
447 * @param points list of points to compute centroid.
448 * @return centroid.
449 * @throws AlgebraException never thrown.
450 */
451 private static Matrix computeCentroid(final List<Point3D> points) throws AlgebraException {
452 var x = 0.0;
453 var y = 0.0;
454 var z = 0.0;
455 final var n = points.size();
456 for (final var p : points) {
457 x += p.getInhomX() / n;
458 y += p.getInhomY() / n;
459 z += p.getInhomZ() / n;
460 }
461
462 final var result = new Matrix(Point3D.POINT3D_INHOMOGENEOUS_COORDINATES_LENGTH, 1);
463 result.setElementAtIndex(0, x);
464 result.setElementAtIndex(1, y);
465 result.setElementAtIndex(2, z);
466 return result;
467 }
468
469 /**
470 * Internal method to set lists of points to be used to estimate an
471 * Euclidean 3D transformation.
472 * This method does not check whether estimator is locked or not.
473 *
474 * @param inputPoints list of input points to be used to estimate an
475 * Euclidean 3D transformation.
476 * @param outputPoints list of output points to be used to estimate an
477 * Euclidean 3D transformation.
478 * @throws IllegalArgumentException if provided lists of points don't have
479 * the same size or their size is smaller than #getMinimumPoints.
480 */
481 private void internalSetPoints(final List<Point3D> inputPoints, final List<Point3D> outputPoints) {
482 if (inputPoints.size() < getMinimumPoints()) {
483 throw new IllegalArgumentException();
484 }
485 if (inputPoints.size() != outputPoints.size()) {
486 throw new IllegalArgumentException();
487 }
488 this.inputPoints = inputPoints;
489 this.outputPoints = outputPoints;
490 }
491 }