SinglePoint3DTriangulator.java
/*
* Copyright (C) 2015 Alberto Irurueta Carro (alberto@irurueta.com)
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
package com.irurueta.ar.sfm;
import com.irurueta.geometry.CoordinatesType;
import com.irurueta.geometry.PinholeCamera;
import com.irurueta.geometry.Point2D;
import com.irurueta.geometry.Point3D;
import com.irurueta.geometry.estimators.LockedException;
import com.irurueta.geometry.estimators.NotReadyException;
import java.util.List;
/**
* Base class to triangulate matched 2D points into a single 3D one by using
* 2D points correspondences on different views along with the corresponding
* cameras on each of those views.
* Subclasses will implement different types of triangulators that can provide
* either LMSE or weighted solutions using either homogeneous or inhomogeneous
* systems of equations.
* Inhomogeneous methods are suitable only for cases where finite points and
* cameras are being used. If points or cameras are located very far or at
* infinity, triangulation will fail when using inhomogeneous methods.
* Homogeneous methods are suitable for any case, however, if points and
* cameras are close and well-defined, inhomogeneous methods might yield better
* accuracy (although the difference is minimal).
*/
public abstract class SinglePoint3DTriangulator {
/**
* Default triangulator type.
*/
public static final Point3DTriangulatorType DEFAULT_TYPE = Point3DTriangulatorType.LMSE_HOMOGENEOUS_TRIANGULATOR;
/**
* Minimum required number of views to triangulate 3D points.
*/
public static final int MIN_REQUIRED_VIEWS = 2;
/**
* Matched 2D points. Each point in the list is assumed to be projected by
* the corresponding camera in the list.
*/
protected List<Point2D> points2D;
/**
* List of cameras associated to the matched 2D point on the same position
* as the camera on the list.
*/
protected List<PinholeCamera> cameras;
/**
* Listener to handle events generated by instances of this class.
*/
protected SinglePoint3DTriangulatorListener listener;
/**
* Indicates whether this instance is locked doing computations.
*/
protected boolean locked;
/**
* Constructor.
*/
protected SinglePoint3DTriangulator() {
}
/**
* Constructor.
*
* @param points2D list of matched 2D points on each view. Each point in the
* list is assumed to be projected by the corresponding camera in the list.
* @param cameras camera for each view where 2D points are represented.
* @throws IllegalArgumentException if provided lists don't have the same
* length or their length is less than 2 views, which is the minimum
* required to compute triangulation.
*/
protected SinglePoint3DTriangulator(final List<Point2D> points2D, final List<PinholeCamera> cameras) {
internalSetPointsAndCameras(points2D, cameras);
}
/**
* Constructor.
*
* @param listener listener to notify events generated by instances of this
* class.
*/
protected SinglePoint3DTriangulator(final SinglePoint3DTriangulatorListener listener) {
this.listener = listener;
}
/**
* Constructor.
*
* @param points2D list of matched 2D points on each view. Each point in the
* list is assumed to be projected by the corresponding camera in the list.
* @param cameras cameras for each view where 2D points are represented.
* @param listener listener to notify events generated by instances of this
* class.
* @throws IllegalArgumentException if provided lists don't have the same
* length or their length is less than 2 views, which is the minimum
* required to compute triangulation.
*/
protected SinglePoint3DTriangulator(final List<Point2D> points2D, final List<PinholeCamera> cameras,
final SinglePoint3DTriangulatorListener listener) {
this(points2D, cameras);
this.listener = listener;
}
/**
* Returns list of matched 2D points on each view. Each point in the list is
* assumed to be projected by the corresponding camera.
*
* @return list of matched 2D points on each view.
*/
public List<Point2D> getPoints2D() {
return points2D;
}
/**
* Returns cameras for each view where 2D points are represented.
*
* @return cameras for each view where 2D points are represented.
*/
public List<PinholeCamera> getCameras() {
return cameras;
}
/**
* Sets list of matched 2D points for each view and their corresponding
* cameras used to project them.
*
* @param points2D list of matched 2D points on each view. Each point in the
* list is assumed to be projected by the corresponding camera in the list.
* @param cameras cameras for each view where 2D points are represented.
* @throws LockedException if this instance is locked.
* @throws IllegalArgumentException if provided lists don't have the same
* length or their length is less than 2 views, which is the minimum
* required to compute triangulation.
*/
public void setPointsAndCameras(final List<Point2D> points2D, final List<PinholeCamera> cameras)
throws LockedException {
if (isLocked()) {
throw new LockedException();
}
internalSetPointsAndCameras(points2D, cameras);
}
/**
* Indicates whether this instance is locked because computations are being
* done.
*
* @return true if instance is locked, false otherwise.
*/
public boolean isLocked() {
return locked;
}
/**
* Indicates whether this instance is ready to start the triangulation.
* An instance is ready when both lists of 2D points and cameras are
* provided, both lists have the same length and at least data for 2 views
* is provided.
*
* @return true if this instance is ready, false otherwise.
*/
public boolean isReady() {
return areValidPointsAndCameras(points2D, cameras);
}
/**
* Returns listener to be notified of events generated by instances of this
* class.
*
* @return listener to be notified of events generated by instances of this
* class.
*/
public SinglePoint3DTriangulatorListener getListener() {
return listener;
}
/**
* Sets listener to be notified of events generated by instances of this
* class.
*
* @param listener listener to be notified of events generated by instances
* of this class.
* @throws LockedException if this instance is locked.
*/
public void setListener(final SinglePoint3DTriangulatorListener listener) throws LockedException {
if (isLocked()) {
throw new LockedException();
}
this.listener = listener;
}
/**
* Triangulates provided matched 2D points being projected by each
* corresponding camera into a single 3D point.
* At least 2 matched 2D points and their corresponding 2 cameras are
* required to compute triangulation. If more views are provided, an
* averaged solution can be found.
*
* @return computed triangulated 3D point.
* @throws LockedException if this instance is locked.
* @throws NotReadyException if lists of points and cameras don't have the
* same length or less than 2 views are provided.
* @throws Point3DTriangulationException if triangulation fails for some
* other reason (i.e. degenerate geometry, numerical
* instabilities, etc.).
*/
public Point3D triangulate() throws LockedException, NotReadyException, Point3DTriangulationException {
final var result = Point3D.create(CoordinatesType.HOMOGENEOUS_COORDINATES);
triangulate(result);
return result;
}
/**
* Triangulates provided matched 2D points being projected by each
* corresponding camera into a single 3D point.
* At least 2 matched 2D points and their corresponding 2 cameras are
* required to compute triangulation. If more views are provided, an
* averaged solution can be found.
*
* @param result instance where data for triangulated 3D point is stored.
* @throws LockedException if this instance is locked.
* @throws NotReadyException if lists of points and cameras don't have the
* same length or less than 2 views are provided.
* @throws Point3DTriangulationException if triangulation fails for some
* other reason (i.e. degenerate geometry, numerical
* instabilities, etc.).
*/
public void triangulate(final Point3D result) throws LockedException, NotReadyException,
Point3DTriangulationException {
if (isLocked()) {
throw new LockedException();
}
if (!isReady()) {
throw new NotReadyException();
}
triangulate(points2D, cameras, result);
}
/**
* Indicates whether provided points and cameras are valid to start the
* triangulation.
* In order to triangulate points, at least two cameras and their
* corresponding 2 matched 2D points are required.
* If more views are provided, an averaged solution can be found.
*
* @param points2D list of matched points on each view.
* @param cameras cameras for each view where 2D points are represented.
* @return true if data is enough to start triangulation, false otherwise.
*/
public static boolean areValidPointsAndCameras(final List<Point2D> points2D, final List<PinholeCamera> cameras) {
return points2D != null && cameras != null && points2D.size() == cameras.size()
&& points2D.size() >= MIN_REQUIRED_VIEWS;
}
/**
* Creates a new 3D point triangulator instance using provided type.
*
* @param type a triangulator type.
* @return a 3D point triangulator instance.
*/
public static SinglePoint3DTriangulator create(final Point3DTriangulatorType type) {
return switch (type) {
case WEIGHTED_INHOMOGENEOUS_TRIANGULATOR -> new WeightedInhomogeneousSinglePoint3DTriangulator();
case WEIGHTED_HOMOGENEOUS_TRIANGULATOR -> new WeightedHomogeneousSinglePoint3DTriangulator();
case LMSE_INHOMOGENEOUS_TRIANGULATOR -> new LMSEInhomogeneousSinglePoint3DTriangulator();
default -> new LMSEHomogeneousSinglePoint3DTriangulator();
};
}
/**
* Creates a new 3D point triangulator instance using provided lists of
* points and corresponding cameras along with provided type.
*
* @param points2D list of matched 2D points on each view. Each point in the
* list is assumed to be projected by the corresponding camera in the list.
* @param cameras camera for each view where 2D points are represented.
* @param type a triangulator type.
* @return a 3D point triangulator instance.
* @throws IllegalArgumentException if provided lists don't have the same
* length or their length is less than 2 views, which is the minimum
* required to compute triangulation.
*/
public static SinglePoint3DTriangulator create(
final List<Point2D> points2D, final List<PinholeCamera> cameras, final Point3DTriangulatorType type) {
return switch (type) {
case WEIGHTED_INHOMOGENEOUS_TRIANGULATOR -> new WeightedInhomogeneousSinglePoint3DTriangulator(points2D,
cameras);
case WEIGHTED_HOMOGENEOUS_TRIANGULATOR -> new WeightedHomogeneousSinglePoint3DTriangulator(points2D,
cameras);
case LMSE_INHOMOGENEOUS_TRIANGULATOR -> new LMSEInhomogeneousSinglePoint3DTriangulator(points2D,
cameras);
default -> new LMSEHomogeneousSinglePoint3DTriangulator(points2D, cameras);
};
}
/**
* Creates a new 3D point triangulator instance using provided lists of
* points, weights and corresponding cameras along with provided type.
*
* @param points2D list of matched 2D points on each view. Each point in the
* list is assumed to be projected by the corresponding camera in the list.
* @param cameras camera for each view where 2D points are represented.
* @param weights weights assigned to each view.
* @param type a triangulator type.
* @return a 3D point triangulator instance.
* @throws IllegalArgumentException if provided lists or weights don't have
* the same length or their length is less than 2 views, which is the
* minimum required to compute triangulation.
*/
public static SinglePoint3DTriangulator create(
final List<Point2D> points2D, final List<PinholeCamera> cameras, final double[] weights,
final Point3DTriangulatorType type) {
return switch (type) {
case WEIGHTED_INHOMOGENEOUS_TRIANGULATOR -> new WeightedInhomogeneousSinglePoint3DTriangulator(points2D,
cameras, weights);
case WEIGHTED_HOMOGENEOUS_TRIANGULATOR -> new WeightedHomogeneousSinglePoint3DTriangulator(points2D,
cameras, weights);
case LMSE_INHOMOGENEOUS_TRIANGULATOR -> new LMSEInhomogeneousSinglePoint3DTriangulator(points2D, cameras);
default -> new LMSEHomogeneousSinglePoint3DTriangulator(points2D, cameras);
};
}
/**
* Creates a new 3D point triangulator instance using provided listener and
* type.
*
* @param listener listener to notify events generated by instances of this
* class.
* @param type a triangulator type.
* @return a 3D point triangulator instance.
*/
public static SinglePoint3DTriangulator create(
final SinglePoint3DTriangulatorListener listener, final Point3DTriangulatorType type) {
return switch (type) {
case WEIGHTED_INHOMOGENEOUS_TRIANGULATOR -> new WeightedInhomogeneousSinglePoint3DTriangulator(listener);
case WEIGHTED_HOMOGENEOUS_TRIANGULATOR -> new WeightedHomogeneousSinglePoint3DTriangulator(listener);
case LMSE_INHOMOGENEOUS_TRIANGULATOR -> new LMSEInhomogeneousSinglePoint3DTriangulator(listener);
default -> new LMSEHomogeneousSinglePoint3DTriangulator(listener);
};
}
/**
* Creates a new 3D point triangulator instance using provided lists of
* points and corresponding cameras, listener and provided type.
*
* @param points2D list of matched 2D points on each view. Each point in the
* list is assumed to be projected by the corresponding camera in the list.
* @param cameras camera for each view where 2D points are represented.
* @param listener listener to notify events generated by instances of this
* class.
* @param type a triangulator type.
* @return a 3D point triangulator instance.
* @throws IllegalArgumentException if provided lists don't have the same
* length or their length is less than 2 views, which is the minimum
* required to compute triangulation.
*/
public static SinglePoint3DTriangulator create(
final List<Point2D> points2D, final List<PinholeCamera> cameras,
final SinglePoint3DTriangulatorListener listener, final Point3DTriangulatorType type) {
return switch (type) {
case WEIGHTED_INHOMOGENEOUS_TRIANGULATOR -> new WeightedInhomogeneousSinglePoint3DTriangulator(points2D,
cameras, listener);
case WEIGHTED_HOMOGENEOUS_TRIANGULATOR -> new WeightedHomogeneousSinglePoint3DTriangulator(points2D,
cameras, listener);
case LMSE_INHOMOGENEOUS_TRIANGULATOR -> new LMSEInhomogeneousSinglePoint3DTriangulator(points2D, cameras,
listener);
default -> new LMSEHomogeneousSinglePoint3DTriangulator(points2D, cameras, listener);
};
}
/**
* Creates a new 3D point triangulator instance using provided lists of
* points, weights, corresponding cameras, listener and provided type.
*
* @param points2D list of matched 2D points on each view. Each point in the
* list is assumed to be projected by the corresponding camera in the list.
* @param cameras camera for each view where 2D points are represented.
* @param weights weights assigned to each view.
* @param listener listener to notify events generated by instances of this
* class.
* @param type a triangulator type.
* @return a 3D point triangulator instance.
* @throws IllegalArgumentException if provided lists or weights don't have
* the same length or their length is less than 2 views, which is the
* minimum required to compute triangulation.
*/
public static SinglePoint3DTriangulator create(
final List<Point2D> points2D, final List<PinholeCamera> cameras, final double[] weights,
final SinglePoint3DTriangulatorListener listener, final Point3DTriangulatorType type) {
return switch (type) {
case WEIGHTED_INHOMOGENEOUS_TRIANGULATOR -> new WeightedInhomogeneousSinglePoint3DTriangulator(points2D,
cameras, weights, listener);
case WEIGHTED_HOMOGENEOUS_TRIANGULATOR -> new WeightedHomogeneousSinglePoint3DTriangulator(points2D,
cameras, weights, listener);
case LMSE_INHOMOGENEOUS_TRIANGULATOR -> new LMSEInhomogeneousSinglePoint3DTriangulator(points2D, cameras,
listener);
default -> new LMSEHomogeneousSinglePoint3DTriangulator(points2D, cameras, listener);
};
}
/**
* Creates a new 3D point triangulator instance using default type.
*
* @return a 3D point triangulator instance.
*/
public static SinglePoint3DTriangulator create() {
return create(DEFAULT_TYPE);
}
/**
* Creates a new 3D point triangulator instance using provided lists of
* points and corresponding cameras along with default type.
*
* @param points2D list of matched 2D points on each view. Each point in the
* list is assumed to be projected by the corresponding camera in the list.
* @param cameras camera for each view where 2D points are represented.
* @return a 3D point triangulator instance.
* @throws IllegalArgumentException if provided lists don't have the same
* length or their length is less than 2 views, which is the minimum
* required to compute triangulation.
*/
public static SinglePoint3DTriangulator create(final List<Point2D> points2D, final List<PinholeCamera> cameras) {
return create(points2D, cameras, DEFAULT_TYPE);
}
/**
* Creates a new 3D point triangulator instance using provided lists of
* points, weights and corresponding cameras along with default type.
*
* @param points2D list of matched 2D points on each view. Each point in the
* list is assumed to be projected by the corresponding camera in the list.
* @param cameras camera for each view where 2D points are represented.
* @param weights weights assigned to each view.
* @return a 3D point triangulator instance.
* @throws IllegalArgumentException if provided lists or weights don't have
* the same length or their length is less than 2 views, which is the
* minimum required to compute triangulation.
*/
public static SinglePoint3DTriangulator create(
final List<Point2D> points2D, final List<PinholeCamera> cameras, double[] weights) {
return create(points2D, cameras, weights, DEFAULT_TYPE);
}
/**
* Creates a new 3D point triangulator instance using provided listener and
* default type.
*
* @param listener listener to notify events generated by instances of this
* class.
* @return a 3D point triangulator instance.
*/
public static SinglePoint3DTriangulator create(final SinglePoint3DTriangulatorListener listener) {
return create(listener, DEFAULT_TYPE);
}
/**
* Creates a new 3D point triangulator instance using provided lists of
* points and corresponding cameras, listener and default type.
*
* @param points2D list of matched 2D points on each view. Each point in the
* list is assumed to be projected by the corresponding camera in the list.
* @param cameras camera for each view where 2D points are represented.
* @param listener listener to notify events generated by instances of this
* class.
* @return a 3D point triangulator instance.
* @throws IllegalArgumentException if provided lists don't have the same
* length or their length is less than 2 views, which is the minimum
* required to compute triangulation.
*/
public static SinglePoint3DTriangulator create(
final List<Point2D> points2D, final List<PinholeCamera> cameras,
final SinglePoint3DTriangulatorListener listener) {
return create(points2D, cameras, listener, DEFAULT_TYPE);
}
/**
* Creates a new 3D point triangulator instance using provided lists of
* points, weights, corresponding cameras, listener and default type.
*
* @param points2D list of matched 2D points on each view. Each point in the
* list is assumed to be projected by the corresponding camera in the list.
* @param cameras camera for each view where 2D points are represented.
* @param weights weights assigned to each view.
* @param listener listener to notify events generated by instances of this
* class.
* @return a 3D point triangulator instance.
* @throws IllegalArgumentException if provided lists or weights don't have
* the same length or their length is less than 2 views, which is the
* minimum required to compute triangulation.
*/
public static SinglePoint3DTriangulator create(final List<Point2D> points2D,
final List<PinholeCamera> cameras,
final double[] weights,
final SinglePoint3DTriangulatorListener listener) {
return create(points2D, cameras, weights, listener, DEFAULT_TYPE);
}
/**
* Returns type of triangulator (a combination of homogeneous or
* inhomogeneous type along with an LMSE or weighted strategy).
*
* @return type of triangulator.
*/
public abstract Point3DTriangulatorType getType();
/**
* Internal method to triangulate provided matched 2D points being projected
* by each corresponding camera into a single 3D point.
* At least 2 matched 2D points and their corresponding 2 cameras are
* required to compute triangulation. If more views are provided, an
* averaged solution is found.
* This method does not check whether instance is locked or ready.
*
* @param points2D matched 2D points. Each point in the list is assumed to
* be projected by the corresponding camera in the list.
* @param cameras list of cameras associated to the matched 2D point on the
* same position as the camera on the list.
* @param result instance where triangulated 3D point is stored.
* @throws Point3DTriangulationException if triangulation fails for some
* other reason (i.e. degenerate geometry, numerical
* instabilities, etc.).
*/
protected abstract void triangulate(
final List<Point2D> points2D, final List<PinholeCamera> cameras, final Point3D result)
throws Point3DTriangulationException;
/**
* Internal method to sets list of matched 2D points for each view and their
* corresponding cameras used to project them.
* This method does not check whether instance is locked.
*
* @param points2D list of matched 2D points on each view. Each point in the
* list is assumed to be projected by the corresponding camera in the list.
* @param cameras cameras for each view where 2D points are represented.
* @throws IllegalArgumentException if provided lists don't have the same
* length or their length is less than 2 views, which is the minimum
* required to compute triangulation.
*/
private void internalSetPointsAndCameras(final List<Point2D> points2D, final List<PinholeCamera> cameras) {
if (!areValidPointsAndCameras(points2D, cameras)) {
throw new IllegalArgumentException();
}
this.points2D = points2D;
this.cameras = cameras;
}
}