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 import com.irurueta.numerical.robust.WeightSelection;
27 import com.irurueta.sorting.SortingException;
28
29 import java.util.List;
30
31 /**
32 * Triangulates matched 2D points into a single 3D one by using 2D point
33 * correspondences on different views along with the corresponding cameras on
34 * each of those views by finding a weighted solution to an inhomogeneous
35 * system of equations.
36 * Each equation on the linear system of equations is weighted using provided
37 * weight for each point and camera correspondence, so that some equations can
38 * be considered more important than others if we are more confident on some
39 * measures than others.
40 */
41 public class WeightedInhomogeneousSinglePoint3DTriangulator extends SinglePoint3DTriangulator {
42
43 /**
44 * Default number of correspondences to be weighted and taken into account.
45 * If more correspondences are provided, they are ignored to avoid numerical
46 * inaccuracies.
47 */
48 public static final int DEFAULT_MAX_CORRESPONDENCES = 50;
49
50 /**
51 * Indicates if weights are sorted by default so that largest weighted
52 * correspondences are used first.
53 */
54 public static final boolean DEFAULT_SORT_WEIGHTS = true;
55
56 /**
57 * Maximum number of correspondences to be weighted and taken into account.
58 */
59 private int maxCorrespondences;
60
61 /**
62 * Indicates if weights are sorted by default so that largest weighted
63 * correspondences are used first.
64 */
65 private boolean sortWeights;
66
67 /**
68 * Array containing weights for all correspondences.
69 */
70 private double[] weights;
71
72 /**
73 * Constructor.
74 */
75 public WeightedInhomogeneousSinglePoint3DTriangulator() {
76 super();
77 maxCorrespondences = DEFAULT_MAX_CORRESPONDENCES;
78 sortWeights = DEFAULT_SORT_WEIGHTS;
79 }
80
81 /**
82 * Constructor.
83 *
84 * @param points2D list of matched 2D points on each view. Each point in the
85 * list is assumed to be projected by the corresponding camera in the list.
86 * @param cameras camera for each view where 2D points are represented.
87 * @throws IllegalArgumentException if provided lists don't have the same
88 * length or their length is less than 2 views, which is the minimum
89 * required to compute triangulation.
90 */
91 public WeightedInhomogeneousSinglePoint3DTriangulator(
92 final List<Point2D> points2D, final List<PinholeCamera> cameras) {
93 super(points2D, cameras);
94 maxCorrespondences = DEFAULT_MAX_CORRESPONDENCES;
95 sortWeights = DEFAULT_SORT_WEIGHTS;
96 }
97
98 /**
99 * Constructor.
100 *
101 * @param points2D list of matched 2D points on each view. Each point in the
102 * list is assumed to be projected by the corresponding camera in the list.
103 * @param cameras camera for each view where 2D points are represented.
104 * @param weights weights assigned to each view.
105 * @throws IllegalArgumentException if provided lists or weights don't have
106 * the same length or their length is less than 2 views, which is the
107 * minimum required to compute triangulation.
108 */
109 public WeightedInhomogeneousSinglePoint3DTriangulator(
110 final List<Point2D> points2D, final List<PinholeCamera> cameras, final double[] weights) {
111 this();
112 internalSetPointsCamerasAndWeights(points2D, cameras, weights);
113 }
114
115
116 /**
117 * Constructor.
118 *
119 * @param listener listener to notify events generated by instances of this
120 * class.
121 */
122 public WeightedInhomogeneousSinglePoint3DTriangulator(final SinglePoint3DTriangulatorListener listener) {
123 super(listener);
124 maxCorrespondences = DEFAULT_MAX_CORRESPONDENCES;
125 sortWeights = DEFAULT_SORT_WEIGHTS;
126 }
127
128 /**
129 * Constructor.
130 *
131 * @param points2D list of matched 2D points on each view. Each point in the
132 * list is assumed to be projected by the corresponding camera in the list.
133 * @param cameras cameras for each view where 2D points are represented.
134 * @param listener listener to notify events generated by instances of this
135 * class.
136 * @throws IllegalArgumentException if provided lists don't have the same
137 * length or their length is less than 2 views, which is the minimum
138 * required to compute triangulation.
139 */
140 public WeightedInhomogeneousSinglePoint3DTriangulator(
141 final List<Point2D> points2D, final List<PinholeCamera> cameras,
142 final SinglePoint3DTriangulatorListener listener) {
143 super(points2D, cameras, listener);
144 maxCorrespondences = DEFAULT_MAX_CORRESPONDENCES;
145 sortWeights = DEFAULT_SORT_WEIGHTS;
146 }
147
148 /**
149 * Constructor.
150 *
151 * @param points2D list of matched 2D points on each view. Each point in the
152 * list is assumed to be projected by the corresponding camera in the list.
153 * @param cameras camera for each view where 2D points are represented.
154 * @param weights weights assigned to each view.
155 * @param listener listener to notify events generated by instances of this
156 * class.
157 * @throws IllegalArgumentException if provided lists or weights don't have
158 * the same length or their length is less than 2 views, which is the
159 * minimum required to compute triangulation.
160 */
161 public WeightedInhomogeneousSinglePoint3DTriangulator(
162 final List<Point2D> points2D, final List<PinholeCamera> cameras, final double[] weights,
163 final SinglePoint3DTriangulatorListener listener) {
164 this(listener);
165 internalSetPointsCamerasAndWeights(points2D, cameras, weights);
166 }
167
168 /**
169 * Returns weights assigned to each view.
170 * The larger a weight is the more reliable a view is considered and
171 * equations to triangulate a 3D point will be take precedence over other
172 * views when estimating an averaged solution.
173 *
174 * @return weights assigned to each view.
175 */
176 public double[] getWeights() {
177 return weights;
178 }
179
180 /**
181 * Sets list of matched 2D points for each view and their corresponding
182 * cameras used to project them along with their weights.
183 *
184 * @param points2D list of matched 2D points on each view. Each point in the
185 * list is assumed to be projected by the corresponding camera in the list.
186 * @param cameras cameras for each view where 2D points are represented.
187 * @param weights weights assigned to each view.
188 * @throws LockedException if this instance is locked.
189 * @throws IllegalArgumentException if provided lists don't have the same
190 * length or their length is less than 2 views, which is the minimum
191 * required to compute triangulation.
192 */
193 public void setPointsCamerasAndWeights(
194 final List<Point2D> points2D, final List<PinholeCamera> cameras, final double[] weights)
195 throws LockedException {
196 if (isLocked()) {
197 throw new LockedException();
198 }
199 internalSetPointsCamerasAndWeights(points2D, cameras, weights);
200 }
201
202 /**
203 * Indicates whether this instance is ready to start the triangulation.
204 * An instance is ready when both lists of 2D points and cameras are
205 * provided, both lists have the same length, at least data for 2 views
206 * is provided and the corresponding weights are also provided.
207 *
208 * @return true if this instance is ready, false otherwise.
209 */
210 @Override
211 public boolean isReady() {
212 return areValidPointsCamerasAndWeights(points2D, cameras, weights);
213 }
214
215 /**
216 * Indicates whether provided points, cameras and weights are valid to start
217 * the triangulation.
218 * In order to triangulate points, at least two cameras and their
219 * corresponding 2 matched 2D points are required along with weights for
220 * each view.
221 * If more views are provided, an averaged solution can be found.
222 *
223 * @param points2D list of matched points on each view.
224 * @param cameras cameras for each view where 2D points are represented.
225 * @param weights weights assigned to each view.
226 * @return true if data is enough to start triangulation, false otherwise.
227 */
228 public static boolean areValidPointsCamerasAndWeights(
229 final List<Point2D> points2D, final List<PinholeCamera> cameras, final double[] weights) {
230 return areValidPointsAndCameras(points2D, cameras) && weights != null && weights.length == points2D.size();
231 }
232
233 /**
234 * Returns maximum number of correspondences to be weighted and taken into
235 * account.
236 *
237 * @return maximum number of correspondences to be weighted.
238 */
239 public int getMaxCorrespondences() {
240 return maxCorrespondences;
241 }
242
243 /**
244 * Sets maximum number of correspondences to be weighted and taken into
245 * account.
246 *
247 * @param maxCorrespondences maximum number of correspondences to be
248 * weighted.
249 * @throws IllegalArgumentException if provided value is less than the
250 * minimum required number of views, which is 2.
251 * @throws LockedException if this instance is locked.
252 */
253 public void setMaxCorrespondences(final int maxCorrespondences) throws LockedException {
254 if (isLocked()) {
255 throw new LockedException();
256 }
257 if (maxCorrespondences < MIN_REQUIRED_VIEWS) {
258 throw new IllegalArgumentException();
259 }
260
261 this.maxCorrespondences = maxCorrespondences;
262 }
263
264 /**
265 * Indicates if weights are sorted by so that largest weighted
266 * correspondences are used first.
267 *
268 * @return true if weights are sorted, false otherwise.
269 */
270 public boolean isSortWeightsEnabled() {
271 return sortWeights;
272 }
273
274 /**
275 * Specifies whether weights are sorted by so that largest weighted
276 * correspondences are used first.
277 *
278 * @param sortWeights true if weights are sorted, false otherwise.
279 * @throws LockedException if this instance is locked.
280 */
281 public void setSortWeightsEnabled(final boolean sortWeights) throws LockedException {
282 if (isLocked()) {
283 throw new LockedException();
284 }
285
286 this.sortWeights = sortWeights;
287 }
288
289 /**
290 * Returns type of triangulator.
291 *
292 * @return type of triangulator.
293 */
294 @Override
295 public Point3DTriangulatorType getType() {
296 return Point3DTriangulatorType.WEIGHTED_INHOMOGENEOUS_TRIANGULATOR;
297 }
298
299 /**
300 * Internal method to triangulate provided matched 2D points being projected
301 * by each corresponding camera into a single 3D point.
302 * At least 2 matched 2D points and their corresponding 2 cameras are
303 * required to compute triangulation. If more views are provided, an
304 * averaged solution is found.
305 * This method does not check whether instance is locked or ready.
306 *
307 * @param points2D matched 2D points. Each point in the list is assumed to
308 * be projected by the corresponding camera in the list.
309 * @param cameras list of cameras associated to the matched 2D point on the
310 * same position as the camera on the list.
311 * @param result instance where triangulated 3D point is stored.
312 * @throws Point3DTriangulationException if triangulation fails for some
313 * other reason (i.e. degenerate geometry, numerical
314 * instabilities, etc.).
315 */
316 @SuppressWarnings("DuplicatedCode")
317 @Override
318 protected void triangulate(
319 final List<Point2D> points2D, final List<PinholeCamera> cameras, final Point3D result)
320 throws Point3DTriangulationException {
321 try {
322 locked = true;
323
324 if (listener != null) {
325 listener.onTriangulateStart(this);
326 }
327
328 final var selection = WeightSelection.selectWeights(weights, sortWeights, maxCorrespondences);
329
330 final var selected = selection.getSelected();
331
332 final var numViews = cameras.size();
333
334 final var a = new Matrix(2 * numViews, 3);
335 final var b = new double[2 * numViews];
336
337 Point2D point;
338 PinholeCamera camera;
339 final var horizontalAxisPlane = new Plane();
340 final var verticalAxisPlane = new Plane();
341 final var principalPlane = new Plane();
342 var row = 0;
343 double rowNorm;
344 for (int i = 0; i < numViews; i++) {
345 if (selected[i]) {
346 point = points2D.get(i);
347 camera = cameras.get(i);
348
349 // to increase accuracy
350 point.normalize();
351 camera.normalize();
352
353 final var homX = point.getHomX();
354 final var homY = point.getHomY();
355 final var homW = point.getHomW();
356
357 // pick rows of camera corresponding to different planes
358 // (we do not normalize planes, as it would introduce errors)
359
360 // 1st camera row (p1T)
361 camera.verticalAxisPlane(verticalAxisPlane);
362 // 2nd camera row (p2T)
363 camera.horizontalAxisPlane(horizontalAxisPlane);
364 // 3rd camera row (p3T)
365 camera.principalPlane(principalPlane);
366
367
368 // 1st equation
369 a.setElementAt(row, 0, homX * principalPlane.getA()
370 - homW * verticalAxisPlane.getA());
371 a.setElementAt(row, 1, homX * principalPlane.getB()
372 - homW * verticalAxisPlane.getB());
373 a.setElementAt(row, 2, homX * principalPlane.getC()
374 - homW * verticalAxisPlane.getC());
375
376 b[row] = homW * verticalAxisPlane.getD() - homX * principalPlane.getD();
377
378 // normalize equation to increase accuracy
379 rowNorm = Math.sqrt(Math.pow(a.getElementAt(row, 0), 2.0)
380 + Math.pow(a.getElementAt(row, 1), 2.0)
381 + Math.pow(a.getElementAt(row, 2), 2.0));
382
383 a.setElementAt(row, 0, a.getElementAt(row, 0) / rowNorm);
384 a.setElementAt(row, 1, a.getElementAt(row, 1) / rowNorm);
385 a.setElementAt(row, 2, a.getElementAt(row, 2) / rowNorm);
386 b[row] /= rowNorm;
387
388 // 2nd equation
389 row++;
390
391 a.setElementAt(row, 0, homY * principalPlane.getA()
392 - homW * horizontalAxisPlane.getA());
393 a.setElementAt(row, 1, homY * principalPlane.getB()
394 - homW * horizontalAxisPlane.getB());
395 a.setElementAt(row, 2, homY * principalPlane.getC()
396 - homW * horizontalAxisPlane.getC());
397
398 b[row] = homW * horizontalAxisPlane.getD() - homY * principalPlane.getD();
399
400 // normalize equation to increase accuracy
401 rowNorm = Math.sqrt(Math.pow(a.getElementAt(row, 0), 2.0)
402 + Math.pow(a.getElementAt(row, 1), 2.0)
403 + Math.pow(a.getElementAt(row, 2), 2.0));
404
405 a.setElementAt(row, 0, a.getElementAt(row, 0) / rowNorm);
406 a.setElementAt(row, 1, a.getElementAt(row, 1) / rowNorm);
407 a.setElementAt(row, 2, a.getElementAt(row, 2) / rowNorm);
408 b[row] /= rowNorm;
409 }
410 }
411
412 // make SVD to find solution of A * M = 0
413 final var decomposer = new SingularValueDecomposer(a);
414
415 decomposer.decompose();
416
417 // solve linear system of equations to obtain inhomogeneous
418 // coordinates of triangulated point
419 final var solution = decomposer.solve(b);
420
421 result.setInhomogeneousCoordinates(solution[0], solution[1], solution[2]);
422
423 if (listener != null) {
424 listener.onTriangulateEnd(this);
425 }
426 } catch (final AlgebraException | SortingException e) {
427 throw new Point3DTriangulationException(e);
428 } finally {
429 locked = false;
430 }
431
432 }
433
434 /**
435 * Internal method to set list of matched 2D points for each view and their
436 * corresponding cameras used to project them along with their weights.
437 * This method does not check whether instance is locked.
438 *
439 * @param points2D list of matched 2D points on each view. Each point in the
440 * list is assumed to be projected by the corresponding camera in the list.
441 * @param cameras cameras for each view where 2D points are represented.
442 * @param weights weights assigned to each view.
443 * @throws IllegalArgumentException if provided lists don't have the same
444 * length or their length is less than 2 views, which is the minimum
445 * required to compute triangulation.
446 */
447 private void internalSetPointsCamerasAndWeights(
448 final List<Point2D> points2D, final List<PinholeCamera> cameras, final double[] weights) {
449 if (!areValidPointsCamerasAndWeights(points2D, cameras, weights)) {
450 throw new IllegalArgumentException();
451 }
452
453 this.points2D = points2D;
454 this.cameras = cameras;
455 this.weights = weights;
456 }
457 }