CupDetector.hpp

#pragma once

#include <array>
#include <atomic>
#include <mutex>
#include <thread>
#include <vector>
#include <Vision/ICupDetector.hpp>
#include <libfreenect2/registration.h>
#include <opencv2/opencv.hpp>

namespace HLR
{
namespace Vision
{
class CupDetector : public ICupDetector
{
public:
    CupDetector();

    ~CupDetector() override;

    CupDetector(CupDetector&& cup_detector) = delete;

    CupDetector(const CupDetector& cup_detector) = delete;

    CupDetector& operator=(const CupDetector& cup_detector) = delete;

    CupDetector& operator=(CupDetector&& cup_detector) = delete;

    CupDetection detect_cups() override;

    std::shared_ptr<cv::Mat> get_detected_cups_image() override;

    std::shared_ptr<cv::Mat> get_latest_depth_frame() override;

    std::shared_ptr<cv::Mat> get_latest_ir_frame() override;

    std::shared_ptr<cv::Mat> get_latest_color_frame() override;

private:
    void update_frames();

    void detect();

    std::vector<cv::Point> find_cups_in_binary_canny(cv::Mat& input);

    void depth_to_binary_canny(const cv::Mat& src, cv::Mat& dst);

    void colour_to_binary(const cv::Mat& src,
                          cv::Mat& dst,
                          const cv::Point& ref_point);

    std::vector<cv::Point> get_cups_color_image(
        const cv::Mat& src,
        std::vector<cv::Point> ref_points);

    std::shared_ptr<cv::Mat> latest_color_frame;
    std::shared_ptr<cv::Mat> latest_ir_frame;
    std::shared_ptr<cv::Mat> latest_depth_frame;
    CupDetection latest_cup_detection;
    std::atomic_bool shutdown;
    std::atomic_bool new_frame;
    std::mutex frame_lock;
    std::thread frame_update_thread;
    std::thread detect_thread;
    cv::Mat modified_depth_mat;
    cv::Mat detected_cups_image;
    std::vector<std::vector<cv::Point>> reference_contours;
    std::unique_ptr<libfreenect2::Registration> registration;
};
} // namespace Vision
} // namespace HLR