View Javadoc
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.algebra.Matrix;
19  import com.irurueta.algebra.SingularValueDecomposer;
20  import com.irurueta.geometry.Line2D;
21  import com.irurueta.geometry.PinholeCamera;
22  import com.irurueta.geometry.Plane;
23  
24  import java.util.List;
25  
26  /**
27   * This class implements an algorithm to estimate pinhole camera using the DLT
28   * algorithm and point correspondences.
29   */
30  @SuppressWarnings("DuplicatedCode")
31  public class DLTLinePlaneCorrespondencePinholeCameraEstimator extends LinePlaneCorrespondencePinholeCameraEstimator {
32  
33      /**
34       * Minimum number of required equations to estimate a pinhole camera.
35       */
36      public static final int MIN_NUMBER_OF_EQUATIONS = 11;
37  
38      /**
39       * Indicates if by default an LMSE (Least Mean Square Error) solution is
40       * allowed if more correspondences than the minimum are provided.
41       */
42      public static final boolean DEFAULT_ALLOW_LMSE_SOLUTION = false;
43  
44      /**
45       * Defines tiny value considered as machine precision.
46       */
47      public static final double EPS = 1e-8;
48  
49      /**
50       * Indicates if an LMSE (Least Mean Square Error) solution is allowed if
51       * more correspondences than the minimum are provided. If false, the
52       * exceeding correspondences will be ignored and only the 6 first
53       * correspondences will be used.
54       */
55      private boolean allowLMSESolution;
56  
57      /**
58       * Constructor.
59       */
60      public DLTLinePlaneCorrespondencePinholeCameraEstimator() {
61          super();
62          allowLMSESolution = DEFAULT_ALLOW_LMSE_SOLUTION;
63      }
64  
65      /**
66       * Constructor with listener.
67       *
68       * @param listener listener to be notified of events such as when estimation
69       *                 starts, ends or estimation progress changes.
70       */
71      public DLTLinePlaneCorrespondencePinholeCameraEstimator(final PinholeCameraEstimatorListener listener) {
72          super(listener);
73          allowLMSESolution = DEFAULT_ALLOW_LMSE_SOLUTION;
74      }
75  
76      /**
77       * Constructor.
78       *
79       * @param planes  list of corresponding 3D planes.
80       * @param lines2D list of corresponding 2D lines.
81       * @throws IllegalArgumentException if any of the lists are null.
82       * @throws WrongListSizesException  if provided lists of correspondences
83       *                                  don't have the same size and enough correspondences.
84       */
85      public DLTLinePlaneCorrespondencePinholeCameraEstimator(
86              final List<Plane> planes, final List<Line2D> lines2D) throws WrongListSizesException {
87          super(planes, lines2D);
88          allowLMSESolution = DEFAULT_ALLOW_LMSE_SOLUTION;
89      }
90  
91      /**
92       * Constructor.
93       *
94       * @param planes   list of corresponding 3D planes.
95       * @param lines2D  list of corresponding 2D lines.
96       * @param listener listener to be notified of events such as when estimation
97       *                 starts, ends or estimation progress changes.
98       * @throws IllegalArgumentException if any of the lists are null.
99       * @throws WrongListSizesException  if provided lists of correspondences
100      *                                  don't have the same size and enough correspondences.
101      */
102     public DLTLinePlaneCorrespondencePinholeCameraEstimator(
103             final List<Plane> planes, final List<Line2D> lines2D, final PinholeCameraEstimatorListener listener)
104             throws WrongListSizesException {
105         super(planes, lines2D, listener);
106         allowLMSESolution = DEFAULT_ALLOW_LMSE_SOLUTION;
107     }
108 
109     /**
110      * Indicates if an LMSE (Least Mean Square Error) solution is allowed if
111      * more correspondences than the minimum are provided. If false, the
112      * exceeding correspondences will be ignored and only the 6 first
113      * correspondences will be used.
114      *
115      * @return true if LMSE solution is allowed, false otherwise.
116      */
117     public boolean isLMSESolutionAllowed() {
118         return allowLMSESolution;
119     }
120 
121     /**
122      * Specifies if an LMSE (Least Mean Square Error) solution is allowed if
123      * more correspondences than the minimum are provided. If false, the
124      * exceeding correspondences will be ignored and only the 6 first
125      * correspondences will be used.
126      *
127      * @param allowed true if LMSE solution is allowed, false otherwise
128      * @throws LockedException if estimator is locked.
129      */
130     public void setLMSESolutionAllowed(final boolean allowed) throws LockedException {
131         if (isLocked()) {
132             throw new LockedException();
133         }
134         allowLMSESolution = allowed;
135     }
136 
137     /**
138      * Indicates if this estimator is ready to start the estimation.
139      *
140      * @return true if estimator is ready, false otherwise.
141      */
142     @Override
143     public boolean isReady() {
144         return areListsAvailable();
145     }
146 
147     /**
148      * Estimates a pinhole camera.
149      *
150      * @return estimated pinhole camera.
151      * @throws LockedException                 if estimator is locked.
152      * @throws NotReadyException               if input has not yet been provided.
153      * @throws PinholeCameraEstimatorException if an error occurs during
154      *                                         estimation, usually because input data is not valid.
155      */
156     @Override
157     public PinholeCamera estimate() throws LockedException, NotReadyException, PinholeCameraEstimatorException {
158 
159         if (isLocked()) {
160             throw new LockedException();
161         }
162         if (!isReady()) {
163             throw new NotReadyException();
164         }
165 
166         try {
167             final var nLines = lines2D.size();
168 
169             locked = true;
170             if (listener != null) {
171                 listener.onEstimateStart(this);
172             }
173 
174             final Matrix a;
175             if (isLMSESolutionAllowed()) {
176                 // initialize new matrix to zero when LMSE is enabled
177                 a = new Matrix(3 * nLines, 12);
178             } else {
179                 // When LMSE is disabled, initialize new matrix to zero only with
180                 // 11 equations
181                 a = new Matrix(MIN_NUMBER_OF_EQUATIONS, 12);
182             }
183 
184             final var iterator2D = lines2D.iterator();
185             final var iterator3D = planes.iterator();
186 
187             Line2D line2D;
188             Plane plane;
189             var counter = 0;
190             double la;
191             double lb;
192             double lc;
193             double pA;
194             double pB;
195             double pC;
196             double pD;
197             double rowNorm;
198             while (iterator2D.hasNext() && iterator3D.hasNext()) {
199                 line2D = iterator2D.next();
200                 plane = iterator3D.next();
201 
202                 // normalize lines and planes to increase accuracy
203                 line2D.normalize();
204                 plane.normalize();
205 
206                 la = line2D.getA();
207                 lb = line2D.getB();
208                 lc = line2D.getC();
209 
210                 pA = plane.getA();
211                 pB = plane.getB();
212                 pC = plane.getC();
213                 pD = plane.getD();
214 
215                 // first row
216                 a.setElementAt(counter, 0, -pD * la);
217                 a.setElementAt(counter, 1, -pD * lb);
218                 a.setElementAt(counter, 2, -pD * lc);
219 
220                 // columns 3, 4, 5, 6, 7, 8 are left with zero values
221 
222                 a.setElementAt(counter, 9, pA * la);
223                 a.setElementAt(counter, 10, pA * lb);
224                 a.setElementAt(counter, 11, pA * lc);
225 
226                 // normalize row
227                 rowNorm = Math.sqrt(Math.pow(a.getElementAt(counter, 0), 2.0)
228                         + Math.pow(a.getElementAt(counter, 1), 2.0)
229                         + Math.pow(a.getElementAt(counter, 2), 2.0)
230                         + Math.pow(a.getElementAt(counter, 9), 2.0)
231                         + Math.pow(a.getElementAt(counter, 10), 2.0)
232                         + Math.pow(a.getElementAt(counter, 11), 2.0));
233 
234                 a.setElementAt(counter, 0, a.getElementAt(counter, 0) / rowNorm);
235                 a.setElementAt(counter, 1, a.getElementAt(counter, 1) / rowNorm);
236                 a.setElementAt(counter, 2, a.getElementAt(counter, 2) / rowNorm);
237                 a.setElementAt(counter, 9, a.getElementAt(counter, 9) / rowNorm);
238                 a.setElementAt(counter, 10, a.getElementAt(counter, 10) / rowNorm);
239                 a.setElementAt(counter, 11, a.getElementAt(counter, 11) / rowNorm);
240                 counter++;
241 
242                 // second row
243 
244                 // columns 0, 1, 2 are left with zero values
245 
246                 a.setElementAt(counter, 3, -pD * la);
247                 a.setElementAt(counter, 4, -pD * lb);
248                 a.setElementAt(counter, 5, -pD * lc);
249 
250                 // columns 6, 7, 8 are left with zero values
251 
252                 a.setElementAt(counter, 9, pB * la);
253                 a.setElementAt(counter, 10, pB * lb);
254                 a.setElementAt(counter, 11, pB * lc);
255 
256                 // normalize row
257                 rowNorm = Math.sqrt(Math.pow(a.getElementAt(counter, 3), 2.0)
258                         + Math.pow(a.getElementAt(counter, 4), 2.0)
259                         + Math.pow(a.getElementAt(counter, 5), 2.0)
260                         + Math.pow(a.getElementAt(counter, 9), 2.0)
261                         + Math.pow(a.getElementAt(counter, 10), 2.0)
262                         + Math.pow(a.getElementAt(counter, 11), 2.0));
263 
264                 a.setElementAt(counter, 3, a.getElementAt(counter, 3) / rowNorm);
265                 a.setElementAt(counter, 4, a.getElementAt(counter, 4) / rowNorm);
266                 a.setElementAt(counter, 5, a.getElementAt(counter, 5) / rowNorm);
267                 a.setElementAt(counter, 9, a.getElementAt(counter, 9) / rowNorm);
268                 a.setElementAt(counter, 10, a.getElementAt(counter, 10) / rowNorm);
269                 a.setElementAt(counter, 11, a.getElementAt(counter, 11) / rowNorm);
270                 counter++;
271 
272 
273                 // in case we want an exact solution (up to scale) when LMSE is
274                 // disabled, we stop after 11 equations
275                 if (!isLMSESolutionAllowed() && (counter >= MIN_NUMBER_OF_EQUATIONS)) {
276                     break;
277                 }
278 
279                 // third row
280 
281                 // columns 0, 1, 2, 3, 4, 5 are left with zero values
282 
283                 a.setElementAt(counter, 6, -pD * la);
284                 a.setElementAt(counter, 7, -pD * lb);
285                 a.setElementAt(counter, 8, -pD * lc);
286 
287                 a.setElementAt(counter, 9, pC * la);
288                 a.setElementAt(counter, 10, pC * lb);
289                 a.setElementAt(counter, 11, pC * lc);
290 
291                 // normalize row
292                 rowNorm = Math.sqrt(Math.pow(a.getElementAt(counter, 6), 2.0)
293                         + Math.pow(a.getElementAt(counter, 7), 2.0)
294                         + Math.pow(a.getElementAt(counter, 8), 2.0)
295                         + Math.pow(a.getElementAt(counter, 9), 2.0)
296                         + Math.pow(a.getElementAt(counter, 10), 2.0)
297                         + Math.pow(a.getElementAt(counter, 11), 2.0));
298 
299                 a.setElementAt(counter, 6, a.getElementAt(counter, 6) / rowNorm);
300                 a.setElementAt(counter, 7, a.getElementAt(counter, 7) / rowNorm);
301                 a.setElementAt(counter, 8, a.getElementAt(counter, 8) / rowNorm);
302                 a.setElementAt(counter, 9, a.getElementAt(counter, 9) / rowNorm);
303                 a.setElementAt(counter, 10, a.getElementAt(counter, 10) / rowNorm);
304                 a.setElementAt(counter, 11, a.getElementAt(counter, 11) / rowNorm);
305                 counter++;
306             }
307 
308             final var decomposer = new SingularValueDecomposer(a);
309             decomposer.decompose();
310 
311             if (decomposer.getNullity() > 1) {
312                 // line/plane configuration is degenerate and exists a linear
313                 // combination of possible pinhole cameras (i.e. solution is not
314                 // unique up to scale)
315                 throw new PinholeCameraEstimatorException();
316             }
317 
318             final var v = decomposer.getV();
319 
320             // use last column of V as pinhole camera vector
321 
322             // the last column of V contains pinhole camera matrix ordered by
323             // columns as: P11, P21, P31, P12, P22, P32, P13, P23, P33, P14, P24,
324             // P34, hence we reorder p
325             final var pinholeCameraMatrix = new Matrix(
326                     PinholeCamera.PINHOLE_CAMERA_MATRIX_ROWS, PinholeCamera.PINHOLE_CAMERA_MATRIX_COLS);
327 
328             pinholeCameraMatrix.setElementAt(0, 0, v.getElementAt(0, 11));
329             pinholeCameraMatrix.setElementAt(1, 0, v.getElementAt(1, 11));
330             pinholeCameraMatrix.setElementAt(2, 0, v.getElementAt(2, 11));
331 
332             pinholeCameraMatrix.setElementAt(0, 1, v.getElementAt(3, 11));
333             pinholeCameraMatrix.setElementAt(1, 1, v.getElementAt(4, 11));
334             pinholeCameraMatrix.setElementAt(2, 1, v.getElementAt(5, 11));
335 
336             pinholeCameraMatrix.setElementAt(0, 2, v.getElementAt(6, 11));
337             pinholeCameraMatrix.setElementAt(1, 2, v.getElementAt(7, 11));
338             pinholeCameraMatrix.setElementAt(2, 2, v.getElementAt(8, 11));
339 
340             pinholeCameraMatrix.setElementAt(0, 3, v.getElementAt(9, 11));
341             pinholeCameraMatrix.setElementAt(1, 3, v.getElementAt(10, 11));
342             pinholeCameraMatrix.setElementAt(2, 3, v.getElementAt(11, 11));
343 
344             // because pinholeCameraMatrix has been obtained as the last column
345             // of V, then its Frobenius norm will be 1 because SVD already
346             // returns normalized singular vector
347 
348             final var camera = new PinholeCamera(pinholeCameraMatrix);
349 
350             if (listener != null) {
351                 listener.onEstimateEnd(this);
352             }
353 
354             return attemptRefine(camera);
355 
356         } catch (final PinholeCameraEstimatorException e) {
357             throw e;
358         } catch (final Exception e) {
359             throw new PinholeCameraEstimatorException(e);
360         } finally {
361             locked = false;
362         }
363     }
364 
365     /**
366      * Returns type of pinhole camera estimator.
367      *
368      * @return type of pinhole camera estimator.
369      */
370     @Override
371     public PinholeCameraEstimatorType getType() {
372         return PinholeCameraEstimatorType.DLT_LINE_PLANE_PINHOLE_CAMERA_ESTIMATOR;
373     }
374 }