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 homogeneous systems
35 * 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 WeightedHomogeneousSinglePoint3DTriangulator 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 WeightedHomogeneousSinglePoint3DTriangulator() {
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 WeightedHomogeneousSinglePoint3DTriangulator(
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 WeightedHomogeneousSinglePoint3DTriangulator(
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 WeightedHomogeneousSinglePoint3DTriangulator(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 WeightedHomogeneousSinglePoint3DTriangulator(
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 WeightedHomogeneousSinglePoint3DTriangulator(
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_HOMOGENEOUS_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 @Override
317 @SuppressWarnings("Duplicates")
318 protected void triangulate(final List<Point2D> points2D, final List<PinholeCamera> cameras, final Point3D result)
319 throws Point3DTriangulationException {
320 try {
321 locked = true;
322
323 if (listener != null) {
324 listener.onTriangulateStart(this);
325 }
326
327 final var selection = WeightSelection.selectWeights(weights, sortWeights, maxCorrespondences);
328
329 final var selected = selection.getSelected();
330
331 final var numViews = cameras.size();
332
333 final var a = new Matrix(2 * numViews, 2 * MIN_REQUIRED_VIEWS);
334
335 final var horizontalAxisPlane = new Plane();
336 final var verticalAxisPlane = new Plane();
337 final var principalPlane = new Plane();
338 var row = 0;
339 double rowNorm;
340 for (var i = 0; i < numViews; i++) {
341 if (selected[i]) {
342 final var point = points2D.get(i);
343 final var camera = cameras.get(i);
344
345 // to increase accuracy
346 point.normalize();
347 camera.normalize();
348
349 final var homX = point.getHomX();
350 final var homY = point.getHomY();
351 final var homW = point.getHomW();
352
353 // pick rows of camera corresponding to different planes
354 // (we do not normalize planes, as it would introduce errors)
355
356 // 1st camera row (p1T)
357 camera.verticalAxisPlane(verticalAxisPlane);
358 // 2nd camera row (p2T)
359 camera.horizontalAxisPlane(horizontalAxisPlane);
360 // 3rd camera row (p3T)
361 camera.principalPlane(principalPlane);
362
363
364 // 1st equation
365 a.setElementAt(row, 0, homX * principalPlane.getA()
366 - homW * verticalAxisPlane.getA());
367 a.setElementAt(row, 1, homX * principalPlane.getB()
368 - homW * verticalAxisPlane.getB());
369 a.setElementAt(row, 2, homX * principalPlane.getC()
370 - homW * verticalAxisPlane.getC());
371 a.setElementAt(row, 3, homX * principalPlane.getD()
372 - homW * verticalAxisPlane.getD());
373
374 // normalize row (equation) to increase accuracy
375 rowNorm = Math.sqrt(Math.pow(a.getElementAt(row, 0), 2.0)
376 + Math.pow(a.getElementAt(row, 1), 2.0)
377 + Math.pow(a.getElementAt(row, 2), 2.0)
378 + Math.pow(a.getElementAt(row, 3), 2.0));
379
380 a.setElementAt(row, 0, a.getElementAt(row, 0) / rowNorm);
381 a.setElementAt(row, 1, a.getElementAt(row, 1) / rowNorm);
382 a.setElementAt(row, 2, a.getElementAt(row, 2) / rowNorm);
383 a.setElementAt(row, 3, a.getElementAt(row, 3) / rowNorm);
384
385 // 2nd equation
386 row++;
387
388 a.setElementAt(row, 0, homY * principalPlane.getA()
389 - homW * horizontalAxisPlane.getA());
390 a.setElementAt(row, 1, homY * principalPlane.getB()
391 - homW * horizontalAxisPlane.getB());
392 a.setElementAt(row, 2, homY * principalPlane.getC()
393 - homW * horizontalAxisPlane.getC());
394 a.setElementAt(row, 3, homY * principalPlane.getD()
395 - homW * horizontalAxisPlane.getD());
396
397 // normalize row (equation) to increase accuracy
398 rowNorm = Math.sqrt(Math.pow(a.getElementAt(row, 0), 2.0)
399 + Math.pow(a.getElementAt(row, 1), 2.0)
400 + Math.pow(a.getElementAt(row, 2), 2.0)
401 + Math.pow(a.getElementAt(row, 3), 2.0));
402
403 a.setElementAt(row, 0, a.getElementAt(row, 0) / rowNorm);
404 a.setElementAt(row, 1, a.getElementAt(row, 1) / rowNorm);
405 a.setElementAt(row, 2, a.getElementAt(row, 2) / rowNorm);
406 a.setElementAt(row, 3, a.getElementAt(row, 3) / rowNorm);
407 }
408 }
409
410 // make SVD to find solution of A * M = 0
411 final var decomposer = new SingularValueDecomposer(a);
412 decomposer.decompose();
413
414 if (decomposer.getNullity() > 1) {
415 // degenerate case. Unique solution (up to scale) cannot be found
416 throw new Point3DTriangulationException();
417 }
418
419 final var v = decomposer.getV();
420
421 // last column of v will contain homogeneous coordinates of
422 // triangulated point
423 result.setHomogeneousCoordinates(v.getElementAt(0, 3),
424 v.getElementAt(1, 3), v.getElementAt(2, 3),
425 v.getElementAt(3, 3));
426
427 if (listener != null) {
428 listener.onTriangulateEnd(this);
429 }
430 } catch (final AlgebraException | SortingException e) {
431 throw new Point3DTriangulationException(e);
432 } finally {
433 locked = false;
434 }
435
436 }
437
438 /**
439 * Internal method to set list of matched 2D points for each view and their
440 * corresponding cameras used to project them along with their weights.
441 * This method does not check whether instance is locked.
442 *
443 * @param points2D list of matched 2D points on each view. Each point in the
444 * list is assumed to be projected by the corresponding camera in the list.
445 * @param cameras cameras for each view where 2D points are represented.
446 * @param weights weights assigned to each view.
447 * @throws IllegalArgumentException if provided lists don't have the same
448 * length or their length is less than 2 views, which is the minimum
449 * required to compute triangulation.
450 */
451 private void internalSetPointsCamerasAndWeights(
452 final List<Point2D> points2D, final List<PinholeCamera> cameras, final double[] weights) {
453 if (!areValidPointsCamerasAndWeights(points2D, cameras, weights)) {
454 throw new IllegalArgumentException();
455 }
456
457 this.points2D = points2D;
458 this.cameras = cameras;
459 this.weights = weights;
460 }
461 }