Create C++ version

cpp/include/chicken_counter/motion.hpp

@@ -0,0 +1,146 @@
+#pragma once
+
+#include <algorithm>
+#include <cmath>
+#include <iostream>
+#include <vector>
+
+#include <opencv2/core/types.hpp>
+#include <opencv2/imgproc.hpp>
+#include <opencv2/video.hpp>
+
+#include "chicken_counter/config.hpp"
+#include "chicken_counter/types.hpp"
+
+namespace cc {
+
+class BackwardMotionDetector {
+public:
+    BackwardMotionDetector() {}
+
+    BackwardMotionDetector(const MotionConfig& config,
+                           const RoiConfig& roi,
+                           bool verbose = false)
+        : config(config), roi(roi), verbose(verbose)
+    {
+        int x1 = roi.points[0].x, y1 = roi.points[0].y;
+        int x2 = x1, y2 = y1;
+        for (const auto& p : roi.points) {
+            if (p.x < x1) x1 = p.x; if (p.y < y1) y1 = p.y;
+            if (p.x > x2) x2 = p.x; if (p.y > y2) y2 = p.y;
+        }
+        roi_bounds = cv::Rect(x1, y1, x2 - x1, y2 - y1);
+    }
+
+    MotionState update(const cv::Mat& frame,
+                       const std::vector<TrackObservation>& tracks,
+                       int frame_index)
+    {
+        if (!config.enabled) return state;
+
+        int stride = std::max(1, config.stride_frames);
+        if (frame_index % stride != 0) return state;
+
+        cv::Mat gray;
+        cv::cvtColor(frame, gray, cv::COLOR_BGR2GRAY);
+
+        cv::Mat gray_roi = gray(roi_bounds);
+
+        double scale = config.flow_scale;
+        if (scale < 1.0) {
+            int tw = std::max(1, static_cast<int>(gray_roi.cols * scale));
+            int th = std::max(1, static_cast<int>(gray_roi.rows * scale));
+            cv::resize(gray_roi, gray_roi, {tw, th}, 0, 0, cv::INTER_AREA);
+        } else {
+            scale = 1.0;
+        }
+
+        cv::Mat mask(gray_roi.size(), CV_8UC1, cv::Scalar(255));
+        int r = config.block_radius;
+        for (const auto& track : tracks) {
+            int lx1 = static_cast<int>((std::max(0, track.bbox_x1 - r) - roi_bounds.x) * scale);
+            int ly1 = static_cast<int>((std::max(0, track.bbox_y1 - r) - roi_bounds.y) * scale);
+            int lx2 = static_cast<int>((std::min(roi_bounds.x + roi_bounds.width, track.bbox_x2 + r) - roi_bounds.x) * scale);
+            int ly2 = static_cast<int>((std::min(roi_bounds.y + roi_bounds.height, track.bbox_y2 + r) - roi_bounds.y) * scale);
+            if (lx2 <= lx1 || ly2 <= ly1) continue;
+            cv::rectangle(mask, {lx1, ly1}, {lx2, ly2}, 0, -1);
+        }
+
+        std::vector<cv::Point2f> points;
+        cv::goodFeaturesToTrack(gray_roi, points, config.max_corners,
+            config.quality_level, config.min_distance, mask, config.block_radius);
+
+        if (previous_gray.empty() || static_cast<int>(points.size()) < config.min_features) {
+            previous_gray = gray_roi.clone();
+            return state;
+        }
+
+        std::vector<cv::Point2f> next_points;
+        std::vector<uint8_t> status;
+        std::vector<float> err;
+        cv::calcOpticalFlowPyrLK(previous_gray, gray_roi, points, next_points, status, err);
+
+        previous_gray = gray_roi.clone();
+
+        if (next_points.empty() || status.empty()) return state;
+
+        int valid_count = 0;
+        double flow_sum = 0.0;
+        for (size_t i = 0; i < status.size(); ++i) {
+            if (!status[i]) continue;
+            double v = (config.axis == "vertical")
+                ? (next_points[i].y - points[i].y)
+                : (next_points[i].x - points[i].x);
+            flow_sum += v;
+            valid_count++;
+        }
+        if (valid_count < config.min_features) return state;
+
+        double median_speed = flow_sum / valid_count * config.forward_sign;
+
+        double alpha = config.ema_alpha;
+        state.smoothed_speed = static_cast<float>(
+            alpha * median_speed + (1.0 - alpha) * state.smoothed_speed);
+
+        if (state.smoothed_speed <= config.reverse_enter_threshold) {
+            ++state.consecutive_reverse_frames;
+        } else if (state.smoothed_speed > config.reverse_exit_threshold) {
+            state.consecutive_reverse_frames = 0;
+            state.backward_active = false;
+        }
+
+        if (state.consecutive_reverse_frames >= config.debounce_frames) {
+            bool was_active = state.backward_active;
+            state.backward_active = true;
+            if (verbose && !was_active) {
+                std::cerr << "[motion] BACKWARD TRIGGERED! smoothed="
+                          << state.smoothed_speed
+                          << " consecutive=" << state.consecutive_reverse_frames << "\n";
+            }
+        }
+
+        if (verbose) {
+            ++update_count;
+            std::cerr << "[motion #" << update_count
+                      << "] features=" << valid_count
+                      << " median_speed=" << median_speed
+                      << " smoothed=" << state.smoothed_speed
+                      << " consecutive_rev=" << state.consecutive_reverse_frames
+                      << " backward=" << state.backward_active << "\n";
+        }
+
+        return state;
+    }
+
+    MotionState state;
+
+private:
+    MotionConfig config;
+    RoiConfig roi;
+    bool verbose;
+    cv::Rect roi_bounds;
+    cv::Mat previous_gray;
+    int update_count = 0;
+};
+
+} // namespace cc