1 /*
2 * Copyright (C) 2015 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.sfm;
17
18 import com.irurueta.algebra.AlgebraException;
19 import com.irurueta.algebra.Matrix;
20 import com.irurueta.algebra.SingularValueDecomposer;
21 import com.irurueta.geometry.CameraException;
22 import com.irurueta.geometry.PinholeCamera;
23 import com.irurueta.geometry.Plane;
24 import com.irurueta.geometry.Point2D;
25 import com.irurueta.geometry.Point3D;
26 import com.irurueta.geometry.estimators.LockedException;
27
28 import java.util.List;
29
30 /**
31 * Triangulates matched 2D points into a single 3D one by using 2D point
32 * correspondences on different views along with the corresponding cameras on
33 * each of those views by finding an LMSE solution to homogeneous systems of
34 * equations.
35 */
36 public class LMSEHomogeneousSinglePoint3DTriangulator extends SinglePoint3DTriangulator {
37
38 /**
39 * Indicates if by default an LMSE (the Least Mean Square Error) is allowed if
40 * 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 (the 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 LMSEHomogeneousSinglePoint3DTriangulator() {
56 super();
57 allowLMSESolution = DEFAULT_ALLOW_LMSE_SOLUTION;
58 }
59
60 /**
61 * Constructor.
62 *
63 * @param points2D list of matched 2D points on each view. Each point in the
64 * list is assumed to be projected by the corresponding camera in the list.
65 * @param cameras camera for each view where 2D points are represented.
66 * @throws IllegalArgumentException if provided lists don't have the same
67 * length or their length is less than 2 views, which is the minimum
68 * required to compute triangulation.
69 */
70 public LMSEHomogeneousSinglePoint3DTriangulator(final List<Point2D> points2D, final List<PinholeCamera> cameras) {
71 super(points2D, cameras);
72 allowLMSESolution = DEFAULT_ALLOW_LMSE_SOLUTION;
73 }
74
75 /**
76 * Constructor.
77 *
78 * @param listener listener to notify events generated by instances of this
79 * class.
80 */
81 public LMSEHomogeneousSinglePoint3DTriangulator(final SinglePoint3DTriangulatorListener listener) {
82 super(listener);
83 allowLMSESolution = DEFAULT_ALLOW_LMSE_SOLUTION;
84 }
85
86 /**
87 * Constructor.
88 *
89 * @param points2D list of matched 2D points on each view. Each point in the
90 * list is assumed to be projected by the corresponding camera in the list.
91 * @param cameras cameras for each view where 2D points are represented.
92 * @param listener listener to notify events generated by instances of this
93 * class.
94 * @throws IllegalArgumentException if provided lists don't have the same
95 * length or their length is less than 2 views, which is the minimum
96 * required to compute triangulation.
97 */
98 public LMSEHomogeneousSinglePoint3DTriangulator(final List<Point2D> points2D,
99 final List<PinholeCamera> cameras,
100 final SinglePoint3DTriangulatorListener listener) {
101 super(points2D, cameras, listener);
102 allowLMSESolution = DEFAULT_ALLOW_LMSE_SOLUTION;
103 }
104
105 /**
106 * Indicates if an LMSE (the Least Mean Square Error) solution is allowed if
107 * more correspondences than the minimum are provided. If false, the
108 * exceeding correspondences will be ignored and only the 2 first matches
109 * corresponding to the first 2 views will be used.
110 *
111 * @return true if LMSE solution is allowed, false otherwise.
112 */
113 public boolean isLMSESolutionAllowed() {
114 return allowLMSESolution;
115 }
116
117 /**
118 * Specifies if an LMSE (the Least Mean Square Error) solution is allowed if
119 * more correspondences than the minimum are provided. If false, the
120 * exceeding correspondences will be ignored and only the 2 first matches
121 * corresponding to the first 2 views will be used.
122 *
123 * @param allowed true if LMSE solution is allowed, false otherwise.
124 * @throws LockedException if estimator is locked.
125 */
126 public void setLMSESolutionAllowed(final boolean allowed) throws LockedException {
127 if (isLocked()) {
128 throw new LockedException();
129 }
130 allowLMSESolution = allowed;
131 }
132
133 /**
134 * Returns type of triangulator.
135 *
136 * @return type of triangulator.
137 */
138 @Override
139 public Point3DTriangulatorType getType() {
140 return Point3DTriangulatorType.LMSE_HOMOGENEOUS_TRIANGULATOR;
141 }
142
143 /**
144 * Internal method to triangulate provided matched 2D points being projected
145 * by each corresponding camera into a single 3D point.
146 * At least 2 matched 2D points and their corresponding 2 cameras are
147 * required to compute triangulation. If more views are provided, an
148 * averaged solution is found.
149 * This method does not check whether instance is locked or ready.
150 *
151 * @param points2D matched 2D points. Each point in the list is assumed to
152 * be projected by the corresponding camera in the list.
153 * @param cameras list of cameras associated to the matched 2D point on the
154 * same position as the camera on the list.
155 * @param result instance where triangulated 3D point is stored.
156 * @throws Point3DTriangulationException if triangulation fails for some
157 * other reason (i.e. degenerate geometry, numerical
158 * instabilities, etc.).
159 */
160 @SuppressWarnings("DuplicatedCode")
161 @Override
162 protected void triangulate(
163 final List<Point2D> points2D, final List<PinholeCamera> cameras, final Point3D result)
164 throws Point3DTriangulationException {
165 try {
166 locked = true;
167
168 if (listener != null) {
169 listener.onTriangulateStart(this);
170 }
171
172 final var numViews = cameras.size();
173
174 final Matrix a;
175 if (allowLMSESolution) {
176 // each view will add 2 equations to the linear system of
177 // equations
178 a = new Matrix(2 * numViews, 2 * MIN_REQUIRED_VIEWS);
179 } else {
180 a = new Matrix(2 * MIN_REQUIRED_VIEWS, 2 * MIN_REQUIRED_VIEWS);
181 }
182
183 Point2D point;
184 PinholeCamera camera;
185 final var horizontalAxisPlane = new Plane();
186 final var verticalAxisPlane = new Plane();
187 final var principalPlane = new Plane();
188 var row = 0;
189 double rowNorm;
190 for (var i = 0; i < numViews; i++) {
191 point = points2D.get(i);
192 camera = cameras.get(i);
193
194 // to increase accuracy
195 point.normalize();
196 camera.normalize();
197 camera.fixCameraSign();
198
199 final var homX = point.getHomX();
200 final var homY = point.getHomY();
201 final var homW = point.getHomW();
202
203 // pick rows of camera corresponding to different planes
204 // (we do not normalize planes, as it would introduce errors)
205
206 // 1st camera row (p1T)
207 camera.verticalAxisPlane(verticalAxisPlane);
208 // 2nd camera row (p2T)
209 camera.horizontalAxisPlane(horizontalAxisPlane);
210 // 3rd camera row (p3T)
211 camera.principalPlane(principalPlane);
212
213 // 1st equation
214 a.setElementAt(row, 0, homX * principalPlane.getA() - homW * verticalAxisPlane.getA());
215 a.setElementAt(row, 1, homX * principalPlane.getB() - homW * verticalAxisPlane.getB());
216 a.setElementAt(row, 2, homX * principalPlane.getC() - homW * verticalAxisPlane.getC());
217 a.setElementAt(row, 3, homX * principalPlane.getD() - homW * verticalAxisPlane.getD());
218
219 // normalize row (equation) to increase accuracy
220 rowNorm = Math.sqrt(Math.pow(a.getElementAt(row, 0), 2.0)
221 + Math.pow(a.getElementAt(row, 1), 2.0)
222 + Math.pow(a.getElementAt(row, 2), 2.0)
223 + Math.pow(a.getElementAt(row, 3), 2.0));
224
225 a.setElementAt(row, 0, a.getElementAt(row, 0) / rowNorm);
226 a.setElementAt(row, 1, a.getElementAt(row, 1) / rowNorm);
227 a.setElementAt(row, 2, a.getElementAt(row, 2) / rowNorm);
228 a.setElementAt(row, 3, a.getElementAt(row, 3) / rowNorm);
229
230 // 2nd equation
231 row++;
232
233 a.setElementAt(row, 0, homY * principalPlane.getA() - homW * horizontalAxisPlane.getA());
234 a.setElementAt(row, 1, homY * principalPlane.getB() - homW * horizontalAxisPlane.getB());
235 a.setElementAt(row, 2, homY * principalPlane.getC() - homW * horizontalAxisPlane.getC());
236 a.setElementAt(row, 3, homY * principalPlane.getD() - homW * horizontalAxisPlane.getD());
237
238 // normalize row (equation) to increase accuracy
239 rowNorm = Math.sqrt(Math.pow(a.getElementAt(row, 0), 2.0)
240 + Math.pow(a.getElementAt(row, 1), 2.0)
241 + Math.pow(a.getElementAt(row, 2), 2.0)
242 + Math.pow(a.getElementAt(row, 3), 2.0));
243
244 a.setElementAt(row, 0, a.getElementAt(row, 0) / rowNorm);
245 a.setElementAt(row, 1, a.getElementAt(row, 1) / rowNorm);
246 a.setElementAt(row, 2, a.getElementAt(row, 2) / rowNorm);
247 a.setElementAt(row, 3, a.getElementAt(row, 3) / rowNorm);
248
249 if (!allowLMSESolution && i == MIN_REQUIRED_VIEWS) {
250 break;
251 }
252 }
253
254 // make SVD to find solution of A * M = 0
255 final var decomposer = new SingularValueDecomposer(a);
256 decomposer.decompose();
257
258 if (decomposer.getNullity() > 1) {
259 // degenerate case. Unique solution (up to scale) cannot be found
260 throw new Point3DTriangulationException();
261 }
262
263 final var v = decomposer.getV();
264
265 // last column of v will contain homogeneous coordinates of
266 // triangulated point
267 result.setHomogeneousCoordinates(v.getElementAt(0, 3),
268 v.getElementAt(1, 3), v.getElementAt(2, 3),
269 v.getElementAt(3, 3));
270
271 if (listener != null) {
272 listener.onTriangulateEnd(this);
273 }
274 } catch (final AlgebraException | CameraException e) {
275 throw new Point3DTriangulationException(e);
276 } finally {
277 locked = false;
278 }
279 }
280 }