vision_demo/src/FeedLevelDetector.cpp

#include "FeedLevelDetector.h"
#include <opencv2/imgproc.hpp>
#include <iostream>
#include <thread>

FeedLevelDetector::FeedLevelDetector() : captureInterval(10), useCamera(false) {
    lastCaptureTime = std::chrono::system_clock::now();
}

void FeedLevelDetector::setCaptureInterval(int interval) {
    if (interval == 10 || interval == 30 || interval == 60) {
        captureInterval = interval;
    } else {
        std::cerr << "Invalid interval. Using default (10s)." << std::endl;
    }
}

bool FeedLevelDetector::shouldCaptureNow() {
    auto now = std::chrono::system_clock::now();
    auto elapsed = std::chrono::duration_cast<std::chrono::seconds>(now - lastCaptureTime).count();
    return (elapsed >= captureInterval);
}

void FeedLevelDetector::startDetectionFromCamera() {
    useCamera = true; // 标记为摄像头模式
    camera.open(0);   // 打开摄像头
    if (!camera.isOpened()) {
        std::cerr << "Camera not available!" << std::endl;
        return;
    }

    cv::Mat frame;
    while (true) {
        camera >> frame; // 获取当前帧
        if (frame.empty()) break;

        // 显示实时画面（但不处理）
        cv::imshow("Camera Feed", frame);

        // 按ESC退出
        if (cv::waitKey(1) == 27) break;

        // 检查是否到达采集时间
        if (shouldCaptureNow()) {
            cv::Mat processed = preprocessImage(frame);
            float distance = findFeedSurface(processed);

            // 显示结果
            displayResult(frame, distance);
            //std::cout << "[Detection] Distance: " << distance << " pixels" << std::endl;

            // 更新时间戳
            lastCaptureTime = std::chrono::system_clock::now();

            // 控制台输出当前距离（可选）
            std::cout << "Detected distance: " << distance << " pixels" << std::endl;
        }
    }

    camera.release();
    cv::destroyAllWindows();
}

cv::Mat FeedLevelDetector::preprocessImage(const cv::Mat& input) {
    cv::Mat gray, blurred;
    
    // 转换为灰度图像
    cv::cvtColor(input, gray, cv::COLOR_BGR2GRAY);
    cv::imshow("gray image", gray);
    // 高斯模糊去噪
    //cv::GaussianBlur(gray, blurred, cv::Size(5, 5), 1.5);
    cv::GaussianBlur(gray, blurred, cv::Size(7, 7), 3.0);
    cv::imshow("blurred image", blurred);
    return blurred;
}

float FeedLevelDetector::findFeedSurface(const cv::Mat& image) {
    cv::Mat edges;
    
    // Canny边缘检测
    //cv::Canny(image, edges, 150, 200);
    cv::Canny(image, edges, 50, 100);
    cv::imshow("edges image", edges);
    //cv::Canny(image, edges, 50, 100);
    // 查找轮廓
    std::vector<std::vector<cv::Point>> contours;
    cv::findContours(edges, contours, cv::RETR_EXTERNAL, cv::CHAIN_APPROX_SIMPLE);
    
    float minY = image.rows; // 初始设置为图像高度
    
    // 查找所有轮廓中的最高点（距离顶部最近的点）
    for (const auto& contour : contours) {
        for (const auto& point : contour) {
            if (point.y < minY) {
                minY = point.y;
            }
        }
    }
    
    // 如果没有找到轮廓，返回图像高度
    if (minY == image.rows) {
        return image.rows;
    }
    
    return minY;
}

void FeedLevelDetector::displayResult(cv::Mat& image, float distance) {
    // 创建显示副本
    cv::Mat displayImage = image.clone();

    // 绘制饲料表面线
    cv::line(displayImage, cv::Point(0, distance),
             cv::Point(image.cols-1, distance), 
             cv::Scalar(0, 0, 255), 2);

    // 显示距离信息
    std::string infoText = "Interval: " + std::to_string(captureInterval) + "s | " +
                           "Distance: " + std::to_string(distance) + " pixels";

    cv::putText(displayImage, infoText,
                cv::Point(20, 30),
                cv::FONT_HERSHEY_SIMPLEX,
                0.8, cv::Scalar(0, 255, 0), 2);

    // 显示处理间隔信息
    auto now = std::chrono::system_clock::now();
    auto nextCapture = lastCaptureTime + std::chrono::seconds(captureInterval);
    auto remain = std::chrono::duration_cast<std::chrono::seconds>(nextCapture - now).count();

    // 更新显示
    cv::imshow("Feed Level Monitoring", displayImage);

}

float FeedLevelDetector::detectFeedLevel(const std::string& imagePath) {
    // 读取图像
    cv::Mat image = cv::imread(imagePath);
    if (image.empty()) {
        std::cerr << "Could not open or find the image: " << imagePath << std::endl;
        return -1;
    }
    
    // 预处理图像
    cv::Mat processed = preprocessImage(image);
    
    // 检测饲料表面
    float distance = findFeedSurface(processed);
    
    // 显示结果
    displayResult(image, distance);
    
    return distance;
}