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.ar.epipolar.FundamentalMatrix;
19 import com.irurueta.ar.epipolar.InvalidFundamentalMatrixException;
20 import com.irurueta.geometry.HomogeneousPoint2D;
21 import com.irurueta.geometry.Line2D;
22 import com.irurueta.geometry.Point2D;
23 import com.irurueta.geometry.Transformation2D;
24 import com.irurueta.geometry.estimators.LockedException;
25 import com.irurueta.geometry.estimators.NotReadyException;
26 import com.irurueta.geometry.refiners.PairMatchesAndInliersDataRefiner;
27 import com.irurueta.geometry.refiners.RefinerException;
28 import com.irurueta.numerical.robust.InliersData;
29
30 import java.util.BitSet;
31 import java.util.List;
32
33 /**
34 * Base class to refine the epipole of a fundamental matrix formed by an initial
35 * epipole estimation and an estimated homography.
36 * Any fundamental matrix can be expressed as F = [e']x*H, where
37 * e' is the epipole on the right view and H is a non-degenerate homography.
38 * This class refines an initial epipole so that residuals from provided point
39 * correspondences generating fundamental matrix F are reduced.
40 * This class is especially useful in cases where geometry of the scene is
41 * degenerate (e.g. planar scene) and provided point correspondences would
42 * generate an inaccurate fundamental matrix.
43 */
44 @SuppressWarnings("DuplicatedCode")
45 public abstract class RightEpipoleRefiner extends PairMatchesAndInliersDataRefiner<Point2D, Point2D, Point2D> {
46
47 /**
48 * Test line to compute epipolar residuals.
49 */
50 protected final Line2D testLine = new Line2D();
51
52 /**
53 * Homography relating two views through a given planar scene.
54 */
55 protected Transformation2D homography;
56
57 /**
58 * Standard deviation used for Levenberg-Marquardt fitting during
59 * refinement.
60 * Returned value gives an indication of how much variance each residual
61 * has.
62 * Typically, this value is related to the threshold used on each robust
63 * estimation, since residuals of found inliers are within the range of
64 * such threshold.
65 */
66 private double refinementStandardDeviation;
67
68 /**
69 * Constructor.
70 */
71 protected RightEpipoleRefiner() {
72 }
73
74 /**
75 * Constructor.
76 *
77 * @param initialEpipoleEstimation initial right epipole estimation to be
78 * set and refined.
79 * @param keepCovariance true if covariance of estimation must be kept after
80 * refinement, false otherwise.
81 * @param inliers set indicating which of the provided matches are inliers.
82 * @param residuals residuals for matched samples.
83 * @param numInliers number of inliers on initial estimation.
84 * @param samples1 1st set of paired samples.
85 * @param samples2 2nd set of paired samples.
86 * @param refinementStandardDeviation standard deviation used for
87 * Levenberg-Marquardt fitting.
88 * @param homography homography relating samples in two views, which is used
89 * to generate a fundamental matrix and its corresponding
90 * epipolar geometry.
91 */
92 protected RightEpipoleRefiner(
93 final Point2D initialEpipoleEstimation, final boolean keepCovariance, final BitSet inliers,
94 final double[] residuals, final int numInliers, final List<Point2D> samples1, final List<Point2D> samples2,
95 final double refinementStandardDeviation, final Transformation2D homography) {
96 super(initialEpipoleEstimation, keepCovariance, inliers, residuals, numInliers, samples1, samples2);
97 this.homography = homography;
98 this.refinementStandardDeviation = refinementStandardDeviation;
99 }
100
101 /**
102 * Constructor.
103 *
104 * @param initialEpipoleEstimation initial right epipole estimation to be
105 * set and refined.
106 * @param keepCovariance true if covariance of estimation must be kept after
107 * refinement, false otherwise.
108 * @param inliersData inlier data, typically obtained from a robust
109 * estimator.
110 * @param samples1 1st set of paired samples.
111 * @param samples2 2nd set of paired samples.
112 * @param refinementStandardDeviation standard deviation used for
113 * Levenberg-Marquardt fitting.
114 * @param homography homography relating samples in two views, which is used
115 * to generate a fundamental matrix and its corresponding
116 * epipolar geometry.
117 */
118 protected RightEpipoleRefiner(
119 final Point2D initialEpipoleEstimation, final boolean keepCovariance, final InliersData inliersData,
120 final List<Point2D> samples1, final List<Point2D> samples2, final double refinementStandardDeviation,
121 final Transformation2D homography) {
122 super(initialEpipoleEstimation, keepCovariance, inliersData, samples1, samples2);
123 this.homography = homography;
124 this.refinementStandardDeviation = refinementStandardDeviation;
125 }
126
127 /**
128 * Gets standard deviation used for Levenberg-Marquardt fitting during
129 * refinement.
130 * Returned value gives an indication of how much variance each residual
131 * has.
132 * Typically, this value is related to the threshold used on each robust
133 * estimation, since residuals of found inliers are within the range of such
134 * threshold.
135 *
136 * @return standard deviation used for refinement.
137 */
138 public double getRefinementStandardDeviation() {
139 return refinementStandardDeviation;
140 }
141
142 /**
143 * Sets standard deviation used for Levenberg-Marquardt fitting during
144 * refinement.
145 * Returned value gives an indication of how much variance each residual
146 * has.
147 * Typically, this value is related to the threshold used on each robust
148 * estimation, since residuals of found inliers are within the range of such
149 * threshold.
150 *
151 * @param refinementStandardDeviation standard deviation used for
152 * refinement.
153 * @throws LockedException if estimator is locked.
154 */
155 public void setRefinementStandardDeviation(final double refinementStandardDeviation) throws LockedException {
156 if (isLocked()) {
157 throw new LockedException();
158 }
159 this.refinementStandardDeviation = refinementStandardDeviation;
160 }
161
162 /**
163 * Gets homography relating samples in two views, which is used to generate
164 * a fundamental matrix and its corresponding epipolar geometry.
165 *
166 * @return homography relating samples in two views.
167 */
168 public Transformation2D getHomography() {
169 return homography;
170 }
171
172 /**
173 * Sets homography relating samples in two views, which is used to generate
174 * a fundamental matrix and its corresponding epipolar geometry.
175 *
176 * @param homography homography relating samples in two views.
177 * @throws LockedException if estimator is locked.
178 */
179 public void setHomography(final Transformation2D homography) throws LockedException {
180 if (isLocked()) {
181 throw new LockedException();
182 }
183 this.homography = homography;
184 }
185
186 /**
187 * Indicates whether this refiner is ready to start refinement computation.
188 *
189 * @return true if refiner is ready, false otherwise.
190 */
191 @Override
192 public boolean isReady() {
193 return homography != null && super.isReady();
194 }
195
196 /**
197 * Refines provided initial right epipole estimation.
198 *
199 * @return refined estimation.
200 * @throws NotReadyException if not enough input data has been provided.
201 * @throws LockedException if estimator is locked because refinement is
202 * already in progress.
203 * @throws RefinerException if refinement fails for some reason (e.g. unable
204 * to converge to a result).
205 */
206 @Override
207 public Point2D refine() throws NotReadyException, LockedException, RefinerException {
208 final var result = new HomogeneousPoint2D();
209 refine(result);
210 return result;
211 }
212
213 /**
214 * Computes a fundamental matrix from a 2D homography and provided epipole
215 * on right view.
216 *
217 * @param homography a 2D homography. Must be invertible.
218 * @param rightEpipole epipole on right view.
219 * @param result instance where computed fundamental matrix will be stored.
220 * @throws InvalidFundamentalMatrixException if provided homography is not
221 * invertible, which would generate a degenerate
222 * fundamental matrix.
223 */
224 public static void computeFundamentalMatrix(
225 final Transformation2D homography, final Point2D rightEpipole, final FundamentalMatrix result)
226 throws InvalidFundamentalMatrixException {
227
228 result.setFromHomography(homography, rightEpipole);
229 }
230
231 /**
232 * Computes the residual between a fundamental matrix and a pair of matched
233 * points.
234 *
235 * @param fundamentalMatrix a fundamental matrix.
236 * @param leftPoint left 2D point.
237 * @param rightPoint right 2D point.
238 * @return residual (distance of point to epipolar line).
239 */
240 protected double residual(
241 final FundamentalMatrix fundamentalMatrix, final Point2D leftPoint, final Point2D rightPoint) {
242 try {
243 leftPoint.normalize();
244 rightPoint.normalize();
245 fundamentalMatrix.normalize();
246 fundamentalMatrix.leftEpipolarLine(rightPoint, testLine);
247 final var leftDistance = Math.abs(testLine.signedDistance(leftPoint));
248 fundamentalMatrix.rightEpipolarLine(leftPoint, testLine);
249 final var rightDistance = Math.abs(testLine.signedDistance(rightPoint));
250 // return average distance as an error residual
251 return 0.5 * (leftDistance + rightDistance);
252 } catch (final NotReadyException e) {
253 return Double.MAX_VALUE;
254 }
255 }
256
257 /**
258 * Computes total residual among all provided inlier samples.
259 *
260 * @param fundamentalMatrix a fundamental matrix.
261 * @return total residual.
262 */
263 protected double totalResidual(final FundamentalMatrix fundamentalMatrix) {
264 var result = 0.0;
265
266 final var nSamples = inliers.length();
267 for (var i = 0; i < nSamples; i++) {
268 if (inliers.get(i)) {
269 // sample is inlier
270 final var leftPoint = samples1.get(i);
271 final var rightPoint = samples2.get(i);
272 leftPoint.normalize();
273 rightPoint.normalize();
274 result += residual(fundamentalMatrix, leftPoint, rightPoint);
275 }
276 }
277
278 return result;
279 }
280 }