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.ar.epipolar.refiners;
17
18 import com.irurueta.algebra.AlgebraException;
19 import com.irurueta.algebra.Matrix;
20 import com.irurueta.ar.epipolar.FundamentalMatrix;
21 import com.irurueta.ar.epipolar.InvalidFundamentalMatrixException;
22 import com.irurueta.geometry.CoordinatesType;
23 import com.irurueta.geometry.Line2D;
24 import com.irurueta.geometry.Point2D;
25 import com.irurueta.geometry.estimators.LockedException;
26 import com.irurueta.geometry.estimators.NotReadyException;
27 import com.irurueta.geometry.refiners.PairMatchesAndInliersDataRefiner;
28 import com.irurueta.geometry.refiners.RefinerException;
29 import com.irurueta.numerical.EvaluationException;
30 import com.irurueta.numerical.GradientEstimator;
31 import com.irurueta.numerical.MultiDimensionFunctionEvaluatorListener;
32 import com.irurueta.numerical.fitting.LevenbergMarquardtMultiDimensionFitter;
33 import com.irurueta.numerical.fitting.LevenbergMarquardtMultiDimensionFunctionEvaluator;
34 import com.irurueta.numerical.robust.InliersData;
35
36 import java.util.BitSet;
37 import java.util.List;
38
39 /**
40 * Refines a fundamental matrix by taking into account an initial estimation,
41 * inlier matches and their residuals.
42 * This class can be used to find a solution that minimizes error of inliers
43 * in LMSE terms.
44 * Typically, a refiner is used by a robust estimator, however it can also be
45 * useful in some other situations.
46 */
47 @SuppressWarnings("DuplicatedCode")
48 public class FundamentalMatrixRefiner extends PairMatchesAndInliersDataRefiner<FundamentalMatrix, Point2D, Point2D> {
49
50 /**
51 * Test line to compute epipolar residuals.
52 */
53 private final Line2D testLine = new Line2D();
54
55 /**
56 * Standard deviation used for Levenberg-Marquardt fitting during
57 * refinement.
58 * Returned value gives an indication of how much variance each residual
59 * has.
60 * Typically, this value is related to the threshold used on each robust
61 * estimation, since residuals of found inliers are within the range of
62 * such threshold.
63 */
64 private double refinementStandardDeviation;
65
66 /**
67 * Constructor.
68 */
69 public FundamentalMatrixRefiner() {
70 }
71
72 /**
73 * Constructor.
74 *
75 * @param initialEstimation initial estimation to be set.
76 * @param keepCovariance true if covariance of estimation must be kept after
77 * refinement, false otherwise.
78 * @param inliers set indicating which of the provided matches are inliers.
79 * @param residuals residuals for matched samples.
80 * @param numInliers number of inliers on initial estimation.
81 * @param samples1 1st set of paired samples.
82 * @param samples2 2nd set of paired samples.
83 * @param refinementStandardDeviation standard deviation used for
84 * Levenberg-Marquardt fitting.
85 */
86 public FundamentalMatrixRefiner(
87 final FundamentalMatrix initialEstimation, final boolean keepCovariance, final BitSet inliers,
88 final double[] residuals, final int numInliers, final List<Point2D> samples1, final List<Point2D> samples2,
89 final double refinementStandardDeviation) {
90 super(initialEstimation, keepCovariance, inliers, residuals, numInliers, samples1, samples2);
91 this.refinementStandardDeviation = refinementStandardDeviation;
92 }
93
94 /**
95 * Constructor.
96 *
97 * @param initialEstimation initial estimation to be set.
98 * @param keepCovariance true if covariance of estimation must be kept after
99 * refinement, false otherwise.
100 * @param inliersData inlier data, typically obtained from a robust
101 * estimator.
102 * @param samples1 1st set of paired samples.
103 * @param samples2 2nd set of paired samples.
104 * @param refinementStandardDeviation standard deviation used for
105 * Levenberg-Marquardt fitting.
106 */
107 public FundamentalMatrixRefiner(
108 final FundamentalMatrix initialEstimation, final boolean keepCovariance, final InliersData inliersData,
109 final List<Point2D> samples1, final List<Point2D> samples2, final double refinementStandardDeviation) {
110 super(initialEstimation, keepCovariance, inliersData, samples1, samples2);
111 this.refinementStandardDeviation = refinementStandardDeviation;
112 }
113
114 /**
115 * Gets standard deviation used for Levenberg-Marquardt fitting during
116 * refinement.
117 * Returned value gives an indication of how much variance each residual
118 * has.
119 * Typically, this value is related to the threshold used on each robust
120 * estimation, since residuals of found inliers are within the range of such
121 * threshold.
122 *
123 * @return standard deviation used for refinement.
124 */
125 public double getRefinementStandardDeviation() {
126 return refinementStandardDeviation;
127 }
128
129 /**
130 * Sets standard deviation used for Levenberg-Marquardt fitting during
131 * refinement.
132 * Returned value gives an indication of how much variance each residual
133 * has.
134 * Typically, this value is related to the threshold used on each robust
135 * estimation, since residuals of found inliers are within the range of such
136 * threshold.
137 *
138 * @param refinementStandardDeviation standard deviation used for
139 * refinement.
140 * @throws LockedException if estimator is locked.
141 */
142 public void setRefinementStandardDeviation(final double refinementStandardDeviation) throws LockedException {
143 if (isLocked()) {
144 throw new LockedException();
145 }
146 this.refinementStandardDeviation = refinementStandardDeviation;
147 }
148
149 /**
150 * Refines provided initial estimation.
151 *
152 * @return refined estimation.
153 * @throws NotReadyException if not enough input data has been provided.
154 * @throws LockedException if estimator is locked because refinement is
155 * already in progress.
156 * @throws RefinerException if refinement fails for some reason (e.g. unable
157 * to converge to a result).
158 */
159 @Override
160 public FundamentalMatrix refine() throws NotReadyException, LockedException, RefinerException {
161 final var result = new FundamentalMatrix();
162 refine(result);
163 return result;
164 }
165
166 /**
167 * Refines provided initial estimation.
168 * This method always sets a value into provided result instance regardless
169 * of the fact that error has actually improved in LMSE terms or not.
170 *
171 * @param result instance where refined estimation will be stored.
172 * @return true if result improves (decreases) in LMSE terms respect to
173 * initial estimation, false if no improvement has been achieved.
174 * @throws NotReadyException if not enough input data has been provided.
175 * @throws LockedException if estimator is locked because refinement is
176 * already in progress.
177 * @throws RefinerException if refinement fails for some reason (e.g. unable
178 * to converge to a result).
179 */
180 @Override
181 public boolean refine(final FundamentalMatrix result) throws NotReadyException, LockedException, RefinerException {
182 if (isLocked()) {
183 throw new LockedException();
184 }
185 if (!isReady()) {
186 throw new NotReadyException();
187 }
188
189 locked = true;
190
191 if (listener != null) {
192 listener.onRefineStart(this, initialEstimation);
193 }
194
195 initialEstimation.normalize();
196
197 final var initialTotalResidual = totalResidual(initialEstimation);
198
199 try {
200 final var internalMatrix = initialEstimation.getInternalMatrix();
201 final var initParams = internalMatrix.getBuffer();
202
203 // output values to be fitted/optimized will contain residuals
204 final var y = new double[numInliers];
205 // input values will contain 2 points to compute residuals
206 final var nDims = 2 * Point2D.POINT2D_HOMOGENEOUS_COORDINATES_LENGTH;
207 final var x = new Matrix(numInliers, nDims);
208 final var nSamples = inliers.length();
209 var pos = 0;
210 Point2D leftPoint;
211 Point2D rightPoint;
212 for (var i = 0; i < nSamples; i++) {
213 if (inliers.get(i)) {
214 // sample is inlier
215 leftPoint = samples1.get(i);
216 rightPoint = samples2.get(i);
217 leftPoint.normalize();
218 rightPoint.normalize();
219 x.setElementAt(pos, 0, leftPoint.getHomX());
220 x.setElementAt(pos, 1, leftPoint.getHomY());
221 x.setElementAt(pos, 2, leftPoint.getHomW());
222 x.setElementAt(pos, 3, rightPoint.getHomX());
223 x.setElementAt(pos, 4, rightPoint.getHomY());
224 x.setElementAt(pos, 5, rightPoint.getHomW());
225
226 y[pos] = residuals[i];
227 pos++;
228 }
229 }
230
231 final var evaluator = new LevenbergMarquardtMultiDimensionFunctionEvaluator() {
232
233 private final Point2D leftPoint = Point2D.create(CoordinatesType.HOMOGENEOUS_COORDINATES);
234
235 private final Point2D rightPoint = Point2D.create(CoordinatesType.HOMOGENEOUS_COORDINATES);
236
237 private final FundamentalMatrix fundMatrix = new FundamentalMatrix();
238
239 private Matrix internalMatrix;
240
241 private final GradientEstimator gradientEstimator = new GradientEstimator(
242 new MultiDimensionFunctionEvaluatorListener() {
243
244 @Override
245 public double evaluate(final double[] params) {
246
247 try {
248 internalMatrix.fromArray(params);
249 fundMatrix.setInternalMatrix(internalMatrix);
250
251 return residual(fundMatrix, leftPoint, rightPoint);
252 } catch (final AlgebraException | InvalidFundamentalMatrixException e) {
253 return initialTotalResidual;
254 }
255 }
256 });
257
258 @Override
259 public int getNumberOfDimensions() {
260 return nDims;
261 }
262
263 @Override
264 public double[] createInitialParametersArray() {
265 return initParams;
266 }
267
268 @Override
269 public double evaluate(
270 final int i, final double[] point, final double[] params, final double[] derivatives)
271 throws EvaluationException {
272 leftPoint.setHomogeneousCoordinates(point[0], point[1], point[2]);
273 rightPoint.setHomogeneousCoordinates(point[3], point[4], point[5]);
274
275 double y;
276 try {
277 if (internalMatrix == null) {
278 internalMatrix = new Matrix(FundamentalMatrix.FUNDAMENTAL_MATRIX_ROWS,
279 FundamentalMatrix.FUNDAMENTAL_MATRIX_COLS);
280 }
281 internalMatrix.fromArray(params);
282
283
284 fundMatrix.setInternalMatrix(internalMatrix);
285
286 y = residual(fundMatrix, leftPoint, rightPoint);
287 } catch (final AlgebraException | InvalidFundamentalMatrixException e) {
288 y = initialTotalResidual;
289 }
290 gradientEstimator.gradient(params, derivatives);
291
292 return y;
293 }
294 };
295
296 final var fitter = new LevenbergMarquardtMultiDimensionFitter(evaluator, x, y,
297 getRefinementStandardDeviation());
298
299 fitter.fit();
300
301 // obtain estimated params
302 final var params = fitter.getA();
303
304 // update fundamental matrix
305 internalMatrix.fromArray(params);
306 result.setInternalMatrix(internalMatrix);
307
308 if (keepCovariance) {
309 // keep covariance
310 covariance = fitter.getCovar();
311 }
312
313 final var finalTotalResidual = totalResidual(result);
314 final var errorDecreased = finalTotalResidual < initialTotalResidual;
315
316 if (listener != null) {
317 listener.onRefineEnd(this, initialEstimation, result, errorDecreased);
318 }
319
320 return errorDecreased;
321
322 } catch (final Exception e) {
323 throw new RefinerException(e);
324 } finally {
325 locked = false;
326 }
327 }
328
329 /**
330 * Computes the residual between a fundamental matrix and a pair of matched
331 * points.
332 *
333 * @param fundamentalMatrix a fundamental matrix.
334 * @param leftPoint left 2D point.
335 * @param rightPoint right 2D point.
336 * @return residual (distance of point to epipolar line).
337 */
338 private double residual(final FundamentalMatrix fundamentalMatrix, final Point2D leftPoint,
339 final Point2D rightPoint) {
340 try {
341 leftPoint.normalize();
342 rightPoint.normalize();
343 fundamentalMatrix.normalize();
344 fundamentalMatrix.leftEpipolarLine(rightPoint, testLine);
345 final var leftDistance = Math.abs(testLine.signedDistance(leftPoint));
346 fundamentalMatrix.rightEpipolarLine(leftPoint, testLine);
347 final var rightDistance = Math.abs(testLine.signedDistance(rightPoint));
348 // return average distance as an error residual
349 return 0.5 * (leftDistance + rightDistance);
350 } catch (final NotReadyException e) {
351 return Double.MAX_VALUE;
352 }
353 }
354
355 /**
356 * Computes total residual among all provided inlier samples.
357 *
358 * @param fundamentalMatrix a fundamental matrix.
359 * @return total residual.
360 */
361 private double totalResidual(final FundamentalMatrix fundamentalMatrix) {
362 var result = 0.0;
363
364 final var nSamples = inliers.length();
365 for (var i = 0; i < nSamples; i++) {
366 if (inliers.get(i)) {
367 // sample is inlier
368 final var leftPoint = samples1.get(i);
369 final var rightPoint = samples2.get(i);
370 leftPoint.normalize();
371 rightPoint.normalize();
372 result += residual(fundamentalMatrix, leftPoint, rightPoint);
373 }
374 }
375
376 return result;
377 }
378 }