convert opencv code to NI code

[3501/stronghold-2016] / src / org / usfirst / frc / team3501 / robot / subsystems / TowerTracker2.java

package org.usfirst.frc.team3501.robot.subsystems;

import com.ni.vision.NIVision.PointDouble;

import edu.wpi.first.wpilibj.image.BinaryImage;
import edu.wpi.first.wpilibj.image.HSLImage;
import edu.wpi.first.wpilibj.image.NIVisionException;
import edu.wpi.first.wpilibj.image.ParticleAnalysisReport;
import edu.wpi.first.wpilibj.networktables.NetworkTable;
import edu.wpi.first.wpilibj.vision.AxisCamera;

/**
 *
 */

/**
 *
 * @author Elijah Kaufman
 * @version 1.0
 * @description Uses opencv and network table 3.0 to detect the vision targets
 *
 */
public class TowerTracker2 {

  /**
   * static method to load opencv and networkTables
   */
  static {
    System.out.println(System.getProperty("java.library.path"));
    System.loadLibrary("libopencv_java310.so");
    NetworkTable.setClientMode();
    NetworkTable.setIPAddress("roborio-3502.local");
  }
  // constants for the color rbg values
  // these are the threshold values in order
  // LOWER_BOUNDS = new Scalar(58, 0, 109),
  // UPPER_BOUNDS = new Scalar(93, 255, 240);

  // the size for resing the image
  // public static final Size resize = new Size(320, 240);

  public static HSLImage originalImage;
  public static AxisCamera camera;
  public static BinaryImage filteredImage;

  // Constants for known variables
  // the height to the top of the target in first stronghold is 97 inches
  public static final int TOP_TARGET_HEIGHT = 97;
  // the physical height of the camera lens
  public static final int TOP_CAMERA_HEIGHT = 32;

  // camera details, can usually be found on the datasheets of the camera
  public static final double VERTICAL_FOV = 51;
  public static final double HORIZONTAL_FOV = 67;
  public static final double CAMERA_ANGLE = 10;

  public static boolean shouldRun = true;

  /**
   *
   * @param args
   *          command line arguments
   *          just the main loop for the program and the entry points
   */
  public static void main(String[] args) {
    // TODO Auto-generated method stub
    NetworkTable table = NetworkTable.getTable("SmartDashboard");
    // main loop of the program
    while (shouldRun) {
      try {
        // opens up the camera stream and tries to load it
        // videoCapture = new VideoCapture();
        // videoCapture.open("http://10.35.01.11/mjpg/video.mjpg");
        // wait until it is opened
        // while (!videoCapture.isOpened()) {}
        // time to actually process the acquired images
        processImage();
      } catch (Exception e) {
        e.printStackTrace();
        break;
      }
    }
    // make sure the java process quits when the loop finishes
    // videoCapture.release();
    System.exit(0);
  }

  /**
   *
   * reads an image from a live image capture and outputs information to the
   * SmartDashboard or a file
   *
   * @throws NIVisionException
   */
  public static void processImage() throws NIVisionException {
    // ArrayList<MatOfPoint> contours = new ArrayList<MatOfPoint>();
    double x, y, targetX, targetY, distance, azimuth;
    // frame counter
    int FrameCount = 0;
    long before = System.currentTimeMillis();
    // only run for the specified time
    while (FrameCount < 100) {
      originalImage = camera.getImage();
      filteredImage = originalImage.thresholdHSL(328, 360, 100, 255, 43, 255);
      ParticleAnalysisReport[] reports = filteredImage
          .getOrderedParticleAnalysisReports();
      // make sure the contours that are detected are at least 20x20
      // pixels with an area of 400 and an aspect ration greater then 1
      // Commented by HB to go past syntax errors
      for (ParticleAnalysisReport report : reports) {
        if (report.boundingRectHeight > 25 && report.boundingRectWidth > 25
            && report.boundingRectWidth > report.boundingRectHeight) {
          y = reports[0].boundingRectTop - reports[0].boundingRectHeight / 2;
          y = -((2 * (y / originalImage.getHeight())) - 1);
          distance = (TOP_TARGET_HEIGHT - TOP_CAMERA_HEIGHT) /
              Math.tan((y * VERTICAL_FOV / 2.0 + CAMERA_ANGLE) * Math.PI / 180);
          // angle to target...would not rely on this
          targetX = reports[0].boundingRectLeft + reports[0].boundingRectWidth
              / 2;
          targetX = (2 * (targetX / originalImage.getWidth())) - 1;
          azimuth = normalize360(targetX * HORIZONTAL_FOV / 2.0 + 0);
          // drawing info on target
          x = reports[0].boundingRectLeft + originalImage.getWidth() / 2;
          y = reports[0].boundingRectTop - originalImage.getHeight() / 2;
          PointDouble center = new PointDouble(x, y);
          System.out.println(distance + "m, " + azimuth + " degrees     "
              + center.x + ", " + center.y);
        }
      }
      // Commented by HB to go past syntax errors
      for (ParticleAnalysisReport report : reports) {
        // Imgproc.rectangle(matOriginal, rec.br(), rec.tl(), BLACK);
        // draw a rectangle on image matOriginal using bottom right vertex and
        // top left vertex as input
      }
      // if there is only 1 target, then we have found the target we want
      // output an image for debugging
      // Imgcodecs.imwrite("output.png", matOriginal);
      FrameCount++;
    }
    shouldRun = false;
  }

  /**
   * @param angle
   *          a nonnormalized angle
   */
  public static double normalize360(double angle) {
    while (angle >= 360.0)
    {
      angle -= 360.0;
    }
    while (angle < 0.0)
    {
      angle += 360.0;
    }
    return angle;
  }
}