Package boofcv.alg.geo

Class MultiViewOps

java.lang.Object
boofcv.alg.geo.MultiViewOps
public class MultiViewOps extends Object

Contains commonly used operations used in 2-view and 3-view perspective geometry.

LINES: lines on the image place are represented in homogeneous or generic form as a 3D vector. If a point in homogeneous coordinates is on a line and the dot product is computed the result will be zero.

  • Constructor Details

    • MultiViewOps

      public MultiViewOps()

  • Method Details

    • createTrifocal

      public static TrifocalTensor createTrifocal(DMatrixRMaj P2, DMatrixRMaj P3, @Nullable @Nullable TrifocalTensor ret)

      Creates a trifocal tensor from two camera matrices. Tijk = a[j,i]*b[k,3] - a[j,3]*b[k,i], where a=P2 and b=P3.

      IMPORTANT: It is assumed that the first camera has the following camera matrix P1 = [I|0], where I is an identify matrix.

      Parameters:
      P2 - Camera matrix for view 2. 3x4 matrix
      P3 - Camera matrix for view 3. 3x4 matrix
      ret - Storage for trifocal tensor. If null a new instance will be created.
      Returns:
      The trifocal tensor

    • createTrifocal

      public static TrifocalTensor createTrifocal(DMatrixRMaj P1, DMatrixRMaj P2, DMatrixRMaj P3, @Nullable @Nullable TrifocalTensor ret)

      Creates a trifocal tensor from three camera matrices. The

      Page 415 in R. Hartley, and A. Zisserman, "Multiple View Geometry in Computer Vision", 2nd Ed, Cambridge 2003

      WARNING: This method of creating a trifocal tensor might be numerically less accurate. A unit test had to be changed which used this method to use another approach to constructing the tensor because it was failing even with perfect data. With some thought this issue could be fixed.

      Parameters:
      P1 - Camera matrix for view 1. 3x4 matrix
      P2 - Camera matrix for view 2. 3x4 matrix
      P3 - Camera matrix for view 3. 3x4 matrix
      ret - Storage for trifocal tensor. If null a new instance will be created.
      Returns:
      The trifocal tensor

    • createTrifocal

      public static TrifocalTensor createTrifocal(Se3_F64 P2, Se3_F64 P3, @Nullable @Nullable TrifocalTensor ret)

      Creates a trifocal tensor from two rigid body motions. This is for the calibrated camera case.

      NOTE: View 1 is the world coordinate system, i.e. [I|0]

      Parameters:
      P2 - Transform from view 1 to view 2.
      P3 - Transform from view 1 to view 3.
      ret - Storage for trifocal tensor. If null a new instance will be created.
      Returns:
      The trifocal tensor

    • constraint

      public static Vector3D_F64 constraint(TrifocalTensor tensor, Vector3D_F64 l1, Vector3D_F64 l2, Vector3D_F64 l3, @Nullable @Nullable Vector3D_F64 ret)

      Trifocal tensor with line-line-line correspondence:
      (l2T*[T1,T2,T3]*L2)*[l1]x = 0

      Parameters:
      tensor - Trifocal tensor
      l1 - A line in the first view.
      l2 - A line in the second view.
      l3 - A line in the third view.
      ret - Storage for output. If null a new instance will be declared.
      Returns:
      Result of applying the constraint. With perfect inputs will be zero.

    • constraint

      public static double constraint(TrifocalTensor tensor, Point2D_F64 p1, Vector3D_F64 l2, Vector3D_F64 l3)

      Trifocal tensor with point-line-line correspondence:
      (l2T*(sum p1i*Ti)*l3 = 0

      Parameters:
      tensor - Trifocal tensor
      p1 - A point in the first view.
      l2 - A line in the second view.
      l3 - A line in the third view.
      Returns:
      Result of applying the constraint. With perfect inputs will be zero.

    • constraint

      public static Vector3D_F64 constraint(TrifocalTensor tensor, Point2D_F64 p1, Vector3D_F64 l2, Point2D_F64 p3, Vector3D_F64 ret)

      Trifocal tensor with point-line-point correspondence:
      (l2T(sum p1i*Ti)[p3]x = 0

      Parameters:
      tensor - Trifocal tensor
      p1 - A point in the first view.
      l2 - A line in the second view.
      p3 - A point in the third view.
      Returns:
      Result of applying the constraint. With perfect inputs will be zero.

    • constraint

      public static Vector3D_F64 constraint(TrifocalTensor tensor, Point2D_F64 p1, Point2D_F64 p2, Vector3D_F64 l3, Vector3D_F64 ret)

      Trifocal tensor with point-point-line correspondence:
      [p2]x(sum p1i*Ti)*l3 = 0

      Parameters:
      tensor - Trifocal tensor
      p1 - A point in the first view.
      p2 - A point in the second view.
      l3 - A line in the third view.
      Returns:
      Result of applying the constraint. With perfect inputs will be zero.

    • constraint

      public static DMatrixRMaj constraint(TrifocalTensor tensor, Point2D_F64 p1, Point2D_F64 p2, Point2D_F64 p3, DMatrixRMaj ret)

      Trifocal tensor with point-point-point correspondence:
      [p2]x(sum p1i*Ti)[p3]x = 0

      Parameters:
      tensor - Trifocal tensor
      p1 - A point in the first view.
      p2 - A point in the second view.
      p3 - A point in the third view.
      ret - Optional storage for output. 3x3 matrix. Modified.
      Returns:
      Result of applying the constraint. With perfect inputs will be zero.

    • constraint

      public static double constraint(DMatrixRMaj F, Point2D_F64 p1, Point2D_F64 p2)

      Applies the epipolar relationship constraint to an essential or fundamental matrix:
      0 = p2T*F*p1
      Input points are in normalized image coordinates for an essential matrix and pixels for fundamental.

      Parameters:
      F - 3x3 essential or fundamental matrix.
      p1 - Point in view 1.
      p2 - Point in view 2.
      Returns:
      Constraint value.

    • constraintHomography

      public static Point2D_F64 constraintHomography(DMatrixRMaj H, Point2D_F64 p1, Point2D_F64 outputP2)

      Applies the homography constraints to two points:
      z*p2 = H*p1
      where z is a scale factor and (p1,p2) are point observations. Note that since 2D points are inputted translation and normalization to homogeneous coordinates with z=1 is automatically handled.

      Parameters:
      H - Input: 3x3 Homography matrix.
      p1 - Input: Point in view 1.
      outputP2 - Output: storage for point in view 2.
      Returns:
      Predicted point in view 2

    • inducedHomography13

      public static DMatrixRMaj inducedHomography13(TrifocalTensor tensor, Vector3D_F64 line2, DMatrixRMaj output)
      Computes the homography induced from view 1 to 3 by a line in view 2. The provided line in view 2 must contain the view 2 observation. p3 = H13*p1
      Parameters:
      tensor - Input: Trifocal tensor
      line2 - Input: Line in view 2. General notation.
      output - Output: Optional storage for homography. 3x3 matrix
      Returns:
      Homography from view 1 to 3

    • inducedHomography12

      public static DMatrixRMaj inducedHomography12(TrifocalTensor tensor, Vector3D_F64 line3, DMatrixRMaj output)
      Computes the homography induced from view 1 to 2 by a line in view 3. The provided line in view 3 must contain the view 3 observation. p2 = H12*p1
      Parameters:
      tensor - Input: Trifocal tensor
      line3 - Input: Line in view 3. General notation.
      output - Output: Optional storage for homography. 3x3 matrix
      Returns:
      Homography from view 1 to 2

    • fundamentalToHomography3Pts

      @Nullable public static @Nullable DMatrixRMaj fundamentalToHomography3Pts(DMatrixRMaj F, AssociatedPair p1, AssociatedPair p2, AssociatedPair p3)
      Computes the homography induced from a planar surface when viewed from two views using correspondences of three points. Observations must be on the planar surface.

      WARNING: As implemented, it seems to have stability issues or a bug. See code for comments. Please fix and update unit test.

      Parameters:
      F - Fundamental matrix
      p1 - Associated point observation
      p2 - Associated point observation
      p3 - Associated point observation
      Returns:
      The homography from view 1 to view 2 or null if it fails
      See Also:

    • fundamentalToHomographyLinePt

      public static DMatrixRMaj fundamentalToHomographyLinePt(DMatrixRMaj F, PairLineNorm line, AssociatedPair point)
      Computes the homography induced from a planar surface when viewed from two views using correspondences of a line and a point. Observations must be on the planar surface.
      Parameters:
      F - Fundamental matrix
      line - Line on the plane
      point - Point on the plane
      Returns:
      The homography from view 1 to view 2 or null if it fails
      See Also:

    • fundamentalToHomography2Lines

      @Nullable public static @Nullable DMatrixRMaj fundamentalToHomography2Lines(DMatrixRMaj F, PairLineNorm line0, PairLineNorm line1)
      Computes the homography induced from a planar surface when viewed from two views using correspondences of two lines. Observations must be on the planar surface.
      Parameters:
      F - Fundamental matrix
      line0 - Line on the plane
      line1 - Line on the plane
      Returns:
      The homography from view 1 to view 2 or null if it fails
      See Also:

    • extractEpipoles

      public static void extractEpipoles(TrifocalTensor tensor, Point3D_F64 e2, Point3D_F64 e3)

      Computes the epipoles of the first camera in the second and third images. Epipoles are found in homogeneous coordinates and have a norm of 1.

      Properties:

      • e2T*F12 = 0
      • e3T*F13 = 0
      where F1i is a fundamental matrix from image 1 to i.

      Parameters:
      tensor - Trifocal tensor. Not Modified
      e2 - Output: Epipole in image 2. Homogeneous coordinates. Modified
      e3 - Output: Epipole in image 3. Homogeneous coordinates. Modified
      See Also:

    • trifocalToFundamental

      public static void trifocalToFundamental(TrifocalTensor tensor, DMatrixRMaj F21, DMatrixRMaj F31)

      Extract the fundamental matrices between views 1 + 2 and views 1 + 3. The returned Fundamental matrices will have the following properties: xiT*Fi*x1 = 0, where i is view 2 or 3.

      NOTE: The first camera is assumed to have the camera matrix of P1 = [I|0]. Thus observations in pixels for the first camera will not meet the epipolar constraint when applied to the returned fundamental matrices.

      Parameters:
      tensor - Trifocal tensor. Not modified.
      F21 - Output: Fundamental matrix for views 1 and 2. Modified.
      F31 - Output: Fundamental matrix for views 1 and 3. Modified.
      See Also:

    • trifocalToCameraMatrices

      public static void trifocalToCameraMatrices(TrifocalTensor tensor, DMatrixRMaj P2, DMatrixRMaj P3)

      Extract the camera matrices up to a common projective transform.

      NOTE: The camera matrix for the first view is assumed to be P1 = [I|0].

      Parameters:
      tensor - Trifocal tensor. Not modified.
      P2 - Output: 3x4 camera matrix for views 1 to 2. Modified.
      P3 - Output: 3x4 camera matrix for views 1 to 3. Modified.
      See Also:

    • createEssential

      public static DMatrixRMaj createEssential(DMatrixRMaj R, Vector3D_F64 T, @Nullable @Nullable DMatrixRMaj E)

      Computes an essential matrix from a rotation and translation. This motion is the motion from the first camera frame into the second camera frame. The essential matrix 'E' is defined as:
      E = hat(T)*R
      where hat(T) is the skew symmetric cross product matrix for vector T.

      Parameters:
      R - Rotation matrix.
      T - Translation vector.
      E - (Output) Storage for essential matrix. 3x3 matrix
      Returns:
      Essential matrix

    • createFundamental

      public static DMatrixRMaj createFundamental(DMatrixRMaj E, DMatrixRMaj K)
      Computes a Fundamental matrix given an Essential matrix and the camera calibration matrix. F = (K-1)T*E*K-1
      Parameters:
      E - Essential matrix
      K - Intrinsic camera calibration matrix
      Returns:
      Fundamental matrix

    • createFundamental

      public static DMatrixRMaj createFundamental(DMatrixRMaj E, CameraPinhole intrinsic)
      Computes a Fundamental matrix given an Essential matrix and the camera's intrinsic parameters.
      Parameters:
      E - Essential matrix
      intrinsic - Intrinsic camera calibration
      Returns:
      Fundamental matrix
      See Also:

    • createFundamental

      public static DMatrixRMaj createFundamental(DMatrixRMaj E, DMatrixRMaj K1, DMatrixRMaj K2)
      Computes a Fundamental matrix given an Essential matrix and the camera calibration matrix. F = (K2-1)T*E*K1-1
      Parameters:
      E - Essential matrix
      K1 - Intrinsic camera calibration matrix for camera 1
      K2 - Intrinsic camera calibration matrix for camera 2
      Returns:
      Fundamental matrix

    • createFundamental

      public static DMatrixRMaj createFundamental(DMatrixRMaj R, Vector3D_F64 T, DMatrixRMaj K1, DMatrixRMaj K2, @Nullable @Nullable DMatrixRMaj F)

      Computes an fudamental matrix from a rotation, translation, and calibration matrix. Motion is from the first camera frame into the second camera frame.

      Parameters:
      R - Rotation matrix. first to second
      T - Translation vector. first to second
      K1 - Intrinsic camera calibration matrix for camera 1
      K2 - Intrinsic camera calibration matrix for camera 2
      F - (Output) Storage for essential matrix. 3x3 matrix
      Returns:
      Essential matrix

    • createHomography

      public static DMatrixRMaj createHomography(DMatrixRMaj R, Vector3D_F64 T, double d, Vector3D_F64 N)

      Computes a homography matrix from a rotation, translation, plane normal and plane distance:
      x[2] = H*x[1]
      where x[1] is the point on the first camera and x[2] the location in the second camera.
      H = R+(1/d)*T*NT
      Where [R,T] is the transform from camera 1 to camera 2.

      Parameters:
      R - Rotation matrix from camera 1 to camera 2.
      T - Translation vector from camera 1 to camera 2.
      d - Distance > 0 of closest point on plane to the origin of camera 1.
      N - Normal of plane with respect to the first camera.
      Returns:
      Calibrated homography matrix

    • createHomography

      public static DMatrixRMaj createHomography(DMatrixRMaj R, Vector3D_F64 T, double d, Vector3D_F64 N, DMatrixRMaj K)

      Computes a homography matrix from a rotation, translation, plane normal, plane distance, and calibration matrix:
      x[2] = H*x[1]
      where x[1] is the point on the first camera and x[2] the location in the second camera.
      H = K*(R+(1/d)*T*NT)*K-1
      Where [R,T] is the transform from camera 1 to camera, and K is the calibration matrix for both cameras.

      Parameters:
      R - Rotation matrix from camera 1 to camera 2.
      T - Translation vector from camera 1 to camera 2.
      d - Distance > 0 of closest point on plane to the origin of camera 1.
      N - Normal of plane with respect to the first camera.
      K - Intrinsic calibration matrix
      Returns:
      Uncalibrated homography matrix

    • extractEpipoles

      public static void extractEpipoles(DMatrixRMaj F, Point3D_F64 e1, Point3D_F64 e2)

      Extracts the epipoles from an essential or fundamental matrix. The epipoles are extracted from the left and right null space of the provided matrix. Note that the found epipoles are in homogeneous coordinates. If the epipole is at infinity then z=0

      Left: e2T*F = 0
      Right: F*e1 = 0

      Parameters:
      F - Input: Fundamental or Essential 3x3 matrix. Not modified.
      e1 - Output: Right epipole in homogeneous coordinates. Can be null. Modified.
      e2 - Output: Left epipole in homogeneous coordinates. Can be null. Modified.

    • fundamentalToProjective

      public static DMatrixRMaj fundamentalToProjective(DMatrixRMaj F, Point3D_F64 e2, Vector3D_F64 v, double lambda)

      Given a fundamental matrix a pair of camera matrices P and P1' are extracted. The camera matrices are 3 by 4 and used to project a 3D homogenous point onto the image plane. These camera matrices will only be known up to a projective transform, thus there are multiple solutions, The canonical camera matrix is defined as: 

       P=[I|0] and P'= [M|-M*t] = [[e']*F + e'*v^t | lambda*e']
      where e' is the epipole FTe' = 0, [e'] is the cross product matrix for the enclosed vector, v is an arbitrary 3-vector and lambda is a non-zero scalar.

      NOTE: Additional information is needed to upgrade this projective transform into a metric transform.

      Page 256 in R. Hartley, and A. Zisserman, "Multiple View Geometry in Computer Vision", 2nd Ed, Cambridge 2003

      Parameters:
      F - (Input) A fundamental matrix
      e2 - (Input) Left epipole of fundamental matrix, FT*e2 = 0.
      v - (Input) Arbitrary 3-vector. Just pick some value, say (0,0,0).
      lambda - (Input) A non zero scalar. Try one.
      Returns:
      The canonical camera (projection) matrix P' (3 by 4) Known up to a projective transform.
      See Also:

    • projectiveToFundamental

      public static DMatrixRMaj projectiveToFundamental(DMatrixRMaj P1, DMatrixRMaj P2, @Nullable @Nullable DMatrixRMaj F21)

      Given two general camera matrices compute fundamental matrix.

      F= [e']_x P2*P1+, where P1+ is the pseudo inverse of P1, and e' = P2*C, with P*C=0
      Parameters:
      P1 - (Input) camera matrix for view 1
      P2 - (Input) camera matrix for view 2
      F21 - (Output) Fundamental matrix from view 1 to 2
      Returns:
      Fundamental matrix.

    • projectiveToFundamental

      public static DMatrixRMaj projectiveToFundamental(DMatrixRMaj P2, @Nullable @Nullable DMatrixRMaj F21)

      Given an implicit P1=[I|0] camera matrix and P2 compute a fundamental matrix.

      F= [e']_x P2*P1+, where P1+ is the pseudo inverse of P1, and e' = P2*C, with P*C=0
      Parameters:
      P2 - (Input) camera matrix for view 2
      F21 - (Output) Fundamental matrix from view 1 to 2
      Returns:
      Fundamental matrix.

    • fundamentalToProjective

      public static DMatrixRMaj fundamentalToProjective(DMatrixRMaj F)

      Given a fundamental matrix a pair of camera matrices P0 and P1 can be extracted. Same fundamentalToProjective(DMatrixRMaj, Point3D_F64, Vector3D_F64, double) but with the suggested values for all variables filled in for you.

      Parameters:
      F - (Input) Fundamental Matrix
      Returns:
      The canonical camera (projection) matrix P' (3 by 4) Known up to a projective transform.
      See Also:

    • fundamentalToEssential

      public static DMatrixRMaj fundamentalToEssential(DMatrixRMaj F, DMatrixRMaj K, @Nullable @Nullable DMatrixRMaj outputE)
      Given the calibration matrix, convert the fundamental matrix into an essential matrix. E = K'*F*k. The singular values of the resulting E matrix are forced to be [1,1,0]
      Parameters:
      F - (Input) Fundamental matrix. 3x3
      K - (Input) Calibration matrix (3x3)
      outputE - (Output) Found essential matrix
      Returns:
      Essential matrix

    • fundamentalToEssential

      public static DMatrixRMaj fundamentalToEssential(DMatrixRMaj F, DMatrixRMaj K1, DMatrixRMaj K2, @Nullable @Nullable DMatrixRMaj outputE)
      Given the calibration matrix, convert the fundamental matrix into an essential matrix. E = K[2]T*F*k[1]. The singular values of the resulting E matrix are forced to be [1,1,0]
      Parameters:
      F - (Input) Fundamental matrix. 3x3
      K1 - (Input) Calibration matrix view 1 (3x3)
      K2 - (Input) Calibration matrix view 2 (3x3)
      outputE - (Output) Found essential matrix
      Returns:
      Essential matrix

    • fundamentalToProjective

      public static void fundamentalToProjective(DMatrixRMaj F21, DMatrixRMaj F31, DMatrixRMaj F32, DMatrixRMaj P2, DMatrixRMaj P3)
      Given three fundamental matrices that describing the relationship between three views, compute a consistent set of projective camera matrices. Consistent means that the camera matrices will give back the same fundamental matrices, see [1]. This function is of dubious practical value, see discussion in FundamentalToProjective.threeView(org.ejml.data.DMatrixRMaj, org.ejml.data.DMatrixRMaj, org.ejml.data.DMatrixRMaj, org.ejml.data.DMatrixRMaj, org.ejml.data.DMatrixRMaj). The first camera matrix, without loss of generality, is assumed to be P1 = [I|0].
      1. Page 301 in Y. Ma, S. Soatto, J. Kosecka, and S. S. Sastry, "An Invitation to 3-D Vision" Springer-Verlad, 2004
      Parameters:
      F21 - (Input) Fundamental matrix between view 1 and 2
      F31 - (Input) Fundamental matrix between view 1 and 3
      F32 - (Input) Fundamental matrix between view 2 and 3
      P2 - (Output) Camera matrix for view 2
      P3 - (Output) Camera matrix for view 3
      See Also:

    • projectiveToIdentityH

      public static DMatrixRMaj projectiveToIdentityH(DMatrixRMaj P, @Nullable @Nullable DMatrixRMaj H)
      Finds the transform such that P*H = [I|0] where P is a 3x4 projective camera matrix and H is a 4x4 matrix
      Parameters:
      P - (Input) camera matrix 3x4
      H - (Output) 4x4 matrix

    • projectiveMakeFirstIdentity

      public static void projectiveMakeFirstIdentity(List<DMatrixRMaj> cameraMatrices, @Nullable @Nullable DMatrixRMaj H)
      Computes a homography transform which will make the first camera in the list identity and modifies the camera matrices using that homography
      Parameters:
      cameraMatrices - (input/output) Converts
      H - (output + Optional) storage for homography

    • fundamentalCompatible3

      public static boolean fundamentalCompatible3(DMatrixRMaj F21, DMatrixRMaj F31, DMatrixRMaj F32, double tol)

      Checks to see if the three fundamental matrices are consistent based on their epipoles.

      e23TF21e13 = 0
      e31TF32e21 = 0
      e32TF31e12 = 0

      Section 15.4 in R. Hartley, and A. Zisserman, "Multiple View Geometry in Computer Vision", 2nd Ed, Cambridge 2003

      Parameters:
      F21 - (Input) Fundamental matrix between view 1 and 2
      F31 - (Input) Fundamental matrix between view 1 and 3
      F32 - (Input) Fundamental matrix between view 2 and 3

    • decomposeMetricCamera

      public static boolean decomposeMetricCamera(DMatrixRMaj cameraMatrix, DMatrixRMaj K, Se3_F64 worldToView)

      Decomposes a metric camera matrix P=K*[R|T], where A is an upper triangular camera calibration matrix, R is a rotation matrix, and T is a translation vector. If PerspectiveOps.createCameraMatrix(org.ejml.data.DMatrixRMaj, georegression.struct.point.Vector3D_F64, org.ejml.data.DMatrixRMaj, org.ejml.data.DMatrixRMaj) is called using the returned value you will get an equivalent camera matrix.

      • NOTE: There are multiple valid solutions to this problem and only one solution is returned.
      • NOTE: The camera center will be on the plane at infinity.
      Parameters:
      cameraMatrix - Input: Camera matrix, 3 by 4
      K - Output: Camera calibration matrix, 3 by 3.
      worldToView - Output: The rotation and translation.
      Returns:
      true if decompose was successful

    • decomposeEssential

      public static List<Se3_F64> decomposeEssential(DMatrixRMaj E21)
      Decomposes an essential matrix into the rigid body motion which it was constructed from. Due to ambiguities there are four possible solutions. See DecomposeEssential for the details. The correct solution can be found using triangulation and the positive depth constraint, e.g. the objects must be in front of the camera to be seen. Also note that the scale of the translation is lost, even with perfect data.
      Parameters:
      E21 - An essential matrix.
      Returns:
      Four possible motions. From view 1 to view 2.
      See Also:

    • decomposeHomography

      public static List<Tuple2<Se3_F64,Vector3D_F64>> decomposeHomography(DMatrixRMaj H)
      Decomposes a homography matrix that's in Euclidean space (computed from features in normalized image coordinates). The homography is defined as H = (R + (1/d)*T*NT), where R is a 3x3 rotation matrix, d is the distance of the plane, N is the plane's normal (unit vector), T is the translation vector. If the homography is from view 'a' to 'b' then transform (R,T) will be from reference 'a' to 'b'. Note that the returned 'T' is divided by 'd'.
      Parameters:
      H - Homography in Euclidean space
      Returns:
      The set of four possible solutions. First param: motion (R,T). Second param: plane normal vector.
      See Also:

    • errorsHomographySymm

      public static void errorsHomographySymm(List<AssociatedPair> observations, DMatrixRMaj H, @Nullable @Nullable DMatrixRMaj H_inv, DogArray_F64 storage)

      Computes symmetric Euclidean error for each observation and puts it into the storage. If the homography projects the point into the plane at infinity (z=0) then it is skipped

      error[i] = (H*x1 - x2')**2 + (inv(H)*x2 - x1')**2
      Parameters:
      observations - (Input) observations
      H - (Input) Homography
      H_inv - (Input) Inverse of homography. if null it will be computed internally
      storage - (Output) storage for found errors

    • transfer_1_to_3

      public static Point3D_F64 transfer_1_to_3(TrifocalTensor T, Point2D_F64 x1, Vector3D_F64 l2, @Nullable @Nullable Point3D_F64 x3)
      Transfers a point from the first view to the third view using a plane induced by a line in the second view.
      Parameters:
      x1 - (Input) point (pixel) in first view
      l2 - (Input) line in second view
      T - (Input) Trifocal tensor
      x3 - (Output) Induced point (pixel) in third view. Homogenous coordinates.
      Returns:
      induced point.

    • transfer_1_to_3

      public static Point3D_F64 transfer_1_to_3(TrifocalTensor T, Point2D_F64 x1, Point2D_F64 x2, @Nullable @Nullable Point3D_F64 x3)
      Transfers a point to the third view given observed locations in the first and second views
      Parameters:
      x1 - (Input) point (pixel) in first view
      x2 - (Input) point (pixel) in second view
      T - (Input) Trifocal tensor
      x3 - (Output) Induced point (pixel) in third view. Homogenous coordinates.
      Returns:
      induced point.

    • transfer_1_to_2

      public static Point3D_F64 transfer_1_to_2(TrifocalTensor T, Point2D_F64 x1, Vector3D_F64 l3, @Nullable @Nullable Point3D_F64 x2)
      Transfers a point from the first view to the second view using a plane induced by a line in the third view.
      Parameters:
      x1 - (Input) point (pixel) in first view
      l3 - (Input) line in third view
      T - (Input) Trifocal tensor
      x2 - (Output) Induced point (pixel) in second view. Homogenous coordinates.
      Returns:
      induced point.

    • transfer_1_to_2

      public static Point3D_F64 transfer_1_to_2(TrifocalTensor T, Point2D_F64 x1, Point2D_F64 x3, @Nullable @Nullable Point3D_F64 x2)
      Transfers a point from the first view to the second view using the observed location in the third view
      Parameters:
      x1 - (Input) point (pixel) in first view
      x3 - (Input) point (pixel) in third view
      T - (Input) Trifocal tensor
      x2 - (Output) Induced point (pixel) in second view. Homogenous coordinates.
      Returns:
      induced point.

    • projectiveToMetric

      public static boolean projectiveToMetric(DMatrixRMaj cameraMatrix, DMatrixRMaj H, Se3_F64 worldToView, DMatrixRMaj K)
      Elevates a projective camera matrix into a metric one using the rectifying homography. Extracts calibration and Se3 pose. 
       P'=P*H
 K,R,t = decompose(P')
      where P is the camera matrix, H is the homography, (K,R,t) are the intrinsic calibration matrix, rotation, and translation
      Parameters:
      cameraMatrix - (Input) camera matrix. 3x4
      H - (Input) Rectifying homography. 4x4
      worldToView - (Output) Transform from world to camera view
      K - (Output) Camera calibration matrix
      See Also:

    • projectiveToMetricKnownK

      public static boolean projectiveToMetricKnownK(DMatrixRMaj cameraMatrix, DMatrixRMaj H, DMatrixRMaj K, Se3_F64 worldToView)

      Convert the projective camera matrix into a metric transform given the rectifying homography and a known calibration matrix. This simplifies the math compared to projectiveToMetric(org.ejml.data.DMatrixRMaj, org.ejml.data.DMatrixRMaj, georegression.struct.se.Se3_F64, org.ejml.data.DMatrixRMaj) where it needs to extract `K`.

      P = K*[R|T]*H where H is the inverse of the rectifying homography.
      Parameters:
      cameraMatrix - (Input) camera matrix. 3x4
      H - (Input) Rectifying homography. 4x4
      K - (Input) Known calibration matrix
      worldToView - (Output) transform from world to camera view

    • enforceAbsoluteQuadraticConstraints

      public static boolean enforceAbsoluteQuadraticConstraints(DMatrix4x4 Q, boolean zeroCenter, boolean zeroSkew)
      If the solution to the absolute quadratic was computed using a linear methods it will not exactly have the required structure. This forces the structure to match.
      1. Positive diagonal elements
      2. (Optional) zero principle point
      3. (Optional) zero skew
      4. Q = H*diag([1 1 1 0])*HT and H = [K 0; -p'*K 1]
      1. R. Hartley, and A. Zisserman, "Multiple View Geometry in Computer Vision", 2nd Ed, Cambridge 2003
      Parameters:
      Q - Approximate solution to absolute quadratic
      See Also:

    • enforceAbsoluteQuadraticConstraints

      public static boolean enforceAbsoluteQuadraticConstraints(DMatrix4x4 Q, boolean zeroCenter, boolean zeroSkew, @Nullable @Nullable DecomposeAbsoluteDualQuadratic alg)

    • absoluteQuadraticToH

      public static boolean absoluteQuadraticToH(DMatrix4x4 Q, DMatrixRMaj H)
      Decomposes the absolute quadratic to extract the rectifying homogrpahy H. This is used to go from a projective to metric (calibrated) geometry. See pg 464 in [1].

      Q = H*I*HT

      where I = diag(1 1 1 0)

      1. R. Hartley, and A. Zisserman, "Multiple View Geometry in Computer Vision", 2nd Ed, Cambridge 2003
      Parameters:
      Q - (Input) Absolute quadratic. Typically found in auto calibration. Not modified.
      H - (Output) 4x4 rectifying homography.
      See Also:

    • rectifyHToAbsoluteQuadratic

      public static void rectifyHToAbsoluteQuadratic(DMatrixRMaj H, DMatrixRMaj Q)
      Rectifying / calibrating homography to dual absolute quadratic.

      Q = H*I*HT = H(:,1:3)*H(:,1:3)'

      where I = diag(1 1 1 0)

      Parameters:
      H - (Input) 4x4 rectifying homography.
      Q - (Output) Absolute quadratic. Typically found in auto calibration. Not modified.

    • canonicalRectifyingHomographyFromKPinf

      public static void canonicalRectifyingHomographyFromKPinf(DMatrixRMaj K, Point3D_F64 planeAtInfinity, DMatrixRMaj H)
      Constructs a canonical rectifying homography from its components, 3x3 intrinsic matrix K and p the plane at infinity. See 19.2 page 460 in [1]. This assumes that camera matrix P1 = [I|0].

      H=[K*K, 0; -p'*k, 1]

      1. R. Hartley, and A. Zisserman, "Multiple View Geometry in Computer Vision", 2nd Ed, Cambridge 2003
      Parameters:
      K - (Input) 3x3 intrinsic calibration matrix
      planeAtInfinity - (Input) Plane at infinity
      H - (Output) rectifying homography.

    • intrinsicFromAbsoluteQuadratic

      public static void intrinsicFromAbsoluteQuadratic(DMatrixRMaj Q, DMatrixRMaj P, CameraPinhole intrinsic)
      Extracts the intrinsic camera matrix from a view given its camera matrix and the dual absolute quadratic.

      wi = Pi*Q*PiT, where wi = Ki*K'i

      Parameters:
      Q - (Input) Dual absolute qudratic
      P - (Input) Camera matrix for a view
      intrinsic - (Output) found intrinsic parameters

    • decomposeDiac

      public static void decomposeDiac(double w11, double w12, double w13, double w22, double w23, double w33, CameraPinhole intrinsic)
      Extracts the camera calibration matrix on the dual image of the absolute conic (DIAC). Elements which could cause problems if negative have the absolute value taken to avoid NaN. The upper triangular elements in w (DIAC) are passed in.
      Parameters:
      intrinsic - (output) K extracted from w

    • decomposeDiac

      public static void decomposeDiac(DMatrixRMaj w, CameraPinhole intrinsic)
      Extracts the camera calibration matrix on the dual image of the absolute conic (DIAC). Elements which could cause problems if negative have the absolute value taken to avoid NaN.
      Parameters:
      w - (input) DIAC 3x3
      intrinsic - (output) K extracted from w

    • createProjectiveToMetric

      public static DMatrixRMaj createProjectiveToMetric(DMatrixRMaj K, double v1, double v2, double v3, double lambda, @Nullable @Nullable DMatrixRMaj H)
      Given the calibration matrix for the first view, plane at infinity, and lambda (scaling factor) compute the rectifying homography for changing a projective camera matrix into a metric one.

      H = [K 0;v' &lambda]

      Parameters:
      K - 3x3 calibration matrix for view 1
      v1 - plane at infinity
      v2 - plane at infinity
      v3 - plane at infinity
      lambda - scaling factor
      H - (Optional) Storage for 4x4 matrix
      Returns:
      The homography

    • decomposeAbsDualQuadratic

      public static boolean decomposeAbsDualQuadratic(DMatrix4x4 Q, DMatrix3x3 w, DMatrix3 p)
      Decomposes the absolute dual quadratic into the following submatrices: Q=[w -w*p;-p'*w p'*w*p]
      Parameters:
      Q - (Input) Absolute quadratic. Typically found in auto calibration. Not modified.
      w - (Output) 3x3 symmetric matrix
      p - (Output) 3x1 vector
      Returns:
      true if successful or false if it failed
      See Also:

    • split2

      public static Tuple2<List<Point2D_F64>,List<Point2D_F64>> split2(List<AssociatedPair> input)
      Splits the associated pairs into two lists
      Parameters:
      input - List of associated features
      Returns:
      two lists containing each set of features

    • split3

      public static Tuple3<List<Point2D_F64>,List<Point2D_F64>,List<Point2D_F64>> split3(List<AssociatedTriple> input)
      Splits the associated triple into three lists
      Parameters:
      input - List of associated features
      Returns:
      three lists containing each set of features

    • triangulatePoints

      public static void triangulatePoints(SceneStructureMetric structure, SceneObservations observations)
      Convenience function for initializing bundle adjustment parameters. Triangulates points using camera position and pixel observations.
      Parameters:
      structure - camera locations
      observations - observations of features in the images

    • findScale

      public static double findScale(DMatrixRMaj a, DMatrixRMaj b)
      Finds this scale which minimizes the difference between `a` and `b`. ||scale*a-b||
      Parameters:
      a - (Input) matrix
      b - (Input) matrix
      Returns:
      scale factor

    • findScale

      public static double findScale(GeoTuple3D_F64<?> a, GeoTuple3D_F64<?> b)
      Finds the scale which minimizes the difference between `a` and `b`. ||scale*a-b||
      Parameters:
      a - (Input) 3D coordinate
      b - (Input) 3D coordinate
      Returns:
      scale factor

    • splits3Lists

      public static List<List<Point2D_F64>> splits3Lists(List<AssociatedTriple> src, @Nullable @Nullable List<List<Point2D_F64>> dst)
      Lists a list of AssociatedTriple into a list of observations for each camera independently.
      Parameters:
      src - (Input) list of AssociatedTriple
      dst - (Output) list of lists. Can be null.
      Returns:
      list of lists.

    • convertTr

      public static void convertTr(List<AssociatedTriple> src, DogArray<AssociatedTuple> dst)
      Converts a list of associated AssociatedTriple in a DogArray of AssociatedTuple
      Parameters:
      src - (Input) List of AssociatedTriple
      dst - (Output) Array of AssociatedTuple. Array is reset.

    • convertTr

      public static void convertTr(List<AssociatedTriple> src, int idx0, int idx1, DogArray<AssociatedPair> dst)
      Converts a list of associated AssociatedTriple in a DogArray of AssociatedPair
      Parameters:
      src - (Input) List of AssociatedTriple
      idx0 - (Input) which feature in the triple is p1 in the pair
      idx1 - (Input) which feature in the triple is p2 in the pair
      dst - (Output) Array of AssociatedPair. Array is reset.

    • convertTu

      public static void convertTu(List<AssociatedTuple> src, int idx0, int idx1, DogArray<AssociatedPair> dst)
      Converts a list of associated AssociatedTuple in a DogArray of AssociatedPair
      Parameters:
      src - (Input) List of AssociatedTuple
      idx0 - (Input) which feature in the tuple is p1 in the pair
      idx1 - (Input) which feature in the tuple is p2 in the pair
      dst - (Output) Array of AssociatedPair. Array is reset.

    • disparityToRange

      public static double disparityToRange(double disparity, double focalLength, double baseline)
      Applies stereo disparity equation to compute the distance of an object. Simple equation but if you use this equation you know you didn't screw it up.

    • scenePointsToPixels

      public static void scenePointsToPixels(SceneStructureMetric scene, int viewIdx, BoofLambdas.ProcessIndex2<Point3D_F64,Point2D_F64> function)
      Projects points in the scene onto the specified image. Results are returned using a lambda that provides the points index, the point's 3D location in the camera frame, and the projected pixels. No checks are done for the following:
      • Was the feature observed by this view
      • Does the feature appear behind the camera
      • Is the projected pixel inside the image
      Parameters:
      scene - (Input) Metric scene
      viewIdx - (Input) Index of view for which points are projected
      function - (Output) Called with results (index, 3D camera location, pixel)

    • sceneToCloud3

      public static void sceneToCloud3(SceneStructureMetric scene, double tol, BoofLambdas.ProcessIndex<Point3D_F64> func)
      Converts the points in the scene into a 3D point cloud. A lambda is used pass in the results. This function will work if it's homogenous or 3D coordinates.
      Parameters:
      scene - (Input) The scene
      tol - (Input) Only used if the scene is in homogenous coordinates. Tolerance for points being at infinity. Smaller values means more tolerant. Try 1e-8.
      func - (Output) Results are passed in to this function with their index and 3D point.

    • sceneToCloudH

      public static void sceneToCloudH(SceneStructureMetric scene, BoofLambdas.ProcessIndex<Point4D_F64> func)
      Converts the points in the scene into a homogenous point cloud. Results are passed in to the lambda. It will work with a scene in homogenous or 3D coordinates.
      Parameters:
      scene - (Input) The scene
      func - (Output) Results are passed in to this function with their index and 3D point.

    • compatibleHomography

      public static double compatibleHomography(DMatrixRMaj F, DMatrixRMaj H)
      IF a homography is compatible with a fundamental matrix then norm(H'*F + F'*H) = 0. A homography is compatible with F, if H is generated from a plane.

      Page 327 in R. Hartley, and A. Zisserman, "Multiple View Geometry in Computer Vision", 2nd Ed, Cambridge 2003

      Parameters:
      F - (Input) Fundamental matrix
      H - (Input) Homography
      Returns:
      A number close to zero if compatible

    • homographyToFundamental

      public static boolean homographyToFundamental(DMatrixRMaj H21, List<AssociatedPair> pairs, DMatrixRMaj F21)
      Given a homography, compute the Fundamental matrix between the two views. This requires at least 2 points which are not on the plane to work. Using this function it is possible to compute a fundamental matrix from 6 points.

      Page 335 in R. Hartley, and A. Zisserman, "Multiple View Geometry in Computer Vision", 2nd Ed, Cambridge 2003

      Parameters:
      H21 - (Input) known homography
      pairs - (Input) two or more point observations which do not lie on the plane
      F21 - (Output) found homography matrix
      Returns:
      true if no errors were encountered

    • homographyFromRotation

      public static DMatrixRMaj homographyFromRotation(DMatrixRMaj R21, DMatrixRMaj K1, DMatrixRMaj K2, @Nullable @Nullable DMatrixRMaj H21)
      Creates a pixel homography from a rotation matrix and the intrinsic calibration matrices. H = K2*R21*inv(K1)
      Parameters:
      R21 - (Input) Rotation matrix
      K1 - (Input) Intrinsic camera matrix 1
      K2 - (Input) Intrinsic camera matrix 2
      Returns:
      The homography