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.Matrix;
19 import com.irurueta.ar.epipolar.FundamentalMatrix;
20 import com.irurueta.ar.epipolar.InvalidFundamentalMatrixException;
21 import com.irurueta.geometry.CoordinatesType;
22 import com.irurueta.geometry.HomogeneousPoint2D;
23 import com.irurueta.geometry.Point2D;
24 import com.irurueta.geometry.Transformation2D;
25 import com.irurueta.geometry.estimators.LockedException;
26 import com.irurueta.geometry.estimators.NotReadyException;
27 import com.irurueta.geometry.refiners.RefinerException;
28 import com.irurueta.numerical.EvaluationException;
29 import com.irurueta.numerical.GradientEstimator;
30 import com.irurueta.numerical.fitting.LevenbergMarquardtMultiDimensionFitter;
31 import com.irurueta.numerical.fitting.LevenbergMarquardtMultiDimensionFunctionEvaluator;
32 import com.irurueta.numerical.robust.InliersData;
33
34 import java.util.BitSet;
35 import java.util.List;
36
37 /**
38 * Refines the epipole of a fundamental matrix formed by an initial epipole
39 * estimation and an estimated homography.
40 * Any fundamental matrix can be expressed as F = [e']x*H, where
41 * e' is the epipole on the right view, []x is the skew matrix, and H is a
42 * non-degenerate homography.
43 * This class refines an initial epipole so that residuals from provided point
44 * correspondences generating fundamental matrix F are reduced.
45 * This class is especially useful in cases where geometry of the scene is
46 * degenerate (e.g. planar scene) and provided point correspondences would
47 * generate an inaccurate fundamental matrix.
48 * This implementation uses Levenberg-Marquardt algorithm for a fast cost
49 * optimization and uses homogeneous points for epipole refinement.
50 */
51 public class HomogeneousRightEpipoleRefiner extends RightEpipoleRefiner {
52
53 /**
54 * Maximum allowed number of refinement iterations.
55 */
56 public static final int MAX_ITERS = 10;
57
58 /**
59 * Constructor.
60 */
61 public HomogeneousRightEpipoleRefiner() {
62 }
63
64 /**
65 * Constructor.
66 *
67 * @param initialEpipoleEstimation initial right epipole estimation to be
68 * set and refined.
69 * @param keepCovariance true if covariance of estimation must be kept after
70 * refinement, false otherwise.
71 * @param inliers set indicating which of the provided matches are inliers.
72 * @param residuals residuals for matched samples.
73 * @param numInliers number of inliers on initial estimation.
74 * @param samples1 1st set of paired samples.
75 * @param samples2 2nd set of paired samples.
76 * @param refinementStandardDeviation standard deviation used for
77 * Levenberg-Marquardt fitting.
78 * @param homography homography relating samples in two views, which is
79 * used to generate a fundamental matrix and its corresponding
80 * epipolar geometry.
81 */
82 public HomogeneousRightEpipoleRefiner(
83 final Point2D initialEpipoleEstimation, final boolean keepCovariance, final BitSet inliers,
84 final double[] residuals, final int numInliers, final List<Point2D> samples1, final List<Point2D> samples2,
85 final double refinementStandardDeviation, final Transformation2D homography) {
86 super(initialEpipoleEstimation, keepCovariance, inliers, residuals, numInliers, samples1, samples2,
87 refinementStandardDeviation, homography);
88 }
89
90 /**
91 * Constructor.
92 *
93 * @param initialEpipoleEstimation initial right epipole estimation to be
94 * set and refined.
95 * @param keepCovariance true if covariance of estimation must be kept after
96 * refinement, false otherwise.
97 * @param inliersData inlier data, typically obtained from a robust
98 * estimator.
99 * @param samples1 1st set of paired samples.
100 * @param samples2 2nd set of paired samples.
101 * @param refinementStandardDeviation standard deviation used for
102 * Levenberg-Marquardt fitting.
103 * @param homography homography relating samples in two views, which is used
104 * to generate a fundamental matrix and its corresponding epipolar
105 * geometry.
106 */
107 public HomogeneousRightEpipoleRefiner(
108 final Point2D initialEpipoleEstimation, final boolean keepCovariance, final InliersData inliersData,
109 final List<Point2D> samples1, final List<Point2D> samples2, final double refinementStandardDeviation,
110 final Transformation2D homography) {
111 super(initialEpipoleEstimation, keepCovariance, inliersData, samples1, samples2, refinementStandardDeviation,
112 homography);
113 }
114
115 /**
116 * Refines provided initial right epipole estimation.
117 * This method always sets a value into provided result instance regardless
118 * of the fact that error has actually improved in LMSE terms or not.
119 *
120 * @param result instance where refined estimation will be stored.
121 * @return true if result improves (decreases) in LMSE terms respect to
122 * initial estimation, false if no improvement has been achieved.
123 * @throws NotReadyException if not enough input data has been provided.
124 * @throws LockedException if estimator is locked because refinement is
125 * already in progress.
126 * @throws RefinerException if refinement fails for some reason (e.g. unable
127 * to converge to a result).
128 */
129 @SuppressWarnings("DuplicatedCode")
130 @Override
131 public boolean refine(final Point2D result) throws NotReadyException, LockedException, RefinerException {
132 if (isLocked()) {
133 throw new LockedException();
134 }
135 if (!isReady()) {
136 throw new NotReadyException();
137 }
138
139 locked = true;
140
141 if (listener != null) {
142 listener.onRefineStart(this, initialEstimation);
143 }
144
145 try {
146 final var epipole = new HomogeneousPoint2D(initialEstimation);
147 var errorDecreased = false;
148 boolean iterErrorDecreased;
149 var numIter = 0;
150 do {
151 final var fundamentalMatrix = new FundamentalMatrix();
152 computeFundamentalMatrix(homography, epipole, fundamentalMatrix);
153 final var initialTotalResidual = totalResidual(fundamentalMatrix);
154 final var initParams = epipole.asArray();
155
156 // output values to be fitted/optimized will contain residuals
157 final var y = new double[numInliers];
158 // input values will contain 2 points to compute residuals
159 final var nDims = 2 * Point2D.POINT2D_HOMOGENEOUS_COORDINATES_LENGTH;
160 final var x = new Matrix(numInliers, nDims);
161 final var nSamples = inliers.length();
162 var pos = 0;
163 for (var i = 0; i < nSamples; i++) {
164 if (inliers.get(i)) {
165 // sample is inlier
166 final var leftPoint = samples1.get(i);
167 final var rightPoint = samples2.get(i);
168 leftPoint.normalize();
169 rightPoint.normalize();
170 x.setElementAt(pos, 0, leftPoint.getHomX());
171 x.setElementAt(pos, 1, leftPoint.getHomY());
172 x.setElementAt(pos, 2, leftPoint.getHomW());
173 x.setElementAt(pos, 3, rightPoint.getHomX());
174 x.setElementAt(pos, 4, rightPoint.getHomY());
175 x.setElementAt(pos, 5, rightPoint.getHomW());
176
177 y[pos] = residuals[i];
178 pos++;
179 }
180 }
181
182 final var evaluator = new LevenbergMarquardtMultiDimensionFunctionEvaluator() {
183
184 private final Point2D leftPoint = Point2D.create(CoordinatesType.HOMOGENEOUS_COORDINATES);
185
186 private final Point2D rightPoint = Point2D.create(CoordinatesType.HOMOGENEOUS_COORDINATES);
187
188 private final HomogeneousPoint2D epipole = new HomogeneousPoint2D();
189
190 private final FundamentalMatrix fundMatrix = new FundamentalMatrix();
191
192 private final GradientEstimator gradientEstimator = new GradientEstimator(point -> {
193 try {
194 epipole.setCoordinates(point);
195 computeFundamentalMatrix(homography, epipole, fundMatrix);
196 return residual(fundMatrix, leftPoint, rightPoint);
197 } catch (final InvalidFundamentalMatrixException e) {
198 throw new EvaluationException(e);
199 }
200 });
201
202 @Override
203 public int getNumberOfDimensions() {
204 return nDims;
205 }
206
207 @Override
208 public double[] createInitialParametersArray() {
209 return initParams;
210 }
211
212 @Override
213 public double evaluate(final int i, final double[] point, final double[] params,
214 final double[] derivatives) throws EvaluationException {
215 try {
216 leftPoint.setHomogeneousCoordinates(point[0], point[1], point[2]);
217 rightPoint.setHomogeneousCoordinates(point[3], point[4], point[5]);
218
219 epipole.setCoordinates(params);
220 computeFundamentalMatrix(homography, epipole, fundMatrix);
221
222 final var y = residual(fundMatrix, leftPoint, rightPoint);
223 gradientEstimator.gradient(params, derivatives);
224
225 return y;
226 } catch (final InvalidFundamentalMatrixException e) {
227 throw new EvaluationException(e);
228 }
229 }
230 };
231
232 final var fitter = new LevenbergMarquardtMultiDimensionFitter(evaluator, x, y,
233 getRefinementStandardDeviation());
234
235 fitter.fit();
236
237 // obtain estimated params
238 final var params = fitter.getA();
239
240 // update initial epipole for next iteration
241 epipole.setHomogeneousCoordinates(params[0], params[1], params[2]);
242
243 computeFundamentalMatrix(homography, epipole, fundamentalMatrix);
244
245 final var finalTotalResidual = totalResidual(fundamentalMatrix);
246 iterErrorDecreased = finalTotalResidual < initialTotalResidual;
247 if (iterErrorDecreased || !errorDecreased) {
248 errorDecreased = true;
249 // update final result
250 result.setHomogeneousCoordinates(params[0], params[1], params[2]);
251
252 if (keepCovariance) {
253 // keep covariance
254 covariance = fitter.getCovar();
255 }
256 }
257 numIter++;
258 } while (iterErrorDecreased && numIter < MAX_ITERS);
259
260 if (listener != null) {
261 listener.onRefineEnd(this, initialEstimation, result, true);
262 }
263
264 return true;
265
266 } catch (final Exception e) {
267 throw new RefinerException(e);
268 } finally {
269 locked = false;
270 }
271 }
272 }