Class QuadPoseEstimator


public class QuadPoseEstimator
extends Object
Estimates the pose using P3P and iterative refinement from 4 points on a plane with known locations. While this seems like it would be a trivial problem it actually takes several techniques to ensure accurate results. At a high level it uses P3P to provide an estimate. If the error is large it then uses EPNP. Which ever is better it then refines. If the target is small and directly facing the camera it will enlarge the target to estimate it's orientation. Otherwise it will over fit location since it takes a large change in orientation to influence the result.
  • Field Details

  • Constructor Details

    • QuadPoseEstimator

      public QuadPoseEstimator​(double refineTol, int refineIterations)
      Constructor which picks reasonable and generally good algorithms for pose estimation.
      refineTol - Convergence tolerance. Try 1e-8
      refineIterations - Number of refinement iterations. Try 200
    • QuadPoseEstimator

      public QuadPoseEstimator​(EstimateNofPnP p3p, RefinePnP refine)
      Constructor in which internal estimation algorithms are provided
  • Method Details

    • setLensDistoriton

      public void setLensDistoriton​(LensDistortionNarrowFOV distortion)
      Specifies the intrinsic parameters.
      distortion - Intrinsic camera parameters
    • setFiducial

      public void setFiducial​(double x0, double y0, double x1, double y1, double x2, double y2, double x3, double y3)
      Specify the location of points on the 2D fiducial. These should be in "world coordinates"
    • pixelToMarker

      public void pixelToMarker​(double pixelX, double pixelY, Point2D_F64 marker)
      Given the found solution, compute the the observed pixel would appear on the marker's surface. pixel -> normalized pixel -> rotated -> projected on to plane
      pixelX - (Input) pixel coordinate
      pixelY - (Input) pixel coordinate
      marker - (Output) location on the marker
    • process

      public boolean process​(Quadrilateral_F64 corners, boolean unitsPixels)

      Estimate the 3D pose of the camera from the observed location of the fiducial.

      MUST call setFiducial(double, double, double, double, double, double, double, double) and setLensDistoriton(boofcv.alg.distort.LensDistortionNarrowFOV) before calling this function.
      corners - Observed corners of the fiducial.
      unitsPixels - If true the specified corners are in original image pixels or false for normalized image coordinates
      true if successful or false if not
    • estimate

      protected boolean estimate​(Quadrilateral_F64 cornersPixels, Quadrilateral_F64 cornersNorm, Se3_F64 foundFiducialToCamera)
      Given the observed corners of the quad in the image in pixels estimate and store the results of its pose
    • estimateP3P

      protected void estimateP3P​(int excluded)
      Estimates the pose using P3P from 3 out of 4 points. Then use all 4 to pick the best solution
      excluded - which corner to exclude and use to check the answers from the others
    • enlarge

      protected void enlarge​(Quadrilateral_F64 corners, double scale)
      Enlarges the quadrilateral to make it more sensitive to changes in orientation
    • extend

      protected void extend​(Point2D_F64 pivot, Point2D_F64 corner, double scale)
    • computeErrors

      protected double computeErrors​(Se3_F64 fiducialToCamera)
      Compute the sum of reprojection errors for all four points
      fiducialToCamera - Transform being evaluated
      sum of Euclidean-squared errors
    • getWorldToCamera

      public Se3_F64 getWorldToCamera()
    • getError

      public double getError()
      Reprojection error of fiducial
    • createCopyPoints2D3D

      public List<Point2D3D> createCopyPoints2D3D()