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.ar.epipolar.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.ar.epipolar.FundamentalMatrix;
23 import com.irurueta.ar.epipolar.InvalidFundamentalMatrixException;
24 import com.irurueta.geometry.Point2D;
25 import com.irurueta.geometry.ProjectiveTransformation2D;
26 import com.irurueta.geometry.estimators.LockedException;
27 import com.irurueta.geometry.estimators.NormalizerException;
28 import com.irurueta.geometry.estimators.NotReadyException;
29 import com.irurueta.geometry.estimators.Point2DNormalizer;
30
31 import java.util.List;
32
33 /**
34 * Non-robust fundamental matrix estimator that uses 8 matched 2D points on
35 * left and right views.
36 */
37 public class EightPointsFundamentalMatrixEstimator extends FundamentalMatrixEstimator {
38
39 /**
40 * Constant indicating that by default an LMSE solution is not allowed.
41 */
42 public static final boolean DEFAULT_ALLOW_LMSE_SOLUTION = false;
43
44 /**
45 * Minimum number of matched 2D points to start the estimation.
46 */
47 public static final int MIN_REQUIRED_POINTS = 8;
48
49 /**
50 * Indicates if by default provided point correspondences are normalized to
51 * increase the accuracy of the estimation.
52 */
53 public static final boolean DEFAULT_NORMALIZE_POINT_CORRESPONDENCES = true;
54
55 /**
56 * Indicates whether an LMSE (the Least Mean Square Error) solution is allowed
57 * or not. When an LMSE solution is allowed, more than 8 matched points can
58 * be used for fundamental matrix estimation. If LMSE solution is not
59 * allowed then only the 8 former matched points will be taken into account.
60 */
61 private boolean allowLMSESolution;
62
63 /**
64 * Indicates whether provided matched 2D points must be normalized to
65 * increase the accuracy of the estimation.
66 */
67 private boolean normalizePoints;
68
69 /**
70 * Constructor.
71 */
72 public EightPointsFundamentalMatrixEstimator() {
73 super();
74 allowLMSESolution = DEFAULT_ALLOW_LMSE_SOLUTION;
75 normalizePoints = DEFAULT_NORMALIZE_POINT_CORRESPONDENCES;
76 }
77
78 /**
79 * Constructor with matched 2D points.
80 *
81 * @param leftPoints 2D points on left view.
82 * @param rightPoints 2D points on right view.
83 * @throws IllegalArgumentException if provided list of points do not
84 * have the same length.
85 */
86 public EightPointsFundamentalMatrixEstimator(final List<Point2D> leftPoints, final List<Point2D> rightPoints) {
87 super(leftPoints, rightPoints);
88 allowLMSESolution = DEFAULT_ALLOW_LMSE_SOLUTION;
89 normalizePoints = DEFAULT_NORMALIZE_POINT_CORRESPONDENCES;
90 }
91
92 /**
93 * Returns boolean indicating whether an LMSE (the Least Mean Square Error)
94 * solution is allowed or not. When an LMSE solution is allowed, more than 8
95 * matched points can be used for fundamental matrix estimation. If LMSE
96 * solution is not allowed then only the 8 former matched points will be
97 * taken into account.
98 *
99 * @return true if an LMSE solution is allowed, false otherwise.
100 */
101 public boolean isLMSESolutionAllowed() {
102 return allowLMSESolution;
103 }
104
105 /**
106 * Sets boolean indicating whether an LMSE (the LEast Mean Square Error)
107 * solution is allowed or not. When an LMSE solution is allowed, more than 8
108 * matched points can be used for fundamental matrix estimation. If LMSE
109 * solution is not allowed then only the 8 former matched points will be
110 * taken into account.
111 *
112 * @param allowed true if an LMSE solution is allowed, false otherwise.
113 * @throws LockedException if this instance is locked because an estimation
114 * is in progress.
115 */
116 public void setLMSESolutionAllowed(final boolean allowed) throws LockedException {
117 if (isLocked()) {
118 throw new LockedException();
119 }
120
121 allowLMSESolution = allowed;
122 }
123
124 /**
125 * Indicates whether provided matched 2D points must be normalized to
126 * increase the accuracy of the estimation.
127 *
128 * @return true if points must be normalized, false otherwise.
129 */
130 public boolean arePointsNormalized() {
131 return normalizePoints;
132 }
133
134 /**
135 * Sets boolean indicating whether provided matched 2D points must be
136 * normalized to increase the accuracy of the estimation.
137 *
138 * @param normalizePoints true if points must be normalized, false
139 * otherwise.
140 * @throws LockedException if this instance is locked because an estimation
141 * is in progress.
142 */
143 public void setPointsNormalized(final boolean normalizePoints) throws LockedException {
144 if (isLocked()) {
145 throw new LockedException();
146 }
147
148 this.normalizePoints = normalizePoints;
149 }
150
151 /**
152 * Returns boolean indicating whether estimator is ready to start the
153 * fundamental matrix estimation.
154 * This is true when the required minimum number of matched points is
155 * provided to obtain a solution and both left and right views have the
156 * same number of matched points.
157 *
158 * @return true if estimator is ready to start the fundamental matrix
159 * estimation, false otherwise.
160 */
161 @Override
162 public boolean isReady() {
163 return leftPoints != null && rightPoints != null && leftPoints.size() == rightPoints.size()
164 && leftPoints.size() >= MIN_REQUIRED_POINTS;
165 }
166
167 /**
168 * Estimates a fundamental matrix using provided lists of matched points on
169 * left and right views.
170 *
171 * @return a fundamental matrix.
172 * @throws LockedException if estimator is locked doing an estimation.
173 * @throws NotReadyException if estimator is not ready because required
174 * input points have not already been provided.
175 * @throws FundamentalMatrixEstimatorException if configuration of provided
176 * 2D points is degenerate and fundamental matrix
177 * estimation fails.
178 */
179 @SuppressWarnings("DuplicatedCode")
180 @Override
181 public FundamentalMatrix estimate() throws LockedException, NotReadyException, FundamentalMatrixEstimatorException {
182
183 if (isLocked()) {
184 throw new LockedException();
185 }
186 if (!isReady()) {
187 throw new NotReadyException();
188 }
189
190 locked = true;
191
192 if (listener != null) {
193 listener.onEstimateStart(this);
194 }
195
196 final var nPoints = leftPoints.size();
197
198 try {
199 ProjectiveTransformation2D leftNormalization = null;
200 ProjectiveTransformation2D rightNormalization = null;
201 final List<Point2D> leftPoints;
202 final List<Point2D> rightPoints;
203 if (normalizePoints) {
204 // normalize points on left view
205 final var normalizer = new Point2DNormalizer(this.leftPoints);
206 normalizer.compute();
207
208 leftNormalization = normalizer.getTransformation();
209
210 // normalize points on right view
211 normalizer.setPoints(this.rightPoints);
212 normalizer.compute();
213
214 rightNormalization = normalizer.getTransformation();
215
216 // normalize to increase accuracy
217 leftNormalization.normalize();
218 rightNormalization.normalize();
219
220 leftPoints = leftNormalization.transformPointsAndReturnNew(this.leftPoints);
221 rightPoints = rightNormalization.transformPointsAndReturnNew(this.rightPoints);
222 } else {
223 leftPoints = this.leftPoints;
224 rightPoints = this.rightPoints;
225 }
226
227 final Matrix a;
228 if (isLMSESolutionAllowed()) {
229 a = new Matrix(nPoints, 9);
230 } else {
231 a = new Matrix(MIN_REQUIRED_POINTS, 9);
232 }
233
234 Point2D leftPoint;
235 Point2D rightPoint;
236 double homLeftX;
237 double homLeftY;
238 double homLeftW;
239 double homRightX;
240 double homRightY;
241 double homRightW;
242 double value0;
243 double value1;
244 double value2;
245 double value3;
246 double value4;
247 double value5;
248 double value6;
249 double value7;
250 double value8;
251 double rowNorm;
252 for (var i = 0; i < nPoints; i++) {
253 leftPoint = leftPoints.get(i);
254 rightPoint = rightPoints.get(i);
255
256 // normalize points to increase accuracy
257 leftPoint.normalize();
258 rightPoint.normalize();
259
260 homLeftX = leftPoint.getHomX();
261 homLeftY = leftPoint.getHomY();
262 homLeftW = leftPoint.getHomW();
263
264 homRightX = rightPoint.getHomX();
265 homRightY = rightPoint.getHomY();
266 homRightW = rightPoint.getHomW();
267
268 // set a row values
269 value0 = homLeftX * homRightX;
270 value1 = homLeftY * homRightX;
271 value2 = homLeftW * homRightX;
272
273 value3 = homLeftX * homRightY;
274 value4 = homLeftY * homRightY;
275 value5 = homLeftW * homRightY;
276
277 value6 = homLeftX * homRightW;
278 value7 = homLeftY * homRightW;
279 value8 = homLeftW * homRightW;
280
281 // normalize row to increase accuracy
282 rowNorm = Math.sqrt(Math.pow(value0, 2.0)
283 + Math.pow(value1, 2.0) + Math.pow(value2, 2.0)
284 + Math.pow(value3, 2.0) + Math.pow(value4, 2.0)
285 + Math.pow(value5, 2.0) + Math.pow(value6, 2.0)
286 + Math.pow(value7, 2.0) + Math.pow(value8, 2.0));
287
288 a.setElementAt(i, 0, value0 / rowNorm);
289 a.setElementAt(i, 1, value1 / rowNorm);
290 a.setElementAt(i, 2, value2 / rowNorm);
291 a.setElementAt(i, 3, value3 / rowNorm);
292 a.setElementAt(i, 4, value4 / rowNorm);
293 a.setElementAt(i, 5, value5 / rowNorm);
294 a.setElementAt(i, 6, value6 / rowNorm);
295 a.setElementAt(i, 7, value7 / rowNorm);
296 a.setElementAt(i, 8, value8 / rowNorm);
297
298 if (!isLMSESolutionAllowed() && i == (MIN_REQUIRED_POINTS - 1)) {
299 break;
300 }
301 }
302
303 final var decomposer = new SingularValueDecomposer(a);
304
305 decomposer.decompose();
306
307 // if nullity of provided a matrix is not of dimension 1 (number of
308 // dimensions of null-space), then epipolar geometry is degenerate
309 // because there is more than one possible solution (up to scale).
310 // This is typically due to co-linearities or co-planarities on
311 // projected 2D points. In this case we throw an exception
312 if (decomposer.getNullity() > 1) {
313 throw new FundamentalMatrixEstimatorException();
314 }
315
316 var v = decomposer.getV();
317
318 // The fundamental matrix is contained in vector form on the last
319 // column of V, we reshape such vector into a 3x3 matrix
320 var fundMatrix = new Matrix(FundamentalMatrix.FUNDAMENTAL_MATRIX_ROWS,
321 FundamentalMatrix.FUNDAMENTAL_MATRIX_COLS);
322 fundMatrix.setElementAt(0, 0, v.getElementAt(0, 8));
323 fundMatrix.setElementAt(0, 1, v.getElementAt(1, 8));
324 fundMatrix.setElementAt(0, 2, v.getElementAt(2, 8));
325 fundMatrix.setElementAt(1, 0, v.getElementAt(3, 8));
326 fundMatrix.setElementAt(1, 1, v.getElementAt(4, 8));
327 fundMatrix.setElementAt(1, 2, v.getElementAt(5, 8));
328 fundMatrix.setElementAt(2, 0, v.getElementAt(6, 8));
329 fundMatrix.setElementAt(2, 1, v.getElementAt(7, 8));
330 fundMatrix.setElementAt(2, 2, v.getElementAt(8, 8));
331
332 if (normalizePoints && leftNormalization != null) {
333 // denormalize fundMatrix
334 final var transposedRightTransformationMatrix = rightNormalization.asMatrix().transposeAndReturnNew();
335 final var leftTransformationMatrix = leftNormalization.asMatrix();
336
337 // compute fundMatrix = transposedRightTransformationMatrix *
338 // fundMatrix * leftTransformationMatrix
339 fundMatrix.multiply(leftTransformationMatrix);
340 transposedRightTransformationMatrix.multiply(fundMatrix);
341 fundMatrix = transposedRightTransformationMatrix;
342
343 // normalize by Frobenius norm to increase accuracy after point
344 // de-normalization
345 final var norm = Utils.normF(fundMatrix);
346 fundMatrix.multiplyByScalar(1.0 / norm);
347 }
348
349 // enforce rank 2
350 decomposer.setInputMatrix(fundMatrix);
351
352 decomposer.decompose();
353
354 // if rank is not already correct, then we enforce it
355 final var rank = decomposer.getRank();
356 if (rank > FundamentalMatrix.FUNDAMENTAL_MATRIX_RANK) {
357 // rank needs to be reduced
358 final var u = decomposer.getU();
359 final var w = decomposer.getW();
360 v = decomposer.getV();
361
362 // transpose V
363 v.transpose();
364 final var transV = v;
365
366 // set last singular value to zero to enforce rank 2
367 w.setElementAt(2, 2, 0.0);
368
369 // compute fundMatrix = U * W * V'
370 w.multiply(transV);
371 u.multiply(w);
372 fundMatrix = u;
373 } else if (rank < FundamentalMatrix.FUNDAMENTAL_MATRIX_RANK) {
374 // rank is 1, which is lower than required fundamental matrix
375 // rank (rank 2)
376 throw new FundamentalMatrixEstimatorException();
377 }
378
379 final var result = new FundamentalMatrix(fundMatrix);
380
381 if (listener != null) {
382 listener.onEstimateEnd(this, result);
383 }
384
385 return result;
386
387 } catch (final InvalidFundamentalMatrixException | AlgebraException | NormalizerException e) {
388 throw new FundamentalMatrixEstimatorException(e);
389 } finally {
390 locked = false;
391 }
392 }
393
394 /**
395 * Returns method of non-robust fundamental matrix estimator.
396 *
397 * @return method of fundamental matrix estimator.
398 */
399 @Override
400 public FundamentalMatrixEstimatorMethod getMethod() {
401 return FundamentalMatrixEstimatorMethod.EIGHT_POINTS_ALGORITHM;
402 }
403
404 /**
405 * Returns minimum number of matched pair of points required to start
406 * the estimation. This implementation requires a minimum of 8 points
407 *
408 * @return minimum number of matched pair of points required to start
409 * the estimation. Always returns 8.
410 */
411 @Override
412 public int getMinRequiredPoints() {
413 return MIN_REQUIRED_POINTS;
414 }
415 }