Package com.irurueta.geometry.estimators

Class UPnPPointCorrespondencePinholeCameraEstimator

java.lang.Object
com.irurueta.geometry.estimators.PinholeCameraEstimator
com.irurueta.geometry.estimators.PointCorrespondencePinholeCameraEstimator
com.irurueta.geometry.estimators.UPnPPointCorrespondencePinholeCameraEstimator
public class UPnPPointCorrespondencePinholeCameraEstimator extends PointCorrespondencePinholeCameraEstimator
UPnP (Uncalibrated Perspective-n-Point) implementation to estimate pinhole cameras from 2D/3D point correspondences. This class besides determining camera pose is also capable to find its focal length assuming unitary aspect ratio (equal horizontal and vertical focal lengths) and that the resulting focal length is positive. This class is an implementation following the one proposed by Adrian Penate-Senchez et al. on "Exhaustive Linearization for Robust Camera Pose and Focal Length Estimation". Paper can be found at: http://www.iri.upc.edu/files/scidoc/1404-Exhaustive-linearization-for-robust-camera-pose-and-focal-length-estimation.pdf

  • 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 not 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:

    • DEFAULT_SKEWNESS

      public static final double DEFAULT_SKEWNESS
      Default skewness value.
      See Also:

    • DEFAULT_HORIZONTAL_PRINCIPAL_POINT

      public static final double DEFAULT_HORIZONTAL_PRINCIPAL_POINT
      Default value for horizontal coordinate of principal point.
      See Also:

    • DEFAULT_VERTICAL_PRINCIPAL_POINT

      public static final double DEFAULT_VERTICAL_PRINCIPAL_POINT
      Default value for vertical coordinate of principal point.
      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.

    • 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.

    • skewness

      private double skewness
      Skewness value of intrinsic parameters to be used on estimated camera.

    • horizontalPrincipalPoint

      private double horizontalPrincipalPoint
      Horizontal coordinate of principal point on intrinsic parameters to be used on estimated camera.

    • verticalPrincipalPoint

      private double verticalPrincipalPoint
      Vertical coordinate of principal point on intrinsic parameters to be used on estimated camera.

    • 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 and focal length. 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) with z terms normalized by an unknown focal length. 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

      private List<UPnPPointCorrespondencePinholeCameraEstimator.Solution> solutions
      Possible solutions for the estimation.

  • Constructor Details

    • UPnPPointCorrespondencePinholeCameraEstimator

      public UPnPPointCorrespondencePinholeCameraEstimator()
      Constructor.

    • UPnPPointCorrespondencePinholeCameraEstimator

      public UPnPPointCorrespondencePinholeCameraEstimator(PinholeCameraEstimatorListener listener)
      Constructor with listener.
      Parameters:
      listener - listener to be notified of events such as when estimation starts, ends or estimation progress changes.

    • UPnPPointCorrespondencePinholeCameraEstimator

      public UPnPPointCorrespondencePinholeCameraEstimator(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.

    • UPnPPointCorrespondencePinholeCameraEstimator

      public UPnPPointCorrespondencePinholeCameraEstimator(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.

  • 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.

    • 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.

    • getSkewness

      public double getSkewness()
      Gets skewness value of intrinsic parameters to be used on estimated camera.
      Returns:
      skewness value of intrinsic parameters to be used on estimated camera.

    • setSkewness

      public void setSkewness(double skewness) throws LockedException
      Sets skewness value of intrinsic parameters to be used on estimated camera.
      Parameters:
      skewness - skewness value of intrinsic parameters to be used on estimated camera.
      Throws:
      LockedException - if estimator is locked.

    • getHorizontalPrincipalPoint

      public double getHorizontalPrincipalPoint()
      Returns horizontal coordinate of principal point on intrinsic parameters to be used on estimated camera.
      Returns:
      horizontal coordinate of principal point on intrinsic parameters to be used on estimated camera.

    • setHorizontalPrincipalPoint

      public void setHorizontalPrincipalPoint(double horizontalPrincipalPoint) throws LockedException
      Sets horizontal coordinate of principal point on intrinsic parameters to be used on estimated camera.
      Parameters:
      horizontalPrincipalPoint - horizontal coordinate of principal point on intrinsic parameters to be used on estimated camera.
      Throws:
      LockedException - if estimator is locked.

    • getVerticalPrincipalPoint

      public double getVerticalPrincipalPoint()
      Returns vertical coordinate of principal point on intrinsic parameters to be used on estimated camera.
      Returns:
      vertical coordinate of principal point on intrinsic parameters to be used on estimated camera.

    • setVerticalPrincipalPoint

      public void setVerticalPrincipalPoint(double verticalPrincipalPoint) throws LockedException
      Sets vertical coordinate of principal point on intrinsic parameters to be used on estimated camera.
      Parameters:
      verticalPrincipalPoint - vertical coordinate of principal point on intrinsic parameters to be used on estimated camera.
      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

      public PinholeCamera estimate() throws LockedException, NotReadyException, PinholeCameraEstimatorException
      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.

    • internalSetListsUPnP

      private void internalSetListsUPnP(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

      private UPnPPointCorrespondencePinholeCameraEstimator.Solution pickBestSolution()
      Picks best solution (the one having the smallest re-projection error).
      Returns:
      best solution.

    • generalSolution2

      private void generalSolution2() throws com.irurueta.algebra.AlgebraException
      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.

    • 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 and focal length using control points (with normalized z coordinates by an unknown focal length) from last 2 columns of v (the null-space). The solution obtained with this constraint matrix and rhos will be control points in camera coordinates and estimated focal length.
      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.

    • 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 solution2. Solution 2 takes into account the last 2 columns of v as its null-space: va = [vax, vay, vaz/f] and vb = [vbx, vby, vbz/f]. Constraint: ||beta*vi - beta*vj||^2 = ||ci - cj||^2, i,j 1...4 we need to find beta to scale control camera points, but since we are using 2 columns of the null-space v, then v is a linear combination v = beta1*vA + beta2*vB and the previous constraint becomes: ||(beta1*vAi + beta2*vBi) - (beta1*vAj + beta2*vBj)||^2 = ||ci - cj||^2, i,j 1...4 This results in a linear system of 6 equations (when we have 4 control points) and 6 unknowns The previous constraint can be expanded as follows: ((beta1*vAi + beta2*vBi) - (beta1*vAj + beta2*vBj))^2 = (ci - cj)^2, i,j 1...4 ((beta1*vAix + beta2*vBix) - (beta1*vAjx + beta2*vBjx))^2 + ((beta1*vAiy + beta2*vBiy) - (beta1*vAjy + beta2*vBjy))^2 + ((beta1*vAiz + beta2*vBiz)*f - (beta1*vAjz + beta2*vBjz)*f)^2 = (cix - cjx)^2 + (ciy - cjy)^2 + (ciz - cjz)^2, i,j 1...4 (beta1*(vAix - vAjx) + beta2*(vBix - vBjx))^2 + (beta1*(vAiy - vAjy) + beta2*(vBiy - vBjy))^2 + (beta1*(vAiz - vAjz)*f + beta2*(vBiz - vBjz)*f)^2 = (cix - cjx)^2 + (ciy - cjy)^2 + (ciz - cjz)^2, i,j 1...4 beta1^2*(vAix - vAjx)^2 + beta1*beta2*2*(vAix - vAjx)*(vBix - vBjx) + beta2^2*(vBix - vBjx)^2 + beta1^2*(vAiy - vAjy)^2 + beta1*beta2*2*(vAiy - vAjy)*(vBiy - vBjy) + beta2^2*(vBiy - vBjy)^2 + beta1^2*f^2*(vAiz - vAjz)^2 + beta1*beta2*f^2*2*(vAiz - vAjz)*(vBiz - vBjz) + beta2^2*f^2*(vBiz - vBjz)^2 = ((cix - cjx)^2 + (ciy - cjy)^2 + (ciz - cjz)^2), i,j 1...4

      Since beta1, beta2 and f are the unknowns, we can reorganize equation as: beta1^2*((vAix - vAjx)^2 + (vAiy - vAjy)^2) + beta1*beta2*2*((vAix - vAjx)*(vBix - vBjx) + (vAiy - vAjy)*(vBiy - vBjy)) + beta2^2*((vBix - vBjx)^2 + (vBiy - vBjy)^2)+ beta1^2*f^2*(vAiz - vAjz)^2 + beta1*beta2*f^2*2*(vAiz - vAjz)*(vBiz - vBjz) + beta2^2*f^2*(vBiz - vBjz)^2 = ((cix - cjx)^2 + (ciy - cjy)^2 + (ciz - cjz)^2), i,j 1...4

      The system is linearized assuming: alpha1 = beta1^2 alpha2 = beta1*beta2 alpha3 = beta2^2 alpha4 = beta1^2*f^2 alpha5 = beta1*beta2*f^2 alpha6 = beta2^2*f^2

      alpha1*((vAix - vAjx)^2 + (vAiy - vAjy)^2) + alpha2*2*((vAix - vAjx)*(vBix - vBjx) + (vAiy - vAjy)*(vBiy - vBjy)) + alpha3*((vBix - vBjx)^2 + (vBiy - vBjy)^2)+ alpha4*(vAiz - vAjz)^2 + alpha5*2*(vAiz - vAjz)*(vBiz - vBjz) + alpha6*(vBiz - vBjz)^2 = ((cix - cjx)^2 + (ciy - cjy)^2 + (ciz - cjz)^2), i,j 1...4

      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 nullspace) where z coordinate is normalized by some unknown focal length.
      vaj - j-th control point in camera coordinates of last column of v (i.e. the nullspace) where z coordinate is normalized by some unknown focal length.
      vbi - i-th control point in camera coordinates of second last column of v (i.e. the nullspace) where z coordinate is normalized by some unknown focal length.
      vbj - j-th control point in camera coordinates of second last column of v (i.e. the nullspace) where z coordinate is normalized by some unknown focal length.

    • generalSolution1

      private void generalSolution1() throws com.irurueta.algebra.AlgebraException
      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:
      com.irurueta.algebra.AlgebraException - if a numerical degeneracy occurs.

    • denormalizeV

      private static void denormalizeV(double[] v, double focalLength)
      Denormalizes v array containing the null-space of M, which contains the control points in camera coordinates in consecutive order but having z coordinates normalized by focal length. After execution of this method, z coordinates will be denormalized.
      Parameters:
      v - array containing the null-space of M with normalized z coordinates.
      focalLength - focal length to use for de-normalization.

    • constraintMatrixSolution1

      private static com.irurueta.algebra.Matrix constraintMatrixSolution1(List<Point3D> controlCameraPoints) throws com.irurueta.algebra.AlgebraException
      Fills constraint matrix to solve beta and focal length using control points (with normalized z coordinates by an unknown focal length) from the last column of v (the null-space). The solution obtained with this constraint matrix and rhos will be control points in camera coordinates and estimated focal length.
      Parameters:
      controlCameraPoints - control points of last column of v.
      Returns:
      constraint matrix to solve a linear system of equations.
      Throws:
      com.irurueta.algebra.AlgebraException - never happens.

    • fillRowConstraintMatrixSolution1

      private static void fillRowConstraintMatrixSolution1(int row, com.irurueta.algebra.Matrix c, Point3D vi, Point3D vj)
      Fills a row of constraint matrix for solution 1. The following constraint is imposed on the null-space of v = [vx, vy, vz/f]. Constraint: ||beta*vi - beta*vj||^2 = ||ci - cj||^2, i,j 1...4 This results in a linear system of 6 equations (when we have 4 control points) and 2 unknowns. The previous constraint can be expanded as follows: (beta*vi - beta*vj)^2 = (ci - cj)^2 (beta*vix - beta*vjx)^2 + (beta*viy - beta*vjy)^2 + (beta*viz*f - beta*vjz*f)^2 = (cix - cjx)^2 + (ciy - cjy)^2 + (ciz - cjz)^2, i,j 1...4 beta^2*(vix - vjx)^2 + beta^2*(viy - vjy)^2 + beta^2*f^2*(viz - vjz)^2 = (cix - cjx)^2 + (ciy - cjy)^2 + (ciz - cjz)^2, i,j 1...4 beta^2*((vix - vjx)^2 + (viy - vjy)^2) + beta^2*f^2*(viz - vjz)^2 = (cix - cjx)^2 + (ciy - cjy)^2 + (ciz - cjz)^2, i,j 1...4

      The system is linearized assuming alpha1 = beta^2 alpha2 = beta^2/f^2

      alpha1*((vix - vjx)^2 + (viy - vjy)^2) + alpha2*(viz - vjz)^2 = (cix - cjx)^2 + (ciy - cjy)^2 + (ciz - cjz)^2, i,j 1...4

      Parameters:
      row - row to be filled.
      c - matrix to be filled.
      vi - i-th control point in camera coordinates of last column of v (i.e. the null-space of m) where z coordinate is normalized by some unknown focal length.
      vj - j-th control point in camera coordinates of last column of v (i.e. the nullspace of m) where z coordinate is normalized by some unknown focal length.

    • computePossibleSolutionWithPoseAndReprojectionError

      private UPnPPointCorrespondencePinholeCameraEstimator.Solution computePossibleSolutionWithPoseAndReprojectionError(List<Point3D> controlCameraPoints, double focalLength) 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.
      focalLength - estimated focal length.
      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:
      re-projection 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/f, x2, y2, z2/f, ... ] where f is an unknown focal length normalizing z terms. 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.