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.ArrayUtils;
20 import com.irurueta.algebra.Matrix;
21 import com.irurueta.algebra.SingularValueDecomposer;
22 import com.irurueta.algebra.Utils;
23 import com.irurueta.geometry.CoincidentPointsException;
24 import com.irurueta.geometry.InvalidRotationMatrixException;
25 import com.irurueta.geometry.MatrixRotation3D;
26 import com.irurueta.geometry.MetricTransformation3D;
27 import com.irurueta.geometry.Point3D;
28
29 import java.util.List;
30
31 /**
32 * Estimator of a 3D metric transformation based on point correspondences.
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="http://stackoverflow.com/questions/13432805/finding-translation-and-scale-on-two-sets-of-points-to-get-least-square-error-in">
40 * http://stackoverflow.com/questions/13432805/finding-translation-and-scale-on-two-sets-of-points-to-get-least-square-error-in
41 * </a>
42 */
43 @SuppressWarnings("DuplicatedCode")
44 public class MetricTransformation3DEstimator {
45
46 /**
47 * Minimum required number of matched points.
48 */
49 public static final int MINIMUM_SIZE = 4;
50
51 /**
52 * For some point configurations a solution can be found with only 3 points.
53 */
54 public static final int WEAK_MINIMUM_SIZE = 3;
55
56 /**
57 * 3D input points.
58 */
59 private List<Point3D> inputPoints;
60
61 /**
62 * 3D output points.
63 */
64 private List<Point3D> outputPoints;
65
66 /**
67 * Listener to be notified of events such as when estimation starts or ends.
68 */
69 private MetricTransformation3DEstimatorListener listener;
70
71 /**
72 * Indicates whether estimation can start with only 3 points or not.
73 * True allows 3 points, false requires 4.
74 */
75 private boolean weakMinimumSizeAllowed;
76
77 /**
78 * Indicates if this estimator is locked because an estimation is being
79 * computed.
80 */
81 private boolean locked;
82
83 /**
84 * Constructor.
85 */
86 public MetricTransformation3DEstimator() {
87 }
88
89 /**
90 * Constructor.
91 *
92 * @param inputPoints 3D input points.
93 * @param outputPoints 3D output points.
94 * @throws IllegalArgumentException if provided lists of points don't have
95 * the same size or their size is smaller than 4.
96 */
97 public MetricTransformation3DEstimator(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 MetricTransformation3DEstimator(final MetricTransformation3DEstimatorListener 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 MetricTransformation3DEstimator(
122 final MetricTransformation3DEstimatorListener 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 MetricTransformation3DEstimator(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 MetricTransformation3DEstimator(
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 MetricTransformation3DEstimator(
160 final MetricTransformation3DEstimatorListener 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 MetricTransformation3DEstimator(
177 final MetricTransformation3DEstimatorListener 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 a metric 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 a metric 3D
193 * transformation.
194 */
195 public List<Point3D> getInputPoints() {
196 return inputPoints;
197 }
198
199 /**
200 * Returns list of output points to be used to estimate a metric 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 a metric 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 a metric 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 MetricTransformation3DEstimatorListener 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 MetricTransformation3DEstimatorListener 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 metric 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 a metric 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 metric 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 MetricTransformation3D estimate() throws LockedException, NotReadyException, CoincidentPointsException {
340 final var result = new MetricTransformation3D();
341 estimate(result);
342 return result;
343 }
344
345 /**
346 * Estimates a metric 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 MetricTransformation3D 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 var inCov = 0.0;
386 for (int i = 0; i < n; i++) {
387 final var inputPoint = inputPoints.get(i);
388 final var outputPoint = outputPoints.get(i);
389
390 col.setElementAtIndex(0, inputPoint.getInhomX() - inCentroid.getElementAtIndex(0));
391 col.setElementAtIndex(1, inputPoint.getInhomY() - inCentroid.getElementAtIndex(1));
392 col.setElementAtIndex(2, inputPoint.getInhomZ() - inCentroid.getElementAtIndex(2));
393
394 row.setElementAtIndex(0, outputPoint.getInhomX() - outCentroid.getElementAtIndex(0));
395 row.setElementAtIndex(1, outputPoint.getInhomY() - outCentroid.getElementAtIndex(1));
396 row.setElementAtIndex(2, outputPoint.getInhomZ() - outCentroid.getElementAtIndex(2));
397
398 // compute covariances of input and output points
399 inCov += Math.pow(Utils.normF(col), 2.0);
400
401 col.multiply(row, tmp);
402 m.add(tmp);
403 }
404
405 if (inCov == 0.0) {
406 throw new CoincidentPointsException();
407 }
408
409 final var decomposer = new SingularValueDecomposer(m);
410 decomposer.decompose();
411
412 if (!weakMinimumSizeAllowed && decomposer.getNullity() > 0) {
413 throw new CoincidentPointsException();
414 }
415
416 final var u = decomposer.getU();
417 final var v = decomposer.getV();
418
419 final var s = decomposer.getSingularValues();
420
421
422 // rotation R = V*U^T
423 final var r = v.multiplyAndReturnNew(u.transposeAndReturnNew());
424
425 final var e = new double[]{1.0, 1.0, 1.0};
426
427 if (Utils.det(r) < 0.0) {
428 // ideally rotation has 3 unitary singular values.
429 // Because of reflection, we must change sign of last singular
430 // value and reconstruct rotation matrix
431
432 e[2] = -1.0;
433
434 // because rotation matrix can be seen as V*e*U^T, we can
435 // simply multiply 3rd column of R by -1
436 r.setElementAt(0, 2, -r.getElementAt(0, 2));
437 r.setElementAt(1, 2, -r.getElementAt(1, 2));
438 r.setElementAt(2, 2, -r.getElementAt(2, 2));
439 }
440
441 final var rotation = new MatrixRotation3D(r);
442
443 // scale
444 final var dot = ArrayUtils.dotProduct(s, e);
445 final var invScale = dot / inCov;
446
447 // translation
448 final var t = r.multiplyAndReturnNew(inCentroid);
449 t.multiplyByScalar(-invScale);
450 t.add(outCentroid);
451
452 result.setRotation(rotation);
453 result.setTranslation(t.getBuffer());
454 result.setScale(invScale);
455
456 if (listener != null) {
457 listener.onEstimateEnd(this);
458 }
459
460 } catch (final AlgebraException | InvalidRotationMatrixException e) {
461 throw new CoincidentPointsException(e);
462 } finally {
463 locked = false;
464 }
465 }
466
467 /**
468 * Computes centroid of provided list of points using inhomogeneous
469 * coordinates.
470 *
471 * @param points list of points to compute centroid.
472 * @return centroid.
473 * @throws AlgebraException never thrown.
474 */
475 private static Matrix computeCentroid(final List<Point3D> points) throws AlgebraException {
476 var x = 0.0;
477 var y = 0.0;
478 var z = 0.0;
479 final var n = points.size();
480 for (final var p : points) {
481 x += p.getInhomX() / n;
482 y += p.getInhomY() / n;
483 z += p.getInhomZ() / n;
484 }
485
486 final var result = new Matrix(Point3D.POINT3D_INHOMOGENEOUS_COORDINATES_LENGTH, 1);
487 result.setElementAtIndex(0, x);
488 result.setElementAtIndex(1, y);
489 result.setElementAtIndex(2, z);
490 return result;
491 }
492
493 /**
494 * Internal method to set lists of points to be used to estimate a
495 * metric 3D transformation.
496 * This method does not check whether estimator is locked or not.
497 *
498 * @param inputPoints list of input points to be used to estimate a
499 * metric 3D transformation.
500 * @param outputPoints list of output points to be used to estimate a
501 * metric 3D transformation.
502 * @throws IllegalArgumentException if provided lists of points don't have
503 * the same size or their size is smaller than #getMinimumPoints.
504 */
505 private void internalSetPoints(final List<Point3D> inputPoints, final List<Point3D> outputPoints) {
506 if (inputPoints.size() < getMinimumPoints()) {
507 throw new IllegalArgumentException();
508 }
509 if (inputPoints.size() != outputPoints.size()) {
510 throw new IllegalArgumentException();
511 }
512 this.inputPoints = inputPoints;
513 this.outputPoints = outputPoints;
514 }
515 }