Clustering gamepiece

.gitignore

@@ -20,3 +20,8 @@ wpilib-install
 *.jpg
 interesting_frames/
 *build*/
+**/__pycache__/
+visualizations/
+sim-output/
+paths/
+field-cad/

constants/camera_constants.json

@@ -55,7 +55,17 @@
       "fps": 60.0,
       "exposure": null,
       "port": 5801,
-      "detector_type": "austin_gpu"
+      "detector_type": "austin_gpu",
+      "camera_type": "uvc"
+    },
+    {
+      "name": "gamepiece_camera",
+      "intrinsics_path": "constants/gamepiece/intrinsics.json",
+      "extrinsics_path": "constants/gamepiece/extrinsics.json",
+      "frame_width": 1920,
+      "frame_height": 1080,
+      "fps": 30.0,
+      "detector_type": "opencv_cpu"
     },
     {
       "name": "second_bot_front",
@@ -98,6 +108,15 @@
       "port": 5801,
       "detector_type": "austin_gpu",
       "camera_type": "uvc"
+    },
+    {
+      "name": "gamepiece_camera",
+      "intrinsics_path": "constants/gamepiece/intrinsics.json",
+      "extrinsics_path": "constants/gamepiece/extrinsics.json",
+      "frame_width": 1920,
+      "frame_height": 1080,
+      "fps": 30.0,
+      "detector_type": "opencv_cpu"
     }
   ]
 }

constants/gamepiece/extrinsics.json

@@ -0,0 +1,8 @@
+{
+  "translation_x": 0.0,
+  "translation_y": 0.0,
+  "translation_z": 0.59,
+  "rotation_x": 0.0,
+  "rotation_y": 0.0698131700798,
+  "rotation_z": 0.0
+}

constants/gamepiece/intrinsics.json

@@ -0,0 +1,11 @@
+{
+  "cx": 905.0642886611503,
+  "cy": 496.38167582251765,
+  "fx": 1002.4958690474144,
+  "fy": 998.538536703553,
+  "k1": -0.24512403608194944,
+  "k2": 0.07928932589087268,
+  "k3": -0.014384100028709823,
+  "p1": 0.0014182760627837333,
+  "p2": 0.0010892061255345102
+}

docs/build-and-run.md

@@ -45,3 +45,20 @@ Built in `src/calibration`:
 - `intrinsics_calibrate`
 - `frame_shower`
 - `focus_calibrate`
+
+## Frame Analysis Tools
+
+`localization_stretch` scans one folder of frames with the 971 GPU AprilTag
+detector. Frames are ordered by a numeric filename stem when available (for
+example, `12.300000.jpg`). A stretch may contain up to `max_gap` consecutive
+frames without a tag; it starts and ends on frames where a tag was detected.
+
+```bash
+./build/bin/localization_stretch \
+  --image_folder=/bos/frames/gamepiece_camera \
+  --intrinsics=constants/gamepiece/intrinsics.json \
+  --max_gap=3
+```
+
+The result includes the start and end files, total span, frames with tags,
+internal gap frames, and elapsed time when filenames are numeric timestamps.

docs/scripts.md

