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.estimators;
17
18 import com.irurueta.ar.epipolar.EssentialMatrix;
19 import com.irurueta.ar.epipolar.FundamentalMatrix;
20 import com.irurueta.geometry.GeometryException;
21 import com.irurueta.geometry.HomogeneousPoint2D;
22 import com.irurueta.geometry.PinholeCameraIntrinsicParameters;
23 import com.irurueta.geometry.Transformation2D;
24 import com.irurueta.geometry.estimators.LockedException;
25 import com.irurueta.geometry.estimators.NotReadyException;
26
27 import java.util.ArrayList;
28 import java.util.List;
29
30 /**
31 * This class takes an input 2D homography (e.g. transformation) and a given
32 * pair of intrinsic parameters for left and right views, and estimates all
33 * possible fundamental matrices generating such homography in a planar scene.
34 * This estimator will generate either 2 or 4 possible solutions, however only
35 * 1 solution will be physically possible, which corresponds to the solution
36 * that generates triangulated points located in front of the cameras generating
37 * such epipolar geometries.
38 * This class is useful in planar scenes where 8-point and 7-point algorithms
39 * will fail since this kind of geometry is a degenerate configuration of
40 * points.
41 */
42 public class PlanarFundamentalMatrixEstimator {
43
44 /**
45 * 2D transformation relating two views (left view to right view).
46 */
47 private Transformation2D homography;
48
49 /**
50 * Intrinsic parameters to be used on left view.
51 */
52 private PinholeCameraIntrinsicParameters leftIntrinsics;
53
54 /**
55 * Intrinsic parameters to be used on right view.
56 */
57 private PinholeCameraIntrinsicParameters rightIntrinsics;
58
59 /**
60 * Listener to attend events generated by this instance.
61 */
62 private PlanarFundamentalMatrixEstimatorListener listener;
63
64 /**
65 * Indicates whether estimator is locked while estimating fundamental
66 * matrix.
67 */
68 private boolean locked;
69
70 /**
71 * Constructor.
72 */
73 public PlanarFundamentalMatrixEstimator() {
74 }
75
76 /**
77 * Constructor.
78 *
79 * @param homography 2D transformation relating two views (left view to
80 * right view).
81 * @param leftIntrinsics intrinsic parameters to be used on left view.
82 * @param rightIntrinsics intrinsic parameters to be used on right view.
83 */
84 public PlanarFundamentalMatrixEstimator(final Transformation2D homography,
85 final PinholeCameraIntrinsicParameters leftIntrinsics,
86 final PinholeCameraIntrinsicParameters rightIntrinsics) {
87 this.homography = homography;
88 this.leftIntrinsics = leftIntrinsics;
89 this.rightIntrinsics = rightIntrinsics;
90 }
91
92 /**
93 * Constructor.
94 *
95 * @param homography 2D transformation relating two views (left view to
96 * right view).
97 * @param leftIntrinsics intrinsic parameters to be used on left view.
98 * @param rightIntrinsics intrinsic parameters to be used on right view.
99 * @param listener listener to attend events generated by this instance.
100 */
101 public PlanarFundamentalMatrixEstimator(final Transformation2D homography,
102 final PinholeCameraIntrinsicParameters leftIntrinsics,
103 final PinholeCameraIntrinsicParameters rightIntrinsics,
104 final PlanarFundamentalMatrixEstimatorListener listener) {
105 this(homography, leftIntrinsics, rightIntrinsics);
106 this.listener = listener;
107 }
108
109 /**
110 * Gets 2D transformation relating two views (left view to right view).
111 *
112 * @return 2D transformation relating two views.
113 */
114 public Transformation2D getHomography() {
115 return homography;
116 }
117
118 /**
119 * Sets 2D transformation relating two views (left view to right view).
120 *
121 * @param homography 2D transformation relating two views.
122 * @throws LockedException if estimator is locked.
123 */
124 public void setHomography(final Transformation2D homography) throws LockedException {
125 if (isLocked()) {
126 throw new LockedException();
127 }
128 this.homography = homography;
129 }
130
131 /**
132 * Gets intrinsic parameters to be used on left view.
133 *
134 * @return intrinsic parameters to be used on left view.
135 */
136 public PinholeCameraIntrinsicParameters getLeftIntrinsics() {
137 return leftIntrinsics;
138 }
139
140 /**
141 * Sets intrinsic parameters to be used on left view.
142 *
143 * @param leftIntrinsics intrinsic parameters to be used on left view.
144 * @throws LockedException if estimator is locked.
145 */
146 public void setLeftIntrinsics(final PinholeCameraIntrinsicParameters leftIntrinsics) throws LockedException {
147 if (isLocked()) {
148 throw new LockedException();
149 }
150 this.leftIntrinsics = leftIntrinsics;
151 }
152
153 /**
154 * Gets intrinsic parameters to be used on right view.
155 *
156 * @return intrinsic parameters to be used on right view.
157 */
158 public PinholeCameraIntrinsicParameters getRightIntrinsics() {
159 return rightIntrinsics;
160 }
161
162 /**
163 * Sets intrinsic parameters to be used on right view.
164 *
165 * @param rightIntrinsics intrinsic parameters to be used on right view.
166 * @throws LockedException if estimator is locked.
167 */
168 public void setRightIntrinsics(final PinholeCameraIntrinsicParameters rightIntrinsics) throws LockedException {
169 if (isLocked()) {
170 throw new LockedException();
171 }
172 this.rightIntrinsics = rightIntrinsics;
173 }
174
175 /**
176 * Gets listener to attend events generated by this instance.
177 *
178 * @return listener to attend events generated by this instance.
179 */
180 public PlanarFundamentalMatrixEstimatorListener getListener() {
181 return listener;
182 }
183
184 /**
185 * Sets listener to attend events generated by this instance.
186 *
187 * @param listener listener to attend events generated by this instance.
188 */
189 public void setListener(final PlanarFundamentalMatrixEstimatorListener listener) {
190 this.listener = listener;
191 }
192
193 /**
194 * Indicates whether estimator is locked while estimating fundamental
195 * matrix.
196 *
197 * @return true if estimator is locked, false otherwise.
198 */
199 public boolean isLocked() {
200 return locked;
201 }
202
203 /**
204 * Indicates whether estimator is ready to start the estimation when all
205 * required data has been provided.
206 *
207 * @return true if estimator is ready, false otherwise.
208 */
209 public boolean isReady() {
210 return homography != null && leftIntrinsics != null && rightIntrinsics != null;
211 }
212
213 /**
214 * Estimates fundamental matrices and returns the estimated result.
215 *
216 * @return estimated fundamental matrices.
217 * @throws LockedException if estimator is locked.
218 * @throws NotReadyException if estimator is not ready.
219 * @throws FundamentalMatrixEstimatorException if estimation fails for some
220 * reason.
221 */
222 public List<FundamentalMatrix> estimate() throws LockedException, NotReadyException,
223 FundamentalMatrixEstimatorException {
224 final var result = new ArrayList<FundamentalMatrix>();
225 estimate(result);
226 return result;
227 }
228
229 /**
230 * Estimates fundamental matrices and stores result into provided instance.
231 *
232 * @param result instance where result will be stored.
233 * @throws LockedException if estimator is locked.
234 * @throws NotReadyException if estimator is not ready.
235 * @throws FundamentalMatrixEstimatorException if estimation fails for some
236 * reason.
237 */
238 public void estimate(final List<FundamentalMatrix> result) throws LockedException, NotReadyException,
239 FundamentalMatrixEstimatorException {
240 if (isLocked()) {
241 throw new LockedException();
242 }
243 if (!isReady()) {
244 throw new NotReadyException();
245 }
246
247 try {
248 locked = true;
249
250 if (listener != null) {
251 listener.onEstimateStart(this);
252 }
253
254 final var decomposer = new HomographyDecomposer(homography, leftIntrinsics, rightIntrinsics);
255
256 final var decompositions = decomposer.decompose();
257 result.clear();
258 for (final var decomposition : decompositions) {
259 final var rotation = decomposition.getTransformation().getRotation();
260 final var translation = new HomogeneousPoint2D(decomposition.getTransformation().getTranslation());
261 final var essential = new EssentialMatrix(rotation, translation);
262 final var fundamentalMatrix = essential.toFundamentalMatrix(leftIntrinsics, rightIntrinsics);
263 result.add(fundamentalMatrix);
264 }
265
266 if (listener != null) {
267 listener.onEstimateEnd(this, result);
268 }
269
270 } catch (final GeometryException e) {
271 throw new FundamentalMatrixEstimatorException(e);
272 } finally {
273 locked = false;
274 }
275 }
276 }