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.calibration.estimators;
17
18 import com.irurueta.ar.calibration.ImageOfAbsoluteConic;
19 import com.irurueta.geometry.HomogeneousPoint3D;
20 import com.irurueta.geometry.MatrixRotation3D;
21 import com.irurueta.geometry.PinholeCamera;
22 import com.irurueta.geometry.ProjectiveTransformation2D;
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
29 /**
30 * This class estimate intrinsic and extrinsic (rotation and camera center)
31 * parameters of a camera by using provided homography.
32 * For intrinsic parameters estimation it is assumed that skewness and principal
33 * point are zero and that aspect ratio are known.
34 * This class can be used in planar scenes where projected points of two views
35 * are related by an homography.
36 */
37 public class SingleHomographyPinholeCameraEstimator {
38
39 /**
40 * Default aspect ratio value.
41 */
42 public static final double DEFAULT_ASPECT_RATIO = 1.0;
43
44 /**
45 * Aspect ratio of intrinsic parameters.
46 */
47 private double focalDistanceAspectRatio = DEFAULT_ASPECT_RATIO;
48
49 /**
50 * Homography relating two views.
51 */
52 private Transformation2D homography;
53
54 /**
55 * True when estimation is in progress.
56 */
57 private boolean locked = false;
58
59 /**
60 * Listener to be notified of events such as when estimation starts or ends.
61 */
62 private SingleHomographyPinholeCameraEstimatorListener listener;
63
64 /**
65 * Constructor.
66 */
67 public SingleHomographyPinholeCameraEstimator() {
68 }
69
70 /**
71 * Constructor with listener.
72 *
73 * @param listener listener to be notified of events such as when estimation
74 * starts or ends.
75 */
76 public SingleHomographyPinholeCameraEstimator(final SingleHomographyPinholeCameraEstimatorListener listener) {
77 this.listener = listener;
78 }
79
80 /**
81 * Constructor with homography.
82 *
83 * @param homography homography to estimate camera and intrinsic parameters.
84 * @throws NullPointerException if provided homography is null.
85 */
86 public SingleHomographyPinholeCameraEstimator(final Transformation2D homography) {
87 internalSetHomography(homography);
88 }
89
90 /**
91 * Constructor with listener and homography.
92 *
93 * @param homography homography to estimate camera and intrinsic parameters.
94 * @param listener listener to be notified of events such as when estimation
95 * starts or ends.
96 */
97 public SingleHomographyPinholeCameraEstimator(
98 final Transformation2D homography, final SingleHomographyPinholeCameraEstimatorListener listener) {
99 internalSetHomography(homography);
100 this.listener = listener;
101 }
102
103 /**
104 * Gets aspect ratio of intrinsic parameters.
105 *
106 * @return aspect ratio of intrinsic parameters.
107 */
108 public double getFocalDistanceAspectRatio() {
109 return focalDistanceAspectRatio;
110 }
111
112 /**
113 * Sets aspect ratio of intrinsic parameters.
114 *
115 * @param focalDistanceAspectRatio aspect ratio of intrinsic parameters.
116 * @throws LockedException if estimator is locked.
117 */
118 public void setFocalDistanceAspectRatio(final double focalDistanceAspectRatio) throws LockedException {
119 if (locked) {
120 throw new LockedException();
121 }
122 this.focalDistanceAspectRatio = focalDistanceAspectRatio;
123 }
124
125 /**
126 * Gets homography relating two views.
127 *
128 * @return homography relating two views.
129 */
130 public Transformation2D getHomography() {
131 return homography;
132 }
133
134 /**
135 * Sets homography relating two views.
136 *
137 * @param homography homography relating two views.
138 * @throws LockedException if estimator is locked.
139 */
140 public void setHomography(final Transformation2D homography) throws LockedException {
141 if (locked) {
142 throw new LockedException();
143 }
144 this.homography = homography;
145 }
146
147 /**
148 * Indicates whether this instance is locked.
149 *
150 * @return true if this estimator is busy doing the estimation, false
151 * otherwise.
152 */
153 public boolean isReady() {
154 return homography != null;
155 }
156
157 /**
158 * Gets listener to be notified of events such as when estimation starts or
159 * ends.
160 *
161 * @return listener to be notified of events.
162 */
163 public SingleHomographyPinholeCameraEstimatorListener getListener() {
164 return listener;
165 }
166
167 /**
168 * Sets listener to be notified of events such as when estimation starts or
169 * ends.
170 *
171 * @param listener listener to be notified of events.
172 * @throws LockedException if estimator is locked.
173 */
174 public void setListener(final SingleHomographyPinholeCameraEstimatorListener listener) throws LockedException {
175 if (locked) {
176 throw new LockedException();
177 }
178 this.listener = listener;
179 }
180
181 /**
182 * Indicates whether this instance is locked or not.
183 *
184 * @return true if instance is locked, false otherwise.
185 */
186 public boolean isLocked() {
187 return locked;
188 }
189
190 /**
191 * Estimates a pinhole camera.
192 *
193 * @return estimated pinhole camera.
194 * @throws LockedException if estimator is locked.
195 * @throws NotReadyException if no homography has been provided yet.
196 * @throws SingleHomographyPinholeCameraEstimatorException if estimation
197 * fails.
198 */
199 public PinholeCamera estimate() throws LockedException, NotReadyException,
200 SingleHomographyPinholeCameraEstimatorException {
201 final var result = new PinholeCamera();
202 estimate(result);
203 return result;
204 }
205
206 /**
207 * Estimates a pinhole camera.
208 *
209 * @param result instance where estimated camera will be stored.
210 * @throws LockedException if estimator is locked.
211 * @throws NotReadyException if no homography has been provided yet.
212 * @throws SingleHomographyPinholeCameraEstimatorException if estimation
213 * fails.
214 */
215 public void estimate(PinholeCamera result) throws LockedException, NotReadyException,
216 SingleHomographyPinholeCameraEstimatorException {
217 if (locked) {
218 throw new LockedException();
219 }
220
221 if (!isReady()) {
222 throw new NotReadyException();
223 }
224
225 try {
226 locked = true;
227
228 if (listener != null) {
229 listener.onEstimateStart(this);
230 }
231
232 // estimate intrinsic parameters
233 final var homographies = new ArrayList<Transformation2D>();
234 homographies.add(homography);
235 // also add its inverse
236 homographies.add(((ProjectiveTransformation2D) homography).inverseAndReturnNew());
237 final var intrinsicEstimator = new LMSEImageOfAbsoluteConicEstimator(homographies);
238 intrinsicEstimator.setPrincipalPointAtOrigin(true);
239 intrinsicEstimator.setFocalDistanceAspectRatioKnown(true);
240 intrinsicEstimator.setFocalDistanceAspectRatio(focalDistanceAspectRatio);
241 ImageOfAbsoluteConic iac = intrinsicEstimator.estimate();
242 iac.normalize();
243
244 final var intrinsic = iac.getIntrinsicParameters();
245
246 // estimate camera pose
247 final var rotation = new MatrixRotation3D();
248 final var center = new HomogeneousPoint3D();
249 CameraPoseEstimator.estimate(intrinsic, homography, rotation, center, result);
250
251 if (listener != null) {
252 listener.onEstimateEnd(this);
253 }
254
255 } catch (final Exception e) {
256 throw new SingleHomographyPinholeCameraEstimatorException(e);
257 } finally {
258 locked = false;
259 }
260 }
261
262 /**
263 * Sets homography to estimate camera and intrinsic parameters.
264 *
265 * @param homography homography to be set.
266 * @throws NullPointerException if provided homography is null.
267 */
268 private void internalSetHomography(final Transformation2D homography) {
269 if (homography == null) {
270 throw new NullPointerException();
271 }
272 this.homography = homography;
273 }
274 }