/**
   * Adjusts the view such that each pixel has a correspondence to the original image while
   * maximizing the view area. In other words no black regions which can cause problems for some
   * image processing algorithms.
   *
   * <p>The original image coordinate system is maintained even if the intrinsic parameter flipY is
   * true.
   *
   * @param param Intrinsic camera parameters.
   * @param paramAdj If not null, the new camera parameters are stored here.
   * @return New transform that adjusts the view and removes lens distortion..
   */
  public static PointTransform_F32 allInside(
      IntrinsicParameters param, IntrinsicParameters paramAdj) {
    RemoveRadialPtoP_F32 removeDistort = new RemoveRadialPtoP_F32();
    AddRadialPtoP_F32 addDistort = new AddRadialPtoP_F32();
    removeDistort.set(param.fx, param.fy, param.skew, param.cx, param.cy, param.radial);
    addDistort.set(param.fx, param.fy, param.skew, param.cx, param.cy, param.radial);

    Rectangle2D_F32 bound =
        LensDistortionOps.boundBoxInside(
            param.width, param.height, new PointToPixelTransform_F32(removeDistort));

    // ensure there are no strips of black
    LensDistortionOps.roundInside(bound);

    double scaleX = bound.width / param.width;
    double scaleY = bound.height / param.height;

    double scale = Math.min(scaleX, scaleY);

    // translation and shift over so that the small axis is in the middle
    double deltaX = bound.tl_x + (scaleX - scale) * param.width / 2.0;
    double deltaY = bound.tl_y + (scaleY - scale) * param.height / 2.0;

    // adjustment matrix
    DenseMatrix64F A = new DenseMatrix64F(3, 3, true, scale, 0, deltaX, 0, scale, deltaY, 0, 0, 1);

    PointTransform_F32 tranAdj =
        PerspectiveOps.adjustIntrinsic_F32(addDistort, false, param, A, paramAdj);

    if (param.flipY) {
      PointTransform_F32 flip = new FlipVertical_F32(param.height);
      return new SequencePointTransform_F32(flip, tranAdj, flip);
    } else return tranAdj;
  }
  /**
   * Renders a 3D point in the left and right camera views given the stereo parameters. Lens
   * distortion is taken in account.
   *
   * @param param Stereo parameters
   * @param X Point location in 3D space
   * @param left location in pixels in left camera
   * @param right location in pixels in right camera
   */
  public static void renderPointPixel(
      StereoParameters param, Point3D_F64 X, Point2D_F64 left, Point2D_F64 right) {
    // compute the location of X in the right camera's reference frame
    Point3D_F64 rightX = new Point3D_F64();
    SePointOps_F64.transform(param.getRightToLeft().invert(null), X, rightX);

    // location of object in normalized image coordinates
    Point2D_F64 normLeft = new Point2D_F64(X.x / X.z, X.y / X.z);
    Point2D_F64 normRight = new Point2D_F64(rightX.x / rightX.z, rightX.y / rightX.z);

    // convert into pixel coordinates
    Point2D_F64 pixelLeft =
        PerspectiveOps.convertNormToPixel(param.left, normLeft.x, normLeft.y, null);
    Point2D_F64 pixelRight =
        PerspectiveOps.convertNormToPixel(param.right, normRight.x, normRight.y, null);

    // take in account lens distortion
    Point2Transform2_F32 distLeft =
        LensDistortionOps.transformPoint(param.left).distort_F32(true, true);
    Point2Transform2_F32 distRight =
        LensDistortionOps.transformPoint(param.right).distort_F32(true, true);

    Point2D_F32 lensLeft = new Point2D_F32();
    Point2D_F32 lensRight = new Point2D_F32();

    distLeft.compute((float) pixelLeft.x, (float) pixelLeft.y, lensLeft);
    distRight.compute((float) pixelRight.x, (float) pixelRight.y, lensRight);

    // output solution
    left.set(lensLeft.x, lensLeft.y);
    right.set(lensRight.x, lensRight.y);
  }
  @Override
  public void setCalibration(StereoParameters parameters) {
    stereo.setCalibration(parameters);

    PointTransform_F64 leftPixelToNorm =
        LensDistortionOps.transformRadialToNorm_F64(parameters.left);
    PointTransform_F64 leftNormToPixel =
        LensDistortionOps.transformNormToRadial_F64(parameters.left);

    alg.setPixelToNorm(leftPixelToNorm);
    alg.setNormToPixel(leftNormToPixel);

    tracker.setCalibration(parameters.left);
  }
 public void setDistorted(IntrinsicParameters param, DenseMatrix64F rect) {
   if (rect == null) {
     this.undoRadial =
         LensDistortionOps.imageRemoveDistortion(
             AdjustmentType.FULL_VIEW,
             BorderType.VALUE,
             param,
             null,
             ImageType.single(ImageFloat32.class));
     this.remove_p_to_p =
         LensDistortionOps.transform_F32(AdjustmentType.FULL_VIEW, param, null, false);
   } else {
     this.undoRadial =
         RectifyImageOps.rectifyImage(param, rect, BorderType.VALUE, ImageFloat32.class);
     this.remove_p_to_p = RectifyImageOps.transformPixelToRect_F32(param, rect);
   }
 }
