1 /*
2 * Copyright (C) 2013 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.geometry.estimators;
17
18 import com.irurueta.geometry.Line2D;
19 import com.irurueta.geometry.NotAvailableException;
20 import com.irurueta.geometry.PinholeCamera;
21 import com.irurueta.geometry.Plane;
22 import com.irurueta.geometry.refiners.DecomposedLinePlaneCorrespondencePinholeCameraRefiner;
23
24 import java.util.BitSet;
25 import java.util.Collections;
26 import java.util.List;
27
28 /**
29 * This file contains abstract implementation for pinhole camera estimators
30 * based on line/plane correspondences.
31 */
32 public abstract class LinePlaneCorrespondencePinholeCameraEstimator extends PinholeCameraEstimator {
33
34 /**
35 * Minimum number of required line/plane correspondences to estimate a
36 * camera.
37 */
38 public static final int MIN_NUMBER_OF_LINE_PLANE_CORRESPONDENCES = 4;
39
40 /**
41 * List of corresponding 3D planes.
42 */
43 protected List<Plane> planes;
44
45 /**
46 * List of corresponding 2D lines.
47 */
48 protected List<Line2D> lines2D;
49
50 /**
51 * Constructor.
52 */
53 protected LinePlaneCorrespondencePinholeCameraEstimator() {
54 super();
55 }
56
57 /**
58 * Constructor.
59 *
60 * @param listener listener to be notified of events such as when estimation
61 * starts, ends or estimation progress changes.
62 */
63 protected LinePlaneCorrespondencePinholeCameraEstimator(final PinholeCameraEstimatorListener listener) {
64 super(listener);
65 }
66
67 /**
68 * Constructor.
69 *
70 * @param planes list of corresponding 3D planes.
71 * @param lines2D list of corresponding 2D lines.
72 * @throws IllegalArgumentException if any of the lists are null.
73 * @throws WrongListSizesException if provided lists of points don't have
74 * the same size and enough correspondences.
75 */
76 protected LinePlaneCorrespondencePinholeCameraEstimator(final List<Plane> planes, final List<Line2D> lines2D)
77 throws WrongListSizesException {
78 super();
79 internalSetLists(planes, lines2D);
80 }
81
82 /**
83 * Constructor.
84 *
85 * @param planes list of corresponding 3D planes.
86 * @param lines2D list of corresponding 2D lines.
87 * @param listener listener to be notified of events such as when estimation
88 * starts, ends or estimation progress changes.
89 * @throws IllegalArgumentException if any of the lists are null.
90 * @throws WrongListSizesException if provided lists of points don't have
91 * the same size and enough correspondences.
92 */
93 protected LinePlaneCorrespondencePinholeCameraEstimator(
94 final List<Plane> planes, final List<Line2D> lines2D, final PinholeCameraEstimatorListener listener)
95 throws WrongListSizesException {
96 super(listener);
97 internalSetLists(planes, lines2D);
98 }
99
100 /**
101 * Internal method to set list of corresponding lines/planes (it does not
102 * check if estimator is locked).
103 *
104 * @param planes list of corresponding 3D planes.
105 * @param lines2D list of corresponding 2D lines.
106 * @throws IllegalArgumentException if any of the lists are null.
107 * @throws WrongListSizesException if provided lists of points don't have
108 * the same size and enough correspondences.
109 */
110 private void internalSetLists(final List<Plane> planes, final List<Line2D> lines2D) throws WrongListSizesException {
111
112 if (planes == null || lines2D == null) {
113 throw new IllegalArgumentException();
114 }
115
116 if (!areValidLists(planes, lines2D)) {
117 throw new WrongListSizesException();
118 }
119
120 this.planes = planes;
121 this.lines2D = lines2D;
122 }
123
124 /**
125 * Set list of corresponding lines/planes.
126 *
127 * @param planes list of corresponding 3D planes.
128 * @param lines2D list of corresponding 2D lines.
129 * @throws LockedException if estimator is locked.
130 * @throws IllegalArgumentException if any of the lists are null.
131 * @throws WrongListSizesException if provided lists of points don't have
132 * the same size and enough correspondences.
133 */
134 public void setLists(final List<Plane> planes, final List<Line2D> lines2D) throws LockedException,
135 WrongListSizesException {
136 if (isLocked()) {
137 throw new LockedException();
138 }
139
140 internalSetLists(planes, lines2D);
141 }
142
143 /**
144 * Returns list of corresponding lines/planes.
145 * Notice that this method returns an unmodifiable list of planes to avoid
146 * undesired modifications.
147 *
148 * @return list of corresponding 3D planes.
149 * @throws NotAvailableException if list of planes is not yet available.
150 */
151 public List<Plane> getPlanes() throws NotAvailableException {
152 if (planes == null) {
153 throw new NotAvailableException();
154 }
155
156 // to avoid undesired modifications
157 return Collections.unmodifiableList(planes);
158 }
159
160 /**
161 * Returns list of corresponding 2D lines.
162 * Notice that this method returns an unmodifiable list of 2D lines to avoid
163 * undesired modifications.
164 *
165 * @return list of corresponding 2D lines.
166 * @throws NotAvailableException if list of 2D lines is not yet available.
167 */
168 public List<Line2D> getLines2D() throws NotAvailableException {
169 if (lines2D == null) {
170 throw new NotAvailableException();
171 }
172
173 // to avoid undesired modifications
174 return Collections.unmodifiableList(lines2D);
175 }
176
177 /**
178 * Indicates if lists of corresponding 2D lines and planes are valid.
179 * Lists are considered valid if they have the same number of points and
180 * both have more than the required minimum of correspondences (which is 4).
181 *
182 * @param planes list of corresponding 3D planes.
183 * @param lines2D list of corresponding 2D lines.
184 * @return true if corresponding 2D lines and planes are valid, false
185 * otherwise.
186 */
187 public static boolean areValidLists(final List<Plane> planes, final List<Line2D> lines2D) {
188 if (planes == null || lines2D == null) {
189 return false;
190 }
191 return planes.size() == lines2D.size() && lines2D.size() >= MIN_NUMBER_OF_LINE_PLANE_CORRESPONDENCES;
192 }
193
194 /**
195 * Indicates if lists have already been provided and are available for
196 * retrieval.
197 *
198 * @return true if available, false otherwise.
199 */
200 public boolean areListsAvailable() {
201 return planes != null && lines2D != null;
202 }
203
204 /**
205 * Attempts to refine provided camera using requested suggestions.
206 * If no suggestions are requested or if refinement fails, provided
207 * camera is returned instead.
208 *
209 * @param pinholeCamera camera to be refined.
210 * @return refined camera.
211 */
212 @SuppressWarnings("DuplicatedCode")
213 @Override
214 protected PinholeCamera attemptRefine(PinholeCamera pinholeCamera) {
215 if (hasSuggestions()) {
216 final var numPoints = planes.size();
217 final var inliers = new BitSet(numPoints);
218 inliers.set(0, numPoints, true);
219 final var residuals = new double[numPoints];
220
221 final var refiner = new DecomposedLinePlaneCorrespondencePinholeCameraRefiner(pinholeCamera,
222 false, inliers, residuals, numPoints, planes, lines2D, 0.0);
223 try {
224 refiner.setMinSuggestionWeight(minSuggestionWeight);
225 refiner.setMaxSuggestionWeight(maxSuggestionWeight);
226 refiner.setSuggestionWeightStep(suggestionWeightStep);
227
228 refiner.setSuggestSkewnessValueEnabled(suggestSkewnessValueEnabled);
229 refiner.setSuggestedSkewnessValue(suggestedSkewnessValue);
230 refiner.setSuggestHorizontalFocalLengthEnabled(suggestHorizontalFocalLengthEnabled);
231 refiner.setSuggestedHorizontalFocalLengthValue(suggestedHorizontalFocalLengthValue);
232 refiner.setSuggestVerticalFocalLengthEnabled(suggestVerticalFocalLengthEnabled);
233 refiner.setSuggestedVerticalFocalLengthValue(suggestedVerticalFocalLengthValue);
234 refiner.setSuggestAspectRatioEnabled(suggestAspectRatioEnabled);
235 refiner.setSuggestedAspectRatioValue(suggestedAspectRatioValue);
236 refiner.setSuggestPrincipalPointEnabled(suggestPrincipalPointEnabled);
237 refiner.setSuggestedPrincipalPointValue(suggestedPrincipalPointValue);
238 refiner.setSuggestRotationEnabled(suggestRotationEnabled);
239 refiner.setSuggestedRotationValue(suggestedRotationValue);
240 refiner.setSuggestCenterEnabled(suggestCenterEnabled);
241 refiner.setSuggestedCenterValue(suggestedCenterValue);
242
243 final var result = new PinholeCamera();
244 final var improved = refiner.refine(result);
245
246 return improved ? result : pinholeCamera;
247
248 } catch (final Exception e) {
249 return pinholeCamera;
250 }
251 } else {
252 return pinholeCamera;
253 }
254 }
255 }