public boolean estimateCameraPose(BufferedImage leftEye) {

    if (!hasIntrinsic) return false;

    gray.reshape(leftEye.getWidth(), leftEye.getHeight());
    ConvertBufferedImage.convertFrom(leftEye, gray);

    if (!target.process(gray)) return false;

    if (!computeH.computeHomography(target.getDetectedPoints())) return false;

    DenseMatrix64F H = computeH.getHomography();

    targetToOrigin.set(decomposeH.decompose(H));

    return true;
  }
/**
 * Using a camera calibration target verify the robot arm's kinematics.
 *
 * @author Peter Abeles
 */
public class DRCArmKinematicsCalibration {
  FullRobotModel robotModel;

  PlanarCalibrationDetector target =
      FactoryPlanarCalibrationTarget.detectorChessboard(new ConfigChessboard(5, 6, 0.01));

  Zhang99ComputeTargetHomography computeH = new Zhang99ComputeTargetHomography(target.getLayout());
  Zhang99DecomposeHomography decomposeH = new Zhang99DecomposeHomography();

  ImageFloat32 gray = new ImageFloat32(1, 1);

  boolean hasIntrinsic = false;

  Se3_F64 targetToOrigin = new Se3_F64();

  public DRCArmKinematicsCalibration() {}

  public void setIntrinsic(IntrinsicParameters intrinsic) {
    DenseMatrix64F K = PerspectiveOps.calibrationMatrix(intrinsic, null);
    decomposeH.setCalibrationMatrix(K);
    hasIntrinsic = true;
  }

  public boolean estimateCameraPose(BufferedImage leftEye) {

    if (!hasIntrinsic) return false;

    gray.reshape(leftEye.getWidth(), leftEye.getHeight());
    ConvertBufferedImage.convertFrom(leftEye, gray);

    if (!target.process(gray)) return false;

    if (!computeH.computeHomography(target.getDetectedPoints())) return false;

    DenseMatrix64F H = computeH.getHomography();

    targetToOrigin.set(decomposeH.decompose(H));

    return true;
  }

  public Se3_F64 getTargetToOrigin() {
    return targetToOrigin;
  }
}