public static void processImage(NetworkTable table){
  ArrayList<MatOfPoint> contours=new ArrayList<MatOfPoint>();
  double x, y, targetX, targetY, distance, azimuth;
  int FrameCount=0;
  long before=System.currentTimeMillis();
  while (FrameCount < 5) {
    contours.clear();
    videoCapture.read(matOriginal);
    Imgproc.cvtColor(matOriginal,matHSV,Imgproc.COLOR_BGR2HSV);
    Core.inRange(matHSV,LOWER_BOUNDS,UPPER_BOUNDS,matThresh);
    Imgproc.findContours(matThresh,contours,matHeirarchy,Imgproc.RETR_EXTERNAL,Imgproc.CHAIN_APPROX_SIMPLE);
    for (    MatOfPoint mop : contours) {
      Rect rec=Imgproc.boundingRect(mop);
      Imgproc.rectangle(matOriginal,rec.br(),rec.tl(),RED);
    }
    for (Iterator<MatOfPoint> iterator=contours.iterator(); iterator.hasNext(); ) {
      MatOfPoint matOfPoint=(MatOfPoint)iterator.next();
      Rect rec=Imgproc.boundingRect(matOfPoint);
      if (rec.height < 15 || rec.width < 15) {
        iterator.remove();
        continue;
      }
      float aspect=(float)rec.width / (float)rec.height;
      if (aspect < 1.0)       iterator.remove();
    }
    table.putNumber("contours",contours.size());
    table.flush();
    if (contours.size() == 1) {
      Rect rec=Imgproc.boundingRect(contours.get(0));
      y=rec.br().y + rec.height / 2;
      y=-((2 * (y / matOriginal.height())) - 1);
      distance=(TOP_TARGET_HEIGHT - TOP_CAMERA_HEIGHT) / Math.tan((y * VERTICAL_FOV / 2.0 + CAMERA_ANGLE) * Math.PI / 180);
      targetX=rec.tl().x + rec.width / 2;
      targetX=(2 * (targetX / matOriginal.width())) - 1;
      azimuth=normalize360(targetX * HORIZONTAL_FOV / 2.0 + 0);
      Point center=new Point(rec.br().x - rec.width / 2 - 15,rec.br().y - rec.height / 2);
      Point centerw=new Point(rec.br().x - rec.width / 2 - 15,rec.br().y - rec.height / 2 - 20);
      table.putNumber("distance",distance - ROBOT_OFFSET_TO_FRONT);
      table.putNumber("azimuth",azimuth);
    }
    Imgcodecs.imwrite("output.png",matOriginal);
  }
}

public static void processImage ( NetworkTable table ) { ArrayList < MatOfPoint > contours = new ArrayList < MatOfPoint > ( ) ; double x , y , targetX , targetY , distance , azimuth ; int FrameCount = 0 ; long before = System . currentTimeMillis ( ) ; while ( FrameCount < 5 ) { contours . clear ( ) ; videoCapture . read ( matOriginal ) ; Imgproc . cvtColor ( matOriginal , matHSV , Imgproc . COLOR_BGR2HSV ) ; Core . inRange ( matHSV , LOWER_BOUNDS , UPPER_BOUNDS , matThresh ) ; Imgproc . findContours ( matThresh , contours , matHeirarchy , Imgproc . RETR_EXTERNAL , Imgproc . CHAIN_APPROX_SIMPLE ) ; for ( MatOfPoint mop : contours ) { Rect rec = Imgproc . boundingRect ( mop ) ; Imgproc . rectangle ( matOriginal , rec . br ( ) , rec . tl ( ) , RED ) ; } for ( Iterator < MatOfPoint > iterator = contours . iterator ( ) ; iterator . hasNext ( ) ; ) { MatOfPoint matOfPoint = ( MatOfPoint ) iterator . next ( ) ; Rect rec = Imgproc . boundingRect ( matOfPoint ) ; if ( rec . height < 15 || rec . width < 15 ) { iterator . remove ( ) ; continue ; } float aspect = ( float ) rec . width / ( float ) rec . height ; if ( aspect < 1.0 ) iterator . remove ( ) ; } table . putNumber ( "contours" , contours . size ( ) ) ; table . flush ( ) ; if ( contours . size ( ) == 1 ) { Rect rec = Imgproc . boundingRect ( contours . get ( 0 ) ) ; y = rec . br ( ) . y + rec . height / 2 ; y = - ( ( 2 * ( y / matOriginal . height ( ) ) ) - 1 ) ; distance = ( TOP_TARGET_HEIGHT - TOP_CAMERA_HEIGHT ) / Math . tan ( ( y * VERTICAL_FOV / 2.0 + CAMERA_ANGLE ) * Math . PI / 180 ) ; targetX = rec . tl ( ) . x + rec . width / 2 ; targetX = ( 2 * ( targetX / matOriginal . width ( ) ) ) - 1 ; azimuth = normalize360 ( targetX * HORIZONTAL_FOV / 2.0 + 0 ) ; Point center = new Point ( rec . br ( ) . x - rec . width / 2 - 15 , rec . br ( ) . y - rec . height / 2 ) ; Point centerw = new Point ( rec . br ( ) . x - rec . width / 2 - 15 , rec . br ( ) . y - rec . height / 2 - 20 ) ; table . putNumber ( "distance" , distance - ROBOT_OFFSET_TO_FRONT ) ; table . putNumber ( "azimuth" , azimuth ) ; } Imgcodecs . imwrite ( "output.png" , matOriginal ) ; } }
reads an image from a live image capture and outputs information to the smartdashboard or a file