@@ -43,6 +43,19 @@ Usage:
 ./scripts/run_tests.sh
 ```
 
+### `scripts/serve_frame.sh`
+
+- Serves one image, read once at startup, as a frozen browser-viewable frame.
+- Listens on `localhost:5801` until interrupted with `Ctrl-C`.
+
+Usage:
+
+```bash
+./scripts/serve_frame.sh /path/to/frame.jpg
+```
+
+Open <http://localhost:5801/> in a browser.
+
 ## Deploy and Remote Sync
 
 ### `scripts/copy_to_bin.sh`

scripts/rename_frames.sh

@@ -0,0 +1,59 @@
+#!/usr/bin/env bash
+set -euo pipefail
+
+IMAGE_FOLDER="${1:-/bos/frames/gamepiece_camera}"
+FPS="${2:-30}"
+
+if [[ ! -d "$IMAGE_FOLDER" ]]; then
+  echo "Image folder does not exist: $IMAGE_FOLDER" >&2
+  exit 1
+fi
+
+if ! awk -v fps="$FPS" 'BEGIN { exit !(fps > 0) }'; then
+  echo "FPS must be greater than zero: $FPS" >&2
+  exit 1
+fi
+
+mapfile -d '' files < <(
+  find "$IMAGE_FOLDER" -maxdepth 1 -type f \
+    \( -iname '*.jpg' -o -iname '*.jpeg' -o -iname '*.png' \) \
+    -printf '%f\0' | sort -z -V
+)
+
+if (( ${#files[@]} == 0 )); then
+  echo "No image files found in: $IMAGE_FOLDER" >&2
+  exit 1
+fi
+
+temporary_folder="$(mktemp -d "$IMAGE_FOLDER/.rename-frames.XXXXXX")"
+declare -a temporary_files=()
+
+restore_on_failure() {
+  local status=$?
+  if [[ -d "$temporary_folder" ]]; then
+    for i in "${!temporary_files[@]}"; do
+      if [[ -e "${temporary_files[$i]}" ]]; then
+        mv -- "${temporary_files[$i]}" "$IMAGE_FOLDER/${files[$i]}"
+      fi
+    done
+    rmdir "$temporary_folder" 2>/dev/null || true
+  fi
+  exit "$status"
+}
+trap restore_on_failure EXIT
+
+# Move everything aside first so neither the old names nor the new names collide.
+for i in "${!files[@]}"; do
+  temporary_files[$i]="$temporary_folder/$i"
+  mv -- "$IMAGE_FOLDER/${files[$i]}" "${temporary_files[$i]}"
+done
+
+for i in "${!files[@]}"; do
+  timestamp="$(awk -v frame="$i" -v fps="$FPS" \
+    'BEGIN { printf "%.6f", frame / fps }')"
+  mv -- "${temporary_files[$i]}" "$IMAGE_FOLDER/$timestamp.jpg"
+done
+
+rmdir "$temporary_folder"
+trap - EXIT
+echo "Renamed ${#files[@]} images in $IMAGE_FOLDER at ${FPS} FPS."

scripts/serve_frame.sh

@@ -0,0 +1,70 @@
+#!/usr/bin/env bash
+set -euo pipefail
+
+if [[ $# -ne 1 ]]; then
+  echo "Usage: $0 <path-to-frame>" >&2
+  exit 2
+fi
+
+FRAME_PATH="$1"
+
+if [[ ! -f "$FRAME_PATH" ]]; then
+  echo "Frame does not exist or is not a file: $FRAME_PATH" >&2
+  exit 1
+fi
+
+exec python3 - "$FRAME_PATH" <<'PY'
+import mimetypes
+import os
+import sys
+from http.server import BaseHTTPRequestHandler, ThreadingHTTPServer
+
+
+frame_path = os.path.abspath(sys.argv[1])
+with open(frame_path, "rb") as frame_file:
+    frame = frame_file.read()
+
+content_type = mimetypes.guess_type(frame_path)[0] or "application/octet-stream"
+
+
+class FrozenFrameHandler(BaseHTTPRequestHandler):
+    def do_GET(self):  # noqa: N802 - required by BaseHTTPRequestHandler
+        if self.path not in ("/", "/frame"):
+            self.send_error(404)
+            return
+
+        self.send_response(200)
+        self.send_header("Content-Type", content_type)
+        self.send_header("Content-Length", str(len(frame)))
+        self.send_header("Cache-Control", "no-store")
+        self.end_headers()
+        self.wfile.write(frame)
+
+    def do_HEAD(self):  # noqa: N802 - required by BaseHTTPRequestHandler
+        if self.path not in ("/", "/frame"):
+            self.send_error(404)
+            return
+
+        self.send_response(200)
+        self.send_header("Content-Type", content_type)
+        self.send_header("Content-Length", str(len(frame)))
+        self.send_header("Cache-Control", "no-store")
+        self.end_headers()
+
+    def log_message(self, format, *args):
+        print(f"{self.address_string()} - {format % args}", file=sys.stderr)
+
+
+class ReusableThreadingHTTPServer(ThreadingHTTPServer):
+    allow_reuse_address = True
+
+
+server = ReusableThreadingHTTPServer(("0.0.0.0", 5801), FrozenFrameHandler)
+print(f"Serving frozen frame {frame_path} on 0.0.0.0:5801", flush=True)
+try:
+    server.serve_forever()
+except KeyboardInterrupt:
+    pass
+finally:
+    server.server_close()
+PY

src/CMakeLists.txt

@@ -21,6 +21,8 @@ add_subdirectory(yolo)
 add_subdirectory(test)
 add_subdirectory(pathing)
 
+add_subdirectory(tools)
+
 add_executable(second_bot_main second_bot_main.cc)
 target_link_libraries(second_bot_main PRIVATE utils localization pathing camera)
 

src/camera/camera_constants.cc

@@ -3,7 +3,7 @@
 #include "absl/flags/flag.h"
 
 ABSL_FLAG(std::string, camera_constants_path,            // NOLINT
-          "/bos/constants/camera_constants.json",        // NOTLINT
+          "constants/camera_constants.json",              // NOTLINT
           "Path to the json file of camera constants");  //NOLINT
 
 namespace camera {
@@ -56,6 +56,7 @@ auto GetCameraConstants(const std::string& path) -> camera_constants_t {
 
   const nlohmann::json& camera_configs = json.at("cameras");
   for (const nlohmann::json& camera_config : camera_configs) {
+    LOG(INFO) << "Examining: " << camera_config.value("name", std::string{});
     if (camera_config.is_null()) {
       LOG(WARNING) << "Found a null camera config";
       continue;
@@ -101,6 +102,7 @@ auto GetCameraConstants(const std::string& path) -> camera_constants_t {
                       camera_config);
     SetConstant<uint>("log_frequency", camera_constant.log_frequency,
                       camera_config);
+    camera_constant.run_gamepiece = camera_config.value("run_gamepiece", false);
 
     if (camera_config.contains("detector_type") &&
         !camera_config["detector_type"].is_null()) {

src/camera/camera_constants.h

@@ -27,6 +27,7 @@ using camera_constant_t = struct CameraConstant {
   std::optional<uint> port = std::nullopt;
   std::optional<uint> streamer_fps = std::nullopt;
   std::optional<uint> log_frequency = std::nullopt;
+  bool run_gamepiece = false;
   DetectorType detector_type = DetectorType::INVALID;
   CameraType camera_type = CameraType::INVALID;
 
@@ -50,6 +51,7 @@ using camera_constant_t = struct CameraConstant {
     print("Brightness", c.brightness);
     print("Sharpness", c.sharpness);
     print("Log Frequency", c.log_frequency);
+    os << '\t' << "Run Gamepiece: " << std::boolalpha << c.run_gamepiece;
     os << std::endl;
 
     return os;

src/gamepiece/CMakeLists.txt

@@ -1,3 +1,6 @@
-add_library(gamepiece gamepiece.cc)
-target_link_libraries(gamepiece camera yolo utils)
-
+add_library(gamepiece gamepiece.cc ellipse.cc
+            hsv_cluster_tracker.cc lane_density.cc)
+target_link_libraries(gamepiece camera yolo utils Eigen3::Eigen)
+# Eigen's unsupported polynomial solver currently fails to compile with its
+# ARM NEON packet path; this target only operates on tiny fixed-size matrices.
+target_compile_definitions(gamepiece PRIVATE EIGEN_DONT_VECTORIZE)