diff --git a/.gitignore b/.gitignore index b3754d8..f1bcb13 100644 --- a/.gitignore +++ b/.gitignore @@ -6,4 +6,5 @@ temp/ *.pyc target/ cookies -.DS_Store \ No newline at end of file +.DS_Store +calibration.txt \ No newline at end of file diff --git a/README.md b/README.md index 8dda2d0..446d2bd 100644 --- a/README.md +++ b/README.md @@ -74,7 +74,7 @@ chmod +x run.sh 7. All match videos and images used in this repository are sourced from [PNW District 2025 Sammamish Event, Lower Bracket, Round 2, Match 6 of the FIRST Robotics Competition](https://www.youtube.com/watch?v=XZDd_Yerab0). -8. The field image used for visualization (field.png) is sourced from [the FRC game manual](https://firstfrc.blob.core.windows.net/frc2025/Manual/2025GameManual.pdf). +8. The field image used for visualization (field.png) is sourced from [the FRC game manual](https://firstfrc.blob.core.windows.net/frc2026/Manual/2026GameManual.pdf). 9. *Some AI assistance is used in this project.* diff --git a/detector.py b/detector.py index 0d940d5..379ffa1 100644 --- a/detector.py +++ b/detector.py @@ -27,7 +27,7 @@ def enablePrint(): enablePrint() -model_id = "1294-ai-scouting/10" +model_id = "1294-ai-scouting/14" input_video_path = "matches/match.mp4" output_path = "temp/output.json" @@ -35,7 +35,7 @@ def enablePrint(): clip = mp.VideoFileClip(input_video_path) total_frames = clip.n_frames -clip.save_frame("matches/cover.png", t=0) +clip.save_frame("matches/cover.png", t=10) output = sv.JSONSink(output_path) @@ -55,6 +55,7 @@ def json_sink_callback(prediction, video_frame): "frame_width": len(video_frame.image[0]), "frame_height": len(video_frame.image), "frame_id": video_frame.frame_id, + "frame_fps": video_frame.fps, #"timestamp": video_frame.timestamp.isoformat(), } diff --git a/field.png b/field.png index 18ba4e8..79ff5d9 100644 Binary files a/field.png and b/field.png differ diff --git a/requirements.txt b/requirements.txt index daecfc3..4b3a317 100644 --- a/requirements.txt +++ b/requirements.txt @@ -23,3 +23,5 @@ roboflow # Linux: sudo apt install ffmpeg # Debian/Ubuntu # sudo yum install ffmpeg # RHEL/CentOS/Fedora # sudo pacman -S ffmpeg # Arch Linux +# +# On Windows, it is also recommended to install DirectML (pip install onnxruntime-directml) for improved performance of the inference engine. diff --git a/run.bat b/run.bat index 65fc72d..11c8c4d 100644 --- a/run.bat +++ b/run.bat @@ -27,6 +27,8 @@ SET /p detect= IF /I "%detect%"=="y" ( + set ONNXRUNTIME_EXECUTION_PROVIDERS=['DmlExecutionProvider'] + call python detector.py IF !ErrorLevel! NEQ 0 ( diff --git a/run.sh b/run.sh old mode 100644 new mode 100755 diff --git a/src/AIScout.java b/src/AIScout.java index fbd00b3..82d9dbe 100644 --- a/src/AIScout.java +++ b/src/AIScout.java @@ -32,13 +32,36 @@ public class AIScout extends JPanel{ // Relative locations of field joints (i.e. 0.5 is half of the screen) // All x vals must differ to avoid errors in pose estimation - // Currently calibrated for PNW District Sammamish Event 2025 + // Currently calibrated for PNW District Bnuuy Lake Event 2026 // Can be easily changed for other fields by changing these 4 points - - private static final Point TOP_LEFT = new Point(0.19620, 0.19515); - private static final Point BOTTOM_LEFT = new Point(0.02259, 0.69198); - private static final Point TOP_RIGHT = new Point(0.84483, 0.21941); - private static final Point BOTTOM_RIGHT = new Point(0.98811, 0.72152); + // Or by using the interactive field calibrator (drag-and-drop GUI) + + // Default values (used if no calibration file exists) + private static final double DEFAULT_TL_X = 0.22063, DEFAULT_TL_Y = 0.23806; + private static final double DEFAULT_BL_X = 0.06609, DEFAULT_BL_Y = 0.67778; + private static final double DEFAULT_TR_X = 0.77969, DEFAULT_TR_Y = 0.22639; + private static final double DEFAULT_BR_X = 0.94609, DEFAULT_BR_Y = 0.64167; + + // Active calibration points (loaded from file or defaults) + private static Point TOP_LEFT; + private static Point BOTTOM_LEFT; + private static Point TOP_RIGHT; + private static Point BOTTOM_RIGHT; + + static { + double[] cal = FieldCalibrator.loadCalibration(); + if (cal != null) { + TOP_LEFT = new Point(cal[0], cal[1], 0); + BOTTOM_LEFT = new Point(cal[2], cal[3], 0); + TOP_RIGHT = new Point(cal[4], cal[5], 0); + BOTTOM_RIGHT = new Point(cal[6], cal[7], 0); + } else { + TOP_LEFT = new Point(DEFAULT_TL_X, DEFAULT_TL_Y, 0); + BOTTOM_LEFT = new Point(DEFAULT_BL_X, DEFAULT_BL_Y, 0); + TOP_RIGHT = new Point(DEFAULT_TR_X, DEFAULT_TR_Y, 0); + BOTTOM_RIGHT = new Point(DEFAULT_BR_X, DEFAULT_BR_Y, 0); + } + } protected static final boolean RED_ON_LEFT = true; // Whether the red alliance is on the left side of the field in the video. If false, then the blue alliance is on the left. public AIScout() { @@ -50,7 +73,6 @@ public static void main(String[] args) throws IOException { if (args.length != 6) { throw new IllegalArgumentException("Exactly 6 team numbers must be provided as arguments, not " + args.length); } - ArrayList>> detections = detect(); JPanel confirm = new AIScout(); @@ -68,12 +90,45 @@ public static void main(String[] args) throws IOException { Scanner scanner = new Scanner(System.in); if (!scanner.nextLine().equalsIgnoreCase("y")) { - scanner.close(); frame.dispose(); - throw new IllegalStateException("Field not properly aligned. Please adjust TOP_LEFT, BOTTOM_LEFT, TOP_RIGHT, BOTTOM_RIGHT, and RED_ON_LEFT so that the green lines are exactly on the field boundaries and the alliances match, then try again."); + System.out.println("Opening field calibration tool... Drag the yellow dots to the field corners, then click Save & Exit."); + double[] result = FieldCalibrator.launch( + TOP_LEFT.getX(), TOP_LEFT.getY(), + BOTTOM_LEFT.getX(), BOTTOM_LEFT.getY(), + TOP_RIGHT.getX(), TOP_RIGHT.getY(), + BOTTOM_RIGHT.getX(), BOTTOM_RIGHT.getY() + ); + if (result == null) { + scanner.close(); + throw new IllegalStateException("Field calibration was cancelled. Exiting."); + } + // Update the calibration points with the new values + TOP_LEFT = new Point(result[0], result[1], 0); + BOTTOM_LEFT = new Point(result[2], result[3], 0); + TOP_RIGHT = new Point(result[4], result[5], 0); + BOTTOM_RIGHT = new Point(result[6], result[7], 0); + System.out.println("Field calibration saved successfully! Continuing with new calibration values..."); + } else { + frame.dispose(); + } + + System.out.println("When did teleop start? Enter the first second of teleop in seconds (e.g. 25), or press Enter to use Auto/Robot class timing from detections:"); + String teleopLine = scanner.nextLine().trim(); + int manualTeleopStartSec = 0; + if (!teleopLine.isEmpty()) { + try { + manualTeleopStartSec = Integer.parseInt(teleopLine); + if (manualTeleopStartSec < 0) { + scanner.close(); + throw new IllegalStateException("Teleop start time cannot be negative. Exiting."); + } + } catch (NumberFormatException ex) { + scanner.close(); + throw new IllegalStateException("Invalid teleop start time: \"" + teleopLine + "\". Enter a number in seconds or leave blank. Exiting."); + } } - frame.dispose(); - + + ArrayList>> detections = detect(manualTeleopStartSec); // Finds the first frame with 6 robots detected int firstFrameIndex = 0; @@ -93,29 +148,72 @@ public static void main(String[] args) throws IOException { } FRCRobot[] robots = new FRCRobot[amountShows]; int insertionIndex = 0; - while (firstFrameIndex < detections.size() && detections.get(firstFrameIndex).size() != amountShows) { - firstFrameIndex++; - } - if (firstFrameIndex >= detections.size()) { - scanner.close(); - throw new IllegalStateException("Cannot confirm starting point. No frame with " + amountShows + " robots detected found in the video. Please abandon this video and try another one, or check that the detector is working correctly. Exiting"); - } + ArrayList> startingDetections = new ArrayList<>(); + try { + while (firstFrameIndex < detections.size() && detections.get(firstFrameIndex).size() != amountShows) { + firstFrameIndex++; + } + if (firstFrameIndex >= detections.size()) { + throw new IllegalStateException("Cannot confirm starting point. No frame with " + amountShows + " robots detected found in the video. Please abandon this video and try another one, or check that the detector is working correctly. Exiting"); + } - System.out.println("First frame with " + amountShows + " robots detected is at index " + firstFrameIndex + " (which is about " + Math.round(firstFrameIndex * 1000.0 / detections.size()) / 10.0 + "% of the video). Confirm as starting point? (y/n)"); + System.out.println("First frame with " + amountShows + " robots detected is at index " + firstFrameIndex + " (which is about " + Math.round(firstFrameIndex * 1000.0 / detections.size()) / 10.0 + "% of the video). Confirm as starting point? (y/n)"); - if (!scanner.nextLine().equalsIgnoreCase("y")) { + if (!scanner.nextLine().equalsIgnoreCase("y")) { + throw new IllegalStateException("Starting point not confirmed. Exiting."); + } scanner.close(); - throw new IllegalStateException("Starting point not confirmed. Exiting."); - } - scanner.close(); - - long time = System.currentTimeMillis(); - System.out.println("Starting point confirmed. Initializing robots and writing data..."); - - ArrayList> startingDetections = detections.get(firstFrameIndex); + System.out.println("Starting point confirmed. Initializing robots and writing data..."); + + startingDetections = detections.get(firstFrameIndex); + } catch (IllegalStateException e){ + firstFrameIndex = 0; + System.out.println("Cannot automatically confirm starting point. Please manually input the starting locations of the robots in the format specified below. If a robot no shows, do not enter a y coordinate for it."); + System.out.println("Please input the y coordinates (0.0 - 1.0, where 0.0 is closest to the far wall of the field) of the starting locations of robots towards the LEFT side of the field, separated by spaces:"); + String leftInput = scanner.nextLine(); + String[] leftCoords = leftInput.trim().split(" "); + if (leftCoords.length != leftShows) { + scanner.close(); + throw new IllegalStateException("Number of coordinates entered does not match number of robot team numbers inputted on the left side. Exiting."); + } + for (String coord : leftCoords) { + try { + double y = Double.parseDouble(coord); + startingDetections.add(Optional.of(new Point(0.25, y, 0))); + if (y < 0 || y > 1) { + scanner.close(); + throw new IllegalStateException("Invalid coordinate: " + coord + ". Y coordinates must be between 0.0 and 1.0. Exiting."); + } + } catch (NumberFormatException ex) { + scanner.close(); + throw new IllegalStateException("Invalid coordinate: " + coord + ". Please enter valid numbers for coordinates. Exiting."); + } + } + System.out.println("Please input the y coordinates (0.0 - 1.0, where 0.0 is closest to the far wall of the field) of the starting locations of robots towards the RIGHT side of the field, separated by spaces:"); + String rightInput = scanner.nextLine(); + String[] rightCoords = rightInput.trim().split(" "); + if (rightCoords.length != rightShows) { + scanner.close(); + throw new IllegalStateException("Number of coordinates entered does not match number of robot team numbers inputted on the right side. Exiting."); + } + for (String coord : rightCoords) { + try { + double y = Double.parseDouble(coord); + startingDetections.add(Optional.of(new Point(0.75, y, 0))); + if (y < 0 || y > 1) { + scanner.close(); + throw new IllegalStateException("Invalid coordinate: " + coord + ". Y coordinates must be between 0.0 and 1.0. Exiting."); + } + } catch (NumberFormatException ex) { + scanner.close(); + throw new IllegalStateException("Invalid coordinate: " + coord + ". Please enter valid numbers for coordinates. Exiting."); + } + } + } + long time = System.currentTimeMillis(); // Splits frame into left and right halvesby x value, then sorts each half by y // value, then concatenates the halves back together. This way, the robots are // ordered from top left to bottom right, which should be consistent with the @@ -204,8 +302,11 @@ public static void main(String[] args) throws IOException { + Math.round((System.currentTimeMillis() - time) / 100.0) / 10.0 + " seconds)"); } - public static ArrayList>> detect() { + + public static ArrayList>> detect(double manualTeleopStartSec) { // Run detector, then read the output + autoFrameIndices.clear(); + teleFrameIndices.clear(); ArrayList>> allDetections = new ArrayList<>(); ArrayList detections = new ArrayList<>(); @@ -220,7 +321,7 @@ public static ArrayList>> detect() { detectionObject.getDouble("y_max"), detectionObject.getString("class_name"), detectionObject.getDouble("confidence"), detectionObject.getString("tracker_id"), detectionObject.getInt("frame_id"), detectionObject.getInt("class_id"), - detectionObject.getInt("frame_width"), detectionObject.getInt("frame_height")); + detectionObject.getInt("frame_width"), detectionObject.getInt("frame_height"), detectionObject.getDouble("frame_fps")); detections.add(detection); } @@ -243,12 +344,12 @@ public static ArrayList>> detect() { frameDetections.add(Optional.empty()); } else if (det.getClassName().equals("Robot")) { - Optional xCoord = estimateXcoord(new Point(centerX, centerY), TOP_LEFT, TOP_RIGHT, + Optional xCoord = estimateXcoord(new Point(centerX, centerY, det.getFrameId() / det.getFrameFps()), TOP_LEFT, TOP_RIGHT, BOTTOM_LEFT, BOTTOM_RIGHT, 10, 0, 1); - Optional yCoord = estimateYcoord(new Point(centerX, centerY), TOP_LEFT, TOP_RIGHT, + Optional yCoord = estimateYcoord(new Point(centerX, centerY, det.getFrameId() / det.getFrameFps()), TOP_LEFT, TOP_RIGHT, BOTTOM_LEFT, BOTTOM_RIGHT, 10, 0, 1); if (xCoord.isPresent() && yCoord.isPresent()) { - frameDetections.add(Optional.of(new Point(xCoord.get(), yCoord.get()))); + frameDetections.add(Optional.of(new Point(xCoord.get(), yCoord.get(), det.getFrameId() / det.getFrameFps()))); } else { // Do nothing, cuz if the robot is outside the field, then we don't want to add // it to the detections @@ -256,20 +357,23 @@ public static ArrayList>> detect() { // indices correct, which is done below } } + if (manualTeleopStartSec >= 0 && det.getFrameId() / det.getFrameFps() < manualTeleopStartSec) { + frameDetections.add(Optional.empty()); + } allDetections.add(frameDetections); prevFrame = det.getFrameId(); } else { ArrayList> frameDetections = allDetections.get(allDetections.size() - 1); + if (det.getClassName().equals("Auto")) { frameDetections.add(Optional.empty()); - } else if (det.getClassName().equals("Robot")) { - Optional xCoord = estimateXcoord(new Point(centerX, centerY), TOP_LEFT, TOP_RIGHT, + Optional xCoord = estimateXcoord(new Point(centerX, centerY, det.getFrameId() / det.getFrameFps()), TOP_LEFT, TOP_RIGHT, BOTTOM_LEFT, BOTTOM_RIGHT, 10, 0, 1); - Optional yCoord = estimateYcoord(new Point(centerX, centerY), TOP_LEFT, TOP_RIGHT, + Optional yCoord = estimateYcoord(new Point(centerX, centerY, det.getFrameId() / det.getFrameFps()), TOP_LEFT, TOP_RIGHT, BOTTOM_LEFT, BOTTOM_RIGHT, 10, 0, 1); if (xCoord.isPresent() && yCoord.isPresent()) { - frameDetections.add(Optional.of(new Point(xCoord.get(), yCoord.get()))); + frameDetections.add(Optional.of(new Point(xCoord.get(), yCoord.get(), det.getFrameId() / det.getFrameFps()))); } else { // Do nothing, cuz if the robot is outside the field, then we don't want to add // it to the detections diff --git a/src/Detection.java b/src/Detection.java index 965c2bb..83bee09 100644 --- a/src/Detection.java +++ b/src/Detection.java @@ -10,9 +10,9 @@ public class Detection { protected int frameHeight; protected int frameId; protected int classId; - + protected double frameFps; - public Detection(double xMin, double yMin, double xMax, double yMax, String className, double confidence, String trackerId, int frameId, int classId, int frameWidth, int frameHeight){ + public Detection(double xMin, double yMin, double xMax, double yMax, String className, double confidence, String trackerId, int frameId, int classId, int frameWidth, int frameHeight, double frameFps){ this.xMin = xMin; this.yMin = yMin; this.xMax = xMax; @@ -24,6 +24,7 @@ public Detection(double xMin, double yMin, double xMax, double yMax, String clas this.classId = classId; this.frameWidth = frameWidth; this.frameHeight = frameHeight; + this.frameFps = frameFps; } public double getXMin(){ return xMin; @@ -58,4 +59,7 @@ public int getFrameWidth(){ public int getFrameHeight() { return frameHeight; } + public double getFrameFps(){ + return frameFps; + } } diff --git a/src/FRCRobot.java b/src/FRCRobot.java index 2388c76..ddf7a9b 100644 --- a/src/FRCRobot.java +++ b/src/FRCRobot.java @@ -14,14 +14,23 @@ public FRCRobot(Point pos, String team){ this.pos = pos; this.team = team; this.positionHistory = new ArrayList<>(); + updatePosition(pos, true); } public void updatePosition(Point pos, boolean isAuto){//isAuto is true for auto, false for teleop - this.pos = pos; - if (AIScout.RED_ON_LEFT) { - positionHistory.add(isAuto + "," + (1-pos.getX()) + "," + (1-pos.getY())); // Invert x and y coordinates to match visualization orientation (if red is on the left, we want to flip the coordinates to match the visualization's orientation) + double time = pos.getTime(); + double distance = this.pos.distanceTo(pos); + double timeDiff = time - this.pos.getTime(); + double speed = distance / timeDiff; + if(speed > 1){ + return; //Speed is too fast, so we don't update the position } else { - positionHistory.add(isAuto + "," + pos.getX() + "," + pos.getY()); + this.pos = pos; + if (AIScout.RED_ON_LEFT) { + positionHistory.add(isAuto + "," + (1-pos.getX()) + "," + (1-pos.getY()) + "," + time); // Invert x and y coordinates to match visualization orientation (if red is on the left, we want to flip the coordinates to match the visualization's orientation) + } else { + positionHistory.add(isAuto + "," + pos.getX() + "," + pos.getY() + "," + time); + } } } diff --git a/src/FieldCalibrator.java b/src/FieldCalibrator.java new file mode 100644 index 0000000..a4e8d53 --- /dev/null +++ b/src/FieldCalibrator.java @@ -0,0 +1,276 @@ + +import javax.imageio.ImageIO; +import javax.swing.*; +import java.awt.*; +import java.awt.event.*; +import java.awt.image.BufferedImage; +import java.io.*; +import java.nio.file.*; + +public class FieldCalibrator extends JPanel implements MouseListener, MouseMotionListener { + private static final long serialVersionUID = 1L; + private static final String CALIBRATION_FILE = "calibration.txt"; + private static final int DOT_RADIUS = 10; + + private BufferedImage backgroundImage; + private double panelWidth; + private double panelHeight; + + // The four corner points in relative coordinates (0.0 - 1.0) + private double[] xCoords = new double[4]; // TL, BL, TR, BR + private double[] yCoords = new double[4]; + private String[] labels = {"TL", "BL", "TR", "BR"}; + + private int dragIndex = -1; // which point is being dragged + + private boolean saved = false; + + public FieldCalibrator(double topLeftX, double topLeftY, + double bottomLeftX, double bottomLeftY, + double topRightX, double topRightY, + double bottomRightX, double bottomRightY) { + xCoords[0] = topLeftX; yCoords[0] = topLeftY; + xCoords[1] = bottomLeftX; yCoords[1] = bottomLeftY; + xCoords[2] = topRightX; yCoords[2] = topRightY; + xCoords[3] = bottomRightX; yCoords[3] = bottomRightY; + + try { + backgroundImage = ImageIO.read(new File("matches/cover.png")); + } catch (IOException e) { + e.printStackTrace(); + } + + addMouseListener(this); + addMouseMotionListener(this); + } + + @Override + public void paintComponent(Graphics g) { + super.paintComponent(g); + panelWidth = getWidth(); + panelHeight = getHeight(); + + if (backgroundImage != null) { + g.drawImage(backgroundImage, 0, 0, (int) panelWidth, (int) panelHeight, null); + } + + Graphics2D g2d = (Graphics2D) g; + g2d.setRenderingHint(RenderingHints.KEY_ANTIALIASING, RenderingHints.VALUE_ANTIALIAS_ON); + + // Draw field boundary lines in green + g2d.setStroke(new BasicStroke(3f)); + g2d.setColor(Color.GREEN); + int tlX = toPixelX(xCoords[0]), tlY = toPixelY(yCoords[0]); + int blX = toPixelX(xCoords[1]), blY = toPixelY(yCoords[1]); + int trX = toPixelX(xCoords[2]), trY = toPixelY(yCoords[2]); + int brX = toPixelX(xCoords[3]), brY = toPixelY(yCoords[3]); + + g2d.drawLine(tlX, tlY, trX, trY); // top edge + g2d.drawLine(tlX, tlY, blX, blY); // left edge + g2d.drawLine(blX, blY, brX, brY); // bottom edge + g2d.drawLine(trX, trY, brX, brY); // right edge + + // Draw draggable yellow dots with labels + g2d.setFont(new Font(Font.SANS_SERIF, Font.BOLD, 14)); + for (int i = 0; i < 4; i++) { + int px = toPixelX(xCoords[i]); + int py = toPixelY(yCoords[i]); + + g2d.setColor(Color.YELLOW); + g2d.fillOval(px - DOT_RADIUS, py - DOT_RADIUS, DOT_RADIUS * 2, DOT_RADIUS * 2); + g2d.setColor(Color.BLACK); + g2d.drawOval(px - DOT_RADIUS, py - DOT_RADIUS, DOT_RADIUS * 2, DOT_RADIUS * 2); + g2d.setColor(Color.WHITE); + g2d.drawString(labels[i], px + DOT_RADIUS + 4, py + 5); + } + } + + private int toPixelX(double relX) { + return (int) (relX * panelWidth); + } + + private int toPixelY(double relY) { + return (int) (relY * panelHeight); + } + + private double toRelX(int px) { + return px / panelWidth; + } + + private double toRelY(int py) { + return py / panelHeight; + } + + private int findClosestPoint(int mx, int my) { + int closest = -1; + double minDist = Double.MAX_VALUE; + for (int i = 0; i < 4; i++) { + int px = toPixelX(xCoords[i]); + int py = toPixelY(yCoords[i]); + double dist = Math.sqrt((mx - px) * (mx - px) + (my - py) * (my - py)); + if (dist < minDist && dist < DOT_RADIUS * 3) { + minDist = dist; + closest = i; + } + } + return closest; + } + + @Override + public void mousePressed(MouseEvent e) { + dragIndex = findClosestPoint(e.getX(), e.getY()); + } + + @Override + public void mouseReleased(MouseEvent e) { + dragIndex = -1; + } + + @Override + public void mouseDragged(MouseEvent e) { + if (dragIndex >= 0) { + double rx = toRelX(e.getX()); + double ry = toRelY(e.getY()); + // Clamp to [0, 1] + rx = Math.max(0, Math.min(1, rx)); + ry = Math.max(0, Math.min(1, ry)); + xCoords[dragIndex] = rx; + yCoords[dragIndex] = ry; + repaint(); + } + } + + @Override public void mouseClicked(MouseEvent e) {} + @Override public void mouseEntered(MouseEvent e) {} + @Override public void mouseExited(MouseEvent e) {} + @Override public void mouseMoved(MouseEvent e) {} + + public boolean isSaved() { + return saved; + } + + public void markSaved() { + saved = true; + } + + public double getTopLeftX() { return xCoords[0]; } + public double getTopLeftY() { return yCoords[0]; } + public double getBottomLeftX() { return xCoords[1]; } + public double getBottomLeftY() { return yCoords[1]; } + public double getTopRightX() { return xCoords[2]; } + public double getTopRightY() { return yCoords[2]; } + public double getBottomRightX() { return xCoords[3]; } + public double getBottomRightY() { return yCoords[3]; } + + /** + * Saves calibration values to calibration.txt. + */ + public static void saveCalibration(double tlX, double tlY, double blX, double blY, + double trX, double trY, double brX, double brY) { + try (PrintWriter pw = new PrintWriter(new FileWriter(CALIBRATION_FILE))) { + pw.println(tlX + "," + tlY); + pw.println(blX + "," + blY); + pw.println(trX + "," + trY); + pw.println(brX + "," + brY); + } catch (IOException e) { + e.printStackTrace(); + } + } + + /** + * Loads calibration values from calibration.txt. + * Returns null if file doesn't exist or is invalid. + * Returns double[8]: tlX, tlY, blX, blY, trX, trY, brX, brY + */ + public static double[] loadCalibration() { + File f = new File(CALIBRATION_FILE); + if (!f.exists()) { + return null; + } + try { + java.util.List lines = Files.readAllLines(f.toPath()); + if (lines.size() < 4) return null; + double[] vals = new double[8]; + for (int i = 0; i < 4; i++) { + String[] parts = lines.get(i).split(","); + if (parts.length != 2) return null; + vals[i * 2] = Double.parseDouble(parts[0].trim()); + vals[i * 2 + 1] = Double.parseDouble(parts[1].trim()); + } + return vals; + } catch (Exception e) { + return null; + } + } + + /** + * Launches the calibrator GUI. Blocks until user clicks Save & Exit. + * Returns the calibrated values as double[8]: tlX, tlY, blX, blY, trX, trY, brX, brY. + * Returns null if the user closes the window without saving. + */ + public static double[] launch(double tlX, double tlY, double blX, double blY, + double trX, double trY, double brX, double brY) { + FieldCalibrator calibrator = new FieldCalibrator(tlX, tlY, blX, blY, trX, trY, brX, brY); + + JFrame frame = new JFrame("Field Calibration - Drag yellow dots to field corners"); + frame.setDefaultCloseOperation(JFrame.DO_NOTHING_ON_CLOSE); + frame.setSize((int) Visualization.WIDTH / 2, (int) Visualization.HEIGHT / 2 + 50); + frame.setLocation(0, 0); + frame.setLayout(new BorderLayout()); + + try { + frame.setIconImage(ImageIO.read(new File("pop.png"))); + } catch (IOException e) { + // ignore + } + + frame.add(calibrator, BorderLayout.CENTER); + + JButton saveButton = new JButton("Save & Exit"); + saveButton.setFont(new Font(Font.SANS_SERIF, Font.BOLD, 16)); + saveButton.setPreferredSize(new Dimension(0, 45)); + saveButton.addActionListener(e -> { + calibrator.markSaved(); + synchronized (calibrator) { + calibrator.notifyAll(); + } + }); + frame.add(saveButton, BorderLayout.SOUTH); + + frame.addWindowListener(new WindowAdapter() { + @Override + public void windowClosing(WindowEvent e) { + // User closed without saving - still unblock + synchronized (calibrator) { + calibrator.notifyAll(); + } + } + }); + + frame.setVisible(true); + + // Block until save or close + synchronized (calibrator) { + try { + calibrator.wait(); + } catch (InterruptedException e) { + // ignore + } + } + + frame.dispose(); + + if (calibrator.isSaved()) { + double[] result = new double[] { + calibrator.getTopLeftX(), calibrator.getTopLeftY(), + calibrator.getBottomLeftX(), calibrator.getBottomLeftY(), + calibrator.getTopRightX(), calibrator.getTopRightY(), + calibrator.getBottomRightX(), calibrator.getBottomRightY() + }; + saveCalibration(result[0], result[1], result[2], result[3], + result[4], result[5], result[6], result[7]); + return result; + } + return null; + } +} diff --git a/src/Line.java b/src/Line.java index c558597..b3d5e32 100644 --- a/src/Line.java +++ b/src/Line.java @@ -58,7 +58,7 @@ public Point intersection(Line l){ } double x = (l.b - b) / (m - l.m); double y = getY(x); - return new Point(x, y); + return new Point(x, y,0); } } diff --git a/src/Point.java b/src/Point.java index 57d384f..92bc4a3 100644 --- a/src/Point.java +++ b/src/Point.java @@ -1,9 +1,11 @@ public class Point { private final double x; private final double y; - public Point(double x, double y){ + private final double time; + public Point(double x, double y, double time){ this.x = x; this.y = y; + this.time = time; } public double getX(){ return x; @@ -11,11 +13,14 @@ public double getX(){ public double getY(){ return y; } + public double getTime(){ + return time; + } public Point invert(){ - return new Point(y, -x); + return new Point(y, -x, time); } public double distanceTo(Point other){ - return Math.sqrt(Math.pow(this.x - other.x, 2) + Math.pow(this.y - other.y, 2)); + return Math.sqrt(Math.pow(this.x - other.x, 2) + Math.pow((this.y - other.y) / 2, 2)); //Y is divided by 2 to account for the fact that 1 units change in y corresponds to a change of 0.5 units in x, so we need to divide the difference in y-coordinates by 2 before squaring it and adding it to the squared difference in x-coordinates. } public String toString(){ return "(" + x + ", " + y + ")"; diff --git a/src/Visualization.java b/src/Visualization.java index ac81c98..a552805 100644 --- a/src/Visualization.java +++ b/src/Visualization.java @@ -6,31 +6,54 @@ import javax.swing.JPanel; import javax.swing.JFrame; +import javax.swing.JToggleButton; +import javax.swing.JLabel; +import java.awt.BorderLayout; +import java.awt.Dimension; import java.awt.Graphics; import java.awt.GraphicsEnvironment; import java.awt.Rectangle; import java.awt.Color; import java.awt.GradientPaint; +import java.awt.FlowLayout; +import java.awt.RenderingHints; import java.awt.geom.AffineTransform; import java.awt.image.BufferedImage; import java.awt.Graphics2D; import java.util.Scanner; import java.util.ArrayList; -import java.util.Optional; +import java.util.Comparator; public class Visualization extends JPanel { private static final Rectangle SIZE = GraphicsEnvironment.getLocalGraphicsEnvironment().getMaximumWindowBounds(); protected static final double WIDTH = SIZE.getWidth(); - protected static final double HEIGHT = SIZE.getHeight() - 50.0; // -50 to account for taskbar + protected static final double HEIGHT = SIZE.getHeight() - 67.6767676767676767676767676767; // -67 to account for taskbar private static final long serialVersionUID = 1L; private static boolean auto = false; private static boolean tele = false; + private static double currentTime = 0.0; + private static final double TIME_STEP = 0.167; // 10ms - private static ArrayList> points = new ArrayList<>(); - private static ArrayList> states = new ArrayList<>(); + private static ArrayList allMatches = new ArrayList<>(); + + private static final double MIN_DIST = 0.0089;// Minimum distance in field coordinates (0.0 - 1.0) for a point to be considered "visited" for heatmap purposes + + private static final int HEAT_W = 200; + private static final int HEAT_H = 120; + private static final int BLUR_RADIUS = 6; + private BufferedImage heatmapImage; + private boolean heatmapDirty = true; + private boolean heatmapEnabled = false; + private String sideFilter = "both"; // "both", "left", "right" + + static class MatchData { + ArrayList points = new ArrayList<>(); + ArrayList states = new ArrayList<>(); + boolean startedLeft; + } public static void main(String[] args) throws IOException { Scanner scanner = new Scanner(System.in); @@ -40,7 +63,6 @@ public static void main(String[] args) throws IOException { System.out.println("Loading data for team " + teamNumber + "..."); - BufferedReader br; try{ br = new BufferedReader(new FileReader("data/" + teamNumber + ".csv")); @@ -48,31 +70,79 @@ public static void main(String[] args) throws IOException { scanner.close(); throw new RuntimeException("Error loading data for team " + teamNumber + ". Likely causes: incorrect team number format or file not found."); } + String line; + MatchData currentMatch = null; + while ((line = br.readLine()) != null) { + if (line.startsWith("---")) { + if (currentMatch != null && !currentMatch.points.isEmpty()) { + allMatches.add(currentMatch); + } + currentMatch = new MatchData(); + continue; + } + + if (currentMatch == null) { + currentMatch = new MatchData(); + } + String[] values = line.split(","); - if (values.length == 3) { + if (values.length == 4) { try { - boolean isAuto = Boolean.parseBoolean(values[0]); - double x = Double.parseDouble(values[1]); - double y = Double.parseDouble(values[2]); - points.add(Optional.of(new Point(x, y))); - states.add(Optional.of(isAuto)); + Point newPoint = new Point(Double.parseDouble(values[1]), Double.parseDouble(values[2]), Double.parseDouble(values[3])); + + if (!currentMatch.points.isEmpty()) { + Point lastPoint = currentMatch.points.get(currentMatch.points.size() - 1); + if (newPoint.distanceTo(lastPoint) < MIN_DIST) { + continue; // Skip this point since it's too close to the last one + + } + + while (currentTime < newPoint.getTime()) { + double percentage = (currentTime - lastPoint.getTime()) / (newPoint.getTime() - lastPoint.getTime()); + double x = lastPoint.getX() + (newPoint.getX() - lastPoint.getX()) * percentage; + double y = lastPoint.getY() + (newPoint.getY() - lastPoint.getY()) * percentage; + currentMatch.points.add(new Point(x, y, currentTime)); + currentMatch.states.add(isAuto); // Add the state of the last point to the new point + currentTime += TIME_STEP; + } + continue; + + } else { + currentTime = newPoint.getTime(); // Set current time to the time of the new point + } + + + + currentMatch.points.add(newPoint); + currentMatch.states.add(isAuto); + + if (currentMatch.points.size() == 1) { + currentMatch.startedLeft = newPoint.getX() < 0.5; + } } catch (Exception e) { System.out.println("Error parsing line: \"" + line + "\". Skipping this line."); - points.add(Optional.empty()); - states.add(Optional.empty()); } - } else { - points.add(Optional.empty()); - states.add(Optional.empty()); } + + } + + if (currentMatch != null && !currentMatch.points.isEmpty()) { + allMatches.add(currentMatch); } br.close(); + int leftCount = 0, rightCount = 0; + for (MatchData m : allMatches) { + if (m.startedLeft) leftCount++; + else rightCount++; + } + System.out.println("Loaded " + allMatches.size() + " match(es) (" + leftCount + " left-start, " + rightCount + " right-start)."); + String choice; System.out.println("Data loaded. How do you want to visualize it?"); do { @@ -102,16 +172,64 @@ public static void main(String[] args) throws IOException { System.out.println("Starting visualization engine for team " + teamNumber + "..."); - JPanel app = new Visualization(); + Visualization app = new Visualization(); JFrame frame = new JFrame(); frame.setDefaultCloseOperation(JFrame.EXIT_ON_CLOSE); - frame.setSize((int) WIDTH, (int) HEIGHT); + frame.setLayout(new BorderLayout()); frame.setLocation(0, 0); frame.setName("The P.A.C.K. (Predictive, Analytical, and Competitive Knowledge-base) Visualization - Team " + teamNumber); frame.setTitle("The P.A.C.K. (Predictive, Analytical, and Competitive Knowledge-base) Visualization - Team " + teamNumber); frame.setIconImage(ImageIO.read(new File("pop.png"))); - frame.add(app); + + JPanel toolbar = new JPanel(new FlowLayout(FlowLayout.LEFT)); + + JToggleButton heatmapBtn = new JToggleButton("Heatmap"); + heatmapBtn.addActionListener(e -> { + app.heatmapEnabled = heatmapBtn.isSelected(); + app.heatmapDirty = true; + app.repaint(); + }); + toolbar.add(heatmapBtn); + + toolbar.add(new JLabel(" | Starting Side: ")); + + JToggleButton bothBtn = new JToggleButton("Both (" + allMatches.size() + ")", true); + JToggleButton leftBtn = new JToggleButton("Left (" + leftCount + ")"); + JToggleButton rightBtn = new JToggleButton("Right (" + rightCount + ")"); + + bothBtn.addActionListener(e -> { + bothBtn.setSelected(true); + leftBtn.setSelected(false); + rightBtn.setSelected(false); + app.sideFilter = "both"; + app.heatmapDirty = true; + app.repaint(); + }); + leftBtn.addActionListener(e -> { + leftBtn.setSelected(true); + bothBtn.setSelected(false); + rightBtn.setSelected(false); + app.sideFilter = "left"; + app.heatmapDirty = true; + app.repaint(); + }); + rightBtn.addActionListener(e -> { + rightBtn.setSelected(true); + bothBtn.setSelected(false); + leftBtn.setSelected(false); + app.sideFilter = "right"; + app.heatmapDirty = true; + app.repaint(); + }); + + toolbar.add(bothBtn); + toolbar.add(leftBtn); + toolbar.add(rightBtn); + + frame.add(toolbar, BorderLayout.NORTH); + frame.add(app, BorderLayout.CENTER); + frame.pack(); frame.setVisible(true); } @@ -119,9 +237,114 @@ public Visualization() { } + @Override + public Dimension getPreferredSize() { + return new Dimension((int) WIDTH, (int) HEIGHT); + } + + private boolean matchIncluded(MatchData m) { + if (sideFilter.equals("left")) return m.startedLeft; + if (sideFilter.equals("right")) return !m.startedLeft; + return true; + } + + private void rebuildHeatmapImage() { + float[][] density = new float[HEAT_W][HEAT_H]; + for (MatchData m : allMatches) { + if (!matchIncluded(m)) continue; + for (int i = 0; i < m.points.size() && i < m.states.size(); i++) { + Point p = m.points.get(i); + Boolean st = m.states.get(i); + if (p == null || st == null) continue; + boolean isAut = st; + if ((auto && tele) || (auto && isAut) || (tele && !isAut)) { + int cx = Math.min(HEAT_W - 1, Math.max(0, (int) (p.getX() * HEAT_W))); + int cy = Math.min(HEAT_H - 1, Math.max(0, (int) (p.getY() * HEAT_H))); + density[cx][cy] += 1.0f; + } + } + } + + float[][] blurred = gaussianBlur(density, HEAT_W, HEAT_H, BLUR_RADIUS); + + float max = 0; + for (int x = 0; x < HEAT_W; x++) + for (int y = 0; y < HEAT_H; y++) + if (blurred[x][y] > max) max = blurred[x][y]; + + heatmapImage = new BufferedImage(HEAT_W, HEAT_H, BufferedImage.TYPE_INT_ARGB); + if (max > 0) { + for (int x = 0; x < HEAT_W; x++) { + for (int y = 0; y < HEAT_H; y++) { + float t = blurred[x][y] / max; + if (t < 0.01f) continue; + heatmapImage.setRGB(x, y, heatColor(t)); + } + } + } + heatmapDirty = false; + } + + /** ARGB color for a 0..1 intensity using a blue->cyan->green->yellow->red gradient. */ + private static int heatColor(float t) { + int alpha = (int) (80 + 175 * t); + float r, g, b; + if (t < 0.25f) { + float s = t / 0.25f; + r = 0; g = s; b = 1; + } else if (t < 0.5f) { + float s = (t - 0.25f) / 0.25f; + r = 0; g = 1; b = 1 - s; + } else if (t < 0.75f) { + float s = (t - 0.5f) / 0.25f; + r = s; g = 1; b = 0; + } else { + float s = (t - 0.75f) / 0.25f; + r = 1; g = 1 - s; b = 0; + } + return (alpha << 24) | ((int)(r * 255) << 16) | ((int)(g * 255) << 8) | (int)(b * 255); + } + + private static float[][] gaussianBlur(float[][] src, int w, int h, int radius) { + float[] kernel = new float[radius * 2 + 1]; + float sigma = radius / 2.5f; + float sum = 0; + for (int i = -radius; i <= radius; i++) { + kernel[i + radius] = (float) Math.exp(-(i * i) / (2 * sigma * sigma)); + sum += kernel[i + radius]; + } + for (int i = 0; i < kernel.length; i++) kernel[i] /= sum; + + float[][] temp = new float[w][h]; + for (int y = 0; y < h; y++) { + for (int x = 0; x < w; x++) { + float v = 0; + for (int k = -radius; k <= radius; k++) { + int sx = Math.min(w - 1, Math.max(0, x + k)); + v += src[sx][y] * kernel[k + radius]; + } + temp[x][y] = v; + } + } + + float[][] out = new float[w][h]; + for (int x = 0; x < w; x++) { + for (int y = 0; y < h; y++) { + float v = 0; + for (int k = -radius; k <= radius; k++) { + int sy = Math.min(h - 1, Math.max(0, y + k)); + v += temp[x][sy] * kernel[k + radius]; + } + out[x][y] = v; + } + } + return out; + } + @Override public void paint(Graphics g){ super.paint(g); + Graphics2D g2d = (Graphics2D) g; try { drawImage(0, 0, WIDTH, HEIGHT, 0, ImageIO.read(new File("field.png")), g); @@ -129,36 +352,56 @@ public void paint(Graphics g){ e.printStackTrace(); } - Optional prevPoint = Optional.empty(); - Optional prevIsAuto = Optional.empty(); + if (heatmapEnabled) { + if (heatmapDirty) rebuildHeatmapImage(); + if (heatmapImage != null) { + g2d.setRenderingHint(RenderingHints.KEY_INTERPOLATION, RenderingHints.VALUE_INTERPOLATION_BILINEAR); + g2d.drawImage(heatmapImage, 0, 0, (int) WIDTH, (int) HEIGHT, null); + } + } - Graphics2D g2d = (Graphics2D) g; + if (heatmapEnabled) { + g.dispose(); + return; + } - for (int i = 0; i < points.size() && i < states.size(); i+=2) { - Optional point = points.get(i); - Optional isAuto = states.get(i); + for (MatchData m : allMatches) { + if (!matchIncluded(m)) continue; - if (point.isPresent() && isAuto.isPresent() && prevPoint.isPresent() && prevIsAuto.isPresent()) { - int x1 = (int) (prevPoint.get().getX() * WIDTH); - int y1 = (int) (prevPoint.get().getY() * HEIGHT); - int x2 = (int) (point.get().getX() * WIDTH); - int y2 = (int) (point.get().getY() * HEIGHT); - if (isAuto.get() && prevIsAuto.get()) { + Point prevPoint = m.points.get(0); + boolean prevIsAuto = m.states.get(0); + + g.setColor(Color.YELLOW); + g.fillOval((int) (WIDTH * prevPoint.getX()) - 8, (int) (HEIGHT * prevPoint.getY()) - 8, 16, 16); + + for (int i = 0; i < m.points.size() && i < m.states.size(); i++) { + Point point = m.points.get(i); + boolean isAuto = m.states.get(i); + + int x1 = (int) (prevPoint.getX() * WIDTH); + int y1 = (int) (prevPoint.getY() * HEIGHT); + int x2 = (int) (point.getX() * WIDTH); + int y2 = (int) (point.getY() * HEIGHT); + if (isAuto && prevIsAuto) { g.setColor(Color.PINK); if (auto) { g2d.setPaint(new GradientPaint(x1, y1, Color.PINK, x2, y2, Color.RED, false)); g2d.drawLine(x1, y1, x2, y2); } - } else if (!isAuto.get() && !prevIsAuto.get()) { + } else if (!isAuto && !prevIsAuto) { g.setColor(Color.CYAN); if (tele) { g2d.setPaint(new GradientPaint(x1, y1, Color.CYAN, x2, y2, Color.BLUE, false)); g2d.drawLine(x1, y1, x2, y2); } - } + } else { + g.setColor(Color.ORANGE); + g2d.setPaint(new GradientPaint(x1, y1, Color.YELLOW, x2, y2, Color.ORANGE, false)); + g2d.drawLine(x1, y1, x2, y2); + } + prevPoint = point; + prevIsAuto = isAuto; } - prevPoint = point; - prevIsAuto = isAuto; } g.dispose(); }