Class 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
  • Field Details

    • DEFAULT_PLANAR_CONFIGURATION_ALLOWED

      public static final boolean DEFAULT_PLANAR_CONFIGURATION_ALLOWED
      Indicates 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_ALLOWED
      Indicates that by default a dimension 2 null-space is allowed.
      See Also:
    • DEFAULT_NULLSPACE_DIMENSION3_ALLOWED

      public static final boolean DEFAULT_NULLSPACE_DIMENSION3_ALLOWED
      Indicates that by default a dimension 3 null-space is allowed.
      See Also:
    • DEFAULT_PLANAR_THRESHOLD

      public static final double DEFAULT_PLANAR_THRESHOLD
      Default 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_POINTS
      Number of control points used in a general configuration.
      See Also:
    • PLANAR_NUM_CONTROL_POINTS

      private static final int PLANAR_NUM_CONTROL_POINTS
      Number of control points used in a planar configuration.
      See Also:
    • planarConfigurationAllowed

      private boolean planarConfigurationAllowed
      Indicates 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 nullspaceDimension2Allowed
      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.
    • nullspaceDimension3Allowed

      private boolean nullspaceDimension3Allowed
      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.
    • planarThreshold

      private double planarThreshold
      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.
    • intrinsic

      Intrinsic parameters of camera to be estimated.
    • isPlanar

      private boolean isPlanar
      Indicates whether provided correspondences were found to be laying in a planar configuration during the estimation.
    • controlWorldPoints

      private List<Point3D> controlWorldPoints
      Computed control points in world coordinates.
    • alphas

      private com.irurueta.algebra.Matrix alphas
      Contains 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 m
      M 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

      private List<double[]> 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

      public EPnPPointCorrespondencePinholeCameraEstimator(PinholeCameraEstimatorListener listener)
      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

      public EPnPPointCorrespondencePinholeCameraEstimator(PinholeCameraIntrinsicParameters intrinsic)
      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:
      setLists in class PointCorrespondencePinholeCameraEstimator
      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

      public void setPlanarThreshold(double planarThreshold) throws LockedException
      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

      public PinholeCameraIntrinsicParameters getIntrinsic()
      Gets intrinsic parameters of camera to be estimated.
      Returns:
      intrinsic parameters of camera to be estimated.
    • setIntrinsic

      public void setIntrinsic(PinholeCameraIntrinsicParameters intrinsic) throws LockedException
      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:
      isReady in class PinholeCameraEstimator
      Returns:
      true if estimator is ready, false otherwise.
    • getType

      public PinholeCameraEstimatorType getType()
      Returns type of pinhole camera estimator.
      Specified by:
      getType in class PinholeCameraEstimator
      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:
      arePointCorrespondencesNormalized in class PointCorrespondencePinholeCameraEstimator
      Returns:
      true if input point correspondences will be normalized, false otherwise.
    • setPointCorrespondencesNormalized

      public void setPointCorrespondencesNormalized(boolean normalize) throws LockedException
      Specifies whether provided point correspondences are normalized to increase the accuracy of the estimation.
      Overrides:
      setPointCorrespondencesNormalized in class PointCorrespondencePinholeCameraEstimator
      Parameters:
      normalize - true if input point correspondences will be normalized, false otherwise.
      Throws:
      LockedException - if estimator is locked.
    • estimate

      Estimates a pinhole camera.
      Overrides:
      estimate in class PointCorrespondencePinholeCameraEstimator
      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:
      internalEstimate in class PointCorrespondencePinholeCameraEstimator
      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 null
      WrongListSizesException - 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, CoincidentPointsException
      Tests 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, CoincidentPointsException
      Tests 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, CoincidentPointsException
      Tests 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

      private static double[] rhos(List<Point3D> controlWorldPoints)
      Right term of linearized system of equations to solve betas.
      Parameters:
      controlWorldPoints - control points in world coordinates.
      Returns:
      right term.
    • reprojectionError

      private double reprojectionError(PinholeCamera camera)
      Total re-projection error for provided camera.
      Parameters:
      camera - camera to estimate re-projection error.
      Returns:
      reprojection error.
    • controlPointsFromV

      private List<Point3D> 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.
      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.AlgebraException
      Solves 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.AlgebraException
      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, ... ]. 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.AlgebraException
      Computes 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.AlgebraException
      Computes 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.