feat: CV blob detection and centroid tracking

Add BFS flood-fill blob extraction, centroid finding, and nearest-neighbour track matching/spawning inside cv_process. Add test verifying a new blob spawns a track.

Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>

firmware/lib/cv/cv.cpp

@@ -3,6 +3,7 @@
 #include <string.h>
 #include <math.h>
 #include <algorithm>
+#include <vector>
 
 void cv_init(CVState& state) {
     state = CVState{};   // value-initialize — calls vector default ctor correctly
@@ -14,6 +15,53 @@ void cv_reset_counts(CVState& state) {
     state.exits   = 0;
 }
 
+struct Point { int x, y; };
+
+// BFS flood fill. Marks visited pixels (sets fg to 0). Returns {-1,-1} if blob < CV_MIN_BLOB_PX.
+static std::pair<float,float> extract_blob(uint8_t* fg, int start_x, int start_y) {
+    std::vector<Point> queue;
+    queue.reserve(512);
+    queue.push_back({start_x, start_y});
+    fg[start_y * CV_W + start_x] = 0;
+
+    float sum_x = 0, sum_y = 0;
+    int count = 0;
+
+    while (!queue.empty()) {
+        Point p = queue.back(); queue.pop_back();
+        sum_x += p.x; sum_y += p.y; count++;
+
+        const int dx[] = {-1, 1, 0, 0};
+        const int dy[] = {0, 0, -1, 1};
+        for (int d = 0; d < 4; d++) {
+            int nx = p.x + dx[d], ny = p.y + dy[d];
+            if (nx < 0 || nx >= CV_W || ny < 0 || ny >= CV_H) continue;
+            int ni = ny * CV_W + nx;
+            if (!fg[ni]) continue;
+            fg[ni] = 0;
+            queue.push_back({nx, ny});
+        }
+    }
+
+    if (count < CV_MIN_BLOB_PX) return {-1.0f, -1.0f};
+    return {sum_x / count, sum_y / count};
+}
+
+static std::vector<std::pair<float,float>> find_centroids(const uint8_t* fg) {
+    std::vector<std::pair<float,float>> result;
+    uint8_t fg_copy[CV_PIXELS];
+    memcpy(fg_copy, fg, CV_PIXELS);
+
+    for (int y = 0; y < CV_H; y++) {
+        for (int x = 0; x < CV_W; x++) {
+            if (!fg_copy[y * CV_W + x]) continue;
+            auto c = extract_blob(fg_copy, x, y);
+            if (c.first >= 0) result.push_back(c);
+        }
+    }
+    return result;
+}
+
 static void frame_diff(const uint8_t* frame, const uint8_t* bg,
                        uint8_t* fg, int pixels) {
     for (int i = 0; i < pixels; i++) {
@@ -53,6 +101,49 @@ CVResult cv_process(CVState& state, const uint8_t* frame, uint8_t line_pct) {
     }
 
     state.last_motion_frame = state.frame_index;
-    // Blob detection and tracking added in Tasks 5-6
+
+    auto centroids = find_centroids(fg);
+
+    std::vector<bool> centroid_matched(centroids.size(), false);
+
+    for (auto& track : state.tracks) {
+        float best_dist = CV_MAX_MOVE * CV_MAX_MOVE;
+        int   best_idx  = -1;
+
+        for (int i = 0; i < (int)centroids.size(); i++) {
+            if (centroid_matched[i]) continue;
+            float dx = centroids[i].first  - track.x;
+            float dy = centroids[i].second - track.y;
+            float d2 = dx*dx + dy*dy;
+            if (d2 < best_dist) { best_dist = d2; best_idx = i; }
+        }
+
+        if (best_idx >= 0) {
+            centroid_matched[best_idx] = true;
+            track.x = centroids[best_idx].first;
+            track.y = centroids[best_idx].second;
+            track.missed = 0;
+        } else {
+            track.missed++;
+        }
+    }
+
+    state.tracks.erase(
+        std::remove_if(state.tracks.begin(), state.tracks.end(),
+            [](const CVTrack& t){ return t.missed > CV_MAX_MISSED; }),
+        state.tracks.end());
+
+    float line_y = (line_pct / 100.0f) * CV_H;
+    for (int i = 0; i < (int)centroids.size(); i++) {
+        if (centroid_matched[i]) continue;
+        CVTrack t;
+        t.id         = state.next_id++;
+        t.x          = centroids[i].first;
+        t.y          = centroids[i].second;
+        t.above_line = (t.y < line_y);
+        t.missed     = 0;
+        state.tracks.push_back(t);
+    }
+    // Line crossing check added in Task 6
     return result;
 }

firmware/test/test_cv/test_cv.cpp

@@ -60,11 +60,34 @@ void test_cv_reset_counts() {
     TEST_ASSERT_EQUAL_INT(0, state.exits);
 }
 
+void test_tracking_spawns_track_for_new_blob() {
+    CVState state;
+    cv_init(state);
+
+    uint8_t bg[CV_PIXELS];
+    fill_frame(bg, 100);
+    cv_process(state, bg, 50); // init background
+
+    // Frame with a bright 30x30 blob in top-left quadrant
+    uint8_t blob_frame[CV_PIXELS];
+    fill_frame(blob_frame, 100);
+    for (int y = 5; y < 35; y++)
+        for (int x = 5; x < 35; x++)
+            blob_frame[y * CV_W + x] = 200;
+
+    cv_process(state, blob_frame, 50);
+
+    TEST_ASSERT_EQUAL_INT(1, (int)state.tracks.size());
+    TEST_ASSERT_FLOAT_WITHIN(5.0f, 20.0f, state.tracks[0].x);
+    TEST_ASSERT_FLOAT_WITHIN(5.0f, 20.0f, state.tracks[0].y);
+}
+
 int main() {
     UNITY_BEGIN();
     RUN_TEST(test_frame_diff_no_change_gives_no_fg);
     RUN_TEST(test_frame_diff_large_change_detected_no_crash);
     RUN_TEST(test_cv_init_clears_state);
     RUN_TEST(test_cv_reset_counts);
+    RUN_TEST(test_tracking_spawns_track_for_new_blob);
     return UNITY_END();
 }