/** @author Peter Abeles */
public class TestCalibrateMonoPlanar {

  CameraPinholeRadial intrinsic =
      new CameraPinholeRadial(200, 210, 0, 320, 240, 640, 480)
          .fsetRadial(0.01, -0.02)
          .fsetTangental(0.03, 0.03);
  Point2Transform2_F64 normToPixel =
      LensDistortionOps.transformPoint(intrinsic).distort_F64(false, true);

  GrayF32 blank = new GrayF32(intrinsic.width, intrinsic.height);

  List<Se3_F64> targetToCamera = new ArrayList<>();

  public TestCalibrateMonoPlanar() {
    double z = 250;
    double w = 40;

    targetToCamera.add(
        new Se3_F64(
            ConvertRotation3D_F64.eulerToMatrix(EulerType.XYZ, 0, 0, 0, null),
            new Vector3D_F64(0, 0, z)));
    targetToCamera.add(
        new Se3_F64(
            ConvertRotation3D_F64.eulerToMatrix(EulerType.XYZ, 0.1, 0, 0, null),
            new Vector3D_F64(w, 0, z)));
    targetToCamera.add(
        new Se3_F64(
            ConvertRotation3D_F64.eulerToMatrix(EulerType.XYZ, 0, 0.1, 0, null),
            new Vector3D_F64(w, w, z)));
    targetToCamera.add(
        new Se3_F64(
            ConvertRotation3D_F64.eulerToMatrix(EulerType.XYZ, 0, 0, 0.1, null),
            new Vector3D_F64(0, -w, z)));
    targetToCamera.add(
        new Se3_F64(
            ConvertRotation3D_F64.eulerToMatrix(EulerType.XYZ, 0.05, 0, 0.1, null),
            new Vector3D_F64(0, -w, z)));
  }

  /**
   * Give it a fake feature detector and a fairly benign scenario and see if it can correctly
   * estimate the camera parameters.
   */
  @Test
  public void fullBasic() {

    FakeDetector detector = new FakeDetector();
    CalibrateMonoPlanar alg = new CalibrateMonoPlanar(detector);
    alg.configure(true, 2, true);

    for (int i = 0; i < targetToCamera.size(); i++) {
      alg.addImage(blank);
    }

    CameraPinholeRadial found = alg.process();

    assertEquals(intrinsic.fx, found.fx, 1e-3);
    assertEquals(intrinsic.fy, found.fy, 1e-3);
    assertEquals(intrinsic.cx, found.cx, 1e-3);
    assertEquals(intrinsic.cy, found.cy, 1e-3);
    assertEquals(intrinsic.skew, found.skew, 1e-3);

    assertEquals(intrinsic.radial[0], found.radial[0], 1e-5);
    assertEquals(intrinsic.radial[1], found.radial[1], 1e-5);

    assertEquals(intrinsic.t1, found.t1, 1e-5);
    assertEquals(intrinsic.t2, found.t2, 1e-5);
  }

  private class FakeDetector implements DetectorFiducialCalibration {

    int count = 0;

    CalibrationObservation set;

    List<Point2D_F64> layout = CalibrationDetectorSquareGrid.createLayout(4, 3, 30, 30);

    @Override
    public boolean process(GrayF32 input) {

      Se3_F64 t2c = targetToCamera.get(count++);

      set = new CalibrationObservation();

      for (int i = 0; i < layout.size(); i++) {
        Point2D_F64 p2 = layout.get(i);
        // location of calibration point on the target
        Point3D_F64 p3 = new Point3D_F64(p2.x, p2.y, 0);

        Point3D_F64 a = SePointOps_F64.transform(t2c, p3, null);

        Point2D_F64 pixel = new Point2D_F64();
        normToPixel.compute(a.x / a.z, a.y / a.z, pixel);

        if (pixel.x < 0
            || pixel.x >= intrinsic.width - 1
            || pixel.y < 0
            || pixel.y >= input.height - 1)
          throw new RuntimeException("Adjust test setup, bad observation");

        set.add(pixel, i);
      }

      return true;
    }

    @Override
    public CalibrationObservation getDetectedPoints() {
      return set;
    }

    @Override
    public List<Point2D_F64> getLayout() {
      return layout;
    }
  }
}