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