59 lines
1.6 KiB
C++
59 lines
1.6 KiB
C++
// firmware/lib/cv/cv.cpp
|
|
#include "cv.h"
|
|
#include <string.h>
|
|
#include <math.h>
|
|
#include <algorithm>
|
|
|
|
void cv_init(CVState& state) {
|
|
memset(&state, 0, sizeof(CVState));
|
|
state.next_id = 1;
|
|
}
|
|
|
|
void cv_reset_counts(CVState& state) {
|
|
state.entries = 0;
|
|
state.exits = 0;
|
|
}
|
|
|
|
static void frame_diff(const uint8_t* frame, const uint8_t* bg,
|
|
uint8_t* fg, int pixels) {
|
|
for (int i = 0; i < pixels; i++) {
|
|
int diff = (int)frame[i] - (int)bg[i];
|
|
if (diff < 0) diff = -diff;
|
|
fg[i] = (diff > CV_DIFF_THRESH) ? 1 : 0;
|
|
}
|
|
}
|
|
|
|
CVResult cv_process(CVState& state, const uint8_t* frame, uint8_t line_pct) {
|
|
CVResult result = {0, 0};
|
|
state.frame_index++;
|
|
|
|
if (!state.bg_valid) {
|
|
memcpy(state.background, frame, CV_PIXELS);
|
|
state.bg_valid = true;
|
|
return result;
|
|
}
|
|
|
|
uint8_t fg[CV_PIXELS];
|
|
frame_diff(frame, state.background, fg, CV_PIXELS);
|
|
|
|
int fg_count = 0;
|
|
for (int i = 0; i < CV_PIXELS; i++) fg_count += fg[i];
|
|
|
|
bool motion = fg_count > CV_MIN_BLOB_PX;
|
|
if (!motion) {
|
|
if (state.frame_index - state.last_motion_frame > 10) {
|
|
memcpy(state.background, frame, CV_PIXELS);
|
|
}
|
|
for (auto& t : state.tracks) t.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());
|
|
return result;
|
|
}
|
|
|
|
state.last_motion_frame = state.frame_index;
|
|
// Blob detection and tracking added in Tasks 5-6
|
|
return result;
|
|
}
|