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.PinholeCamera;
21  import com.irurueta.geometry.Point2D;
22  import com.irurueta.geometry.Point3D;
23  
24  import java.util.List;
25  
26  /**
27   * This class implements an algorithm to estimate pinhole cameras using the DLT
28   * algorithm and point correspondences.
29   */
30  @SuppressWarnings("DuplicatedCode")
31  public class DLTPointCorrespondencePinholeCameraEstimator extends PointCorrespondencePinholeCameraEstimator {
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       * Indicates if an LMSE (Least Mean Square Error) solution is allowed if
46       * more correspondences than the minimum are provided. If false, the
47       * exceeding correspondences will be ignored and only the 6 first
48       * correspondences will be used.
49       */
50      private boolean allowLMSESolution;
51  
52      /**
53       * Constructor.
54       */
55      public DLTPointCorrespondencePinholeCameraEstimator() {
56          super();
57          allowLMSESolution = DEFAULT_ALLOW_LMSE_SOLUTION;
58      }
59  
60      /**
61       * Constructor with listener.
62       *
63       * @param listener listener to be notified of events such as when estimation
64       *                 starts, ends or estimation progress changes.
65       */
66      public DLTPointCorrespondencePinholeCameraEstimator(final PinholeCameraEstimatorListener listener) {
67          super(listener);
68          allowLMSESolution = DEFAULT_ALLOW_LMSE_SOLUTION;
69      }
70  
71      /**
72       * Constructor.
73       *
74       * @param points3D list of corresponding 3D points.
75       * @param points2D list of corresponding 2D points.
76       * @throws IllegalArgumentException if any of the lists are null.
77       * @throws WrongListSizesException  if provided lists of points don't have
78       *                                  the same size and enough points.
79       */
80      public DLTPointCorrespondencePinholeCameraEstimator(
81              final List<Point3D> points3D, final List<Point2D> points2D) throws WrongListSizesException {
82          super(points3D, points2D);
83          allowLMSESolution = DEFAULT_ALLOW_LMSE_SOLUTION;
84      }
85  
86      /**
87       * Constructor.
88       *
89       * @param points3D list of corresponding 3D points.
90       * @param points2D list of corresponding 2D points.
91       * @param listener listener to be notified of events such as when estimation
92       *                 starts, ends or estimation progress changes.
93       * @throws IllegalArgumentException if any of the lists are null.
94       * @throws WrongListSizesException  if provided lists of points don't have
95       *                                  the same size and enough points.
96       */
97      public DLTPointCorrespondencePinholeCameraEstimator(
98              final List<Point3D> points3D, final List<Point2D> points2D, final PinholeCameraEstimatorListener listener)
99              throws WrongListSizesException {
100         super(points3D, points2D, listener);
101         allowLMSESolution = DEFAULT_ALLOW_LMSE_SOLUTION;
102     }
103 
104     /**
105      * Indicates if an LMSE (Least Mean Square Error) solution is allowed if
106      * more correspondences than the minimum are provided. If false, the
107      * exceeding correspondences will be ignored and only the 6 first
108      * correspondences will be used.
109      *
110      * @return true if LMSE solution is allowed, false otherwise.
111      */
112     public boolean isLMSESolutionAllowed() {
113         return allowLMSESolution;
114     }
115 
116     /**
117      * Specifies if an LMSE (Least Mean Square Error) solution is allowed if
118      * more correspondences than the minimum are provided. If false, the
119      * exceeding correspondences will be ignored and only the 6 first
120      * correspondences will be used.
121      *
122      * @param allowed true if LMSE solution is allowed, false otherwise.
123      * @throws LockedException if estimator is locked.
124      */
125     public void setLMSESolutionAllowed(final boolean allowed) throws LockedException {
126         if (isLocked()) {
127             throw new LockedException();
128         }
129         allowLMSESolution = allowed;
130     }
131 
132     /**
133      * Indicates if this estimator is ready to start the estimation.
134      *
135      * @return true if estimator is ready, false otherwise.
136      */
137     @Override
138     public boolean isReady() {
139         return areListsAvailable() && areValidLists(points3D, points2D);
140     }
141 
142     /**
143      * Internal method that actually computes the normalized pinhole camera
144      * internal matrix.
145      * Returned matrix must have norm equal to one and might be estimated using
146      * any convenient algorithm (i.e. DLT or weighted DLT).
147      *
148      * @param points3D list of 3D points. Points might or might not be
149      *                 normalized.
150      * @param points2D list of 2D points. Points might or might not be
151      *                 normalized.
152      * @return matrix of estimated pinhole camera.
153      * @throws PinholeCameraEstimatorException if estimation fails for some
154      *                                         reason (i.e. numerical instability or geometric degeneracy).
155      */
156     @Override
157     protected Matrix internalEstimate(
158             final List<Point3D> points3D, final List<Point2D> points2D) throws PinholeCameraEstimatorException {
159 
160         try {
161             final var nPoints = points2D.size();
162 
163             final Matrix a;
164             if (isLMSESolutionAllowed()) {
165                 // initialize new matrix to zero when LMSE is enabled
166                 a = new Matrix(2 * nPoints, 12);
167             } else {
168                 //When LMSE is disabled, initialize new matrix to zero only with
169                 //11 equations
170                 a = new Matrix(MIN_NUMBER_OF_EQUATIONS, 12);
171             }
172 
173             final var iterator2D = points2D.iterator();
174             final var iterator3D = points3D.iterator();
175 
176             Point2D point2D;
177             Point3D point3D;
178             var counter = 0;
179             double homImageX;
180             double homImageY;
181             double homImageW;
182             double homWorldX;
183             double homWorldY;
184             double homWorldZ;
185             double homWorldW;
186             double rowNorm;
187             while (iterator2D.hasNext() && iterator3D.hasNext()) {
188                 point2D = iterator2D.next();
189                 point3D = iterator3D.next();
190 
191                 // normalize points to increase accuracy
192                 point2D.normalize();
193                 point3D.normalize();
194 
195                 homImageX = point2D.getHomX();
196                 homImageY = point2D.getHomY();
197                 homImageW = point2D.getHomW();
198 
199                 homWorldX = point3D.getHomX();
200                 homWorldY = point3D.getHomY();
201                 homWorldZ = point3D.getHomZ();
202                 homWorldW = point3D.getHomW();
203 
204                 // first row (even)
205                 a.setElementAt(counter, 0, homImageW * homWorldX);
206                 a.setElementAt(counter, 1, homImageW * homWorldY);
207                 a.setElementAt(counter, 2, homImageW * homWorldZ);
208                 a.setElementAt(counter, 3, homImageW * homWorldW);
209 
210                 // columns 4, 5, 6, 7 are left with zero values
211 
212                 a.setElementAt(counter, 8, -homImageX * homWorldX);
213                 a.setElementAt(counter, 9, -homImageX * homWorldY);
214                 a.setElementAt(counter, 10, -homImageX * homWorldZ);
215                 a.setElementAt(counter, 11, -homImageX * homWorldW);
216 
217                 // normalize row
218                 rowNorm = Math.sqrt(Math.pow(a.getElementAt(counter, 0), 2.0)
219                         + Math.pow(a.getElementAt(counter, 1), 2.0)
220                         + Math.pow(a.getElementAt(counter, 2), 2.0)
221                         + Math.pow(a.getElementAt(counter, 3), 2.0)
222                         + Math.pow(a.getElementAt(counter, 8), 2.0)
223                         + Math.pow(a.getElementAt(counter, 9), 2.0)
224                         + Math.pow(a.getElementAt(counter, 10), 2.0)
225                         + Math.pow(a.getElementAt(counter, 11), 2.0));
226 
227                 a.setElementAt(counter, 0, a.getElementAt(counter, 0) / rowNorm);
228                 a.setElementAt(counter, 1, a.getElementAt(counter, 1) / rowNorm);
229                 a.setElementAt(counter, 2, a.getElementAt(counter, 2) / rowNorm);
230                 a.setElementAt(counter, 3, a.getElementAt(counter, 3) / rowNorm);
231                 a.setElementAt(counter, 8, a.getElementAt(counter, 8) / rowNorm);
232                 a.setElementAt(counter, 9, a.getElementAt(counter, 9) / rowNorm);
233                 a.setElementAt(counter, 10, a.getElementAt(counter, 10) / rowNorm);
234                 a.setElementAt(counter, 11, a.getElementAt(counter, 11) / rowNorm);
235                 counter++;
236 
237                 // in case we want an exact solution (up to scale) when LMSE is
238                 // disabled, we stop after 11 equations
239                 if (!isLMSESolutionAllowed() && (counter >= MIN_NUMBER_OF_EQUATIONS)) {
240                     break;
241                 }
242 
243                 // second row (odd)
244 
245                 // columns 0, 1, 2, 3 are left with zero values
246 
247                 a.setElementAt(counter, 4, homImageW * homWorldX);
248                 a.setElementAt(counter, 5, homImageW * homWorldY);
249                 a.setElementAt(counter, 6, homImageW * homWorldZ);
250                 a.setElementAt(counter, 7, homImageW * homWorldW);
251 
252                 a.setElementAt(counter, 8, -homImageY * homWorldX);
253                 a.setElementAt(counter, 9, -homImageY * homWorldY);
254                 a.setElementAt(counter, 10, -homImageY * homWorldZ);
255                 a.setElementAt(counter, 11, -homImageY * homWorldW);
256 
257                 // normalize row
258                 rowNorm = Math.sqrt(Math.pow(a.getElementAt(counter, 4), 2.0)
259                         + Math.pow(a.getElementAt(counter, 5), 2.0)
260                         + Math.pow(a.getElementAt(counter, 6), 2.0)
261                         + Math.pow(a.getElementAt(counter, 7), 2.0)
262                         + Math.pow(a.getElementAt(counter, 8), 2.0)
263                         + Math.pow(a.getElementAt(counter, 9), 2.0)
264                         + Math.pow(a.getElementAt(counter, 10), 2.0)
265                         + Math.pow(a.getElementAt(counter, 11), 2.0));
266 
267                 a.setElementAt(counter, 4, a.getElementAt(counter, 4) / rowNorm);
268                 a.setElementAt(counter, 5, a.getElementAt(counter, 5) / rowNorm);
269                 a.setElementAt(counter, 6, a.getElementAt(counter, 6) / rowNorm);
270                 a.setElementAt(counter, 7, a.getElementAt(counter, 7) / rowNorm);
271                 a.setElementAt(counter, 8, a.getElementAt(counter, 8) / rowNorm);
272                 a.setElementAt(counter, 9, a.getElementAt(counter, 9) / rowNorm);
273                 a.setElementAt(counter, 10, a.getElementAt(counter, 10) / rowNorm);
274                 a.setElementAt(counter, 11, a.getElementAt(counter, 11) / rowNorm);
275                 counter++;
276             }
277 
278             final var decomposer = new SingularValueDecomposer(a);
279             decomposer.decompose();
280 
281             if (decomposer.getNullity() > 1) {
282                 // point configuration is degenerate and exists a linear
283                 // combination of possible pinhole cameras (i.e. solution is not
284                 // unique up to scale)
285                 throw new PinholeCameraEstimatorException();
286             }
287 
288             final var v = decomposer.getV();
289 
290             // use last column of V as pinhole camera vector
291 
292             // the last column of V contains pinhole camera matrix ordered by
293             // rows as: P11, P12, P13, P14, P21, P22, P23, P24, P31, P32, P33,
294             // P34, hence we reorder p
295             final var pinholeCameraMatrix = new Matrix(
296                     PinholeCamera.PINHOLE_CAMERA_MATRIX_ROWS, PinholeCamera.PINHOLE_CAMERA_MATRIX_COLS);
297 
298             pinholeCameraMatrix.setElementAt(0, 0, v.getElementAt(0, 11));
299             pinholeCameraMatrix.setElementAt(0, 1, v.getElementAt(1, 11));
300             pinholeCameraMatrix.setElementAt(0, 2, v.getElementAt(2, 11));
301             pinholeCameraMatrix.setElementAt(0, 3, v.getElementAt(3, 11));
302 
303             pinholeCameraMatrix.setElementAt(1, 0, v.getElementAt(4, 11));
304             pinholeCameraMatrix.setElementAt(1, 1, v.getElementAt(5, 11));
305             pinholeCameraMatrix.setElementAt(1, 2, v.getElementAt(6, 11));
306             pinholeCameraMatrix.setElementAt(1, 3, v.getElementAt(7, 11));
307 
308             pinholeCameraMatrix.setElementAt(2, 0, v.getElementAt(8, 11));
309             pinholeCameraMatrix.setElementAt(2, 1, v.getElementAt(9, 11));
310             pinholeCameraMatrix.setElementAt(2, 2, v.getElementAt(10, 11));
311             pinholeCameraMatrix.setElementAt(2, 3, v.getElementAt(11, 11));
312 
313             // because pinholeCameraMatrix has been obtained as the last column
314             // of V, then its Frobenius norm will be 1 because SVD already
315             // returns normalized singular vector
316 
317             return pinholeCameraMatrix;
318 
319         } catch (final PinholeCameraEstimatorException e) {
320             throw e;
321         } catch (final Exception e) {
322             throw new PinholeCameraEstimatorException(e);
323         }
324     }
325 
326     /**
327      * Returns type of pinhole camera estimator.
328      *
329      * @return type of pinhole camera estimator.
330      */
331     @Override
332     public PinholeCameraEstimatorType getType() {
333         return PinholeCameraEstimatorType.DLT_POINT_PINHOLE_CAMERA_ESTIMATOR;
334     }
335 }