Package com.irurueta.geometry.estimators
Class EPnPPointCorrespondencePinholeCameraEstimator
java.lang.Object
com.irurueta.geometry.estimators.PinholeCameraEstimator
com.irurueta.geometry.estimators.PointCorrespondencePinholeCameraEstimator
com.irurueta.geometry.estimators.EPnPPointCorrespondencePinholeCameraEstimator
public class EPnPPointCorrespondencePinholeCameraEstimator
extends PointCorrespondencePinholeCameraEstimator
EPnP (Efficient Perspective-n-Point) implementation to estimate pinhole
cameras from 2D/3D point correspondences.
This class is an implementation following the one proposed by Vincent Lepetit
on "EPnP: An Accurate O(n) Solution to the PnP Problem" with some minor
changes and improvements.
Paper and source code can be found at:
http://cvlabwww.epfl.ch/~lepetit/papers/lepetit_ijcv08.pdf
http://cvlab.epfl.ch/EPnP/index.php
-
Nested Class Summary
Nested ClassesModifier and TypeClassDescriptionprivate static classA possible solution. -
Field Summary
FieldsModifier and TypeFieldDescriptionprivate com.irurueta.algebra.MatrixContains barycentric coordinates to express 3D world point in terms of control points.Computed control points in world coordinates.static final booleanIndicates that by default a dimension 2 null-space is allowed.static final booleanIndicates that by default a dimension 3 null-space is allowed.static final booleanIndicates that by default planar configuration is checked to determine whether point correspondences are in such configuration and find a specific solution for such case.static final doubleDefault threshold to determine whether 3D matched points are in a planar configuration.private static final intNumber of control points used in a general configuration.private PinholeCameraIntrinsicParametersIntrinsic parameters of camera to be estimated.private booleanIndicates whether provided correspondences were found to be laying in a planar configuration during the estimation.private com.irurueta.algebra.MatrixM matrix to find control points in camera coordinates.private List<double[]> List containing columns of null-space of M.private booleanIndicates whether the case where a dimension 2 null-space is allowed.private booleanIndicates whether the case where a dimension 3 null-space is allowed.private static final intNumber of control points used in a planar configuration.private booleanIndicates whether planar configuration is checked to determine whether point correspondences are in such configuration and find a specific solution for such case.private doubleThreshold to determine whether 3D matched points are in a planar configuration.Possible solutions for the estimation.Fields inherited from class com.irurueta.geometry.estimators.PointCorrespondencePinholeCameraEstimator
DEFAULT_NORMALIZE_POINT_CORRESPONDENCES, EPS, MIN_NUMBER_OF_POINT_CORRESPONDENCES, points2D, points3DFields inherited from class com.irurueta.geometry.estimators.PinholeCameraEstimator
DEFAULT_ESTIMATOR_TYPE, DEFAULT_MAX_SUGGESTION_WEIGHT, DEFAULT_MIN_SUGGESTION_WEIGHT, DEFAULT_SUGGEST_ASPECT_RATIO_ENABLED, DEFAULT_SUGGEST_CENTER_ENABLED, DEFAULT_SUGGEST_HORIZONTAL_FOCAL_LENGTH_ENABLED, DEFAULT_SUGGEST_PRINCIPAL_POINT_ENABLED, DEFAULT_SUGGEST_ROTATION_ENABLED, DEFAULT_SUGGEST_SKEWNESS_VALUE_ENABLED, DEFAULT_SUGGEST_VERTICAL_FOCAL_LENGTH_ENABLED, DEFAULT_SUGGESTED_ASPECT_RATIO_VALUE, DEFAULT_SUGGESTED_SKEWNESS_VALUE, DEFAULT_SUGGESTION_WEIGHT_STEP, listener, locked, maxSuggestionWeight, minSuggestionWeight, suggestAspectRatioEnabled, suggestCenterEnabled, suggestedAspectRatioValue, suggestedCenterValue, suggestedHorizontalFocalLengthValue, suggestedPrincipalPointValue, suggestedRotationValue, suggestedSkewnessValue, suggestedVerticalFocalLengthValue, suggestHorizontalFocalLengthEnabled, suggestionWeightStep, suggestPrincipalPointEnabled, suggestRotationEnabled, suggestSkewnessValueEnabled, suggestVerticalFocalLengthEnabled -
Constructor Summary
ConstructorsConstructorDescriptionConstructor.Constructor with listener.Constructor.EPnPPointCorrespondencePinholeCameraEstimator(PinholeCameraIntrinsicParameters intrinsic, PinholeCameraEstimatorListener listener) Constructor with listener.EPnPPointCorrespondencePinholeCameraEstimator(PinholeCameraIntrinsicParameters intrinsic, List<Point3D> points3D, List<Point2D> points2D) Constructor.EPnPPointCorrespondencePinholeCameraEstimator(PinholeCameraIntrinsicParameters intrinsic, List<Point3D> points3D, List<Point2D> points2D, PinholeCameraEstimatorListener listener) Constructor.EPnPPointCorrespondencePinholeCameraEstimator(List<Point3D> points3D, List<Point2D> points2D) Constructor.EPnPPointCorrespondencePinholeCameraEstimator(List<Point3D> points3D, List<Point2D> points2D, PinholeCameraEstimatorListener listener) Constructor. -
Method Summary
Modifier and TypeMethodDescriptionbooleanIndicates if provided point correspondences are normalized to increase the accuracy of the estimation.private voidbuildM()In order to find control points in camera coordinates, an homogeneous linear system of equations must be solved having the form M*x = 0, where x contains the coordinates of all control points in the form [x1, y1, z1, x2, y2, z2, ... ].private voidComputes the coordinates of each provided world point in terms of estimated control points in world coordinates.computePossibleSolutionWithPoseAndReprojectionError(List<Point3D> controlCameraPoints) Computes a possible solution with camera, transformation, re-projection error and control points in camera coordinates.private voidComputes control points in world coordinates and determines whether they are located in a planar configuration or not.private static com.irurueta.algebra.MatrixconstraintMatrixSolution2(List<Point3D> controlCameraPointsA, List<Point3D> controlCameraPointsB) Fills constraint matrix to solve betas when using control points from the last 2 columns of v (the null-space).private static com.irurueta.algebra.MatrixconstraintMatrixSolution3(List<Point3D> controlCameraPointsA, List<Point3D> controlCameraPointsB, List<Point3D> controlCameraPointsC) Fills constraint matrix to solve betas when using control points from the last 3 columns of v (the null-space).controlPointsFromV(double[] v) Computes list of control points from provided array containing one column of the null-space of M or a linear combination of columns of the null-space.estimate()Estimates a pinhole camera.private static voidfillRowConstraintMatrixSolution2(int row, com.irurueta.algebra.Matrix c, Point3D vai, Point3D vaj, Point3D vbi, Point3D vbj) Fills a row of constraint matrix for solution 2.private static voidfillRowConstraintMatrixSolution3(int row, com.irurueta.algebra.Matrix c, Point3D vai, Point3D vaj, Point3D vbi, Point3D vbj, Point3D vci, Point3D vcj) Fills a row of constraint matrix for solution 3.private voidTests solution 1 for general point configuration.private voidTests solution 2 for general point configuration.private voidTests solution 3 for general point configuration.Gets intrinsic parameters of camera to be estimated.doubleGets threshold to determine whether 3D matched points are in a planar configuration.getType()Returns type of pinhole camera estimator.protected com.irurueta.algebra.MatrixinternalEstimate(List<Point3D> points3D, List<Point2D> points2D) Internal method that actually computes the normalized pinhole camera internal matrix.private voidinternalSetListsEpnP(List<Point3D> points3D, List<Point2D> points2D) Internal method to set list of corresponding points (it does not check if estimator is locked).booleanIndicates whether the case where a dimension 2 null-space is allowed.booleanIndicates whether the case where a dimension 3 null-space is allowed.booleanisPlanar()Indicates whether provided correspondences were found to be laying in a planar configuration during the estimation.booleanIndicates whether planar configuration is checked to determine whether point correspondences are in such configuration and find a specific solution for such case.booleanisReady()Indicates if this estimator is ready to start the estimation.private static voidnormalizeRow(com.irurueta.algebra.Matrix m, int row) Normalizes provided row of m.private static intnumEquations(int numControl) Number of equations required to solve constraints for case 1 to 4.Picks best solution (the one having the smallest re-projection error).private doublereprojectionError(PinholeCamera camera) Total re-projection error for provided camera.private static double[]Right term of linearized system of equations to solve betas.voidsetIntrinsic(PinholeCameraIntrinsicParameters intrinsic) Sets intrinsic parameters of camera to be estimated.voidSets list of corresponding points.voidsetNullspaceDimension2Allowed(boolean nullspaceDimension2Allowed) Specifies whether the case where a dimension 2 null-space is allowed.voidsetNullspaceDimension3Allowed(boolean nullspaceDimension3Allowed) Specifies whether the case where a dimension 3 null-space is allowed.voidsetPlanarConfigurationAllowed(boolean planarConfigurationAllowed) Specifies whether planar configuration is checked to determine whether point correspondences are in such configuration and find a specific solution for such case.voidsetPlanarThreshold(double planarThreshold) Sets threshold to determine whether 3D matched points are in a planar configuration.voidsetPointCorrespondencesNormalized(boolean normalize) Specifies whether provided point correspondences are normalized to increase the accuracy of the estimation.private voidSolves null-space of matrix M containing possible solutions of camera coordinates of control points.private MetricTransformation3DworldToCameraTransformationMetric(List<Point3D> controlCameraPoints) Estimates world to camera transformation using estimated control points in world and camera coordinates as a metric transformation.Methods inherited from class com.irurueta.geometry.estimators.PointCorrespondencePinholeCameraEstimator
areListsAvailable, areValidLists, attemptRefine, getPoints2D, getPoints3DMethods inherited from class com.irurueta.geometry.estimators.PinholeCameraEstimator
create, create, getListener, getMaxSuggestionWeight, getMinSuggestionWeight, getSuggestedAspectRatioValue, getSuggestedCenterValue, getSuggestedHorizontalFocalLengthValue, getSuggestedPrincipalPointValue, getSuggestedRotationValue, getSuggestedSkewnessValue, getSuggestedVerticalFocalLengthValue, getSuggestionWeightStep, hasSuggestions, isLocked, isSuggestAspectRatioEnabled, isSuggestCenterEnabled, isSuggestHorizontalFocalLengthEnabled, isSuggestPrincipalPointEnabled, isSuggestRotationEnabled, isSuggestSkewnessValueEnabled, isSuggestVerticalFocalLengthEnabled, setListener, setMaxSuggestionWeight, setMinMaxSuggestionWeight, setMinSuggestionWeight, setSuggestAspectRatioEnabled, setSuggestCenterEnabled, setSuggestedAspectRatioValue, setSuggestedCenterValue, setSuggestedHorizontalFocalLengthValue, setSuggestedPrincipalPointValue, setSuggestedRotationValue, setSuggestedSkewnessValue, setSuggestedVerticalFocalLengthValue, setSuggestHorizontalFocalLengthEnabled, setSuggestionWeightStep, setSuggestPrincipalPointEnabled, setSuggestRotationEnabled, setSuggestSkewnessValueEnabled, setSuggestVerticalFocalLengthEnabled
-
Field Details
-
DEFAULT_PLANAR_CONFIGURATION_ALLOWED
public static final boolean DEFAULT_PLANAR_CONFIGURATION_ALLOWEDIndicates that by default planar configuration is checked to determine whether point correspondences are in such configuration and find a specific solution for such case.- See Also:
-
DEFAULT_NULLSPACE_DIMENSION2_ALLOWED
public static final boolean DEFAULT_NULLSPACE_DIMENSION2_ALLOWEDIndicates that by default a dimension 2 null-space is allowed.- See Also:
-
DEFAULT_NULLSPACE_DIMENSION3_ALLOWED
public static final boolean DEFAULT_NULLSPACE_DIMENSION3_ALLOWEDIndicates that by default a dimension 3 null-space is allowed.- See Also:
-
DEFAULT_PLANAR_THRESHOLD
public static final double DEFAULT_PLANAR_THRESHOLDDefault threshold to determine whether 3D matched points are in a planar configuration. Points are considered to be laying in a plane when the smallest singular value of their covariance matrix has a value much smaller than the second smallest as many times as this value.- See Also:
-
GENERAL_NUM_CONTROL_POINTS
private static final int GENERAL_NUM_CONTROL_POINTSNumber of control points used in a general configuration.- See Also:
-
PLANAR_NUM_CONTROL_POINTS
private static final int PLANAR_NUM_CONTROL_POINTSNumber of control points used in a planar configuration.- See Also:
-
planarConfigurationAllowed
private boolean planarConfigurationAllowedIndicates whether planar configuration is checked to determine whether point correspondences are in such configuration and find a specific solution for such case. -
nullspaceDimension2Allowed
private boolean nullspaceDimension2AllowedIndicates whether the case where a dimension 2 null-space is allowed. When allowed, additional constraints are taken into account to ensure equality of scales so that less point correspondences are required. Enabling this parameter is usually ok. -
nullspaceDimension3Allowed
private boolean nullspaceDimension3AllowedIndicates whether the case where a dimension 3 null-space is allowed. When allowed, additional constraints are taken into account to ensure equality of scales so that less point correspondences are required. Enabling this parameter is usually ok although less precise than when a null-space of dimension 2 is used. -
planarThreshold
private double planarThresholdThreshold to determine whether 3D matched points are in a planar configuration. Points are considered to be laying in a plane when the smallest singular value of their covariance matrix has a value much smaller than the largest one as many times as this value. -
intrinsic
Intrinsic parameters of camera to be estimated. -
isPlanar
private boolean isPlanarIndicates whether provided correspondences were found to be laying in a planar configuration during the estimation. -
controlWorldPoints
Computed control points in world coordinates. -
alphas
private com.irurueta.algebra.Matrix alphasContains barycentric coordinates to express 3D world point in terms of control points. For general configuration, each row contains 4 coordinates and alphas has size nx4, where n is the number of provided 3D world points. For planar configuration, each row contains 3 coordinates and alphas has size nx3, where n is the number of provided 3D world points. both reference frames are centered in the centroid, alphas can be used in both world and camera coordinates. -
m
private com.irurueta.algebra.Matrix mM matrix to find control points in camera coordinates. M has size 2*n x 12 (general configuration) or 2*n x 9 (planar configuration), where n is the number of provided 2D observed points. -
nullspace
List containing columns of null-space of M. Linear combinations of these columns contain possible solutions for control points coordinates in camera reference (up to scale). First item of the list contains last column of v, which corresponds to the smallest singular value. Last item of the list contains (column - number of control points) column of v. -
solutions
Possible solutions for the estimation.
-
-
Constructor Details
-
EPnPPointCorrespondencePinholeCameraEstimator
public EPnPPointCorrespondencePinholeCameraEstimator()Constructor. -
EPnPPointCorrespondencePinholeCameraEstimator
Constructor with listener.- Parameters:
listener- listener to be notified of events such as when estimation starts, ends or estimation progress changes.
-
EPnPPointCorrespondencePinholeCameraEstimator
public EPnPPointCorrespondencePinholeCameraEstimator(List<Point3D> points3D, List<Point2D> points2D) throws WrongListSizesException Constructor.- Parameters:
points3D- list of corresponding 3D points.points2D- list of corresponding 2D points.- Throws:
IllegalArgumentException- if any of the lists are null.WrongListSizesException- if provided lists of points don't have the same size and enough points.
-
EPnPPointCorrespondencePinholeCameraEstimator
public EPnPPointCorrespondencePinholeCameraEstimator(List<Point3D> points3D, List<Point2D> points2D, PinholeCameraEstimatorListener listener) throws WrongListSizesException Constructor.- Parameters:
points3D- list of corresponding 3D points.points2D- list of corresponding 2D points.listener- listener to be notified of events such as when estimation starts, ends or estimation progress changes.- Throws:
IllegalArgumentException- if any of the lists are null.WrongListSizesException- if provided lists of points don't have the same size and enough points.
-
EPnPPointCorrespondencePinholeCameraEstimator
Constructor.- Parameters:
intrinsic- intrinsic parameters of camera to be estimated.
-
EPnPPointCorrespondencePinholeCameraEstimator
public EPnPPointCorrespondencePinholeCameraEstimator(PinholeCameraIntrinsicParameters intrinsic, PinholeCameraEstimatorListener listener) Constructor with listener.- Parameters:
intrinsic- intrinsic parameters of camera to be estimated.listener- listener to be notified of events such as when estimation starts, ends or estimation progress changes.- Throws:
IllegalArgumentException- if absolute values of focal lengths are too small.
-
EPnPPointCorrespondencePinholeCameraEstimator
public EPnPPointCorrespondencePinholeCameraEstimator(PinholeCameraIntrinsicParameters intrinsic, List<Point3D> points3D, List<Point2D> points2D) throws WrongListSizesException Constructor.- Parameters:
intrinsic- intrinsic parameters of camera to be estimated.points3D- list of corresponding 3D points.points2D- list of corresponding 2D points.- Throws:
IllegalArgumentException- if any of the lists are null or if absolute values of focal lengths are too small.WrongListSizesException- if provided lists of points don't have the same size and enough points.
-
EPnPPointCorrespondencePinholeCameraEstimator
public EPnPPointCorrespondencePinholeCameraEstimator(PinholeCameraIntrinsicParameters intrinsic, List<Point3D> points3D, List<Point2D> points2D, PinholeCameraEstimatorListener listener) throws WrongListSizesException Constructor.- Parameters:
intrinsic- intrinsic parameters of camera to be estimated.points3D- list of corresponding 3D points.points2D- list of corresponding 2D points.listener- listener to be notified of events such as when estimation starts, ends or estimation progress changes.- Throws:
IllegalArgumentException- if any of the lists are null or if absolute values of focal lengths are too small.WrongListSizesException- if provided lists of points don't have the same size and enough points.
-
-
Method Details
-
setLists
public void setLists(List<Point3D> points3D, List<Point2D> points2D) throws LockedException, WrongListSizesException Sets list of corresponding points.- Overrides:
setListsin classPointCorrespondencePinholeCameraEstimator- Parameters:
points3D- list of corresponding 3D points.points2D- list of corresponding 2D points.- Throws:
LockedException- if estimator is locked.IllegalArgumentException- if any of the lists are null.WrongListSizesException- if provided lists of points don't have the same size and enough points.
-
isPlanarConfigurationAllowed
public boolean isPlanarConfigurationAllowed()Indicates whether planar configuration is checked to determine whether point correspondences are in such configuration and find a specific solution for such case.- Returns:
- true to allow specific solutions for planar configurations, false to always find a solution assuming the general case.
-
setPlanarConfigurationAllowed
public void setPlanarConfigurationAllowed(boolean planarConfigurationAllowed) throws LockedException Specifies whether planar configuration is checked to determine whether point correspondences are in such configuration and find a specific solution for such case.- Parameters:
planarConfigurationAllowed- true to allow specific solutions for planar configurations, false to always find a solution assuming the general case.- Throws:
LockedException- if estimator is locked.
-
isNullspaceDimension2Allowed
public boolean isNullspaceDimension2Allowed()Indicates whether the case where a dimension 2 null-space is allowed. When allowed, additional constraints are taken into account to ensure equality of scales so that less point correspondences are required. Enabling this parameter is usually ok.- Returns:
- true to allow 2-dimensional null-space, false otherwise.
-
setNullspaceDimension2Allowed
public void setNullspaceDimension2Allowed(boolean nullspaceDimension2Allowed) throws LockedException Specifies whether the case where a dimension 2 null-space is allowed. When allowed, additional constraints are taken into account to ensure equality of scales so that less point correspondences are required. Enabling this parameter is usually ok.- Parameters:
nullspaceDimension2Allowed- true to allow 2-dimensional null-space, false otherwise.- Throws:
LockedException- if estimator is locked.
-
isNullspaceDimension3Allowed
public boolean isNullspaceDimension3Allowed()Indicates whether the case where a dimension 3 null-space is allowed. When allowed, additional constraints are taken into account to ensure equality of scales so that less point correspondences are required. Enabling this parameter is usually ok although less precise than when a null-space of dimension 2 is used.- Returns:
- true to allow 3-dimensional null-space, false otherwise.
-
setNullspaceDimension3Allowed
public void setNullspaceDimension3Allowed(boolean nullspaceDimension3Allowed) throws LockedException Specifies whether the case where a dimension 3 null-space is allowed. When allowed, additional constraints are taken into account to ensure equality of scales so that less point correspondences are required. Enabling this parameter is usually ok although less precise than when a null-space of dimension 2 is used.- Parameters:
nullspaceDimension3Allowed- true to allow 3-dimensional null-space, false otherwise.- Throws:
LockedException- if estimator is locked.
-
getPlanarThreshold
public double getPlanarThreshold()Gets threshold to determine whether 3D matched points are in a planar configuration. Points are considered to be laying in a plane when the smallest singular value of their covariance matrix has a value much smaller than the largest one as many times as this value.- Returns:
- threshold to determine whether 3D matched points are in a planar configuration.
-
setPlanarThreshold
Sets threshold to determine whether 3D matched points are in a planar configuration. Points are considered to be laying in a plane when the smallest singular value of their covariance matrix has a value much smaller than the largest one as many times as this value.- Parameters:
planarThreshold- threshold to determine whether 3D matched points are in a planar configuration.- Throws:
IllegalArgumentException- if provided threshold is negative.LockedException- if estimator is locked.
-
getIntrinsic
Gets intrinsic parameters of camera to be estimated.- Returns:
- intrinsic parameters of camera to be estimated.
-
setIntrinsic
Sets intrinsic parameters of camera to be estimated.- Parameters:
intrinsic- intrinsic parameters of camera to be estimated.- Throws:
LockedException- if estimator is locked.
-
isReady
public boolean isReady()Indicates if this estimator is ready to start the estimation.- Specified by:
isReadyin classPinholeCameraEstimator- Returns:
- true if estimator is ready, false otherwise.
-
getType
Returns type of pinhole camera estimator.- Specified by:
getTypein classPinholeCameraEstimator- Returns:
- type of pinhole camera estimator.
-
arePointCorrespondencesNormalized
public boolean arePointCorrespondencesNormalized()Indicates if provided point correspondences are normalized to increase the accuracy of the estimation.- Overrides:
arePointCorrespondencesNormalizedin classPointCorrespondencePinholeCameraEstimator- Returns:
- true if input point correspondences will be normalized, false otherwise.
-
setPointCorrespondencesNormalized
Specifies whether provided point correspondences are normalized to increase the accuracy of the estimation.- Overrides:
setPointCorrespondencesNormalizedin classPointCorrespondencePinholeCameraEstimator- Parameters:
normalize- true if input point correspondences will be normalized, false otherwise.- Throws:
LockedException- if estimator is locked.
-
estimate
public PinholeCamera estimate() throws LockedException, NotReadyException, PinholeCameraEstimatorExceptionEstimates a pinhole camera.- Overrides:
estimatein classPointCorrespondencePinholeCameraEstimator- Returns:
- estimated pinhole camera.
- Throws:
LockedException- if estimator is locked.NotReadyException- if input has not yet been provided.PinholeCameraEstimatorException- if an error occurs during estimation, usually because input data is not valid.
-
isPlanar
public boolean isPlanar()Indicates whether provided correspondences were found to be laying in a planar configuration during the estimation.- Returns:
- true if point correspondences are in a planar configuration, false otherwise.
-
internalEstimate
protected com.irurueta.algebra.Matrix internalEstimate(List<Point3D> points3D, List<Point2D> points2D) Internal method that actually computes the normalized pinhole camera internal matrix. This implementation makes no action.- Specified by:
internalEstimatein classPointCorrespondencePinholeCameraEstimator- Parameters:
points3D- list of 3D points. Points might or might not be normalized.points2D- list of 2D points. Points might or might not be normalized.- Returns:
- matrix of estimated pinhole camera.
-
internalSetListsEpnP
private void internalSetListsEpnP(List<Point3D> points3D, List<Point2D> points2D) throws WrongListSizesException Internal method to set list of corresponding points (it does not check if estimator is locked).- Parameters:
points3D- list of corresponding 3D points.points2D- list of corresponding 2D points.- Throws:
IllegalArgumentException- if any of the lists are nullWrongListSizesException- if provided lists of points don't have the same size and enough points.
-
pickBestSolution
Picks best solution (the one having the smallest re-projection error).- Returns:
- best solution.
-
generalSolution3
private void generalSolution3() throws com.irurueta.algebra.AlgebraException, LockedException, NotReadyException, CoincidentPointsExceptionTests solution 3 for general point configuration. Because solution is up to scale, 8 different solutions for different beta1, beta2 and beta3 signs are tried.- Throws:
com.irurueta.algebra.AlgebraException- if a numerical degeneracy occurs.LockedException- never happens.NotReadyException- never happens.CoincidentPointsException- if a point degeneracy has occurred.
-
generalSolution2
private void generalSolution2() throws com.irurueta.algebra.AlgebraException, LockedException, NotReadyException, CoincidentPointsExceptionTests solution 2 for general point configuration. Because solution is up to scale, 4 different solutions for different beta1 and beta2 signs are tried.- Throws:
com.irurueta.algebra.AlgebraException- if a numerical degeneracy occurs.LockedException- never happens.NotReadyException- never happens.CoincidentPointsException- if a point degeneracy has occurred.
-
constraintMatrixSolution3
private static com.irurueta.algebra.Matrix constraintMatrixSolution3(List<Point3D> controlCameraPointsA, List<Point3D> controlCameraPointsB, List<Point3D> controlCameraPointsC) throws com.irurueta.algebra.AlgebraException Fills constraint matrix to solve betas when using control points from the last 3 columns of v (the null-space). The solution will be the linear combination of control points from the last 3 columns using estimated betas. This solution will be control points in camera coordinates.- Parameters:
controlCameraPointsA- control points of last column of v.controlCameraPointsB- control points of second last column of v.controlCameraPointsC- control points of third last column of v.- Returns:
- constraint matrix to solve a linear system of equations.
- Throws:
com.irurueta.algebra.AlgebraException- never happens.
-
constraintMatrixSolution2
private static com.irurueta.algebra.Matrix constraintMatrixSolution2(List<Point3D> controlCameraPointsA, List<Point3D> controlCameraPointsB) throws com.irurueta.algebra.AlgebraException Fills constraint matrix to solve betas when using control points from the last 2 columns of v (the null-space). The solution will be the linear combination of control points from the last 2 columns using estimated betas. This solution will be control points in camera coordinates.- Parameters:
controlCameraPointsA- control points of last column of v.controlCameraPointsB- control points of second last column of v.- Returns:
- constraint matrix to solve a linear system of equations.
- Throws:
com.irurueta.algebra.AlgebraException- never happens.
-
fillRowConstraintMatrixSolution3
private static void fillRowConstraintMatrixSolution3(int row, com.irurueta.algebra.Matrix c, Point3D vai, Point3D vaj, Point3D vbi, Point3D vbj, Point3D vci, Point3D vcj) Fills a row of constraint matrix for solution 3.- Parameters:
row- row to be filled.c- matrix to be filled.vai- i-th control point in camera coordinates of last column of v (i.e. the null-space).vaj- j-th control point in camera coordinates of last column of v (i.e. the null-space).vbi- i-th control point in camera coordinates of second last column of v (i.e. the null-space).vbj- j-th control point in camera coordinates of second last column of v (i.e. the null-space).vci- i-th control point in camera coordinates of third last column of v (i.e. the null-space).vcj- j-th control point in camera coordinates of third last column of v (i.e. the null-space).
-
fillRowConstraintMatrixSolution2
private static void fillRowConstraintMatrixSolution2(int row, com.irurueta.algebra.Matrix c, Point3D vai, Point3D vaj, Point3D vbi, Point3D vbj) Fills a row of constraint matrix for solution 2.- Parameters:
row- row to be filled.c- matrix to be filled.vai- i-th control point in camera coordinates of last column of v (i.e. the null-space).vaj- j-th control point in camera coordinates of last column of v (i.e. the null-space).vbi- i-th control point in camera coordinates of second last column of v (i.e. the null-space).vbj- j-th control point in camera coordinates of second last column of v (i.e. the null-space).
-
generalSolution1
private void generalSolution1() throws LockedException, NotReadyException, CoincidentPointsExceptionTests solution 1 for general point configuration. Because solution is up to scale. Two possible solutions must be evaluated (positive or negative scale). The one with the smallest re-projection error will be picked.- Throws:
LockedException- never happens.NotReadyException- never happens.CoincidentPointsException- if a point degeneracy has occurred.
-
computePossibleSolutionWithPoseAndReprojectionError
private EPnPPointCorrespondencePinholeCameraEstimator.Solution computePossibleSolutionWithPoseAndReprojectionError(List<Point3D> controlCameraPoints) throws LockedException, NotReadyException, CoincidentPointsException Computes a possible solution with camera, transformation, re-projection error and control points in camera coordinates.- Parameters:
controlCameraPoints- control points in camera coordinates.- Returns:
- a possible solution.
- Throws:
LockedException- never happens.NotReadyException- never happens.CoincidentPointsException- if a point degeneracy has occurred.
-
worldToCameraTransformationMetric
private MetricTransformation3D worldToCameraTransformationMetric(List<Point3D> controlCameraPoints) throws LockedException, NotReadyException, CoincidentPointsException Estimates world to camera transformation using estimated control points in world and camera coordinates as a metric transformation.- Parameters:
controlCameraPoints- control points in camera coordinates.- Returns:
- metric transformation relating control points from world to camera coordinates.
- Throws:
LockedException- never happens.NotReadyException- never happens.CoincidentPointsException- if a point degeneracy has occurred.
-
numEquations
private static int numEquations(int numControl) Number of equations required to solve constraints for case 1 to 4.- Parameters:
numControl- number of control points.- Returns:
- number of constraint equations.
-
rhos
Right term of linearized system of equations to solve betas.- Parameters:
controlWorldPoints- control points in world coordinates.- Returns:
- right term.
-
reprojectionError
Total re-projection error for provided camera.- Parameters:
camera- camera to estimate re-projection error.- Returns:
- reprojection error.
-
controlPointsFromV
Computes list of control points from provided array containing one column of the null-space of M or a linear combination of columns of the null-space.- Parameters:
v- one column of the null-space of M or a linear combination of columns of the null-space.- Returns:
- control points.
-
solveNullspace
private void solveNullspace() throws com.irurueta.algebra.AlgebraExceptionSolves null-space of matrix M containing possible solutions of camera coordinates of control points.- Throws:
com.irurueta.algebra.AlgebraException- if something fails due to numerical instabilities.
-
normalizeRow
private static void normalizeRow(com.irurueta.algebra.Matrix m, int row) Normalizes provided row of m.- Parameters:
m- matrix to be normalized.row- row to be normalized.
-
buildM
private void buildM() throws com.irurueta.algebra.AlgebraExceptionIn order to find control points in camera coordinates, an homogeneous linear system of equations must be solved having the form M*x = 0, where x contains the coordinates of all control points in the form [x1, y1, z1, x2, y2, z2, ... ]. For general configuration there are 4 control points, hence x has length 12 (3 coordinates * 4 control points). For a planar configuration there are 3 control points, hence x has length 9 (3 coordinates * 3 control points). This method builds M matrix required to solve such linear system of equations, where M has size 2*n x 12 (general configuration) or 2*n x 9 (planar configuration), where n is the number of provided 2D observed points.- Throws:
com.irurueta.algebra.AlgebraException- if numerical instabilities occur.
-
computeBarycentricCoordinates
private void computeBarycentricCoordinates() throws com.irurueta.algebra.AlgebraExceptionComputes the coordinates of each provided world point in terms of estimated control points in world coordinates. Such coordinates (i.e. barycentric coordinates) are stored in alphas matrix, where each row contains the coordinates of each world point in terms of control points. For general configuration, each row contains 4 coordinates and alphas has size nx4, where n is the number of provided 3D world points. For planar configuration, each row contains 3 coordinates and alphas has size nx3, where n is the number of provided 3D world points. Because world and camera coordinates are related by a rotation (since both reference frames are centered in the centroid), alphas can be used in both world and camera coordinates.- Throws:
com.irurueta.algebra.AlgebraException- if there are numerical instabilities.
-
computeWorldControlPointsAndPointConfiguration
private void computeWorldControlPointsAndPointConfiguration() throws com.irurueta.algebra.AlgebraExceptionComputes control points in world coordinates and determines whether they are located in a planar configuration or not. This method computes the centroid of provided 3D points and their covariance. Uses PCA by means of SVD decomposition of their covariance matrix in order to find the principal directions of the cloud formed by the collection of points and sets control points as the computed centroid and points along the principal axes so that they form a basis that can be used to express any 3D points into. If the smallest singular value is close to zero in comparison to the largest one, then it is assumed that 3D points are in a planar configuration. If a planar configuration is allowed, then only 3 control points are computed along the plane using the centroid and two points on the principal directions of such plane. Otherwise, in general configuration, 4 control points are computed as the centroid and 3 points along the principal axes of the cloud of 3D points.- Throws:
com.irurueta.algebra.AlgebraException- if something fails because of numerical instabilities.
-